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

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

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

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

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

組合導(dǎo)航；擴(kuò)展卡爾曼濾波；慣性導(dǎo)航；里程計；藍(lán)牙陣列

0 引言

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

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

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

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

1 組合定位模型

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

1.1 狀態(tài)方程

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

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

1.2 觀測方程

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

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

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

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

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

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

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

1.3 融合整體框架

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

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

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

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

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

圖2 實驗設(shè)備

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

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

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

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

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

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

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

圖4 路徑1藍(lán)牙陣列定位軌跡

圖5 路徑2藍(lán)牙陣列定位軌跡

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

圖6 藍(lán)牙陣列/IMU/里程計組合實驗路徑1

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

圖8 藍(lán)牙陣列/IMU/里程計組合實驗路徑2

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

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

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

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

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

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

從以上實驗結(jié)果可以看出，藍(lán)牙陣列/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。

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

3 結(jié)束語

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

[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] 陳映秋, 曠儉, 牛小驥, 等. 基于車輪安裝慣性測量單元的車載組合導(dǎo)航[J]. 中國慣性技術(shù)學(xué)報, 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] 嚴(yán)恭敏, 鄧瑀. 傳統(tǒng)組合導(dǎo)航中的實用 Kalman 濾波技術(shù)評述[J]. 導(dǎo)航定位與授時, 2020, 7(2): 50-64.

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

[6] 李文華, 汪立新, 沈強(qiáng), 等. 基于魯棒EKF的MEMS-INS/GNSS/VO組合導(dǎo)航方法[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]. 機(jī)床與液壓. (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] 甄杰, 吳建新, 劉紀(jì)平, 等. 一種單基站高精度室內(nèi)應(yīng)急定位方法[J]. 武漢大學(xué)學(xué)報(信息科學(xué)版), 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

?？谍垼缃?，李晨輝，等. 藍(lán)牙陣列/IMU/里程計室內(nèi)組合定位方法[J]. 導(dǎo)航定位學(xué)報, 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

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

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