运动规划¶
运动规划是机器人学中的核心问题，涉及如何让机器人从一个位置安全地移动到另一个位置。本章将介绍运动规划的核心概念和算法。
学习目标¶
完成本章后，你将能够：
- 理解运动规划的基本概念
- 掌握路径规划算法
- 了解运动规划方法
- 应用运动规划解决实际问题
1. 运动规划基础¶
1.1 运动规划问题定义¶
运动规划问题：
- 配置空间（C-space）：机器人所有可能的位置和姿态
- 障碍物空间（C-obstacle）：与障碍物碰撞的配置
- 自由空间（C-free）：不与障碍物碰撞的配置
- 初始配置（q_init）：机器人起始位置
- 目标配置（q_goal）：机器人目标位置
目标：找到一条从 q_init 到 q_goal 的路径，完全在 C-free 中
数学公式¶
场景：在规划之前，我们需要数学上定义机器人所处的空间。
配置空间 \(\mathcal{C}\) 的形式化定义为：
- 配置空间：\(\mathcal{C} = \{ \mathbf{q} \mid \mathbf{q} \in \mathbb{R}^n, \mathbf{q}_{min} \leq \mathbf{q} \leq \mathbf{q}_{max} \}\)
- 障碍物空间：\(\mathcal{C}_{obs} = \{ \mathbf{q} \mid \text{robot}(\mathbf{q}) \cap \text{obstacle} \neq \emptyset \}\)
- 自由空间：\(\mathcal{C}_{free} = \mathcal{C} \setminus \mathcal{C}_{obs}\)
- 距离度量：\(d(\mathbf{q}_1, \mathbf{q}_2) = \|\mathbf{q}_1 - \mathbf{q}_2\|_2 = \sqrt{\sum_{i=1}^{n}(q_{1,i} - q_{2,i})^2}\)
代码实现¶
import numpy as np
class ConfigurationSpace:
"""配置空间"""
def __init__(self, dimension, bounds):
"""
初始化配置空间
参数：
dimension: 维度
bounds: 边界 [(min1, max1), (min2, max2), ...]
"""
self.dimension = dimension
self.bounds = bounds
self.obstacles = []
def add_obstacle(self, obstacle):
"""添加障碍物"""
self.obstacles.append(obstacle)
def is_collision_free(self, config):
"""检查配置是否无碰撞"""
for obstacle in self.obstacles:
if obstacle.contains(config):
return False
return True
def sample_random(self):
"""随机采样配置"""
config = []
for low, high in self.bounds:
config.append(np.random.uniform(low, high))
return tuple(config)
def distance(self, config1, config2):
"""计算两个配置之间的距离"""
return np.sqrt(sum((a - b) ** 2 for a, b in zip(config1, config2)))
class Obstacle:
"""障碍物"""
def __init__(self, center, size):
"""
初始化障碍物
参数：
center: 中心点
size: 大小
"""
self.center = center
self.size = size
def contains(self, config):
"""检查配置是否在障碍物内"""
for i, (c, s) in enumerate(zip(self.center, self.size)):
if abs(config[i] - c) > s / 2:
return False
return True
# 示例：2D 配置空间
cspace = ConfigurationSpace(2, [(0, 10), (0, 10)])
# 添加障碍物
cspace.add_obstacle(Obstacle((3, 3), (2, 2)))
cspace.add_obstacle(Obstacle((7, 7), (2, 2)))
# 测试
config1 = (1, 1)
config2 = (3, 3)
print(f"配置 {config1} 无碰撞: {cspace.is_collision_free(config1)}")
print(f"配置 {config2} 无碰撞: {cspace.is_collision_free(config2)}")
1.2 路径表示¶
数学公式¶
场景：路径由通过线段连接的航点组成。
路径表示为航点序列：
-
路径长度：\(L = \sum_{i=1}^{n-1} \|\mathbf{w}_{i+1} - \mathbf{w}_i\|_2\)
-
线性插值：\(\mathbf{p}(t) = (1-t)\mathbf{a} + t\mathbf{b}, \quad t \in [0, 1]\)
代码实现¶
class Path:
"""路径"""
def __init__(self, waypoints):
"""
初始化路径
参数：
waypoints: 路径点列表
"""
self.waypoints = waypoints
@property
def length(self):
"""路径长度"""
total_length = 0
for i in range(len(self.waypoints) - 1):
total_length += np.sqrt(sum((a - b) ** 2
for a, b in zip(self.waypoints[i],
self.waypoints[i + 1])))
return total_length
def interpolate(self, num_points):
"""插值路径"""
if len(self.waypoints) < 2:
return self.waypoints
interpolated = []
for i in range(len(self.waypoints) - 1):
start = np.array(self.waypoints[i])
end = np.array(self.waypoints[i + 1])
for t in np.linspace(0, 1, num_points // (len(self.waypoints) - 1)):
point = start + t * (end - start)
interpolated.append(tuple(point))
return interpolated
def is_valid(self, cspace):
"""检查路径是否有效"""
for waypoint in self.waypoints:
if not cspace.is_collision_free(waypoint):
return False
return True
# 示例
path = Path([(0, 0), (1, 1), (2, 2), (3, 3)])
print(f"路径长度: {path.length:.2f}")
print(f"路径有效: {path.is_valid(cspace)}")
2. 路径规划算法¶
2.1 基于采样的规划¶
随机路径图（PRM）¶
数学公式¶
场景：如何在连续空间中找路径？采样并连接。
PRM（概率路线图法）构建配置空间的图表示：
- 采样：从 \(\mathcal{C}_{free}\) 中生成 \(N\) 个随机配置
- 图构建：\(\mathcal{G} = (\mathcal{V}, \mathcal{E})\)，其中：
- \(\mathcal{V}\) = 采样配置
- \(\mathcal{E} = \{ (v_i, v_j) \mid d(v_i, v_j) < r \text{ 且路径无碰撞} \}\)
- 查询阶段：将起点/终点连接到最近节点，使用 BFS 或 Dijkstra 找最短路径
代码分解：
- build_roadmap()：采样配置 + 连接有碰撞自由路径的附近节点
- find_path()：将查询点添加到图 + 使用 BFS 搜索
代码实现¶
class PRM:
"""概率路线图（PRM）"""
def __init__(self, cspace, num_samples=100, connection_radius=2.0):
"""
初始化 PRM
参数：
cspace: 配置空间
num_samples: 采样数量
connection_radius: 连接半径
"""
self.cspace = cspace
self.num_samples = num_samples
self.connection_radius = connection_radius
self.nodes = []
self.edges = {}
def build_roadmap(self):
"""构建路线图"""
# 采样节点
for _ in range(self.num_samples):
config = self.cspace.sample_random()
if self.cspace.is_collision_free(config):
self.nodes.append(config)
# 连接节点
for i, node1 in enumerate(self.nodes):
self.edges[i] = []
for j, node2 in enumerate(self.nodes):
if i != j:
dist = self.cspace.distance(node1, node2)
if dist <= self.connection_radius:
# 检查路径是否无碰撞
if self._is_path_collision_free(node1, node2):
self.edges[i].append(j)
def _is_path_collision_free(self, config1, config2):
"""检查路径是否无碰撞"""
# 简化：只检查中间点
num_checks = 10
for t in np.linspace(0, 1, num_checks):
config = tuple(
a + t * (b - a)
for a, b in zip(config1, config2)
)
if not self.cspace.is_collision_free(config):
return False
return True
def find_path(self, start, goal):
"""查找路径"""
# 将起点和终点添加到路线图
start_idx = len(self.nodes)
self.nodes.append(start)
self.edges[start_idx] = []
goal_idx = len(self.nodes)
self.nodes.append(goal)
self.edges[goal_idx] = []
# 连接起点和终点
for i, node in enumerate(self.nodes):
if i != start_idx and i != goal_idx:
if self.cspace.distance(start, node) <= self.connection_radius:
if self._is_path_collision_free(start, node):
self.edges[start_idx].append(i)
self.edges[i].append(start_idx)
if self.cspace.distance(goal, node) <= self.connection_radius:
if self._is_path_collision_free(goal, node):
self.edges[goal_idx].append(i)
self.edges[i].append(goal_idx)
# 使用 BFS 查找路径
return self._bfs(start_idx, goal_idx)
def _bfs(self, start_idx, goal_idx):
"""广度优先搜索"""
from collections import deque
queue = deque([start_idx])
visited = {start_idx}
parent = {start_idx: None}
while queue:
current = queue.popleft()
if current == goal_idx:
# 重建路径
path = []
while current is not None:
path.append(self.nodes[current])
current = parent[current]
return Path(path[::-1])
for neighbor in self.edges[current]:
if neighbor not in visited:
visited.add(neighbor)
parent[neighbor] = current
queue.append(neighbor)
return None # 无路径
# 示例
prm = PRM(cspace, num_samples=100, connection_radius=3.0)
prm.build_roadmap()
start = (0, 0)
goal = (9, 9)
path = prm.find_path(start, goal)
if path:
print(f"找到路径，长度: {path.length:.2f}")
print(f"路径点: {path.waypoints}")
else:
print("未找到路径")
快速探索随机树（RRT）¶
数学公式¶
场景：不构建完整图，而是逐步向未探索区域生长树。
RRT（快速探索随机树）通过构建树来探索配置空间：
- 最近邻：\(q_{nearest} = \arg\min_{q \in \mathcal{V}} \|q - q_{rand}\|_2\)
- 导向：\(q_{new} = q_{nearest} + \frac{\delta}{\|q_{rand} - q_{nearest}\|}_2 \cdot (q_{rand} - q_{nearest})\)
- 目标偏置：\(P(q_{rand} = q_{goal}) = \varepsilon\)（通常 \(\varepsilon = 0.1\)）
- 扩展条件：\(q_{new} \in \mathcal{C}_{free} \land q_{new} \in \text{bounds}\)
代码分解：
- _find_nearest()：找到离随机样本最近的树节点
- _extend()：沿样本方向创建新节点
- _build_path()：从目标到起点追踪父指针
代码实现¶
class RRT:
"""快速探索随机树（RRT）"""
def __init__(self, cspace, step_size=0.5, max_iterations=1000):
"""
初始化 RRT
参数：
cspace: 配置空间
step_size: 步长
max_iterations: 最大迭代次数
"""
self.cspace = cspace
self.step_size = step_size
self.max_iterations = max_iterations
self.nodes = []
self.parent = {}
def plan(self, start, goal):
"""
规划路径
参数：
start: 起始配置
goal: 目标配置
返回：
path: 路径（如果找到）
"""
# 初始化
self.nodes = [start]
self.parent = {start: None}
for iteration in range(self.max_iterations):
# 随机采样
if np.random.random() < 0.1: # 10% 概率采样目标
random_config = goal
else:
random_config = self.cspace.sample_random()
# 找到最近的节点
nearest_node = self._find_nearest(random_config)
# 扩展
new_node = self._extend(nearest_node, random_config)
if new_node:
self.nodes.append(new_node)
self.parent[new_node] = nearest_node
# 检查是否到达目标
if self.cspace.distance(new_node, goal) < self.step_size:
# 构建路径
path = self._build_path(new_node, goal)
return path
return None # 未找到路径
def _find_nearest(self, config):
"""找到最近的节点"""
min_dist = float('inf')
nearest = None
for node in self.nodes:
dist = self.cspace.distance(node, config)
if dist < min_dist:
min_dist = dist
nearest = node
return nearest
def _extend(self, from_config, to_config):
"""扩展节点"""
# 计算方向
direction = np.array(to_config) - np.array(from_config)
length = np.linalg.norm(direction)
if length < 1e-6:
return None
# 归一化方向
direction = direction / length
# 计算新配置
new_config = tuple(
np.array(from_config) + self.step_size * direction
)
# 检查边界
for i, (low, high) in enumerate(self.cspace.bounds):
if new_config[i] < low or new_config[i] > high:
return None
# 检查碰撞
if not self.cspace.is_collision_free(new_config):
return None
return new_config
def _build_path(self, last_node, goal):
"""构建路径"""
path = [goal]
current = last_node
while current is not None:
path.append(current)
current = self.parent[current]
return Path(path[::-1])
# 示例
rrt = RRT(cspace, step_size=0.5, max_iterations=1000)
start = (0, 0)
goal = (9, 9)
path = rrt.plan(start, goal)
if path:
print(f"找到路径，长度: {path.length:.2f}")
print(f"路径点数: {len(path.waypoints)}")
else:
print("未找到路径")
2.2 基于搜索的规划¶
A* 搜索¶
数学公式¶
场景：如果我们大致知道目标在哪里，利用这个知识引导搜索。
A* 结合实际代价和启发式估计来找到最优路径：
- 代价函数：\(f(n) = g(n) + h(n)\)，其中：
- \(g(n)\) = 从起点到 \(n\) 的实际代价（累积距离）
- \(h(n)\) = 从 \(n\) 到目标的启发式估计（如欧几里得距离）
- \(f(n)\) = 通过 \(n\) 的估计总代价
- 最优性条件：如果 \(h(n) \leq h^*(n)\) 对所有 \(n\) 成立（可采纳），则 A* 保证最优
- 优先队列：按 \(f\) 值排序节点（最低 \(f\) = 最高优先级）
代码分解：
- g_score 更新：跟踪从起点到每个节点的实际代价
- heuristic 计算：估计到目标的剩余代价
- 路径重建：从目标向后追踪父指针
代码实现¶
def a_star_path_planning(cspace, start, goal, heuristic=None):
"""
A* 路径规划
参数：
cspace: 配置空间
start: 起始配置
goal: 目标配置
heuristic: 启发函数
返回：
path: 路径（如果找到）
"""
import heapq
# 初始化
if heuristic is None:
heuristic = lambda config: cspace.distance(config, goal)
# 优先队列：(f值, 配置)
frontier = [(heuristic(start), start)]
explored = set()
g_score = {start: 0}
parent = {start: None}
while frontier:
f, current = heapq.heappop(frontier)
# 检查是否到达目标
if cspace.distance(current, goal) < 0.5:
# 构建路径
path = [goal]
while current is not None:
path.append(current)
current = parent[current]
return Path(path[::-1])
if current in explored:
continue
explored.add(current)
# 扩展邻居
for neighbor in _get_neighbors(cspace, current):
if neighbor in explored:
continue
new_g = g_score[current] + cspace.distance(current, neighbor)
if neighbor not in g_score or new_g < g_score[neighbor]:
g_score[neighbor] = new_g
f = new_g + heuristic(neighbor)
heapq.heappush(frontier, (f, neighbor))
parent[neighbor] = current
return None # 无路径
def _get_neighbors(cspace, config, step_size=0.5):
"""获取邻居配置"""
neighbors = []
# 生成邻居（简化：只考虑基本方向）
for delta in [(step_size, 0), (-step_size, 0), (0, step_size), (0, -step_size)]:
neighbor = tuple(c + d for c, d in zip(config, delta))
# 检查边界
valid = True
for i, (low, high) in enumerate(cspace.bounds):
if neighbor[i] < low or neighbor[i] > high:
valid = False
break
if valid and cspace.is_collision_free(neighbor):
neighbors.append(neighbor)
return neighbors
3. 运动规划¶
3.1 轨迹规划¶
数学公式¶
场景：规划路径只是几何形状。现在需要添加时间——以最大速度移动且不超过加速度限制。
梯形速度曲线将运动分为三个阶段：
- 加速阶段 (\(0 \leq t \leq t_a\))：
- \(v(t) = a \cdot t\)
-
\(d(t) = \frac{1}{2}a \cdot t^2\)
-
匀速阶段 (\(t_a \leq t \leq t_a + t_c\))：
- \(v(t) = V_{max}\)
-
\(d(t) = d_{accel} + V_{max} \cdot (t - t_a)\)
-
减速阶段 (\(t_a + t_c \leq t \leq T\))：
- \(v(t) = a \cdot (T - t)\)
-
\(d(t) = D - \frac{1}{2}a \cdot (T - t)^2\)
-
时间计算：
- 梯形当 \(D \geq 2D_{accel}\)：\(t_a = \frac{V_{max}}{a}\), \(t_c = \frac{D - 2D_{accel}}{V_{max}}\)
- 三角形当 \(D < 2D_{accel}\)：\(t_a = t_d = \sqrt{\frac{D}{a}}\)
代码分解： - 加速阶段：\(d = 0.5 \cdot a \cdot t^2\), \(v = a \cdot t\) - 匀速阶段：\(d = d_{accel} + V_{max} \cdot (t - t_a)\), \(v = V_{max}\) - 减速阶段：\(d = D - 0.5 \cdot a \cdot t_{decel}^2\), \(v = a \cdot t_{decel}\)
代码实现¶
class TrajectoryPlanner:
"""轨迹规划器"""
def __init__(self, path, max_velocity=1.0, max_acceleration=0.5):
"""
初始化轨迹规划器
参数：
path: 路径
max_velocity: 最大速度
max_acceleration: 最大加速度
"""
self.path = path
self.max_velocity = max_velocity
self.max_acceleration = max_acceleration
def plan_trajectory(self):
"""规划轨迹"""
# 计算路径长度
path_length = self.path.length
# 计算时间
# 使用梯形速度曲线
t_accel = self.max_velocity / self.max_acceleration
d_accel = 0.5 * self.max_acceleration * t_accel ** 2
if 2 * d_accel > path_length:
# 三角形速度曲线
t_accel = np.sqrt(path_length / self.max_acceleration)
t_total = 2 * t_accel
else:
# 梯形速度曲线
d_constant = path_length - 2 * d_accel
t_constant = d_constant / self.max_velocity
t_total = 2 * t_accel + t_constant
# 生成轨迹点
trajectory = []
num_points = 100
for t in np.linspace(0, t_total, num_points):
if t < t_accel:
# 加速阶段
d = 0.5 * self.max_acceleration * t ** 2
v = self.max_acceleration * t
elif t < t_total - t_accel:
# 匀速阶段
d = d_accel + self.max_velocity * (t - t_accel)
v = self.max_velocity
else:
# 减速阶段
t_decel = t_total - t
d = path_length - 0.5 * self.max_acceleration * t_decel ** 2
v = self.max_acceleration * t_decel
# 在路径上插值
config = self._interpolate_path(d / path_length)
trajectory.append({
'time': t,
'position': config,
'velocity': v,
'distance': d
})
return trajectory
def _interpolate_path(self, t):
"""在路径上插值"""
waypoints = self.path.waypoints
if t <= 0:
return waypoints[0]
if t >= 1:
return waypoints[-1]
# 找到对应的路径段
segment_length = 1.0 / (len(waypoints) - 1)
segment_index = int(t / segment_length)
segment_t = (t - segment_index * segment_length) / segment_length
if segment_index >= len(waypoints) - 1:
return waypoints[-1]
start = np.array(waypoints[segment_index])
end = np.array(waypoints[segment_index + 1])
return tuple(start + segment_t * (end - start))
# 示例
if path:
planner = TrajectoryPlanner(path, max_velocity=1.0, max_acceleration=0.5)
trajectory = planner.plan_trajectory()
print(f"轨迹点数: {len(trajectory)}")
print(f"总时间: {trajectory[-1]['time']:.2f}")
3.2 运动学约束¶
数学公式¶
场景：机器人路径必须满足物理约束——非完整系统不能侧向移动。
非完整约束：像汽车或差分驱动机器人这样的系统，其自由度受到约束： - 配置：\(\mathbf{q} = [x, y, \theta]^T\) - 约束：\(\dot{x}\sin\theta - \dot{y}\cos\theta = 0\)（禁止侧滑）
路径捷径平滑：通过移除不必要的航点来减少路径长度： - 对于航点 \(\mathbf{w}_i, \mathbf{w}_j\)（\(i < j\)），如果直线无碰撞，用直接连接替换 \(\mathbf{w}_{i+1}, ..., \mathbf{w}_{j-1}\) - 有效捷径：\(\forall t \in [0,1]: (1-t)\mathbf{w}_i + t\mathbf{w}_j \in \mathcal{C}_{free}\)
代码分解：
- _smooth_path()：随机捷径迭代——尝试移除中间航点
- _is_path_valid()：通过采样检查捷径路径是否无碰撞
代码实现¶
class KinematicPlanner:
"""运动学规划器"""
def __init__(self, robot_model, cspace):
"""
初始化运动学规划器
参数：
robot_model: 机器人模型
cspace: 配置空间
"""
self.robot_model = robot_model
self.cspace = cspace
def plan(self, start_config, goal_config):
"""规划运动"""
# 使用 RRT 规划
rrt = RRT(self.cspace, step_size=0.1, max_iterations=5000)
path = rrt.plan(start_config, goal_config)
if path:
# 应用运动学约束
constrained_path = self._apply_kinematic_constraints(path)
return constrained_path
return None
def _apply_kinematic_constraints(self, path):
"""应用运动学约束"""
# 简化：只进行路径平滑
smoothed_path = self._smooth_path(path)
return smoothed_path
def _smooth_path(self, path, iterations=100):
"""平滑路径"""
waypoints = list(path.waypoints)
for _ in range(iterations):
# 随机选择两个点
i = np.random.randint(1, len(waypoints) - 1)
j = np.random.randint(1, len(waypoints) - 1)
if i > j:
i, j = j, i
# 检查直接连接是否无碰撞
if self._is_path_valid(waypoints[i], waypoints[j]):
# 删除中间点
waypoints = waypoints[:i + 1] + waypoints[j:]
return Path(waypoints)
def _is_path_valid(self, config1, config2, num_checks=10):
"""检查路径是否有效"""
for t in np.linspace(0, 1, num_checks):
config = tuple(
a + t * (b - a)
for a, b in zip(config1, config2)
)
if not self.cspace.is_collision_free(config):
return False
return True
4. 实践练习¶
练习 1：实现 PRM¶
def prm_exercise():
# 创建配置空间
cspace = ConfigurationSpace(2, [(0, 10), (0, 10)])
cspace.add_obstacle(Obstacle((3, 3), (2, 2)))
cspace.add_obstacle(Obstacle((7, 7), (2, 2)))
# 创建 PRM
prm = PRM(cspace, num_samples=200, connection_radius=3.0)
prm.build_roadmap()
# 查找路径
start = (0, 0)
goal = (9, 9)
path = prm.find_path(start, goal)
if path:
print(f"PRM 找到路径，长度: {path.length:.2f}")
else:
print("PRM 未找到路径")
练习 2：实现 RRT¶
def rrt_exercise():
# 创建配置空间
cspace = ConfigurationSpace(2, [(0, 10), (0, 10)])
cspace.add_obstacle(Obstacle((3, 3), (2, 2)))
cspace.add_obstacle(Obstacle((7, 7), (2, 2)))
# 创建 RRT
rrt = RRT(cspace, step_size=0.5, max_iterations=1000)
# 规划路径
start = (0, 0)
goal = (9, 9)
path = rrt.plan(start, goal)
if path:
print(f"RRT 找到路径，长度: {path.length:.2f}")
print(f"路径点数: {len(path.waypoints)}")
else:
print("RRT 未找到路径")
练习 3：实现轨迹规划¶
def trajectory_exercise():
# 创建路径
path = Path([(0, 0), (2, 2), (5, 5), (8, 8), (10, 10)])
# 创建轨迹规划器
planner = TrajectoryPlanner(path, max_velocity=1.0, max_acceleration=0.5)
# 规划轨迹
trajectory = planner.plan_trajectory()
print(f"轨迹点数: {len(trajectory)}")
print(f"总时间: {trajectory[-1]['time']:.2f}")
# 显示轨迹
for i in range(0, len(trajectory), 10):
point = trajectory[i]
print(f"t={point['time']:.2f}, pos={point['position']}, v={point['velocity']:.2f}")
5. 常见问题¶
1. 规划失败¶
问题：找不到路径
解决方案： - 增加采样数量 - 增加连接半径 - 检查起始和目标配置是否有效
2. 路径质量差¶
问题：路径不平滑或太长
解决方案： - 使用路径平滑 - 增加采样密度 - 使用 RRT* 算法
3. 计算时间长¶
问题：规划时间太长
解决方案： - 减少采样数量 - 使用启发式搜索 - 使用并行计算
