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

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

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

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

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

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

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

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

本文主要研究的內(nèi)容如下:

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

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

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

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

1 場景建模與碰撞檢測

1.1 空間機械臂模型

本文研究的空間機械臂是空間開鏈結(jié)構(gòu),某工業(yè)機械臂平面結(jié)構(gòu)簡圖如圖1所示,其中串聯(lián)機械臂的運動關(guān)節(jié)包括移動關(guān)節(jié)和轉(zhuǎn)動關(guān)節(jié)。根據(jù)其結(jié)構(gòu)特點,將機械臂各桿件碰撞模型簡化為長方體模型。

圖1 工業(yè)機械臂平面結(jié)構(gòu)簡圖

1.2 障礙物模型

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

圖2 長方體包絡(luò)檢測示意圖

1.3 碰撞檢測方法

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

已知空間線段上2個端點p1(x1,y1,z1)和p2(x2,y2,z2)、空間四邊形的法向量M=[m1m2m3]以及4個頂點的坐標(biāo),由空間解析幾何知識可知,直線段與平面位置關(guān)系可以用空間直線段方向向量與空間四邊形法向量之間的關(guān)系判斷,其中T=[x2-x1y2-y1z2-z1]。若M·T=0,則直線段與平面平行,沒有交點;若M·T≠0,則直線段與平面有交點,設(shè)為p0(x0,y0,z0)。判斷該交點是否是碰撞點,若是，則必須滿足2個條件[3],即交點必須在線段上和交點在四邊形內(nèi)部。障礙物是長方體包絡(luò)而成,因此前、后面平行于z軸,左、右面平行于y軸,上下面平行于x軸。對平行于x軸的上平面來說,設(shè)平面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*算法規(guī)劃路徑

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

(1)

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

2.2 路徑平滑優(yōu)化

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

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

(2)

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

3 機械臂規(guī)劃算法實現(xiàn)

3.1 空間機械臂建模

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

其中,θi、αi-1、ai-1、di為相應(yīng)的連桿參數(shù),即各關(guān)節(jié)的轉(zhuǎn)角、相鄰關(guān)節(jié)之間的扭轉(zhuǎn)角、連桿長度、相鄰關(guān)節(jié)之間的偏距。

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

(3)

3.2 機械臂運動學(xué)逆解

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

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

(4)

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

“最短行程原則”優(yōu)化算法步驟如下：

(1) 已知當(dāng)前機械臂各個關(guān)節(jié)角θ1、θ2、θ3、θ4、θ5、θ6。

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

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

(4) 再根據(jù)“最短行程原則”優(yōu)化步驟(3)中的機械臂各關(guān)節(jié)角的解集,得到1組最優(yōu)解,即

3.3 機械臂避障路徑規(guī)劃算法

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

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

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

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

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

4 仿真結(jié)果分析

本文仿真試驗的空間機械臂是以工業(yè)上最常用的PUMA560型機器人為研究對象。利用標(biāo)準(zhǔn)的D-H法[6]對機械臂進行數(shù)學(xué)建模,各關(guān)節(jié)坐標(biāo)如圖3所示,得到的D-H參數(shù)見表1所列。

圖3 采用D-H表示的PUMA560機械臂關(guān)節(jié)坐標(biāo) 表1 六自由度的D-H參數(shù)

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

設(shè)置機械臂的D-H參數(shù)為：a2=4,a3=1,d2=3,d4=4;初始關(guān)節(jié)角θ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,算法搜索的最優(yōu)避障路徑結(jié)果如圖4所示。采用二次B樣條曲線對A*算法搜索的最優(yōu)避障路徑進行優(yōu)化,其中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個點構(gòu)成的8段二次B樣條曲線仿真結(jié)果如圖5所示。

圖4 A*算法搜索的最優(yōu)避障路徑規(guī)劃

圖5 二次B樣條優(yōu)化的避障路徑規(guī)劃

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

圖6 機械臂工作仿真模型

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

5 結(jié) 論

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

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

[2] 熊有倫.機器人技術(shù)基礎(chǔ)[M].武漢：華中科技大學(xué)出版社,1996.

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

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

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

[6] 蔡自興,謝斌.機器人學(xué)[M].北京:清華大學(xué)出版社,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].北京:國防工業(yè)出版社,2006:191-193.

(責(zé)任編輯 胡亞敏)

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

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

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

10.3969/j.issn.1003-5060.2017.02.005

TP391.9

A

1003-5060(2017)02-0164-05