基于容錯(cuò)決策樹(shù)的UWB輔助慣性定位方法

劉 宇，陳自然，顧洪洋，付樂(lè)樂(lè)，路永樂(lè)，李星海

(1.重慶郵電大學(xué) 智能傳感技術(shù)與微系統(tǒng)重慶市高校工程研究中心，重慶 400065；2. 中國(guó)電子科技集團(tuán)公司第二十六研究所，重慶 400060)

0 引言

基于位置服務(wù)是現(xiàn)代生活里許多應(yīng)用場(chǎng)景所需的關(guān)鍵特征，人們?cè)絹?lái)越依賴其帶來(lái)的便利性。在室外，GPS、北斗等得到了很好的探索和標(biāo)準(zhǔn)化，但由于其信號(hào)易受室內(nèi)復(fù)雜環(huán)境的阻隔，導(dǎo)致無(wú)法在室內(nèi)復(fù)雜環(huán)境實(shí)現(xiàn)高精度的定位[1]。因此，室內(nèi)高精度定位研究具有很高的商用價(jià)值。

慣性導(dǎo)航系統(tǒng)利用載體自身的角速率和加速度進(jìn)行實(shí)時(shí)測(cè)量解算姿態(tài)信息和相對(duì)位置信息[2]，具有成本低,體積小,無(wú)源等優(yōu)點(diǎn)，在短期內(nèi)對(duì)室內(nèi)復(fù)雜環(huán)境能有效實(shí)現(xiàn)較高精度定位，但慣性導(dǎo)航系統(tǒng)隨著時(shí)間增長(zhǎng)，陀螺和加速度計(jì)等慣性元器件本身產(chǎn)生的測(cè)量誤差等使其相對(duì)定位點(diǎn)出現(xiàn)較大誤差[3]，單一模式不適合長(zhǎng)時(shí)間定位[4]。

為解決慣性導(dǎo)航存在的自身缺陷，在基于射頻(RF)的定位技術(shù)中，超寬帶(UWB)系統(tǒng)由于其具有高精度、低成本和低功耗，已經(jīng)非常流行。與超聲波相比，紅外、藍(lán)牙、ZigBee、射頻識(shí)別技術(shù)和WIFI等有顯著優(yōu)勢(shì)，雖然這些技術(shù)在一定程度上滿足了部分室內(nèi)活動(dòng)的需求[5]，但仍存在成本高，定位精度差等缺點(diǎn)。目前，國(guó)內(nèi)外學(xué)者提出了采用UWB定位技術(shù)的多源信息對(duì)慣性導(dǎo)航進(jìn)行輔助修正。Lukasz Zwirello等[6]提出了一個(gè)基于仿真的緊密UWB和慣性數(shù)據(jù)集成的可行性研究；Qigao Fan等[7]提出了利用雙態(tài)自適應(yīng)卡爾曼濾波器的INS/UWB定位系統(tǒng)；曾慶化等[8]提出了基于零速修正的UWB優(yōu)化配置室內(nèi)行人導(dǎo)航。但多數(shù)學(xué)者未進(jìn)行組合定位的相互容錯(cuò)判定，在建筑結(jié)構(gòu)復(fù)雜的室內(nèi)環(huán)境中，UWB定位易受到非視距問(wèn)題、多徑效應(yīng)和人體的影響出現(xiàn)粗大誤差，可能導(dǎo)致慣性導(dǎo)航/UWB定位精度下降，甚至錯(cuò)誤地輔助修正慣性導(dǎo)航。

本文提出了一種基于容錯(cuò)決策樹(shù)的UWB定位信息輔助修正慣性定位的方法。首先提出并采用陀螺儀高精度分段擬合誤差補(bǔ)償模型，抑制慣性導(dǎo)航誤差漂移；同時(shí)將慣性導(dǎo)航解算的定位信息與UWB單點(diǎn)定位數(shù)據(jù)作為參考量共同構(gòu)建容錯(cuò)決策樹(shù)模型流程，有效剔除UWB粗大誤差，進(jìn)而利用2個(gè)定位結(jié)果的參數(shù)進(jìn)行擴(kuò)展卡爾曼濾波，實(shí)現(xiàn)UWB輔助增強(qiáng)慣性定位，實(shí)現(xiàn)連續(xù)性可靠定位。

1 室內(nèi)慣性導(dǎo)航定位系統(tǒng)解算

本系統(tǒng)采用嵌入式系統(tǒng)作為慣性導(dǎo)航定位系統(tǒng)硬件測(cè)試平臺(tái)，集成了多個(gè)慣性傳感器件。該定位系統(tǒng)首先進(jìn)行慣性導(dǎo)航的初始加速度計(jì)/磁力計(jì)組合姿態(tài)解算；然后對(duì)慣性器件誤差校準(zhǔn)，構(gòu)建了陀螺儀高精度分段擬合誤差補(bǔ)償模型；根據(jù)載體自身的角速率和加速度進(jìn)行實(shí)時(shí)、步數(shù)檢測(cè)和步長(zhǎng)估計(jì)測(cè)量解算姿態(tài)信息和相對(duì)位置導(dǎo)航信息。

1.1 初始加速度計(jì)/磁力計(jì)組合姿態(tài)解算

本文采用地理坐標(biāo)系即東北天坐標(biāo)系(n系)，使用加速度解算的初始俯仰角、橫滾角，再加上磁力計(jì)進(jìn)行共同解算，得到姿態(tài)角信息數(shù)據(jù)。設(shè)θ，γ，ψ分別為俯仰角、橫滾角和航向角，且

(1)

1.2 陀螺儀高精度分段擬合誤差補(bǔ)償

三軸陀螺儀用于采集姿態(tài)解算過(guò)程中所需要的角速度數(shù)據(jù)信息，其輸出值的精確程度直接影響到姿態(tài)數(shù)據(jù)精度。為了進(jìn)一步減小誤差，分析了傳感器自身的安裝標(biāo)定誤差、非線性誤差等，利用角速率輸出誤差模型，本文提出對(duì)陀螺儀進(jìn)行高精度分段擬合誤差補(bǔ)償。角速率輸出誤差模型為

(2)

式中：ωx，ωy，ωz為陀螺儀真實(shí)角速率；Sx，Sz，Sy為陀螺儀標(biāo)定因數(shù)；Kyx，Kzx，Kxy，Kzy，Kxz，Kyz為陀螺儀安裝誤差系數(shù)；Bx，By，Bz為陀螺儀零偏；wx，wy，wz為陀螺儀實(shí)際角速率測(cè)量值。

在高精度分段擬合誤差補(bǔ)償?shù)膶?shí)驗(yàn)中，選取三軸轉(zhuǎn)臺(tái)動(dòng)態(tài)速率為-100～100 (°)/s，以10 (°)/s作為間隔，共20個(gè)角速率，每組采集保留500 kbytes數(shù)據(jù)。按照速率數(shù)據(jù)正負(fù)對(duì)稱和動(dòng)態(tài)速率高低，將20個(gè)角速率分為8組，在每個(gè)分段組中，以取三軸轉(zhuǎn)臺(tái)動(dòng)態(tài)速率值為橫坐標(biāo)，陀螺儀真實(shí)角速率為縱坐標(biāo)，對(duì)其數(shù)據(jù)進(jìn)行擬合，得到各軸該分段組標(biāo)定因數(shù)Sx、Sz、Sy，安裝誤差Kyx、Kzx、Kxy、Kzy、Kxz、Kyz及零偏Bx、By、Bz。

以x軸為例,設(shè)置動(dòng)態(tài)速率為-100 ～100 (°)/s，對(duì)x軸原始誤差和校準(zhǔn)后誤差進(jìn)行均值記錄(見(jiàn)表1)。

表1 陀螺儀高精度分段擬合誤差補(bǔ)償前、后對(duì)比表

由表1可知，經(jīng)過(guò)高精度分段擬合誤差補(bǔ)償后，可區(qū)分補(bǔ)償高低動(dòng)態(tài)速率段，更進(jìn)一步消除角速率輸出誤差，減少了1～2個(gè)數(shù)量級(jí)。

1.3 四元數(shù)姿態(tài)解算

采用四元數(shù)算法對(duì)傳感器測(cè)量值進(jìn)行姿態(tài)解算，利用慣性導(dǎo)航姿態(tài)解算的四元數(shù)參考值，實(shí)時(shí)計(jì)算出坐標(biāo)矩陣，結(jié)合姿態(tài)轉(zhuǎn)換算法，即可解算出i時(shí)刻三軸θ、γ、ψ分別為

(3)

式中：q0為四元數(shù)的標(biāo)量；q1、q2、q3為四元數(shù)的矢量。

1.4 步數(shù)檢測(cè)和步長(zhǎng)估計(jì)

根據(jù)加速度特征數(shù)據(jù)仿真發(fā)現(xiàn)，行人走路的波峰、零點(diǎn)和波谷數(shù)據(jù)有似正弦波的周期性規(guī)律，利用漢明窗提取特征數(shù)據(jù)。但由于行走過(guò)程中身體存在抖動(dòng)，加速度波形可能出現(xiàn)偽波峰，進(jìn)一步消除其造成的步數(shù)誤判采用閾值判別法：

(4)

閾值范圍內(nèi)，檢測(cè)到一個(gè)有效步數(shù)是以有效波峰值和有效波谷值為周期。經(jīng)實(shí)驗(yàn)驗(yàn)證，人員行走108步，則檢測(cè)到的有效波峰為108個(gè)(見(jiàn)圖1)，此方法能準(zhǔn)確檢測(cè)人員行走的步數(shù)。

圖1 加速度步數(shù)檢測(cè)圖

利用加速度與人員行走時(shí)刻步長(zhǎng)的波峰、波谷差值之間的關(guān)聯(lián)性，采用下式進(jìn)行自適應(yīng)步長(zhǎng)計(jì)算：

(5)

式中：j為行人的第j步；Lj為行人第j步的實(shí)時(shí)步長(zhǎng)；K為行人步長(zhǎng)標(biāo)定參考值；amax,j、amin,j分別為第j步目標(biāo)加速度最大波峰值、最小波谷值。

2 UWB定位系統(tǒng)解算

UWB定位是一種基于距離測(cè)量的室內(nèi)高精度定位通信新技術(shù)，一定范圍內(nèi)準(zhǔn)確獲得人或物的位置和導(dǎo)航信息。本文的UWB定位測(cè)距通信模塊使用雙向飛行時(shí)間(TW-TOF)測(cè)量2個(gè)以上UWB模塊的間距，其可提供具有短脈沖的精確時(shí)間測(cè)量，同時(shí)保持對(duì)多徑信號(hào)傳播的魯棒性。

TW-TOF不要求通信雙方時(shí)間的同步性，自模塊運(yùn)作后將擁有其獨(dú)立的時(shí)間戳,如圖2所示。

圖2 TW-TOF測(cè)距方法

首先分別得到模塊A、B的脈沖發(fā)送和接收時(shí)刻的傳遞時(shí)間Ta、Tb為

(6)

再利用無(wú)線電信號(hào)傳播的速度即光速c，可計(jì)算2個(gè)實(shí)體的距離p為

(7)

3 基于容錯(cuò)決策樹(shù)的UWB輔助慣性定位

慣性導(dǎo)航系統(tǒng)對(duì)載體自身的角速率和加速度進(jìn)行實(shí)時(shí)測(cè)量，解算得到姿態(tài)信息和相對(duì)位置導(dǎo)航信息后，利用UWB定位輔助增強(qiáng)室內(nèi)定位精度，實(shí)現(xiàn)多傳感器多源定位信息下的連續(xù)性可靠定位。UWB設(shè)備的布設(shè)將針對(duì)不同的室內(nèi)環(huán)境而定，在復(fù)雜狹窄巷道環(huán)境時(shí)，兩端布設(shè)UWB基站進(jìn)行輔助；常規(guī)環(huán)境下，在同一樓層拐點(diǎn)或分叉口布設(shè)UWB基站進(jìn)行輔助，合理減少室內(nèi)環(huán)境中UWB基站數(shù)量。UWB所獲得的定位數(shù)據(jù)用于輔助修正慣性導(dǎo)航解算的定位信息，降低系統(tǒng)隨時(shí)間累積誤差。

3.1 UWB單點(diǎn)定位數(shù)據(jù)的容錯(cuò)決策樹(shù)判定

在室內(nèi)環(huán)境中，由于UWB定位存在易受遮擋物引起的非視距(NLOS)誤差及信號(hào)不穩(wěn)定等多種影響定位精度情況，先行對(duì)UWB是否接收到多邊基站信號(hào)和出現(xiàn)粗大誤差進(jìn)行有效判定，并將慣性導(dǎo)航解算的定位數(shù)據(jù)作為決策樹(shù)容錯(cuò)判定的參考量共同構(gòu)建模型流程。

在容錯(cuò)判定開(kāi)始前將當(dāng)前k時(shí)刻的UWB單點(diǎn)定位數(shù)據(jù)作為輸入量。

首先進(jìn)去到第一層容錯(cuò)判定，需要獲知UWB標(biāo)簽是否接收到定位基站的脈沖信號(hào)，則利用Tuwb標(biāo)志位判定是否存在UWB單點(diǎn)定位數(shù)據(jù),即

(8)

通過(guò)Tuwb標(biāo)志位判定后，利用UWB單點(diǎn)定位數(shù)據(jù)獲得其標(biāo)簽與定位基站之間的飛行時(shí)間后，使用TW-TOF測(cè)量標(biāo)簽與各基站的定位距離，由式(7)可解算得距離p。

在通過(guò)TW-TOF法獲得標(biāo)簽到多個(gè)定位基站的距離后，本文選用三邊測(cè)距定位法計(jì)算行人當(dāng)前k時(shí)刻的移動(dòng)位置最優(yōu)解Puwb,k(xn,yn)：

(9)

式中:xn,yn分別為由n個(gè)UWB定位基站共同解算的標(biāo)簽x、y軸移動(dòng)位置最優(yōu)解;pn為標(biāo)簽到第n個(gè)定位基站的距離。

UWB定位基站的布設(shè)數(shù)量根據(jù)不同的室內(nèi)環(huán)境而定，若行人處于復(fù)雜狹窄巷道環(huán)境中，行人將UWB標(biāo)簽與慣性定位模塊集成一同放在腰間，可能出現(xiàn)無(wú)法接收3個(gè)及以上的基站時(shí)間戳的情況，則默認(rèn)由TW-TOF得到的2個(gè)基站距離p1、p2計(jì)算位置信息；若行人處于常規(guī)方正室內(nèi)環(huán)境，在房間4個(gè)拐點(diǎn)UWB基站進(jìn)行輔助，則默認(rèn)由TW-TOF得到的多個(gè)基站距離p1、p2、p3、p4計(jì)算二維位置信息。

通過(guò)第一層容錯(cuò)判定，若不存在UWB單點(diǎn)定位數(shù)據(jù)，單邊基站所得到定位數(shù)據(jù)不具可信度，則不使用UWB輔助慣性導(dǎo)航解算。如果存在UWB單點(diǎn)定位數(shù)據(jù)，由于UWB數(shù)據(jù)存在粗大誤差在第一層容錯(cuò)判定中無(wú)法完全消除，需結(jié)合慣性導(dǎo)航定位數(shù)據(jù)作為參考量,進(jìn)一步提高可信度。

接下來(lái)進(jìn)入第二層容錯(cuò)判定，在復(fù)雜狹窄巷道環(huán)境時(shí)，根據(jù)行人運(yùn)動(dòng)規(guī)律特性，安放在人員腰間的定位模塊出現(xiàn)航向角度大幅變化時(shí)，UWB數(shù)據(jù)存在粗大誤差的幾率降低。故在此環(huán)境下，需要利用當(dāng)前k時(shí)刻慣導(dǎo)解算對(duì)應(yīng)的航向角ψk和初始航向角ψzero進(jìn)行判斷行人是否發(fā)生轉(zhuǎn)彎模式變化。

(10)

式中：Δψ為k時(shí)刻航向角與初始航向角的差值絕對(duì)值；ε為判斷行人轉(zhuǎn)彎模式的活動(dòng)閾值。

從第二層容錯(cuò)判定中可看出，行人轉(zhuǎn)彎模式是對(duì)UWB粗大誤差的剔除，在確定轉(zhuǎn)彎判定能接收到基本精確的UWB單點(diǎn)定位數(shù)據(jù)后，需結(jié)合慣性導(dǎo)航解算的位置數(shù)據(jù)和行人步數(shù)進(jìn)行判斷位置條件，進(jìn)入第三層容錯(cuò)判定。若不在復(fù)雜狹窄巷道環(huán)境下，直接進(jìn)入第三層容錯(cuò)判定。

根據(jù)在行走定位過(guò)程中的運(yùn)動(dòng)特性，佩戴人員在常規(guī)情況下，其前、后采樣點(diǎn)的位置不會(huì)突變，應(yīng)該滿足UWB位置和慣性導(dǎo)航位置之間的距離差條件。并且隨著運(yùn)動(dòng)時(shí)間累積，其距離差值將會(huì)保持在合理范圍內(nèi)。因此，采用同一采樣點(diǎn)的慣性導(dǎo)航與UWB定位信息差Δp進(jìn)行測(cè)量數(shù)據(jù)的活動(dòng)閾值判定，即

(11)

式中：Pins,k為慣性導(dǎo)航在k時(shí)刻的位置數(shù)據(jù)；Puwb,k為UWB在k時(shí)刻單點(diǎn)定位距離數(shù)據(jù)；σ為慣性導(dǎo)航與UWB位置數(shù)據(jù)之差的活動(dòng)閾值。

考慮到隨時(shí)間累積后定位誤差的線性變化，若UWB和慣性導(dǎo)航解算位置差保持在合理的范圍內(nèi)，允許其σ存在波動(dòng)。由于在時(shí)域中無(wú)法將每一時(shí)刻與距離誤差一一對(duì)應(yīng)，故使用行人運(yùn)動(dòng)步數(shù)與距離間的關(guān)系，其σ能同時(shí)滿足下列行走步數(shù)范圍的關(guān)系為

(12)

式中：step為加速度解算的有效步數(shù)；σ1，σ2，σ3為分段步數(shù)對(duì)應(yīng)的閾值。

經(jīng)過(guò)容錯(cuò)決策樹(shù)的三層判定，最終實(shí)現(xiàn)了利用慣性導(dǎo)航解算的定位信息與UWB單點(diǎn)定位數(shù)據(jù)作為參考量，共同構(gòu)建容錯(cuò)決策樹(shù)模型流程，提高了定位精度。

3.2 基于擴(kuò)展卡爾曼(EKF)濾波的慣性導(dǎo)航/UWB定位信息融合

通過(guò)UWB單點(diǎn)定位數(shù)據(jù)決策樹(shù)容錯(cuò)判定后得到了當(dāng)前時(shí)刻穩(wěn)定的位置信息，將其作為觀測(cè)信息與慣性導(dǎo)航解算數(shù)據(jù)進(jìn)行濾波，其狀態(tài)方程與觀測(cè)方程如下：

(13)

式中：k為離散時(shí)間所對(duì)應(yīng)的時(shí)刻，系統(tǒng)在k時(shí)刻的狀態(tài)Xk∈Rn；Zk∈Rm為對(duì)應(yīng)狀態(tài)的觀測(cè)信號(hào)；Φk為狀態(tài)轉(zhuǎn)移矩陣；Gk為狀態(tài)噪聲驅(qū)動(dòng)矩陣;Wk∈Rn為輸入信號(hào)的白噪聲;φk為非隨機(jī)的外作用項(xiàng);Hk為觀測(cè)矩陣;Vk∈Rm為觀測(cè)信號(hào)的白噪聲。

設(shè)計(jì)X=[δp,δv,δφ,δa]T為該EKF濾波的狀態(tài)向量，包含12個(gè)狀態(tài)量，其中行人的位置誤差δp(δpx、δpy、δpz)、慣性導(dǎo)航東、北、天坐標(biāo)的速度誤差δv(δvE、δvN、δvU)、當(dāng)前時(shí)刻慣性導(dǎo)航所解算的姿態(tài)角誤差δφ(δφγ、δφθ、δφψ)以及三軸加速度誤差δa(δax、δay、δaz)作為狀態(tài)向量，E、N、U分別為東、北、天坐標(biāo)系中的東向、北向和天向位置。

針對(duì)姿態(tài)角誤差補(bǔ)償，當(dāng)初始時(shí)刻檢測(cè)到開(kāi)機(jī)站立時(shí)，默認(rèn)此時(shí)慣性導(dǎo)航解算的姿態(tài)角為初始姿態(tài)角。將慣性導(dǎo)航解算的實(shí)際姿態(tài)角與初始姿態(tài)角之差Δψ作為當(dāng)前時(shí)刻姿態(tài)角的觀測(cè)量。針對(duì)位置誤差補(bǔ)償，使用慣性導(dǎo)航與UWB解算的位置信息之差作為當(dāng)前時(shí)刻位置的觀測(cè)量Δp。觀測(cè)向量Z=[Δp,Δψ]。其中：

(14)

式中wΔp，wΔψ為對(duì)應(yīng)觀測(cè)量的觀測(cè)模型噪聲。在這個(gè)過(guò)程中，假設(shè)wΔp，wΔψ相互獨(dú)立且滿足高斯白噪聲。首先對(duì)狀態(tài)方程進(jìn)行求導(dǎo)可得Φk為12×12的狀態(tài)轉(zhuǎn)移矩陣，即

(15)

Hk為觀測(cè)量的轉(zhuǎn)換矩陣，且

(16)

4 實(shí)驗(yàn)驗(yàn)證與分析

為了驗(yàn)證本文提出的基于容錯(cuò)決策樹(shù)的UWB輔助慣性導(dǎo)航解算可靠性，將UWB標(biāo)簽與慣性定位模塊集成，佩戴在行人后背的腰間(見(jiàn)圖3)，以便在行走過(guò)程獲得更穩(wěn)定的位置數(shù)據(jù)。實(shí)驗(yàn)中慣性定位采用集成了三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)等傳感器的嵌入式微系統(tǒng)作為硬件平臺(tái)，通過(guò)傳感器數(shù)據(jù)實(shí)時(shí)解算行人的位置和航向；UWB使用定位測(cè)距通信模塊，定位基站的布設(shè)根據(jù)不同的室內(nèi)環(huán)境而定。定位系統(tǒng)的上位機(jī)實(shí)時(shí)接收慣性導(dǎo)航和容錯(cuò)判定后的UWB解算定位數(shù)據(jù)，再利用EKF濾波對(duì)定位信息融合。

圖3 慣性定位模塊和UWB裝置

本文分別在兩種不同環(huán)境下進(jìn)行實(shí)驗(yàn)，以證明基于容錯(cuò)決策樹(shù)的UWB輔助人員室內(nèi)定位在復(fù)雜和常規(guī)環(huán)境均有效。第一種實(shí)驗(yàn)在復(fù)雜狹窄巷道環(huán)境中，驗(yàn)證能否準(zhǔn)確進(jìn)入到對(duì)應(yīng)的房間；第二種實(shí)驗(yàn)在常規(guī)方正室內(nèi)環(huán)境，軌跡類似于矩形，驗(yàn)證系統(tǒng)的可靠性。由于UWB信號(hào)通常無(wú)法穿過(guò)建筑物的混凝土墻，這導(dǎo)致大量的信號(hào)延遲，故本實(shí)驗(yàn)不考慮高度問(wèn)題，在2D平面進(jìn)行定位。

4.1 復(fù)雜狹窄巷道環(huán)境

設(shè)定實(shí)驗(yàn)環(huán)境在重慶郵電大學(xué)第一教學(xué)樓(見(jiàn)圖4)，激光測(cè)距可得其直線過(guò)道長(zhǎng)為33.23 m。該過(guò)道配備有2個(gè)UWB接入點(diǎn)，分別置于左、右兩端AP1(0,0)、AP2(0,33.23)，在圖中用▲表示。為了更好地對(duì)比純慣性導(dǎo)航解算和UWB輔助的定位系統(tǒng)精度，選取5個(gè)教學(xué)房間，規(guī)定統(tǒng)一的行走路線：起點(diǎn)AP1→房間①→房間②→房間③→房間④→房間⑤。行人出發(fā)航向由初始加速度計(jì)/磁力計(jì)組合姿態(tài)解算ψzero，二維坐標(biāo)為(0,0)，并規(guī)定行人結(jié)束時(shí)方向與出發(fā)時(shí)一致。

圖4 第一教學(xué)樓建筑結(jié)構(gòu)圖及行走路線

圖5(a)為行人在過(guò)道及進(jìn)入教學(xué)房間過(guò)程中，純慣性導(dǎo)航和慣性導(dǎo)航/UWB輔助定位的上位機(jī)行走界面。圖5(b)為實(shí)驗(yàn)效果對(duì)比仿真圖。

圖5 上位機(jī)實(shí)際行走及實(shí)驗(yàn)效果對(duì)比仿真圖

圖5中行走路線的理論起點(diǎn)、終點(diǎn)分別為AP1(0,0)、AP2(0,33.23)，通過(guò)實(shí)驗(yàn)對(duì)比分析，復(fù)雜狹窄巷道環(huán)境下，在無(wú)UWB輔助的純慣性導(dǎo)航時(shí)，位置和航向角逐漸偏離真實(shí)定位軌跡，且隨著時(shí)間的增長(zhǎng)，其相對(duì)定位點(diǎn)出現(xiàn)較大誤差，呈現(xiàn)連續(xù)變化特性。

由圖5(b)可看出，當(dāng)行人進(jìn)入到房間時(shí)，有效的UWB測(cè)量單點(diǎn)定位信息輔助修正慣性導(dǎo)航，將行走軌跡拉回到正確的區(qū)域，無(wú)UWB輔助修正的慣性導(dǎo)航定位無(wú)法解算原始軌跡現(xiàn)。當(dāng)行人到達(dá)理論終點(diǎn)AP2時(shí)，純慣性導(dǎo)航解算點(diǎn)為A(6.2，36.56)，慣性導(dǎo)航/UWB定位解算點(diǎn)為B(0.39，34.03)，實(shí)際行走距離長(zhǎng)度為116.86 m。根據(jù)數(shù)據(jù)計(jì)算可得，A點(diǎn)到起點(diǎn)的均方位置差為7.038 m，距離均方誤差占路線長(zhǎng)度的6.02%；B點(diǎn)到起點(diǎn)的均方位置差為0.89 m，距離均方誤差占路線長(zhǎng)度的0.76%。

4.2 常規(guī)方正室內(nèi)環(huán)境

設(shè)定實(shí)驗(yàn)環(huán)境在重慶郵電大學(xué)第七教學(xué)樓,室內(nèi)配備4個(gè)UWB定位基站(見(jiàn)圖6)，均安裝在房間的拐角處天花板墻上。UWB定位基站坐標(biāo)由激光測(cè)距儀得到，分別為AP1(0,0)、AP2(0,39.5)、AP3(18.5,39.5)、AP4(18.5,0)，在圖中用▲表示。實(shí)驗(yàn)統(tǒng)一的行走路線為從起點(diǎn)AP1沿逆時(shí)針?lè)较?，分別經(jīng)過(guò)AP2、AP3、AP4，繞行兩圈回到起點(diǎn)AP1。行人出發(fā)航向由初始加速度計(jì)/磁力計(jì)組合姿態(tài)解算ψzero，并規(guī)定行人結(jié)束時(shí)方向與出發(fā)時(shí)一致。

圖6 第七教學(xué)樓建筑結(jié)構(gòu)圖及行走路線

圖7為行走路線的純慣性導(dǎo)航和慣性導(dǎo)航/UWB輔助定位的實(shí)驗(yàn)效果對(duì)比仿真圖。由圖可以看出，純慣性導(dǎo)航定位路線隨著時(shí)間累積誤差，在第二圈時(shí)出現(xiàn)了較大的位置漂移；慣性導(dǎo)航/UWB定位路線有效剔除了UWB粗大誤差定位信息，并快速輔助修正慣性導(dǎo)航長(zhǎng)時(shí)間連續(xù)定位的誤差，與實(shí)驗(yàn)規(guī)劃路線吻合較好。當(dāng)行人繞行兩圈回到AP1時(shí)，純慣性導(dǎo)航解算點(diǎn)為A(0.542，1.294)，慣性導(dǎo)航/UWB定位解算點(diǎn)為B(-0.124，-0.061)，實(shí)際行走距離為232 m。由于實(shí)際行走軌跡為常規(guī)方正矩形，存在各個(gè)方向漂移相互抵消，故不只限于分析終點(diǎn)誤差值。純慣性導(dǎo)航的距離誤差為-1.38～5.12 m，最大誤差占路線長(zhǎng)度的2.207%；慣性導(dǎo)航/UWB定位的距離誤差范圍為-1.473 8～0.160 3 m，最大誤差占路線長(zhǎng)度的0.635%。

圖7 實(shí)驗(yàn)效果對(duì)比仿真圖

綜上所述可知，與純慣性導(dǎo)航相比，在復(fù)雜狹窄巷道和常規(guī)方正室內(nèi)環(huán)境下，慣性導(dǎo)航/UWB定位系統(tǒng)人員行走真實(shí)路徑與規(guī)劃路徑更貼合，顯著降低了定位誤差，有效抑制誤差隨時(shí)間累積漂移，實(shí)現(xiàn)了多傳感器多源定位信息下的連續(xù)性可靠定位。

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

本文針對(duì)慣性導(dǎo)航系統(tǒng)誤差隨時(shí)間累積和UWB定位受到非視距問(wèn)題、多徑效應(yīng)和人體影響出現(xiàn)粗大誤差的問(wèn)題，提出了一種基于容錯(cuò)決策樹(shù)的UWB輔助慣性定位方法。該方法采用陀螺儀高精度分段擬合誤差補(bǔ)償模型，抑制慣性導(dǎo)航誤差漂移；同時(shí)構(gòu)建UWB單點(diǎn)定位數(shù)據(jù)的容錯(cuò)決策樹(shù)判定，剔除粗大誤差因子，進(jìn)而使用慣性導(dǎo)航和UWB參數(shù)進(jìn)行擴(kuò)展卡爾曼濾波，實(shí)現(xiàn)UWB輔助增強(qiáng)慣性導(dǎo)航定位。

實(shí)驗(yàn)中慣性定位采用集成了三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)等傳感器的嵌入式微系統(tǒng)作為硬件平臺(tái)；UWB使用定位測(cè)距通信模塊，定位基站的布設(shè)根據(jù)不同的室內(nèi)環(huán)境而定。根據(jù)實(shí)驗(yàn)平臺(tái)對(duì)兩種不同環(huán)境進(jìn)行實(shí)驗(yàn)驗(yàn)證。在復(fù)雜狹窄巷道環(huán)境，本算法將距離均方誤差占路線長(zhǎng)度的比例從6.02%提升到0.76%；在常規(guī)方正室內(nèi)環(huán)境，本算法將最大誤差占路線長(zhǎng)度的比例從2.207%提升到0.635%。實(shí)現(xiàn)了長(zhǎng)時(shí)間的連續(xù)可靠定位，具有很強(qiáng)的工程應(yīng)用價(jià)值。