用matlab做一个简单的机械手轨迹三次曲线和抛物线插补习题

题目如下（《机器人基础》P155.1）

%第七章题目，单连杆机械手关节静止位置为θ0=-5.该机械手从静止位置开始在4s内平滑转动到θf=80°停止位置。试进行下列计算：
%1）计算完成此运动并使机械臂停在目标点的三次曲线的系数
%2)计算带抛物线过渡的线型插值的各个参数
%3)画出该关节的位移、速度和加速度曲线

（以下代码为无初速度情况，可以改代码角度，分段曲线的衔接没弄好懒得想了，欢迎指导。）

clc ,clear
%先列一下题目条件的数据
tf=4;
t0=0;
theta0=-5;
thetaf=80;

%准备
syms  t1 t2
p=0.001%精度

%==1
t1=0:p:tf;%时间从0到4s，步长为0.1
A2=((thetaf-theta0)*3)/(tf)^2
A3=-((thetaf-theta0)*2)/(tf)^3
theta1=theta0+A2*t1.^2+A3*t1.^3;



%==2
LimAlpha=4*(thetaf-theta0)/tf/tf%根据公式，加速度>21.25，自己取个值
alpha=3;
tb=tf/2-sqrt((alpha*tf)^2-4*(thetaf-theta0)*alpha)/2/alpha; %第一段抛物线末时间
vb=alpha*tb;
t2=0:p:tf;
%====接下来构造一个分段函数，分别分a1、a2、a3====
a1=theta0+0.5*alpha*(t2).^2;

mid=a1(fix(tb*100)/100/p+1);
a2=vb*t2+(mid-vb*tb);
%抛物线为y=ax²+bx+c，对称点是(m，n)，则中心对称的抛物线y=-ax²+(4am-b)x+2n-4am²-2bm-c
%对称点（2，42.5）
a3=-0.5*alpha*(t2).^2+(4*tf/2*0.5*alpha)*t2+(thetaf-theta0)-4*(tf/2)^2*0.5*alpha+theta0;
%theta2=-theta0+0.5*25*(t2).^2;
theta2=a1.*(t2<tb)+a2.*(t2>=tb&t2<=(tf-tb))+a3.*(t2>(tf-tb));


dtt1=diff(theta1)/p;
dtt2=diff(theta2)/p;

dtt11=diff(theta1,2)/p;
dtt22=diff(theta2,2)/p;


%==3
subplot(1,3,1)
plot(t1,theta1,'LineWidth',4)

xlabel( '                                                                                     t/s')
ylabel( '                                                                             \Theta/°')
hold on;
plot(t2,theta2,'--','LineWidth',5)
box off

hold on;

subplot(1,3,2)
plot(t1(:,1:length(dtt1)),dtt1,'LineWidth',4)
axis([0,4,-40,40])
xlabel( '                                                                                     t/s')
ylabel( '                                                                             \Theta’/°')
hold on;
plot(t2(:,1:length(dtt2)),dtt2,'--','LineWidth',5)
box off 

subplot(1,3,3)
plot(t1(:,1:length(dtt11)),dtt11,'LineWidth',4)
axis([0,4,-0.05,0.05])
xlabel( '                                                                                     t/s')
ylabel( '                                                                             \Theta’/°')
hold on;
plot(t2(:,1:length(dtt22)),dtt22,'--','LineWidth',5)
box off 

得到的位移、速度和加速度曲线如下