行人導(dǎo)航系統(tǒng)航向角約束算法研究

余 佩，宋春雷，陳家斌，凌藝菲

(北京理工大學(xué)自動(dòng)化學(xué)院，北京 100081)

0 引言

行人導(dǎo)航系統(tǒng)，又稱為單兵導(dǎo)航系統(tǒng)，是導(dǎo)航定位領(lǐng)域里的一個(gè)重要分支，近年來(lái)得到越來(lái)越多的研究，被廣泛應(yīng)用于救援搶險(xiǎn)、軍事作戰(zhàn)等各個(gè)領(lǐng)域[1]。目前，全球定位系統(tǒng)(Global Positioning System,GPS)室外導(dǎo)航技術(shù)的研究已經(jīng)非常成熟且應(yīng)用十分廣泛。然而由于GPS導(dǎo)航技術(shù)依賴于接收的衛(wèi)星信號(hào)，在室內(nèi)或被建筑物等遮擋的復(fù)雜環(huán)境中，衛(wèi)星信號(hào)易受到干擾或遮蔽，定位導(dǎo)航信號(hào)將丟失或出現(xiàn)錯(cuò)誤，不能很好地發(fā)揮其導(dǎo)航定位功能[2]。基于低成本慣性測(cè)量模塊(Interial Measurement Unit, IMU)的自包含傳感器行人導(dǎo)航系統(tǒng)不需要依靠外部設(shè)備，可以獨(dú)立自主地實(shí)現(xiàn)導(dǎo)航定位功能，同時(shí)具有體積小、質(zhì)量小、便于攜帶等特點(diǎn)，因此有著很好地發(fā)展前景和研究?jī)r(jià)值[3]。

基于自包含傳感器的行人導(dǎo)航系統(tǒng)，利用可穿戴式慣性傳感器測(cè)量行人運(yùn)動(dòng)過(guò)程中的加速度、角速率信息，通過(guò)導(dǎo)航算法對(duì)所測(cè)量的數(shù)據(jù)進(jìn)行處理和解算，實(shí)現(xiàn)了對(duì)行人的定位和導(dǎo)航。然而系統(tǒng)內(nèi)部存在慣性器件，其誤差會(huì)隨時(shí)間累積，是造成導(dǎo)航系統(tǒng)誤差的主要原因。為了抑制導(dǎo)航誤差，一般加入零速修正技術(shù)作為輔助。加入零速修正的行人導(dǎo)航系統(tǒng)定位精度雖然會(huì)得到很大的提高，然而由于航向角不可觀測(cè)，無(wú)法估計(jì)出航向誤差，因此零速修正對(duì)于航向角誤差修正起不到很好的作用，航向角依舊會(huì)逐漸發(fā)散[4]。此種航向角漂移誤差在行人行走路線存在多次轉(zhuǎn)彎時(shí)會(huì)更加明顯。

針對(duì)行人導(dǎo)航系統(tǒng)存在的航向角漂移誤差問(wèn)題，本文在捷聯(lián)慣導(dǎo)解算和零速修正的基礎(chǔ)上，加入磁力計(jì)作為輔助手段，補(bǔ)償航向角誤差；同時(shí)采用一種地圖線路匹配的算法對(duì)航向角誤差進(jìn)行修正。在行人行走過(guò)程中，彎道部分采用磁力計(jì)作為參考航向角，對(duì)捷聯(lián)解算出的航向角進(jìn)行誤差修正，直道部分采用地圖線路匹配算法輔助修正航向角誤差。最終得到的導(dǎo)航軌跡圖，航向角漂移得到抑制，位置誤差大幅減少，導(dǎo)航精度明顯提高。

1 基礎(chǔ)原理

1.1 坐標(biāo)系定義

1.1.1 載體坐標(biāo)系

載體坐標(biāo)系(b系)與載體固連，原點(diǎn)O位于載體的重心處，三軸分別表示為xb、yb和zb。文中，行人導(dǎo)航系統(tǒng)的載體坐標(biāo)系定義為前-右-下，即xb軸沿載體的縱軸指向前進(jìn)的方向，yb軸沿載體橫軸指向自身右側(cè)，zb軸沿豎直方向指向底部并垂直于xb軸和yb軸構(gòu)成的平面，構(gòu)成右手正交坐標(biāo)系。實(shí)驗(yàn)設(shè)備選用的足綁式IMU的輸出為載體坐標(biāo)系下的慣性數(shù)據(jù)。

1.1.2 導(dǎo)航坐標(biāo)系

導(dǎo)航坐標(biāo)系(n系)也叫作當(dāng)?shù)氐乩碜鴺?biāo)系，是根據(jù)實(shí)際導(dǎo)航需要而選取的基準(zhǔn)坐標(biāo)系。本文中將北-東-地(N-E-D)正交坐標(biāo)系選取為導(dǎo)航坐標(biāo)系(xn-yn-zn)。坐標(biāo)系原點(diǎn)位于慣性平臺(tái)原點(diǎn)在大地水平面上的投影，北軸指向真北方向，地軸沿著當(dāng)?shù)劂U錘方向指向地球內(nèi)部，東軸水平向東且與北地兩軸構(gòu)成右手正交坐標(biāo)系。文中，導(dǎo)航解算結(jié)果選取導(dǎo)航坐標(biāo)系為參考坐標(biāo)系。

1.2 零速修正

1.2.1 零速區(qū)間檢測(cè)

根據(jù)行人運(yùn)動(dòng)學(xué)模型，行人行走的一個(gè)步態(tài)周期主要分為觸地期、站立期、離地期和擺動(dòng)期4個(gè)階段[5]。當(dāng)行人處于站立期時(shí)，速度本該為零，此階段也叫作零速區(qū)間，誤差的存在使得行人零速區(qū)間里的實(shí)際速度并不為零，因此，可以在零速區(qū)間將速度和角速度置零進(jìn)行誤差補(bǔ)償[6]。

零速修正的前提和關(guān)鍵是零速區(qū)間的檢測(cè)。由于行人行走的步態(tài)呈現(xiàn)周期性變化，因此，可以按規(guī)律對(duì)陀螺儀輸出的角速度和加速度計(jì)輸出的加速度設(shè)置閾值來(lái)識(shí)別零速區(qū)間。

理想情況下，零速區(qū)間內(nèi)行人角速度和水平方向加速度為零，而z軸方向上加速度應(yīng)為g。實(shí)際上，受到干擾作用后，雖然輸出與理論值有一定的差別，但是輸出值仍然會(huì)保持在一定的閾值范圍內(nèi)波動(dòng)[7]。選取零速區(qū)間內(nèi)特征最明顯的加速度模值，z軸加速度、角速度模值和y軸角速度，對(duì)它們?cè)O(shè)置閾值。

設(shè)在k時(shí)刻加速度計(jì)在三軸上的輸出分別為axk、ayk和azk，陀螺儀三軸上的輸出分別為ωxk、ωyk和ωzk，計(jì)算角速度和加速度模值為：

設(shè)置閾值判斷條件：

只有同時(shí)滿足C1、C2、C3、C4為1時(shí)，該時(shí)刻才可被判斷為零速區(qū)間[8]。

其中，閾值的設(shè)定與陀螺儀和加速度計(jì)的零偏噪聲的方差特性有關(guān)，因此，可以在初始靜止時(shí)測(cè)得的數(shù)據(jù)平均值上加入一定的擾動(dòng)來(lái)獲得。這里閾值的設(shè)定需要足夠精確，過(guò)大會(huì)導(dǎo)致誤判，而過(guò)小會(huì)導(dǎo)致漏檢。

針對(duì)本文的行人導(dǎo)航系統(tǒng)，經(jīng)過(guò)干擾測(cè)試后設(shè)置的閾值為thamin=8.5，thamax=11，thazmin=-11，thazmax=-8.5，thω=0.87和thωy=0.5。設(shè)定閾值處理后檢測(cè)出的零速區(qū)間如圖1所示，綠線表示最終得到的零速區(qū)間。

1.2.2 EKF零速修正技術(shù)

在檢測(cè)出的零速區(qū)間中，使用擴(kuò)展卡爾曼濾波對(duì)導(dǎo)航的速度和角速度誤差進(jìn)行估計(jì)和補(bǔ)償。系統(tǒng)狀態(tài)變量為

Q=diag(0.012,0.012,0.012,0,0,0,0.012,0.012,0.012,0.032,0.032,0.032)

R=diag(0.012,0.012,0.012,0.032,0.032,0.032)

零速修正后，行人的速度和角速度誤差得到補(bǔ)償，捷聯(lián)解算出的位置誤差也能得到修正。行人足綁IMU沿直線行走采集數(shù)據(jù)，經(jīng)過(guò)零速修正后得到的速度如圖2所示。

經(jīng)過(guò)修正后，速度誤差得到了很好的抑制，航向角誤差也得到了一定的修正，但是航向誤差仍然較大。得到的航向角如圖3所示。為了解決航向誤差修正的問(wèn)題，提出了磁力計(jì)輔助修正算法和地圖線路匹配算法。

2 航向角誤差修正集成算法

2.1 磁力計(jì)輔助航向修正

可以計(jì)算出航向角：

其中，ψ0為磁偏角，可以通過(guò)查表得到。

在零速區(qū)間內(nèi)，以磁力計(jì)測(cè)得的航向角作為參考理論航向角，捷聯(lián)解算航向角與磁力計(jì)測(cè)得航向角的差值作為擴(kuò)展卡爾曼濾波的一個(gè)觀測(cè)量，記為δψ，得到系統(tǒng)的觀測(cè)方程為：

對(duì)應(yīng)的觀測(cè)矩陣為

R=diag(0.032,0.012,0.012,0.012,0.032,0.032,0.032)

使用磁力計(jì)輔助補(bǔ)償航向角誤差，可以使得系統(tǒng)的航向角測(cè)量精度進(jìn)一步提高，然而當(dāng)磁干擾較大時(shí)，磁力計(jì)受影響較大，會(huì)使得航向角誤差得不到很好的補(bǔ)償。

2.2 地圖線路匹配約束航向

考慮到行人行走經(jīng)常受到各種環(huán)境的約束，如行人行走的道路多以直線為主，許多建筑物都呈矩形等。在此情況下，當(dāng)行人直線行走時(shí),可以通過(guò)建筑方向、道路方向等基礎(chǔ)信息，為行人導(dǎo)航提供參考航向角，約束行人航向角的漂移誤差。

假設(shè)行人沿一個(gè)閉合矩形行走，可以根據(jù)簡(jiǎn)易地圖獲取道路的方向來(lái)作為參考航向，矩形的四方向正東、正南、正西和正北分別代表著航向角90°、±180°、-90°和0°，分別記為ψB(i)，其中，i=1,2,3,4。

如圖4所示，行人每行走一步可以通過(guò)公式計(jì)算出一個(gè)航向ψs。

地圖線路匹配算法的基本步驟為：

1)判斷當(dāng)前時(shí)刻是否為零速區(qū)間結(jié)束后的第一個(gè)采樣點(diǎn)，若是，表示此采樣點(diǎn)處行人開(kāi)始下一步行走，將ψs作為實(shí)際航向角，否則將捷聯(lián)解算出的航向角作為實(shí)際航向角。

2)比較實(shí)際航向角與ψB(i)，如果|ψ-ψB(i)|

360°-thψ，則選擇其為參考航向ψB；否則i=i+1，比較下一個(gè)方位角ψB(i)。

3)以實(shí)際航向與選擇的ψB做差作為擴(kuò)展卡爾曼濾波器的一個(gè)觀測(cè)量，得到航向角的誤差估計(jì)量，對(duì)航向角誤差進(jìn)行補(bǔ)償。

地圖線路匹配的初始航向角可以由磁力計(jì)測(cè)出的航向角確定。由于其在直線行走時(shí)對(duì)航向角約束明顯，因此，僅在行人直線行走時(shí)采用地圖線路匹配的算法輔助修正航向角。

3 實(shí)驗(yàn)及結(jié)果

3.1 實(shí)驗(yàn)設(shè)備

設(shè)計(jì)實(shí)驗(yàn)驗(yàn)證算法的可行性，實(shí)驗(yàn)設(shè)備選取荷蘭Xsens公司研發(fā)的MTi-G-710 GNSS/INS，此設(shè)備重21g，是一個(gè)集全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)信號(hào)接收器和微型慣性測(cè)量單元(Miniature Inertial Measurement Unit, MIMU)于一體的微慣性測(cè)量系統(tǒng),內(nèi)部還包含了磁力計(jì)、氣壓計(jì)等輔助傳感器,具有體積小、質(zhì)量小、便于拆卸等特點(diǎn)。其主要性能指標(biāo)如表1所示。

表1 MTi-G-710性能指標(biāo)

IMU安裝于鞋子后跟，設(shè)備通過(guò)USB與電腦相連，采樣頻率設(shè)置為100Hz，傳感器輸出信息經(jīng)過(guò)USB接口傳輸?shù)诫娔X，通過(guò)MT-Manager進(jìn)行采集存儲(chǔ)。

3.2 實(shí)驗(yàn)設(shè)計(jì)及結(jié)果

實(shí)驗(yàn)地點(diǎn)選取在北京理工大學(xué)東操場(chǎng)，行人佩戴裝配沿著操場(chǎng)逆時(shí)針行走一圈，全長(zhǎng)總計(jì)400m。在相同的環(huán)境下，分別使用捷聯(lián)解算加零速修正算法、磁力計(jì)輔助捷聯(lián)解算零速修正算法和地圖線路匹配/磁力計(jì)輔助的捷聯(lián)解算零速修正算法進(jìn)行導(dǎo)航實(shí)驗(yàn)。得到的結(jié)果如圖5所示。

以起始點(diǎn)作為原點(diǎn)，行人逆時(shí)針行走一圈之后回到起始點(diǎn)位置。從圖5中可以看出，僅對(duì)零速區(qū)間的速度和角速度誤差補(bǔ)償后得到的導(dǎo)航軌跡圖誤差較大，軌跡終點(diǎn)在(1.713，56.59)，位置誤差為56.93m，雖然軌跡圖整體形狀規(guī)則，但是航向角漂移明顯，嚴(yán)重偏離真實(shí)軌跡。加入磁力計(jì)輔助后，軌跡終點(diǎn)在(-3.03，-2.956)，位置誤差為4.23m，航向角誤差得到補(bǔ)償，航向漂移明顯減弱，導(dǎo)航精度顯著提高。最后加入地圖線路匹配算法，軌跡終點(diǎn)在(-0.85，-0.86)，位置誤差為1.2m，導(dǎo)航精度得到進(jìn)一步提高。

4 結(jié)論

由于航向誤差不可觀測(cè)，捷聯(lián)慣導(dǎo)解算加零速修正技術(shù)對(duì)航向角漂移誤差起不到很好的抑制作用，航向角誤差依舊呈發(fā)散狀態(tài)。在零速區(qū)間內(nèi)對(duì)速度和角速度誤差進(jìn)行補(bǔ)償?shù)幕A(chǔ)上，加入磁力計(jì)測(cè)量值解算出的航向角作為輔助，輔助修正航向角漂移誤差。考慮到磁力計(jì)容易受到外界磁干擾的影響，引入地圖線路匹配算法。根據(jù)道路方向具有規(guī)律性的特點(diǎn)，由簡(jiǎn)易地圖分析確定出的行人行走線路方向作為參考航向修正航向角漂移誤差。最終的導(dǎo)航系統(tǒng)中，彎道部分采用磁力計(jì)輔助航向角修正，直道部分采用地圖線路匹配算法輔助航向角修正。并將算法應(yīng)用到實(shí)際實(shí)驗(yàn)中，實(shí)驗(yàn)結(jié)果表明，磁力計(jì)輔助航向修正使得航向角漂移得到明顯的抑制，而磁力計(jì)加地圖線路匹配算法輔助航向修正使得航向誤差進(jìn)一步減小，導(dǎo)航精度進(jìn)一步提高，最終的導(dǎo)航位置誤差僅為1.2m，驗(yàn)證了算法的可行性，取得了很好的導(dǎo)航效果。