一种通信受限的多机器人协作探索方法与流程

1.本发明涉及机器人通信技术领域，具体涉及一种通信受限的多机器人协作探索方法。


背景技术：

2.当前，机器人探索系统在矿业勘探、应急救灾、农业监测、地理测绘等领域广泛应用。而面对愈加复杂和大规模的任务，多机器人协作探索具有对比单个机器人更强大的性能，更好的容错性和更小的不确定性。多机器人有很强的信息共享需求，然而在现实世界中，尤其是大范围环境中，通常不具备全局通信保障，且大量传感器数据可能超过通信网络容量。
3.结合burgard、andre等人发表在机器人顶级期刊上的研究，可以总结出多机器人协作探索方法一般有如图1所示流程，多机器人系统中共有n个机器人(n≥2)，每个机器人都会用自身传感器感知环境，收集环境信息进行环境建图，并从建图信息中提取前沿信息和已探索空间信息。前沿信息指已知无障碍空间和未知空间的分界，将当前空间中存在的前沿位置设置为机器人的目标即可驱动机器人探索未知环境。机器人通过通信网络互相连接并共享信息，通常所有互相连接的机器人会从中选择一个作为主持者，整合所有机器人的信息，并根据整合后的信息进行任务分配并下发给机器人。
4.目前多数多机器人协作探索方法对于通信网络做理想化假设，然而现实世界中，待探索的环境通常不具备全局通信保障，机器人之间的通信有不可忽视的通信距离限制与通信带宽限制。通信距离限制将影响任务分配，多机器人在完成探索任务的同时需要保持通信以共享信息；通信带宽限制将影响已探索环境信息的表示，前沿信息本身数据量较小，基本不受带宽影响。为提供使主持者做出决策的环境信息，需要采用轻量化表示方法表征已探索环境，减少传输过程中的带宽占用。
5.在通信受限条件下多机器人协作探索方法主要有两个难点：(1) 通信距离受限的多机器人协同探索决策；(2)通信带宽受限的已探索环境信息高效表示。
6.目前，针对(1)受限的通信距离的多机器人探索决策方案有：a、不要求通信保持方案；b、要求连续通信方案；c、间歇性通信保持方案。其中a种类的方案将使系统有较大不确定性，多机器人之间的信息共享是偶发且机会主义的，只有在机器人互相可通信的情况下进行基于竞标法或市场法的任务分配。这种方法并不能保证机器人之间始终可通信，一旦超出通信范围很难再次建立通信，无法保证信息在多机器人间被有效共享，这可能导致重复探索和无收益的能源消耗。b 种类的方案要求在整个探索过程中，多机器人互相保持完整的通信连接，随时进行数据共享。机器人将成团探索，只被允许在可通信范围内选择待探索目标点，这将引入很强的决策限制，导致重复探索面积很大，探索效率提升有限。c种类的方法允许机器人主动地断开连接，并在完成部分探索后寻求主动重连。现此类方法多有中心基站，且基于角色，即在多机器人系统中，部分机器人作为探索者负责环境信息收集，部分机器人作为传送者在探索者和中心基站之间传递信息。然而基站的要求仍限制了系统的
灵活性。
7.传统的环境信息表征方法有：栅格地图，八叉树地图，点云地图，这些地图信息丰富，但数据量大，一旦环境范围过大很容易超出通信带宽，不利于数据共享。针对(2)受限通信带宽下的环境信息表征方法有：a.高斯混合模型表征；b.凸包表示。其中，a能对环境中障碍物以高压缩率表示，但由于其不具备记录无障碍空间能力，导致在探索决策时需要重建无障碍区域，该重建过程计算量大且精度不高。b 使用一系列凸多面体的并集对环境中无障碍空间进行表征，传输所需数据量较小，且包含区域是否已探索的信息，更适用于探索决策。为使凸包表示能力更强，凸包的变体星凸被提出，星凸的凸限制比凸包稍弱，能对环境表示更精确。但该技术尚未在多机器人探索系统中被使用。
8.总体上，当前的在通信受限条件下的多机器人协作探索方法受制于以上两个难点，在不引入额外的信息辅助设备条件下，如中心基站，并不能很好地既保持一定的通信连通性，又避免大面积重复探索，提高探索效率。


技术实现要素：

9.针对现有技术的不足，本发明旨在提供一种通信受限的多机器人协作探索方法，解决在现实世界的通信距离限制和通信带宽限制下的多机器人协同探索问题，适用于任意型号多旋翼、固定翼或混合翼无人机、地面机器人，在通信带宽与通信距离受限的条件下，对未知环境进行探索所需要的多机器人协作场景。
10.为了实现上述目的，本发明采用如下技术方案：
11.一种通信受限条件下的多机器人协作探索方法，每个机器人均有一个用于储存环境信息的环境信息库并有两个规划器，两个规划器分别为独立规划器和联合规划器；
12.各机器人启动时聚集在一起，互相之间均可通信，此时处于联合集会阶段；在联合集会阶段，触发各个机器人的联合规划器进行规划，机器人将选出一个主持者，并将自己的环境信息库中的所有信息上传给主持者，主持者在进行信息整合后进行任务分配，向每个机器人分配需要访问的目标点和最终约定的进行下一次集会的地点和时间；至此联合集会阶段结束，各机器人将主动断开连接，进入独立探索阶段；在独立探索阶段，各机器人触发独立规划器进行规划，通过传感器收集新的环境信息，生成新的无障碍空间和前沿信息并对旧的环境信息进行更新，然后以被分配的目标点和最终需要到达的集会地点和时间作为限制进行独立规划，生成新的目标序列；在约定的集会时间各机器人到达约定的联合集会地点进行环境数据共享，再次从独立探索阶段进入联合规划阶段。
13.进一步地，将每次独立规划器被触发时生成的所有环境数据称为一帧数据，所述独立规划器的规划过程为：
14.步骤a1、采样障碍物点：对障碍物点进行采样，生成用于生成星凸的点集s
star
；用于生成星凸的点集s
star
需要包含两部分：当前观测到的障碍物点s
obs
和传感器最大观测半径上的采样点集s
free
；
15.步骤a1.1、生成障碍物点云s
obs
：在融合多帧传感器数据生成的局部占用栅格地图中采样，生成在当前机器人位置能够观察到的障碍物点云s
obs
；以当前机器人位置pr为柱坐标系原点，在多个半径下进行不同密度采样，其中n
表示采用的不同半径的数量，r
senor
为传感器最大感知半径；对于每个采样点 ps，从pr为起点，向方向进行光线投影，如果在投影过程中和障碍物碰撞，记碰撞点为p
hit
，则将p
hit
加入s
obs
集合；
16.步骤a1.2、生成最大观测区域点云s
free
：在步骤a1.1进行柱坐标系采样时，当采样半径r＝r
sen
且光线投影未与障碍物发生碰撞时，将该采样点ps加入s
free
点集；
17.步骤a2、星凸生成：利用步骤a1生成的点集，构成用于生成星凸的点集s
star
＝s
obs
∪s
free
，并利用hpr方法计算星凸；将当前帧的星凸sk和当前机器人位置rk记录到环境信息库中；记第k帧的星凸为sk，则当前机器人已探索的无障碍空间表示为s1∪s2∪...∪sn，其中n是当前机器人的环境信息中记录的空间星凸数量；
18.步骤a3、前沿面元更新：由于采用星凸来表示已知无障碍区域，那么组成星凸的面元即是已知无障碍区域的边界，只要除去完全由障碍物生成的星凸面元，剩下的面元就是已知无障碍空间与未知空间的分界，称其为前沿面元；
19.步骤a3.1、当前帧前沿面元生成：对所有面元进行判断，如果该面元满足以下任意一个条件，该面元不是前沿面元：a.该面元的所有顶点均属于s
obs
；b.该面元的面积小于最小有效面积阈值η
area
；c. 该面元的法向量z轴分量的绝对值大于z轴分量阈值z
thr
；筛选完当前帧星凸的所有面元后将得到当前帧的前沿面元集合fk；
20.步骤a3.2、前沿面元的帧间更新：假设当前帧为第k帧，此步骤将完成两方面更新：(1)删除处于之前帧星凸{s1，s2，...，s
k-1
}中的当前帧的前沿面元，更新fk；(2)删除处于当前帧星凸sk中的之前帧的前沿面元，更新{f1，f2，...，f
k-1
}；
21.步骤a4、前沿面元聚类：对前沿面元进行聚类，并对每个前沿聚类生成一个对应的视点，将距离较近的视点融合为一个超级视点。
22.更进一步地，步骤a1.1的采样过程对于障碍物密度是自适应的，对于相邻的两个采样点pi，pj，将在pi，pj采样的中间角度继续递归地采样，直到如下条件满足其一：
[0023][0024]
||p
i-pj||＜d
thr
。
[0025]
更进一步地，步骤a3.2中，对每个星凸建立一个面元表，利用该面元表查询一个面元是否处于一个星凸中；
[0026] (1)面元表的建立过程如下：
[0027]
1.1)建立面元表的对象：将星凸si的所有面元投影到一个栅格化的单位球上，该单位球上的每个格子记为g
i，j
；对于一个被投影的面元fi，可以得到其在单位球上的轴向边界矩形，将处于该bi中的所有格子记为gb＝{g
i，j
|g
i，j
∈bi}；
[0028]
1.2)将gb中的格子和fi的对应关系记录到面元表中；最终栅格化的单位球上的每个格子将对应一个或多个面元；
[0029]
(2)使用面元表快速查询过程如下：
[0030]
对于待查询的面元fi和星凸sj，先将fi的中心投影到以星凸sj对应的机器人位置rj为球心的栅格单位球上，投影点落到g
i，j
格子中，在sj的面元表中查询g
i，j
对应的面元{f
′j|j∈{1，2，...，j}}，共j个；查询到的每个面元f
′j的三个顶点与rj连接形成一个三棱锥，共形
成j个三棱锥，如果fi的中心在其中一个三棱锥中，就判断为fi在星凸sj中。
[0031]
更进一步地，步骤a4的具体过程为：
[0032]
步骤a4.1、前沿面元谱聚类：进行谱聚类需要先构建两个矩阵：度矩阵d和相似矩阵s；对于度矩阵d，将所有面元与其k个欧氏距离最近的邻居连接起来，形成一个图，计算该图的度矩阵作为d；对于相似矩阵s按下式进行计算：
[0033]
s＝(exp(-s(fi，fj)2/2σ2))
i，j＝1...j
[0034]
其中，s(fi，fj)是fi，fj两个面元之间的总距离，s(fi，fj)＝ ||[d
t
，dn，δn]||
ω
，σ是预设的高斯函数的参数；d
t
，dn，δn分别为3种衡量相似度的距离，d
t
(fi，fj)＝||(c
i-cj)
·
nj||2为切向距离， dn(fi，fj)＝||(c
i-cj)
×
nj||2为法向距离，δn(fi，fj)＝||(n
i-nj)/2||2为法向角度差；
[0035]
计算出d和s后，即可通过谱聚类得到前沿聚类；
[0036]
步骤a4.2、视点选择：每个前沿聚类生成一个对应的最佳视点；在生成视点时，为确定视点相对于前沿聚类的最佳位置，先将前沿聚类建模为一个椭圆面；其位姿由三个互相正交的单位向量描述其中n0等于该聚类中所有面元的法向量的平均值单位化后的向量；然后通过对该聚类中的所有面元的中心点做pca主成分分析，将得到一个特征向量矩阵n
pca
和对应的特征值组成的向量λ
pca
＝[λ0，λ1，λ2]，该向量中的特征值以递减的顺序储存；将n
pca
矩阵的第一列和第二列分别投影到由n0确定的平面，分别得到n1、n2；在距离该椭圆面一定距离范围内，以较垂直与该面的角度生成一系列采样点：
[0037][0038]
其中，vp是生成的采样点，c是聚类中心，ri是采样的观察距离；ω＝[1，ω1，ω2]，其中ω1∈[0，ω
max
]，通过选择不同的ω可以得到不同的采样点，ω1，ω2分别表示生成采样点时向量 n1、n2的权重；先选取与椭圆面垂直的方向采样视点，如果采样视点在已知的无障碍空间内，则选择其作为聚类对应的视点，否则继续采样，直到采样到在已知的无障碍空间内的视点；若在采样空间内无法找到满足要求的视点，则认为该聚类不可观测，直接将其从环境信息中删去；
[0039]
步骤a4.3、超级视点生成：被包含于同一个给定阈值半径的球体中的视点生成一个超级视点；超级视点将作为探索决策中机器人需要到达的目标点。
[0040]
进一步地，联合规划器的规划过程如下：
[0041]
步骤b1、主持者选择：确定主持者机器人的规则为：a.机器人通过检测其他机器人的时间戳来确定新的会议的主持者；b.当一个机器人进入一个由主持者主导的会议时，它会跟随其他老与会者保持同一个主持者；c.如果主持者离开会议，其余的与会者将根据时间戳重新决定主持者；
[0042]
步骤b2、信息整合：参与集会的机器人将所有环境信息库中的环境信息上传到主持者，信息整合分为两个方面：(1)整合无障碍空间信息；(2)整合前沿信息；对于(1)，直接将所有机器人的无障碍空间信息全部汇聚，则所有空间星凸的并集即为所有的已知无障碍
空间；对于(2)，每个机器人的前沿面元都需要被检查是否在其他机器人的空间星凸中，如果在，则删除该前沿面元；为寻找任一前沿面元对应的需要检查的星凸，对所有空间星凸对应的关键位姿点建立kd
‑ꢀ
树，查找出以该面元中心为原点，以最大传感器半径为半径的球形范围的所有关键位姿，然后检查该面元是否在这些关键位姿对应的星凸中；在信息整合过程中，若一个前沿聚类中的所有面元均被删除，则该前沿聚类和其对应的视点被删除，若一个超级视点中的所有视点均被删除，则该超级视点被删除；
[0043]
步骤b3、集中规划：将信息整合后所有的超级视点作为需要到达的目标点分配给各个机器人，并约定一个下一次集会的时间和地点，在经过被分配的超级视点后回到集会地点进行联合集会；每个超级视点只被分配给一个机器人；定义两个点之间的成本为：
[0044][0045]
其中，v
max
是机器人的最大速度；
[0046]
各个机器人位置记为rc＝{1，...，n}，各超级视点位置记为 sc＝{n+1，...，m}，定义3个0-1决策变量：当且仅当机器人k选择从i点到j点赋值为1；当且仅当节点i被机器人k 经过时赋值为1；(3)ti：当且仅当节点i被选为下一次集会地点时赋值为1；然后集中决策规划问题可表述为：
[0047][0048][0049][0050][0051][0052][0053][0054]
[0055][0056]
其中，nc＝rc∪sc，d
ij
＝tm(pi，pj)
[0057]
在求解上述问题之前，先选定下一次集会位置，即确定ti；
[0058]
集会位置确定后，决策规划问题转化为车辆路径问题，先使用启发式函数进行初始路径搜索，然后使用元启发式方法进行局部路径搜索；从机器人的位置出发，通过迭代增加最小成本的圆弧来扩展路径；用这种方法得到了一个初始解；最后，采用扩展引导局部搜索算法来寻找改进的解；至此，已确定下一次集会位置pc和每个机器人的路径ψ＝{1，...，n}；然后确定集会时间，用求解结果作为基础时间tb，设置额外时间供机器人自由探索te，机器人k的上一个未完成的集会地点和时间为t
cur
是当前时刻，那么下一次集会时刻为：
[0059][0060]
在此基础上，在任务和路径被分配给机器人后，它们分散开来独立探索。
[0061]
更进一步地，选择与所有机器人距离最大的节点作为集会位置。
[0062]
更进一步地，随着环境的探索和新的超级视点的生成，每个机器人不断通过独立规划器重新规划路径，通过各个超级视点，并准时到达下一个集合点；为了减少重复的探索，对分配给其他机器人进行探索的区域引入惩罚；对于机器人r，将其位置记为pr，将下一次集会地点记为pm，超级视点集合记为sd，那么定义节点i的惩罚ai为
[0063][0064]
其中，ψv是分配给机器人v的路径；则该规划问题为：
[0065][0066][0067][0068]
[0069][0070][0071]
其中，nd＝pr∪pm∪sd，tm是下一次集会的时刻，t
cur
是当前时刻；当机器人重规划时，在上一次解的基础上进行局部搜索，生成新的解。
[0072]
本发明的有益效果在于：本发明考虑了现实世界中的通信网络的限制，尤其在探索决策部分和已探索区域信息的高效表示部分对通信限制进行针对性设计。机器人将间歇地回到约定的地点，结束上一次任务并进行下一次任务分配，且该方法并不依赖于额外的中心基站。同时提出基于星凸的已探索区域表征方法，在保证空间表示的精确性的同时大大降低所需通信带宽。
附图说明
[0073]
图1为现有技术的方法流程图；
[0074]
图2为本发明实施例中方法的整体流程图；
[0075]
图3为本发明实施例中多机器人协同探索示例图；
[0076]
图4为本发明实施例中独立规划器的规划方法示意图；
[0077]
图5为本发明实施例中无障碍空间与前沿信息生成过程示意图；
[0078]
图6为本发明实施例中面元表方法示意图及结果图；
[0079]
图7为本发明实施例中前沿面元聚类方法及结果；
[0080]
图8为本发明实施例中前沿面元聚类视点选择方法示意图；
[0081]
图9为本发明实施例中联合规划器的规划示意图；
[0082]
图10为本发明实施例中主持者信息整合步骤示意图。
具体实施方式
[0083]
以下将结合附图对本发明作进一步的描述，需要说明的是，本实施例以本技术方案为前提，给出了详细的实施方式和具体的操作过程，但本发明的保护范围并不限于本实施例。
[0084]
本实施例提供一种通信受限条件下的多机器人协作探索方法，该方法流程如图2所示。每个机器人均有一个用于储存环境信息的环境信息库并有两个规划器，两个规划器分别为独立规划器和联合规划器。相应地，多机器人协同探索过程被分为两个阶段：独立探索阶段和联合集会阶段，分别由独立规划器和联合规划器执行。
[0085]
多机器人协同探索过程大致可描述为，机器人启动时聚集在一起，互相之间均可通信，处于联合集会阶段。在联合集会阶段，机器人将选出一个主持者，并将自己的环境信息库中的所有信息上传给主持者，主持者在进行信息整合后进行任务分配，向每个机器人分配需要访问的目标点和最终约定的进行下一次联合集会的地点和时间。至此联合集会阶段结束，各机器人将主动断开连接，进入独立探索阶段。在独立探索阶段，各机器人通过传感器收集新的环境信息，生成新的无障碍空间和前沿信息并对旧的环境信息进行更新，然后以被分配的目标点和最终需要到达的联合集会地点和时间作为限制进行独立规划，生成
新的目标序列。在约定的联合集会时间各机器人到达约定的联合集会地点进行环境数据共享，从独立探索阶段进入联合规划阶段。
[0086]
需要说明的是，联合规划器具有通用性，能够灵活处理多机器人偶然相遇的情况。图3展示了一个多机器人协同探索的示例。该示例有3个机器人，在图中用不同颜色的方块表示。如图3(a)所示，首先多机器人启动探索进入第一次联合集会阶段，经过联合规划，得到下一次联合集会的时间和地点，即在图3(a)中标注为中心带’m’字母的方块。然后，机器人分散开来独立探索。随着环境的探索和新环境信息的产生，每个机器人不断地利用独立规划器进行重新规划，生成新的目标点的同时保证准时到达指定的联合集会地点。在图3(b)中，其中两个机器人在独立探索过程中偶然相遇，约定了属于他们两个机器人的集会(标准为不同颜色的另一个m方块)，而此时两机器人具有同样的任务，那么两个机器人的联合规划器将对任务序列进行调整，只留一个机器人保留相同的未完成任务，该机器人返回图3(a)中的集会地点与余下一个机器人集会(图3(c)所示)，然后再与另一个机器人前往约定的方块集会地点(图3(b)中新生成的m方块)完成数据共享(如图3(d)所示)。
[0087]
以下对独立规划器和联合规划器分别做具体描述。
[0088]
如图4所示，所述独立规划器的规划过程为：在独立探索阶段，每当机器人距离前一次独立规划器规划的运行位置运动了足够长的距离时，说明机器人已收集到较多新环境信息，此时将触发新一次的独立规划器规划。将每次独立规划器被触发时生成的所有环境数据称为一帧数据，所述环境数据包括无障碍空间和前沿信息。无障碍空间与前沿信息生成过程如图5所示。
[0089]
步骤a1、采样障碍物点：此步骤目的是通过对障碍物点的采样生成用于生成星凸的点集s
star
。这里先说明星凸的定义：星凸是一种特殊的多面体，如果多面体s中存在一个点0，从0点到多面体s任一顶点的连线都被包含在s中，则多面体s是星凸。应用于机器人表示无障碍区域时，0点是机器人所在位置，仅记录组成星凸的面元就能描述整个星凸包含的无障碍区域。在生成星凸时，利用katz提出的隐藏点删除(hpr：hidden point removal)方法。给定一个观察点，该方法能从障碍物点云中生成该观察点的可视区域，该可视区域用星凸表示。直观地看，若要生成在当前机器人位置观察到的无障碍区域，用于生成星凸的点集s
star
需要包含两部分：当前观测到的障碍物点云 s
obs
和传感器最大观测半径上的采样点集s
free
。
[0090]
步骤a1.1、生成障碍物点云s
obs
：为了降低计算量并减少传感器噪声，在融合多帧传感器数据生成的局部占用栅格地图中采样，生成在当前机器人位置能够观察到的障碍物点云s
obs
。如图5(a)所示，以当前机器人位置pr为柱坐标系原点，在多个半径下为柱坐标系原点，在多个半径下进行不同密度采样，其中n表示采用的不同半径的数量，r
senor
为传感器最大感知半径。对于每个采样点ps，从pr为起点，向方向进行光线投影，如果在投影过程中和障碍物碰撞，记碰撞点为p
hit
，则将p
hit
加入s
obs
集合。采用多半径采样是为了对距离机器人近的障碍物进行更高密度的采样，以生成更准确的星凸。值得说明的是，该采样过程对于障碍物密度是自适应的，对于相邻的两个采样点pi，pj，将在pi，pj采样的中间角度继续递归地采样，直到如下条件满足其一：
[0091][0092]
||p
i-pj||＜d
thr
[0093]
上式的含义是：和的夹角小于角度阈值θ
thr
，或者点pi和 pj之间的距离小于距离阈值d
thr
，就停止采样的递归。
[0094]
需要说明的是，局部占用栅格地图不是为生成s
obs
专门生成的，其主要功能是用于轨迹规划，因此此处利用局部占用栅格地图并未引入额外的计算量，且局部占用栅格地图是位置动态变化的，不会被长期储存而占用内存。
[0095]
步骤a1.2、生成最大观测区域点云s
free
：在步骤a1.1进行柱坐标系采样时，当采样半径r＝r
senor
且光线投影未与障碍物发生碰撞时，将该采样点ps加入s
free
点集。
[0096]
步骤a2、星凸生成：利用步骤a1生成的点集，构成用于生成星凸的点集s
star
＝s
obs
∪s
free
，并利用hpr方法计算星凸，如图5(b) 所示。将当前帧的星凸sk和当前机器人位置rk记录到环境信息库中。记第k帧的星凸为sk，则当前机器人已探索的无障碍空间可以表示为 s1∪s2∪...∪sn，其中n是当前机器人的环境信息中记录的空间星凸数量。
[0097]
步骤a3、前沿面元更新：前沿在探索邻域的定义为：已探索的无障碍空间与未知空间的分界。由于采用星凸来表示已知无障碍区域，那么组成星凸的面元即是已知区域的边界，只要除去完全由障碍物生成的星凸面元，剩下的面元就是已知无障碍空间与未知空间的分界，称其为前沿面元。如图5(c)所示。
[0098]
步骤a3.1、当前帧前沿面元生成：对所有面元进行判断，如果该面元满足以下任意一个条件，该面元不是前沿面元：a.该面元的所有顶点均属于障碍物点云s
obs
；b.该面元的面积小于最小有效面积阈值η
area
；c.该面元的法向量z轴分量的绝对值大于z轴分量阈值z
thr
；条件a剔除了由障碍物生成的面元，条件b剔除了不能提供有效信息的微小面元，条件c剔除了在传感器视域范围外的无效面元；筛选完当前帧星凸的所有面元后将得到当前帧的前沿面元集合fk。
[0099]
步骤a3.2、前沿面元的帧间更新：假设当前帧为第k帧，此步骤将完成两方面更新：(1)删除之前帧的星凸{s1，s2，...，s
k-1
}中属于当前帧的前沿面元，更新fk；(2)删除当前帧的星凸sk中属于之前帧的前沿面元，更新{f1，f2，...，f
k-1
}。为了高效地进行上述两方面更新，本实施例对每个星凸建立一个面元表，利用该面元表能够实现快速查询一个面元是否处于一个星凸中。
[0100] (1)面元表的建立过程如下：
[0101]
1.1)建立面元表的对象：将星凸si的所有面元投影到一个栅格化的单位球上，该单位球上的每个格子记为g
i，j
；对于一个被投影的面元fi，可以得到其在单位球上的轴向边界矩形(aabb：axis-aligned bounding box)记为bi，即如图6(a)所示单位球上的染色部分网格，将处于该bi中的所有格子记为gb＝{g
i，j
|g
i，j
∈bi}；
[0102]
1.2)将gb中的格子和fi的对应关系记录到面元表中。最终栅格化的单位球上的每个格子将对应一个或多个面元。
[0103]
(2)使用面元表快速查询过程如下：
[0104]
对于待查询的面元fi和星凸sj，先将fi的中心投影到以星凸sj对应的机器人位置rj为球心的栅格单位球上，投影点落到g
i，j
格子中，在sj的面元表中查询g
i，j
对应的面元{f
′j|j∈{1，2，...，j}}，共j个。查询到的每个面元f
′j的三个顶点与rj连接形成一个三棱锥，共形成j个三棱锥，如果fi的中心在其中一个三棱锥中，就判断为fi在星凸sj中。图6(b) 展示了点是否在星凸内的查询结果，图中方形点查询后判定为在星凸中，圆点判定在星凸外部。
[0105]
利用面元表可以快速实现步骤a3.2的两方面帧间更新。在星凸第一次作为查询对象时对其建立面元表，之后再查询时就无需建立。面元表数据量极小，几乎不影响内存。
[0106]
步骤a4、前沿面元聚类：为了降低前沿面元数量对决策效率的影响，对前沿面元进行聚类，同时为使机器人更好地对前沿进行观察 (在适当距离以较垂直视角观察)，对每个前沿聚类生成一个对应的视点。由于机器人传感器有足够大的感知范围，将距离较近的视点融合为一个超级视点。
[0107]
步骤a4.1、前沿面元谱聚类：一般的聚类方法通常量化描述待聚类对象的属性，然后在属性空间进行聚类，然而对于面元，难以给出其在高维属性空间的位置，但可以简单地描述两个面元之间的相似度：此处对面元的聚类考虑3种衡量相似度的距离：a.切向距离： d
t
(fj，fj)＝||(c
i-cj)
·
nj||2；b.法向距离：dn(fi，fj)＝||(c
i-cj)
×ꢀ
nj2；c.法向角度差：δnfi，fj＝ni-nj/22。对于只能描述聚类对象在高维属性空间中的相似度，而无法描述其在高维属性空间中的位置的聚类问题，通常使用谱聚类方法。进行谱聚类需要先构建两个矩阵：度矩阵d和相似矩阵s。对于度矩阵d，本实施例将所有面元与其k 个欧氏距离最近的邻居连接起来，形成一个图，计算该图的度矩阵作为d。对于相似矩阵s，按照上述定义的3种度量相似性的标准计算 s：
[0108]
s＝(exp(-s(fi，fj)2/2σ2))
i，j＝1...j
[0109]
其中，s(fi，fj)是fi，fj两个面元之间的总距离，s(fi，fj)＝ ||[d
t
，dn，δn]||
ω
，其中ω是三种相似度距离加权时的权值，σ是预设的高斯函数的参数。计算出d和s后，即可通过谱聚类得到前沿聚类。聚类结果如图7，数字为各聚类中心，同一聚类用直线相连。
[0110]
步骤a4.2、视点选择：为了在合适的角度和距离观察前沿聚类，本实施例为每个前沿聚类生成一个对应的最佳视点。在生成视点时，为确定视点相对于前沿聚类的最佳位置，先将前沿聚类建模为一个椭圆面。其位姿可以由三个互相正交的单位向量描述如图8(a)所示。其中n0等于该聚类中所有面元的法向量的平均值单位化后的向量。然后通过对该聚类中的所有面元的中心点做pca主成分分析，将得到一个特征向量矩阵n
pca
和对应的特征值组成的向量λ
pca
＝[λ0，λ1，λ2]，该向量中的特征值以递减的顺序储存。将n
pca
矩阵的第一列和第二列分别投影到由n0确定的平面，分别得到n1、n2。在距离该椭圆面一定距离范围内，以较垂直与该面的角度生成一系列采样点：
[0111][0112]
其中，vp是生成的采样点，c是聚类中心，ri是采样的观察距离。ω＝[1，ω1，ω2]，其中ω1∈[0，ω
max
]，通过选择不同的ω可以得到如图8(b)所示弧面范
围内不同的采样点，ω1，ω2分别表示生成采样点时向量n1、n2的权重。先选取与椭圆面垂直的方向采样视点，如果采样视点在已知的无障碍空间内，则选择其作为聚类对应的视点，否则继续采样，直到采样到在已知的无障碍空间内的视点。若在采样空间内无法找到满足要求的视点，则认为该聚类不可观测，直接将其从环境信息中删去。
[0113]
步骤a4.3、超级视点生成：由于机器人传感器有足够大的感知范围，所以本实施例将距离较近的视点融合为一个超级视点。如图 5(c)所示，被包含于同一个给定阈值半径的球体中的视点生成一个超级视点。超级视点将作为探索决策中机器人需要到达的目标点。
[0114]
步骤a5、任务限制下的vrp(车辆路径问题：vehicle routingproblem)：本步骤需要联合规划器的解作为初值，在下文中更进一步描述。
[0115]
本实施例中，如图9所示，联合规划器的规划过程如下：
[0116]
步骤b1、主持者选择：多机器人一旦相遇进入联合集会阶段，首先需要确定一个负责信息整合和集中决策规划的机器人作为主持者，所有参与集会的机器人将把自己收集到的环境库中的环境信息上传给主持者，主持者完成信息整合和集中规划后将整合后的信息和规划的结果下传至所有机器人，结束联合集会阶段。本实施例设计了3 条确定主持者机器人的规则：a.每个机器人通过通信获得其他机器人进入会议的时间戳，根据时间戳确定最先进入会议的为主持者，这将在参与会议的所有机器人之间形成主持者的共识；b.当一个机器人进入一个由主持者主导的会议时，它会跟随其他老与会者保持同一个主持者；c.如果主持者离开会议，其余的与会者将根据时间戳重新决定主持者。
[0117]
步骤b2、信息整合，如图10所示：参与集会的机器人将所有环境信息库中的环境信息上传到主持者，信息整合分为两个方面：(1) 整合无障碍空间信息；(2)整合前沿信息。对于(1)，直接将所有机器人的无障碍空间信息全部汇聚，则所有空间星凸的并集即为所有的已知无障碍空间。对于(2)，每个机器人的前沿面元都需要被检查是否在其他机器人的空间星凸中，如果在，则删除该前沿面元。为寻找任一前沿面元对应的需要检查的星凸，对所有空间星凸对应的关键位姿点建立kd-树，查找出以该面元中心为原点，以最大传感器半径为半径的球形范围的所有关键位姿，然后检查该面元是否在这些关键位姿对应的星凸中。检查方法使用步骤a3.2中的面元表法进行快速判断。在信息整合过程中，若一个前沿聚类中的所有面元均被删除，则该前沿聚类和其对应的视点都要被删除，若一个超级视点中的所有视点均被删除，则该超级视点一并被删除。
[0118]
步骤b3、集中规划：该问题表述为，将信息整合后，所有的超级视点作为需要到达的目标点分配给各个机器人，并约定一个下一次集会的时间和地点，各个机器人在经过被分配的超级视点后回到集会地点进行联合集会。每个超级视点只被分配给一个机器人。该问题是一个带约束的整数优化问题。为了进行优化，定义两个点之间的成本为：
[0119][0120]
其中，v
max
是机器人的最大速度。
[0121]
各个机器人位置记为rc＝{1，...，n}，各超级视点位置记为 sc＝{n+1，...，m}，定义3个0-1决策变量：当且仅当机器人k选择从i点到j点赋值为1；当且仅当节点i被机器人k 经过时赋值为1；(3)ti：当且仅当节点i被选为下一次集会地点时赋值为
1；然后集中决策规划问题可表述为：
[0122][0123][0124][0125][0126][0127][0128][0129][0130][0131]
其中，nc＝rc∪sc，d
ij
＝tm(pi，pj)
[0132]
如果整体求解该问题会非常困难，故提出一种分层求解方法，即先选定下一次集会位置，即确定ti，然后再求解上述问题。过程为：
[0133]
(1)确定下一次集会位置：下一次集会位置有以下三种方案： (1)最远集会：选择与所有机器人距离最大的节点作为集会位置；(2) 最近集会：选择与所有机器人距离最小的节点作为集会位置；(3)最优集会：穷举所有节点为集会位置的可能，选择具有最小j的解中的集会位置。方法(3)使用穷举法列举了所有可能，是最优解。本实施例对这三种方法的性能做了评估，综合求解速度与解的最优性，选择最远集会方案。
[0134]
(2)整数规划求解：当集会位置确定后，决策规划问题转化为车辆路径问题(vrp)，先使用启发式函数进行初始路径搜索，然后使用元启发式方法进行局部路径搜索。从机器人的当前位置出发，通过迭代增加最小成本的圆弧来扩展路径。用这种方法，有效地得到了一个初始解。最后，采用扩展引导局部搜索算法来寻找改进的解。至此，已确定下一次集会位置pc和每个机器人的路径ψ＝{1，...，n}；然后确定集会时间，用求解结果作为基础时间tb，设置额外时间供机器人自由探索te，机器人v的上一个未完成的集会地点和时间为
t
cur
是当前时刻，那么下一次集会时刻为：
[0135][0136]
在此基础上，在任务和路径被分配给机器人后，它们分散开来独立探索。同时，随着环境的探索和新的超级视点的生成，每个机器人不断重新规划路径，通过一些超级视点，并准时到达下一个集合点。该步骤即是新增超级视点后的重规划方法(即上述的步骤a5)。同时，为了减少重复的探索，本实施例对分配给其他机器人进行探索的区域引入惩罚。对于机器人r，将其位置记为pr，将下一次集会地点记为pm，超级视点集合记为sd，那么定义节点i的惩罚ai为
[0137][0138]
其中，ψv是分配给机器人v的路径。则该规划问题为：
[0139][0140][0141][0142][0143][0144][0145]
其中，nd＝pr∪pm∪sd，tm是下一次集会的时刻，t
cur
是当前时刻。该任务限制下的vrp问题可以看作是有容量限制的车辆路径规划问题(cvrp：capacitated vehicle routing problem)的变种。当机器人重规划，在上一次解的基础上进行局部搜索，生成新的解。
[0146]
对于本领域的技术人员来说，可以根据以上的技术方案和构思，给出各种相应的改变和变形，而所有的这些改变和变形，都应该包括在本发明权利要求的保护范围之内。