基于歷史信息約束的室內(nèi)行人三維慣性定位誤差修正方法

丁一鳴，熊 智，曹志國，李婉玲，王鉦淳

（南京航空航天大學 自動化學院 導航研究中心，南京 211106）

慣性行人定位系統(tǒng)在消防安保、地下巡檢、智慧城市等領(lǐng)域具有巨大的潛在應用價值，然而由于慣性傳感器（Inertial Measurement Unit, IMU）噪聲的影響，慣性行人定位系統(tǒng)的定位誤差隨時間累積。尤其是在高度估計中，由于重力場的變化、哥氏加速度和慣性平臺姿態(tài)角的影響以及加速度計本身的誤差，使得慣性定位系統(tǒng)的高度誤差呈現(xiàn)發(fā)散的趨勢。

國內(nèi)外學者針對行人慣性定位系統(tǒng)累計誤差的抑制技術(shù)進行了深入研究，按照是否需要其他信源可以劃分為兩類：一類是多源信息融合方法，這類方法使用如衛(wèi)星定位系統(tǒng)（Global Navigation Satellite System,GNSS）[1]、WiFi[2]、超寬帶（Ultra-Wide Band, UWB）[3]等被動定位技術(shù)，或如視覺[4]、激光雷達[5]、氣壓計[6]等自主定位技術(shù)與慣性定位進行融合，修正慣性系統(tǒng)的定位誤差。這類系統(tǒng)能夠?qū)崿F(xiàn)長時間穩(wěn)定的行人室內(nèi)定位，同時也受使用環(huán)境、體積成本等因素的影響。與被動定位技術(shù)融合的行人定位方法由于需要在環(huán)境中預先部署輔助定位設(shè)施，因此其使用范圍相對受限，與主動定位技術(shù)融合的定位系統(tǒng)體積較大、成本和功耗較高。另一類是將慣性傳感器與人體固聯(lián)，如足部[7]、胸部[8]、腿部[9]固聯(lián)，使用人體運動模型建立虛擬觀測約束慣性累積誤差。這類行人定位系統(tǒng)無需任何輔助定位設(shè)施，具有自主性好、可靠性高、適用范圍廣等優(yōu)勢，但由于人體運動模型對位置的觀測性較弱，因此該類系統(tǒng)能夠在短時間內(nèi)保持較好的定位性能，但其定位誤差仍呈發(fā)散趨勢，難以適應長時間精準定位需求。

為滿足行人長時間穩(wěn)定定位需求，Puyol等人[10]提出一種針對行人慣性航位推算系統(tǒng)的后端優(yōu)化方法，使用六邊形網(wǎng)格表征地圖，采用FastSLAM[11]算法架構(gòu)估計行人位姿與地圖的聯(lián)合后驗概率分布來消除行人慣性航位推算系統(tǒng)的累計誤差。該方法在提高純慣性定位系統(tǒng)長時間定位精度穩(wěn)定性的同時無需任何其他傳感器，對室內(nèi)慣性行人定位的發(fā)展具有重要意義。然而這種方法中六邊形的地圖表征形式和穿邊概率的表征方式存儲空間大、計算復雜度高，難以在低成本系統(tǒng)中實際應用，是一種準實時行人慣性定位方法。文獻[12]提出一種基于四邊形占用柵格地圖的慣性構(gòu)圖與誤差修正方法，該方法使用四邊形占用地圖表征空間可通行區(qū)域的概率分布，降低了算法所需存儲空間，提高了計算效率，能夠在低成本系統(tǒng)中實現(xiàn)慣性地圖的實時構(gòu)建與行人慣性系統(tǒng)誤差的實時修正。但是該算法僅適用于行人水平方向定位誤差修正與二維地圖構(gòu)建。

為此本文將基于四邊形占用柵格地圖的慣性構(gòu)圖與誤差修正方法進行推廣，提出一種基于歷史信息約束的室內(nèi)行人三維慣性定位誤差修正方法，使用六面體構(gòu)建相應區(qū)域的三維占用柵格地圖。并利用行人的歷史位姿修正地圖的構(gòu)建誤差和慣性系統(tǒng)的累計誤差，提高純慣性系統(tǒng)長時間水平和高度方向上定位精度的穩(wěn)定性。本文搭建了半物理實驗平臺，驗證了所提出的方法的有效性。

1 基于三維慣性構(gòu)圖的室內(nèi)行人定位算法

1.1 系統(tǒng)架構(gòu)

基于三維慣性構(gòu)圖的室內(nèi)行人導航系統(tǒng)由行人慣性航位推算和后端構(gòu)圖優(yōu)化兩部分組成，其本質(zhì)是估計行人三維位姿x1:t和地圖Θ1:t的聯(lián)合概率分布：

其中u1:t為行人慣性航位推算的步長-航向變化-高度變化控制向量，z1:t為根據(jù)行人對環(huán)境可通行區(qū)域的虛擬觀測。根據(jù)Rao-Blackwellize法則[11]，式(1)可分解為：

其中， Θ= {θ1,θ2,… ,θN},θi為三維占用柵格地圖中的第i個六面體，N為地圖中六面體總數(shù)量。次分解表明對于位姿和地圖的聯(lián)合后驗概率分布問題可以分解為一個位姿估計問題和N個已知行人位姿的地圖六面體可通行概率估計問題。

本系統(tǒng)中行人慣性航位推算部分將慣性傳感器固聯(lián)于足部，慣導系統(tǒng)經(jīng)過導航解算實時輸出行人的位置、速度、姿態(tài)等導航信息，利用足部運動的周期性零速特性分割步態(tài)，并通過卡爾曼濾波對導航參數(shù)進行修正，輸出行人每一步的步長、航向變化與高度變化構(gòu)成控制向量。利用控制向量，通過構(gòu)建行人三維航位推算模型獲得行人三維位姿的一步預測。后端構(gòu)圖優(yōu)化利用前端的輸出構(gòu)建三維占用地圖，當行人再次經(jīng)過已構(gòu)建地圖區(qū)域時優(yōu)化地圖與位姿估計，并將優(yōu)化后的導航參數(shù)作為系統(tǒng)的導航結(jié)果輸出。算法架構(gòu)如圖1所示。

圖1 基于三維慣性構(gòu)圖的室內(nèi)行人定位算法架構(gòu)圖Fig.1 Architecture diagram for indoor pedestrian 3D inertial-SLAM

本文主要針對后端構(gòu)圖優(yōu)化算法進行設(shè)計，關(guān)于足綁式零速修正行人慣性航位推算系統(tǒng)這里不再贅述，讀者可以參考文獻[12]。

1.2 三維地圖建模

本文對基于四邊形占用柵格地圖的慣性構(gòu)圖方法進行推廣，使用六面體占用地圖劃分行人的運動空間。圖2展示了一個樓梯情景下六面體環(huán)境表征示意圖，圖中三維空間被等尺寸六面體均勻分割，其中灰色六面體為樓梯建筑結(jié)構(gòu)，紅色曲線為行人的運動軌跡。當行人初次到達該空間中，所有的六面體皆為未知區(qū)域，隨著行人登上樓梯，行人運動軌跡所歷經(jīng)的六面體可通行的概率變大（對應圖中藍色六面體）。每一個六面體包含三維地圖坐標系下位置索引和該六面體為可通行區(qū)域的后驗概率上標θ表示該坐標為地圖坐標系的索引，與行人的位置作區(qū)分。將六面體長、寬、高分別記為lθ、wθ、hθ，則載體位置p=[px,py,pz]T與六面體索引存在如下關(guān)系：

圖2 樓梯情景下六面體環(huán)境表征示意圖Fig.2 Schematic representation of the hexahedral environment in the staircase scenario

其中＜*＞表示對向量中的每一個元素進行四舍五入運算。值得注意的是，為了減小地圖離散化造成的誤差，六面體各邊長參數(shù)的選擇要盡量保證根據(jù)位姿所構(gòu)建的可通行區(qū)域是連續(xù)的，因此通常將六面體的長、寬設(shè)置為平均步長的2倍長，將六面體的高度選擇為運動平均高度變化的2倍長。

六面體的可通行后驗概率使用靜態(tài)二值貝葉斯濾波器進行迭代估計：

其中

為六面體可通行后驗概率對數(shù)比，x1:t為行人1到t時刻的位姿，z1:t為1到t時刻行人對所在六面體的觀測，θi為地圖中第i個六面體。p（θi|zt,xt） ? （ 0.5,1]為行人對六面體可通行概率的反觀測模型，p(θi)為六面體可通行概率的先驗概率，由于事先沒有環(huán)境地圖信息，因此對于環(huán)境中未被經(jīng)歷過六面體的可通行先驗概率皆為0.5。

1.3 地圖與位姿誤差修正

由于行人和地圖的傳播過程是非線性的，同時其分布并非高斯分布，因此本文采用粒子濾波器對式(4)進行估計。每個粒子的狀態(tài)包括行人所有時刻的位姿和地圖：

根據(jù)行人航位推算模型，建立狀態(tài)方程如下：

其中上標[m]表示第m個粒子。

權(quán)重更新后根據(jù)粒子當前狀態(tài)結(jié)合1.2節(jié)所述地圖的靜態(tài)二值貝葉斯濾波器更新地圖各六邊形為可通行區(qū)域的后驗概率。再依據(jù)粒子的權(quán)重對粒子集合進行重要性重采樣，重采樣后的粒子服從目標分布，即得到了粒子和地圖的聯(lián)合后驗概率分布。最后將位姿估計期望反饋到行人慣性航位推算系統(tǒng)中，修正行人位姿。

2 實驗結(jié)果分析

2.1 實驗條件

本文所提算法為基于零速修正行人慣性定位算法的級聯(lián)濾波器，為了驗證所提算法的性能，利用原始的零速修正算法進行了對比試驗。為了避免粒子濾波中蒙特卡洛抽樣隨機性對結(jié)果的影響，在相同測試條件下，進行了10次對比試驗。實驗環(huán)境為南京航空航天大學自動化學院1號樓，實驗樓衛(wèi)星俯視圖如圖3所示，實驗樓平面圖如圖4所示，占地面積約為3000平方米，共五層，層高為3米，單層走廊通道周長約為200米。為評估系統(tǒng)定位精度，使用高精度輪式里程計預先標定了如圖4紅色圓點所示的四個真值點。

圖3 實驗環(huán)境衛(wèi)星圖Fig.3 Satellite image of experimental environment

圖4 實驗環(huán)境平面圖Fig.4 Floor plan of the experimental environment

實驗使用For Sense公司的FSS-IMU6132型IMU固聯(lián)于足部，安裝方式如圖5所示。

圖5 足綁式慣性傳感器安裝示意圖Fig.5 Foot-mounted inertial sensor installation diagram

IMU性能參數(shù)如表1所示，IMU采樣頻率為100 Hz。測試人員在真值點1處出發(fā)，沿藍色軌跡運動，并在兩處樓梯上下樓兩層，軌跡重復多次后最終回到起點。測試總時長為34分鐘，總路程為1992米。本實驗中所提算法的濾波器相關(guān)參數(shù)設(shè)置如表2所示。

表1 傳感器性能Tab.1 sensor performance

表2 濾波器參數(shù)設(shè)置Tab.2 filter parameters setting

2.2 實驗結(jié)果分析

2.2.1 行人慣性航位推算系統(tǒng)定位結(jié)果及分析

行人慣性航位推算系統(tǒng)利用足部IMU測量足部運動的加速度和角速度，通過捷聯(lián)慣導解算和零速修正獲得行人運動控制向量，并經(jīng)過三維航位推算模型最終獲得行人的三維位姿估計。圖6為原始行人慣性航位推算系統(tǒng)的定位軌跡，圖7為對應軌跡的俯視圖，可以看到當行人運動第一圈時水平定位誤差較小，但隨行人經(jīng)過多次轉(zhuǎn)彎后，由于傳感器噪聲、標度因數(shù)誤差等因素，航向逐漸偏離真值，軌跡逐漸偏轉(zhuǎn)。圖8為行人慣性航位推算系統(tǒng)定位誤差的側(cè)視圖，圖中高度估計和水平定位趨勢相同，初始時高度估計誤差較小，但隨時間推移逐漸發(fā)散，且發(fā)散速度較快。這主要是因為行人每一步觸地時垂直方向加速度變化速度快、范圍大，激勵出的加速度噪聲較大、成分復雜。

圖6 行人慣性航位推算系統(tǒng)定位結(jié)果（三維視圖）Fig.6 Inertial pedestrian dead reckoning system positioning results（3D view）

圖7 行人慣性航位推算系統(tǒng)定位結(jié)果（俯視圖）Fig7 Inertial pedestrian dead reckoning system positioning results (top view)

圖8 行人慣性航位推算系統(tǒng)定位結(jié)果（側(cè)視圖）Fig.8 Inertial pedestrian dead reckoning system positioning results (side view)

圖9 -12中紅色曲線分別為原始行人慣性航位推算系統(tǒng)在真值點處x方向、y方向、高度方向三維定位誤差，可以看到，每一步定位誤差經(jīng)過行人行位推算模型逐步累積，水平定位誤差逐漸增大，又由于缺少航向和位置觀測，水平定位誤差和航向的累積誤差得不到有效修正。

圖9 行人慣性航位推算系統(tǒng)定位結(jié)果與本文方法x方向定位誤差對比圖Fig.9 Comparison of inertial pedestrian dead reckoning system with the x-directional positioning error of the proposed method

圖10 行人慣性航位推算系統(tǒng)定位結(jié)果與本文方法y方向定位誤差對比圖Fig.10 Comparison of inertial pedestrian dead reckoning system with the y-directional positioning error of the proposed method

圖11 行人慣性航位推算系統(tǒng)定位結(jié)果與本文方法高度定位誤差對比圖Fig.11 Comparison of inertial pedestrian dead reckoning system with the height positioning error of the proposed method

圖12 行人慣性航位推算系統(tǒng)定位結(jié)果與本文方法三維定位誤差對比圖Fig.12 Comparison of inertial pedestrian dead reckoning system with the proposed method for 3D positioning errors

2.2.2 基于歷史信息約束的室內(nèi)行人三維慣性定位系統(tǒng)地圖構(gòu)建結(jié)果及分析

圖13 -15為本文所提方法的地圖構(gòu)建結(jié)果，圖中顏色隨高度變化，注意圖中坐標軸為六面體索引，即水平方向索引總數(shù)與表2中六面體對應邊長之積為所構(gòu)地圖的實際長度。從俯視圖（圖14）中可以看到地圖中可通行區(qū)域與圖4中的走廊區(qū)域基本一致，其中紅色線框處藍色四邊形實際應與黃色四邊形重合，對應三維視圖紅色框處藍色樓層x為30處走廊x方向估計出現(xiàn)偏差。

圖13 基于歷史信息約束的室內(nèi)行人三維慣性定位系統(tǒng)構(gòu)圖結(jié)果（三維視圖）Fig.13 Proposed method mapping results(3D view)

圖14 基于歷史信息約束的室內(nèi)行人三維慣性定位系統(tǒng)構(gòu)圖結(jié)果（俯視圖）Fig.14 Proposed method mapping results(top view)

圖15 基于歷史信息約束的室內(nèi)行人三維慣性定位系統(tǒng)構(gòu)圖結(jié)果（側(cè)視圖）Fig.15 Proposed method mapping results(side view)

導致該偏差主要原因有二：其一是粒子不足以表征對應時刻的狀態(tài)分布。行人航位推算模型的非線性特性和地圖分布的非高斯特性，本文使用粒子濾波表征行人位姿和地圖分布。在定位初始時行人位姿的不確定較小，7000個粒子尚能夠較為準確地表征粒子狀態(tài)分布，但隨著行人的運動位姿和地圖的不確定增加，粒子數(shù)逐漸不足以完整地表征狀態(tài)的聯(lián)合分布，當行人運動到已經(jīng)構(gòu)建的可通行區(qū)域時，狀態(tài)估計無法收斂到真值處。

其二地圖的離散化誤差也使得定位里程較真值里程更短。行人每到達一個新的六面體時根據(jù)前一個時刻的地圖更新權(quán)重，此時新的六面體可通行概率為0.5，其附近已經(jīng)歷過的六面體可通行概率經(jīng)過地圖更新后較新的六面體更大，當位姿的一步預測分布有一部分落入已經(jīng)歷過的六面體時，該部分粒子被賦予較高權(quán)重，粒子位姿的期望即更傾向于已經(jīng)構(gòu)建的六面體所在位置，即與真值之間產(chǎn)生偏差，這部分偏差經(jīng)過地圖更新留存在地圖估計中，在此后的估計過程中持續(xù)造成影響。

2.2.3 基于三維慣性構(gòu)圖的室內(nèi)行人導航系統(tǒng)定位結(jié)果及分析

圖16為本文所提方法平均定位結(jié)果的三維視圖，圖17和圖18分別為對應的俯視圖與側(cè)視圖。從圖16可以看到本文所提方法能夠較準確地反映行人的運動軌跡。10次試驗的平均三維定位誤差如圖11中藍色曲線所示，其中灰色區(qū)域表示這10次試驗三維定位誤差的標準差，在可以看到相較原始行人慣性航位推算系統(tǒng)誤差（紅色曲線）隨時間累積的情況，本文定位誤差始終穩(wěn)定在較低水平。

圖16 基于歷史信息約束的室內(nèi)行人三維慣性定位系統(tǒng)定位結(jié)果（三維視圖）Fig.16 Proposed method mapping results(3D view)

可以觀察到的是藍色曲線雖然保持在較低水平，但仍存在固定偏差，該偏差即為地圖構(gòu)建偏差對定位估計的影響導致。對應圖17可以看到，在右側(cè)兩個轉(zhuǎn)角處本文方法的位置估計與起點的距離和真值與起點的距離相比較短。高度估計上由于地圖構(gòu)建所需六面體個數(shù)較少，地圖構(gòu)建偏差較小，對高度估計的影響也較小，如圖18所示，因此高度估計相對準確。表3為原始行人慣性航位推算系統(tǒng)與本文方法在所有真值點處定位均方根誤差對比，可以看到通過級聯(lián)后端構(gòu)圖優(yōu)化算法，足綁式行人慣性航位推算系統(tǒng)的三維定位精度由22.7268米/1992米提升至3.0712米/1992米,性能提升約86.5%。

圖17 基于歷史信息約束的室內(nèi)行人三維慣性定位系統(tǒng)定位結(jié)果（俯視圖）Fig.17 Proposed method mapping results(top view)

圖18 基于歷史信息約束的室內(nèi)行人三維慣性定位系統(tǒng)定位結(jié)果（側(cè)視圖）Fig.18 Proposed method mapping results(side view)

表3 行人慣性航位推算系統(tǒng)與本文方法均方根誤差Tab.3 pedestrian inertial odometer with proposed method root mean square error

3 結(jié) 論

本文針對慣性行人航位推算系統(tǒng)的累積誤差問題提出了一種僅使用慣性信息的三維構(gòu)圖與定位方法。通過在行人慣性航位推算系統(tǒng)后端級聯(lián)構(gòu)圖優(yōu)化算法，使用六面體表征行人運動環(huán)境，在Fast SLAM框架下采用靜態(tài)二值貝葉斯濾波估計環(huán)境中的可通行區(qū)域，并利用構(gòu)建的可通行區(qū)域地圖修正系統(tǒng)的定位誤差。本文結(jié)合實際室內(nèi)環(huán)境，對算法進行了驗證，實驗結(jié)果表明本文提出的三維慣性構(gòu)圖室內(nèi)行人導航方法能夠有效抑制行人慣性航位推算系統(tǒng)的累積誤差，提升定位精度?？梢灶A見的是本文算法并不僅限于行人載體，有助于提升封閉環(huán)境下機器人、飛行器等多種載體的純慣性性能。