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

(海軍航空大學(xué)航空基礎(chǔ)學(xué)院, 山東 煙臺(tái) 264001)

0 引 言

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

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

1 零速檢測(cè)算法

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

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

本文采用基于GLRT算法獲得零速區(qū)間開(kāi)始時(shí)刻與結(jié)束時(shí)刻,其分析過(guò)程如下。

(1)

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

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

(2)

yk=sk(Θ)+vk

(3)

式中:

(4)

(5)

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

(6)

(7)

(8)

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

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

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

(9)

(10)

2.2 慣性行人導(dǎo)航系統(tǒng)軟件結(jié)構(gòu)

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

圖2 慣性行人導(dǎo)航系統(tǒng)軟件結(jié)構(gòu)Fig.2 Software structure of inertial pedestrian navigation system

3 ZUPT、零角速率修正與航向角誤差非線性預(yù)測(cè)校正算法

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

3.1 卡爾曼濾波設(shè)計(jì)

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

(11)

其中,

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

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

(12)

對(duì)式(11)和式(12)進(jìn)行離散化可得

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

(13)

Zk=HXk+Vk

(14)

其中,

3.2 ZUPT算法

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

(15)

3.3 零角速率修正算法

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

(16)

3.4 航向角修正算法

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

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

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

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

(17)

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

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

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

(18)

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

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

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

(19)

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

(20)

對(duì)應(yīng)的卡爾曼濾波觀測(cè)矩陣為

航向誤差非線性預(yù)測(cè)的慣性行人導(dǎo)航零速修正算法流程圖如圖4所示。

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

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

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

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

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

4.1 復(fù)雜運(yùn)動(dòng)算法有效性驗(yàn)證

為驗(yàn)證算法在復(fù)雜運(yùn)動(dòng)路線下的有效性,選擇非閉合曲線作為實(shí)驗(yàn)基準(zhǔn)路線且實(shí)驗(yàn)人員多次進(jìn)行90°轉(zhuǎn)向。實(shí)驗(yàn)人員按照轉(zhuǎn)折點(diǎn)1至轉(zhuǎn)折點(diǎn)6的順序沿基準(zhǔn)路線行走一圈。同時(shí),取3種算法的解算路徑相對(duì)應(yīng)的轉(zhuǎn)折點(diǎn)及終點(diǎn),作為實(shí)際行走軌跡參考點(diǎn)在解算軌跡中的對(duì)應(yīng)點(diǎn),如圖6所示。

圖6 實(shí)驗(yàn)路線圖Fig.6 Experimental roadmap

對(duì)應(yīng)點(diǎn)與參考點(diǎn)的位置誤差定義為

(21)

式中:下標(biāo)“C”表示對(duì)應(yīng)點(diǎn);下標(biāo)“R”表示參考點(diǎn)。

利用導(dǎo)航軌跡誤差平均值ΔR與導(dǎo)航軌跡誤差標(biāo)準(zhǔn)差ΔS來(lái)量化3種算法的導(dǎo)航精度,二者的數(shù)值越小,導(dǎo)航解算軌跡越貼近實(shí)驗(yàn)基準(zhǔn)路線,算法的精度越高。導(dǎo)航軌跡誤差平均值計(jì)算方法如下所示:

(22)

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

(23)

實(shí)驗(yàn)結(jié)果如圖7所示,黑色實(shí)線為實(shí)驗(yàn)基準(zhǔn)路徑,藍(lán)色虛線是通過(guò)算法1解算后的路徑,紅色點(diǎn)劃線是通過(guò)算法2解算后的路徑,綠色點(diǎn)線是通過(guò)算法3解算后的路徑。

圖7 復(fù)雜運(yùn)動(dòng)狀態(tài)下的實(shí)驗(yàn)路徑對(duì)比Fig.7 Path comparison of experiments under complex motion state

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

表1 3種算法的導(dǎo)航精度

4.2 長(zhǎng)距離長(zhǎng)航時(shí)運(yùn)動(dòng)算法有效性驗(yàn)證

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

表2 3種算法解算終點(diǎn)的相對(duì)位置誤差

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

圖9 長(zhǎng)距離長(zhǎng)航時(shí)運(yùn)動(dòng)狀態(tài)下的實(shí)驗(yàn)路徑對(duì)比Fig.9 Path comparison of experiments under long-distance and long endurance motion state

5 結(jié) 論

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

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