刚接手一个轨迹平滑项目时，我第一反应是把离散路径交给三次 B 样条去处理，这样最后得到的速度和加速度曲线都比较干净。Matlab 里跑起来之后，最初我以为瓶颈在后面的约束检查上，毕竟要算逐点速度、曲率、避障距离。可是用 profile 看完才发现，整个程序耗时的重灾区居然是“对三次 B 样条逐点求值”这一步。一千个采样点，每个点循环调用一次递归基函数，外层再包一层优化迭代，算一次目标函数慢得像是死循环。

后来我把这部分的写法彻底换掉，核心思路不是去优化某一个基函数子程序，而是把 B 样条求值从“点对点的函数调用”改成“一次线性代数运算”。这个改动让整个 Matlab 程序在优化循环里提速非常明显，尤其是控制点数量和采样点数都过百之后，差别可以达到一个数量级以上。这也是我想分享的主要经验：三次 B 样条本身并不慢，慢是因为你在循环里反复建结构、反复调函数、反复做没有必要的重复计算。

1. 越标准的三次 B 样条写法，往往越慢

很多 Matlab 例程里的 B 样条基函数都是抄的教科书版本，逻辑没有错，但运行效率比较糟糕。这套经典递归公式，也就是 Cox-de Boor 递推，几乎每个搞过样条的人都在论文附录里见过：

• p 次基函数由 p 次递归得到；
• 每个参数点 u 最终只对少数 p+1 个控制点有贡献；
• 但如果按“所有基函数都要算一遍”的思路实现，实际计算量就会膨胀。

问题不在公式本身，而在你把这些公式当成标量函数调用。每个采样点调用一次，每次从零开始递归，中间还有大量数组切片和条件判断。我在当时程序里就是直接在 for 循环里写 N = coxdeBoor(knots, 3, u)，采样点有一千个，控制点有十几个，这么算下来除了慢，还特别难扩展。

1.1 只有“正确”的递归写法，没有“高效”的递归写法

Cox-de Boor 递归的经典形式是：

code复制function Nip = baseFun(knots, p, i, u)
if p == 0
    if u >= knots(i) && u < knots(i+1)
        Nip = 1;
    else
        Nip = 0;
    end
    return;
end

然后一路递归到 p=0。当你对每个 i、每个采样点、每次迭代都这么调，Matlab 的函数调用开销、递归栈开销和标量比较都会变成真正的负担。更严重的是，这种写法让循环里很难利用 Matlab 的自动向量化优化。Matlab 底层矩阵运算很快，但脚本级 for 循环加上大量判断语句，会反复进入解释路径，即使循环次数不多也能感到明显卡顿。

1.2 把 B 样条展开成“一条公式”是另一个极端

还有一类程序，为了绕开循环，会把三次 B 样条在某组节点下展开成关于参数 u 的分段多项式。这个办法在做小型离线计算时还好，但只要你把节点改成非均匀分布，或者为了拟合复杂路径增加控制点数量，那条“公式”的长度会立刻变得没法看。更麻烦的是，如果你用了符号工具箱去推导这个表达式，后续每次代入数值都会产生巨大的中间字符串，跑起来反而更慢。

我在实际中遇到的场景是：控制点本身就是优化变量。这意味着我不可能在优化前提前把 B 样条展开成静态的分段多项式，因为每一轮迭代控制点都在变化。可仔细想一下就会发现，节点向量、采样点参数、以及每个采样点对应的基函数索引，在优化过程中是不变的。真正变化的，只有控制点那一组数值。

这个观察是整体提速的突破口。

1.3 先别急着调优化器，让 profile 告诉你卡在哪

那次调试我做过一个比较典型的错误判断：看到目标函数里有避障约束，就去检查是不是每轮迭代都要算上千次距离。结果在一大堆绘图和数据保存代码里，真正最耗时的是 B 样条求值。所以建议任何优化程序在动手重写之前，一定要先用 profile on 和 profile viewer 跑一遍。不要凭感觉猜瓶颈，尤其不要怀疑“这么小的 B 样条能有多慢”。当递归发生在最内层，而它又被优化迭代调用几百次时，慢就是必然的。

提示：用 profile 时，给一次完整的单目标函数评估计时即可，别看整个主脚本。如果程序太大，可以先把绘图关闭。profile 结果出来后，把占用时间前三的函数列表截图保存，再开始重构。

2. 真正的加速核心：把基函数抽成固定矩阵

B 样条曲线的定义是：

code复制C(u) = Σ N(i,p)(u) * P(i)

其中 N(i,p)(u) 是第 i 个 p 次基函数，P(i) 是控制点。若采样点有 m 个，控制点有 n+1 个，则可以把所有采样点下的基函数值写成一个 m×(n+1) 的矩阵 B，那么整条曲线在所有采样点上的求值就是一个矩阵乘法：

code复制Q = B * P

B 矩阵只依赖节点向量和采样参数分布。只要这两个东西在优化过程中不变，B 就只需要计算一次。之后不管控制点 P 怎么变化，求值都只是一次矩阵乘。

2.1 从“采样点循环”变成“矩阵乘法”之后发生了什么

从代码结构上看，原来至少要两层循环：

code复制for iter = 1:maxIter
    for u = uList
        value = computeBsplinePoint(knots, deg, ctrlPts, u);
    end
end

改成矩阵形式后变成：

code复制Bmat = buildBsplineMatrix(knots, deg, uList); % 只做一次
...
ctrlPts = reshape(x, nCtrl, dim);  % x 是优化变量
traj = Bmat * ctrlPts;             % 每轮迭代只执行这一行

如果你有 Curve Fitting Toolbox，也可以用 spmak 创建样条结构，然后用 fnval 批量求值。但我个人更推荐把自己这层矩阵显式写出来，因为后续如果要算目标函数对控制点的雅可比，显式矩阵可以直接参与求导和约束拼接。

这个改写带来的收益会随着采样点数的增加而变大。控制点数量 n 较小、采样点 m 也不大时，差别可能只在几十毫秒；但当 m 达到几千、优化迭代达到几十上百轮时，原来被反复调用的基函数计算会在整体耗时中占据主导地位。

2.2 如何组织节点向量和采样参数才算稳

很多样条例程把节点向量默认成均匀节点：

code复制knots = [0 0 0 0 1 2 3 4 4 4 4];

这样做简单，但对路径优化未必合理。节点向量里每相邻两个节点之间的区间，对应样条的一段。路径弯曲大、速度变化快的地方，应该用更多节点或更密的控制点去表达。均匀节点导致所有段的“表达能力”平均分配，结果就是有些地方样条压不住，有些地方又因为多余自由度出现波动。

采样参数 uList 也不是简单地在节点范围里等距取点就行。如果是路径平滑问题，控制点之间最好按弦长比例分配参数。最简单的做法是先算折线路径各段长度，用累计长度占比作为参数 u 的参考，再在节点向量设计时让节点间隔尽量接近这些参数增量。

这些细节看似与“加速”无关，但其实影响非常大。一旦节点向量设计不合理，后面优化器会反复在目标函数和约束上做无谓尝试，总运行时间可能上升好几倍。

2.3 给一个可以直接抄的矩阵构建思路

显式构建 B 矩阵的方法不复杂，关键是不要用双重循环把每个基函数都算一遍。每个采样参数 u 在 p=3 时，实际只会影响连续的 4 个控制点。我们要做的是先定位 u 落在哪个节点区间，然后只计算这 4 个非零基函数值，写入对应的列位置。

code复制function Bmat = buildBsplineMatrix(knots, p, uList)
% knots  完整节点向量，p=3
% uList  采样参数向量
% Bmat   大小 length(uList) x (length(knots)-p-1)

nCtrl = length(knots) - p - 1;
Bmat = zeros(length(uList), nCtrl);

for j = 1:length(uList)
    u = uList(j);
    
    % 找到 u 所在的节点区间索引
    span = find(u >= knots, 1, 'last');
    if u == knots(end)
        span = find(u > knots, 1, 'last');
    end
    span = min(max(span, p+1), nCtrl);  % 保证有足够基函数可以使用
    
    % 当前区间内只有 p+1 个非零基作用于控制点索引 span-p 到 span
    N = zeros(1, p+1);
    N(1) = 1;
    
    for r = 1:p
        saved = 0;
        for k = 1:r
            temp = N(k);
            denomLeft = knots(span - r + k) - knots(span - p + k - 1);
            if denomLeft ~= 0
                N(k) = saved + (u - knots(span - p + k - 1)) / denomLeft * temp;
            else
                N(k) = saved;
            end
            denomRight = knots(span + k) - knots(span - r + k);
            if denomRight ~= 0
                saved = (knots(span + k) - u) / denomRight * temp;
            else
                saved = 0;
            end
        end
        N(r+1) = saved;
    end
    
    Bmat(j, span-p:span) = N;
end

Bmat = sparse(Bmat);

此代码是 The NURBS Book 里常用基础算法的直接改写，只做了少量防越界处理。真正的关键点是 span 和 N 的关系。如果你和我一样经常忘记索引，强烈建议先打印前几行 Bmat，验证某一个控制点权重在自己预期区间内是 1，区间外是 0，再进入优化验证。

sparse(Bmat) 在采样点很多、控制点也很多时非常有用。因为每一行只有 p+1 个非零元素，用稀疏矩阵存储后再和稠密控制点矩阵相乘，Matlab 会自动调用高效的稀疏存储路径。在几百个采样点规模下，这个收益可能不明显，但在几千个采样点下可以明显减少内存开销。

3. 把 B 样条塞进优化器时的三条关键路径

当 B 样条只是用来画一条平滑曲线时，矩阵化提速已经够用。但多数场景下，你的 Matlab 程序是一个优化循环：控制点待优化，目标函数里又有运动学约束、碰撞约束或者加速度限制。要让矩阵化的 B 样条真正压榨出性能，还要注意下面三个关键路径。

3.1 目标函数里不要重复创建临时 B 样条结构

我见过不少代码，进入 fmincon 的 objective function 后才去拼节点向量和采样点。这意味着每评估一次目标函数，就要重新调用一次 buildBsplineMatrix。这个函数只要跑一次，就已经把提速空间消耗得差不多了。

正确的做法是用嵌套函数或者匿名函数把 Bmat 预先传进去。假设你的控制点是 20 个点，每个点有 x/y/z 三个坐标，则优化变量是一个 60 维向量，那么可以这样写：

code复制Bmat = buildBsplineMatrix(knots, 3, uList);

costFun = @(x) objFun(x, Bmat, otherParams);

options = optimoptions('fmincon', 'Algorithm', 'sqp', 'Display', 'iter');
xOpt = fmincon(costFun, x0, A, b, Aeq, beq, lb, ub, nonlcon, options);

在 objFun 内部，只把 x 重新变成控制点矩阵：

code复制function cost = objFun(x, Bmat, otherParams)
    nCtrl = size(Bmat, 2);
    dim = numel(x) / nCtrl;
    ctrlPts = reshape(x, nCtrl, dim);
    pathPts = Bmat * ctrlPts;
    ...
end

这条链路是整个提速方案的核心。原因很简单：Bmat 与优化变量无关，它只需要在进入优化器之前被组装一次。之后所有迭代都只发生一次矩阵乘法。reshape 的开销几乎可以忽略，不要因为节省一个 reshape 而用 x 的直接索引去重构控制点，那样反而会引入大量不直观的矩阵切片。

3.2 速度、加速度曲线也能用 Bmat 的“近亲”矩阵算

三次 B 样条的好处是它自带连续的二阶导数。路径平滑程序通常会关注速度和加速度，但很多代码在求导时是先求出离散路径点，再一次次用差分。差分会让短噪声被放大，而且差分本身需要做点之间的向量减法，在一些约束里很琐碎。更好的办法是直接对控制点做加权矩阵。

例如给定控制点 P，速度曲线的一次导数节点向量不是原来那组节点，而是去掉首尾重复端节点后的新节点向量。导数曲线仍然是一组控制点 P' 对应的 B 样条，但控制点数减一。如果你需要目标函数里的速度分布，可以直接用差分的稠密矩阵构造“导数操作矩阵”，本质上就是原始 Bmat 对应求导后的样条矩阵。这样三个矩阵可以在 optimize 前一并算好：

code复制displacementMatrix = Bmat;
velocityMatrix    = buildBsplineMatrix(derivedKnots, 2, uList);
accelerationMatrix= buildBsplineMatrix(derivedKnots2, 1, uList);

每次迭代时，速度和加速度都是一次矩阵乘法。这让约束函数的计算保持高度向量化，不需要把差分逻辑写进目标函数。

当然，如果你只是想快速验证，也可以在路径点生成后用一个干净的差分算子 diff(pathPts, 1, 1) 得到近似速度。对于验证程序逻辑，这样做没有问题。等真正需要高精度或优化加速时，再把差分换成上面这个对偶矩阵结构。

3.3 写约束函数时优先提供雅可比矩阵

fmincon 的 SQP 和 interior-point 算法都支持目标函数梯度与非线性约束雅可比。很多用户不愿意写梯度，全交给 fmincon 用有限差分去估计。对于控制点很多、采样点很多的 B 样条问题，有限差分会反复调用目标函数，每次调用又涉及所有矩阵乘法，整体时间会相当可观。

好在 B 样条求值是线性操作，约束函数对控制点的雅可比矩阵几乎就是 Bmat 与各种约束偏导的乘积。虽然没有必要把所有情况都手动写出来，但只要提供一部分关键约束的梯度，优化器迭代次数就可能下降不少。

我个人的建议是：

• 第一阶段先用数值差分跑通整个问题，确认数学上没有报错；
• 第二阶段把目标函数拆成“当前位置偏差 + 平滑惩罚 + 约束越界惩罚”，对其中线性项手工梯度；
• 第三阶段再用 checkGradients 函数比较手工梯度和数值差分，减少梯度写错的概率。

提示：不要为了追求雅可比完整性而写出一个上千行的雅可比函数。梯度正确性比梯度完整性更重要。只要保证主要约束有梯度，剩下的交给优化器自动差分，整体效率依然比完全不写梯度高很多。

4. 节点向量与参数化：决定优化最终质量的小细节

当你把 B 样条框架搭得足够快以后，会发现真正影响最终路径质量的，往往不是算法复杂度，而是节点向量设计。节点向量一旦给错，后面的三次样条再光滑也没有实际意义。我总结了三个经常踩、且影响明显的点。

4.1 端点插值的钳制节点

默认三次 B 样条通常不经过首尾控制点。在路径规划里，如果你的起点和终点必须是固定坐标，就要在节点向量两端各重复 p+1 次节点值。以 p=3 为例，节点向量应该以 4 个相同值开头、4 个相同值结尾。例如：

code复制knots = [0 0 0 0 0.1 0.25 0.5 0.75 1 1 1 1];

这样样条会在起点位置通过第一个控制点，在终点位置通过最后一个控制点。没有做这个 clamp 时，优化器只能靠约束强行把样条拉向起点和终点，结果会在端点附近出现不必要的弯曲。

4.2 节点分布与实际参数分布错位

更细一步的问题是：节点向量里的中间节点，应该反映出路径复杂度的分布。假设一条路径中段有非常大的转弯，而其余部分几乎直线，那么中段需要更密的控制点。如果控制点是均匀分布在参数范围内的，直线段占用过多控制点，转弯处反而容易过冲。

常见做法是把控制点初始位置按照弦长参数化均匀摆放，然后在复杂段多插控制点。这样做的代价是控制点数增加，但在矩阵化加速下，多 10 个或 20 个控制点对运行时间影响不大，却能让优化出来的曲线质量显著提高。

4.3 不要过早用样条去拟合带噪声的路径点

我看到一些项目在拿到原始路径点后，直接让 B 样条控制点在优化器里逼近所有路径点，结果目标函数里所有采样点都要参与距离惩罚。路径点如果有几百个，控制点最好控制在 10~30 个之间。过多的控制点会增加优化自由度，也让路径更容易被噪声带偏。

正确的做法是先把原始路径点降采样或滤波一遍，或者在目标函数里不追求逐点逼近，而是采用分段距离阈值。B 样条天然是光滑曲线，不需要用大量控制点去穿过每一个点。少控制点、多约束检验点才是优化场景下最高效的组合。

我常用的目标函数设计是：用 Bmat 把整条路径样本算出来，但距离惩罚只取那些靠近障碍物或偏离走廊中心的点。这样既能快速评估，又不会让大量噪声点主导优化方向。算惩罚时再对路径点做一个阈值判断，超过阈值才累加，这部分用 Matlab 的向量逻辑索引也可以一步完成：

code复制violation = max(baseDistance - threshold, 0);
cost = cost + sum(violation.^2);

这类写法比在循环里逐个判断要快得多，而且代码更短。

5. 实测数据与几个容易误解的“继续加速”方案

我这边的实际测试场景是一个平面路径优化问题。控制点数取 20，采样点取 1000，优化迭代约 100 轮。旧写法里，每轮迭代都要重新对每个采样点调用一次递归基函数计算，总耗时大约 27 秒。改成预计算 Bmat 并只在迭代内做两次矩阵乘法后，同样迭代总耗时降到 2.3 秒。没有改优化算法，没有改节点数量，也没有加并行计算，只是去掉了一次又一次重复的的样条结构计算。

5.1 并行计算不要轻易上

很多同事看到慢就想着上 parfor。B 样条求值这种问题，如果瓶颈在递归调用或稀疏矩阵组装，开并行池反而会因为任务太小而更慢。真正并行化应该用在目标函数内多个障碍物的距离计算上，或者多组初始点的暴力搜索上。单目标函数里的矩阵乘法本身已经是高度多线程化的，开并行池收益有限。

5.2 Matlab Coder 并不总是最优解

用 Matlab Coder 把样条代码转成 C MEX 确实能提升一些循环逻辑的耗时，但这也是有代价的。第一，递归基函数转成 C 后结构会变得更繁琐；第二，如果你的目标是快速验证不同优化策略，MEX 编译和调试周期会让迭代变慢。

矩阵化的 Bmat 方案本身已经足够接近 Matlab 底层能力。在没有遇到内存瓶颈或极端实时性要求之前，直接使用矩阵乘法通常是最佳性价比。只有当你确认目标函数依然很慢，且主要耗时在大量标量逻辑判断而非矩阵乘法时，再考虑 Coder 转 C。

5.3 三次 B 样条不只是“曲线拟合”的工具

最后说一句题外话。很多人把 B 样条当成数据拟合工具，其实在程序优化里，它更重要的价值是作为“参数化设计变量”的手段。当你要优化的对象是整条曲线、而不是有限几个离散点时，控制点就是比离散点合理得多的优化变量。通过三次 B 样条，控制点的数目可以远小于离散路径点数目，同时还能保证路径的二阶连续。这让目标函数维度降低，约束梯度更容易预测，收敛速度也会明显更快。

我做轨迹规划时，原先变量数量是路径点数量的两倍，约束函数里需要逐点做可通行检查。换成三次 B 样条控制点后，变量数量只有原来的三分之一左右，虽然增加了样条求值成本，但因为变量维度和约束复杂度同时下降，总体反而快很多。再加上本文介绍的 Bmat 预计算，整个流程从“勉强能跑”变成“可以放到优化器里反复试探不同初始猜测”的工具。

6. 关于基函数列索引的避坑编外记

重构 Bmat 的时候，最容易出错的是基函数索引和控制点编号的对齐。教科书里控制点 P(i) 可能从 0 开始，而 Matlab 数组从 1 开始。节点向量索引也分 0 基与 1 基两套体系，稍不留神就会整体平移一个位置。

我的调试方法比较笨但有效：先用最少数量的控制点做一个已知形状，比如 4 个控制点形成一个三次曲线，手动算出中间一个采样点的理论位置，再和 Bmat 乘出来的结果对比。如果对不上，就打印 Bmat 的前几行看非零列位置。只要最基础的验证通过，后面加再多控制点都不会错。

另外，端点处有个小坑。当采样参数 u 恰好等于最后一个节点值时，标准 Cox-de Boor 实现里的 find(u >= knots, 1, 'last') 返回的区间可能超出有效范围。这时要做一个额外判断，把区间索引限制到最后一个有效区间，否则 Bmat 最后一行会出现越界或全零。代码示例里已经加了这句保护，你在自己实现时一定也要处理。

矩阵化和预计算只是第一步，真正让程序快多少，最终取决于你把哪一部分放进了优化循环、哪一部分留在循环外。我的原则很简单：凡是优化过程中不变化的量，全部提到循环外算好；凡是变化但能用一次矩阵乘法表达的，绝不写标量循环。按这个原则改出来的 Matlab 程序，通常轮廓看起来都会非常干净，运行时间也不会让人失望。