1. 项目概述为什么B样条曲线是局部路径规划的“瑞士军刀”在机器人、自动驾驶或者游戏AI的路径规划里我们总在寻找一种平衡生成的路径既要足够平滑让机器人能顺畅地转弯又要能灵活地避开突然出现的障碍物。你可能试过直线连接、圆弧甚至高阶的贝塞尔曲线但总会遇到这样那样的问题——要么路径不够平滑导致控制抖动要么调整一个点整条路径都跟着“牵一发而动全身”。B样条曲线B-Spline就是来解决这个痛点的。它不是某个实验室里的新潮概念而是经过几十年工程实践检验的“老将”。我最早接触它是在一个移动机器人项目里当时用多项式曲线做局部避障机器人一到转角就抖得跟筛糠似的后来换成了三次B样条问题迎刃而解。它的核心魅力在于“局部支撑性”修改曲线的一个控制点只会影响曲线的一小段而不是全局。这意味着当传感器发现前方出现一个动态障碍物时算法可以快速、局部地调整路径而不用重新规划整条路线这对实时性要求极高的场景简直是救命稻草。简单来说如果你需要一条可由少数几个点控制、平滑且易于局部调整的路径B样条曲线法是一个绕不开的经典选择。本文将彻底拆解它的原理并手把手带你用Python和C分别实现你会看到同样的数学核心在不同语言环境下的实现细节和性能考量有何不同。无论你是做算法研究、机器人开发还是游戏中的NPC移动这篇内容都能给你一套可直接落地的工具箱。2. B样条曲线的数学内核从定义到“局部性”的直观理解很多教程一上来就扔出一堆公式让人望而却步。我们换个方式先忘掉复杂的数学用做木匠活的思路来理解。想象你要用一根富有弹性的木条曲线穿过几个固定的钉子控制点。贝塞尔曲线就像是用胶水把整根木条死死地粘在所有钉子上动一个钉子整根木条的形状都会变。而B样条曲线呢它像是用几个小夹子每个夹子只夹住木条的一小段使其靠近某个钉子。移动一个钉子只有被附近夹子影响的那段木条会变形其他部分保持原样。这种“一个夹子只影响一段木条”的特性就是“局部支撑性”。现在我们给这个比喻加上数学的精确性。一条p次的B样条曲线其数学定义是C(u) Σ(i0 to n) N(i,p)(u) * P(i)看起来有点唬人我们拆开看C(u)就是曲线上参数u对应的那个点坐标。P(i)第i个控制点就是我们的“钉子”。N(i,p)(u)这是核心叫做p次的第i个B样条基函数。它本质上就是一个权重函数决定了第i个控制点P(i)对参数u处曲线点C(u)的影响力有多大。它的值只在某个参数区间内非零这就是局部性的来源。**基函数的递推定义德布尔-考克斯递推公式**是理解的关键零次p0基函数这是基础。如果参数u落在节点区间[u(i), u(i1))内那么N(i,0)(u) 1否则为0。可以理解为在这个最基础的区间里曲线完全由对应的控制点“掌控”。高次p0基函数高次的基函数由低次的基函数“混合”而来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)这个递推公式是算法实现的核心。它告诉我们计算一个p次的基函数需要用到两个p-1次的基函数。这种结构天生适合用递归或动态规划来计算。节点向量Knot Vector这是另一个核心概念U {u0, u1, ..., u(m)}。你可以把它理解为定义参数u如何沿着曲线“行走”的标尺。节点向量的选择直接决定了曲线的类型均匀B样条节点等距分布如[0,1,2,3,4,5]。计算简单是最常用的入门选择。准均匀B样条两端节点具有重复度p1中间均匀。这能保证曲线起点和终点分别与第一个和最后一个控制点重合非常符合路径规划中“从起点出发到终点结束”的直观需求。非均匀B样条节点非均匀。这提供了最大的灵活性可以通过在某个参数区间密集布置节点让曲线在该区间更“贴近”控制点适合处理路径曲率变化剧烈的复杂地形。对于路径规划三次p3准均匀B样条是一个甜点选择。三次保证了曲线的二阶连续性即曲率连续运动平滑准均匀保证了起止点可控。后续我们的实现也将围绕它展开。注意节点向量的长度m、控制点数n和次数p之间有一个固定关系m n p 1。在初始化时务必满足否则曲线无法正确定义。3. 算法实现蓝图从理论公式到代码步骤理解了数学原理实现起来就是按部就班的“翻译”工作。无论是Python还是C核心算法流程都是一致的。下面这个流程图描绘了从输入到生成路径点的完整过程flowchart TD A[输入: 控制点P, 次数p, 节点向量U] -- B[确定参数u的取值区间br及对应节点跨度] B -- C{遍历每一个待求参数u} C -- D[调用基函数计算函数br计算所有非零基函数值 N(i,p)(u)] D -- E[加权求和brC(u) Σ N(i,p)(u) * P(i)] E -- F[得到曲线点 C(u)] F -- C C --遍历结束-- G[输出: 所有曲线点br构成规划路径]接下来我们深入到流程中的每一个关键步骤看看在代码中具体如何实现。3.1 核心中的核心B样条基函数的高效计算根据递推公式直接递归实现是最直观的但存在大量的重复计算效率低下。在工程中我们普遍采用德布尔算法。它本质上是一种动态规划通过迭代的方式填表计算出对于给定参数u所有非零的基函数值。算法步骤如下我们假设要计算次数为p的B样条在参数u处的值找到u所在的节点区间在节点向量U中找到下标k使得u ∈ [u(k), u(k1))。这是后续计算的基础。初始化零次基函数创建一个长度为p1的数组N。对于j0 to p如果(u U[k-j] u U[k-j1])则N[j] 1.0否则为0.0。这里N[j]对应的是N(k-j, 0)。迭代计算高次基函数# 伪代码逻辑 for r in 1 to p: # r代表当前计算的次数 for j in p down to r: # 倒序更新避免覆盖未使用的值 left (u - U[k - j]) / (U[k - j r] - U[k - j]) right (U[k - j r 1] - u) / (U[k - j r 1] - U[k - j 1]) N[j] left * N[j] right * N[j-1]经过p轮迭代后数组N中存储的就是N(k-p, p), N(k-p1, p), ..., N(k, p)这p1个非零的p次基函数值。这个算法的时间复杂度是O(p²)对于通常p2或3的情况效率极高。它是后续所有计算的基础。3.2 生成路径点参数采样与曲线求值得到了基函数计算器生成整条曲线就很简单了。我们无法得到连续的曲线而是通过离散采样来获取路径点。参数采样在曲线的定义域[U[p], U[n]]对于准均匀B样条通常是[0, 1]内均匀地取一系列参数值u。采样点越多生成的路径点越密集路径越“光滑”但计算量也越大。通常在两个控制点之间采样5-10个点是个不错的经验值。逐点求值对于每一个采样参数u a. 调用上述德布尔算法计算出该u处所有非零的基函数值N(i,p)(u)。 b. 根据公式C(u) Σ N(i,p)(u) * P(i)对相应的控制点进行加权求和得到该点的坐标。 c. 将这个点存入路径点列表。路径输出将所有计算出的路径点按u值顺序连接就得到了最终的规划路径。3.3 局部调整如何让路径“绕开”障碍物这是B样条在路径规划中最大的优势所在。假设我们的路径已经生成但通过传感器发现在路径中段C(u)附近出现了一个障碍物。定位影响区间找到距离障碍物最近的那个路径点并反推出它对应的参数值u_obs。确定受影响的控制点根据B样条的局部支撑性参数u_obs处的曲线点只被节点区间[u(k), u(k1))内的p1个控制点即P(k-p), P(k-p1), ..., P(k)所影响。调整控制点在这p1个控制点中选择一个或几个微调它们的位置。通常移动离障碍物最近的那个控制点或者同时移动相邻的两个效果最直接。将控制点向远离障碍物的方向移动一个小的位移。重新生成局部路径由于只改变了少数几个控制点我们不需要对整个参数域重新采样计算。只需要对受影响的参数区间大致是[U(k-p), U(k1)]重新进行密集采样计算新的路径点并替换掉原来该区间的旧路径点即可。这极大地节省了计算资源。实操心得在动态避障中控制点的调整量不宜过大。建议采用梯度下降的思想每次只进行微小调整然后检测碰撞迭代进行直到路径无碰撞。粗暴地大幅度移动控制点可能导致曲线出现意外的震荡或环。4. Python实现快速原型与可视化验证Python的优势在于生态和快速验证。我们用numpy处理向量运算用matplotlib进行可视化可以非常直观地看到算法效果。4.1 类设计与关键函数首先我们设计一个BSpline类。import numpy as np import matplotlib.pyplot as plt class BSpline: def __init__(self, degree, control_points, knot_vectorNone): 初始化B样条曲线 :param degree: 次数 p :param control_points: 控制点列表形如 [[x0,y0], [x1,y1], ...] :param knot_vector: 节点向量如果为None则生成准均匀节点向量 self.p degree self.ctrl_pts np.array(control_points) self.n len(self.ctrl_pts) - 1 # 控制点最大索引 if knot_vector is None: # 生成准均匀节点向量 self.knots self._generate_quasi_uniform_knots() else: self.knots np.array(knot_vector) self.m len(self.knots) - 1 # 检查节点向量长度关系: m n p 1 assert self.m self.n self.p 1, f节点向量长度错误应有 mnp1{self.nself.p1}, 但传入的为 {self.m} def _generate_quasi_uniform_knots(self): 生成准均匀节点向量 # 总共需要 m1 个节点 m n p 1 total_knots self.n self.p 2 knots np.zeros(total_knots) # 前p1个节点为0 knots[:self.p1] 0.0 # 后p1个节点为1 knots[-(self.p1):] 1.0 # 中间节点均匀分布 internal_knots_num total_knots - 2*(self.p1) if internal_knots_num 0: knots[self.p1: self.p1internal_knots_num] np.linspace(0, 1, internal_knots_num2)[1:-1] return knots关键函数basis_function实现了德布尔算法def basis_function(self, u): 计算参数u处所有非零的p次基函数值 (使用德布尔算法) :param u: 参数 :return: 列表包含p1个基函数值 [N(k-p,p)(u), ..., N(k,p)(u)] 以及对应的控制点起始索引 start_idx # 1. 找到u所在的节点区间 [knots[k], knots[k1]) # 特别处理u等于最后一个节点的情况 if u self.knots[-1]: k self.m - self.p - 1 else: k np.searchsorted(self.knots, u, sideright) - 1 k min(k, self.n) # 确保k不超过控制点最大索引n # 2. 初始化零次基函数 N np.zeros(self.p 1) for j in range(0, self.p 1): idx k - j if idx 0 or idx 1 len(self.knots): N[j] 0.0 elif self.knots[idx] u self.knots[idx 1]: N[j] 1.0 # 处理边界当u等于最后一个节点时最后一个区间是闭区间 elif u self.knots[-1] and idx self.n and self.knots[idx] u self.knots[idx 1]: N[j] 1.0 else: N[j] 0.0 # 3. 迭代计算高次基函数 for r in range(1, self.p 1): saved 0.0 for j in range(self.p, r-1, -1): idx k - j left_denom self.knots[idx r] - self.knots[idx] right_denom self.knots[idx r 1] - self.knots[idx 1] left 0.0 if left_denom ! 0: left (u - self.knots[idx]) / left_denom right 0.0 if right_denom ! 0: right (self.knots[idx r 1] - u) / right_denom N[j] left * N[j] right * saved saved N[j-1] if j 0 else 0.0 # 控制点起始索引 start_idx k - self.p return N, start_idx然后是曲线求值函数evaluate和批量生成路径的函数generate_curvedef evaluate(self, u): 计算曲线在参数u处的点 N, start_idx self.basis_function(u) point np.zeros(2) for j in range(self.p 1): ctrl_idx start_idx j if 0 ctrl_idx self.n: point N[j] * self.ctrl_pts[ctrl_idx] return point def generate_curve(self, num_samples100): 生成曲线上的采样点 u_values np.linspace(self.knots[self.p], self.knots[self.n1], num_samples) curve_points [] for u in u_values: curve_points.append(self.evaluate(u)) return np.array(curve_points)4.2 可视化与局部调整演示让我们创建一个实例并可视化同时演示局部调整。def main(): # 1. 定义控制点 control_points np.array([[0, 0], [1, 2], [3, 1], [5, 3], [7, 0], [9, 2]]) degree 3 # 三次B样条 # 2. 创建B样条对象 spline BSpline(degree, control_points) # 3. 生成曲线 curve spline.generate_curve(num_samples200) # 4. 可视化 plt.figure(figsize(10, 6)) # 绘制控制点和控制多边形 plt.plot(control_points[:, 0], control_points[:, 1], ro--, labelControl Points, linewidth1, markersize8) # 绘制B样条曲线 plt.plot(curve[:, 0], curve[:, 1], b-, labelB-Spline Curve (p3), linewidth2) plt.grid(True) plt.axis(equal) plt.legend() plt.title(B-Spline Path Planning) plt.xlabel(X) plt.ylabel(Y) # 5. 演示局部调整 print(原始控制点:, spline.ctrl_pts.tolist()) # 假设我们需要调整第三个控制点(索引2)来避障 spline.ctrl_pts[2] np.array([0.5, 1.0]) # 向右上方移动 curve_modified spline.generate_curve(num_samples200) plt.plot(curve_modified[:, 0], curve_modified[:, 1], g--, labelAfter Local Adjustment, linewidth2, alpha0.7) plt.legend() plt.show() # 打印调整前后的部分曲线点对比 print(\n局部调整后曲线中段发生了变化而两端基本保持不变。) print(例如取参数u0.5附近的点) u_mid (spline.knots[spline.p] spline.knots[spline.n1]) / 2 pt_orig spline.evaluate(u_mid) # 注意此时spline的控制点已修改需用旧数据重算此处为演示逻辑 # 实际对比应保存旧曲线数据此处从略 if __name__ __main__: main()运行这段代码你会看到一幅图红色的点和虚线是控制多边形蓝色的实线是原始B样条曲线绿色的虚线是调整一个控制点后的新曲线。可以清晰看到只有曲线中段对应被调整的控制点附近发生了形变曲线的起点和终点纹丝不动。这就是局部修改威力的直观体现。踩坑记录在实现basis_function时最容易出错的是边界处理特别是当参数u等于最后一个节点值时。如果处理不当会导致k值计算错误进而引发数组越界或基函数计算错误。务必像代码中那样对u self.knots[-1]的情况进行特殊处理。5. C实现追求性能与工程集成当你的路径规划算法需要集成到ROS机器人操作系统、自动驾驶仿真环境或对实时性要求极高的游戏服务器中时Python的解释器开销可能成为瓶颈。这时用C重写核心算法就很有必要。我们的目标是构建一个高效、可嵌入的BSpline类。5.1 工程化的类设计我们使用标准的C11并利用Eigen库进行线性代数运算它提供了优化过的矩阵向量操作比手动循环快得多。// BSpline.h #ifndef BSPLINE_H #define BSPLINE_H #include vector #include Eigen/Dense class BSpline { public: using Point Eigen::Vector2d; using PointList std::vectorPoint; BSpline(int degree, const PointList control_points); BSpline(int degree, const PointList control_points, const std::vectordouble knot_vector); // 核心功能 Point evaluate(double u) const; PointList generateCurve(int num_samples 100) const; // 工具函数 std::vectordouble generateQuasiUniformKnots() const; int findKnotSpan(double u) const; std::vectordouble computeBasisFunctions(double u) const; // 获取器 const PointList getControlPoints() const { return ctrl_pts_; } void setControlPoint(int index, const Point pt) { if(index 0 index ctrl_pts_.size()) ctrl_pts_[index] pt; } private: int p_; // 次数 PointList ctrl_pts_; // 控制点 std::vectordouble knots_; // 节点向量 int n_; // 控制点最大索引 (ctrl_pts_.size() - 1) int m_; // 节点向量最大索引 (knots_.size() - 1) void validateKnotVector() const; }; #endif // BSPLINE_H5.2 核心算法的C实现实现文件BSpline.cpp包含了算法的核心。// BSpline.cpp #include BSpline.h #include cassert #include algorithm #include stdexcept BSpline::BSpline(int degree, const PointList control_points) : p_(degree), ctrl_pts_(control_points) { if (ctrl_pts_.size() p_ 1) { throw std::invalid_argument(控制点数量至少为 degree 1); } n_ static_castint(ctrl_pts_.size()) - 1; knots_ generateQuasiUniformKnots(); m_ static_castint(knots_.size()) - 1; validateKnotVector(); } BSpline::BSpline(int degree, const PointList control_points, const std::vectordouble knot_vector) : p_(degree), ctrl_pts_(control_points), knots_(knot_vector) { n_ static_castint(ctrl_pts_.size()) - 1; m_ static_castint(knots_.size()) - 1; validateKnotVector(); } std::vectordouble BSpline::generateQuasiUniformKnots() const { int total_knots n_ p_ 2; // m1 std::vectordouble knots(total_knots, 0.0); // 前p1个节点为0 std::fill(knots.begin(), knots.begin() p_ 1, 0.0); // 后p1个节点为1 std::fill(knots.end() - (p_ 1), knots.end(), 1.0); // 中间节点均匀分布 int internal_knots_num total_knots - 2 * (p_ 1); if (internal_knots_num 0) { double step 1.0 / (internal_knots_num 1); for (int i 0; i internal_knots_num; i) { knots[p_ 1 i] step * (i 1); } } return knots; } int BSpline::findKnotSpan(double u) const { // 二分查找优化处理边界情况 if (u knots_[m_]) return n_; // 最后一个节点 if (u knots_[p_]) return p_; // 第一个有效区间起点 int low p_; int high n_ 1; int mid (low high) / 2; while (u knots_[mid] || u knots_[mid 1]) { if (u knots_[mid]) { high mid; } else { low mid; } mid (low high) / 2; } return mid; } std::vectordouble BSpline::computeBasisFunctions(double u) const { int k findKnotSpan(u); std::vectordouble N(p_ 1, 0.0); std::vectordouble left(p_ 1, 0.0); std::vectordouble right(p_ 1, 0.0); // 初始化零次基函数 N[j] 对应 N(k-j, 0) N[0] 1.0; for (int r 1; r p_; r) { left[r] u - knots_[k 1 - r]; right[r] knots_[k r] - u; double saved 0.0; for (int j 0; j r; j) { double temp N[j] / (right[j 1] left[r - j]); N[j] saved right[j 1] * temp; saved left[r - j] * temp; } N[r] saved; } // 注意这里计算出的N[0]到N[p]对应的是 N(k-p, p), ..., N(k, p) return N; } Point BSpline::evaluate(double u) const { // 1. 获取非零基函数值 auto basis_funcs computeBasisFunctions(u); int k findKnotSpan(u); // 2. 加权求和 Point result(0.0, 0.0); for (int j 0; j p_; j) { int ctrl_idx k - p_ j; result basis_funcs[j] * ctrl_pts_[ctrl_idx]; } return result; } PointList BSpline::generateCurve(int num_samples) const { PointList curve_points; curve_points.reserve(num_samples); double u_start knots_[p_]; double u_end knots_[n_ 1]; double step (u_end - u_start) / (num_samples - 1); for (int i 0; i num_samples; i) { double u u_start i * step; // 对最后一个样本确保u精确等于u_end避免浮点误差 if (i num_samples - 1) u u_end; curve_points.push_back(evaluate(u)); } return curve_points; } void BSpline::validateKnotVector() const { if (knots_.size() ! n_ p_ 2) { // m1 throw std::invalid_argument(节点向量长度必须等于 n p 2); } // 检查节点向量非递减 for (size_t i 1; i knots_.size(); i) { if (knots_[i] knots_[i - 1]) { throw std::invalid_argument(节点向量必须是非递减序列); } } }5.3 性能对比与集成注意事项性能对比在相同控制点20个点和采样点1000个的情况下用C实现开启-O2优化相比纯PythonNumPy实现计算速度通常有10倍到50倍的提升。这主要得益于C的静态编译、更少的内存分配以及Eigen库的SIMD指令优化。在需要每几毫秒就规划一次路径的实时系统中这个差距是决定性的。工程集成要点内存管理C没有垃圾回收在频繁生成路径时要避免在循环内部动态分配大量小内存如std::vector。我们的generateCurve函数一次分配好所需内存reserve是良好的实践。异常安全构造函数和关键函数中加入了参数验证并抛出标准异常std::invalid_argument便于上层调用者捕获和处理错误。接口设计提供了setControlPoint方法便于进行局部路径调整。在实际的避障循环中可以只修改少数几个控制点然后调用generateCurve重新生成路径或者更高效地只重新计算受影响的曲线段。与ROS等框架集成可以将Point类型轻松转换为ROS的geometry_msgs::Point或Pose消息。将BSpline类打包成一个独立的库或ROS节点中的工具类是常见的集成方式。实操心得在C中实现德布尔算法时我采用了算法导论中常见的“从左到右”的迭代方式并使用left和right数组预先计算了差值避免了内层循环中的重复除法和边界判断比递归版本和朴素的动态规划版本效率更高。这是从《The NURBS Book》等经典教材中学到的优化技巧。6. 在路径规划中的实战应用与进阶调优将B样条曲线应用到实际的路径规划中远不止调用一个生成函数那么简单。它需要与前端的全局规划、后端的轨迹优化以及感知模块紧密配合。6.1 与全局路径的衔接从A*到平滑曲线通常的流程是全局规划器如A*、Dijkstra生成一条由离散点组成的“折线”路径。这条路径对于机器人运动来说太粗糙了。我们的任务就是将这些离散点作为控制点生成一条平滑的B样条曲线。# 假设 global_path 是A*算法输出的离散点列表 global_path [[0,0], [2,1], [4,0], [6,2], [8,1], [10,0]] # 直接使用这些点作为控制点 control_points global_path degree 3 spline BSpline(degree, control_points) smoothed_path spline.generate_curve(num_samples200)但这里有个问题A*路径可能很曲折直接用作控制点生成的曲线可能会过于“紧绷”或出现尖角。一个常见的技巧是对全局路径进行稀疏化或采样选取关键拐点作为控制点或者在直线段上减少控制点密度在转弯处增加密度这样能生成更合理的平滑路径。6.2 动态避障实时控制点调整策略当局部传感器如激光雷达检测到障碍物时碰撞检测将生成的B样条曲线离散为密集点与障碍物地图进行碰撞检测找到发生碰撞的曲线段。参数映射找到碰撞点对应的曲线参数u_collision。影响控制点定位根据u_collision和节点向量定位出影响的p1个控制点。梯度调整定义一个“排斥力”场。障碍物对受影响的控制点产生一个排斥向量方向为从障碍物中心指向控制点大小与距离成反比如使用人工势场法思想。同时为了保持路径整体形状可以增加一个“弹性力”试图将控制点拉回原始位置。控制点的调整量是这两种力的加权和。迭代优化微调控制点后重新生成局部曲线再次进行碰撞检测。如此迭代直到路径无碰撞或达到最大迭代次数。这个过程类似于一个微型的局部轨迹优化。6.3 曲率约束与运动学可行性一条数学上平滑的曲线对机器人来说不一定可行。例如差速机器人有最小转弯半径限制。我们需要确保生成的B样条路径的曲率不超过机器人能承受的最大曲率。曲率κ(u)的计算公式为κ(u) |C(u) × C(u)| / |C(u)|^3其中C(u)和C(u)分别是曲线的一阶和二阶导数。对于B样条曲线导数的计算也有高效的递推公式。实施步骤在生成路径后对路径进行密集采样计算每个采样点的曲率。找到最大曲率κ_max。如果κ_max κ_robot机器人最大曲率则说明路径不可行。此时需要调整控制点来“拉直”曲线。一个启发式方法是找到曲率最大的点对应的参数u然后将其附近的影响控制点沿着使曲线曲率减小的方向通常是使控制多边形更平直的方向移动。这是一个带约束的优化问题实践中常使用梯度下降、二次规划QP甚至非线性优化如Ceres Solver, NLopt来求解在满足曲率约束的同时尽可能使路径短且平滑。6.4 进阶非均匀有理B样条NURBS如果项目对路径形状有更精细的要求比如需要精确表示圆弧或圆锥曲线那么就需要用到B样条的升级版——NURBS。它在B样条的基础上为每个控制点引入了一个权重因子w。NURBS曲线的公式是C(u) Σ (N(i,p)(u) * w(i) * P(i)) / Σ (N(i,p)(u) * w(i))权重w的作用w增大曲线会被拉向该控制点。w减小曲线会远离该控制点。当所有权重相等时NURBS退化为普通B样条。在路径规划中我们可以利用权重来实现一些特殊效果。例如在需要紧贴某个走廊边缘行驶时可以增加靠近边缘的那个控制点的权重在需要快速通过一个开阔区域时可以减小中间控制点的权重让路径更“直”。NURBS的实现只需在原有B样条代码的基础上增加权重的存储和计算即可核心的基函数算法完全复用。从基础的均匀B样条到应对动态环境的局部调整再到满足运动学约束的曲率优化最后到更强大的NURBS这条技术链为机器人、自动驾驶和智能体导航提供了强大而灵活的路径描述工具。理解其原理并掌握其实现意味着你手里多了一把解决复杂运动规划问题的利器。
1. 项目概述为什么B样条曲线是局部路径规划的“瑞士军刀”在机器人、自动驾驶或者游戏AI的路径规划里我们总在寻找一种平衡生成的路径既要足够平滑让机器人能顺畅地转弯又要能灵活地避开突然出现的障碍物。你可能试过直线连接、圆弧甚至高阶的贝塞尔曲线但总会遇到这样那样的问题——要么路径不够平滑导致控制抖动要么调整一个点整条路径都跟着“牵一发而动全身”。B样条曲线B-Spline就是来解决这个痛点的。它不是某个实验室里的新潮概念而是经过几十年工程实践检验的“老将”。我最早接触它是在一个移动机器人项目里当时用多项式曲线做局部避障机器人一到转角就抖得跟筛糠似的后来换成了三次B样条问题迎刃而解。它的核心魅力在于“局部支撑性”修改曲线的一个控制点只会影响曲线的一小段而不是全局。这意味着当传感器发现前方出现一个动态障碍物时算法可以快速、局部地调整路径而不用重新规划整条路线这对实时性要求极高的场景简直是救命稻草。简单来说如果你需要一条可由少数几个点控制、平滑且易于局部调整的路径B样条曲线法是一个绕不开的经典选择。本文将彻底拆解它的原理并手把手带你用Python和C分别实现你会看到同样的数学核心在不同语言环境下的实现细节和性能考量有何不同。无论你是做算法研究、机器人开发还是游戏中的NPC移动这篇内容都能给你一套可直接落地的工具箱。2. B样条曲线的数学内核从定义到“局部性”的直观理解很多教程一上来就扔出一堆公式让人望而却步。我们换个方式先忘掉复杂的数学用做木匠活的思路来理解。想象你要用一根富有弹性的木条曲线穿过几个固定的钉子控制点。贝塞尔曲线就像是用胶水把整根木条死死地粘在所有钉子上动一个钉子整根木条的形状都会变。而B样条曲线呢它像是用几个小夹子每个夹子只夹住木条的一小段使其靠近某个钉子。移动一个钉子只有被附近夹子影响的那段木条会变形其他部分保持原样。这种“一个夹子只影响一段木条”的特性就是“局部支撑性”。现在我们给这个比喻加上数学的精确性。一条p次的B样条曲线其数学定义是C(u) Σ(i0 to n) N(i,p)(u) * P(i)看起来有点唬人我们拆开看C(u)就是曲线上参数u对应的那个点坐标。P(i)第i个控制点就是我们的“钉子”。N(i,p)(u)这是核心叫做p次的第i个B样条基函数。它本质上就是一个权重函数决定了第i个控制点P(i)对参数u处曲线点C(u)的影响力有多大。它的值只在某个参数区间内非零这就是局部性的来源。**基函数的递推定义德布尔-考克斯递推公式**是理解的关键零次p0基函数这是基础。如果参数u落在节点区间[u(i), u(i1))内那么N(i,0)(u) 1否则为0。可以理解为在这个最基础的区间里曲线完全由对应的控制点“掌控”。高次p0基函数高次的基函数由低次的基函数“混合”而来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)这个递推公式是算法实现的核心。它告诉我们计算一个p次的基函数需要用到两个p-1次的基函数。这种结构天生适合用递归或动态规划来计算。节点向量Knot Vector这是另一个核心概念U {u0, u1, ..., u(m)}。你可以把它理解为定义参数u如何沿着曲线“行走”的标尺。节点向量的选择直接决定了曲线的类型均匀B样条节点等距分布如[0,1,2,3,4,5]。计算简单是最常用的入门选择。准均匀B样条两端节点具有重复度p1中间均匀。这能保证曲线起点和终点分别与第一个和最后一个控制点重合非常符合路径规划中“从起点出发到终点结束”的直观需求。非均匀B样条节点非均匀。这提供了最大的灵活性可以通过在某个参数区间密集布置节点让曲线在该区间更“贴近”控制点适合处理路径曲率变化剧烈的复杂地形。对于路径规划三次p3准均匀B样条是一个甜点选择。三次保证了曲线的二阶连续性即曲率连续运动平滑准均匀保证了起止点可控。后续我们的实现也将围绕它展开。注意节点向量的长度m、控制点数n和次数p之间有一个固定关系m n p 1。在初始化时务必满足否则曲线无法正确定义。3. 算法实现蓝图从理论公式到代码步骤理解了数学原理实现起来就是按部就班的“翻译”工作。无论是Python还是C核心算法流程都是一致的。下面这个流程图描绘了从输入到生成路径点的完整过程flowchart TD A[输入: 控制点P, 次数p, 节点向量U] -- B[确定参数u的取值区间br及对应节点跨度] B -- C{遍历每一个待求参数u} C -- D[调用基函数计算函数br计算所有非零基函数值 N(i,p)(u)] D -- E[加权求和brC(u) Σ N(i,p)(u) * P(i)] E -- F[得到曲线点 C(u)] F -- C C --遍历结束-- G[输出: 所有曲线点br构成规划路径]接下来我们深入到流程中的每一个关键步骤看看在代码中具体如何实现。3.1 核心中的核心B样条基函数的高效计算根据递推公式直接递归实现是最直观的但存在大量的重复计算效率低下。在工程中我们普遍采用德布尔算法。它本质上是一种动态规划通过迭代的方式填表计算出对于给定参数u所有非零的基函数值。算法步骤如下我们假设要计算次数为p的B样条在参数u处的值找到u所在的节点区间在节点向量U中找到下标k使得u ∈ [u(k), u(k1))。这是后续计算的基础。初始化零次基函数创建一个长度为p1的数组N。对于j0 to p如果(u U[k-j] u U[k-j1])则N[j] 1.0否则为0.0。这里N[j]对应的是N(k-j, 0)。迭代计算高次基函数# 伪代码逻辑 for r in 1 to p: # r代表当前计算的次数 for j in p down to r: # 倒序更新避免覆盖未使用的值 left (u - U[k - j]) / (U[k - j r] - U[k - j]) right (U[k - j r 1] - u) / (U[k - j r 1] - U[k - j 1]) N[j] left * N[j] right * N[j-1]经过p轮迭代后数组N中存储的就是N(k-p, p), N(k-p1, p), ..., N(k, p)这p1个非零的p次基函数值。这个算法的时间复杂度是O(p²)对于通常p2或3的情况效率极高。它是后续所有计算的基础。3.2 生成路径点参数采样与曲线求值得到了基函数计算器生成整条曲线就很简单了。我们无法得到连续的曲线而是通过离散采样来获取路径点。参数采样在曲线的定义域[U[p], U[n]]对于准均匀B样条通常是[0, 1]内均匀地取一系列参数值u。采样点越多生成的路径点越密集路径越“光滑”但计算量也越大。通常在两个控制点之间采样5-10个点是个不错的经验值。逐点求值对于每一个采样参数u a. 调用上述德布尔算法计算出该u处所有非零的基函数值N(i,p)(u)。 b. 根据公式C(u) Σ N(i,p)(u) * P(i)对相应的控制点进行加权求和得到该点的坐标。 c. 将这个点存入路径点列表。路径输出将所有计算出的路径点按u值顺序连接就得到了最终的规划路径。3.3 局部调整如何让路径“绕开”障碍物这是B样条在路径规划中最大的优势所在。假设我们的路径已经生成但通过传感器发现在路径中段C(u)附近出现了一个障碍物。定位影响区间找到距离障碍物最近的那个路径点并反推出它对应的参数值u_obs。确定受影响的控制点根据B样条的局部支撑性参数u_obs处的曲线点只被节点区间[u(k), u(k1))内的p1个控制点即P(k-p), P(k-p1), ..., P(k)所影响。调整控制点在这p1个控制点中选择一个或几个微调它们的位置。通常移动离障碍物最近的那个控制点或者同时移动相邻的两个效果最直接。将控制点向远离障碍物的方向移动一个小的位移。重新生成局部路径由于只改变了少数几个控制点我们不需要对整个参数域重新采样计算。只需要对受影响的参数区间大致是[U(k-p), U(k1)]重新进行密集采样计算新的路径点并替换掉原来该区间的旧路径点即可。这极大地节省了计算资源。实操心得在动态避障中控制点的调整量不宜过大。建议采用梯度下降的思想每次只进行微小调整然后检测碰撞迭代进行直到路径无碰撞。粗暴地大幅度移动控制点可能导致曲线出现意外的震荡或环。4. Python实现快速原型与可视化验证Python的优势在于生态和快速验证。我们用numpy处理向量运算用matplotlib进行可视化可以非常直观地看到算法效果。4.1 类设计与关键函数首先我们设计一个BSpline类。import numpy as np import matplotlib.pyplot as plt class BSpline: def __init__(self, degree, control_points, knot_vectorNone): 初始化B样条曲线 :param degree: 次数 p :param control_points: 控制点列表形如 [[x0,y0], [x1,y1], ...] :param knot_vector: 节点向量如果为None则生成准均匀节点向量 self.p degree self.ctrl_pts np.array(control_points) self.n len(self.ctrl_pts) - 1 # 控制点最大索引 if knot_vector is None: # 生成准均匀节点向量 self.knots self._generate_quasi_uniform_knots() else: self.knots np.array(knot_vector) self.m len(self.knots) - 1 # 检查节点向量长度关系: m n p 1 assert self.m self.n self.p 1, f节点向量长度错误应有 mnp1{self.nself.p1}, 但传入的为 {self.m} def _generate_quasi_uniform_knots(self): 生成准均匀节点向量 # 总共需要 m1 个节点 m n p 1 total_knots self.n self.p 2 knots np.zeros(total_knots) # 前p1个节点为0 knots[:self.p1] 0.0 # 后p1个节点为1 knots[-(self.p1):] 1.0 # 中间节点均匀分布 internal_knots_num total_knots - 2*(self.p1) if internal_knots_num 0: knots[self.p1: self.p1internal_knots_num] np.linspace(0, 1, internal_knots_num2)[1:-1] return knots关键函数basis_function实现了德布尔算法def basis_function(self, u): 计算参数u处所有非零的p次基函数值 (使用德布尔算法) :param u: 参数 :return: 列表包含p1个基函数值 [N(k-p,p)(u), ..., N(k,p)(u)] 以及对应的控制点起始索引 start_idx # 1. 找到u所在的节点区间 [knots[k], knots[k1]) # 特别处理u等于最后一个节点的情况 if u self.knots[-1]: k self.m - self.p - 1 else: k np.searchsorted(self.knots, u, sideright) - 1 k min(k, self.n) # 确保k不超过控制点最大索引n # 2. 初始化零次基函数 N np.zeros(self.p 1) for j in range(0, self.p 1): idx k - j if idx 0 or idx 1 len(self.knots): N[j] 0.0 elif self.knots[idx] u self.knots[idx 1]: N[j] 1.0 # 处理边界当u等于最后一个节点时最后一个区间是闭区间 elif u self.knots[-1] and idx self.n and self.knots[idx] u self.knots[idx 1]: N[j] 1.0 else: N[j] 0.0 # 3. 迭代计算高次基函数 for r in range(1, self.p 1): saved 0.0 for j in range(self.p, r-1, -1): idx k - j left_denom self.knots[idx r] - self.knots[idx] right_denom self.knots[idx r 1] - self.knots[idx 1] left 0.0 if left_denom ! 0: left (u - self.knots[idx]) / left_denom right 0.0 if right_denom ! 0: right (self.knots[idx r 1] - u) / right_denom N[j] left * N[j] right * saved saved N[j-1] if j 0 else 0.0 # 控制点起始索引 start_idx k - self.p return N, start_idx然后是曲线求值函数evaluate和批量生成路径的函数generate_curvedef evaluate(self, u): 计算曲线在参数u处的点 N, start_idx self.basis_function(u) point np.zeros(2) for j in range(self.p 1): ctrl_idx start_idx j if 0 ctrl_idx self.n: point N[j] * self.ctrl_pts[ctrl_idx] return point def generate_curve(self, num_samples100): 生成曲线上的采样点 u_values np.linspace(self.knots[self.p], self.knots[self.n1], num_samples) curve_points [] for u in u_values: curve_points.append(self.evaluate(u)) return np.array(curve_points)4.2 可视化与局部调整演示让我们创建一个实例并可视化同时演示局部调整。def main(): # 1. 定义控制点 control_points np.array([[0, 0], [1, 2], [3, 1], [5, 3], [7, 0], [9, 2]]) degree 3 # 三次B样条 # 2. 创建B样条对象 spline BSpline(degree, control_points) # 3. 生成曲线 curve spline.generate_curve(num_samples200) # 4. 可视化 plt.figure(figsize(10, 6)) # 绘制控制点和控制多边形 plt.plot(control_points[:, 0], control_points[:, 1], ro--, labelControl Points, linewidth1, markersize8) # 绘制B样条曲线 plt.plot(curve[:, 0], curve[:, 1], b-, labelB-Spline Curve (p3), linewidth2) plt.grid(True) plt.axis(equal) plt.legend() plt.title(B-Spline Path Planning) plt.xlabel(X) plt.ylabel(Y) # 5. 演示局部调整 print(原始控制点:, spline.ctrl_pts.tolist()) # 假设我们需要调整第三个控制点(索引2)来避障 spline.ctrl_pts[2] np.array([0.5, 1.0]) # 向右上方移动 curve_modified spline.generate_curve(num_samples200) plt.plot(curve_modified[:, 0], curve_modified[:, 1], g--, labelAfter Local Adjustment, linewidth2, alpha0.7) plt.legend() plt.show() # 打印调整前后的部分曲线点对比 print(\n局部调整后曲线中段发生了变化而两端基本保持不变。) print(例如取参数u0.5附近的点) u_mid (spline.knots[spline.p] spline.knots[spline.n1]) / 2 pt_orig spline.evaluate(u_mid) # 注意此时spline的控制点已修改需用旧数据重算此处为演示逻辑 # 实际对比应保存旧曲线数据此处从略 if __name__ __main__: main()运行这段代码你会看到一幅图红色的点和虚线是控制多边形蓝色的实线是原始B样条曲线绿色的虚线是调整一个控制点后的新曲线。可以清晰看到只有曲线中段对应被调整的控制点附近发生了形变曲线的起点和终点纹丝不动。这就是局部修改威力的直观体现。踩坑记录在实现basis_function时最容易出错的是边界处理特别是当参数u等于最后一个节点值时。如果处理不当会导致k值计算错误进而引发数组越界或基函数计算错误。务必像代码中那样对u self.knots[-1]的情况进行特殊处理。5. C实现追求性能与工程集成当你的路径规划算法需要集成到ROS机器人操作系统、自动驾驶仿真环境或对实时性要求极高的游戏服务器中时Python的解释器开销可能成为瓶颈。这时用C重写核心算法就很有必要。我们的目标是构建一个高效、可嵌入的BSpline类。5.1 工程化的类设计我们使用标准的C11并利用Eigen库进行线性代数运算它提供了优化过的矩阵向量操作比手动循环快得多。// BSpline.h #ifndef BSPLINE_H #define BSPLINE_H #include vector #include Eigen/Dense class BSpline { public: using Point Eigen::Vector2d; using PointList std::vectorPoint; BSpline(int degree, const PointList control_points); BSpline(int degree, const PointList control_points, const std::vectordouble knot_vector); // 核心功能 Point evaluate(double u) const; PointList generateCurve(int num_samples 100) const; // 工具函数 std::vectordouble generateQuasiUniformKnots() const; int findKnotSpan(double u) const; std::vectordouble computeBasisFunctions(double u) const; // 获取器 const PointList getControlPoints() const { return ctrl_pts_; } void setControlPoint(int index, const Point pt) { if(index 0 index ctrl_pts_.size()) ctrl_pts_[index] pt; } private: int p_; // 次数 PointList ctrl_pts_; // 控制点 std::vectordouble knots_; // 节点向量 int n_; // 控制点最大索引 (ctrl_pts_.size() - 1) int m_; // 节点向量最大索引 (knots_.size() - 1) void validateKnotVector() const; }; #endif // BSPLINE_H5.2 核心算法的C实现实现文件BSpline.cpp包含了算法的核心。// BSpline.cpp #include BSpline.h #include cassert #include algorithm #include stdexcept BSpline::BSpline(int degree, const PointList control_points) : p_(degree), ctrl_pts_(control_points) { if (ctrl_pts_.size() p_ 1) { throw std::invalid_argument(控制点数量至少为 degree 1); } n_ static_castint(ctrl_pts_.size()) - 1; knots_ generateQuasiUniformKnots(); m_ static_castint(knots_.size()) - 1; validateKnotVector(); } BSpline::BSpline(int degree, const PointList control_points, const std::vectordouble knot_vector) : p_(degree), ctrl_pts_(control_points), knots_(knot_vector) { n_ static_castint(ctrl_pts_.size()) - 1; m_ static_castint(knots_.size()) - 1; validateKnotVector(); } std::vectordouble BSpline::generateQuasiUniformKnots() const { int total_knots n_ p_ 2; // m1 std::vectordouble knots(total_knots, 0.0); // 前p1个节点为0 std::fill(knots.begin(), knots.begin() p_ 1, 0.0); // 后p1个节点为1 std::fill(knots.end() - (p_ 1), knots.end(), 1.0); // 中间节点均匀分布 int internal_knots_num total_knots - 2 * (p_ 1); if (internal_knots_num 0) { double step 1.0 / (internal_knots_num 1); for (int i 0; i internal_knots_num; i) { knots[p_ 1 i] step * (i 1); } } return knots; } int BSpline::findKnotSpan(double u) const { // 二分查找优化处理边界情况 if (u knots_[m_]) return n_; // 最后一个节点 if (u knots_[p_]) return p_; // 第一个有效区间起点 int low p_; int high n_ 1; int mid (low high) / 2; while (u knots_[mid] || u knots_[mid 1]) { if (u knots_[mid]) { high mid; } else { low mid; } mid (low high) / 2; } return mid; } std::vectordouble BSpline::computeBasisFunctions(double u) const { int k findKnotSpan(u); std::vectordouble N(p_ 1, 0.0); std::vectordouble left(p_ 1, 0.0); std::vectordouble right(p_ 1, 0.0); // 初始化零次基函数 N[j] 对应 N(k-j, 0) N[0] 1.0; for (int r 1; r p_; r) { left[r] u - knots_[k 1 - r]; right[r] knots_[k r] - u; double saved 0.0; for (int j 0; j r; j) { double temp N[j] / (right[j 1] left[r - j]); N[j] saved right[j 1] * temp; saved left[r - j] * temp; } N[r] saved; } // 注意这里计算出的N[0]到N[p]对应的是 N(k-p, p), ..., N(k, p) return N; } Point BSpline::evaluate(double u) const { // 1. 获取非零基函数值 auto basis_funcs computeBasisFunctions(u); int k findKnotSpan(u); // 2. 加权求和 Point result(0.0, 0.0); for (int j 0; j p_; j) { int ctrl_idx k - p_ j; result basis_funcs[j] * ctrl_pts_[ctrl_idx]; } return result; } PointList BSpline::generateCurve(int num_samples) const { PointList curve_points; curve_points.reserve(num_samples); double u_start knots_[p_]; double u_end knots_[n_ 1]; double step (u_end - u_start) / (num_samples - 1); for (int i 0; i num_samples; i) { double u u_start i * step; // 对最后一个样本确保u精确等于u_end避免浮点误差 if (i num_samples - 1) u u_end; curve_points.push_back(evaluate(u)); } return curve_points; } void BSpline::validateKnotVector() const { if (knots_.size() ! n_ p_ 2) { // m1 throw std::invalid_argument(节点向量长度必须等于 n p 2); } // 检查节点向量非递减 for (size_t i 1; i knots_.size(); i) { if (knots_[i] knots_[i - 1]) { throw std::invalid_argument(节点向量必须是非递减序列); } } }5.3 性能对比与集成注意事项性能对比在相同控制点20个点和采样点1000个的情况下用C实现开启-O2优化相比纯PythonNumPy实现计算速度通常有10倍到50倍的提升。这主要得益于C的静态编译、更少的内存分配以及Eigen库的SIMD指令优化。在需要每几毫秒就规划一次路径的实时系统中这个差距是决定性的。工程集成要点内存管理C没有垃圾回收在频繁生成路径时要避免在循环内部动态分配大量小内存如std::vector。我们的generateCurve函数一次分配好所需内存reserve是良好的实践。异常安全构造函数和关键函数中加入了参数验证并抛出标准异常std::invalid_argument便于上层调用者捕获和处理错误。接口设计提供了setControlPoint方法便于进行局部路径调整。在实际的避障循环中可以只修改少数几个控制点然后调用generateCurve重新生成路径或者更高效地只重新计算受影响的曲线段。与ROS等框架集成可以将Point类型轻松转换为ROS的geometry_msgs::Point或Pose消息。将BSpline类打包成一个独立的库或ROS节点中的工具类是常见的集成方式。实操心得在C中实现德布尔算法时我采用了算法导论中常见的“从左到右”的迭代方式并使用left和right数组预先计算了差值避免了内层循环中的重复除法和边界判断比递归版本和朴素的动态规划版本效率更高。这是从《The NURBS Book》等经典教材中学到的优化技巧。6. 在路径规划中的实战应用与进阶调优将B样条曲线应用到实际的路径规划中远不止调用一个生成函数那么简单。它需要与前端的全局规划、后端的轨迹优化以及感知模块紧密配合。6.1 与全局路径的衔接从A*到平滑曲线通常的流程是全局规划器如A*、Dijkstra生成一条由离散点组成的“折线”路径。这条路径对于机器人运动来说太粗糙了。我们的任务就是将这些离散点作为控制点生成一条平滑的B样条曲线。# 假设 global_path 是A*算法输出的离散点列表 global_path [[0,0], [2,1], [4,0], [6,2], [8,1], [10,0]] # 直接使用这些点作为控制点 control_points global_path degree 3 spline BSpline(degree, control_points) smoothed_path spline.generate_curve(num_samples200)但这里有个问题A*路径可能很曲折直接用作控制点生成的曲线可能会过于“紧绷”或出现尖角。一个常见的技巧是对全局路径进行稀疏化或采样选取关键拐点作为控制点或者在直线段上减少控制点密度在转弯处增加密度这样能生成更合理的平滑路径。6.2 动态避障实时控制点调整策略当局部传感器如激光雷达检测到障碍物时碰撞检测将生成的B样条曲线离散为密集点与障碍物地图进行碰撞检测找到发生碰撞的曲线段。参数映射找到碰撞点对应的曲线参数u_collision。影响控制点定位根据u_collision和节点向量定位出影响的p1个控制点。梯度调整定义一个“排斥力”场。障碍物对受影响的控制点产生一个排斥向量方向为从障碍物中心指向控制点大小与距离成反比如使用人工势场法思想。同时为了保持路径整体形状可以增加一个“弹性力”试图将控制点拉回原始位置。控制点的调整量是这两种力的加权和。迭代优化微调控制点后重新生成局部曲线再次进行碰撞检测。如此迭代直到路径无碰撞或达到最大迭代次数。这个过程类似于一个微型的局部轨迹优化。6.3 曲率约束与运动学可行性一条数学上平滑的曲线对机器人来说不一定可行。例如差速机器人有最小转弯半径限制。我们需要确保生成的B样条路径的曲率不超过机器人能承受的最大曲率。曲率κ(u)的计算公式为κ(u) |C(u) × C(u)| / |C(u)|^3其中C(u)和C(u)分别是曲线的一阶和二阶导数。对于B样条曲线导数的计算也有高效的递推公式。实施步骤在生成路径后对路径进行密集采样计算每个采样点的曲率。找到最大曲率κ_max。如果κ_max κ_robot机器人最大曲率则说明路径不可行。此时需要调整控制点来“拉直”曲线。一个启发式方法是找到曲率最大的点对应的参数u然后将其附近的影响控制点沿着使曲线曲率减小的方向通常是使控制多边形更平直的方向移动。这是一个带约束的优化问题实践中常使用梯度下降、二次规划QP甚至非线性优化如Ceres Solver, NLopt来求解在满足曲率约束的同时尽可能使路径短且平滑。6.4 进阶非均匀有理B样条NURBS如果项目对路径形状有更精细的要求比如需要精确表示圆弧或圆锥曲线那么就需要用到B样条的升级版——NURBS。它在B样条的基础上为每个控制点引入了一个权重因子w。NURBS曲线的公式是C(u) Σ (N(i,p)(u) * w(i) * P(i)) / Σ (N(i,p)(u) * w(i))权重w的作用w增大曲线会被拉向该控制点。w减小曲线会远离该控制点。当所有权重相等时NURBS退化为普通B样条。在路径规划中我们可以利用权重来实现一些特殊效果。例如在需要紧贴某个走廊边缘行驶时可以增加靠近边缘的那个控制点的权重在需要快速通过一个开阔区域时可以减小中间控制点的权重让路径更“直”。NURBS的实现只需在原有B样条代码的基础上增加权重的存储和计算即可核心的基函数算法完全复用。从基础的均匀B样条到应对动态环境的局部调整再到满足运动学约束的曲率优化最后到更强大的NURBS这条技术链为机器人、自动驾驶和智能体导航提供了强大而灵活的路径描述工具。理解其原理并掌握其实现意味着你手里多了一把解决复杂运动规划问题的利器。
![[C++]内存管理：串顺序存储的内存回收](http://pic.xiahunao.cn/yaotu/[C++]内存管理：串顺序存储的内存回收)
