Điều khiển mặt động thích nghi bám quỹ đạo cho robot tự hành bốn bánh đa hướng

BỘ GIÁO DỤC VÀ ĐÀO TẠO HỌC VIỆN KHOA HỌC VÀ CÔNG NGHỆ …… ….***………… HÀ THỊ KIM DUYÊN ĐIỀU KHIỂN MẶT ĐỘNG THÍCH NGHI BÁM QUỸ ĐẠO CHO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG LUẬN ÁN TIẾN SĨ KỸ THUẬT ĐIỆN, ĐIỆN TỬ VÀ VIỄN THÔNG Hà Nội - 2020 i VIỆN HÀN LÂM KHOA HỌC VÀ CÔNG NGHỆ VIỆT NAM HỌC VIỆN KHOA HỌC VÀ CÔNG NGHỆ …… ….***………… HÀ THỊ KIM DUYÊN ĐIỀU KHIỂN MẶT ĐỘNG THÍCH NGHI BÁM QUỸ ĐẠO CHO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG Chuyên ngành : Kỹ thuật điều khiển tự động hóa Mã số: 52 02 16 LUẬN ÁN TIẾN SĨ KỸ THUẬT ĐIỆN, ĐIỆN TỬ VÀ VIỄN THÔNG Người hướng dẫn khoa học: GS.TS Phan Xuân Minh TS Phạm Văn Bạch Ngọc Hà Nội – 2020 ii LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi, hoàn thành hướng dẫn GS.TS Phan Xuân Minh TS Phạm văn Bạch Ngọc Các kết nêu luận án trung thực chưa cơng bố cơng trình khác Tôi xin chịu trách nhiệm lời cam đoan Hà nội, tháng 10 năm 2020 Tác giả Hà Thị Kim Duyên iii LỜI CẢM ƠN Luận án hồn thành với nỗ lực khơng ngừng tác giả giúp đỡ từ thầy giáo hướng dẫn, bạn bè người thân Đầu tiên, tác giả xin bày tỏ lời tri ân tới GS.TS Phan Xuân Minh TS Phạm Văn Bạch Ngọc, giáo, thầy giáo tận tình hướng dẫn tác giả hoàn thành luận án Tác giả xin gửi lời cảm ơn tới thầy, cô giáo cán Viện Công nghệ thông tin, Học viện Khoa học Công nghệ (Viện Hàn lâm Khoa học Cơng nghệ Việt Nam) nhiệt tình giúp đỡ tạo môi trường nghiên cứu tốt để tác giả hồn thành cơng trình mình; cảm ơn thầy, cô đồng nghiệp nơi mà tác giả tham gia viết có góp ý xác để tác giả có công bố ngày hôm Tác giả xin cảm ơn tới Ban Giám hiệu trường Đại học Công nghiệp Hà Nội, đồng nghiệp khoa Điện tử nơi tác giả công tác ủng hộ để luận án hoàn thành thời hạn Cuối cùng, tác giả xin gửi tới gia đình, bạn bè, người thân lời cảm ơn chân thành đồng hành tác giả suốt thời gian qua Hà Nội, tháng 10 năm 2020 Nghiên cứu sinh Hà Thị Kim Duyên iv MỤC LỤC Trang phụ bìa……… ………………………………………………… ……… i LỜI CAM ĐOAN ii LỜI CẢM ƠN iii MỤC LỤC iv DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT vii BẢNG CÁC KÝ HIỆU CÁC THAM SỐ viii DANH MỤC CÁC BẢNG ix DANH MỤC HÌNH VẼ xi MỞ ĐẦU CHƯƠNG TỔNG QUAN VỀ ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG 1.1 Robot tự hành di chuyển bánh đa hướng 1.2 Bài toán điều khiển bám quỹ đạo 1.2.1 Giai đoạn lập phương án chuyển động 1.2.2 Giai đoạn thiết kế quỹ đạo 1.2.3 Điều khiển bám theo quỹ đạo chuyển động 1.3 Tổng quan tình hình nghiên cứu ngồi nước 1.3.1 Tình hình nghiên cứu nước 1.3.2 Tình hình nghiên cứu ngồi nước 11 1.4 Kết luận chương 16 CHƯƠNG MƠ HÌNH HĨA VÀ THUẬT TỐN ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG 17 2.1 Xây dựng mô hình động học, động lực học cho robot tự hành bốn bánh đa hướng 17 2.1.1 Bánh xe Omni 17 2.1.2 Mơ hình động học robot tự hành bốn bánh đa hướng [41], [42] 19 2.1.3 Mơ hình động lực học robot tự hành bốn bánh đa hướng [41], [42] 22 2.1.4 Mô kiểm chứng kết mơ hình hóa mơ hình robot 23 2.2 Một số thuật toán điều khiển bám quỹ đạo cho robot tự hành bốn bánh đa hướng thông dụng 24 2.2.1 Bộ điều khiển PID cho FWOMR 25 v 2.2.2 Bộ điều khiển trượt cho FWOMR 26 2.2.3 Bộ điều khiển đa mặt trượt cho FWOMR 28 2.2.4 Mô kiểm chứng thuật toán 31 2.3 Kết luận chương 35 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN THÍCH NGHI BÁM QUỸ ĐẠO CHO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG 36 3.1 Thuật toán điều khiển mặt trượt động 36 3.1.1 Xây dựng thuật toán điều khiển bám quĩ đạo mặt trượt động cho FWOMR… 36 3.1.2 Mô kiểm chứng kết thuật toán 40 3.2 Thuật tốn điều khiển mặt trượt động thích nghi mờ điều khiển bám quỹ đạo cho FWOMR 44 3.2.1 Thuật toán điều khiển mặt trượt động thích nghi mờ 44 3.2.2 Mơ kiểm chứng thuật tốn 47 3.3 Thuật toán điều khiển mặt trượt động thích nghi nơ ron mờ điều khiển bám quỹ đạo cho FWOMR 50 3.3.1 Xấp xỉ thành phần bất định mơ hình FWOMR sử dụng mạng nơ ron bán kính xuyên tâm 50 3.3.2 Xây dựng luật mờ cho AFNNDSC 55 3.3.3 Kết mô 56 3.4 Kết luận chương 62 CHƯƠNG CHẾ TẠO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG VÀ CHẠY THỬ NGHIỆM THUẬT TOÁN ĐIỀU KHIỂN 64 4.1 Thiết kế chế tạo robot tự hành bốn bánh đa hướng 64 4.1.1 Thiết kế cấu, khí xây dựng Omni thực tế 64 4.1.2 Thiết kế cấu trúc mạch điều khiển cho robot 65 4.1.3 Phần mềm điều khiển cho robot 66 4.2 Cài đặt thuật toán chạy thử nghiệm 67 4.2.1 Lập trình phần mềm nhúng vi xử lý thuật toán điều khiển 67 4.2.2 Kết robot môi trường Gazebo Rviz để mô kiểm chứng…… 69 4.2.3 Kết chạy thực nghiệm thực tế 70 vi 4.3 Kết luận chương 73 KẾT LUẬN VÀ KIẾN NGHỊ 74 DANH MỤC CƠNG TRÌNH CỦA TÁC GIẢ 76 TÀI LIỆU THAM KHẢO 78 PHỤ LỤC 86 vii DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT Chữ viết tắt NCS FWOMR OMR RBFNN DSC AFDSC ANNDSC AFNNDSC MSSC PID PC SMC VSS MCU FLS ROS viii BẢNG CÁC KÝ HIỆU CÁC THAM SỐ Ký hiệu ( x , y, θ ) θ vi ωi f i M d H vx vy r M(q) G(q) τd e1 e2 J q F C G α x 1d T λ τ τ eq PL14 Hình PL3.7 Mạch in thiết kế Altium 3.3.2 Mạch driver động DC Servo Mạch cầu H dùng IC kích FET chuyên dụng IR2184 cho phép điều khiển động với dòng liên tục 10A dòng tức thời lên đến 30A Mạch thiết kế dành cho ứng dụng điều khiển tốc độ vị trí cho động DC Hình PL3.1 Sơ đồ nguyên lý mạch cầu H driver động DC Servo PL15 Mosfet sử dụng mạch IRF840 cho dòng đỉnh tối đa 22A Điện áp hoạt động lên tới 500VDC Dòng liên tục 8A chế độ cầu kép Điện áp cấp cho FET cao dịng nhỏ Với ứng dụng cầu FET cho robot điện áp cấp thường 24V~25,6V Chu kỳ xung nhỏ cho dịng qua FET lớn, nên băm xung tần số cao có lợi cơng suất hơn, robot chạy nhanh khỏe Sử dụng IC kich FET chuyên dụng IR2184 bảo đảm FET dẫn tốt, chống tượng trùng dẫn có hãm động tốt Diode sử dụng cho IR2184 phải có tần số hoạt động cao Driver có biến trở dùng chỉnh dịng giới hạn qua động giúp bảo vệ thiết bị Tín hiệu vào điều khiển bao gồm: DIR+, DIR-, PWM+, PWM- Điều giúp người dùng dễ dàng tùy chọn tín hiệu điều khiển tác động mức cao hay mức thấp Board gồm led SMD báo nguồn báo chiều quay động Hình PL3.9 Mạch cầu H dùng để điểu khiển động 3.3.3 Khối nguồn cung cấp: Hình PL3.10 Mạch nguồn cung cấp 5V PL16 Bên cạnh có sử dụng mạch có IC LM2576-5 để hạ áp từ 12V xuống 3.3V ưu điểm :  Bản thân IC nguồn Switching nguyên khối nên cần thêm vài linh kiện điện tử  Điện áp vào dải từ 7VDC-40VDC, dòng lên đến 3A, dòng áp ổn định  Sự tiêu hao lượng thấp Hình PL3.11 Sơ đồ nguyên lý mạch nguồn 3.3V 3.3.4 Các khối giao tiếp  Khối giao tiếp RS232 RF: Hình PL3.12 Mạch giao tiếp RS232 PL17  Khối mạch giao tiếp khơng dây RF Hình 3.13 Mạch truyền thơng COM to USB Hình PL3.14 Mạch truyền thơng RF  Khối giao tiếp blutooth HC05 Bluetooth chuẩn truyền thông không dây để trao đổi liệu khoảng cách ngắn Chuẩn truyền thơng sử dụng sóng radio ngắn (UHF radio) dải tần số ISM (2.4 tới 2.485 GHz) Khoảng cách truyền module vào khoảng 10m Module HC-05 thiết kế dựa chip BC417 sử dụng nhớ flash ngồi 8Mbit Hình PL3.15 Sơ đồ modul HC - 05 HC-05 có hai chế độ hoạt động Command Mode Data Mode Ở chế độ Command Mode ta giao tiếp với module thông qua cổng serial module PL18 tập lệnh AT quen thuộc Ở chế độ Data Mode module truyền nhận liệu tới module bluetooth khác Chân KEY dùng để chuyển đổi qua lại hai chế độ Ở chế độ SLAVE: Ta cần thiết lập kết nối từ smartphone, laptop, usb bluetooth để dò tìm module sau pair với mã PIN 1234 Sau pair thành cơng, ta có cổng serial từ xa hoạt động baud rate 9600 Ở chế độ MASTER: module tự động dị tìm thiết bị bluetooth khác (1 module bluetooth HC-05, usb bluetooth, bluetooth laptop ) tiến hành pair chủ động mà khơng cần thiết lập từ máy tính smartphone 3.4 Code lập trình điều khiển robot tự hành bốn bánh đa hướng 3.4.1 Code vi điều khiển cho robot Phần mềm viết code cho vi điều khiển STM32F103C8T6 IAR Hình PL3.16 Giao diện phần mềm IAR Chương trình lập trình vi điều khiển: /* Includes */ #include "main.h" /* Private includes */ /* USER CODE BEGIN Includes */ #include "stdio.h" #include "stdlib.h" #include "string.h" PL19 #include "math.h" /* USER CODE END Includes */ /* USER CODE BEGIN PV */ / UART variable uint8_t rx_data; char rx_index; char rx_buffer[20]; char temp[100]; int check_var = 0; //encoder variable int pulse_counter1 = 0, pulse_counter2 = 0, pulse_counter3 = 0, pulse_counter4 = 0; //velocity variable double k = sqrt(2)/2; float vx = 0, vy = 0, w_angle = 0; float w_reference1_temp = 0; float w_reference2_temp = 0; float w_reference3_temp = 0; float w_reference4_temp = 0; float w_reference1 = 0, w_reference2 = 0, w_reference3 = 0, w_reference4 = 0; float w1 = 0, w2 = 0, w3 = 0, w4 = 0; float e1 = 0, e2 = 0, e3 = 0, e4 = 0; float pre_e1 = 0, pre_e2 = 0, pre_e3 = 0, pre_e4 = 0; float const max_pwm = 300; //robot variable float d_robot = 0.25; //robot radius float r_wheel = 0.07; /* Private function prototypes -*/ void SystemClock_Config(void); PL20 static void MX_GPIO_Init(void); static void MX_TIM3_Init(void); static void MX_TIM2_Init(void); static void MX_USART3_UART_Init(void); static void MX_TIM9_Init(void); static void MX_TIM12_Init(void); static void MX_TIM5_Init(void); /* USER CODE BEGIN PFP */ void check_w_ref() { sprintf(temp, "vx_ref = %.2f\n", vx); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "vy_ref = %.2f\n", vy); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w_ref = %.2f\n", w_angle); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w1_temp = %.2f, w1_ref = %.2f\n", w_reference1_temp ,w_reference1); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w2_temp = %.2f, w2_ref = %.2f\n", w_reference2_temp ,w_reference2); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w3_temp = %.2f, w3_ref = %.2f\n", w_reference3_temp ,w_reference3); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w4_temp = %.2f, w4_ref = %.2f\n", w_reference4_temp PL21 ,w_reference4); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); HAL_Delay(2000); } void signal_feedback_w_wheel() { sprintf(temp, "w1 = %.2f rpm r\n", w1); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w2 = %.2f rpm r\n", w2); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w3 = %.2f rpm r\n", w3); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(100); sprintf(temp, "w4 = %.2f rpm r\n", w4); HAL_UART_Transmit(&huart3, temp, strlen(temp), 10); HAL_Delay(2000); } int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_TIM3_Init(); MX_TIM2_Init(); MX_USART3_UART_Init(); MX_TIM9_Init(); MX_TIM12_Init(); MX_TIM5_Init(); HAL_TIM_Base_Start_IT(&htim2); PL22 HAL_UART_Receive_IT(&huart3, &rx_data, 1); Init_Motor1(); Init_Motor2(); Init_Motor3(); Init_Motor4(); /* USER CODE END */ } /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOE, M01_EN_R_Pin|M01_EN_L_Pin|M02_EN_L_Pin|M02_EN_R_Pin | M02_3_3V_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(M01_3_3V_GPIO_Port, M01_3_3V_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(M01_GND_GPIO_Port, M01_GND_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(M02_GND_GPIO_Port, M02_GND_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOD, M03_EN_R_Pin|M03_EN_L_Pin|M03_3_3V_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(M03_GND_GPIO_Port, M03_GND_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(M04_GND_GPIO_Port, M04_GND_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOG, M04_3_3V_Pin|M04_EN_L_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(M04_EN_R_GPIO_Port, M04_EN_R_Pin, GPIO_PIN_SET); *****END OF FILE****/ 3.4.2 Code lập trình ROS cho TX2 điều khiển cho robot - Node lấy tín hiệu từ Lidar PL23 type="string" Hình PL3.17 Cấu trúc lập trình phần mềm PL24 - Node lấy tín hiệu từ Camera Astra /@/ "15120410023" : serial number > - Node tính tốn vị trí robot type="rf2o_laser_odometry_node" PL25 # topic where the lidar scans are being published # topic where tu publish the odometry estimations # wheter or not to publish the tf::transform (base->odom) # frame_id (tf) of the mobile robot base A tf transform from the laser_frame to the base_frame is mandatory # frame_id (tf) to publish the odometry estimations # (Odom topic) Leave empty to start at point (0,0) # Execution frequency # verbose - Node tính tốn điều khiển PL26 PL27 > ... động thích nghi bám quỹ đạo cho robot tự hành bốn bánh đa hướng? ?? Nội dung nghi? ?n cứu luận án tập trung vào tổng hợp điều khiển cho robot tự hành bốn bánh đa hướng (FWOMR) dạng holonomic có khả bám. .. THUẬT TỐN ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHO ROBOT TỰ HÀNH BỐN BÁNH ĐA HƯỚNG 17 2.1 Xây dựng mơ hình động học, động lực học cho robot tự hành bốn bánh đa hướng 17 2.1.1 Bánh xe Omni... giá rút học kinh nghi? ??m việc nghi? ?n cứu đề xuất thuật toán điều khiển bám quỹ đạo thích nghi Chương 3: "Thiết kế điều khiển thích nghi bám quỹ đạo cho robot tự hành bốn bánh đa hướng? ?? Đây đóng