Stabilization and trajectory control of a quadrotor with uncertain payload

ĐẠI HỌC QUỐC GIA TPHCM TRƯỜNG ĐẠI HỌC BÁCH KHOA TRẦN KHA CÂN BẰNG VÀ QUY HOẠCH QUỸ ĐẠO CHO QUADROTOR VỚI TẢI KHÔNG CHẮC CHẮN STABILIZATION AND TRAJECTORY CONTROL OF A QUADROTOR WITH UNCERTAIN PAYLOAD Chuyên ngành: Kỹ thuật Điều khiển Tự động hóa Mã số: 60.52.02.16 LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, THÁNG NĂM 2020 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐHQG - HCM Cán hướng dẫn khoa học : PGS TS Huỳnh Thái Hoàng Cán chấm nhận xét : TS Nguyễn Vĩnh Hảo Cán chấm nhận xét : PGS.TS Trương Đình Nhơn Luận văn thạc sĩ bảo vệ Trường Đại học Bách Khoa, ĐHQG Tp HCM ngày 04 tháng 09 năm 2020 Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm: (Ghi rõ họ, tên, học hàm, học vị Hội đồng chấm bảo vệ luận văn thạc sĩ) GS.TS Hồ Phạm Huy Ánh PGS.TS Trương Đình Nhơn TS Nguyễn Vĩnh Hảo TS Ngô Thanh Quyền TS Nguyễn Trọng Tài Xác nhận Chủ tịch Hội đồng đánh giá LV Trưởng Khoa quản lý chuyên ngành sau luận văn sửa chữa (nếu có) CHỦ TỊCH HỘI ĐỒNG TRƯỞNG KHOA ĐẠI HỌC QUỐC GIA TP.HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: TRẦN KHA MSHV: 1770036 Ngày, tháng, năm sinh: 30/07/1994 Nơi sinh: TÂY NINH Chuyên ngành: Kỹ thuật Điều khiển Tự động hóa Mã số : 60520216 I TÊN ĐỀ TÀI: Cân quy hoạch quỹ đạo cho quadrotor với tải không chắn (Stabilization and Trajectory Control of a Quadrotor with Uncertain Payload) II NHIỆM VỤ VÀ NỘI DUNG: Xây dựng mơ hình tốn học, mơ hình hố quadrotor Xây dựng giải thuật điều khiển PID cho điều khiển cân điều khiển vị trí Xây dựng giải thuật điều khiển AFPID để điều khiển độ cao với tải không chắn Thực mô phỏng, so sánh đánh giá kết điều khiển thiết kế Xây dựng phần cứng, thực lập trình đánh giá kết thực nghiệm III NGÀY GIAO NHIỆM VỤ : 10/02/2020 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 06/06/2020 V CÁN BỘ HƯỚNG DẪN: PGS.TS Huỳnh Thái Hoàng Tp HCM, ngày CÁN BỘ HƯỚNG DẪN (Họ tên chữ ký) tháng năm 2020 CHỦ NHIỆM BỘ MÔN ĐÀO TẠO (Họ tên chữ ký) TRƯỞNG KHOA….……… (Họ tên chữ ký) LỜI CẢM ƠN Lời đầu tiên, em xin gửi lời cám ơn đến toàn thể quý Thầy Cô thuộc Khoa Điện – Điện Tử trường Đại học Bách Khoa TP Hồ Chí Minh đồng hành với em suốt năm học, trao cho em kiến thức quý báu hành trang cho em suốt đời Lời cám ơn tiếp theo, em xin chân thành gửi đến Mẹ em, người hy sinh đời để tạo tất điều kiện cho em học tập, cố gắng phấn đấu ủng hộ em tất định học tập Con xin cám ơn Mẹ! Em xin cám ơn người thầy, người đồng hành em xuyên suốt trình thực luận văn từ thời điểm Đại học đến Thạc sĩ Em xin cám ơn Thầy Huỳnh Thái Hồng kiến thức thầy trao giúp đỡ tận tình thầy đề tài luận văn Cuối em xin gửi lời cảm ơn đến tất người thân, bạn bè, đồng nghiệp, bạn Nguyễn Phúc Khôi, em Phạm Ngọc Trung, em Phạm Ngọc Nghĩa, bạn Hứa Minh Tuấn em Nguyễn Công Thức bên ủng hộ em mặt tình cảm, tinh thần tài Do điều kiện nghiên cứu cịn gặp nhiều khó khăn trở ngại kiến thức thời gian, kết luận văn tránh khỏi sai sót Do đó, em kính mong nhận ý kiến đóng góp từ q Thầy Cơ TP.HCM, tháng năm 2020 Cao học Tự Động Hoá khố 2017 Trần Kha i TĨM TẮT LUẬN VĂN Luận án tập trung vào mơ hình hóa điều khiển UAV loại Quadrotor xây dựng điều khiển thích hợp cân điều khiển vị trí mang tải có khối lượng thay đổi Lý chọn Quadrotor thiết bị bay có ưu điểm trội mặt kinh tế, ứng dụng rộng rãi lĩnh vực nghiên cứu dân dụng, công nghiệp nông nghiệp ngành vận tải hứa hẹn năm gần Quadrotor hệ thống đa biến, có tính phi tuyến cao (6 bậc tự DOF) thú vị điều khiển thông qua motors Luận văn nhắm tới việc đưa mơ hình tốn học xác chi tiết UAV, phát triển thuật toán điều khiển cân quy hoạch quỹ đạo kiểm tra mơ hình tốn học sử dụng mơ máy tính Thuật tốn điều khiển mờ thích nghi phát triển để đáp ứng khả điều khiển hệ thống tác động thay đổi khối lượng vật tải ii ABSTRACT This thesis will focus on modelling & control of UAV – Quadrotor and building up stabilization and trajectory control to deal with uncertain payload Quadrotor is a state of the art aerial vehicle in UAV field recent years due to its prime advatages as cost saving for setting up and easy to implement Besides it is also widely used in reaseach, industrial and argicultural field and many promising applications as delivery, sport Quadrotor is a high nonlinear system with degrees of freedom (DOF) and is very interesting for controlling via only motors fixed to frame Thus, this thesis will focus on high precisely mathetical modelling, developing stabilizing & trajectory control and assessing it via Matlab simulation The apdative fuzzy PID will be proposed in this thesis to stabilze quadrotor under the impact of payload uncertainty iii LỜI CAM ĐOAN CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA TP HỒ CHÍ MINH Tơi xin cam đoan cơng trình nghiên cứu riêng tơi hướng dẫn khoa học PGS.TS Huỳnh Thái Hoàng, nội dung nghiên cứu, kết đề tài trung thực chưa cơng bố hình thức trước Những số liệu bảng biểu phục vụ cho việc phân tích, nhận xét, đánh giá tác giả thu thập từ mơ thực nghiệm thân thực Ngồi ra, luận văn có tham khảo số tài liệu ngồi nước, tất có ghi cụ thể Nếu phát có gian lận tơi xin chịu hồn tồn trách nhiệm nội dung luận văn Trường Đại học Bách khoa thành phố Hồ Chí Minh khơng liên quan đến vi phạm tác quyền, quyền gây q trình thực (nếu có) TP Hồ Chí Minh, ngày tháng năm 2020 Trần Kha iv MỤC LỤC LỜI CẢM ƠN i TÓM TẮT LUẬN VĂN ii ABSTRACT iii LỜI CAM ĐOAN iv Chương 1.1 Tổng quan máy bay không người lái 1.2 Ứng dụng máy bay không người lái 1.3 Phân loại loại UAV 1.3.1 Phân loại theo tầm hoạt động 1.3.2 Phân loại theo cấu hình khí động học 1.3.3 Phân loại dựa kích cỡ trọng tải 1.4 Giới thiệu Quadrotor 1.5 Các khái niệm Quadrotor 1.5.1 Ứng dụng quadcopter vận chuyển hàng hoá, ưu nhược điểm 1.6 Cấu trúc luận văn 12 Chương 14 2.1 Mơ hình động học 14 2.2 Kết dự kiến 14 2.3 Nghiên cứu lý thuyết 15 2.3.1 Mơ hình hoá hệ thống quadrotor (System Modelling) 15 2.3.2 Động lực học Rotor 21 2.3.3 Mô hình khơng gian trạng thái 23 Chương 26 3.1 Linh kiện quadrotor 26 3.2 Khung quadrotor (Frame) 26 3.3 Động BLDC (Rotor) 27 3.4 Cánh quạt (Propeller) 29 3.5 Bộ điều khiển tốc độ điện tử (ESC) 29 3.6 Nguồn Pin 30 3.7 Bộ điều khiển APM 2.8 (Flight controller) 31 3.8 Bộ điều khiển từ xa 32 3.9 Cảm biến cân điện tử (loadcell) 32 3.10 Sơ đồ kết nối thiết bị 34 v Chương 35 4.1 Các khối điều khiển 35 4.1.1 Khối điều khiển độ cao (Altitude Controller Block) 35 4.1.2 Khối điều khiển hướng (Attitude & Heading Controller Block) 35 4.1.3 Khối điều khiển vị trí (Position Controller Block) 36 4.2 Các điều khiển (Controller) 37 4.2.1 Điều khiển PID (PID Controller) 37 4.2.2 Bộ điều khiển thích nghi mờ AFPID (Adaptive Fuzzy PID) 39 4.3 Thực giải thuật 42 4.3.1 Lập trình với thư viện Ardupilot 3.2.1 42 4.3.2 Cấu trúc vòng lặp (main loop) 42 4.3.3 Giải thuật điều khiển 44 Chương 60 5.1 Sơ đồ kết nối mô 60 5.1.1 Kết thiết kế 60 5.2 Kết thực nghiệm 68 5.2.1 Kiểm định giá trị đọc loadcell 68 5.2.2 Kiểm định điều khiển AFPID 69 5.3 Kết luận 71 5.4 Hướng phát triển tương lai 72 TÀI LIỆU THAM KHẢO 73 PHỤ LỤC 75 A Thông số Quadrotor 75 B Mô Matlab 76 C Thông số phần cứng 81 SƠ YẾU LÝ LỊCH 83 vi DANH MỤC HÌNH ẢNH Hình 1-1 Máy bay cánh Hình 1-2 Máy bay trực thăng trục đồng trục Hình 1-3 Quadrotor Hexrotor phiên thương mại Hình 1-4 Khinh khí cầu Hình 1-5 UAV mơ cử động chim bay vỗ cánh Hình 1-6 UAV cỡ lớn cỡ vừa Hình 1-7 UAV cỡ nhỏ siêu nhỏ Hình 1-8 Quadrotor (drone) thương mại phổ biến thị trường Hình 1-9 Mơ hình quadrotor, góc trục toạ độ Hình 1-10 Minh hoạ thay đổi vận tốc xoay rotor ảnh hướng đến hướng di chuyển quadrotor Hình 1-11 Ứng dụng quadrotor việc mang đồ cứu hộ giải cứu Hình 1-12 UAV ứng dụng ngành nông nghiệp công nghiệp vận tải Hình 1-13 Quy trình thực luận văn 13 Hình 2-2-1 Các hệ toạ độ Quadrotor 15 Hình 2-2-2 Các lực moment tác động lên Quadrotor 19 Hình 2-2-3 Schematic BLDC 21 Hình 3-1 Ảnh 3D Cad hình ảnh so sánh thực tế 27 Hình 3-2 Cấu tạo BLDC sức phản điện động hình sin, hình thang 27 Hình 3-3 cách quấn dây Rotor động BLDC 28 Hình 3-4 Nguyên lý xuất tín hiệu điều khiển động BLDC từ vi xử lý trung tâm 28 Hình 3-5 Động BLDC X2212 980KV II Motor 28 Hình 3-6 Cánh quạt nhựa 1047 29 Hình 3-7 ESC iPeaka 2S-5S 35A Dshot1200 30 Hình 3-8 Gens Ace 11.1V 5200mAh 50C 3S Lipo Battery XT60 Plug 30 Hình 3-9 APM 2.8 phiên giới hạn 31 Hình 3-10 Chú thích port ngoại vi APM 2.8 31 Hình 3-11 RX TX FlySky FS-i6 i6 2.4G 6CH 32 Hình 3-12 Loadcell phiên thương mại strain gauge 33 Hình 3-13 Sơ đồ đấu nối đọc giá trị khối lượng đo sử dụng loadcell, HX711 adruino 33 Hình 3-14 Hình ảnh lắp đặt thực tế loadcell quadrotor 33 Hình 3-15 Sơ đồ kết nối thiết bị quadrotor 34 Hình 4-4-1 Sơ đồ khối điều khiển độ cao 35 Hình 4-4-2 Sơ đồ khối cho điều khiển phương hướng 36 Hình 4-4-3 Sơ đồ khối điều khiển vị trí (hệ thống hồn chỉnh) 37 Hình 4-4-4 Sơ đồ khối điều khiển PID 37 Hình 4-4-5 Sơ đồ khối điều khiển AFPID cho trục z 40 Hình 4-4-6 Hàm liên thuộc cho ngõ vào cho sai số (𝑒𝑧), sai số (𝑒𝑧) 𝑚 41 Hình 4-4-7 Hàm liên thuộc cho ngõ alpha, beta gamma 42 Hình 4-4-8 Luật điều khiển mờ cho AFPID 42 Hình 4-9 Cấu trúc vịng lặp chương trình (main loop) 43 Hình 4-10 Các chế độ xử lý tác vụ, dòng lệnh APM 2.8 44 vii } double HX711::get_value(uint8_t times) { return read_average(times) - OFFSET; } float HX711::get_units(uint8_t times) { return get_value(times) / SCALE; } void HX711::tare(uint8_t times) { double sum = read_average(times); set_offset(sum); } void HX711::set_scale(float scale) { SCALE = scale; } float HX711::get_scale() { return SCALE; } void HX711::set_offset(long offset) { OFFSET = offset; } Hình 5-9 Thực hiện cân chỉnh loadcell 5.2.2 Kiểm định điều khiển AFPID Để kiểm định điều khiển AFPID, giá trị hệ số 𝛼, 𝛽, 𝛾 𝑘𝑃𝐴𝐹𝑃𝐼𝐷 , 𝑘𝐼𝐴𝐹𝑃𝐼𝐷 , 𝑘𝐷𝐴𝐹𝑃𝐼𝐷 giá trị tín hiệu điều khiển 𝑈1−4 thu thập kiểm định Tải tải thay đổi với giá trị khối lượng 0.159g 0.29g 69 Hình 5-10 Thu thập giá trị của bộ điều khiển AFPID thông qua UART Thử nghiệm thực tế quan sát, thu thập với giá trị m payload thay đổi:  Gắn tải có m = 0.159g t =120s  Tháo tải m = 0.159g t = 220s Hình 5-11 Giá trị thu thập trọng tải thay đổi 70 Hình 5-12 Hệ số alpha, beta gamma, kP,kI,kD, U1-4 cho bộ điều khiển AFPID Hình 5-133 Kết quả bay cân bằng thực tế 5.3 Kết luận Đề tài đưa giải trọn vẹn quy trình thiết kế điều khiển thích nghi mờ PID (AFPID) đáp ứng nhu cầu điều khiển Quadrotor cho mục đích vận tải với tải trọng biến thiên mức cho phép Đề tài trình bày giải vấn đề mơ hình hố, nhận dạng hệ thống Quadrotor Đồng thời đề xuất khối điều khiển xây dựng điều khiển PID cho khối điều khiển góc vị trí AFPID cho khối điều khiển độ cao Với kết kiểm định mơ phỏng, thấy kết từ điều khiển PID cho độ cao trọng tải biến thiên không đáp ứng yêu cầu chất lượng điều khiển Và, sử dụng điều khiển AFPID đề xuất, yêu cầu chất lượng điều khiển thoả mãn 71 Với ưu điểm trội điều khiển mờ áp dụng kinh nghiệm điều khiển, nhạy bền vững với nhiễu, dễ dàng lập trình vi điều khiển, điều khiển AFPID hứa hẹn khả sử dụng rộng rãi ngành công nghiệp vận tải quadrotor 5.4 Hướng phát triển tương lai Đề tài cho thấy hướng phát triển áp dụng cho ngành công nghiệp vận tải quadrotor với khả phổ biến triển khai nhanh Với tương lai xây dựng việc điều khiển vị trí cơng nghệ GPS, việc vận tải từ xa quadrotor với quỹ đạo điểm điểm lập trình phát triển mạnh mẽ 72 TÀI LIỆU THAM KHẢO [1] Kimon P.Valavanis, “Advances in Unmanned Aerial Vehicles,” 1st Ed., Springer Ltd., 2007 Link: https://www.researchgate.net/, date access: 09-01-2020 [2] D Mellinger, M Shomin, N Michael, and V Kumar, “Cooperative Grasping and Transport using Multiple Quadrotors,” in Distributed Autonomous Robotic Systems, Lausanne, Switzerland, 2010 Link: https://www.researchgate.net/, date access: 0901-2020 [3] H Bouadi, M Bouchoucha and M Tadjine, “Sliding Mode Control based on Backstepping Approach for an UAV Type-Quadrotor,” International Journal of Applied Mathematics and Computer Sciences, vol.4, no 1, pp 12-17, 2008 Link: https://www.researchgate.net/, date access: 09-01-2020 [4] Altug, E., Ostrowski, J P., and Taylor, C J., “Control of a Quadrotor Helicopter Using Dual Camera Visual Feedback,” In Proc 2003 IEEE Int Conf on Robotics and Automation, pp 4294-4299, 2003 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 03-03-2020 [5] Mandani, T., Benallegue, A., “Backstepping Control for a Quadrotor Helicopter,” Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, pp 3255-3260, 2006 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 01-022020 [6] S Bouabdallah and R Siegwart, “Backstepping and sliding-mode techniques applied to an indoor micro quadrotor,” in Proc (IEEE) International Conference on Robotics and Automation (ICRA05),Barcelona, Spain, 2005 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 03-03-2020 [7] A.A Mian, M.I Ahmad, D Wang, “Backstepping based PID Control Strategy for an Underactuated Aerial Robot,” Proceedings of the 17th World Congress, The International Federation of Automatic Control, Seoul, Korea, July 6-11, 2008 Link: https://www.researchgate.net/, date access: 09-01-2020 [8] B Erginer, E Altug, “Modelling and PD Control of a Quadrotor VTOL Vehicle,” Proceedings of the 2007 IEEE Intelligent Vehicles Symposium, Istanbul, Turkey, June 13-15, 2007 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 03-032020 [9] [9] R Xu and U Ozguner, “Sliding mode control of a quadrotor helicopter,” Proc of the 45th IEEE Conference on Decision and Control, pp 4957-4962, 2006 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 03-03-2020 [10] B C Min, C H Cho, K M Choi, and D H Kim, “Development of a Micro QuadRotor UAV for Monitoring an Indoor Environment,” Lecture Notes in Computer Science, Vol 5744/2009, pp 262-271, 2009 Link: https://www.researchgate.net/, date access: 09-01-2020 [11] Bin Yao and M Tomizuka, “Adaptive robust control of SISO nonlinear systems in a semi-strict feedback form,” Automatica, Vol 33, Issue 5, pp 893-900, 1997 Link: https://www.researchgate.net/, date access: 09-01-2020 73 [12] Li Xu and Bin Yao, “Adaptive robust precision motion control of linear motors with negligible electrical dynamics: theory and experiments,” IEEE/ASME Transactions on Mechatronics, Vo 6, Issue: 4, pp.444-452, 2001 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 03-03-2020 [13] Bin Yao, “Advanced Motion Control: From Classical PID to Nonlinear Adaptive Robust Control (Plenary Paper),” The 11th IEEE International Workshop on Advanced Motion Control, Nagaoka, Japan,2010 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 03-03-2020 [14] Bin Yao and M Tomizuka, “Adaptive Robust Control of a Class of Multivariable Nonlinear Systems,” In IFAC World Congress, Vol F, pp 335-340, 1996 Link: https://ieeexplore.ieee.org/Xplore/home.jsp, date access: 03-03-2020 [15] Byung-Cheol Min, Ji-Hyeon Hong, and Eric T Matson, “Adaptive Robust Control (ARC) for an Altitude Control of a Quadrotor Type UAV Carrying an Unknown Payloads”, the 11th International Conference on Control, Automation and Systems, Oct 26-29, 2011 in KINTEX, Gyeonggi-do, Korea Link: https://www.researchgate.net/, date access: 09-01-2020 [16] Goh Ming Qian, Dwi Pebrianti, Luhur Bayuaji, Rosdiyana Samad, Mahfuzah Mustafa and Mohammad Syafrullah, “Adaptive Fuzzy-PID Controller for QuadRotor MAV with Mass Changes”, © Springer Nature Singapore Pte Ltd 2019 Z Md Zain et al (eds.), Proceedings of the 10th National Technical Seminar on Underwater System Technology 2018, Lecture Notes in Electrical Engineering 538 Link: https://www.researchgate.net/, date access: 09-01-2020 [17] Samir Bouabdallah Design and control of quadrotors with application to autonomous flying Phd thesis, Ecole Polytechnique Federale de Lausanne, 2007 Link: https://www.researchgate.net/, date access: 09-01-2020 [18] Heba talla Mohamed Nabil ElKholy, Dynamic Modeling and Control of a Quadrotor Using Linear and Nonlinear Approaches, 2014 Link: https://www.researchgate.net/, date access: 09-01-2020 74 PHỤ LỤC A Thông số Quadrotor Bảng tra cứu thông số dùng mô Matlab, tham khảo từ nghiên cứu Samir Bouabdallah [17] Thông số Mô tả Giá trị Đơn vị 𝐼𝑥𝑥 Moment quán tính thân quadrotor theo trục x 7.50E-03 kg.m2 𝐼𝑦𝑦 Moment quán tính thân quadrotor theo trục x 7.50E-03 kg.m2 𝐼𝑧𝑧 Moment quán tính thân quadrotor theo trục x 1.30E-02 kg.m2 l Jr m Kf KM Rmot Kmot Kt Kt Chiều dài cánh tay quadrotor Moment quán tính rotor Khối lượng quadrotor Hằng số lực kéo khí động học Hằng số moment khí động học Trở nội rotor Hằng số moment xoắn rotor Hệ số khí động học chuyển động tịnh tiến Hệ số khí động học chuyển động xoay 0.23 6.00E-05 0.65 3.13E-05 7.50E-07 0.6 5.2 diag(0.1, 0.1, 0.15) diag(0.1, 0.1, 0.15) m kg.m2 kg N s2 Nm s2 Ω mNm/A Ns/m Nm s 75 B Mô Matlab B1 Thông số thiết lập cho Quadrotor clear clc close all Ts=0.01; %% Ineria Ixx=7.5e-3; Iyy=7.5e-3; Izz=1.3e-2; % axel length l=0.23; %% rotor inertia Jr=6e-5; %% mass m=0.65; %0.65 g=9.81; %% aerodynamic force and moments constant Kf=3.13e-5; KM=7.5e-7; %% constants calculations a1=(Iyy-Izz)/Ixx; a2=Jr/Ixx; a3=(Izz-Ixx)/Iyy; a4=Jr/Iyy; a5=(Ixx-Iyy)/Izz; b1=l/Ixx; b2=l/Iyy; b3=l/Izz; %% Rotor Dynamics R_mot=0.6; %Motor Circuit Resistance K_mot=5.2; %Motor Torque Constant % Disturbances noise_rot=0; noise_trans=0; %% PID Constants kp_X=7.1; ki_X=0; kd_X=5.2; kp_Y=7.1; ki_Y=0; kd_Y=5.2; kp_Z=4.7;%3.2; %5.2; kp_Z_min=4.2; %5.2; kp_Z_max=8; %5.2; ki_Z=3.4; %2; kd_Z=1.8;%0.5; %1.3; kd_Z_min=0.1; %1.3; kd_Z_max=1.8; %1.3; kp_Pitch=4.5; ki_Pitch=0; kd_Pitch=0.5; kp_Roll=4.5; ki_Roll=0; 76 kd_Roll=0.5; kp_Yaw=3.9; ki_Yaw=0; kd_Yaw=0.7; % desired velocities and accelerations initial Pitch2dot_d=0; Roll2dot_d=0; Yaw2dot_d=0; z2dot_d=0.1; y2dot_d=0; x2dot_d=0; Pitchdot_d=0; Rolldot_d=0; Yawdot_d=0; zdot_d=0; xdot_d=0; ydot_d=0; % Saturation omega_hover=sqrt((m*g)/(4*Kf)); deltaU1_max=2*Kf*4*omega_hover^2; deltaU2_max=2*Kf*omega_hover^2; deltaU3_max=2*Kf*omega_hover^2; deltaU4_max=2*KM*2*omega_hover^2; omega_max=0.9*9000*(2*pi/(60)); %90% of 9000 rpm to rad/s U1_max=Kf*4*omega_max^2; U2_max=Kf*omega_max^2; U3_max=Kf*omega_max^2; U4_max=KM*2*omega_max^2; Pitch_max = 20*(pi/90); %small angle