林木聯(lián)合采育機機械臂避障路徑規(guī)劃*

楊英浩 劉晉浩 鄭一力 黃青青

(1.北京林業(yè)大學工學院 北京 100083； 2.北京林業(yè)大學林業(yè)與環(huán)境特種裝備研究所 北京 100083)

森林既是人類賴以生存的生態(tài)系統(tǒng)，也是人類獲取林業(yè)資源的重要途徑(Brockerhoffetal.，2008； Ratcliffeetal., 2015)。新中國成立初期，我國森林面積約0.83億hm2，經(jīng)過60多年的艱苦努力，2018年森林面積已達2.08億hm2，其中人工林面積0.79億hm2，居世界首位1)(1)1)http:∥www.forestry.gov.cn/main/195/20181228/094216171727273.html.。林業(yè)產(chǎn)業(yè)是國民經(jīng)濟發(fā)展的重要組成部分，2018年我國林業(yè)主產(chǎn)品產(chǎn)值達7.63萬億元2)(2)2)http:∥www.forestry.gov.cn/main/62/20200427/150949147968678.html.； 但是我國人均森林面積少，且人工林生產(chǎn)力較低，平均每公頃森林蓄積量89.8 m3，僅為世界平均水平的68.7%(彭云龍等，2018； 許曉東等，2020)。森林作業(yè)是獲取林業(yè)產(chǎn)業(yè)的重要手段，科學開展林間撫育采伐可以改善林分結構、提升林木質(zhì)量、減少無益競爭、加速林木生長、提高木材利用量(徐海燕等，2017； 周國相等, 2004)，而先進的林業(yè)技術裝備是森林作業(yè)的支撐和保障(周玉申，2011)。在國外，Morales等(2014a)針對林木集材機械臂的軌跡規(guī)劃等問題，考慮機械臂的動作時間、關節(jié)速度和最大角度，提出了時間分解的運動控制算法，機械臂的軌跡規(guī)劃效率有效提高； Hera等(2019)在林業(yè)機械臂中加入傳感器記錄每次運動軌跡，通過大量軌跡數(shù)據(jù)，可在重復動作中評估出最佳軌跡； Dong等(2019)比較了2種類型集材車并提出能量最佳軌跡規(guī)劃，節(jié)省了運動能量。在國內(nèi)，鮑際平等(2009)研發(fā)出了首臺林木聯(lián)合采育機CFJ30，在桉樹(Eucalyptus)人工林作業(yè)試驗中，實現(xiàn)了撫育采伐、打枝去皮、造材歸楞等功能，但是在機械臂移動和伐木頭校準環(huán)節(jié)，由于視野遮擋、采伐臂震動、自然環(huán)境復雜等因素，需要操作人員反復觀測判斷路徑是否碰撞以及手動調(diào)節(jié)采伐頭是否校準目標。判斷校驗時間占單位采伐周期的2/3以上，單位采伐周期內(nèi)的無功時間增加，既降低了作業(yè)效率，同時采伐頭反復對準目標也造成系統(tǒng)燃燒損耗，提高了作業(yè)成本(戚春華等，2003； 劉延鶴等，2020)。國外林業(yè)機械臂研究主要集中在軌跡規(guī)劃(trajectory planning)上，以降低能耗為目的，在路徑規(guī)劃上只是簡單軌跡運動； 國內(nèi)林業(yè)機械集中于結構研究(廖旭恩，2019； 耿志斌等，2020)，以期從機械結構上的優(yōu)化來提高作業(yè)水平； 而且，林業(yè)機械操作許可證獲取困難，需要大量的培訓與工作經(jīng)驗(Moralesetal.，2014b)。因此，發(fā)展一種林木聯(lián)合采育機機械臂避障路徑規(guī)劃算法以提高機械作業(yè)效率，具有十分重要的理論和現(xiàn)實意義。

機器人避障運動規(guī)劃(motion planning)是指通過對應的算法，在存在障礙的空間搜索規(guī)劃出滿足機器人自身約束和環(huán)境約束的無碰撞路徑(余卓平等，2017)。Lavalle等(1998)提出了快速搜索隨機樹(rapidly-exploring random trees，RRT)算法，該算法能夠避開障礙物得到最優(yōu)路徑，且在理論規(guī)劃時間足夠長的情況下可以確保找到一條最優(yōu)路徑。Urmson等(2003)提出了啟發(fā)式搜索hRRT，該算法能夠提高搜索路徑的效率。Weghe等(2007)提出了JT-RRT算法，可保證機械臂末端向目標方向搜索。姬偉等(2013)改進了人工勢場法，使蘋果采摘機器臂逃離局部值，從而實現(xiàn)自主避障。Kim等(2014)將任務需求考慮進RRT算法內(nèi)，加快了雙臂機器人的規(guī)劃速度。朱宏輝等(2017)提出了加入規(guī)避步長延伸法的改進RRT*算法，但對于復雜狹小路徑不容易獲得較優(yōu)解。祝敬等(2018)選用球形障礙物，采用RRT方法解決了機械臂規(guī)劃面臨局部值的問題。陳滿意等(2019)在多障礙物中運用RRT算法逃離人工勢場法的最小值，并且加入貝塞爾函數(shù)進行了軌跡優(yōu)化。但是林區(qū)作業(yè)與工業(yè)存在部分差異：1) 林區(qū)環(huán)境更加復雜，目標立木與障礙立木沒有規(guī)律可循，避障路徑完全由算法獲得。林木聯(lián)合采育機工作時，需要在一片林區(qū)“撫育伐”完成后移動到另一片林區(qū)，目標立木與障礙立木等工作環(huán)境屬于動態(tài)變化，每當移動到新的環(huán)境，采伐臂需要重新規(guī)劃路徑，傳統(tǒng)的搜索算法效率降低或不能完成任務。2) 立木的近似建模不同。在工業(yè)機器人中，多數(shù)將障礙物簡化為長方體包絡； 而在林區(qū)，樹冠大多為自然圓頭形，樹干也并非垂直于地面，簡單長方體包絡建模并不能精確描繪樹木形態(tài)。

鑒于此，本研究針對林木聯(lián)合采育機在林區(qū)復雜環(huán)境中的避障作業(yè)要求，提出一種新型機械臂避障路徑規(guī)劃方法，以解決RRT算法耗時長、隨機性強、存在奇異解的問題。首先對機械臂進行D-H(Denavit-Hartenberg)建模(Hartenbergetal.，1955)，求得機械臂的正逆解； 然后采用A-RRT算法進行目標點搜索，運用三次B樣條曲線進行平滑擬合； 最后按照機械臂最小能量法對每個點求逆解，使用多點碰撞檢測是否碰撞。通過仿真試驗，證明該方法的可行性和有效性，且能夠減少規(guī)劃時間，保證路徑平滑。本研究基于林業(yè)聯(lián)合采育機機械臂提出的避障算法，實現(xiàn)了機械臂避障自動規(guī)劃，可為林區(qū)環(huán)境自主作業(yè)提供理論參考。

1 研究方法

1.1 機械臂運動學分析

1.1.1 機械臂模型與D-H參數(shù) 本研究對象為北京林業(yè)大學研發(fā)的林木聯(lián)合采育機(圖1a)，其SoildWorks裝配圖如圖1b所示。避障規(guī)劃模型為林木聯(lián)合采育機機械臂，該模型由6個關節(jié)串聯(lián)組成，其中前5個關節(jié)為旋轉關節(jié)，第6個關節(jié)為移動關節(jié)。圖1c為D-H模型坐標系。

1.1.2 正向運動學 根據(jù)模型，建立D-H參數(shù)如表1所示。

圖1 林木聯(lián)合采育機工作原理示意Fig. 1 Working principle and schematic diagram of forestry felling & cultivation machinea. 林木聯(lián)合采育機工作試驗Working experiment of machine； b. 林木聯(lián)合采機SoildWorks裝配圖The SoildWorks assembly drawing of machine； c. 機械臂D-H模型坐標系D-H model coordinate system of manipulator.

表1 機械臂模型參數(shù)①Tab.1 Manipulator model parameters

MDH的變換矩陣為：

(1)

1.1.3 逆向運動學 機械臂運動學逆解(inverse kinematics)為已知末端位姿，求解機械臂關節(jié)的角度，其方法有代數(shù)數(shù)值法(Dubeyetal.，1991； Chanetal.，1995)、迭代法(Oyamaetal.，2001； Xiaetal.，2001)和幾何法等(Shengetal.，2006； Mohamedetal.，2009)。代數(shù)數(shù)值法存在累計誤差，控制不準確；迭代法計算量大，有時只能得出一組解；幾何法一般求解困難，不具有通用性，但從控制角度來說其精度高、計算速度快。林木聯(lián)合采育機機械臂不滿足Pieper準則(熊有倫，1996)，不存在封閉解，故本研究采用幾何法求機械臂逆解。

1.2 規(guī)避RRT算法

1.2.1 路徑搜索 路徑搜索(path planning)是指規(guī)劃出機械臂終端機構(end-effector)兩點之間的空間路徑，并要求終端機構不能碰撞障礙物。經(jīng)典RRT搜索算法是LaValle等(1998)提出的基于隨機采樣的路徑規(guī)劃算法，該算法存在3點不足：1) 采樣隨機性強，搜索路徑時間較長； 2) 路徑節(jié)點均為隨機獲取，節(jié)點通過直線連接，路徑不夠平滑； 3) 路徑步長固定，障礙物邊緣規(guī)劃時間花費更長。

針對經(jīng)典RRT算法的不足，本研究引入改進人工勢場(artificial potential field)思想(劉成菊等，2013)，提出A-RRT算法：在搜索過程中加入引力函數(shù)，引導搜索樹朝著目標方向搜索； 在搜索樹距離障礙物一定范圍時，加入斥力函數(shù)和限制函數(shù)，減少引力函數(shù)的引力，引導搜索樹避開障礙物。具體算法流程如下。

在機械臂運動空間建立人工勢場，某個q點的能量函數(shù)為引力函數(shù)與斥力函數(shù)之和：

W(q)=Uat(q)+Urep(q)。

(2)

式中：Uat(q)代表引力函數(shù)；Urep(q)代表斥力函數(shù)。

引力函數(shù)Uat(q)表達式如下：

(3)

式中：kp為引力增量；qgoal為目標點；xnear為RRT隨機采樣最近的點。

斥力函數(shù)Urep(q)表達式如下：

(4)

式中：ks為斥力增量；D為當前點xnear距離障礙物的距離；D0為障礙物的作用距離的閾值。

(5)

根據(jù)式(3)得xnear處引力函數(shù)為：

(6)

限制函數(shù)S如下：

(7)

式中：Ri為障礙物半徑；kx為限制增量，且大于0小于1。在多障礙物環(huán)境中，S為限制函數(shù)的連乘。

在滿足障礙物閾值的條件下，根據(jù)式(4)得斥力函數(shù)和為：

(8)

式中：xnear為隨機樹中上一個節(jié)點坐標；oi代表障礙物坐標；n代表滿足條件的障礙物個數(shù)；Di0的閾值設為障礙物Ri的2倍。

根據(jù)式(5)-(8)得改進算法后新的節(jié)點公式為：

(9)

式中：xnew為下一個搜索的點，由隨機采樣點、斥力函數(shù)和引力函數(shù)3個矢量和乘步長step得到。

加入斥力函數(shù)和引力函數(shù)能夠增加采樣點的有效性，提高狹窄空間的規(guī)劃效率，平滑采樣路徑。為進一步提高規(guī)劃效率和復雜環(huán)境適應能力，本研究采用目標偏執(zhí)雙向隨機樹。隨機樹的生長過程如圖2所示。

1.2.2 路徑優(yōu)化 A-RRT算法搜索得出的避障路徑由多條直線段組成，線段連接并不平滑，機械臂在運動過程會出現(xiàn)抖動等問題。本研究運用三次B樣條(B-spline)算法進行擬合路徑(趙玉剛，2012)，搜索路徑采用準均勻三次B樣條曲線擬合后，解決了機械臂抖動問題。

圖2 改進RRT算法示意Fig. 2 Schematic diagram of improved RRT algorithm

1.3 機械臂關節(jié)角度優(yōu)化與碰撞檢測

1.3.1 最小能量法 本研究機械臂避障基于笛卡爾空間(Cartesian space)，末端位姿由路徑規(guī)劃算法得出，分別求逆解計算出機械臂各關節(jié)角度，機械臂末端沿著路徑運動(尹斌，2014)。機械臂末端位姿有多組逆解，針對避障路徑出現(xiàn)“關節(jié)漂移”等奇異解問題，提出機械臂能量最小方法，即機械臂空間搜索每一步長的最小能量解是每個關節(jié)角度變化最小的解。滿足以下條件：

(10)

式中：θi為機械臂的8組逆解； ‖…‖2表示構型空間的l2范數(shù)；θ′為θi的父節(jié)點逆解；θ為關節(jié)角度變化最小的解。

1.3.2 障礙物模型 由于樹干、樹冠等為不規(guī)則集合模型，空間占位描述復雜，花費大量時間建模會影響機械臂避障規(guī)劃時間； 而對立木和障礙物采用包圍盒法(bounding box)近似建模，可以很大程度提高計算速度。樹木的樹冠形狀類似球體，用大的球體代替，樹干橫截面為圓形，用多個球體疊加代替。圖3中紅色區(qū)域代表近似障礙物。用球體建?？梢愿泳珳拭枥L樹木形態(tài)，也更加符合樹木生長情況，同時能夠保證路徑規(guī)劃的安全性。

圖3 障礙物球體包絡幾何模型Fig. 3 Model of obstacles spherical envelope

1.3.3 多點碰撞檢測法 機械臂具有高耦合性(high coupling)、非線性等特點，路徑規(guī)劃雖然可保證機械臂終端機構不會發(fā)生碰撞，但還需要檢測機械臂整體是否與障礙物碰撞(汪首坤等，2011)。圖4中，障礙物簡化成多個球體建模，機械臂簡化成圓柱體模型。oi(x,y,z)為障礙物的世界球形坐標，半徑為Ri。機械臂的連桿長為Li，橫截面半徑為r，d為機械臂中線到障礙物球體的距離。

圖4 包絡幾何模型Fig. 4 Bounding box geometry

為了解決傳統(tǒng)碰撞檢測時求解點到軸線段的最短距離時間過長問題，本研究提出多點法碰撞檢測。機械臂的連桿長為Li，從一端固定等距插值10個點，每個點分別計算與障礙物的距離。對于連桿Li的第n節(jié)與n-1節(jié)的末端位姿為：

(11)

(12)

則每個連桿L的10個檢測點為：

[Pn(t)]=([Ttn]-[Ttn-1])/t+[Ttn-1]

(n=1,2,3,4,5,6；t=1,2…10)。

(13)

式中：[Ttn]-[Ttn-1]為連桿末端相對于起點的向量。

等距分割加上起點坐標即可獲得連桿上檢測的10個點坐標，代入下式即求得距離：

di(t)=Pn(t)-oi(t=1,2,…10)。

(14)

求出10個點距離障礙物oi的距離di(t)(t=1,2,…10)，若di(t) >r+1.1×R，則機械臂連桿與障礙物無碰撞； 若di(t) ≤r+1.1×R，則機械臂與障礙物碰撞。相比經(jīng)典點到線段最短距離判斷機械臂碰撞，本研究提出的多點碰撞檢測能夠避免解方程等一系列耗時運算，提高計算效率。

2 算法流程

算法流程簡圖如圖5所示。第一步,獲取環(huán)境中起始點、目標點和障礙物的位姿； 第二步，進行A-RRT路徑搜索，得到路徑； 第三步，對路徑進行三次B曲線擬合； 第四步，采用能量最小準則得到關節(jié)角的最小逆解； 最后，將關節(jié)角度輸入機械臂進行多點碰撞檢測。

圖5 算法流程Fig. 5 Schematic diagram of algorithm flow

3 仿真試驗結果與分析

為了驗證本研究提出算法在笛卡爾空間中機械臂避障規(guī)劃的可行性和有效性，共分為2個試驗：試驗一采用Matlab和Robotics Toolbox聯(lián)合仿真驗證算法的可行性； 試驗二在運算時間和迭代步數(shù)方面，比較改進后的RRT算法與其他算法，從而驗證算法的有效性。算法環(huán)境為Matlab2018b，計算機處理器為Intel Core i5-8300H，內(nèi)存16 GB。試驗相關參數(shù)如表2所示。

表2 機械臂試驗參數(shù)Tab.2 Experimental parameters of the manipulator

3.1 試驗一：機械臂算法可行性試驗

為了驗證本研究理論的可行性，采用機器人工具箱建模得到機械臂。機械臂模型為林木聯(lián)合采育機機械臂，其D-H參數(shù)見表1，與實物參數(shù)一致。障礙環(huán)境分為4種情況：環(huán)境一包含1個障礙物，障礙物原點坐標為(5，50，15)，半徑為22 dm； 環(huán)境二包含2個障礙物，障礙物原點坐標分別為(35，50，20)、(-35，40，1)，半徑依次為18、12 dm； 環(huán)境三包含3個障礙物，障礙物原點坐標為(58，40，20)、(0，55，8)、(-52，45，-10)，半徑依次為15、12、12 dm； 環(huán)境四包含4個障礙物，障礙物原點坐標分別為(45，30，30)、(20，48，25)、(-16，35，18)、(-47，45，-8)，半徑依次為13、12、10、12 dm。圖6中紅色點為初始點，綠色點為目標點，分別從1～4個障礙環(huán)境中驗證試驗。避障算法為本研究提出算法，其中藍色為Tree1搜索路線，紅色線為Tree2搜索路線，青色線為避障路線，黑色線是經(jīng)過三次B樣條曲線擬合的機械臂末端路線。

圖6中左圖為機械臂運動空間的整個畫面，右面為路徑部分細節(jié)?？梢钥闯?，加入B樣條曲線可以使得路徑更加平滑，更適合機械臂運動。圖7分別對應圖6中4個環(huán)境的機械臂關節(jié)角度變化。

圖6 機械臂在不同障礙環(huán)境中仿真Fig. 6 Simulation of the manipulator in different obstacle environments

圖7 機械臂關角度變化曲線Fig. 7 Curve of joint angle change of manipulator

由圖7可知，機械臂避開障礙物角度發(fā)生了變化，距離障礙物到達一定范圍，采樣點受斥力函數(shù)和引力函數(shù)的共同作用，使得下個采樣點取值遠離障礙物，從而達到避障目的。4個環(huán)境起始點、目標點都是同一點，所有關節(jié)角度起始點、末尾點的位姿也相同。結果表明，本研究提出的避障算法能夠引導機械臂成功避障，且關節(jié)角度相對平滑，證明算法可行。

3.2 試驗二：改進路徑規(guī)劃算法有效性試驗

在Matlab中建立100×100×100空間，如圖8所示依次為不同障礙物地圖。分別在4個不同場景進行測試，與之比較的還有4種不同算法，分別為經(jīng)典RRT(LaValleetal.，1998)、節(jié)點偏置hRRT(Urmsonetal.，2003)、雙項bi-RRT(由弘揚等，2019)和引力LRRT(劉成菊等，2017)。使用每種算法在相同地圖進行10次避障規(guī)劃，記錄每次規(guī)劃時間和規(guī)劃步數(shù)。

由于試驗數(shù)據(jù)較多，選取2個障礙圖進行搜索圖形展示，結果如圖9所示。

試驗二中的具體數(shù)據(jù)如表3所示。

圖10為表3的雙y折線圖，因迭代步數(shù)差距太大，右側y值迭代次數(shù)采用log進行化簡。紅線至黑線依次為所使用的算法，實線為算法花費時間，與實線相對應顏色的虛線為該算法迭代次數(shù)。

圖8 4種不同的障礙物地圖Fig. 8 Four different obstacle maps

表3 算法花費時間與迭代步數(shù)對比Tab.3 Comparison of algorithm time and iteration steps

根據(jù)數(shù)據(jù)分析得出，經(jīng)典RRT算法在沒有任何優(yōu)化情況下搜索目標花費時間相對較多，無論是簡單環(huán)境還是復雜環(huán)境，花費時間和采樣節(jié)點的有效性均相對較差。偏置RRT算法花費時間較少，偏置RRT少障礙物是最優(yōu)解，能沿著目標偏置搜索，但是其面臨與A*算法一樣的弊端，多障礙物容易陷入局部解，應對復雜環(huán)境搜索性能下降。LRRT算法加入引力分類，能進一步提高搜索效率，但是單一引力容易與偏置RRT算法一樣陷入局部值。幾乎所有算法在地圖1環(huán)境中花費時間較多，原因是環(huán)境1障礙物在2個目標點中間，如果沒有斥力函數(shù)加入，偏置采樣點容易落在障礙物內(nèi)，造成搜索時間增加。雙向隨機數(shù)加入多個節(jié)點，提供了更多接觸條件，能夠總體提高搜索效率。本研究算法在雙向隨機數(shù)基礎上加入斥力函數(shù)、引力函數(shù)和限制函數(shù)，使得雙向隨機數(shù)采樣點避開障礙物引導目標點，在多種環(huán)境中無論是花費時間還是迭代次數(shù)均是最小值。相比于經(jīng)典RRT算法、偏置RRT算法、引力RRT與雙向RRT算法，改進后算法效率分別提高約95.9%、56.9%、52.7%和40.5%。

圖10 算法花費時間與迭代步數(shù)對比Fig. 10 Comparison of algorithm time and iteration steps

圖11 Matlab仿真以及關節(jié)角度變化Fig. 11 Matlab simulation and joint angle changesa

4 虛擬林木聯(lián)合采育機仿真及其結果

為了驗證算法的可行性，本研究結合機器人操作系統(tǒng)(robot operating system，ROS)聯(lián)合試驗。試驗模型為圖1b的SoildWorks模型，將模型中的液壓及其附屬部分刪除，避免碰撞檢測結果受到影響。通過URDF插件，將模型轉化成機械人通用的URDF格式。試驗環(huán)境設置為2個障礙物，障礙物原點坐標分別為(35，50，20)、(-35，40，1)，半徑依次為18、13 dm。首先在Robot toolbox中運行得到各關節(jié)變化角度，如圖11所示。

將得到的關節(jié)變化角度保存，在ROS環(huán)境中相同位置建立與Robot toolbox相同障礙物的環(huán)境。通過ROS中MoveIt的最優(yōu)時間算法將試驗得到的多個角度規(guī)劃成一條路徑，并驗證是否算法試驗成功，如果機械臂碰撞則MoveIt停止工作。圖12為MoveIt試驗圖，圖13為機械臂運動各關節(jié)角度的變化。

圖12 MoveIt中的林木聯(lián)合采育機運動Fig. 12 The movement of the machine in MoveIte

圖11b與圖13關節(jié)角度相比較，前20～22 s以及最終到達目標點關節(jié)角度不同是因為在ROS環(huán)境中林木聯(lián)合采育機的建模初始值與Matlab不同，相差±π/2。圖11b中θ1、θ2、θ3、θ4與圖12中的變化趨勢幾乎一樣，在相同障礙物環(huán)境下，能夠引導機械臂避開障礙物。從圖13仿真可以看出，本研究提出算法可以引導林木聯(lián)合采育機機械臂避開障礙物并到達目標點。

5 結論

本研究針對林木聯(lián)合采育機機械臂在笛卡爾空間避障規(guī)劃的特點，在傳統(tǒng)RRT避障算法基礎上做出以下改進：1) 提出規(guī)避RRT算法，在搜索隨機樹中引入斥力函數(shù)、引力函數(shù)和限制函數(shù)，提高了搜索路徑的效率； 2) 構建機械臂構型空間中l(wèi)2型范數(shù)，提出最小能量法，保證關節(jié)角度的連續(xù)性，解決了機械臂奇異解問題； 3) 建立多點碰撞檢測公式，提出多點碰撞檢測法，提高了碰撞檢測的運算速度。

仿真結果表明，本研究提出算法能夠引導林木聯(lián)合采育機機械臂完成避障規(guī)劃。在搜索路徑中，A-RRT相比于其他RRT算法提高了搜索節(jié)點效率，降低了搜索時間。本研究提出算法可以滿足避障效果，為林木聯(lián)合采育機在林中撫育采伐、造材歸楞等功能提供理論支持，從而加快林業(yè)機械發(fā)展。在后續(xù)工作中，將會研究在搜索過程中避開奇異解，進一步提高總體避障效率。

圖13 MoveIt中機械臂運動的角度變化Fig. 13 The angle change of the manipulator movement in MoveIt