2. 数学基础

轨迹规划的本质，是把“机器人/车辆/飞行器该怎么动”这一物理问题，翻译成可计算、可优化、可验证的数学问题。速度约束与动力学约束之所以难，往往不是算法本身复杂，而是数学工具没有选对、没有用透。本节把后续章节反复用到的五类数学工具集中梳理一遍，并说明它们在轨迹规划中各自承担什么角色。

一、核心原理：五类工具在轨迹规划中的分工

轨迹规划中，一条轨迹通常写成关于时间 t 的函数 q(t)，其一阶导 q̇(t) 是速度，二阶导 q̈(t) 是加速度，三阶导 q⃛(t) 是加加速度（jerk）。速度约束限制 q̇，动力学约束限制 q̈ 与力矩/推力，平滑性约束则常落在 q⃛ 上。理解这条“导数链”，就理解了为什么微积分是轨迹规划的母语。

向量与矩阵把多自由度系统统一表达：位姿是向量，坐标系变换是矩阵，雅可比矩阵把关节速度映射为末端速度。线性代数则进一步提供求解手段：最小二乘解超定约束，特征值分解判断二次型正定性，QR/SVD 处理病态与奇异。微分方程描述系统在约束下的演化，概率统计则回答“在噪声与扰动下，这条轨迹有多可靠”。

二、方法流程：从约束到可执行轨迹的数学步骤

把数学工具落到工程上，通常遵循以下流程。每一步都对应一类数学操作，缺一不可。

1. 建模：用向量表示位姿 q ∈ Rⁿ，用矩阵表示坐标系变换 T ∈ SE(3)，写出运动学关系 x = f(q)。
2. 求导：对时间求导得到速度与加速度关系 ẋ = J(q)q̇，其中 J 为雅可比矩阵。
3. 约束表达：把速度上限、力矩上限写成不等式 |q̇| ≤ q̇_max、|τ| ≤ τ_max。
4. 离散化：将连续轨迹在时间网格上采样，转化为有限维优化问题。
5. 求解：用线性代数方法（最小二乘、QR、SVD）或二次规划求解。
6. 评估：用概率统计方法分析噪声传播与轨迹鲁棒性。

# 以五次多项式轨迹为例，展示导数链与约束检查
q(t)   = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
q̇(t)  = a1 + 2*a2*t + 3*a3*t^2 + 4*a4*t^3 + 5*a5*t^4
q̈(t)  = 2*a2 + 6*a3*t + 12*a4*t^2 + 20*a5*t^3
q⃛(t)  = 6*a3 + 24*a4*t + 60*a5*t^2

# 约束检查
assert max(|q̇(t)|) <= v_max
assert max(|q̈(t)|) <= a_max
assert max(|q⃛(t)|) <= j_max

三、案例/场景：五类工具在真实任务中的协同

假设一台六轴机械臂需要从点 A 运动到点 B，同时末端速度不超过 0.5 m/s，关节力矩不超过额定值，且路径要避开一个圆柱形障碍物。这个任务中，五类工具是这样协同的：

• 向量与矩阵：用齐次变换矩阵描述末端位姿，用雅可比矩阵把末端速度约束映射为关节速度约束。
• 微积分：用五次多项式或 B 样条构造平滑轨迹，保证速度、加速度连续，jerk 有界。
• 微分方程：用机械臂动力学方程 M(q)q̈ + C(q,q̇)q̇ + G(q) = τ 计算所需力矩，判断是否超限。
• 线性代数：把多约束优化写成二次规划，用 QR 分解或内点法求解。
• 概率统计：考虑关节编码器噪声与负载扰动，用协方差传播评估末端定位误差。

四、误区/复盘：常见数学使用错误

在工程实践中，以下误区出现频率最高，值得逐条对照排查。