1. 路径平滑到底在解决什么问题做机器人导航、自动驾驶决策规划或者游戏寻路的朋友大概率都遇到过这种场景规划器给出一条从起点到终点的路径数学上完全可行但实际跑起来要么原地抖动要么转弯像直角尺画出来的要么跟踪控制器频繁报错。这就是典型的“路径能用但不好用”。路径平滑要干的事情就是把这些“能用但不好用”的原始路径处理成曲率连续、贴合运动学约束、适合底层控制器执行的优质路径。我最早接触这个概念是在做移动机器人导航的时候。当时用的全局规划器输出的是栅格地图上的A*路径一格一格连起来全是45度折线。机器人跟着走的时候每到一个拐点就要减速、转向、再加速整个运动过程一顿一顿的坐在上面的人体验极差电机也容易发热。后来加了路径平滑模块同样的规划器同样的地图运动流畅度直接上了一个台阶。从那以后我就意识到路径平滑不是可选项而是规划系统里必须有的一环。这篇文章主要面向三类人第一类是刚入门机器人或自动驾驶规划控制的同学想搞清楚路径平滑的基本原理和常用方法第二类是有一定经验但想系统梳理平滑算法选型和实现细节的工程师第三类是做游戏AI寻路或者无人机航迹规划的朋友虽然应用场景不同但核心思路是相通的。我会从为什么需要平滑讲起然后拆解几种主流方法的原理和实现再给出一套可以直接参考的实操流程最后分享一些踩过的坑和排查技巧。2. 为什么原始路径必须做平滑处理2.1 从规划器的输出说起大多数全局规划器比如A*、Dijkstra、RRT系列它们工作的空间是离散的。A*在栅格地图上搜索路径由一系列相邻栅格的中心点组成方向只有8个如果允许对角移动。RRT在连续空间采样但输出的路径是树节点之间的直线段连接段与段之间是折线。这些路径有一个共同特点一阶导数不连续也就是说切线方向在拐点处发生突变。对于差速驱动机器人来说这意味着在拐点处需要原地旋转或者急转弯而实际执行时由于惯性、轮子打滑等因素根本做不到精确的原地旋转结果就是轨迹偏差。对于阿克曼转向的车辆来说问题更大因为车辆有最小转弯半径约束折线路径的拐角可能根本转不过去。对于无人机来说折线路径意味着加速度突变飞行姿态会剧烈变化影响传感器稳定性和拍摄质量。2.2 不平滑路径带来的具体问题我把实际项目中遇到过的问题归纳成下面几类跟踪误差大底层控制器如纯追踪、MPC跟踪折线路径时在拐点处误差显著增大因为控制器需要预测未来一段路径而折线路径的预测方向在拐点处跳变。速度频繁波动为了在拐点处转向速度必须降下来过了拐点再加速导致整个运动过程速度不连续能耗增加乘坐体验差。机械磨损加剧频繁的急加速急减速和急转弯对电机、减速器、轮胎都是额外的负担长期运行会缩短硬件寿命。曲率不满足约束对于有最小转弯半径的车辆折线路径的等效曲率在拐点处趋于无穷大物理上不可执行。路径长度不是最优折线路径往往比平滑后的曲线路径更长因为折线绕了远路。注意路径平滑不是万能的。如果原始路径本身穿越了障碍物或者违反了安全距离约束平滑操作可能会让路径更贴近障碍物甚至发生碰撞。所以平滑之前必须确认原始路径是安全可行的平滑之后也要做碰撞检测。2.3 平滑的目标定义一个好的平滑路径应该满足哪些条件我通常用下面几个指标来衡量指标说明理想状态曲率连续性曲率是否连续变化一阶或二阶连续最大曲率路径上最大曲率值不超过车辆最小转弯半径对应曲率路径长度平滑后路径总长度尽量接近原始路径长度安全距离与障碍物的最小距离不小于安全阈值跟踪误差控制器跟踪时的偏差越小越好计算效率平滑算法耗时满足实时性要求这几个指标之间往往存在矛盾。比如过度追求曲率连续可能导致路径偏离原始路径太远增加碰撞风险过度追求路径长度最优可能导致曲率不满足约束。所以平滑算法的核心是在这些约束之间找平衡。3. 主流路径平滑方法拆解与选型3.1 基于几何的平滑方法这类方法直接对路径的几何形状做处理不涉及复杂的优化求解计算量小适合实时性要求高的场景。Chaikin曲线细分是一种经典的几何平滑方法。它的核心思想是“切角”对路径上的每个线段取两个端点附近的两个点用这两个点替换原来的端点。反复迭代几次折线就会逐渐变成光滑曲线。我试过用三次迭代对于栅格地图的A*路径基本能消除明显的折角。优点是实现极其简单计算量线性增长缺点是平滑后的路径不经过原始路径点可能偏离障碍物安全边界而且曲率连续性只能达到一阶。B样条曲线拟合是更常用的方法。给定一组控制点B样条可以生成任意阶连续的曲线。实际用的时候通常把原始路径点作为控制点生成三次B样条曲线。三次B样条能保证曲率连续而且有局部支撑性修改一个控制点只影响局部曲线形状。但B样条曲线一般不经过控制点除非用插值样条所以平滑后的路径会偏离原始路径。偏离量取决于控制点的分布路径拐角越急偏离越大。贝塞尔曲线在游戏寻路里用得很多。它的优点是曲线一定经过起点和终点而且完全包含在控制点凸包内安全性好。缺点是控制点数量多时曲线阶数高计算不稳定而且修改一个控制点会影响整条曲线。实际工程中通常用分段三次贝塞尔曲线拼接保证拼接点处切线连续。3.2 基于优化的平滑方法这类方法把路径平滑建模成一个优化问题目标函数通常包含平滑项和偏离项约束包括曲率约束、碰撞约束等。求解方法有梯度下降、二次规划、非线性优化等。最小化曲率平方和是最常见的优化目标。把路径离散成一系列点用相邻三点的曲率近似然后最小化所有点曲率平方的和。这个目标函数是二次的可以用稀疏线性系统高效求解。但单纯最小化曲率会让路径趋向于直线偏离原始路径所以需要加一个偏离惩罚项约束平滑后路径不能离原始路径太远。弹性带模型是另一种思路。把路径想象成一条橡皮筋原始路径点受到两种力一种是内部力让相邻点之间保持一定距离并趋向于直线另一种是外部力让点不要偏离原始位置太远。迭代求解力的平衡状态就得到平滑路径。弹性带模型的优点是物理意义直观容易加入各种约束比如障碍物排斥力。缺点是迭代次数不好控制可能不收敛。二次规划方法把平滑问题转化成标准的QP问题。决策变量是平滑后路径点的坐标目标函数是曲率平方和加偏离惩罚约束是线性化的碰撞约束和曲率约束。QP有成熟的求解器能保证找到全局最优解在凸化条件下。但碰撞约束通常是非凸的需要线性化或者用序列二次规划迭代求解计算量较大。3.3 基于插值的平滑方法这类方法在原始路径点之间插入额外的点然后用插值函数生成平滑曲线。三次样条插值是最常用的。给定一系列路径点三次样条可以生成经过所有点的C2连续曲线。优点是曲线经过原始路径点不会偏离缺点是如果原始路径点分布不均匀或者有噪声样条曲线可能出现剧烈振荡曲率不满足约束。实际用的时候通常要先对路径点做重采样让点间距均匀再拟合样条。圆弧拼接方法在自动驾驶里用得比较多。它的思路是在相邻直线段之间插入一段圆弧圆弧与两段直线都相切。这样生成的路径由直线和圆弧组成曲率是分段常数虽然不连续但满足车辆运动学约束。圆弧拼接的优点是计算简单曲率可控缺点是曲率不连续在直线和圆弧的连接点处曲率突变对控制器不够友好。3.4 方法选型对比下面这张表是我在实际项目中总结的选型参考方法计算复杂度曲率连续性是否经过原始点适合场景Chaikin细分低C1否实时性要求高、精度要求低B样条拟合中C2否机器人导航、无人机航迹贝塞尔曲线中C1/C2是端点游戏寻路、简单路径最小曲率优化高C2否可约束自动驾驶、高精度场景弹性带中高C1否可约束动态避障、局部规划三次样条插值中C2是路径点稀疏且准确圆弧拼接低C0是自动驾驶、阿克曼车辆选型的时候我一般遵循几个原则如果原始路径点很准优先考虑插值方法如果原始路径有噪声或者点间距不均匀优先考虑拟合方法如果对曲率连续性要求高用B样条或优化方法如果计算资源紧张用Chaikin或圆弧拼接。4. 实操过程与核心环节实现4.1 整体流程设计我通常把路径平滑分成四个步骤预处理、平滑、后处理、验证。预处理包括去除重复点、重采样、计算原始路径的曲率和长度。平滑是核心步骤根据选型调用相应算法。后处理包括碰撞检测、曲率约束检查、路径重采样。验证是把平滑后的路径送给控制器做仿真或者实车测试。下面以B样条平滑为例给出完整的实现流程。选择B样条是因为它在曲率连续性和计算效率之间平衡得比较好适合大多数机器人导航场景。4.2 路径预处理原始路径点往往存在重复点、间距不均匀、噪声等问题。直接拿来做平滑效果会很差。预处理的第一步是去除重复点两个点距离小于一个阈值比如0.01米就合并成一个点。第二步是重采样用等间距的方式在原始路径上取点间距根据场景设定一般取0.1到0.5米。重采样用线性插值就行不需要太复杂。import numpy as np def resample_path(path, spacing0.2): 对路径做等间距重采样 path np.array(path) # 计算累积弧长 diffs np.diff(path, axis0) seg_lengths np.linalg.norm(diffs, axis1) cum_lengths np.concatenate([[0], np.cumsum(seg_lengths)]) total_length cum_lengths[-1] # 生成等间距采样点 num_points int(total_length / spacing) 1 sample_dists np.linspace(0, total_length, num_points) # 线性插值 resampled np.zeros((num_points, 2)) for i, d in enumerate(sample_dists): idx np.searchsorted(cum_lengths, d) - 1 idx max(0, min(idx, len(path) - 2)) t (d - cum_lengths[idx]) / (cum_lengths[idx1] - cum_lengths[idx] 1e-8) resampled[i] path[idx] * (1 - t) path[idx1] * t return resampled重采样之后路径点间距均匀了但折角还在。这时候可以计算一下原始路径的最大曲率作为后续平滑的参考。曲率用相邻三点的外接圆曲率近似def compute_curvature(path): 计算路径上每一点的曲率 path np.array(path) dx np.gradient(path[:, 0]) ddx np.gradient(dx) dy np.gradient(path[:, 1]) ddy np.gradient(dy) curvature np.abs(dx * ddy - dy * ddx) / (dx**2 dy**2)**1.5 return curvature4.3 B样条平滑实现B样条曲线的数学定义这里不展开网上资料很多。实际用的时候我直接用scipy的splprep和splev省得自己造轮子。关键参数是平滑因子s它控制曲线对原始点的拟合程度。s0时曲线经过所有点s越大曲线越平滑但偏离越大。from scipy import interpolate def bspline_smooth(path, s0.5, num_samples200): 用B样条对路径做平滑 path np.array(path) # 参数化 tck, u interpolate.splprep([path[:, 0], path[:, 1]], ss, k3) # 均匀采样 u_new np.linspace(0, 1, num_samples) smooth_path interpolate.splev(u_new, tck) smooth_path np.array(smooth_path).T return smooth_paths的取值很关键。我一般先用s0试一下看曲线是否经过所有点然后逐渐增大s直到曲率满足约束或者路径偏离不超过阈值。这个过程可以自动化写一个循环逐步增大s每次检查最大曲率和最大偏离量找到满足条件的最小s。4.4 曲率约束处理B样条平滑后的路径曲率虽然连续了但最大值可能还是超过车辆的最小转弯半径约束。这时候有两种处理方式一种是增大平滑因子s让曲线更平缓另一种是对路径做速度规划在曲率大的地方降速。实际项目中我通常两种结合先通过增大s把最大曲率降到约束的1.2倍以内剩下的通过速度规划解决。如果曲率还是超标可以考虑用优化方法做二次平滑。以B样条平滑后的路径为初始解建立QP问题目标函数是曲率平方和约束是最大曲率不超过阈值。QP求解用OSQP或者quadprog都行。import osqp import scipy.sparse as sparse def curvature_constrained_smooth(path, max_curvature, weight_smooth1.0, weight_dev0.1): 带曲率约束的二次规划平滑 n len(path) # 决策变量是每个点的x和y坐标展成2n维向量 # 目标函数平滑项 偏离项 # 平滑项用二阶差分近似曲率 # 这里省略具体矩阵构造核心思路是构造稀疏矩阵 # 然后用OSQP求解 pass4.5 碰撞检测与后处理平滑后的路径必须做碰撞检测。我通常用膨胀后的栅格地图做快速检测把平滑路径离散成点查每个点是否在障碍物膨胀区域内。如果发生碰撞就把平滑路径往远离障碍物的方向推或者减小平滑因子重新平滑。后处理还包括路径重采样和速度规划。平滑后的路径点间距可能不均匀需要重新等间距采样方便控制器使用。速度规划根据曲率大小分配速度曲率大的地方速度低曲率小的地方速度高保证向心加速度不超过阈值。实操心得B样条平滑的s参数不要一次调太大。我试过直接设s10结果路径直接切过障碍物。正确的做法是从小到大逐步增加每次增加后做碰撞检测一旦发现碰撞就回退到上一个安全值。5. 常见问题与排查技巧实录5.1 平滑后路径偏离障碍物安全距离这是最常见的问题。原因通常是平滑因子过大或者原始路径本身就贴着障碍物。排查方法是把原始路径、平滑路径、障碍物地图画在一起直观看到偏离情况。解决办法有三个减小平滑因子、在障碍物附近增加控制点约束、用带碰撞约束的优化方法。我一般会在平滑之前先把原始路径往远离障碍物的方向推一点给平滑留出余量。具体做法是计算每个路径点到最近障碍物的距离如果小于安全阈值就沿梯度方向移动路径点直到满足安全距离。5.2 曲率不连续或突变B样条理论上保证C2连续但实际计算时如果参数化不合理或者点间距变化太大曲率可能出现尖峰。排查方法是画出曲率随弧长的变化曲线看是否有异常尖峰。解决办法是重采样让点间距均匀或者降低样条阶数从三次降到二次。还有一种情况是路径起点和终点的曲率不连续。B样条在端点处需要额外的边界条件默认是自然边界条件曲率为零。如果实际路径在起点处有曲率就会突变。解决办法是用夹持边界条件指定端点的切向量。5.3 平滑算法耗时过长优化类平滑方法计算量大可能无法满足实时性。排查方法是profile每个步骤的耗时看瓶颈在哪里。常见的优化手段包括减少路径点数量重采样时增大间距、降低优化变量维度只优化曲率大的区域、用热启动以上一帧的平滑结果为初始解。我在实际项目中遇到过QP求解耗时超过100毫秒的情况后来把路径点从500个降到100个耗时直接降到20毫秒以内平滑效果没有明显下降。所以路径点不是越多越好够用就行。5.4 平滑后路径长度增加太多平滑操作通常会让路径变长因为曲线绕过了折角。如果长度增加超过10%就需要检查了。原因可能是平滑因子过大或者原始路径折角太急。解决办法是限制平滑因子的上限或者在折角处插入过渡点。下面这张表是我总结的常见问题速查问题现象可能原因排查方法解决办法路径偏离障碍物平滑因子过大可视化对比减小s加碰撞约束曲率突变点间距不均画曲率曲线重采样降阶计算耗时高路径点太多Profile减点热启动路径变长太多折角太急对比长度限制s加过渡点路径抖动原始路径噪声观察原始点先滤波再平滑端点曲率不连续边界条件不当检查端点用夹持边界5.5 独家避坑技巧第一个技巧平滑之前一定要做重采样。我早期偷懒直接拿A*的原始路径做B样条结果因为栅格路径点间距是1米样条曲线在拐角处剧烈振荡曲率爆表。重采样到0.2米间距后同样参数下曲线平滑多了。第二个技巧平滑因子s不要用固定值要根据路径的曲率分布自适应调整。曲率大的路段用小的s曲率小的路段用大的s。实现上可以分段平滑每段用不同的s然后在段之间做平滑过渡。第三个技巧如果用的是优化方法目标函数里的权重需要仔细调。平滑项权重太大路径会偏离原始路径偏离项权重太大平滑效果不明显。我一般先用权重比1:0.1试然后根据结果微调。调参的时候固定一个权重调另一个观察曲率和偏离量的变化。第四个技巧平滑后的路径一定要做仿真验证。我习惯用纯追踪控制器跑一遍看跟踪误差和速度曲线。如果跟踪误差在拐弯处明显增大说明平滑还不够或者曲率约束没满足。6. 不同场景下的平滑策略差异6.1 室内机器人导航室内机器人速度慢通常小于1米每秒对曲率连续性要求不高但对计算效率和避障安全性要求高。我通常用Chaikin细分或者轻量级B样条平滑因子取小值保证路径贴近原始路径。碰撞检测用膨胀栅格膨胀半径取机器人半径加安全余量。室内场景还有一个特点路径经常需要穿过窄通道。平滑时要注意不要让曲线切到通道墙壁。我的做法是在窄通道区域禁用平滑或者用很小的平滑因子出了窄通道再正常平滑。6.2 自动驾驶决策规划自动驾驶对曲率连续性和运动学约束要求严格。我通常用二次规划方法显式加入曲率约束和碰撞约束。路径点间距取0.5到1米平滑范围覆盖整个规划时域。计算耗时控制在50毫秒以内满足实时性。自动驾驶还有一个特殊需求路径要满足车辆动力学约束比如侧向加速度不超过阈值。这需要在平滑之后做速度规划根据曲率分配速度。曲率大的地方速度低保证侧向加速度在舒适范围内。6.3 无人机航迹规划无人机是三维运动路径平滑要在三维空间做。我通常用三维B样条分别对x、y、z三个维度做平滑。无人机的约束主要是最大加速度和最大加加速度对应路径的二阶和三阶导数。所以平滑目标里要加入加加速度惩罚项。无人机场景还有一个特点路径点可能来自RRT*等采样规划器点间距不均匀且可能有噪声。预处理阶段要做离群点去除和重采样否则平滑效果很差。6.4 游戏AI寻路游戏寻路对实时性要求极高通常每帧都要重新规划。平滑方法要非常轻量Chaikin细分或者简单的贝塞尔曲线拼接就够了。游戏场景对物理约束不严格主要追求视觉上的流畅感。游戏寻路的另一个特点是路径点来自导航网格点间距大且形状规则。平滑时要注意保持路径在可行走区域内不能切到不可行走区域。通常用导航网格的边界做碰撞检测。7. 写在最后的一些个人体会路径平滑这个方向看起来简单做深了坑很多。我最大的体会是没有一种平滑方法能通吃所有场景必须根据具体需求做选型和调参。B样条适合大多数机器人场景QP适合自动驾驶Chaikin适合游戏圆弧拼接适合阿克曼车辆。选对了方法事半功倍选错了调参调到怀疑人生。另一个体会是平滑不是孤立的模块它和规划器、控制器紧密耦合。规划器输出的路径质量直接影响平滑难度控制器的跟踪能力决定了平滑精度的要求。做平滑的时候一定要考虑上下游不能只盯着平滑算法本身。最后分享一个我常用的调试手段把原始路径、平滑路径、曲率曲线、速度曲线画在一个图里一眼就能看出问题在哪里。曲率曲线有尖峰说明平滑不够速度曲线有突变说明速度规划没做好路径偏离障碍物太近说明平滑因子太大。可视化是排查平滑问题最有效的工具没有之一。