最近整理自己的几何算法工具箱正好把三次B样条曲线拟合的C实现重新梳理了一遍。如果你也遇到过这类场景手里有一串密集的离散点要得到一条光顺曲线或者要把几百个采样点压缩成几十个控制点那这篇就是冲着你来的。与线性插值相比B样条曲线天然二阶连续不会出现折线和一阶跳变与Bezier相比它支持局部修改不至于动一个点全身跟着动。文章会从原理讲到控制点反算、节点向量构造、de Boor求值最后给出一套可以直接跑起来修改的C代码骨架。1. 为什么拟合曲线我首选三次B样条1.1 几种拟合方案的优劣对比散点拟合这件事可选方案太多了多项式插值、Bezier曲线、Catmull-Rom曲线、NURBS、B样条每个都有人用。我做技术选型时一般先拉一张表对比方案局部性连续性实现成本适用场景多项式插值无次数升高后容易震荡低数据点少且规律均匀Bezier偏全局光滑但控制点互相牵连低交互式造型、单段曲线Catmull-Rom好C1连续中路径平滑、插值需求明确三次B样条好C2连续中轨迹平滑、曲线重建、数据压缩多项式插值在点数一多就露馅Runge震荡会让人怀疑人生。Bezier虽然数学漂亮但移动一个控制点整体曲线都会跟着变这对“局部微调”来说是灾难。Catmull-Rom能精确穿过每个点视觉效果好但它只有C1连续——曲率在节点处会跳变。如果你的下游算法要算曲率、要规划加加速度这个跳变会很难受。B样条的优势就在这每个控制点只影响附近一段天然满足C2连续。C2连续意味着曲线的曲率是连续变化的无论是做视觉渲染还是机器人轨迹规划体验都更顺滑。1.2 三次阶数为什么“刚刚好”三次是保证C2连续的最低阶数。用二次B样条只有C1连续用四次、五次虽然连续阶更高但过冲更明显计算代价和数值稳定性都会变差。工程上有一句经验之谈能不用高次就不用高次三次是最顺手的档位。三次B样条还有一个非常实用的性质参数域内任意一点的值最多只由4个控制点决定。这意味着你反算控制点时系数矩阵是窄带的求解速度非常快而且调试时你改一个控制点影响范围是可预测的不会出现“蝴蝶效应”。2. 动手前必须吃透的数学细节2.1 基函数、控制点和节点向量之间的关系先说人话版本。B样条曲线可以理解成一根软绳控制点就是绳子上挂的重物。每个重物只影响自己附近一小段绳子而这个“附近”的范围由节点向量决定。节点向量是参数u轴上的一组非降刻度写法像一个数组[u0, u1, u2, ...]。它决定了每个控制点的影响力区间是宽是窄。控制点的权重函数叫做B样条基函数记为N_{i,p}(u)其中i是控制点索引p是次数。我们把所有控制点乘上各自的基函数再相加就得到曲线上某个参数u对应的坐标。这种感觉很像“加权投票”参数u落在哪个范围就由附近几个控制点投票决定当前位置票数就是基函数值。2.2 de Boor-Cox递推公式与代码基函数的计算标准算法是de Boor-Cox递推。公式不复杂但第一次看容易头晕N_{i,0}(u) 1当 u_i ≤ u u_{i1}否则为0。N_{i,p}(u) (u - u_i) / (u_{ip} - u_i) * N_{i,p-1}(u) (u_{ip1} - u) / (u_{ip1} - u_{i1}) * N_{i1,p-1}(u)翻译成C核心就是这个函数double bsplineBasis(int i, int p, double u, const std::vectordouble knots) { if (p 0) { if (u knots[i] u knots[i 1]) return 1.0; return 0.0; } double left 0.0; double right 0.0; double d1 knots[i p] - knots[i]; double d2 knots[i p 1] - knots[i 1]; if (d1 1e-15) { left (u - knots[i]) / d1 * bsplineBasis(i, p - 1, u, knots); } if (d2 1e-15) { right (knots[i p 1] - u) / d2 * bsplineBasis(i 1, p - 1, u, knots); } return left right; }注意分母为0时要把该项当成0处理这是clamped节点向量里非常常见的坑。2.3 节点向量构造均匀、clamped、弦长参数化节点向量的构造直接影响曲线质量和矩阵条件数。我见过不少人在这里踩坑直接用均匀节点数据点分布不均匀的时候曲线直接在密集段扭成麻花。实际工程里我推荐的做法是clamped节点向量加弦长参数化。clamped的意思是在节点向量首尾各重复p1次。对三次B样条来说就是开头4个0结尾4个1[0, 0, 0, 0, ..., 1, 1, 1, 1]这样曲线会精确通过首末控制点不会出现两端“够不着”的尴尬情况。弦长参数化是指数据点对应的参数值不是平均分配而是按相邻点之间的距离累加再归一化u[0] 0 u[k] u[k-1] distance(Q[k-1], Q[k]) / totalLength这样密集的地方参数间隔小稀疏的地方参数间隔大曲线形状更贴合实际几何距离。2.4 控制点求解的统一视角最小二乘很多人一上来就问“怎么反算控制点”其实反算控制点本质上是最小二乘问题。给你m个数据点指定c个控制点目标就是找一组控制点P让曲线在参数位置上的值与数据点尽可能接近。用矩阵写法就是minimize ||A * P - Q||^2其中A是m行c列的基函数矩阵A[k][j] N_{j,3}(u[k])Q是数据点坐标矩阵P是待求控制点坐标矩阵。如果c等于m且A满秩那这就是插值曲线严格穿过每个数据点。如果c小于m就是逼近拟合。同一个代码框架能同时覆盖两种需求这也是我偏爱这个方案的原因。工程上求解这个最小二乘问题不要手写正规方程(A^T A)求解条件数平方问题会把数值稳定性毁掉。用SVD、QR或者专用的稀疏最小二乘迭代求解器更可靠。3. C代码骨架从散点到B样条3.1 类的设计与基础结构我习惯把三次B样条封装成一个独立的类对外只暴露两个核心接口输入数据点拟合、按参数求值采样。内部细节全部藏起来。#include Eigen/Sparse #include vector struct Point3 { double x 0.0, y 0.0, z 0.0; }; class CubicBSplineFitter { public: explicit CubicBSplineFitter(int degree 3) : m_degree(degree) {} bool fit(const std::vectorPoint3 dataPts, int ctrlCount); Point3 evaluate(double u) const; std::vectorPoint3 sample(int numSamples) const; const std::vectorPoint3 getCtrlPts() const { return m_ctrlPts; } private: int m_degree 3; std::vectordouble m_knots; std::vectorPoint3 m_ctrlPts; double basisFunc(int i, int p, double u) const; Point3 deBoor(double u) const; double clampParam(double u) const; };Point3够用且简单二维数据把z留成0就行。不要一上来就引入向量库能让代码少一层依赖就少一层。3.2 数据预处理去重、参数化原始数据点往往有重复坐标或者间隔极近的点。重复点会让弦长参数化出现长度为0的增量后面除零全是NaN。所以我第一件事就是去重。std::vectorPoint3 cleaned; for (const auto p : dataPts) { if (cleaned.empty() || dist(p, cleaned.back()) 1e-8) { cleaned.push_back(p); } }然后做弦长参数化size_t m cleaned.size(); std::vectordouble u(m, 0.0); double total 0.0; for (size_t i 1; i m; i) { total dist(cleaned[i], cleaned[i - 1]); } double accum 0.0; for (size_t i 1; i m; i) { accum dist(cleaned[i], cleaned[i - 1]); u[i] accum / total; } u.back() 1.0;这里有一个工程细节最后一个点的参数不一定非要严格等于1可以改成1.0 - 1e-8可以避开B样条基函数在末端区间的右开区间边界问题。实际误差比头发丝还小完全不影响结果。3.3 组装基函数矩阵并反解控制点控制点数量ctrlCount需要你指定一般远小于数据点数量。节点向量用clamped方式构造内部节点按参数序列的分位位置抽取保证每个节点区间都有数据支撑int c ctrlCount; if (c 4) return false; if (c (int)m) c (int)m; m_knots.assign(c 4, 0.0); for (int i 0; i 4; i) m_knots[i] 0.0; for (int i c; i c 4; i) m_knots[i] 1.0; int internalCount c - 4; double lastKnot 0.0; for (int j 1; j internalCount; j) { int idx (int)std::round(1.0 * j * (int)(m - 1) / (internalCount 1)); if (idx 1) idx 1; if (idx (int)m - 2) idx (int)m - 2; double val u[idx]; if (val lastKnot 1e-12) val lastKnot 1e-12; if (val 1.0 - 1e-12) val 1.0 - 1e-12; m_knots[3 j] val; lastKnot val; }组装基函数矩阵我直接用稀疏矩阵因为每个参数值真正非零的基函数只有4个std::vectorEigen::Tripletdouble triplets; triplets.reserve(m * 4); for (int k 0; k (int)m; k) { for (int j 0; j c; j) { double v basisFunc(j, m_degree, u[k]); if (v 1e-12) { triplets.emplace_back(k, j, v); } } } Eigen::SparseMatrixdouble A(m, c); A.setFromTriplets(triplets.begin(), triplets.end());右侧数据点坐标矩阵是一个m行3列的稠密矩阵Eigen::MatrixXd B(m, 3); for (int k 0; k (int)m; k) { B(k, 0) cleaned[k].x; B(k, 1) cleaned[k].y; B(k, 2) cleaned[k].z; }求解最小二乘。我的默认选项是LeastSquaresConjugateGradient专门针对稀疏最小二乘数据量几千个点时依然很快Eigen::LeastSquaresConjugateGradientEigen::SparseMatrixdouble solver; solver.compute(A); Eigen::MatrixXd P solver.solve(B); m_ctrlPts.resize(c); for (int j 0; j c; j) { m_ctrlPts[j] { P(j, 0), P(j, 1), P(j, 2) }; }如果数据量特别小或者你对矩阵稳定性有疑虑可以把A转成稠密矩阵后改用ColPivHouseholderQR效果更直接。两种写法我都留过实际工程里小数据量是用QR更稳大数据量是稀疏迭代更快看场景取舍。3.4 de Boor算法采样曲线拟合完成后核心就是按参数u求曲线上的点。de Boor算法是计算B样条值的标准方法比直接套基函数求和更高效数值也更稳定。Point3 CubicBSplineFitter::deBoor(double u) const { double uu clampParam(u); int p m_degree; int n (int)m_ctrlPts.size() - 1; int k p; if (uu m_knots[p]) { for (int i p; i n; i) { if (uu m_knots[i] uu m_knots[i 1]) { k i; break; } } if (uu m_knots[n 1]) k n; } std::vectorPoint3 d(p 1); for (int j 0; j p; j) { d[j] m_ctrlPts[k - p j]; } for (int r 1; r p; r) { for (int j p; j r; --j) { double denom m_knots[k j - r 1] - m_knots[k - p j]; double alpha (denom 0.0) ? 0.0 : (uu - m_knots[k - p j]) / denom; d[j].x (1.0 - alpha) * d[j - 1].x alpha * d[j].x; d[j].y (1.0 - alpha) * d[j - 1].y alpha * d[j].y; d[j].z (1.0 - alpha) * d[j - 1].z alpha * d[j].z; } } return d[p]; }采样曲线时只要在[0,1]参数域内均匀取点再逐个evaluate就行。比如取100个点输出到文件或直接画出来就能看到一条连续光顺的曲线。4. 实操案例与参数调优经验4.1 一个具体案例我拿一组类似轨迹的数据做过测试数据点大概60多个坐标有轻微噪声形状是一个不规则的环。直接用线连起来会出现很多小折角用三次B样条拟合控制点设置成12个采样200个点输出。拟合效果非常直观曲线整体光顺没有出现局部震荡端点也因为clamped节点的作用自然落在首末数据点附近。把控制点数量从12减到8曲线变得更“圆润”局部细节丢失从12加到20曲线更贴近原始数据但控制点波动幅度开始变大。这组对比给我印象很深控制点数量其实就是“拟合精度”和“光顺程度”之间的旋钮。4.2 控制点数量如何选择我自己的经验法则数据点几百个以内时控制点数量取数据点数量的1/5到1/3先跑一版看效果。如果曲线离数据点太远说明控制点太少如果曲线出现不自然的波浪说明控制点太多。具体操作可以二分设candidates为数据点数量的0.4倍、0.2倍、0.1倍分别拟合采样肉眼一看哪个合适就用哪个。这个参数没有唯一正确答案完全看你的下游需要什么——要保特征就多留控制点要平滑就少留。4.3 噪声数据和正则化的一点想法如果数据点本身噪声很大直接最小二乘拟合出来的控制点可能抖动剧烈。最直接的手段是减少控制点数量平滑效果立竿见影。如果减少控制点之后仍然不够再考虑加Tikhonov正则化项对控制点的二阶差分做惩罚。不过我不建议一开始就上正则化。它引入了一个新的超参数lambda调起来很费神。先试控制点数量再试节点分布最后才动正则化这是最省时间的路线。5. 常见问题排查速查我把实际调试中踩过的坑整理成了一张表方便直接对号入座现象可能原因解决办法曲线两端飘离首末点节点向量没有clamped首尾各重复degree1个节点参数化出现NaN数据点有重复坐标先去重再做弦长参数化控制点剧烈震荡控制点数量过多减少控制点数量矩阵求解不收敛节点区间太窄或重复内部节点加最小间距或改用QR数据点密集处曲线过冲使用了均匀节点而非弦长节点改用分位抽取内部节点曲线在末端少了一截u1落在开区间边界采样时最后一个点取1.0-1e-8最后一个问题尤其隐蔽。B样条基函数定义里区间是左闭右开理论上参数u等于节点向量末尾时要单独处理右端点。处理不好曲线末端会出现一个莫名其妙的小缺口。我在工程里直接绕开最后一个采样点不要卡死在1.0取个极小偏移量曲线末端自然衔接不细看根本分不出来。还有一点要提醒如果数据点是三维坐标Point3的x、y、z三列各自独立求解即可代码不用改。如果数据点在二维平面上z全填0结果依然正确。这套代码骨架我前前后后改过三版从最初的稠密矩阵QR求解到现在的稀疏矩阵迭代求解再配合de Boor采样性能和稳定性都够用。如果你也在做轨迹平滑、点云曲线重建或者CAD轮廓拟合直接用这套框架改一改应该能省不少试错的功夫。