基于邏輯分離的無人機仿真系統(tǒng)設(shè)計

劉祖均，何 明，劉錦濤，張 喬

(陸軍工程大學，江蘇 南京210007)

1 引言

隨著無人機技術(shù)的迅速發(fā)展，在許多領(lǐng)域中變得非常流行，包括交通監(jiān)控[1]、農(nóng)業(yè)[2]或災難危機管理[3]，因為這些機器人的使用使得人們避免冒著生命危險執(zhí)行任務。

在無人機的開發(fā)過程中，當一些硬件平臺功能難以實現(xiàn)時，可以借助仿真軟件和編寫控制程序來輔助研究人員進行實驗[7]。無人機一般需要在各種復雜環(huán)境下導航并成功執(zhí)行特定任務。然而，構(gòu)建不同的測試場景并在多種條件下檢查無人機狀態(tài)是昂貴且耗時的。因此，使用完善的仿真環(huán)境可以對正在開發(fā)的無人機系統(tǒng)進行安全且經(jīng)濟的測試，不僅可以縮短開發(fā)周期，還可以降低研發(fā)成本。

但以往的無人機仿真系統(tǒng)存在軟件依賴過多，職能不明確等問題，模型的前端表示與后端控制沒有獨立出來，導致系統(tǒng)過于繁雜，而且不易于操作人員進行分析調(diào)試[16-17]。針對以上問題，本文設(shè)計了一個基于邏輯分離的無人機仿真系統(tǒng)，目的是使用在ROS下的集成環(huán)境為無人機仿真和指揮控制開發(fā)一個可擴展的、可控制的、業(yè)務邏輯可分離的系統(tǒng)。

本文的主要貢獻是：

1)內(nèi)容可以作為在ROS環(huán)境下Gazebo無人機仿真系統(tǒng)的設(shè)計參考，為進行無人機實物飛控算法研發(fā)提供保障。

2)針對地面控制站軟件的不足，設(shè)計了一款基于PX4開源固件的工具包，該軟件可配合Gazebo、QGroundControl完成多種仿真任務。

3)借鑒經(jīng)典Model - View-Controller(MVC)體系結(jié)構(gòu)設(shè)計了一個邏輯分離的無人機仿真控制系統(tǒng)。該系統(tǒng)有效實現(xiàn)了輸入、業(yè)務處理、輸出三端分離。

2 相關(guān)工作

在過去幾年中，一些學者設(shè)計無人機仿真環(huán)境，用于監(jiān)督飛行控制[10]、協(xié)作[11]或訓練[12]。

目前常用的機器人仿真軟件Webots、OpenAI Gym、Gazebo等。為了能夠精準而高效的模擬工作在復雜室內(nèi)和室外環(huán)境的機器人行為，Meyer J等人使用ROS與Gazebo環(huán)境進行了四旋翼無人機綜合仿真[9]。

在使用無人機的模擬和控制時，必須區(qū)分兩種類型的軟件：自動駕駛儀軟件和地面控制站軟件(GCS)。GCS可以控制UAV的軌跡并且提供車輛的遙測。為了解決無人機在任務執(zhí)行中自動規(guī)劃路線的難題，David等人在QGroundControl軟件中使用了幾種擴展工具包并完成自動任務規(guī)劃[8]。

另一方面，GCS軟件側(cè)重于操作員使用，可以提供一個或多個無人機的飛行控制和手動路徑規(guī)劃[4]。為了將這些GCS指令傳送到自動駕駛儀，需要有通信協(xié)議，能夠提供與自駕儀的通信，最常用的協(xié)議是MAVLink。

QGroundControl地面控制軟件通過MAVLink協(xié)議與自駕儀進行數(shù)據(jù)通信，為一個或多個無人機提供地面站支持和飛行姿態(tài)控制和參數(shù)配置。QGroundControl的主要優(yōu)點在于它提供了友好簡單的使用方式，同時還可以擴展一些高端功能。缺點是需要配置其依賴環(huán)境，軟件耦合度高，而且對于仿真環(huán)境中飛機的位置控制方式不友好。

鑒于QGroundControl的缺點，設(shè)計了一款基于PX4開源固件的工具包——PX4Command。與現(xiàn)有控制軟件不同的是，工具包使用時會打開多個腳本文件，用戶只需在腳本中輸入數(shù)字命令來選擇所需功能。消息傳遞、命令控制均由工具包中底層編寫的cpp文件實現(xiàn)。在邏輯上實現(xiàn)了用戶輸入、數(shù)據(jù)處理、仿真演示的分離，使得操作員只需輸入數(shù)字命令即可完成相應的仿真操作。為科研人員帶來簡潔快速的仿真測試，減少項目前期驗證時間。

3 基于ROS的仿真系統(tǒng)設(shè)計

3.1 設(shè)計思路

3.1.1 經(jīng)典MVC體系結(jié)構(gòu)

MVC全稱Model(模型)-View(視圖)-Controller(控制器)，它是于1970年首次提出，設(shè)計該模式是為了解決用戶界面與業(yè)務邏輯的代碼分離，便于程序員進行維護和代碼重用，尤其是在一個完整的前端與后端交互的系統(tǒng)中。下面根據(jù)圖1簡單介紹下MVC體系結(jié)構(gòu)：

1)M：即Model模型，用于處理業(yè)務規(guī)則。該層負責的處理任務最多，既要與前端界面進行數(shù)據(jù)交互，又要作為后端控制指令的“傳達者”。

2)V：即View視圖是指用戶界面。主要負責與用戶進行交互，并把用戶請求發(fā)送給模型。

3)C：即Controller控制器，并不直接與視圖交互，而是使用模型中的相關(guān)功能響應用戶的需求。

圖1 經(jīng)典MVC體系結(jié)構(gòu)

3.1.2 基于ROS的分離式體系結(jié)構(gòu)

本文借鑒MVC的邏輯分離思想，提出了一種基于ROS環(huán)境下的分離式體系結(jié)構(gòu)(即自駕儀-仿真器-命令、控制體系結(jié)構(gòu)，簡稱為ASC2體系結(jié)構(gòu)，如圖2所示)，用于設(shè)計表示層、業(yè)務層、控制層三層分離的無人機仿真系統(tǒng)。該體系結(jié)構(gòu)不僅具有MVC的特點，又由于在每一層使用軟件具有很好的開源性，使得體系具有很好的開放性和擴展性。

圖2 基于ROS的ASC2體系結(jié)構(gòu)

1)A：即自駕儀Autopilot，代表業(yè)務層。主要負責與仿真器進行數(shù)據(jù)交互并完成飛控姿態(tài)狀態(tài)量的計算，接受控制層的指令并通知表示層更新視圖。

2)S：即仿真器Simulator，代表表示層。在設(shè)計的系統(tǒng)中主要負責可視化界面，模擬計算傳感器參數(shù)并發(fā)送給業(yè)務層處理。

3)C2：即命令、控制(Command、Control)，代表控制層。提供友好的操作界面由用戶操作，并與自駕儀建立通信鏈接發(fā)送指令實現(xiàn)控制，操作員只需輸入命令，無需關(guān)注數(shù)據(jù)如何發(fā)送及處理，通過仿真器的界面反饋指令的執(zhí)行。

3.2 設(shè)計要素

基于3.1節(jié)中提出的ASC2體系結(jié)構(gòu)設(shè)計三層邏輯分離系統(tǒng)，分別在每一層對應了軟件。該系統(tǒng)設(shè)計要素包括一下五點：

1)ROS：是用于有效開發(fā)和構(gòu)建機器人系統(tǒng)的庫，驅(qū)動程序和工具的集合[5]。采用了一種分布式進程結(jié)構(gòu)，使得執(zhí)行程序可以分別設(shè)計并進行有機結(jié)合，按照使用的各種包的形式分別管理[6]。也是本文系統(tǒng)的基礎(chǔ)環(huán)境。

2)PX4：一款功能強大的開源自動駕駛儀，是無人機平臺的核心部分。在本系統(tǒng)中負責業(yè)務層的數(shù)據(jù)處理任務，同時與表示層、控制層軟件進行交互。

3)Gazebo：是一個免費的開源機器人模擬環(huán)境，可以在三維室內(nèi)和室外環(huán)境中模擬機器人和傳感器應用[17]。在本系統(tǒng)中主要負責表示層的界面可視化和模擬無人機傳感器參數(shù)。

4)MAVLink：一種用于微型飛行器的開源通信協(xié)議。在本系統(tǒng)中主要負責控制層與業(yè)務層的數(shù)據(jù)通信。

5)PX4Command：一款基于PX4和Mavros的工具包，為操作員提供簡潔快速的開發(fā)。在本系統(tǒng)中處于控制層，是ASC2體系結(jié)構(gòu)的核心部分。

3.2.1 PX4Command工具包

PX4Command工具包是一個基于PX4和Mavros的控制程序。能夠配合板載計算機(樹莓派、TX2、Nano)等運算能力比較強的處理器，來實現(xiàn)復雜算法的運行，控制指令通過串口或者網(wǎng)口通信發(fā)送給底層控制板。

如圖3所示，該工具包通過命令界面發(fā)送指令，由Mavros程序把負責消息傳遞給自駕儀，然后經(jīng)過數(shù)據(jù)處理后反饋到前端狀態(tài)界面。通過這樣的雙向交互信息，操作員并不需要知道底層數(shù)據(jù)的處理，只需輸入控制命令并觀察界面反饋。

圖3 工具包交互式控制流程

圖4 PX4Command工具包框架

工具包主要包括外部位置估計、位置控制規(guī)則、消息傳遞、應用層文件四大模塊。

1)應用層：包含實現(xiàn)無人機定點、自主著陸、碰撞避免、目標追蹤等上層應用的cpp文件。

2)外部位置估計：該模塊功能主要由px4_pos_estimator.cpp文件負責實現(xiàn)，訂閱激光雷達或者mocap發(fā)布的位置信息，并進行坐標轉(zhuǎn)換， 根據(jù)訂閱的數(shù)據(jù)，發(fā)布相應的位置，偏航角給飛控。

3)位置控制規(guī)則：該模塊主要由px4_pos_controller.cpp文件負責實現(xiàn)，提供有關(guān)位置控制基礎(chǔ)參數(shù)、控制方式的頭文件。訂閱由位置估計發(fā)布的飛機狀態(tài)，初始化當前飛機狀態(tài)的時間。訂閱Control_Command發(fā)布的消息參數(shù)，對move節(jié)點中數(shù)據(jù)等進行邏輯處理。

4)消息傳遞：主要功能是負責PX4Command工具包與Mavros功能包的信息交互，發(fā)送/接收期望位置，速度(本地系與機體系)，角度，角速度，底層控制(遙控器輸入)。

由于該工具包的使用，使得用戶只需關(guān)注應用層的實現(xiàn)，消息傳遞、位置估計、位置控制均由相應模塊負責與PX4進行交互處理，有效實現(xiàn)了表示層、業(yè)務層、控制層的邏輯分離。

3.3 集成仿真系統(tǒng)設(shè)計

應用3.1節(jié)中的邏輯分離思想，集成仿真系統(tǒng)的通信流程遵循下圖中的體系架構(gòu)。PX4與模擬器(例如Gazebo)通信，以接受來自模擬世界的傳感器數(shù)據(jù)并發(fā)送電機和執(zhí)行器值；與GCS和Offboard API(例如ROS)通信，以從模擬環(huán)境發(fā)送遙測并接受命令。

圖5 仿真系統(tǒng)體系架構(gòu)

圖6 系統(tǒng)整體通信原理圖

結(jié)合以上兩圖來闡述整體系統(tǒng)的數(shù)據(jù)流通。

1)PX4—仿真器：所有仿真器與PX4的通訊都是通過MAVLink消息來進行的，軟件在環(huán)仿真(SITL)使用simulator模塊中的simulator_mavlink.cpp來處理這些消息。例如，在PX4終端中輸入命令commander takeoff，命令以MAVLink消息的形式發(fā)送到Gazebo仿真器并執(zhí)行起飛操作。

2)仿真器—PX4：仿真器的作用就是模擬真實飛行，即模擬計算出真實飛行時的傳感器狀態(tài)，包括GPS，IMU等，將這些信息發(fā)送給飛控后，再由飛控中的相關(guān)模塊計算出飛機狀態(tài)量。

3)控制器—PX4：控制程序PX4Command，它支持運行ROS的計算機、支持MAVLink的自動駕駛儀和支持MAVLink的GCS的計算機之間的可擴展通信[8]。PX4Command通過UDP協(xié)議與PX4自駕儀進行通信鏈接，用戶通過該地面站發(fā)送傳輸指令(位置期望點、速度期望值等)并且監(jiān)控飛機狀態(tài)，還可以使用操縱桿游戲手柄來控制虛擬世界的飛機。

由以上分析，ASC2體系結(jié)構(gòu)應用到無人機仿真系統(tǒng)設(shè)計中，能夠完成簡潔快速的仿真開發(fā)，操作員無需關(guān)注底層數(shù)據(jù)處理過程，通過簡單的控制文件可以有效實現(xiàn)表示層、業(yè)務層、控制層的邏輯分離，使系統(tǒng)具有更好的可控性。

4 仿真驗證與分析

本節(jié)為了充分驗證本文仿真系統(tǒng)的易用性、可控性、開放性分別進行了三組實驗。從單機仿真到多機仿真再到拓展使用外部軟件進行飛行任務。實驗一、實驗二均使用自駕儀狀態(tài)信息回顯進行飛行數(shù)據(jù)分析驗證，實驗三中則拓展使用較為流行的飛行日志分析軟件Flightplot進行數(shù)據(jù)可視化分析驗證。

4.1 實驗一單機仿真

在配置好ROS+PX4+Gazebo+PX4Command集成仿真環(huán)境后，配合Mavros 軟件包來完成外部程序控制虛擬世界仿真。

運行工具包中單機仿真腳本，如圖所示，啟動了六個腳本，其中①②③腳本為啟動ROS服務、Gazebo、自駕儀以及完成ROS與自駕儀的通信端口連接；④腳本由px4_pos_controller.cpp實現(xiàn)控制方式與子模式選擇；⑤腳本由move.cpp實現(xiàn)無人機切換offboard模式并解鎖；⑥腳本由px4_pos_estimator.cpp實現(xiàn)無人機相關(guān)信息的計算處理并回顯到屏幕。

圖7 單機仿真

如上圖中④腳本所示，控制方式為機體坐標系，控制子模式為xyz坐標點位置控制，輸入坐標(4，3，2)。待無人機穩(wěn)定后查看腳本⑥的回顯信息，無人機狀態(tài)信息顯示為已連接、已解鎖、offboard模式；位置信息顯示坐標(4.19，2.94，1.85)，誤差屬于飛行正常波動。

4.2 實驗二多機仿真

當前啟動文件初始三架UAV坐標分別為(2，0，0)、(-2，2，0)、(-2，-2，0)。

設(shè)定任務：選擇地球固連坐標系(ENU)，控制子模式為xyz坐標點位置控制，①輸入坐標+(0，0，1)，即原地起飛。②輸入坐標+(1，0，0)，即三機沿X軸編隊飛行一格。

圖8 多機仿真

根據(jù)rostopic話題中三架無人機返回的位置信息和Gazebo中實際飛行軌跡作出圖8。編隊飛行實現(xiàn)為當前(x，y，z)與輸入坐標(1，0，0)對應項相加，即三架無人機整體沿X軸移動一格。

4.3 實驗三配合QGC進行路徑規(guī)劃

本文仿真系統(tǒng)具有很高的開放性，可以配合多種軟件使用以獲得更多的功能，實驗三將配合常用地面控制站QGC完成路徑規(guī)劃仿真測試。

首先設(shè)定預想任務要求：

1)選擇任務模式并進行航線規(guī)劃，任務結(jié)束啟用返航模式并且要求返航模式需爬升至20 m空中并降落在起飛點。

2)默認任務飛行高度10 m，飛行速度5 m/s。

3)任務模式飛行中可執(zhí)行其它操作，如中斷任務，立即返航等。

其次在QGroundControl中按照任務要求設(shè)定飛行參數(shù)。設(shè)定完成后，執(zhí)行任務以及相關(guān)要求的操作。

圖9 任務中途執(zhí)行返航操作

由圖9中航線明顯觀察到任務在執(zhí)行中中斷并執(zhí)行返航操作，儀表盤數(shù)據(jù)顯示飛行高度20 m，速度為4.8 m/s。仿真結(jié)果能夠完成預設(shè)任務的三點要求。

最后，使用日志分析軟件Flightplot對實驗數(shù)據(jù)進行分析。從仿真系統(tǒng)中獲取了本次飛行的日志文件，其中包括許多數(shù)據(jù)包和參數(shù)信息，通過數(shù)據(jù)來驗證仿真。通常比較關(guān)注的數(shù)據(jù)包有：

1)vechile_gps_position：GPS數(shù)據(jù)包，包括了定位的經(jīng)緯度、固定狀態(tài)、衛(wèi)星數(shù)。

2)vechile_local_position：ekf數(shù)據(jù)融合之后的當?shù)刈鴺讼底鴺?NED)、航向、水平和垂直精度，及其它信息。

圖10 仿真數(shù)據(jù)分析

通過上圖，可以清晰的觀察出，綠色曲線為衛(wèi)星顆數(shù)，穩(wěn)定維持在10左右，說明衛(wèi)星信號正常無干擾；紅色曲線代表飛機高度變化曲線，可以看到飛機在執(zhí)行飛行高度為10 m的任務，最后返航模式爬升至20 m再降落起飛點；藍色曲線為實時動態(tài)定位(RTK)曲線，GPS定位狀態(tài)穩(wěn)定。

該系統(tǒng)由Gazebo實現(xiàn)可視化仿真、PX4完成業(yè)務數(shù)據(jù)處理、PX4Command工具包實現(xiàn)外部控制。實驗一、實驗二充分說明系統(tǒng)能夠完成單機、多機的簡單仿真測試，并且可以實現(xiàn)表示層、業(yè)務層、控制層的有效分離，體現(xiàn)出整體系統(tǒng)的易用性、可控性。實驗三說明系統(tǒng)具有良好的開放性，更加方便科研人員進行開發(fā)調(diào)試。

5 總結(jié)

本文基于邏輯分離思想設(shè)計了一個無人機仿真控制系統(tǒng)，編寫的PX4Commnd工具包可以完成對無人機的仿真測試，并且可以實現(xiàn)系統(tǒng)的表示層、業(yè)務層、控制層三層分離，對于科研人員能夠有效的縮短研發(fā)周期并便于分析調(diào)試。由于各層開源軟件的使用，可以快速有效的對機器人的各種傳感器參數(shù)實現(xiàn)控制，也使得本文的集成仿真環(huán)境具有較高開放性。文章最后仿真驗證了集成仿真系統(tǒng)的易用性、可控性。

由于多機通信問題較為復雜，實現(xiàn)困難，下一步研究工作將會探索如何優(yōu)化多機仿真測試，并且使用該集成仿真系統(tǒng)進行無人機集群飛控算法的開發(fā)與實體機測試。