AirSim航路点飞行

钰见梵星 / 2024-01-30 / 原文

航路点飞行API

AirSim 提供的轨迹跟踪 API 是基于位置控制的，所以严格意义上并不能算是轨迹跟踪，而应该称之为连续航路点飞行。无人机会依次飞过多个航路点，形成特定的飞行轨迹，其调用格式如下。

# 航路点轨迹飞行控制
client.moveOnPathAsync(
    path, 
    velocity, 
    drivetrain = DrivetrainType.MaxDegreeOfFreedom, 
    yaw_mode = YawMode(),
    lookahead = -1, 
    adaptive_lookahead = 1, 
    vehicle_name = ''
)

参数说明：

path (list[Vector3r])：多个航路点的3维坐标（全局 NED 坐标系，原点是无人机的初始位置）；
velocity (float)：无人机的飞行速度大小；
lookahead, adaptive_lookahead：设置路径跟踪算法的参数。

参数 path 包含了多个航路点的全局位置坐标，也就是说无人机要飞过 path 中包含的每一个坐标点（也就是航路点）。将 path 中的航路点依次连接起来，就会组成一条路径，无人机依次飞过航路点，就像是沿着特定路径飞行一样，形成路径跟踪的效果。

这里需要注意的是全局坐标系的原点是无人机的初始位置，所以当有多架无人机同时仿真的时候，每架无人机的原点是不同的，所以当使用统一的全局坐标系的时候，需要对不同的无人机添加位置补偿。

moveOnPathAsync 中使用的算法是 Carrot Chasing Algorithm，这是一个非常经典的路径跟踪算法，其中需要设置 lookahead 参数，这个参数的意义是设置在路径上向前看的距离。lookahead 设置的越大，在路径拐弯的时候，无人机就越提前转弯；如果 lookahead 参数设置的过小，则可能会出现超调，无人机会飞跃航路点，然后才拐弯。

下面用一组例子来展示 lookahead 参数大小对最终飞行轨迹的影响。设置3个航路点，控制无人机以1m/s 的速度以此飞过这3个航路点，分别设置不同的 lookahead 参数值，使用 moveOnPath 接口函数进行不同的仿真实验，无人机的飞行轨迹分别如下所示。

从最终的飞行轨迹上可以很明显可以看到，lookahead 参数设置的越大，无人机就越早转弯。所以要针对具体的场景设置合适的参数。

任何时候 lookahead 值的设置要大于位置控制允许的误差，并且小于整个路径的总长度。如果 lookahead 值小于位置控制允许的误差，则无人机会认为已经到达了第一个目标点，最终效果是无人机悬停在原地不会移动。如果 lookahead 值大于轨迹的总长度，则无人机会认为已经达到了最后一个航路点，最终效果还是无人机悬停在原地。

当 lookahead 参数小于0时，adaptive_lookahead 设置为大于 0 的数值，这样可以让算法根据当前的无人机飞行速度自动设置 lookahead 的值，通常按照默认设置（lookahead=1, adaptive_lookahead=-1）就可以达到比较好的效果。

如果航路点组成的轨迹是一条直线，不存在转弯的情况，那么 lookahead 值的大小对最终轨迹的影响将减小很多，此时 lookahead 的值只要小于轨迹的总长度即可。例如 moveToPositionAsync() 接口函数内容就是调用的 moveOnPathAsync 接口，也就是只有一个航路点的轨迹，此时轨迹就是一条直线，只要 lookahead 值小于当前到唯一航路点的距离，最终的飞行轨迹都是一样的。

当无人机飞正方形时，代码如下：

import airsim

# 初始化，建立连接
client = airsim.MultirotorClient()  # 创建连接
client.confirmConnection()          # 检查连接
client.reset()
client.enableApiControl(True)       # 获取控制权
client.armDisarm(True)              # 电机启转
client.takeoffAsync().join()        # 起飞
client.moveToZAsync(-3, 1).join()   # 上升到3米高度
client.simSetTraceLine([1, 0, 0, 1], thickness=5) 
# 设置了后续绘制的轨迹线的颜色和厚度, thickness=5 设置了轨迹线的厚度
# [1, 0, 0, 1]定义了轨迹线的颜色，这里是红色（RGBA 格式，红色值为 1，绿色和蓝色值为 0，透明度为 1，即不透明）
client.simFlushPersistentMarkers()  # 清空画图

# 初始化4个点的坐标，并在视口中标识出来
points = [airsim.Vector3r(5, 0, -3),
          airsim.Vector3r(5, 5, -3),
          airsim.Vector3r(0, 5, -3),
          airsim.Vector3r(0, 0, -3)]
client.simPlotPoints(points, color_rgba=[0, 1, 0, 1], size=30, is_persistent=True)
# is_persistent指定这些点是持久性的，即它们会在仿真环境中持续存在，直到被显式清除
# color_rgba设置点的颜色为绿色（RGBA 格式，红色值为 0，绿色值为 1，蓝色值为 0，透明度为 1）

# 方法1：按照逐个点飞，形成正方形
client.moveToPositionAsync(5, 0, -3, 1).join()
client.moveToPositionAsync(5, 5, -3, 1).join()
client.moveToPositionAsync(0, 5, -3, 1).join()
client.moveToPositionAsync(0, 0, -3, 1).join()

# 方法2：直接按照航路点飞正方形轨迹
client.moveOnPathAsync(points, 1).join()

# 仿真结束
client.goHomeAsync().join()         # 返航
client.landAsync().join()           # 降落
client.armDisarm(False)             # 电机上锁
client.enableApiControl(False)      # 释放控制权

当采用方法1的moveToPosition函数时，飞行轨迹如下：

当采用方法2的moveOnPath函数时，飞行轨迹如下：

方法1使用4次 moveToPositionAsync 接口函数，无人机到达一个航路点后才继续飞往下一个航路点，所以可以明显看到其飞行轨迹飞跃了正方形的每一个顶点。方法2 使用 moveOnPathAsync 接口函数，其中运行的是 carrot chasing 路径跟踪算法，由于 lookahead 参数的存在，无人机会在接近航路点时，就开始飞往下一个航路点了。所以最后的飞行轨迹中，在转弯的地方弧度比较大，而且轨迹只是接近但没有经过正方形的顶点。

多无人机位置控制

当使用多无人机同时仿真的时候，位置控制、航路点飞行等API 给出的坐标指令所使用的全局坐标系原点是不同的，所以在使用位置控制的时候，需要减去无人机的初始位置，从而实现原点的统一。

位置控制和航路点飞行的函数封装如下，这些函数的主要目的是确保在使用 AirSim 进行多无人机控制时，无人机的移动指令是相对于它们各自的初始位置而言的。

# 初始位置定义，第一架无人机位置为(0,0,0)，第二架无人机位置为(3,0,0)
orig_pos = np.array([[0., 0., 0.], [3., 0., 0.]])

# 无人机航路点飞行函数封装
# path航路点列表 waited是否等待无人机完成飞行
# 函数内部调整 path 中的每个点，使其相对于无人机的初始位置
def move_on_path(client, path, velocity, name='', waited=False, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(False, 0)):
    num = int(name[-1])		# 获取无人机编号
    # 根据编号从orig_pos数组中获取该无人机的初始位置，并创建一个airsim.Vector3r对象作为偏移量
    offset = airsim.Vector3r(orig_pos[num, 0], orig_pos[num, 1], orig_pos[num, 2])
    # 遍历path中的每个航路点，并从每个点中减去初始位置的偏移量。这样做是为了将航路点转换为相对于无人机初始位置的坐标
    for i in range(len(path)):
        path[i] -= offset
    # 实现飞行等待
    if waited:
        client.moveOnPathAsync(path, velocity, drivetrain=drivetrain, yaw_mode=yaw_mode, vehicle_name=name).join()
    else:
        client.moveOnPathAsync(path, velocity, drivetrain=drivetrain, yaw_mode=yaw_mode, vehicle_name=name)
    return


# 无人机位置控制函数封装
def move_to_position(client, x, y, z, velocity, name='', waited=False,
                     drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode()):
    num = int(name[-1])
    # 从orig_pos数组中获取该无人机的初始位置,并从传入的目标位置坐标中减去这个初始位置的偏移量
    # 这样做是为了将目标位置转换为相对于无人机初始位置的坐标
    x -= orig_pos[num, 0]
    y -= orig_pos[num, 1]
    z -= orig_pos[num, 2]
    if waited:
        client.moveToPositionAsync(x, y, z, velocity, drivetrain=drivetrain, yaw_mode=yaw_mode, vehicle_name=name).join()
    else:
        client.moveToPositionAsync(x, y, z, velocity, drivetrain=drivetrain, yaw_mode=yaw_mode, vehicle_name=name)
    return

在代码中可以看到，将给出的指令都减去了无人机的初始位置坐标，这样就能实现全局坐标系原点的统一。