基于Karto算法的割草機器人建圖及其運動畸變校正*

侯可馨 ， 李霽翔 ， 鄒騰躍 ， 黃浩軍

（1.福建農(nóng)林大學機電工程學院，福建 福州 350002；2.福建農(nóng)林大學金山學院，福建 福州 350002）

1 割草機器人概況

隨著農(nóng)業(yè)信息化的發(fā)展，無論是智能物聯(lián)網(wǎng)設(shè)備的部署還是機器人平臺的投放，都為農(nóng)業(yè)現(xiàn)代化的普及打下了堅實的基礎(chǔ)，成為了農(nóng)業(yè)領(lǐng)域的研究熱點。SLAM技術(shù)可以非常有效地構(gòu)建農(nóng)產(chǎn)品區(qū)域的柵格地圖，作為功能型機器人的行動基礎(chǔ)，無論是機器人割草、運輸還是自動化采摘都必須以有一幅精確的地圖作為前提。

李小敏等[1]提出了以機器視覺為基礎(chǔ)的割草機器人避障、自動返航等路徑規(guī)劃方法。徐晨[2]分析了機器人作業(yè)的環(huán)境特點，運用計算機視覺算法對目標進行準確識別。方言海等[3]利用數(shù)據(jù)融合與實時地圖構(gòu)建技術(shù)構(gòu)建三維信息，為大棚內(nèi)無人化作業(yè)提供了技術(shù)支持。李盛輝等[4]對IMU的姿態(tài)進行了解分析。這些方法實現(xiàn)的前提是要有一個精確構(gòu)建的地圖和里程計信息，但目前仍存在建圖不準和運動畸變問題。對此，課題組提出了一種基于Karto算法的建圖方法，引入G2O后端優(yōu)化庫，同時進行IMU及里程計標定，進行運動畸變校正，從而優(yōu)化激光雷達的幀間匹配，提升回環(huán)效果。

傳統(tǒng)的割草機器人采用的建圖算法在前端里程計部分有所欠缺，很少通過回環(huán)約束匹配相似幀。若采用視覺與3D激光融合則又提高了成本，不具備經(jīng)濟效益，而且算法的精確度其實很大程度上依賴于先驗數(shù)據(jù)的預處理[5]。

2 傳感器標定

2.1 IMU的時間同步與角度積分

IMU（Inertial Measurement Unit）即慣性測量單元，是用來檢測機器人相應(yīng)坐標上的加速度、角速度等的多軸傳感器。本文的激光數(shù)據(jù)均是采用一款低成本、高性能的思嵐激光雷達RPLIDAR-A1測出的激光點數(shù)據(jù)。該雷達頻率為10 Hz，而IMU的頻率是100 Hz，首先要對兩者進行時間戳的配準，也就是進行時間同步。在程序設(shè)計中使用雙端隊列進行IMU數(shù)據(jù)的預存儲，所以隊列中的IMU時間戳也會是連續(xù)的。需要修剪IMU的數(shù)據(jù)隊列，直到接近當前幀的雷達點云數(shù)據(jù)。將單線雷達掃描一圈時間內(nèi)的IMU數(shù)據(jù)進行角度積分，并存儲三個方向上的角度值，從而進行數(shù)據(jù)對齊和姿態(tài)外參的聯(lián)合調(diào)優(yōu)。IMU模型如下：

式中，bωt是陀螺儀的偏置，bαt是加速度計的偏置，nω和nα都是IMU模型的附加噪聲。在時間同步時需要計算IMU在指定時間范圍內(nèi)的角度積分，時間同步的示意圖如圖1所示。

圖1 IMU與激光雷達當前幀時間同步

2.2 輪式里程計的標定

設(shè)底盤當前位姿為（x,y,θ），運動學解算增量為（dx,dy,dθ）[6]，于是本平臺割草機器人的航跡推算位姿變換為：

通常采用線性最小二乘方法[7]進行里程計標定，讓誤差函數(shù)趨近于0，從線性空間的角度進行位姿投影。

在里程計標定過程中，由于幀間匹配算法PL-ICP計算得到的里程計數(shù)據(jù)更加準確，所以用激光雷達的scan-match數(shù)據(jù)作為真值里程計測量得到的數(shù)據(jù)為在模型中假設(shè)兩者成線性關(guān)系于是對于每一組數(shù)據(jù)，可得：

合并為最小二乘的線性方程組形式：

將編碼器測得的輪式里程計數(shù)據(jù)乘上這個標定矩陣，就可以得到一個校正后的里程計數(shù)據(jù)。同時，里程計數(shù)據(jù)與scan數(shù)據(jù)的時間戳同步，即可得到最終的畸變校正結(jié)果。

2.3 運動畸變校正結(jié)果

本文的實現(xiàn)平臺基于ROS kinetic版本進行開發(fā)，畸變校正測試時，割草機器人處于園區(qū)花壇的直角轉(zhuǎn)彎處。因為當機器人主要平移運動時，產(chǎn)生的畸變較小，所以畸變校正主要是對拐彎處的雷達數(shù)據(jù)進行姿態(tài)校正。通過ROS系統(tǒng)下的Rviz插件觀測花壇90°拐角處機器人車身以0.8 rad/s的角速度旋轉(zhuǎn)時激光點的運動畸變校正情況，如圖2所示。左圖是原始激光點集合與畸變校正后的激光點集合對比，右圖為校正后的雷達數(shù)據(jù)點分布?；冃Ｕ蟮募す恻c，在數(shù)據(jù)集播放過程中旋轉(zhuǎn)的雷達數(shù)據(jù)明顯更加整齊。

圖2 IMU與里程計聯(lián)合校正激光點畸變

3 Karto建圖與G2O后端優(yōu)化

3.1 圖優(yōu)化策略

傳統(tǒng)的SLAM算法都是基于濾波器實現(xiàn)的，缺點就是濾波器只估計當前的機器人位姿，而之前的累計誤差不會被消除[8-9]，后期的回環(huán)檢測和優(yōu)化只會將錯誤的位姿進行優(yōu)化與回環(huán)，導致與真實地圖大相徑庭。

本文采用的Karto建圖算法是基于圖優(yōu)化的，在基于圖優(yōu)化的SLAM中，機器人的位姿是一個節(jié)點（Node）或者頂點，是待優(yōu)化的變量；而位姿之間的約束關(guān)系構(gòu)成邊（Edge），通常表示誤差項。后端采用非線性最小二乘來優(yōu)化建圖過程中的累計誤差，最終得到一個一致性地圖。所以，SLAM的圖優(yōu)化問題基本可以分解為兩大步：首先是構(gòu)建圖，假設(shè)前一個位姿矩陣為H1，當前位姿矩陣為H2，那么可以認為其邊即兩點之間的相對位置關(guān)系為H1-1H2；其次是優(yōu)化圖，調(diào)整機器人的位姿，使其盡量滿足邊的約束。

設(shè)機器人的每個位姿為x0~i，用其相對位姿來構(gòu)建殘差的平方和函數(shù)：

在圖優(yōu)化過程中，需要使得回環(huán)位姿的殘差平方和最小，最終得到機器人的最優(yōu)位姿預測?；丨h(huán)約束是個權(quán)重占比很高的圖優(yōu)化約束，將這個約束放入圖結(jié)構(gòu)中同時進行優(yōu)化，以達到消除誤差的目的。

3.2 采用G2O進行后端優(yōu)化

G2O（General Graphic Optimization）是基于圖優(yōu)化的庫，契合Karto的位姿圖的接口。不同于視覺中的光束平差法BA（Bundle Adjustment），G2O優(yōu)化庫不需要相機進行重投影來將相機姿態(tài)和特征點空間位置作出最優(yōu)調(diào)整后，最后收束到相機光心。在實際應(yīng)用過程中，即使圖優(yōu)化BA是稀疏的，也常常會出現(xiàn)實時性差、能耗高的問題[10]。引入G2O優(yōu)化庫后，只需要將Karto算法中對應(yīng)的頂點和約束分別賦值給G2O對應(yīng)接口即可。

相比于Ceres庫而言，雖然G2O的代碼實現(xiàn)更為復雜，但通用面更廣，可以實現(xiàn)重定位，Ceres把優(yōu)化問題定義為最小二乘問題，而G2O把優(yōu)化問題表現(xiàn)為圖的一種方式。G2O需要根據(jù)平臺SLAM算法來適配參數(shù)，并分配頂點與邊，其代碼實現(xiàn)構(gòu)造流程如圖3所示。

圖3 G2O適配Karto構(gòu)造流程

程序?qū)崿F(xiàn)步驟如下：

1）創(chuàng)建線性求解器LinearSolver。在圖優(yōu)化中，增量方程的形式為HΔX=-b，當機器人節(jié)點位姿觀測數(shù)量增多時，H矩陣維數(shù)很大，求逆和求解過程很復雜，可以用C++內(nèi)置的庫來解決高維矩陣求逆問題。

2）創(chuàng)建BlockSolver，用線性求解器來初始化。割草機器人行進過程中需要姿態(tài)pose的維數(shù)和觀測點，這些在程序開始運行時都無法確定，設(shè)置可變尺寸的Solver來創(chuàng)建塊求解器。

3）創(chuàng)建總求解器Solver，用BlockSolver來初始化，選擇LM（Levenberg-Marquardt）下降法為迭代的優(yōu)化策略。

4）創(chuàng)建整個圖優(yōu)化的核心，即SparseOptimizer稀疏優(yōu)化器，設(shè)置求解器為上一步定義的總求解器，并且打開調(diào)試輸出。

5）自定義圖優(yōu)化的頂點和邊，并將其添加到SparseOptimizer中。設(shè)置SparseOptimizer的初始化，并輸入迭代次數(shù)最終保存結(jié)果。

4 試驗與最終建圖分析

在本實驗平臺下，將割草機器人置于一個大致100 m*30 m的花壇數(shù)據(jù)集環(huán)境進行測試，選取特征較為明顯的一處花壇圓弧邊緣區(qū)域進行建圖特征對比分析。測試結(jié)果如圖4所示。

圖4 測試結(jié)果

從圖4中觀察到，圖4（c）花壇圓弧邊緣不再出現(xiàn)疊圖現(xiàn)象，且構(gòu)建的二維柵格地圖邊界重合，不存在分層現(xiàn)象，對割草機器人的穩(wěn)定運行起到了至關(guān)重要的作用。因此，運動畸變校正、里程計標定及高效的后端優(yōu)化策略，對于SLAM建圖來說缺一不可。即使有算法效率再高的后端優(yōu)化庫，若前端幀間匹配有擾動，激光點數(shù)據(jù)畸變不予校正，所傳入后端優(yōu)化的粗糙原始數(shù)據(jù)也會被不斷迭代，導致最終建圖失敗，進而導致部署的機器人運行不穩(wěn)定，降低作業(yè)效率。

5 結(jié)束語

本文在Karto算法的基礎(chǔ)上，增加實時運動畸變校正和里程計標定功能，并引入了G2O后端圖優(yōu)化庫，實現(xiàn)了割草機器人在工作過程中的穩(wěn)定高效，而不會出現(xiàn)建圖失敗或者定位失敗等問題。利用IMU和輪式里程計的結(jié)合校正激光點位姿，使得割草機器人可以以一定速率行進，而且行進過程中不會產(chǎn)生建圖失敗或者疊圖的現(xiàn)象。同時由于農(nóng)產(chǎn)品園區(qū)通常是回環(huán)的，所以當引入G2O優(yōu)化時，有一個優(yōu)化權(quán)重占比很大的起始和終止相對位姿，為割草機器人后續(xù)要實現(xiàn)的各種功能打下了穩(wěn)定的基礎(chǔ)。該方案的設(shè)計架構(gòu)還可以進一步改進，可以通過深度相機來得到三維的環(huán)境地圖，同時將相機的像素點引入算法的激光雷達幀間匹配，進而獲得更好的后端優(yōu)化效果。