Trajectory Planning （2）——三次样条的程序实现

Matlab Code:

% referce:Trajectory Planning for Automatic Machines and Robots
% Page:17-Example 2.7-2.8
% time:2019/09/22
% Note：several parabolic trajectory
%*************************************************************************%
clc;clear all;
syms tt;
t=[0,2,4,8,10];
q=[10,20,0,30,40];
v=[0,-10,10,3,0];
[qt,vt,at,jt]=myCubicTraj(t,q,v);


%%
% 分段函数处理
k=0:0.05:10;
for i=1:length(k)
    if (t(1)<k(i) & k(i)<=t(2))   %注意使用逻辑运算符&
        qt1=qt(1);
        qty(i)=subs(qt1,{tt},{k(i)});
        
        vt1=vt(1);
        vty(i)=subs(vt1,{tt},{k(i)});
        
        at1=at(1);
        aty(i)=subs(at1,{tt},{k(i)});
        
        jt1=jt(1);
        jty(i)=subs(jt1,{tt},{k(i)});
        
    elseif (t(2)<k(i) & k(i)<=t(3))
        qt2=qt(2);
        qty(i)=subs(qt2,{tt},{k(i)});
        
        vt2=vt(2);
        vty(i)=subs(vt2,{tt},{k(i)});
        
        at2=at(2);
        aty(i)=subs(at2,{tt},{k(i)});
        
        jt2=jt(2);
        jty(i)=subs(jt2,{tt},{k(i)});
        
    elseif (t(3)<k(i) & k(i)<=t(4))
        qt3=qt(3);
        qty(i)=subs(qt3,{tt},{k(i)});
        
        vt3=vt(3);
        vty(i)=subs(vt3,{tt},{k(i)});
        
        at3=at(3);
        aty(i)=subs(at3,{tt},{k(i)});
        
        jt3=jt(3);
        jty(i)=subs(jt3,{tt},{k(i)});  
               
    elseif (t(4)<k(i) & k(i)<=t(5))
        qt4=qt(4);
        qty(i)=subs(qt4,{tt},{k(i)});
        
        vt4=vt(4);
        vty(i)=subs(vt4,{tt},{k(i)});
        
        at4=at(4);
        aty(i)=subs(at4,{tt},{k(i)});
        
        jt4=jt(4);
        jty(i)=subs(jt4,{tt},{k(i)});
    end
    
end

%%
%绘图
subplot(2,2,1);plot(k,qty,'linewidth',0.5);title('qt');grid on;
subplot(2,2,2);plot(k,vty,'linewidth',0.5);title('vt');grid on;
subplot(2,2,3);plot(k,aty,'linewidth',0.5);title('at');grid on;
subplot(2,2,4);plot(k,jty,'linewidth',0.5);title('jt');grid on;


%%
function [qt,vt,at,jt]=myCubicTraj(t,q,v)
% 多段曲线，每段曲线为抛物线形式
%输入参数：[t],[q],[v]，行矩阵
%输出参数：曲线表达式
%位置和速度约束
%*************************************%
%经过n个点拟合n-1段曲线构成
%    t      T     qt    vt
%    t0     -     q0    v0
%    t1   t1-t0   q1    v1
%    t2   t2-t1   q2    v2
%    t3   t3-t2   q3    v3
%    t4   t4-t3   q4    v4
% *************************************%
% q(t) = a0 + a1(t − t0) + a2(t − t0).^2 + a3(t − t0).^3, t0 ≤ t ≤ t1

syms tt;
N=size(t,2);   %获取节点数
if N<2
    print("Error! N must more than 2.\n");
    return;
end
for k=1:N-1
    a0(1,k)=q(1,k);
    a1(1,k)=v(1,k);
    a2(1,k)=(3*(q(1,k+1)-q(1,k))-(2*v(1,k)+v(1,k+1))*(t(1,k+1)-t(1,k)))/(t(1,k+1)-t(1,k)).^2;
    a3(1,k)=(-2*(q(1,k+1)-q(1,k))+(v(1,k)+v(1,k+1))*(t(1,k+1)-t(1,k)))/(t(1,k+1)-t(1,k)).^3;
    
    qt(k,:) = a0(1,k)+a1(1,k)*(tt-t(1,k))+a2(1,k)*((tt-t(1,k))).^2+a3(1,k)*((tt-t(1,k))).^3;
    qt(k,:)= vpa(qt(k,:));  %转化为数值
    vt(k,:)= diff(qt(k,:));
    at(k,:)= diff(vt(k,:));
    jt(k,:)= diff(at(k,:));    
end
end

Output: