章有，翟 華，2

（1.合肥工業(yè)大學(xué) 機(jī)械工程學(xué)院，安徽 合肥 230009；2.合肥工業(yè)大學(xué) 工業(yè)與裝備技術(shù)研究院，安徽 合肥 230009）

六自由度機(jī)器人逐加步長(zhǎng)路徑規(guī)劃算法研究

（1.合肥工業(yè)大學(xué) 機(jī)械工程學(xué)院，安徽 合肥 230009；2.合肥工業(yè)大學(xué) 工業(yè)與裝備技術(shù)研究院，安徽 合肥 230009）

針對(duì)六自由度機(jī)器人路徑規(guī)劃問(wèn)題，采用D－H法建立機(jī)器人的運(yùn)動(dòng)學(xué)方程，提出逐加步長(zhǎng)算法，搜索有效路徑，仿真結(jié)果證明了該算法的有效性。

六自由度機(jī)器人；運(yùn)動(dòng)學(xué)方程；逐加步長(zhǎng)搜索算法；路徑規(guī)劃

在汽車(chē)大型覆蓋件沖壓生產(chǎn)線中，搬運(yùn)機(jī)器人 合理的路徑規(guī)劃能使機(jī)器人運(yùn)動(dòng)避免自身結(jié)構(gòu)缺陷，讓機(jī)器人安全穩(wěn)定地在復(fù)雜環(huán)境下工作[1]。路徑是指空間的曲線，對(duì)于六自由度關(guān)節(jié)式機(jī)器人來(lái)說(shuō)，它是指機(jī)器人運(yùn)動(dòng)位姿特定的一個(gè)序列[2]。機(jī)器人路徑規(guī)劃是指在給定的初始、目標(biāo)位姿，通過(guò)采用恰當(dāng)?shù)乃惴ㄋ阉鞒鲆粭l從初始位姿到目標(biāo)位姿的有效路徑[3－4]。

1 六自由度機(jī)器人D-H坐標(biāo)系的運(yùn)動(dòng)學(xué)模型

機(jī)器人運(yùn)動(dòng)學(xué)是研究機(jī)器人性能的基礎(chǔ)，描述了關(guān)節(jié)空間與任務(wù)空間的關(guān)系，是路徑規(guī)劃的基礎(chǔ)[5]。建立六自由度機(jī)器人連桿坐標(biāo)系如圖1所示，α1、α2、α3為連桿1、2、3的長(zhǎng)度，d2、d4為連桿1與2、連桿3與4之間的偏距，θi為各連桿的轉(zhuǎn)角。

圖1 六自由度機(jī)器人連桿坐標(biāo)系

令si=sin（θi），ci=cos（θi），sij=sin（θi+θj），cij=cos（θi+θj），分別為相鄰連桿的變換矩陣，用來(lái)描述末端連桿坐標(biāo)系{6}在基坐標(biāo)系{0}的位姿，用來(lái)描述坐標(biāo)系{6}的原點(diǎn)在基坐標(biāo)系{0}中的位置矢量。機(jī)器人末端連桿坐標(biāo)系相對(duì)于基坐標(biāo)系的齊次變換矩陣[6]，即為：

某型六自由度機(jī)器人相應(yīng)的連桿參數(shù)如表1所示。

表1 機(jī)器人連桿參數(shù)

將表1中的數(shù)據(jù)帶入，則可得到以下運(yùn)動(dòng)方程[6]：

2 路徑規(guī)劃算法

通過(guò)選擇恰當(dāng)?shù)乃惴ㄊ箼C(jī)器人實(shí)現(xiàn)路徑搜索，能反映機(jī)器人的自主能力和高度智能化的自主行為[7-8]。本文提出逐加步長(zhǎng)搜索算法來(lái)對(duì)機(jī)器人路徑進(jìn)行規(guī)劃。該算法的基本思想是：對(duì)于六自由度關(guān)節(jié)式機(jī)器人，它的前三個(gè)連桿的尺寸較大，后面三個(gè)桿件相對(duì)較小，機(jī)器人的腕部位置主要由前三個(gè)關(guān)節(jié)變量決定，它的腕部姿態(tài)由后三個(gè)關(guān)節(jié)所決定。因此，在對(duì)六自由度關(guān)節(jié)式機(jī)器人進(jìn)行路徑規(guī)劃時(shí)，可以選擇優(yōu)先轉(zhuǎn)動(dòng)前三個(gè)關(guān)節(jié)，使機(jī)器人達(dá)到目標(biāo)位置，之后再轉(zhuǎn)動(dòng)后三個(gè)關(guān)節(jié)，使機(jī)器人運(yùn)動(dòng)至目標(biāo)姿態(tài)。機(jī)器人的路徑規(guī)劃分成兩步進(jìn)行，先達(dá)位置，后達(dá)姿態(tài)。

由于機(jī)器人末端執(zhí)行器固接在連桿坐標(biāo)系{6}上，因此可以用坐標(biāo)系{6}原點(diǎn)的路徑代替末端執(zhí)行器的路徑。根據(jù)式（1），可得{6}原點(diǎn)在系{0}的位置描述為：

若已知六自由度關(guān)節(jié)式機(jī)器人初始位姿（初始節(jié)點(diǎn)）的關(guān)節(jié)變量值q0為，目標(biāo)位姿（目標(biāo)節(jié)點(diǎn)）的關(guān)節(jié)變量值qt為，算法的搜索步長(zhǎng)為δ。在搜索路徑節(jié)點(diǎn)的過(guò)程中，相應(yīng)關(guān)節(jié)增加步長(zhǎng)的正負(fù)與機(jī)器人初始位姿和目標(biāo)位姿處關(guān)節(jié)的大小有關(guān)，正負(fù)規(guī)定如下：①若時(shí)，步長(zhǎng)為正；②若時(shí)，步長(zhǎng)為負(fù)；即：

將q0、qt代入式（3），可得機(jī)器人處于初始位姿和目標(biāo)位姿時(shí)系{6}的原點(diǎn)在系{0}的坐標(biāo)分別為：

從節(jié)點(diǎn)n-1搜索后繼節(jié)點(diǎn)n的基本過(guò)程如下：

將節(jié)點(diǎn)n-1處的前三個(gè)關(guān)節(jié)的關(guān)節(jié)變量值分別增加一個(gè)步長(zhǎng)δ，得到候選節(jié)點(diǎn)n1、n2、n3，如圖2所示，

圖2 搜索節(jié)點(diǎn)

候選節(jié)點(diǎn)n1、n2、n3的關(guān)節(jié)變量值為：

將式（7）代入式（3），可得機(jī)器人分別處于候選節(jié)點(diǎn)n1、n2、n3所對(duì)應(yīng)的位姿時(shí)，系{6}的原點(diǎn)相對(duì)于系{0}位置分別描述為：

此時(shí)，引入評(píng)價(jià)函數(shù)：

式中：f（n）表示節(jié)點(diǎn)n-1經(jīng)過(guò)節(jié)點(diǎn)n到達(dá)目標(biāo)節(jié)點(diǎn)的總距離代價(jià)，g（n）表示節(jié)點(diǎn)n距上一節(jié)點(diǎn)n-1的距離代價(jià)，h（n）表示節(jié)點(diǎn)n距目標(biāo)節(jié)點(diǎn)的距離代價(jià)。利

用評(píng)價(jià)函數(shù)對(duì)三個(gè)候選節(jié)點(diǎn)進(jìn)行評(píng)價(jià)，比較評(píng)價(jià)函數(shù)值（fn1）、（fn2）、（fn3）的大小，取其中最小值，

fmin（n）所對(duì)應(yīng)的候選節(jié)點(diǎn)就是所要搜索的節(jié)點(diǎn)n。

其他的路徑節(jié)點(diǎn)均可按上述過(guò)程進(jìn)行搜索，當(dāng)搜索到的路徑節(jié)點(diǎn)的前三個(gè)關(guān)節(jié)已到達(dá)目標(biāo)節(jié)點(diǎn)相應(yīng)值時(shí)，由于關(guān)節(jié)軸4和6均垂直于軸5，因此優(yōu)先選擇將關(guān)節(jié)軸5由轉(zhuǎn)至接著依次將關(guān)節(jié)軸4由關(guān)節(jié)軸6由 。此時(shí)，機(jī)器人已經(jīng)到達(dá)了目標(biāo)節(jié)點(diǎn)，路徑搜索過(guò)程結(jié)束。

3 機(jī)器人路徑規(guī)劃仿真

六自由度機(jī)器人初始位姿處的關(guān)節(jié)角為q0=（-55°，-65°，45°，0°，-50°，0°，），目標(biāo)位姿處的關(guān)節(jié)角為qt=（-30°，-30°，25°，15°，-45°，80°，），步長(zhǎng)設(shè)定為1°。在MATLAB環(huán)境下，結(jié)合機(jī)器人工具箱對(duì)機(jī)器人路徑進(jìn)行仿真，得到的路徑規(guī)劃仿真如圖3所示。

從圖3可以看出，根據(jù)本文提出的算法，可以規(guī)劃出一條平滑且有效的路徑，同時(shí)也能有效發(fā)現(xiàn)機(jī)器人在實(shí)際工作過(guò)程中的安全問(wèn)題。

4 結(jié)論

針對(duì)六自由度機(jī)器人路徑規(guī)劃問(wèn)題，提出了逐加步長(zhǎng)搜索算法實(shí)現(xiàn)了路徑節(jié)點(diǎn)搜索，將路徑節(jié)點(diǎn)依次連接得到機(jī)器人完整路徑。仿真結(jié)果驗(yàn)證了該方法的有效性和可行性。該算法的優(yōu)點(diǎn)是：求解過(guò)程中計(jì)算量小，提高了路徑規(guī)劃的效率。

[1]H.C.Fang，S.K.Ong，A.Y.C.Nee.Interactive robot trajectory planning and simulation using Augmented Reality [J].Robotics and Computer-Integrated Manufacturing，2012，28（2）：227-237.

[2]Cesare Rossi，Sergio Savino.Robottrajectory planning by assigning positions and tangential velocities[J].Robotics and Computer-Integrated Manufacturing，2013，29（1）：139-156.

[3]J.W.S.Chong， S.K.Ong. Robot programming using augmented reality：An interactive method for planning collision-free paths[J].Roboticsand Computer Integrated Manufacturing，2009，25（3）：689-701.

[4]賈慶軒，陳 鋼.基于A*算法的空間機(jī)械臂避障路徑規(guī)劃[J].機(jī)械工程學(xué)報(bào)，2010，46（13）：109-115.

[5]熊有倫.機(jī)器人技術(shù)基礎(chǔ)[M].武漢：華中科技大學(xué)出版社，1996：47-49.

[6]蔡自興.機(jī)器人學(xué)[M].北京：機(jī)械工業(yè)出版社，2009：32-35.

[7]Garg D P，Kumar M.Optimization techniques applied to multiple mannipulators for path planning and torque minimization[J]. Engineering Applications of Artificial Intelligence，2002，15（3-4）：241-252.

[8]張 雷.基于啟發(fā)式搜索的最優(yōu)規(guī)劃算法研究[D].南京：南京大學(xué)，2010

Study on step by step searchpath planning algorithm of six-degree freedom robot

ZHANG Youqi1,ZHAI Hua1,2
（1.School of Mechanical Engineering,Hefei University of Technology,Hefei 230009,Anhui China; 2.Institute of Industry and Equipment Technology,Hefei University of Technology,Hefei 230009,Anhui China）

Aiming at the path planning problem of six-degree freedom robot,the kinematic equation has been built to the robot with D-H method.The step by step search algorithm has been proposed to search an effective path.The validity of algorithm has been demonstrated by simulation result.

Six-degree freedom robot;Kinematic equation;Step by step search algorithm;Path planning

圖3 機(jī)器人路徑仿真圖

TP272；TP278

A

10.16316/j.issn.1672－0121.2016.04.015

1672－0121（2016）04－0046－04

2016－04－07；

2016－05－16

2013年度安徽省合蕪蚌自主創(chuàng)新試驗(yàn)區(qū)專項(xiàng)資助（20130205）

翟 華（1973－），男，博士，教授，碩導(dǎo)，從事機(jī)械設(shè)計(jì)、精密校直技術(shù)、機(jī)器人技術(shù)研究。E－mail：jxzhaihuajx＠sina.com