基于非均匀B样条曲线的机器人轨迹过渡算法*

杨轶焬，刘建群，高伟强

(广东工业大学机电工程学院，广州 510006)

0 引言

工业机器人的笛卡尔空间运动一般是由多段直线或者圆弧轨迹构成的运动[1]。轨迹衔接处理的优劣影响加工质量和工作效率。如果机器人在拐角处不做处理，直接减速停止，将会增加工作时间，导致大幅度的电机加减速；如果保持速度直接通过拐角，可能产生机器振动，影响工件质量，降低机器人寿命[2]。

轨迹段间的衔接处理主要体现在轨迹规划和速度规划。轨迹规划，其规划的优劣可以通过曲线连续度判断。一般而言，G3级的曲线连续度可以满足机器加速度平稳的要求。速度规划，不仅要考虑衔接处的速度，还需要考虑轨迹长度的情况。目前，针对轨迹衔接过渡的常规做法是曲线拟合，在衔接处插入一段微型曲线，曲线不经过拐点，通过牺牲位置精度来换取过渡平滑。王涛等[3]提出了通过圆弧转接实现拐角过渡的方法，但是忽略了衔接点向心加速度突变；BI等[4]提出了3次双Bezier曲线构建实时拟合曲线的方法，但是轨迹只能达到G2连续；BEUDAERT等[5]提出了3次B样条曲线进行直线段间过渡的方法，但是没有给出对应的插补规划方法。

针对机器人拐角过渡的问题，对轨迹段的衔接曲线模型及该模型的轨迹规划、速度规划进行研究，实现了整体轨迹的G3连续和加速度连续，提升了多段连续轨迹拐角的衔接速度，减小了速度波动，避免加速度突变，实现机器人的平稳运动。

1 轨迹平滑过渡模型

1.1 过渡模型建立

假设当前的运行轨迹是由两段相邻的直线段构成，直线段间的轨迹过渡模型如图1所示，B、A、C是两段直线的端点，相交于A点，其中两段直线之间的夹角为α，一段微型样条曲线构造于d0d6之间。

图1 直线段间的轨迹过渡模型

为了实现衔接曲线具有G3连续性，样条曲线的次数为5次，控制点di个数为7，分别对应d0,d1,…,d6；为了保证曲线起始点和终点能与首尾控制点重合，首尾向量节点应设置6次的重复度，即u0=…=u5=0,u7=…=u12=1；为了保证曲线的对称性，u6=0.5，所以该模型的节点向量设置为U=[0 0 0 0 0 0 0.5 1 1 1 1 1 1]T。

该模型的非均匀B样条曲线构造公式[6]为：

(1)

式中，k为样条曲线次数；u为样条参数；di为样条曲线控制点；Ni,k(u)为k次B样条基函数。

为了实现样条曲线在衔接点处与直线相切，控制点d0、d1、d2应该位于直线AB上，控制点d3位于拐点A。同理，控制点d4、d5、d6应该位于直线AC上。为了使曲线在控制点d0和d6处与直线相接具有G3的连续性，在u=0和u=1，曲线p(u)相对于弧长s的二阶和三阶导数应该为零，即：

(2)

由导数的链式法则可以将上式展开得：

(3)

由文献[7]可知，上式成立的充分条件为：

(4)

5次B样条曲线在u=0的二次三次导数为：

(5)

化简可得：

(6)

同理u=1时可得：

(7)

由于样条曲线的节点向量中心对称，所以得到的曲线具有中心对称的性质。当曲线参数u=0.5时，此时样条曲线的最高点F与拐点A距离最远，曲线与原轨迹的偏移距离最大，所以可以通过控制拐点处F的偏移距离来控制过渡曲线的轨迹精度。该模型轨迹最大偏移距离δ推导如下：

(8)

(9)

将式(9)代入式(8)进行化简，最大偏移距离为：

(10)

(11)

确定过渡模型的单位过渡长度后，将该值代入式(9)，可以获得控制点间的相对位移关系。最后，利用直线方程可以求得曲线控制点的空间坐标，将曲线控制点代入式(1)，即可建立过渡曲线的方程。

1.2 过渡曲线长度估算

使用样条曲线过渡后，优化后的轨迹与原直线轨迹相比，路径长度缩短，需要重新计算样条曲线的实际长度。由于样条曲线弧长公式难以直接求得解析解，所以使用自适应辛普森积分方法，通过抛物线近似曲线的方式求取弧长近似解。先求取样条曲线公式的一阶导数。该模型的衔接曲线一阶导数公式为：

(12)

由于曲线光滑，并且长度较短，适合使用数值积分法估算曲线长度。使用自适应辛普森法[8]进行数值积分，计算u=[0,1]区间的弧长，有：

(13)

2 多轨迹段衔接运动规划算法

常见的过渡轨迹规划是对整个过渡轨迹进行速度和位置规划，但是由于本模型过渡曲线的速度约束主要源于高曲率点F，若直接限制衔接曲线的最大速度将严重削减过渡效率。利用该模型的对称性，将样条曲线对半拆分，把拆分曲线与原轨迹段拼接成新轨迹段。假设当前运行至第i段直线轨迹A→B，使用曲线过渡模型进行优化后，当前段的运行轨迹变为F1→F2，由两段拆分样条曲线和直线拼接(左样条F1P1-直线P1P2-右样条P2F2)，拼接轨迹形状如图2所示,实线为当前段轨迹，虚线为相邻段轨迹。下文将从速度规划和位置规划两个层面，阐述该模型的运动规划算法。

图2 拼接轨迹形状示意

2.1 轨迹段间的自适应速度前瞻

为了让机器人在轨迹段间过渡时，末端TCP能够平稳运行，需要在F1和F2处设置一个合理的轨迹衔接速度。针对曲线过渡模型给出了自适应速度规划方法。步骤如下：

步骤1：计算所有轨迹与下一段轨迹衔接的最大允许拐角速度Vcor。拐角速度是由三个约束速度的最小值决定的，分别有用户设定速度Vuser、弦高误差限制允许速度Verr、向心加速度限制允许速度Vcir。

(14)

由于样条曲线的曲率越高，曲率半径越小，电机需要越大的加速度改变运动方向，所以需保证衔接点处的加速度不超过最大加速度设定值Amax。最大加速度允许的速度Vcir为：

(15)

综上所述，每一轨迹段的最大允许拐角速度Vcor：

Vcor=min{Vuser,Verr,Vcir}

(16)

步骤2：计算第i段的理论终点速度Ve(i)。若当前段为首段(i=1)，则该段的起始速度设置为系统最小速度Vmin；若当前段为尾段，则该段的终点速度设置为系统最小速度Vmin，同时结束速度规划；若当前段为中间段，以上一段Ve(i-1)的终点速度作为当前段的起始速度Vs(i)。利用起始速度Vs(i)和轨迹长度Si计算当前段能实现的最大速度Vm(i)。第i段的理论终点速度为Ve(i)=min{Vcor(i),Vm(i)}。由于前一段的终点速度就是后一段的起始速度，所以第i+1段的起点速度Vs(i+1)=Ve(i)。

步骤3：计算以第i+1段的起始速度Vs(i+1)减速到系统最小速度Vmin的极限减速距离Sde。

步骤4：计算前瞻段数k。累加后续段的长度L=L+S(i+k)，累加1次k递增1，直到累加长度L大于极限减速距离Sde或者所有轨迹段遍历完成，记录当前k值。

步骤5：重算第i段的理论终点速度Ve(i)。将第i+k段的终点速度Ve(i+k)设置为系统最小速度Vmin。从第i+k段终点速度Ve(i+k)反向前推得到该段能实现的起始速度Vs(i+k)，那么第i+k-1段的终点速度为Ve(i+k-1)=min{Vs(i+k),Vcor(i+k-1)}，依次递推计算前一段的终点速度，直到计算到第i段，可得其实际终点速度Ve(i)=min{Vs(i+2),Ve(i)}，退出前推循环。

步骤6：进行样条曲线、圆弧轨迹的切向加速度调整。由于曲线运动的加速度是切线加速度和向心加速度的叠加，所以有必要对切向加速度进行限制，使合成加速度满足设定的要求。由于S型速度曲线(如图4所示)当插补时间处于t=T1/2时，此时切向加速度达到最大值，利用之前速度前瞻得到的当前段起始速度、终点速度、最大速度，计算当前段所需运动时间，可以得到当切向加速度到达最大时对应的速度V和曲率半径R，则第i段的切向加速度调整为：

(17)

若切向加速度需要修改，则再次执行步骤(2)～步骤(5)。最终，第i段的实际终点速度计算完成。

步骤7：计算第i+1段的终点速度Ve(i+1)。将第i段的实际终点速度Ve(i)作为第i+1段的实际起始速度Vs(i+1)，再次执行步骤(2)～步骤(5)，直到循环退出。

2.2 基于龙格库塔的轨迹位置规划

样条曲线的位置规划是本模型规划算法的核心。下文先进行样条曲线插补方法的研究。

由于样条曲线上的插补点是通过参数u求取的，而插补点的分布与速度规划相关，所以需要寻找参数u与速度v之间存在的关系。根据速度的定义，将样条曲线p(u)对时间t作一阶导数，展开可得关系式：

(18)

上式将参数u与时间t的映射问题转变为微分方程的求解问题。由于较难直接求得解析解，方程采用数值方法求解；为了提高计算的精度阶数，避免求取样条曲线的高阶导矢，采用四阶龙格库塔方法对参数u进行估算，计算公式为：

(19)

上述中利用插补速度可以求出曲线参数，但是由于实际曲线运动轨迹是由多段细微直线逼近的，在曲率变化频繁的区域，实际直线插补位移比理论的曲线弧长短，导致实际运动速度与预期规划速度不同，容易在衔接区域发生速度波动的情况，所以有必要对曲线参数u进行矫正。这里采用割线法，将上步使用龙格库塔计算的参数当作初值代入割线迭代公式，迭代计算u，直到当前插补周期的实际位移近似等于期望规划位移，结束迭代。该模型的n次迭代割线公式为：

(20)

为了实现机器人末端工具TCP运动具有高阶速度连续性，同时避免速度波动。针对曲线过渡模型给出了一个衔接轨迹的位置规划方法。假设当前第拼接轨迹如图3所示。

图3 拼接轨迹和插补线段图

轨迹位置规划流程具体步骤如下：

步骤1：进行S型速度曲线插补规划。将轨迹段的速度信息V(起始速度Vs，终止速度Ve，最大速度Vm)和轨迹长度S(左右曲线各自长度的一半和中间直线段长度的和)，代入三次S型曲线的加减速控制公式[9]，就可以进行轨迹段插补规划。该算法可以实现机器人运动时加速度平滑和加加速度柔性，三次S型加减速模型曲线如图4所示。

图4 三次S型加减速模型

使用三次S型速度规划后，可以得到对应的绝对位移(速度)-时间序列。然后将绝对位移(速度)序列上的元素通过直线方程或者样条曲线方程，进行插补规划，映射为机器人工具末端TCP坐标点。

步骤2：进行样条部分的插补规划。进行“左样条”的插补，规划方向从左到右，曲线起始参数u=0.5，使用龙格库塔-割线公式依次迭代递增直到当前u>1结束，将最后一个插补点作为uL，理论插补位置对应图3插补线段中的SL。再进行“右样条”的插补，规划方向从右往左，曲线起始参数u=0.5，使用龙格库塔-割线公式依次迭代递减直到当前u<0，将最后一个插补点作为uR,理论插补位置对应插补线段中的SR，完成左右样条部分的插补。

3 实验过程及分析

为了验证本文算法在机器人上的实际运行效果，搭建实验平台如图5所示。使用工业机器人Yaskawa-HP20D，平均位置精度为0.17 mm；使用激光跟踪仪Leica-AT960，配合安装于机器人末端的跟踪探测器T-Mac，对机器人末端的运动轨迹进行数据采集。

图5 实验平台

设定一个货物搬运的场景如图6所示，图中线条为机器人末端TCP的运动轨迹，要求机器人按照设定的工作轨迹进行运动。

图6 货物搬运场景

以搬运点P10的纸箱为例，首先机器人末端TCP位于起始点P1，末端夹具运动到点P2夹取流水线上的纸箱，回到起始点P1后运动到点位P9托板正上方，然后运动到点位P10将纸箱放置于托板上，最后原路返回到起始点P1。整个轨迹由10个空间点构成，原始轨迹长度为11 519.76 mm。表1为机器人笛卡尔空间运动规划参数，其中过渡偏移距离是指被过渡曲线最高点到原轨迹衔接点的距离，即图2中的F2B段。

表1 货物搬运实验运动规划参数

使用SpatialAnalyzer软件进行空间轨迹的测量如图7所示。将标定实验得到的机器人基座坐标系设置为测量软件的工作坐标系，然后设置激光跟踪仪的测量模式为空间扫描，测量频率为1000 Hz。使用本文算法与常用的圆弧过渡算法进行搬运轨迹运动，对比两种算法的机器人末端速度和加速度的测量曲线和运动所需时间。

图7 SpatialAnalyzer空间轨迹测量

图8为使用圆弧过渡算法对应的末端速度、加速度曲线，图9为使用本文算法的末端速度、加速度曲线。其中实线为使用激光跟踪仪的测量数据，虚线为理论规划数据。

图8 圆弧过渡法末端速度、加速度曲线

图9 样条过渡法末端速度、加速度曲线

由两种算法的速度曲线对比可知，本文算法和圆弧过渡算法在拐角处有明显的速度提升。本文算法在拐角区域过渡速度较为平稳，而圆弧过渡存在速度波动。从加速度曲线对比可知，本文算法在图9中的加速度曲线变化相对平稳、接近规划曲线；圆弧过渡在图8中加速度曲线出现明显的震荡，虽然该算法保证了G1的轨迹相切连续，但是瞬间产生的向心加速度导致在过渡区域有较大幅度的加速度跳变，导致机器人末端运动过程伴随颤振。不同算法的规划时间如表2所示，本文算法上相对于不使用过渡方法在运动耗时上减少约18.8%，相较于圆弧过渡法减少约4.5%。

表2 三种算法运动所需时间对比

4 结束语

本文提出了一种基于非均匀B样条曲线的过渡模型和针对该模型的运动规划算法。该算法提升了多段连续轨迹的衔接速度，保证了示教编程机器人末端轨迹G3的连续性，实现机器人运动加速度变化连续，避免了加速度突变。利用安川机器人和激光跟踪仪进行实验，结果表明，该模型对比传统圆弧过渡衔接，运动时间缩短4.5%，机器人运动更加平稳，适用于搬运机器人等对速度要求较高、同时要求运动平稳的场景，具有一定的应用价值。