一级倒立摆控制 —— 动力学系统建模

系列文章目录

最优控制介绍
一级倒立摆控制 —— PID 控制器设计及 MATLAB 实现
一级倒立摆控制 —— LQR 控制器设计及 MATLAB 实现
一级倒立摆控制 —— MPC 控制器设计及 MATLAB 实现
一级倒立摆控制 —— ROS2 仿真
一级倒立摆控制 —— LQR 控制器 GAZEBO 仿真

---

---

一、倒立摆系统的特点、目标及原理

特点：非线性、高阶次、多变量、强耦合；
目标：通过给小车底座加力 $F$（控制量），使小车停留在预定的位置，并使摆杆不倒下，即不超过预先定义的竖直偏离角度范围。
原理：小车位置编码器将小车的位移和速度信号反馈给运动控制卡，角度编码器将摆角的角度、角速度信号反馈给运动控制卡。计算机从运动控制卡中读取实时反馈数据，确定小车向哪个方向移动、移动速度、加速度等，产生相应的控制量 $F$。

---

二、倒立摆系统建模

2.1 系统变量表：

参数	符号	数值
小车质量	$M$	0.5 kg
摆杆质量	$m$	0.2 kg
小车摩擦系数	$b$	0.1 N/m/sec
到摆杆质心的长度	$l$	0.3 m
摆杆转动惯量	$I$	0.006 kg*m²
施加在小车上的力	$F$
小车位置坐标	$x$
摆杆偏离竖直方向的角度	$\theta$

2.2 系统受力分析

根据所取变量的方式，目前主要有两种建模方式：
如图所示，一种取钝角 $\theta$ 为变量建立系统方程组，另一种以锐角 $\varphi$ 为变量建立系统方程组。下面分别分析。

2.2.1 以锐角 $\varphi$ 为变量建立系统模型

注意：这里的 $\varphi$ 为 $\pi -\theta$，与下面以钝角 $\theta$ 为变量建立系统模型中的 $\varphi$ 并不相同。

2.2.1.1 取顺时针为正方向

注意：刚体绕定轴转动动力学方程需要实现规定正方向，一般取逆时针方向为正方向。（其实我觉得下面的不太正确，但是几本书上是这么写的）
根据刚体绕固定轴转动的动力学方程，转动惯量与加速度乘积等于作用于刚体主动力对该轴力矩的代数和，则
摆杆绕其重心的转动方程为
$$J\ddot{\varphi }=F_{y}l\sin \varphi -F_{x}l\cos \varphi$$
摆杆重心的水平运动可以描述为
$$F_x=m\frac{d^2}{d{t}^2}(x+l\sin \varphi )$$
摆杆重心在竖直方向上的运动可描述为
$$F_y-mg=m\frac{d^2}{d{t}^2}(l\cos \varphi )$$
小车水平方向的运动可描述为
$$F-F_x-b\dot{x}=M\frac{d^{2}x}{d{t}^2}$$
由式（2）和式（4）得
$$(M+m)\ddot{x}+b\dot{x}+ml\ddot{\varphi }\cos \varphi -ml{\dot{\varphi }}^2\sin \varphi =F$$
由式（1）和式（3）得
$$(J+m{l}^2)\ddot{\varphi }+ml\cos \varphi \cdot \ddot{x}=mlg\cdot \sin \varphi$$

---

2.2.1.2 取逆时针为正方向

根据刚体绕固定轴转动的动力学方程，转动惯量与加速度乘积等于作用于刚体主动力对该轴力矩的代数和，则
摆杆绕其重心的转动方程为
$$J\ddot{\varphi }=F_{x}l\cos \varphi -F_{y}l\sin \varphi$$
摆杆重心的水平运动可以描述为
$$F_x=m\frac{d^2}{d{t}^2}(x+l\sin \varphi )$$
摆杆重心在竖直方向上的运动可描述为
$$F_y-mg=m\frac{d^2}{d{t}^2}(l\cos \varphi )$$
小车水平方向的运动可描述为
$$F-F_x-b\dot{x}=M\frac{d^{2}x}{d{t}^2}$$
由式（2）和式（4）得
$$(M+m)\ddot{x}+b\dot{x}+ml\ddot{\varphi }\cos \varphi -ml{\dot{\varphi }}^2\sin \varphi =F$$
由式（1）和式（3）得
$$(m{l}^2-J)\ddot{\varphi }+ml\cos \varphi \cdot \ddot{x}=mlg\cdot \sin \varphi$$

---

2.2.1 以钝角 $\theta$ 为变量建立系统模型

注意： $\theta$ 为钝角，$\cos \theta$ 为负，即乘以 $\cos \theta$ 后方向会改变为反方向。

小车水平方向上的受力方程
$$M\ddot{x}+b\dot{x}+N=F$$
摆杆水平方向上的受力方程
$$N=m\ddot{x}+ml\ddot{\theta }\cos \theta -ml{\dot{\theta }}^2\sin \theta$$
垂直于摆杆方向上的受力方程
$$P\sin \theta +N\cos \theta -mg\sin \theta =ml\ddot{\theta }+m\ddot{x}\cos \theta$$
其中，$ml\ddot{\theta }$ 为摆杆所受向心力，$m\ddot{x}\cos \theta$ 为摆杆沿 $x$ 方向加速度在垂直摆杆方向的分量。
根据刚体绕定轴转动的动力学公式，摆杆转动动力学方程
$$-Pl\sin \theta -Nl\cos \theta =I\ddot{\theta }$$
由式（7）和（8）得
$$(M+m)\ddot{x}+b\dot{x}+ml\ddot{\theta }\cos \theta -ml{\dot{\theta }}^2\sin \theta =F$$
由式（9）和式（10）得
$$(I+m{l}^2)\ddot{\theta }+mgl\sin \theta =-ml\ddot{x}\cos \theta$$
整理得系统动力学方程组
$$\{\begin{array}{l}(M+m)\ddot{x}+b\dot{x}+ml\ddot{\theta }\cos \theta -ml{\dot{\theta }}^2\sin \theta =F\\ (I+m{l}^2)\ddot{\theta }+mgl\sin \theta =-ml\ddot{x}\cos \theta \end{array}$$
使用不同的变量所得模型不同，但是并不影响后续的控制，因此以后采用以钝角为变量建模所得方程。

---

三、动力学方程雅可比线性化

雅可比线性化可参考：
将系统在平衡位置的方程组线性化，系统的平衡点为 $\theta =\pi$，取 $\theta =\pi +\varphi$，则可对非线性方程组作以下小角度近似：

注意：这里的 $\varphi$ 为摆杆向左偏离竖直方向的角度，与上面摆杆向右偏离竖直方向的角度并不是同一个角度。

$$\{\begin{array}{l}\cos \theta =\cos (\pi +\varphi )\approx -1\\ \sin \theta =\sin (\pi +\varphi )\approx -\varphi \\ {\dot{\theta }}^2={\dot{\varphi }}^2\approx 0\end{array}$$
将上述近似代入非线性动力学方程组中得线性化动力学方程组
$$\{\begin{array}{l}(M+m)\ddot{x}+b\dot{x}-ml\ddot{\varphi }=u\\ (I+m{l}^2)\ddot{\varphi }-mgl\varphi =ml\ddot{x}\end{array}$$

---

四、传递函数

对初始条件为零的系统动力学方程（15）进行拉普拉斯变换
$$\{\begin{array}{l}(M+m)X(s)s^2+bX(s)s-ml\Phi (s)s^2=U(s)\\ (I+m{l}^2)\Phi (s)s^2-mgl\Phi (s)s=mlX(s)s^2\end{array}$$
整理得摆杆传递函数方程
$$P_{pend}(s)=\frac{\Phi (s)}{U(s)}=\frac{\frac{ml}{q}{s}^2}{s^4+\frac{b(I+m{l}^2)}{q}{s}^3-\frac{(M+m)mgl}{q}{s}^2-\frac{bmgl}{q}s}$$
式中，$q=[(M+m)(I+m{l}^2)-(ml{)}^2]$
由式（17）可以看出，传递函数在原点既有一个极点，也有一个零点，约去得
$$P_{pend}(s)=\frac{\Phi (s)}{U(s)}=\frac{\frac{ml}{q}s}{s^3+\frac{b(I+m{l}^2)}{q}{s}^2-\frac{(M+m)mgl}{q}s-\frac{bmgl}{q}}$$
类似的，小车的传递函数也可以得到
$$P_{cart}(s)=\frac{\Phi (s)}{U(s)}=\frac{\frac{(I+m{l}^2)s^2-mgl}{q}}{s^4+\frac{b(I+m{l}^2)}{q}{s}^3-\frac{(M+m)mgl}{q}{s}^2-\frac{bmgl}{q}s}$$

---

五、状态空间表示

将动力学方程组（15）写为一阶微分方程组的形式，用矩阵表示为
$$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\varphi }\\ \ddot{\varphi }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{-(I+m{l}^2)b}{I(M+m)+Mm{l}^2}& \frac{m^{2}g{l}^2}{I(M+m)+Mm{l}^2}& 0\\ 0& 0& 0& 1\\ 0& \frac{-mlb}{I(M+m)+Mm{l}^2}& \frac{mgl(M+m)}{I(M+m)+Mm{l}^2}& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \varphi \\ \dot{\varphi }\end{array}\right]+\left[\begin{array}{c}0\\ \frac{I+m{l}^2}{I(M+m)+Mm{l}^2}\\ 0\\ \frac{ml}{I(M+m)+Mm{l}^2}\end{array}\right]u$$
$$\mathbf{y}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\varphi }\\ \ddot{\varphi }\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]u$$

---

六、MATLAB 表示

6.1 传递函数

代码为

M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;
q = (M+m)*(I+m*l^2)-(m*l)^2;
s = tf('s');

P_cart = (((I+m*l^2)/q)*s^2 - (m*g*l/q))/(s^4 + (b*(I + m*l^2))*s^3/q - ((M + m)*m*g*l)*s^2/q - b*m*g*l*s/q);

P_pend = (m*l*s/q)/(s^3 + (b*(I + m*l^2))*s^2/q - ((M + m)*m*g*l)*s/q - b*m*g*l/q);

sys_tf = [P_cart ; P_pend];

inputs = {'u'};
outputs = {'x'; 'phi'};

set(sys_tf,'InputName',inputs)
set(sys_tf,'OutputName',outputs)

sys_tf

输出为

sys_tf =
 
  从输入 "u" 到输出...
                       4.182e-06 s^2 - 0.0001025
   x:  ---------------------------------------------------------
       2.3e-06 s^4 + 4.182e-07 s^3 - 7.172e-05 s^2 - 1.025e-05 s
 
                              1.045e-05 s
   phi:  -----------------------------------------------------
         2.3e-06 s^3 + 4.182e-07 s^2 - 7.172e-05 s - 1.025e-05
 
Continuous-time transfer function.

---

6.2 状态空间

代码为

M = .5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;

p = I*(M+m)+M*m*l^2; %denominator for the A and B matrices

A = [0      1              0           0;
     0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p   0;
     0      0              0           1;
     0 -(m*l*b)/p       m*g*l*(M+m)/p  0];
B = [     0;
     (I+m*l^2)/p;
          0;
        m*l/p];
C = [1 0 0 0;
     0 0 1 0];
D = [0;
     0];

states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'};
outputs = {'x'; 'phi'};

sys_ss = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs)

输出为

sys_ss =
 
  A = 
                  x    x_dot      phi  phi_dot
   x              0        1        0        0
   x_dot          0  -0.1818    2.673        0
   phi            0        0        0        1
   phi_dot        0  -0.4545    31.18        0
 
  B = 
                u
   x            0
   x_dot    1.818
   phi          0
   phi_dot  4.545
 
  C = 
              x    x_dot      phi  phi_dot
   x          1        0        0        0
   phi        0        0        1        0
 
  D = 
        u
   x    0
   phi  0
 
连续时间状态空间模型。

---