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

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

0 引 言

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

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

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

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

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

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

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

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

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

1.2 坐標(biāo)系定義

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

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

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

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

1.4 IMU的初始對(duì)準(zhǔn)

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

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

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

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

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

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

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

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

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

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

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

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

3 基于EKF的信息融合

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

3.1 慣導(dǎo)解算過程

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

式中：am——加速度計(jì)的測量值；

a——IMU相對(duì)于慣性系的全加速度；

ωm——陀螺的測量值；

ω——IMU相對(duì)于慣性系的角速度；

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

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

可將零漂當(dāng)作噪聲引起的隨機(jī)游走過程。

IMU的運(yùn)動(dòng)方程可用下式表示：

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

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

通過下式可完成對(duì)速度的解算：

對(duì)速度進(jìn)行積分可解算出IMU當(dāng)前的位置信息，如下式所示：

其中T為IMU的采樣周期。

3.2 EKF預(yù)測過程

當(dāng)旋轉(zhuǎn)角較小時(shí)，可通過小角度近似法求得四元數(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é)果與討論

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

圖4 機(jī)載視覺慣導(dǎo)系統(tǒng)和實(shí)驗(yàn)現(xiàn)場

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

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

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

表1 尺度估計(jì)值

圖5 ORB-SLAM尺度估計(jì)室內(nèi)實(shí)驗(yàn)

圖6 ORB-SLAM尺度估計(jì)室外實(shí)驗(yàn)

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

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

本節(jié)將進(jìn)行通過純慣導(dǎo)對(duì)位姿信息進(jìn)行解算實(shí)驗(yàn)，然后通過信息融合解算出最終的位姿估計(jì)信息。

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

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

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

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

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

圖7 IMU動(dòng)態(tài)實(shí)驗(yàn)解算結(jié)果

圖8 EKF融合位姿真值對(duì)比圖

圖9 EKF融合位姿誤差對(duì)比圖

表2 融合實(shí)驗(yàn)誤差分析值（距離約200 m）

5 結(jié)束語

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