PSO 优化 LQR 权重系数（2）

PSO 优化 LQR 权重系数（2）

主要编写了 MATLAB 代码，能够正常运行，注释还是比较详细的

%% 清空工作空间并跑一下路径规划
clc;
clear all;
run('double_lane.m');

%% 基本参数设置
% 车辆参数
cf=-66900*2;
cr=-62700*2;
m=1723;
Iz=4175;
a=1.232;
b=1.468;

% 纵向速度及AB矩阵计算
vx_km = 30;
vx = vx_km / 3.6;
A=[0,1,0,0;
    0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx);
    0,0,0,1;
    0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];
B=[0;
    -cf/m;
    0;
    -a*cf/Iz];

%% PSO参数设置
w = 0.6;    % 惯性因子
c1 = 2;     % 权重因子
c2 = 2;

Dim = 5;    % 维数
SwarmSize = 5;     % 粒子群规模
MaxIter = 5;      % 最大迭代次数
MinFit = 0.03;       % 最小适应值
Vmax = 20;   % 最大速度
Vmin = -20;  % 最小速度
Ub = [1000 1000 1000 1000 1000];   % 函数上界
Lb = [1 1 1 1 1]; % 函数下界
ObjFun = @PSO_LQR;  % 待优化函数句柄

%% 初始化PSO
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);
kSwarm = zeros(SwarmSize, 4);   % 记录不同种群的K值
for i = 1 : SwarmSize
    [fSwarm(i,:), kSwarm(i,:)] = feval(ObjFun, A, B, Swarm(i,:));
end
% 最小适应值的值及索引
[bestf,bestindex] = min(fSwarm);
zbest = Swarm(bestindex,:);     % 全局最佳
kbest = kSwarm(bestindex,:);    % 最佳适应度对应的K
gbest = Swarm;      % 个体最佳
fzbest = bestf;     % 全局最佳适应值
fgbest = fSwarm;    % 个体最佳适应值

%% PSO迭代，循环搜索最优的QR
iter = 0;
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,:), kSwarm(j,:)] = feval(ObjFun, A, B, 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);
            kbest = kSwarm(j,:);
        end
    end
    iter = iter + 1;     % 迭代次数更新
end
% 将工作空间的内容保存
save('results.mat');

%% PSO_LQR函数，通过不同的QR求解出K，跑Simulink模型确定适应度函数
function [f, k] = PSO_LQR(A, B, qr)
    Q = [qr(1),0,0,0;
        0,qr(2),0,0;
        0,0,qr(3),0;
        0,0,0,qr(4)];
    R = qr(5);
    
    % 进行LQR求解
    K = lqr(A, B, Q, R);
    
    % 第一列作为时间戳，防止报错
    k1 = [0, K(1)];
    k2 = [0, K(2)];
    k3 = [0, K(3)];
    k4 = [0, K(4)];    
    assignin('base','k1',k1);
    assignin('base','k2',k2);
    assignin('base','k3',k3);
    assignin('base','k4',k4);
    
    % 运行模型并将结果输出到工作空间
    simOut = sim('pso_lqr_test');
    assignin('base','simOut',simOut);
    
    % 计算适应度函数
    err = simOut.err.signals.values;
    assignin('base','err',err);
    [m,n] = size(err);
    fitness = sqrt(sum(err(:,1).^2) / m) + sqrt(sum(err(:,3).^2) / m);    
    
    f = fitness;
    k = K;
end

编写完 MATLAB 代码后对算法有了进一步认识，算法的主要思路就是通过粒子生成不同的 QR 值，从而生成不同的反馈增益 K，运行 Simulink 代码输出该 QR 值下的适应值，已达到期望的适应值

对 Simulink 模型进行更改

主要是 lqr_offline 模块，直接将工作空间中的 k1,k2,k3,k4 输入到 Simulink 模型中

function k  = fcn(k1,k2,k3,k4)
    k=[k1,k2,k3,k4];
end

参考《**Path tracking controller of an autonomous armoured vehicle using modified Stanley controller optimized with particle swarm optimization》**论文设置适应度函数

以横向距离误差ed和横摆角误差eφ的均方根之和作为适应度函数

fitness = sqrt(sum(err(:,1).^2) / m) + sqrt(sum(err(:,3).^2) / m); 

添加 kSwarm 和 kbest 保存粒子群的 K 值和最优 K 值

%% 初始化PSO
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);
kSwarm = zeros(SwarmSize, 4);   % 记录不同种群的K值
for i = 1 : SwarmSize
    [fSwarm(i,:), kSwarm(i,:)] = feval(ObjFun, A, B, Swarm(i,:));
end
% 最小适应值的值及索引
[bestf,bestindex] = min(fSwarm);
zbest = Swarm(bestindex,:);     % 全局最佳
kbest = kSwarm(bestindex,:);    % 最佳适应度对应的K
gbest = Swarm;      % 个体最佳
fzbest = bestf;     % 全局最佳适应值
fgbest = fSwarm;    % 个体最佳适应值

跑完 PSO 优化后，直接将最优的 K 值作为 Constant 输入到 lqr_offline，输出优化后的跟踪效果

运行后全局最佳 zbest = [1000 112.897583812313 568.027423573199 1000 1000]

对应的 QR 如下

Q = [1000 0 0 0
			0 112.897583812313 0 0
			0 0 568.027423573199	0
			0 0 0 1000]

R = 1000		  

对应的 kbest = [0.999999999999997 0.164764226510168 2.97084954431260 0.665701721954984]

效果如下，直观上看确实有所提升，精度和稳定性都还可以