CN117519119A - 一种路径规划方法、装置及计算机程序产品 - Google Patents

Abstract

本发明公开了一种涉及移动机器人领域、尤其是移动机器人运动控制领域的路径规划方法、装置及计算机程序产品，该路径规划方法包括：获取起点、终点间的初始路径；基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，并将选取的点与所述起点、所述终点均作为途经点；选取任意的至少两个途经点，构建经过所述至少两个途经点的曲率连续的曲线，作为所述至少两个途经点对应的新路径。本发明在初始路径的基础上进行优化，能够得到经过初始路径中相应点的曲率连续的新路径，从而使得运动路径更加平滑，满足移动机器人运动时效及硬件性能方面的需求。

Description

一种路径规划方法、装置及计算机程序产品

技术领域

本发明涉及移动机器人领域、尤其是移动机器人的运动控制领域，更具体地说，涉及一种路径规划方法、装置及计算机程序产品。

背景技术

目前在进行移动机器人的局部路径规划时，通常可以基于TEB(Timed-Elastic-Band，时间弹性带)算法实现。TEB算法是基于时间最优的多目标规划算法，但是基于TEB算法得到的局部路径无法保证平滑，尤其是在移动机器人遇到障碍物绕行时，会严重影响移动机器人运动的平稳和连续，平滑的运动路径对移动机器人的运动时效及硬件性能需求均具有重要的意义，急需一种方法解决上述技术问题。

发明内容

本发明的目的是提供一种路径规划方法、装置及计算机程序产品，能够使得运动路径更加平滑，满足移动机器人运动时效及硬件性能方面的需求。

为了实现上述目的，本发明提供如下技术方案：

一种路径规划方法，包括：

获取起点、终点间的初始路径；

基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，并将选取的点与所述起点、所述终点均作为途经点；

选取任意的至少两个途经点，构建经过所述至少两个途经点的曲率连续的曲线，作为所述至少两个途经点对应的新路径。

可选地，基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，包括：

基于所述初始路径中各点的曲率从所述初始路径中选取至少一个点。

可选地，基于所述初始路径中各点的曲率从所述初始路径中选取至少一个点，包括：

从所述初始路径中选取出曲率变号点和/或曲率极值点。

可选地，选取任意的至少两个途经点，包括：

选取任意的两个相邻的途经点。

可选地，构建经过所述至少两个途经点的曲率连续的曲线，包括：

构建经过所述两个相邻的途经点的二次B样条曲线。

可选地，所述起点为移动机器人的当前位置，所述终点为所述移动机器人前瞻距离范围内的最远点或者局部路径的终点。

可选地，还包括：

每经过预设时间间隔和/或预设距离间隔则执行所述获取起点、终点间的初始路径的步骤。

可选地，所述起点和终点分别为全局路径中的起点及终点。

可选地，构建经过所述至少两个途经点的曲率连续的曲线之后，还包括：

如果构建的曲线是与障碍物发生碰撞的曲线，则向远离所述障碍物的方向移动所述至少两个途经点，并重新构建相应的曲线。

可选地，向远离所述障碍物的方向移动所述至少两个途经点，包括：

基于所述障碍物的代价地图，向远离所述障碍物的方向移动所述至少两个途经点。

一种路径规划装置，包括：

获取模块，用于：获取起点、终点间的初始路径；

选取模块，用于：基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，并将选取的点与所述起点、所述终点均作为途经点；

构建模块，用于：选取任意的至少两个途经点，构建经过所述至少两个途经点的曲率连续的曲线，作为所述至少两个途经点对应的新路径。

一种计算机程序产品，其中，所述计算机程序产品包括计算机程序，所述计算机程序在被处理器执行时，使得处理器实施如上任一项所述的路径规划方法。

本发明提供了一种路径规划方法、装置及计算机程序产品，该路径规划方法包括：获取起点、终点间的初始路径；基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，并将选取的点与所述起点、所述终点均作为途经点；选取任意的至少两个途经点，构建经过所述至少两个途经点的曲率连续的曲线，作为所述至少两个途经点对应的新路径。本发明在初始路径的基础上进行优化，能够得到经过初始路径中相应点的曲率连续的新路径，从而使得运动路径更加平滑，满足移动机器人运动时效及硬件性能方面的需求。

附图说明

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

图1为本发明实施例提供的一种路径规划方法的流程图；

图2为本发明实施例提供的一种路径规划方法中曲线曲率示意图；

图3为本发明实施例提供的一种路径规划方法中二次B样条曲线示意图；

图4为本发明实施例提供的一种路径规划方法中途经点移动方向示意图；

图5为本发明实施例提供的一种路径规划装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

附图中所示的流程图仅是示例性说明，不是必须包括所有的内容和操作/步骤，也不是必须按所描述的顺序执行。例如，有的操作/步骤还可以分解，而有的操作/步骤可以合并或部分合并，因此实际执行的顺序有可能根据实际情况改变。

本发明可以适用于移动机器人，其可以是任何能够自主地进行空间移动的机器人，例如AGV、AMR等；移动机器人可用于执行各种任务，例如用作仓储机器人、清扫型机器人、家庭陪护机器人、迎宾机器人等。当然也可以根据实际需要适用于其他需要实现路径规划的目标，如汽车、自动滑板等，均在本发明的保护范围之内。以下以本发明用于移动机器人进行具体说明。

请参阅图1，其示出了本发明实施例提供的一种路径规划方法的流程图，具体可以包括以下步骤：

S11：获取起点、终点间的初始路径。

起点、终点分别为本次所规划路径的起始点及终止点；起点、终点间的初始路径可以利用任何适用的已知方法来获取，通常初始路径为已经考虑对于移动机器人的约束条件和优化目标的较优的路径，如初始路径为起点、终点间的最短路径，或如初始路径为起点、终点间的避障路径。考虑到初始路径无法保证移动机器人运动路径的平滑，本发明在初始路径的基础上进行优化，使得优化后的路径能够更加平滑。

S12：基于初始路径的轨迹特点从初始路径中选取至少一个点，并将选取的点与起点、终点均作为途经点。

基于初始路径的轨迹特点从中选取至少一个点，与起点、终点一起作为途经点，进而基于途经点实现相应的路径规划。其中，在基于初始路径的轨迹特点选取至少一个点时，可以是选取初始路径的轨迹发生显著变化的点，如速度方向发生改变的点，也可以是选取轨迹明显不平滑的点，如速度不连续的点，还可以根据实际需要进行其他选取原则的设定，均在本发明的保护范围之内。

S13：选取任意的至少两个途经点，构建经过至少两个途经点的曲率连续的曲线，作为至少两个途经点对应的新路径。

从全部途经点中选取出任意的至少两个途经点，基于该至少两个途经点构建经过该至少两个途经点的曲率连续的曲线，并利用该曲率连续的曲线替换原始路径中经过该至少两个途经点的对应部分路径，使得该曲率连续的曲线成为经过该至少两个途经点部分的新路径。其中，曲率连续表示二阶导连续，而二阶导连续表示加速度、速度连续，也即加速度及速度的变化平缓，相应的运动路径则为平滑的路径。

本发明对于起点、终点间的初始路径，基于该初始路径的轨迹特点从中选取出至少一个点，与起点、终点共同作为途经点，基于任意的至少两个途经点构建经过该至少两个途经点的曲率连续的曲线，作为该至少两个途经点对应的新路径。可见，本发明在初始路径的基础上进行优化，能够得到经过初始路径中相应点的曲率连续的新路径，从而使得运动路径更加平滑，满足移动机器人运动时效及硬件性能方面的需求。

本发明实施例提供的一种路径规划方法，基于初始路径的轨迹特点从初始路径中选取至少一个点，可以包括：基于初始路径中各点的曲率从初始路径中选取至少一个点。

基于初始路径中各点的曲率从初始路径中选取至少一个点，可以包括：从初始路径中选取出曲率变号点和/或曲率极值点。

需要说明的是，曲线的曲率是针对曲线上某个点的切线方向角对弧长的转动率，能够有效的体现出曲线的轨迹特点，因此本实施例以曲线中各点的曲率来表示曲线的轨迹特点，从而基于初始路径中各点的曲率选取至少一个点作为相应的途经点，能够使得对初始路径中点的选取更加简便直观。其中，曲率由正变负或者由负变正的点，曲率由负变零或者由零变负的点，均可以称之为曲率变号点；正曲率中的极大值点，负曲率中的极小值点，均可以称之为曲率极值点；如图2所示，图中的曲线表示初始路径，初始路径中的圆心点表示曲率变号点，初始路径中的叉形点表示曲率极值点。在一种具体实现方式中，基于初始路径中各点的曲率选取至少一个点时，可以选取初始路径中包含的曲率变号点及曲率极值点的至少一种，在选取曲率变号点时，可以选取曲率由正变负的点、曲率由负变正的点、曲率由负变零的点、曲率由零变负的点中的至少一种，在选取曲率极值点时，可以选取正曲率中的极大值点、负曲率中的极小值点的至少一种；从而使得选取的点为初始路径的轨迹具有不同于其他点的具有显著特征的点，进而基于这些点得到平滑的新路径。

本发明实施例提供的一种路径规划方法，选取任意的至少两个途经点，可以包括：选取任意的两个相邻的途经点。

构建经过至少两个途经点的曲率连续的曲线，可以包括：构建经过两个相邻的途经点的二次B样条曲线。

本实施例可以选取初始路径中任意的两个相邻的途经点实现曲线构建，例如，将全部途经点依顺序两两作为选取的两个相邻的途经点，即第一点及第二点作为一组相邻的途经点，第二点及第三点作为一组相邻的途经点，以此类推，直至倒数第二点及倒数第一点作为一组相邻的途经点，进而基于选取的每组途经点构建相应的曲线后，则能够得到可分别替换初始路径各部分的新路径，以由新路径替换整个初始路径，使得整个路径均更加平滑。

本实施例中构建的曲线可以是任意的曲率连续的曲线，如贝塞尔曲线、B样条曲线等。例如，采用B样条曲线、尤其是二次B样条曲线作为曲率连续的曲线，从而将两个相邻的途经点分别作为端点，得到两端点间的二次B样条曲线，也即为两端点间的新路径。基于图3，对基于两端点构建二次B样条曲线的原理进行具体说明：

1、二次B样条曲线满足以下参数方程：

P(t)＝A₀+A₁·t+A₂·t²，t∈[0，1]；1]

其中，

2、二次B样条曲线满足以下条件：

(1)在t＝0时，曲线经过控制点P0和P1的中点Vs；

(2)在t＝0时，曲线与P0和P1所在直线相切；

(3)在t＝1时，曲线经过控制点P1和P2的中点Ve；

(4)在t＝1时，曲线与P1和P2所在直线相切；

将上述条件表示为以下条件方程：

3、基于上述参数方程及条件方程求解可得到以下结果：

4、基于上述三点已推导出由控制点P₀，P₁，P₂计算二次B样条曲线的过程；而已知两个端点V_s、V_e，以及在两个端点处的速度方向计算各控制点的约束如下：

其中，基于右侧第二个等式可以求出c1和c2的值，进而可以基于c1和c2的值求得P₀，P₁，P₂。

上述四点即为基于两端点构建二次B样条曲线的原理；相应的，在确定两端点后，可以先基于两端点的状态计算出各控制点，然后再基于各控制点计算得到二次B样条曲线，从而基于两端点实现对二次B样条曲线的构建。

需要说明的时，本发明提供的路径规划方法可以基于实际需求应用于全局路径规划或者局部路径规划，从而能够在满足实际需求的同时，使得对移动机器人的路径规划更加灵活。

在将本发明提供的路径规划方法应用于全局路径规划时，可以先规划得到全局路径作为初始路径，初始路径的起点为全局路径的起点，初始路径的终点为全局路径的终点，从而在得到新路径后，利用新路径替换全局路径中的对应部分路径，达到使运动路径更加平滑的目的。其中，全局路径可以基于任何适用的已知方法来获取，如Astar算法、混合Astar算法等。

在将本发明提供的路径规划方法应用于局部路径规划时，初始路径的起点为移动机器人的当前位置，初始路径的终点为移动机器人前瞻距离范围内最远点或者局部路径的终点，如图2中虚线框内的路径则为局部路径。其中，初始路径的终点为移动机器人前瞻距离范围内最远点时，表示需要实现规划的局部路径为移动机器人能够检测到的前瞻范围内的路径；初始路径的终点为局部路径的终点时，表示需要实现规划的局部路径不以移动机器人的前瞻范围为标准，而是为基于实际需求做局部路径规划所得的路径。在一种具体实现方式中，可以基于静态地图生成全局路径，然后在移动机器人的运动过程中，再基于全局路径、障碍物、运动学/动力学等约束生成局部路径作为初始路径，进而基于生成的初始路径进行移动机器人前瞻范围内的局部路径或者局部路径本身进行优化，得到更平滑的新路径。

另外，在将本发明提供的路径规划方法应用于局部路径规划时，可以每经过预设时间间隔和/或预设距离则执行获取起点、终点间初始路径的步骤，也即定时和/或定点的执行本发明提供的路径规划方法中得到新路径的相关步骤，从而实现移动机器人运动全过程的分段路径规划，使得移动机器人运动全过程的运动路径更平滑。其中，预设时间间隔和/或预设距离可以根据实际需要设定为定值或者不定值，如预设时间间隔可以设定为3min、5min等定值，预设距离可以设定为10m、15m、移动机器人前瞻距离等定值，而当将预设时间间隔和/或预设距离设定为不定值时，可以基于移动机器人所处的场景或者其他因素设置预设时间间隔和/或预设距离的值的变化规律，如可以设置为移动机器人由宽敞简单环境进入窄小复杂环境时，预设时间间隔和/预设距离适当减小，反之则适当增大；从而使得路径规划的循环实现满足实际的不同需求。

本发明实施例提供的一种路径规划方法，构建经过至少两个途经点的曲率连续的曲线之后，还可以包括：如果构建的曲线是与障碍物发生碰撞的曲线，则向远离障碍物的方向移动至少两个途经点，并重新构建相应的曲线。

向远离障碍物的方向移动至少两个途经点，可以包括：基于障碍物的代价地图，向远离障碍物的方向移动至少两个途经点。

在构建经过至少两个途经点的曲率连续的曲线之后，为避免移动机器人按照该曲线运动过程中会与障碍物发生碰撞，还可以判断构建的曲线是否可能与障碍物发生碰撞，如果判断的结果为是，则需要将上述至少两个途经点向远离可能会发生碰撞的障碍物的方向移动(如平移)，然后对于基于上述至少两个途经点中至少一个途经点构建的曲线(或者说按照经过上述至少两个途经点中至少一个途经点来构建的曲线)进行重新构建，如果判断的结果为否，则可以直接将构建的曲线作为相应的新路径，也即移动机器人的运动路径。从而能够在使得运动路径平滑的同时，还能够避免移动机器人与障碍物发生碰撞，提高移动机器人的使用寿命及工作效率等。

在向远离障碍物的方向移动上述至少两个途经点时，可以基于可能会发生碰撞的障碍物的代价地图实现。其中，代价地图中的单个像素代表实际的单块面积，其利用像素灰度值来表示相应面积存在障碍物的可能性；在一种具体实现方式中，代价地图可以呈现成圆形区域，如图4中的圆形区域则为单个障碍物的代价地图，此时越远离圆形区域的中心说明障碍物存在的可能性越小，与障碍物碰撞的可能性也越小，因此如果构建的曲线与圆形区域存在交点(或者是存在足够多的交点，即存在的交点数量大于预先根据实际需求设定的阈值)，则认为移动机器人沿着构建的曲线运动时会与障碍物发生碰撞，反之则认为移动机器人沿着构建的曲线运动时不会与障碍物发生碰撞。基于此，本实施例在需要向远离障碍物的方向移动上述至少两个途经点时，对于需要向远离障碍物的方向移动的相邻两个途经点，可以将每个途经点分别与圆形区域的圆心相邻得到与每个途经点分别对应的直线，然后将每个途经点由未进行任何移动前的位置开始、按照各自所对应的直线向远离圆心的方向移动一定距离，也可以将两个途经点相连得到相应的直线，确定圆形区域的与该直线互相垂直的圆形半径(圆形半径也可称之为梯度半径)，并将两个途经点由未进行任何移动前的位置开始、沿着该圆形半径向远离圆心的方向移动一定距离，当然也可以根据实际需要进行其他设定，以达到使途经点快速远离障碍物的目的。

在一种具体实现方式中，在需要移动的途经点为两个时，确定该两个点之间的曲线为当前曲线，在需要移动的途经点为至少三个时，则将该至少三个途经点对应曲线划分为多个子曲线(单个子曲线仅经过该至少三个途经点中相邻的两个途经点)，并确定与障碍物会发生碰撞的任意子曲线为当前曲线；在确定出当前曲线后，将当前曲线中包含的需要移动的两个途经点间用直线连接，再由障碍物的代价地图中选取与该直线互相垂直的圆形半径，进而令当前曲线中包含的需要移动的两个途经点由未进行任何移动时的位置开始、沿着该圆形半径向远离圆形区域的方向移动。例如：图4中各圆心所在点(A、B、C、D)可以代表途经点，经过各途经点的曲线为基于各途经点构建所得曲线，如果与障碍物的代价地图存在交点则认为会与障碍物发生碰撞，那么AB段曲线、BC段曲线及CD段曲线均会与障碍物发生碰撞；以BC段曲线为例说明途经点的移动方式，将BC之间用直线连接，然后从障碍物的代价地图中确定出与BC间的直线相互垂直的圆形半径，然后令BC由图中所示位置、沿着该圆形半径向远离圆形区域圆心的方向移动。

本发明实施例还提供了一种路径规划装置，如图5所示，可以包括：

获取模块11，用于：获取起点、终点间的初始路径；

选取模块12，用于：基于初始路径的轨迹特点从初始路径中选取至少一个点，并将选取的点与起点、终点均作为途经点；

构建模块13，用于：选取任意的至少两个途经点，构建经过至少两个途经点的曲率连续的曲线，作为至少两个途经点对应的新路径。

本实施例基于获取模块获取初始路径，基于选取模块由初始路径中选取确定出途经点，基于构建模块利用任意的至少两个途经点构建曲率连续的曲线。可见，本发明在初始路径的基础上进行优化，能够得到经过初始路径中相应点的曲率连续的新路径，从而使得运动路径更加平滑，满足移动机器人运动时效及硬件性能方面的需求。对于相关部分的具体说明请参见上述方法实施例，在此不再赘述。

本发明实施例提供的一种路径规划装置，选取模块可以包括：

选取单元，用于：基于初始路径中各点曲率从初始路径中选取至少一个点。

本发明实施例提供的一种路径规划装置，选取单元可以包括：

选取子单元，用于：从初始路径中选取出曲率变号点和/或曲率极值点。

本发明实施例提供的一种路径规划装置，构建模块可以包括：

第一构建单元，用于：选取任意的两个相邻的途经点；

第二构建单元，用于：构建经过两个相邻的途经点的二次B样条曲线。

本发明实施例提供的一种路径规划装置，起点为移动机器人的当前位置，终点为移动机器人前瞻距离范围内的最远点或者局部路径的终点；相应的，路径规划装置还可以包括：

循环模块，用于：每经过预设时间间隔和/或预设距离间隔则指示获取模块执行获取起点、终点间的初始路径的步骤。

本发明实施例提供的一种路径规划装置，起点和终点分别为全局路径中的起点及终点。

本发明实施例提供的一种路径规划装置，还可以包括：

判断模块，用于：构建经过至少两个途经点的曲率连续的曲线之后，如果构建的曲线是与障碍物发生碰撞的曲线，则向远离障碍物的方向移动至少两个途经点，并指示构建模块重新构建相应的曲线。

本发明实施例提供的一种路径规划装置，判断模块可以包括：

移动单元，用于：基于障碍物的代价地图，向远离障碍物的方向移动至少两个途经点。

本发明实施例还提供了一种计算机程序产品，计算机程序产品包括计算机程序，计算机程序在被处理器执行时，使得处理器实施如上任一项实施例所述的路径规划方法。

在本发明中，计算机程序产品可存储在计算机可读存储介质中。计算机可读存储介质例如可包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡，安全数字卡，闪存卡、至少一个磁盘存储器件、闪存器件、或其它易失性固态存储器件。处理器可以是中央处理单元，还可以是其它通用处理器、数字信号处理器、专用集成电路、现成可编程门阵列或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者也可以是任何常规的处理器等。

对所公开的实施例的上述说明，使本领域技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (12)

1.一种路径规划方法，其特征在于，包括：

获取起点、终点间的初始路径；

基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，并将选取的点与所述起点、所述终点均作为途经点；

选取任意的至少两个途经点，构建经过所述至少两个途经点的曲率连续的曲线，作为所述至少两个途经点对应的新路径。

2.根据权利要求1所述的方法，其特征在于，基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，包括：

基于所述初始路径中各点的曲率从所述初始路径中选取至少一个点。

3.根据权利要求2所述的方法，其特征在于，基于所述初始路径中各点的曲率从所述初始路径中选取至少一个点，包括：

从所述初始路径中选取出曲率变号点和/或曲率极值点。

4.根据权利要求1所述的方法，其特征在于，选取任意的至少两个途经点，包括：

选取任意的两个相邻的途经点。

5.根据权利要求4所述的方法，其特征在于，构建经过所述至少两个途经点的曲率连续的曲线，包括：

构建经过所述两个相邻的途经点的二次B样条曲线。

6.根据权利要求1所述的方法，其特征在于，所述起点为移动机器人的当前位置，所述终点为所述移动机器人前瞻距离范围内的最远点或者局部路径的终点。

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

每经过预设时间间隔和/或预设距离间隔则执行所述获取起点、终点间的初始路径的步骤。

8.根据权利要求1所述的方法，其特征在于，所述起点和终点分别为全局路径中的起点及终点。

9.根据权利要求1至8任一项所述的方法，其特征在于，构建经过所述至少两个途经点的曲率连续的曲线之后，还包括：

如果构建的曲线是与障碍物发生碰撞的曲线，则向远离所述障碍物的方向移动所述至少两个途经点，并重新构建相应的曲线。

10.根据权利要求9所述的方法，其特征在于，向远离所述障碍物的方向移动所述至少两个途经点，包括：

基于所述障碍物的代价地图，向远离所述障碍物的方向移动所述至少两个途经点。

11.一种路径规划装置，其特征在于，包括：

获取模块，用于：获取起点、终点间的初始路径；

选取模块，用于：基于所述初始路径的轨迹特点从所述初始路径中选取至少一个点，并将选取的点与所述起点、所述终点均作为途经点；

构建模块，用于：选取任意的至少两个途经点，构建经过所述至少两个途经点的曲率连续的曲线，作为所述至少两个途经点对应的新路径。

12.一种计算机程序产品，其特征在于，所述计算机程序产品包括计算机程序，所述计算机程序在被处理器执行时，使得处理器实施权利要求1-10中的任一项所述的路径规划方法。