基于自激原理的负载倒立摆的摆起与稳摆

Swing-Up and Stability Control Method of Loaded Inverted Pendulum Based on Self-Excitation Principle

摘要: 本文提出了基于自激原理的倒立摆摆起方法，推导出了自激摆起的幅值条件和相位条件，对能量摆起法进行了一定程度上的优化。将此方法应用于负载倒立摆系统，获得了很好的摆起效果。同时本次研究中还使用了线性二次型最优控制算法(LQR法)、Q学习法实现负载倒立摆的稳摆控制。通过仿真与实物实验，证实该方案在负载倒立摆控制中具有稳定，高效，硬件负担小，可重复性强的优点，为大负载倒立摆的摆起与实际应用打下了良好的基础。

Abstract: This paper proposed a swing-up method of inverted pendulum which was based on self-excitation principle, and deduced the amplitude and phase conditions. This method could improve the effect of energy swing-up method, and performed well in loaded inverted pendulum system. This paper also used the linear quadratic optimal control algorithm (LQR method) and Q-learning for stability control. According to the result of simulation and experiment, it showed that the scheme has fol-lowing advantages: good stability, high efficiency, small hardware burden and strong repeatability. The method lays a good foundation for the practical application in swing-up controlling of the heavy-loaded inverted pendulum system.

1. 引言

倒立摆系统是一个典型的多变量、非线性、强耦合的自然不稳定系统，该系统的控制策略对许多实际应用问题具有很好的参考意义 [1]。负载倒立摆是倒立摆系统的一种衍生模型，该模型在普通一级倒立摆摆杆末端增加一个负载重物。负载倒立摆系统是针对一些特殊控制问题而提出的，其控制难度相对更高。目前为止，控制学领域对该模型的研究主要集中于两类问题，一是该模型对欠驱动起重问题的理论参考，如桥式起重机的物资运输等问题 [2]，二是轻质变形体负载倒立摆模型的稳摆控制研究，该成果主要应用于足式机器人的步行问题 [3]。

本文以自激原理为启发，提出一种刚体负载倒立摆模型的摆起控制思路，其理论可以为空间飞行器、地外卫星、月球探测器等航天器设备的雷达天线控制问题 [4] 提供改进策略，也能为轮足式机器人的躺倒再起问题提供另一种解决方案。

2. 负载倒立摆的数学建模

一级负载倒立摆的系统由计算机、A/D、D/A、放大器、电机、编码器及一些机械构件组成。电机是主要执行单位，编码器是测量元件，实时获取小车位置信号和摆杆角度信号。倒立摆系统的整套机械部件固定在滑轨上，摆杆前端装有配重。

2.1. 力学模型

首先将负载一级倒立摆系统进行抽象建模，如图1所示。

Figure 1. Diagram of single pendulum with weight

图1. 一级负载倒立摆示意图

设重力加速度为g，小车所受控制力为F，小车质量为M，重物质量为Ms，摆杆质量为Ml，小车距原点的横坐标为x，摆长为l，摆角为 $\theta$。

对摆杆质量Ml与重物质量Ms分两种情况讨论：1) 重物质量远大于摆杆质量，在理想状态下，将摆杆与重物视作一个重为Ms，长为l的刚性摆锤；2) 重物质量与摆杆质量接近，将重物与摆杆抽象转换为一个质量为Ml + Ms，质心距离小车l/2的刚体长杆，设整个摆杆体系质量为m，摆杆体系的质心距离为L，则L与m符合公式(l)，公式(2)。

$\{\begin{array}{l}L=l;Ms\gg Ml\\ L=l/2;Ms\approx Ml\end{array}$ (1)

$\{\begin{array}{l}m=Ms;Ms\gg Ml\\ m=Ml+Ms;Ms\approx Ml\end{array}$ (2)

实际操作中由于硬件状况，重物质量Ms存在一定限制，这与摆杆的材质、摆幅、甚至与小车及系统其它各硬件的物理性能有关。本次研究中负载质量要控制在摆杆质量的3倍以内，综合各方因素考虑，保持负载质量为3 kg，摆杆质量为1.4 kg。

接下来进行推导，忽略小车所受摩擦和摆杆形变，则根据牛顿力学可得公式(3)，

$\{\begin{array}{l}F=\left(M+m\right)\ddot{x}-mL\left(\ddot{\theta }L\cos \theta -{\stackrel{\dot{}}{\theta }}^2\sin \theta \right)\\ g\sin \theta =\ddot{x}\cos \theta -\ddot{\theta }L\end{array}$ (3)

令 $\sin \theta =\theta$， $\cos \theta =1$， ${\sin }^2\theta =0$， ${\stackrel{\dot{}}{\theta }}^2\sin \theta =0$ 作线性化处理，将 $\ddot{x},\ddot{\theta }$ 作为因变量，得到公式(4)。

$\{\begin{array}{l}\ddot{x}=\frac{F}{M}-\frac{m}{M}g\theta \\ \ddot{\theta }=\frac{F}{Ml}-\frac{M+m}{Ml}g\theta \end{array}$ (4)

令 $x_1=x$， $x_2=\stackrel{\dot{}}{x}$， $x_3=\theta$， $x_4=\stackrel{\dot{}}{\theta }$，得到公式(6)的状态方程形式，即公式(5)。

$\left[\begin{array}{c}{\stackrel{\dot{}}{x}}_1\\ {\stackrel{\dot{}}{x}}_2\\ {\stackrel{\dot{}}{x}}_3\\ {\stackrel{\dot{}}{x}}_4\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& -\frac{m}{M}g& 0\\ 0& 0& 0& 1\\ 0& 0& -\frac{M+m}{ML}g& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{M}\\ 0\\ \frac{1}{ML}\end{array}\right]F$ (5)

状态方程中的输出y为小车的位置x与摆杆的角度 $\theta$，令 $y=\left[x,\theta \right]$，得到公式(6)。公式(5)与公式(6)共同组成了负载倒立摆的物理状态方程。

$y=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]$ (6)

2.2. 电机模型

本研究中系统还存在电机等设备，需要在建模中体现。设电机电压大小为U_a，导轨皮带轮的半径为r，电枢电阻为R_a，力矩系数为K_t，转动惯量为J，反电势系数为K_e。小车位置x与所受控制力F的关系见公式(7)。

$\left[\left(U_a-K_e\omega \right)\frac{K_t}{K_e}-Fr\right]\frac{r}{J^2}=x$ (7)

将公式(7)代入公式3可得到关于F的关系式(8)。

$F=\left[\begin{array}{cccc}0& -\frac{M{K}_e{K}_t}{M{R}_a{r}^2+R_{a}J}\stackrel{\dot{}}{x}& \frac{Jmg}{M{r}^2+J}& 0\end{array}\right]X+\frac{Mr{K}_t}{M{R}_a{r}^2+R_{a}J}{U}_a$ (8)

则公式(5)将改成如公式(9)所示。本研究以公式(9)的状态方程为基础，代入实测数据进行仿真。

$\left[\begin{array}{c}{\stackrel{\dot{}}{x}}_1\\ {\stackrel{\dot{}}{x}}_2\\ {\stackrel{\dot{}}{x}}_3\\ {\stackrel{\dot{}}{x}}_4\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{K_e{K}_t}{M{R}_a{r}^2+R_{a}J}& -\frac{mg{r}^2}{m{r}^2+J}& 0\\ 0& 0& 0& 1\\ 0& -\frac{K_e{K}_t}{\left(M{R}_a{r}^2+R_{a}J\right)L}& \frac{\left(M+m\right)g{r}^2+Jg}{\left(M{r}^2+J\right)L}& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]+\left[\begin{array}{c}0\\ -\frac{K_{t}r}{M{R}_a{r}^2+R_{a}J}\\ 0\\ -\frac{K_{t}r}{M{R}_a{r}^2+R_{a}J}\end{array}\right]U_a$ (9)

2.3. 系统辨识模型

以上模型为力学模型。本研究中系统还存在电学元器件，需要将其纳入模型。由于无法完全消除误差，可以通过系统辨识方法计算一个数学模型作为辅助研究。系统辨识指的是通过获取系统在指定输入下的输出数据，计算出一个等价系统。理想情况下，对实际系统与该系统进行相同输入能得到相同输出。

本实验中，使用MATLAB自带的系统辨识(System Identification)工具箱进行快速拟合，系统辨识的数据使用基于PID控制的负载倒立摆摆起与稳摆过程的电机电信号输入与编码器的电信号输出。实验采取灰盒辨识的方法，规定拟合的系统为4阶的状态方程。辨识步骤如下：

1) 导入数据集并进行去均值化处理；

2) 对数据集采样，分别用作辨识估计与结果检验；

3) 使用最小二乘法对数据进行拟合；

系统辨识结果如公式组(10)所示。在仿真中若存在精度要求，将使用该模型进行辅助计算。

$\{\begin{array}{l}A={\left[\begin{array}{cccc}-0.09& 0.63& -0.83& -0.37\\ -0.95& 0.23& -2.20& 2.36\\ 0.71& 0.89& 0.10& 0.64\\ 0.34& -3.70& 1.94& 0.31\end{array}\right]}_{{}_{}}\\ B={\left[\begin{array}{l}0.00031\\ 0.00241\\ 0.00967\\ 0.02098\end{array}\right]}_{{}_{}}\\ C=\left[\begin{array}{cccc}-43537.46& 724.88& 185.51& 63.01\\ -1876.71& 29032.07& -278.20& 353.48\end{array}\right]\\ D={\left[\begin{array}{c}0\\ 0\end{array}\right]}^{{}^{}}\end{array}$ (10)

3. 摆起控制

负载倒立摆摆起控制的难度在于其在该过程中的能量损耗相较普通倒立摆更大。本研究要求最终摆起效果具有可复现性，稳定性与高效性。

3.1. 自激原理

自激振荡是指反馈运放电路的一种现象，即整个电路在无外部激励的情况下，仍能自行产生恒稳振荡输出。对于一个控制系统而言，设反馈网络的反馈系数为 $\stackrel{\dot{}}{F}$，相角为 $\phi F$，整个系统的开环增益为 $\stackrel{\dot{}}{A}$，相角为 $\phi A$。系统达成自激的幅值条件与相角条件见公式(11)。

$\{\begin{array}{l}\left|\stackrel{\dot{}}{A}\stackrel{\dot{}}{F}\right|=1\\ \phi F+\phi A=2n\pi \end{array}$ (11)

自激系统能够达成自我调节，自动依据系统状态选择控制能量的行为，使系统损耗能量小于或等于吸收能量，则系统的状态与输出会呈周期性变化。对于自激系统，仅需要初始的微小激励作为启动输入，而不用持续地供给激励。

3.2. 临界条件

在摆起阶段可将整个摆锤体系视作一个刚体钟摆模型，为了实现摆起，需要计算整个系统处于自激的临界条件。

本研究中，为了使摆起效果最大化，选择在摆杆摆至无速度状态时控制小车移动，因此小车移动的周期应当为钟摆摆动周期的1/2，这即是倒立摆系统实现自激的“相角条件”，如公式(12)所示。

$T=\pi \sqrt{\frac{L}{g}}$ (12)

从钟摆模型的相轨迹入手可以推导幅值条件，不妨设摆起阶段系统所受整体摩擦的幅值大小为 $f_x$，那么可以根据动力学方程得到公式(13)。

$\ddot{\theta }+\frac{L}{g}\left(\theta +f_x\mathrm{sgn}\stackrel{\dot{}}{\theta }\right)=0$ (13)

令 $y=\stackrel{\dot{}}{\theta }$， $x=\theta \pm f_x$ (正负号由y的极性确定)，则有 $y=dx$， $\ddot{\theta }=dy$，此时再令摆角幅值 $\theta$ 的初始偏角为 $\xi$，初始角速度为0，则能推出相轨迹公式(14)。

$\{\begin{array}{l}{y}^2+{\left(\theta +f_x\right)}^2={\left(\xi +f_x\right)}^2,y<0\\ y^2+{\left(\theta -f_x\right)}^2={\left(\xi -f_x\right)}^2,y>0\end{array}$ (14)

根据该公式绘制的相轨迹如图2所示。

Figure 2. Pendulum phase trajectory

图2. 钟摆相轨迹

从该结果中不难看出，若系统自身没有吸收能量的措施，摆角 $\theta$ 最终经过多次摆动后会变成0，因而要使用摆起算法给系统增加损耗得能量。

以下半相面为例，如图3，在 $\theta =\alpha$ 处小车移动给予系统大小为 $\Delta E$ 的能量，那么根据公式(14)不难推得三个关键坐标的幅值 $y_1,y_2,\eta$，见公式(15)。

Figure 3. Diagram of phase trajectory

图3. 相轨迹示意

$\{\begin{array}{l}{y}_1=-\sqrt{{\left(\xi -f_x\right)}^2-{\left(\alpha -f_x\right)}^2}\\ y_2=-\sqrt{y_1^2+2\Delta E}\\ \eta =\sqrt{{\left(\xi -f_x\right)}^2+2\Delta E}-f_x\end{array}$ (15)

则当 $\eta =\xi$ 时，即 $\Delta E=2f_x\xi =2f_x\eta$ 时，达到临界条件，系统进入自激等幅振荡，想要实现摆起，给予的能量必须超过该临界条件。在实际实验中为了摆起效果最大化，可令 $\alpha$ 无限接近 $\xi$。

在摆起问题中，损耗的能量会随 $\theta$ 增加而增加，为了计算方便，可将损耗的能量近似为与 $\theta$ 呈比例关系。通过上述推导可得，在摆起问题中，摆杆每次摆动至初始位置(摆动周期的一半)给予的能量必须大于系统动能与该次摆动能量损耗之和，即有公式(16)。

$\Delta E>\frac{1}{2}J{\stackrel{\dot{}}{\theta }}^2+mgL\left(\cos \theta -1\right)+f_x\theta$ (16)

3.3. 能量法

能量法是一级倒立摆问题中常用的摆起控制方法，其核心思路是在摆动周期内不断给予系统随摆角幅值 $\theta$ 而变化的能量，其原理公式见公式(17)。

$v=\{\begin{array}{l}sign\left[\left(\Delta E\right)\stackrel{\dot{}}{\theta }\cos \theta \right]ng,\left|\theta \right|<\frac{\pi }{2}\\ 0\end{array}$ (17)

该公式说明了小车速度与能量变化 $\Delta E$ 以及摆角幅值 $\theta$ 的关系，根据本文的推导，将公式(17)进一步修改为公式(18)，其中K为比例系数。

$v=2Ksign\left[\left(\Delta E\right)\stackrel{\dot{}}{\theta }\cos \theta \right]\Delta E\theta$ (18)

根据公式(18)在simulink中搭建的电路程序进行仿真实验，仿真结果表明，基于自激原理改进的能量法能够快速积蓄动能，在极少的摆动周期内成功实现摆起。接下来通过具体实验来验证理论的可靠性。

3.4. 实验验证

将simulink程序用A/D、D/A设备连接至实际系统，通过比例系数调整输出的电信号，令系统正好进入等幅振荡，记录此时的系数，作为自激的基准系数。用比例系数的大小表示给予能量的大小。不断修改算法比例系数，以系统初始偏角 $\xi =80˚$ 为初始条件，以摆角幅值 $\theta =150˚$ 为成功摆起条件，观察带有3 kg负载的倒立摆系统的摆起控制效果，记录实现摆起所需的摆动周期次数以及实验现象，与无负载的倒立摆系统进行对比研究。结果如表1所示。

Table 1. System resulting data of swing-up experiment

表1. 摆起实验结果数据

初始 $\xi =80˚$，目标 $\theta =150˚$，负载 $Ms=3\text{kg}$。

首先仅从带负载的实验结果出发，可以得出，以自激等幅振荡的能量输出为基准，此时若减少能量的输出，系统将无法成功起摆，摆动幅值也随着能量损耗而不断减小；若是从基准上增加给予的能量，则系统会逐渐积蓄能量并成功起摆，同时能量积蓄的速度随着单次给予能量幅值的增加而加快。

而无负载倒立摆系统的实验现象可以看出，即使比例系数较小，即给予系统能量较小，系统自身也能保持等幅振荡，这说明其能量损耗不大，幅值条件的区间范围相较带负载倒立摆系统更大；并且在比例系数稍微增加后系统便能实现快速起摆，说明改进过后的能量法能够快速积蓄系统的动能，作为对比，同一条件下负载对倒立摆系统摆幅不断减小，说明其能量损耗更大，幅值条件更加苛刻，因此需要通过优化后的能量法快速弥补损耗的动能，并进一步蓄能。

实验从上述两个角度成功说明了理论的可靠性。同时实物操作中的摆起控制还具有良好的鲁棒性；硬件的噪声情况也很小，避免了计算机的算力消耗。

4. 稳摆控制

负载倒立摆的稳摆控制有两个难点。一是由于负载的巨大惯性，摆杆所受扰动相较普通倒立摆更大，对算法鲁棒性要求高。二是切换摆起与稳摆的控制算法时，若摆杆的瞬时速度很大，可能会无法收敛，对算法灵敏度要求高。

结合本研究的硬件条件，选择使用LQR法进行控制参数计算，再用Q学习法进一步调整。

4.1. LQR法

LQR (Linear Quadratic Regulator)即线性二次型调节器，该方法是通过调整Q、R矩阵来改变状态反馈控制器K，从而使线性二次型目标函数得到最小值，进而获得状态空间形式的线性反馈结构 [5]，其优点在于其计算控制器参数的计算方式完全立足于系统的数学建模与参数，具有很好的鲁棒性。

在仿真实验中分别使用机理建模法与系统辨识法得到的模型进行增益计算。令性能指标矩阵R = 1；设置合适的状态权矩阵Q，令反馈矩阵。最终由机理建模法得到的K1矩阵为[−220.3; −37.1; −102.4; −98.3]，而系统辨识建模所得到的K2矩阵为[−3; −0.04; −1.4; −0.9]，经过实际系统的简单测试，K2矩阵更加满足实际控制的需求，因此后续仿真以K2反馈矩阵为基础进行进一步调整。

4.2. Q学习

Q学习法的核心思路如下：随机选择一个初始动作并执行，若未达到目标状态，则根据学习策略重新选择行为并继续执行。学习策略主要取决于当前动作对达到目标状态的“比重”。本研究令负载倒立摆自然下垂状态为初始态，倒立稳定状态作为目标态，状态反馈器的参数选取为动作，系统辨识得到的状态方程模型为环境，以K2矩阵系数作为Q矩阵的初始行为。当摆角幅值大于150˚时用Q学习对状态反馈器的参数进行随机训练。

在MATLAB中使用算法仿真，输出摆角经过转换，以 $\theta =0˚$ (实际 $\theta =180˚$ )作为稳定状态，在经过120,000次训练后系统成功收敛，同时输出矩阵Q。Q矩阵末尾行数据即为实现稳摆控制所需要的状态反馈器系数，为[−0.9; −0.07; −1.6; −1.1]。

4.3. 结果记录

将上述所用算法整合为simulink程序，分别进行仿真与实际硬件实验，摆角q输出曲线结果见图4。

Figure 4. Result of stability control experiment (left is simulation result, right is real result)

图4. 稳摆控制实验结果(左为仿真结果，右为实物结果)

从仿真结果可以看出，系统在三个摆动周期内就能快速起摆，同时立刻稳定收敛，在仿真前25 s系统的输出和理论一致，但在25 s之后，系统进入摆起–稳摆–摆起的循环。这是由于系统辨识采集的数据较少，拟合的状态方程属于自然发散系统，与实际系统存在舍入误差，无法实现绝对收敛，但该系数能作为实际调参的参考。

实际曲线则表明，系统整体鲁棒性较好，在人为对摆角施加大扰动后仍能快速收敛。

5. 结论

本文基于自激原理，推导得出了负载倒立摆系统达成自激摆起所需的幅值条件与相位条件。经由对比摆起实验可知，若电机给系统所施加能量的幅值与相位满足该条件，便能用力矩很小的电机完成大负载倒立摆系统的摆起控制，并且系统的摆起效率与小车单次给予能量超过条件基准值的比重呈正相关关系。另外，使用幅值条件还能反推计算负载倒立摆系统的承重极限，为大负载倒立摆模型的实际应用问题提供一种新的解决思路。

本研究通过上述推导结果，对能量法摆起控制进行了一定优化，同时使用Q学习算法与LQR法进行稳摆控制，进而规划出了一套针对负载倒立摆模型控制难点的方案。通过实验操作证实了该方案具有稳定、可重复、鲁棒性好的优点。

最后以本次研究为基础，可将控制效率的提高、算法灵敏度的优化以及系统负载极限的精确推导等课题作为今后主要的研究内容。

基金项目

此项工作得到北京航空航天大学“双一流”课程建设资助。