一種基于多傳感信息融合的定位算法研究

梁高麗 鄧仕軍 雷浩

摘? 要：隨著社會的不斷發(fā)展，人們對室內(nèi)定位精度的要求越來越高。針對單傳感信息定位精度不高、抗干擾能力不強(qiáng)、算法復(fù)雜度較大等問題，提出一種融合多傳感信息的定位算法。該算法通過對RFID標(biāo)簽徑向速度與移動目標(biāo)徑向速度的匹配，快速識別并定位移動目標(biāo)。已在服務(wù)機(jī)器人身上針對該算法展開實驗測試，實驗結(jié)果表明，該算法能夠快速準(zhǔn)確地定位到動態(tài)目標(biāo)。

關(guān)鍵詞：多傳感信息；信息融合；徑向速度匹配

中圖分類號：TP301.6? 文獻(xiàn)標(biāo)識碼：A? ? 文章編號：2096-4706（2023）08-0033-04

Abstract： With the continuous development of society， people require higher and higher accuracy of indoor positioning. Aiming at the problems of low positioning accuracy of single-sensor information， weak anti-interference ability， and large algorithm complexity， this paper proposes a positioning algorithm that integrates multi-sensor information. The algorithm matches the radial velocity of the RFID tag with the radial velocity of the moving target to quickly identify and locate the moving target. The algorithm has been tested on the service robot， and the experimental results show that the algorithm can locate the dynamic target quickly and accurately.

Keywords： multi-sensor information; information fusion; radial velocity matching

0? 引? 言

隨著物聯(lián)網(wǎng)技術(shù)的飛速發(fā)展，人們對定位服務(wù)的要求越來越高。目前，室外環(huán)境主要利用全球定位系統(tǒng)（GPS）來進(jìn)行定位，該系統(tǒng)通過衛(wèi)星信號來對移動物體進(jìn)行定位，定位精度基本能滿足人們的需求[1-3]。由于建筑物的遮擋，衛(wèi)星信號被遮擋甚至阻斷，導(dǎo)致GPS不再適用于室內(nèi)環(huán)境。但是，人們長期處在室內(nèi)環(huán)境當(dāng)中，因此提升室內(nèi)移動物體的定位精度尤為重要[4]。室內(nèi)定位系統(tǒng)的精度不僅與傳感器采集數(shù)據(jù)的精確度有關(guān)，還與數(shù)據(jù)處理的算法息息相關(guān)。使用單個傳感器對移動物體進(jìn)行定位，容易產(chǎn)生較大的定位誤差，因此許多研究者融合多傳感器信息來對移動物體進(jìn)行定位。趙妍等人提出了一種基于同步定位與地圖創(chuàng)建技術(shù)（SLAM）的智能激光定位技術(shù)。該技術(shù)采用最短路徑優(yōu)化算法對機(jī)器人進(jìn)行路徑規(guī)劃；采用SLAM算法構(gòu)建避障規(guī)則；利用激光傳感技術(shù)來跟蹤機(jī)器人的移動路徑[5]。劉琴等人提出一種多傳感器數(shù)據(jù)融合的定位系統(tǒng)，該系統(tǒng)融合GPS傳感數(shù)據(jù)與慣性導(dǎo)航單元數(shù)據(jù)來定位動態(tài)目標(biāo)[6]。王銘藝等人為了降低單一傳感器的定位誤差，提出了一種融合激光傳感器與雙目視覺的定位方法，該方法能精確地定位移動機(jī)器人的位置[7]。

為了進(jìn)一步提高定位精度，許多研究開始對融合算法進(jìn)行研究。劉金等人利用BP神經(jīng)網(wǎng)絡(luò)來對卡爾曼濾波算法進(jìn)行優(yōu)化，并利用優(yōu)化后的算法來對多傳感器信息進(jìn)行融合，最后再對移動機(jī)器人進(jìn)行定位[8]。毛文勇等人同樣采用卡爾曼濾波算法融合激光信息與里程計信息，來對移動機(jī)器人進(jìn)行定位，該算法有效提高了機(jī)器人的定位精度[9]。Sun等人提出了多源信息融合算法，該算法對擴(kuò)展卡爾曼濾波器進(jìn)行擴(kuò)展，并且利用擴(kuò)展后的濾波器來融合超寬帶定位技術(shù)、里程計信息、陀螺加速計信息，實現(xiàn)了無軌送餐機(jī)器人的定位與導(dǎo)航[10]。Gu等人提出了基于多傳感器融合的運(yùn)動航向檢測算法，首先利用系統(tǒng)本身的信息來判別物體的運(yùn)動狀態(tài)，然后利用擴(kuò)展卡爾曼濾波算法，將加速度計的輸出信息作為觀測量，來更新測量值，進(jìn)而得到物體運(yùn)動方向[11]。

綜上所述，多傳感器具有互補(bǔ)特性，因此利用多傳感器信息來進(jìn)行定位能有效地提高定位精度，但算法的復(fù)雜度較高[12，13]。一個好的定位系統(tǒng)不僅需要提高定位精度，還需要降低算法復(fù)雜度。由于RFID能對物體進(jìn)行識別，但定位物體需要復(fù)雜的算法；激光掃描儀能對物體的位置進(jìn)行精確估算，但識別物體也許復(fù)雜的算法。為了提高傳感器的定位精度，同時降低算法復(fù)雜度，本文提出一種融合RFID信息與激光信息的定位算法。

1? 系統(tǒng)描述

該定位系統(tǒng)由RFID系統(tǒng)與二維激光掃描儀構(gòu)成，系統(tǒng)框圖如圖1所示。RFID系統(tǒng)的讀寫器可以采集標(biāo)簽的相位信息以及標(biāo)簽的ID信息。信號強(qiáng)度信息可以對移動目標(biāo)進(jìn)行定位，但是該信息受環(huán)境影響較大，導(dǎo)致定位精度低；相位信息可以對移動目標(biāo)的徑向速度進(jìn)行估算；標(biāo)簽的ID信息可以對移動目標(biāo)進(jìn)行識別。二維激光掃描儀可以采集物體的位置信息，但是需要復(fù)雜的算法才能對物體進(jìn)行識別。因此該系統(tǒng)通過標(biāo)簽的ID信息對定位進(jìn)行識別，并提取定位目標(biāo)的相位信息，通過相位信息來估算標(biāo)簽的徑向速度，然后找出與該徑向速度最相似的激光簇的徑向速度，最后獲取該激光簇的中心坐標(biāo)，即為定位目標(biāo)的位置。

2? 信息融合算法

2.1? 激光聚類

二維激光掃描儀能向周圍環(huán)境發(fā)射激光波束，可通過激光波束的發(fā)射時間與接收時間來估算物體與激光掃描儀之間的距離[14]。本文采用SICK公司，型號為S30B-2011BA的二維激光傳感器。該傳感器的掃描角度為270°；最遠(yuǎn)掃描距離為29.9 m；角度分辨率為0.5°。使用該掃描儀可向周圍-45°至225°的范圍內(nèi)發(fā)射激光波束。當(dāng)這些波束打在某物體上面，該物體的極坐標(biāo)信息將被返回至激光傳感器。因此通過該激光掃描儀采集的物體位置信息是以極坐標(biāo)形式存在，如：（rn， φn），其中rn為第n束激光的長度信息；φn為第n束激光與激光正前方形成的夾角信息。該極坐標(biāo)可通過下式轉(zhuǎn)換為笛卡爾坐標(biāo)形式：

其中，（xn， yn）為第n束激光形成的激光點，xn為該點距掃描儀垂直方向的距離；yn為該點距掃描儀水平方向的距離。通過轉(zhuǎn)換之后，采集的物體信息變?yōu)槎S坐標(biāo)下許多離散的點。這些離散的點用pk（1，2，3，…，k）表示，兩點之間的歐氏距離用d（ pn， pm）表示為：

由于打在同一物體上的激光不止一束，因此這些離散的激光點，不利于對物體進(jìn)行區(qū)分。為了準(zhǔn)確定位目標(biāo)對象，本文將這些離散的激光點進(jìn)行聚類處理，聚類之后的每一個激光簇表示一個物體。聚類的整個過程為：首先對原始數(shù)據(jù)進(jìn)行分組，然后將組分割為簇，最后，合并過濾具有大半徑的簇，即可得到聚類結(jié)果。

2.1.1? 分組處理

通過歐式距離來對掃描點進(jìn)行分組，如果相鄰兩點之間的歐式距離滿足式（3），則分為一組：

d（ pi， pi-1）＜dg+Ridp? ? ? ? ? ? ? ? ? ? ? ? ? ?（3）

其中，dg為分組閾值，Ri為第i個掃描點與激光掃描儀之間的距離，dp為距離參數(shù)。如果pi滿足分組條件則被分進(jìn)Sj當(dāng)中，如果不滿足分組條件，則創(chuàng)建一個新的分組Sj+1。當(dāng)所有激光點都被分組之后，得到分組集合 ，Ng為分組數(shù)目。

2.1.2? 分組處理

在分組處理當(dāng)中，只關(guān)注了相鄰兩點之間的直線距離，不同物體可能被分為同一組。為了提高聚類結(jié)果的準(zhǔn)確性，本文采用迭代擬合算法對每個分組再進(jìn)行分割。首先在分組Sj中連接距離最遠(yuǎn)的兩個掃描點，然后搜索離連接線最遠(yuǎn)的點，最后求得該點與連接線之間的距離 ?di。若 ?di滿足分割標(biāo)準(zhǔn)，則在該點處對Sj進(jìn)行分割。分割標(biāo)準(zhǔn)為：

?di＜ds+Rjdp? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（4）

其中，ds為分割閾值，Rj為連接線的長度。若 ?di滿足分割條件，Sj在該點處被分割為兩組。然后遞歸循環(huán)分割每一個新的分組，直到不能再分割為止。這時所得到的分組可以用圓來表示，該圓的圓心為連接線的中點，直徑為連接線的長度。因此t時刻的聚類結(jié)果為：

其中? 為t時刻第i個簇的半徑， 為t時刻第i個簇的中心點，Nt為t時刻簇的個數(shù)。

2.1.3? 合并過濾

分割結(jié)束后，若兩個簇之間是包含或相交的關(guān)系，則認(rèn)為這兩個簇為同一個物體。因此需要對這兩個簇進(jìn)行合并，當(dāng)圓? 與圓? 滿足式（6），說明兩圓相交，將這兩圓合并為一個簇：

本文需要識別的動態(tài)目標(biāo)為人體，因此可對半徑較大的圓進(jìn)行過濾。當(dāng)某個圓的半徑 （rm為物體半徑閾值），將該圓過濾。合并過濾結(jié)束之后，原始掃描點已經(jīng)被分成許多不同的簇，每一個簇表示一個物體。

2.1.4? 激光傳感信息估算徑向速度

同一物體的徑向移動速度，可通過相鄰時刻簇的移動情況來進(jìn)行估算。對于t時刻激光聚類完成之后得到的簇 ，可尋找在t-1時刻與其最相近的簇 ，這兩個簇可被認(rèn)為是同一物體。 為：

2.2? 相位信息估算徑向速度

RFID閱讀器通過發(fā)射無線電波激活標(biāo)簽中的電路，當(dāng)標(biāo)簽收獲足夠的能量后，反向散射無線電信號。標(biāo)簽的旋轉(zhuǎn)相位為：

其中λ為波長，dt為RFID標(biāo)簽與天線之間的距離。φT為讀寫器傳輸電路的相位旋轉(zhuǎn)；φR為閱讀器接收回路的相位旋轉(zhuǎn)；φTag為標(biāo)簽的反射特征的相位旋轉(zhuǎn)。通過相鄰時刻同一標(biāo)簽的相位差可以估算標(biāo)簽的徑速度，假設(shè)讀寫器在t-1與t時刻獲得的相位分別為θt-1與θt。標(biāo)簽的徑向速度為：

2.3? 速度匹配

同一物體通過不同傳感器估算的徑向速度應(yīng)該極為相似。因此在t時刻尋找與相位差估算的速度? 最接近的徑向速度 ， 所對應(yīng)的簇? 的中心位置? 可認(rèn)為是定位目標(biāo)的位置：

3? 實驗驗證

本文將二維激光掃描儀與RFID系統(tǒng)安裝在機(jī)器人身上，并將機(jī)器人固定在室內(nèi)環(huán)境某一位置處，佩戴RFID標(biāo)簽的行人作為動態(tài)定位目標(biāo)在機(jī)器人前方移動，移動軌跡為一個4×2 m的矩形，實驗環(huán)如圖2所示。

本定位算法當(dāng)中涉及的參數(shù)有：分組參數(shù)dg，距離參數(shù)dp，分割參數(shù)ds，半徑閾值rm，這些參數(shù)對激光聚類的結(jié)果存在一定影響，從而影響系統(tǒng)的定位精度。圖3給出不同ds與dg的誤差圖，由表1可以看出，激光分組參數(shù)與分割參數(shù)，對定位誤差存在較大的影響，當(dāng)分組參數(shù)過大，同一組數(shù)據(jù)可能不止包含一個物體；當(dāng)分組參數(shù)過小，同一物體可能被分為兩組數(shù)據(jù)，這兩種情況都會導(dǎo)致激光簇不準(zhǔn)確，從而影響定位精度。當(dāng)分割參數(shù)過小時，同一物體被分割為兩個組；當(dāng)分割參數(shù)過大時，不能將不同物體分割成兩組，從而影響定位精度。當(dāng)dg=0.2，ds=0.2，激光聚類結(jié)果較佳，定位精度較為理想。

表2給出了不同dp與rm對定位精度的影響，由表2可以看出，激光測距誤差的修補(bǔ)參數(shù)dp過小時，激光測距誤差不能得到較好的補(bǔ)償，導(dǎo)致定位精度較低；當(dāng)dp過大時，激光測距誤差補(bǔ)償過度，也會導(dǎo)致定位精度較差。最大半徑rm過小時，將目標(biāo)圓排除在外；rm過大時，不能很好地過濾掉無效圓，也會導(dǎo)致系統(tǒng)誤差增加。當(dāng)rm=0.4，dp=0.1時，系統(tǒng)定位效果較佳。

將該算法的各參數(shù)設(shè)置如下：dg=0.2、ds=0.2、rm=0.4、dp=0.1來對目標(biāo)進(jìn)行定位。圖3給出了該算法的定位圖，由圖可知，定位點分布在目標(biāo)移動的真實軌跡附近，少數(shù)定位點離真實軌跡較遠(yuǎn)，其原因是激光簇估算徑向速度產(chǎn)生錯誤，導(dǎo)致系統(tǒng)定位到錯誤的移動物體，從而使定位點遠(yuǎn)離真實軌跡。當(dāng)系統(tǒng)無法找到與標(biāo)簽徑向速度較為接近的激光簇時，可根據(jù)移動目標(biāo)前一時刻的位置，與當(dāng)前時刻的位置來估計移動物體的移動路徑，從而實現(xiàn)移動目標(biāo)準(zhǔn)確快速的定位。

4? 結(jié)? 論

本文提出了一種融合多傳感信息的定位算法，其通過相位信息與二維激光掃描信息來對室內(nèi)動態(tài)目標(biāo)進(jìn)行定位。實驗表明，該系統(tǒng)不需要復(fù)雜的分類算法，能快速識別目標(biāo)，并進(jìn)行定位。但當(dāng)激光簇的速度與標(biāo)簽徑向速度相差較大時，系統(tǒng)獲得的定位誤差也較大，因此后期需要加入視覺信息或信號強(qiáng)度信息來降低系統(tǒng)定位誤差。

參考文獻(xiàn)：

[1] 張勝軍，林若琳.淺談室內(nèi)定位技術(shù)現(xiàn)狀 [J].測繪與空間地理信息，2018，41（7）：128-131.

[2] LIU X L，XIAO B，LI K Q，et al. RFID Estimation With Blocker Tags [J].IEEE/ACM Transactions on Networking，2017，25（1）：224-237.

[3] HE S N，CHAN S H G. Wi-Fi Fingerprint-Based Indoor Positioning：Recent Advances and Comparisons [J].IEEE Communications Surveys & Tutorials，2016，18（1）：466-490.

[4] 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.

[5] 趙妍，解迎剛，陳莉莉.基于SLAM算法的機(jī)器人智能激光定位技術(shù)的研究 [J].激光雜志，2019，40（7）：169-173.

[6] 劉琴，明振，李金波.多傳感器數(shù)據(jù)融合定位方法及其安防機(jī)器人：CN109612462A [P].2019-04-12.

[7] 王銘藝，賀利樂，李育，等.多傳感器信息融合的移動機(jī)器人定位算法研究 [J].測試科學(xué)與儀器：英文版，2020，13（4）：152-160.

[8] 劉金.基于傳感器融合的室內(nèi)定位技術(shù)研究 [D].北京：北京郵電大學(xué)，2019.

[9] 毛文勇，張文安，仇翔.基于多傳感器融合的機(jī)器人軌跡跟蹤控制 [J].控制工程，2020，27（7）：1125-1130.

[10] SUN Y L，GUAN L W，CHANG Z Y，et al. Design of a Low-Cost Indoor Navigation System for Food Delivery Robot Based on Multi-Sensor Information Fusion [J/OL].Sensors（Basel，Switzerland），2019，19（22）：[2022-10-28].https：//www.mdpi.com/1424-8220/19/22/4980.

[11] GU H Y，JIN C C，YUAN H Y，et al. Design and Implementation of Attitude and Heading Reference System with Extended Kalman Filter Based on MEMS Multi-Sensor Fusion [J].International Journal of Uncertainty，F(xiàn)uzziness and Knowledge-Based Systems，2021，29（Supp01）：157-180.

[12] 吳佳怡，馮鋒.基于多傳感信息融合的倉儲定位追蹤系統(tǒng)研究 [J].計算機(jī)技術(shù)與發(fā)展，2019，29（6）：134-137.

[13] 周亞瓊.一種基于多傳感器信息融合定位技術(shù)的機(jī)器人：CN110370245A [P].2022-08-12.

[14] 王廣奇.基于RFID和激光傳感器的語義地圖更新及機(jī)器人定位方法研究 [D].濟(jì)南：山東大學(xué)，2013.

作者簡介：梁高麗（1992—），女，漢族，四川成都人，助教，碩士，研究方向：機(jī)器人定位與導(dǎo)航；鄧仕軍（1989—），男，漢族，四川成都人，工程師，學(xué)士，研究方向：室內(nèi)定位技術(shù)。