五次多项式代码编写

1、基础知识

在大地坐标系下，五次多项式的约束条件是x(t)起点和终点的位置、速度和加速度约束和

y(x)的起点和终点的位置、速度和加速度约束。 如下所示：

 

始末状态约束：

这里显然始末状态带入，可以得出a0、a1、a2  b0、b1、b2例如

 

当t = 0时，a0 =  x_init，同理带入

 求出a1 = 0；也即是：

  

同时可以得到如下方程

简单变换可得：

也即是：

总结也就是： 

在五次多项式轨迹中： 

 

2、代码编写 

function [xr,yr,thetar,kr,vp,ap] = fcn(t,x_init,y_init)
    dx=100;%终点x
    dy=10;%终点y
    T=30;
    xstart=[x_init,0,0];
    xend=[dx+x_init,0,0];
    ystart=[y_init,0,0];
    yend=[dy+y_init,0,0];
    a=zeros(1,6);
    b=zeros(1,6);
    
    a(1)=xstart(1);
    a(2)=xstart(2);
    a(3)=xstart(3)/2;
    A1=[T^3,T^4,T^5;
        3*T^2,4*T^3,5*T^4;
        6*T,12*T^2,20*T^3];
    B1=[xend(1)-a(1)-a(2)*T-a(3)*T^2;
        xend(2)-a(2)-2*a(3)*T;
        xend(3)-2*a(3)];
    xs=inv(A1)*B1;
    a(4)=xs(1);
    a(5)=xs(2);
    a(6)=xs(3);
    b(1)=ystart(1);
    b(2)=ystart(2);
    b(3)=ystart(3)/2;
    A2=[dx^3,dx^4,dx^5;
        3*dx^2,4*dx^3,5*dx^4;
        6*dx,12*dx^2,20*dx^3];
    B2=[yend(1)-b(1)-b(2)*dx-b(3)*dx^2;
        yend(2)-b(2)-2*b(3)*dx;
        yend(3)-2*b(3)];
    ys=inv(A2)*B2;
    b(4)=ys(1);
    b(5)=ys(2);
    b(6)=ys(3);
    xr=a(1)+a(2)*t+a(3)*t^2+a(4)*t^3+a(5)*t^4+a(6)*t^5;
    yr=b(1)+b(2)*xr+b(3)*xr^2+b(4)*xr^3+b(5)*xr^4+b(6)*xr^5;
    xr_dot=a(2)+2*a(3)*t+3*a(4)*t^2+4*a(5)*t^3+5*a(6)*t^4;
    yr_dx=b(2)+2*b(3)*xr+3*b(4)*xr^2+4*b(5)*xr^3+5*b(6)*xr^4;
    yr_dot=yr_dx*xr_dot;
    thetar=atan(yr_dx);
    xr_dot2=2*a(3)+6*a(4)*t+12*a(5)*t^2+20*a(6)*t^3;
    yr_dx2=2*b(3)+6*b(4)*xr+12*b(5)*xr^2+20*b(6)*xr^3;
    yr_dot2=yr_dx2*xr_dot^2+yr_dx*xr_dot2;
    kr=yr_dx2/((1+yr_dx^2)^1.5);
    vp=sqrt(xr_dot^2+yr_dot^2);
    if xr_dot2>=0
        ap=sqrt(xr_dot2^2+yr_dot2^2);
    else
        ap=-sqrt(xr_dot2^2+yr_dot2^2);
    end

3、结果展示 

这里参考了B站老王的代码与思路。