CN114055459B - 轨迹规划的方法、装置、电子设备及存储介质 - Google Patents

Abstract

本公开提供了一种轨迹规划的方法、装置、电子设备及存储介质，所述方法包括：获取待规划轨迹所要经过的路径点；基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数；基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数与二阶导数，规划所述相邻的两个路径点之间的贝塞尔曲线；依次连接所述相邻的两个路径点之间的贝塞尔曲线，得到规划完成的轨迹。本公开实施例能够提高所规划轨迹的平滑程度。

Description

轨迹规划的方法、装置、电子设备及存储介质

技术领域

本公开涉及机器人领域，具体涉及一种轨迹规划的方法、装置、电子设备及存储介质。

背景技术

在机器人的各种应用中，常常会涉及到平面运动上的轨迹规划。现有技术中，多孤立地针对从起点到终点的这一条单一轨迹进行轨迹规划。通过这种方法所规划出的轨迹缺乏对全局的考虑，当待规划轨迹是需要机器人连贯地经过多个单一轨迹拼接而成的拼接轨迹时，现有技术所规划的轨迹在拼接处会出现较为明显的突兀，轨迹的整体不够平滑。

发明内容

本公开的一个目的在于提出一种轨迹规划的方法、装置、电子设备及存储介质，能够提高所规划轨迹的平滑程度。

根据本公开实施例的一方面，公开了一种轨迹规划的方法，所述方法包括：

获取待规划轨迹所要经过的路径点；

基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数；

基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数与二阶导数，规划所述相邻的两个路径点之间的贝塞尔曲线；

依次连接所述相邻的两个路径点之间的贝塞尔曲线，得到规划完成的轨迹。

根据本公开实施例的一方面，公开了一种轨迹规划的装置，所述装置包括：

获取模块，配置为获取待规划轨迹所要经过的路径点；

第一确定模块，配置为基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

第二确定模块，配置为基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数；

规划模块，配置为基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数与二阶导数，规划所述相邻的两个路径点之间的贝塞尔曲线；

连接模块，配置为依次连接所述相邻的两个路径点之间的贝塞尔曲线，得到规划完成的轨迹。

在本公开的一示例性实施例中，所述装置配置为：

基于所述线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述路径点包括起点、终点以及中间点，所述装置配置为：

获取所述起点与所述起点的相邻路径点之间的第一线段，以及所述终点与所述终点的相邻路径点之间的第二线段；

获取各个中间点与所述中间点的两侧相邻路径点之间的线段，得到所述中间点所连接的第三线段以及第四线段；

基于所述第一线段，确定所述起点在所述待规划轨迹上的一阶导数；

基于所述第二线段，确定所述终点在所述待规划轨迹上的一阶导数；

基于所述第三线段以及所述第四线段，确定所述中间点在所述待规划轨迹上的一阶导数。在本公开的一示例性实施例中，所述装置配置为：

在本公开的一示例性实施例中，所述装置配置为：

获取运动对象在所述起点的运动朝向；

基于所述运动朝向以及所述第一线段的长度，确定所述起点在所述待规划轨迹上的一阶导数。

在本公开的一示例性实施例中，所述装置配置为：

将所述第三线段与所述第四线段所构成角的角平分线的垂直方向确定为所述中间点在所述待规划轨迹上的一阶导数的方向；

基于所述第三线段的长度以及所述第四线段的长度，确定所述中间点在所述待规划轨迹上的一阶导数的大小；

根据所述中间点在所述待规划轨迹上的一阶导数的方向以及所述中间点在所述待规划轨迹上的一阶导数的大小，确定所述中间点在所述待规划轨迹上的一阶导数。

在本公开的一示例性实施例中，所述装置配置为：

基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数，模拟所述相邻的两个路径点之间的三阶贝塞尔曲线；

基于所述线段以及所述路径点分别在所述三阶贝塞尔曲线上的二阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述路径点包括起点、终点以及中间点，所述装置配置为：

将所述起点在所述三阶贝塞尔曲线上的二阶导数确定为所述起点在所述待规划轨迹上的二阶导数；

将所述终点在所述三阶贝塞尔曲线上的二阶导数确定为所述终点在所述待规划轨迹上的二阶导数；

基于所述线段以及各个中间点在所连接的两个三阶贝塞尔曲线上的二阶导数，确定所述中间点在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述装置配置为：

获取所述各个中间点与所述中间点的两侧相邻路径点之间的线段，得到所述中间点所连接的第三线段以及第四线段；

基于所述第三线段的长度以及所述第四线段的长度，分别为所述在所连接的两个三阶贝塞尔曲线上的二阶导数分配对应的权重；

基于所述权重对所述在所连接的两个三阶贝塞尔曲线上的二阶导数进行加权，得到所述中间点在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述装置配置为：

获取运动对象的运动约束参数；

基于对所述规划完成的轨迹的离散化处理，确定所述规划完成的轨迹上的各离散点；

基于所述运动约束参数规划所述运动对象分别在所述各离散点上的运动速度。

根据本公开实施例的一方面，公开了一种轨迹规划的电子设备，包括：存储器，存储有计算机可读指令；处理器，读取存储器存储的计算机可读指令，以执行以上实现方式中的任一个所述的方法。

根据本公开实施例的一方面，公开了一种计算机可读存储介质，其上存储有计算机可读指令，当所述计算机可读指令被计算机的处理器执行时，使计算机执行以上实现方式中的任一个所述的方法。

根据本公开实施例的一个方面，提供了一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中。计算机设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行上述各种可选实现方式中提供的方法。

本公开实施例中，基于相邻的两个路径点之间的线段确定路径点分别在待规划轨迹上的一阶导数与二阶导数，进而基于该一阶导数与二阶导数规划相邻的两个路径点之间的贝塞尔曲线，进而依次连接该贝塞尔曲线得到规划完成的轨迹。通过这种方法，使得即使待规划轨迹是由多条轨迹拼接而成的，也能够得到全局平滑的规划完成的轨迹，提高了所规划轨迹的平滑程度。

本公开的其他特性和优点将通过下面的详细描述变得显然，或部分地通过本公开的实践而习得。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性的，并不能限制本公开。

附图说明

通过参考附图详细描述其示例实施例，本公开的上述和其它目标、特征及优点将变得更加显而易见。

图1示出了根据本公开一个实施例的轨迹规划的方法的流程图。

图2示出了根据本公开一个实施例的以直线将路径点依次连接后所得到的线段。

图3示出了根据本公开一个实施例的确定图2中D点的一阶导数的方向的示意图。

图4示出了根据本公开一个实施例的局部避障的示意图。

图5示出了根据本公开一个实施例的路线优化的示意图。

图6示出了根据本公开一个实施例的轨迹规划的装置的框图。

图7示出了根据本公开一个实施例的轨迹规划的电子设备的硬件图。

具体实施方式

现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些示例实施方式使得本公开的描述将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。附图仅为本公开的示意性图解，并非一定是按比例绘制。图中相同的附图标记表示相同或类似的部分，因而将省略对它们的重复描述。

此外，所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多示例实施方式中。在下面的描述中，提供许多具体细节从而给出对本公开的示例实施方式的充分理解。然而，本领域技术人员将意识到，可以实践本公开的技术方案而省略所述特定细节中的一个或更多，或者可以采用其它的方法、组元、步骤等。在其它情况下，不详细示出或描述公知结构、方法、实现或者操作以避免喧宾夺主而使得本公开的各方面变得模糊。

附图中所示的一些方框图是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。

本公开实施例提出了一种轨迹规划的方法、装置、电子设备及存储介质，可以应用于机器人运动的轨迹规划、车辆自动驾驶的轨迹规划或者其他运动相关的轨迹规划中。

图1示出了本公开提供了一种轨迹规划的方法的流程图，如图所示，该方法包括：

步骤S110、获取待规划轨迹所要经过的路径点；

步骤S120、基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

步骤S130、基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数；

步骤S140、基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数与二阶导数，规划所述相邻的两个路径点之间的贝塞尔曲线；

步骤S150、依次连接所述相邻的两个路径点之间的贝塞尔曲线，得到规划完成的轨迹。

本公开实施例中，基于相邻的两个路径点之间的线段确定路径点分别在待规划轨迹上的一阶导数与二阶导数，进而基于该一阶导数与二阶导数规划相邻的两个路径点之间的贝塞尔曲线，进而依次连接该贝塞尔曲线得到规划完成的轨迹。通过这种方法，使得即使待规划轨迹是由多条轨迹拼接而成的，也能够得到全局平滑的规划完成的轨迹，提高了所规划轨迹的平滑程度。

需要说明的是，出于简要说明的目的，在后续描述中，主要是示例性地以机器人的控制器为执行主体，以及控制器规划机器人的运动轨迹为应用场景对本公开实施例的具体实施过程进行描述。但并不代表本公开实施例只能应用于规划机器人的运动轨迹，也不代表规划机器人的运动轨迹只能由机器人的控制器执行。

还需要说明的是，出于简要说明的目的，在无特别限定的情况下，“路径点的一阶导数”指的是“路径点在待规划轨迹上的一阶导数”，“路径点的二阶导数”指的是“路径点在待规划轨迹上的二阶导数”。

本公开实施例中，控制器获取机器人所要经过的路径点，即，待规划轨迹所要经过的路径点，以规划机器人经过该路径点时的轨迹。其中，该路径点包括起点、终点以及中间点。中间点位于起点与终点之间，至少有一个，可以有两个或者两个以上。

在一实施例中，获取待规划轨迹所要经过的路径点，包括：获取预设的路径点。

该实施例中，预先设置机器人在运动时所要经过的路径点。具体的，用户可以预先设置机器人开始运动的起点、途中经过的中间点以及最终停止运动的终点，进而将该预先设置的路径点的相关信息输入控制器。

在一实施例中，获取待规划轨迹所要经过的路径点，包括：

通过视觉系统获取所要经过的运动区域的区域图像；

基于对区域图像的处理，获取待规划轨迹所要经过的路径点。

该实施例中，控制器通过图像处理获取待规划轨迹所要经过的路径点。具体的，控制器通过视觉系统对机器人的监控，获取机器人从起点运动至终点所有经过的运动区域的区域图像；进而对该区域图像进行处理，根据图像处理结果(例如：从图像中识别出的运动区域中障碍物的位置以及大小)确定运动至终点所有经过的路径点。

该实施例的优点在于，通过图像处理的方式获取路径点，能够根据实时获取的图像进行轨迹规划，实现了轨迹规划的实时应用性。

本公开实施例中，控制器使用直线依次连接路径点，进而基于以直线依次连接路径点所得到的线段，确定路径点分别在待规划轨迹上的一阶导数；进而，控制器基于该线段以及路径点分别在待规划轨迹上的一阶导数，确定各路径点分别在待规划轨迹上的二阶导数。具体的，点在轨迹上的一阶导数描述的是该点在轨迹上的切线向量，点在轨迹上的二阶导数描述的是该点在轨迹上的曲线曲率。

在一实施例中，路径点包括起点、终点以及中间点，基于以直线依次连接路径点所得到的线段，确定路径点分别在待规划轨迹上的一阶导数，包括：

获取起点与起点的相邻路径点之间的第一线段，以及终点与终点的相邻路径点之间的第二线段；

获取各个中间点与中间点的两侧相邻路径点之间的线段，得到中间点所连接的第三线段以及第四线段；

基于第一线段，确定起点在待规划轨迹上的一阶导数；

基于第二线段，确定终点在待规划轨迹上的一阶导数；

基于第三线段以及第四线段，确定中间点在待规划轨迹上的一阶导数。

该实施例中，控制器以直线依次连接路径点，得到各个线段。针对位于待规划轨迹两端的起点与终点，控制器基于起点所在的第一线段确定起点的一阶导数，基于终点所在的第二线段确定终点的一阶导数；针对位于待轨迹轨迹中间的中间点，控制器基于中间点两侧的第三线段以及第四线段确定该中间点的一阶导数。其中，第一线段以及第二线段均是全局有效的；第三线段以及第四线段与特定中间点相关联，每一中间点均有着对应的第三线段以及第四线段。

图2示出了本公开一实施例的以直线将路径点依次连接后所得到的线段。

参考图2所示，该实施例中，A点为起点，F点为终点，B点、C点、D点以及E点均为中间点。其中，线段AB为第一线段，线段EF为第二线段；对于中间D点点来说，线段CD为D点的第三线段，线段DE为D点的第四线段。同理，对于中间B点点来说，线段AD为B点的第三线段，线段BC为B点的第四线段。

从而，该实施例中，控制器基于线段AB确定A点的一阶导数，基于线段EF确定F点的一阶导数，基于线段AB以及线段BC确定B点的一阶导数，基于线段BC以及线段CD确定C点的一阶导数，基于线段CD以及线段DE确定D点的一阶导数，基于线段DE以及线段EF确定E点的一阶导数。

需要说明的是，该实施例只是示例性的说明，不应对本公开的功能和使用范围造成限制。

在一实施例中，基于第一线段，确定起点在待规划轨迹上的一阶导数，包括：

获取运动对象在起点的运动朝向；

基于运动朝向以及第一线段的长度，确定起点在待规划轨迹上的一阶导数。

该实施例中，运动对象为机器人。控制器基于机器人在起点的运动朝向以及第一线段的长度，确定起点的一阶导数。具体的，控制器可以将该运动朝向确定为起点的一阶导数的方向，将第一线段的长度的二分之一确定为起点的一阶导数的大小。其中，该运动朝向与第一线段的方向可能平行，也可能不平行。

例如：参考图2，控制器将机器人在A点的运动朝向确定为A点的一阶导数的方向，将线段AB的长度的二分之一确定为A点的一阶导数的大小，从而确定A点的一阶导数。其中，机器人在A点的运动朝向可能平行于线段AB，也可能不平行于线段AB。

需要说明的是，该实施例只是示例性的说明，不应对本公开的功能和使用范围造成限制。可以理解的，根据应用需求，可以将运动朝向与第一线段的方向相结合确定起点的一阶导数的方向(例如：将运动朝向与第一线段的夹角的角平分线所在方向确定为起点的一阶导数的方向)；可以将其他比例的第一线段的长度确定为起点的一阶导数的大小(例如：将第一线段的长度的三分之一确定为起点的一阶导数的大小)。

在一实施例中，基于第二线段，确定终点在待规划轨迹上的一阶导数，包括：

基于第二线段的方向，确定终点在待规划轨迹上的一阶导数的方向；

基于第二线段的长度，确定终点在待规划轨迹上的一阶导数的大小。

该实施例中，控制器可以将第二线段指向终点的方向确定为终点在待规划轨迹上的一阶导数的方向，将第二线段的长度的二分之一确定为终点在待规划轨迹上的一阶导数的大小。

例如：参考图2，控制器将E点到F点的方向确定为F点的一阶导数的方向，将线段EF的长度的二分之一确定为F点的一阶导数的大小。

需要说明的是，该实施例只是示例性的说明，不应对本公开的功能和使用范围造成限制。

在一实施例中，基于第三线段以及第四线段，确定中间点在待规划轨迹上的一阶导数，包括：

将第三线段与第四线段所构成角的角平分线的垂直方向确定为中间点在待规划轨迹上的一阶导数的方向；

基于第三线段的长度以及第四线段的长度，确定中间点在待规划轨迹上的一阶导数的大小；

根据中间点在待规划轨迹上的一阶导数的方向以及中间点在待规划轨迹上的一阶导数的大小，确定中间点在待规划轨迹上的一阶导数。

该实施例中，控制器将中间点两侧的第三线段与第四线段所构成角的角平分线的垂直方向确定为该中间点的一阶导数的方向；并基于第三线段的长度以及第四线段的长度确定该中间点的一阶导数的大小。具体的，控制器可以将第三线段与第四线段中长度最小的线段的线段长度确定为该中间点的一阶导数的大小；也可以将第三线段的长度与第四线段的长度的平均值确定为该中间点的一阶导数的大小。

图3示出了本公开一实施例中确定图2中D点的一阶导数的方向的示意图。

参考图3所示，该实施例中，D点的第三线段CD与D点的第四线段DE构成角CDE，直线p为角CDE的角平分线，直线h垂直于直线p，且直线h的方向与C点到E点的方向大致同向。则，控制器将直线h的方向确定为D点的一阶导数的方向。

若线段CD的长度小于线段DE的长度，则控制器将线段CD的长度确定为D点的一阶导数的大小；若线段CD的长度大于线段DE的长度，则控制器将线段DE的长度确定为D点的一阶导数的大小。

需要说明的是，该实施例只是示例性的说明，不应对本公开的功能和使用范围造成限制。

在一实施例中，基于线段以及路径点分别在待规划轨迹上的一阶导数，确定路径点分别在待规划轨迹上的二阶导数，包括：

基于相邻的两个路径点分别在待规划轨迹上的一阶导数，模拟该相邻的两个路径点之间的三阶贝塞尔曲线；

基于线段以及路径点分别在三阶贝塞尔曲线上的二阶导数，确定路径点分别在待规划轨迹上的二阶导数。

该实施例中，控制器通过模拟得到的三阶贝塞尔曲线上的二阶导数，确定路径点分别在待规划轨迹上的二阶导数。

具体的，针对相邻的两个路径点，控制器基于该两个路径点的一阶导数，模拟该两个路径点之间的三阶贝塞尔曲线，得到三阶贝塞尔曲线的数目与所得到的线段的数目一致。模拟得到确切的三阶贝塞尔曲线后，即可以根据几何学的处理确定路径点分别在三阶贝塞尔曲线上的曲线曲率，即，路径点分别在三阶贝塞尔曲线上的二阶导数。进而，控制器基于所得到的线段以及路径点分别在三阶贝塞尔曲线上的二阶导数，确定路径点在待规划轨迹上的二阶导数。其中，各个三阶贝塞尔曲线的模拟过程相互独立，即使是同一中间点，分别在所连接的两个三阶贝塞尔曲线上的二阶导数一般也是不相同的。

该实施例的优点在于，由于三阶贝塞尔曲线的曲率变化是最小的，因此，通过模拟三阶贝塞尔曲线确定点的二阶导数，能够使得点的曲率的变化程度尽量小，从而提高所规划轨迹的平滑度。

在一实施例中，路径点包括起点、终点以及中间点，基于线段以及路径点分别在三阶贝塞尔曲线上的二阶导数，确定路径点分别在待规划轨迹上的二阶导数，包括：

将起点在三阶贝塞尔曲线上的二阶导数确定为起点在待规划轨迹上的二阶导数；

将终点在三阶贝塞尔曲线上的二阶导数确定为终点在待规划轨迹上的二阶导数；

基于线段以及各个中间点在所连接的两个三阶贝塞尔曲线上的二阶导数，确定中间点在待规划轨迹上的二阶导数。

该实施例中，控制器模拟得到的三阶贝塞尔曲线中，起点所在的仅有一个；终点所在的仅有一个；每一中间点所在的有两个三阶贝塞尔曲线，即，中间点所连接的两个三阶贝塞尔曲线。控制器将起点在其所在唯一一个的三阶贝塞尔曲线上的二阶导数确定为起点的二阶导数；将终点在其所在唯一一个的三阶贝塞尔曲线上的二阶导数确定为终点的二阶导数；基于线段以及中间点在所连接的两个三阶贝塞尔曲线上的二阶导数，确定中间点的二阶导数。

例如：路径点依次为起点A点、中间点B点、终点C点。控制器根据A点的一阶导数以及B点的一阶导数，模拟得到A点与B点之间的三阶贝塞尔曲线，记为曲线AB；根据B点的一阶导数以及C点的一阶导数，模拟得到B点与C点之间的三阶贝塞尔曲线，记为曲线BC。根据三阶贝塞尔曲线的模拟过程，可以确定S_A″＝6B+2t_B-4t_A-6A，S_AB″＝6A+2t_A+4t_B-6B，S_BC″＝6C+2t_C-4t_B-6B，S_C″＝6B+2t_B+4t_C-6C。

其中，A为A点的坐标，B为B点的坐标，C为C点的坐标；t_A为A点的一阶导数，t_B为B点的一阶导数，t_C为C点的一阶导数；S_A″为A点在曲线AB上的二阶导数，S_AB″为B点在曲线AB上的二阶导数，S_BC″为B点在曲线BC上的二阶导数，S_C″为C点在曲线BC上的二阶导数。

则，控制器将S_A″确定为A点的二阶导数a_A，将S_C″确定为C点的二阶导数a_C；基于线段AB以及线段BC以及S_AB″和S_BC″，确定B点的二阶导数a_B。

在一实施例中，基于线段以及各个中间点在所连接的两个三阶贝塞尔曲线上的二阶导数，确定中间点在待规划轨迹上的二阶导数，包括：

获取各个中间点与中间点的两侧相邻路径点之间的线段，得到中间点所连接的第三线段以及第四线段；

基于第三线段的长度以及第四线段的长度，分别为在所连接的两个三阶贝塞尔曲线上的二阶导数分配对应的权重；

基于权重对在所连接的两个三阶贝塞尔曲线上的二阶导数进行加权，得到中间点在待规划轨迹上的二阶导数。

该实施例中，控制器基于线段长度对中间点在所连接的两个三阶贝塞尔曲线上的二阶导数进行加权，得到中间点的二阶导数。

具体的，控制器获取中间点所连接的第三线段以及第四线段；基于第三线段的长度以及第四线段的长度，分别为中间点在所连接的两个三阶贝塞尔曲线上的二阶导数分配对应的权重，并基于分配的权重进行加权，得到中间点的二阶导数。

例如：路径点依次为起点A点、中间点B点、终点C点。控制器进行三阶贝塞尔曲线的模拟后，计算得到B点在曲线AB上的二阶导数S_AB″以及B点在曲线BC上的二阶导数S_BC″。控制器获取线段AB以及线段BC后，计算线段AB的长度|AB|与线段BC的长度|BC|之和(|AB|+|BC|)；计算a＝|AB|/(|AB|+|BC|)以及β＝|BC|/(|AB|+|BC|)，进而将α作为S_AB″的权重，将p作为S_BC″的权重；进而加权得到中间点的二阶导数a_B＝αS_AB″+βS_BC″。

需要说明的是，该实施例只是示例性的说明，不应对本公开的功能和使用范围造成限制。

本公开实施例中，控制器确定路径点分别在待规划轨迹上的一阶导数与二阶导数后，基于相邻的两个路径点分别在待规划轨迹上的一阶导数与二阶导数，规划该相邻的两个路径点之间的贝塞尔曲线。优选的，规划相邻的两个路径点之间的五阶贝塞尔曲线。

例如：路径点依次为起点A点、中间点B点、终点C点。控制器确定A点的一阶导数t_A，A点的二阶导数a_A，B点的一阶导数t_B，B点的二阶导数a_B，C点的一阶导数t_C，C点的二阶导数a_C后，基于t_A、a_A、t_B以及a_B规划A点与B点之间的五阶贝塞尔曲线，基于t_B、a_B、t_C以及a_C规划B点与C点之间的五阶贝塞尔曲线。

具体的，相邻的两个路径点之间的五阶贝塞尔曲线表示为：

S(t)＝(1-t)⁵P₀+5(1-t)⁴tP₁+10(1-t)³t²P₂+10(1-t)²t³P₃+5(1-t)t⁴P₄+t⁵P₅

其中，t∈[0，1]。P₀为五阶贝塞尔曲线的起点，为t＝0时S(t)的取值；P₅为五阶贝塞尔曲线的终点，为t＝1时s(t)的取值；即，P₀和P₅为该相邻的两个路径点；P₁、P₂、P₃、P₄为五阶贝塞尔曲线的四个控制点，用于控制五阶贝塞尔曲线的形状。将该五阶贝塞尔曲线求取一阶导数和二阶导数，t_s表示P₀处的一阶导数，t_e表示P₅处的一阶导数，a_s表示P₀处的二阶导数，a_e表示P₅处的二阶导数。从而求得中间四个控制点的表示：

从而，控制器能够根据相邻的两个路径点的一阶导数以及二阶导数，确定该相邻的两个路径点之间的四个控制点，进而根据确定的该四个控制点规划该相邻的两个路径点之间的五阶贝塞尔曲线。

该实施例中，通过规划相邻的两个路径点之间的五阶贝塞尔曲线，使得依次连接这些五阶贝塞尔曲线所得到的规划完成的轨迹具有高自由度以及优秀的拓展度，能够有效抵抗单一曲线发生改变对全局轨迹造成的振荡；还可以保证各路径点的曲率连续。

若采用四阶或者四阶之下的贝塞尔曲线规划相邻的两个路径点之间的曲线，会导致每一个曲线所提供的自由度不足以满足全局轨迹的局部可改性，即，其中一个曲线的形状发生改变会引起全局轨迹的改变。这种情况下全局轨迹会很不稳定，单一参数的改变会使得全局轨迹发生严重振荡，无法有效抵抗单一曲线发生改变对全局轨迹造成的振荡；而且采用四阶或者四阶之下的贝塞尔曲线规划相邻的两个路径点之间的曲线，无法保证各路径点的曲率连续。

其中，单一曲线发生改变的情况一般发生在局部避障或者路线优化的情况下。

图4示出了本公开一实施例的局部避障的示意图。

该实施例中，A点、B点、C点是全局轨迹上的局部路径点，全局轨迹是通过规划五阶贝塞尔曲线得到的。左侧示出的是未出现障碍物的情况下全局轨迹上的A点、B点、C点；右侧示出的是出现障碍物后全局轨迹上的A点、B点、C点。由图可见，当障碍物出现后，出于规避该障碍物的目的，需要调整B点的位置，从而改变了A点到B点的轨迹以及B点到C点的轨迹。在这种情况下，部分曲线所发生的改变会对全局轨迹造成影响，由于五阶贝塞尔曲线提供了足够的自由度，从而即使A点到B点的轨迹以及B点到C点的轨迹发生了改变，也不会对全局轨迹造成严重振荡。

图5示出了本公开一实施例的路线优化的示意图。

该实施例中，A点、B点、C点是全局轨迹上的局部路径点。左侧示出的是未优化路线的情况下全局轨迹上的A点、B点、C点；右侧示出的是优化路线后全局轨迹上的A点、B点、C点。由图可见，出于优化路线的目的，需要调整B点的位置，从而改变了A点到B点的轨迹以及B点到C点的轨迹。同理，在这种情况下，即使A点到B点的轨迹以及B点到C点的轨迹发生了改变，也不会对全局轨迹造成严重振荡。

本公开实施例中，控制器规划每一对相邻的两个路径点之间的贝塞尔曲线后，依次连接所规划的贝塞尔曲线，从而得到规划完成的轨迹。

例如：参考图2。控制器规划A点与B点之间的五阶贝塞尔曲线、B点与C点之间的五阶贝塞尔曲线、C点与D点之间的五阶贝塞尔曲线、D点与E点之间的五阶贝塞尔曲线、E点与F点之间的五阶贝塞尔曲线后，依次连接所规划的这五条五阶贝塞尔曲线，从而得到规划完成的机器人从A点出发，经过B点、C点、D点、E点，最终到达F点的轨迹。

在一实施例中，该方法还包括：

获取运动对象的运动约束参数；

基于对规划完成的轨迹的离散化处理，确定规划完成的轨迹上的各离散点；

基于运动约束参数规划运动对象分别在各离散点上的运动速度。

该实施例中，运动对象为机器人。控制器得到规划完成的轨迹后，进一步地规划机器人在轨迹上运动时的运动速度。

具体的，控制器获取机器人的运动约束参数，例如：最大速度、最大加速度、最大角速度、最大向心力；基于对规划完成的轨迹进行离散化处理，确定规划完成的轨迹上的各离散点；进而基于运动约束参数规划机器人分别在各离散点上的运动速度，以使得机器人在规划完成的轨迹上运动时，其运动速度符合该运动约束参数的约束。

控制器可以通过根据运动约束参数得到的以下约束条件，规划机器人在每个离散点上的运动速度v_i。其中，运动约束参数包括最大速度v_max、最大加速度

最大角速度

最大向心力

根据该运动约束参数得到的约束条件有：

a.运动速度大于0且小于最大速度，即，v_i∈[0,v_max]；

b.最大转速造成的线速度大于0且小于最大速度，即，v_i∈[0,v_max|ω]，

c.向心力为f＝m·|c|·v²，最大速度满足最大向心力的限制，即，v_i[0,v_max|f]，

d.运动速度满足最大加速度的限制，即，

其中，

该实施例的优点在于，通过进一步的速度规划，进一步地提高了运动对象在运动时的合理性。

需要说明的是，该实施例只是示例性的说明，不应对本公开的功能和使用范围造成限制。

在一实施例中，控制器采用固定弧长离散法对规划完成的轨迹进行离散化处理，确定各离散点，进而基于运动约束参数规划机器人分别在各离散点上的运动速度。

具体的，针对规划完成的轨迹中的每一个贝塞尔曲线：从该贝塞尔曲线S(t)的起点开始，将t从0开始每次增加0.01，代入求出临时坐标点。计算该临时坐标点与起点的欧式距离，若该欧式距离小于预设的固定弧长，则继续增加t；若该欧式距离大于等于预设的固定弧长，则将该临时坐标点确定为一离散点。直至t增加至1，该贝塞尔曲线便被通过固定弧长离散化完毕，确定该贝塞尔曲线上的所有离散点。

需要说明的是，该实施例只是示例性的说明，不应对本公开的功能和使用范围造成限制。控制器除了可以采用固定弧长离散法进行离散化处理外，还可以采用插值法或者其他离散化处理的方法进行离散化处理。

图6示出了根据本公开一实施例的轨迹规划的装置，所述装置包括：

获取模块210，配置为获取待规划轨迹所要经过的路径点；

第一确定模块220，配置为基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

第二确定模块230，配置为基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数；

规划模块240，配置为基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数与二阶导数，规划所述相邻的两个路径点之间的贝塞尔曲线；

连接模块250，配置为依次连接所述相邻的两个路径点之间的贝塞尔曲线，得到规划完成的轨迹。

在本公开的一示例性实施例中，所述装置配置为：

基于所述线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述路径点包括起点、终点以及中间点，所述装置配置为：

获取所述起点与所述起点的相邻路径点之间的第一线段，以及所述终点与所述终点的相邻路径点之间的第二线段；

获取各个中间点与所述中间点的两侧相邻路径点之间的线段，得到所述中间点所连接的第三线段以及第四线段；

基于所述第一线段，确定所述起点在所述待规划轨迹上的一阶导数；

基于所述第二线段，确定所述终点在所述待规划轨迹上的一阶导数；

基于所述第三线段以及所述第四线段，确定所述中间点在所述待规划轨迹上的一阶导数。在本公开的一示例性实施例中，所述装置配置为：

在本公开的一示例性实施例中，所述装置配置为：

获取运动对象在所述起点的运动朝向；

基于所述运动朝向以及所述第一线段的长度，确定所述起点在所述待规划轨迹上的一阶导数。

在本公开的一示例性实施例中，所述装置配置为：

将所述第三线段与所述第四线段所构成角的角平分线的垂直方向确定为所述中间点在所述待规划轨迹上的一阶导数的方向；

基于所述第三线段的长度以及所述第四线段的长度，确定所述中间点在所述待规划轨迹上的一阶导数的大小；

根据所述中间点在所述待规划轨迹上的一阶导数的方向以及所述中间点在所述待规划轨迹上的一阶导数的大小，确定所述中间点在所述待规划轨迹上的一阶导数。

在本公开的一示例性实施例中，所述装置配置为：

基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数，模拟所述相邻的两个路径点之间的三阶贝塞尔曲线；

基于所述线段以及所述路径点分别在所述三阶贝塞尔曲线上的二阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述路径点包括起点、终点以及中间点，所述装置配置为：

将所述起点在所述三阶贝塞尔曲线上的二阶导数确定为所述起点在所述待规划轨迹上的二阶导数；

将所述终点在所述三阶贝塞尔曲线上的二阶导数确定为所述终点在所述待规划轨迹上的二阶导数；

基于所述线段以及各个中间点在所连接的两个三阶贝塞尔曲线上的二阶导数，确定所述中间点在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述装置配置为：

获取所述各个中间点与所述中间点的两侧相邻路径点之间的线段，得到所述中间点所连接的第三线段以及第四线段；

基于所述第三线段的长度以及所述第四线段的长度，分别为所述在所连接的两个三阶贝塞尔曲线上的二阶导数分配对应的权重；

基于所述权重对所述在所连接的两个三阶贝塞尔曲线上的二阶导数进行加权，得到所述中间点在所述待规划轨迹上的二阶导数。

在本公开的一示例性实施例中，所述装置配置为：

获取运动对象的运动约束参数；

基于对所述规划完成的轨迹的离散化处理，确定所述规划完成的轨迹上的各离散点；

基于所述运动约束参数规划所述运动对象分别在所述各离散点上的运动速度。

下面参考图7来描述根据本公开实施例的电子设备30。图7显示的电子设备30仅仅是一个示例，不应对本公开实施例的功能和使用范围带来任何限制。

如图7所示，电子设备30以通用计算设备的形式表现。电子设备30的组件可以包括但不限于：上述至少一个处理单元310、上述至少一个存储单元320、连接不同系统组件(包括存储单元320和处理单元310)的总线330。

其中，所述存储单元存储有程序代码，所述程序代码可以被所述处理单元310执行，使得所述处理单元310执行本说明书上述示例性方法的描述部分中描述的根据本发明各种示例性实施方式的步骤。例如，所述处理单元310可以执行如图1中所示的各个步骤。

存储单元320可以包括易失性存储单元形式的可读介质，例如随机存取存储单元(RAM)3201和/或高速缓存存储单元3202，还可以进一步包括只读存储单元(ROM)3203。

存储单元320还可以包括具有一组(至少一个)程序模块3205的程序/实用工具3204，这样的程序模块3205包括但不限于：操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。

总线330可以为表示几类总线结构中的一种或多种，包括存储单元总线或者存储单元服务器、外围总线、图形加速端口、处理单元或者使用多种总线结构中的任意总线结构的局域总线。

电子设备30也可以与一个或多个外部设备400(例如键盘、指向设备、蓝牙设备等)通信，还可与一个或者多个使得用户能与该电子设备30交互的设备通信，和/或与使得该电子设备30能与一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口350进行。输入/输出(I/O)接口350与显示单元340相连。并且，电子设备30还可以通过网络适配器360与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图所示，网络适配器360通过总线330与电子设备30的其它模块通信。应当明白，尽管图中未示出，可以结合电子设备30使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。

通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据本公开实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中或网络上，包括若干指令以使得一台计算设备(可以是个人计算机、服务器、终端装置、或者网络设备等)执行根据本公开实施方式的方法。

在本公开的示例性实施例中，还提供了一种计算机可读存储介质，其上存储有计算机可读指令，当所述计算机可读指令被计算机的处理器执行时，使计算机执行上述方法实施例部分描述的方法。

根据本公开的一个实施例，还提供了一种用于实现上述方法实施例中的方法的程序产品，其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码，并可以在终端设备，例如个人电脑上运行。然而，本发明的程序产品不限于此，在本文件中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

所述程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如JAVA、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算设备，或者，可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。

应当注意，尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元，但是这种划分并非强制性的。实际上，根据本公开的实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。

此外，尽管在附图中以特定顺序描述了本公开中方法的各个步骤，但是，这并非要求或者暗示必须按照该特定顺序来执行这些步骤，或是必须执行全部所示的步骤才能实现期望的结果。附加的或备选的，可以省略某些步骤，将多个步骤合并为一个步骤执行，以及/或者将一个步骤分解为多个步骤执行等。

通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据本公开实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中或网络上，包括若干指令以使得一台计算设备(可以是个人计算机、服务器、移动终端、或者网络设备等)执行根据本公开实施方式的方法。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由所附的权利要求指出。

Claims (10)

1.一种轨迹规划的方法，其特征在于，所述方法包括：

获取待规划轨迹所要经过的路径点；

基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数；

基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数与二阶导数，规划所述相邻的两个路径点之间的贝塞尔曲线；

依次连接所述相邻的两个路径点之间的贝塞尔曲线，得到规划完成的轨迹；

所述路径点包括起点、终点以及中间点，基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数，包括：

获取所述起点与所述起点的相邻路径点之间的第一线段，以及所述终点与所述终点的相邻路径点之间的第二线段；

获取各个中间点与所述中间点的两侧相邻路径点之间的线段，得到所述中间点所连接的第三线段以及第四线段；

基于所述第一线段，确定所述起点在所述待规划轨迹上的一阶导数；

基于所述第二线段，确定所述终点在所述待规划轨迹上的一阶导数；

基于所述第三线段以及所述第四线段，确定所述中间点在所述待规划轨迹上的一阶导数。

2.根据权利要求1所述的方法，其特征在于，基于所述第一线段，确定所述起点在所述待规划轨迹上的一阶导数，包括：

获取运动对象在所述起点的运动朝向；

基于所述运动朝向以及所述第一线段的长度，确定所述起点在所述待规划轨迹上的一阶导数。

3.根据权利要求1所述的方法，其特征在于，基于所述第三线段以及所述第四线段，确定所述中间点在所述待规划轨迹上的一阶导数，包括：

将所述第三线段与所述第四线段所构成角的角平分线的垂直方向确定为所述中间点在所述待规划轨迹上的一阶导数的方向；

基于所述第三线段的长度以及所述第四线段的长度，确定所述中间点在所述待规划轨迹上的一阶导数的大小；

根据所述中间点在所述待规划轨迹上的一阶导数的方向以及所述中间点在所述待规划轨迹上的一阶导数的大小，确定所述中间点在所述待规划轨迹上的一阶导数。

4.根据权利要求1所述的方法，其特征在于，基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数，包括：

基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数，模拟所述相邻的两个路径点之间的三阶贝塞尔曲线；

基于所述线段以及所述路径点分别在所述三阶贝塞尔曲线上的二阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数。

5.根据权利要求4所述的方法，其特征在于，基于所述线段以及所述路径点分别在所述三阶贝塞尔曲线上的二阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数，包括：

将所述起点在所述三阶贝塞尔曲线上的二阶导数确定为所述起点在所述待规划轨迹上的二阶导数；

将所述终点在所述三阶贝塞尔曲线上的二阶导数确定为所述终点在所述待规划轨迹上的二阶导数；

基于所述线段以及各个中间点在所连接的两个三阶贝塞尔曲线上的二阶导数，确定所述中间点在所述待规划轨迹上的二阶导数。

6.根据权利要求5所述的方法，其特征在于，基于所述线段以及各个中间点在所连接的两个三阶贝塞尔曲线上的二阶导数，确定所述中间点在所述待规划轨迹上的二阶导数，包括：

获取所述各个中间点与所述中间点的两侧相邻路径点之间的线段，得到所述中间点所连接的第三线段以及第四线段；

基于所述第三线段的长度以及所述第四线段的长度，分别为所述在所连接的两个三阶贝塞尔曲线上的二阶导数分配对应的权重；

基于所述权重对所述在所连接的两个三阶贝塞尔曲线上的二阶导数进行加权，得到所述中间点在所述待规划轨迹上的二阶导数。

7.根据权利要求1所述的方法，其特征在于，所述方法还包括：

获取运动对象的运动约束参数；

基于对所述规划完成的轨迹的离散化处理，确定所述规划完成的轨迹上的各离散点；

基于所述运动约束参数规划所述运动对象分别在所述各离散点上的运动速度。

8.一种轨迹规划的装置，其特征在于，所述装置包括：

获取模块，配置为获取待规划轨迹所要经过的路径点；

第一确定模块，配置为基于以直线依次连接所述路径点所得到的线段，确定所述路径点分别在所述待规划轨迹上的一阶导数；

第二确定模块，配置为基于所述线段以及所述路径点分别在所述待规划轨迹上的一阶导数，确定所述路径点分别在所述待规划轨迹上的二阶导数；

规划模块，配置为基于相邻的两个路径点分别在所述待规划轨迹上的一阶导数与二阶导数，规划所述相邻的两个路径点之间的贝塞尔曲线；

连接模块，配置为依次连接所述相邻的两个路径点之间的贝塞尔曲线，得到规划完成的轨迹；

所述路径点包括起点、终点以及中间点，所述第一确定模块配置为：

获取所述起点与所述起点的相邻路径点之间的第一线段，以及所述终点与所述终点的相邻路径点之间的第二线段；

获取各个中间点与所述中间点的两侧相邻路径点之间的线段，得到所述中间点所连接的第三线段以及第四线段；

基于所述第一线段，确定所述起点在所述待规划轨迹上的一阶导数；

基于所述第二线段，确定所述终点在所述待规划轨迹上的一阶导数；

基于所述第三线段以及所述第四线段，确定所述中间点在所述待规划轨迹上的一阶导数。

9.一种轨迹规划的电子设备，其特征在于，包括：

存储器，存储有计算机可读指令；

处理器，读取存储器存储的计算机可读指令，以执行权利要求1－7中的任一个所述的方法。

10.一种计算机可读存储介质，其特征在于，其上存储有计算机可读指令，当所述计算机可读指令被计算机的处理器执行时，使计算机执行权利要求1－7中的任一个所述的方法。

Images