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

藍牙陣列/IMU/里程計室內組合定位方法

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

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

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

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

0 引言

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

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

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

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

1 組合定位模型

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

1.1 狀態(tài)方程

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

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

1.2 觀測方程

考慮速度誤差時的速度轉換方程可以表示為

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

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

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

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

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

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

1.3 融合整體框架

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

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

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

2 實驗與結果分析

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

圖2 實驗設備

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

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

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

表1 Jackal小型移動平臺底盤技術指標

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

從以上實驗結果可以看出，藍牙陣列/IMU/里程計組合結果整體精度較高，保持在0.5 m以內，其中：路徑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。

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

3 結束語

本文主要研究多傳感器定位方法，設計了藍牙陣列/IMU/里程計的組合方法，構建IMU誤差模型，同時提取藍牙陣列天線和里程計數據作為觀測信息。實測數據結果表明，本文提出的組合方法能利用這3個傳感器各自優(yōu)點，且基于誤差狀態(tài)的EKF濾波結果有著較好的實用性，既可以削弱藍牙信號受NLOS影響的定位偏差，同時也能消除IMU、里程計存在的誤差累計問題，有效地將相對定位與絕對定位方式相結合，提升了定位導航系統的精度與魯棒性，同時也為室內環(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]. 中國慣性技術學報, 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] 嚴恭敏, 鄧瑀. 傳統組合導航中的實用 Kalman 濾波技術評述[J]. 導航定位與授時, 2020, 7(2): 50-64.

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

[6] 李文華, 汪立新, 沈強, 等. 基于魯棒EKF的MEMS-INS/GNSS/VO組合導航方法[J/OL]. 系統工程與電子技術. (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的室內組合方法[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] 甄杰, 吳建新, 劉紀平, 等. 一種單基站高精度室內應急定位方法[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/里程計室內組合定位方法[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—），男，河南商丘人，碩士研究生，研究方向為室內定位。