一種多時間尺度融合的視覺輔助慣性定姿算法

張琳 曾成 王羿帆

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

關鍵詞： 姿態解算; 多時間尺度融合; 慣性測量單元; 數據采集; 姿態角校正; 角度誤差更新

中圖分類號： 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 ?引 ?言

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

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

1 ?多時間尺度融合算法

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

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

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

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

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

2 ?視覺姿態解算

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

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

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

4 ?實驗結果與分析

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

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

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

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

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

單時間尺度和多時間尺度的姿態角數據結果如表1和表2所示。

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

5 ?結 ?語

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

參考文獻

[1] 王奇，黎海濤.基于機器學習與慣性導航的室內定位技術研究[J].電子測量技術，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] 史俊，楊功流，陳雅潔，等.視覺輔助慣性定位定姿技術研究[J].航空計算技術，2016，46（1）：119?122.

[4] 孫長庫，徐懷遠，張寶尚，等.基于IMU與單目視覺融合的姿態測量方法[J].天津大學學報（自然科學與工程技術版），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].儀器儀表學報，2013，34（1）：166?172.

[8] 冀亮，錢正洪，白茹.基于四元數的四軸無人機姿態的估計和控制[J].現代電子技術，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] 詹煜欣，董文永.基于對極幾何約束的動態背景下運動目標檢測[J].計算機應用研究，2018，35（11）：268?271.

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