摘要
本次更新为 AL-iLQR 求解器引入了完整的解析求导能力 ，覆盖动力学 Jacobian、所有代价函数的梯度/Hessian、以及全部约束（PolygonCollisionConstraint 除外）的 Jacobian。用户可通过 UI 界面实时切换数值差分与解析求导模式，便于对比验证。
性能提升
| 指标 | 数值差分（旧） | 解析求导（新） | 提升倍数 |
|---|---|---|---|
| 单次规划耗时 | ~150 ms | ~9.6 ms | 15.6× |
一、为什么快了这么多？
1.1 旧方案的瓶颈：中心差分的 O(n2)O(n^2)O(n2) 调用量
旧版对每个时间步的每个求导项都使用中心差分，以 n=4n=4n=4（状态维度）、m=2m=2m=2（控制维度）计算：
| 求导项 | 每步差分调用次数 |
|---|---|
| A=∂f/∂xA = \partial f/\partial xA=∂f/∂x | 2n=82n = 82n=8 次 NextState |
| B=∂f/∂uB = \partial f/\partial uB=∂f/∂u | 2m=42m = 42m=4 次 NextState |
| ℓxx\ell_{xx}ℓxx（Hessian） | 4n2=644n^2 = 644n2=64 次 StageCost |
| ℓuu\ell_{uu}ℓuu | 4m2=164m^2 = 164m2=16 次 StageCost |
| ℓux\ell_{ux}ℓux（交叉项） | 4nm=324nm = 324nm=32 次 StageCost |
| ℓx,ℓu\ell_x, \ell_uℓx,ℓu（梯度） | 2(n+m)=122(n+m) = 122(n+m)=12 次 StageCost |
每步合计 ：12 次动力学 + 124 次代价函数（每次代价函数内部还会遍历所有约束的 Evaluate）。80 步时域下，每次 iLQR 迭代约需 10,000+ 次函数调用。
1.2 新方案：零差分调用
解析求导直接通过闭式公式计算所有导数，无循环差分、无重复函数调用。特别是：
- 代价 Hessian ：从 4n2=644n^2 = 644n2=64 次函数调用 → 1 次矩阵外积运算
- 约束 AL 项 ：从隐式标量差分 → 显式 J⊤diag(μ) JJ^\top \operatorname{diag}(\mu)\, JJ⊤diag(μ)J Gauss-Newton 近似
- 动力学 Jacobian ：从 12 次
NextState→ 1 次三角函数计算
二、架构设计：可切换的求导模式
2.1 DerivativeMode 枚举
cpp
enum class DerivativeMode {
kFiniteDifference, // 保留原有中心差分（兜底/对照）
kAnalytical, // 解析求导（新增）
};在 ILQROptions 中新增 derivative_mode 字段，默认为 kFiniteDifference（向后兼容）。
2.2 三层可选接口
在 DynamicsModel、CostFunction、ConstraintFunction 三个基类中统一新增可选的解析求导虚方法：
动力学模型 --- DynamicsModel：
cpp
virtual bool HasAnalyticalJacobian() const { return false; }
virtual Matrix JacobianState(const Vector& state, const Vector& control, double dt) const;
virtual Matrix JacobianControl(const Vector& state, const Vector& control, double dt) const;代价函数 --- CostFunction：
cpp
virtual bool HasAnalyticalDerivatives() const { return false; }
virtual CostExpansion AnalyticalStageCostExpansion(const Vector& state, const Vector& control) const;
virtual std::pair<Vector, Matrix> AnalyticalTerminalCostExpansion(const Vector& state) const;约束函数 --- ConstraintFunction：
cpp
virtual bool HasAnalyticalJacobian() const { return false; }
virtual Matrix JacobianState(const Vector& state, const Vector& control) const;
virtual Matrix JacobianControl(const Vector& state, const Vector& control) const;2.3 ILQRSolver 自动分派
ILQRSolver 在计算导数时检查 derivative_mode 和对应组件的 HasAnalytical*() 标志：
- 若模式为
kAnalytical且 组件支持解析求导 → 调用解析方法 - 否则 → 回退到中心差分
这意味着即使某个约束（如 PolygonCollisionConstraint）没有实现解析 Jacobian，系统仍能正常工作------对该约束对应的 AL 代价项仍使用差分。
2.4 AL 罚项解析展开
AugmentedLagrangianKnotCost 直接利用约束的解析 Jacobian 组装 AL 项的梯度和 Hessian，避免了对标量 AL 代价的全维度差分：
不等式约束展开（Gauss-Newton 近似）：
∂φ∂x=Jx⊤max (0, λ+μ⊙c) \frac{\partial \varphi}{\partial x} = J_x^\top \max\!\bigl(0,\;\lambda + \mu \odot c\bigr) ∂x∂φ=Jx⊤max(0,λ+μ⊙c)
∂2φ∂x2≈Jx⊤diag(Iactive⊙μ) Jx \frac{\partial^2 \varphi}{\partial x^2} \approx J_x^\top \operatorname{diag}(I_{\text{active}} \odot \mu)\, J_x ∂x2∂2φ≈Jx⊤diag(Iactive⊙μ)Jx
2.5 UI 实时切换
在 ImGui 前端的 Planning Config 面板中新增 Derivative Mode 下拉框，用户可实时在 Finite Difference 和 Analytical 之间切换，立即对比求解耗时和轨迹质量。
三、已实现解析求导的组件清单
| 组件 | 解析求导 | 说明 |
|---|---|---|
| KinematicBicycleModel | A, B | sin/cos/tan 闭式 |
| QuadraticCost | 全部 | 二次型闭式 |
| LaneTrackingCost | 全部 | 外积分解 |
| GuidanceTrackingCost | 全部 | 含纵向投影终端 |
| ControlRateCost | 全部 | 仅控制量项 |
| ControlBoxConstraint | Jacobian | 常数矩阵 |
| SpeedLimitConstraint | Jacobian | 一个非零元素 |
| RoadBoundaryConstraint | Jacobian | 参考线航向 |
| CircularObstacleConstraint | Jacobian | 距离对位置的偏导 |
| MultiCircleRoadBoundary | Jacobian | 刚体变换链式法则 |
| MultiCircleVehicleObs | Jacobian | 刚体变换链式法则 |
| LSEPolygonObstacle | Jacobian | softmax 梯度 |
| TerminalGoalConstraint | Jacobian | 单位矩阵 |
| PolygonCollisionConstraint | 回退差分 | GJK 距离不可微 |