【MATLAB机器人系统工具箱】【manipulatorRRT规划器】属性和方法解析

启用了连接启发式（heuristic）后，双向快速扩展随机树（RRT）算法会在以下情况下忽略 `MAXCONNECTIONDISTANCE` 的限制：当两棵树（起始树和目标树）之间的节点距离足够接近时，算法会直接尝试连接这两个节点，而不是按照通常的固定步长进行扩展。这意味着即使两个节点之间的距离超过了 `MAXCONNECTIONDISTANCE`，如果启用了连接启发式，算法仍然会尝试直接连接它们。

具体解释：

标准扩展：在没有启用连接启发式的情况下，树的扩展是一步步进行的。每次扩展的步长由 MAXCONNECTIONDISTANCE 决定。这确保了每次扩展不会超过指定的距离，以便更精确地控制树的生成。
启用连接启发式后：
- 当两棵树的节点之间的距离小于一定阈值（通常很小），启发式算法认为这两个节点可以直接连接，而不再受 MAXCONNECTIONDISTANCE 的限制。
- 系统会尝试直接从一个树的末端节点连接到另一棵树的末端节点。如果成功，这条路径就会成为最终的规划路径，从而加速了找到可行路径的过程。

举例：

假设在扩展的过程中，起始树的某个节点A与目标树的某个节点B之间的距离是10个单位，而 MAXCONNECTIONDISTANCE 被设定为5个单位。

没有启用连接启发式：算法会尝试先从节点A向目标方向扩展5个单位的距离，然后再从这个新的节点继续扩展，直到接近节点B。
启用连接启发式：当发现节点A和节点B的距离为10个单位，且这两个节点之间没有障碍物时，算法会忽略 MAXCONNECTIONDISTANCE 的限制，直接尝试连接这两个节点，从而快速找到一条可行路径。

这种方式减少了不必要的扩展步骤，从而加快了路径规划的速度，特别是在树的两端已经非常接近但还未完全连接的情况下。

`ValidationDistance` 属性用于确定在路径规划过程中，树中两个相邻节点之间的运动是否有效。它通过在这两个节点之间插入若干个中间节点，并检查这些中间节点的有效性（例如，是否发生碰撞）来实现。

举例说明：

假设你有一个机器人在一个环境中进行运动规划。规划器生成了一个路径，其中包括一系列的配置节点。这些节点表示机器人在不同位置的状态。

场景1: 如果 ValidationDistance 设为较大值（例如 0.1），在两个相邻节点之间，规划器可能只会检查较少的插值节点。如果环境中有障碍物，而这些障碍物刚好出现在两个相邻节点之间的路径上，较少的插值节点可能无法发现这些障碍物，从而导致路径的某一段实际上不可行。
场景2: 如果 ValidationDistance 设为较小值（例如 0.01，这是默认值），则在两个相邻节点之间，规划器会插入更多的中间节点进行检查。这样，即使环境中有微小的障碍物，这些插值节点也更有可能发现它们，从而提高路径的安全性和可行性。

总结：
ValidationDistance 值越小，路径的检查越精细，规划出的路径越安全，但计算量也会增加。相反，值越大，计算速度越快，但可能会错过某些潜在的碰撞，导致路径不可行。

`WorkspaceGoalRegionBias` 值越高，规划器会在目标区域内采样更多的目标配置，从而增加找到可行路径的概率，但这也可能导致规划时间增加。相反，值越低，规划器更倾向于只使用一个目标配置，路径可能更直接，但成功找到路径的概率可能会降低，特别是在复杂环境中。

场景设定：

你有一个机械臂，任务是从一个起始位置将手臂移动到一个特定区域（目标区域），这个区域是一个3D空间中的盒子。目标区域内部有很多可以到达的目标点，机械臂只需要到达这个区域的任意一个点即可完成任务。

WorkspaceGoalRegionBias = 0.1 的情况：

在这种情况下，规划器主要会尝试直接连接起始位置和目标区域内的一个固定目标点。因为 WorkspaceGoalRegionBias 很低，所以规划器很少会在目标区域内采样额外的目标配置。

示例结果：
1. 机械臂从起始位置出发，规划器尝试直接到达目标区域的中心点。
2. 如果规划路径中存在障碍物，规划器可能难以找到一条无碰撞的路径，因为它只在目标区域内选择了一个点作为目标。
3. 如果路径找到失败，可能需要多次尝试或改变目标点，但因为 WorkspaceGoalRegionBias 低，探索的范围有限，规划时间可能较短，但成功率低。
WorkspaceGoalRegionBias = 0.9 的情况：

在这种情况下，规划器会频繁地在目标区域内采样多个额外的目标配置点。也就是说，除了尝试直接连接到中心点之外，规划器还会尝试连接到目标区域内的其他点。

示例结果：
1. 机械臂从起始位置出发，规划器在目标区域内随机采样多个可能的目标点。
2. 如果一条直接的路径被障碍物阻挡，规划器可能会找到另一条路径到达目标区域内的不同点，从而成功完成任务。
3. 由于规划器在目标区域内有更多的选择，找到无碰撞路径的概率增加了，但由于计算了更多可能的路径，规划时间可能更长。

总结：

在 WorkspaceGoalRegionBias = 0.1 的情况下，规划器尝试直线式的路径规划，路径更直接，但在复杂环境中可能更难成功。
在 WorkspaceGoalRegionBias = 0.9 的情况下，规划器广泛探索目标区域内的多个目标点，从而增加成功率，特别是在存在障碍物的复杂环境中，但计算时间也会增加。

适用场景：

如果你的任务需要快速找到一条路径，而且目标区域比较容易到达，使用较低的 WorkspaceGoalRegionBias 可以节省时间。
如果目标区域复杂或者有很多障碍物，使用较高的 WorkspaceGoalRegionBias 可以提高成功率，尽管会增加计算时间。

`properties(Access=private)` 部分定义了两个私有属性 `HasUserSetSkippedSelfCollisions` 和 `Map`，这些属性只能在类内部访问和使用。

1. `HasUserSetSkippedSelfCollisions`

作用: 这个布尔型属性用于记录用户是否主动设置了 SkippedSelfCollisions 属性。

举例说明:

假设你在使用 manipulatorRRT 类进行路径规划。类提供了一个属性 SkippedSelfCollisions，允许你指定在路径规划过程中哪些机器人部分的自碰撞可以被忽略。

场景1: 用户没有主动设置 SkippedSelfCollisions:
- 默认情况下，SkippedSelfCollisions 可能被设置为忽略子体和父体之间的碰撞（默认值为 “parent”）。
- 因为用户没有主动设置此属性，所以 HasUserSetSkippedSelfCollisions 将保持为 false。
- 如果在路径规划过程中发现潜在的自碰撞问题，类可以根据 HasUserSetSkippedSelfCollisions 的值来决定是否给用户提供警告或提示。
场景2: 用户主动设置了 SkippedSelfCollisions:
- 用户可能手动将 SkippedSelfCollisions 设置为 “adjacent” 或者指定了具体的身体对来跳过自碰撞检查。
- 在这种情况下，HasUserSetSkippedSelfCollisions 会被设置为 true。
- 当路径规划开始时，类会知道用户已经主动设置了 SkippedSelfCollisions，因此不会再发出与默认行为相关的警告。

总结: HasUserSetSkippedSelfCollisions 的作用是帮助类在运行时判断用户是否主动调整了 SkippedSelfCollisions，从而决定是否需要对默认行为做出额外的处理或提醒。

2. `Map`

作用: Map 属性用于存储用于碰撞检测的地图数据（通常是 occupancyMap3D 对象）。

举例说明:

假设你在一个复杂的环境中使用 manipulatorRRT 进行路径规划，环境中有多个障碍物。

场景1: 用户提供了一个地图对象:
- 用户在实例化 manipulatorRRT 时，传入了一个 occupancyMap3D 对象作为地图。
- 这个地图将被存储在 Map 属性中，路径规划器会使用这个地图来检查每个机器人配置的可行性，确保路径不会穿过障碍物。
- 当规划路径时，类会使用 Map 属性中的地图来进行碰撞检测，从而生成一条无碰撞的路径。
场景2: 用户没有提供地图对象:
- 如果用户没有提供地图对象，Map 属性将为空。
- 在这种情况下，路径规划器可能只考虑机器人与自身和环境中的静态对象的碰撞，而不涉及地图中的障碍物。
- 如果类的某些方法需要使用地图（例如基于地图的碰撞检测），而 Map 为空，则类可能会给出提示或错误，提醒用户需要提供地图。

总结: Map 属性的作用是存储用户提供的地图数据，并在路径规划过程中用于碰撞检测。该属性确保规划出的路径能够避开环境中的障碍物，从而使路径更为安全可行。

在路径规划过程中，如果发现潜在的自碰撞问题，类可以使用 HasUserSetSkippedSelfCollisions 属性来判断是否需要向用户提供警告或提示。下面通过具体场景来解释这个过程。

场景说明

假设你在使用 manipulatorRRT 类为一个机械臂规划路径。这个机械臂有多个关节，每个关节可以移动到不同的位置。如果两个关节的位置过于接近，可能会导致机械臂的自碰撞。

步骤1: 初始化规划器

在初始化 manipulatorRRT 时，用户可能会或可能不会设置 SkippedSelfCollisions 属性。

如果用户没有设置 SkippedSelfCollisions，则系统会采用默认值（例如，忽略父子体之间的自碰撞），并且 HasUserSetSkippedSelfCollisions 保持为 false。
如果用户主动设置了 SkippedSelfCollisions（例如，指定要忽略特定关节对之间的自碰撞），则 HasUserSetSkippedSelfCollisions 被设置为 true。

步骤2: 进行路径规划

当路径规划开始时，规划器会尝试生成一条从起始点到目标点的路径。在此过程中，规划器会检查路径上每个配置是否与自身或环境发生碰撞。

默认行为: 如果 HasUserSetSkippedSelfCollisions 为 false，意味着用户没有主动调整自碰撞检查行为，此时规划器按照默认设置进行自碰撞检查。
用户定制行为: 如果 HasUserSetSkippedSelfCollisions 为 true，意味着用户已经自定义了自碰撞检查规则，规划器会按照用户的设置进行自碰撞检查。

步骤3: 处理自碰撞检测

在路径规划过程中，如果检测到潜在的自碰撞问题，类会根据 HasUserSetSkippedSelfCollisions 的值采取不同的行动：

HasUserSetSkippedSelfCollisions = false（默认设置）:
- 规划器可能会检测到某些关节对之间的自碰撞。
- 由于用户没有自定义设置，规划器会向用户发出警告，提示路径中可能存在自碰撞问题，并建议用户调整 SkippedSelfCollisions 设置以避免不必要的碰撞检查。
HasUserSetSkippedSelfCollisions = true（用户自定义设置）:
- 规划器同样可能检测到自碰撞，但由于用户已经主动设置了 SkippedSelfCollisions，规划器会假设用户已经考虑了这些碰撞情况，并不会发出警告。
- 规划器会继续按照用户的设置进行路径规划，即使检测到自碰撞，也不会打断或提示，因为这可能是用户期望的行为。

结论

HasUserSetSkippedSelfCollisions 的主要作用是在路径规划过程中，帮助规划器决定是否需要向用户发出与自碰撞相关的警告或提示。通过判断这个属性的值，规划器可以区分用户是否已经考虑了自碰撞问题，并相应地调整其行为。如果 HasUserSetSkippedSelfCollisions 为 false，则意味着用户依赖默认行为，规划器可能需要更严格的碰撞检查并提供提示；而如果为 true，则意味着用户已经自定义了碰撞检查规则，规划器将尊重用户的设置，不发出额外警告。

这行代码用于设置 DEFAULT_SKIPPED_TYPE 属性的默认值，它代表在路径规划过程中默认要跳过的自碰撞检查类型。具体的解释如下：

`char(...)`，将枚举值转换为字符串（字符数组）， `lower` 函数将字符串转换为小写形式

详细解释

matlab">DEFAULT_SKIPPED_TYPE = lower(char(robotics.manip.internal.SkippedSelfCollisionType.PARENT));

robotics.manip.internal.SkippedSelfCollisionType.PARENT:
- 这是一个枚举值，表示路径规划器在默认情况下会跳过的自碰撞类型。
- PARENT 通常指的是跳过机械臂的子关节与其直接父关节之间的碰撞检查。这是因为在大多数情况下，子关节和父关节之间的碰撞可能性较小或没有实际意义。
char(...):
- 枚举类型在 MATLAB 中是以字符形式存储的。通过 char(...)，将枚举值转换为字符串（字符数组）。这样，枚举值 PARENT 就变成了字符串 "PARENT"。
lower(...):
- lower 函数将字符串转换为小写形式。在这里，它将 "PARENT" 转换为 "parent"。
DEFAULT_SKIPPED_TYPE:
- 最终，DEFAULT_SKIPPED_TYPE 被设置为字符串 "parent"，表示在默认情况下，路径规划器会跳过子关节和父关节之间的自碰撞检查。

举例说明

假设你有一个六自由度的机械臂（比如 robot），每个关节之间有一定的距离和运动范围。在路径规划过程中，你可能不想检查某些关节之间的自碰撞，特别是那些物理上不太可能发生碰撞的关节，例如相邻的父关节和子关节。

场景1:
- 默认情况下，规划器会跳过检查父关节和子关节之间的碰撞，因为这些关节通常在运动中不会互相碰撞。
- 这个默认设置是通过 DEFAULT_SKIPPED_TYPE 实现的，它被设定为 "parent"，告诉规划器在进行自碰撞检查时忽略父关节和子关节之间的碰撞。
场景2:
- 如果用户需要在路径规划时更改默认行为，例如希望检查父关节和子关节之间的碰撞，他们可以通过设置不同的 SkippedSelfCollisions 属性来覆盖默认值。

总结

这行代码的主要作用是设置一个默认值，告诉路径规划器在默认情况下应跳过检查父关节和子关节之间的自碰撞。这是通过将枚举类型转换为小写字符串 "parent" 来实现的，从而为路径规划器提供一个默认的行为设置。

这行代码用于验证 map 参数的属性，以确保其符合预期的类型和条件。具体的功能和解释如下：

`validateattributes`是 MATLAB 的一个内置函数，用于验证输入变量是否符合指定的属性要求。如果输入不符合要求，它会抛出一个错误。

功能解释

matlab">validateattributes(map, {'occupancyMap3D'}, ...{'nonempty', 'scalar'}, 'manipulatorRRT', 'Map');

第一个参数 (map):
- validateattributes 的第一个参数是需要验证的变量。在这里，它是 map，代表传递给函数的地图对象。
第二个参数 ({'occupancyMap3D'}):
- 第二个参数是一个包含预期类名称的元胞数组。在这里，{'occupancyMap3D'} 表示 map 变量应该是 occupancyMap3D 类的对象。
第三个参数 ({'nonempty', 'scalar'}):
- 第三个参数是一个包含属性要求的元胞数组。这里的要求包括：
  - 'nonempty': map 必须是非空的，即不能为 [] 或空对象。
  - 'scalar': map 必须是一个标量值，即不能是数组或矩阵，而是单个对象。
第四个参数 ('manipulatorRRT'):
- 第四个参数是出现错误时显示的函数名称，用于生成错误消息时帮助定位问题。在这里，它是 'manipulatorRRT'，表示错误是由 manipulatorRRT 函数（或类）引发的。
第五个参数 ('Map'):
- 第五个参数是出现错误时显示的变量名称，同样用于生成错误消息。在这里，它是 'Map'，表示 map 变量未通过验证。

代码的作用

这行代码的作用是确保 map 变量满足以下条件：

类型: map 必须是 occupancyMap3D 类型的对象。这意味着 map 必须是 3D 占用栅格地图，用于表示环境中的障碍物和可行空间。
非空: map 不能是空的。空的 map 对象无法用于碰撞检测或路径规划，因此该条件确保了地图对象的有效性。
标量: map 必须是标量，不能是数组或矩阵。也就是说，它必须是一个单独的地图对象，而不是一组地图。

总结

这行代码通过 validateattributes 函数验证 map 参数，确保它是一个有效的 occupancyMap3D 对象，并且是非空和标量的。如果 map 参数不符合这些要求，MATLAB 会抛出一个错误，提示用户输入的 Map 参数无效。这种验证有助于确保函数或类在接收到有效的输入参数后再继续执行，避免潜在的运行时错误。

让我们通过一个具体的示例来说明这段代码的功能和作用。

`interpolate` 方法既灵活又强大，用户可以根据自己的需求选择指定插值次数或让系统根据预定义的验证距离进行插值。

背景说明

假设你有一个机械臂的路径 path，它包含了从起始点到目标点的一系列配置。你想要生成一个更平滑的路径，这需要在每个相邻的配置之间进行插值。这个 interpolate 方法可以根据两种不同的方式进行插值：

指定插值次数：你可以指定每两个配置之间要生成的中间配置数量。
基于验证距离：你也可以让系统根据 ValidationDistance 属性来决定插值的密度。

场景1: 指定插值次数

假设你调用 interpolate 方法，并指定了要在每两个配置之间进行5次插值：

matlab">interpolatedPath = planner.interpolate(path, 5);

在这个场景中：

nargin 为 3，因为你传递了三个参数（planner 对象、path 路径，以及插值次数 5）。
代码会进入 if 分支，提取 varargin{1}，即 5，赋值给 numInterpolations。
然后，调用 interpolateByNumber 方法，将路径 path、状态空间 StateSpace 以及 numInterpolations（即 5）传递给它。
interpolateByNumber 会在每两个相邻的配置之间插入5个中间配置，从而生成更平滑的路径。

具体流程：

提取 5 作为插值次数。
使用 interpolateByNumber 方法，在路径中每对相邻的配置之间插入5个中间配置。
返回经过插值后的 interpolatedPath。

场景2: 使用验证距离

假设你调用 interpolate 方法，而不指定插值次数，只传递路径：

matlab">interpolatedPath = planner.interpolate(path);

在这个场景中：

nargin 为 2，因为你只传递了两个参数（planner 对象和 path 路径）。
代码不会进入 if 分支，而是进入 else 分支。
然后，调用 interpolateByResolution 方法，将路径 path、状态空间 StateSpace 以及 ValidationDistance 属性传递给它。
interpolateByResolution 方法会基于 ValidationDistance 属性，在每两个相邻配置之间插入适量的中间配置，以确保路径的平滑性和安全性。