基于自適應(yīng)步長約束的行人導(dǎo)航系統(tǒng)誤差修正算法

路永樂，惠嘉威，楊 杰，羅 毅，修蔚然

（重慶郵電大學(xué) 智能傳感技術(shù)與微系統(tǒng)重慶市高校工程研究中心，重慶 400065）

行人導(dǎo)航系統(tǒng)可以在陌生環(huán)境中為行人提供導(dǎo)航定位信息[1]。在室外環(huán)境中，主要依靠衛(wèi)星定位系統(tǒng)實現(xiàn)行人導(dǎo)航系統(tǒng)，但是在高樓、森林等復(fù)雜環(huán)境中，衛(wèi)星信號會嚴(yán)重衰退，無法滿足導(dǎo)航需求[2]。而慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）具有隱蔽性高、抗干擾能力強等優(yōu)勢，因此行人慣性導(dǎo)航系統(tǒng)成為了主要研究方向[3]。

然而慣性測量單元（Inertial Measurement Unit,IMU）存在誤差，且導(dǎo)航誤差會隨時間迅速累積，導(dǎo)致INS 長航時定位精度無法滿足需求[4]。研究人員采用了多種補償方式解決此問題，常見的包括：使用磁力計修正航向誤差[5]，采用藍(lán)牙、WIFI、超帶寬等定位技術(shù)輔助IMU 定位[4]。但磁力計易受周圍環(huán)境影響而產(chǎn)生大量磁干擾，藍(lán)牙等定位技術(shù)無法滿足特殊環(huán)境需求。針對足綁式IMU 行人導(dǎo)航系統(tǒng)，通常采用零速修正(Zero Velocity Update,ZUPT)算法，將靜止時刻的速度作為誤差觀測量修正其他信息[6]，但僅能在行人靜止區(qū)間內(nèi)修正誤差。

針對以上問題，研究人員利用行人運動時足部特征進(jìn)行誤差修正：（1）利用多IMU 進(jìn)行約束。文獻(xiàn)[7]在行人足部安裝IMU 陣列并根據(jù)各IMU 數(shù)據(jù)誤差情況進(jìn)行選擇，剔除其中漂移誤差較大的數(shù)據(jù)，并使用剩余IMU 數(shù)據(jù)計算行人導(dǎo)航信息；文獻(xiàn)[8]根據(jù)腳尖與腳跟相對距離為常量建立約束方程，利用兩個IMU信息相互校正提高精度。（2）基于距離進(jìn)行約束。文獻(xiàn)[9]提出雙腳間位置始終處于一個半徑為雙足最大距離的球體空間內(nèi)，由此構(gòu)建不等式約束算法；文獻(xiàn)[10]利用行人運動過程中步長及抬腳高度作為約束條件，構(gòu)建雙足橢球約束模型修正導(dǎo)航誤差。（3）多傳感器信息融合，利用測距傳感器實時測量步長。文獻(xiàn)[11]在行人足部安裝超聲波模塊測量雙足間相對距離輔助雙足IMU 定位；文獻(xiàn)[12]將毫米波雷達(dá)安裝在行人足部，實時提供行人雙足相對位置信息。

上述方法通過引入多個IMU 或其它傳感器修正慣性導(dǎo)航系統(tǒng)誤差，可以有效提高系統(tǒng)精度，但增加了行人導(dǎo)航系統(tǒng)的復(fù)雜性和硬件成本，限制了其推廣應(yīng)用。基于以上問題，本文在固定距離約束的基礎(chǔ)上，提出基于自適應(yīng)步長約束的行人導(dǎo)航系統(tǒng)誤差修正算法，通過零速檢測劃分運動區(qū)間，在靜止與運動區(qū)間內(nèi)分別進(jìn)行零速修正與步長約束修正，并根據(jù)IMU 信息自適應(yīng)行人步長，提高算法穩(wěn)定性。該算法利用行人腳跟處的IMU 實現(xiàn)慣性定位，在不增加系統(tǒng)復(fù)雜性的基礎(chǔ)上，提高行人導(dǎo)航系統(tǒng)定位精度。

1 足綁式IMU 行人導(dǎo)航系統(tǒng)

1.1 行人足部運動特征

如圖1所示，行人行走過程中足部特征呈周期性變化。其中每個步態(tài)周期又分為運動區(qū)間和靜止區(qū)間[13]。一個完整的運動區(qū)間以腳跟離地為開始，依次經(jīng)歷腳尖離地、擺動、腳跟觸地，最后以腳尖觸地為結(jié)束。然后進(jìn)入靜止區(qū)間，足部與地面保持靜止，直到下一次腳跟離地。

圖1 足部運動特征Fig.1 Foot movement characteristics

行人的一個步態(tài)周期中，單側(cè)腳軌跡存在步長約束條件。如圖2所示，設(shè)行人跨步長為l，則單側(cè)腳的空間位置一定處于半徑l為的球體內(nèi)。若將連續(xù)兩次觸地點間距離差值記為Δd，則一定滿足Δd≤l。

圖2 足部軌跡模型Fig.2 Foot trajectory model

1.2 步長約束模型

足綁式慣性元件的行人導(dǎo)航系統(tǒng)，通過IMU 采集行人加速度與角速度信息，利用捷聯(lián)慣導(dǎo)算法解算得到位姿信息[14]。捷聯(lián)慣導(dǎo)微分方程如下所示：

設(shè)k-1時刻和k時刻導(dǎo)航系統(tǒng)的狀態(tài)向量分別為：

剛才還整齊列著的隊伍一下子散亂開來,大家涌進(jìn)倉庫。張連長望著知青們奔向倉庫的背影,束手無策地自語:“這老爺子,真添亂!”

式(4)表示任意時刻單側(cè)腳的運動軌跡必須滿足步長約束條件，通過利用帶約束條件的最小二乘法，可以將位置信息表示為：

利用拉格朗日乘數(shù)法求解式(6)，引入拉格朗日乘子λ，構(gòu)建拉格朗日函數(shù)：

對式(7)進(jìn)行求解：

最終得到行人當(dāng)前時刻位置信息為：

2 自適應(yīng)步長誤差修正

2.1 總體框架

圖3 總體框架Fig.3 Overall framework

2.2 零速修正

零速修正算法將靜止時的速度為零作為誤差觀測量，利用卡爾曼濾波修正導(dǎo)航誤差。常用的零速檢測有加速度方差法、加速度閾值法、角速度能量法等[16]。本文采用基于廣義似然比檢測法（Generalized Likelihood Ratio Test,GLRT）進(jìn)行零速檢測，該方法同時利用到加速度計信息和陀螺儀信息，檢測準(zhǔn)確度更高[17]。

如圖4所示，GLRT 檢測結(jié)果為高電平時表示為零速，檢測結(jié)果為低電平時表示為非零速。由于零速檢測存在誤判，如圖4 虛線區(qū)域所示，有部分零速點出現(xiàn)在非零速區(qū)域，導(dǎo)致導(dǎo)航解算結(jié)果錯誤，最終引起導(dǎo)航誤差。因此必須消除誤判情況，提高零速檢測準(zhǔn)確度。

圖4 GLRT 零速檢測結(jié)果Fig.4 Result of GLRT zero-speed detection

通過行人步態(tài)特征可知，行人每行進(jìn)一步，足部狀態(tài)都會分為靜止階段和運動階段，而每一種狀態(tài)都必須持續(xù)一段時間，才能轉(zhuǎn)變?yōu)榱硪环N狀態(tài)。因此設(shè)計基于時間閾值檢測的零速檢測算法：

(1)利用零速檢測算法對行人足部狀態(tài)分組，將運動階段和靜止階段分別定義為Cmove,m和Cstatic,s，其中m表示該運動階段內(nèi)包含的數(shù)據(jù)數(shù)量，s表示該靜止階段內(nèi)包含的數(shù)據(jù)數(shù)量。

(2)將各階段的數(shù)據(jù)數(shù)量與時間閾值ΔT進(jìn)行比較。若各階段的數(shù)據(jù)數(shù)量均大于時間閾值，則表示零速區(qū)間檢測正確；反之，若某一階段的數(shù)據(jù)數(shù)量小于時間閾值，則表明該階段檢測錯誤。

(3)對行人每一步的足部運動狀態(tài)結(jié)果都進(jìn)行時間閾值檢測，對誤檢區(qū)間進(jìn)行修正。

2.3 自適應(yīng)步長估計

根據(jù)1.2 節(jié)分析，步長約束模型首先需要得到行人的步長數(shù)據(jù)，而運動中的行人步長為一個動態(tài)變量。影響行人步長的因素有很多，例如行人的身高、腿長、體重、路面情況、運動狀態(tài)等。根據(jù)分析可知行人在運動過程中，邁步長度與垂直方向加速度信息之間具有很大的關(guān)聯(lián)性[18]。根據(jù)z軸加速度與邁步長度的關(guān)系建立自適應(yīng)步長估計模型：

式中，Sn為第n步邁步長度；分別為第n次步態(tài)周期中z 軸加速度的最大值與最小值；τ 為步長系數(shù)，該系數(shù)針對不同的行人及其運動狀態(tài)具有特定的值。

由1.2 節(jié)分析可知，當(dāng)前時刻的位置信息可以由式(10)得到。在此理論之上，根據(jù)IMU 數(shù)據(jù)識別步態(tài)，并根據(jù)自適應(yīng)步長估計模型計算當(dāng)前步長。將計算出的步長Sn帶入式(10)，得到帶有自適應(yīng)步長的位置信息：

式中，Δt為IMU 采樣周期。

3 實驗驗證與分析

本文選擇在重慶郵電大學(xué)香樟園五號樓進(jìn)行實驗驗證。設(shè)定起點坐標(biāo)為(0,0)，實驗人員沿矩形路線行走，通過RTS-820S 全站儀標(biāo)定，矩形的長為50 m，寬為10 m。行走路線的起點與終點坐標(biāo)重合，以終點定位誤差作為衡量算法效果的依據(jù)。實驗過程中將WT901BC 姿態(tài)儀固定在實驗人員右腳腳跟。

實驗(1)：實驗人員按實驗路線以1 m 的固定步長行走兩圈，行進(jìn)距離共計240 m，實驗結(jié)果如圖5所示，軌跡誤差結(jié)果如表1所示。圖5(a)為未步長約束的運動軌跡，其終點位置坐標(biāo)為(0.87 m，3.25 m)，閉環(huán)誤差為1.40%D；圖5(b)為使用步長約束的運動軌跡，其終點位置坐標(biāo)為(-1.05 m，-1.02 m)，閉環(huán)誤差為0.61%D，且圖5(b)更符合實際運動軌跡，導(dǎo)航系統(tǒng)精度得到提高。

圖5 運動軌跡Fig.5 Motion trajectory

表1 實驗方案誤差比較Tab.1 Error comparison of test schemes

實驗(2)：選取三名身高不同的實驗人員在一條長為24 m 的走廊上進(jìn)行三次行走實驗，通過比較三次實驗估計距離值與實際距離值之間的誤差關(guān)系，選取各實驗人員τ 的最優(yōu)值并驗證自適應(yīng)步長估計模型的準(zhǔn)確性，實驗結(jié)果如表2所示。表中跨步長指一側(cè)腳足跟著地到該側(cè)腳足跟再次著地所進(jìn)行的距離，即本文中單側(cè)腳單次運動行進(jìn)的距離。對比三次實驗結(jié)果可知，本文使用的自適應(yīng)步長估計模型估計的平均步長誤差為1.94%，能夠較為準(zhǔn)確地估計步長。

表2 自適應(yīng)步長模型誤差Tab.2 Error of adaptive step length estimation model

實驗(3)：三名實驗人員按實驗路線行走兩圈，單次實驗行進(jìn)距離為240 m。每次實驗包括零速修正、零速修正結(jié)合固定步長約束、零速修正結(jié)合自適應(yīng)步長約束三種情況，實驗結(jié)果如圖6-8所示，軌跡誤差結(jié)果如表3所示。對比圖6-8 中各行走軌跡以及表3數(shù)據(jù)可知，不使用步長約束的軌跡誤差較大，起終點間距離誤差平均值為2.50 m，閉環(huán)誤差平均值為1.04%D；使用固定步長約束的軌跡，起終點間距離誤差平均值為2.90 m，閉環(huán)誤差平均值為1.20%D，且實驗軌跡會出現(xiàn)不符合實際運動路線的情況；而使用自適應(yīng)步長約束算法修正后的起終點間距離誤差平均值下降至0.18 m，閉環(huán)誤差平均值下降至0.07%D，且實驗軌跡更符合實際運動路線。因此本文提出的算法可以有效提高導(dǎo)航系統(tǒng)定位精度。

圖6 實驗一運動軌跡Fig.6 The trajectory of experiment one

表3 自適應(yīng)步長約束實驗結(jié)果Tab.3 Experimental results of adaptive step size constraint

圖7 實驗二運動軌跡Fig.7 The trajectory of experiment two

圖8 實驗三運動軌跡Fig.8 The trajectory of experiment three

4 結(jié)論

本文針對基于足綁式慣性傳感器的行人導(dǎo)航系統(tǒng)誤差隨時間累積而導(dǎo)致定位精度下降的問題，提出基于自適應(yīng)步長約束的導(dǎo)航誤差修正算法。算法在行人足部靜止時進(jìn)行零速修正，在運動時通過自適應(yīng)步長約束修正誤差，最終提高行人導(dǎo)航系統(tǒng)的精度。針對零速修正、零速修正結(jié)合固定步長約束以及零速修正結(jié)合自適應(yīng)步長約束進(jìn)行了三組對比實驗，數(shù)據(jù)顯示本文提出的基于自適應(yīng)步長約束的行人導(dǎo)航系統(tǒng)誤差修正算法相比于只使用零速修正情況，終點與起點間距離誤差的平均值由2.50 m 下降至0.18 m，導(dǎo)航閉環(huán)誤差平均值由1.04%D下降至0.07%D；相比于零速修正結(jié)合固定步長約束算法修正，終點與起點間距離誤差的平均值由2.90 m 下降至0.18 m，導(dǎo)航閉環(huán)誤差平均值由1.20%D下降至0.07%D。實驗結(jié)果表明，本文提出的方案能夠有效提升導(dǎo)航精度，具有一定的工程應(yīng)用價值。