how to use matlab-function to planning trajectory and why I met these errors?
Afficher commentaires plus anciens
function q = hip_wangfu30_guihua(t)
% q0=hip(:,1);
% qf=hip(:,2);
% qf1=hip(:,3);
q0=30; qf=-30; qf1=30;
% total_time=linspace(0,2,7);
total_time=[0,0.3,0.7,1,1.3,1.7,2];
% total_time=period;
% t0=0; tb=0.3; th=0.7; tf=1;
t0=total_time(:,1);
tb=total_time(:,2);
th=total_time(:,3);
tf=total_time(:,4);
tb1=total_time(:,5);
th1=total_time(:,6);
tf1=total_time(:,7);
step=0.0001;
t1=t0:step:tb;
t2=tb:step:th;
t3=th:step:tf;
t4=tf:step:tb1;
t5=tb1:step:th1;
t6=th1:step:tf1;
% 30至-30 抛物线段加速度计算
ddq=(qf-q0)/(tb*(tf-tb));
qb=q0+0.5*ddq*(tb^2);
% t1 首段方程
a01=q0+t0^2*(qb-q0)/(tb-t0)^2;
a02=-2*t0*(qb-q0)/(tb-t0)^2;
a03=(qb-q0)/(tb-t0)^2;
q1=a01+a02*t1+a03*(t1.^2);
v1=a02+2*a03*t1;
a1=2*a03*ones(1,length(t1));
% t2 线性段方程
k=v1(end);
% k=a2(end)+2*a3(end)*t1(end);
b=qb-tb*k;
q2=k*t2+b;
v2=k*ones(1,length(t2));
a2=zeros(1,length(t2));
qh=k*th+b;
% t3 尾段方程
b1=qh+((qf-qh)*th*(th-2*tf))/(tf-th)^2;
b2=2*tf*(qf-qh)/(tf-th)^2;
b3=(qf-qh)/(-(tf-th)^2);
q3=b1+b2*t3+b3*t3.^2;
v3=b2+2*b3*t3;
a3=2*b3*ones(1,length(t3));
% -30至30抛物线段加速度计算
ddq1=(qf1-qf)/(tb*(tf1-tb1));
qb1=qf+0.5*ddq1*(tb^2);
% t4 首段方程
c1=qf+tf^2*(qb1-qf)/(tb1-tf)^2;
c2=-2*tf*(qb1-qf)/(tb1-tf)^2;
c3=(qb1-qf)/(tb1-tf)^2;
q4=c1+c2*t4+c3*(t4.^2);
v4=c2+2*c3*t4;
a4=2*c3*ones(1,length(t4));
% t5 线性段方程
k1=v4(end);
% k=a2(end)+2*a3(end)*t1(end);
b1=qb1-tb1*k1;
q5=k1*t5+b1;
v5=k1*ones(1,length(t5));
a5=zeros(1,length(t5));
qh1=k1*th1+b1;
% t6 尾段方程
d1=qh1+((qf1-qh1)*th1*(th1-2*tf1))/((tf1-th1)^2);
d2=2*tf1*(qf1-qh1)/(tf1-th1)^2;
d3=(qf1-qh1)/(-(tf1-th1)^2);
q6=d1+d2*t6+d3*t6.^2;
v6=d2+2*d3*t6;
a6=2*d3*ones(1,length(t6));
T=[t1,t2,t3,t4,t5,t6];
q=[q1,q2,q3,q4,q5,q6];
v=[v1,v2,v3,v4,v5,v6];
a=[a1,a2,a3,a4,a5,a6];
if t<=t1(end)
Q=a01+a02*t+a03*(t.^2);
elseif t>t1(end) && t<t2(end)
Q=k*t+b;
elseif t>=t2(end) && t<t3(end)
Q=b1+b2*t+b3*t.^2;
elseif t>=t3(end) && t<t4(end)
Q=c1+c2*t+c3*(t.^2);
elseif t>=t4(end) && t<t5(end)
Q=k1*t+b1;
else
Q=d1+d2*t+d3*t.^2;
end
q=Q;

This is my planning trajectory.
Size mismatch
Réponses (0)
Catégories
En savoir plus sur MATLAB dans Centre d'aide et File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!