深松機器人作業環境建模和改進路徑規劃研究

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

（1.天津職業技術師范大學 機器人及智能裝備研究院，天津 300222；2.天津職業技術師范大學 天津市智能機器人技術及應用企業重點實驗室，天津 300222）

0 引言

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

1 深松機器人仿真系統設計

深松機器人在solidwork 軟件中經行機器人模型磚換，根據機器人運動學模型建立坐標軸、連桿和創建continue 類型joint 的旋轉軸并且進行urdf 文件轉換，并導入碰撞系數模型后在ROS 中顯示。

搭建基于ROS 的深松機器人需要在urdf 模型的基礎上優化成xacro 文件，并加入激光雷達、里程計、IMS 模塊，ROS 系統下的軟件可以查看機器人模型，Gazebo 會根據導入模型的運動模型進行動力學仿真，通過Gazebo 模擬器的動態模擬，能夠復現深松機需要的耕地環境，搭建一個80m×80m 的仿真環境，搭建的仿真過程如圖1 所示。

圖1 仿真建立過程

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

圖2 ROS 各節點話題通信

2 深松機器人的Cartographer-SLAM 算法

根據根據深松機器人運動學模型如圖3 所示，可以進一步計算機器人里程計模型下的公式推導。

圖3 機器人運動學模型

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

深松機器人先對輪式里程計和IMS 進行積分處理，得到深松機器人的原始位姿，接著對雷達點云數據進行優化原始的優化，進行濾波后與IMS 和輪式里程計數據融合，做好初始準備后構建地圖。

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

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

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

3 深松機器人的路徑規劃算法

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

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

圖4 全局路徑規劃的算法具體流程

深松機在工作的時候工作環境人煙稀少，但為了避免誤傷到人或動物，在全局路徑規劃的基礎上加入局部路徑規劃動態窗口法（DWA 算法）實現實時避障。動態窗口法在深松機移動速度空間內（v，w）進行多組樣本，根據速度模擬軌跡，通過軌跡評價函數選取適合用于選擇最優運動軌跡轉化為約束當前軌跡速度。為了實現動態障礙物避障與突變點的路徑規劃，將所有的分割區域視作待訪問節點，求取經過所有節點的最短路徑，能夠實現最短路徑，采用回溯方法來實現求解，運用優化后的ROS 框架下的move_base 導航棧功能包作為控制機器人追蹤全覆蓋路徑的模塊。系統結構圖如圖5 所示。

圖5 move_base 導航框架

傳統的全局歷遍路徑規劃導致機器人行動緩慢深松效率低，參考純追蹤算法的思想，編寫目標點位發送程序，實現目標點的動態發送，算法步驟如下：

（1）根據當前里程計反饋回來的位姿數據，計算機器人到當前目標點的歐式距離，當小于設定閾值時候，發送下一目標點；

（2）在路徑回調函數中對接受的路徑進行判斷，當路徑長度為0 或者長度沒有發生變化時候，判定為上個路徑，不對系統中存儲的路徑進行更新，反之則更新路徑，并從頭追蹤。

4 仿真結果及分析

4.1 SLAM 建圖

采用cartogrpher 算法進行深松機器人的建圖和定位，通過2D 雷達構建柵格地圖的過程如下圖所示，圖6 使用仿真環境建立柵格地圖過程。考慮到田地里田埂的高度，為了避免觸碰翻越田埂，設置田埂區域為地圖邊界，設置地圖邊界的膨脹半徑為11 圖中深色區域表示不可通行區域，淺色區域表示可通行區域，黑色區域表示激光雷達掃描到的障礙物，灰色代表未知區域。深松機器人采用cartographer 算法作為SLAM 算法，可以構建大型地圖，圖6 中c 圖為深松機器人進行路徑規劃的基礎，深松機器人構建的地圖邊界清晰且有極高的準確性。

圖6 柵格地圖建立過程

4.2 路徑規劃及避障

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

圖7 路徑規劃效果

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

5 結語

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

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

（2）采用基于Cartographer 算法實現環境中的定位與建圖清晰、有效。

（3）采用基于A*算法在地圖上進行路徑規劃等技術的研究，能夠完成避障和自主導航的功能。