一種腳綁IMU輔助的行人室內(nèi)定位定姿方法

曠 儉，劉 韜，牛小驥

(武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079)

0 引言

現(xiàn)階段的室內(nèi)定位技術(shù)非常多，比如WiFi[1]、藍(lán)牙[2]、磁場[3]、視覺[4]和超寬帶(UWB)[5]等。然而對于不同的定位手段，定位精度和系統(tǒng)成本差異巨大。因此，室內(nèi)定位技術(shù)不能一概而談，必須針對應(yīng)用選擇合適的定位技術(shù)，以期滿足用戶需求的情況下達(dá)到定位精度和定位成本的平衡[6]。

針對車站、機(jī)場和醫(yī)院等公共區(qū)域的消費(fèi)類定位與導(dǎo)航應(yīng)用需求，WiFi、藍(lán)牙、磁場和視覺等方法已然是主流技術(shù)，主要是因?yàn)檫@類方法采用指紋庫匹配方法，系統(tǒng)的布設(shè)成本低或無布設(shè)成本，從而大大降低了系統(tǒng)的構(gòu)建成本[7]。然而，該類方法迫切需求一種高效率高精度的定位指紋數(shù)據(jù)庫采集方法。

另外，為了準(zhǔn)確地評價室內(nèi)定位技術(shù)的性能，需要兼顧靜態(tài)和動態(tài)環(huán)境。靜態(tài)參考值因具有不變性，獲取成本低；而動態(tài)參考值獲取就存在較大難度?，F(xiàn)階段，動態(tài)參考值的獲取主要有2種方法：規(guī)劃軌跡和高精度即時定位與測圖(SLAM)。其中，規(guī)劃軌跡方法要求測試人員按照規(guī)定的路線進(jìn)行測試，該方法操作簡單，但參考值精度較低[8]；高精度SLAM能夠提供高精度的動態(tài)參考值，然而設(shè)備體積較大且價格昂貴[9]。因此，室內(nèi)定位性能評價缺少一種操作簡單、價格低廉和精度高的參考值獲取方法。

本文的主要工作就是使用2個低性能的微機(jī)械慣性傳感器(MEMS-IMU)設(shè)計了一種行人室內(nèi)定位和定姿方法。該方法主要利用行人足部周期性與地面接觸的規(guī)律，人體骨骼的相對不變性，以及少量的坐標(biāo)已知點(diǎn)(即控制點(diǎn))輔助。該方法具有成本低、操作便捷、精度高和不受環(huán)境影響等特點(diǎn)，且能夠同時提供位置和姿態(tài)信息?；谠摲椒ǎ軌蚋咝У夭杉ㄎ恢讣y數(shù)據(jù)以及獲得動態(tài)參考值。

1 系統(tǒng)設(shè)計

行人室內(nèi)定位定姿方法的硬件構(gòu)成主要分為2個部分：腳綁IMU和背部IMU。整體思路：首先，腳綁IMU結(jié)合少量的控制點(diǎn)提供連續(xù)可靠的位置坐標(biāo)；然后，利用腳綁IMU獲得的位置修正背部IMU的導(dǎo)航狀態(tài)，以背部IMU估計的導(dǎo)航狀態(tài)作為位置和姿態(tài)輸出值。

系統(tǒng)結(jié)構(gòu)如圖1所示。行人正常行走時，腳部位形成了觸地、靜止和離地3個階段的周期運(yùn)動，腳綁IMU通過檢測靜止時間段，然后使用零速度修正當(dāng)前導(dǎo)航狀態(tài)，從而提供高精度的相對位置。為了控制腳綁IMU的位置誤差，使用少量的控制點(diǎn)，從而使腳綁IMU提供長、短時間尺度的高精度位置。獲得腳綁IMU的位置信息后，并不能直接用于修正背部IMU的導(dǎo)航狀態(tài)，這是因?yàn)槟_綁IMU與背部IMU的相對位置關(guān)系在一個腳步周期內(nèi)不斷變化。幸運(yùn)的是，在靜止時段的中間時刻，腳綁IMU與背部IMU的相對位置關(guān)系具有不變的表現(xiàn)。基于這樣的客觀事實(shí)，通過選擇準(zhǔn)確的靜止時段中間時刻，從而使用腳綁IMU輸出的位置信息修正背部IMU，控制背部IMU的導(dǎo)航誤差，最后提供準(zhǔn)確的位置和姿態(tài)。

圖1 系統(tǒng)結(jié)構(gòu)Fig.1 System structure

2 算法原理

室內(nèi)行人定位定姿方法的算法流程如圖2所示。由圖2思路可以看出，腳綁IMU的數(shù)據(jù)處理思路和背部IMU的思路基本類似，核心部分都需要進(jìn)行慣性機(jī)械編排，只有在濾波量測更新部分有差異。腳綁IMU使用的量測信息有零速修正和控制點(diǎn)位置修正，背部IMU只有腳綁IMU的位置修正。最后，反向平滑算法被用于進(jìn)一步提升導(dǎo)航狀態(tài)的估計精度。

圖2 室內(nèi)行人定位定姿數(shù)據(jù)處理流程Fig.2 Data processing flow of the pedestrian indoor positioning and orientation system

2.1 慣導(dǎo)解算

低端IMU的零偏和噪聲水平普遍都非常大，該類微小的誤差改正無法提升最終的導(dǎo)航性能。因此，本文采用的捷聯(lián)慣導(dǎo)算法不考慮地球自轉(zhuǎn)等微小誤差改正項(xiàng)，只考慮傳感器零偏和噪聲等影響；目的在于降低算法的復(fù)雜度，同時不損失算法的自主推算精度。簡化后的捷聯(lián)慣導(dǎo)算法如下[10-11]：

(1)

2.2 濾波設(shè)計

捷聯(lián)慣導(dǎo)算法性能嚴(yán)重依賴傳感器本身的性能，使用卡爾曼濾波被融合其他觀測信息能夠在保證定位性能的基礎(chǔ)上，最大程度地降低對傳感器的硬件依賴。為了應(yīng)對系統(tǒng)模型的非線性，本文采用擴(kuò)展卡爾曼濾波，其中15維的誤差狀態(tài)變量定義如下：

(2)

式中，δpn，δvn，ψ分別為n系下位置誤差、速度誤差和姿態(tài)誤差；bg，bf分別為陀螺零偏誤差和加速度計零偏誤差。離散后的系統(tǒng)誤差模型表示如下：

δXk|k-1=Φk|k-1δXk-1|k-1+wk，

(3)

式中，下標(biāo)k-1，k為歷元編號；δXk-1|k-1為誤差狀態(tài)向量的估計值；δXk|k-1為誤差狀態(tài)向量的預(yù)測值；wk為系統(tǒng)噪聲和測量噪聲；Φk|k-1為狀態(tài)轉(zhuǎn)移矩陣。通過對式(1)進(jìn)行擾動分析，可得[10-11]：

Φk|k-1=

(4)

2.3 量測更新

零速修正是腳綁IMU的基礎(chǔ)量測更新，依賴于行人必須在路面行走的特性。其中，特性是指行人的腳部位與地面存在周期性地接觸。通過準(zhǔn)確地檢測該時間段，可以構(gòu)建零速度向量，更新導(dǎo)航狀態(tài)向量。本文采用角速度和比力對零速狀態(tài)進(jìn)行檢測[12-13]：

(5)

(6)

為了提供可靠的位置估計，當(dāng)行人行走一段距離(比如50 m)后，需要主動查找一個已知點(diǎn)，從而控制位置誤差累積。位置觀測方程如下：

(7)

背部IMU的數(shù)據(jù)處理思路與腳綁PDR的處理思路基本一致，慣導(dǎo)算法和濾波設(shè)計相同，只是修正信息有所差異。其中，行走狀態(tài)時，背部IMU不存在零速修正信息，唯一的修正信息是來自后處理的腳綁IMU位置信息。當(dāng)行人站立不動時，因行人身體存在不同程度的晃動，無法準(zhǔn)確地使用零速修正，但腳綁IMU仍然能夠準(zhǔn)確檢測行人真實(shí)狀態(tài)，此時腳綁IMU按照均勻時間間隔(比如0.5 s)輸出位置信息用于修正背部IMU。

2.4 平滑算法

對于非實(shí)時定位應(yīng)用，不使用未來時間點(diǎn)的觀測量優(yōu)化當(dāng)前狀態(tài)就顯得數(shù)據(jù)利用率不夠充分。最優(yōu)平滑算法的本質(zhì)是利用了過去、當(dāng)前以及未來時刻的所有測量數(shù)據(jù)來估計當(dāng)前系統(tǒng)的狀態(tài)，最終獲得比卡爾曼濾波更高精度的狀態(tài)估計。RTS算法是一種典型的固定區(qū)間平滑算法，該方法可利用測量時段內(nèi)所有量測信息對時段內(nèi)任一時刻的系統(tǒng)狀態(tài)量進(jìn)行估計，具體方程為[11]：

(8)

式中，k=N-1，N-2，…，0；N為觀測信息的總數(shù)；Ak為平滑增益矩陣。

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

3.1 實(shí)驗(yàn)描述

本文設(shè)計的行人室內(nèi)定位定姿方法適用于行人行走場景，在室內(nèi)外具有相同的性能表現(xiàn)。由于室內(nèi)缺少一種能夠覆蓋大面積的高精度定位參考，所以在室外場景進(jìn)行數(shù)據(jù)采集，目的在于使用衛(wèi)星定位技術(shù)提供高精度(比如cm級)參考真值對本文方法進(jìn)行性能評估。

測試平臺如圖3所示。腳綁IMU(YDL-602)為一個消費(fèi)級MEMS IMU，背部IMU(POS-1200)是一個集成了GNSS板卡和較高性能IMU的組合導(dǎo)航系統(tǒng)，具體的IMU性能參數(shù)如表1所示。POS-1200通過胸腰椎固定支架緊緊地附著在測試人員的背部，YDL-602則通過螺釘安裝在右靴上。

圖3 測試平臺Fig.3 Experimental test platform

表1 IMU性能參數(shù)Tab.1 IMU performance parameters

采集行人正常步行數(shù)據(jù)前，需要對IMU的位置關(guān)系進(jìn)行標(biāo)定測量，從而保證腳綁IMU的位置能夠準(zhǔn)確投影到背部IMU位置。位置和姿態(tài)參考是通過差分GNSS / INS組合導(dǎo)航系統(tǒng)后處理結(jié)果給出，解算軟件為武漢邁普時空提供的GINS軟件?；鶞?zhǔn)位置精度優(yōu)于5 cm，基準(zhǔn)橫搖和俯仰精度均優(yōu)于0.03°，基準(zhǔn)偏航精度均優(yōu)于0.1°。

為了分析本文方法的性能，在武漢市一個空曠地區(qū)進(jìn)行了現(xiàn)場測試。測試距離約為1 900 m，時間約為24 min，平均步行速度約為1.5 m/s，參考軌跡如圖4所示。

圖4 參考軌跡Fig.4 Reference trajectory

3.2 結(jié)果分析

圖5給出了不同數(shù)據(jù)處理策略對腳綁IMU的北向位置誤差的改善幅度，其中策略主要包括：腳綁IMU(FM-INS)、腳綁IMU+控制點(diǎn)修正(FM-INS+CP)和腳綁IMU+控制點(diǎn)修正+反向平滑算法(FM-INS+CP+RTS)。可以看出，控制點(diǎn)修正能夠有效地消除腳綁IMU的誤差累積，RTS算法能夠進(jìn)一步提升定位性能和腳綁IMU的定位穩(wěn)定性。因此，后續(xù)數(shù)據(jù)處理都采用FM-INS+CP+RTS方案。

圖5 不同數(shù)據(jù)處理策略的北向位置誤差Fig.5 North position error of different data processing strategies

使用控制點(diǎn)修正腳綁IMU時，控制點(diǎn)的修正頻率不同，系統(tǒng)的定位精度表現(xiàn)存在明顯差異。因此，本文通過對5種不同距離間距控制點(diǎn)修正方案(30，40，50，60，70 m)的定位性能進(jìn)行評價分析，從而為實(shí)際使用提供參考。圖6給出了不同距離間隔控制點(diǎn)條件下的位置誤差均方根，5種不同間距的北向位置誤差均小于1 dm，而東向位置誤差均小于1.2 dm，垂向位置誤差均小于1.5 dm。因此，若只需要提供分米級定位精度，腳綁IMU的修正點(diǎn)距離間隔可以最大放到70 m。

圖6 5種距離間隔控制點(diǎn)條件下的位置誤差Fig.6 Position errors of the five different distance spacing control point conditions

由前面的數(shù)據(jù)分析可以看出，隨著控制點(diǎn)間距增大，定位誤差緩慢增大，但是變化幅度不明顯。因此，在使用腳綁IMU估計的位置修正背部IMU時，本文只分析3種不同距離間隔控制點(diǎn)(30，50，70 m)條件下，背部IMU的定位和定姿性能。圖8給出了3種不同距離間隔條件下，背部IMU的北方向、東方向和高程誤差。

圖7 3種距離間隔控制點(diǎn)條件下背部IMU的位置誤差Fig.7 Position errors of the back IMU in the three different distance spacing control point conditions

位置誤差RMS值如表2所示。可以看出，當(dāng)控制點(diǎn)間距為30 m時，平面和高度誤差的RMS值分別約為5，7 cm；當(dāng)控制點(diǎn)間距為70 m時，平面和高度誤差的RMS值分別約為10，15 cm。與腳綁IMU的定位誤差基本一致。

表2 3種距離間隔控制點(diǎn)條件下背部IMU位置誤差Tab.2 Position errors of the back IMU in the three different distance spacing control point conditions m

不同距離間隔控制點(diǎn)條件下的橫滾角、俯仰角和航向角差異如圖8所示。

圖8 3種距離間隔控制點(diǎn)條件下背部IMU的姿態(tài)差異Fig.8 Attitude differences of the back IMU in the three different distance spacing control point conditions

姿態(tài)差異的RMS值如表3所示?？梢钥闯觯褂媚_綁IMU輔助與使用GNSS輔助，背部IMU輸出的姿態(tài)差異非常小，RMS值約為0.01°。而航向角差異則與控制點(diǎn)的分布密度呈正相關(guān)現(xiàn)象，控制點(diǎn)間距為30 m時，航向角度差異的RMS值約為0.5°；當(dāng)控制點(diǎn)間距為70 m時，航向角差異的RMS值上升到0.78°。這是因?yàn)殡S著控制點(diǎn)間距增大，腳綁IMU估計的位置誤差正在逐漸增大；另外，腳綁IMU輸出的位置為濾波估計結(jié)果，位置之間存在相關(guān)性，破壞了卡爾曼濾波認(rèn)為觀測值噪聲為白噪聲的假設(shè)，通常會降低濾波估計的精度?？偟膩碚f，背部IMU仍然能夠提供優(yōu)于1°的航向角估計精度。

表3 3種距離間隔控制點(diǎn)條件下背部IMU姿態(tài)差異Tab.3 Attitude differences of the back IMU in the three different distance spacing control point conditions

4 結(jié)束語

本文提出了一種腳綁IMU輔助的行人室內(nèi)定位和定姿方法。該方法首先使用零速修正和少量控制點(diǎn)用于獲得腳綁IMU的準(zhǔn)確位置；然后，將腳綁IMU估計的位置作為背部IMU的位置修正信息，達(dá)到準(zhǔn)確的位置和姿態(tài)估計。通過在室外空曠地區(qū)的現(xiàn)場測試，以GNSS / INS解決方案為參考，對連續(xù)誤差序列進(jìn)行評估，驗(yàn)證了該方法的有效性。實(shí)驗(yàn)結(jié)果表明，該方法在2個相鄰控制點(diǎn)之間的距離為50 m時具有亞米級的定位能力。橫滾角和俯仰角差異小于0.1°，偏航角精度為1°～2°。與傳統(tǒng)的直接控制點(diǎn)校正方法相比，該方法具有顯著的準(zhǔn)確性和魯棒性。

所提出的方法具有成本低、不受非視線條件影響、不受測試面積大小影響和操作便捷等優(yōu)點(diǎn)，在室內(nèi)環(huán)境可提供高精度和可靠的定位定姿解決方案。此外，所提出的方法可以用作其他室內(nèi)定位技術(shù)(例如WLAN和藍(lán)牙)的室內(nèi)參考解決方案。