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]
效果如下，直观上看确实有所提升，精度和稳定性都还可以
