a8598671的博客

在之前的章节里面我们展示了最优化理论的基础求解算法，包括高斯-牛顿法（Gauss-Newton Method）、梯度下降法（Gradient Descent Method）、牛顿法（Newton's Method）和勒文贝格-马夸尔特法（Levenberg-Marquardt Method, LM方法）法。在实际工程应用中，我们一般采用C++进行开发，所以本文补充了上述求解方法的C++代码。在实际应用中，我们既可以自己进行简单的求解，也可以采用第三方库进行求解。

​之前的章节我们介绍过，iLQR（迭代线性二次调节器）是一种用于求解非线性系统最优控制最优控制最优控制和规划问题的算法。本章节介绍采用iLQR算法对设定的自动驾驶轨迹进行跟踪，与第十章节纯跟踪算法采用同样跟踪轨迹，同时，我们仅对控制量的上下边界进行约束，使用简单的投影法进行约束（更详细的约束参考第九章 CILQR约束条件下的ILQR求解）。其实，iLQR可以直接进行轨迹规划，主要做法是将障碍物或者边界约束通过增广拉格朗日法将原始问题的约束条件通过拉格朗日乘子和惩罚项结合到代价函数中。

纯跟踪算法（Pure Pursuit Algorithm）是一种用于路径跟踪的几何控制算法，广泛应用于自动驾驶、机器人导航等领域。其基本思想是通过选择预定路径上的目标点（预瞄点），并控制转向角，使车辆不断逼近并跟随该目标点，从而达到路径跟踪的效果。

我们考虑一个离散时间的动态系统，其状态方程为：其中，是系统在时间步k的状态，是控制输入，描述系统的动态方程。我们的目标是找到控制序列其中，是阶段成本函数，是终端成本。由于LQR算法要求系统的状态方程为线性状态方程，成本函数为二次型，因此iLQR是通过牛顿高斯方法进行迭代逼近最优解。为了方便优化和递推，我们引入值函数，它表示从时刻k开始，给定当前状态，剩余时间内的最小代价。其定义如下：值函数依赖于当前状态，并反映了从当前状态到终端状态所需支付的最小累积代价。

LQR 是基于线性系统和二次型代价函数的最优控制方法，在实际工程应用中，我们一般采用离散时间系统的LQR设计，其目标是通过设计反馈控制律，使系统在离散时间步长内稳定运行，最小化某个二次型代价函数。给定一个离散时间系统，其状态和控制输入通过以下方程表示：是时刻k的系统状态，是时刻k的控制输入，是状态转移矩阵，是输入矩阵。系统描述的物理意义：系统状态通过矩阵A演变到下一时刻，同时控制输入通过矩阵B影响系统的状态。

iLQR 的终止条件可以根据实际需求设置，最常见的是控制输入变化量或成本函数的变化量来判断是否收敛。合理选择阈值和最大迭代次数可以在保证收敛的同时避免过度计算。

本文介绍了高斯-牛顿法（Gauss-Newton Method）、梯度下降法（Gradient Descent Method）、牛顿法（Newton's Method）和勒文贝格-马夸尔特法（Levenberg-Marquardt Method, LM方法）的应用场景和迭代方法，并通过python和C++展示了四种不同的算法在非线性最小二乘问题的应用，最小二乘函数选择指数衰减函数y=a*exp(bx)。

DDP的问题定义和ILQR是一致的，都是建立离散时间系统的状态方程和代价函数，目标是找到控制序列最小化成本函数。Q函数的定义为：将1. 对状态通过链式法则对2. 对控制输入的一阶导数4. 对控制的二阶导数5. 对状态和控制的混合二阶导数。

（Line Search）是一种常见的技巧，用于在更新控制序列时进一步提高算法的数值稳定性和收敛性。在 iLQR 的反向传播阶段，通过局部线性化的系统模型和二次近似的目标函数来计算新的控制增量。然而，由于近似并不精确，直接应用所得到的控制更新可能会导致目标函数值的增加，尤其是当系统具有高度非线性时。线搜索的核心思想是在一条给定的方向上缩小步长，直到找到一个合适的步长，使得目标函数有所改善。收敛性： 适当的正则化能够提高 DDP 收敛的稳定性，但过大的正则化参数。，继续计算目标函数值，直到找到合适的步长。

在实际工程应用中，噪声是系统无法避免的因素，主要包括过程噪声和观测噪声。在自动控制、机器人、自主驾驶等领域，噪声的影响尤其显著。为了提高控制系统的鲁棒性和性能，像LQG和iLQG这样的算法被广泛应用。其主要思想是在LQR和ILQR的状态方程中增加高斯噪声，LQG 是结合LQR和卡尔曼滤波器减少噪声干扰，ILQG通过在Q-函数对状态的二阶导数中增加噪声的协方差矩阵来更新控制律，使得系统能够在噪声环境下得到更为鲁棒的控制方案。

引入违反控制输入或状态约束的罚项，例如对于控制输入 uk\mathbf{u}_kuk​ 的上下限约束，可以定义一个惩罚项：其中是惩罚参数，用来调节约束的严格性。

最优化理论是数学与工程学中用于寻找最优解的分支，它研究如何在给定约束条件下最大化或最小化某个目标函数。其核心思想是通过分析变量的取值来找出最优的决策方案。最优化问题可以是线性的（线性规划）或非线性的（非线性规划），可以有约束（约束优化）或无约束（无约束优化）。在自动驾驶中，最优化理论主要解决路径规划、运动控制、感知与决策、鲁棒性优化等问题。