规划器代价

在这里插入图片描述
ISAAC教程合集地址: https://blog.csdn.net/kunhe0512/category_12163211.html

文章目录

规划器代价
- 组件
- 入门
- 通过应用程序图自定义成本

导航本地规划器基于线性二次调节器 (LQR) 规划器。它通过生成最小化成本函数的轨迹来工作。不幸的是，没有适用于所有应用程序的单一成本函数；因此，为您自己的应用定制成本函数非常重要。这是 PlannerCost 和 PlannerCostBuilder 发挥作用的地方：

PlannerCost 提供了一个接口来生成成本函数：

class PlannerCost {
 public:
  // Returns true if the current state is valid.
  virtual bool isValid(double time, const VectorXd& state);

  // Returns the evaluation at a given state and time.
  virtual double evaluate(double time, const VectorXd& state) = 0;

  // Adds the gradient of this cost function for the given state to the given vector `gradient`.
  // `gradient` uses a Ref<VectorXd> to allow block operation to passed to this function.
  virtual void addGradient(double time, const VectorXd& state,
                           Eigen::Ref<Eigen::VectorXd> gradient) = 0;

  // Adds the hessian of this cost function for the given state to the given matrix `hessian`.
  // `hessian` uses a Ref<VectorXd> to allow block operation to passed to this function.
  virtual void addHessian(double time, const VectorXd& state,
                          Eigen::Ref<Eigen::MatrixXd> hessian) = 0;
};

PlannerCostBuilder 提供了一个组件接口来将 Planner 成本添加到您的应用程序中：

class PlannerCostBuilder : public alice::Component {
 public:
  // Creates the cost function initially. Makes sure all necessary memory required for subsequent
  // calls to `update` is allocated.
  virtual PlannerCost* build() = 0;

  // Prepares the cost function for the given time interval.
  // Does not do any dynamic memory allocations.
  virtual void update(double start_time, double end_time) {}

  // Destroys the cost function and all memory which was allocated during `build`.
  virtual void destroy() = 0;

  // Returns a pointer to the maintained cost function
  virtual PlannerCost* get() = 0;
};

组件

主要组件是isaac.planner_cost.PlannerCostBuilder，是新增PlannerCost的接口。

以下是已在 Isaac SDK 中实现并可供使用的成本函数：

isaac.planner_cost.AdditionBuilder：用来把几个成本加在一起。
isaac.planner_cost.SmoothMinimumBuilder：用于计算成本列表的最小值。它不是确切的最小值，而是可微分的近似值。
isaac.planner_cost.RangeConstraintsCostBuilder：帮助您创建基于状态的二次成本。您可以定义最小值和最大值，如果状态超出此范围，将创建二次成本。
isaac.planner_cost.DistanceQuadraticCostBuilder：期望 PlannerCost 返回距离。它将根据目标距离和实际距离创建二次成本。
isaac.planner_cost.ObstacleDistanceBuilder：返回障碍物的有符号距离。它本身不是成本函数，但可以与 isaac.planner_cost.DistanceQuadraticCostBuilder 一起使用来创建二次成本函数。
isaac.planner_cost.CirclesUnionSmoothDistanceBuilder：这是一个辅助函数，用于为 RobotShape 的所有圆调用另一个 PlannerCost 函数。

入门

您可以运行 Flatsim 以查看 Navigation local planner 的执行情况：

bazel run //packages/flatsim/apps:flatsim -- --demo demo_1

如果您想创建自己的成本函数，您应该首先确定 packages/planner_cost/gems 中的现有成本是否满足您的需要。如果这些成本都不够，您将需要首先创建一个实现 PlannerCost 接口的类。

例如，让我们看一下 ScalarMultiplication，它将 PlannerCost 作为输入并将其乘以一个常数：

// This is an implementation of PlannerCost.
// It takes another PlannerCost and simply multiplies by a constant value.
class ScalarMultiplication : public PlannerCost {
 public:
  ScalarMultiplication(PlannerCost* cost, double constant) : cost_(cost), constant_(constant) {}

  // Returns true if the current state is valid. Here we will just rely on the other PlannerCost
  bool isValid(double time, const VectorXd& state) override {
    return cost_->isValid(time, state);
  }

  // Returns the evaluation at a given state and time.
  // We can multiply the result of cost_->evaluate() by our constant.
  double evaluate(double time, const VectorXd& state) override {
    return constant_ * cost_->evaluate(time, state);
  }

  // Adds the gradient of this cost function for the given state to the given vector `gradient`.
  // `gradient` uses a Ref<VectorXd> to allow block operation to passed to this function.
  // We need to scale the gradient by our constant.
  void addGradient(double time, const VectorXd& state, Eigen::Ref<VectorXd> gradient) override {
    VectorXd tmp_gradient = VectorXd::Zero(gradient.size());
    cost_->addGradient(time, state, tmp_gradient);
    gradient += tmp_gradient * constant_;
  }

  // Adds the hessian of this cost function for the given state to the given matrix `hessian`.
  // `hessian` uses a Ref<VectorXd> to allow block operation to passed to this function.
  // We need to scale the hessian by our constant.
  void addHessian(double time, const VectorXd& state, Eigen::Ref<MatrixXd> hessian) override {
    MatrixXd tmp_hessian = MatrixXd::Zero(hessian.rows(), hessian.cols());
    cost_->addHessian(time, state, tmp_hessian);
    hessian += tmp_hessian * constant_;
  }

 private:
  // Hold another cost_
  PlannerCost* cost_ = nullptr;
  double constant_ = 1.0;
};

获得新的 PlannerCost 后，您可以使用自定义构建器，如下所示。请注意，它必须实现接口 PlannerCostBuilder：

class ScalarMultiplicationBuilder : public PlannerCostBuilder {
 public:
  // Creates the cost function initially. Makes sure all necessary memory required for subsequent
  // calls to `update` is allocated.
  PlannerCost* build() override {
    builder_ = node()->app()->findComponentByName<PlannerCostBuilder>(get_component_name());
    ASSERT(builder_ != nullptr,
           "Failed to load the component: %s", get_component_name().c_str());
    cost_.reset(new ScalarMultiplication(builder_->build(), get_constant()));
    return static_cast<PlannerCost*>(cost_.get());
  }

  // Prepares the cost function for the given time interval.
  // Does not do any dynamic memory allocations.
  void update(double start_time, double end_time) override {
    builder_->update(start_time, end_time);
  }

  // Destroys the cost function and all memory which was allocated during `build`.
  void destroy() override {
    cost_.reset();
    builder_->destroy();
  }

  // Returns a pointer to the maintained cost function
  PlannerCost* get() override {
    return static_cast<PlannerCost*>(cost_.get());
  }

  // Name of the component implementating a PlannerCostBuilder to be used as distance function
  ISAAC_PARAM(std::string, component_name);

  // Constant multiplication factor
  ISAAC_PARAM(double, constant, 20.0);

 private:
  std::unique_ptr<ScalarMultiplication> cost_;
  PlannerCostBuilder* builder_;
};

我们现在有一个新的 PlannerCost，我们可以使用它来扩展任何现有的 PlannerCost。我们也有一个建造者。在下一节中，我们将研究如何扩展现有导航图以扩展现有成本。

通过应用程序图自定义成本

要自定义图形，请编辑 packages/navigation/apps/differential_base_control.subgraph.json 文件。

首先，您应该找到包含所有构建器的节点：

{
  "name": "lqr_state_cost",
  "components": [
    {
      "name": "TotalSum",
      "type": "isaac::planner_cost::AdditionBuilder"
    },
    {
      "name": "LimitRange",
      "type": "isaac::planner_cost::RangeConstraintsCostBuilder"
    },
    {
      "name": "TargetRange",
      "type": "isaac::planner_cost::RangeConstraintsCostBuilder"
    },
    {
      "name": "SmoothMinimumBuilder",
      "type": "isaac::planner_cost::SmoothMinimumBuilder"
    },
    {
      "name": "CirclesUnionSmoothDistanceBuilder",
      "type": "isaac::planner_cost::CirclesUnionSmoothDistanceBuilder"
    },
    {
      "name": "ObstacleLocalMap",
      "type": "isaac::planner_cost::ObstacleDistanceBuilder"
    },
    {
      "name": "ObstacleRestrictedArea",
      "type": "isaac::planner_cost::ObstacleDistanceBuilder"
    },
    {
      "name": "DistanceQuadraticCostBuilder",
      "type": "isaac::planner_cost::DistanceQuadraticCostBuilder"
    }
  ]
},
{
  "name": "lqr_control_cost",
  "components": [
    {
      "name": "RangeConstraintsCostBuilder",
      "type": "isaac::planner_cost::RangeConstraintsCostBuilder"
    }
  ]
},

lqr_state_cost 包含用于计算与沿轨迹状态相关的成本的构建器列表，而 lqr_control_cost包含与控制相关的成本。

再往下，您可以找到与这些成本关联的配置参数：

"lqr": {
  "isaac.lqr.DifferentialBaseLqrPlanner": {
    ...
    "state_planner_cost_name": "$(fullname lqr_state_cost/TotalSum)",
    "control_planner_cost_name": "$(fullname lqr_control_cost/RangeConstraintsCostBuilder)"
    ...
  }
},

这里我们定义了与控制相关的成本的根和与状态相关的根：

对于控件，我们有一个类型为 isaac.planner_cost.RangeConstraintsCostBuilder 的成本
对于状态，根是 isaac.planner_cost.AdditionBuilder 类型，这意味着我们将添加成本列表。查看 TotalSum 的配置，我们可以找到添加了哪些成本：

"TotalSum": {
  "component_names": [
    "$(fullname lqr_state_cost/DistanceQuadraticCostBuilder)",
    "$(fullname lqr_state_cost/LimitRange)",
    "$(fullname lqr_state_cost/TargetRange)"
  ]
},

添加了三个成本来计算最终成本：

其中两个是 isaac.planner_cost.RangeConstraintsCostBuilder 类型。
最后一个是 isaac.planner_cost.DistanceQuadraticCostBuilder 类型。这是另一个递归调用，它依赖于另一个 isaac.planner_cost.CirclesUnionSmoothDistanceBuilder 类型的 Builder，它本身依赖于 isaac.planner_cost.SmoothMinimumBuilder 类型的 Builder，它计算 isaac.planner_cost.ObstacleDistanceBuilder 列表的最小值：

"DistanceQuadraticCostBuilder": {
  "component_name": "$(fullname lqr_state_cost/CirclesUnionSmoothDistanceBuilder)"
},

"CirclesUnionSmoothDistanceBuilder": {
  "component_name": "$(fullname lqr_state_cost/SmoothMinimumBuilder)"
},

"SmoothMinimumBuilder": {
  "component_names": [
    "$(fullname lqr_state_cost/ObstacleLocalMap)",
    "$(fullname lqr_state_cost/ObstacleRestrictedArea)"
  ]
},

"ObstacleLocalMap": {
  "obstacle_name": "local_map"
},
"ObstacleRestrictedArea": {
  "obstacle_name": "map/restricted_area"
},

这乍一看可能很复杂——让我们从头开始分析：

ObstacleLocalMap 和 ObstacleRestrictedArea 都从 Atlas 加载障碍物并返回从 2d 到障碍物的有符号距离。
SmoothMinimumBuilder 有助于估算最小距离——最终，我们想知道机器人离最近的障碍物有多近。如果你需要处理更多的障碍，这将是一个添加的好地方。
CirclesUnionSmoothDistanceBuilder 是一个帮助计算距离的辅助函数，不仅适用于单个 2d 点，还适用于 SphericalRobotShape 中的所有圆。它将从障碍物列表中返回机器人的距离。
最后 isaac.planner_cost.DistanceQuadraticCostBuilder 需要一个距离函数并计算成本： $0.5*gain*min(0,distance(state)−targetdistance−alpha*speed)^2$ 。我们只需传递由 CirclesUnionSmoothDistanceBuilder 计算的距离函数。

让我们探讨如何修改上面的示例以添加自定义成本函数。假设您有以下内容：

一个新的 CustomDistanceBuilder，它返回到某些障碍物的距离，但以厘米为单位。
我们在上面定义的 ScalarMultiplicationBuilder。

现在我们需要结合两者来计算以米为单位的距离，我们需要将它添加到障碍物列表中。首先，我们需要将这两个组件添加到我们的节点：


{
  "name": "lqr_state_cost",
  "components": [
    {
      "name": "TotalSum",
      "type": "isaac::planner_cost::AdditionBuilder"
    },
    ...
    {
      "name": "DistanceQuadraticCostBuilder",
      "type": "isaac::planner_cost::DistanceQuadraticCostBuilder"
    },
    {
      "name": "ScalarMultiplicationBuilder",
      "type": "isaac::planner_cost::ScalarMultiplicationBuilder"
    },
    {
      "name": "CustomDistanceBuilder",
      "type": "isaac::planner_cost::CustomDistanceBuilder"
    }
  ]
},

之后，我们需要为他们创建配置：

"lqr_state_cost": {
  ...
  "ScalarMultiplicationBuilder": {
    "component_name": "$(fullname lqr_state_cost/CustomDistanceBuilder)",
    "constant": 100.0
  },
  "CustomDistanceBuilder": {
    ...
  }
}

最后，我们需要将新距离添加到现有障碍列表中：

"SmoothMinimumBuilder": {
  "component_names": [
    "$(fullname lqr_state_cost/ObstacleLocalMap)",
    "$(fullname lqr_state_cost/ObstacleRestrictedArea)",
    "$(fullname lqr_state_cost/ScalarMultiplicationBuilder)"
  ]
},