一、 题目背景
海康威视智能泊车机器人依托海康威视多年在图像处理、硬件设计及嵌入式软件领域的技术积累，以及阡陌系列智能仓储机器人历经两年多来精雕细琢已成熟应用的核心技术，瞄准“停车难”痛点，为用户带来完美的停车体验。智能泊车机器人采用海康机器人成熟的视觉和惯性双导航技术实现自主定位，定位精度误差小于5mm，可完成2000kg汽车的升举、搬运、旋转、下放，智能泊车机器人系统可同时调度500辆汽车，同等面积停车场停车位数量增加40%。系统配备海康威视智能停车系统的仓库，最多可并排停放4排汽车，自主研发的智能算法保证用户需取走停在中间排的汽车时，系统会根据车库内车辆停泊的实际情况合理安排其周边车辆以最短路线避让，挪出空位，供待取汽车以最短时间进入客户所在的交互区，2000平方米的停车场，取车平均时间仅2分钟。作为全球首个机器人智能停车应用案例，海康威视智能泊车机器人已应用于第三届世界互联网大会·乌镇峰会，出色呈现真正“互联网化”的智能停车体验。
二、 设计目标
在车库中安排若干泊车机器人，根据给定的车位地图，合理优化机器人的数量及其运动路径，尽量减少客户在停车和取车中的等待时间，并使总成本最小。
三、 总体设计
本算法主要分为两部分，第一部分是预处理，第二部分是调度算法。
预处理包括拒载预处理和车位分配预处理，预处理中忽略了机器人的影响，只从车位和汽车信息来计算出初步方案。其中拒载预处理解决了车位资源紧缺问题，当车位不够时合理拒载一部分车辆以获得更低的总体代价。车位预分配是指在调度算法工作前为每辆车计算好预留车位数，入库时通过预留车位数选择合理车位，相比安排具体车位，这种方式能够提高车位的利用率。通过这种预处理方式将原本的三维数据降低至二维，大大加快了数据的处理速度，从而可以得到较优的全局解。
调度算法包括入库处理，出库处理，返回处理和信息更新。
调度算法采用时间驱动来调度机器人，每隔一个单位时间更新所有的机器人和车辆状态。
本方案的总体设计如图1所示，首先将初始待入库车进行预处理，预拒载一部分，然后对剩余的车分配预留车位数。接下来将处理后的待入库车传给调度系统进行处理。入库前需要计算入库代价来判断选择入库还是拒载。出库时需要机器人提前到车位上做好准备。机器人完成入库或出库任务后都要立即返回入口，然后执行下一个任务。
一、 细节设计
4.1机器人任务调度状态机
机器人有五种状态，分别是入库状态，取车状态，出库状态，返回状态和空闲状态。
状态内容说明：
1）入库状态：机器人运载车辆入库。
2）取车状态：机器人到车位上准备载车出库
3）出库状态：机器人运载车辆出库
4）返回状态：机器人返回入口
5）空闲状态：无任务等待被调度
4.2车位预处理算法
4.2.1拒载预处理
原理：
本方案采用链表来预处理拒载车辆。如3是拒载处理示意图，方格中的数字是车重。在入库时将车辆添加至链表，出库时则移出链表。若链表发生溢出，则从链表中移出重量最大的车，然后继续入库。
优点：
预先拒载一部分车辆后，不仅可以避免后续入库算法中对车位资源紧缺的处理，同时也减少了计算量，加快了运行速度。
4.2.2预留车位数计算
原理：
预留车位数含义：车入库选择车库时，为后面待入库车辆预留数个更优的车库。
具体步骤如下：
1.计算每个车位离出入口的总距离，并进行降序排序。
2.计算每辆车的优先级系数，其中
是车重量，
是车的申请入库时间，
是车的申请入库时间，
是车得停车时间，
是车的平均停车时间，β和θ是比重系数，P是车的优先级：是车的平均重量，
3. 建立任务队列和待出库队列，将所有待入库车添加到任务队列，其中任务队列按时间排序，待出库队列按优先级系数排序。设置每辆车的记录为0。
4. 依次读取任务：如果是入库任务，将该车插入到待出库队列中，根据优先级和辅助计数值更新预留车位数，然后将该任务修改为出库任务。如果是出库任务，更新待出库队列中的辅助计数值。
5. 任务队列为空时，得到最终的预留车位数。
优点：
总距离越长的库，机器人处理时的功耗和时间代价相应也越较高，因此提高总距离较短车库的使用率有利于降低总体代价。预留车位数的机制不涉及具体车位，车位计算和当前车库使用情况有关，因此入库中不会因车辆拒载而浪费车位资源，从而提高了车库的使用率。
车的优先级决定了车库的安排，对结果有很大影响。因此需要合理设置β和θ这两个比重系数。本方案采用分段搜索的方法，每段使用最速下降法搜索最优解，这样既加快了搜索速度，有能避免陷入局部最优解。
4.3机器人数量计算
原理：
机器人数量大小对总成本有很大的影响，设置过大会导致机器人使用率低下，设置过小又会导致机器人超负荷，造成入库出库困难。因此需要计算出一个合理机器人数量。
本文采用了固定机器人数量和动态分配相结合的策略。首先采用动态机器人分配算法，尽量满足出入库的需求，以此得到机器人的最大个数。然后往小搜索机器人个数，用固定机器人的方式求出最优解。
优点：
本算法迭代次数少，有很强的适用性，总能在理想的时间内计算出最优机器人个数。
4.4调度算法设计
4.4.1整体流程
调度算法有四部分组成：机器人返回处理，入库处理，出库处理和信息更新。调度系统以时间驱动方式编写，前三部分负责给机器人安排任务，信息更新用于更新调度系统的运行状态。
4.4.2入库流程
原理：
入库前首先要计算入库代价，如果入库代价大于拒载代价，则执行拒载处理。如果入库代价满足入库要求，则计算出最优路径，然后执行入库任务。
入库代价计算公式如下所示，其中Walt是入库功耗，μ是单位时间成本，
是入库时间延迟。
入库时还涉及到机器人分配模式，如果是固定机器人数模式，当入口无机器人时，估算得到空闲机器人的等待时间，然后重新计算入库代价，从而作出更优决策。如果是动态分配模式，入口无机器人可用时无条件新增一辆机器人，然后执行入库任务。
优点：
机器人在运动过程中实时记录着它离入口的时间距离，这样就能得到精确地入库的延迟时间，进而计算延迟对本车以后面待入库车产生的总代价，然后作出合理的拒载决策。
4.4.3 出库流程
原理：
出库处理时需要同时规划两条路径，一条是取车路径，另一条是出库路径。
当出库剩余时间长度小于该车库的最短入库路径长时， 释放车库资源。
优点：
由于出库路径长度计入功耗，而取车路径不计，因此提前规划出库路径较容易得到最短路径，降低功耗成本。
提前释放车库资源有利于提高车库的利用率。
4.5.1二维A*算法
原理：
A*是一种启发式的算法，所谓的"启发式"，就是对每一个搜索的位置进行评估，也就是把找的位置离目标的距离当成找点的一个依据，然后猜测这个点是否最佳("启发式"就是猜测)。
步骤：
先设定两个集合，open集，close集
1、将起始点加入open集（设置父亲节点为空）
2、在open集中选着一个F值最小的节点作为当前节点
2.1 将其添加到close集，并从open集中移除！
2.2 如果为终点节点，那么结束搜索
2.3 处理当前节点的所有邻接节点
如果不在open集中，那么就将其添加到open集，（最好设置该节点的父节点为当前节点，以便确定路径）；
如果已经添加到open集中，重新计算G值，如果G值小于先前的G值，那么就跟新此值(同时跟新父节点)；
如果该节点不可通过或者已经被添加到close集，那么不予处理；
3、如果open集不为空，那么转到步骤2继续执行
4.5.2三维A*算法
原理：
1.可达点判断改进：
为了解决时间冲突问题，改进了二维A*的可达点判断函数，当时间发生相交或冲突时，使该点不可达，这样就可以进一步在时间维度上避免冲突。
在时间维度上有两种冲突类型：时间点重叠冲突和时间点互换冲突。由于出入口允许重叠，因此对出入口特殊处理，只检查时间点互换冲突，而其他区域需要检查两种冲突。
1.路径优化：
改进了可达点判断函数后，虽然彻底避免了碰撞和相遇冲突，但是很容易导致路径规划过长甚至规划失败。
如下图所示。蓝色的线是一号机器人的入库路径，绿色的的线是二号机器人的返回路径，从图中可以看到出入库路径和返回路径有很大一段发生了重叠，如果路径重叠出时间刚好出现冲突，其中一条路径必须往一边躲避一格，这会增加路径搜索长度，特别是当机器人较多的时候运算时间会急剧上升。
针对以上问题，本文改进了启发函数，在曼哈顿距离公式的基础上修改y轴的比例系数，式中
为比例系数：
这样一来，搜索路径时便会优先搜索y轴方向。图10中黑色线便是改进后的入库路径，减少路径重叠，加快了路径搜索的同时也更容易获得更优路径。
4.5.3最优路径求解
原理：
在多机器人环境中，运动错综复杂，因此往往导致规划的路径过长甚至规划失败，因此本文采用了任务延迟机制，通过延迟时间来规划更优路径。
下图是最优路径求解流程，设置任务延时初始值为0，然后调用三维A*算法寻找最优路径，如果找到路径是最短的，则路径规划成功。否则递增时间延时，每次迭代记录最小路径代价，直至时间延迟代价大于最小路径代价，终止循环，返回最优路径。
优点：
仅通过增加一个任务延迟变量优化了路径，算法简单利于实现。
