CN103970139B - 一种机器人连续点位运动规划方法 - Google Patents

Abstract

本发明公开了一种机器人连续点位运动规划方法，包括如下步骤：(1)确定至少两个数据点的运动信息；(2)在相邻两个数据点所形成的运动曲线上增加至少两个辅助点，以将运动曲线按时间等分为至少三个曲线段；(3)根据相邻两个数据点的运动信息以及辅助点的约束条件，确定边界条件；(4)基于三次样条函数与边界条件，确定运动曲线上各个曲线段的待定参数，以实现机器人连续点位运动规划。本技术方案提供的机器人连续点位运动规划方法采用三次样条函数进行连续点位运动规划时，能够指定更多的边界条件，从而实现了整个运动过程光滑、平稳、无冲击；同时还能够解决过冲回调问题。本发明还提供实现机器人连续点位运动规划方法的运动控制器。

Description

一种机器人连续点位运动规划方法

技术领域

本发明涉及自动控制技术领域，尤其涉及一种机器人连续点位运动规划方法。

背景技术

目前，随着自动化技术的不断发展和应用的广泛普及，工业机器人已在搬运、焊接、包装、喷绘等领域替代了传统的人力。工业机器人的运动过程需要在满足各种工作条件下给出一个高效合理的规划。传统上工业机器人的运动规划分为PTP运动(point-to-point)和CP运动(continuous path)，其中PTP控制最为简单，没有插补运算，各轴同时独立地走完自己的规划即可，其常用于快速定位和搬运作业。

基本的PTP运动从静止开始，到达目标点位置时速度减为零，相应加速度也为零。在实际作业中，为了避开可能的障碍，或者在不同的阶段采用不同的运动速度，往往需要用若干中间点来对整个PTP运动轨迹进行约束，即此时机器人需要连续进行多点的PTP运动规划。如果简单地逐段应用基本的PTP规划，那么在每一个中间点都有一个减速、停顿、加速的过程，这对于搬运等实际作业时不合理的，不仅大大降低了工作效率，而且还增加了能耗。

一般的工业机器人产品都提供了连续点位运动规划，典型的做法是当机器人以给定的范围“接近”目标点时，立即以此当前位姿作为起点，下一点作为目标点，当前速度和加速度作为初始条件，进行下一段的规划。在该做法中，“接近”范围一般是以总行程的百分比表示，例如一旦完成该段行程的90％，即距离目标点不足10％的时候，立即规划并向下一个目标点进发。其优点是：计算简单，能够实现连续的点位运动。缺点是：1，示教或计算得到的中间点不会准确到达，只是“接近”该点；2、整个轨迹的速度可能仍然不连续，比如，当“接近”的范围过小时，其速度连续的效果不明显，仍然会有明显的减速过程，只是不会减速至零；而“接近”范围过大则距离中间目标点的误差较大时就会开始下一目标点的运动。

仅采用传统三次样条曲线进行连续点位运动规划，具有以下不足之处：

1.由于该方式下只能指定2个边界条件，通常选择指定速度，那么起点和终点加速度不为零，有一定的跳变，这会使得在实际应用中产生震动(可以参考图1中所示的采用传统三次样条曲线下的角位移曲线和角加速度曲线的示意图中，在t＝0和t＝10时刻的曲线)；

2.不能保形，即会产生过冲回调问题。样条的光滑性会导致不期望的运动，例如某关节在特定时段内是不参与运动的，但是由于样条对曲线进行光滑会使其产生不必要的运动。

发明内容

本发明所要解决的一个技术问题是采用三次样条函数进行连续点位运动规划时，无法限制起点和终点加速度，产生跳变从而造成运动过程中产生震动；本发明所要解决的另一个技术问题是采用三次样条函数进行连续点位运动规划时运动过程中过冲回调问题。

本发明公开了一种基于样条函数的机器人连续点位运动规划方法，能够指定更多的边界条件，从而实现了整个运动过程光滑、平稳、无冲击；同时还能够解决过冲回调问题。

本发明提供的机器人连续点位运动规划方法，在运动指令所给定的数据点之间增加辅助点，在相邻的数据点之间插入n个中间辅助点，将一段运动曲线分为n+1段，这样就可以进一步地约束所规划的运动轨迹，从而满足更多的边界条件和其他条件。根据多项式的数学原理，每增加1个辅助点，可以增加一个约束条件与之对应。通过满足更多的边界条件和其他条件，使得整个运动过程光滑、平稳、无冲击。

本发明提供的机器人连续点位运动规划方法，通过各个相邻数据点之间的运动位移确定机器人经过中间每一个数据点处的运动速度，从而实现运动轨迹保形性，即保证在每一段相邻数据点之间的运动轨迹是单向的，不存在过冲回调的现象。

本发明提供的机器人连续点位运动规划方法，包括如下步骤：

(1)确定至少两个数据点的运动信息，机器人依次经过数据点，机器人经过的第一个数据点为起点，机器人经过的最后一个数据点为终点；

(2)在相邻两个数据点所形成的运动曲线上增加至少两个辅助点，以将运动曲线按时间等分为至少三个曲线段；

(3)根据相邻两个数据点的运动信息以及辅助点的约束条件，确定边界条件；

(4)基于三次样条函数与边界条件，确定运动曲线上各个曲线段的待定参数，以实现机器人连续点位运动规划。

本发明提供的机器人连续点位运动规划方法，约束规划运动轨迹以满足更多的边界条件，精确地经过每一个给定的数据点(运动指令中给出的途径点，一般由示教或者离线计算获得)。

进一步地，步骤(1)中的运动信息包括：位置、速度及加速度，本发明提供的机器人连续点位运动规划方法，保证机器人在每一数据点处达到给定的速度、加速度。

进一步地，步骤(1)确定至少两个数据点的运动信息包括以下步骤：

(11)分别确定每一个数据点的位置；

(12)确定机器人经过所有数据点处的加速度为零；

(13)确定机器人经过起点处和终点处的运动速度为零；

(14)确定机器人经过中间每一个数据点处的运动速度。

进一步地，机器人经过相邻的数据点形成机器人连续点位运动中的一段，步骤(14)中确定机器人经过中间每一个数据点处的运动速度包括以下步骤：

(141)依次读取所有数据点的位置；

(142)确定各个相邻数据点之间的运动位移；

(143)根据步骤(142)中确定的各个相邻数据点之间的运动位移，确定机器人经过中间每一个数据点处的运动速度。

本发明提供的机器人连续点位运动规划方法，通过各个相邻数据点之间的运动位移确定机器人经过中间每一个数据点处的运动速度，从而实现运动轨迹保形性，即保证在每一段相邻数据点之间的运动轨迹是单向的，不存在过冲回调的现象。

进一步地，步骤(143)包括以下步骤：

(1431)若当前相邻数据点之间的运动位移为机器人连续点位运动的最后一段，或者机器人连续点位运动的下一段运动位移为零，或者机器人连续点位运动的下一段运动位移的方向与当前相邻数据点之间的运动位移的方向相反，当前相邻数据点中所述机器人后经过的数据点处的速度降为零；

(1432)若机器人连续点位运动的下一段运动位移的方向与当前相邻数据点之间的运动位移的方向相同，根据该方向上的实际剩余的运动距离与机器人的最大减速度来确定安全速度；若安全速度小于运动指令速度，则当前相邻数据点中机器人后经过的数据点处的速度为安全速度；否则，当前相邻数据点中机器人后经过的数据点处的速度为根据运动指令速度确定。

本发明提供的机器人连续点位运动规划方法，通过各个相邻数据点之间的运动位移确定机器人经过中间每一个数据点处的运动速度，从而实现运动轨迹保形性，即保证在每一段相邻数据点之间的运动轨迹是单向的；通过机器人的最大减速度确定的安全速度，比较指令速度与安全速度，保证机器人运动的安全性。

进一步地，辅助点的约束条件包括：辅助点的位置连续、速度连续以及加速度连续。

本发明提供的机器人连续点位运动规划方法，能保证机器人在整个运动轨迹上实现速度连续、加速度连续，从而实现了整个运动过程光滑、平稳、无冲击。

进一步地，步骤(2)中在相邻两个数据点所形成的运动曲线上增加的辅助点数目为两个，以将这段运动曲线按时间等分为三个曲线段。

本发明提供的机器人连续点位运动规划方法，能保证机器人在每一数据点处达到给定的速度、加速度，且速度、加速度连续，从而彻底消除了加速度跳变，满足所插入的中间点处位置、速度、加速度连续的要求，实现了位置、速度的平滑过渡。

进一步地，步骤(4)中采用三次样条函数表示三个曲线段的表达式为：

$\left\{\begin{array}{c}{S}_1=a_{10}+a_{11}t+a_{12}{t}^2+a_{13}{t}^3\\ S_2=a_{20}+a_{21}t+a_{22}{t}^2+a_{23}{t}^3\\ S_3=a_{30}+a_{31}t+a_{32}{t}^2+a_{33}{t}^3\end{array}\right.$

其中，S₁、S₂、S₃分别为三个曲线段，a₁₀、a₁₁、……、a₃₃为待定参数，所述待定参数的表达式为：

$\left\{\begin{array}{c}{a}_{10}=P_0\\ a_{11}=V_0\\ a_{12}=\frac{A_0}{2}\\ a_{13}=\frac{6(P_f-P_0)-12V_0{t}_s-6V_f{t}_s-11A_0{t_s}^2+2A_f{t_s}^2}{36{t_s}^3}\end{array}\right.$

$\left\{\begin{array}{c}{a}_{20}=\frac{30P_0+6P_f+6(4V_0-V_f)t_s+(7A_0+2A_f){t_s}^2}{36}\\ a_{21}=\frac{6(P_f-P_0)-6V_f{t}_s+A_0{t_s}^2+2{A_s}^2}{12t_s}\\ a_{22}=\frac{6(P_f-P_0)-6(2V_0+V_f)t_s-(5A_0-2A_f){t_s}^2}{12{t_s}^2}\\ a_{23}=\frac{12(P_0-P_f)+18(V_0+V_f)t_s+7(A_0-A_f){t_s}^2}{{{36}_s}^3}\end{array}\right.$

$\left\{\begin{array}{c}{a}_{30}=\frac{6(P_0+5P_f)+6(V_0-4V_f)t_s+(2A_0+7A_f){t_s}^2}{36}\\ a_{31}=\frac{6(P_f-P_0)-6V_0{t}_s-(2A_0+A_f){t_s}^2}{12t_s}\\ a_{32}=\frac{6(P_0-P_f)+6(V_0+2V_f)t_s+(2A_0+5A_f){t_s}^2}{12{t_s}^2}\\ a_{33}=\frac{6(P_f-P_0)-6(V_0+2V_f)t_s-(2A_0-11A_f){t_s}^2}{36{t_s}^3}\end{array}\right.$

其中，P₀、P_f分别为相邻两个所述数据点的位置；V₀、V_f分别为所述机器人经过相邻两个所述数据点处的速度；A₀、A_f分别为所述机器人经过相邻两个所述数据点处的加速度；t_f为所述机器人在相邻两个所述数据点之间运动所需的总时间；t_s为所述机器人在每一个所述曲线段运动所需的时间，其中，

进一步地，步骤(3)中的边界条件为：

$\left\{\begin{array}{c}{S}_1\left(0\right)=P_0\\ S_1^{\′}\left(0\right)=V_0\\ S_1^{\′\′}\left(0\right)=A_0\\ S_1\left(t_s\right)=S_2\left(0\right)\\ S_1^{\′}\left(t_s\right)=S_2^{\′}\left(0\right)\\ S_1^{\′\′}\left(t_s\right)=S_2^{\′\′}\left(0\right)\\ S_2\left(t_s\right)=S_3\left(0\right)\\ S_2^{\′}\left(t_s\right)=S_3^{\′}\left(0\right)\\ S_2^{\′\′}\left(t_s\right)=S_3^{\′\′}\left(0\right)\\ S_3\left(t_s\right)=P_f\\ S_3^{\′}\left(t_s\right)=V_f\\ S_3^{\′\′}\left(t_s\right)=A_f\end{array}\right.$

本发明还提供一种实现机器人连续点位运动规划方法的运动控制器，将数据点与辅助点构成新的数据点序列，如果运动控制器自身支持三次样条函数插补，可直接将新的数据点序列输入运动控制器，运动控制器将自行完成插补运算，实现运动控制，即可实现本发明的机器人连续点位运动规划方法；

如果运动控制器不支持三次样条函数插补，可以根据新的数据点序列，按照标准三次样条算法进行插补运算得到插补点序列，输入插补点序列到运动控制器，令运动控制器实现普通位置跟随控制，即可实现本发明的机器人连续点位运动规划方法。

与现有技术相比，本发明提供的机器人连续点位运动规划方法具有以下有益效果：

(1)通过在相邻两个数据点之间增加辅助点，从而约束规划运动轨迹以满足更多的边界条件，精确地经过每一个给定的数据点；

(2)通过设定数据点的运动信息(包括位置、速度及加速度)以及辅助点的约束条件(包括辅助点的位置连续、速度连续以及加速度连续)，使得机器人经过每一数据点处达到给定的速度和加速度，且在辅助点上速度连续、加速度连续，从而实现机器人的整个运动过程平滑、平稳、无冲击，彻底消除了加速度跳变，实现位置、速度的平滑过渡；

(3)根据各个相邻所述数据点之间的运动位移，确定机器人经过中间每一个数据点处的运动速度，从而实现运动轨迹保形性，即保证在每一段相邻数据点之间的运动轨迹是单向的，不存在过冲回调的现象；

(4)在实际应用中，可以根据机器人运动所要求的边界条件的数目，来确定辅助点的数目，在需要增加边界条件的情况下，相应地增加辅助点的数目，以满足对机器人的控制要求。

附图说明

图1是采用传统三次样条曲线下的角位移曲线和角加速度曲线的示意图；

图2是本发明的一个实施例中的机器人连续点位运动规划方法的流程示意图；

图3是利用图2所示的连续点位运动规划方法，经过两个数据点做点位运动的角位移、角速度、角加速度曲线的示意图；

图4是利用图2所示的连续点位运动规划方法，经过三个数据点做点位运动的角位移、角速度、角加速度曲线的示意图；

图5是利用图2所示的连续点位运动规划方法，经过三个数据点做点位运动的角位移、角速度、角加速度曲线的示意图；

图6是利用图2所示的连续点位运动规划方法，经过三个数据点做点位运动的角位移、角速度、角加速度曲线的示意图。

具体实施方式

下面结合附图对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

图2是本发明的一个实施例中的机器人连续点位运动规划方法的流程示意图，如图2所示，机器人连续点位运动规划方法包括如下步骤：

步骤S1：确定至少两个数据点的运动信息，机器人依次经过所述数据点，所述机器人经过的第一个数据点为起点，所述机器人经过的最后一个数据点为终点；

步骤S2：在相邻两个数据点所形成的运动曲线上增加至少两个辅助点，以将这段运动曲线按时间等分为至少三个曲线段；

步骤S3：根据相邻两个数据点的运动信息以及辅助点的约束条件，确定边界条件；

步骤S4：基于三次样条函数与所述边界条件，确定运动曲线上各个曲线段的待定参数，以实现机器人连续点位运动规划。

需要说明的是，本发明实施例提供的机器人连续点位运动规划方法可以通过运动控制器(控制机器人)来实现完成，并且该运动规划方法利用了标准的样条函数，该样条函数在工程实践中得到广泛应用，诸多运动控制器产品都支持样条函数，即使有些运动控制器产品不支持样条函数，也可以结合插补运算方式来实现。

具体来说，如步骤S1所述，确定至少两个数据点的运动信息，机器人依次经过所述数据点，所述机器人经过的第一个数据点为起点，所述机器人经过的最后一个数据点为终点。

所述数据点是在关节空间坐标系下，机器人要途经或到达的空间位置。这些空间位置可以由运动指令(示教或者离线计算得到)来确定。所述运动信息包括位置、速度及加速度，但在实践中并不限于此。各个数据点的运动信息也可以通过运动指令(示教或者离线计算得到)来确定。

在本步骤中，确定至少两个数据点的运动信息包括：

步骤(11)分别确定每一个所述数据点的位置。即机器人在运动过程中，途径或到达的空间位置，在关节空间坐标系中可以以坐标点的形式来确定。

步骤(12)确定所述机器人经过所有所述数据点处的加速度为零。需要说明的是，在本实施例中，为了简化条件，在无特殊要求情况下，规定所有所述数据点处的加速度为零。在其他实施例中，也可以根据机器人的控制要求确定所有数据点处的加速度为其他数值。

步骤(13)确定所述机器人经过所述起点处和所述终点处的运动速度为零。

步骤(14)确定所述机器人经过中间每一个所述数据点处的运动速度。其中，中间每一个所述数据点是指机器人经过所有的数据点中除了上述起点和终点之外的其他数据点。

在本实施例中，上述各个运动信息都可以通过运动控制器发出的运动指令来确定。

在步骤(14)中，确定所述机器人经过中间每一个所述数据点处的运动速度包括以下步骤：

步骤(141)依次读取所有所述数据点的位置。

在实践中，运动控制器基于运动指令，预读取所有所述数据点，并确定各个数据点的位置。

步骤(142)确定各个相邻所述数据点之间的运动位移。其中，所述运动位移是指相邻两个数据点之间的矢量距离(包括运动距离和运动方向)。在实际应用中，可以确定从起点到终点的方向为运动位移的正方向，与之相反的即为运动位移的反方向。

步骤(143)根据上述步骤(142)中确定的各个相邻所述数据点之间的运动位移，确定所述机器人经过中间每一个所述数据点处的运动速度。

在本实施例中，为了保证在每一段相邻数据点之间的运动轨迹是单向的，不存在过冲回调的现象。本步骤具体包括如下步骤：

步骤(1431)：若当前相邻所述数据点之间的运动位移为所述机器人连续点位运动的最后一段，或者所述机器人连续点位运动的下一段运动位移为零，或者所述机器人连续点位运动的下一段运动位移的方向与当前相邻所述数据点之间的运动位移的方向相反，当前相邻所述数据点中所述机器人后经过的所述数据点处的速度降为零。

其中，当前相邻所述数据点之间的运动位移为所述机器人连续点位运动的最后一段是指该运动位移为倒数第二个数据点与终点之间的运动位移。

所述机器人连续点位运动的下一段运动位移为零是指机器人从当前数据点所在位置起作运动又回到该数据点的情况。

步骤(1432)：若所述机器人连续点位运动的下一段运动位移的方向与当前相邻所述数据点之间的运动位移的方向相同，根据所述方向上的实际剩余的运动距离与所述机器人的最大减速度来确定安全速度；若所述安全速度小于运动指令速度，则当前相邻所述数据点中所述机器人后经过的所述数据点处的速度为安全速度；否则，当前相邻所述数据点中所述机器人后经过的所述数据点处的速度为根据运动指令速度确定。

在本步骤中，为了保证机器人运行安全，需要确定机器人经过一个数据点处的安全速度。具体地，在机器人连续点位运动的下一段运动位移的方向与当前相邻两个数据点之间的运动位移的方向相同的情况下，运动控制器可以基于方向上的实际剩余的运动距离与所述机器人的最大减速度来确定安全速度，这里可以根据上述已知数据利用物理上的运动公式来计算得到，在此不再赘述。

需要说明的是，在实际应用中，若运动指令中未具体指定各个中间数据点的运动指令速度，则直接根据计算得到的安全速度来确定当前相邻所述数据点中所述机器人后经过的所述数据点处的速度。

另外，为保证机器人各关节同步，计算本段运动中各关节运动时间，以其中时间最长者作为实际运动时间，其余各关节依据此时间以及各自的关节位移重新计算速度。

如步骤S2所述，在相邻两个数据点所形成的运动曲线上增加至少两个辅助点，以将这段运动曲线按时间等分为至少三个曲线段。

与现有技术不同，除了通过运动指令等方式给定的数据点及其运动信息之外，在相邻两个数据点之间增加至少两个辅助点。在实践中，可以通过在算法中设定所要增加的辅助点的数目，并自动在相邻两个数据点所形成的运动曲线上增加辅助点，而所增加辅助点的数目可以根据后续所采用的三次样条函数所需的边界条件的数目来确定。

根据多项式的数学原理，每增加一个待定点，可以相应地增加一个边界条件与之对应。在利用传统的三次样条函进行连续点运动规划时，只能设定2个边界条件(通常指定数据点处的速度)。若进一步指定相邻两个数据点的加速度，则需要增加2个辅助点。在实践中，若机器人应用中要求更多的约束条件，则还需要增加更多的辅助点。

这样相邻两个数据点所形成的运动曲线被分成至少三个曲线段，其中，曲线段的数目为增加的辅助点的数目加1。例如，增加2个辅助点，则运动曲线被分成3个曲线段。

如步骤S3所述，根据相邻两个数据点的运动信息以及辅助点的约束条件，确定边界条件。

在本实施例中，为了保证整个运动过程光滑、平稳、无冲击。所增加的辅助点需满足的约束条件包括辅助点的位置连续、速度连续以及加速度连续。所述边界条件包括数据点的运动信息以及辅助点的约束条件。例如，所述边界条件包括：各个数据点的位置信息、速度和加速度，以及辅助点的位置连续、速度连续以及加速度连续。

如步骤S4所述，基于三次样条函数与边界条件，确定所述运动曲线上各个曲线段的待定参数，以实现机器人连续点位运动规划。

与现有技术不同，本实施例中，由于在上述步骤S2中，在相邻两个数据点所形成的运动曲线上增加至少两个辅助点，从而进一步约束规划运动轨迹以满足更多的边界条件。而且通过这样的规划方法得到的运动轨迹可以精确地经过每一个给定的数据点，并在各个数据点处达到给定的速度和加速度。

如上文所述，每增加一个边界条件，可以对应增加一个辅助点。进一步地，根据已知的数据(例如数据点的位置信息、速度和加速度、以及辅助点的约束条件等)，利用公式推导、迭代计算可以确定各个待定参数，从而实现机器人连续点位运动规划。

下面以在相邻两个数据点所形成的运动曲线上增加两个辅助点，将这段运动曲线按时间等分为三个曲线段，并基于三次样条函数进行连续点位运动规划为例来进一步描述本发明实施例提供的机器人连续点位运动规划方法。

具体地，应用三次样条函数表示三个曲线段的表达式为：

$\left\{\begin{array}{c}{S}_1=a_{10}+a_{11}t+a_{12}{t}^2+a_{13}{t}^3\\ S_2=a_{20}+a_{21}t+a_{22}{t}^2+a_{23}{t}^3\\ S_3=a_{30}+a_{31}t+a_{32}{t}^2+a_{33}{t}^3\end{array}\right.---\left(1\right)$

其中，S₁、S₂、S₃分别为三个曲线段、a₁₀、a₁₁、……a₃₃为12个待定参数。在本实施例中，共设定12个边界条件，分别为：相邻两个数据点的位置信息(即两端点的位置)、相邻两个数据点处的速度和加速度；以及在两个辅助点处所要求的约束条件(包括位置连续、速度连续和加速度连续)。由此可消除加速度跳变，更好地约束位移曲线形状并使得运动曲线更加平顺。

设定P₀、P_f分别为相邻两个数据点的位置；

V₀、V_f分别为机器人经过这两个数据点处的速度；

A₀、A_f分别为机器人经过这两个数据点处的加速度；

t_f为机器人在相邻两个数据点之间运动所需的总时间；

t_s为机器人在每一个曲线段运动所需的时间，其中，

根据Hermite插值原理，12个边界条件可列得如下方程：

$\left\{\begin{array}{c}{S}_1\left(0\right)=P_0\\ S_1^{\′}\left(0\right)=V_0\\ S_1^{\′\′}\left(0\right)=A_0\\ S_1\left(t_s\right)=S_2\left(0\right)\\ S_1^{\′}\left(t_s\right)=S_2^{\′}\left(0\right)\\ S_1^{\′\′}\left(t_s\right)=S_2^{\′\′}\left(0\right)\\ S_2\left(t_s\right)=S_3\left(0\right)\\ S_2^{\′}\left(t_s\right)=S_3^{\′}\left(0\right)\\ S_2^{\′\′}\left(t_s\right)=S_3^{\′\′}\left(0\right)\\ S_3\left(t_s\right)=P_f\\ S_3^{\′}\left(t_s\right)=V_f\\ S_3^{\′\′}\left(t_s\right)=A_f\end{array}\right.---\left(2\right)$

其中，S₁'()为S₁()的一阶导数(对应速度)、S₁”()为S₁()的二阶导数(对应加速度)。

由(2)式可以导出12个待定参数满足的方程为：

$\left\{\begin{array}{c}{a}_{10}=P_0\\ a_{11}=V_0\\ 2a_{12}=A_0\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}{a}_{10}+a_{11}{t}_s+a_{12}{t_s}^2+a_{13}{t_s}^3=a_{20}\\ a_{11}+2a_{12}{t}_s+3a_{13}{t_s}^2=a_{21}\\ 2a_{12}+6a_{13}{t}_s=2a_{22}\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{a}_{20}+a_{21}{t}_s+a_{22}{t_s}^2+a_{23}{t_s}^3=a_{30}\\ a_{21}+2a_{22}{t}_s+3a_{23}{t_s}^2=a_{31}\\ 2a_{22}+6a_{23}{t}_s=2a_{32}\end{array}\right.---\left(5\right)$

$\left\{\begin{array}{c}{a}_{30}+a_{31}{t}_s+a_{32}{t_s}^2+a_{33}{t_s}^2=P_f\\ a_{31}+2a_{32}{t}_s+3a_{33}{t_s}^2=V_f\\ 2a_{32}+6a_{33}{t}_s=A_f\end{array}\right.---\left(6\right)$

进一步地，由(3)-(6)式可得到(1)中各式的待定参数：

$\left\{\begin{array}{c}{a}_{10}=P_0\\ a_{11}=V_0\\ a_{12}=\frac{A_0}{2}\\ a_{13}=\frac{6(P_f-P_0)-12V_0{t}_s-6V_f{t}_s-11A_0{t_s}^2+2A_f{t_s}^2}{36{t_s}^3}\end{array}\right.---\left(7\right)$

$\left\{\begin{array}{c}{a}_{20}=\frac{30P_0+6P_f+6(4V_0-V_f)t_s+(7A_0+2A_f){t_s}^2}{36}\\ a_{21}=\frac{6(P_f-P_0)-6V_f{t}_s+A_0{t_s}^2+2{A_s}^2}{12t_s}\\ a_{22}=\frac{6(P_f-P_0)-6(2V_0+V_f)t_s-(5A_0-2A_f){t_s}^2}{12{t_s}^2}\\ a_{23}=\frac{12(P_0-P_f)+18(V_0+V_f)t_s+7(A_0-A_f){t_s}^2}{{{36}_s}^3}\end{array}\right.---\left(8\right)$

$\left\{\begin{array}{c}{a}_{30}=\frac{6(P_0+5P_f)+6(V_0-4V_f)t_s+(2A_0+7A_f){t_s}^2}{36}\\ a_{31}=\frac{6(P_f-P_0)-6V_0{t}_s-(2A_0+A_f){t_s}^2}{12t_s}\\ a_{32}=\frac{6(P_0-P_f)+6(V_0+2V_f)t_s+(2A_0+5A_f){t_s}^2}{12{t_s}^2}\\ a_{33}=\frac{6(P_f-P_0)-6(V_0+2V_f)t_s-(2A_0-11A_f){t_s}^2}{36{t_s}^3}\end{array}\right.---\left(9\right)$

根据上述(7)-(9)式可以确定这12个待定参数的具体数值，再将这12个已定参数代入(1)式中，可以实现机器人连续点位运动规划。利用本技术方案提供的连续点位运动规划方法，能实现数据点位置准确到达且速度、加速度连续，而且由于该方法可以满足更多的边界条件，从而彻底消除了加速度跳变，满足所插入的辅助点处位置、速度、加速度连续的要求，实现了位置、速度及加速度的平滑过渡。

如图3是利用本实施例中的连续点位运动规划方法，经过两个数据点做点位运动的角位移、角速度、角加速度曲线的示意图。

如图4-图6是利用图2所示的连续点位运动规划方法，经过三个数据点做点位运动的角位移、角速度、角加速度曲线的示意图。其中，图4对应两端点速度为0，中间点高速的运动情况；图5对应两端点为高速，中间点速度为0的运动情况；图6对应起始点和中间点高速运动，末端点降速为0的运动情况。

需要说明的是，在上述图3、图4-图6中已对角位移、角速度、角加速度、时间量做了归一化处理。

本发明提供的机器人连续点位运动规划方法是基于标准的三次样条函数，该函数在工程实践中得到广泛的应用，现有的很多运动控制器产品都直接支持这一基本函数，因此本发明提供的规划方法能够简单方便地在这些控制器上实现。即使不能直接支持此函数的运动控制器产品，由于三次多项式函数的算法比较简单，也易于采用软件插补的方式实现。

对于能够直接支持三次样条函数的运动控制器，可以依据公式(1)，计算S₁(t_s)以及S₂(t_s)作为各中间辅助点的位置。在指令给定的数据点之间插入辅助点后构成新的数据点序列，可以将此数据点序列输入到运动控制器，令运动控制器执行三次样条运动，即可执行本发明所描述的连续点位运动。

对于不支持三次样条函数的运动控制器，可以依据公式(1)进行插补计算，所得插补点输入到运动控制器，令运动控制器执行位置跟随控制，即可执行本发明所描述的连续点位运动。

本发明提供的机器人连续点位运动规划方法，增加至少两个辅助点，但不限于两个辅助点，目前公式推导只给出了增加两个辅助点的情况，根据约束条件的需要增加多于两个辅助点，可以根据新的约束条件，利用本实施例中给出的方法作新的推导，以满足对机器人的控制要求。

本发明提供的机器人连续点位运动规划方法具有以下有益效果：通过在相邻两个数据点之间增加辅助点，从而约束规划运动轨迹以满足更多的边界条件，精确地经过每一个给定的数据点；通过设定数据点的运动信息(包括位置、速度及加速度)以及辅助点的约束条件(包括辅助点的位置连续、速度连续以及加速度连续)，使得机器人经过每一数据点处达到给定的速度和加速度，且在辅助点上速度连续、加速度连续，从而实现机器人的整个运动过程平滑、平稳、无冲击，彻底消除了加速度跳变，实现位置、速度的平滑过渡；根据各个相邻所述数据点之间的运动位移，确定机器人经过中间每一个数据点处的运动速度，从而实现运动轨迹保形性，即保证在每一段相邻数据点之间的运动轨迹是单向的，不存在过冲回调的现象；在实际应用中，可以根据机器人运动所要求的边界条件的数目，来确定辅助点的数目，在需要增加边界条件的情况下，相应地增加辅助点的数目，以满足对机器人的控制要求。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (5)

1.一种机器人连续点位运动规划方法，包括如下步骤：

(1)确定至少两个数据点的运动信息，机器人依次经过所述数据点，所述机器人经过的第一个数据点为起点，所述机器人经过的最后一个数据点为终点；

(2)在相邻两个所述数据点所形成的运动曲线上增加至少两个辅助点，以将所述运动曲线按时间等分为至少三个曲线段；

(3)根据相邻两个所述数据点的运动信息以及所述辅助点的约束条件，确定边界条件；

(4)基于三次样条函数与所述边界条件，确定所述运动曲线上各个所述曲线段的待定参数，以实现机器人连续点位运动规划；

步骤(1)中所述的运动信息包括：位置、速度以及加速度；

步骤(1)确定至少两个数据点的运动信息包括以下步骤：

(11)分别确定每一个所述数据点的位置；

(12)确定所述机器人经过所有所述数据点处的加速度为零；

(13)确定所述机器人经过所述起点处和所述终点处的运动速度为零；

(14)确定所述机器人经过中间每一个所述数据点处的运动速度；

所述机器人经过相邻的所述数据点形成所述机器人连续点位运动中的一段，步骤(14)中确定所述机器人经过中间每一个所述数据点处的运动速度包括以下步骤：

(141)依次读取所有所述数据点的位置；

(142)确定各个相邻所述数据点之间的运动位移；

(143)根据步骤(142)中确定的各个相邻所述数据点之间的运动位移，确定所述机器人经过中间每一个所述数据点处的运动速度；

其特征在于，步骤(143)包括以下步骤：

(1431)若当前相邻所述数据点之间的运动位移为所述机器人连续点位运动的最后一段，或者所述机器人连续点位运动的下一段运动位移为零，或者所述机器人连续点位运动的下一段运动位移的方向与当前相邻所述数据点之间的运动位移的方向相反，当前相邻所述数据点中所述机器人后经过的所述数据点处的速度降为零；

(1432)若所述机器人连续点位运动的下一段运动位移的方向与当前相邻所述数据点之间的运动位移的方向相同，根据所述方向上的实际剩余的运动距离与所述机器人的最大减速度来确定安全速度；若所述安全速度小于运动指令速度，则当前相邻所述数据点中所述机器人后经过的所述数据点处的速度为安全速度；否则，当前相邻所述数据点中所述机器人后经过的所述数据点处的速度为根据运动指令速度确定。

2.如权利要求1所述的机器人连续点位运动规划方法，其特征在于，步骤(3)中所述辅助点的约束条件包括：所述辅助点的位置连续、速度连续以及加速度连续。

3.如权利要求2所述的机器人连续点位运动规划方法，其特征在于，步骤(2)中在相邻两个数据点所形成的运动曲线上增加的辅助点数目为两个，以将这段运动曲线按时间等分为三个曲线段。

4.如权利要求3所述的机器人连续点位运动规划方法，其特征在于，步骤(4)中采用三次样条函数表示三个曲线段的表达式为：

$\left\{\begin{array}{c}{S}_1=a_{10}+a_{11}t+a_{12}{t}^2+a_{13}{t}^3\\ S_2=a_{20}+a_{21}t+a_{22}{t}^2+a_{23}{t}^3\\ S_3=a_{30}+a_{31}t+a_{32}{t}^2+a_{33}{t}^3\end{array}\right.$

其中，S₁、S₂、S₃分别为三个曲线段，a₁₀、a₁₁、……、a₃₃为待定参数，所述待定参数的表达式为：

$\left\{\begin{array}{c}{a}_{10}=P_0\\ a_{11}=V_0\\ a_{12}=\frac{A_0}{2}\\ a_{13}=\frac{6\left(P_f-P_0\right)-12V_0{t}_s-6V_f{t}_s-11A_0{t_s}^2+2A_f{t_s}^2}{36{t_s}^3}\end{array}\right.$

$\left\{\begin{array}{c}{a}_{20}=\frac{30P_0+6P_f+6\left(4V_0-V_f\right)t_s+\left(7A_0+2A_f\right){t_s}^2}{36}\\ a_{21}=\frac{6\left(P_f-P_0\right)-6V_f{t}_s+A_0{t_s}^2+2A_f{t_s}^2}{12t_s}\\ a_{22}=\frac{6\left(P_f-P_0\right)-6\left(2V_0+V_f\right)t_s-\left(5A_0-2A_f\right){t_s}^2}{12{t_s}^2}\\ a_{23}=\frac{12\left(P_0-P_f\right)+18\left(V_0+V_f\right)t_s+7\left(A_0-A_f\right){t_s}^2}{36{t_s}^3}\end{array}\right.$

$\left\{\begin{array}{c}{a}_{30}=\frac{6\left(P_0+5P_f\right)+6\left(V_0-4V_f\right)t_s+\left(2A_0+7A_f\right){t_s}^2}{36}\\ a_{31}=\frac{6\left(P_f-P_0\right)-6V_0{t}_s-\left(2A_0+A_f\right){t_s}^2}{12t_s}\\ a_{32}=\frac{6\left(P_0-P_f\right)+6\left(V_0+2V_f\right)t_s+\left(2A_0-5A_f\right){t_s}^2}{12{t_s}^2}\\ a_{33}=\frac{6\left(P_f-P_0\right)-6\left(V_0+2V_f\right)t_s-\left(2A_0-11A_f\right){t_s}^2}{36{t_s}^3}\end{array}\right.$

其中，P₀、P_f分别为相邻两个所述数据点的位置；V₀、V_f分别为所述机器人经过相邻两个所述数据点处的速度；A₀、A_f分别为所述机器人经过相邻两个所述数据点处的加速度；t_f为所述机器人在相邻两个所述数据点之间运动所需的总时间；t_s为所述机器人在每一个所述曲线段运动所需的时间，其中，

5.如权利要求4所述的机器人连续点位运动规划方法，其特征在于，步骤(3)中的边界条件为：

$\left\{\begin{array}{c}{S}_1\left(0\right)=P_0\\ S_1^{,}\left(0\right)=V_0\\ S_1^{,,}\left(0\right)=A_0\\ S_1\left(t_s\right)=S_2\left(0\right)\\ S_1^{,}\left(t_s\right)=S_2^{,}\left(0\right)\\ S_1^{,,}\left(t_s\right)=S_2^{,,}\left(0\right)\\ S_2\left(t_s\right)=S_3\left(0\right)\\ S_2^{,}\left(t_s\right)=S_3^{,}\left(0\right)\\ S_2^{,,}\left(t_s\right)=S_3^{,,}\left(0\right)\\ S_3\left(t_s\right)=P_f\\ S_3^{,}\left(t_s\right)=V_f\\ S_3^{,,}\left(t_s\right)=A_f\end{array}\right..$