自动驾驶——控制部分

汽车横向动力学模型

$\frac{d}{dt}\begin{bmatrix} y \\ \dot y \\ \phi \\ \dot \phi \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{\alpha f}+2C_{\alpha r}}{m V_x} & 0 & -V_x-\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{m V_x} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{I_z V_x} & 0 & -\frac{2C_{\alpha f}l_f^2+2C_{\alpha r}l_r^2}{I_z V_x} \end{bmatrix} \begin{bmatrix} y \\ \dot y \\ \phi \\ \dot \phi \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha f}}{m} \\ 0 \\ \frac{2C_{\alpha f}l_f}{I_z}\end{bmatrix} \delta$

状态空间定义

车辆状态几何推导

\begin{bmatrix} lateral\ error \\ lateral\ error\ rate \\ heading\ error \\ heading\ error\ rate \\ station\ error \\ speed\ error\end{bmatrix}

Desire\ Point纵向方向：(cos\theta_{des}, sin\theta_{des}) \quad \quad Desire\ Point横向方向：(-sin\theta_{des}, cos\theta_{des})

Real\ Point坐标：(x,y) \qquad \qquad \qquad \qquad \quad \ \ \ Desire\ Point坐标：(x_{des},y_{des})

Desire\ Point指向Real\ Point的向量：(dx, dy) = (x,y) - (x_{des},y_{des})

$lateral\ error=(dx, dy)*(-sin\theta_{des}, cos\theta_{des})=dy\ cos\theta_{des}-dx\ sin\theta_{des}$

lateral\ error\ rate=V*sin\ (\phi -\phi_{des})

heading\ error=\phi -\phi_{des}

heading\ error\ rate=\dot \phi -\dot \phi_{des}

station\ error=-(dx, dy)*(cos\theta_{des}, sin\theta_{des})=-(dx\ cos\theta_{des}+dy\ sin\theta_{des})

speed\ error=V_{des}-V*cos( \triangle \phi)/k

方向盘模型

$\frac{d}{dt}\begin{bmatrix} e_1 \\ \dot e_1 \\ e_2 \\ \dot e_2 \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{\alpha f}+2C_{\alpha r}}{m V_x} & \frac{2C_{\alpha f}+2C_{\alpha r}}{m} & -\frac{2C_{\alpha f}l_f+2C_{\alpha r}l_r}{m V_x} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{I_z V_x} & \frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{I_z} & -\frac{2C_{\alpha f}l_f^2+2C_{\alpha r}l_r^2}{I_z V_x} \end{bmatrix} \begin{bmatrix} e_1 \\ \dot e_1 \\ e_2 \\ \dot e_2 \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha f}}{m} \\ 0 \\ \frac{2C_{\alpha f}l_f}{I_z}\end{bmatrix}\delta + \begin{bmatrix} 0 \\ -\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{m V_x}-V_x \\ 0 \\ -\frac{2C_{\alpha f}l_f^2+2C_{\alpha r}l_r^2}{I_z V_x}\end{bmatrix}\dot \phi_{des} + \begin{bmatrix} 0 \\ 0 \\ 0 \\ 1 \end{bmatrix} \ddot \phi_{des}$

Apollo LQR控制器原理

微分方程： $\dot x=A_0x+B_0u$

离散处理： $x_{k+1}=Ax_k+Bu_k$

构建优化目标： $J=\sum_{k=1}^N(X^TQX+u^TRu)$

最优控制： $u=-[(R+B^TPB)^{-1}B^TPA]X=-KX$
其中： $P=A^TPA-A^TPB(R+B^TPB)^{-1}B^TPA+Q$

注：LQR只能给出横向的控制

附录

1、latex中的希腊字母：

https://blog.csdn.net/xxzhangx/article/details/52778539

2、latex编写矩阵：

\begin{gathered} \begin{matrix} 0 & 1 \\ 1 & 0 \end{matrix} \begin{pmatrix} 0 & -i \\ i & 0 \end{pmatrix} \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix} \begin{Bmatrix} 1 & 0 \\ 0 & -1 \end{Bmatrix} \begin{vmatrix} a & b \\ c & d \end{vmatrix} \begin{Vmatrix} i & 0 \\ 0 & -i \end{Vmatrix} \end{gathered}

3、github下载repo中特定文件夹: http://kinolien.github.io/gitzip/#

4、typedef Matrix<double ,Dynamic,Dynamic > MatrixXd;

5、LQR求解原理: https://blog.csdn.net/datase/article/details/78487126

6、Frenet坐标变换：

https://blog.csdn.net/l898985121/article/details/109048855

https://blog.csdn.net/weixin_37395438/article/details/112973098?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control

7、LQR求解原理：https://blog.csdn.net/datase/article/details/78487126

C++ debug

1. override关键字：

Override是伪代码，所以是可写可不写的．它表示方法重写，写上会给我们带来好处．

可以当注释用,方便阅读．

告诉阅读你代码的人，这是方法的复写．

编译器可以给你验证@Override下面的方法名是否是你父类中所有的，如果没有则报错

2. static 函数：

声明该函数的文件能用，其他文件不能用。

3. C++的三角函数：

C++中cos,sin,asin,acos这些三角函数操作的是弧度,而非角度,

你可能感兴趣的文章