车辆/单车运动学建模、线性化、离散化过程详细推导

车辆/单车运动学建模、线性化、离散化过程详细推导

引用

CSDN

1.

https://blog.csdn.net/qq_44339029/article/details/137547935

运动学建模、线性化、离散化过程，是使用控制算法进行控制前常用的操作，本文主要以车辆/单车模型为例，详细介绍运动学建模、线性化、离散化等过程，为后续的使用LQR、MPC等控制算法进行轨迹跟踪或其他控制任务做准备。

一、车辆/单车 运动学建模

设机器人当前的位姿为（$x_c$, $y_c$, $\varphi_c$），其中（$x_c$, $y_c$）为其当前位置坐标，$\varphi_c$为其当前姿态角，当前前轮的转向角为$\delta_c$，当前的后轮车速为$v_c$，前轮车速为$v_f$，前后轮距为$l$，默认采用后轮驱动模式，即可控的是后轮车速$v_c$，如下图所示：

容易得到$v_c$在$x$轴和$y$轴的分量分别为$v_c \cos(\varphi_c)$、$v_c \sin(\varphi_c)$，即：

$$
\dot{x_c} = v_c \cos(\varphi_c) \
\dot{y_c} = v_c \sin(\varphi_c)
$$

刚体在同一方向上的速度是相同的，即前轮车速在车辆当前移动方向$v_c$上的分量应该与$v_c$相同，即$v_{fx} = v_c$，这个分量与车辆运行方向相同，对车辆的转向无作用，导致车辆运行方向改变的是分量$v_{fy} = v_c \tan(\delta_c)$，将其视为车辆绕后轮旋转的线速度，除以半径$l$，即为旋转角速度，如下所示：

$$
\dot{\varphi_c} = \frac{v_c \tan(\delta_c)}{l}
$$

所以，我们可以得到完整的单车运动学模型的状态空间表达式如下：

$$
\begin{cases}
\dot{x_c} = v_c \cos(\varphi_c) \
\dot{y_c} = v_c \sin(\varphi_c) \
\dot{\varphi_c} = \frac{v_c \tan(\delta_c)}{l}
\end{cases}
$$

二、将状态空间表达式线性化

第一部分中得到了运动学建模的状态空间表达式，然而它是非线性的，想要用来做控制，需要将其化为线性的，先将其写成向量的形式 ，取状态量$X_c = [x_c \ y_c \ \varphi_c]^T$，控制输入为$u = [v_c \ \delta_c]^T$，即前轮转向，后轮驱动。则向量形式的状态空间可写为(因为是非线性的，不能写成矩阵形式)：

$$
\dot{X} = f(X,u)
$$

最常用的近似线性化方法就是泰勒展开，在$x_0$点处展开的泰勒展开式如下所示：

$$
f(x) = \frac{f(x_0)}{0!} + \frac{f'(x_0)}{1!}(x-x_0) + \frac{f''(x_0)}{2!}(x-x_0)^2 + ... + \frac{f^{(n)}(x_0)}{n!}(x-x_0)^n + R_n(x)
$$

使用泰勒展开进行线性化需要选择一个展开点，前面我们已经设定了当前位姿（状态）为（$x_c$, $y_c$, $\varphi_c$），现在补充设定目标位姿（状态）为（$x_r$, $y_r$, $\varphi_r$），将目标位姿作为展开点，对$f(X,u)$进行泰勒展开，展开到一阶线性项，如下所示：

$$
\dot{X} \approx f(X_r,u_r) + \frac{\partial f(X_r,u_r)}{\partial X_r}(X-X_r) + \frac{\partial f(X_r,u_r)}{\partial u_r}(u-u_r)
$$

即：

$$
\dot{X} - \dot{X_r} \approx \frac{\partial f(X_r,u_r)}{\partial X_r}(X-X_r) + \frac{\partial f(X_r,u_r)}{\partial u_r}(u-u_r)
$$

这里的线性化，并不是把非线性关系，转换为线性关系，而是通过展开，使其误差变为一个线性关系，如上式所示，展开后的误差是线性的，所以我们可以取当前量与目标量（参考量）之间的差值为新的状态量和输入项，如下所示：

$$
\dot{\tilde{X}} = \begin{bmatrix}
\dot{x_c} - \dot{x}_r \
\dot{y_c} - \dot{y}_r \
\dot{\varphi_c} - \dot{\varphi}_r
\end{bmatrix}, \quad
\tilde{X} = \begin{bmatrix}
x_c - x_r \
y_c - y_r \
\varphi_c - \varphi_r
\end{bmatrix}, \quad
\mathbf{\tilde{u}} = \begin{bmatrix}
v_c - v_r \
\delta_c - \delta_r
\end{bmatrix}
$$

则上面得到的展开式可以写为：

$$
\dot{\tilde{X}} \approx \frac{\partial f(X_r,u_r)}{\partial X_r}\tilde{X} + \frac{\partial f(X_r,u_r)}{\partial u_r}\tilde{u}
$$

也就是我们常见的$\dot{X} = AX + Bu$的状态空间表达式的形式，成功完成了线性化，求矩阵A和B，也就是$\frac{\partial f(X_r,u_r)}{\partial X_r}$和$\frac{\partial f(X_r,u_r)}{\partial u_r}$实际上，也就是在求雅克比矩阵，如下所示：

注：下式中的$f$是$f(X_r,u_r)$的简称

$$
\begin{cases}
f_1 = v_r \cos(\varphi_r) \
f_2 = v_r \sin(\varphi_r) \
f_3 = \frac{v_r \tan(\delta_r)}{l}
\end{cases}
$$

$$
A = \frac{\partial f(X_r,u_r)}{\partial X_r} = \begin{bmatrix}
\frac{\partial f_1}{\partial x_r} & \frac{\partial f_1}{\partial y_r} & \frac{\partial f_1}{\partial \varphi_r} \
\frac{\partial f_2}{\partial x_r} & \frac{\partial f_2}{\partial y_r} & \frac{\partial f_2}{\partial \varphi_r} \
\frac{\partial f_3}{\partial x_r} & \frac{\partial f_3}{\partial y_r} & \frac{\partial f_3}{\partial \varphi_r}
\end{bmatrix} = \begin{bmatrix}
0 & 0 & -v_r \sin \varphi_r \
0 & 0 & v_r \cos \varphi_r \
0 & 0 & 0
\end{bmatrix}
$$

$$
B = \frac{\partial f(X_r,u_r)}{\partial u_r} = \begin{bmatrix}
\frac{\partial f_1}{\partial v_r} & \frac{\partial f_1}{\partial \delta_r} \
\frac{\partial f_2}{\partial v_r} & \frac{\partial f_2}{\partial \delta_r} \
\frac{\partial f_3}{\partial v_r} & \frac{\partial f_3}{\partial \delta_r}
\end{bmatrix} = \begin{bmatrix}
\cos \varphi_r & 0 \
\sin \varphi_r & 0 \
\frac{\tan \delta_r}{l} & \frac{v_r}{l \cos^2 \delta_r}
\end{bmatrix}
$$

三、离散化

为了方便使用递推式进行迭代，我们还需要对线性化后的结果进行离散化，用差商代替微分，如下所示：

$$
\dot{\widetilde{X}} = \frac{\widetilde{X}(k+1) - \widetilde{X}(k)}{T} = A \widetilde{X}(k) + B \widetilde{u}(k)
$$

所以：

$$
\widetilde{X}(k+1) = (I + TA) \widetilde{X}(k) + TB \widetilde{u}(k)
$$

我们定义$\widetilde{A} = (I + TA)$，$\widetilde{B} = TB$，则上式可写为：

$$
\widetilde{X}(k+1) = \widetilde{A} \widetilde{X}(k) + \widetilde{B} \widetilde{u}(k)
$$

由前面推出的A和B的结果，容易得知：

$$
\widetilde{A} = \begin{bmatrix}
1 & 0 & -Tv_r \sin \varphi_r \
0 & 1 & Tv_r \cos \varphi_r \
0 & 0 & 1
\end{bmatrix}
$$

$$
\widetilde{B} = \begin{bmatrix}
T \cos \varphi_r & 0 \
T \sin \varphi_r & 0 \
\frac{T \tan \delta_r}{l} & \frac{T v_r}{l \cos^2 \delta_r}
\end{bmatrix}
$$

至此，我们已经得到完整的离散化的状态空间表达式，上面式子中带有较多的差值符号~，这是因为在第二部分中将状态量和输入量分别取为$X = [x \ y \ \varphi]^T$，控制输入为$u = [v \ \delta]^T$导致的，所以，现在我们直接将状态量和输入量取为当前量与期望值的差值，即

$$
X = \begin{bmatrix}
x_c - x_r \
y_c - y_r \
\varphi_c - \varphi_r
\end{bmatrix}, \quad
u = \begin{bmatrix}
v_c - v_r \
\delta_c - \delta_r
\end{bmatrix}
$$

此时：

$$
X(k+1) = \begin{bmatrix}
1 & 0 & -Tv_r \sin \psi_r \
0 & 1 & Tv_r \cos \psi_r \
0 & 0 & 1
\end{bmatrix} X(k) + \begin{bmatrix}
T \cos \psi_r & 0 \
T \sin \psi_r & 0 \
\frac{T \tan \delta_r}{l} & \frac{T v_r}{l \cos^2 \delta_r}
\end{bmatrix} u(k) = AX(k) + Bu(k)
$$

有了上面的离散化线性运动学模型的状态空间方程，就可以基于此来使用LQR、MPC等控制算法进行轨迹跟踪控制或完成其他控制任务了

参考资料：

1、【控制】模型预测控制 MPC 【合集】Model Predictive Control