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

2 views (last 30 days)
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

Answers (0)

Categories

Find more on MATLAB in Help Center and File Exchange

Community Treasure Hunt

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

Start Hunting!