基于里程計/LiDAR/GNSS 的室內外連續定位方法研究*

錢 偉陳 析陳 鑫孫丙宇*

(1.重慶三峽學院機械工程學院，重慶 404130；2.中國科學院合肥物質科學研究院，安徽 合肥 230031)

隨著制造業與物流行業的飛速發展，市場對自動導引車(Automatic guided Vehicle，AGV)定位導航的需求逐漸擴大，其中室內外連續定位技術需求更是日益爆增[1－2]。 室內外連續定位技術已經獨立于室內定位，成為導航定位領域的一個新的研究方向。室內外連續定位方法以定位精度高、穩定性好、環境適應性強為特點，其核心算法在于室內和室外融合定位算法與定位信號源平滑濾波算法。 然而受室內外光照、遮擋物等變化和不平滑的定位信號源切換的影響，在實際室內外連續定位過程中易出現定位信號丟失、定位信號精度差等問題，極大地影響了AGV 在室內外連續場景的定位導航。 因此，如何實現穩定性高、精度高的AGV 室內外連續定位算法，成為了AGV 定位導航工程應用中的一個研究熱點。

基于激光雷達的信標定位算法是當前室內定位領域熱點算法之一[3]。 在室內定位過程中，激光信標定位算法相對于RFID、WIFI、激光即時定位與地圖構建(simultaneous localization and mapping，SLAM)、視覺SLAM 等[4]室內定位算法具有定位精度高、定位平穩、魯棒性高等顯著優勢。 在市場對室內外連續定位的高需求下，基于激光信標、全球導航衛星系統(Global Navigation Satellite System，GNSS)、里程計組合實現AGV 室內外連續定位，具有巨大的潛在應用價值。 傳統室內外連續定位算法種類較多，如文獻[5]提出了一種基于UWB 協助低成本慣性測量單元(IMU)系統的連續定位解決方案，評估移動機器人的位置、速度和位姿。 文獻[6]提出了一種室內外連續定位系統，該系統考慮了UWB、GPS 以及兩種定位技術的結合，并可靠地估計在室內和室外場景中移動的車輛的位置。 文獻[7]提出一種基于GPS/INS/磁力計進行多傳感器融合的室內外連續定位方法，定位軌跡誤差在2 m 以內。綜上所述，傳統室內外連續定位算法，定位精度大都在米級，信號源切換實時性較差，定位服務適應對象為手機、人類等，不能滿足AGV 定位精度、穩定性的要求。

基于此背景，AGV 室內外連續定位技術要解決的技術難題在于室內外定位精度問題和不同定位信號源平滑跳轉問題。 在室內外連續定位領域，本文首次提出一種基于里程計/激光雷達(Light Detection and Ranging，LiDAR)/GNSS 的多元異構數據融合方法解決室內外定位精度問題，提出一種基于擴展卡爾曼濾波的多模型融合連續定位方法，利用GNSS 和LiDAR 協方差、水平精度因子動態計算定位信號源概率，借助定位信號源概率實現不同信號源的平滑跳轉，從而提高室內外連續定位系統的穩定性和定位精確，滿足AGV 的定位導航要求。

1 室內外高精度定位

1.1 室內外連續定位算法框架

本文針對傳統AGV 在室內外不同動態環境下工作易出現定位丟失，定位精度下降的問題，提出了一種AGV 室內外連續定位算法，原理如圖1 所示。 是一種基于擴展卡爾曼濾波的多元異構融合定位算法，采集了來自里程計、GNSS、LiDAR 的數據。 利用里程計獲取實時6 軸角速度、加速度，并通過積分運算實時推算AGV 實時速度和位姿，并提取姿態矩陣。 然而由于定位信息是通過積分運算進行的，里程計長時運行會產生嚴重的誤差漂移。 本文采用擴展卡爾曼濾波(Extended Kalman Filter，EKF)算法來提高室內外連續定位精度，算法系統方程由里程計提供參數更新，量測方程分兩段式進行更新，室內環境時由LiDAR 信標算法提供更新，室外環境由GNSS 提供參數計算更新，同時提出了一種基于擴展卡爾曼濾波的多模型融合連續定位濾波方法，提高信號源切換的平滑性。

圖1 組合定位原理圖

AGV 室內外連續定位過程開始時，自動獲取上一時刻定位狀態估計，并通過里程計進行速度積分和位姿積分，更新系統方程。 室外情況下，GNSS 接收機獲取AGV 定位數據計算量測方程，采集傳感器協方差矩陣計算卡爾曼增益系數，并更新AGV 的姿態最優估計；室內情況下，采用LiDAR 信標定位算法計算AGV 定位數據，更新量測方程，并實時更新卡爾曼增益系數，更新AGV 姿態最優估計。 AGV最優估計作為下一時刻系統方程初始位姿，循環迭代，實現AGV 室內外的連續定位。

1.2 基于EKF 室內外融合定位算法

在室內外連續定位場景下，障礙物遮擋、動態環境改變的問題難以避免，為了獲取更準確的AGV 位姿，本文采用擴展卡爾曼濾波(EKF)進行多傳感器信息融合。 由于系統狀態變量和測量數據間的關系是非線性的，需要將非線性問題轉化為線性問題，其中擴展卡爾曼濾波(EKF)使用泰勒展開式來解決轉化問題，再根據最小均方差規則進行系統狀態的最優估計[8]，求解最優估計的流程如圖2 所示，YLk為LiDAR 信標定位算法計算室內環境量測方程，YGk為GNSS-RTK 計算室外環境量測方程，Yk為最優量測估計，Xk為最優預測估計，Uk為里程計增量，＾Xk為后驗最優估計。

圖2 最優定位估計

里程計模型建立系統狀態方程，由于里程計存在累計誤差較大，需提供量測方程進行誤差修正更新:

對式(1)和式(2)進行線性化處理，采用雅克比矩陣來替代系統方程的非線性轉移過程。H表示h的雅克比矩陣，F表示f的雅克比矩陣。 雅克比矩陣求解方式為方程矩陣對變量的偏導數。

式(1)和式(2)系統狀態方程和量測方程線性化具體表達式為:

式中:Xk、Xk－1分別為k時刻與k－1 時刻的狀態變量，p為位置，v為速度，θ為航向角；f表示非線性系統模型；wk－1考慮為系統噪聲，假定噪聲均值為0，Rk為協方差。Yk表示實時最優量測方程，h表示非線性觀測模型，vk表示量測噪聲，考慮量測噪聲是均值為0，協方差為Qk的高斯噪聲。為系統狀態轉移系數矩陣，T為AGV間隔采樣時間；H＝為量測系數矩陣，θ為觀測航跡與推算航跡的夾角，Vx、Vy為k時AGV 速度。 系統狀態預測量計算的協方差矩陣:

計算AGV 預測信息與量測信息后，更新此時AGV 狀態的后驗概率分布。 AGV 系統在時刻k時的擴展卡爾曼濾波增益為:

由量測信息對預測信息進行更新:

最后再次更新后驗協方差矩陣:

通過上式可以理解AGV 的系統運動模型迭代更新過程。 根據前一時刻的狀態Xk－1獲取AGV 先驗狀態X(k｜k－1)及其協方差矩陣P(k｜k－1)。 計算出k時刻擴展卡爾曼濾波的增益系數后，可更新k時刻AGV 的后驗最優估計X(k｜k)及其協方差矩陣P(k｜k)。因此，在數據融合定位中，隨著量測數據的不斷輸入，AGV 狀態參數將不斷迭代更新，實現AGV 的實時高精度定位。

2 室內外定位平滑濾波

2.1 基于SVD 坐標變換

根據1.1 設計多傳感器融合實現連續定位，需設計算法將GNSS、LiDAR、里程計定位轉換到同一全局靜態坐標系下。 另外因為傳感器外形尺寸影響，需要將傳感器轉換到AGV 上同一位置。 本文室內外坐標系統具體如圖3 所示。

圖3 AGV 多傳感器坐標系

圖中XOY 為LiDAR 定位坐標系；X′O′Y′為GNSS-RTK 定位坐標系；X″O″Y″為AGV 車身坐標系，同時也是里程計坐標系。 AGV 運動軌跡由室內經過混合場景到達室外，GNSS-RTK、LiDAR 均可以提供混合場景下的AGV 定位。Pi為AGV 在混合場景中里程計處定位點，Pi在XOY坐標系下坐標為PLi＝(XLi，YLi)，在X′O′Y′坐標系下坐標為PGi＝(XGi，YGi)，傳感器安裝位置1，2，3 處于AGV 中軸線上，位置1 與2 相距d1，位置2 與3 相距d2，α為AGV 航向角。 多傳感器坐標系統轉換步驟如下:

步驟1 將LiDAR 定位數據由雷達安裝位置1P′Li，轉換到里程計安裝位置2PLi:

步驟2 將GNSS 接收機定位數據由接收機安裝位置3P′Gi，轉換到里程計安裝位置2PGi:

步驟3 將GNSS、LiDAR 定位數據統一到同一靜態坐標系下，將LiDARPLi轉移到GNSSPGi坐標系下，采用SVD 統計方法計算坐標系平移矩陣T(a，b)、旋轉矩陣R:

坐標轉換誤差Ei分析結果:

定義標定多對點坐標質心公式:

使誤差最小的R和T就是最優的旋轉矩陣和平移向量，利用多對點構造最小二乘:

式中:第一項只與旋轉矩陣R有關，第二項與R和平移向量T都有關。 可以令第一項為0 求得R，再令第二項為0 求得T求PGi去質心點:

最終計算旋轉矩陣R、平移矩陣T:

2.2 室內外信號源平滑濾波

為了提高室內外連續定位穩定性和精確度，研究人員最關心的熱點在于定位信號源切換頻率、信號源切換是否造成定位精度下降。 如果室內外信號源平滑濾波算法較差，將嚴重影響組合定位的穩定性和精度。 本文提出一種基于卡爾曼濾波的信號源平滑濾波算法，在室內外連續定位中，AGV 定位的穩定性、精度均有較明顯提高。 室外環境下，GNSS協方差矩陣，水平精度因子均可以有效評估GNSS定位的準確度和穩定性[9－10]；室內環境下，雷達連續多幀掃描到信標數、雷達信標定位算法協方差都可以直接有效地反映室內雷達定位的精度和穩定性。

其中GNSS 水平精度因子(HDOP)計算公式如式(18)，hii是權重矩陣H的對角元素。αKn和θKn分別代表衛星方位角和俯仰角

定位信號源最優概率估計由GNSS、LiDAR 協方差矩陣、水平精度因子、LiDAR 信標數等共同推算，具體算法如表1。

表1 室內外信號源平滑濾波算法

3 實驗

3.1 實驗條件和環境

本文提出室內外連續定位算法，在合肥某園區進行了AGV 的室內外連續定位實驗，以評估室內外連續的定位穩定性和精度。

圖4(a)所示為麥克納姆輪實驗平臺，搭載SICK-NAV 型雷達、司南M600-GNSS 定位系統，實驗場景下GNSS、LiDAR 安裝在帶有里程計的麥克納姆輪實驗小車上，并在GNSS失效區域布置好雷達定位信標，室外無遮擋處安裝GNSS 主機站和電臺。

如圖4(b)所示，LiDAR 信標用實線圈出，實驗開始定位進行對準，因為里程計進行軌跡推算需要初始位姿變量，初始位姿在室內外分別由精度較高的LiDAR 信標定位算法或者GNSS-RTK 計算。 完成初始對準后，測試平臺開始進行室內外連續定位測試。

圖4 麥克納姆輪實驗平臺和實驗場地

本實驗共采集8 次數據，如表2 所示。 其中實驗1～8 分別為室內定位采集、室外定位采集、室內外連續定位采集、室內外隨機定位采集。 實驗數據的采集與室內外連續定位算法實現均在筆記本電腦(Intel i5-9300H CPU＠2.4 GHz，16G RAM)上運行。

表2 采集數據集

3.2 實驗結果

根據3.1 采集數據集，選取實驗5 進行室內外連續定位算法精度與穩定性分析。 測試平臺實驗定位軌跡如圖5 所示，虛線為相對真值軌跡，由GNSS、LiDAR 靜態定位采集擬合；實線為最終室內外連續定位算法計算軌跡。

圖5 實驗環境和實驗軌跡圖

圖6為麥克納姆輪測試平臺實驗過程中LiDAR接收雷達信標數和GNSS 從站接收衛星數隨時間變化圖。 其中0～90 s 和230 s～726 s 測試平臺處于室外環境；90 s～230 s 測試平臺處于室內環境。 可以看出室外向室內移動過程中，從站接收衛星數減少，GNSS 定位精度降低，同時雷達掃描信標增多，LiDAR 定位精度提高；室內向室外行進過程中，從站接收衛星數增加，GNSS 定位精度提高，同時LiDAR 掃描信標減少定位精度降低

圖6 雷達信標數和GNSS-RTK 從站接收衛星數

表3 為室內外連續定位平均誤差對比。 可以發現在定位效果上，本文提出GNSS、LiDAR、里程計連續定位系統相比傳統室內外連續定位方法[7－8，11]，定位精度上具有較大提升。 平均絕對定位誤差為0.122 m，滿足AGV 的室內外連續定位導航需求。

表3 不同組合定位平均誤差

圖7 所示為本次室內外連續定位實驗的定位誤差繪圖，系統在只有GNSS/里程計融合定位下，室外具有較好的定位精度，室內定位基本丟失；在只有Li-DAR/里程計定位情況下，室內具有較高的定位精度，室外環境下定位基本丟失。 本文提出室內外連續定位方法，系統可以自適應地切換定位模型，連續定位誤差明顯減小，最大絕對定位誤差為0.5 m，平均絕對定位誤差為0.122 m。 對比傳統室內外連續定位方法[12－15]，在定位精度和穩定性上均有明顯提高。

圖7 室內外連續定位實驗定位誤差圖

4 結論

本文首次提出了一種基于里程計、LiDAR、GNSS 融合的室內外連續定位方法，通過建立室內雷達信標數據庫和室外GNSS-RTK 定位基站，實現AGV 的全局定位。 針對室內外遮擋物和光照改變造成AGV 定位精度、穩定性降低的問題，本文首次提出了一種室內融合LiDAR/里程計、室外融合GNSS/里程計的全局定位方法；同時為了抑制因為信號源切換造成定位穩定性下降的問題，提出了一種室內外信號源平滑濾波算法，該方法利用LiDAR、GNSS 定位協方差、GNSS 水平定位精度因子、LiDAR掃描信標等評估信號源可信度，剔除不可信定位結果。 最后，通過實際實驗驗證了所提出方法的有效性。