手写求导公式，让轨迹优化性能飞升，150ms变成9ms

摘要

本次更新为 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 距离不可微