基于MEMS/UWB組合的室內(nèi)定位方法

郁嘉宇，趙 忠，李 凡，黃思文

(西北工業(yè)大學(xué)自動(dòng)化學(xué)院，西安 710072)

0 引言

隨著人們生活水平的不斷提高，各種通信和定位技術(shù)也在不斷發(fā)展和完善，復(fù)雜的道路網(wǎng)絡(luò)和空間結(jié)構(gòu)帶給人們生活和工作的壓力，使得人們對(duì)定位的需求愈加強(qiáng)烈[1]。根據(jù)范圍和場(chǎng)景，定位可分為室內(nèi)定位和室外定位。室外定位技術(shù)主要借助于全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)，利用衛(wèi)星和移動(dòng)對(duì)象之間的直接通信，可完成精確定位[2]。而對(duì)于室內(nèi)場(chǎng)景，GNSS信號(hào)易受遮擋，無法實(shí)現(xiàn)定位。

現(xiàn)已出現(xiàn)多種室內(nèi)定位解決方案，包括紅外線、射頻識(shí)別、視覺、WiFi、藍(lán)牙信標(biāo)(如iBeacon)、超聲波、LED信標(biāo)、地磁場(chǎng)、超寬帶(Ultra-Wide Bandwidth, UWB)及微慣性器件等定位技術(shù)[3-4]。它們都有各自的優(yōu)勢(shì)和不足。

UWB定位系統(tǒng)具有功耗低、抗多路徑效應(yīng)效果好、安全性高、信號(hào)穿透力好等優(yōu)點(diǎn)；但其不提供姿態(tài)信息，動(dòng)態(tài)范圍和魯棒性較差，對(duì)外界硬件設(shè)施依賴程度較高，存在信號(hào)遮擋(鐵材料和結(jié)構(gòu)復(fù)雜的混凝土墻壁等)、信號(hào)作用距離有限、信號(hào)易被吸收和成本高等問題。微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)定位精度與慣性器件本身精度密切相關(guān)，不易受環(huán)境干擾、獨(dú)立工作能力強(qiáng)、短期精度高，可以準(zhǔn)確測(cè)得定位目標(biāo)的運(yùn)動(dòng)參數(shù)，但其定位誤差會(huì)隨時(shí)間增長[5-6]。當(dāng)下MEMS慣性傳感器朝著低成本、低功耗、高精度的方向發(fā)展，在室內(nèi)定位領(lǐng)域?qū)?huì)有更大的應(yīng)用空間。

UWB與MEMS定位在全球范圍內(nèi)受到了廣泛關(guān)注與深入研究。劉見輝等提出了慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)和掃描儀相結(jié)合的測(cè)量系統(tǒng)，同時(shí)在該量測(cè)系統(tǒng)中又加入了UWB以解決INS誤差積累的問題[7]。鄔春明等結(jié)合差分全球定位系統(tǒng)(Differential Global Positioning System，DGPS)和UWB，利用Kalman濾波器消除UWB非視距誤差，采用粒子濾波器進(jìn)行數(shù)據(jù)融合，可將整體定位精度提高19%[8]。王芳等采用UWB與里程計(jì)信息融合的定位技術(shù)，提高了里程計(jì)航位推算穩(wěn)定性與精度[9]。Qigao Fan等通過建立INS/UWB融合濾波的誤差模型，提出了模糊適應(yīng)卡爾曼濾波(Fuzzy Adaptive Kalman Filter，F(xiàn)AKF)，根據(jù)模糊控制理論對(duì)量測(cè)噪聲進(jìn)行調(diào)整[10]。Ascher C等將UWB和INS進(jìn)行緊組合，利用INS得到用戶的軌跡和航向信息，結(jié)合UWB精確的定位數(shù)據(jù)，可完成室內(nèi)環(huán)境下的實(shí)時(shí)定位[11]。

本文旨在將UWB和MEMS兩種技術(shù)相結(jié)合，采用合適的算法對(duì)兩系統(tǒng)進(jìn)行信息與數(shù)據(jù)融合，充分發(fā)揮兩者在室內(nèi)定位中的優(yōu)勢(shì)，解決UWB系統(tǒng)由于外在因素?zé)o法完成有效定位、MEMS長期定位精度差等問題。本文所構(gòu)建的混合定位系統(tǒng)，在復(fù)雜室內(nèi)環(huán)境下定位的連續(xù)性、穩(wěn)定性和可靠性得到大大增強(qiáng)[12]。此外，成本較高的UWB定位設(shè)備也會(huì)因低成本MEMS的加入使得成本有所下降，在保證定位精度的前提下，UWB定位基站的布設(shè)成本也會(huì)有所降低。

1 基礎(chǔ)定位系統(tǒng)

1.1 UWB定位系統(tǒng)

UWB定位技術(shù)源于超寬帶無載波通信技術(shù)，其利用納秒至微秒級(jí)的非正弦波窄脈沖進(jìn)行通信，工作頻段為3.1～10.6GHz，可直接脈沖調(diào)制，不受載波干擾[13]。UWB定位在本質(zhì)上與GPS衛(wèi)星定位原理一致，即自己搭建基站，利用發(fā)射的UWB脈沖信號(hào)進(jìn)行測(cè)距或者定向，然后根據(jù)和GPS定位類似的算法，計(jì)算出每個(gè)基站到移動(dòng)站(定位標(biāo)簽)的距離，從而解算出移動(dòng)站的空間坐標(biāo)。UWB定位簡易示意圖如圖1所示。

系統(tǒng)主要由定位基站(也稱接收機(jī))、定位標(biāo)簽和中心處理器組成[13]。

按照測(cè)量參數(shù)不同，將UWB定位方法分為基于接收信號(hào)強(qiáng)度法(Received Signal Strength Indication, RSSI)和基于到達(dá)角度法(Angle of Arrival, AOA)、基于到達(dá)時(shí)間法(Time of Arrival, TOA)和基于到達(dá)時(shí)間差法(Time Difference of Arrival, TDOA)等[14-16]。本文采用AOA/TDOA算法，充分利用距離或角度信息，其原理如圖2所示。

傳感器能得到定位目標(biāo)的到達(dá)時(shí)間差Δt和到達(dá)角度α1和α2，設(shè)移動(dòng)站MS(x0,y0)、基準(zhǔn)站BS1(x1,y1)和BS2(x2,y2)，則

(1)

其中，c代表光速。聯(lián)立式(1)兩式即可求得MS的二維坐標(biāo)。對(duì)于三維情況，TDOA公式與二維類似，如式(2)所示。

(2)

AOA包括方向角λ和仰角φ，如式(3)所示。

(3)

本文所使用的AOA/TDOA算法，避免了TOA方法嚴(yán)格時(shí)鐘同步問題，采用了天線陣列，雖然設(shè)備相對(duì)比較復(fù)雜，但是精度得到了保證。每個(gè)傳感器探測(cè)一個(gè)標(biāo)簽的二維到達(dá)角度(AOA，方向角和仰角)，同時(shí)記錄下感應(yīng)的TDOA，從傳感器得來的數(shù)據(jù)可通過相應(yīng)的算法綜合起來使用，從而得到定位結(jié)果。

1.2 MEMS慣性定位系統(tǒng)

首先介紹本文坐標(biāo)系的定義及坐標(biāo)系間的轉(zhuǎn)換關(guān)系。載體坐標(biāo)系(b系)是指在定位過程中載體位置處的坐標(biāo)系，本文所指b系原點(diǎn)在載(車)體重心，x軸沿載(車)體橫軸向前，y軸沿載(車)體向左，z軸沿載(車)體豎直向上，如圖3所示。

(4)

MEMS慣性定位算法由初始對(duì)準(zhǔn)和定位解算兩部分組成，前者為后者提供初始姿態(tài)信息[17]。對(duì)慣性系統(tǒng)而言，其MEMS陀螺精度太低以至無法感測(cè)地球自轉(zhuǎn)角速度(約15(°)/h)，因而低精度MEMS捷聯(lián)慣導(dǎo)系統(tǒng)初始自對(duì)準(zhǔn)無法完成方位對(duì)準(zhǔn)，只能根據(jù)加速度計(jì)輸出解算出載體俯仰角和橫滾角[17]，即水平對(duì)準(zhǔn)。方位對(duì)準(zhǔn)則需要利用其他外部信息給出。

(1)水平初始對(duì)準(zhǔn)

靜基座條件下，重力加速度只在地垂線方向上有輸出(輸出為g)，有：

(5)

(6)

(2)方位初始對(duì)準(zhǔn)

對(duì)于初始航向角，若以地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，則初始航向角是指與真北的夾角，此角度需要磁力計(jì)或磁羅盤等提供；若以地面正交坐標(biāo)系為導(dǎo)航坐標(biāo)系，則直接裝訂初始方位角為0°，最終解算出來的坐標(biāo)即為該坐標(biāo)系下的結(jié)果。對(duì)于初始航向問題，本文采用直接裝訂的方式，將初始時(shí)刻計(jì)算得到的磁航向作為系統(tǒng)初始航向裝訂。

(3)定位解算和反饋校正

圖4所示為慣導(dǎo)解算過程的詳細(xì)框圖，主要顯示了根據(jù)陀螺儀和加速度計(jì)輸出計(jì)算車輛最終姿態(tài)、速度和位置的過程。

相關(guān)算法和公式如下所述[19]：

設(shè)計(jì)15維Kalman濾波器，其狀態(tài)向量對(duì)應(yīng)為：

(7)

KF估計(jì)出來的這些誤差量將被反饋到三軸陀螺儀、三軸加速度計(jì)、常規(guī)慣導(dǎo)解算的姿態(tài)、速度和位置，通過修正得到更為精確的定位結(jié)果。

(8)

根據(jù)補(bǔ)償后的陀螺輸出對(duì)姿態(tài)矩陣進(jìn)行更新：

(9)

(10)

由于MEMS器件采樣頻率較高，故可認(rèn)為在極短的時(shí)間內(nèi)受到一個(gè)恒力，載體做勻變速直線運(yùn)動(dòng)。用梯形算法計(jì)算速度和位置，公式如下：

(11)

前2個(gè)部分得到的姿態(tài)、速度、位置信息是慣導(dǎo)解算直接得到的信息，誤差會(huì)隨時(shí)間的增加而累積，故需利用KF估計(jì)量對(duì)其進(jìn)行進(jìn)一步修正：

(12)

通過KF得到的3個(gè)的姿態(tài)角誤差(小角度)對(duì)姿態(tài)矩陣進(jìn)行修正：

(13)

(14)

(15)

(16)

其中，arctan2(a，b)為反正切函數(shù)，值域?yàn)?-π，π)，與橫滾角、航向角定義范圍一致。

2 Kalman濾波器設(shè)計(jì)

2.1 動(dòng)態(tài)零速修正

假設(shè)小車在勻速、剎車、加速、轉(zhuǎn)彎等不同運(yùn)動(dòng)狀態(tài)過程中，不發(fā)生側(cè)滑與跳躍，則可以認(rèn)為車輛的天向與側(cè)向速度在車體運(yùn)動(dòng)中始終保持為零，即存在動(dòng)態(tài)零速：

(17)

可以利用車輛的天向速度與側(cè)向速度為零的速度信息，輔以里程計(jì)輸出的軸向速度信息與慣導(dǎo)解算得到的速度信息作差，估算捷聯(lián)解算的誤差信息，進(jìn)行反饋修正，從而提高定位精度。

以15維狀態(tài)向量為例，狀態(tài)量取慣導(dǎo)系統(tǒng)的3個(gè)姿態(tài)誤差角、3個(gè)速度誤差、3個(gè)位置誤差，三軸陀螺常值漂移、三軸加計(jì)常值偏置，如下所示：

(18)

(19)

定位算法簡化后的誤差模型：

(20)

根據(jù)定位算法的誤差模型和Kalman狀態(tài)量的選擇，可以得到狀態(tài)轉(zhuǎn)移矩陣為：

(21)

(22)

根據(jù)量測(cè)向量的選取，可得到對(duì)應(yīng)的量測(cè)矩陣：

(23)

通過狀態(tài)量的估計(jì)，完成Kalman濾波的估計(jì)與補(bǔ)償，可以得到最終的定位結(jié)果。

2.2 MEMS/UWB融合濾波器設(shè)計(jì)

根據(jù)實(shí)際UWB系統(tǒng)的輸出信息以及所采用的MEMS定位算法，本文最終采用KF松組合方式實(shí)現(xiàn)兩系統(tǒng)的融合定位[7，20-22]。單獨(dú)的車載MEMS定位系統(tǒng)，是采用帶動(dòng)態(tài)零速修正的慣導(dǎo)解算算法，利用實(shí)際解算速度和里程計(jì)速度動(dòng)態(tài)零速構(gòu)造量測(cè)信息對(duì)慣導(dǎo)誤差進(jìn)行修正，此過程通過KF實(shí)現(xiàn)。

對(duì)于捷聯(lián)慣導(dǎo)而言，以速度誤差為觀測(cè)量進(jìn)行狀態(tài)估計(jì)與反饋，進(jìn)而抑制慣導(dǎo)誤差積累，此過程不依賴于外界其他硬件設(shè)施。對(duì)于MEMS/UWB融合而言，在MEMS基礎(chǔ)上加入了UWB位置信息，需要構(gòu)造位置誤差觀測(cè)量再進(jìn)行KF?？紤]到2個(gè)系統(tǒng)的獨(dú)立性，組合結(jié)構(gòu)圖如圖5所示。

在上述2個(gè)KF當(dāng)中，考慮到其狀態(tài)量相同，只是量測(cè)信息有所不同，因此時(shí)間更新相似，可根據(jù)條件判斷選擇對(duì)應(yīng)的量測(cè)更新。因此，2個(gè)KF估計(jì)器是可以合并的。對(duì)圖5進(jìn)行合并，可得到MEMS和UWB的松組合方式的結(jié)構(gòu)框架如圖6所示。

由UWB提供位置信息P以及里程計(jì)信息與動(dòng)態(tài)零速信息，形成六維量測(cè)向量：

(24)

其中：Δpk=pINS-pUWB,Δvk由式(19)可得，則其對(duì)應(yīng)的量測(cè)矩陣為：

(25)

3 試驗(yàn)與分析

本文中在7.5m×7.5m×3m的實(shí)驗(yàn)室里布設(shè)4個(gè)UWB定位傳感器，大致布局如圖7所示。

UWB標(biāo)簽固定在小車頂部，MEMS慣性傳感器固定在小車中心。UWB標(biāo)簽示意如圖8所示。 MEMS器件選用ADI公司的ADIS6448十軸慣性器件，其外形如圖9所示，參數(shù)如表1所示。

表1 ADIS16448主要參數(shù)(A+G)

本文試驗(yàn)的路線設(shè)計(jì)為U型運(yùn)動(dòng)路線，便于分區(qū)間進(jìn)行誤差計(jì)算和定位精度分析。往返一次試驗(yàn)路徑為A-B-C-D-C-B-A，軌跡示意如圖10所示。

重復(fù)10次U型往返路徑作為融合試驗(yàn)路徑。整個(gè)試驗(yàn)的運(yùn)動(dòng)時(shí)間為348.2860s，運(yùn)動(dòng)距離為253.4369m。

融合過程中，由于MEMS解算周期與UWB位置輸出周期不一致，并且兩者周期存在非整數(shù)倍關(guān)系，所以需要考慮長時(shí)間解算的時(shí)鐘同步性問題。MEMS輸出頻率為102.4Hz，UWB輸出頻率為9.25Hz，根據(jù)兩者的時(shí)間關(guān)系，每20s進(jìn)行一次數(shù)據(jù)調(diào)整。一次調(diào)整時(shí)間偏差從0.1270s減小到0.0195s。時(shí)鐘同步性調(diào)整如圖11所示。

MEMS、UWB、MEMS/UWB三者定位效果如圖12～圖14所示。

由圖12可以看出，MEMS定位結(jié)果平滑，但隨著時(shí)間的積累，由于航向誤差的存在與不斷積累，解算路徑慢慢偏離實(shí)際運(yùn)動(dòng)路徑，誤差逐漸增加。

由圖13可以看出，UWB屬于絕對(duì)定位，其誤差在一個(gè)合理的范圍內(nèi)，整體運(yùn)動(dòng)軌跡在實(shí)際運(yùn)動(dòng)軌跡的兩側(cè)跳動(dòng)分布。由圖14可以看出，MEMS/UWB融合定位因?yàn)橛辛薝WB位置信息的加入，對(duì)MEMS的積累誤差有著明顯的抑制作用，可以得到近似于UWB定位的效果。對(duì)于UWB而言，該融合算法在UWB定位系統(tǒng)出現(xiàn)定位失效的情況下，仍能保持比較好的定位效果。表2所示為三種定位方式的終點(diǎn)坐標(biāo)以及誤差情況。

表2 融合定位試驗(yàn)的終點(diǎn)坐標(biāo)及誤差對(duì)比結(jié)果

由表2可以看出，融合定位對(duì)MEMS定位的積累誤差有著顯著的修正和抑制作用，將相對(duì)誤差從0.2401%降到0.0265%。而UWB自身定位精度較高，融合對(duì)UWB定位精度無顯著提升作用，但對(duì)UWB定位易受遮擋的缺陷有著很好的彌補(bǔ)作用。整體而言，MEMS/UWB融合定位可適當(dāng)彌補(bǔ)兩者的缺陷，擁有比單獨(dú)MEMS子系統(tǒng)更好的定位效果。

4 結(jié)論

本文以室內(nèi)定位為背景，針對(duì)UWB定位存在易受環(huán)境影響與遮擋的問題和MEMS定位中誤差隨時(shí)間積累的問題，文中完成了以下工作：

1)設(shè)計(jì)了以里程計(jì)速度信息和動(dòng)態(tài)零速作為觀測(cè)量的MEMS慣導(dǎo)解算算法，通過卡爾曼濾波器抑制慣導(dǎo)的誤差積累，實(shí)現(xiàn)了短期誤差在1%以內(nèi)的定位效果。

2)通過分析UWB和MEMS這2個(gè)定位子系統(tǒng)，在MEMS定位算法的基礎(chǔ)上，完善量測(cè)方程，建立速度、位置六維量測(cè)方程，完成MEMS/UWB融合算法的設(shè)計(jì)，實(shí)現(xiàn)兩者的融合定位。

3)針對(duì)融合過程中MEMS與UWB時(shí)鐘不同步的問題，提出了定時(shí)調(diào)整方法，減小時(shí)鐘不同步帶來的誤差。

本文提出的MEMS/UWB組合的室內(nèi)定位方法實(shí)現(xiàn)了兩者的優(yōu)勢(shì)互補(bǔ)，解決了MEMS定位長期發(fā)散問題與UWB定位短期失效問題。MEMS/UWB融合定位擁有比單獨(dú)子系統(tǒng)定位更好的定位精度與魯棒性。