基于高阶非均匀有理B样条插补的多轴运动控制方法研究

张旭阳,鲁文其,鲍 敏，王浩亮，潘国忠

(1.浙江理工大学机械与自动控制学院，浙江 杭州 310018；2.江苏远东电机制造有限公司，江苏 泰州 225300；3.浙江禾川科技有限公司，浙江 杭州311100)

0 引言

在工业生产中很多复杂的运动都需要依靠多轴联动来完成[1-2]。如何让多轴运动控制系统更好地完成作业一直是运动控制领域中的研究热点和难点[3]。为了在现代工业中提高生产制造过程的作业质量，从算法层面提高多轴控制系统的性能显得尤为重要[4-7]。轨迹规划是多轴运动控制系统的重要组成部分，其拟合精度和平顺性关系到末端位姿的精确度。传统的三次多项式插补算法在作业中会产生加速度不连续，导致电机抖动和冲击，五次多项式插补在多轴运动过程中对加加速度无限制导致设备产生较大冲击[8-10]，而且三次和五次多项式插补在插补过程中无匀速运行段，因此在很多工作场景中不适合。在梯形插补过程中因为其加速度不连续从而切换点产生的加加速度值为无穷大，很可能造成设备的损伤[11]。S曲线插补算法虽然可以解决梯形插补算法中的加速度不连续性，能够保持最大速度运行，但是插补轨迹不能进行局部修改[12]。马睿等[13]通过三次多项式插值进行轨迹插补，为保证运动中速度、加速度的有效性，引入了2个虚拟插值点。但是由于三次多项式根本不能保证加速度平滑性和加加速度连续性，在实际运行过程中会造成比较大的冲击和震荡。游文辉等[14]采用五次多项式进行轨迹规划，进行四轴机械臂轨迹规划插补。但是五次多项式插补算法中加加速度不能进行限制，对冲击不能进行有效限制，轨迹误差较大。王鹏[15]通过非均匀B样条曲线插补以及速度规划的原理，面向复杂工件的高速、高精度制造问题,提出了一种新的前瞻控制算法。首先根据加工精度要求得到自适应进给速度曲线，然后识别速度曲线中的速度极值点，分析速度极值点之间的速度干涉情况，对速度极值点进行优化，最后对每个插补区间分别进行速度规划。王光泽田[16]通过加减速规划，系统分析了传统的五次多项式在关节空间下进行规划时存在的难题，通过研究对此方法进行改进的方法，给出了优化后五次多项式规划的一些重要参数的解决方法以及如何提出约束条件的方法。实现了在保持五次多项式优点的前提条件下提高速度的结果。因为非均匀有理B样条能够通过统一的数学表达式精确表达任意自由曲线[17-18]，相应地其导矢也是具有同样的B样条性质。为了保证运动控制过程中的速度、加速度、加加速度的连续以及轨迹过渡的稳定性和平滑性[19-23]，对设计的运动控制器进行高阶非均匀有理B样条插补，达到速度、加速度和加加速度的连续性，避免产生突变，以及具有好的轨迹平滑性[24-26]。

为了解决传统轨迹插补算法轨迹不平顺和容易引起变速冲击问题，本文采用高阶非均匀有理B样条对多轴机械臂进行轨迹规划，对其速度、加速度和加加速度进行约束。

1 基于非均匀有理B样条的轨迹插补算法

本文提出基于非均匀B样条的多轴控制插补算法，其中包含B样条插补和非均匀B样条插补。

1.1 B样条插补原理

B样条函数是一种进行数值分析的特殊函数，由分段多项式进行构造。在插补过程中样条函数拥有比多项式函数更好的插值性质。

B样条函数的基本表达式为

(1)

pi(i=0,1,…,n)为控制曲线形状的顶点，又称为德布尔点；Ni,k(u)为k次规范B样条曲线基函数，也叫调和函数。且有

(2)

i为序号；k为B样条的次数；ui为节点；由节点ui组成的集合U即为节点向量；由控制点pi逐次连接而成的折线即是控制整个B样条曲线的多边形；[ui,ui+1]为第i个节点空间。

在集合U中，某些节点ui(ui=ui+1=…=ui+k-1)的值可能相等，也存在节点区间不存在的情况。若节点ui的值出现s次，则记重复次数为重复度s。若节点间距长度相同，则为均匀节点向量，反之为非均匀节点向量。

由该函数的基本公式(2)可知，k=0基函数为阶梯函数，当ui≤u≤ui+1时Ni,0(u)=1，u不在此区间时则Ni,0(u)=0。计算第i个函数Ni,k(u)时，必须要用到k+2个节点ui，ui+1，…，ui+k+1，k+2个节点构成k+1个节点区间[ui，ui+k+1]，而且区间[ui，ui+k+1]为基函数Ni,k(u)的支撑区间。因此，在参数轴上任意点u∈[ui,ui+k+1]处时，最多有k+1个非零的k次B样条Ni,k(u)，而其余k次B样条在此均为0。所以B样条曲线方程为

(3)

在运动插补过程中，需要B样条曲线首末控制点与运动插补过程中的首末插值点彼此重叠，以便于对曲线端点产生更好的控制作用。基函数与各个节点之间的相对间距长度有关，与u的绝对值的大小无关，为使B样条曲线具备相同阶次贝齐尔曲线的端点几何特性，针对开曲线与非周期性闭曲线，对首末节点取k+1的重复度并对曲线的定义域进行归一化的过程处理，相对于均匀参数化，弦长参数化可以将2个相邻数据点的参数值之差与其距离成一定的比例，更加合理。将曲线的定义域与规范参数域进行匹配，即取u∈[uk,un+1]=[0,1]。因此，其余节点u0=u1=…=uk=0，un+1=un+2=…=un+k+1=1。并通过哈德利-贾德法对剩余n-k个内节点uk+1,uk+2,…，un进行计算。记控制B样条曲线多边形的边长为li=|di-di-1|，节点为k重顶点时将控制多边形k条边的和进行替代，相邻分段连接点参数值之差与相邻顶点间距离成正比于实际存在差异，使

(4)

故内节点ui为

(5)

得到所有节点的值后，将节点值ui顺序代入式(1)，可得到B样条曲线上的点p(u)。但为了简便计算，本文采用德布尔算法进行p(u)的计算，即

(6)

1.2 非均匀B样条函数的构造

非均匀B样条曲线方程有理分式为

(7)

ωi(i=0,1,…,n)为权因子组成的集合，与控制顶点pi(i=0,1,…,n)逐次对应。首末权重ω0>0,ωn>0，其他权因子ωi≥0。A(u)为矢量函数，将其当做与原非均匀B样条曲线节点矢量值相同，而控制顶点则为ωipi(i=0,1,…,n)的k次B样条曲线，B(u)将其看做与原来的非均匀B样条曲线节点矢量相同，而控制顶点为pi(i=0,1,…,n)的k次B样条曲线。Ni,k(u) (i=0,1,…,n)为定义在非均匀、非周期节点向量U上第i段为k次B样条基函数，由式(2)可得。

假定所给的时间-位置点对应序列为(ti,pi)，则从起点到终点的总运动时间ttotal=tend-t0。为使2个数据点的参数值之差与其距离成比例，通过累计弦长参数法对时间节点ti进行归一化处理，得出k阶非均匀B样条的节点向量为u=[u0,u1,…,un+2k]，且

(8)

将k阶非均匀B样条曲线定义域[uk，uk+1]范围内的节点值逐次代入k阶非均匀B样条曲线方程中，让其分别等于n+1个插值数据点pi，得到符合插值要求的n+1个方程为

(9)

由式(9)可知对于k阶非均匀B样条曲线的构造，必须先对节点向量U和控制顶点pi加以确定。

k阶非均匀B样条曲线上节点处的r阶导矢pr(u),u∈[ui,ui+1]⊂[uk,uk+1]，根据下列递推公式可求得：

(10)

(11)

由式(11)可知，k阶B样条曲线的r阶导矢，通过k-r次的B样条曲线构建，可由式(11)递推求得它的控制顶点。

对于k阶非均匀B样条曲线，需要n+k个控制顶点di(i=0,1,…,n+k-1)。因此，就需要用k-1个由切矢条件来构建的附加方程。假设初始点速度、加速度分别为v0、a0，终止点速度和加速度分别为vf、af，再根据B样条曲线公式得r阶导矢公式，即：

(12)

(13)

(14)

(15)

联系式(9)～式(13)便可以得出n+k个关于样条曲线的控制顶点。

1.3 高阶非均匀B样条曲线数据预处理

运动插补的关键前提是对曲线相应的几何数据进行预处理，主要目标是得出对应的非均匀有理B样条曲线方程，使运动过程更加平稳、光滑以及可靠。选取五次非均匀B样条曲线作为研究对象，因此k取值为5，则节点向量中的首末2个节点的重复度均取为k+1次(6次)，则节点向量U=[0,0,0,0,0,0,u6,…,un,1,1,1,1,1,1]。1条非均匀B样条是由多段多项式曲线依次连接而成。对曲线中的单段多项式曲线进行研究是研究整条非均匀B样条曲线的关键。记pi(u)为第i段曲线，控制这段曲线的6个控制点pi与权因子wj相关，基函数Nj,5(u)同理。因此可知第i段曲线pi(u)可表示为

(16)

u∈[ui,ui+1]

在获得样条曲线的次数、权因子集合以及节点列表数据后，式(16)中只有Nj,k(u)未知，因此求出Nj,k(u)的值，根据式(2)可以推导出

(17)

由式(2)可知：

(18)

(19)

由式(18)和式(19)解得Ni,5(u)，同理可得Ni-1,5(u)、Ni-2,5(u)、Ni-3,5(u)、Ni-4,5(u)、Ni-5,5(u)，通过求得以上基函数便可求出i段曲线方程pi(u)，其他段方程也可由此求得，最后将这些曲线段依次连接起来，从而得到整条非均匀有理B样条p(u)。

1.4 高阶非均匀B样条控制点的反算

存在n个型值点的k次非均匀B样条曲线，存在n+2个未知控制点。为了保证速度、加速度等的约束，令控制点为n+k。五次非均匀B样条曲线，首末节点取重复度r=k+1=6，因此具有n+k+3个节点，由式(8)可知u1=u2=u3=u4=u5=u6=0，un+5=un+6=un+7=un+8=un+9=un+10=1。通过积累弦长参数化法求得ui。

在计算机辅助设计时，通常需要建立插值曲线用于已存在的曲线形状描述。非均匀有理B样条曲线能够通过给定型值点，从而构建通过该组型值点的曲线所对应的控制点，作为已存在的样条曲线的形状描述。控制点的反算开始通过积累弦长参数化法对节点矢量进行计算，其次要求首末端点的切线方向固定，即

(20)

1.5 基于高阶非均匀B样条的运动轨迹插补

确定控制点与节点向量之后，非均匀B样条曲线的插补即在曲线起始点与终止点之间针对不同的ui值，得到曲线上的点构成插补点的集合，由式(8)可知曲线上ui的值。

五次非均匀B样条曲线首末节点重复度s为6,整条曲线的定义域为u∈[u6,un+1]，由此可知式(16)中i的取值范围为6in，第i段非均匀曲线方程pi(u)和控制点pj为三维向量，表示为

(21)

因此边界条件得以确定，并且首末端点具有零曲率。设置每个控制点对应的权因子，对控制点进行反算。

2 机械臂轨迹插补仿真与分析

已知型值点，对样条控制点进行反算后，对整个样条曲线轨迹进行拟合插补，通过MATLAB建立多轴机械臂模型，进行插补算法仿真。

2.1 机械臂建模

在进行目标轨迹规划时，轨迹插补算法的好坏直接影响运动控制系统的整体质量，所以在进行多轴运动的轨迹规划插补前，需要对插补算法的可行性进行相关的理论建模和系统分析。但是在进行插补算法仿真之前，必须要对多轴机械臂进行建模。采用标准D-H法构建机械臂模型[27]，机械臂的D-H参数如表1所示。

表1 D-H参数

2.2 机械臂末端轨迹规划

通过给定的型值点[28]，生成期望位置，从空间原点(0,0,0)通过三次非均匀B条和五次非均匀B样条经过型值点(3,11,1)、(21,17,2)、(26,6,3)、(32,1,10)、(60,13,11)、(50,23,15)、(70,28,20)，计算出实际位置运行至(75,35,24)。三次非均匀B样条运行实际位置如图1所示，五次非均匀B样条运行实际位置如图2所示。虽然五次相对于三次计算量更大，但是通过图1和图2对比可知三次和五次非均匀B样条曲线都可以平顺地到达末端。

图1 三次非均匀B样条运行实际位置

图2 五次非均匀B样条运行实际位置

2.3 非均匀B样条控制点

通过给定型值点根据非均匀B样条算法生成从起始点(0,0,0)到终点(75,35,24)的样条曲线，根据控制点求解算法，求出三次非均匀B样条控制点依次连接生成如图3所示的控制多边形，五次非均匀B样条控制点依次连接生成图4所示的控制多边形。由于五次相对于三次控制点更多，因此，图4比图3控制局部修改作用更好。

图3 三次非均匀B样条控制多边形

图4 五次非均匀B样条控制多边形

2.4 高阶非均匀有理B样条算法仿真结果分析

为验证本文算法的可行性和有效程度，采用机械臂模型作为仿真的对象，对本文算法进行仿真，轨迹规划由高阶非均匀B样条通过给定型值点进行构建，权重因子皆为1。将图1和图2运行轨迹在三维方向进行分解，得到x,y,z轴分别对应的位置(图5和图6)、速度(图7和图8)、x轴加速度(图9)、y轴加速度(图10)、z轴加速度(图13)。

由图5～图8仿真波形可知，五次非均匀B样条相对于三次非均匀B样条位置和速度的过渡处都比较平滑，位置轨迹基本一致。

但是由图9～图11仿真对比波形可知，五次非均匀B样条在加速度的过渡处相对于三次非均匀B样条更加平滑，能够有效减少运动过程中产生的冲击。

图9 x轴加速度对比 图10 y轴加速度对比

图11 z轴加速度对比 图12 x轴加加速度对比

图13 y轴加加速度对比 图14 z轴加加速度对比

由图12～图14加加速度对比波形可知，五次非均匀B样条在加加速度方面的过渡平滑提升更加明显，能够有效地减少运行过程中的冲击和振动，增加运动过程中的平稳性。

3 结束语

为了解决传统轨迹规划转折处不平顺，易产生变速冲击和轨迹不能进行局部修改的问题，本文进行样条算法的构造，进行数据点预处理，生成样条曲线，计算得到曲线控制点，采用MATLAB对机械臂进行建模，用相同的型值点对机械臂进行三维空间的轨迹插补。根据仿真结果，在满足速度、加速度和加加速度的要求下，均能够获得比较平滑的运行轨迹和速度曲线，但是五次非均匀B样条实际轨迹更加圆滑，而且机械臂的加速度和加加速度突变处过渡更加光滑，能够有效的减少机械臂在运行过程中的冲击，使运行更加平稳。