自动驾驶控制算法——基础控制算法原理

Pure Pursuit 纯几何跟踪算法

Pure Pursuit算法是一种通过车辆运动几何学（阿克曼转向几何）对前轮转角进行调整来消除横向偏差的横向控制算法,其参考点为后轴中心点。

该算法的思想是：基于当前车辆后轴中心位置，在参考路径上向前$l_d$的距离匹配一个预瞄点，将此预瞄点作为该周期的goal point。假设车辆后轴中心点可以按照一定的转弯半径R形式抵达该预瞄点，然后根据预瞄距离$l_d$、转弯半径R和车辆坐标系下预瞄点的朝向角$\alpha$之间的几何关系来确定前轮转角，其原理图如下：

为了使车辆后轴中心点跟踪圆弧虚线路径到达C点，在三角形OAC中，需要满足的正弦定理关系：

$$\frac{l_d}{sin2\alpha }=\frac{R}{sin(\frac{\pi }{2}-\alpha )}\text{\hspace{1em}(1)}$$

又因为：$sin2\alpha =2sin\alpha cos\alpha ,sin(\frac{\pi }{2}-\alpha )=cos\alpha$,所以化简上述公式可得：

$$R=\frac{l_d}{2sin\alpha }\text{\hspace{1em}(2)}$$

为了达到这种几何关系，作为控制量的前轮转角需要满足什么关系呢？在阿克曼转向（三角形OAB）中，有：

$$tan\delta \approx \frac{L}{R}\text{\hspace{1em}(3)}$$

因此，联立上面的公式(2)(3)，得到：

$$\delta (t)=arctan(\frac{2Lsin\alpha (t)}{l_d})\text{\hspace{1em}(4)}$$

其中，车辆坐标系下预瞄点的朝向角$\alpha (t)$为变量。

另外，定义横向位置误差为车辆当前姿态和预瞄点在横向上的误差：

$$e_y(t)=l_{d}sin\alpha (t)\text{\hspace{1em}(5)}$$

联立公式（4）（5），可得到前轮转角计算公式：
$$\delta (t)=arctan(\frac{2L}{l_d^2}{e}_y(t))\text{\hspace{1em}(6)}$$
一般而言，考虑小角度假设，公式（6）可变成：
$$\delta (t)=arctan(\frac{2L}{l_d^2}{e}_y(t))\approx \frac{2L}{l_d^2}{e}_y(t)\text{\hspace{1em}(7)}$$

因此，纯跟踪法本质上是一个P控制器,输入为横向位置误差，输出为前轮转角。跟踪的控制效果将由预瞄距离$l_d$决定，通常$l_d$定义为关于速度的函数表达式：
$$l_d=l_{d0}+kv\text{\hspace{1em}(8)}$$

算法特点：

• 短的预瞄距离提供更精确的跟踪，而较长预瞄距离提供更平滑的跟踪。
• 参数k值太小将导致不稳定，而k值太大将导致跟踪精度下降。
• 高水平的鲁棒性：例如，对路径中的不连续性的良好处理。
• 弯道跟踪中，随着速度的增加，稳态误差也增大。

Stanley算法（前轮反馈控制）

前轮反馈控制(Front wheel feedback)又称Stanley控制其核心思想是基于前轴中心的路径跟踪偏差量对方向盘转向控制量进行计算。

Stanley方法是一种基于横向跟踪误差为前轴中心到最近路径点的距离的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量。

Stanley法计算得到的前轮转角控制量由两部分构成:

• 一部分是航向误差引起的转角，即当前车身方向与参考轨迹最近点的切线方向的夹角;
• 另一部分是横向误差引起的转角，即前轴中心到参考轨迹最近点的横向距离。

Stanley法的原理示意图如下所示：

在不考虑横向跟踪误差的情况下，前轮转角应当与给定路径参考点的切线方向一致。其中，${\theta }_{\phi }$表示车辆航向与最近路径点切线方向之间的夹角，在没有任何横向误差的情况下，前轮方向应与所在路径点的方向相同,故消除航向误差的前轮转角为；
$${\delta }_{\phi }(t)={\theta }_{\phi }(t)\text{\hspace{1em}(9)}$$
在不考虑航向跟踪偏差的情况下，横向跟踪误差越大，前轮转向角越大，假设车辆预期轨迹在距离前轮$d(t)$处与参考路径上最近点切线相交，根据几何关系得出如下非线性比例函数:
$${\delta }_y(t)={\theta }_y(t)=arctan(\frac{e_y(t)}{d(t)})=arctan(\frac{k{e}_y(t)}{v(t)})\text{\hspace{1em}(10)}$$
其中$d(t)$与车速相关，用车速$v(t)$和增益参数k表示.

因此，前轮转角为:
$$\delta (t)={\delta }_{\phi }(t)+{\delta }_y(t)={\delta }_{\phi }(t)+arctan(\frac{k{e}_y(t)}{v(t)}),\delta (t)\in [{\delta }_{min},{\delta }_{max}]\text{\hspace{1em}(11)}$$

算法特点：

• 如果速度反向可能导致数值不稳定，可以向控制器添加正的松弛常数$k_s$;
• 在实车中,以更高的速度行驶时成为一个问题;
• 可以通过在航向上添加前馈项来改进曲线的跟踪；
• 与Pure Pursuit相比，Stanley方法更直观，但它在调优时也会遇到类似的问题。
• 与Pure Pursuit相比，经过精心调整的Stanley跟踪器不会“偷工减料”，但相当于过冲转弯，这种影响可以归因于没有预瞄距离。
• 与Pure Pursuit方法类似，中高速情况下，曲线跟踪的的稳态误差变得显著。

LQR控制算法（全状态反馈控制-最优控制）

LQR控制算法是一种广泛应用于工业控制和机器人控制中的线性控制算法，它利用状态反馈控制来实现系统的稳定性和优化性能。

LQR控制算法的基本思想：通过对系统的状态进行反馈控制，使系统的输出响应达到最优，同时保持系统的稳定性。具体来说，LQR控制算法通过将系统的状态反馈控制矩阵K计算出来，使得系统的状态能够以最小的代价函数进行控制，控制示意图如下：

在LQR控制算法中，代价函数用来描述系统的响应性能和能耗。通常情况下，代价函数可以表示为系统状态与控制输入的加权和，即：
$$J=\int [X(t{)}^{T}QX(t)+U(t{)}^{T}RU(t)]dt\text{\hspace{1em}(12)}$$
其中，Q和R分别是状态和控制输入的权重矩阵，它们的大小和系统状态和控制输入的维度相同。通过对代价函数进行求导并令其等于0，可以得到连续的状态反馈矩阵K的表达式：
$$K=-R^{-1}{B}^{T}P\text{\hspace{1em}(13)}$$
其中，$P$是Riccati方程的解，它可以通过求解Riccati方程得到，具体的求解方法可以使用数值方法，例如MATLAB中的lqr函数。

一旦得到了状态反馈矩阵K，就可以将其用于控制系统中。具体来说，控制输入（连续的）可以表示为：
$$U(t)=-KX(t)\text{\hspace{1em}(14)}$$
通过这种方式，系统的状态可以被反馈到控制输入中，从而实现系统的控制。

以车辆的横向控制为例：

步骤1：建立道路-车辆误差状态空间方程
$$\begin{gathered} \dot{X(t)}=AX(t)+BU(t) \\ Y(t)=CX(t)\text{\hspace{1em}(15)} \end{gathered}$$
步骤2：对连续的误差状态方程进行离散化，一般A矩阵采用中点欧拉离散，B及C矩阵采用前向欧拉离散，得到离散的误差状态方程如下：
$$\begin{gathered} X(k+T)=A_{d}X(k)+B_{d}U(k) \\ Y(k)=C_{d}X(k)\text{\hspace{1em}(16)} \end{gathered}$$

步骤3：构建代价函数$J$,建立离散的LQR问题
$$J=\frac{1}{2}{X}^T(N)FX(N)+\frac{1}{2}\sum _{k=0}^{N-1}[X^T(k)QX(k)+U^T(k)RU(k)]\text{\hspace{1em}(17)}$$

步骤4：求解离散的LQR问题(动态规划法)

结论:
$$U(k)=-K(k)X(k)\text{\hspace{1em}(18)}$$
其中：
$$K(k)=[B^T(k)P(k+1)B(k)+R(k){]}^{-1}{B}^T(k)P(k+1)A(k)\text{\hspace{1em}(19)}$$
而且离散的Riccati方程为：
$$P(k)=A^T(k)P(k+1)A(k)-A^T(k)P(k+1)B(k)[B^T(k)P(k+1)B(k)+R(k){]}^{-1}{B}^T(k)P(k+1)A(k)\text{\hspace{1em}(20)}$$
因此，求解出矩阵$P$就可以得到控制律。

求解矩阵$P$：

1、令$P$等于最终状态权重矩阵;

2、迭代黎卡提方程求出新的$P_{next}$;

3、当两次P的差值足够小时，计算反馈矩阵K;

4、根据反馈矩阵K获取最优控制量U;

matrix lqrSolver(matrix A,matrix B,matrix Q, matrix R,double esp,double iterate_max)
{
    double gap = 999999; 
    double iterate = 0; 
    matrix P = Q; 
    matrix P_next = zero(4, 4); 
    while (iterate < iterate_max && gap > esp) 
    { 
        P_next = A.transpose() * P * A - A.transpose() * P * B * (B.transpose() * P * B + R).inv() * B.transpose() * P * A; 
        gap = (P_next - P).norm(); 
        P = P_next; 
        iterate++; 
    } 
    return P_next;
}