三个差速无人车，一辆车作为领航车，两辆车作为跟随车，基于滑模控制器，先排成三角形，再排成一字型，再排成纵队，python代码

以下是基于滑模控制器实现无人车排成三角形、一字型、纵队的 Python 代码：

import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt

# 定义控制器参数
k1 = 1
k2 = 1

# 定义无人车初始状态
x1_0 = np.array([0, 0, 0, 0])
x2_0 = np.array([5, 0, 0, 0])
x3_0 = np.array([2.5, 4.3, np.pi/3, 0])
x4_0 = np.array([10, 0, 0, 0])
x5_0 = np.array([7.5, 4.3, np.pi/3, 0])
x6_0 = np.array([5, 8.7, np.pi/3, 0])

# 定义控制输入函数
def u(t, x, x_des):
    e = x - x_des
    v_des = -k1 * e[:2] - k2 * e[2:]
    return np.array([v_des[0] * np.cos(x[2]) + v_des[1] * np.sin(x[2]),
                     -v_des[0] * np.sin(x[2]) + v_des[1] * np.cos(x[2])])

# 定义运动学函数
def f(t, x, u):
    return np.array([x[3] * np.cos(x[2]), x[3] * np.sin(x[2]), u[0], u[1]])

# 定义仿真函数
def simulate(x1_0, x2_0, x3_0, x4_0, x5_0, x6_0, x_des, t_end):
    x1 = x1_0
    x2 = x2_0
    x3 = x3_0
    x4 = x4_0
    x5 = x5_0
    x6 = x6_0
    t_eval = np.linspace(0, t_end, 1000)
    x1_hist = [x1]
    x2_hist = [x2]
    x3_hist = [x3]
    x4_hist = [x4]
    x5_hist = [x5]
    x6_hist = [x6]
    for t in t_eval:
        u1 = u(t, x1, x_des[0])
        u2 = u(t, x2, x_des[1])
        u3 = u(t, x3, x_des[2])
        u4 = u(t, x4, x_des[3])
        u5 = u(t, x5, x_des[4])
        u6 = u(t, x6, x_des[5])
        x1 = solve_ivp(f, (t, t+0.01), x1, args=(u1,)).y[:, -1]
        x2 = solve_ivp(f, (t, t+0.01), x2, args=(u2,)).y[:, -1]
        x3 = solve_ivp(f, (t, t+0.01), x3, args=(u3,)).y[:, -1]
        x4 = solve_ivp(f, (t, t+0.01), x4, args=(u4,)).y[:, -1]
        x5 = solve_ivp(f, (t, t+0.01), x5, args=(u5,)).y[:, -1]
        x6 = solve_ivp(f, (t, t+0.01), x6, args=(u6,)).y[:, -1]
        x1_hist.append(x1)
        x2_hist.append(x2)
        x3_hist.append(x3)
        x4_hist.append(x4)
        x5_hist.append(x5)
        x6_hist.append(x6)
    return np.array(x1_hist), np.array(x2_hist), np.array(x3_hist), np.array(x4_hist), np.array(x5_hist), np.array(x6_hist)

# 排成三角形
x_des = [np.array([0, 0, 0, 0]), np.array([5, 0, 0, 0]), np.array([2.5, 4.3, np.pi/3, 0])]
t_end = 10
x1_hist, x2_hist, x3_hist, _, _, _ = simulate(x1_0, x2_0, x3_0, x4_0, x5_0, x6_0, x_des, t_end)

# 绘制图像
plt.plot(x1_hist[:, 0], x1_hist[:, 1], label='Vehicle 1')
plt.plot(x2_hist[:, 0], x2_hist[:, 1], label='Vehicle 2')
plt.plot(x3_hist[:, 0], x3_hist[:, 1], label='Vehicle 3')
plt.legend()
plt.axis('equal')
plt.show()

# 排成一字型
x_des = [np.array([0, 0, 0, 0]), np.array([5, 0, 0, 0]), np.array([10, 0, 0, 0])]
t_end = 10
x1_hist, x2_hist, _, x4_hist, x5_hist, _ = simulate(x1_0, x2_0, x3_0, x4_0, x5_0, x6_0, x_des, t_end)

# 绘制图像
plt.plot(x1_hist[:, 0], x1_hist[:, 1], label='Vehicle 1')
plt.plot(x2_hist[:, 0], x2_hist[:, 1], label='Vehicle 2')
plt.plot(x4_hist[:, 0], x4_hist[:, 1], label='Vehicle 4')
plt.plot(x5_hist[:, 0], x5_hist[:, 1], label='Vehicle 5')
plt.legend()
plt.axis('equal')
plt.show()

# 排成纵队
x_des = [np.array([0, 0, 0, 0]), np.array([0, 2, np.pi/2, 0]), np.array([0, 4, np.pi/2, 0]),
         np.array([0, 6, np.pi/2, 0]), np.array([0, 8, np.pi/2, 0]), np.array([0, 10, np.pi/2, 0])]
t_end = 10
x1_hist, _, _, x4_hist, _, x6_hist = simulate(x1_0, x2_0, x3_0, x4_0, x5_0, x6_0, x_des, t_end)

# 绘制图像
plt.plot(x1_hist[:, 0], x1_hist[:, 1], label='Vehicle 1')
plt.plot(x4_hist[:, 0], x4_hist[:, 1], label='Vehicle 4')
plt.plot(x6_hist[:, 0], x6_hist[:, 1], label='Vehicle 6')
plt.legend()
plt.axis('equal')
plt.show()

以上代码中，我们首先定义了控制器参数 `k1` 和 `k2`，以及无人车的初始状态 `x1_0`、`x2_0`、`x3_0`、`x4_0`、`x5_0`、`x6_0`。

然后，我们定义了控制输入函数 `u` 和运动学函数 `f`，并使用 `solve_ivp` 函数进行数值积分。最后，我们通过 `simulate` 函数来模拟三辆无人车排成三角形、一字型和纵队的运动。

在绘制图像时，我们使用 `matplotlib` 库来绘制无人车的运动轨迹。