一種多時(shí)間尺度融合的視覺(jué)輔助慣性定姿算法

張琳 曾成 王羿帆

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

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

中圖分類號(hào)： TN820.2+2?34; TP212.9 ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼： A ? ? ? ? ? ? ? ? ? ?文章編號(hào)： 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 ?引 ?言

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

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

1 ?多時(shí)間尺度融合算法

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

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

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

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

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

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

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

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

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

4 ?實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

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

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

5 ?結(jié) ?語(yǔ)

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

參考文獻(xiàn)

[1] 王奇，黎海濤.基于機(jī)器學(xué)習(xí)與慣性導(dǎo)航的室內(nèi)定位技術(shù)研究[J].電子測(cè)量技術(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] 史俊，楊功流，陳雅潔，等.視覺(jué)輔助慣性定位定姿技術(shù)研究[J].航空計(jì)算技術(shù)，2016，46（1）：119?122.

[4] 孫長(zhǎng)庫(kù)，徐懷遠(yuǎn)，張寶尚，等.基于IMU與單目視覺(jué)融合的姿態(tài)測(cè)量方法[J].天津大學(xué)學(xué)報(bào)（自然科學(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] 夏凌楠，張波，王營(yíng)冠，等.基于慣性傳感器和視覺(jué)里程計(jì)的機(jī)器人定位[J].儀器儀表學(xué)報(bào)，2013，34（1）：166?172.

[8] 冀亮，錢正洪，白茹.基于四元數(shù)的四軸無(wú)人機(jī)姿態(tài)的估計(jì)和控制[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] 詹煜欣，董文永.基于對(duì)極幾何約束的動(dòng)態(tài)背景下運(yùn)動(dòng)目標(biāo)檢測(cè)[J].計(jì)算機(jī)應(yīng)用研究，2018，35（11）：268?271.

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