企业官网建设流程全解析

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，使倒立摆保持竖直平衡。这是一个典型的非线性、开环不稳定的欠驱动系统，也是验证线性控制与最优控制理论的经典实验平台。

模型涉及的主要物理量如下：

1.2 拉格朗日方程推导

利用拉格朗日方程（Lagrange's 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)·u

2.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(M+m)/(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.1s）：

A = np.eye(nx) + delta_t * A # 离散化：A_d = I + Δt·A B = delta_t * B # 离散化：B_d = Δt·B

也就是说，两个控制器（LQR 与 MPC）实际使用的都是离散时间模型x[k+1] = A·x[k] + B·u[k]，与文档中 DARE（离散代数 Riccati 方程）的求解方式完全对应。MPC 实现 inverted_pendulum_mpc_control.py 中复用了完全相同的get_model_matrix()函数，两套控制器共享同一套模型参数，便于公平对比。

三、LQR 控制器：最优状态反馈

3.1 控制原理

LQR（Linear 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 + Q

3.2 源码实现细节

LQR 实现位于 inverted_pendulum_lqr_control.py，核心流程分为三步：

1. solve_DARE(A, B, Q, R)（第 72-85 行）：采用不动点迭代法求解 Riccati 方程，初始值P = Q，最大迭代maxiter=150，收敛阈值eps=0.01，当max(|Pₙ - P|) < eps时提前终止；
2. dlqr(A, B, Q, R)（第 88-103 行）：基于 DARE 解计算增益K，并通过对闭环矩阵A - B·K求特征值eigVals来验证闭环稳定性（所有特征值落在单位圆内则系统稳定）；
3. 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 控制原理

MPC（Model Predictive Control，模型预测控制）在每个控制周期内，基于当前状态求解一个有限时域的带约束最优控制问题，并只施加第一个最优控制量，然后滚动推进。其代价函数与 LQR 形式相同：

J = xᵀ·Q·x + uᵀ·R·u

但受限于两个约束条件：

• 线性化倒立摆模型：x[k+1] = 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(verbose=False, solver=cvxpy.CLARABEL)

• 求解器选用CLARABEL（cvxpy 内置的开源内点法求解器）；
• 若求解状态为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()入口，说明两个脚本在设计上"即插即用"——既可作为独立演示程序运行，也可被测试框架无头化复用。

六、总结与工程要点

工程实践上，从本项目源码可以提炼三点可复用的经验：

1. 模型离散化是关键桥梁：LQR 与 MPC 共用同一份get_model_matrix()离散化代码，任何模型参数（M、m、l）的修改都会同时作用于两套控制器，保证了对比实验的公平性；
2. Q/R的权重语义直接决定控制行为：本项目中Q对角速度与摆角加权而位置不加权，控制器只维持摆角平衡；如需"定点平衡"（同时约束小车位移），只需调整Q中对应位置的权重并按文档补充C矩阵；
3. 动画与无头模式双通道：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