Kỹ thuật dẫn đường và truyền thông thời gian thực cho thiết bị bay không người lái phục vụ cứu hộ chữa cháy nhà cao tầng

NGUYỄN DUY HÙNG 2013B BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI NGUYỄN DUY HÙNG KỸ THUẬT DẪN ĐƢỜNG VÀ TRUYỀN THÔNG THỜI GIAN THỰC CHO THIẾT BỊ BAY KHÔNG NGƢỜI LÁI PHỤC VỤ CỨU HỘ KỸ THUẬT TRUYỀN THÔNG CHỮA CHÁY NHÀ CAO TẦNG LUẬN VĂN THẠC SĨ KỸ THUẬT KỸ THUẬT TRUYỀN THÔNG 2013B Hà Nội - 2014 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI NGUYỄN DUY HÙNG KỸ THUẬT DẪN ĐƢỜNG VÀ TRUYỀN THÔNG THỜI GIAN THỰC CHO THIẾT BỊ BAY KHÔNG NGƢỜI LÁI PHỤC VỤ CỨU HỘ CHỮA CHÁY NHÀ CAO TẦNG Chuyên ngành: Kỹ thuật truyền thông LUẬN VĂN THẠC SĨ KỸ THUẬT KỸ THUẬT TRUYỀN THÔNG NGƢỜI HƢỚNG DẪN KHOA HỌC: TS ĐỖ TRỌNG TUẤN Hà Nội - 2014 MỤC LỤC LỜI CAM ĐOAN iii DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT iv DANH MỤC CÁC BẢNG v DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ vi MỞ ĐẦU ix CHƢƠNG TỔNG QUAN VỀ CÔNG TÁC PCCC VÀ PHƢƠNG TIỆN BAY 10 1.1 Khảo sát công tác PCCC cho nhà cao tầng địa bàn TP Hà Nội 10 1.1.1 Thực trạng cháy nhà cao tầng địa bàn TP Hà Nội 10 1.1.2 Khảo sát thiết bị hỗ trợ PCCC cho nhà cao tầng 11 1.2 Tổng quan hệ thống phƣơng tiện bay 14 1.2.1 Sơ lƣợc UAV 14 1.2.2 Sơ đồ tổng quan hệ thống Multicopter 18 1.2.3 Cấu trúc thiết bị bay 19 1.3 Yêu cầu tính phƣơng tiện bay 26 CHƢƠNG BÀI TOÁN DẪN ĐƢỜNG CHO PHƢƠNG TIỆN BAY 27 2.1 Phƣơng án sử dụng phƣơng tiện bay hỗ trợ công tác PCCC 27 2.2 Bài toán điều khiển dẫn đƣờng cho thiết bị bay 29 CHƢƠNG HỆ THỐNG ĐỊNH VỊ GPS VÀ HỆ ĐỊNH VỊ QUÁN TÍNH INS 30 3.1 Hệ thống định vị toàn cầu GPS 30 3.1.1 Tín hiệu GPS 30 3.1.2 Nguồn lỗi tín hiệu GPS 31 3.2 Hệ định vị quán tính INS 31 3.3 Đơn vị đo lƣờng quán tính IMU 32 3.3.1 Xử lý liệu từ cảm biến gia tốc Accelerometer 34 3.3.2 Xử lý liệu từ cảm biến vận tốc góc Gyroscope 35 3.3.3 Xử lý liệu từ cảm biến từ trƣờng Magnetometer 35 3.4 Bộ lọc sai số sử dụng cho IMU 37 3.5 Dead Reckoning 38 CHƢƠNG KẾT HỢP GPS VÀ INS 41 4.1 Bộ lọc Kalman 41 4.1.1 Giới thiệu lọc Kalman 41 4.1.2 Bộ lọc Kalman rời rạc 41 4.2 Áp dụng lọc Kalman tích hợp INS GPS 42 4.3 Xây dựng thuật toán chƣơng trình 47 4.4 Kết mô dẫn đƣờng GPS INS 48 CHƢƠNG : BỘ ĐIỀU KHIỂN DẪN ĐƢỜNG 50 5.1 Tổng quan điều khiển 50 5.2 Bộ điều khiển PID lớp 52 5.3 Bộ điều khiển PID hai lớp 58 i 5.3.1 Thuật toán điều khiển 58 5.3.1 Mô điều khiển PID hai lớp 59 CHƢƠNG : THIẾT KẾ VÀ CHẾ TẠO MẠCH ĐIỀU KHIỂN 71 6.1 Sơ đồ khối chức 71 6.2 Vi điều khiển ATMEGA2560 72 6.4 Khối vi điều khiển ATMEGA32U2 74 6.5 Khối cảm biến 76 6.6 Khối IC flash AT45DB161D 77 6.7 Thiết kế mạch in 78 CHƢƠNG : TRUYỀN THÔNG THỜI GIAN THỰC CHO THIẾT BỊ BAY 81 7.1 Mơ hình truyền thơng 81 7.2 Giao thức MAVLink 82 7.3 Đánh giá độ trễ truyền thông 83 CHƢƠNG : KẾT QUẢ THỰC NGHIỆM THIẾT BỊ BAY TRONG CÔNG TÁC PCCC 86 KẾT LUẬN 89 TÀI LIỆU THAM KHẢO 90 PHỤ LỤC 91 ii LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi Các số liệu kết nghiên cứu luận văn trung thực không trùng lặp với đề tài khác TÁC GIẢ LUẬN VĂN Nguyễn Duy Hùng iii DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT UAV Unmanned Aerial Vehicle Máy bay không ngƣời lái GPS Hệ thống định vị toàn cầu INS Global Position System Global Navigation Satellite System Inertial Navigation System IC Intergrated Circuit Mạch tích hợp USB Universal Serial Bus IMU Inertial Measurement Unit Chuẩn kết nối đa dụng Đơn vị đo lƣờng quán tính ESC Electronic Speed Controller[3] Điều tốc điện tử UART Chuẩn truyền/nhận không đồng SPI Universal Asynchronous Receiver/Transmitter Serial Peripheral Interface PC Personal Computer Máy tính cá nhân PWM Pulse Width Modulator I2 C Inter-Integrated Circuit BLDC Brushless DC Modun điều chế độ rộng xung Truyền thông nối tiếp đồng đƣờng dây Động chiều khơng chổi than PCCC Phịng cháy chữa cháy Rx Receiver Bộ thu tín hiệu điều khiển Tx Transmitter Bộ phát tín hiệu điều khiển RC Radio Controller Bộ điều khiển vô tuyến MEMS Microelectromecanical Systems Hệ thống vi điện tử GNSS Hệ thống vệ tinh định vị tồn cầu Hệ thống định vị qn tính Truyền thông nối tiếp đồng iv DANH MỤC CÁC BẢNG Bảng 1: Tên gọi loại Multicopter 15 Bảng 2: Phân loại UAV theo cự ly hoạt động khối lƣợng 17 Bảng 3: Thông số module RF 900MHz 21 Bảng 4: Tín hiệu đầu vào khối điều khiển 59 Bảng 5: Tín hiệu đầu motor 60 Bảng 6: Các tín hiệu đầu vào khối cảm biến 61 Bảng 7: Các tín hiệu đầu khối cảm biến 62 Bảng 8: Các tín hiệu đầu vào khối chuẩn hóa tốc độ góc 63 Bảng 9: Các tín hiệu đầu khối chuẩn hóa tốc độ góc 63 Bảng 10: Các tín hiệu đầu vào khối PID lớp 64 Bảng 11: Cái tín hiệu đầu khối PID lớp 65 Bảng 12: Các tín hiệu đầu vào khối PID lớp 65 Bảng 13: Các tín hiệu đầu khối PID lớp 65 Bảng 14 Cấu trúc tin MAVLink 82 v DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ Hình 1.1: Tiếp cận đám cháy nhà cao tầng hoàn toàn phụ thuộc xe thang 10 Hình 1.2: Xe chữa cháy khơng thể tiếp cận trƣờng 11 Hình 1.3: Bình chữa cháy 12 Hình 1.4: Mặt nạ phòng độc 13 Hình 1.5: Chai nƣớc khăn mặt 13 Hình 1.6: Thang dây cứu nạn 14 Hình 1.7: UAV Multicopter – Quadcopter 15 Hình 1.8: UAV cánh 16 Hình 1.9: UAV đĩa bay 16 Hình 1.10: Sơ đồ hệ thống quadcopter 18 Hình 1.11: Quadcopter 19 Hình 1.12: Cấu trúc hệ thống phƣơng tiện bay 19 Hình 1.13: TX Audora hãng Hitec 20 Hình 1.14: Bộ thu tín hiệu điều khiển Optima 20 Hình 1.15: Module RFD 900 nguyên đƣợc đóng gói 21 Hình 1.16: Động BLCD Tiger MT3506 22 Hình 1.17: Sơ đồ kết nối ESC 23 Hình 1.18: Cánh quạt 23 Hình 1.19: Pin lipo Turnigy 24 Hình 1.20: Module GPS U-blox NEO-6M 25 Hình 2.1: Sơ đồ 3D tồ nhà xảy đám cháy 28 Hình 2.2: Mơ hình phƣơng tiện bay khơng ngƣời lái UAV X16 29 Hình 3.1: Góc Yaw, Pitch Roll 32 Hình 3.2: Các góc Yaw, Pitch Roll hệ thống định vị quán tính 33 Hình 3.3: Vector R đƣợc chiếu lên trục Accelerometer 34 Hình 3.4: Dữ liệu cảm biến từ trƣờng 35 Hình 3.5: Mơ hình phƣơng tiện bay hệ trục tọa độ NEU 38 Hình 3.6: Chuyển động phƣơng tiện bay với góc Pitch 39 Hình 3.7: Chuyển động phƣơng tiện bay với góc Roll 39 Hình 4.1: Sơ đồ trình lọc Kalman 42 Hình 4.2: Giải thuật chƣơng trình 47 Hình 4.3: Đồ thị quỹ đạo tín hiệu GPS, khơng có INS hỗ trợ 48 Hình 4.4: Đồ thị quỹ đạo GPS nhƣng có INS hỗ trợ 49 Hình 5.1: Thơng số điều khiển 50 Hình 5.2: Các điều khiển cho phƣơng tiện bay 52 Hình 5.3: Bộ điều khiển PID lớp 52 vi Hình 5.4: Bộ điều khiển PID lớp với thiết bị bay 54 Hình 5.5 : Đồ thị ứng với mức ga thấp ( mức ga đủ để thiết bị bay nhấc bắt đầu cất cánh) 55 Hình 5.6: Đồ thị ứng với mức ga trung bình 55 Hình 5.7: Đồ thị ứng với mức ga lớn 55 Hình 5.8: Cơ cấu kiểm tra PID trục 56 Hình 5.9: Đồ thị kiểm tra PID góc Roll thực tế 56 Hình 5.10: Thuật tốn điều khiển PID hai lớp 58 Hình 5.11: Chi tiết thành phần khối điều khiển 59 Hình 5.12: Khối cảm biến tổng hợp khối chuẩn hóa góc pitch roll 61 Hình 5.13: Khối chuẩn hóa tốc độ góc p, q, r 62 Hình 5.14: Sơ đồ khối mô điều khiển PID hai lớp 64 Hình 5.15: Đồ thị ứng với mức ga thấp 66 Hình 5.16: Đồ thị ứng với mức ga trung bình 66 Hình 5.17: Đồ thị ứng với mức ga cao 66 Hình 5.18: Đồ thị ứng với mức ga thấp 67 Hình 5.19: Đồ thị ứng với mức ga trung bình 67 Hình 5.20: Đồ thị ứng với mức ga cao 67 Hình 5.21: Đồ thị liệu thu đƣợc từ hệ số PID thứ 68 Hình 5.22: Đồ thị liệu thu đƣợc từ hệ số PID thứ hai 68 Hình 5.23: Đồ thị liệu thu đƣợc từ hệ số PID thứ ba 69 Hình 6.1: Sơ đồ khối mạch điều khiển phƣơng tiện bay 71 Hình 6.2: Tín hiệu vào/ra vi điều khiển Atmega2560 73 Hình 6.3: Sơ đồ nguyên lý vi điều khiển Atmega2560 74 Hình 6.4: Tín hiệu vào, vi điều khiển ATMega32U2 75 Hình 6.5: Sơ đồ nguyên lý khối vi điều khiển Atmega32U2 75 Hình 6.6: Tín hiệu cảm biến 76 Hình 6.7: Sơ đồ nguyên lý cảm biến MPU6000 MS5611 77 Hình 6.8: Sơ đồ nguyên lý cảm biến HMC5883L 77 Hình 6.9: Tín hiệu vào/ra IC Flash AT45DB161D 77 Hình 6.10: Sơ đồ nguyên lý IC Flash AT45DB161D 78 Hình 6.11: Hình ảnh thiết kế mạch điều khiển phƣơng tiện bay 79 Hình 6.12: Hình ảnh mạch điều khiển phƣơng tiện bay hồn thiện 80 Hình 7.1: Mơ hình hệ thống truyền thơng 81 Hình 7.2: Bộ thu phát RFD 900 82 Hình 7.3: Cấu trúc gói liệu MAVLink 82 Hình 7.4: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 0.4m 83 Hình 7.5: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 10m 84 Hình 7.6: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 20m 84 Hình 7.7: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 50m 84 vii Hình 7.8: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 100m 85 Hình 7.9: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 300m 85 Hình 8.1: Toà nhà chƣa xảy đám cháy bị khói bao phủ 86 Hình 8.2: Thiết bị bay cất cánh tiếp cận đám cháy 86 Hình 8.3: Hình ảnh đám cháy truyền từ thiết bị bay (quan sát trạm mặt đất) 87 Hình 8.4: Trinh sát tình hình thực tế đám cháy 87 Hình 8.6: Hình ảnh quan sát tới vị trí thả đồ( khoanh trịn màu đỏ) 88 Hình 8.7: Thiết bị bay thả đồ cứu hộ cứu nạn cho nạn nhân 88 viii IC flash có chức ghi liệu thông số offset cảm biến Nhóm đề tài sử dụng TXB0104 chuyển mức logic để giao tiếp Atmega2560 IC flash IC flash AT45DB161D có kích thƣớc nhớ 16MB có mức điện áp hoạt động 3.3V nên muốn giao tiếp với vi điều khiển Atmega2560 cần sử dụng IC đệm TXB0104 để chuyển mức điện áp 3.3 sang 5V Hình 6.10: Sơ đồ nguyên lý IC Flash AT45DB161D 6.7 Thiết kế mạch in Từ mạch nguyên lý nhƣ phân tích phần trên, mạch điều khiển thiết bị bay đƣợc chia thành phần mạch vi điều khiển mạch cảm biến Lý cần tách riêng khối thành hai mạch muốn giảm nhiễu tín hiệu cho cảm biến giảm thiểu diện tích mạch để dễ dàng lắp đặt phƣơng tiện bay  Mạch vi điều khiển: Kích thƣớc: 66x89mm  Mạch cảm biến: Kích thƣớc: 31x37mm 78 Hình 6.11: Hình ảnh thiết kế mạch điều khiển phƣơng tiện bay 79 Hình ảnh dƣới sản phẩm mạch điều khiển hoàn thiện khâu hàn linh kiện Mạch cảm biến đƣợc bố trí gắn cố định tầng mạch vi điều khiển Hình 6.12: Hình ảnh mạch điều khiển phƣơng tiện bay hoàn thiện 80 CHƢƠNG : TRUYỀN THƠNG THỜI GIAN THỰC CHO THIẾT BỊ BAY 7.1 Mơ hình truyền thơng Hình 7.1: Mơ hình hệ thống truyền thơng Trong q trính vận hành, thơng số vị trí input, output động cơ, thơng số điều khiển, góc nghiêng, hƣớng, vị trí, vận tốc đƣợc thiết bị bay cập nhật liên tục gửi trạm mặt đất thông qua đƣờng truyền RF Về phần cứng: tác giả sử dụng thu phát sóng RFD 900MHz để phù hợp với điều khiển tầm nhìn thẳng quãng ngắn (50m) Bộ thu phát sử dụng băng tần UHF có bƣớc sóng siêu ngắn từ 10cm-1m nên kích thƣớc anten nhỏ (xấp xỉ bƣớc sóng), dễ dàng lắp đặt thiết bị bay 81 Hình 7.2: Bộ thu phát RFD 900 Giao thức truyền thông đƣợc sử dụng giao thức MAVLink (Micro Air Vehicle Link) MAVLink giao thức đƣợc sử dụng ứng dụng thực tế điều khiển thiết bị không ngƣời lái 7.2 Giao thức MAVLink Cấu trúc gói liệu MAVLink gồm có trƣờng liệu nhƣ sau: Start of Frame [0] Payload Lengh [1] Packet Sepuence [2] System ID [3] Component ID [4] Message ID [5] Payload [6, n+6] CRC [n+7,n+8] Hình 7.3: Cấu trúc gói liệu MAVLink Trong đó, trƣờng đƣợc gắn số mô tả nhƣ bảng dƣới: Bảng 14 Cấu trúc tin MAVLink Field name Index Size Decription Start-of-frame byte Bắt đầu khung Luôn 0xFE Payload length 1 byte Độ dài Payload Packet sequence byte System ID byte Trình tự gói liệu đƣợc gửi component Cho phép phát gói (0 - 255) ID xác định hệ thống gửi tin Cho phép phân biệt hệ thống mạng ID xác định component gửi tin Cho phép xác Component ID định component khác hệ byte thống Message ID Payload 6-(n+6) n bytes ID xác định tin byte Dữ liệu tin Phụ thuộc vào ID tin 82 CRC (n+7)(n+8) bytes Check-sum Trƣờng Start-of-frame cho ta biết điểm bắt đầu khung truyền Phiên MAVLink 1.0 có Start-of-frame 0xFE Giữa byte có giá trị 0xFE liên tiếp ta xác định đƣợc tin MAVLink Một đƣờng truyền MAVLink đƣợc sử dụng nhiều hệ thống, hệ thống lại có nhiều thành phần gửi tin MAVLink (khối IMU, GPS,…) Vậy để xác định hệ thống gửi tin ta dựa vào trƣờng System ID, để xác định thành phần hệ thống gửi tin ta dựa vào Component ID Ta ý đến trƣờng Message ID Message ID thành phần dùng xác định tin nhận đƣợc tin Trong MAVLink, tin có ID xác định, kích thƣớc 1byte Giá trị ID nằm khoảng [0,255] Tuy nhiên, MAVLink có 92 tin, nên số giá trị không ID tin 7.3 Đánh giá độ trễ truyền thơng Hình 7.4: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 0.4m 83 Hình 7.5: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 10m Hình 7.6: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 20m Hình 7.7: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 50m 84 Hình 7.8: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 100m Hình 7.9: Đồ thị trễ truyền tin RFD 900 với khoảng cách truyền 300m 85 CHƢƠNG : KẾT QUẢ THỰC NGHIỆM THIẾT BỊ BAY TRONG CÔNG TÁC PCCC Trong tháng 8/2014 thiết bị bay đƣợc nghiên cứu chế tạo đƣợc tham gia thử nghiệm đợt tập huấn PCCC phòng CS PCCC Sơn Tây – Hà Nội Kịch tham gia hỗ trợ PCCC đƣợc thực theo chƣơng Hình 8.1: Tồ nhà chƣa xảy đám cháy bị khói bao phủ Lực lƣợng chữa cháy triển khai thiết bị chữa cháy động nhƣ bình bột, bình CO2 v.v… tiến hành dập lửa nhỏ, mở đƣờng vào bên tịa nhà Trong lúc này, khói tiếp tục bốc lên, làm hạn chế tầm nhìn, hạn chế khả cứu hộ Các xe chữa cháy đƣợc đƣa tới sát vị trí tịa nhà Hệ thống cấp nƣớc vịi phun đƣợc triển khai Hình 8.2: Thiết bị bay cất cánh tiếp cận đám cháy 86 Hình 8.3: Hình ảnh đám cháy truyền từ thiết bị bay (quan sát trạm mặt đất) Sau thiết bị bay cất cánh, tăng dần độ cao, thiết bị bay bắt đầu thực nhiệm vụ trinh sát, thu thập tình hình thực tế, hình ảnh video gửi trực tiếp trạm mặt đất Nhƣ hình 7.8 hình ảnh thu đƣợc trạm mặt đất thấy nạn nhân đám cháy chạy lên tầng thƣợng tìm cách xuống mặt đất khói đám cháy sinh bao phủ hồn tồn tồ nhà Hình 8.4: Trinh sát tình hình thực tế đám cháy Phát nạn nhân đứng sân thƣợng tòa nhà, ngƣời điều khiển cho thiết bị bay tiến vào Camera đƣợc điều khiển nhìn xuống dƣới để xác định vị trí thả vật cứu hộ thuận lợi 87 Sau quan sát đến điểm thả đồ từ trạm mặt đất, thiết bị bay bắt đầu thả đồ cứu hộ cứu nạn Hình 8.5: Hình ảnh quan sát tới vị trí thả đồ( khoanh trịn màu đỏ) Hình 8.6: Thiết bị bay thả đồ cứu hộ cứu nạn cho nạn nhân 88 KẾT LUẬN Trong tình hình công tác PCCC đƣợc đẩy mạnh nâng cao vể chất lƣợng lẫn số lƣợng phƣơng tiện PCCC luận văn đóng góp sản phẩm hữu ích, đƣợc kiểm nghiệm đánh giá qua thực tế triển khai Sơn Tây- Hà Nội Trên sở vận dụng phƣơng pháp nghiên cứu, luận văn đạt đƣợc kết chủ yếu nhƣ sau: Thứ nhất, hoàn thành mạch điều khiển cho thiết bị bay, thiết bị điện tử quan trọng thiết bị bay Thứ hai, đƣa kiểm nghiệm điều khiển PID hai lớp hệ số phù hợp cho thiết bị bay Xác định đánh giá thông số truyền thông thời gian thực Thứ ba, luận văn đề xuất kết hợp hệ thống định vị vệ tinh GPS với hệ thống định vị quán tính INS nhằm tăng cƣờng chất lƣợng tín hiệu đầu vào cho thiết bị bay Đây bƣớc quan trọng để thiết bị bay hoạt động với độ tin cậy cao 89 TÀI LIỆU THAM KHẢO [1] Đỗ Trọng Tuấn et al (2013) “Nghiên cứu, thiết kế, chế tạo phƣơng tiện bay hỗ trợ cơng tác phịng cháy chữa cháy địa bàn thành phố Hà Nội”, mã số 01C – 02/04/2013 – 2, Sở KHCN thành phố Hà Nội [2] Eric N Johnson, Michael A Turbe, Allen D Wu, Suresh K Kannan and James C Neidhoefer, “Flight Test Results of Autonomous Fixed-Wing UAV Transitions to and from Stationary Hover”, AIAA Guidance, Navigation, and Control Conference and Exhibit 21 - 24 August 2006, Keystone, Colorado [3] Francois Hugon, Robert F Hartley, Brian DeRosa, Christopher Carvalho, (2012), “ArduPilot 2.0 Simulink Block Set”, Embry-Riddle Aeronautical University Savannah, Georgia, Embry-Riddle Aeronautical University Daytona Beach, Floria [4] Guillaume J J D., (2009) Fault-tolerant Flight Control and Guidance Systems: Practical Methods for Small Unmanned Aerial Vehicles Springer [5] Harbick, K., Montgomery, J F., and Sukhatme, G S., (2004), “Planar Spline Trajectory Following for an Autonomous Helicopter”, Journal of Advanced Computational Intelligence – Computational Intelligence in Robotics and Automation, Vol 8, No 3, 2004, pp 237–242 [6] Randal Beard, Derek Kingston, Morgan Quigley, Deryl Snyder, Reed Christiansen, Walt Johnson, Timothy McLain,and Michael A Goodrich, “Autonomous Vehicle Technologies for Small Fixed-Wing UAVs”, Journal of Aerospace computing, inormation and communication, Vol 2, January 2005 [7] Samir B., Roland S., (2007) “Full Control of a Quadrotor” Proc of the 2007 IEEE/RSI International Conference on IROS, San Diego, CA, USA, ISBN 14244-0912-8, pp 153-158 [8] Vikas Kumar N, (2004), “Intergration of Inertial Navigation System and Global Positioning System Using Kalman Filtering”, Department of Aerospace engineering indian institite of Technology, Bombay, Mumbai 90 PHỤ LỤC Code thuật tốn PID mơ float CalcPitchPID(float dt ,float *Total_Error_Pitch , float Setpoint , float Input , float Gyros_OutputPitch , float KpPitch , float KiPitch , float KdPitch ) { float Output; float Error_Pitch; /*Compute all the working error variables*/ Error_Pitch = Setpoint - Input; *(Total_Error_Pitch)=*(Total_Error_Pitch)+Error_Pitch; Output = KpPitch*Error_Pitch + *(Total_Error_Pitch)* KiPitch*dt+ KdPitch*Gyros_OutputPitch*(-1); return Output; } float CalcRollPID(float dt ,float *Total_Error_Roll , float Setpoint , float Input , float Gyros_OutputRoll , float KpRoll , float KiRoll , float KdRoll ) { float Output; float Error_Roll; /*Compute all the working error variables*/ Error_Roll = Setpoint - Input; *(Total_Error_Roll)=*(Total_Error_Roll)+Error_Roll; Output = KpRoll*Error_Roll + *(Total_Error_Roll)* KiRoll*dt+ KdRoll*Gyros_OutputRoll*(-1); return Output; } float CalcYawPID(float dt ,float *Total_Error_Yaw , float Setpoint , float Input , float KpYaw , float KiYaw , float KdYaw ) { float Output; float Error_Yaw; /*Compute all the working error variables*/ Error_Yaw = Setpoint - Input; *(Total_Error_Yaw)=*(Total_Error_Yaw)+Error_Yaw; Output = KpYaw*Error_Yaw + *(Total_Error_Yaw)* KiYaw*dt+ KdYaw*Error_Yaw/dt; return Output; } 91 float CalcRollGyroPID(float dt ,float *Total_Error_GyroRoll , float Setpoint , float Input , float KpGyroRoll , float KiGyroRoll , float KdGyroRoll ) { float Output; float Error_GyroRoll; /*Compute all the working error variables*/ Error_GyroRoll = Setpoint - Input; *(Total_Error_GyroRoll)=*(Total_Error_GyroRoll)+Error_G yroRoll; Output = KpGyroRoll*Error_GyroRoll + *(Total_Error_GyroRoll)* KiGyroRoll*dt+ KdGyroRoll*Error_GyroRoll/dt; return Output; } float CalcPitchGyroPID(float dt ,float *Total_Error_GyroPitch , float Setpoint , float Input , float KpGyroPitch , float KiGyroPitch , float KdGyroPitch ) { float Output; float Error_GyroPitch; /*Compute all the working error variables*/ Error_GyroPitch = Setpoint - Input; *(Total_Error_GyroPitch)=*(Total_Error_GyroPitch)+Error _GyroPitch; Output = KpGyroPitch*Error_GyroPitch + *(Total_Error_GyroPitch)* KiGyroPitch*dt+ KdGyroPitch*Error_GyroPitch/dt; return Output; } output_pitch = CalcPitchPID(t, &total_error_pitch , setpointPitch , angles[2] ,-gyro[0] , KpPitch , KiPitch , KdPitch ); output_roll = CalcRollPID(t, &total_error_roll , setpointRoll , angles[1] ,gyro[1] , KpRoll , KiRoll , KdRoll ); output_pitch = CalcPitchGyroPID(t ,&total_error_gyropitch , output_pitch , gyro[0] , KpGyroPitch , KiGyroPitch , KdGyroPitch ); output_roll = CalcRollGyroPID(t,&total_error_gyroroll, output_roll, -gyro[1] , KpGyroRoll , KiGyroRoll , KdGyroRoll ); 92 ... GIÁO DỤC VÀ ĐÀO TẠO TRƢỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI NGUYỄN DUY HÙNG KỸ THUẬT DẪN ĐƢỜNG VÀ TRUYỀN THÔNG THỜI GIAN THỰC CHO THIẾT BỊ BAY KHÔNG NGƢỜI LÁI PHỤC VỤ CỨU HỘ CHỮA CHÁY NHÀ CAO TẦNG Chuyên... mang đƣợc thiết bị hỗ trợ cứu hộ cứu nạn cần thiết phục vụ cho cơng tác tìm kiếm cứu nạn có cháy xảy 1.1.2 Khảo sát thiết bị hỗ trợ PCCC cho nhà cao tầng Khi đám cháy xảy tòa nhà cao tầng, lực lƣợng... điều kiện để thiết bị bay hoạt động Đó trở ngại lớn việc thực thuật toán dẫn đƣờng cho thiết bị bay không ngƣời lái Phƣơng án giải Xây dựng điều khiển cho thiết bị bay đồng thời khắc phục vấn đề