车辆编队纵向跟踪控制律设计与仿真

一、车辆纵向动力学建模

1. 轮胎动力学方程

假设每个轮胎都是刚性的，即不考虑轮胎的弹性迟滞损失导致的地面法向反作用力 $F_z$ 的偏移。又假设车辆四轮驱动，每个轮胎都受到驱动力矩 $T_d$ 、制动力矩 $T_b$ 、地面切向反作用力 $F_x$ 、地面法向反作用力 $F_z$ 、驱动轴作用于车轮的水平力 $F_p$ 、车轮负荷 $W$ 。对轮胎受力分析，如下图1所示。

图1 轮胎的地面受力图

以车轮滚动中心 $O$ 为参考点，列出如下的轮胎动力学力矩平衡方程

I_{w} \overset{\cdot}{ω_{w i}} = T_{d i} - T_{b i} - F_{x i} r_{e f f} (i = f l, f r, r l, r r)

$I_w \overset{·}{\omega_{wi}} = T_{di}-T_{bi}-F_{xi}r_{eff}\qquad(i=fl,fr,rl,rr)$

其中 $r_{eff}$ 是车轮的滚动半径。
假如四个轮具有相同的结构参数和运动规律，那么上面的方程可以简化成

\begin{matrix} (1) & I_{w} \overset{\cdot}{ω_{w}} = T_{d} - T_{b} - F_{x} r_{e f f} \end{matrix}

$I_w \overset{·}{\omega_{w}} = T_{d}-T_{b}-F_x r_{eff}\tag{1}$

2. 车辆行驶方程式

汽车在倾斜道路上行驶，对它进行纵向力分析，如下图2所示。

图2 在倾斜道路上行驶的车辆的纵向力分析图

根据牛顿第二定律，得到

m \overset{\cdot \cdot}{x} = F_{x f} + F_{x r} - R_{x f} - R_{x r} - F_{a e r o} - m g s i n θ

$m\overset{··}{x} = F_{xf}+ F_{xr}-R_{xf}-R_{xr}-F_{aero}-mgsin\theta$

其中 $F_{xf}、F_{xr}$ 是驱动力，大小为 $\dfrac{T_di}{r_{eff}}$ 。 $R_{xf}、R_{xr}$ 是滚动阻力， $F_{aero}$ 是等效的纵向空气阻力。令 $F_x = F_{xf}+F_{xr},R_x=R_{xf}+R_{xr}$ ，则有

\begin{matrix} (2) & m \overset{\cdot \cdot}{x} = F_{x} - R_{x} - F_{a e r o} - m g s i n θ \end{matrix}

$m\overset{··}{x} = F_x-R_x-F_{aero}-mgsin\theta\tag{2}$

而车辆的纵向加速度为

\begin{matrix} (3) & \overset{\cdot \cdot}{x} = r_{e f f} \overset{\cdot}{ω_{w}} = r_{e f f} R \overset{\cdot}{ω_{e}} \end{matrix}

$\overset{··}{x}=r_{eff}\overset{·}{\omega _w}=r_{eff}R\overset{·}{\omega _e}\tag{3}$

其中 $R$ 是发动机到车轮的传动比， $\omega _e$ 是发动机输出的角速度。
结合 $(2)、(3)$ 可得车辆行驶方程式

\begin{matrix} (4) & F_{x} = m r_{e f f} R \overset{\cdot}{ω_{e}} + R_{x} + F_{a e r o} + m g s i n θ \end{matrix}

$F_x = mr_{eff}R\overset{·}{\omega _e}+R_x+F_{aero}+mgsin\theta\tag{4}$

3. 车轮动力流和负载

假设车轮不存在液力变矩器，车辆动力传动系统中的动力流和负载可由下图3所示

图3 车辆动力传动系统中的动力流和负载

3.1 轮胎驱动转矩

因为地面切向反作用力与驱动力近似相等，所以联立轮胎动力学方程 $(1)$ 、车辆行驶方程式 $(4)$ 消去 $F_x$ ，可以得到轮胎驱动转矩表达式

T_{d} = I_{w} R \overset{\cdot}{ω_{e}} + m r_{e f f}^{2} R \overset{\cdot}{ω_{e}} + T_{b} + r_{e f f} (R_{x} + F_{a e r o} + m g s i n θ)

$T_d = I_wR\overset{·}{\omega _e}+mr_{eff}^2R\overset{·}{\omega _e}+T_{b}+r_{eff}(R_x+F_{aero}+mgsin\theta)$

3.2 变速器驱动转矩

图4 变速器模型示意图

变速器具有减速增距的效果，力矩增大R倍。有如下方程式

{\begin{cases} I_{t} \overset{\cdot}{ω_{t}} = T_{t} - T_{w h e e l s} \\ T_{w h e e l s} = R T_{d} \\ ω_{t} = ω_{e} \end{cases}

$\begin{cases} I_t \overset{·}{\omega _t}=T_t-T_{wheels}\\ T_{wheels}=RT_d\\ \omega _t=\omega _e \end{cases}$

其中 $I_t、\overset{·}{\omega _t}$ 是变速器传动轴的转动惯量和角加速度， $I_t \overset{·}{\omega _t}$ 是变速器的力矩损耗， $T_t、\omega _t$ 是输入给变速器的转矩和角速度。
输入给变速器的转矩 $T_t$ 为

T_{t} = (I_{t} + I_{w} R^{2} + m r_{e f f}^{2} R^{2}) \overset{\cdot}{ω_{e}} + R T_{b} + r_{e f f} R (R_{x} + F_{a e r o} + m g s i n θ)

$T_t=(I_t+I_wR^2+mr_{eff}^2R^2)\overset{·}{\omega _e}+RT_{b}+r_{eff}R(R_x+F_{aero}+mgsin\theta)$

3.3 发动机净扭矩

图5 发动机惯性模型示意图

有如下方程式：

{\begin{cases} I_{e} \overset{\cdot}{ω_{e}} = T_{e} - T_{p} \\ T_{p} = T_{t} \end{cases}

$\begin{cases} I_e \overset{·}{\omega _e}=T_e-T_p\\ T_p=T_t \end{cases}$

其中 $I_e、\overset{·}{\omega _e}$ 是曲轴的转动惯量和角加速度， $I_e \overset{·}{\omega _e}$ 是曲轴的力矩损耗， $T_p$ 是发动机输出的转矩。
输入给曲轴的转矩 $T_e$ 为

\begin{matrix} (5) & T_{e} = (I_{e} + I_{t} + I_{w} R^{2} + m r_{e f f}^{2} R^{2}) \overset{\cdot}{ω_{e}} + R T_{b} + r_{e f f} R (R_{x} + F_{a e r o} + m g s i n θ) \end{matrix}

$T_e = (I_e+I_t+I_wR^2+mr_{eff}^2R^2)\overset{·}{\omega _e}+RT_{b}+r_{eff}R(R_x+F_{aero}+mgsin\theta)\tag{5}$

4. 车辆加速度与发动机净扭矩关系式

忽略风速，则空气阻力与车辆纵向运动相对速度的平方成正比，有 $F_{aero}=c_x\overset{·}{x}^2$ ， $c_x$ 为空气阻力系数。而

\begin{matrix} (6) & \overset{\cdot}{x} = r_{e f f} ω_{w} = r_{e f f} R ω_{e} \end{matrix}

$\overset{·}{x}=r_{eff}\omega _w=r_{eff}R{\omega _e}\tag{6}$

结合 $(5)、(6)$ ，得到发动机输出的角加速度与发动机净扭矩的关系

\begin{matrix} (7) & \overset{\cdot}{ω_{e}} = \frac{1}{J_{e}} (T_{e} - c_{x} R^{3} r_{e f f}^{3} ω_{e}^{2} - R T_{b} - r_{e f f} R (R_{x} + m g s i n θ)) \end{matrix}

$\overset{·}{\omega _e}=\dfrac{1}{J_e}(T_e-c_xR^3r_{eff}^3\omega _e^2-RT_{b}-r_{eff}R(R_x+mgsin\theta))\tag{7}$

其中等效转动惯量 $J_e=I_e+I_t+I_wR^2+mr_{eff}^2R^2$ 。
结合 $(3)、(6)、(7)$ ，得到车辆加速度与发动机净扭矩之间关系

\begin{matrix} (8) & \overset{\cdot \cdot}{x} = \frac{r_{e f f} R}{J_{e}} (T_{e} - c_{x} R r_{e f f} {\overset{\cdot}{x}}^{2} - R T_{b} - r_{e f f} R (R_{x} + m g s i n θ)) \end{matrix}

$\small \overset{··}{x}=\dfrac{r_{eff}R}{J_e}(T_e-c_xRr_{eff}\overset{·}{x}^2-RT_{b}-r_{eff}R(R_x+mgsin\theta))\tag{8}$

5. 简化的车辆纵向动力学方程

不考虑路面的倾斜度，即 $\theta=0$ ，公式(8)可化简为

\overset{\cdot \cdot}{x} = \frac{r_{e f f} R}{J_{e}} (T_{e} - c_{x} R r_{e f f} {\overset{\cdot}{x}}^{2} - R (r_{e f f} R_{x} + T_{b}))

$\overset{··}{x}=\dfrac{r_{eff}R}{J_e}(T_e-c_xRr_{eff}\overset{·}{x}^2-R(r_{eff}R_x+T_{b}))$

令

{\begin{cases} M = J_{e} \\ f = R^{2} r_{e f f}^{2} R_{x} \\ c = R^{2} r_{e f f}^{2} c_{x} \\ u = R r_{e f f} (T_{e} - R T_{b}) \end{cases}

$\begin{cases} M=J_e\\ f=R^2r_{eff}^2R_x\\ c=R^2r_{eff}^2c_x\\ u=Rr_{eff}(T_e-RT_b) \end{cases}$

其中 $M、f、c$ 为系统参数， $u$ 为控制参数。

则得到简化后车辆纵向动力学方程

\begin{matrix} (9) & \overset{\cdot \cdot}{x} = \frac{u - c {\overset{\cdot}{x}}^{2} - f}{M} \end{matrix}

$\overset{··}{x}=\dfrac{u-c\overset{·}{x}^2-f}{M}\tag{9}$

二、车辆编队纵向滑模控制系统结构

1、车辆纵向跟随队列结构

如下图6所示是车辆跟随队列结构图，第i辆车应与前一辆车(即第 $i-i$ 车)应该保持 $l_i$ 的期望车距， $x_i$ 表示第 $i$ 辆车到参考位置的距离。定义间距误差 $\epsilon$ 是与前一车辆的实际间距与期望的车辆间距之间的差异，即

\begin{matrix} (10) & ϵ_{i} = x_{i - 1} - x_{i} + l_{i} i = 1, 2, . . . \end{matrix}

$\epsilon_i=x_{i-1}-x_i+l_i\qquad i=1,2,...\tag{10}$

图6 车辆纵向跟随队列结构图

显然，系统控制目标是

\begin{matrix} (11) & ϵ_{i} \to 0 \end{matrix}

$\epsilon_i\rarr 0\tag{11}$

假设车辆编队中的所有车均满足车辆纵向动力学方程 $(9)$ ，则 $(9)$ 可以改写成

\begin{matrix} (12) & \overset{\cdot \cdot}{x_{i}} = \frac{u_{i} - c_{i} {\overset{\cdot}{x_{i}}}^{2} - f_{i}}{M_{i}} i = 0, 1, 2, . . . \end{matrix}

$\overset{··}{x_i}=\dfrac{u_i-c_i\overset{·}{x_i}^2-f_i}{M_i}\qquad i=0,1,2,...\tag{12}$

2、车辆跟随滑模系统设计

滑模变结构控制是根据系统所期望的动态特性来设计系统的切换超平面，通过滑动模态控制器使系统状态从超平面之外向切换超平面收束。系统一旦到达切换超平面，控制作用将保证系统沿切换超平面到达系统原点，这一沿切换超平面向原点滑动的过程称为滑模控制。接下来具体说明如何把滑模控制应用在车辆纵向跟随队列中。

2.1 切换函数设计

图7 车辆跟随滑模系统状态轨迹运动图

令 $e_{i1}=\epsilon_i、e_{i2}=\overset{·}{e_{i1}}$ ，结合公式 $(10)、(12)$ ，可以整理出状态方程

\begin{matrix} (13) & {\begin{cases} \overset{\cdot}{e_{i 1}} = e_{i 2} \\ \overset{\cdot}{e_{i 2}} = \frac{u_{i}}{M_{i}} - \frac{c_{i}}{M_{i}} (e_{i 2} + \overset{\cdot}{x_{i - 1}})^{2} - \frac{F_{i}}{M_{i}} - \overset{\cdot \cdot}{x_{i - 1}} \end{cases} \end{matrix}

$\small\begin{cases} \overset{·}{e_{i1}}=e_{i2}\\ \overset{·}{e_{i2}}=\dfrac{u_i}{M_i}-\dfrac{c_i}{M_i}(e_{i2}+\overset{·}{x_{i-1}})^2-\dfrac{F_i}{M_i}-\overset{··}{x_{i-1}}\tag{13} \end{cases}$

可以设计切换函数

\begin{matrix} (14) & S_{i} = q_{1} e_{i 1} + q_{2} e_{i 2} q_{1} > 0, q_{2} > 0 \end{matrix}

$S_i=q_1e_{i1}+q_2e_{i2}\qquad q_1>0,q_2>0\tag{14}$

该切换函数满足滑模稳定性，即图7中的②。这是因为一旦系统状态达到切换超平面，即 $S_i=0$ ，那么可以解得 $\epsilon_i=e^{-\frac{q_1}{q_2}t}$ 。所以当 $t\rarr\infty$ 时， $\epsilon_i\rarr0$ ，满足控制目标要求。

2.2 滑模控制律设计

常用的滑模趋近律有

指数趋近律： $\small\overset{·}{S_i}=-\lambda S_i$
等速趋近律： $\small\overset{·}{S_i}=-\lambda sgn(S_i)$
准滑模控制： $\small\overset{·}{S_i}=-\lambda sat(S_i)$ ， $\small sat(s)= \begin{cases} -1,\qquad s<-Δ\\ s/Δ,\quad-Δ\le s \leΔ\\ \quad 1,\qquad s>Δ \end{cases}$

这里仅采用指数趋近律，设计滑模控制律(仿真中三种趋近律都采用了，并且进行了比较)。
对公式 $(14)$ 左右两边求导，结合公式 $(13)$ ，得到的表达式与指数趋近律联立消去 $\overset{·}{S_i}$ ，得到控制律

\begin{matrix} (15) & u_{i} = c_{i} (e_{i 2} + \overset{\cdot}{x_{i - 1}})^{2} - \frac{M_{i} q_{1}}{q_{2}} e_{i 2} + M_{i} \overset{\cdot \cdot}{x_{i - 1}} + F_{i} - \frac{M_{i}}{q_{2}} λ S_{i} \end{matrix}

$u_i = c_i(e_{i2}+\overset{·}{x_{i-1}})^2-\dfrac{M_iq_1}{q_2}e_{i2}+M_i\overset{··}{x_{i-1}}+F_i-\dfrac{M_i}{q_2}\lambda S_i\tag{15}$

该控制律满足滑模可达性，即图7中的①。证明方法如下：
取 $Lyapunov$ 函数 $V_i=\frac{1}{2}S_i^2$ ，对时间求导，结合公式 $(13)、(14)、(15)$ 得

\begin{aligned} \overset{\cdot}{V_{i}} & = S_{i} \overset{\cdot}{S_{i}} = S_{i} (q_{1} \overset{\cdot}{e_{i 1}} + q_{2} \overset{\cdot}{e_{i 2}}) \\ = S_{i} (q_{1} e_{i 2} + q_{2} [\frac{u_{i}}{M_{i}} - \frac{c_{i}}{M_{i}} (e_{i 2} + \overset{\cdot}{x_{i - 1}})^{2} - \frac{F_{i}}{M_{i}} - \overset{\cdot \cdot}{x_{i - 1}}]) \\ = - λ S_{i}^{2} < 0 \end{aligned}

$\begin{aligned} \overset{·}{V_i}&=S_i\overset{·}{S_i}=S_i(q_1\overset{·}{e_{i1}}+q_2\overset{·}{e_{i2}})\\ &=S_i(q_1e_{i2}+q_2[\dfrac{u_i}{M_i}-\dfrac{c_i}{M_i}(e_{i2}+\overset{·}{x_{i-1}})^2-\dfrac{F_i}{M_i}-\overset{··}{x_{i-1}}])\\ &=-\lambda S_i^2<0 \end{aligned}$

可知当 $S_i>0$ 时， $\overset{·}{S_i}<0$ ； $S_i<0$ 时， $\overset{·}{S_i}>0$ ，所以 $S_i\rarr0$ 。

三、车辆编队滑模控制算法设计

下图8展示的是车辆跟随滑模控制算法流程。算法在设计上的主要难点是如何把一个连续的运动过程表达出来，这里将其离散化，以 $min\_time$ 为时间间隔，间隔数 $nums$ ，车辆系统运行的总时间为 $min\_time*nums$ 。每个时间间隔内，根据起始车0的加速度得到车0的状态，然后依次更新车1、车2、车3...的控制律，控制这些车的状态。更新某车控制律的方法就是公式 $(15)$ ，得到某车(非车0)加速度的方法就是公式 $(12)$ ，得到某车速度的方法是前一时间间隔的速度同这两次加速度平均值与时间间隔的乘积之和，得到某车位移的方法是前一时间间隔的位移同这两次速度平均值与时间间隔的乘积之和。至于车0的加速度规律，我们可以自主设计。(在第四节具体程序实现时，没有按j进行循环，而是重复列举计算，效果一致)

图8 车辆跟随滑模控制算法流程图

四、仿真程序的编写

实验环境：AMD R53500U 64位win10操作系统。编程语言是conda python3.8.13 。绘图库：matplotlib3.5.1。仿真程序脚本如下：

import matplotlib.pyplot as plt

'''
指数趋近律、等速趋近律、准滑模控制的车辆跟随问题仿真,
运行结果以图片形式保存在同目录下。
'''

# q1, q2分别是切换函数ei1, ei2前面的系数
q1, q2 = 1.5, 2
# lan是指数趋近律前面的系数
lan = 0.6
# 设定期望车间距均为18
l1, l2, l3, l4 = 18, 18, 18, 18
# 设定汽车质量
m1, m2, m3, m4 = 2000, 1500, 1500, 1000
# 设定动力学模型分子的速度平方项前的系数ci(按照模型符号是负的)
c1, c2, c3, c4 = 0.8, 0.6, 0.6, 0.5
# 设定动力学模型分子的常数项系数Fi(按照模型符号是负的)
f1, f2, f3, f4 = 300, 250, 250, 200
# 设定五辆车汽车的位移、速度、加速度
x0, x1, x2, x3, x4 = [100.], [84.], [70.], [49.5], [32.]  
v0, v1, v2, v3, v4 = [20.], [12.], [16.], [15.], [16.]
a1, a2, a3, a4 = [0.], [0.], [0.], [0.]


# 设定趋近律
def reaching_law(m:int , s:float, q2:int, mode='exponential'):
    '''
    mode: 指数趋近律exponential
          等速趋近律uniform 
          准滑模控制quasi_sliding
    '''
    if mode == 'exponential':
        return -m * lan * s / q2
    if mode == 'uniform':
        epslion = 0.3
        if s > 0:
            return -m * epslion / q2
        if s == 0:
            return 0
        if s < 0:
            return m * epslion / q2
    if mode == 'quasi_sliding':
        delta, epslion = 0.8, 2.
        if s < -delta:
            return m * epslion / q2
        if s > delta:
            return -m * epslion / q2
        else:
            return -m * epslion * s / (delta * q2)


# 设定第一辆车的加速度(分段函数), 要注意t的长度和a0的长度相等
def get_a0(t:list):    
    a0 = []
    for i in t:
        if i < 2:
            a0.append(0)
            continue
        if i >= 2 and i < 5:
            a0.append(-1 / 6 * (i-2))
            continue
        if i >= 5 and i < 12:
            a0.append(-0.5)
            continue
        if i >= 12 and i < 18:
            a0.append((i-12) / 3 - 0.5) 
            continue
        if i >= 18 and i < 21:
            a0.append(1.5)
            continue
        if i >= 21 and i < 24:
            a0.append((21 - i)/2 + 1.5)
            continue
        if i >= 24 and i <= 30:
            # 注意i=30, 所以是取两端, 故为301份
            a0.append(0)
    return a0


if __name__ == "__main__":
    # 将30秒划分成301份, [0, 0.1, 0.2, ..., 29.9, 30]
    t = [float(i/10) for i in range(301)] 
    a0 = get_a0(t)

    # 四辆车的车间距误差ei1列表
    e11 = [x1[0] - x0[0] + l1]
    e21 = [x2[0] - x1[0] + l2]
    e31 = [x3[0] - x2[0] + l3]
    e41 = [x4[0] - x3[0] + l4]

    # 四辆车的车间距误差导数ei2的列表
    e12 = [v1[0] - v0[0]]
    e22 = [v2[0] - v1[0]]
    e32 = [v3[0] - v2[0]]
    e42 = [v4[0] - v3[0]]

    # 四辆车切换函数的列表
    s1 = [q1 * e11[0] + q2 * e12[0]]
    s2 = [q1 * e21[0] + q2 * e22[0]]
    s3 = [q1 * e31[0] + q2 * e32[0]]
    s4 = [q1 * e41[0] + q2 * e42[0]]

    # 四辆车控制律的列表
    u1, u2, u3, u4 = [0], [0], [0], [0]

    for i in range(1, 301):
        # 最前车0的速度、加速度更新，可以看出更新时用了直线等效, 
        # 0.1指的是时间标度(列表t划分的, 也是之后绘图打印的x轴)
        v0.append(v0[i-1] + 0.1 * (a0[i] + a0[i - 1]) * 0.5)
        x0.append(x0[i-1] + 0.1 * (v0[i] + v0[i - 1]) * 0.5)

        # 车1的车间距误差及导数更新
        e11.append(x1[i-1] - x0[i-1]+l1)
        e12.append(v1[i-1] - v0[i-1])
        # 车1的切换函数更新
        s1.append(q1 * e11[i] + q2 * e12[i])
        # 等效控制
        u1equ = c1 * (e12[i] + v0[i]) * (e12[i] + v0[i]) - m1 * \
            q1 * e12[i] / q2 + m1 * a0[i] + f1 
        # 反馈控制(指数趋近律)
        # # 默认采用指数趋近律, 下同
        # u1n = reaching_law(m1, s1[i], q2)    
        # # 采用等速趋近律
        # u1n = reaching_law(m1, s1[i], q2, mode='uniform')
        # 采用准滑模控制
        u1n = reaching_law(m1, s1[i], q2, mode='quasi_sliding')
        # 更新控制律
        u1.append(u1equ + u1n)
        # 利用控制律更新车1的加速度、速度、位移,加速度是利用动力学模型得到的
        a1.append((-c1 * v1[i-1] * v1[i-1] + u1[i] - f1) / m1)
        v1.append(v1[i-1] + 0.1 * (a1[i] + a1[i - 1]) * 0.5)
        x1.append(x1[i-1] + 0.1 * (v1[i] + v1[i - 1]) * 0.5)
        
        # 车2、3、4过程同车1  
        e21.append(x2[i-1] - x1[i-1]+l2)
        e22.append(v2[i-1] - v1[i-1])
        s2.append(q1 * e21[i] + q2 * e22[i])
        u2equ = c2 * (e22[i] + v1[i]) * (e22[i] + v1[i]) - m2 * q1 *\
             e22[i] / q2 + m2 * a1[i] + f2
        # 默认采用指数趋近律
        # u2n = reaching_law(m2, s2[i], q2)     
        # # 采用等速趋近律
        # u2n = reaching_law(m2, s2[i], q2, mode='uniform')     
        # # 采用准滑模控制
        u2n = reaching_law(m2, s2[i], q2, mode='quasi_sliding') 

        u2.append(u2equ + u2n)
        a2.append((-c2 * v2[i-1] * v2[i-1] + u2[i] -f2) / m2)
        v2.append(v2[i-1] + 0.1 * (a2[i] + a2[i - 1]) * 0.5)
        x2.append(x2[i-1] + 0.1 * (v2[i] + v2[i - 1]) * 0.5)

        e31.append(x3[i-1] - x2[i-1]+l3)
        e32.append(v3[i-1] - v2[i-1])
        s3.append(q1 * e31[i] + q2 * e32[i])
        u3equ = c3 * (e32[i] + v2[i]) * (e32[i] + v2[i]) - m3 * q1 *\
             e32[i] / q2 + m3 * a2[i] + f3 
        # u3n = reaching_law(m3, s3[i], q2)
        # u3n = reaching_law(m3, s3[i], q2, mode='uniform')
        u3n = reaching_law(m3, s3[i], q2, mode='quasi_sliding')
        u3.append(u3equ + u3n)
        a3.append((-c3 * v3[i-1] * v3[i-1] + u3[i] -f3) / m3)
        v3.append(v3[i-1] + 0.1 * (a3[i] + a3[i - 1]) * 0.5)
        x3.append(x3[i-1] + 0.1 * (v3[i] + v3[i - 1]) * 0.5)


        e41.append(x4[i-1] - x3[i-1]+l4)
        e42.append(v4[i-1] - v3[i-1])
        s4.append(q1 * e41[i] + q2 * e42[i])
        u4equ = c4 * (e42[i] + v3[i]) * (e42[i] + v3[i]) - m4 * q1 *\
             e42[i] / q2 + m4 * a3[i] + f4 
        # u4n = reaching_law(m4, s4[i], q2)
        # u4n = reaching_law(m4, s4[i], q2, mode='uniform')
        u4n = reaching_law(m4, s4[i], q2, mode='quasi_sliding')
        u4.append(u4equ + u4n)
        a4.append((-c4 * v4[i-1] * v4[i-1] + u4[i] -f4) / m4)
        v4.append(v4[i-1] + 0.1 * (a4[i] + a4[i - 1]) * 0.5)
        x4.append(x4[i-1] + 0.1 * (v4[i] + v4[i - 1]) * 0.5)
    
    
    # 开始绘图
    # 绘制加速度曲线
    plt.figure()                            # 设置画布
    plt.plot(t, a0, label='car 0')     # :是指绘制点划线
    plt.plot(t, a1, label='car 1')
    plt.plot(t, a2, label='car 2')
    plt.plot(t, a3, label='car 3')
    plt.plot(t, a4, label='car 4')
    plt.xlabel("Time(s)",fontsize=13)
    plt.ylabel("Acceleration(m/s^2)",fontsize=13)
    plt.xlim(0, 30)    
    plt.legend()
    plt.savefig('./acceleration.png')       # 保存图像

    # 绘制速度曲线
    plt.clf()                        # 清空画布，不然会前后图像会重叠
    plt.plot(t, v0, ':', label='car 0')    
    plt.plot(t, v1, ':', label='car 1')
    plt.plot(t, v2, ':', label='car 2')
    plt.plot(t, v3, ':', label='car 3')
    plt.plot(t, v4, ':', label='car 4')
    plt.xlabel("Time(s)",fontsize=13)
    plt.ylabel("velocity(m/s)",fontsize=13)
    plt.xlim(0, 30)    
    plt.legend()
    plt.savefig('./velocity.png')    # 保存图像

    # 绘制位置曲线
    plt.clf()
    plt.plot(t, x0, ':', label='car 0')
    plt.plot(t, x1, ':', label='car 1')
    plt.plot(t, x2, ':', label='car 2')
    plt.plot(t, x3, ':', label='car 3')
    plt.plot(t, x4, ':', label='car 4')
    plt.xlabel("Time(s)",fontsize=13)
    plt.ylabel("position(m)",fontsize=13)
    plt.xlim(0, 30)    
    plt.legend()
    plt.savefig('./position.png')

    # 绘制车间距误差ei1曲线
    plt.clf()
    plt.plot(t, e11, label='car 1')
    plt.plot(t, e21, label='car 2')
    plt.plot(t, e31, label='car 3')
    plt.plot(t, e41, label='car 4')
    plt.xlabel("Time(s)",fontsize=13)
    plt.ylabel("space error(m)",fontsize=13)
    plt.xlim(0, 30)
    plt.legend()
    plt.savefig('./space_error.png')

    # 绘制车间距误差导数ei2曲线
    plt.clf()
    plt.plot(t, e12, ':', label='car 1')
    plt.plot(t, e22, ':', label='car 2')
    plt.plot(t, e32, ':', label='car 3')
    plt.plot(t, e42, ':', label='car 4')
    plt.xlabel("Time(s)",fontsize=13)
    plt.ylabel("space_error_derivative(m)",fontsize=13)
    plt.xlim(0, 30)
    plt.legend()
    plt.savefig('./space_error_derivative.png')

    # 绘制切换函数曲线
    plt.clf()
    plt.plot(t, s1, label='car 1')
    plt.plot(t, s2, label='car 2')
    plt.plot(t, s3, label='car 3')
    plt.plot(t, s4, label='car 4')
    plt.xlabel("Time(s)",fontsize=13)
    plt.ylabel("Switching Function",fontsize=13)
    plt.xlim(0, 30)
    plt.legend()
    plt.savefig('./Switching_Function.png')

    # 绘制控制输入U曲线
    plt.clf()
    plt.plot(t, u1, label='car 1')
    plt.plot(t, u2, label='car 2')
    plt.plot(t, u3, label='car 3')
    plt.plot(t, u4, label='car 4')
    plt.xlabel("Time(s)",fontsize=13)
    plt.ylabel("Control Input",fontsize=13)
    plt.xlim(0, 30)
    plt.legend()
    plt.savefig('./Control_Input.png')

五、仿真结果和分析

1. 参数设定

分别设置四辆车的质量、空气阻力相关的系数、常数项系数；设置期望间距；设置五辆车的初始位移、速度、加速度；设置指数趋近律前面的系数 $\lambda$ ；设置切换函数前的系数，具体结果如下

# q1, q2分别是切换函数ei1, ei2前面的系数
q1, q2 = 1.5, 2
lan = 0.6 # lan是指数趋近律前面的系数
epslion = 0.3 # 等速趋近律参数
delta, epslion = 0.8, 2. # 准滑模控制参数
# 设定期望车间距均为18
l1, l2, l3, l4 = 18, 18, 18, 18
# 设定汽车质量
m1, m2, m3, m4 = 2000, 1500, 1500, 1000
# 设定动力学模型分子的速度平方项前的系数ci(按照模型符号是负的)
c1, c2, c3, c4 = 0.8, 0.6, 0.6, 0.5
# 设定动力学模型分子的常数项系数Fi(按照模型符号是负的)
f1, f2, f3, f4 = 300, 250, 250, 200
# 设定五辆车汽车的位移、速度、加速度
x0, x1, x2, x3, x4 = [100.], [84.], [70.], [49.5], [32.]  
v0, v1, v2, v3, v4 = [20.], [12.], [16.], [15.], [16.]
a1, a2, a3, a4 = [0.], [0.], [0.], [0.]

设置车0的加速度，方程如下

a_{0} = {\begin{cases} 0 t < 2 \\ - \frac{t - 2}{6} 2 \leq t < 5 \\ - 0.5 5 \leq t < 12 \\ \frac{t - 12}{3} - 0.5 12 \leq t < 18 \\ 1.5 18 \leq t < 21 \\ - \frac{t - 21}{2} + 1.5 21 \leq t < 24 \\ 0 24 \leq t < 30 \end{cases}

$\small a_0=\begin{cases} 0\qquad \qquad \qquad \qquad \quad t<2\\ -\frac{t-2}{6}\qquad\qquad \quad 2\le t<5\\ -0.5\qquad\qquad\qquad 5\le t<12\\ \frac{t-12}{3}-0.5\qquad \quad 12\le t<18\\ 1.5\qquad \quad\qquad \qquad 18\le t<21\\ -\frac{t-21}{2}+1.5\qquad 21\le t<24\\ 0\qquad \quad\qquad \quad\qquad 24\le t<30 \end{cases}$

2. 指数趋近律

图9 指数趋近律控制仿真结果

上图是指数趋近律控制仿真结果。结果与我们前面理论推导的相符合，采用指数趋近律，滑模在非有限时间内可达，切换函数最终趋近于0，车间距误差最终趋近于0，趋近律的控制速度很快。没有抖振，并且控制输入比较平滑，在本场景的参数设定下，是比较理想的控制方式。

3. 等速趋近律

下图表示的是等速趋近律仿真结果。发现加速度出现明显的抖振，尤其是队列中后面的车抖振的更加明显，滑模稳定性很差。同时可以发现等速趋近律控制速度很慢，相同时间内可达性能远不如指数趋近律。在本场景的参数设定下，该控制方式不理想。

图10 等速趋近律仿真结果

4. 准滑模控制

图11 准滑模控制仿真结果

上图表示的是准滑模控制仿真结果。没有抖振，滑模稳定性较好，趋近律的控制速度比较快，滑模在有限时间内可达，但加速度和控制输入的平滑性不如指数趋近律。在本场景的参数设定下，也是比较理想的控制方式。

5. 仿真结论

在本场景的参数设定下，如果追求控制的速度和平滑性，选用指数趋近律作为控制方式；如果追求控制的长期精确性，选用准滑模控制作为控制方式；等速趋近律会产生抖振，且控制速度很慢，一般不选用。

上一篇悬架模糊控制

下一篇ubuntu使用相关——更新中

posted @ 2022-06-17 18:38 静候佳茵阅读(1112) 评论(0) 编辑收藏举报

刷新页面返回顶部

登录后才能查看或发表评论，立即登录或者逛逛博客园首页

静候佳茵