開源APM模擬器在測(cè)繪無(wú)人機(jī)中的應(yīng)用

劉亞非

(中國(guó)鐵路設(shè)計(jì)集團(tuán)有限公司，天津 300308)

近年來(lái)，無(wú)人機(jī)在鐵路勘察設(shè)計(jì)領(lǐng)域得到越來(lái)越多的應(yīng)用。無(wú)人機(jī)具有快速高效、機(jī)動(dòng)靈活、應(yīng)用成本低等優(yōu)勢(shì)[1-2]，但針對(duì)鐵路勘察行業(yè)特點(diǎn)設(shè)計(jì)的專用無(wú)人機(jī)并不多見。飛行模擬器能夠模擬無(wú)人機(jī)實(shí)測(cè)飛行，因而能夠節(jié)約開發(fā)成本，大幅提高研發(fā)效率。目前，市場(chǎng)上的無(wú)人機(jī)模擬器主要用于飛手培訓(xùn)，如微軟公司開發(fā)的AriSim仿真器，可支持多平臺(tái)運(yùn)行，可通過多個(gè)API接口實(shí)現(xiàn)數(shù)據(jù)讀取、無(wú)人機(jī)控制、環(huán)境條件控制，大疆飛行模擬器是一款面向無(wú)人機(jī)仿真培訓(xùn)軟件，基于Windows系統(tǒng)和大疆先進(jìn)的飛控技術(shù)，能與多款大疆無(wú)人機(jī)及遙控器無(wú)縫銜接，能夠提供從基礎(chǔ)知識(shí)教學(xué)仿真訓(xùn)練以及作業(yè)場(chǎng)景練習(xí)，但該模擬器僅限于大疆型號(hào)無(wú)人機(jī)及相關(guān)設(shè)備[3]。不難看出，無(wú)人機(jī)飛行模擬器通過計(jì)算機(jī)圖形學(xué)技術(shù)實(shí)時(shí)模擬出飛行器地理位置和姿態(tài)相對(duì)應(yīng)的連續(xù)的無(wú)人機(jī)飛行場(chǎng)景，形成一個(gè)逼真的飛行情景[4]。以下對(duì)開源APM模擬器的研發(fā)過程進(jìn)行介紹。

1 無(wú)人機(jī)飛行模擬控制的發(fā)展

1.1 無(wú)人機(jī)飛行姿態(tài)模擬的數(shù)學(xué)邏輯

根據(jù)矢量運(yùn)算的數(shù)量積定義，機(jī)體坐標(biāo)系b單位矢量在大地坐標(biāo)系e上的投影為

(1)

同理，大地坐標(biāo)系e的三軸單位矢量在機(jī)體坐標(biāo)系b上的投影為

(2)

無(wú)人機(jī)飛行姿態(tài)決定于3個(gè)歐拉角：橫滾角φ(右滾為正)；俯仰角θ(抬頭為正)；航向角φ(機(jī)頭在地面投影與正北方向夾角，俯視順時(shí)針為正)。不難看出，三維空間的無(wú)人機(jī)無(wú)論處于何種飛行姿態(tài)，都可以由大地坐標(biāo)e的初始狀態(tài)通過3次旋轉(zhuǎn)實(shí)現(xiàn)隨機(jī)飛行姿態(tài)，有

(3)

方向余弦矩陣姿態(tài)估算方法在APM開源文件libraries/ap_math/Matrix3.cpp中，通過from_euler()函數(shù)來(lái)實(shí)現(xiàn)[6]。

1.2 無(wú)人機(jī)飛行姿態(tài)控制的實(shí)現(xiàn)

芯片技術(shù)的發(fā)展使無(wú)人機(jī)控制技術(shù)產(chǎn)生了質(zhì)的飛越，徹底改變了無(wú)人機(jī)“航?！钡母拍睢Ｒ?2位STM32F427(FMUV3框架)系列單片機(jī)為核心，加載MPU6000系列電子三軸加速度計(jì)、Micro L3GD20系列16位電子陀螺儀、Micro LSM303D系列電子加速度計(jì)/磁力計(jì)、MEAS MS5611系列高精度電子氣壓高度計(jì)，構(gòu)成了無(wú)人機(jī)飛控系統(tǒng)[7]。飛控計(jì)算機(jī)獲得無(wú)人機(jī)隨機(jī)飛行的姿態(tài)數(shù)據(jù)后，依據(jù)姿態(tài)估算理論得到無(wú)人機(jī)飛行預(yù)期的飛行姿態(tài)數(shù)據(jù)，再通過PID控制理論實(shí)現(xiàn)對(duì)電子調(diào)速器(ESC)的同步調(diào)節(jié)，從而達(dá)到對(duì)飛行姿態(tài)的精準(zhǔn)控制。PID控制流程見圖1[8]。

圖1 PID控制流程

2 無(wú)人機(jī)模擬器的集成與二次開發(fā)

2.1 模擬器集成環(huán)境配置

該模擬器基于目前開源市場(chǎng)最新的飛控固件Ardupilot4.0.7(截止2021年底)，建立仿真模擬環(huán)境之前，需要完成APM固件編譯環(huán)境的配置[9]。其他插件工具包括：Cygwin64、Mavproxy、Gcc編譯器、Githubdesktop、Git-2.32.0.2-64-bit、Eclipse，使用該工具需要配套相應(yīng)的Java開發(fā)環(huán)境，建議選擇最新的Vscode集成開發(fā)環(huán)境。采用安全緩沖區(qū)數(shù)據(jù)采集思路，具有作業(yè)風(fēng)險(xiǎn)低、建模精度高、數(shù)據(jù)冗余低的優(yōu)勢(shì)[10]。

2.2 模擬器程序的二次開發(fā)

(1)模擬器仿真實(shí)質(zhì)

實(shí)際上，模擬器二次開發(fā)編譯的是實(shí)際的飛控源代碼，只不過仿真模擬時(shí)固件編譯的目標(biāo)是SITL，和實(shí)物固件用的是同一套源代碼，二者的關(guān)系見圖2。當(dāng)模擬飛行的腳本運(yùn)行時(shí)，編譯器會(huì)自動(dòng)生成模擬固件，然后自動(dòng)運(yùn)行，當(dāng)修改或者重新編寫飛控代碼時(shí)，修改的部分就會(huì)被編譯仿真，這就是仿真模擬飛行能夠驗(yàn)證軟件邏輯可行性的原因。

圖2 實(shí)物固件與模擬固件編譯對(duì)比

(2)模擬腳本的配置和運(yùn)行

在“C:/cygwin64home本機(jī)名稱/”目錄下創(chuàng)建一個(gè)新的文件夾，重命名為sim。

在sim文件夾中新建txt文件，輸入如下內(nèi)容：“/cygdrive/d/ardupilot/tools/autotest/sim_vehicle.p-v ArduCopter”，將這個(gè)文件保存為“s”，并刪除“.txt”后綴，這樣這個(gè)文件就變成一個(gè)可執(zhí)行腳本文件。

進(jìn)入Cygwin64 Terminal終端，輸入“cd sim/./”，模擬腳本執(zhí)行情況見圖3。

圖3 執(zhí)行腳本文件

若腳本文件運(yùn)行結(jié)果為“Configure finished successfully”，則表明執(zhí)行腳本成功。

(3)模擬固件編譯

該模擬固件是基于APM開源的4.0系列固件，編譯器共需要編譯處理642個(gè)文件，模擬固件編譯界面見圖4。

圖4 模擬固件編譯界面

若編譯文件顯示“Build finished successfully”，則表示模擬固件編譯成功。

2.3 二次開發(fā)注意事項(xiàng)

由于開源的APM代碼需要其他開源項(xiàng)目支持，如Uavcan、Mavlink、Chibios等若編譯過程中出現(xiàn)報(bào)錯(cuò)，解決辦法有以下幾種。

(1)“ImportError: No module named pymavlink”，這個(gè)報(bào)錯(cuò)的意思是缺少一個(gè)叫“pymavlink”的Python庫(kù)，解決辦法：打開cygw64，輸入代碼“pip install pymavlink”，然后安裝該庫(kù)。采用背向散射(backscatter)通信技術(shù)和無(wú)人機(jī)，設(shè)計(jì)遠(yuǎn)距離無(wú)源鐵路監(jiān)測(cè)信息采集系統(tǒng)，以降低鐵路區(qū)間軌旁監(jiān)測(cè)信息傳輸系統(tǒng)的建設(shè)成本和維護(hù)難度[10-11]。

(2)“Build failed：task in ‘objs/AP_NavEKF2’ failed”，這個(gè)報(bào)錯(cuò)是由于cygwin64環(huán)境內(nèi)部使用了較新的gcc導(dǎo)致的，這時(shí)會(huì)出現(xiàn)編譯實(shí)物固件時(shí)可以編譯成功，但是無(wú)法仿真。解決方法：將cygwin64環(huán)境內(nèi)的gcc版本號(hào)降級(jí)，再刪除“ardupilot/build”文件夾，以防止之前編譯產(chǎn)生的臨時(shí)文件影響編譯結(jié)果。

(3)“Build failed：a bytes-like object is required，not ‘str’”，導(dǎo)致這個(gè)報(bào)錯(cuò)的原因是build文件夾中的之前編譯的中間文件與本次編譯沖突。解決辦法：只需要將ardupilot/build文件夾刪掉然后重新開始仿真即可。

2.4 模擬器程序創(chuàng)新點(diǎn)

(1)虛擬環(huán)境重新配置

由于cygwin64環(huán)境內(nèi)部使用了最新的gcc編譯器，在進(jìn)行仿真的時(shí)候會(huì)報(bào)錯(cuò)，需要重新運(yùn)行cygwin64，將插件列表中的gcc-core、gcc-g++、libgcc1 3個(gè)安裝插件包的版本改為“9.3.0-2”(原環(huán)境默認(rèn)的是10.2.0-1系列版本)，改完后重啟電腦，此問題就能被解決。

(2)顯示函數(shù)選擇

要想在開源地面站MissionPlanner中顯示課題的內(nèi)容，而又不影響其他主要數(shù)據(jù)的顯示傳遞，函數(shù)頻率的選擇至關(guān)重要，經(jīng)過多次反復(fù)嘗試，選擇“one_hz_loop()”函數(shù)作為該軟件顯示本課題內(nèi)容的函數(shù)，最終成功實(shí)現(xiàn)在MissionPlanner中顯示“CRDC-2021A240505”課題的內(nèi)容。另外，無(wú)人機(jī)飛控與地面站之間的通信鏈路帶寬有限，大部分采用57600波特率的串口。飛控不可能一直向地面站發(fā)送信息，否則姿態(tài)解算很可能會(huì)出錯(cuò)。可采用每隔一定的時(shí)間發(fā)送一次，實(shí)現(xiàn)過程如下。

選擇函數(shù)：one_hz_loop()，添加代碼：Gcs().send_text(MAV_SEVERITY_CRITICAL，

“CRDC-2021A240505”)，即可實(shí)現(xiàn)課題相關(guān)內(nèi)容在MissionPlanner中的顯示。

(3)修改MissionPlanner起始點(diǎn)位置

修改飛控固件源碼中的自動(dòng)測(cè)試文件，將需要位置的經(jīng)緯度(十進(jìn)制)、海拔高度、初始航向等參數(shù)設(shè)置為變量CRDC_Home，然后把該變量加入到腳本文件，通過腳本文件調(diào)用該變量，即可實(shí)現(xiàn)三維位置地圖在該模擬器中的調(diào)用。

3 模擬器測(cè)試與評(píng)估

3.1 模擬飛行測(cè)試

模擬固件編譯成功后，軟件將自動(dòng)連接虛擬機(jī)。如果連接不成功，刪除飛控源代碼文件夾Ardupilot下的Build文件夾，重新編譯仿真固件。此時(shí)，打開地面站軟件MissionPlanner，連接模式設(shè)置為UDP，波特率選擇115200，點(diǎn)擊地面站連接按鈕，直接點(diǎn)擊彈出窗口的“OK”，模擬器連接地面站一般會(huì)成功。如果連接不成功，關(guān)閉MissionPlanner地面站，并關(guān)閉仿真環(huán)境，刪除源代碼中“ardupilot/build”文件夾，重新運(yùn)行仿真環(huán)境再試。

模擬器連接地面站成功后，根據(jù)需要，可以把模擬仿真位置設(shè)置為地球上的任意地方，也可以根據(jù)任務(wù)要求修改初始航向。飛行模式為stablize，定位形式為GPSrtk模式，HDOP的值為1.2，定位良好，衛(wèi)星數(shù)量10顆星，左側(cè)下半部顯示數(shù)據(jù)為模擬出的飛行姿態(tài)。然后，打開本地電腦文件夾ArdupilotToolsAutotest下的locations.txt文件，添加了1個(gè)名稱為“CRDC_Home”、緯度為“39.1636181°”、經(jīng)度為“117.2465090°”、海拔高度為2m、初始航向?yàn)?°的位置點(diǎn)，在腳本文件中添加預(yù)先選定的位置：/cygdrive/d/ardupilot/Tools/autotest/sim_vehicle.py-v Ardu Copter-L CRDC_Home。若不能顯示正確的home點(diǎn)，在“飛行計(jì)劃”頁(yè)面，點(diǎn)擊右側(cè)邊欄中的“起始位置”，即可跳轉(zhuǎn)到預(yù)先設(shè)置的位置(見圖5)。

圖5 模擬飛行流程

打開地面站MissionPlanner，點(diǎn)擊“飛行計(jì)劃”功能菜單，進(jìn)入航線規(guī)劃頁(yè)面，點(diǎn)擊鼠標(biāo)右鍵彈出功能選項(xiàng)菜單，按照菜單提示規(guī)劃自己想要的航線，航線規(guī)劃完成后點(diǎn)擊頁(yè)面右側(cè)的“寫入航點(diǎn)”和“保存航點(diǎn)文件”兩個(gè)功能鍵，保存自己規(guī)劃好的航線(因篇幅所限，不再具體講解)。航線規(guī)劃結(jié)束后，依次輸入以下命令“arm throttleenter c 3 1500entermode autoenter”，此時(shí)飛機(jī)會(huì)按照既定規(guī)劃航線完成飛行。

圖5中紫色航跡線為模擬飛行完成的規(guī)劃航線。模擬起飛時(shí)需要注意，APM固件不允許在自動(dòng)航線模式下起飛。如果在自動(dòng)航線模式下輸入解鎖命令，仿真環(huán)境將提示Mode not armable(解鎖失敗)。正確的做法是先將飛控切換到自穩(wěn)模式(目前有20多種飛行模式可以選擇)，然后解鎖起飛，起飛后再切換到自動(dòng)航線模式。

3.2 模擬器評(píng)估

評(píng)估一款無(wú)人機(jī)飛行模擬器的性能，主要是驗(yàn)證該款模擬器是否能夠逼真地模擬出無(wú)人機(jī)實(shí)際飛行時(shí)的各種要素[13-15]，如飛行姿態(tài)、高度、速度、無(wú)線電遙控信號(hào)狀態(tài)，導(dǎo)航定位信號(hào)狀態(tài)等。實(shí)踐表明，該款模擬器不但能夠顯示鐵路無(wú)人機(jī)裝備研發(fā)及應(yīng)用“CRDC-2021A240505”課題的相關(guān)內(nèi)容，而且飛行姿態(tài)等數(shù)據(jù)通過左側(cè)畫面顯示出來(lái)，導(dǎo)航定位信息為GPSrtk定位，定位狀態(tài)良好(HUD顯示區(qū)右下角顯示Fixed)，HODP值等于2，衛(wèi)星數(shù)量為10顆，無(wú)線電信號(hào)良好，其他硬件信息也可在相應(yīng)位置顯示出來(lái)。

采用相同的參數(shù)指標(biāo)比較實(shí)測(cè)飛行和模擬飛行的加速度日志圖像數(shù)據(jù)(見圖6)，從2次日志的4個(gè)轉(zhuǎn)彎加速度姿態(tài)圖像來(lái)看，第1轉(zhuǎn)彎處加速度差值為+2 cm/s2，第2轉(zhuǎn)彎處重合，第3轉(zhuǎn)彎處差值為-2 cm/s2，第4轉(zhuǎn)彎處差值為+1 cm/s2，不難看出，模擬飛行圖像與實(shí)測(cè)飛行圖像相互吻合度較高。

圖6 加速度圖像對(duì)比

4 結(jié)語(yǔ)

近年來(lái)，隨著無(wú)人機(jī)技術(shù)地不斷發(fā)展，無(wú)人機(jī)在鐵道勘察領(lǐng)域的應(yīng)用越來(lái)越廣泛，同時(shí)，圍繞無(wú)人機(jī)進(jìn)行的二次開發(fā)利用項(xiàng)目也越來(lái)越多。無(wú)人機(jī)裝備的研發(fā)需要多次的實(shí)際試飛驗(yàn)證，受實(shí)際試飛測(cè)試受天氣、空域管制、飛行安全、炸機(jī)等各方面的限制，試飛成本非常高。因此，對(duì)于新研發(fā)的無(wú)人機(jī)，應(yīng)首先在模擬環(huán)境上測(cè)試，這樣不但能夠有效地節(jié)約研發(fā)成本，還可以大幅提高研發(fā)效率。