基于LQR和前馈控制的自动驾驶控制算法(1):二自由度动力学模型

---

前言

本章内容主要参考bilibili的up主【忠厚老实的老王】中的自动驾驶控制系列视频，可以说许多内容都是我对该系列视频的学习笔记，非常感谢该大佬的用心讲解。以下是推导中涉及了较为小众的基础知识。

【基础1】向量微分[2]

设经过 $dt$ 时刻，向量 $\vec{r}$ 移动为 $\vec{r}+d\vec{r}$ ，有：
$$\frac{d\vec{r}}{dt}=\frac{d\vec{r}}{ds}\cdot \frac{ds}{dt}$$
当 $dt\to 0$ , $|d\vec{r}|=|d\vec{s}|$ 方向趋于切线方向 $\vec{\tau }$ ，即：
$$\frac{d\vec{r}}{dt}=\frac{|d\vec{r}|}{|ds|}\vec{\tau }\cdot \frac{ds}{dt}=\vec{\tau }\cdot \frac{ds}{dt}=\dot{s}\vec{\tau }$$

【基础2】Frenet公式[2]

$$\begin{array}{rl}\frac{d\vec{\tau }}{ds}& =k\vec{n}\\ \frac{d\vec{n}}{ds}& =-k\vec{\tau }\end{array}$$
其中，$\vec{\tau }$ 、$\vec{n}$ 分别为切向和法向单位向量，$k$ 为曲率。

---

车身系下的线性二自由度动力学模型

二自由度车辆模型[1]

考虑车辆的横向运动和横摆运动，有：
$$\begin{array}{rl}m(\ddot{y}+v_x\dot{\psi })& =F_{yf}\cos \delta +F_{yr}\\ I_z\ddot{\psi }& =a{F}_{yf}\cos \delta -b{F}_{yr}\end{array}$$
假设轮胎特性处于线性区，轮胎侧向力 $F_y$ 近似计算为：
$$F_y=C_{\alpha }\alpha$$
其中 $C_{\alpha }$ 为左右双轮的侧偏刚度，$\alpha$ 为轮胎侧偏角，可计算为：
$$\{\begin{array}{rl}{\alpha }_f& =\arctan \frac{\dot{y}+a\dot{\psi }}{v_x}-\delta \approx \frac{\dot{y}+a\dot{\psi }}{v_x}-\delta \\ {\alpha }_r& =\arctan \frac{\dot{y}-b\dot{\psi }}{v_x}\approx \frac{\dot{y}-b\dot{\psi }}{v_x}\end{array}$$
采用小转角假设，设 $\cos \delta \approx 1$ ，整理可得：
$$\left[\begin{array}{c}\ddot{y}\\ \ddot{\psi }\end{array}\right]=\left[\begin{array}{cc}\frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}& -v_x+\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}\\ \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I_z{v}_x}& \frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}\dot{y}\\ \dot{\psi }\end{array}\right]+\left[\begin{array}{c}-\frac{C_{\alpha f}}{m}\\ -\frac{a{C}_{\alpha f}}{I_z}\end{array}\right]\delta \text{\hspace{1em}(1)}$$
或可写为：
$$\dot{\left[\begin{array}{c}y\\ \dot{y}\\ \psi \\ \dot{\psi }\end{array}\right]}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}& 0& -m{v}_x+\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I_z{v}_x}& 0& \frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}y\\ \dot{y}\\ \psi \\ \dot{\psi }\end{array}\right]+\left[\begin{array}{c}0\\ -\frac{C_{\alpha f}}{m}\\ 0\\ -\frac{a{C}_{\alpha f}}{I_z}\end{array}\right]\delta$$

Frenet坐标系下的误差模型

误差定义[3]

横向误差 $e_1$：从参考路径到车辆质心的距离
航向误差 $e_2$：相对参考路径的横摆角误差

模型推导[2]

如图，若车辆质心位置为 $P$ ，对应参考路径中最近点为 $Pr$ ，易知 $P$ 位于 ${\vec{n}}_r$ 上，误差可计算为：
$$\begin{array}{rl}{e}_1& =(\overrightarrow{OP}-\overrightarrow{O{P}_r})\cdot \overrightarrow{n_r}\\ e_2& =\psi +\beta -{\theta }_r\approx \psi -{\theta }_r\end{array}$$
对 $e_1$ 求导得：
$${\dot{e}}_1=(\dot{\overrightarrow{OP}}-\dot{\overrightarrow{O{P}_r}})\cdot {\vec{n}}_r+(\overrightarrow{OP}-\overrightarrow{O{P}_r})\cdot {\dot{\vec{n}}}_r$$
根据【基础1】【基础2】可知 $\dot{\overrightarrow{OP}}=\left|\vec{V}\right|\vec{\tau }$，$\dot{\overrightarrow{O{P}_r}}=\dot{s}{\vec{\tau }}_r$，$\dot{{\vec{n}}_r}=\frac{d{\vec{n}}_r}{ds}\cdot \frac{ds}{dt}=-k\dot{s}{\vec{\tau }}_r$，且注意有 ${\vec{n}}_r\cdot {\vec{\tau }}_r=0$，$\overrightarrow{OP}-\overrightarrow{O{P}_r}=e_1{\vec{n}}_r$ ($P$ 位于 ${\vec{n}}_r$ 上)，得：
$$\begin{array}{rl}{\dot{e}}_1& =\left|\vec{V}\right|\vec{\tau }\cdot {\vec{n}}_r\\ & =\left|\vec{V}\right|\cos (\frac{\pi }{2}-\theta +{\theta }_r)\\ & =\left|\vec{V}\right|\sin (\theta -{\theta }_r)\end{array}$$
注意到：
$$\begin{array}{rl}\sin (\theta -{\theta }_r)& =\sin (\beta +\psi -{\theta }_r)\\ & =\sin \beta \cos (\psi -{\theta }_r)+\cos \beta \sin (\psi -{\theta }_r)\end{array}$$
故：
$$\begin{array}{rl}{\dot{e}}_1& =\left|\vec{V}\right|\sin \beta \cos (\psi -{\theta }_r)+\left|\vec{V}\right|\cos \beta \sin (\psi -{\theta }_r)\\ & =v_y\cos (\psi -{\theta }_r)+v_x\sin (\psi -{\theta }_r)\\ & \approx v_y+v_x\sin (\psi -{\theta }_r)\text{(认为ψ−θr是小量)}\\ & \approx v_y+v_x{e}_2\end{array}$$
得：
$$\begin{array}{rl}{v}_y& ={\dot{e}}_1-v_x{e}_2\\ {\dot{v}}_y& ={\ddot{e}}_1-v_x{\dot{e}}_2\end{array}$$
对 $e_2$ 求导得：
$$\begin{array}{rl}{\dot{e}}_2& =\dot{\psi }-{\dot{\theta }}_r\\ {\ddot{e}}_2& =\ddot{\psi }-{\ddot{\theta }}_r\approx \ddot{\psi }\end{array}$$
整理可得（$\dot{y}=v_y$，$\ddot{y}={\dot{v}}_y$）：
$$\{\begin{array}{rl}\dot{y}& ={\dot{e}}_1-v_x{e}_2\\ \ddot{y}& ={\ddot{e}}_1-v_x{\dot{e}}_2\\ \dot{\psi }& ={\dot{e}}_2+{\dot{\theta }}_r\\ \ddot{\psi }& ={\ddot{e}}_2\end{array}\text{\hspace{1em}(2)}$$

式(2)在[3]中给出另一种定义方法，最终得到的式子是相似的。

将(2)代入(1)，使用Matlab整理：

%% 二自由度模型
syms C_af C_ar m v_x a b I_z delta real;
syms y_dot psi_dot real;

A = [(C_af + C_ar)/(m*v_x), -v_x + (a*C_af - b*C_ar)/(m*v_x);
     (a*C_af - b*C_ar)/(I_z*v_x), (a^2*C_af + b^2*C_ar)/(I_z*v_x)];
B = [-C_af/m; -a*C_af/I_z];

X = [y_dot; psi_dot];
u = delta;

y_psi_ddot = A * X + B * u;

disp('y_ddot:');
disp(y_psi_ddot(1));
disp('psi_ddot:');
disp(y_psi_ddot(2));
%% 代入
syms e1 e1_dot e1_ddot e2 e2_dot e2_ddot theta_dot_r
y_dot_n = e1_dot-v_x*e2;
psi_dot_n = e2_dot + theta_dot_r;

rightEq1 = y_psi_ddot(1)+v_x*e2_dot;
rightEq2 = y_psi_ddot(2);

rightEq1 = subs(rightEq1, y_dot, y_dot_n);
rightEq1 = subs(rightEq1, psi_dot, psi_dot_n);
rightEq1 = collect(rightEq1, [e1 e1_dot e2 e2_dot delta])
rightEq2 = subs(rightEq2, y_dot, y_dot_n);
rightEq2 = subs(rightEq2, psi_dot, psi_dot_n);
rightEq2 = collect(rightEq2, [e1 e1_dot e2 e2_dot delta])

得：
$$\begin{array}{rl}{\ddot{e}}_1& =\frac{C_{\mathrm{af}}+C_{\mathrm{ar}}}{m{v}_x}{\dot{e}}_1+\left(-\frac{C_{\mathrm{af}}+C_{\mathrm{ar}}}{m}\right)e_2+\frac{C_{\mathrm{af}}a-C_{\mathrm{ar}}b}{m{v}_x}{\dot{e}}_2-\frac{C_{\mathrm{af}}}{m}\delta -{\dot{\theta }}_r\left(v_x-\frac{C_{\mathrm{af}}a-C_{\mathrm{ar}}b}{m{v}_x}\right)\\ {\ddot{e}}_2& =\frac{C_{\mathrm{af}}a-C_{\mathrm{ar}}b}{I_z{v}_x}{\dot{e}}_1+\left(-\frac{C_{\mathrm{af}}a-C_{\mathrm{ar}}b}{I_z}\right)e_2+\frac{C_{\mathrm{af}}{a}^2+C_{\mathrm{ar}}{b}^2}{I_z{v}_x}{\dot{e}}_2-\frac{C_{\mathrm{af}}a}{I_z}\delta +\frac{{\dot{\theta }}_r\left(C_{\mathrm{af}}{a}^2+C_{\mathrm{ar}}{b}^2\right)}{I_z{v}_x}\end{array}$$
进一步改写为：
$$\dot{\left[\begin{array}{c}{e}_1\\ {\dot{e}}_1\\ e_2\\ {\dot{e}}_2\end{array}\right]}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}& -\frac{C_{\mathrm{af}}+C_{\mathrm{ar}}}{m}& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I_z{v}_x}& -\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I_z}& \frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I_z{v}_x}\end{array}\right]\left[\begin{array}{c}{e}_1\\ {\dot{e}}_1\\ e_2\\ {\dot{e}}_2\end{array}\right]+\left[\begin{array}{c}0\\ -\frac{C_{\alpha f}}{m}\\ 0\\ -\frac{a{C}_{\alpha f}}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ -v_x+\frac{a{C}_{\mathrm{af}}-b{C}_{\mathrm{ar}}}{m{v}_x}\\ 0\\ \frac{a^2{C}_{\mathrm{af}}+b^2{C}_{\mathrm{ar}}}{I_z{v}_x}\end{array}\right]{\dot{\theta }}_r$$

---

参考

[1] 余志生. 汽车理论[J]. 2000.
[2] 【基础】自动驾驶控制算法第四讲 坐标变换与横向误差微分方程
[3] Rajamani R. Vehicle dynamics and control[M]. Springer Science & Business Media, 2011.