粒子群優化噪聲參數的行人導航零速修正算法*

李 磊，蘇 中，吳學佳，雷 明，王一靜

(北京信息科技大學高動態導航技術北京市重點實驗室，北京 100192)

隨著導航定位技術的不斷發展，各行各業對于位置服務的需求日益增加。行人導航是導航定位領域的重要課題之一，行人導航系統(Pedestrian Navigation System，PNS)在軍用和民用領域都具有重要作用[1]。目前，成熟的行人導航系統大部分仍基于基礎設施，如全球導航衛星系統(Global Navigation Satellite System，GNSS)、超寬帶和無線局域網等，但對于地下遮蔽空間、封閉建筑物內部等環境，易受到信號遮擋、基礎設施損毀或設備需預先布置等條件約束，上述行人導航系統通常難以實現[2]。此類復雜環境下，主要應用慣性測量單元(Inertial Measurement Unit，IMU)的自主定位定向技術成為行人導航的研究熱點。隨著微機電系統(Micro-Electro-Mechanical System，MEMS)的迅速發展，MEMS-IMU 成為復雜環境下行人自主定位定向的主要設備[3]。由于慣性器件本身存在漂移，定位誤差隨時間迅速累積[4]。行人慣性導航系統通常使用零速修正(Zero Velocity Update，ZUPT)方法修正定位誤差，ZUPT 要求將IMU 固定于行人足部，檢測足部靜止階段，再通過擴展卡爾曼濾波(Extended Kalman Filter，EKF)利用零速區間的速度誤差作為量測來修正捷聯解算的結果[5]。田曉春等[6]構造了一種小波系數介于軟硬閾值之間的連續小波閾值函數，對MEMS 陀螺儀信號進行降噪后應用于行人導航系統，通過實驗對比了不同閾值法去噪后的陀螺儀信號，但未進行行人導航實驗。余志鵬等[7]采用秩卡爾曼濾波來適應非高斯噪聲對系統的影響，在時間更新前進行秩采樣，再使用采樣點集進行卡爾曼濾波，在室內環境下實驗證明行人導航系統定位精度有所提高。

對于使用EKF 實現行人ZUPT 方法，系統建模誤差和測量環境的不確定性對濾波器性能有重要影響，上述方法考慮了慣性器件在使用中產生的噪聲信號，而濾波器中合適的噪聲參數也能夠有效提高行人慣性導航定位精度，但噪聲參數通常未知或給定的參數不適用，將導致定位誤差較大。劉韜等[8]利用Sage-Husa 濾波和Huber 函數建立自適應擴展卡爾曼濾波(Adaptive Extended Kalman Filter，AEKF)，分別對系統過程噪聲協方差Q和量測噪聲協方差R進行在線修正并應用于室內無人車定位，實驗結果表明僅在線修正Q時平均誤差反而會增大。胡高歌等[9]針對過程噪聲協方差的不確定問題提出了一種基于極大似然估計的無味卡爾曼濾波，在濾波過程中對Q進行極大似然估計并將問題轉化為對Q的對角元素的估計。Yan 等[10]提出了利用遞歸神經網絡對IMU 輸出序列進行某些運動模式的識別，并通過知識庫獲取Q值，還提出了一種逐步增加數據子集的神經網絡訓練方法以提高網絡的訓練效率。該方法在無人機組合導航系統上取得了良好的定位精度，但行人足部相對于無人機的慣性信息更加多變，此類方法可能仍需建立足夠龐大的行人慣性數據集。

本文在僅使用由三軸陀螺儀和三軸加速度計構成的MEMS-IMU 的情況下，提出了一種基于粒子群優化(Particle Swarm Optimization，PSO)的ZUPT 算法，該算法通過零速修正時最小化濾波器新息序列來優化EKF 的噪聲參數，能夠在零速區間自主調整過程噪聲參數和量測噪聲參數，得到更加精確、平滑的行人運動軌跡。本文分別使用ZUPT 算法和提出的PSO-ZUPT 算法對行人實測足部慣性數據進行解算，實現了行人自主定位定向，并對兩種算法解算的運動軌跡及定位誤差進行了比較分析，充分驗證了所提出算法的有效性。

1 粒子群優化的零速修正算法

將IMU 捆綁于行人足部，獲取慣性數據后進行捷聯解算，通過零速檢測識別人員足部的靜止階段后得到零速區間的速度誤差量測，利用EKF 對解算結果進行零速修正[11]。噪聲參數對EKF 的性能有重要影響，本文利用PSO 算法自主優化EKF 的過程噪聲和量測噪聲參數，實現行人慣性導航，算法流程如圖1 所示。

圖1 算法流程圖

1.1 捷聯解算

捷聯解算過程主要包括姿態更新、速度更新和位置更新。IMU 捆綁位置及坐標系如圖2 所示，選取“北-東-地”作為導航坐標系，與地理坐標系一致。選取人員足部的“前-右-下”作為載體坐標系，IMU 固定在人員左足跟部，獲取行人行走時足部的加速度和角速度信息，行人運動軌跡為長40.3 m、寬16.5 m 的矩形，IMU 輸出的原始數據如圖3 所示。

圖2 IMU 捆綁位置及坐標系

圖3 IMU 輸出原始數據

利用如下導航方程對行人的姿態、速度、位置進行計算:

1.2 零速檢測

零速檢測的主要任務是識別行人足部的靜止階段，并確定步態周期內的零速區間。使用廣義似然比檢測法(Generalized Likelihood Ratio Test，GLRT)進行零速檢測[12]，第n組IMU 輸出序列的檢驗統計量為:

式中:T(un)為檢驗統計量，W為滑動窗口大小。un為窗口內傳感器輸出分別為加速度計和陀螺儀的輸出值，為滑動窗口內加速度計輸出值的平均值，g為當地重力加速度，分別為加速度計和陀螺儀的噪聲方差。Ck為零速檢測輸出標志位，γ為零速檢測閾值。

1.3 零速修正

在行人行走過程中，足部周期性地與地面接觸，ZUPT 算法將足部靜止階段的速度輸出作為速度誤差的量測值，使用EKF 對導航狀態誤差進行濾波估計，在量測更新過程中進行零速修正。

行人慣性導航系統的十五維向量Xk為:

式中:pk表示k時刻位置、vk表示k時刻速度、φk表示k時刻姿態、ak表示k時刻加速度計零偏誤差，ωk表示k時刻陀螺儀零偏誤差。

取導航系統狀態向量的誤差項δXk建立卡爾曼濾波方程:

以上15 維狀態誤差向量分別為k時刻的三軸位置誤差、速度誤差、姿態誤差、加速度計零偏誤差和陀螺儀零偏誤差。

慣性導航系統的狀態誤差方程如下:

式中:Δt為采樣時間，ft為加速度計測量值的反對稱矩陣，b1和b2分別為加速度計和陀螺儀零偏誤差的比例系數，Wk·a和Wk·ω分別為k時刻的加速度計和陀螺儀誤差的隨機系統動態噪聲。

當行人足部處于靜止階段時，足部的理論速度應該為零，但由于存在測量誤差等原因，靜止階段計算得到的速度并不為零。因此選擇靜止階段的速度誤差作為量測向量Zk:

由此可以得到離散時間的線性系統狀態空間模型:

Wk-1和Vk分別表示過程噪聲和量測噪聲，其噪聲統計為:

式中:Qk表示k時刻過程噪聲協方差矩陣，Rk表示k時刻量測噪聲協方差矩陣，δkg表示Kronecker 函數。在標準EKF 中，需設定合適的Qk和Rk。

EKF 濾波過程主要分為時間更新和量測更新，時間更新過程中，EKF 將誤差狀態轉移到捷聯解算系統中，在測量后立即將誤差狀態置零，并計算先驗估計誤差協方差矩陣。當Ck＝0 時，不進行量測更新；當Ck＝1 時，進入量測更新過程，執行ZUPT 算法。

1.4 粒子群優化的零速修正

粒子群優化算法主要思想是通過粒子之間的信息交流在候選解空間內尋找最優解。PSO 算法作為一種經典的群智能優化算法被廣泛應用于求最優解與參數優化問題[13]。

本文提出的PSO-ZUPT 算法在行人足部靜止時，利用PSO 優化零速修正階段的濾波器噪聲協方差矩陣參數，使濾波器能夠根據行人的運動狀態來自主調整參數。當零速檢測判斷當前狀態處于零速區間時，對系統狀態誤差進行濾波，并執行PSO 算法，將當前的噪聲協方差矩陣Qk和Rk的對角線元素作為粒子的位置輸入到PSO 算法中，并利用當前量測值和估計值計算新息序列作為PSO 算法的適應度函數。通過PSO 算法來最小化量測值與預測值之間的差，從而自主更新噪聲矩陣Qk和Rk[14]。根據更新后的協方差矩陣參數，分別生成協方差為Qσi和Rσi的高斯噪聲Wpi和Vpi，并加入到濾波估計值和量測值中，以計算更新后的新息序列，得到優化后的最優個體。

EKF 中的過程噪聲協方差矩陣Qk和量測噪聲協方差矩陣Rk都為對角矩陣，Qk和Rk的優化問題可轉化為其對角元素的優化問題。選取對角元素值作為PSO 算法中粒子的位置，使用PSO 算法對加速度計噪聲、陀螺儀噪聲及速度測量噪聲的參數向量進行優化，第i個粒子的位置用σi表示:

式中:σai和σωi分別表示加速度計噪聲和陀螺儀噪聲參數，σvi為速度測量噪聲參數。第i個粒子的速度vσi和位置σi的更新公式如下:

式中:t表示PSO 算法的迭代次數，i∈[1，M]，M表示種群個數，均為正整數。w表示慣性權重，c1和c2表示加速因子，r1和r2為[0，1]之間的隨機數。bσi代表個體最優解，gb 代表全局最優解。

優化后的噪聲協方差Qσi和Rσi可以通過粒子的位置σi得到:

式中:σab和σωb分別表示加速度計和陀螺儀的零位穩定性噪聲，采用固定值不參與優化。

PSO 算法中適應度函數的設計決定了粒子的優化目標，計算卡爾曼濾波器新息序列的平方均值作為適應度函數并向新息序列減小的方向優化粒子，適應度函數定義為:

式中:zpij為量測值zpi的第j維數值，xpij為濾波估計值xpi的第j維數值。zpi和xpi分別引入了參數為σvi和[σaiσωiσabσωb]T的高斯噪聲，其數值可以用如下表達式計算:

式中:Wpi和Vpi分別為協方差為Qσi和Rσi的高斯噪聲，即:

由于對零速區間的所有時刻進行PSO 參數優化會導致軌跡解算時間過長，需設置一個連續迭代次數h，當每連續執行h次零速修正后執行一次PSO 算法。實驗中，為xpid設置了不同的搜索邊界來提高算法的穩定性。為提高PSO 算法的效率，還應該設置合適的最大迭代次數tmax和適應度閾值ε。圖4 展示了一次PSO 參數優化過程中的最優個體適應度變化曲線，粒子群在545 次到1 000 次迭代中適應度已不再下降，根據實驗經驗，將tmax設為800。在算法1 和算法2 處分別給出PSO 優化噪聲協方差參數算法流程和PSO-ZUPT 算法流程。

圖4 PSO 最優個體適應度曲線

算法1 PSO 優化噪聲協方差參數算法

算法2 PSO-ZUPT 算法

2 實驗驗證及分析

2.1 實驗條件

實驗使用圖5 中實驗室自研的MEMS-IMU 采集行人行走時的足部慣性數據，IMU 僅包含三軸加速度計和三軸陀螺儀，其相關技術指標如表1 所示。使用MATLAB R2018b 軟件進行慣性數據解算，對算法進行驗證，計算機處理器為Intel Core i7-10700K，32GB運行內存。

表1 IMU 技術指標

圖5 MEMS-IMU 實物圖

實驗分別使用標準ZUPT 算法和PSO-ZUPT 算法對1.1 節中采集的慣性數據進行解算，此外，還進行了其他軌跡的行人行走實驗。行人運動軌跡為沿足球場中圈連續行走3 圈回到起點，如圖6 中實線圓圈所示，球場中圈半徑為9.15 m，3 圈的路徑總長度約為172.5 m，共采集5 組足部慣性數據。分別使用標準ZUPT 和PSO-ZUPT 兩種算法進行解算，對定位結果進行分析。

圖6 沿足球場中圈的運動軌跡

PSO 算法中，將M設為30，c1和c2均為2.05，零速修正連續迭代次數h設為1 000，最大迭代次數tmax為800。慣性器件噪聲σai和σωi的搜索空間選為[0.01，0.5]，σvi的搜索空間選為[0.001，0.5]。PSOZUPT 算法的噪聲協方差初值與ZUPT 算法的Q和R相等:

慣性權重進行線性遞減，利用如下公式計算:

式中:wmax和wmin分別為慣性權重的最大值和最小值，在實驗中分別設為1 和0.8。

2.2 實驗結果

使用兩種算法對1.1 節中慣性數據解算的結果如圖7 所示。ZUPT 算法的解算軌跡與真實軌跡形狀相似，但仍存在較大誤差，軌跡在y 軸方向上的最大誤差約為3.75 m，終點位置誤差約為1.42 m。PSO-ZUPT 算法的解算軌跡更接近真實軌跡且更加平滑。經計算，最大位置誤差約為1.85 m，終點位置誤差約為0.64 m，與ZUPT 算法結果相比，定位誤差分別減小了50.67%和54.93%。

圖7 兩種算法解算軌跡對比圖

對5 組足球場中圈數據的解算位置誤差進行計算，并給出了在MATLAB 軟件下的運行時間，算法A 表示標準ZUPT 算法，算法B 表示PSO-ZUPT 算法，結果如表2 所示。平均誤差為所有采樣點解算位置到中圈圓心的距離與中圈半徑之差的平均值，終點誤差為解算軌跡終點到起點之間的距離，最大誤差為所有采樣點解算位置到中圈圓心的距離和中圈半徑之差的最大值。

表2 中圈軌跡解算結果誤差表

由表2 可知，PSO-ZUPT 算法相對于ZUPT 算法在上述三個指標上均有提高。ZUPT 算法的平均誤差為0.796 m，PSO-ZUPT 算法的平均誤差為0.092 m，平均誤差減小了88.31%，終點誤差減小了34.13%，最大誤差減小了88.66%。5 組數據的行人行走時間在145 s 至151 s 之間，PSO-ZUPT 算法運行時間在86 s 至91 s 之間，相對于ZUPT 運行時間提高4.3 倍左右，但仍可以支持算法在線運行。

第1 組數據解算的運動軌跡如圖8 和圖9 所示，圖8 為ZUPT 解算軌跡，圖9 為PSO-ZUPT 解算軌跡，實線是作為參考的真實軌跡，點畫線為解算軌跡。由于PSO-ZUPT 濾波過程中Qk和Rk是變化的，給出每次PSO 算法中全局最優解的平均值:

圖8 ZUPT 解算軌跡

圖9 PSO-ZUPT 解算軌跡

使用標準EKF 需要根據不同測試人員的生物特性、不同慣性器件指標及不同測試環境來確定合適的Qk和Rk，由圖8 可見，解算軌跡會出現明顯的修正跳變點，且誤差較大，軌跡逐漸漂移。PSO 算法可以在EKF 量測過程中對Qk和Rk進行自主尋優，解算軌跡更加平滑，定位精度提高。

為驗證算法在長距離行走下的有效性，實驗中人員沿400 m 跑道行走并形成閉環路線，采集了行人足部運動的慣導數據，行走軌跡如圖10 所示，圖中跑道處實線為實際行走軌跡，圓點為起點位置，行人沿實際軌跡行走一圈后回到起點位置。

圖10 沿操場跑道的運動軌跡

分別利用ZUPT 和PSO-ZUPT 對跑道數據進行解算，得到的行人運動軌跡如圖11 所示。在行人長距離運動的情況下，ZUPT 解算的軌跡與真實軌跡之間的誤差明顯，本文提出的算法解算的軌跡更接近真實軌跡。經計算，ZUPT 算法解算的軌跡終點位置誤差為24.39 m，PSO-ZUPT 算法解算的軌跡終點位置誤差為3.02 m，誤差減小了87.62%，定位精度有明顯提升。

圖11 ZUPT 和PSO-ZUPT 解算軌跡對比圖

3 總結

本文在研究ZUPT 進行行人自主定位定向的基礎上，提出了一種PSO 優化的ZUPT 算法，設計新息序列作為適應度函數，可以調整濾波器的噪聲矩陣參數。經多組實驗驗證，該算法能夠有效提高行人零速修正算法解算軌跡的平滑程度，并減小定位誤差。在足球場中圈和400 m 跑道的運動軌跡下檢驗了算法有效性，在足球場中圈行走軌跡的終點位置相對誤差為0.25%，在400 m 跑道軌跡的終點位置相對誤差為0.76%，結果均優于標準ZUPT 算法，對于行人自主定位定向技術的應用具有一定意義。