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

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

0 引言

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

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

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

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

1.1 常用坐標系

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

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

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

4)導(dǎo)航坐標系簡稱n系，一般采用當?shù)氐牡乩碜鴺讼底鳛閷?dǎo)航系。

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

1.2 坐標系轉(zhuǎn)換

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

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

(1)

1.3 INS機械編排

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

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

2 差分視覺零速校準

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

2.1 RGB相機載體靜止判斷

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

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

圖3 光流法表示的物體連續(xù)運動

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

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

圖4 背景差分法計算原理

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

2.2 改進連續(xù)幀時間差分法

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

圖5 靜止判斷流程圖

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

圖6 間隔采用示意圖

圖7 采樣像素點做差運算

(2)

(3)

(4)

(5)

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

3 擴展卡爾曼濾波信息融合

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

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

圖8 卡爾曼濾波估計流程圖

4 仿真實驗與分析

4.1 仿真實驗設(shè)置

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

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

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

圖10 載體三向速度

圖11 載體俯仰角和橫滾角

圖12 載體航向角

4.2.1 行駛軌跡對比分析

圖14 載體行駛軌跡

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

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

4.2.2 RMSE誤差分析

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

(14)

5 結(jié)束語

圖15 東向位置誤差

圖16 北向位置誤差

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