單兵導航多傳感器信息融合自適應濾波算法

，， ，，

(1.陸軍裝甲兵學院 兵器與控制系，北京 100072; 2.清華大學 精密儀器系，北京 100084)

0 引言

單兵導航，主要是為士兵、警察、消防員等單兵在徒步執行作戰任務時提供定位和導航服務，單兵的方位信息、位置信息是單兵作戰的必備信息要素，直接關系到作戰的規劃、指揮、執行及最終的勝負[1-3]。在衛星信號無法到達或者被拒止的情況下，迫切需要單兵導航或者行人導航系統具有自主定位導航的能力，而且要求體積小、質量小、功耗低、可便攜性強。

基于足綁式慣性導航系統的單兵導航系統，主要特征是通過將MEMS慣性傳感器組成的IMU固定于足部，構成足綁式慣性導航系統[4-5]。慣性導航系統(Inertial Navigation System, INS)是一種重要的自主導航系統，它建立在牛頓經典力學定律的基礎之上，利用陀螺和加速度計提供的測量數據確定所在運載體的位置[6-7]。慣性導航的精度受限于傳感器精度，特別是對于MEMS慣性傳感器，其誤差發散非常快，低精度的INS在沒有約束的情況下無法單獨使用。單兵在步行時，足部的動作具有一定的規律，如足部在落地時處于相對靜止狀態，可以在足部落地靜止時對足綁式INS進行零速修正(Zero Velocity Update，ZUPT)，限制INS誤差的增長[8-10]。除ZUPT外，還可以利用磁強計、氣壓高度計、多普勒雷達等其他傳感器信息對INS的誤差進行修正。

單兵導航時徒步行走的步態不斷變化，包括慢速步行、快速步行、慢跑、快跑、上樓、下樓、上坡、下坡等多種步態模式，每種步態模式下進行ZUPT和多傳感器融合濾波時，實際濾波器量測噪聲不同，如果采用相同的量測噪聲進行濾波，估計值不能達到最優，嚴重情況下將引起濾波器發散。濾波器長時間運行后協方差矩陣P和濾波增益矩陣K逐漸收斂趨于穩態，量測新息的修正作用將非常小，隨著時間增加，特別是在系統模型、量測模型不準的情況下，濾波器將不能快速有效地利用零速和其他傳感器信息對足綁式INS的誤差進行約束。所以，為保證長時間導航情況下濾波器能夠快速有效的工作，需要抑制濾波發散，減弱濾波增益矩陣K的收斂速度，提升當前量測新息對狀態變量的修正作用。

多傳感器信息融合可采用Kalman濾波器，為適應多傳感器融合單兵導航的需求，Kalman濾波器需要進行優化改進，一方面Kalman濾波器的量測噪聲要能夠自適應調整，以適應不同步態的量測噪聲特點；另一方面為充分利用外部觀測量信息，在進行濾波時，需要提升當前量測新息對狀態變量的修正作用。為此，本文設計了一種自適應濾波算法進行單兵導航多傳感器信息融合。

1 單兵導航多傳感器融合誤差模型

1.1 誤差傳遞模型

對于足綁式INS，系統狀態模型是非線性模型，所以普通線性Kalman濾波器無法使用，而足綁式INS的誤差狀態模型可近似為線性模型[11]。所以在足綁式INS中，可采用線性Kalman濾波器對系統誤差狀態變量進行估計，當估計出誤差狀態變量后，再對足綁式INS的姿態、速度和位置進行反饋校正。為保證誤差狀態模型的線性特性，誤差反饋校準采用實時校正模式，對每次濾波估計值均進行反饋校正。

足綁式INS誤差傳遞模型的狀態變量設計為15維，如下

x=[ δθδγδψδVNδVEδVDδLδλδh

(1)

對基于足綁式INS的單兵導航系統，由于1/(RM+h)2和1/(RN+h)2的量級很小，而且高度通道有氣壓高度計約束，高度誤差δh較小，所以可簡化誤差狀態方程，提高單兵導航算法的實時性，簡化后的足綁式INS誤差狀態方程如式(2)～式(10)所示[12]。

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

將式(2)～式(12)等系統誤差狀態方程寫成在原點附近的雅克比矩陣形式，如式(13)～式(19)所示。

(13)

(14)

其中：

(15)

(16)

(17)

(18)

(19)

1.2 多傳感器信息融合誤差觀測模型

基于足綁式INS的多傳感器融合單兵導航系統，其外部觀測量主要有虛擬零速、磁航向、氣壓高度、GNSS經緯度等，在同一時刻，可能只有其中一部分觀測量有效，在實際使用時，可實時調整觀測方程和觀測矩陣以適應不同的觀測量。本文將采用磁航向和虛擬零速作為觀測量，如式(20)所示。

Z=[δYδVNδVEδVD]T

(20)

利用足綁式INS的計算結果減去外部觀測量，得到誤差狀態方程的直接觀測量：

(21)

其中，YNav和Ymag分別為INS和磁強計輸出的偏航角，VN_Nav、VE_Nav和VD_Nav分別為INS輸出的北東地速度。

由觀測量可得到系統觀測方程的觀測矩陣H為一4×15維的常量矩陣，如下

(22)

2 基于Kalman濾波的誤差狀態估計

2.1 狀態方程離散化

足綁式INS的誤差狀態模型離散化后，其狀態方程和觀測方程為[11]：

(23)

其中狀態誤差方程可表示為

Xk=(I+AΔt)Xk-1+Wk-1

(24)

其中，Δt為濾波周期，Φk，k-1為15×15維矩陣，如式(25)所示。

Φk,k-1=

圖9為彈丸前定心部受力曲線，由曲線可以看出，就總體趨勢而言，高溫未磨損身管彈丸前定心部受力較其他2種工況較大，但彈丸前定心部在磨損身管內的受力個別時間點上較大，最大值約為26 kN，而未磨損身管的彈丸前定心部受力最大值約為145 kN。由于彈丸與身管發生了瞬間碰撞產生的，在動態作用下，由于材料的變形與屈服是一個緩慢過程，因此較大的瞬間力不會對彈丸結構產生破壞。但是，彈丸的橫向和徑向碰撞對裝藥的設計有一定的影響。

(25)

式中，Xk為系統的n維狀態向量，Zk為系統的m維觀測向量，Wk為n維過程噪聲序列，Vk為m維觀測噪聲序列，Φk，k-1為n×n維的狀態轉移矩陣，Hk為m×n維觀測矩陣。

系統過程噪聲和觀測噪聲的統計特性如下：

(26)

其中，Qk為過程噪聲方差矩陣，Rk為量測噪聲方差矩陣，δkj為Kronecker-δ函數。

2.2 Kalman濾波估計及反饋校正

足綁式INS的誤差狀態模型是線性模型，采用線性Kalman濾波器作為多傳感器輔助單兵導航的多源信息融合濾波器。Kalman濾波器估計結果可表示為[13]：

Kalman濾波器是針對線性系統的最優濾波器，INS誤差狀態方程必須工作在小誤差狀態時才能滿足線性條件。對于足綁式INS采用的MEMS慣性傳感器，其隨機誤差較大，INS的誤差發散較快，所以本文采用實時反饋校正的模式。利用濾波器估計結果對足綁式INS進行實時反饋校正，使誤差狀態始終保持小量，減小系統的非線性誤差，避免濾波器發散。反饋校正公式如下：

(28)

3 衰減記憶新息自適應Kalman濾波

學者A.P.Sage和G.W.Husa于1969年提出了一種自適應Kalman濾波算法，可以在進行狀態估計時利用量測輸出在線估計出系統的噪聲參數，即Sage-Husa自適應Kalman濾波[14]。但這種算法是利用全部新息進行量測噪聲的計算，當實際量測噪聲發生快速變化時，如在單兵導航中不斷改變步態模式，量測噪聲不能反映當前真實的噪聲情況。所以本文采用新息自適應濾波算法[13]，量測噪聲矩陣主要根據當前一段時間內的量測新息進行計算，可以使量測噪聲始終反映當前的量測噪聲變化。雖然通過新息自適應濾波可以得到比較準確的量測噪聲，但是由于系統模型存在誤差，濾波器仍然可能發散，所以在新息自適應濾波的基礎上同時采用衰減記憶濾波算法，衰減過去舊的量測新息，提高當前量測新息的比重，與新息自適應濾波算法組成衰減記憶新息自適應Kalman濾波算法。

(29)

理想狀態下的新息為零均值白噪聲，濾波器達到正常工作穩態時，其協方差陣如式(30)所示。

(30)

量測新息協方差矩陣Ak的估計值可采用一段滑動窗口內的量測新息進行估算，對于足綁式INS，滑動窗口的選取范圍是落地ZUPT期間，如式(31)所示。

(31)

由式(30)可得量測方差矩陣R的估計值為

(32)

(33)

在新息自適應濾波的基礎上引入衰減記憶濾波，其基本思想是加大新量測數據的作用，減小陳舊數據的影響，這樣可以更加有效地利用外部觀測信息對足綁式INS進行誤差約束。

(34)

綜合式(32)～式(34)與Kalman基本方程式(27)，可得到衰減記憶新息自適應Kalman濾波器表達式，如式(35)所示。

(35)

4 單兵導航實驗驗證

為驗證本文提出的單兵導航自適應濾波算法的有效性，在某操場上進行足綁式INS單兵導航實驗。繞操場行走，起點和終點重合，在行走的同時利用GPS模塊記錄行走軌跡。圖1所示為實驗時徒步行走軌跡，不同于一般的標準操場，它擁有一個方形的跑道，總距離約460m。

圖1 實驗時徒步行走軌跡Fig.1 Walking track

實驗驗證采用的IMU為ADI公司生產的MEMS IMU模塊，型號為ADIS16488；該模塊集成了三軸陀螺、三軸加速度計、三軸磁強計和氣壓高度計。實驗時設計了專用的數據采集電路，將IMU固定于腳面，手持筆記本電腦，通過串口實時采集并保存IMU輸出原始數據和GPS模塊坐標信息。數據采集頻率為200Hz。數據處理和算法實現通過離線方式在Matlab中計算完成。足綁式INS的初始坐標由GPS給定，初始方向由磁強計給定，三軸磁強計、三軸陀螺和三軸加速度計提前在實驗室進行了校準，在實驗過程中利用磁強計對航向角誤差進行約束。

實驗采集了3組整圈徒步單兵導航數據，第一組和第二組數據為勻速步行單兵導航數據，第三組為步行、跑步混合單兵導航數據。

為驗證本文提出的衰減記憶新息自適應Kalman濾波器算法，在單兵導航多源信息融合基本算法架構的基礎上，對3組數據分別采用普通Kalman濾波器和自適應Kalman濾波器進行處理，比較3組數據的單兵導航終點誤差，由于起點和終點重合，起點和終點坐標之差即為單兵導航終點誤差。圖2～圖7所示為3組數據導航軌跡與GPS軌跡比較圖，單兵導航軌跡起點與GPS軌跡起點一致。表1所示為3組導航數據采用不同濾波器時的終點誤差統計。

圖2 第一組導航軌跡圖(普通濾波)Fig.2 Navigation track of the first group(normal filter)

圖3 第一組導航軌跡圖(自適應濾波)Fig.3 Navigation track of the first group(adaptive filtering)

圖4 第二組導航軌跡圖(普通濾波)Fig.4 Navigation track of the second group(normal filter)

圖5 第二組導航軌跡圖(自適應濾波)Fig.5 Navigation track of the second group(adaptive filtering)

圖6 第三組導航軌跡圖(普通濾波)Fig.6 Navigation track of the third group(normal filter)

圖7 第三組導航軌跡圖(自適應濾波)Fig.7 Navigation track of the third group(adaptive filter)

濾波類型類別第一組勻速步行第二組勻速步行第三組步行跑步混合普通Kalman濾波東向誤差/m056248-649北向誤差/m009035835自適應Kalman濾波東向誤差/m-011141206北向誤差/m103091247誤差占總距離比例022%031%053%

5 結論

通過比較采用不同濾波器時的單兵導航結果，勻速步行時使用普通Kalman濾波和自適應Kalman濾波導航誤差區別不大，單兵導航的軌跡與GPS軌跡基本重合，由表1可知，單兵導航的誤差可控制在2m左右，約占總距離的0.5%，驗證了單兵導航多源信息融合基礎算法的有效性。對于第三組步行和跑步混合的單兵導航數據，采用衰減記憶新息自適應Kalman濾波器后，單兵導航的終點誤差比普通Kalman濾波器大幅減小。自適應濾波器可自適應估計混合步態時濾波器的量測噪聲，并能更好地利用量測新息，濾波器估計結果更加準確，實驗驗證了本文所提出的單兵導航多傳感器信息融合自適應濾波算法的有效性。

[1] Harle R. A survey of indoor inertial positioning systems for pedestrians[J].IEEE Communications Surveys & Tutorials, 2013, 15(3):1281-1293.

[2] 李石兵, 龔貽華, 郭小榮, 等. 行人導航技術及攜行式單兵導航應用[C]//第三屆中國指揮控制大會. 北京, 2015.

[3] Foxlin E. Pedestrian tracking with SHOE-mounted inertial sensors[J]. IEEE Computer Graphics and Applications, 2005, 25(6): 38-46.

[4] Godha S， Lachapelle G. Foot mounted inertial system for pedestrian navigation[J]. Measurement Science & Technolgogy，2008,,19(7):1-9.

[5] Feliz A R，Zalama C E, Gómez G J. Pedestrian tracking using inertial sensors[J]. Journal of Physical Agents, 2009,3(1):35-43.

[6] 高鐘毓. 慣性導航系統技術[M]. 北京: 清華大學出版社, 2012.

[7] 秦永元. 慣性導航(第二版)[M].北京: 科學出版社, 2013.

[8] Skog I, Nilsson J O, Handel P. Evaluation of zero-velocity detectors for foot-mounted inertial navigation systems[C]//2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN). Zurich, 2010.

[9] Skog I,Handel P,Nilsson J O， et al. Zero-velocity detection-an algorithm evaluation[J]. IEEE Transactions on Biomedical Engineering, 2010,57(11): 2657-2666.

[10] Fourati H. Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(1): 221-229.

[11] 郭美鳳，林思敏，周斌，等. MINS/GPS一體化緊組合導航系統[J]. 中國慣性技術學報, 2011，19(2): 214-219.

[12] 嚴恭敏. 車載自主定位定向系統研究[D]. 西安：西北工業大學, 2006.

[13] 秦永元. 卡爾曼濾波與組合導航原理(第二版)[M]. 北京: 科學出版社, 2012.

[14] Sage A P,Husa G W.Adaptive filtering with unknown prior statistics[C]//Proceedings of Joint Automatic Control Conference, 1969.