行人導航系統航向角約束算法研究
2018-11-21 03:35:00宋春雷陳家斌凌藝菲
導航定位與授時 2018年6期
余 佩，宋春雷，陳家斌，凌藝菲
(北京理工大學自動化學院，北京 100081)
0 引言
行人導航系統，又稱為單兵導航系統，是導航定位領域里的一個重要分支，近年來得到越來越多的研究，被廣泛應用于救援搶險、軍事作戰等各個領域[1]。目前，全球定位系統(Global Positioning System,GPS)室外導航技術的研究已經非常成熟且應用十分廣泛。然而由于GPS導航技術依賴于接收的衛星信號，在室內或被建筑物等遮擋的復雜環境中，衛星信號易受到干擾或遮蔽，定位導航信號將丟失或出現錯誤，不能很好地發揮其導航定位功能[2]。基于低成本慣性測量模塊(Interial Measurement Unit, IMU)的自包含傳感器行人導航系統不需要依靠外部設備，可以獨立自主地實現導航定位功能，同時具有體積小、質量小、便于攜帶等特點，因此有著很好地發展前景和研究價值[3]。
基于自包含傳感器的行人導航系統，利用可穿戴式慣性傳感器測量行人運動過程中的加速度、角速率信息，通過導航算法對所測量的數據進行處理和解算，實現了對行人的定位和導航。然而系統內部存在慣性器件，其誤差會隨時間累積，是造成導航系統誤差的主要原因。為了抑制導航誤差，一般加入零速修正技術作為輔助。加入零速修正的行人導航系統定位精度雖然會得到很大的提高，然而由于航向角不可觀測，無法估計出航向誤差，因此零速修正對于航向角誤差修正起不到很好的作用，航向角依舊會逐漸發散[4]。此種航向角漂移誤差在行人行走路線存在多次轉彎時會更加明顯。
針對行人導航系統存在的航向角漂移誤差問題，本文在捷聯慣導解算和零速修正的基礎上，加入磁力計作為輔助手段，補償航向角誤差；同時采用一種地圖線路匹配的算法對航向角誤差進行修正。在行人行走過程中，彎道部分采用磁力計作為參考航向角，對捷聯解算出的航向角進行誤差修正，直道部分采用地圖線路匹配算法輔助修正航向角誤差。最終得到的導航軌跡圖，航向角漂移得到抑制，位置誤差大幅減少，導航精度明顯提高。
1 基礎原理
1.1 坐標系定義
1.1.1 載體坐標系
載體坐標系(b系)與載體固連，原點O位于載體的重心處，三軸分別表示為xb、yb和zb。文中，行人導航系統的載體坐標系定義為前-右-下，即xb軸沿載體的縱軸指向前進的方向，yb軸沿載體橫軸指向自身右側，zb軸沿豎直方向指向底部并垂直于xb軸和yb軸構成的平面，構成右手正交坐標系。實驗設備選用的足綁式IMU的輸出為載體坐標系下的慣性數據。
1.1.2 導航坐標系
導航坐標系(n系)也叫作當地地理坐標系，是根據實際導航需要而選取的基準坐標系。本文中將北-東-地(N-E-D)正交坐標系選取為導航坐標系(xn-yn-zn)。坐標系原點位于慣性平臺原點在大地水平面上的投影，北軸指向真北方向，地軸沿著當地鉛錘方向指向地球內部，東軸水平向東且與北地兩軸構成右手正交坐標系。文中，導航解算結果選取導航坐標系為參考坐標系。
1.2 零速修正
1.2.1 零速區間檢測
根據行人運動學模型，行人行走的一個步態周期主要分為觸地期、站立期、離地期和擺動期4個階段[5]。當行人處于站立期時，速度本該為零，此階段也叫作零速區間，誤差的存在使得行人零速區間里的實際速度并不為零，因此，可以在零速區間將速度和角速度置零進行誤差補償[6]。
零速修正的前提和關鍵是零速區間的檢測。由于行人行走的步態呈現周期性變化，因此，可以按規律對陀螺儀輸出的角速度和加速度計輸出的加速度設置閾值來識別零速區間。
理想情況下，零速區間內行人角速度和水平方向加速度為零，而z軸方向上加速度應為g。實際上，受到干擾作用后，雖然輸出與理論值有一定的差別，但是輸出值仍然會保持在一定的閾值范圍內波動[7]。選取零速區間內特征最明顯的加速度模值，z軸加速度、角速度模值和y軸角速度，對它們設置閾值。
設在k時刻加速度計在三軸上的輸出分別為axk、ayk和azk，陀螺儀三軸上的輸出分別為ωxk、ωyk和ωzk，計算角速度和加速度模值為：
設置閾值判斷條件：
只有同時滿足C1、C2、C3、C4為1時，該時刻才可被判斷為零速區間[8]。
其中，閾值的設定與陀螺儀和加速度計的零偏噪聲的方差特性有關，因此，可以在初始靜止時測得的數據平均值上加入一定的擾動來獲得。這里閾值的設定需要足夠精確，過大會導致誤判，而過小會導致漏檢。
針對本文的行人導航系統，經過干擾測試后設置的閾值為thamin=8.5，thamax=11，thazmin=-11，thazmax=-8.5，thω=0.87和thωy=0.5。設定閾值處理后檢測出的零速區間如圖1所示，綠線表示最終得到的零速區間。
1.2.2 EKF零速修正技術
在檢測出的零速區間中，使用擴展卡爾曼濾波對導航的速度和角速度誤差進行估計和補償。系統狀態變量為
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)
零速修正后，行人的速度和角速度誤差得到補償，捷聯解算出的位置誤差也能得到修正。行人足綁IMU沿直線行走采集數據，經過零速修正后得到的速度如圖2所示。
經過修正后，速度誤差得到了很好的抑制，航向角誤差也得到了一定的修正，但是航向誤差仍然較大。得到的航向角如圖3所示。為了解決航向誤差修正的問題，提出了磁力計輔助修正算法和地圖線路匹配算法。
2 航向角誤差修正集成算法
2.1 磁力計輔助航向修正
可以計算出航向角：
其中，ψ0為磁偏角，可以通過查表得到。
在零速區間內，以磁力計測得的航向角作為參考理論航向角，捷聯解算航向角與磁力計測得航向角的差值作為擴展卡爾曼濾波的一個觀測量，記為δψ，得到系統的觀測方程為：
對應的觀測矩陣為
R=diag(0.032,0.012,0.012,0.012,0.032,0.032,0.032)
使用磁力計輔助補償航向角誤差，可以使得系統的航向角測量精度進一步提高，然而當磁干擾較大時，磁力計受影響較大，會使得航向角誤差得不到很好的補償。
2.2 地圖線路匹配約束航向
考慮到行人行走經常受到各種環境的約束，如行人行走的道路多以直線為主，許多建筑物都呈矩形等。在此情況下，當行人直線行走時,可以通過建筑方向、道路方向等基礎信息，為行人導航提供參考航向角，約束行人航向角的漂移誤差。
假設行人沿一個閉合矩形行走，可以根據簡易地圖獲取道路的方向來作為參考航向，矩形的四方向正東、正南、正西和正北分別代表著航向角90°、±180°、-90°和0°，分別記為ψB(i)，其中，i=1,2,3,4。
如圖4所示，行人每行走一步可以通過公式計算出一個航向ψs。
地圖線路匹配算法的基本步驟為：
1)判斷當前時刻是否為零速區間結束后的第一個采樣點，若是，表示此采樣點處行人開始下一步行走，將ψs作為實際航向角，否則將捷聯解算出的航向角作為實際航向角。
2)比較實際航向角與ψB(i)，如果|ψ-ψB(i)|
3)以實際航向與選擇的ψB做差作為擴展卡爾曼濾波器的一個觀測量，得到航向角的誤差估計量，對航向角誤差進行補償。
地圖線路匹配的初始航向角可以由磁力計測出的航向角確定。由于其在直線行走時對航向角約束明顯，因此，僅在行人直線行走時采用地圖線路匹配的算法輔助修正航向角。
3 實驗及結果
3.1 實驗設備
設計實驗驗證算法的可行性，實驗設備選取荷蘭Xsens公司研發的MTi-G-710 GNSS/INS，此設備重21g，是一個集全球導航衛星系統(Global Navigation Satellite System, GNSS)信號接收器和微型慣性測量單元(Miniature Inertial Measurement Unit, MIMU)于一體的微慣性測量系統,內部還包含了磁力計、氣壓計等輔助傳感器,具有體積小、質量小、便于拆卸等特點。其主要性能指標如表1所示。
表1 MTi-G-710性能指標
IMU安裝于鞋子后跟，設備通過USB與電腦相連，采樣頻率設置為100Hz，傳感器輸出信息經過USB接口傳輸到電腦，通過MT-Manager進行采集存儲。
3.2 實驗設計及結果
實驗地點選取在北京理工大學東操場，行人佩戴裝配沿著操場逆時針行走一圈，全長總計400m。在相同的環境下，分別使用捷聯解算加零速修正算法、磁力計輔助捷聯解算零速修正算法和地圖線路匹配/磁力計輔助的捷聯解算零速修正算法進行導航實驗。得到的結果如圖5所示。
以起始點作為原點，行人逆時針行走一圈之后回到起始點位置。從圖5中可以看出，僅對零速區間的速度和角速度誤差補償后得到的導航軌跡圖誤差較大，軌跡終點在(1.713，56.59)，位置誤差為56.93m，雖然軌跡圖整體形狀規則，但是航向角漂移明顯，嚴重偏離真實軌跡。加入磁力計輔助后，軌跡終點在(-3.03，-2.956)，位置誤差為4.23m，航向角誤差得到補償，航向漂移明顯減弱，導航精度顯著提高。最后加入地圖線路匹配算法，軌跡終點在(-0.85，-0.86)，位置誤差為1.2m，導航精度得到進一步提高。
4 結論
由于航向誤差不可觀測，捷聯慣導解算加零速修正技術對航向角漂移誤差起不到很好的抑制作用，航向角誤差依舊呈發散狀態。在零速區間內對速度和角速度誤差進行補償的基礎上，加入磁力計測量值解算出的航向角作為輔助，輔助修正航向角漂移誤差。考慮到磁力計容易受到外界磁干擾的影響，引入地圖線路匹配算法。根據道路方向具有規律性的特點，由簡易地圖分析確定出的行人行走線路方向作為參考航向修正航向角漂移誤差。最終的導航系統中，彎道部分采用磁力計輔助航向角修正，直道部分采用地圖線路匹配算法輔助航向角修正。并將算法應用到實際實驗中，實驗結果表明，磁力計輔助航向修正使得航向角漂移得到明顯的抑制，而磁力計加地圖線路匹配算法輔助航向修正使得航向誤差進一步減小，導航精度進一步提高，最終的導航位置誤差僅為1.2m，驗證了算法的可行性，取得了很好的導航效果。
