汪少初，劉開華，劉 昱

(1. 天津大學(xué)電子信息工程學(xué)院，天津 300072；2. 天津市測繪院，天津 300381)

脛骨捷聯(lián)慣性導(dǎo)航的人員行進(jìn)距離估計(jì)

汪少初1,2，劉開華1，劉 昱1

(1. 天津大學(xué)電子信息工程學(xué)院，天津 300072；2. 天津市測繪院，天津 300381)

研究了慣性測量單元(IMU)固連在人員脛骨中間的行走步長計(jì)算方案，提出了捷聯(lián)慣性導(dǎo)航算法中零速率修正(ZUPT)時(shí)刻導(dǎo)航參數(shù)的重置方法，準(zhǔn)確得到了以初始方位角為未知數(shù)的三維位移函數(shù)表達(dá)式，避免二重積分對近似誤差的擴(kuò)散．通過分析人員行進(jìn)特性并結(jié)合 ZUPT時(shí)刻脛骨與路面垂直的關(guān)系，以每步中垂直地面方向的位移幾乎為零作為約束條件，解算另外兩個(gè)平行地面方向的位移，進(jìn)而計(jì)算每一步的步長．該方法針對不同的人員和行走環(huán)境無需修改相關(guān)參數(shù)，有較好的適應(yīng)性和通用性．經(jīng)多人次行走實(shí)驗(yàn)驗(yàn)證，行進(jìn)距離計(jì)算誤差在 8%以內(nèi)的概率為91%．

捷聯(lián)慣性導(dǎo)航；零速率修正；行人航位推算

近年來，行人航位推算(pedestrian dead-reckoning，PDR)作為室內(nèi)人員定位的一種解決方案，成為研究熱點(diǎn)[1]，其中，將低成本的慣性測量單元(inertial measurement unit，IMU)固連在人員身體上，利用捷聯(lián)慣性導(dǎo)航方法計(jì)算行進(jìn)方向和步長，再以步長作為最小位移單元來推算人員行進(jìn)軌跡的方法得到了廣泛的研究和應(yīng)用．該方法無需在待定位的環(huán)境中布置額外的參考設(shè)備并進(jìn)行大量的前期測量[2]，系統(tǒng)簡單且通用性好．然而，IMU的測量誤差、捷聯(lián)慣性導(dǎo)航固有的累積誤差和坐標(biāo)系轉(zhuǎn)換偏差等誤差源嚴(yán)重影響步長計(jì)算的精度．無相關(guān)改進(jìn)算法的情況下，這些誤差源將使定位系統(tǒng)的累積誤差在 1,min內(nèi)超過100,m[3]．文獻(xiàn)[4]也提到，利用精度相對較高的戰(zhàn)術(shù)級慣性傳感元件，按照捷聯(lián)慣性導(dǎo)航的一般計(jì)算方法，30,min內(nèi)位移誤差達(dá)到 1,131,km(約 1/6地球半徑)，而利用消費(fèi)級慣性傳感元件計(jì)算的位移誤差更是達(dá)到1個(gè)地球半徑．

隨著微機(jī)電系統(tǒng)(micro-electro-mechanical sys-tems，MEMS)技術(shù)的發(fā)展和生產(chǎn)工藝的改善，低成本IMU的測量精度越來越高[5]．因此，如何減小系統(tǒng)固有的累積誤差和坐標(biāo)系轉(zhuǎn)換偏差，提高步長計(jì)算精度成為研究難點(diǎn)．

在基于慣性導(dǎo)航的 PDR算法研究中，文獻(xiàn)[6-9]均采用零速率修正(zero velocity update，ZUPT)的方法，將導(dǎo)航計(jì)算截?cái)嘣谝幌盗邢鄬^短的時(shí)間段內(nèi)，以相鄰兩個(gè) ZUPT點(diǎn)作為一個(gè)導(dǎo)航周期的開始和結(jié)束點(diǎn)．在導(dǎo)航周期的起始時(shí)刻，定位系統(tǒng)重置導(dǎo)航參數(shù)，如初始速度和 IMU機(jī)體坐標(biāo)系與目標(biāo)坐標(biāo)系間的初始方位角．在該導(dǎo)航周期內(nèi)利用慣性導(dǎo)航原理計(jì)算行進(jìn)的位移，進(jìn)而控制累積誤差的無限擴(kuò)散．文獻(xiàn)[10]將 IMU綁定于腳尖，通過慣性數(shù)據(jù)特性尋找行進(jìn)過程中的準(zhǔn)靜止?fàn)顟B(tài)，以該狀態(tài)對應(yīng)時(shí)刻為ZUPT點(diǎn)，認(rèn)為此時(shí)測量的加速度僅為重力加速度，利用重力加速度在IMU三軸的分量，得到IMU的初始方位角，重置導(dǎo)航參數(shù)．但是，腳尖較小的擾動或準(zhǔn)靜止?fàn)顟B(tài)的不準(zhǔn)確均會導(dǎo)致初始方位角存在一定的偏差，后續(xù)每一時(shí)刻方位角的計(jì)算均建立在這個(gè)存在偏差的初始方位角之上，使導(dǎo)航方程中重力加速度的去除過程產(chǎn)生較大的誤差，導(dǎo)航計(jì)算的位移誤差隨時(shí)間不斷累積并有發(fā)散趨勢．

文獻(xiàn)[11]將 IMU置于腰間，利用行進(jìn)過程中腰部運(yùn)動符合倒立擺模型的特點(diǎn)尋找ZUPT點(diǎn)，計(jì)算腰部在垂直方向上的最大和最小位移，利用幾何比例關(guān)系得到步長．該方法對最大和最小位移的計(jì)算精度要求較高，較小的偏差導(dǎo)致步長計(jì)算誤差成倍地增大．并且針對不同的人員，需要設(shè)定腿長和相應(yīng)的比例因子，通用性較差．文獻(xiàn)[12]將 IMU置于脛骨中間，認(rèn)為行走過程中脛骨運(yùn)動符合倒立擺模型，僅在前進(jìn)方向和垂直方向運(yùn)動，用兩軸加速度和單軸角速度實(shí)現(xiàn)步長計(jì)算，忽略行走過程的三維運(yùn)動特性．當(dāng)行走過程不符合倒立擺和二維運(yùn)動模型時(shí)，加速度的坐標(biāo)系轉(zhuǎn)換將存在較大的偏差，導(dǎo)致重力加速度的去除過程發(fā)生錯(cuò)誤．此外，該方法對行走方式有很高的要求，局限性較大．文獻(xiàn)[11-12]均通過倒立擺模型尋找ZUPT點(diǎn)，僅靠高要求的綁定位置簡單地將初始速度和方位角重置為零．然而，初始方位角作為導(dǎo)航計(jì)算的基準(zhǔn)，其準(zhǔn)確性直接影響步長計(jì)算的精度，較小的偏差將導(dǎo)致重力加速度的去除過程產(chǎn)生較大的誤差．初始方位角的偏差是坐標(biāo)系轉(zhuǎn)換偏差的主要因素．

本文在文獻(xiàn)[12]的基礎(chǔ)上，詳細(xì)分析了行走時(shí)脛骨的運(yùn)動過程，充分考慮行走過程的三維特性，提出了一種在 ZUPT時(shí)刻初始方位角的重置方法．不直接地將初始方位角設(shè)定為零或某一固定數(shù)值，而是將初始方位角設(shè)定為未知數(shù)，導(dǎo)航計(jì)算每一步在三軸方向上的位移函數(shù)關(guān)系式．無近似地去除重力加速度，將重力加速度去除過程產(chǎn)生的誤差從重積分里剝離開來．利用ZUPT時(shí)刻脛骨與地面垂直這一特性，以垂直路面方向的位移幾乎為零作為約束條件，結(jié)合行走過程的三維特性對位移進(jìn)行適當(dāng)?shù)慕坪托拚馑愠銎叫杏诼访娴钠渌麅蓚€(gè)方向上的位移，進(jìn)而計(jì)算步長并進(jìn)行總位移的累加．本方法避免了二重積分對重力加速度去除誤差的擴(kuò)散，大大提高了步長計(jì)算的精度．提出的算法方案對人員的行進(jìn)方式和行進(jìn)路面沒有特殊要求，對不同的人員無需進(jìn)行相關(guān)的參數(shù)設(shè)置，具有較強(qiáng)的適應(yīng)性和通用性．

1 捷聯(lián)慣性步長計(jì)算

本文以人員行進(jìn)的每一步步長作為最小位移單元對人員行走距離進(jìn)行估計(jì)，步長計(jì)算的精度直接影響行走距離的精度．結(jié)合行走過程測量的慣性參量，對人員行走時(shí)脛骨的運(yùn)動過程進(jìn)行詳細(xì)分解，找到脛骨與地面垂直時(shí)慣性參量的特性，依此尋找 ZUPT點(diǎn)．利用 ZUPT點(diǎn)將連續(xù)的行進(jìn)過程分為若干單步過程，以每個(gè)單步持續(xù)的時(shí)間為一個(gè)導(dǎo)航周期，利用捷聯(lián)慣性導(dǎo)航原理計(jì)算步長．

1.1 行走過程脛骨運(yùn)動分析

本文將 IMU安置于右腳脛骨中間外側(cè)位置，盡量保證人員在解剖學(xué)姿勢站立時(shí)，IMU機(jī)體坐標(biāo)系Oxbybzb與當(dāng)?shù)刈鴺?biāo)系 Ox(F)y(R)z(D)重合，如圖 1所示．xb軸指向 IMU機(jī)體的正前方，yb軸指向機(jī)體的正右方，zb軸指向機(jī)體的正下方．x(F)軸指向人員行進(jìn)的正前方，y(R)指向人員行進(jìn)的正右方，z(D)軸垂直向下指向地心，與當(dāng)?shù)刂亓铀俣确较蛞恢拢?/p>

圖1 IMU的安置位置Fig.1 Position of IMU mounted

文獻(xiàn)[11]給出了行走姿勢和動作的分解圖．在其動作分解的基礎(chǔ)上，將行走過程中測量得到的慣性數(shù)據(jù)與之對應(yīng)．為更直觀地展現(xiàn)行走過程中脛骨運(yùn)動的三維特性，圖2給出幾個(gè)典型的IMU位置及坐標(biāo)系 Oxbybzb與 Ox(F)y(R)z(D)的相對位置，與動作分解對應(yīng)的慣性數(shù)據(jù)為某測試人員勻速行走時(shí)，測量得到的 IMU機(jī)體坐標(biāo)軸 xb、yb和 zb的角速度信號．由圖解可知，人員在行走過程中，左腳和右腳分別經(jīng)歷的動作和狀態(tài)基本一致，包括腳面完全貼地(flat foot)、腳跟抬起(heel lift)、腳尖離地(toe off)、腳跟著地(heel strike)等．持續(xù)的行走過程可認(rèn)為左腳和右腳周期性地重復(fù)這些動作，重復(fù)周期為人員向前行進(jìn) 1步所需的時(shí)間．本文后續(xù)提到的 1步指左腳和右腳均向前邁進(jìn) 1步．人員勻速行走時(shí)，可認(rèn)為相鄰兩步持續(xù)的時(shí)間和步長基本相同．變速行走時(shí)，所測得的三軸角速度信號波形與圖 2類似．隨著人員行進(jìn)步伐的不斷重復(fù)，測得的慣性參量也周期性地重復(fù)，且每步持續(xù)的時(shí)間和參量大小也基本相同．因此，無論人員勻速還是變速行走，只需準(zhǔn)確判別行走過程所包含的動作和狀態(tài)即可區(qū)分每一步．

本文利用IMU機(jī)體坐標(biāo)系yb軸角速度信號檢測行進(jìn)過程中所有脛骨垂直地面的時(shí)刻(shank vertical point，SVP)，即圖 2中 t1和 t10．該時(shí)刻yb軸角速度信號接近零且脛骨相對地面靜止，對應(yīng)的脛骨運(yùn)動速度也可近似看作零．因此，t1和t10可作為行走過程的ZUPT點(diǎn)，作為每一步的起始和結(jié)束點(diǎn)，將行走過程分割為若干單步．

圖2 人員行走過程分析Fig.2 Analysis of pedestrian walking

1.2 脛骨捷聯(lián)慣性步長計(jì)算方法

根據(jù)上述對脛骨運(yùn)動過程的分析，將行走過程分為若干單步．在每一步內(nèi)，ZUPT點(diǎn)作為導(dǎo)航周期的起始，運(yùn)用捷聯(lián)慣性導(dǎo)航原理計(jì)算行進(jìn)步長．圖 3為捷聯(lián)慣性導(dǎo)航系統(tǒng)原理．

圖3 捷聯(lián)慣性導(dǎo)航系統(tǒng)原理示意Fig.3 Block diagram of strap-down inertial navigation system

t1時(shí)刻Oxbybzb到Ox(F)y(R)z(D)的轉(zhuǎn)換矩陣為即假設(shè)初始方位角為若 IMU在脛骨處綁定位置足夠好(使兩坐標(biāo)系完全重合)，則初始方位角全為零．但行走人員不同，每次綁定的位置和方式也不完全一致，很難做到兩坐標(biāo)系完全重合，因此本文考慮更一般的情況．

人員行走過程中，IMU測得其機(jī)體坐標(biāo)系下三軸角速度為_xiω 、_yiω 和_ziω ，加速度為_xia 、_yia 和az_i，采樣周期為 δt，i為采樣時(shí)間點(diǎn)．假設(shè) i時(shí)坐標(biāo)系 Oxbybzb對應(yīng)位置到 t1時(shí)刻 Oxbybzb對應(yīng)位置的轉(zhuǎn)換矩陣為文獻(xiàn)[10]給出其迭代關(guān)系為

其中

為了簡化公式的表達(dá)，引入Ω這一中間變量，其物理意義為 i到 i+1時(shí)刻轉(zhuǎn)換矩陣R的改變量．因此，在 Ox(F)y(R)z(D)坐標(biāo)系下，捷聯(lián)慣性導(dǎo)航方程為

2 ZUPT點(diǎn)初始方位角解算及改進(jìn)步長計(jì)算

圖2中t1和t10時(shí)刻為SVP點(diǎn)，作為捷聯(lián)慣性導(dǎo)航步長計(jì)算的起始點(diǎn)，IMU機(jī)體坐標(biāo)系 Oxbybzb與 Ox(F)y(R)z(D)間的初始方位角和決定導(dǎo)航計(jì)算的目標(biāo)坐標(biāo)系．

2.1 初始方位角對位移計(jì)算的影響分析

SVP點(diǎn)脛骨與地面垂直，如圖4(a)所示．將此時(shí)Oxbybzb對應(yīng)位置所處的坐標(biāo)系定義為 OXYZ．若IMU位置理想，行走路面與水平面平行，則OXYZ與Ox(F)y(R)z(D)完全重合，初始方位角均為零．一般情況下，IMU綁定位置存在差異，路面并不平整，行走過程中SVP點(diǎn)時(shí)OXYZ并不與Ox(F)y(R)z(D)重合，而是存在一定的初始方位角．圖4(b)表示兩坐標(biāo)系間存在的方位角關(guān)系，繞 y(R)軸旋轉(zhuǎn)的角度為俯仰角(Pitch)，繞 x(F)軸旋轉(zhuǎn)的角度為橫滾角(Roll)，繞z(D)軸旋轉(zhuǎn)的角度為偏航角(Heading)．圖4(c)給出了兩坐標(biāo)系存在方位角ψ、ρ γ和 時(shí)的轉(zhuǎn)換示意.

圖4 SVP時(shí)刻機(jī)體坐標(biāo)系位置Fig.4 Position of Oxbybzbat shank vertical point

Li等[12]認(rèn)為行進(jìn)過程中的脛骨運(yùn)動符合倒立擺模型，利用該模型對式(2)描述的導(dǎo)航方程進(jìn)行近似：①假設(shè)圖 2中 t1和 t10時(shí)刻 IMU機(jī)體坐標(biāo)系Oxbybzb對應(yīng)位置與 Ox(F)y(R)z(D)完全重合，即和全為零；②假設(shè)行進(jìn)過程中IMU所在脛骨部位只在Ox(F)z(D)平面運(yùn)動，即ψ、γ為零．根據(jù)①和②近似可知，坐標(biāo)系 OXYZ和 Ox(F)y(R)z(D)完全重合．OXYZ和Ox(F)y(R)z(D)坐標(biāo)系下的導(dǎo)航方程為

該近似方法在平整的地面或跑步機(jī)上測試，能達(dá)到較高的計(jì)算精度且所需計(jì)算量小，慣性數(shù)據(jù)采集只需二軸加速度計(jì)和單軸陀螺儀．但這種簡單的初始方位角假設(shè)同時(shí)又忽略行進(jìn)過程的三維特性，對IMU綁定位置和人員行走方式的要求十分苛刻，測試系統(tǒng)操作復(fù)雜，局限性大．

每一步SVP點(diǎn)時(shí)，OXYZ相對于Ox(F)y(R)z(D)的方位角為和重力加速度方向指向地心，與z(D)方向相同，因此重力加速度g只在z(D)軸上有分量．根據(jù) OXYZ與 Ox(F)y(R)z(D)間的轉(zhuǎn)換關(guān)系，可得重力加速度在 OXYZ坐標(biāo)系下三軸分量為

文獻(xiàn)[12]將不為零的初始方位角假設(shè)為零值，在此基礎(chǔ)上通過角速度去更新坐標(biāo)系轉(zhuǎn)換關(guān)系，等同于將1步內(nèi)所有時(shí)刻的加速度值映射到OXYZ坐標(biāo)系，在該坐標(biāo)系內(nèi)計(jì)算位移．但重力加速度的去除卻是按照 Ox(F)y(R)z(D)坐標(biāo)系下的處理方式進(jìn)行．該方法在重力加速度去除過程中存在的誤差為

由圖 2中可看出測試人員行走 1步持續(xù)時(shí)間 t1到 t10大約為 1,s．若取和均為 5°，重力加速度為 9.8,m/s2，則式(7)描述的加速度誤差在 O,XYZ坐標(biāo)系內(nèi)計(jì)算的位移誤差為：該誤差足以導(dǎo)致錯(cuò)誤的步長計(jì)算，可見初始方位角對重力加速度去除的影響不可忽略．

2.2 初始方位角為參量的步長計(jì)算方法

聯(lián)立式(6)和式(8)，結(jié)合慣性導(dǎo)航原理可計(jì)算出行走 1步在 OXYZ坐標(biāo)系下產(chǎn)生的三軸位移矢量和將 OXYZ下的位移投影到定位目標(biāo)坐標(biāo)系Ox(F)y(R)z(D)內(nèi)，有

計(jì)算得到坐標(biāo)系OXYZ和Ox(F)y(R)z(D)下的位移均為初始方位角和的函數(shù)表達(dá)式．Ox(F)y(R)z(D)是以人員行走方向 x(F)為基準(zhǔn)建立的本地坐標(biāo)系，由 OXYZ坐標(biāo)系的定義可知，兩坐標(biāo)系間的方位角(偏航角)0ψ為零．

由第1.1節(jié)可知，在每個(gè)SVP點(diǎn)，人體需克服重力使脛骨與地面垂直來實(shí)現(xiàn)身體平衡．可認(rèn)為在坡度較小(小于 20°)的路面上，脛骨垂直于地面，即圖4(a)中OYZ和Oy(R)z(D)在同一平面內(nèi)，0γ為零．

結(jié)合式(6)和式(8)，OXYZ坐標(biāo)系下的導(dǎo)航方程簡化為

式中N為1步內(nèi)采樣得到的慣性參量個(gè)數(shù)．

由OXYZ坐標(biāo)系的位置定義可知，X軸位移分量SX為人員沿著路面向前的位移，Y軸位移分量 SY為人員沿著路面右向的位移．X軸和Y軸位移分量的矢量和即為步長．因此，步長Step_Length的計(jì)算式為

本文致力于人員行走距離的計(jì)算，式(12)可計(jì)算出每1步的步長，累加所有步長即得行走距離．

2.3 初始方位角的解算

通過大量的測量實(shí)驗(yàn)，人的行走步長一般不超過1.6,m(1步包括左腳和右腳均向前邁進(jìn) 1步)．對于坡度較小的道路，可認(rèn)為1.6,m內(nèi)的路面在一個(gè)平面上且Z軸指向垂直于地面方向．因此在1步內(nèi)，垂直地面方向的位移為零，即結(jié)合式(11)，有

由三角函數(shù)關(guān)系式，有

將式(14)代入式(11)中XS的表達(dá)式計(jì)算出XS的數(shù)值，結(jié)合式(11)中已經(jīng)計(jì)算出具體數(shù)值的YS，據(jù)式(12)得到行進(jìn)步長．

3 系統(tǒng)實(shí)現(xiàn)及實(shí)驗(yàn)結(jié)果分析

3.1 實(shí)驗(yàn)系統(tǒng)搭建

戰(zhàn)略柔性是企業(yè)技術(shù)創(chuàng)新的源泉，是企業(yè)高效利用資源的重要保障。通過增強(qiáng)戰(zhàn)略柔性，提高資源柔性和協(xié)調(diào)柔性水平，增強(qiáng)中小企業(yè)自主創(chuàng)新能力，提高生產(chǎn)效率，先于競爭對手進(jìn)入新市場。

慣性測量單元 IMU選用模擬器件公司 ADI生產(chǎn)的ADIS16405,BMLZ，如圖5所示．該慣性模塊集成測量范圍可伸縮設(shè)置的三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)和內(nèi)置的溫度傳感器．內(nèi)部元件正交坐標(biāo)軸與封裝機(jī)體坐標(biāo)軸的對準(zhǔn)角度偏差小于0.05°，噪聲密度小于 0.05°·s-1在設(shè)定的內(nèi)部采樣周期下，IMU對其運(yùn)動慣性參量進(jìn)行采樣并更新對應(yīng)的內(nèi)部寄存器值，其最高采樣率可達(dá) 819.2次/s，并提供標(biāo)準(zhǔn)串行外設(shè)接口(serial peripheral interface，SPI)．外部處理單元可通過 SPI接口直接訪問其內(nèi)部寄存器，讀取慣性參量．

圖5 實(shí)驗(yàn)系統(tǒng)采用的慣性測量單元Fig.5 Inertial measurement unit for experimental system

外部處理單元主要由便攜式計(jì)算機(jī)組成，利用Cypress公司 CY7C68013A-128AXC橋接計(jì)算機(jī)和IMU器件．計(jì)算機(jī)通過USB接口將IMU采樣的慣性參量讀取到內(nèi)存，進(jìn)行實(shí)時(shí)分析和計(jì)算，并保存原始慣性數(shù)據(jù)．本文設(shè)定IMU的采樣頻率為409.6次/s．

實(shí)驗(yàn)中，便攜式計(jì)算機(jī)選擇聯(lián)想 Thinkpad X220，其配置為：CPU i5-2520，2.5,GHz，內(nèi)存DDR3 4,GB．操作系統(tǒng)為Windows XP Professional SP3，計(jì)算軟件環(huán)境為Matlab 2008a．

3.2 實(shí)驗(yàn)人員及實(shí)驗(yàn)描述

實(shí)驗(yàn)人員為課題組中志愿進(jìn)行行走測試的 3名學(xué)生，身高分別為168,cm、171,cm和175,cm．

行走路面選擇室外長為103.5,m的普通道路．每個(gè)測試人員均按平常的走路習(xí)慣，分別以正常速度、快速、慢速和快慢速無規(guī)律交替的變速 4種行走速度，在該段路面進(jìn)行來回行走實(shí)驗(yàn)．要求每個(gè)測試人員每個(gè)速度等級均有一次來回實(shí)驗(yàn)．每名測試者至少有 7次實(shí)驗(yàn)數(shù)據(jù)，其中每個(gè)實(shí)驗(yàn)至少進(jìn)行了 4次．3名測試人員按上述方法共測得34組數(shù)據(jù)，每組數(shù)據(jù)按測試人員和行走速度等級分別標(biāo)記．

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

為了驗(yàn)證本文提出的基于改進(jìn)步長估計(jì)的行走距離計(jì)算方法的準(zhǔn)確性，利用Matlab對34次行走實(shí)驗(yàn)保存的慣性數(shù)據(jù)進(jìn)行離線分析與計(jì)算．

計(jì)算步長前，先對連續(xù)行走過程進(jìn)行分步處理使慣性數(shù)據(jù)與行走過程每個(gè)階段相互對應(yīng)．第1.1節(jié)中對行走過程進(jìn)行了詳細(xì)的分解，脛骨與地面垂直時(shí)，三軸角速度幾乎為零．行走過程中，脛骨沿yb軸做類似倒立擺運(yùn)動，其角速度信號較為明顯，通過 yb軸角速度找出每一步 SVP點(diǎn)．選取其中一位測試人員在正常行走速度下測得的慣性數(shù)據(jù)為例，標(biāo)出選定的SVP點(diǎn)，如圖6所示的紅色圓點(diǎn)．圖6中，xb和zb軸的角速度信號在 SVP點(diǎn)也幾乎為零，進(jìn)一步印證了SVP點(diǎn)可作為ZUPT點(diǎn)．

圖6 行走過程角速度數(shù)據(jù)及ZUPT點(diǎn)分步Fig.6 Angular velocity of walking and ZUPT points

利用第 2節(jié)所述的脛骨捷聯(lián)慣性位移導(dǎo)航計(jì)算方法，以每一步為導(dǎo)航周期計(jì)算步長，累加所有步長得到每次實(shí)驗(yàn)所行走的距離，用Proposed_Method標(biāo)記．同時(shí)，按照文獻(xiàn)[12]所運(yùn)用的方法計(jì)算每一步的步長和總的行走距離，標(biāo)記為 Ref_Method．文獻(xiàn)[12]只考慮了人員行走過程的二維運(yùn)動，忽略其三維特性．本文在其基礎(chǔ)上考慮了運(yùn)動的三維特性，將單軸角速度改為三軸角速度來計(jì)算轉(zhuǎn)換矩陣，實(shí)現(xiàn)三軸加速度的分解，其他計(jì)算方法和步驟與之完全相同，計(jì)算步長和總的行走距離，標(biāo)記為3DMotion_Method．

分別將3種方法計(jì)算的總距離與實(shí)際行走的距離103.5,m進(jìn)行比較，得到34次實(shí)驗(yàn)3種算法距離誤差的百分比曲線，如圖7所示，橫軸表示實(shí)驗(yàn)次數(shù)，縱軸表示該次實(shí)驗(yàn)行走距離誤差百分?jǐn)?shù)．對比34組實(shí)驗(yàn)結(jié)果，并按其誤差范圍進(jìn)行相應(yīng)的統(tǒng)計(jì)，如表1所示，得出行走距離誤差在一定范圍內(nèi)的概率分布情況．

圖7 3種算法下行走距離的誤差比較Fig.7 Comparison of the distance error for three methods

表1 3種算法性能比較Tab.1 Comparison of the performance for three methods

由圖7可知，只考慮二維運(yùn)動且設(shè)定初始方位角為零的 Ref_Method方法，每次實(shí)驗(yàn)計(jì)算的總距離與實(shí)際行走距離間的誤差較大，每次實(shí)驗(yàn)誤差的波動范圍也較大．導(dǎo)致此類誤差的主要原因是：①對不同的測試人員，IMU的綁定位置并不能完全滿足該算法的要求，使得在 SVP點(diǎn)，初始方位角與零值相差較大，按零值處理產(chǎn)生較大的位移偏差；②在不平整的道路上，走路姿勢隨意，使脛骨運(yùn)動不是理想的二維模型，從而導(dǎo)致重力加速度的分解并不完全在一個(gè)二維平面內(nèi)．導(dǎo)航方程中水平和垂直兩方向的加速度并不完全由人員行走運(yùn)動產(chǎn)生，而是疊加了重力加速度去除過程產(chǎn)生的誤差．

相比Ref_Method方法，3DMotion_Method方法充分考慮了行走的三維特性．但初始方位角設(shè)定為零，導(dǎo)致在 SVP點(diǎn)時(shí)分解重力加速度產(chǎn)生與Ref_Method方法相同的去除誤差．初始方位角為零還使得 3DMotion_Method沒有去除重力加速度在水平方向的分量，而是與線性加速度疊加．水平方向的線性加速度比實(shí)際加速度小，計(jì)算的步長普遍偏小，總距離誤差較大．相對 Ref_Method，3DMotion_ Method誤差波動范圍較平緩，說明考慮運(yùn)動的三維特性能減小步長計(jì)算的差異程度．

Proposed_Method在 3DMotion_Method的基礎(chǔ)上，假設(shè)初始方位角未知，并不直接將其設(shè)定為零值．導(dǎo)航計(jì)算出含有未知數(shù)的位移表達(dá)式，從而使重力加速度的去除過程無任何近似．方位角的解算和步長的計(jì)算過程，可看成由垂直地面方向位移ZS的誤差，得到水平位移的補(bǔ)償參量0sinρ，用來修正水平方向的位移XS和YS，使水平位移更準(zhǔn)確，提高步長計(jì)算的精度．實(shí)驗(yàn)結(jié)果表明，Proposed_Method的位移誤差較 3DMotion_Method和 Ref_Method小得多．因此，三維運(yùn)動特性和初始方位角均對步長計(jì)算的精度有較大影響，應(yīng)充分考慮這兩者在基于捷聯(lián)慣性導(dǎo)航的人員位置跟蹤算法中的處理方法．本文提出的方位角解算步長改進(jìn)算法提高了步長計(jì)算的精度和穩(wěn)定性，將誤差控制在行走距離的10%以內(nèi)．

結(jié)合圖7，表1顯示Proposed_Method距離誤差范圍小的概率較 3DMotion_Method和 Ref_Method有較大幅度提高. 3DMotion_Method相比Ref_Method小誤差范圍的概率小，大部分為零；但其誤差曲線相對較平緩，得益于三維運(yùn)動特性下的加速度轉(zhuǎn)換．Proposed_Method在3DMotion_Method基礎(chǔ)上，加入初始方位角解算，性能大幅度提升，計(jì)算的行走距離誤差在8%以內(nèi)的概率為91%．

圖 8對 4種行走速度等級下距離計(jì)算誤差進(jìn)行了對比．由圖8可見，Proposed_Method和3DMotion_ Method在各種速率等級下誤差相對平緩，Ref_Method誤差波動大，無規(guī)律．說明行走速度對運(yùn)動的三維特性有較大的影響．Proposed_Method在3DMotion_Method的基礎(chǔ)上進(jìn)行初始方位角解算，誤差減?。f明行走速度對 SVP點(diǎn)初始方位角的影響不可忽視．Proposed_Method能很好地適應(yīng)各種行走速度，適用性較好．

圖8 不同行走速度下的距離誤差比較Fig.8 Comparison of the distance error for different walking speeds

圖9對比了3位測試人員的行走實(shí)驗(yàn)結(jié)果．由于測試人員1的實(shí)驗(yàn)次數(shù)較多，故將測試人員2和3的實(shí)驗(yàn)數(shù)據(jù)放在一起．圖 9(a)為測試人員 1的行走實(shí)驗(yàn)結(jié)果；圖9(b)為測試人員2和3的實(shí)驗(yàn)結(jié)果，其中前 7次為測試人員 2，后 6次為測試人員 3．由圖 9的實(shí)驗(yàn)結(jié)果可知，每位測試人員距離計(jì)算誤差Proposed_Method較3DMotion_Method和 Ref_ Method均小得多．Proposed_Method對人員的適應(yīng)性較好，不同的行走人員無需更改任何系統(tǒng)參數(shù)，通用性較好．

圖9 不同測試者的行走距離誤差比較Fig.9 Comparison of the distance error for differenet volunteers

4 結(jié) 語

本文改進(jìn)了步長計(jì)算過程中 ZUPT時(shí)刻捷聯(lián)慣性導(dǎo)航初始方位角參數(shù)的設(shè)定方法，以該方位角為中間量推導(dǎo)步長的函數(shù)關(guān)系式．詳細(xì)分析人員行走時(shí)脛骨運(yùn)動特性和初始方位角的特點(diǎn)，提出三軸位移的近似方法和約束條件，解算初始方位角并求得步長，進(jìn)而估計(jì)人員行進(jìn)距離．該方法將重力加速度的去除誤差從重積分中剝離出來，避免了重積分對誤差的擴(kuò)散，提高了步長計(jì)算的精度．且該方法對人員行走方式無特殊要求，對不平整路面的適應(yīng)性也較好．通過 34次 103.5,m的不平整道路上直線行走實(shí)驗(yàn)驗(yàn)證，計(jì)算的行走距離誤差在8%以內(nèi)的概率為91%．

本文基于慣性導(dǎo)航原理對人員行進(jìn)的距離進(jìn)行估計(jì)，為室內(nèi)人員定位跟蹤提供一種可參考的相對位置估計(jì)方法．但慣性導(dǎo)航固有的累積誤差無法滿足長時(shí)間的室內(nèi)人員定位需求．未來的研究中，將會加入一定精度的絕對定位方法，與精度相對較高的短時(shí)慣性導(dǎo)航定位結(jié)合，實(shí)現(xiàn)高精度的室內(nèi)人員定位跟蹤及導(dǎo)航系統(tǒng)．

［1］ Jimenez A R，Seco F，Prieto C，et al. A comparison of pedestrian dead-reckoning algorithms using a low-cost MEMS IMU[C]// Intelligent Signal Processing(WISP). Budapest，Hungary，2009：37-42.

［2］ Torres-Solis J，F(xiàn)alk T H，Chau T. A Review of Indoor Localization Technologies：Towards Navigational Assistance for Topographical Disorientation[M]. Rijeka，Croatia：Ambient Intelligence，2010：51-83.

［3］ Woodman O. An Introduction to Inertial Navigation[R]. Cambridge，UK：University of Cambridge，2007.

［4］ Kelly A. Personal Navigation System Based on Dual Shoe-Mounted IMUs and Intershoe Ranging[R]. MA，USA：Precision Personnel Locator Workshop，2011.

［5］ Mathiassen K，Hanssen L，Hallingstad O. A low cost navigation unit for positioning of personnel after loss of GPS position[C]// International Conference on Indoor Positioning and Indoor Navigation(IPIN). Zurich，Switzerland，2010.

［6］ Abdulrahim K，Hide C，Moore T，et al. Aiding MEMS IMU with building heading for indoor pedestrian navigation[C]// Ubiquitous Positioning Indoor Navigation and Location Based Service(UPINLBS). Helsinki，

Finland，2010：1-6.

［7］ Castaneda N，Lamy-Perbal S. An improved shoemounted inertial navigation system[C]// International Conference on Indoor Positioning and Indoor Navigation(IPIN). Zurich，Switzerland，2010.

［8］ Jimenez A R. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a footmounted IMU[C]// 7th Workshop on Positioning Navigation and Communication(WPNC). Germany，2010：135-143.

［9］ Nilsson J O，Skog I，El P. Performance characterization of foot-mounted ZUPT-aided INSs and other related systems[C]// International Conference on Indoor Positioning and Indoor Navigation(IPIN). Zurich，Switzerland，2010.

［10］ Huang C，Liao Z，Zhao L. Synergism of INS and PDR in self-contained pedestrian tracking with a miniature sensor module[J]. IEEE Sensors Journal，2010，10(8)：1349-1359.

［11］ Alvarez D，Gonzalez R C，Lopez A，et al. Comparison of step length estimators from wearable accelerometer devices[C]// 28th Annual International Conference of the IEEE Engineering in Medicine and Biology Society. USA，2006：5964-5967.

［12］ Li Q，Young M，Naing V，et al. Walking speed estimation using a shank-mounted inertial measurement unit[J]. Journal of Biomechanics，2010，43(8)：1640-1643.

（責(zé)任編輯：金順愛）

Pedestrian Walking Distance Estimation Based on Strap-Down Inertial Navigation of Shank

Wang Shaochu1,2，Liu Kaihua1，Liu Yu1
(1. School of Electronic Information Engineering，Tianjin University，Tianjin 300072，China；2. Tianjin Institute of Surveying and Mapping，Tianjin 300381，China)

This paper studies the step length calculation scheme based on shank mounted inertial measurement unit (IMU), and proposes a method for initial parameters resetting at the zero velocity update (ZUPT) time of one navigation period. The accurate expressions of three-dimensional displacement functions based on initial orientationwere obtained without diffusion of approximation error by double integral. Taking into account the pedestrian walkingcharacteristics and the vertical relationship between the shank and pavement at the moment of ZUPT, with the restraint of vertical displacement being regarded as zero at one navigation period, the other two horizontal displacementswere calculated, and the step length was easily obtained. Without modifying the parameters of the system, this method has the advantages of adaptability and versatility for different pedestrians and walking environments. By volunteers’ walking experiment verification, the probability of total walking distance error less than 8% is 91%.

strap-down inertial navigation；zero velocity update；pedestrian dead-reckoning

TN967.2

A

0493-2137(2014)08-0719-09

10.11784/tdxbz201211045

2012-11-23；

2013-01-07.

國家自然科學(xué)基金資助項(xiàng)目(61373102).

汪少初（1983— ），男，博士研究生，wangshaochu@tju.edu.cn.

劉開華，liukaihua@tju.edu.cn.