动手学习：路径规划原理及常用算法
一、路径规划的基本原理
路径规划（Path Planning）是机器人导航的核心任务，目标是为机器人找到一条从起点到终点的无碰撞路径，同时满足约束条件（如最短路径、最优能耗、安全性等）。在人形机器人场景中，路径规划需要考虑复杂环境（如障碍物、地形变化）和机器人自身的运动学约束（如步态、关节限制）。
1. 路径规划的核心要素
- 环境建模：将环境表示为可计算的形式（如栅格地图、拓扑图）。
- 搜索空间：定义机器人可移动的区域，通常是二维（2D）或三维（3D）空间。
- 约束条件：
- 运动学约束：人形机器人的关节角度、步幅限制。
- 动态约束：速度、加速度限制。
- 环境约束：避开障碍物，适应地形。
- 优化目标：最短路径、最小能耗、最大安全性等。
2. 路径规划的分类
- 全局路径规划：基于已知环境地图，规划从起点到终点的完整路径。
- 局部路径规划：基于实时传感器数据（如激光雷达、视觉），动态调整路径以避开障碍物。
- 混合路径规划：结合全局和局部规划，适用于复杂动态环境。
二、常用路径规划算法
1. Dijkstra 算法
- 原理：
- Dijkstra 是一种基于图搜索的算法，适用于离散化的环境（如栅格地图）。
- 它通过计算从起点到所有节点的最短路径，找到目标点的最优路径。
- 特点：保证最优解，但计算复杂度较高（(O(V^2)) 或 (O(E + V \log V)) 使用优先队列）。
- 实现方案：
- 环境建模：将环境离散化为栅格地图，每个格子为一个节点，相邻格子间有边，边的权重为移动代价（如距离）。
- 算法实现：使用 Python 和优先队列（
heapq）实现 Dijkstra。import heapq def dijkstra(grid, start, goal): rows, cols = len(grid), len(grid[0]) dist = {(r, c): float('inf') for r in range(rows) for c in range(cols)} dist[start] = 0 pq = [(0, start)] came_from = {} while pq: d, (r, c) = heapq.heappop(pq) if (r, c) == goal: break for nr, nc in [(r+1, c), (r-1, c), (r, c+1), (r, c-1)]: # 4方向移动 if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0: # 0表示可通行 new_dist = d + 1 if new_dist < dist[(nr, nc)]: dist[(nr, nc)] = new_dist came_from[(nr, nc)] = (r, c) heapq.heappush(pq, (new_dist, (nr, nc))) path = [] curr = goal while curr in came_from: path.append(curr) curr = came_from[curr] path.append(start) return path[::-1] - 应用到人形机器人：将路径点转换为机器人可执行的步态序列，结合运动学模型调整步伐。
- 避坑指南：
- 计算效率：Dijkstra 适合小规模地图，地图过大（>1000x1000）时会变慢，建议使用 A* 替代。
- 障碍物处理：确保障碍物在地图中正确标记（例如用 1 表示不可通行，0 表示可通行）。
- 参数经验值：栅格分辨率建议为 [0.05m, 0.1m]，过小增加计算量，过大丢失细节。
- 扩展资源：
- 论文：《A Formal Basis for the Heuristic Determination of Minimum Cost Paths》（Dijkstra 算法基础）
- 开源实现：Python Robotics（GitHub）
2. A 算法*
- 原理：
- A* 是 Dijkstra 的改进版，引入启发式函数（heuristic function）加速搜索。
- 总代价 (f(n) = g(n) + h(n))，其中 (g(n)) 是起点到当前节点的代价，(h(n)) 是当前节点到目标的估计代价（如欧几里得距离）。
- 特点：比 Dijkstra 更快，适合实时性要求较高的场景。
- 实现方案：
- 启发式函数：使用曼哈顿距离或欧几里得距离作为 (h(n))。
- 算法实现：
def a_star(grid, start, goal): rows, cols = len(grid), len(grid[0]) open_set = [(0, start)] g_score = {(r, c): float('inf') for r in range(rows) for c in range(cols)} g_score[start] = 0 f_score = {(r, c): float('inf') for r in range(rows) for c in range(cols)} f_score[start] = abs(goal[0] - start[0]) + abs(goal[1] - start[1]) # 曼哈顿距离 came_from = {} while open_set: _, (r, c) = heapq.heappop(open_set) if (r, c) == goal: break for nr, nc in [(r+1, c), (r-1, c), (r, c+1), (r, c-1)]: if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0: tentative_g = g_score[(r, c)] + 1 if tentative_g < g_score[(nr, nc)]: came_from[(nr, nc)] = (r, c) g_score[(nr, nc)] = tentative_g f_score[(nr, nc)] = tentative_g + abs(goal[0] - nr) + abs(goal[1] - nc) heapq.heappush(open_set, (f_score[(nr, nc)], (nr, nc))) path = [] curr = goal while curr in came_from: path.append(curr) curr = came_from[curr] path.append(start) return path[::-1] - 人形机器人适配：A* 生成的路径可以作为全局路径，结合局部规划（如动态避障）调整。
- 避坑指南：
- 启发式函数选择：(h(n)) 必须是可接受的（admissible），即不能高估真实代价，否则可能找不到最优解。
- 性能瓶颈：在动态环境中，频繁重规划可能导致延迟，建议结合局部规划。
- 参数经验值：启发式函数权重（如果加权）建议为 [0.5, 1.5]，过大可能导致非最优解。
- 扩展资源：
- 论文：《A* Search Algorithm》（Hart et al., 1968）
- 开源项目：ROS Navigation Stack（包含 A* 实现，ROS）
3. RRT（Rapidly-exploring Random Tree）
- 原理：
- RRT 是一种基于采样的路径规划算法，通过随机采样构建树形结构，探索可行路径。
- 特点：适合高维空间和复杂环境，不需要离散化地图。
- 实现方案：
- 环境建模：直接使用连续空间，定义障碍物区域。
- 算法实现：
import numpy as np class RRT: def __init__(self, start, goal, obstacle_list, step_size=0.1, max_iter=1000): self.start = np.array(start) self.goal = np.array(goal) self.obstacle_list = obstacle_list # 障碍物列表 [(x, y, radius), ...] self.step_size = step_size self.max_iter = max_iter self.nodes = [self.start] def plan(self): for _ in range(self.max_iter): rand_point = self.random_point() nearest_node = self.nearest_node(rand_point) new_node = self.steer(nearest_node, rand_point) if self.is_collision_free(nearest_node, new_node): self.nodes.append(new_node) if np.linalg.norm(new_node - self.goal) < self.step_size: return self.reconstruct_path(len(self.nodes) - 1) return None def random_point(self): if np.random.rand() < 0.1: # 10% 概率直接采样目标点 return self.goal return np.array([np.random.uniform(0, 10), np.random.uniform(0, 10)]) def nearest_node(self, point): return self.nodes[np.argmin([np.linalg.norm(node - point) for node in self.nodes])] def steer(self, from_node, to_node): direction = to_node - from_node distance = np.linalg.norm(direction) if distance < self.step_size: return to_node return from_node + self.step_size * direction / distance def is_collision_free(self, from_node, to_node): for obs in self.obstacle_list: center, radius = obs[:2], obs[2] if np.linalg.norm(to_node - center) < radius: return False return True def reconstruct_path(self, end_idx): path = [self.goal] idx = end_idx while idx != 0: path.append(self.nodes[idx]) idx = np.argmin([np.linalg.norm(self.nodes[idx] - self.nodes[i]) for i in range(idx)]) path.append(self.start) return path[::-1] - 人形机器人适配：RRT 生成的路径可能不平滑，需后处理（如样条插值）生成适合步态的轨迹。
- 避坑指南：
- 随机性：RRT 的路径非最优，多次运行结果可能不同，建议结合 RRT*（优化版）。
- 计算开销：迭代次数（
max_iter）过大（如>5000）会影响实时性，建议范围 [500, 2000]。 - 参数经验值：步长（
step_size）建议为 [0.05m, 0.2m]，过小导致路径过密，过大可能错过狭窄通道。
- 扩展资源：
- 论文：《Rapidly-exploring Random Trees》（LaValle, 1998）
- 开源项目：OMPL（Open Motion Planning Library，OMPL）
4. 强化学习路径规划（扩展方案）
- 原理：
- 使用强化学习（如 PPO）训练一个策略，直接从状态（机器人位置、障碍物信息）映射到动作（移动方向）。
- 特点：适合动态环境，能学习复杂策略。
- 实现方案：
- 环境定义：使用 OpenAI Gym 自定义环境，状态包括机器人位置和目标位置，动作是移动方向。
- 奖励函数：
- 接近目标奖励：
exp(-distance_to_goal)。 - 碰撞惩罚：
-10（如果撞到障碍物）。 - 移动惩罚：
-0.1（每步小惩罚，鼓励效率）。
- 接近目标奖励：
- 训练：使用 PPO 训练策略网络。
from stable_baselines3 import PPO import gym class PathPlanningEnv(gym.Env): def __init__(self): super().__init__() self.action_space = gym.spaces.Discrete(4) # 上、下、左、右 self.observation_space = gym.spaces.Box(low=0, high=10, shape=(2,)) # 位置 self.position = [0, 0] self.goal = [9, 9] def reset(self): self.position = [0, 0] return np.array(self.position) def step(self, action): if action == 0: self.position[1] += 1 # 上 elif action == 1: self.position[1] -= 1 # 下 elif action == 2: self.position[0] -= 1 # 左 elif action == 3: self.position[0] += 1 # 右 distance = np.linalg.norm(np.array(self.position) - np.array(self.goal)) reward = np.exp(-distance) done = distance < 0.5 return np.array(self.position), reward, done, {} env = PathPlanningEnv() model = PPO("MlpPolicy", env, verbose=1) model.learn(total_timesteps=10000)
- 避坑指南：
- 奖励稀疏：如果目标距离远，奖励可能过于稀疏，建议添加中间奖励（如接近目标的奖励）。
- 探索不足：PPO 可能陷入局部最优，建议增加探索（如熵正则化系数 [0.01, 0.1]）。
- 参数经验值：学习率建议为 [1e-4, 3e-4]，批次大小 [32, 128]。
- 扩展资源：
- 论文：《Deep Reinforcement Learning for Path Planning》（arXiv）
- 开源项目：Stable Baselines3（GitHub）
三、结合人形机器人场景的路径规划
在人形机器人中，路径规划需要考虑以下特殊性：
- 运动学约束：人形机器人不能像轮式机器人那样自由移动，需考虑步态、关节角度限制。
- 解决方案：将路径点转换为步态序列，使用逆运动学（IK）计算关节角度。
- 动态环境：人形机器人可能在动态环境中操作（如移动障碍物）。
- 解决方案：结合局部规划（如 DWA，下一节会讲到）动态调整路径。
- 地形适应：人形机器人需要适应不平坦地形（如台阶、斜坡）。
- 解决方案：在路径规划中引入高度信息，使用 3D 地图。
四、学习建议
- 实践：
- 使用 Python Robotics 或 ROS Navigation Stack 实现 Dijkstra 和 A*。
- 在 PyBullet 或 Gazebo 中搭建人形机器人仿真环境，测试 RRT 算法。
- 尝试用 PPO 训练一个简单的路径规划策略。
- 调试：
- 可视化路径（使用 Matplotlib 或 RViz），检查是否合理。
- 记录规划时间，优化算法效率。
- 进阶：
- 学习 RRT*（RRT 的优化版），提升路径平滑性。
- 结合强化学习和传统算法（如 A* 提供初始路径，RL 优化动态调整）。
五、扩展资源
- 书籍：
- 《Probabilistic Robotics》 by Sebastian Thrun et al.（第7章，路径规划）
- 《Planning Algorithms》 by Steven M. LaValle（免费在线版，http://planning.cs.uiuc.edu/）
- 数据集：
- MoveIt! Benchmarks：提供路径规划测试场景（MoveIt）。
- KITTI Dataset：包含真实环境数据，可用于测试（KITTI）。
- 开源项目：
- Python Robotics：包含多种路径规划算法实现。
- ROS Navigation：提供 A*、Dijkstra 等算法的 ROS 实现。