4. 轨迹优化问题建模
轨迹优化不是“把一条曲线画得好看”，而是把一个连续时间上的运动规划问题，写成计算机可以求解的数学优化问题。建模质量直接决定求解器能否收敛、解是否物理可行、以及实时性是否达标。本节围绕三个核心环节展开：目标函数设计、约束条件建模、问题转化与归一化。
一、目标函数设计：把工程偏好写成代价
目标函数是优化问题的“指挥棒”。在轨迹优化中，常见的目标函数包括时间最优、能量最优、平滑性最优，以及它们的加权组合。
- 时间最优：最小化总运动时间 \(T\)，或最小化各段轨迹时间之和。适合抓取、避障穿越等对节拍敏感的场景。缺点是容易产生 bang-bang 控制，加速度不连续，对执行器冲击大。
- 能量最优：最小化控制输入平方积分 \(\int u^2 dt\)，或最小化关节力矩平方和。适合续航敏感、发热受限的机器人系统。缺点是可能产生“慢而软”的轨迹，节拍不理想。
- 平滑性最优：最小化加加速度（jerk）平方积分 \(\int \dddot{q}^2 dt\)，或最小化加速度变化率。适合人机协作、视觉跟踪、精密装配等对振动敏感的场景。
- 加权组合：实际工程中通常写成 \(J = w_1 T + w_2 \int u^2 dt + w_3 \int \dddot{q}^2 dt\)，通过权重调节偏好。
目标函数设计的关键不是“选哪个”，而是“权重怎么定”。权重过大或过小都会导致求解器在时间、能量、平滑之间失衡。建议先用归一化后的量纲做初值，再根据仿真结果微调。
二、约束条件：可行域的边界
约束条件定义了轨迹必须满足的物理与任务边界。轨迹优化中常见的约束可分为四类：
| 约束类型 | 数学形式 | 典型场景 |
|---|---|---|
| 动力学约束 | \(\dot{x}=f(x,u)\) | 机械臂、无人机、移动机器人 |
| 避障约束 | \(g(x) \ge 0\) 或距离函数 | 狭窄通道、多障碍环境 |
| 边界条件 | \(x(0)=x_0,\ x(T)=x_f\) | 起点终点固定、速度连续 |
| 执行器限制 | \(u_{\min} \le u \le u_{\max}\) | 力矩、速度、加速度饱和 |
动力学约束是轨迹优化的“骨架”。对于机械臂，通常写成 \(M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau\)；对于无人机，写成位置、速度、加速度与推力、姿态的耦合关系。避障约束常用符号距离函数或凸多边形分离条件表达，边界条件则决定轨迹是否“接得上”。执行器限制是最容易被忽略但最致命的一类约束：如果优化出的轨迹要求力矩超过电机峰值，再漂亮的曲线也无法执行。
三、问题转化与归一化：让求解器“吃得下”
原始轨迹优化问题通常是连续时间、非线性、非凸的，直接求解非常困难。工程上通常做三步转化：
- 离散化：把连续时间 \([0,T]\) 分成 \(N\) 段，用配点法、打靶法或直接转录法，把微分方程转化为代数约束。
- 归一化：对时间、位置、速度、控制量做尺度变换，使各变量量级接近 1。例如时间除以 \(T\)，位置除以工作空间尺度，力矩除以峰值力矩。
- 凸化或松弛：对非凸避障约束做凸近似，或引入松弛变量，把问题转化为二次规划或序列凸规划。
归一化的意义常被低估。如果时间变量在 \(10^{-3}\) 量级、位置在 \(10^0\) 量级、力矩在 \(10^2\) 量级，求解器的数值条件会非常差，收敛速度急剧下降。归一化后，各变量在同一量级，求解器迭代更稳定。
# 归一化示例（伪代码）
T_norm = T / T_ref # 时间归一化
q_norm = (q - q_min) / (q_max - q_min) # 位置归一化
u_norm = u / u_max # 控制量归一化
J = w1 * T_norm + w2 * sum(u_norm**2) + w3 * sum(jerk_norm**2)
四、案例：机械臂抓取轨迹建模
以六轴机械臂从 A 点抓取物体到 B 点为例。目标函数选为时间与平滑性的加权组合：\(J = 0.6T + 0.4\int \dddot{q}^2 dt\)。约束包括：关节角度限位、关节速度限位、关节力矩限位、末端执行器与桌面及障碍物的距离约束、起点和终点的位姿约束。离散化采用 50 个配点，时间归一化到 \([0,1]\)，力矩除以峰值力矩。求解后得到一条平滑、无碰撞、力矩不饱和的轨迹。
如果去掉平滑性项，轨迹会出现明显抖动；如果去掉力矩约束，轨迹在加速段要求力矩超过峰值 30%，实机无法执行。这说明目标函数与约束必须同时考虑，不能只追求某一项指标。
建模完成后，建议先用小规模离散点做快速验证，确认目标函数与约束的量级合理，再逐步增加配点数和约束复杂度。实时求解工程中，建模阶段多花十分钟做归一化和约束检查，往往能节省现场调试数小时。