航向誤差非線性預測的慣性行人導航零速修正算法

戴洪德, 馬宇峰, 戴邵武, 鄭百東, 張笑宇

(海軍航空大學航空基礎學院, 山東 煙臺 264001)

0 引 言

隨著全球衛(wèi)星定位系統(tǒng)的發(fā)展,基于位置的服務(location based services, LBS)[1-2]更加成為了人們生產和生活中必不可少的重要組成部分。同時,基于位置信息的實時行人導航技術也普遍地應用到人們的日常生活之中。目前,常用的行人導航技術一般以全球衛(wèi)星導航系統(tǒng)(global navigation satellite system, GNSS)為主,同時引入地圖匹配來減少行人導航系統(tǒng)中的隨機誤差,通過該方法行人可以在地形開闊且衛(wèi)星信號良好的區(qū)域獲得準確的導航信息[3]。但是當行人在室內環(huán)境時,衛(wèi)星信號不良和缺失就會導致行人導航系統(tǒng)無法完成定位和導航任務?，F(xiàn)有的室內導航技術主要有室內偽衛(wèi)星射頻識別(radio frequency identification, RFID)、超寬帶(ultra wide band, UWB)、無線局域網(wireless local area network, WLAN)指紋、WIFI、藍牙、視覺和ZigBee等[4-10]。而對于消防救援、搶險救災、室內近距離巷戰(zhàn)(close quarter battle, CQB)等,通常應急人員所處建筑室內沒有提前鋪設室內導航設備或現(xiàn)場設備已損毀,人員無法使用上述室內導航技術實施救援或作戰(zhàn),這就要求人員所攜帶的導航設備具有極高的自主性?；谖C電系統(tǒng)(micro-electro-mechanical system, MEMS)慣性測量單元(MEMS inertial measurement unit, MIMU)的慣性行人導航系統(tǒng)因其自主性強,成本低逐漸成為行人導航系統(tǒng)的主流研究方向,但是MIMU定位誤差會隨時間逐漸積累。針對此問題,20世紀90年代末,Elwell提出了零速修正(zero velocity update, ZUPT)算法[11],通過周期性應用速度為零對導航誤差進行修正。但是慣性行人導航系統(tǒng)存在航向角不可觀測的問題,ZUPT算法無法對航向角誤差進行修正,最終導致導航精度降低。美國密歇根大學的Borestein等人提出了啟發(fā)式漂移抑制(heuristic drift reduction, HDR )算法,利用了全球定位系統(tǒng)(global positioning system, GPS)和磁強計無效的許多環(huán)境具有直線特征的事實,估計行人沿著直線行走的可能性,對陀螺儀輸出進行校正,減少漂移[12]。美國圣路易斯華盛頓大學的Prateek等學者提出雙慣性測量單元(inertial measurement unit, IMU)系統(tǒng),在雙腳各安裝一個IMU,通過兩個IMU之間的距離約束條件來抑制航向角的發(fā)散[13]。武漢大學的Qiu等人提出了一種基于磁性信息校正的神經網絡慣性導航系統(tǒng),檢測磁場的穩(wěn)定性,進而校正航向[14],北京理工大學的Tian等人提出了一種基于零速區(qū)間端點平滑濾波的航向校正算法,通過磁力計計算出零速區(qū)間內的所有航向角,再應用平滑濾波得到最優(yōu)的航向角,并在零速區(qū)間終端時刻進行航向修正[15]。

上述方法雖然能夠對航向角進行校正,但僅能在特定環(huán)境下進行使用,并且引入多傳感器增加了行人導航系統(tǒng)的復雜性。本文針對上述問題,提出了一種基于ZUPT,零角速率修正與航向角誤差非線性預測校正的慣性行人導航算法。首先通過廣義似然比檢測廣義似然比檢測(genera-lized likelihood ratio test, GLRT)算法劃分行人步態(tài)區(qū)間,在零速區(qū)間內利用ZUPT和零角速率修正抑制捷聯(lián)慣導解算過程中產生的誤差。對于航向角誤差修正,建立零速區(qū)間航向角誤差觀測模塊和非零速區(qū)間航向角誤差非線性預測模塊,定義出零速區(qū)間的航向角誤差。利用卡爾曼濾波對行人導航系統(tǒng)的誤差進行估計,并利用估計誤差對捷聯(lián)慣導解算結果進行修正,提高慣性行人導航系統(tǒng)的導航定位精度。

1 零速檢測算法

根據人體運動學可知,人的行走過程存在周期性特征[16-17]。通常,將行人行走過程中從單只腳著地開始,經兩只腳步態(tài)交替變換,到該側腳跟再次著地定義為一個完整的步態(tài)周期[18]。當行人足部處于站立區(qū)間時速度為0 m/s,因此將腳與地面完全接觸的時間段稱為零速區(qū)間。零速區(qū)間如圖1所示。

當行人處于零速區(qū)間時,足部運動的角速度及水平加速度值近似為零,豎直方向的加速度值近似為重力加速度。

本文采用基于GLRT算法獲得零速區(qū)間開始時刻與結束時刻,其分析過程如下。

(1)

式中:L(zn) 表示對于zn的似然比,即對于每組zn,假設H1相對于假設H0的可能性。

然后,用未知參數(shù)的最大似然估計代替未知參數(shù)可以得到如下公式:

(2)

yk=sk(Θ)+vk

(3)

式中:

(4)

(5)

式中:Ωn={∈N:n≤≤n+N-1},通過對未知參數(shù)的最大似然估計代替未知參數(shù)可得到:

(6)

(7)

(8)

2 慣性行人導航系統(tǒng)

2.1 慣性行人導航系統(tǒng)誤差模型

基于捷聯(lián)慣導算法的行人導航系統(tǒng)在小擾動情況下,系統(tǒng)誤差模型[18]表示如下:

(9)

(10)

2.2 慣性行人導航系統(tǒng)軟件結構

慣性行人導航系統(tǒng)的軟件結構主要包括數(shù)據采集、數(shù)據預處理、零速檢測、捷聯(lián)慣導解算以及誤差修正等部分。慣性行人導航系統(tǒng)的軟件結構圖如圖2所示。

圖2 慣性行人導航系統(tǒng)軟件結構Fig.2 Software structure of inertial pedestrian navigation system

3 ZUPT、零角速率修正與航向角誤差非線性預測校正算法

常規(guī)慣性行人導航系統(tǒng)軟件結構中的誤差修正算法主要包括ZUPT算法與零角速率修正算法。本文針對慣性行人導航系統(tǒng)中存在的航向角不可觀測的問題,在ZUPT和零角速率修正算法的基礎上增設了航向角誤差非線性預測校正算法。該算法包括零速區(qū)間航向角誤差觀測模塊和非零速區(qū)間航向角誤差非線性預測模塊,將二者相結合定義出一個步態(tài)周期內的航向角誤差觀測量。利用卡爾曼濾波得到航向角誤差估計值,實現(xiàn)對航向角誤差進行修正。

3.1 卡爾曼濾波設計

根據捷聯(lián)慣導系統(tǒng)誤差模型,設計了一種12維狀態(tài)量的卡爾曼濾波器,連續(xù)系統(tǒng)狀態(tài)方程為

(11)

其中,

連續(xù)系統(tǒng)的觀測方程為

z(t)=Hx(t)+v(t)

(12)

對式(11)和式(12)進行離散化可得

Xk=Φk/k-1Xk-1+Γk/k-1Wk-1

(13)

Zk=HXk+Vk

(14)

其中,

3.2 ZUPT算法

當行人處于零速區(qū)間時,理論上足部的速度為零,但是在捷聯(lián)慣導解算過程中,由于加速度計存在不可避免的噪聲,實際在零速區(qū)間內解算出的速度不為零。因此,可以將零速區(qū)間內的速度誤差作為卡爾曼濾波器的觀測量,估計出捷聯(lián)慣性導航系統(tǒng)的誤差并及時對導航系統(tǒng)解算結果進行校正。ZUPT的觀測量如下:

(15)

3.3 零角速率修正算法

當行人處于零速區(qū)間時,足部處于靜止狀態(tài),此時IMU輸出角速率理論值為零。但陀螺儀存在常值零偏誤差和測量噪聲導致實際導航解算得到的足部角速率不為零。因此,可以將零速區(qū)間內的角速率誤差作為卡爾曼濾波器的觀測量,估計出捷聯(lián)慣性導航系統(tǒng)的誤差并及時對導航系統(tǒng)解算結果進行校正。零角速率修正的觀測量如下:

(16)

3.4 航向角修正算法

當行人處于零速區(qū)間時,由于腳部與地面保持靜止,此時IMU只受重力加速度的作用。基于航向角在零速區(qū)間內保持不變的特點,設計了零速區(qū)間航向角誤差觀測模塊和非零速區(qū)間航向角誤差非線性預測模塊,從而在不引入其他傳感器的情況下,實現(xiàn)慣性行人導航航向角的自觀測。

3.4.1 零速區(qū)間航向角誤差觀測模塊

當行人運動處于零速區(qū)間時,航向角具有保持不變的特性。因此,將航向角在零速區(qū)間內的變化視為由陀螺儀漂移引起的航向角誤差。將當前時刻采樣點的航向角相對于上一時刻采樣點的航向角的變化量作為當前時刻相比于上一時刻的航向角誤差。同時,為了應對干擾對計算航向角誤差的影響,利用滑動窗口平均濾波的原理對航向角誤差進行計算。

因此,本文提出了零速區(qū)間航向角誤差觀測模塊。在該模塊中,將當前采樣點航向角解算值與前兩個采樣點航向角平均值之差作為零速區(qū)間內該采樣時刻相比于上一采樣時刻的航向角誤差。具體公式如下:

(17)

3.4.2 非零速區(qū)間航向角誤差非線性預測模塊

零速區(qū)間航向角誤差觀測模塊無法準確計算非零速區(qū)間內捷聯(lián)慣導解算時刻的航向角誤差。針對此問題,本文設計了一種非零速區(qū)間航向角誤差非線性預測模塊,通過對上一個步態(tài)周期內零速區(qū)間的每一個捷聯(lián)慣導解算時刻的航向角相較于零速區(qū)間起始時刻航向角的誤差進行多項式擬合,得到航向角誤差增量擬合函數(shù)f(t)。本算法采用4階多項式擬合:

f(t)=at4+bt3+ct2+dt+e

(18)

某一步態(tài)周期零速區(qū)間內航向角誤差增量的擬合曲線如圖3所示。

圖3 航向角誤差擬合曲線Fig.3 Heading angle error fitting curve

利用非零速區(qū)間航向角誤差非線性預測模塊計算出的航向角誤差定義為

(19)

式中:T0表示當前所處步態(tài)周期內非零速區(qū)間的持續(xù)時間。則將零速區(qū)間航向角誤差觀測模塊與非零速區(qū)間航向角誤差非線性預測模塊相結合,即可定義此步態(tài)周期內零速區(qū)間航向角的誤差觀測量,具體公式為

(20)

對應的卡爾曼濾波觀測矩陣為

航向誤差非線性預測的慣性行人導航零速修正算法流程圖如圖4所示。

圖4 航向誤差非線性預測的慣性行人導航ZUPT算法流程圖Fig.4 Flow chart of ZUPT algorithm for inertial pedestrian navigation based on nonlinear prediction of heading error

4 實驗驗證

課題組所用MIMU慣性傳感器采用荷蘭Xsens公司生產的Mti-G-710系列MEMS慣性傳感器,其具有三軸加速度計和陀螺儀。加速度計零偏穩(wěn)定性為15 μg,陀螺儀零偏穩(wěn)定性為10°/g,加速度計的量程為±20g,陀螺儀的量程為±450°/s,Mti-G-710采集加速度及角速度數(shù)據的頻率設置為200 Hz。導航計算機操作系統(tǒng)為Windows 10,處理器型號為AMD Ryzen 7 5800H。MIMU安裝位置如圖5(a)所示,慣性行人導航系統(tǒng)組成如圖5(b)所示。

圖5 MIMU安裝位置及慣性行人導航系統(tǒng)組成Fig.5 Installation position of MIMU and composition of inertial pedestrian navigation system

為驗證本文算法的有效性,首先利用GLRT算法對零速區(qū)間進行檢測,再分別利用ZUPT算法及零角速率修正算法(簡稱算法1)、應用零速區(qū)間航向角誤差觀測模塊的ZUPT算法及零角速率修正算法(簡稱算法2)和應用零速區(qū)間航向角誤差觀測模塊及非零速區(qū)間航向角誤差非線性預測模塊的ZUPT算法及零角速率修正算法(簡稱算法3)對Mti-G-710采集到的慣性傳感器行人運動數(shù)據進行解算。

4.1 復雜運動算法有效性驗證

為驗證算法在復雜運動路線下的有效性,選擇非閉合曲線作為實驗基準路線且實驗人員多次進行90°轉向。實驗人員按照轉折點1至轉折點6的順序沿基準路線行走一圈。同時,取3種算法的解算路徑相對應的轉折點及終點,作為實際行走軌跡參考點在解算軌跡中的對應點,如圖6所示。

圖6 實驗路線圖Fig.6 Experimental roadmap

對應點與參考點的位置誤差定義為

(21)

式中:下標“C”表示對應點;下標“R”表示參考點。

利用導航軌跡誤差平均值ΔR與導航軌跡誤差標準差ΔS來量化3種算法的導航精度,二者的數(shù)值越小,導航解算軌跡越貼近實驗基準路線,算法的精度越高。導航軌跡誤差平均值計算方法如下所示:

(22)

式中:下標“t”表示轉折點;下標“e”表示終點;i=1,2,…,n-1分別代表轉折點1至轉折點n-1。n為轉折點和終點數(shù)量的總和,導航軌跡誤差標準差計算方式如下所示:

(23)

實驗結果如圖7所示,黑色實線為實驗基準路徑,藍色虛線是通過算法1解算后的路徑,紅色點劃線是通過算法2解算后的路徑,綠色點線是通過算法3解算后的路徑。

圖7 復雜運動狀態(tài)下的實驗路徑對比Fig.7 Path comparison of experiments under complex motion state

統(tǒng)計結果如表1所示,實驗的起點為同一點。算法1、算法2和算法3解算出的路徑與基準實驗路徑比較,計算得到的導航軌跡誤差平均值ΔR分別為3.04 m、1.19 m和0.43 m,導航誤差標準差ΔS分別為2.16 m、0.64 m和0.16 m。算法2相較于算法1導航軌跡誤差平均值和導航誤差標準差均有所減小,其中導航軌跡誤差平均值僅為總路程的0.95%。算法3在算法2的基礎上進一步減小了導航軌跡誤差平均值和導航誤差標準差,導航軌跡誤差平均值僅為總路程的0.35%。證明零速區(qū)間航向角誤差觀測模塊和非零速區(qū)間航向角誤差非線性預測模塊可以有效提高多次大幅度轉向運動下的導航精度。

表1 3種算法的導航精度

4.2 長距離長航時運動算法有效性驗證

為驗證航向角自觀測算法在長距離、長航時運動狀態(tài)下的可靠性,將實驗場地替換為標準400 m跑道,實驗人員按照最內環(huán)跑道逆時針行走一圈。400 m跑道衛(wèi)星圖如圖8所示。分別利用算法1、算法2和算法3對慣性傳感器采集到的數(shù)據進行解算,解算結果如圖9所示。其中,黑色實線為基準路線,藍色虛線是通過算法1解算后的路徑,紅色點劃線是通過算法2解算后的路徑,綠色點線是通過算法3解算后的路徑,統(tǒng)計結果如表2所示。可以看出,在不引入零速區(qū)間航向角誤差觀測模塊和非零速區(qū)間航向角誤差非線性預測模塊時,導航精度極差,導航失效。在引入零速區(qū)間航向角誤差觀測模塊后,導航精度有所提高,導航誤差為11.81%里程。引入非零速區(qū)間航向角誤差非線性預測模塊后,導航精度進一步提高,導航誤差為1.25%里程。證明航向誤差非線性預測的慣性行人導航ZUPT算法可以有效提高長距離、長航時運動下的導航精度。

表2 3種算法解算終點的相對位置誤差

圖8 400 m跑道衛(wèi)星圖Fig.8 Satellite map of 400 m runway

圖9 長距離長航時運動狀態(tài)下的實驗路徑對比Fig.9 Path comparison of experiments under long-distance and long endurance motion state

5 結 論

設計了零速區(qū)間航向角誤差觀測模塊與非零速區(qū)間航向角誤差非線性預測模塊。通過兩個模塊構建航向角誤差觀測量,與ZUPT算法、零角速率修正算法相結合,提高了卡爾曼濾波誤差估計的精度。通過實際行人導航系統(tǒng)驗證,在復雜運動狀態(tài)下,導航軌跡誤差平均值僅為0.43 m,只占總路程的0.35%。在長距離、長航時運動狀態(tài)下,導航誤差僅為1.25%里程。

本文提出的航向誤差非線性預測的慣性行人導航ZUPT算法,在不引入外部傳感器以及約束行人行走軌跡的條件下可以有效提高行人導航精度,具有良好的工程應用價值。