張琳 曾成 王羿帆

摘 ?要： 針對基于微機電系統(tǒng)的慣性傳感器中，陀螺儀在姿態(tài)解算過程中存在誤差累積的問題，提出一種多時間尺度融合的視覺輔助慣性定姿算法。利用視覺傳感器在不同時間尺度的角度變化量，不斷修正并更新慣性器件的角度累積誤差，對姿態(tài)角進行多次校正，進而輸出較高精度的姿態(tài)角。實驗結(jié)果表明，采用多時間尺度融合的視覺輔助慣性定姿算法，相對于單時間尺度的融合算法，航向角誤差降低了27%，俯仰角誤差降低了23%，橫滾角誤差降低了63%，角度誤差能夠保持在2°以內(nèi)，滿足定姿定位的需求。

關(guān)鍵詞： 姿態(tài)解算; 多時間尺度融合; 慣性測量單元; 數(shù)據(jù)采集; 姿態(tài)角校正; 角度誤差更新

中圖分類號： TN820.2+2?34; TP212.9 ? ? ? ? ? ? ?文獻標識碼： A ? ? ? ? ? ? ? ? ? ?文章編號： 1004?373X（2020）12?0014?04

Abstract： In allusion to the error accumulation in the attitude resolving of the gyroscope in the inertial sensor based on micro?electro mechanical system， a visual aided inertial attitude determination algorithm based on multi?time scale fusion is proposed. By means of the angle variation of the vision sensor at different time scales， the angle cumulative error of the inertial device are corrected and updated continuously， and the attitude angle is corrected for many times to output the attitude angle with higher precision. The experimental results show that， in comparison with the single?time scale fusion algorithm， the visual aided inertial attitude determination algorithm based on the multi?time scale fusion reduces the course angle error by 27%， the pitch angle error by 23%， and the roll angle error by 63%. The angle error can be able to keep within 2° to meet the needs of attitude?determination and positioning.

Keywords： attitude resolving; multi?time scale fusion; inertial measurement unit; data acquisition; attitude angle correction; angle error renewal

0 ?引 ?言

基于微機電系統(tǒng)傳感器的慣性測量單元（Inertial Measurement Unit，IMU）在慣性定位技術(shù)中得到了廣泛應(yīng)用，尤其是衛(wèi)星信號不能穿透建筑物的室內(nèi)環(huán)境[1]。由于IMU中的陀螺儀存在較高的漂移誤差，而且捷聯(lián)慣導(dǎo)系統(tǒng)在解算過程中采用積分的方法，導(dǎo)致內(nèi)部誤差和外部誤差不斷累積，并且缺乏自我糾正的手段，遠遠達不到高精度室內(nèi)定位的需求。視覺傳感器誤差不隨時間累積，成本低，但比較依賴光照條件和可視環(huán)境[2]。將視覺傳感器和慣性傳感器的信息進行優(yōu)勢互補：求解載體的姿態(tài)采用以慣性傳感器為主，視覺傳感器為輔的數(shù)據(jù)融合方式[3]。在無視覺輸出信息或視覺信息不充足的情況下，IMU能夠持續(xù)提供載體的姿態(tài)變化;在視覺信息充足的情況下，視覺信息抑制IMU的姿態(tài)發(fā)散，校正慣性器件的參數(shù)，保證載體的姿態(tài)精度。

針對視覺傳感器和慣性傳感器的信息融合問題，已經(jīng)出現(xiàn)了眾多的融合算法。比如：使用次優(yōu)[H∞]濾波迭代融合算法，融合IMU和單目相機的測量結(jié)果，用融合結(jié)果和慣性結(jié)果的差值修正并更新漂移誤差曲線，精度達到了理想效果，但計算繁瑣[4];將IMU與視覺進行擴展卡爾曼濾波，并與其他算法結(jié)合的融合算法，但在融合過程中消除部分IMU累積誤差的同時，視覺的誤差也會被融進一部分，過濾時條件苛刻，具有一定的局限性[5?7]?；谝陨蠁栴}，本文提出一種多時間尺度融合的慣性定姿算法，多時間尺度的短時間尺度實時性強，長時間尺度約束短時間尺度，減小觀測噪聲的累積，長時間尺度和短時間尺度相互影響，利用視覺的不同時間尺度的角度變化量不斷地去修正并更新IMU的解算姿態(tài)，對角度信息進行多次校正，從而消除IMU解算的累積誤差，得到精度較高的姿態(tài)角。

1 ?多時間尺度融合算法

多時間尺度融合算法的具體實現(xiàn)方法如圖1所示。

首先，對視覺傳感器采集的視頻進行分幀，每隔a幀（a為自然數(shù)）提取一次圖片，所提取的相鄰兩圖之間的時間間隔為t1。此時，視覺信息得到的是載體坐標系下以t1為周期的角度變化量，根據(jù)姿態(tài)矩陣，將其轉(zhuǎn)換到地理坐標系下作為融合值。視覺傳感器的幀速率小于慣性傳感器的頻率，慣性器件利用四階龍格?庫塔法更新四元數(shù)[8]，解算的每t1時間內(nèi)數(shù)據(jù)不止一個，因此慣性數(shù)據(jù)具有一定的累積誤差。慣性器件解算的每t1時間內(nèi)姿態(tài)角的變化量作為融合值，進行第一次數(shù)據(jù)融合，即短時間尺度融合，保證結(jié)果的實時性，輸出第一次最優(yōu)估計值。

其次，每隔b幀（b>a，b為自然數(shù)）提取圖片，相鄰兩圖之間的時間間隔為t2，顯然，t2>t1。以t2為周期的視覺角度變化量和第一次最優(yōu)估計值中每t2時間的角度變化量作為融合值，進行第二次數(shù)據(jù)融合，即長時間尺度融合，約束短時間尺度，減小短時間尺度累積的觀測噪聲，輸出第二次最優(yōu)估計值。

若第二次最優(yōu)估計值符合定姿的需求，則以第二次最優(yōu)估計值為姿態(tài)角;否則，每隔c幀 （c>b，c為自然數(shù)）解算得到的數(shù)值與第二次最優(yōu)估計值進行融合，融合后的角度即為姿態(tài)角。

多時間尺度形成一個逐級反饋調(diào)度機制，視覺信息不斷更新IMU信息，利用遞推的數(shù)值方法估計狀態(tài)變量，不斷修正角度誤差，直到姿態(tài)角精度滿足長時間定姿定位的需求。

2 ?視覺姿態(tài)解算

用單目相機采集視頻，將視頻分幀，根據(jù)相鄰圖片之間的差異，可以估計載體的角度變化量。

首先，從單幅圖像中提取特征點，提取特征點采用具有保持不變性且處理效率高效的Surf算法[9];其次，匹配相鄰圖像的特征點，利用窮舉法對圖片中的每一個特征點與其相鄰圖片中的所有特征點測量描述子的距離，方差最小的點為匹配點;最后，根據(jù)匹配的像素點求解相關(guān)參數(shù)，問題的實質(zhì)為對極幾何約束[10]。

利用視覺相鄰圖片之間解算結(jié)果無累積誤差且漂移率小的優(yōu)勢修正陀螺儀的累積誤差和較大的漂移率，但引入了觀測噪聲。根據(jù)多尺度融合算法的思路，選擇視覺傳感器的長時間尺度數(shù)據(jù)作為觀測量，卡爾曼濾波融合后的最優(yōu)估計作為狀態(tài)量，根據(jù)式（8）～式（12）進行下一個過程的卡爾曼濾波融合，不斷減小觀測噪聲和累積誤差，直到精度滿足定姿定位的需求。

4 ?實驗結(jié)果與分析

采用HI219姿態(tài)傳感器進行IMU數(shù)據(jù)的采集，頻率為100 Hz。采用單目視覺模塊進行視覺數(shù)據(jù)的采集，幀速率30 Hz，圖像尺寸為1 280×720。將視覺傳感器和慣性傳感器進行安裝，由于相機坐標系和IMU坐標系在空間位置上存在安裝誤差，將它們的坐標系統(tǒng)移到物體坐標系中，以物體坐標系作為中間介質(zhì)，完成相機傳感器坐標系到慣性傳感器坐標系的轉(zhuǎn)換。本次實驗的轉(zhuǎn)換結(jié)果用T來表示：

標定了硬件設(shè)備的信息之后，進行數(shù)據(jù)采集。將固定后的器件安裝在轉(zhuǎn)臺上，由于單目相機的工作范圍有限，載體突然移動一個很大的角度，可能會產(chǎn)生果凍效應(yīng)。實驗中，采取低速的方式，使得特征點能夠提取并成功匹配。

為了驗證結(jié)果的有效性和可行性，將載體放置在任意姿態(tài)，繞轉(zhuǎn)臺的航向軸逆時針緩慢旋轉(zhuǎn)約4 min，整個過程中完整地轉(zhuǎn)了5圈，每圈之間稍有停頓。

對視頻信息進行分幀，每隔2幀選取一幅圖片，即視覺傳感器的幀速率為10 Hz，相鄰圖片進行圖像處理，解算得出地理坐標系下的角度變化量，與IMU解算的姿態(tài)角相同時間間隔（0.1 s）的角度變化量進行第一次短時間尺度濾波融合，得到第一次最優(yōu)估計，如圖2所示。整個過程中，因為航向角的波動范圍較大（（-π，π]），在圖中的偏差不是很明顯;俯仰角和橫滾角的始末偏差和運動過程中的偏差比較明顯。

進一步，每隔5幀提取一張圖片，即周期為0.2 s，將其轉(zhuǎn)移到地理坐標系下的角度變化量與第一次卡爾曼濾波后的最優(yōu)估計進行長時間尺度的數(shù)據(jù)融合。兩次濾波之后，姿態(tài)角精度有了提高，如圖3所示。

單時間尺度和多時間尺度的姿態(tài)角數(shù)據(jù)結(jié)果如表1和表2所示。

該實驗結(jié)果表明，相比于單時間尺度融合算法，多時間尺度融合算法在始末誤差方面，航向角誤差降低了42.29%，俯仰角誤差降低了49.19%，橫滾角誤差降低了72.23%;在整個過程中，航向角誤差降低了26.88%，俯仰角誤差降低了23.16%，橫滾角誤差降低了63.21%，并且航向角、俯仰角和橫滾角的精度可以保持在2°之內(nèi)。利用多時間尺度融合的視覺輔助慣性定姿算法，各個角度的始末誤差可以控制在1°以內(nèi)，整個過程中最大偏差保持在2°以內(nèi)，提高了姿態(tài)角的精度，滿足在室內(nèi)環(huán)境下長時間定姿定位的需求。

5 ?結(jié) ?語

本文針對慣性導(dǎo)航中陀螺儀解算具有角度發(fā)散和累積誤差的問題，提出一種多時間尺度融合的視覺輔助慣性定姿算法。短時間尺度消除IMU的累積誤差并保持結(jié)果的實時性，長時間尺度約束短時間尺度并減小累積觀測噪聲，長、短時間尺度相互影響，相互作用。通過實驗結(jié)果可以看出，此算法能夠有效地抑制陀螺儀的發(fā)散速度和姿態(tài)角的累積誤差，精準地校正了姿態(tài)角，實現(xiàn)了姿態(tài)角高精度的穩(wěn)定輸出。

參考文獻

[1] 王奇，黎海濤.基于機器學(xué)習(xí)與慣性導(dǎo)航的室內(nèi)定位技術(shù)研究[J].電子測量技術(shù)，2016，39（8）：138?143.

[2] SHABAYEK A E R， DEMONCEAUX C， MOREL O， et al. Vision based UAV attitude estimation： progress and insights [J]. Journal of intelligent & robotic systems， 2012， 65（1/4）： 295?308.

[3] 史俊，楊功流，陳雅潔，等.視覺輔助慣性定位定姿技術(shù)研究[J].航空計算技術(shù)，2016，46（1）：119?122.

[4] 孫長庫，徐懷遠，張寶尚，等.基于IMU與單目視覺融合的姿態(tài)測量方法[J].天津大學(xué)學(xué)報（自然科學(xué)與工程技術(shù)版），2017，50（3）：313?320.

[5] ALATISE M B， HANCKE G P. Pose estimation of a mobile robot based on fusion of IMU data and vision data using an extended Kalman filter [J]. Sensors， 2017， 17（10）： 2164.

[6] OH H， YOU S C， AN J， et al. Pose estimation of surgical instrument using sensor data fusion with optical tracker and IMU based on Kalman filter [J]. MATEC web of conferences， 2015， 32： 04008.

[7] 夏凌楠，張波，王營冠，等.基于慣性傳感器和視覺里程計的機器人定位[J].儀器儀表學(xué)報，2013，34（1）：166?172.

[8] 冀亮，錢正洪，白茹.基于四元數(shù)的四軸無人機姿態(tài)的估計和控制[J].現(xiàn)代電子技術(shù)，2015，38（11）：112?116.

[9] JINDAL A， AGGARWAL N， GUPTA S. An obstacle detection method for visually impaired persons by ground plane removal using speeded?up robust features and gray level co?occurrence matrix [J]. Pattern recognition and image analysis， 2018， 28（2）： 288?300.

[10] 詹煜欣，董文永.基于對極幾何約束的動態(tài)背景下運動目標檢測[J].計算機應(yīng)用研究，2018，35（11）：268?271.

[11] 楊承凱，曾軍，黃華.多傳感器融合中的卡爾曼濾波探討[J].現(xiàn)代電子技術(shù)，2009，32（14）：159?161.