Điều khiển cân bằng và giữ vị trí cho hệ quadcopter Điều khiển cân bằng và giữ vị trí cho hệ quadcopter Điều khiển cân bằng và giữ vị trí cho hệ quadcopter Điều khiển cân bằng và giữ vị trí cho hệ quadcopter Điều khiển cân bằng và giữ vị trí cho hệ quadcopter Điều khiển cân bằng và giữ vị trí cho hệ quadcopter Điều khiển cân bằng và giữ vị trí cho hệ quadcopter Điều khiển cân bằng và giữ vị trí cho hệ quadcopter
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO KHÓA LUẬN TỐT NGHIỆP Đề tài : ĐIỀU KHIỂN CÂN BẰNG VÀ GIỮ VỊ TRÍ CHO HỆ QUADCOPTER GVHD SVTH MSSV Lớp Chuyên ngành Hệ đào tạo Khóa đào tạo : PGS.TS NGUYỄN MINH TÂM : VÕ ANH KHOA : 16151036 : 16151CL3 : CNKT Điều khiển Tự động hóa : Chính Quy : 2016 - 2020 TP Hồ Chí Minh, tháng 08 năm 2020 Khóa Luận Tốt Nghiệp Lời Cảm Ơn Để luận văn đạt kết tốt đẹp, nhận hỗ trợ, giúp đỡ Q thầy/cơ bạn Với tình cảm sâu sắc, chân thành, cho phép tơi bày tỏ lịng biết ơn sâu sắc đến tất cá nhân tạo điều kiện giúp đỡ trình học tập nghiên cứu đề tài Đặc biệt xin gửi lời cảm ơn chân thành tới PGS TS Nguyễn Minh Tâm, trưởng Khoa Điện – Điện quan tâm giúp đỡ, hướng dẫn tơi hồn thành tốt luận văn thời gian qua TS Nguyễn Văn Đông Hải hỗ trợ trang thiết bị định hướng học tập cho từ năm đầu đại học Tơi xin bày tỏ lịng biết ơn đến quý thầy cô Khoa Điện – Điện Tử, khoa Đào tạo Chất lượng cao trực tiếp gián tiếp giúp đỡ tơi suốt q trình học tập nghiên cứu đề tài Ngồi ra, tơi xin gửi lời đến gia đình bạn Võ Minh Tài tạo điều kiện không gian cho thực nghiệm đề tài, bạn Nguyễn Văn Hiển, Võ Minh Tài, Lư Minh Triết bên cạnh giúp đỡ, động viên ngày vừa qua Tôi xin gửi lời cảm ơn đến gia đình tạo điều kiện hỗ trợ tài cho tơi sớm hồn thành đồ án Với điều kiện thời gian kinh nghiệm hạn chế học viên, luận văn tránh thiếu sót Tơi mong nhận bảo, đóng góp ý kiến thầy để tơi có điều kiện bổ sung, nâng cao ý thức mình, phục vụ tốt cơng tác thực tế sau Tôi xin chân thành cảm ơn! i Khóa Luận Tốt Nghiệp Mục Lục Lời Cảm Ơn i Mục Lục ii Danh Mục Hình Ảnh iv Danh Mục Bảng vi Danh Mục Từ Viết Tắt vii Tóm Tắt viii Chương 1: Tổng Quan 1.1 Đặt vấn đề 1.1.1 Các kết nghiên cứu nước 1.1.2 Các kết nghiên cứu nước 1.2 Lý chọn đề tài 1.3.Mục tiêu đề tài 1.4 Giới hạn đề tài 1.5 Nội dung nghiên cứu Chương 2: Cơ Sở Lý Thuyết 2.1 Các khái niệm 2.2 Cascade Control [10] 2.3 Phương pháp optical flow Chương 3: Thiết Kế Hệ Thống 3.1 Thiết kế phần cứng 3.1.1 Máy tính nhúng 3.1.2 Cảm biến 3.1.3 Động mạch điều khiển tốc độ động 11 3.1.4 Camera 12 3.1.5 Khung quadcopter 13 3.1.6 Cánh quạt 13 3.1.7 Sơ đồ khối hệ thống 15 ii Khóa Luận Tốt Nghiệp 3.1.8 Bộ thu phát tín hiệu 15 3.2 Thiết kê phần mềm 16 Chương 4: Mơ Hình Hóa Hệ Thống 18 4.1 Mơ hình động lực học hệ quadcopter 18 4.2 Mơ hình hóa động với cánh quạt 23 4.3 Tính moment quán tính hệ thống 25 Chương 5: Thiết Kế Bộ Điều Khiển 28 5.1 Cascade PI 28 5.2 Thiết kế lọc 32 5.3 Đo vận tốc góc sử dụng phương pháp optical flow 34 5.3.1 Thuật toán Lucas-Kanade [13] 34 5.3.2 Theo dõi đặc trưng (feature tracking) 36 5.3.3 Mơ hình pin-hole [14] 38 Chương 6: Kết Quả Mô Phỏng Thực Nghiệm 43 6.1 Kết mô 43 6.2 Kết thực nghiệm 48 6.2.1 Hiệu chỉnh camera bám theo đặc trưng ảnh 48 6.2.2 Điều khiển cân giữ vị trí 50 Chương 7: Kết Luận Hướng Phát Triển 59 7.1 Kết luận 59 7.2 Hướng phát triển 59 Tài Liệu Tham Khảo 60 Phụ Lục 62 iii Khóa Luận Tốt Nghiệp Danh Mục Hình Ảnh Hình 2.1: Quadcopter tình trạng bay Hình 2.2: Chuyển động throttle Hình 2.3: Chuyển động pitch Hình 2.4: Chuyển động roll Hình 2.5: Chuyển động yaw Hình 2.6: Sơ đồ khối cascade control Hình 3.1: Raspberry Pi 3B+ Hình 3.2: Phương pháp xử lý đa luồng Hình 3.3: Cảm biến GY-86 10 Hình 3.4: Xung PPM điều khiển ESC 11 Hình 3.5: ESC BLHeli 12 Hình 3.6: Động Phantom3 12 Hình 3.7: Camera pi v1.3 12 Hình 3.8: Webcam logitech C270 12 Hình 3.9: Khung quadcopter 13 Hình 3.10: Cánh quạt EPP1045 14 Hình 3.11: Lực nâng công suất cánh quạt [11] 14 Hình 3.12: Sơ đồ khối thiết bị phần cứng 15 Hình 3.13: Bộ thu phát tín hiệu 16 Hình 3.14: Giao diện hệ điều hành Raspbian 17 Hình 3.15: Giao tiếp máy tính raspberry qua wifi 17 Hình 4.1: Quadcopter hệ quy chiếu B-frame 18 Hình 4.2: Quadcopter hệ quy chiếu H-frame 19 Hình 4.3: Dữ liệu động thu thập 23 Hình 4.4: So sánh kết nhận dạng với liệu thưc tế 25 Hình 4.5: Khoảng cách từ motor đến trọng tâm quadcopter 26 Hình 5.1: Phương pháp chọn khâu điều khiển [10] 28 Hình 5.2: Sơ đồ khối điều khiển Cascade PI cho hệ quadcopter 30 Hình 5.3: Sơ đồ khối điều khiển Cascade PI với điều khiển độ cao cho hệ quadcopter 31 Hình 5.4: Sơ đồ khối điều khiển cân bằng, giữ vị trí độ cao cho quadcopter 32 Hình 5.5: Gia tốc kế quay quanh trục x 33 Hình 5.6: Bộ lọc complementary 34 Hình 5.7: Pyramid Lucas-Kanade 36 Hình 5.8: Phương pháp Harris 37 iv Khóa Luận Tốt Nghiệp Hình 5.9: Phương pháp Shi-Tomasi 37 Hình 5.10: Lưu đồ theo dõi đặc trưng 38 Hình 5.11: Mơ hình chiếu pin-hole 39 Hình 5.12: Hình chiếu phối cảnh mơ hình pin-hole 40 Hình 5.13: Góc camera từ quadcopter chiếu xuống đất 41 Hình 6.1: Sơ đồ khối mơ điều khiển cho quadcopter 44 Hình 6.2: Góc roll tốc độ góc tương ứng tín hiệu đặt 45 Hình 6.3: Góc pitch tốc độ góc tương ứng tín hiệu đặt 45 Hình 6.4: Góc yaw tốc độ góc tương ứng tín hiệu đặt 46 Hình 6.5: Góc roll tốc độ góc tương ứng tín hiệu đặt 15 46 Hình 6.6: Góc pitch tốc độ góc tương ứng tín hiệu đặt 15 47 Hình 6.7: Góc yaw tốc độ góc tương ứng tín hiệu đặt 15 47 Hình 6.8: Quá trình hiệu chỉnh camera 48 Hình 6.9: Kết áp dụng thuật tốn PLK có đặc trưng 49 Hình 6.10: Kết áp dụng thuật tốn PLK có nhiều đặc trưng 49 Hình 6.11: Mơ hình thực tế quadcopter 50 Hình 6.12: Góc nhìn phía trước quadcopter 51 Hình 6.13: Góc nhìn phía bên cạnh quadcopter 51 Hình 6.14: Kết góc roll lọc chưa lọc 52 Hình 6.15: Kết góc pitch lọc chưa lọc 52 Hình 6.16: Kết thực tế quadcopter giữ ổn định góc nghiêng vị trí 53 Hình 6.17: Góc roll quadcopter bay thực tế 54 Hình 6.18: Góc pitch quadcopter bay thực tế 54 Hình 6.19: Góc yaw quadcopter bay thực tế 55 Hình 6.20: Tốc độ góc quanh trục x quadcopter bay thực tế 55 Hình 6.21: Tốc độ góc quanh trục y quadcopter bay thực tế 56 Hình 6.22: Tốc độ góc quanh trục y quadcopter bay thực tế 56 Hình 6.23: Tốc độ cánh quạt độ cao tương ứng 57 Hình 6.24: Vận tốc tuyến tính quadcopter hệ tọa độ Oxy 57 Hình 6.25: Vị trí quadcopter hệ tọa độ Oxy 58 v Khóa Luận Tốt Nghiệp Danh Mục Bảng Bảng 3.1: Thông số kỹ thuật Raspberry Pi 3B+ Bảng 3.2: Thông số kỹ thuật cảm biến có GY-86 10 Bảng 3.3: Thơng số kỹ thuật ESC 12 Bảng 3.4: Thơng số kỹ thuật RX701 16 Bảng 4.1: Mô tả thông số hệ thống 22 Bảng 4.2: Hàm truyền nhận dạng động 24 Bảng 4.3: Các thơng số tính tốn moment quán tính quadcopter 26 Bảng 6.1: Thông số hệ thống 43 Bảng 6.2: Thông số cánh quạt [11] 43 vi Khóa Luận Tốt Nghiệp Danh Mục Từ Viết Tắt Từ viết tắt Từ nguyên nghĩa PI Proportional–Integral LK Lucas - Kanade PLK Pyramid Lucas - Kanade GFTT Good Feature To Track CSI Camera Serial Interface B-frame Body-frame E-frame Earth-frame vii Khóa Luận Tốt Nghiệp Tóm Tắt Trong luận văn này, người thực thiết kế điều khiển cascade PI cho hệ quadcopter mô hệ quadcopter với điều khiển cascade PI với thông số hệ quadcopter người thực tìm cách đo đạc số liệu mơ hình thực tế Bước đầu, thơng số điều khiển mơ để tìm điều khiển cho đáp ứng mong muốn Từ kết mơ đó, thơng số điều khiển cascade PI áp dụng vào thực tế tinh chỉnh để hệ thống đạt chất lượng ổn định tốt Hơn nữa, người thực kết hợp xử lý ảnh để đo vị trí quadcopter phương pháp optical flow để điều khiển giữ vị trí cho hệ thống Kết thực nghiệm cho thấy quadcopter giữ vị trí khoảng thời gian dài Điều chứng minh điều khiển thiết kế thành công thỏa mãn yêu cầu đặt Từ khóa: Quadcopter; optical flow; cascade PI Abstract In this thesis, the author design the PI cascade controller for quadcopter and simulate quadcopter system with this PI cascade controller along with the parameters of quadcopter system which found by measuring actual model data Initially, the controller parameters will be simulated to find the controller to find the respond as well as posible From the simulation results, PI cascade controller parameters are applied in practice and fine-tuned to give the system the best stable quality Moreover, the author combines image processing to measure quadcopter position by optical flow method to control the position of the system Experimental results show that the quadcopter can hold position for a long time This proves that the controller has been successfully designed and satisfies the requirements Keywords: Quadcopter; optical flow; cascade PI viii Khóa Luận Tốt Nghiệp Chương 1: Tổng Quan 1.1 Đặt vấn đề Trong vài năm qua, máy bay không người lái trở thành trung tâm chức doanh nghiệp tổ chức phủ khác tìm cách xuyên qua khu vực nơi số ngành cơng nghiệp bị đình trệ tụt lại phía sau Từ việc giao hàng nhanh vào cao điểm đến quét quân truy cập, máy bay không người lái chứng tỏ có lợi nơi mà người khơng thể tiếp cận thực kịp thời hiệu Tăng hiệu suất làm việc, giảm khối lượng cơng việc chi phí sản xuất, cải thiện độ xác, tinh chỉnh dịch vụ quan hệ khách hàng giải vấn đề an ninh quy mô lớn vài số ngành công nghiệp hàng đầu sử dụng máy bay khơng người lái tồn cầu Việc áp dụng công nghệ máy bay không người lái ngành công nghiệp chuyển từ giai đoạn mốt sang giai đoạn xu hướng lớn nhanh ngày nhiều doanh nghiệp bắt đầu nhận tiềm năng, phạm vi quy mơ phạm vi tồn cầu Cho dù máy bay không người lái điều khiển từ xa truy cập thông qua ứng dụng điện thoại thơng minh, chúng có khả tiếp cận khu vực xa mà không cần nhiều nhân lực cần nỗ lực, thời gian lượng Đây lý lớn khiến chúng áp dụng toàn giới, đặc biệt bốn lĩnh vực: Quân sự, Thương mại, Cá nhân Cơng nghệ Tương lai Vì vậy, nhiều nghiên cứu quadcopter nhiều lĩnh vực ứng dụng khác thực nước 1.1.1 Các kết nghiên cứu nước Việc điều khiển ổn định hệ thống quadcopter đề tài hấp dẫn nhiều công trình nghiên cứu đến Năm 2006, tài liệu [1] đề xuất điều khiển hồi tiếp quadcopter để điều khiển ổn định độ cao theo cấp số nhân quadcopter cất cánh hạ cánh Cũng năm 2006, hai tác giả Tarek Madani ; Abdelaziz Benallegue thực điều khiển backstepping để điều khiển hệ thống ổn định bám theo quỹ đạo cho trước [2] Năm 2010, mơ hình quadcopter cỡ lớn thiết kế điều khiển nhằm mục đích mang vật có khối lượng nặng quadcopter [3] Năm Khóa Luận Tốt Nghiệp Chương 7: Kết Luận Hướng Phát Triển 7.1 Kết luận Thông qua kết mô thực tế, người thực áp dụng thành công phương pháp optical flow để đo đạc vị trí, vận tốc tuyến tính quadcopter Từ đó, thiết kế tinh chỉnh thành cơng thơng số điều khiển cascade PI để điều khiển giữ ổn định vị trí cho quadcopter 7.2 Hướng phát triển Tuy hệ thống giữ ổn định tốt việc tìm kiếm tinh chỉnh thơng số điều khiển cascade PI khó khăn nhiều thời gian Người thực nhận thấy hệ quadcopter dễ bị ảnh hưởng nhiễu bên ngồi gió Bên cạnh đó, cascade PI điều khiển tuyến tính nên cịn nhiều hạn chế Do đó, người thực đề xuất sử dụng điều khiển phi tuyến thích nghi điều khiển hệ thống tình trạng nhiễu sử dụng học củng cố để huấn luyện điều khiển thực tế cho hệ quadcopter 59 Khóa Luận Tốt Nghiệp Tài Liệu Tham Khảo [1] Abdelhamid Tayebi and Stephen McGilvray “Attitude Stabilization of a VTOL Quadrotor Aircraft” IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, Vol 14, No 3, 2006 [2] Tarek Madani ; Abdelaziz Benallegue “Backstepping Control for a Quadrotor Helicopter” International Conference on Intelligent Robots and Systems, 2006 [3] P Pounds a,, R Mahony b , P Corke “Modelling and control of a large quadrotor robot” Control Engineering Practice, Vol 18, Pp 691-699, 2010 [4] Adriano Garcia, Edward Mattison and Kanad Ghose “High-speed Vision-based Autonomous Indoor Navigation of a Quadcopter” International Conference on Unmanned Aircraft Systems, 2015 [5] M Anwar Ma'sum, Grafika Jati, M Kholid Arrofi, Adi Wibowo, Petrus Mursanto, Wisnu Jatmiko “Autonomous quadcopter swarm robots for object localization and tracking” MHS2013, 2013 [6] Salman Bari, Syeda Shabih Zehra Hamdani, Hamza Ullah Khan, Mutte ur Rehman, Haroon Khan “Artificial Neural Network Based Self-Tuned PID Controller for Flight Control of Quadcopter” International Conference on Engineering and Emerging Technologies, 2019 [7] Lâm Ngọc Tâm “THIẾT KẾ VÀ CHẾ TẠO MƠ HÌNH MÁY BAYQUADROCOPTER” Tuyển tập Báo cáo Hội nghị Sinh viên Nghiên cứu Khoa học lần thứ Đại học Đà Nẵng, 2012 [8] Huu Khoa Tran and Tran Thanh Trang “Continuous Genetic Algorithm Aiding to Quadcopter Controller Design” International Conference on Advanced Engineering Theory and Applications, Pp 571-579, 2017 [9] Manh-Cuong Le, My-Ha Le “Human Detection and Tracking for Autonomous Human-following Quadcopter” International Conference on System Science and Engineering, 2019 60 Khóa Luận Tốt Nghiệp [10] Bolton, W “Control Systems” Instrumentation and Control Systems, 281–302, 2015 [11] Jorge Miguel Brito Domingues “Quadrotor prototype” Instituto Superior Técnico, Portugal, 2009 [12] Tommaso Bresciani “Modeling identification and control of a quadrotor helicopter” Master Thesis, Department of automatic control, Lund University, 2008 [13] Krzysztof Blachut, Tomasz Kryjak and Marek Gorgon “Hardware implementation of multi-scale Lucas-Kanade optical flow computation algorithm-a demo” Conference on Design and Architectures for Signal and Image Processing, 2018 [14] Omar Daniel Mora Granillo, Zizilia Zamudio Beltrán “Real-Time Drone (UAV) Trajectory Generation and Tracking by Optical Flow” International Conference on Mechatronics, Electronics and Automotive Engineering, 2018 [15] Jianbo Shi, Tomasi “Good features to track” Proceedings of IEEE Conference on Computer Vision and Pattern Recognition, 1994 [16] https://www.learnopencv.com/camera-calibration-using-opencv/ [17] W Barnes and W McCormick “Aerodynamics, Aeronautics and Flight Mechanics” New York: Wiley, 2nd edition, 1995 [18] C Harris, M Stephens “A combined corner and edge detector” Proceedings of the Alvey Vision Conference, pp 147–151, 1988 61 Khóa Luận Tốt Nghiệp Phụ Lục Chương trình điều khiển quadcopter import os import smbus import time import math import serial import MPU6050_lib import GY_86_lib import MS5611_lib import Rotation_lib import Butterworth_lib import PID_lib import RPS2PULSE as R2P import numpy as np from threading import Thread import OpticalFlow_Thread_lib_3 as OpticalFlow_Thread_lib # os.system("sudo pigpiod -s 5") #Run once to start the pigpio daemon, sample rate 1us # os.system("sudo killall pigpiod") #To stop the pigpio daemon time.sleep(1) import pigpio from gpiozero import MCP3202 # file = open("/media/pi/SAVE/data_24_7_2.csv", "a") # i=0 # if os.stat("/media/pi/SAVE/data_24_7_2.csv").st_size == 0: # file.write("roll,pitch,yaw,throttle,altitude,xpos,ypos\n") #PID parameters PID_RA_P_GAIN = PID_RA_I_GAIN = PID_RA_D_GAIN = PID_PA_P_GAIN = PID_PA_I_GAIN = PID_PA_D_GAIN = PID_YA_P_GAIN = PID_YA_I_GAIN = PID_YA_D_GAIN = PID_A_I_TH = 50 for angle PIDs 0 PID_RA_P_GAIN PID_RA_I_GAIN PID_RA_D_GAIN 0 #PID parameters PID_PR_P_GAIN = PID_PR_I_GAIN = PID_PR_D_GAIN = PID_RR_P_GAIN = PID_RR_I_GAIN = for rotation rate PIDs 0.5 0.6 PID_PR_P_GAIN PID_PR_I_GAIN 62 Khóa Luận Tốt Nghiệp PID_RR_D_GAIN = PID_YR_P_GAIN = PID_YR_I_GAIN = PID_YR_D_GAIN = PID_R_I_TH = 50 PID_PR_D_GAIN 0.7 0.4 #PID for position PIDs PID_X_P_GAIN = PID_X_I_GAIN = PID_X_D_GAIN = 20 PID_Y_P_GAIN = PID_Y_I_GAIN = PID_Y_D_GAIN = 20 PID_X_I_TH = 10 # -# Start the position (x, y) PID - note the different PID class for yaw # -x_pid = PID_lib.PID(PID_X_P_GAIN, PID_X_I_GAIN, PID_X_D_GAIN, PID_X_I_TH) y_pid = PID_lib.PID(PID_Y_P_GAIN, PID_Y_I_GAIN, PID_Y_D_GAIN, PID_X_I_TH) # -# Start the pitch, roll and yaw angle PID - note the different PID class for yaw # -pa_pid = PID_lib.PID(PID_PA_P_GAIN, PID_PA_I_GAIN, PID_PA_D_GAIN, PID_A_I_TH) ra_pid = PID_lib.PID(PID_RA_P_GAIN, PID_RA_I_GAIN, PID_RA_D_GAIN, PID_A_I_TH) ya_pid = PID_lib.YAW_PID(PID_YA_P_GAIN, PID_YA_I_GAIN, PID_YA_D_GAIN, PID_A_I_TH) # -# Start the pitch, roll and yaw rotation rate PIDs # -pr_pid = PID_lib.PID(PID_PR_P_GAIN, PID_PR_I_GAIN, PID_PR_D_GAIN, PID_R_I_TH) rr_pid = PID_lib.PID(PID_RR_P_GAIN, PID_RR_I_GAIN, PID_RR_D_GAIN, PID_R_I_TH) yr_pid = PID_lib.PID(PID_YR_P_GAIN, PID_YR_I_GAIN, PID_YR_D_GAIN, PID_R_I_TH) motion_rate = 200 63 Khóa Luận Tốt Nghiệp adc_frequency = 1000 FULL_FIFO_BATCHES=20 sampling_rate=500 ESC1 ESC2 ESC3 ESC4 = = = = 17 27 19 26 ELEV AILE THRO RUDD GEAR AUX1 AUX2 = = = = = = = 13 16 12 20 21 inPINS = [ELEV, AILE, THRO, RUDD, GEAR, AUX1, AUX2] pi = pigpio.pi() pi.set_servo_pulsewidth(ESC1, 1000) pi.set_servo_pulsewidth(ESC2, 1000) pi.set_servo_pulsewidth(ESC3, 1000) pi.set_servo_pulsewidth(ESC4, 1000) time.sleep(3) upTimes=0 downTimes=0 deltaTimes=0 deltaTimes_pre_1=0 deltaTimes_pre_2=0 deltaTimes_pre_3=0 deltaTimes_pre_4=0 deltaTimes_pre_5=0 deltaTimes_pre_6=0 throttle =0 roll_setpoint=0 pitch_setpoint=0 yaw_setpoint=0 gear=0 MIX=0 def my_callback1(gpio,level,tick): global upTimes, downTimes, deltaTimes, deltaTimes_pre_1, deltaTimes_pre_2, deltaTimes_pre_3, deltaTimes_pre_4, deltaTimes_pre_5, deltaTimes_pre_6, throttle, roll_setpoint, pitch_setpoint, yaw_setpoint, gear, MIX if (level==0): downTimes=tick else: upTimes=tick 64 Khóa Luận Tốt Nghiệp deltaTimes=(downTimes-upTimes) if gpio==ELEV: if deltaTimes 10: ra_target = np.sign(ra_target)*10 #======================================================================= =============== # Angle PIDs #======================================================================= =============== [p_out, i_out, d_out] = pa_pid.Compute(pitch, pa_target + pitch_setpoint, motion_dt, gear, throttle) pr_target = p_out + i_out + d_out [p_out, i_out, d_out] = ra_pid.Compute(roll, ra_target + roll_setpoint, motion_dt, gear, throttle) rr_target = p_out + i_out + d_out # motion_dt, gear) [p_out, i_out, d_out] = ya_pid.Compute(0, 0, 70 Khóa Luận Tốt Nghiệp # # # yr_target = p_out + i_out + d_out if init_pressure == 0: init_pressure = actual_pressure h = MS5611.getAltitude(actual_pressure,init_pressure) if yaw_offset == 0: yaw_offset = heading if roll_offset == 0: roll_offset = roll if pitch_offset == 0: pitch_offset = pitch yaw_input = heading - yaw_offset yaw_error = ya_pid.Error(yaw_input, yaw_setpoint) if throttle>400: yaw1_angle += qrz*elapse_time if abs(yaw1_angle) > 180: yaw1_angle= np.sign(yaw1_angle)*180 yaw_error = yaw1_angle- yaw_setpoint yaw_i_gain += yaw_error*0.03*motion_dt yr_target = -(yaw_error*0.5 + yaw_i_gain) else: yaw1_angle = yaw_i_gain = yr_target = if pr_target > 50: pr_target = 50 if rr_target > 50: rr_target = 50 if yr_target > 50: yr_target = 50 #======================================================================= =============== # Rotation rate PIDs #======================================================================= =============== [p_out, i_out, d_out] = pr_pid.Compute(qry, pr_target, motion_dt, gear, throttle) pr_out = p_out + i_out + d_out [p_out, i_out, d_out] = rr_pid.Compute(qrx, rr_target, motion_dt, gear, throttle) rr_out = p_out + i_out + d_out [p_out, i_out, d_out] = yr_pid.Compute(qrz, yr_target, motion_dt, gear, throttle) yr_out = p_out + i_out + d_out if pr_out > 50: pr_out = 50 71 Khóa Luận Tốt Nghiệp if rr_out > 50: rr_out = 50 if yr_out > 50: yr_out = 50 # if lipo_volt < or lipo_volt > 15: off_rps = else: off_rps = (12.6-lipo_volt)*15 off_rps=0 esc_1_rps = throttle - pr_out - rr_out + yr_out off_rps + PID_pressure esc_2_rps = throttle - pr_out + rr_out - yr_out off_rps + PID_pressure esc_3_rps = throttle + pr_out + rr_out + yr_out off_rps + PID_pressure esc_4_rps = throttle + pr_out - rr_out - yr_out off_rps + PID_pressure # esc_1_rps = throttle + yr_out + off_rps # esc_2_rps = throttle - yr_out + off_rps # esc_3_rps = throttle + yr_out + off_rps # esc_4_rps = throttle - yr_out + off_rps else: pa_target=0 ra_target=0 pr_target=0 rr_target=0 yr_target=0 pr_out=0 rr_out=0 yr_out=0 esc_1_rps=0 esc_2_rps=0 esc_3_rps=0 esc_4_rps=0 yaw_i_gain=0 + + + + pi.set_servo_pulsewidth(ESC1, R2P.Rps2Pulse_M1(esc_1_rps)) pi.set_servo_pulsewidth(ESC2, R2P.Rps2Pulse_M2(esc_2_rps)) pi.set_servo_pulsewidth(ESC3, R2P.Rps2Pulse_M3(esc_3_rps)) pi.set_servo_pulsewidth(ESC4, R2P.Rps2Pulse_M4(esc_4_rps)) # ser.write(b'%f %f %f %f %f %f\r\n'%(qax,qay,qaz,qrx,qry,qrz)) # file.write(str(roll)+","+str(pitch)+","+str(yaw1_angle)+","+str(throttle )+","+str(h)+","+str(x_pos)+","+str(y_pos)+"\n") # ser.write(b'%f %f\r\n'%(roll, pitch)) if elapse_time2>1: 72 Khóa Luận Tốt Nghiệp end2=time.time() print(roll,pitch,yaw1_angle,nfb,throttle,lipo_volt,actual_pressure,PID_p ressure,h,pa_target,ra_target,x_pos,y_pos) # print(pa_target,ra_target,x_pos,y_pos) except: pi.set_servo_pulsewidth(ESC1, 1000) pi.set_servo_pulsewidth(ESC2, 1000) pi.set_servo_pulsewidth(ESC3, 1000) pi.set_servo_pulsewidth(ESC4, 1000) print("Sensor error") # file.flush() # time.sleep(5) # file.close() 73 ... ảnh để đo vị trí quadcopter phương pháp optical flow để điều khiển giữ vị trí cho hệ thống Kết thực nghiệm cho thấy quadcopter giữ vị trí khoảng thời gian dài Điều chứng minh điều khiển thiết... học hệ quadcopter, mơ hình hóa động BLDC, phân tích moment quán tính quadcopter Chương 5: Thiết kế điều khiển: thiết kế điều khiển cascade PI để điều khiển cân bằng, giữ độ cao giữ vị trí cho hệ. .. tốn học cho hệ quadcopter Thiết kế điều khiển cascade PI Dùng camera để đo vị trí quadcopter Mô hệ quadcopter với điều khiển Thiết kế khí điện tử cho mơ hình thực tế Lập trình điều khiển quadcopter