使用手册 - 车队管控
1. 概述
2. 路径规划
2.1 管控地图
2.2 寻路算法
3. 交通管制
3.1 死锁检测
3.2 锁点
3.3 包络算法
4. 任务管理
1. 概述
S(imple)实现车队管控的核心功能，包括路线规划、交通管制和任务管理，其余都是围绕这三个的支撑功能。管控功能要用好，仅知道操作是不够的，还要理解其背后的原理。我们用思维导图来描述，以便读者构造出知识树，在使用过程中学习并掌握这些原理，更有效管控移动机器人车队作业。
图 1-1 车队管控的三个核心功能
2. 路径规划
路径规划是指机器人在执行任务时，根据任务需求和环境条件，通过算法规划出一条最优的行进路径。路径规划是机器人导航的关键环节之一，其在机器人的控制、决策和执行过程中起到了核心作用。路径规划可以分解为两个关键问题：管控地图和寻路算法。
设想我们在一个陌生城市，手上有一张城市地图，现在要从火车站去酒店，计划骑车过去。如果要用最短路线到达，那就在地图上找去酒店的路线，可以找到多条路线，这些路线由若干路段构成，测量每段路长度，加起来就是路线长度，找最短的路线。如果路上有施工，则要删除不能通行的路线。如果要经过某条路，比如买个特产，则该路线未必最短。
找路例子中包括了几个要素，也是规划机器人路线要考虑的问题：
- 任务类型。是路程最短？还是运量最大？前者要找最短路线，后者必须经过特定站点。
- 路段通断。规划路线中是否有不能通行的路段？
- 通行效率。只能通行一台车的路段，规划为双向单车道，还是单向车道？
2.1 管控地图
管控地图是指将机器人所处的环境进行抽象和建模，以便于机器人理解和分析环境信息。管控地图主要包括机器人可通行的区域、目标点等信息。读者可能会对管控地图和激光地图产生疑问，都叫地图，有什么差别？
我们先看激光地图，如图2-1。
图 2-1 激光地图
激光地图也称为稠密地图，在建图过程中创建。机器人通过激光雷达来获取环境信息，并以点云形式存储在激光地图中。图2-1是缩小后的激光地图，在D中放大激光地图可看到轮廓由点组成。机器人在建图过程中扫描到的墙体、安全围栏、路边机台被记录下来。激光地图精度高，可以提供详细的环境信息，以便机器人据此定位。激光地图存储空间大，计算量也大，因此存储在车载工控机中本地使用。
管控地图也称为稀疏地图，与稠密地图相对而言。它只包含环境中的关键点，比如库位、机台、转弯点、道路等，如图2-2所示。管控地图信息量较小，存储空间也较小，便于快速搜索和匹配。
图 2-2 管控地图
我们以城市公交来比喻，管控地图就是20路公交车的线路图，标记了从中山公园到外滩的所有站点，在站牌上一目了然，但是它不会标记途径道路的具体信息。
图 2-3 公交线路图
激光地图就是司机记忆中的道路，站牌标记就是关键帧（参考定位导航章）。
图 2-4 公交站牌
我们总结一下，管控地图用于规划路线，激光地图用于导航定位。管控地图主要由站点和路径组成，站点表示机器人应停驻的地方，路径表示站点之间的通行关系。我们用图2-5来说明。
图 2-5 管控地图的路径和站点
路径和站点在S的“场景”标签中管理。使用鼠标滚轮放大地图，并选择一条路径（图中红色线），窗口右下方的“属性”标签显示该路径属性。这是一条双向路径，属性中的direction=0。双击direction属性可修改其方向，1、2分别表示从A到B和从B到A（单向路），单向路径的方向以箭头表示。
路径是一条线段，因此它有两个端点，在线段两头以橙色方框表示。
既然管控地图用路径来表示站点间的联通关系，那么给定起点和终点，就能找出一条过去的路线，这个过程称为路线规划，由寻路算法实现。
2.2 寻路算法
路算法是指在管控地图的基础上，根据机器人当前位置和目标位置，通过一定的搜索策略找到一条最优的行进路径。我们以地图找路来比喻，假设我们在一个路口，手上有张地图，要去酒店，在地图上搜路的思路如下：
- 在地图上找到路口，确定现在位置。
- 从当前路口出发，向酒店大致方向，顺着连接路口的道路往前走。
- 如果道路方向到不了酒店，则后退回上一个路口，再找一条不重复的路尝试，直到找到去酒店的路。
寻路算法原理和我们找路差不多，依靠计算机的算力，它能够找到去目的地的所有路线，并从中选择一条最近的路线。
图 2-6 寻找从1号楼去5号楼的最短路径
我们以图2-6来演示，从1号楼去5号楼的最短路径，很明显是“1 - 3 - 5”，总里程8 + 9 = 17公里。试着在图上找找，去5号楼的路线不止3条哦。
3. 交通管制
3.1 死锁检测
图 3-1 单车道的对头死锁
假设我们开个小电驴走在图3-1的木桥上，对面来个小电驴，谁都不让，就会对头堵死，这就是死锁。所谓死锁，就是两个机器人争夺同一个路段资源，谁也不让。
死锁检测是管控难题之一。当多个机器人在一个场景下作业时，其死锁可能性无法在有限时间内穷尽计算出来，因此死锁检测基本是近似计算，以满足交通管制的实时性要求。
S根据通行顺序来推断死锁可能。我们还是用小电驴过独木桥为例，假设张三和李四都要过独木桥，那么他们两的通行顺序可以是：
- 张三先行。
- 李四先行。
没毛病，对吧。但张三是法外狂徒，他跑到桥中停下了，大概要拦路打劫。那么通行顺序就剩下一个，让李四先走，让张三在桥上玩去。
S在规划路线时，首先寻路，路线找到后做死锁探测，过程如下：
- 将规划路线的机器人与车队其他机器人逐对计算；
- 逐步推算A机器人和B机器人的通行顺序，规划路线的每个站点可理解为独木桥，让谁先走不会堵住；
- 如果全部组合（自己和其他机器人）均可通行，则死锁检测通过，在通行顺序组合中选择一个最优的实施。（通行顺序不止一个哦，想想张三和李四过独木桥例子）
正如张三在桥中打劫，死锁检测算法对机器人终点是敏感的。如果一个机器人的终点位于另一个机器人的路上，那么它必须等对方通过后才能行走。如果两个机器人的终点是一个，那么死锁检测一定失败（张三占了李四的老家）。
死锁检测是管控应用中的一个难点，理解其算法原理，多想想张三在那打劫，就能明白S的路线配置方法。这是个实践过程，除了自己琢磨，还要多与人讨论，如此才能掌握。
3.2 锁点
机器人在规划线路上行驶时，需要锁点。我们还是以张三李四举例，两人小电驴同向回村，不会死锁对吧？张三在桥上停了，这次是小电驴没电。李四有两个对策，一是在上一个站点停下（管停），二是跟在张三后面停下（避障停车）。对管控来说，站点是交管的最小单位，即机器人被管停时都在站点上。如此则带来一个问题，机器人如何知道前面道路通畅（站点空闲）的？
图 3-2 头文字D 椎冰山道对决
机器人在行走时向管控申请锁（站）点，默认3个点，构成2条路段，即正在走的路段和下一个路段。管控会根据站点状态返回锁点结果，如果站点被其他机器人占用，则锁点失败，机器人走完锁定的最后一个路段时会缓行停车，如此则避免了管控干预下的急停问题。锁点时机器人与管控的对话过程，锁点成功则至少有一条路段（当前站点和下一个锁定站点），锁点失败则缓行停在最后一个锁定站点。
图 3-3 锁点过程
图3-3展示了模拟车001行驶时的锁点。在“车辆”标签中选择这台车，在其状态标签的"holdingLocks"就是锁定的点，一共有3个，在管控地图中也能看到锁定的点（站点上方显示锁定站点的机器人名称）。
一般来说，锁点在以下情况失败：
- 站点被另一台机器人锁定。
- 站点禁用。（一般发生在取消任务时，机器人所在的当前站点被禁用）
注意：请注意区分锁点和死锁检测。死锁检测在路线规划时发生，检测到死锁时路线规划也会失败，此时不会下发规划路线给机器人。锁点在机器人执行规划路线时，此时死锁检测肯定通过。
3.3 包络算法
图3-3中，模拟车001行走的路线是彩色的（橙色渐变到绿色），这表示包络线。
"包络"算法（Envelope algorithm）用于生成机器人在环境中的可行运动路径。该算法的原理如下：
- 初始位置设置：设置机器人的初始位置和目标位置。初始位置是机器人的起始点，目标位置是机器人要达到的目标点。
- 包络生成：从初始位置开始，通过不断扩展机器人的包络（envelope），逐步探索可行的运动空间。包络是指机器人在当前位置的占据空间，以矩形表示，比如空载时是较小矩形（表示车身轮廓），带载时是较大矩形（表示托盘轮廓）。
- 包络更新：根据车队中其他机器人的路线（包络线），更新机器人的包络。如果包络与任意一台机器人相交，说明机器人无法通过该区域（两台机器人发生机械干涉）。
- 目标达成：当包络包含目标位置时，表示机器人找到了一条可行的路径到达目标。此时，算法终止，并输出路径。
- 路径提取：从初始位置到目标位置，通过回溯包络生成过程，提取出机器人的路径。这条路径将作为最终的规划结果。
包络可以理解为机器人空载或带载时，在地面的投影面积。如果两台机器人的投影面积有交集，则判定为机械干涉。还记得张三和李四吗？张三小电驴上带了一根电线杆，李四不能跟太近，尤其是张三转弯时候电线杆可能拍李四后脑勺上。电线杆在地面投影就是包络，张三跑路上它是一条线段，张三转弯它就是一个圆。
在包络功能发布之前，S用“必空点”来处理双车机械干涉问题。必空点就是机器人在地面投影所占据的站点集合，一般用在路口，比如叉车在路口转向时，至少需要1.5倍车身长度的空间，则把路口站点以及转向影响到的站点设为“必空点”。 必空点优点是直观，处理简单场景时方便，但是在动态场景下配置工作量极大，因此用包络算法应对。
4. 任务管理
S内核不实现任务管理，该功能由CDM（Chained Delivery Mission，链式搬运任务）插件实现。CDM对任务进行有效的分配、调度和管理。
- 选车算法（Vehicle Selection Algorithm）：选车算法用于从可用的移动机器人中选择最适合执行特定任务的机器人。该算法基于机器人的状态、能力和任务需求进行决策。考虑机器人的当前位置、电量等因素，并结合任务的紧急程度，选择最合适的机器人来执行任务。
- 调度算法（Scheduling Algorithm）：调度算法用于对任务进行合理的调度和安排，以提高整个系统的效率和性能。调度算法考虑到任务的优先级、时效性要求、资源利用率等因素，将任务分配给合适的机器人，并根据机器人的状态和环境变化进行动态调整。
- 任务管理（Task Management）：任务管理涉及对任务的创建、分配、执行和监控等各个阶段的管理。具体包括任务的创建和登记、任务的分配和调度、任务的执行和监控、任务的状态更新和报告等。任务管理系统可以追踪任务的状态、进度和执行结果，以及记录和处理异常情况。
图 4-1 CDM的用户界面
CDM插件实现功能如下：
- 点对点的单取货和取放货任务。
- 调度合适的机器人执行搬运任务。
- 任务列表与任务取消。
CDM一般与维护任务插件协同工作，后者负责将空闲机器人调度去待命点，或者维护充电。