3-5-3机械臂轨迹规划matlab仿真
发布时间
阅读量：
阅读量
查阅了许多文献中有关于三次多项式插值和五次多项式插值的内容 经过一番查找后 并未发现有关3-5-3混合型多项式插值的具体代码实现 这可能是因为这类问题相对简单而没有深入探讨 通常这类较为基础的研究不会深入到混合型插值的具体实现 为了彻底弄清楚这个问题的实现细节 我决定动手编写这段代码 虽然这项工作看似简单 但对于刚接触MATLAB的新手来说仍有一定难度
本文代码借鉴：<>
编写代码时我也不是很确定是否正确，请各位大神指正和指导。纯小白一枚。
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;
代码解读位置，速度，加速度 图：
还有一点地方我是没能完全理解清楚吧？为什么不选择其他的模式呢？比如说...如果换成别的方法会不会更好呢？这样做却集两端之弊啊，在变化幅度上非常显著的同时还伴随着明显的波动性呢。这让我开始怀疑自己是不是哪里理解错了原理
感谢各位纠错！！
全部评论 (0)
还没有任何评论哟~