轨迹规划实现 tfaar_example2.7

实现 Trajectory Planning for Automatic Machines and Robots  example 2.7 的例子。使用三次多项式经过一系列路径点。

%tfaar_example2.7
%需要填入的参数为起点和终点的位置   起点和终点的速度 起点和终点的时间 一共6个参数 第七个参数决定是否需要画图
clear
[p1,sd1,sdd1,t1]=cubic_trajectory(10,20,0,-10,0,2,0);
[p2,sd2,sdd2,t2]=cubic_trajectory(20,0,-10,10,2,4,0);
[p3,sd3,sdd3,t3]=cubic_trajectory(0,30,10,3,4,8,0);
[p4,sd4,sdd4,t4]=cubic_trajectory(30,40,3,0,8,10,0);
%将数组进行拼接
  length=size(p1,2)+size(p2,2)+size(p3,2)+size(p4,2);
  position=zeros(1,length);
  velocity=zeros(1,length);
  acceleration=zeros(1,length);
   
    position=[p1,p2,p3,p4]
    velocity=[sd1,sd2,sd3,sd4]
    acceleration=[sdd1,sdd2,sdd3,sdd4]
    t=[t1,t2,t3,t4]   
    %位置图像
    subplot(221);
    plot( t,position);
    set(gca,'XTick',0:0.5:10) 
    %速度图像
    subplot(222);
    plot(t,velocity);
    set(gca,'XTick',0:0.5:10) 
    %加速度图像
    subplot(223);
    plot(t,acceleration);
    set(gca,'XTick',0:0.5:10) 

 图像：