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

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

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

1 割草機器人概況

隨著農業信息化的發展，無論是智能物聯網設備的部署還是機器人平臺的投放，都為農業現代化的普及打下了堅實的基礎，成為了農業領域的研究熱點。SLAM技術可以非常有效地構建農產品區域的柵格地圖，作為功能型機器人的行動基礎，無論是機器人割草、運輸還是自動化采摘都必須以有一幅精確的地圖作為前提。

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

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

2 傳感器標定

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

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

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

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

2.2 輪式里程計的標定

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

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

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

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

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

2.3 運動畸變校正結果

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

圖2 IMU與里程計聯合校正激光點畸變

3 Karto建圖與G2O后端優化

3.1 圖優化策略

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

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

設機器人的每個位姿為x0~i，用其相對位姿來構建殘差的平方和函數：

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

3.2 采用G2O進行后端優化

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

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

圖3 G2O適配Karto構造流程

程序實現步驟如下：

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

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

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

4）創建整個圖優化的核心，即SparseOptimizer稀疏優化器，設置求解器為上一步定義的總求解器，并且打開調試輸出。

5）自定義圖優化的頂點和邊，并將其添加到SparseOptimizer中。設置SparseOptimizer的初始化，并輸入迭代次數最終保存結果。

4 試驗與最終建圖分析

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

圖4 測試結果

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

5 結束語

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