基于視慣融合的無(wú)人飛行器導(dǎo)航數(shù)據(jù)仿真方法*

張逸舟，曾慶化,2,3，許 睿，王景琪，羅怡雪

（1.南京航空航天大學(xué) 自動(dòng)化學(xué)院 導(dǎo)航研究中心·南京·211106；2.南京航空航天大學(xué) 先進(jìn)飛行器導(dǎo)航、控制與健康管理工業(yè)和信息化部重點(diǎn)實(shí)驗(yàn)室·南京·211106；3.南京航空航天大學(xué) 江蘇省物聯(lián)網(wǎng)與控制技術(shù)重點(diǎn)實(shí)驗(yàn)室·南京·211106）

0 引 言

針對(duì)當(dāng)前無(wú)人飛行器在使用過(guò)程中可能面臨的衛(wèi)星信號(hào)拒止等條件，依靠載體自身所搭載的傳感設(shè)備完成智能導(dǎo)航定位與準(zhǔn)確的路徑規(guī)劃將變得更具挑戰(zhàn)性。當(dāng)飛行器執(zhí)行隱蔽工作任務(wù)或是飛行環(huán)境相對(duì)復(fù)雜時(shí)，使用自主式的傳感系統(tǒng)如慣性器件和對(duì)外式的傳感系統(tǒng)如相機(jī)器件、激光雷達(dá)、氣壓高度計(jì)等，將更有利于實(shí)現(xiàn)同步定位與建圖（Simultaneous Localization and Mapping，SLAM）功能，因而被廣泛研究。

單獨(dú)的傳感器導(dǎo)航系統(tǒng)具有一定局限性，例如慣性器件在短時(shí)間內(nèi)可以得到較準(zhǔn)確的位姿信息，但會(huì)因噪聲的積累使得誤差隨時(shí)間發(fā)散。視覺(jué)導(dǎo)航中,單目圖像序列難以提供有效的尺度信息，在運(yùn)動(dòng)模糊、特征丟失的情況下將無(wú)法正常工作。因此，慣性導(dǎo)航通常被認(rèn)為是與視覺(jué)導(dǎo)航互補(bǔ)的導(dǎo)航系統(tǒng)：通過(guò)運(yùn)動(dòng)恢復(fù)結(jié)構(gòu)的約束可限制慣性測(cè)量積分結(jié)果的發(fā)散，且與慣性測(cè)量融合后系統(tǒng)的尺度、俯仰角和滾轉(zhuǎn)角均具備能觀性[1]?，F(xiàn)如今，視覺(jué)慣性里程計(jì)（Visual-Inertial Odometry，VIO）已成為發(fā)展較成熟的組合導(dǎo)航算法，該技術(shù)被廣泛應(yīng)用在機(jī)器人[2]、陸地車輛[3]等自主導(dǎo)航設(shè)備中。

在視慣融合導(dǎo)航方法中，ORB（Oriented FAST and Rotated BRIEF）特征點(diǎn)[4]提取方法較為經(jīng)典，但該方法容易錯(cuò)誤匹配特征且運(yùn)算效率有待提升；光流法[5]則是跟蹤特征的另一種方式，光流可用于表示載體的運(yùn)動(dòng)，在連續(xù)幀間跟蹤特征時(shí)能兼顧魯棒性與速度。在后端中有基于濾波和基于優(yōu)化兩種組合策略，基于李群的視慣融合算法[6]利用無(wú)跡卡爾曼濾波對(duì)算法進(jìn)行優(yōu)化，較典型的是利用光流的雙目多狀態(tài)約束卡爾曼濾波（Multi-State Constraint Kalman Filter， MSCKF）[7]的視覺(jué)慣性里程計(jì)，也有基于Census變換改進(jìn)光流的單目視覺(jué)導(dǎo)航算法[8]。采用圖優(yōu)化方法能夠有效提高視慣融合的魯棒性，基于該方法的尺度不敏感多傳感器融合框架[9]已在多種平臺(tái)下有所應(yīng)用。

香港科技大學(xué)的開(kāi)源視覺(jué)慣性里程計(jì)算法VINS-Mono[10]是基于圖優(yōu)化的單目視覺(jué)慣性里程計(jì)算法，在啟動(dòng)后需要首先完成初始化過(guò)程，在載體做勻速直線運(yùn)動(dòng)的過(guò)程中啟動(dòng)將難以完成求解。針對(duì)此問(wèn)題，本文設(shè)計(jì)了一種該場(chǎng)景下視覺(jué)特征與慣性數(shù)據(jù)的仿真方案，研究并解決了初始化、航跡設(shè)置和特征點(diǎn)仿真等系列問(wèn)題。仿真實(shí)驗(yàn)表明，在使用本文算法輔助完成初始化過(guò)程后，VINS-Mono可以很好地應(yīng)用于無(wú)人飛行器自主導(dǎo)航作業(yè)，在不加入噪聲時(shí)，無(wú)人機(jī)500m平直飛行過(guò)程的定位誤差不超過(guò)真值的3.5%，為該場(chǎng)景下的無(wú)人飛行器視覺(jué)慣性組合導(dǎo)航問(wèn)題提供了重要的算法參考。

1 VINS-Mono算法模型研究

VINS-Mono方案是一個(gè)基于緊耦合與圖優(yōu)化的滑動(dòng)窗口單目視覺(jué)慣性里程計(jì)，使用緊耦合算法將具備更高的精度與準(zhǔn)確性[11]。算法使用圖優(yōu)化進(jìn)行組合，該優(yōu)化模型的三部分殘差組成為：基于光束平差法（Bundle Adjustment，BA）的視覺(jué)殘差、慣性測(cè)量殘差以及邊緣化先驗(yàn)殘差。本節(jié)將對(duì)與仿真過(guò)程直接相關(guān)的視覺(jué)殘差和慣性殘差模型進(jìn)行簡(jiǎn)要分析。

1.1 視覺(jué)殘差模型分析

VINS-Mono中使用光束平差法構(gòu)造誤差函數(shù)，得到重投影誤差之和最小的狀態(tài)估計(jì)。視覺(jué)重投影誤差被定義為一個(gè)特征點(diǎn)在歸一化相機(jī)坐標(biāo)系下的估計(jì)值與觀測(cè)值的差。假設(shè)第i、j幀為前后相鄰幀，前幀的某個(gè)歸一化特征點(diǎn)坐標(biāo)可用（ui,vi）表示，對(duì)應(yīng)逆深度λ在算法中已求，后幀中依據(jù)光流法跟蹤觀測(cè)到的該特征點(diǎn)坐標(biāo)可用（uj,vj）表示，相機(jī)與載體系外參數(shù)陣為Tbc，載體在世界坐標(biāo)系中的位姿為Twbi、Twbj，則后幀中對(duì)于該特征點(diǎn)的估計(jì)值計(jì)算如下

（1）

該點(diǎn)的視覺(jué)重投影誤差rc可用平移、旋轉(zhuǎn)量（包含于Twbi、Twbj中）的映射函數(shù)表示，見(jiàn)式（2）。對(duì)于滑動(dòng)窗口中的m幀圖像,使用式（3）構(gòu)建誤差函數(shù)，由此得到視覺(jué)殘差量。

（2）

（3）

其中，pci為第i幀時(shí)相機(jī)在空間中的位置；qci為第i幀時(shí)相機(jī)在空間中的旋轉(zhuǎn)四元數(shù)。

1.2 慣性殘差模型分析

慣性測(cè)量數(shù)學(xué)模型可用式（4）表示，bgyro、ngyro表示陀螺儀的偏置和白噪聲，bacc、nacc表示加速度計(jì)的偏置和白噪聲，偏置滿足隨機(jī)游走模型,即對(duì)應(yīng)一階導(dǎo)數(shù)為白噪聲形式。

（4）

VIO系統(tǒng)中慣性測(cè)量頻率通常高于圖像，為避免每次更新相機(jī)姿態(tài)后對(duì)數(shù)據(jù)重復(fù)積分，VINS-Mono中引入了預(yù)積分概念。此處同樣假設(shè)第i、j幀為前后相鄰幀，預(yù)積分中的積分項(xiàng)是第j幀相對(duì)于第i幀圖像坐標(biāo)的姿態(tài)，而非相對(duì)世界坐標(biāo)系的姿態(tài)，因此，預(yù)積分的值只與慣性測(cè)量單元（Inertial Measurement Unit，IMU）短期內(nèi)測(cè)量值相關(guān)。定義位移、速度和角度的預(yù)積分增量如式（5）所示

（5）

式中，abt表示在機(jī)體系下t時(shí)刻的加速度真值；ωbt表示在機(jī)體系下t時(shí)刻的角速度真值；αbibj、βbibj、qbibj分別表示在關(guān)鍵幀之間的位置、速度、角度預(yù)積分增量項(xiàng)。由此可以得到式（6）中定義在i、j時(shí)刻IMU測(cè)量的位移、姿態(tài)、速度與偏置的殘差，姿態(tài)通過(guò)四元數(shù)乘法計(jì)算得到，位移、速度與偏置的殘差均直接相減獲得。

（6）

2 無(wú)人機(jī)飛行視覺(jué)慣性仿真方案設(shè)計(jì)

由視覺(jué)慣性測(cè)量模型可知，若需對(duì)特定場(chǎng)景下的無(wú)人機(jī)飛行進(jìn)行視覺(jué)慣性數(shù)據(jù)仿真，其難點(diǎn)在于構(gòu)造無(wú)人機(jī)觀測(cè)到的視覺(jué)特征點(diǎn)，以及陀螺儀和加速度計(jì)的三軸輸出。本文使用賀一家團(tuán)隊(duì)[12]的開(kāi)源視覺(jué)慣性仿真工具imusim進(jìn)行方案設(shè)計(jì)與實(shí)驗(yàn)驗(yàn)證。

2.1 系統(tǒng)初始化仿真方法

VINS-Mono為單目VIO系統(tǒng)，此類算法的一大難題是退化運(yùn)動(dòng)將導(dǎo)致尺度的不確定性，諸如勻速直線運(yùn)動(dòng)、懸停、靜止均被視作退化運(yùn)動(dòng)，但對(duì)于無(wú)人機(jī)而言，此類是常見(jiàn)的飛行狀態(tài)[13]。初始化通常是VIO系統(tǒng)中非常重要的步驟，其目的是在未知情況下估計(jì)系統(tǒng)的重力方向、速度以及尺度的初始狀態(tài)。

為了呈現(xiàn)可觀測(cè)的尺度，系統(tǒng)在初始化過(guò)程中至少需要有兩個(gè)軸上的非零加速度，故需要額外對(duì)初始化狀態(tài)的視覺(jué)慣性數(shù)據(jù)進(jìn)行仿真。仿真無(wú)人機(jī)在平飛作業(yè)前的初始化過(guò)程，其航跡應(yīng)滿足如下兩點(diǎn)要求：1）初始化軌跡需要有對(duì)三軸的加速度激勵(lì)，但為確保軌跡的真實(shí)性，三軸速度不可以發(fā)生跳變；2）起飛點(diǎn)與初始化軌跡結(jié)束點(diǎn)位置要確保一致，在初始化結(jié)束至平飛階段三軸的速度不應(yīng)發(fā)生跳變。

無(wú)人機(jī)起飛過(guò)程中勢(shì)必會(huì)產(chǎn)生晃動(dòng)，因此使用正弦、余弦函數(shù)進(jìn)行起飛前的初始化模擬是合理的。導(dǎo)航坐標(biāo)系為東北天（ENU）坐標(biāo)系，初始化參數(shù)設(shè)計(jì)為：起點(diǎn)（0，0，82），東向、北向分別做幅值20m的余弦、正弦運(yùn)動(dòng)，天向做幅值2m的余弦運(yùn)動(dòng)且角速率為另兩軸一定整數(shù)倍，運(yùn)動(dòng)時(shí)長(zhǎng)40s，控制角速度使得初始化過(guò)程為整圈運(yùn)動(dòng)并回到起點(diǎn)。依據(jù)確定的軌跡參數(shù)可以推導(dǎo)仿真出相應(yīng)的IMU數(shù)據(jù)[14]，具體的軌跡位置和運(yùn)動(dòng)公式如式（7）所示。軌跡如圖1（b）所示。

（7）

式中，P（t）、V（t）、a（t）分別為初始化模擬過(guò)程中載體的位置、速度和加速度的預(yù)設(shè)表達(dá)式。

（a） 初始化過(guò)程視覺(jué)特征點(diǎn)跟蹤

（b） 初始化航跡（紅色標(biāo)記處為起/終點(diǎn)）圖1 初始化過(guò)程示意圖Fig.1 Diagrams of the initialization process

在完成對(duì)于初始化過(guò)程的補(bǔ)充后，VINS-Mono算法可以順利恢復(fù)系統(tǒng)初值，并延續(xù)對(duì)于勻速平飛過(guò)程的里程計(jì)求解。該仿真方案亦可用于對(duì)接其他的非退化運(yùn)動(dòng)狀態(tài)的軌跡解算。上述的初始化補(bǔ)充過(guò)程思路可簡(jiǎn)述為如下兩部分：1）第一階段：對(duì)系統(tǒng)狀態(tài)值進(jìn)行初始化運(yùn)動(dòng)補(bǔ)充，使算法完成初始化過(guò)程，獲得系統(tǒng)狀態(tài)初始值包括速度、重力加速度、偏差、白噪聲和姿態(tài)初始值，同時(shí)獲得以零位置為基準(zhǔn)的位移量；2）第二階段：繼續(xù)完成整周的飛行過(guò)程，目的是使初始化完成后的航向角與所需對(duì)接的勻速平飛過(guò)程起點(diǎn)時(shí)刻的航向角相等或相近。整個(gè)過(guò)程的運(yùn)動(dòng)軌跡、特征點(diǎn)均為程序設(shè)置的虛擬軌跡、路標(biāo)點(diǎn)，仿真設(shè)置的特征點(diǎn)經(jīng)過(guò)光流跟蹤后被轉(zhuǎn)化為像素坐標(biāo)點(diǎn)的形式存儲(chǔ)在文本中。

2.2 平飛過(guò)程的地面特征設(shè)置

完成初始化后，進(jìn)行無(wú)人機(jī)的正常作業(yè)階段，出于算法驗(yàn)證的目的，此處仍使用imusim工具繼續(xù)生成數(shù)據(jù)。平飛過(guò)程初速度設(shè)置與初始化結(jié)束瞬間相同且保持勻速直線運(yùn)動(dòng)，航向角保持為0°，過(guò)程中保持視野中始終存在多組地面特征點(diǎn)，且當(dāng)無(wú)人機(jī)平行移動(dòng)一段時(shí)間后,特征點(diǎn)群會(huì)周期性更新。VINS-Mono使用光流法進(jìn)行跟蹤，因此需要分段設(shè)置視野特征點(diǎn)，并保證每組特征點(diǎn)間均有重合部分，否則會(huì)導(dǎo)致特征跟蹤中斷。平飛過(guò)程對(duì)地面點(diǎn)的跟蹤效果如圖2所示。

圖2 平飛500m地面特征點(diǎn)仿真示意圖Fig.2 Simulation diagram of ground feature points in 500 meters flight mission

飛行過(guò)程中,假設(shè)慣性器件的坐標(biāo)系與載體坐標(biāo)系完全重合，相機(jī)與慣性器件坐標(biāo)系垂直且有一定的位置偏移量。關(guān)于外參數(shù)矩陣以及相機(jī)的內(nèi)參數(shù)矩陣[15]設(shè)置如表1所示。表1中,R、t為相機(jī)系至IMU系間的旋轉(zhuǎn)矩陣和平移向量；fx、fy表示軸分辨率；u0、v0表示光心像素坐標(biāo)零點(diǎn)偏置。

表1 相機(jī)與IMU仿真參數(shù)

3 高空平直飛行仿真實(shí)驗(yàn)與分析

用VINS-Mono算法解算無(wú)人機(jī)勻速平飛的俯拍視圖與慣性數(shù)據(jù)，目的是驗(yàn)證該算法在此類場(chǎng)景下的可用性。結(jié)合imusim仿真工具，設(shè)置圖像頻率為30Hz，IMU輸出頻率為200Hz，以上述設(shè)計(jì)方案分別生成得到視覺(jué)關(guān)鍵幀點(diǎn)信息文件和陀螺儀與加速度計(jì)輸出（數(shù)據(jù)為七維：時(shí)間戳、陀螺儀角速度輸出、加速度計(jì)輸出），結(jié)合時(shí)間戳對(duì)關(guān)鍵幀文件進(jìn)行命名，實(shí)現(xiàn)數(shù)據(jù)的對(duì)齊。

（a） 無(wú)噪聲仿真軌跡

飛行過(guò)程中的飛行距離與航向角統(tǒng)計(jì)如表2所示，因添加噪聲后平飛軌跡不能正確跟蹤，故加噪后的數(shù)據(jù)只統(tǒng)計(jì)到前80s作為參照。航距真值隨平飛時(shí)間遞增，航向角真值理論上始終為0°。由表2可知，在未加入IMU噪聲時(shí)，無(wú)人機(jī)勻速平飛情況下可使用VINS-Mono對(duì)視覺(jué)慣性數(shù)據(jù)進(jìn)行里程計(jì)求解。

表2 仿真數(shù)據(jù)解算值統(tǒng)計(jì)表格

VINS-Mono以光流法對(duì)視覺(jué)特征點(diǎn)進(jìn)行跟蹤，說(shuō)明光流法跟蹤的視覺(jué)導(dǎo)航精度尚可，平飛500m過(guò)程中航距解算誤差不超過(guò)真值的3.5%，且航向角始終保持穩(wěn)定。加入IMU噪聲后，在誤差積累到一定程度時(shí)系統(tǒng)需要二次初始化以實(shí)現(xiàn)故障恢復(fù)，由于本文中算法對(duì)地面特征點(diǎn)的仿真規(guī)律分布于北向且缺乏對(duì)三軸的激勵(lì)，因此不能很好地進(jìn)行二次初始化。

4 結(jié) 論

針對(duì)無(wú)人飛行器在可能的衛(wèi)星拒止條件下的平直飛行作業(yè)場(chǎng)景，本文基于緊耦合的開(kāi)源視覺(jué)慣性里程計(jì)算法VINS-Mono，設(shè)計(jì)了相應(yīng)的數(shù)據(jù)仿真方案。針對(duì)單目VIO系統(tǒng)在退化運(yùn)動(dòng)下初始化困難的問(wèn)題，進(jìn)行了相應(yīng)的初始化運(yùn)動(dòng)軌跡補(bǔ)充，具體為使用正余弦運(yùn)動(dòng)模擬無(wú)人機(jī)穩(wěn)定飛行前的加速運(yùn)動(dòng)，并在后續(xù)對(duì)平飛過(guò)程的地面特征點(diǎn)進(jìn)行設(shè)置和仿真。實(shí)驗(yàn)結(jié)果表明，該初始化補(bǔ)充方案可以正確恢復(fù)系統(tǒng)初值，并延續(xù)導(dǎo)航參數(shù)的解算，在理想條件下，500m平直飛行過(guò)程中航距解算誤差比例不超過(guò)設(shè)置真值的3.5%，說(shuō)明VINS-Mono算法具備在該場(chǎng)景下使用的可行性。但本文尚未對(duì)IMU噪聲和故障恢復(fù)部分進(jìn)行仿真，在加入IMU噪聲后將無(wú)法進(jìn)行正確解算，因此仍需更深層次的研究。