粒子群算法优化3-5-3多项式工业机器人时间最优轨迹规划算法matlab代码

本资源采用标准粒子群算法对3-5-3多项式进行轨迹优化，目标函数为时间最优，程序为matlab代码，仅供学习参考

%本算法将位置/速度/加速度分成3个时间段一共9个曲线段拼成

%粒子群优化算法:
%主函数源程序
%增加pgf（h）c1c2w设置，速度80
clear all;
clc;
F=2;
NUM=0;
NUM1=0;
NUMFULL=0;
NUMFULL1=0;
%设置PSO参数
m = 20;
D= 3;
pgf = 100;%给定的极大的适应度值
wmax = 0.9;
wmin = 0.3;
c1f= 0.05;
c2f = 0.05;
c1i= 0.02;
c2i= 0.02;
Nmax = 100;%最大迭代次数
%初始化各个粒子的位置和速度
pgf（1）= 10;
pnf（1）= 10;
h = 1;
pnx = zeros（1003）
for i= 1 : m
    for j= 1:D
        x（ij） = rand * （4.0 -0.1）+0.1;
        v（ij） = -2 + 4*rand;
    end
    px（i:） = x（i:）;%每个粒子的最优位置
    pf（i） = fitness（x（i1）  x（i2） x（i3）D）%每个粒子的最优位置的适应度
    if pgf（h）>pf（i）
        a（ : i） = Aa（x（i1）x（i2）x（i3））
        F= 1
        for t= 0:0.01 ; x（i1）
            if （abs（3 * a（1i） * t^2+ 2* a（2i） * t+a（3i）））>（80 * pi/180）    %3次多项式倒数（速度限制公式），速度超过80°/s即归为超速
                F= 0;
                f = 10;
                NUM = NUM +1;
            end
        end
        for t = 0:0.01 : x（i2）
            if （abs（5 * a（5i） * t^4+ 4 * a（6i） * t^3+ 3 * a（7i） * t^2+2 * a（8i） * t +a（9i）））>（80 * pi/180）    %5次多项式倒数（速度限制公式），速度超过80°/s即归为超速
                F=0;
                f = 10;
                NUM= NUM +1;
            end
        end
        for t= 0:0.01:x（i3）
            if （abs（3 * a（111） *t^2+2* a（12i） * t+a（13i）））>（80 * pi/180）   %3次多项式倒数（速度限制公式），速度超过80°/s即归为超速
                F=0;
                f =10;
                NUM = NUM+1;
            end
        end
        if F== 1
            h = h+ 1;
            pgx = x（i:）;%所有粒子的最优位置
            pgf（h）= pf（i）;%所有粒子的最优位置的适应度
            pnf（1） = pgf（h）;
            HI= 1
            NUMEULL= NUMFULL+1
        end
    end
end
for k = 2:Nmax
    c1 = （c1f -c1i） * k /Nmax +c1i;
    c2 = （c2f -c2i） * k/Nmax + c2i;
    w = （wmax - （wmax - wmin）/ exp（（ 4 * k /Nmax）^2））; % w = （ Wmax - k * （ Nmax -wmin）/ Nmax） ;
    for i= 1 :m %粒子数
        for j= 1:D
            v（i j）= w* v（ij）+ c1 * rand * （ px（ij）- x（ij））+ c2 * rand *（pgx（j）-x（ij））;
            if v（ij）<-2
                v（ij）=-2;
            end
            if v（ij）>2
                v（ij） =2;
            end
            x（ij）= x（ij）+v（ij）
            if x（ij）<0.1
                x（ij）= 0.1;
            end
            if x（ij）>4
                x（ij）=4;
            end
            if x（ij）<-4
                x（ij） = 4;
            end
        end
        f = fitness（x（i1）x（i2）x（i3）D）
        if pf（i）>f
            a（ : i） = Aa（x（i1）  x（i2） x（i3））;
            F=1;
            for t= 0:0.01;x（i1）
                if（abs（3 * a（1i） * t^2+ 2* a（2i）* t+a（3i）））>（80 * pi/180）
                    F=0;
                    f =10;
                    NUM1 = NUM1 +1;
                end
            end
            for t= 0:0.01:x（i2）
                if （abs（5 * a（5i） * t^4+ 4 * a（6i） * t^3+ 3 * a（7i）*t^2+ 2* a（8i） * t+a（9i）））>（80 * pi/180）
                    F= 0;
                    f = 10;
                    NUM1 = NU

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-12-09 17:31  粒子群优化3-5-3时间最优\
     文件        6869  2020-11-28 11:24  粒子群优化3-5-3时间最优\liziqun_weizhifensanshijianduan_time_3_5_3.m
     文件        6967  2020-11-28 11:35  粒子群优化3-5-3时间最优\liziqu_2_time_3_5_3.m
     文件        4291  2020-11-27 10:56  粒子群优化3-5-3时间最优\liziqu_time_3_5_3.m
     目录           0  2020-12-09 17:31  粒子群优化3-5-3时间最优\付荣——粒子群优化时间3-5-3\
     文件        8103  2020-11-29 15:44  粒子群优化3-5-3时间最优\付荣——粒子群优化时间3-5-3\liziqun_time_gaijin1.m