一種基于A*算法的空間多自由度機械臂路徑規劃方法

宗成星, 陸 亮, 雷新宇, 趙 萍

(合肥工業大學 機械工程學院,安徽 合肥 230009)

一種基于A*算法的空間多自由度機械臂路徑規劃方法

宗成星, 陸 亮, 雷新宇, 趙 萍

(合肥工業大學 機械工程學院,安徽 合肥 230009)

針對機械臂在運動過程中可能會與工作環境中的障礙物發生碰撞的問題,文章提出了一種基于A*算法的機器人避障路徑規劃算法。在給定的避障環境下運用A*算法搜索一條給定的自起點到終點的最優避障路徑,考慮到機械臂在運動過程中的穩定性,采用二次B樣條曲線對該路徑進行平滑優化;根據標準的D-H法對機械臂進行建模,建立機械臂的運動學方程,并求取一組能夠最優實現避障路徑的運動學逆解;檢測機械臂本體與障礙物是否發生碰撞,若發生碰撞,結合機器人逆解的多解性,重新選擇最優的一組可行解。通過軟件Solidworks建立虛擬樣機導入ADAMS軟件進行仿真實驗,最終驗證了基于A*算法的空間多自由度機械臂避障路徑規劃的有效性和可行性。

空間機械臂;A*算法;路徑規劃;二次B樣條;碰撞檢測;虛擬樣機

路徑規劃是機器人學領域研究的熱點課題之一,對工業機器人的生產制造至關重要。機器人路徑規劃問題主要解決的是在給定的避障環境下機器人能夠自主地搜索一條自起始點到目標點的最優避障路徑[1]。為了使機器人能夠快速、準確地完成任務,一方面要保證機械臂沿著避障路徑平穩地運動,另一方面還要確保機械臂不能與障礙物發生碰撞[2]。目前，已有的二維平面運動路徑規劃算法技術已不能滿足更復雜的工作需要，因此迫切需要一套成熟的三維路徑規劃算法。文獻[3]提出了基于快速擴展隨機樹(rapidly-exploring random tree,RRT)搜索算法的路徑規劃,該算法以機器人的起始點作為隨機數的根節點,以基準向量作為基準選擇節點的擴展方向,通過運動學反解規劃全局優化路徑；文獻[4]提出了基于A*算法變步長搜索的路徑規劃法；文獻[5]提出了一種基于遺傳算法的避障路徑規劃,其針對單個或多個避障物的工作環境,以運動的時間、移動的空間距離以及軌跡長度作為優化指標,在實現避障路徑規劃的同時進行優化。

本文主要研究的內容如下:

(1) 三維避障路徑規劃。建立三維坐標下包含障礙物的幾何模型,選取合適的算法規劃一條自起始點到目標點的最優避障路徑,因為考慮到三維空間建模的難度,所以提出了三維環境下A*啟發式搜索算法,該算法能夠有效快速地對笛卡爾空間中6個方向進行均勻步長搜索,得到一條基于機械臂末端位置的自起始點到目標點的最優避障路徑。考慮到機械臂運動的穩定性,利用n次B樣條曲線優化搜索的避障路徑,以確保機械臂能夠穩定地避開障礙物。

(2) 針對上述避障路徑,以某具體空間機械臂為例,分析并建立運動學方程,規劃基于機械臂末端的軌跡算法。

(3) 碰撞檢測。檢測機械臂各桿件與障礙物是否發生碰撞，如果發生碰撞,結合機器人逆解的多解性,重新選擇最優的一組可行解。

本文最后通過Solidworks建立虛擬樣機導入ADAMS軟件進行仿真實驗,驗證了基于A*算法的空間多自由度機械臂避障路徑規劃的有效性和可行性。

1 場景建模與碰撞檢測

1.1 空間機械臂模型

本文研究的空間機械臂是空間開鏈結構,某工業機械臂平面結構簡圖如圖1所示,其中串聯機械臂的運動關節包括移動關節和轉動關節。根據其結構特點,將機械臂各桿件碰撞模型簡化為長方體模型。

圖1 工業機械臂平面結構簡圖

1.2 障礙物模型

空間障礙物一般都是不規則的幾何形狀物,為了方便地判斷機械臂碰撞問題,必然要對障礙物進行規則化處理。基于空間機械臂的模型,障礙物模型采用長方體包絡[1],如圖2所示。采用長方體包絡障礙物必然會擴大障礙物的區域,但大大簡化了碰撞檢測方法,對規劃出來的路徑更具有安全性。

圖2 長方體包絡檢測示意圖

1.3 碰撞檢測方法

為了解決機械臂整體與障礙物之間的碰撞問題,本文采用了長方體包絡檢測法,即把機械臂各桿件和障礙物都包絡成長方體。假設包絡N自由度機械臂各桿件的長方體分別為{R1,R2,…,RN},而包絡L個障礙物的長方體分別為{O1,O2,…,OL}。通過對{R1,R2,…,RN}的各條棱邊線段與{O1,O2,…,OL}各個面是否有交點的檢測,把上述碰撞問題轉化為空間直線段與四邊形之間的位置關系的判斷。

已知空間線段上2個端點p1(x1,y1,z1)和p2(x2,y2,z2)、空間四邊形的法向量M=[m1m2m3]以及4個頂點的坐標,由空間解析幾何知識可知,直線段與平面位置關系可以用空間直線段方向向量與空間四邊形法向量之間的關系判斷,其中T=[x2-x1y2-y1z2-z1]。若M·T=0,則直線段與平面平行,沒有交點;若M·T≠0,則直線段與平面有交點,設為p0(x0,y0,z0)。判斷該交點是否是碰撞點,若是，則必須滿足2個條件[3],即交點必須在線段上和交點在四邊形內部。障礙物是長方體包絡而成,因此前、后面平行于z軸,左、右面平行于y軸,上下面平行于x軸。對平行于x軸的上平面來說,設平面4個頂點為:s1(x3,y3,z3),s2(x4,y4,z3),s3(x5,y5,z5),s4(x6,y6,z6)。

若有:

min(z1,z2)≤z0≤max(z1,z2)，

min(x3,x4)≤x0≤max(x3,x4)，

min(y3,y4)≤y0≤max(y3,y4),

則p0是線段與四邊形的交點,其他情況類似。

2 基于A*算法避障搜索路徑

2.1A*算法規劃路徑

A*算法是一種啟發式搜索算法,是通過估算函數來實現的,機械臂每走一步都需要計算該函數的值,且該函數值最小的節點也就是機械臂下一步需要到達的位置。啟發式函數的一般表達式如下:

(1)

其中,g(n)為機械臂從路徑的初始節點到路徑規劃中的節點n所花費的實際代價;因為機械臂已知從初始節點到節點n實際花費的時間和走過的實際長度,所以函數g(n)值是實際數值;h(n)為從當前節點n到路徑規劃終點所花費的估算代價;函數F(n)為機械臂在整個路徑規劃中的總代價。本文中估算函數h(n)為機械臂末端位置與目標節點之間的距離，即

2.2 路徑平滑優化

由A*算法均勻步長搜索出的避障路徑是一條空間直線段,機械臂在運動過程中會出現抖動,因此采用二次B樣條曲線對避障路徑進行平滑處理,使得機械臂能夠連續穩定地工作。

設有控制頂點D1,D2,…,Dn,則K階(K-1)次B樣條曲線的數學表達式為:

(2)

其中,Ni,k(t)是k-1次B樣條曲線的基函數,本文提出的二次B樣條曲線每段由3個控制點組成,其基函數矩陣表示為:

3 機械臂規劃算法實現

3.1 空間機械臂建模

建模的基本思想為:針對給定的機械臂,1個連桿建立1個坐標系，用齊次變換來描述該坐標系之間的位置與姿態,通過遞歸的方式得到末端機械臂相對于基坐標系的齊次變換矩陣。建立的第i-1坐標系和第i坐標系之間可以用平移和旋轉來實現變換,最后得到相鄰坐標之間的變換通式[6],即

其中,θi、αi-1、ai-1、di為相應的連桿參數,即各關節的轉角、相鄰關節之間的扭轉角、連桿長度、相鄰關節之間的偏距。

根據D-H法,可以依次計算出相鄰坐標系之間的變換矩陣，再順序右乘即可得到六自由度空間機械臂末端相對于基坐標系的齊次變換矩陣，即

(3)

3.2 機械臂運動學逆解

機器人逆運動學問題,即已知笛卡爾坐標系中機械臂末端位姿矩陣,求機械臂各關節的轉角變量。逆運動學求解的問題主要包括3個方面:① 解的存在性,只要保證期望位姿在機械臂的工作空間內即可。② 多解性問題,一般來說,六自由度串聯機械臂最多有16組運動學逆解[7-8],逆解的數目取決于機械臂的結構連桿參數里連桿長度和偏距所含非零項的個數;本文研究的機械臂最多有8組運動學逆解,實際上只需要每個關節的1組解。針對該問題,本文選擇的方法是結合每個關節的運動范圍及“最短行程解”來優化多解問題。③ 求解方法,包括數值解法和封閉解法2類,本文采用封閉解法中的代數解法對機械臂運動學方程進行求解;但是并不是所有的機械臂都能使用該方法,必須在滿足Pieper準則[2]的前提下才可以進行代數求解,最后得到所有關節轉角變量的數學表達式。

將(3)式進行變換可得:

(4)

由矩陣對應項相等得到12個方程組,求解這些方程組得到8組不同的關節角變量值,然后結合各關節角變化范圍及“最短行程”[6]進行優化。如果環境中沒有障礙物時,那么采用最短行程原則,即尺寸大的機械臂關節角移動距離少,尺寸小的機械臂關節角移動距離大。如果可行解有多組,那么結合最短行程原則選擇1組最優解。

“最短行程原則”優化算法步驟如下：

(1) 已知當前機械臂各個關節角θ1、θ2、θ3、θ4、θ5、θ6。

(2) 通過機械臂運動學逆解得到8組解θn1、θn2、θn3、θn4、θn5、θn6,其中n=1,2,…,8。

(3) 根據機械臂各關節角的變化范圍刪除部分不可行解,需滿足如下關系,即

(4) 再根據“最短行程原則”優化步驟(3)中的機械臂各關節角的解集,得到1組最優解,即

3.3 機械臂避障路徑規劃算法

(1) 初始化機械臂的起始點、終止點以及每個關節的初始關節角,在三維空間中建立障礙物模型。

(2) 三維路徑規劃。采用A*算法在給定的三維空間6個方向進行搜索,得到一條基于機械臂末端位姿的、自起點到終點的最優避障路徑。為確保機械臂平穩地工作,提取獲得的路徑數據信息,采用二次B樣條曲線對該路徑進行平滑處理。

(3) 機械臂運動學逆解。將步驟(2)中得到的三維路徑帶入Matlab中編寫逆解函數進行運動學反解。如果反解失敗,那么該點不在機械臂末端執行器工作空間范圍內,返回步驟(2)。

(4) 機械臂運動學正解。規劃路徑上的每個點逆解之后都將得到8組不同的關節角,將最優的1組關節角帶入機器人正運動學方程,并得到每個關節的空間坐標。

(5) 碰撞檢測。由步驟(4)得到的各關節的空間坐標可以確定具體機械臂相鄰兩關節間的線段方程,檢測機械臂與障礙物間的碰撞情況。若發生碰撞,則返回步驟(4)結合機器人逆解的多解性,在其他幾組解中重新選擇1組可行解；反之,該解為最優的1組關節角度。

4 仿真結果分析

本文仿真試驗的空間機械臂是以工業上最常用的PUMA560型機器人為研究對象。利用標準的D-H法[6]對機械臂進行數學建模,各關節坐標如圖3所示,得到的D-H參數見表1所列。

圖3 采用D-H表示的PUMA560機械臂關節坐標 表1 六自由度的D-H參數

θi/(°)αi-1/(°)ai-1/cmdi/cm關節變量范圍/(°)θ1000-160～160θ2-900d2-225～45θ30a20-45～225θ4-90a3d4-110～170θ59000-100～100θ6-9000-266～266

設置機械臂的D-H參數為：a2=4,a3=1,d2=3,d4=4;初始關節角θ1=90°,θ2=0°,θ3=-90°,θ4=0°,θ5=0°,θ6=0°。仿真實驗是在5 cm×5 cm×5 cm的空間里進行的,障礙物是長、寬、高均為2 mm的長方體,其中任取工作空間的點(1,5,3)為起點,點(5,1,2)為終點,A*算法的搜索步長為1,算法搜索的最優避障路徑結果如圖4所示。采用二次B樣條曲線對A*算法搜索的最優避障路徑進行優化,其中A*算法搜索的直線段由10個點組成,即

D0(1,5,3);D1(1,5,2);D2(2,5,2);D3(2,4,2);

D4(3,4,2);D5(4,4,2);D6(5,4,2);D7(5,3,2);

D8(5,2,2);D9(5,1,2)。

每段二次B樣條曲線由3個相鄰的控制點組成,則10個點構成的8段二次B樣條曲線仿真結果如圖5所示。

圖4 A*算法搜索的最優避障路徑規劃

圖5 二次B樣條優化的避障路徑規劃

將末端位置的軌跡方程結合機器人逆運動學方程,求解出每個關節的關節角度,并把最后一個關節的關節角導入ADAMS中生成樣條曲線spline-1,在末端位置添加一個旋轉驅動,定義為AKISPLZ(time,0,spline-1,0)[9]。在ADAMS中對PUMA560進行運動仿真,可以看到機器人按照給定的要求運動,當桿件之間有干涉運動時,系統會自動報錯并停止,結果如圖6和圖7所示。

圖6 機械臂工作仿真模型

圖7 機械臂末端位置避障路徑規劃仿真模型

5 結 論

本文針對空間多自由度機械臂三維路徑規劃問題,提出并實現了在三維空間內基于A*算法的軌跡避障方法,同時考慮機械臂運動的平穩性,得到了一條自起始點到目標點的最優避障路徑。ADAMS仿真結果驗證了該方法下機械臂本體在運動過程中能夠避開所有障礙物,因此,本文方法對機械臂避障路徑規劃具有可行性和有效性。

[1] KENNEDY J,EBERHART R C.Particle swarm optimization[C]//Proceedings of IEEE International Conference on Neuarl Networks.Perth:IEEE，1995:1942-1948.

[2] 熊有倫.機器人技術基礎[M].武漢：華中科技大學出版社,1996.

[3] 代彥輝,梁艷陽,謝鋼,等.基于RRT搜索算法的六自由度機械臂避障路徑規劃[J].工業控制與應用,2012,31(10):31-37.

[4] 汪首坤,邸智,王軍政,等.基于A*改進算法的機械臂避障路徑規劃[J].北京理工大學學報,2011,31(11):1302-1306.

[5] 韓濤,吳懷宇,杜釗君,等.基于遺傳算法的機械臂實時避障路徑規劃研究[J].計算機應用研究,2013,30(5):1373-1376.

[6] 蔡自興,謝斌.機器人學[M].北京:清華大學出版社,2000,59-65.

[7] MANOCHA D,CANNY J F.Efficient inverse kinematics for general 6R manipulators[J].IEEE Transaction on Robotics and Automation,1945,5(9):648-657.

[8] TEVATIA G,SCHAAL S.Inverse kinematics for humanoid robots[C]//IEEE International Conference on Robotics and Automation.NJ,USA:IEEE,2000:294-299.

[9] 李增剛.ADAMS入門詳解與實例[M].北京:國防工業出版社,2006:191-193.

(責任編輯 胡亞敏)

A path planning approach for multi-DOF spatial manipulator via A*algorithm

ZONG Chengxing, LU Liang, LEI Xinyu, ZHAO Ping

(School of Mechanical Engineering, Hefei University of Technology, Hefei 230009, China)

A novel approach for path planning of spatial serial robot manipulator based on A*algorithm is proposed, seeking to solve the problem that the manipulator might collide with the obstacles in the work space. Firstly, the optimal obstacle avoidance path from a given start point to the end point is obtained by using A*algorithm in a given obstacle avoidance space, and then quadratic B-spline curve is used to smooth the path so as to ensure the stability of the motion. Secondly, the manipulator is modeled and analyzed with standard D-H method, the kinematics equations are built up and a group of optimal inverse kinematic solutions to realize the path are obtained. Finally, this approach detects whether there is a collision between robot and the obstacles, and if there is, then the alternative optimal solution is chosen within the multiple solutions of the inverse kinematics. The result of simulation experiment which introduces the virtual prototype established by Solidworks into ADAMS verifies the effectiveness and feasibility of the proposed path planning approach for multi-DOF spatial manipulator to avoid obstacle.

spatial manipulator; A*algorithm; path planning; quadratic B-spline; collision detection; virtual prototype

2015-10-14；

2016-01-10

國家自然科學基金資助項目(51405128);安徽省自然科學基金資助項目(1508085QE82)

宗成星(1991-),男,安徽宣城人,合肥工業大學碩士生； 趙 萍(1988-),女,山東煙臺人,博士,合肥工業大學副教授,碩士生導師.

10.3969/j.issn.1003-5060.2017.02.005

TP391.9

A

1003-5060(2017)02-0164-05