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

自动驾驶控制算法-横向误差微分方程LQR前馈控制

news 来源：原创 2025/5/24 0:35:58

本文是学习自动驾驶控制算法第六讲前馈控制与航向误差以及前两节的学习笔记。

1 横向误差微分方程

以规划的轨迹作为自然坐标系，计算自车在轨迹上的投影点，进而计算误差：
在这里插入图片描述
如图所示，横向误差为 $d$ ，航向误差为 $\theta-\theta_r$ ，投影点的速度大小为 $\dot{s}$ ，注意这里的 $\theta$ 是航向角，与横摆角 $\varphi$ 相差一个侧偏角 $\beta$
$\begin{equation} \theta=\varphi+\beta \end{equation}$
根据之前所介绍的笛卡尔坐标系与自然坐标系的转换关系可知：
$\begin{equation} \dot{d}=v\sin(\theta-\theta_r) \end{equation}$
$\begin{equation} \dot{s}=\frac{v\cos(\theta-\theta_r)}{1-k_rd} \end{equation}$
这里 $k_r$ 是投影点处的曲率。
结合式1和2
$\begin{equation} \dot{d}=v\sin(\varphi+\beta-\theta_r)=v\sin{\beta}\cos{(\varphi-\theta)}+v\cos{\beta}\sin{(\varphi-\theta)} \end{equation}$
$\varphi-\theta_r$ 为小量，所以上式进一步简化为
$\begin{equation} \dot{d}=v_y+v_x(\varphi-\theta_r) \end{equation}$
令
$\begin{equation} e_d=d \end{equation}$
$\begin{equation} e_{\varphi}=\varphi-\theta_{r} \end{equation}$
求一阶二阶导数则有
$\begin{equation} \dot{e_d}=v_xe_{\varphi}+v_y \end{equation}$
假设 $v_x$ 是常数
$\begin{equation} \dot{v_y}=\ddot{e_d}-v_x\dot{e_{\varphi}} \end{equation}$
$\begin{equation} \ddot{e}_{\varphi}=\ddot{\varphi}-\ddot{\theta}_{r}≈\ddot{\varphi} \end{equation}$
这里 $\ddot{\theta}_r$ 约等于0，是因为轨迹通常比较平滑。
综合可得
$\begin{equation} \begin{cases} v_y=\dot{e}_d-v_xe_{\varphi}\\ \dot{v}_y=\ddot{e}_d-v_x\dot{e}_{\varphi} \\ \dot{\varphi}=\dot{e}_{\varphi}+\dot{\theta}_r \\ \ddot{\varphi}=\ddot{e}_{\varphi} \end{cases} \end{equation}$
由上节的公式：
$\begin{equation} \begin{bmatrix} \dot{v_y} \\ \ddot{\varphi} \end{bmatrix}= \begin{bmatrix} \frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} & \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x \\ \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x} & \frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix} \begin{bmatrix} v_y \\ \dot{\varphi} \end{bmatrix}+ \begin{bmatrix} -\frac{C_{\alpha{f}}}{m} \\ -\frac{aC_{\alpha{f}}}{I} \end{bmatrix}\delta \end{equation}$
结合式11和式12可得
$\begin{equation} \ddot{e}_d=\frac{C_{\alpha{f}}+C_{\alpha{r}}}{mv_x} \dot{e}_d+(-\frac{C_{\alpha{f}}+C_{\alpha{r}}}{m})e_{\varphi}+\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}\dot{e}_{\varphi}+(\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x)\dot{\theta}_r+(-\frac{C_{\alpha{f}}}{m})\delta \end{equation}$
$\begin{equation} \ddot{e}_{\varphi}=\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{Iv_x} \dot{e}_d+(-\frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{I})e_{\varphi}+\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x}\dot{e}_{\varphi}+(\frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x})\dot{\theta}_r+(-\frac{aC_{\alpha{f}}}{I})\delta \end{equation}$
进而有
$\begin{equation} \begin{bmatrix} \dot{e}_d \\ \ddot{e}_{d} \\ \dot{e}_{\varphi} \\ \ddot{e}_{\varphi} \end{bmatrix}= \begin{bmatrix} 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^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix} \begin{bmatrix} e_d \\ \dot{e}_d \\ e_{\varphi} \\ \dot{e}_{\varphi} \end{bmatrix}+ \begin{bmatrix} 0\\ -\frac{C_{\alpha{f}}}{m} \\ 0 \\ -\frac{aC_{\alpha{f}}}{I} \end{bmatrix}\delta+ \begin{bmatrix} 0 \\ \frac{aC_{\alpha{f}}-bC_{\alpha{r}}}{mv_x}-v_x \\ 0 \\ \frac{a^2C_{\alpha{f}}+b^2C_{\alpha{r}}}{Iv_x} \end{bmatrix}\dot{\theta}_r \end{equation}$
$\begin{equation} \dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_r \end{equation}$

2 LQR原理

对于式16，先暂时不考虑最后一项，那么有
$\begin{equation} \dot{e}_{rr}=Ae_{rr}+Bu \end{equation}$
目的是选择合适的 $u$ 使得 $|\boldsymbol{\bar{e}}_{\boldsymbol{rr}}|$ 尽可能小，也即式
$\begin{equation} J=w_a{\boldsymbol{e}}^2_{\boldsymbol{rr}}+w_bu^2 \end{equation}$
尽可能小，进一步也即
$\begin{equation} J={\boldsymbol{e}}^T_{\boldsymbol{rr}}Q{\boldsymbol{e}}_{\boldsymbol{rr}}+u^TRu \end{equation}$
尽可能小，其中 $Q$ 和 $R$ 是对角矩阵，问题就变成了在式17的约束下使 $J$ 取最小值。

2.1 连续方程离散化

式17写成一般形式
$\begin{equation} \dot{x}=Ax+Bu \end{equation}$
上式两边积分
$\begin{equation} \int_t^{t+dt}\dot{x}(\tau)d\tau=\int_t^{t+dt}Ax(\tau)d\tau+\int_t^{t+dt}Bu(\tau)d\tau \end{equation}$
得到
$\begin{equation} x(t+dt)-x(t)=Ax(\xi)dt+Bu(\xi)dt \end{equation}$
对 $A(\xi)$ 采用中值欧拉法，对 $u(\xi)$ 采用向前欧拉法（因为 $u (t + d t)$ 未知）得到：
$\begin{equation} x(t+dt)=x(t)+Adt(\frac{x(t+dt)+x(t)}{2})+Bu(t)dt \end{equation}$
$\begin{equation} (I-\frac{Adt}{2})x(t+dt)=(I+\frac{Adt}{2})x(t)+Bu(t)dt \end{equation}$
$\begin{equation} \begin{split} x(t+dt) &= (I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})x(t)+(I-\frac{Adt}{2})^{-1}Bdtu(t) \\ &≈(I-\frac{Adt}{2})^{-1}(I+\frac{Adt}{2})x(t)+Bdtu(t) \end{split} \end{equation}$
$\begin{equation} x_{k+1}=\bar{A}x_k+\bar{B}{u_k} \end{equation}$

2.2 LQR

问题就是在式26的约束下，求 $u$ 使式
$\begin{equation} J=\sum_{k=1}^\infty{x^T_{k}Qx_k}+u^T_kRu_k \end{equation}$
取得最小值。
$u$ 的形式为
$\begin{equation} u=-Kx \end{equation}$
$\begin{equation} K=(R+\bar{B}^TP\bar{B})^{-1}\bar{B}^TP\bar{A} \end{equation}$
其中其 $P$ 就是离散时间 $R i cc a t i$ 方程
$\begin{equation} P=Q+\bar{A}^TP\bar{A}-\bar{A}^TP\bar{B}(R+\bar{B}^TP\bar{B})^{-1}\bar{B}^TP\bar{A} \end{equation}$
的解。

3 前馈控制与航向误差

对于式16，如果使用上一节的LQR结果（式28、29），
$\begin{equation} \dot{e}_{rr}=(A-BK)e_{rr}+C\dot{\theta}_r \end{equation}$
无论 $K$ 取何值， $\dot{e}_{rr}$ 和 ${e}_{rr}$ 不可能同时为0，那么 ${e}_{rr}$ 也就不会为0，系统存在稳态误差
引入前馈控制消除稳态误差
$\begin{equation} u=-Kx+\delta_f \end{equation}$
在这里插入图片描述
$\begin{equation} \dot{e}_{rr}=(A-BK)e_{rr}+B\delta_f+C\dot{\theta}_r \end{equation}$
系统稳定后， $\dot{e}_{rr}=0$
$\begin{equation} e_{rr}=-(A-BK)^{-1}(B\delta_f+C\dot{\theta}_r) \end{equation}$
选取合适的 $\delta_f$ ，使 ${e}_{rr}$ 尽可能接近0。
式34展开后得：
$\begin{equation} e_{rr}= \begin{bmatrix} \frac{1}{k_1}\{\delta_f-\frac{\dot{\theta}_r}{v_x}[a+b-bk_3-\frac{mv^2_x}{a+b}(\frac{b}{c_f}+\frac{a}{c_r}k_3-\frac{a}{c_r})]\} \\ 0\\ -\frac{\dot{\theta}_r}{v_x}(b+\frac{a}{a+b}\frac{mv^2_x}{c_{\alpha{f}}})\\ 0 \end{bmatrix} \end{equation}$
可知当
$\begin{equation} \delta_f=\frac{\dot{\theta}_r}{v_x}[a+b-bk_3-\frac{mv^2_x}{a+b}(\frac{b}{c_f}+\frac{a}{c_r}k_3-\frac{a}{c_r})] \end{equation}$
时， ${e}_{d}$ 等于0，其中 $k_3$ 是反馈 $K$ 中的第三个元素。
通过一系列化简，式35的第三个元素可近似等于 $-\beta$ ，即
$\begin{equation} e_{\varphi}=-\beta \end{equation}$
因为目的是 $\theta-\theta_r=0$ ，那么 ${e}_{\varphi}$ 的稳态误差刚好就是 $-\beta$ 。

1 横向误差微分方程

2 LQR原理

2.1 连续方程离散化

2.2 LQR

3 前馈控制与航向误差

相关文章：