LQR的横向控制与算法仿真实现

文章目录

- 1. 引言
- 2. 车辆运动学线性离散模型
- 3. LQR求解
- 4. 算法和仿真实现

1. 引言

在现代控制理论的领域中，线性二次型调节器（Linear Quadratic Regulator，简称LQR）被广泛认可为一种高效的优化控制方法。LQR的核心优势在于其能力，通过最小化一个定义良好的二次型代价函数，来设计出能够引导系统达到预定性能指标的控制策略。尽管LQR最初是为线性时不变系统（Linear Time-Invariant, LTI）设计的，但其在稳定性和性能优化方面的卓越表现，已经使得它在航空航天、机器人技术、汽车工业等多个高端技术领域得到了广泛应用。

在车辆横向控制的具体应用场景中，我们面临车辆运动学模型的非线性特性的挑战。为了克服这一难题，我们通常采用线性化技术，将非线性模型转化为线性近似模型，从而使得LQR方法得以应用。此外，为了适应计算机控制系统的实现需求，模型的离散化处理也成为了一个不可或缺的步骤。通过将连续时间模型转换为离散时间模型，我们可以有效地利用LQR算法进行控制设计，实现对车辆横向运动的精确控制。

2. 车辆运动学线性离散模型

在车辆运动学模型的线性化和离散化及代码实现中，我们详细介绍了单车模型的线性化和离散化，其离散线性化后的微分方程如下
$\begin{align*} X_e(k+1)&=(TA+I)AX_e(k)+TBu_e(k)\\ &= \begin{bmatrix} 1 & 0 & -Tv_rsin\varphi_r\\ 0 & 1 & Tv_rcos\varphi_r\\ 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} x-x_r\\ y-y_r\\ \varphi-\varphi_r\\ \end{bmatrix} + \begin{bmatrix} Tcos\varphi_r & 0 \\ Tsin\varphi_r & 0 \\ \frac{Tv_r tan\delta_{fr}}{L} & \frac{Tv_r}{Lcos^2\delta_{fr}} \\ \end{bmatrix} \begin{bmatrix} v-v_r\\ \delta-\delta_r\\ \end{bmatrix} \\ \end{align*} \tag{1}$
其中 $T$ 为采样步长， $I$ 为3x3的单位矩阵。

这里的 $(T A + I) A$ 为该系统的控制矩阵， $TB$ 为输入矩阵， $u_e(k)$ 为输入控制量误差，状态 $X_e(K+1)$ 为状态误差，在控制过程中，我们期望状态误差逐渐稳定趋近为0，因此，定义代价函数
$\sum_{k=0}^{N-1} (x_k^T Q x_k + u_k^T R u_k) + x_N^T F x_N \tag{2}$
其中：

$x_k$ 是在离散时间步 $k$ 的系统状态。
$u_k$ 是在时间步 $k$ 的控制输入。
$Q$ 是状态权重 $\times m$ 的半正定方阵，用于衡量状态的代价，通常将其设计为对角矩阵。
$R $是控制权重$ n \times n$的正定对称矩阵，用于衡量控制输入的代价，通常将其设计为对角矩阵。
$F$ 是末端状态权重 $\times m$ 的半正定方阵，用于衡量最终状态的代价，通常将其设计为对角矩阵。
$N$ 是控制的总时间步数。

3. LQR求解

采用LQR算法进行控制率求解的步骤（推导过程详见LQR求解推导及代码实现）概括为：

确定迭代范围 $N$ 。
设置迭代初始值 $P_{N}=F$ ，其中 $Q_f=Q$
循环迭代，从后往前 $\ldots, 0$ ，
$\begin{align*} K_{k}&=(B^TP_{k+1}B + R)^{-1}B^TP_{k+1}A\\ P_{k}&=(A-BK_{k})^T P_{k+1} (A-BK_{k}) + Q + K_{k}^T R K_{k} \end{align*}$

判断 $K_k$ 和 $K_{k+1}$ 每个对应元素的差值是否小于 $\epsilon$ （这里 $\epsilon$ 代表迭代精度，一般是非常小的数字），如果都小于则跳出循环，此时的 $K_t$ 即为最终的最优反馈矩阵，否则继续循环。
最终得优化的控制量 $u_{t}^{*}=-K_{t} x_{t}$

4. 算法和仿真实现

这里我们将权重矩阵 $Q$ 、 $R$ 、 $F$ 分别设为
$Q=\begin{bmatrix} 8 & 0 & 0\\ 0 & 8 & 0\\ 0 & 0 & 8\\ \end{bmatrix} \\ R=\begin{bmatrix} 2 & 0 & 0\\ 0 & 2 & 0\\ 0 & 0 & 2\\ \end{bmatrix} \\ F=\begin{bmatrix} 10 & 0 & 0\\ 0 & 10 & 0\\ 0 & 0 & 10\\ \end{bmatrix} \tag{3}$
实际使用过程可以根据需要动态调整相关权重。其具体实现如下

kinematicsLQR.py

import numpy as np
import math
from scipy.linalg import inv
from kinematic_bicycle_model import update_ABMatrix


N = 200  # 迭代范围
EPS = 1e-4  # 迭代精度
Q = np.eye(3) * 8
R = np.eye(2) * 2
F = np.eye(3) * 10


def cal_lqr_k(A, B, Q, R, F):
    """计算LQR反馈矩阵K
    Args:
        A : mxm状态矩阵A
        B : mxn状态矩阵B
        Q : Q是状态权重mxm的半正定方阵，用于衡量状态的代价，通常将其设计为对角矩阵。
        R : R是控制权重nxn的正定对称矩阵，用于衡量控制输入的代价，通常将其设计为对角矩阵。
        F : F是末端状态权重mxm的半正定方阵，用于衡量最终状态的代价，通常将其设计为对角矩阵。
    Returns:
        K : 反馈矩阵K
    """
    # 设置迭代初始值
    P = F
    # 循环迭代
    for t in range(N):
        K_t = inv(B.T @ P @ B + R) @ B.T @ P @ A
        P_t = (A - B @ K_t).T @ P @ (A - B @ K_t) + Q + K_t.T @ R @ K_t
        if (abs(P_t - P).max() < EPS):
            break
        P = P_t
    return K_t


def normalize_angle(angle):
    a = math.fmod(angle + np.pi, 2 * np.pi)
    if a < 0.0:
        a += (2.0 * np.pi)
    return a - np.pi


def calc_preparation(vehicle, ref_path):
    """
    计算角度误差theta_e、横向误差er、曲率rk和索引index
    """

    rx, ry, ref_yaw, ref_kappa = ref_path[:, 0], ref_path[:, 1], ref_path[:, 2], ref_path[:, 4]
    dx = [vehicle.x - icx for icx in rx]
    dy = [vehicle.y - icy for icy in ry]
    d = np.hypot(dx, dy)
    index = np.argmin(d)
    rk = ref_kappa[index]
    ryaw = ref_yaw[index]
    rdelta = math.atan2(vehicle.L * rk, 1)

    vec_nr = np.array([math.cos(ryaw + math.pi / 2.0),
                       math.sin(ryaw + math.pi / 2.0)])

    vec_target_2_rear = np.array([vehicle.x - rx[index],
                                  vehicle.y - ry[index]])

    er = np.dot(vec_target_2_rear, vec_nr)
    theta_e = normalize_angle(vehicle.yaw - ryaw)

    return dx[index], dy[index], theta_e, er, rdelta, ryaw, index


def LQRController(vehicle, ref_path):
    x_e, y_e, theta_e, er, rdelta, ryaw, index = calc_preparation(vehicle, ref_path)
    x = np.matrix([[x_e],
                   [y_e],
                   [theta_e]])
    A, B = update_ABMatrix(vehicle, rdelta, ryaw)
    K = cal_lqr_k(A, B, Q, R, F)

    u = -K @ x
    delta_f = rdelta + u[1,0]
    return delta_f, index, er

kinematic_bicycle_model.py

import math
import numpy as np

class Vehicle:
    def __init__(self,
                 x=0.0,
                 y=0.0,
                 yaw=0.0,
                 v=0.0,
                 dt=0.1,
                 l=3.0):
        self.steer = 0
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.dt = dt
        self.L = l  # 轴距
        self.x_front = x + l * math.cos(yaw)
        self.y_front = y + l * math.sin(yaw)

    def update(self, a, delta, max_steer=np.pi):
        delta = np.clip(delta, -max_steer, max_steer)
        self.steer = delta

        self.x = self.x + self.v * math.cos(self.yaw) * self.dt
        self.y = self.y + self.v * math.sin(self.yaw) * self.dt
        self.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dt

        self.v = self.v + a * self.dt
        self.x_front = self.x + self.L * math.cos(self.yaw)
        self.y_front = self.y + self.L * math.sin(self.yaw)


class VehicleInfo:
    # Vehicle parameter
    L = 2.0  # 轴距
    W = 2.0  # 宽度
    LF = 2.8  # 后轴中心到车头距离
    LB = 0.8  # 后轴中心到车尾距离
    MAX_STEER = 0.6  # 最大前轮转角
    TR = 0.5  # 轮子半径
    TW = 0.5  # 轮子宽度
    WD = W  # 轮距
    LENGTH = LB + LF  # 车辆长度

def draw_vehicle(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):
    vehicle_outline = np.array(
        [[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],
         [vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])

    wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],
                      [vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2,
                       vehicle_info.TW / 2]])

    rr_wheel = wheel.copy()  # 右后轮
    rl_wheel = wheel.copy()  # 左后轮
    fr_wheel = wheel.copy()  # 右前轮
    fl_wheel = wheel.copy()  # 左前轮
    rr_wheel[1, :] += vehicle_info.WD / 2
    rl_wheel[1, :] -= vehicle_info.WD / 2

    # 方向盘旋转
    rot1 = np.array([[np.cos(steer), -np.sin(steer)],
                     [np.sin(steer), np.cos(steer)]])
    # yaw旋转矩阵
    rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],
                     [np.sin(yaw), np.cos(yaw)]])
    fr_wheel = np.dot(rot1, fr_wheel)
    fl_wheel = np.dot(rot1, fl_wheel)
    fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])
    fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])

    fr_wheel = np.dot(rot2, fr_wheel)
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    fl_wheel = np.dot(rot2, fl_wheel)
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rr_wheel = np.dot(rot2, rr_wheel)
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    rl_wheel = np.dot(rot2, rl_wheel)
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y
    vehicle_outline = np.dot(rot2, vehicle_outline)
    vehicle_outline[0, :] += x
    vehicle_outline[1, :] += y

    ax.plot(fr_wheel[0, :], fr_wheel[1, :], color)
    ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)
    ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)
    ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)

    ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)
    ax.axis('equal')


def update_ABMatrix(vehicle, ref_delta, ref_yaw):
    """
    计算离散线性车辆运动学模型状态矩阵A和输入矩阵B
    return: A, b
    """
    A = np.matrix([
        [1.0, 0.0, -vehicle.v * vehicle.dt * math.sin(ref_yaw)],
        [0.0, 1.0, vehicle.v * vehicle.dt * math.cos(ref_yaw)],
        [0.0, 0.0, 1.0]])

    B = np.matrix([
        [vehicle.dt * math.cos(ref_yaw), 0],
        [vehicle.dt * math.sin(ref_yaw), 0],
        [vehicle.dt * math.tan(ref_delta) / vehicle.L,
         vehicle.v * vehicle.dt / (vehicle.L * math.cos(ref_delta) * math.cos(ref_delta))]])
    return A, B


def update_ABMatrix1(vehicle, ref_delta, ref_yaw):
    """将模型离散化后的状态空间表达

    Args:
        delta (_type_): 参考输入

    Returns:
        _type_: _description_
    """
    A = np.matrix([
        [1.0, 0.0, -vehicle.v * vehicle.dt * math.sin(ref_yaw)],
        [0.0, 1.0, vehicle.v * vehicle.dt * math.cos(ref_yaw)],
        [0.0, 0.0, 1.0]])

    B = np.matrix([
        [vehicle.dt * math.cos(ref_yaw), 0],
        [vehicle.dt * math.sin(ref_yaw), 0],
        [vehicle.dt * math.tan(ref_delta) / vehicle.L,
         vehicle.v * vehicle.dt / (vehicle.L * math.cos(ref_delta) * math.cos(ref_delta))]
    ])
    return A, B

path_generator.py

"""
路径轨迹生成器
"""

import math
import numpy as np

class Path:
    def __init__(self):
        self.ref_line = self.design_reference_line()
        self.ref_yaw = self.cal_yaw()
        self.ref_s = self.cal_accumulated_s()
        self.ref_kappa = self.cal_kappa()

    def design_reference_line(self):
        rx = np.linspace(0, 50, 1000) + 5 # x坐标
        ry = 20 * np.sin(rx / 20.0) + 60  # y坐标
        return np.column_stack((rx, ry))
    def cal_yaw(self):
        yaw = []
        for i in range(len(self.ref_line)):
            if i == 0:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i, 1],
                                 self.ref_line[i + 1, 0] - self.ref_line[i, 0]))
            elif i == len(self.ref_line) - 1:
                yaw.append(math.atan2(self.ref_line[i, 1] - self.ref_line[i - 1, 1],
                                 self.ref_line[i, 0] - self.ref_line[i - 1, 0]))
            else:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i -1, 1],
                                  self.ref_line[i + 1, 0] - self.ref_line[i - 1, 0]))
        return yaw

    def cal_accumulated_s(self):
        s = []
        for i in range(len(self.ref_line)):
            if i == 0:
                s.append(0.0)
            else:
                s.append(math.sqrt((self.ref_line[i, 0] - self.ref_line[i-1, 0]) ** 2
                                 + (self.ref_line[i, 1] - self.ref_line[i-1, 1]) ** 2))
        return s

    def cal_kappa(self):
        # 计算曲线各点的切向量
        dp = np.gradient(self.ref_line.T, axis=1)
        # 计算曲线各点的二阶导数
        d2p = np.gradient(dp, axis=1)
        # 计算曲率
        kappa = (d2p[0] * dp[1] - d2p[1] * dp[0]) / ((dp[0] ** 2 + dp[1] ** 2) ** (3 / 2))

        return kappa

    def get_ref_line_info(self):
        return self.ref_line[:, 0], self.ref_line[:, 1], self.ref_yaw, self.ref_s, self.ref_kappa

main.py

from kinematic_bicycle_model import Vehicle, VehicleInfo, draw_vehicle
from kinematicsLQR import LQRController
from path_generator import Path
import numpy as np
import matplotlib.pyplot as plt
import imageio.v2 as imageio

MAX_SIMULATION_TIME = 200.0  # 程序最大运行时间200*dt

def main():
    # 设置跟踪轨迹
    rx, ry, ref_yaw, ref_s, ref_kappa = Path().get_ref_line_info()
    ref_path = np.column_stack((rx, ry, ref_yaw, ref_s, ref_kappa))
    # 假设车辆初始位置为（5，60），航向角yaw=0.0，速度为2m/s，时间周期dt为0.1秒
    vehicle = Vehicle(x=5.0,
                      y=60.0,
                      yaw=0.0,
                      v=2.0,
                      dt=0.1,
                      l=VehicleInfo.L)

    time = 0.0  # 初始时间
    target_ind = 0
    # 记录车辆轨迹
    trajectory_x = []
    trajectory_y = []
    lat_err = []  # 记录横向误差

    i = 0
    image_list = []  # 存储图片
    plt.figure(1)

    last_idx = ref_path.shape[0] - 1  # 跟踪轨迹的最后一个点的索引
    while MAX_SIMULATION_TIME >= time and last_idx > target_ind:
        time += vehicle.dt  # 累加一次时间周期

        # rear_wheel_feedback
        delta_f, target_ind, e_y = LQRController(vehicle, ref_path)

        # 横向误差
        lat_err.append(e_y)

        # 更新车辆状态
        vehicle.update(0.0, delta_f, np.pi / 10)  # 由于假设纵向匀速运动，所以加速度a=0.0
        trajectory_x.append(vehicle.x)
        trajectory_y.append(vehicle.y)

        # 显示动图
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
        draw_vehicle(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)

        plt.plot(trajectory_x, trajectory_y, "-r", label="trajectory")
        plt.plot(ref_path[target_ind, 0], ref_path[target_ind, 1], "go", label="target")
        plt.axis("equal")
        plt.grid(True)
        plt.pause(0.001)
    #     plt.savefig("temp.png")
    #     i += 1
    #     if (i % 5) == 0:
    #         image_list.append(imageio.imread("temp.png"))
    #
    # imageio.mimsave("display.gif", image_list, duration=0.1)

    plt.figure(2)
    plt.subplot(2, 1, 1)
    plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
    plt.plot(trajectory_x, trajectory_y, 'r')
    plt.title("actual tracking effect")

    plt.subplot(2, 1, 2)
    plt.plot(lat_err)
    plt.title("lateral error")
    plt.show()


if __name__ == '__main__':
    main()