孫宏昌，何婉凌，蔣永翔，鄧三鵬

（1.天津職業(yè)技術(shù)師范大學(xué) 機(jī)器人及智能裝備研究院，天津 300222；2.天津職業(yè)技術(shù)師范大學(xué) 天津市智能機(jī)器人技術(shù)及應(yīng)用企業(yè)重點(diǎn)實(shí)驗(yàn)室，天津 300222）

0 引言

隨著農(nóng)業(yè)機(jī)械化的發(fā)展，深松土壤的機(jī)械種類(lèi)也日漸增多，目前深松機(jī)械主要為拖拉機(jī)和深松機(jī)器人[1]。農(nóng)田環(huán)境下的路徑規(guī)劃以IMS（Inertial measurement system 慣性導(dǎo)航系統(tǒng)）和GPS（Global position system，全球定位系統(tǒng)）的多傳感器融合為主[2]。針對(duì)路徑軌跡跟蹤問(wèn)題，有學(xué)者設(shè)計(jì)了模糊的閉環(huán)控制方式[3]，但是模糊閉環(huán)控制對(duì)于中高速車(chē)輛效果不理想[4]；DWA（dynamic window approach 滑動(dòng)窗口法）可以在采集加速度后通過(guò)輪式里程計(jì)進(jìn)行速率集合的采集[5]，由于輪子打滑會(huì)造成誤差，趙明等[6]針對(duì)路徑規(guī)劃中局部穩(wěn)定點(diǎn)的問(wèn)題提出用域的方法解決，但是用此方法的機(jī)器人無(wú)法準(zhǔn)確到達(dá)目標(biāo)點(diǎn)。Melchior等[7]提出基于粒子RRT（Rapidly-exploring Random Trees 快速擴(kuò)展隨機(jī)樹(shù)算法）的不確定性算法，但此算法不適應(yīng)于室外環(huán)境；Connolly 等[8]提出一種使用Laplace 方程的路徑規(guī)劃算法，但該算法不適用于特征點(diǎn)少的環(huán)境；謝飛等[9]優(yōu)化邊界條件改進(jìn)了模擬退火法，但是此方法調(diào)參復(fù)雜實(shí)用性不高。這些機(jī)器人導(dǎo)航定位技術(shù)有一定準(zhǔn)確性[10]，但嚴(yán)重依賴GPS，在部分地區(qū)GPS 信號(hào)很差[11]，慣導(dǎo)系統(tǒng)在短時(shí)間內(nèi)有準(zhǔn)確性，但由于慣導(dǎo)易受到外界影響產(chǎn)生累計(jì)誤差[12]，且機(jī)器人易忽略自身姿態(tài)引起的定位誤差[13]，因此開(kāi)發(fā)一種多傳感器數(shù)據(jù)融合的深松機(jī)器人具有重要意義。采用移動(dòng)機(jī)器人組合導(dǎo)航定位方式，通過(guò)多傳感器系統(tǒng)進(jìn)行融合定位，發(fā)揮移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)優(yōu)勢(shì)，修正機(jī)器人在田間作業(yè)時(shí)因GPS 信號(hào)弱引起的定位誤差，從而提高系統(tǒng)的定位精度和可靠性。

1 深松機(jī)器人仿真系統(tǒng)設(shè)計(jì)

深松機(jī)器人在solidwork 軟件中經(jīng)行機(jī)器人模型磚換，根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)模型建立坐標(biāo)軸、連桿和創(chuàng)建continue 類(lèi)型joint 的旋轉(zhuǎn)軸并且進(jìn)行urdf 文件轉(zhuǎn)換，并導(dǎo)入碰撞系數(shù)模型后在ROS 中顯示。

搭建基于ROS 的深松機(jī)器人需要在urdf 模型的基礎(chǔ)上優(yōu)化成xacro 文件，并加入激光雷達(dá)、里程計(jì)、IMS 模塊，ROS 系統(tǒng)下的軟件可以查看機(jī)器人模型，Gazebo 會(huì)根據(jù)導(dǎo)入模型的運(yùn)動(dòng)模型進(jìn)行動(dòng)力學(xué)仿真，通過(guò)Gazebo 模擬器的動(dòng)態(tài)模擬，能夠復(fù)現(xiàn)深松機(jī)需要的耕地環(huán)境，搭建一個(gè)80m×80m 的仿真環(huán)境，搭建的仿真過(guò)程如圖1 所示。

圖1 仿真建立過(guò)程

ROS 是基于分布式框架操作機(jī)器人的開(kāi)源系統(tǒng)，各模塊通過(guò)節(jié)點(diǎn)（即通信單元）與節(jié)點(diǎn)間的松耦合連接。圖2 為ROS 各節(jié)點(diǎn)話題通信，其節(jié)點(diǎn)關(guān)系為：目標(biāo)發(fā)送節(jié)點(diǎn)/next_goel 發(fā)送下一目標(biāo)點(diǎn)，/move_base 速度控制節(jié)點(diǎn)控制機(jī)器人移動(dòng)，gazebo 節(jié)點(diǎn)/gazebo 發(fā)布雷達(dá)數(shù)據(jù)，機(jī)器人位姿/robot_state_publisher 發(fā)布/tf_static 數(shù)據(jù)到/amcl 進(jìn)行定位與建圖，通過(guò)/tf 進(jìn)行機(jī)器人各個(gè)位姿下的坐標(biāo)變換，并將變換后的位姿加載到/map 地圖，將地圖/map 傳輸?shù)?move_base 導(dǎo)航功能中，根據(jù)A* 算法生成全局路徑規(guī)劃，下位機(jī)輸出移動(dòng)控制指令，/cmd_vel 控制速度實(shí)現(xiàn)全覆蓋路徑規(guī)劃。

圖2 ROS 各節(jié)點(diǎn)話題通信

2 深松機(jī)器人的Cartographer-SLAM 算法

根據(jù)根據(jù)深松機(jī)器人運(yùn)動(dòng)學(xué)模型如圖3 所示，可以進(jìn)一步計(jì)算機(jī)器人里程計(jì)模型下的公式推導(dǎo)。

圖3 機(jī)器人運(yùn)動(dòng)學(xué)模型

（x0，y0，θ0）是0 時(shí)刻時(shí)機(jī)器人在世界坐標(biāo)系下的位姿；（x1，y1，θ1）是1 時(shí)刻時(shí)機(jī)器人在世界坐標(biāo)系下的位姿；（dx，dy，dθ）是機(jī)器人坐標(biāo)系下的位姿變化增量；（ζx，ζy，ζθ）是機(jī)器人位姿的表示ζx，ζy，表示平移，ζθ表示旋轉(zhuǎn)。

深松機(jī)器人先對(duì)輪式里程計(jì)和IMS 進(jìn)行積分處理，得到深松機(jī)器人的原始位姿，接著對(duì)雷達(dá)點(diǎn)云數(shù)據(jù)進(jìn)行優(yōu)化原始的優(yōu)化，進(jìn)行濾波后與IMS 和輪式里程計(jì)數(shù)據(jù)融合，做好初始準(zhǔn)備后構(gòu)建地圖。

Cartographer 算法利用局部?jī)?yōu)化和全局優(yōu)化的方法對(duì)機(jī)器人位姿進(jìn)行優(yōu)化[14]。深松機(jī)器人根據(jù)輪式里程計(jì)和IMS 數(shù)據(jù)融合得到機(jī)器人的初始位姿，開(kāi)始進(jìn)行局部?jī)?yōu)化。局部?jī)?yōu)化是由兩個(gè)部分構(gòu)建：子地圖和掃描匹配，首先，通過(guò)迭代的方式對(duì)齊子地圖參考幀和激光雷達(dá)掃描幀（激光雷達(dá)掃描圖像被稱為幀）構(gòu)建子地圖，下一步將構(gòu)建好的子地圖與激光雷達(dá)掃描幀的進(jìn)行匹配，設(shè)置初始掃描幀位于機(jī)器人初始位姿的原點(diǎn)（0，0），后續(xù)掃描的點(diǎn)集則描述為H＝{hk}k＝1，2，3，4，5，…，k。TζM為轉(zhuǎn)換系數(shù)，激光數(shù)據(jù)幀構(gòu)建子地圖和激光關(guān)鍵幀的過(guò)程就是用轉(zhuǎn)換系數(shù)TζM將掃描點(diǎn)集轉(zhuǎn)換到子地圖的姿態(tài)變換的過(guò)程，通過(guò)Tζ可以嚴(yán)格地將掃描點(diǎn)從掃描幀中映射到子地圖中，這個(gè)過(guò)程可以用（1）式表達(dá)：

在迭代掃描幀建立子地圖后，在子地圖被插入雷達(dá)掃描幀之前，需要對(duì)觀測(cè)到的位姿進(jìn)行優(yōu)化，通過(guò)Ceres 庫(kù)掃描匹配器對(duì)位姿進(jìn)行優(yōu)化，將姿態(tài)求解問(wèn)題轉(zhuǎn)化為非線性最小二乘求解問(wèn)題。如公式（2）所示。

其中Tξ將ξ從雷達(dá)坐標(biāo)系轉(zhuǎn)換到子地圖坐標(biāo)系，通過(guò)對(duì)局部子地圖映射中的概率值使用雙三次插值進(jìn)行平滑處理，并且將局部子地圖中的概率值限制在[0，1]內(nèi)。在求出閉環(huán)掃描幀后，進(jìn)行全局地圖優(yōu)化部分，全局地圖優(yōu)化主要根據(jù)掃描幀之間的姿態(tài)關(guān)系進(jìn)行全局映射優(yōu)化，每個(gè)掃描幀只對(duì)應(yīng)當(dāng)前子地圖進(jìn)行映射匹配構(gòu)成子地圖，多個(gè)子地圖構(gòu)成地圖，隨著子圖的搭建，掃描匹配處理過(guò)程中子地圖、掃描幀之間的誤差會(huì)隨著時(shí)間而累積。在深松機(jī)器人構(gòu)建子地圖后繼續(xù)進(jìn)行激光關(guān)鍵幀數(shù)據(jù)采集，生成回環(huán)檢測(cè)用于改善校準(zhǔn)子地圖信息。為了優(yōu)化所有掃描幀和子圖的位姿，采用了稀疏姿態(tài)的方法調(diào)整子地圖、掃描幀的累計(jì)誤差，將用于閉環(huán)優(yōu)化的掃描幀的相對(duì)姿勢(shì)存儲(chǔ)在存儲(chǔ)器中，當(dāng)子圖不再改變時(shí)，將子地圖和掃描幀構(gòu)成的組用于閉環(huán)檢測(cè)，如果檢測(cè)到子地圖與掃描幀映射匹配良好，則將該組相對(duì)位姿添加到優(yōu)化環(huán)節(jié)中。在優(yōu)化子地圖之間的誤差后，建立地圖。

3 深松機(jī)器人的路徑規(guī)劃算法

全局路徑規(guī)劃采用A* 算法可以讓深松機(jī)在諸多路徑中能夠快速找出最短路徑，在A*算法的路徑規(guī)劃中，利用Close 以及Open 兩個(gè)列表對(duì)節(jié)點(diǎn)進(jìn)行儲(chǔ)存。已經(jīng)被搜索過(guò)，生成估價(jià)值的節(jié)點(diǎn)會(huì)被Open存儲(chǔ)起來(lái)。估計(jì)代價(jià)最小的節(jié)點(diǎn)由Close 存儲(chǔ)起來(lái)，通過(guò)Close 各節(jié)點(diǎn)軌跡做處理，形成移動(dòng)軌跡。估價(jià)函數(shù)的設(shè)置對(duì)A* 算法來(lái)說(shuō)至關(guān)重要，估價(jià)函數(shù)以（3）式子來(lái)表示：

G（n）表示耗散函數(shù)，從起始節(jié)點(diǎn)到節(jié)點(diǎn)n的真實(shí)代價(jià)值；啟發(fā)函數(shù)用H（n）來(lái)代表，研究從當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)值，F(xiàn)（n）表示當(dāng)前節(jié)點(diǎn)的估價(jià)函數(shù)，A*算法采用歐幾里得距離：在二維平面內(nèi)，假設(shè)起始點(diǎn)坐標(biāo)為（Xx，Xy），目標(biāo)點(diǎn)坐標(biāo)（Dx，Dy），則歐幾里得距離的啟發(fā)函數(shù)H（n）表達(dá)式為式（4）全局路徑規(guī)劃的算法具體流程見(jiàn)圖4。

圖4 全局路徑規(guī)劃的算法具體流程

深松機(jī)在工作的時(shí)候工作環(huán)境人煙稀少，但為了避免誤傷到人或動(dòng)物，在全局路徑規(guī)劃的基礎(chǔ)上加入局部路徑規(guī)劃動(dòng)態(tài)窗口法（DWA 算法）實(shí)現(xiàn)實(shí)時(shí)避障。動(dòng)態(tài)窗口法在深松機(jī)移動(dòng)速度空間內(nèi)（v，w）進(jìn)行多組樣本，根據(jù)速度模擬軌跡，通過(guò)軌跡評(píng)價(jià)函數(shù)選取適合用于選擇最優(yōu)運(yùn)動(dòng)軌跡轉(zhuǎn)化為約束當(dāng)前軌跡速度。為了實(shí)現(xiàn)動(dòng)態(tài)障礙物避障與突變點(diǎn)的路徑規(guī)劃，將所有的分割區(qū)域視作待訪問(wèn)節(jié)點(diǎn)，求取經(jīng)過(guò)所有節(jié)點(diǎn)的最短路徑，能夠?qū)崿F(xiàn)最短路徑，采用回溯方法來(lái)實(shí)現(xiàn)求解，運(yùn)用優(yōu)化后的ROS 框架下的move_base 導(dǎo)航棧功能包作為控制機(jī)器人追蹤全覆蓋路徑的模塊。系統(tǒng)結(jié)構(gòu)圖如圖5 所示。

圖5 move_base 導(dǎo)航框架

傳統(tǒng)的全局歷遍路徑規(guī)劃導(dǎo)致機(jī)器人行動(dòng)緩慢深松效率低，參考純追蹤算法的思想，編寫(xiě)目標(biāo)點(diǎn)位發(fā)送程序，實(shí)現(xiàn)目標(biāo)點(diǎn)的動(dòng)態(tài)發(fā)送，算法步驟如下：

（1）根據(jù)當(dāng)前里程計(jì)反饋回來(lái)的位姿數(shù)據(jù)，計(jì)算機(jī)器人到當(dāng)前目標(biāo)點(diǎn)的歐式距離，當(dāng)小于設(shè)定閾值時(shí)候，發(fā)送下一目標(biāo)點(diǎn)；

（2）在路徑回調(diào)函數(shù)中對(duì)接受的路徑進(jìn)行判斷，當(dāng)路徑長(zhǎng)度為0 或者長(zhǎng)度沒(méi)有發(fā)生變化時(shí)候，判定為上個(gè)路徑，不對(duì)系統(tǒng)中存儲(chǔ)的路徑進(jìn)行更新，反之則更新路徑，并從頭追蹤。

4 仿真結(jié)果及分析

4.1 SLAM 建圖

采用cartogrpher 算法進(jìn)行深松機(jī)器人的建圖和定位，通過(guò)2D 雷達(dá)構(gòu)建柵格地圖的過(guò)程如下圖所示，圖6 使用仿真環(huán)境建立柵格地圖過(guò)程?？紤]到田地里田埂的高度，為了避免觸碰翻越田埂，設(shè)置田埂區(qū)域?yàn)榈貓D邊界，設(shè)置地圖邊界的膨脹半徑為11 圖中深色區(qū)域表示不可通行區(qū)域，淺色區(qū)域表示可通行區(qū)域，黑色區(qū)域表示激光雷達(dá)掃描到的障礙物，灰色代表未知區(qū)域。深松機(jī)器人采用cartographer 算法作為SLAM 算法，可以構(gòu)建大型地圖，圖6 中c 圖為深松機(jī)器人進(jìn)行路徑規(guī)劃的基礎(chǔ)，深松機(jī)器人構(gòu)建的地圖邊界清晰且有極高的準(zhǔn)確性。

圖6 柵格地圖建立過(guò)程

4.2 路徑規(guī)劃及避障

基于農(nóng)作物對(duì)土壤含氧量要求的不同以及土地松散度不同的情況，研究出兩種深松機(jī)器人工作模式：（1）大型微耕模式：設(shè)置深松機(jī)器人的路徑稀疏規(guī)劃，快速開(kāi)始路徑規(guī)劃和尋線導(dǎo)航，適用于土壤較為疏松且土壤含氧量較好的環(huán)境；（2）大型深耕模式：設(shè)置深松機(jī)器人的路徑稠密規(guī)劃，適用于土壤較為緊實(shí)且土壤含氧量低的環(huán)境根據(jù)上一節(jié)設(shè)計(jì)的兩種路徑規(guī)劃方式進(jìn)行實(shí)驗(yàn)，深松機(jī)器人可以根據(jù)土壤深松模式的不同執(zhí)行不同的路徑規(guī)劃，現(xiàn)進(jìn)行兩組不同稀疏路徑導(dǎo)航的效果展示，進(jìn)一步來(lái)說(shuō)明深松機(jī)器人實(shí)驗(yàn)的可靠性，路徑規(guī)劃圖如圖7 所示。

圖7 路徑規(guī)劃效果

圖中細(xì)線表示深松機(jī)器人的全局路徑規(guī)劃路線，粗線表示深松機(jī)器人走過(guò)的路徑。由于稠密的全局歷遍路徑規(guī)劃和稀疏的全局歷遍路徑規(guī)劃的運(yùn)作場(chǎng)合過(guò)于特殊，通過(guò)激光雷達(dá)建立柵格地圖，實(shí)現(xiàn)路徑可視化，邊緣處的小點(diǎn)是激光雷達(dá)掃描的田地邊緣，從該界面可以清楚看到田地環(huán)境及障礙物。

5 結(jié)語(yǔ)

用深松機(jī)器人硬件系統(tǒng)設(shè)計(jì)的深松機(jī)器人仿真系統(tǒng)根據(jù)田地情況模擬試驗(yàn)場(chǎng)地進(jìn)行深松機(jī)器人仿真的可行性驗(yàn)證，實(shí)驗(yàn)先采用Cartographer 算法實(shí)現(xiàn)定位與建圖，根據(jù)建立柵格的地圖，采用A* 算法在地圖上進(jìn)行全局路徑規(guī)劃等技術(shù)的研究，確定其可行性。結(jié)果表明：

（1）采用ROS 框架搭建深松機(jī)器人，ROS 的開(kāi)源性和代碼易于移植的優(yōu)勢(shì)為機(jī)器人研發(fā)提供了更多的算法選擇。

（2）采用基于Cartographer 算法實(shí)現(xiàn)環(huán)境中的定位與建圖清晰、有效。

（3）采用基于A*算法在地圖上進(jìn)行路徑規(guī)劃等技術(shù)的研究，能夠完成避障和自主導(dǎo)航的功能。