OMPL 入门Tutorial 8：带约束的规划(Constrained Planning)

最新推荐文章于 2022-11-11 20:03:58 发布

Lbaron98

最新推荐文章于 2022-11-11 20:03:58 发布

阅读量777

点赞数

分类专栏： OMPL 路径规划文章标签：算法

本文链接：https://blog.csdn.net/Lbaron980810/article/details/126823492

版权

OMPL 同时被 2 个专栏收录

10 篇文章 8 订阅

订阅专栏

路径规划

10 篇文章 0 订阅

订阅专栏

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

定义一个受约束的运动规划问题很简单，而且也和无约束规划问题十分相近。主要的区别在于需要定义一个约束和受约束的状态空间的运用。在这个例子中，我们会实现定义一个简单地受约束的规划问题： $R^3$ 上的一个点别约束在一个球面空间上，给定约束方程为 $\Vert q \Vert - 1$ 。这与实例程序ConstrainedPlanningSphere定义的问题非常接近。

定义约束

首先，我们需要定义约束类（constraint class）。约束必须继承自基类ompl::base::Constraint。挂不是故意ompl::base::Constraint::function()必须要由约束的具体实现来施加。如下是一个球面约束的示例版本：

// 约束必须继承自基类。默认情况下，会用中心有限差分计算得到约束方程的Jacobian的一个数值逼近。
class Sphere : public ob::Constraint
{
public:
    // ob::Constraint的构造函数需要两个参数，周围状态空间的维数和约束映射到的实向量的空间维数。在我们这个球面的例子中，因为我们规划的是R^3空间，那么第一个参数就是3；我们的约束输出一个实数，那么第二个参数就是1（这也是约束流形的co-dimension）
    Sphere() : ob::Constraint(3, 1)
    {
    }
 
    // 这里我们定义实际的约束函数，它的输入是一些状态"x"（来自于周围的空间），然后将输出的值(out)设置为约束函数的结果。注意我们正在实现具有此函数签名（function signature）的`function`，而不是ompl::base::State的那个
    void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
    {
        // 如上所述，定义一个球的函数是f(q) = || q || - 1。Eigen让这个方程易于表达
        out[0] = x.norm() - 1;
    }
};

现在我们有一个在R^3空间中定义了一个球面的约束函数。接下来可以用这个约束来定义约束状态空间。

约束Jacobian

然而，我们希望做得更好。我们可以包含一个约束函数的解析的Jacobian，这样可以使规划求解更高效。我们可以手动计算，也可以用一些符号求解器（例如ConstraintGeneration.py 描述了如何用 SymPy实现）。因此我们给类添加了如下函数来计算Jacobian

// 实现约束函数的Jacobian是一个矩阵，其维度等于约束的co-dimension x 周围环境的维度（此例中为1x3）。与上面的`function`相同，我们用下面的函数签名来实现`jacobian`方法
void Sphere::jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
{
    out = x.transpose().normalized();
}

一般来说，高度推荐你给带约束的规划问题提供一个解析的Jacobian，尤其是对于高维问题。

投影

ompl::base::Constraint的主要特性之一就是投影函数，ompl::base::Constraint::project()。这个函数的输入为一个可能不满足约束的状态，然后将其映射到约束流形上，生成一个满足约束的状态。默认情况下，ompl::base::Constraint::project()采用牛顿方法，因为他在大多数场景下都很好用。

然而，也可以用你自己的方法覆盖。比如，在这个球形的例子中，可以单纯的归一化状态，将其放置于球面上。另一个例子是对一个复杂的机械臂用逆运动学。

约束参数

ompl::base::Constraint有两个会影响表现的参数。第一个是tolerance，可以通过ompl::base::Constraint::setTolerance()来设置，或者通过构造函数。用构造函数，Spher类可以定义如下：

...
    // 设置环境和约束的维数，以及tolerance
    Sphere() : ob::Constraint(3, 1, 1e-3)
...

tolerance用于ompl::base::Constraint::project()来判断一个状态是否满足约束，在ompl::bae::Constraint::isSatisfied()中也是同样的作用。低tolerances一般可以简化规划问题，也即约束更容易被满足。

另外一个参数是maxIterations，可以通过ompl::base::ConstraintL::setMaxIterations()设置。maxIterations在无法找到满足条件的情况下，用于限制projection的使用的最大次数。对于容易/不易满足的约束，可能需要刻意调节这一值。

定义约束状态空间

周围状态空间

在定义约束状态空间之前，我们需要定义周围的状态空间R^3

// 定义状态空间
auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);
 
// 设置空间的边界
ob::RealVectorBounds bounds(3);
bounds.setLow(-2);
bounds.setHigh(2);
 
rvss->setBounds(bounds);

周围空间(ambient space)是constraint所在的空间，会被约束状态空间使用

约束实例

接下来需要建立一个约束的实例(constraint instance)，用于后面约束状态空间

auto constraint = std::make_shared<Sphere>();

约束状态空间

现在我们已经定义了周为状态空间和约束，可以定义约束状态空间了。此例中，我们将用ompl::base::ProjectedStateSpace，他实现了一种基于投影算子的方法来满足约束。然而，我们也可以简单地用其他的约束状态空间，如ompl::base::AtlasStateSpace或ompl::base::TangentBundleStateSpace来解决这个问题。我们也需要创建一个ompl::base::ConstrainedSpaceInformation，即带约束的ompl::base::SpaceInformation。

// 将周围空间和约束放在一起得到约束状态空间。
auto css = std::make_shared<ob::ProjectedStateSpace>(rvss, constraint);
 
// 为这个约束状态空间定义约束空间信息
auto csi = std::make_shared<ob::ConstrainedSpaceInformation>(css);

ompl::base::ConstrainedSpaceInformation所做的最重要的事情就是调用ompl::base::ConstrainedStateSpace::setSpaceInformation，这个函数允许流形遍历与离散测地线计算一起进行碰撞检测。现在，我们有一个约束的状态空间和我们规划的空间的信息。

定义问题

现在我们为这个约束的空间定义一个简单地问题来求解：从球面的南极点向北极点穿越，且要必考赤道上的一个简易的障碍。用约束框架定义一个问题与定义无约束问题一样简单，也用的是一样的工具。例如，我们将创建一个ompl::geometric::SimpleSetup来帮助定义问题

auto ss = std::make_shared<og::SimpleSetup>(csi);

可行状态检测

定义一个可行状态检测器，这里的障碍物是赤道上一条带有洞的细长带（图见原文对应位置，看图更易理解）

bool obstacle(const ob::State *state)
{
    // 因为ob::ConstrainedStateSpace::StateType继承自Eigen::Map<Eigen::VectorXd>，我们可以使用引用来简化获取状态
    const Eigen::Map<Eigen::VectorXd> &x = *state->as<ob::ConstrainedStateSpace::StateType>();
 
    // 或者，我们可以用如下命令来获得潜在的实向量状态：
    //   auto x = state->as<ob::ConstrainedStateSpace::StateType>()->getState()->as<ob::RealVectorStateSpace::StateType>();
    // 注意，我们对受约束的状态用"getState()"。这里获取的潜在的状态时周围环境所分配的
 
    // 定义一个细长的障碍带，其中一侧有一个洞
    if (-0.1 < x[2] && x[2] < 0.1)
    {
        if (-0.05 < x[0] && x[0] < 0.05)
            return x[1] < 0;
 
        return false;
    }
 
    return true;
}
 
// 在simple setup中设定state validity checker
ss->setStateValidityChecker(obstacle)

起点终点

接下来设置起点终点，即球的南北极。注意对于约束问题而言，起点和终点必须满足约束函数，如果不满足就会出报错。

// Start and goal vectors.
Eigen::VectorXd sv(3), gv(3);
sv << 0, 0, -1;
gv << 0, 0,  1;
 
// 我们将加入simple setup中的Scoped states
ob::ScopedState<> start(css);
ob::ScopedState<> goal(css);
 
// 将vector中的值复制到起始状态和目标状态中
start->as<ob::ConstrainedStateSpace::StateType>()->copy(sv);
goal->as<ob::ConstrainedStateSpace::StateType>()->copy(gv);
 
// 如果我们用 Atlas 或者 TangentBundleStateSpace，我们还需要锚定这些状态到charts中
//   css->anchorChart(start.get());
//   css->anchorChart(goal.get());
// 他们给atlas一个起点来生长
 
ss->setStartAndGoalStates(start, goal);

规划器

最后，我们可以加入一个规划器。这里使用ompl::geometric::PRM，但其他规划器也是同样可以的。

auto pp = std::make_shared<og::PRM>(csi);
ss->setPlanner(pp);

求解问题

现在我们已经准备好做规划了。

ss->setup();

求解的过程也和无约束问题相类似。我们这里给求解器5s时间求解。

ob::PlannerStatus stat = ss->solve(5.);
if (stat)
{
    // 用带约束的状态空间时路径简化同样有效
    ss->simplifySolution(5.);
 
    // 得到求解路径
    auto path = ss->getSolutionPath();
 
    // 插值对于约束状态空间同样有些，而且一般是必须的
    path.interpolate();
 
    // 像往常一样做任何你想对路径做的处理！
}
else
    OMPL_WARN("No solution found!");