基于關節構形空間的混聯采摘機械臂避障路徑規劃

陽涵疆，李立君，高自成

?

基于關節構形空間的混聯采摘機械臂避障路徑規劃

陽涵疆，李立君※，高自成

（中南林業科技大學機電工程學院，長沙 410000）

針對混聯采摘機器人在非結構性環境中進行避障采摘作業的要求，該文提出了一種基于關節構形空間的混聯采摘機械臂避障路徑規劃算法。根據機械臂和障礙物的幾何特征，對機械臂及障礙物模型進行合理簡化，通過分析末端執行器目標點和串聯機械臂結構參數選取合適的并聯機械臂動平臺目標點，然后采用遍歷法構建串聯機械臂關節構形空間，并利用快速擴展隨機樹（rapidly-exploring random tree，RRT）算法搜尋串聯機械臂無撞路徑，再通過同樣的方法獲得并聯機械臂關節空間障礙物映射模型和無撞路徑，最后綜合串、并聯機械臂的無撞路徑，獲得混聯機械臂整體的避障路徑。仿真和試驗結果表明，文中所提出的算法搜索的避障路徑能夠驅動采摘機械臂避開工作空間內的障礙物，引導末端執行器到達目標點。

機器人；機械臂；算法；避障；混聯；路徑規劃；關節構形空間

0 引 言

果實采摘機器人通常在非結構性的復雜自然環境下作業，分布在成熟果實周圍的樹枝，不成熟果實和其他雜物就成為阻礙采摘機械臂運動的障礙物，給機器人自動采摘作業造成很大的困難。因此，為使得采摘機器人能夠順利完成果實采摘作業任務，機械臂避障路徑規劃成為果實采摘機器人的關鍵技術問題之一[1-3]。

多自由度機械臂避障路徑規劃是在給定障礙物以及機械臂起始、目標位姿的條件下，搜尋一組連續的關節角度值序列，該角度序列能夠驅動機械臂安全、無碰撞地從起始位姿運動到目標位姿[4]。多自由度機械臂避障路徑規劃通常采用關節構形空間法[5-8]，該方法以機械臂的關節值為坐標建立關節構形空間，再將障礙物映射到關節構形空間以構建關節構形障礙，關節構形障礙的補集即為無撞空間，然后利用搜索算法在無撞空間中尋找連接機械臂初始位形和目標位形的無撞路徑，能夠有效避免人工勢場法[9]所存在的局部最小點和復雜障礙物環境下產生振蕩的問題。

目前，國內外學者對機械臂避障路徑規劃問題開展了大量研究，取得了一系列成果[10-17]。蔡健榮等[18]利用概率地圖法（probabilistic roadmap method，PRM）對多自由度機械臂進行避障路徑規劃，該方法無需求取障礙物在機械臂關節構形空間中的精確映射模型，只要通過采樣獲得部分機械臂關節構形空間無撞點，通過連接無撞點而獲得機械臂避障路徑，但受限于探索步數、搜索時間等因素，無法窮盡所有可能的路徑；姚立健等[19]對一種茄子收獲機械臂進行了避障路徑規劃，算法將障礙物映射到收獲機械臂關節構形空間中，在關節構形空間中利用A*算法搜尋避障路徑，但該方法為簡化路徑規劃模型將空間障礙物投影到平面中，無法充分發揮高自由度采摘機械臂在避障方面的優勢；尹建軍等[20]將高自由度機械臂避障路徑規劃問題轉化為多個平面2轉動關節機械臂避障問題，但算法需要對高自由度機械臂整體進行逆運動學分析，并在每個無障礙的平面內搜尋關節角度，算法前期理論分析復雜，通用性不強。綜上可以看出，構形空間法雖然在采摘機械臂避障路徑規劃問題上得到了一定的應用，但尚未提出一種通用的高自由度混聯機械臂避障路徑規劃算法。

本文針對6自由度混聯采摘機械臂的構形特點，對混聯采摘機械臂及障礙物模型進行合理簡化；根據末端執行器目標點和串聯機械臂結構參數選取并聯機械臂動平臺目標點；采用遍歷法構建障礙物在串聯機械臂關節構形空間中的映射模型，并利用快速擴展隨機樹（rapidly-exploring random tree，RRT）算法搜索串聯機械臂無撞路徑；然后通過同樣的方法獲得并聯機械臂關節空間障礙物映射模型和無撞路徑；綜合串、并聯機械臂的無撞路徑獲得混聯機械臂整體的避障路徑；最后通過仿真和試驗驗證所提出算法的可行性和有效性。

1 路徑規劃碰撞模型

1.1 混聯機械臂運動學模型

圖1為2P4R6自由度混聯采摘機器人機械臂三維模型，該機械臂是在碼垛并聯機械臂的動平臺上串接一個3自由度串聯機械臂而構成的，主要包括腰部、手臂和腕部[21]3大模塊。機械臂能夠實現以下6種關節運動：腰部旋轉運動，手臂水平滑塊和豎直滑塊的平移運動以及腕部的3個旋轉運動。

機械臂結構形式如圖2所示，從圖中可知，由于腕部串聯機械臂末端旋轉關節僅驅動末端執行器繞其自身軸線旋轉，不影響末端執行器位置，因此，本文算法主要通過控制關節1（1）至關節5（5）的運動來進行避障路徑規劃。

1.2 障礙物模型

由于實際采摘環境中的障礙物通常不是規則的幾何體，難以用精確的模型來描述，因此本文利用軸對齊包圍盒（axis-aligned bounding box，AABB）來近似建模。這種建模方法雖然在一定程度上擴大了障礙域，但是大大簡化了障礙域的描述和機械臂與障礙物干涉檢測的計算量，有效地提高了路徑規劃的效率，同時也使得所規劃的路徑具有更高的安全性。

障礙物可以采用長方體包絡描述為(s,e)，s(s,s,s)為長方體的某頂點，e(e,e,e)為s的對角頂點，其中s，s，s分別為障礙物在基礎坐標系，，軸方向的最小坐標，e，e，e分別為障礙物在基礎坐標系，，軸方向的最大坐標。

2 基于關節構形空間的混聯機械臂避障路徑規劃

混聯采摘機械臂是在并聯機械臂的動平臺上擴展串聯機械臂得到的，利用關節構形空間法直接對高自由度混聯機械臂進行避障路徑規劃，所構建的關節構形空間維度高，幾何意義不明顯，且高維空間中搜索無撞路徑計算量大。因此，將高自由度混聯機械臂避障路徑規劃問題，拆分成串聯和并聯機械臂避障路徑規劃子問題，降低關節構形空間的維度，減少路徑規劃整體耗時。

前面提到本文只研究串聯機械臂的2個自由度，而并聯機械臂具有3個自由度，所以并聯機械臂具有比串聯機械臂維度更高的關節構形空間，這就使得并聯機械臂路徑規劃耗時更長。因此，為提高算法整體效率，先對串聯機械臂進行路徑規劃，找到合適的動平臺目標點后，再對并聯機械臂進行避障路徑規劃。

2.1 關節構形空間理論基礎

對于自由度機械臂，(1,2,…, ξ)為該機械臂一組主動關節值（由電機直接驅動的關節），其中，ξ為機械臂第個主動關節的轉動角度或位移量，(°)或mm，若表示機械臂上的一點，則該點與障礙物的相對位置關系可以表示為

式中(())為機械臂在關節值所驅動的位姿下，該機械臂上點與障礙物的相對位置關系；()為機械臂在所驅動的位姿下，該機械臂上點在基礎坐標系中的位置坐標，mm。

若將機械臂各桿件按照步長link，mm，均勻離散成個點，個離散點構成點集link，則關節值驅動的機械臂整體與障礙物的相對位置關系可以表示為

式中Γ()為關節值所驅動的機械臂與障礙物的相對位置關系。

由式（1）、式（2）可知當且僅當Γ()=0時機械臂與障礙物不發生干涉。

以自由度機械臂各關節值為坐標建立關節構形空間，若機械臂關節值的集合用表示，則關節構形空間中的任意一點都唯一對應著集合中的某一元素，反之亦正確，即關節構形空間與集合為雙射的。

因此由關節值驅動的機械臂與障礙物的相對位置關系在關節構形空間中的映射可以通過下式表示

式中()為關節構形空間中以機械臂關節值中元素為坐標的點的值，記錄了當機械臂處于對應的位姿時與障礙物的相對位置關系，機械臂與障礙物發生干涉則()等于1，否則()為0。

那么集合part(1,2, …,E)可表示機械臂一組連續變化的位姿序列，同時該集合表示關節構形空間中的一組離散點，若連接該組離散點可獲得一條路徑，其中，為路徑控制點個數。當且僅當滿足下式時，機械臂在由關節值集合part驅動時與障礙物不發生碰撞。

式中為機械臂由關節值集合part驅動時，機械臂與障礙物的相對位置關系。

因此對機械臂進行避障路徑規劃，即在關節構形空間中搜尋一個關節值的集合part，使這個集合連接機械臂初始位形關節值s和目標位形關節值g，且滿足如式（4）所示的路徑無撞條件。

2.2 串聯機械臂的避障路徑規劃

2.2.1 初選并聯機械臂動平臺目標點

給定末端執行器的目標點ge(gex,gey,gez)，根據目標點坐標和串聯機械臂結構參數，確定空間中一點作為并聯機械臂動平臺目標點gp(gpx,gpy,gpz)。動平臺目標點gp位置坐標公式為

式中gpx、gpy和gpz分別為gp在基礎坐標系、和軸方向的坐標值，mm；gex、gey和gez分別為ge在基礎坐標系、和軸方向的坐標值，mm；4、5分別為關節4和關節5的旋轉角度，(°)；rand4、rand5為范圍在0～1的隨機數。

由于選取的動平臺目標點ge可能位于障礙物內或并聯機械臂工作空間外，出現串聯、并聯機械臂無法搜尋到無撞路徑的情況，因此動平臺目標點ge可能需要多次重選。用select表示目標點ge選取次數，設置選點迭代閾值select，當選取次數select大于閾值select時，認為算法無法對當前給定環境下的機械臂進行避障路徑規劃。

當動平臺目標點坐標值滿足下式時動平臺目標點gp位于障礙物內。

式中s，s，s分別為障礙物在，，軸方向的最小坐標；e，e，e分別為障礙物在，，軸方向的最大坐標。

本文研究對象中的并聯機械臂工作空間為以腰部為中心軸的空間環體，環體截面為矩形[22]。因此可以通過下式確定動平臺目標點位于并聯機械臂工作空間內。

式中mint、maxt分別為并聯機械臂工作空間環體截面與腰部中心軸的最小距離和最大距離，mm；minz、maxz分別為并聯機械臂工作空間在軸的最小和最大坐標值，mm。

若通過式（6）、（7）判定目標點gp位于障礙物內部或者并聯機械臂工作空間外，應重新選擇動平臺目標點。

2.2.2 遍歷法建立串聯機械臂關節構形空間

為避免求解混聯采摘機械臂復雜的碰撞條件解析式[18-20]，本文采用遍歷的方法建立障礙物在關節構形空間中的映射模型。

由圖2中機械臂構形特點可知，動平臺目標點gp與末端執行器目標點ge唯一確定了串聯機械臂目標位形關節值gs。在進行并聯機械臂路徑規劃時，串聯機械臂看作并聯機械臂動平臺上的固定部件，因此動平臺目標點gp與并聯機械臂基座點確定了串聯機械臂進行避障路徑規劃時的初始位形關節值ss。為確定串聯機械臂能夠無撞的從起始位形到達目標位形，將障礙物模型映射到串聯機械臂關節構形空間。機械臂關節構形空間障礙物模型構建步驟如下

1）對機械臂的關節構形空間以步長link，mm或(°)，進行離散，獲得表示空間的離散化矩陣；

2）將簡化后的機械臂各連桿按照步長link取點，link為所取點的集合；

3）對于矩陣中的任意元素，利用式（2）遍歷機器人離散點集合link，檢測矩陣中的任意元素對應位形的機械臂與障礙物的干涉關系；

4）上一步驟中檢測到有干涉的機械臂位形所對應的元素組成碰撞元素集合obs，補集free=-obs；

5）以集合obs中的元素為坐標，在機械臂關節構形空間中繪制離散點，離散點云就構成了障礙物在關節構形空間中的模型。

圖3為障礙物與空間2自由度串聯機械臂相對位置關系以及利用遍歷法所構建的關節構形空間障礙物模型。

1.障礙物 2.空間2R機械臂

1.Obstacle 2.Spatial 2R manipulator

注：1和2分別為平面2R機械臂關節1和關節2角度值。

Note:1,2represents rotation angle of first and second joint of planar 2R manipulator.

圖3 障礙物與2自由度串聯機械臂相對位置關系及障礙物在關節構形空間中的映射模型

Fig.3 Relative position relationship between obstacle and 2 dof serial manipulator and corresponding model of obstacle in joint configuration space

2.2.3 搜索串聯機械臂避障路徑

建立障礙物在串聯機械臂關節構形空間中的映射模型后，通過邊界序列法判斷串聯機械臂關節值gs與關節構形障礙物模型obss的相對位置關系，當目標位形關節值gs位于空間障礙物模型obss內部時，無法繼續避障路徑規劃，因此重新選定動平臺目標點gp，否則利用RRT算法在關節構形空間中嘗試搜索一條連接串聯機械臂初始位形關節值ss和目標位形關節值gs的無撞路徑ser，其中ser為ser行3列的矩陣，ser的第1列和第2列分別表示關節4和關節5的旋轉角度，第3列為關節6的角度值，設置為0，ser為路徑ser中控制點個數。RRT算法主要步驟[23-29]包括

1）令為一個有個節點的隨機樹，以機械臂初始位形所對應的關節值s作為隨機樹的起始點；

2）在機械臂關節構形空間中隨機選擇一點rand，且滿足rand∈free；

3）找到隨機樹中距離rand最近的節點near；

4）在點near與點rand的連線上求取new，使得new滿足new∈free和(near,new)=valve，其中(near,new)為點near和點new之間的距離，valve> 0為RRT算法距離閾值；

5）若對于點near與點rand可求得new，且new滿足new∈free，其中new為點near與點new之間的連線，則將new加入到隨機樹中，新的隨機樹可以表示為1，其中1=(;new)，否則重新選取rand；

6）當((1),goal)

由于RRT算法通過狹窄通道的能力較差，為避免算法陷入無用循環，提高算法整體效率，將算法迭代次數用rrt記錄，定義迭代次數閾值rrt，當滿足rrt≥rrt時，認為RRT算法無法為當前場景找到一條避障路徑，則重新選取動平臺目標點gp。

2.3 并聯機械臂避障路徑規劃

2.3.1 確定并聯機械臂目標位形關節值

要確定并聯機械臂的目標位形所對應的關節值gp，就需要先求解并聯機械臂主動關節值與動平臺坐標值之間的映射關系。由于并聯機械臂的復雜構形，直接求解該映射關系比較困難，因此引入圖2中連桿l和連桿l的旋轉角度作為中間變量。先求取動平臺坐標值與中間變量之間的映射關系和中間變量與主動關節值之間的映射關系，然后綜合獲得并聯機械臂的目標位形關節值gp。

分析圖2中的機械臂結構可得到并聯機械臂動平臺目標點gp的坐標值與中間變量之間的映射關系式

式中gpx、gpy和gpz分別為點gp在基礎坐標系、、軸方向的坐標值，mm；1、、分別為腰部關節、連桿l和連桿l的旋轉角度，(°)。

中間變量、與主動關節之間的映射關系[21]為

式中為圖2中與點固連的水平滑塊的位移量，mm；為與點固連的豎直滑塊的位移量，mm。

根據并聯機械臂動平臺目標點坐標值gp，聯立式（9）和（10）則可以求解并聯機械臂目標關節值gp。

2.3.2 并聯機械臂關節構形空間和避障路徑規劃

由于并聯機械臂關節構形空間以并聯機械臂主動關節值為坐標，因此在并聯機械臂關節構形空間障礙物模型的求解過程中將串聯機械臂各關節固定。采用前面描述的機械臂關節構形空間構建方法建立障礙物在并聯機械臂關節構形空間中的模型。

通過邊界序列法判斷并聯機械臂目標位形關節值gp與并聯機械臂關節構形空間障礙物模型obsp的相對位置關系，當目標位形關節值gp位于障礙物模型obsp內部時，無法繼續避障路徑規劃，重新選定動平臺目標點gp，否則將RRT算法擴展至三維空間，利用該算法在并聯機械臂關節構形空間中搜索連接初始位形關節值sp和目標位形關節值gp的無撞路徑par，其中par為par行3列的矩陣，par的第1列至第3列分別表示關節1至關節3的旋轉角度或位移量，par為路徑par中控制點個數。

2.4 混聯機械臂避障路徑規劃

綜合并聯機械臂無撞路徑關節值集合par和串聯機械臂無撞路徑關節值集合ser，并根據式(11)可以獲得混聯機械臂整體的無撞路徑關節值集合hyb。

式中parl=[parlf,parls,parlt]，其中parlf，parls，parlt分別為以par(end, 1)，par(end, 2)，par(end, 3)為元素的ser維列向量，par(end,)為矩陣par第列最后1行元素；serl=ser+sera，其中sera= [parlf, 0, 0]。

通過關節值集合hyb驅動混聯機械臂各關節，可使混聯機械臂避開障礙物，引導末端執行器到達目標點位置，完成避障路徑規劃。

整理前面的分析可以得到如圖4所示的基于關節構形空間的混聯采摘機械臂避障路徑規劃流程圖。

4 避障路徑規劃仿真與試驗

為了驗證文中所提出算法的可行性和有效性，以6自由度混聯采摘機械臂作為研究對象開展仿真試驗。表1為機械臂結構參數，障礙物參數如表2所示，表3為算法閾值以及參數設定值，機械臂起始位形關節值為（90, 150, 50, 0, 0, 0），目標點坐標為（925, ?1 602, 525）。

利用MATLAB 2016a搭建了混聯機械臂仿真系統[21,30-31]，仿真試驗平臺是主頻為3.6 GHz，內存為16 GB的計算機。圖5為混聯機械臂仿真試驗的主要過程。從圖5中可知，算法先后對串聯機械臂和并聯機械臂構建關節構形空間并搜索無撞路徑，然后綜合獲得混聯機械臂整體的避障路徑。

表1 機械臂結構參數

注：,分別為圖2中關節2（2）和關節3（3）的位移；1為關節角度1（1）的，4，5和6同理；其余符號具體含義見圖2。

Note:,represent displacement of second and third joint respectively in Fig.2;1represents rotation angle of first joint (1) in Fig.2,4,5and6are in same way; meaning of other symbols in Table 1 are given in Fig.2.

表2 障礙物位形參數

表3 算法主要控制參數

利用混聯采摘機械臂實體樣機所搭建的路徑規劃試驗平臺進行避障路徑規劃試驗。在前面的仿真過程中，將機械臂各桿件簡化成了沒有體積的線段，而實際情況中，需要考慮機械臂各桿件的厚度、粗細。為簡化理論分析過程，將障礙物在實際尺寸基礎上增加機械臂最粗桿件的半徑作為障礙物碰撞檢測尺寸。為避免引入過多的試驗影響因素，直接向機械臂控制軟件給定末端執行器目標點位置和障礙物相關參數，然后利用本文算法模塊對機械臂進行避障路徑規劃，再由控制軟件驅動機械臂各關節運動，使機械臂末端執行器到達目標點位置，完成避障路徑規劃。圖6為機械臂在試驗室環境下的避障試驗運動過程。從圖中可以看出，機械臂避開了位于其工作空間內的障礙物，成功引導末端執行器到達了目標點位置。仿真和試驗結果表明，利用文中所提出的算法進行混聯機器人避障路徑規劃是可行的，有效的。

a. 機械臂初始狀態a. Initial state of hybrid manipulatorb. 中間運動狀態1b. Intermediate state of manipulator 1 c. 中間運動狀態2c. Intermediate state of manipulator 2d. 中間運動狀態3d. Intermediate state of manipulator 3 e. 中間運動狀態4e. Intermediate state of manipulator 4f. 中間運動狀態5f. Intermediate state of manipulator 5 g. 中間運動狀態6g. Intermediate state of manipulator 6h. 機械臂最終運動狀態h. Final state of manipulator

5 討 論

文中快速擴展隨機樹搜索算法是通過不斷選取隨機點加入到隨機樹來進行路徑搜索的，因此在以周轉關節值為坐標的關節構形空間中進行路徑搜索時，RRT算法無法越過關節構形空間的邊界，搜索到空間的另一端。如圖5d所示，RRT算法無法搜索到一條越過關節構形空間底部邊界且連接并聯機械臂初始位形sp和目標位形gp的路徑，這就在腰部關節的零位形成了一堵“墻”，而實際工作過程中，并聯機械臂能夠從初始位形sp越過腰部關節的零位到達目標位形gp。此不足導致在機械臂路徑規劃過程中限制了機械臂周轉關節的轉動方向，不利于機械臂的避障路徑規劃。

在建立并聯機械臂關節構形空間障礙物模型的過程中，將串聯機械臂各關節固定，并使其始終保持在初始零位，串聯機械臂無法實時避障，對并聯機械臂的避障路徑規劃造成了一定的限制。因此為提高混聯機械臂整機避障能力，就要使串聯機械臂擁有全局避障路徑規劃的能力，即并聯機械臂在避障路徑規劃時串聯機械臂也能夠實時地進行避障，項目組后續擬針對混聯機械臂全局實時避障問題開展相關研究工作。

6 結 論

1）本文提出了一種基于關節構形空間的混聯采摘機械臂避障路徑規劃算法，算法將混聯機械臂避障路徑規劃問題轉化成對并聯和串聯機械臂分別進行避障路徑規劃的子問題，降低了關節構形空間障礙物映射模型的復雜度，避免了建立混聯機械臂高維關節構形空間。

2）本文基于遍歷法構建障礙物在關節構形空間中的映射模型，并利用快速擴展隨機樹算法在關節構形空間中進行避障路徑搜索，無需分析復雜構形的混聯機械臂各桿件與障礙物發生碰撞的解析表達式以及混聯機械臂整體逆運動學方程，提高了算法通用性。

3）利用本文所提出的算法對混聯采摘機械臂進行避障路徑規劃仿真和試驗，結果表明通過此算法構建的路徑能使混聯機械臂有效地避開障礙物，引導機械臂末端執行器到達目標點，驗證了算法的可行性和有效性。

[1] Li Peilin, Lee Sang-heon, Hsu Hung-yao. Review on fruit harvesting method for potential use of automatic fruit harvesting systems[J]. Procedia Engineering, 2011, 23(5): 351－366.

[2] 宋健，張鐵中，徐麗明，等. 果蔬采摘機器人研究進展與展望[J]. 農業機械學報，2006，37(5)：158－162.

Song Jian, Zhang Tiezhong, Xu Liming. Progress and prospects of the fruit and vegetable picking robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2006, 37(5): 158－162. (in Chinese with English abstract)

[3] Zhang Libin, Yang Qinghua, Bao Guanjun, et al. Overview of research on agricultural robots in China[J]. International Journal of Agricultural and Biological Engineering, 2008, 1(1): 12－21.

[4] 黃獻龍，梁斌，吳宏鑫. 機器人避碰規劃綜述[J]. 航天控制，2002，20(1)：34－40.

Huang Xianlong, Liang Bin, Wu Hongxin. A survey on robotics collision avoidance planning[J]. Aerospace Control, 2002, 20(1): 34－40. (in Chinese with English abstract)

[5] Lavalle S M. Planning Algorithm[M]. UK: Cambridge University Press, 2006: 127－180.

[6] Lozano-Perez T. Spatial planning: A configuration space approach[J]. IEEE Transaction on Computers, 1983, C-32(2): 108－120.

[7] Newman W S, Branicky M S. Real-time configuration space transforms for obstacle avoidance[J]. International Journal of Robotics Research, 1991; 10(6): 650－667.

[8] Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[J]. The International Journal of Robotics Research, 1986, 5(1): 90－98.

[9] 姬偉，程風儀，趙德安，等. 基于改進人工勢場的蘋果采摘機器人機械手避障方法[J]. 農業機械學報，2013，44(11)：253－259.

Ji Wei, Cheng Fengyi, Zhao De’an, et al. Obstacle avoidance method of apple harvesting robot manipulator[J]. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(11): 253－259. (in Chinese with English abstract)

[10] Van Henten E J, Hemming J, Van Tuijl B A J, et al. Collision-free motion planning for a cucumber picking robot[J]. Biosystems Engineering, 2003, 86(2): 135－144.

[11] 祁若龍，周維佳，王鐵軍，等. 一種基于遺傳算法的空間機械臂避障軌跡規劃方法[J]. 機器人，2014，36(3)：263－270.

Qi Ruolong, Zhou Weijia, Wang Tiejun, et al. An obstacle avoidance trajectory planning scheme for space manipulators based on genetic algorithm[J]. Robot, 2014, 36(3): 263－270. (in Chinese with English abstract)

[12] 戈志勇，陳樹人，王新忠. 神經網絡在果蔬收獲機器人機械臂避障路徑中的應用[J]. 農機化研究，2007(2)：172－174.

Ge Zhiyong, Chen Shuren, Wang Xinzhong. Neural network used for avoidance obstacle of fruit and vegetables harvest robot[J]. Journal of Agricultural Mechanization Research, 2007(2): 172－174. (in Chinese with English abstract)

[13] 賈慶軒，陳鋼，孫漢旭，等. 基于A*算法的空間機械臂避障路徑規劃[J]. 機械工程學報，2010，46(13)：109－115.

Jia Qingxuan, Chen Gang, Sun Hanxu, et al. Path planning for space manipulator to avoid obstacle based on A* algorithm[J]. Journal of Mechanical Engineering, 2010, 46(13): 109－115. (in Chinese with English abstract)

[14] 謝碧云，趙京，劉宇. 基于快速擴展隨機樹的7R機械臂避障達點運動規劃[J]. 機械工程學報，2012，48(3)：63－69.

Xie Biyun, Zhao Jing, Liu Yu. Motion planning of reaching point movements for 7R robotic manipulators in obstacle environment based on rapidly-exploring random tree algorithm[J]. Journal of Mechanism Engineering, 2012, 48(3): 63－69. (in Chinese with English abstract)

[15] Gómez-Bravo F. Collision free trajectory planning for hybrid manipulators[J]. Mechatronics, 2012, 22(6): 836－851.

[16] Zhao D A, Lü J D, Ji W. Smoothing obstacle avoidance path planning based on C-space for harvesting robot[C]// Proceedings of the 32nd Chinese Control Conference, Xi’an: IEEE, 2013: 26－28.

[17] Zhao D A, Lü J D, Ji W, Zhang Y, Chen Y. Design and control of an apple harvesting robot[J]. Biosystems Engineering, 2011, 110(2): 112－122.

[18] 蔡健榮，趙杰文，Thomas R，等. 水果收獲機器人避障路徑規劃[J]. 農業機械學報，2007，38(3)：102－105，135.

Cai Jianrong, Zhao Jiewen, Thomas R, et al. Path planning of fruits harvesting robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2007, 38(3): 102－105, 135. (in Chinese with English abstract)

[19] 姚立健，丁為民，陳玉侖，等. 茄子收獲機器人機械臂避障路徑規劃[J]. 農業機械學報，2008，39(11)：94－98.

Yao Lijian, Ding Weimin, Chen Yulun, et al. Obstacle avoidance path planning of eggplant harvesting robot manipulator[J]. Transactions of the Chinese Society for Agricultural Machinery, 2008, 39(11): 94－98. (in Chinese with English abstract)

[20] 尹建軍，武傳宇，Yang S X，等. 番茄采摘機器人機械臂避障路徑規劃[J]. 農業機械學報，2012，43(12)：171－175.

Yin Jianjun, Wu Chuanyu, Yang S X, et al. Obstacle- avoidance path planning of robot arm for tomato-picking robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2012, 43(12): 171－175. (in Chinese with English abstract)

[21] 陽涵疆，李立君，高自成.基于旋量理論的混聯采摘機器人正運動學分析與試驗[J]. 農業工程學報，2016，32(9)：53－59.

Yang Hanjiang, Li Lijun, Gao Zicheng. Forward kinematics analysis and experiment of hybrid harvesting robot based on screw theory[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(9): 53－59. (in Chinese with English abstract)

[22] 李成偉，贠超. 碼垛機器人機構設計與運動學分析[J]. 機械設計與制造，2009，6(6)：181－183.

Li Chengwei, Yun Chao. Stacking robot kinematics design and research institutions[J]. Machinery Design and Manufacture, 2009, 6(6): 181－183. (in Chinese with English abstract)

[23] Lavalle S M. Rapidly-Exploring Random Trees: A New Tool for Path Planning[R]. Iowa City: Computer Science Department of Iowa State University, 1998.

[24] Lavalle S M. Randomized kinodynamic planning[C]// Proceedings of International Conference on Robotics and Automation, Piscataway: IEEE 1999: 473－479

[25] Kuffner J J, LaValle S M. RRT-Connect: An efficient approach to single-query path planning[C]//The International Conference on Robotics and Automation. San Francisco: IEEE, 2000: 1－7.

[26] Jordan M, Perez A. Optimal Bidirectional Rapidly-Exploring Random Trees[R]. Computer Science and Artificial Intelligence Laboratory Technical Report, 2013, 021: 1－14.

[27] Klemm S, Oberlander J, Hermann A, et al. RRT*-Connect: Faster, asymptotically optimal motion planning[C]//IEEE Conference on Robotics and Biomimetics. Zhuhai: IEEE, 2015: 1670－1677.

[28] Karaman S, Frazzoli E. Sampling-based Algorithms for Optimal Motion Planning[J]. The International Journal of Robotics Research, 2011, 30(7): 846－894.

[29] 康亮，趙春霞，郭劍輝. 未知環境下改進的基于RRT算法 的移動機器人路徑規劃[J]. 模式識別與人工智能，2009，22(3)：337－343.

Kang Liang, Zhao Chunxia, Guo Jianhui. Improved path planning based on rapidly-exploring random tree for mobile robot in unknown environment[J]. Pattern Recognition and Artificial Intelligence, 2009, 22(3): 337－343. (in Chinese with English abstract)

[30] Corke P. Robotics, Vision and Control[M]. USA: Springer, 2011: 137－158.

[31] Craig J J. Introduction to Robotics Mechanics and Control[M]. USA: Person, 2005: 16-68.

Obstacle avoidance path planning of hybrid harvesting manipulator based on joint configuration space

Yang Hanjiang, Li Lijun※, Gao Zicheng

(,,410000,)

Aiming to realize the obstacle avoidance of fruit harvesting robot manipulator in unstructured environments, a kind of obstacle avoidance path planning algorithm of hybrid harvesting manipulator based on joint configuration space was proposed in this study. The structure of the 2P4R hybrid camellia oleifera fruit harvesting robot manipulator composed of a palletizing manipulator and a spherical wrist serial manipulator was simplified, and the link connecting the palletizing manipulator and serial manipulator was named as moving platform. The manipulator could accomplish six kinds of movements, including waist rotation, translational motions of vertical slider and horizontal slider, as well as three kinds of rotational motions of the wrist part. In this study, only five joint motions were taken into consideration. It means that the last link was considered as a fixed part of its previous link when a collision-free path planning operation was conducted. This is because that the motion of the last link was not related to the position of the end effector which could only affect its posture. Firstly, a goal point for the moving platform of the manipulator in Cartesian space was selected with the proposed algorithm. The goal point should be located in the workspace of the parallel manipulator, rather than inside the obstacles. The initial and goal configurations of the serial manipulator were determined by the moving platform of the goal point, the goal position of the end effector and the initial posture of the serial manipulator. Secondly, the obstacle mapping model in serial manipulator joint configuration space was built by using traversal method. Then, an attempt was made to search for a collision-free route from the initial point to the goal point in this space with each point uniquely corresponding to a configuration of the serial manipulator by using rapid-exploring random tree (RRT) algorithm. Thirdly, if the algorithm failed to find such a route in the previous step, the selection of goal point and collision-free route searching operation would be repeated. Otherwise, the joint configuration space of the parallel manipulator would be established. Fourthly, the unique posture of the parallel manipulator was determined based on the mapping relationship between driving joint value and the position coordinates of the moving platform. The obstacle model was built in the joint configuration space of the parallel manipulator. Subsequently, the goal point of the moving platform would be selected again, if the point in parallel manipulator configuration space corresponding to the goal configuration was located in the mapping model of obstacle. Otherwise, a collision-free route from the start point to the goal point in the configuration space corresponding to the initial posture and goal configuration of the parallel manipulator respectively should be searched by using RRT algorithm. The obstacle avoidance path of the hybrid manipulator was obtained from the synthesized results of the collision-free paths of the serial manipulator and parallel manipulator. In order to verify the feasibility and effectiveness of the proposed algorithm, the path planning simulation of a hybrid manipulator in Matlab was carried out. The proposed algorithm was also applied in the obstacle avoidance path planning experiment on the camellia oleifera fruit harvesting manipulator. According to the simulation and experiment results, the path planned by the proposed algorithm could successfully drive the end effector from its initial position to the goal position without any collision. That is to say, these results can validate the feasibility and effectiveness of the proposed algorithm.

robots; manipulators; algorithms; obstacle avoidance; hybrid; path planning; joint configuration space

10.11975/j.issn.1002-6819.2017.04.008

TP24

A

1002-6819(2017)-04-0055-08

2016-06-08

2017-01-22

國家林業公益性項目（201104090）；湖南省高校科技創新團隊支持計劃（2014207）；湖南省研究生科研創新項目（CX2016B326）；中南林業科技大學研究生科技創新基金項目（CX2016B12）。

陽涵疆，男，湖南益陽人，主要從事機器人運動控制研究。長沙 中南林業科技大學機電工程學院，410000。Email：yanghanjiang@hotmail.com

李立君，女，湖南寧鄉人，教授，博士，主要從事現代林業裝備研究。長沙 中南林業科技大學機電工程學院，410000。 Email：junlili1122@163.com

陽涵疆，李立君，高自成. 基于關節構形空間的混聯采摘機械臂避障路徑規劃[J]. 農業工程學報，2017，33(4)：55－62. doi：10.11975/j.issn.1002-6819.2017.04.008 http://www.tcsae.org

Yang Hanjiang, Li Lijun, Gao Zicheng. Obstacle avoidance path planning of hybrid harvesting manipulator based on joint configuration space[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(4): 55－62. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.04.008 http://www.tcsae.org