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

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

1 引言

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

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

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

本文的主要貢獻(xiàn)是：

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

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

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

2 相關(guān)工作

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

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

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

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

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

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

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

3.1 設(shè)計(jì)思路

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

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

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

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

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

圖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è)計(jì)表示層、業(yè)務(wù)層、控制層三層分離的無人機(jī)仿真系統(tǒng)。該體系結(jié)構(gòu)不僅具有MVC的特點(diǎn)，又由于在每一層使用軟件具有很好的開源性，使得體系具有很好的開放性和擴(kuò)展性。

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

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

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

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

3.2 設(shè)計(jì)要素

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

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

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

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

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

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

3.2.1 PX4Command工具包

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

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

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

圖4 PX4Command工具包框架

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

1)應(yīng)用層：包含實(shí)現(xiàn)無人機(jī)定點(diǎn)、自主著陸、碰撞避免、目標(biāo)追蹤等上層應(yīng)用的cpp文件。

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

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

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

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

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

應(yīng)用3.1節(jié)中的邏輯分離思想，集成仿真系統(tǒng)的通信流程遵循下圖中的體系架構(gòu)。PX4與模擬器(例如Gazebo)通信，以接受來自模擬世界的傳感器數(shù)據(jù)并發(fā)送電機(jī)和執(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消息來進(jìn)行的，軟件在環(huán)仿真(SITL)使用simulator模塊中的simulator_mavlink.cpp來處理這些消息。例如，在PX4終端中輸入命令commander takeoff，命令以MAVLink消息的形式發(fā)送到Gazebo仿真器并執(zhí)行起飛操作。

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

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

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

4 仿真驗(yàn)證與分析

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

4.1 實(shí)驗(yàn)一單機(jī)仿真

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

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

圖7 單機(jī)仿真

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

4.2 實(shí)驗(yàn)二多機(jī)仿真

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

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

圖8 多機(jī)仿真

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

4.3 實(shí)驗(yàn)三配合QGC進(jìn)行路徑規(guī)劃

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

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

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

2)默認(rèn)任務(wù)飛行高度10 m，飛行速度5 m/s。

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

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

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

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

最后，使用日志分析軟件Flightplot對實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析。從仿真系統(tǒng)中獲取了本次飛行的日志文件，其中包括許多數(shù)據(jù)包和參數(shù)信息，通過數(shù)據(jù)來驗(yàn)證仿真。通常比較關(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ù)融合之后的當(dāng)?shù)刈鴺?biāo)系坐標(biāo)(NED)、航向、水平和垂直精度，及其它信息。

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

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

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

5 總結(jié)

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

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