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

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

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

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

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

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

1 LeGO-LOAM算法特征提取

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

1.1 三維點云線束分割

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

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

1.2 基于余弦值提取特征

LeGO-LAOM算法是通過曲率值來表達掃描點的平滑程度, 進而提取一幀點云中特定數(shù)量的角點和平面點.由于曲率值沒有上下限, 無法通過設定閾值對每一幀中點云特征點的數(shù)量進行定量統(tǒng)計, 即無法獲知環(huán)境結構化信息的豐富程度.因此, 本文選取掃描點的夾角余弦值來提取角點、平面點的特征同時對兩者數(shù)量進行分別統(tǒng)計.掃描點夾角的余弦值的變化區(qū)間為[-1,1], 而在實際環(huán)境中平面上掃描點夾角的余弦值理論上為-1, 角點夾角的余弦值理論上為0, 這為尋找特征點提供了量化的參考值, 同時可設定余弦值區(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, 由于噪音的存在, 設定平面點余弦值區(qū)間為[-1,-0.984], 即夾角在 [170°, 190°]內.同理, 設定角點余弦值區(qū)間為 [-0.174, 0.174], 即夾角在 [80°, 100°]內, 并且統(tǒng)計角點和平面點余弦值區(qū)間內的特征點個數(shù).為了保證所選取特征的均勻性, 將每一幀點云在水平平面上平分為6等份, 在這6個子圖中等量選取以下4類特征點: (1)余弦值最接近-1的, 每個子圖選取4個共計24個掃描點為平面點; (2)選取余弦值較接近-1的, 每個子圖選取4個共計24個掃描點為待選平面點; (3)余弦值最接近0的, 每個子圖選取2個共計12個掃描點為角點; (4)余弦值較接近0的, 每個子圖選取2個掃描點, 共計12個掃描點為待選角點.

1.3 特征關聯(lián)與誤差函數(shù)構建

在第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小車的航向、速度、位姿等導航參數(shù)的自主式導航系統(tǒng).根據(jù)文獻[14]AGV小車的狀態(tài)C可以表示為:

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

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

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

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

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

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

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

3 混合匹配算法

3.1 ICP匹配算法

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

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

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

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

3.2 混合匹配算法

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

將LeGO-LOAM算法中的曲率提取特征點的方式改進為以余弦值提取特征點的方式, 可準確判別出環(huán)境中結構化信息的豐富程度.系統(tǒng)運行框架如圖1所示, 當環(huán)境中平面特征點數(shù)量大于或等于48個時, 激光里程計采用LeGO-LOAM算法, 當環(huán)境中平面特征點數(shù)量小于48個時, 激光里程計采用ICP算法.將IMU位姿信息作為混合匹配算法的初始值, 構建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軸方向的旋轉角度,pk、qk表示激光雷達第k幀時刻AGV小車的位置信息和角度信息.構建系統(tǒng)誤差函數(shù)就需要對AGV小車的位姿進行求解, 使得誤差函數(shù)最小.迭代的位姿初始值為IMU狀態(tài)估計的位姿, 這樣能夠有效減少迭代次數(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.由于室內RTK無法工作, 故選用結構化信息較多的室外停車場作為實驗場景一(圖2中衛(wèi)星圖教學樓內環(huán)), 教學大樓外環(huán)道路上結構化信息較少, 結合內部停車場可作為實驗場景二(圖2中衛(wè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次實驗結果取平均值繪制成表1.

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

從表1中可以看出, 在結構信息較為豐富的場景一中, 混合匹配算法以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種算法得到的相鄰幀點云之間的位姿轉換矩陣將所有點云拼接成點云地圖.如圖5所示, 圖5(a)、圖5(b)和圖5(c)分別為LeGO-LOAM算法、IMU/LeGOLOAM算法和混合匹配算法建立的場景二俯視圖,圖5中白色曲線、黑色曲線、黃色曲線和紅色曲線分別為RTK估計的軌跡、LeGO-LOAM算法估計的軌跡、IMU/LeGO-LOAM算法估計的軌跡和混合匹配算法估計的軌跡.從圖5中可看出3種算法在前期結構化信息豐富的場景中漂移并不明顯, 后期AGV小車行駛至教學樓外圈結構信息較少時, LeGO-LOAM算法開始出現(xiàn)較大漂移, IMU/LeGO-LOAM算法由于IMU提供了一定的位姿信息進行優(yōu)化, 故漂移小于LeGO-LOAM算法, 混合匹配算法在此階段轉換到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%.在場景二中結構化信息較少的外環(huán)環(huán)境及大轉彎、減速帶區(qū)域,混合匹配算法累積誤差相較于IMU/LeGO-LOAM算法和LeGO-LOAM算法累計誤差分別降低了44.3%和89.8%

表2 累積誤差對比 (m)

5 結論與展望

為實現(xiàn)AGV小車在室內、室外不同場景實時定位及高精度建圖, 提出了基于IMU與激光雷達緊耦合的混合匹配算法.實驗結果表明, 在結構化信息豐富的場景一中, 混合匹配算法與IMU/LeGO-LOAM算法相較LeGO-LOAM算法相對位姿誤差均降低18.5%, 累計誤差均降低33.8%; 在結構化信息較少的場景二中,混合匹配算法相較于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)檢測, 進一步提高復雜環(huán)境中AGV小車激光雷達定位與建圖精度.