自动驾驶控制算法---求解误差微分方程（LQR算法及基于CARLA的Python代码实现）

Be No.1

已于 2024-04-28 16:03:53 修改

阅读量1k

点赞数 2

文章标签：自动驾驶人工智能机器学习

于 2023-11-01 21:25:02 首次发布

本文链接：https://blog.csdn.net/weixin_46072670/article/details/134156668

版权

$\begin{pmatrix} \dot{e_{d}}\\ \ddot{e_{d}}\\ \dot{e_{\varphi }}\\ \ddot{e_{\varphi }} \end{pmatrix}=\begin{pmatrix} 0 & 1 & 0 & 0\\ 0 & \frac{C_{\alpha f}+C_{\alpha r}}{mv_{x}} & -\frac{C_{\alpha f}+C_{\alpha r}}{m} & \frac{aC_{\alpha f}-bC_{\alpha r}}{mv_{x}}\\ 0 & 0 & 0 & 1\\ 0 & \frac{aC_{\alpha f}-bC_{\alpha r}}{Iv_{x}} & -\frac{aC_{\alpha f}-bC_{\alpha r}}{I} & \frac{a^{2}C_{\alpha f}+b^{2}C_{\alpha r}}{Iv_{x}} \end{pmatrix}\begin{pmatrix} e_{d}\\ \dot{e_{d}}\\ e_{\varphi }\\ \dot{e_{\varphi }} \end{pmatrix}+$

$\begin{pmatrix} 0\\ -\frac{C_{\alpha f}}{m}\\ 0\\ -a\frac{C_{\alpha f}}{I} \end{pmatrix}\delta +\begin{pmatrix} 0\\ \frac{aC_{\alpha f}-bC_{\alpha r}}{mv_{x}}-v_{x}\\ 0\\ \frac{a^{2}C_{\alpha f}+b^{2}C_{\alpha r}}{Iv_{x}} \end{pmatrix}\dot{\theta _{r}}$

$\dot{e_{rr}} = Ae_{rr} + Bu + C\dot{\theta _{r}}$

　　上述公式是系统的状态空间方程，控制的目的就是设计控制器使得控制输入u能够使得误差e趋于0。

1、LQR　　

　　对这个得到的 $\dot{e_{rr}} = Ae_{rr} + Bu + C\dot{\theta _{r}}$ 误差微分方程，因为不是标准的LQR中的线性约束形式，所以先忽略 $C\dot{\theta _{r}}$ 这一项，对 $\dot{e_{rr}} = Ae_{rr} + Bu$ 这个LQR线性约束系统进行代价函数最小值的求解，如下图就是系统（这里的x就是 $e_{rr}$ ）。

　　LQR原理分为连续的LQR和离散的LQR。在实际应用上用的都是离散的LQR，因为处理的数据大多都是离散的；连续的LQR原理涉及泛函数与变分法。

　　1.1、离散LQR

　　离散LQR问题就是将 $\dot{x} = Ax+Bu$ 离散化为 $x_{k+1} = \bar{A}x_{k}+\bar{B}u_{k}$ ，然后求出 $J= \sum_{k=0}^{+\infty }(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k})$ 在约束 $x_{k+1} = \bar{A}x_{k}+\bar{B}u_{k}$ 下取极小值的 $u_{k}$ 。

　　对于 $\dot{x} = Ax$ ，利用积分中值定理得出 $x(t+dt) = x(t)+Ax(\xi )dt$ ， $\xi \in (t,t+dt)$

　　向前欧拉法：令 $x(\xi ) = x(t)$ ，则 $x(t+dt) = (I+Adt)x(t)$

　　向后欧拉法：令 $x(\xi ) = x(t+dt)$ ，则 $x(t+dt) = (I-Adt)^{-1}x(t)$

　　中点欧拉法：令 $x(\xi )=\frac{x(t)+x(t+dt)}{2}$ ，则 $x(t+dt) = (I-Adt)^{-1}(I+Adt)x(t)$

　　先对 $\dot{x} = Ax+Bu$ 利用积分定理得出 $x(t+dt) = x(t)+Ax(\xi )dt +Bu(\xi )dt$ ，然后对 $x(\xi )$ 采用中点欧拉法，对 $u(\xi )$ 采用向前欧拉法，得出离散化的方程：

$x_{k+1} = \bar{A}x_{k}+\bar{B}u_{k}$ ，其中 $\bar{A}=(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})$ ， $\bar{B}=Bdt$

　　1.2、求解离散LQR

　　对于 $J= \sum_{k=0}^{+\infty }(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k})$ 在约束 $x_{k+1} = \bar{A}x_{k}+\bar{B}u_{k}$ 下的极小值的问题，可以先求 $J= \sum_{k=0}^{n-1}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k}) +x_{n}^{T}Qx_{n}$ 在约束 $x_{k+1} = \bar{A}x_{k}+\bar{B}u_{k}$ 下的极小值然后将n趋于无穷。

　　 $J= \sum_{k=0}^{n-1}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k}) +x_{n}^{T}Qx_{n}$ 在约束 $x_{k+1} = \bar{A}x_{k}+\bar{B}u_{k}$ 下的极小值可以用拉格朗日乘子法写为： $J= \sum_{k=0}^{n-1}(x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k}) +x_{n}^{T}Qx_{n}+\sum_{k=0}^{n-1}\lambda _{k+1}^{T}(\bar{A}x_{k}+\bar{B}u_{k}-x_{k+1})$

　　化简得到 $J= \sum_{k=0}^{n-1}(H_{k}-\lambda _{k}^{T}x_{k}) +x_{n}^{T}Qx_{n}-\lambda _{n}^{T}x_{n}+\lambda _{0}^{T}x_{0}$ ，其中 $H_{k}= x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k} +\lambda _{k+1}^{T}(\bar{A}x_{k}+\bar{B}u_{k})$ ， $\lambda _{0}=0$ 或 $\vec{0}$ 。

　　J分别对 $x_{k}$ 、 $u_{k}$ 、 $\lambda _{k}$ 求导为0，解出：

$\lambda _{k} = 2Qx_{k}+\bar{A}^{T}\lambda _{k+1}$ ， $k\in (1,2...n-1)$

$u _{k} = -\frac{R^{-1}\bar{B}^{T}\lambda _{k+1}}{2}$ ， $k\in (1,2...n-1)$

$x_{k+1} = \bar{A}x_{k}+\bar{B}u_{k}$ ， $k\in (1,2...n-1)$

$\lambda _{n} = 2Qx_{n}$

　　由这四个式子可以推出 $\lambda _{n-1} = 2[Q+A^{T}Q(I+BR^{-1}B^{T}Q)^{-1}A]x_{n-1}$ 。

　　设 $\lambda _{k} = 2P_{k}x_{k}$ ， $k\in (1,2...n)$ ，则 $P_{n}=Q$ ， $P _{n-1} = Q+A^{T}P _{n}(I+BR^{-1}B^{T}P _{n})^{-1}A$

　　得出递推式 $P _{k-1} = Q+A^{T}P _{k}(I+BR^{-1}B^{T}P _{k})^{-1}A$ ，这就是黎卡提方程。

　　那么就可以算出 $u_{k}$ 的表达式：

$u_{k}=-(R+B^{T}P_{k+1}B)^{-1}B^{T}P_{k+1}Ax_{k}$

　　这里的 $x_{k}$ 就是误差微分方程的 $e_{rr}$ 。

　　对于黎卡提方程来说迭代几十次就会收敛，何况是无穷次，因此对这个方程求解 $P = Q+A^{T}P(I+BR^{-1}B^{T}P)^{-1}A$ ，就可以得出收敛的P的值，那么收敛的控制量u：

$u=-(R+B^{T}PB)^{-1}B^{T}PAx_{k}=-kx_{k}$ 。

　　一般利用矩阵求逆引理得到黎卡提方程的另一个表达式（计算量小）：

$P = Q+A^{T}PA-A^{T}PB(R+B^{T}PB)^{-1}B^{T}PA$

总结就是对于误差微分方程的前两项 $\dot{e_{rr}} = Ae_{rr} + Bu$ ，首先离散化为 $e_{rr}(k+1) = \bar{A}e_{rr}(k)+\bar{B}u_{k}$ ，然后求解黎卡提方程 $P = Q+A^{T}PA-A^{T}PB(R+B^{T}PB)^{-1}B^{T}PA$ 得出P，代入 $u=-(R+B^{T}PB)^{-1}B^{T}PAx_{k}=-kx_{k}$ 得出控制量u。

2、前馈控制

　　对这个 $\dot{e_{rr}} = Ae_{rr} + Bu + C\dot{\theta _{r}}$ 误差微分方程，由LQR得出的 $u=-ke_{rr}$ 代入误差微分方程得出误差不可能一直为0，所以不能只用LQR得出u，因此要加入一个前馈控制 $\delta _{f}$ （前馈控制不受x的影响）处理 $C\dot{\theta _{r}}$ 这一项，使得稳态误差为0。

　　控制量u包含两部分，LQR算出的k和前馈控制 $\delta _{f}$ ：

$u=-ke_{rr}+\delta _{f}$

　　控制量u代入 $\dot{e_{rr}} = Ae_{rr} + Bu + C\dot{\theta _{r}}$ ，稳定时 $\dot{e_{rr}}$ =0，此时的 $e_{rr} = -(A-Bk)^{-1}(B\delta _{f}+C\dot{\theta _{r}})$ ，则目标就是选取合适的前馈控制 $\delta _{f}$ 使得 $e_{rr} = -(A-Bk)^{-1}(B\delta _{f}+C\dot{\theta _{r}})$ 接近0。

　　（1）、求 $\delta _{f}$

　　 $e_{rr}$ 可以写为向量：

$e_{rr}=\begin{pmatrix} \frac{1}{k}\left \{ \delta _{f}-\frac{\dot{\theta _{r}}}{v_{x}}\left [ a+b-bk_{3}-\frac{mv_{x}^{2}}{a+b}\left ( \frac{b}{C_{f}}+\frac{a}{C_{r}}k_{3}-\frac{a}{C_{r}} \right ) \right ] \right \}\\ 0\\ -\frac{\dot{\theta _{r}}}{v_{x}}\left ( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C\alpha _{r}} \right )\\ 0 \end{pmatrix}$

　　 $k_{3}$ 是向量k的第三个分量。当 $\delta _{f}=\frac{\dot{\theta _{r}}}{v_{x}}\left [ a+b-bk_{3}-\frac{mv_{x}^{2}}{a+b}\left ( \frac{b}{C_{f}}+\frac{a}{C_{r}}k_{3}-\frac{a}{C_{r}} \right ) \right ]$ 时， $e_{rr}$ 的第一个分量 $e_{d}=0$ 。而 $e_{rr}$ 第三个分量 $e_{\varphi } = -\frac{\dot{\theta _{r}}}{v_{x}}\left ( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C\alpha _{r}} \right )$ 不受LQR的k和前馈控制 $\delta _{f}$ 控制，并且要让航向误差 $\varphi +\beta -\theta _{r}$ 为0的话，那么 $e_{\varphi }=\varphi -\theta _{r}$ 就应该为 $-\beta$ 。

　　车在自然坐标系下的投影的 $\dot{s}=\frac{\left |\vec{v} \right |cos(\theta -\theta _{r})}{1-ke_{d}}$ ，可以根据角度的关系写为 $\dot{s}=\frac{v_{x}cose_{\varphi }-v_{y}sine_{\varphi }}{1-ke_{d}}$ ，近似为 $\dot{s}\approx v_{x}$ 。由曲率的定义式 $k=\frac{\dot{\theta _{r}}}{\dot{s}}$ 得出：

$\dot{\theta _{r}}\approx kv_{x}=\frac{v_{x}}{R}$

　　又公式 $a_{y}=\dot{v_{y}}+v_{x}\dot{\varphi }$ 和公式 $\dot{\varphi }=\frac{\vec{v_{x}}+\vec{v_{y}}}{R}$ ，假设无漂移（ $\dot{v_{y}}$ 可以舍去），得出：

$a_{y} \approx \frac{v_{x}^{2}}{R}$

　　由以上两个公式代入 $e_{\varphi } = -\frac{\dot{\theta _{r}}}{v_{x}}\left ( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C\alpha _{r}} \right )$ ，则：

$e_{\varphi } = -\left ( \frac{b}{R}+\frac{a}{a+b}\frac{ma_{y}}{C\alpha _{r}} \right )$

　　将车按质心不变等效为前后两部分，再根据侧向力的公式得出：

$e_{\varphi } = -\left ( \frac{b}{R}+\alpha _{r}\right )$

　　　　进一步的，如上图所示， $r\approx \frac{b}{R}$ 。在三角形中， $r+\frac{\pi }{2}-(-\alpha _{r})+\frac{\pi }{2}-\beta =\pi$ ，得出 $\beta =\frac{b}{R}+\alpha _{r}$ ，最终化简出：

$e_{\varphi } = -\beta$

　　稳态时 $e_{\varphi }=\varphi -\theta _{r}$ 就为 $-\beta$ ，则说明稳态时的航向误差就是0。

$\delta _{f}=k\left [ a+b-bk_{3}-\frac{mv_{x}^{2}}{a+b}\left ( \frac{b}{C_{f}}+\frac{a}{C_{r}}k_{3}-\frac{a}{C_{r}} \right ) \right ]$

总结：总的目标就是通过控制u使得 $e_{rr}$ 中的 $e_{d}$ 、 $e_{d\dot{}}$ 、 $e_{\dot{\varphi }}$ 为0， $e_{\varphi }$ 为 $-\beta$ (即满足航向误差为0)。过程是先使用LQR算出反馈控制的k值，然后利用k去算出前馈控制 $\delta _{f}$ ，此时施加控制 $u=-ke_{rr}+\delta _{f}$ 就满足 $e_{rr}$ 中的 $e_{d}$ 、 $e_{d\dot{}}$ 、 $e_{\dot{\varphi }}$ 为0， $e_{\varphi }$ 为 $-\beta$ (即满足航向误差为0)。

3、结合自动驾驶问题的基于LQR控制代码实现

　　LQR用于自动驾驶控制的横向控制。

　　1、导入库文件

import numpy as np
import math
import carla
import cvxopt
from collections import deque

　　2、初始化信息（车的位置信息、车的参数信息、主车变量、主车 $V_{x}$ 、规划出的信息、预测区间、控制区间、矩阵A、矩阵B、误差等）

class Lateral_LQR_controller(object):    #横向控制
    def __init__(self, ego_vehicle, vehicle_para, pathway_xy_theta_kappa):
        """
        self.vehicle_para = (a, b, Cf, Cr, m, Iz)
        a，b：前后轮中心距离车质心的距离
        CF, Cr：前后轮的侧偏刚度（按负的处理，apollo按正的处理）
        m：车的质量
        Iz：车的转动惯量
        """

        self.vehicle_para = vehicle_para
        self.vehicle_state = None                   # self.vehicle_state = (x, y, fai, vy, fai_dao)存储车的位置信息
        self.vehicle = ego_vehicle                  # ego_vehicle是carla中的主车
        self.vehicle_vx = 0                         # ego_vehicle的车辆速度在车轴纵向方向的分量
        self.target_path = pathway_xy_theta_kappa   # 规划出的信息集(x_m, y_m, theta_m, k_m)
        self.m = 4  # 状态变量x有四个分量
        self.p = 1  # 控制输入u有一个分量
        # 根据误差微分方程来写A、B
        self.A = np.zeros(shape=(self.m, self.m), dtype="float64")
        self.B = np.zeros(shape=(self.m, self.p), dtype="float64")
        self.A_bar = None    # 离散化的A矩阵
        self.B_bar = None    # 离散化的B矩阵
        self.K = None        # 反馈控制的K
        self.k_r = None      # 投影点曲率
        self.err = None      # 车的信息和规划的信息的误差即状态变量
        self.delta_f = None  # 前馈控制δf
        self.min_index = 0

        self.x_pre = 0        # 预测点x
        self.y_pre = 0        # 预测点y
        self.fai_pre = 0      # 预测点fai
        self.fai_dao_pre = 0  # 预测点fai_dao
        self.vx_pre = 0       # 预测点vx
        self.vy_pre = 0       # 预测点vy

        self.x_r = 0       # 投影点x
        self.y_r = 0       # 投影点y
        self.theta_r = 0   # 投影点θ

　　3、获取车辆的位置和状态信息（位置（x，y）、车身横摆角φ、速度向量、航向角、质心侧偏角β、角速度 $\dot{\phi }$ 、vx以及vy）

    def vehicle_info(self):
        """
        函数：获取车辆的位置和状态信息
        return: None
        """

        vehicle_location = self.vehicle.get_location()  # self.vehicle.get_location()的格式：Location(x=70.000000, y=200.000000, z=1.942856)
        x, y = vehicle_location.x, vehicle_location.y  # 70.0   200.0
        fai = self.vehicle.get_transform().rotation.yaw * (math.pi / 180)  # 车身横摆角φ即车轴纵向和x轴的夹角，结果转成弧度制：79*π/180
        v = self.vehicle.get_velocity()  # self.vehicle.get_velocity()的格式：Vector3D(x=0.000000, y=0.000000, z=-0.194462)，航向角是车速v的方向与x轴夹角（=质心侧偏角β+车身横摆角φ）即arctan(v.y/v.x)
        v_length = math.sqrt(v.x * v.x + v.y * v.y + v.z * v.z)  # 速度大小
        beta = (math.atan2(v.y, v.x) - fai)  # 质心侧偏角β，车速和车轴纵向之间的夹角
        vx = v_length * math.cos(beta)  # 车速在车身坐标系下x轴(即纵向)的分量
        vy = v_length * math.sin(beta)  # 车速在车身坐标系下y轴(即横向)的分量
        if abs(vx) < 0.005 and vx >= 0:
            vx = 0.005
        if abs(vx) < 0.005 and vx < 0:
            vx = -0.005
        fai_dao = self.vehicle.get_angular_velocity().z * (math.pi / 180) # 角速度 self.vehicle.get_angular_velocity()的格式：Vector3D(x=0.000000, y=0.000000, z=0.000000)
        self.vehicle_state = (x, y, fai, vy, fai_dao)  # 得到车的位置和状态信息
        self.vehicle_vx = vx  # 得到ego_vehicle的车辆速度在纵向方向的分量

　　4、计算k时刻横向控制的误差err（即状态空间方程的 $x_{k}$ ）以及曲率 $k_{r}$

    def cal_err_k_r(self, ts=0.1):
        """
        函数：计算预测点和规划点的误差err
        ts:预测的时间
        self.target_path:规划模块输出的轨迹
        [(x_m1, y_m1, theta_m1, k_m1),
         (x_m2, y_m2, theta_m2, k_m2),
         ...
         (x_mn, y_mn, theta_mn, k_mn)]
        x_r, y_r:直角坐标系下位置
        theta_r:速度方向与x轴夹角
        k_r:规划点的曲率
        self.vehicle_state: 车辆当前位置(x, y, fai, vy, fai_dao)
        x,y:车辆当前的的实际位置
        fai:航向角即车轴和x轴的夹角
        fai_dao:fai对时间的导数即角速度
        vx:车的质心速度在车轴(纵向)方向的分量
        vy:车的质心速度在垂直车轴(横向)方向的分量
        return: None
        """

        vx = self.vehicle_vx
        x, y, fai, vy, fai_dao = self.vehicle_state

        # 预测模块计算预测点信息(因为算法具有滞后性)
        self.x_pre = x + vx * ts * math.cos(fai) - vy * ts * math.sin(fai)
        self.y_pre = y + vy * ts * math.cos(fai) + vx * ts * math.sin(fai)
        self.fai_pre = fai + fai_dao * ts
        self.fai_dao_pre = fai_dao
        self.vx_pre = vx
        self.vy_pre = vy

        # 1、找匹配点
        path_length = len(self.target_path)
        min_d = 1000
        for i in range(self.min_index, min(self.min_index + 50, path_length)):  # 向前搜索50个点
            d = (self.target_path[i][0] - x) ** 2 + (self.target_path[i][1] - y) ** 2
            if d < min_d:
                min_d = d
                self.min_index = i
        min_index = self.min_index

        # 2、计算自然坐标系下规划点的轴向向量tor和法向量n
        tor = np.array([math.cos(self.target_path[min_index][2]), math.sin(self.target_path[min_index][2])])
        n = np.array([-math.sin(self.target_path[min_index][2]), math.cos(self.target_path[min_index][2])])

        # 3、计算匹配点指向车位置的向量d_err
        d_err = np.array([x - self.target_path[min_index][0], y - self.target_path[min_index][1]])

        # 4、计算横向距离误差ed, 纵向距离误差es
        ed = np.dot(n, d_err)
        es = np.dot(tor, d_err)

        # 5、获取投影点坐标(x_r,y_r)
        self.x_r, self.y_r = np.array([self.target_path[min_index][0], self.target_path[min_index][1]]) + es * tor

        # 6、计算theta_r
        self.theta_r = self.target_path[min_index][2] + self.target_path[min_index][3] * es  # (按投影点的theta_m)
        # self.theta_r = self.target_path[min_index][2]  # apollo的方案(按匹配点的theta_m)

        # 7、计算投影点的曲率k_r，近似等于匹配点的曲率k_m
        self.k_r = self.target_path[min_index][3]

        # 8、计算ed的导数ed_dao
        ed_dao = self.vy_pre * math.cos(fai - self.theta_r) + vx * math.sin(fai - self.theta_r)

        # 9、计算e_fai
        e_fai = math.sin(fai - self.theta_r)
        # e_fai = fai - theta_r

        # 10、计算投影点速度(s的导数)s_dao
        s_dao = (vx * math.cos(fai - self.theta_r) - vy * math.sin(fai - self.theta_r)) / (1 - self.k_r * ed)

        # 11、计算e_fai的导数e_fai_dao
        e_fai_dao = fai_dao - self.k_r * s_dao

        self.err = (ed, ed_dao, e_fai, e_fai_dao)

　　5、计算矩阵A、B（根据状态空间方程代入车辆参数、vx），并计算离散化矩阵 $\bar{A}$ 、 $\bar{B}$

　　系统的状态空间方程：

$\dot{e_{rr}} = Ae_{rr} + Bu + C\dot{\theta _{r}}$

　　计算 $A$ 、 $B$ 、 $\bar{A}$ 、 $\bar{B}$ 矩阵：

    def cal_A_B_and_discretion(self):
        """
        函数：根据整车参数self.vehicle_para和vx,计算矩阵A,B,并离散化状态空间方程(不带常数项Cθr_dao)
        return: None
        """

        vx = self.vehicle_vx
        (a, b, Cf, Cr, m, Iz) = self.vehicle_para

        # 1、A
        self.A[0][1] = 1

        self.A[1][1] = (Cf + Cr) / (m * vx)
        self.A[1][2] = -(Cf + Cr) / m
        self.A[1][3] = (a * Cf - b * Cr) / (m * vx)

        self.A[2][3] = 1

        self.A[3][1] = (a * Cf - b * Cr) / (Iz * vx)
        self.A[3][2] = -(a * Cf - b * Cr) / Iz
        self.A[3][3] = (a ** 2 * Cf + b ** 2 * Cr) / (Iz * vx)

        # 2、B
        self.B[1][0] = -Cf / m
        self.B[3][0] = -a * Cf / Iz

        # 3、A_bar、B_bar
        dt = 0.1  # 状态空间方程离散化的时间间隔dt
        e = np.linalg.inv(np.eye(4) - (dt * self.A) / 2)  # np.linalg.inv()是矩阵求逆
        self.A_bar = e @ (np.eye(4) + (dt * self.A) / 2)
        self.B_bar = e @ self.B * dt

　　6、求解黎卡提方程得出K

　　黎卡提方程：

$P = Q+A^{T}PA-A^{T}PB(R+B^{T}PB)^{-1}B^{T}PA$

　　K：

$K=(R+B^{T}PB)^{-1}B^{T}PA$

    def cal_LQR_K(self, Q, R):
        """
        函数：根据Q、R、self.A_bar、self.B_bar,通过迭代黎卡提方程P = Q + A.TPA - A.TPB(R+B.TPB)'B.TPA('是求逆)求出向量K(这里的A和B均是离散过的self.A_bar和self.B_bar)
        Q: 每一时刻误差代价的权重对应的对角矩阵，矩阵大小为self.m * self.m,对角线的数值越大算法的性能越好，但是会牺牲算法稳定性，而且最终控制量u很大。
        R: 每一时刻控制代价的权重对应的对角矩阵，矩阵大小为self.p * self.p，对角线的数值越大越平稳，变化越小，控制效果越好，但是误差会很大。
        self.A_bar: 状态空间方程系数矩阵，大小(self.m, self.m)
        self.B_bar: 状态空间方程系数矩阵，大小(self.m, self.p)
        return: None
        """

        P = Q
        P_pre = Q
        max_iterate = 5000
        eps = 0.1
        A = self.A_bar
        B = self.B_bar
        for i in range(max_iterate):
            P = Q + A.T @ P @ A - (A.T @ P @ B) @ np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)
            if abs(P - P_pre).max() < eps:
                print("黎卡提方程迭代次数：", i)  # 输出迭代的次数
                break
            P_pre = P

        self.K = np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)  # K的大小为(1×self.m)

　　7、代入公式求解前馈控制 $\delta _{f}$

    def forward_control_delta_f(self):
        """
        函数：计算前馈控制量delta_f
        self.vehicle_para = (a, b, Cf, Cr, m, Iz)
        K: LQR的输出结果
        self.k_r: 投影点曲率
        vx:车的质心速度在车轴(纵向)方向的分量
        return: 前馈控制量delta_f
        """

        a, b, Cf, Cr, m, Iz = self.vehicle_para
        K_3 = self.K[0][2]
        vx = self.vehicle_vx
        self.delta_f = self.k_r * (a + b - b * K_3 - (m * vx * vx) / (a + b) * (b / Cf + a * K_3 / Cr - a / Cr))
        self.delta_f = self.delta_f * np.pi / 180  # 将前馈控制量转化为弧度形式

　　8、设置Q、R，LQR控制输出转角 $u_{k}$

    def LQR_control(self):
        """
        函数：LQR控制算法
        K: LQR输出
        e_rr: 误差输出
        delta_f: 前馈输出
        return: steer_r
        """

        Q = np.eye(4)
        Q[0][0] = 200
        Q[1][1] = 1
        Q[2][2] = 50
        Q[3][3] = 1
        b = 1
        R = b

        self.vehicle_info()
        self.cal_err_k_r(ts=0.1)
        self.cal_A_B_and_discretion()
        self.cal_LQR_K(Q, R)
        self.forward_control_delta_f()

        steer_r = -np.dot(self.K, np.array(self.err)) + self.delta_f
        steer_r = steer_r[0]

        return steer_r

Lateral_LQR_control = Lateral_LQR_controller(ego_vehicle, vehicle_para, pathway)
steer = Lateral_LQR_control.LQR_control()
print("steer:", steer)

讲解了LQR的求解过程，介绍了前馈控制的作用以及求解 $\delta _{f}$ ，最终完成了结合自动驾驶问题的基于LQR横向控制的代码实现。