一、算法简介

1、贝塞尔曲线于1962年由法国工程师皮埃尔·贝塞尔(Pierre Bézier)所广泛发表，他运用贝塞尔曲线来为汽车的主体进行设计。

2、贝塞尔曲线是应用于二维图形应用程序的数学曲线，由一组称为控制点的向量来确定，给定的控制点按顺序连接构成控制多边形，贝塞尔曲线逼近这个多边形，进而通过调整控制点坐标改变曲线的形状。

二、算法思想

1、对于车辆系统，规划的轨迹应满足以下准则：轨迹连续；轨迹曲率连续； 轨迹容易被车辆跟随，且容易生成。

2、给定n+1个数据点，p0~pn，生成一条曲线，使得该曲线与这些点描述的形状相符。

三、算法推导

1、设P0，P1两个控制点，t取值范围为[0,1]。则贝塞尔曲线生成点可以表达为：

2、针对P0，P1，P2三个控制点，t取值范围为[0,1]。P0和P1构成一阶，P1和P2也构成一阶，即：

在生成的两个一阶点基础上，可以生成二阶贝塞尔点：

设P0，P1，P2，P3个控制点，P0和P1、P1和P2、P2和P3都构成一阶，即：

在生成的三个一阶点基础上，可以生成两个二阶贝塞尔点：

在生成的两个二阶点基础上，可以生成三阶贝塞尔点：

针对P0，P1，P2三个控制点而言，由以下三个递推式得到贝塞尔点：

则贝塞尔点与3个控制点的关系为：

二阶贝塞尔曲线最多一阶导数不为常数。

3、针对P0、P1、P2、P3四个控制点而言，由一下三个递推式得到贝塞尔点

则贝塞尔点与四个控制点的关系为：

最多二阶导数不为常数

4、对于PO，P1，P2,....，Pn共n+1个控制点而言，贝塞尔点定义为：

% 贝塞尔曲线法:直观动图感受贝塞尔曲线行程过程% 作者：Ally% 日期：/1/30clcclearclose all%% 直观动图感受贝塞尔曲线行程过程% 一次贝塞尔曲线P0 = [0, 0];P1 = [1, 1];P = [P0; P1];figure;plot(P(:,1), P(:,2), 'k');MakeGif('一次贝塞尔曲线.gif',1);hold onfor t = 0:0.01:1P_t = (1-t)*P0 + t*P1;scatter(P_t(1), P_t(2), 200, '.r');stringName = "一次贝塞尔曲线：t =" + num2str(t);title(stringName)MakeGif('一次贝塞尔曲线.gif',t*100+1);end% 二次贝塞尔曲线P0 = [0, 0];P1 = [1, 1];P2 = [2, 1];P = [P0; P1; P2];figure;plot(P(:,1), P(:,2), 'k');MakeGif('二次贝塞尔曲线.gif',1);hold onscatter(P(:,1), P(:,2), 200, '.b');for t = 0:0.01:1P_t_1 = (1-t)*P0 + t*P1;P_t_2 = (1-t)*P1 + t*P2;P_t_3 = (1-t)*P_t_1 + t*P_t_2;h1 = scatter(P_t_1(1), P_t_1(2), 300, '.g');h2 = scatter(P_t_2(1), P_t_2(2), 300, '.g');h3 = plot([P_t_1(1), P_t_2(1)], [P_t_1(2), P_t_2(2)], 'g', 'linewidth',2); scatter(P_t_3(1), P_t_3(2), 300, '.r');stringName = "二次贝塞尔曲线：t =" + num2str(t);title(stringName)MakeGif('二次贝塞尔曲线.gif',t*100+1);delete(h1);delete(h2);delete(h3);end% 三次贝塞尔曲线P0 = [0, 0];P1 = [1, 1];P2 = [2, 1];P3 = [3, 0];P = [P0; P1; P2; P3];figure;plot(P(:,1), P(:,2), 'k');MakeGif('三次贝塞尔曲线.gif',1);hold onscatter(P(:,1), P(:,2), 200, '.b');for t = 0:0.01:1P_t_1_1 = (1-t)*P0 + t*P1;P_t_1_2 = (1-t)*P1 + t*P2;P_t_1_3 = (1-t)*P2 + t*P3;P_t_2_1 = (1-t)*P_t_1_1 + t*P_t_1_2;P_t_2_2 = (1-t)*P_t_1_2 + t*P_t_1_3;P_t_3 = (1-t)*P_t_2_1 + t*P_t_2_2;h1 = scatter(P_t_1_1(1), P_t_1_1(2), 300, '.b');h2 = scatter(P_t_1_2(1), P_t_1_2(2), 300, '.b');h3 = scatter(P_t_1_3(1), P_t_1_3(2), 300, '.b');h4 = plot([P_t_1_1(1); P_t_1_2(1)], [P_t_1_1(2); P_t_1_2(2)], 'b', 'linewidth',2);h5 = plot([P_t_1_2(1); P_t_1_3(1)], [P_t_1_2(2); P_t_1_3(2)], 'b', 'linewidth',2);h6 = scatter(P_t_2_1(1), P_t_2_1(2), 300, '.g');h7 = scatter(P_t_2_2(1), P_t_2_2(2), 300, '.g');h8 = plot([P_t_2_1(1); P_t_2_2(1)], [P_t_2_1(2); P_t_2_2(2)], 'g', 'linewidth',2);scatter(P_t_3(1), P_t_3(2), 300, '.r');stringName = "三次贝塞尔曲线：t =" + num2str(t);title(stringName)MakeGif('三次贝塞尔曲线.gif',t*100+1);delete(h1);delete(h2);delete(h3);delete(h4);delete(h5);delete(h6);delete(h7);delete(h8);end

% 贝塞尔曲线法:基于贝塞尔曲线规划换道轨迹% 作者：Ally% 日期：/1/30clcclearclose 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:1if n == 1p_t = P;elseif n >= 2p_t = [0, 0];for i = 0:nk_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,:);endPath(end+1,:) = p_t;elsedisp('控制点输入有误，请重新输入')endend%% 画图d = 3.5;% 道路标准宽度W = 1.8;% 汽车宽度L = 4.7;% 车长figurelen_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 onfill([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 equalset(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');%路径点

function MakeGif(filename,index) f = getframe(gcf); imind = frame2im(f); [imind,cm] = rgb2ind(imind,256); if index==1 imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',0.001);else imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.001);end end

学习自B站：小黎的Ally

视频链接：路径规划与轨迹跟踪系列算法学习_第7讲_贝塞尔曲线法_哔哩哔哩_bilibili