多机器人协同全覆盖路径规划

REF：面向多机器人协作的路径规划与跟踪 控制研究

1. 基础模型

• 单机器人全覆盖规划算法：通常包括三种方法：单元分解法、基于栅格的方法和基于图的方法

cfdf6c01a0e54deff8b5f08cbdcdaafe.png

• 单元分解法：将区域划分为多个子区域，按顺序依次对每个子区域全覆盖路径规划
• 基于栅格方法：基于栅格地图上进行全路径覆盖的优化方法
• 基于图的方法：基于遗传算法或蚁群算法进行建模和遍历

• 多机器人全覆盖：在单机器人全覆盖路径规划的基础上，并结合任务分配、区域划分、路径优化、机器人协作和环境适应性等策略，实现更智能的全覆盖路径规划
• 基于图优化的 Cartographer 算法：通过前后端分离的结构，结合激光雷达、IMU 及里程计信息，实现机器人的定位与建图

24e521097c7b0d8e6ced3d46c8b208cb.png

• 前端：由扫描匹配、运动滤波和子图构建等模块，实现机器人当前位姿的实时估计
• 回环：当检测到机器人空间距离近和激光匹配度高，则与之前子图建立约束边
• 后端：通过构建约束建立稀疏位姿图，建立最小化各约束之间的误差，得到优化后的子图位姿

• DARP（Divide Areas based on Robots initial Positions）方法：基于机器人数目与初始位置对空闲区域进行空间分区，使每个机器人能够在各自的子区域内独立执行路径规划任务实现分布式的全覆盖路径规划，将全覆盖路径规划的多机器人协作的问题转化为单机器人规划问题（生成树覆盖算法），从而减少计算复杂度。

8b083e2c7b03bf1951172d0720a468ae.png

• TMSTC*（Turn-Minimizing Multi-Robot Spanning Tree Coverage Star）算法：采用最小砖块覆盖和贪婪策略合并砖块生成树方法，优化区域分区和路径规划，以减少路径中不必要转向。
• 牛耕法：通过“Z”字形的往复直线运动覆盖整个区域，在无障碍且边界规整的场景中具有较高的覆盖效率，但复杂环境中容易陷入覆盖死区
• 时间弹性带算法（TEB）：将机器人的轨迹表述为加上时间间隔的离散位姿序列，将时间作为变量纳入到轨迹优化中，使机器人的局部路径不仅优化空间上的轨迹，还考虑时间维度，从而直接对速度、加速度等动力学约束进行优化。
  • 避障与路径跟踪约束：基于轨迹点到障碍物、参考路径点距离、偏离全局路径最大不惩罚边界、最小安全距离等
  • 速度与加速度约束：速度与加速度在有限范围变化，由相邻位姿与时间信息计算机器人状态，得到线速度、角速度、线加速度及角加速度约束
  • 非完整运动学约束：底盘模型运动不具备完整性，拥有最小转弯半径，两个位姿点之间是曲率不变的弧
• 评价指标：可以进行综合评价地图分区质量以及路径规划效果的指标
  • 任务均衡性：评估各机器人所承担的任务负载是否均衡，可使用各子区域面积的标准差表示
  • 分区形状复杂度：避免狭长或碎片化区域导致的不必要转向，可使用区域边界长度的平方与面积的比值表示
  • 转弯次数：转弯次数越少意味着越平滑，定义机器人转弯幅度大于 60°认为是转弯计入转弯次数
  • 路径长度：各机器人所需路径的总长度，路径长度越短，说明任务执行效率越高
  • 覆盖率：表示机器人实际覆盖区域与待覆盖区域的比值，覆盖率越高效果越好
  • 任务执行时间：整个多机器人系统完成全覆盖任务所需的时间，分别统计各机器人的执行时间
  • 死区点数：子区域内无法覆盖的区域数量，死区点数越少意味着地图分区方法越合理

2. 改进DARP 算法实现路径跟踪

• 波前算法计算各栅格的距离代价：先将机器人初始位置和障碍物栅格的波前值设置为 0和无穷大，代表代价为零和不可通行；再以机器人初始位置为起点，取其上下左右 4 个邻域栅格波前值最小者加 1 作为当前栅格的波前值，依次扩展直至整个地图被完整覆盖

3d232bb9625fb6b6b604bfed029f1c82.png

• 子区域边界平滑：代价函数增加边界的曲率惩罚，使 DARP 算法在分区过程中更倾向于生成平滑、规则的子区域边界，可以减少后续子区域内的机器人过多的转弯次数

1b230a216704078f7d909d3c0161e7b0.png

• 融合前两项：同时优化区域划分的精确性和边界平滑性，权重因子用于调节评估矩阵与边界平滑代价的相对重要程度

db203172bff7b2e5f475bec64bae913a.png

• 融合牛耕法和Astar算法（BA*）：结合牛耕法的高效覆盖策略，并通过 Astar的路径连接能力解决牛耕法在复杂环境下产生的死区问题，有效地提升多机器人协作全覆盖任务的整体效率，流程如下：
  1. 按照牛耕方式沿平行线覆盖，并在遇到障碍物时进行左右避障处理
  2. 在规划路径完成后，检测地图中的未覆盖区域，记录机器人进入死区点所在的最近位置
  3. 以机器人当前位置为起点，未覆盖区域路径起点为目标，利用 A搜索出一条无碰撞路径进行连接，机器人沿 A路径前进，并在到达目标后继续执行牛耕覆盖。
  4. 通过路径规划的状态管理机制，实现机器人在牛耕路径与 A*路径之间的自动切换，直至所有目标区域覆盖完成
• 路径平滑优化：使用二次贝塞尔曲线路径平滑优化方法，将生成路径中的转弯处进行插值优化，以实现更加平滑的路径衔接。
• move_base_flex路径规划框架：其能够更灵活地调用不同的全局规划器

a763aa1bed5a1e394c71256f7e6ef7df.png

1. 生成全局路径：将 BA*封装为 MBF 的全局规划器插件，并替换默认导航栈全局规划器 Global_planner，根据机器人初始位置与地图子区域生成机器人全局覆盖路径
2. 轨迹跟踪：采用 TEB 规划控制器进行局部路径跟踪，并依据全局路径、机器人定位信息与传感器信息进行轨迹跟踪
3. 考虑底盘的运动特性，设定25cells作为全局路径规划的标准，以满足机器人最小转弯半径（0.6 m）的运动约束

• 动态障碍物感知和轨迹预测：从激光雷达数据中提取点云信息，并通过聚类算法识别出动态障碍物，具体步骤：
  1. 激光雷达数据预处理： 将点云数据从激光雷达坐标系转换到全局坐标系，并采用局部密度滤波方法，剔除离群点
  2. 动态障碍物聚类识别：采用 DBSCAN算法，基于密度分布特征，将距离接近的点归为同一簇，并剔除噪声点
  3. 动态障碍物跟踪：障碍物静态与动态的区别是点云在时序上的位置变化，采用目标匹配和运动估计方法，跟踪障碍物的运动状态
  4. 轨迹预测：采用匀速运动模型，假设障碍物在短时间内保持恒定速度运动，预测障碍物未来位置
• 基于 TEB 动态避障优化：传统的 TEB 方法仅能进行静态障碍物位置判断，为增强 TEB 规划控制器在动态避障性能，使用融合距离、时间以及速度方向约束的动态避障约束函数，提高TEB 规划控制器对动态障碍物的避障能力
  • 避障速度约束：使用椭圆模型精确地模拟障碍物的运动趋势，长轴代表运动方向，长度代表速度，椭圆形状覆盖未来可能到达的区域

  

  fd900607afb1b57911034500332a1868.png

  

  da7fa0b78cc080dd32f315d8bbb9bad7.png

  • 目标对齐约束：确保机器人始终朝向目标的方向运动，约束机器人速度向量与目标方向之间夹角的余弦值

  

  b0997d0e11a5b75e4ed9b9ab433ecb1c.png

  

  d23fe4d858f0f60c200e519296a3e138.png

  • 位置计算：从动态障碍物的预测轨迹中，选择一个与机器人位置最近的点作为预测点
  • 时间计算：分别计算机器人和障碍物到达该预测点的时间
  • 计算时间差：评估机器人和障碍物到达预测点的时间差
  • 惩罚函数：在动态障碍物的预测路径上计算机器人路径的安全性

  

  dfd08d03318ce6a3eb95207a3b088548.png

  • 归一化距离：反映障碍物在不同方向上的影响
  • 距离约束惩罚函数：为了确保机器人轨迹点与障碍物预测轨迹点之间保持足够的安全距离

  

  207824f7efa7c7e944374070bbf9e806.png

  • 预测轨迹点索引：根据机器人与障碍物的相对位置，动态调整选取的预测点索引
  • 障碍物距离约束：对机器人路径点与障碍物未来预测位置之间的距离进行约束
  • 避障时间约束：预测机器人与障碍物同时到达同一位置的可能性，并确保两者之间存在足够的时间间隔
  • 速度方向约束：仅依靠距离和时间约束，不足以确保机器人安全且高效地避开动态障碍物