当前位置：首页 > news >正文

基于LQR最优控制算法的车辆轨迹跟踪控制实践

news 2026/5/12 18:24:53

基于LQR最优控制算法实现的轨迹跟踪控制，建立了基于车辆的质心侧偏角、横摆角速度，横向误差，航向误差四自由度动力学模型作为控制模型，通过最优化航向误差和横向误差，实时计算最优的K值，计算期望的前轮转角实现轨迹跟踪，仿真效果良好，有对应的资料，包运行和。

在自动驾驶领域，实现精准的轨迹跟踪控制是关键任务之一。今天来聊一聊基于LQR（线性二次型调节器）最优控制算法达成这一目标的相关内容，顺便带大家看看具体代码实现以及背后的原理。

控制模型建立

我们选用基于车辆的质心侧偏角、横摆角速度，横向误差，航向误差四自由度动力学模型作为控制模型。这个模型能较为全面地反映车辆行驶状态，为后续的控制算法提供基础。

质心侧偏角与横摆角速度

质心侧偏角（$\beta$）和横摆角速度（$\omega_r$）是描述车辆行驶稳定性的重要参数。在车辆动力学中，它们的关系可以通过一系列方程来描述，比如：

\[ \dot{\beta} = \frac{1}{m Vx} ( -2 C{f} (\beta + \frac{lf}{Vx} \omegar) + 2 C{r} (\frac{lr}{Vx} \omega_r - \beta)) \]

\[ \dot{\omegar} = \frac{1}{Iz} ( 2 lf C{f} (\beta + \frac{lf}{Vx} \omegar) - 2 lr C{r} (\frac{lr}{Vx} \omegar - \beta)) \]

这里，$m$ 是车辆质量，$Vx$ 是纵向速度，$Cf$ 和 $Cr$ 分别是前后轮的侧偏刚度，$lf$ 和 $lr$ 分别是质心到前后轴的距离，$Iz$ 是车辆绕 $z$ 轴的转动惯量。

横向误差与航向误差

横向误差（$ey$）和航向误差（$e{\theta}$）用于衡量车辆当前位置与期望轨迹的偏差。假设车辆当前位置为 $(x, y)$，期望轨迹上对应点为 $(xd, yd)$，航向角为 $\theta$，期望航向角为 $\theta_d$，则：

\[ ey = (y - yd) \cos \thetad - (x - xd) \sin \theta_d \]

\[ e{\theta} = \theta - \thetad \]

LQR算法实现轨迹跟踪

LQR的核心思想是通过最优化一个性能指标，实时计算出最优的控制输入。在我们的场景中，就是要最优化航向误差和横向误差，从而实时计算最优的 $K$ 值，来得到期望的前轮转角实现轨迹跟踪。

性能指标

定义性能指标 $J$ 为：

\[ J = \int_{0}^{\infty} ( \mathbf{x}^T \mathbf{Q} \mathbf{x} + \mathbf{u}^T \mathbf{R} \mathbf{u} ) dt \]

其中，$\mathbf{x}$ 是状态向量，包含质心侧偏角、横摆角速度、横向误差、航向误差；$\mathbf{u}$ 是控制输入，这里就是前轮转角；$\mathbf{Q}$ 和 $\mathbf{R}$ 是权重矩阵，通过调整它们可以平衡状态跟踪和控制输入的大小。

代码实现

下面是一段简化的Python代码示例（使用numpy库）来计算LQR的反馈增益 $K$：

import numpy as np def lqr(A, B, Q, R): # 解离散代数Riccati方程 X = np.matrix(np.zeros(A.shape)) i = 0 maxiter = 150 eps = 1e-6 while i < maxiter: Xn = A.T * X * A - A.T * X * B * np.linalg.inv(R + B.T * X * B) * B.T * X * A + Q if (np.linalg.norm(Xn - X) < eps): break X = Xn i = i + 1 K = np.linalg.inv(R + B.T * X * B) * B.T * X * A return K # 假设已经定义好系统矩阵A、B，权重矩阵Q、R A = np.array([[0, 1, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1], [0, 0, 0, -1]]) B = np.array([[0], [1], [0], [1]]) Q = np.diag([1, 1, 1, 1]) R = np.diag([1]) K = lqr(A, B, Q, R) print("计算得到的反馈增益K:", K)