1 引言

工业机器人在笛卡尔任务空间内的轨迹规划包括位置、姿态插补及速度、加速度、加加速度曲线的规划^[1].一般的工业机器人轨迹规划控制系统往往只有直线和圆弧轨迹规划与插补，而这种系统对于复杂零部件的自由曲线插补能力有限，会产生较大的误差，同时由于自由曲线的特殊性会造成执行器在插补周期的相邻处的速度与加速度不连续，从而造成工业机器人产生振动，导致加工出的零部件表面不光滑^[2].

面向机器人任务空间复杂轨迹曲线的运动，国内外众多学者进行了相关理论算法研究与实践应用，与B样条理论相统一的NURBS曲线理论方法由于具有诸多优点而获得广泛的应用^[3].另外，NURBS不仅能够精确统一的表达标准解析曲线和自由曲线，而且其形状控制也相当灵活. Wang等^[4]利用短直线将空间轨迹进行等距划分，继而通过NURBS插补对短直线进行拟合处理，同时采用S型曲线速度控制，可减小速度突变对机械的冲击、超程、失步或震荡. Lin等^[5]采用弦高误差约束的方法对直线和圆弧轨迹插入节点进行分割，继而对每个分割段采用NURBS曲线拟合，同时可保证曲线衔接处的精确度、光滑性和边缘的连续性. Nien等^[6]采用插补前加减速控制和插补后加减速控制的混合控制方法，可实现空间曲线衔接尖角处的平滑过渡，为改善轮廓跟踪和轨迹轮廓精度提供了理论基础.

本文针对工业机器人在笛卡尔空间的自由曲线轨迹规划进行研究，采用阿当姆斯微分方程理论方法对空间自由曲线进行密化，可避免使用泰勒展开算法中的二阶求导运算，从而简化运算；同时采用基于德布尔 (de-Boor) 算法的NURBS曲线实时变步长插补算法，在这种插补算法下插补步长可根据插补误差大小和相关要求进行自动调整，从而满足实际应用的精度和速度的要求，同时也可以简化运算；采用正弦波加速度曲线运动速度控制，不仅能够满足C₂连续，加加速度曲线仍能满足一定的连续与平滑，并且运算量较小，可有效提高机器人轨迹平滑性和降低机器人在加工中的冲击和震荡.

2 NURBS曲线理论基础

任意一条k次NURBS曲线可表示为一分段有理多项式矢函数^[7-10]：

(1)

其中，ω_i称为权因子；d_i为曲线控制定点；N_i，k(u) 是由节点矢量U=[u₀，u₁，…，u_n+k+1]决定的B样条基函数，由广泛使用的德布尔-考克斯 (De Boor-Cox) 递推定义公式表示：

(2)

式中，规定.

由于NURBS曲线的计算需要反复的迭代，直接将参数u_k+1代入求解相应的曲线上的坐标需要很大的计算量，显然不能满足实时插补的需要，而采用德布尔 (De Boor) 算法^[10-12]，只需简单的加减乘除就可以很快计算出曲线上的点一级导数，从而避免了NURBS的直接求导和迭代^[6]，其计算过程如下：

NURBS曲线的齐次坐标形式控制顶点可写作：

(3)

套用非有理B样条曲线的de Boor算法，即可得到NURBS曲线上点的de Boor算法计算公式：

设u=[u_i，u_i+1]⊂[u_k，u_n+1]，可得

(4)

式中，

为得到NURBS曲线的长度，用于计算各插补周期的进给速度，采用复合辛普森 (Simpson) 求积公式法计算NURBS曲线的长度^[13]，具体计算步骤如下：

(1) 将NURBS曲线节点参数区间[u_i-1，u_i]分成n等份，路径p(u) 对参数u的微分公式为

(5)

将NURBS曲线在区间[u_i-1，u_i]的长度记为l_i，可得

(6)

对式 (5) 在区间[u_i-1，u_i]上进行n等份的复合辛普森求积运算，即：

简化得

(7)

式中.

(2) NURBS曲线路径总长L为

(8)

式中，k为NURBS曲线的次数，n为控制顶点数.

3 NURBS曲线的平滑插补计算 3.1 哈特利—贾德法求解节点矢量

给定NURBS曲线的控制点d_i(i=0，1，…，n)，预定义一条k次非均匀有理B样条曲线，还必须确定它的节点矢量U=[u₀，u₁，…，u_n+k+1]中的具体的节点值.

为便于对曲线在曲线端点的行为有较好的控制，本文在两端节点的重复度取为k+1，通常将曲线的定义域取成规范参数域，即u∈[u_k，u_n+1]且[u_k，u_n+1]=[0，1]，于是u₀=u₁=…=u_k=0，u_n+1=u_n+2=…=u_n+k+1=1，其余内节点需计算求解.

本文所采用的哈特利—贾德法与曲线次数的奇偶性无关^[3]，采用统一的计算公式，计算方法更具合理性，其计算公式如下：

(9)

式中l_j为控制多边形的各边长，即l_j=|d_j-d_j-1|.由式 (9) 可得：

(10)

3.2 阿当姆斯法参数密化

参数密化是指由3维轨迹空间到1维参变量空间的映射^[14-15]，在参数化插补方式下，数据的密化即表现为参数的密化过程，即由轨迹空间的进给步长ΔL映射到参变量空间以求取参变量增量Δu及下一参数坐标u_i+1=u_i+Δu_i.本文采用阿当姆斯微分方程对参变数进行密化，计算公式如下：

采用3步4阶阿当姆斯微分方程的隐格式：

(11)

将代入式 (11)，可得：

(12)

其局部阶段误差为

而泰勒公式展开法中阶段误差为Γ_n+1=O(T²).

插补周期T一般取0.001 s~0.002 s，可见该算法具有比2阶泰勒展开式法更高的计算精度.

为保证高速NURBS曲线直接插补的要求，保证实时插补的计算速度，采用前、后向差分 (如式 (13) 所示) 结合代替微分的方法对该算法进行简化：

(13)

将式 (13) 代入式 (11)，得到简化后的阿当姆斯微分方程插补算法迭代公式：

(14)

将参数作为参数插补的预估值代入NURBS方程，得到相对应的预估插补点：

(15)

从而得到对应的预估进给步长为

(16)

采用预估法得到的和期望步长ΔL之间存在的偏差，可用相对误差进行评定：

(17)

当误差δ在允许范围内时，可认为为所求p_i+1；否则，按式 (18) 进行修正 (修正原理如图 1所示)，直至达到δ允许范围内：

(18)

4 正弦波加速度曲线运动规划

为满足工业机器人轨迹平滑性要求提出的正弦波加速度曲线，加速度呈现正弦波曲线变化，如图 2所示，其加加速度呈现余弦波曲线，可保证规划的机器人末端执行器轨迹的平滑，减少机器人关节机构的机械振动^[16-18]. 图 2中，纵坐标Jerk、ACC、Vel分别为加加速度、加速度、速度；DT_max为加速度从0加速到最大加速度A_max的时间；V_s、V_e分别为起始速度值和终止速度值；P_s、P_e分别为起始位置值和终止位置值.

在 (0，DT_max) 时间段内，加速度曲线的正弦函数方程为

(19)

式中，t为变化时间.对式 (19) 进行1阶求导，可得到加加速度的函数方程为

(20)

当时，加加速度为最大，即.

根据图 2，可将规划轨迹划分为3段，即位移段D₁(加加速段)、D₂(匀加速段) 和D₃(减加速段)，可得3个轨迹段间的两个过渡点的速度V_a和V_b：

(21)

由式 (21) 可得，匀加速段运行时间，和加加速段与匀加速段总时间DT₁₂=DT_max+DT_unif=T-DT_max，式中T为从起始速度V_s到最大速度V_e的加速总时间.

根据划分的3段D₁、D₂和D₃，可得对应轨迹段的加速度函数，分别为

(22)

由式 (22) 可得，对应3个轨迹段对应位移量为

(23)

(24)

(25)

根据式 (21)~式 (25) 可得随时间变化的位置函数d(t)：

(26)

5 四元数的线性插值

四元数作为一种超复数，由实数加上3个虚数单位i、j、k组成，其本身的几何意义可理解为一种旋转^[19-21]，也可说它是4维空间上的一种代数结构.四元数通常表示为

式中，q₀为实部，q₁i+q₂j+q₃k为虚部.

当四元数的模时，称为单位四元数.单位四元数，可表示为刚体绕旋转轴 (x，y，z) 旋转σ角度，如图 3所示.

单位四元数集合和所有3维空间旋转有一一对应的关系，其组合成一个3维球面上在乘法下的旋转李群.所以四元数的乘法就代表了空间的旋转运动.因此，在规划机械臂末端从一种姿态R₁变化到另一种姿态R₂，可利用单位四元数进行轨迹姿态的插补.

由旋转矩阵R可得到刚体旋转轴和旋转角度，转换公式为

可以得到机器人末端起始姿态和目标姿态对应的四元数为Q_s和Q_e，计算姿态四元数函数采用四元数的“球形线性插值法”(spherical linear interpolation，即Slerp)：

(27)

式中，ω=arccos (Q_s·Q_e)(Q_s·Q_e是两个四元数的点积)，l(t)∈[0，1]通过上述运动规划得到.

在圆弧轨迹规划过程中，圆弧位置通过正弦波加速度曲线运动规划算法进行插补得到实时位置点P(t)，同时圆弧姿态也是运用正弦波加速度曲线运动规划算法对四元数参数进行球形线性插补算法得到实时四元数旋转Q(t)，然后将四元数旋转Q(t) 变换回旋转矩阵R(t)，与位置P(t) 组成齐次变换矩阵T(t)，即每次插补所得机器人末端的位姿，如式 (28) 所示：

(28)

6 实验仿真与验证

本文以埃夫特ER10L-C10型工业机器人为实验平台，该型号机器人适用于机床上下料、搬运、喷釉、打磨抛光等场合，其结构模型如图 4所示.

基于ER10L-C10型机器人，采用上述自由曲线轨迹规划算法和正弦波加速度曲线速度控制对机器人末端空间自由曲线的轨迹位姿进行规划，利用Matlab工具对轨迹规划进行仿真与验证.该型号工业机器人DH参数见表 1所示.

使用NURBS插补算法对工业机器人任务空间自由曲线的工作路线进行轨迹规划，通过Keba机器人控制器示教功能记录所需机器人末端执行器自由曲线控制点的位姿数据，并通过ER10L-C10型机器人实验平台得到自由曲线各控制点处机器人执行器末端相对于基坐标系的位姿齐次矩阵_T^BT₁、_T^BT₂，…，_T^BT₁₃为

通过上述算法，以3次NURBS曲线为例，曲线特征和各项参数分别为：控制顶点为齐次矩阵_T^BT₁，_T^BT₂，…，_T^BT₁₃的位姿数据；权因子假设全为1，即；插补周期T=0.001 s；允许最大加速度a_max=4.9 m/s²；最大加加速时间D_tmax=0.05 s；允许最大弓高误差h_max=0.001 mm. NURBS曲线轨迹规划仿真结果如图 5所示.

轨迹规划仿真过程中，实时检测轨迹规划插补期望步长L与弓高误差约束步长L_h、期望弓高误差h与约束弓高误差h_m，修正轨迹与修正前原轨迹的相对偏差r与弓高误差约束步长条件下的轨迹与修正前原轨迹的相对偏差r_h，检测结果分别如图 6~图 8所示.由v=L·T可知，自由曲线轨迹规划的插补速度可用插补步长与插补周期的乘积表示.

由仿真图 6~图 9所示，在对NURBS曲线进行插补的过程中，采用基于正弦波加速度曲线的速度控制对NURBS曲线的插补方法，采用轮廓误差和最大进给加速度对进给步长进行约束，即对进给步长的控制就是对进给速度的控制.整个插补过程中，插补速度陡坡即插补步长突变处，相应的曲线插补弓高误差明显增大，进给速度变化较平滑，并且进给速度接近最大值，速度波动很小；插补过程中弓高误差的波动被控制在最大弓高误差范围内且波动过渡平滑；插补过程中在弓高误差陡降处相对偏差增大，说明轨迹得以修正.由此可见，该算法可对基于NURBS的自由曲线轨迹规划进行有效的修正处理.

通过对NURBS曲线进行插补获得自由曲线轨迹位置插补点的同时，采用四元数对轨迹姿态进行插补，将获得的姿态插补数据转化为对应的超球面旋转矢量轴，旋转矢量轴变化轨迹如图 10所示.

由图 10可见，在正弦波加速度曲线速度控制下的四维空间超球面的旋转矢量过渡平滑，则在自由曲线轨迹规划中曲线的姿态可平滑过渡，同时可避免当欧拉角万向死锁的缺陷、等效轴法当旋转量为0时存在无法确定旋转轴的问题.

7 结束语

本文针对工业机器人在笛卡尔空间复杂零部件的加工，即零部件表面自由曲线的轨迹规划进行研究，提出了一种基于NURBS曲线的轨迹规划控制方法.采用阿当姆斯微分方程进行NURBS插补计算，结合正弦波加速度曲线速度控制对插补过程中曲线位姿与姿态的加速度、加加速度进行控制.该方法为机器人轨迹规划提供了一种高效的位姿规划的解决方案，可避免使用泰勒展开算法中的二阶求导运算，升降速控制不仅能够保证C₂连续且计算量相对较小，能够较好地满足机器人控制的实时性要求，具有计算速度快、鲁棒性强的特点.通过测试和仿真，可以看出所提算法的有效性.该方法能够有效提高工业机器人的工作效率和工作质量，能够减小速度波动，改善机器人的工作环境.