潘新宇，趙英策，李建勛

(1． 上海交通大學自動化系，上海 200240；2． 中國航空工業(yè)沈陽飛機設計研究所，遼寧 沈陽 110035)

要實現(xiàn)無人駕駛，如何高精度地進行導航是不可避免的一個問題。常用的導航系統(tǒng)如全球衛(wèi)星導航系統(tǒng)，慣性導航系統(tǒng)，激光雷達等，各自都具有一些優(yōu)缺點。慣性導航系統(tǒng)(INS)具有短期精度高，連續(xù)工作的特點，但隨著慣性傳感器的誤差的累積，長期導航的誤差會無限增長。而全球導航衛(wèi)星(GNSS)導航系統(tǒng)，可以提供良好的長期高精度導航結果，但GNSS信號的輸出頻率相比于慣性導航系統(tǒng)會低很多，同時當信號被遮擋以及干擾時，GNSS導航系統(tǒng)就會停止工作，沒有輸出的結果。如何將兩個或者多個傳感器的導航結果以合適的方式進行組合，以得到穩(wěn)定、高精度的導航結果成為國內外眾多學者的研究方向。

GNSS的信號獲取是通過車載接收機與繞地衛(wèi)星進行通信完成的，在通信過程中，信號不可避免地會通過電離層和對流層，加上衛(wèi)星上存在時鐘對準的問題，會出現(xiàn)接收機接收到的偽距和偽距率相比于真實的位置和速度具有隨機的時間延遲。

面對測量信號具有隨機延遲的情況，出現(xiàn)了很多對卡爾曼濾波算法進行改進的策略，主要分為抑制干擾與提前補償兩個角度。文獻[4]中將隨機延時視為量測方程的乘性不確定性，將當前噪聲和滯后噪聲結合成一個整體，將原系統(tǒng)轉化為一個離散，具有自相關噪聲的隨機不確定系統(tǒng)，從抑制干擾的角度設計了魯棒卡爾曼濾波器來提高濾波結果的穩(wěn)定性和精度。文獻[5]同樣從抑制干擾角度，研究了基于性能準則的不確定時變狀態(tài)時滯隨機非線性耦合復雜網(wǎng)絡的魯棒濾波問題。從抑制干擾的角度出發(fā)的方法，在已知隨機延遲的概率時，無法利用該信息。文獻[6]中提出了一種利用數(shù)學期望的整數(shù)值函數(shù)來補償隨機延遲的新型濾波器結構，適當設計濾波器的增益，使誤差協(xié)方差在跟蹤意義上最小，并證明該算法經(jīng)過線性化后的穩(wěn)定性和收斂性。文獻[6]中的方法從數(shù)學期望的整數(shù)值角度出發(fā)，取整過程中有截斷誤差，得到的濾波結果是有偏估計，并且最新接收的量測值不能直接用于更新最新的結果，輸出具有非實時性。本文從平均狀態(tài)時延補償?shù)慕嵌?，對卡爾曼濾波器進行改進，得到了一個實時無偏估計的濾波器，將測量值用于更新過去的多個狀態(tài)。

1 GNSS與INS松組合系統(tǒng)模型

1.1 GNSS/INS系統(tǒng)的狀態(tài)方程

INS的導航方程構成了系統(tǒng)的狀態(tài)方程，組合導航系統(tǒng)中待估計的位置姿態(tài)為系統(tǒng)的狀態(tài)，系統(tǒng)的速度和位置相對于地心地固(ECEF)坐標系而言，待估計的狀態(tài)向量表示為式(1)

(1)

式中，上標T表示向量的轉置，表示移動物體三個方向的姿態(tài)角，表示移動物體的三個方向速度，表示移動物體的三維位置，表示加速度計的零偏，表示陀螺儀的零偏，是一個15維的狀態(tài)向量。組合導航系統(tǒng)的狀態(tài)方程表達式，可以由式(2)表示：

=-1+

(2)

其中，∈×是系統(tǒng)的狀態(tài)轉移矩陣，由系統(tǒng)參數(shù)確定，∈是系統(tǒng)待估計的狀態(tài)，為系統(tǒng)待估計的狀態(tài)的維度，本文中取為15，為系統(tǒng)所處的時刻，對應到實際模型中即是加速度計和陀螺儀的所提供的加速度和角速度與實際的差值經(jīng)過噪聲驅動矩陣驅動后對系統(tǒng)狀態(tài)的影響，這是由于陀螺儀和加速度的精度和漂移導致的，出于簡單考慮，將驅動后的結果作為系統(tǒng)的過程噪聲。將其建模成零均值的白噪聲，應有～(0,)，為系統(tǒng)的過程噪聲協(xié)方差矩陣。

1.2 GNSS/INS系統(tǒng)的量測方程

本文中選取的GNSS與INS組合方式是松耦合，相比于深耦合和緊耦合，松耦合具有結構簡單，易于實現(xiàn)的特點。松耦合中GNSS與INS單獨進行工作，GNSS將系統(tǒng)接收到的信號，單獨進行解算，輸出系統(tǒng)的位置和速度，再與INS系統(tǒng)的導航結果進行融合，并對INS系統(tǒng)中加速度計和陀螺儀的零偏進行反饋校正。

GNSS系統(tǒng)單獨獲得的位置速度信息可以由式(3)表示

=+

(3)

由于GNSS的信號在傳輸過程中，會有衛(wèi)星時鐘和地面時鐘配準的問題，以及信號通過電離層和對流層會有較大的傳輸延遲，經(jīng)常會出現(xiàn)GNSS的信號滯后于當前時刻的情況出現(xiàn)。為了獲得針對量測具有隨機延時的狀態(tài)估計器，需要建立一個描述隨機延遲性質的數(shù)學模型。當隨機延時存在時，此時濾波器接收到的量測值可能不等于此時系統(tǒng)狀態(tài)的觀測值，而是之前某個時刻的測量。

本文的研究是在已知隨機延遲的概率分布的條件下，重新設計一個迭代的濾波器，并給出該濾波器的具體的結構與參數(shù)的設置。

首先對隨機延遲進行建模，設隨機延遲的取值在集合={0,1,……,}中，其中是給定的最大可能的延遲時間。隨機時延，過程噪聲以及測量噪聲三個隨機量兩兩獨立。

將隨機延遲的概率可以表示為式(4)

Prob{=}=，∈

(4)

其中0≤≤1，∑∈=1。

此時濾波器所接收到的量測可以由式(5)描述：

=-

(5)

考慮以上兩點，那么測量具有隨機延遲的測量方程可以由式(6)表示

(6)

其中，

2 時延卡爾曼濾波的結構和原理

2.1 時延卡爾曼濾波的結構

常規(guī)的線性高斯的系統(tǒng)用式(7)、式(8)來表示：

=-1+

(7)

=+

(8)

其中，∈是系統(tǒng)待估計的狀態(tài)，∈是系統(tǒng)的測量，和是協(xié)方差矩陣為和的零均值白噪聲，是系統(tǒng)的狀態(tài)轉移矩陣。具有高斯白噪聲的線性系統(tǒng)的濾波問題，傳統(tǒng)的卡爾曼濾波已經(jīng)給出了最優(yōu)的無偏估計。

隨機延時系統(tǒng)由狀態(tài)方程(2)與觀測方程(6)進行表示。本文針對該類系統(tǒng)設計的濾波器結構上與卡爾曼濾波器保持一致，分為先驗預測估計以及后驗更新校正兩步。

時延卡爾曼濾波器的結構可由式(9)和(10)來表示，重點在于式(10)中的+1的確定：

(9)

(10)

其中

最新時刻的狀態(tài)估計可由式(11)得到：

(11)

2.2 時延卡爾曼濾波器的原理

本文在已知上一時刻的估計值和誤差的前提下，從最小化誤差的協(xié)方差角度出發(fā)得到下一時刻的估計值和誤差，以得到一個遞歸的濾波器。

(12)

(13)

其中

(14)

(15)

(16)

即可得到+1的表達式(17)。

(17)

那么算法就可以用式(17)～(21)進行描述：

首先是濾波器的先驗預測值：

(18)

(19)

根據(jù)式(17)，便可以得到濾波器的后驗估計：

(20)

(21)

值得注意的是，當隨機延遲被設置為無延遲的時候，該濾波算法就會退化為基本的卡爾曼濾波。

3 仿真示例

3.1 仿真實現(xiàn)

本文的仿真中真實軌跡是由仿真平臺Spirent SimGen產(chǎn)生的車載運動，包含以下運動：

初始速度為10 m/s，加速至20 m/s，減速到 10 m/s,90°轉彎，加速到20 m/s,-30°轉向；加速到30 m/s，30°轉向，減速到5 m/s, -90°轉彎，停車，共持續(xù)175 s。

系統(tǒng)中INS系統(tǒng)每0.01 s輸出一次結果，而GNSS系統(tǒng)每0.1 s輸出一次信號，分別對較低延遲和較高延遲分別進行仿真。仿真對比的結果由未考慮延時的卡爾曼濾波[6]中的算法，以及本文所提出的算法的結果對比。

3.2 仿真結果

當測量量具有較低延遲，延遲概率取為:

=07,=03

此時，三種濾波算法的北向的位移誤差如圖1所示。

圖1 低時延下不同濾波算法的北向位置誤差

北向速度誤差如圖2所示。

圖2 低時延下不同濾波算法的北向速度誤差

東向位置誤差和速度誤差如圖3、圖4所示。

圖3 低時延下不同濾波算法的東向位置誤差

圖4 低時延下不同濾波算法的東向速度誤差

進行100次蒙特卡洛仿真，低延遲下北東兩個方向的速度位移的均方差(RMSE)如表1所示，其中算法1指的是[6]中的方法，算法2指的是本文提出的方法。

表1 低延遲RMSE比較

當測量量具有較高延遲，延遲概率取為：

=02,=04,=02,=02

此時，三種濾波算法的東向位置誤差和速度誤差如圖5、圖6所示。

圖5 高時延下不同濾波算法的北向位置誤差

圖6 高時延下不同濾波算法的北向速度誤差

東向位置誤差和速度誤差如圖7、圖8所示。

圖7 高時延下不同濾波算法的東向位置誤差

圖8 高時延下不同濾波算法的東向速度誤差

進行100次蒙特卡洛仿真，高延遲下北東兩個方向的速度位移的均方差(RMSE)如表2所示，算法1和算法2同表1。

表2 高延遲RMSE平均比較

由圖1～圖8可知，本文提出的算法在載體發(fā)生速度變化的時候，與其余兩種算法相比較，可以有效抑制由于量測時延所引起的濾波器輸出的尖峰問題，系統(tǒng)的速度估計誤差始終被維持在一個較小的區(qū)間中，位置曲線的震蕩幅度也較為穩(wěn)定。

從表1和表2的數(shù)據(jù)可以看出，本文提出的算法的濾波誤差要明顯小于其余兩種，量測信號具有的隨機延遲較大時，改進后的濾波器的輸出誤差減小更為明顯。

4 結束語

針對測量具有隨機延遲的濾波問題，本文首先建立一個描述隨機延遲性質的數(shù)學模型，并將該數(shù)學模型用于松組合的組合導航系統(tǒng)，若忽略延遲，濾波器的輸出會出現(xiàn)尖峰現(xiàn)象。本文設計了一個新的濾波器結構，從最小化濾波器后驗誤差的協(xié)方差矩陣的角度出發(fā)，給出了該濾波器的具體的結構與參數(shù)的設置。仿真結果表明，本文的濾波算法可以充分地利用隨機時延信息，保證了INS/GNSS組合導航系統(tǒng)在隨機延時下狀態(tài)估計的精度與穩(wěn)定性。