CN111791236A - 一种工业机器人笛卡尔空间轨迹过渡方法 - Google Patents

Abstract

本发明公开了一种工业机器人笛卡尔空间轨迹过渡方法，包括贝塞尔样条的不同运动类型位姿过渡方法、运动段不同工具坐标系位姿过渡方法和抛物线样条的连续小线段位姿过渡方法，本发明作为位姿过渡的基础，阐述了基于贝塞尔样条的构造，包含了位置空间不同运动类型的过渡以及姿态空间的四元数过渡；然后针对相邻运动段工具坐标系不同无法构造过渡曲线的问题，对连续运动段不同工具坐标系平滑过渡算法进行了研究；针对连续小线段过渡计算量复杂的问题，提出基于抛物线样条的连续小运动段轨迹过渡方法包含构造方法以及优化方法；最后基于仿真平台，验证经过过渡处理后末端轨迹可以满足G1连续性，提高了运动的平滑性，具有推广应用的价值。

Description

一种工业机器人笛卡尔空间轨迹过渡方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种工业机器人笛卡尔空间轨迹过渡方法。

背景技术

随着工业机器人应用领域的不断扩大，人们对机器人的控制技术提出了不少新的要求。轨迹规划是工业机器人控制技术研究的难点和重点，对机器人运行性能有很大影响。其中，工业机器人轨迹规划的精度和平滑性尤为重要。机器人轨迹过渡可以改善轨迹运动的平滑性并提高运行效率和精度，因此对于机器人轨迹规划的平滑过渡控制非常关键。

通过机器人轨迹过渡技术，能够提高机器人运动轨迹的平滑性和位姿的连续性，避免末端在运行过程中的速度和加速度的突变，防止出现剧烈的振动和冲击。当前对机器人轨迹过渡技术的研究较少，主要研究笛卡尔空间直线段与直线段之间的过渡，缺少对笛卡尔空间其轨迹类型、关节空间过渡类型以及过渡后速度规划的研究，

发明内容

本发明的目的就在于为了解决上述问题而提供一种工业机器人笛卡尔空间轨迹过渡方法。

本发明通过以下技术方案来实现上述目的：

本发明包括贝塞尔样条的不同运动类型位姿过渡方法、运动段不同工具坐标系位姿过渡方法和抛物线样条的连续小线段位姿过渡方法，所述贝塞尔样条的不同运动类型位姿过渡方法包括笛卡尔空间位置过渡方法和笛卡尔空间姿态过渡方法；

所述笛卡尔空间位置过渡方法：设三次贝塞尔样条的表达式为：P(t)＝(1-t)³P₀+(1-t)²tP₁+(1-t)t²P₂+t³P₃，三次贝塞尔样条起点P₀和终点P₃的切矢方向与P₀P₁和P₂P₃方向相同，并且在P₀和P₃点满足G2曲率连续；笛卡尔空间中由于运动段类型的不同，连续两运动段有直线-直线、直线-圆弧以及圆弧-直线四种拼接方式；两段轨迹P_aP_b、P_bP_c相交于P_b点，构造样条过渡曲线的主要工作是选择四个控制点和计算过渡长度；过渡长度S为两相邻运动段轨迹长度中较小值的一半与过渡比例的乘积；选择控制点P₁和P₂与交点P_b重合，在P_aP_b上选择P₀点，在P_bP_c上选择P₃点，保证了运动段与过渡段在衔接点处G1连续，并且满足|P₀P₁|＝|P₂P₃|＝S；首先通过过渡比例首先决定P₀和P₃，然后在P0Pb上选择满足|P₀P₁|＝3S关系的P1点，最后在P3切矢方向选择满足

关系的P₂点；

所述笛卡尔空间姿态过渡方法：使用四元数表示姿态时，如果连续两运动段之间姿态的转角为0，则不进行姿态过渡，否则采用三次贝塞尔样条曲线的四元数形式，计算公式如下：

其中q_i表示控制点姿态，t(t≥0&&t<1)表示的比例因子；

姿态过渡控制点和角度的选择方法与直线-直线过渡方法类似，过渡转角S为两相邻运动段轨迹转角中较小值的一半与过渡比例的乘积；控制点q₁、q₂与两运动轨迹连接点q_b重合，在q_aq_b和q_bq_c上通过Slerp方法选择控制点q₀和q₃，并满足q₀到q_b的转角等于过渡转角S；则三次贝塞尔样条曲线可以化简表示为：

所述运动段不同工具坐标系位姿过渡方法包括不同工具坐标系过渡段构造与插补方法：两段连续运动段的工具坐标系发生变化时，末端轨迹不能满足平滑性，P0为起点，依次运动到P1、P2和P3，P0P1运动段和P2P3运动段的工具坐标系不同，末端工具坐标系中心TCP点的位置由P1点突变为P2点，使得轨迹不能满足G0连续性，在通常的规划过程中，需要在P0P1运动段终点处停止，然后更换工具坐标系为P2P3段的工具坐标系，会造成运行时速度的不连续性，所以需要通过合适的过渡方法对轨迹和速度进行优化；

通过在两运动段之间添加一段基于贝塞尔样条实现的样条进行优化，重点在于贝塞尔样条控制点的选取、工具坐标系变化的处理以及运动的连续性速度处理；控制点起始点按直线-直线过渡方法按过渡比例选择P0和P3，为满足P0点和P3点的G1连续性，其余两个控制点选择交点P1和P2；

姿态插补的实现使用球面线性插值方法；位置插补的实现参照直线插补，设起点的位置为Pb，终点的位置为Pe，则当前的位置为：

P＝Pb+t(Pe-Pb) (3)

参数t＝max(L,Q)，其中L表示过渡段已完成位置位移除以总位置位移，Q表示过渡段已完成姿态位移除以总姿态位移，由于在过渡过程中过渡段的位移和姿态角度可能出现其中之一为0的情况，因此需要通过选择最大值保证插补的进行；

所述抛物线样条的连续小线段位姿过渡方法包括连续小线段过渡样条构造方法和抛物线样条非均匀优化方法；

所述连续小线段过渡样条构造方法：抛物线样条的一般形式：F(t)＝at2+bt+c(0≤t≤1)，a、b和c为控制系数；过P1，P2和P3三点可以定义一条参数形式的抛物线方程，如图3-7所示该抛物线以P1为起点，以P3为终点，中间通过P2点；

抛物线方程需要构造控制系数a、b和c。当t＝0时，曲线过P1点；当t＝0.5时，曲线过P2点；当t＝1时，曲线过P3点，计算公式如下：

联立方程可以得到控制点：

通过以上方法可以通过三点确定一条抛物线，对于型值点多于3个的情况，需要通过合成抛物线的方法进行曲线拟合。

设有型值点Pi(i＝1,2,3...,n)，每经过相邻三点可以做一段抛物线，由于有n个型值点，所以可以做n-2条抛物线。第i条抛物线Fi经过Pi，Pi+1，Pi+2三点，第i+1条抛物线Fi+1经过Pi+1，Pi+2，Pi+3三点；

每两段曲线之间包含有重叠部分。通过加权合成方法将重叠部分合并，最后整个运动轨迹可以合成一条光滑曲线G(t)，合成公式如下：

G(t)＝(1-s)×F_i(t_i)+s×F_i+1(t_i+1) (6)

s表示权重，G表示合成后的曲线，合成部分为Fi的后半段以及Fi+1的前半段，所以ti取值范围为0.5-1，抛物线Fi+1的参数ti+1与ti的关系为t_i+1＝t_i-0.5，权重s＝2ti-1。同理，合成除首末两段外的所有重叠区域，可以得到一条光滑拟合曲线；

所述抛物线样条非均匀优化方法：构造样条时，p1、p2和p3之间的距离可能差值比较大。取t＝0.5时，曲线过p2点，当在[0,1]范围内均匀取t值，得到的拟合点G(t)会分布不均匀。构造样条时，首先计算p1、p2和p3之间的距离|p1p2|和|p2p3|，当t＝|p1p2|/(|p1p2|+|p2p3|)时，抛物线过p2点。

对于样条合成需要进行非均匀化处理，对于第i条抛物线Fi，过Pi+1点时t的值设为ki，对于第i+1条抛物线Fi+1，过Pi+2点时t的值设为ki+1，合成部分抛物线Fi的参数ti取值范围为(ki,1]，抛物线Fi+1的参数ti+1与ti的关系为ti+1＝ki+1(ti-ki)/(1-ki)，权重s＝(1-ti)/(1-ki)。通过优化参数，优化后曲线最小曲率半径增大，对速度的限制明显减小，因此曲线的平滑性得到改善，且在合成抛物线样条在插补过程中，只需选择前后四个型值点的信息，不受型值点数目的影响，合成计算速度比较快，拟合曲线更加平滑，适合曲线拟合插补。

本发明的有益效果在于：

本发明是一种工业机器人笛卡尔空间轨迹过渡方法，与现有技术相比，本发明作为位姿过渡的基础，阐述了基于贝塞尔样条的构造，包含了位置空间不同运动类型的过渡以及姿态空间的四元数过渡；然后针对相邻运动段工具坐标系不同无法构造过渡曲线的问题，对连续运动段不同工具坐标系平滑过渡算法进行了研究；针对连续小线段过渡计算量复杂的问题，提出基于抛物线样条的连续小运动段轨迹过渡方法包含构造方法以及优化方法；最后基于仿真平台，验证经过过渡处理后末端轨迹可以满足G1连续性，提高了运动的平滑性，具有推广应用的价值。

附图说明

图1是三次贝塞尔样条构造示意图；

图2是不同类型位置过渡示意图；

图3是姿态贝塞尔样条过渡示意图；

图4是工具坐标系变化示意图；

图5是控制点构造示意图；

图6是过渡前后关节速度前后对比图；

图7是抛物线构造示意图；

图8是样条合成示意图；

图9是非均匀优化前后对比图；

图10是均匀取点不同算法结果对比图；

图11是非均匀取点不同算法结果对比图；

图12是短运动段过渡仿真示意图；

图13是短运动段过渡仿真示意图(型值点重复)。

具体实施方式

下面结合附图对本发明作进一步说明：

本发明首先设计了基于样条规划的笛卡尔空间位姿过渡方法，以解决G1不连续的问题；然后分析两种不适应于直接添加样条的特殊情况：因工具坐标系不同导致连续运动段不满足G0连续性，以及因连续小线段导致添加过渡段计算复杂的问题。为保证工业机器人末端过渡轨迹的G1连续性，常使用圆弧、贝塞尔样条、B样条和Ferguson样条等作为过渡段曲线。因为贝塞尔样条容易控制切矢方向，并且计算速度快。

基于贝塞尔样条的不同运动类型位姿过渡方法：

在笛卡尔空间中，由于连续运动段的坐标通过位置和姿态描述，并且运动段的运动类型可以分为直线和圆弧运动，所以在进行过渡规划时需要分情况进行设计。

笛卡尔空间位置过渡方法：

三次贝塞尔样条的表达式为：P(t)＝(1-t)³P₀+(1-t)²tP₁+(1-t)t²P₂+t³P₃，如图1所示，三次贝塞尔样条起点P₀和终点P₃的切矢方向与P₀P₁和P₂P₃方向相同，并且在P₀和P₃点满足G2曲率连续。

笛卡尔空间中由于运动段类型的不同，连续两运动段有直线-直线、直线-圆弧以及圆弧-直线四种拼接方式。两段轨迹P_aP_b、P_bP_c相交于P_b点，构造样条过渡曲线的主要工作是选择四个控制点和计算过渡长度。过渡长度S为两相邻运动段轨迹长度中较小值的一半与过渡比例的乘积。

直线-直线过渡如图2(a)，选择控制点P₁和P₂与交点P_b重合，在P_aP_P上选择P₀点，在P_bP_c上选择P₃点，保证了运动段与过渡段在衔接点处G1连续，并且满足|P₀P₁|＝|P₂P₃|＝S。

直线-圆弧过渡如图2(b)，首先通过过渡比例首先决定P₀和P₃，然后在P0Pb上选择满足|P₀P₁|＝3S关系的P1点，最后在P3切矢方向选择满足

关系的P₂点。

笛卡尔空间姿态过渡方法：

使用四元数表示姿态时，如果连续两运动段之间姿态的转角为0，则不进行姿态过渡，否则采用三次贝塞尔样条曲线的四元数形式，计算公式如下：

其中q_i表示控制点姿态，t(t≥0&&t<1)表示的比例因子。

运动段q_aq_b和运动段q_bq_c姿态过渡示意图如图3所示。

姿态过渡控制点和角度的选择方法与直线-直线过渡方法类似，过渡转角S为两相邻运动段轨迹转角中较小值的一半与过渡比例的乘积。控制点q₁、q₂与两运动轨迹连接点q_b重合，在q_aq_b和q_bq_c上通过Slerp方法选择控制点q₀和q₃，并满足q₀到q_b的转角等于过渡转角S。

则三次贝塞尔样条曲线可以化简表示为：

运动段不同工具坐标系位姿过渡方法：

在切换打磨头以及多手爪抓取工件等场景，需要切换机器人末端工具坐标系，切换过程为空走行程，需要通过连续轨迹来提高运行效率。

不同工具坐标系过渡段构造与插补方法：

两段连续运动段的工具坐标系发生变化时，末端轨迹不能满足平滑性。如图4所示，P0为起点，依次运动到P1、P2和P3，P0P1运动段和P2P3运动段的工具坐标系不同，末端TCP(工具坐标系中心)点的位置由P1点突变为P2点，使得轨迹不能满足G0连续性(位置不连续)，在通常的规划过程中，需要在P0P1运动段终点处停止，然后更换工具坐标系为P2P3段的工具坐标系，会造成运行时速度的不连续性，所以需要通过合适的过渡方法对轨迹和速度进行优化。

通过在两运动段之间添加一段基于贝塞尔样条实现的样条进行优化，重点在于贝塞尔样条控制点的选取、工具坐标系变化的处理以及运动的连续性速度处理。

如图5所示，控制点起始点按直线-直线过渡方法按过渡比例选择P0和P3，为满足P0点和P3点的G1连续性，其余两个控制点选择交点P1和P2。

姿态插补的实现使用Slerp插值(球面线性插值)方法。位置插补的实现参照直线插补，设起点的位置为Pb，终点的位置为Pe，则当前的位置为：

P＝Pb+t(Pe-Pb) (3)

参数t＝max(L,Q)，其中L表示过渡段已完成位置位移除以总位置位移，Q表示过渡段已完成姿态位移除以总姿态位移，由于在过渡过程中过渡段的位移和姿态角度可能出现其中之一为0的情况，因此需要通过选择最大值保证插补的进行。

不同工具坐标系过渡仿真与分析：

在不同工具坐标系类型的运动段，对过渡段工具坐标系参数进行插补，到达过渡段结束点时，过渡段工具坐标系已插补为运动段2的工具坐标系。在通过仿真验证速度的连续性，采集各关节数据，绘制各轴关节速度时间如图6所示。

通过图6可知，在不同工具坐标系的运动段之间加上过渡段后，在衔接点(虚线框内)速度不需要降为0，保证了速度的连续性，过渡前运行时间为27.8s，过渡后减到23.4s，时间减少了15.6％，提高了机器人的工作效率。

基于抛物线样条的连续小线段位姿过渡：

连续小线段过渡不适合采用贝塞尔曲线过渡方法，因为会产生过多的计算，影响计算性能和占用资源。本节通过合成抛物线的样条构造方法对连续小线段过渡插补进行优化，主要是在型值点较多时，避免大量的计算，控制计算资源的使用；在运动中断时，保持前后运动的一致性。连续小线段轨迹过渡方法首先使用抛物线样条构造合成方法，然后使用非均匀方法进行优化，使得过渡轨迹更加平滑。

连续小线段过渡样条构造方法：

抛物线样条的一般形式：F(t)＝at2+bt+c(0≤t≤1)，a、b和c为控制系数。过P1，P2和P3三点可以定义一条参数形式的抛物线方程，如图7所示该抛物线以P1为起点，以P3为终点，中间通过P2点。

抛物线方程需要构造控制系数a、b和c。当t＝0时，曲线过P1点；当t＝0.5时，曲线过P2点；当t＝1时，曲线过P3点，计算公式如下：

联立方程可以得到控制点：

通过以上方法可以通过三点确定一条抛物线，对于型值点多于3个的情况，需要通过合成抛物线的方法进行曲线拟合。

设有型值点Pi(i＝1,2,3...,n)，每经过相邻三点可以做一段抛物线，由于有n个型值点，所以可以做n-2条抛物线。第i条抛物线Fi经过Pi，Pi+1，Pi+2三点，第i+1条抛物线Fi+1经过Pi+1，Pi+2，Pi+3三点，如示意图8(a)。

每两段曲线之间包含有重叠部分。通过加权合成方法将重叠部分合并，最后整个运动轨迹可以合成一条光滑曲线G(t)，如图8(b)所示，合成公式如下：

G(t)＝(1-s)×F_i(t_i)+s×F_i+1(t_i+1) (6)

s表示权重，G表示合成后的曲线，合成部分为Fi的后半段以及Fi+1的前半段，所以ti取值范围为(0.5,1]，抛物线Fi+1的参数ti+1与ti的关系为t_i+1＝t_i-0.5，权重s＝2ti-1。同理，合成除首末两段外的所有重叠区域，可以得到一条光滑拟合曲线。

抛物线样条非均匀优化方法

构造样条时，p1、p2和p3之间的距离可能差值比较大。取t＝0.5时，曲线过p2点，当在[0,1]范围内均匀取t值，得到的拟合点G(t)会分布不均匀。构造样条时，首先计算p1、p2和p3之间的距离|p1p2|和|p2p3|，当t＝|p1p2|/(|p1p2|+|p2p3|)时，抛物线过p2点。

对于样条合成需要进行非均匀化处理，如图8所示，对于第i条抛物线Fi，过Pi+1点时t的值设为ki，对于第i+1条抛物线Fi+1，过Pi+2点时t的值设为ki+1，合成部分抛物线Fi的参数ti取值范围为(ki,1]，抛物线Fi+1的参数ti+1与ti的关系为ti+1＝ki+1(ti-ki)/(1-ki)，权重s＝(1-ti)/(1-ki)。

通过优化参数，如图9所示，优化后曲线最小曲率半径增大，对速度的限制明显减小，因此曲线的平滑性得到改善，且在合成抛物线样条在插补过程中，只需选择前后四个型值点的信息，不受型值点数目的影响，合成计算速度比较快，拟合曲线更加平滑，适合曲线拟合插补。

连续小线段过渡仿真与分析：

通过实验对比样条在均匀化前后不同效果，分析均匀化样条和非均化样条的优缺点，以及考虑机器人在实际插补过程中遇到问题，选用合适的算法进行连续小线段过渡的实验。

1)非均匀样条和均匀样条对比实验

在matlab平台进行，选择多个点，分别采用均匀化样条和非均匀化样条计算，首先选取均匀点集结果如图10所示(上面一条表示非均匀化，下面一条表示均匀化样条)。

如图10所示，均匀样条和非均匀样条能够拟合选择的型值点，并且保证拟合曲线的G1连续性。型值点选取非均匀点集再进行仿真结果如图11所示(上面一条表示非均匀化，下面一条表示均匀化样条)。

通过对比图11，在型值点不均匀的情况下，非均匀抛物线拟合曲线(上面一条曲线)的总长度要长于均匀抛物线样条曲线(下面一条曲线)，机器人末端的运动规划时间更长，效率低于均匀抛物线样条；并且在运动范围容易偏离型值点，在机器人实际运用过程中，容易到达奇异点，或者超出机器人的规定的运动空间。观察A和B两区域，均匀样条在型值点相对相近时，非均匀抛物线拟合曲线的曲率半径为33.6mm，非均匀抛物线拟合曲线的曲率半径57.8mm，非均匀拟合曲率半径更小，容易超过速度和加速度的限制，造成相对较大的冲击。

综合考虑采用样条连续过渡的适应场景为连续小线段时采用样条连续过渡，运动段的长度过短，会减少运动范围以及长度带来的影响，加大曲率过大造成的冲击，对于拟合曲线，运动范围较大和长度较长的影响不及曲率，所以最终采用非均匀样条进行过短运动段的连续规划。

2)非均匀样条仿真实验

示教多个型值点得到如图12仿真轨迹，观察到末端运行轨迹满足G1连续性，验证了在非均匀样条在插补中的可行性。

在机器人实际运用中，需要考虑型值点距离比较近或者重复等特殊情况，选择五个型值点并且让第2点和第3点重合，得到以下仿真轨迹。

非均匀抛物线通过型值点的距离决定分配曲线占比，将1-2段和3-4段已经转换为直线段，2-3段转换为一点，说明非均匀抛物线插值算法解决了型值点重合的问题，与预期相符，并且改善了均匀抛物线在重合点曲率过小的问题，非均匀抛物线通过型值点的距离决定分配曲线占比。

通过使用非均匀抛物线算法，在相对保证平滑的同时，相比与常用的B样条拟合算法，计算速度快，在插补过程中，只需要前后四点的信息，占用资源少，并且局部性较好。

本发明主要研究了机器人笛卡尔空间轨迹平滑过渡技术。首先，作为位姿过渡的基础，阐述了基于贝塞尔样条的构造，包含了位置空间不同运动类型的过渡以及姿态空间的四元数过渡；然后针对相邻运动段工具坐标系不同无法构造过渡曲线的问题，对连续运动段不同工具坐标系平滑过渡算法进行了研究；针对连续小线段过渡计算量复杂的问题，提出基于抛物线样条的连续小运动段轨迹过渡方法包含构造方法以及优化方法；最后基于仿真平台，验证经过过渡处理后末端轨迹可以满足G1连续性，提高了运动的平滑性。

以上显示和描述了本发明的基本原理和主要特征及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (1)

1.一种工业机器人笛卡尔空间轨迹过渡方法，其特征在于：包括贝塞尔样条的不同运动类型位姿过渡方法、运动段不同工具坐标系位姿过渡方法和抛物线样条的连续小线段位姿过渡方法，所述贝塞尔样条的不同运动类型位姿过渡方法包括笛卡尔空间位置过渡方法和笛卡尔空间姿态过渡方法；

所述笛卡尔空间位置过渡方法：设三次贝塞尔样条的表达式为：P(t)＝(1-t)³P₀+(1-t)²tP₁+(1-t)t²P₂+t³P₃，三次贝塞尔样条起点P₀和终点P₃的切矢方向与P₀P₁和P₂P₃方向相同，并且在P₀和P₃点满足G2曲率连续；笛卡尔空间中由于运动段类型的不同，连续两运动段有直线-直线、直线-圆弧以及圆弧-直线四种拼接方式；两段轨迹P_aP_b、P_bP_c相交于P_b点，构造样条过渡曲线的主要工作是选择四个控制点和计算过渡长度；过渡长度S为两相邻运动段轨迹长度中较小值的一半与过渡比例的乘积；选择控制点P₁和P₂与交点P_b重合，在P_aP_b上选择P₀点，在P_bP_c上选择P₃点，保证了运动段与过渡段在衔接点处G1连续，并且满足|P₀P₁|＝|P₂P₃|＝S；首先通过过渡比例首先决定P₀和P₃，然后在P0Pb上选择满足|P₀P₁|＝3S关系的P1点，最后在P3切矢方向选择满足

关系的P₂点；

所述笛卡尔空间姿态过渡方法：使用四元数表示姿态时，如果连续两运动段之间姿态的转角为0，则不进行姿态过渡，否则采用三次贝塞尔样条曲线的四元数形式，计算公式如下：

其中q_i表示控制点姿态，t(t≥0&&t＜1)表示的比例因子；

姿态过渡控制点和角度的选择方法与直线-直线过渡方法类似，过渡转角S为两相邻运动段轨迹转角中较小值的一半与过渡比例的乘积；控制点q₁、q₂与两运动轨迹连接点q_b重合，在q_aq_b和q_bq_c上通过Slerp方法选择控制点q₀和q₃，并满足q₀到q_b的转角等于过渡转角S；则三次贝塞尔样条曲线可以化简表示为：

所述运动段不同工具坐标系位姿过渡方法包括不同工具坐标系过渡段构造与插补方法：两段连续运动段的工具坐标系发生变化时，末端轨迹不能满足平滑性，P0为起点，依次运动到P1、P2和P3，P0P1运动段和P2P3运动段的工具坐标系不同，末端工具坐标系中心TCP点的位置由P1点突变为P2点，使得轨迹不能满足G0连续性，在通常的规划过程中，需要在P0P1运动段终点处停止，然后更换工具坐标系为P2P3段的工具坐标系，会造成运行时速度的不连续性，所以需要通过合适的过渡方法对轨迹和速度进行优化；

通过在两运动段之间添加一段基于贝塞尔样条实现的样条进行优化，重点在于贝塞尔样条控制点的选取、工具坐标系变化的处理以及运动的连续性速度处理；控制点起始点按直线-直线过渡方法按过渡比例选择P0和P3，为满足P0点和P3点的G1连续性，其余两个控制点选择交点P1和P2；

姿态插补的实现使用球面线性插值方法；位置插补的实现参照直线插补，设起点的位置为Pb，终点的位置为Pe，则当前的位置为：

P＝Pb+t(Pe-Pb) (3)

参数t＝max(L，Q)，其中L表示过渡段已完成位置位移除以总位置位移，Q表示过渡段已完成姿态位移除以总姿态位移，由于在过渡过程中过渡段的位移和姿态角度可能出现其中之一为0的情况，因此需要通过选择最大值保证插补的进行；

所述抛物线样条的连续小线段位姿过渡方法包括连续小线段过渡样条构造方法和抛物线样条非均匀优化方法；

所述连续小线段过渡样条构造方法：抛物线样条的一般形式：F(t)＝at2+bt+c(0≤t≤1)，a、b和c为控制系数；过P1，P2和P3三点可以定义一条参数形式的抛物线方程，如图3-7所示该抛物线以P1为起点，以P3为终点，中间通过P2点；

抛物线方程需要构造控制系数a、b和c；当t＝0时，曲线过P1点；当t＝0.5时，曲线过P2点；当t＝1时，曲线过P3点，计算公式如下：

联立方程可以得到控制点：

通过以上方法可以通过三点确定一条抛物线，对于型值点多于3个的情况，需要通过合成抛物线的方法进行曲线拟合；

设有型值点Pi(i＝1，2，3...，n)，每经过相邻三点可以做一段抛物线，由于有n个型值点，所以可以做n-2条抛物线；第i条抛物线Fi经过Pi，Pi+1，Pi+2三点，第i+1条抛物线Fi+1经过Pi+1，Pi+2，Pi+3三点；

每两段曲线之间包含有重叠部分；通过加权合成方法将重叠部分合并，最后整个运动轨迹可以合成一条光滑曲线G(t)，合成公式如下：

G(t)＝(1-s)×F_i(t_i)+s×F_i+1(t_i+1) (6)

s表示权重，G表示合成后的曲线，合成部分为Fi的后半段以及Fi+1的前半段，所以ti取值范围为0.5-1，抛物线Fi+1的参数ti+1与ti的关系为t_i+1＝t_i-0.5，权重s＝2 ti-1；同理，合成除首末两段外的所有重叠区域，可以得到一条光滑拟合曲线；

所述抛物线样条非均匀优化方法：构造样条时，p1、p2和p3之间的距离可能差值比较大；取t＝0.5时，曲线过p2点，当在[0，1]范围内均匀取t值，得到的拟合点G(t)会分布不均匀；构造样条时，首先计算p1、p2和p3之间的距离|p1p2|和|p2p3|，当t＝|p1p2|1/(|1p1p2|+|p2p3|1)时，抛物线过p2点；

对于样条合成需要进行非均匀化处理，对于第i条抛物线Fi，过Pi+1点时t的值设为ki，对于第i+1条抛物线Fi+1，过Pi+2点时t的值设为ki+1，合成部分抛物线Fi的参数ti取值范围为(ki，1]，抛物线Fi+1的参数ti+1与ti的关系为ti+1＝ki+1(ti-ki)/(1-ki)，权重s＝(1-ti)/(1-ki)；通过优化参数，优化后曲线最小曲率半径增大，对速度的限制明显减小，因此曲线的平滑性得到改善，且在合成抛物线样条在插补过程中，只需选择前后四个型值点的信息，不受型值点数目的影响，合成计算速度比较快，拟合曲线更加平滑，适合曲线拟合插补。

Images