錢 偉陳 析陳 鑫孫丙宇*

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

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

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

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

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

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

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

圖1 組合定位原理圖

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

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

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

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

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

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

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

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

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

由量測(cè)信息對(duì)預(yù)測(cè)信息進(jìn)行更新:

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

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

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

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

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

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

圖中XOY 為L(zhǎng)iDAR 定位坐標(biāo)系；X′O′Y′為GNSS-RTK 定位坐標(biāo)系；X″O″Y″為AGV 車身坐標(biāo)系，同時(shí)也是里程計(jì)坐標(biāo)系。 AGV 運(yùn)動(dòng)軌跡由室內(nèi)經(jīng)過(guò)混合場(chǎng)景到達(dá)室外，GNSS-RTK、LiDAR 均可以提供混合場(chǎng)景下的AGV 定位。Pi為AGV 在混合場(chǎng)景中里程計(jì)處定位點(diǎn)，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ù)由雷達(dá)安裝位置1P′Li，轉(zhuǎn)換到里程計(jì)安裝位置2PLi:

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

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

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)

3.1 實(shí)驗(yàn)條件和環(huán)境

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

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

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

圖4 麥克納姆輪實(shí)驗(yàn)平臺(tái)和實(shí)驗(yàn)場(chǎng)地

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

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

3.2 實(shí)驗(yàn)結(jié)果

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

圖5 實(shí)驗(yàn)環(huán)境和實(shí)驗(yàn)軌跡圖

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

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

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

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

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

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

4 結(jié)論

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