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

劉亞非

(中國鐵路設(shè)計集團有限公司，天津 300308)

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

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

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

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

(1)

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

(2)

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

(3)

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

1.2 無人機飛行姿態(tài)控制的實現(xiàn)

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

圖1 PID控制流程

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

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

該模擬器基于目前開源市場最新的飛控固件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)險低、建模精度高、數(shù)據(jù)冗余低的優(yōu)勢[10]。

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

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

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

圖2 實物固件與模擬固件編譯對比

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

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

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

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

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

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

(3)模擬固件編譯

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

圖4 模擬固件編譯界面

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

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

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

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

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

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

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

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

由于cygwin64環(huán)境內(nèi)部使用了最新的gcc編譯器，在進行仿真的時候會報錯，需要重新運行cygwin64，將插件列表中的gcc-core、gcc-g++、libgcc1 3個安裝插件包的版本改為“9.3.0-2”(原環(huá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ù)，最終成功實現(xiàn)在MissionPlanner中顯示“CRDC-2021A240505”課題的內(nèi)容。另外，無人機飛控與地面站之間的通信鏈路帶寬有限，大部分采用57600波特率的串口。飛控不可能一直向地面站發(fā)送信息，否則姿態(tài)解算很可能會出錯。可采用每隔一定的時間發(fā)送一次，實現(xiàn)過程如下。

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

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

(3)修改MissionPlanner起始點位置

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

3 模擬器測試與評估

3.1 模擬飛行測試

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

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

圖5 模擬飛行流程

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

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

3.2 模擬器評估

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

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

圖6 加速度圖像對比

4 結(jié)語

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