【笔记】运动学LQR横向控制简单推导

原创于 2026-01-06 13:32:01 发布 · 367 阅读

3 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

标签

#算法 #lqr #自动驾驶 #运动学lqr

前言：发现好多以前收藏的文章变成了付费，只能自己写一篇方便随时回忆回忆回忆……

简而言之，LQR的推导过程可分为四部分：

建立状态空间方程
状态方程线性化
连续状态方程离散化
离散方程求解（转化为黎卡提方程）

一、建立单车运动学模型状态空间方程

$\left\{\begin{matrix} \dot{x} = v\cdot cos(\varphi)\\ \dot{y} = v\cdot sin(\varphi)\\ \dot{\varphi} = \frac{v\cdot tan(\delta)}{l} \end{matrix}\right.$

其中， $x$ 、 $y$ 、 $\varphi$ 分别是车辆的横、纵坐标及航向角， $v$ 为纵向车速， $l$ 为轴距， $\delta$ 为前轮转角，也是将来要计算的控制量。

二、状态方程线性化

用求偏导的方式进行梳理

令

$\left\{\begin{matrix} f_{1} = \dot{x} = v\cdot cos(\varphi)\\ f_{2} = \dot{y} = v\cdot sin(\varphi)\\ f_{3} = \dot{\varphi} = \frac{v\cdot tan(\delta)}{l} \end{matrix}\right.$

求偏导

$\begin{bmatrix} d\dot{x}\\ d\dot{y}\\ d\dot{\varphi} \end{bmatrix} = \begin{bmatrix} \frac{\partial f_{1}}{\partial x} & \frac{\partial f_{1}}{\partial y} & \frac{\partial f_{1}}{\partial \varphi} & \frac{\partial f_{1}}{\partial \delta}\\ \frac{\partial f_{2}}{\partial x}& \frac{\partial f_{2}}{\partial y} & \frac{\partial f_{2}}{\partial \varphi} & \frac{\partial f_{2}}{\partial \delta}\\ \frac{\partial f_{3}}{\partial x}& \frac{\partial f_{3}}{\partial y} & \frac{\partial f_{3}}{\partial \varphi} & \frac{\partial f_{3}}{\partial \delta} \\ \end{bmatrix}\begin{bmatrix} dx\\ dy\\ d\varphi\\ d\delta \end{bmatrix}$

$= \begin{bmatrix} \frac{\partial f_{1}}{\partial x} & \frac{\partial f_{1}}{\partial y} & \frac{\partial f_{1}}{\partial \varphi}\\ \frac{\partial f_{2}}{\partial x}& \frac{\partial f_{2}}{\partial y} & \frac{\partial f_{2}}{\partial \varphi}\\ \frac{\partial f_{3}}{\partial x}& \frac{\partial f_{3}}{\partial y} & \frac{\partial f_{3}}{\partial \varphi}\\ \end{bmatrix}\begin{bmatrix} dx\\ dy\\ d\varphi \end{bmatrix}+\begin{bmatrix} \frac{\partial f_{2}}{\partial \delta} \\ \frac{\partial f_{2}}{\partial \delta} \\ \frac{\partial f_{2}}{\partial \delta} \end{bmatrix}d\delta$

令误差 $\dot{e}_{x}=dx$ ， $\dot{e}_{y}=dy$ ， $\dot{e}_{\varphi}=d\varphi$ ， $\dot{e}_{\delta}=d\delta=u$ （ $e_{\delta}$ 表示控制量为前轮转角增量）

$\begin{bmatrix} \dot{e}_{x} \\ \dot{e}_{y} \\ \dot{e}_{\varphi} \end{bmatrix}=\begin{bmatrix} 0 & 0 & -v \cdot sin\varphi\\ 0 & 0 & v \cdot cos\varphi\\ 0 & 0 & 0 \end{bmatrix}\begin{bmatrix} e_{x}\\ e_{y}\\ e_{\varphi} \end{bmatrix}+\begin{bmatrix} 0\\ 0\\ \frac{v}{l \cdot cos\delta^{2}} \end{bmatrix}\cdot u$

于是形成了形如 $\dot{x} = A \cdot x + B \cdot u$ 的线性化方程，其中

$\dot{x} = \begin{bmatrix} \dot{e}_{x} \\ \dot{e}_{y} \\ \dot{e}_{\varphi} \end{bmatrix}$ ， $x =\begin{bmatrix} {e}_{x} \\ {e}_{y} \\ {e}_{\varphi} \end{bmatrix}$ ， $A=\begin{bmatrix} 0 & 0 & -v \cdot sin\varphi\\ 0 & 0 & v \cdot cos\varphi\\ 0 & 0 & 0 \end{bmatrix}$ ， $B=\begin{bmatrix} 0\\ 0\\ \frac{v}{l \cdot cos\delta^{2}} \end{bmatrix}$

$u$ 为控制输入

三、连续方程离散化

$x(t)=x(t-1)+A\cdot x(t-1)\cdot t +B\cdot u\cdot t$

$=(E+A\cdot t)\cdot x(t-1) +B\cdot t\cdot u$

$=\bar{A} \cdot x(t-1) + \bar{B} \cdot u$

其中， $\bar{A} = E+A\cdot t$ ， $\bar{B}=B\cdot t$ ， $t$ 为状态变化周期/控制周期

另一形式：

$\dot{x}=\frac{x(k+1)-x(k)}{T}=A\cdot x(k)+B\cdot u(k)$

$\Rightarrow x(k+1)=(I+T\cdot A)\cdot x(k) +T\cdot B \cdot u(k)$

$\Rightarrow x(k+1)=\bar{A} \cdot x(k) +\bar{B} \cdot u(k)$

其中， $\bar{A} = I +T\cdot A$ ， $\bar{B} = T \cdot B$ ， $T$ 为状态变化周期/控制周期

两种形式均可得

$\bar{A}=\begin{bmatrix} 1 & 0 & -t\cdot v \cdot sin \varphi\\ 0 & 1 & t \cdot v \cdot cos\varphi\\ 0 & 0 & 1 \end{bmatrix}$ ， $\bar{B}=\begin{bmatrix} 0\\ 0\\ \frac{t\cdot v}{l\cdot cos^2\delta} \end{bmatrix}$

四、离散方程求解

求解具体细节可参考《控制之美》书里写的方法，Dr.Can写得很详细，也可以参考这个链接https://zhuanlan.zhihu.com/p/636305927，这里只写简略的过程

考虑LQR的目标函数

$J=\sum_{k=0}^{N}(x_{k}^{T} \cdot Q\cdot x_{k}+u_{k}^{T}\cdot R \cdot u_{k})$

$\Rightarrow \sum_{k=0}^{N-1}(x_{k}^{T}\cdot Q\cdot x_{k}+u_{k}^{T}\cdot R \cdot u_{k}) +x_{N}^{T}\cdot Q_{f} \cdot x_{N}$

其中， $Q$ 为状态权重矩阵，半正定， $R$ 为控制权重矩阵，正定； $Q_{f}$ 为状态权重矩阵的初始值

当 $J$ 最小时，所得 $u$ 即最优，故构造价值函数

$Q_{t}(x_{t},u_{t})=x_{t}^{T}\cdot Q\cdot x_{t} +u_{t}^{T} \cdot R \cdot u_{t}+x_{t+1}^{T}\cdot P_{t+1} \cdot x_{t+1}$ ， $t=N-1, N-2 ... 1,0$

其中 $P_{N}=Q_{f}$ ，将 $x_{k+1}=A \cdot x_{k} +B \cdot u_{k}$ 代入上式可得

$Q_{t}(x_{t},u_{t})=x_{t}^{T}\cdot Q\cdot x_{t} +u_{t}^{T} \cdot R \cdot u_{t}+(Ax_{t}+Bu_{t})^{T}\cdot P_{t+1} \cdot (Ax_{t}+Bu_{t})$

然后对上式使用对 $u$ 求偏导的方式求极小点可得 $t$ 时刻最优解

$u_{t}^*=-(R+B^T \cdot P_{t+1} \cdot B) ^{-1}\cdot B^T \cdot P_{t+1} \cdot A \cdot x_{t}$

又考虑全反馈控制中， $u=-K\cdot x$ ，则

$\left\{\begin{matrix} u_{t}^*=-K_{t}\cdot x_{t}\\ K_{t}=(R+B^T \cdot P_{t+1} \cdot B) ^{-1}\cdot B^T \cdot P_{t+1} \cdot A \end{matrix}\right.$

代入价值函数中可得

$Q_{t}(x_{t},u_{t}^*)=x_{t}^{T}(Q+A^TP_{t+1}A-A^TP_{t+1}B(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A)x_{t}=x_{t}^TP_{t}x_{t}$

其中 $x_{t}^TP_{t}x_{t}$ 为 $t$ 时刻最小代价，且

$P_{t}=Q+A^TP_{t+1}A-A^TP_{t+1}B(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A$

至此，将求解最优控制量转化为求 $K$ 之后又转化为求 $P$ 。

五、实际使用须知

1、经上述方式求解的控制量 $u$ 实际只是增量控制量，需配合前馈（通常与目标点的曲率相关）食用，LQR也可以说是一种对前馈进行调节的控制方法

2、Q矩阵取值越大，误差的影响越大；R取值越大，控制量的变化越小，平衡两者的取值很重要，可以把两个矩阵的比例作为一种调参的标准

3、LQR对轨迹的曲率连续性要求很高，也就是轨迹点的二阶连续性，如果轨迹曲率变化较大，那么LQR很容易震荡而且无法收敛

4、自动驾驶用LQR要加很多很多滤波，输入的误差、输出的控制量、前馈等都要各种滤波保障一定的稳定性，避免突变，否则很容易跑飞

后记：本篇既是给自己记下LQR的思路，也希望能方便需要的伙伴能对LQR求解有个大体了解，欢迎讨论，轻喷。