操作面板页面
下图展示了操作面板，其中包含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
逻辑：
-
首先，函数接受一系列参数：
current_pos（当前位置）、radius（旋转半径，默认为0.5）、delta_theta_val（每次旋转的角度增量，默认为0.003）、last_angle（上一次旋转后的角度，默认为全零数组）、first_call（是否是第一次调用函数，默认为True）。 -
函数计算当前位置相对于原点的极坐标角度（通过
np.arctan2函数），并将其转换为区间[0, 2π)内的角度值。 -
如果是第一次调用函数，则将
first_call设置为 False，并将当前角度值复制给last_angle。 -
计算每个维度的角度增量
delta_theta。 -
计算飞行器旋转后的新角度，即上一次角度加上角度增量。
-
根据新的角度值，计算飞行器旋转后的目标位置，即绕着原点旋转半径
radius所得的新位置。 -
返回旋转后的目标位置、新的角度值以及更新后的
first_call参数。
动作4
以不用的半径进行圆形机动，实现圆环的效果
代码实现：
def rotational_motion_in_row(current_pos, radius=[0.3, 0.6, 0.9], 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
逻辑：
-
函数接受一系列参数：
current_pos（当前位置）、radius（旋转半径数组，默认为[0.3, 0.6, 0.9]）、delta_theta_val（每次旋转的角度增量，默认为0.003）、last_angle（上一次旋转后的角度，默认为全零数组）、first_call（是否是第一次调用函数，默认为True）。 -
函数计算当前位置相对于原点的极坐标角度（通过
np.arctan2函数），并将其转换为区间[0, 2π)内的角度值。 -
如果是第一次调用函数，则将
first_call设置为 False，并将当前角度值复制给last_angle。 -
计算每个维度的角度增量
delta_theta。 -
计算飞行器旋转后的新角度，即上一次角度加上角度增量。
-
根据新的角度值和给定的半径数组，计算飞行器旋转后的目标位置。
-
返回旋转后的目标位置、新的角度值以及更新后的
first_call参数。
动作5
螺旋运动，高度也进行变化，三个无人机依此进行螺旋上升与下降运行
代码实现:
def spiral_motion(current_pos, radius=[0.5, 0.5, 0.5],
drone_delta_theta=np.pi / 3,
delta_theta_val=0.003,
delta_up_val=0.003,
last_angle=np.zeros(3),
start_angle_flag=0,
first_call=True):
current_angle = np.arctan2(current_pos[:, 1], current_pos[:, 0])
current_angle = (current_angle + 2 * np.pi) % (2 * np.pi)
delta_theta = np.array([delta_theta_val, delta_theta_val, delta_theta_val])
R = radius
if start_angle_flag != 2:
if start_angle_flag == 0:
delta_theta = np.array([delta_theta_val, 0, 0])
dist_r = np.sqrt(current_pos[:, 1] ** 2 + current_pos[:, 0] ** 2)
R = np.array([radius[0], dist_r[1], dist_r[2]])
if current_angle[0] >= drone_delta_theta > current_angle[0] - 0.2:
start_angle_flag = 1
elif start_angle_flag == 1:
delta_theta = np.array([delta_theta_val, delta_theta_val, 0])
dist_r = np.sqrt(current_pos[:, 1] ** 2 + current_pos[:, 0] ** 2)
R = np.array([radius[0], radius[1], dist_r[2]])
if current_angle[1] >= drone_delta_theta > current_angle[1] - 0.2:
start_angle_flag = 2
new_angle = last_angle + delta_theta
target_x = R * np.cos(new_angle)
target_y = R * np.sin(new_angle)
target_z = 1 - 0.5 * np.cos(new_angle * delta_up_val)
target_position = np.array([target_x, target_y, target_z]).T
return target_position, np.array(new_angle), start_angle_flag, first_call
逻辑：
-
函数接受一系列参数：
current_pos（当前位置）、radius（螺旋运动的半径，默认为[0.5, 0.5, 0.5]）、drone_delta_theta（每个飞行器之间的角度差，默认为 π/3）、delta_theta_val（每次旋转的角度增量，默认为0.003）、delta_up_val（高度方向的增量，默认为0.003）、last_angle（上一次旋转后的角度，默认为全零数组）、start_angle_flag（起始角度标志，默认为0）、first_call（是否是第一次调用函数，默认为True）。 -
函数计算当前位置相对于原点的极坐标角度（通过
np.arctan2函数），并将其转换为区间[0, 2π)内的角度值。 -
根据不同的起始角度标志
start_angle_flag，调整每个飞行器的旋转方式。起始角度标志用于控制螺旋运动的启动方式，分为三个阶段：0 表示第一个阶段，1 表示第二个阶段，2 表示第三个阶段。在第一个阶段，飞行器按照设置的角度增量进行旋转；在第二个阶段，飞行器继续旋转并逐渐增大半径；在第三个阶段，飞行器继续旋转并逐渐增加高度。 -
计算每个维度的角度增量
delta_theta。 -
计算飞行器旋转后的新角度，即上一次角度加上角度增量。
-
根据新的角度值和给定的半径数组，计算飞行器旋转后的目标位置，并根据螺旋运动的特性计算高度。
-
返回旋转后的目标位置、新的角度值、更新后的起始角度标志和
first_call参数。
序列动作
为了方便将所有的动作都进行演示，面板右侧可进行动作的序列组合，以下为：1-2-3-4-5组合的效果
数据的读取与保存
保存面板信息：
将用户界面（UI）中的各项数据保存到名为 config.txt 的文本文件中。获取UI中的IP地址、初始位置、位置1、位置2、动作3、动作4、动作5以及任务序列等数据。
def save_ui_data(self):
# Create a confirmation dialog
print(type(self.ui))
confirmation = QMessageBox.question(
self, '确认', '覆盖配置文件?',
QMessageBox.Yes | QMessageBox.No
)
if confirmation == QMessageBox.Yes:
ips = [self.ui.ip1.text(), self.ui.ip2.text(), self.ui.ip3.text(), self.ui.local_ip.text(),
self.ui.opti_ip.text()]
init_pos = [
[self.ui.init_pos_u1x.text(), self.ui.init_pos_u1y.text(), self.ui.init_pos_u1z.text()],
[self.ui.init_pos_u2x.text(), self.ui.init_pos_u2y.text(), self.ui.init_pos_u2z.text()],
[self.ui.init_pos_u3x.text(), self.ui.init_pos_u3y.text(), self.ui.init_pos_u3z.text()]
]
pos1 = [
[self.ui.pos1_1_x.text(), self.ui.pos1_1_y.text(), self.ui.pos1_1_z.text()],
[self.ui.pos1_2_x.text(), self.ui.pos1_2_y.text(), self.ui.pos1_2_z.text()],
[self.ui.pos1_3_x.text(), self.ui.pos1_3_y.text(), self.ui.pos1_3_z.text()]
]
pos2 = [
[self.ui.pos2_1_x.text(), self.ui.pos2_1_y.text(), self.ui.pos2_1_z.text()],
[self.ui.pos2_2_x.text(), self.ui.pos2_2_y.text(), self.ui.pos2_2_z.text()],
[self.ui.pos2_3_x.text(), self.ui.pos2_3_y.text(), self.ui.pos2_3_z.text()]
]
action3_data = [self.ui.action3_R.text(), self.ui.action3_w.text()]
action4_data = [self.ui.action4_R1.text(), self.ui.action4_R2.text(), self.ui.action4_R3.text(),
self.ui.action4_w.text()]
action5_data = [self.ui.action5_R.text(), self.ui.action5_w.text(), self.ui.action5_delta.text(),
self.ui.action5_v.text()]
sequence_task = [self.ui.task_action1.currentText(), self.ui.task_action2.currentText(),
self.ui.task_action3.currentText(), self.ui.task_action4.currentText(),
self.ui.task_action5.currentText(), self.ui.task_action6.currentText(),
self.ui.task_action7.currentText(), self.ui.task_action8.currentText(),
self.ui.task_action9.currentText()]
with open('config.txt', 'w', encoding='utf-8') as file:
file.write('\n'.join(ips) + '\n')
for init_pos_values in init_pos:
file.write(','.join(init_pos_values) + '\n')
for pos1_values in pos1:
file.write(','.join(pos1_values) + '\n')
for pos2_values in pos2:
file.write(','.join(pos2_values) + '\n')
file.write(','.join(action3_data) + '\n')
file.write(','.join(action4_data) + '\n')
file.write(','.join(action5_data) + '\n')
file.write(','.join(sequence_task) + '\n')
QMessageBox.information(self, '成功', '成功保存配置文件！')
else:
QMessageBox.information(self, '取消', '取消保存配置文件！')
加载文件信息：
从名为 config.txt 的文本文件中读取数据，并将这些数据加载到用户界面（UI）的相应部件中。
def load_ui_data(self):
try:
with open('config.txt', 'r', encoding='utf-8') as file:
lines = file.readlines()
ips = [line.strip() for line in lines[:5]]
self.ui.ip1.setText(ips[0])
self.ui.ip2.setText(ips[1])
self.ui.ip3.setText(ips[2])
self.ui.local_ip.setText(ips[3])
self.ui.opti_ip.setText(ips[4])
init_pos_values = [line.strip().split(',') for line in lines[5:8]]
self.ui.init_pos_u1x.setText(init_pos_values[0][0])
self.ui.init_pos_u1y.setText(init_pos_values[0][1])
self.ui.init_pos_u1z.setText(init_pos_values[0][2])
self.ui.init_pos_u2x.setText(init_pos_values[1][0])
self.ui.init_pos_u2y.setText(init_pos_values[1][1])
self.ui.init_pos_u2z.setText(init_pos_values[1][2])
self.ui.init_pos_u3x.setText(init_pos_values[2][0])
self.ui.init_pos_u3y.setText(init_pos_values[2][1])
self.ui.init_pos_u3z.setText(init_pos_values[2][2])
# Read pos1
pos1_values = [line.strip().split(',') for line in lines[8:11]]
# Set the text for pos1 widgets
self.ui.pos1_1_x.setText(pos1_values[0][0])
self.ui.pos1_1_y.setText(pos1_values[0][1])
self.ui.pos1_1_z.setText(pos1_values[0][2])
self.ui.pos1_2_x.setText(pos1_values[1][0])
self.ui.pos1_2_y.setText(pos1_values[1][1])
self.ui.pos1_2_z.setText(pos1_values[1][2])
self.ui.pos1_3_x.setText(pos1_values[2][0])
self.ui.pos1_3_y.setText(pos1_values[2][1])
self.ui.pos1_3_z.setText(pos1_values[2][2])
# Read pos2
pos2_values = [line.strip().split(',') for line in lines[11:14]]
# Set the text for pos2 widgets
self.ui.pos2_1_x.setText(pos2_values[0][0])
self.ui.pos2_1_y.setText(pos2_values[0][1])
self.ui.pos2_1_z.setText(pos2_values[0][2])
self.ui.pos2_2_x.setText(pos2_values[1][0])
self.ui.pos2_2_y.setText(pos2_values[1][1])
self.ui.pos2_2_z.setText(pos2_values[1][2])
self.ui.pos2_3_x.setText(pos2_values[2][0])
self.ui.pos2_3_y.setText(pos2_values[2][1])
self.ui.pos2_3_z.setText(pos2_values[2][2])
# Read action3_data
action3_data = lines[14].strip().split(',')
self.ui.action3_R.setText(action3_data[0])
self.ui.action3_w.setText(action3_data[1])
# Read action4_data
action4_data = lines[15].strip().split(',')
self.ui.action4_R1.setText(action4_data[0])
self.ui.action4_R2.setText(action4_data[1])
self.ui.action4_R3.setText(action4_data[2])
self.ui.action4_w.setText(action4_data[3])
# Read action5_data
action5_data = lines[16].strip().split(',')
self.ui.action5_R.setText(action5_data[0])
self.ui.action5_w.setText(action5_data[1])
self.ui.action5_delta.setText(action5_data[2])
self.ui.action5_v.setText(action5_data[3])
# Read sequence_task
sequence_task = lines[17].strip().split(',')
# Set the text for task_action QComboBox widgets
self.ui.task_action1.setCurrentText(sequence_task[0])
self.ui.task_action2.setCurrentText(sequence_task[1])
self.ui.task_action3.setCurrentText(sequence_task[2])
self.ui.task_action4.setCurrentText(sequence_task[3])
self.ui.task_action5.setCurrentText(sequence_task[4])
self.ui.task_action6.setCurrentText(sequence_task[5])
self.ui.task_action7.setCurrentText(sequence_task[6])
self.ui.task_action8.setCurrentText(sequence_task[7])
self.ui.task_action9.setCurrentText(sequence_task[8])
except FileNotFoundError:
print("Config file not found.")
except Exception as e:
print(f"Error reading config file: {e}")
评论（0）
您还未登录，请登录后发表或查看评论