面向教學實踐的多無人機任務仿真系統

齊 鐸， 王 鵬， 任曉岳， 李 哲， 梁丹亞， 楊麗薇

（空軍工程大學空管領航學院，西安 710051）

0 引 言

小型無人機具有機動靈活、攜帶方便、易于維護和操作、起降要求簡單等特點。平時可用于物流運輸［1］、航拍攝影［2］和線路巡檢［3-4］等民用生活領域；戰時，用來執行對地偵察打擊和動態目標跟蹤等軍事任務，應用前景十分廣泛。受限于能量約束，單無人機在執行復雜任務時存在諸多不足。如將不同功能的多無人機按照相關邏輯進行協同，使其具備一定的自主性和智能性，發揮集群優勢，完成相關復雜任務，可大大提升任務完成效果，這也是當前自動化領域、人工智能領域的熱點研究問題之一［5-6］。

仿真和實飛是多無人機復雜任務測試中最常用的兩種手段。與實飛相比，仿真測試的應用更加廣泛。多無人機任務仍處于快速發展階段，可利用仿真方法對任務中可能出現的風險點進行預測研究；多無人機由單個無人機節點組成，通過仿真模型替代部分子節點可節省試驗成本，加快試驗進度［7-8］。通過調研可知，當前無人機任務仿真手段和平臺存在一定不足：使用Matlab等進行程序編寫主要用于測試算法本身，對工程實現關注不夠［9-10］；FlightGear等開源仿真軟件側重于飛行操作訓練，自主性和智能性較差［11-12］；Gazebo三維仿真環境對開源飛控支持度較好，但需要在特定模式下進行二次開發［13］。本文基于機器人操作系統（Robot Operating System，ROS）和開源飛控PX4 搭建了一套軟件在環的多無人機任務仿真系統，綜合Matlab和Gazebo的優勢，完成復雜任務的仿真。仿真結果顯示，該系統的仿真與實飛結果具有較高的吻合性，既適用于相關專業的教學過程，也可為多種復雜任務實現提供支撐。

1 系統總體框架

考慮多無人機復雜任務仿真需求，本文設計的仿真系統總體框架如下圖1 所示。

圖1 仿真系統總體框架

系統主要包含協同規劃、地面站、機載邏輯任務、執行、場景顯示及飛行參數設置/數據顯示等模塊。系統采用分層遞階控制和模塊化設計思想，將系統總體架構分為決策層、控制層、執行層和顯示層，向上能滿足堅持面向任務的基本原則，向下能兼容不同動力學特性的異構平臺，具有較好的通用性和擴展性。決策層為協同規劃模塊，可根據具體任務需求，規劃、生成任務參數；控制層為地面站和機載邏輯任務模塊，主要完成多無人機系統狀態監控、控制指令發送以及任務參數的讀取、加載和交互；執行層是開源飛控PX4 固件和虛擬外接傳感器，用于對控制層發送的多機期望狀態進行跟蹤以及執行外接傳感器的相關動作；顯示層為Gazebo 三維物理仿真平臺和虛擬地面站（QGroundControl，QGC），除完成不同機型虛擬無人機類型和數量加載以及實時狀態顯示兩大功能，還可利用QGC對無人機的速度、姿態、電量等進行設置。

2 系統設計

2.1 硬件組成

協同規劃和地面站模塊均運行在地面站計算機，地面站計算機是一臺MateBook X Pro 筆記本，安裝了Windows10 系統，搭載了第8 代的i7 處理器，16GB 內存，1TB硬盤。機載邏輯任務、執行、場景顯示和飛行參數設置/數據顯示等模塊運行在一臺RADEN 小尺寸電腦主機，主機安裝了Ubuntu18.04 操作系統，搭載第8 代i7 處理器，32GB 內存。地面站計算機和Ubuntu主機通過串口線進行數據通信，系統硬件組成如圖2 所示。

圖2 系統硬件組成

2.2 軟件設計

（1）協同規劃模塊。使用Matlab 2017b編寫協同規劃模塊中涉及的復雜算法，Matlab 具有強大的計算能力、豐富的仿真環境、直觀的數據顯示和簡單易操特性，被廣泛應用于各種仿真計算中。

（2）地面站模塊。地面站具備任務規劃/決策、任務啟動/終止、指令生成/發送、狀態監控/顯示和應急處置/控制等功能。作為任務指令的發送中樞，地面具備任務計算功能，包括：任務參數設置、任務資源分配、任務指令生成和任務流程調度。地面站又是多機任務的數據匯集中心，通過數傳鏈路，接收所有無人機廣播的心跳包數據，解碼后得到飛參數據用于實時顯示無人機飛行狀態和任務狀態。所有飛行數據、任務指令等數據都在后臺備份，生成飛行日志，供后期查閱并復盤整個任務過程。地面站軟件界面使用Qt編寫，如圖3 所示。

圖3 地面站界面

（3）機載邏輯任務模塊程序。機載邏輯任務程序是多無人機任務決策和協同控制的內核，部署在Ubuntu主機。通過在主機配置ROS、MAVROS（ROS中的MAVLINK 功能包）、OpenCV（圖像處理庫）、Python、C/C++等開發環境，實現任務計算、圖像識別以及控制指令解算等多種功能。

為滿足任務需求，設計邏輯任務程序使無人機具備數據通信、任務解算、飛行控制和圖像識別等功能。數據通信是建立無人機和任務地面站之間的數據傳輸，實現任務參數和飛行狀態的交互。任務解算是針對不同任務階段選擇不同的協同控制方法，并生成控制指令。飛行控制是向飛行控制器發送執行指令。圖像識別是指實現目標識別及目標位置信息解算。基于上述功能，機載計算機程序代碼采用C ++語言編寫，利用ROS庫和pthread 庫實現節點和多線程運行，利用serial庫實現串口通信，利用MARVOS庫（飛行控制庫）和iocu_proxy庫（GPIO輸入輸出函數庫）實現飛行和外部設備控制，機載程序架構設計如圖4 所示。

圖4 機載程序框架結構

機載程序開發充分利用ROS 操作系統高度靈活的軟件架構，將整個程序設計為任務控制節點和圖像處理節點，節點內部采用多線程處理多路數據，通過節點之間的通信機制和線程之間的數據交互實現所需功能。對于任務控制節點4 個線程的功能分別如下：

線程1訂閱飛參和圖像數據。任務控制節點作為一個獨立的節點利用ROS 的話題通信和服務通信機制分別從飛行控制器節點和圖像處理節點按照指定頻率獲取相關參數。其中，從飛行控制器訂閱飛機的絕對坐標、相對坐標、姿態、速度、加速度、飛行模式、起始點、海拔高、電量等信息；從圖像處理節點訂閱目標識別標識、目標相對位置信息。把訂閱到的最新數據存儲在數據庫中，供其他線程調用。

線程2實時發送心跳包數據。心跳包如同無人機“心臟”的跳動，只要無人機機載程序處于運行狀態就會按一定頻率持續發送心跳包數據。從飛參數據庫中調用最新的飛行參數，按照一定協議打包成心跳包，心跳包中包含無人機的ID 號、無人機的類型、正在執行的任務及飛行參數等。心跳包的發送需調用串口發送函數，通過通信模塊廣播給地面站。

線程3實時接收任務指令。任務指令通過地面站發送給各無人機。任務地面站根據任務階段發送不同的任務指令，任務指令包含指令和參數，按一定的協議進行封裝，機載計算機解析完成后調用相應的協同控制算法。

線程4實時解算控制指令。該線程設計不同任務階段之間的調度和切換，根據當前所處的任務階段調用對應的協同控制算法，帶入任務參數，解算飛行控制指令。控制指令解算完成后利用MAVROS 功能包中的MAVSDK 將控制指令按照MAVLINK 協議封裝，發送給PX4 飛控執行。

2009年，十堰市區一位家庭婦女在陽臺晾衣服時突然暈厥。當時，她9歲的兒子就在邊上，曾經在學校接受過急救小分隊的宣教，竟然想起學習過的心肺復蘇技術來急救自己的媽媽。幾分鐘以后，媽媽竟然蘇醒過來，小男孩兒隨后立刻撥打120，成功“救母”。羅杰認為：這無非是對小分隊的培訓最好的檢驗。

圖像處理節點具備圖像識別功能，采用單線程部署。對單目相機捕獲的圖像進行目標識別，生成目標識別標識，一旦識別到目標，則調用線程中的定位算法，獲取目標在機體坐標系中的相對坐標。目標識別標識和目標相對位置坐標按照ROS 的消息格式封裝完成后輸入指定的話題，供任務控制節點訂閱使用。

在ROS系統下，軟件架構的設計采用模塊化設計思想，通過開發工作節點（Node）的方式實現功能靈活擴展，后續更換、添加傳感器只需修改或增加節點。同時，機載軟件的開發充分借助開源飛控強大的2 次開發能力，飛行參數可調，限制較少，降低了對商用飛控的依賴，提升了任務的自主性。

（4）執行模塊。執行模塊為開源飛控PX4，版本號為1.11.2，其源碼通過Github 開源項目獲取，下載至本地后通過編譯可在仿真環境中使用。

（5）場景顯示模塊。場景顯示模塊為Gazebo 三維物理仿真軟件，PX4 的源代碼提供了無人機的Gazebo模型，通過launch文件直接運行就能得到一個Gazebo環境下的無人機模型。通過修改配置文件，還可設置無人機的類型和數量。

（6）飛行參數設置/數據顯示模塊。飛行參數設置/數據顯示模塊為虛擬QGC地面站，通過QGC 可實時監控無人機運行狀態，并顯示無人機航跡，方便與規劃結果進行對比。

3 雙機路徑規劃任務測試

以2 架多旋翼無人機完成路徑規劃并實施仿真為例，說明仿真系統工作的全流程。

3.1 任務場景

設計場景：某城市發生地質災害，大片建筑物倒塌、通信中斷，需派遣2 架無人機對可能存在傷員的區域進行搜索和識別。目前已經獲悉目標區域的大體位置，將任務環境劃分為10 ×10 大小的柵格地圖，2 架無人機的具體偵察區域由上級指定，航線存在交叉，規劃系統需進行進一步判斷，具體如圖5 所示。

圖5 任務場景示意圖

3.2 基于蟻群算法的雙機路徑規劃

蟻群算法受螞蟻種群覓食行為的啟發，在此過程，種群中每只螞蟻都會在其移動路徑中留下信息素以向種群傳達信息。移動路徑越短，則信息素濃度越高，該路徑被螞蟻選中的概率較大，這就形成了螞蟻尋找最短路徑的正反饋機制。基本蟻群算法原理如下［14-15］。

螞蟻移動時，依照信息素濃度與距離啟發函數選擇路徑，螞蟻k從柵格i運動至柵格j的概率

式中：α為信息素啟發因子；β 為距離期望函數因子；Ak為螞蟻下一步可到達目的地集合；τij（t）為t時刻移動路線上的信息素濃度；ηij（t）為距離啟發函數

柵格i和柵格j之間的距離

由柵格坐標（xi，yi）和（xj，yj）計算得到。每只螞蟻在移動時均會遺留一定量的信息素，算法不斷迭代時，路徑中信息素含量逐漸累積，也在不斷揮發，當種群中所有螞蟻均完成每一輪的迭代后，將依照以下規則刷新路徑上信息素含量

式中：ρ為信息素揮發系數；Δτij為兩節點上釋放信息素的和；Δτkij為兩節點上信息素增量；Lk為螞蟻k 經過路徑長度；Q 為信息素增強系數。其基本流程如圖6所示。

圖6 蟻群算法流程圖

通過對曲線進行平滑，得到無人機飛行軌跡如圖7 所示。算法對兩架無人機的交匯點進行了時間判別，兩機在設定速度下，抵達交匯點的時間不一致，規劃航線有效可行。

圖7 雙機路徑規劃結果

3.3 仿真分析

地面站啟動任務后，無人機按照預先設計路徑飛行，不同仿真階段無人機狀態如圖8 所示。

圖8 不同仿真階段無人機狀態

為便于對比驗證，將規劃航跡效果按一定順序進行翻轉，與虛擬地面站QGC記錄的無人機運行軌跡進行對比，如圖9 所示，結果發現，仿真飛行數據與規劃數據具有較好的吻合度。

圖9 仿真系統記錄無人機飛行軌跡

3.4 實飛驗證

為測試雙機路徑規劃仿真結果，用2 架多旋翼無人機對任務進行實飛驗證。將機載邏輯任務中的狀態切換程序注銷，修改為遙控器模式。仿真無人機系統自帶IRIS，實飛使用自研多旋翼教學無人機，任務前對無人機進行傳感器校準。實飛無人機和飛行場景如圖10 所示。

圖10 實飛無人機和飛行場景

圖11 分別為雙機飛行位置數據，其中，折線為規劃航跡，直線為返航航跡。由于普通GPS 具有誤差，加之實飛設置降低了數據接收頻率，導致無人機實際飛行路徑與規劃計算結果存在小幅度誤差，并不能完全吻合。

圖11 實飛路徑曲線

圖12 分別為雙機實飛過程的速度變化曲線，實飛采用的是位置控制模式，無人機每抵達期望航點之前，都有一個較為明顯的減速，以判斷自身是否抵達目標點。從速度曲線上看，表現為歸零過程。無論是仿真和實飛，單一的位置控制使飛行過程不連貫，這一缺陷在單純的Matlab 仿真中無法體現，后續還需進一步改進。

4 結 語

本文設計的多無人機任務仿真系統將規劃算法、地面站、控制、識別等模塊進行解耦，后續可在此框架基礎上完成不同模塊研究的迭代和更新，成為學習無人機控制、人工智能、多機協同、避撞算法的實踐平臺。目前系統功能還存在以下不足，一是虛擬無人機配置的載荷較少，缺乏整體態勢感知能力，局部規劃能力欠缺；二是當前使用的Gazebo 環境較為單一，還需進一步豐富。