基于SSADMO混合算法机械臂时间最优轨迹规划

function [pgx,pnf]=IIIDMOA(aa,aa1,x0,x1,x2,x3) MaxIt =100; %最大迭代次数 nVar = 3; % 决定变量的数目 VarSize =[1 nVar]; % 决定变量矩阵大小 VarMin = 0.15; % 决定变量下界 VarMax = 5; % 决定变量上界 %% ABC Settings nPop = 100; % 种群大小 nBabysitter =5; % 保姆组猫鼬个数 nAlphaGroup = nPop - nBabysitter; % Alpha组猫鼬个数 nScout = nAlphaGroup; % 侦查组猫鼬个数 L = round(0.7 * nBabysitter); % 保姆组交换参数 需要改变 peep = 2; % 雌性鸣叫系数 pop = zeros(nAlphaGroup,nVar); BestSol_Cost = 30; tau = inf; Iter = 1; P_percent=0.2; sm = inf(nAlphaGroup,1); C = zeros(nAlphaGroup,1); pNum = round(nAlphaGroup * P_percent ); h = 1; pgf(1) = 10; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 猫鼬种群的位置 for i =1:nAlphaGroup pop(i,:) = unifrnd(VarMin,VarMax,VarSize); %计算适应度值 % [LB,UB,Dim,F_obj] = Get_F('F1'); pop_Cost(i) = F_obj(pop(i,:)); x =pop; pf(i) = F_obj(pop(i,:)); if pop_Cost(i) <= BestSol_Cost if pgf(h)>pf(i) a( : ,i) = Aa(x(i,1),x(i,2),x(i,3),x0,x1,x2,x3); F = 1 ; for t= 0:0.01 ; x(i,1) if (abs(3 * a(1,i) * t^2+ 2* a(2,i) * t+a(3,i)))>(aa * pi/180)||(abs(6* a(1,i) * t+ 2* a(2,i)))>(aa1 * pi/180) %3次多项式倒数（速度限制公式），速度超过80°/s即归为超速 F= 0; f = 10; end end for t = 0:0.01 : x(i,2) if (abs(5 * a(5,i) * t^4+ 4 * a(6,i) * t^3+ 3 * a(7,i) * t^2+2 * a(8,i) * t +a(9,i)))>(aa * pi/180)||(abs(20*a(5,i))*t^3 + 12*a(6,i)*t^2 + 6*a(7,i)*t + 2*a(8,i))>(aa1 *pi/180) %5次多项式倒数（速度限制公式），速度超过80°/s即归为超速 F=0; f = 10; end end for t= 0:0.01:x(i,3) if (abs(3 * a(11,1) *t^2+2* a(12,i) * t+a(13,i)))>(aa * pi/180)||(abs(6* a(1,i) * t+ 2* a(2,i)))>(aa1 * pi/180) %3次多项式倒数（速度限制公式），速度超过80°/s即归为超速 F=0; f =10; end end if F == 1 h = h+ 1; pgx = x(i,:);%最优位置 pgf(h)= pf(i);%最优位置的适应度 pnf(1) = pgf(h); end end BestSol_Cost = pop_Cost(i); end Best_Cost(i)= BestSol_Cost; end X0 = x; [ fMin, bestI ] = min( pop_Cost ); % fMin denotes the global optimum fitness value BestSol = x( bestI, : ); % bestX denotes the global optimum position corresponding to fMin BestSol_Cost = pop_Cost(bestI); [ fMin, bestI ] = sort( pop_Cost ); for i = 1:nAlphaGroup X(i,:) = X0(bestI (i),:); end %% 主循环 for it = 1:MaxIt if rand >0.4 %轮盘赌法 F=zeros(nAlphaGroup,1); MeanCost = mean(pop_Cost); for i=1:nAlphaGroup F(i) = exp(-pop_Cost(i)/(MeanCost+eps)); % Convert Cost to Fitness end P=F/(sum(F)+eps); % Alpha组 % 由Alpha雌性领导的觅食 for m = 1:nAlphaGroup % 选择Alpha组中的雌性 i=RouletteWheelSelection(P); % 随机选择k，且不能选择自身 K = [1:i-1 i+1:nAlphaGroup]; k = K(randi([1 numel(K)])); %numel返回数组 A 中的元素数目 n %randi随机生成1到numel(K)之间的整数 % 定义鸣叫系数。 phi=(peep/2)* unifrnd(-1,+1,VarSize); % 新猫鼬位置 newpop_Position = pop(i,:) + phi.* (pop(i,:) - pop(k,:)); newpop_Position = Bounds(newpop_Position,VarMin,VarMax,nVar); % 评价 newpop_Cost = F_obj(newpop_Position); % 可比性 if newpop_Cost <= pop_Cost(i) pop(i,:) = newpop_Position; pop_Cost(i) = newpop_Cost; else C(i) = C(i) + 1; end end % 侦查组 for i = 1:nScout % 随机选择k，不等于i K = [1:i-1 i+1:nAlphaGroup]; k = K(randi([1 numel(K)])); % 定义鸣叫系数。 phi=(peep/2) * unifrnd(-1,+1,VarSize); % 新猫鼬位置 newpop_Position = pop(i,:) + phi.* pop(i,:) - (pop(i,:) - pop(k,:)); newpop_Position = Bounds(newpop_Position,VarMin,VarMax,nVar); % 计算适应度值 newpop_Cost = F_obj(newpop_Position); % display(num2str(newpop.Cost)) % 睡眠巢穴 sm(i) = (newpop_Cost - pop_Cost(i))/max(newpop_Cost+eps,pop_Cost(i)+eps);%可以修改 % 比较取优 if newpop_Cost <= pop_Cost(i) pop(i,:) = newpop_Position; pop_Cost(i)=newpop_Cost; else C(i)=C(i)+1; end end % 保姆组 for i=1:nBabysitter if C(i) >= L pop(i,:)=unifrnd(VarMin,VarMax,VarSize); pop_Cost(i)=F_obj(pop(i,:)); C(i)=0; end end % 更新找到的最佳解决方案 for i = 1:nAlphaGroup if pop_Cost(i) <= pf(i) BestSol = pop(i,:); BestSol_Cost = pf(i) ; end end % 下一个猫鼬位置 newtau = mean(sm); CF = (1-it/MaxIt)^(2*it/MaxIt); %CF = 2*exp(-(it/MaxIt)); for i=1:nScout M = (pop(i,:) .* sm(i))./(pop(i,:)+eps); qq = CF * rand * (pop(i,:)-M); %%需要进行改进 if newtau > tau newpop_P(i,:) = pop(i,:)- CF * rand * (pop(i,: )- M); else newpop_P(i,:) = pop(i,:)+ CF * rand * (pop(i,:)- M ); end newpop_P(i,:) = Bounds(newpop_P(i,:),VarMin,VarMax,nVar); newpop_Cost(i) = F_obj(newpop_P(i,:)); f1 = newpop_Cost(i); if pf(i)> f1 a( : , i ) = Aa(newpop_P(i,1) ,newpop_P(i,2) ,newpop_P(i,3),x0,x1,x2,x3); F=1; for t= 0:0.01;newpop_P(i,1) if(abs(3 * a(1,i) * t^2+ 2* a(2,i)* t+a(3,i)))>(aa * pi/180)||(abs(6* a(1,i) * t+ 2* a(2,i)))>(aa1 * pi/180) F=0; f1 =10; end end for t= 0:0.01:newpop_P(i,2) if (abs(5 * a(5,i) * t^4+ 4 * a(6,i) * t^3+ 3 * a(7,i) * t^2+2 * a(8,i) * t +a(9,i)))>(aa * pi/180)||(abs(20*a(5,i))*t^3 + 12*a(6,i)*t^2 + 6*a(7,i)*t + 2*a(8,i))>(aa1 *pi/180) F= 0; f1 = 10; end end for t=0: 0.01:newpop_P(i,3) if (abs(3 * a(11,i) * t^2+ 2 * a(12,i)* t+a(13,i)))>(aa* pi/180)||(abs(6* a(1,i) * t+ 2* a(2,i)))>(aa1 * pi/180) F= 0; f1 = 10; end end if F == 1 %F=1表示满足速度约束，则3段时间x(i,:)赋值给px(i,:)，最优适应度f赋值给pf(i) px(i,:)= newpop_P(i,:); pf(i)= f1; LI= 5; end end if pgf(h)>pf(i) %最短时间比较 h = h+ 1; pgf(h) = pf(i); %群体最优适应度 pgx = newpop_P(i,:); %群体最优时间（3个时间段） pnx(it, : )= pgx; %群体最优时间（3个时间段） pop_Cost( i ) = newpop_Cost( i ); pop( i, : ) = newpop_P( i, : ); BestSol_Cost = pop_Cost( i ); BestSol = pop