简介这是一份面向自动控制、机电一体化及机器人方向学习者的倒立摆系统建模文档核心内容是拉格朗日方程在直线一级倒立摆建模中的完整应用。文档首先介绍倒立摆闭环系统的组成包括计算机、数据采集卡、电源与功率放大器、直流伺服电机、倒立摆本体和光电编码器等模块随后给出小车质量、摆杆质量、摆杆长度、质心位置与重力加速度等关键参数并利用动能、势能和耗散能构造Lagrange方程逐步推导系统的动力学模型。压缩包中仅包含1个doc文件大小约838KB目录脉络清晰适合作为课程设计、毕业设计或控制理论入门的参考资料。内容还覆盖了稳定性分析、频率响应与阶跃响应等性能分析方法并对PID控制、状态空间控制等常用控制思路作了方向性说明有助于读者从建模到分析形成整体认识。目前已有290人学习下载。1. 倒立摆建模与拉格朗日方程先把数学写对再谈把杆立住听到“倒立摆”这三个字控制工程师的第一反应往往是 PID、LQR、卡尔曼滤波但一套能支撑这些控制器的数学模型才是整个系统的地基。标题里的“拉格朗日方程”之所以成为倒立摆建模的首选是因为它把约束力从方程里消掉了只要选好广义坐标能量一算动力学方程就机械地出来了根本不用去拆杆子内部的反力。下面按五个部分展开完整推导、平衡点线性化、状态空间表达、用 SymPy 和 SciPy 做仿真验证以及把模型交给 LQR 控制器和飞控场景的方法。正在做数学建模竞赛或者刚接手自平衡机器人项目的工程师都能在这里找到可以直接抄走的步骤。2. 用拉格朗日方程推导倒立摆动力学模型的完整步骤2.1 从牛顿力学到拉格朗日方程为什么建模首选是能量如果有人用牛顿第二定律来建倒立摆的模型最常见的结果是小车和摆杆之间的相互作用力反复在方程里出现必须联立消元稍有不慎就把某个约束反力的符号写反。而拉格朗日力学的观点是系统运动只取决于它的动能 T 和势能 V外力比如小车的驱动力 F、铰链处的粘滞摩擦作为广义力单独加入方程。约束力根本不进入 T 和 V所以最后得到的方程数量少、结构清晰也不容易出符号错误。拉格朗日方程写作$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i}\right) - \frac{\partial L}{\partial q_i} Q_i $$其中 $L T - V$ 是拉格朗日量$q_i$ 是广义坐标。对倒立摆这种“单摆体加小车”的系统只需要描述两个广义坐标就能完全确定系统状态小车水平位移 $x$以及摆杆相对竖直方向的夹角 $\theta$。2.2 广义坐标与物理参数的定义先明确建模对象小车在水平轨道上运动摆杆通过铰链安装在小车上杆体为匀质细杆质心位于杆长一半处。这里用到的全部符号和单位如下符号物理含义单位$M$小车质量kg$m$摆杆质量kg$l$摆杆长度m$J_c$摆杆绕质心的转动惯量kg·m²$x$小车相对原点的水平位移m$\theta$摆杆相对竖直方向的夹角rad$F$作用在小车上的水平外力N$b$摆杆铰链处的粘滞摩擦系数N·m·s这里要特别注意约定如果把 $\theta 0$ 定义为摆杆竖直向上那么整个系统的平衡点就在 $\theta 0$ 附近控制目标就是让 $\theta$ 收敛到 0。这个约定与很多教材把竖直向下定义为 0 的做法不同会导致后面线性化时正负号翻车建议一开始就固定下来。注意所有角度一律使用弧度制。微分方程、状态空间矩阵和控制器反馈增益的全部计算都基于弧度角度单位错位是这类模型中最隐蔽的错误之一。2.3 动能、势能与拉格朗日量的完整推导摆杆质心的坐标由小车位移和摆杆角度共同决定$$ x_c x \frac{l}{2}\sin\theta, \quad y_c \frac{l}{2}\cos\theta $$对时间求导得到质心速度分量$$ \dot{x}_c \dot{x} \frac{l}{2}\dot{\theta}\cos\theta, \quad \dot{y}_c -\frac{l}{2}\dot{\theta}\sin\theta $$系统的动能分为三部分小车的平动动能、摆杆质心的平动动能、摆杆绕质心的转动动能。合并后得到$$ T \frac{1}{2}M\dot{x}^2 \frac{1}{2}m\left(\dot{x}_c^2 \dot{y}_c^2\right) \frac{1}{2}J_c\dot{\theta}^2 $$把质心速度代进去并使用匀质细杆绕质心的转动惯量 $J_c \frac{1}{12}ml^2$整理后得到$$ T \frac{1}{2}(Mm)\dot{x}^2 \frac{1}{2}ml\dot{x}\dot{\theta}\cos\theta \frac{1}{6}ml^2\dot{\theta}^2 $$注意最后一项的系数平动部分贡献了 $\frac{1}{8}ml^2$转动部分贡献了 $\frac{1}{24}ml^2$相加后恰好是 $\frac{1}{6}ml^2$等价于摆杆对端点转轴的转动惯量 $\frac{1}{3}ml^2$ 的一半。很多初学者在这里把 $l$ 和 $l/2$ 混用最后的方程数值结果自然对不上。势能只来自摆杆质心高度$$ V mg\frac{l}{2}\cos\theta $$于是拉格朗日量 $L T - V$ 就明确了。这套推导的关键在于杆长的处理势能里用的是质心到转轴的距离 $l/2$动能里平动项用的也是 $l/2$但转动惯量那一项必须折算到转轴。2.4 代入拉格朗日方程得到系统的非线性微分方程组系统有外力 $F$ 作用在 $x$ 方向有粘滞摩擦力矩 $-b\dot{\theta}$ 作用在 $\theta$ 方向。分别对广义坐标 $x$、$\theta$ 写出拉格朗日方程整理成矩阵形式$$ \begin{bmatrix} Mm \frac{ml}{2}\cos\theta \ \frac{ml}{2}\cos\theta \frac{1}{3}ml^2 \end{bmatrix} \begin{bmatrix} \ddot{x} \ \ddot{\theta} \end{bmatrix} \begin{bmatrix} F \frac{ml}{2}\dot{\theta}^2\sin\theta \ \frac{mgl}{2}\sin\theta - b\dot{\theta} \end{bmatrix} $$这个矩阵形式是后续所有工作的核心左边是广义质量矩阵右边是外力和速度耦合项。方程组是非线性的因为矩阵里含有 $\cos\theta$右侧含有 $\sin\theta$ 和 $\dot{\theta}^2$。想要避免手算出错可以用 SymPy 把符号推导流程固定下来import sympy as sp t, M, m, l, g, b sp.symbols(t M m l g b) x sp.Function(x)(t) theta sp.Function(theta)(t) x_dot sp.diff(x, t) theta_dot sp.diff(theta, t) # 摆杆质心坐标与速度 xc x l/2 * sp.sin(theta) yc l/2 * sp.cos(theta) xc_dot sp.diff(xc, t) yc_dot sp.diff(yc, t) Jc m * l**2 / 12 T sp.Rational(1, 2) * M * x_dot**2 sp.Rational(1, 2) * m * (xc_dot**2 yc_dot**2) sp.Rational(1, 2) * Jc * theta_dot**2 V m * g * l/2 * sp.cos(theta) L sp.simplify(T - V) print(sp.simplify(L))这段代码把推导 $L$ 的过程变成可复核的步骤。sp.Rational(1, 2)保证系数以精确分数参与符号运算避免浮点误差sp.diff自动完成对时间的求导。得到 $L$ 后再对 $\dot{x}$、$\dot{\theta}$ 分别求偏导并代入拉格朗日方程就能机械地得到上面的矩阵方程整个过程可以在 Jupyter Notebook 里逐步复查。3. 让倒立摆模型可被控制器使用在平衡点做线性化与状态空间化3.1 为什么要做线性化非线性模型与控制器之间的鸿沟上一章得到的非线性矩阵方程可以直接用于数值仿真但用到控制器设计上就麻烦了。PID 设计依赖线性系统和传递函数LQR 和卡尔曼滤波需要状态空间矩阵 $A$、$B$。非线性模型虽然可以放进仿真器直接跑但从设计控制器的角度先在平衡点附近做线性化是标准操作。倒立摆的实际运行状态是控制器把摆杆维持在竖直向上附近$\theta$ 一直在一个很小的角度范围内摆动。因此可以用小角度假设把非线性项替换成线性项。这套做法在处理自平衡机器人、四旋翼的姿态控制器时也同样适用数学建模竞赛里也要求把这些近似条件写清楚评审会关注你是在哪个平衡点、什么范围内做的线性化。3.2 小角度近似快速得到能用的线性方程在 $\theta 0$ 附近做一阶近似$\cos\theta \approx 1$$\sin\theta \approx \theta$并直接把 $\dot{\theta}^2\sin\theta$ 这类二阶小量丢弃。于是矩阵方程化简为$$ (Mm)\ddot{x} \frac{ml}{2}\ddot{\theta} F $$$$ \frac{ml}{2}\ddot{x} \frac{1}{3}ml^2\ddot{\theta} \frac{mgl}{2}\theta - b\dot{\theta} $$消去 $\ddot{\theta}$ 和 $\ddot{x}$可以得到两个解耦后的二阶方程。这里需要注意一个常见误用有人把“竖直向上”写成 $\theta \pi$然后做近似 $\sin(\pi \delta) \approx -\delta$虽然物理上正确但符号上很容易出错。我更推荐一开始就定义 $\theta 0$ 为平衡点让后续所有矩阵元素都无需处理符号翻转。3.3 用雅可比矩阵得到状态空间矩阵 A 和 B如果系统中摆角波动较大或者想要一个不依赖手工小角度近似的线性化结果可以用符号雅可比矩阵。令状态变量为 $s [x,\ \dot{x},\ \theta,\ \dot{\theta}]^T$非线性动力学写成 $\dot{s} f(s, F)$然后在平衡点 $s_0 [0,0,0,0]^T$ 处计算雅可比import sympy as sp x, xd, theta, theta_dot, F sp.symbols(x xd theta theta_dot F) M, m, l, g, b sp.symbols(M m l g b) J m * l**2 / 3 # 从矩阵方程反解出二阶导M_acc * [xdd; thetadd] rhs M_acc sp.Matrix([[M m, m*l/2*sp.cos(theta)], [m*l/2*sp.cos(theta), J]]) rhs sp.Matrix([F m*l/2*theta_dot**2*sp.sin(theta), m*g*l/2*sp.sin(theta) - b*theta_dot]) acc sp.simplify(M_acc.inv() * rhs) f sp.Matrix([xd, acc[0], theta_dot, acc[1]]) state sp.Matrix([x, xd, theta, theta_dot]) A f.jacobian(state).subs({x: 0, xd: 0, theta: 0, theta_dot: 0}) B f.diff(F).subs({x: 0, xd: 0, theta: 0, theta_dot: 0}) sp.print_latex(sp.simplify(A)) sp.print_latex(sp.simplify(B))代码的逻辑是先用质量矩阵求逆从代数方程里解出 $\ddot{x}$ 和 $\ddot{\theta}$组成完整的 $f$再对状态向量求雅可比得到 $A$对输入 $F$ 求偏导得到 $B$最后代入平衡点数值。这样得到的 $A$ 、$B$ 是$$ A \begin{bmatrix} 0 1 0 0 \ 0 0 -\frac{mg}{M} 0 \ 0 0 0 1 \ 0 0 \frac{(Mm)g}{Ml} 0 \end{bmatrix}, \quad B \begin{bmatrix} 0 \ \frac{1}{M} \ 0 \ -\frac{1}{Ml} \end{bmatrix} $$值得强调的是$\theta$ 和 $\dot{\theta}$ 对 $x$、$\dot{x}$ 没有直接耦合但小车的加速度项依赖于 $\theta$这意味着小车位移完全是通过摆角间接控制的。直观来说就是“要想让小车动必须先让杆动”。这个结构是之后设计 LQR 权重矩阵时重点考虑的地方。3.4 给模型配一套可复现的物理参数为了后面仿真和控制器调参我常用下面这套模拟实验台参数参数数值说明M1.0 kg小车质量m0.1 kg摆杆质量l0.5 m摆杆长度g9.81 m/s²重力加速度b0.1 N·m·s铰链粘滞摩擦系数把参数代入 $A$ 矩阵后$A[3][2] (Mm)g / (M l) \approx 21.58$但这里最终的仿真步长和 LQR 权重都受这个量级影响。这个数字决定了系统的自然频率和不稳定极点位置也决定了控制器反馈增益的量级换一套参数时应该先重新算一次 $A$ 矩阵而不是照抄原来的增益。4. 用 SymPy SciPy 把倒立摆模型跑起来非线性仿真与模型校验4.1 最小可复现代码开环仿真的 Python 脚本前面推导的是符号方程这一节把它变成能跑的数值模型。开环仿真的目的是看系统在没有任何控制力时的运动摆杆从偏离平衡点的初始角度释放如果不加控制它会倒下并来回摆动。这里取初始角度 $\theta 0.1$ rad对应偏离竖直约 5.7°。import numpy as np from scipy.integrate import solve_ivp import matplotlib.pyplot as plt M, m, l, g, b 1.0, 0.1, 0.5, 9.81, 0.1 J m * l**2 / 3 def pendulum_dynamics(t, s, F): x, xd, theta, theta_dot s c np.cos(theta) s_th np.sin(theta) M_mat np.array([[M m, 0.5 * m * l * c], [0.5 * m * l * c, J]]) rhs np.array([F 0.5 * m * l * theta_dot**2 * s_th, 0.5 * m * g * l * s_th - b * theta_dot]) xdd, thetadd np.linalg.solve(M_mat, rhs) return [xd, xdd, theta_dot, thetadd] s0 [0.0, 0.0, 0.1, 0.0] t_eval np.linspace(0, 3, 300) sol solve_ivp(pendulum_dynamics, [0, 3], s0, t_evalt_eval, args(0.0,), rtol1e-8) plt.plot(sol.t, np.rad2deg(sol.y[2])) plt.xlabel(t [s]) plt.ylabel(theta [deg]) plt.title(Open-loop release of inverted pendulum) plt.grid(True) plt.show()pendulum_dynamics返回四个状态变量的导数。rhs中第一项对应外力 $F$ 和向心耦合项第二项对应重力项和阻尼项。np.linalg.solve求解的是广义质量矩阵线性方程组这样能保留完整的非线性信息比手写解耦公式更不容易出错。rtol1e-8是一个关键参数倒立摆对积分误差极其敏感默认的容差可能导致轨迹在中途发散。如果发现仿真曲线出现“假发散”现象先检查求解器容差而不是先怀疑模型。4.2 模型校验的 3 个标准做法开环仿真跑出来的摆角曲线看起来会像单摆一样摆动但这不能说明模型正确。我一般按下面三个顺序做校验校验方法预期结果失败时可能原因能量守恒机械能 $E T V$ 在无外力时保持恒定积分容差不够或初始条件设置错误退化测试$m \to 0$ 时退化为纯小车模型$M \to \infty$ 时退化为单摆模型非线性耦合项在代码中没消干净极点比对线性模型 $A$ 的特征值包含一个正实部极点平衡点约定不一致符号写反第一个方法可以直接在仿真循环里每隔 0.01 秒记录一次 $E$如果能量漂移超过 1%说明容差需要收紧。第二个方法把质量矩阵的对角项替换为极限值观察是否复现标准方程。第三个方法对 $A$ 求特征值虚部和开环仿真的摆动频率应该吻合如果一个正实部极点都没有说明 $A$ 矩阵符号错了问题通常出在 $\theta$ 方向的定义上。4.3 常见参数坑转动惯量、单位与小角度假设第一个坑是转动惯量的位置。如果建模时把摆杆质量集中到质心转动惯量不能用 $\frac{1}{3}ml^2$ 而要用 $\frac{1}{12}ml^2$如果建模时把转动惯量折算到端点那平动动能里的系数也会变化。最稳妥的做法是从一开始就按质心惯性矩加平行轴定理统一写代码里的J和推导时的Jc要保持一致。第二个坑是角度单位前面已经强调过所有动力学方程必须使用弧度制。PID 控制器里如果把角度反馈量用度状态空间矩阵里的 $g$ 和 $l$ 组合出来是正确定纲但一旦把度代入仿真结果会完全乱套。第三个坑是小角度假设的成立范围。线性模型大约在 $\theta 0.35$ rad 即 20° 以内才足够精确。如果实际系统里摆角经常超过这个范围线性模型只能作为参考控制器需要改用非线性鲁棒方法LQR 的权重矩阵也要重新标定。5. 让模型走向应用LQR 控制器、飞控场景与模型时效性验证5.1 在仿真里给系统接上 LQR 控制器拿到线性模型后我一般会先用 LQR 验证一下模型动力学是否合理。LQR 会解出使代价函数 $J \int (s^T Q s F^T R F) dt$ 最小的反馈增益 $K$。这个步骤只需要几行代码但 $Q$ 和 $R$ 的选取决定了控制效果增加对应 $\theta$ 的权重会让摆杆更快直立增加对应 $x$ 的权重会让小车更快回到原点减小 $R$ 会让执行器输出更强。from scipy.linalg import solve_continuous_are A np.array([[0, 1, 0, 0], [0, 0, -m * g / M, 0], [0, 0, 0, 1], [0, 0, (M m) * g / (M * l), 0]]) B np.array([[0], [1 / M], [0], [-1 / (M * l)]]) Q np.diag([10.0, 1.0, 100.0, 1.0]) R np.array([[1.0]]) P solve_continuous_are(A, B, Q, R) K np.linalg.inv(R) B.T P print(K.flatten())反馈增益 $K$ 是 1×4 行向量控制律是 $F -K s$。求解结果中对应 $\theta$ 的第三个分量通常远大于其他项配合 $B$ 的符号控制力的正负正好给出“向摆杆倾斜一侧推动小车”的直观逻辑这也反向印证了 $A$ 矩阵方向约定的正确性。把 $K$ 代回非线性仿真模型摆角能在约 1 秒内收敛到零。这里的Q和R可以先用对角线矩阵试跑再根据超调量和稳态误差迭代微调。5.2 从单摆到飞控把拉格朗日建模方法复用到多体系统拉格朗日法最值钱的不是这一次推导而是“选广义坐标、算能量、代入方程”这条流水线可以复制。做两轮自平衡机器人时可以把车身倾角、左右轮位移都写成广义坐标再补上轮子和底盘的动能项做飞控里的姿态解算时把欧拉角或四元数作为广义坐标旋翼推力作为广义力用同一套方法可以推导出俯仰和横滚轴的动力学方程。很多飞控团队在写姿态控制器之前都会先搭这套模型来检验控制律。数学建模竞赛里这类题目也很常见全国大学生数学建模竞赛和研究生数学建模竞赛的控制类题目给的往往不是现成动力学方程而是需要先用工科方法把系统的数学模型整理出来。模型的推导过程本身就是评审的重要观察点符号表、坐标定义、能量函数书写规范这些都比最后的仿真图更能决定论文档次。5.3 一个容易忽略的验证技巧10 分钟完成模型可信度检测最后推荐一个我在交付模型前必做的检测把摆杆质量 $m$ 设为 0代入矩阵方程系统应当退化为纯小车模型 $\ddot{x} F / M$。如果此时仿真曲线出现不该有的振荡说明非线性耦合项在推导或代码实现时没有完全消掉问题出在质量矩阵或右侧力项的某一行。再做一个逆检测把小车的质量设为极大值模型应该退化为绕固定轴转动的单摆方程重力项和阻尼项应与标准单摆完全一致。这两步检测耗时不到 10 分钟可以在方法复用到双连杆摆、平衡车、飞控场景时随时验证。不少团队把符号推导和数值仿真放在两个文件里修改参数后忘记同步更新状态空间矩阵退化测试能直观地把这种不一致暴露出来。在 SymPy 里sp.simplify(A - expected_A)可以直接打印两个矩阵的逐项偏差哪一行符号反了、哪一项漏了系数一目了然不用靠肉眼对着一大坨矩阵元素找差异。本文还有配套的精品资源点击获取 倒立摆建模与拉格朗日方程：从动力学推导到LQR控制
简介这是一份面向自动控制、机电一体化及机器人方向学习者的倒立摆系统建模文档核心内容是拉格朗日方程在直线一级倒立摆建模中的完整应用。文档首先介绍倒立摆闭环系统的组成包括计算机、数据采集卡、电源与功率放大器、直流伺服电机、倒立摆本体和光电编码器等模块随后给出小车质量、摆杆质量、摆杆长度、质心位置与重力加速度等关键参数并利用动能、势能和耗散能构造Lagrange方程逐步推导系统的动力学模型。压缩包中仅包含1个doc文件大小约838KB目录脉络清晰适合作为课程设计、毕业设计或控制理论入门的参考资料。内容还覆盖了稳定性分析、频率响应与阶跃响应等性能分析方法并对PID控制、状态空间控制等常用控制思路作了方向性说明有助于读者从建模到分析形成整体认识。目前已有290人学习下载。1. 倒立摆建模与拉格朗日方程先把数学写对再谈把杆立住听到“倒立摆”这三个字控制工程师的第一反应往往是 PID、LQR、卡尔曼滤波但一套能支撑这些控制器的数学模型才是整个系统的地基。标题里的“拉格朗日方程”之所以成为倒立摆建模的首选是因为它把约束力从方程里消掉了只要选好广义坐标能量一算动力学方程就机械地出来了根本不用去拆杆子内部的反力。下面按五个部分展开完整推导、平衡点线性化、状态空间表达、用 SymPy 和 SciPy 做仿真验证以及把模型交给 LQR 控制器和飞控场景的方法。正在做数学建模竞赛或者刚接手自平衡机器人项目的工程师都能在这里找到可以直接抄走的步骤。2. 用拉格朗日方程推导倒立摆动力学模型的完整步骤2.1 从牛顿力学到拉格朗日方程为什么建模首选是能量如果有人用牛顿第二定律来建倒立摆的模型最常见的结果是小车和摆杆之间的相互作用力反复在方程里出现必须联立消元稍有不慎就把某个约束反力的符号写反。而拉格朗日力学的观点是系统运动只取决于它的动能 T 和势能 V外力比如小车的驱动力 F、铰链处的粘滞摩擦作为广义力单独加入方程。约束力根本不进入 T 和 V所以最后得到的方程数量少、结构清晰也不容易出符号错误。拉格朗日方程写作$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i}\right) - \frac{\partial L}{\partial q_i} Q_i $$其中 $L T - V$ 是拉格朗日量$q_i$ 是广义坐标。对倒立摆这种“单摆体加小车”的系统只需要描述两个广义坐标就能完全确定系统状态小车水平位移 $x$以及摆杆相对竖直方向的夹角 $\theta$。2.2 广义坐标与物理参数的定义先明确建模对象小车在水平轨道上运动摆杆通过铰链安装在小车上杆体为匀质细杆质心位于杆长一半处。这里用到的全部符号和单位如下符号物理含义单位$M$小车质量kg$m$摆杆质量kg$l$摆杆长度m$J_c$摆杆绕质心的转动惯量kg·m²$x$小车相对原点的水平位移m$\theta$摆杆相对竖直方向的夹角rad$F$作用在小车上的水平外力N$b$摆杆铰链处的粘滞摩擦系数N·m·s这里要特别注意约定如果把 $\theta 0$ 定义为摆杆竖直向上那么整个系统的平衡点就在 $\theta 0$ 附近控制目标就是让 $\theta$ 收敛到 0。这个约定与很多教材把竖直向下定义为 0 的做法不同会导致后面线性化时正负号翻车建议一开始就固定下来。注意所有角度一律使用弧度制。微分方程、状态空间矩阵和控制器反馈增益的全部计算都基于弧度角度单位错位是这类模型中最隐蔽的错误之一。2.3 动能、势能与拉格朗日量的完整推导摆杆质心的坐标由小车位移和摆杆角度共同决定$$ x_c x \frac{l}{2}\sin\theta, \quad y_c \frac{l}{2}\cos\theta $$对时间求导得到质心速度分量$$ \dot{x}_c \dot{x} \frac{l}{2}\dot{\theta}\cos\theta, \quad \dot{y}_c -\frac{l}{2}\dot{\theta}\sin\theta $$系统的动能分为三部分小车的平动动能、摆杆质心的平动动能、摆杆绕质心的转动动能。合并后得到$$ T \frac{1}{2}M\dot{x}^2 \frac{1}{2}m\left(\dot{x}_c^2 \dot{y}_c^2\right) \frac{1}{2}J_c\dot{\theta}^2 $$把质心速度代进去并使用匀质细杆绕质心的转动惯量 $J_c \frac{1}{12}ml^2$整理后得到$$ T \frac{1}{2}(Mm)\dot{x}^2 \frac{1}{2}ml\dot{x}\dot{\theta}\cos\theta \frac{1}{6}ml^2\dot{\theta}^2 $$注意最后一项的系数平动部分贡献了 $\frac{1}{8}ml^2$转动部分贡献了 $\frac{1}{24}ml^2$相加后恰好是 $\frac{1}{6}ml^2$等价于摆杆对端点转轴的转动惯量 $\frac{1}{3}ml^2$ 的一半。很多初学者在这里把 $l$ 和 $l/2$ 混用最后的方程数值结果自然对不上。势能只来自摆杆质心高度$$ V mg\frac{l}{2}\cos\theta $$于是拉格朗日量 $L T - V$ 就明确了。这套推导的关键在于杆长的处理势能里用的是质心到转轴的距离 $l/2$动能里平动项用的也是 $l/2$但转动惯量那一项必须折算到转轴。2.4 代入拉格朗日方程得到系统的非线性微分方程组系统有外力 $F$ 作用在 $x$ 方向有粘滞摩擦力矩 $-b\dot{\theta}$ 作用在 $\theta$ 方向。分别对广义坐标 $x$、$\theta$ 写出拉格朗日方程整理成矩阵形式$$ \begin{bmatrix} Mm \frac{ml}{2}\cos\theta \ \frac{ml}{2}\cos\theta \frac{1}{3}ml^2 \end{bmatrix} \begin{bmatrix} \ddot{x} \ \ddot{\theta} \end{bmatrix} \begin{bmatrix} F \frac{ml}{2}\dot{\theta}^2\sin\theta \ \frac{mgl}{2}\sin\theta - b\dot{\theta} \end{bmatrix} $$这个矩阵形式是后续所有工作的核心左边是广义质量矩阵右边是外力和速度耦合项。方程组是非线性的因为矩阵里含有 $\cos\theta$右侧含有 $\sin\theta$ 和 $\dot{\theta}^2$。想要避免手算出错可以用 SymPy 把符号推导流程固定下来import sympy as sp t, M, m, l, g, b sp.symbols(t M m l g b) x sp.Function(x)(t) theta sp.Function(theta)(t) x_dot sp.diff(x, t) theta_dot sp.diff(theta, t) # 摆杆质心坐标与速度 xc x l/2 * sp.sin(theta) yc l/2 * sp.cos(theta) xc_dot sp.diff(xc, t) yc_dot sp.diff(yc, t) Jc m * l**2 / 12 T sp.Rational(1, 2) * M * x_dot**2 sp.Rational(1, 2) * m * (xc_dot**2 yc_dot**2) sp.Rational(1, 2) * Jc * theta_dot**2 V m * g * l/2 * sp.cos(theta) L sp.simplify(T - V) print(sp.simplify(L))这段代码把推导 $L$ 的过程变成可复核的步骤。sp.Rational(1, 2)保证系数以精确分数参与符号运算避免浮点误差sp.diff自动完成对时间的求导。得到 $L$ 后再对 $\dot{x}$、$\dot{\theta}$ 分别求偏导并代入拉格朗日方程就能机械地得到上面的矩阵方程整个过程可以在 Jupyter Notebook 里逐步复查。3. 让倒立摆模型可被控制器使用在平衡点做线性化与状态空间化3.1 为什么要做线性化非线性模型与控制器之间的鸿沟上一章得到的非线性矩阵方程可以直接用于数值仿真但用到控制器设计上就麻烦了。PID 设计依赖线性系统和传递函数LQR 和卡尔曼滤波需要状态空间矩阵 $A$、$B$。非线性模型虽然可以放进仿真器直接跑但从设计控制器的角度先在平衡点附近做线性化是标准操作。倒立摆的实际运行状态是控制器把摆杆维持在竖直向上附近$\theta$ 一直在一个很小的角度范围内摆动。因此可以用小角度假设把非线性项替换成线性项。这套做法在处理自平衡机器人、四旋翼的姿态控制器时也同样适用数学建模竞赛里也要求把这些近似条件写清楚评审会关注你是在哪个平衡点、什么范围内做的线性化。3.2 小角度近似快速得到能用的线性方程在 $\theta 0$ 附近做一阶近似$\cos\theta \approx 1$$\sin\theta \approx \theta$并直接把 $\dot{\theta}^2\sin\theta$ 这类二阶小量丢弃。于是矩阵方程化简为$$ (Mm)\ddot{x} \frac{ml}{2}\ddot{\theta} F $$$$ \frac{ml}{2}\ddot{x} \frac{1}{3}ml^2\ddot{\theta} \frac{mgl}{2}\theta - b\dot{\theta} $$消去 $\ddot{\theta}$ 和 $\ddot{x}$可以得到两个解耦后的二阶方程。这里需要注意一个常见误用有人把“竖直向上”写成 $\theta \pi$然后做近似 $\sin(\pi \delta) \approx -\delta$虽然物理上正确但符号上很容易出错。我更推荐一开始就定义 $\theta 0$ 为平衡点让后续所有矩阵元素都无需处理符号翻转。3.3 用雅可比矩阵得到状态空间矩阵 A 和 B如果系统中摆角波动较大或者想要一个不依赖手工小角度近似的线性化结果可以用符号雅可比矩阵。令状态变量为 $s [x,\ \dot{x},\ \theta,\ \dot{\theta}]^T$非线性动力学写成 $\dot{s} f(s, F)$然后在平衡点 $s_0 [0,0,0,0]^T$ 处计算雅可比import sympy as sp x, xd, theta, theta_dot, F sp.symbols(x xd theta theta_dot F) M, m, l, g, b sp.symbols(M m l g b) J m * l**2 / 3 # 从矩阵方程反解出二阶导M_acc * [xdd; thetadd] rhs M_acc sp.Matrix([[M m, m*l/2*sp.cos(theta)], [m*l/2*sp.cos(theta), J]]) rhs sp.Matrix([F m*l/2*theta_dot**2*sp.sin(theta), m*g*l/2*sp.sin(theta) - b*theta_dot]) acc sp.simplify(M_acc.inv() * rhs) f sp.Matrix([xd, acc[0], theta_dot, acc[1]]) state sp.Matrix([x, xd, theta, theta_dot]) A f.jacobian(state).subs({x: 0, xd: 0, theta: 0, theta_dot: 0}) B f.diff(F).subs({x: 0, xd: 0, theta: 0, theta_dot: 0}) sp.print_latex(sp.simplify(A)) sp.print_latex(sp.simplify(B))代码的逻辑是先用质量矩阵求逆从代数方程里解出 $\ddot{x}$ 和 $\ddot{\theta}$组成完整的 $f$再对状态向量求雅可比得到 $A$对输入 $F$ 求偏导得到 $B$最后代入平衡点数值。这样得到的 $A$ 、$B$ 是$$ A \begin{bmatrix} 0 1 0 0 \ 0 0 -\frac{mg}{M} 0 \ 0 0 0 1 \ 0 0 \frac{(Mm)g}{Ml} 0 \end{bmatrix}, \quad B \begin{bmatrix} 0 \ \frac{1}{M} \ 0 \ -\frac{1}{Ml} \end{bmatrix} $$值得强调的是$\theta$ 和 $\dot{\theta}$ 对 $x$、$\dot{x}$ 没有直接耦合但小车的加速度项依赖于 $\theta$这意味着小车位移完全是通过摆角间接控制的。直观来说就是“要想让小车动必须先让杆动”。这个结构是之后设计 LQR 权重矩阵时重点考虑的地方。3.4 给模型配一套可复现的物理参数为了后面仿真和控制器调参我常用下面这套模拟实验台参数参数数值说明M1.0 kg小车质量m0.1 kg摆杆质量l0.5 m摆杆长度g9.81 m/s²重力加速度b0.1 N·m·s铰链粘滞摩擦系数把参数代入 $A$ 矩阵后$A[3][2] (Mm)g / (M l) \approx 21.58$但这里最终的仿真步长和 LQR 权重都受这个量级影响。这个数字决定了系统的自然频率和不稳定极点位置也决定了控制器反馈增益的量级换一套参数时应该先重新算一次 $A$ 矩阵而不是照抄原来的增益。4. 用 SymPy SciPy 把倒立摆模型跑起来非线性仿真与模型校验4.1 最小可复现代码开环仿真的 Python 脚本前面推导的是符号方程这一节把它变成能跑的数值模型。开环仿真的目的是看系统在没有任何控制力时的运动摆杆从偏离平衡点的初始角度释放如果不加控制它会倒下并来回摆动。这里取初始角度 $\theta 0.1$ rad对应偏离竖直约 5.7°。import numpy as np from scipy.integrate import solve_ivp import matplotlib.pyplot as plt M, m, l, g, b 1.0, 0.1, 0.5, 9.81, 0.1 J m * l**2 / 3 def pendulum_dynamics(t, s, F): x, xd, theta, theta_dot s c np.cos(theta) s_th np.sin(theta) M_mat np.array([[M m, 0.5 * m * l * c], [0.5 * m * l * c, J]]) rhs np.array([F 0.5 * m * l * theta_dot**2 * s_th, 0.5 * m * g * l * s_th - b * theta_dot]) xdd, thetadd np.linalg.solve(M_mat, rhs) return [xd, xdd, theta_dot, thetadd] s0 [0.0, 0.0, 0.1, 0.0] t_eval np.linspace(0, 3, 300) sol solve_ivp(pendulum_dynamics, [0, 3], s0, t_evalt_eval, args(0.0,), rtol1e-8) plt.plot(sol.t, np.rad2deg(sol.y[2])) plt.xlabel(t [s]) plt.ylabel(theta [deg]) plt.title(Open-loop release of inverted pendulum) plt.grid(True) plt.show()pendulum_dynamics返回四个状态变量的导数。rhs中第一项对应外力 $F$ 和向心耦合项第二项对应重力项和阻尼项。np.linalg.solve求解的是广义质量矩阵线性方程组这样能保留完整的非线性信息比手写解耦公式更不容易出错。rtol1e-8是一个关键参数倒立摆对积分误差极其敏感默认的容差可能导致轨迹在中途发散。如果发现仿真曲线出现“假发散”现象先检查求解器容差而不是先怀疑模型。4.2 模型校验的 3 个标准做法开环仿真跑出来的摆角曲线看起来会像单摆一样摆动但这不能说明模型正确。我一般按下面三个顺序做校验校验方法预期结果失败时可能原因能量守恒机械能 $E T V$ 在无外力时保持恒定积分容差不够或初始条件设置错误退化测试$m \to 0$ 时退化为纯小车模型$M \to \infty$ 时退化为单摆模型非线性耦合项在代码中没消干净极点比对线性模型 $A$ 的特征值包含一个正实部极点平衡点约定不一致符号写反第一个方法可以直接在仿真循环里每隔 0.01 秒记录一次 $E$如果能量漂移超过 1%说明容差需要收紧。第二个方法把质量矩阵的对角项替换为极限值观察是否复现标准方程。第三个方法对 $A$ 求特征值虚部和开环仿真的摆动频率应该吻合如果一个正实部极点都没有说明 $A$ 矩阵符号错了问题通常出在 $\theta$ 方向的定义上。4.3 常见参数坑转动惯量、单位与小角度假设第一个坑是转动惯量的位置。如果建模时把摆杆质量集中到质心转动惯量不能用 $\frac{1}{3}ml^2$ 而要用 $\frac{1}{12}ml^2$如果建模时把转动惯量折算到端点那平动动能里的系数也会变化。最稳妥的做法是从一开始就按质心惯性矩加平行轴定理统一写代码里的J和推导时的Jc要保持一致。第二个坑是角度单位前面已经强调过所有动力学方程必须使用弧度制。PID 控制器里如果把角度反馈量用度状态空间矩阵里的 $g$ 和 $l$ 组合出来是正确定纲但一旦把度代入仿真结果会完全乱套。第三个坑是小角度假设的成立范围。线性模型大约在 $\theta 0.35$ rad 即 20° 以内才足够精确。如果实际系统里摆角经常超过这个范围线性模型只能作为参考控制器需要改用非线性鲁棒方法LQR 的权重矩阵也要重新标定。5. 让模型走向应用LQR 控制器、飞控场景与模型时效性验证5.1 在仿真里给系统接上 LQR 控制器拿到线性模型后我一般会先用 LQR 验证一下模型动力学是否合理。LQR 会解出使代价函数 $J \int (s^T Q s F^T R F) dt$ 最小的反馈增益 $K$。这个步骤只需要几行代码但 $Q$ 和 $R$ 的选取决定了控制效果增加对应 $\theta$ 的权重会让摆杆更快直立增加对应 $x$ 的权重会让小车更快回到原点减小 $R$ 会让执行器输出更强。from scipy.linalg import solve_continuous_are A np.array([[0, 1, 0, 0], [0, 0, -m * g / M, 0], [0, 0, 0, 1], [0, 0, (M m) * g / (M * l), 0]]) B np.array([[0], [1 / M], [0], [-1 / (M * l)]]) Q np.diag([10.0, 1.0, 100.0, 1.0]) R np.array([[1.0]]) P solve_continuous_are(A, B, Q, R) K np.linalg.inv(R) B.T P print(K.flatten())反馈增益 $K$ 是 1×4 行向量控制律是 $F -K s$。求解结果中对应 $\theta$ 的第三个分量通常远大于其他项配合 $B$ 的符号控制力的正负正好给出“向摆杆倾斜一侧推动小车”的直观逻辑这也反向印证了 $A$ 矩阵方向约定的正确性。把 $K$ 代回非线性仿真模型摆角能在约 1 秒内收敛到零。这里的Q和R可以先用对角线矩阵试跑再根据超调量和稳态误差迭代微调。5.2 从单摆到飞控把拉格朗日建模方法复用到多体系统拉格朗日法最值钱的不是这一次推导而是“选广义坐标、算能量、代入方程”这条流水线可以复制。做两轮自平衡机器人时可以把车身倾角、左右轮位移都写成广义坐标再补上轮子和底盘的动能项做飞控里的姿态解算时把欧拉角或四元数作为广义坐标旋翼推力作为广义力用同一套方法可以推导出俯仰和横滚轴的动力学方程。很多飞控团队在写姿态控制器之前都会先搭这套模型来检验控制律。数学建模竞赛里这类题目也很常见全国大学生数学建模竞赛和研究生数学建模竞赛的控制类题目给的往往不是现成动力学方程而是需要先用工科方法把系统的数学模型整理出来。模型的推导过程本身就是评审的重要观察点符号表、坐标定义、能量函数书写规范这些都比最后的仿真图更能决定论文档次。5.3 一个容易忽略的验证技巧10 分钟完成模型可信度检测最后推荐一个我在交付模型前必做的检测把摆杆质量 $m$ 设为 0代入矩阵方程系统应当退化为纯小车模型 $\ddot{x} F / M$。如果此时仿真曲线出现不该有的振荡说明非线性耦合项在推导或代码实现时没有完全消掉问题出在质量矩阵或右侧力项的某一行。再做一个逆检测把小车的质量设为极大值模型应该退化为绕固定轴转动的单摆方程重力项和阻尼项应与标准单摆完全一致。这两步检测耗时不到 10 分钟可以在方法复用到双连杆摆、平衡车、飞控场景时随时验证。不少团队把符号推导和数值仿真放在两个文件里修改参数后忘记同步更新状态空间矩阵退化测试能直观地把这种不一致暴露出来。在 SymPy 里sp.simplify(A - expected_A)可以直接打印两个矩阵的逐项偏差哪一行符号反了、哪一项漏了系数一目了然不用靠肉眼对着一大坨矩阵元素找差异。本文还有配套的精品资源点击获取