2023 华为软件精英挑战赛复盘

1
# 方向调整控制
2
expected_angle = calculate_angle(bot_id, table_id)  # 计算目标方向角
3
# 目标方向和当前方向之差大于pi时需要转换
4
if numpy.abs(expected_angle - bot['direction']) > numpy.pi:
5
    # 当前朝向先做转换变到与目标方向同号
6
    # 当前朝向方向角为0不需要转换
7
    # 转换结果只在临时变量中用于计算偏差角，并不修改数据字典中的值
8
    if bot['direction'] < 0:
9
        bot['direction'] += 2 * numpy.pi
10
    elif bot['direction'] > 0:
11
        bot['direction'] -= 2 * numpy.pi
12
# 控制旋转速度，偏差角度越大角速度越大
13
if expected_angle - bot['direction'] < 0:
14
    command['rotate'][bot_id] = -min_rotate_velocity + angular_velocity_kp * (expected_angle - bot['direction'])
15
elif expected_angle - bot['direction'] > 0:
16
    command['rotate'][bot_id] = min_rotate_velocity + angular_velocity_kp * (expected_angle - bot['direction'])
17
# 到达偏差角阈值以最大速度前进
18
if numpy.abs(expected_angle - bot['direction']) < start_move_angle_threshold:
19
    command['forward'][bot_id] = max_forward_velocity
20
# 否则以最小速度前进
21
else:
22
    command['forward'][bot_id] = min_forward_velocity

1
# 计算碰撞区
2
collision_area_half_angle = numpy.arcsin(
3
    (bot_1_radius + bot_2_radius) / distance_list[bot_id_2])  # 碰撞区半角
4
# 适当增加碰撞区半角，但不超出最大值
5
if collision_area_half_angle * collision_area_half_angle_kp > numpy.pi / 2:
6
    collision_area_half_angle = numpy.pi / 2
7
else:
8
    collision_area_half_angle *= collision_area_half_angle_kp
9
collision_area_center_angle = calculate_angle_between_bot(bot_id_1, bot_id_2)  # 碰撞区中心方向角
10
collision_area_left_boundary_angle = collision_area_center_angle + collision_area_half_angle  # 碰撞区左边界方向角
11
collision_area_right_boundary_angle = collision_area_center_angle - collision_area_half_angle  # 碰撞区右边界角
12
# 计算机器人1相对于机器人2的速度
13
bot_1_relative_velocity_x = bot_1['linear_velocity'][0] - bot_2['linear_velocity'][0]  # x方向相对速度
14
bot_1_relative_velocity_y = bot_1['linear_velocity'][1] - bot_2['linear_velocity'][1]  # y方向相对速度
15
bot_1_relative_velocity_angle = numpy.arctan2(bot_1_relative_velocity_y,
16
                                                bot_1_relative_velocity_x)  # 相对速度方向
17
# 如果相对速度方向和碰撞区中心方向角之差大于pi（例如碰撞区是负角度，但相对速度方向在其+2pi后的正角度区间内）
18
if numpy.abs(bot_1_relative_velocity_angle - collision_area_center_angle) > numpy.pi:
19
    # 变换到同号区间
20
    if bot_1_relative_velocity_angle < 0:
21
        bot_1_relative_velocity_angle += 2 * numpy.pi
22
    elif bot_1_relative_velocity_angle > 0:
23
        bot_1_relative_velocity_angle -= 2 * numpy.pi
24
# 如果相对速度方向在碰撞区内则会发生碰撞（恰巧在边界会相切，所以也避免），需要避让
25
if collision_area_right_boundary_angle < bot_1_relative_velocity_angle < collision_area_left_boundary_angle:
26
    # 检测离哪边边界近就往哪边转，直接和中心方向角比较即可
27
    # 离右边界近
28
    if bot_1_relative_velocity_angle < collision_area_center_angle:
29
        command['rotate'][bot_id_1] = min_rotate_velocity + angular_velocity_kp * (
30
                collision_area_right_boundary_angle - bot_1_relative_velocity_angle)
31
    # 离左边界近
32
    elif bot_1_relative_velocity_angle > collision_area_center_angle:
33
        command['rotate'][bot_id_1] = min_rotate_velocity + angular_velocity_kp * (
34
                collision_area_left_boundary_angle - bot_1_relative_velocity_angle)
35
    # 恰好位于中间
36
    else:
37
        # 查看目前的角速度方向，往当前已有的角速度方向转，角速度为0则默认顺时针旋转
38
        # 角速度大于0，即逆时针旋转中
39
        if bot_1['angular_velocity'] > 0:
40
            command['rotate'][bot_id_1] = min_rotate_velocity + angular_velocity_kp * (
41
                    collision_area_left_boundary_angle - bot_1_relative_velocity_angle)
42
        # 角速度小于0，即顺时针旋转中
43
        elif bot_1['angular_velocity'] < 0:
44
            command['rotate'][bot_id_1] = min_rotate_velocity + angular_velocity_kp * (
45
                    collision_area_right_boundary_angle - bot_1_relative_velocity_angle)
46
        # 角速度为0，默认顺时针旋转
47
        else:
48
            command['rotate'][bot_id_1] = min_rotate_velocity + angular_velocity_kp * (
49
                    collision_area_right_boundary_angle - bot_1_relative_velocity_angle)