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

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

1 割草機(jī)器人概況

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

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

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

2 傳感器標(biāo)定

2.1 IMU的時(shí)間同步與角度積分

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

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

圖1 IMU與激光雷達(dá)當(dāng)前幀時(shí)間同步

2.2 輪式里程計(jì)的標(biāo)定

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

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

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

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

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

2.3 運(yùn)動(dòng)畸變校正結(jié)果

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

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

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

3.1 圖優(yōu)化策略

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 試驗(yàn)與最終建圖分析

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

圖4 測(cè)試結(jié)果

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

5 結(jié)束語

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