基于改進時間差分的視覺/慣性組合導航研究

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

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

0 引言

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

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

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

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

1.1 常用坐標系

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

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

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

4)導航坐標系簡稱n系，一般采用當地的地理坐標系作為導航系。

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

1.2 坐標系轉換

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

圖1 坐標系的旋轉

(1)

1.3 INS機械編排

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

圖2 慣性導航力學編排原理

2 差分視覺零速校準

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

2.1 RGB相機載體靜止判斷

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

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

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

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

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

圖4 背景差分法計算原理

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

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

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

圖5 靜止判斷流程圖

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

圖6 間隔采用示意圖

圖7 采樣像素點做差運算

(2)

(3)

(4)

(5)

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

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

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

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

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

4 仿真實驗與分析

4.1 仿真實驗設置

圖9 視覺/SINS組合導航系統(tǒng)結構

4.2 仿真結果與分析

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

圖10 載體三向速度

圖11 載體俯仰角和橫滾角

圖12 載體航向角

4.2.1 行駛軌跡對比分析

圖14 載體行駛軌跡

圖13 載體經緯高變化

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

4.2.2 RMSE誤差分析

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

(14)

5 結束語

圖15 東向位置誤差

圖16 北向位置誤差

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