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

조회 수: 1 (최근 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