PythonRobotics 模型预测轨迹生成器（MPTG）实战：高斯-牛顿轨迹优化与查找表生成

本篇技术指南以 PythonRobotics 仓库中的 ModelPredictiveTrajectoryGenerator 模块为对象，系统讲解模型预测轨迹生成器（Model Predictive Trajectory Generator, MPTG）的路径优化算法：如何用数值微分 + 高斯-牛顿迭代求解满足运动学约束的可行轨迹，如何离线生成 lookup_table 以加速在线查询，以及它如何作为状态晶格规划器（State Lattice Planner）的边界问题求解器被复用。读完本文，你将掌握该模块的算法流程、核心参数含义与调参方法，并能直接运行仓库示例复现路径优化与查找表生成过程。

1. 模块定位：为状态晶格规划器求解边界问题

在 PythonRobotics 的路径规划目录中，ModelPredictiveTrajectoryGenerator 是一个路径优化示例模块，其文档明确指出：该算法被用于 State Lattice Planner（状态晶格规划器）（见 model_predictive_trajectory_generator_main.rst）。

状态晶格规划的核心思路是：在状态空间（位置 + 航向角）中采样一组终端状态，然后为每个终端状态生成一条满足车辆运动学约束的可行轨迹。这一步"从给定起始状态到达指定终端状态"的求解，本质是一个两点边值问题（boundary value problem）。MPTG 正是被设计用来解决这一问题的——它接收一个目标状态和一个初始参数猜测，通过迭代优化输出一条动力学可行的轨迹参数。

从源码结构看，该模块包含三个相互依赖的文件：

文件	职责
motion_model.py	车辆运动学模型与轨迹生成（前向积分）
trajectory_generator.py	轨迹优化主算法（迭代求解参数）
lookup_table_generator.py	离线生成查找表并保存为 CSV

2. 运动学模型：自行车模型与二次曲率插值

motion_model.py 提供了轨迹生成的基础设施，其核心是一个自行车模型（bicycle model），运动参数在文件头部定义：

L = 1.0   # wheel base [m]
ds = 0.1  # course distance [m]
v = 10.0 / 3.6  # velocity [m/s]  (即 10 km/h)

模型的状态由 State 类描述，包含位置 x, y、航向角 yaw 和速度 v。update 函数完成单步离散积分：

state.x = state.x + state.v * math.cos(state.yaw) * dt
state.y = state.y + state.v * math.sin(state.yaw) * dt
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt

其中 delta 是前轮转角（等效于曲率控制输入），L 是轮距，dt 是积分步长；每步积分后航向角通过 pi_2_pi（内部调用 utils.angle.angle_mod，见 angle.py）归一化到 [-pi, pi]。

轨迹不是用一组离散转角点列表达的，而是用一个二次（quadratic）插值函数描述曲率随时间的连续变化。generate_trajectory(s, km, kf, k0) 接受四个参数：

• s：轨迹总路程（course distance）
• k0：起始曲率（初始时刻）
• km：中点曲率（时刻 time/2）
• kf：终点曲率（末端时刻）

函数在时间轴 [0, time/2, time] 上取三个曲率控制点 [k0, km, kf]，用 scipy.interpolate.interp1d 以 quadratic 方式插值出整条曲率曲线，再以 ds = 0.1 m 的间隔离散积分生成轨迹点列。也就是说，一条可行轨迹只需 3 个参数 (s, km, kf) 即可完整描述——这正是后续优化算法只迭代这 3 个变量的原因。generate_last_state 是它的变体，只返回末端状态 (x, y, yaw)，供优化器频繁调用以评估代价。

3. 核心算法：基于数值雅可比的迭代优化

trajectory_generator.py 实现了 MPTG 的核心优化流程，文档中通过 .. autofunction:: 直接索引的公开接口是 optimize_trajectory。它本质上是一个高斯-牛顿（Gauss-Newton）迭代法，目标是最小化末端状态与目标状态之间的误差。

3.1 目标与代价函数

给定目标状态 target 和初始曲率 k0，算法迭代调整轨迹参数向量 p = [s, km, kf]^T，使轨迹末端尽可能接近目标。代价定义为末端状态偏差向量的 2-范数：

dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1)
cost = np.linalg.norm(dc)

其中 calc_diff 计算 [target.x - x[-1], target.y - y[-1], pi_2_pi(target.yaw - yaw[-1])]，航向角误差同样经过角度归一化处理。

3.2 数值雅可比矩阵

由于轨迹末端状态与参数 p 之间不存在解析导数，算法使用中心差分法计算 3×3 雅可比矩阵。calc_j 对 p 的每个分量分别做 +h 和 -h 扰动，调用 generate_last_state 得到扰动后的末端状态，再求差分商。参数采样距离定义在文件顶部：

max_iter = 100                     # 最大迭代次数
h = np.array([0.5, 0.02, 0.02]).T  # 参数采样距离：s, km, kf
cost_th = 0.1                      # 代价收敛阈值

即路程 s 的差分步长为 0.5 m，曲率 km、kf 的差分步长为 0.02 rad/m（从代码结构看，该模块采用的数值微分精度为二阶中心差分，可推断其精度优于一阶前向差分）。

3.3 迭代更新与学习率选择

主循环 optimize_trajectory 的迭代逻辑为：

1. 用当前 p 生成完整轨迹，计算末端偏差 dc 与代价；
2. 若 cost <= cost_th，输出 "path is ok" 并终止；
3. 否则计算雅可比 J，求解增量 dp = -J^{-1} @ dc（即高斯-牛顿步）；
4. 若矩阵奇异（捕获 np.linalg.LinAlgError），打印 "cannot calc path LinAlgError" 并返回空结果；
5. 用 selection_learning_param 在 [1.0, 2.0] 区间内以 0.5 步长搜索使代价最小的学习率 alpha，执行 p += alpha * dp；
6. 若 100 次迭代内未收敛，返回 None 并打印 "cannot calc path"。

selection_learning_param 采用线搜索策略：对候选步长 a in {1.0, 1.5, 2.0}，计算试探轨迹的末端偏差范数，选择代价最小且非零的 a 作为学习率，从而在保证收敛性的同时避免过冲。

3.4 演示示例与参数

直接运行模块即可复现文档所述的"路径优化示例"：

python PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py

optimize_trajectory_demo 中的默认场景为：

• 目标状态：x = 5.0 m, y = 2.0 m, yaw = 90°
• 初始曲率：k0 = 0.0
• 初始参数猜测：init_p = [6.0, 0.0, 0.0]^T（即先假设一条 6 m 的直线轨迹）

迭代过程中若 show_animation = True，会以动画实时绘制当前轨迹（红色曲线）与目标箭头，直观展示轨迹逐步"逼近"目标状态的过程。该演示恰好对应文档中引用的外部 GIF 动画所展示的效果。

4. 查找表生成：把在线优化变为离线查询

文档的第二部分"Lookup table generation sample"对应 lookup_table_generator.py。其动机很直接：每次在线调用 optimize_trajectory 都需迭代求解，代价较高；如果离线对一大片目标状态空间批量优化，并把成功的结果存成表，在线时就只需查最近邻条目作为初始猜测，大幅加速收敛。

4.1 状态空间采样

calc_states_list 以默认 max_yaw = -30° 生成终端状态网格：

• x：从 10.0 到 30.0，步长 5.0（即 10/15/20/25/30）
• y：从 0.0 到 20.0，步长 2.0
• yaw：从 -30° 到 +30°，步长为 30°（即 -30°/0°/30°）

三层嵌套循环共产生 5 × 11 × 3 = 165 个终端状态。

4.2 逐状态优化与最近邻引导

generate_lookup_table 的处理流程为：

1. 查找表中预置一个平凡条目 [1.0, 0.0, 0.0, 1.0, 0.0, 0.0]（直线 1 m）；
2. 对每个目标状态，调用 search_nearest_one_from_lookup_table 在已生成的表中按欧氏距离（含航向角维度）找最近条目；
3. 以该条目的 (km, kf) 和当前状态到原点的距离 hypot(x, y) 作为初始猜测 init_p，调用 trajectory_generator.optimize_trajectory 求解；
4. 优化成功（返回非 None）则把末端状态与参数 [x, y, yaw, s, km, kf] 追加进表。

这种"用已解出的近邻解引导新解"的增量求解策略，让后续优化总是从接近最优的参数出发，显著提高成功率与收敛速度（从源码结构看，这是对 165 个状态批量求解能够高效完成的关键）。

4.3 CSV 输出格式

save_lookup_table 将结果写入 lookup_table.csv，表头为：

x,y,yaw,s,km,kf

每行含义：(x, y, yaw) 为轨迹末端状态，(s, km, kf) 为可复现该轨迹的三个参数。运行：

python PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py

会在当前目录生成 lookup_table.csv，并弹出红色轨迹簇的绘图窗口——所有轨迹从原点向右侧呈扇形发散，对 yaw 正负两侧近似镜像对称（由 generate_lookup_table 中对 -km, -kf 的对称回放实现），直观展示该查找表覆盖的可行轨迹族：

5. 与 State Lattice Planner 的集成

MPTG 不是孤立模块，它在同目录下的 StateLatticePlanner 中被直接复用：

import ModelPredictiveTrajectoryGenerator.trajectory_generator as planner
import ModelPredictiveTrajectoryGenerator.motion_model as motion_model

TABLE_PATH = os.path.dirname(os.path.abspath(__file__)) + "/lookup_table.csv"

状态晶格规划器的工作方式为：

1. 用 calc_uniform_polar_states / calc_biased_polar_states / calc_lane_states 等函数生成终端状态采样集合；
2. generate_path 对每个目标状态调用 search_nearest_one_from_lookup_table 查表获取初始猜测，再调用 planner.optimize_trajectory 求解；
3. 成功求解的轨迹以 [x, y, yaw, s, km, kf] 追加到结果列表，供后续 lattice 图搜索使用。

仓库中随附的 lookup_table.csv（共 82 行，含表头）就是由 lookup_table_generator.py 生成的预置查找表，其首条平凡条目 1.0,0.0,0.0,1.0,0.0,0.0 与生成器中的预置条目完全一致，印证了文档中"该算法用于 state lattice planner"的说明。规划器模块的文档也明确指出："This code uses the model predictive trajectory generator to solve boundary problem"（见 state_lattice_planner_main.rst）。

6. 测试与运行环境

仓库测试目录中 test_state_lattice_planner.py 会设置 show_animation = False 后调用 m.main()，从而间接覆盖 optimize_trajectory 的完整调用链（含 calc_uniform_polar_states、calc_biased_polar_states、calc_lane_states 三条采样分支），可用如下命令执行：

python tests/test_state_lattice_planner.py

或通过仓库根目录的 runtests.sh 批量运行全部测试。运行本模块所需的核心依赖在 requirements.txt 中锁定版本：numpy == 2.3.5、scipy == 1.18.1（轨迹曲率插值依赖 scipy.interpolate.interp1d）、matplotlib == 3.11.0（可视化与动画）。

7. 小结

PythonRobotics 的 ModelPredictiveTrajectoryGenerator 提供了一个简洁而完整的"可行轨迹参数化 + 迭代优化 + 离线查找表"范式：

• 参数化：一条轨迹仅由 (s, km, kf) 三个参数决定，通过二次曲率插值与自行车模型前向积分生成；
• 优化：高斯-牛顿迭代配合中心差分数值雅可比与线搜索学习率，收敛阈值 cost_th = 0.1，最多迭代 100 次；
• 加速：离线对 165 个终端状态批量求解生成 lookup_table.csv，在线时以最近邻条目作为初始猜测，实现"查表引导 + 快速收敛"；
• 复用：作为边界问题求解器被 State Lattice Planner 直接调用，构成采样-求解-搜索的完整状态晶格规划链路。

该算法的理论依据来自文档末尾引用的经典文献 Optimal rough terrain trajectory generation for wheeled mobile robots（发表于 The International Journal of Robotics Research），如需深入研究其数学推导，可循此文献继续阅读。

<输出文章>