robotics section 7 matlab practice

题目

problem

代码

a

clear

%定义已知量
syms theta_s theta_f t_f
theta_s=120;
theta_f=60;
t_f=1;

%运用公式算出系数
a0=theta_s;
a1=0;
a2=3/t_f^2*(theta_f-theta_s);
a3=-2/t_f^3*(theta_f-theta_s);

%得出多项式并多次求导
syms theta(t)
theta=a0+a1*t+a2*t^2+a3*t^3
theta1=diff(theta)
theta2=diff(theta1)
theta3=diff(theta2)

%作图
subplot(2,2,1)
fplot(theta,[0,1])
xlabel('s')
ylabel('deg')
title('position')
subplot(2,2,2)
fplot(theta1,[0,1])
xlabel('s')
ylabel('deg/s')
title('velocity')
subplot(2,2,3)
fplot(theta2,[0,1])
xlabel('s')
ylabel('deg/s^{2} ')
title('acceleration')
subplot(2,2,4)
fplot(theta3,[0,1])
xlabel('s')
ylabel('deg/s^{3} ')
title('acceleration change rate')

b

clear

%定义已知量
syms theta_s theta_f t_f theta_s_1 theta_f_1 theta_s_2 theta_f_2
theta_s=120;
theta_f=60;
theta_s_1=0;
theta_f_1=0;
theta_s_2=0;
theta_f_2=0;
t_f=1;

%运用公式算出系数
a0=theta_s;
a1=theta_s_1;
a2=theta_s_2/2;
a3=(20*theta_f-20*theta_s-(8*theta_f_1+12*theta_s_1)*t_f-(3*theta_s_2-theta_f_2)*t_f^2)/(2*t_f^3);
a4=(30*theta_s-30*theta_f+(14*theta_f_1+16*theta_s_1)*t_f+(3*theta_s_2-2*theta_f_2)*t_f^2)/(2*t_f^4);
a5=(12*theta_f-12*theta_s-(6*theta_f_1+6*theta_s_1)*t_f-(theta_s_2-theta_f_2)*t_f^2)/(2*t_f^5);

%得出多项式并多次求导
syms theta(t)
theta=a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5
theta1=diff(theta)
theta2=diff(theta1)
theta3=diff(theta2)

%作图
subplot(2,2,1)
fplot(theta,[0,1])
xlabel('s')
ylabel('deg')
title('position')
subplot(2,2,2)
fplot(theta1,[0,1])
xlabel('s')
ylabel('deg/s')
title('velocity')
subplot(2,2,3)
fplot(theta2,[0,1])
xlabel('s')
ylabel('deg/s^{2} ')
title('acceleration')
subplot(2,2,4)
fplot(theta3,[0,1])
xlabel('s')
ylabel('deg/s^{3} ')
title('acceleration change rate')

c

clear

%定义已知量
syms theta_s theta_v theta_f t1 t2
theta_s=60;
theta_v=120;
theta_f=30;
t1=1;
t2=1;

%符号化为与书上公式相同
theta0=theta_s;
theta_g=theta_f;
t_f=t1;

%运用公式算出系数
a10=theta0;
a11=0;
a12=(12*theta_v-3*theta_g-9*theta0)/(4*t_f^2);
a13=(-8*theta_v+3*theta_g+5*theta0)/(4*t_f^3);
a20=theta_v;
a21=(3*theta_g-3*theta0)/(4*t_f);
a22=(-12*theta_v+6*theta_g+6*theta0)/(4*t_f^2);
a23=(8*theta_v-5*theta_g-3*theta0)/(4*t_f^3);

%得出两个多项式，并将其连接，然后求导
syms theta(t)
theta=piecewise(0<=t<t1,a10+a11*t+a12*t^2+a13*t^3,t1<=t<=t1+t2,a20+a21*(t-t1)+a22*(t-t1)^2+a23*(t-t1)^3)
theta1=diff(theta)
theta2=diff(theta1)
theta3=diff(theta2)

%作图
subplot(2,2,1)
fplot(theta,[0,2])
xlabel('s')
ylabel('deg')
title('position')
subplot(2,2,2)
fplot(theta1,[0,2])
xlabel('s')
ylabel('deg/s')
title('velocity')
subplot(2,2,3)
fplot(theta2,[0,2])
xlabel('s')
ylabel('deg/s^{2} ')
title('acceleration')
subplot(2,2,4)
fplot(theta3,[0,2])
xlabel('s')
ylabel('deg/s^{3} ')
title('acceleration change rate')

d

clear

theta_s=120;
theta_f=60;
t_f=1;
steps=100;%点个数为100
[q,qd,qdd]=jtraj(theta_s,theta_f,steps);
t=linspace(0,t_f,steps)';
qddd=diff(qdd)./diff(t);%用做差相除求加速度变化率

%作图
subplot(2,2,1)
plot(t,q)
xlabel('s')
ylabel('deg')
title('position')
subplot(2,2,2)
plot(t,qd)
xlabel('s')
ylabel('deg/s')
title('velocity')
subplot(2,2,3)
plot(t,qdd)
xlabel('s')
ylabel('deg/s^{2} ')
title('acceleration')
subplot(2,2,4)
plot(t(1:end-1),qddd)
xlabel('s')
ylabel('deg/s^{3} ')
title('acceleration change rate')

结果

d