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; endif VStep(j,:) < Vmin, VStep(j,:) = Vmin; end% 位置更新Swarm(j,:) = Swarm(j,:) + VStep(j,:);for k = 1 : Dimif Swarm(j, k) > Ub(k), Swarm(j,k) = Ub(k); endif Swarm(j, k) < Lb(k), Swarm(j,k) = Lb(k); endend% 个体最优更新[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) < fzbestzbest = Swarm(j,:);fzbest = fSwarm(j);kbest = kSwarm(j,:);endenditer = 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 00 112.897583812313 0 00 0 568.027423573199 00 0 0 1000]R = 1000
对应的 kbest = [0.999999999999997 0.164764226510168 2.97084954431260 0.665701721954984]
效果如下，直观上看确实有所提升，精度和稳定性都还可以
