【机器人学习】六自由度puma560轨迹规划画圆(末端指向有有圆心)-matlab代码
matlab代码下载视频五次多项式子代码t_array=linspace(0,4,length(q_array));%指定起止时间v_array=linspace(0,0,length(q_array)-2);%指定起止速度v_array=[0,v_array,0];%指定起止速度a_array=linspace(0,0,length(q_array)-2);%指定起止加速度a_array=[5,
matlab代码下载
视频
五次多项式子代码
t_array=linspace(0,4,length(q_array));%指定起止时间
v_array=linspace(0,0,length(q_array)-2);%指定起止速度
v_array=[0,v_array,0];%指定起止速度
a_array=linspace(0,0,length(q_array)-2);%指定起止加速度
a_array=[5,a_array,5];%指定起止加速度
t=t_array(1);q=q_array(1);v=v_array(1);a=a_array(1);%初始状态
for i=1:1:length(q_array)-1%每一段规划的时间
T=t_array(i+1)-t_array(i);
a0=q_array(i);
a1=v_array(i);
a2=a_array(i)/2;
a3=(20q_array(i+1)-20q_array(i)-(8v_array(i+1)+12v_array(i))T-(3a_array(i)-a_array(i+1))T2)/(2*T3);
a4=(30q_array(i)-30q_array(i+1)+(14v_array(i+1)+16v_array(i))T+(3a_array(i)-2a_array(i+1))T2)/(2*T4);
a5=(12q_array(i+1)-12q_array(i)-(6v_array(i+1)+6v_array(i))T-(a_array(i)-a_array(i+1))T2)/(2*T5);
%计算五次多项式系数
ti=t_array(i):0.02:t_array(i+1);
qi=a0+a1(ti-t_array(i))+a2(ti-t_array(i)).2+a3*(ti-t_array(i)).3+a4(ti-t_array(i)).4+a5*(ti-t_array(i)).5;
vi=a1+2a2(ti-t_array(i))+3a3(ti-t_array(i)).2+4*a4*(ti-t_array(i)).3+5a5(ti-t_array(i)).^4;
ai=2a2+6a3*(ti-t_array(i))+12a4(ti-t_array(i)).2+20*a5*(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)];
end
六自由度puma560轨迹规划画圆
更多推荐
所有评论(0)