AirSim多无人机协同集群编队

首先要修改settings.json文件，来设置多无人机的初始位置

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/main/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "Vehicles": {
    "UAV1": {
      "VehicleType": "SimpleFlight",
      "X": 0, "Y": 0, "Z": 0,
      "Yaw": 0
    },
    "UAV2": {
      "VehicleType": "SimpleFlight",
      "X": 2, "Y": 0, "Z": 0,
      "Yaw": 0
    },
    "UAV3": {
      "VehicleType": "SimpleFlight",
      "X": 4, "Y": 0, "Z": 0,
      "Yaw": 0
    },
    "UAV4": {
      "VehicleType": "SimpleFlight",
      "X": 0, "Y": -3, "Z": 0,
      "Yaw": 0
    },
    "UAV5": {
      "VehicleType": "SimpleFlight",
      "X": 2, "Y": -2, "Z": 0,
      "Yaw": 0
    },
    "UAV6": {
      "VehicleType": "SimpleFlight",
      "X": 4, "Y": -3, "Z": 0,
      "Yaw": 0
    },
    "UAV7": {
      "VehicleType": "SimpleFlight",
      "X": 0, "Y": 3, "Z": 0,
      "Yaw": 0
    },
    "UAV8": {
      "VehicleType": "SimpleFlight",
      "X": 2, "Y": 2, "Z": 0,
      "Yaw": 0
    },
    "UAV9": {
      "VehicleType": "SimpleFlight",
      "X": 4, "Y": 3, "Z": 0,
      "Yaw": 0
    }
  }
}

​ 集群编队控制有集中式和分布式两种，集中式控制受限于中心计算机计算资源的限制，无法做到大规模集群编队。而分布式控制，理论上可以做到无限规模的集群编队。本文实现的集群控制算法就属于一种分布式的控制算法。

​ 分布式集群控制最早是由Reynolds在1987提出的分布式协同的三定律：避碰、速度一致、中心聚集。只要每个无人机都满足这三个条件就能形成集群飞行的效果。后续的集群研究都是在三定律的基础上进行的，或者说后续的集群算法都基本满足这三个定律，只是满足的形式各有区别。

​ 本文参考论文中的集群算法，在AirSim中实线多无人机集群飞行的效果。论文中的集群控制算法是三个速度指令相加的：

• 避碰：\(v_i^\mathrm{sep}=-\dfrac{k^\mathrm{sep}}{\|N_i\|}\sum\dfrac{r_{ij}}{\|r_{ij}\|^2}\) ,
• 中心聚集：\(v_i^\mathrm{coh}=\dfrac{k^\mathrm{coh}}{\|N_i\|}\sum r_{ij}\) ,
• 整体迁移（速度一致）： \(v_i^\mathrm{mig}=k^\mathrm{mig}\dfrac{r_i^\mathrm{mig}}{\|r_i^\mathrm{mig}\|}\) .

其中：

• \(k\) 是系数，
• \(N_i=\{\mathrm{agents} \ j:j\neq i \bigwedge \|r_{ij}\|<r^\mathrm{max} \}\) ,
  • \(N_i\) 表示的是每个无人机的邻居是如何选择的，也就是说对于每个无人机来说，其周围半径范围内的其他无人机都是自己的邻居，
  • 所以 \(\|N_i\|\) 表示的是第个无人机周围邻居的个数。
• $r_{ij}=p_j-p_i $表示的是两架无人机之间的距离
• \(r_i^\mathrm{mig}=p^\mathrm{mig}-p_i\) 表示的是第个无人机到目标点的距离

这个集群算法比较简单，避碰和中心聚集这两项的公式很好理解。论文将速度一致项变形为了整体迁移，因为论文想要实现的目标是让无人机集群到达全局的一个固定位置。

添加速度限幅后，最终无人机的速度指令为：

\(v_i=\dfrac{\tilde{v}_i}{\|\tilde{v}_i\|}\min(\|\tilde{v}_i\|,v^\max)\)

其中：\(\tilde{v}_i=v_i^\mathrm{sep}+v_i^\mathrm{coh}+v_i^\mathrm{mig}\)

其中的参数设置如下：

• \(r^{\max}=20\mathrm{m}\) ,
• $v^{\max}=2\mathrm{m/s} $,
• \(k^\mathrm{sep}=7\) ,
• \(k^\mathrm{coh}=1\) ,
• \(k^\mathrm{mig}=1\) .

最后形成的集群效果应该是每两个相邻的无人机都保持同样的距离的编队。

import airsim
import time
import numpy as np

origin_x = [0, 2, 4,  0,  2,  4, 0, 2, 4]       # 无人机初始位置
origin_y = [0, 0, 0, -3, -2, -3, 3, 2, 3]

# 用于获取指定无人机的当前位置,这个位置是相对于其原始起始位置的偏移
def get_UAV_pos(client, vehicle_name="SimpleFlight"):
    global origin_x
    global origin_y
    state = client.simGetGroundTruthKinematics(vehicle_name=vehicle_name)	# 获取运动学状态
    x = state.position.x_val		# 提取xy坐标
    y = state.position.y_val
    i = int(vehicle_name[3])		# 获取无人机编号vehicle_name=UAV2,则i=2
    x += origin_x[i - 1]
    y += origin_y[i - 1]			# 将无人机的当前x和y坐标调整为相对于其初始位置的坐标
    pos = np.array([[x], [y]])
    return pos						# 返回调整后的xy坐标


client = airsim.MultirotorClient()  		# 建立到AirSim仿真器的链接
for i in range(9):
    name = "UAV"+str(i+1)
    client.enableApiControl(True, name)     # 获取控制权
    client.armDisarm(True, name)            # 解锁（螺旋桨开始转动）
    if i != 8:                              # 起飞
        client.takeoffAsync(vehicle_name=name)
    else:
        client.takeoffAsync(vehicle_name=name).join()
        # 前8架无人机直接起飞即可,但要确保最后一架无人机完成起飞才能继续执行代码

for i in range(9):                          # 全部都飞到同一高度层
    name = "UAV" + str(i + 1)
    if i != 8:
        client.moveToZAsync(-3, 1, vehicle_name=name)
    else:
        client.moveToZAsync(-3, 1, vehicle_name=name).join()


# 参数设置
v_max = 2     # 无人机最大飞行速度
r_max = 20    # 邻居选择的半径
k_sep = 7     # 控制算法分离系数
k_coh = 1	  # 凝聚系数
k_mig = 1	  # 迁移系数
pos_mig = np.array([[25], [0]])   # 目标位置(25, 0)
v_cmd = np.zeros([2, 9])
# 两行九列的二维数组, 每列对应一架无人机的速度指令,两行分别代表x和y方向上的速度分量
# 在每轮循环中都要重新计算来控制每架无人机的飞行方向和速度


for t in range(500):
    for i in range(9):   # 计算每个无人机的速度指令
        # 计算每架无人机的速度指令,使它们朝向目标位置移动,同时保持适当的间距
        name_i = "UAV"+str(i+1)
        pos_i = get_UAV_pos(client, vehicle_name=name_i)    # 由实际位置得到相对位置
        r_mig = pos_mig - pos_i
        v_mig = k_mig * r_mig / np.linalg.norm(r_mig)		# 整体迁移速度
        v_sep = np.zeros([2, 1])
        v_coh = np.zeros([2, 1])
        N_i = 0
        for j in range(9):
            if j != i:
                N_i += 1
                name_j = "UAV"+str(j+1)
                pos_j = get_UAV_pos(client, vehicle_name=name_j)
                if np.linalg.norm(pos_j - pos_i) < r_max:
                    r_ij = pos_j - pos_i
                    v_sep += -k_sep * r_ij / np.linalg.norm(r_ij)
                    v_coh += k_coh * r_ij
        v_sep = v_sep / N_i								# 避碰速度
        v_coh = v_coh / N_i								# 中心聚集速度
        v_cmd[:, i:i+1] = v_sep + v_coh + v_mig         # 根据公式计算速度分量

    for i in range(9):   # 每个无人机的速度指令执行
        name_i = "UAV"+str(i+1)
        client.moveByVelocityZAsync(v_cmd[0, i], v_cmd[1, i], -3, 0.1, vehicle_name=name_i)

# 循环结束
client.simPause(False)
for i in range(9):
    name = "UAV"+str(i+1)
    if i != 8:                                              # 降落
        client.landAsync(vehicle_name=name)
    else:
        client.landAsync(vehicle_name=name).join()
for i in range(9):
    name = "UAV" + str(i + 1)
    client.armDisarm(False, vehicle_name=name)              # 上锁
    client.enableApiControl(False, vehicle_name=name)       # 释放控制权