为了方便进行无人机的编队演示，以及在各种场景下实现队形的灵活切换，开发了一套专门的上位机控制平台。本文将重点介绍应用于tello无人机的编队操作面板及其核心功能。
操作面板页面
下图展示了操作面板，其中包含5种编队动作和3个可选位置设定。用户可以根据实际需求选择动作，并对动作参数进行配置。该平台嵌入了两套通讯系统：仿真系统和物理系统。用户可以在仿真环境中验证动作的合理性和安全性，然后在物理系统中进行实验。接下来将在仿真环境中介绍各个动作。
仿真动作
启动仿真
点击启动仿真，可运行嵌入系统中的pybullet组件，并初始化无人机位置如下：(复位即此位置)
位置1，一字排开，为执行动作4，动作5做准备
动作1
三个无人机滚动交换位置，实现三个无人机换位置的效果
代码实现：
def shift_drone_positions(current_pos, target_position, task_stage_count):
if np.all(target_position == None):
target_position = current_pos
if np.linalg.norm(current_pos - target_position) < 0.05:
target_position = np.roll(target_position, 1, axis=0)
task_stage_count += 1
return target_position, task_stage_count
逻辑：
- 首先检查是否有指定的目标位置，如果没有，则将目标位置设定为当前位置。
- 计算当前位置与目标位置之间的欧几里得距离，如果小于 0.05，则执行下一步。
- 将目标位置在坐标轴上向前滚动一个单位，模拟位置的移动。
- 增加任务阶段计数。
- 返回更新后的目标位置和任务阶段计数。
动作2
无人机两两交换位置，同时采用CBF进行避障运动，保证运动的同时不会与其他无人机发生碰撞
代码实现：
def swap_drone_positions(current_pos, target_position, swap_count):
if np.all(target_position == None):
target_position = current_pos
new_drone_positions = np.copy(target_position)
if np.linalg.norm(current_pos - target_position) < 0.05:
if swap_count % 2 == 0:
01-10
temp = copy.deepcopy(new_drone_positions[0])
new_drone_positions[0] = new_drone_positions[1]
new_drone_positions[1] = temp
elif swap_count % 2 == 1:
12-21
temp = copy.deepcopy(new_drone_positions[1])
new_drone_positions[1] = new_drone_positions[2]
new_drone_positions[2] = temp
swap_count += 1
return new_drone_positions, swap_count
逻辑：
- 首先，函数检查是否有指定的目标位置，如果没有，则将目标位置设定为当前位置。
- 然后，函数创建了一个新的数组 new_drone_positions，用于存储目标位置的副本。
- 接着，函数计算当前位置与目标位置之间的欧几里得距离，如果小于 0.05，则执行下一步。
- 如果 swap_count 是偶数，则执行第一个交换操作，将第一个和第二个飞行器的位置交换。
- 如果 swap_count 是奇数，则执行第二个交换操作，将第二个和第三个飞行器的位置交换。
- 每次执行交换操作后，增加 swap_count。
- 最后，返回更新后的飞行器位置和增加后的交换次数 swap_count。
动作3
圆形运动，三个无人机以设定的半径及角速度进行圆形机动，实现画圆的效果
代码实现：
def rotational_motion(current_pos, radius=0.5, delta_theta_val=0.003,
last_angle=np.zeros(3),
first_call=True):
current_angle = np.arctan2(current_pos[:, 1], current_pos[:, 0])
current_angle = (current_angle + 2 * np.pi) % (2 * np.pi)
if first_call:
first_call = False
last_angle = copy.deepcopy(current_angle)
delta_theta = np.array([delta_theta_val, delta_theta_val, delta_theta_val])
R = radius
new_angle = last_angle + delta_theta
target_x = R * np.cos(new_angle)
target_y = R * np.sin(new_angle)
target_position = np.array([target_x, target_y, [current_pos[0][2]] * 3]).T
return target_position, np.array(new_angle), first_call
点击【Tello无人机】无人机编队操作面板实现 - 古月居可查看全文