惯性轮摆的控制策略

第16章 惯性轮摆的控制

16.1 惯性轮摆描述

在本章中，为一种称为惯性轮摆（IWP）的机构设计了一种控制策略。图 16.1展示了IWP在下垂位置运行时的示意图，其中涉及的变量和参数在图 16.2中定义。该系统是一个倒立摆系统，在其自由端固定有一个永磁有刷直流电机，该电机驱动一个轮。摆通过一个不受外部转矩作用的支点悬挂。该支点没有执行器，而轮上装有执行器，因为它接收由电机产生的转矩。由于摆和轮构成了两个自由度[1]但只有一个执行器（位于轮上），因此该机构被称为欠驱动[2]。

惯性轮摆由 Spong 等人提出[2]，除了其学术应用外，还被广泛用作非线性控制器研究的实验平台[2–11]。

所采用的术语如下：
- q1 是摆杆角位置。
- q2 是轮盘角位置。
- m1 是摆杆质量。
- m2 是轮质量。
- τ是施加在轮盘上的扭矩。
- lc1 是摆杆质心位置。
- l1 是摆杆长度。
- I1 是摆杆绕其质心旋转时的转动惯量。
- I2 是车轮惯量（加上电机转子惯量）。
- g是重力加速度。
- R是电机电枢电阻。
- L是电机电枢电感。
- kb和km分别是电机反电动势常数和电机转矩常数。
- u和i分别表示施加在电机电枢端子上的电压和通过电机电枢的电流。

根据对惯性轮摆（IWP）的上述描述，显然很难将摆直接从q1= 0, ˙q1= 0 转移到 q1=+π 或 q1= −π 和 ˙q1= 0。此外，后一种状态是不稳定的，被称为倒立不稳定状态。因此，设计一种控制策略具有重要意义，使得：

16.2 数学模型

由于倒立摆系统是由两个相互作用的物体组成的机构，其数学模型是通过使用欧拉-拉格朗日方程得到的，即

$$
\frac{d}{dt} \left(\frac{\partial L}{\partial \dot{q}_1}\right) - \frac{\partial L}{\partial q_1} = 0, \quad (16.1)
$$
$$
\frac{d}{dt} \left(\frac{\partial L}{\partial \dot{q}_2}\right) - \frac{\partial L}{\partial q_2} = \tau.
$$

第一个表达式右侧的零表示在摆悬挂点处未施加外部转矩。Lagrangian定义为：
$$
L = K - P, \quad (16.2)
$$
$$
K = K_1 + K_2, \quad P = P_1 + P_2,
$$
其中K1和K2分别表示摆和轮的动能，而P1和P2分别表示摆和轮的势能。

由于摆是一个质量沿杆分布的物体，其动能通过以下两部分相加得到：质量为m1的质点沿半径为lc1的圆周平动的动能，以及该杆绕其质心旋转的动能，即：
$$
K_1 = \frac{1}{2} m_1 l_{c1}^2 \dot{q}_1^2 + \frac{1}{2} I_1 \dot{q}_1^2.
$$

轮的动能以类似的方式获得。然而，必须考虑到，从固定在实验室中的观察者测量的角度来看，轮的角速度为 $\dot{q}_1 + \dot{q}_2$。由此可得：
$$
K_2 = \frac{1}{2} m_2 l_1^2 \dot{q}_1^2 + \frac{1}{2} I_2 (\dot{q}_1 + \dot{q}_2)^2.
$$

为了计算势能，将 q1= 0 的位置作为参考，即 P1= 0 和 P2= 0。回顾一下，摆的势能等于作用在摆上的力（即摆的重量）与从当前实际位置 q1 处摆的质心到势能为零时摆的质心位置（即当 q1= 0 时）之间距离在摆的重量方向上的分量的乘积。则：
$$
P_1 = g m_1 l_{c1}(1 - \cos(q_1)).
$$

采用类似的方法，可得轮的势能表达式为：
$$
P_2 = g m_2 l_1(1 - \cos(q_1)).
$$

因此，根据(16.2)：
$$
L = \frac{1}{2} m_1 l_{c1}^2 \dot{q} 1^2 + \frac{1}{2} I_1 \dot{q}_1^2 + \frac{1}{2} m_2 l_1^2 \dot{q}_1^2 + \frac{1}{2} I_2 (\dot{q}_1 + \dot{q}_2)^2 - g m (1 - \cos(q_1)),
$$
其中 $m = m_1 l {c1} + m_2 l_1$。将 L 代入欧拉‐拉格朗日方程(16.1)并适当整理各项后，得到如下结果：
$$
D \begin{bmatrix} \ddot{q} 1 \ \ddot{q}_2 \end{bmatrix} + \begin{bmatrix} m g \sin(q_1) \ 0 \end{bmatrix} = \begin{bmatrix} 0 \ \tau \end{bmatrix}, \quad (16.3)
$$
$$
D = \begin{bmatrix} d {11} & d_{12} \ d_{21} & d_{22} \end{bmatrix} = \begin{bmatrix} m_1 l_{c1}^2 + m_2 l_1^2 + I_1 + I_2 & I_2 \ I_2 & I_2 \end{bmatrix}.
$$

容易验证矩阵D的行列式为正，即：
$$
\det(D) = d_{11} d_{22} - d_{12} d_{21} > 0. \quad (16.4)
$$

式(16.3)中的模型是惯性轮摆最完整的模型，将在下一节中用于设计控制策略，以将摆升至倒立不稳定状态。

施加在驱动轮的永磁有刷直流电机电枢端子上的电压是实际的控制信号， 即可以直接操纵的变量，而轮上的转矩则是该电压作用所产生的变量。然而， 在实际中，如果假设电枢电感很小，则可以认为轮上的转矩是控制信号。永磁有刷直流电机的电枢电路模型如下：

$$
L_a \frac{di}{dt} + R i + k_b \dot{q}_2 = u. \quad (16.5)
$$

如果La和kb被假设为零，则：
$$
\tau = k_m i = \frac{k_m}{R} u. \quad (16.6)
$$

这意味着施加电压对产生的转矩有直接影响；因此，可以合理地假设转矩是控制信号。假设kb为零的理由如下。从(16.5)可以看出，kb为机构引入了额外的粘性摩擦。由于这通常会提高控制系统的稳定性，因此假设kb= 0使得当前的控制问题更具挑战性，因为设计时假设机构本身不存在任何自然的稳定作用。

16.3 摆起非线性控制

本节设计一个控制器，用于将摆驱动至其倒立不稳定状态，即任意一点 $(q_1, \dot{q}_1)=(+\pi,0)$或 $(q_1, \dot{q}_1)=(-\pi,0)$。稍后将解释为何在此任务执行期间不考虑轮的位置和速度。

求解(16.3)中第一行的 $\ddot{q} 2$，将该值代入(16.3)的第二行，并结合(16.6)可得：d12= d22,以下结果：
$$
J \ddot{q}_1 + m g l \sin(q_1) = \tau_1. \quad (16.7)
$$
$$
J = \frac{d {22} d_{11} - d_{21} d_{12}}{d_{12}}, \quad m g l = m g, \quad \tau_1 = - \frac{k_m}{R} u.
$$

注意，由于(16.4)，J> 0。因此，(16.7)中的模型对应于一个简单摆，其中 J、m、l和g分别表示等效惯性、质量、长度和重力加速度。该摆的总能量由其动能和势能相加得到，即：
$$
V(q_1, \dot{q}_1) = \frac{1}{2} J \dot{q}_1^2 + m g l (1 - \cos(q_1)). \quad (16.8)
$$

V的等值面如图16.3所示。这意味着该图中的每条曲线代表了函数$(q_1, \dot{q}_1)$取恒定值的所有点的集合。读者应注意到，越靠外侧的曲线对应的V(q1, ˙q1) 值越大，而最内侧的曲线对应一个单独的点$(q_1, \dot{q})=(0,0)$，此时 V= 0取得最小值。

对V(q1, ˙q1)的时间导数，由式(16.8)给出，计算得：
$$
\frac{d V(q_1, \dot{q}_1)}{dt} = \dot{q}_1 J \ddot{q}_1 + m g l \dot{q}_1 \sin(q_1).
$$

如果 $J \ddot{q}_1$由（16.7）中的表达式替换，则得到以下结果：
$$
\frac{d V(q_1, \dot{q}_1)}{dt} = \dot{q}_1 \tau_1. \quad (16.9)
$$

所计算的是能量V沿(16.7)轨迹的时间导数。这意味着(16.9)表示当(16.7)中的摆在外力矩τ1作用下运动时，$\frac{d}{dt}V$所取的值。例如，如果 τ1= 0 ， 则根据(16.9)， $\frac{d}{dt}V = 0$，因为 τ1= 0，因此随着时间推移，摆仅在代表等值面的点上运动，其中$V(q_1(t), \dot{q}_1(t)) = V(q_1(0), \dot{q}_1(0))$，如图16.3所示。等值面上的箭头表示摆运动的方向。这些运动方向很容易确定。简单摆的状态向量定义为

当$y=[q_1, \dot{q}]^T$时，变量 $\dot{y}=[\dot{q}_1, \ddot{q}]^T$是一个向量，表示状态y的移动方向。注意 $\dot{y}$的水平分量，即 $\dot{q}_1$，只要摆的速度 $\dot{q}_1$为正，就指向右侧。这足以确定等值面V上箭头的方向。

根据上述讨论的思想，要使摆摆起，只需将摆锤能量增加到：
$$
V(q_1, \dot{q} 1)| {q_1=+\pi \text{ 或 } q_1=-\pi, \dot{q}_1=0} = \frac{1}{2} J(0)^2 + m g l (1 - \cos(\pm\pi)) = 2 m g l = V_0,
$$
并且，当达到该能量时，若处于$(q_1, \dot{q}_1) = (+\pi, 0)$或$(q_1, \dot{q}_1) = (-\pi,0)$的状态，则施加一个转矩 $\tau_1$，以确保此后 $\frac{d}{dt}V = 0$。注意，根据图16.3，一旦 V 保持恒定且 V = V0,，则摆将继续运动，直到到达点$(q_1, \dot{q}_1)=(+\pi, 0)$或$(q_1, \dot{q}_1)=(-\pi,0)$之一。接下来将证明，若采用以下转矩即可实现上述控制：
$$
\tau_1 = -k_d \text{sat}(\dot{q}_1) \text{sign}(V - V_0), \quad (16.10)
$$
$$
\text{sat}(\dot{q}_1) =
\begin{cases}
\varepsilon_0, & \dot{q}_1 > \varepsilon_0 \
-\varepsilon_0, & \dot{q}_1 < -\varepsilon_0 \
\dot{q}_1, & |\dot{q}_1| \leq \varepsilon_0
\end{cases},
\quad
\text{sign}(V - V_0) =
\begin{cases}
+1, & V > V_0 \
-1, & V < V_0 \
0, & V = V_0
\end{cases}, \quad (16.11)
$$
其中 $\varepsilon_0$和 $k_d$为任意正的常量。将(16.10)代入(16.9)：
$$
\frac{d V(q_1, \dot{q}_1)}{dt} = -k_d \dot{q}_1 \text{sat}(\dot{q}_1) \text{sign}(V - V_0).
$$

由(16.11)容易看出，乘积 $\dot{q}_1 \text{sat}(\dot{q}_1)$始终为正，且仅当 $\dot{q}_1= 0$时为零。因此，只要 $\dot{q}_1 \neq 0$，就有：
- 如果 $V < V_0$，则 $\frac{d}{dt}V > 0$，且能量 $V$增加。
- 如果 $V > V_0$，则 $\frac{d}{dt}V < 0$，且能量 $V$减小。
- 如果 $V = V_0$，则 $\frac{d}{dt}V = 0$，且能量 $V$保持在 $V_0$。

然后可以确保摆锤能量 $V$ 达到所需的值，从而到达点$(q_1, \dot{q}_1)=(+\pi, 0)$ 或$(q_1, \dot{q}_1)=(-\pi, 0)$之一。此外，能量将保持该值，直到到达这些点之一。现在分析当 $\dot{q}_1= 0$ 时会发生什么。注意，在此条件下摆处于静止状态。然而，要使 $\dot{q}_1= 0$ 恒成立的唯一方式是$q_1= 0$，因为只有在那里摆才能保持静止。因此，有必要在开始时轻微敲击摆，以迫使它离开配置$(q_1, \dot{q}_1)=(0, 0)$。

之前的分析确保了如果摆从向下稳定状态开始，并受到轻微的撞击而开始运动，那么它将到达倒立不稳定状态。不难发现，上述结果均不会改变

如果 $\text{sat}(V - V_0)$在控制器(16.10)中被用于替代 $\text{sign}(V - V_0)$。公式(16.10)、(16.11)中的控制器源于文献[13]中报告的思想。

另一方面，需要强调的是，一旦达到倒立不稳定状态，控制器（16.10）并不能保证摆保持在此状态。这需要设计另一个控制器，当摆接近倒立不稳定状态时投入工作。这意味着从那时起必须断开（16.10）中的控制器。在下一节中，将设计一个控制器来解决控制问题的第二部分：摆的平衡在倒立不稳定状态。

最后，请注意在摆起摆阶段并未提及轮的位置和速度。由于轮是对称且均匀的，因此轮位置无关紧要。另一方面，尽管在此时间段内轮的速度以未知方式增加，但在摆达到倒立不稳定状态时，该速度是有限的；因此，平衡控制器只需将轮的速度调节至该变量在控制器开始工作时所具有的值即可。

例 16.1 考虑替代控制器：
$$
\tau_1 = -k_d(V - V_0) \text{sat}(\dot{q} 1), \quad u = - \frac{R}{k_m} \tau_1, \quad (16.12)
$$
$$
\text{sat}(\dot{q}_1) =
\begin{cases}
d, & \dot{q}_1 > d \
-d, & \dot{q}_1 < -d \
\dot{q}_1, & |\dot{q}_1| \leq d
\end{cases}, \quad d = \frac{u {max} k_m}{k_d R V_0}, \quad (16.13)
$$
其中 $d$, $u_{max}$和 $k_d$是正的任意常数。将 (16.12) 代入 (16.9)：
$$
\frac{d V(q_1, \dot{q}_1)}{dt} = -k_d(V - V_0) \dot{q}_1 \text{sat}(\dot{q}_1).
$$

由 (16.13) 可知，乘积 $\dot{q}_1 \text{sat}(\dot{q}_1)$始终为正，且仅当 $\dot{q}_1= 0$ 时为零。因此，只要 $\dot{q}_1 \neq 0$，我们有以下结论：
- 如果 $V < V_0$，则 $\frac{d}{dt}V > 0$，且能量 $V$增加。
- 如果 $V > V_0$，则 $\frac{d}{dt}V < 0$，且能量 $V$减少。
- 如果 $V = V_0$，则 $\frac{d}{dt}V = 0$，且能量 $V$保持在 $V_0$。

因此，再次确保摆锤能量 $V$达到所需值，以到达点$(q_1, \dot{q}_1)=(+\pi,0)$或$(q_1, \dot{q}_1)=(-\pi, 0)$。

16.4 平衡控制器

平衡摆的策略是基于对（16.3）的线性近似而设计的线性状态反馈控制器。该线性近似如下获得。注意（16.3）中的模型不依赖于轮位置 $q_2$。因此，状态向量可以定义为：
$$
x = \begin{bmatrix} x_1 \ x_2 \ x_3 \end{bmatrix} = \begin{bmatrix} q_1 \ \dot{q} 1 \ \dot{q}_2 \end{bmatrix}, \quad (16.14)
$$
由(16.3)可得：
$$
\begin{bmatrix} \ddot{q}_1 \ \ddot{q}_2 \end{bmatrix} = D^{-1} \left{ -\begin{bmatrix} m g \sin(q_1) \ 0 \end{bmatrix} + \begin{bmatrix} \tau \ 0 \end{bmatrix} \right}, \quad (16.15)
$$
$$
D^{-1} = \begin{bmatrix} d {11} & d_{12} \ d_{21} & d_{22} \end{bmatrix} = \frac{1}{d_{11} d_{22} - d_{12} d_{21}} \begin{bmatrix} d_{22} & -d_{12} \ -d_{21} & d_{11} \end{bmatrix}. \quad (16.16)
$$

注意，$\det(D^{-1}) \neq 0$，因为它是非奇异矩阵的逆矩阵。利用(16.14)和(16.15)，式(16.3)中的模型可写为：
$$
\dot{x} = f(x, \tau) = \begin{bmatrix} x_2 \ -d_{11} m g \sin(x_1) + d_{12} \tau \ -d_{21} m g \sin(x_1) + d_{22} \tau \end{bmatrix}. \quad (16.17)
$$

根据第7.3.2节，式(16.17)的工作点$(x^ , \tau^ )$由条件$f(x^ , \tau^ )= 0$确定，即：
$$
\begin{bmatrix} x^ 2 \ -d {11} m g \sin(x^ 1) + d {12} \tau^ \ -d_{21} m g \sin(x^ 1) + d {22} \tau^* \end{bmatrix} = \begin{bmatrix} 0 \ 0 \ 0 \end{bmatrix}. \quad (16.18)
$$

显然：
$$
x^*_2 = 0. \quad (16.19)
$$

另一方面，由(16.18)的第二行和第三行可得：
$$
\tau^ = \frac{d_{11} m g \sin(x^ 1)}{d {12}}, \quad (16.20)
$$
$$
\tau^ = \frac{d_{21} m g \sin(x^ 1)}{d {22}}.
$$

同时求解这些表达式：
$$
m g \frac{d_{12} d_{22}}{}[d_{11} d_{22} - d_{21} d_{12}] \sin(x_1^*) = 0. \quad (16.21)
$$

As $\det(D^{-1}) = d_{11} d_{22} - d_{21} d_{12} \neq 0$，则(16.21)仅在以下情况下成立：
$$
x^*_1 = \pm n\pi, \quad n = 0, 1, 2, 3, … \quad (16.22)
$$

将此条件代入（16.20）中的任意表达式，可得：
$$
\tau^* = 0. \quad (16.23)
$$

最后，由于$x^ _3$不存在限制，因此得出结论：
$$
x^ 3 = c, \quad (16.24)
$$
其中 $c$为任意实常数。根据第7.3.2节，在工作点(16.17)、(16.19)、(16.22)、(16.23)、(16.24)处，且$n= \pm1$时的线性近似为：
$$
\dot{z} = A z + B w, \quad (16.25)
$$
$$
A = \frac{\partial f(x, \tau)}{\partial x} \bigg| {x^ , \tau^ } = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \frac{\partial f_1}{\partial x_3} \ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \frac{\partial f_2}{\partial x_3} \ \frac{\partial f_3}{\partial x_1} & \frac{\partial f_3}{\partial x_2} & \frac{\partial f_3}{\partial x_3} \end{bmatrix} \bigg| {x^ , \tau^ } = \begin{bmatrix} 0 & 1 & 0 \ \frac{d {11} m g}{0} & 0 & 0 \ \frac{d_{21} m g}{0} & 0 & 0 \end{bmatrix},
$$
$$
B = \frac{\partial f(x, \tau)}{\partial \tau} \bigg| {x^ , \tau^ } = \begin{bmatrix} \frac{\partial f_1}{\partial \tau} \ \frac{\partial f_2}{\partial \tau} \ \frac{\partial f_3}{\partial \tau} \end{bmatrix} \bigg| {x^ , \tau^ } = \begin{bmatrix} 0 \ d_{12} \ d_{22} \end{bmatrix},
$$
其中：
$$
z = x - x^ , \quad (16.26)
$$
$$
w = \tau - \tau^ , \quad (16.27)
$$
保持较小，即 $z \approx 0, w \approx 0$。另一方面，回顾 $\tau^* = 0$且因此，$w = \tau$。然后，可利用(16.6)得到(16.25)的如下修正版本：
$$
\dot{z} = A z + B u, \quad (16.28)
$$
$$
B = \begin{bmatrix} 0 \ d_{12} \ d_{22} \end{bmatrix} \frac{k_m}{R}.
$$

该近似线性模型将用于设计平衡控制器。

首先，检查(A,B)这对是否可控。这需要计算可控性矩阵$C=[B|AB|A^2B]$，的行列式，其结果为：
$$
\det(C) = \frac{m g k_m^3}{R^3} d_{12}^2 (d_{11} d_{22} - d_{12} d_{21}) \neq 0. \quad (16.29)
$$

因此，可以得出结论：(A,B)这对是可控的。这意味着总可以找到一个增益向量K，使得闭环矩阵(A −BK)的所有特征值被配置在任意指定的位置。这通过以下控制器实现：
$$
u = -K z. \quad (16.30)
$$

增益向量K的计算需在矩阵A和B中涉及的参数的数值已知后进行。控制器(16.30)使得能够在由工作点(16.19)、(16.22)、(16.23)、(16.24)定义的倒立不稳定状态下平衡IWP，其中$n= \pm1$。需要强调的是，这要求机构的状态向量x在控制器(16.30)开始工作时足够接近该不稳定状态。

最后，需要说明的是，公式(16.24)中引入的常数c取值为控制器(16.30)开始工作时轮的速度。由于该速度可能取任意值，因此关键在于c可被定义为任意实常数。

16.5 原型构建和参数识别

图16.4展示了所搭建的惯性轮摆的示意图，其中显示了主要组件。部分组件用于成功组装机械部件。此外还包括一个增量编码器和一个转速表，分别用于测量摆的位置$q_1$和轮的速度 $\dot{q}_2$。所用部件的尺寸见表16.1。

表16.1 简单参数

符号	描述	值	单位
l1	金属板长度	0.117	m
asol	金属板宽度	0.0254	m
msol	金属板质量	0.016	Kg
mcar	执行器定子质量	0.03	Kg
msop	测速计支架质量	0.01375	Kg
mrot	执行器转子质量	0.02	Kg
rrot	执行器转子半径	0.00915	m
mrue	轮质量	0.038	Kg
rrue	轮半径	0.0189	m
R	电枢电阻	4.172	Ohm
L	电枢电感	0.0009	H
kb	反电动势常数	0.00775	伏秒/弧度
km	转矩常数	0.00775	Nm/A

摆的等效质量 $m_1$ 定义为：
$$
m_1 = m_{sol} + m_{car} + m_{sop} = 0.05975[\text{Kg}]. \quad (16.31)
$$

为了计算包含所有组件的摆的质心，采用平衡原理，即支点处所有力矩之和为零。因此，当在金属板另一端距离 $l_{c1}$处施加一个等效重量时，图 16.5中的组件必须达到平衡。利用(16.31)式以及表 16.1中的数据，可得：
$$
l_{c1} = \frac{m_{sol} \frac{l_1}{2} + m_{car} l_1 + m_{sop} l_1}{m_1} = 0.10133[\text{m}]. \quad (16.32)
$$

质量为 $m$、宽度为$b$、长度为 $d$的矩形厚板，当其绕垂直于平面且通过几何中心的x轴旋转时，其转动惯量由下式给出：[14, 15],第271页：
$$
I_x = \frac{1}{12} m(b^2 + d^2). \quad (16.33)
$$

使用(16.33)和表16.1中的数据，可得到转动惯量$I^ $为：
$$
I^ = \frac{1}{12} m_{sol}(a_{sol}^2 + l_1^2) = 0.0000222[\text{Kg m}^2]. \quad (16.34)
$$

然而，转动惯量 $I_1$ 是相对于通过摆的质心的轴定义的，该质心位于距离矩形中心为 $l_{c1} - l_1/2$处，矩形的边长为 $b$ 和 $d$。根据 平行轴定理或施泰纳定理 [14, 15], 第 272 页：
$$
I_1 = I^* + m_{sol}(l_{c1} - \frac{l_1}{2})^2 = 0.000048463[\text{Kg m}^2].
$$

另一方面，一个质量为 $m$、半径为 $r$ 的圆盘，其旋转轴 $x$ 垂直于圆盘且通过其几何中心时，转动惯量为 [14, 15], pp. 271:
$$
I_x = \frac{1}{2} m r^2. \quad (16.35)
$$

使用表16.1中的数据以及公式(16.35)可得，轮的转动惯量$I_{rue}$为：
$$
I_{rue} = \frac{1}{2} m_{rue} r_{rue}^2 = 0.000006787[\text{Kg m}^2]. \quad (16.36)
$$

质量为$m$、半径为$a$、高度为$l$的圆柱体，当其旋转轴$x$通过其纵轴时，其转动惯量为[14, 15],第271页：
$$
I_x = \frac{1}{2} m a^2. \quad (16.37)
$$

使用公式(16.37)中的表达式以及表 16.1中的数据，可求得执行器转子的转动惯量 $I_{rot}$ 为：
$$
I_{rot} = \frac{1}{2} m_{rot} r_{rot}^2 = 0.0000008372[\text{Kg m}^2]. \quad (16.38)
$$

等效轮质量 $m_2$ 定义为：
$$
m_2 = m_{rot} + m_{rue} = 0.058[\text{Kg}]. \quad (16.39)
$$

使用(16.36)和(16.38)，可得等效轮转动惯量$I_2$为：
$$
I_2 = I_{rue} + I_{rot} = 0.0000076242[\text{Kg m}^2]. \quad (16.40)
$$

最后，利用表16.1中的数据，以及公式(16.34)、(16.31)、(16.32)、(16.39)、(16.40)和$g= 9.81[\text{m/s}^2]$，中的参数，得到模型(16.3)参数的以下数值：
$$
d_{11} = 0.0014636, \quad d_{12} = 0.0000076, \quad (16.41)
$$
$$
d_{21} = 0.0000076, \quad d_{22} = 0.0000076, \quad mg = 0.12597.
$$

值得一提的是，在计算$I_2$时，测速计质量被视为测速计支架质量的一部分，且忽略了测速计转子的影响。这是因为其质量小于0.0001[Kg]，半径小于0.001[m]。另一方面，在计算惯性$I_1$时，固定骰子的螺丝、骰子本身以及增量式编码器轴被忽略。这是因为这些质量的总和小于0.008[kg]，更重要的是，它们位于摆悬挂的位置，这大大降低了它们的惯性影响。最后，连接测速计与轮的电线和塑料管的质量也被忽略。

Example 16.2 在控制IWM（16.3）时，采用（16.41）中的数值参数，使用 （16.10）中的控制器完成起摆任务，并使用（16.30）中的控制器将摆平衡在倒立状态，以下展示了相应的仿真结果。控制器之间的切换条件为：
$$
\text{If: } z_1^2 + z_2^2 \leq 0.01, \text{ controller in (16.30) is on, else: controller in (16.10) is on,}
$$
其中$z_1$和$z_2$由(16.26)定义。所有初始条件均设为零，除了 $\dot{q}_1(0) = 0.00001[\text{rad/s}]$。控制器(16.10)设置为$k_d= 20$。控制器(16.30)的增益向量K被选择，使得闭环矩阵A−BK的特征值被配置在 $-9.27\pm20.6j$和 $-0.719$处。这些仿真使用图16.6中的MATLAB/Simulink框图进行，该框图包含四个主要模块，其代码列于附录G.2中。在运行图16.6中的仿真之前，必须执行附录 G.2.5中的代码，以对所有参数赋数值。

从图16.7中可以观察到，摆向上摆动并最终在其倒立不稳定状态达到平衡。此外，在整个仿真过程中，轮的速度保持有界。还应注意，施加电压保持在合适范围内，即介于 $-13[\text{V}]$和 $+13[\text{V}]$之间，最后，摆锤能量收敛到其期望值 $V_0$。因此，这些仿真结果验证了上述理论结果。

16.6 控制器实现

使用第16.5节中的数值，$k_d= 1$并根据(16.7)、(16.10)，可得起摆控制器为：
$$
V = C_1 \dot{q} 1^2 + m g (1 - \cos(q_1)), \quad C_1 = \frac{1}{2} J, \quad V_0 = 2 m g, \quad u = C_2 \text{sat}(\dot{q}_1) \text{sign}(V - V_0), \quad C_2 = \frac{R k_d}{k_m}, \quad \varepsilon_0 = \frac{u {max} k_m}{k_d R}, \quad (16.42)
$$
其中$u_{max}= 13[\text{V}]$，而常量$C_1$和$C_2$在初始化变量时仅计算一次，以减少实现所需的乘法次数。

另一方面，根据第7章的第7.14.1和7.14.2节，发现线性控制器(16.30) 与增益向量$K = [-340, -11, -0.0085]$ 可将矩阵 $(A - BK)$ 的特征值配置在： $\lambda_1 = -5.8535 + 177192j$, $\lambda_2 = -5.8535 - 177192j$, $\lambda_3 = -0.5268$。因此，控制器(16.30) 表示为：
$$
u = -k_1(q_1 - q_{1d}) - k_2 \dot{q} 1 - k_3(\dot{q}_2 - \dot{q} {2d}), \quad (16.43)
$$
其中使用 $q_{1d}=+\pi$或 $q_{1d}= -\pi$中的任意一个，具体取决于 $q_1$首先达到哪一个。请记住， $\dot{q}_{2d}= c$是控制器（16.43）开始工作时轮的速度。

定义条件：
$$
(q_1 - q_{1d})^2 + \dot{q} 1^2 < \delta, \quad (16.44)
$$
对于某个足够小的常数 $\delta > 0$且$q {1d}=+\pi$或$q_{1d}= -\pi$。该条件表明摆接近于点$(q_1, \dot{q}_1)=(+\pi, 0)$或$(q_1, \dot{q}_1)=(-\pi,0)$之一。在后续呈现的实验中， 已使用了 $\delta= 0.3$。

用于摆起和平衡IWP的控制策略是采用(16.42)，直到满足(16.44)，此后关闭(16.42)并开启(16.43)。此任务通过Microchip PIC18F4431微控制器实现。接下来将说明如何完成这一过程。本部分并非旨在详细解释该微控制器的工作原理，而仅简要描述如何使用该微控制器来实现控制策略(16.42)、(16.43)和(16.44)。

本章末尾给出了用于编程微控制器的完整代码。微控制器PIC18F 4431[16]的最大时钟频率为40 MHz。在本章中，使用了11‐MHz晶体。共有五个可编程输入‐输出端口、三个16位定时器、一个8位定时器（用于确定采样周期$T_s= 0.01[\text{s}]$），以及九个10位模拟/数字通道（测速电压通过通道 0输入）。此外，还有八个14位PWM通道，其中CCP1被设置为以8位工作， 以提供施加到执行器（永磁有刷直流电机）的电压。

位置 $q_1$使用 Computer Optical Products 公司的增量编码器型号 CP‐360‐S 进行测量。[17]该设备连接到微控制器后，分辨率为 1440 ppr。增量编码器的脉冲通过微控制器的 5 和 6 引脚读取（见图16.8）。该增量编码器计数值存储在两个8位字节中，分别称为 POSCNTH（高有效位）和 POSCNTL（低有效位）。实际计数值计算如下：
$$
\text{pos} = 256 \times \text{POSCNTH} + \text{POSCNTL},
$$
指令“q1=(signed long)pos”将编码器计数的数值（包含相应符号，即沿 图16.2所示方向运动时为正，否则为负）赋给关节1位置。最后，在执行以下操作时，位置$q_1$以弧度为单位给出：
$$
q_1 = \frac{2\pi}{1440} q_1, \quad \frac{2\pi}{1440} = 0.004363.
$$

值得指出的是，根据图$q_1= 0$中的约定，当摆处于静止状态时，必须开启整个控制系统16.2。摆的速度 $\dot{q}_1$通过摆的位置 $q_1$的数值微分计算得出，即：
$$
\dot{q}_1 = \frac{q_1(k) - q_1(k -1)}{T_s},
$$
其中$k$表示离散时间， $\dot{q}_1$表示以弧度/秒为单位的摆的速度的估计值，而$T_s= 0.01[\text{s}]$是采样周期。因此，$q_1(k)$表示在当前采样时刻测得的$q_1$的值， 而$q_1(k -1)$表示在上一采样时刻测得的$q_1$的值。

轮的速度 $\dot{q}_2$ 使用测速发电机型号34PC进行测量。它是一种微型永磁有刷直流电机，常用于手机中的蜂鸣器。测速发电机输出的电压通过图16.8所示的RC电路进行低通滤波。已知当轮的速度最大时，即 $1634[\text{rad/s}]$，该测速发电机输出 $0.58[\text{V}]$ 。假设在任何其他中间速度下，该电压均成正比。为了提高此速度测量的分辨率，采用连接到引脚4的分压器（见 图16.8），将模数转换器通道0的模拟范围最小值设定为3.8372[V]（最大值固定为 $5[\text{V}]$）。另一方面，测速发电机与另一个分压器串联连接，使得当测速发电机静止时，即 $\dot{q}_2= 0$，通道0上的电压为 $(5-3.8372)/2+3.8372=4.4186[\text{V}]$。注意，在模数转换器模拟范围中点上下允许约0.58[V],，即 $1634[\text{rad/s}]$, 的变化。因此，当 $\dot{q}_2= 1634[\text{rad/s}]$ 时，模数转换器输出计数值 $1023= 2^{10}-1$（模数转换器为10位），而当 $\dot{q}_2= 0[\text{rad/s}]$ 时，模数转换器输出计数值 $1023/2= 511$。小于511的计数值表示负速度 $\dot{q}_2$。根据这些信息，轮的速度计算如下：指令“vel=read-adc()-511”将对应于轮速度的代码赋给变量“vel”，而指令“q2_p=(signed long)vel”和“q2_p=3.2039*q2_p”将一个数值上等于

轮的速度 $\dot{q}_2$，单位为弧度/秒，包含符号。注意，常数 $3.2039=1634/510[(\text{弧度/秒})/\text{计数}]$表示测量系统增益。

根据这些信息，可计算出(16.42)或(16.43)中的任意表达式。得到的变量 u将按照下文所述发送至微控制器PWM。

通过以下方式实现（16.42）中的饱和函数，可减少计算量。已知 $u$的取值范围仅为 $[-13,+13][\text{V}]$；因此，可通过如下指令将它的值限制在此范围内：

if u > 13 then u = 13, if u < -13 then u = -13.

由于PWM分辨率为$2^8 - 1= 255$ counts，因此必须将以下值发送到微控制器PWM，以正确地将u传递给功率放大器：
$$
\text{cuenta} = \frac{255}{13} \text{abs}(u),
$$
其中 $\text{abs}(u)$表示 $u$ 的绝对值。 $u$的符号存储在两个控制位中。PWM信号被施加到一个H桥（SGS‐汤姆逊微电子公司的L293B），该H桥适当地放大 PWM信号的功率。该H桥由一个15.3[V]电源供电。然而，根据该器件的数据手册[18],，电压降将输出电压信号限制在 $\pm13[\text{V}]$以内。该器件有两个位， 用于指示待输出功率电压的极性。因此，u的符号通过微控制器端口D的位0和位1发送至H桥。这意味着输送给电机的平均功率电压范围为$[-13,+13][\text{V}]$。因此，可确保施加在电机端子上的电压u的平均值在数值上等于通过表达式(u或(16.42)或(16.43)中任意一个计算出的值。用于实现(16.42)、(16.43)、(16.44)中控制策略的电路图如图16.8所示。

16.7 实验结果

所提出的控制方案的实验结果如图16.9、16.10、16.11、16.12和16.13所示。需要记住的是，必须在开始时对摆施加轻微的冲击，以离开初始构型$(q_1, \dot{q}_1)=(0,0)$。尽管这可以通过在开始时向执行器发送一个小电压脉冲来替代， 但这一操作并未被编程实现。

在图16.9中，可以观察到摆以不断增大的振幅振荡，直到位置$q_1= -\pi$处以零速度到达，在$t \approx 14[\text{s}]$时，此后摆保持在此位置。在图16.10中，可以观察到

轮的速度在 $380[\text{弧度/秒}]$从$t \approx 17[\text{s}]$开始保持恒定。值得注意的是，轮的速度被指令调节至$c \approx 150[\text{弧度/秒}]$,，即在 $t \approx 14[\text{s}]$,时的轮速度，也就是控制器（16.43）开始工作时的速度。$c$ 与最终轮速度 $380[\text{弧度/秒}]$之间的差异是由于轮轴处存在的摩擦所致，而该摩擦在设计阶段未被考虑。

在图16.11中，显示了控制信号$u$。注意，该变量在摆起阶段的大部分时间内都处于饱和状态（$t< 14[\text{s}]$）。这就是为什么控制器（16.42）中使用了饱和和符号函数的原因。实际上， $\varepsilon_0= 0.024$ 被选择为使得在此值和系数$4.172/0.00775$ 的作用下，$u$ 在 $\pm13[\text{V}]$ 处达到饱和。

在图16.12中，展示了摆如何在图16.3和第16.3节定义的平面上运动。显然，通过比较两幅图可以看出，摆的运动使其能量$V$增加（另见图16.13），直到$V = V_0= 0.2519$。注意，$V$在$t \approx 11[\text{s}]$,时达到该值，但摆保持平衡直到$t \approx 14[\text{s}]$。这表明能量被调节至$V = V_0$，直到达到点$(q_1, \dot{q}_1)=(-\pi, 0)$为止。显然，摆也在$t \approx 13[\text{s}]$时经过点$(q_1, \dot{q}_1)=(+\pi,0)$附近，但并未平衡，可能是因为当时条件(16.44)未满足，即摆未足够接近该点。

最后，在图16.14中，展示了IWP在倒立不稳定状态下保持平衡的图片。

16.8 PIC18F4431微控制器编程

有关以下程序中出现的每条指令的详细说明，请参阅 [19]。相应的流程图如图16.15所示。

#include<18f4431.h>
#device adc=10 //adc，10 位 
#include< stdlib.h>
#include<math.h>
#fuses HS, NOWDT,NOPROTECT,PUT,NOLVP,NOBROWNOUT,NOWRTC, MCLR,SSP_RC_
#define pi_ 3.14159 //非线性控制器增益 
#define kd 1.0 //线性控制器增益 
#define k1 340 
#define k2 11 
#define k3 0.0085 //参数 
#define mbg 0.12597 
#define R 4.172
#define L 0.0009 
#define Kb 0.00775 
#define Km 0.00775 
#define 阻尼系数d11 0.0014636 
#define 阻尼系数d12 0.0000076
#define 阻尼系数d21 0.0000076 
#define 阻尼系数d22 0.0000076
#define delta 0.3 
#define ts 0.01 //使用定时器=108，每次计数=(4/FXtal)*256秒
/*寄存器地址*/ 
#use delay(clock=11000000) //时间基准 (11 MHz) 
#byte porta = 0xf80 //端口地址 
#byte portb = 0xf81
#byte 端口C寄存器 = 0xf82 
#byte 端口D寄存器 = 0xf83 
#byte 端口D寄存器 = 0xf84
#byte TMR0H = 0xfd7 
#byte TMR0L = 0xfd6 
#byte 定时器 0控制寄存器 = 0xfd5
#byte INTCON= 0xff2 
# byte TMR5H=0xf88 //高编码器计数
#byte TMR5L=0xf87 //低编码器计数
#byte 正交编码模块配置寄存器=0xfB6 //config. quadrature module
#byte T5CON=0xfB7 //Config. 定时器5
#byte POSCNTL=0xF66 //CAP2BUFL（低位计数寄存器） 
#byte POSCNTH=0xF67 //CAP2BUFH（高位计数寄存器） 
#byte CAP1CON=0xF63 //配置复位时间基准寄存器
#byte PIE3=0xFA3 
/*位地址 */ 
#bit VCFG1=0xfc1.7//ADC位配置
#bit PD0=0x0f83.0//控制逆时针H桥 
#bit PD1=0x0f83.1 //控制顺时针H桥 
#bit PD2=0x0f83.2 //板上 LED 
#bit PD3=0x0f83.3//板上LED 
/*变量声明*/
浮点数 u,q1,q1_p,q1_1,q2_p,q1_d,q2_pd,gr,V,S,nf,i_ts, Je2,V0,RKdeKm; 
长整型 pos,vel,inter; 
整型 pwm; 
有符号整型 n,mod; 
/*主程序*/ 
空主函数 {
    设置端口A方向寄存器(0b11111111); 
    //配置I/O端口 
    设置端口B方向寄存器(0b00000000);
    设置端口C方向寄存器(0b10000000);
    设置端口D方向寄存器 (0b00000000);
    端口B=0X00; 
    端口C=0X00; 
    端口D=0X00; 
    //配置PWM 
    setup_ccp1(CCP_PWM); 
    setup_ccp2(CCP_PWM); 
    setup_timer_2(T2_DIV_BY_16,255,1); 
    // 配置编码器读取 
    正交编码模块配置寄存器=正交编码模块配置寄存器 | (0b00010101); 
    正交编码模块配置寄存器=正交编码模块配置寄存器 & (0b01110101); 
    T5CON=T5CON | (0b00011001); 
    T5CON=T5CON & (0b10011101); 
    CAP1CON=CAP1CON | (0b01001111);
    setup_timer_1(T1_INTERNAL | T1_DIV_BY_8); 
    TMR5L=0; 
    TMR5H=0; 
    POSCNTL=0; 
    POSCNTH=0; 
    // 配置ADC
    setup_port_a(sAN0); 
    setup_adc(ADC_CLOCK_INTERNAL); 
    设置ADC通道(0); 
    _delay_us(50); 
    VCFG1=1; 
    //‐Vref=AN2 
    INTCON=0;//关闭中断 
    //配置定时器0 
    TMR0L=0; 
    T0CON=0xC7; 
    //初始化变量 
    i_ts=1/ts;
    _Je2= (d11*d22-d12*d21)/(2*d12); 
    V0=2*mbg; 
    RKdeKm= R*Kd/Km; 
    q1=0.0; 
    q1_1=0.0; 
    q2_pd=0.0; 
    while (TRUE) {
        PD3=1; 
        q1_1=q1; 
        // q1(k-1) 
        pos=POSCNTH; 
        pos=pos<<8;
        pos=pos+POSCNTL; 
        q1=(signed long)pos; 
        q1=0.004363 * q1; 
        //关节1位置，单位为弧度 
        q1_p=(q1-q1_)/ts;//关节速度 
        vel=read_adc()-511; 
        //轮的速度 
        q2_p=(signed long)vel; 
        q2_p=3.2039*q2_p; 
        //轮的速度，单位为弧度/秒 
        //期望的 q1值 
        n=q1/pi_; 
        nf=(float)n; 
        mod=n%2; 
        if(nf==0) 
            if(q1>=0)
                //期望值 
                q1_d=pi_; 
            else 
                q1_d=-pi_; 
        if((nf!=0)&&!PD2) {
            if(q1>=0) 
                //期望值 
            {
                if(mod==0) 
                    q1_d=pi_*(nf+1.0); 
                else 
                    q1_d=pi_*nf; 
            } 
            else 
            {
                if(mod==0) 
                    q1_d=pi_*(nf-1.0); 
                else 
                    q1_d=pi_*nf;
            }
        }
        //定义每个控制器的操作区域 
        if((q1-q1_d)*(q1-q1_d)+ q1_p*q1_p < delta) {
            PD2=1; 
            //线性控制器 
            u= k1*(q1-q1_d)+ k2*q1_p + k3*(q2_p-q2_pd); 
        } else {
            PD2=0; 
            //非线性控制器 
            V=Je2*q1_p*q1_p+mbg*(1-cos(q1)); 
            V1_V-V0; 
            S=0; 
            //符号函数 
            if(V>0) 
                S=1.0; 
            if(V<0) 
                S=-1.0; 
            u=RKdeKm*q1_p*S; 
            //非线性控制器结束 
            q2_pd=q2_p; 
            //轮期望速度 
        }
        //饱和 13V(15.3 V ‐ 2.3 V drop at H桥= 13 V) 
        if(u>13) 
            u=13; 
        if(u<-13) 
            u=-13; 
        //输出缩放，pwm 8位，13 V 
        u=19.6*u; 
        //旋转方向 
        if(u>=0) {
            PD0=1; 
            PD1=0; 
        } else {
            PD0=0; 
            PD1=1; 
        }
        //发送pwm 
        pwm=(unsigned int)abs(u); 
        set_pwm1_duty(pwm); 
        //pwm 
        PD3=0; 
        //控制计算结束 
        while(TMR0L<108);//每次计数 =(4/FXtal)*256 sec 
        TMR0L=0; 
        T0CON=0xC7; 
    }
    //关闭无限循环 
}
//关闭main

16.9 总结

在本章中，介绍了另一种欠驱动非线性系统：倒立摆系统。已获得其数学模型，并使用了两个控制器来解决两个不同的控制问题：起摆控制问题和平衡控制问题。其中，平衡控制问题通过线性状态反馈控制器解决，而起摆控制问题则通过非线性控制器解决。这样做的目的是向读者介绍非线性控制系统的研究。该机构的所有参数均已识别，并且完整的控制系统已在实验中进行了测试。同时也详细描述了机构构造。

16.10 复习题

1. 为什么惯性轮摆是一个欠驱动机构？
2. 为什么IWP在到达倒立位置之前必须振荡多次？
3. 图[16.8中所示的转速表电阻‐电容滤波器的截止频率是多少，单位为[弧度/秒]和 13[Hz]？
4. 在执行起摆任务时， 图13[16.11中的电压幅值为何保持在{v33}V{v36}和‐{v39}V{v42}之间？