基于LQR和前馈控制的自动驾驶控制算法(2.1):离散LQR原理

车队老哥记录生活

已于 2025-06-15 17:02:33 修改

阅读量969

点赞数 14

CC 4.0 BY-SA版权

分类专栏：自动驾驶控制 LQR横向控制器文章标签：自动驾驶

于 2025-05-21 14:08:20 首次发布

本文链接：https://blog.youkuaiyun.com/chen_aoi/article/details/148091441

自动驾驶控制 LQR横向控制器专栏收录该内容

6 篇文章

订阅专栏

前言

本文通关动态规划的思路推导离散LQR的求解方法，本文思路参考离散LQR黎卡提方程推导。
同时也可通关拉格朗日乘子法推导，可参考【基础】自动驾驶控制算法第五讲连续方程的离散化与离散LQR原理。

目标函数

针对离散线性自治系统（时不变系统、与时间无关）：
$X_{k+1}=AX_{k}+Bu_{k} \tag{1}$
其中， $X$ 为系统状态， $u$ 为系统输入。
定义优化的目标函数（代价）为：
$X_N^TPX_N+\sum_{k=1}^{N}{\left(X_k^TQX_k + u_k^TRu_k\right)} \tag{2}$
LQR的目标是求解得最优控制律 $u_k=KX_k$ 来使目标函数最小化。
下面将推导如何得到使目标函数最小化的 $K$ 。

动态规划求解

动态规划: 将复杂问题分解为相互重叠的子问题来求解复杂问题

将目标函数记为新的形式:

最终状态 $k = N$ 的代价（由于 $X_1$ 不受 $u_1$ 影响，故定义 $J_1$ 不包含 $u_1$ ）：
$J_N=X_N^TPX_N$
从最终状态到状态 $k = N - 1$ 的总代价：
$J_{N-1}=X_{N-1}^TQX_{N-1} + u_{N-1}^TRu_{N-1} + J_N = X_{N-1}^TQX_{N-1} + u_{N-1}^TRu_{N-1} + X_N^TPX_N$
通过式 (1) 知 $X_N=AX_{N-1}+Bu_{N-1}$ ，代入上式得：
$J_{N-1}=X_{N-1}^T(A^TPA + Q)X_{N-1} + (AX_{N-1})^TP(Bu_{N-1}) + (Bu_{N-1})^TP(AX_{N-1}) + u_{N-1}^T(B^TPB + R)u_{N-1}$
因为此处 $X$ 和 $u$ 为列向量，且P为对角阵，有：
$\left\{ \begin{aligned} AX &=[x_1~x_2~x_3~...]^T \\ (AX)^T &=[x_1~x_2~x_3~...] \\ Bu &=[u_1~u_2~u_3~...]^T \\ (Bu)^T &=[u_1~u_2~u_3~...] \\ P^T &= P \end{aligned} \right. \Rightarrow (AX_{N-1})^TP(Bu_{N-1}) = (Bu_{N-1})^TP(AX_{N-1})$
有：
$J_{N-1}=X_{N-1}^T(A^TPA + Q)X_{N-1} + 2u_{N-1}^T(B^TPA)X_{N-1} + u_{N-1}^T(B^TPB + R)u_{N-1} \tag{3}$
通过令导数为 0，求 $J_{N-1}$ 取最小值时的输入 $u_{N-1}$ ：
$\frac{\partial{J}_{N-1}}{\partial{u}_{N-1}} = 2(B^TPB + R)u_{N-1} + 2(B^TPA)X_{N-1} = 0$
得：
$u_{N-1} = K_1X_{N-1} \tag{4}$
其中反馈增益 $K_1=-(B^TPB + R)^{-1}(B^TPA)$ 。
当P和R正定或半正定时，有2阶导数 $\ge0$ ，保证导数为 0 时取到的是最小值：
$\frac{\partial^2{J_2}}{\partial{u_1}^2} = 2(B^TPB + R) \ge 0$
此时的控制律 $u_{N-1}=K_1X_{N-1}$ 只能保证 $J_{N-1}$ 最小化。
但通过不断迭代（动态规划），就可以进一步求得使 $J_1$ 最小化的反馈增益（ $J_1$ 为式 (2) ）。
构建动态规划：
将式 (4) 代入式 (3) 得：
$\begin{align*} J_{N-1} &= X_{N-1}^T(A^TPA + Q)X_{N-1} + 2X_{N-1}^TK_1^T(B^TPA)X_{N-1} + X_{N-1}^TK_1^T(B^TPB + R)K_1X_{N-1} \\ &= X_{N-1}^T(A^TPA + Q)X_{N-1} + 2X_{N-1}^TK_1^T(B^TPA)X_{N-1} - X_{N-1}^TK_1^T(B^TPA)X_{N-1} \\ &= X_{N-1}^T(A^TPA + Q + K_1^T(B^TPA))X_{N-1} \end{align*}$
记 $P_1 = A^TPA + Q + K_1^T(B^TPA)$ ，有：
$J_{N-1}=X_{N-1}^TP_1X_{N-1}$
由此， $J_{N-1}$ 转化为了 $J_N$ 的形式，可以类推得到 $J_{N-2}$ 、 $J_{N-3}$ 、…、 $J_1$ 。
类推最终状态到状态 $k = N - 2$ 的总代价：
$J_{N-2}=X_{N-2}^TQX_{N-2} + u_{N-2}^TRu_{N-2} + J_{N-1} = X_{N-2}^TQX_{N-2} + u_{N-2}^TRu_{N-2} + X_{N-1}^TP_1X_{N-1}$
将 $X_{N-1}=AX_{N-2}+Bu_{N-2}$ 代入上式得：
$J_{N-2}=X_{N-2}^T(A^TP_1A + Q)X_2 + 2u_{N-2}^T(B^TP_1A)X_{N-2} + u_{N-2}^T(B^TP_1B + R)u_{N-2}$
同理可求得 $K_2=-(B^TP_1B + R)^{-1}(B^TP_1A)$ ，把 $u_{N-2} = K_2 X_{N-2}$ 代入上式得：
$J_{N-2} = X_{N-2}^T(A^TP_1A + Q + K_2^T(B^TP_1A))X_{N-2}$
记 $P_2 = A^TP_1A + Q + K_2^T(B^TP_1A)$ ，有：
$J_{N-2}=X_{N-2}^TP_2X_{N-2}$
…
类推得到最终状态到状态 $k = 1$ 的总代价：
$J_{1}=X_{1}^TQX_{1} + u_{1}^TRu_{1} + J_{2} = X_{1}^TQX_{1} + u_{1}^TRu_{1} + X_{2}^TP_{N-2}X_{2}$
将 $X_{2}=AX_{1}+Bu_{1}$ 代入上式得：
$J_{1}=X_{1}^T(A^TP_{N-2}A + Q)X_1 + 2u_{1}^T(B^TP_{N-2}A)X_{1} + u_{1}^T(B^TP_{N-2}B + R)u_{1}$
同样可以求得 $K_{N-1}=-(B^TP_{N-2}B + R)^{-1}(B^TP_{N-2}A)$ 。
注意到 $J_1$ 已经包含目标函数式 (2) 中的所有项，故控制律 $u_{1} = K_{N-1}X_1$ 已使目标函数最小化，此时得到LQR的最终解。

迭代流程

设置矩阵 $P$ , $P$ 需为对称矩阵，可令 $P = Q$
计算 $P_1$ ：
$\begin{align*} & \left\{ \begin{aligned} P_1 &= A^TPA + Q + K_1^T(B^TPA) \\ K_1 &=-(B^TPB + R)^{-1}(B^TPA) \end{aligned} \right. \Rightarrow P_1 = A^TPA + Q - A^TPB(B^TPB + R)^{-1}B^TPA \end{align*}$
上式中，每一项都是对称矩阵，故 $P_1^T=P_1$ ，相似地有 $P_k^T=P_k$

矩阵基础：矩阵A的逆的转置等于矩阵A的转置的逆： $A^T)^{-1}=(A^{-1})^T$

同理可计算 $P_2$ 、 $P_3$ 、…：
$\begin{align*} P_2 &= A^TP_1A + Q - A^TP_1B(B^TP_1B + R)^{-1}B^TP_1A \\ P_3 &= A^TP_2A + Q - A^TP_2B(B^TP_2B + R)^{-1}B^TP_2A \\ &\hspace{0.5cm} \vdots \end{align*}$
迭代停止条件：
在LQR的优化中， $\to \infty$ ，但实际中不可能让其无限迭代。
若系统是渐近稳定的， ${X_\infty}=\mathbf{0}$ ，这也意味当 $\to \infty$ ， $P_N$ 趋向于定值（因为总代价 $J_k = X_k^TP_NX_k$ 在系统收敛后不再增加，总代价只与当前状态 $X_k$ 有关）。
故迭代停止条件可设为：
$P_{k+1}-P_{k}|<C$
其中 $C$ 为定值。
若系统纯在稳态误差， ${X_\infty} \ne \mathbf{0}$ ，当 $\to \infty$ ， $P_N$ 可能不会收敛。
故需设置最大迭代次数 $N$ ，认为只优化 $N$ 步的目标函数。
根据迭代得到的 $P_k$ 计算反馈增益：
$\begin{align*} K &=-(B^TP_kB + R)^{-1}(B^TP_kA) \\ u_k &= KX_k \end{align*}$

MATLAB中实现：

function K = LQR(A, B, Q, R, maxIter, eps)
% A、B分别为系统矩阵和输入矩阵，Q和R分别为状态误差和输入的对角权重矩阵
% maxIter为最大迭代步数N，eps为迭代精度C
	i = 1; P = Q; delta = 1e9;
	while i < maxIter && delta > eps
	    Pn = Q + A' * (P - P*B* inv(R+B'*P*B) *B'*P) * A;
	    delta = max(abs(Pn-P), [], "all");
	    P = Pn;
	    i = i+1;
	end
	K = -inv(R + B' * P * B) * B' * P * A;
end