HUAWEIRobot

• 概要：机器人在各大领域显现出巨大的商业价值，本次赛题抽象自华为5G应用场景，在一个地图中对每个机器人做路径规划，进行买卖物品操作，使得最后的总收益最大。
• 成果：2023年华为软件精英挑战赛上合赛区 初赛正式赛 rk7 、复赛正式赛 rk11

项目结构

_________
 |____Demo
 |____Img
 |____SDK
 |____test
 |___PID.py
 	     |___calcation.py
 	     |___config.py
 	     |___contraception.py
 	     |___halfoptimizie.py
 	     |___main.py
 	     |___policy.py
 	     |___robot.py
 	     |___wall.py
 	     |___workbench.py
 |____log.txt

• Demo、Img、SDK为华为封装好的接口文件；log.txt为调试中间信息。

• PID.py为机器人移动控制函数，calcation.py为项目常用函数集合，config.py为项目所用超参数，main.py为项目主函数，policy.py为过往的机器人任务分配函数集合，wall.py为墙类，workbench.py为工作台类，robot.py为机器人类，contraception.py和halfoptimize.py为实现orca的防碰撞函数。

• 整个项目仅需numpy依赖。

项目流程

• 通过指令 ./Robot_gui -m maps "python test/main.py"与判题器交互来运行代码。

• 在首次读入地图之后，有5s的初始化时间；判题器每帧会返回当前帧信息，即所有工作台的位置与工作状态和所有机器人的位置与工作台状态，每帧处理并返回指令的时间0.02s；其余具体信息可以查看项目报告书。

• 算法流程：
  	初始化地图信息
  		保存所有的工作台信息和机器人信息
  		对所有机器人做BFS求得其到所有工作台的路径
  		对所有工作台做BFS求得工作台到工作台之间的路径
  	针对每一帧
  		对空闲机器人和可接的任务做分配
  			对没有涉及过的路径做路径优化
  		调整机器人所处状态
  			根据PID控制机器人移动
  			根据orca防碰撞

项目核心

整个项目共可分为三大部分：初始化路径规划（复赛添加）、物品分配策略、实际移动策略。下面开始逐个介绍三个部分的创新点。

路径规划

路径规划部分使用两个算法来控制，分别是规划算法和优化算法。

• 规划算法：规划算法需要指定有限的可能动作，才能对空间进行遍历。机器人在计算机存储时状态是离散的，但在实际移动时状态是连续的。为了防止在实际移动中由离散到连续中所忽略的部分状态，提出一种“走半步”的想法，在地图上每次不是移动1的单位距离，而是移动0.5的单位距离。所以最终的有限可能动作是朝八个方位移动0.5的单位距离，尽可能排除因量化导致的状态偏差。

  具体实现使用了BFS和ASTAR两种算法。

• 优化算法：根据规划算法得到的路径是一连串的点，其中很多点都是没有意义的。为了只保留关键点，使用了基于二分算法的路径优化算法。对于答案数组，选定st和ed，如果两者不能优化那么选择两者的mid为另一个关键点；之后判断优化[st, mid], [mid, ed]这两段能否优化，如果[st, mid]可以优化，那么需要优化的路径仅剩[mid, ed]，如果[st, mid]不能优化，那么需要优化的路径为[st, mid2], [mid2, mid], [mid, ed]，每一段都同理。

  优化的具体逻辑是判断每一段的起点-终点连接起来的直线中间是否会碰到墙体。注意墙体并非是一个点（在官方给出的txt地图中是一个点，很有迷惑性），而是一个0.5m*0.5m的区域。同时这个区域不是圆形，而是正方形，这导致了碰撞逻辑存在各向异性，即不同角度的直线对于同一块墙体的极限碰撞距离并不相同。因此，对于直线-墙体的碰撞检测算法，需要精心设计，本项目共设计了两种碰撞检测算法：

  • 沿线逐离散像素点检测。

    1. 该算法从直线的视角出发，从直线的起点到终点，找到所有x坐标为整数或者y坐标为整数的点，根据机器人是否持物体的状态，检测x坐标为整数的点左/右的1/2个单位区域内是否存在墙体，检测y坐标为整数的点上/下的1/2个单位区域内是否存在墙体。（检测不充分？关键点确保不碰撞墙体，但是关键点与关键点之间有可能碰撞墙体？）

  • 区域墙体碰撞检测。

    1. 该算法从单位墙体的视角出发，检测墙体是否会碰撞到直线。注意直线也是一块斜着的矩形区域（想象机器人质心从起点运动到终点覆盖的面积）。检测的方式是计算墙体质心到直线的距离（通过直线距离计算公式），这种计算是连续的，所以保证是准确的距离值。
    2. 但实际的距离值并不仅仅如此，由于上述所讲的各向异性，真正的距离值是墙体离直线最近的角点到直线的距离。这个距离可以通过计算几何计算得到（根据直线离x正半轴的夹角为0、pi/4、pi/2、3pi/4、pi共分为4种情况）。最终只需要检测距离值是否大于某个阈值即可（持物机器人、未持物机器人共有2种情况）。
    3. 为了加速算法，我们并不需要对场景所有墙体做上述操作，因为有些墙体明显不会与直线碰撞。通过机器人的持物状态，我们可以得到上述的斜着的矩形区域，将这个矩形区域往外扩展一定距离，保证所有可能与直线发生碰撞的墙体都包含在这个矩形区域内，然后计算矩阵区域的Xmin、Xmax、Ymin、Ymax，通过这四个值继续扩展出一个轴向的矩形区域（这样便于计算），最终我们只需要计算这个矩形区域内的所有墙体即可。这种加速是自适应的，即小/短直线检测的墙体少，大/长直线检测的墙体多。

  针对优化出来的路径，还需要遍历一遍，只保留每条直线的起点和终点。

分配策略

分配策略部分有两种不同的策略，分别是以机器人为视角的自底向上的贪心和以工作台为视角的自顶向下的分配。

• 自底向上：考虑每一个机器人的任务分配，根据价值及潜在价值选择单位时间内价值最高的任务。同时为了避免陷入局部最优，通过限制基础任务之间的任务次数来保证最高级任务能被完成。

• 自顶向下：从工作台的角度来考虑离工作台最近的机器人，需要先看最高级任务所需要的物品，再看次高级任务所需要的物品，再选择同一层次物品中价值最高且单位时间内价值最高的任务。该策略保证了最高级任务一定是最优先完成的。

  具体实现1：使用了多级队列。

  具体实现2：

  1. 构建了两个图，分别是最近邻图和锁图。其中最近邻图中高级工作台指向最近邻的低级工作台集，比如7->456、6->23，指向的前提是高级工作台到低级工作台之间没有被阻塞（阻塞的意思仅仅是该任务的起点和终点对应物品没有被其他机器人抢占，在这里低级工作台成品槽没有物品不会被判断为阻塞，确保建图的顺利）。锁图中记录高级工作台到所有相关的低级工作台的可达信息，比最近邻图的“阻塞”更加严格，这里低级工作台的成品槽没有物品就会被判断为不可达。所以在最近邻图能通过的边，不一定能通过锁图，通过这种分级加锁机制确保算法的顺利执行。
  2. 最近邻图和锁图每一帧都会刷新，在进行任务分配时，先找到场上所有可用的工作台，并对这些工作台进行排序，排序的规则是：高级工作台排在低级工作台前面，同等级工作台情况下，到最近邻低级工作台平均距离小的排在前面。从排好序的列表开始，对最近邻图进行BFS搜索，每一级新入队的低级工作台集都要进行上述的排序。若当前工作台在锁图中通过，就去寻找离这个工作台最近的机器人，若这个机器人是free的，就直接分配任务，若这个机器不是free的，就把这个任务加入任务缓存中，若任务缓存中已经有该机器人的任务，就比较高级工作台的等级和机器人到低级工作台的距离来判断哪个任务更优，将更优的放入任务缓存中。每次开始分配任务时，先检查任务缓存中的任务是否都可用，将不可用的剔除，若缓存中某任务的机器人刚好是free的，就直接分配任务，并把该任务从缓存中剔除。
  3. 设置任务缓存的目的是，如果工作台只找free的离它最近的机器人，就有可能找到一个很远的机器人（因为只有它是free），而该机器人附近也许有一个即将完成的任务，但是等到该任务完成后，该机器人已经被分配，所以该任务也只能找较远的free的机器人，这将导致恶性循环。所以寻找全场所有的机器人才能避免这个问题，但是找到的最近机器人就有可能不是free的，这时就有了任务缓存的必要。因为任务缓存每一次分配也会刷新，所以其保留的是最近时间的最优分配信息，当完成任务后的机器人搜索任务缓存时，如果能搜到，这个任务对于当前机器人来说就是最优的，可以直接分配。在上述场景中，较远的free机器人不会被分配，而该机器人会被附近的工作台标记放入任务缓存，等到该机器人完成任务后，就可以接到附近工作台的任务。

• 合并策略：在任务刚开始时，使用自顶向下的策略来保证有一个或多个最高级任务肯定可以被生产，之后切换回自底向上的任务来保证部分时间最优。

移动策略

这部分主要由两个算法组成：PID算法和ORCA算法。整个项目通过该模块来控制机器人的移动。 同时为了应对更复杂的移动场景，通过优化移动控制算法实现了对机器人更精细化的控制。

• PID算法：根据实际输出值与目标输出值之间的误差来调整控制器的控制量，以使得输出值能够稳定地达到目标值。基于三个参数：比例增益（P）、积分时间（I）和微分时间（D）。其中，比例增益通过将误差乘以一个常数来产生控制量；积分时间通过对误差进行积分，以减小稳态误差并提高系统鲁棒性；微分时间则通过对误差变化率进行测量，用于抑制系统的超调和振荡。

• ORCA算法：通过对智能体运动的速度和方向进行限制来避免碰撞。每个智能体都会计算出一个“优先速度”，该速度既可以保证其尽量朝着目标点移动，又可以避免在某个时间范围内与周围其他智能体发生碰撞。为了实现这一目标，ORCA算法引入了一个“半平面交”概念，即根据当前智能体的速度和其它所有智能体的速度，计算出限制其运动的半平面区域，然后在该区域内选择一个最优速度作为其优先速度（到该半平面的垂线）。墙体可以视作不动的机器人加入计算，使得机器人获得小区域内灵活避障的能力。避免碰撞会导致机器人的速度下降，为了保证最大收益，设计了特权避免碰撞机制：持物等级低的机器人让行持物等级高的机器人，保证高等级机器人能够拥有尽量大的速度，若机器人等级相同，则对彼此的碰撞负有同等责任，墙体拥有最高等级。

• 细粒度移动控制算法：在复赛的场景中，由于障碍的存在，需要更细粒度地对机器人的移动进行控制，防止其在路程中途卡死。 具体实现:

  1. 循环移动队列。设置一个循环移动队列供机器人进行移动。在每次任务分配完成之后，队列的初始值为路径规划并优化后，得到的有序稀疏点集合，表示机器人到达目标点过程中的关键控制点，并在队列结尾插入一个结束标志。机器人每到达一个关键点，将队列进行一次rotate操作，将队首元素移动至队尾。当机器人到达目标点时，其移动队列的首位为结束标志。

  2. 记忆移动队列。由于多机器人移动可能存在碰撞的影响，以及ORCA算法的存在，会导致机器人的移动路径出现不可预测的变化，导致机器人在“陷阱点”卡死。因此为每个机器人设立了一个固定大小的记忆移动队列。从每个关键点开始，根据固定时间间隔对机器人的当前位置进行采样，并在到达下一个关键点之后进行刷新。保证机器人的移动完全可回溯。

  3. 死锁检测。

     • 机器人发生死锁存在两种情况，一是机器人与机器人顶牛，二是机器人卡死在墙上（墙也可看作静态的机器人）。
     • 每帧中PID算法会根据机器人与目标的之间的角度与方向偏差为其设置速度，若设置速度不为零且与环境中测得的机器人速度极小，则可以判定当前帧机器人的移动受到了较大的影响。称为阻塞帧。
     • 当检测到连续多帧为阻塞帧时，即可判断当前机器人进入了死锁。

  4. 回退机制。

     • 检测到机器人进入死锁，则将机器人设置为回退状态。
     • 机器人根据记忆移动队列先返回到上一个目标点。
     • 机器人将循环移动队列进行逆向的rotate操作，根据路径关键点进行回退，直到回退距离达到设定值。

改进方向

• ORCA无解问题：ORCA会出现无解情况，代表这时候已经避无可避。这时候机器人会选择摆烂，即按原速度前进。撞上物体后可能会出现长时间的“顶牛”，这时候可以考虑对机器人进行“顶牛”检测，对于正在“顶牛”的机器人，将其原地顺时针/逆时针旋转90度，然后再让其根据PID和ORCA进行调整。
• 窄道碰撞问题：在狭窄通道不能容忍两个机器人并行，且无法进行防碰撞。考虑实时分级加锁的方法，来保证分配到高等级任务的机器人会优先完成任务，而低等级任务的机器人会在和高等级任务机器人重复路径前停住脚步。由于我们的路径规划基于关键点，而在无法进行防碰撞的窄道中，关键点会是相同的。高级任务机器人会将沿途关键点路径加锁，实现的方式是将路径变成墙（这个墙跟地图里原有的墙区别开，只在移动策略中发挥作用，在分配和路径规划中路径墙不会出现），这样低级任务机器人就会在重复路径前先等待，等到高级任务机器人完成任务解锁后，才会继续任务。这样保证了窄道只会有一个机器人，从而避免了窄道碰撞问题。
• 初始化超时问题：为了避免初始化超时，限制全局BFS的工作台数量，只对工作台种类集合[4, 5, 6, 7, 9]或[1, 2, 3, 4, 5, 6, (7 or 8)]的工作台种类集合进行初始化。
• 代码优化：根据Python最快写法来优化整个项目，部分运算使用numpy进行加速
• 针对每张图调参