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

章有，翟 華，2

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

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

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

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

六自由度機器人；運動學方程；逐加步長搜索算法；路徑規(guī)劃

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

1 六自由度機器人D-H坐標系的運動學模型

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

圖1 六自由度機器人連桿坐標系

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

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

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

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

2 路徑規(guī)劃算法

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

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

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

將q0、qt代入式（3），可得機器人處于初始位姿和目標位姿時系{6}的原點在系{0}的坐標分別為：

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

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

圖2 搜索節(jié)點

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

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

此時，引入評價函數(shù)：

式中：f（n）表示節(jié)點n-1經(jīng)過節(jié)點n到達目標節(jié)點的總距離代價，g（n）表示節(jié)點n距上一節(jié)點n-1的距離代價，h（n）表示節(jié)點n距目標節(jié)點的距離代價。利

用評價函數(shù)對三個候選節(jié)點進行評價，比較評價函數(shù)值（fn1）、（fn2）、（fn3）的大小，取其中最小值，

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

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

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

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

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

4 結(jié)論

針對六自由度機器人路徑規(guī)劃問題，提出了逐加步長搜索算法實現(xiàn)了路徑節(jié)點搜索，將路徑節(jié)點依次連接得到機器人完整路徑。仿真結(jié)果驗證了該方法的有效性和可行性。該算法的優(yōu)點是：求解過程中計算量小，提高了路徑規(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*算法的空間機械臂避障路徑規(guī)劃[J].機械工程學報，2010，46（13）：109-115.

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

[6]蔡自興.機器人學[M].北京：機械工業(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].南京：南京大學，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 機器人路徑仿真圖

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)新試驗區(qū)專項資助（20130205）

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