王馨蕊， 薛 梓， 黄 垚， 张福民

(1.天津大学，天津 300072；2.中国计量科学研究院，北京 100029)

1 引 言

随着工业智能化和自动化的快速发展，多自由度非正交系统凭借其高效灵活性、高精度和高稳定性等特点在工业领域得到了广泛应用。以工业机器人为典型代表，机器人的轨迹规划可使其运动连续平滑，减少冲击，很大程度上提高了机器人的稳定性和精度，是机器人运动学和动力学的综合应用。目前常用的轨迹规划方法主要分为在关节空间和在笛卡尔空间进行轨迹规划。在笛卡尔空间中的轨迹规划计算繁琐，且在实际轨迹中存在奇异点；在关节空间中的轨迹规划无机构奇异性但避障性不好[1]。目前针对这些缺陷和不足，研究了各种解决方法，例如在关节空间内，Chwa等[2]在样条曲线规划基础上施加速度和力矩约束，可以使机械臂合理避障并有效提升速度；Saravanan等[3]针对B样条曲线的不足，应用NURBS曲线算法规划机器人的运动轨迹来提升其精度和稳定性；Xu X R等[4]针对规划时速度和加速度曲线不连续的问题，采用了3-5-3样条函数的分段轨迹规划方法进行关节空间插补，简化了规划过程并提高了精确度。

本文研究的目标是完成对不同测量设备的统一溯源，保证其量值一致可比。用于测量多自由度非正交系统的设备种类繁多，例如激光跟踪仪、计算机视觉测量设备等[5,6]，然而其各自的校准精度、连续性、速度等差异较大，使用时会造成用户对同一系统采用不同设备测量时得到不同的位姿结果，无法准确判定、比较和评价。因此要规划一个标准路径，既能保证测量速度又能保证测量设备均能完整测得其空间曲线，并能反映出不同测量设备间的差异。

本文以工业机器人为研究对象，建立D-H参数模型，利用MATLAB Robotics toolbox对其运动学进行仿真验证，并对三次和五次多项式插值法等多种不同运动轨迹进行仿真，观察不同轨迹对设备的测量过程产生的影响；比较机器人在不同轨迹下的关节平顺性，为后续进一步研究和规划标准路径，进行测量设备的溯源提供理论基础。

2 工业机器人运动学分析

2.1 正向运动学

机器人的正向运动学是指给定各关节变量即角度值，通过计算公式，求解末端执行器的位置和姿态，实现从关节空间到笛卡尔坐标系的转换。首先建立机器人模型，本文采用最基本的D-H参数模型法[7]建立空间坐标系，如图1所示，得到机器人的各连杆参数和关节变量值，见表1。其中αi为杆件扭角，ai为杆件长度，di为关节距离，θi为关节转角。

根据相邻连杆相对位置的4个齐次变换矩阵及其在基坐标系下的变换规则，可以得到相邻两连杆间的变换矩阵为：

(1)

式中：si= sinθi；ci= cosθi；i=1,2,3,…，6。

图1 六自由度工业机器人D-H参数模型空间坐标系Fig.1 D-H parameter model space coordinate system of six-degree-of-freedom industrial robot

表1 六自由度工业机器人D-H参数表Tab.1 D-H parameters of six-degree-of-freedom industrial robot

将D-H参数代入可以得到各相邻关节间的变换矩阵。根据坐标变换理论，将6个齐次变换矩阵相乘，可以得到机器人末端执行器相对于基坐标系的运动学矩阵：

(2)

其中：

nx=c1[c23(c4c5c6-s4s6)-s23s5c6]+s1(s4c5c6+c4s6);

ny=s1[c23(c4c5c6-s4s6)-s23s5c6]-c1(s4c5c6+c4s6);

nz=-s23(c4c5c6-s4s6)-c23s5c6;

ox=-c1[c23(c4c5s6+s4c6)+s23s5s6]+s1(c4c6-s4c5c6);

oy=s1[c23(c4c5s6+s4c6)-s23s5s6]-c1(c4c6-s4c5s6);

oz=s23(c4c5s6+s4c6)+c23s5s6;

ax=-c1(c23c4s5+s23c5)-s1s4s5;

ay=-s1(c23c4s5+s23c5)+c1s4s5;

az=s23c4s5-c23c5;

px=-c1[c23(c4s5d6-a3)+s23(c5d6+d4)-a1-a2c2]-

s1s4s5d6;

py=-s1[c23(c4s5d6-a3)+s23(c5d6+d4)-a1-2a2c2]+

c1s4s5d6;

pz=s23(c4s5d6-a3)-c23(c5d6+d4)-a2s2。

式中：s23= sin (θ2+θ3)；c23= cos (θ2+θ3)。

2.2 逆向运动学

机器人的逆向运动学是指给定机器人末端执行器的期望位姿，求解关节变量使得末端执行器达到期望位姿，将解算的关节变量传递给控制器，进而驱动各个关节旋转角度使机器人末端到达预期位置和姿态。因此逆向运动学的解算是机器人轨迹规划问题的重要部分。

但机器人逆向运动学的解具有不唯一性，六自由度关节机器人逆解一般分为8种情况，需遵循各关节运动范围限制和运动距离最小原则，选择最佳关节角度[9]。

3 轨迹规划

轨迹指机器人在运动过程中各个关节的位置、速度和加速度的变化联系起来形成的一条曲线。轨迹规划指根据客户需求、工作任务和机器人性能来设定机器人末端执行器的期望运动轨迹，保证机械臂末端位置、速度和加速度连续、轨迹平滑，避免过渡处出现较大振动和冲击[10]。对工业机器人进行轨迹规划时要考虑到各方面因素，包括机器人各部分结构和零部件间的摩擦，要保证运动过程的平稳性[11～13]。

轨迹规划与时间函数有关，关节空间规划是已知各关节变量的时间函数，利用其连续的一阶、二阶导数描述空间轨迹，计算速率快但避障性不好；而笛卡尔空间规划是将机械臂末端的位置、速度和加速度用时间函数表示，通过逆向运动学解算各关节角度值，得到机器人末端的空间轨迹，轨迹直观易观察，但逆运动学解算计算量大，且可能存在奇异点。

3.1 关节空间轨迹规划

关节空间轨迹规划是利用关节变量的时间函数描述机器人的运动轨迹。对空间中已知点的坐标采用逆运动学解算出关节角，在关节空间中采用适当的算法拟合出光滑的插值函数，使机器人末端能够依次通过起始点到目标点间的所有路径点，保证各关节在每段路径的运动时间相同，使各关节同时达到目标点，机器人才能按计划轨迹到达预期位姿。通常采用多项式插补和抛物线线性拟合方法进行规划[14]。

3.1.1 三次多项式插值

设三次多项式的位移-时间函数方程为：

θ(t)=a0+a1t+a2t2+a3t3

(3)

4个约束条件为：起始点和终止点关节角度已知，且速度均为0。

(4)

(5)

可以解得三次多项式的系数为：

(6)

但三次多项式插值法具有局限性，在多个路径点间进行连续关节轨迹规划时,会出现加速度变化不平滑的问题，因此选择五次多项式来保持路径点的加速度平滑连续。

3.1.2 五次多项式插值

设机器人运动过程中关节角度是关于时间的函数为五次多项式：

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

其中对起始点和终止点的角度、速度和加速度均有要求，需满足约束条件有：

(7)

(8)

根据约束条件，可以求解出五次多项式的6个系数为：

(9)

利用五次多项式插值拟合得到的轨迹曲线、速度曲线、加速度曲线平滑连续，但轨迹曲线的曲率无规则变化，控制机器人运动变得困难。

3.1.3 抛物线过渡的线性插值

线性插值能够实现控制轨迹曲率有规则变化，但直接线性插值会导致起始点和终止点的关节速度不连续，因此需在每个路径点附近用一段抛物线进行拟合，使整个运动过程平稳，速度连续，加速度不发生突变，其原理如图2所示。

图2 抛物线过渡的插值原理图Fig.2 Interpolation principle diagram of parabolic transition

在过渡区域(t0,tb)内，所有路径都经过中点H且对称，且中点速度等于直线段速度。根据确定的加速度值可以解出：

为保证tb有解，需保证加速度足够大，满足：

拟合区域加速度越大，抛物线长度越小，直线段长度越大；当速度增大到一定程度时，抛物线长度减小为0，路径变为一条直线[10]。

3.2 笛卡尔空间轨迹规划

笛卡尔空间轨迹规划是根据已知的末端执行器位姿矩阵，利用逆运动学求解关节角度。主要方法有直线插补法和圆弧插补法[15]。

3.2.1 直线插补

直线插补是已知机器人末端的起始点和终止点位姿，求解各路径点位姿。以图3为例，已知起始点坐标和终止点坐标，机器人以速度v沿直线运动，插补时间间隔为t。可以通过计算直线轨迹长度，时间间隔t内机器人运动距离，插补总次数和机器人沿各坐标轴的运动增量，得到各插补点坐标值为：

(10)

再将坐标值转化为关节变量，利用关节空间中的插补方法进行轨迹规划。

图3 笛卡尔空间直线插补Fig.3 Linear interpolation of Cartesian space

3.2.2 圆弧插补

1) 平面圆弧插补

图4为平面圆弧插补原理图，假设机器人末端以速度v沿圆弧轨迹运动，t为插补时间间隔，可以求得由已知平面内不共线的3点确定的圆弧半径R；由总圆心角θi+1=θi+Δθ，时间间隔t内角位移量和总插补步数N，由式(11)可推算出路径中任意插补点坐标。

2) 空间圆弧插补

如图5所示，已知不共线3点确定一半径为R的圆弧，与坐标面相交于直线AB，BC,CA。

图4 平面圆弧插补原理图Fig.4 Plane arc interpolation schematic

(11)

设ZR轴与Z0轴夹角为α，XR轴与X0轴夹角为θ，圆心OR在基坐标系下的坐标为(XO，YO，ZO)，则圆弧坐标系向基坐标系过渡的转换矩阵Tr为：

(12)

图5 空间圆弧与基坐标系的关系Fig.5 Relationship between space arc and base coordinate system

基坐标系中的坐标值向圆弧坐标系转换可由Tr的逆矩阵表示：

(13)

4 仿真验证

本文基于MATLAB中的Robotics toolbox进行仿真，该工具箱涵盖了很多机器人研究中的重要函数，为研究机器人的运动学、轨迹规划等问题及图形仿真实验提供了支持[16]。

基于本文仿真可以验证机器人运动学正解和逆解的正确性，确定轨迹规划的较优空间，比较多项式插值法的不同次数时轨迹、速度和加速度的差异。

4.1 机械臂运动学仿真

首先建立数学模型，运用Link函数结合D-H参数对各杆件建模；再运用SeriaLink函数根据各杆件之间的关系，对模型命名，运行程序可得到图6模型。

图6 工业机器人模型Fig.6 Industrial robot model

正向运动学仿真是利用工具箱中的fkine函数，

T=fkine(robot,[0 0 0 0 0 0]),

得到运动学矩阵T：

逆向运动学仿真需调用工具箱中的ikine函数，已知机器人末端位姿求解关节角度Q，Q=ikine(robot,T)，代入正向运动学位姿T可以求得机器人在该位姿下的六轴关节角度值Q：

Q=1.0×10-16[-0.893 3 0 0 -0.446 6 0 -0.446 6]

可以看出仿真结果与逆向运动学方程求解一致。

4.2 轨迹规划仿真

4.2.1 机器人工具箱轨迹规划函数仿真

在关节空间中,使用jtraj函数进行轨迹规划，规定起始点和终止点的关节角度和轨迹点数，使用正向运动学函数，运行程序即可得到如图7所示轨迹模型。

图7 jtraj函数生成轨迹模型Fig.7 Trajectory model using jtraj function

在笛卡尔空间中,使用ctraj函数进行轨迹规划，规定起始点和终止点位姿和步长，即可得到如图8所示的直线轨迹；若先进行逆运动学求解出关节角度，再用jtraj函数进行轨迹规划，可以得到图10所示的曲线轨迹，机器人末端在X,Y,Z方向上的位置变化如图9和图11所示。

图8 ctraj函数生成的直线轨迹Fig.8 Straight track using ctraj function

图9 直线轨迹机器人末端位置变化图Fig.9 Linear path robot end position change diagram

图10 逆向运动学和jtraj函数规划轨迹Fig.10 Trajectory planning using reverse kinematics and jtraj function

图11 曲线轨迹机器人末端位置变化图Fig.11 Curve path robot end position change diagram

在机器人工具箱中利用两种函数进行轨迹规划，可以看出在笛卡尔空间中可以较好观察机器人的实时规划动态，但计算量较大且机构具有奇异性；而在关节空间中可以控制关节速度和加速度，并对关节力矩进行约束，随时调整轨迹，可有效避免机器人冗余度和机构奇异性问题。因此本文设计标准路径时选择在关节空间中进行规划。

4.2.2 多项式插值法轨迹仿真

设定起始点和终止点的角度、速度和时间等参数，解算出三次多项式和五次多项式，编程绘制轨迹。图12所示为三次多项式和五次多项式插值法所得轨迹规划。由图12可以看出：三次多项式插值法所得轨迹规划虽然位置曲线连续平滑，但速度和加速度曲线明显不平滑，且在路径点处加速度会发生突变，对机器人运动产生冲击，损坏电机和减速器；采取同样的时间参数对五次多项式轨迹规划进行仿真，其速度和加速度曲线的平滑性和稳定性明显优于三次多项式插值法。

图12 多项式插值法轨迹规划Fig.12 Trajectory planning by polynomial interpolation

以机器人关节1～6为例，设起始点关节角度为Q1=[-π/12π/3π/60-π/6-π/12]，终止点关节角度为Q2=[π/4-π4π/12π-π/33π/4],分别采用三次多项式插值法和五次多项式插值法进行轨迹规划，得到的位置、速度、加速度曲线如图13～图15所示。

图13 多项式轨迹规划的位置曲线Fig.13 Position curve of polynomial trajectory planning

图14 多项式轨迹规划的速度曲线Fig.14 Speed curve of polynomial trajectory planning

图15 多项式轨迹规划的加速度曲线Fig.15 Acceleration curve of polynomial trajectory planning

由图可以看出：在多项式插值法中多项式次数越高，各关节的位置、速度和加速度曲线越平滑连续，可以有效避免激光跟踪仪的丢光挡光等问题，也能够增加视觉测量设备测量的实时性。

5 总结与展望

本文以工业机器人为研究对象，建立D-H坐标系和参数模型，推算了机器人两连杆间的位姿变换矩阵和正向、逆向运动方程，使用MATLAB Robotics Toolbox进行了仿真验证；分析研究了在关节空间中的三次和五次多项式插值法，以及在笛卡尔空间中的直线插补和圆弧插补，并在机器人工具箱中对不同轨迹规划进行仿真，模拟机器人各关节在运动过程中的变化，总结不同轨迹对测量设备产生的影响，得出在设计标准路径时选择在关节空间中采用高次多项式插值法的初步结论。后续还需进一步研究在关节空间中优于高次多项式插值法的轨迹规划方法，保证测量设备的连续性和实时性，为进行测量设备的溯源技术研究提供理论基础。