【OMPL】最优化之最短路径规划（4）

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

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

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

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

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

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

2 提出问题

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

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，规划结果如图所示。可以看到，由于采样点太少，因此结果明显并非最短路径。

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

·   规划使用了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并编译工程

编译完成后，将在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】单刚体运动规划演示中的完整规划完全一致，下面简单分析代码中涉及优化器和规划器的部分：

plan()函数中：

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

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

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

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

资料

[1] 官网示例

[2] 源代码与项目地址