周志全, 劉 飛, 屈婧婧,2, 匡 兵, 景 暉, 鄒鈺杰

1(桂林電子科技大學(xué) 機電工程學(xué)院, 桂林 541004)

2(桂林航天工業(yè)學(xué)院 科技處, 桂林 541004)

隨著人工智能的發(fā)展, 激光雷達以其分辨率高、測距遠、不受光照影響和抗干擾能力強等優(yōu)點獲得了廣泛的關(guān)注.激光雷達可幫助AGV (自動導(dǎo)引運輸車,Automated Guided Vehicle)小車在未知環(huán)境中實時定位與建立地圖信息, 為后續(xù)軌跡跟蹤、路徑規(guī)劃等提供良好基礎(chǔ).激光雷達即時定位與地圖構(gòu)建(Simultaneous Localization And Mapping, SLAM)可分為前端里程計、后端優(yōu)化、閉環(huán)檢測和地圖構(gòu)建4個模塊, 其中最重要的為前端里程計模塊[1].當前, 前端里程計主要有直接匹配方案、基于特征的匹配方案和多傳感器融合方案[2].

直接匹配方案主要有ICP算法(迭代最近點, Iterative Closest Point)[3]和正態(tài)分布變換算法(Normal Distributions Transform, NDT), ICP算法是一種基于對應(yīng)點的匹配算法, 定位精度高, 但依賴初值, 實時性較低;NDT算法是一種基于概率模型的匹配算法, 實時性較好, 但定位精度低于ICP算法.基于特征的匹配方案主要有提取曲率[4]、角點[5]和法失[6]等特征的ICP變種算法和LeGO-LOAM (激光雷達測距與地圖構(gòu)建算法,Lightweight and Ground-Optimized Lidar Odometry and Mapping)算法[7,8].其中, ICP變種算法將定位精度進一步提高, 但實時性依舊較低; LeGO-LOAM算法根據(jù)線、面特征進行匹配, 采用高頻率定位, 低頻率建圖的方案, 定位精度和實時性均較好, 但在結(jié)構(gòu)信息較少的環(huán)境中, 累積的誤差使得地圖變得模糊[9].多傳感器融合方案有激光雷達與實時動態(tài)載波相位差分定位技術(shù)(Real-Time Kinematic, RTK)[10]、激光雷達與IMU等融合算法.其中, 激光雷達與RTK融合算法定位精度高,RTK定位無累計誤差, 但RTK在室內(nèi)或有嚴重遮擋環(huán)境下無法工作, 魯棒性較差; 激光雷達與IMU融合算法有松耦合[11]和緊耦合[12]兩種算法: 基于卡爾曼融合的松耦合算法擴展性強, 可接入其他傳感器同時工作,但當長時間運行后IMU漂移過于嚴重, 從而導(dǎo)致定位精度降低; 緊耦合算法通過建立一個統(tǒng)一的誤差函數(shù),同時優(yōu)化激光雷達和IMU的位姿, 定位精度高, 實時性較好.

AGV小車需要在室內(nèi)倉庫結(jié)構(gòu)化信息豐富的場景中行駛, 同時需要將貨物搬運至其他倉庫過程中經(jīng)過結(jié)構(gòu)化信息較少的室外場景, 此時標準的LeGOLOAM算法能夠提取的線、面特征較少, 定位精度下降.綜上, 為實現(xiàn)AGV小車在室內(nèi)室外實時高精度建圖與定位, 本文采用IMU與激光雷達緊耦合的混合匹配算法, 當環(huán)境中結(jié)構(gòu)信息較多時, 激光里程計采用LeGO-LOAM算法, 而當環(huán)境中結(jié)構(gòu)化信息較少時采用ICP算法.

1 LeGO-LOAM算法特征提取

為建立統(tǒng)一坐標系, 本文將世界坐標系記為W, 激光雷達坐標系記為L, 假設(shè)慣性測量單元IMU與AGV小車重心重合, 可將AGV小車與IMU坐標系統(tǒng)一記為I.

1.1 三維點云線束分割

根據(jù)研究需求, 需將激光雷達無序點云轉(zhuǎn)化為有序點云, 將獲取的激光雷達原始點云坐標信息記為(x,y,z), 掃描點到激光雷達原點距離記為l, 則有:

將一幀無序的三維點云按垂直角度α和水平角度β大小排序, 使其劃分為二維有序點云, 為下一步提取特征做準備.

1.2 基于余弦值提取特征

LeGO-LAOM算法是通過曲率值來表達掃描點的平滑程度, 進而提取一幀點云中特定數(shù)量的角點和平面點.由于曲率值沒有上下限, 無法通過設(shè)定閾值對每一幀中點云特征點的數(shù)量進行定量統(tǒng)計, 即無法獲知環(huán)境結(jié)構(gòu)化信息的豐富程度.因此, 本文選取掃描點的夾角余弦值來提取角點、平面點的特征同時對兩者數(shù)量進行分別統(tǒng)計.掃描點夾角的余弦值的變化區(qū)間為[-1,1], 而在實際環(huán)境中平面上掃描點夾角的余弦值理論上為-1, 角點夾角的余弦值理論上為0, 這為尋找特征點提供了量化的參考值, 同時可設(shè)定余弦值區(qū)間提取并統(tǒng)計所需要的特征信息.

每一線束上第i掃描點與第i-1和i+1掃描點組成夾角余弦值:

其中,d(i-1)i、di(i+1)和d(i-1)(i+1)分別表示第i-1第i掃描點的距離、第i第i+1掃描點的距離和第i-1第i+1掃描點的距離.

在實際環(huán)境中平面上掃描點夾角的余弦值理論上為-1, 由于噪音的存在, 設(shè)定平面點余弦值區(qū)間為[-1,-0.984], 即夾角在 [170°, 190°]內(nèi).同理, 設(shè)定角點余弦值區(qū)間為 [-0.174, 0.174], 即夾角在 [80°, 100°]內(nèi), 并且統(tǒng)計角點和平面點余弦值區(qū)間內(nèi)的特征點個數(shù).為了保證所選取特征的均勻性, 將每一幀點云在水平平面上平分為6等份, 在這6個子圖中等量選取以下4類特征點: (1)余弦值最接近-1的, 每個子圖選取4個共計24個掃描點為平面點; (2)選取余弦值較接近-1的, 每個子圖選取4個共計24個掃描點為待選平面點; (3)余弦值最接近0的, 每個子圖選取2個共計12個掃描點為角點; (4)余弦值較接近0的, 每個子圖選取2個掃描點, 共計12個掃描點為待選角點.

1.3 特征關(guān)聯(lián)與誤差函數(shù)構(gòu)建

在第k+1幀掃描點云pk+1中, 選取角點和平面點組成角點集和平面點集.將第k+1幀點云根據(jù)IMU提供的位姿作為初值投影到第k幀激光雷達坐標系下.對于k+1幀中某一角點i, 首先在第k幀點云中尋找歐式距離最近的角點j, 然后在第k幀中尋找與j角點不同線束上歐式距離最近的角點l, 將jl連成直線,將i,j,l的坐標記為根據(jù)文獻[13],角點i到直線的距離即誤差計算公式:

同理, 對于k+1幀中的某一平面點i, 首先在第k幀中尋找距離平面點i最近的平面點j, 其次在第k幀中尋找距離平面點j最近的同一線束上的平面點l和不同線束上的平面點m, 平面點jlm組成一個平面,將i,j,l,m的坐標記為:點i到平面的距離即為誤差記為:

LeGO-LOAM算法的誤差函數(shù)為:

2 IMU狀態(tài)估計與誤差函數(shù)

IMU是利用加速度計、陀螺儀測量出線加速度、角加速度, 再通過積分推算出AGV小車的航向、速度、位姿等導(dǎo)航參數(shù)的自主式導(dǎo)航系統(tǒng).根據(jù)文獻[14]AGV小車的狀態(tài)C可以表示為:

其中,R為3行3列的旋轉(zhuǎn)矩陣,P為1行3列的位置矩陣,v為速度, IMU的偏置b可以寫成:

其中,bg、ba分別表示陀螺儀和加速度計的偏置.

根據(jù)IMU原始數(shù)據(jù)角速度w和加速度a, 測量值和受白斯噪聲η和偏置b的影響有:

根據(jù)IMU的測量值, 推導(dǎo)出無人車的旋轉(zhuǎn)、位置和速度等運動信息, 在時間t內(nèi)按式(13)-式(15)進行:在式(13)、式(14)和式(15)中, 假設(shè)在時間t+Δt內(nèi)角速度和加速度保持不變.

然后根據(jù)IMU預(yù)積分方法從而得到從時刻i到時刻j相對運動的旋轉(zhuǎn)角度ΔRij, 位置ΔPij和速度Δvij的偏差.

由于IMU的漂移主要在旋轉(zhuǎn)角度和空間位置上比較明顯, 因此本文將IMU的誤差函數(shù)EB構(gòu)建為:

構(gòu)建IMU的誤差函數(shù)是為了讓IMU與激光雷達的總誤差最小化, 進一步約束激光雷達點云匹配, 進而求解出AGV小車的位姿信息.

3 混合匹配算法

3.1 ICP匹配算法

標準ICP算法的原理是: 在源點云和目標點云間,按照歐式距離最小為對應(yīng)點的規(guī)則, 找到對應(yīng)點集, 然后迭代計算出最優(yōu)匹配參數(shù), 即旋轉(zhuǎn)矩陣R和平移矩陣T, 使得誤差函數(shù)EL2最小或者達到設(shè)定迭代次數(shù).

其中,Pi、Qi、R、T和n分別表示源點云、目標點云、旋轉(zhuǎn)矩陣、平移矩陣和源點云中的點云個數(shù).

ICP算法流程如算法1所示.

算法1.ICP算法1) 找尋對應(yīng)點集: 在目標點云數(shù)據(jù)中找到源點云的對應(yīng)點集, 對應(yīng)點間的歐式距離作為誤差, 構(gòu)建誤差函數(shù);2) 計算變換矩陣: 計算旋轉(zhuǎn)矩陣R和平移矩陣T;3) 更新目標點云: 使用步驟2中參數(shù)R、T對目標點云進行旋轉(zhuǎn)和平移得到新的目標點云;4) 判斷迭代停止: 如果誤差函數(shù)小于設(shè)定閾值或者迭代次數(shù)達到設(shè)定上限, 則停止迭代計算并輸出最終轉(zhuǎn)換矩陣參數(shù)R、T, 否則返回步驟2.

3.2 混合匹配算法

在結(jié)構(gòu)化信息較少時, 基于線、面特征的LeGOLOAM算法定位精度降低, 而基于點到點的ICP算法仍可以準確、可靠的工作; 在結(jié)構(gòu)化信息較多時, LeGOLOAM算法定位精度高于ICP算法.同時, 點到線、面的LeGO-LOAM算法收斂速度為二階收斂, 比點到點的ICP算法一階收斂速度快, 實時性更好.由此可知,LeGO-LOAM算法和ICP算法具有互補性, 因此提出基于IMU與激光雷達緊耦合的混合匹配算法來進一步提高AGV小車定位精度和系統(tǒng)魯棒性.

將LeGO-LOAM算法中的曲率提取特征點的方式改進為以余弦值提取特征點的方式, 可準確判別出環(huán)境中結(jié)構(gòu)化信息的豐富程度.系統(tǒng)運行框架如圖1所示, 當環(huán)境中平面特征點數(shù)量大于或等于48個時, 激光里程計采用LeGO-LOAM算法, 當環(huán)境中平面特征點數(shù)量小于48個時, 激光里程計采用ICP算法.將IMU位姿信息作為混合匹配算法的初始值, 構(gòu)建IMU與ICP算法的聯(lián)合誤差函數(shù), 實現(xiàn)位姿共同優(yōu)化.此時系統(tǒng)的誤差函數(shù)為:

圖1 系統(tǒng)運行框架圖

AGV小車的位姿可表示為:

其中,I表示AGV小車的坐標系,tx、ty、tz表示激光雷達第k幀時刻AGV小車的x、y、z軸方向位移, θz、θx、 θy表示AGV小車繞z、x、y軸方向的旋轉(zhuǎn)角度,pk、qk表示激光雷達第k幀時刻AGV小車的位置信息和角度信息.構(gòu)建系統(tǒng)誤差函數(shù)就需要對AGV小車的位姿進行求解, 使得誤差函數(shù)最小.迭代的位姿初始值為IMU狀態(tài)估計的位姿, 這樣能夠有效減少迭代次數(shù), 提高運行效率, 同時消除了零初值或者勻速運動假設(shè)初值帶來的錯誤特征匹配, 更加符合真實的運功.此外在實時輸出位姿的同時, 將輸出位姿對IMU的位姿解算進行實時修正, 可降低IMU零漂、溫漂等誤差影響, 使得位姿解算更加準確.

4 實驗分析

本實驗使用的激光雷達為16線束, 水平角度分辨率為0.2°, 垂直角度分辨率為2°, 更新頻率為10 Hz.IMU的角度偏移量2°/h, 加速度偏移量0.003 mg, 更新頻率為300 Hz.RTK的水平定位精度10 mm, 垂直定位精度20 mm, 更新頻率100 Hz.由于室內(nèi)RTK無法工作, 故選用結(jié)構(gòu)化信息較多的室外停車場作為實驗場景一(圖2中衛(wèi)星圖教學(xué)樓內(nèi)環(huán)), 教學(xué)大樓外環(huán)道路上結(jié)構(gòu)化信息較少, 結(jié)合內(nèi)部停車場可作為實驗場景二(圖2中衛(wèi)星圖教學(xué)樓內(nèi)環(huán)加外環(huán)).

圖2 衛(wèi)星圖

4.1 定位精度分析

為了驗證3種算法的有效性, 所有算法不做閉環(huán)檢測, 利用RTK測得的位姿作為參考值, 3種算法估計位姿為測量值.運用LeGO-LOAM算法、IMU與LeGO-LOAM緊耦合算法(簡稱IMU/LeGO-LOAM)和基于IMU與激光雷達緊耦合的混合匹配算法(簡稱混合匹配算法)分別對場景一和場景二進行5次試驗,每隔10米計算一次相對位姿誤差PRE.以PRE平均值作為水平定位精度標準, 統(tǒng)計5次實驗結(jié)果取平均值繪制成表1.

表1 相對位姿誤差PRE對比 (m)

從表1中可以看出, 在結(jié)構(gòu)信息較為豐富的場景一中, 混合匹配算法以IMU/LeGO-LOAM算法模式運行, 混合匹配算法相比LeGO-LOAM算法定位精度提高了18.5%.圖3為混合匹配算法建立的場景一三維點云地圖.對比圖1的衛(wèi)星圖可知, 圖3中無重影較好的建立了三維點云地圖.在場景二中, 混合匹配算法相比LeGO-LOAM算法和IMU/LeGO-LOAM算法定位精度分別提高了33.0%和10.9%.由此可以得出混合掃描匹配算法定位精度均高于IMU/LeGO-LOAM算法和LeGO-LOAM算法.圖4為3種算法在場景二中RPE與AGV小車行駛里程之間的變化曲線, 其中LeGO-LOAM算法用黑色正方形表示, IMU/LeGOLOAM算法用紅色圓表示, 混合匹配算法用藍色三角形表示.由于未做閉環(huán)檢測, 當AGV小車行駛到起始點附近時, 位姿漂移嚴重, 故圖4中只選總軌跡長度836 m的前800 m的RPE誤差對比, 在后續(xù)中會給出起始點附近的地圖和累積誤差.

圖3 場景一三維點云地圖

圖4 RPE誤差與里程的變化曲線對比

4.2 建圖效果與累積誤差分析

為驗證3種算法的建圖效果, 本文將通過3種算法得到的相鄰幀點云之間的位姿轉(zhuǎn)換矩陣將所有點云拼接成點云地圖.如圖5所示, 圖5(a)、圖5(b)和圖5(c)分別為LeGO-LOAM算法、IMU/LeGOLOAM算法和混合匹配算法建立的場景二俯視圖,圖5中白色曲線、黑色曲線、黃色曲線和紅色曲線分別為RTK估計的軌跡、LeGO-LOAM算法估計的軌跡、IMU/LeGO-LOAM算法估計的軌跡和混合匹配算法估計的軌跡.從圖5中可看出3種算法在前期結(jié)構(gòu)化信息豐富的場景中漂移并不明顯, 后期AGV小車行駛至教學(xué)樓外圈結(jié)構(gòu)信息較少時, LeGO-LOAM算法開始出現(xiàn)較大漂移, IMU/LeGO-LOAM算法由于IMU提供了一定的位姿信息進行優(yōu)化, 故漂移小于LeGO-LOAM算法, 混合匹配算法在此階段轉(zhuǎn)換到IMU與ICP緊耦合的匹配算法, 故漂移低于IMU/LeGO-LOAM.本文為檢測3種算法性能, 未做閉環(huán)檢測, 所以當AGV小車行駛至起始點附近時, 地圖出現(xiàn)較大重影.起始點的局部放大圖如圖6所示, 對比可以得出LeGO-LOAM算法建立的三維點云地圖重影較大, 起點終點位姿相差較大, 且路徑?jīng)]有閉合; IMU/LeGO-LOAM算法建立的三維點云地圖重影較小, 起點終點位姿相差較小, 且路徑基本閉合; 混合匹配算法建立的三維點云地圖重影最小, 起點終點位姿相差很小, 且路徑閉合.

圖5 軌跡點云地圖對比

圖6 起始點放大圖對比

首先將3種算法估計的起點位姿和RTK估計的起點位姿進行對齊, 然后將3種算法估計的終點坐標與RTK給出的終點坐標進行對比, 計算出累積誤差.從表2中可以看出混合匹配算法在場景一中累積誤差與IMU/LeGO-LOAM算法一致, 混合匹配算法相較于LeGO-LOAM算法累計誤差降低33.8%.在場景二中結(jié)構(gòu)化信息較少的外環(huán)環(huán)境及大轉(zhuǎn)彎、減速帶區(qū)域,混合匹配算法累積誤差相較于IMU/LeGO-LOAM算法和LeGO-LOAM算法累計誤差分別降低了44.3%和89.8%

表2 累積誤差對比 (m)

5 結(jié)論與展望

為實現(xiàn)AGV小車在室內(nèi)、室外不同場景實時定位及高精度建圖, 提出了基于IMU與激光雷達緊耦合的混合匹配算法.實驗結(jié)果表明, 在結(jié)構(gòu)化信息豐富的場景一中, 混合匹配算法與IMU/LeGO-LOAM算法相較LeGO-LOAM算法相對位姿誤差均降低18.5%, 累計誤差均降低33.8%; 在結(jié)構(gòu)化信息較少的場景二中,混合匹配算法相較于LeGO-LOAM算法相對位姿誤差降低33.0%, 累計誤差降低89.8%; 混合匹配算法相較于IMU/LeGO-LOAM相對位姿誤差降低10.9%, 累計誤差降低44.3%; 混合掃描匹配算法定位精度均高于IMU/LeGO-LOAM算法和LeGO-LOAM算法,同時對比分析3種算法建立的地圖重影程度, 得出混合掃描匹配算法比IMU/LeGO-LOAM算法和LeGO-LOAM算法建立的地圖重影程度更小, 地圖精度更高.下一步將會增加系統(tǒng)閉環(huán)檢測, 進一步提高復(fù)雜環(huán)境中AGV小車激光雷達定位與建圖精度.