轴空间多约束下的五轴B样条路径速度规划

钟泽杉，杨 敏，赵现朝+，岳 义，时 云，杨天豪

(1.上海交通大学 机械与动力工程学院，上海 200240;2.上海航天设备制造总厂有限公司，上海 200245)

0 引言

五轴机床刀具位姿与各驱动轴位置具有非线性映射关系[1]，在高速加工直线、圆、空间曲线等路径的过程中，各驱动轴电机的速度、加速度、跃度等动力参数变化剧烈，容易超过伺服电机上限值，造成各驱动轴的运动精度、响应特性下降，影响零件加工的质量和机床的使用寿命。速度规划算法综合考虑弓高误差、刀具末端进给的速度、加速度、跃度，以及各轴电机速度、加速度和跃度限制条件[2-3]，动态调整进给速度，保证机床运动的平顺性，提高加工精度与使用寿命。

速度规划算法广泛采用指令前瞻和S型加减速方法[4-6]，这类方法计算量小、控制简单，适合以刀具运动与驱动轴运动为线性映射的机床。王斌锐等[5]根据误差要求给出最优的两段路径衔接点处的速度约束，推导了非对称S曲线加减速公式，并将其应用在自适应前瞻算法中；朱长峰[6]采用路径前瞻回溯法进行连续微线段路径规划，然后用S型加减速调节进给速度。对于五轴机床，需考虑驱动轴的运动约束，第一类方法是改进S型加减速方法：采用路径关键点检测并考虑驱动的加速度及跃度约束限制，以减小曲线拐点处的加速度及跃度突变[7-9]。王田苗等[10]、李建伟等[11]、王允森等[12]依据弓高误差确定曲线上各关键点的速度限制并实施S型加减速控制；徐荣珍[13]分析了三轴机床加工过程中切削力、惯性力、加工精度对各轴速度的约束限制条件，应用S型加减速控制策略进行速度调整。第二类方法是路径或时间离散后进行扫描优化：根据允许的路径拟合误差和驱动轴约束条件建立优化模型，求解获得满足要求的加工轨迹[14-16]。黄昕等[17]考虑并建立了各轴的动力学约束条件,采用双向扫描法确定两个小段直线衔接点最佳速度，提高加工效率；Consolini[18]等采用“前后向扫描取小”的方法进行小车的速度规划，并在理论上证明了该方法结果为时间最优速度规划；Beudaer等[19]将时间等距划分，每次进给均按照最大跃度前进，然后验证各轴约束条件，并采用“回退”策略解决无解问题。第三类方法是使用数值最优化方法：将速度规划问题看做一个最优化问题，进行数学建模、简化、求解[20-21]。Sencer等[21]将速度曲线表示为路径的函数，根据各轴动力学约束条件建立了非线性最优化模型； Erkorkmaz等[22]和Fan等[23]将轴空间的加速度、跃度约束线性化，采用线性规划求解，以加快求解速度。

随着数控技术的发展和越来越高的加工要求，机床高速加工是必然的发展趋势，速度规划将越来越重要。本文考虑空间基样条(B-样条)曲线加工的速度规划，建立了速度规划的时间最短目标函数，基于五轴机床运动学反解推导了各驱动轴的速度、加速度、跃度约束条件，应用序列二次型规划(Sequential Quadratic Programming, SQP)方法求解非线性最优问题。基于贪心法思想，分为速度、加速度、跃度约束三阶段求解，减小约束维度以加快求解速度。然后利用一种典型的“正交XYZ平移轴+AC转动轴”结构的五轴机床进行算法仿真验证。仿真结果表明，该算法保证了刀具末端和各驱动轴的速度、加速度始终连续且不超过限制值，跃度不超过限制值，同时算法求解速度能够保证加工连续性。加工一条S型路径进行实验测试，得到了末端速度、加速度、跃度以及驱动轴的最大约束载荷在路径上的分布。本文所提算法借鉴了机器人控制领域的思想，在一定程度上二者具有通用性。实际应用中，机床的结构多种多样，但在本文讨论的速度规划方面，它们的本质区别是机床的运动学模型不同，因此针对不同结构的机床，只需要修改算法中的运动学模型(速度规划的最优化模型不变)，就能实现算法在不同结构机床上的应用。

1 速度曲线的表示

1.1 刀具位姿

在这种位姿坐标定义下，一条三次B样条表示的待加工曲线路径C，有m个控制点CP=[P1…Pi…Pm]T，其中Pi=[xi,yi,zi,φi,θi]T，i=1,2,…,m；有m+4个参数节点Cu=[u1,…,um+4]T,u1,…,um+4是递增不减序列；给定样条参数u∈[u1,um+4]，由德布尔-考克斯递推公式可计算路径C上点的坐标：

(1)

根据定义，式(1)表达为：

(2)

其中Ni,3(u)为路径B-样条曲线第i个三次样条基函数的值。

一般地，基函数Ni,ρ(u)表示B-样条第i个ρ阶基函数，ρ≥0；定义0阶基函数在u处的值：

(3)

因此，ρ阶基函数在u处的值可以通过下述递推公式得到：

(4)

当ρ>0时，ρ阶基函数的值是ρ-1阶基函数值的线性组合。

1.2 速度曲线的表示

若路径C总长度为S，曲线上任意位置距离起点的路径长度s∈[0,S]。速度规划的目标是确定曲线上任意位置s处的进给速度v，即速度曲线V。设V的参数方程为：

(5)

(6)

根据定义，式(6)可表示为：

(7)

采用二维B-样条表示速度曲线的优点为：

(1)样条曲线形状可塑性强，形状修改方式灵活，固定控制点横坐标sj，调整纵坐标vj的值就能调整速度曲线轮廓。

(2)速度曲线光滑，没有重合控制点的情况下具有C2连续性。速度曲线两次求导后仍然是连续函数，这保证了插补过程中，加速度、跃度连续，不会出现跃度突变的情况(首末端点需控制跃度不超过最大值)。

(3)参数方程(5)可表示为进给速度v关于曲线处长度s的函数v(s)，根据德布尔—考克斯递推公式能方便计算函数v(s)的任意阶导数。

如图2所示为一条长度S=10 mm路径的速度曲线，速度样条由11个控制点生成；在这种表示方法下，若要调整速度曲线，可固定横坐标s1…s11不变，调整中间9个点的高度v2…v10(因为加工的起点和终点速度为0)。

1.3 路径长度计算

本节给出路径曲线长度的数值计算方法与误差控制。

计算路径时只考虑刀具中心的位置，即(x,y,z)。给定需要计算长度的参数区间[u1,u2]⊆[0,1]，则该参数区间对应的曲线的长度L表示为：

(8)

式中{xu,yu,zu} 表示曲线位置坐标x,y,z关于u的导数，可以利用式(1)对u求导获得。

理论上可以写出L的解析表达式，但形式较复杂，因此本文采用高斯—勒让德数值积分。积分公式的变量区间固定为[-1,1]，函数求积分时需先做变量代换。例如函数f(x)定积分：

(9)

变量代换，令

(10)

得

(11)

则数值积分表示为：

tk∈{p1,…,pg}Ak∈{a1,…,ag}。

(12)

式中：g为节点数量，当g确定后，{p1,…,pg}，{a1,…,ag}随之确定且固定不变，可查数学手册获取，本文所用算法取g=8，即取8个积分节点，积分代数精度较高且积分稳定；tk,Ak分别为节点值和积分权值。式(12)中将函数f(x)替换为L(u)就得到了曲线的数值积分长度。

数值积分存在精度误差，但误差可控。提高精度的方法是等距细分积分区间[u1,u2]，分别计算子区间的积分，然后求和。细分段数可以采用常用的数值计算精度控制方法确定，即首先细分p段，获得长度Lp；进一步细分q段，q>p，获得长度Lq。设允许的长度误差为ε，由于高斯积分方法稳定、收敛，如果|Lq-Lp|

反过来，由弧长s转化为曲线参数u采用牛顿迭代法，按照上述的曲线长度计算方法同样能够保证计算精度在允许范围内。

2 效率最优目标函数

高速加工中，高效率是指刀具进给速度尽量达到指令速度Vtarget来提高加工效率。用加工时间来衡量加工效率，建立时间最短的优化目标函数。

2.1 优化变量

对于加工路径C的速度轮廓P，加工时间Γ表示为：

(13)

该问题的核心在于确定一条合适的速度曲线v(s)，在满足相应的约束条件下使加工时间最短，数学上为泛函极值问题，即：

(14)

(15)

2.2 目标函数计算

(16)

(17)

(1)高斯—勒让德积分公式的代数精度与阶数成正比，阶数越高代数精度越高，但如果阶数过高会导致积分不稳定甚至不收敛，因此采用7阶公式。

(2)目标函数在优化过程中本质上是一种优化变量的评价函数(约束函数本质上也是评价函数)，如果优化变量使目标函数越小，则认为优化变量的取值就越“好”。另外，本文希望计算目标函数时的采样点越少越好，以减少计算量，因此目标函数在具备正确评价优化变量的能力基础上，应当使分段数尽量少；考虑到路径曲线控制点个数体现了曲线可能出现的拐点数量，因此控制点个数乘以系数2作为分段积分的段数能够使目标函数具备良好的评价能力，且不会使采样点过多。

3 驱动轴性能约束

3.1 速度、加速度、跃度约束公式

高速高精度加工的速度规划要考虑驱动轴空间的性能约束，使各驱动轴速度、加速度、跃度小于各轴限定值，在五轴混联加工机床中，这一点尤为重要。其中速度约束考虑曲线加工的弓高误差、用户设置的目标速度；加速度约束考虑刀具进给方向加速度限制和各驱动轴电机的加速度限制；跃度约束考虑刀具进给方向跃度限制和各驱动轴电机跃度限制。

五轴机床5个驱动轴关节空间、关节速度空间、关节加速度空间、关节跃度空间分别定义为：

(18)

v(s)≥0。

(19)

d=(1,…,L)。

(20)

(21)

(22)

(23)

d=(1,…,L)。

(24)

(25)

(26)

(27)

式中：

(28)

(29)

(30)

一般而言，用户也会设置刀具进给的加速度限制Tacc和加加速度限制Tjerk，则

(31)

式(17)、式(19)、式(20)、式(24)、式(27)和式(31)组成多约束下非线性最优化模型，令：

有：

vj≥0;

t=1,2,3,4,5;

d=1,…,L。

(32)

4 多阶段优化的求解方法

分阶段优化基于“贪心法”思想，确保最终优化结果接近最优解。首先，根据刀具进给的指令速度、加速度限制、加加速度限制，按照s型曲线公式确定初始速度曲线V0。显然，若V0满足所有约束，V0即为最优解。在V0基础上，分3个阶段调整，将约束条件分为速度约束CV，加速度约束CA，跃度约束CJ，第一阶段求解CV约束下的最优解V1，不考虑CA、CJ，V1如果满足CA、CJ，则V1已经是最优解。第二阶段在V1基础上求解CA约束下的最优解V2，这时为了保证CV约束，约束条件增加上界约束，即V2≤V1(V2向量元素分别小于V1)。同理，第三阶段在V2基础上求解CJ约束下的最优解V3，为了保证CV、CA不超出约束，更新上界约束V3≤V2；仅更新上界约束还不足以保证加速度约束不超过限制，可以在CJ约束下优化完成后，返回第二阶段检测约束情况，考虑再次进行CA约束下的优化。算法流程如图3所示。

5 速度规划仿真与实验

现采用一种典型结构的五轴机床进行算法仿真试验，其基本结构和轴空间坐标定义如图4所示。

若已知刀具位姿[x,y,z,φ,θ]T，求其运动学反解表示为：

X=xsinθ-ycosθ,

Y=xcosφcosθ+ycosφsinθ-zsinφ,

Z=xsinφcosθ+ysinφsinθ+zcosφ,

A=φ,

(33)

根据运动学反解，结合给定的路径C、优化变量取值点sj(j=1…n)、目标函数采样点位置srl(r=1…N,l=0…7)、约束条件采样点位置sd(d=1…L)，可以确定式(32)表示的非线性最优化模型。

给定加工路径如图5所示，图5a中实线是待加工路径，加工过程经过点的先后顺序为ABCDEFGH。待加工曲线跨越3个区域：区域1为倾斜45度的平面，区域2为圆柱面，区域3为倾斜45度的平面。实际上，路径是零件底部平面上的直线、圆弧草图投影到这些面上形成的复杂图形。图5b包含了刀具姿态信息，倾斜平面上路径的刀具姿态向量(刀具中心指向刀柄中心的向量)平行于面的法向量；圆柱面上的路径，在点B，C，D，E，F，G附近对刀具姿态角θ做了平滑处理，以防止在经过这些点时反解的角度发生突变，其余部分刀具姿态向量平行于该点处的曲面法向量。

速度规划的驱动轴约束条件和沿路径切向进给的约束条件如表1、表2所示。其中X,Y,Z,A,C代表5个驱动轴，Tvel,Tacc,Tjerk分别为路径切向进给的目标速度、最大加速度和最大加加速度。

表1 驱动轴约束条件

表2 切向进给约束条件

分阶段的速度规划过程如图6所示。

图6a是根据刀具末端的目标速度、加速度、跃度条件，结合S型加减速算法给出的初始速度曲线。图6b对速度曲线进行速度约束检测，发现有超出速度约束的点并标记为黑色三角形，然后进行速度约束下优化后获得图6c，图6d是再次检测速度约束条件的结果，已经没有超出速度约束的点，然后进行加速度约束检测，得到图6e，其中加速度约束超出点标记为黑色矩形，在此基础上进行加速度优化得到图6f，再次检测加速度约束条件得到结果图6g，已经没有超出加速约束的点。图6h检测跃度约束，超出约束标记为黑色“*”形，进行跃度约束下的速度规划得到图6i，再次检测跃度约束结果为图6j，已经没有跃度超出的点。最后，检测跃度约束下规划结果的加速度约束满足情况，结果为图6k，发现满足该结果也满足加速度约束条件，可以输出速度规划最后结果。

将速度规划结果进行插补，得到刀具中心关于时间的运动曲线以及各轴关于时间的运动曲线，如图7所示。

由图7可以看出：

(1)进给速度在该路径上可以达到目标速度，但不能维持。一方面因为曲线拐角处由于弓高误差要求，需减小进给速度来保证精度；另一方面因为五轴加工存在刀具姿态调整过程，该过程中切向进给速度与各轴速度为非线性映射关系，很容易超出电机的最大速度限制，此现象最明显的是第4～12 s和第17～24 s区间，进给速度很小，但轴的速度已经接近于限制值。

(2)切向加速和加加速度在整个过程中没有达到最大限制值，留有余量。因此以表1和表2的参数为约束条件加工上述路径，起作用的限制条件是各驱动轴的运动性能限制。

(3)驱动轴的加速度和加加速度在少数点处仍会超出限制。最优化建模与求解都是直接针对约束采样点进行优化，因此采样点处不会超过限制条件；约束点之间的部分由速度曲线连续性保证，有可能超出约束限制，但超出约束的点很少，且超出幅度不大。

(4)形状相同的路径具有不同的进给速度特征。上述路径中ABC段与FGH段路径形状基本相同，但优化结果中两段路径对应的速度曲线形状完全不同。五轴机床在不同位置、姿态下具有不同的运动学特征，上述两段路径所处位置不同，因此速度优化结果不同；同样地，零件的装夹位置也会影响速度规划结果。

算法规划各函数执行时间通过MATLAB探查器给出，如表3所示。

表3 MATLAB中优化建模及求解函数调用时间

函数aCalNidNiSi计算了目标函数采样点、优化变量点、约束点的速度曲线样条参数；GetFixParas计算了1 420个约束点处约束方程中的固定系数1γtd，2γtd，3γtd；StepSQP是分阶段优化函数。建模、求解过程耗时70.578 s，PlotV,PlotCons是中间曲线作图函数，被StepSQP调用，扣除后实际建模优化耗时64.316 s，小于曲线加工总时间86.872 s，满足加工过程连续的要求。如果考虑MATLAB代码执行效率，用C++实现上述算法优化耗时将更低。

在开放式数控系统中编写速度规划算法，进行速度规划加工实验。数控系统硬件系统基于EtherCat总线搭建实时通信网络，主站采用倍福工控机，从站为科尔摩根伺服驱动器；软件系统基于TwinCat3.0运动控制平台，将速度规划算法编写为C++程序模块，系统通过函数接口实现对算法模块的调用。如图8a所示为机床的坐标系定义，加工路径如图8b所示，加工区域是零件倾斜侧壁的根部，该路径五轴联动才能加工，零件如图8c所示。

系统控制周期为1 ms，即每隔1 ms控制器计算每个轴的位置，然后通过总线发送给驱动器执行位置指令，同时系统也能采集5个轴的反馈位置，根据正向运动学可以获得刀具姿态。

通过差分方法获得各轴的速度、加速度、跃度以及刀具中心进给速度、加速度、跃度，如图9所示，其中图9a～图9c显示了加工路径上切向进给的速度、加速度、跃度分布；图9d图显示了加工过程，各驱动轴的“最大载荷”分布。路径上一个点的“最大载荷”是指该点处，各轴的实际速度与速度限制值的百分比、实际加速度与加速速度限制值的百分比、实际跃度与跃度限制值的百分比中，取最大的比值。“最大载荷”能够反映轴的约束条件满足情况，即理想情况是所有点的“最大载荷”不超过1。由图9可以看出，切向进给的速度在曲线平直区域能达到目标速度，在拐角处会减速以满足设置的约束条件；切向加速度、跃度均没有超过限制值；轴的约束中，少部分点超出了其限制值，但超出幅度小于20%，加工过程平滑，无冲击。

5 结束语

速度规划是数控机床高速高精度加工的重要保证，由于刀具位姿与轴空间的非线性映射，在高速运动中刀具末端的平滑特性并不能保证各驱动轴的运动也具有平滑特性，速度规划需考虑轴空间的性能约束。本文介绍了一种基于非线性最优化的速度规划方法，该方法以加工时间最短为目标建立了最优化模型的目标函数，考虑了各驱动轴的速度、加速度、跃度限制条件，并推导了最优化模型的非线性约束条件。研究了SQP方法并应用多阶段优化策略求解非线性最优化问题。为了尽量提高计算效率，采取高斯—勒让德积分公式减少目标函数的采样点个数；采用贪心法原理，将优化分为3个阶段进行减少求解时约束的维度，这样有利于算法收敛，提高求解效率。仿真试验采用典型的XYZ-AC结构的五轴机床，在柱面上加工刀具姿态变化的空间曲线，仿真结果表明算法保证了加工过程中刀具进给和各驱动轴的运动满足约束条件，同时计算速度满足加工连续性要求；在开发式数控平台上进行了算法的实验验证，经算法优化后，控制器发送给驱动器的指令位置满足各项约束条件，各轴运动基本满足限制条件，加工过程平滑、无冲击。下一步，将拓展算法的应用领域，用于五轴联动的工业机械臂速度规划。