三维空间三次b样条平滑处理。 可输入离散路径点就可以进行平滑处理拿来能用。最近在做一个机器人路径规划的项目发现直接生成的路径点总是很生硬机器人走起来一顿一顿的看着就难受。于是研究了一下三次B样条曲线发现这东西简直就是为路径平滑而生的啊先来点硬核的看看B样条曲线的公式def b_spline(t, points, degree3): n len(points) - 1 if degree n: raise ValueError(Degree must be less than or equal to the number of control points minus one.) def basis_function(i, k, t): if k 0: return 1 if i t i1 else 0 else: return ((t - i) / k) * basis_function(i, k-1, t) ((i k 1 - t) / k) * basis_function(i1, k-1, t) result [0, 0, 0] for i in range(n 1): basis basis_function(i, degree, t) result[0] points[i][0] * basis result[1] points[i][1] * basis result[2] points[i][2] * basis return result这代码看着有点复杂但其实核心思想很简单通过一组控制点和基函数的加权求和就能生成一条平滑的曲线。这里的degree3表示我们用的是三次B样条也是最常用的。现在假设我们有一组离散的路径点points [ [0, 0, 0], [1, 2, 3], [3, 4, 5], [6, 7, 8], [9, 10, 11] ]把这些点丢进我们的B样条函数里就能生成一条平滑的曲线。比如我们可以在t从0到n之间均匀采样生成一系列平滑后的点smoothed_points [] for t in np.linspace(0, len(points) - 1, 100): smoothed_points.append(b_spline(t, points))这样smoothed_points就是平滑后的路径点了。你可以把这些点直接喂给机器人它走起来就会丝滑很多。三维空间三次b样条平滑处理。 可输入离散路径点就可以进行平滑处理拿来能用。不过这里有个小坑要注意B样条曲线不会经过所有的控制点除非你是用特殊的节点向量。如果你想让曲线经过某些关键点可以在这些点附近多放几个控制点或者用其他插值方法先处理一下。最后B样条的好处不仅仅是平滑它还能很方便地调整局部形状。比如你觉得某一段路径不够理想只需要调整附近的几个控制点整条曲线就会跟着变化而不会影响到其他部分。这在实际应用中非常有用毕竟机器人路径规划经常需要根据环境变化动态调整。总之三次B样条曲线是个好东西搞路径平滑的兄弟们可以试试。代码虽然看起来有点复杂但用起来其实很简单效果也相当不错。