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

1 次查看（过去 30 天）

Qiaoli Ji 2019-6-30

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/469536-how-to-use-matlab-function-to-planning-trajectory-and-why-i-met-these-errors

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;