基于Carla的EMplanner教程一：动力学模型到LQR

Carla教程一、动力学模型到LQR

从运动学模型和动力学模型到LQR

模型就是可以描述车辆运动规律的模型。车辆建模都是基于自行车模型的设定，也就是将四个轮子抽象为自行车一样的两个轮子来建模。

1、运动学模型

运动学模型是基于几何关系分析出来的，一般适用于低俗情况下，这种情况可以不考虑轮胎的侧偏特性。另外，这里假设前轮的方向就是当前车辆速度的方向。但是高速情况并非如此。整个控制量可以简化为加速度a和偏航角$\delta$。其中油门代表a为正，刹车代表a为负。偏航角是方向盘的控制量。然后可以定义下面的几个状态量，用来描述车辆当前状态：

• x：车辆当前横轴坐标
• y：车辆纵轴坐标
• v: 车辆速度
• $\phi$：车辆横摆角
  

最后推导得到的动力学模型如下：
$$\begin{gathered} \dot{x}=v\cos \phi \\ \dot{y}=v\sin \phi \\ \dot{\phi }=\frac{v\tan {\delta }_f}{L} \end{gathered}$$
其中${\delta }_f$是前轮的转向角。因为后轮一般都是不转向的，所以${\delta }_r$是零。

2、动力学模型

需要考虑轮胎的侧偏性质。动力学模型是通过对轮胎和地面之间的相互作用来描述车辆的运动。动力学模型是基于车辆的动力学原理进行建模的，它考虑了车辆的质量、加速度、速度、转向等因素，可以更加精确地预测车辆的运动轨迹。动力学模型的优点在于可以考虑更多的因素，更加精确地预测车辆的运动轨迹，适用于高速行驶和紧急情况下的自动驾驶。但是，动力学模型对传感器的精度要求较高，对于复杂的路况和车辆之间的交互，需要更多的计算资源。

动力学模型可以将车辆运动的横纵向解耦，而运动学模型则无法解耦。

根据自行车动力学模型的推导，注意动力学模型中一个很关键的参数就是轮胎测偏参数，轮胎测偏力和轮胎测偏角度成正比。在实际中，测偏刚度是负数。

经过推导可以计算出关于横向速度和角速度作为状态变量，横向加速度和横向角速度变化量为因变量的状态方程。如下：
$$\left(\begin{array}{c}\ddot{y}\\ \ddot{\phi }\end{array}\right)=(\begin{array}{c}\frac{C_{{\phi }_f}+C_{{\phi }_r}}{m{v}_x}\\ \frac{a{C}_{{\phi }_f}-b{C}_{{\phi }_r}}{I{v}_x}\end{array}\begin{array}{c}\frac{a{C}_{{\phi }_f}+b{C}_{{\phi }_r}}{m{v}_x}-v_x\\ \frac{a^2{C}_{{\phi }_f}+b^2{C}_{{\phi }_r}}{I{v}_x}\end{array})(\begin{array}{c}\dot{y}\\ \dot{\phi }\end{array})+\left(\begin{array}{c}-\frac{C_{{\phi }_f}}{m}\\ -\frac{a{C}_{\phi f}}{I}\end{array}\right)\delta$$
从公式中能看到，通过控制delta转向角来实现控制角速度和y轴方面的加速度。delta代表的方向盘的转角，也就是控制量。

规划模块获得了$(x_r,y_r,v_r,{\theta }_r,a_r)$,这些都是参考信息，是希望车辆能够达到的状态。因此需用通过车辆当前的状态$(x_{},y,v,\theta ,a)$。然后计算如下误差：

• 纵向误差
• 横向误差
• 航向误差
• 速度误差
• 加速度误差

但是首先需要将$(x_r,y_r,v_r,{\theta }_r,a_r)$和$(x_{},y,v,\theta ,a)$转化为frenet坐标系信息。然后计算基于Frenet坐标系下的误差。

所以，控制器的目标就是让$x_r$和$x$尽可能接近，也就是误差$e_{rr}$最小。

所以可以构造一个损失函数$J$，如下：
$$J=e_{rr}^2+u^2$$
其中$u$是为了控制更加平顺。写成二次型如下：
$$J={e_{rr}}^{T}Q{e}_{rr}+u^{T}Ru$$
变成了目标$J$在约束条件：${\dot{e}}_{rr}=\bar{A}{e}_{rr}+\bar{B}u$ 下的最小求解。这就是所谓的LQR(线性二次调节器)

根据动力学公式推导，找到这个约束条件中的A和B。得到的公式如下：
$$\left(\begin{array}{c}{\dot{e}}_d\\ {\ddot{e}}_d\\ {\dot{e}}_{\alpha }\\ {\ddot{e}}_{\alpha }\end{array}\right)=\left(\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{C_{\phi f}+C_{\phi r}}{m{v}_x}& -\frac{C_{\phi f}+C_{\phi r}}{m}& \frac{a{C}_{\phi f}-b{C}_{\phi r}}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{a{C}_{\phi f}-b{C}_{\phi r}}{I{v}_x}& \frac{-a{C}_{\phi f}-b{C}_{\phi r}}{I}& \frac{a^2{C}_{\phi f}+b^2{C}_{\phi r}}{I{v}_x}\end{array}\right)\left(\begin{array}{c}{e}_d\\ {\dot{e}}_d\\ e_{\phi }\\ {\dot{e}}_{\phi }\end{array}\right)+\left(\begin{array}{c}0\\ -\frac{C_{\phi f}}{m}\\ 0\\ -\frac{a{C}_{\phi f}}{I}\end{array}\right)\delta +\left(\begin{array}{c}0\\ \frac{a{C}_{\phi f}-b{C}_{\phi r}}{m{v}_x}-v_x\\ 0\\ \frac{a^2{C}_{\phi f}+b^2{C}_{\phi r}}{I{v}_x}\end{array}\right){\dot{\theta }}_r$$
要求解J在上述约束下的解，得需要将其离散化，然后可用拉格朗日乘子法就可以直接求解。

补充一下拉格朗日乘子法的知识：

• 拉格朗日乘子法（Lagrange multipliers）是一种寻找多元函数在一组约束下的极值的方法。

• 通过引入拉格朗日乘子，可将有 d个变量与 k 个约束条件的最优化问题转化为具有 d+k 个变量的无约束优化问题求解。

离散化以后的称之为DLQR, 离散LQR只涉及到拉格朗日乘子法，问题变成了$J={\sum }_{k=0}^{+\infty }\left(x_K^{T}Q{x}_k+u_k^{T}R{u}_k\right)$在约束$x_{k+1}=\bar{A}{x}_k+\bar{B}{u}_k$的求解极小值。

3、LQR

问题一、如何将状态方程离散化

现在要将$\dot{x}=\dot{A}x+\dot{B}u$离散化，有前向欧拉法、中点欧拉法和后向欧拉法。

欧拉法回顾：

如果要对$\dot{x}=\dot{A}x$进行离散化，将其左右同时积分，结果如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HNOgDUhH-1690441990297)(E:\文档\A校招面试准备内\images\oula.PNG)]

对于$\dot{x}=\dot{A}x+\dot{B}u$离散化，采用重点欧拉法和向前欧拉法结合，最后得到的$x_{k+1}=\bar{A}{x}_k+\bar{B}{u}_k$,其中：
$$\begin{gathered} \bar{A}={\left(I-\frac{Adt}{2}\right)}^{-1}\left(I+\frac{Adt}{2}\right) \\ \bar{B}=Bdt \end{gathered}$$
dt可以设置为0.01。

问题二、如何求解DLQR？

推导过程过于复杂，此处战且不表，结论如下：

对于状态方程：${\dot{e}}_{rr}=A{e}_{rr}+Bu$

1. 离散化：$e_{rr}\left( k+1 \right) ,,=,,\bar{A}e_{rr}\left( k \right) +\bar{B}u\left( k \right) $

2. 求解Riccati方程：$P=Q+{\bar{A}}^{T}P\bar{A}-{\bar{A}}^{T}P\bar{B}{\left(R+{\bar{B}}^{T}P\bar{B}\right)}^{-1}{\bar{B}}^{T}P\bar{A}$

   最初P初始化为Q，然后持续迭代P。

3. 求解：$K={\left(R+{\bar{B}}^{T}P\bar{B}\right)}^{-1}{\bar{B}}^{T}P\bar{A}$

4. 得：$u_k=-k{e}_{rr}\left(k\right)$