三次B样条、三维空间平滑处理、离散路径点Matlab程序

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

在多机器人协作场景中，除了要避免碰撞，“路径是否平滑” 同样关键 —— 比如无人机在三维空间巡检时，若路径颠簸会消耗更多电量；AGV 机器人在搬运精密零件时，路径突变可能导致货物晃动；协作机器人机械臂运动时，不平滑路径会加剧设备磨损。而 “三次 B 样条” 技术，正是解决 “三维离散路径点平滑处理” 的核心工具，它能让机器人从 “折线式移动” 升级为 “流畅式运动”，同时与 “缓冲不确定性感知 Voronoi 单元” 技术配合，实现 “安全与平滑” 的双重目标。今天我们就来拆解这项技术，看看它如何让多机器人的三维路径更 “丝滑”。

一、先搞懂痛点：三维离散路径点为何需要 “平滑”？

在多机器人路径规划中，算法通常会先生成一系列 “三维离散路径点”（比如无人机巡检时的 “起点→杆塔 A→杆塔 B→终点”，每个点包含 X、Y、Z 坐标），但直接按这些离散点运动，会面临三大问题：

1. 运动 “卡顿”：机器人像 “走折线”

离散路径点之间是直线连接，若相邻两点距离近、角度大（比如从 “X=1,Y=1,Z=1” 突然转向 “X=2,Y=3,Z=2”），机器人需要频繁调整速度和方向，导致运动过程卡顿。例如 AGV 机器人按离散点搬运货物时，每到一个路径点就要减速、转向、再加速，不仅耗时，还可能让货物因惯性偏移。

2. 能耗 “飙升”：设备负担加重

不平滑的路径会增加机器人的能耗 —— 无人机在折线飞行时，需要频繁改变飞行姿态（如突然爬升、转弯），电机功率波动大，续航时间可能缩短 20% 以上；协作机器人机械臂若按折线运动，关节电机需反复启停，长期使用会加剧齿轮磨损，降低设备寿命。

3. 安全 “隐患”：与缓冲单元适配难

前文提到的 “缓冲不确定性感知 Voronoi 单元” 需要机器人运动状态稳定（如速度、加速度可控），若路径不平滑，机器人可能在离散点附近出现 “速度突变”，导致缓冲单元边界计算偏差，甚至突破安全区域。比如无人机在折线转弯时，瞬时速度可能超过预期，让原本的缓冲距离不足以应对误差，增加碰撞风险。

传统的平滑方法（如线性插值、多项式拟合）要么平滑效果差（仍有明显拐点），要么计算复杂（难以实时调整），而 “三次 B 样条” 技术凭借 “局部可控、平滑度高、计算高效” 的优势，成为三维离散路径点平滑处理的首选方案。

二、核心原理：三次 B 样条 —— 让离散点 “连成丝滑曲线”

要理解三次 B 样条，我们可以先把它想象成 “用弹性绳子连接离散点”—— 绳子会自然弯曲，形成平滑曲线，且调整其中一个点的位置，只会影响附近的曲线形状，不会改变整体趋势。这种 “局部可控性” 和 “高平滑度”，正是三次 B 样条的核心优势。

1. 基础概念：B 样条的 “三个关键要素”

三次 B 样条是 “B 样条曲线” 的一种（阶数为 3，对应三次多项式），其平滑处理的核心依赖三个要素：

• 离散路径点（控制顶点）：即机器人规划出的原始路径点，记为 P₀、P₁、P₂、…、Pₙ（每个点包含 X、Y、Z 三维坐标），这些点是 B 样条曲线的 “支撑点”，决定曲线的整体走向；

• 节点向量：用于分割曲线的 “分段标记”，记为 t₀、t₁、…、tₘ，三次 B 样条的节点向量长度需满足 “m = n + 4”（比如有 5 个离散点，节点向量就有 9 个值）。节点向量通常采用 “均匀分布”，确保曲线在各段的平滑度一致；

• 基函数：三次 B 样条的 “曲线生成公式”，记为 Nᵢ₃(t)（i 为控制顶点索引，3 代表三次），它是一个关于参数 t 的三次多项式。通过基函数，可将离散路径点 “加权组合” 成连续曲线，公式为：

C (t) = Σ（i=0 到 n）Nᵢ₃(t) × Pᵢ（t∈[t₃, tₘ₋₃]）

简单来说，参数 t 从 0 到 1 变化时，基函数会不断调整每个离散点的权重，最终生成一条连续、平滑的曲线，且曲线会 “靠近但不必然经过” 所有离散点（避免拐点），同时保证曲线的一阶导数（速度）和二阶导数（加速度）连续 —— 这意味着机器人沿曲线运动时，速度和加速度不会突变，彻底消除 “卡顿”。

2. 三维空间适配：一次处理 “X、Y、Z 三个维度”

三次 B 样条在三维空间的应用，本质是对 X、Y、Z 三个维度分别进行 B 样条平滑，再将三个维度的曲线组合成三维路径。例如无人机的离散路径点为 P₀(X₀,Y₀,Z₀)、P₁(X₁,Y₁,Z₁)、P₂(X₂,Y₂,Z₂)，处理时：

• 对 X 坐标序列 [X₀,X₁,X₂] 做三次 B 样条平滑，得到 X 方向的曲线 X (t)；

• 对 Y 坐标序列 [Y₀,Y₁,Y₂]、Z 坐标序列 [Z₀,Z₁,Z₂] 分别做同样处理，得到 Y (t) 和 Z (t)；

• 组合 X (t)、Y (t)、Z (t)，即为无人机的三维平滑路径 C (t)=(X (t), Y (t), Z (t))。

这种 “分维度处理、再组合” 的方式，不仅计算高效（三个维度可并行计算），还能灵活适配不同维度的需求 —— 比如在 Z 方向（高度）需要更平缓的曲线（避免无人机频繁升降），可通过调整 Z 方向的基函数权重实现，而不影响 X、Y 方向的路径。

三、实现步骤：从离散点到三维平滑路径的 “四步流程”

用三次 B 样条处理多机器人三维离散路径点，可分为四个关键步骤，我们以 “无人机巡检集群（3 台无人机，原始离散路径点各 5 个）” 为例拆解：

步骤 1：预处理离散路径点 —— 去除 “异常点”

原始离散路径点可能存在 “异常值”（如因传感器误差导致的 “跳变点”，比如某点 Z 坐标突然从 10 米升至 50 米），若直接用于平滑，会导致曲线出现 “突兀凸起”。因此第一步需要：

• 异常点检测：通过 “相邻点距离阈值” 判断（如设定相邻点三维距离不超过 10 米，超过则视为异常点）；

• 异常点修正：用 “线性插值” 替换异常点（如 P₂为异常点，可用 (P₁+P₃)/2 的坐标值替代），确保离散点序列连续、合理。

例如某无人机原始离散点为 P₀(0,0,10)、P₁(5,5,12)、P₂(6,6,60)（异常点）、P₃(10,10,15)、P₄(15,15,18)，修正后 P₂变为 (8,8,13.5)，避免后续曲线异常。

步骤 2：构建节点向量 —— 确定 “曲线分段”

根据离散路径点数量 n（本例 n=4，5 个点），按 “m = n + 4” 确定节点向量长度 m=8，采用 “均匀节点向量”（便于实时计算），取值为：

t₀=0, t₁=0, t₂=0, t₃=0, t₄=1, t₅=2, t₆=3, t₇=3, t₈=3, t₉=3（注：三次 B 样条需 “两端各 3 个重复节点”，确保曲线从第一个离散点 P₀开始，到最后一个离散点 P₄结束）。

节点向量的作用是将曲线分为 “n - 2” 段（本例为 3 段），每段对应参数 t 的一个区间（如 t∈[t₃,t₄]=[0,1] 对应第一段，t∈[t₄,t₅]=[1,2] 对应第二段），便于机器人分阶段调整运动状态。

步骤 3：计算基函数与平滑曲线 —— 生成 “丝滑路径”

根据三次 B 样条基函数公式，计算每个参数 t 对应的基函数值，再结合离散路径点加权得到平滑曲线。以第一段曲线（t∈[0,1]）为例：

• 基函数 N₀₃(t)、N₁₃(t)、N₂₃(t)、N₃₃(t) 均为关于 t 的三次多项式（如 N₀₃(t) = (1-t)³/6）；

• 将 t 从 0 到 1 均匀取 100 个值（确保曲线足够平滑），每个 t 值对应一个三维坐标 C (t) = N₀₃(t) P₀ + N₁₃(t) P₁ + N₂₃(t) P₂ + N₃₃(t) P₃；

• 同理计算第二段（t∈[1,2]）和第三段（t∈[2,3]）的曲线，最终得到从 P₀到 P₄的完整三维平滑路径。

通过计算可发现，平滑后的路径在相邻离散点之间没有明显拐点，且曲线的速度（一阶导数）和加速度（二阶导数）连续 —— 比如无人机从 P₀到 P₁时，速度从 0 平稳提升至 5m/s，加速度始终控制在 0.5m/s² 以内，避免剧烈运动。

步骤 4：与缓冲单元协同 —— 确保 “平滑且安全”

平滑后的路径需要与 “缓冲不确定性感知 Voronoi 单元” 技术配合，避免因路径调整导致碰撞风险：

• 路径边界检查：计算平滑路径上每个点到相邻机器人缓冲单元边界的距离，确保距离始终大于 “缓冲距离的 1.2 倍”（预留安全余量）；

• 动态调整平滑度：若某段平滑路径靠近其他机器人的缓冲单元（如距离仅为缓冲距离的 1.1 倍），可通过 “调整基函数权重” 增加该段曲线的弯曲程度，远离缓冲单元边界；

• 实时更新：当机器人感知到动态障碍物（如突然出现的鸟类）时，重新生成局部离散路径点（如在障碍物附近新增 2 个点），快速重新计算三次 B 样条曲线，实现 “平滑避让”。

⛳️ 运行结果

📣 部分代码

length(P) - 1;    T = cumsum([0, ones(n-2,1), 0.5*ones(n-3,1), 0.1667*ones(n-4,1)]);    T = T(1:n-3);        % 基函数计算    B = bspline(T, 3);        % 插值结果    S = P(1:n-3, :) * B';        % 辅助函数    functionbspline(t, k)        if k == 0            return delta(t, 0);        else            return bspline(t, k-1) - bspline(t, k-1, 1) + bspline(t, k-1, 2);        end    endend% 三维路径平滑算法functionQ = smooth_3d_path(P, lambda, iterations)    % 初始化控制点    Q = P;        % 迭代优化    for iter = 1:iterations

🔗 参考文献

🎈 部分理论引用网络文献，若有侵权联系博主删除

 👇 关注我领取海量matlab电子书和数学建模资料 

🏆团队擅长辅导定制多种科研领域MATLAB仿真，助力科研梦：

🌟 各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化、背包问题、 风电场布局、时隙分配优化、 最佳分布式发电单元分配、多阶段管道维修、 工厂-中心-需求点三级选址问题、 应急生活物质配送中心选址、 基站选址、 道路灯柱布置、 枢纽节点部署、 输电线路台风监测装置、 集装箱调度、 机组优化、 投资优化组合、云服务器组合优化、 天线线性阵列分布优化、CVRP问题、VRPPD问题、多中心VRP问题、多层网络的VRP问题、多中心多车型的VRP问题、 动态VRP问题、双层车辆路径规划（2E-VRP）、充电车辆路径规划（EVRP）、油电混合车辆路径规划、混合流水车间问题、 订单拆分调度问题、 公交车的调度排班优化问题、航班摆渡车辆调度问题、选址路径规划问题、港口调度、港口岸桥调度、停机位分配、机场航班调度、泄漏源定位、冷链、时间窗、多车场等、选址优化、港口岸桥调度优化、交通阻抗、重分配、停机位分配、机场航班调度、通信上传下载分配优化

🌟 机器学习和深度学习时序、回归、分类、聚类和降维

2.1 bp时序、回归预测和分类

2.2 ENS声神经网络时序、回归预测和分类

2.3 SVM/CNN-SVM/LSSVM/RVM支持向量机系列时序、回归预测和分类

2.4 CNN|TCN|GCN卷积神经网络系列时序、回归预测和分类

2.5 ELM/KELM/RELM/DELM极限学习机系列时序、回归预测和分类

2.6 GRU/Bi-GRU/CNN-GRU/CNN-BiGRU门控神经网络时序、回归预测和分类

2.7 ELMAN递归神经网络时序、回归\预测和分类

2.8 LSTM/BiLSTM/CNN-LSTM/CNN-BiLSTM/长短记忆神经网络系列时序、回归预测和分类

2.9 RBF径向基神经网络时序、回归预测和分类

2.10 DBN深度置信网络时序、回归预测和分类

2.11 FNN模糊神经网络时序、回归预测

2.12 RF随机森林时序、回归预测和分类

2.13 BLS宽度学习时序、回归预测和分类

2.14 PNN脉冲神经网络分类

2.15 模糊小波神经网络预测和分类

2.16 时序、回归预测和分类

2.17 时序、回归预测预测和分类

2.18 XGBOOST集成学习时序、回归预测预测和分类

2.19 Transform各类组合时序、回归预测预测和分类

方向涵盖风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、用电量预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

🌟图像处理方面

图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

🌟 路径规划方面

旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、 充电车辆路径规划（EVRP）、 双层车辆路径规划（2E-VRP）、 油电混合车辆路径规划、 船舶航迹规划、 全路径规划规划、 仓储巡逻、公交车时间调度、水库调度优化、多式联运优化

🌟 无人机应用方面

无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化、车辆协同无人机路径规划、

🌟 通信方面

传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化、水声通信、通信上传下载分配

🌟 信号处理方面

信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化、心电信号、DOA估计、编码译码、变分模态分解、管道泄漏、滤波器、数字信号处理+传输+分析+去噪、数字信号调制、误码率、信号估计、DTMF、信号检测

🌟电力系统方面

微电网优化、无功优化、配电网重构、储能配置、有序充电、MPPT优化、家庭用电、电/冷/热负荷预测、电力设备故障诊断、电池管理系统（BMS）SOC/SOH估算（粒子滤波/卡尔曼滤波）、 多目标优化在电力系统调度中的应用、光伏MPPT控制算法改进（扰动观察法/电导增量法）、电动汽车充放电优化、微电网日前日内优化、储能优化、家庭用电优化、供应链优化

🌟 元胞自动机方面

交通流 人群疏散 病毒扩散 晶体生长 金属腐蚀

🌟 雷达方面

卡尔曼滤波跟踪、航迹关联、航迹融合、SOC估计、阵列优化、NLOS识别

🌟 车间调度

零等待流水车间调度问题NWFSP 、 置换流水车间调度问题PFSP、 混合流水车间调度问题HFSP 、零空闲流水车间调度问题NIFSP、分布式置换流水车间调度问题 DPFSP、阻塞流水车间调度问题BFSP

👇

5 往期回顾扫扫下方二维码