融合WiFi、激光雷達與地圖的機器人室內定位

胡釗政 劉佳蕙 黃 剛 陶倩文

①(武漢理工大學信息工程學院 武漢 430070)

②(武漢理工大學智能交通系統研究中心 武漢 430063)

1 引言

高精度定位作為移動機器人系統的核心技術問題，在室外環境可以依靠全球導航衛星系統進行定位，而室內環境由于無法接收GPS信號，對機器人實現自主定位具有一定限制，因此建立一套準確實時的室內定位系統對智能機器人發展至關重要。目前室內定位技術主要分為基于無線網絡[1]、基于視覺[2]和基于激光雷達[3]3類，其中基于視覺傳感器的定位方法存在受環境光照影響較大的不足，不利于移動機器人的推廣，而基于無線網絡或激光雷達(Light Detection And Ranging,LiDAR)的室內定位技術則可以適應更豐富的室內環境情況。

基于無線網絡的定位方法主要包括WiFi、藍牙和超寬帶等，其中由于大部分室內公共場所基本實現無線局域網的覆蓋，WiFi定位已成為目前最廣泛使用的室內定位方案之一。考慮到室內多徑效應和非視距誤差等干擾因素，WiFi定位研究大多采用的是基于接收的信號強度指示(Received Signal Strength Indication,RSSI)建立指紋定位的方案。比如文獻[4]設計了一種輕量擴展的指紋構建方法，以無線接入點(Access Point,AP)為圓心來分割采集指紋，并利用RSSI計算距離以提取參考指紋數據，最后通過等高線聚類進行匹配定位。文獻[5]對WiFi指紋引入RSSI標準偏差，利用投票機制更新的數據來計算接入點信任因子，從而提取出可信指紋，避免異常信號強度影響定位精度。可以看到目前傳統WiFi指紋定位算法往往通過對匹配指紋庫獲得的參考點與當前位置的RSSI差值來得到信號距離，并將其作為權值依據來計算物理坐標，針對WiFi指紋定位的研究改進也主要集中在對樣本采集數據的選擇和數據降噪[6]方面。但從信號損耗模型來看，相同大小的信號強度差值與物理距離變化呈非線性關系，若僅通過對RSSI差值的倒數對參考點坐標進行加權并不能準確反映真實物理距離。且考慮到依靠單一無線信號傳感器定位時，指紋的分布和選擇會對定位性能產生較大干擾，系統整體魯棒性較差，所以目前更傾向于與其他定位方法進行融合以獲取更好的室內定位效果[7–9]。

基于激光雷達的定位技術核心是對不同時刻或位置采集的激光點云進行匹配從而計算機器人的相對運動關系實現定位。例如經典的迭代最近點(Iterative Closest Point,ICP)算法[10]。近年來許多研究人員提出了更加穩定魯棒的匹配算法，比如激光雷達里程計與地圖構建(LiDAR Odometry And Mapping,LOAM)算法[11]、輕量級和地面優化的LOAM(Lightweight and ground op timized-LOAM,Lego-LOAM)算法[12]中通過提取點云的點線特征進行配準來估計位姿變換關系以實現機器人定位。但是，激光定位方法要求場景具有比較豐富的特征，而室內環境往往存在大量單一、重復的場景結構(如走廊)，導致激光雷達在不同的位置提取的點云沒有顯著差異，無法有效實現點云匹配從而完成機器人定位。

本文針對目前WiFi指紋定位算法往往忽視信號衰減特性對距離權值影響的不足之處，以及在室內環境存在大量單一、重復的場景中激光雷達無法有效實現點云配準定位的問題，提出了基于卡爾曼濾波框架的WiFi、激光雷達與地圖的融合定位新算法。其核心思想是利用激光雷達獲取的厘米級相對定位(如走廊中的橫向定位)并結合地圖信息，形成對機器人位置的線性約束，同時，結合WiFi指紋的定位結果，實現融合定位。本文的主要創新工作如下：(1)提出了基于多環路分割地圖下信號衰減模型的強度加權WiFi指紋定位算法，降低了大面積場景下離散分布的指紋對定位結果的影響，并減少指紋匹配數量，進一步提高WiFi定位的穩定性與精度；(2)結合地圖與激光相對定位結果，提出了新的機器人位置線性約束方程，并將其轉化為卡爾曼濾波器觀測值，有效提高機器人的定位精度與穩定性。

2 本文算法

算法整體流程如圖1所示，主要由WiFi指紋定位、激光雷達獲取的相對位置，以及結合室內地圖提供的墻面建筑輪廓結構信息來進行卡爾曼融合定位3個部分組成。在進行WiFi定位時，本文在基于多環狀分割地圖采集指紋的情況下，利用信號衰減模型對不同信號強度的指紋分配不同匹配權值，將當前機器人所在位置采集到的WiFi數據與預先利用WiFi地址與信號強度構建的室內場景指紋表征匹配，計算獲得WiFi定位坐標；同時利用隨機采樣一致(RANdom SAmple Consensus,RANSAC)算法對機器人所載激光雷達掃描獲取的環境點云進行建筑結構直線檢測擬合，從而計算獲取機器人在室內與建筑輪廓的相對橫向距離信息，并結合室內地圖提供的先驗墻面直線方程對機器人位置坐標建立線性約束條件；最后通過卡爾曼濾波融合輸出得到結合WiFi、激光和地圖的機器人室內定位結果。

圖1 本文方法整體流程圖

2.1 基于多環狀分割地圖的信號強度加權WiFi指紋定位

WiFi指紋定位主要分為兩個階段：離線構建室內WiFi指紋表征和在線定位。針對傳統的網格指紋采樣方法在后續對采集得到的指紋數據庫中大量指紋進行匹配會降低系統實時性，且大范圍場景下過于分散的指紋分布會影響定位精度的問題[4]，本文基于文獻[4]中提出的輕量環狀指紋地圖構建方法對室內地圖進行分割并構建環狀WiFi指紋表征。

圖2 多環狀WiFi指紋地圖構建

根據式(2)、式(3)計算得到當前機器人WiFi信號與全局WiFi指紋表征的距離后，為進一步降低定位誤差，引入K最近鄰(K-Nearest Neighbor,KNN)算法思想，通過選取距離最小的前k個指紋表征采樣點位置坐標，來對機器人所在位置進行定位，獲取機器人全局物理坐標。WiFi指紋定位物理坐標計算如式(4)，其中，(X,Y)為移動機器人測試點所在的全局位置坐標，(X a,Y a)為WiFi指紋表征第a個采樣點坐標

2.2 激光相對定位與地圖位置約束

室內環境下的機器人激光定位，由于周圍建筑結構重復率較高，利用點云幀間配準方法進行定位往往存在一定困難。但是激光測距精度高，可達厘米級，因此，利用該優勢，研究采用將2維或3維激光傳感器采集得到的激光環境點云，通過擬合建筑結構邊緣直線后計算獲取機器人相對距離位置信息，并結合預先存儲了建筑直線方程信息的室內地圖來構建機器人位置約束。在獲得原始激光點云{p i}后，本文利用RANSAC算法對點云墻面邊緣直線進行擬合并得到相應的直線方程，若采用3維激光雷達，則需預先對地面點云進行濾除并將其投影至2維平面上，在此基礎上再進行相應的RANSAC直線擬合步驟，步驟如下：

在利用RANSAC算法處理點云過程中，由于待擬合模型為直線，因此從激光點云{p i}中重復迭代隨機選擇兩點構造直線l p，并計算{p i}中其余點到l p的距離，當該距離小于閾值ζ時，設定該點為內點(inliers)，并統計l p內點個數為S(l p)，其中迭代重復次數為T次，滿足關系如式(5)、式(6)所示

利用擬合的直線方程，以激光傳感器自身作為原點，則可通過式(8)計算出當前機器人在激光坐標系下與墻面邊緣的相對定位結果d k

獲得激光相對定位后，結合室內地圖信息對機器人的位置方程即可建立線性約束條件，其中，室內地圖信息同樣以直線方程形式預先存儲，Lm i表示室內地圖存儲的第i個 建筑墻面方程直線Lm的信息，其中θm和dm為方程參數，H為地圖中墻體輪廓直線總數

借助地圖存儲的先驗方程信息，結合式(7)、式(8)獲得的激光相對距離d k即可對機器人所在位置坐標(x l,y l)的狀態方程建立線性約束，推導過程如式(10)、式(11)所示，若檢測到單面，則構建一個機器人位置約束方程，若檢測到機器人兩邊墻體，則可利用地圖信息構建兩個線性約束方程

圖3 RANSAC檢測點云中建筑墻面邊緣直線

可以發現：利用激光雷達獲取的相對定位結果，通過地圖中存儲的信息，可以形成對機器人位置的線性約束條件。在定位過程中，利用激光雷達實時獲取機器人距離周圍障礙物/墻面的距離，通過地圖中存儲的墻面位置信息(如墻面在2維地圖中對應的直線方程)，即可完成對機器人的位置約束。由于激光和地圖均具有較高的精度，這種線性約束條件是一種高度可靠的約束，可以極大程度地提高后續定位精度。

2.3 基于卡爾曼濾波的融合定位方法

卡爾曼濾波是一種利用線性狀態方程對系統進行遞歸數據處理的最優估計算法。本文在上述WiFi與激光獲取的全局位置坐標與相對位置信息的基礎上，結合地圖信息，構建一種線性距離約束下的卡爾曼濾波器，將全局位置信息與局部橫向距離融合，以實現機器人室內高精度定位。與現有方法中通常將機器人定位坐標點之間的距離作為卡爾曼濾波器輸入有所不同，在此，本文引入室內地圖信息，來獲取點到線的距離作為觀測值輸入，以此構建的線性卡爾曼濾波器，克服了傳統的點與點之間距離的非線性約束。

通過獲得的狀態轉移方程和觀測矩陣Z，利用卡爾曼濾波器將WiFi、激光與地圖得到的信息聯系起來進行融合，對機器人運動狀態實時更新優化，以獲取更精確的定位結果。

3 實驗結果與分析

實驗地點選取了兩種不同的典型室內場景，一種為環境較為狹窄，建筑結構信息較少的辦公樓區走廊；一種為環境較空闊，面積較大，可提取的墻面輪廓信息較多的教學樓平臺。但這兩種室內環境均變化較小，重復場景較多，難以提取有效特征點，若采用傳統激光或視覺定位則基本失效，而僅采用WiFi定位時，誤差較大且不穩定，這些都對移動機器人適用的工作環境有較大局限。實驗分別在這兩種場景中對本文算法進行測試與驗證，兩組實驗中，定位的真值均通過場景中外置的高精度3維激光雷達獲取。

3.1 辦公樓區走廊場景定位實驗結果

如圖4(a)所示，該實驗場景為某辦公樓區長度47.5 m的直走廊，場景輪廓信息單一重復，無法提供豐富的結構特征。實驗過程中，每隔13 m部署1個AP，利用基于多環狀分割地圖的指紋采樣方法對室內場景進行WiFi數據采集，其中環狀指紋采樣以3 m為半徑，采樣點之間間隔為1.5 m。利用采集的數據生成WiFi指紋庫，每個采樣點均存儲對應的WiFi指紋信息，并通過采樣點本身的位置坐標實現了WiFi指紋與位置的關聯。實驗所用機器人為EAI智能機器人平臺(Dashgo D1)。機器人搭載掃描頻率為5 Hz掃描角度為360°的2維激光雷達(YD-LiDAR G4)進行相對定位。定位過程中WiFi信號由機器人搭載的WiFi接收機采集。其中，WiFi接收機與激光雷達進行離線標定，用以確定兩個傳感器之間的相對關系。

圖4 實驗場景及實驗設備

在移動機器人定位實驗過程中，首先利用機器人搭載的WiFi接收機采集當前WiFi的信息，通過獲取最近鄰環上的WiFi指紋表征進行匹配，根據指紋表征對應的位置坐標計算得到當前機器人的全局位置信息，完成WiFi指紋全局定位。同時，機器人搭載的激光雷達掃描獲取激光點云數據，并利用RANSAC算法計算機器人附近墻體對應的直線方程，通過這些直線方程計算出機器人與墻體的相對定位結果(即橫向距離信息)，將相對定位結果與地圖相結合，形成了對機器人位置的線性約束條件。這種約束條件與WiFi指紋定位構成了后續卡爾曼濾波融合定位的兩種類型的觀測值。

本實驗獨立進行6次算法驗證，如圖5所示。每次實驗中，機器人從不同的起點移動到不同的終點，移動路徑長約40 m，每次實驗記錄50個機器人在不同時刻測試點的定位誤差，從表1可以看到，未加權前的WiFi指紋定位算法最大誤差達到近5 m，平均誤差也有1.57 m，而強度加權的WiFi定位相比較于未加權的WiFi定位平均誤差降低了約9.6%，顯然，基于強度加權的WiFi指紋定位在沒有明顯增加計算量的同時更能有效反映真實物理距離。但是僅將單一WiFi定位結果作為卡爾曼濾波器唯一觀測值時，受限于WiFi本身定位誤差較大且易跳變的不足，雖抑制了部分WiFi定位的不規律抖動誤差，但結果依然不夠理想。

表1 單一WiFi指紋結合卡爾曼濾波前后定位結果對比

圖5 辦公區走廊場景下不同定位算法實驗結果對比

而利用本文所提融合算法思想，在該實驗場景中，將激光檢測到的與左右墻面輪廓之間的相對橫向距離，結合室內地圖預先存儲的信息可對機器人坐標建立2個約束條件，最后得到的定位平均誤差為0.40 m，在基于強度加權的WiFi指紋定位基礎上誤差還降低了約71.8%，且誤差閾值為0.5 m的數據占66.7%，小于1 m的定位數據占81%，極大地提高了單純利用WiFi指紋定位算法的定位精度與穩定性。

3.2 教學樓平臺場景定位實驗結果

該場景實驗選取某高校教學樓內平臺通道作為實驗地點，如圖6(a)所示為實驗場景整體建筑墻體平面圖，該場景下可提取更多的室內輪廓結構，在激光探測范圍內可為本文提出的機器人室內定位算法提供更多約束條件。其中場景結構信息通過2維地圖的方式進行預先存儲，墻體信息在地圖中表現為線段，在地圖存儲中通過各線段的起始點與終點獲取墻面直線方程參數。在該場景下，同樣每隔13 m部署1個AP，并以半徑3 m，采樣點間距為1.5 m來構建多環狀分割地圖的環狀WiFi指紋表征。實驗中采用的機器人為課題組與企業合作開發的“小卒2號”安防機器人平臺(如圖6(b)所示)。為驗證該算法對裝載3維激光雷達的平臺具備同樣的適用性，該平臺裝載了1臺16線3維激光雷達(Velod yne VLP-16)用于獲取室內環境點云，其中激光雷達采集頻率為20 Hz。WiFi信號同樣由機器人裝載的已完成標定的WiFi接收機進行采集。

圖6 實驗場景及實驗設備

該場景實驗中，分別選擇在早晚人流量不同的時刻，在該場景進行6次實驗驗證，每次機器人行駛路徑長約50 m，對該路徑上行駛的機器人選取50個測試點計算位置坐標，結果如表2和圖7所示。

圖7 教學樓平臺場景下不同定位算法實驗結果對比

表2 單一WiFi指紋結合卡爾曼濾波前后結果對比

可以看到，在面積較大、人員流動較多的室內環境中，即使采用基于多環狀分割地圖的指紋采樣提取的方法可以更好地約束大場景下指紋表征離散型較大的問題，但僅使用單一WiFi指紋方法定位時，誤差與真值之間的偏差依然明顯，單一WiFi指紋定位平均誤差為2.46 m，僅有36.3%的情況可以控制誤差在1.5 m以內。而在考慮WiFi信號強度加權后，平均誤差雖有所降低，但依然難以滿足室內定位需求。而采用本文的融合算法，得到的計算結果則與真值更為貼近，平均誤差為0.45 m，其中定位誤差閾值在0.5 m內的占62.7%，平均誤差在1 m以內的占92.0%。相比較于在3.1節實驗中的場景輪廓結構單一的環境，在能夠提取較多建筑結構信息的室內環境建立約束條件的情況下，本文的融合算法對單一WiFi指紋定位算法優化程度更高，誤差降低了81.7%，更有效地提高了室內定位的精度。

同時，從實驗結果中可以看出，當WiFi定位誤差較小時，本文提出的算法定位誤差也隨之降低，若采用更精確的傳感器定位方法提供更精確的初值，結合本文融合算法思想，可以得到更優的定位結果。為驗證本文融合算法思想對定位精度提高的有效性、可擴展性及應用于室內機器人快速實時定位的可行性，將本文提出的算法運用到公開文獻算法中進行多次實驗比較定位結果，并統計算法耗時，結果如表3所示。

表3 不同定位算法融合前后誤差與耗時結果對比

文獻[14]中結合WiFi信號強度圓重疊性質與對數衰減模型，對3邊測量距離重新加權分配以進行定位，文獻[15]在指紋定位基礎上生成等強度輪廓線進行3邊測量提高定位精度。可以看出文獻[4]、文獻[13]和文獻[14,15]中提出的WiFi指紋定位算法在集成本文提出的融合方法后，定位精度明顯改善，平均誤差分別降低了78.3%,76.7%,73.7%和78.7%。證明本文所提算法在沒有顯著增加定位耗時的情況下，可以有效結合不同算法(WiFi或藍牙等)提供的初定位，通過融合激光獲得的厘米級橫向距離與地圖約束信息，極大地優化定位精度，降低誤差。且由于激光雷達本身測距精度高，在人員流通較多情況下也能提供較高精度的相對定位信息，彌補了其他提供初始全局定位的算法的不足，增強了系統在不同室內環境下的魯棒性。

4 結論

利用室內環境建筑固定易于構建獲得地圖信息的優勢，本文提出一種基于卡爾曼濾波融合WiFi、激光與地圖的機器人室內定位算法，在基于多環狀分割地圖下的信號衰減模型的強度加權WiFi指紋匹配算法獲取的全局定位坐標下，通過機器人所載激光雷達提供的精確相對定位(如橫向偏離)與室內地圖結構信息結合，建立機器人位置的線性約束，并利用卡爾曼濾波將其與WiFi指紋定位結果進行融合優化從而獲得機器人在室內的位置坐標。實地測試并與其他算法對比結果表明，在保證機器人滿足實時定位的需求下，該融合算法在各個室內場景實驗中均保持了良好的定位精度，與單一WiFi定位相比較，定位平均誤差降低70%～80%，且在建筑輪廓可提取并建立更多約束方程的情況下，本文算法可進一步降低定位誤差。相比較于其他傳統算法，該方法擁有更好的室內定位性能及魯棒性，同時，結合室內地圖信息，利用對機器人位置的獨特約束，該方法還可融合其他不同定位算法，同樣可以有效提高相應的定位精度。