赞
踩
- 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;
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。