90、双层遗传算法在机器人运动规划中的应用

双层遗传算法在机器人运动规划中的应用

1. 双层遗传算法概述

遗传算法在实现上十分有趣，它既可以作为全局优化器用于更具规划性的场景，也能作为接近反应式的局部搜索算法。接近反应式的特性并非劣势，因为像弹性条带和轨迹遗传校正等轨迹校正算法，能够以合理的频率进行调整，使机器人在移动过程中适应轨迹变化。虽然它不如以接近控制算法频率工作的反应式算法速度快，但足以让机器人对变化的环境或移动障碍物做出反应。因此，算法的实现设置在很大程度上决定了其性能。

为了展示这种特性，我们来看一个将规划性算法和接近反应式算法融合的有趣例子，即使用双层遗传算法（Kala 等人，2010b，2011）。此后，我们将较粗粒度的遗传算法用于表示规划性算法，较细粒度的遗传算法用于表示接近反应式遗传算法。其总体概念总结如下：

2. 较粗粒度遗传算法

较粗粒度遗传算法分两个阶段运行。第一阶段，它会快速尝试找到规划性轨迹。初始移动可能会稍有延迟，就像人类在行动前会思考一下。之后，算法会持续调整规划性轨迹，以应对可能发现的任何变化。

由于需要快速得到结果，且算法要能快速适应地图变化，碰撞检查的分辨率通常较低。这是合理的，因为较粗粒度遗传算法仅为较细粒度遗传算法提供参考，机器人的实际运动由较细粒度遗传算法控制。

较粗粒度遗传算法在离线可用的静态地图上工作，该地图以较低分辨率进行概括。具体做法是在地图周围划分单元格，并记录单元格中障碍物内容的百分比作为灰度级别。将包含合理障碍物内容的单元格标记为黑色，这相当于降低输入配置空间的分辨率。该算法采用增量复杂化方法，机器人路径中的途经点数量定义了问题的复杂度，随着时间推移，复杂度会增加，但有最大限制。

较粗粒度遗传算法会在机器人移动时不断优化较粗的轨迹。在优化过程中，它能处理新出现或消失的静态障碍物。目标始终固定，但起始点会不断变化。机器人在任何时刻都会将最优个体作为运动规划的指导。当个体越过途经点时，该点会从遗传个体中删除，通过投影的符号来检测是否越过，如下图所示：

graph LR
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px;
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px;

    A([开始]):::startend --> B(较粗粒度遗传算法初始化):::process
    B --> C{机器人是否到达目标}:::decision
    C -- 否 --> D(获取较粗粒度遗传算法的最佳轨迹):::process
    D --> E(较粗粒度遗传算法进行遗传操作):::process
    E --> F(较粗粒度遗传算法进行适应度评估):::process
    F --> G(删除越过的途经点):::process
    G --> C
    C -- 是 --> H([结束]):::startend

较粗粒度遗传算法不考虑持续移动的动态障碍物。因为当机器人接近目标时，目标附近障碍物的动态情况会发生巨大变化且难以预测。由于机器人不知道障碍物的轨迹，只能进行猜测，而这种猜测仅在短时间内准确。因此，在全局优化中不考虑动态障碍物，从速度障碍物方法到势场算法，通常只在障碍物靠近机器人时才考虑动态障碍物。

3. 较细粒度遗传算法

较细粒度遗传算法负责对突然出现的障碍物做出反应，考虑附近的移动障碍物，并在更精细的层面上优化轨迹。由于需要在接近实时的情况下进行优化，因此需要进行一些限制以提高性能。

较粗粒度遗传算法限制了分辨率，而较细粒度遗传算法则限制了优化的空间区域，同时在尽可能高的分辨率下工作。这意味着从整个空间中，取机器人位置周围每个维度大小为 γ 的小区域，并将其提供给较细粒度遗传算法。由于划分出的小区域内途经点很少，因此复杂度得到了限制。

机器人主要朝着子目标移动，很少后退。因此，大部分地图（启发式地认为是 75% 的空间）朝向子目标，25% 背离子目标。较细粒度遗传算法会得到一个体积为 γd（d 为维度数）的局部地图。对于二维空间中的点机器人，所有可能情况的局部地图如下图所示：

较粗粒度和较细粒度遗传算法的融合再次使用了子目标，较粗粒度遗传算法规划路径中的子目标作为较细粒度遗传算法的目标。子目标是较粗粒度遗传算法轨迹中距离机器人当前位置固定距离的点，且位于局部地图窗口之外，用于优化。

graph LR
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px;
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px;

    A([开始]):::startend --> B(较细粒度遗传算法初始化):::process
    B --> C{机器人是否到达目标}:::decision
    C -- 否 --> D(获取较细粒度遗传算法的最佳轨迹):::process
    D --> E(较细粒度遗传算法进行遗传操作):::process
    E --> F(较细粒度遗传算法进行适应度评估):::process
    F --> G(预测障碍物运动):::process
    G --> H(生成局部地图):::process
    H --> I(删除越过的途经点):::process
    I --> J(生成新个体):::process
    J --> C
    C -- 是 --> K([结束]):::startend

4. 动态障碍物避障策略

动态障碍物通常分为两类：一类是轨迹预先已知的，另一类是轨迹未知的。前者的例子包括具有通信功能的多机器人运动规划，其中机器人会将其精确轨迹传达给其他所有机器人。如果障碍物的轨迹已知，避障策略可以预先计算。例如，如果知道人行道上前方的人要去特定商店并转弯，对面行走的人可以提前考虑到这一转弯。这对于规划来说是有价值的信息。

然而，如果没有关于障碍物轨迹的信息，规划就会变得更加困难。在这种情况下，需要猜测障碍物（可能是其他机器人、人、宠物或其他动态主体）的可能轨迹，这被称为意图检测，是一个难题。解决该问题的一种方法是通过了解地图中的有趣位置和障碍物主要朝向的方向来猜测目标位置。例如，根据运动情况可以猜测人可能前往出口、自动售货机等。知道目标后，使用连接到目标的最简单轨迹。但这会将问题转化为无通信的多机器人运动规划问题，这也是一个难题。最近解决轨迹预测和意图检测问题的方法是使用长短期记忆网络（LSTM），它可以回归运动行为并外推人的运动。

作为最简单的情况，可以通过人在过去一段时间内面对的最大方向来猜测其预期运动方向。人类大多直线朝着子目标行进，轨迹仅会因周围障碍物而暂时受到影响，因为人类会转弯避开它们。同样，人的首选导航速度是其在最近一段时间内显示的速度。较细粒度遗传算法仅进行短期障碍物轨迹预测，假设障碍物的运动由恒定的首选速度（vo）和恒定的运动方向（θo）决定，这两个参数都可以从最近的观察窗口中获得。其运动由以下公式给出：

[o(t) = o(0) + v_ot[\cos\theta_o, \sin\theta_o]^T]

其中，o(0) 是障碍物的当前位置，θo 是猜测的障碍物运动方向，vo 是障碍物的首选运动速度。

较细粒度遗传算法会创建一个随时间变化的地图，并将其用于运动规划。在遍历候选轨迹时，会模拟机器人以候选轨迹移动以及所有其他具有猜测轨迹的障碍物的运动。如果机器人不与任何静态障碍物或具有猜测运动的动态障碍物发生碰撞，则该候选轨迹被认为是可行的，相应的适应度将作为质量度量。

5. 总体算法及结果

由于静态障碍物可能突然出现，因此每次都需要添加足够的随机个体。否则，如果障碍物突然出现在机器人附近，所有个体可能都变得不可行，且难以快速修复。

两种算法的融合很值得关注。较细粒度遗传算法能够生成更精细的轨迹，具有避开突然出现的障碍物、适应动态障碍物以及为局部最优性创建精确精细轨迹的优势。同时，较粗粒度遗传算法选择要考虑的同伦组、较粗的避障策略以及避开每个障碍物的方式（顺时针/逆时针）。

总体算法的架构如下：

graph LR
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px;
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px;

    A([开始]):::startend --> B(空间分辨率降低):::process
    B --> C{机器人是否到达目标}:::decision
    C -- 否 --> D(获取较粗粒度遗传算法的最佳轨迹):::process
    D --> E(获取较细粒度遗传算法的最佳轨迹):::process
    E --> F(机器人运动控制):::process
    F --> C
    C -- 是 --> G([结束]):::startend

    H(较粗粒度遗传算法初始化):::process --> I(较粗粒度遗传算法进行遗传操作):::process
    I --> J(较粗粒度遗传算法进行适应度评估):::process
    J --> K(删除越过的途经点):::process
    K --> I

    L(较细粒度遗传算法初始化):::process --> M(较细粒度遗传算法进行遗传操作):::process
    M --> N(较细粒度遗传算法进行适应度评估):::process
    N --> O(预测障碍物运动):::process
    O --> P(生成局部地图):::process
    P --> Q(删除越过的途经点):::process
    Q --> R(生成新个体):::process
    R --> M

该算法在两种场景下的工作情况如下：在两种情况下，机器人移动后，当接近障碍物时会添加新的障碍物，这些障碍物在图中会明确标记。

通过上述双层遗传算法，机器人能够在复杂的环境中实现有效的运动规划，同时应对静态和动态障碍物的挑战。在实际应用中，我们可以根据具体场景选择合适的参数和算法设置，以达到最佳的运动规划效果。

双层遗传算法在机器人运动规划中的应用（续）

6. 算法相关问题探讨

为了更好地理解和应用双层遗传算法，我们针对一些常见问题进行探讨：

6.1 算法的最优性和完整性

在不同场景下，双层遗传算法的最优性和完整性表现不同：
- 静态场景 ：如果规划性组件是最优且完整的，在静态场景下，双层遗传算法通常可以实现较好的规划效果。较粗粒度遗传算法负责全局规划，较细粒度遗传算法进行局部优化，整体上能够找到一条相对较优的路径。
- 有移动人员的场景 ：由于移动人员的轨迹难以准确预测，算法可能无法保证找到最优路径。例如，当人员突然改变方向或速度时，机器人可能需要重新规划路径。此时可以结合一些预测算法，如 LSTM 网络，来提高对人员轨迹的预测准确性。
- 突然出现障碍物的场景 ：突然出现的障碍物可能导致当前规划的路径不可行，算法需要快速调整。如果添加随机个体的频率不够，可能会出现所有个体都不可行的情况。可以通过增加随机个体的数量和频率，以及加快算法的响应速度来解决。

6.2 与弹性路线图的比较

弹性路线图和双层遗传算法在动态环境下的移动机器人运动规划中各有特点：
| 比较项 | 弹性路线图 | 双层遗传算法 |
| ---- | ---- | ---- |
| 原理 | 通过弹性变形来适应环境变化 | 分两个层次进行轨迹优化 |
| 优点 | 能够快速适应局部环境变化 | 可以在全局和局部进行优化，适应复杂地图 |
| 缺点 | 全局规划能力相对较弱 | 计算复杂度较高 |

6.3 不同算法在狭窄走廊场景的表现

不同算法在狭窄走廊场景下的表现如下：
| 算法 | 表现 | 问题及解决方法 |
| ---- | ---- | ---- |
| A | 可能会陷入局部最优，导致路径过长 | 可以采用启发式函数的调整来改善 |
| 概率路线图 | 可能无法生成足够的路径点，导致规划失败 | 增加采样点数量 |
| 可见性路线图 | 在狭窄走廊中可能会出现大量的可见边，计算复杂度高 | 可以进行边的筛选和简化 |
| 人工势场法 | 可能会陷入局部极小值，导致机器人停滞 | 结合其他算法进行改进 |
| 快速探索随机树 | 可能需要较长时间才能找到可行路径 | 优化树的扩展策略 |
| A 和势场法的混合算法 | 综合了两者的优点，但仍可能存在局部最优问题 | 调整混合比例和参数 |

对于 6 自由度机械臂在狭窄走廊场景的规划问题，通过图像处理算子缩小障碍物尺寸并迭代优化路径的方法可能在一定程度上有效，但需要注意的是，缩小障碍物尺寸可能会导致路径在实际环境中不可行，需要进行充分的验证。

6.4 不同场景下的算法框架

根据不同的场景，为机器人从当前位置到办公室角落的运动规划提供以下算法框架：
- 无障碍物场景 ：
- 传感器 ：可以使用简单的定位传感器，如 GPS 或室内定位系统，来确定机器人的当前位置。
- 执行器 ：驱动机器人的电机等执行器。
- 算法 ：直接规划一条直线路径到达目标。
- 只有静态障碍物（复杂框架）场景 ：
- 传感器 ：激光雷达、摄像头等，用于检测障碍物的位置和形状。
- 执行器 ：驱动机器人的电机等执行器。
- 算法 ：较粗粒度遗传算法进行全局规划，较细粒度遗传算法进行局部优化。
- 静态障碍物（复杂框架）和周围有移动人员的场景 ：
- 传感器 ：激光雷达、摄像头、人体检测传感器等，用于检测障碍物和人员的位置和运动。
- 执行器 ：驱动机器人的电机等执行器。
- 算法 ：在上述基础上，增加对人员轨迹的预测，如使用 LSTM 网络。
- 静态障碍物（复杂框架）、周围有移动人员和一组相同类型机器人的场景 ：
- 传感器 ：除了上述传感器外，还需要机器人之间的通信设备，用于交换位置和轨迹信息。
- 执行器 ：驱动机器人的电机等执行器。
- 算法 ：考虑机器人之间的协作和避碰，采用多机器人运动规划算法。
- 无障碍物，一组相同类型机器人形成链/线的场景 ：
- 传感器 ：定位传感器和机器人之间的通信设备。
- 执行器 ：驱动机器人的电机等执行器。
- 算法 ：通过通信协调机器人的运动，保持链/线的形状。
- 静态障碍物和周围有移动人员，一组相同类型机器人形成链/线的场景 ：
- 传感器 ：综合上述传感器的功能。
- 执行器 ：驱动机器人的电机等执行器。
- 算法 ：在考虑障碍物和人员的同时，协调机器人之间的运动。
- 无障碍物，一组相同类型机器人形成圆圈并巡逻的场景 ：
- 传感器 ：定位传感器和异常检测传感器。
- 执行器 ：驱动机器人的电机等执行器。
- 算法 ：规划圆圈的巡逻路径，当检测到异常时，指定一个机器人前往安全办公室报告。

6.5 自动驾驶汽车行为控制

自动驾驶汽车的不同行为需要相应的控制器和适用前提条件：
| 行为 | 控制器 | 适用前提条件 |
| ---- | ---- | ---- |
| 变道 | 基于模型预测控制的变道控制器 | 前方道路状况良好，有足够的安全距离 |
| 超车 | 超车决策控制器和轨迹跟踪控制器 | 前方车辆速度较慢，有足够的超车空间 |
| 进出交叉口 | 交叉口通行决策控制器和轨迹规划控制器 | 交通信号灯状态明确，无其他冲突车辆 |
| 在行人区前停车 | 基于传感器的停车控制器 | 检测到行人区，有行人通过 |

可以采用分层架构来组织这些行为，上层负责决策，下层负责执行。同时，可以使用有限状态机或行为树来建模这些行为，通过不同架构的比较和权衡，选择最合适的方案。在室内环境中与人类一起导航时，需要考虑社交礼仪，如避让行人、合作超车等，可以通过增加社交规则的判断和执行模块来实现。

7. 编程实践

以下是一些编程实践的建议：
- 使用混合算法规划机器人路径 ：选择一种规划性算法和一种反应式算法，如 A 算法和人工势场法，进行混合。首先使用 A 算法进行全局规划，然后在局部使用人工势场法进行优化。在多机器人场景中，需要考虑机器人之间的避碰和协作。
- 编程实现两连杆旋转机械臂 ：模拟机械臂在有静态和动态障碍物的环境中向目标移动。可以使用机器人操作系统（ROS）来实现，通过传感器获取障碍物信息，使用运动规划算法生成机械臂的运动轨迹。
- 模拟机器人的多种行为 ：使用有限状态机（FSM）或行为树（BT）来模拟机器人的行为。例如，机器人在接到指令后前往厨房取零食或查找邮件，同时需要考虑电池电量和安全检查等因素。可以使用 Python 语言结合相关的库来实现。
- 融合多个算法的轨迹 ：使用多个规划性算法在不同参数设置下生成轨迹，然后通过加权平均或其他方法将这些轨迹融合成一个轨迹。同样，对于反应式算法也可以进行融合。在实际应用中，可以根据不同的场景和需求调整权重，使机器人的运动更加灵活和高效。

通过以上的探讨和实践，我们可以更深入地理解双层遗传算法在机器人运动规划中的应用，以及如何根据不同的场景和需求进行算法的选择和优化。在未来的研究和应用中，可以进一步探索更高效的算法和技术，提高机器人在复杂环境中的运动规划能力。