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

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

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

0 引 言

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

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

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

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

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

1 系統的整體框架及初始化處理

1.1 系統的整體結構

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

圖1 系統的整體結構

1.2 坐標系定義

圖2 系統坐標系定義

1.3 視覺和IMU內外參標定

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

圖3 固聯的視覺慣導系統

1.4 IMU的初始對準

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

其中，俯仰角和橫滾角用θx、θy表示，g表示當地的重力加速度，IMU在x軸、y軸和z軸上的加速度用gx、gy、gz表示。通過式（3）、式（4）和式（5）可計算得到偏航角θx：

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

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

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

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

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

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

式中：λ——待估計的參數；

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

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

經過多次實驗發現此方法估計的尺度不僅計算簡便，而且估計的精度較高，在發揮充分穩定性的同時能實時完成尺度的估計。

3 基于EKF的信息融合

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

3.1 慣導解算過程

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

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

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

ωm——陀螺的測量值；

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

na、ng——服從正態分布的高斯白噪聲；

ba、——慣導內加速度計和陀螺的零漂。

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

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

σ——通過角速度積分求得的角度旋轉矢量。

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

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

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

其中T為IMU的采樣周期。

3.2 EKF預測過程

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

3.3 EKF更新過程

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

系統的增益矩陣K為

隨后通過式（22）和（23）更新系統的狀態向量和狀態協方差矩陣。

4 結果與討論

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

圖4 機載視覺慣導系統和實驗現場

4.1 尺度估計結果分析

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

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

表1 尺度估計值

圖5 ORB-SLAM尺度估計室內實驗

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

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

4.2 位姿估計結果分析

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

4.2.1 慣導解算結果分析

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

4.2.2 EKF融合結果分析

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

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

圖7 IMU動態實驗解算結果

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

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

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

5 結束語

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