自动驾驶运动学模型的线性离散化

Posted 2022-06-01 CHH3213

文章目录

• 1. 运动学模型线性化
• 2. 线性模型离散化
• 3. python实现

1. 运动学模型线性化

之前讲解了车辆的运动学模型，这边以以后轴中心为车辆中心的单车运动学模型为例进行线性化。

以后轴中心为车辆中心的单车运动学模型如下：

$$\{\begin{array}{l}\dot{x}=v\cos (\psi )=f_1\\ \dot{y}=v\sin (\psi )=f_2\\ \dot{\psi }=\frac{v}{L}\tan {\delta }_f=f_3\end{array}\text{\hspace{1em}(1)}$$

选取状态量为 $\mathbf{\chi }=[x,y,\psi {]}^T$ ，控制量为 $\mathbf{u}=[v,\delta {]}^T$ ，则对于参考轨迹的任意一个参考点，用 $r$ 表示，上式可以改写为:
$$\dot{\mathbf{\chi }}=f(\mathbf{\chi },\mathbf{u})\Rightarrow {\dot{\mathbf{\chi }}}_r=f\left({\mathbf{\chi }}_r,{\mathbf{u}}_r\right)\text{\hspace{1em}(2)}$$
其中 ${\chi }_r={\left[x_r,y_r,{\psi }_r\right]}^T,{\mathbf{u}}_r={\left[v_r,{\delta }_r\right]}^T$ 。对上式在参考点采用泰勒级数展开，并忽略高阶项：
$$\dot{\mathbf{\chi }}=f\left({\mathbf{\chi }}_r,{\mathbf{u}}_r\right)+\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{\chi }}\left(\mathbf{\chi }-{\mathbf{\chi }}_r\right)+\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{u}}\left(\mathbf{u}-{\mathbf{u}}_r\right)\text{\hspace{1em}(3)}$$

对 $\frac{\partial f(\chi ,\mathbf{u})}{\partial \chi }$ 和$\frac{\partial f(\chi ,\mathbf{u})}{\partial \mathbf{u}}$ 求雅克比矩阵，有：
$$\begin{array}{rl}\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{\chi }}=& [\begin{array}{l}\frac{\partial f_1}{\partial x}\\ \frac{\partial f_2}{\partial x}\end{array}\end{array}$$