4.5 轨迹优化与平滑
4.5 轨迹优化与平滑
本节摘要：搜索算法吐出的是一串贴着膨胀障碍的栅格折线，控制器吃到这种输入要么抖动要么超调。本节讲路径简化、样条拟合与基于优化的平滑三步加工链，并强调平滑必须与安全裕度联算——顺滑不能以擦墙为代价。
为什么不让控制器直接跟折线
A* 给出的路径由几十个直角折线段组成。让底盘沿它走会发生什么？每个拐角处角速度指令阶跃，差速底盘要么原地猛转（乘员不适、定位抖动），要么因动力学惯性冲出拐角（切角撞向膨胀区）。机械臂场景更糟：关节轨迹的速度不连续意味着力矩无穷大——物理上不存在这种运动。搜索器不懂动力学，平滑器负责把几何解翻译成动力学可行的轨迹，这是规划链上最后一道加工工序。
三步加工链
第一步：路径简化（道格拉斯-普克或视线剪枝）。折线里大量拐点毫无必要——两拐点之间若直线可走，中间点全是冗余。视线剪枝从起点出发，找能"直接看到"的最远节点，连成一段；重复到终点。典型效果：60 个节点的 A* 路径剪到 8 段以内。注意剪枝用的碰撞检查应针对未膨胀的原始障碍并留出裕度，因为剪出的直线段偏离原折线，可能扫过膨胀区的边缘。
第二步：样条拟合。把简化后的路径点作为控制点，用三次样条或 B 样条拟合成 C² 连续（位置、速度、加速度都连续）的曲线。B 样条的优势在局部可控性：动一个控制点只影响邻近几段，微调转弯半径不惊动全局。
第三步：基于优化的平滑。当约束复杂（多障碍、曲率上限、速度上限），拟合式平滑不够用，就升级成数值优化：目标函数里同时放"路径长度、平滑度（二阶差分范数）、离障碍距离"三项，约束里放最大曲率与安全距离，交给非线性优化器求解。TEB（时间弹性带）就是这条思路的成熟实现，它连时间维一起优化——"多快到达"也成了决策变量。
图：从栅格折线到可执行轨迹的三步加工
平滑与安全的联算
平滑天然"往空旷处拉直"，处理不当会把路线拉向膨胀区边缘甚至障碍本身。工程上把安全作为硬项写进平滑目标——每一平滑步都做距离场查询，离障碍小于裕度即回退或加惩罚：
def smooth_step(pts, i, alpha=0.4, beta=0.3, dfield=None, margin=0.25): """一步梯度平滑：alpha 拉直，beta 均匀化，遇安全裕度回退""" p_prev, p_cur, p_next = pts[i-1], pts[i], pts[i+1] grad = alpha*( (p_prev + p_next)/2 - p_cur ) # 拉直项 grad += beta*( p_prev + p_next - 2*p_cur ) # 曲率项（二阶差分） cand = p_cur + 0.5*grad if dfield is not None and dfield.dist(cand) < margin: return p_cur # 侵入裕度：放弃本步 return cand
这段"梯度下降式平滑"是最小可用的优化平滑：迭代若干轮，路径肉眼可见地变顺，同时距离场守门。裕度取值直接引用 4.1 节的膨胀半径——两处的数字必须同源，否则平滑器会把路径推进规划器以为危险的区域。
时间参数化：给轨迹上表针
几何平滑完还没结束——控制器要的是"每个时刻的 q、q̇、q̈"。时间参数化的经典法是梯形速度曲线沿弧长分配（第 2.5 节的方法原样复用），进阶法是时间最优路径参数化（TOPP）：沿路径逐点求"当前速度下受曲率与力矩约束允许的最大速度"，得到一条速度剖面，保证在所有约束贴边的情况下最快走完。机械臂高速搬运的节拍优化，几乎都落在 TOPP 手里——同一几何路径， TOPP 能比朴素梯形分配快两成以上。
💡 关键直觉：平滑的本质是"把搜索器不懂的动力学知识补写进路径"。凡是约束（曲率、速度、力矩），要么在平滑时显式满足，要么在执行时被控制器付出代价——没有第三种结局。
本节要点
- 三步加工链：视线剪枝去冗余、B 样条给连续性、优化式平滑吃复杂约束。
- 平滑必须与安全联算：距离场守门、裕度与膨胀半径同源，顺滑不能换来擦墙。
- 剪枝与样条分别在"最优性"与"过点精度"上让步，这是有意为之的交换。
- 时间参数化把几何路径变成带表针的轨迹；追求节拍用 TOPP，贴约束跑出最快剖面。
曲率约束：差速底盘平滑的硬门槛
差速底盘的瞬时曲率上限由最小转弯半径决定（差速原地转可视为半径为零的特例，但高速下必须走大半径）。平滑若不显式约束曲率，B 样条会在拐点附近"自由发挥"，产出底盘跟不上的急弯。工程检查在平滑后追加一道曲率审计：沿曲线逐点算曲率，超限区段回炉（加大该处过渡圆角或降低该段速度限制）。速度与曲率要联算——侧向加速度（速度平方乘曲率）才是底盘真正感受的量，曲率审计因此常与速度剖面联查：弯道处自动降速，直线段放开跑。
非完整约束的平滑再思考
几何平滑（拉直、拟合）解决"形"，非完整约束还要解决"怎么走"。对差速底盘，把平滑曲线转成执行序列时要决定"边走边转"还是"先转后走"：前者顺滑但弯道占空间，后者动作碎但空间需求小。判断依据是过渡区宽度：宽（大于三倍车身长）用边走边转，窄用先转后走。对阿克曼底盘（汽车构型），倒车不可忽略——狭窄掉头场景需要 Reeds-Shepp 类的含倒车路径，平滑器必须支持速度符号的切换点处理。这些细节在开源导航栈里都有成熟实现，读它的参数文档比自己发明快得多。
问题：平滑后路径离障碍太近怎么调
先定位责任：是平滑目标里"贴近障碍惩罚"权重太小（路径主动贴边），还是距离场没参与平滑（平滑根本不知道障碍在哪）？后者是新手高发——只做了拉直与均匀化，没有安全项。修复后仍贴边，检查距离场的分辨率是否够（粗于栅格两倍的距离场会把"离墙 10 厘米"误判成 20 厘米）。最后的手段是局部回退：平滑前缓存原始折线，平滑结果的逐点安全距离低于阈值处混回原始点，牺牲局部平滑保全局安全。
补平滑与约束的优先级排序：工程轨迹优化的目标不是最平滑，而是约束层级下的可行。优先级从高到低：硬约束（障碍不碰撞、关节限位、速度加速度上限）必须满足；软约束（平滑度、离障碍距离、时间最优）按权重折衷。数值实现上，硬约束用惩罚项软化时要加障碍裕量（惩罚函数在边界附近才陡起，留出数值余量），否则优化器会在可行域边界抖动不收敛。另一个实战细节：优化初值用几何规划（A* 加样条）的结果而不是直线插值——初值落在可行域内，收敛速度与成功率都显著提升。诊断优化失败（迭代不收敛或结果违反约束）的顺序：先查约束冲突（起点终点间无可行走廊）再查权重失衡（两项软约束数量级差太大互相碾压）最后查初值质量——三步八成能定位。