第227篇 最小化Jerk轨迹——让运动丝滑的数学方法

上一篇讲了轨迹优化的框架和评价标准。今天深入一个具体问题怎么用数学方法让运动丝滑。答案是——最小化Jerk。Jerk是加速度的时间导数jerk da/dt d³x/dt³。人类的体感对jerk非常敏感——你坐车时觉得顿挫不是加速度大而是加速度变化快jerk大。老司机和新手的区别就在这里老司机刹车时缓慢加力jerk小新手一脚踩死jerk大全车人前仰后合。自动驾驶和机器人领域有个共识最小化jerk的轨迹是人类最舒适的运动方式。这个结论有实验支撑——MIT的Flash和Hogan在1985年就证明了人类手臂的点到点运动近似于最小jerk轨迹。给个直观对比电梯启动时如果加速度瞬间从0跳到1m/s²jerk无穷大你会觉得被猛推一下。好的电梯控制是加速度缓慢增加到1m/s²比如用0.5秒完成jerk控制在2m/s³以内——你几乎感觉不到电梯在动。这就是jerk最小化的物理意义。一、最小化Jerk的数学推导问题定义给定起点位置x0和终点位置xf以及起止点的速度和加速度通常为0找一条轨迹x(t)使得总jerk的平方积分最小。# 最小化Jerk的优化问题 # minimize: integral(jerk^2, dt) from t0 to tT # 约束: x(0)x0, x(T)xf # v(0)v0, v(T)vf # a(0)a0, a(T)af这个问题的解析解是五次多项式x(t) a0 a1t a2t² a3t³ a4t⁴ a5*t⁵。为什么是五次因为jerk是x的三阶导数jerk²的积分是六次多项式的积分。要最小化六次多项式的积分x(t)需要是五次多项式——六次以上的项只会增大积分值。六个系数a0-a5由六个边界条件唯一确定起点和终点的位置、速度、加速度各提供一个方程。解一个6×6的线性方程组就完事了——不需要迭代优化计算量极小。举个具体例子起点x0终点x10m时间T2s起止速度和加速度都为0。解出来的系数a00, a10, a20, a350, a4-75, a530归一化后的系数。最大速度出现在t1s值为5m/s最大加速度出现在t0.5s和t1.5s值为7.5m/s²。整条轨迹的jerk是一个常数——这就是最小化jerk的特性jerk恒定。对比一下梯形速度规划加速阶段加速度恒定jerk0但在加速到匀速的切换点加速度突变jerk无穷大。最小化jerk没有这个问题——加速度从零平滑增加到峰值再平滑减小到零。二、多段最小化Jerk轨迹单段五次多项式只能处理起点到终点的简单运动。实际轨迹通常有多个waypoint——需要多段五次多项式拼接。拼接的关键是连续性条件相邻两段在连接点处的位置、速度、加速度必须连续。假设有N个waypoint就有N-1段轨迹每段6个系数总共6(N-1)个未知数。约束条件包括每段的起点和终点位置2(N-1)个连接点处速度连续N-2个连接点处加速度连续N-2个起止点的速度和加速度4个# 多段最小化Jerk的矩阵求解 # 每段: x_i(t) a0 a1*t a2*t^2 a3*t^3 a4*t^4 a5*t^5 # 把所有段的系数排成向量 c [c1, c2, ..., c_{N-1}] # 约束写成 Ac b 的形式 # 目标函数写成 c^T Q c 的形式 # 求解: 带等式约束的二次规划 # min c^T Q c s.t. Ac b这个二次规划问题有闭式解——用KKT条件或者直接消元法。计算量与waypoint数量成正比通常N50求解时间在微秒级别。三、工程实现中的细节实际使用最小化Jerk轨迹时有几个细节需要注意。时间分配每段轨迹的时间Ti怎么确定这是个关键问题。如果Ti太小速度和加速度可能超过物理限制如果Ti太大执行时间太长。工程上通常用启发式方法Ti与路径长度成正比比例系数由最大速度决定。比如某段路径长度5m最大速度2m/s则Ti至少2.5s。再乘以安全系数1.2-1.5留出加速度峰值的余量。有一个更精确的时间分配方法——Time-Optimal Path ParameterizationTOPP。TOPP在给定路径的前提下算出满足速度/加速度约束的最短时间分配。后面第229篇会详细讲。速度和加速度约束最小化Jerk的解析解不保证速度和加速度在物理限制以内。算出来的轨迹可能在中间段超速。解决方案算完轨迹后检查速度/加速度峰值如果超限就增大时间Ti重新计算。这个过程通常迭代2-3次就收敛。多维解耦对于二维或三维运动每个维度独立做最小化Jerk。x方向、y方向、z方向各一条五次多项式共用同一套时间分配。这种解耦方法在无人机轨迹规划中用得最多——CMU和ETH Zurich的无人机系统都是这么做的。# 多维最小化Jerk def min_jerk_trajectory_2d(waypoints, times): x_traj min_jerk_1d([wp[0] for wp in waypoints], times) y_traj min_jerk_1d([wp[1] for wp in waypoints], times) return Trajectory2D(x_traj, y_traj)四、最小化Jerk vs 最小化Snap最小化Jerk是五次多项式最小化Snap加加速度的导数d⁴x/dt⁴是七次多项式。哪个更好最小化Snap的轨迹比最小化Jerk更平滑——加速度变化更缓慢。但代价是计算更复杂七次多项式8个系数且中间点的速度/加速度峰值可能更大。经验法则无人机用最小化Snap四旋翼对加速度变化敏感地面车辆和机械臂用最小化Jerk。Penn的Kumar实验室和ETH的Scaramuzza团队在无人机轨迹规划方面做了大量工作他们的开源代码值得参考。说白了最小化Snap和最小化Jerk的本质区别在于Jerk最小化让加速度变化均匀jerk恒定Snap最小化让加速度的变化更缓慢snap恒定。如果你的执行器对加速度变化率敏感比如电机力矩带宽有限用Snap最小化更好。五、面试实战Q为什么最小化Jerk的轨迹是五次多项式AJerk是位置的三阶导数。最小化jerk²的积分变分法推导出最优轨迹的六阶导数为零——所以是五次多项式。六次以上的系数都为零。Q多段最小化Jerk怎么保证连续性A在连接点处强制位置、速度、加速度连续。这转化为等式约束和最小化Jerk的目标函数一起构成带等式约束的二次规划问题有闭式解。Q最小化Jerk轨迹的缺点是什么A两个主要问题。一是不保证速度/加速度在物理限制内——需要后处理检查。二是对时间分配敏感——时间分配不好轨迹质量会很差。工程上通常用迭代方式调整时间分配直到满足约束。Q你在项目中用过最小化Jerk吗A用过。做机械臂的点到点运动规划时用的最小化Jerk。给定起止关节角和零速度零加速度边界条件解五次多项式得到关节角的时间序列。轨迹平滑关节力矩变化小执行时间约2-3秒。比梯形速度规划好得多——梯形规划在加减速切换点有jerk跳变。小结最小化Jerk轨迹是五次多项式由起止点的位置、速度、加速度六个边界条件唯一确定。多段拼接时加连续性约束转化为二次规划问题求解。计算快微秒级轨迹平滑是机器人和无人机轨迹规划的基础方法。面试中如果被问到最小化Jerk核心说清楚五次多项式的原因、多段拼接的连续性处理、以及速度/加速度约束的后处理方法。下一篇讲B样条曲线——用参数化方式表示平滑轨迹的另一种方法。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第226篇 轨迹优化基础——什么是好的轨迹下一篇预告第228篇 B样条曲线——平滑轨迹的参数化表示有任何问题欢迎评论区留言我会尽量回复。