直线二级倒立摆的一种稳定控制算法研究

蒋凌云，徐炳吉，张峰华，张钦琛

(中国地质大学(北京)信息工程学院，北京100083)

1 引言

倒立摆被誉为是“控制理论的试金石[1]”，是检验控制理论与算法是否可靠，能否实施的极佳平台，并将研究的控制算法应用到航天，机器人等很多领域中。倒立摆是一个具有非线性，不稳定，多变量，强耦合等特性的系统，任何重心在上、支点在下的控制问题研究都与倒立摆的控制相似，比如，机器人行走中的平衡控制，电动平衡车的平衡控制，火箭垂直度控制等等[2][3]。研究倒立摆的控制不仅具有理论意义也具实际意义。

倒立摆的控制分为起摆控制和稳定控制两部分。本文研究的是直线二级倒立摆的平衡点附近的稳定控制。现有的倒立摆稳定控制方法有LQR，PID控制等等。彭锦，尹逊和，曹戈等人[2][3][4]对直线二级倒立摆进行分析，并在倒立摆平衡点处进行线性化处理，得到倒立摆的状态空间方程，设计了LQR对其进行稳定控制，但仅仅只是通过人工试凑得到LQR的一组相对合适的参数，这样的“最优控制”并不是严格意义上的最优，人为因素太多。由于直线二级倒立摆状态变量较多，人工试凑LQR的参数很繁琐，利用粒子群算法来优化LQR参数是个很好的选择。武俊峰，邹忱忱等人[5][6]分别利用基本PSO和惯性权重线性递减的PSO来优化LQR，并对二级倒立摆进行稳定控制，控制效果优于人工试凑法。但是基本粒子群算法存在收敛速度慢，容易陷入局部最优等缺点，算法搜索过程是一个复杂的非线性过程，惯性权重线性递减也未必是一个好的改进措施[7]。吴永红[8]等人从惯性权重和学习因子两个角度来改进基本PSO，改进后效果要优于基本PSO，但是收敛速度和精度依然存在一定问题。南洁琼[9]等人采用惯性权重随机正弦调整策略来改进基本PSO。陈林[10]等人采用惯性权重随机正弦调整策略和概率变异策略相结合来改基本PSO。但是这两种方法中，惯性权重在初期减少得比较快，不利于权衡全局搜索能力和局部搜索能力。

本文从惯性权重非线性动态调整，学习因子非线性动态调整以及基于自然选择机理三个角度来改进基本粒子群算法。惯性权重非线性动态调整有利于权衡算法的前期全局搜索能力和后期局部搜索能力；学习因子非线性动态调整有利于权衡粒子不同阶段自我学习能力和社会学习能力。进行自然选择有利于提高算法的收敛速度和寻优能力。将改进后的算法应用于LQR的参数优化，实现对直线二级倒立摆的稳定控制，并与基本粒子群算法，惯性权重线性递减粒子群算法，遗传算法优化的LQR控制效果相对比。仿真结果表明，改进后的算法优化的LQR具有更佳的控制效果。

2 直线二级倒立摆数学模型与稳定控制描述

直线二级倒立摆简化图如图1所示，小车质量为M，摆杆1和摆杆质量分别为m1，m2，质量块质量为m3，摆杆1和摆杆2质心到转动轴心的距离分别为l1，l2，F为作用在小车上的合外力。x为小车位移，θ1和θ2分别为摆杆1和摆杆2与竖直方向上的夹角。

图1 直线二级倒立摆简化图

为了简化建模过程，忽略空气所带来的阻力，认为两个摆杆都是刚体且质量分布均匀，小车受到导轨对它的摩檫力正比于小车的速度，摆杆1所受到的转动摩擦力矩正比于它相对于小车的相对角速度，摆杆2所受到的转动摩擦力矩正比于它相对于摆杆1的相对角速度。

在上述假设条件的基础上，以小车位移x，摆杆1的角度θ1和摆杆2的角度θ2为广义坐标，采用第二类Lagrange方程对倒立摆系统进行建模。

(1)

L为Lagrange算子，T为系统的动能，V为系统的势能，q=[x，θ1，θ2]。

系统在广义坐标x所受合外力为F，在广义坐标θ1和θ2并未受到外力，所以有：

(2)

(3)

(4)

对小车，摆杆1和摆杆2和质量块进行能量分析，得到系统动能和势能，得出Lagrange算子，利用式(2)(3)(4)得到系统的非线性微分方程，详细推导见文献[2][3]。由于在稳摆控制过程中，θ1和θ2很小，所以可以认为cos(θ2-θ1)，sin(θ2-θ1)，cosθ2≈cosθ1，sinθ2≈θ2，sinθ1≈θ1[11]。

取状态向量：

令u=F，在系统的平衡位置[0，0，0，0，0，0]T进行线性化处理，详细推导及数据见文献[2][3]，得到直线二级倒立摆线性处理后的状态空间模型

(5)

(6)

根据线性系统能控性和能观性判据，得

所以直线二级倒立摆在平衡位置线性化处理后得到状态空间模型能控能观，可以设计LQR控制器使得系统稳定。直线二级倒立摆的稳定控制就是通过设计一个合适的控制器，对小车施加合适的控制信号，让小车位置发生改变，使得两个摆杆维持竖直向上状态。

3 线性二次型最优调节器的设计

3.1 线性二次型调节器(LQR)原理

假定一个线性时不变(LTI)系统

(7)

y(t)=Cx(t)

(8)

其中x、u、y分别为n维状态变量，r维输入矢量，m维输出矢量；A∈Rn×n，B∈Rn×r，C∈Rm×n分别为系统状态矩阵，输入矩阵，输出矩阵。引入最优控制二次型性能指标

(9)

其中，Q为n×n维半正定状态加权矩阵，Q=diag(q11，q22，q33，q44，q55，q66)；R为r×r维正定输入加权矩阵，R=[q77]；S为n×n维半正定终端状态加权矩阵。设计一个输入量u(t)，使得J最小化，达到能量和误差综合最优。由线性二次型最优控制理论可得，要使得J最小化，控制信号应为

u*(t)=-R-1BTP(t)x(t)=-K(t)x(t)

(10)

其中，P(t)为n×n维实对称正定矩阵，K(t)为r×n维最优反馈增益矩阵。P(t)满足黎卡提(Riccati)矩阵微分方程

(11)

(12)

式(11)变为黎卡提(Riccati)矩阵代数方程

0=-P(t)A-ATP(t)+P(t)BR-1BTP(t)-Q

(13)

解式(13)即可得到P(t)，控制信号为：u*(t)=-R-1BTP(t)x(t)=-K(t)x(t)，使得J最小。

3.2 直线二级倒立摆LQR仿真研究与分析

通常情况下，LQR控制器的Q和R矩阵选取没有一个固定解析类方法，所以控制器的性能好坏基本取决于人工经验对Q和R的选取。而且直线二级倒立摆的各状态变量存在强耦合性，不断通过调整各个状态变量的权重，进行试凑来达到理想的控制效果，十分繁琐，并且不能确定性能指标是否满足限定范围。

直线二级倒立摆是个非线性系统，而LQR的设计是建立在其平衡点线性化处理后得到的线性模型基础上，因此要考虑系统的状态变量变化不能超过这个线性范围，即摆杆的角度和角速度不能过大；并且由于导轨有一定长度限制，小车的位移也不能过大。倒立摆初始状态设置为[0，0，0，0，0，0]T，R设置为[1]，然后施加阶跃信号，本文通过试凑多组参数，最终选取一组合适参数：Q=diag(1500，1000，600，0，0，0)，R=[1]，得到K=[38.7298，128.6358，-277.4202，35.4025，0.5085，-46.0550]，系统的阶跃响应曲线如图2。

图2 倒立摆阶跃响应

可以看见摆杆摆角波动小，小车位移很小，响应迅速，满足摆杆的角度和角速度不能过大，小车的位移不能过大的条件。

4 LQR的优化

4.1 基本粒子群算法原理

粒子群算法(PSO)是一种随机搜索算法、迭代进化算法，启发式优化算法[7]，适用于动态，多目标优化问题的寻优，与一些传统优化算法比较具有快速的计算速度和更好的全局寻优能力。

假定被优化函数的解空间是D维空间，粒子群包含N个粒子，PSO首先初始化N个随机粒子，即被优化函数的N个随机解，每个粒子都有自己的位置(被优化函数的潜在解)Xi和速度Vi，第i个粒子搜索到的最优位置(解)PBEST(i)称为个体极值，整个粒子群搜索到的最优位置(解)GBEST，分别记为

PSO通过迭代搜索被优化函数的全局最优解，确定个体最优位置和全局最优位置后，通过公式更新个体位置和速度

(14)

(15)

k为当前迭代次数；ω为惯性权重；C1，C2为取值非负的学习因子，两者通常取2；r1，r2为[0，1]范围内产生的均匀随机数。通常粒子的速度会有一个限制范围，粒子的位置也会有边界限制范围，速度和粒子位置超过限制范围则采用其边界值。然后代入被优化函数评价出新的个体最优位置，全局最优位置和全局最优适应值，循环上述式(14)(15)，直至达到最大迭代次数或者满足其它设定条件，算法寻优结束，输出最优解和相应的适应值。

基本粒子群算法有一定的缺陷，算法容易产生早熟收敛或者收敛速度慢，环境的变化会影响算法性能，容易陷入局部最优，不能保证全局收敛[7][8]。针对这些问题，本文从惯性权重，学习因子以及基于自然选择机理来改进基本粒子群算法。

4.2 标准粒子群算法改进

4.2.1 权重改进

粒子的惯性权重ω的选取十分重要，较大的ω有利于全局搜索，较小的ω有利于局部搜索和收敛，因此动态调整ω十分重要。由于搜索过程是非线性的，采用固定ω或者惯性权重线性递减方式

ω=ωmax-(ωmax-ωmin)*(k-1)/(kmax-1)

(16)

ω随着迭代次数增加从ωmax线性递减至ωmin，并不能很好地反映搜索过程，不能很好地权衡全局搜索和局部搜索。所以本文使用一种非线性递减方式

ω=ωmax*cos[π(k-1)/(2(kmax-1))]

+ωmin*{1-cos[π(k-1)/(2(kmax-1))]}

(17)

其中，ωmax和ωmin分别为选取的最大和最小惯性权重；k为当前迭代次数，kmax为最大迭代次数。ωmax取1，ωmin取0.35。

如图3前期随着k增大，可以看出惯性权重线性递减方式中，ω减小得快，这样会导致全局搜索能力下降得快，容易过早陷入局部最优；而本文使用的惯性权重非线性递减方式中ω减小缓慢，有利于维持全局搜索；中后期减小得较快，有利于局部搜索和收敛，提高搜索精度。

图3 惯性权重随迭代次数变化曲线

4.2.2 学习因子改进

学习因子C1和C2分别反映了对自我经验学习能力和对社会的学习能力，越大反映相应的学习能力越强，学习因子也能像惯性权重一样起到平衡局部搜索和全局搜索的作用。通常情况下，C1和C2都取一个固定值，这样不利于迭代初期和后期的搜索能力的权衡。在迭代初期，C1较大，C2较小，能更好地进行全局搜索；迭代后期C1较小，C2较大，有利于局部搜索和收敛。因此本文设置学习因子如下

C1=C1i-(C1i-C1f)[1-cos(π(k-1)/(2(kmax-1)))]

(18)

C2=C2i-(C2i-C2f)[1-cos(π(k-1)/(2(kmax-1)))]

(19)

C1i和C1f分别为C1的初始值和终值，C2i和C2f分别为C2的初始值和终值；k为当前迭代次数，kmax为最大迭代次数。

4.2.3 基于自然选择改进

借鉴达尔文自然学说中优胜劣汰，适者生存这一原理，淘汰一部分位置不是太好的粒子，以达到提高算法的收敛速度和寻优能力的目的。在算法每一次迭代后，将每一个粒子按照其适应值大小从左到右进行升序排列，用排序后靠近左端的1/N适应值小的粒子的速度和位置去替换对应的靠近右端的1/N适应值大的粒子的速度和位置，同时保留粒子所记忆的历史最优适应值。为了避免陷入局部最优，本文只把百分之十最靠左的粒子的速度和位置去替换百分之十最靠右的粒子的速度和位置。

4.3 改进粒子群算法优化LQR参数

(20)

Q=diag(q11，q22，q33，q44，q55，q66)，R=[q77]

ω1=0.4，ω2=0.6，tf为仿真时间，选取一个合适的值以确保倒立摆动态响应曲线已经进入了稳态，本文设置为50s。

改进粒子群算法优化LQR参数步骤：

Step1：初始化粒子群的粒子个数，惯性权重变化范围，学习因子变化范围，粒子速度范围限制，粒子每一维位置限制，迭代步数等参数。随机初始粒子位置和速度，计算粒子个体适应值和全局最优个体适应值。

Step2：开始迭代，由式(17)(18)(19)计算粒子惯性权重和学习因子。

Step3：由式(14)(15)更新粒子速度和位置，计算个体适应值，更新粒子个体历史最优适应值和最优位置，全局最优个体适应值和位置。

Step4：将粒子按照适应值从小到大排序，进行自然选择操作。

Step5：判断是否达到最大迭代次数，未达到最大次数返回到Step2，继续循环迭代。达到最大迭代次数则跳出循环。

Step6：输出适应值函数的全局最优适应值和最优位置，输出相应的LQR最佳参数。

上述改进粒子群算法优化LQR参数流程图如图4。

图4 改进粒子群算法优化LQR参数的过程

5 仿真结果与分析

5.1 模型搭建与算法参数设置

如图5是根据直线二级倒立摆状态方程以及适应值函数搭建的直线二级倒立摆simulink仿真图。

本文分别采用惯性权重固定的基本粒子群算法，惯性权重线性递减粒子群算法，遗传算法(GA)，以及提出的改进粒子群算法对LQR进行参数优化。

基本粒子群算法的惯性权重ω设置为1，粒子个数为60个，学习因子均设为2，迭代最大次数为150次。

惯性权重线性递减粒子群算法的权重初始值和终值分别为1，0.35，粒子个数为60个，学习因子均设为2，迭代最大次数为150次。

遗传算法种群大小设置为60，进化次数为150次，交叉概率0.8，变异概率0.05。

本文提出的改进粒子群算法的权重初始值和终值分别为1，0.35，粒子个数为60个，学习因子C1初始值C1i设置为2，终值C1f设置为1.5；C2初始值C2i设置为1.5，C2f设置为2，迭代最大次数为150次。

粒子的位置下限都为[1，1，1，1，1，1，1]，上限为[1500，1500，1500，1500，1500，1500，10]。

图5 simulink直线二级倒立摆模型

5.2 仿真结果与分析

如图6(a)所示，为本文改进的粒子群算法和其它三种算法适应值变化曲线，基本粒子群算法在迭代120次以上开始收敛，惯性权重线性递减的粒子群算法在迭代85次开始收敛，改进粒子群算法在迭代少于65次就开始收敛，改进粒子群算法收敛速度明显快于另外两种粒子群算法。虽然遗传算法在一开始迭代就搜索到最优解附近，然后开始收敛进行局部精细寻优，但是从图6(b)可以看出本文提出的改进的粒子算法寻优精度高于遗传算法。说明改进的粒子群算法收敛速度快，寻优精度高。

四种算法在迭代150次后，各自输出的反馈增益矩阵K如表1所示：

表1 人工试凑和四种算法获取的反馈增益矩阵K

为了仿真绘图方便，本文把惯性权重线性递减PSO简写成wLDPSO。

实验1：倒立摆初始状态为[0，0，0，0，0，0]T，t=0s时刻，施加一个幅值为6的阶跃信号，模拟倒立摆中的小车受到外界一个持续大小为6N的恒力时的稳定控制。人工试凑和采用四种不同算法的直线二级倒立摆的摆杆1和摆杆2的角度响应曲线分别如图7(a)、7(b)所示。

由此可以看出五种控制器都能实现倒立摆的稳摆控制，本文改进的粒子群算法相对于人工试凑和其它三种算法，摆杆1和摆杆2的角度响应具有超调量小，响应快的特点。

由此可以看出五种控制器都能实现倒立摆的稳摆控制，本文改进的粒子群算法相对于人工试凑和其它三种算法，摆杆1和摆杆2的角度响应具有超调量小，响应快的特点。

实验2：倒立摆初始状态为[0，0，0，0，0，0]T，2s时刻施加一个宽为 0.1 s，幅值为6的脉冲干扰脉冲信号，模拟小车受到一个持续时间为0.1s，大小为6N的外力扰动时的稳定控制。摆杆1和摆杆2的角度响应曲线分别如图8(a)、8(b)所示。可以看出，改进粒子算法优化后的控制器使得系统抗干扰能力强，反应速度快，摆杆角度响应的超调量更小。

实验3：无扰动情况下，倒立摆初始状态为[0，0，0，0，0，0]T，期望状态为[0.1，0，0，0，0，0]T。如图9(a)、图9(b)，经过改进的粒子群算法优化后，摆杆1，摆杆2的角度响应超调量最小，恢复到平衡位置快。

实验4：在倒立摆初始状态为[0，0，0，0，0，0]T时，加入一个宽为 0.1 s，幅值为0.1 rad 的脉冲干扰，模拟摆杆2受到扰动。经过改进粒子群算法优化后，在摆杆受到扰动情况下，摆杆1、摆杆2的角度响应超调量均比人工试凑，基本粒子算法以及惯性权重线性递减粒子群算法的要小；人工试凑，摆杆恢复到平衡位置时间更短，但是超调量很大，几乎不满足前面讨论的摆角要小这个条件；比遗传算法优化后的超调量略小，均能很快恢复到平衡位置，有很强抗干扰能力。

6 结论

本文利用LQR来实现直线二级倒立摆的稳定控制，但是直线二级倒立摆状态变量较多，传统人工试凑得到LQR参数的方法过于繁琐且具有人为意义上的最优，以及基本粒子群算法存在收敛速度慢，容易陷入局部最优等缺点。针对这些问题，从惯性权重非线性动态调整，学习因子非线性动态调整以及基于自然选择机理三个角度来改进粒子群算法，来优化LQR参数，提升稳定控制效果。在算法迭代寻找适应值函数最优值方面，改进粒子群算法与基本粒子群算法、惯性权重线性递减粒子群算法以及遗传算法相比，具有收敛快，寻优精度高的优点。仿真结果表明，改进粒子群算法优化后的LQR对倒立摆的稳定控制效果与人工试凑以及其它算法优化后的控制效果相比，摆杆角度响应具有更小的超调量，响应快，抗干扰能力强。