CN113074737B - 一种基于场景辨识的多机器人分布式协作视觉建图方法 - Google Patents

Abstract

本发明属于机器人技术领域，一种基于场景辨识的多机器人分布式协作视觉建图方法，包括以下步骤：(1)每个机器人进行单机器人视觉建图，(2)相似场景辨识，(3)相似场景投票，(4)点云配准，(5)得到当前时刻的全局点云地图。本发明利用的是视觉传感器，与激光雷达传感器相比，具有成本低的优点。另外，机器人与邻居机器人通信的内容和通信方式实现了，在三个及三个以上机器人时，不需要保证每个机器人的运动区域都有重叠区域，只需要保证机器人与邻居机器人的运动区域有重叠的前提，就可以让每个机器人都得到全局点云地图。

Description

一种基于场景辨识的多机器人分布式协作视觉建图方法

技术领域

本发明涉及一种基于场景辨识的多机器人分布式协作视觉建图方法，属于机器人技术领域。

背景技术

机器人设备已经融入到生产生活的各个方面，无论是靠双腿行走的仿人型机器人，还是靠轮式结构运动的移动机器人，其目的都是在日常生产生活或者危险的环境中自主的工作。在未知大环境中进行自主工作之前，需要构建有效的环境地图。但对于未知大规模环境或较复杂的作业场景，仅仅凭单个机器人去完成建图的任务显得力不从心，主要表现在效率不高。因此，多个机器人来协同完成建图任务的想法被提出来。

目前，为解决单机器人建图效率低的问题，在中国发明专利说明书CNCN107491071B中公开了一种基于激光雷达的多机器人协作建图方法，方法是将雷达采集到的点云数据提取点线特征，然后使用点云匹配模块对特征信息采用轨迹到轨迹以及帧与帧的方式进行匹配，通过相似性判断是否通过了相似的地方，若匹配成功，则利用更加细致的点云匹配得到之间的变换矩阵，进而融合局部地图得到全局地图。但是，因为雷达成本高的缘故，该发明专利在解决效率问题的同时也带来了成本高的问题。在中国发明专利说明书CN111369640A中公开了一种基于激光雷达的多机器人协作建图方法，方法是选一个机器人的子地图作为初始子地图上传给主机后，由主机将该地图下发至其他机器人，其他机器人在该初始子地图中运动，定位得到自身在该初始子地图中的初始位姿，然后再各自进行子地图的构建，构建完成后各机器人将子地图发送给主机，在主机上由初始位姿将各自子地图拼接到初始子地图中，该方法采用的集中式方法，主机将地图拼接完成后才能将全局地图下发给各机器人，因此，无法实时得到全局地图。

发明内容

为了克服现有视觉建图技术中存在的建图效率不高和集中式带来的无法实时得到全局地图的问题，本发明目的是提供一种基于场景辨识的多机器人分布式协作视觉建图方法，该建图方法采用视觉传感器和多机器人协作的方式解决了单机器人效率低和雷达传感器成本高的问题，其次，采用的分布式方法解决了集中式导致的无法实时拼接的问题，最后，设计的场景辨识方法实现了机器人不相遇也可进行地图拼接的目的。

为了实现上述发明目的，解决现有技术存在的问题，本发明采取的技术方案是：一种基于场景辨识的多机器人分布式协作视觉建图方法，包括以下步骤：

步骤1、每个机器人进行单机器人视觉建图，每个机器人在视觉建图过程中保存自己路过的场景信息，包括该机器人的标号、关键帧类中的关键帧序号、关键帧描述子、关键帧的点云信息以及关键帧的连接帧的序号和权重，每保存100至300帧就将存储文件通过主机通信发送给邻居机器人，实现将自己路过场景通知给邻居机器人的功能；

步骤2、相似场景辨识，机器人从邻居机器人收到存储场景信息的文件后，将文件中每帧的所有二值描述子即所有单词转换成词袋向量，得到的邻居机器人的每个关键帧信息作为相似场景辨识的候选帧按顺序存储到向量容器中，机器人在对当前帧进行闭环检测的同时，将当前帧与候选帧中的场景进行相似场景辨识，具体包括以下子步骤：

(a)将当前帧与当前帧的连接帧一一进行词袋相似度计算，得到最大的词袋相似度乘上一个0.3～0.8的系数作为词袋相似度阈值；

(b)找出候选帧中和当前帧具有相同单词的所有候选帧，同时统计候选帧中与当前帧具有相同单词数最多的候选帧，两帧得到最大相同单词的数目乘上一个0.3～0.8的系数作为最小相同单词数目阈值；

(c)遍历候选帧，挑选出相同单词数大于阈值且词袋相似度大于阈值的候选帧作为一级候选帧，连同词袋相似度一起保存；

(d)遍历一级候选帧，每个一级候选帧的前几连接帧组成一个连接帧组，每组中的每帧与当前帧的词袋相似度求和作为该组的词袋相似度和，遍历结束后，最大的词袋相似度和乘上一个0.3～0.8的系数作为最小词袋相似度和的阈值，从所有连接帧组中挑选出词袋相似度和大于阈值的连接帧组，每组中与当前帧词袋相似度最大的帧作为二级候选帧，即相似候选帧，由此得到两个相似场景；

利用词袋检测算法进行词袋相似度计算，该算法使用快速旋转描述子特征点，即结合了快速特征点和二值描述子的特征点，图像的特征点简单的理解为图像中显著的点，即轮廓点，暗区域中的亮点，亮区域中的暗点，描述子是用来描述某个点周围的特征，将一幅图像中所有特征点的描述子空间离散化，即将该图像上的描述子转换为单词表示，通过式(1)进行描述：

A＝1·w₁+0·w₂+…+1·w_n＝[w₁，w₂，…，w_n][1,0，…，1]^T (1)

式中，w₁…w_n表示n个不同类型的描述子，1表示图像中包含该类型描述子，0表示图像中不包含该类型描述子，T表示向量转置，A表示图像中包含的所有描述子，由于视觉单词在向量中的位置是固定的，所以一幅图像采用包含的描述子类型转换为词袋向量表示，通过式(2)进行描述：

v_A＝[1，0，…，1]^T (2)

式中，v_A表示一副图像的词袋向量，然后，对比词袋向量，计算两幅图像的相似度，通过式(3)进行描述：

式中，v_B表示另一幅图像的词袋向量，||表示1-范数，score(v_A，v_B)表示两幅图像的相似度得分；

步骤3、相似场景投票，机器人的当前帧向满足投票条件的候选相似场景帧投票，当某个候选相似场景帧的票数达到投票阈值，则认为该帧与给其投票的帧为相似场景，将相似场景的点云信息以及该机器人的子点云地图保存到点云文件中，发送回该相似场景帧存在的邻居机器人；当前帧向由场景辨识得到的相似候选帧进行投票，当某相似候选帧满足投票条件时，当前帧向该相似候选帧投一票；投票条件为：若某相似候选帧的最优连接帧也是当前帧最优连接帧的相似候选帧则投一票，当某帧x票数大于阈值，则认为当前机器人给x帧投票的帧是x帧的相似场景；

步骤4、点云配准，当有相似场景的机器人收到从邻居机器人发来的点云文件后，对点云文件中两个相似场景对应的点云信息进行配准得到点云间的变换矩阵，采样一致性初始配准算法进行粗点云配准，正太分布变换算法进行精点云配准，以此来计算相同场景点云之间的变换矩阵，改善了只用一种点云配准算法结果误差大的问题，具体包括以下子步骤：

(a)采用采样一致性初始配准算法进行粗点云配准，从步骤1中保存的关键帧点云中取出第x帧的点云，将其与给x帧投票的关键帧点云信息分别进行配准，点云信息作为输入，得到的粗变换矩阵作为精匹配的初始变换矩阵；从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的快速点特征直方图特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d，在目标点云Q中查找与点云P中采样点具有相似快速点特征直方图特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点，计算对应点之间刚体变换矩阵，通过求解对应点变换后的距离误差和函数判断配准变换的性能，此处的距离误差和函数使用Huber罚函数表示，记为：

其中

式(5)中，a为一预先给定值，l_k为第k组对应点变换之后的距离差，上述配准的最终目的是在所有变换中找到一组最优的变换，使得误差函数的值最小，此时的变换即为最终的配准变换矩阵；

(b)采用正太分布变换算法进行精点云配准，采样一致性初始配准算法得到的粗变换矩阵作为正太分布变换算法的初始矩阵，x帧的点云和投票帧的点云作为输入得到两片点云之间的精变换矩阵，正态分布变换算法是一种基于概率分布的点云配准算法，其算法原理是将原始点云空间B按一定尺度分成i个立体单元格{b₁,…,b_i}，计算每个单元格中点的概率密度函数，若单元格内的点数量少于三个，则不计算该单元格，概率密度函数的均值μ和协方差∑按正太分布进行计算，通过式(6)、(7)进行描述：

式中，m表示某个单元格b_i中点的数量，y_k表示单元格b_i中第k个点的坐标向量，对待配准点云空间A中的点x_k的坐标按采样一致性初始配准算法得到的初始变换矩阵N进行仿射变换得到x′_k，计算变换后的待配准点云空间中的每个点落在原始点云空间相应位置的单元格内的概率分布函数，通过式(8)进行描述：

式中，D表示向量维数，x′_k表示仿射变换后的点坐标，∑表示原始点云相应

位置单元格内点的概率密度函数的协方差，μ表示原始点云相应位置单元格内点的概率密度函数的均值，构造目标函数通过式(9)进行描述，

式中，T(N，x_k)表示将待配准点云空间A中的点x_k按变换矩阵N进行变换,n表示点云空间A中点的总数量，迭代得到使目标函数最大的变换矩阵N，若给x帧投票的帧数不只1个，会得到多个配准结果，利用正太分布变换算法的误差计算函数对多个配准结果进行误差计算，取误差最小的作为最终的变换矩阵；

步骤5、得到当前时刻的全局点云地图，通过配准算法得到的变换矩阵将点云文件中子点云地图旋转添加到该机器人的子点云地图中，通过八叉树地图包将点云地图转换为占据栅格地图，得到当前时刻的全局点云地图。

本发明有益效果是：一种基于场景辨识的多机器人分布式协作视觉建图方法，包括以下步骤：(1)每个机器人进行单机器人视觉建图，(2)相似场景辨识，(3)相似场景投票，(4)点云配准，(5)得到当前时刻的全局点云地图。与已有技术相比，本发明采用的是视觉传感器，与激光雷达传感器相比，具有成本低的优点。另外，机器人与邻居机器人通信的内容和通信方式实现了，在三个及三个以上机器人时，不需要保证每个机器人的运动区域都有重叠区域，只需要保证机器人与邻居机器人的运动区域有重叠的前提，就可以让每个机器人都得到全局地图。

附图说明

图1是本发明方法步骤流程图。

图2是相似场景投票原理示意图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，一种基于场景辨识的多机器人分布式协作视觉建图方法，包括以下步骤：

步骤1、每个机器人进行单机器人视觉建图，每个机器人在视觉建图过程中保存自己路过的场景信息，包括该机器人的标号、关键帧类中的关键帧序号、关键帧描述子、关键帧的点云信息以及关键帧的连接帧的序号和权重，每保存100至300帧就将存储文件通过主机通信发送给邻居机器人，实现将自己路过场景通知给邻居机器人的功能；

步骤2、相似场景辨识，机器人从邻居机器人收到存储场景信息的文件后，将文件中每帧的所有二值描述子即所有单词转换成词袋向量，得到的邻居机器人的每个关键帧信息作为相似场景辨识的候选帧按顺序存储到向量容器中，机器人在对当前帧进行闭环检测的同时，将当前帧与候选帧中的场景进行相似场景辨识，具体包括以下子步骤：

(a)将当前帧与当前帧的连接帧一一进行词袋相似度计算，得到最大的词袋相似度乘上一个0.3～0.8的系数作为词袋相似度阈值；

(b)找出候选帧中和当前帧具有相同单词的所有候选帧，同时统计候选帧中与当前帧具有相同单词数最多的候选帧，两帧得到最大相同单词的数目乘上一个0.3～0.8的系数作为最小相同单词数目阈值；

(c)遍历候选帧，挑选出相同单词数大于阈值且词袋相似度大于阈值的候选帧作为一级候选帧，连同词袋相似度一起保存；

(d)遍历一级候选帧，每个一级候选帧的前几连接帧组成一个连接帧组，每组中的每帧与当前帧的词袋相似度求和作为该组的词袋相似度和，遍历结束后，最大的词袋相似度和乘上一个0.3～0.8的系数作为最小词袋相似度和的阈值，从所有连接帧组中挑选出词袋相似度和大于阈值的连接帧组，每组中与当前帧词袋相似度最大的帧作为二级候选帧，即相似候选帧，由此得到两个相似场景；

利用词袋检测算法进行词袋相似度计算，该算法使用快速旋转描述子特征点，即结合了快速特征点和二值描述子的特征点，图像的特征点简单的理解为图像中显著的点，即轮廓点，暗区域中的亮点，亮区域中的暗点，描述子是用来描述某个点周围的特征，将一幅图像中所有特征点的描述子空间离散化，即将该图像上的描述子转换为单词表示，通过式(1)进行描述：

A＝1·w₁+0·w₂+…+1·w_n＝[w₁，w₂，…，w_n][1,0，…，1]^T (1)

式中，w₁…w_n表示n个不同类型的描述子，1表示图像中包含该类型描述子，0表示图像中不包含该类型描述子，T表示向量转置，A表示图像中包含的所有描述子，由于视觉单词在向量中的位置是固定的，所以一幅图像采用包含的描述子类型转换为词袋向量表示，通过式(2)进行描述：

v_A＝[1，0，…，1]^T (2)

式中，v_A表示一副图像的词袋向量，然后，对比词袋向量，计算两幅图像的相似度，通过式(3)进行描述：

式中，v_B表示另一幅图像的词袋向量，||表示1-范数，score(v_A，v_B)表示两幅图像的相似度得分；

步骤3、相似场景投票，机器人的当前帧向满足投票条件的候选相似场景帧投票，当某个候选相似场景帧的票数达到投票阈值，则认为该帧与给其投票的帧为相似场景，将相似场景的点云信息以及该机器人的子点云地图保存到点云文件中，发送回该相似场景帧存在的邻居机器人；当前帧向由场景辨识得到的相似候选帧进行投票，当某相似候选帧满足投票条件时，当前帧向该相似候选帧投一票；投票条件如图2所示：若某相似候选帧的最优连接帧也是当前帧最优连接帧的相似候选帧则投一票，当某帧x票数大于阈值，则认为当前机器人给x帧投票的帧是x帧的相似场景；

步骤4、点云配准，当有相似场景的机器人收到从邻居机器人发来的点云文件后，对点云文件中两个相似场景对应的点云信息进行配准得到点云间的变换矩阵，采样一致性初始配准算法进行粗点云配准，正太分布变换算法进行精点云配准，以此来计算相同场景点云之间的变换矩阵，改善了只用一种点云配准算法结果误差大的问题，具体包括以下子步骤：

(a)采用采样一致性初始配准算法进行粗点云配准，从步骤1中保存的关键帧点云中取出第x帧的点云，将其与给x帧投票的关键帧点云信息分别进行配准，点云信息作为输入，得到的粗变换矩阵作为精匹配的初始变换矩阵；从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的快速点特征直方图特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d，在目标点云Q中查找与点云P中采样点具有相似快速点特征直方图特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点，计算对应点之间刚体变换矩阵，通过求解对应点变换后的距离误差和函数判断配准变换的性能，此处的距离误差和函数使用Huber罚函数表示，记为：

其中

式(5)中，a为一预先给定值，l_k为第k组对应点变换之后的距离差，上述配准的最终目的是在所有变换中找到一组最优的变换，使得误差函数的值最小，此时的变换即为最终的配准变换矩阵；

(b)采用正太分布变换算法进行精点云配准，采样一致性初始配准算法得到的粗变换矩阵作为正太分布变换算法的初始矩阵，x帧的点云和投票帧的点云作为输入得到两片点云之间的精变换矩阵，正态分布变换算法是一种基于概率分布的点云配准算法，其算法原理是将原始点云空间B按一定尺度分成i个立体单元格{b₁,…,b_i}，计算每个单元格中点的概率密度函数，若单元格内的点数量少于三个，则不计算该单元格，概率密度函数的均值μ和协方差∑按正太分布进行计算，通过式(6)、(7)进行描述：

式中，m表示某个单元格b_i中点的数量，y_k表示单元格b_i中第k个点的坐标向量，对待配准点云空间A中的点x_k的坐标按采样一致性初始配准算法得到的初始变换矩阵N进行仿射变换得到x′_k，计算变换后的待配准点云空间中的每个点落在原始点云空间相应位置的单元格内的概率分布函数，通过式(8)进行描述：

式中，D表示向量维数，x′_k表示仿射变换后的点坐标，∑表示原始点云相应

位置单元格内点的概率密度函数的协方差，μ表示原始点云相应位置单元格内点的概率密度函数的均值，构造目标函数通过式(9)进行描述，

式中，T(N，x_k)表示将待配准点云空间A中的点x_k按变换矩阵N进行变换,n表示点云空间A中点的总数量，迭代得到使目标函数最大的变换矩阵N，若给x帧投票的帧数不只1个，会得到多个配准结果，利用正太分布变换算法的误差计算函数对多个配准结果进行误差计算，取误差最小的作为最终的变换矩阵；

步骤5、得到当前时刻的全局点云地图，通过配准算法得到的变换矩阵将点云文件中子点云地图旋转添加到该机器人的子点云地图中，通过八叉树地图包将点云地图转换为占据栅格地图，得到当前时刻的全局点云地图。

Claims (1)

1.一种基于场景辨识的多机器人分布式协作视觉建图方法，其特征在于包括以下步骤：

步骤1、每个机器人进行单机器人视觉建图，每个机器人在视觉建图过程中保存自己路过的场景信息，包括该机器人的标号、关键帧类中的关键帧序号、关键帧描述子、关键帧的点云信息以及关键帧的连接帧的序号和权重，每保存100至300帧就将存储文件通过主机通信发送给邻居机器人，实现将自己路过场景通知给邻居机器人的功能；

步骤2、相似场景辨识，机器人从邻居机器人收到存储场景信息的文件后，将文件中每帧的所有二值描述子即所有单词转换成词袋向量，得到的邻居机器人的每个关键帧信息作为相似场景辨识的候选帧按顺序存储到向量容器中，机器人在对当前帧进行闭环检测的同时，将当前帧与候选帧中的场景进行相似场景辨识，具体包括以下子步骤：

(a)将当前帧与当前帧的连接帧一一进行词袋相似度计算，得到最大的词袋相似度乘上一个0.3～0.8的系数作为词袋相似度阈值；

(b)找出候选帧中和当前帧具有相同单词的所有候选帧，同时统计候选帧中与当前帧具有相同单词数最多的候选帧，两帧得到最大相同单词的数目乘上一个0.3～0.8的系数作为最小相同单词数目阈值；

(c)遍历候选帧，挑选出相同单词数大于阈值且词袋相似度大于阈值的候选帧作为一级候选帧，连同词袋相似度一起保存；

(d)遍历一级候选帧，每个一级候选帧的前几连接帧组成一个连接帧组，每组中的每帧与当前帧的词袋相似度求和作为该组的词袋相似度和，遍历结束后，最大的词袋相似度和乘上一个0.3～0.8的系数作为最小词袋相似度和的阈值，从所有连接帧组中挑选出词袋相似度和大于阈值的连接帧组，每组中与当前帧词袋相似度最大的帧作为二级候选帧，即相似候选帧，由此得到两个相似场景；

利用词袋检测算法进行词袋相似度计算，该算法使用快速旋转描述子特征点，即结合了快速特征点和二值描述子的特征点，图像的特征点简单的理解为图像中显著的点，即轮廓点，暗区域中的亮点，亮区域中的暗点，描述子是用来描述某个点周围的特征，将一幅图像中所有特征点的描述子空间离散化，即将该图像上的描述子转换为单词表示，通过式(1)进行描述：

A＝1·w₁+0·w₂+…+1·w_n＝[w₁，w₂，…，w_n][1,0，…，1]^T (1)

式中，w₁…w_n表示n个不同类型的描述子，1表示图像中包含该类型描述子，0表示图像中不包含该类型描述子，T表示向量转置，A表示图像中包含的所有描述子，由于视觉单词在向量中的位置是固定的，所以一幅图像采用包含的描述子类型转换为词袋向量表示，通过式(2)进行描述：

v_A＝[1，0，…，1]^T (2)

式中，v_A表示一幅图像的词袋向量，然后，对比词袋向量，计算两幅图像的相似度，通过式(3)进行描述：

式中，v_B表示另一幅图像的词袋向量，||表示1-范数，score(v_A，v_B)表示两幅图像的相似度得分；

步骤3、相似场景投票，机器人的当前帧向满足投票条件的候选相似场景帧投票，当某个候选相似场景帧的票数达到投票阈值，则认为该帧与给其投票的帧为相似场景，将相似场景的点云信息以及该机器人的子点云地图保存到点云文件中，发送回该相似场景帧存在的邻居机器人；当前帧向由场景辨识得到的相似候选帧进行投票，当某相似候选帧满足投票条件时，当前帧向该相似候选帧投一票；投票条件为：若某相似候选帧的最优连接帧也是当前帧最优连接帧的相似候选帧则投一票，当某帧x票数大于阈值，则认为当前机器人给x帧投票的帧是x帧的相似场景；

步骤4、点云配准，当有相似场景的机器人收到从邻居机器人发来的点云文件后，对点云文件中两个相似场景对应的点云信息进行配准得到点云间的变换矩阵，采样一致性初始配准算法进行粗点云配准，正态分布变换算法进行精点云配准，以此来计算相同场景点云之间的变换矩阵，改善了只用一种点云配准算法结果误差大的问题，具体包括以下子步骤：

(a)采用采样一致性初始配准算法进行粗点云配准，从步骤1中保存的关键帧点云中取出第x帧的点云，将其与给x帧投票的关键帧点云信息分别进行配准，点云信息作为输入，得到的粗变换矩阵作为精匹配的初始变换矩阵；从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的快速点特征直方图特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d，在目标点云Q中查找与点云P中采样点具有相似快速点特征直方图特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点，计算对应点之间刚体变换矩阵，通过求解对应点变换后的距离误差和函数判断配准变换的性能，此处的距离误差和函数使用Huber罚函数表示，记为：

其中

式(5)中，a为一预先给定值，l_k为第k组对应点变换之后的距离差，上述配准的最终目的是在所有变换中找到一组最优的变换，使得误差函数的值最小，此时的变换即为最终的配准变换矩阵；

(b)采用正态分布变换算法进行精点云配准，采样一致性初始配准算法得到的粗变换矩阵作为正态分布变换算法的初始矩阵，x帧的点云和投票帧的点云作为输入得到两片点云之间的精变换矩阵，正态分布变换算法是一种基于概率分布的点云配准算法，其算法原理是将原始点云空间B按一定尺度分成i个立体单元格{b₁,…,b_i}，计算每个单元格中点的概率密度函数，若单元格内的点数量少于三个，则不计算该单元格，概率密度函数的均值μ和协方差∑按正态分布进行计算，通过式(6)、(7)进行描述：

式中，m表示某个单元格b_i中点的数量，y_k表示单元格b_i中第k个点的坐标向量，对待配准点云空间A中的点x_k的坐标按采样一致性初始配准算法得到的初始变换矩阵N进行仿射变换得到x_k′，计算变换后的待配准点云空间中的每个点落在原始点云空间相应位置的单元格内的概率分布函数，通过式(8)进行描述：

式中，D表示向量维数，x′_k表示仿射变换后的点坐标，∑表示原始点云相应位置单元格内点的概率密度函数的协方差，μ表示原始点云相应位置单元格内点的概率密度函数的均值，构造目标函数通过式(9)进行描述，

式中，T(N，x_k)表示将待配准点云空间A中的点x_k按变换矩阵N进行变换,n表示点云空间A中点的总数量，迭代得到使目标函数最大的变换矩阵N，若给x帧投票的帧数不只1个，会得到多个配准结果，利用正态分布变换算法的误差计算函数对多个配准结果进行误差计算，取误差最小的作为最终的变换矩阵；

步骤5、得到当前时刻的全局点云地图，通过配准算法得到的变换矩阵将点云文件中子点云地图旋转添加到该机器人的子点云地图中，通过八叉树地图包将点云地图转换为占据栅格地图，得到当前时刻的全局点云地图。