前言
比赛介绍
参考初赛博客: 华为软挑赛2023-初赛笔记_没有梦想的大白兔的博客-CSDN博客
赛题变动
官网赛题: 华为云论坛_云计算论坛_开发者论坛_技术论坛-华为云 (huaweicloud.com)
在真实的业务场景中,机器人往往需要在不同的房间之间穿行,并且需要躲避各种障碍物,故在复赛中引入这一业务难题,新增障碍物来模拟真实业务场景。
-
复赛新增障碍物,在地图元素中通过字符’#'来表示
-
运行时间由3分钟提升至5分钟
-
线上运行随机种子不再固定,改为每次随机
代码介绍
整体思路设计
相较于初赛,复赛引入障碍带来了三个核心问题:
- 机器人到工作台的路径不再是简单的直线,需要使用相关算法规划路径。
- 运动控制方面如何追踪路径。
- 在狭窄的区域机器人不再能通过简单的停车或者转向完成避让,需要通过寻路机制找到可以避让的点。
数据结构设计
复赛中,我们对初步代码进行了重构,相关变量均作为类的成员变量,并新增了地图类用于计算路径。
机器人类
机器人类主要增加了用于路径规划和避让的变量,由于添加了障碍,一次机器人可能不能到达所有工作台,因此每个机器人会记录其可达的工作台。主要参数如下。
class Robot:
# 状态常量 0 空闲, 1 购买途中, 2 等待购买, 3 出售途中, 4 等待出售, 5 避撞
FREE_STATUS = 0
MOVE_TO_BUY_STATUS = 1
WAIT_TO_BUY_STATUS = 2
MOVE_TO_SELL_STATUS = 3
WAIT_TO_SELL_STATUS = 4
AVOID_CLASH = 5
def __init__(self, ID: int, loc: Tuple[int]):
self.status: int = 0 # 0 空闲, 1 购买途中, 2 等待购买, 3 出售途中, 4 等待出售
self.move_status: int = 0 # 移动时的状态
self.target = -1 # 当前机器人的目标控制台 -1代表无目标
self.__plan = (-1, -1) # 设定买和卖的目标工作台
self.target_workbench_list = [] # 可到达的工作台列表
self.path = [] # 机器人当前路径
self.deadlock_with = -1
# 避让等待
self.frame_wait = 0
self.backword_speed = 0 # 避让速度
self.frame_backword = 0 # 后退帧数
# 预估剩余时间
self.frame_reman_buy = 0 # 预计多久能买任务
self.frame_reman_sell = 0 # 预计多久能卖任务
# 路径追踪的临时点
self.temp_target = None
self.anoter_robot = -1 # 记录和它冲突的机器人
def get_frame_reman(self):
'''
预计还需要多久可以完成当前任务, 与状态有关
'''
def update_frame_reman(self):
'''
更新预估值, 与状态有关, 等待时不计时
'''
def update_frame_pisition(self, frame):
"""
更新pre_frame,pre_position,pre_toward
"""
def set_plan(self, buy_ID: int, sell_ID: int):
'''
设置机器人计划, 传入购买和出售工作台的ID
'''
def set_path(self, path: List[Tuple[float, float]]):
'''
设置机器人移动路径
:param path: 路径, float型的坐标列表
:return:
'''
工作台类
工作台类主要将预售预购等常用的功能集中了进来。此外由于障碍的引入,从当前工作台购买的商品不一定可以到达所有工作台。因此我们添加了成员变量,用于记录在当前工作台买完商品之后,可达的能够收购本工作台产出商品的工作台。
class Workbench:
ITEMS_BUY = [0, 3000, 4400, 5800, 15400, 17200, 19200, 76000] # 每个物品的购买价
ITEMS_SELL = [0, 6000, 7600, 9200, 22500, 25000, 27500, 105000]
ITEMS_NEED = [[] for _ in range(8)] # 记录收购每个商品的工作台编号
WORKSTAND_IN = {1: [], 2: [], 3: [], 4: [1, 2], 5: [1, 3],
6: [2, 3], 7: [4, 5, 6], 8: [7], 9: list(range(1, 8))}
WORKSTAND_OUT = {i: i for i in range(1, 8)}
WORKSTAND_OUT[8] = None
WORKSTAND_OUT[9] = None
WORKSTAND_FULL = { # 记录每种工作台材料格满的情况
4: sum([1 << i for i in (1, 2)]),
5: sum([1 << i for i in (1, 3)]),
6: sum([1 << i for i in (2, 3)]),
7: sum([1 << i for i in (4, 5, 6)])
}
# 7号工作台急需原料的情况
WORKSTAND_STARVE = {
sum([1 << i for i in (4, 5)]):6,
sum([1 << i for i in (4, 6)]):5,
sum([1 << i for i in (5, 6)]):4,
}
def __init__(self, ID: int, typeID: int, loc: Tuple[float]):
self.material_pro = 0 # 原料格预定状态, 防止有多个机器人将其作为出售目标
self.product_pro = 0 # 防止有多个机器人将其作为购买目标
self.target_workbench_list = [] # 收购此工作台的产品且可到达的工作台列表
self.buy_price = self.ITEMS_BUY[self.typeID] if self.typeID < len(self.ITEMS_BUY) else 0 # 进价
self.sell_price = self.ITEMS_SELL[self.typeID] if self.typeID < len(self.ITEMS_SELL) else 0 # 售价
def get_materials_num(self): # 返回已被占用的格子数目
def check_materials_full(self) -> bool:
'''
检测原料格是否已满
'''
def check_material(self, mateial_ID: int) -> bool:
'''
检测原材料是否已放置
True表示已有材料
'''
def check_material_pro(self, mateial_ID: int) -> bool:
'''
检测原材料格子是否已被预订
True表示已有材料
'''
def pro_sell(self, mateial_ID: int, sell=True) -> bool:
'''
预售接口 如果是89特殊处理
mateial_ID: 传入要出售商品的类型ID
sell: True表示预售, False表示取消预售
设置成功返回True
'''
def pro_buy(self, buy=True):
'''
预购接口, 如果是123特殊处理
sell: True表示预购, False表示取消预购
设置成功返回True
'''
def update(self, s: str):
# 根据判题器传来的状态修订本机状态
地图类
针对复赛引入的新类,负责读入地图并处理地图数据,导航和部分避让工作,将在下一句详细阐述。
'''
地图类,保存整张地图数据
概念解释:
窄路: 只用未手持物品的机器人才能通过的路, 判定条件,右上三个点都不是障碍(可以是边界)
宽路: 手持物品的机器人也能通过的路, 判定条件, 周围一圈都必须是空地(不是障碍或边界)
广路: 可以冲刺
'''
class Workmap:
BLOCK = 0 # 障碍
GROUND = 1 # 空地
ROAD = 2 # 窄路, 临近的四个空地的左下角,因此如果经过这类点请从右上角走
BROAD_ROAD = 3 # 宽路 瞄着中间走就行
SUPER_BROAD_ROAD = 4 # 广路 可以冲刺
PATH = 5 # 算法规划的路径,绘图用
# TURNS = [(-1, 0), (1, 0), (0, 1), (0, -1)]
TURNS = list(itertools.product([-1, 0, 1], repeat=2)) # 找路方向,原则: 尽量减少拐弯
TURNS.remove((0, 0))
def __init__(self, debug=False) -> None:
self.map_data: List[str] = [] # 原始地图信息
self.robots_loc: dict = {} # k 机器人坐标点 v 机器人ID
self.workbenchs_loc: dict = {} # k 工作台坐标点 v 工作台ID
# 灰度地图,对不同路进行了区分
self.map_gray = [[self.GROUND] * 100 for _ in range(100)]
if debug:
import matplotlib.pyplot as plt
self.plt = plt
self.draw_map = self.__draw_map
self.draw_path = self.__draw_path
self.buy_map = {} # 空手时到每个工作台的路径
self.sell_map = {} # 手持物品时到某个工作台的路径
self.unreanchble_warkbench = set() # 记录不能正常使用的工作台
self.broad_shifting = {} # 有些特殊宽路的偏移量
def loc_int2float_normal(self, i: int, j: int):
'''
地图离散坐标转实际连续坐标, 不作额外操作
'''
@lru_cache(None)
def loc_int2float(self, i: int, j: int, rode=False):
'''
地图离散坐标转实际连续坐标, 导航时使用,某些路会附加偏移
rode: 标识是否是窄路,窄路按原先右上角
'''
def loc_float2int(self, x, y):
'''
地图实际连续坐标转离散坐标
'''
def dis_loc2path(self, loc, path):
'''
获取某个点到某路径的最短距离
'''
def read_map(self):
'''
从标准输入读取地图
'''
def init_roads(self):
'''
识别出窄路和宽路
'''
def robot2workbench(self):
'''
获取每个机器人可以访问的工作台列表, 买的过程由此构建
'''
def workbench2workbench(self):
'''
获取每个工作台可以访问的收购对应商品的工作台列表, 卖的过程由此构建
'''
def gen_a_path(self, workbench_ID, workbench_loc, broad_road=False):
'''
生成一个工作台到其他节点的路径,基于迪杰斯特拉优化
workbench_ID: 工作台ID
workbench_loc: 当前节点坐标
broad_road: 是否只能走宽路
'''
def gen_paths(self):
'''
基于查并集思想,优先选择与自己方向一致的节点
以工作台为中心拓展
'''
def get_avoid_path(self, wait_flaot_loc, work_path, robots_loc, broad_road=False, safe_dis: float = None):
'''
为机器人规划一条避让路径, 注意此函数会临时修改map_gray如果后续有多线程优化, 请修改此函数
wait_flaot_loc: 要避让的机器人坐标
work_path: 正常行驶的机器人路径
robots_loc: 其他机器人坐标
broad_road: 是否只能走宽路, 根据机器人手中是否 持有物品确定
return: 返回路径, 为空说明无法避让
'''
def get_float_path(self, float_loc, workbench_ID, broad_road=False, key_point=False):
'''
获取浮点型的路径
float_loc: 当前位置的浮点坐标
workbench_ID: 工作台ID
broad_road: 是否只能走宽路
key_point: 关键点模式, 若指定为True则只返回路径上需要转弯的关键点
返回路径(换算好的float点的集合)
'''
def get_better_path(self, float_loc, workbench_ID, threshold=15):
'''
同时计算宽路和窄路,如果宽路比窄路多走不了阈值, 就选宽路
threshold: 阈值,如果宽路窄路都有,宽路比窄路多走不了阈值时走宽路
'''
def get_path(self, float_loc, workbench_ID, broad_road=False):
'''
获取某点到某工作台的路径
float_loc: 当前位置的浮点坐标
workbench_ID: 工作台ID
broad_road: 是否只能走宽路
返回路径(int点的集合)
'''
决策类
决策类主要添加了路径规划相关的调用。
'''
控制类 决策,运动
'''
class Controller:
# 总帧数
TOTAL_FRAME = 50 * 60 * 5
# 控制参数
MOVE_SPEED_MUL = 4.65
MOVE_SPEED = 50 * 0.5 / MOVE_SPEED_MUL # 因为每个格子0.5, 所以直接用格子数乘以它就好了
MAX_WAIT_MUL = 1.5
MAX_WAIT = MAX_WAIT_MUL * 50 # 最大等待时间
SELL_WEIGHT = 1.85 # 优先卖给格子被部分占用的
SELL_DEBUFF = 0.55 # 非 7 卖给89的惩罚
CONSERVATIVE = 1 + 2 / MOVE_SPEED # 保守程度 最后时刻要不要操作
STARVE_WEIGHT = SELL_WEIGHT
FRAME_DIFF_TO_DETECT_DEADLOCK = 20 # 单位为帧,一个机器人 frame_now - pre_frame >这个值时开始检测死锁
FRAME_DIFF = 10 # 单位为帧
MIN_DIS_TO_DETECT_DEADLOCK = 0.15 # 如果机器人在一个时间段内移动的距离小于这个值,
MIN_TOWARD_DIF_TO_DETECT_STUCK = np.pi / 30 # 并且角度转动小于这个值,需要进行检测
# 判断两个机器人是否死锁的距离阈值,单位为米
MIN_DIS_TO_DETECT_DEADLOCK_BETWEEN_N_N = 0.92 # 两个机器人都未装载物品
MIN_DIS_TO_DETECT_DEADLOCK_BETWEEN_N_Y = 1.01 # 一个机器人装载物品,一个未装
MIN_DIS_TO_DETECT_DEADLOCK_BETWEEN_Y_Y = 1.1 # 两个机器人都装载物品
WILL_CLASH_DIS = 1.5 # 很可能要撞的距离
WILL_HUQ_DIS = 2.5 # 可能冲突的距离
# 避让等待的帧数
AVOID_FRAME_WAIT = 20
FLAG_HUQ = True
def __init__(self, robots: List[Robot], workbenchs: List[Workbench], m_map: Workmap):
self.robots = robots
self.workbenchs = workbenchs
self.m_map = m_map
self.m_map_arr = np.array(m_map.map_gray)
self.starve = {4: 0, 5: 0, 6: 0} # 当7急需4 5 6 中的一个时, +1 鼓励生产
# 预防跳帧使用, 接口先留着
self.buy_list = [] # 执行过出售操作的机器人列表
self.sell_list = [] # 执行过购买操作的机器人列表
self.tmp_avoid = {} # 暂存避让计算结果
def set_control_parameters(self, move_speed: float, max_wait: int, sell_weight: float, sell_debuff: float):
'''
设置参数, 建议取值范围:
move_speed: 3-5 估算移动时间
max_wait: 1-5 最大等待时间
sell_weight: 1-2 优先卖给格子被部分占用的
sell_debuff: 0.5-1 将456卖给9的惩罚因子
'''
def detect_deadlock(self, frame):
# if frame % 10 != 0:
# return
"""
每帧调用一次。当检测到两个机器人卡在一起时,会把机器人的成员变量 is_deadlock 设为True。
在采取措施解决两个机器人死锁时, call set_robot_state_undeadlock(self,robot_idx, frame)
把该机器人设为不死锁状态
@param: frame: 当前的帧数
"""
def set_robot_state_undeadlock(self, robot_idx, frame):
"""
当开始做出解除死锁的动作时,调用此函数,把机器人设置为不死锁
@param: robot_idx 机器人的idx
@param: frame 当前的帧数
"""
def could_run(self, loc0, loc1, carry_flag):
# 位置0到位置1区间是否有符合要求
def select_target(self, idx_robot):
# 选择直线过去的下一跳
def get_other_col_info2(self, idx_robot, idx_other, t_max=0.3):
# 返回t_max时间内 最短质心距离
def move(self, idx_robot):
# 运动控制
def get_time_rate(self, frame_sell: float) -> float:
# 计算时间损失
def choise(self, frame_id: int, robot: Robot) -> bool:
# 进行一次决策
def re_path(self, robot):
'''
为机器人重新规划路劲
'''
def process_deadlock(self, robot1_idx, robot2_idx, priority_idx=-1, safe_dis: float = None):
'''
处理死锁
robot1_idx, robot2_idx 相互死锁的两个机器人ID
priority_idx 优先要避让的机器人ID
返回 避让的id及路径
如果都无路可退 id为-1 建议让两个直接倒车
'''
def process_long_deadlock(self, frame_id):
'''
处理长时间死锁,如果死锁处理失败会出现这种情况
'''
寻路模型设计
训练任务是复赛的核心任务之一,官网对机器人的尺寸和地图的大小其实经过了专门的设计。
地图数据是一个 100 行 100 列的字符矩阵。 地图大小为 50 米*50米。 从此得知, 每个字符对应地图的 0.5 米*0.5 米区域。
而机器人空手时一个半径 0.45 米的圆形物体, 携带物品时,其半径会变大为 0.53米。机器人在各种路况中的工作性如下图所示,红色代表手持物品的机器人,黄色代表未持有物品的机器人。
其他选手和官方的普遍方案是对地图进一步细化,将其分成200*200的格子,可以从容地分析通过性,但由于python的效率限制,为了防止超时,我们不得不另辟蹊径。(但其实这一方案埋了很多坑,有些问题我们到决赛才发现并解决的)。
道路等级划分
我们将每一个格子划分为了五个等级
- BLOCK = 0 # 障碍,即这个格子附近有障碍物
- GROUND = 1 # 空地,此处没有障碍物
- ROAD = 2 # 窄路, 临近的四个空地的左下角,即右上四个格子没有障碍的,因此如果经过这类点请从右上角走
- BROAD_ROAD = 3 # 宽路 瞄着中间走就行
- SUPER_BROAD_ROAD = 4 # 广路 可以冲刺
偏移量的引入
如上所述的窄路中,如果瞄着窄路的中心走的话,显然是会被卡住的,实际上我们希望的是瞄着这个四个空格的中心走,因此我们引入了偏移量的概念,当机器人经过此格子时,我们为其附加一个向上和向右各0.5的偏移量。
此外,为了应对上图中右上角的情况,我们同样需要施加一个向下或者向右的偏移量。
特殊情况的处理
1. 禁用实际不可行的路
按照窄路的定义,下图中间的格子也会被定义为窄路,但实际是无法通行的,因此此类格子不能被标记为窄路。
2. 启用实际可行的路
按照宽路的定义,下面中间的两个点也都不属于宽路,但实际上是可以通过的,因此需要将这两个点标记为宽路,并且施加偏移量,使机器人瞄着两个格子中心的位置通过。
3. 工作台的处理
开始时,我们将工作台当成普通的空地处理,只有工作台所处位置是宽路时(因为机器人到工作台要么买要么卖,机器人肯定会有一个手持物品的状态)。但实际上机器人不用完全到达工作台即可与工作台交互。因此,算路时我们先把工作台当做空地处理,最后集中处理无法到达的工作台。
机器人与工作台间的连通性计算
我们分别对每个机器人为原点,在窄路上进行bfs(机器人开始可能被关在一个狭窄的区域,因此要用窄路),获取机器人可达的工作台列表,如果这一过程中发现了其他机器人,说明他们的可达工作台列表是相同的,此机器人的可达列表就无需再次计算。
同样的,我们以工作台为原点,在宽路上进行bfs(从工作台到工作台一定是卖的过程),寻找收购可达的收购本工作台商品的工作台。
路径的计算与存储
在比赛的准备阶段,我们会计算每个工作台到任意可达地点的全部路径,及任意节点到任意工作台的路径。即以每个工作台为原点的最短路径树。简单计算的话,可以将整张地图当做无向无权图处理,直接BFS即可 ,但这样规划出来的路径会频繁拐弯,严重影响了效率。为此我们结合迪斯的思想对算法进行了改进,我们将节点间的权重定义为了方向差,即此节点到上一跳的方向和上一跳到上上跳的方向的差,作为二级权重。同一轮bfs中,新节点优先选择方向差最小的节点作为上一跳节点。
例如,5-2与2-1的方向一致,而5-3与3-1和5-4与4-1的方向差为1,因此5会优先选择2作为自己前往1的路径。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-u8miGG2B-1685123291977)(华为软挑赛2023-复赛笔记.assets/image-20230526225505898.png)]
存储方面,我们借鉴了路由表和查并集的思想。任意点前往一个工作台的相关路径只需一张100*100的列表,每个节点记录自己的下一跳即可。
此部分的核心代码如下:
def gen_a_path(self, workbench_ID, workbench_loc, broad_road=False):
'''
生成一个工作台到其他节点的路径,基于迪杰斯特拉优化
workbench_ID: 工作台ID
workbench_loc: 当前节点坐标
broad_road: 是否只能走宽路
'''
reach = [] # 上一轮到达的节点集合
if broad_road:
target_map = self.sell_map[workbench_ID]
low_value = self.BROAD_ROAD
else:
target_map = self.buy_map[workbench_ID]
low_value = self.ROAD
node_x, node_y = workbench_loc
target_map[node_x][node_y] = workbench_loc
# node_x/y 当前节点 next_x/y 将要加入的节点 last_x/y 当前节点的父节点
for x, y in self.TURNS:
next_x, next_y = node_x + x, node_y + y
if next_x < 0 or next_y < 0 or next_x >= 100 or next_y >= 100 or self.map_gray[next_x][next_y] < low_value:
continue
reach.append((next_x, next_y)) # 将周围节点标记为可到达
target_map[next_x][next_y] = (node_x, node_y)
while reach:
tmp_reach = {} # 暂存新一轮可到达点, k可到达点坐标, v 角度差
for node_x, node_y in reach:
last_x, last_y = target_map[node_x][node_y]
for i, j in self.TURNS:
next_x, next_y = node_x + i, node_y + j
if next_x < 0 or next_y < 0 or next_x >= 100 or next_y >= 100 or self.map_gray[next_x][
next_y] < low_value:
continue
if (next_x, next_y) not in tmp_reach:
if target_map[next_x][next_y]: # 已被访问过说明是已经添加到树中的节点
continue
else:
angle_diff = abs(last_x + node_x - 2 * next_x) + \
abs(last_y + node_y - 2 * next_y)
tmp_reach[(next_x, next_y)] = angle_diff
target_map[next_x][next_y] = (node_x, node_y)
else:
angle_diff = abs(last_x + node_x - 2 * next_x) + \
abs(last_y + node_y - 2 * next_y)
if angle_diff < tmp_reach[(next_x, next_y)]:
tmp_reach[(next_x, next_y)] = angle_diff
target_map[next_x][next_y] = (node_x, node_y)
reach = tmp_reach.keys()
规划效果如下,黄色是规划出来的路径,深紫色是障碍物:
运动控制优化
循迹优化
由于机器人可以全向移动,而机所规划的路径实际只有八个方向,因此有很多不必要的转弯,如图此部分可以直接走直线。
为了实现这一效果,我们会检测当前节点与路径前面的节点之间的直线上是否都是正常宽路,我们选择一个距离我们最远的此类节点,直接直线过去。
此部分核心代码如下:
def could_run(self, loc0, loc1, carry_flag):
if carry_flag:
# 携带物品
for i_point in range(len(x_set_mid)):
x = x_set_mid[i_point]
y = y_set_mid[i_point]
raw, col = tools.cor2rc(x, y)
if raw <= -1 or raw >= 100 or col <= -1 or col >= 100 or self.m_map_arr[
raw, col] < Workmap.SUPER_BROAD_ROAD:
return False
# if raw <= -1 or raw >= 100 or col <= -1 or col >= 100 or self.m_map_arr[raw, col] == 0 or self.m_map_arr[raw, col] == 2:
# # 障碍物
# idx_ob = i_point
# break
else:
for i_point in range(len(x_set_mid)):
x = x_set_mid[i_point]
y = y_set_mid[i_point]
raw, col = tools.cor2rc(x, y)
if raw <= -1 or raw >= 100 or col <= -1 or col >= 100 or self.m_map_arr[
raw, col] < Workmap.BROAD_ROAD or (raw, col) in self.m_map.broad_shifting:
return False
# if raw <= -1 or raw >= 100 or col <= -1 or col >= 100 or self.m_map_arr[raw, col] == 0:
# # 障碍物
# idx_ob = i_point
# break
return True
适时重新选路
因为系统运动控制存在随机性且存在和其他机器人的碰撞,机器人运动时难免会偏离原有路径导致卡死。而我们提前计算出了所以点前往工作台的最短路径,因此我们偏离路径时,可以快速重新选路,使机器人步入正轨。
策略模型优化
- 原先的移动时间估算我们采用直线距离拟合,但在有障碍物的情况下显然是不使用的,因此距离我们改成了格子数*0.5。
- 原有的模型只能控制优先将商品买入格子被部分占用的工作台,并不能指定优先生产哪类商品。例如,当7号工作台只差5号商品就能生成8号商品时,因为我们的机器人距离4号工作台更近,因此他们仍然在生成四号并卖给九号。为了解决这一问题,我们引入了急需商品列表,当7号工作台原料格子被部分占用时,生产他格子对应的商品会得到额外的奖励。
避让模型优化
如前所述,简单的转向并不一定能避让,尤其是机器人“狭路相逢”时,为此,我们额外添加了两个解决方案。
- 当两个机器人无法简单转向避让时,我们分别为两个机器人计算避让路径。根据避让距离长短、手持物品价值以任务执行时间等一系列因素,决定哪个机器人要避让。计算避让路径的逻辑为,寻找一个最近的与要避让的机器人的路线的距离超过安全距离的可达点。
- 当两个机器人陷入长时间僵持时,我们采用了一个简单粗暴但相对有效的方案:两机器人同时后退一段距离,之后其中一个机器人随机等待一段时间,另一个机器人继续前往其目标。
效果展示
代码链接
gitee: Huawei_gives_me_wisdom: 华为软件精英挑战赛2023复赛 (gitee.com)
github: ningfenger/huaweicc2023_semifinal: 华为软挑赛2023复赛 (github.com)