任 元，黃麗斌，趙立業(yè)，丁 姝

基于足部運(yùn)動(dòng)約束的行人導(dǎo)航方法

任 元，黃麗斌，趙立業(yè)，丁 姝

（東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

針對(duì)微型慣性測(cè)量單元（MIMU）行人導(dǎo)航系統(tǒng)，在衛(wèi)星信號(hào)受限環(huán)境中存在定位誤差迅速累積的問(wèn)題，研究了基于足部運(yùn)動(dòng)約束的導(dǎo)航誤差修正方法。在分析行人步態(tài)特征的基礎(chǔ)上，構(gòu)建基于零速修正和足部運(yùn)動(dòng)側(cè)向約束組合的足部運(yùn)動(dòng)約束模型，進(jìn)一步對(duì)步態(tài)周期中的導(dǎo)航誤差進(jìn)行修正。實(shí)驗(yàn)結(jié)果表明，受行人足部動(dòng)態(tài)過(guò)程的影響，未加約束時(shí)的導(dǎo)航定位結(jié)果存在顯著誤差，而導(dǎo)航誤差修正的方法，將導(dǎo)航終點(diǎn)定位誤差的均值提升至行走距離的1.51%，，顯著提高了解算精度，能夠滿足行人導(dǎo)航系統(tǒng)的定位需求。

行人導(dǎo)航；步態(tài)分析；零速修正；運(yùn)動(dòng)約束

0 引言

行人導(dǎo)航定位信息在日常生活和工作中發(fā)揮著日漸重要的作用，極大地提高了生活和工作效率?；谖⑿蛻T性測(cè)量單元（micro inertial measurement unit, MIMU）的行人導(dǎo)航定位系統(tǒng)，具有成本低、尺寸小、質(zhì)量輕等特點(diǎn)，在行人導(dǎo)航領(lǐng)域受到廣泛關(guān)注[1-3]。然而由于MIMU具有精度低、噪聲大等缺點(diǎn)，在衛(wèi)星信號(hào)受限的復(fù)雜環(huán)境中，導(dǎo)航解算誤差會(huì)迅速累積，為此常采用足部安裝傳感器的方式，利用零速修正（zero velocity update, ZUPT）對(duì)導(dǎo)航解算誤差進(jìn)行修正[4-6]。

目前，已有一些研究人員和學(xué)者圍繞基于MIMU的行人導(dǎo)航方法開(kāi)展了富有建設(shè)性的研究工作。在零速區(qū)間的檢測(cè)方法研究方面：文獻(xiàn)[7]中提出了一種基于多條件約束的零速區(qū)間檢測(cè)算法，與單一的判斷條件相比較，多條件組合的檢測(cè)算法能夠顯著降低誤判概率，在不同步速下均取得了較好的檢測(cè)結(jié)果；文獻(xiàn)[8]中提出了一種基于足底壓力計(jì)輔助的零速區(qū)間檢測(cè)方法，通過(guò)對(duì)足底壓力變化情況的檢測(cè)來(lái)輔助零速區(qū)間的檢測(cè)；文獻(xiàn)[9]中提出了一種自適應(yīng)閾值的零速區(qū)間檢測(cè)方法，通過(guò)實(shí)驗(yàn)建立了加速度大小和判別閾值之間的關(guān)系，可根據(jù)運(yùn)動(dòng)狀態(tài)自適應(yīng)調(diào)整零速區(qū)間的判別閾值。在多傳感器融合的行人導(dǎo)航方法研究方面：文獻(xiàn)[10-12]中分別提出了基于超帶寬/航跡推算、基于行人航位推算（pedestrian dead reckoning, PDR）/無(wú)線保真（wireless fidelity, WiFi）/氣壓計(jì)、基于PDR/WiFi/地磁和基于射頻識(shí)別（radio frequency identification, RFID）/MIMU的室內(nèi)行人導(dǎo)航算法，相較于僅依靠慣性器件的導(dǎo)航算法，它們?cè)谔岣呦到y(tǒng)定位精度的同時(shí)，也使得復(fù)雜度增加，并且降低了系統(tǒng)的自主性，使得系統(tǒng)在未知的復(fù)雜環(huán)境中適用性不高。在行人導(dǎo)航中約束模型的研究方面：文獻(xiàn)[13]中研究了基于ZUPT輔助的行人導(dǎo)航算法，實(shí)驗(yàn)結(jié)果表明，行人導(dǎo)航解算的誤差累積速度能夠得到有效抑制；文獻(xiàn)[14]中提出了一種基于主導(dǎo)航向約束的航向修正算法，取得了較好的修正效果；文獻(xiàn)[15-17]中分別構(gòu)建了一種基于雙足MIMU的行人導(dǎo)航系統(tǒng)，通過(guò)構(gòu)建雙足間的不等式約束關(guān)系對(duì)系統(tǒng)的解算結(jié)果進(jìn)行約束。

在不引入其他傳感器的前提下，本文在零速修正的基礎(chǔ)上，添加足部運(yùn)動(dòng)側(cè)向約束，構(gòu)建針對(duì)整個(gè)步態(tài)周期的足部運(yùn)動(dòng)約束模型，進(jìn)一步提高了行人導(dǎo)航算法的定位精度，并通過(guò)相關(guān)實(shí)驗(yàn)，驗(yàn)證算法的有效性。

1 行人導(dǎo)航算法基本框架

通過(guò)足部MIMU，敏感行人運(yùn)動(dòng)時(shí)足部的加速度和角速度信息，并通過(guò)捷聯(lián)慣導(dǎo)算法，計(jì)算得到速度、位置和姿態(tài)等信息。捷聯(lián)慣導(dǎo)解算的基本方程[18]可表述為

在捷聯(lián)慣導(dǎo)算法解算的基礎(chǔ)上，結(jié)合足部運(yùn)動(dòng)約束，構(gòu)成本文的行人導(dǎo)航算法，其算法框圖如圖1所示。

圖1 行人導(dǎo)航算法框圖

在利用MIMU器件輸出的加速度和角速度信息進(jìn)行捷聯(lián)慣導(dǎo)解算的同時(shí)，對(duì)足部的運(yùn)動(dòng)狀態(tài)進(jìn)行判別，并根據(jù)判別結(jié)果選擇相應(yīng)的修正算法：當(dāng)處于零速區(qū)間時(shí)，通過(guò)零速修正算法進(jìn)行誤差修正；當(dāng)處于非零速區(qū)間時(shí)，通過(guò)足部側(cè)向約束對(duì)誤差進(jìn)行修正。

2 足部運(yùn)動(dòng)約束模型構(gòu)建

2.1 步態(tài)分析

在正常行走過(guò)程中，從一只腳著地到該只腳再次著地所經(jīng)歷的步態(tài)過(guò)程稱為一個(gè)完整的步態(tài)周期，運(yùn)動(dòng)過(guò)程[19]如圖2所示。

圖2 步態(tài)周期

步態(tài)周期中存在足部與地面保持相對(duì)靜止的一段時(shí)間，將其稱為零速區(qū)間，在此區(qū)間內(nèi)，足部速度的理論值為零，步態(tài)周期內(nèi)其余部分稱為非零速區(qū)間，并認(rèn)為在該期間，足部?jī)H沿腳尖朝向擺動(dòng)，而無(wú)橫向運(yùn)動(dòng)。行人行走過(guò)程中，由于步態(tài)的重復(fù)性，使得零速區(qū)間和非零速區(qū)間會(huì)交替重復(fù)出現(xiàn)，這是基于足部固定MIMU的行人導(dǎo)航系統(tǒng)區(qū)別于其他導(dǎo)航系統(tǒng)的顯著特征。

2.2 零速檢測(cè)

基于足部運(yùn)動(dòng)約束的修正算法，是建立在步態(tài)周期中零速區(qū)間與非零速區(qū)間準(zhǔn)確判別的基礎(chǔ)上，因此，零速區(qū)間的準(zhǔn)確檢測(cè)是進(jìn)行導(dǎo)航誤差修正的重要前提，對(duì)誤差的修正效果有著重要影響。采用基于廣義似然比檢測(cè)法，對(duì)步態(tài)周期中的零速狀態(tài)進(jìn)行檢測(cè)，當(dāng)滑動(dòng)窗口內(nèi)足部固定MIMU輸出數(shù)據(jù)的時(shí)間序列滿足判別條件時(shí)，即為零速狀態(tài)，判別式[20]為：

因此，滑動(dòng)窗口內(nèi)足部的零速狀態(tài)檢測(cè)結(jié)果為

行人運(yùn)動(dòng)過(guò)程中，足部固定MIMU的原始數(shù)據(jù)及零速檢測(cè)結(jié)果如圖3所示。MIMU中加速度計(jì)和陀螺儀的原始測(cè)量數(shù)據(jù)分別如圖3（a）和圖3（b）所示，由于行走過(guò)程中步態(tài)的重復(fù)性，使得MIMU的數(shù)據(jù)也呈現(xiàn)出相應(yīng)的周期性，其中數(shù)據(jù)波動(dòng)較為平穩(wěn)基本不變的時(shí)間段，即為步態(tài)周期中的零速區(qū)間。零速區(qū)間的檢測(cè)結(jié)果如圖3（c）所示，為了便于對(duì)照查看零速區(qū)間的檢測(cè)結(jié)果，同時(shí)給出了足部的俯仰角速率變化曲線，由此可見(jiàn)，步態(tài)周期中的零速區(qū)間能夠被準(zhǔn)確檢測(cè)出來(lái)，沒(méi)有誤判和漏判現(xiàn)象的發(fā)生，該檢測(cè)算法能夠滿足對(duì)行人行走過(guò)程步態(tài)零速區(qū)間檢測(cè)的需求。

圖3 傳感器原始數(shù)據(jù)及零速檢測(cè)結(jié)果

2.3 足部運(yùn)動(dòng)側(cè)向約束

將式（6）代入式（8）有

根據(jù)式（9）即可得足部側(cè)向約束的量測(cè)方程。

3 卡爾曼濾波模型構(gòu)建

將行人足部的運(yùn)動(dòng)約束分為兩部分：在零速狀態(tài)時(shí)，利用足部的零速條件進(jìn)行修正；在非零速狀態(tài)，利用足部運(yùn)動(dòng)的側(cè)向約束條件進(jìn)行修正。這兩部分修正方法共同構(gòu)成了整個(gè)步態(tài)周期的足部運(yùn)動(dòng)約束。為此，需要分別構(gòu)建卡爾曼濾波模型，實(shí)現(xiàn)導(dǎo)航解算誤差的修正，其中，導(dǎo)航算法誤差方程是建立卡爾曼濾波方程的理論基礎(chǔ)，即

系統(tǒng)狀態(tài)方程的矩陣形式為

式中：為系統(tǒng)噪聲矩陣；為15維的系統(tǒng)狀態(tài)變量，即

系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣為

系統(tǒng)量測(cè)方程為

式中：為量測(cè)噪聲矩陣；為量測(cè)矩陣。

4 實(shí)驗(yàn)驗(yàn)證

為對(duì)基于足部運(yùn)動(dòng)約束的行人導(dǎo)航方法的性能進(jìn)行評(píng)估，在真實(shí)場(chǎng)景下進(jìn)行了相關(guān)實(shí)驗(yàn)，選用法國(guó)斯伯格（SBG）公司Ellipse2系列傳感器，其主要性能參數(shù)見(jiàn)表1。將MIMU固定到實(shí)驗(yàn)人員的腳背上，安裝示意如圖4中所示，傳感器的采樣率設(shè)置為100 Hz，并通過(guò)數(shù)據(jù)線將行走過(guò)程中采集的數(shù)據(jù)實(shí)時(shí)傳輸?shù)絺€(gè)人計(jì)算機(jī)中進(jìn)行顯示和存儲(chǔ)。

圖4 傳感器安裝方式

表1 傳感器主要性能參數(shù)

為驗(yàn)證算法的有效性，在參考軌跡為15 m×28 m的矩形閉合路徑下進(jìn)行實(shí)驗(yàn)。圖5所示軌跡為未修正的純慣性導(dǎo)航解算結(jié)果，考慮到行人運(yùn)動(dòng)時(shí)足部存在顯著的動(dòng)態(tài)過(guò)程，相較于一般慣導(dǎo)的車(chē)載使用環(huán)境，足部慣性器件存在明顯的沖擊和振動(dòng)，以及較為頻繁的姿態(tài)變化，這些原因可能造成了慣導(dǎo)解算精度的急劇下降，導(dǎo)致未修正時(shí)的行人導(dǎo)航解算軌跡與參考軌跡存在顯著偏差。

圖5 未修正的解算結(jié)果

基于足部運(yùn)動(dòng)約束方法解算得到的行人運(yùn)動(dòng)軌跡如圖6所示。解算軌跡與參考軌跡基本一致，其終點(diǎn)與起點(diǎn)的定位誤差僅為1.09 m，占行走距離的1.27%。說(shuō)明基于足部運(yùn)動(dòng)約束的行人導(dǎo)航算法能夠有效地抑制慣導(dǎo)解算誤差的累積，行人的運(yùn)動(dòng)軌跡得到了較好的復(fù)現(xiàn)。

圖6 導(dǎo)航解算結(jié)果

行走過(guò)程中足部速度的變化情況如圖7所示，解算出的速度變化曲線很好地呈現(xiàn)了行走過(guò)程中的步態(tài)特性。

行走過(guò)程中傳感器姿態(tài)角的變化情況如圖8所示，俯仰角曲線的周期性變化，反映了行走過(guò)程中足部的周期性擺動(dòng)，橫滾角體現(xiàn)了足部的不規(guī)則晃動(dòng)，航向角表示了行人足部的運(yùn)動(dòng)方向，以北偏西為正，角度范圍為-180°~180°。由于本文實(shí)驗(yàn)是沿矩形路徑進(jìn)行的，航向的變化過(guò)程較為清晰，主要經(jīng)歷了如下變化過(guò)程：0°→90°→ 180°(-180°)→-90°，可將其作為航向角解算結(jié)果的參考值。

圖7 足部運(yùn)動(dòng)速度解算結(jié)果

圖8 行走過(guò)程姿態(tài)角解算結(jié)果

分別沿著地面上事先畫(huà)好的大小不同的矩形路線（學(xué)校籃球場(chǎng)或排球場(chǎng)中部分區(qū)域的邊線）進(jìn)行多次實(shí)驗(yàn)，并以解算軌跡的終點(diǎn)與起點(diǎn)之間的距離占總行走距離的百分比，作為解算誤差的評(píng)價(jià)標(biāo)準(zhǔn)，實(shí)驗(yàn)結(jié)果見(jiàn)表2。表2中86×的含義為：圍繞總長(zhǎng)為86 m的矩形閉合路徑行走了圈，實(shí)驗(yàn)10所對(duì)應(yīng)的行人導(dǎo)航解算軌跡如圖9所示。

表2 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)結(jié)果表明，未加約束的行人導(dǎo)航算法解算出的終點(diǎn)位置明顯偏離原點(diǎn)，且由于解算過(guò)程中漂移方向的變化，導(dǎo)致其終點(diǎn)誤差并沒(méi)有呈現(xiàn)明顯的規(guī)律，但總體來(lái)講，隨著行走距離和時(shí)間的延長(zhǎng)，誤差越來(lái)越大；基于足部運(yùn)動(dòng)約束的行人導(dǎo)航算法誤差的均值為行走距離的1.51%，而未加約束時(shí)純慣導(dǎo)解算的誤差均值為行走距離的495.66%，相比而言，足部約束的方法顯著提高了行人導(dǎo)航系統(tǒng)的解算精度，能夠滿足行人導(dǎo)航的定位需求。

圖9 導(dǎo)航解算結(jié)果

5 結(jié)束語(yǔ)

本文研究了基于足部運(yùn)動(dòng)約束的行人導(dǎo)航方法，在零速修正的基礎(chǔ)上，添加足部運(yùn)動(dòng)側(cè)向約束，并分別構(gòu)建了相應(yīng)的卡爾曼濾波器，對(duì)誤差進(jìn)行修正，從而建立了針對(duì)整個(gè)步態(tài)周期的足部運(yùn)動(dòng)約束模型。實(shí)驗(yàn)結(jié)果表明，基于足部運(yùn)動(dòng)約束的行人導(dǎo)航方法的終點(diǎn)定位誤差均值為行走距離的1.51%，能夠滿足行人導(dǎo)航的定位需求。

[1]趙慧, 王斌, 阮巍. 基于MEMS慣性傳感器的行人導(dǎo)航軌跡復(fù)現(xiàn)研究[J]. 傳感器與微系統(tǒng), 2018, 37(3): 54-57.

[2]徐元, 陳熙源, 王宜敏, 等. 一種采用足部航姿參考和肩部航向參考的室內(nèi)個(gè)人導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2016, 24(3): 325-329.

[3]張苗, 許建新, 黃欣, 等. 基于人體運(yùn)動(dòng)學(xué)輔助的可穿戴式行人導(dǎo)航系統(tǒng)[J]. 導(dǎo)航與控制, 2018, 17(4): 1-6.

[4]費(fèi)程羽, 蘇中, 李擎. 行人慣性導(dǎo)航零速檢測(cè)算法[J]. 傳感器與微系統(tǒng), 2016, 35(3): 147-150, 153.

[5]張健敏, 修春娣, 楊威, 等. 一種多運(yùn)動(dòng)模式下自適應(yīng)閾值零速修正算法[J]. 北京航空航天大學(xué)學(xué)報(bào), 2018, 44(3): 636-644.

[6]孫偉, 丁偉, 李瑞豹. 徒步導(dǎo)航系統(tǒng)中的自適應(yīng)步態(tài)優(yōu)化檢測(cè)方法研究[J]. 傳感技術(shù)學(xué)報(bào), 2016, 29(9): 1389-1394.

[7]田曉春, 陳家斌, 韓勇強(qiáng), 等. 多條件約束的行人導(dǎo)航零速區(qū)間檢測(cè)算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2016, 24(1): 1-5.

[8]趙小明, 鄧芳瑾, 楊松普, 等. 基于壓力傳感器輔助的行人室內(nèi)定位零速修正方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2018, 26(1): 1-5.

[9]ZHANG R, YANG H, HOFLINGER F, et al. Adaptive zero velocity update based on velocity classification for pedestrian tracking[J]. IEEE Sensors Journal, 2017, 17(7): 2137-2145.

[10]儀玉杰, 黃智剛, 蘇雨. 一種UWB與PDR融合的行人室內(nèi)定位方法[J]. 導(dǎo)航定位學(xué)報(bào), 2019, 7(3): 38-43.

[11]CHAI W N, CHEN C, EDWAN E, et al. 2D/3D indoor navigation based on multi-sensor assisted pedestrian navigation in Wi-Fi environments[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of 2012 Ubiquitous Positioning, Indoor Navigation, and Location Based Service (UPINLBS). Palms Springs: IEEE, 2012[2020-06-10].

[12]宋世銘, 王繼, 韓李濤. 一種基于改進(jìn)粒子濾波算法的室內(nèi)融合定位方法[J]. 導(dǎo)航定位學(xué)報(bào), 2020, 8(1): 99-106.

[13]李劉頌, 徐向波. 基于慣性導(dǎo)航的步行者零速檢測(cè)算法[J]. 傳感器與微系統(tǒng), 2019, 38(3): 154-156.

[14]MUHAMMAD M N, SALCIC Z, WANG K I. Indoor pedestrian tracking using consumer-grade inertial sensors with PZTD heading correction[J]. IEEE Sensors Journal, 2018, 18(12): 5164-5172.

[15]NIU X J, LI Y, KUANG J, et al. Data fusion of dual foot-mounted IMU for pedestrian navigation[J]. IEEE Sensors Journal, 2019, 19(12): 4577-4584.

[16]SHI W, WANG Y, WU Y X. Dual MIMU pedestrian navigation by inequality constraint Kalman filtering[J]. Sensors, 2017, 17(2): 427.

[17]WANG Q Y, CHENG M, NOURELDIN A, et al. Research on the improved method for dual foot-mounted inertial/magnetometer pedestrian positioning based on adaptive inequality constraints Kalman filter algorithm[J]. Measurement, 2019, 135: 189-198.

[18]TIAN X C, CHEN J B, HAN Y Q, et al. A novel zero velocity interval detection algorithm for self-contained pedestrian navigation system with inertial sensors[J]. Sensors, 2016, 16(10): 1578.

[19]RUPPELT J, KRONENWETT N, TROMMER G F. A novel finite state machine based step detection technique for pedestrian navigation systems[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of 2015 International Conference on Indoor Positioning and Indoor Navigation (IPIN). Palms Springs: IEEE, 2015[2020-06-10].

[20]SKOG I, HANDEL P, NILSSON J O, et al. Zero-velocity detection-an algorithm evaluation[J]. IEEE Transactions on Biomedical Engineering, 2010, 57(11): 2657-2666.

[21]程向紅, 趙瑩, 田蕓. 一種自適應(yīng)H∞濾波的運(yùn)動(dòng)學(xué)約束慣性導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2019, 27(3): 295-300.

Research on pedestrian navigation methods based on foot motion constraints

REN Yuan, HUANG Libin, ZHAO Liye, DING Shu

(School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China)

Aiming at the problem that it is easy to rapidly accumulate positioning errors for the pedestrian navigation system based on Micro Inertial Measurement Unit (MIMU) under the condition of Global Navigation Satellite System (GNSS) failure, the paper proposed a navigation error correction method based on foot motion constraint: pedestrian gait characteristics were analyzed; and the foot motion constraint model was established by combining Zero Velocity Update (ZUPT) with the lateral constraint of foot motion; finally the navigation error in the gait cycle was further correctedby establishing Kalman filter model. Experimental resultshowed that: there would be significant error in the navigation and positioning results without constraints due to the influence ofpedestrianfoot dynamics; while the proposed method would increase the average value of navigation and positioning error to 1.51% of the walking distance,which significantly improved the accuracy of calculation, indicating that the method could meet the positioning requirements of pedestrian navigation system.

pedestrian navigation; gait analysis; zero velocity update; motion constraints

P228

A

2095-4999(2021)02-0021-07

任元，黃麗斌，趙立業(yè)，等. 基于足部運(yùn)動(dòng)約束的行人導(dǎo)航方法[J]. 導(dǎo)航定位學(xué)報(bào), 2021, 9(2): 21-27.（REN Yuan, HUANG Libin, ZHAO Liye, et al. Research on pedestrian navigation methods based on foot motion constraints[J].Journal of Navigation and Positioning, 2021, 9(2): 21-27.）

10.16547/j.cnki.10-1096.20210204.

2020-06-22

任元（1994—），男，河南鄭州人，碩士研究生，研究方向?yàn)樾腥藢?dǎo)航算法。

黃麗斌（1974—），女，江蘇南京人，博士，教授，研究方向?yàn)槲⑿蛢x表及微系統(tǒng)技術(shù)、組合導(dǎo)航技術(shù)。