Ros2 实时编程实践第四章 -- 倒立摆数学原理分析

1 前言和资料

本文是 Ros2 实时编程实践系列第四章，我们将对一级倒立摆: Ros2 实时编程实践第一章 – 倒立摆引入与 ros2 pendulum 仿真 进行数学原理分析，为自己实现倒立摆样例做理论准备。本文将用到牛顿力学定律和微积分知识，初看起来，会显得比较高深，但如果读者随着文章一起动手推导，相信大家都能理解。
本文参考资料如下：
（1）wiki Inverted_pendulum
（2）Inverted-Pendulum-Control-Theory-and-Dynamics/
（3）一阶倒立摆的PID控制和LQR控制
（4）倒立摆模型分析
（5）线性系统大作业——1.一阶倒立摆建模与控制系统设计
（6）由拉普拉斯变换到传递函数 （本文用不到拉普拉斯变换和传递函数，但这个文章讲的很好，所以也列在了这里）

2 正文

（1）倒立摆示意图
M ：小车质量（kg）
m ：摆杆质量（kg）
2 * L ：摆杆长度（m）
I ：摆杆转动惯量（kg * m ^ 2）
x：小车水平位移（m）
F：作用在小车水平方向上的力（N）
φ：摆杆转动角度（rad）
h：摆杆重心在垂直方向上的移动距离（m）
s：摆杆重心在水平方向上的移动距离（m）

针对 h 和 s ，有如下表达式成立：
$$\begin{gathered} h=L\ast cos(\phi ) \\ s=x+L\ast sin(\phi ) \end{gathered}$$
（2）对摆杆进行受力分析
P：小车转轴对摆杆垂直方向上的分力（N）
N：小车转轴对摆杆水平方向上的分力（N）
mg：摆杆重力
φ：摆杆转动角度（rad）

摆杆垂直方向和水平方向上的受力方程：
$$\begin{gathered} P=mg+m\ast \ddot{h} \\ N=m\ast \ddot{s} \end{gathered}$$
摆杆转动方程：
$$I\ast \ddot{\phi }=L\ast (P\ast sin(\phi )-N\ast cos(\phi ))$$
（3）小车受力示意图

对小车进行水平方向上的受力分析（垂直方向没有意义）
$$M\ast \ddot{x}=F-N$$
（4）推导 F（小车水平方向上的力） 与 φ（摆杆转动角度） 和 x（ 小车水平位移）之间的关系：
特别提示：针对下面的表达式，其中 h，s，x，φ 都随时间 t 变化，因此他们都是时间 t 的函数。
$$\begin{gathered} h=L\ast cos(\phi ) \\ s=x+L\ast sin(\phi ) \end{gathered}$$
明确的写法如下。但是，为了推导简便，我们仍采用上面的写法。
$$\begin{gathered} h(t)=L\ast cos(\phi (t)) \\ s(t)=x(t)+L\ast sin(\phi (t)) \end{gathered}$$
第一步，让 h 对时间求二阶导 ，根据微积分复合函数求导的链式法则和乘积函数求导的乘积法则，有如下结果：
$$\begin{gathered} h=L\ast cos(\phi ) \\ \dot{h}=L\ast (-sin(\phi )\ast \dot{\phi }) \\ \ddot{h}=L\ast (-cos(\phi )\ast {\dot{\phi }}^2-sin(\phi )\ast \ddot{\phi })=-L\ast cos(\phi )\ast {\dot{\phi }}^2-L\ast sin(\phi )\ast \ddot{\phi } \end{gathered}$$
第二步，让 s 对时间求二阶导，根据链式法则和乘积法则，有如下结果：
$$\begin{gathered} s=x+L\ast sin(\phi ) \\ \dot{s}=\dot{x}+L\ast cos(\phi )\ast \dot{\phi } \\ \ddot{s}=\ddot{x}+L\ast (-sin(\phi )\ast {\dot{\phi }}^2+cos(\phi )\ast \ddot{\phi })=\ddot{x}-L\ast sin(\phi )\ast {\dot{\phi }}^2+L\ast cos(\phi )\ast \ddot{\phi }) \end{gathered}$$
第三步，将 h 和 s 的二阶导结果，带入到 P 和 N 的方程中：
$$\begin{gathered} P=mg+m\ast \ddot{h}=mg-mLcos(\phi )\ast {\dot{\phi }}^2-mLsin(\phi )\ast \ddot{\phi } \\ N=m\ast \ddot{s}=m\ast \ddot{x}-mL\ast sin(\phi )\ast {\dot{\phi }}^2+mL\ast cos(\phi )\ast \ddot{\phi } \end{gathered}$$
第四步，将 P 和 N 的结果，带入摆杆的转动方程中：
$$\begin{gathered} I\ast \ddot{\phi }=L\ast (P\ast sin(\phi )-N\ast cos(\phi )) \\ =L\ast sin(\phi )\ast P-L\ast cos(\phi )\ast N \\ =L\ast sin(\phi )\ast (mg-mLcos(\phi )\ast {\dot{\phi }}^2-mLsin(\phi )\ast \ddot{\phi }) \\ -L\ast cos(\phi )\ast (m\ast \ddot{x}-mL\ast sin(\phi )\ast {\dot{\phi }}^2+mL\ast cos(\phi )\ast \ddot{\phi }) \\ =mg\ast L\ast sin(\phi )-m\ast L^2\ast sin(\phi )\ast cos(\phi )\ast {\dot{\phi }}^2-m\ast L^2\ast sin(\phi {)}^2\ast \ddot{\phi } \\ -m\ast L\ast cos(\phi )\ast \ddot{x}+m\ast L^2\ast sin(\phi )\ast cos(\phi )\ast {\dot{\phi }}^2-m\ast L^2\ast cos(\phi {)}^2\ast \ddot{\phi } \\ =mg\ast L\ast sin(\phi )-m\ast L^2\ast \ddot{\phi }-m\ast L\ast cos(\phi )\ast \ddot{x} \end{gathered}$$
最后可得：
$$(I+m\ast L^2)\ast \ddot{\phi }=mg\ast L\ast sin(\phi )-m\ast L\ast cos(\phi )\ast \ddot{x}$$
第五步，将 N 的结果，带入小车的水平受力方程中：
$$\begin{gathered} M\ast \ddot{x}=F-N \\ ==> \\ F=M\ast \ddot{x}+N \\ =M\ast \ddot{x}+m\ast \ddot{x}-mL\ast sin(\phi )\ast {\dot{\phi }}^2+mL\ast cos(\phi )\ast \ddot{\phi } \\ =(M+m)\ast \ddot{x}-mL\ast sin(\phi )\ast {\dot{\phi }}^2+mL\ast cos(\phi )\ast \ddot{\phi } \end{gathered}$$
第七步，当摆杆偏转比较小的时候，可利用如下近似简化方程
$$\begin{gathered} sin(\phi )=\phi \\ cos(\phi )=1 \\ \dot{\phi }=0 \end{gathered}$$
最终可得：
$$\begin{gathered} (I+m\ast L^2)\ast \ddot{\phi }=mg\ast L\ast \phi -m\ast L\ast \ddot{x} \\ F=(M+m)\ast \ddot{x}+mL\ast \ddot{\phi } \end{gathered}$$

3 总结

数学表达式只看不动手是很难懂的。另外，PPT 画图很强大。写这篇博客，考研的记忆碎片不时闪现。算了，不说了。。