融合Wi-Fi與激光的機器人室內(nèi)大型環(huán)境SLAM

摘 要：同步定位與地圖構(gòu)建（SLAM）是實現(xiàn)移動機器人自主導(dǎo)航定位的關(guān)鍵。針對室內(nèi)大型環(huán)境下激光SLAM閉環(huán)檢測容易產(chǎn)生錯誤閉環(huán)，導(dǎo)致機器人位姿估計誤差較大的問題，提出了一種融合Wi-Fi與激光信息的圖優(yōu)化SLAM算法。首先，構(gòu)建Wi-Fi指紋序列與激光子地圖；然后，根據(jù)每對指紋序列的相似度均值和標準差篩選用于閉環(huán)檢測的激光子地圖。在此基礎(chǔ)上，提取所篩選子地圖的特征點并匹配，以確定激光閉環(huán)；最后，通過圖優(yōu)化方法融合里程計與激光閉環(huán)，優(yōu)化機器人的軌跡并構(gòu)建全局地圖。在170 m×30 m和180 m×80 m的室內(nèi)環(huán)境中采集了三組數(shù)據(jù)，對所提算法性能進行驗證。實驗結(jié)果顯示，所提算法的定位精度在三組數(shù)據(jù)上分別達到0.78 m、0.67 m和0.89 m，與激光SLAM算法相比分別提升了48.6%、53.1%和68.7%，證明所提算法有效提高了室內(nèi)大型環(huán)境下激光SLAM的位姿估計精度。

關(guān)鍵詞：Wi-Fi指紋序列；激光子地圖篩選；閉環(huán)檢測；圖優(yōu)化；同步定位與地圖構(gòu)建

中圖分類號：TP242.6"" 文獻標志碼：A"" 文章編號：1001-3695（2025）03-022-0812-06

doi：10.19734/j.issn.1001-3695.2024.06.0244

Large-scale indoor environment SLAM for mobile robot based on Wi-Fi and LiDAR

Xiong Zhuanga，b，Liu Rana，b，Guo Lina，b，Xiao Yufenga，b

（a.School of Information Engineering，b.Robot Technology Used for Special Environment Key Laboratory of Sichuan Province，Southwest University of Science amp; Technology，Mianyang Sichuan 621000，China）

Abstract：SLAM is crucial for the autonomous navigation and positioning of robots.Aiming at the problem of considerable pose estimation errors for robots，which were caused by incorrect loop closures in LiDAR SLAM within large-scale indoor environments，this paper proposed a graph-based SLAM algorithm that fused Wi-Fi and LiDAR information.Initially，the algorithm constructed Wi-Fi fingerprint sequences and LiDAR submaps.Subsequently，it selected LiDAR submaps for loop closure detection based on the mean and standard deviation of similarity between each pair of fingerprint sequences.Then，it extracted feature points from the selected submaps and matched them to confirm LiDAR loop closure.Ultimately，using a graph optimization approach，odometry and LiDAR loop closures were fused to optimize the robot’s trajectory and construct a global map.Three datasets were collected in 170 m×30 m and 180 m×80 m indoor environments to verify the performance of the proposed algorithm.The experimental results show that positioning accuracy values of proposed algorithm in three datasets reach 0.78 m，0.67 m，and 0.89 m，which give improvements of 48.6%，53.1%，and 68.7% when compared to the LiDAR SLAM algorithm，demonstrating its effectiveness for enhancing pose estimation accuracy in large-scale indoor environments.

Key words：Wi-Fi fingerprint sequence;LiDAR submap selection;loop closure detection;graph optimization;simulta-neous localization and mapping（SLAM）

0 引言

隨著機器人技術(shù)的不斷發(fā)展，機器人在日常生活中的應(yīng)用越來越普遍，例如商場服務(wù)［1］、安全巡檢［2］、物流倉儲［3］和危險環(huán)境搜救［4］等，這些應(yīng)用場景對機器人在室內(nèi)大型環(huán)境中的定位與導(dǎo)航提出了更高的要求。SLAM技術(shù)［5］能夠通過機器人自身的傳感器實時感知周圍環(huán)境，并同時構(gòu)建環(huán)境地圖和進行自我定位，因此成為了解決室內(nèi)環(huán)境導(dǎo)航定位問題的主流方案之一。SLAM大體上可以采用濾波和圖優(yōu)化兩種方法來實現(xiàn)?；跒V波的SLAM［6］通常使用遞歸貝葉斯濾波器來維護機器人和地圖的狀態(tài)估計，數(shù)據(jù)處理速度較快，適用于實時性要求較高的場景。然而，在大型室內(nèi)環(huán)境中，這種方法容易受到累積誤差的影響?；趫D優(yōu)化的SLAM［7］將采集數(shù)據(jù)構(gòu)成一個由頂點和約束邊組成的姿態(tài)圖，并建立誤差函數(shù)，通過最小化誤差函數(shù)來優(yōu)化機器人的軌跡，從而有效解決大型室內(nèi)環(huán)境SLAM問題。閉環(huán)檢測［8］是圖優(yōu)化SLAM的核心環(huán)節(jié)，正確可靠的閉環(huán)檢測能夠修正機器人里程計的漂移，使得地圖保持全局一致性。

在實際應(yīng)用中，通常使用不同類型的傳感器來實現(xiàn)SLAM，包括慣性測量單元（inertial measurement unit，IMU）、里程計、超寬帶（ultra-wideband，UWB）、視覺傳感器、激光雷達和Wi-Fi。IMU和里程計適用于測量機器人短時間內(nèi)的位姿變化，但長時間使用會產(chǎn)生不可避免的累積誤差［9］。UWB雖然可以獲得較高的定位精度，但其硬件成本較高，部署難度較大［10］?；谝曈X的方法通過匹配視覺特征來估計機器人的姿態(tài)，但視覺傳感器對環(huán)境光照條件敏感，在光線不足或過強的環(huán)境中性能較差［11］。

相較于視覺傳感器，激光雷達具有更高的測量精度，同時不受環(huán)境光照變化的影響。Gmapping［12］、Cartographer［13］是比較流行的激光SLAM解決方案，Gmapping算法使用粒子濾波作為一種自適應(yīng)策略來更新機器人位姿，但是在特征匱乏的環(huán)境下（長直走廊）可靠性較低。Cartographer算法融合了激光雷達與IMU的信息，通過圖優(yōu)化SLAM來消除累積誤差，但計算資源占用相對較高。Jiang等人［14］提出了一種激光雷達點云平面算法，實現(xiàn)了激光點云之間相對位姿的準確估計，但在點云數(shù)據(jù)量過大時，算法時間開銷明顯增加。Cattaneo等人［15］提出一種深度學習網(wǎng)絡(luò)LCDNet，實現(xiàn)了高效魯棒的位姿估計，然而該網(wǎng)絡(luò)需要大量的樣本數(shù)據(jù)進行訓(xùn)練優(yōu)化，構(gòu)建難度與成本高。在大型室內(nèi)環(huán)境中，因為復(fù)雜的環(huán)境特征容易產(chǎn)生錯誤的閉環(huán)信息，導(dǎo)致位姿估計出現(xiàn)較大誤差［16］，激光SLAM效果不佳。

近些年來，以Wi-Fi為代表的無線傳感器因其成本較低、覆蓋廣等優(yōu)勢而受到關(guān)注，現(xiàn)有的Wi-Fi網(wǎng)絡(luò)基礎(chǔ)設(shè)施可用于實現(xiàn)低成本的室內(nèi)定位［17］。通過Wi-Fi接入點（access point，AP）的信號強度（receive signal strength，RSS），可以大致識別出機器人是否訪問過某個區(qū)域［18］，因為每個AP的硬件地址是唯一的?；赪i-Fi指紋的定位方法依賴于指紋地圖，然而在大型室內(nèi)環(huán)境中，人工創(chuàng)建指紋地圖非常繁瑣且成本高昂［19］。Huang等人［20］提出了一種基于信號強度與位置模型的Wi-Fi SLAM，但由于Wi-Fi信號的不穩(wěn)定性，該模型的構(gòu)建難度大且魯棒性差?？傮w而言，由于Wi-Fi信號在室內(nèi)環(huán)境中易受到阻擋和干擾，基于Wi-Fi的室內(nèi)定位方法的定位精度通常不高［21］。

考慮到每種傳感器的局限性，研究人員設(shè)計了基于多傳感器的SLAM方法，通過處理不同傳感器的數(shù)據(jù)來減少單一傳感器測量誤差帶來的不確定性，從而構(gòu)建全局一致的環(huán)境地圖［22］。Liu等人［23］提出了一種將IMU與UWB測距信息結(jié)合的系統(tǒng)，用于未知環(huán)境中多個用戶之間的相對定位。然而，IMU產(chǎn)生的累計誤差導(dǎo)致該系統(tǒng)定位精度不足。Zhang等人［24］將激光雷達與IMU進行融合，利用IMU消除了激光雷達的運動畸變，修正了機器人姿態(tài)估計，但該方法容易受到IMU測量噪聲的影響。Ismail等人［25］提出了一種融合激光與Wi-Fi的兩層圖優(yōu)化SLAM算法，實現(xiàn)了機器人位姿的準確估計。然而由于觀測約束過多，該算法的計算效率較低。Stathoulopoulos等人［26］提出了一種融合激光雷達和Wi-Fi傳感器的冗余系統(tǒng)，利用兩種傳感器的互補優(yōu)勢預(yù)測定位結(jié)果，為地下礦井定位提供了有效的解決方案。然而，Wi-Fi信號受到環(huán)境多徑效應(yīng)的干擾，導(dǎo)致該系統(tǒng)產(chǎn)生了較大定位誤差。

在室內(nèi)大型環(huán)境中，由于閉環(huán)檢測的挑戰(zhàn)，機器人使用激光SLAM進行定位的精度較差。現(xiàn)有Wi-Fi設(shè)備在室內(nèi)大型環(huán)境中分布廣泛且成本較低，可以快速識別機器人是否經(jīng)過某個區(qū)域。因此，為了提升室內(nèi)大型環(huán)境下激光SLAM的位姿估計精度，本文提出了一種融合Wi-Fi與激光信息的圖優(yōu)化SLAM算法。首先，算法在連續(xù)時間內(nèi)利用滑動窗口構(gòu)建Wi-Fi指紋序列和激光子地圖。之后，通過計算每對指紋序列的相似度均值和標準差，篩選出用于閉環(huán)檢測的激光子地圖。然后，提取篩選出的子地圖的特征點并匹配，以獲取激光閉環(huán)。最后，通過圖優(yōu)化方法融合里程計數(shù)據(jù)和激光閉環(huán)，糾正里程計漂移并構(gòu)建全局地圖。實驗結(jié)果表明，本文算法在三組數(shù)據(jù)上的定位精度分別達到0.78 m、0.67 m以及0.89 m，與激光SLAM算法相比，分別提高了48.6%、53.1%和68.7%。

1 算法設(shè)計

本文提出的融合Wi-Fi與激光信息的圖優(yōu)化SLAM算法主要包括基于Wi-Fi指紋序列的激光子地圖篩選和融合里程計與激光閉環(huán)的圖優(yōu)化兩個部分。首先，機器人搭載傳感器分別采集里程計數(shù)據(jù)、激光數(shù)據(jù)與Wi-Fi數(shù)據(jù)。通過設(shè)置滑動窗口，在連續(xù)時間內(nèi)構(gòu)建Wi-Fi指紋序列。同時，對激光掃描進行匹配，并使用相同長度的滑動窗口構(gòu)建激光子地圖。然后，計算每對指紋序列中指紋點的相似度，生成相似度矩陣。根據(jù)相似度矩陣計算相似度的均值和標準差，并將它們與設(shè)定閾值比較，從而篩選出對應(yīng)的激光子地圖進行閉環(huán)檢測。最后，使用AKAZE算法提取和匹配所篩選子地圖的特征點，以獲取激光閉環(huán)信息。將激光閉環(huán)與里程計數(shù)據(jù)融合構(gòu)建姿態(tài)圖，通過圖優(yōu)化方法調(diào)整姿態(tài)圖，修正里程計誤差，并構(gòu)建全局地圖。整個算法流程如圖1所示。

1.1 基于Wi-Fi指紋序列的激光子地圖篩選

1.1.1 計算指紋序列相似度的均值和標準差

Wi-Fi指紋信息包含在實驗場景中采集到的來自AP的信號強度RSS，將t時刻在環(huán)境中位置xt采集到的Wi-Fi指紋表示為Ft={ft，xt}。其中，采集的指紋信息ft包含t時刻掃描到Z個AP的硬件地址以及對應(yīng)的信號強度ft={ft，1，ft，2，…，ft，Z}。為了估計兩個指紋點位置的接近程度，本文采用了如式（1）所示的余弦相似度方法。

S（F1，F(xiàn)2）=Scos（F1，F(xiàn)2）=∑Zz=1（f1，z·f2，z）∑Zz=1（f1，z）2·∑Zz=1（f2，z）2

（1）

指紋點F1與F2的余弦相似度在0～1，相似度為0表示兩個指紋完全不具備相似性，相似度為1表示兩個指紋完全相似。相似度值越大，表示指紋相似性越強，兩個指紋點在空間上的位置越接近。由于Wi-Fi信號易受環(huán)境多徑效應(yīng)的干擾，僅計算單對指紋點的相似度來判斷兩個指紋點位置接近程度的可靠性較低。為了解決這一問題，本文將單個指紋點擴展為指紋序列。指紋序列包含多個指紋點，其信息量相比單個指紋點更加豐富，能夠更全面地表達環(huán)境的特征。

連續(xù)時間內(nèi)，設(shè)置一個固定長度w的滑動窗口，將采集Wi-Fi指紋切割成指紋序列。假設(shè)當前構(gòu)建了兩個指紋序列L=l1，l2，…，lM和K=k1，k2，…，kN，首先對兩個序列中的指紋點進行遍歷配對，得到M×N對指紋組合。然后，根據(jù)式（1）計算所有指紋組合的余弦相似度，并構(gòu)造一個相似度矩陣S。S包含了M×N對指紋組合的相似度數(shù)據(jù)，如圖2所示。

基于構(gòu)建的相似度矩陣S，通過式（2）（3）計算相似度的均值與標準差。

μ=∑Mm=1∑Nn=1S（lm，kn）M×N

（2）

σ=∑Mm=1∑Nn=1（S（lm，kn）-μ）2M×N

（3）

1.1.2 篩選激光子地圖

指紋序列相似度的均值表示一對指紋序列中所有指紋點相似度的平均值，而標準差則衡量相似度數(shù)據(jù)分布的離散程度。當兩個指紋序列位于同一位置，指紋序列的相似度均值通常較高且標準差較低，表明機器人經(jīng)過了同一個場景。通過選擇合適的相似度閾值Sth和標準差閾值σth，可以篩選出位于同一位置的指紋序列。

L和K在同一位置μgt;Sth且σlt;σthL和K不在同一位置其他情況

（4）

如式（4）所示，當相似度均值μ大于設(shè)定的相似度閾值Sth，同時標準差σ小于標準差閾值σth時，可以識別出指紋序列L和K處在同一位置，從而判斷機器人訪問了同一個場景。如果L和K的相似度均值與標準差不滿足這兩個閾值條件，則識別出兩個指紋序列不在同一位置，判斷機器人沒有經(jīng)過同一個場景。

與此同時，算法利用關(guān)聯(lián)掃描匹配（correlative scan ma-tching，CSM）［27］方法實現(xiàn)激光掃描匹配。在概率框架下，CSM將每幀激光掃描與已知子地圖進行配準，其中的觀測概率指標用于衡量預(yù)測掃描結(jié)果與實際掃描結(jié)果的匹配程度。通過最大化觀測概率，可以計算出機器人在已知子地圖中的最佳位姿。

連續(xù)時間內(nèi)，算法同樣在固定長度w的滑動窗口內(nèi)構(gòu)建激光子地圖（由占用柵格地圖表示）。首先，采用Bresenham算法［28］，結(jié)合機器人位姿和采集的激光掃描數(shù)據(jù)，計算每個柵格的占用概率。然后，根據(jù)貝葉斯濾波器更新柵格的占用概率。根據(jù)不同占用概率，將柵格的占用狀態(tài)分為障礙物占用、障礙物未占用和未知三種。

通過計算指紋序列L和K的相似度均值和標準差，成功識別出它們處于同一位置時，算法會在候選子地圖中搜索與指紋序列L和K相對應(yīng)的子地圖，通過檢測并匹配這些子地圖的特征點來確定激光閉環(huán)。對于不滿足此條件的子地圖，則不會進行處理。這樣就能通過Wi-Fi信息有效篩選出用于閉環(huán)檢測的激光子地圖。

1.2 融合里程計與激光閉環(huán)的圖優(yōu)化

1.2.1 基于子地圖特征點匹配的激光閉環(huán)檢測

激光閉環(huán)檢測采用了特征點匹配算法，首先將子地圖轉(zhuǎn)換為灰度圖像，然后基于OpenCV中的AKAZE算法提取視覺特征點，最后對這些特征點進行匹配，計算機器人的相對位姿。如式（5）所示，AKAZE算法通過檢測圖像中像素點的Hessian矩陣是否包含局部最大值來確認特征點。該算法檢測特征點的速度快，檢測的特征點在旋轉(zhuǎn)、尺度和光照變化方面具有較強的魯棒性，并且在不同分辨率的圖像中表現(xiàn)出更強的顯著性。

LIHessian=σ2I，norm（LIxxLIyy-LIxyLIxy）

（5）

其中：σ2I，norm表示圖像的縮放尺度；LIxx表示圖像坐標系中像素點I在x方向的二階微分;LIyy表示圖像坐標系中像素點I在y方向的二階微分;LIxy是二階交叉微分。當LIHessian取得最大值時，表示像素點I是圖像中的一個特征點。提取到子地圖特征點后，應(yīng)用OpenCV中的AffineBestOf2NearestMatcher函數(shù)對特征點進行匹配，計算兩張子地圖圖像之間的仿射變換Timage和一致性置信度。置信度越高，表示特征點匹配越精準可靠，激光閉環(huán)可信度也越高，圖3展示了子地圖及其特征點提取和匹配的示例。由于將機器人的位姿設(shè)置為子地圖的原點，所以將仿射變換Timage與子地圖圖像的分辨率scale相乘，即可得到全局坐標系下機器人的相對位姿變換Trobot。

Trobot=scale×Timage

（6）

1.2.2 基于圖優(yōu)化的機器人軌跡優(yōu)化

圖優(yōu)化方法將SLAM表述為一個最大似然估計問題，構(gòu)建一個由頂點與邊組成的位姿圖來描述機器人與環(huán)境的關(guān)系。其中，頂點表示機器人位姿，邊則表示相鄰時刻的里程計記錄數(shù)據(jù)或是不相鄰時刻觀測信息推斷出的閉環(huán)約束。如式（7）所示，圖優(yōu)化的目的是調(diào)整頂點X*的值以更好地滿足里程計邊和激光閉環(huán)邊的約束。

arg minX∑ti=2（Zii-1-ii-1（Xi-1，Xi））TΩii-1（Zii-1-ii-1（Xi-1，Xi））里程計約束+

∑t（i，j）∈C（Zji-ji（Xi，Xj））T Ωji（Zji-ji（Xi，Xj））激光閉環(huán)約束

（7）

其中：Xi和Xj表示機器人在時刻i和j的位姿（包括二維位置和方向）；Zii-1和Zji是兩個頂點之間的實際觀測值，分別表示相鄰頂點（Xi-1，Xi）之間由里程計記錄的相對位姿變換、不相鄰頂點（Xi，Xj）之間由激光閉環(huán)產(chǎn)生的相對位姿變換。此外，ii-1（Xi-1，Xi）表示相鄰頂點（Xi-1，Xi）之間里程計的預(yù)測值，ji（Xi，Xj）是基于不相鄰頂點（Xi，Xj）之間激光子地圖特征點匹配產(chǎn)生的位姿變換預(yù)測值。Ωii-1表示里程計約束中的信息矩陣，Ωji表示了激光閉環(huán)約束中的信息矩陣，C表示激光閉環(huán)約束集合。圖優(yōu)化方法在相鄰頂點之間添加基于里程計的邊，在不相鄰頂點之間添加基于激光的邊作為約束條件，使用圖優(yōu)化（general graph optimization，g2o）［29］中的列文伯格-馬夸爾特算法作為求解器，最小化里程計約束與激光閉環(huán)約束的殘差，從而實現(xiàn)機器人軌跡的優(yōu)化。

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

2.1 實驗平臺與環(huán)境

為了驗證本文算法的性能，在校園內(nèi)一個占地面積約170 m×30 m的地下停車場進行實驗。機器人從實驗場景中的不同位置出發(fā)，以0.4 m/s的平均速度移動，采集了兩組數(shù)據(jù)。在這兩組數(shù)據(jù)中，機器人的累計移動距離分別為1 256.81 m和1 369.37 m。兩條運動路線差異顯著，一條經(jīng)過了包含大量停車位的中心區(qū)域，另一條集中在復(fù)雜的邊緣區(qū)域，例如停車場的入口、出口和車道等。為了增加實驗的復(fù)雜性和對算法的干擾因素，將實驗場景的面積從170 m×30 m擴展到180 m×80 m，使機器人的運動路線盡可能復(fù)雜化并覆蓋整個實驗場景，以模擬室內(nèi)大型環(huán)境中復(fù)雜多變的情況。與此同時，將機器人的累計運動距離增加到2 726.31 m，以測試算法在長時間下運行的穩(wěn)定性?；谶@些調(diào)整，采集了第三組數(shù)據(jù)。數(shù)據(jù)采集平臺如圖4所示。

實驗使用了Clearpath Husky UGV機器人，機器人通過底盤的編碼器采集里程計數(shù)據(jù)，數(shù)據(jù)采集頻率為10 Hz。機器人上裝備了型號為Hokuyo UST-20LX的激光雷達（雷達掃描頻率40 Hz、最大掃描距離20 m、測量角度范圍270°、角度分辨率0.25°）用于采集激光數(shù)據(jù)。機器人位姿真實值使用自適應(yīng)蒙特卡羅定位（adaptive Monte Carlo localization，AMCL）獲得。此外，機器人前端放置了5部小米手機，每部手機都以0.5 Hz的頻率采集Wi-Fi數(shù)據(jù)。本文采用平均定位精度作為算法性能的評估指標，即經(jīng)過SLAM算法優(yōu)化后的機器人軌跡與真實值之間的歐氏距離。

2.2 實驗分析

2.2.1 不同相似度閾值與標準差閾值對算法性能的影響

本文算法中，采用不同的相似度閾值Sth和標準差閾值σth會對算法性能產(chǎn)生一定的影響。首先設(shè)置標準差閾值σth為0.2，滑動窗口長度w為10 s，調(diào)節(jié)相似度閾值Sth進行了多組測試。測試結(jié)果見表1。

表1的實驗結(jié)果表明，當相似度閾值設(shè)置過高或過低時，算法的定位精度都會下降。相似度閾值設(shè)置為0.9時，算法在三組數(shù)據(jù)上的定位精度分別是8.95 m、7.72 m和11.36 m，定位精度最差。而當相似度閾值設(shè)置為0.1時，算法在三組數(shù)據(jù)上的定位精度分別是1.93 m、2.05 m和3.17 m，定位精度仍然較差。相似度閾值設(shè)為0.3時，算法在三組數(shù)據(jù)中的定位精度分別是0.96 m、0.87 m和1.34 m，定位精度最佳。

總體而言，相似度閾值對算法的激光閉環(huán)數(shù)與定位精度有顯著影響，高閾值將導(dǎo)致閉環(huán)數(shù)量減少，從而降低定位精度；而低閾值會產(chǎn)生大量閉環(huán)約束，雖然增加了閉環(huán)數(shù)量，但也引入了更高的不確定性，同樣導(dǎo)致算法定位精度下降。基于表1的結(jié)果，將相似度閾值Sth設(shè)置為0.3，滑動窗口長度w為10 s，調(diào)節(jié)標準差閾值σth以測試算法性能，實驗結(jié)果見表2。

表2結(jié)果顯示，在數(shù)據(jù)集1中，當標準差閾值設(shè)置為0.2時，算法的定位精度是0.96 m。當標準差閾值設(shè)置為0.4時，算法在數(shù)據(jù)集2和3的定位精度分別是0.83 m和1.21 m。與之相比，當標準差閾值設(shè)置為1.0時，算法在三組數(shù)據(jù)上的定位精度分別是1.47 m、1.4 m和2.06 m。這表明，隨著標準差閾值的增加，算法的定位精度逐漸降低。原因是較高的標準差閾值雖然能獲取更多的激光閉環(huán)，但是激光閉環(huán)數(shù)的增加可能導(dǎo)致算法接受不準確的閉環(huán)信息，從而降低定位精度。

2.2.2 不同子地圖篩選方法對算法性能的影響

本文算法中，基于Wi-Fi指紋序列的激光子地圖篩選方法對算法的激光閉環(huán)數(shù)和定位精度具有關(guān)鍵作用。為了評估該方法的效果，增加了一種基于Wi-Fi指紋序列相似度最大值的子地圖篩選方法。在圖2的相似度矩陣中，遍歷計算的相似度數(shù)據(jù)，選擇最大相似度Smax表示一對指紋序列的相似度。當Smax大于設(shè)定的相似度閾值Sth時，判斷機器人經(jīng)過同一場景，當Smax小于預(yù)設(shè)閾值Sth時，判斷機器人未經(jīng)過同一場景。然后，設(shè)計了相關(guān)實驗評估兩種子地圖篩選方法對算法性能的影響。

對于不同的數(shù)據(jù)集，最合適的閾值參數(shù)有所不同。根據(jù)表1、2的實驗結(jié)果，對于數(shù)據(jù)集1，相似度閾值Sth與標準差閾值σth分別設(shè)置為0.3和0.2；對于數(shù)據(jù)集2和3，相似度閾值Sth與標準差閾值σth分別設(shè)置為0.3和0.4。實驗中，通過調(diào)節(jié)滑動窗口長度w，測試了兩種子地圖篩選方法對算法定位精度以及獲取激光閉環(huán)數(shù)量的影響，結(jié)果記錄在表3中。

表3結(jié)果顯示，隨著滑動窗口長度w的增加，激光閉環(huán)的數(shù)量也逐漸增加，因為在更大的滑動窗口范圍內(nèi)，算法能獲取更多的激光閉環(huán)。然而，窗口長度的增大并不總是能提升定位精度。具體而言，最大相似度方法在三組數(shù)據(jù)中的最佳定位精度分別是0.86 m、0.84 m和1.43 m，而本文算法在三組數(shù)據(jù)中的最佳定位精度分別是0.78 m、0.67 m和0.89 m，可以證明本文算法在兩種子地圖篩選方法中表現(xiàn)更優(yōu)，其定位精度普遍優(yōu)于最大相似度方法。

2.2.3 本文算法與其他算法的性能比較

為了驗證本文算法相較于其他定位算法的性能，在三組數(shù)據(jù)上比較了幾種不同定位算法的性能。這些定位算法包括基于里程計的定位算法、基于激光的SLAM算法［30］以及基于Wi-Fi的SLAM算法［31］。定位算法的相關(guān)參數(shù)根據(jù)表1～3中的測試結(jié)果進行了設(shè)置：對于數(shù)據(jù)集1，相似度閾值為0.3，標準差閾值為0.2，滑動窗口長度為20 s；對于數(shù)據(jù)集2和3，相似度閾值為0.3，標準差閾值為0.4，滑動窗口長度為20 s。實驗記錄了激光SLAM與本文算法在閉環(huán)檢測上的耗時，并測試了幾種不同定位算法的定位精度。相關(guān)實驗結(jié)果展示在表4、5中。

表4的結(jié)果顯示，隨著滑動窗口長度增加，兩種算法的閉環(huán)檢測耗時都在逐漸增加。在滑動窗口長度w設(shè)置為20 s的情況下，本文算法在三組數(shù)據(jù)中的閉環(huán)檢測耗時分別是1 361.64 s、1 416.74 s以及3 609.38 s。相比之下，激光SLAM算法的閉環(huán)檢測耗時分別是2 280.13 s、2 209.79 s以及4 663.86 s。與激光SLAM算法相比，本文算法的閉環(huán)檢測耗時分別減少了40.3%、35.9%與22.6%。通過不同數(shù)據(jù)集的測試實驗可以看出，本文算法能夠減少閉環(huán)檢測的耗時，從而提高SLAM的執(zhí)行效率。

表5的結(jié)果顯示，基于里程計的定位算法在三組數(shù)據(jù)上的定位精度分別是19.49 m、12.33 m和32.41 m，由于無法消除累計誤差，定位精度非常差?；赪i-Fi的SLAM算法在三組數(shù)據(jù)上的定位精度分別是3.09 m、2.36 m和5.02 m，盡管該算法融合了里程計和Wi-Fi閉環(huán)約束來優(yōu)化機器人軌跡，但在大型室內(nèi)環(huán)境中，Wi-Fi信號容易受到阻擋和干擾，導(dǎo)致定位精度不足?；诩す獾腟LAM算法在三組數(shù)據(jù)上的定位精度分別是1.52 m、1.43 m和2.85 m，受錯誤激光閉環(huán)的影響，定位精度有所降低。本文算法在三組數(shù)據(jù)上的定位精度分別是0.78 m、0.67 m和0.89 m，相較于激光SLAM算法，分別提升了48.6%、53.1%和68.7%。這些結(jié)果證明了本文算法能夠有效提高室內(nèi)大型環(huán)境中激光SLAM的位姿估計精度。圖5展示了幾種定位算法在三組數(shù)據(jù)上的定位軌跡。

最后，通過結(jié)合本文算法優(yōu)化后的機器人軌跡與采集的激光掃描數(shù)據(jù)，構(gòu)建了三組數(shù)據(jù)的全局地圖，從圖6可以看出，本文算法生成的全局地圖質(zhì)量較高，沒有明顯的傾斜或扭曲現(xiàn)象，能夠較好地反映室內(nèi)大型環(huán)境的真實情況。

3 結(jié)束語

本文提出了一種融合激光與Wi-Fi信息的圖優(yōu)化SLAM算法。在移動機器人上搭載了里程計、智能手機和激光雷達，采集里程計數(shù)據(jù)、Wi-Fi數(shù)據(jù)與激光數(shù)據(jù)。首先，構(gòu)建Wi-Fi指紋序列與激光子地圖，計算每對指紋序列中指紋點相似度的均值與標準差，篩選出用于閉環(huán)檢測的激光子地圖。然后，提取相應(yīng)子地圖的特征點并進行匹配，以確定激光閉環(huán)。最后，通過圖優(yōu)化方法融合里程計與激光閉環(huán)，對機器人軌跡進行優(yōu)化，構(gòu)建全局地圖。在170×30 m和180×80 m的室內(nèi)環(huán)境中，采集了三組數(shù)據(jù)進行驗證。結(jié)果表明，本文算法的定位精度在三組數(shù)據(jù)中分別是0.78 m、0.67 m和0.89 m，相較于激光SLAM算法，定位精度分別提高了48.6%、53.1%和68.7%。實驗結(jié)果證明了所提算法能夠提升室內(nèi)大型環(huán)境下激光SLAM的位姿估計精度。后續(xù)工作將考慮融合激光雷達與其他傳感器，進一步提升機器人的位姿估計精度，并將構(gòu)建的地圖應(yīng)用于室內(nèi)定位與導(dǎo)航。

參考文獻：

［1］Satake S，Hayashi K，Nakatani K，et al.Field trial of an information-providing robot in a shopping mall［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2015：1832-1839.

［2］Sweatt M，Ayoade A，Han Qi，et al.Wi-Fi based communication and localization of an autonomous mobile robot for refinery inspection［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2015：4490-4495.

［3］Wu Chong，Gong Zeyu，Tao Bo，et al.RF-SLAM：UHF-RFID based simultaneous tags mapping and robot localization algorithm for smart warehouse position service［J］.IEEE Trans on Industrial Informa-tics，2023，19（12）：11765-11775.

［4］Ebadi K，Bernreiter L，Biggie H，et al.Present and future of SLAM in extreme environments：the DARPA SubT challenge［J］.IEEE Trans on Robotics，2023，40：936-959.

［5］Cadena C，Carlone L，Carrillo H，et al.Past，present，and future of si-multaneous localization and mapping：toward the robust-perception age［J］.IEEE Trans on Robotics，2016，32（6）：1309-1332.

［6］Grisetti G，Tipaldi G D，Stachniss C，et al.Fast and accurate SLAM with Rao-Blackwellized particle filters［J］.Robotics and Autonomous Systems，2007，55（1）：30-38.

［7］張洪華，劉璇，陳付豪，等.基于圖優(yōu)化的SLAM后端優(yōu)化研究與發(fā)展［J］.計算機應(yīng)用研究，2019，36（1）：11-17.（Zhang Honghua，Liu Xuan，Chen Fuhao，et al.Research and development of SLAM back-end optimization based on graph optimization［J］.Application Research of Computers，2019，36（1）：11-17.）

［8］Folkesson J，Christensen H I.Closing the loop with graphical SLAM［J］.IEEE Trans on Robotics，2007，23（4）：731-741.

［9］Brossard M，Bonnabel S.Learning wheel odometry and IMU errors for localization［C］//Proc of International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2019：291-297.

［10］賀軍義，吳夢翔，宋成，等.基于UWB的密集行人三維協(xié)同定位算法［J］.計算機應(yīng)用研究，2022，39（3）：790-796.（He Junyi，Wu Mengxiang，Song Cheng，et al.Three-dimensional cooperative location algorithm of dense pedestrians based on UWB［J］.Application Research of Computers，2022，39（3）：790-796.）

［11］Park S，Schps T，Pollefeys M.Illumination change robustness in direct visual SLAM［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2017：4523-4530.

［12］Grisetti G，Stachniss C，Burgard W.Improved techniques for grid mapping with Rao-Blackwellized particle filters［J］.IEEE Trans on Robotics，2007，23（1）：34-46.

［13］Hess W，Kohler D，Rapp H，et al.Real-time loop closure in 2D LiDAR SLAM［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2016：1271-1278.

［14］Jiang Jianwen，Wang Jikai，Wang Peng，et al.LiPMatch：LiDAR point cloud plane based loop-closure［J］.IEEE Robotics and Automation Letters，2020，5（4）：6861-6868.

［15］Cattaneo D，Vaghi M，Valada A.LCDNet：deep loop closure detection and point cloud registration for LiDAR SLAM［J］.IEEE Trans on Robotics，2022，38（4）：2074-2093.

［16］Wu Yuanqing，Zhao Jiajun.A robust and precise LiDAR-inertial-GPS odometry and mapping method for large-scale environment［J］.IEEE/ASME Trans on Mechatronics，2022，27（6）：5027-5036.

［17］Tan Sheng，Ren Yili，Yang Jie，et al.Commodity Wi-Fi sensing in ten years：status，challenges，and opportunities［J］.IEEE Internet of Things Journal，2022，9（18）：17832-17843.

［18］Bisio I，Pan R L C，Lavagetto F，et al.Smartphone-based automatic place recognition with Wi-Fi signals for location-aware services［C］//Proc of IEEE International Conference on Communications.Pisca-taway，NJ：IEEE Press，2012：4943-4948.

［19］Jang B，Kim H.Indoor positioning technologies without offline fingerprinting map：a survey［J］.IEEE Communications Surveys amp; Tutorials，2019，21（1）：508-525.

［20］Huang J，Millman D，Quigley M，et al.Efficient，generalized indoor Wi-Fi GraphSLAM［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：1038-1043.

［21］Yang Chouchang，Shao Huairong.Wi-Fi-based indoor positioning［J］.IEEE Communications Magazine，2015，53（3）：150-157.

［22］Wagner R，Birbach O，F(xiàn)rese U.Rapid development of manifold-based graph optimization systems for multi-sensor calibration and SLAM［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2011：3305-3312.

［23］Liu Ran，Yuen C，Do T N，et al.Cooperative relative positioning of mobile users by fusing IMU inertial and UWB ranging information［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2017：5623-5629.

［24］Zhang Yi.LILO：a novel LiDAR-IMU SLAM system with loop optimization［J］.IEEE Trans on Aerospace and Electronic Systems，2022，58（4）：2649-2659.

［25］Ismail K，Liu Ran，Qin Zhenghong，et al.Efficient WiFi LiDAR SLAM for autonomous robots in large environments［C］//Proc of the 18th International Conference on Automation Science and Engineering.Piscataway，NJ：IEEE Press，2022：1132-1137.

［26］Stathoulopoulos N，Pagliari E，Davoli L，et al.Redundant and loosely coupled LiDAR-Wi-Fi integration for robust global localization in autonomous mobile robotics［C］//Proc of the 21st International Confe-rence on Advanced Robotics.Piscataway，NJ：IEEE Press，2023：121-127.

［27］Olson E B.Real-time correlative scan matching［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2009：4387-4393.

［28］Cohen-Or D，Kaufman A.3D line voxelization and connectivity control［J］.IEEE Computer Graphics and Applications，1997，17（6）：80-87.

［29］Kümmerle R，Grisetti G，Strasdat H，et al.G2o：a general framework for graph optimization［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：3607-3613.

［30］Yu Jincheng，Tong Jianming，Xu Yuanfan，et al.SMMR-explore：submap-based multi-robot exploration system with multi-robot multi-target potential field exploration method［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2021：8779-8785.

［31］秦正泓，劉冉，肖宇峰，等.基于Wi-Fi指紋序列匹配的機器人同步定位與地圖構(gòu)建［J］.計算機應(yīng)用，2022，42（10）：3268-3274.（Qin Zhenghong，Liu Ran，Xiao Yufeng，et al.Simultaneous localization and mapping for mobile robots based on Wi-Fi fingerprint sequence matching［J］.Journal of Computer Applications，2022，42（10）：3268-3274.）