曠 儉，劉 韜，牛小驥

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

0 引言

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

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

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

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

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

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

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

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

2.1 慣導(dǎo)解算

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

(1)

2.2 濾波設(shè)計(jì)

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

(2)

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

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

(3)

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

Φk|k-1=

(4)

2.3 量測(cè)更新

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

(5)

(6)

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

(7)

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

2.4 平滑算法

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

(8)

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

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

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

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

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

圖3 測(cè)試平臺(tái)Fig.3 Experimental test platform

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

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

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

圖4 參考軌跡Fig.4 Reference trajectory

3.2 結(jié)果分析

圖5給出了不同數(shù)據(jù)處理策略對(duì)腳綁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時(shí)，控制點(diǎn)的修正頻率不同，系統(tǒng)的定位精度表現(xiàn)存在明顯差異。因此，本文通過(guò)對(duì)5種不同距離間距控制點(diǎn)修正方案(30，40，50，60，70 m)的定位性能進(jìn)行評(píng)價(jià)分析，從而為實(shí)際使用提供參考。圖6給出了不同距離間隔控制點(diǎn)條件下的位置誤差均方根，5種不同間距的北向位置誤差均小于1 dm，而東向位置誤差均小于1.2 dm，垂向位置誤差均小于1.5 dm。因此，若只需要提供分米級(jí)定位精度，腳綁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估計(jì)的位置修正背部IMU時(shí)，本文只分析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時(shí)，平面和高度誤差的RMS值分別約為5，7 cm；當(dāng)控制點(diǎn)間距為70 m時(shí)，平面和高度誤差的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時(shí)，航向角度差異的RMS值約為0.5°；當(dāng)控制點(diǎn)間距為70 m時(shí)，航向角差異的RMS值上升到0.78°。這是因?yàn)殡S著控制點(diǎn)間距增大，腳綁IMU估計(jì)的位置誤差正在逐漸增大；另外，腳綁IMU輸出的位置為濾波估計(jì)結(jié)果，位置之間存在相關(guān)性，破壞了卡爾曼濾波認(rèn)為觀測(cè)值噪聲為白噪聲的假設(shè)，通常會(huì)降低濾波估計(jì)的精度?？偟膩?lái)說(shuō)，背部IMU仍然能夠提供優(yōu)于1°的航向角估計(jì)精度。

表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é)束語(yǔ)

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

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