工业机器人实时高精度路径跟踪与轨迹规划
谭冠政, 徐 雄, 肖宏峰
(中南大学 信息科学与工程学院,湖南 长沙, 410083)
摘要: 手部路径跟踪和关节轨迹规划是机器人应用领域中一个非常重要的课题。在以往的研究中,往往是通过在设定的路径上增加节点数和路径分段数来提高机器人手部跟踪设定路径的精度,但这种方法会导致在线计算量大幅度增加。针对这一缺陷,提出了一种新的实时高精度路径跟踪与关节轨迹规划方法。该方法通过在设定的手部路径上按一定规则额外选取多个附加节点,使每一轨迹段上的节点数由2个增加到4个,并利用1个3次多项式、1个正弦函数、1个余弦函数以及1个由正弦函数和1次多项式的乘积构成的函数来构造每一段的关节轨迹方程。计算机仿真结果表明,在路径分段数不变、关节轨迹方程总数不变以及计算量不显著增加的前提下,运用该方法能大幅度提高机器人手部跟踪设定路径的精度。
关键词:工业机器人;实时;高精度;路径跟踪;关节轨迹规划;附加节点
中图分类号:TP24 文献标识码:A 文章编号: 1672-7207(2005)01-0102-06
Real-time and Accurate Hand Path Tracking and Joint
Trajectory Planning for Industrial Robots
TAN Guan-zheng, XU Xiong, XIAO Hong-feng
(School of Information Science and Engineering, Central South University, Changsha 410083, China)
Abstract: The hand path tracking and joint trajectory planning is a very important research topic in the field of robot applications. Previously, researchers raised the accuracy that a robot′s hand tracks a specified path in Cartesian space mainly through increasing the number of knots on the path and the number of the path′s segments. However, this method results in the heavier online computational burden for the robot controller. Aiming at this drawback, a new kind of real-time and accurate hand path tracking and joint trajectory planning method for industrial robots was proposed. Through selecting some extra knots on the specified hand path by a certain rule, which enables the number of knots in each segment to increase from 2 to 4, and utilizing a cubic polynomial, a sinusoidal function, a cosinoidal function and a sinusoidal function with the amplitude of first-order polynomial to construct the joint displacement equation of each segment, this method can greatly raise the path tracking accuracy of robot′s hand but does not change the number of the path′s segments and the sum of joint displacement equations. It also does not markedly increase the computational burden of robot controller.
Key words: industrial robots; real-time; accuracy; hand path tracking; joint trajectory planning; extra knot
手部路径跟踪与关节轨迹规划是工业机器人应用领域中一个非常重要的课题,它在弧焊、喷涂、装配、切割、外科手术以及绘画等作业中有着广泛的应用[1- 14]。通常,为了减少工业机器人控制系统的在线计算负担,运动控制是在关节坐标空间中进行的。为此,首先必须在笛卡儿空间中设定的路径上选取足够多的点(称为节点),再将它们一一转换成对应的关节坐标。然后,在关节空间中将这些坐标用某种方式进行拟合,形成光滑的关节轨迹。在实际控制过程中,只需让各关节沿其对应的轨迹运动,所有关节运动的综合效果就表现为机器人手部近似地沿笛卡儿空间中设定的路径运动。显然,所取的节点数越多,路径跟踪的精度也就越高。但这会导致关节轨迹分段数和关节轨迹方程数量大大增加,在线计算时控制系统的运算量大幅度增加。从文献[10]的仿真实例可以看出,手部路径跟踪精度在首、尾两段路径中的各节点之间有所降低。在此, 作者针对这一缺陷,将对其首、尾2段路径上的关节轨迹方程进行改进。在这2段路径上,除引入1个正弦函数和1个余弦函数外,再引入1个新的函数,该函数由1个正弦函数和1个1次多项式的乘积构成,并且这2段上原来的4次多项式将降低为3次多项式。这样,在首、尾2段路径上,关节轨迹方程将由1个3次多项式与1个正弦函数、1个余弦函数以及新引入的函数相加组成。
1 含2个附加节点的路径跟踪与关节轨迹规划原理
设O0-X0Y0Z0为机器人的基础坐标系。如图1所示,假定沿机器人手部设定的路径从起点P0到终点Pn已选取(n+1)个节点,即P0,P1, …, Pi-1,Pi, … , Pn,它们将路径分成n段。相应地,在关节空间中,对某一关节j (j=1~N, N为机器人的自由度),从起点qj0到终点qjn, 其轨迹也被分成n段,整段轨迹可由n个方程来表示。
图 1 含2个附加节点的实时高精度路径规划原理
Fig. 1 Principle of hand path tracking with
two extra knots on each segment
现在,再在原来相邻的2个节点Pi-1和Pi之间,在设定的手部路径上额外选取2个附加节点Bi和Ci,如图1所示。它们分别位于时间点t=ti-1+hi/3和t=ti-1+2hi/3处。其中,ti-1为节点Pi-1对应的时间,hi为节点Pi-1与Pi之间的时间间隔。于是,从起点P0到终点Pn,节点总数将由原来的(n+1)个增加到(3n+1)个。由于节点总数增加了近2倍,所以路径跟踪精度也将提高近2倍。下面介绍关节轨迹构造方法。由于每次只涉及1个关节,为表述简单,将省去关节序号j。
图2所示为机器人某一关节随时间t变化的位移曲线。图中,q0,qi-1,qi及qn分别为节点P0,Pi-1,Pi及Pn对应的关节坐标;βi为附加节点Bi对应的关节坐标,γi为附加节点Ci对应的关节坐标。关节坐标可由节点的位置和姿态坐标经逆运动学计算求得。
图 2 机器人某一关节轨迹构造图
Fig. 2 Joint trajectory construction method
for some joint
设t0和tn分别是起点P0和终点Pn对应的时间,这样,区间[t0 , tn]也被分割成n段。在某一子区间[ti-1, ti]上有4个节点,即起、止节点Pi-1和Pi,附加节点Bi和Ci 。其中,Bi和Ci对应的时间分别为:
其中:hi=ti-ti-1。
1.1 在第2段到第n-1段轨迹上
在这些段轨迹上,引入附加节点Bi和Ci后,同时引入下面2个三角函数:
为了构造关节轨迹,再引入1个三次多项式,其表达式为:
在[ti-1, ti] (i =2,3,…,n-1) 上,关节轨迹Qi(t)可由式(4)与f1,f2叠加而成,即:
在[ti-1, ti](i=2,3,…,n-1)上,关节运动速度Q[DD(-*4]·[DD)]i(t)和加速度Q[DD(-*4]··[DD)]i(t)分别为:
函数f1和f2之所以这样选取,主要是为了保证计算节点Bi的关节坐标 βi时只与bi有关,而与ci无关;计算节点Ci的关节坐标γi时只与ci有关,而与bi无关,以简化计算。 βi和γi的计算公式为:
bi和ci满足下列关系:
因此,不论节点Bi和Ci的位置是在Q1i(t)的上方还是下方,只要适当选取bi和ci的符号,就能使Qi(t)通过Bi和Ci2个附加节点。
1.2 在第1段和第n段轨迹上
在这2段轨迹上,轨迹方程的构造与式(5)不同。
2 关节轨迹方程推导
设起点处,关节坐标为q0,速度为v0,加速度为a0;终点处,关节坐标为qn,速度为vn,加速度为an。下面依次建立各时间区间上的关节轨迹方程。
a. 在区间[t0 , t1]上,引入附加节点B1和C1后,同时引入下面3个函数:
在该区间上,关节轨迹Q1(t)可由式(4)与f1,f2及f3叠加而成,即:
其速度、加速度方程分别为:
代入起点t0处的初始条件Q1(t0)=q0, Q[DD(-*4]·[DD)]1(t0)=v0,Q[DD(-*4]··[DD)]1(t0)=a0,以及t=t0+h0/3,t=t0+2h0/3和t=t1处的位置条件Q1(t0+h0/3)=β1,Q1(t0+2h0/3)=γ1和Q1(t1)=q1,并假定t=t1处的关节速度v1=(q1-q0)/h1,可得7个方程。联立求解可得第1段轨迹方程(12)~(14)的各个系数为:
其中:
b. 在区间[ti-1 , ti] (i=2,3,…,n-1)上,关节位移、速度和加速度方程见式(5)~(7),其中相邻2段轨迹方程的系数可以建立递推关系。将t=ti-1,t=ti-1+hi/3,t=ti-1+2hi/3以及t=ti处的位置坐标条件qi-1,βi,γi和qi代入Qi(t),注意到ti-1处的速度连续条件Q[DD(-*4]·[DD)]i-1(ti-1)=Q[DD(-*4]·[DD)]i(ti-1),又设ti处的关节速度为:vi=(qi-qi-1)/hi。联立求解可得如下系数递推关系
:
其中：
c. 在区间[tn-1, tn]上,轨迹方程的推导与式(1)的推导类似,其关节位移、速度和加速度的方程分别为:
其初始条件和连续性条件分别为:Qn(tn-1)=qn-1,Q[DD(-*4]·[DD)]n(tn-1)=vn-1,Qn(tn)=qn,Q[DD(-*4]·[DD)]n(tn)=vn,Q[DD(-*4]··[DD)]n(tn)=an,Qn(tn-1+hn/3)=βn,Qn(tn-1+2hn/3)=γn。将这些条件代入方程式(30)~(32),解联立方程可求得第n段轨迹方程(30)~(32)的各系数为:
其中:
需指出的是,以上以3次多项式加三角函数所构造的关节轨迹,在附加节点Bi和Ci(i=1,2,…,n)处关节加速度是连续的,但在节点Pi(i=1,2,…,n-1) 处,关节加速度不连续。若要保证在Pi(i=1,2,…,n-1)处关节的加速度连续,则必须采用4次多项式加三角函数来构造关节轨迹。
3 超调性及振荡性
3.1 超调性
超调指关节坐标超出其上界或下界值。在图2中,在区间[ti-1, ti](i=2,3,…,n-1)上Q1i(t)是一个3次多项式曲线,它仅有1个可能的拐点或没有拐点。但是,由于增加了三角函数f1和f2,所以不论曲线Q1i(t)在区间[ti-1, ti]上有无拐点,新的关节轨迹Qi(t)在该区间上的最大值和最小值都不一定存在于qi-1,βi,γi以及qi四者之中。但是,作者特意规定正弦函数f1和余弦函数f2分别在其对应的附加节点Bi和Ci处取得幅值,在该区间内的其他点处,它们的值都小于其幅值。由于这种限制,因而新的关节轨迹Qi(t)在区间[ti-1, ti](i=2,3,…,n-1)上的最大值和最小值都不会大大超过max(qi-1,βi,γi,qi)或min(qi-1, βi, γi,qi)。因此,只要设定的轨迹不在关节边界值附近,在这些轨迹段上就不会发生超调问题。类似地,在区间[t0, t1]和[tn-1, tn]上尽管关节轨迹方程多增加了一个函数f3,但由于函数f1,f2和f3的幅值同样受到其上的节点和附加节点的关节坐标值的限制,所以,只要设定的轨迹不在关节边界值附近,在这2段轨迹上也不会发生超调问题。
3.2 振荡性
首先应指出,在区间[ti-1,ti] (i=1,2,…,n)上引入函数f1,f2和f3,不仅不会造成严重的关节轨迹振荡,而且有利于关节轨迹通过附加节点,从而提高轨迹跟踪精度。
在区间[ti-1, ti] (i=2,3,…,n-1)上,由于引入的三角函数f1和f2的周期等于区间[ti-1, ti]长度的4/3倍,f1和f2两者叠加后的曲线在该区间上将产生1次振荡。这样,视三次多项式函数Q1i(t)在区间[ti-1, ti]上有无拐点,新的关节轨迹Qi(t)在该区间上可能发生1~2次振荡。但是,由于区间[ti-1, ti]一般比较小,区间内关节坐标值的变化不会很大,加上三角函数f1和f2必须在附加节点处取得幅值,因而使f1和f2的幅值受到限制。在这些轨迹段上即使关节轨迹有振荡,其幅度也不会太大,不会给机械或电器部件造成损伤。在区间[t0, t1]或[tn-1, tn]上,函数f1,f2和f3的周期都等于区间长度的2倍,但由于f1和f3是正弦函数,而f2是余弦函数,所以,三者叠加后的曲线在该区间上将产生1次振荡。这样,视3次多项式函数Q1i(t)在区间[ti-1, ti]上有无拐点(i=1或n),新的关节轨迹Qi(t)在该区间上可能产生1~2次振荡。同样,由于函数f1,f2和f3的幅值都将受到节点和附加节点的关节坐标值的限制,所以,在这2个轨迹段上即使关节轨迹有振荡,其幅度也不会太大。
4 计算机仿真实例
以6自由度的PUMA560机器人为例,采用提出的方法对含2个附加节点的路径跟踪情况进行计算机仿真,并与文献[10]中所采用的方法进行比较。PUMA560机器人各坐标架之间的关系参数以及各关节变量的变化范围如表1所示。
为便于观察路径跟踪的精度,仿真时取机器人手部的设定路径为直线。在该机器人的基础坐标系O0-X0Y0Z0中,直线的起点P0和终点Pn的坐标分别设定为:Pox=400 mm,Poy=600 mm,Poz=-400 mm;Pnx=-500 mm,Pny=600 mm,Pnz=-360 mm。
利用计算机动态图形仿真技术,可在计算机屏幕上显示PUMA560机器人手部直线路径,如图3所示。假设将时间区间[t0, tn]等分成n个子区间,并设每一子区间[ti-1, ti] (i=1,2,…,n)的长度都等于h,则该直线路径的参数方程可表示为:
假定机器人手部沿该直线路径运动时的姿态不变,设其3个欧拉角为:=-65°,θ=185°,φ=-105°。仿真时,设关节速度矢量vθ=vn=[0,0,0,0,0,0];加速度矢量a0=an=[0,0,0,0,0,0]。并设t0=0 s,tn=6 s,分段数n=5 ,则h=1.2 s。仿真的采样周期取0.2 s。其仿真结果如图4所示。
图4给出了机器人手部跟踪笛卡儿空间中设定的直线路径时,分别在基础坐标系的X0,Y0和Z0轴方向的跟踪情况。其中,虚线表示采用文献[10]的方法所得出的机器人手部的跟踪路径,实线则表示采用本文提出的方法所得出的机器人手部的跟踪路径。从图中可看出,在首、尾2段,即第1和第5段,引入新的函数f3后,机器人手部跟踪直线路径的精度明显提高,更加接近设定的直线,从而证明所提出的方法可较有效地克服文献[10]中存在的缺陷。
表 1 PUMA560机器人的连杆参数
Table 1 Link parameters of PUMA560 robot
图 3 设定的机器人手部路径
Fig. 3 Specified hand path of PUMA560 robot
(a)—X轴跟踪轨迹; (b)—Y轴跟踪轨迹; (c)—Z轴跟踪轨迹
…—有2个附加节点并引入函数f1和f2的手部跟踪路径;
-—有2个附加节点并引入函数f1,f2和f3的手部跟踪路径。
图 4 机器人手部跟踪直线路径的情况
Fig. 4 Robot′s hand tracking a specified
straight line path
5 结 论
a. 提出了一种新的机器人高精度路径跟踪及关节轨迹规划方法,通过在每一轨迹段上增加2个附加节点并引入2~3个三角函数,在增加较少计算量的情况下,可大幅度提高机器人手部跟踪设定路径的精度。
b. 研究了关节轨迹方程的构造方法,并建立了相邻轨迹段关节轨迹方程系数的递推公式。
c. 以6自由度的PUMA560机器人为例,对所提出的方法进行了计算机动态图形仿真,证实了该方法的正确性和有效性。
参考文献:
[1]TONDU B, BAZAZ A. The Three-cubic Method: An Optimal Online Robot Joint Trajectory Generator Under Velocity, Acceleration and Wandering Constraints[J]. The International Journal of Robotics Research, 1999, 18(9): 893-901.
[2]GALICKI M. The Planning of Robotic Optimal Motions in the Presence of Obstacles[J]. The International Journal of Robotics Research, 1998, 17(3): 248-259.
[3]TAN Guan-zheng, LI Tan. Optimum Motion Planning of CS-I Industrial Robot with Two Manipulators [A]. Proceedings of IEEE 1997 International Conference on Intelligent Processing Systems [C]. Beijing, 1997.
[4]LIN C S, CHANG P R, LUH J Y S. Formulation and Optimization of Cubic Polynomial Joint Trajectories for Mechanical Manipulators[J]. IEEE Transactions on Automatic Control, 1983, 28(12): 1066-1078.
[5]CHAND S, DOTY K L. On-line Polynomial Trajectories for Robot Manipulators[J]. The International Journal of Robotics Research, 1985, 4(2): 38-48.
[6]BOBOW J E, DUBOWSKY S, GIBSON J S. Time-optimal Control of Robotic Manipulators along Specified Paths[J]. The International Journal of Robotics Research, 1985, 4(3): 3-17.
[7]AKEN L V, BRUSSEL H V. On-line Robot Trajectory Control in Joint Coordinates by Means of Imposed Acceleration Profiles [J]. Robotica, 1988, 6(2): 185-195.
[8]LUH J Y S, LIN C S. Approximate Joint Trajectories for Control of Industrial Robots Along Cartesian Path[J]. IEEE Transactions on Systems, Man and Cybernetics, 1984, 14(3): 444-450.
[9]TAN Guan-zheng, LIANG Feng, WANG Yue-chao. Real-time Accurate Hand Path Tracking and Joint Trajectory Planning for Industrial Robots (I)[J]. Journal of Central South University of Technology (English Edition), 2002, 9(3): 191-196.
[10]TAN Guan-zheng, HU Sheng-yuan. Real-time Accurate Hand Path Tracking and Joint Trajectory Planning for Industrial Robots (II)[J]. Journal of Central South University of Technology (English Edition), 2002, 9(4): 273-278.
[11]MEZOUAR Y, CHAUMETTE F. Path Planning for Robust Image-based Control[J]. IEEE Transactions on Robotics and Automation, 2002, 18(4):534-549.
[12]LANGE F, HIRZINGER G. Predictive Visual Tracking of Lines by Industrial Robots[J]. The International Journal of Robotics Research, 2003, 22(10): 889-903.
[13]KONG Fan-sen, GUO Kong-hui, SUN Guang-sheng, et al. Fuzzy Evaluation and its Simulation for Handling and Stability of Driver/vehicle/road Closed-loop System[J]. Chinese Journal of Mechanical Engineering, 2002, 38(11): 75-79.
[14]MEZOUAR Y, CHAUMETTE F. Optimal Camera Trajectory with Image-based Control[J]. The International Journal of Robotics Research, 2003, 22(10): 781-803.
收稿日期:2004-05-02
基金项目:国家自然科学基金资助项目(50275150); 中国科学院机器人学开放研究实验室基金资助项目(RL200002)
作者简介:谭冠政(1962-),男,湖南湘潭人,教授,博士,从事智能机器人系统与控制、人工智能及其应用和先进控制理论与先进算法研究
论文联系人: 徐 雄,男,硕士研究生;电话:0731-8716259(O); E-mail: wisdom8305@tom.com
