?？谍?，甄 杰，李晨輝，楊文東

藍牙陣列/IMU/里程計室內(nèi)組合定位方法

?？谍?,2，甄 杰1，李晨輝1,2，楊文東1

（1. 中國測繪科學研究院，北京 100036；2. 遼寧工程技術(shù)大學測繪與地理科學學院，遼寧阜新 123000）

在室內(nèi)定位領(lǐng)域，慣性測量單元（IMU）、里程計等常見傳感器存在累積誤差，且無法利用全球衛(wèi)星導航系統(tǒng)（GNSS）提供位置校準服務，而采用基于藍牙信號的陣列天線定位結(jié)果易受到非視距（NLOS）誤差影響。針對上述問題，提出基于藍牙陣列/IMU/里程計組合的擴展卡爾曼濾波（EKF）模型。根據(jù)慣性導航的誤差推算模型，同時利用里程計運動特性與單基站陣列天線測角定位原理，構(gòu)造了速度觀測及2維位置觀測信息，從而提高組合導航性能?；谳喪揭苿訙y量平臺的室內(nèi)實測結(jié)果表明：該組合方法有效減小IMU、里程計存在的誤差累計以及藍牙陣列NLOS誤差影響，整體運動軌跡與真實軌跡較為符合，能夠為運動載體提供準確且豐富的位姿信息。

組合導航；擴展卡爾曼濾波；慣性導航；里程計；藍牙陣列

0 引言

在車載導航及移動機器人等領(lǐng)域，常常搭載慣性測量單元（inertial measurement unit, IMU）、輪式里程計（簡稱為里程計）、全球衛(wèi)星導航系統(tǒng)（global navigation satellite system, GNSS）等多種定位設(shè)備。在室內(nèi)環(huán)境下，由于衛(wèi)星信號受到建筑物的遮擋后衰減嚴重，GNSS難以在室內(nèi)環(huán)境下提供有效的定位服務，因此室內(nèi)定位方法往往采用無線保真（wireless fidelity, WiFi）、藍牙、超寬帶（ultra-wide band, UWB）等多種定位技術(shù)。

不同傳感器系統(tǒng)存在各自的優(yōu)勢與不足。在室內(nèi)領(lǐng)域基于藍牙信號的陣列天線系統(tǒng)定位結(jié)果受信號遮擋，多徑影響明顯[1]；此外陣列天線無法提供載體的姿態(tài)角信息，其數(shù)據(jù)更新速率較慢，當面對復雜環(huán)境時，單一的藍牙陣列系統(tǒng)難以滿足實時快速定位需求。慣性導航系統(tǒng)（inertial navigation system, INS）輸出頻率高，不受外界環(huán)境影響，但是其速度與位置誤差隨著時間積分會加速累積。里程計系統(tǒng)在平整的路面上能夠提供較為準確的線速度測量值，然而在復雜不平的地形和光滑地面上的性能較差，且缺乏部分位姿信息[2-3]。因此面對復雜環(huán)境時，單一的傳感器系統(tǒng)往往不能滿足位置服務需求，需要利用不同傳感器之間的互補性來面對不同的定位場景。傳感器之間的多層次、多空間的信息互補和優(yōu)化組合處理是目前數(shù)據(jù)融合的難點，也是當今研究的熱點[4]-[5]。

文獻[6]采用了基于魯棒擴展卡爾曼濾波（extended Kalman filter, EKF）的INS/GNSS/視覺里程計組合導航方法，并將EKF與胡貝爾（Huber）方法結(jié)合，克服觀測量受噪聲干擾時對導航性能的影響，以提升系統(tǒng)魯棒性。文獻[7]采取了UWB與INS的融合定位方案，利用單個UWB基于往返時間的測距值與INS進行卡爾曼濾波組合，采用UWB/INS緊組合模型來解決UWB測距的時間異步問題。文獻[8]采用卡爾曼濾波算法對輪式里程計與UWB數(shù)據(jù)進行不同信賴度的融合，實驗結(jié)果顯示；組合后的數(shù)據(jù)相較于單獨UWB系統(tǒng)而言，穩(wěn)定性和定位精度均有了較大的提高，很好地解決了里程計的誤差累計與UWB系統(tǒng)存在非視距（non line of sight, NLOS）誤差的問題。文獻[9]提出了雙通道開關(guān)切換輪采雙極化天線陣，外加比對天線的比相測角方法，消除了由天線開關(guān)切換引起的頻率估計誤差，結(jié)合角度和極化解耦的超分辨估計算法，實現(xiàn)了單基站高精度定位。

在定位導航過程中，常采用對獲得的數(shù)據(jù)信息中的誤差進行濾波的方法，因為誤差狀態(tài)是小量，線性化時更精確，同時能夠避免與過度參數(shù)化相關(guān)的問題以及相關(guān)協(xié)方差矩陣奇異性的風險[10]。本文提出藍牙陣列/IMU/里程計3種傳感器組合方法，并基于IMU誤差模型并融合里程計、藍牙陣列天線的觀測值構(gòu)建基于誤差狀態(tài)的擴展卡爾曼濾波器，以期有效結(jié)合各種傳感器的優(yōu)勢特性，增加室內(nèi)定位結(jié)果的可靠性[11]。

1 組合定位模型

本文采用基于誤差狀態(tài)的擴展卡爾曼濾波模型，分為預測和更新2個主要過程。預測過程通過狀態(tài)方程確定，觀測方程給出數(shù)據(jù)觀測部分，通過結(jié)合先驗值以及當前觀測來更新出最優(yōu)估計值。

1.1 狀態(tài)方程

IMU的誤差方程可以根據(jù)慣性導航解算公式[12-13]得到，即為

根據(jù)誤差方程可以寫成狀態(tài)方程的通用形式為

1.2 觀測方程

考慮速度誤差時的速度轉(zhuǎn)換方程可以表示為

將通過觀測得到的值作為真實值，加入測量誤差后稱為理想值。二者之間的關(guān)系可以表示為

將式（6）代入式（5）可得

將式（4）代入式（7）可得

將載體正常行進中橫向和天向的速度設(shè)置為零。觀測方程寫為

對于觀測方程，不需要乘以濾波周期，可直接寫為

根據(jù)文中1.1、1.2節(jié)中推導的狀態(tài)方程與觀測方程，并將式（12）和式（13）中各量帶入EKF中，可以得到后驗狀態(tài)量與后驗協(xié)方差值，構(gòu)建的完整濾波器預測與更新過程可以表示為

1.3 融合整體框架

基于EKF融合的整體框架如圖1所示。

圖1 基于EKF融合的整體框架

將獲取的IMU角速度與加速度數(shù)據(jù)代入狀態(tài)方程進行慣性解算，從而得到先驗狀態(tài)值。當存在有里程計速度觀測或者藍牙陣列位置觀測值時，代入觀測方程，并計算得到卡爾曼增益矩陣；同時根據(jù)濾波器獲得位置、速度與姿態(tài)誤差；最后得到誤差修正后的后驗估計值。當不存在觀測值時，不需要執(zhí)行卡爾曼濾波的后3個方程，即后驗值等于先驗值。本文設(shè)計的方案即采用藍牙陣列/IMU/里程計松耦合方式，結(jié)合多種傳感器優(yōu)勢，一方面可以依據(jù)IMU和里程計的短時間內(nèi)高頻率、高精度的優(yōu)點來有效解決藍牙陣列天線中存在NLOS誤差的問題，另一方面根據(jù)藍牙陣列天線定位結(jié)果消除IMU和里程計存在的累積誤差，從而得到更加豐富的載體信息以及魯棒性更強的導航定位結(jié)果。

2 實驗與結(jié)果分析

實驗所使用的數(shù)據(jù)采集設(shè)備為杰克卡爾（Jackal）移動測量平臺。該平臺裝備的工控機為8 GB內(nèi)存，搭載Intel（因特）Core（酷睿）i7-7567U處理器，烏班圖（Ubuntu）18.04操作系統(tǒng)。外部傳感器上采用克森斯（Xsense）慣性測量單元，內(nèi)置于移動測量平臺，車輪上有著獨立的運動控制模塊。同時在Jackal移動測量平臺上在放置藍牙標簽，作為藍牙陣列信源以采集陣列天線定位結(jié)果。移動測量平臺及藍牙陣列天線系統(tǒng)設(shè)備如圖2所示。

圖2 實驗設(shè)備

移動測量平臺具體技術(shù)指標如表1所示。

實驗地點選區(qū)某棟大樓的3樓，包含走廊及318會議室房間，實驗場地實現(xiàn)藍牙陣列天線全覆蓋，實驗環(huán)境如圖3所示。

圖3 實驗環(huán)境示意圖

表1 Jackal小型移動平臺底盤技術(shù)指標

在時間基準統(tǒng)一方面，本文采用服務器時間作為時間基準。藍牙標簽的位置數(shù)據(jù)由手機客戶端獲取，里程計及IMU原始數(shù)據(jù)通過移動測量平臺上搭載的米尼（mini）計算機可以獲得。2個終端均以服務器時間為基準，并根據(jù)網(wǎng)絡(luò)時間協(xié)議（network time protocol, NTP）進行時間同步。

考慮到實際應用層面場景的多樣性，本次實驗采用復雜程度不同的2條實驗路徑進行測試，以驗證設(shè)計方法的可行性。操縱移動測量平臺經(jīng)過不同的路徑環(huán)境，第1條實驗路徑為移動測量平臺從點出發(fā)，依次經(jīng)由點后沿走廊移動至點結(jié)束。第2條實驗路徑為移動測量平臺以點為起始點，經(jīng)過點后沿走廊返回至點結(jié)束。路徑1的采樣時間為190 s，路徑2的采樣時間為316 s，IMU的數(shù)據(jù)輸出頻率為70 Hz，里程計頻率為 50 Hz，獲取藍牙陣列天線定位結(jié)果的頻率約為 1 Hz左右。圖4、圖5分別為2條路徑下藍牙陣列定位軌跡。

從圖4和圖5可以看出：藍牙陣列動態(tài)實驗定位結(jié)果較符合真實運動軌跡，整體精度較高；但載體運動過程中實驗環(huán)境存在墻體等障礙物，信號傳播受到NLOS誤差影響，部分點位定位結(jié)果存在一定程度偏差，若依賴系統(tǒng)本身的原始輸出，遠距離點的定位精度無法保證。因此考慮加入慣性導航和里程計模型，結(jié)合多種傳感器信息，并構(gòu)造基于誤差狀態(tài)的卡爾曼濾波器以改善定位結(jié)果。

圖4 路徑1藍牙陣列定位軌跡

圖5 路徑2藍牙陣列定位軌跡

對藍牙陣列/IMU/里程計數(shù)據(jù)進行處理，得到移動測量平臺的路徑軌跡圖以及橫滾角、俯仰角、方位角等3個方向的角度值。本文選用高精度激光點云與地圖匹配軌跡作為真實軌跡，且通過高精度全站儀對點云匹配輸出的定位結(jié)果進行測試，定位精度保持在5 cm之內(nèi)。對路徑1和路徑2分別進行3次定位實驗，3次實驗定位結(jié)果相近。圖6給出路徑1的藍牙陣列/IMU/里程計組合的其中1組的實驗軌跡以及真實軌跡。圖7給出了對應的3個方向角度。圖8給出了一組藍牙陣列/ IMU/里程計組合實驗路徑2的實驗軌跡以及真實軌跡。圖9給出了對應的3個方向角度。

圖6 藍牙陣列/IMU/里程計組合實驗路徑1

圖7 實驗路徑1三個方向角度

圖8 藍牙陣列/IMU/里程計組合實驗路徑2

從圖6至圖9可以看出，藍牙陣列/IMU/里程計組合定位結(jié)果能夠很好地改正藍牙陣列定位結(jié)果，且較好地消除IMU、里程計的累計誤差，整體軌跡較貼合真實軌跡，并存在3個方向姿態(tài)信息。在真實軌跡中找相同時刻對應的位置坐標進行作差處理，得到藍牙陣列/IMU/里程計組合的平面位置誤差曲線，如圖10至圖11所示。

對路徑1和路徑2的3次實驗結(jié)果進行定量分析，統(tǒng)計得到藍牙陣列/IMU/里程計組合實驗的均方根誤差，同時對位置誤差取絕對值從而計算得到最大值，結(jié)果統(tǒng)計如表2所示。

圖9 實驗路徑2三個方向角度值

圖10 藍牙陣列/IMU/里程計組合實驗路徑1位置誤差

圖11 藍牙陣列/IMU/里程計組合實驗路徑2位置誤差

表2 藍牙陣列/IMU/里程計融合實驗平面位置誤差 m

從以上實驗結(jié)果可以看出，藍牙陣列/IMU/里程計組合結(jié)果整體精度較高，保持在0.5 m以內(nèi)，其中：路徑1中方向最大值為0.49 m，均方根誤差平均值為0.12 m，方向最大值為0.46 m，均方根誤差平均值為0.14 m；路徑2中方向最大值為0.48 m，均方根誤差平均值為0.09 m，方向上最大值為0.47 m，均方根誤差平均值為0.13 m。

不同復雜程度路徑下室內(nèi)環(huán)境的實測數(shù)據(jù)結(jié)果表明，本文組合算法較為可靠，同時結(jié)合了多種傳感器的優(yōu)勢，彌補單一傳感器存在的性能不足。融合結(jié)果能夠有效地消除IMU、里程計等傳感器存在的累積誤差，同時依據(jù)IMU和里程計準確的誤差模型，組合方法能夠提高藍牙陣列定位的穩(wěn)定可靠性，削弱由于NLOS誤差存在的異常值影響，且能提供載體較為精確且高頻的姿態(tài)及位置信息。藍牙陣列/IMU/里程計組合定位方法可以在室內(nèi)環(huán)境下提供較為可靠的定位精度。

3 結(jié)束語

本文主要研究多傳感器定位方法，設(shè)計了藍牙陣列/IMU/里程計的組合方法，構(gòu)建IMU誤差模型，同時提取藍牙陣列天線和里程計數(shù)據(jù)作為觀測信息。實測數(shù)據(jù)結(jié)果表明，本文提出的組合方法能利用這3個傳感器各自優(yōu)點，且基于誤差狀態(tài)的EKF濾波結(jié)果有著較好的實用性，既可以削弱藍牙信號受NLOS影響的定位偏差，同時也能消除IMU、里程計存在的誤差累計問題，有效地將相對定位與絕對定位方式相結(jié)合，提升了定位導航系統(tǒng)的精度與魯棒性，同時也為室內(nèi)環(huán)境下組合導航提供了一種可行性方案。

[1] LI C H, ZHEN J, CHANG K L, et al. An indoor positioning and tracking algorithm based on angle-of-arrival using a dual-channel array antenna[J]. Remote Sensing, 2021, 13(21): 4301.

[2] 陳映秋, 曠儉, 牛小驥, 等. 基于車輪安裝慣性測量單元的車載組合導航[J]. 中國慣性技術(shù)學報, 2018, 26(6): 799-804.

[3] BORENSTEIN J, FENG L Q. Measurement and cor-rection of systematic odometry errors in mobile robots [J]. IEEE Transactions on Robotics and Automation, 1996, 12(6): 869-880.

[4] 嚴恭敏, 鄧瑀. 傳統(tǒng)組合導航中的實用 Kalman 濾波技術(shù)評述[J]. 導航定位與授時, 2020, 7(2): 50-64.

[5] 李燦, 沈強, 秦偉偉, 等. 基于平滑變結(jié)構(gòu)-卡爾曼濾波的MIMU/BDS組合導航技術(shù)[J]. 航空兵器, 2021, 28(3): 51-58.

[6] 李文華, 汪立新, 沈強, 等. 基于魯棒EKF的MEMS-INS/GNSS/VO組合導航方法[J/OL]. 系統(tǒng)工程與電子技術(shù). (2022-01-18)[2022-01-20]. https: //kns. cnki. net/kcms/detail/detail. aspx?dbcode=CAPJ&dbname=CAPJLAST& filename=XTYD20220115007&uniplatform=NZKPT&v=zJsPjaDuhsTEorkxMPIyJ83Z1H65HMU63LkwB1jL8CvgZtbTzpDpVXbNdh6sBIV0.

[7] 隋心, 高嵩, 黃俊堯, 等. UWB輔助INS的室內(nèi)組合方法[J]. 無線電工, 2020, 50(8): 624-630.

[8] 黃俊杰, 李海濱, 賈翠玲. 基于卡爾曼濾波的UWB與里程計融合定位方法[J/OL]. 機床與液壓. (2021-06-24) [2022-01-20]. https://kns.cnki.net/kcms/detail/detail.aspx?dbcode=CAPJ&dbname=CAPJLAST&filename=JCYY 20210624000&uniplatform=NZKPT&v=A85CcwTx4DoXWb0MIa7EK6TMK8MRfEEB8SDbgGJBL3mXqeMN-RMDrhdOE9JrldGX.

[9] 甄杰, 吳建新, 劉紀平, 等. 一種單基站高精度室內(nèi)應急定位方法[J]. 武漢大學學報(信息科學版), 2020, 45(8): 1146-1154.

[10] CHOU J. Quaternion kinematic and dynamic differential equations[J]. IEEE Transactions on Robotics and Automation, 1992, 8(1): 53-64.

[11] 周文鵬, 路林, 王建明. 多傳感器信息融合在無人駕駛中的研究綜述[J]. 汽車文摘, 2022(1): 45-51.

[12] CHANG G B. On Kalman filter for linear system with colored measurement noise[J]. Journal of Geodesy, 2014, 88(12): 1163-1170.

[13] MARTINELLI A. Vision and IMU data fusion: closed-form solutions for attitude, speed, absolute scale, and bias determination [J]. IEEE Transactions on Robotics, 2012, 28(1): 44-60.

Indoor integrated positioning method based on bluetooth array/IMU/odometer

CHANG Kanglong1,2, ZHEN Jie1, LI Chenhui1,2, YANG Wendong1

（1. Chinese Academy of Surveying and Mapping, Beijing 100036, China；2. School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China）

In the field of indoor positioning,common sensors such as inertial measurement units (IMU) and odometers have cumulative errors and cannot use the global navigation satellite system (GNSS) to provide position calibration services, while array antenna positioning results based on bluetooth signals are easily affected by non-line-of-sight (NLOS) errors. In view of the above problems,In this paper, an extended Kalman filter (EKF) model based on the combination of bluetooth array/IMU/odometer is proposed. According to the error estimation model of inertial navigation,the velocity observation and two-dimensional position observation information are constructed by using the motion characteristics of odometer and the principle of angle measurement of single base station array antenna,so as to improve the performance of integrated navigation.The actual measurement results based on the wheeled mobile measurement platform show that the combined method can effectively reduce the error accumulation of IMU and odometer and the influence of the NLOS error of the bluetooth array, The overall motion trajectory is more consistent with the real trajectory, and can provide accurate and rich pose information for the motion carrier.

integrated navigation; extended Kalman filter; inertial navigation; odometer; bluetooth array

P228

A

2095-4999(2022)06-0088-09

常康龍，甄杰，李晨輝，等. 藍牙陣列/IMU/里程計室內(nèi)組合定位方法[J]. 導航定位學報, 2022, 10(6): 88-96.（CHANG Kanglong, ZHEN Jie, LI Chenhui, et al. Indoor integrated positioning method based on bluetooth array/IMU/odometer[J]. Journal of Navigation and Positioning, 2022, 10(6): 88-96.）

10.16547/j.cnki.10-1096.20220612.

2022-01-28

國家重點研發(fā)計劃項目（2016YFB0502201）。

?？谍垼?997—），男，河南商丘人，碩士研究生，研究方向為室內(nèi)定位。