车辆运动学模型的线性化和离散化及代码实现

线性化

线性化的方法

泰勒级数展开是一种将非线性微分方程在某个特定点（通常是平衡点或操作点）附近线性化的方法。这种方法基于在该点附近，非线性函数可以近似为线性函数加上高阶无穷小。以下是使用泰勒级数展开进行线性化的步骤：

1. 选择平衡点：
   选择系统的平衡点，即系统在没有外部扰动时的稳态工作点。在这个点上，系统的所有导数（或状态变量的变化率）都为零。

2. 计算函数值和导数：
   在平衡点处计算非线性函数的值以及所有一阶和高阶导数。这些导数将构成泰勒级数展开的系数。

3. 泰勒级数展开：
   将非线性函数在展开点附近展开为泰勒级数。通常只保留一阶项（线性项），因为高阶项在小扰动下的影响较小。泰勒级数展开的一般形式为：
   $$f(x)\approx f(a)+f^{\prime }(a)(x-a)\text{\hspace{1em}(1)}$$
   其中，$f(x)$ 是非线性函数，$a$ 是展开点，$f^{\prime }(a)$ 是函数在点 $a$ 的导数。

4. 线性近似：
   通过忽略高阶项，得到非线性函数在展开点附近的线性近似。这个近似可以用来分析系统的局部行为，例如稳定性分析。

5. 应用线性化模型：
   使用线性化后的模型来分析系统的行为，设计控制器，或者进行数值模拟。线性系统的理论工具可以直接应用于这个线性化模型。

举例说明，假设有一个非线性微分方程：
$$\dot{x}=f(x,u)\text{\hspace{1em}(2)}$$
其中，$\dot{x}$ 表示 $x$ 的导数，$u$ 是控制输入。如果我们选择展开点 $(x_0,u_0)$，那么在该点附近的泰勒级数展开的一般形式是：
$$f(x,u)=f(x_0,u_0)+\frac{\partial f}{\partial x}(x_0,u_0)(x-x_0)+\frac{\partial f}{\partial u}(x_0,u_0)(u-u_0)+\cdots \text{\hspace{1em}(3)}$$
如果我们只保留到一阶项，那么非线性方程就可以近似为线性方程：
$$f(x,u)\approx f(x_0,u_0)+\frac{\partial f}{\partial x}(x_0,u_0)(x-x_0)+\frac{\partial f}{\partial u}(x_0,u_0)(u-u_0)\text{\hspace{1em}(4)}$$
这个线性化过程的关键在于选择一个合适的点 $(x_0,u_0)$ 作为展开点，以及确定展开到哪个阶数。通常，我们会选择平衡点或某个特定的工作点作为展开点。需要注意的是，线性化是一种近似方法，只在一定条件下成立。当扰动或偏差较大时，线性化可能不再准确。因此，在应用线性化方法时，需要谨慎评估其适用性和误差范围。

车辆运动学线性化

$$\{\begin{array}{l}\dot{x}=vcos\phi \\ \dot{y}=vsin\phi \\ \dot{\phi }=\frac{vtan{\delta }_f}{L}\end{array}\text{\hspace{1em}(5)}$$
这里状态变化量和控制量分别为
$$\begin{array}{rl}\dot{X}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}vcos\phi \\ vsin\phi \\ \frac{vtan{\delta }_f}{L}\end{array}\right],& u=\left[\begin{array}{c}v\\ {\delta }_f\end{array}\right]\end{array}\text{\hspace{1em}(6)}$$
则对应的非线性微分方程为
$$\dot{X}=f(X,u)\text{\hspace{1em}(7)}$$
我们选取参考轨迹上的任意一点作为展开点，我们用$\dot{X_r}=f(X_r,u_r)$表示，其中$X_r=[x_r,y_r,{\phi }_r{]}^T$，$u_r=[v_r,{\delta }_{fr}{]}^T$。对该点采用泰勒级数展开，并忽略高阶项，可得
$$\begin{array}{rl}\dot{X}& =f(X_r,u_r)+\frac{\partial f}{\partial x}(X_r,u_r)(X-X_r)+\frac{\partial f}{\partial u}(X_r,u_r)(u-u_r)\\ \dot{X}& =\dot{X_r}+\frac{\partial f}{\partial x}(X_r,u_r)(X-X_r)+\frac{\partial f}{\partial u}(X_r,u_r)(u-u_r)\end{array}\text{\hspace{1em}(8)}$$
移项可得状态误差变化量
$$\dot{X_e}=\dot{X}-\dot{X_r}=\frac{\partial f}{\partial x}(X_r,u_r)(X-X_r)+\frac{\partial f}{\partial u}(X_r,u_r)(u-u_r)\text{\hspace{1em}(9)}$$
其中
$$\begin{array}{r}\frac{\partial f}{\partial x}(X_r,u_r)={\left[\begin{array}{ccc}\frac{\partial vcos\phi }{\partial x}& \frac{\partial vcos\phi }{\partial y}& \frac{\partial vcos\phi }{\partial \phi }\\ \frac{\partial vsin\phi }{\partial x}& \frac{\partial vsin\phi }{\partial y}& \frac{\partial vsin\phi }{\partial \phi }\\ \frac{\partial \frac{vtan{\delta }_f}{L}}{\partial x}& \frac{\partial \frac{vtan{\delta }_f}{L}}{\partial y}& \frac{\partial \frac{vtan{\delta }_f}{L}}{\partial \phi }\end{array}\right]}_{(X_r,u_r)}=\left[\begin{array}{ccc}0& 0& -v_{r}sin{\phi }_r\\ 0& 0& v_{r}cos{\phi }_r\\ 0& 0& 0\end{array}\right]\\ \frac{\partial f}{\partial u}(X_r,u_r)={\left[\begin{array}{cc}\frac{\partial vcos\phi }{\partial v}& \frac{\partial vcos\phi }{\partial {\delta }_f}\\ \frac{\partial vsin\phi }{\partial v}& \frac{\partial vsin\phi }{\partial {\delta }_f}\\ \frac{\partial \frac{vtan{\delta }_f}{L}}{\partial v}& \frac{\partial \frac{vtan{\delta }_f}{L}}{\partial {\delta }_f}\end{array}\right]}_{(X_r,u_r)}=\left[\begin{array}{cc}cos{\phi }_r& 0\\ sin{\phi }_r& 0\\ \frac{tan{\delta }_{fr}}{L}& \frac{v_r}{Lco{s}^2{\delta }_{fr}}\end{array}\right]\end{array}\text{\hspace{1em}(10)}$$
则状态误差变化量$\dot{X_e}$可表示为
$$\begin{array}{rl}\dot{X_e}& =\left[\begin{array}{c}\dot{x}-\dot{x_r}\\ \dot{y}-\dot{y_r}\\ \dot{\phi }-\dot{{\phi }_r}\end{array}\right]\\ & =\left[\begin{array}{ccc}0& 0& -v_{r}sin{\phi }_r\\ 0& 0& v_{r}cos{\phi }_r\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{cc}cos{\phi }_r& 0\\ sin{\phi }_r& 0\\ \frac{tan{\delta }_{fr}}{L}& \frac{v_r}{Lco{s}^2{\delta }_{fr}}\end{array}\right]\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]\\ & =A{X}_e+B{u}_e\end{array}\text{\hspace{1em}(11)}$$

离散化

离散化方法

欧拉法

对于线性微分方程
$$\dot{x}=Ax+Bu\text{\hspace{1em}(12)}$$
其中$A$、$B$分别为状态矩阵和输入矩阵，对上式进行$t$到$t+dt$的积分可得
$${\int }_t^{t+dt}\dot{x}dx={\int }_t^{t+dt}(Ax+Bu)dt\text{\hspace{1em}(13)}$$
由微分中值定理可得
$$\begin{array}{r}x(t+dt)-x(t)=Ax(\xi )dt+Bu(\xi )dt\end{array}\text{\hspace{1em}(14)}$$
其中$\xi \in (t.dt)$

整理可得
$$x(t+dt)=x(t)+Ax(\xi )dt+Bu(\xi )dt\text{\hspace{1em}(15)}$$
根据$\xi$的计算方式不同，欧拉法又分为前向欧拉法、后向欧拉法和中值欧拉法。其具体的形式如下

• 前向欧拉法：$x(\xi )=x(t)$​；
• 后向欧拉法：$x(\xi )=x(t+dt)$；
• 中值欧拉法：$x(\xi )=\frac{x(t)+x(t+dt)}{2}$。

设离散间隔为$T$，离散时间为$k$，则（15）式子$t$、$t+dt$和$dt$中分别对应的就是$k$、$k+1$和$T$，结合（15）式，整理可得对应的微分方程如下

• 前向欧拉法：$x(k+1)=x(k)+TAx(k)+TBu(k)=(I+TA)x(k)+TBu(k)$​​；
• 后向欧拉法：$x(k+1)=x(k)+TAx(k+1)+TBu(k+1)=(I+TA)x(k+1)+TBu(t+1)$；
• 中值欧拉法：$x(k+1)=x(k)+TAx(\frac{k+1}{2})+TBu(\frac{k+1}{2})=(I+TA)x(\frac{k+1}{2})+TBu(\frac{k+1}{2})$。

零阶保持法

对于（12）式的微分方程，设初始时间为$t_0$，则其任意时刻状态$x(t)$为
$$x(t)=x(t_0)e^{A(t-t_0)}+{\int }_{t_0}^t{e}^{A(t-\tau )}Bu(\tau )d\tau \text{\hspace{1em}(16)}$$
式（16）的证明过程如下

证明：

将状态方程（12）移项并左右同乘$e^{-At}$得
$$e^{-At}(\dot{x}(t)-Ax(t))=e^{-At}Bu(t)\text{\hspace{1em}(17)}$$
上式整理可得
$$\frac{d(e^{-At}x(t))}{dt}=e^{-At}Bu(t)\text{\hspace{1em}(18)}$$
方程左右同时积分得
$${\int }_{t_0}^t\frac{d(e^{-A\tau }x(\tau ))}{d\tau }d\tau ={\int }_{t_0}^t{e}^{-A\tau }Bu(\tau )d\tau \text{\hspace{1em}(19)}$$
化简可得
$$e^{-At}x(t)-e^{-A{t}_0}x(t_0)={\int }_{t_0}^t{e}^{-A\tau }Bu(\tau )d\tau \text{\hspace{1em}(20)}$$
移项可得
$$x(t)=x(t_0)e^{A(t-t_0)}+{\int }_{t_0}^t{e}^{A(t-\tau )}Bu(\tau )d\tau \text{\hspace{1em}(21)}$$

设离散系统的采样时间间隔为$T$，则离散时间$t_{k+1}=t_k+T$，结合（12）式状态微分方程的解（16）可得
$$x(t_{k+1})=x(t_k)e^{A(t_{k+1}-t_k)}+{\int }_{t_k}^{t_{k+1}}{e}^{A(t_{k+1}-\tau )}Bu(\tau )d\tau \text{\hspace{1em}(22)}$$
根据零阶保持器的特性（即动态系统的输入$u(t)$在每一个采样时间内保持不变）。可得
$$u(t)=u(t_k)t_k\le t<t_{k+1}\text{\hspace{1em}(23)}$$
因此结合$t_{k+1}-t_k=T$，可将式（22）写为
$$\begin{array}{rl}x(t_{k+1})& =x(t_k)e^{AT}+Bu(t_k){\int }_{t_k}^{t_k+T}{e}^{A(t_k+T-\tau )}d\tau \\ & =e^{AT}x(t_k)+({\int }_{t_k}^{t_k+T}{e}^{A(t_k+T-\tau )}d\tau )Bu(t_k)\\ & =e^{AT}x(t_k)+(-A^{-1}{e}^{A(t_k+T-\tau )}{|}_{t_k}^{t_k+T})Bu(t_k)\\ & =e^{AT}x(t_k)+(-A^{-1}(e^0-e^{AT}))Bu(t_k)\\ & =e^{AT}x(t_k)+A^{-1}(e^{AT}-I)Bu(t_k)\end{array}\text{\hspace{1em}(24)}$$
根据上式可得零阶保持法离散化后的微分方程：
$$\{\begin{array}{l}x(k+1)=Fx(k)+Gu(k)\\ F=e^{AT}\\ G=A^{-1}(e^{AT}-I)B=A^{-1}(F-I)B\end{array}\text{\hspace{1em}(25)}$$

车辆运动学离散化

对（11）式进行前向欧拉离散化，可得
$$\dot{X_e(k)}=\frac{X_e(k+1)-X_e(k)}{T}=A{X}_e(k)+B{u}_e(k)\text{\hspace{1em}(26)}$$
整理后的矩阵形式如下所示
$$\begin{array}{rl}{X}_e(k+1)& =(TA+I)A{X}_e(k)+TB{u}_e(k)\\ & =\left[\begin{array}{ccc}1& 0& -T{v}_{r}sin{\phi }_r\\ 0& 1& T{v}_{r}cos{\phi }_r\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{cc}Tcos{\phi }_r& 0\\ Tsin{\phi }_r& 0\\ \frac{T{v}_{r}tan{\delta }_{fr}}{L}& \frac{T{v}_r}{Lco{s}^2{\delta }_{fr}}\end{array}\right]\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]\end{array}\text{\hspace{1em}(27)}$$
其中$T$为采样步长，$I$为3x3的单位矩阵。

代码实现

python代码实现基于前面文章用到的车辆运动学模型，新增update_ABMatrix(vehicle, ref_delta, ref_yaw)方法实现，其中vehicle为Vehicle类实例，ref_delta和ref_yaw分别为当前时刻参考线匹配点对应的角度变化${\delta }_{fr}$和航向角${\phi }_r$，具体实现如下

import math
import numpy as np

class Vehicle:
    def __init__(self,
                 x=0.0,
                 y=0.0,
                 yaw=0.0,
                 v=0.0,
                 dt=0.1,
                 l=3.0):
        self.steer = 0
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.dt = dt
        self.L = l  # 轴距
        self.x_front = x + l * math.cos(yaw)
        self.y_front = y + l * math.sin(yaw)

    def update(self, a, delta, max_steer=np.pi):
        delta = np.clip(delta, -max_steer, max_steer)
        self.steer = delta

        self.x = self.x + self.v * math.cos(self.yaw) * self.dt
        self.y = self.y + self.v * math.sin(self.yaw) * self.dt
        self.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dt

        self.v = self.v + a * self.dt
        self.x_front = self.x + self.L * math.cos(self.yaw)
        self.y_front = self.y + self.L * math.sin(self.yaw)


class VehicleInfo:
    # Vehicle parameter
    L = 3.0  # 轴距
    W = 2.0  # 宽度
    LF = 3.8  # 后轴中心到车头距离
    LB = 0.8  # 后轴中心到车尾距离
    MAX_STEER = 0.6  # 最大前轮转角
    TR = 0.5  # 轮子半径
    TW = 0.5  # 轮子宽度
    WD = W  # 轮距
    LENGTH = LB + LF  # 车辆长度

def draw_vehicle(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):
    vehicle_outline = np.array(
        [[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],
         [vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])

    wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],
                      [vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2,
                       vehicle_info.TW / 2]])

    rr_wheel = wheel.copy()  # 右后轮
    rl_wheel = wheel.copy()  # 左后轮
    fr_wheel = wheel.copy()  # 右前轮
    fl_wheel = wheel.copy()  # 左前轮
    rr_wheel[1, :] += vehicle_info.WD / 2
    rl_wheel[1, :] -= vehicle_info.WD / 2

    # 方向盘旋转
    rot1 = np.array([[np.cos(steer), -np.sin(steer)],
                     [np.sin(steer), np.cos(steer)]])
    # yaw旋转矩阵
    rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],
                     [np.sin(yaw), np.cos(yaw)]])
    fr_wheel = np.dot(rot1, fr_wheel)
    fl_wheel = np.dot(rot1, fl_wheel)
    fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])
    fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])

    fr_wheel = np.dot(rot2, fr_wheel)
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    fl_wheel = np.dot(rot2, fl_wheel)
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rr_wheel = np.dot(rot2, rr_wheel)
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    rl_wheel = np.dot(rot2, rl_wheel)
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y
    vehicle_outline = np.dot(rot2, vehicle_outline)
    vehicle_outline[0, :] += x
    vehicle_outline[1, :] += y

    ax.plot(fr_wheel[0, :], fr_wheel[1, :], color)
    ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)
    ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)
    ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)

    ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)
    ax.axis('equal')


def update_ABMatrix(vehicle, ref_delta, ref_yaw):
    """
    计算离散线性车辆运动学模型状态矩阵A和输入矩阵B
    return: A, B
    """
    A = np.matrix([
        [1.0, 0.0, -vehicle.v * vehicle.dt * math.sin(ref_yaw)],
        [0.0, 1.0, vehicle.v * vehicle.dt * math.cos(ref_yaw)],
        [0.0, 0.0, 1.0]])

    B = np.matrix([
        [vehicle.dt * math.cos(ref_yaw), 0],
        [vehicle.dt * math.sin(ref_yaw), 0],
        [vehicle.dt * math.tan(ref_delta) / vehicle.L,
         vehicle.v * vehicle.dt / (vehicle.L * math.cos(ref_delta) * math.cos(ref_delta))]])
    return A, B

文章首发公众号：iDoitnow如果喜欢话，可以关注一下