一種優化高度通道的行人導航算法

周紹磊，李松林，戴洪德，全聞捷

（海軍航空大學，山東煙臺264001）

零速修正（Zero-Velocity Updating，ZUPT）[1-3]是目前慣性行人導航最常用的抑制和消除導航誤差的方法。Elwell[4]首次提出了人在行走過程中存在零速區間的特性。隨后，E.Foxlin[5]全面、系統地介紹了將慣性導航系統安裝在足部的行人導航方法，并利用零速修正對導航參數誤差進行校正，取得了較為理想的實驗結果。在此基礎上，國內外學者David[6]、Park[7]、Maan[8]、Carlos[9]、Terra[10]、高哲[11]、趙小明[12]、田曉春[13]等對慣性行人導航的零速修正算法做了進一步的研究，這些研究的共同特點是在同一零速區間內同時進行水平誤差校正和高度誤差校正，在純慣性導航條件下，這種零速校正方法會使高度誤差很快發散。

本文在研究零速區間的區間長度對導航精度的影響時發現，在同一零速區間內同時進行水平誤差校正和高度誤差校正不能使導航精度達到綜合最高，因為使得高度誤差達到最小的零速區間與使得水平誤差達到最小的零速區間不一樣。研究發現相對水平誤差校正，高度誤差在更嚴格的零速區間進行校正，可達到更高的導航精度。因此，本文提出一種將水平誤差和高度誤差分開校正的方法，即在不同的零速區間劃分下分別進行水平誤差校正和高度誤差校正。經大量實驗驗證，本文提出的零速校正算法能在純慣性導航系統下對高度誤差進行有效抑制，并達到很高的高度估計精度，為行人導航系統提供了可靠參考。

1 零速檢測

本文利用一種基于偽標準差和N-P準則算法進行零速檢測，該檢測算法僅需要進行一次比較就能夠準確判斷零速區間，該算法的檢測步驟如下。

步驟1：假設(G yrox(k),Gyroy(k),Gyroz(k))為k時刻三軸陀螺儀的輸出。按照滑動窗口的方法選取一個采樣窗口k=[i-s,i+s]，其中s為半窗口采樣數，并求得這個采樣窗口內所有陀螺儀輸出的2s+1個模值：

步驟2：將步驟1求得的2s+1個模值加上2s個0擴充為一個含有4s+1個元素的數列：

步驟3：對步驟2得到的擴充數列求取標準差，并稱為陀螺儀模值的偽標準差：

步驟4：將步驟3求得的偽標準差值與閾值作比較，若標準差值小于閾值則判定采樣點為零速區間。反之，則判定采樣點為非零速區間。

2 零速修正與改進算法的提出

在小擾動假設下，捷聯慣性導航系統的系統誤差模型如下[14]：

式（5）～（7）中：δφ為姿態角誤差；δvn為導航坐標系內的速度誤差；為導航坐標系內的位置誤差；為導航坐標系相對慣性坐標系的角速度在導航坐標系下的投影；為姿態余弦矩陣；是陀螺儀的輸出；為加速度計輸出在導航坐標系內的投影；為地球自轉角速率；為位置速率。

式（8）、（9）中：εb和 ?b分別為陀螺儀漂移和加速度計零偏。

選取卡爾曼濾波的狀態向量為導航參數誤差，即由姿態誤差、速度誤差、位置誤差組成的九維向量[16]

系統噪聲由陀螺儀噪聲和加速度計噪聲組成的向量[17]：

根據系統誤差模型，卡爾曼濾波的連續狀態方程可表示為：

狀態轉移矩陣和噪聲驅動矩陣分別為：

將零速區間內的真實速度認為是0，則零速區間內經導航解算得到的速度就是速度誤差，將速度誤差作為的偽觀測值，可得到卡爾曼濾波的觀測方程：

連續狀態方程的離散化[18]：

慣性導航系統中，由于重力不確定誤差和加速度計器件誤差的影響，在純慣性導航條件下，高度誤差很快發散。以實驗者A的10次八字形路徑實驗為例，見圖1，在第40 s后，高度誤差迅速增大，在經過僅僅2min的行走時間后，高度誤差達到了2×104m左右。

圖1 純慣導解算下高度估計圖Fig.1 Height estimation by pure inertial solution

利用本節的零速修正算法對10次八字形實驗導航參數進行校正，取零速檢測閾值時，得到高度估計如圖2所示。從圖中可以看出，經過零速修正后，高度估計準確性大大提高，高度誤差已經由原來的2×104m減小為6 m左右，高度估計圖已經能夠反映行走過程中腳步擺動。

圖2 本節零速修正解算下高度估計圖Fig.2 Height estimation by ZUPT in this section

比較圖1和圖2可以發現，八字形路徑實驗實際為水平行走，高度不變，但是在純慣導解算條件下，由于重力不確定誤差和加速度計器件誤差的影響，高度估計很快就脫離水平面，以很快的速度向下偏移，這與實際不符且最后誤差太大。而通過本節的零速修正算法之后，從誤差大小來看，高度誤差比純慣導解算條件下小很多，但是還存在誤差；從高度估計的方向來看，通過本節零速修正算法之后，高度估計由純慣導解算條件下的向下偏移，變為向上偏移。高度估計誤差減小是利用零速區間的特性對導航誤差周期性進行減小；而高度估計向上偏移是因為零速區間內對高度誤差過度修正，導致高度估計偏向相反方向。于是，自然就提出這樣的問題：是否能夠通過控制高度修正的程度，來使得高度估計既不偏上，又不偏下，恰好能夠反映實際行走高度的變化呢？答案是肯定的。

在本節提出的零速修正算法的基礎上做部分改進，使高度修正與水平修正分開，從而方便控制高度修正的程度。由于上文提到的高度誤差過度修正導致高度估計反而向上偏移，所以需要減小高度誤差的修正程度，可以通過縮短高度修正的區間來減小高度誤差的修正程度。圖3為優化后的誤差修正示意圖，圖中邏輯“1”表示零速區間，在水平修正區間對姿態角進行修正同時對速度和位置的水平分量進行修正，在高度修正區間對速度和位置的豎直分量進行修正。在這里只對零速修正模塊的系統狀態修正做部分修改，使水平通道與高度通道分開修正，仍然使用本節的狀態方程和觀測方程。用本文第1節的方法進行零速檢測時，假設水平修正的零速判定閾值為，而高度修正的零速判定閾值為。

圖3 優化算法示意圖Fig.3 Optimization algorithm diagram

改進高度通道的行人導航算法只在系統狀態修正的過程中分別修正水平誤差和高度誤差，而不重新設計設計濾波器。改進后，系統狀態修正公式如下。

3 實驗驗證

利用改進高度通道的零速修正算法對第2節中的八字形路徑進行解算，水平修正閾值。為了減小高度修正的程度，經多次試驗，結果表明對實驗者A而言，當高度修正閾值時，高度估計精度較高。得到的高度估計結果如圖4所示。

圖4 改進高度通道算法高度估計圖Fig.4 Height estimation by improved height channel algorithm

圖4中可以看出，改進算法后，在2min的行走時間內，高度估計誤差始終不超過1.5 m，并且高度估計能夠很好反映行走時腳步的上下擺動，符合實際運動情景。與本節算法解算結果對比，可以發現改進后的算法減小了對高度誤差的修正程度，使得原來過度修正算法得到優化，因此高度估計誤差得以減小。

3.1 水平路徑實驗

為研究零速檢測閾值對導航精度的影響，本文利用多種閾值進行零速檢測并用檢測得到的結果進行零速校正。在研究將水平誤差和高度誤差分開校正的過程中，發現對于實驗者A而言，當高度校正閾值減小到水平校正閾值的一半左右時，高度誤差校正能夠達到較好的效果。圖5為10次矩形路徑和10次八字形路徑實驗，當取不同值時的水平誤差圖。

圖5 水平誤差圖Fig.5 Horizontal error

圖6表示實驗者A的10次矩形路徑和10次八字形路徑實驗，當取不同值時的高度誤差圖。

圖6 高度誤差圖Fig.6 Height error

從橫向來看圖6，可以發現在選取的零速檢測閾值范圍內，高度誤差隨著閾值的增大而不斷減小。從縱向來看，的關系對高度誤差的大小產生了很大的影響，10次同樣的實驗，當利用進行解算時，高度誤差相對于利用進行解算時大大減小。

表1分別表示不同零速檢測閾值下，實驗者A的10次矩形路徑和10次八字形路徑行走的平均高度誤差實驗結果。從表1可以看出，如果利用傳統零速校正方法，在60 m和120 m的閉合路徑行走實驗中，分別產生了2 m和6 m左右的高度誤差。而如果將高度校正閾值減小為水平校正閾值的一半，利用本文的校正方法，可以大大減小高度誤差，當時，高度誤差甚至減小為原誤差的10%左右，高度估計誤差減小為行走路程的0.5%左右。

表1 平均高度誤差Tab.1 Average height error

3.2 上下樓梯實驗

室內上下樓梯實驗在大學綜合實驗樓進行，實驗路徑按圖7所示的樓層示意圖進行。

圖8 上下樓梯實驗高度估計圖Fig.8 Estimation of the height of the up and down stairs

實驗樓左右對稱，中間為電梯間，電梯間兩側各有一個樓梯，在實驗樓左右兩側還有一對樓梯。實驗時將起點和終點設置在4樓電梯間的同一點，構成閉合路徑。

該實驗由2名實驗者完成，每人進行3次重復實驗。圖8表示實驗者A的一次實驗結果圖，其中左圖為傳統零速校正方法，將水平誤差和高度誤差同時校正，取時，得到的y/z圖和距離/高度圖；右圖為本文提出的方法，將水平誤差和高度誤差分開校正，取時，得到的y/z圖和距離/高度圖，假設起始高度為0。

實驗樓每層樓高4 m，比較2種方法的解算結果可以發現，傳統零速校正方法解算得到的高度估計圖完全不能反映實際的行走高度變化，而本文提出的零速校正方法解算得到的高度估計圖能夠很好反映實際行走過程中高度和樓層的變化，這為行人導航系統提供了較好的高度參考。

2名實驗者進行的6次上下樓梯實驗，得到的高度解算誤差如表2所示。由于行走習慣的差異，適應不同行人的高度通道修正閾值不同，經過多次試驗，在的情況下，適應實驗者A的高度通道修正閾值為；適應實驗者B的高度通道修正閾值為σ*2=13.5。

表2 上下樓梯實驗高度誤差表Tab.2 Height error of up and down stairs test

從表2可以看出，當利用傳統零速修正算法進行解算時，實驗者A的平均高度誤差達到17.45 m，實驗者B的平均高度誤差達到15.79 m；而利用改進高度通道的零速修正算法進行解算時，實驗者A的平均高度誤差減小為0.73 m，實驗者B的平均高度誤差減小為0.97 m。該實驗驗證了改進高度通道的零速修正算法不僅在水平路徑實驗中對高度誤差有抑制作用，而且在上下樓梯的立體路徑實驗中對高度誤差也有很好的抑制作用；由于上下樓梯實驗是由2名實驗者完成的，實驗結果表明改進高度通道的零速修正算法對2名實驗者的高度誤差均有很好的抑制作用，驗證了改進算法并不僅僅適用于特定的行走習慣，具有普適性，可作為一種抑制高度誤差發散的方法，用于其他可以進行零速修正的導航領域。

4 結束語

由于重力不確定和傳感器器件誤差的影響，在純慣性導航系統中，很難對高度進行準確估計，高度誤差會很快發散。慣性行人導航系統中。傳統零速校正算法將水平誤差和高度誤差同時進行校正沒有考慮到水平誤差和高度誤差的特性，無法更加有效地修正高度誤差。本文在研究零速區間的區間長度對導航精度的影響時發現，使得高度誤差達到最小的零速區間與使得水平誤差達到最小的零速區間不一樣，并且兩個零速區間之間的關系對高度估計精度的影響很大。因此，本文設計了一種將水平誤差和高度誤差分開校正的算法，在零速檢測的時候，劃分出2個零速區間，并在這兩個零速區間內分別進行水平誤差和高度誤差校正。經過大量實驗驗證，與傳統零速校正算法對比，本文提出的算法能夠充分考慮到各導航參數誤差的特點，解決了純慣性導航系統中高度誤差的發散問題，大大提高了高度估計的精度，為行人導航系統提供了可靠的高度參考。本文首次提出將水平誤差和高度誤差分開進行校正，為零速校正提供了新的方法。