OMPL 入门Tutorial 7：基于优化的规划(Optimal Planning)

以下内容为OMPL官方Tutorial Optimal planning 的翻译

定义一个基于优化的运动规划问题几乎与常规的问题定义一样，除了以下两点区别：

• 需要给ompl::base::ProblemDefinition定义优化目标
• 需要一个优化规划器来做真正的运动规划

找寻最短路径

我们将通过一个例子演示OMPL的最优路径规划框架。这个例子中，机器人被表示为一个方形区域上的(x,y)坐标点，其中(0,0)是方形的左下角，(1,1)是方向的右上角。方形区域中有一个中心为(0.5, 0.5)，半径为0.25的障碍物。为了表示这个环境，我们用到了二维ompl::base::RealVectorStateSpace并按照如下方式定义了我们的可行状态检测：

// 碰撞检测。在这个例子中，机器人的状态空间位于[0,1]x[0,1]，其中有一个半径为0.25，中心位于(0.5,0.5)的圆。 任何位于这个圆中的状态都认为是碰撞的。
class ValidityChecker : public ob::StateValidityChecker
{
public:
    ValidityChecker(const ob::SpaceInformationPtr& si) :
        ob::StateValidityChecker(si) {}
 
    // 返回给定的状态位置是否与圆形障碍重合
    bool isValid(const ob::State* state) const
    {
        return this->clearance(state) > 0.0;
    }
 
    // 返回给定的状态与原型障碍物的举例
    double clearance(const ob::State* state) const
    {
        // 我们在这个例子中用到的是RealVectorStateSpace，因此我们将state装换为对应的类型
        const ob::RealVectorStateSpace::StateType* state2D =
            state->as<ob::RealVectorStateSpace::StateType>();
 
        // 从状态中提取(x,y)位置坐标
        double x = state2D->values[0];
        double y = state2D->values[1];
 
        // 两点之间距离公式，增加了一个圆圈半径的偏移
        return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
    }
};

在我们的路径规划代码中，我们定义了状态空间及其边界，和我们的起始及目标状态。在这个例子中，起始状态为(0,0)，目标状态为(1,1)。如果你已经使用过OMPL一般的运动规划代码，那么下面的代码应该对你来说很熟悉。

// 创建机器人状态空间[0,1]x[0,1], R^2的子集.
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
 
// 设定空间边界 [0,1].
space->as<ob::RealVectorStateSpace>()->setBounds(0.0, 1.0);
 
// 为这个状态空间创建空间信息
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
 
// 设定有效状态检测函数
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
 
si->setup();
 
// 设置机器人初始位置为(0,0)
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
 
// 设置机器人目标位置为(1,1)
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
 
// 创建问题实例
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
 
// 设定起点终点
pdef->setStartAndGoalStates(start, goal);

接下来，我们需要给优化规划定义一个ompl::base::OptimizationObjective。现在，我们希望定义一个目标函数为起点与终点之间的最短距离。我们将定义另一个函数返回这个目标值

// 返回表示最优运动规划的优化目标。这个方法返回一个试图最小化路径的objective。
ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
{
    return ob::OptimizationObjectivePtr(new ob::PathLengthOptimizationObjective(si));
}

然后就可以设置目标函数了

pdef->setOptimizationObjective(getPathLengthObjective(si));

注意：我们所用的优化规划器，ompl::geometric::RRTstar，默认为路径长度优化。也就意味着对于这个例子而言，其实上面的声明是不必要的。但如果你想定义一个其他的优化指标，则需要手动的给ompl::base::ProblemDefinition设定

接下来，我们定义优化规划器并尝试求解问题：

// 用RRTstar算法建立优化规划器
ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));
 
// 为我们的规划器设置问题定义
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
 
// 尝试在1s时间内求解问题
ob::PlannerStatus solved = optimizingPlanner->solve(1.0);

这里有一个动图演示RRTstar规划器求解上述定义问题结果（见原文链接）

注意，当试图求解优化问题的时候，我们设定时间限制为1秒。在常规运动规划中，规划器会在找到解后停止，这会用远小于1秒的时间。然而，如果你运行这个例子就会发现他总是使用了完整的1秒时间。这是因为优化求解器有一个更严格的终止条件。常规规划器在找到解之后停止；然而优化规划器需要在找到满足优化目标的解才会停止。

定义优化阈值

如何定义满足优化目标？这个行为可以个人定制，但默认是寻找最短的满足一些质量阈值的路径。对于最短路径规划而言，这意味着寻找一条比给定长度更短的路径。你可能会发现我们之前从未定义这个阈值，因为如果没有设置的话，ompl::base::PathLengthOptimizationObjective会默认将threshold设置为0.0。这就意味着只有长度小于0.0的路径才满足目标，那永远不可能满足。这么做的原因是我们假设如果你希望在给定时间内返回找到的最短的路径。因此，threshold设为0.0永远不能被满足，并保证规划器一直运行到时间上限，并返回他能找到的最优路径。

我们可以通过setCostThreshold方法创建一个质量阈值为1.51的OptimizationObjective：

ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
{
    ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
    obj->setCostThreshold(ob::Cost(1.51));
    return obj;
}

如果你将这个设置为ompl::base::ProblemDefinition的目标，你会发现规划器会终止得快很多，因为只要他找到一个满足阈值的路径就会返回了。注意当调用ompl::base::OptimizationObjective::setCostThreshold时，我们将阈值放在ompl::base::Cost中。后续会讲解更多关于ompl::base::Cost对象的内容，但现在你可以简单地把他当做一个用double值所表示路径cost的对象。