3-5-3机械臂轨迹规划matlab仿真

        查看很多文章有三次多项式插值，五次多项式插值，找了很久没有发现有3-5-3混合多项式插值的代码，想想估计是太简单了， 没有人写这个东西。我当机立断，想把这个代码写出来。虽然简单，但是对于我这种matlab小白来说还是有些吃力。

        本文代码借鉴：https://blog.csdn.net/jldemanman?type=blog

        代码写出来，我也不知道对不对，希望各位大佬能给我提提建议，纯小白一枚。

clear;
clc;
q_array=[0,5,10,20];%指定起止位置
t_array=[0,1.5,2,5];%指定起止时间
v_array=[0,10,20,0];%指定起止速度
a_array=[0,2,5,0];%指定起止加速度

t=t_array(1);%初始状态
q=q_array(1);
v=v_array(1);
a=a_array(1);

for i=1:1:length(q_array)-1;%每一段规划的时间
    if i==1
        a10 = q_array(i);
        a11 = v_array(i);
        a12=(3/(t_array(i+1)-t_array(i))^2)*(q_array(i+1)-q_array(i))-(1/(t_array(i+1)-t_array(i)))*(2*v_array(i)+v_array(i+1));
        a13=(2/(t_array(i+1)-t_array(i))^3)*(q_array(i)-q_array(i+1))+(1/(t_array(i+1)-t_array(i))^2)*(v_array(i)+v_array(i+1));%计算三次多项式系数
        
        tz = t_array(i)+0.001:0.001:t_array(i+1);
        qz = a10+a11*(tz-t_array(i))+a12*(tz-t_array(i)).^2+a13*(tz-t_array(i)).^3;
        vz = a11+2*a12*(tz-t_array(i))+3*a13*(tz-t_array(i)).^2;
        az = 2*a12+6*a13*(tz-t_array(i));
        t=[t,tz];
        q=[q,qz];
        v=[v,vz];
        a=[a,az];
        
    elseif i==2
        T=t_array(i+1)-t_array(i)
        a20=q_array(i);
        a21=v_array(i);
        a22=a_array(i)/2;
        a23=(20*q_array(i+1)-20*q_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);
        a24=(30*q_array(i)-30*q_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);
        a25=(12*q_array(i+1)-12*q_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);%计算五次多项式系数 
        ti=t_array(i):0.001:t_array(i+1);
        qi=a20+a21*(ti-t_array(i))+a22*(ti-t_array(i)).^2+a23*(ti-t_array(i)).^3+a24*(ti-t_array(i)).^4+a25*(ti-t_array(i)).^5;
        vi=a21+2*a22*(ti-t_array(i))+3*a23*(ti-t_array(i)).^2+4*a24*(ti-t_array(i)).^3+5*a25*(ti-t_array(i)).^4;
        ai=2*a22+6*a23*(ti-t_array(i))+12*a24*(ti-t_array(i)).^2+20*a25*(ti-t_array(i)).^3;
        t=[t,ti(2:end)];
        q=[q,qi(2:end)];
        v=[v,vi(2:end)];
        a=[a,ai(2:end)];
    else
        a30 = q_array(i);
        a31 = v_array(i);
        a32=(3/(t_array(i+1)-t_array(i))^2)*(q_array(i+1)-q_array(i))-(1/(t_array(i+1)-t_array(i)))*(2*v_array(i)+v_array(i+1));
        a33=(2/(t_array(i+1)-t_array(i))^3)*(q_array(i)-q_array(i+1))+(1/(t_array(i+1)-t_array(i))^2)*(v_array(i)+v_array(i+1));%计算三次多项式系数
        
        ts = t_array(i)+0.001:0.001:t_array(i+1);
        qs = a30+a31*(ts-t_array(i))+a32*(ts-t_array(i)).^2+a33*(ts-t_array(i)).^3;
        vs = a31+2*a32*(ts-t_array(i))+3*a33*(ts-t_array(i)).^2;
        as = 2*a32+6*a33*(ts-t_array(i));
        t=[t,ts];q=[q,qs];v=[v,vs];a=[a,as];
        
    end
    
end

subplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');grid on;

位置，速度，加速度 图：

还有一点不明白，为什么要使用3-5-3，用5-5-5不好吗？这样写完全是融合了两者的缺点。加速度很大，突变。难道是我理解的不对？？？

感谢各位纠错！！