基于里程計/LiDAR/GNSS 的室內(nèi)外連續(xù)定位方法研究*

錢 偉陳 析陳 鑫孫丙宇*

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

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

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

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

1 室內(nèi)外高精度定位

1.1 室內(nèi)外連續(xù)定位算法框架

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

圖1 組合定位原理圖

AGV 室內(nèi)外連續(xù)定位過程開始時，自動獲取上一時刻定位狀態(tài)估計，并通過里程計進行速度積分和位姿積分，更新系統(tǒng)方程。 室外情況下，GNSS 接收機獲取AGV 定位數(shù)據(jù)計算量測方程，采集傳感器協(xié)方差矩陣計算卡爾曼增益系數(shù)，并更新AGV 的姿態(tài)最優(yōu)估計；室內(nèi)情況下，采用LiDAR 信標(biāo)定位算法計算AGV 定位數(shù)據(jù)，更新量測方程，并實時更新卡爾曼增益系數(shù)，更新AGV 姿態(tài)最優(yōu)估計。 AGV最優(yōu)估計作為下一時刻系統(tǒng)方程初始位姿，循環(huán)迭代，實現(xiàn)AGV 室內(nèi)外的連續(xù)定位。

1.2 基于EKF 室內(nèi)外融合定位算法

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

圖2 最優(yōu)定位估計

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

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

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

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

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

由量測信息對預(yù)測信息進行更新:

最后再次更新后驗協(xié)方差矩陣:

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

2 室內(nèi)外定位平滑濾波

2.1 基于SVD 坐標(biāo)變換

根據(jù)1.1 設(shè)計多傳感器融合實現(xiàn)連續(xù)定位，需設(shè)計算法將GNSS、LiDAR、里程計定位轉(zhuǎn)換到同一全局靜態(tài)坐標(biāo)系下。 另外因為傳感器外形尺寸影響，需要將傳感器轉(zhuǎn)換到AGV 上同一位置。 本文室內(nèi)外坐標(biāo)系統(tǒng)具體如圖3 所示。

圖3 AGV 多傳感器坐標(biāo)系

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

步驟1 將LiDAR 定位數(shù)據(jù)由雷達安裝位置1P′Li，轉(zhuǎn)換到里程計安裝位置2PLi:

步驟2 將GNSS 接收機定位數(shù)據(jù)由接收機安裝位置3P′Gi，轉(zhuǎn)換到里程計安裝位置2PGi:

步驟3 將GNSS、LiDAR 定位數(shù)據(jù)統(tǒng)一到同一靜態(tài)坐標(biāo)系下，將LiDARPLi轉(zhuǎn)移到GNSSPGi坐標(biāo)系下，采用SVD 統(tǒng)計方法計算坐標(biāo)系平移矩陣T(a，b)、旋轉(zhuǎn)矩陣R:

坐標(biāo)轉(zhuǎn)換誤差Ei分析結(jié)果:

定義標(biāo)定多對點坐標(biāo)質(zhì)心公式:

使誤差最小的R和T就是最優(yōu)的旋轉(zhuǎn)矩陣和平移向量，利用多對點構(gòu)造最小二乘:

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

最終計算旋轉(zhuǎn)矩陣R、平移矩陣T:

2.2 室內(nèi)外信號源平滑濾波

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

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

定位信號源最優(yōu)概率估計由GNSS、LiDAR 協(xié)方差矩陣、水平精度因子、LiDAR 信標(biāo)數(shù)等共同推算，具體算法如表1。

表1 室內(nèi)外信號源平滑濾波算法

3 實驗

3.1 實驗條件和環(huán)境

本文提出室內(nèi)外連續(xù)定位算法，在合肥某園區(qū)進行了AGV 的室內(nèi)外連續(xù)定位實驗，以評估室內(nèi)外連續(xù)的定位穩(wěn)定性和精度。

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

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

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

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

表2 采集數(shù)據(jù)集

3.2 實驗結(jié)果

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

圖5 實驗環(huán)境和實驗軌跡圖

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

圖6 雷達信標(biāo)數(shù)和GNSS-RTK 從站接收衛(wèi)星數(shù)

表3 為室內(nèi)外連續(xù)定位平均誤差對比。 可以發(fā)現(xiàn)在定位效果上，本文提出GNSS、LiDAR、里程計連續(xù)定位系統(tǒng)相比傳統(tǒng)室內(nèi)外連續(xù)定位方法[7－8，11]，定位精度上具有較大提升。 平均絕對定位誤差為0.122 m，滿足AGV 的室內(nèi)外連續(xù)定位導(dǎo)航需求。

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

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

圖7 室內(nèi)外連續(xù)定位實驗定位誤差圖

4 結(jié)論

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