搞机器人路径规划的人大概率都有过这种经历全局规划给出一条直线折线车走到拐点的时候不是急停就是急转看着都心疼。更麻烦的是把轨迹交给运动控制器时速度和加速度在这里直接断掉不加平滑根本跑不起来。我最早接触贝塞尔曲线就是为了解决这类问题。后来在避障小车、机械臂轨迹、喷涂路径规划这些场景里贝塞尔曲线几乎成了默认的平滑方案。这篇文章把贝塞尔曲线的原理、Matlab实现、路径规划里的实际应用一次性讲透代码直接给可跑通版本。适合刚接触路径规划的初学者也适合想快速在Matlab里出结果的工程师。1. 贝塞尔曲线的数学本质与图形直觉1.1 从一根橡皮筋说起什么是贝塞尔曲线如果只给一个控制点贝塞尔曲线就是那个点本身。给两个控制点它就是一条直线段。麻烦的是三个及以上的控制点这时候曲线怎么走你可以在心里想象一块木板上钉了几颗钉子然后把一根橡皮筋绕过这些钉子。橡皮筋不会紧贴每颗钉子而是被它们共同“拉”着形成一个光滑的弧线。贝塞尔曲线就是这根橡皮筋的数学表达。它最反直觉的地方在于除了第一个和最后一个控制点曲线上绝大多数点都不经过中间控制点。中间控制点只是“吸引”曲线向自己的方向弯曲距离越近吸引越强。这跟很多初学者想的“用曲线把点连起来”完全不是一回事但恰恰是这个特性让贝塞尔曲线在路径平滑中特别有用——你不需要让轨迹精确穿过每一个“想法点”只需要让轨迹被这些点引导就能得到一条安全、平滑、可执行的路径。贝塞尔曲线的数学定义是用Bernstein基函数对控制点做加权平均阶数等于控制点数量减一。如果有n1个控制点曲线方程长这样P(u) Σ (i0到n) B(i,n)(u) * P(i)u ∈ [0,1]其中B(i,n)(u) C(n,i) * u^i * (1-u)^(n-i)C(n,i)是组合数。这个式子看着唬人但拆开看就是一个加权平均的推广每个控制点乘以一个权重所有权重加起来恒等于1。u从0走到1的过程中权重分配不断变化曲线就从起点平滑移动到终点。1.2 德卡斯特里奥几何构造法公式归公式真要直观理解贝塞尔曲线的形状变化我推荐德卡斯特里奥算法。这个算法把曲线生成变成了一组反复的线性插值手工都能算。以三个控制点P0、P1、P2为例生成一条二次贝塞尔曲线。先取参数u在线段P0-P1上按比例u找点A满足A (1-u)P0 uP1在线段P1-P2上同样找点B满足B (1-u)P1 uP2。然后把A和B再按比例u连一次得到点C (1-u)A uB。让u从0到1遍历一遍点C的轨迹就是二次贝塞尔曲线。三个控制点做两次线性插值四个控制点就做三次线性插值n1个控制点做n轮插值。这就是德卡斯特里奥算法的核心重复线性插值直到只剩一个点。这个算法最大的价值在于它的数值稳定性。直接按Bernstein基函数公式计算当阶数比较高时u^i和(1-u)^(n-i)的数值可能过大或过小组合数也可能非常大计算机算起来容易丢精度。德卡斯特里奥算法只涉及乘法和加法避免了组合数的大数计算在工程里更稳。我在教学和项目调试里习惯先用德卡斯特里奥算法的思路推导再用Bernstein公式做向量化实现这样既理解几何意义又拿到运行效率。1.3 贝塞尔曲线的几个手算样例直接看一个具体例子。假设控制点P0(0,0)P1(2,4)P2(4,0)求这条二次贝塞尔曲线在u0.5处的点。第一步A 0.5P0 0.5P1 (1,2)。第二步B 0.5P1 0.5P2 (3,2)。第三步C 0.5A 0.5B (2,2)。所以曲线上u0.5处的点是(2,2)正好是抛物线顶点。这个结果也能从公式验证二次贝塞尔曲线公式展开后是P(u) (1-u)²P0 2u(1-u)P1 u²P2带u0.5进去得到0.25P0 0.5P1 0.25P2 (2,2)。两条路结果一致。这种手算的价值在于建立直觉参数u是比例的概念不是时间的概念。曲线上的点始终是对控制点的调和平均权重由当前u决定。后面讲路径规划中速度控制的时候这个区别会非常重要。2. 贝塞尔曲线在路径规划中的工程价值2.1 为什么不用折线和普通多项式拟合路径规划里最朴素的方案是折线连接。机器人先走到A点再走到B点再走到C点。缺点很明显每个转折点处方向突变如果机器人是差速底盘这里必须减速到接近零再转向效率极低如果是机械臂这里会带来加速度冲击机构振动大、寿命缩短。那用高次多项式拟合呢比如让曲线精确经过所有路径点。问题更麻烦。高次多项式对单个点的扰动极其敏感一个噪声点就能让曲线在远离该点的地方剧烈振荡这就是所谓的Runge现象。控制点数量一多多项式阶数水涨船高曲线形态基本不可控。贝塞尔曲线恰好在这两者之间找到了平衡。它的阶数虽然也是由控制点数量决定但曲线的形状被凸包性质紧紧约束住不会像高次多项式那样失控。对于路径规划这种“我需要一条光滑且不会跑偏太远的曲线”的需求贝塞尔曲线是非常自然的候选。2.2 贝塞尔曲线的四个关键几何性质端点插值性。曲线的起点一定在第一个控制点P0上终点一定在最后一个控制点Pn上。这意味着路径规划的起止位置约束可以直接通过第一个和最后一个控制点精确满足非常方便。端点切线方向。曲线在起点的切线方向由P0指向P1在终点的切线方向由P(n-1)指向Pn。这个性质在泊车路径规划里是宝贝因为泊车对起点和终点的航向角有严格要求。只要控制点选在车辆朝向的延长线上曲线出入方向就完全可控。凸包性。整条曲线完全落在所有控制点构成的凸包内。这个性质在避障里极其重要只要把控制点选在障碍物之外曲线就绝对不可能穿过障碍物内部。对于动态避障小车规划时只需要检查凸包是否与障碍物碰撞计算量远小于逐点检查。变差缩减性。曲线与任意直线的交点数量不超过控制点折线与该直线的交点数量。翻译成人话就是曲线比控制点折线更“平稳”不会出现比控制点更剧烈的摆动。这个性质让贝塞尔曲线天然适合生成平滑轨迹不会自己“抽风”。2.3 一个容易忽略的问题参数速度不等于实际速度这是贝塞尔曲线在实际工程中最容易踩的坑。参数u从0均匀增长到1对应的曲线点并不是匀速运动的。因为曲线上点的实际运动速度是|dP/du| * (du/dt)而|dP/du|在曲线不同位置不恒定即使u是线性变化的实际速度也在改变。举个例子四个控制点(0,0)、(0,10)、(10,10)、(10,0)构成的三次贝塞尔曲线u从0到0.5时走过的弧长和u从0.5到1时走过的弧长并不相等。在喷涂路径规划里如果直接把u当作时间喷枪在工件表面移动速度就不均匀涂层厚度会忽厚忽薄在焊接里会导致焊缝质量不一致。解决方案是对曲线做弧长参数化先把曲线按弧长离散采样再建立弧长s到参数u的映射关系最后按期望速度控制s的变化。细节我放到第4章讲这里先记住结论u均匀不等于速度均匀涉及运动控制时必须显式处理弧长与速度的关系。3. Matlab实现与路径规划实战3.1 核心函数基于Bernstein的向量化实现Matlab适合向量化编程。直接用for循环遍历每个采样点算基函数写法直观但效率差采样点一多就慢。这里给出一个向量化实现核心是利用linspace生成u向量再用矩阵运算同时计算所有采样点。function curve bezier_curve(P, res) % 生成贝塞尔曲线 % 输入: % P - 控制点矩阵大小为(n1) x dim每行一个控制点 % res - 采样点数即曲线上生成多少个点 % 输出: % curve - res x dim 的密集曲线点集 n size(P, 1) - 1; dim size(P, 2); u linspace(0, 1, res); curve zeros(res, dim); for i 0:n % Bernstein基函数向量化计算 coeff nchoosek(n, i) .* (u.^i) .* ((1 - u).^(n - i)); % 每个控制点按权重叠加 curve curve coeff * P(i 1, :); end endnchoosek(n, i)计算组合数。u是res×1的列向量u.^i得到每个采样点的u幂次(1-u).^(n-i)得到另一部分两者逐元素相乘后再乘以组合数得到res×1的权重向量。coeff * P(i1,:)是res×1乘1×dim的矩阵乘法结果是res×dim等价于把每个点都平移到控制点的位置并按权重缩放。实测下来100个控制点、5000个采样点的情况下这个函数在普通笔记本上十几毫秒跑完够实时使用的。如果控制点特别多可以用德卡斯特里奥算法的递推实现避免组合数溢出但一般路径规划控制点不会超过十几个这个版本够用。3.2 完整示例小车平滑绕障路径生成假设一台小车要从起点(0,0)走到终点(10,0)中间遇到一个障碍物需要绕过去。传统做法规划出折线路径(0,0)-(3,1)-(7,1)-(10,0)。转折点处方向突变直接跟踪很难受。用贝塞尔曲线把这条折线平滑一下% 控制点: 起点、两个绕障点、终点 P [0, 0; 3, 1; 7, 1; 10, 0]; % 采样生成曲线 res 200; curve bezier_curve(P, res); % 绘图对比 figure; plot(P(:,1), P(:,2), k--o, LineWidth, 1.5); hold on; plot(curve(:,1), curve(:,2), b-, LineWidth, 2.5); legend(控制点折线, 贝塞尔曲线, Location, southeast); title(四控制点Bezier曲线路径平滑); axis equal; grid on;跑完之后你会看到曲线从(0,0)出发平滑绕过中间两个控制点附近最后精确落在(10,0)上。中间两个控制点像磁极一样把曲线“吸”过去但曲线并没有真正触碰到它们。这个例子里最核心的认知是把路径规划的折点直接当作控制点使用不一定能得到理想曲线。想让曲线更贴近或更远离折线需要主动调整控制点位置。工程上一般会从折线顶点沿角平分线方向偏移一定距离偏移量决定曲线的“切弯”程度。这个调参思路在第4章展开。3.3 动态避障场景的贝塞尔生成思路动态避障小车的局部路径规划里贝塞尔曲线也能派上大用场。思路是在每个控制周期内根据当前车辆位置、目标路径上的前瞻点、障碍物位置动态生成一组贝塞尔控制点实时计算局部避障轨迹。具体做法当前车辆位置作为P0车辆航向方向前方1米处作为P1保证轨迹起点方向与当前航向一致。目标路径上的一个前瞻点作为P3保证轨迹终点方向符合全局路径趋势。P2放在障碍物外侧的安全距离上让曲线被“推开”远离障碍物。用三次贝塞尔曲线生成局部轨迹取未来1-2秒的点作为参考轨迹。每个控制周期都重新计算一遍控制点避障结束之后P2逐步回归到方向线上曲线自然恢复为直线。这里面有个工程细节P2偏移方向要和障碍物位置实时联动。最简单的方法是把P2的坐标写成障碍物相对位置函数比如P2 目标方向中点 k * 障碍物单位方向向量k是距离系数。碰撞风险大就增大k风险小就减小k。这样避障的“幅度”就有了连续性不会一帧猛拐、下一帧猛回。我在实测小车时用频率是20Hz每次生成50个采样点整体计算开销可以忽略不计。贝塞尔在这里最大的优势是计算轻、实时性强对嵌入式平台很友好。4. 参数调优与曲线质量检查4.1 控制点布局这五个经验直接抄贝塞尔曲线能不能用得好90%靠控制点布局。这里整理几条实际项目里反复验证过的经验第一控制点数量宁少勿多。很多人习惯把路径上的所有关键点都塞进去当控制点结果控制点多、阶数高曲线开始在个别区域扭曲调整一个点会波及整条曲线。路径规划里我用得最多的是三次贝塞尔即四个控制点一个起点、一个终点、两个中间控制点大多数平滑需求都能满足。点再多也建议拆成多段低阶曲线而不是一段高阶曲线。第二控制点间距和方向要符合直觉。曲线在起点处的方向完全由P0指向P1决定所以P1的方向决定了出口角度。想让车头朝向不发生突变P1必须选在车辆当前朝向的延长线上且与P0的距离应根据期望的“入弯速度”来定距离太短曲线会拐得很急距离太长曲线会偏离预期路径很远。第三对称性是释放控制点位置的捷径。对于绕障这种对称场景让P2和P3中间控制点关于中轴线对称布置曲线对称且没有诡异扭转。对称结构出问题的概率远小于非对称布局。第四请把凸包可视化出来。控制点设好之后先用fill函数把控制点凸包画出来再看凸包是否与障碍物重叠。如果凸包已经碰到障碍物无论怎么微调曲线都没用因为曲线保证落在凸包内正确做法是调整控制点让凸包离开障碍物而不是在那里面试图用“曲线细节”去找安全空间。第五曲线末端方向不能忘。终点的切线方向由P(n-1)指向Pn需要确保这个方向与后续路径衔接方向一致否则即使位置连续方向不连续也会导致机器人到终点时突然转向。泊车场景尤其敏感后面专门讲。4.2 曲率检查与最差点处理路径规划出来之后一个常规检查项是曲线的最大曲率因为曲率直接对应转向半径而转向半径受底盘机械结构限制。差速小车最小转弯半径可能很小但阿克曼底盘或麦克纳姆轮底盘有明确限制。用离散点近似计算曲率的方法很简单function k curvature(curve) x curve(:,1); y curve(:,2); dx gradient(x); dy gradient(y); ddx gradient(dx); ddy gradient(dy); k abs(ddx .* dy - dx .* ddy) ./ (dx.^2 dy.^2).^1.5; end然后找最大曲率位置k curvature(curve); [max_k, idx] max(k); % 最大曲率点对应的曲线位置 worst_point curve(idx, :);如果最大曲率超出底盘限制优先调整离该点最近的控制点把那个控制点往外“拉”增大局部弯曲半径。用手拖动一个控制点并重新运行检查观察最大曲率的变化这样反复几次就有手感了。另外离散求导对噪声敏感密集采样后如果曲率曲线毛刺明显可以先对curve用smoothdata做一次平滑再做曲率计算。曲率分段不连续反而说明曲线拼接点可能有连续性缺陷这是分段贝塞尔时需要重点检查的项目。4.3 Matlab常见报错与排查笔记矩阵维度不匹配。这是初学者最常见的错。P写成一行多列而不是多行两列。报错通常出现在coeff * P(i1,:)这一句如果P是1×n的向量P(i1,:)取到的是单个标量coeff是res×1的列向量两者无法直接相乘。解决办法是用size函数显式检查P维度确保P是(n1)×2的矩阵。nchoosek数值溢出。控制点数量在20以上时nchoosek(20,10)已经是18万多再高阶一些会变得巨大甚至超过double类型的精确表示范围。路径规划控制点数在10个以上就应该警惕解决办法是改用德卡斯特里奥递推算法或者把曲线拆成多段三次贝塞尔。曲线“穿墙”。很多人的第一反应是我控制点明明没靠近障碍物为什么曲线还是穿过障碍物回到凸包性质控制点没靠近障碍物但控制点连成的凸包可能覆盖了障碍物区域。画出凸包把凸包往外推曲线自然外移。曲线起点不贴合当前位置。检查你是否把第一个控制点精确设置为起点还是随手填了个大概值。贝塞尔曲线精确穿过P0如果P0偏了起点就偏了。这在接管全局路径时经常发生车辆实际位置和路径起点之间相差几厘米导致起始段需要紧急修正。典型报错原因排查手段Matrix dimensions must agree控制点矩阵维度错误size(P)应输出[n1, 2]曲线起点偏移P0坐标未设置精确打印P(1,:)与真实起点对比曲线诡异扭曲控制点数量过多/阶数过高拆成多段三次贝塞尔再拼接最大曲率超限中间控制点过于接近折角沿角平分线外移控制点5. 进阶方向多段贝塞尔拼接与工程延伸5.1 多段贝塞尔曲线的C1连续拼接单段贝塞尔曲线控制点一多局部调整变得困难现实中更常用多段三次贝塞尔拼接成一条长轨迹。拼接最关键的是连续性。两段三次贝塞尔曲线第一段控制点是A0、A1、A2、A3第二段控制点是B0、B1、B2、B3。要保证位置连续B0必须等于A3也就是第二段起点就是第一段终点。要保证切线方向连续A3和B1必须位于A2延长线上且满足B1 - A3 A3 - A2或者写成 B1 2*A3 - A2。这个条件的几何意义是第二段的第一个控制点B1是第一段倒数第二个控制点A2关于公共点A3的镜像。这样拼接点处的切线方向一致曲线看起来是一根光滑的线而不是折线。如果需要更平滑的曲率连续C2条件更苛刻一般推荐直接用B样条或NURBS而不是强行拼贝塞尔。绝大多数移动机器人路径规划到C1就够了因为车辆运动学能容忍方向连续曲率跳变则可以通过速度规划里的加速度限制来处理。5.2 泊车路径和喷涂路径里的贝塞尔应用泊车路径规划对这个“C1连续”的需求特别突出。车辆从停车位驶出或者倒车入库起点和终点都有明确的位姿约束。用分段贝塞尔做泊车路径的思路是把整个倒车过程拆成几段每段用三次贝塞尔段与段之间满足C1连续。起点段的P1设置在车辆当前航向延长线上终点段的倒数第二个控制点设置在目标车位朝向的反延长线上。这样车辆从出发到停车全过程航向角都是连续的不会出现在车位门口突然调头的滑稽场面。喷涂路径规划的贝塞尔用法稍有不同。喷枪走的是覆盖折返路径在每个折返点需要平滑过渡。直接用贝塞尔在折返拐角处生成一个平滑的“U形”过渡段可以避免喷枪在角点处减速过度。过渡段的形状直接影响喷涂均匀性所以必须结合前面讲的弧长参数化来设计速度曲线保证喷枪在过渡段和直线段的速度尽可能接近。把贝塞尔当“拐角圆角器”用而不是把所有轨迹都替换掉是最省力的思路。5.3 什么时候该换B样条或NURBS在写这篇文章之前我需要诚实说一句贝塞尔不是万能的。当你遇到以下场景认真考虑换B样条轨迹有几十个甚至上百个需要精确经过的位置点而你又不想把它们变成控制点。B样条可以做到曲线不经过控制点也能逼近形状且支持局部修改改一个点不影响整条曲线。需要曲率连续的复杂路径。三次B样条天然支持C2连续拼接起来比多段贝塞尔省心得多。需要精确控制曲线对控制点折线的逼近程度。NURBS通过权因子可以调节“吸引力”大小贝塞尔做不到。不过B样条和NURBS的代价是参数更多、实现更复杂、调参门槛更高。如果你的场景是“路径点不多、平滑要求适中、计算平台有限”用分段三次贝塞尔的性价比远高于上B样条。我自己的经验是80%的路径规划平滑需求四控制点三次贝塞尔就能解决剩下20%再考虑B样条。从最简单的一段三次贝塞尔绕障到分段拼接长轨迹再到配合速度规划解决执行问题这条技术路线的每一步都不复杂但环环相扣。我在实际项目里最深的体会是贝塞尔曲线真正的门槛不在数学而在你有没有把控制点当成“力的方向”而不是“路径的终点”来理解。搞明白这一点很多调参困惑都会迎刃而解。
贝塞尔曲线原理与Matlab实现：路径规划平滑轨迹实战指南
搞机器人路径规划的人大概率都有过这种经历全局规划给出一条直线折线车走到拐点的时候不是急停就是急转看着都心疼。更麻烦的是把轨迹交给运动控制器时速度和加速度在这里直接断掉不加平滑根本跑不起来。我最早接触贝塞尔曲线就是为了解决这类问题。后来在避障小车、机械臂轨迹、喷涂路径规划这些场景里贝塞尔曲线几乎成了默认的平滑方案。这篇文章把贝塞尔曲线的原理、Matlab实现、路径规划里的实际应用一次性讲透代码直接给可跑通版本。适合刚接触路径规划的初学者也适合想快速在Matlab里出结果的工程师。1. 贝塞尔曲线的数学本质与图形直觉1.1 从一根橡皮筋说起什么是贝塞尔曲线如果只给一个控制点贝塞尔曲线就是那个点本身。给两个控制点它就是一条直线段。麻烦的是三个及以上的控制点这时候曲线怎么走你可以在心里想象一块木板上钉了几颗钉子然后把一根橡皮筋绕过这些钉子。橡皮筋不会紧贴每颗钉子而是被它们共同“拉”着形成一个光滑的弧线。贝塞尔曲线就是这根橡皮筋的数学表达。它最反直觉的地方在于除了第一个和最后一个控制点曲线上绝大多数点都不经过中间控制点。中间控制点只是“吸引”曲线向自己的方向弯曲距离越近吸引越强。这跟很多初学者想的“用曲线把点连起来”完全不是一回事但恰恰是这个特性让贝塞尔曲线在路径平滑中特别有用——你不需要让轨迹精确穿过每一个“想法点”只需要让轨迹被这些点引导就能得到一条安全、平滑、可执行的路径。贝塞尔曲线的数学定义是用Bernstein基函数对控制点做加权平均阶数等于控制点数量减一。如果有n1个控制点曲线方程长这样P(u) Σ (i0到n) B(i,n)(u) * P(i)u ∈ [0,1]其中B(i,n)(u) C(n,i) * u^i * (1-u)^(n-i)C(n,i)是组合数。这个式子看着唬人但拆开看就是一个加权平均的推广每个控制点乘以一个权重所有权重加起来恒等于1。u从0走到1的过程中权重分配不断变化曲线就从起点平滑移动到终点。1.2 德卡斯特里奥几何构造法公式归公式真要直观理解贝塞尔曲线的形状变化我推荐德卡斯特里奥算法。这个算法把曲线生成变成了一组反复的线性插值手工都能算。以三个控制点P0、P1、P2为例生成一条二次贝塞尔曲线。先取参数u在线段P0-P1上按比例u找点A满足A (1-u)P0 uP1在线段P1-P2上同样找点B满足B (1-u)P1 uP2。然后把A和B再按比例u连一次得到点C (1-u)A uB。让u从0到1遍历一遍点C的轨迹就是二次贝塞尔曲线。三个控制点做两次线性插值四个控制点就做三次线性插值n1个控制点做n轮插值。这就是德卡斯特里奥算法的核心重复线性插值直到只剩一个点。这个算法最大的价值在于它的数值稳定性。直接按Bernstein基函数公式计算当阶数比较高时u^i和(1-u)^(n-i)的数值可能过大或过小组合数也可能非常大计算机算起来容易丢精度。德卡斯特里奥算法只涉及乘法和加法避免了组合数的大数计算在工程里更稳。我在教学和项目调试里习惯先用德卡斯特里奥算法的思路推导再用Bernstein公式做向量化实现这样既理解几何意义又拿到运行效率。1.3 贝塞尔曲线的几个手算样例直接看一个具体例子。假设控制点P0(0,0)P1(2,4)P2(4,0)求这条二次贝塞尔曲线在u0.5处的点。第一步A 0.5P0 0.5P1 (1,2)。第二步B 0.5P1 0.5P2 (3,2)。第三步C 0.5A 0.5B (2,2)。所以曲线上u0.5处的点是(2,2)正好是抛物线顶点。这个结果也能从公式验证二次贝塞尔曲线公式展开后是P(u) (1-u)²P0 2u(1-u)P1 u²P2带u0.5进去得到0.25P0 0.5P1 0.25P2 (2,2)。两条路结果一致。这种手算的价值在于建立直觉参数u是比例的概念不是时间的概念。曲线上的点始终是对控制点的调和平均权重由当前u决定。后面讲路径规划中速度控制的时候这个区别会非常重要。2. 贝塞尔曲线在路径规划中的工程价值2.1 为什么不用折线和普通多项式拟合路径规划里最朴素的方案是折线连接。机器人先走到A点再走到B点再走到C点。缺点很明显每个转折点处方向突变如果机器人是差速底盘这里必须减速到接近零再转向效率极低如果是机械臂这里会带来加速度冲击机构振动大、寿命缩短。那用高次多项式拟合呢比如让曲线精确经过所有路径点。问题更麻烦。高次多项式对单个点的扰动极其敏感一个噪声点就能让曲线在远离该点的地方剧烈振荡这就是所谓的Runge现象。控制点数量一多多项式阶数水涨船高曲线形态基本不可控。贝塞尔曲线恰好在这两者之间找到了平衡。它的阶数虽然也是由控制点数量决定但曲线的形状被凸包性质紧紧约束住不会像高次多项式那样失控。对于路径规划这种“我需要一条光滑且不会跑偏太远的曲线”的需求贝塞尔曲线是非常自然的候选。2.2 贝塞尔曲线的四个关键几何性质端点插值性。曲线的起点一定在第一个控制点P0上终点一定在最后一个控制点Pn上。这意味着路径规划的起止位置约束可以直接通过第一个和最后一个控制点精确满足非常方便。端点切线方向。曲线在起点的切线方向由P0指向P1在终点的切线方向由P(n-1)指向Pn。这个性质在泊车路径规划里是宝贝因为泊车对起点和终点的航向角有严格要求。只要控制点选在车辆朝向的延长线上曲线出入方向就完全可控。凸包性。整条曲线完全落在所有控制点构成的凸包内。这个性质在避障里极其重要只要把控制点选在障碍物之外曲线就绝对不可能穿过障碍物内部。对于动态避障小车规划时只需要检查凸包是否与障碍物碰撞计算量远小于逐点检查。变差缩减性。曲线与任意直线的交点数量不超过控制点折线与该直线的交点数量。翻译成人话就是曲线比控制点折线更“平稳”不会出现比控制点更剧烈的摆动。这个性质让贝塞尔曲线天然适合生成平滑轨迹不会自己“抽风”。2.3 一个容易忽略的问题参数速度不等于实际速度这是贝塞尔曲线在实际工程中最容易踩的坑。参数u从0均匀增长到1对应的曲线点并不是匀速运动的。因为曲线上点的实际运动速度是|dP/du| * (du/dt)而|dP/du|在曲线不同位置不恒定即使u是线性变化的实际速度也在改变。举个例子四个控制点(0,0)、(0,10)、(10,10)、(10,0)构成的三次贝塞尔曲线u从0到0.5时走过的弧长和u从0.5到1时走过的弧长并不相等。在喷涂路径规划里如果直接把u当作时间喷枪在工件表面移动速度就不均匀涂层厚度会忽厚忽薄在焊接里会导致焊缝质量不一致。解决方案是对曲线做弧长参数化先把曲线按弧长离散采样再建立弧长s到参数u的映射关系最后按期望速度控制s的变化。细节我放到第4章讲这里先记住结论u均匀不等于速度均匀涉及运动控制时必须显式处理弧长与速度的关系。3. Matlab实现与路径规划实战3.1 核心函数基于Bernstein的向量化实现Matlab适合向量化编程。直接用for循环遍历每个采样点算基函数写法直观但效率差采样点一多就慢。这里给出一个向量化实现核心是利用linspace生成u向量再用矩阵运算同时计算所有采样点。function curve bezier_curve(P, res) % 生成贝塞尔曲线 % 输入: % P - 控制点矩阵大小为(n1) x dim每行一个控制点 % res - 采样点数即曲线上生成多少个点 % 输出: % curve - res x dim 的密集曲线点集 n size(P, 1) - 1; dim size(P, 2); u linspace(0, 1, res); curve zeros(res, dim); for i 0:n % Bernstein基函数向量化计算 coeff nchoosek(n, i) .* (u.^i) .* ((1 - u).^(n - i)); % 每个控制点按权重叠加 curve curve coeff * P(i 1, :); end endnchoosek(n, i)计算组合数。u是res×1的列向量u.^i得到每个采样点的u幂次(1-u).^(n-i)得到另一部分两者逐元素相乘后再乘以组合数得到res×1的权重向量。coeff * P(i1,:)是res×1乘1×dim的矩阵乘法结果是res×dim等价于把每个点都平移到控制点的位置并按权重缩放。实测下来100个控制点、5000个采样点的情况下这个函数在普通笔记本上十几毫秒跑完够实时使用的。如果控制点特别多可以用德卡斯特里奥算法的递推实现避免组合数溢出但一般路径规划控制点不会超过十几个这个版本够用。3.2 完整示例小车平滑绕障路径生成假设一台小车要从起点(0,0)走到终点(10,0)中间遇到一个障碍物需要绕过去。传统做法规划出折线路径(0,0)-(3,1)-(7,1)-(10,0)。转折点处方向突变直接跟踪很难受。用贝塞尔曲线把这条折线平滑一下% 控制点: 起点、两个绕障点、终点 P [0, 0; 3, 1; 7, 1; 10, 0]; % 采样生成曲线 res 200; curve bezier_curve(P, res); % 绘图对比 figure; plot(P(:,1), P(:,2), k--o, LineWidth, 1.5); hold on; plot(curve(:,1), curve(:,2), b-, LineWidth, 2.5); legend(控制点折线, 贝塞尔曲线, Location, southeast); title(四控制点Bezier曲线路径平滑); axis equal; grid on;跑完之后你会看到曲线从(0,0)出发平滑绕过中间两个控制点附近最后精确落在(10,0)上。中间两个控制点像磁极一样把曲线“吸”过去但曲线并没有真正触碰到它们。这个例子里最核心的认知是把路径规划的折点直接当作控制点使用不一定能得到理想曲线。想让曲线更贴近或更远离折线需要主动调整控制点位置。工程上一般会从折线顶点沿角平分线方向偏移一定距离偏移量决定曲线的“切弯”程度。这个调参思路在第4章展开。3.3 动态避障场景的贝塞尔生成思路动态避障小车的局部路径规划里贝塞尔曲线也能派上大用场。思路是在每个控制周期内根据当前车辆位置、目标路径上的前瞻点、障碍物位置动态生成一组贝塞尔控制点实时计算局部避障轨迹。具体做法当前车辆位置作为P0车辆航向方向前方1米处作为P1保证轨迹起点方向与当前航向一致。目标路径上的一个前瞻点作为P3保证轨迹终点方向符合全局路径趋势。P2放在障碍物外侧的安全距离上让曲线被“推开”远离障碍物。用三次贝塞尔曲线生成局部轨迹取未来1-2秒的点作为参考轨迹。每个控制周期都重新计算一遍控制点避障结束之后P2逐步回归到方向线上曲线自然恢复为直线。这里面有个工程细节P2偏移方向要和障碍物位置实时联动。最简单的方法是把P2的坐标写成障碍物相对位置函数比如P2 目标方向中点 k * 障碍物单位方向向量k是距离系数。碰撞风险大就增大k风险小就减小k。这样避障的“幅度”就有了连续性不会一帧猛拐、下一帧猛回。我在实测小车时用频率是20Hz每次生成50个采样点整体计算开销可以忽略不计。贝塞尔在这里最大的优势是计算轻、实时性强对嵌入式平台很友好。4. 参数调优与曲线质量检查4.1 控制点布局这五个经验直接抄贝塞尔曲线能不能用得好90%靠控制点布局。这里整理几条实际项目里反复验证过的经验第一控制点数量宁少勿多。很多人习惯把路径上的所有关键点都塞进去当控制点结果控制点多、阶数高曲线开始在个别区域扭曲调整一个点会波及整条曲线。路径规划里我用得最多的是三次贝塞尔即四个控制点一个起点、一个终点、两个中间控制点大多数平滑需求都能满足。点再多也建议拆成多段低阶曲线而不是一段高阶曲线。第二控制点间距和方向要符合直觉。曲线在起点处的方向完全由P0指向P1决定所以P1的方向决定了出口角度。想让车头朝向不发生突变P1必须选在车辆当前朝向的延长线上且与P0的距离应根据期望的“入弯速度”来定距离太短曲线会拐得很急距离太长曲线会偏离预期路径很远。第三对称性是释放控制点位置的捷径。对于绕障这种对称场景让P2和P3中间控制点关于中轴线对称布置曲线对称且没有诡异扭转。对称结构出问题的概率远小于非对称布局。第四请把凸包可视化出来。控制点设好之后先用fill函数把控制点凸包画出来再看凸包是否与障碍物重叠。如果凸包已经碰到障碍物无论怎么微调曲线都没用因为曲线保证落在凸包内正确做法是调整控制点让凸包离开障碍物而不是在那里面试图用“曲线细节”去找安全空间。第五曲线末端方向不能忘。终点的切线方向由P(n-1)指向Pn需要确保这个方向与后续路径衔接方向一致否则即使位置连续方向不连续也会导致机器人到终点时突然转向。泊车场景尤其敏感后面专门讲。4.2 曲率检查与最差点处理路径规划出来之后一个常规检查项是曲线的最大曲率因为曲率直接对应转向半径而转向半径受底盘机械结构限制。差速小车最小转弯半径可能很小但阿克曼底盘或麦克纳姆轮底盘有明确限制。用离散点近似计算曲率的方法很简单function k curvature(curve) x curve(:,1); y curve(:,2); dx gradient(x); dy gradient(y); ddx gradient(dx); ddy gradient(dy); k abs(ddx .* dy - dx .* ddy) ./ (dx.^2 dy.^2).^1.5; end然后找最大曲率位置k curvature(curve); [max_k, idx] max(k); % 最大曲率点对应的曲线位置 worst_point curve(idx, :);如果最大曲率超出底盘限制优先调整离该点最近的控制点把那个控制点往外“拉”增大局部弯曲半径。用手拖动一个控制点并重新运行检查观察最大曲率的变化这样反复几次就有手感了。另外离散求导对噪声敏感密集采样后如果曲率曲线毛刺明显可以先对curve用smoothdata做一次平滑再做曲率计算。曲率分段不连续反而说明曲线拼接点可能有连续性缺陷这是分段贝塞尔时需要重点检查的项目。4.3 Matlab常见报错与排查笔记矩阵维度不匹配。这是初学者最常见的错。P写成一行多列而不是多行两列。报错通常出现在coeff * P(i1,:)这一句如果P是1×n的向量P(i1,:)取到的是单个标量coeff是res×1的列向量两者无法直接相乘。解决办法是用size函数显式检查P维度确保P是(n1)×2的矩阵。nchoosek数值溢出。控制点数量在20以上时nchoosek(20,10)已经是18万多再高阶一些会变得巨大甚至超过double类型的精确表示范围。路径规划控制点数在10个以上就应该警惕解决办法是改用德卡斯特里奥递推算法或者把曲线拆成多段三次贝塞尔。曲线“穿墙”。很多人的第一反应是我控制点明明没靠近障碍物为什么曲线还是穿过障碍物回到凸包性质控制点没靠近障碍物但控制点连成的凸包可能覆盖了障碍物区域。画出凸包把凸包往外推曲线自然外移。曲线起点不贴合当前位置。检查你是否把第一个控制点精确设置为起点还是随手填了个大概值。贝塞尔曲线精确穿过P0如果P0偏了起点就偏了。这在接管全局路径时经常发生车辆实际位置和路径起点之间相差几厘米导致起始段需要紧急修正。典型报错原因排查手段Matrix dimensions must agree控制点矩阵维度错误size(P)应输出[n1, 2]曲线起点偏移P0坐标未设置精确打印P(1,:)与真实起点对比曲线诡异扭曲控制点数量过多/阶数过高拆成多段三次贝塞尔再拼接最大曲率超限中间控制点过于接近折角沿角平分线外移控制点5. 进阶方向多段贝塞尔拼接与工程延伸5.1 多段贝塞尔曲线的C1连续拼接单段贝塞尔曲线控制点一多局部调整变得困难现实中更常用多段三次贝塞尔拼接成一条长轨迹。拼接最关键的是连续性。两段三次贝塞尔曲线第一段控制点是A0、A1、A2、A3第二段控制点是B0、B1、B2、B3。要保证位置连续B0必须等于A3也就是第二段起点就是第一段终点。要保证切线方向连续A3和B1必须位于A2延长线上且满足B1 - A3 A3 - A2或者写成 B1 2*A3 - A2。这个条件的几何意义是第二段的第一个控制点B1是第一段倒数第二个控制点A2关于公共点A3的镜像。这样拼接点处的切线方向一致曲线看起来是一根光滑的线而不是折线。如果需要更平滑的曲率连续C2条件更苛刻一般推荐直接用B样条或NURBS而不是强行拼贝塞尔。绝大多数移动机器人路径规划到C1就够了因为车辆运动学能容忍方向连续曲率跳变则可以通过速度规划里的加速度限制来处理。5.2 泊车路径和喷涂路径里的贝塞尔应用泊车路径规划对这个“C1连续”的需求特别突出。车辆从停车位驶出或者倒车入库起点和终点都有明确的位姿约束。用分段贝塞尔做泊车路径的思路是把整个倒车过程拆成几段每段用三次贝塞尔段与段之间满足C1连续。起点段的P1设置在车辆当前航向延长线上终点段的倒数第二个控制点设置在目标车位朝向的反延长线上。这样车辆从出发到停车全过程航向角都是连续的不会出现在车位门口突然调头的滑稽场面。喷涂路径规划的贝塞尔用法稍有不同。喷枪走的是覆盖折返路径在每个折返点需要平滑过渡。直接用贝塞尔在折返拐角处生成一个平滑的“U形”过渡段可以避免喷枪在角点处减速过度。过渡段的形状直接影响喷涂均匀性所以必须结合前面讲的弧长参数化来设计速度曲线保证喷枪在过渡段和直线段的速度尽可能接近。把贝塞尔当“拐角圆角器”用而不是把所有轨迹都替换掉是最省力的思路。5.3 什么时候该换B样条或NURBS在写这篇文章之前我需要诚实说一句贝塞尔不是万能的。当你遇到以下场景认真考虑换B样条轨迹有几十个甚至上百个需要精确经过的位置点而你又不想把它们变成控制点。B样条可以做到曲线不经过控制点也能逼近形状且支持局部修改改一个点不影响整条曲线。需要曲率连续的复杂路径。三次B样条天然支持C2连续拼接起来比多段贝塞尔省心得多。需要精确控制曲线对控制点折线的逼近程度。NURBS通过权因子可以调节“吸引力”大小贝塞尔做不到。不过B样条和NURBS的代价是参数更多、实现更复杂、调参门槛更高。如果你的场景是“路径点不多、平滑要求适中、计算平台有限”用分段三次贝塞尔的性价比远高于上B样条。我自己的经验是80%的路径规划平滑需求四控制点三次贝塞尔就能解决剩下20%再考虑B样条。从最简单的一段三次贝塞尔绕障到分段拼接长轨迹再到配合速度规划解决执行问题这条技术路线的每一步都不复杂但环环相扣。我在实际项目里最深的体会是贝塞尔曲线真正的门槛不在数学而在你有没有把控制点当成“力的方向”而不是“路径的终点”来理解。搞明白这一点很多调参困惑都会迎刃而解。