无人机集群仿真

首先要修改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)       # 释放控制权

状态读取

​ 无人机的真值状态读取接口可以得到无人机的真值状态（无误差），此接口同样适用于无人车，属于 vehicle 类的通用APIs。接口的调用格式如下：

kinematic_state_groundtruth = client.simGetGroundTruthKinematics(vehicle_name='')

其中返回值 kinematic_state_groundtruth包含6个属性：

# state_groundtruth 的6个属性
kinematic_state_groundtruth.position               # 位置信息
kinematic_state_groundtruth.linear_velocity	   	   # 速度信息
kinematic_state_groundtruth.linear_acceleration    # 加速度信息
kinematic_state_groundtruth.orientation	           # 姿态信息
kinematic_state_groundtruth.angular_velocity       # 姿态角速率信息
kinematic_state_groundtruth.angular_acceleration   # 姿态角加速度信息

​ 以上6个属性中，除了 orientation 其他每个都包含了 x_val、y_val和z_val三个属性，分别代表x,y,z3个方向的值。例如位置真值的读取如下：

# 无人机全局位置坐标真值
x = kinematic_state_groundtruth.position.x_val            # 全局坐标系下，x轴方向的坐标
y = kinematic_state_groundtruth.position.y_val            # 全局坐标系下，y轴方向的坐标
z = kinematic_state_groundtruth.position.z_val            # 全局坐标系下，z轴方向的坐标

​ 同理，速度、加速度、姿态角速度、姿态角加速度真值的读取如下：

# 无人机全局速度真值
vx = kinematic_state_groundtruth.linear_velocity.x_val      # 无人机 x 轴方向 (正北) 的速度大小真值
vy = kinematic_state_groundtruth.linear_velocity.y_val      # 无人机 y 轴方向 (正东) 的速度大小真值
vz = kinematic_state_groundtruth.linear_velocity.z_val      # 无人机 z 轴方向 (垂直向下) 的速度大小真值
# 无人机全局加速度真值
ax = kinematic_state_groundtruth.linear_acceleration.x_val  # 无人机 x 轴方向 (正北) 的加速度大小真值
ay = kinematic_state_groundtruth.linear_acceleration.y_val  # 无人机 y 轴方向 (正东) 的加速度大小真值
az = kinematic_state_groundtruth.linear_acceleration.z_val  # 无人机 z 轴方向 (垂直向下) 的加速度大小真值
# 机体角速率
kinematic_state_groundtruth.angular_velocity.x_val        # 机体俯仰角速率
kinematic_state_groundtruth.angular_velocity.y_val        # 机体滚转角速率
kinematic_state_groundtruth.angular_velocity.z_val        # 机体偏航角速率
# 机体角加速度
kinematic_state_groundtruth.angular_acceleration.x_val    # 机体俯仰角加速度
kinematic_state_groundtruth.angular_acceleration.y_val    # 机体滚转角加速度
kinematic_state_groundtruth.angular_acceleration.z_val    # 机体偏航角加速度

​ 而对于姿态的读取，姿态信息是用四元数表示的，而AirSim同时也提供了四元数转换为欧拉角的接口：

# 无人机姿态真值的四元数表示
kinematic_state_groundtruth.orientation.x_val
kinematic_state_groundtruth.orientation.y_val
kinematic_state_groundtruth.orientation.z_val
kinematic_state_groundtruth.orientation.w_val
# 四元数转换为欧拉角，单位rad
(pitch, roll, yaw) = airsim.to_eularian_angles(kinematic_state_groundtruth.orientation)

​ 在实际的无人机中，无法获得无人机的状态真值，状态的读取是无人机上传感器融合得到的估计值。AirSim API 还提供了获取无人机状态（估计值）的功能接口，其调用格式如下：

state_multirotor = client.getMultirotorState(vehicle_name='')

​ 这里的无人机状态 state_multirotor 包含：时间戳、运动状态信息 (估计值)、碰撞信息、GPS经纬度信息、遥控器信息、降落信息等。读取方式如下：

state_multirotor.collision              # 碰撞信息
state_multirotor.kinematics_estimated   # 运动信息
state_multirotor.gps_location           # GPS经纬度信息
state_multirotor.timestamp              # 时间戳
state_multirotor.landed_state           # 降落信息
state_multirotor.rc_data                # 遥控器信息

​ 其中的降落信息 state_multirotor.landed_state 是整数类型，其值为 0 表示在地上，值为 1 表示在空中。其中的时间戳 state_multirotor.timestamp 是仿真开始后的时间，单位是纳秒。

​ 无人机状态中的运动信息 state_multirotor.kinematics_estimated 是由多个传感器测量值融合得到的，与上面的 kinematic_state_groundtruth 是同样的变量类型，所以 state_multirotor.kinematics_estimated 同样也包含6种运动信息，读取方式相同：

# 无人机运动信息的估计值的6个属性
state_multirotor.kinematics_estimated.position               # 位置信息估计值
state_multirotor.kinematics_estimated.linear_velocity        # 速度信息估计值
state_multirotor.kinematics_estimated.linear_acceleration    # 加速度信息估计值
state_multirotor.kinematics_estimated.orientation            # 姿态信息估计值
state_multirotor.kinematics_estimated.angular_velocity       # 姿态角速率信息估计值
state_multirotor.kinematics_estimated.angular_acceleration   # 姿态角加速度信息估计值

​ 但是很遗憾的是，Simple Flight 不支持模拟传感器，所以如果使用的是 Simple Flight，则使用 getMultirotorState() 得到的运动状态与使用 simGetGroundTruthKinematics() 得到的运动状态真值是一模一样的。如果使用硬件在环仿真 (如PX4等)，则可以使用 getMultirotorState() 获得无人机的估计状态信息。

多无人机位置读取

​ 无人机位置读取时使用的是全局坐标系，这里的全局坐标系定义是北东地（NED），全局坐标系的原点是无人机的初始位置。这意味着无人机初始时的位置被视为坐标系的 (0, 0, 0) 点。

​ 也就是说当有多个无人机时，而且每个无人机的初始位置不同时，每个无人机的全局坐标系是以其自己的初始位置为原点的。这就导致了一个问题：当尝试比较或处理多个无人机的位置数据时，它们各自的坐标系可能不一致。此接口读取到的不同的无人机的状态是以其读的无人机的初始位置为原点的，所以在读取的时候需要加上其初始位置的补偿。通过这种方式，可以把所有无人机的位置数据转换到一个共同的参考框架下。

​ 基于此做如下位置读取的函数封装：

# 无人机位置读取函数封装
orig_pos = np.array([[0., 0., 0.],[3., 0., 0.]])	# 定义了两个无人机的初始位置(0,0,0)和(3,0,0)
def get_uav_pos(client, name):						# 获取特定无人机的调整后的全局位置
    uav_state = client.getMultirotorState(vehicle_name=name).kinematics_estimated	# 获取当前无人机位置
    num = int(name[-1])
    pos = np.array([[uav_state.position.x_val + orig_pos[num, 0]],
                    [uav_state.position.y_val + orig_pos[num, 1]],
                    [uav_state.position.z_val + orig_pos[num, 2]]])		# 计算调整后的全局位置
    return pos

​ 其中 orig_pos 是每个无人机的初始位置，上面的代码中，有两个无人机，无人机0的初始位置是[0, 0, 0]，无人机1的初始位置是 [3, 0, 0]。使用上面的方法添加补偿后，就可以将每个无人机的全局坐标系统一了。

一开始两架无人机的位置坐标为(0,0,0)和(3,0,0)，起飞到3米高度后，两架无人机均沿x方向飞行5米，这个时候按照上述封装的无人机位置读取函数，两架无人机的位置坐标是多少？

​ 在这里给出的无人机初始位置是指相对于全局坐标系而言的，因此给出的坐标是(0, 0, 0)和(3, 0, 0)。在每架无人机自己所采用的机体坐标系中，沿x方向飞行5米，那么此时两架无人机的坐标均为(5, 0, 0)。这时候就要利用全局坐标系进行初始位置补偿，即第一架无人机在全局坐标系下的位置为(5, 0, 0)，第二架无人机在全局坐标系下的位置为(8, 0, 0)。