CN110045732B - 一种启发式路径规划方法 - Google Patents

Abstract

本发明公开了一种启发式路径规划方法，具体包括以下步骤：获取路径起点和终点；计算运行空间速度场，生成部分路径，计算已有路径的点的曲率值；基于容许启发函数，利用A*算法计算规划最优路径。本发明可控制机器人运行的速度，根据障碍物的大小生成更加符合人类行走的路径，保证机器人在行走时与周围环境交互时的安全性。

Description

一种启发式路径规划方法

技术领域

本发明属于机器人技术领域，特别涉及一种人类行走经验启发的路径规划方法。

背景技术

机器人路径规划是寻找给定两点之间的一条距离最短或者其它最优准则下的路径，且沿着这路径路行走不会和其它的物体发生碰撞。计算路径的传统方法一般是设计一个能量函数，用此能量函数指导最优路径的计算，而计算的方法采用常见的优化方法。人工势场法认为障碍物具有排斥力，排斥机器人靠近障碍物，而目标位置对机器人具有吸引力，其吸引机器人走向目标位置。人工势场法通过建立引力和斥力间的势场，进行路径搜索。人工势场法计算出的路径一般安全而且光滑，但是其容易陷入局部最优解，也即存在一些不动点，当机器人走到这些位置时，算法会认为已经找到了目标位置，而实质上这些点却是一些伪目标点。基于能量函数的方法还包括整数规划，深度优先，广度优先，最佳优先，神经网络等路径规划方法。

基于几何方法的传统路径规划算法一般将机器人运动的空间划分为一些细小的几何单元(如多边形)，然后构造机器人运动的无碰撞空间，然后在此无碰撞空间中寻找最优路径，典型的方法包括：自由空间法，Voronoi图方法，可视图方法等。路径图法一般先构建一个路径图网，然后在此网络中搜索最优路径,而且在寻找完最优路径后，程序还会将搜索到的路径做进一步的平滑，使机器人行走的路径比较顺畅自然。但是基于几何的方法一般本身不具备搜索功能，需要结合其它的搜索方法一起使用。

因为路径规划问题实质是一个NP问题，传统方法对于NP问题一般都会出现诸如计算复杂性高，计算时间长，易陷入局部极小值点等问题，特别地，当机器人处于动态复杂环境中时，传统方法更是难于处理。启发式方法是为了克服传统方法在处理路径规划所面临的困境的基础上发展而来的。

模拟退火算法由Alfaro和Garcia于1998年首次引入到机器人路径规划领域，使得大场景的路径规划问题的求解成为可能，但是此方法收敛速度较慢，而且搜索得到的路径具有随机性。基于增强的模拟退火方法搜索动态环境下的最优路径，部分地克服了模拟退火算法的弱点。禁忌搜索应用于路径规划的能够有效地避免陷入局部最优点。蚁群优化方法是另一种有效地最优路径计算方法,如SCAO-MH(Simple Ant Colony OptimizationMeta-Heuristic)方法存储一张已访问节点信息表的方法，能够有效地克服蚁群算法处理路径规划时的局部极值点问题。概率路径图法构建了一个启发函数，然后根据此启发函数寻找最优路径。另外，粒子群方法在路径规划中也有广泛的应用。上述的这些方法都可视为元启发式方法，这些方法的优点是能够高效地获得路径解，但是这些方法一般都是随机的，故不能获得唯一解，而且很多时候它们也并不能获得实时解。

目前，在路径规划中用的较多的启发式算法当属A*算法及其变种。A*算法一般是建立一个优化目标函数，且此优化目标函数能够转化为启发函数，然后用A*算法求解此优化函数。A*方法是在Dijkstra算法的基础之上发展而来的，其中心思想是利用启发函数减少搜索过程中的节点数。因此，一般地A*方法要比Dijkstra算法要快。

在动态环境中，如出现新的障碍物，D*算法和Focussed D*算法仍然能够计算出最优路径。但是D*算法需要消耗较大的内存，且算法原理晦涩，实现也较难。LPA*算法第一次运行时和A*算法没有区别，但是其随后的计算速度要快很多，因为它利用了以前的计算结果。因此，LPA*算法要求起始点位置不能改变。D*Lite算法发明的目的是为了减轻D*算法的理解和编程实现，但是实验证明其速度至少和D*算法一样。

任意角度A*算法不再约束机器人在格子间运动，此时机器人可以按照任意角度方向行驶，而不再要求是45°或90°，其代表方法有Field D*算法，Theta*算法，IncrementalPhi*方法等。

当目标移动时，前述的A*搜索方法虽然仍然有效，但是它们的性能大打折扣，因此需要设计专门的A*变种算法处理此类问题，其中的代表方法包括：广义自适应A*算法，移动目标的D*-Lite方法，广义端-检索A*算法(Generalized Fringe-RetrievingA*)，树自适应A*算法等。

在有些情况下，留给***做决策的时间非常有限。比如说，为了将一架直升机非礼禁飞区，飞行***要给出一个极其快速的解决方案以避免任何事故发生，而这个解决方案并不需要找到一条到达目标位置的最优路径。在此种情形下，任意时间路径规划(anytimepathplanning)A*算法能够快速地寻找到一个次优解。分层路径寻找A*算法(HPA*)将路径规划问题分解为若干子问题，使其具有较高的计算性能。任意时间D*(anytime D*)算法吸收任意时间算法和增量算法的优点，使其能够处理复杂动态问题。

还有一类重要的启发式算法是快速探索随机树方法(Rapidly-exploring randomtrees,RRT)，已有结果表明RRT算法计算速度非常快，而且也是概率完备的(probabilistically complete)。

与本发明最相近的技术是Voronoi场路径规划方法，此方法也考虑了安全性因素，但无法控制机器人运行时的速度，无法根据障碍物的大小，生成更加符合人类行走的路径。

总之，当前的路径规划技术的缺点包括：1、容易得到局部最优解；2、最终计算出的路径不确定，也即运行同一算法会出现不同的最优路径；3、路径习惯于贴近障碍物边缘，安全性能差，没有考虑到机器人在路径上行走所带来的安全交互问题所带来安全隐患；4，没有考虑到人类的行走经验。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提供一种启发式路径规划方法，其可控制机器人运行的速度，根据障碍物的大小生成更加符合人类行走的路径，保证机器人在行走时与周围环境交互时的安全性。

本发明的目的通过以下的技术方案实现：一种启发式路径规划方法，包括以下步骤：

获取路径起点和终点；

计算运行空间速度场，生成部分路径，计算已有路径的点的曲率值；

基于容许启发函数，利用A*算法计算规划最优路径。

优选的，采用质心Voronoi剖分法(CVT，Centroid Voronoi Tessellations)将机器人的***空间剖分为若干多边形单元的集合，使得靠近障碍物的多边形单元的面积较小，而远离障碍物的多边形单元的面积较大。则根据下式，对于每个靠近障碍物的多边形，控制机器人在质心点的速度较小，而对于每个远离障碍物的多边形，控制机器人在质心点的速度较大，这样通过CVT的剖分方式，达到控制机器人速度的目的。令质心c_i所属的多边形单元区域的面积为A_i，其对应的速度为v_i，而面积最大的多边形单元的面积记为A_max，则每个质心位置的速度可以建立为：

其中w＜1，是一个常数；V_max＞0，表示机器人的最大运动速度；对于其它的非质心点位置x，采用质心速度的高斯核函数权重的线性加权方法，计算其相应的速度；则运行空间的速度场v(x)：

其中，c＝{x₀＝x_s,x₁,…,x_n＝x_g}表示路径；x_i表示路径上的点；x_s和x_g分别表示路径上的起点和终点；σ＞0，是高斯核函数的带宽，控制高斯核函数的径向作用范围。

更进一步的，对于点云曲面，令点云曲面上运动的最大速度为V_max，将每点的运动速度定义为：

其中w＜1，是一个常数；H_min是绝对最小平均曲率值；H(x_i)是位置x_i的平均曲率值；ε＞0，是一个较小的常数。

优选的，所述路径曲率采用近似曲率计算方法，代替计算复杂、计算量大的解析计算格式，具体为：

其中x_i表示路径上的点；i＝0,1,2,…,n；x_s和x_g分别表示路径上的起点和终点；α是向量

和

的夹角；当i＝0时，k(x_i)＝1。

更进一步的，对于点云曲面，用平均曲率表示能量函数中的路径曲率。

优选的，所述A*算法选择的路径极小化目标函数f(x)＝g(x)+h(x),x表示位置，g(x)表示从起始位置x_s到当前位置x的能量消耗，而容许启发函数(admissible heuristicfunction)h(x)度量的是从当前位置x到目标位置x_g的能量消耗的下界；A*算法的关键是设计一个合理的容许启发函数h(x)，使得h(x)的值不能超过实际从x到目标位置x_g的的消耗；

启发函数h(x)的定义如下：

其中λ₁,λ₂＞0为两个控制参数。令机器人已行驶过的轨迹点为x₀＝x_s,x₁,...,x_M，且x_M＝x，则从起始位置x_s到当前位置x的能量函数g(x)定义为：

其中S(x)表示x到x_s的折线段长度，即x到x_s的欧式距离，λ₁,λ₂＞0为两个控制参数。只要两点之间有可行路径链接，A*算法总能够在机器人***空间中搜索到一条最优路径。

更进一步的，对于点云曲面，A*算法的容许启发函数定义为：

其中，H_min表示点云上所有点的平均曲率的绝对最小值，v_min表示所有点的最小速度，λ₁,λ₂＞0为两个控制参数。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明从人们的驾驶经验出发，建立符合人类行车经验和***滑性约束，得到全局最优解。

2、本发明采用近似曲率计算方案代替解析计算格式，简化计算的同时拟合出未生成的路径，以达到预估路径的目的。

3、本发明不仅可求得2D平面路径最优解，还基于点云曲面的平均曲率求得点云曲面上任意两点的最优路径。

附图说明

图1是本发明的路径规划方法流程图。

图2为基于不同尺寸的障碍物的路径规划结果对比图，(a)当障碍物Q较小时，本发明规划的路径为虚线轨迹，实线是最短路径；(b)当障碍物Q较大时，本发明规划的路径为虚线轨迹，绿色实线是最短路径。

图3为模糊逻辑路径规划结果与本发明路径规划结果对比图，其中(a)为基于模糊逻辑方法的路径规划结果，(b)为本发明的路径规划结果。

图4为本发明在室内场景中的路径规划结果。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例1

一种路径规划方法，具体包括以下步骤：

S1、取路径起点x_s和终点x_g；令路径为c＝{x₀＝x_s,x₁,…,x_n＝x_g}，x_i表示路径上的点；

S2、Voronoi剖分法(CVT，Centroid Voronoi Tessellations)将机器人的***空间剖分为若干多边形单元的集合，使得靠近障碍物的多边形单元的面积较小，而远离障碍物的多边形单元的面积较大。则根据下式，对于每个靠近障碍物的多边形，控制机器人在质心点的速度较小，而对于每个远离障碍物的多边形，控制机器人在质心点的速度较大，这样通过CVT的剖分方式，达到控制机器人速度的目的。令质心c_i所属的多边形单元区域的面积为A_i，其对应的速度为v_i，而面积最大的多边形单元的面积记为A_max，则每个质心位置的速度可以建立为：

其中w＜1，是一个常数；V_max＞0，表示机器人的最大运动速度；对于其它的非质心点位置x，采用质心速度的高斯核函数权重的线性加权方法，计算其相应的速度；则运行空间的速度场v(x)：

其中σ＞0，是高斯核函数的带宽，控制高斯核函数的径向作用范围。

S3、分路径，计算已有路径的点的曲率值；

所述路径曲率采用近似曲率计算方法，代替计算复杂、计算量大的解析计算格式，具体为：

其中x_i表示路径上的点；i＝0,1,2,…,n；x_s和x_g分别表示路径上的起点和终点；α是向量

和

的夹角；当i＝0时，k(x_i)＝1。

S4、许启发函数，利用A*算法计算规划最优路径。

所述A*算法选择的路径极小化目标函数f(x)＝g(x)+h(x),x表示位置，g(x)表示从起始位置x_s到当前位置x的能量消耗，而容许启发函数(admissible heuristicfunction)h(x)度量的是从当前位置x到目标位置x_g的能量消耗的下界；A*算法的关键是设计一个合理的容许启发函数h(x)，使得h(x)的值不能超过实际从x到目标位置x_g的的消耗；

启发函数h(x)的定义如下：

其中λ₁,λ₂＞0为两个控制参数。容易证明，如此定义的h(x)确实是从x到目标位置x_g的一个下界。而g(x)的定义可以如前能量函数的定义。令机器人已行驶过的轨迹点为x₀＝x_s,x₁,...,x_M，且x_M＝x，则从起始位置x_s到当前位置x的能量函数g(x)定义为：

其中s(x)表示x到x_s的折线段长度，即x到x_s的欧式距离，λ₁,λ₂＞0为两个控制参数。只要两点之间有可行路径链接，A*算法总能够在机器人***空间中搜索到一条最优路径。

本发明已经经过了仿真模拟实验，实验结果见图2-4，可以看出本发明的路径规划方法生成的路径更加平滑自然，更符合人类行走习惯。

实施例2

一种点云曲面路径规划方法，将能量函数中的曲率用平均曲率替代，则其能量函数定义为：

其中v是速度，H(x)是x点的平均曲率，λ₁,λ₂＞0为两个控制参数，对于点云曲面上的每点的平均曲率，已有许多计算方法，在此不再多述。

令点云曲面上运动的最大速度为V_max，考虑到平坦区域运动速度较快，而陡峭区域运动较慢，因此将每点的运动速度定义为：

其中w＜1是一个常数；H_min是绝对最小平均曲率值；H(x_i)是位置x_i的平均曲率值；ε＞0是一个较小的常数。

其对应的A*算法的容许启发函数的定义与2维地图上的A*算法的容许启发函数的定义略有不同，其定义如下：

其中H_min表示点云上所有点的平均曲率的绝对最小值，v_min表示所有点的最小速度，λ₁,λ₂＞0为两个控制参数。

除上述区别外，实施例2的步骤同实施例1。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (5)

1.一种启发式路径规划方法，其特征在于，包括以下步骤：

获取路径起点和终点；

计算运行空间速度场，生成部分路径，计算已有路径的点的曲率值；

基于容许启发函数，利用A*算法计算规划最优路径；

采用质心Voronoi剖分法将机器人的***空间剖分为若干多边形单元的集合，使得靠近障碍物的多边形单元的面积较小，而远离障碍物的多边形单元的面积较大；令质心c_i所属的多边形单元区域的面积为A_i，其对应的速度为v_i，而面积最大的多边形单元的面积记为A_max，则每个质心位置的速度可以建立为：

其中w<1，是一个常数；V_max>0，表示机器人的最大运动速度；对于其它的非质心点位置x，采用质心速度的高斯核函数权重的线性加权方法，计算其相应的速度；则运行空间的速度场v(x)：

其中，c＝{x₀＝x_s,x₁,…,x_n＝x_g}表示路径；x_i表示路径上的点；x_s和x_g分别表示路径上的起点和终点；σ＞0，是高斯核函数的带宽，控制高斯核函数的径向作用范围；

所述A*算法选择的路径极小化目标函数f(x)＝g(x)+h(x),x表示位置，g(x)表示从起始位置xs到当前位置x的能量消耗，而容许启发函数h(x)度量的是从当前位置x到目标位置xg的能量消耗的下界；A*算法的关键是设计一个合理的容许启发函数h(x)，使得h(x)的值不能超过实际从x到目标位置xg的的消耗；

启发函数h(x)的定义如下：

其中λ₁,λ₂＞0为两个控制参数；令机器人已行驶过的轨迹点为x0＝xs,x1,...,xM，且xM＝x，则从起始位置xs到当前位置x的能量函数g(x)定义为：

其中S(x)表示x到xs的折线段长度，即x到xs的欧式距离，λ₁,λ₂＞0为两个控制参数；只要两点之间有可行路径链接，A*算法总能够在机器人***空间中搜索到一条最优路径。

2.根据权利要求1所述的启发式路径规划方法，其特征在于，对于点云曲面，令点云曲面上运动的最大速度为V_max，将每点的运动速度定义为：

其中w<1,是一个常数；H_min是绝对最小平均曲率值；H(x_i)是位置x_i的平均曲率值；ε＞0,是一个较小的常数。

3.根据权利要求1所述的启发式路径规划方法，其特征在于，所述路径曲率采用近似曲率计算方法，代替计算复杂、计算量大的解析计算格式，具体为：

其中x_i表示路径上的点；i＝0,1,2,…,n；x_s和x_g分别表示路径上的起点和终点；α是向量

和

的夹角；当i＝0时，k(x_i)＝1。

4.根据权利要求3所述的启发式路径规划方法，其特征在于，对于点云曲面，用平均曲率表示能量函数中的路径曲率。

5.根据权利要求1所述的启发式路径规划方法，其特征在于，对于点云曲面，A*算法的容许启发函数定义为：

其中，H_min表示点云上所有点的平均曲率的绝对最小值，v_min表示所有点的最小速度，λ₁,λ₂＞0为两个控制参数。

Images