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

PythonRobotics 如何用 C-GMRES 求解非线性模型预测控制做路径跟踪【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics在 PythonRobotics 的PathTracking目录中有一个用 C-GMRESGMRES 的修正版本求解非线性模型预测控制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 stabilityht 是 NMPC 的更新量且 should be decided by zetatf 是预测时间alpha 是预测时间增益N 是离散预测步数threshold 是 C-GMRES 的收敛阈值max_iteration decide by the solved matrix size。求解循环中残差judge_value的范数小于 threshold 或迭代到 max_iteration60 次时停止取上一轮的迭代解更新输入序列。准备 Python 环境与依赖先克隆仓库并进入目录git clone https://gitcode.com/GitHub_Trending/py/PythonRobotics cd PythonRobotics安装依赖有两条等价路径README.md 和 getting started 文档都给出# 使用 condarequirements/environment.yml 中声明 python3.13 conda env create -f requirements/environment.yml# 使用 pip pip install -r requirements/requirements.txtrequirements/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.xrequirements/environment.yml也是python3.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()组装的最优条件与约束方程向量每步 2N20 个方程。这个数值就是判断求解是否收敛的直接依据文档配图上文的第四张图为文档示例对应的 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.pytests/conftest.py会自动把仓库根目录加入sys.path所以单文件测试可以直接从根目录运行无需先激活特殊路径。测试通过说明整个 150 s 仿真包括每步 C-GMRES 迭代没有异常抛出且收敛循环正常执行。如果要跑全部样例的完整测试套件仓库提供了runtests.sh等价于pytest tests -l -Werror --durations0但它会执行所有算法样例的测试本文只关心 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），仅供参考