PythonRobotics 如何用 C-GMRES 求解非线性模型预测控制做路径跟踪-平芜编程栈

PythonRobotics 如何用 C-GMRES 求解非线性模型预测控制做路径跟踪

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics

在 PythonRobotics 的PathTracking目录中有一个用 C-GMRES（GMRES 的修正版本）求解非线性模型预测控制（NMPC）的样例：让一个两轮车模型从给定的初始位姿出发，每一步在线求解一个非线性方程组得到最优加速度与转角输入，最终收敛到原点位姿。本文的目标是跑通 cgmres_nmpc.py 这个仿真脚本，并知道从哪些输出判断控制器是否按预期收敛。运行环境方面，README.md 要求 Python 3.13.x 以及 NumPy、SciPy、Matplotlib、cvxpy，仓库在requirements/下提供了固定版本号的安装清单。

样例对应的模型与求解方式

读懂下面的运行结果之前，先确认样例的数学设定，这些内容在 cgmres_nmpc_main.rst 的 "Mathematical Formulation" 一节中给出。

运动模型为四状态（x, y, θ, v），控制输入是加速度 u_a 与转角 u_δ：

x_dot = v cos(θ) y_dot = v sin(θ) theta_dot = v / WB * sin(u_δ) v_dot = u_a

文档特别注明 "tan is not good for optimization"，所以航向角方程采用sin(u_δ)而不是通常的tan(u_δ)。代价函数为J = 1/2(u_a² + u_δ²) - φ_a d_a - φ_δ d_δ，输入约束|u_a| ≤ u_a_max、|u_δ| ≤ u_δ_max通过 dummy 变量 d_a、d_δ 改写进目标函数，再按 Hamiltonian 的偏导条件（∂H/∂u 与约束方程）组成非线性方程组 F = 0，每个控制步用 C-GMRES 迭代求解。

模块内的关键常量（均来自 cgmres_nmpc.py 文件头部）：

U_A_MAX = 1.0 # 加速度幅值上限 U_OMEGA_MAX = radians(45.0) # 转角幅值上限 PHI_V = 0.01 PHI_OMEGA = 0.01 WB = 0.25 # [m] wheel base

控制器NMPCControllerCGMRES的构造参数及代码中的注释：

self.zeta = 100. # stability gain self.ht = 0.01 # difference approximation tick self.tf = 3.0 # final time self.alpha = 0.5 # time gain self.N = 10 # division number self.threshold = 0.001 self.input_num = 6 # input number of dummy, constraints self.max_iteration = self.input_num * self.N # 即 60

按类 docstring 的说法：zeta 是 "gain of optimal answer stability"，ht 是 NMPC 的更新量且 "should be decided by zeta"，tf 是预测时间，alpha 是预测时间增益，N 是离散预测步数，threshold 是 C-GMRES 的收敛阈值，max_iteration "decide by the solved matrix size"。求解循环中，残差judge_value的范数小于 threshold 或迭代到 max_iteration（60 次）时停止，取上一轮的迭代解更新输入序列。

准备 Python 环境与依赖

先克隆仓库并进入目录：

git clone https://gitcode.com/GitHub_Trending/py/PythonRobotics cd PythonRobotics

安装依赖有两条等价路径，README.md 和 getting started 文档都给出：

# 使用 conda（requirements/environment.yml 中声明 python=3.13） conda env create -f requirements/environment.yml

# 使用 pip pip install -r requirements/requirements.txt

requirements/requirements.txt 固定了具体版本：numpy == 2.3.5、scipy == 1.18.1、matplotlib == 3.11.0、cvxpy == 1.8.1，以及用于单元测试的pytest、pytest-xdist等。

Python 版本有一处文档不一致需要注意：README.md 写明 "Python 3.13.x"，requirements/environment.yml也是python=3.13；但 getting started 文档（2_how_to_run_sample_codes_main.rst）写的是 "Python 3.12.x"，并说明样例代码只在该版本上测试过。conda 路线以 environment.yml 的 3.13 为准，pip 路线建议同样使用 3.13。

运行 C-GMRES NMPC 仿真

getting started 文档给出的运行方式是进入样例目录执行脚本：

cd PathTracking/cgmres_nmpc python cgmres_nmpc.py

脚本是单文件自包含的，没有相对导入，因此也可以从仓库根目录执行python PathTracking/cgmres_nmpc/cgmres_nmpc.py，效果相同。

main()中的仿真设定（全部来自源码）：

dt = 0.1 iteration_time = 150.0 # [s] init_x = -4.5 init_y = -2.5 init_yaw = radians(45.0) init_v = -1.0

即车辆从 (-4.5, -2.5)、航向 45°、速度 -1.0 出发，总仿真时长 150 s，共约 1500 个控制步。每一步的流程是：

调用controller.calc_input(x, y, yaw, v, time)，内部用当前输入序列前向预测状态、逆向计算伴随状态，组装非线性方程组 F，再用 C-GMRES 迭代更新u_1s、u_2s以及 dummy、raw 约束变量序列；
取序列首元素u_1s[0]、u_2s[0]作为当前实际输入，调用plant_system.update_state()更新车辆状态。

每个控制步都会向控制台打印一行：

norm(F) = ...

这里的 F 就是_calc_f()组装的最优条件与约束方程向量（每步 2N=20 个方程）。这个数值就是判断求解是否收敛的直接依据：文档配图（上文的第四张图，为文档示例）对应的 "optimal error" 曲线在起始有尖峰后迅速衰减，150 s 内基本保持接近 0，说明 C-GMRES 每步都把 F 压到了很低的水平。你不需要与文档示例的具体数值对齐，但衰减趋势应当一致。

查看与验证结果

运行完成后（show_animation = True为默认值）会弹出动画窗口和 4 组图形窗口，这是animation()与plot_figures()的输出：

轨迹图：红色线是车辆轨迹，起点和原点处的目标位姿各画了一辆车（plot_car(0.0, 0.0, 0.0, 0.0)）。文档示例中车辆从左下角平滑驶向原点并在终点附近静止，对应下文状态图。
状态时间序列图：x、y、yaw、v 四条曲线，最终收敛到 0（即停在原点、航向为 0）。
输入时间序列图：u_a、u_omega 以及两个 dummy 输入 u_1、u_2 的取值。
约束与误差图：约束变量 raw_1、raw_2 和 optimal error（即norm(F)的历史记录history_f）。

动画窗口中车辆按时间回放行驶过程，标题栏实时显示Time[s]、accel[m/s]、speed[km/h]；代码注册了键盘事件，在动画窗口按 Esc 键会直接退出程序（exit(0)），这是源码注释明确说明的停止方式。

如果不想打开图形窗口，只跑仿真本身，把文件头的show_animation改为False再运行即可，控制台仍会逐步打印norm(F)。

用单元测试验证

仓库自带了这个样例的测试 tests/test_cgmres_nmpc.py，它把show_animation置为False后完整调用main()：

def test1(): m.show_animation = False m.main()

在仓库根目录执行：

python -m pytest tests/test_cgmres_nmpc.py

tests/conftest.py会自动把仓库根目录加入sys.path，所以单文件测试可以直接从根目录运行，无需先激活特殊路径。测试通过说明整个 150 s 仿真（包括每步 C-GMRES 迭代）没有异常抛出且收敛循环正常执行。如果要跑全部样例的完整测试套件，仓库提供了runtests.sh（等价于pytest tests -l -Werror --durations=0），但它会执行所有算法样例的测试，本文只关心 cgmres_nmpc 时请使用上面的单文件命令。

限制说明

该样例是算法演示：初始位姿、仿真时长、约束上限和控制器参数都硬编码在文件中，代码没有提供命令行参数来修改这些值，需要改动时直接编辑 cgmres_nmpc.py。
动画和plt.show()图形窗口依赖图形显示环境；无显示器的环境请关闭show_animation或用 pytest 方式运行。
文档中该样例的数学推导引用了外部项目与文章（见 cgmres_nmpc_main.rst 的 "Ref" 一节），本文不展开这些外部参考的内容。

【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics

创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考