【OMPL】最优化之最短路径规划(4)

325
0
2020年4月27日 17时42分

1 回顾与引出——什么是最短路径?

 

上一篇文章中,我们尝试了基于三种不同的采样方法的PRM运动规划。其中自定义的无障碍空间采样器在效率上的表现最为优异,因为我们无需浪费过多的时间在状态有效性检查上。但有时,我们需要的不仅是更少的规划时间,还有更优的规划结果,例如,路径长度越短越好。这里需要解释一下所谓“最短路径”的概念:

 

在图论中,我们用A*算法来搜索无向图的最短路径。如果读者对RRT或者PRM算法有所了解(详见本专栏早期文章)的话,就应该知道,这类采样算法其实就是“采样 + A*搜索”的组合,而A*算法能够保证找到图中的最短路径。既然如此,为什么我们还需要再次寻找最短路径呢?

 

请看下图,这是一个简单二维空间PRM算法规划结果,其中紫色为障碍区域,浅蓝色细线与绿色节点为采样器生成的无向图,红色为A*算法找到的最短路径。但这究竟是不是真实的最短路径呢?

 

【OMPL】最优化之最短路径规划(4)插图

 

我们增加采样的密度,重新使用PRM规划得到如下结果。可以看到,同一个任务中,不同的采样密度,竟然使最短路径发生了变化!

 

【OMPL】最优化之最短路径规划(4)插图(1)

 

 

因此我们知道了,简单的PRM算法,其实是利用A*等搜索算法来寻找图模型中的最短路径,由于图模型只是真实空间的近似,因此结果往往并非真实空间中的最短路径。二者的接近程度,取决于采样器所建立图模型的合理程度。因此,通过优化方法来不断优化图模型(增加采样点),我们可以让规划结果不断接近真实的最短路径,这就是本篇文章所指的“最短路径”。读者需要与A*算法等图搜索算法所指代的“最短路径”区分开。

2 提出问题

考虑如下规划问题:在 [公式] 空间中存在圆心坐标[0.5, 0.5, 0.5],半径0.25的球形障碍物,以[0, 0, 0]为起始点,[1, 1, 1]为目标点进行运动规划,要求路径长度尽可能更短。状态空间如下图所示,其中障碍区域用红色表示,起始点和目标点用绿色表示。可以想象,最短路径一定是一条与障碍区域相切的路径。

【OMPL】最优化之最短路径规划(4)插图(3)

 

OMPL使用了路径长度优化器来寻找最短路径,简而言之,就是以路径总长度为优化指标,在规划过程中不断增加采样点,以寻找长度更短的路径。直到超过预设规划时间,或减小到期望的路径长度阈值。

3 规划与分析

代码修改自OMPL库自带示例OptimalPlanning.cpp,该文件可选8种规划算法,并可选4种优化指标,因此略显复杂。我将其稍作简化,得到MiniPathLength.cpp,只使用了PRM算法,并只使用路径长度作为优化指标,优化停止条件为:达到最大优化时间。

 

简化版源码见:

https://github.com/XM522706601/robotics_tutorial_for_zhihu/blob/master/ompl_demo/MiniPathLength.cpp​github.com

 

3.1 无优化的规划

首先,我们不使用长度优化器,采用基本方法(见【OMPL】单刚体运动规划演示)进行规划,规划时间为1s,规划结果如图所示。可以看到,由于采样点太少,因此结果明显并非最短路径。

【OMPL】最优化之最短路径规划(4)插图(4)

 

系统输出信息如下。有以下重要信息:

·   规划使用了PRM方法

·   路径长度为1.8903

·   共采样了6877个状态,找到了2个中间状态(路径共4个状态)

 

Info:    PRM: Starting planning with 2 states already in datastructure
Info:    PRM: Created 6877 states
PRM found a solution of length 1.8903
RealVectorState [0 0 0]
RealVectorState [0.221384 0.207497 0.555206]
RealVectorState [0.476419 0.627542 0.915896]
RealVectorState [1 1 1]

3.2 有优化的最短路径规划

下载示例文件并编译

 

接下来,我们运行有优化的规划器来解决同一问题。首先将MiniPathLength.cpp放置到/ompl/demos/目录下,并修改该目录下的CmakeList.txt,在第14行左右增加代码:

 

add_ompl_demo(MiniPathLength MiniPathLength.cpp)

 

进入build/Release并编译工程

 

cd build/Release
cmake ../..

 

运行可执行文件

 

编译完成后,将在ompl/build/Release/bin/中生成可执行文件,在该目录下打开终端,运行:

./MiniPathLength -t 1 # 规划时间为1s

 

规划时间同样为1s,将得到如下结果,明显得到了更多中间状态,意味着路径的分段更加细腻。注意路径长度减小为了1.82195,明显好于不带优化器的规划。

 

Info:    PRM: Starting planning with 2 states already in datastructure
Info:    PRM: Created 7145 states
PRM found a solution of length 1.82195 with an optimization objective value of 1.82195
Geometric path with 10 states
RealVectorState [0 0 0]
RealVectorState [0.0534555 0.156167 0.14527]
RealVectorState [0.106474 0.277279 0.261043]
RealVectorState [0.129875 0.351931 0.341709]
RealVectorState [0.174937 0.422874 0.409814]
RealVectorState [0.193775 0.468501 0.435066]
RealVectorState [0.276276 0.638581 0.556293]
RealVectorState [0.456366 0.737632 0.678964]
RealVectorState [0.52502 0.793527 0.733191]
RealVectorState [1 1 1]

 

将规划结果绘图如下。路径明显紧贴障碍物,意味着比不带优化器时更加接近最短路径。

【OMPL】最优化之最短路径规划(4)插图(5)

 

代码的其余步骤和【OMPL】单刚体运动规划演示中的完整规划完全一致,下面简单分析代码中涉及优化器和规划器的部分:

 

plan()函数中:

pdef->setOptimizationObjective(std::make_shared<ob::PathLengthOptimizationObjective>(si));

 

该语句为本问题设置了一个长度优化器,来自于ompl库自带的路径长度优化类ob::PathLengthOptimizationObjective

 

另外,如下语句指定使用PRM规划算法。

ob::PlannerPtr optimizingPlanner = std::make_shared<og::PRM>(si);

资料

[1] 官网示例

[2] 源代码与项目地址

发表评论

后才能评论