胡 銳 吳 飛 通雁輝 朱 海 鄢 松 陸雯霞

1 上海工程技術(shù)大學(xué)電子電氣工程學(xué)院，上海市龍騰路333號，201620

室外環(huán)境中的行人導(dǎo)航研究以衛(wèi)星導(dǎo)航或全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)/慣性導(dǎo)航(INS)組合導(dǎo)航為主，近年取得了較好的成果[1]。對于行人定位導(dǎo)航技術(shù)的研究主要分為兩類：一類是利用外部部署設(shè)備定位，主要基于室內(nèi)超寬帶(UWB)、Wi-Fi(wireless fidelity)及藍(lán)牙等無線射頻設(shè)備；另一類則是以慣性傳感器定位，主要基于慣性導(dǎo)航技術(shù)等。目前，已有較多針對步數(shù)估計(jì)[2-9]、步長估計(jì)[10-12]及航向角估計(jì)[13-16]等方面的研究，并取得了不錯的進(jìn)展。

為提高行人在有磁場干擾環(huán)境中軌跡跟蹤的準(zhǔn)確性，本文在步數(shù)估計(jì)階段采用雙重閾值二值化算法提高計(jì)步的準(zhǔn)確性；同時(shí)在航向估計(jì)階段，利用自適應(yīng)融合算法對加速度數(shù)據(jù)、角速度數(shù)據(jù)及磁力計(jì)數(shù)據(jù)融合得出更精準(zhǔn)的航向；最后設(shè)計(jì)在有磁干擾和無磁干擾環(huán)境中進(jìn)行對比實(shí)驗(yàn)，證明本文算法在航跡跟蹤上的有效性。

1 行人位置推算

行人航位推算時(shí)通過計(jì)步檢測、步長估計(jì)、航向估計(jì)3個(gè)步驟完成用戶位置的更新，并據(jù)此繪制出移動軌跡。在計(jì)步檢測中，通常利用三軸加速度信息檢測步數(shù)，當(dāng)檢測到用戶移動1步時(shí)，步長估計(jì)和航向估計(jì)模塊發(fā)生作用，開始進(jìn)行位置的更新，其過程見圖1。假設(shè)初始時(shí)刻為t0，行人的初始位置為p0(e0，n0)，沿北偏東α0的方向移動，在t1時(shí)刻行人的運(yùn)動距離為S0(步長)，此時(shí)行人的位置為p1(e1，n1)。同理可得，在t2時(shí)刻行人的位置為p2(e2，n2)，距離為S1(步長)。

圖1 PDR位置更新Fig.1 PDR location update

推廣到一般情況，根據(jù)t時(shí)刻的步長St和行進(jìn)方向αt，計(jì)算t+1時(shí)刻行人的位置為：

(1)

式中，nt為t時(shí)刻行人在全局坐標(biāo)系中的橫坐標(biāo)，et為t時(shí)刻行人在全局坐標(biāo)系中的縱坐標(biāo)。

由于傳統(tǒng)的計(jì)步檢測過程中會出現(xiàn)偽峰谷值的影響，因此提出利用雙重閾值二值化算法避免誤判；傳統(tǒng)的航向估計(jì)精度不高，本文采用自適應(yīng)融合算法進(jìn)行航向估計(jì)。

2 計(jì)步檢測

傳統(tǒng)的波峰檢測和閾值檢測算法均無法處理行人在行走過程中出現(xiàn)的抖動情況，表現(xiàn)在加速度數(shù)據(jù)上的特征是出現(xiàn)偽波峰或偽波谷。本文提出利用雙重閾值二值化算法進(jìn)行計(jì)步檢測，可精準(zhǔn)計(jì)步，并處理出現(xiàn)偽波峰或偽波谷的情況。利用改進(jìn)的步數(shù)算法進(jìn)行計(jì)步檢測的步驟為：

1)求加速度模值。通過手機(jī)加速度傳感器獲取三軸加速度(圖2)，再利用三軸加速度信息求加速度模值，其公式為：

圖2 沿矩形路徑行走的三軸加速度Fig.2 Triaxial acceleration along rectangular path

(2)

式中，ax、ay、az為三軸加速度。

2)低通濾波。利用四階的低通濾波器對加速度模值進(jìn)行濾波，正常人勻速行走的頻率為每秒2步，變速行走平均為每秒3步，截止頻率可設(shè)置為2 Hz或3 Hz，濾波前后的數(shù)據(jù)見圖3。

圖3 低通濾波后的加速度Fig.3 Low-pass filtered acceleration

3)去除重力。將濾波之后的加速度模值去除重力的影響：

acc_detrend=acc_mag_filt-g

(3)

式中，acc_detrend為去除重力影響的加速度，acc_mag_filt為濾波后的加速度，g為重力加速度(取9.8 m/s2)。

4)雙重閾值二值化。由于傳統(tǒng)閾值方法對偽峰谷值的判斷不準(zhǔn)確，因此設(shè)計(jì)兩重閾值細(xì)化加速度信息，其上限根據(jù)去除重力后加速度模值的最大值設(shè)置，下限根據(jù)行走多組實(shí)驗(yàn)取平均得出。同時(shí)，為從細(xì)化的加速度信息中提取峰谷值，采用二值化方法進(jìn)行計(jì)算。雙重閾值二值化流程見圖4，二值化后的加速度為：

圖4 雙重閾值二值化Fig.4 Double threshold binarization

(4)

(5)

式中，low_acc和up_acc分別為閾值下限和上限，B1i、B2i分別為第i個(gè)樣本在二值化后的加速度，B1i-1為前一個(gè)樣本二值化后的值。

5)計(jì)步。需同時(shí)滿足從高加速度到低加速度，如B1i從1到0，且在1個(gè)時(shí)間窗口內(nèi)加速度出現(xiàn)0，即可記為1步。

3 航向估計(jì)

3.1 基于歐拉角的坐標(biāo)變換

由于手機(jī)在行走過程中會不斷發(fā)生移動，加速度傳感器采集的數(shù)據(jù)因手機(jī)姿態(tài)的不斷變化很難同前一時(shí)刻保持一致，因此采用空間坐標(biāo)系轉(zhuǎn)換的方法將數(shù)據(jù)從機(jī)體坐標(biāo)系(body frame, b)映射至靜態(tài)的導(dǎo)航坐標(biāo)系(navigation frame，n)。機(jī)體坐標(biāo)系如圖5所示。

圖5 機(jī)體坐標(biāo)系Fig.5 Body frame

在初始時(shí)刻，機(jī)體坐標(biāo)系xbybzb與導(dǎo)航坐標(biāo)系xnynzn是重合的，行走過程中設(shè)備載體的姿態(tài)會隨行人的運(yùn)動發(fā)生改變。導(dǎo)航坐標(biāo)系可經(jīng)過航向角(機(jī)體繞z軸旋轉(zhuǎn))、橫滾角(機(jī)體繞y軸旋轉(zhuǎn))及俯仰角(機(jī)體繞x軸旋轉(zhuǎn))變化后得到機(jī)體坐標(biāo)系，該旋轉(zhuǎn)過程可表示為：

(6)

式中，θ、φ、φ分別為航向角、橫滾角及俯仰角。

3.2 改進(jìn)的航跡推算算法描述

針對航向估計(jì)準(zhǔn)確性低的問題，傳統(tǒng)算法利用陀螺儀積分得到角度，但積分會使角度誤差隨時(shí)間不斷累積，導(dǎo)致航向錯誤，同時(shí)周圍磁場也在發(fā)生變化，使得方向估計(jì)更加困難。因此，本文先將加速度計(jì)及陀螺儀數(shù)據(jù)進(jìn)行融合，再根據(jù)磁感應(yīng)強(qiáng)度閾值確定是否需要融合磁力計(jì)數(shù)據(jù)，具體流程見圖6。

圖6 融合框架Fig.6 Fusion framework

具體算法步驟如下。

1)根據(jù)加速度求取角度。根據(jù)加速度傳感器采集的信息初始化手機(jī)的姿態(tài)信息，默認(rèn)初始狀態(tài)的yaw(航向角)為0°，roll(橫滾角)和pitch(俯仰角)為：

(7)

2)坐標(biāo)轉(zhuǎn)換。根據(jù)§3.1的描述，得出機(jī)體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下的初始旋轉(zhuǎn)矩陣。

3)由微分方程確定旋轉(zhuǎn)矩陣，進(jìn)行姿態(tài)解算。對姿態(tài)的解算本質(zhì)上是求旋轉(zhuǎn)矩陣，由于姿態(tài)隨時(shí)間變化，在時(shí)間t處姿態(tài)的變化率為：

(8)

式中，δt為陀螺儀的周期。式(8)可利用極限性質(zhì)進(jìn)行簡化：

(9)

(10)

式中，δΨ為t到t+δt時(shí)間段內(nèi)的旋轉(zhuǎn)矩陣，δθ為該時(shí)間段內(nèi)微小的偏航角，δφ為該時(shí)間段內(nèi)微小的橫滾角，δφ為該時(shí)間段內(nèi)微小的俯仰角。

(11)

(12)

式中，Ω(t)為陀螺儀角速率矩陣，t為陀螺儀周期開始時(shí)刻，ωbx(t)、ωby(t)、ωbz(t)分別為三軸角速率。由式(9)～(12)可得：

(13)

陀螺儀實(shí)際上是不提供連續(xù)信號ωb(t)的，而是通常以固定頻率提供角速度的樣本，必須使用積分方案來積分采樣信號。因此式(13)可改寫為：

(14)

由式(14)的微分方程可解得旋轉(zhuǎn)矩陣為：

(15)

式中，C(0)由步驟1)中加速度初始化的旋轉(zhuǎn)矩陣確定，因此更新過程中融合了加速度信息。

4)根據(jù)旋轉(zhuǎn)矩陣求航向角。由步驟3)求得的旋轉(zhuǎn)矩陣可以得出加速度融合陀螺儀的航向角為：

(16)

式中，C21為旋轉(zhuǎn)矩陣2行1列處的元素，同理C11為矩陣中1行1列處的元素。

5)根據(jù)磁力計(jì)數(shù)據(jù)求航向角。由磁力計(jì)估計(jì)的航向角為：

ψmag=

(17)

式中，hx、hy、hz為磁力計(jì)測得的三軸磁感應(yīng)強(qiáng)度，D為當(dāng)?shù)卮牌恰?/p>

6)融合策略。根據(jù)是否受磁場干擾確定加速度計(jì)、陀螺儀及磁力計(jì)的權(quán)重分配，其中有、無磁場干擾是根據(jù)傳感器采集的磁感應(yīng)強(qiáng)度模值決定的，大于閾值時(shí)認(rèn)為有磁場干擾，反之則認(rèn)為無磁場干擾。本文采取的策略為：如果受到磁場干擾，則磁力計(jì)的權(quán)重小，加速度計(jì)和陀螺儀融合結(jié)果權(quán)重大；如果沒有受到磁場干擾，則磁力計(jì)權(quán)重大，加速度數(shù)據(jù)和陀螺儀融合結(jié)果權(quán)重小，以適應(yīng)有磁場干擾的環(huán)境。

4 實(shí)驗(yàn)與結(jié)果分析

4.1 實(shí)驗(yàn)條件及數(shù)據(jù)采集

為對本文提出的改進(jìn)計(jì)步算法和航向估計(jì)算法在航跡跟蹤上的效果進(jìn)行驗(yàn)證，設(shè)計(jì)多組實(shí)驗(yàn)，并讓行人按照標(biāo)定好的矩形路徑行走，比較航跡的誤差。

實(shí)驗(yàn)的采集設(shè)備為華為P20手機(jī)，加速度計(jì)、陀螺儀和磁力計(jì)數(shù)據(jù)的采集利用LOPSI實(shí)驗(yàn)室(http://car.upm-csic.es/lopsi)開發(fā)的GetSensorData2.0應(yīng)用程序，數(shù)據(jù)采集頻率均為50 Hz，采集數(shù)據(jù)時(shí)右手持手機(jī)放在胸前。

4.2 步數(shù)估計(jì)實(shí)驗(yàn)

在計(jì)步檢測實(shí)驗(yàn)中，采用本文提出的雙重閾值二值化算法進(jìn)行計(jì)步，閾值上限根據(jù)去除重力影響后加速度模值的最大值設(shè)置為2.2 m/s2，下限根據(jù)加速度模值的最小值設(shè)置為0.4 m/s2，實(shí)驗(yàn)過程中行人可以不同速度行走，以檢驗(yàn)算法的魯棒性。以直線行走和沿矩形路徑行走為例，分析加速度變化特性并可視化，如圖7所示?？梢钥闯?，相鄰的波峰與波谷之間為1步，但由于行人在運(yùn)動過程中的擺動可能出現(xiàn)偽波峰或偽波谷，采用二值化的方式并結(jié)合時(shí)間約束可避免誤判。從圖7(b)中可清晰地看出，在5 200～5 300 ms之間加速度模值出現(xiàn)偽波峰谷，二值化算法在該時(shí)段沒有計(jì)步。

圖7 步數(shù)可視化Fig.7 Visualization of the number of steps

為驗(yàn)證算法的有效性，通過計(jì)步準(zhǔn)確性衡量指標(biāo)分析本文算法與文獻(xiàn)[8]中動態(tài)閾值算法在計(jì)步檢測上的表現(xiàn)，結(jié)果見表1。根據(jù)行人在行走過程中速度的變化將實(shí)驗(yàn)分為勻速運(yùn)動和變速運(yùn)動，行人在勻速行走時(shí)兩種算法的計(jì)步準(zhǔn)確率都在95%以上；行人在變速行走時(shí)，基于動態(tài)閾值波峰檢測算法的計(jì)步準(zhǔn)確率明顯低于本文提出的雙閾值二值化算法。由此可知，雙閾值二值化算法可避免傳統(tǒng)方法遇到的偽峰谷值的影響，進(jìn)而提高計(jì)步的準(zhǔn)確率，同時(shí)本文算法對行人的行走速度要求不高，更具有普適性。

表1 步數(shù)判斷結(jié)果Tab.1 Step judgment result

4.3 航向估計(jì)實(shí)驗(yàn)

由于地球表面任一點(diǎn)的地球磁場強(qiáng)度矢量方向與水平面存在夾角，在航向估計(jì)中需要移除本地磁偏角(5°)。為驗(yàn)證自適應(yīng)融合算法在航向估計(jì)中的準(zhǔn)確性，設(shè)計(jì)行人沿著矩形路徑行走，通過平均角度誤差將本文改進(jìn)的航向估計(jì)算法同傳統(tǒng)算法以及文獻(xiàn)[17]中的多傳感器融合算法進(jìn)行比對。圖8展示了行人沿矩形路徑行走37步中角度的變化情況，其中自適應(yīng)融合算法即為本文算法。由圖可知，本文算法和文獻(xiàn)[17]算法均在第20步時(shí)判斷發(fā)生第1次轉(zhuǎn)彎，但傳統(tǒng)算法在行走到第18步時(shí)判斷發(fā)生第1次轉(zhuǎn)彎，從參考航向來看，第1次轉(zhuǎn)彎發(fā)生在第20步，因此傳統(tǒng)算法的轉(zhuǎn)彎識別錯誤。在發(fā)生第1次轉(zhuǎn)彎過程中3種算法的精度各不相同，本文算法在第1次轉(zhuǎn)彎處與參考角度之間的誤差為5.7°，文獻(xiàn)[17]算法的誤差為8.6°，傳統(tǒng)算法的誤差為11.4°；在第2次轉(zhuǎn)彎時(shí)，傳統(tǒng)算法判斷在第20步時(shí)發(fā)生第2次轉(zhuǎn)彎，而參考航向證明發(fā)生第2次轉(zhuǎn)彎是在第21步，傳統(tǒng)算法轉(zhuǎn)彎識別錯誤，本文算法與文獻(xiàn)[17]算法均在第21步時(shí)識別轉(zhuǎn)彎，文獻(xiàn)[17]算法與參考角度之間的誤差為12.5°，本文算法的誤差為5.4°。從圖中還可看出，在行走到第24步時(shí)3種算法均判斷發(fā)生第3次轉(zhuǎn)彎，即3種算法在第3次轉(zhuǎn)彎識別上的判斷上是一致的，傳統(tǒng)算法與參考角度之間的誤差為5.7°，文獻(xiàn)[17]算法的誤差為2.5°，本文算法的誤差為2.7°。為量化誤差及衡量算法的準(zhǔn)確性，利用平均角度誤差衡量在3次轉(zhuǎn)彎過程中的角度誤差，由于傳統(tǒng)算法未能正確識別第1次和第2次轉(zhuǎn)彎，所以在平均角度誤差衡量過程中不考慮傳統(tǒng)算法。本文算法的平均角度誤差為4.5°，文獻(xiàn)[17]算法的平均角度誤差為6.8°。綜上所述，本文算法在航向估計(jì)中較文獻(xiàn)[17]算法的角度誤差更小，更有利于航跡跟蹤時(shí)準(zhǔn)確捕獲航向變化。

圖8 航向估計(jì)結(jié)果Fig.8 Heading estimation result

4.4 航跡跟蹤實(shí)驗(yàn)

為得到更精確的位置和航向信息，實(shí)驗(yàn)通過計(jì)算最終定位結(jié)果與起始點(diǎn)之間的誤差來比較算法的優(yōu)劣。將磁感應(yīng)強(qiáng)度閾值設(shè)置為30 μT，以鑒別是否存在磁場干擾；實(shí)驗(yàn)設(shè)計(jì)行人分別在有磁場干擾和無磁場干擾的環(huán)境中沿矩形路徑行走，有磁場干擾的實(shí)驗(yàn)場地選在有鐵質(zhì)和鋼質(zhì)及無線通信設(shè)備的環(huán)境中，無磁場干擾實(shí)驗(yàn)場則是在空曠環(huán)境中，盡量屏蔽手機(jī)和鋼鐵帶來的影響。矩形路徑長為11 m，寬為5 m，其中步長估計(jì)采用文獻(xiàn)[10]中的經(jīng)典Weinberg步長模型，計(jì)步檢測和航向軌跡利用本文的雙重閾值二值化和自適應(yīng)融合算法，并將本文算法與傳統(tǒng)PDR算法及PDR+Mahony融合算法進(jìn)行對比，圖9為在不同場景下對行人運(yùn)動軌跡的跟蹤情況。

圖9 行人軌跡跟蹤Fig.9 Pedestrian tracking

從圖9可以看出，在無磁場的環(huán)境下，傳統(tǒng)算法和本文算法對行人軌跡的跟蹤總體一致；但在有磁場環(huán)境中，傳統(tǒng)算法未能對磁場干擾進(jìn)行有效排除，因此最終的定位結(jié)果為(-0.88 m，0.22 m)，與起始點(diǎn)的距離為0.91 m，對于32 m的行走距離而言誤差為2.8%；PDR+Mahony融合算法在航跡上的跟蹤結(jié)果為(-0.23 m, 0.12 m)，與起始點(diǎn)的距離為0.26 m，誤差為1.4%；而本文通過自適應(yīng)融合算法，根據(jù)磁場強(qiáng)弱分配權(quán)重，最終的航跡跟蹤定位結(jié)果為(-0.15 m, 0.14 m)，與起始點(diǎn)的距離為0.21 m，誤差為1.1%，精度分別提高了55%和25%。選取參考路徑中的部分實(shí)驗(yàn)點(diǎn)作為誤差分析參考點(diǎn)，將行人的二維定位坐標(biāo)與對應(yīng)參考點(diǎn)之間的歐氏距離定義為定位誤差，其計(jì)算公式為：

(18)

圖10為在有磁場干擾和無磁場干擾下根據(jù)定位誤差繪制的累積概率分布，可以看出，本文算法定位誤差小于0.75 m的概率為59%，PDR+Mahony融合算法定位誤差小于0.75 m的概率為55%，傳統(tǒng)算法定位誤差小于0.75m的概率為22%；本文算法在定位誤差小于0～1.0 m之間的概率均大于或等于PDR+Mahony融合算法；3種算法在無磁場干擾情況下的定位誤差大致一致。

圖10 不同環(huán)境下的定位誤差累積概率分布Fig.10 Probability distribution of positioning errors in different environments

為了說明在不同環(huán)境下各算法的平均定位誤差，分別讓行人沿矩形路徑行走32 m、64 m及96 m，其余實(shí)驗(yàn)變量與行走32 m的相同，表2～4為在不同環(huán)境中的平均定位誤差統(tǒng)計(jì)。從3個(gè)表中均可以看出，在有磁場干擾環(huán)境下，本文算法的定位誤差更?。辉跓o磁場干擾環(huán)境下，3種算法的性能大體一致。由此可知，本文算法對于航跡的跟蹤更貼近真實(shí)軌跡，同時(shí)適應(yīng)在有磁場干擾環(huán)境中的跟蹤，并在定位精度方面有所提高。

表2 行走32 m平均定位誤差統(tǒng)計(jì)Tab.2 Statistics of average positioning error of 32 m walking

表3 行走64 m平均定位誤差統(tǒng)計(jì)Tab.3 Statistics of average positioning error of 64 m walking

表4 行走96 m平均定位誤差統(tǒng)計(jì)Tab.4 Statistics of average positioning error of 96 m walking

5 結(jié) 語

本文提出一種基于行人航位推算的改進(jìn)航跡跟蹤算法，該算法利用雙重閾值二值化算法對加速度數(shù)據(jù)進(jìn)行處理，避免偽峰谷值的影響，并利用自適應(yīng)融合算法對行人航向進(jìn)行準(zhǔn)確估計(jì)，最終推算出行人的運(yùn)動軌跡。實(shí)驗(yàn)結(jié)果表明，本文算法在32 m矩形行走實(shí)驗(yàn)中定位誤差為1.1%，能夠有效減少誤差積累，抑制航向發(fā)散，提高導(dǎo)航精度，使導(dǎo)航復(fù)現(xiàn)路線更加貼合真實(shí)行走軌跡。