机器人运动轨迹规划方法及相关装置制造方法及图纸

本方案涉及运动控制领域，尤其涉及机器人运动轨迹规划技术。在一种机器人运动轨迹规划方法中，根据第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点，n不小于4(401)；根据上述特征点构建n次Bezier曲线，作为第一CP运动轨迹段和第二CP运动轨迹段之间的平滑过渡轨迹段(402)。由于采用n次Bezier曲线作为两CP运动轨迹段之间的平滑过渡轨迹段，该平滑过渡轨迹段在其起始点与第一CP运动轨迹段相切且曲率相同，同时在其终止点与第二CP运动轨迹段相切且曲率相同，从而可实现连续CP运动的平滑转接，同时，速度不必降为0，并且，加速度不跳变。

Robot trajectory planning and related devices

This scheme involves the field of motion control, especially the robot trajectory planning technology. In a robot trajectory planning method, the characteristic points of the n Bezier curve are determined according to the first CP motion trajectory and the second CP motion trajectory, and N is not less than 4 (401). According to the above feature points, the N sub Bezier curve is constructed as the smooth transition trajectory (402) between the first CP motion trajectory and the second CP motion trajectory. Since the n Bezier curve is used as a smooth transition path between the two CP trajectories, the smooth transition trajectory is tangent to the first CP trajectory segment and the curvature is the same, at the same time, the smooth transition of the continuous CP motion can be realized by cutting the same angle between the termination point and the second CP motion track. The speed does not have to drop to 0, and the acceleration does not jump.

【技术实现步骤摘要】
【国外来华专利技术】机器人运动轨迹规划方法及相关装置
本专利技术涉及运动控制领域，更具体地说，涉及机器人运动轨迹规划技术。
技术介绍
在对机器人运动控制中，被控部件(例如操作臂)的轨迹运动方式可分为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)4+4P1t(1-t)3+6P2t2(1-t)2+4P3t3(1-t)+P4t4，t∈[0，1]基于上述研究发现，本专利技术实施例提供如下技术方案：一方面，本申请的实施例提供机器人运动轨迹规划方法，至少用于基于n次(n不小于4)Bezier曲线实现连续的两CP运动轨迹段之间的平滑转接，所述连续的两CP运动轨迹段包括第一CP运动轨迹段和第二CP运动轨迹段；该方法包括：根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点；所述特征本文档来自技高网...

【技术保护点】
一种机器人运动轨迹规划方法，其特征在于，至少用于实现连续的两CP运动轨迹段之间的平滑转接，所述连续的两CP运动轨迹段包括第一CP运动轨迹段和第二CP运动轨迹段；所述方法包括：根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点，n为不小于4的正整数；根据所述特征点构建n次Bezier曲线，作为所述第一CP运动轨迹段和第二CP运动轨迹段之间的平滑过渡轨迹段，所述平滑过渡轨迹段的起始点为所述第一CP运动轨迹段的拐出点，所述平滑过渡轨迹段的终止点为所述第二CP运动轨迹段的拐入点；其中，所述平滑过渡轨迹段与所述第一CP运动轨迹段在所述起始点上的切矢量方向相同且曲率相同；所述平滑过渡轨迹段与所述第二CP运动轨迹段在所述终止点上的切矢量方向相同且曲率相同。

【技术特征摘要】
【国外来华专利技术】1.一种机器人运动轨迹规划方法，其特征在于，至少用于实现连续的两CP运动轨迹段之间的平滑转接，所述连续的两CP运动轨迹段包括第一CP运动轨迹段和第二CP运动轨迹段；所述方法包括：根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点，n为不小于4的正整数；根据所述特征点构建n次Bezier曲线，作为所述第一CP运动轨迹段和第二CP运动轨迹段之间的平滑过渡轨迹段，所述平滑过渡轨迹段的起始点为所述第一CP运动轨迹段的拐出点，所述平滑过渡轨迹段的终止点为所述第二CP运动轨迹段的拐入点；其中，所述平滑过渡轨迹段与所述第一CP运动轨迹段在所述起始点上的切矢量方向相同且曲率相同；所述平滑过渡轨迹段与所述第二CP运动轨迹段在所述终止点上的切矢量方向相同且曲率相同。2.如权利要求1所述的方法，其特征在于，在所述确定n次Bezier曲线的特征点之前，还包括：规划连续的CP运动轨迹段。3.如权利要求1或2所述方法，其特征在于，所述n＝4，所述特征点包括起始点P0、终止点P4，以及，第一至第三中间点P1、P2、P3；所述第一CP运动轨迹段和第二CP运动轨迹段的交点表示为O点；所述根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点包括：确定所述交点O为所述第二中间点P2。4.如权利要求3所述的方法，其特征在于，所述根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点还包括：若所述第一CP运动轨迹段为直线段，选择所述交点O和起始点P0之间的线段上的一点作为所述第一中间点P1；若所述第一CP运动轨迹段为圆弧段，确定第一中间点P1位于起始点P0的切线上，并且，线段P1P0的长度为其中，r1表示所述第一CP运动轨迹段的半径，α1表示所述起始点处的圆心角。5.如权利要求3或4所述的方法，其特征在于，所述根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点还包括：若所述第二CP运动轨迹段为直线段，选择所述交点O和终止点P4之间的线段上的一点作为所述第三中间点P3；若所述第二CP运动轨迹段为圆弧段，确定所述第三中间点P3位于终止点P4的切线上，并且，线段P3P4的长度为其中，r2表示所述第二CP运动轨迹段的半径，α2表示终止点处的圆心角。6.如权利要求1或2所述方法，其特征在于，所述n＝5，所述特征点包括起始点P0、终止点P5以及第一至第四中间点P1、P2、P3、P4；所述第一CP运动轨迹段和第二CP运动轨迹段的交点表示为O点。7.如权利要求6所述方法，其特征在于，所述根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点包括：若所述第一CP运动轨迹段为圆弧段，确定所述第一中间点P1位于所述起始点P0的切线上，并且，线段P1P0的长度为其中，r1表示所述第一CP运动轨迹段的半径，α1表示所述起始点处的圆心角；确定所述第二中间点P2为，以所述起始点P0和交点O为端点的圆弧段上的三等分点。8.如权利要求6所述方法，其特征在于，所述根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点包括：若所述第一CP运动轨迹段为直线段，选择以所述起始点P0和交点O为端点的直线段上的三等分点分别作为第一中间点P1和第二中间点P2。9.如权利要求7或8所述方法，其特征在于，所述根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点还包括：若所述第二CP运动轨迹段为圆弧段，确定所述第三中间点P3为，以所述终止点和交点O为端点的圆弧段上的三等分点；确定所述第四中间点P4位于终止点P5的切线上，并且，线段P4P5的长度为其中，r2表示所述第二CP运动轨迹段的半径，α2表示所述终止点处的圆心角。10.如权利要求7或8所述方法，其特征在于，所述根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点还包括：若所述第二CP运动轨迹段为直线段，选择以所述终止点和交点O为端点的直线段上的三等分点分别作为所述第三中间点P3和第四中间点P4。11.一种机器人运动轨迹规划装置，其特征在于，至少用于实现连续的两CP运动轨迹段之间的平滑转接，所述连续的两CP运动轨迹段包括第一CP运动轨迹段和第二CP运动轨迹段；所述装置包括：特征点确定单元，用于根据所述第一CP运动轨迹段和第二CP运动轨迹段确定n次Bezier曲线的特征点，n为不小于4的正整数；平滑过渡轨迹构建单元，用于根据所述特征点构建n次Bezier曲线，作为所述第一CP运动轨迹段和第二CP运动轨迹段之间的平滑过渡轨迹段，所述平滑过渡轨迹段的起始点为所述第一CP运动轨迹段的拐出点，所述平滑过渡轨迹段的终止点为所述第二CP运动轨迹段的拐入点；其中，所述平滑过渡轨迹段与所述第一CP运动轨迹段在所述起始点上的切矢量方向相同且曲率相同；所述平滑过渡轨迹段与所述第二CP运动轨迹段在所述终止点上的切矢量方向相同且曲率相同。12.如权利要求11所述的装置，其特征在于，还包括：规划单元，用于在所述特征点确定单元确定n次Bezier曲线的特征点之前，规划连续的CP运动轨迹段。13.如权利要求11或12所述装置，其特征在于，所述n＝4，所述特征点包括起始点P0、终止点P...

【专利技术属性】
技术研发人员：张志明，
申请(专利权)人：深圳配天智能技术研究院有限公司，
类型：发明
国别省市：广东,44