轨迹验证
ISAAC教程合集地址: https://blog.csdn.net/kunhe0512/category_12163211.html
trajectory_validation 包有三个用途:
-
模拟可编程逻辑控制器 (PLC) 在机器人上的安全检查,这对于测试导航堆栈的规划器和控制器很有用。
-
避免硬件紧急停止,从而提高生产率。
-
验证主要规划器并使用后备规划器来避免可避免的冲突。
组件
trajectory_validation 包目前提供五种检查,每种检查都由此处描述的组件执行。 虽然所有检查都可用于验证轨迹,但其中大部分也可用于检查机器人的当前状态。 这将在下一节中进一步解释。
测速
SpeedCheck 确保线速度和角速度值相互兼容。 约束由 valid_speed_regions 参数定义,其格式如下所示。 如果车辆离开此多边形,则速度检查失败。
"SpeedCheck": {
"valid_speed_region": [
[linear_speed_0, angular_speed_0], [linear_speed_1, angular_speed_1],
[linear_speed_2, angular_speed_2], [linear_speed_3, angular_speed_3],
[...]
]
}
RangeCheck
RangeCheck 确保机器人不会试图超出所需的运动限制。 例如,为确保线速度在所需范围内,可以将 RangeCheck 与以下配置结合使用。 您可以根据需要使用尽可能多的 RangeCheck 组件来检查机器人状态的各个方面。
"RangeCheck": {
"type": "linear_speed",
"min": -0.5,
"max": 1.5
}
碰撞检查
收到轨迹后,CollisionCheck 使用 time_step_trajectory 参数从轨迹中及时采样状态,如下图所示。 该参数越小,来自输入轨迹的样本数越多。 然后,对于每个轨迹样本,机器人以全减速模拟制动,用 deceleration_limit 参数化。 对于差速轮式机器人,该参数限制了每个轮子的减速度。 生成的制动轨迹在下图中用红色表示。
然后,对于每个制动轨迹,使用 time_step_brake 进行另一次采样。 下图显示了零角速度的特殊情况的采样。
对于通过碰撞检查的轨迹,使用该方法获得的每个样本状态需要与环境中的静态和动态物体或障碍物保持至少 distance_threshold 米的距离。
"CollisionCheck": {
"deceleration_limit": 12.5,
"distance_threshold": 0.1,
"obstacle_names": [
"map/isaac.navigation.DistanceMap",
"map/restricted_area",
"local_map"
]
}
适用性检查
为了适用,轨迹需要接近机器人的当前状态。 例如,当当前线速度为零时,恒定线速度为-1.0 m/s 的候选轨迹不适用。 下面的 JSON 块显示了 ApplicabilityCheck 的示例配置,它要求最接近机器人当前状态的轨迹状态在时间上在 0.1 秒以内。 它还期望这两个状态在 x 中的位置相差不超过 0.2 m。
"ApplicabilityCheck": {
"threshold_time": 0.1,
"threshold_pos_x": 0.2,
"threshold_pos_y": 0.2,
"threshold_heading": 0.2,
"threshold_linear_speed": 1.0,
"threshold_angular_speed": 1.0,
"threshold_linear_acceleration": 1.0,
"threshold_angular_acceleration": 1.0
}
可行性检查
为了可行,轨迹的位置、速度和加速度应该是一致的。 例如,要求恒定的非零加速度和速度的轨迹没有意义。 为了检查可行性,FeasibilityCheck 集成加速度以检查速度,并集成速度以检查位置和角度。 差异的限制可以如下所示配置。
"FeasibilityCheck": {
"limit_pos_x": 0.01,
"limit_pos_y": 0.01,
"limit_heading": 0.01,
"limit_linear_speed": 1.0,
"limit_angular_speed": 1.0
}
Codelets
本节介绍如何使用上面列出的检查组件来检查当前状态或候选轨迹以及如何采取必要的操作。
状态验证
如下图所示,StateValidation 小代码从基础接收当前状态信息,并从导航堆栈接收所需命令。 如果当前状态未通过使用下述方法插入此小代码的检查,则命令机器人停止。 否则,所需的命令被转发到基地。 因此,StateValidation codelet 可用于在仿真机器人硬件上复制 PLC,也可用于在 PLC 在真实机器人上启动之前进行干预。 您可以轻松地对 StateValidation codelet 进行编程以根据需要重新启动,而不是恢复 PLC 的状态,这可能很麻烦; 因此,StateValidation 极大地提高了生产力。
要启用检查,请将检查组件放置在与 StateValidation codelet 相同的节点上。 例如,下面 JSON 块中的 StateValidation 小代码将发现其节点具有 SpeedCheck 组件,并将检查它从基地接收的当前速度值。
{
"name": "state_validation",
"components": [
{
"name": "MessageLedger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "StateValidation",
"type": "isaac::trajectory_validation::StateValidation"
},
{
"name": "SpeedCheck",
"type": "isaac::trajectory_validation::SpeedCheck"
}
]
},
轨迹验证
就像 StateValidation codelet 一样,TrajectoryValidation codelet 也可以使用插入到同一节点的检查组件来执行验证检查,如下面的 JSON 块所示。 然而,这两个小码的用例是不同的。 TrajectoryValidation 不是从 base 接收当前状态并充当导航堆栈和 base 之间的中间人,而是从规划器接收候选轨迹并将轨迹是否有效作为消息发布,稍后由 TrajectorySelection 使用,如中所述 下一节。
{
"name": "trajectory_validation",
"components": [
{
"name": "MessageLedger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "TrajectoryValidation",
"type": "isaac::trajectory_validation::TrajectoryValidation"
},
{
"name": "SpeedCheck",
"type": "isaac::trajectory_validation::SpeedCheck"
}
]
},
轨迹选择
如下图所示,多个 TrajectoryValidation codelet 可用于评估不同规划器的冗余度。 TrajectorySelection 然后有一个简单的逻辑:如果主要轨迹无效,请尝试替代轨迹。 如果备选轨迹也无效,则使用应急预案,可能是最大减速度。
用法
trajectory_validation 包用于差分基础导航子图中 (packages/navigation/apps/differential_base_control.subgraph.json)。 TrajectoryValidation 和 TrajectorySelection 小码防止违反五项检查中任何一项的轨迹被执行。 如果碰撞或速度检查失败,StateValidation 会阻止机器人移动至少 2 秒。 重新启动失败的 StateValidation 之前的持续时间由 state_validation_behavior 节点的 wait_duration 参数确定。 可以配置检查,如 packages/cart_delivery/apps/navigate.app.json 中所示。
由于各种原因,机器人可能会发现自己的姿势已经离障碍物太近了。 在这种情况下,机器人将被卡住,因为 StateValidation 将阻止它移动,而 TrajectoryValidation 将认为任何轨迹候选无效。 为了让机器人摆脱这种情况,有两种选择:
-
手动控制机器人使其脱离困境。
-
使用 disable_check 参数暂时禁用状态和轨迹验证的 CollisionCheck。
要禁用附加到 StateValidation 或 TrajectoryValidation 的所有检查,请使用 disable_checks 参数。
更多精彩内容:
https://www.nvidia.cn/gtc-global/?ncid=ref-dev-876561