人形机器人第四章全局路径规划与局部避障

jjhw_ · 2026-07-06 21:56:31 发布

动态障碍物去除

方法

下图是两种方法全局路径规划

一、左侧：ROS 程序外层调度（程序整体运行逻辑）

启动 ROS 节点 打开规划程序，注册 ROS 通信节点，准备接收地图、机器人位置、目标点数据。
节点初始化，开启 3 个数据回调
- 地图回调：接收激光 SLAM 建好的栅格地图，做膨胀地图（把障碍物向外扩一圈，防止机器人贴墙碰撞）；
- 里程计回调：实时读取机器人当前位置，作为寻路起点；
- 目标点回调：接收用户 / 上层下发的终点坐标。
判断是否同时拿到地图 + 起点 + 终点
- 没凑齐数据 → 等待数据持续监听；
- 三样数据全部到位 → 启动 A * 核心寻路计算。
A * 算出路径后处理把栅格地图坐标转成真实世界坐标系，通过话题发布规划好的路线，流程结束。

二、右侧虚线框：A * 核心寻路循环（找最短无碰撞路径）

1. 初始化准备

创建两个集合：

2. 主循环（反复执行直到找到终点 / 无路可走）

检查开放集是否空了 空 = 所有可行格子都遍历完，找不到终点，规划失败；
从开放集取出「综合代价最小」的最优格子 A * 核心：代价 = 已经走的路程 + 预估到终点的剩余距离，优先走综合代价最低的格子；
判断这个格子是不是终点
- 是终点：直接跳出循环，规划结束；
- 不是终点：把这个格子放进封闭集，不再重复计算；
遍历当前格子上下左右 8 个邻居格子 逐个校验邻居： ① 邻居是障碍物 / 已经在封闭集 → 直接跳过，不处理； ② 有效空白格子：计算走到这个邻居的综合代价； ③ 对比旧路径代价，如果这条新路更优：更新代价、记录父节点（用来回溯整条路径），放入开放集；
回到主循环开头，重复迭代。

3. 循环结束后回溯路径

从终点顺着记录的父节点反向倒推，就能得到完整从起点到终点的最优路线。

大白话总结

程序一边实时收地图、机器人位置、目标点，三样齐全就启动 A； A不断优先走 “离起点近、离终点也近” 的格子，避开障碍物，记录每个格子的来路；碰到终点就反向拉出完整路线，没格子可走就报规划失败。

DWA 局部路径规划通俗完整讲解

整张图分为两大模块：左侧DWA Planner 局部规划器、右侧Position Control 总控制调度，配合前面 A * 全局路径实现「全局指路 + 局部避障」导航。

一、右侧 Position Control 总调度（ROS 顶层循环）

初始化节点 + 订阅所有话题
- 里程计话题：获取机器人实时位置；
- 目标点话题：接收导航终点；
- A全局路径话题：拿到 A算出的远距离全局路线；
- 障碍物话题：接收激光点云识别的障碍物信息。
进入持续控制循环 判断当前视野内有没有障碍物：
- 有障碍物：切 DWA 避障控制，走左侧 DWA 逻辑；
- 无障碍物：正常跟踪 A * 全局路径，根据当前路径点计算前进速度；
统一输出速度指令/cmd_vel发给机器人底盘，循环往复直到抵达终点。

二、左侧 DWA Planner 局部避障核心流程

1. 初始化与数据实时更新

启动时先设置机器人尺寸、最大速度、安全距离等参数；三个回调实时刷新数据：

2. 环境状态判断

结合机器人位置、全局参考路径、障碍物点云，识别前方障碍物，判断是否需要启动避障 DWA：

需要避障（有障碍物）→ DWA 避障模式
1. 速度采样：在机器人速度上下限内，批量生成多组「线速度 + 角速度」候选组合，每组速度推演一条短期局部轨迹；
2. 打分评估：给每条模拟轨迹算综合分数（贴近全局路径分 + 远离障碍物安全分 + 速度顺滑分）；
3. 择优选择：选出总分最高、安全又贴合全局路线的速度参数。
无需避障（无障碍物）→ 正常跟随模式 直接沿 A * 全局路径匀速前进，不做复杂轨迹采样。

3. 终点判断与指令下发