简介无论是图形学中的曲线设计还是几何建模与工程计算B样条曲线编程都是重要的基础技能。这套基于MATLAB的B样条实现包包含25个文件其中23个为.m源程序按功能分为基函数生成、Deboor递推求值、曲线逼近与估计、导数计算、GUI交互等多个模块同时提供多个可直接运行的示例脚本便于对照学习输出结果另有README和license.txt分别说明使用方法与许可信息压缩包整体仅22KB非常轻量。目前已有360人学习下载适合希望从代码层面理解B样条原理的学生和开发者。通过研读核心函数与示例读者既能明白控制点、节点向量与基函数如何共同决定曲线形状也能掌握可复用的编程思路顺利迁移到CAD建模、数据拟合或机器人轨迹规划等实际任务中。1. 为什么 B 样条曲线比 Bezier 更适合做轨迹点生成做机械臂路径、无人机航迹或者 CAD 曲线时控制点一多Bezier 的全局性就开始拖后腿你只想微调中间一个点整条曲线都跟着变形。B 样条用节点向量把曲线切成分段多项式每个控制点只影响附近几段局部修改、局部收敛都更直观。MATLAB 里做 B 样条有两条常见路线:一条是照着 Cox-de Boor 递推手写基函数适合理解原理、完全可控;另一条是直接用bsplinepolytraj生成轨迹点适合快速做路径规划。这篇把两条路都打通从基函数、节点向量、参数怎么设到手写代码和工具箱函数怎么互相验证最后补一个反算控制点的技巧。2. B 样条的基函数、节点向量与阶数参数怎么设才算对2.1 从 Bezier 到 B 样条控制点为什么只影响局部Bezier 曲线的 Bernstein 基函数在整个区间都有非零值所以任何一个控制点变化都会改变整条曲线。B 样条把区间切成若干段第 i 个 p 次基函数只在局部节点区间上有支撑。给定 n1 个控制点 P_1,...,P_{n1} 和次数 p曲线方程写作C(u) Σ_{i1}^{n1} N_{i,p}(u) * P_i这里 N_{i,p}(u) 由 Cox-de Boor 递推定义。0 次基函数只有一段非零高次基函数由低次线性插值得到递推公式就是 B 样条编程的核心N_{i,0}(u) 1当 u ∈ [k_i, k_{i1})否则为 0N_{i,p}(u) (u - k_i)/(k_{ip} - k_i) * N_{i,p-1}(u) (k_{ip1} - u)/(k_{ip1} - k_{i1}) * N_{i1,p-1}(u)递推里分母是相邻节点差。节点出现重复时分母可能为 0这时候对应项直接按 0 处理这是后面 MATLAB 代码里必须显式判断的地方。2.2 节点向量怎么设Clamped、均匀与重复度节点向量 knots 决定曲线在参数空间里的分段方式。同样是 5 个控制点、3 次曲线用不同节点向量得到的曲线性质差别很大。最常用的三种如下表。节点向量类型构造方式边界行为典型用途Clamped两端节点各重复 p1 次中间均匀分布曲线经过首末控制点端点切线沿控制多边形首末边轨迹规划、曲线插值非 Clamped 均匀节点在区间内等距分布两端不重复曲线不经过端点控制点但参数分布均匀形状设计、光顺拟合周期闭合首尾节点循环相接曲线闭合拼接处连续性好闭环路径、齿轮轮廓轨迹规划里我一般直接用 Clamped因为它保证起点和终点就是控制点的第一个和最后一个物理上更容易对接。比如 5 个控制点、p3 的 Clamped 节点向量0 重复 4 次、1 重复 4 次中间只有一个内部节点 0.5构造代码是p 3; nctrl 5; numInner nctrl - p - 1; % 内部节点个数 1 inner linspace(0, 1, numInner 2); % 先取 [0, 0.5, 1] inner inner(2:end-1); % 去掉端点只剩 0.5 knots [zeros(1, p1), inner, ones(1, p1)];numInner等于控制点数量减 p 再减 1这个式子决定了 Clamped 节点向量的内部节点数。当控制点恰好比次数多 1 时numInner为 0节点向量退化成两端各 p1 个重复节点B 样条就退化为 Bezier。后面写通用代码时linspace(0,1,numInner2)这种写法天然覆盖了退化情况。2.3 维度对不上就报错n、p、knots 的计数关系B 样条编程里最常见的报错就是维度对不上。控制点数量、次数和节点向量长度之间是严格等式控制点数量 p 1 节点向量长度。用符号表示n1 个控制点对应 n 作为最大索引节点向量长度必须是 np2。符号含义约束关系n1控制点个数至少 p1 个p曲线次数线性为 1抛物线为 2三次为 3knots节点向量length(knots) np2u参数值定义域 [knots(p1), knots(n1)]通常取 [0,1]MATLAB 索引从 1 开始所以基函数下标也要整体平移。很多人把 np2 记成 np1差一个 1spcol或者spmak立刻报错。如果你的节点向量来自linspace(0,1,N)而没有做 Clamped 处理长度就会差 p1这类错误用length(knots)一行命令就能查出来。3. 用 MATLAB 手写 Cox-de Boor 递推从基函数到曲线点3.1 一个文件跑通的 bspline_curve.m先给一个可以直接抄的完整函数。它输入控制点矩阵、次数、节点向量和一组参数点输出每个参数对应的曲线坐标同时附带基函数矩阵方便和工具箱函数对照。function [C, Nmat] bspline_curve(ctrlpts, p, knots, uq) % [C, Nmat] bspline_curve(ctrlpts, p, knots, uq) % ctrlpts : (n1) x dim每行是一个控制点 % p : 曲线次数 % knots : 节点向量长度 np2 % uq : 参数点列向量取值在 [knots(1), knots(end)] % C : numel(uq) x dim每个参数点对应的曲线坐标 % Nmat : numel(uq) x (n1)每个参数点对应的基函数值 uq uq(:); numQ numel(uq); n size(ctrlpts, 1) - 1; N zeros(n 1, numQ); for s 1:numQ u uq(s); % 右端点是闭区间统一平移到端点左侧避免半开区间漏值 if u knots(end) u knots(end) - eps * max(1, abs(knots(end))); end % 0 次基函数N_{i,0}(u) 1当 u 落在第 i 个节点区间 for i 1:n 1 if u knots(i) u knots(i 1) N(i, s) 1; end end % Cox-de Boor 递推k 从 1 到 p for k 1:p for i 1:n 1 - k d1 knots(i k) - knots(i); d2 knots(i k 1) - knots(i 1); left N(i, s); if d1 0 left left * (u - knots(i)) / d1; else left 0; % 重复节点分母为 0项为 0 end right N(i 1, s); if d2 0 right right * (knots(i k 1) - u) / d2; else right 0; end N(i, s) left right; end end end C N * ctrlpts; Nmat N; end3.2 半开区间与重复节点递推里最容易错的三个地方第一个坑是 0 次基函数的区间判定。标准定义是左闭右开也就是 u knots(i) 且 u knots(i1)。但如果 u 恰好等于 knots(end)最后一个区间是 [1,1)任何数都不满足所以代码里先把右端点左移一个机器 epsilon。第二个坑是重复节点。Clamped 节点向量里两端各有 p1 个相同节点递推时会遇到 d1 或 d2 为 0必须先判断再赋值否则 MATLAB 会产生 NaN 并在整个矩阵里扩散。第三个坑是内层循环的覆盖顺序。left取的是 N(i,s) 的旧值right取的是 N(i1,s) 的旧值因为内层循环从左往右走i1 位置还没有被本层覆盖取到的仍然是上一层的值这个顺序写反了结果就是错的。调用示例给一组 2D 控制点生成 200 个曲线点并绘图p 3; ctrlpts [1 0; 2 3; 4 2; 6 3; 8 1]; numInner size(ctrlpts, 1) - p - 1; inner linspace(0, 1, numInner 2); knots [zeros(1, p 1), inner(2:end-1), ones(1, p 1)]; uq linspace(0, 1, 200); [C, Nmat] bspline_curve(ctrlpts, p, knots, uq); figure(Color, w); plot(ctrlpts(:,1), ctrlpts(:,2), o--); hold on; plot(C(:,1), C(:,2), LineWidth, 1.6); legend(控制点, B样条曲线, Location, best);uq是参数采样200 个点足够画出平滑的三次曲线。C的第 j 行对应uq(j)处的曲线坐标和ctrlpts的行布局完全一致不需要转置。3.3 用 spcol 做数值对照验证手写递推最怕算法对但索引错。Curve Fitting Toolbox 提供spcol直接生成 B 样条配置矩阵可以用它来验证Nmat的正确性误差应该在 1e-14 量级A spcol(knots, p 1, uq); disp(max(max(abs(Nmat - A))));spcol的第二个参数是 order也就是 p1不是 p。返回矩阵的每一行对应一个参数点每一列对应一个基函数。如果你的Nmat和A形状一致、数值接近递推就写对了。如果发现列数对不上先检查节点向量长度是不是 np2再看spcol传入的是 p 还是 p1这两个原因占了九成报错。4. bsplinepolytraj 实战生成 B 样条路径点并验证轨迹4.1 bsplinepolytraj 最小调用示例Robotics System Toolbox 里的bsplinepolytraj把 B 样条曲线和轨迹规划封装在一起。它和手写代码的坐标布局不同控制点按列存每列是一个控制点行对应坐标维度。最小调用就是一个函数ctrlpts [0 1 2 3 3 2; 0 2 2 0 -2 -2]; % 每列一个控制点 tInterval [0 10]; tSamples linspace(tInterval(1), tInterval(2), 300); [traj, tvec, info] bsplinepolytraj(ctrlpts, tInterval, tSamples); dt tvec(2) - tvec(1); vel gradient(traj, dt); acc gradient(vel, dt); figure(Color, w); plot(ctrlpts(1,:), ctrlpts(2,:), ko--); hold on; plot(traj(1,:), traj(2,:), LineWidth, 1.6); quiver(traj(1,1:30:end), traj(2,1:30:end), ... vel(1,1:30:end), vel(2,1:30:end), 0.5); legend(控制点, B样条轨迹, 速度, Location, best);traj是 2x300 的位置序列tvec是每个点对应的时间info里带轨迹段的多项式系数和时间信息用disp(info)查看具体字段。速度用gradient差分得到后面会说明为什么这只是观察用的近似值。4.2 ctrlpts、tInterval、tSamples 三个参数怎么调参数形状作用注意事项ctrlptsdim x N控制点每列一个列数过少时曲线光顺性差一般是 5 个以上tInterval1x2轨迹起止时间起止时间必须严格递增tSamples1xNs输出点的采样时刻必须落在 tInterval 区间内可以非均匀trajdim x Ns轨迹位置第一点对应 tInterval(1)最后一点对应 tInterval(2)vel/accdim x Ns速度/加速度由梯度差分得到末端有截断误差tSamples不要求均匀这在工程里很有用。比如无人机经过某片净空区时需要更高频率的路径点就可以在 tSamples 里局部加密轨迹点会自动按 B 样条参数插值。控制点的排列顺序也有讲究控制多边形是曲线的骨架不要出现相邻控制点来回交叉否则生成的轨迹会出现不必要的回绕。如果拿到的路径点是稀疏的我会先用直线段粗排再用bsplinepolytraj加密而不是把原始点直接灌进去。如果没有 Robotics System Toolbox调用会报 Undefined function 或者类未注册。这时回到第 3 章的手写bspline_curve把ctrlpts转置成每行一个控制点得到的就是同一条曲线。手写版本的参数是节点向量里的 ubsplinepolytraj的参数是时间 t两者都只是参数化方式不同几何形状由控制点和节点向量唯一决定。提示bsplinepolytraj生成的轨迹默认满足 Clamped 边界起点和终点精确落在首末控制点上。如果你发现轨迹起终点有偏移先检查tSamples是否包含tInterval的端点而不是怀疑代码写错。4.3 画出来并校验速度曲线轨迹生成完第一件事是画图第二件事是看速度有没有跳变。上面的quiver每 30 个点画一个速度箭头箭头方向和长度对应速度方向和大小。B 样条轨迹本身是连续的但差分求导会在起点附近产生偏差因为gradient用单向差分处理边界点速度首末点误差通常比中间大一个数量级。如果需要精确的端点速度用info里的多项式系数做解析求导或者把首尾两个采样点去掉只看中间段的速度曲线。加速度校验也一样gradient二次差分后的噪声更明显。判断标准不是看是否平滑而是看加速度是否有突变控制点越密、次数越高曲线越贴近控制多边形加速度峰值一般越大。如果加速度曲线在某个时间段高频振荡通常是控制点间距差异过大导致把间距拉匀再生成一次即可。5. 从曲线点到控制点B 样条反算与凸包校验5.1 用 spcol 反算控制点插值与最小二乘的区分标题里的“B 样条曲线点”还有一层操作是反着来的给一组期望经过的曲线点反求控制点。这在插值任务里很常用比如机械臂末端必须经过指定姿态点。原理是把待求控制点当作未知数基函数矩阵作为系数矩阵解线性方程组。用spcol构建矩阵一行代码完成反算p 3; Q [1 0; 2 3; 4 2; 6 3; 8 1]; % 期望经过的曲线点一行一点 L size(Q, 1); numInner L - p - 1; inner linspace(0, 1, max(numInner, 0) 2); inner inner(2:end-1); knots [zeros(1, p 1), inner, ones(1, p 1)]; % 弦长参数化按点距累计归一化 d [0; cumsum(sqrt(sum(diff(Q, 1, 1).^2, 2)))]; uj d / d(end); A spcol(knots, p 1, uj); % L x (n1) P A \ Q; % 反算控制点每行一个 [C, ~] bspline_curve(P, p, knots, uj); disp(max(abs(C - Q))); % 插值误差1e-12 量级这里uj用弦长参数化也就是按目标点之间的欧氏距离累计归一化比均匀参数化更能反映点距分布。当控制点数量和目标点数量相等时A是方阵A\Q给出严格插值当控制点数量少于目标点数量时A是过定矩阵A\Q自动变成最小二乘拟合插值误差不再是零但曲线更光顺。如果你的应用不需要严格过点我会少设两个控制点换取稳定性必须严格过点时尽量限制点数是次数的 2 到 3 倍不要一次塞上百个点。如果装了优化工具箱带约束的反算可以用lsqlin处理比如要求控制点落在某个工作空间内。普通场景下A\Q的数值精度已经足够反算出的控制点多注意曲率突变不用急着上优化。5.2 凸包校验与最后一个提醒B 样条有个重要性质整条曲线落在控制点形成的凸包内。反算结果对不对可以用这个性质做最后一道校验。二维情况下直接用inpolygon检查采样点是否落在控制点凸包内uq linspace(0, 1, 500); Cq bspline_curve(P, p, knots, uq); k convhull(P(:,1), P(:,2)); ok all(inpolygon(Cq(:,1), Cq(:,2), P(k,1), P(k,2))); disp(ok);convhull返回凸包顶点的索引inpolygon判断曲线点是否在凸包内。如果ok为 0说明控制点反算结果自交叉或者曲线越界。注意凸包性质是对完整曲线段而言的如果只采样uq的一部分曲线段只落在对应局部控制点的凸包内拿全局凸包去校验局部段会误报。三维场景下inpolygon不适用可以分别投影到三个坐标平面逐层检查。这一节的反算代码和前面手写求值函数组合起来就是一个完整的 B 样条正向求点、反向定点的闭环工具链。本文还有配套的精品资源点击获取
B样条轨迹生成：MATLAB手写Cox-de Boor与工具箱实践
简介无论是图形学中的曲线设计还是几何建模与工程计算B样条曲线编程都是重要的基础技能。这套基于MATLAB的B样条实现包包含25个文件其中23个为.m源程序按功能分为基函数生成、Deboor递推求值、曲线逼近与估计、导数计算、GUI交互等多个模块同时提供多个可直接运行的示例脚本便于对照学习输出结果另有README和license.txt分别说明使用方法与许可信息压缩包整体仅22KB非常轻量。目前已有360人学习下载适合希望从代码层面理解B样条原理的学生和开发者。通过研读核心函数与示例读者既能明白控制点、节点向量与基函数如何共同决定曲线形状也能掌握可复用的编程思路顺利迁移到CAD建模、数据拟合或机器人轨迹规划等实际任务中。1. 为什么 B 样条曲线比 Bezier 更适合做轨迹点生成做机械臂路径、无人机航迹或者 CAD 曲线时控制点一多Bezier 的全局性就开始拖后腿你只想微调中间一个点整条曲线都跟着变形。B 样条用节点向量把曲线切成分段多项式每个控制点只影响附近几段局部修改、局部收敛都更直观。MATLAB 里做 B 样条有两条常见路线:一条是照着 Cox-de Boor 递推手写基函数适合理解原理、完全可控;另一条是直接用bsplinepolytraj生成轨迹点适合快速做路径规划。这篇把两条路都打通从基函数、节点向量、参数怎么设到手写代码和工具箱函数怎么互相验证最后补一个反算控制点的技巧。2. B 样条的基函数、节点向量与阶数参数怎么设才算对2.1 从 Bezier 到 B 样条控制点为什么只影响局部Bezier 曲线的 Bernstein 基函数在整个区间都有非零值所以任何一个控制点变化都会改变整条曲线。B 样条把区间切成若干段第 i 个 p 次基函数只在局部节点区间上有支撑。给定 n1 个控制点 P_1,...,P_{n1} 和次数 p曲线方程写作C(u) Σ_{i1}^{n1} N_{i,p}(u) * P_i这里 N_{i,p}(u) 由 Cox-de Boor 递推定义。0 次基函数只有一段非零高次基函数由低次线性插值得到递推公式就是 B 样条编程的核心N_{i,0}(u) 1当 u ∈ [k_i, k_{i1})否则为 0N_{i,p}(u) (u - k_i)/(k_{ip} - k_i) * N_{i,p-1}(u) (k_{ip1} - u)/(k_{ip1} - k_{i1}) * N_{i1,p-1}(u)递推里分母是相邻节点差。节点出现重复时分母可能为 0这时候对应项直接按 0 处理这是后面 MATLAB 代码里必须显式判断的地方。2.2 节点向量怎么设Clamped、均匀与重复度节点向量 knots 决定曲线在参数空间里的分段方式。同样是 5 个控制点、3 次曲线用不同节点向量得到的曲线性质差别很大。最常用的三种如下表。节点向量类型构造方式边界行为典型用途Clamped两端节点各重复 p1 次中间均匀分布曲线经过首末控制点端点切线沿控制多边形首末边轨迹规划、曲线插值非 Clamped 均匀节点在区间内等距分布两端不重复曲线不经过端点控制点但参数分布均匀形状设计、光顺拟合周期闭合首尾节点循环相接曲线闭合拼接处连续性好闭环路径、齿轮轮廓轨迹规划里我一般直接用 Clamped因为它保证起点和终点就是控制点的第一个和最后一个物理上更容易对接。比如 5 个控制点、p3 的 Clamped 节点向量0 重复 4 次、1 重复 4 次中间只有一个内部节点 0.5构造代码是p 3; nctrl 5; numInner nctrl - p - 1; % 内部节点个数 1 inner linspace(0, 1, numInner 2); % 先取 [0, 0.5, 1] inner inner(2:end-1); % 去掉端点只剩 0.5 knots [zeros(1, p1), inner, ones(1, p1)];numInner等于控制点数量减 p 再减 1这个式子决定了 Clamped 节点向量的内部节点数。当控制点恰好比次数多 1 时numInner为 0节点向量退化成两端各 p1 个重复节点B 样条就退化为 Bezier。后面写通用代码时linspace(0,1,numInner2)这种写法天然覆盖了退化情况。2.3 维度对不上就报错n、p、knots 的计数关系B 样条编程里最常见的报错就是维度对不上。控制点数量、次数和节点向量长度之间是严格等式控制点数量 p 1 节点向量长度。用符号表示n1 个控制点对应 n 作为最大索引节点向量长度必须是 np2。符号含义约束关系n1控制点个数至少 p1 个p曲线次数线性为 1抛物线为 2三次为 3knots节点向量length(knots) np2u参数值定义域 [knots(p1), knots(n1)]通常取 [0,1]MATLAB 索引从 1 开始所以基函数下标也要整体平移。很多人把 np2 记成 np1差一个 1spcol或者spmak立刻报错。如果你的节点向量来自linspace(0,1,N)而没有做 Clamped 处理长度就会差 p1这类错误用length(knots)一行命令就能查出来。3. 用 MATLAB 手写 Cox-de Boor 递推从基函数到曲线点3.1 一个文件跑通的 bspline_curve.m先给一个可以直接抄的完整函数。它输入控制点矩阵、次数、节点向量和一组参数点输出每个参数对应的曲线坐标同时附带基函数矩阵方便和工具箱函数对照。function [C, Nmat] bspline_curve(ctrlpts, p, knots, uq) % [C, Nmat] bspline_curve(ctrlpts, p, knots, uq) % ctrlpts : (n1) x dim每行是一个控制点 % p : 曲线次数 % knots : 节点向量长度 np2 % uq : 参数点列向量取值在 [knots(1), knots(end)] % C : numel(uq) x dim每个参数点对应的曲线坐标 % Nmat : numel(uq) x (n1)每个参数点对应的基函数值 uq uq(:); numQ numel(uq); n size(ctrlpts, 1) - 1; N zeros(n 1, numQ); for s 1:numQ u uq(s); % 右端点是闭区间统一平移到端点左侧避免半开区间漏值 if u knots(end) u knots(end) - eps * max(1, abs(knots(end))); end % 0 次基函数N_{i,0}(u) 1当 u 落在第 i 个节点区间 for i 1:n 1 if u knots(i) u knots(i 1) N(i, s) 1; end end % Cox-de Boor 递推k 从 1 到 p for k 1:p for i 1:n 1 - k d1 knots(i k) - knots(i); d2 knots(i k 1) - knots(i 1); left N(i, s); if d1 0 left left * (u - knots(i)) / d1; else left 0; % 重复节点分母为 0项为 0 end right N(i 1, s); if d2 0 right right * (knots(i k 1) - u) / d2; else right 0; end N(i, s) left right; end end end C N * ctrlpts; Nmat N; end3.2 半开区间与重复节点递推里最容易错的三个地方第一个坑是 0 次基函数的区间判定。标准定义是左闭右开也就是 u knots(i) 且 u knots(i1)。但如果 u 恰好等于 knots(end)最后一个区间是 [1,1)任何数都不满足所以代码里先把右端点左移一个机器 epsilon。第二个坑是重复节点。Clamped 节点向量里两端各有 p1 个相同节点递推时会遇到 d1 或 d2 为 0必须先判断再赋值否则 MATLAB 会产生 NaN 并在整个矩阵里扩散。第三个坑是内层循环的覆盖顺序。left取的是 N(i,s) 的旧值right取的是 N(i1,s) 的旧值因为内层循环从左往右走i1 位置还没有被本层覆盖取到的仍然是上一层的值这个顺序写反了结果就是错的。调用示例给一组 2D 控制点生成 200 个曲线点并绘图p 3; ctrlpts [1 0; 2 3; 4 2; 6 3; 8 1]; numInner size(ctrlpts, 1) - p - 1; inner linspace(0, 1, numInner 2); knots [zeros(1, p 1), inner(2:end-1), ones(1, p 1)]; uq linspace(0, 1, 200); [C, Nmat] bspline_curve(ctrlpts, p, knots, uq); figure(Color, w); plot(ctrlpts(:,1), ctrlpts(:,2), o--); hold on; plot(C(:,1), C(:,2), LineWidth, 1.6); legend(控制点, B样条曲线, Location, best);uq是参数采样200 个点足够画出平滑的三次曲线。C的第 j 行对应uq(j)处的曲线坐标和ctrlpts的行布局完全一致不需要转置。3.3 用 spcol 做数值对照验证手写递推最怕算法对但索引错。Curve Fitting Toolbox 提供spcol直接生成 B 样条配置矩阵可以用它来验证Nmat的正确性误差应该在 1e-14 量级A spcol(knots, p 1, uq); disp(max(max(abs(Nmat - A))));spcol的第二个参数是 order也就是 p1不是 p。返回矩阵的每一行对应一个参数点每一列对应一个基函数。如果你的Nmat和A形状一致、数值接近递推就写对了。如果发现列数对不上先检查节点向量长度是不是 np2再看spcol传入的是 p 还是 p1这两个原因占了九成报错。4. bsplinepolytraj 实战生成 B 样条路径点并验证轨迹4.1 bsplinepolytraj 最小调用示例Robotics System Toolbox 里的bsplinepolytraj把 B 样条曲线和轨迹规划封装在一起。它和手写代码的坐标布局不同控制点按列存每列是一个控制点行对应坐标维度。最小调用就是一个函数ctrlpts [0 1 2 3 3 2; 0 2 2 0 -2 -2]; % 每列一个控制点 tInterval [0 10]; tSamples linspace(tInterval(1), tInterval(2), 300); [traj, tvec, info] bsplinepolytraj(ctrlpts, tInterval, tSamples); dt tvec(2) - tvec(1); vel gradient(traj, dt); acc gradient(vel, dt); figure(Color, w); plot(ctrlpts(1,:), ctrlpts(2,:), ko--); hold on; plot(traj(1,:), traj(2,:), LineWidth, 1.6); quiver(traj(1,1:30:end), traj(2,1:30:end), ... vel(1,1:30:end), vel(2,1:30:end), 0.5); legend(控制点, B样条轨迹, 速度, Location, best);traj是 2x300 的位置序列tvec是每个点对应的时间info里带轨迹段的多项式系数和时间信息用disp(info)查看具体字段。速度用gradient差分得到后面会说明为什么这只是观察用的近似值。4.2 ctrlpts、tInterval、tSamples 三个参数怎么调参数形状作用注意事项ctrlptsdim x N控制点每列一个列数过少时曲线光顺性差一般是 5 个以上tInterval1x2轨迹起止时间起止时间必须严格递增tSamples1xNs输出点的采样时刻必须落在 tInterval 区间内可以非均匀trajdim x Ns轨迹位置第一点对应 tInterval(1)最后一点对应 tInterval(2)vel/accdim x Ns速度/加速度由梯度差分得到末端有截断误差tSamples不要求均匀这在工程里很有用。比如无人机经过某片净空区时需要更高频率的路径点就可以在 tSamples 里局部加密轨迹点会自动按 B 样条参数插值。控制点的排列顺序也有讲究控制多边形是曲线的骨架不要出现相邻控制点来回交叉否则生成的轨迹会出现不必要的回绕。如果拿到的路径点是稀疏的我会先用直线段粗排再用bsplinepolytraj加密而不是把原始点直接灌进去。如果没有 Robotics System Toolbox调用会报 Undefined function 或者类未注册。这时回到第 3 章的手写bspline_curve把ctrlpts转置成每行一个控制点得到的就是同一条曲线。手写版本的参数是节点向量里的 ubsplinepolytraj的参数是时间 t两者都只是参数化方式不同几何形状由控制点和节点向量唯一决定。提示bsplinepolytraj生成的轨迹默认满足 Clamped 边界起点和终点精确落在首末控制点上。如果你发现轨迹起终点有偏移先检查tSamples是否包含tInterval的端点而不是怀疑代码写错。4.3 画出来并校验速度曲线轨迹生成完第一件事是画图第二件事是看速度有没有跳变。上面的quiver每 30 个点画一个速度箭头箭头方向和长度对应速度方向和大小。B 样条轨迹本身是连续的但差分求导会在起点附近产生偏差因为gradient用单向差分处理边界点速度首末点误差通常比中间大一个数量级。如果需要精确的端点速度用info里的多项式系数做解析求导或者把首尾两个采样点去掉只看中间段的速度曲线。加速度校验也一样gradient二次差分后的噪声更明显。判断标准不是看是否平滑而是看加速度是否有突变控制点越密、次数越高曲线越贴近控制多边形加速度峰值一般越大。如果加速度曲线在某个时间段高频振荡通常是控制点间距差异过大导致把间距拉匀再生成一次即可。5. 从曲线点到控制点B 样条反算与凸包校验5.1 用 spcol 反算控制点插值与最小二乘的区分标题里的“B 样条曲线点”还有一层操作是反着来的给一组期望经过的曲线点反求控制点。这在插值任务里很常用比如机械臂末端必须经过指定姿态点。原理是把待求控制点当作未知数基函数矩阵作为系数矩阵解线性方程组。用spcol构建矩阵一行代码完成反算p 3; Q [1 0; 2 3; 4 2; 6 3; 8 1]; % 期望经过的曲线点一行一点 L size(Q, 1); numInner L - p - 1; inner linspace(0, 1, max(numInner, 0) 2); inner inner(2:end-1); knots [zeros(1, p 1), inner, ones(1, p 1)]; % 弦长参数化按点距累计归一化 d [0; cumsum(sqrt(sum(diff(Q, 1, 1).^2, 2)))]; uj d / d(end); A spcol(knots, p 1, uj); % L x (n1) P A \ Q; % 反算控制点每行一个 [C, ~] bspline_curve(P, p, knots, uj); disp(max(abs(C - Q))); % 插值误差1e-12 量级这里uj用弦长参数化也就是按目标点之间的欧氏距离累计归一化比均匀参数化更能反映点距分布。当控制点数量和目标点数量相等时A是方阵A\Q给出严格插值当控制点数量少于目标点数量时A是过定矩阵A\Q自动变成最小二乘拟合插值误差不再是零但曲线更光顺。如果你的应用不需要严格过点我会少设两个控制点换取稳定性必须严格过点时尽量限制点数是次数的 2 到 3 倍不要一次塞上百个点。如果装了优化工具箱带约束的反算可以用lsqlin处理比如要求控制点落在某个工作空间内。普通场景下A\Q的数值精度已经足够反算出的控制点多注意曲率突变不用急着上优化。5.2 凸包校验与最后一个提醒B 样条有个重要性质整条曲线落在控制点形成的凸包内。反算结果对不对可以用这个性质做最后一道校验。二维情况下直接用inpolygon检查采样点是否落在控制点凸包内uq linspace(0, 1, 500); Cq bspline_curve(P, p, knots, uq); k convhull(P(:,1), P(:,2)); ok all(inpolygon(Cq(:,1), Cq(:,2), P(k,1), P(k,2))); disp(ok);convhull返回凸包顶点的索引inpolygon判断曲线点是否在凸包内。如果ok为 0说明控制点反算结果自交叉或者曲线越界。注意凸包性质是对完整曲线段而言的如果只采样uq的一部分曲线段只落在对应局部控制点的凸包内拿全局凸包去校验局部段会误报。三维场景下inpolygon不适用可以分别投影到三个坐标平面逐层检查。这一节的反算代码和前面手写求值函数组合起来就是一个完整的 B 样条正向求点、反向定点的闭环工具链。本文还有配套的精品资源点击获取