基于室內定位的改進PDR 算法研究

衛(wèi)慶芳，陳 勇，薛文軍，馬文秀，裴科科

（北方自動控制技術研究所，太原 030006）

0 引言

近年來，美國、中國等多個國家各自建立了衛(wèi)星導航系統(tǒng)（global navigation satellite system，GNSS），已經(jīng)具備實現(xiàn)區(qū)域、全球的定位導航能力。在室外應用環(huán)境下，其定位精度能夠達到3 m 左右［1］，基本能夠滿足人們的定位需求；在室內應用環(huán)境下，受限于建筑物遮蔽和導航定位芯片功率等原因，衛(wèi)星導航系統(tǒng)無法實現(xiàn)精準、連續(xù)、有效的室內定位。因此，提高室內應用環(huán)境下的定位精度是當前定位領域的研究熱點。

截止目前，國內外學者提出了多種室內定位技術，例如：基于藍牙［2］、UWB［3］、磁場［4］等室內定位技術。其中，基于藍牙、UWB 等定位技術需要提前布設配套基礎設置；基于磁場的定位技術需要離線建立離線定位信息并實時更新，定位成本高，難以推廣。而基于微型慣性測量單元（miniature inertial measurement unit，MIMU）的行人航跡推算（pedestrian dead reckoning，PDR）算法［5］能夠實現(xiàn)低成本無源定位，應用場景十分廣闊。行人航跡推算屬于遞推式定位方法，通過加速度計、磁力計、陀螺儀獲取連續(xù)變化的信號，解算得到當前位置信息，但定位解算是通過遞推累加實現(xiàn)的，這導致定位誤差隨時間持續(xù)增長，定位精度降低。為了提升PDR 算法的室內定位精度，文獻［6］提出了基于深度循環(huán)神經(jīng)網(wǎng)絡（recurrent neural network，RNN）模型預測行人步長，算法復雜，實現(xiàn)難度較大；文獻［7］以加速度傳感器獲取數(shù)據(jù)為依據(jù)，進行上下樓梯、乘坐電梯等特殊行動進行識別，進行地標匹配，但匹配精度不高；文獻［8］提出地磁與PDR 融合算法，但采集地磁信息繁瑣，無法適應大范圍室內環(huán)境。基于以上問題，本文利用最小二乘線性相位FIR 濾波器對加速度計采樣值進行濾波，利用改進Weinberg 模型進行步長估計，利用粒子濾波算法對單步定位進行優(yōu)化，提升室內定位精度。

1 PDR 算法

PDR 算法是在給定初始位置后，在每個步態(tài)周期進行解算，通過加速度計、陀螺儀、磁力計解算出行人的步長、步頻和航向信息，通過每一步的信息迭代計算，實現(xiàn)行人定位［9］，PDR 算法原理如圖1 所示。

圖1 PDR 算法原理圖Fig.1 Schematic diagram of PDR algorithm principle

行人的位置可由以下公式計算得到：

式中，i 表示步數(shù)，Li+1表示下一時刻行人步長；φi+1表示下一時刻單步行進方向；（Xi，Yi）表示當前時刻位置信息；（Xi+1，Yi+1）表示下一時刻位置信息［10］。PDR算法由步頻、步長、航向檢測3 部分組成。

1.1 步頻檢測

行人在行走過程中，左右腿交替擺動，由此加速度傳感器在三軸輸出的信號波形都類似于正弦周期性變化［11］，由于單軸加速度數(shù)據(jù)誤差較大，容易受到行人姿態(tài)、環(huán)境噪聲的影響，因此，以和加速度作為計步判決依據(jù)。和加速度波形圖如圖2所示。

圖2 和加速度波形圖Fig.2 Combined acceleration waveform

計算公式如下：

式中，a 表示和加速度值；ax、ay、az分別表示x 軸、y軸、z 軸的加速度值。

1.2 步長檢測

對于行人來說，步長數(shù)據(jù)常常與行人步頻息息相關，本文采用Weinberg 模型進行步長估計。計算公式如下：

式中，Li表示對行人在第i 步時的步幅估算；amax、amin分別表示行人在第i 步時的加速度極值；K 表示該算式的擬合系數(shù)，是需要利用實際數(shù)據(jù)測算的經(jīng)驗值。參數(shù)K 的計算模型如下：

式中，L 表示每次測試中行人行走的距離；N 表示每次測試中行人行走的步數(shù)；Sˉ表示每次試驗中行人的平均步長；Si表示第i 步加速度極值差相關數(shù)；Ki第i 步的參數(shù)K 值；Kˉ表示參數(shù)K 的均值。該模型輸入數(shù)據(jù)易獲取，計算模型簡單，計算結果高效可靠。

1.3 航向估計

根據(jù)傳感器采集到的數(shù)據(jù)特性，加速度計和磁力計可以獲取絕對方向，隨著使用時間延長不存在累積誤差，但易受到環(huán)境噪聲的影響，數(shù)據(jù)波動較大，穩(wěn)定性不高；陀螺儀恰好與之相反，在短期內可以獲得極高的航向精度，但隨著時間的延長，誤差不斷累積，航向精度逐漸下降。因此，以加速度計和磁力計測算的姿態(tài)角作為卡爾曼濾波的觀測值，陀螺儀測算姿態(tài)角作為卡爾曼濾波的預測值，通過卡爾曼濾波融合航向測算，降低噪聲影響，提升航向精度?？柭鼮V波［12］預測與更新校驗公式描述如下：

卡爾曼濾波方程是由狀態(tài)預測方程及更新校驗方程組成。

狀態(tài)預測方程包括：

1）預測當前的狀態(tài)

式中，狀態(tài)預測方程能夠提供下一時刻的先驗估計值，更新校驗方程能夠提供后驗估計值。

2 改進的PDR 算法

2.1 步頻檢測改進

由于實際加速度計采樣值中存在高頻噪聲，因此，設計采樣頻率50 Hz，濾波器階數(shù)10 階，歸一化頻率向量［0，0.040 0，0.060 0，1.000 0］，振幅向量［1，1，0，0］的最小二乘線性相位FIR 濾波器進行濾波。濾波效果如圖3 所示。

圖3 最小二乘線性相位DIR 濾波圖Fig.3 Least squares linear phase DIR filter diagram

濾波后通過過零檢測法或相鄰波峰- 波谷檢測法［13］都可較高精度地檢測步頻信息。

2.2 步長檢測改進

由于PDR 算法的遞歸性，行人的單步步長估計尤為重要。本文對傳統(tǒng)Weinberg 模型進行改進，引入加速度相鄰波谷差（單步時間）作為影響因子，改進方程如下：

式中，Li表示對行人在第i 步時的步幅估算；hi、hi+1分別表示行人在第i 步和第i+1 步波谷時間；amax、amin分別表示行人在第i 步時的和加速度極值；A、B、C 表示該算式的擬合系數(shù)。

為了驗證改進的步長估計算法提升了定位精度，仿真計算步長誤差進行對比，計算公式如下：

2.3 單步定位優(yōu)化

通過遞推的方式解算行人位置信息存在較大的累積誤差，因此，通過粒子濾波算法對定位結果進行優(yōu)化，提升定位精度［14］。

粒子的初始化：

對粒子權重進行歸一化處理：

進行多次濾波迭代后，粒子群發(fā)生粒子匱乏現(xiàn)象，由于權重的變化，粒子間的權值方差逐步增大，方差較低的粒子變成無效粒子，這些粒子在后續(xù)的計算中幾乎沒有什么作用，還會降低濾波的運算效能［15］。為了確保粒子的有效性，設立重采樣判斷準則：

當neff

3 實驗結果與分析

3.1 計步檢測實驗

實驗數(shù)據(jù)由型號BTW901CL 的九軸MIMU 進行采集，內置三軸加速度計、三軸磁力計、三軸陀螺儀，采樣頻率10 Hz，采集到的數(shù)據(jù)通過藍牙進行傳輸，導入MATLAB 進行仿真實驗。如圖4～圖5 所示。

圖4 傳感器采集實驗圖Fig.4 Sensor acquisition experiment picture

圖5 傳感器采集數(shù)據(jù)圖Fig 5 Sensor acquisition data graph

實驗方式為：3 名身高不同的實驗對象沿直線行走100 m，人工計數(shù)實際行走步數(shù)，實驗結果如表1所示。

表1 實際步數(shù)與測算步數(shù)對比表Table 1 Comparison table of actual and measured numbers of steps

經(jīng)實驗測試發(fā)現(xiàn)，計步準確度達到98%。

3.2 步長檢測實驗

由3 位實驗者多次實驗后，利用最小二乘擬合獲得參數(shù)A 為0.402 ；參數(shù)B 為0.000 162；參數(shù)C為0.188 6。由3 位實驗者分別沿地面鋪設瓷磚（60*60）直線行走30 m，確保單步步長一致，MATLAB 分別仿真Weinberg 模型和改進步長檢測模型。實驗結果如下頁圖6～圖8 所示。

圖6 實驗對象A 步長檢測模型累積誤差對比圖Fig.6 Cumulative error comparison diagram of step length detection model for experimental subject A

圖7 實驗對象B 步長檢測模型累積誤差對比圖Fig.7 Cumulative error comparison diagram of step length detection model for experimental subject B

圖8 實驗對象C 步長檢測模型累積誤差對比圖Fig.8 Cumulative error comparison diagram of step length detection model for experimental subject C

由圖6～圖8 對比可知，改進后的步長檢測模型累積誤差更低。

3.3 定位檢測實驗

實驗1：選取實驗對象C，向東沿直線行走100 m，MATLAB 分別仿真原PDR 算法與改進PDR 算法。實驗對比結果如圖9～圖10 所示。

圖9 直行定位結果對比圖Fig.9 Positioning results comparison diagram of straight walking

圖10 直行定位結果對比細節(jié)圖Fig.10 Detail comparison diagram of positioning results of straight walking

由圖9、圖10 對比可得，改進后的PDR 算法定位結果比原PDR 算法定位結果單步波動更小，軌跡更平滑，改進后航跡偏移也更?。ㄌ摼€為真實路徑）。

實驗2：選取實驗對象A，沿矩形路徑（10*10）行走一周，MATLAB 分別仿真原PDR 算法與改進PDR 算法。實驗對比結果如圖11 所示。

圖11 步行一周定位結果對比圖Fig.11 Comparison diagram of positioning results after walking along a rectangle

由圖11 對比可得，改進后的PDR 算法定位結果比原PDR 算法定位結果更接近圖上虛線矩形框（真實路徑），改進后航跡偏移更小。

4 結論

本文在分析PDR 算法原理的基礎上，提出基于步頻檢測、步長估計、單步優(yōu)化的PDR 改進算法。針對加速度計采樣存在的高頻噪聲這一問題，設計最小二乘線性相位FIR 濾波器對數(shù)據(jù)進行處理，獲得有效步頻波形；針對單步步長估計誤差較大這一問題，對傳統(tǒng)Weinberg 模型進行改進，將加速度極值差和單步時間差共同作為步長估計相關參數(shù)，提升步長估計精度；針對遞推法導致累積誤差較大這一問題，利用粒子濾波算法提升單步定位精度。實驗數(shù)據(jù)基于微型慣性測量單元MIMU 采集獲得，實驗仿真基于MATLAB 平臺進行，實驗結果表明改進后的PDR 算法在定位精度上優(yōu)于原始PDR 算法。