粒子群算法(PSO)优化的直线二级倒立摆系统LQR控制器

1.研究对象

 

直线二级倒立摆结构参数

2.数学模型

 直线二级倒立摆系统数学模型的建立基于以下假设： 

1. 每一级摆杆都是均匀的刚体； 
2. 忽略同步带的弹性； 
3. 驱动力与放大器输入成正比，并无延迟地施加于小车；
4. 所有摩擦力足够小，可忽略不计； 

2.1 列拉格朗日方程

2.1.1 系统总动能

2.1.2 系统总势能 

2.1.3 拉格朗日算子

2.1.4 拉格朗日方程

2.2 线性化

2.3 状态空间方程

% 求解状态空间方程
% Copyright(C)2020 WANG-Haotian 1752533 All Rights Reserved
M = 1.32;      % 小车质量
m1 = 0.05;     % 摆杆1质量
m2 = 0.13;     % 摆杆2质量
m3 = 0.236;    % 质量块质量
L1 = 0.0775;   % 摆杆1半长度
L2 = 0.25;     % 摆杆2半长度
g = 9.81;      % 重力加速度
syms theta1 theta2 u;
F = [-(m1+2*m2+2*m3)*L1 ((4/3)*m1+4*m2+4*m3)*L1^2 2*m2*L1*L2;
    -m2*L2 2*m2*L1*L2 (4/3)*m2*L2^2;
    M+m1+m2+m3 -(m1+2*m2+2*m3)*L1 -m2*L2];   %式（2-14）右边第一项
H = [(m1+2*m2+2*m3)*g*L1 0 0;
    0 m2*g*L2 0;
    0 0 1];          % 式（2-14）右边第二项
Result = inv(F)*H;   % 求解式（2-14）
A = [0 0 0 1 0 0;
    0 0 0 0 1 0
    0 0 0 0 0 1;
    0 Result(1,1) Result(1,2) 0 0 0;
    0 Result(2,1) Result(2,2) 0 0 0;
    0 Result(3,1) Result(3,2) 0 0 0];
B = [0;0;0;Result(1,3);Result(2,3);Result(3,3)];

 

3.LQR控制器设计

3.1 LQR基本原理

最优控制是通过优化性能指标来搜索能使目标函数最小的控制算法。

而线性二次最优控制问题是指系统对应的指标函数是状态或控制变量的二次型函数的积分的线性系统的最优控制问题。

在 LQR 控制中，L 是指受控系统限定为以状态空间形式给出的线性系统，Q 是指系统性能指 标函数限定为二次型函数及其积分，R 是指调节器的意思。LQR 算法的目标函数和控制对象分别 是以被控对象状态结合输入组成的二次型函数和线性系统。

Q 矩阵是系统性能指标函数对于状态量的权阵，属于半正定对角阵，矩阵内的元素值的 大小和该变量在性能函数中的比重有关；

R 矩阵是系统中控制量的权重，正定对角阵，矩阵内的元素值越大，控制约束越大。 

3.2 粒子群寻优流程

1. 利用粒子群算法产生粒子群（初始化）。将粒子群中的粒子依次赋给 LQR 控制器参数， 即 Q 矩阵的 6 个参数；
2. 运行 Simulink，将输出传递到 workspace 中，计算该组参数相应的目标函数值；
3. 将（2）所得的目标函数值传递到 PSO 程序中，作为粒子的适应度，对比新、旧粒子的 适应度值，更新个体最优和全局最优位置；
4. 对解进行判断，检查是否符合迭代次数要求或是否满足精度要求，若满足性能指标要 求，直接退出算法，得到最优参数；若不满足，直接返回（2），继续循环；
5. 经优化后的程序能够得到 Q 阵的最佳解，并获得最佳增益矩阵 K 的值。 

Simulink模型

粒子群寻优MATLAB主程序： 

%% 清空环境
clear
clc
%% 参数设置
w = 0.729;   %惯性因子
c1 = 1.49445;   %加速常数
c2 = 1.49445;   %加速常数
Dim = 6;   %维数
SwarmSize = 30;   %粒子群规模
ObjFun = @PSO_LQR;   %待优化函数句柄
MaxIter =30;   %最大迭代次数
MinFit = 0.1;   %最小适应度
Vmax = 0.01;   %速度范围上限
Vmin = -0.01;   %速度范围下限
Ub = [1000 1000 1000 1000 1000 1000];   %待优化参数范围上限
Lb = [0.1 0.1 0.1 0.1 0.1 0.1];   %待优化参数范围下限
%% 粒子群初始化
Range = ones(SwarmSize,1)*(Ub-Lb);
Swarm = rand(SwarmSize,Dim).*Range + ones(SwarmSize,1)*Lb;   %初始化粒子群
Vstep = rand(SwarmSize,Dim)*(Vmax-Vmin) + Vmin;   %初始化速度
fSwarm = zeros(SwarmSize,1);
for i = 1:SwarmSize
    fSwarm(i,:) = feval(ObjFun,Swarm(i,:));   %粒子群适应值计算
end
%% 个体极值和群体极值
[bestf,bestindex] = min(fSwarm);
zbest = Swarm(bestindex,:);   %全局最佳
gbest = Swarm;   %个体最佳
fgbest = fSwarm;   %个体最佳适应值
fzbest = bestf;   %全局最佳适应值
%% 迭代寻优
iter = 0;
y_fitness = zeros(1,MaxIter);
K_p = zeros(1,MaxIter);
K_theta1 = zeros(1,MaxIter);
K_theta2 = zeros(1,MaxIter);
K_dp = zeros(1,MaxIter);
K_dtheta1 = zeros(1,MaxIter);
K_dtheta2 = zeros(1,MaxIter);
while((iter<MaxIter)&&(fzbest>MinFit))
    for j = 1:SwarmSize
        %速度更新
        Vstep(j,:) = w*Vstep(j,:) + c1*rand*(gbest(j,:)-Swarm(j,:)) + c2*rand*(zbest-Swarm(j,:));
        if Vstep(j,:)>Vmax
            Vstep(j,:) = Vmax;
        end
        if Vstep(j,:)<Vmin
            Vstep(j,:) = Vmin;
        end
        %位置更新
        Swarm(j,:) = Swarm(j,:) + Vstep(j,:);
        for k = 1:Dim
            if Swarm(j,k)>Ub(k)
                Swarm(j,k) = Ub(k);
            end
            if Swarm(j,k)<Lb(k)
                Swarm(j,k) = Lb(k);
            end
        end
        %适应值
        fSwarm(j,:) = feval(ObjFun,Swarm(j,:));
        %个体最优更新
        if fSwarm(j)<fgbest(j)
            gbest(j,:) = Swarm(j,:);
            fgbest(j) = fSwarm(j);
        end
        %群体最优更新
        if fSwarm(j)<fzbest
            zbest = Swarm(j,:);
            fzbest = fSwarm(j);
        end
    end
    iter = iter + 1;   %迭代次数更新
    y_fitness(1,iter) = fzbest;
    K_p(1,iter) = zbest(1);
    K_theta1(1,iter) = zbest(2);
    K_theta2(1,iter) = zbest(3);
    K_dp(1,iter) = zbest(4);
    K_dtheta1(1,iter) = zbest(5);
    K_dtheta2(1,iter) = zbest(6);
end
%% 绘图输出
figure(1);   %绘制性能指标ITAE变化曲线
plot(y_fitness,'LineWidth',2)
title('最优个体适应值','fontsize',18);
xlabel('迭代次数','fontsize',18);
ylabel('适应值','fontsize',18);
set(gca,'fontsize',18);
figure(2);   %绘制变化曲线
plot(K_p)
hold on
plot(K_theta1,'k')
plot(K_theta2,'--r')
title('优化曲线','fontsize',18);
xlabel('迭代次数','fontsize',18);
ylabel('参数值','fontsize',18);
set(gca,'fontsize',18);
legend('小车位移','摆杆1角度','摆杆2角度',1);
figure(3)
plot(K_dp)
hold on
plot(K_dtheta1)
plot(K_dtheta2)
title('优化曲线','fontsize',18);
xlabel('迭代次数','fontsize',18);
ylabel('参数值','fontsize',18);
set(gca,'fontsize',18);
legend('小车速度','摆杆1角速度','摆杆2角速度',1);

 待优化句柄：PSO_LQR

function z = PSO_LQR(x)
t = [0:0.01:50];
%% 状态空间方程
A = [0 0 0 1 0 0;
     0 0 0 0 1 0 ;
     0 0 0 0 0 1;
     0 2.954 -0.209 0 0 0;
     0 106.3942 -21.7782 0 0 0;
     0 -40.6113 39.4941 0 0 0];
B = [0;0;0;0.7483;4.9686;-0.656];
C = diag([1 1 1 1 1 1]);
D = [0;0;0;0;0;0];
Q = diag([x(1) x(2) x(3) x(4) x(5) x(6)]);
R = 1;
K = lqr(A,B,Q,R);
%% 
assignin('base','t',t);
assignin('base','K',K);
assignin('base','q1',x(1));
assignin('base','q2',x(2));
assignin('base','q3',x(3));
assignin('base','q4',x(4));
assignin('base','q5',x(5));
assignin('base','q6',x(6));
[t_time,x_state,y_out] = sim('c02',[0,50]);
sum = 0;
for num = 1:length(J)-1
    sum = sum+((J(num)+J(num+1))/2)*(50/(length(J)-1));
end
po_s =po(length(po));
time = [];
for num0 = 1:length(po)
    if (po(num0)>0.98*po_s)&&(po(num0)<1.02*po_s)
        time = [time num0];
    else
        time = [];
    end
end
time0 = time(1);
ts = (50/(length(po)-1))*(time0-1);
w01=1;
w02=1;
z = w01*sum/(50*x(1))+w02*ts/50;