TRƯỜNG ĐẠI HỌC BÁCH KHOA TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG NGUYỄN ĐÌNH THƠNG LUẬN VĂN TỐT NGHIỆP THIẾT KẾ BỘ ĐIỀU KHIỂN VẬN TỐC VÀ VỊ TRÍ CHO QUADCOPTER SỬ DỤNG CẢM BIẾN OPTICAL FLOW KỸ SƯ NGÀNH KỸ THUẬT ĐIỀU KHIỂN & TỰ ĐỘNG HÓA Note: Some pages (forewords, thesis overview, ) was removed from the original version, the main content is kept the same MỤC LỤC TRƯỜNG ĐẠI HỌC BÁCH KHOA TP HỒ CHÍ MINH MỤC LỤC DANH MỤC HÌNH VẼ DANH MỤC BẢNG 10 DANH MỤC TỪ VIẾT TẮT 11 TÓM TẮT LUẬN VĂN MỞ ĐẦU Chương Giới thiệu .4 1.1 Giới thiệu chung UAV quadcopter 1.2 Tình hình nghiên cứu UAV .5 1.3 Mục tiêu nội dung luận văn 1.3.1 Lý chọn đề tài 1.3.2 Tính thiết thực đề tài luận văn 1.3.2.1 Bộ điều khiển 1.3.2.2 Điều khiển quadcopter Optical Flow 1.3.3 Mục tiêu nội dung 10 Chương Hệ thống cảm biến phần cứng 11 2.1 Sơ đồ kết nối hệ thống 11 2.2 Phần cứng 12 2.2.1 Pin Lipo 12 2.2.2 Động BLDC 14 2.2.3 Bộ điều khiển ESC 15 2.2.4 Cánh quạt 15 2.2.5 Tay cầm DEVO7 16 2.2.6 Đọc tín hiệu PPM 17 2.2.7 Khung quadcopter 18 2.3 Hệ thống cảm biến 19 2.3.1 Cảm biến vận tốc góc gia tốc MPU6050 19 2.3.2 Cảm biến từ trường HMC5883 21 2.3.3 Cảm biến độ cao dùng sóng siêu âm HC-SR04 21 2.3.4 Cảm biến Optical Flow PX4FLOW 22 2.3.4.1 Giới thiệu thông số kỹ thuật PX4FLOW 22 2.3.4.2 Calib cảm biến px4flow 25 2.3.4.3 Giải thuật xử lý giữ liệu px4flow 25 2.4 Mạch Altium 27 2.4.1 Sơ đồ mạch Schematics 28 2.4.2 Sơ đồ mạch PCB 29 Chương Cơ sở lý thuyết 32 3.1 Lập trình vi điều khiển ARM 32 3.1.1 Bo mạch STM32F407 Discovery vi điều khiển 32 bits ARM Cortex M4 33 3.1.2 Lập trình vi điều khiển ARM Cortex M4 34 3.1.2.1 Thư viện lập trình 34 3.1.2.2 Hệ thống ngắt vi điều khiển ARM Cortex M4 35 3.1.2.3 Giao tiếp I2C 39 3.2 Đơn vị ước lượng trạng thái 41 3.2.1 Quy ước hệ trục tọa độ 42 3.2.2 Quaternion 43 3.2.2.1 Đại số quaternion 44 3.2.2.2 Phương trình động học dạng quaternion 47 3.2.3 Giải thuật Mahony Filter 48 3.2.4 Bộ lọc thông thấp 51 3.2.4.1 Giải thuật lọc bù 52 3.2.4.2 Giải thuật lọc trung bình ( moving averaging filter ) 52 3.3 Xây dựng phương trình tốn động lực học 54 3.3.1 Mơ hình động cánh quạt 54 3.3.1.1 Hệ thống động cơ, cánh quạt ESC 54 3.3.1.2 Mối liên hệ tốc độ động xung PWM 55 3.3.1.3 Mối liên hệ tốc độ động lực tạo 55 3.3.1.4 Mối liên hệ tốc độ động điện áp cấp cho động 56 3.3.2 Phương tình động lực học quadcopter 56 3.3.3 Phương trình động lực học vật rắn 58 3.3.4 Phương trình tuyến tính hóa quanh điểm làm việc 60 Chương Thiết kế điều khiển 60 4.1 Lý thuyết điều khiển PID 60 4.1.1 Bộ điều khiển PID tổng quát 60 4.1.2 Bộ điều khiển cascade PID 61 4.2 Áp dụng điều khiển cho hệ thống quadcopter 62 4.2.1 Bộ điều khiển góc roll pitch yaw 62 4.2.1.1 Bộ điều khiển góc roll, pitch, yaw 62 4.2.1.2 Áp dụng thuật toán điều khiển roll, pitch, yaw 62 4.2.2 Bộ điều khiển độ cao 65 4.2.2.1 Chế độ tự động cất cánh xác định thrust hover 65 4.2.2.2 Thiết kế điều khiển giữ độ cao 65 4.2.3 Bộ điều khiển vận tốc vị trí Optical Flow 67 4.2.3.1 Chuyển đổi vị trí Reference Frame Body Frame 67 4.2.3.2 Bộ điều khiển vận tốc 70 4.2.3.3 Bộ điều khiển giữ vị trí 72 Chương Kết hệ thống hướng phát triển đề tài 73 5.1 Kết chất lượng hệ thống 73 5.1.1 Hệ thống quadcopter xây dựng 73 5.1.2 Kết cân roll pitch yaw 75 5.1.3 Kết giữ độ cao 77 5.1.4 Kết giữ vận tốc vị trí dùng Optical Flow 77 5.2 Hướng phát triển đề tài 79 5.2.1 Sử dụng RTOS 80 5.2.2 Áp dụng lọc Extended Kalman Filter GPS 80 5.2.3 Tích hợp máy tính nhúng 80 TÀI LIỆU THAM KHẢO 82 DANH MỤC HÌNH VẼ Hı̀nh 1.1.1 Máy bay phản lực chiến đấu Sukhoi Su-57 Hı̀nh 1.1.2 Quadcopter sử dụng mạch bay DJI Naza Hı̀nh 1.2.1 Mạch điều khiển Pixhawk 4, sử dụng firmware Ardupilot PX4 Hı̀nh 1.2.2 Ứng dụng Drone để phục vụ cho nông nghiệp khoa Kỹ thuật hàng không Đại học bách khoa TP HCM Hı̀nh 1.2.3 Drone quay phim chụp hình DJI Phantom Hı̀nh 1.2.4 Luận văn hệ thống định vị nhà với độ xác 2cm sinh viên Bách Khoa HCM Hı̀nh 1.3.1 Hệ thống quadcopter có tích hợp GPS Hı̀nh 2.1.1 Thành phần phần cứng đầy đủ hệ thống 11 Hı̀nh 2.1.2 Sơ đồ kết nối giao tiếp thành phần quadcopter 12 Hı̀nh 2.2.1 Pin Phantom LiPo 13 Hı̀nh 2.2.2 Đồ thị điện áp thời gian thể đặc tuyến xả pin LiPo 6C 14 Hı̀nh 2.2.3 Động BLDC 14 Hı̀nh 2.2.4 Bộ điều khiển ESC cho động BLDC 15 Hı̀nh 2.2.5 Cánh quạt cho quadcopter 16 Hı̀nh 2.2.6 Bộ điều khiển DEVO 16 Hı̀nh 2.2.7 Bộ chuyển đổi cho RX DEVO7 PWM to PPM 17 Hı̀nh 2.2.8 Sơ đồ tín hiệu xung PPM 18 Hı̀nh 2.2.9 Khung quadcopter S500 Carbon Fiber Arms 19 Hı̀nh 2.3.1 Cảm biến MPU6050 20 Hı̀nh 2.3.2 Cảm biến từ trường HMC5883 21 Hı̀nh 2.3.3 Giãn đồ đọc khoảng cách từ cảm biến sóng siêu âm HC-SR04 22 Hı̀nh 2.3.4 Cảm biến px4flow 23 Hı̀nh 2.3.5 Damping foam cho px4flow 23 Hı̀nh 2.3.6 Đồ thị kết calib px4flow trục y 25 Hı̀nh 2.3.7 Hình hoạ thay đổi góc roll, pitch ảnh hưởng lên flow nhận 26 Hı̀nh 2.3.8 Sự ảnh hưởng độ cao tới độ dịch chuyển thực tế 27 Hı̀nh 2.4.1 Mạch Schematics 28 Hı̀nh 2.4.2 PCB lớp bottom 29 Hı̀nh 2.4.3 PCB lớp top 30 Hı̀nh 2.4.4 Cả hai lớp 31 Hı̀nh 2.4.5 Lớp bottom mạch PCB gia công thực tế 32 Hı̀nh 3.1.1 Mạch phát triển STM32F4 Discovery 33 Hı̀nh 3.1.2 Mơ hình hệ thống firmware dùng thư viện CMSIS vi điều khiển ARM-Cortex M4 34 Hı̀nh 3.1.3 Các thư viện hỗ trợ lập trình chip STM32 mức trừu tượng tương ứng 35 Hı̀nh 3.1.4 Sơ đồ khối NVIC minh họa ngắt GPIO vi điều khiểm ARM Cortex M4 36 Hı̀nh 3.1.5 Một phần bảng vector ngắt chip STM32F407VG 37 Hı̀nh 3.1.6 Chế độ chọn nhóm ngắt NVIC 38 Hı̀nh 3.1.7 Mơ hình kết nối phần cứng giao tiếp I2C 39 Hı̀nh 3.1.8 Giãn đồ đọc liệu px4flow dùng I2C 40 Hı̀nh 3.2.1 Quy ước hệ tọa độ BF EF 42 Hı̀nh 3.2.2 Quy ước chiều góc xoay 43 Hı̀nh 3.2.3 Minh họa phép xoay dùng đại số quaternion 46 Hı̀nh 3.2.4 Lưu đồ giải thuật Mahony Filter 49 Hı̀nh 3.2.5 Đồ thị đáp ứng miền tần số lọc thông thấp 51 Hı̀nh 3.3.1 Hệ thống điều khiển động BLDC 54 Hı̀nh 3.3.2 Quy ước số thứ tự chiều quay động 58 Hı̀nh 4.1.1 Sơ đồ tổng quát điều khiển cascade PID 61 Hı̀nh 4.2.1 Sơ đồ điều khiển góc roll, pitch, yaw 62 Hı̀nh 4.2.2 Sơ đồ điều khiển PID độ cao 66 Hı̀nh 4.2.3 Hệ tọa độ phương x-y quadcopter 68 Hı̀nh 4.2.4 Chuyển đổi vận tốc từ BF sang RF 69 Hı̀nh 4.2.5 Chuyển đổi tọa độ vị trí từ RF sang BF 70 Hı̀nh 4.2.6 Sơ đồ điều khiển vận tốc 70 Hı̀nh 4.2.7 Sơ đồ điều khiển vị trí 72 Hı̀nh 5.1.1 Hệ thống quadcopter thiết kế 73 Hı̀nh 5.1.2 Mặt quadcopter 74 Hı̀nh 5.1.3 Hệ thống quadcopter hoạt động không 75 Hı̀nh 5.1.4 Kết điều khiển góc roll, pitch 76 Hı̀nh 5.1.5 Kết điều khiển tốc độ góc 76 Hı̀nh 5.1.6 Kết giữ độ cao dùng cảm biến siêu âm 77 Hı̀nh 5.1.7 Kết giữ vị trí với px4flow 79 Hı̀nh 5.1.8 Kết điều khiển giữ vị trí với px4flow theo thời gian 79 Hı̀nh 5.2.1 Mơ hình tích hợp máy tính nhúng Odroid điều khiển bay Pixhawk 81 DANH MỤC BẢNG Bả ng 2.1 Các liệu px4flow i2c integral frame 25 Bả ng 5.1 Bảng thông số điều khiển roll, pitch, yaw 76 Bả ng 5.2 Bảng thông số điều khiển độ cao 77 Bả ng 5.3 Bảng thông số điều khiển vận tốc vị trí 78 Hı̀nh 4.2.3 Hệ tọa độ phương x-y quadcopter Chọn hệ trục tọa độ RF (hệ tọa độ tham chiếu làm mốc), chiều dương góc yaw chiều ngược kim đồng hồ Tại thời điểm chuyển sang trạng thái giữ vị trí, gán mốc thời gian ban đầu t=0 , ta chọn góc 𝜓 = 𝜓 = , 𝜓 = Đồng thời reset giá trị tọa độ RF BF Hệ tọa độ gốc RF 𝑋 − 𝑌 mô tả hình Chuyển giá trị vận tốc từ BF sang RF Tại thời điểm bất kỳ, góc yaw quadcopter thay đổi, ta tính góc 𝜓 68 Hı̀nh 4.2.4 Chuyển đổi vận tốc từ BF sang RF Áp dụng công thức Euler cho đại lượng dài vận tốc, ta có vận tốc quadcopter RF là: 𝑣 =𝑣 𝑒 −𝜓 xoay vector vận tốc theo chiều ngược chiều kim đồng hồ Tính vị trí RF 𝑝 = 𝑣 (𝜏)𝑑𝜏 Tính giá trị vị trí BF 69 Hı̀nh 4.2.5 Chuyển đổi tọa độ vị trí từ RF sang BF Góc tọa độ 𝑋 − 𝑌 trùng với gốc tọa độ chọn 𝑋 − 𝑌 RF Áp dụng cơng thức Euler cho đại lượng dài vị trí, ta có vị trí quadcopter BF 𝑝 =𝑝 𝑒 Đây giá trị vị trí cho lượng bù góc roll pitch tương ứng phân tích phần 4.2.3.2 Bộ điều khiển vận tốc Hı̀nh 4.2.6 Sơ đồ điều khiển vận tốc 𝑒 =𝑣 −𝑣 70 Áp dụng điều khiển PID 𝑒 (𝜏)𝑑𝜏 + 𝑘 𝑢 =𝑘 𝑒 +𝑘 𝑑𝑒 𝑑𝑡 Từ hai phương trình kết sau từ mục (3.3.2): 𝑥̈ = −𝑠 𝑇 𝑚 𝑦̈ = 𝑠 𝑐 𝑇 𝑚 Ta điều khiển gia tốc quadcopter từ phương trình trên, 𝑢 ngõ điều khiển PID vận tốc là: −𝑠 𝑇 𝑢 𝑢 = 𝑢 = 𝑚 = 𝑠 𝑐 𝑇 𝑇 𝑚 −𝑠 𝑠 𝑐 𝑚 𝑇 tính từ điều khiển độ cao thiết kể , điều khiển vận tốc trục x, y thực cách điều khiển góc 𝜙, 𝜃, nhờ điều khiển ổn định góc thiết kế mục (4.2.1) Ta có: 𝜃 𝜙 = 𝑎𝑟𝑐𝑠𝑖𝑛 = 𝑎𝑟𝑐𝑠𝑖𝑛 𝑢 𝑚 𝑇 − sin (𝜃 ) 𝑢 𝑚 𝑇 𝑢 𝑚 = 𝑎𝑟𝑐𝑠𝑖𝑛 ⎛ ⎝ 𝑇 1− ⎞ ⎠ Do đặc tính điều khiển hệ thống ta khơng thể đưa giá trị đặt thay đổi nhanh làm hệ thống đáp ứng khơng kịp, nên hình sơ đồ điều khiển cần lọc thích hợp để hệ thống có chất lượng đáp ứng tốt 71 4.2.3.3 Bộ điều khiển giữ vị trí Từ lý thuyết điều khiển cascade PID mục (4.1.2), điều khiển vận tốc phương x, y xây dựng trên, ta xây dựng điều khiển vị trí Hı̀nh 4.2.7 Sơ đồ điều khiển vị trí Áp dụng cơng thức tích phân để nhận vị trí 𝑝= 𝑣𝑑𝑡 Cơng thức tính tọa độ phương ngang miền rời rạc 𝑝 (𝑘) 𝑝 (𝑘 − 1) 𝑣 (𝑘) = + 𝑇 𝑝 (𝑘) 𝑝 (𝑘 − 1) 𝑣 (𝑘) Sai số vị trí 𝑝 =𝑝 −𝑝 Vịng ngồi điều khiển có thành phần khâu tỷ lệ P đơn giản, hệ số chuyển đổi 𝐾 văn 𝐾 chuyển sai lệch vị trí sang vận tốc bù tương ứng, luận chọn cho có thay đổi vị trí 100𝑐𝑚 vận tốc đặt để bù lại 0.55 𝑚/𝑠, áp dụng công thức chuyển đổi 𝑣 𝑣 = 𝑣 =𝐾 𝑝 72 Chương Kết hệ thống hướng phát triển đề tài 5.1 Kết chất lượng hệ thống Thể chất lượng hệ thống thiết kế điều khiển, đồ thị vẽ Matlab 5.1.1 Hệ thống quadcopter xây dựng Hı̀nh 5.1.1 Hệ thống quadcopter thiết kế 73 Hı̀nh 5.1.2 Mặt quadcopter 74 Hı̀nh 5.1.3 Hệ thống quadcopter hoạt động không 5.1.2 Kết cân roll pitch yaw Bảng giá trị thông số chọn: Thông số 𝐾 Giá trị 0.55 Chú thích Chuyển đổi từ sai số góc sang vận tốc góc đặt roll, pitch 𝑘 0.62 Hệ số P PID vận tốc góc 𝑘 0.4 Hệ số I PID vận 75 tốc góc 𝑘 0.0514 Hệ số D PID vận tốc góc Bả ng 5.1 Bảng thông số điều khiển roll, pitch, yaw Hı̀nh 5.1.4 Kết điều khiển góc roll, pitch Hı̀nh 5.1.5 Kết điều khiển tốc độ góc 76 5.1.3 Kết giữ độ cao Bảng giá trị thông số chọn: Thơng số 𝑘 Giá trị Chú thích 0.38 Hệ số P PID độ cao 0.12 𝑘 Hệ số P PID độ cao 0.62 𝑘 Hệ số P PID độ cao Bả ng 5.2 Bảng thông số điều khiển độ cao Hı̀nh 5.1.6 Kết giữ độ cao dùng cảm biến siêu âm 5.1.4 Kết giữ vận tốc vị trí dùng Optical Flow Bảng giá trị thông số chọn: Thông số 𝑘 Giá trị 0.20588 77 Chú thích Hệ số P PID vận tốc 0.05 𝑘 Hệ số I PID vận tốc 0.02735 𝑘 Hệ số D PID vận tốc 0.55 𝐾 Hệ số chuyển từ sai số vị trí sang vận tốc đặt PX4FLOW_VEL_SIZE Kích thước lọc trung bình vận tốc từ px4flow velocity_sp_lpf_hz 12 Tần số lọc thông thấp cho vận tốc đặt pid_vel_filter_hz 17 Tần số lọc thông thấp cho ngõ PID vận tốc d_filter_hz 15 Tần số lọc thông thấp cho thành D PID vận tốc POS_CONTROLLER_T 10 Tỷ số tần số cập nhật điều khiển vận tốc chia điều khiển vị trí Bả ng 5.3 Bảng thông số điều khiển vận tốc vị trí 78 Hı̀nh 5.1.7 Kết giữ vị trí với px4flow Hı̀nh 5.1.8 Kết điều khiển giữ vị trí với px4flow theo thời gian 5.2 Hướng phát triển đề tài Phần trình bày phương pháp để cải thiện chất lượng hệ thống quadcopter luận văn này, nhằm để xây dựng hệ thống hoàn thiện 79 bền vững hơn, mà giới hạn thời gian, khối lượng luận văn kiến thức có, mà luận văn chưa thể thực 5.2.1 Sử dụng RTOS Việc sử dụng phương pháp state-machine, kiểm tra biến trạng thái dựa vào chọn phần code thực thi, sử dụng ngắt timer có sẵn vi điều khiển để xây dựng chương trình luận văn gặp vấn đề muốn mở rộng hệ thống phần mềm, gặp nhiều khó khăn khó quản lý RTOS đảm nhận việc quản lý tài nguyên hệ thống, để người dùng chạy song song nhiều vòng lặp, việc thiết kế chương trình đơn giản nhiều chế quản lý biến mutex, semaphore, , nhiệm vụ chạy coi lại độc lập với Các hệ điều hành RTOS phổ biến cho dòng vi điều khiển thông dụng ARM Cortex-M3, ARM Cortex-M3 là: FreeRTOS, NuttX, ChibiOS,… 5.2.2 Áp dụng lọc Extended Kalman Filter GPS Thuật toán Extended Kalman Filter tận dụng tối đa tất thông tin cảm biến mà hệ thống có được, kết tính tốn thơng tin quadcopter cải thiện từ EKF Kết hợp GPS để xác định vị trí, dùng Optical Flow để xác định vị trí có độ xác cao thời điểm tại, hoạt động thời gian dài bị sai số làm tròn, sai số cảm biến tích lũy dần, dead reckoning, GPS điều chỉnh lại sai số 5.2.3 Tích hợp máy tính nhúng Khi tốn điều khiển động học động lực học đảm bảo Việc tích hợp máy tính nhúng kèm thiết bị bay giúp ứng dụng vào nhiệm vụ thực tiễn sống 80 Hı̀nh 5.2.1 Mơ hình tích hợp máy tính nhúng Odroid điều khiển bay Pixhawk Ví dụ ứng dụng máy tính nhúng tích hợp drone như: + Xử lý ảnh, phát đối tượng + Tận dụng tài ngun sẵn có mà máy tính nhúng hỗ trợ hệ thống ROS Linux, điều khiển qua mạng, … + Gắn dụng cụ lên quadcopter vịi nước để ứng dụng cơng tắc chữa cháy 81 TÀI LIỆU THAM KHẢO [1] Chang Liu , Member, IEEE and Stephen D Prior, “Design and Implementation of a Mini Quadrotor Control System in GPS Denied Environments” [2] Luis E Romero, David F Pozo, Jorge A Rosales, “Quadcopter stabilization by using PID controllers” [3] Đặng Anh Tùng, “Xây Dựng Bộ Ước Lượng Góc Quay Ba Trục” [4] Mark Euston, Paul Coote, Robert Mahony, Jonghyuk Kim and Tarek Hamel “A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV” [5] Huỳnh Thái Hoàng, “bài giảng Cở sở tự động” [6] Yan-Bin Jia, “Quaternions (Com S 477/577 Notes)” [7] Sebastian O.H Madgwick, “An efficient orientation filter for inertial and inertial/magnetic sensor arrays” [8] Các trang web: ardupilot.org, px4.io, diydrones.com, github.com, pudn.com, brokking.net, sci-hub.tw 82 ... Cảm biến px 4flow PX 4FLOW cố định bên quadcopter gắn damping foam để hạn chế ảnh hưởng rung động khí quadcopter hoạt động gây Hı̀nh 2.3.5 Damping foam cho px 4flow Các liệu trả px 4flow đọc integral... giữ vị trí Optical Flow khắc phục 1.3.3 Mục tiêu nội dung Áp dụng giải thuật xử lý liệu cảm biến, xây dựng phương trình động học động lực học cho quadcopter, từ thiết kế điều khiển cho quadcopter. .. khung quadcopter (từ 2cm – 100 cm) 2.3.4 Cảm biến Optical Flow PX 4FLOW 2.3.4.1 Giới thiệu thông số kỹ thuật PX 4FLOW Sản phẩm nghiên cứu ETH Zurich, ưu điểm hoạt động môi trường ánh sáng thấp PX4FLOW