基本模型

电机模型：

$V_m=i_aR_a+L_a\frac{di_a}{dt}+E_b$

反电动势 (emf) $E_{b}$ 与角速度 $\dot{\varphi}$ 相关。

$E_{b}=K_{b}\dot{\varphi}$

因为： $L_a<<R_a$

所以： $i_a=\frac{V_m-K_b \dot{\varphi}}{R_a}$

电机转矩 $\tau$ 和反电动势 (emf) 有关：

\tau=K_t i_a=K_t \frac{V_m-K_b \dot{\varphi}}{R_a} (公式4)

符号	单位	定义
θ	rad	摆杆角度
φ	rad	惯性轮角度
J₁	kg.m²	包括电机定子的摆杆转动惯量
J₂	kg.m²	包括电机转子的轮子转动惯量
c₁	N.m.s/rad	摆杆摩擦系数
c₂	N.m.s/rad	轮子摩擦系数
m₁	kg	摆杆和定子的质量
m₂	kg	轮子和转子的质量
l₁	m	从原点到摆杆重心的长度
l₂	m	从原点到轮子重心的长度
Kb	V/(rad/s)	反电动势常数
Kt	N.m/A	电机扭矩常数
Ra	Ω	电枢绕组电阻

平动动能 Translational kinetic:

T_1=\frac{1}{2} m_1\left(l_1 \dot{\theta}\right)^2+\frac{1}{2} m_2\left(l_2 \dot{\theta}\right)^2 (公式6)

转动动能 Rotational kinetic:

T_2=\frac{1}{2} J_1 \dot{\theta}^2+\frac{1}{2} J_2(\dot{\theta}+\dot{\varphi})^2(公式7)

系统的动能：

\begin{aligned} T & =T_1+T_2 \\ & =\frac{1}{2}\left(m_1 l_1^2+m_2 l_2^2+J_1+J_2\right) \dot{\theta}^2+J_2 \dot{\theta} \dot{\varphi}+\frac{1}{2} J_2 \dot{\varphi}^2 \end{aligned} (公式8)

以O点作为势能的基准。因此，势能为：

V=\left(m_1 l_1+m_2 l_2\right) g \cos (\theta)(公式9)

平面倒立摆的拉格朗日函数为：

\begin{aligned} L & =T-V=\frac{1}{2}\left(m_1 l_1^2+m_2 l_2^2+J_1+J_2\right) \dot{\theta}^2 \\ & +J_2 \dot{\theta} \dot{\varphi}+\frac{1}{2} J_2 \dot{\varphi}^2-\left(m_1 l_1+m_2 l_2\right) g \cos (\theta) \end{aligned}(公式10)

耗散能量：

R=\frac{1}{2} c_1 \dot{\theta}^2+\frac{1}{2} c_2 \dot{\varphi}^2(公式11)

从拉格朗日方程得到的关于 θ 的旋转轴上的力矩为，这个公式是最后一个公式需要用的：

\begin{gathered} \frac{d}{d t}\left(\frac{\partial L}{\partial \dot{\theta}}\right)+\frac{\partial R}{\partial \dot{\theta}}-\frac{\partial L}{\partial \theta}=0 (公式12)\\ \Leftrightarrow\left(m_1 l_1^2+m_2 l_2^2+J_1+J_2\right) \ddot{\theta}+J_2 \ddot{\varphi}+c_1 \dot{\theta} -\left(m_1 l_1+m_2 l_2\right) g \sin (\theta)=0 (公式13)\end{gathered}

从拉格朗日方程得到的关于 φ 的旋转轴上的力矩为：

\begin{aligned} & \frac{d}{d t}\left(\frac{\partial L}{\partial \dot{\varphi}}\right)+\frac{\partial R}{\partial \dot{\varphi}}-\frac{\partial L}{\partial \varphi}=0 (公式14)\\ & \Leftrightarrow J_2(\ddot{\theta}+\ddot{\varphi})+c_2 \dot{\varphi}=\tau (公式15)\end{aligned}

$\tau$ 是电机的扭矩。

因为：

\tau=K_t i_a=K_t \frac{V_m-K_b \dot{\varphi}}{R_a}(公式4)

由公式4和公式15可以得到：

J_2(\ddot{\theta}+\ddot{\varphi})+c_2 \dot{\varphi}=K_t \frac{V_m-K_b \dot{\varphi}}{R_a}(公式16)

由公式13和公式16，平面倒立摆的非线性模型为：

\begin{aligned} & {\left[\begin{array}{cc} m_1 l_1^2+m_2 l_2^2+J_1+J_2 & J_2 \\ J_2 & J_2 \end{array}\right]\left[\begin{array}{c} \ddot{\theta} \\ \ddot{\varphi} \end{array}\right]+\left[\begin{array}{cc} c_1 & 0 \\ 0 & \frac{K_t K_b}{R_a}+c_2 \end{array}\right]\left[\begin{array}{c} \dot{\theta} \\ \dot{\varphi} \end{array}\right]} \\ & +\left[\begin{array}{c} -\left(m_1 l_1+m_2 l_2\right) g \sin (\theta) \\ 0 \end{array}\right]=\left[\begin{array}{c} 0 \\ \frac{K_t}{R_a} \end{array}\right] V_m (公式17) \end{aligned}

给IWP的参数：

Symbol	Value	Symbol	Value
J₁	0.01186 (kg·m²)	l₁	0.1053 (m)
J₂	0.0005711 (kg·m²)	l₂	0.14 (m)
c₁	0.04 (N·m·s/rad)	K_b	0.0987 (V/(rad/s))
c₂	0.0001 (N·m·s/rad)	K_t	0.0987 (N·m/A)
m₁	0.826 (kg)	R_a	1.5562 (Ω)
m₂	0.583 (kg)

电压输入 $V_{\mathrm{m}}$ 是控制变量
四个状态变量分别是 $\theta, \dot{\theta}, \varphi$ 和 $\dot{\varphi}$
输出扭矩 $\tau$ 根据方程 (4) 计算

控制变量法-Vm 分析

从Simulink模型中，电压输入 $V_{\mathrm{m}}$ 以10V、15V和20V的步进形式施加。电压输入引起了车轮和摆锤的运动，如图8所示。

仿真结果表明：

在图8.a中，车轮的速度在约0.5秒时达到稳态。同时，直流电动机在车轮上产生一个力矩，如图8.b所示，惯性轮以一定的加速度旋转，如图8.c所示。摆锤被力矩驱动并围绕平衡位置摆动。从图8.d可以看出，较高的 $V_{\mathrm{m}}$ 使得摆锤角度的振幅更大，这表明在摆锤上生成的力更大。然而，摆锤的振荡频率在所有情况下都是相同的，这意味着 $V_{\mathrm{m}}$ 不影响摆锤的振荡频率。
当车轮速度进入稳态时，如图8.b所示，电机无法长时间维持转子力矩。在这个状态下，该力矩将等于摩擦力矩，车轮的加速度为零，摆锤仅受重力影响，并逐渐回到平衡位置 oscillates gradually back to equilibrium position。

控制变量法-l2 分析

在图 9.a、9.b、9.c 中，所有情形的响应都是相同的，这意味着 l2 对惯性轮运动的响应没有影响。然而，当 l2 改变时，摆的角度是不同的。在给电机相同电压的情况下，较大的 l2 会导致摆角幅度较小，这意味着作用在摆上的总力较小。此外，振荡频率较低且更稳定。

摆的长度会影响作用于摆上的力。选择适合 l2 的电机是有用的。较大的 l2 对摆角响应有负面影响。

控制变量法-J2 分析

另一个重要的IWP参数是飞轮的惯性矩J2。与l2的情况相同，直流电机将接收到20V的阶跃输入Vm。J2的值在0.0005kg.m²、0.002kg.m²和0.005kg.m²之间变化。其他参数保持与表2中一致。仿真结果显示：

在图10.a中，飞轮速度的定常时间随着J2的增加而变长，但稳态值在所有情况下都相同。在短时间内，电机扭矩在0秒时相同，但随着J2的增加，其影响时间也变长，如图10.b所示。在图10.c中，飞轮的加速度在0秒时不同：惯性矩较小的飞轮在起始时的加速度较高，但更快趋于零。
因此，在图10.d中，摆角的振幅在J2较大时也较大，这是因为电机扭矩的影响时间更长。
J2对电机的加速过程有积极影响，因此较大的J2有助于保持摆动的动量，但系统的质量也会增加。

控制变量法-质量m2分析

最后一个要模拟的参数是飞轮的质量m2。与l2、J2相似，直流电机将接收到20V的阶跃输入Vm，并将m2的值改变为0.5kg、0.8kg和1kg。其他参数保持不变。仿真结果显示：

从图11.a、11.b、11.c可以看出，所有m2的情况下，飞轮的速度、加速度和电机扭矩相同。因此，飞轮的质量对上述物理参数的响应没有影响。
从图11.d可以看出，较大的m2使得摆角的振幅较小，这是由于重力的影响。
m2对系统的性能有负面影响。

对公式17的理解

动能与势能公式：

T = \frac{1}{2}\left(m_1 l_1^2 + m_2 l_2^2 + J_1 + J_2\right) \dot{\theta}^2 + J_2 \dot{\theta} \dot{\varphi} + \frac{1}{2} J_2 \dot{\varphi}^2

V = \left(m_1 l_1 + m_2 l_2\right) g \cos(\theta)

拉格朗日方程导出：

(m_1 l_1^2 + m_2 l_2^2 + J_1 + J_2) \ddot{\theta} + J_2 \ddot{\varphi} + c_1 \dot{\theta} - \left(m_1 l_1 + m_2 l_2\right) g \sin(\theta) = 0

J_2(\ddot{\theta} + \ddot{\varphi}) + c_2 \dot{\varphi} = \tau

\tau = K_t \frac{V_m - K_b \dot{\varphi}}{R_a}

矩阵方程：

\begin{aligned} & \left[\begin{array}{cc} m_1 l_1^2 + m_2 l_2^2 + J_1 + J_2 & J_2 \\ J_2 & J_2 \end{array}\right] \left[\begin{array}{c} \ddot{\theta} \\ \ddot{\varphi} \end{array}\right] + \left[\begin{array}{cc} c_1 & 0 \\ 0 & \frac{K_t K_b}{R_a} + c_2 \end{array}\right] \left[\begin{array}{c} \dot{\theta} \\ \dot{\varphi} \end{array}\right] \\ & + \left[\begin{array}{c} - \left(m_1 l_1 + m_2 l_2\right) g \sin(\theta) \\ 0 \end{array}\right] = \left[\begin{array}{c} 0 \\ \frac{K_t}{R_a} V_m \end{array}\right] \end{aligned}

结合矩阵形式，含有状态方程 $\theta$ 和 $\varphi$ 的运动方程可以理解为：

使用矩阵表示：

A \left[\begin{array}{c} \ddot{\theta} \\ \ddot{\varphi} \end{array}\right] = -B \left[\begin{array}{c} \dot{\theta} \\ \dot{\varphi} \end{array}\right] + C

其中：

A = \left[\begin{array}{cc} m_1 l_1^2 + m_2 l_2^2 + J_1 + J_2 & J_2 \\ J_2 & J_2 \end{array}\right]

B = \left[\begin{array}{cc} c_1 & 0 \\ 0 & \frac{K_t K_b}{R_a} + c_2 \end{array}\right]

C = \left[\begin{array}{c} - (m_1 l_1 + m_2 l_2) g \sin(\theta) \\ \frac{K_t}{R_a} V_m \end{array}\right]

将 $\tau$ 代入第二个方程：

J_2 (\ddot{\theta} + \ddot{\varphi}) + c_2 \dot{\varphi} = K_t \frac{V_m - K_b \dot{\varphi}}{R_a}

将矩阵形式重写：

A \ddot{X} = -B \dot{X} + C

代码里acceleration是什么

在 MATLAB 中，A \ b 表示将线性方程组 (A x = b) 解出 (x)，即 (x = A^{-1} b)。这个操作称为“左除”运算符，用于求解涉及矩阵的线性方程。

现在详细解释这行代码：

matlab
展开代码
acceleration = A \ (-B * [theta_dot; phi_dot] + C);

背景和公式推导

我们原来推导的系统方程是：

A \ddot{X} = -B \dot{X} + C

其中:

$\ddot{X}$ 表示系统的加速度
$\dot{X}$ 表示系统的速度
$A$ 、 $B$ 和 $C$ 是系统的参数矩阵和向量

分解解释

矩阵定义:
- (A):
$A = \begin{pmatrix} m_1 l_1^2 + m_2 l_2^2 + J_1 + J_2 & J_2 \\ J_2 & J_2 \end{pmatrix}$
- (B):
$B = \begin{pmatrix} c_1 & 0 \\ 0 & \frac{K_t K_b}{R_a} + c_2 \end{pmatrix}$
- (C):
$C = \begin{pmatrix} - (m_1 l_1 + m_2 l_2) g \sin(\theta) \\ \frac{K_t}{R_a} V_m \end{pmatrix}$
速度向量 $\dot{X}$ : $\dot{X} = \begin{pmatrix} \dot{\theta} \\ \dot{\varphi} \end{pmatrix}$
公式运算:
- 计算 $-B \dot{X} + C$ :
$-B \begin{pmatrix} \dot{\theta} \\ \dot{\varphi} \end{pmatrix} + C = - \begin{pmatrix} c_1 & 0 \\ 0 & \frac{K_t K_b}{R_a} + c_2 \end{pmatrix} \begin{pmatrix} \dot{\theta} \\ \dot{\varphi} \end{pmatrix} + \begin{pmatrix} - (m_1 l_1 + m_2 l_2) g \sin(\theta) \\ \frac{K_t}{R_a} V_m \end{pmatrix}$
线性方程求解:
- 我们需要解出 $\ddot{X}$ :
$A \ddot{X} = -B \dot{X} + C$
- 由此，得到加速度 $\ddot{X}$ :
$\ddot{X} = A^{-1} \left( -B \dot{X} + C \right)$

在 MATLAB 中，我们使用左除运算符 \ 直接求解这个逆矩阵乘法，因此：

matlab
展开代码
acceleration = A \ (-B * [theta_dot; phi_dot] + C);

这行代码计算的是系统当前状态下的加速度 $\ddot{\theta}$ 和 $\ddot{\varphi}$ ，它们存储在 acceleration 向量中。

用简单的话来说，这行代码的目的是根据速度 $\dot{\theta}$ 和 $\dot{\varphi}$ 以及其他参数，计算系统在当前状态下的角加速度 $\ddot{\theta}$ 和 $\ddot{\varphi}$ 。

加入PID控制器

倒立摆系统符号形式状态空间方程

本文档提供了倒立摆系统的符号形式状态空间方程，保留了系统参数的符号表示。

系统参数符号

符号	描述
$J_1$	摆杆转动惯量 (kg·m²)
$J_2$	惯性轮转动惯量 (kg·m²)
$c_1$	摆杆摩擦系数 (N·m·s/rad)
$c_2$	惯性轮摩擦系数 (N·m·s/rad)
$m_1$	摆杆质量 (kg)
$m_2$	惯性轮质量 (kg)
$l_1$	摆杆重心距离 (m)
$l_2$	惯性轮重心距离 (m)
$K_b$	反电动势常数 (V/(rad/s))
$K_t$	电机扭矩常数 (N·m/A)
$R_a$	电枢电阻 (Ω)
$g$	重力加速度 (m/s²)

状态变量

状态向量 $\mathbf{x} = [\theta, \dot{\theta}, \phi, \dot{\phi}]^T$ 包含：

$\theta$ - 摆杆角度
$\dot{\theta}$ - 摆杆角速度
$\phi$ - 惯性轮角度
$\dot{\phi}$ - 惯性轮角速度

输入 $u = V_m$ 是电机电压，输出可以是摆杆角度 $\theta$ 或惯性轮角速度 $\dot{\phi}$ 。

状态空间模型

倒立摆系统的状态空间模型采用标准形式：

$\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}$

$\mathbf{y} = \mathbf{C}\mathbf{x} + \mathbf{D}\mathbf{u}$

系统基本矩阵

系统由以下基本矩阵描述：

质量矩阵

\mathbf{M} = \begin{bmatrix} J1 + J2 + l1^2*m1 + l2^2*m2 & J2 \\ J2 & J2 \end{bmatrix}

阻尼矩阵

\mathbf{C} = \begin{bmatrix} c1 & 0 \\ 0 & c2 + (Kb*Kt)/Ra \end{bmatrix}

重力项矩阵

\mathbf{K} = \begin{bmatrix} -g*(l1*m1 + l2*m2) & 0 \\ 0 & 0 \end{bmatrix}

输入矩阵

\mathbf{B}_{\text{input}} = \begin{bmatrix} 0 \\ Kt/Ra \end{bmatrix}

加速度表达式

基于动力学方程，加速度表达式为：

\begin{bmatrix} \ddot{\theta} \\ \ddot{\phi} \end{bmatrix} = \mathbf{M}^{-1} \left( -\mathbf{C} \begin{bmatrix} \dot{\theta} \\ \dot{\phi} \end{bmatrix} - \mathbf{K} \begin{bmatrix} \theta \\ 0 \end{bmatrix} + \mathbf{B}_{\text{input}} V_m \right)

具体展开后得到：

\ddot{\theta} = (phi_dot*(c2 + (Kb*Kt)/Ra) - (Kt*Vm)/Ra)/(J1 + l1^2*m1 + l2^2*m2) - (c1*theta_dot - g*theta*(l1*m1 + l2*m2))/(J1 + l1^2*m1 + l2^2*m2)

\ddot{\phi} = (c1*theta_dot - g*theta*(l1*m1 + l2*m2))/(J1 + l1^2*m1 + l2^2*m2) - ((phi_dot*(c2 + (Kb*Kt)/Ra) - (Kt*Vm)/Ra)*(J1 + J2 + l1^2*m1 + l2^2*m2))/(J1*J2 + J2*l1^2*m1 + J2*l2^2*m2)

状态空间矩阵A

A矩阵定义状态变量之间的关系：

\mathbf{A} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ \frac{\partial \ddot{\theta}}{\partial \theta} & \frac{\partial \ddot{\theta}}{\partial \dot{\theta}} & \frac{\partial \ddot{\theta}}{\partial \phi} & \frac{\partial \ddot{\theta}}{\partial \dot{\phi}} \\ 0 & 0 & 0 & 1 \\ \frac{\partial \ddot{\phi}}{\partial \theta} & \frac{\partial \ddot{\phi}}{\partial \dot{\theta}} & \frac{\partial \ddot{\phi}}{\partial \phi} & \frac{\partial \ddot{\phi}}{\partial \dot{\phi}} \end{bmatrix}

展开具体表达式：

\mathbf{A} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ (g*(l1*m1 + l2*m2))/(J1 + l1^2*m1 + l2^2*m2) & -c1/(J1 + l1^2*m1 + l2^2*m2) & 0 & (c2 + (Kb*Kt)/Ra)/(J1 + l1^2*m1 + l2^2*m2) \\ 0 & 0 & 0 & 1 \\ -(g*(l1*m1 + l2*m2))/(J1 + l1^2*m1 + l2^2*m2) & c1/(J1 + l1^2*m1 + l2^2*m2) & 0 & -((Ra*c2 + Kb*Kt)*(J1 + J2 + l1^2*m1 + l2^2*m2))/(J2*Ra*(J1 + l1^2*m1 + l2^2*m2)) \end{bmatrix}

状态空间矩阵B

B矩阵定义输入对状态变量变化率的影响：

\mathbf{B} = \begin{bmatrix} \frac{\partial \dot{\theta}}{\partial V_m} \\ \frac{\partial \ddot{\theta}}{\partial V_m} \\ \frac{\partial \dot{\phi}}{\partial V_m} \\ \frac{\partial \ddot{\phi}}{\partial V_m} \end{bmatrix}