面向未知空间探索的多机协同融合定位与建图方法

1.本发明涉及机器人技术领域，尤其涉及面向未知空间探索的多机协同融合定位与建图方法。


背景技术：

2.复杂的未知空间如地下溶洞、原始森林等一般具有屏蔽卫星定位信号、通信受限、地形地貌复杂且无法提前布置路标点等特点，因此在没有环境先验信息的情况下，面向未知空间探索的机器人如何利用自身搭载的传感器在运动过程中对周围环境信息建模并同时估计自身位姿具有重要的意义。近些年，研究人员不断地对实时定位与建图技术进行改进，但应用场景大多针对室内或室外开阔环境，应用于未知空间探索的研究并不多。
3.单一传感器在感知能力和鲁棒性方面有着不足之处，无法应对复杂环境带来的各种挑战，因此多传感器融合是目前的主流方案。而面向未知空间探索的机器人由于自身载重和功耗的限制，其机载电脑计算资源有限，并且通信受限的未知空间也不具备使用云端计算资源的条件，因此如何使多传感器融合算法在机器人平台上实时运行变得极具挑战性。而现有方案主要通过各传感器之间实现松耦合的方式来减少运算成本。该方法缺少对传感器自身特性的综合考虑，不利于提升机器人的定位精度。
4.机器人在未知空间探索时会遇到存在大量几何结构重复或纹理单一的特征稀疏的场景，由于缺乏某些方向上的约束，此类场景通常会导致一些传感器的状态估计发生退化，并对系统稳定性造成严重影响。而现有方法主要通过预测值和观测值做比较的方式来判断传感器是否发生退化，该方法没有从传感器自身观测方程去分析，存在一定的局限性。
5.受限于传感器安装位置以及周围环境障碍物遮挡等原因，机器人在未知空间探索时会存在较大的感知盲区。并且单机器人在执行长时间建图任务时，会存在建图时间长、算力需求大，难以满足实时性要求等问题。而现有的针对多机器人协同建图的研究还处于萌芽阶段，并且多以室内建图为主，无法满足未知空间探索对于多机协同建图的需求。


技术实现要素：

6.本发明的目的在于提出面向未知空间探索的多机协同融合定位与建图方法，实现机器人在未知复杂环境中的高精度定位和建图。
7.为达此目的，本发明采用以下技术方案：面向未知空间探索的多机协同融合定位与建图方法包括如下步骤：
8.轻量化多传感器融合里程计算法设计了惯性里程计、激光-惯性里程计和视觉-惯性里程计，实现对多传感器信息的融合；所述惯性里程计、所述激光-惯性里程计和所述视觉-惯性里程计分别用于估计机器人的位姿信息；
9.设计基于能观性分析的退化识别与融合决策策略算法，基于能观性分析对激光-惯性里程计以及视觉-惯性里程计是否发生退化进行判断；若存在退化，则将激光-惯性里程计获取的位姿估计信息以及视觉-惯性里程计的位姿估计信息进行退化处理，将退化处
理后的位姿估计信息和惯性里程计的位姿估计信息根据融合决策进行融合，得到总位姿估计输出；若不存在退化，则将所述惯性里程计、所述激光-惯性里程计和所述视觉-惯性里程计获取到的位姿估计信息根据融合决策进行融合，得到总位姿估计输出；
10.多机协同建图算法，各个机器人之间通过所述信息共享网络完成全局一致地图的构建和协同建图任务的分配。
11.作为一种可选的实施例，所述里程计系统融合多传感器信息的方法为：
12.所述惯性里程计通过惯性测量单元与空速计构成航位推算系统，所述航位推算系统用于为机器人提供高频的初始位置估算；并通过气压计与激光测距模块产生的高度约束消除惯性里程计在垂直方向上的位置漂移；
13.所述激光-惯性里程计接收所述惯性里程计的初始位姿估计和固态激光雷达的观测量，通过构建迭代误差状态卡尔曼滤波估计器为惯性里程计提供修正后的位姿；
14.所述视觉-惯性里程计接收所述惯性里程计的初始位姿估计、视觉传感器采集的图像数据和点云数据，采用公式(1)构建局部滑窗优化估计器：
[0015][0016]
其中，表示集合的最优估计；表示k时刻滑窗内所有关键帧的集合；分别表示第i时刻与第j时刻之间激光-惯性里程计、视觉-惯性里程计、惯性里程计的相对约束；表示滑窗中第一个状态量的先验约束；∑
lio
、∑
vio
、∑
imu
、∑0分别表示各约束的协方差矩阵。
[0017]
作为一种可选的实施例，基于能观性分析对激光-惯性里程计以及视觉-惯性里程计是否发生退化进行判断的方法为：
[0018]
根据激光约束和视觉约束，构建激光-惯性里程计和视觉-惯性里程计的信息矩阵；
[0019]
对信息矩阵进行特征值分解，通过特征值的大小表征里程计的退化程度：
[0020]
若特征值小于设定的阈值，则判断为里程计发生退化，且此时特征值对应的特征向量被视为退化方向向量；若特征值大于等于设定的阈值，则判断里程计无退化。
[0021]
作为一种可选的实施例，退化处理包括如下步骤：
[0022]
通过特征值可以确定位姿估计问题的非线性优化解在退化方向的方向向量和非退化方向的方向向量；
[0023]
将位姿估计的非线性优化解分解成真实解在非退化方向的投影和预测值在退化方向的投影，并考虑在位姿估计问题的部分子空间中添加约束，在退化方向上不执行迭代优化，并只输出非退化方向的解；
[0024]
通过丢弃最优解在退化方向的分量，只把非退化方向解的增量叠加到状态量中，得到退化处理后的位姿估计信息。
[0025]
作为一种可选的实施例，得到总位姿估计输出包括如下步骤：
[0026]
向融合决策表输入激光-惯性子系统和视觉-惯性子系统的退化状态信息，基于退化状态信息得到融合决策表的决策输出；
[0027]
通过融合决策表的决策输出和各里程计子系统的观测数据，决策得到里程计系统的总位姿估计输出，并将其输入到后端优化系统中；
[0028]
后端优化系统通过位姿图构建各时刻状态量之间的关系，并建模成批量递归平滑问题，具体如公式(2)所示：
[0029][0030]
其中，表示集合的最优估计；表示截止到k时刻所有关键帧的集合；分别表示第i时刻与第j时刻之间里程计和回环的相对约束；∑
odom
、∑
loop
分别表示各约束的协方差矩阵。
[0031]
作为一种可选的实施例，各个机器人之间通过所述信息共享网络完成全局一致地图的构建和协同建图任务的分配包括如下步骤：
[0032]
基于通信质量与群并视野约束的聚类算法，在规范约束下最大化由无人机通信质量与群作业视野集组成的耦合性能，获得一组优解，以划分无人机群信息共享区域，每个信息共享区域内基于计算资源最优的原则选择网络主控单元；
[0033]
通过寻找机器人地图之间的重叠区域进行信息关联，确定机器人间的坐标变换关系，并以此为依据构建全局一致地图；其中，基于关键帧的词袋模型检测重叠区域；词袋模型利用两幅图像之间的相似性，以判断出这两幅图像是否来自同一个场景；若两个机器人经过了同一个场景，则证明这两个机器人的地图之间很大可能存在重叠区域；
[0034]
主控单元接收到机器人发来的相似关键帧和局部地图数据后，先对相似关键帧和其自身的关键帧进行特征匹配并利用对极约束关系得到初始位姿变换矩阵，将其提供给正态分布变换算法作为初始值，再使用正态分布变换算法迭代计算出精确的位姿变换矩阵，并根据最终的位姿变换矩阵将机器人发来的局部地图与主控单元维护的全局地图进行融合；
[0035]
正态分布变换算法主要是通过利用点分布的概率密度函数进行配准，并通过最优化算法确定点云之间的最佳匹配，包括如下步骤：
[0036]
step1：网格化，将主控单元维护的全局点云地图划分为均匀大小的立体单元格；
[0037]
step2：计算多维正态分布参数，根据公式(3)、(4)分别求出每个单元格内空间点的均值向量q和协方差矩阵σ：
[0038][0039][0040]
其中，n是每个单元格中点的总数，xk代表单元格中的一点；
[0041]
step3：坐标变换，假设位姿变换矩阵为t，初值由对极约束关系提供，并且有：
其中r是3
×
3的旋转矩阵，t是3
×
1的平移矩阵。利用位姿变换矩阵t将局部点云地图中的每个点变换到全局地图上，变换后的点用yi(i＝1，
…
，m)来表示，m为局部点云地图中所有点的总数；
[0042]
step4：计算概率密度，根据变换点yi所在单元格的正态分布参数计算每个变换点的概率密度，其中，计算公式(5)为：
[0043][0044]
其中detσ表示协方差矩阵∑的行列式；
[0045]
step5：创建目标函数，对于每个点的概率密度，利用公式(6)构建目标函数评估位姿变换矩阵t：
[0046][0047]
step6：优化目标函数，利用牛顿优化方法求解目标函数的最小值，从而得到最佳的位姿变换矩阵t；
[0048]
基于快速随机搜索树算法去寻找已经建图的区域与未知区域的边界点，rrt算法包括：
[0049]
step1：调用init函数初始化树t；
[0050]
step2：调用randomsample函数在状态空间x内随机采样一个点x
rand
；
[0051]
step3：调用nearest函数找到在当前树t上最接近x
rand
的点x
near
；
[0052]
step4：调用steer函数，以x
near
为起点，ε为步长，迭代地检查x
near
与x
rand
之间的点是否为无碰撞的自由状态，如果不是则停止迭代，此时距离x
near
最远的可通行点即为新的节点x
new
；
[0053]
step5：将新节点x
new
以及x
near
与x
new
之间的新边添加到树t，完成本次树的扩展并进入到下一次迭代；
[0054]
step6：迭代结束，生成新的树t；
[0055]
主控单元对全局和局部检测器得到的大量边界点利用均值漂移聚类算法进行筛选得到需要的任务目标点，均值漂移聚类算法的包括步骤：
[0056]
step1：在未被标记的边界点中随机选择一个点作为起始中心点x
center
；
[0057]
step2：找出以x
center
为中心，半径为r的球形区域内的所有边界点并对这些边界点进行标记；
[0058]
step3：计算从x
center
开始到球形区域中每个边界点的向量，将这些向量相加，得到漂移向量x
shift
；
[0059]
step4：根据x
center
＝x
center
+x
shift
移动中心点，即x
center
沿着x
shift
的方向移动，移动距离是||x
shift
||；
[0060]
step5：重复step2到step4，直到||x
shift
||很小，此时说明已经迭代到收敛，记录当前的中心点x
center
，在迭代过程中遇到的所有边界点都将会被标记；
[0061]
step6：如果收敛时当前中心点x
center
与其它已经存在的中心点的距离小于阈值，那么保留球形区域内存在更多边界点的中心点；否则，把x
center
作为新的中心点；
[0062]
step7：重复之前的步骤，一直到所有的点都被标记访问；
[0063]
主控单元将利用拍卖机制将任务目标点分配给共享网络中的机器人。
[0064]
本发明还提供了面向未知空间探索的多机协同融合定位与建图设备，包括存储器，处理器及存储在所述存储器上并可在所述处理器上运行的面向未知空间探索的多机协同融合定位与建图方法程序，所述处理器执行所述面向未知空间探索的多机协同融合定位与建图方法程序时实现上述的面向未知空间探索的多机协同融合定位与建图方法的步骤。
[0065]
一种计算机可读存储介质，所述计算机可读存储介质上存储有面向未知空间探索的多机协同融合定位与建图方法程序，所述面向未知空间探索的多机协同融合定位与建图方法程序被处理器执行时实现上述的面向未知空间探索的多机协同融合定位与建图方法的步骤。
[0066]
与现有技术相比，本发明实施例具有以下有益效果：
[0067]
本发明设计的轻量化多传感器融合里程计算法可以实现惯性里程计、激光-惯性里程计以及视觉-惯性里程计的紧耦合状态估计。与现有方法相比，可以有效提高机器人对位姿估计的准确性以及应对未知空间中各种挑战的鲁棒性并实现多传感器融合算法在机载电脑上的实时运行。
[0068]
本发明设计的基于能观性分析的退化识别与融合决策算法可以判断出里程计是否发生退化，并且能够基于里程计的退化状态完成里程计信息的融合。与现有方法相比，能够改善特征稀疏场景中机器人的位姿估计结果，并实现机器人的多源信息融合，为机器人在复杂未知空间中的自主导航、实时避障和地图构建提供了可靠保障。
[0069]
本发明设计的多机协同建图算法可以实现各机器人的地图信息共享以及计算资源的最优化分配，并基于信息共享网络完成全局一致地图的构建和协同建图任务的分配。与现有方法相比，本专利设计了一种面向未知空间探索的多机协同建图算法，提高了机器人构建未知空间地图的效率。
附图说明
[0070]
图1是本发明其中一个实施例的算法框架示意图；
[0071]
图2是本发明其中一个实施例的传感器信息融合流程示意图；
[0072]
图3是本发明其中一个实施例的迭代误差卡尔曼滤波器示意图；
[0073]
图4是本发明其中一个实施例的里程计融合流程示意图；
[0074]
图5是本发明其中一个实施例基于能观性分析退化判断示意图；
[0075]
图6是本发明其中一个实施例的信息融合框架示意图；
[0076]
图7是本发明其中一个实施例的重叠区域检测流程示意图；
[0077]
图8是本发明其中一个实施例的构建全局一致地图的流程示意图；
[0078]
图9是本发明其中一个实施例的协同建图系统框架示意图；
[0079]
图10是本发明其中一个实施例的拍卖机制流程示意图。
具体实施方式
[0080]
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0081]
下面结合图1至图10，描述本发明实施例的面向未知空间探索的多机协同融合定位与建图方法及系统。
[0082]
针对复杂的未知空间环境，亟需提高机器人的自主定位和环境感知能力，如图1所示，本发明通过设计轻量化多传感器融合里程计算法、基于能观性分析的退化识别与融合决策策略算法、多机协同建图算法去提高机器人在未知复杂空间环境探索时定位与建图的鲁棒性和平滑性。
[0083]
作为一种可选的实施例，设计轻量化多传感器融合里程计算法的实施例如图2所示，轻量化多传感器融合里程计算法采用冗余的设计，分别设计了惯性里程计、激光-惯性里程计、视觉-惯性里程计三个子系统，实现对多传感器信息的融合，增强机器人应对未知空间各种挑战的鲁棒性。
[0084]
首先，惯性里程计通过惯性测量单元与空速计构成航位推算系统，为机器人提供高频的初始位姿估计。同时气压计和激光测距模块产生的高度约束能有效消除惯性里程计垂直方向上的位姿漂移。并且由于惯性里程计只是对机器人自身状态的估计，所以即使视觉-惯性里程计和激光-惯性里程计出现退化或失效，也能在短时间内保证机器人位姿估计的准确性。
[0085]
其次，激光-惯性里程计接收惯性里程计的初始位姿估计和固态激光雷达观测量，如图3所示，通过构建迭代误差状态卡尔曼滤波估计器为惯性里程计提供修正后的位姿，消除惯性里程计的累积误差。
[0086]
最后，视觉-惯性里程计接收惯性里程计的初始位姿估计、视觉传感器图像数据和点云数据，采用如下公式(1)构建局部滑窗优化估计器：
[0087][0088]
其中，表示集合的最优估计；表示k时刻滑窗内所有关键帧的集合；分别表示第时刻与第时刻之间激光-惯性里程计、视觉-惯性里程计、惯性里程计的相对约束；表示滑窗中第一个状态量的先验约束；∑
lio
、∑
vio
、∑
imu
、∑0分别表示各约束的协方差矩阵。此融合方法通过使用点云数据来辅助视觉进行深度估计来克服由于深度估计不准，影响位姿估计的问题。
[0089]
进一步，设计基于能观性分析的退化识别与融合决策算法的实施例如下：
[0090]
通过惯性里程计、视觉-惯性里程计、激光-惯性里程计可以分别得到机器人的位姿估计，接下来需要对三个里程计的位姿估计信息进行融合。
[0091]
对于视觉-惯性里程计和激光-惯性里程计而言，视觉传感器需要纹理信息决定特征点，而激光传感器则需要几何结构信息来决定特征点。而在如长直走廊、长隧道、单侧墙、开阔公路、桥梁等纹理单一或几何结构重复的特征稀疏场景中，视觉传感器和激光传感器
都将无法提取到足够的特征点。而缺乏足够的特征点将会导致视觉-惯性里程计和激光-惯性里程计的状态估计器发生退化，从而影响位姿估计的准确性和稳定性。针对视觉-惯性里程计和激光-惯性里程计可能发生退化的问题，本发明基于能观性分析对里程计是否发生退化进行判断分析，并基于里程计的退化状态对多个里程计子系统的位姿估计信息进行融合。
[0092]
如图4所示，由于惯性里程计不会受到结构稀疏场景的影响，因此直接将惯性里程计的位姿估计输入到融合决策系统中。而激光-惯性里程计和视觉-惯性里程计则需要先对其是否发生退化进行判断，若没有发生退化的话，则和惯性里程计一样直接将位姿估计输入到融合决策系统中，而发生退化的里程计的位姿估计信息将会在处理后再输入到融合决策系统中。
[0093]
如图5所示，本发明基于能观性分析去判断特征稀疏场景下视觉-惯性里程计和激光-惯性里程计是否发生退化。
[0094]
首先分别根据激光约束和视觉约束，构建激光-惯性里程计和视觉-惯性里程计的信息矩阵，进一步地将能观性分析问题转换成分析能观测子空间的问题。然后对信息矩阵进行特征值分解，通过特征值的大小来表征里程计的退化程度，当特征值小于设定的阈值时认为里程计发生了退化，并且此时特征值对应的特征向量被视为退化方向向量。
[0095]
当判断到激光-惯性里程计和/或视觉-惯性里程计发生退化后，需要对其位姿估计信息进行处理。对于里程计系统而言，位姿估计问题最终可以转化为一个非线性优化问题。而里程计退化与否和位姿估计问题是否存在解没有必然联系。因此即使里程计出现退化，位姿估计问题也是有可能存在解的。而通过特征值可以确定位姿估计问题的非线性优化解在退化方向的方向向量和非退化方向的方向向量。因此本发明将位姿估计的非线性优化解分解成真实解在非退化方向的投影和预测值在退化方向的投影，并考虑在位姿估计问题的部分子空间中添加约束，在退化方向上不执行迭代优化，并只输出非退化方向的解。最后，通过丢弃最优解在退化方向的分量，只把非退化方向解的增量叠加到状态量中，从而防止最优解在零空间中移动，保证最优解的精确性。最后，将处理好的位姿估计信息输入到融合决策系统中。倘若里程计在所有方向上均发生了退化，则直接丢弃该位姿估计信息，不输入到融合决策系统中。
[0096]
融合决策系统接收三个里程计的位姿估计信息，针对机器人多个里程计子系统的信息融合问题，本发明设计基于退化状态的决策驱动融合方法。首先向决策表输入激光-惯性子系统和视觉-惯性子系统的退化状态信息，然后基于里程计子系统的退化状态得到决策表的决策输出。显然当里程计没有发生退化或者退化程度相对较低时，其在决策输出中的占比越高。然后通过融合决策表的决策数据和各里程计子系统的观测数据，决策得到轻量级多传感器融合里程计的总位姿估计输出，并将其输入到后端优化系统中。
[0097]
后端优化系统通过位姿图构建各时刻状态量之间的关系，并建模成批量递归平滑问题，具体如公式(2)所示：
[0098]
[0099]
其中，表示集合的最优估计；表示截止到k时刻所有关键帧的集合；分别表示第i时刻与第j时刻之间里程计和回环的相对约束；∑
odom
、∑
loop
分别表示各约束的协方差矩阵。最后，通过求解公式(2)得到具有全局一致性的轨迹，进而得到稠密的三维点云地图。
[0100]
如图6所示，各个里程计子系统以紧耦合的方式融合多传感数据进行状态估计，然后再以松耦合的方式对各个里程计的位姿估计信息进行融合决策。这种紧耦合和松耦合相结合的融合方式，在提高定位精度的同时也保证了系统的稳定性。
[0101]
进一步，设计多机协同建图算法的实施例如下：
[0102]
首先，针对未知空间网络复杂且通讯受限的问题，本发明基于通信质量与群并视野约束的聚类算法，在规范约束下最大化由无人机通信质量与群作业视野集组成的耦合性能，获得一组较优的解，实现无人机群信息共享区域的高效划分，在每个信息共享区域内基于计算资源最优的原则选择网络主控单元。
[0103]
其次，针对机器人在未知空间交会几率较低以及无法提前布置路标的问题，本发明基于地图重叠区域来构建全局一致地图。通过寻找机器人地图之间的重叠区域进行信息关联，从而确定机器人间的坐标变换关系，并以此为依据构建全局一致地图。重叠区域检测采用基于关键帧的词袋模型来完成。词袋模型度量的是两幅图像之间的相似性，从而可以判断出这两幅图像是否来自同一个场景。因此通过词袋模型去比较两个机器人之间的关键帧可以判断出两个机器人是否经过了同一个场景，若两个机器人经过了同一个场景，则证明它们的地图之间很大可能存在重叠区域。
[0104]
同时，如图7所示，为了避免不断传输三维地图数据影响系统通信的实时性，主控单元会先将其关键帧对应的全局描述子通过在通信网络广播的方式发送给网络中的每个机器人。而网络中的机器人在接收到主控单元发送的全局描述子后，会通过词袋模型去判断是否和主控单元存在地图重叠区域。机器人只有和主控单元存在重叠区域时才进行数据传输，将对应的相似关键帧和局部地图数据发送给主控单元。通过这种机制可以有效避免大量冗余数据的传输，提高传输效率。
[0105]
如图8所示，主控单元接收到机器人发来的相似关键帧和局部地图数据后，先对相似关键帧和其自身的关键帧进行特征匹配并利用对极约束关系得到初始位姿变换矩阵，将其提供给正态分布变换算法作为初始值，再使用正态分布变换算法迭代计算出精确的位姿变换矩阵，并根据最终的位姿变换矩阵将机器人发来的局部地图与主控单元维护的全局地图进行融合。
[0106]
正态分布变换(normal distribution transformation,ndt)算法主要是通过利用点分布的概率密度函数进行配准，并通过最优化算法确定点云之间的最佳匹配。具体步骤如下：
[0107]
step1:网格化，将主控单元维护的全局点云地图划分为均匀大小的立体单元格。
[0108]
step2：计算多维正态分布参数，根据公式(3)、(4)分别求出每个单元格内空间点的均值向量q和协方差矩阵∑：
[0109][0110][0111]
其中，n是每个单元格中点的总数，xk代表单元格中的一点。
[0112]
step3：坐标变换，假设位姿变换矩阵为t，初值由对极约束关系提供，并且有：其中r是3
×
3的旋转矩阵，t是3
×
1的平移矩阵。利用位姿变换矩阵t将局部点云地图中的每个点变换到全局地图上，变换后的点用yi(i＝1，
…
，m)来表示，m为局部点云地图中所有点的总数。
[0113]
step4：计算概率密度，根据变换点yi所在单元格的正态分布参数计算每个变换点的概率密度：
[0114][0115]
其中detσ表示协方差矩阵∑的行列式；
[0116]
step5：创建目标函数，对于每个点的概率密度，利用公式(6)构建目标函数评估位姿变换矩阵t：
[0117][0118]
step6：优化目标函数，利用牛顿优化方法求解目标函数的最小值，从而得到最佳的位姿变换矩阵t。
[0119]
基于位姿图优化算法进行全局优化来获得更高精度的全局一致地图与机器人之间的位姿约束关系。
[0120]
最后，针对多个机器人在探索未知空间时如何协同建图的问题，基于快速随机搜索树(rapidly-exploring random trees，rrt)算法去寻找已经建图的区域与未知区域的边界点。rrt算法包括：
[0121]
step1：调用init函数初始化树t；
[0122]
step2：调用randomsample函数在状态空间x内随机采样一个点x
rand
；
[0123]
step3：调用nearest函数找到在当前树t上最接近x
rand
的点x
near
；
[0124]
step4：调用steer函数，以x
near
为起点，ε为步长，迭代地检查x
near
与x
rand
之间的点是否为无碰撞的自由状态，如果不是则停止迭代，此时距离x
near
最远的可通行点即为新的节点x
new
；
[0125]
step5：将新节点x
new
以及x
near
与x
new
之间的新边添加到树t，完成本次树的扩展并进入到下一次迭代；
[0126]
step6：迭代结束，生成新的树t。
[0127]
如图9所示，本发明通过设置全局边界检测器与局部边界检测器来兼顾边界点搜索过程的高效与完整性。网络中的每个机器人都会维护一个局部检测器，通过rrt算法对周围的环境进行随机探测，当检测到边界点后机器人将会删除原来的树并在当前位置重新开始扩展树，从而加快边界点检测的速度。而全局边界检测器由主控单元维护，与局部检测器不同，全局检测器中的树根节点不会更新，在任务期间只会生成一个树，全局检测器可以检测到被局部检测器忽略的边界点，并在局部检测器的rrt算法陷入生长“陷阱”时提供更有价值的边界点。主控单元对全局和局部检测器得到的大量边界点利用均值漂移聚类算法进行筛选得到需要的任务目标点。
[0128]
均值漂移聚类算法的具体步骤如下所示：
[0129]
stepl：在未被标记的边界点中随机选择一个点作为起始中心点x
center
。
[0130]
step2：找出以x
center
为中心，半径为r的球形区域内的所有边界点并对这些边界点进行标记。
[0131]
step3：计算从x
center
开始到球形区域中每个边界点的向量，将这些向量相加，得到漂移向量x
shift
。
[0132]
step4：根据x
center
＝x
center
+x
shift
移动中心点，即x
center
沿着x
shift
的方向移动，移动距离是||x
shift
||。
[0133]
step5：重复step2到step4，直到||x
shift
||很小，此时说明已经迭代到收敛，记录当前的中心点x
center
，在迭代过程中遇到的所有边界点都将会被标记。
[0134]
step6：如果收敛时当前中心点x
center
与其它已经存在的中心点的距离小于阈值，那么保留球形区域内存在更多边界点的中心点。否则，把x
center
作为新的中心点。
[0135]
step7：重复之前的步骤，一直到所有的点都被标记访问。
[0136]
经过均值漂移聚类后，原来数量庞大的边界点就会被聚类到一些中心点上，保留中心点并删除其余的边界点，此时这些中心点就是机器人的任务目标点。
[0137]
在得到任务目标点之后，主控单元将利用如图10所示的拍卖机制将任务目标点分配给共享网络中的机器人。拍卖算法通过模拟市场拍卖过程，由主控单元扮演拍卖者，而信息共享网络中空闲的机器人则作为竞拍者。拍卖者基于系统利益最大化的原则在所有竞拍者中选择回报最高的作为中标者。其中，本专利以探索路径、视场重叠以及“孤岛”作为代价准则，通过衡量每个机器人到达任务节点的路径长短、重复覆盖率的高低以及将会形成“孤岛”的数量来对任务节点进行分配，实现多机器人协同建图任务的最优化分配。
[0138]
本发明还公开了面向未知空间探索的多机协同融合定位与建图设备，包括存储器，处理器及存储在所述存储器上并可在所述处理器上运行的面向未知空间探索的多机协同融合定位与建图方法程序，所述处理器执行所述面向未知空间探索的多机协同融合定位与建图方法程序时实现上述的面向未知空间探索的多机协同融合定位与建图方法的步骤。
[0139]
包括至少一个处理器、存储器。处理器是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器可以是通用处理器、数字信号处理器(dsp)、专用集成电路(asic)、现场可编程门阵列(fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。软件模块可以位于随
机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步骤。
[0140]
可以理解，本发明实施例中的存储器可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(read only memory，rom)、可编程只读存储器(programmable rom，prom)、可擦除可编程只读存储器(erasable prom，eprom)、电可擦除可编程只读存储器(electrically eprom，eeprom)或闪存。易失性存储器可以是随机存取存储器(random access memory，ram)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的ram可用，例如静态随机存取存储器(static ram，sram)、动态随机存取存储器(dynamic ram，dram)、同步动态随机存取存储器(synchronous dram，sdram)、双倍数据速率同步动态随机存取存储器(double datarate sdram，ddrsdram)、增强型同步动态随机存取存储器(enhanced sdram，esdram)、同步连接动态随机存取存储器(synch link dram，sldram)和直接内存总线随机存取存储器(direct rambus ram，drram)。本发明实施例描述的系统和方法的存储器旨在包括但不限于这些和任意其它适合类型的存储器。
[0141]
本发明还公开了一种计算机可读存储介质，所述计算机可读存储介质上存储有面向未知空间探索的多机协同融合定位与建图方法程序，所述面向未知空间探索的多机协同融合定位与建图方法程序被处理器执行时实现上述的面向未知空间探索的多机协同融合定位与建图方法的步骤。
[0142]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0143]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0144]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0145]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0146]
根据本发明实施例的面向未知空间探索的多机协同融合定位与建图方法的其他
构成等以及操作对于本领域普通技术人员而言都是已知的，这里不再详细描述。
[0147]
在本说明书的描述中，参考术语“实施例”、“示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0148]
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。