PythonRobotics 倒立摆控制实战从拉格朗日建模到 LQR 与 MPC 的完整实现【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics导读本文以 PythonRobotics 项目中的倒立摆Inverted Pendulum模块为核心完整讲解小车-倒立摆系统的数学建模拉格朗日方程、线性化与状态空间表达并深入剖析仓库中两套控制方案的源码实现基于离散代数 Riccati 方程的 LQR 控制器inverted_pendulum_lqr_control.py与基于凸优化求解的 MPC 控制器inverted_pendulum_mpc_control.py。读完本文你将掌握倒立摆模型的推导全流程、两种控制器各自的代价函数与求解路径以及如何在本仓库环境中一键运行并可视化验证控制效果。一、倒立摆系统与建模1.1 系统构成倒立摆Inverted Pendulum on a Cart由一根长度为l、顶端带有质量m的摆杆组成摆杆底部通过转轴安装在可以水平移动的小车上。控制系统的目标是通过对小车施加水平推力u使倒立摆保持竖直平衡。这是一个典型的非线性、开环不稳定的欠驱动系统也是验证线性控制与最优控制理论的经典实验平台。模型涉及的主要物理量如下符号含义M小车质量kgm摆杆顶端负载质量kgl摆杆长度mu施加在小车上的水平力Nx小车水平位置坐标mθ摆杆相对竖直方向的偏角radg重力加速度m/s²1.2 拉格朗日方程推导利用拉格朗日方程Lagranges equations可得到系统的完整非线性动力学方程(M m)ẍ - mlθ̈cosθ mlθ̇²sinθ u lθ̈ - g·sinθ ẍ·cosθ其中第一式为小车平动方向的力平衡第二式描述摆杆绕转轴的转动。经过整理可以得到两个广义加速度的显式表达式ẍ [m(g·cosθ - θ̇²·l)·sinθ u] / (M m - m·cos²θ) θ̈ [g(M m)·sinθ - θ̇²·l·m·sinθ·cosθ u·cosθ] / (l·(M m - m·cos²θ))注意式中的非线性耦合项sinθ、cosθ以及角速度平方项θ̇²它们使得系统无法直接使用线性控制理论求解。二、线性化与状态空间模型2.1 小角度线性化在倒立摆竖直平衡点附近θ很小采用近似cosθ ≈ 1, sinθ ≈ θ, θ̇² ≈ 0代入非线性方程后得到线性化模型ẍ (g·m/M)·θ (1/M)·u θ̈ g(M m)/(M·l)·θ 1/(M·l)·u2.2 状态空间表示选取状态向量x [x, ẋ, θ, θ̇]ᵀ可将系统写成标准状态空间形式ẋ A·x B·u y C·x D·u其中A | 0 1 0 0 | | 0 0 g·m/M 0 | | 0 0 0 1 | | 0 0 g(Mm)/(M·l) 0 | B | 0 | |1/M | | 0 | |1/(M·l) |若只控制摆角θ则输出矩阵为C | 0 0 1 0 |, D [0]若同时控制小车位置x与摆角θ则C | 1 0 0 0 | | 0 0 1 0 |, D | 0 | | 0 |2.3 源码中的离散化实现仓库源码在 inverted_pendulum_lqr_control.py 的get_model_matrix()函数中实现了连续矩阵A、B的一阶欧拉离散化时间步长delta_t 0.1sA np.eye(nx) delta_t * A # 离散化A_d I Δt·A B delta_t * B # 离散化B_d Δt·B也就是说两个控制器LQR 与 MPC实际使用的都是离散时间模型x[k1] A·x[k] B·u[k]与文档中 DARE离散代数 Riccati 方程的求解方式完全对应。MPC 实现 inverted_pendulum_mpc_control.py 中复用了完全相同的get_model_matrix()函数两套控制器共享同一套模型参数便于公平对比。三、LQR 控制器最优状态反馈3.1 控制原理LQRLinear Quadratic Regulator线性二次型调节器通过最小化如下二次型代价函数来设计状态反馈增益J xᵀ·Q·x uᵀ·R·u其中Q为状态加权矩阵R为控制输入加权矩阵。使代价函数最小化的反馈控制律为u -K·x反馈增益矩阵K (Bᵀ·P·B R)⁻¹·Bᵀ·P·A其中P是离散时间代数 Riccati 方程DARE的唯一正定解P Aᵀ·P·A - Aᵀ·P·B·(R Bᵀ·P·B)⁻¹·Bᵀ·P·A Q3.2 源码实现细节LQR 实现位于 inverted_pendulum_lqr_control.py核心流程分为三步solve_DARE(A, B, Q, R)第 72-85 行采用不动点迭代法求解 Riccati 方程初始值P Q最大迭代maxiter150收敛阈值eps0.01当max(|Pₙ - P|) eps时提前终止dlqr(A, B, Q, R)第 88-103 行基于 DARE 解计算增益K并通过对闭环矩阵A - B·K求特征值eigVals来验证闭环稳定性所有特征值落在单位圆内则系统稳定lqr_control(x)第 106-113 行在每个控制周期实时求解dlqr并计算u -K·x同时打印单次计算耗时。模型默认参数如下l_bar 2.0 # 摆杆长度 l [m] M 1.0 # 小车质量 [kg] m 0.3 # 摆杆顶端质量 [kg] g 9.8 # 重力加速度 [m/s²] nx 4 # 状态数 nu 1 # 输入数 Q np.diag([0.0, 1.0, 1.0, 0.0]) # 状态代价矩阵仅对角速度与摆角加权 R np.diag([0.01]) # 输入代价矩阵 delta_t 0.1 # 仿真时间步长 [s] sim_time 5.0 # 总仿真时长 [s]从Q的设置可以看出θ̇摆角速度与θ摆角被加权权重 1.0而位置x与其速度ẋ权重为 0即该配置下控制器只关心摆角平衡不约束小车位移摆杆平衡后小车会自由漂移——这正是文档中只控制 θ情形的工程体现。3.3 运行方式# 在仓库根目录下执行 python InvertedPendulum/inverted_pendulum_lqr_control.py初始状态为x0 [0.0, 0.0, 0.3, 0.0]ᵀ摆角初始偏移 0.3 rad ≈ 17.2°。运行结束后终端会打印最终状态Finish x... [m] , theta... [deg]默认show_animation True会弹出 matplotlib 动画窗口实时绘制小车与摆杆的运动按Esc键可随时终止仿真见 plot_cart 函数。也可在代码中或通过测试将其置为False以无头模式运行。四、MPC 控制器滚动时域优化4.1 控制原理MPCModel Predictive Control模型预测控制在每个控制周期内基于当前状态求解一个有限时域的带约束最优控制问题并只施加第一个最优控制量然后滚动推进。其代价函数与 LQR 形式相同J xᵀ·Q·x uᵀ·R·u但受限于两个约束条件线性化倒立摆模型x[k1] A·x[k] B·u[k]初始状态x[0] x₀。4.2 源码实现细节MPC 实现位于 inverted_pendulum_mpc_control.py依赖cvxpy凸优化库核心函数mpc_control(x0)第 77-108 行的构造过程x cvxpy.Variable((nx, T 1)) # 状态变量4 维 × 31 步 u cvxpy.Variable((nu, T)) # 输入变量1 维 × 30 步在预测时域T 30对应 30 × 0.1s 3s 的预测窗口内逐时刻累加代价cost cvxpy.quad_form(x[:, t 1], Q) # 状态代价 cost cvxpy.quad_form(u[:, t], R) # 输入代价 constr [x[:, t 1] A x[:, t] B u[:, t]] # 模型约束 constr [x[:, 0] x0[:, 0]] # 初始状态约束随后构建并求解优化问题prob cvxpy.Problem(cvxpy.Minimize(cost), constr) prob.solve(verboseFalse, solvercvxpy.CLARABEL)求解器选用CLARABELcvxpy 内置的开源内点法求解器若求解状态为cvxpy.OPTIMAL则返回整条最优轨迹位置、速度、摆角、摆角速度、输入序列主循环只取第一个控制量u opt_input[0]施加给系统若求解失败各返回值置为None。与 LQR 版本相比MPC 版本额外引入了预测时域参数T 30 # 预测时域长度Horizon length这是两套控制器最本质的区别LQR 通过一次离线/在线求解 Riccati 方程得到全局反馈增益而 MPC 则在每个时间步在线求解一次 30 步的凸优化问题天然支持未来状态预测与约束扩展如输入饱和、状态边界代价是更高的单步计算开销。4.3 运行方式# 在仓库根目录下执行 python InvertedPendulum/inverted_pendulum_mpc_control.py运行前提环境中需安装cvxpy含 CLARABEL 求解器。项目 requirements/requirements.txt 中锁定的版本为cvxpy 1.8.1同时依赖ecos 2.0.14可通过pip install -r requirements/requirements.txt一键安装。MPC 版的仿真时长、步长、初始状态与 LQR 版完全一致便于直接对比两种控制器在相同工况下的表现与单步计算耗时源码中均通过time.time()计时并打印。五、测试验证仓库为两个控制器均配备了自动化测试tests/test_inverted_pendulum_lqr_control.py测试中先将m.show_animation False关闭动画再调用m.main()执行完整 5 秒仿真验证 LQR 控制流程可正常运行tests/test_inverted_pendulum_mpc_control.py以同样方式验证 MPC 控制流程。执行方式# 在仓库根目录下运行 pytest tests/test_inverted_pendulum_lqr_control.py tests/test_inverted_pendulum_mpc_control.py测试与主程序使用同一套main()入口说明两个脚本在设计上即插即用——既可作为独立演示程序运行也可被测试框架无头化复用。六、总结与工程要点对比维度LQRinverted_pendulum_lqr_control.pyMPCinverted_pendulum_mpc_control.py核心思想求解 DARE 得到全局最优状态反馈增益滚动求解有限时域凸优化问题代价函数J xᵀQx uᵀRu同左逐时刻累加约束处理无显式约束模型约束 初始状态约束可扩展求解依赖NumPy矩阵运算cvxpy CLARABEL关键参数Q、R、delta_tQ、R、delta_t、T预测时域单步计算Riccati 方程迭代求解开销小每步在线求解 QP开销较大工程实践上从本项目源码可以提炼三点可复用的经验模型离散化是关键桥梁LQR 与 MPC 共用同一份get_model_matrix()离散化代码任何模型参数M、m、l的修改都会同时作用于两套控制器保证了对比实验的公平性Q/R的权重语义直接决定控制行为本项目中Q对角速度与摆角加权而位置不加权控制器只维持摆角平衡如需定点平衡同时约束小车位移只需调整Q中对应位置的权重并按文档补充C矩阵动画与无头模式双通道show_animation开关配合Esc键退出机制兼顾了教学演示可视化与自动化测试无头两种场景值得在机器人算法示例代码中推广。如果想进一步探索最优控制理论细节可参考模块文档 docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst其中包含完整的拉格朗日推导、线性化过程与状态空间矩阵与本文源码分析相互印证。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 
长河君 · 2026/9/11 23:47:04 · 阅读 1.2k PythonRobotics 倒立摆控制实战：从拉格朗日建模到 LQR 与 MPC 的完整实现
PythonRobotics 倒立摆控制实战从拉格朗日建模到 LQR 与 MPC 的完整实现【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics导读本文以 PythonRobotics 项目中的倒立摆Inverted Pendulum模块为核心完整讲解小车-倒立摆系统的数学建模拉格朗日方程、线性化与状态空间表达并深入剖析仓库中两套控制方案的源码实现基于离散代数 Riccati 方程的 LQR 控制器inverted_pendulum_lqr_control.py与基于凸优化求解的 MPC 控制器inverted_pendulum_mpc_control.py。读完本文你将掌握倒立摆模型的推导全流程、两种控制器各自的代价函数与求解路径以及如何在本仓库环境中一键运行并可视化验证控制效果。一、倒立摆系统与建模1.1 系统构成倒立摆Inverted Pendulum on a Cart由一根长度为l、顶端带有质量m的摆杆组成摆杆底部通过转轴安装在可以水平移动的小车上。控制系统的目标是通过对小车施加水平推力u使倒立摆保持竖直平衡。这是一个典型的非线性、开环不稳定的欠驱动系统也是验证线性控制与最优控制理论的经典实验平台。模型涉及的主要物理量如下符号含义M小车质量kgm摆杆顶端负载质量kgl摆杆长度mu施加在小车上的水平力Nx小车水平位置坐标mθ摆杆相对竖直方向的偏角radg重力加速度m/s²1.2 拉格朗日方程推导利用拉格朗日方程Lagranges equations可得到系统的完整非线性动力学方程(M m)ẍ - mlθ̈cosθ mlθ̇²sinθ u lθ̈ - g·sinθ ẍ·cosθ其中第一式为小车平动方向的力平衡第二式描述摆杆绕转轴的转动。经过整理可以得到两个广义加速度的显式表达式ẍ [m(g·cosθ - θ̇²·l)·sinθ u] / (M m - m·cos²θ) θ̈ [g(M m)·sinθ - θ̇²·l·m·sinθ·cosθ u·cosθ] / (l·(M m - m·cos²θ))注意式中的非线性耦合项sinθ、cosθ以及角速度平方项θ̇²它们使得系统无法直接使用线性控制理论求解。二、线性化与状态空间模型2.1 小角度线性化在倒立摆竖直平衡点附近θ很小采用近似cosθ ≈ 1, sinθ ≈ θ, θ̇² ≈ 0代入非线性方程后得到线性化模型ẍ (g·m/M)·θ (1/M)·u θ̈ g(M m)/(M·l)·θ 1/(M·l)·u2.2 状态空间表示选取状态向量x [x, ẋ, θ, θ̇]ᵀ可将系统写成标准状态空间形式ẋ A·x B·u y C·x D·u其中A | 0 1 0 0 | | 0 0 g·m/M 0 | | 0 0 0 1 | | 0 0 g(Mm)/(M·l) 0 | B | 0 | |1/M | | 0 | |1/(M·l) |若只控制摆角θ则输出矩阵为C | 0 0 1 0 |, D [0]若同时控制小车位置x与摆角θ则C | 1 0 0 0 | | 0 0 1 0 |, D | 0 | | 0 |2.3 源码中的离散化实现仓库源码在 inverted_pendulum_lqr_control.py 的get_model_matrix()函数中实现了连续矩阵A、B的一阶欧拉离散化时间步长delta_t 0.1sA np.eye(nx) delta_t * A # 离散化A_d I Δt·A B delta_t * B # 离散化B_d Δt·B也就是说两个控制器LQR 与 MPC实际使用的都是离散时间模型x[k1] A·x[k] B·u[k]与文档中 DARE离散代数 Riccati 方程的求解方式完全对应。MPC 实现 inverted_pendulum_mpc_control.py 中复用了完全相同的get_model_matrix()函数两套控制器共享同一套模型参数便于公平对比。三、LQR 控制器最优状态反馈3.1 控制原理LQRLinear Quadratic Regulator线性二次型调节器通过最小化如下二次型代价函数来设计状态反馈增益J xᵀ·Q·x uᵀ·R·u其中Q为状态加权矩阵R为控制输入加权矩阵。使代价函数最小化的反馈控制律为u -K·x反馈增益矩阵K (Bᵀ·P·B R)⁻¹·Bᵀ·P·A其中P是离散时间代数 Riccati 方程DARE的唯一正定解P Aᵀ·P·A - Aᵀ·P·B·(R Bᵀ·P·B)⁻¹·Bᵀ·P·A Q3.2 源码实现细节LQR 实现位于 inverted_pendulum_lqr_control.py核心流程分为三步solve_DARE(A, B, Q, R)第 72-85 行采用不动点迭代法求解 Riccati 方程初始值P Q最大迭代maxiter150收敛阈值eps0.01当max(|Pₙ - P|) eps时提前终止dlqr(A, B, Q, R)第 88-103 行基于 DARE 解计算增益K并通过对闭环矩阵A - B·K求特征值eigVals来验证闭环稳定性所有特征值落在单位圆内则系统稳定lqr_control(x)第 106-113 行在每个控制周期实时求解dlqr并计算u -K·x同时打印单次计算耗时。模型默认参数如下l_bar 2.0 # 摆杆长度 l [m] M 1.0 # 小车质量 [kg] m 0.3 # 摆杆顶端质量 [kg] g 9.8 # 重力加速度 [m/s²] nx 4 # 状态数 nu 1 # 输入数 Q np.diag([0.0, 1.0, 1.0, 0.0]) # 状态代价矩阵仅对角速度与摆角加权 R np.diag([0.01]) # 输入代价矩阵 delta_t 0.1 # 仿真时间步长 [s] sim_time 5.0 # 总仿真时长 [s]从Q的设置可以看出θ̇摆角速度与θ摆角被加权权重 1.0而位置x与其速度ẋ权重为 0即该配置下控制器只关心摆角平衡不约束小车位移摆杆平衡后小车会自由漂移——这正是文档中只控制 θ情形的工程体现。3.3 运行方式# 在仓库根目录下执行 python InvertedPendulum/inverted_pendulum_lqr_control.py初始状态为x0 [0.0, 0.0, 0.3, 0.0]ᵀ摆角初始偏移 0.3 rad ≈ 17.2°。运行结束后终端会打印最终状态Finish x... [m] , theta... [deg]默认show_animation True会弹出 matplotlib 动画窗口实时绘制小车与摆杆的运动按Esc键可随时终止仿真见 plot_cart 函数。也可在代码中或通过测试将其置为False以无头模式运行。四、MPC 控制器滚动时域优化4.1 控制原理MPCModel Predictive Control模型预测控制在每个控制周期内基于当前状态求解一个有限时域的带约束最优控制问题并只施加第一个最优控制量然后滚动推进。其代价函数与 LQR 形式相同J xᵀ·Q·x uᵀ·R·u但受限于两个约束条件线性化倒立摆模型x[k1] A·x[k] B·u[k]初始状态x[0] x₀。4.2 源码实现细节MPC 实现位于 inverted_pendulum_mpc_control.py依赖cvxpy凸优化库核心函数mpc_control(x0)第 77-108 行的构造过程x cvxpy.Variable((nx, T 1)) # 状态变量4 维 × 31 步 u cvxpy.Variable((nu, T)) # 输入变量1 维 × 30 步在预测时域T 30对应 30 × 0.1s 3s 的预测窗口内逐时刻累加代价cost cvxpy.quad_form(x[:, t 1], Q) # 状态代价 cost cvxpy.quad_form(u[:, t], R) # 输入代价 constr [x[:, t 1] A x[:, t] B u[:, t]] # 模型约束 constr [x[:, 0] x0[:, 0]] # 初始状态约束随后构建并求解优化问题prob cvxpy.Problem(cvxpy.Minimize(cost), constr) prob.solve(verboseFalse, solvercvxpy.CLARABEL)求解器选用CLARABELcvxpy 内置的开源内点法求解器若求解状态为cvxpy.OPTIMAL则返回整条最优轨迹位置、速度、摆角、摆角速度、输入序列主循环只取第一个控制量u opt_input[0]施加给系统若求解失败各返回值置为None。与 LQR 版本相比MPC 版本额外引入了预测时域参数T 30 # 预测时域长度Horizon length这是两套控制器最本质的区别LQR 通过一次离线/在线求解 Riccati 方程得到全局反馈增益而 MPC 则在每个时间步在线求解一次 30 步的凸优化问题天然支持未来状态预测与约束扩展如输入饱和、状态边界代价是更高的单步计算开销。4.3 运行方式# 在仓库根目录下执行 python InvertedPendulum/inverted_pendulum_mpc_control.py运行前提环境中需安装cvxpy含 CLARABEL 求解器。项目 requirements/requirements.txt 中锁定的版本为cvxpy 1.8.1同时依赖ecos 2.0.14可通过pip install -r requirements/requirements.txt一键安装。MPC 版的仿真时长、步长、初始状态与 LQR 版完全一致便于直接对比两种控制器在相同工况下的表现与单步计算耗时源码中均通过time.time()计时并打印。五、测试验证仓库为两个控制器均配备了自动化测试tests/test_inverted_pendulum_lqr_control.py测试中先将m.show_animation False关闭动画再调用m.main()执行完整 5 秒仿真验证 LQR 控制流程可正常运行tests/test_inverted_pendulum_mpc_control.py以同样方式验证 MPC 控制流程。执行方式# 在仓库根目录下运行 pytest tests/test_inverted_pendulum_lqr_control.py tests/test_inverted_pendulum_mpc_control.py测试与主程序使用同一套main()入口说明两个脚本在设计上即插即用——既可作为独立演示程序运行也可被测试框架无头化复用。六、总结与工程要点对比维度LQRinverted_pendulum_lqr_control.pyMPCinverted_pendulum_mpc_control.py核心思想求解 DARE 得到全局最优状态反馈增益滚动求解有限时域凸优化问题代价函数J xᵀQx uᵀRu同左逐时刻累加约束处理无显式约束模型约束 初始状态约束可扩展求解依赖NumPy矩阵运算cvxpy CLARABEL关键参数Q、R、delta_tQ、R、delta_t、T预测时域单步计算Riccati 方程迭代求解开销小每步在线求解 QP开销较大工程实践上从本项目源码可以提炼三点可复用的经验模型离散化是关键桥梁LQR 与 MPC 共用同一份get_model_matrix()离散化代码任何模型参数M、m、l的修改都会同时作用于两套控制器保证了对比实验的公平性Q/R的权重语义直接决定控制行为本项目中Q对角速度与摆角加权而位置不加权控制器只维持摆角平衡如需定点平衡同时约束小车位移只需调整Q中对应位置的权重并按文档补充C矩阵动画与无头模式双通道show_animation开关配合Esc键退出机制兼顾了教学演示可视化与自动化测试无头两种场景值得在机器人算法示例代码中推广。如果想进一步探索最优控制理论细节可参考模块文档 docs/modules/10_inverted_pendulum/inverted_pendulum_main.rst其中包含完整的拉格朗日推导、线性化过程与状态空间矩阵与本文源码分析相互印证。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考