多机器人协同导航技术综述

张 辰，周乐来，李贻斌

（1.山东大学控制科学与工程学院，济南250061；2.智能无人系统教育部工程研究中心，济南250061）

1 引 言

机器人在人类生产生活中扮演重要的角色，极大提高了生产效率。随着机器人技术的飞速发展，多机器人系统相关技术不断涌现，多机器人系统逐渐在工农业生产、高危环境作业、水下与空间环境探测、卫星协同控制等领域大量应用。相对于单机器人系统而言，多机器人系统适应更加复杂多变的动态环境，对环境中的干扰和机器人故障具备更好的鲁棒性；通过多个结构简单、成本较低的机器人组建团队，能够达到甚至超越成本高昂的单机器人所能产生的效果；多个机器人组成协作团队，能够并行执行更加复杂的分布式任务，效率更高。因此多机器人系统及其协调控制成为机器人技术的研究热点之一。

多机器人系统的导航作为一项关键技术，是多机器人系统正常执行设计功能的基础。多机器人通过协同定位方法，获取当前各机器人的位置；通过路径规划方法对每一个机器人的行进路径进行规划计算，并完成协调控制；通过任务分配方法，对多机器人多任务进行最优化分配，提高整体团队的协同效率。

2 多机器人协同定位技术

单一机器人受限于传感器种类、感知范围和信息处理能力，其定位精度有限，且对于环境中的扰动和自身的故障缺乏鲁棒性。多机器人系统通过融合各个机器人的定位信息，提高机器人团队的整体定位精度，增强抗干扰能力和鲁棒性。

定位信息的获取是实现协同定位的基础。目前多机器人系统可以通过多种方式获取环境信息和机器人之间的相对信息，这些方式可以划分为绝对式与相对式两类［1］。绝对式定位技术是通过计算实时获取机器人在环境中的绝对位置，与机器人初始状态和历史信息无关。相对式定位技术则需要根据初始状态和历史信息，估计当前的位置，即本地测量；或者借助某种技术估算自身相对于多机器人团队中其他机器人的位置，即外感测量。根据测量的相对信息种类不同，可分为基于相对距离、相对方位、相对航向等类型［2］。常用的绝对式和相对式定位技术如表1和表2所示。

表1 绝对式定位技术Table 1 The absolute positioning technology

表2 相对式定位技术Table 2 The relative positioning technology

根据多机器人协同定位的结构，可以分为主从式和并行式两类。在主从式协作定位系统中，由携带高精度导航传感器的机器人作为主节点，其他携带低成本低精度导航传感器的机器人作为从节点。在协同定位过程中，主节点为团队提供高精度的导航参考信息，从节点在自身定位基础上，通过接收主节点的导航数据，或对主节点进行观测获得相对位姿数据，对自身定位结果进行校正［3］。主从式结构简单，通信拓扑明确，系统成本低，但是对于主机器人的定位精度依赖较高，要求主机器人具备较好的稳定性和鲁棒性。当主机器人发生故障时，容易导致整个团队的定位精度下降，甚至定位失败。

在并行式协作定位系统中，各个机器人搭载相同的传感设备，在团队中地位相同。在定位过程中，通过融合各机器人的定位数据，实现互相校正。并行式结构没有主从之分，不依赖于某一个机器人的定位精度，在机器人节点发生故障或者通信失败时，仍可保证其余个体的准确定位，具备更好的鲁棒性。但是去中心化的并行网络结构拓扑复杂，各机器人之间互相校正机制容易导致状态相关性问题，融合算法更为复杂。

2.1 概率估计方法

概率估计方法是将每个机器人的位置视为概率分布，通过对机器人的位置进行优化估计，获得各机器人的准确定位。常用的概率估计方法有滤波类方法、极大似然估计法、最大后验概率估计法等。

目前常见的基于滤波的多机器人协同定位方法包括扩展卡尔曼滤波（EKF）［4］、无迹卡尔曼滤波（UKF）［5］、容积卡尔曼滤波（CKF）［6］、粒子滤波（PF）［7］、信息滤波（IF）［8］方法，以及上述方法的改进算法等。

EKF、UKF、CKF是针对基于高斯假设的非线性系统状态估计问题提出的方法，是在卡尔曼滤波框架下，通过对系统非线性的不同处理实现状态估计。在多机器人系统中，非线性卡尔曼滤波框架包括以下4 个步骤：（1）系统建模：分析系统运动模型，构建运动方程和观测方程。（2）时间更新：依据前一时刻的最佳状态估计值，结合机器人线速度、角速度等运动信息，在运动方程中进行一步预测，计算当前时刻机器人的预测位置，并对预测协方差进行一步预测。（3）量测更新：根据机器人外部感知获取的当前时刻相对数据，结合时间更新过程得到的当前时刻预测位置，对机器人位置进行最优估计，并计算当前时刻的估计协方差。（4）循环（2）、（3）步，递归地进行最优估计。

基于以上框架，经典的EKF算法通过对非线性函数的线性化，将非线性函数进行泰勒展开，忽略高阶项部分，保留一阶项部分以实现线性化近似。经典EKF 算法由于忽略了高阶项，引入了线性误差，容易导致算法发散。经典的UKF算法不再对非线性函数进行逼近，而是通过选择一定数量的确定的采样点对非线性函数的概率分布进行逼近，获得了更好的精度。CKF 算法基于三阶球面径向准测，通过容积积分近似高斯加权积分，利用容积点逼近非线性系统的概率分布，是近年来新出现的一种滤波方法。

与以上三种使用近似思想解决非线性系统估计问题的滤波方法不同，PF 方法抛弃高斯假设，基于蒙特卡洛法思想，通过大量的随机采样点对非线性模型和非高斯分布进行模拟。PF 方法理论上能够适应任意形状的分布，但是需要的粒子数量巨大，计算负担较重。

最大后验概率方法通过最大化后验概率的状态位置作为机器人位置的最佳估计，来求解多机器人定位问题。机器人i通过本地测量获取自身线速度与角速度，组成向量u=[v，ω]T，通过外感测量获取相对于机器人j的相对距离和角度数据z=[d，θ]T，通过迭代方法求解使后验概率函数P(x|u，z) 取得最大值的，即为机器人位置的估计［9］。

极大似然估计通过最大化似然函数的状态位姿作为机器人位姿的估计［10］。通过机器人之间的相对观测结果和机器人本地测量数据构建似然函数P(z|x)，通过求解使似然函数取得最大值的，作为机器人位姿的估计。

2.2 优化方法

优化方法采用了与滤波方法不同的思想。滤波方法通过前一时刻各机器人的位姿估计当前时刻各机器人的位姿。优化方法通过建立约束方程和目标函数，通过优化求解目标函数极值，来计算各机器人的位姿。研究人员提出了一系列的优化方法，包括滚动时域法、粒子群优化法、梯度下降法［2］等。

滚动时域估计（Moving Horizon Estimation，MHE）方法近年来逐渐在许多领域开展应用，取得了良好的效果。MHE 是一种动态滚动式的最优估计方法，通过建立固定时间域长度的优化计算窗口，设置先入先出的状态滚动更新机制，引入多机器人系统状态约束条件，使用时间窗开始时刻的多机器人位姿数据组成系统状态向量、时间窗内各时刻的状态噪声序列和到达代价函数，构建性能指标函数。通过最小化性能指标函数，估计时间窗结束时刻的各机器人的位姿数据［11］。MHE 方法的滚动时间窗如图1所示。由于MHE 方法的实时性和动态性特点，算法对于计算能力要求较高。

图1 滚动时间窗口示意图Fig.1 The moving horizon of MHE

梯度下降法作为一种经典的估计目标函数极小值的算法，近年来在深度学习等领域广泛应用，研究人员也将梯度下降方法引入到多机器人协同定位中，对机器人的位姿进行优化求解。测量每个机器人与邻居机器人的局部距离信息或相对方位信息，通过每个机器人的位姿估计值计算对应的相对距离或相对方位，构造测量值与估计值的均方差目标函数。通过梯度下降法求解目标函数的最小值，获得机器人位置的最佳估计［12］。梯度下降法示意图如图2所示，其中锚点机器人用于确定多机器人团队的全局坐标。

图2 梯度下降法协同定位过程示意图Fig.2 Co-localization process with gradient descent method

2.3 地图匹配方法

基于地图匹配的方法常用于对无人飞行器进行导航。提前建立飞行器航线经过地区的地图，在无人机飞行过程中通过感知地形地貌，与提前建立的地图进行匹配，并结合飞行器的惯性导航系统，以确定飞行器当前的位置。飞行器可搭载相机系统，对地形地貌进行拍摄，并与实景地图进行匹配；或者搭载测距传感器，对地面起伏进行建模，并与地图进行匹配。

基于地图匹配的思想近年来也在多机器人协同定位问题中广泛使用，尤其在空地异构机器人协同定位中，地图匹配成为联系空地不同视角的桥梁：将无人机俯拍视角与地面机器人平视全景视角进行匹配，可以实现二者的协同定位。通过无人机单目相机搜索地面机器人，并触发SLAM，通过雷达点云建立地面机器人周围2.5D 地图；地面机器人搭载全景相机，将全景图像中的特征点与2.5D地图进行匹配，以估计自身在地图中的位置和朝向［13］。基于地图匹配的空地协同定位示意图如图3所示。

3 多机器人路径规划技术

路径规划是多机器人团队实现移动和顺利执行任务的关键技术，相对于单机器人路径规划问题，多机器人路径规划问题存在以下几个特点：（1）机器人团队存在更多约束，例如需要保持一定队形，或者需要满足运动的先后顺序约束，即某机器人的行动完成作为另一机器人开始行动的条件。（2）机器人团队内部存在规划冲突问题，即在执行规划路径过程中两机器人可能在同一时刻到达同一位置，导致碰撞。因此多机器人路径规划的研究成为多机器人系统的研究重点之一。

图3 基于地图匹配的空地协同定位示意图Fig.3 Air-ground cooperative positioning based on map matching method

多机器人路径规划技术可以分为耦合式方法和解耦式方法。耦合式方法是将多机器人视为一个整体，将各机器人的所有自由度整合成一个多自由度空间，对其进行搜索和规划。解耦式方法是对每个机器人进行独立的路径规划，之后再通过协调方法对多个独立路径进行协调和修改，以解决冲突问题。

3.1 耦合式路径规划方法

在耦合方法中，许多单机器人路径规划的经典方法被扩展到多机器人路径规划中。人工势场法是机器人路径规划问题的经典方法，通过在机器人的运行空间中构建势能场，目标点对机器人产生引力，障碍物对机器人以及机器人之间产生斥力，通过合力引导机器人走向目标点。由于引入机器人之间的斥力，人工势场法能够较好地解决多机冲突问题，广泛应用于解决多机编队中的路径规划问题［14］。但是在复杂环境中容易出现合力为零情况，陷入“死锁”状态，因此研究者提出了多种改进方法，通过加入偏向力使机器人跳出“死锁”状态，继续行进。多机器人通过人工势场法绕过障碍物的过程示意图如图4所示。

概率类规划方法通过随机采样的方法，在复杂环境中规划路径，计算量小且速度较快，具备概率完备性，尤其适合在高维系统中进行可行路径的规划。常见的概率类方法有概率路线图法（Probabilistic Roadmap，PRM）、快速搜索随机树（Rapid-exploration Random Tree，RRT）等。将m个机器人在某时刻的位置组合成m维空间的一点（起始点），将m个目标位置组合成m维空间内的路径规划目标点，通过对该m维空间进行随机采样，建立随机路线图或生成随机扩展树，建立起始点与目标点的连接线路［15］。快速搜索随机树方法的规划示意图如图5所示。

图4 人工势场法示意图Fig.4 Formation control with artificial potential field method

图5 RRT方法的规划过程Fig.5 The planning process of rrt method

耦合式规划方法能够在全局范围内进行优化搜索，但是多机器人组合形成的多自由度空间维度高，尤其对于机器人团队中机器人数目较多的情况，计算和搜索的复杂度将会呈指数增长，严重影响规划的速度和实时性。

3.2 解耦式路径规划方法

解耦式方法中，首先对各机器人进行单独的路径规划，再通过协调算法将发生冲突的机器人路径进行调整。因此经典的面向单机器人的路径规划方法可以用于生成每个机器人的路径。

研究者们提出了许多不同的路径协调方法。M*算法［16］使用亚维展开框架，以经典的A*算法作为底层路径规划器。在多机器人系统空间中创建一维的搜索空间，通过A*算法为每个机器人单独规划最优路径；在规划过程中若发现两机器人碰撞，则局部增加搜索空间的维数，以协调机器人之间的运动。

基于冲突的搜索算法［17］（Conflict Based Search，CBS），采用两级算法结构，底层使用A*算法对单机器人进行路径规划搜索，在顶层建立基于单个机器人的时间、位置约束的二叉约束树。当多机器人之间的路径产生冲突时，对树执行节点搜索，实现冲突调节。

基于保留区域的方法［18-19］考虑了机器人在未来几个时间步的运动过程，在栅格地图中对单个机器人进行独立路径规划，同时根据运动方向计算未来几个时间步内机器人将要到达的栅格位置，称为保留区域。当两机器人保留区域重叠，则形成多机器人冲突描述，交由中央模块进行任务分配和协调。

以上多机器人路径规划方法都是针对短生命周期过程的一次路径规划，机器人数量和目标点数量基本对等且固定不变，各个机器人到达各自的目标点后，路径规划过程随即结束。针对短生命周期的路径规划方法不具备持续性和时变性问题，有研究者提出了长生命周期的路径规划概念［20］。长生命周期的路径规划方法面向变化的多目标点，团队中某机器人在到达某一目标点后会再次获得新的目标点任务，规划过程长期运行，提高了方法的动态性，更加符合实际应用需求。

解耦式方法将路径规划分散到单机器人尺度，通过协调算法调解冲突，问题复杂度较低，动态性较好。但是解耦式方法难以获得全局最优解，容易陷入局部极小值或者陷入“死锁”状态。

4 多机器人任务分配技术

任务分配技术是多机器人协同控制系统中的关键技术之一，在传感器覆盖问题、兵器多目标打击、以及无人机多点任务分配等场景中处于基础性的地位。任务分配问题是组合优化问题，目标是找到机器人和任务之间的最佳匹配，实现完成任务耗时最少，机器人整体移动路径长度最短，机器人团队总体能量消耗最低等优化目标，最大限度地提高团队的整体效能。

任务分配问题的解决方法主要有基于行为的分配方法、市场机制方法、群体智能方法等。

4.1 基于行为的分配方法

基于行为的任务分配算法［21］是较早提出的任务分配方法之一，通过找到一个具有最大效用的机器人-任务对，将任务分配给机器人。

典型的基于行为的任务分配方法包括ALLIANCE 方法、本地资格广播（Broadcast of Local Eligibility，BIE）方法等。对于每个任务，每个机器人都有能够执行该任务的行为。针对对应的任务，每个机器人对每个任务维持效用评估，用于描述自身解决该任务的效能和成本。在运行过程中，各机器人定期进行任务重新分配的规划，每个机器人向其他机器人传播它对每个任务的效用值。在收到其他机器人的效用后，每个机器人中执行贪婪策略，针对一个任务，将其自身的效用评价与其他每个机器人的效用评价进行比较，选择自身效用最高的为高优先级任务，实现任务分配。

在效用值传播过程中，借助通信的传播方法对于通信的要求较高，研究人员提出了相对观测的分散计算方式：通过维持包含“默许”和“耐心”的离散效能评估方法，当某机器人正在执行一项任务时，随着时间的推移，其自身对于该任务的默许值逐渐增加，而其他机器人对于该任务的耐心程度下降，导致该机器人对于该任务的效用评价逐渐降低。各个机器人在通信心跳帧作用下同步且独立地刷新各机器人的效能评估，降低通信压力。

基于行为的方法实时性、容错性和鲁棒性好，但只能求得局部最优解，无法实现全局的优化求解。

4.2 市场机制方法

市场机制方法［22-23］是一种基于协商思想的任务分配方法，多机器人系统在某种协议基础上通过机器人之间的相互协商、谈判来完成任务分配。

市场机制方法包括多种分配方法，其中最典型的是基于合同网协议的方法。该方法基于市场拍卖模型，主要包括任务发布、任务投标和任务分配三个阶段。首先对任务和机器人的能力进行建模，建立机器人的能力集和环境中的任务集。

在任务发布阶段，当系统被指定一个新的任务，或某机器人根据当前局势发现一个新的任务时，任务中心通过通信网络将任务广播至每一个机器人中。在任务投标阶段，接收到任务的机器人根据自身的能力，对任务进行评价，计算完成任务需要花费的代价，以及预计的完成时间等要素，组合成投标信息发送给任务中心。在任务分配阶段，任务中心收到各机器人的投标信息之后，根据投标信息中的要素计算各机器人或机器人组的投标收益，选择投标收益最高的机器人或机器人组作为该任务的赢家，向其分配任务。

基于市场的方法适合于在任务和机器人状态可知的中小规模异构机器人中进行分布式问题的协作求解，能够实现全局最优任务分配。

4.3 群体智能方法

群体智能（Swarm Intelligence，SI）是仿照自然界生物体的自组织行为，通过对鸟群、蚁群、蜂群等生物群体系统的行为进行研究和模拟，提出的用于解决群机器人行为自组织和规划的智能方法。该类方法没有中央控制单元，由相互作用的简单独立自主个体，按照一定的规律，通过涌现的方法求解某一类问题。

在群体智能方法中，蚁群算法［24］是最为典型的代表之一，在各个领域获得了广泛的应用。通过模仿自然界蚂蚁的觅食行为，对机器人的任务空间进行优化求解。机器人从完成某任务转移到下一个任务过程中，根据完成任务的代价，在两任务之间的路径上留下不同浓度的信息素。通过信息素累加和蒸发因子作用，更新路径上的信息素浓度。信息素浓度影响任务转移概率函数的值，机器人根据转移概率函数选择下一步将要执行的任务，直到所有任务被多机器人团队执行完成。

4.4 人机共融的多机器人任务分配方法

近年来，许多基于人机协同的多机器人协作系统出现，多机器人系统不再仅仅由机器人构成，人在闭环的人-多异构机器人协作模式成为新的研究热点。

2018年，Chowdhury等［25］提出了一种基于距离、剩余能量、机器人的能力和可用性的异构机器人选择算法，通过建立任务请求-任务应答-代价评估-择优分配-故障实时监测，在网络协议层集成本地和非本地任务分配，解决如何有效地将给定的人工任务分配给合适的机器人、如何低成本求解任务分配和非本地任务分配的问题。

Anima等［26］研究了基于层次任务计划的人机协同任务执行问题，将人-机器人协同任务以树状结构表示，并针对重叠和非重叠子任务的冲突进行了不同处理，提高了人-机器人团队中任务的动态分配能力和不同环境条件下的机会式任务执行能力。

Talebpour 等［27］提出了一种在多机器人任务分配环境下的基于风险的自适应重新规划策略，基于社会风险和人类运动预测不确定性的变化，实时动态的调整任务分配，以处理局部感知的局限性和不可预测的人类行为。

以上的成果能够将人这一因素加入到多机器人任务分配过程中，但人仍在任务分配回环以外，通过指令或任务下达的方式对多机器人任务分配形成干预。伴随着机器人任务的复杂化，尤其在多机器人协作团队在有人存在的复杂环境中作业，作为增强人机共融协作环境适应能力，提高人-群机器人协作工作效率的关键，人机共融的多机器人任务分配研究具有重要意义。

5 结束语

本文针对多机器人协同导航技术的研究进行了综合评述，重点针对多机器人系统导航过程中涉及到的三个关键问题：协同定位、路径规划和任务分配问题，分析了多机器人系统的特点和面临的挑战。总结并比较了多机器人定位数据获取技术，重点评述了协同定位算法和技术的种类及特点；总结了多机器人路径规划的方法，以及避免机器人之间冲突的协调方法；总结并比较了多机器人任务分配常用的方法，针对有人存在的多机器人协同环境，总结了现有的方法。

多机器人协同导航技术经过多年的发展，已经取得巨大的进步。但仍面临一些挑战：

（1）如何实现更加高效可靠的多机器人通信、更加简洁的系统结构、更加智能的协调策略等基础技术，仍需进一步研究。

（2）目前的多机器人协同导航技术涉及的机器人数量较少，在大规模多机器人协同方面成果较少，存在一定困难。如何扩展机器人团队数量需要深入研究。

（3）如何将人这一要素引入多机器人协同导航过程中，突破人以简单命令下达或参数设定方式对多机器人导航过程进行干预的现状，实现真正的人在回环内的协同控制，具有重要的研究价值。