基于5次非均匀B样条曲线的4自由度机器人轨迹规划

王 青， 叶明露， 贾秀海

(西安工程大学 机电工程学院， 陕西 西安 710048)

机器人的轨迹是指机器人根据外部环境任务要求，从起点运动到终点的一段运行轨迹。通常任务会给定首末端点或一些中间路径点，再通过运动学逆解获取相应的关节角度，最后利用插值函数对轨迹进行插值计算，力求获得光滑连续没有冲击突变的运行轨迹，这一过程通常称为轨迹规划。

机器人的轨迹规划是运动控制的前提，机器人在运动过程中能够快速平稳、连续无冲击地完成指定任务是研究者追求的目标[1]。文献[2-3]中对3次多项式插值方法进行了研究，规划的轨迹虽然满足对位置和速度的任务要求，但加速度存在突变，这会产生冲击振动，增加机器人的磨损进而影响其使用寿命。文献[4]中提出了一种考虑状态和输入约束的解析方法，但该方法的模型建立及计算较为复杂，对机器人的控制器性能要求较高。文献[5-6]中研究了5次多项式插值方法，实验结果表明加速度无突变，整体精度和稳定性比3次多项式好，但并未考虑加加速度对机器人运行轨迹的影响。

机器人的加加速度存在突变，往往意味着加速度曲线存在尖点，即加速度不光滑连续，这会增加机器人运行过程中的不稳定性，影响工作的完成[7]。李黎等[8]利用5次和7次等高阶多项式进行插值，虽然弥补了3次多项式插值的缺点，但是随着多项式阶次的提高，约束条件、计算复杂程度亦会增加。SEDDAOUI等[9]在笛卡尔空间中进行轨迹规划，利用动力学耦合效应完成目标的抓取，但并未考虑到逆运动学方程的存在而导致的动力学奇异性问题。针对此问题，文献[10]中提出了一种结合阻尼最小二乘和反馈补偿的方法，将轨迹规划问题转化为多目标优化问题，有效避免了奇异性的影响。文献[11-12]中分别对4-3-4和3-5-3分段多项式插值方法进行了研究，结果表明轨迹和速度曲线均平滑连续，但加速度曲线不平滑，加加速度曲线不连续。文献[13]中采用3次B样条曲线进行机械臂的轨迹规划研究，并基于随机分布示教点对机械臂进行轨迹规划；文献[14]中也采用3次B样条曲线进行机械臂的轨迹规划研究，采用复合柯特斯积分公式完成了对自由曲线的拟合，但都未考虑3次B样条曲线存在的加加速度突变问题。文献[15-16]中以时间、冲击等为优化目标，采用5次非均匀B样条插值方法进行轨迹规划，结果表明机械臂轨迹精度与稳定性均较好，工作效率显著提高。

综上分析，轨迹规划中常用的低阶多项式和分段多项式插值都存在加加速度不连续光滑的问题，高阶多项式虽然保证了加加速度的连续性，但是约束条件和计算复杂性都显著增加；而低阶B样条曲线插值曲线同样也不能保证加加速度的连续光滑性。针对上述问题，课题组以某公司SCARA 4自由度机器人为对象，研究一种5次非均匀B样条曲线插值方法，以期得到一种光滑性和稳定性良好的机器人运行轨迹。

1 SCARA 4自由度机器人

课题组以图1所示的SCARA 4自由度机器人为对象进行研究。该机器人由3个旋转关节和1个移动关节组成，各旋转关节轴线互相平行，在水平面内进行定位和定向，移动关节用于实现末端执行器在垂直方向上的的运动。这种机器人具有速度快、精度高和柔性好等特点。

图1 SCARA 4自由度机器人

对机械臂进行轨迹规划的前提是对机械臂进行运动学分析。由各关节的姿态变量经齐次变换矩阵计算得到末端执行器的位姿，这是正运动学解；由末端执行器的位姿逆推得到各关节的姿态是运动学逆解。首先对机械臂各连杆关节从0开始依次编号，然后根据D-H参数法建立机械臂连杆坐标系如图2所示。

图2 机器人连杆坐标系

由建立好的连杆坐标系得到表1所示的各连杆参数。其中，l1=250 mm，l2=150 mm，θ1，θ2和θ4为旋转关节变量，d3为移动关节变量。

表1 机器人D-H连杆参数

2 B样条插值曲线构造

2.1 B样条曲线概述

B样条曲线是贝塞尔曲线的一种一般化，具有几何不变性、凸包性和局部支撑性等许多优良性质，在计算机图形学中应用广泛[17]。

给定n+1个控制点Di(i=0,1,2,…,n),可定义k次B样条曲线的表达式：

(1)

式中，Ni,k(u)是k次B样条基函数。

由Cox-de Boor递归公式推导如下：

(2)

U是m+1个非递减数的集合，u0≤u1≤u2≤…≤um，ui称为节点，集合U称为节点向量，n，m和k必须满足m=n+k+1。特别地，规定0/0=0。[ui,ui+1]是第i个节点区间，某些ui可能相等，节点区间会不存在。如果一个节点ui出现k次(即ui=ui+1=…=ui+k-1)，其中k>1，则ui是一个重复度为k的多重节点。为保证样条曲线经过首、末端点，一般令两端点处重复度为k+1，即u0=u1=…=uk=0，un+k=un+k+1=…=un+2k=1。基函数Ni,k(u)在[ui,ui+k+1]上非零,即Ni,k(u)在k+1个节点区间[ui,ui+1],[ui+1,ui+2],…,[ui+k,ui+k+1]上非零，在其他区间均为零。因此B样条曲线也可表示为：

(3)

式(3)体现出k次B样条曲线上的点p(u)最多和k+1个控制点有关，与其他顶点无关。也就是说如果想要修改机械臂的某一段轨迹，只要重新计算该段的轨迹控制点，并不用对整条轨迹重新进行计算。

2.2 5次B样条插值曲线

k次B样条曲线具有Ck-1连续的特性[18]。且为保证轨迹曲线无冲击突变，插值函数至少4阶可导，即要求k≥4。由于4次B样条加加速度存在突变，且高次B样条函数计算更为复杂，因此课题组采用5次B样条函数进行曲线插值，既降低了计算难度又可保证运动轨迹平滑、连续、无突变。

由2.1节内容可知若想规划一条B样条曲线，则需求解出控制点di和节点矢量U。然而B样条虽由一系列控制点决定，但并不经过这些控制点，所以在轨迹规划时，选取的机械臂末端路径点并不能直接作为控制点使用，但是可以将这些路径点作为型值点，进而反求出曲线上对应的控制点。

具有n+1个型值点pi(i=0,1,2,…,n)的k次B样条曲线将由n+3个控制顶点Di(i=0,1,2,…,n,n+1,n+2)以及节点矢量U=[u0,u1,…,un+k+3]定义。节点矢量最后的下标是n+k+3，是因为节点数比控制顶点多出的数量为r=k+1，而控制顶点数比型值点数多2个，多出的2个为与首末型值点重合的点。

(4)

(5)

由Cox-de Boor递归公式推导出机械臂关节各阶导矢的递推公式：

(6)

设位置时间节点序列为：

Q=(pi,ti),i=1,2,…,n。

(7)

为使规划的轨迹经过选定的路径点，下面进行控制点Di的求解，列出如下n+1个方程：

(8)

曲线所要求解的控制点为n+k个，即需要n+k个方程，因此在式(8)提供的n+1个方程的基础上再补充k-1=4个条件：

(9)

式中：v0和ve分别表示关节起始和终止时的速度;a0和ae分别表示关节起始和终止时的加速度。

由式(8)和(9)可得到下列n+5个方程组：

AD=p。

(10)

式(10)中：

(11)

(12)

由式(10)可反解出控制点：

D=A-1p。

(13)

根据求出的控制点和节点便可得到关节在t∈[t0,tn]的轨迹，由式(6)可推导出其各阶导曲线：

(14)

由式(14)可知，只要知道关节的角度位置与其对应的时间节点，便可求解出其轨迹曲线及相应的速度v(t)、加速度a(t)和加加速度j(t)曲线。

3 仿真实验

以SCARA 4自由度机器人为研究对象，利用MATLAB中的机器人工具箱Robotics Toolbox对机器人进行建模，其三维模型如图3所示。左侧Teach部分为示教模块，x，y和z为机器人末端坐标；R，P和Y为末端绕定轴的旋转角，分别代表滚动、俯仰和偏转；q1，q2和q4分别为第1、2和4关节的角度；q3为第3关节的移动距离，可通过拖动滑块调节数值以控制机械臂的运动。

图3 机器人三维模型

将机器人末端执行器的运行路径离散化，选择一组8个路径点作为型值点，通过建立的机器人模型进行逆运动学的求解，得到各关节在路径点处对应的位置如表2所示。由于关节4本身的转动对机械臂整体的运行轨迹并无影响，所以为简化计算过程将其值均设为零。关节3为移动关节，其余为转动关节。

表2 各关节节点

针对上述数据和建立的模型，采用第2节中构造的5次B样条函数对其轨迹曲线进行插值，得到4自由度机器人末端执行器的运行轨迹曲线如图4所示。从图中可以看出插值曲线均通过8个型值点，有10个控制点，通过调节控制点便可改变插值曲线的形状。图5所示为机器人末端执行器的实时轨迹，与图4中5次B样条函数插值出的曲线基本一致。

图4 5次B样条插值的轨迹曲线

图5 机器人末端执行器运行轨迹

设定总运行时间为20 s，起始和终止时刻的速度、加速度均为零，进行5次B样条插值，得到如图6所示的的机器人各关节位置、速度、加速度和加加速度曲线。为对比分析，同时进行了5次多项式轨迹规划，得到结果如图7所示。

图6 5次B样条插值的各关节运动曲线

由以上仿真结果可看出：

1) 机器人按照5次多项式与5次B样条插值分别得到的运行轨迹运动，均能完成任务，且起始和终止时刻的速度、加速度均为零，满足约束要求。

2) 观察图6(a)和图7(a)的位置曲线发现按5次多项式插值方法规划出的运动轨迹明显没有按5次B样条插值得到的运动轨迹光滑流畅，即机器人按5次多项式插值规划轨迹运动时不断地转弯动作，使得机器人实际运动难度加大。

图7 5次多项式插值的各关节运动曲线

3) 观察图6(b)、6(c)和图7(b)、7(c)的速度和加速度曲线均发现5次多项式插值规划得到的速度和加速度曲线虽然也光滑连续，但是曲线波动非常剧烈，且波动幅度也非常大，速度波动区间为-1.05～1.22 rad/s，加速度波动区间为-1.75～1.75 rad/s2；反观5次B样条插值得到的速度和加速度曲线波动则要缓和得多，且波动范围较小，分别为-0.87～1.05 rad/s和-0.79～0.61 rad/s2。速度和加速度波动幅度小、波动频率低都意味着机器人运行更平稳，因此5次B样条插值轨迹显然要好很多。

4) 由图7(d)的加加速度曲线发现5次多项式对应的加加速度曲线变化非常剧烈，波动范围为-13.96～15.71 rad/s3，且存在很多突变点，说明机器人在不断地加速和减速，这会影响机器人运动的平稳性；在机器人末端执行器载有目标物的情况下，非常容易导致目标物的掉落，严重影响机器人的有效工作。图6(d)5次B样条插值的加加速度曲线则波动较为缓和，波动范围为-0.70～0.70 rad/s3，且曲线光滑连续，说明对应的机器人加减速较为缓慢和连续，即机器人运动平稳。

综上可得，与5次多项式的插值曲线相比，5次B样条插值的曲线更加光滑顺畅，波动更少且峰值较低。这表明机器人在整个运动过程中顺畅连续不会发生抖动卡顿等现象，能够有效地完成工作任务。

4 结语

课题组以SCARA 4自由度机器人为研究对象，采用5次B样条曲线对其运行轨迹进行了规划研究。利用弦长参数法和平均值法选取节点，并用离散路径点反解出控制点，通过指定端点导矢并且保证起始终止时刻速度、加速度都为零，得到机器人轨迹曲线。通过MATLAB仿真实验，并与5次多项式插值结果进行对比，得出5次B样条插值的曲线更加光滑顺畅，波动更少且峰值较低，机器人末端执行器运行路径平稳顺滑，各关节位置、速度曲线光滑连续，加速度以及加加速度曲线亦无冲击突变。分析结果表明：基于5次B样条轨迹规划的方法合理有效，可降低机器人运行过程中的振动冲击、减少磨损，进而延长使用寿命。本研究也为后续的轨迹优化和控制系统设计提供了参考。