本教程解决一个核心问题：给定一组路径点（Waypoints），如何生成一条光滑、安全且可执行的飞行轨迹？ 各章节层层递进：
| 章节 | 解决的核心问题 | 与其他章节的关系 |
|---|---|---|
| 一、光滑轨迹生成基础 | 什么是"光滑"？用什么数学工具描述轨迹？ | 定义问题框架，引入多项式轨迹和约束体系 |
| 二、Minimum Snap 轨迹优化 | 如何将"最光滑轨迹"转化为可求解的数学优化问题？ | 依赖一（约束体系），构建 QP 问题 |
| 三、Corridor 约束与时间分配 | 如何保证轨迹不撞障碍物？各段飞多久？ | 依赖二（QP 框架），加入不等式约束和工程细节 |
| 四、Minimum Snap 闭式求解 | 不用迭代求解器，能否直接算出最优解？ | 依赖二（QP 问题），提供高效的解析解方法 |
与控制教程的关系：
- 本教程的前置知识是控制教程中的"三、微分平坦性"——它解释了为什么最小化 snap（四阶导数）是有物理意义的（snap 对应控制力矩）
- 本教程的输出（位置/速度/加速度参考轨迹）是控制教程中各控制器（PID、SE(3)、MPC）的输入
阅读建议：按顺序 一 → 四 即可。如果只关心原理不关心实际的工程实现细节，可以跳过三中的时间分配和归一化部分。
规划教程（4 个代码块）：
一、光滑轨迹基础 — 5 次多项式插值，P/V/A 曲线
二、Minimum Snap — 多 waypoint 最小 snap 轨迹 + KKT 求解
三、Corridor 约束 — 安全通道 + 障碍物 + 时间分配策略可视化
四、闭式求解 — 闭式解 vs QP 结果对比验证
轨迹生成需要满足以下条件：
边界条件：
- 起始位置（确定）
- 终点位置（确定）
中间条件：
- 精确位置（确定，即 Waypoints）
个人理解：Waypoints 由前端路径规划算法提供（如
$A^* $ 、$RRT$ 、$RRT^*$ 等），轨迹规划是在这些离散路径点基础上生成光滑的时间参数化曲线。
个人理解：轨迹规划是在路径规划（
$A^*$ 、$RRT$ 等给出离散 waypoints）之后的步骤。路径规划解决"去哪"的问题，轨迹规划解决"怎么去"的问题——将离散路径点连成一条满足动力学约束的光滑时间参数化曲线。
个人理解：光滑性的度量标准通常是最小化某阶导数（即"输入"的变化率）的平方积分。阶数越高，轨迹越光滑，但多项式次数也越高。
对于四旋翼无人机，关键观察是：
- 位置的 二阶导数（加速度）对应推力方向
- 位置的 三阶导数（jerk）对应角速度
- 位置的 四阶导数（snap）对应控制力矩
因此最小化 snap 等价于最小化控制力矩的变化，保证执行器输出平滑。
个人理解：最优化轨迹的核心思想——优化的是轨迹高阶导数的平方积分（变化率越小越光滑）。关键关系：
- 若最小化
$K_r$ 阶导数，则每段多项式至少$2K_r - 1$ 阶（$2K_r$ 个系数）- 相邻段需保证
$K_r - 1$ 阶导数连续- 例如 minimum snap（
$K_r = 4$ ）：每段至少 7 阶多项式，保证 0~3 阶导数连续
个人理解：为什么是
$2K_r$ 次多项式？因为最小化$K_r$ 阶导数的平方积分，被优化的变量是多项式系数，每段需要$2K_r$ 个约束（两端各$K_r$ 个），所以多项式至少需要$2K_r$ 个系数，即$2K_r - 1$ 阶。对于 minimum snap（$K_r = 4$ ），每段至少 7 阶多项式（8个系数）；实际中通常取 5 阶或 7 阶。
- 轨迹的约束（高阶约束）：状态约束
- 轨迹在交叉处的连续性：相邻段端点处各阶导数必须相等
- 轨迹的 4 个导数：系数矩阵和约束
- 最优化指标：轨迹 4 阶导数平方的积分
光滑直线段处的特殊处理：
个人理解：在直线段处，速度方向不变且加速度为零。但光滑轨迹在直线段两端（转弯处）的速度方向需要平滑过渡，不能出现速度/加速度突变，因此直线段与曲线段的衔接需要特殊处理。
各段端点处的
相邻段在交界处的各阶导数必须相等：
个人理解：以 minimum snap 为例，需要保证 0~3 阶导数连续（位置、速度、加速度、jerk），共 4 个连续性约束。这些约束确保飞行器经过路径点时不会出现突变的力或力矩。
对于 minimum jerk（$K_r = 3$），每段使用 5 次多项式（6 个参数）：
边界条件（6个方程解6个参数）：
-
$t = 0$ ：位置$a$ 、速度$\dot{a}$ 、加速度$\ddot{a}$ -
$t = T$ ：位置$b$ 、速度$\dot{b}$ 、加速度$\ddot{b}$
矩阵求解形式：
方案1：相对时间（每段从
- 优势：数值稳定性比较高
方案2：绝对时间（使用全局时间
- 优势：数学表达清楚
个人理解：方案 2 的致命缺点——如果使用绝对时间，例如
$t = 1000$ 秒，那么$t^7 = 10^{21}$ ，远超浮点数精度，数值会爆炸。方案 1 每段从$t = 0$ 开始，$t$ 的范围始终在段持续时间内，数值可控。
个人理解：实际工程中通常采用方案1（相对时间），因为数值稳定性更重要。绝对时间在
$t$ 较大时会导致$t^N$ 数值爆炸。例如$t = 100$ 秒时$t^7 = 10^{14}$ ，远超 float 精度。
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei'] # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False # 解决负号显示为方框的问题
# ============================================================
# 五次多项式两点插值 —— 无人机从静止起飞到目标点
# 给定起点和终点的位置、速度、加速度，求解6x6矩阵方程得到多项式系数
# ============================================================
# --- 边界条件设置（模拟无人机垂直起飞） ---
T = 3.0 # 飞行时间 3 秒
# t=0 时刻：地面静止
pos0, vel0, acc0 = 0.0, 0.0, 0.0
# t=T 时刻：到达 10m 高度，悬停（速度和加速度均为零）
posT, velT, accT = 10.0, 0.0, 0.0
# --- 构造 6x6 约束矩阵 ---
# X(t) = p0 + p1*t + p2*t^2 + p3*t^3 + p4*t^4 + p5*t^5
A = np.array([
[1, 0, 0, 0, 0, 0 ], # X(0) = pos0
[0, 1, 0, 0, 0, 0 ], # X'(0) = vel0
[0, 0, 2, 0, 0, 0 ], # X''(0) = acc0
[1, T, T**2, T**3, T**4, T**5 ], # X(T) = posT
[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4 ], # X'(T) = velT
[0, 0, 2, 6*T, 12*T**2, 20*T**3 ], # X''(T) = accT
])
b = np.array([pos0, vel0, acc0, posT, velT, accT])
# --- 求解多项式系数 ---
coeffs = np.linalg.solve(A, b)
print("多项式系数 [p0, p1, p2, p3, p4, p5]:", np.round(coeffs, 4))
# --- 在时间轴上采样并计算 位置/速度/加速度 ---
t = np.linspace(0, T, 200)
pos = sum(coeffs[i] * t**i for i in range(6))
vel = sum(i * coeffs[i] * t**(i-1) for i in range(1, 6))
acc = sum(i * (i-1) * coeffs[i] * t**(i-2) for i in range(2, 6))
# --- 绘图 ---
fig, axes = plt.subplots(3, 1, figsize=(8, 8), sharex=True)
titles = ["位置 (m)", "速度 (m/s)", "加速度 (m/s²)"]
data = [pos, vel, acc]
colors = ["#1f77b4", "#ff7f0e", "#2ca02c"]
for ax, title, y, c in zip(axes, titles, data, colors):
ax.plot(t, y, color=c, linewidth=2)
ax.set_ylabel(title, fontsize=12)
ax.grid(True, alpha=0.3)
ax.axhline(0, color='gray', linewidth=0.5)
# 标注起点和终点
axes[0].plot(0, pos0, 'ko', markersize=8, label='起飞点')
axes[0].plot(T, posT, 'r*', markersize=12, label='目标悬停点')
axes[0].legend(fontsize=10)
axes[-1].set_xlabel("时间 (s)", fontsize=12)
fig.suptitle("五次多项式轨迹 —— 无人机从地面起飞到 10m 悬停", fontsize=13)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/planning1.png', dpi=150, bbox_inches='tight')
plt.show()运行结果：
一段
共
位置（0阶）：
速度（1阶）：
加速度（2阶）：
Jerk（3阶，加加速度）：
Snap（4阶）：
个人理解：$k$ 阶导数可以统一写为
$p^{(k)}(t) = \mathbf{c}_k(t)^T \mathbf{p}$ ，其中$\mathbf{c}_k(t)$ 的第$i$ 个元素为$\frac{i!}{(i-k)!} t^{i-k}$ （当$i \geq k$ 时），否则为 0。
对于
每段轨迹的参数 $\mathbf{p}i = [p{i0}, p_{i1}, \ldots, p_{iN}]^T$ 为第
将所有段参数堆叠为一个大向量：
不同的优化目标对应不同的光滑性度量：
| 优化目标 | 表达式 | 含义 | 最低多项式阶数 |
|---|---|---|---|
| Minimum Snap | 最小化力矩变化 | 7 阶 | |
| Minimum Jerk | 最小化角速度变化 | 5 阶 | |
| Minimum Acce | 最小化加速度 | 3 阶 |
个人理解：一般不选择最小化速度或位置，因为最小化低阶导数会让轨迹过于"懒惰"（趋向于不动），失去实际意义。最小化加速度以上的导数才有物理意义（能量、力、力矩）。
个人理解：对于四旋翼，最常用的是 minimum snap，因为 snap 直接关联到控制力矩输入，最小化 snap 等价于最小化能量消耗和执行器磨损。Minimum jerk 用于对角速度敏感的场景（如搭载相机的航拍无人机）。
目标函数
其平方为：
对第
写成二次型：
其中
个人理解：约束的层级——首先保证 0 阶导数（位置）在交界处连续（即轨迹经过 waypoint），然后逐级添加高阶导数连续约束（速度、加速度、jerk），保证相邻两段在交界处平滑过渡。
约束条件分为以下几类：
① 初始点约束（位置、速度、加速度等的等式约束）：
个人理解：如果使用相对时间（每段从
$t=0$ 开始），初始点约束在$t_0 = 0$ 处代入，多项式的低次项系数直接对应初始的位置、速度、加速度值，形式非常简洁。
② 中间第
③ 终端约束（位置、速度、加速度等式约束）
④ 连续性约束（相邻段在交界处各阶导数相等）
约束数量统计：
个人理解：约束数量统计（以 minimum snap，
$M$ 段为例）：起始 PVA = 3 个，终端 PVA = 3 个，中间 waypoint 位置 =$(M-1)$ 个，中间连续性（P, V, A 各 1 个$\times (M-1)$ 个交界）=$3(M-1)$ 个。总计$= 6 + 4(M-1)$ 。当约束数$<$ 未知参数数时，剩余自由度用于最小化目标函数。
| 约束类型 | 数量 |
|---|---|
| 起始点 PVA | 3 |
| 终端 PVA | 3 |
| 中间点位置（$M-1$ 个点） | |
| 中间点连续性（$K_r$ 阶 |
|
| 总计 |
最终问题转化为线性等式约束的二次规划（QP）：
个人理解：这是一个凸二次规划问题（
$\mathbf{Q}$ 半正定），有全局最优解。可以使用标准 QP 求解器（如 OSQP、qpOASES）高效求解。
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei'] # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False # 解决负号显示为方框的问题
# ============================================================
# Minimum Snap 轨迹生成 —— 无人机依次飞过多个航点
# 使用 KKT 系统（拉格朗日乘子法）求解等式约束 QP 的闭式解
# ============================================================
# --- 航点定义（二维：x, y） ---
waypoints = np.array([
[0.0, 0.0], # 起点
[2.0, 2.5], # 中间点1
[5.0, 4.0], # 中间点2
[8.0, 3.0], # 中间点3
[10.0, 5.0], # 终点
])
M = len(waypoints) - 1 # 4 段
N = 7 # 7阶多项式（8个系数，minimum snap）
# --- 时间分配（按段长等比例分配，总时间8秒） ---
dists = np.linalg.norm(np.diff(waypoints, axis=0), axis=1)
T_total = 8.0
T_seg = T_total * dists / dists.sum() # 各段时间
def build_Q_seg(n, T, kr=4):
"""构造单段 snap 代价矩阵 Q（相对时间，t in [0, T]）"""
Q = np.zeros((n+1, n+1))
for i in range(kr, n+1):
for j in range(kr, n+1):
ci = np.prod(range(i, i-kr, -1)) # i!/(i-kr)!
cj = np.prod(range(j, j-kr, -1))
Q[i, j] = ci * cj * T**(i+j-2*kr+1) / (i+j-2*kr+1)
return Q
def poly_coeff_vec(n, t, deriv=0):
"""返回 n 阶多项式在 t 处 deriv 阶导数的系数行向量"""
c = np.zeros(n+1)
for i in range(deriv, n+1):
c[i] = np.prod(range(i, i-deriv, -1)) * t**(i-deriv)
return c
# --- 构造总 Q 矩阵（分块对角） ---
dim_total = M * (N+1)
Q_all = np.zeros((dim_total, dim_total))
for k in range(M):
idx = k * (N+1)
Q_all[idx:idx+N+1, idx:idx+N+1] = build_Q_seg(N, T_seg[k])
# --- 构造等式约束矩阵 Aeq @ P = beq ---
constraints_rows = []
constraints_rhs_x = []
constraints_rhs_y = []
def add_constraint(row_vec, rhs_x, rhs_y):
constraints_rows.append(row_vec)
constraints_rhs_x.append(rhs_x)
constraints_rhs_y.append(rhs_y)
# 起始点约束：位置、速度、加速度、jerk（段0，t=0）
for d in range(4):
row = np.zeros(dim_total)
row[0:N+1] = poly_coeff_vec(N, 0, d)
val = waypoints[0] if d == 0 else np.array([0.0, 0.0])
add_constraint(row, val[0], val[1])
# 终端约束：位置、速度、加速度、jerk（最后一段，t=T_M）
for d in range(4):
row = np.zeros(dim_total)
idx = (M-1)*(N+1)
row[idx:idx+N+1] = poly_coeff_vec(N, T_seg[-1], d)
val = waypoints[-1] if d == 0 else np.array([0.0, 0.0])
add_constraint(row, val[0], val[1])
# 中间航点位置约束 + 连续性约束
for k in range(M-1):
# 第 k 段终点位置 = waypoints[k+1]
row = np.zeros(dim_total)
idx = k * (N+1)
row[idx:idx+N+1] = poly_coeff_vec(N, T_seg[k], 0)
add_constraint(row, waypoints[k+1, 0], waypoints[k+1, 1])
# 连续性：第 k 段终点 = 第 k+1 段起点（0~3阶导数）
for d in range(4):
row = np.zeros(dim_total)
row[idx:idx+N+1] = poly_coeff_vec(N, T_seg[k], d)
idx2 = (k+1) * (N+1)
row[idx2:idx2+N+1] = -poly_coeff_vec(N, 0, d)
add_constraint(row, 0.0, 0.0)
Aeq = np.array(constraints_rows)
beq_x = np.array(constraints_rhs_x)
beq_y = np.array(constraints_rhs_y)
# --- KKT 系统求解：[2Q, A^T; A, 0] [P; lam] = [0; b] ---
n_eq = Aeq.shape[0]
KKT = np.zeros((dim_total + n_eq, dim_total + n_eq))
KKT[:dim_total, :dim_total] = 2 * Q_all
KKT[:dim_total, dim_total:] = Aeq.T
KKT[dim_total:, :dim_total] = Aeq
def solve_axis(beq):
rhs = np.zeros(dim_total + n_eq)
rhs[dim_total:] = beq
sol = np.linalg.solve(KKT, rhs)
return sol[:dim_total]
Px = solve_axis(beq_x)
Py = solve_axis(beq_y)
# --- 采样轨迹并计算各阶导数 ---
t_all, x_all, y_all = [], [], []
derivs = {d: ([], []) for d in range(5)} # 0~4阶
t_cum = 0.0
for k in range(M):
ts = np.linspace(0, T_seg[k], 100)
idx = k * (N+1)
px, py = Px[idx:idx+N+1], Py[idx:idx+N+1]
for d in range(5):
cx = np.array([poly_coeff_vec(N, t, d) @ px for t in ts])
cy = np.array([poly_coeff_vec(N, t, d) @ py for t in ts])
derivs[d][0].extend(cx)
derivs[d][1].extend(cy)
t_all.extend(ts + t_cum)
t_cum += T_seg[k]
# --- 绘图 ---
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 左图：二维轨迹
ax = axes[0]
ax.plot(derivs[0][0], derivs[0][1], 'b-', linewidth=2, label='Minimum Snap 轨迹')
ax.plot(waypoints[:, 0], waypoints[:, 1], 'ro-', markersize=8, linewidth=1, label='航点')
for i, wp in enumerate(waypoints):
ax.annotate(f'P{i}', wp, textcoords="offset points", xytext=(5, 8), fontsize=10)
ax.set_xlabel('X (m)'); ax.set_ylabel('Y (m)')
ax.set_title('无人机 Minimum Snap 二维轨迹'); ax.legend(); ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
# 右图：各阶导数随时间变化（仅 x 轴分量）
ax2 = axes[1]
labels = ['位置 (m)', '速度 (m/s)', '加速度 (m/s²)', 'Jerk', 'Snap']
for d in range(5):
ax2.plot(t_all, derivs[d][0], label=labels[d], alpha=0.85)
ax2.set_xlabel('时间 (s)'); ax2.set_ylabel('X 轴分量')
ax2.set_title('X 轴各阶导数曲线'); ax2.legend(fontsize=8); ax2.grid(True, alpha=0.3)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/planning2.png', dpi=150, bbox_inches='tight')
plt.show()运行结果：
其中
个人理解：$Q_k$ 矩阵中出现的
$(t_i - t_{i-1})^{r+c-7}$ 项只依赖于该段的时间长度$\Delta T_k = t_k - t_{k-1}$ ，这也是为什么用相对时间（每段从 0 开始）更方便——直接用$T_k$ 代入即可。
个人理解：实际求解时采用相对时间——每段轨迹的起始时间设为 0，只关心段内经过的时间
$\Delta T_k$ 。这样避免了绝对时间导致的数值问题。
个人理解：求解完成后，需要对整条轨迹进行 Feasibility Check（可行性检查）——逐段检查 P.V.A. 是否满足物理约束（最大速度、最大加速度、最大推力等），如果某段超限则需要调整该段的时间分配。
在基本的等式约束 QP 中，轨迹只需经过指定的 waypoints。但实际飞行中，轨迹在 waypoints 之间可能偏离过大，进入障碍物区域。Corridor 约束通过引入不等式约束来保证轨迹始终在安全通道内。
个人理解：之前的等式约束要求轨迹必须精确经过每个 waypoint。但使用 Corridor 后，中间 waypoint 的约束可以从等式松弛为不等式——轨迹不必精确经过该点，只需保持在 Corridor 范围内即可。这给优化器更多自由度，生成的轨迹更光滑。
对每个关键路径点
个人理解：Corridor 通常取矩形（轴对齐的方盒），每个 waypoint 周围构建一个半宽为
$r$ 的安全通道。轨迹只需留在 Corridor 内，不必精确穿过 waypoint。每个 Corridor 在每个轴方向上产生 2 个不等式约束（上界和下界）。
不等式约束形式：
个人理解：Corridor 的宽度
$2r$ 通常由路径规划阶段的障碍物间隙决定。$r$ 越大，轨迹越自由（更光滑），但安全裕度越小；$r$ 越小，轨迹被迫贴近 waypoints，可能出现较大的 snap。
| 类型 | 约束内容 | 数量 |
|---|---|---|
| 等式约束 | 起始 PVA | 3 |
| 等式约束 | 终止 PVA | 3 |
| 等式约束 | 中间点 PVA 连续 | |
| 不等式约束 | 中间点位置 Corridor |
个人理解：引入不等式约束后，问题从等式约束 QP 变为一般 QP，仍然是凸优化，但不再有闭式解，需要迭代求解器。
个人理解：为了确保轨迹不会在 waypoint 之间偏出 Corridor，需要在每段轨迹上增加离散采样点并检查约束。采样点越密，安全性越高，但约束数量也越多。轨迹段数 = waypoint 数 - 1。
个人理解：实用的迭代策略：① 先不加 Corridor 约束求解一次，得到初始轨迹；② 检查轨迹是否有超出 Corridor 的部分（波峰、波谷处最容易超出）；③ 在超出的位置添加 Corridor 不等式约束，重新求解；④ 重复直到所有位置都满足约束。
个人理解：实际做法是在每段轨迹上采样若干中间时刻点，检查这些点是否都在 Corridor 内。如果某些点超出了 Corridor，有两种处理策略：
- 增加约束点：在超出位置添加新的不等式约束，重新求解
- 增加 waypoint：在超出位置拆分段，添加新的路径点
个人理解：总飞行时间确定后（如根据总路程和期望平均速度估算），还需要将总时间合理分配到各段。如果各段时间分配不当（某段过长或过短），轨迹质量会大幅下降。
各段时间的分配标准：
方法一：匀速分配
假设每段内速度恒定，按段长度比例分配总时间：
方法二：梯形速度分配
假设每段执行梯形速度曲线（加速→匀速→减速），以
设段间时间分别为
个人理解：时间分配后如果某段的速度或加速度超出物理极限，说明该段时间分配不足。解决方法：增大该段的时间分配，重新求解轨迹，再次 check feasibility（可行性检验）。反复迭代直到所有段都满足动力学约束。也可以使用 time scaling（时间缩放）方法统一调整。
个人理解：时间分配是轨迹规划中非常关键的一步——时间分配不当会导致：
- 时间过短：加速度/jerk/snap 过大，超出执行器能力
- 时间过长：飞行效率低，总时间浪费
常用的迭代策略：先用匀速或梯形分配初始时间 → 求解轨迹 → 检查最大速度/加速度是否超限 → 对超限的段增加时间 → 重新求解，直到满足约束。
个人理解：当某段时间很短时，多项式系数
$p_i$ 可能非常大（因为要在短时间内完成位移），而$p_i \cdot T^N$ 可能溢出 float 精度。因此通常使用 double 精度浮点数，并且配合时间归一化来控制数值范围。
为避免数值问题，引入轨迹时间归一化。对每段定义归一化时间：
其中
原始多项式变为归一化多项式：
其中归一化系数
归一化后的 PVA 关系：
个人理解：工作流程：先将时间归一化到
$[0, 1]$ ，在归一化时间域下求解多项式系数$\hat{p}_i$ ，求解完成后再反归一化得到真实时间域下的系数$p_i = \hat{p}_i / T^i$ 。
个人理解：归一化将所有时间段映射到
$[0, 1]$ ，使得多项式系数的数值范围可控。求解完成后需要反归一化：$p_i = \hat{p}_i / T^i$ 。注意反归一化后的求导也要做对应的尺度变换。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
plt.rcParams['font.sans-serif'] = ['SimHei'] # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False # 解决负号显示为方框的问题
# ============================================================
# Corridor 约束可视化 + 时间分配对比
# 展示航点、安全走廊、障碍物以及两种时间分配策略的速度差异
# ============================================================
np.random.seed(42)
# --- 航点与障碍物定义 ---
waypoints = np.array([[0, 0], [3, 2], [6, 4], [9, 3], [12, 5]])
obstacles = [
{"center": [1.5, 3.8], "w": 1.2, "h": 1.0}, # 障碍物1：走廊上方
{"center": [4.5, 1.2], "w": 1.0, "h": 1.2}, # 障碍物2：走廊下方
{"center": [7.5, 5.8], "w": 1.4, "h": 0.8}, # 障碍物3：走廊上方
{"center": [10.5, 1.5], "w": 1.0, "h": 1.0}, # 障碍物4：走廊下方
]
corridor_half = 1.2 # 走廊半宽
# --- 生成一条穿过走廊的光滑曲线（用样条插值模拟） ---
from numpy.polynomial import polynomial as P_mod
t_wp = np.linspace(0, 1, len(waypoints))
t_dense = np.linspace(0, 1, 300)
# 简单参数化插值
degree = min(4, len(waypoints) - 1)
cx = np.polyfit(t_wp, waypoints[:, 0], degree)
cy = np.polyfit(t_wp, waypoints[:, 1], degree)
traj_x = np.polyval(cx, t_dense)
traj_y = np.polyval(cy, t_dense)
# ==================== 图1：Corridor 可视化 ====================
fig, axes = plt.subplots(1, 2, figsize=(15, 6))
ax1 = axes[0]
# 绘制走廊矩形
for wp in waypoints:
rect = Rectangle(
(wp[0] - corridor_half, wp[1] - corridor_half),
2 * corridor_half, 2 * corridor_half,
linewidth=1.5, edgecolor='#2ca02c', facecolor='#2ca02c',
alpha=0.15, linestyle='--', label='安全走廊' if wp is waypoints[0] else ''
)
ax1.add_patch(rect)
# 绘制障碍物
for i, obs in enumerate(obstacles):
rect = Rectangle(
(obs["center"][0] - obs["w"]/2, obs["center"][1] - obs["h"]/2),
obs["w"], obs["h"],
linewidth=1.5, edgecolor='red', facecolor='red', alpha=0.35,
label='障碍物' if i == 0 else ''
)
ax1.add_patch(rect)
# 绘制轨迹和航点
ax1.plot(traj_x, traj_y, 'b-', linewidth=2, label='规划轨迹')
ax1.plot(waypoints[:, 0], waypoints[:, 1], 'ko', markersize=8, zorder=5, label='航点')
for i, wp in enumerate(waypoints):
ax1.annotate(f'P{i}', wp, textcoords="offset points", xytext=(6, 8), fontsize=10)
ax1.set_xlabel('X (m)'); ax1.set_ylabel('Y (m)')
ax1.set_title('Corridor 约束示意 —— 轨迹在走廊内避开障碍物')
ax1.legend(loc='upper left', fontsize=9); ax1.grid(True, alpha=0.3)
ax1.set_aspect('equal'); ax1.set_xlim(-2, 14); ax1.set_ylim(-2, 9)
# ==================== 图2：两种时间分配的速度对比 ====================
ax2 = axes[1]
seg_lengths = np.linalg.norm(np.diff(waypoints, axis=0), axis=1)
T_total = 10.0 # 总飞行时间
# 方法一：匀速分配（各段时间相等）
T_uniform = np.full(len(seg_lengths), T_total / len(seg_lengths))
# 方法二：按距离等比例分配
T_prop = T_total * seg_lengths / seg_lengths.sum()
# 计算各段平均速度
v_uniform = seg_lengths / T_uniform
v_prop = seg_lengths / T_prop
# 绘制速度阶梯图
seg_labels = [f'段{i+1}\n({seg_lengths[i]:.1f}m)' for i in range(len(seg_lengths))]
x_pos = np.arange(len(seg_lengths))
width = 0.35
bars1 = ax2.bar(x_pos - width/2, v_uniform, width, color='#ff7f0e', alpha=0.8, label='匀速时间分配')
bars2 = ax2.bar(x_pos + width/2, v_prop, width, color='#1f77b4', alpha=0.8, label='按距离比例分配')
# 标注数值
for bar, v in zip(bars1, v_uniform):
ax2.text(bar.get_x() + bar.get_width()/2, bar.get_height() + 0.05,
f'{v:.2f}', ha='center', fontsize=8)
for bar, v in zip(bars2, v_prop):
ax2.text(bar.get_x() + bar.get_width()/2, bar.get_height() + 0.05,
f'{v:.2f}', ha='center', fontsize=8)
ax2.set_xticks(x_pos); ax2.set_xticklabels(seg_labels, fontsize=9)
ax2.set_ylabel('平均速度 (m/s)'); ax2.set_title('两种时间分配策略的速度对比')
ax2.legend(fontsize=9); ax2.grid(True, alpha=0.3, axis='y')
# 在图上添加时间信息
info_uniform = ' '.join([f'{t:.1f}s' for t in T_uniform])
info_prop = ' '.join([f'{t:.1f}s' for t in T_prop])
ax2.text(0.02, 0.95, f'匀速分配时间: [{info_uniform}]', transform=ax2.transAxes,
fontsize=8, verticalalignment='top', color='#ff7f0e')
ax2.text(0.02, 0.88, f'比例分配时间: [{info_prop}]', transform=ax2.transAxes,
fontsize=8, verticalalignment='top', color='#1f77b4')
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/planning3.png', dpi=150, bbox_inches='tight')
plt.show()运行结果：
回顾 QP 问题：
将约束分为已知（Fixed）和未知（Free）两部分：
其中
总约束矩阵
个人理解：闭式解的计算量主要在
$A$ 矩阵求逆和$R_{PP}$ 矩阵求逆，其余都是矩阵乘法，效率很高。但闭式解只能处理等式约束——Corridor 的不等式约束无法直接纳入，只能将 Corridor 近似为 waypoint 等式约束（即要求轨迹精确经过 Corridor 中心点）。
个人理解：闭式解适用于实时性要求高的场景——当没有不等式约束（无 Corridor），且不想引入 QP solver 的依赖和计算开销时，闭式解是最佳选择。
核心思想：将所有端点处的导数值作为决策变量，分为两类：
-
Fixed (
$\mathbf{d}_F$ )：已知的约束值（起点 PVA、终点 PVA、中间点位置等） -
Free (
$\mathbf{d}_P$ )：待优化的自由变量（中间点的速度、加速度等）
结构化向量——按时间顺序排列所有端点的导数值：
维度为
由于
因此多项式参数向量可以表示为：
其中
个人理解：$M$ 矩阵处理连续性约束（将相邻段共享的端点值合并），$C$ 矩阵将合并后的变量按 Fixed/Free 重排。$K$ 矩阵建立了从"端点导数值"到"多项式系数"的线性映射。
代入目标函数：
将
展开为标量表达式：
对自由变量 $\mathbf{d}P$ 求导并令其为零（注意 $R{PF} = R_{FP}^T$）：
求得
| 比较项 | 闭式解 | QP 求解器 |
|---|---|---|
| 计算速度 | 快（矩阵求逆 + 乘法） | 慢（迭代求解） |
| 等式约束 | 支持 | 支持 |
| 不等式约束 | 不支持 | 支持 |
| Corridor 约束 | 需近似为等式约束 | 直接支持 |
| 适用场景 | 实时性要求高、无 Corridor | 有 Corridor / 动态约束 |
个人理解：闭式解的核心优势是计算速度快——只需矩阵求逆和乘法，无需迭代求解。主要的计算瓶颈在于
$A$ 矩阵的求逆和$R_{PP}$ 的求逆。当段数$M$ 较大时，$A$ 为稀疏带状矩阵，可以利用稀疏结构加速。在实际应用中，如果飞行环境简单（无 Corridor 约束），闭式解是首选；如果需要处理障碍物约束，则必须使用 QP 求解器。
import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import block_diag
plt.rcParams['font.sans-serif'] = ['SimHei'] # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False # 解决负号显示为方框的问题
# ============================================================
# Minimum Snap 闭式求解 vs KKT/QP 求解 —— 3航点（2段）对比验证
# 闭式解：d_P = -R_PP^{-1} R_FP^T d_F
# KKT解：通过拉格朗日乘子法求解等式约束QP
# ============================================================
N = 7 # 7阶多项式
M = 2 # 2段（3个航点）
n_c = N + 1 # 每段系数数 = 8
# --- 航点定义（一维 x 轴，简化演示） ---
waypoints = np.array([0.0, 5.0, 12.0]) # 起点、中间点、终点
T_seg = np.array([2.0, 3.0]) # 各段时间
def poly_coeff_vec(n, t, deriv=0):
"""n阶多项式在t处deriv阶导数的系数行向量"""
c = np.zeros(n + 1)
for i in range(deriv, n + 1):
c[i] = np.prod(range(i, i - deriv, -1)) * t ** (i - deriv)
return c
def build_Q_seg(n, T, kr=4):
"""构造单段 snap 代价矩阵"""
Q = np.zeros((n + 1, n + 1))
for i in range(kr, n + 1):
for j in range(kr, n + 1):
ci = np.prod(range(i, i - kr, -1))
cj = np.prod(range(j, j - kr, -1))
Q[i, j] = ci * cj * T ** (i + j - 2*kr + 1) / (i + j - 2*kr + 1)
return Q
# ===================== 方法1：KKT 求解 =====================
Q_all = block_diag(*[build_Q_seg(N, T) for T in T_seg])
dim = M * n_c
rows, rhs = [], []
def add_eq(row, val):
rows.append(row); rhs.append(val)
# 起点约束：p=0, v=0, a=0
for d in range(3):
r = np.zeros(dim); r[:n_c] = poly_coeff_vec(N, 0, d)
add_eq(r, waypoints[0] if d == 0 else 0.0)
# 终点约束：p=12, v=0, a=0
for d in range(3):
r = np.zeros(dim); r[n_c:] = poly_coeff_vec(N, T_seg[1], d)
add_eq(r, waypoints[2] if d == 0 else 0.0)
# 中间航点位置约束（段0终点）
r = np.zeros(dim); r[:n_c] = poly_coeff_vec(N, T_seg[0], 0)
add_eq(r, waypoints[1])
# 连续性约束（0~3阶导数）
for d in range(4):
r = np.zeros(dim)
r[:n_c] = poly_coeff_vec(N, T_seg[0], d)
r[n_c:] = -poly_coeff_vec(N, 0, d)
add_eq(r, 0.0)
Aeq = np.array(rows); beq = np.array(rhs)
n_eq = len(beq)
KKT = np.zeros((dim + n_eq, dim + n_eq))
KKT[:dim, :dim] = 2 * Q_all
KKT[:dim, dim:] = Aeq.T
KKT[dim:, :dim] = Aeq
rhs_kkt = np.zeros(dim + n_eq); rhs_kkt[dim:] = beq
P_kkt = np.linalg.solve(KKT, rhs_kkt)[:dim]
# ===================== 方法2：闭式解 =====================
# 构造映射矩阵 A_map: 每段端点导数值 -> 多项式系数
# 每段8个系数，端点处有 0~3阶导数（起点4个+终点4个=8个方程）
def build_A_seg(n, T):
"""构造单段的端点约束矩阵（8x8）"""
A = np.zeros((2*4, n+1))
for d in range(4):
A[d, :] = poly_coeff_vec(n, 0, d) # 起点 d 阶导数
A[4+d, :] = poly_coeff_vec(n, T, d) # 终点 d 阶导数
return A
A_map = block_diag(*[build_A_seg(N, T) for T in T_seg]) # 16x16
# d_alpha: 按段排列的端点导数值 [seg0起(p,v,a,j), seg0终(p,v,a,j), seg1起(...), seg1终(...)]
# M矩阵：合并连续性（seg0终 = seg1起），将16维d_alpha映射为12维独立变量d
# 独立端点集：seg0起(4), 中间点(4，共享), seg1终(4) = 12 个变量
M_mat = np.zeros((16, 12))
M_mat[0:4, 0:4] = np.eye(4) # seg0 起点 -> 独立变量 0~3
M_mat[4:8, 4:8] = np.eye(4) # seg0 终点 -> 中间点变量 4~7
M_mat[8:12, 4:8] = np.eye(4) # seg1 起点 -> 中间点变量 4~7（连续性）
M_mat[12:16, 8:12] = np.eye(4) # seg1 终点 -> 独立变量 8~11
# C矩阵：将独立变量重排为 [Fixed; Free]
# Fixed: seg0起_p(0), seg0起_v(0), seg0起_a(0), 中间_p(5), seg1终_p(12), seg1终_v(0), seg1终_a(0) = 7个
# Free: seg0起_j, 中间_v, 中间_a, 中间_j, seg1终_j = 5个
fixed_idx = [0, 1, 2, 4, 8, 9, 10] # 在12维独立变量中的索引
free_idx = [3, 5, 6, 7, 11]
d_F = np.array([waypoints[0], 0, 0, waypoints[1], waypoints[2], 0, 0])
n_fixed, n_free = len(fixed_idx), len(free_idx)
C_mat = np.zeros((12, 12))
for i, fi in enumerate(fixed_idx):
C_mat[fi, i] = 1.0
for i, fi in enumerate(free_idx):
C_mat[fi, n_fixed + i] = 1.0
# K = A_map^{-1} @ M @ C
K = np.linalg.solve(A_map, M_mat @ C_mat)
# R = K^T Q K，分块为 R_FF, R_FP, R_PF, R_PP
R_full = K.T @ Q_all @ K
R_FF = R_full[:n_fixed, :n_fixed]
R_FP = R_full[:n_fixed, n_fixed:]
R_PP = R_full[n_fixed:, n_fixed:]
# 闭式解核心公式
d_P = -np.linalg.solve(R_PP, R_FP.T @ d_F)
d_prime = np.concatenate([d_F, d_P])
P_closed = K @ d_prime
# ===================== 采样与对比绘图 =====================
def sample_traj(P_vec, T_seg, N):
t_all, pos_kkt, vel_kkt = [], [], []
t_offset = 0.0
for k in range(len(T_seg)):
ts = np.linspace(0, T_seg[k], 150)
p = P_vec[k*n_c:(k+1)*n_c]
pos_kkt.extend([poly_coeff_vec(N, t, 0) @ p for t in ts])
vel_kkt.extend([poly_coeff_vec(N, t, 1) @ p for t in ts])
t_all.extend(ts + t_offset)
t_offset += T_seg[k]
return np.array(t_all), np.array(pos_kkt), np.array(vel_kkt)
t1, pos1, vel1 = sample_traj(P_kkt, T_seg, N)
t2, pos2, vel2 = sample_traj(P_closed, T_seg, N)
fig, axes = plt.subplots(2, 1, figsize=(10, 7), sharex=True)
# 位置对比
axes[0].plot(t1, pos1, 'b-', linewidth=2.5, label='KKT/QP 求解')
axes[0].plot(t2, pos2, 'r--', linewidth=2, label='闭式解')
t_wp = [0, T_seg[0], T_seg[0]+T_seg[1]]
axes[0].plot(t_wp, waypoints, 'ko', markersize=8, zorder=5, label='航点')
axes[0].set_ylabel('位置 (m)'); axes[0].legend(); axes[0].grid(True, alpha=0.3)
axes[0].set_title('Minimum Snap 闭式解 vs KKT 求解 —— 轨迹完全重合验证')
# 速度对比
axes[1].plot(t1, vel1, 'b-', linewidth=2.5, label='KKT/QP 速度')
axes[1].plot(t2, vel2, 'r--', linewidth=2, label='闭式解速度')
axes[1].set_ylabel('速度 (m/s)'); axes[1].set_xlabel('时间 (s)')
axes[1].legend(); axes[1].grid(True, alpha=0.3)
# 打印误差
err = np.max(np.abs(P_kkt - P_closed))
print(f"两种方法系数最大绝对误差: {err:.2e}")
fig.text(0.5, 0.01, f'系数最大绝对误差: {err:.2e}（验证两种方法等价）',
ha='center', fontsize=11, color='green')
plt.tight_layout(rect=[0, 0.03, 1, 1])
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/planning4.png', dpi=150, bbox_inches='tight')
plt.show()运行结果：
| 符号 | 含义 |
|---|---|
| 位置轨迹（标量，单个轴） | |
| 第 |
|
| 所有段系数的堆叠向量 | |
| 多项式阶数 | |
| 轨迹段数 | |
| 第 |
|
| 第 |
|
| 第 |
|
| 总 Hessian 矩阵（分块对角） | |
| 等式约束矩阵 | |
| 等式约束右端向量 | |
| Fixed 约束值 | |
| Free 变量（待优化） | |
| 端点值到系数的映射矩阵 | |
| 分块 Hessian 子矩阵 | |
| 优化的导数阶数（snap: |
|
| 归一化时间 |
|
| Corridor 半宽 |