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

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

0慣性系統(tǒng)中的融合算法在微機(jī)械系統(tǒng)中，基于傳感器的柔性測(cè)量單元（imu）被廣泛應(yīng)用于動(dòng)態(tài)跟蹤技術(shù)，尤其是衛(wèi)星信號(hào)不能穿過(guò)建筑物的環(huán)境。針對(duì)視覺(jué)傳感器和慣性傳感器的信息融合問(wèn)題，已經(jīng)出現(xiàn)了眾多的融合算法。比如：使用次優(yōu)H1次最優(yōu)估計(jì)值為姿態(tài)角多時(shí)間尺度融合算法的具體實(shí)現(xiàn)方法如圖1所示。首先，對(duì)視覺(jué)傳感器采集的視頻進(jìn)行分幀，每隔a幀（a為自然數(shù)）提取一次圖片，所提取的相鄰兩圖之間的時(shí)間間隔為t其次，每隔b幀（b>a,b為自然數(shù)）提取圖片，相鄰兩圖之間的時(shí)間間隔為t若第二次最優(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提取本質(zhì)矩陣用單目相機(jī)采集視頻，將視頻分幀，根據(jù)相鄰圖片之間的差異，可以估計(jì)載體的角度變化量。首先，從單幅圖像中提取特征點(diǎn)，提取特征點(diǎn)采用具有保持不變性且處理效率高效的Surf算法由相機(jī)成像原理的針孔相機(jī)模型得：式中：pp根據(jù)歸一化后的匹配點(diǎn)位置，有：式中，E為本質(zhì)矩陣。本質(zhì)矩陣E包含了相機(jī)運(yùn)動(dòng)的平移矢量信息t和旋轉(zhuǎn)矩陣R:本質(zhì)矩陣E為3×3矩陣，由矩陣運(yùn)算關(guān)系和尺度等價(jià)性可知，只需5對(duì)匹配點(diǎn)的歸一化像素位置即可求解出本質(zhì)矩陣。如果圖片中匹配點(diǎn)點(diǎn)數(shù)大于5，充分利用這些點(diǎn)，采用隨機(jī)采樣一致性（RandomSampleConsensus,RANSAC）算法，避免錯(cuò)誤數(shù)據(jù)對(duì)整體結(jié)果的影響。由奇異值分解本質(zhì)矩陣得到旋轉(zhuǎn)矩陣R，將其轉(zhuǎn)化為比較直觀的歐拉角。令繞Z,X,Y軸旋轉(zhuǎn)的角度分別為航向角ψ、俯仰角θ、橫滾角γ，根據(jù)旋轉(zhuǎn)矩陣，求解姿態(tài)角。由視覺(jué)估計(jì)載體姿態(tài)，利用連續(xù)的圖像序列進(jìn)行特征匹配得到匹配點(diǎn)，根據(jù)匹配點(diǎn)的坐標(biāo)估計(jì)圖像之間的角度變化量，進(jìn)而得出導(dǎo)航坐標(biāo)系下的角度變化量。3誤差估計(jì)和噪聲修正卡爾曼濾波采用均方誤差最小準(zhǔn)則式中：Δψ，確定了狀態(tài)方程和量測(cè)方程之后，卡爾曼濾波的過(guò)程如下：預(yù)測(cè)狀態(tài)：預(yù)測(cè)協(xié)方差矩陣：計(jì)算卡爾曼增益矩陣（權(quán)重）：更新?tīng)顟B(tài)（k時(shí)刻的最優(yōu)解）：更新協(xié)方差矩陣（k時(shí)刻的狀態(tài)變量的估計(jì)誤差均方值最?。豪靡曈X(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視覺(jué)姿態(tài)精度實(shí)驗(yàn)采用HI219姿態(tài)傳感器進(jìn)行IMU數(shù)據(jù)的采集，頻率為100Hz。采用單目視覺(jué)模塊進(jìn)行視覺(jué)數(shù)據(jù)的采集，幀速率30Hz，圖像尺寸為1280×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)表示：利用Matlab的CameraCalibrator工具箱標(biāo)定相機(jī)的內(nèi)參矩陣。拍攝不同方向、不同距離、規(guī)格為9×12的棋盤(pán)格16張，得到相機(jī)的內(nèi)參矩陣K:結(jié)果顯示，內(nèi)參矩陣的平均重投影誤差是0.07像素，滿足求解視覺(jué)姿態(tà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)約4min，整個(gè)過(guò)程中完整地轉(zhuǎn)了5圈，每圈之間稍有停頓。對(duì)視頻信息進(jìn)行分幀，每隔2幀選取一幅圖片，即視覺(jué)傳感器的幀速率為10Hz，相鄰圖片進(jìn)行圖像處理，解算得出地理坐標(biāo)系下的角度變化量，與IMU解算的姿態(tài)角相同時(shí)間間隔（0.1s）的角度變化量進(jìn)行第一次短時(shí)間尺度濾波融合，得到第一次最優(yōu)估計(jì)，如圖2所示。整個(gè)過(guò)程中，因?yàn)楹较蚪堑牟▌?dòng)范圍較大（（-π，π]），在圖中的偏差不是很明顯；俯仰角和橫滾角的始末偏差和運(yùn)動(dòng)過(guò)程中的偏差比較明顯。進(jìn)一步，每隔5幀提取一張圖片，即周期為0.2s，將其轉(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慣性定姿算法本文針對(duì)慣性導(dǎo)航中陀螺儀解算具有角度發(fā)散和累積誤差的問(wèn)題，提出一種多時(shí)間尺度融合的視覺(jué)輔助慣性定姿算法。短時(shí)間尺度消除IMU