clear
clc %清空环境
A=[0,0,0,1,0,0;
0,0,0,0,1,0;
0,0,0,0,0,1;
0,0,0,0,0,0;
0,86.69,-21.62,0,0,0;
0,-40.31,39.45,0,0,0];
B=[0,0,0,1,6.64,-0.088];
C=[1,0,0,0,0 0;0,1,0,0,0,0;0,0,1,0,0,0];
D=[0;0;0];
t=[0:0.005:50];
w=0.6;%惯性因子;
c1=2;%权重因子
c2=2;
Dim=6;%维数
SwarmSize=50;%粒子群规模
ObjFun=@PSO_LQR;%待优化函数句柄
MaxIter = 100; % 最大迭代次数
MinFit = 0.1; % 最小适应值
Vmax = 1;%最大适应值
Vmin = -1;%最小适应值
Ub = [1000 1000 1000 1000 1000 1000];%函数上界
Lb = [0 0 0 0 0 0];%函数下界
Q=[q(1),0,0,0,0;0,q(2),0,0,0;0,0,q(3),0,0;0,0,0,q(4),0,0;0,0,0,0,q(5),0;0,0,0,0,0,q(6)];
R=1;
[K,S,E]=lqr(A,B,C,D,Q);
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); % 预先产生 7 个空矩阵
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 = 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);
end
figure(1) % 绘制性能指标 ITAE 的变化曲线
plot(y_fitness,'LineWidth',2)
title('最优个体适应值','fontsize',18);
xlabel('迭代次数','fontsize',18);ylabel('适应值','fontsize',18);
set(gca,'Fontsize',18);
figure(2) % 绘制 LQR 控制器参数变化曲线
plot(K_p)
hold on
plot(K_theta1,'k','LineWidth',3)
plot(K_theta2,'--r')
plot(K_dp)
plot(K_dtheta1)
plot(K_dtheta2)
title('小车位移、摆杆 1 角度、摆杆 2 角度、小车速度、摆杆 1 角速度、摆杆 2 角速度优化曲线','fontsize',18);
xlabel('迭代次数','fontsize',18);ylabel('参数值','fontsize',18);
set(gca,'Fontsize',18);
legend({'小车位移’,’摆杆 1 角度’,’摆杆 2 角度’,'小车速度','摆杆 1 角速度','摆杆 2 角速'},1);
function z=PSO_LQR(q)
assignin('base','A',A);
assignin('base','B',B);
assignin('base','C',C);
assignin('base','D',D);
assignin('base','t',t);
- 1
- 2
- 3
前往页