CILQR算法数学公式总结

目录

• 概述
• 函数路径说明表
• 车辆动力学模型
• 代价函数
• 约束处理
• 后向传播算法
• 前向传播算法
• 线搜索
• 约束导数计算
• 收敛判据
• 阻尼因子调整
• 轨迹质量指标
• 主求解循环
• 总结与展望
• 参考文献
• 附录：符号说明

---

函数路径说明表

为了帮助读者更好地理解代码结构，以下是本文档中涉及的主要函数及其在代码中的位置：

核心求解器函数 (src/cilqr_solver.cpp)

• solve() - 主求解循环
• backward_pass() - 后向传播算法
• forward_pass() - 前向传播算法
• iter_step() - 单步迭代
• line_search() - 线搜索
• get_total_cost() - 计算总代价
• get_cost_derivatives() - 计算代价导数
• check_convergence() - 收敛检查
• adjust_damping_factor() - 阻尼因子调整
• initialize_trajectory() - 轨迹初始化

约束处理函数 (src/constraint_handler.cpp)

• get_dynamics_constraints() - 动力学约束
• get_road_boundary_constraints() - 道路边界约束
• get_obstacle_avoidance_constraints() - 障碍物避让约束
• calculate_lateral_deviation() - 横向偏差计算
• calculate_obstacle_safety_margin() - 障碍物安全裕度

代价函数函数 (src/cost_functions.cpp)

• barrier_function() - 障碍函数
• get_barrier_cost() - 障碍函数代价
• augmented_lagrangian_cost() - 增广拉格朗日代价
• get_alm_cost() - 增广拉格朗日总代价
• update_lagrange_multipliers() - 拉格朗日乘子更新

工具函数 (src/utils/kinematic_model.cpp)

• kinematic_propagate() - 运动学模型前向传播
• get_state_jacobian() - 状态雅可比矩阵
• get_control_jacobian() - 控制雅可比矩阵

评估函数 (src/trajectory_evaluator.cpp)

• calculate_trajectory_metrics() - 轨迹质量指标计算

---

概述

CILQR（Constrained Iterative Linear Quadratic Regulator）是一种基于iLQR的约束优化算法，用于解决带约束的非线性最优控制问题。该算法结合了迭代线性二次调节器（iLQR）的高效性和约束处理方法的鲁棒性，特别适用于自动驾驶车辆轨迹规划等需要满足多种约束条件的场景。

算法核心思想

CILQR算法的核心思想是通过迭代的方式，将非线性最优控制问题线性化，然后利用线性二次调节器的理论求解。在每次迭代中，算法会：

1. 在当前轨迹点附近进行线性化
2. 求解线性化后的二次规划问题
3. 通过线搜索确定最优步长
4. 更新轨迹并检查收敛性

主要优势

• 高效性：相比直接求解非线性优化问题，计算效率更高
• 鲁棒性：能够处理多种类型的约束条件
• 可扩展性：适用于不同的车辆模型和约束类型
• 实时性：适合在线轨迹规划应用

本文档将详细介绍算法中各个函数的数学公式和实现细节。

1. 车辆动力学模型

对应源代码： src/utils/kinematic_model.cpp 中的 utils::kinematic_propagate() 和 get_kinematic_model_derivatives() 函数

车辆动力学模型是CILQR算法的基础，它描述了车辆状态如何随时间和控制输入变化。在自动驾驶应用中，我们通常使用简化的运动学模型来平衡计算效率和模型精度。

1.1 运动学自行车模型

运动学自行车模型是一种简化的车辆模型，它将车辆简化为一个具有前后轴的刚体。这个模型假设：

• 车辆在水平面上运动（忽略俯仰和侧倾）
• 轮胎与地面之间有足够的摩擦力
• 车辆质量集中在几何中心
• 转向时前后轮保持平行

这种简化使得模型更容易处理，同时仍然能够捕捉车辆运动的主要特征。
$$x_{k+1}=f(x_k,u_k)=x_k+dt\cdot \left[\begin{array}{c}{v}_k\cdot \cos ({\psi }_k)\\ v_k\cdot \sin ({\psi }_k)\\ a_k\\ \frac{v_k\cdot \tan ({\delta }_k)}{L}\end{array}\right]$$

其中：

• $x_k=[x,y,v,\psi {]}^T$：状态向量（位置、速度、朝向）
• $u_k=[a,\delta {]}^T$：控制向量（加速度、前轮转向角）
• $L$：车辆轴距
• $dt$：时间步长

核心代码展示：

// 位于 src/utils/kinematic_model.cpp
// 运动学模型前向传播
Eigen::Vector4d kinematic_propagate(const Eigen::Vector4d& x, const Eigen::Vector2d& u, double dt) {
    double v = x(2);
    double psi = x(3);
    double a = u(0);
    double delta = u(1);
    
    Eigen::Vector4d x_next;
    x_next(0) = x(0) + dt * v * cos(psi);           // x位置
    x_next(1) = x(1) + dt * v * sin(psi);           // y位置
    x_next(2) = x(2) + dt * a;                      // 速度
    x_next(3) = x(3) + dt * v * tan(delta) / L;     // 朝向角
    
    return x_next;
}

1.2 雅可比矩阵

雅可比矩阵是CILQR算法中的关键概念，它描述了系统对状态和控制的敏感性。在算法的线性化过程中，我们需要计算这些导数来构建局部线性近似。

状态雅可比矩阵

状态雅可比矩阵 $\frac{\partial f}{\partial x}$ 描述了状态变化对当前状态的敏感性。这个矩阵告诉我们：

• 位置变化如何影响下一时刻的位置
• 速度变化如何影响位置和朝向
• 朝向变化如何影响位置

矩阵中的每个元素都有明确的物理意义，例如 $\frac{\partial x_{k+1}}{\partial v_k}=dt\cdot \cos ({\psi }_k)$ 表示速度变化对X位置的影响。
$$\frac{\partial f}{\partial x}=\left[\begin{array}{cccc}1& 0& dt\cdot \cos (\psi )& -dt\cdot v\cdot \sin (\psi )\\ 0& 1& dt\cdot \sin (\psi )& dt\cdot v\cdot \cos (\psi )\\ 0& 0& 1& 0\\ 0& 0& \frac{\tan (\delta )}{L}& 1\end{array}\right]$$

控制雅可比矩阵

控制雅可比矩阵 $\frac{\partial f}{\partial u}$ 描述了状态变化对控制输入的敏感性。这个矩阵告诉我们：

• 加速度如何影响速度
• 转向角如何影响朝向角

值得注意的是，加速度只影响速度，而转向角只影响朝向角，这反映了运动学模型的简化特性。
$$\frac{\partial f}{\partial u}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ dt& 0\\ 0& \frac{dt\cdot v}{L\cdot {\cos }^2(\delta )}\end{array}\right]$$

核心代码展示：

// 位于 src/utils/kinematic_model.cpp
// 计算状态雅可比矩阵
Eigen::Matrix4d get_state_jacobian(const Eigen::Vector4d& x, const Eigen::Vector2d& u, double dt) {
    double v = x(2);
    double psi = x(3);
    double delta = u(1);
    
    Eigen::Matrix4d A = Eigen::Matrix4d::Identity();
    A(0, 2) = dt * cos(psi);
    A(0, 3) = -dt * v * sin(psi);
    A(1, 2) = dt * sin(psi);
    A(1, 3) = dt * v * cos(psi);
    A(3, 2) = tan(delta) / L;
    
    return A;
}

// 位于 src/utils/kinematic_model.cpp
// 计算控制雅可比矩阵
Eigen::Matrix<double, 4, 2> get_control_jacobian(const Eigen::Vector4d& x, const Eigen::Vector2d& u, double dt) {
    double v = x(2);
    double delta = u(1);
    
    Eigen::Matrix<double, 4, 2> B = Eigen::Matrix<double, 4, 2>::Zero();
    B(2, 0) = dt;  // 加速度对速度的影响
    B(3, 1) = dt * v / (L * cos(delta) * cos(delta));  // 转向角对朝向的影响
    
    return B;
}

2. 代价函数

对应源代码： src/cilqr_solver.cpp 中的 get_total_cost() 函数

代价函数是CILQR算法的核心，它定义了我们要优化的目标。在自动驾驶轨迹规划中，我们希望车辆能够：

• 跟踪参考轨迹
• 保持平滑的控制输入
• 满足各种安全约束

代价函数通常由多个部分组成，每个部分对应一个优化目标。

2.1 主要目标函数

主要目标函数包含两个关键部分：状态跟踪误差和控制输入代价。

状态跟踪误差：衡量车辆实际状态与参考状态之间的差异。我们希望车辆能够精确地跟踪预定义的参考轨迹，这通过状态权重矩阵 $Q$ 来调节不同状态分量的重要性。

控制输入代价：惩罚过大的控制输入，确保轨迹的平滑性。过大的加速度或转向角会导致乘客不适，也可能超出车辆物理限制。
$$J_{prime}=\sum _{k=0}^N\left[(x_k-x_{ref,k}{)}^{T}Q(x_k-x_{ref,k})\right]+\sum _{k=0}^{N-1}\left[u_k^{T}R{u}_k\right]$$

其中：

• $Q=\text{diag}([w_{pos},w_{pos},w_{vel},w_{yaw}])$：状态权重矩阵
• $R=\text{diag}([w_{acc},w_{stl}])$：控制权重矩阵
• $x_{ref,k}$：参考状态

核心代码展示：

// 位于 src/cilqr_solver.cpp
double get_total_cost(const std::vector<Eigen::Vector4d>& states, 
                     const std::vector<Eigen::Vector2d>& controls,
                     const std::vector<Eigen::Vector4d>& ref_states) {
    double cost = 0.0;
    
    // 状态误差代价
    for (size_t k = 0; k < states.size(); ++k) {
        Eigen::Vector4d error = states[k] - ref_states[k];
        cost += error.transpose() * state_weight * error;
    }
    
    // 控制输入代价
    for (size_t k = 0; k < controls.size(); ++k) {
        cost += controls[k].transpose() * ctrl_weight * controls[k];
    }
    
    return cost;
}

2.2 约束惩罚项

在实际应用中，车辆必须满足各种物理和安全约束。CILQR算法通过将约束转换为惩罚项来处理这些限制，主要有两种方法：Barrier方法和增广拉格朗日方法（ALM）。

2.2.1 Barrier方法

Barrier方法是一种经典的约束处理方法，它通过指数函数将约束违反转换为惩罚。当约束被违反时（$c>0$），惩罚会指数增长，从而"推开"解远离不可行区域。

工作原理：Barrier函数在可行区域内（$c\le 0$）不产生惩罚，在不可行区域（$c>0$）产生快速增长的惩罚。参数 $q_1$ 和 $q_2$ 控制惩罚的强度和增长速率。

优势：能够有效防止解进入不可行区域，适合处理硬约束。
劣势：当解接近约束边界时，梯度可能变得很大，导致数值不稳定。
$$B(c)=\{\begin{array}{ll}\frac{\exp (q_2\cdot c)}{q_1}& \text{当 }c>0\\ 0& \text{当 }c\le 0\end{array}$$

总约束惩罚：
$$J_{barrier}=\sum _{k=1}^N\left[\sum _{i}B(c_{i,k})\right]$$

核心代码展示：

// 位于 src/cost_functions.cpp
double barrier_function(double constraint_value, double q1, double q2) {
    if (constraint_value > 0) {
        return exp(q2 * constraint_value) / q1;
    }
    return 0.0;
}

// 位于 src/cost_functions.cpp
double get_barrier_cost(const std::vector<double>& constraints) {
    double barrier_cost = 0.0;
    for (double c : constraints) {
        barrier_cost += barrier_function(c, obstacle_exp_q1, obstacle_exp_q2);
    }
    return barrier_cost;
}

2.2.2 增广拉格朗日方法（ALM）

增广拉格朗日方法是一种更先进的约束处理方法，它结合了拉格朗日乘子和二次惩罚项的优点。

工作原理：ALM通过迭代更新拉格朗日乘子 $\mu$ 和惩罚因子 $\rho$ 来逐步满足约束。当约束被违反时，拉格朗日乘子会增加，从而产生更强的惩罚；当约束满足时，惩罚会减少。

参数说明：

• $\mu$：拉格朗日乘子，表示对约束违反的"价格"
• $\rho$：惩罚因子，控制惩罚的强度

优势：相比Barrier方法，ALM具有更好的数值稳定性，能够处理更复杂的约束系统。
劣势：需要额外的参数调整和迭代过程。

核心代码展示：

// 位于 src/cost_functions.cpp
double augmented_lagrangian_cost(double constraint_value, double mu, double rho) {
    double term = constraint_value + mu / rho;
    if (term > 0) {
        return 0.5 * rho * term * term;
    }
    return 0.0;
}

// 位于 src/cost_functions.cpp
double get_alm_cost(const std::vector<double>& constraints, 
                   const std::vector<double>& multipliers) {
    double alm_cost = 0.0;
    for (size_t i = 0; i < constraints.size(); ++i) {
        alm_cost += augmented_lagrangian_cost(constraints[i], multipliers[i], alm_rho);
    }
    return alm_cost;
}

2.3 总代价函数

$$J_{total}=J_{prime}+J_{barrier}$$

核心代码展示：

// 位于 src/cilqr_solver.cpp
double get_total_cost_with_constraints(const std::vector<Eigen::Vector4d>& states,
                                     const std::vector<Eigen::Vector2d>& controls,
                                     const std::vector<Eigen::Vector4d>& ref_states) {
    // 主要目标函数代价
    double primary_cost = get_total_cost(states, controls, ref_states);
    
    // 约束惩罚代价
    double constraint_cost = 0.0;
    if (solve_type == SolveType::BARRIER) {
        constraint_cost = get_barrier_cost(constraints);
    } else if (solve_type == SolveType::ALM) {
        constraint_cost = get_alm_cost(constraints, lagrange_multipliers);
    }
    
    return primary_cost + constraint_cost;
}

3. 约束处理

对应源代码： src/constraint_handler.cpp 中的 get_bound_constr() 和 get_obstacle_avoidance_constr() 函数

约束处理是CILQR算法的关键组成部分，它确保生成的轨迹满足物理可行性和安全性要求。在实际应用中，车辆必须遵守多种约束，包括物理限制、道路边界和安全距离等。

3.1 车辆动力学约束

车辆动力学约束反映了车辆本身的物理能力限制，这些约束是硬性的，不能违反。

加速度约束：车辆的最大加速度和减速度受到发动机功率、轮胎摩擦力和制动系统能力的限制。过大的加速度可能导致轮胎打滑或乘客不适。

转向角约束：转向角受到转向机构物理限制和车辆稳定性的约束。过大的转向角可能导致车辆失控或侧翻。

速度约束：速度必须保持在安全范围内，既要考虑道路限速，也要考虑车辆性能和稳定性要求。
$$\begin{array}{rl}{a}_{min}& \le a_k\le a_{max}\\ -st{l}_{lim}& \le {\delta }_k\le st{l}_{lim}\\ vel{o}_{min}& \le v_k\le vel{o}_{max}\end{array}$$

核心代码展示：

// 位于 src/constraint_handler.cpp
std::vector<double> get_dynamics_constraints(const std::vector<Eigen::Vector2d>& controls,
                                           const std::vector<Eigen::Vector4d>& states) {
    std::vector<double> constraints;
    
    for (const auto& u : controls) {
        // 加速度约束
        constraints.push_back(u(0) - acc_max);  // a <= acc_max
        constraints.push_back(acc_min - u(0));  // acc_min <= a
        
        // 转向角约束
        constraints.push_back(u(1) - stl_lim);  // delta <= stl_lim
        constraints.push_back(-stl_lim - u(1)); // -stl_lim <= delta
    }
    
    for (const auto& x : states) {
        // 速度约束
        constraints.push_back(x(2) - velo_max); // v <= velo_max
        constraints.push_back(velo_min - x(2)); // velo_min <= v
    }
    
    return constraints;
}

3.2 道路边界约束

道路边界约束确保车辆始终在允许的行驶区域内，这是交通安全的基本要求。

横向偏差计算：横向偏差衡量车辆相对于参考路径的侧向位置。正值表示车辆在参考路径右侧，负值表示在左侧。

计算原理：通过将车辆位置投影到参考路径上，计算车辆到参考路径的垂直距离。这种方法考虑了参考路径的曲率，使得约束更加准确。

安全裕度：约束中包含了车辆宽度的一半作为安全裕度，确保车辆不会过于接近道路边界，为驾驶员操作和系统误差留出余量。
$$d_k=\text{sign}(d_{sign})\cdot \Vert p_k-p_{ref,k}{\Vert }_2$$

其中：
$$d_{sign}=(y_k-y_{ref,k})\cdot \cos ({\psi }_{ref,k})-(x_k-x_{ref,k})\cdot \sin ({\psi }_{ref,k})$$

边界约束：
$$roa{d}_{left}+\frac{width}{2}\le d_k\le roa{d}_{right}-\frac{width}{2}$$

核心代码展示：

// 位于 src/constraint_handler.cpp
double calculate_lateral_deviation(const Eigen::Vector4d& state, 
                                 const Eigen::Vector4d& ref_state) {
    double dx = state(0) - ref_state(0);
    double dy = state(1) - ref_state(1);
    double ref_yaw = ref_state(3);
    
    // 计算横向偏差
    double lateral_deviation = dy * cos(ref_yaw) - dx * sin(ref_yaw);
    return lateral_deviation;
}

// 位于 src/constraint_handler.cpp
std::vector<double> get_road_boundary_constraints(const std::vector<Eigen::Vector4d>& states,
                                                const std::vector<Eigen::Vector4d>& ref_states) {
    std::vector<double> constraints;
    
    for (size_t k = 0; k < states.size(); ++k) {
        double lateral_dev = calculate_lateral_deviation(states[k], ref_states[k]);
        
        // 左边界约束
        constraints.push_back(road_left + vehicle_width/2 - lateral_dev);
        // 右边界约束
        constraints.push_back(lateral_dev - (road_right - vehicle_width/2));
    }
    
    return constraints;
}

3.3 障碍物避让约束

障碍物避让约束是自动驾驶中最关键的安全约束之一，它确保车辆与周围障碍物保持安全距离。

椭圆安全区域：我们使用椭圆形状来定义安全区域，这比圆形更加灵活，能够适应不同形状的障碍物和车辆。椭圆的长轴和短轴可以根据障碍物的几何特征和车辆的运动特性来调整。

安全裕度设计：安全裕度考虑了多个因素：

• 车辆尺寸和形状
• 障碍物的动态特性
• 传感器测量误差
• 系统响应时间

约束形式：当安全裕度大于0时，车辆处于安全状态；当安全裕度小于0时，车辆与障碍物过于接近，需要调整轨迹。
$$safety\_margin=1-{\left(\frac{x_{ego}-x_{obs}}{a}\right)}^2-{\left(\frac{y_{ego}-y_{obs}}{b}\right)}^2$$

其中：

• $a,b$：椭圆半轴长度
• 前部安全距离：$margi{n}_{front}>0$
• 后部安全距离：$margi{n}_{rear}>0$

核心代码展示：

// 位于 src/constraint_handler.cpp
double calculate_obstacle_safety_margin(const Eigen::Vector4d& ego_state,
                                      const Obstacle& obstacle) {
    double dx = ego_state(0) - obstacle.x;
    double dy = ego_state(1) - obstacle.y;
    
    // 椭圆安全区域
    double safety_margin = 1.0 - (dx * dx) / (obstacle.a * obstacle.a) 
                                   - (dy * dy) / (obstacle.b * obstacle.b);
    
    return safety_margin;
}

// 位于 src/constraint_handler.cpp
std::vector<double> get_obstacle_avoidance_constraints(const std::vector<Eigen::Vector4d>& states,
                                                     const std::vector<Obstacle>& obstacles) {
    std::vector<double> constraints;
    
    for (const auto& state : states) {
        for (const auto& obstacle : obstacles) {
            double safety_margin = calculate_obstacle_safety_margin(state, obstacle);
            constraints.push_back(safety_margin);  // 安全边界约束
        }
    }
    
    return constraints;
}

4. 后向传播算法

对应源代码： src/cilqr_solver.cpp 中的 backward_pass() 函数

后向传播算法是CILQR算法的核心，它通过动态规划的思想，从终端时刻开始向前计算最优控制律和价值函数。这个过程为前向传播提供了必要的反馈增益和前馈项。

4.1 价值函数

价值函数 $V_k(x)$ 表示从时刻 $k$ 开始，以状态 $x$ 为初始条件的最优代价。它由三部分组成：

• 二次项 $x^T{P}_{k}x$：表示状态偏离的代价
• 线性项 $q_k^{T}x$：表示状态偏差的线性代价
• 常数项 $r_k$：表示基准代价

价值函数的形式是二次的，这使得我们能够使用线性二次调节器的理论来求解最优控制问题。
$$V_k(x)=x^T{P}_{k}x+q_k^{T}x+r_k$$

4.2 Q函数

Q函数 $Q_k(x,u)$ 是后向传播算法的关键概念，它表示在时刻 $k$ 选择控制 $u$ 后，从该时刻开始的总代价。

物理意义：Q函数包含两部分：

• 即时代价 $l_k(x,u)$：当前时刻的状态和控制代价
• 未来代价 $V_{k+1}(f_k(x,u))$：应用控制后，下一时刻的最优代价

通过最小化Q函数，我们可以找到当前时刻的最优控制策略。

4.3 Q函数导数

Q函数的导数提供了寻找最优控制的方向信息。这些导数告诉我们如何调整状态和控制来减少总代价。

一阶导数：

• $Q_x$：状态梯度，表示状态变化对代价的影响
• $Q_u$：控制梯度，表示控制变化对代价的影响

二阶导数：

• $Q_{xx}$：状态Hessian矩阵，表示状态变化的二阶效应
• $Q_{uu}$：控制Hessian矩阵，表示控制变化的二阶效应
• $Q_{ux}$：状态-控制混合导数，表示状态和控制之间的耦合效应

阻尼因子：在 $Q_{uu}$ 中添加 $\lambda I$ 项是为了确保矩阵的可逆性，提高算法的数值稳定性。
$$\begin{array}{rl}{Q}_x& =l_x+{\left(\frac{\partial f}{\partial x}\right)}^T{V}_{x,k+1}\\ Q_u& =l_u+{\left(\frac{\partial f}{\partial u}\right)}^T{V}_{x,k+1}\\ Q_{xx}& =l_{xx}+{\left(\frac{\partial f}{\partial x}\right)}^T{V}_{xx,k+1}\left(\frac{\partial f}{\partial x}\right)\\ Q_{uu}& =l_{uu}+{\left(\frac{\partial f}{\partial u}\right)}^T{V}_{xx,k+1}\left(\frac{\partial f}{\partial u}\right)+\lambda I\\ Q_{ux}& =l_{ux}+{\left(\frac{\partial f}{\partial u}\right)}^T{V}_{xx,k+1}\left(\frac{\partial f}{\partial x}\right)\end{array}$$

其中 $\lambda$ 是阻尼因子。

核心代码展示：

// 位于 src/cilqr_solver.cpp
void backward_pass(const std::vector<Eigen::Vector4d>& states,
                  const std::vector<Eigen::Vector2d>& controls,
                  const std::vector<Eigen::Vector4d>& ref_states) {
    // 初始化终端价值函数
    Eigen::Matrix4d P = state_weight;
    Eigen::Vector4d q = -state_weight * ref_states.back();
    double r = 0.5 * ref_states.back().transpose() * state_weight * ref_states.back();
    
    std::vector<Eigen::Matrix4d> P_sequence;
    std::vector<Eigen::Vector4d> q_sequence;
    std::vector<double> r_sequence;
    
    // 后向传播
    for (int k = N-1; k >= 0; --k) {
        // 计算Q函数及其导数
        Eigen::Vector4d Q_x = state_weight * (states[k] - ref_states[k]);
        Eigen::Vector2d Q_u = ctrl_weight * controls[k];
        
        Eigen::Matrix4d Q_xx = state_weight;
        Eigen::Matrix2d Q_uu = ctrl_weight + lambda * Eigen::Matrix2d::Identity();
        Eigen::Matrix<double, 2, 4> Q_ux = Eigen::Matrix<double, 2, 4>::Zero();
        
        // 更新价值函数
        Eigen::Matrix2d Q_uu_inv = Q_uu.inverse();
        Eigen::Vector2d d_k = -Q_uu_inv * Q_u;
        Eigen::Matrix<double, 2, 4> K_k = -Q_uu_inv * Q_ux;
        
        // 存储控制律
        feedforward_gains[k] = d_k;
        feedback_gains[k] = K_k;
        
        // 更新价值函数参数
        P = Q_xx + K_k.transpose() * Q_uu * K_k + K_k.transpose() * Q_ux + Q_ux.transpose() * K_k;
        q = Q_x + K_k.transpose() * Q_uu * d_k + K_k.transpose() * Q_u + Q_ux.transpose() * d_k;
        r = r + 0.5 * d_k.transpose() * Q_uu * d_k + d_k.transpose() * Q_u;
        
        P_sequence.push_back(P);
        q_sequence.push_back(q);
        r_sequence.push_back(r);
    }
}

4.4 最优控制律

最优控制律是后向传播算法的输出，它定义了如何根据当前状态偏差来调整控制输入。

前馈项 $d_k$：表示在零状态偏差情况下的最优控制调整。它考虑了参考轨迹和当前轨迹之间的差异。

反馈增益 $K_k$：表示状态偏差对控制调整的敏感度。它确保系统能够快速响应状态变化，保持轨迹跟踪精度。

最优性条件：这些控制律是通过求解 $\frac{\partial Q_k}{\partial u}=0$ 得到的，确保在当前线性化点附近，控制策略是最优的。

4.5 价值函数更新

价值函数更新是后向传播算法的核心步骤，它通过当前时刻的最优控制律来更新价值函数参数。

更新原理：新的价值函数参数通过将最优控制律代入Q函数得到，这确保了价值函数的一致性。

递归关系：价值函数参数从终端时刻开始，逐步向前传播，每个时刻的最优策略都考虑了未来时刻的最优性。
$$\begin{array}{rl}{V}_{x,k}& =Q_x+K_k^T{Q}_{uu}{d}_k+K_k^T{Q}_u+Q_{ux}^T{d}_k\\ V_{xx,k}& =Q_{xx}+K_k^T{Q}_{uu}{K}_k+K_k^T{Q}_{ux}+Q_{ux}^T{K}_k\end{array}$$

4.6 期望代价下降

$$\Delta V=\left[0.5\cdot d_k^T{Q}_{uu}{d}_k,d_k^T{Q}_u\right]$$

5. 前向传播算法

对应源代码： src/cilqr_solver.cpp 中的 forward_pass() 函数

前向传播算法是CILQR算法的执行阶段，它使用后向传播计算出的控制律来生成新的轨迹。这个过程模拟了车辆在实际应用中的行为。

5.1 控制律应用

控制律应用是将后向传播计算出的最优策略转化为实际控制输入的过程。

控制更新公式：新的控制输入由三部分组成：

• 当前控制 $u_k$：保持轨迹的连续性
• 反馈项 $K_k(x_{new,k}-x_k)$：根据状态偏差进行调整
• 前馈项 $\alpha \cdot d_k$：根据参考轨迹进行调整

步长参数 $\alpha$：控制更新的幅度，通过线搜索确定最优值，确保代价函数下降。
$$u_{new,k}=u_k+K_k(x_{new,k}-x_k)+\alpha \cdot d_k$$

5.2 状态预测

状态预测使用车辆动力学模型来预测应用新控制后的车辆状态。

预测过程：从初始状态开始，逐步预测每个时刻的状态。每个状态都通过前一个状态和当前控制输入计算得到。

模型一致性：状态预测使用与后向传播相同的动力学模型，确保算法的一致性。

轨迹生成：通过状态预测，我们得到一条完整的轨迹，这条轨迹应该比原始轨迹更接近最优解。

核心代码展示：

// 位于 src/cilqr_solver.cpp
void forward_pass(const std::vector<Eigen::Vector4d>& states,
                 const std::vector<Eigen::Vector2d>& controls,
                 std::vector<Eigen::Vector4d>& new_states,
                 std::vector<Eigen::Vector2d>& new_controls,
                 double alpha) {
    new_states[0] = states[0];  // 初始状态保持不变
    
    for (size_t k = 0; k < controls.size(); ++k) {
        // 应用控制律
        Eigen::Vector2d control_update = feedback_gains[k] * (new_states[k] - states[k]) 
                                       + alpha * feedforward_gains[k];
        new_controls[k] = controls[k] + control_update;
        
        // 状态预测
        if (k + 1 < new_states.size()) {
            new_states[k + 1] = kinematic_propagate(new_states[k], new_controls[k], dt);
        }
    }
}

6. 线搜索

对应源代码： src/cilqr_solver.cpp 中的 iter_step() 函数内的线搜索逻辑

线搜索是CILQR算法中的重要组成部分，它确定控制更新的步长，确保每次迭代都能有效减少代价函数。合适的步长既能保证算法收敛，又能避免过大的更新导致的不稳定。

6.1 Armijo条件

Armijo条件是线搜索的基本准则，它确保选择的步长能够产生足够的代价下降。

条件含义：实际代价下降必须大于等于期望下降的一定比例。参数 $c_1$ 控制这个比例，通常取0.1。

物理意义：这个条件确保我们选择的步长不会太小（导致收敛缓慢），也不会太大（导致算法不稳定）。

实现策略：从初始步长开始，如果条件不满足，则逐步减小步长，直到找到合适的值。
$$J_{old}-J_{new}\ge c_1\cdot \alpha \cdot \nabla J^T\cdot p$$

其中：

• $\alpha$：步长
• $c_1$：Armijo常数（通常为0.1）
• $p$：搜索方向

6.2 实际vs期望代价下降

实际vs期望代价下降的比较是线搜索的另一个重要准则，它帮助我们评估步长的质量。

实际下降：通过前向传播计算得到的真实代价减少量。这是算法实际取得的进展。

期望下降：基于后向传播的理论预测值。它基于当前线性化点的信息，提供了代价下降的理论上限。

接受比例：实际下降与期望下降的比值，衡量步长的有效性。接近1表示步长选择得很好，远小于1表示步长过大。

接受条件：步长被接受需要满足两个条件：

1. 实际代价下降为正（确保算法在正确的方向上）
2. 接受比例大于阈值（确保步长选择合理）

核心代码展示：

// 位于 src/cilqr_solver.cpp
double line_search(const std::vector<Eigen::Vector4d>& states,
                  const std::vector<Eigen::Vector2d>& controls,
                  const std::vector<Eigen::Vector4d>& ref_states,
                  double alpha_init = 1.0) {
    double alpha = alpha_init;
    double c1 = 0.1;  // Armijo常数
    
    double J_old = get_total_cost_with_constraints(states, controls, ref_states);
    
    while (alpha > 1e-6) {
        // 前向传播
        std::vector<Eigen::Vector4d> new_states(states.size());
        std::vector<Eigen::Vector2d> new_controls(controls.size());
        forward_pass(states, controls, new_states, new_controls, alpha);
        
        double J_new = get_total_cost_with_constraints(new_states, new_controls, ref_states);
        double actual_decay = J_old - J_new;
        
        // 检查Armijo条件
        if (actual_decay > 0) {
            return alpha;
        }
        
        alpha *= 0.5;  // 减小步长
    }
    
    return 0.0;  // 未找到合适步长
}

7. 约束导数计算

对应源代码： src/cilqr_solver.cpp 中的 get_total_cost_derivatives_and_Hessians() 函数

约束导数计算是CILQR算法中处理约束的关键步骤。为了将约束转换为代价函数，我们需要计算约束函数对状态和控制的导数，这些导数决定了约束惩罚项的梯度信息。

7.1 边界约束导数

边界约束是最简单的约束类型，它们直接限制状态或控制变量的取值范围。

上界约束：形式为 $c=variable-bound$，当变量超过上界时，约束被违反。导数 $\frac{\partial c}{\partial variable}=1$ 表示增加变量值会增大约束违反。

下界约束：形式为 $c=bound-variable$，当变量低于下界时，约束被违反。导数 $\frac{\partial c}{\partial variable}=-1$ 表示增加变量值会减少约束违反。

物理意义：这些导数告诉我们如何调整变量来满足约束。正值表示需要减小变量，负值表示需要增大变量。
上界约束 $c=variable-bound$：
$$\frac{\partial c}{\partial variable}=1$$

下界约束 $c=bound-variable$：
$$\frac{\partial c}{\partial variable}=-1$$

7.2 障碍函数导数

障碍函数的导数计算相对复杂，因为障碍函数本身是非线性的。

一阶导数：障碍函数对状态的一阶导数包含三个因子：

• $q_2$：控制惩罚增长的速率参数
• $B(c)$：障碍函数值本身
• $\frac{\partial c}{\partial x}$：约束函数对状态的导数

二阶导数：障碍函数的Hessian矩阵用于二阶优化方法，它提供了代价函数的曲率信息。

数值特性：当约束接近边界时，障碍函数值会快速增长，导数也会变得很大。这可能导致数值不稳定，需要适当的阻尼策略。

7.3 增广拉格朗日导数

增广拉格朗日方法的导数计算相对简单，因为惩罚项是二次的。

激活条件：只有当 $c+\frac{\mu }{\rho }>0$ 时，约束惩罚项才会产生非零导数。这个条件确保我们只对违反的约束进行惩罚。

一阶导数：包含三个因子：

• $\rho$：惩罚因子，控制惩罚的强度
• $(c+\frac{\mu }{\rho })$：调整后的约束值，考虑了拉格朗日乘子的影响
• $\frac{\partial c}{\partial x}$：原始约束的导数

二阶导数：Hessian矩阵是常数矩阵，这提供了更好的数值稳定性，相比障碍函数方法。

7.4 拉格朗日乘子更新

拉格朗日乘子的更新是增广拉格朗日方法的核心，它通过迭代调整来逐步满足约束。

更新规则：新的拉格朗日乘子基于当前乘子、约束违反程度和惩罚因子计算得到。

约束满足时：如果约束满足（$c_k\le 0$），拉格朗日乘子会减少，降低惩罚强度。

约束违反时：如果约束违反（$c_k>0$），拉格朗日乘子会增加，增强惩罚强度。

边界限制：拉格朗日乘子被限制在 $[0,{\mu }_{max}]$ 范围内，确保算法的稳定性。

惩罚因子调整：随着迭代进行，惩罚因子会逐渐增加，使得约束满足更加严格。

核心代码展示：

// 位于 src/cost_functions.cpp
void update_lagrange_multipliers(const std::vector<double>& constraints,
                               std::vector<double>& multipliers) {
    for (size_t i = 0; i < constraints.size(); ++i) {
        double new_multiplier = multipliers[i] + alm_rho * constraints[i];
        multipliers[i] = std::max(0.0, std::min(new_multiplier, max_mu));
    }
    
    // 更新惩罚因子
    alm_rho = std::min(alm_rho * alm_gamma, max_rho);
}

// 位于 src/cilqr_solver.cpp
void get_cost_derivatives(const std::vector<Eigen::Vector4d>& states,
                         const std::vector<Eigen::Vector2d>& controls,
                         const std::vector<Eigen::Vector4d>& ref_states,
                         std::vector<Eigen::Vector4d>& state_gradients,
                         std::vector<Eigen::Vector2d>& control_gradients) {
    // 计算主要目标函数的梯度
    for (size_t k = 0; k < states.size(); ++k) {
        state_gradients[k] = 2.0 * state_weight * (states[k] - ref_states[k]);
    }
    
    for (size_t k = 0; k < controls.size(); ++k) {
        control_gradients[k] = 2.0 * ctrl_weight * controls[k];
    }
    
    // 添加约束惩罚项的梯度
    if (solve_type == SolveType::BARRIER) {
        add_barrier_gradients(states, controls, state_gradients, control_gradients);
    } else if (solve_type == SolveType::ALM) {
        add_alm_gradients(states, controls, state_gradients, control_gradients);
    }
}

8. 收敛判据

对应源代码： src/cilqr_solver.cpp 中的 iter_step() 函数内的收敛检查逻辑

收敛判据是CILQR算法的重要组成部分，它决定何时停止迭代。合适的收敛判据既能确保算法达到足够的精度，又能避免不必要的计算。

8.1 代价下降判据

代价下降判据是最直观的收敛判断标准，它衡量算法在每次迭代中取得的进展。

判断条件：当前后两次迭代的代价差异小于预设阈值时，认为算法已经收敛。

阈值选择：收敛阈值 ${\epsilon }_{convergence}$ 需要根据具体应用场景选择：

• 过小：可能导致过度迭代，增加计算时间
• 过大：可能导致解不够精确，影响控制性能

物理意义：代价函数不再显著下降意味着算法已经找到了局部最优解，或者接近了最优解。
$$|J_{k+1}-J_k|<{\epsilon }_{convergence}$$

8.2 梯度判据

梯度判据基于最优性条件，当代价函数的梯度足够小时，说明我们已经接近局部最优解。

最优性条件：在无约束优化中，局部最优解的必要条件是梯度为零。在约束优化中，我们检查梯度的范数是否足够小。

梯度计算：梯度包含状态梯度 ${\nabla }_{x}J$ 和控制梯度 ${\nabla }_{u}J$，它们的组合范数反映了当前解与最优解的接近程度。

阈值选择：梯度阈值 ${\epsilon }_{gradient}$ 通常比收敛阈值更严格，因为它直接反映了最优性。

数值考虑：由于数值误差的存在，我们通常不要求梯度完全为零，而是要求它小于一个合理的阈值。

8.3 步长判据

步长判据是线搜索的补充收敛条件，它检查算法是否能够接受最大步长。

步长含义：当 $\alpha =1.0$ 时，表示线搜索选择了最大步长，这通常意味着当前线性化近似很好。

代价下降：即使步长为1.0，如果代价下降仍然很小，说明算法已经接近收敛。

组合判断：步长判据通常与其他判据结合使用，提供更可靠的收敛判断。

物理意义：能够接受最大步长且代价下降很小，说明当前轨迹已经接近最优解，进一步优化空间有限。

核心代码展示：

// 位于 src/cilqr_solver.cpp
bool check_convergence(double J_old, double J_new, double alpha, 
                      const std::vector<Eigen::Vector4d>& state_gradients,
                      const std::vector<Eigen::Vector2d>& control_gradients) {
    // 代价下降判据
    if (std::abs(J_new - J_old) < convergence_threshold) {
        return true;
    }
    
    // 梯度判据
    double gradient_norm = 0.0;
    for (const auto& grad : state_gradients) {
        gradient_norm += grad.squaredNorm();
    }
    for (const auto& grad : control_gradients) {
        gradient_norm += grad.squaredNorm();
    }
    gradient_norm = std::sqrt(gradient_norm);
    
    if (gradient_norm < gradient_threshold) {
        return true;
    }
    
    // 步长判据
    if (alpha == 1.0 && std::abs(J_new - J_old) < convergence_threshold) {
        return true;
    }
    
    return false;
}

9. 阻尼因子调整

对应源代码： src/cilqr_solver.cpp 中的 solve() 函数内的阻尼因子调整逻辑

阻尼因子调整是CILQR算法中提高数值稳定性的重要机制。阻尼因子 $\lambda$ 在Q函数的Hessian矩阵中添加正则化项，确保矩阵的可逆性和算法的稳定性。

9.1 调整策略

阻尼因子的调整基于迭代的成功与否，采用自适应策略来平衡收敛速度和数值稳定性。

成功迭代：当代价函数下降时，说明当前线性化近似很好，可以减小阻尼因子，提高收敛速度。

失败迭代：当代价函数上升时，说明当前线性化近似可能不够好，需要增大阻尼因子，提高数值稳定性。

边界限制：阻尼因子被限制在合理范围内，避免过小导致数值不稳定，或过大导致收敛缓慢。

调整参数：

• $decay\_factor$：成功时的衰减因子，通常小于1
• $amplify\_factor$：失败时的放大因子，通常大于1
• ${\lambda }_{max}$：最大阻尼因子，防止过度保守
• 成功迭代：$\lambda =\lambda \cdot decay\_factor$
• 失败迭代：$\lambda =\max (amplify\_factor,\lambda \cdot amplify\_factor)$
• 最大限制：$\lambda \le {\lambda }_{max}$

核心代码展示：

// 位于 src/cilqr_solver.cpp
void adjust_damping_factor(bool iteration_successful) {
    if (iteration_successful) {
        // 成功迭代，减小阻尼因子
        lambda = std::max(lambda * lamb_decay, 1e-6);
    } else {
        // 失败迭代，增大阻尼因子
        lambda = std::min(lambda * lamb_amplify, max_lamb);
    }
}

10. 轨迹质量指标

对应源代码： src/trajectory_evaluator.cpp 中的 calculate_trajectory_metrics() 函数

轨迹质量指标是评估CILQR算法性能的重要工具，它们从不同角度衡量生成轨迹的质量。这些指标不仅用于算法调优，也用于实际应用中的性能评估。

10.1 平滑性指标

平滑性指标衡量轨迹的连续性和舒适性，这对于乘客体验和车辆控制都很重要。

控制平滑性：衡量相邻控制输入之间的变化幅度。过大的控制变化可能导致：

• 乘客不适
• 车辆机械磨损
• 控制执行困难

状态平滑性：衡量相邻状态之间的变化幅度。过大的状态变化可能导致：

• 轨迹不够平滑
• 跟踪困难
• 安全性降低

计算方法：通过计算相邻时刻的欧几里得距离来量化平滑性，较小的值表示更平滑的轨迹。
$$smoothness=\frac{\sum |u_{k+1}-u_k|+\sum |x_{k+1}-x_k|}{N_u+N_x}$$

10.2 安全性指标

安全性指标是轨迹质量评估中最重要的指标之一，它直接关系到车辆和乘客的安全。

约束违反统计：统计所有约束中违反的数量，包括：

• 动力学约束违反
• 道路边界约束违反
• 障碍物避让约束违反

安全裕度：不仅检查约束是否违反，还考虑约束的违反程度。轻微的违反可能比严重违反更容易接受。

权重分配：不同类型的约束可以分配不同的权重，反映它们在安全性中的重要性。

实时监控：在实际应用中，安全性指标需要实时计算和监控，确保车辆始终处于安全状态。

核心代码展示：

// 位于 src/trajectory_evaluator.cpp
TrajectoryMetrics calculate_trajectory_metrics(const std::vector<Eigen::Vector4d>& states,
                                             const std::vector<Eigen::Vector2d>& controls,
                                             const std::vector<double>& constraints) {
    TrajectoryMetrics metrics;
    
    // 计算平滑性指标
    double control_smoothness = 0.0;
    for (size_t k = 1; k < controls.size(); ++k) {
        control_smoothness += (controls[k] - controls[k-1]).norm();
    }
    
    double state_smoothness = 0.0;
    for (size_t k = 1; k < states.size(); ++k) {
        state_smoothness += (states[k] - states[k-1]).norm();
    }
    
    metrics.smoothness = (control_smoothness + state_smoothness) / 
                        (controls.size() + states.size());
    
    // 计算安全性指标
    int violated_constraints = 0;
    for (double constraint : constraints) {
        if (constraint < 0) {
            violated_constraints++;
        }
    }
    
    metrics.safety = 1.0 - static_cast<double>(violated_constraints) / constraints.size();
    
    return metrics;
}

11. 主求解循环

对应源代码： src/cilqr_solver.cpp 中的 solve() 函数

主求解循环是CILQR算法的顶层结构，它协调各个子模块的工作，实现完整的优化过程。这个循环将理论算法转化为可执行的代码，是算法的核心实现。

算法流程概述

主求解循环遵循以下步骤：

1. 初始化：设置初始轨迹和参数
2. 迭代优化：重复执行后向传播、前向传播和线搜索
3. 收敛检查：判断是否达到收敛条件
4. 参数调整：根据迭代结果调整算法参数
5. 结果输出：返回最优轨迹和控制序列

关键设计考虑

迭代控制：设置最大迭代次数，防止无限循环
收敛判断：综合使用多种判据，确保收敛的可靠性
参数自适应：根据算法表现动态调整参数，提高鲁棒性
约束处理：在每次迭代中更新约束信息，确保解的可行性

// 位于 src/cilqr_solver.cpp
bool CILQRSolver::solve(const Eigen::Vector4d& initial_state,
                        const std::vector<Eigen::Vector4d>& ref_states,
                        std::vector<Eigen::Vector4d>& solution_states,
                        std::vector<Eigen::Vector2d>& solution_controls) {
    // 初始化轨迹
    initialize_trajectory(initial_state, ref_states, solution_states, solution_controls);
    
    double J_old = get_total_cost_with_constraints(solution_states, solution_controls, ref_states);
    
    for (int iter = 0; iter < max_iter; ++iter) {
        // 计算代价函数及其导数
        std::vector<Eigen::Vector4d> state_gradients;
        std::vector<Eigen::Vector2d> control_gradients;
        get_cost_derivatives(solution_states, solution_controls, ref_states,
                           state_gradients, control_gradients);
        
        // 后向传播
        backward_pass(solution_states, solution_controls, ref_states);
        
        // 前向传播和线搜索
        std::vector<Eigen::Vector4d> new_states = solution_states;
        std::vector<Eigen::Vector2d> new_controls = solution_controls;
        
        double alpha = line_search(solution_states, solution_controls, ref_states);
        forward_pass(solution_states, solution_controls, new_states, new_controls, alpha);
        
        // 计算新代价
        double J_new = get_total_cost_with_constraints(new_states, new_controls, ref_states);
        
        // 检查收敛
        if (check_convergence(J_old, J_new, alpha, state_gradients, control_gradients)) {
            solution_states = new_states;
            solution_controls = new_controls;
            return true;
        }
        
        // 更新轨迹
        solution_states = new_states;
        solution_controls = new_controls;
        J_old = J_new;
        
        // 调整阻尼因子
        adjust_damping_factor(J_new < J_old);
        
        // 更新拉格朗日乘子（如果使用ALM）
        if (solve_type == SolveType::ALM) {
            std::vector<double> constraints = get_all_constraints(solution_states, solution_controls);
            update_lagrange_multipliers(constraints, lagrange_multipliers);
        }
    }
    
    return false;  // 达到最大迭代次数
}

12. 总结与展望

算法特点总结

CILQR算法作为一种约束最优控制方法，具有以下显著特点：

理论优势：

• 基于成熟的iLQR理论，具有良好的收敛性保证
• 能够处理多种类型的约束条件
• 支持实时应用，适合在线轨迹规划

实现优势：

• 模块化设计，各组件职责明确
• 参数自适应调整，提高算法鲁棒性
• 支持多种约束处理方法，适应不同应用场景

应用优势：

• 适用于自动驾驶车辆轨迹规划
• 能够处理动态环境和实时约束
• 生成的轨迹平滑、安全、可执行

算法局限性

尽管CILQR算法具有诸多优势，但也存在一些局限性：

计算复杂度：每次迭代需要计算雅可比矩阵和Hessian矩阵，计算量较大
局部最优：算法可能收敛到局部最优解，而非全局最优解
参数敏感性：算法性能对参数选择较为敏感，需要仔细调优
约束处理：某些复杂约束可能难以有效处理

未来发展方向

CILQR算法的未来发展可能包括：

算法改进：

• 开发更高效的线性化方法
• 改进约束处理策略
• 引入机器学习方法优化参数选择

应用扩展：

• 扩展到多车辆协同控制
• 支持更复杂的车辆模型
• 集成感知和预测模块

性能优化：

• 并行化计算提高效率
• 自适应预测时域选择
• 实时性能优化

13. 参考文献

本文档基于以下参考资料：

• iLQR算法原理及实现
• CILQR约束处理方法
• 车辆动力学建模
• 最优控制理论

进一步学习建议

对于希望深入了解CILQR算法的读者，建议：

1. 理论基础：学习最优控制理论和动态规划
2. 数值方法：了解约束优化和数值优化算法
3. 应用实践：尝试在不同场景下应用算法
4. 性能调优：学习参数选择和算法调优技巧

---

附录：符号说明

状态变量

符号	含义	单位	物理意义
$x$	车辆X坐标	m	车辆在全局坐标系中的X位置
$y$	车辆Y坐标	m	车辆在全局坐标系中的Y位置
$v$	车辆速度	m/s	车辆的前进速度
$\psi$	车辆朝向角	rad	车辆前进方向与X轴的夹角

控制变量

符号	含义	单位	物理意义
$a$	加速度	m/s²	车辆的纵向加速度
$\delta$	前轮转向角	rad	前轮相对于车身的转向角度

系统参数

符号	含义	单位	物理意义
$L$	轴距	m	前后轴之间的距离
$dt$	时间步长	s	离散化时间间隔
$N$	预测时域	-	预测步数

算法参数

符号	含义	单位	物理意义
$Q$	状态权重矩阵	-	状态跟踪误差的权重
$R$	控制权重矩阵	-	控制输入代价的权重
$\lambda$	阻尼因子	-	数值稳定性参数
$\rho$	惩罚因子	-	约束违反的惩罚强度
$\mu$	拉格朗日乘子	-	约束的"价格"

约束参数

符号	含义	单位	物理意义
$a_{min},a_{max}$	加速度范围	m/s²	车辆加速度的物理限制
$st{l}_{lim}$	转向角限制	rad	转向机构的物理限制
$vel{o}_{min},vel{o}_{max}$	速度范围	m/s	车辆速度的安全限制