基于改進(jìn)時(shí)間差分的視覺(jué)/慣性組合導(dǎo)航研究

王 豐，李 沅，李佳潞，侯 琪，李 皓

(1.中北大學(xué) 信息與通信工程學(xué)院，太原 030051;2.中國(guó)電子科技集團(tuán)公司第十二研究所 微波電真空器件國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京 100015)

0 引言

隨著社會(huì)的發(fā)展，導(dǎo)航越來(lái)越成為軍事應(yīng)用、多種地形目標(biāo)探測(cè)和輔助個(gè)人定位以及路徑規(guī)劃的核心。在軍事上，每個(gè)戰(zhàn)斗單位的實(shí)時(shí)定位與多種狀態(tài)信息對(duì)于戰(zhàn)場(chǎng)的指揮調(diào)度起著至關(guān)重要的作用，對(duì)于飛彈的準(zhǔn)確命中目標(biāo)也是必不可少的，在陸地單位的戰(zhàn)斗中坦克以及駕駛員往往需要較高的位置和狀態(tài)信息精度以便對(duì)戰(zhàn)略和路徑做出準(zhǔn)確的規(guī)劃。

為了獲得更高的精度，現(xiàn)代導(dǎo)航技術(shù)往往是采用多種技術(shù)如里程計(jì)、慣性導(dǎo)航、衛(wèi)星導(dǎo)航等多種模塊組成的組合導(dǎo)航，目前最主流的方法是GNSS與慣性系統(tǒng)的組合導(dǎo)航系統(tǒng)。INS是以經(jīng)典牛頓力學(xué)定律為基礎(chǔ)理論的一種可以實(shí)現(xiàn)完全自主的導(dǎo)航技術(shù)。其根據(jù)原理和有無(wú)現(xiàn)實(shí)的平臺(tái)可以分為平臺(tái)式慣性導(dǎo)航系統(tǒng)和捷聯(lián)式慣性導(dǎo)航系統(tǒng)(SINS，strap-down inertial navigation system)。平臺(tái)式慣性導(dǎo)航由三軸陀螺儀，三軸加速度計(jì)和穩(wěn)定的平臺(tái)組成，由平臺(tái)的三根穩(wěn)定軸來(lái)對(duì)一種導(dǎo)航坐標(biāo)系進(jìn)行模擬，能夠隔離載體自身的角運(yùn)動(dòng)，所以其具有精度高的優(yōu)點(diǎn)。但是同時(shí)也由于有平臺(tái)的存在導(dǎo)致其結(jié)構(gòu)比較復(fù)雜，體積較大，制作成本也非常之高，不適用于普通車(chē)載。SINS使用虛擬的數(shù)學(xué)平臺(tái)代替了真正的平臺(tái)，使導(dǎo)航系統(tǒng)體積大幅度縮小，集成度大幅提升，成本也大幅度降低，逐漸應(yīng)用在民用領(lǐng)域，如汽車(chē)導(dǎo)航，無(wú)人機(jī)探測(cè)技術(shù)，水下探測(cè)等領(lǐng)域，但同時(shí)也由于其虛擬的數(shù)學(xué)平臺(tái)的引入也導(dǎo)致了很多計(jì)算上的問(wèn)題，使得算法更加復(fù)雜化，SINS的解算需要用到計(jì)算機(jī)對(duì)加速度計(jì)測(cè)得的飛行器加速度信號(hào)和陀螺儀角速度數(shù)據(jù)以及當(dāng)前的姿態(tài)信息進(jìn)行坐標(biāo)變換，然后再進(jìn)行INS的計(jì)算得出需要的導(dǎo)航狀態(tài)信息參數(shù)。INS的本質(zhì)是測(cè)量加速度與角速度并進(jìn)行積分運(yùn)算，其不可避免的問(wèn)題就是誤差隨時(shí)間的累積導(dǎo)致長(zhǎng)時(shí)間計(jì)算所得AVP信息的不準(zhǔn)確不可信[1]。為了克服慣性導(dǎo)導(dǎo)航長(zhǎng)航時(shí)誤差過(guò)大的問(wèn)題，可以采用慣性與多傳感器組合的方法來(lái)減少誤差以便于慣性設(shè)備可以更長(zhǎng)時(shí)間的使用。

GNSS雖具有精度高、實(shí)時(shí)性好的優(yōu)點(diǎn)[2]，且GNSS/慣性組合導(dǎo)航也取得了一定的成果，但是在很多情況下幾乎無(wú)法使用如軍事作戰(zhàn)、建筑物遮擋與水下、礦洞等[3]，目前國(guó)際形勢(shì)動(dòng)蕩，迫切需要研究在衛(wèi)星信號(hào)不可用情況下的導(dǎo)航與定位。為了實(shí)現(xiàn)對(duì)慣性誤差的補(bǔ)償，且視覺(jué)傳感器的低成本和便捷性，越來(lái)越多的傳感器被使用在導(dǎo)航技術(shù)之中，從仿生學(xué)導(dǎo)航來(lái)說(shuō)，動(dòng)物與人類(lèi)都可以通過(guò)視覺(jué)和運(yùn)動(dòng)的感知來(lái)實(shí)現(xiàn)對(duì)自身的定位導(dǎo)航[4-7]，2014年Martinelli提出了慣性測(cè)量單元(IMU，inertial measurement unit)偏置的初始化方法[8]，2017年Scaramuzza等人發(fā)現(xiàn)對(duì)慣性導(dǎo)航定位和狀態(tài)信息誤差影響最大的是陀螺儀的偏置，而加速度計(jì)的偏置影響相對(duì)較小[9]，2021年北京郵電大學(xué)趙鴻儒、喬秀全等人設(shè)計(jì)了基于神經(jīng)網(wǎng)絡(luò)的視覺(jué)慣性里程計(jì)提高了載體平移和旋轉(zhuǎn)角度的準(zhǔn)確性[10]，2022年火箭軍工程大學(xué)的薛海建等人提出了一種里程計(jì)輔助慣性導(dǎo)航設(shè)備定位與狀態(tài)估計(jì)的方法減小了慣性積分誤差[11]。本文基于載體運(yùn)動(dòng)特點(diǎn)提出一種可靠性較高(判斷零速的準(zhǔn)確程度遠(yuǎn)高于視覺(jué)實(shí)時(shí)速度解算)，運(yùn)算簡(jiǎn)單的視覺(jué)輔助慣性導(dǎo)航進(jìn)行速度校準(zhǔn)抑制誤差累積的方法,從而有效減少I(mǎi)NS積分的誤差累積，便于實(shí)現(xiàn)更高精度和更長(zhǎng)時(shí)間的導(dǎo)航定位和多狀態(tài)信息的估計(jì)[12-16]，可以適用于低精度、低光照和視覺(jué)傳感器噪聲較大的場(chǎng)景，有效的輔助了慣性導(dǎo)航。

1 慣性導(dǎo)航系統(tǒng)

1.1 常用坐標(biāo)系

1)地心慣性坐標(biāo)系簡(jiǎn)稱i系，坐標(biāo)軸相對(duì)恒星不變且與地球的自轉(zhuǎn)無(wú)關(guān)，以地球之心O為原點(diǎn)，其Z軸與地球自轉(zhuǎn)軸重合并以指向北極為正方向，它的X軸指向平均春分點(diǎn)，且指向春分點(diǎn)為正方向，Y軸則是倍定為與X，Z兩軸同時(shí)垂直且符合右手直角坐標(biāo)系。

2)地心地固坐標(biāo)系簡(jiǎn)稱e系，也叫地球坐標(biāo)系，它以地球質(zhì)心O為坐標(biāo)原點(diǎn)，隨地球自轉(zhuǎn)而改變，相對(duì)于i系旋轉(zhuǎn)角速度為15°/h，Z軸與地球自轉(zhuǎn)軸重合且正方向指向地球北極，X，Y軸在赤道平面內(nèi)，X軸指向本初子午線，符合右手坐標(biāo)系。

3)地理坐標(biāo)系簡(jiǎn)稱g系，隨著載體位置的變化而變化，相對(duì)載體靜止。一般地坐標(biāo)系有兩周表示，即東北天坐標(biāo)系和北東地坐標(biāo)系，它們以載體重心為原點(diǎn)O，X軸，Y軸指向當(dāng)?shù)氐臇|向和北向，在g系中采用經(jīng)度、緯度和高度來(lái)描述載體當(dāng)前的位置信息。

4)導(dǎo)航坐標(biāo)系簡(jiǎn)稱n系，一般采用當(dāng)?shù)氐牡乩碜鴺?biāo)系作為導(dǎo)航系。

5)載體坐標(biāo)系簡(jiǎn)稱b系，主要表現(xiàn)載體的運(yùn)動(dòng)方向、運(yùn)動(dòng)速度和當(dāng)前位置等信息，主要載體系有前右下坐標(biāo)系和右前上坐標(biāo)系(即載體的X，Y，Z軸正方向分別對(duì)應(yīng)前右下和右前上)分別對(duì)應(yīng)地理坐標(biāo)系的北東地和東北天。

1.2 坐標(biāo)系轉(zhuǎn)換

INS載體的姿態(tài)信息、速度信息與位置信息的計(jì)算需要將載體系輸出信息轉(zhuǎn)換到導(dǎo)航系坐標(biāo)，載體坐標(biāo)系通過(guò)虛擬的數(shù)學(xué)平臺(tái)轉(zhuǎn)換到當(dāng)前的導(dǎo)航坐標(biāo)系。通過(guò)載體當(dāng)前的姿態(tài)信息即航向角ψ、俯仰角θ、橫滾角γ，導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的轉(zhuǎn)換可以分成3個(gè)方向的旋轉(zhuǎn)，整體的轉(zhuǎn)換可以通過(guò)一個(gè)旋轉(zhuǎn)姿態(tài)矩陣來(lái)表示(如圖1所示)，具體姿態(tài)矩陣的計(jì)算方法如式(1)所示。

圖1 坐標(biāo)系的旋轉(zhuǎn)

(1)

1.3 INS機(jī)械編排

在慣性導(dǎo)航設(shè)備工作之前，INS需要先對(duì)載體狀態(tài)信息初始化，初始化的準(zhǔn)確度會(huì)影響后續(xù)的解算誤差和精度。初始化主要是指的是對(duì)初始位置、初始速度和初始姿態(tài)進(jìn)行初始化對(duì)準(zhǔn)，慣性導(dǎo)航傳感器本身只有初始化初始姿態(tài)的能力，而初始速度和初始位置需要GPS給定或者人工設(shè)置。初始化之后通過(guò)更新算法和IMU加速度計(jì)輸出的加速度數(shù)據(jù)和陀螺儀輸出的角速度數(shù)據(jù)進(jìn)行實(shí)時(shí)解算與當(dāng)前狀態(tài)信息更新，狀態(tài)信息主要包括姿態(tài)信息、速度信息和位置信息，由于不可避免的傳感器噪聲、精度和力學(xué)編排算法具有一定誤差的問(wèn)題，慣性導(dǎo)航的誤差會(huì)在每一次積分的時(shí)候逐漸累積，需要每隔一段時(shí)間使用無(wú)誤差累積的外部觀測(cè)手段對(duì)其進(jìn)行誤差校準(zhǔn)，當(dāng)前最主流的是慣性和GNSS的組合使用，但是在特定場(chǎng)景下GNSS因?yàn)檎趽鹾推帘螣o(wú)法使用，我們必須使用他傳感器的輔助導(dǎo)航，增強(qiáng)系統(tǒng)魯棒性，本文采用了視覺(jué)傳感器與慣性傳感器的組合方法。INS的牛頓力學(xué)編排原理的基本實(shí)現(xiàn)如圖2所示。

圖2 慣性導(dǎo)航力學(xué)編排原理

2 差分視覺(jué)零速校準(zhǔn)

視覺(jué)傳感器是所有機(jī)器視覺(jué)信息的源頭，其主要是指利用一些光學(xué)感應(yīng)器件和成像結(jié)構(gòu)的組合來(lái)獲取周?chē)h(huán)境的光學(xué)信息的儀器，目前視覺(jué)傳感器的種類(lèi)和應(yīng)用日益增加，最常用的有RGB、RGB-D相機(jī)、雷達(dá)成像和紅外成像等，最普通的RGB相機(jī)已經(jīng)廣泛的進(jìn)入到人民的日常生活中[10,17-18]，個(gè)人電腦，個(gè)人手機(jī)以及行車(chē)記錄儀都有著視覺(jué)傳感器的身影。慣性導(dǎo)航器件的加速度計(jì)與陀螺儀積分運(yùn)算造成的三向速度不準(zhǔn)確以及長(zhǎng)時(shí)間誤差累積較大的缺點(diǎn)，普通視覺(jué)里程計(jì)需要較高精度和良好的環(huán)境才能作為算法正確發(fā)揮作用的保障，通常視覺(jué)傳感器會(huì)受到目標(biāo)物體結(jié)構(gòu)相似、紋理重復(fù)以及光照不理想的影響導(dǎo)致的誤差較大，不利于準(zhǔn)確高精度的輔助慣性系統(tǒng)導(dǎo)航[19]。針對(duì)上述的問(wèn)題，本文利用軍事作戰(zhàn)單位或者行駛汽車(chē)經(jīng)常駐停的特點(diǎn)提出了使用視覺(jué)傳感器數(shù)據(jù)獲取當(dāng)前速度狀態(tài)并對(duì)載體當(dāng)前速度進(jìn)行校準(zhǔn)，純慣性導(dǎo)航的核心誤差主要有九個(gè)，分別為位置誤差3個(gè)方向的分量、速度誤差的3個(gè)方向的分量和姿態(tài)誤差的3個(gè)分量，通過(guò)速度的校準(zhǔn)可以有效的減少速度誤差和位置誤差。

2.1 RGB相機(jī)載體靜止判斷

視覺(jué)傳感器記錄了相機(jī)與世界的相對(duì)運(yùn)動(dòng)，既可以用于檢測(cè)相機(jī)中運(yùn)動(dòng)目標(biāo)的檢測(cè)，也可以用來(lái)推算相機(jī)的運(yùn)動(dòng)與否，對(duì)傳統(tǒng)的慣性導(dǎo)航起到了良好的輔助作用。目前，視覺(jué)傳感器運(yùn)動(dòng)判斷最主流的方法通常有3種，分別為光流法，連續(xù)幀時(shí)間差分法和背景差分法。

1)光流法指的是在連續(xù)的兩幀圖像中由于圖像中的視覺(jué)傳感器自身和視覺(jué)場(chǎng)景中物體相對(duì)運(yùn)動(dòng)導(dǎo)致傳感器圖像中目標(biāo)像素的改變。光流法使用的前提是假定相鄰兩幀之間具有灰度不變性和相鄰像素間相似運(yùn)動(dòng)，它的理想輸出應(yīng)該是兩幀圖像中每個(gè)像素的運(yùn)動(dòng)情況(如圖3所示)，并且可以根據(jù)算法使用的圖像目標(biāo)像素范圍將光流法分成稀疏光流法和稠密光流法。

圖3 光流法表示的物體連續(xù)運(yùn)動(dòng)

由于光流法實(shí)現(xiàn)種類(lèi)較多且計(jì)算量較大(最簡(jiǎn)單的稀疏光流法也需要特征點(diǎn)的計(jì)算與判斷)，不符合本文要求的計(jì)算簡(jiǎn)單。

2)背景差分法通過(guò)背景圖像與傳感器輸入信息進(jìn)行比較從而分辨出運(yùn)動(dòng)的物體，它要求運(yùn)動(dòng)物體的像素值與背景的像素值存在較大的差別，需要能夠使計(jì)算機(jī)輕易的區(qū)分出運(yùn)動(dòng)物體和背景，通過(guò)簡(jiǎn)單的相減便可以得出運(yùn)動(dòng)物體的輪廓、形狀和大小等信息(如圖4所示)，這種方法比較適合攝像機(jī)靜止的場(chǎng)景，不適用運(yùn)動(dòng)中的攝像機(jī)，因此不適用本文的研究。

圖4 背景差分法計(jì)算原理

3)連續(xù)幀時(shí)間差分法也叫做幀間差分法是一種通過(guò)對(duì)視頻相鄰的連續(xù)幀做差分運(yùn)算獲取運(yùn)動(dòng)目標(biāo)大小和形狀的方法。它根據(jù)靜止視覺(jué)傳感器連續(xù)幀變化非常微弱的特點(diǎn)來(lái)設(shè)計(jì)的，如果相機(jī)靜止，那么前后兩幀的像素值變化微弱，對(duì)這兩幀的像素值進(jìn)行運(yùn)算，得到的應(yīng)該是一副純黑色的圖像，由于連續(xù)幀時(shí)間差分法對(duì)光線等場(chǎng)景變化不敏感，能適應(yīng)多種不同的復(fù)雜環(huán)境，魯棒性較高并且計(jì)算量最小，原理簡(jiǎn)單，能夠最準(zhǔn)確、最快速的判斷出載體的運(yùn)動(dòng)與靜止?fàn)顟B(tài)，所以本文采用此種方法并對(duì)其進(jìn)行適當(dāng)?shù)乃惴ǜ倪M(jìn)以便于外界信息的觀測(cè)與使用。

2.2 改進(jìn)連續(xù)幀時(shí)間差分法

相機(jī)連續(xù)兩幀時(shí)間差分是一種較為常用的的相機(jī)靜止判定方法，只需要選取視覺(jué)傳感器輸入的時(shí)間上的前后兩幀進(jìn)行運(yùn)算即可得到相機(jī)的運(yùn)動(dòng)靜止信息，其可以用于檢測(cè)運(yùn)動(dòng)物體，對(duì)與動(dòng)態(tài)的物體有很強(qiáng)的自適應(yīng)性[20]，也可以用于檢測(cè)相機(jī)本身的運(yùn)動(dòng)，相比傳統(tǒng)視覺(jué)里程計(jì)計(jì)算更為簡(jiǎn)單且零速誤差極小(連續(xù)幀差分法的像素級(jí)運(yùn)算)，只需要檢測(cè)的光源信息具有一定區(qū)分度即可，無(wú)需較好的光源和高精度的傳感器，具有更大的速度參考價(jià)值，主要流程如圖5所示。

圖5 靜止判斷流程圖

對(duì)于某些視覺(jué)傳感器分辨率較高，得到的是很多分辨率較高的圖像，數(shù)據(jù)圖像的長(zhǎng)度和寬度過(guò)大導(dǎo)致的計(jì)算量巨大且運(yùn)算速度慢，因?yàn)槭褂眠B續(xù)幀時(shí)間差分的方法運(yùn)算數(shù)據(jù)量較大，為了減少像素的差值運(yùn)算量，由于本文研究不需要獲取運(yùn)動(dòng)目標(biāo)的清晰輪廓，所以可以通過(guò)采用間隔取點(diǎn)運(yùn)算的方法進(jìn)行差分運(yùn)算，這種方法基本不會(huì)影響其判定運(yùn)算的結(jié)果，但是卻可以節(jié)約大量的運(yùn)算成本。例如，對(duì)于一幅1 000*1 000的高分辨率RGB圖像，時(shí)間差分計(jì)算至少需要3*1 000*1 000次運(yùn)算，而采用每隔10個(gè)點(diǎn)進(jìn)行采樣計(jì)算的情況下計(jì)算量為3*100*100，運(yùn)算量足足減少100倍，對(duì)于某些幀率過(guò)高的視覺(jué)傳感器輸入，可以采取關(guān)鍵幀的策略，即每間隔一定的時(shí)間或幀數(shù)來(lái)選取需要解算的幀作為關(guān)鍵幀，也可以大幅減少運(yùn)算耗時(shí)，具體間隔時(shí)間和幀數(shù)應(yīng)當(dāng)根據(jù)具體的要求來(lái)確定，以便在獲取足夠高的精度的同時(shí)減少計(jì)算量。圖6為每5個(gè)像素選取一個(gè)關(guān)鍵像素點(diǎn)信息進(jìn)行差分計(jì)算的原理示意圖。

圖6 間隔采用示意圖

圖7 采樣像素點(diǎn)做差運(yùn)算

(2)

(3)

(4)

(5)

T為采樣點(diǎn)位置像素不變的指標(biāo)，范圍為[0，1] ，我們可以根據(jù)需求設(shè)定一個(gè)合適閾值來(lái)屏蔽圖像中運(yùn)動(dòng)物體造成的影響，當(dāng)T大于所設(shè)閾值時(shí)可以定為載體靜止，如果閾值設(shè)置太小，無(wú)法抑制差分圖像中的噪聲，設(shè)置過(guò)大則會(huì)導(dǎo)致運(yùn)動(dòng)信息的誤判。設(shè)定閾值的優(yōu)點(diǎn)在于對(duì)運(yùn)動(dòng)目標(biāo)干擾有較強(qiáng)的魯棒性，T值越接近1，則對(duì)載體的靜止置信度越高，由于使用時(shí)間差分法對(duì)載體的靜止進(jìn)行判斷，誤差級(jí)別屬于像素級(jí)，判斷結(jié)果有較高的可靠性和可信度。

3 擴(kuò)展卡爾曼濾波信息融合

多傳感器信息融合相關(guān)的研究開(kāi)始于20世紀(jì)的80年代。多傳感器融合的常用方法有:加權(quán)平均法、統(tǒng)計(jì)決策、卡爾曼濾波和神經(jīng)網(wǎng)絡(luò)等方法。加權(quán)平均的方法雖然是最簡(jiǎn)單、最直接的辦法，但是它的理論結(jié)果不符合統(tǒng)計(jì)學(xué)的最優(yōu)估計(jì)理論且只適用于低動(dòng)態(tài)水平的數(shù)據(jù)處理;統(tǒng)計(jì)決策理論多用在多個(gè)傳感器融合相同種類(lèi)的數(shù)據(jù)，一般只用于圖像的觀測(cè)數(shù)據(jù)信息;對(duì)于系統(tǒng)狀態(tài)的噪聲和觀測(cè)值的噪聲為高斯白噪聲或者近似相當(dāng)于高斯白噪聲的線性系統(tǒng)模型可以采用卡爾曼濾波的方法來(lái)融合動(dòng)態(tài)冗余的多傳感信息，對(duì)于非線性系統(tǒng)模型采用擴(kuò)展卡爾曼濾波或者無(wú)跡卡爾曼濾波，擴(kuò)展卡爾曼濾波通過(guò)非線性當(dāng)前時(shí)刻值的一階泰勒展開(kāi)式來(lái)表示其線性狀態(tài)以此將其線性化以便于符合卡爾曼濾波的基本條件，而無(wú)跡卡爾曼濾波則是通過(guò)對(duì)狀態(tài)輸入的選值運(yùn)算來(lái)求取函數(shù)的均值和協(xié)方差矩陣，這樣做可以避免擴(kuò)展卡爾曼濾波泰勒展開(kāi)計(jì)算雅可比矩陣運(yùn)算量巨大的問(wèn)題;而基于神經(jīng)網(wǎng)絡(luò)的方法根據(jù)系統(tǒng)模型和應(yīng)求的解，通過(guò)一定的神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)對(duì)和損失函數(shù)對(duì)結(jié)果進(jìn)行評(píng)價(jià)并進(jìn)行非線性優(yōu)化來(lái)學(xué)習(xí)確定各個(gè)網(wǎng)絡(luò)連接的權(quán)值，對(duì)各個(gè)傳感器的輸入信息進(jìn)行融合，由于神經(jīng)網(wǎng)絡(luò)內(nèi)部的黑盒特性，通常無(wú)法對(duì)其進(jìn)行精確的解釋?zhuān)液苋菀紫萑刖植孔顑?yōu)化的問(wèn)題，目前也是一個(gè)研究的熱門(mén)方向之一。

卡爾曼濾波是一種通過(guò)利用線性系統(tǒng)狀態(tài)方程，并通過(guò)系統(tǒng)輸入觀測(cè)實(shí)時(shí)數(shù)據(jù)，以此對(duì)下一時(shí)刻系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。由于其觀測(cè)數(shù)據(jù)中包括了線性系統(tǒng)中符合高斯模型噪聲和干擾的影響，所以最優(yōu)估計(jì)的過(guò)程也可被看作是一種濾波過(guò)程，也可以稱為卡爾曼濾波器。它最初用于阿波羅計(jì)劃的導(dǎo)航電腦，是隨機(jī)系統(tǒng)實(shí)時(shí)最優(yōu)估計(jì)估計(jì)問(wèn)題的唯一實(shí)際的有限維解決方法，它設(shè)計(jì)的概率分布只需要做出很少的假設(shè)，只需要狀態(tài)方程含有有限的均值和協(xié)方差。目前卡爾曼濾波應(yīng)用的主要目的一般分為兩種，即估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)信息和估計(jì)系統(tǒng)的性能分析。

(6)

Zk=HkXk+Vk

(7)

(8)

(9)

(10)

(11)

(12)

Pk=(I+KkHk)Pk/k-1

(13)

我們需要提前確定并輸入P，Q，R矩陣，初始值的設(shè)置應(yīng)該是初始狀態(tài)信息、慣性系統(tǒng)的噪聲和量測(cè)方程的噪聲方差陣。具體原理和流程如圖8所示。

圖8 卡爾曼濾波估計(jì)流程圖

4 仿真實(shí)驗(yàn)與分析

4.1 仿真實(shí)驗(yàn)設(shè)置

圖9 視覺(jué)/SINS組合導(dǎo)航系統(tǒng)結(jié)構(gòu)

4.2 仿真結(jié)果與分析

仿真數(shù)據(jù)在載體運(yùn)動(dòng)期間一共設(shè)置了三次短時(shí)間駐停用以給視覺(jué)傳感器做駐停判斷并以擴(kuò)展卡爾曼濾波融合的方式校準(zhǔn)慣性導(dǎo)航推算的載體三向速度。仿真實(shí)驗(yàn)設(shè)置的真實(shí)的AVP信息如圖10～13所示。

圖10 載體三向速度

圖11 載體俯仰角和橫滾角

圖12 載體航向角

4.2.1 行駛軌跡對(duì)比分析

圖14 載體行駛軌跡

圖13 載體經(jīng)緯高變化

經(jīng)過(guò)載體零速校準(zhǔn)信息進(jìn)行校準(zhǔn)后，多種導(dǎo)航方法的軌跡對(duì)比如圖14所示，標(biāo)準(zhǔn)軌跡為仿真的GPS位置數(shù)據(jù)添加3 m的高斯位置誤差，與純慣導(dǎo)相比載體運(yùn)行軌跡更加接近真實(shí)的軌跡，證明運(yùn)行時(shí)間越長(zhǎng)與純慣性導(dǎo)航的軌跡相差越大，有效的抑制了慣性導(dǎo)航定位誤差隨時(shí)間的發(fā)散，證明本文方法的有效性。其位置的東向和北向位置誤差在全局上比純慣性導(dǎo)航的表現(xiàn)更加接近優(yōu)秀，如圖15～16所示。

4.2.2 RMSE誤差分析

均方根誤差(RMSE,root mean square error)是導(dǎo)航研究領(lǐng)域的一種比較常用的誤差評(píng)價(jià)標(biāo)準(zhǔn)，RMSE可以反映出測(cè)量數(shù)據(jù)偏離真實(shí)值的程度，數(shù)值越小表示測(cè)量值越接近真實(shí)值，測(cè)量精度越高。本文以位置的RMSE作為其北向和東向位置誤差的評(píng)價(jià)標(biāo)準(zhǔn)，其計(jì)算結(jié)果如表1所示。顯然，本文的方法從誤差結(jié)果上要更加優(yōu)于純慣性導(dǎo)航，RMSE在北向位置上減少了19.0%，在東向誤差上減少了32.1%，證明了視覺(jué)對(duì)慣性導(dǎo)航校準(zhǔn)和抑制誤差發(fā)散的有效性。

(14)

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

圖15 東向位置誤差

圖16 北向位置誤差

由于慣性導(dǎo)航的姿態(tài)，速度和位置誤差隨時(shí)間發(fā)散，需要每隔一定的時(shí)間對(duì)其進(jìn)行校準(zhǔn)，本文根據(jù)汽車(chē)和一些軍事作戰(zhàn)載體具有經(jīng)常駐停的特性使用視覺(jué)進(jìn)行判斷速度并校準(zhǔn)，本文方法對(duì)視覺(jué)傳感器要求低(無(wú)需高質(zhì)量圖像和良好的光源環(huán)境)，魯棒性高，可以更加穩(wěn)定高精度的輔助慣性導(dǎo)航設(shè)備，計(jì)算簡(jiǎn)單快捷能夠滿足實(shí)時(shí)解算，并且可以有效抑制慣性導(dǎo)航的誤差發(fā)散，使其定位與速度狀態(tài)信息更加具有參考性，提升了在特定條件下GNSS信號(hào)不可用的慣性導(dǎo)航性能，是一種有效的導(dǎo)航誤差解決方案。慣性導(dǎo)航的姿態(tài)誤差是一種影響較大的誤差，未來(lái)研究應(yīng)加入位置校準(zhǔn)和偏振光航向角對(duì)姿態(tài)進(jìn)行校準(zhǔn)，提升慣性導(dǎo)航在長(zhǎng)時(shí)間上使用的性能。