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

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

0 引 言

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

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

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

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

1 VINS-Mono算法模型研究

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

1.1 視覺殘差模型分析

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

（1）

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

（2）

（3）

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

1.2 慣性殘差模型分析

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

（4）

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

（5）

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

（6）

2 無人機飛行視覺慣性仿真方案設計

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

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

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

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

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

（7）

式中，P（t）、V（t）、a（t）分別為初始化模擬過程中載體的位置、速度和加速度的預設表達式。

（a） 初始化過程視覺特征點跟蹤

（b） 初始化航跡（紅色標記處為起/終點）圖1 初始化過程示意圖Fig.1 Diagrams of the initialization process

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

2.2 平飛過程的地面特征設置

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

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

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

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

3 高空平直飛行仿真實驗與分析

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

（a） 無噪聲仿真軌跡

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

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

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

4 結(jié) 論

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