韓鈺寒　李宏達　劉佳樂　崔鵬　段俊博

摘? 要：基于ROS的無人機飛行控制研究是目前無人機領(lǐng)域一項十分重要的研究內(nèi)容。文章在ROS melodic中建立四旋翼無人機的Gazebo模型，其與Px4飛控、QGC地面站之間利用MAVLink建立穩(wěn)定低耗的無線雙向通信，對無人機的飛行與軌跡規(guī)劃等功能進行研究。結(jié)果表明：文章在ROS中所仿真的四旋翼無人機能夠?qū)崿F(xiàn)人為控制飛行與軌跡規(guī)劃等功能，并且理論數(shù)據(jù)與實際飛行數(shù)據(jù)誤差小。

關(guān)鍵詞：ROS；無人機；Gazebo；QGC；飛行與軌跡規(guī)劃

中圖分類號：TP391.9；V217+.39 文獻標識碼：A 文章編號：2096-4706（2023）20-0075-06

Research on UAV Simulation Control Based on ROS

HAN Yuhan， LI Hongda， LIU Jiale， CUI Peng， DUAN Junbo

（Harbin Far East Institute of Technology， Harbin? 150025， China）

Abstract： Research on UAV flight control based on ROS is a very important research in the field of UAV at present. This paper builds a Gazebo model of quadrotor UAV in ROS melodic， and uses MAVLink to establish stable and low-consumption wireless two-way communication between the remaining Px4 flight control and QGC ground station to research the UAV functions of flight and trajectory planning. The results show that the quadrotor UAV simulated in ROS in this paper can realize the functions of human-controlled flight and trajectory planning， and the error between the theoretical data and the actual flight data is small.

Keywords： ROS; UAV; Gazebo; QGC; flight and trajectory planning

0? 引? 言

不管在軍用還是商用方面，四旋翼無人機的自主飛行一直都是研究熱點，實際應(yīng)用的領(lǐng)域也越來越多，如：農(nóng)業(yè)植保、航空拍攝、安全巡邏、地震救援，等等。通過四旋翼無人機執(zhí)行危險任務(wù)，在保障人身安全的同時，還可以很好地降低成本?；谶@些需求，各大研究機構(gòu)和企業(yè)都投入大量精力加強對四旋翼無人機的研究[1，2]。

人工智能的崛起也促進了無人機領(lǐng)域的快速發(fā)展。近年來，隨著深度圖像識別和三維激光雷達的引入，為無人機仿真控制研究帶來了前所未有的巨大挑戰(zhàn)。具備高精度和自主飛行能力的旋翼無人機成為目前研究的熱門方向[3，4]。由于ROS（Robot Operating System）的模塊化設(shè)計，架構(gòu)簡單精練，集成度高以及便于測試等優(yōu)點，所以基于ROS的無人機控制系統(tǒng)會逐漸地成為無人機控制算法、無人機導航避障算法、無人機多機協(xié)同等眾多算法的驗證平臺。

本文對基于ROS的四旋翼無人機飛行進行仿真研究。首先，在ROS melodic中建立四旋翼無人機的Gazebo三維物理仿真模型；其次，利用MAVLink（Micro Air Vehicle Link）通信協(xié)議在Px4飛控與QGC（QGround Control）地面站之間建立無線雙向通信；然后，使用基于擴展卡爾曼濾波的MSF多傳感器融合框架對無人機進行狀態(tài)估計；最后，對無人機的飛行與軌跡規(guī)劃等功能進行仿真研究。

1? 四旋翼無人機系統(tǒng)數(shù)學模型

1.1? 坐標系建立

本文四旋翼飛行器采用“+”型結(jié)構(gòu)，如圖1所示。通過控制四個旋翼的轉(zhuǎn)速實現(xiàn)懸停、垂直起降、偏航、俯仰、滾轉(zhuǎn)等飛行姿態(tài)。

為了描述飛行器在空間中的位姿，需要分別建立慣性坐標系W（OW XW YW ZW）和飛行坐標系q（oq xq yq zq），慣性坐標系W與飛行坐標系q均滿足右手定則。如圖1所示。

圖1中四旋翼飛行器在空間中姿態(tài)的變化可以用歐拉角進行表示。機體坐標系相對于初始狀態(tài)繞xq、yq、zq軸旋轉(zhuǎn)的角度分別記為滾轉(zhuǎn)角?、俯仰角θ、偏航角ψ[5，6]。繞三軸旋轉(zhuǎn)的關(guān)系矩陣公式R為：

2? 無人機仿真環(huán)境搭建

本文的仿真研究所用的系統(tǒng)為基于Ubuntu18的ROS melodic。

2.1? MAV系統(tǒng)與Gazebo無人機三維建模

2.1.1? MAV系統(tǒng)

在MAV（Micro Air Vehicle）系統(tǒng)中，使用的所有組件均可以使用Gazebo的插件和物理引擎模擬出來。本文的仿真無人機模型由機身框架、旋翼和傳感器組成；將實驗所用到的傳感器（如慣性運動單元）連接到無人機上并且將在無人機傳感器中添加噪聲來模擬實際飛行情況。MAV系統(tǒng)結(jié)構(gòu)如圖2所示。

2.1.2? Gazebo無人機三維建模

Gazebo具有強大的物理引擎、高質(zhì)量的圖形渲染、方便的編程與圖形接口等優(yōu)點。本文在Gazebo中建立的四旋翼無人機三維物理仿真模型如圖3所示。

2.2? QGC與PX4

2.2.1? QGC地面控制站

本文使用QGC為PX4動力飛行器提供飛行器參數(shù)設(shè)置和飛行環(huán)境。

2.2.2? PX4飛控

本文在無人機控制方面采用PX4飛控。PX4軟件架構(gòu)主要包括中間層和飛行控制棧。中間層用于提供內(nèi)部及外部的通信和硬件集成，具有存儲、通信和仿真等功能，可控制仿真環(huán)境中的無人機模型，實現(xiàn)對飛控程序正確性的驗證；飛行控制棧用于估計和控制無人機系統(tǒng)，是無人器導航、制導與控制的集成，負責各種類型載具的控制與位姿估計，其結(jié)構(gòu)框圖如圖4所示。

2.3? Ethzasl MSF多傳感器融合框架

本文用Ethzasl MSF多傳感器融合框架對無人機的位姿進行狀態(tài)估計。MSF使用基于6D模型信息的EKF（ExtendedKalmanFilter）對傳感器數(shù)據(jù)和IMU（Inertial Measurement Unit）數(shù)據(jù)進行整個從而獲得完成的狀態(tài)。該框架本質(zhì)分為預測（基于系統(tǒng)模型和IMU讀數(shù)進行）和更新兩個階段?？蚣艿暮诵牟糠郑╩sg_core）設(shè)計用于最通用的更新傳感器：相對于其自己的參考系的任意縮放的6DoF姿勢測量，相對于（重力對齊）固定導航框的位置和姿態(tài)會發(fā)生漂移。MSF框架的算法基本流程如圖5所示。

從圖5中可以看出，MSF框架以main函數(shù)開始；其次創(chuàng)建系統(tǒng)模型（微分方程）和測量更新模型；然后讀取激光雷達或深度相機等傳感器和IMU數(shù)據(jù)；最后進行濾波器更新，發(fā)布消息話題。

圖6為本文實驗中未用MSF框架對無人機傳感器和IMU數(shù)據(jù)進行融合時的計算圖。

從圖6中可以看出，gazebo節(jié)點將里程計（odometry）信息直接發(fā)送給位姿控制節(jié)點（lee_position_controller_node），位姿控制節(jié)點將速度指令（motor_speed）直接下發(fā)給gazebo。

圖7為使用MSF框架對無人機傳感器和IMU數(shù)據(jù)進行融合后將狀態(tài)估計數(shù)據(jù)下發(fā)給gazebo節(jié)點的計算圖。

從圖7中可以看出，Gazebo節(jié)點將位姿協(xié)方差數(shù)據(jù)（pose_with_covariance）發(fā)送給MSF框架，然后將經(jīng)過MSF融合得到的狀態(tài)估計數(shù)據(jù)（odometry）發(fā)送給位姿控制節(jié)點，位姿控制節(jié)點再將速度指令直接下發(fā)給Gazebo。

3? Gazebo、PX4與QGC通信

Gazebo、QGC與PX4之間通過MAVLink協(xié)議進行通信。

3.1? MAVLink協(xié)議

MAVLink協(xié)議支持通過無線通道進行雙向通信，可實現(xiàn)無人機與無人機地面控制站QGC間的無線雙向通信，以此實現(xiàn)QGC對無人機的控制以及無人機數(shù)據(jù)回傳。通過MAVLink協(xié)議發(fā)送的數(shù)據(jù)幀為幀頭為FE、幀尾為16位CRC校驗碼的數(shù)據(jù)幀，每一幀帶有發(fā)送包序從0到255重復計數(shù)用于監(jiān)測傳輸過程中的丟包情況[10]。

3.2? QGC與PX4和Gazebo通信

從圖8中可以看出在啟動PX4飛控節(jié)點后，PX4與Gazebo成功進行通信。

從圖9可以看出，QGC與Gazebo和PX4成功通信，無人機成功起飛。

4? 仿真過程及結(jié)果分析

在四旋翼無人機飛行仿真測試過程中，測得系統(tǒng)參數(shù)有：懸架側(cè)傾剛度參數(shù)（Roll Rate）、豎直位置參數(shù)（Vertical Position）、姿態(tài)控制參數(shù)（Attitude Controller，包括Roll Attitude、Pitch Attitude、Yaw Attitude）。

4.1? 無人機懸停及豎直飛行仿真

QGC與Gazebo中無人機懸停姿態(tài)如圖9所示，從圖中可以看出無人機懸停高度為10 m。

懸停姿態(tài)下，對無人機傳感器添加噪聲，并將無人機的Roll Rate參數(shù)和經(jīng)過MSF框架融合后的無人機Roll Rate參數(shù)進行對比如圖10所示。

從圖10中可以看出，四旋翼無人機在懸停姿態(tài)下，由于添加噪聲模擬真實環(huán)境，無人機存在抖動現(xiàn)象（Response曲線數(shù)據(jù)），和期望數(shù)據(jù)（Setpoint參數(shù)曲線）相比較誤差較大（0±3°）；而由于EKF正確的評估了誤差，所以經(jīng)過MSF框架融合后的無人機狀態(tài)估計值（MSFResponse參數(shù)曲線）更加接近期望數(shù)據(jù)，誤差小（0±0.5°），懸停姿態(tài)穩(wěn)定。

豎直飛行運動狀態(tài)下無人機Vertical Position參數(shù)如圖11所示。

從圖11中可以看出，無人機在豎直飛行運動過程中，無論是向上或向下飛行，無人機均能夠快速響應(yīng)，Setpoint與Response兩條參數(shù)曲線基本重合。

4.2? 無人機進行偏航、俯仰和滾轉(zhuǎn)運動仿真

控制無人機飛行一定距離，如圖12所示。在飛行過程中無人機滾轉(zhuǎn)運動參數(shù)部分曲線如圖13所示，俯仰運動參數(shù)部分曲線如圖14所示，偏航運動參數(shù)部分曲線如圖15所示。

從圖13～15中可以看出，無人機在進行滾轉(zhuǎn)、俯仰和偏航運動過程中，無人機在接收到控制指令后，能夠快速響應(yīng)，達到期望目標值，飛行仿真效果好。

4.3? 無人機進行軌跡規(guī)劃運動仿真

4.3.1? 無人機進行定點巡航運動仿真

在QGC地面站中操控無人機進行定點巡航運動，首先在QGC中確定三個路徑點，然后路徑點之間形成規(guī)劃路徑，最后無人機沿規(guī)劃路徑飛行，仿真過程如圖16所示。

從圖16中可以看出無人機能夠按照給定路徑點生成的路徑飛行，成功實現(xiàn)定點巡航運動。

4.3.2? 無人機在指定位置盤旋運動仿真

首先，在QGC中設(shè)置無人機盤旋中心點；然后，設(shè)置盤旋區(qū)域大小；最后操控無人機在指定位置進行盤旋運動。盤旋運動仿真如圖17所示。

從圖17可以看出，在設(shè)置無人機盤旋中心點后，無人機能夠按照盤旋規(guī)劃路徑進行自主飛行，成功實現(xiàn)自主盤旋運動。

5? 結(jié)? 論

本文對基ROS的四旋翼無人機進行仿真控制研究。首先，在Gazebo中建立了四旋翼無人機三維物理仿真模型，并且通過MAVLink實現(xiàn)了Gazebo、QGC與Px4之間的無線雙向通信。然后，在QGC中控制無人機實現(xiàn)懸停及豎直上下運動、滾轉(zhuǎn)運動、俯仰運動、偏航運動和無人機軌跡規(guī)劃運動，分別對懸架傾斜剛度參數(shù)、豎直位置參數(shù)、橫滾姿態(tài)參數(shù)、俯仰姿態(tài)參數(shù)和偏航姿態(tài)參數(shù)進行期望數(shù)據(jù)和實際仿真數(shù)據(jù)的對比參考，并將參考結(jié)果以曲線圖的形式呈現(xiàn)出來。結(jié)果表明，使用MSF框架對無人機傳感器和IMU數(shù)據(jù)進行融合后得到的狀態(tài)估計數(shù)據(jù)與為融合前的數(shù)據(jù)相對比更加精確，能夠成功將添加的模擬噪聲數(shù)據(jù)修正；飛行過程中的無人機期望數(shù)據(jù)與實際飛行數(shù)據(jù)誤差較小，對于后期搭建四旋翼無人機硬件設(shè)備并在真實環(huán)境中飛行有一定的指導意義。

參考文獻：

[1] 梅武軍.基于ROS的多旋翼飛行器飛行控制系統(tǒng)的開發(fā)與設(shè)計 [J].電子科技，2017，30（10）：23-25.

[2] 白慮召.基于PIXHAWK四旋翼無人機飛控算法的研究 [D].石家莊：河北科技大學，2022.

[3] 馬良，穆朝絮，楊萬扣，等.四旋翼無人機目標跟蹤系統(tǒng)設(shè)計 [J].控制工程，2015，22（6）：1076-1081.

[4] 林曉華，王茂森，戴勁松.基于ROS的無人機占據(jù)地圖構(gòu)建方案設(shè)計 [J].兵器裝備工程學報，2021，42（5）：215-220.

[5] 單一.基于視覺導航的四旋翼無人機自主著降控制研究 [D].南京：南京航空航天大學，2018.

[6] 陳胤龍.基于無人機平臺的室內(nèi)自主導航系統(tǒng)設(shè)計與實現(xiàn) [D].南昌：南昌大學，2021.

[7] 降晶晶.面向無人機集群的仿真平臺設(shè)計和實現(xiàn) [D].杭州：杭州電子科技大學，2021.

[8] 羅連杰，佃松宜，蒲明.四旋翼飛行器姿態(tài)的非奇異快速終端滑?？刂?[J].電光與控制，2018，25（4）：12-15+60.

[9] 侯恭，董明飛，康立鵬，等.基于NI myRIO的四旋翼飛行器設(shè)計與實現(xiàn) [J].科技創(chuàng)新與應(yīng)用，2018（23）：101-102.

[10] 丁中濤. 基于ROS的無人機自主降落技術(shù)研究 [D].成都：成都理工大學，2020.

[11] 謝吉海. 小型固定翼無人機制導系統(tǒng)控制器設(shè)計 [D].重慶：重慶大學，2016.

作者簡介：韓鈺寒（2002—），漢族，黑龍江綏化人，本科在讀，研究方向：機器人與智能控制；通訊作者：李宏達（1996—），漢族，黑龍江賓縣人，助教，碩士研究生，研究方向：機器人與智能控制。

收稿日期：2023-04-03

基金項目：2022年黑龍江省大學生創(chuàng)新創(chuàng)業(yè)訓練計劃項目（S202213301037）