从零开始：用Python和Simulink复现经典倒立摆建模与控制（附代码）
从零开始：用Python和Simulink复现经典倒立摆建模与控制（附代码）
倒立摆作为控制理论中的"Hello World"，完美诠释了如何用数学语言描述物理世界的不稳定系统。记得第一次在实验室看到教授演示时，那根倔强的摆杆在电机驱动下反复倒下又立起，像极了初学者跌跌撞撞的建模过程。本文将用两种工程师最熟悉的工具链——Python科学计算栈和MATLAB/Simulink，带你完整实现从微分方程推导到控制器调参的全流程。不同于教科书上的理论推导，我们会重点关注那些真正影响仿真结果的工程细节：比如为什么线性化时不能忽略科里奥利力？状态观测器噪声过大时该如何调整Q/R矩阵？
1. 物理建模：从牛顿力学到状态方程
在开始写代码前，我们需要建立准确的数学模型。假设我们使用经典的直线型倒立摆装置，包含三个核心组件：带有编码器的直流电机、可在导轨移动的小车、以及顶端配重的轻质摆杆。为简化分析，先做以下理想化假设：
- 摆杆质量集中于顶端配重块（点质量模型）
- 导轨摩擦系数恒定且已知
- 电机响应延迟可忽略不计
关键参数表：
| 符号 | 物理意义 | 典型值 | 单位 |
|---|---|---|---|
| M | 小车质量 | 0.5 | kg |
| m | 摆杆配重质量 | 0.2 | kg |
| l | 摆杆长度 | 0.3 | m |
| b | 小车摩擦系数 | 0.1 | N·s/m |
| g | 重力加速度 | 9.81 | m/s² |
通过受力分析建立非线性方程：
# Python符号计算推导 import sympy as sp x, theta, F = sp.symbols('x theta F') x_dot = sp.diff(x, 't') theta_dot = sp.diff(theta, 't') # 系统动能和势能表达式 T = 0.5*M*x_dot**2 + 0.5*m*((x_dot + l*theta_dot*sp.cos(theta))**2 + (l*theta_dot*sp.sin(theta))**2) V = m*g*l*sp.cos(theta) # 拉格朗日方程推导 L = T - V eq1 = sp.diff(sp.diff(L, x_dot), 't') - sp.diff(L, x) + b*x_dot - F eq2 = sp.diff(sp.diff(L, theta_dot), 't') - sp.diff(L, theta)在平衡点θ=0附近线性化后，得到状态空间标准形式：
ẋ = Ax + Bu y = Cx + Du其中状态变量x=[位置, 角度, 速度, 角速度]，输入u为电机施加的力F。
2. Python实现：从建模到控制器设计
2.1 使用Control库构建系统模型
import numpy as np import control as ct # 系统参数（与上表对应） M, m, l, b, g = 0.5, 0.2, 0.3, 0.1, 9.81 # 状态空间矩阵 A = np.array([ [0, 0, 1, 0], [0, 0, 0, 1], [0, -m*g/M, -b/M, 0], [0, (M+m)*g/(M*l), b/(M*l), 0] ]) B = np.array([[0], [0], [1/M], [-1/(M*l)]]) C = np.eye(4) D = np.zeros((4,1)) sys = ct.ss(A, B, C, D)2.2 PID控制器调参实战
对于角度控制回路，建议采用串级PID结构：
- 内环先整定角速度PD控制器
- 外环再整定角度P控制器
- 最后加入积分项消除稳态误差
# 角速度环（内环） Kp_vel = 0.15 Kd_vel = 0.05 vel_ctrl = ct.tf([Kd_vel, Kp_vel], [1, 0]) # 角度环（外环） Kp_ang = 8.0 Ki_ang = 0.5 ang_ctrl = ct.tf([Kp_ang, Ki_ang], [1, 0]) # 组合成串级控制器 pid_sys = ct.series(ang_ctrl, vel_ctrl)调试技巧：先增大P直到出现等幅振荡，此时记录临界增益Ku和振荡周期Tu。根据Ziegler-Nichols法则，P=0.6Ku, I=0.5Tu, D=0.125Tu
2.3 状态反馈与LQR控制
相比试错法的PID调参，LQR提供系统化的最优控制方案：
Q = np.diag([1, 10, 0.1, 0.5]) # 状态权重矩阵 R = 0.1 # 输入权重 K, S, E = ct.lqr(A, B, Q, R) closed_loop = ct.ss(A-B@K, B, C, D)3. Simulink建模技巧与实时调试
在Simulink中搭建模型时，推荐采用模块化设计：
- 物理建模层：使用Simscape Multibody精确构建刚体动力学
- 控制算法层：用MATLAB Function块实现自定义控制律
- 可视化层：添加Scope和Animation Viewer实时监控
常见问题排查表：
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 小车持续加速冲出轨道 | 积分饱和 | 增加抗饱和环节或限制积分项 |
| 摆杆高频抖动 | 微分增益过大或噪声放大 | 添加低通滤波器 |
| 响应迟缓 | 控制器增益不足 | 逐步提高P增益直至临界状态 |
| 稳态误差不收敛 | 缺少积分项或执行器死区 | 检查电机最小启动电压 |
4. 进阶话题：从仿真到实物部署
当仿真结果满意后，还需考虑：
- 离散化影响：采样周期应小于系统最小时间常数的1/10
dt = 0.01 # 采样周期 discrete_sys = ct.c2d(sys, dt, 'zoh')- 状态观测器设计：当无法直接测量所有状态时
# 设计卡尔曼滤波器 Q_noise = np.diag([0.1, 0.1]) # 过程噪声协方差 R_noise = np.diag([0.5, 0.5]) # 观测噪声协方差 kf = ct.kalman_filter(sys, Q_noise, R_noise)- 代码生成：将控制算法部署到STM32等嵌入式平台
% MATLAB Coder配置 cfg = coder.config('lib'); cfg.TargetLang = 'C'; codegen('control_algorithm.m', '-config', cfg)记得第一次成功让实物倒立摆稳定时，电机发出的嗡嗡声仿佛变成了胜利的号角。这种将数学方程转化为物理运动的奇妙体验，正是控制工程最迷人的地方。建议尝试故意给摆杆一个扰动，观察控制器如何应对——优秀的控制策略应该像经验丰富的骑手，能迅速化解突如其来的失衡。