深松機(jī)器人作業(yè)環(huán)境建模和改進(jìn)路徑規(guī)劃研究

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

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

0 引言

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

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

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

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

圖1 仿真建立過程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

深松機(jī)在工作的時候工作環(huán)境人煙稀少，但為了避免誤傷到人或動物，在全局路徑規(guī)劃的基礎(chǔ)上加入局部路徑規(guī)劃動態(tài)窗口法（DWA 算法）實現(xiàn)實時避障。動態(tài)窗口法在深松機(jī)移動速度空間內(nèi)（v，w）進(jìn)行多組樣本，根據(jù)速度模擬軌跡，通過軌跡評價函數(shù)選取適合用于選擇最優(yōu)運(yùn)動軌跡轉(zhuǎn)化為約束當(dāng)前軌跡速度。為了實現(xiàn)動態(tài)障礙物避障與突變點的路徑規(guī)劃，將所有的分割區(qū)域視作待訪問節(jié)點，求取經(jīng)過所有節(jié)點的最短路徑，能夠?qū)崿F(xiàn)最短路徑，采用回溯方法來實現(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ī)器人行動緩慢深松效率低，參考純追蹤算法的思想，編寫目標(biāo)點位發(fā)送程序，實現(xiàn)目標(biāo)點的動態(tài)發(fā)送，算法步驟如下：

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

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

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

4.1 SLAM 建圖

采用cartogrpher 算法進(jìn)行深松機(jī)器人的建圖和定位，通過2D 雷達(dá)構(gòu)建柵格地圖的過程如下圖所示，圖6 使用仿真環(huán)境建立柵格地圖過程。考慮到田地里田埂的高度，為了避免觸碰翻越田埂，設(shè)置田埂區(qū)域為地圖邊界，設(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 柵格地圖建立過程

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

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

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

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

5 結(jié)語

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

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

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

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