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

鄒?強，陸甫光，蘭馗博，竇?彥

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

鄒?強1, 2，陸甫光1, 2，蘭馗博1，竇?彥3, 4

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

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

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

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

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

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

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

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

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

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

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

圖2?INS定位原理示意

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

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

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

因為狀態(tài)轉(zhuǎn)移模型是線性的，所以狀態(tài)轉(zhuǎn)移過程采用標準卡爾曼線性濾波形式．離散時間下系統(tǒng)狀態(tài)轉(zhuǎn)移模型定義為

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

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

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

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

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

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

1.3.2?UWB偽距更新

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

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

式中：為當前時刻第個基站測得的偽距信息；為當前時刻INS解算出來的位置；為第個UWB基站的位置．

2?改進的平方根無跡卡爾曼濾波

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

2.1?UWB偽距信息平滑

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

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

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

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

2.3?平方根無跡卡爾曼濾波

標準SRUKF的流程如下．

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

步驟2 初始化．

步驟3 計算Sigma點．

步驟4 時間更新．

步驟5 再次計算Sigma點.

步驟6 觀測更新.

步驟7 濾波更新．

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

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

3?實驗結(jié)果及分析

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

3.1?實驗器材及環(huán)境

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

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

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

Tab.1?UWB performance parameters

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

Tab.2?Accelerometer performance parameters

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

Tab.3?Gyroscope performance parameters

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

3.2?UWB定位實驗結(jié)果及分析

圖6顯示了UWB分別在LOS和NLOS環(huán)境下的定位結(jié)果．圖中的黑色點線代表人為預(yù)設(shè)的真實路徑，藍色虛線代表LOS環(huán)境下的UWB定位軌跡，紅色曲線代表NLOS環(huán)境下的UWB定位軌跡．從圖中可以看出，LOS環(huán)境下，UWB定位軌跡比較平滑且基本貼近于真實路徑；而NLOS環(huán)境下，UWB定位軌跡不夠平滑且偏離真實路徑明顯，在真實路徑的4個直角處甚至出現(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定位最大誤差增長了83.87%，平均絕對誤差增長了54.85%，均方根誤差增長了46.46%，這同樣印證了圖6的結(jié)論．

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

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

3.3?INS定位實驗結(jié)果及分析

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

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

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

UWB和INS的融合定位方案中，在NLOS環(huán)境下分別進行了基于EKF、SRUKF和ISRUKF的融合定位實驗．如圖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定位精度最高．另外，通過比較表４和表５可知，ISRUKF的定位精度相較于UWB-NLOS提高了24.28%．

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

Tab.5?UWB/INS fusion positioning errors 　m

4?結(jié)?語

針對UWB定位技術(shù)容易受NLOS環(huán)境影響的問題，本文提出了一種基于ISRUKF的UWB/INS組合室內(nèi)定位方法．在該方法中，首先通過INS優(yōu)化框架來初步限制INS的累積誤差，使得INS輸出的位置、速度和姿態(tài)等信息較為準確；然后利用TOA定位方法輸出UWB定位的坐標和偽距信息；最后基于ISRUKF實現(xiàn)UWB和INS的融合定位，其中ISRUKF在標準SRUKF的基礎(chǔ)上增加了UWB偽距信息平滑、UWB偽距信息離群值修正以及狀態(tài)協(xié)方差矩陣更新策略．實驗結(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]. 計算機工程與應(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ā)展趨勢[J]. 自動化學報，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.

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

陸甫光，m15161133133@163.com.

國家自然科學基金資助項目(6167010973).

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

(責任編輯：孫立華)