【自动驾驶】运动规划丨轨迹规划丨Bspline

如图1所示，随机生成横纵向的坐标点

随机横纵向采样点

如图2所示，随机生成的81条折线轨迹

随机候选的折线轨迹

如图3所示，81条折线的候选成本值

81条折线的候选成本值


如图4所示，筛选出的折线轨迹

筛选出的折线轨迹

如图5所示，筛选出的Bspline轨迹

筛选出的Bspline轨迹

如图6所示，Bspline轨迹的航向角和曲率

航向角和曲率

function [cx,cy,cyaw] = cubic_spline(wx,wy)
%    figure
%     plot(wx,wy,'ro');
%     hold on

N = length(wx);
A = zeros(N,N);
B = zeros(N,1);

for i = 1:N-1
    h(i) = wx(i+1) - wx(i);
end

A(1,1) = 1;
A(N,N) = 1;
for i = 2:N-1
    A(i,i) = 2*(h(i-1) + h(i));
    A(i,i+1) = h(i);
    A(i,i-1) = h(i-1);
    B(i) = 6* (wy(i+1)-wy(i))/h(i) - 6* (wy(i)-wy(i-1))/h(i-1);
end


m= A\B

for i = 1:N
    a(i) = wy(i);
    c(i) = m(i)/2;
    d(i) =( c(i+1)-c(i) )/(3*h(i));

end

for i = 1:N-1
    b(i)  = (a(i+1)-a(i))/h(i)- h(i)/3*(c(i+1)+ 2*c(i));
    d(i) =( c(i+1)-c(i) )/(3*h(i));

end
cx= [];
cy = [];
for  i= 1:N-1
    X = wx(i):0.1:wx(i+1);
    Y = a(i)+ b(i)*(X-wx(i)) + c(i) * (X- wx(i)).^2 + d(i) * (X - wx(i)).^3;
    cx = [cx,X];
    cy = [cy,Y];
    %        plot(X, Y,'g-','LineWidth',3)
end

for i = 1: length(cx)-1
    yaw(i) = atan((cy(i+1)-cy(i))/(cx(i+1)- cx(i)));
end
yaw(end+1) = yaw(end);
cyaw = yaw;

end