鄒?強(qiáng)，陸甫光，蘭馗博，竇?彥

基于ISRUKF的UWB/INS組合室內(nèi)定位方法研究

鄒?強(qiáng)1, 2，陸甫光1, 2，蘭馗博1，竇?彥3, 4

(1. 天津大學(xué)微電子學(xué)院，天津 300072；2. 天津市物聯(lián)網(wǎng)國(guó)際聯(lián)合研究中心，天津 300072；3. 天津衣聯(lián)網(wǎng)生態(tài)科技有限公司，天津 300350；4. 天津海爾洗滌電器有限公司，天津 300350)

超寬帶(UWB)定位技術(shù)是室內(nèi)定位領(lǐng)域中極具競(jìng)爭(zhēng)力的技術(shù)，但是在室內(nèi)非視距(NLOS)環(huán)境下，UWB定位技術(shù)會(huì)出現(xiàn)精度低和穩(wěn)定性差等問(wèn)題，難以滿足較高的室內(nèi)定位精度需求．UWB定位技術(shù)和慣性導(dǎo)航系統(tǒng)(INS)結(jié)合，可以減輕NLOS環(huán)境對(duì)UWB定位的影響，因此提出了一種基于改進(jìn)平方根無(wú)跡卡爾曼濾波(ISRUKF)的UWB/INS組合室內(nèi)定位方法．UWB/INS融合定位系統(tǒng)工作時(shí)，首先，通過(guò)INS輸出定位目標(biāo)的定位信息，但是單獨(dú)的INS解算結(jié)果誤差很大．然后，利用由零速更新(ZUPT)、零角速率更新(ZARU)和啟發(fā)式漂移消減(HDR)組成的INS優(yōu)化框架初步消除INS的累積誤差．最后，基于ISRUKF以緊耦合方式深度融合UWB定位信息和INS定位信息，得到融合定位結(jié)果．其中，ISRUKF在標(biāo)準(zhǔn)平方根無(wú)跡卡爾曼濾波(SRUKF)的基礎(chǔ)上增加了UWB偽距信息平滑、UWB偽距信息離群值修正以及狀態(tài)協(xié)方差矩陣更新策略．在室內(nèi)NLOS環(huán)境下進(jìn)行了一系列實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明所提方法既可以補(bǔ)償U(kuò)WB定位技術(shù)在NLOS環(huán)境下的精度丟失，也可以進(jìn)一步限制INS的累積誤差．另外，相較于擴(kuò)展卡爾曼濾波(EKF)和標(biāo)準(zhǔn)SRUKF，ISRUKF分別提高了9.48%和4.59%的定位精度，具有更好的濾波效果和魯棒性．

室內(nèi)定位；超寬帶定位技術(shù)；慣性導(dǎo)航系統(tǒng)；改進(jìn)平方根無(wú)跡卡爾曼濾波；緊耦合

隨著無(wú)線通信技術(shù)和移動(dòng)設(shè)備的發(fā)展，室內(nèi)定位服務(wù)逐漸成為現(xiàn)代社會(huì)生產(chǎn)生活中的重要內(nèi)容．傳統(tǒng)的室外定位技術(shù)，如GPS、GALILEO、GLONASS和北斗衛(wèi)星導(dǎo)航系統(tǒng)等在面對(duì)新興的室內(nèi)定位場(chǎng)景時(shí)，容易受墻體、家具和裝飾品等障礙物的影響，無(wú)法達(dá)到室內(nèi)定位的精度標(biāo)準(zhǔn)[1]．這就推動(dòng)了包括Wi-Fi、藍(lán)牙、可見(jiàn)光、ZigBee和RFID等室內(nèi)定位技術(shù)的發(fā)展[2]．雖然這些技術(shù)在室內(nèi)定位領(lǐng)域中應(yīng)用比較廣泛，但是仍然存在定位精度較差和成本較高的問(wèn)題．近年來(lái)，UWB定位技術(shù)憑借其精度高、功耗低和系統(tǒng)容量大等優(yōu)點(diǎn)從眾多室內(nèi)定位技術(shù)中脫穎而出，成為室內(nèi)定位領(lǐng)域中的關(guān)鍵技術(shù)[3]．然而UWB定位技術(shù)容易受到室內(nèi)NLOS環(huán)境的干擾，定位結(jié)果會(huì)出現(xiàn)較大誤差．因此如何識(shí)別并抑制NLOS環(huán)境對(duì)UWB定位的影響是一個(gè)亟待解決的問(wèn)題．傳統(tǒng)上的解決方法均是利用UWB自身信息來(lái)識(shí)別NLOS條件并進(jìn)行NLOS測(cè)距誤差補(bǔ)償[4-6]．盡管這類方法起到一定作用，但是單一UWB定位的性能終究是有限的，因此利用UWB和其他定位技術(shù)實(shí)現(xiàn)多源信息融合定位成為該領(lǐng)域的研究重點(diǎn)．Bargshady等[7]提出了一種UWB/Wi-Fi聯(lián)合定位方法，但是Wi-Fi部署復(fù)雜且需要時(shí)刻聯(lián)網(wǎng)．李中道等[8]提出了一種基于UWB/LiDAR的融合定位算法，但是LiDAR價(jià)格昂貴且體積較大．

INS是一種自主式導(dǎo)航系統(tǒng)，無(wú)需依賴任何外部信息就能提供定位目標(biāo)的位置、速度和姿態(tài)等，不受NLOS環(huán)境的影響．INS的核心器件是慣性測(cè)量單元(inertial measurement unit，IMU)，IMU具有體積小、重量輕、成本低和可靠性高等優(yōu)點(diǎn)[9]．INS隨著時(shí)間推移會(huì)出現(xiàn)誤差累積現(xiàn)象，通常INS可以利用自身信息來(lái)抑制累積誤差，如ZUPT[10]、ZARU[11]、HDR[12]和地磁偏航[13]等，但是該類方法效果有限.另一種更有效的方法是引入無(wú)線定位技術(shù)，如UWB、Wi-Fi、RFID和GPS等．

當(dāng)前，UWB/INS融合定位是實(shí)現(xiàn)室內(nèi)行人高精度定位的主流方案．過(guò)去已經(jīng)有相關(guān)人員做過(guò)了大量研究：Hol等[14]提出了一種UWB和IMU緊耦合的集成方法，該方法信息融合徹底，相較于松耦合提高了定位精度．緊耦合的融合方式通常涉及到非線性濾波．Yuan等[15]提出了一種基于EKF的UWB/INS數(shù)據(jù)融合方法，然而EKF一階泰勒級(jí)數(shù)展開(kāi)忽略了二階及以上的高階項(xiàng)，對(duì)于非線性系統(tǒng)可能導(dǎo)致濾波發(fā)散，并且涉及到的雅可比矩陣計(jì)算復(fù)雜，容易出錯(cuò)．Chen等[16]提出了一種基于UKF的UWB/PDR聯(lián)合定位系統(tǒng)，較好解決了非線性系統(tǒng)中濾波發(fā)散的問(wèn)題，但是高維數(shù)據(jù)下UKF容易因?yàn)閰f(xié)方差矩陣非正定而崩潰．Tian等[17]提出了一種使用粒子濾波(particle filter，PF)進(jìn)行行人跟蹤的INS/UWB融合定位系統(tǒng)，通過(guò)粒子重置的方法，解決了跟蹤丟失的問(wèn)題，但是PF計(jì)算量大．文獻(xiàn)[18-19]在原有UWB/INS的基礎(chǔ)上進(jìn)一步集成了其他傳感器或數(shù)據(jù)，雖然獲得了更高的定位精度，但是相應(yīng)的系統(tǒng)成本和復(fù)雜度也有所增加．

綜合考慮精度、成本和魯棒性等因素，本文提出了一種基于ISRUKF的UWB/INS組合室內(nèi)定位方法，該方法主要應(yīng)用于室內(nèi)行人動(dòng)態(tài)定位．在該UWB/INS融合定位系統(tǒng)中，通過(guò)分析各部分模型，設(shè)計(jì)了一種KF-EKF-ISRUKF架構(gòu)．其中，KF用于狀態(tài)轉(zhuǎn)移模型，EKF用于INS優(yōu)化框架，ISRUKF用于UWB偽距更新．本文驗(yàn)證了上述方法在室內(nèi)NLOS環(huán)境下對(duì)于行人動(dòng)態(tài)定位的有效性，實(shí)驗(yàn)結(jié)果表明ISRUKF的定位精度高于傳統(tǒng)的EKF和SRUKF．

1?UWB/INS融合定位系統(tǒng)

圖1顯示了UWB/INS融合定位系統(tǒng)的總框架.該系統(tǒng)主要分為3個(gè)部分，第1部分是INS解算，IMU通過(guò)內(nèi)置的加速度計(jì)和陀螺儀分別采集加速度數(shù)據(jù)和角速度數(shù)據(jù)，經(jīng)慣性導(dǎo)航方程解算后輸出定位目標(biāo)的位置、速度和姿態(tài)[9]；第2部分是INS優(yōu)化框架，當(dāng)定位目標(biāo)滿足靜止條件時(shí)，通過(guò)基于EKF的ZUPT、ZARU和HDR來(lái)限制INS的累積誤差；第3部分是UWB偽距更新，當(dāng)定位目標(biāo)滿足UWB偽距更新條件時(shí)，通過(guò)ISRUKF實(shí)現(xiàn)UWB/INS的融合定位．其中，ISRUKF由標(biāo)準(zhǔn)SRUKF、UWB偽距信息平滑、UWB偽距信息離群值修正和狀態(tài)協(xié)方差矩陣更新策略組成．

圖1?UWB/INS融合定位系統(tǒng)總框架

1.1?慣性導(dǎo)航方程

圖2?INS定位原理示意

本文中INS的導(dǎo)航坐標(biāo)系(n系)采取“東-北-天”坐標(biāo)系，機(jī)體坐標(biāo)系(b系)采取“左-前-上”坐標(biāo)系．在數(shù)學(xué)上，INS的導(dǎo)航過(guò)程可以描述為

1.2?狀態(tài)轉(zhuǎn)移模型

本文中，無(wú)論是基于EKF的INS優(yōu)化框架還是基于ISRUKF的UWB/INS融合定位，其狀態(tài)向量均是15維的狀態(tài)誤差向量，即

因?yàn)闋顟B(tài)轉(zhuǎn)移模型是線性的，所以狀態(tài)轉(zhuǎn)移過(guò)程采用標(biāo)準(zhǔn)卡爾曼線性濾波形式．離散時(shí)間下系統(tǒng)狀態(tài)轉(zhuǎn)移模型定義為

1.3?系統(tǒng)測(cè)量模型

系統(tǒng)測(cè)量模型由兩部分組成，分別是INS優(yōu)化框架和UWB偽距更新．

1.3.1?INS優(yōu)化框架

通過(guò)觀察人類行走時(shí)的步態(tài)特點(diǎn)，可以將人類行走過(guò)程簡(jiǎn)單地劃分為靜止階段和移動(dòng)階段．當(dāng)人類處于靜止階段時(shí)，理論上人腳的速度和角速度幾乎為零，并且航向也基本不會(huì)發(fā)生變化．但是實(shí)際情況中，受限于MEMS-IMU自身的精度，此時(shí)計(jì)算得到的速度并不為零、測(cè)量得到的角速度并不為零，航向也會(huì)有所變化．如果不對(duì)這個(gè)階段的相關(guān)數(shù)據(jù)進(jìn)行處理，那么隨著時(shí)間推移，INS的累積誤差會(huì)持續(xù)增大，嚴(yán)重時(shí)甚至導(dǎo)致INS無(wú)法定位．

考慮到室內(nèi)環(huán)境中地磁不穩(wěn)定，所以本文僅選取ZUPT、ZARU和HDR來(lái)組成INS優(yōu)化框架．如圖3所示，INS優(yōu)化框架首先基于廣義似然比檢測(cè)算法來(lái)檢測(cè)靜止階段，然后根據(jù)設(shè)定的條件分別提取出誤差觀測(cè)向量，最后通過(guò)EKF將這些誤差觀測(cè)向量用于更新?tīng)顟B(tài)誤差向量，并最終融入慣性導(dǎo)航方程中．

圖3?基于EKF的INS優(yōu)化框架示意

1.3.2?UWB偽距更新

UWB技術(shù)是近年來(lái)新興的一種無(wú)載波通信技術(shù)．通過(guò)發(fā)射納秒至皮秒級(jí)的極窄脈沖，UWB技術(shù)可以實(shí)現(xiàn)快速的數(shù)據(jù)傳輸．基于UWB技術(shù)的定位方案具有功耗低、傳播速率快、安全性好和定位精度高等優(yōu)點(diǎn)[3]．本文中的UWB定位采用的是到達(dá)時(shí)間(time of arrival，TOA)定位方法．TOA定位方法通過(guò)測(cè)量UWB信號(hào)從UWB待測(cè)標(biāo)簽到達(dá)UWB基站的傳輸時(shí)間來(lái)計(jì)算待測(cè)標(biāo)簽與基站間的距離．針對(duì)TOA定位方法中存在因基站和待測(cè)標(biāo)簽時(shí)鐘不同步而導(dǎo)致的測(cè)距誤差的問(wèn)題，本文決定采用雙邊雙向測(cè)距(double-sided two-way ranging，DS-TWR)算法來(lái)實(shí)現(xiàn)TOA定位．如圖4所示，二維平面內(nèi)，基于TOA定位方法的UWB定位至少需要3個(gè)UWB基站和1個(gè)UWB待測(cè)標(biāo)簽，以UWB基站為圓心，以TOA定位方法計(jì)算的距離為半徑作圓，理想條件下，三圓的交點(diǎn)就是定位目標(biāo)的位置，其具體計(jì)算過(guò)程為

緊耦合方式下，用UWB偽距信息和INS解算位置相對(duì)于UWB基站距離的距離差來(lái)作為狀態(tài)誤差向量的觀測(cè)值，定義觀測(cè)值為

式中：為當(dāng)前時(shí)刻第個(gè)基站測(cè)得的偽距信息；為當(dāng)前時(shí)刻INS解算出來(lái)的位置；為第個(gè)UWB基站的位置．

2?改進(jìn)的平方根無(wú)跡卡爾曼濾波

卡爾曼濾波理論是動(dòng)態(tài)系統(tǒng)狀態(tài)估計(jì)領(lǐng)域中的重要技術(shù)，廣泛應(yīng)用于參數(shù)估計(jì)、系統(tǒng)控制和定位導(dǎo)航等方面．SRUKF是標(biāo)準(zhǔn)UKF的改進(jìn)，其通過(guò)QR分解和Cholesky因子更新確保協(xié)方差矩陣的半正定性[20]，有效解決了UKF穩(wěn)定性差的問(wèn)題．但傳統(tǒng)的SRUKF沒(méi)有考慮測(cè)量數(shù)據(jù)離群值的影響以及狀態(tài)協(xié)方差矩陣在復(fù)雜系統(tǒng)下仍然容易產(chǎn)生非正定的問(wèn)題.因此本文提出的ISRUKF在標(biāo)準(zhǔn)SRUKF的基礎(chǔ)上，引入了UWB偽距信息平滑、UWB偽距信息離群值修正和狀態(tài)協(xié)方差矩陣更新策略，提高了UWB/INS融合定位系統(tǒng)在NLOS環(huán)境下的容錯(cuò)性和魯棒性．

2.1?UWB偽距信息平滑

UWB待測(cè)標(biāo)簽通常固定在人體上，人在運(yùn)動(dòng)的過(guò)程中難免會(huì)使UWB待測(cè)標(biāo)簽產(chǎn)生位移，進(jìn)而導(dǎo)致UWB定位系統(tǒng)所測(cè)得的偽距信息波動(dòng)．針對(duì)這一問(wèn)題，在加固UWB待測(cè)標(biāo)簽的傳統(tǒng)方法上，提出了一種UWB偽距信息窗口平滑方法，該方法的窗口大小設(shè)置為7．經(jīng)過(guò)該方法處理過(guò)的原始UWB數(shù)據(jù)會(huì)變得更平滑且準(zhǔn)確．其公式為

2.2?UWB偽距信息離群值修正

嚴(yán)重的NLOS環(huán)境會(huì)造成TOA定位方法的測(cè)距誤差較大．如何降低NLOS測(cè)距誤差對(duì)定位精度的影響是室內(nèi)定位系統(tǒng)的重要挑戰(zhàn)．針對(duì)NLOS環(huán)境的影響，通過(guò)設(shè)定某一閾值來(lái)鑒別NLOS誤差．對(duì)于當(dāng)前時(shí)刻下的位置，可以得到兩組距離信息：一組是UWB基站直接測(cè)量UWB待測(cè)標(biāo)簽位置所得的距離信息；另一組是由INS的位置相對(duì)于UWB基站位置解算出來(lái)的距離信息．將二者相減并取絕對(duì)值，如果該絕對(duì)值大于所設(shè)定的閾值，那么便判定此時(shí)的UWB信號(hào)受到NLOS環(huán)境干擾，該組UWB偽距信息不能直接參加UWB偽距更新過(guò)程．其計(jì)算式為

之后需要對(duì)該組UWB偽距信息進(jìn)行修正，將UWB偽距離群值依次減去NLOS誤差經(jīng)驗(yàn)值便可得到較為正確的新UWB偽距信息，從而完成對(duì)NLOS誤差的抑制．修正公式為

2.3?平方根無(wú)跡卡爾曼濾波

標(biāo)準(zhǔn)SRUKF的流程如下．

步驟1 Sigma點(diǎn)權(quán)值設(shè)置．

步驟2 初始化．

步驟3 計(jì)算Sigma點(diǎn)．

步驟4 時(shí)間更新．

步驟5 再次計(jì)算Sigma點(diǎn).

步驟6 觀測(cè)更新.

步驟7 濾波更新．

步驟8 狀態(tài)協(xié)方差矩陣更新．

2.4?狀態(tài)協(xié)方差矩陣更新策略

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

在這項(xiàng)工作中，首先，進(jìn)行了UWB在視距(line-of-sight，LOS)和NLOS環(huán)境下的定位實(shí)驗(yàn)，分析了NLOS環(huán)境對(duì)于UWB定位的影響；然后，進(jìn)行了單一的INS定位實(shí)驗(yàn)和加入INS優(yōu)化框架后的INS定位實(shí)驗(yàn)，分析了INS優(yōu)化框架對(duì)于INS定位的影響；最后，在NLOS環(huán)境下，進(jìn)行了UWB/INS融合定位實(shí)驗(yàn)，其中重點(diǎn)比較了ISRUKF、SRUKF和EKF的融合性能，實(shí)驗(yàn)結(jié)果表明本文所提出的ISRUKF具有優(yōu)越性．

3.1?實(shí)驗(yàn)器材及環(huán)境

實(shí)驗(yàn)中所涉及的器材主要包括UWB基站4個(gè)、UWB待測(cè)標(biāo)簽1個(gè)、MEMS-IMU 1個(gè)、筆記本電腦2臺(tái)、三角支架4個(gè)、白板1塊、膠帶1卷、移動(dòng)電源和數(shù)據(jù)線若干．其中，UWB是DWM1000模塊，工作頻率設(shè)置為10Hz，其具體性能參數(shù)如表1所示；MEMS-IMU是博世BMI-055，工作頻率設(shè)置為100Hz，其內(nèi)置的加速計(jì)和陀螺儀的具體性能參數(shù)分別如表2和表3所示．

如圖5所示，實(shí)驗(yàn)場(chǎng)景是長(zhǎng)10m、寬6m的室內(nèi)矩形區(qū)域．障礙物是長(zhǎng)1.55m、寬0.1m、高1.8m的白板，當(dāng)需要制造NLOS環(huán)境時(shí)將其放置在矩形區(qū)域中心．4個(gè)UWB基站分別垂直固定在高為1.7m的三角支架上，1個(gè)UWB待測(cè)標(biāo)簽垂直固定在定位目標(biāo)頭部，MEMS-IMU水平固定在定位目標(biāo)右腳背上.

表1?UWB性能參數(shù)

Tab.1?UWB performance parameters

表2?加速度計(jì)性能參數(shù)

Tab.2?Accelerometer performance parameters

表3?陀螺儀性能參數(shù)

Tab.3?Gyroscope performance parameters

圖5?實(shí)驗(yàn)環(huán)境平面示意

3.2?UWB定位實(shí)驗(yàn)結(jié)果及分析

圖6顯示了UWB分別在LOS和NLOS環(huán)境下的定位結(jié)果．圖中的黑色點(diǎn)線代表人為預(yù)設(shè)的真實(shí)路徑，藍(lán)色虛線代表LOS環(huán)境下的UWB定位軌跡，紅色曲線代表NLOS環(huán)境下的UWB定位軌跡．從圖中可以看出，LOS環(huán)境下，UWB定位軌跡比較平滑且基本貼近于真實(shí)路徑；而NLOS環(huán)境下，UWB定位軌跡不夠平滑且偏離真實(shí)路徑明顯，在真實(shí)路徑的4個(gè)直角處甚至出現(xiàn)了比較大的“毛刺狀”凸起．

圖6?LOS和NLOS環(huán)境下的UWB定位結(jié)果比較

表4是LOS和NLOS環(huán)境下的UWB定位誤差的數(shù)據(jù)分析．由表4可知，相較于LOS環(huán)境，NLOS環(huán)境下的UWB定位最大誤差增長(zhǎng)了83.87%，平均絕對(duì)誤差增長(zhǎng)了54.85%，均方根誤差增長(zhǎng)了46.46%，這同樣印證了圖6的結(jié)論．

表4?LOS和NLOS環(huán)境下的UWB定位誤差

Tab.4 UWB positioning errors under LOS and NLOS environment 　m

3.3?INS定位實(shí)驗(yàn)結(jié)果及分析

圖7是原始的INS和經(jīng)過(guò)優(yōu)化后的INS的定位結(jié)果比較．圖中黑色點(diǎn)線是人為預(yù)設(shè)的真實(shí)路徑，藍(lán)色虛線是原始的INS定位軌跡，紅色曲線是加入INS優(yōu)化框架后的INS定位軌跡．由圖7可知，原始的INS定位軌跡在起始階段就發(fā)生了巨大漂移，無(wú)法實(shí)現(xiàn)正常的定位．而引入INS優(yōu)化框架后的INS定位，其定位軌跡平滑且形狀基本與真實(shí)路徑形狀相吻合，但是受累積誤差影響，軌跡整體偏移的趨勢(shì)依然嚴(yán)重.

圖7?INS和INS優(yōu)化框架定位結(jié)果比較

3.4?UWB/INS融合定位實(shí)驗(yàn)結(jié)果及分析

UWB和INS的融合定位方案中，在NLOS環(huán)境下分別進(jìn)行了基于EKF、SRUKF和ISRUKF的融合定位實(shí)驗(yàn)．如圖8所示，UWB/INS融合定位結(jié)果明顯優(yōu)于圖6中的UWB-NLOS定位結(jié)果，但是3種融合定位算法的定位軌跡基本重合，肉眼較難分辨．

圖8?UWB/INS融合定位結(jié)果

圖9是誤差累積分布函數(shù)．由圖9可知，EKF、SRUKF和ISRUKF中80%的定位誤差分別在0.2017m、0.1988m和0.1901m以下，均小于UWB-NLOS的0.2200m．這表明融合定位方案在NLOS環(huán)境下具有突出性能，其中以ISRUKF的定位效果最好．

圖9?誤差累積分布函數(shù)

表5是3種融合定位算法的誤差分析，以均方根誤差為例，NLOS環(huán)境下，ISRUKF的定位精度相較于SRUKF提高了4.59%，相較于EKF提高了9.48%．3種融合定位算法中，本文提出的ISRUKF定位精度最高．另外，通過(guò)比較表４和表５可知，ISRUKF的定位精度相較于UWB-NLOS提高了24.28%．

表5?UWB/INS融合定位誤差

Tab.5?UWB/INS fusion positioning errors 　m

4?結(jié)?語(yǔ)

針對(duì)UWB定位技術(shù)容易受NLOS環(huán)境影響的問(wèn)題，本文提出了一種基于ISRUKF的UWB/INS組合室內(nèi)定位方法．在該方法中，首先通過(guò)INS優(yōu)化框架來(lái)初步限制INS的累積誤差，使得INS輸出的位置、速度和姿態(tài)等信息較為準(zhǔn)確；然后利用TOA定位方法輸出UWB定位的坐標(biāo)和偽距信息；最后基于ISRUKF實(shí)現(xiàn)UWB和INS的融合定位，其中ISRUKF在標(biāo)準(zhǔn)SRUKF的基礎(chǔ)上增加了UWB偽距信息平滑、UWB偽距信息離群值修正以及狀態(tài)協(xié)方差矩陣更新策略．實(shí)驗(yàn)結(jié)果表明，基于ISRUKF的UWB/INS組合室內(nèi)定位方法提高了融合定位系統(tǒng)的定位性能和魯棒性，在NLOS環(huán)境下，以均方根誤差為例，ISRUKF的定位精度相較于SRUKF提高了4.59%，相較于EKF提高了9.48%，相較于UWB-NLOS提高了24.28%．

［1］ Yassin A，Nasser Y，Awad M，et al. Recent advances in indoor localization：A survey on theoretical approaches and applications[J]. IEEE Communications Surveys & Tutorials，2017，19(2)：1327-1346.

［2］ Zafari F，Gkelias A，Leung K K. A survey of indoor localization systems and technologies[J]. IEEE Communications Surveys & Tutorials，2019，21(3)：2568-2599.

［3］ Abdulrahman A，Abdulmalik A S，Mansour A，et al. Ultra wideband indoor positioning technologies：Analysis and recent advances[J]. Sensors，2016，16(5)：1-36.

［4］ Yan L B，Lu Y，Zhang Y R. An improved NLOS identifica-tion and mitigation approach for target tracking in wireless sensor networks[J]. IEEE Access，2017，5：2798-2807.

［5］ Yang X F，Zhao F，Chen T J. NLOS identification for UWB localization based on import vector machine[J]. AEU-International Journal of Electronics and Communications，2018，87：128-133.

［6］ Yu K G，Wen K，Li Y B，et al. A novel NLOS mitigation algorithm for UWB localization in Harsh indoor environments[J]. IEEE Transactions on Vehicular Technology，2019，68(1)：686-699.

［7］ Bargshady N，Pahlavan K，Alsindi N A. Hybrid WiFi/UWB，cooperative localization using particle filter[C]//2015 International Conference on Computing，Networking and Communications(ICNC). Garden Grove，USA，2015：1055-1060.

［8］ 李中道，劉元盛，常飛翔，等. 室內(nèi)環(huán)境下UWB與LiDAR融合定位算法研究[J]. 計(jì)算機(jī)工程與應(yīng)用，2021，57(6)：260-266.

Li Zhongdao，Liu Yuansheng，Chang Feixiang，et al. Research on UWB and LiDAR fusion positioning algorithm in indoor environment[J]. Computer Engineering and Applications，2021，57(6)：260-266(in Chinese).

［9］ 王?巍. 慣性技術(shù)研究現(xiàn)狀及發(fā)展趨勢(shì)[J]. 自動(dòng)化學(xué)報(bào)，2013，39(6)：723-729.

Wang Wei. Status and development trend of inertial technology[J]. Acta Automatica Sinica，2013，39(6)：723-729(in Chinese).

［10］ Foxlin E. Pedestrian tracking with shoe-mounted inertial sensors[J]. IEEE Computer Graphics and Applications，2005，25(6)：38-46.

［11］ Jiménez A R，Seco F，Prieto J C，et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU[C]//2010 7th Workshop on Positioning，Navigation and Commu-nication(WPNC). Dresden，Germany，2010：135-143.

［12］ Borenstein J，Ojeda L，Kwanmuang S. Heuristic reduction of gyro drift for personnel tracking systems [J]. Journal of Navigation，2009，62(1)：41-58.

［13］ Zhang W C，Li X H，Wei D Y，et al. A foot-mounted PDR system based on IMU/EKF＋HMM＋ZUPT＋ZARU+HDR+compass algorithm[C]//2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN). Sapporo，Japan，2017：1-5.

［14］ Hol J D，Dijkstra F，Luinge H，et al. Tightly coupled UWB/IMU pose estimation[C]// 2009 IEEE Interna-tional Conference on Ultra-Wideband(ICUWB). Vancouver，Canada，2009：688-692.

［15］ Yuan X，Chen X Y，Jin C，et al. Improving tightly-coupled model for indoor pedestrian navigation using foot-mounted IMU and UWB measurements[C]//2016 IEEE International Instrumentation and Measurement Technology Conference Proceedings. Taipei，China，2016：1-5.

［16］ Chen P Z，Kuang Y，Chen X Y. A UWB/improved PDR integration algorithm applied to dynamic indoor positioning for pedestrians[J]. Sensors，2017，17(9)：2065.

［17］ Tian Q L，Wang I K，Salcic Z. A resetting approach for ins and uwb sensor fusion using particle filter for pedestrian tracking[J]. IEEE Transactions on Instru-mentation and Measurement，2020，69(8)：5914-5921.

［18］ Benini A，Mancini A，Longhi S. An IMU/UWB/vision-based extended Kalman filter for mini-UAV localization in indoor environment using 802.15.4a wireless sensor network[J]. Journal of Intelligent & Robotic Systems，2012，70(1/2/3/4)：461-476.

［19］ Liu F，Wang J，Zhang J X，et al. An indoor localization method for pedestrians base on combined UWB/ PDR/floor map[J]. Sensors(Basel，Switzerland)，2019，19(11)：2578-2596.

［20］ Merwe R，Wan E A. The square-root unscented Kalman filter for state and parameter-estimation[C]//2001 IEEE International Conference on Acoustics，Speech，and Signal Processing(ICASSP). Salt Lake City，USA，2001：3461-3464.

UWB/INS Combined Indoor Positioning Method Based on ISRUKF

Zou Qiang1, 2，Lu Fuguang1, 2，Lan Kuibo1，Dou Yan3, 4

(1. School of Microelectronics，Tianjin University，Tianjin 300072，China；2. International Joint Research Center for Internet of Things，Tianjin University，Tianjin 300072，China；3. Tianjin Internet of Clothes Ecological Technology Co.，Ltd.，Tianjin 300350，China；4. Tianjin Haier Washing Electric Appliance Co.，Ltd.，Tianjin 300350，China)

Ultra-wide band(UWB)positioning technology is a highly competitive technology in the field of indoor positioning；however，its low accuracy and poor stability in the indoor non-line-of-sight(NLOS)environment makes it difficult to meet the high indoor positioning accuracy requirements. Combining UWB positioning technology and inertial navigation system(INS)can reduce the impact of the NLOS environment on UWB positioning；therefore，a UWB/INS combined indoor positioning method based on improved square root unscented Kalman filter(ISRUKF)is proposed in this study. The positioning information of the positioning target was first output through the INS when the UWB/INS fusion positioning system was in operation；however，the single INS results contain very large errors. Then，to initially eliminate the INS cumulative errors，an INS optimized framework consisting of zero velocity update(ZUPT)，zero angular rate update(ZARU)，and heuristic drift reduction(HDR) was utilized. Finally，based on ISRUKF，the UWB and INS positioning information was deeply fused in a tightly coupled manner，and the fusion positioning results were obtained. Using the standard square root unscented Kalman filter(SRUKF)，the ISRUKF added UWB pseudo-range information smoothing，UWB pseudo-range information outlier correction，and state covariance matrix update strategy. A series of experiments were completed in the indoor NLOS environment. The experimental results showed that the proposed method could compensate UWB positioning technology accuracy loss in the NLOS environment and further eliminate the INS cumulative errors. In addition，the ISRUKF improves the positioning accuracy by 9.48% and 4.59%，compared to the EKF and SRUKF，respectively，and it has a better filtering effect and robustness.

indoor positioning；ultra-wide band positioning technology；inertial navigation system；improved square root unscented Kalman filter；tightly coupled

10.11784/tdxbz202105006

TN967.2

A

0493-2137(2022)05-0496-08

2021-05-05；

2021-06-29.

鄒?強(qiáng)（1978—??），男，博士，副教授，zouqiang@tju.edu.cn.

陸甫光，m15161133133@163.com.

國(guó)家自然科學(xué)基金資助項(xiàng)目(6167010973).

Supported by the National Natural Science Foundation of China(No. 6167010973) .

(責(zé)任編輯：孫立華)