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

（海軍航空大學(xué)，山東煙臺(tái)264001）

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

本文在研究零速區(qū)間的區(qū)間長(zhǎng)度對(duì)導(dǎo)航精度的影響時(shí)發(fā)現(xiàn)，在同一零速區(qū)間內(nèi)同時(shí)進(jìn)行水平誤差校正和高度誤差校正不能使導(dǎo)航精度達(dá)到綜合最高，因?yàn)槭沟酶叨日`差達(dá)到最小的零速區(qū)間與使得水平誤差達(dá)到最小的零速區(qū)間不一樣。研究發(fā)現(xiàn)相對(duì)水平誤差校正，高度誤差在更嚴(yán)格的零速區(qū)間進(jìn)行校正，可達(dá)到更高的導(dǎo)航精度。因此，本文提出一種將水平誤差和高度誤差分開校正的方法，即在不同的零速區(qū)間劃分下分別進(jìn)行水平誤差校正和高度誤差校正。經(jīng)大量實(shí)驗(yàn)驗(yàn)證，本文提出的零速校正算法能在純慣性導(dǎo)航系統(tǒng)下對(duì)高度誤差進(jìn)行有效抑制，并達(dá)到很高的高度估計(jì)精度，為行人導(dǎo)航系統(tǒng)提供了可靠參考。

1 零速檢測(cè)

本文利用一種基于偽標(biāo)準(zhǔn)差和N-P準(zhǔn)則算法進(jìn)行零速檢測(cè)，該檢測(cè)算法僅需要進(jìn)行一次比較就能夠準(zhǔn)確判斷零速區(qū)間，該算法的檢測(cè)步驟如下。

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

步驟2：將步驟1求得的2s+1個(gè)模值加上2s個(gè)0擴(kuò)充為一個(gè)含有4s+1個(gè)元素的數(shù)列：

步驟3：對(duì)步驟2得到的擴(kuò)充數(shù)列求取標(biāo)準(zhǔn)差，并稱為陀螺儀模值的偽標(biāo)準(zhǔn)差：

步驟4：將步驟3求得的偽標(biāo)準(zhǔn)差值與閾值作比較，若標(biāo)準(zhǔn)差值小于閾值則判定采樣點(diǎn)為零速區(qū)間。反之，則判定采樣點(diǎn)為非零速區(qū)間。

2 零速修正與改進(jìn)算法的提出

在小擾動(dòng)假設(shè)下，捷聯(lián)慣性導(dǎo)航系統(tǒng)的系統(tǒng)誤差模型如下[14]：

式（5）～（7）中：δφ為姿態(tài)角誤差；δvn為導(dǎo)航坐標(biāo)系內(nèi)的速度誤差；為導(dǎo)航坐標(biāo)系內(nèi)的位置誤差；為導(dǎo)航坐標(biāo)系相對(duì)慣性坐標(biāo)系的角速度在導(dǎo)航坐標(biāo)系下的投影；為姿態(tài)余弦矩陣；是陀螺儀的輸出；為加速度計(jì)輸出在導(dǎo)航坐標(biāo)系內(nèi)的投影；為地球自轉(zhuǎn)角速率；為位置速率。

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

選取卡爾曼濾波的狀態(tài)向量為導(dǎo)航參數(shù)誤差，即由姿態(tài)誤差、速度誤差、位置誤差組成的九維向量[16]

系統(tǒng)噪聲由陀螺儀噪聲和加速度計(jì)噪聲組成的向量[17]：

根據(jù)系統(tǒng)誤差模型，卡爾曼濾波的連續(xù)狀態(tài)方程可表示為：

狀態(tài)轉(zhuǎn)移矩陣和噪聲驅(qū)動(dòng)矩陣分別為：

將零速區(qū)間內(nèi)的真實(shí)速度認(rèn)為是0，則零速區(qū)間內(nèi)經(jīng)導(dǎo)航解算得到的速度就是速度誤差，將速度誤差作為的偽觀測(cè)值，可得到卡爾曼濾波的觀測(cè)方程：

連續(xù)狀態(tài)方程的離散化[18]：

慣性導(dǎo)航系統(tǒng)中，由于重力不確定誤差和加速度計(jì)器件誤差的影響，在純慣性導(dǎo)航條件下，高度誤差很快發(fā)散。以實(shí)驗(yàn)者A的10次八字形路徑實(shí)驗(yàn)為例，見圖1，在第40 s后，高度誤差迅速增大，在經(jīng)過(guò)僅僅2min的行走時(shí)間后，高度誤差達(dá)到了2×104m左右。

圖1 純慣導(dǎo)解算下高度估計(jì)圖Fig.1 Height estimation by pure inertial solution

利用本節(jié)的零速修正算法對(duì)10次八字形實(shí)驗(yàn)導(dǎo)航參數(shù)進(jìn)行校正，取零速檢測(cè)閾值時(shí)，得到高度估計(jì)如圖2所示。從圖中可以看出，經(jīng)過(guò)零速修正后，高度估計(jì)準(zhǔn)確性大大提高，高度誤差已經(jīng)由原來(lái)的2×104m減小為6 m左右，高度估計(jì)圖已經(jīng)能夠反映行走過(guò)程中腳步擺動(dòng)。

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

比較圖1和圖2可以發(fā)現(xiàn)，八字形路徑實(shí)驗(yàn)實(shí)際為水平行走，高度不變，但是在純慣導(dǎo)解算條件下，由于重力不確定誤差和加速度計(jì)器件誤差的影響，高度估計(jì)很快就脫離水平面，以很快的速度向下偏移，這與實(shí)際不符且最后誤差太大。而通過(guò)本節(jié)的零速修正算法之后，從誤差大小來(lái)看，高度誤差比純慣導(dǎo)解算條件下小很多，但是還存在誤差；從高度估計(jì)的方向來(lái)看，通過(guò)本節(jié)零速修正算法之后，高度估計(jì)由純慣導(dǎo)解算條件下的向下偏移，變?yōu)橄蛏掀?。高度估?jì)誤差減小是利用零速區(qū)間的特性對(duì)導(dǎo)航誤差周期性進(jìn)行減小；而高度估計(jì)向上偏移是因?yàn)榱闼賲^(qū)間內(nèi)對(duì)高度誤差過(guò)度修正，導(dǎo)致高度估計(jì)偏向相反方向。于是，自然就提出這樣的問(wèn)題：是否能夠通過(guò)控制高度修正的程度，來(lái)使得高度估計(jì)既不偏上，又不偏下，恰好能夠反映實(shí)際行走高度的變化呢？答案是肯定的。

在本節(jié)提出的零速修正算法的基礎(chǔ)上做部分改進(jìn)，使高度修正與水平修正分開，從而方便控制高度修正的程度。由于上文提到的高度誤差過(guò)度修正導(dǎo)致高度估計(jì)反而向上偏移，所以需要減小高度誤差的修正程度，可以通過(guò)縮短高度修正的區(qū)間來(lái)減小高度誤差的修正程度。圖3為優(yōu)化后的誤差修正示意圖，圖中邏輯“1”表示零速區(qū)間，在水平修正區(qū)間對(duì)姿態(tài)角進(jìn)行修正同時(shí)對(duì)速度和位置的水平分量進(jìn)行修正，在高度修正區(qū)間對(duì)速度和位置的豎直分量進(jìn)行修正。在這里只對(duì)零速修正模塊的系統(tǒng)狀態(tài)修正做部分修改，使水平通道與高度通道分開修正，仍然使用本節(jié)的狀態(tài)方程和觀測(cè)方程。用本文第1節(jié)的方法進(jìn)行零速檢測(cè)時(shí)，假設(shè)水平修正的零速判定閾值為，而高度修正的零速判定閾值為。

圖3 優(yōu)化算法示意圖Fig.3 Optimization algorithm diagram

改進(jìn)高度通道的行人導(dǎo)航算法只在系統(tǒng)狀態(tài)修正的過(guò)程中分別修正水平誤差和高度誤差，而不重新設(shè)計(jì)設(shè)計(jì)濾波器。改進(jìn)后，系統(tǒng)狀態(tài)修正公式如下。

3 實(shí)驗(yàn)驗(yàn)證

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

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

圖4中可以看出，改進(jìn)算法后，在2min的行走時(shí)間內(nèi)，高度估計(jì)誤差始終不超過(guò)1.5 m，并且高度估計(jì)能夠很好反映行走時(shí)腳步的上下擺動(dòng)，符合實(shí)際運(yùn)動(dòng)情景。與本節(jié)算法解算結(jié)果對(duì)比，可以發(fā)現(xiàn)改進(jìn)后的算法減小了對(duì)高度誤差的修正程度，使得原來(lái)過(guò)度修正算法得到優(yōu)化，因此高度估計(jì)誤差得以減小。

3.1 水平路徑實(shí)驗(yàn)

為研究零速檢測(cè)閾值對(duì)導(dǎo)航精度的影響，本文利用多種閾值進(jìn)行零速檢測(cè)并用檢測(cè)得到的結(jié)果進(jìn)行零速校正。在研究將水平誤差和高度誤差分開校正的過(guò)程中，發(fā)現(xiàn)對(duì)于實(shí)驗(yàn)者A而言，當(dāng)高度校正閾值減小到水平校正閾值的一半左右時(shí)，高度誤差校正能夠達(dá)到較好的效果。圖5為10次矩形路徑和10次八字形路徑實(shí)驗(yàn)，當(dāng)取不同值時(shí)的水平誤差圖。

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

圖6表示實(shí)驗(yàn)者A的10次矩形路徑和10次八字形路徑實(shí)驗(yàn)，當(dāng)取不同值時(shí)的高度誤差圖。

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

從橫向來(lái)看圖6，可以發(fā)現(xiàn)在選取的零速檢測(cè)閾值范圍內(nèi)，高度誤差隨著閾值的增大而不斷減小。從縱向來(lái)看，的關(guān)系對(duì)高度誤差的大小產(chǎn)生了很大的影響，10次同樣的實(shí)驗(yàn)，當(dāng)利用進(jìn)行解算時(shí)，高度誤差相對(duì)于利用進(jìn)行解算時(shí)大大減小。

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

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

3.2 上下樓梯實(shí)驗(yàn)

室內(nèi)上下樓梯實(shí)驗(yàn)在大學(xué)綜合實(shí)驗(yàn)樓進(jìn)行，實(shí)驗(yàn)路徑按圖7所示的樓層示意圖進(jìn)行。

圖8 上下樓梯實(shí)驗(yàn)高度估計(jì)圖Fig.8 Estimation of the height of the up and down stairs

實(shí)驗(yàn)樓左右對(duì)稱，中間為電梯間，電梯間兩側(cè)各有一個(gè)樓梯，在實(shí)驗(yàn)樓左右兩側(cè)還有一對(duì)樓梯。實(shí)驗(yàn)時(shí)將起點(diǎn)和終點(diǎn)設(shè)置在4樓電梯間的同一點(diǎn)，構(gòu)成閉合路徑。

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

實(shí)驗(yàn)樓每層樓高4 m，比較2種方法的解算結(jié)果可以發(fā)現(xiàn)，傳統(tǒng)零速校正方法解算得到的高度估計(jì)圖完全不能反映實(shí)際的行走高度變化，而本文提出的零速校正方法解算得到的高度估計(jì)圖能夠很好反映實(shí)際行走過(guò)程中高度和樓層的變化，這為行人導(dǎo)航系統(tǒng)提供了較好的高度參考。

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

表2 上下樓梯實(shí)驗(yàn)高度誤差表Tab.2 Height error of up and down stairs test

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

4 結(jié)束語(yǔ)

由于重力不確定和傳感器器件誤差的影響，在純慣性導(dǎo)航系統(tǒng)中，很難對(duì)高度進(jìn)行準(zhǔn)確估計(jì)，高度誤差會(huì)很快發(fā)散。慣性行人導(dǎo)航系統(tǒng)中。傳統(tǒng)零速校正算法將水平誤差和高度誤差同時(shí)進(jìn)行校正沒(méi)有考慮到水平誤差和高度誤差的特性，無(wú)法更加有效地修正高度誤差。本文在研究零速區(qū)間的區(qū)間長(zhǎng)度對(duì)導(dǎo)航精度的影響時(shí)發(fā)現(xiàn)，使得高度誤差達(dá)到最小的零速區(qū)間與使得水平誤差達(dá)到最小的零速區(qū)間不一樣，并且兩個(gè)零速區(qū)間之間的關(guān)系對(duì)高度估計(jì)精度的影響很大。因此，本文設(shè)計(jì)了一種將水平誤差和高度誤差分開校正的算法，在零速檢測(cè)的時(shí)候，劃分出2個(gè)零速區(qū)間，并在這兩個(gè)零速區(qū)間內(nèi)分別進(jìn)行水平誤差和高度誤差校正。經(jīng)過(guò)大量實(shí)驗(yàn)驗(yàn)證，與傳統(tǒng)零速校正算法對(duì)比，本文提出的算法能夠充分考慮到各導(dǎo)航參數(shù)誤差的特點(diǎn)，解決了純慣性導(dǎo)航系統(tǒng)中高度誤差的發(fā)散問(wèn)題，大大提高了高度估計(jì)的精度，為行人導(dǎo)航系統(tǒng)提供了可靠的高度參考。本文首次提出將水平誤差和高度誤差分開進(jìn)行校正，為零速校正提供了新的方法。