how to use matlab-function to planning trajectory and why I met these errors?

2 次查看(过去 30 天)
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;
微信图片_20190630120506.png
This is my planning trajectory.
Size mismatch

回答(0 个)

类别

Help CenterFile Exchange 中查找有关 MATLAB 的更多信息

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by