【机器人学习】六自由度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,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))T^2)/(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))T^2)/(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))T^2)/(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轨迹规划画圆

matlab正逆运动学分析与轨迹规划服务