Nghiên cứu, thiết kế hệ điều khiển robot đa năng phục vụ trinh sát và chiến đấu trên mặt đất

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - Bùi Đức Thịnh NGHIÊN CỨU, THIẾT KẾ HỆ ĐIỀU KHIỂN ROBOT ĐA NĂNG PHỤC VỤ TRINH SÁT VÀ CHIẾN ĐẤU TRÊN MẶT ĐẤT LUẬN VĂN THẠC SĨ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA Hà Nội – Năm 2019 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - BÙI ĐỨC THỊNH NGHIÊN CỨU, THIẾT KẾ HỆ ĐIỀU KHIỂN ROBOT ĐA NĂNG PHỤC VỤ TRINH SÁT VÀ CHIẾN ĐẤU TRÊN MẶT ĐẤT Chuyên ngành: ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA LUẬN VĂN THẠC SĨ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA NGƯỜI HƯỚNG DẪN KHOA HỌC TS NGUYỄN MẠNH TIẾN HÀ NỘI – 2019 LỜI CAM ĐOAN Tên là: Bùi Đức Thịnh Học viên lớp cao học Điều khiển tự động hóa 2016A – Trường đại học Bách khoa Hà Nội Xin cam đoan: đề tài “Nghiên cứu, thiết kế hệ điều khiển robot đa phục vụ trinh sát chiến đấu mặt đất” thầy giáo TS Nguyễn Mạnh Tiến hướng dẫn riêng “Tôi cam đoan rằng, ngoại trừ kết tham khảo từ cơng trình khác ghi rõ luận văn, cơng việc trình bày luận văn tơi thực chưa có phần nội dung luận văn nộp để lấy cấp trường trường khác” LỜI CẢM ƠN Tôi xin chân thành cảm ơn Viện Đào tạo sau đại học, môn Tự động hóa cơng nghiệp thuộc trường đại học Bách khoa Hà Nội tạo điều kiện thuận lợi cho thực luận văn Đặc biệt, xin bày tỏ long biết ơn sâu sắc tới thầy giáo TS Nguyễn Mạnh Tiến tận tình hướng dẫn, động viên giúp đỡ tơi hồn thành luận văn Hà Nội, ngày tháng 04 năm 2019 Học viên Bùi Đức Thịnh MỤC LỤC LỜI CAM ĐOAN LỜI CẢM ƠN MỤC LỤC DANH MỤC KÝ HIỆU VÀ CHỮ VIẾT TẮT DANH MỤC BẢNG DANH MỤC HÌNH VẼ PHẦN MỞ ĐẦU 10 CHƯƠNG 1: 12 TỔNG QUAN VỀ ROBOT DI ĐỘNG 1.1 Giới thiệu robot di động 12 1.2 Cấu tạo, phân loại robot di động 14 1.2.1 Cấu tạo robot di động 14 1.2.2 Phân loại robot di động 15 1.3 Ứng dụng robot di động 16 1.4 Mơ hình robot sử dụng luận văn 18 CHƯƠNG 2: XÂY DỰNG MÔ HÌNH TỐN HỌC ROBOT VÀ HỆ TRUYỀN ĐỘNG CHO ĐỘNG CƠ BLDC 20 2.1 Một số đặc điểm xây dựng mơ hình tốn học robot 20 2.2 Khung tọa độ robot 20 2.3 Mơ hình động học robot bánh xích 21 2.4 Mơ hình động lực học robot bánh xích 23 2.5 Mơ hình cấu chấp hành (động BLDC) 26 2.6 Mơ hình robot 31 CHƯƠNG 3: XÂY DỰNG VÀ THIẾT KẾ KHỐI TRUYỀN THÔNG GIỮA ROBOT VÀ TRUNG TÂM ĐIỀU KHIỂN 3.1 Một số phương thức truyền thơng khơng dây 33 33 3.1.1 Sóng vi ba mặt đất 33 3.1.2 Sóng vi ba vệ tinh 34 3.1.3 Sóng vơ tuyến quảng bá 36 3.1.4 Tia hồng ngoại 36 3.2 Phân tích lựa chọn phương thức truyền thơng 37 3.3 Tính tốn, thiết kế hệ thống truyền thông 37 3.4 Lựa chọn thiết bị truyền thông 39 3.5 Thiết kế phần mềm kết nối trung tâm điều khiển robot 40 CHƯƠNG 4: XÂY DỰNG HỆ THỐNG ĐIỀU KHIỂN ROBOT VÀ MÔ PHỎNG 44 4.1 Xây dựng hệ thống điều khiển truyền động điện cho động bánh lái 44 4.1.1 Tổng hợp mạch vòng dòng điện 44 4.1.2 Tổng hợp mạch vòng điều chỉnh tốc độ 45 4.2 Xây dựng hệ thống điều khiển vị trí robot di động 46 4.3 Tính tốn tham số 48 4.4 Mô hệ thống 50 4.4.1 Sơ đồ mô Simulink 50 4.4.2 Kết mô 52 CHƯƠNG 5: 5.1 XÂY DỰNG HỆ THỐNG VÀ KẾT QUẢ THỰC NGHIỆM Cấu hình mơ hình robot 5.1.1 55 55 Khung xe sở robot 56 5.1.2 Bộ điều khiển công suất cấu chấp hành 57 5.1.3 Bộ chuyển đổi tín hiệu 58 5.1.4 Hệ thống cảm biến định vị GNSS xử lý tín hiệu vị trí 59 5.1.5 Trung tâm điều khiển 64 5.2 Kết thực nghiệm 66 KẾT LUẬN VÀ KIẾN NGHỊ 67 TÀI LIỆU THAM KHẢO 68 Phụ lục 1: Chương trình xử lý liệu vị trí (Python) 70 Phụ lục 2: chương trình trung tâm điều khiển (C++) 73 Phụ lục 3: chương trình mạch chuyển đổi tín hiệu (Arduino) 81 DANH MỤC KÝ HIỆU VÀ CHỮ VIẾT TẮT Các ký hiệu Ký hiệu Đơn vị Ý nghĩa Fi N Lực đẩy robot bên động có lực nhỏ Fo N Lực đẩy robot bên động có lực lớn Vji, Vjo m/s Vận tốc dài bên xích robot Vi, Vo m/s Vận tốc dài bên bánh lái ωi, ωo rad/s Tốc độ góc động robot 2b m Khoảng cách bên xích robot fy N Lực ma sát bên robot Ri, Ro N Lực cản theo chiều dọc xích robot MF N.m Momen tạo lực đẩy robot Mr N.m Momen tạo lực cản robot 2l m Chiều dài xích Các chữ viết tắt Ý nghĩa Chữ viết tắt UAVs Thiết bị bay không người lái AUVs Thiết bị tự động nước (robot di động nước) UGVs Thiết bị tự động mặt đất (robot di động mặt đất) GNSS Hệ thống vệ tinh định vị toàn cầu (Global Navigation Satellite System) BLDC Động chiều không chổi than UHF Tần số cực cao (Ultra high frequency) SHF Tần số siêu cao (Super high frequency) FM Điều chế tần số (Frequency modulation) VHF Tần số cao (Very high frequency) DANH MỤC BẢNG Bảng 4.1: Thông số động sử dụng robot 48 Bảng 5.1: Thơng số đặc tính kỹ thuật robot .56 Bảng 5.2: Thơng số kỹ thuật tính điều khiển công suất 57 DANH MỤC HÌNH VẼ Hình 1.1 Robot di động 13 Hình 1.2 Cấu tạo loại robot di động đơn giản 15 Hình 1.3 Robot di động không (UAVs) 16 Hình 1.4 Robot vận chuyển hàng kho hàng Amazon .16 Hình 1.5 Robot lau nhà 17 Hình 1.6 Robot di động dùng quân 18 Hình 1.7 Các thành phần hệ thống Robot .19 Hình 2.1 Các hệ tọa độ robot 21 Hình 2.2 Mơ hình tác động lực lên robot 24 Hình 2.3 Sơ đồ điện chuyển mạch điện tử động BLDC 28 Hình 2.4 Mơ hình hàm truyền động BLDC 30 Hình 2.5 Mơ hình động học robot 32 Hình 3.1 Vệ tinh sử dụng kết nối điểm – điểm kết nối điểm – đa điểm 35 Hình 3.2 Truyền thông sử dụng tia hồng ngoại 37 Hình 3.3 Sơ đồ truyền thơng 39 Hình 3.4 Ăng ten thu Ubiquiti Rocket M2 40 Hình 3.5 Biểu đồ luồng liệu .41 Hình 3.7 Cấu trúc liệu lệnh gửi mạch tích hợp điều khiển 43 Hình 3.8 Các bước xử lý luồng liệu 43 Hình 4.1 Sơ đồ điều khiển hệ truyền động điện động bánh lái robot 44 Hình 4.2 Sơ đồ cấu trúc mạch vòng dòng điện .45 Hình 4.3 Sơ đồ mạch vòng tốc độ 46 Hình 4.4 Sơ đồ hệ thống điều khiển vị trí robot 46 Hình 4.5 Sơ đồ mơ hệ thống điều khiển robot di động 50 Hình 4.6: Bộ điều khiển vị trí tuyến tính hóa phản hồi 51 Hình 4.7 Mơ hình động BLDC 51 Hình 4.8 Sơ đồ mạch phát xung cho chuyển mạch 52 Hình 4.9 Mơ hình động học robot 52 Methodologies: A Unified Framework, College of Engineering, American University of Sharjah, Sharjah, UAE [11] Phan Vân Hoàn, Nghiên cứu kiểm chứng thuật toán bám tường cho robot tự hành, Đại học Sư Phạm Kỹ Thuật Tp Hồ Chí Minh [12] https://en.wikipedia.org/wiki/Shear strength (soil) [13] https://vi.wikipedia.org/wiki/Robot_di_%C4%91%E1%BB%99ng#cite_note-2 [14] http://www.cesti.vn/khong-gian-cong-nghe/robot-di-dong.html [15] http://indotech.vn/vi/2016/03/02/bang-tra-cac-he-so-ma-sat/ [16] https://vi.kisspng.com/kisspng-93jiii/ 69 Phụ lục 1: Chương trình xử lý liệu vị trí (Python) import socket import time import os import struct import threading import serial #import msvcrt #Du lieu dung chung lat,lon,speed class LocationData: 10 lat=0.0; 11 lon=0.0; 12 speed=0.0; 13 loc = LocationData(); 14 conn_finish =False; 15 read_finish =False; 16 def Read_Worker(): 17 ser = serial.Serial( 18 port = '/dev/ttyUSB0', 19 baudrate=9600, 20 parity=serial.PARITY_NONE, 21 stopbits=serial.STOPBITS_ONE, 22 bytesize=serial.EIGHTBITS, 23 timeout=1 24 ) 25 while True: 26 #strRMC ="$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1, W*6A" 27 strRMC = ser.readline().decode('utf-8'); 28 index = strRMC.find("$GPRMC"); 29 if index >=0: 30 31 data = strRMC.split(','); #if (data[2]=='A'): #Neu du lieu GPS la active thi cap nhat vi tri 32 #Chuyen doi du lieu lattitute 33 lat = float(data[3]); 34 lat1 = int(lat/100); 35 lat2 = lat - lat1*100; 70 36 loc.lat = lat1 + lat2/60; 37 #Chuyen doi du lieu longtitute 38 lon=float(data[5]); 39 lon1= int(lon/100); 40 lon2 = lon - lon1*100; 41 loc.lon = lon1+lon2/60; 42 #Chuyen doi du lieu speed 43 speed = float(data[7]); 44 loc.speed = speed *0.514444444; #convert from knot to m/s 45 print (loc.lat,loc.lon,loc.speed) 46 47 if (read_finish): break; 48 def Connection_Worker(): 49 maxbytes=1024 50 serversocket = socket.socket(socket.AF_INET,socket.SOCK_STREAM); 51 serversocket.bind(("192.168.1.30",1986)) 52 serversocket.listen(1); 53 print("Bat dau nhan lenh: "); 54 while True: 55 #Tao ket noi moi 56 if conn_finish==True: 57 serversocket.close(); 58 break; 59 connection, address = serversocket.accept() 60 print ("Chap nhan ket noi"); 61 #Thuc hien truyen du lieu 62 while True: 63 64 try: if connection.recv(1)==b'\xb5': 65 if connection.recv(1)==b'\xc6': 66 #print ("Da xac nhan"); 67 Class = connection.recv(1); 68 ID = connection.recv(1); 69 if Class==b'\x00': #Lenh dong ket noi 70 print ("Dong ket noi"); 71 connection.close(); 72 break; 73 74 75 if Class==b'\x01': #Lenh gui thong tin sz_Ret struct.pack('!3d',loc.lat,loc.lon,loc.speed); connection.send(sz_Ret); 71 = 76 #print ("Da chuyen du lieu vi tri"); 77 except (Exception) as error: 78 print(error) 79 connection.close() 80 break; 81 if name == " main ": 82 print ("Day la luong chinh"); 83 Readthread = threading.Thread(target=Read_Worker) 84 Readthread.start() #Khoi dong luong doc du lieu 85 print ("Da khoi dong luong doc du lieu"); 86 87 Connthread = threading.Thread(target=Connection_Worker) 88 Connthread.start() #Khoi dong luong Connection 89 print ("Da khoi dong luong ket noi"); 90 ''' 91 while True: 92 key = ord(msvcrt.getch()); 93 if key == 27: 94 conn_finish = True; 95 read_finish = True; 96 break; 97 ''' 98 print ("Cho luong worker tat di moi ket thuc"); 99 Readthread.join() # Cho luong read 100 Connthread.join() #Cho luong connection 72 Phụ lục 2: chương trình trung tâm điều khiển (C++) // ConsoleApplication1.cpp : Defines the entry point for the console application // #include "stdafx.h" #include "ConsoleApplication1.h" #include #include #include #define PI 3.1415926 10 double lat0 = 21.077046;//latitude diem dau 11 double lon0 = 105.775165;//lon diem dau 12 double lat1 = 21.077239;//latitude diem cuoi 13 double lon1 = 105.775159;//lon diem cuoi 14 double theta0 = atan((lon1 - lon0) / (lat1 - lat0));//goc quay dat 15 SOCKET Vec_socket, Loc_socket; 16 SOCKADDR_IN Vec_SocAddr, Loc_SocAddr; 17 18 int timeout = 3000; //in milliseconds this is seconds 19 //Sync char 20 #define Syn1 0xB5 21 #define Syn2 0xC6 22 23 #ifdef _DEBUG 24 #define new DEBUG_NEW 25 #endif 26 27 28 // The one and only application object 29 30 CWinApp theApp; 31 32 using namespace std; 33 34 ofstream loc_out("C:\\location.txt"); 35 ofstream v_out("C:\\status.txt"); 36 BOOL SendCommand(char Class, char ID, int iDistance, int iSpeed) 37 { 38 //Command 73 39 char sz_Command[12]; 40 sz_Command[0] = Syn1; 41 sz_Command[1] = Syn2; 42 sz_Command[2] = Class; 43 sz_Command[3] = ID; 44 sz_Command[4] = iDistance & 0x000000ff; 45 sz_Command[5] = (iDistance & 0x0000ff00) >> 8; 46 sz_Command[6] = (iDistance & 0x00ff0000) >> 16; 47 sz_Command[7] = (iDistance & 0xff000000) >> 24; 48 sz_Command[8] = iSpeed & 0x000000ff; 49 sz_Command[9] = (iSpeed & 0x0000ff00) >> 8; 50 sz_Command[10] = (iSpeed & 0x00ff0000) >> 16; 51 sz_Command[11] = (iSpeed & 0xff000000) >> 24; 52 if (send(Vec_socket, sz_Command, 12, 0) < 0) 53 { 54 //closesocket(Socket); 55 AfxMessageBox(_T("Truyen du lieu khong cong")); 56 return FALSE; 57 } 58 //AfxMessageBox(_T("Truyen du lieu cong")); 59 //else 60 //{ 61 //closesocket(Socket); 62 return TRUE; 63 } 64 BOOL Connect() 65 { 66 Vec_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 67 Vec_SocAddr.sin_addr.s_addr = inet_addr("192.168.1.151"); 68 Vec_SocAddr.sin_port = htons(1986); 69 Vec_SocAddr.sin_family = AF_INET; 70 char sz_Command[12] = { }; 71 sz_Command[0] = Syn1; 72 sz_Command[1] = Syn2; 73 sz_Command[2] = 0x01; 74 if (connect(Vec_socket, sizeof(Vec_SocAddr)) != 0) 75 { (SOCKADDR*)(&Vec_SocAddr), 76 AfxMessageBox("Ket noi khong cong"); 77 closesocket(Vec_socket); 78 return FALSE; 74 79 80 } 81 else 82 { 83 84 //Dat thong so cho socket setsockopt(Vec_socket, SOL_SOCKET, SO_RCVTIMEO, *)&timeout, sizeof(int)); //setting the receive timeout 85 //Gui message test 86 if (send(Vec_socket, sz_Command, 12, 0) < 0) 87 { (char 88 AfxMessageBox(_T("Truyen du lieu khong cong")); 89 closesocket(Vec_socket); 90 return FALSE; 91 92 } 93 else 94 { 95 char ret = 0; 96 int msglen = recv(Vec_socket, &ret, 1, 0); 97 //char szNote[10]; 98 //sprintf_s(szNote, _T("%d %d"), msglen, ret); 99 //AfxMessageBox(szNote); 100 if ((msglen == 1) && (ret == 0x01)) 101 { 102 AfxMessageBox(_T("Ket noi cong")); 103 return TRUE; 104 105 } 106 else 107 { 108 AfxMessageBox(_T("Khong nhan duoc phan hoi")); 109 closesocket(Vec_socket); 110 return FALSE; 111 } 112 113 } } 114 115 } 116 117 double Do_kc(double lat1, double lon1, double lat2, double lon2)//tinh khoang cach m 75 118 { 119 double dlat = (lat2 - lat1)*PI / 180; 120 double dlon = (lon2 - lon1)*PI / 180; 121 double rlat1 = lat1*PI / 180; 122 double rlat2 = lat2*PI / 180; 123 double a = sin(dlat / 2)*sin(dlat cos(rlat1)*cos(rlat2)*sin(dlon / 2)*sin(dlon / 2); 124 double c = * atan2(sqrt(a), sqrt(1 - a)); 125 double d = 6371.0 * c * 1000; 126 return d; / 2) + 127 } 128 double sign(double a)//ham lay dau 129 { 130 if (a >= 0) 131 return 1; 132 else return -1; 133 } 134 int _tmain(int argc, TCHAR* argv[], TCHAR* envp[]) 135 { 136 int nRetCode = 0; 137 138 HMODULE hModule = ::GetModuleHandle(NULL); 139 140 if (hModule != NULL) 141 { 142 // initialize MFC and print and error on failure 143 if (!AfxWinInit(hModule, NULL, ::GetCommandLine(), 0)) 144 { 145 146 // TODO: change error code to suit your needs _tprintf(_T("Fatal failed\n")); 147 Error: MFC initialization nRetCode = 1; 148 } 149 else 150 { 151 // TODO: code your application's behavior here 152 union { char b_lat[8]; double d_lat; }; 153 union { char b_lon[8]; double d_lon; }; 154 union { char b_speed[8]; double d_speed; }; 155 //Khoi tao socket 156 WSADATA wsaData; 76 157 if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0){ 158 system("pause"); 159 } 160 //Ket noi dieu khien xe 161 162 while (true) 163 { 164 if (Connect()) 165 { 166 break; 167 } 168 else 169 { 170 int msg = AfxMessageBox("Dong chi co muon ket noi lai khong?", MB_YESNOCANCEL); 171 if (msg == IDNO) break; 172 else if (msg == IDCANCEL) 173 { 174 break; 175 } 176 } 177 } 178 179 //Ket noi lay vi tri 180 Loc_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 181 Loc_SocAddr.sin_addr.s_addr inet_addr("192.168.1.31"); = 182 Loc_SocAddr.sin_port = htons(1986); 183 Loc_SocAddr.sin_family = AF_INET; 184 if (connect(Loc_socket, sizeof(Loc_SocAddr)) != 0) 185 (SOCKADDR*)(&Loc_SocAddr), { 186 cout