【轨迹生成】贝塞尔曲线（Bézier）

1 贝塞尔曲线

• 曲线定义：起始点、终止点（也称锚点）、控制点
• 数据点： 指一条路径的起始点和终止点。
• 控制点：控制点决定了一条路径的弯曲轨迹
• 根据控制点的个数，贝塞尔曲线被分为一阶贝塞尔曲线（0个控制点）、二阶贝塞尔曲线（1个控制点）、三阶贝塞尔曲线（2个控制点）等等。

贝塞尔曲线特点：

• 在贝塞尔曲线中，只有起点和终点在曲线上，其他点均为调整曲线形状和结束的控制点；
• 贝塞尔曲线通过起始点和终止点，并与起始点和终止点的折线相切，在对自车路径规划过程中可根据曲线的起始点和终止点的切线方向确定自车起始点姿态和目标点姿态；
• 至少需要三阶贝塞尔曲线（四个控制点）才能生成曲率连续的路径；
• 若要求两端弧线拼接在一起依然是曲率连续的，必须要求两段弧线在连接处的曲率是相等的；
• 贝塞尔曲线中一个控制点改变，曲线的形状都会随之改变；

贝塞尔曲线的应用：

• 城市环境下局部路径规划，如贝塞尔曲线能够拟合直道和弯道，在曲率变化较大的地方可以选用两个贝塞尔曲线来拟合。
• 无人驾驶车辆的运动规划，目标轨迹曲率是连续的且轨迹的曲率不超过车辆可行驶轨迹曲率的限制。

---

% 贝塞尔曲线法:基于贝塞尔曲线规划换道轨迹
clc
clear
close all

%% 
% 定义控制点
d = 3.5;
P0 = [0, -d/2];
P1 = [25, -d/2];
P2 = [25, d/2];
P3 = [50, d/2];
P = [P0; P1; P2; P3];

% 直接根据贝塞尔曲线定义式得到路径点
n = length(P)-1;
Path = [];
for t = 0:0.01:1
    if n == 1
        p_t = P;
    elseif n >= 2
        p_t = [0, 0];
        for i = 0:n
            k_C = factorial(n) / (factorial(i) * factorial(n-i));
            k_t = (1-t)^(n-i) * t^i;
            p_t = p_t + k_C * k_t * P(i+1,:);
        end
        Path(end+1,:) = p_t;
    
    else
        disp('控制点输入有误，请重新输入')
    end
end


%% 画图
d = 3.5;               % 道路标准宽度
W = 1.8;               % 汽车宽度
L = 4.7;               % 车长
figure
len_line = 50;

% 画灰色路面图
GreyZone = [-5,-d-0.5; -5,d+0.5; len_line,d+0.5; len_line,-d-0.5];
fill(GreyZone(:,1),GreyZone(:,2),[0.5 0.5 0.5]);
hold on
fill([P0(1),P0(1),P0(1)-L,P0(1)-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'b')  

% 画分界线
plot([-5, len_line],[0, 0], 'w--', 'linewidth',2);  %分界线
plot([-5,len_line],[d,d],'w','linewidth',2);     %左边界线
plot([-5,len_line],[-d,-d],'w','linewidth',2);  %左边界线

% 设置坐标轴显示范围
axis equal
set(gca, 'XLim',[-5 len_line]); 
set(gca, 'YLim',[-4 4]); 

% 绘制路径
scatter(P(:,1),P(:,2),'g')
plot(P(:,1),P(:,2),'r');%路径点
scatter(Path(:,1),Path(:,2),200, '.b');%路径点