> 文档中心 > 31.Isaac教程--规划器代价

31.Isaac教程--规划器代价


规划器代价

在这里插入图片描述
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 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 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 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 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 0.5*gain*min(0,distance(state)−targetdistance−alpha*speed)^20.5gainmin(0,distance(state)targetdistancealphaspeed)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)"  ]},

我们已经使用自定义构建器成功添加了一个新障碍。

更多精彩内容:
https://www.nvidia.cn/gtc-global/?ncid=ref-dev-876561

在这里插入图片描述