視覺和慣導信息融合小型無人機位姿估計研究

王繼紅，吳伯彪，張亞超，趙明冬

（鄭州科技學院電氣工程學院，河南 鄭州 450064）

0 引 言

無人機執(zhí)行艱巨復雜的任務(wù)時，對位姿的估計及其重要，衛(wèi)星導航是常用的方法。但是衛(wèi)星的信號在許多復雜的環(huán)境下會變得非常微弱甚至失鎖。因此，可采用自主導航方式（主要包含慣性導航、視覺導航及雷達導航）代替。而較輕的單目視覺傳感器以其小體積、低功耗聞名，將其用于無人機上，既可用來定位，也可用來進行無人值守和目標追蹤與鎖定。視覺同步定位與建圖（visual simultaneous localization and mapping,VSLAM）技術(shù)[1]在近些年發(fā)展較快，這為無人機位姿估計提供了一種新的思路。

VSLAM技術(shù)指利用相機拍攝的圖像獲取自身位姿的同時完成對周圍環(huán)境圖像的構(gòu)建。目前主要包含兩種方法：一是通過兩幅圖像特征的提取獲取相機此時的位姿，較為典型的為PTAM算法[2]以及ORB-SLAM算法[3]。另一種則是通過優(yōu)化圖像中的像素幀間獲取位姿變化，稱為直接法，較為典型的為SVO算法[4]和LSDSLAM算法[5]。由于直接法計算量較大，無法滿足無人機位姿的實時解算。因此，本文利用ORB-SLAM算法進行位姿估計。

單目視覺的輸出頻率較低，無法滿足無人機對高動態(tài)性能的需求，且利用純視覺在復雜環(huán)境中獲取的位姿不夠精確。因此，將慣性測量單元（inertial measurement unit，IMU）和視覺進行組合用于無人機可獲得高精度的位姿估計。

將視覺傳感器和IMU進行融合可分為松緊耦合[6-7]兩種。其中，松耦合通過擴展卡爾曼濾波[8]或無跡卡爾曼濾波（unscented Kalman filter，UKF）[9]將視覺傳感器和IMU兩個單獨模塊獲得的位姿信息進行融合，文獻[10]和[11]采用的信息融合屬于松耦合。緊耦合則利用濾波器對視覺和IMU產(chǎn)生的中間數(shù)據(jù)進行處理后獲得的位姿信息，其算法相對松耦合較為復雜，且擴展性不強，無法對多傳感器信息進行融合，文獻[12]和[13]采用的信息融合屬于緊耦合。

綜上所述，基于松耦合具有更低的復雜度，可以滿足無人機對輸出頻率的高要求；且松耦合具備較強的擴展性，可以滿足無人機上多傳感器的信息融合。因此本文將通過松耦合完成無人機上的多信息融合，以此實現(xiàn)位姿信息的估計：首先通過無人機上的超聲波傳感器完成尺度的估計，然后利用最小二乘法完成尺度估計值的優(yōu)化。最后把單目視覺傳感器獲得的位姿估計值當作觀測量，通過擴展卡爾曼濾波完成對IMU位姿估計值的修正以提高位姿估計的精度。

1 系統(tǒng)的整體框架及初始化處理

1.1 系統(tǒng)的整體結(jié)構(gòu)

系統(tǒng)的整體框架如圖1所示，主要包括超聲波傳感器和IMU用來進行數(shù)據(jù)采集，單目相機用來進行圖像采集。通過帶尺度的ORB-SLAM算法對單目相機和超聲波傳感器采集的信息進行解算獲得頻率為25 Hz的六自由度位姿信息；通過慣導對IMU采集的數(shù)據(jù)進行解算得到頻率為200 Hz的六自由度位姿信息。利用擴展卡爾曼濾波算法對25 Hz和200 Hz的六自由度位姿信息進行融合，最終獲得頻率為200 Hz的六自由度位姿估計。

圖1 系統(tǒng)的整體結(jié)構(gòu)

1.2 坐標系定義

圖2 系統(tǒng)坐標系定義

1.3 視覺和IMU內(nèi)外參標定

本文對固聯(lián)后的單目相機和IMU進行安裝，如圖3所示，坐標系符合右手規(guī)則，以IMU初始時刻的姿態(tài)作為世界坐標系。采用二維碼標定板和三軸轉(zhuǎn)臺對相機與IMU進行單獨標定，以此獲取相機的內(nèi)參和IMU的零漂等參數(shù)。

圖3 固聯(lián)的視覺慣導系統(tǒng)

1.4 IMU的初始對準

眾所周知，慣性器件使用之前必須進行初始對準以獲取IMU的初始估計姿態(tài)，進而得到導航坐標系下IMU的初始姿態(tài)，也就是世界坐標系下的重力矢量。本文采用的方法是通過IMU自帶的磁強計計算出偏航角，再結(jié)合重力矢量在三軸上的分量及IMU測量的加速度進行俯仰角和橫滾角的解算，如式（1）和式（2）所示。

其中，俯仰角和橫滾角用θx、θy表示，g表示當?shù)氐闹亓铀俣?，IMU在x軸、y軸和z軸上的加速度用gx、gy、gz表示。通過式（3）、式（4）和式（5）可計算得到偏航角θx：

其中，磁強計在三個軸上的輸出分別用Mx、My、Mz表示。但是這種方法只是粗對準，后續(xù)還需采用卡爾曼濾波進行一個較為精確的對準，以獲取精確的結(jié)果。

2 基于最小二乘的單目視覺尺度估計

由于在位姿估計時單目VSLAM算法無法獲得絕對的尺度信息，這就需要其他信息進行輔助，以獲取可在無人機上使用的尺度估計。目前最好的單目VSLAM算法是ORB-SLAM算法，其在VSLAM算法的基礎(chǔ)上加入了閉環(huán)檢測，提高了整個程序的運行效率。但是本文研究的無人機位姿估計是在大場景下進行的。對算法的實時解算要求非常高。因此本文在ORB-SLAM算法的基礎(chǔ)上關(guān)閉了閉環(huán)檢測線程，且限制了關(guān)鍵幀和地圖點的數(shù)量，這樣可大大提高算法的實時解算速度。

由于以單目相機為傳感器的ORB-SLAM算法無法獲取深度/尺度信息。因此本文將通過超聲波傳感器獲取無人機的飛行高度，然后利用最小二乘法估計出其絕對尺度。

在一個固定的時間間隔內(nèi)，單目ORB-SLAM可獲取無人機在起飛階段上升的高度差估計值，記為zi，這個時間段內(nèi)無人機上升的實際高度差通過超聲波傳感器測得，記為hi，也稱為絕對高度，因此尺度λ近似滿足λ≈h/z。

但是這種方法誤差較大，很難得到最優(yōu)解。因此，本文將通過最小二乘估計完成尺度參數(shù)的估計。結(jié)合n組測量的觀測值及尺度高度兩者的關(guān)系可得：

式中：λ——待估計的參數(shù)；

ε1，ε2，···，εi——相互獨立的誤差值。

但是單目視覺ORB-SLAM在實際應(yīng)用中會發(fā)生漂移，需要對不同時間段的測量值分配不同的權(quán)重ω，然后通過加權(quán)最小二乘法完成最終尺度的估計，如下式所示：

經(jīng)過多次實驗發(fā)現(xiàn)此方法估計的尺度不僅計算簡便，而且估計的精度較高，在發(fā)揮充分穩(wěn)定性的同時能實時完成尺度的估計。

3 基于EKF的信息融合

雖然無人機的位姿信息通過VSLAM和慣導均可獲取，但是慣導解算時的輸出頻率相對VSLAM較快，慣導隨著時間的增長會出現(xiàn)累積誤差，而VSLAM姿態(tài)誤差較大。因此，利用EKF進行信息融合時，本文將VSLAM的輸出當作觀測值，對慣導的狀態(tài)進行預(yù)測，實時修正慣導解算得到的位姿。

3.1 慣導解算過程

利用IMU對慣導進行解算時需考慮噪聲對其產(chǎn)生的影響，模型如下：

式中：am——加速度計的測量值；

a——IMU相對于慣性系的全加速度；

ωm——陀螺的測量值；

ω——IMU相對于慣性系的角速度；

na、ng——服從正態(tài)分布的高斯白噪聲；

ba、——慣導內(nèi)加速度計和陀螺的零漂。

可將零漂當作噪聲引起的隨機游走過程。

IMU的運動方程可用下式表示：

σ——通過角速度積分求得的角度旋轉(zhuǎn)矢量。

通過圓錐算法可消除旋轉(zhuǎn)矢量帶來的常值漂移[14]。

通過下式可完成對速度的解算：

對速度進行積分可解算出IMU當前的位置信息，如下式所示：

其中T為IMU的采樣周期。

3.2 EKF預(yù)測過程

當旋轉(zhuǎn)角較小時，可通過小角度近似法求得四元數(shù)誤差和角度誤差之間的關(guān)系，分別用δq和δθ表示，如下式所示：

3.3 EKF更新過程

作為濾波可調(diào)參數(shù)的協(xié)方差矩陣R可通過觀測噪聲給定。通過觀測矩陣和R可求得EKF的更新過程，如式（21）～（23）所示。

系統(tǒng)的增益矩陣K為

隨后通過式（22）和（23）更新系統(tǒng)的狀態(tài)向量和狀態(tài)協(xié)方差矩陣。

4 結(jié)果與討論

實驗選用的無人機為大疆的M100小型多旋翼無人機，無人機自身配備了GPS接收器以及機載IMU，通過內(nèi)部算法可以直接得到無人機的運行軌跡。整個視覺慣導位姿估計系統(tǒng)搭載于無人機的下方，如圖4所示，相機為普通的USB攝像頭，可獲得1080P的彩色圖像，圖像輸出最高頻率為60 Hz，IMU的型號為MTI300。室內(nèi)實驗和室外實驗均選擇開闊的場地進行，實驗當天天氣晴朗，可見度較高，非常適合實驗，其中室外無人機飛行的環(huán)境如圖4所示。系統(tǒng)中將超聲波傳感器也垂直安裝于相機正下方，三者形成固聯(lián)。

圖4 機載視覺慣導系統(tǒng)和實驗現(xiàn)場

4.1 尺度估計結(jié)果分析

實驗前需對小型無人機進行初始化，讓無人機進行多次的垂直起飛以獲取多組高度差數(shù)據(jù)，以保證尺度優(yōu)化的精度和穩(wěn)定性。本文進行了包含閉環(huán)檢測的近距離尺度估計實驗和不包含閉環(huán)檢測的遠距離尺度估計實驗。近距離實驗無人機位姿信息的真值通過VICON運動捕捉系統(tǒng)提供，遠距離實驗無人機位姿信息通過高精度組合導航系統(tǒng)提供。

實驗結(jié)果如圖5、圖6和表1所示，其中圖5為近距離實驗，在室內(nèi)進行；圖6為遠距離實驗，在室外進行。起始位置和終止位置分別由圓點和三角形表示，星號代表閉環(huán)檢測的位置。圖中紅線代表帶尺度估計的ORB-SLAM估計位置，綠線代表無尺度估計的ORB-SLAM估計位置，藍線是無人機的真實位置。

表1 尺度估計值

圖5 ORB-SLAM尺度估計室內(nèi)實驗

圖6 ORB-SLAM尺度估計室外實驗

從實驗結(jié)果可得，無人機運行的前半段時間，帶尺度的ORB-SLAM軌跡與無人機真實的軌跡幾乎重合，體現(xiàn)了尺度估計效果的明顯。但是隨著時間的增長，尺度發(fā)生了一定的漂移，導致通過ORBSLAM得到的軌跡相比真實軌跡產(chǎn)生了誤差。此時通過無人機產(chǎn)生的高度變化對尺度估計進行重啟可對漂移進行一定程度上的修正。另隨著時間的增長，ORB-SLAM自身也產(chǎn)生了一定的累積誤差，當閉環(huán)檢測成功后會對累積誤差進行一定程度的修正，對尺度估計的精度有較為明顯的提高。

4.2 位姿估計結(jié)果分析

本節(jié)將進行通過純慣導對位姿信息進行解算實驗，然后通過信息融合解算出最終的位姿估計信息。

4.2.1 慣導解算結(jié)果分析

本文選擇的IMU輸出頻率為200 Hz，通過靜態(tài)實驗對IMU的初始姿態(tài)和零漂進行獲?。唤又M行IMU在動態(tài)環(huán)境下的實驗，利用IMU圍繞半徑為2 m的圓旋轉(zhuǎn)一周形成一個圓形軌跡。從實驗結(jié)果圖7中可得，IMU解算的姿態(tài)信息與真值基本重合，體現(xiàn)了姿態(tài)解算具有較高的準確度。但是位置結(jié)果卻在后幾十秒出現(xiàn)了較大的漂移，這是因為無人機上使用的IMU精度較低，解算的位置自然會出現(xiàn)較大的漂移，因此需加入其他傳感器進行信息融合以完成對位置的修正，從而解算出更準確的位姿。

4.2.2 EKF融合結(jié)果分析

本文將ORB-SLAM的位姿估計結(jié)果當作觀測值以修正IMU解算結(jié)果。本文在數(shù)據(jù)融合時利用ROS發(fā)布節(jié)點的時間進行視覺和慣導的數(shù)據(jù)對齊。信息融合實驗選擇在室外的大場景下進行，飛行軌跡如圖6所示，誤差見表2，實驗分析結(jié)果見圖8和圖9。

由圖8可得，融合后的位置信息與真值基本一致，相比圖7精度得到了一定的提升，且隨著時間的增長也不再發(fā)生漂移。由圖9可得，融合后的位置誤差和姿態(tài)誤差都有一定的降低，尤其姿態(tài)誤差下降地較為明顯。表2中，無人機在200 m的動態(tài)飛行中，融合后的位置誤差和姿態(tài)誤差相對純慣導或者單目ORB-SLAM算法均有很大程度的減小，位置誤差的均方根低于0.995 m，水平姿態(tài)角的均方根誤差低于1.915°，偏航角的均方根誤差達到了2.235°。上述實驗表明，通過信息的融合不但提高了位姿的估計精度，而且解決了純視覺輸出頻率低的問題，滿足了無人機對高動態(tài)特性的需求，解決了無人機在衛(wèi)星信號失鎖時無法精確定位的問題。

圖7 IMU動態(tài)實驗解算結(jié)果

圖8 EKF融合位姿真值對比圖

圖9 EKF融合位姿誤差對比圖

表2 融合實驗誤差分析值（距離約200 m）

5 結(jié)束語

本文以無人機在衛(wèi)星信號失鎖時無法實現(xiàn)精確定位為研究背景，設(shè)計了一個包含單目視覺、IMU及超聲波傳感器的組合系統(tǒng)，并且提出利用最小二乘法完成尺度估計值的優(yōu)化。通過擴展卡爾曼濾波實現(xiàn)單目視覺與慣導的信息融合，可提高位姿的估計精度，并解決純視覺輸出頻率低的問題，滿足無人機對高動態(tài)特性的需求，為無人機在衛(wèi)星信號失鎖時無法精確定位的問題提供一種解決辦法。