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站老王的代码与思路。