2. 数学基础

轨迹平滑优化的本质，是在满足运动约束的前提下，对路径的几何形态与时间分配进行数学建模与求解。无论是样条插值还是贝塞尔曲线构造，其底层都依赖向量运算、参数曲线理论以及连续性、曲率、挠率等微分几何概念。本章将系统梳理这些数学工具的定义、几何意义以及在轨迹优化中的工程映射，为后续章节的算法推导奠定严格基础。

2.1 向量：轨迹优化的基本语言

向量是描述轨迹最自然的数学工具。在三维空间中，一个位置点 P 可以用坐标向量 (x, y, z) 表示，而轨迹上的运动则体现为向量的连续变化。在轨迹平滑优化中，向量运算主要服务于三个目的：几何关系的表达、运动学量的计算以及优化目标的构建。

核心运算及其几何意义：

• 加法与数乘：用于线性插值、凸组合（贝塞尔曲线的核心操作）以及加权平均。
• 点积（内积）：a·b = |a||b|cosθ，用于计算投影、判断正交性、计算角度约束。
• 叉积（外积）：a×b 得到垂直于 a、b 所在平面的向量，用于计算法向量、判断转向方向。
• 模长与单位化：|a| = √(a·a)，单位向量 â = a/|a|，用于归一化方向。

2.2 参数曲线：从几何路径到运动规律

参数曲线将轨迹表示为参数 t（通常为时间或弧长）的函数：P(t) = (x(t), y(t), z(t))。这种表示方式将几何形状与运动规律解耦——改变参数化方式不会改变轨迹的几何形状，但会改变沿轨迹的运动速度。

关键概念：

概念	数学定义	几何/物理意义	轨迹优化中的用途
切向量	P'(t) = dP/dt	曲线在该点的瞬时运动方向	速度方向约束、C1连续性判断
弧长参数化	s(t) = ∫|P'(τ)|dτ	以曲线自身长度为参数，消除速度影响	几何形状分析、曲率计算
速度	v(t) = |P'(t)|	沿轨迹运动的快慢	时间最优规划、速度约束
加速度	a(t) = |P''(t)|	速度变化的快慢	动力学可行性、冲击限制

参数化的选择策略：在轨迹平滑优化中，通常优先使用弧长参数化进行几何分析（如曲率计算），再映射回时间参数化进行运动规划。这是因为弧长参数化下的导数具有纯几何意义，不受速度影响。

2.3 连续性（C0/C1/C2）：平滑度的分级标准

连续性描述的是两条曲线段在连接点处的光滑程度。在轨迹优化中，连续性直接决定了运动执行时的平稳性——不连续的切向量会导致速度突变，不连续的曲率会导致加速度突变。

各级连续性的严格定义：

1. C0（位置连续）：两段曲线在连接点处位置相同，即 P₁(t₀) = P₂(t₀)。这是最基本的要求，否则轨迹会出现断点。
2. C1（切向量连续）：在C0基础上，一阶导数连续，即 P₁'(t₀) = P₂'(t₀)。这保证了速度方向无突变，但速度大小可能突变。
3. C2（曲率连续）：在C1基础上，二阶导数连续，即 P₁''(t₀) = P₂''(t₀)。这保证了加速度连续，运动更加平稳。

工程中的连续性选择：

• AGV/移动机器人路径规划：通常要求C1，保证转向不突变。
• 机械臂轨迹规划：要求C2，避免加速度突变导致振动。
• 高速加工/3D打印：要求C2甚至C3，减少机床冲击。

2.4 曲率与挠率：轨迹的弯曲与扭转

曲率 κ 和挠率 τ 是描述空间曲线几何形态的两个核心不变量。曲率衡量曲线偏离直线的程度，挠率衡量曲线偏离平面曲线的程度。

数学定义（弧长参数化下）：

曲率：κ(s) = |P''(s)| = |dT/ds|
挠率：τ(s) = -(dB/ds)·N

其中 T = P'(s)（单位切向量）
      N = T'/|T'|（主法向量）
      B = T × N（副法向量）

几何意义：

量	几何意义	轨迹优化中的影响
曲率 κ	曲线弯曲的剧烈程度，κ=0为直线，κ=1/R（R为曲率半径）	限制最小转弯半径、影响向心加速度
挠率 τ	曲线偏离平面的程度，τ=0为平面曲线	影响空间轨迹的平滑性、减少不必要的扭转

2.5 案例拆解：AGV路径平滑中的数学应用

场景：某工厂AGV需要沿一条由离散路径点组成的路线行驶，原始路径由直线段连接，在拐角处存在尖角。AGV最大转向角速度有限，且货物对加速度敏感。

初始问题：原始路径在拐角处仅满足C0连续性，AGV到达拐角时必须停车转向，严重影响效率；若强行通过，则会产生巨大加速度冲击。

分析过程：

1. 计算原始路径的连续性：直线段连接处为C0，切向量方向突变。
2. 计算拐角处的曲率：理想尖角处 κ→∞，实际表现为曲率半径接近0。
3. 确定约束：AGV最小转弯半径 R_min = 0.8m，最大加速度 a_max = 2m/s²。

解决方案：采用三次样条插值，在拐角处构造C2连续的过渡曲线。

# 伪代码示例：三次样条构造
# 输入：路径点 P0, P1, P2, ..., Pn
# 输出：分段三次多项式 S(t)

# 约束条件：
# S_i(t_i) = P_i          (位置连续)
# S_i'(t_i) = S_{i+1}'(t_i)  (切向量连续)
# S_i''(t_i) = S_{i+1}''(t_i) (曲率连续)

结果对比：

指标	原始折线路径	三次样条平滑后
连续性	C0	C2
最大曲率	∞（尖角）	0.8 m⁻¹（满足约束）
最大加速度	5.2 m/s²（超限）	1.8 m/s²（满足约束）
通过时间	12.5s（含停车）	8.3s（无需停车）

复盘经验：连续性等级的选择必须与执行器的物理能力匹配。盲目追求C3以上连续性会增加计算复杂度，但实际收益有限；而低于C1的路径在大多数运动控制场景中不可用。

2.6 常见误区与总结

误区一：混淆参数连续性与几何连续性。在工程中，有时两条曲线在连接点处切线方向相同但速度大小不同（G1连续），这会导致速度突变。必须检查参数连续性，而不仅仅是几何外观。

误区二：忽视挠率的影响。在三维轨迹优化中，仅关注曲率而忽略挠率，可能导致轨迹在空间中出现不必要的扭转，增加执行机构的复杂度。

误区三：将曲率与曲率半径混淆。曲率 κ 是曲率半径 R 的倒数（κ=1/R），两者是倒数关系，在优化目标中需要明确使用哪一个。