1 回顾
上一篇文章中，我们介绍了如何使用OMPL进行单刚体运动规划。其中包含以下步骤：
· 定义状态空间的类型：我们在SE(3)、SO(3)还是 中进行规划
· 定义状态有效性检查方法：即碰撞检测，需要自己设计或使用别的库，因为OMPL本身没有实现碰撞检测功能
· 定义规划问题：设置规划起点和终点
· 选择规划器：使用哪种的规划方法？OMPL支持的规划方法
我们知道，OMPL所提供的大部分方法都是是基于采样的方法。这其中就涉及到一个问题：我们应该使用什么样的采样器呢？OMPL默认使用均匀采样。但有时，选择另一个采样器，甚至自行定义一个采样器也许会更好，下面通过一个实例来看看。
2 提出问题
考虑如下问题：有一个三维空间 ，需要在
区域内进行运动规划，起始状态为[0,0,0]，目标状态为[0,0,1]，如下区域存在障碍物：
障碍物区域用透明化的红色线框表示，起始状态和目标状态用绿色点表示：
此时，我们假设有三种不同的采样策略：
1.均匀采样。从空间中完全随机采样，通过有效性检测去除无效的采样点。
2.基于障碍物的采样[2]。在这种方法中，所有采样点都紧贴着障碍区域。这是OMPL自带可选的采样方法。
3.只在无障碍区域采样。同样也是均匀采样，但只在无障碍区域中进行。我们自己来实现这个采样方法。
那么，采取这三种策略，会给我们的规划过程和结果带来哪些不同呢？让我们通过示例来看看吧。
3 实现与分析
本篇文章的源代码见OMPL的自带示例文件：ompl/demos/StateSampling.cpp，也可见：
如果已经成功安装并编译了ompl库（如何安装？见【OMPL】安装与介绍），在ompl-1.4.2-Source/build/Release/bin/中将会看到大量示例可执行文件，在该目录下打开终端，运行：
./demo_StateSampling
对代码和运行结果分析如下
3.1 状态有效性检查
和上一篇文章一样，我们需要自行实现状态有效性检查，这里就依照问题中的障碍区域进行检查。其中，我们为每一次检查增加了一些时间消耗，这样有助于真实体现不同采样方法在时间成本上的区别（因为真实情况下的障碍区域远不止题目中这样简单，碰撞检测方法也相应更为复杂）。状态有效性检查在isStateValid()函数中实现。
bool isStateValid(const ob::State *state)
{
const ob::RealVectorStateSpace::StateType& pos = *state->as<ob::RealVectorStateSpace::StateType>();
// 为每一次检查增加了一些时间消耗，这样有助于体现不同采样方法在时间成本上的区别
std::this_thread::sleep_for(ompl::time::seconds(.0005));
// 按照预设的障碍物情况进行有效性检查
// -1<= x,y,z <=1
// if .25 <= z <= .5, then |x|>.8 and |y|>.8
return !(fabs(pos[0])<.8 && fabs(pos[1])<.8 && pos[2]>.25 && pos[2]<.5);
}
3.1 均匀采样
均匀采样是OMPL默认的采样方法。完全按照上一篇文章的步骤实现即可，区别是状态空间需要指定为 空间：
void plan(int samplerIndex)
{
// 指定状态空间的类型
auto space(std::make_shared<ob::RealVectorStateSpace>(3));
...
}
示例输出的第1部分是均匀采样的规划结果，我们可以看到以下结论：
· 系统采用了PRM即概率路线图方法进行规划
· 均匀采样方法共生成了61个状态
· 规划过程共消耗4.278275秒
· 找到了3个中间状态，加上起始和目标状态，整条路径共5个状态
Using default uniform sampler:
Info: PRM: Starting planning with 2 states already in datastructure
Info: PRM: Created 61 states
Info: Solution found in 4.278275 seconds
Found solution:
Geometric path with 5 states
RealVectorState [0 0 0]
RealVectorState [-0.401632 0.0143846 -0.0341568]
RealVectorState [-0.992403 0.313683 0.183282]
RealVectorState [-0.64365 0.661452 0.722154]
RealVectorState [0 0 1]
根据规划结果绘图如下，为了便于观察，障碍区域已经透明化：
3.2 基于障碍的采样
allocOBValidStateSampler()函数分配了一个基于障碍的采样器：
ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si)
{
return std::make_shared<ob::ObstacleBasedValidStateSampler>(si);
}
在规划函数plan()中通过如下方法确认使用基于障碍的采样方法（这里使用了简单规划类SimpleSetup，详见【OMPL】单刚体运动规划演示）
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
示例输出的第2部分是基于障碍的采样的规划结果：
Using obstacle-based sampler:
Info: PRM: Starting planning with 2 states already in datastructure
Info: PRM: Created 29 states
Info: Solution found in 1.989092 seconds
Found solution:
Geometric path with 5 states
RealVectorState [0 0 0]
RealVectorState [-0.727578 0.197752 0.244763]
RealVectorState [-0.929759 0.165892 -0.268329]
RealVectorState [-0.812952 0.231858 0.570833]
RealVectorState [0 0 1]与均匀采样不同之处有：
· 基于障碍的采样共生成了29个状态，比均匀采样方法的61个少
· 规划过程共消耗1.989092秒，比均匀采样方法的4.278275秒少
根据规划结果绘图如下，为了便于观察，障碍区域已经透明化。可以看到，所有路径点都紧贴障碍区域（有一个点似乎离障碍区域很远？不要忘了，边界以外也是障碍区域）：
3.3 只在无障碍区域采样（自定义采样）
通过MyValidStateSampler类，我们实现了一个只在本题目所规定的无障碍区域的进行采样的采样器，其中关键代码如下：
class MyValidStateSampler : public ob::ValidStateSampler
{
...
bool sample(ob::State *state) override
{
...
if (z>.25 && z<.5)
{
double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
switch(rng_.uniformInt(0,3))
{
case 0: val[0]=x-1; val[1]=y-1; break;
case 1: val[0]=x-.8; val[1]=y+.8; break;
case 2: val[0]=y-1; val[1]=x-1; break;
case 3: val[0]=y+.8; val[1]=x-.8; break;
}
}
else
{
val[0] = rng_.uniformReal(-1,1);
val[1] = rng_.uniformReal(-1,1);
}
val[2] = z;
...
}
...
};
在规划函数plan()中通过如下方法确认使用基于障碍的采样方法（这里使用了简单规划类SimpleSetup，详见【OMPL】单刚体运动规划演示）
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);
示例输出的第3部分是自定义采样的规划结果：
Using my sampler:
Info: PRM: Starting planning with 2 states already in datastructure
Info: PRM: Created 11 states
Info: Solution found in 0.685157 seconds
Found solution:
Geometric path with 5 states
RealVectorState [0 0 0]
RealVectorState [-0.928433 0.298254 0.0035227]
RealVectorState [-0.88657 0.291854 0.421643]
RealVectorState [-0.712372 0.759317 0.601411]
RealVectorState [0 0 1]
与其它两种方法的不同之处有：
· 采样共生成了11个状态，是所有方法中最少的
· 规划过程共消耗0.685157秒，也是所有方法中最少的
根据规划结果绘图如下，为了便于观察，障碍区域已经透明化：
4 结论
从以上结果看出，利用自定义方法，只在无障碍区域内采样，虽然普适性不如其它两种方法，但生成状态数量最少，所消耗的时间也是最少的。（但必需提到的是，采样始终存在随机性，这个结论有时也会出现例外的）
参考资料与注释
[1] 官网示例
[2] 基于障碍物的采样方法：N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo, OBPRM: an obstacle-based PRM for 3D workspaces, in Third Workshop on the Algorithmic Foundations of Robotics, pp. 155-168, 1998.
