ROS2入门到精通—— 2-8 ROS2实战：机器人安全通过狭窄区域的方案

0 前言

室内机器人需要具备适应性和灵活性，以便在狭窄的空间中进行安全、高效的导航。本文提供一些让机器人在狭窄区域安全通过的思路，希望帮助读者根据实际开发适当调整和扩展

1 Voronoi图

Voronoi图：根据给定的一组“种子点”，将平面划分为各个区域，每个区域内的所有点到该区域的种子点的距离比到其他任何种子点的距离都近

Voronoi diagram特征：

• 每个泰森多边形仅含有一个离散点数据
• 泰森多边形内的点到相应离散点的距离最近
• 位于泰森多边形边上的点到其两边的离散点的距离相等，两边离散点的垂直平分线

如图所示，使用维诺图可以保证机器人行走在狭窄区域的最中间位置，从而可以安全通过狭窄区域

实现思路：
（1）环境建模：对机器人所在环境建模，并生成包含障碍物和可行区域的地图（利用建图算法）
（2）生成Voronoi图：使用环境地图，通过计算生成Voronoi图
（3）路径规划：基于生成的Voronoi图，应用A*或DP找到最优路径
（4）局部跟踪：跟踪找到的最优路径，在执行过程中，机器人可能会使用实时感知数据动态避障，确保在狭窄区域不会与障碍物发生碰撞

使用voronoi planner的优点：有效将机器人的路径规划和避障结合起来，考虑了环境的局部特征，使机器人能够选择相对较宽的通道穿越狭窄区域，提高了导航的效率和安全性

2 代价地图

A*算法会贴着障碍物走，过窄门等狭窄环境效果不好，容易撞上去，这时候可以调大代价地图中的膨胀层

（1） Global Costmap
全局代价地图主要用于全局的路径规划器
包含的图层：

• Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图，包含了环境中固定不变的信息，如墙壁等，静态地图层提供了机器人导航的基本参考
• Obstacle Map Layer：障碍地图层，用于动态记录传感器感知到的障碍物信息，确保机器人能够避开这些障碍物
• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物

（2） Local Costmap
局部代价地图主要用于局部的路径规划器
包含的图层有：

• Obstacle Map Layer：障碍地图层，用于动态记录传感器感知到的障碍物信息
• Inflation Layer：膨胀层，在障碍地图层上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物

自定义代价地图层：根据业务需求进行临时设置的地图数据，根据特定的场景或任务，在代价地图中添加自定义的代价信息，如优先避让某些区域或者设置特定的路径约束等

实现思路：

• 动态调整膨胀层参数
• 自定义代价地图插件

3 按照预先给定的路径给控制器去跟踪

实现思路：

• 方案一：键盘控制实车录制路径，然后录制的路径作为全局路径给局部控制器去跟踪
• 方案二：写一个脚本让实车到达指定目标点再进行下一次跟踪，例如过门，一遍导不出去，会撞到门，像如下图将一个目标点拆分为多个目标点（原先是(1)->(4)，现在变成(1)->(2)->(3)->(4)）
  
• 方案三：在Rviz上或者网页B端点几个点，每个点之间用直线或贝塞尔或B样条，生成你想要的全局路径给局部控制器去跟踪

4 通过二次规划的方法帮助机器人在狭窄区域中实现路径规划和避障

实现思路：

• 环境建模：机器人通过SLAM算法，利用传感器数据建图
• 路径生成：根据上面的环境建模，生成一条全局路径穿过狭窄区域（Voronoi算法）
• 路径优化：得到的初始路径可能不是最优的或者不满足机器人动力学约束，接下来使用二次规划的方法对初始路径进行优化。路径的二次规划方法可以考虑机器人的动力学模型和环境约束，生成一个在狭窄通道中最优的可行路径
• 约束建模：将机器人的动力学约束和环境约束表示为一组等式或者不等式约束。动力学约束可以包含速度、加速度、转弯半径，环境约束可以包含避开障碍物或保持一定的安全距离
• 代价函数定义：根据具体的应用场景定义
• 优化求解
• 路径执行：求解的路径给局部控制器去跟踪

---

机器人导航通过狭窄区域是一个具有挑战性的任务，本文讨论了四种方法：voronoi、costmap、预先给定路径、二次规划优化全局路径，这些方法提供了一种思路和参考，可以根据实际需求改进和扩展