Path Planning - 路径规划¶
路径规划是机器人导航的核心技术，它使机器人能够从起点找到一条到达终点的安全路径。
什么是路径规划？¶
┌─────────────────────────────────────────────────────────────┐
│ 路径规划问题 │
├─────────────────────────────────────────────────────────────┤
│ │
│ 输入： │
│ - 起点位置 │
│ - 终点位置 │
│ - 环境地图 │
│ - 机器人模型 │
│ │
│ 输出： │
│ - 路径点序列 │
│ - 路径长度 │
│ - 路径安全性 │
│ │
│ 约束： │
│ - 避障 │
│ - 运动学约束 │
│ - 动力学约束 │
│ - 时间约束 │
│ │
└─────────────────────────────────────────────────────────────┘
路径规划算法¶
1. 搜索算法¶
| 算法 | 特点 | 适用场景 |
|---|---|---|
| Dijkstra | 最短路径，无启发式 | 任意图 |
| A* | 最短路径，有启发式 | 已知地图 |
| D* | 动态规划 | 动态环境 |
| Lattice | 离散化搜索 | 网格地图 |
2. 采样算法¶
| 算法 | 特点 | 适用场景 |
|---|---|---|
| RRT | 快速随机探索 | 高维空间 |
| RRT* | 渐近最优 | 需要最优路径 |
| PRM | 概率路线图 | 多查询 |
| Informed RRT* | 启发式采样 | 高效搜索 |
3. 优化算法¶
| 算法 | 特点 | 适用场景 |
|---|---|---|
| TrajOpt | 轨迹优化 | 平滑路径 |
| CHOMP | 协作优化 | 避障优化 |
| STOMP | 随机优化 | 非梯度优化 |
ROS Navigation Stack¶
ROS Navigation Stack 是 ROS 中最常用的导航框架。
架构¶
┌─────────────────────────────────────────────────────────────┐
│ ROS Navigation Stack │
├─────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ Global Planner │ │
│ │ (全局路径规划) │ │
│ └─────────────────────┬───────────────────────────────┘ │
│ │ │
│ ▼ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ Local Planner │ │
│ │ (局部路径规划) │ │
│ └─────────────────────┬───────────────────────────────┘ │
│ │ │
│ ▼ │
│ ┌─────────────────────────────────────────────────────┐ │
│ │ Controller │ │
│ │ (控制器) │ │
│ └─────────────────────────────────────────────────────┘ │
│ │
│ 输入： │
│ - 地图 (map_server) │
│ - 传感器数据 (激光雷达、深度相机) │
│ - 里程计 (odom) │
│ - 目标点 (move_base) │
│ │
│ 输出： │
│ - 速度命令 (cmd_vel) │
│ │
└─────────────────────────────────────────────────────────────┘
核心组件¶
1. move_base¶
move_base 是导航栈的核心节点，负责协调全局和局部规划。
2. 地图服务器¶
3. 定位¶
配置文件¶
costmap_common_params.yaml¶
# 通用代价地图参数
obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]
inflation_radius: 0.55
observation_sources: laser_scan_sensor
laser_scan_sensor: {
sensor_frame: laser,
data_type: LaserScan,
topic: /scan,
marking: true,
clearing: true
}
global_costmap_params.yaml¶
# 全局代价地图参数
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
static_map: true
rolling_window: false
resolution: 0.05
inflation_radius: 0.55
local_costmap_params.yaml¶
# 局部代价地图参数
local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 5.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
inflation_radius: 0.3
base_local_planner_params.yaml¶
# 局部规划器参数
TrajectoryPlannerROS:
max_vel_x: 0.5
min_vel_x: 0.1
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 0.4
escape_vel: -0.1
acc_lim_x: 2.5
acc_lim_theta: 3.2
holonomic_robot: false
meter_scoring: true
pdist_scale: 0.6
gdist_scale: 0.8
occdist_scale: 0.1
heading_lookahead: 0.325
dwa: true
使用 Navigation Stack¶
步骤 1：启动导航¶
# 启动机器人
roslaunch my_robot bringup.launch
# 启动导航
roslaunch my_robot navigation.launch
# 启动 RViz
roslaunch my_robot rviz_navigation.launch
步骤 2：设置初始位置¶
在 RViz 中使用 "2D Pose Estimate" 设置机器人初始位置。
步骤 3：设置目标位置¶
在 RViz 中使用 "2D Nav Goal" 设置目标位置。
步骤 4：监控导航¶
# 查看路径规划
rostopic echo /move_base/NavfnROS/plan
# 查看速度命令
rostopic echo /cmd_vel
# 查看导航状态
rostopic echo /move_base/status
路径规划算法实现¶
1. A* 算法¶
import heapq
def astar(grid, start, goal):
"""A* 路径规划算法"""
# 启发式函数（曼哈顿距离）
def heuristic(a, b):
return abs(a[0] - b[0]) + abs(a[1] - b[1])
# 初始化
open_set = []
heapq.heappush(open_set, (0, start))
came_from = {}
g_score = {start: 0}
f_score = {start: heuristic(start, goal)}
while open_set:
current = heapq.heappop(open_set)[1]
if current == goal:
# 重建路径
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(start)
return path[::-1]
# 探索邻居
for dx, dy in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
neighbor = (current[0] + dx, current[1] + dy)
# 检查边界和障碍物
if (0 <= neighbor[0] < len(grid) and
0 <= neighbor[1] < len(grid[0]) and
grid[neighbor[0]][neighbor[1]] == 0):
tentative_g = g_score[current] + 1
if neighbor not in g_score or tentative_g < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
heapq.heappush(open_set, (f_score[neighbor], neighbor))
return None # 没有找到路径
2. RRT 算法¶
import random
import math
def rrt(start, goal, obstacles, max_iter=1000, step_size=0.5):
"""RRT 路径规划算法"""
# 初始化树
tree = {start: None}
for _ in range(max_iter):
# 随机采样
if random.random() < 0.1:
q_rand = goal
else:
q_rand = (random.uniform(0, 10), random.uniform(0, 10))
# 找到最近节点
q_near = min(tree.keys(), key=lambda q: distance(q, q_rand))
# 扩展树
q_new = steer(q_near, q_rand, step_size)
# 检查碰撞
if not collision_free(q_near, q_new, obstacles):
continue
# 添加到树
tree[q_new] = q_near
# 检查是否到达目标
if distance(q_new, goal) < step_size:
# 重建路径
path = [goal]
current = q_new
while current is not None:
path.append(current)
current = tree[current]
return path[::-1]
return None # 没有找到路径
避障策略¶
1. 静态避障¶
使用代价地图（Costmap）进行避障。
2. 动态避障¶
使用局部规划器（DWA、TEB）进行动态避障。
# DWA 参数
max_vel_x: 0.5
min_vel_x: 0.1
max_vel_theta: 1.0
min_vel_theta: -1.0
acc_lim_x: 2.5
acc_lim_theta: 3.2
3. 人群避障¶
使用社会力模型或深度学习进行人群避障。
导航调试¶
1. 参数调优¶
2. 可视化¶
在 RViz 中添加： - 全局代价地图 - 局部代价地图 - 全局路径 - 局部路径 - 机器人模型
3. 日志分析¶
常见问题¶
1. 路径规划失败¶
原因：目标点在障碍物内或代价地图配置错误
解决方案： - 检查目标点是否有效 - 调整代价地图参数 - 增加探索范围
2. 机器人抖动¶
原因：局部规划器参数不当
解决方案： - 调整速度限制 - 增加平滑参数 - 使用更好的规划器（TEB）
3. 避障失败¶
原因：传感器数据不准确或代价地图配置错误
解决方案： - 校准传感器 - 调整障碍物检测范围 - 增加安全距离
实验¶
Lab 3-1: 路径规划¶
- 启动导航栈
- 设置初始位置
- 设置目标位置
- 观察路径规划过程
- 测试避障功能
