机器人运动轨迹规划方法及相关装置与流程

本发明涉及运动控制领域，更具体地说，涉及机器人运动轨迹规划技术。

背景技术：

在对机器人运动控制中，被控部件(例如操作臂)的轨迹运动方式可分为cp运动(连续运行，包括直线运动和圆弧运动)和ptp运动(点到点运动)。

在对机器人进行运动轨迹规划时，可能需要多段cp运动连续进行。以两段cp运动连续进行为例，请参见图1a，cp运动轨迹段1(也可称为轨迹段1)和cp运动轨迹段2(也可称为轨迹段2)可分别为直线段，此外，还可圆弧段与圆弧段(图1b)、圆弧段与直线段(图1c和图1d)连续进行。

在单段cp运动的规划中，其初速度和末速度都为0，如果在多段cp连续运动的过程中令每段cp运动的初始速度和末速度都降为0，会大大降低工作效率，并且频繁的加减速也会对电机和减速器的寿命有影响。因此，运动轨迹规则的趋势是不令多段cp连续运动中的各段cp运动的速度降为零。

这就面临另一问题：仍请参见图1a，若连续的直线段不在同一直线上，就会有尖角。尖角的顶点为o(o点是两轨迹段的交点)。同理，请参见图1b，若连续的两圆弧段切线不重合，也会有尖角。同理，若直线段与的圆弧段的切线不重合(请参见图1c和图1d)，也会有尖角。速度不为0通过尖角的话，会产生振动。

为此，可在连续的两cp运动轨迹段之间设计平滑过渡轨迹段。例如，请参见图2，可采用矢量叠加法得到平滑过渡轨迹段。这种方式通过空间向量加法，令图2中轨迹段1从拐出点a到交点o的插补点，与轨迹段2从拐入点b到交点o的插补点重合。叠加后的插补点会组成空间的一段弧形(也即图2中点a与点b之间的弧形)，使轨迹平滑。

也即，最终规划出的运行轨迹是，由c点运行至a点，之后拐出轨迹段1，进入点a和点b之间的平滑过渡轨迹段，再由点b拐入轨迹段2，最后到达d。

当然，对于超过两段cp运动的连续运动，可分别对两两连续的cp运动轨迹段进行如此设计。

矢量叠加法虽然对前后相连的cp运动进行了平滑，但在实际应用中，如图1a-1d所示，有直线段与直线段转接、直线段与圆弧段转接、圆弧段与圆弧段转接。矢量叠加法虽可用圆弧替代尖角，但其仅在直线段与直线段转接的场景下肯定适用，在直线段与圆弧段转接、圆弧段与圆弧段的转接中可能无法找到与两条轨迹都相切的圆弧。这是因为空间中相交的直线段与直线段是肯定共面的，但是直线段与圆弧段、圆弧段与圆弧段都不一定共面，在不共面的情况下就无法找出公切的圆弧转接。

因此，现在需要机器人运动轨迹规划技术解决方案，以令无论连续的运动轨迹段是否共面均能进行平滑转接，并在拐点处(拐入点和拐出点)速度连续(也即速度不必降为0)。

技术实现要素：

有鉴于此，本发明实施例的目的在于提供机器人运动轨迹规划方法及相关装置，以令连续的cp运动平滑转接，同时，速度不必降为0。

本申请发明人在研究过程中发现：

bezier曲线由线段和节点组成的数学曲线。其整体形状并不局限于圆弧，可实现非共面的轨迹平滑，从而可应用在机器人运动轨迹规划方面。

可令bezier曲线作为连续的运动轨迹段间的平滑过渡轨迹段，bezier曲线的起始点与一运动轨迹段(可称为lc轨迹段)的拐出点重合，bezier曲线的终止点与另一运动轨迹段(可称为ln轨迹段)的拐入点重合。

发明人发现，欲实现在拐点处(拐出点和拐入点)速度连续，则bezier曲线在其起始点需与lc轨迹相切，同时在其终止点需与ln轨迹相切。

n次(n次)bezier曲线公式为：

其中bi,n(t)称为bernstein基函数，t＝0对应beizer曲线的起始点，t＝1对应beizer曲线的终止点。

p0-pn这n个特征点或控制点在平面或在三维空间定义了n次bezier曲线，其组成的多边形称为特征多边形或控制多边形。其中，p0为起始点，pn为终止点，p1-pn-1为中间点。例如请参见图3，p0、p1、p2、p3四个点定义了三次bezier曲线，三次bezier曲线起始于p0走向p1，并从p2方向到p3。需要说明的是，一般三次bezier曲线不经过p1、p2，这两个点只提供方向。

根据bezier基函数的导函数性质可求取n次bezier曲线始末点的切矢量为则：

当t＝0时，b′(0)＝n(p1-p0)，b′(0)即为n次bezier曲线在起始点的切矢量；

当t＝1时，b′(1)＝n(pn-pn-1)，b′(1)即为n次bezier曲线在终止点的切矢量。

观察上述切矢量可知，n次bezier曲线在起始点和终止点处的切线方向(即切矢量)和特征多边型的第一条边及最后一条边走向一致。以图3所示三次bezier曲线为例，b′(0)＝3(p1-p0)，b′(1)＝3(p3-p2)，而图3的第一条边是p1p0，最后一条边是p3p2。

为令n次bezier曲线在始末点的切矢方向分别与lc轨迹段和ln轨迹段切矢方向相同，需要最少四个特征点或控制点，所以可选用三次bezier曲线作为平滑过渡轨迹段。

然而，本申请发明人在进一步的研究过程中发现：

使用三次bezier曲线作为平滑过渡轨迹段虽然能令连续的cp运动平滑转接，同时，速度不必降为0，但仅能保证在拐点处速度连续，而在拐点处的加速度则不一定连续。随着机器人科技的发展，对于机器人轨迹规划的要求越来越高，如果加速度不连续，在拐点处跳变的加速度会造成电机震动，对电机和减速器造成损伤。

本申请发明人通过分析加速度跳变的原因，发现加速度跳变的原因是因为突增的向心加速度。

加速度连续的充要条件为在速度连续的基础上曲率连续。

某一点的向心加速度有公式其中v表示该点的速度，ρ表示该点的曲率。在v连续的情况下，保证曲率连续就能保证轨迹加速度连续。

曲线的曲率公式有：其中b′(t)表示bezier曲线的一阶导函数，b″(t)表示bezier曲线的二阶导函数。通过曲率公式可以得出：确保bezier曲线二阶可导即可确保bezier曲线上任一点加速度连续。所以只要次数大于1的bezier曲线，其轨迹一定曲率连续。

另一方面，为保证拐点处不跳变，要求拐点处速度和加速度均连续，这需要bezier曲线与lc轨迹段在起始点(即拐出点)相切且曲率连续，同时，bezier曲线与ln轨迹段在终止点(即拐入点)相切且曲率连续。

根据bezier基函数的导函数性质可求取n次bezier曲线始末点的二阶导矢：

求得：当t＝0时，b″(0)＝n(n-1)(p2-2p1+p0)

当t＝1时，b″(1)＝n(n-1)(pn-2pn-1+pn-2)

上述公式表明，二阶导矢只与相邻的3个控制点有关。

为了二阶导矢方向与lc轨迹段和ln轨迹段的二阶导矢方向一致，需要最少五个控制点(或特征点)，所以可选用n次(n不小于4)bezier曲线，四次bezier曲线有公式：

b(t)＝p0(1-t)⁴+4p1t(1-t)³+6p2t²(1-t)²+4p3t³(1-t)+p4t⁴，t∈[0，1]

基于上述研究发现，本发明实施例提供如下技术方案：

一方面，本申请的实施例提供机器人运动轨迹规划方法，至少用于基于n次(n不小于4)bezier曲线实现连续的两cp运动轨迹段之间的平滑转接，所述连续的两cp运动轨迹段包括第一cp运动轨迹段和第二cp运动轨迹段；该方法包括：根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点；所述特征点包括p0、p1、p2、p3，其中，p0为起始点、p3为终止点，p1、p2为中间点；根据所述特征点构建n次(n不小于4)bezier曲线，作为所述第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段，所述平滑过渡轨迹段的起始点为所述第一cp运动轨迹段的拐出点，所述平滑过渡轨迹段的终止点为所述第二cp运动轨迹段的拐入点；其中，所述平滑过渡轨迹段与所述第一cp运动轨迹段在所述起始点上的切矢量方向相同且曲率相同；所述平滑过渡轨迹段与所述第二cp运动轨迹段在所述终止点上的切矢量方向相同且曲率相同。在本实施例中，采用n次(n不小于4)bezier曲线作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段，由于平滑过渡轨迹段在始末点的切矢方向分别与所述第一cp运动轨迹段和第二cp运动轨迹段的切矢方向相同，因此，该平滑过渡轨迹段在其起始点与第一cp运动轨迹段相切，同时在其终止点与第二cp运动轨迹段相切，从而可实现在拐点处(拐出点和拐入点)速度连续，也即可实现连续的cp运动平滑转接，同时，速度不必降为0。此外，该平滑过渡轨迹段在其起始点的曲率与第一cp运动轨迹段在拐出点的曲率相同(起始点与拐出点重合，或也可认为起始点即拐出点)，在其终止点的曲率与第二cp运动轨迹段在拐入点的曲率相同(终止点与拐入点重合，或也可认为终止点即拐入点)，从而可实现在拐点处(拐出点和拐入点)加速度连续，不跳变。

在一个可能的设计中，在所述确定n次(n不小于4)bezier曲线的特征点之前，还包括：规划连续的cp运动轨迹段。

在一个可能的设计中，所述n＝4，所述特征点包括p0、p1、p2、p3和p4，其中，p0为起始点、p4为终止点，p1、p2和p3为第一至第三中间点；所述第一cp运动轨迹段和第二cp运动轨迹段的交点表示为o点；所述根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点包括：确定所述交点o为所述第二中间点p2。

在一个可能的设计中，所述根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点还包括：若所述第一cp运动轨迹段为直线段，选择所述交点o和起始点p0之间的线段上的一点作为所述第一中间点p1；若所述第一cp运动轨迹段为圆弧段，确定第一中间点p1位于起始点p0的切线上，并且，线段p1p0的长度为其中，r1表示所述第一cp运动轨迹段的半径，α1表示所述起始点处的圆心角。

在一个可能的设计中，所述根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点还包括：若所述第二cp运动轨迹段为直线段，选择所述交点o和终止点p4之间的线段上的一点作为所述第三中间点p3；若所述第二cp运动轨迹段为圆弧段，确定所述第三中间点p3位于终止点p4的切线上，并且，线段p2p4的长度为其中，r2表示所述第二cp运动轨迹段的半径，α2表示终止点处的圆心角。

在一个可能的设计中，所述n＝5，所述特征点包括p0、p1、p2、p3、p4和p5，其中，p0为起始点、p5为终止点，p1至p4为第一至第四中间点；所述第一cp运动轨迹段和第二cp运动轨迹段的交点表示为o点。

在一个可能的设计中，所述根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点包括：若所述第一cp运动轨迹段为圆弧段，确定所述第一中间点p1位于所述起始点p0的切线上，并且，线段p1p0的长度为其中，r1表示所述第一cp运动轨迹段的半径，α1表示所述起始点处的圆心角；确定所述第二中间点p2为，以所述起始点p0和交点o为端点的圆弧段上的三等分点。

在一个可能的设计中，所述根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点包括：若所述第一cp运动轨迹段为直线段，选择以所述起始点p0和交点o为端点的直线段上的三等分点分别作为第一中间点p1和第二中间点p2。

在一个可能的设计中，所述根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点还包括：若所述第二cp运动轨迹段为圆弧段，确定所述第三中间点p3位于终止点的切线上，并且，线段p3p5的长度为其中，r2表示所述第二cp运动轨迹段的半径，α2表示所述终止点处的圆心角；确定所述第四中间点p4端点为，以所述终止点和交点o为端点的圆弧段上的三等分点。

在一个可能的设计中，所述根据所述第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线的特征点还包括：若所述第二cp运动轨迹段为直线段，选择以所述终止点和交点o为端点的直线段上的三等分点分别作为所述第三中间点p3和第四中间点p4。

另一方面，本发明实施例提供了一种机器人运动轨迹规划装置，该装置具有实现上述方法实际中机器人运动轨迹规划装置行为的功能。所述功能可以通过硬件实现，也可以通过硬件执行相应的软件实现。所述硬件或软件包括一个或多个与上述功能相对应的模块。

在一个可能的设计中，机器人运动轨迹规划装置的结构包括：处理器和存储器，所述处理器通过运行存储在所述存储器内的软件程序、调用存储在所述存储器内的数据，执行上述方法。

再一方面，本发明实施例提供了一种机器人，其包括上述的机器人运动轨迹规划装置，以及按照上述机器人运动轨迹规划装置规划出的连续的两cp运动轨迹段及平滑过渡轨迹段运行的被控器件。该控制器件例如可以是操作臂。

再一方面，本发明实施例提供了一种计算机存储介质，用于储存为上述机器人运动轨迹规划装置所用的计算机软件指令，其包含用于执行上述方面所设计的程序。

再一方面，本发明实施例提供了一种计算机存储介质，用于储存为上述机器人所用的计算机软件指令，其包含用于执行上述方面所设计的程序。

相较于现有技术，本实施例采用n次(n不小于4)bezier曲线作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段，由于平滑过渡轨迹段在始末点的切矢方向分别与所述第一cp运动轨迹段和第二cp运动轨迹段的切矢方向相同，因此，该平滑过渡轨迹段在其起始点与第一cp运动轨迹段相切，同时在其终止点与第二cp运动轨迹段相切，从而可实现在拐点处(拐出点和拐入点)速度连续，也即可实现连续的cp运动平滑转接，同时，速度不必降为0。此外，该平滑过渡轨迹段在其起始点的曲率与第一cp运动轨迹段在拐出点的曲率相同(起始点与拐出点重合，或也可认为起始点即拐出点)，在其终止点的曲率与第二cp运动轨迹段在拐入点的曲率相同(终止点与拐入点重合，或也可认为终止点即拐入点)，从而可实现在拐点处(拐出点和拐入点)加速度连续，不跳变。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1a-1d为本发明实施例提供的连续的运动轨迹段的示意图；

图2为本发明实施例提供的采用矢量叠加法得到平滑过渡轨迹段示意图；

图3为本发明实施例提供的n次(n不小于4)bezier曲线示意图；

图4-5，6a、7a、8a、9a、10a、11a、12a为本发明实施例提供的机器人运动轨迹规划方法示例性流程图；

图6b、7b、8b、9b、10b、11b、12b为本发明实施例提供的两连续运动轨迹段利用平滑过渡轨迹段过渡示意图；

图6c、7c、9c、10c、11c、12c为本发明实施例提供的曲率图；

图8c为本发明实施例提供的几何原理图；

图13为本发明实施例提供的机器人运动轨迹规划装置结构示意图；

图14为本发明实施例提供的机器人运动轨迹规划装置通用计算机架构示意图。

具体实施方式

本发明可能使用的技术名词、简写或缩写如下：

tcp点：工具坐标中心点；

直线运动：机器人tcp点在笛卡尔空间内沿直线轨迹运动；

圆弧运动：机器人cp点在笛卡尔空间内沿圆弧轨迹运动；

cp：连续运动，笛卡尔空间下的轨迹运动方式，包括直线运动和圆弧运动；

n次bezier曲线：n次bezier曲线公式为：称为bernstein基函数，t＝0对应beizer曲线的起始点，t＝1对应beizer曲线的终止点。

p0-pn这n个特征点或控制点在平面或在三维空间定义了n次bezier曲线，其组成的多边形称为特征多边形或控制多边形。其中，p0为起始点，pn为终止点，p1-pn-1为中间点。例如请参见图3，p0、p1、p2、p3四个点定义了三次bezier曲线，三次bezier曲线起始于p0走向p1，并从p2方向到p3。需要说明的是，一般三次bezier曲线不经过p1、p2，这两个点只提供方向。

本发明实施例提供了机器人运动轨迹规划方法及相关装置，以令连续的cp运动平滑转接，同时，速度不必降为0并且加速度不跳变。本发明实施例所涉及到的机器人运动轨迹规划装置应用于机器人中，其具体可为机器人中的控制器/处理器。

本发明的一个实施例提供一种实现机器人运动轨迹规划的方法，和基于这个方法的机器人运动轨迹规划装置。该装置通过执行上述方法,可实现连续的两cp运动轨迹段(第一cp运动轨迹段和第二cp运动轨迹段)之间的平滑转接，请参见图4，其具体操作包括：

在401部分：根据第一cp运动轨迹段和第二cp运动轨迹段确定n次bezier曲线(n不小于4)的特征点；

以n＝4为例，特征点可包括p0、p1、p2、p3和p4，其中，p0为起始点、p4为终止点，p1、p2、p3为中间点。

而若n＝5，则特征点可包括p0、p1、p2、p3、p4和p5，其中，p0为起始点、p5为终止点，其他点为中间点。

在402部分：根据上述特征点构建上述n次bezier曲线(n不小于4)，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段。

在确定出特征点后，根据特征点构建n次bezier曲线(n不小于4)可采用现有的构建方式，在此不作赘述。

需要注意的是，上述平滑过渡轨迹段的起始点与第一cp运动轨迹段的拐出点相重合(或可认为平滑过渡轨迹段的起始点即为第一cp运动轨迹段的拐出点)，平滑过渡轨迹段的终止点与所述第二cp运动轨迹段的拐入点相重合(或可认为平滑过渡轨迹段的终止点为第二cp运动轨迹段的拐入点)。这样可保证路径连续。

此外，平滑过渡轨迹段在起始点p0上的切矢量方向、与第一cp运动轨迹段在拐出点上的切矢量方向相同，这样，平滑过渡轨迹段在其起始点与第一cp运动轨迹段相切，并且平滑过渡轨迹段在起始点p0上的曲率与第一cp运动轨迹段在拐出点上的曲率相同，这样，可保证在拐出点的速度及加速度均连续。

同理，平滑过渡轨迹段在其终止点上的切矢量方向和第二cp运动轨迹段在拐入点上的切矢量方向的方向相同。这样，平滑过渡轨迹段在其终止点与第二cp运动轨迹段相切。并且平滑过渡轨迹段在终止点上的曲率与第二cp运动轨迹段在拐入点上的曲率相同。这样，可保证在拐入点的速度及加速度均连续。

在一个示例中，拐出点和拐入点可根据用户的配置来确定。具体的，用户可指示拐出点与交点之间的距离(交点是第一cp运动轨迹段和第二cp运动轨迹段的交点)，拐入点与交点之间的距离与拐出点与交点之间的距离相等，这样，就可确定出拐入点和拐出点。

需要说明的是，对于连续的多段cp运动轨迹段，可针对每两个连续的cp运动轨迹段执行上述401部分和402部分。例如，对于连续的三段cp运动轨迹段1-3，可先规划出cp运动轨迹段1-2之间的平滑过渡轨迹段，再规划出cp运动轨迹段2-3之间的平滑过渡轨迹段。

下面结合附图5，对本发明的实施例提供的方案进行说明。

图5为本发明实施例提供的机器人运动轨迹规划方法的另一示例性流程图。

在500部分：规划连续的cp运动轨迹段。

连续的cp运动轨迹段也是需要预先规划的。500部分是后续501部分和502部分的基础。

在一个示例中，可一次性规划出所需的所有cp运动轨迹段。例如，一共需要连续的4个cp运动轨迹段以完成机器人的某个操作，则可一次性规划出4个连续的cp运动轨迹段。

在另一个示例中，也可分批次规划出所需的所有cp运动轨迹段。仍以需要连续的cp运动轨迹段1-4以完成机器人的某个操作为例，可先规划cp运动轨迹段1-2，然后，执行后续的501部分和502部分，以得到cp运动轨迹段1-2之间的平滑过渡轨迹段。之后，再规划出cp运动轨迹段3，再执行后续的501部分和502部分，以得到cp运动轨迹段2-3之间的平滑过渡轨迹段，以此类推。

在又一个示例中，也可采用前瞻x段的方式进行规划，x一般取3。以需要连续的cp运动轨迹段1-8以完成机器人的某个操作为例，可先规划cp运动轨迹段1-2，然后，执行后续的501部分和502部分，以得到cp运动轨迹段1-2之间的平滑过渡轨迹段。之后，再规划出cp运动轨迹段3，再执行后续的501部分和502部分，以得到cp运动轨迹段2-3之间的平滑过渡轨迹段。然后，等cp运动轨迹段1运行完后，再规划出cp运动轨迹段4，然后，执行后续的501部分和502部分，以得到cp运动轨迹段3-4之间的平滑过渡轨迹段。再等cp运动轨迹段2运行完后，规划出cp运动轨迹段5，然后，执行后续的501部分和502部分，以得到cp运动轨迹段4-5之间的平滑过渡轨迹段，以此类推。

在501部分：根据第一cp运动轨迹段和第二cp运动轨迹段确定n次(n不小于4)bezier曲线的特征点。

501部分与401部分相类似，在此不作赘述。

在502部分：根据上述特征点构建n次(n不小于4)bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段。

502部分与402部分相类似，在此不作赘述。

前已述及，n＝4时，有五个特征点，n＝5时，有六个特征点。在n＝4的情况下，中间点的确定，与n＝5时，中间点的确定并不相同。并且，在实际应用中，第一cp运动轨迹段和第二cp运动轨迹段可分别为：直线段与直线段、直线段与圆弧段、圆弧段与直线段、圆弧段与圆弧段四种情况。下面将分情况，对本发明的实施例做进一步说明。

图6a为n＝4且第一cp运动轨迹段和第二cp运动轨迹段分别为直线段时，本发明实施例提供的又一种机器人运动轨迹规划方法示例性流程图。图6b显示的是第一cp运动轨迹段(lc)和第二cp运动轨迹段(ln)以及平滑过渡轨迹段。lc轨迹段和ln轨迹段相交于交点o。

在601部分：确定lc轨迹段的拐出点为平滑过渡轨迹段的起始点p0，确定ln轨迹段的拐入点为平滑过渡轨迹段的终止点p4。

在602部分：确定交点o为(第二)中间点p2。

前已证明，n次(4次)bezier曲线在起始点的曲率与相邻的3个特征点(p0、p1、p2)相关，在终止点处的曲率也与相邻的3个特征点(p2、p3和p4)有关，则p2点同时需要与p0、p1、p3和p4共面，因此，选择交点o作为p2。

在603部分：选择线段p0o(也即交点o和起始点p0之间的线段)上的一点作为(第一)中间点p1；线段p0o是交点o和起始点p0之间的线段。

在一个示例中，中间点p1到交点o的距离，等于其到起始点p0的距离。因此，在求解p1坐标时，可使p0p1＝p1o，进而获得p1点坐标。p0p1＝p1o可以使特征点分布均匀，特征点均匀分布则可以使beizer曲线的参数均匀，轨迹曲率变化平稳。

在604部分：选择线段op4上的一点作为(第三)中间点p3。

线段op4是交点o和终止点p4之间的线段。

在一个示例中，中间点p3到交点o的距离，等于中间点p3到终止点p4的距离。因此，在求解p3坐标时，可使p3p4＝p3o，从而获得p3点坐标。

在605部分：根据上述特征点构建四次bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段(图6b虚线部分即为平滑过渡轨迹段)。

605部分与前述的402和502部分相类似，在此不作赘述。

请参见图6c，通过图6a所示的方法，在始末点曲率皆为0，进而实现在拐点处加速度连续。

图7a为n＝4且第一cp运动轨迹段为直线段、第二cp运动轨迹段为圆弧段时，本发明实施例提供的又一种机器人运动轨迹规划方法示例性流程图。图7b显示的是第一cp运动轨迹段(lc)和第二cp运动轨迹段(ln)以及平滑过渡轨迹段。

在701部分：确定lc轨迹段的拐出点为平滑过渡轨迹段的起始点p0，确定ln轨迹段的拐入点为平滑过渡轨迹段的终止点p4。

701部分与601部分相类似，在此不作赘述。

在702部分：确定交点o为(第二)中间点p2。

702部分与602部分相类似，在此不作赘述。

在703部分：选择线段p0o(也即交点o和起始点p0之间的线段)上的一点作为中间点p1。

703部分与603部分相类似，在此不作赘述。

在704部分：确定中间点p3位于终止点p4的切线上，并且，线段p3p4的长度为(可参见图7b)。

其中，r2表示ln轨迹段的半径，α2表示终止点p4处的圆心角。

公式的推导过程请参见图8a所示实施例。

在705部分：根据上述特征点构建四次bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段(图7b虚线部分即为平滑过渡轨迹段)。

705部分与前述的402、502和605部分相类似，在此不作赘述。

请参见图7c，通过图7a所示的方法，在起始点曲率为0，在终止点曲率为半径(50)的倒数0.02，进而实现在拐点处加速度连续。

图8a为n＝4且第一cp运动轨迹段为圆弧段、第二cp运动轨迹段为直线段时，本发明实施例提供的又一种机器人运动轨迹规划方法示例性流程图。图8b显示的是第一cp运动轨迹段(lc)和第二cp运动轨迹段(ln)以及平滑过渡轨迹段。

在801部分，确定lc轨迹段的拐出点为平滑过渡轨迹段的起始点p0，确定ln轨迹段的拐入点为平滑过渡轨迹段的终止点p4。

801部分与601、701部分相类似，在此不作赘述。

在802部分：确定交点o为(第二)中间点p2。

802部分与602、702部分相类似，在此不作赘述。

在803部分：确定中间点p1位于起始点p0的切线上，并且，线段p1p0的长度为

其中，r1表示lc轨迹段的半径，α1表示起始点的圆心角。

下面讲述公式和的由来。

请参见图8c，有两条连续cp运动轨迹段，前一段为圆弧段，后一段为直线段，点a为交点，点c为拐出点，圆弧段半径为r，拐出点处的圆心角为α。根。

b点为p1点或p3点，b点位于圆弧在点c处的切线上，并满足n次bezier曲线在起始点处(即c点)曲率等于圆弧曲率。已知圆弧段的曲率为半径的倒数，则列等式：

其中：

n为bezier曲线次数；

∠θ为向量ab与向量bc的夹角。

建立直角三角形abd，得|ab|·sinθ＝ad＝r-r·cosα。

化简等式两边得到

在804部分：选择线段op4上的一点作为中间点p3。线段op4是交点o和终止点之间的线段。

804部分与604部分相类似，在此不作赘述。

在805部分：根据上述特征点构建四次bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段(图8b虚线部分即为平滑过渡轨迹段)。

805部分与前述的402、502、605和705部分相类似，在此不作赘述。

图9a为n＝4且第一cp运动轨迹段和第二cp运动轨迹段均为圆弧段时，本发明实施例提供的又一种机器人运动轨迹规划方法示例性流程图。图9b显示的是第一cp运动轨迹段(lc)和第二cp运动轨迹段(ln)以及平滑过渡轨迹段。

在901部分，确定lc轨迹段的拐出点为平滑过渡轨迹段的起始点p0，确定ln轨迹段的拐入点为平滑过渡轨迹段的终止点p4。

901部分与601、701、801部分相类似，在此不作赘述。

在902部分：确定交点o为(第二)中间点p2。

902部分与602、702和802部分相类似，在此不作赘述。

在903部分：确定中间点p1位于起始点p0的切线上，并且，线段p1p0的长度为

903部分与803部分相类似，在此不作赘述。

在904部分：确定中间点p3位于终止点p4的切线上，并且，线段p2p4的长度为(可参见图9b)。

其中，r2表示ln轨迹段的半径，α2表示终止点p4处的圆心角。

904部分与704部分相类似，在此不作赘述。

在905部分：根据上述特征点构建四次bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段(图9b虚线部分即为平滑过渡轨迹段)。

905部分与前述的402、502、605、705、805部分相类似，在此不作赘述。

请参见图9c，通过图9a所示的方法，在始末点曲率皆为半径(50)的倒数0.02，进而实现在拐点处加速度连续。

考虑到四次bezier曲线的五个控制点中，p2点选择两轨迹交点，自由度不够。为了使平滑轨迹可调，增加更多的选择，可以把四次bezier曲线升阶为五次bezier曲线。

下面将介绍基于五次bezier曲线的运动轨迹规划方法。五次bezier曲线的公式为：

b(t)＝p0(1-t)⁵+5p1t(1-t)⁴+10p2t2⁽1-t)³+10p3t³(1-t)2...+5p4t⁴(1-t)+p5t⁵，t∈[0，1]

图10a为n＝5且第一cp运动轨迹段和第二cp运动轨迹段分别为圆弧段时，本发明实施例提供的又一种机器人运动轨迹规划方法示例性流程图。图10b显示的是第一cp运动轨迹段(lc)和第二cp运动轨迹段(ln)以及平滑过渡轨迹段，lc轨迹段和ln轨迹段相交于交点o。特征点包括p0、p1、p2、p3、p4和p5，其中，p0为起始点、p5为终止点，p1至p4为第一至第四中间点。

在1001部分：确定lc轨迹段的拐出点为平滑过渡轨迹段的起始点p0，确定ln轨迹段的拐入点为平滑过渡轨迹段的终止点p5。

1001部分与601、701、801、901部分相类似，在此不作赘述。

在1002部分：确定(第一)中间点p1位于起始点p0的切线上，并且，线段p1p0的长度为

其中，r1表示lc轨迹段的半径，α1表示起始点的圆心角。

1002部分与803部分相类似，在此不作赘述。

在1003部分：确定(第二)中间点p2为，以起始点p0和交点o为端点的圆弧段(该圆弧段属于lc轨迹段的一部分)上的三等分点。

由于三等分点有两个，在一个示例中，可选取靠近交点的三等分点作为中间点p2。

在1004部分：确定第四中间点p4位于终止点p5的切线上，并且，线段p4p5的长度为

其中，r2表示ln轨迹段的半径，α2表示终止点处的圆心角。

1004部分与904部分相类似，在此不作赘述。

在1005部分：确定(第三)中间点p3端点为，以终止点p5和交点o为端点的圆弧段(该圆弧段属于ln轨迹段的一部分)上的三等分点。

由于三等分点有两个，在一个示例中，可选取靠近交点的三等分点作为中间点p3。

在1006部分：根据上述特征点构建五次bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段(图10b虚线部分即为平滑过渡轨迹段)。

请参见图10c，通过图10a所示的方法，在始末点曲率皆为半径(50)的倒数0.02，进而实现在拐点处加速度连续。

图11a为n＝5且第一cp运动轨迹段为直线段，第二cp运动轨迹段为圆弧段时，本发明实施例提供的又一种机器人运动轨迹规划方法示例性流程图。图11b显示的是第一cp运动轨迹段(lc)和第二cp运动轨迹段(ln)以及平滑过渡轨迹段，lc轨迹段和ln轨迹段相交于交点o。

在1101部分：确定lc轨迹段的拐出点为平滑过渡轨迹段的起始点p0，确定ln轨迹段的拐入点为平滑过渡轨迹段的终止点p5。

1101部分与601、701、801、901和1001部分相类似，在此不作赘述。

在1102部分：选择以起始点p0和交点o为端点的直线段上的三等分点分别作为第一中间点p1和第二中间点p2。

由于直线曲率为0，所以p0、p1、p2共线。

由于三等分点有两个，在一个示例中，可选取靠近交点的三等分点作为第二中间点p2，选择靠近起始点p0的三等分点作为第一中间点p1。

在1103部分：确定第四中间点p4位于终止点p5的切线上，并且，线段p4p5的长度为

其中，r2表示ln轨迹段的半径，α2表示终止点处的圆心角。

1103部分与904、1004部分相类似，在此不作赘述。

在1104部分：确定第三中间点p3端点为，以终止点和交点o为端点的圆弧段上的三等分点。

由于三等分点有两个，在一个示例中，可选取靠近交点的三等分点作为中间点p3。

1104部分与1005部分相类似，在此不作赘述。

在1105部分：根据上述特征点构建五次bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段(图11b虚线部分即为平滑过渡轨迹段)。

请参见图11c，通过图11a所示的方法，在起始点曲率为0，在终止点曲率为半径(50)的倒数0.02，进而实现在拐点处加速度连续。

图12a为n＝5且第一cp运动轨迹段为直线段，第二cp运动轨迹段为圆弧段时，本发明实施例提供的又一种机器人运动轨迹规划方法示例性流程图。

图12b显示的是第一cp运动轨迹段(lc)和第二cp运动轨迹段(ln)以及平滑过渡轨迹段，lc轨迹段和ln轨迹段相交于交点o。

在1201部分：确定lc轨迹段的拐出点为平滑过渡轨迹段的起始点p0，确定ln轨迹段的拐入点为平滑过渡轨迹段的终止点p5。

1201部分与601、701、801、901、1001和1101部分相类似，在此不作赘述。

在1202部分：选择以起始点p0和交点o为端点的直线段上的三等分点分别作为(第一)中间点p1和第二中间点p2。

1202部分与1102部分相类似，在此不作赘述。

在1203部分：选择以终止点p5和交点o为端点的直线段上的三等分点分别作为第三中间点p3和第四中间点p4。

由于直线曲率为0，所以p3、p4、p5共线。

由于三等分点有两个，在一个示例中，可选取靠近交点的三等分点作为第三中间点p3，选择靠近终止点p5的三等分点作为第四中间点p4。

在1204部分：根据上述特征点构建五次bezier曲线，作为第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段(图12b虚线部分即为平滑过渡轨迹段)。

请参见图12c，通过图12a所示的方法，在始末点曲率皆为0，进而实现在拐点处加速度连续。

至于n＝5且第一cp运动轨迹段为圆弧段和第二cp运动轨迹段为直线段时，平滑过渡轨迹段的起始点p0、终止点p5的确定方式可参见601、701、801、901、1001和1201部分，第一中间点p1和第二中间点p2的确定方式可参见1002部分和1003部分，第三中间点p3和第四中间点p4的确定方式可参见1203部分，在此不作赘述。

图13示出了上述实施例中所涉及的机器人运动轨迹规划装置的一种可能的结构示意图，包括：

特征点确定单元131，用于根据第一cp运动轨迹段和第二cp运动轨迹段确定n次(n不小于4)bezier曲线的特征点。

平滑过渡轨迹构建单元132，用于根据所述特征点构建n次(n不小于4)bezier曲线，作为所述第一cp运动轨迹段和第二cp运动轨迹段之间的平滑过渡轨迹段，所述平滑过渡轨迹段的起始点为所述第一cp运动轨迹段的拐出点，所述平滑过渡轨迹段的终止点为所述第二cp运动轨迹段的拐入点；其中，所述平滑过渡轨迹段与所述第一cp运动轨迹段在所述起始点上的切矢量方向相同且曲率相同；所述平滑过渡轨迹段与所述第二cp运动轨迹段在所述终止点上的切矢量方向相同且曲率相同。

此外，还可包括规划单元133，用于在所述特征点确定单元确定n次(n不小于4)bezier曲线的特征点之前，规划连续的cp运动轨迹段。

其中，特征点确定单元131可用于执行图4所示的401部分、图5所示的501部分、图6a所示的601-604部分、图7a所示的701-704部分、图8a所示的801-804部分，图9a所示的901-904部分，图10a所示的1001-1005部分，图11a所示的1101-1104部分，以及，图12a所示的1201-1203部分。

平滑过渡轨迹构建单元132可用于执行图4所示的402部分、图5所示的502部分、图6a所示的605部分、图7a所示的705部分、图8a所示的805部分，图9a所示的905部分，图10a所示的1006部分，图11a所示的1105部分，以及，图12a所示的1204部分。

规划单元133可用于执行图5所示的500部分。

图14示出了上述实施例中所涉及的机器人的一种可能的结构示意图，包括：

总线、控制器/处理器1、存储器2、通信接口3、输入设备4、输出设备5和被控器件6。处理器1、存储器2、通信接口3、输入设备4、输出设备5和被控器件6可通过总线相互连接。其中：

总线可包括一通路，在计算机系统各个部件之间传送信息。

控制器/处理器1(机器人运动轨迹规划装置)可以是通用处理器，例如通用中央处理器(cpu)、网络处理器(networkprocessor，简称np)、微处理器等，也可以是特定应用集成电路(application-specificintegratedcircuit，asic)，或一个或多个用于控制本发明方案程序执行的集成电路。还可以是数字信号处理器(dsp)、专用集成电路(asic)、现成可编程门阵列(fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。控制器/处理器1也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，dsp和微处理器的组合等等。

存储器2中保存有执行本发明技术方案的程序，还可以保存有操作系统和其他应用程序。具体地，程序可以包括程序代码，程序代码包括计算机操作指令。更具体的，存储器2可以是只读存储器(read-onlymemory，rom)、可存储静态信息和指令的其他类型的静态存储设备、随机存取存储器(randomaccessmemory，ram)、可存储信息和指令的其他类型的动态存储设备、磁盘存储器等等。

输入设备4可包括接收用户输入的数据和信息的装置，例如键盘、鼠标、摄像头、扫描仪、光笔、语音输入装置、触摸屏等。

输出设备5可包括允许输出信息给用户的装置，例如显示屏、打印机、扬声器等。

通信接口3可包括使用任何收发器一类的装置，以便与其他设备或通信网络通信，如以太网，无线接入网(ran)，无线局域网(wlan)等。

控制器/处理器1可用于执行图4、5、6a、7a、8a、9a、10a、11a、12a中涉及机器人运动轨迹规划装置的处理过程和/或用于本申请所描述的技术的其他过程。控制器/处理器也可用于实现前述特征点确定单元131、平滑过渡轨迹构建单元132和规划单元133的功能。

被控器件6可用于按照控制器/处理器1规划出的连续的两cp运动轨迹段及平滑过渡轨迹段运行。

可以理解的是，图14仅仅示出了机器人的简化设计。在实际应用中，机器人可以包含任意数量的发射器，接收器，处理器，控制器，存储器，通信接口等，而所有可以实现本发明的机器人都在本发明的保护范围之内。

上述所有实施例所公开的方法和装置可运用于机械臂控制中，实现机械臂在笛卡尔空间下两条连续运行轨迹中的平滑转接。当然也可以拓展到任何需要规划路径的设备中，如小车、飞行器等等。

结合本发明公开内容所描述的方法或者算法的步骤可以硬件的方式来实现，也可以是由处理器执行软件指令的方式来实现。软件指令可以由相应的软件模块组成，软件模块可以被存放于ram存储器、闪存、rom存储器、eprom存储器、eeprom存储器、寄存器、硬盘、移动硬盘、cd-rom或者本领域熟知的任何其它形式的存储介质中。一种示例性的存储介质耦合至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于asic中。

本领域技术人员应该可以意识到，在上述一个或多个示例中，本发明所描述的功能可以用硬件、软件、固件或它们的任意组合来实现。当使用软件实现时，可以将这些功能存储在计算机可读介质中或者作为计算机可读介质上的一个或多个指令或代码进行传输。计算机可读介质包括计算机存储介质和通信介质，其中通信介质包括便于从一个地方向另一个地方传送计算机程序的任何介质。存储介质可以是通用或专用计算机能够存取的任何可用介质。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的技术方案的基础之上，所做的任何修改、等同替换、改进等，均应包括在本发明的保护范围之内。