CN108227717B - 基于orb特征的多移动机器人地图融合方法及融合平台 - Google Patents

Abstract

本发明公开了基于ORB特征的多移动机器人地图融合方法，包括以下步骤：多机器人网络构建；将机器人实时获取的环境点云信息进行局部栅格地图构建；分别提取格栅地图的ORB特征，并进行特征点匹配，将匹配后的特征点进行提纯；利用经过提纯的二维特征点对计算偏仿射变换，得到最优仿射变换矩阵；对输入最优仿射变换矩阵的特征点计算最优变换，得到单应矩阵，通过对单应矩阵求解得到最优匹配点，设定融合比例得到全局栅格地图。本发明提取了融合地图的ORB特征，解决了融合效率不高的问题；融合时计算了两幅地图最优匹配点集的最优仿射变换参数，解决了重合区域少会导致融合精度降低的问题。

Description

基于ORB特征的多移动机器人地图融合方法及融合平台

技术领域

本发明涉及机器人技术领域，更具体的说是涉及基于ORB特征的多移动机器人地图融合方法及融合平台。

背景技术

随着智能时代的来临，机器人广泛进入了研究人员的视野。近年来，机器人学得到了蓬勃发展，作为一门综合交叉学科，机器人学涵盖了控制、导航、机械和计算机科学等领域的最新成果，是当前科技发展最为活跃的领域之一。机器人学的显著成就使我们更加容易处理一些信息收集的问题，诸如环境监测、搜索救援、以及动态的同时定位与地图创建SLAM(Simultaneous localization and mapping)系统。SLAM技术包括同时定位和地图构建两部分，利用机器人自身携带的传感器感知周围的环境以确定机器人在环境中的位置同时对环境进行建模，SLAM成为当前智能机器人研究的热点问题，被很多著名学者认为是实现真正全自主移动机器人的关键。

面对复杂多变的动态环境时，单个机器人往往难以胜任，需要多机器人的协作配合。地图融合作为多机器人SLAM的一个子问题，是解决多机器人SLAM的有效手段，同时也是其中的难点问题。地图融合现存的解决方法分成直接融合法和间接融合法。直接融合法多依靠的是机器人的相对位姿，要求机器人在某一时刻相遇，而间接融合法多考虑局部地图间的重合区域，不需要知晓机器人相对位置的先验信息，所以应用范围更广，产生了更多解决策略；间接融合法存在的问题其一是地图重复区域较少时容易融合失败；其二是算法实时性较差，融合效率较慢。

因此，如何提供一种融合效率高的多移动机器人地图融合平台及融合方法成为了本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了基于ORB特征的多移动机器人地图融合方法及融合平台，提取了融合地图的ORB特征，解决了融合效率不高的问题；融合时计算了两幅地图最优匹配点集的最优仿射变换参数，解决了重合区域少会导致融合精度降低的问题。

为了实现上述目的，本发明采用如下技术方案：

一种基于ORB特征的多移动机器人地图融合方法，其中，包括如下步骤：

1)、建立网络通信实现多机器人网络构建；

包括2个全向机器人和1台笔记本电脑，每个全向机器人搭载树莓派2，安装Ubuntu14.04系统，Robot operating system版本即ROS版本Indigo，笔记本电脑作为终端计算机，搭载AMD FX-8800P处理器，内存8G，同样安装Ubuntu14.04系统，ROS版本Indigo；使用终端计算机发布指令，独立控制每个全向机器人：首先建立wifi网络，相互通信，终端计算机通过SSH网络协议远程登录机器人上的数据处理单元树莓派2，终端计算机和每个机器人分别启动ROS，使得终端计算机可以独立的控制2个机器人，控制的机器人数量不受限制，由于每个树莓派2有不同的IP地址，则机器人相互之间不会受到影响，同时利用现有的master_discovery软件包和master_sync软件包进行时间上的同步；

2)、机器人通过漫游扫描周围的环境，实时获取的环境点云信息进行局部栅格地图构建；

全向机器人通过漫游扫描周围的环境，搭载激光雷达RPLIDAR实时获取环境点云信息，树莓派2运行Hector SLAM算法进行二维栅格地图构建，为了保证地图构建的效率，使用ROS中的rviz实时显示地图构建情况；

3)、将机器人扫描得到的栅格地图分别提取ORB特征，并进行特征点匹配，将匹配后的特征点基于最小距离比例提纯和随机采样一致性RANSAC方法提纯；

3-1)、ORB特征提取匹配。图像处理使用开源图像处理库OpenCV，版本为2.4.9。将机器人扫描得到的栅格地图分别提取ORB特征，得到关键点oFAST集合k_i＝{k_i|1≤i≤m,m∈N⁺}和k_j＝{k_j|1≤j≤n,n∈N⁺}，将每个特征点依据描述子rBRIEF的方向向量化，用坐标表示为

和

针对ORB的描述子为二进制的BRIEF描述子，故选择汉明距离作为描述子的度量手段。特征点匹配选用穷举法Brute Force，由于栅格地图特征分明，重复相似的区域较多，会出现很多的误匹配，需要进一步的提纯。

3-2)、剔除误匹配。先对Brute Force匹配后的特征点，使用基于阈值的方法，将描述子距离大于30的特征点剔除，进行第一步提纯；然后找到两幅地图中距离差Δd相同的特征点对，

将距离差大于5的点对剔除，进行第二步提纯；对剩余的特征点对利用RANSAC算法筛选，进行第三步提纯；

4)、利用经过提纯的二维特征点对计算偏仿射变换，得到最优仿射变换矩阵；

对经过提纯的二维特征点对计算偏仿射变换Partial affine，定义旋转矩阵R_φ、尺度变换S和平变换t：

φ代表旋转的角度，s代表缩放的比例，δ_x和δ_y代表x和y方向上的平移量；使用estimateRigidTransform函数计算特征点对的最优偏仿射变换矩阵；

偏仿射变换T表示如下：

5)、对输入最优仿射变换矩阵的特征点计算最优变换，得到单应矩阵，通过对单应矩阵求解得到最优解；根据得到的最优匹配点对将多幅地图联系起来，设定融合比例得到全局栅格地图；

待融合的栅格地图设为m₁、m₂，T有4个变量，输入2组匹配点对即可求解，所以对输入的特征点利用最小二乘计算最优变换，得到单应矩阵T₀＝(R₀,t₀)后，作为裁剪ICP算法的初值，采用迭代方式计算最优解(R,t)，建立点集之间的对应关系时，使用基于k-d树的最近邻搜索算法，基于前一次迭代获得的转换关系(R_k-1,t_k-1)，建立点集p_i和q_j之间的对应关系：

在得到最优解(R,t)之后，T(m₂)与m₁的重叠区域可以进行较好的匹配，根据得到的最优匹配点对将两幅地图联系起来，最后设定融合比例，将m₁与m₂融合得到全局栅格地图。

此外，还提供了一种基于ORB特征的多移动机器人地图融合平台，其包括平台基座，电池，驱动电机，电机驱动模块和电压转换模块，所述平台基座与所述驱动电机传动连接，所述驱动电机与所述电机驱动模块电性连接；其中还包括：激光传感器，数据处理单元以及控制单元；

所述激光传感器通过所述数据处理单元与所述控制单元相连，扫描获取周围环境的点云信息，进行局部栅格地图构建；

所述激光传感器采用激光雷达RPLIDAR，包括测距核心和安装平台，测距核心安装于安装平台顶端。激光雷达RPLIDAR作为二维激光传感器，可以扫描获取周围环境的点云信息，进行二维栅格地图构建。所述激光传感器所采用规格如下：系统供电3.6～5V伏，通讯接口为3.3V UART/USB，距离范围为在白色物体下0.2～6m米，角度范围为0～360°度，在小于1.5m范围内距离分辨力为小于0.5mm毫米，在全量程范围内为小于1％，角分辨力为5.5Hz扫描频率下小于等于1°，单次测距时间为0.5ms毫秒，采样频率为大于等于2000Hz，扫描频率为1～10Hz，典型值5.5Hz，每周采样360个点时为典型值；

所述数据处理单元与所述控制单元电连，运行地图构建算法，将局部栅格地图融合成全局栅格地图；所述数据控制单元还与所述激光传感器电连，为激光传感器供电并提供驱动控制其运行；

所述数据处理单元通过输出PWM信号控制电机驱动模块，所述数据处理单元为激光雷达RPLIDAR供电并提供驱动控制其运行；所述数据处理单元采用树莓派2，通过USB转TTL串口线连接控制单元，并为控制单元提供5V电压。树莓派2规格参数如下：处理器为4核ARM Cortex-A7，内存为1GB，具有4个USB 2.0端口，40Pin GPIO接口，一个HDMI视频信号输入接口，一个10/100自适应以太网接口，额定功率为3.5W，电源为InputAC 5V；存储带MicroSD卡插槽，支持通过Micro SD卡启动；

所述电压转换模块，选用了XL4015大功率直流可调降稳压电源模块，最大电流5A，功率75W，带电压显示带数显；

所述电池与所述电机驱动模块电连，并且所述电池通过电压转换模块与所述数据处理单元电连；电池通过电压转换模块，将12V降压为5V稳压，为树莓派2供电；

所述控制单元支持串口控制，提供速度位置双环PID控制，使得机器人工作在速度控制或者位置控制模式；

所述控制单元为主板，其上设置有OLED显示屏、所述电机驱动模块和MPU-6050模块，所述主板还集成了三轴加速度计和三轴陀螺仪。

所述控制单元为主板，主板配置了0.96寸OLED显示屏，3个TB6612FNG电机驱动模块，传感器包括型号GY-521的MPU-6050模块，集成了三轴加速度计和三轴陀螺仪，共6DOF，提供Z轴陀螺仪校正，并通过CAN总线输出小车三轴姿态；支持串口控制，提供速度位置双环PID控制，机器人可以工作在速度控制或者位置控制模式；

所述平台基座包括：底板、驱动轮、联轴器和滚筒，所述滚筒和所述联轴器设置于所述底板顶端，并且所述滚筒通过所述联轴器与所述驱动轮连接；

采用低成本的激光雷达RPLIDAR传感器和三轮全向机器人基座，构建了三轮全向轮式移动机器人硬件平台，拥有速度位置双环控制控制模式，可以构建清晰的环境栅格地图，具备在室内进行自主漫游避障和导航的功能，同时可以作为多机器人地图融合的验证平台。

经由上述的技术方案可知，与现有技术相比，本发明公开提供了基于ORB特征的多移动机器人地图融合方法及融合平台，有益效果在于：

本发明所提供的基于ORB特征的多移动机器人地图融合平台，采用低成本的激光传感器、数据处理单元和三轮全向机器人平台基座，构建了三轮全向轮式移动机器人硬件平台，拥有速度位置双环控制控制模式，可以构建清晰的环境栅格地图，具备在室内进行自主漫游避障和导航的功能，同时可以作为多机器人地图融合的验证平台。

本发明还提供了基于ORB特征的多移动机器人地图融合方法，将传统方法和ORB算法结合起来，应用到多机器人栅格地图融合，通过自主搭建的数据处理单元的全向机器人平台，经过双机器人构图实验，成功得使用Hector SLAM算法构建了局部栅格地图并融合成全局栅格地图，同时选择公开的栅格地图数据集进行验证，本发明提出的融合算法在融合效率和融合精度上都有较好的表现。

移动机器人的可扩展性强，软件系统选择ROS系统，硬件主体使用数据处理单元，二者均为开源产品，可开发程度高，通过硬件的升级和程序的改进，本平台能够开发多机器人在地图融合方面的应用研究。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1附图为本发明一种栅格地图融合流程图。

图2附图为本发明另一种栅格地图融合流程图。

图3附图为本发明的结构框图。

图4附图为本发明平台基座的结构示意图。

其中，图中，

1-底板；2-驱动轮；3-联轴器；4-滚筒。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参阅附图1，本发明提供了一种基于ORB特征的多移动机器人地图融合方法，包括以下步骤：

建立网络通信实现多机器人网络构建；

机器人通过漫游扫描周围的环境，实时获取的环境点云信息进行局部栅格地图构建；

将机器人扫描得到的栅格地图分别提取ORB特征，并进行特征点匹配，将匹配后的特征点基于最小距离比例提纯和随机采样一致性RANSAC方法提纯；

利用经过提纯的二维特征点对计算偏仿射变换，得到最优仿射变换矩阵；

对输入最优仿射变换矩阵的特征点计算最优变换，得到单应矩阵，通过对单应矩阵求解得到最优解；根据得到的最优匹配点对将多幅地图联系起来，设定融合比例得到全局栅格地图。

本发明还提供了一种基于ORB特征的多移动机器人地图融合平台，包括平台基座，电池，驱动电机，电机驱动模块和电压转换模块，所述平台基座与所述驱动电机传动连接，所述驱动电机与所述电机驱动模块电性连接；其特征在于，还包括：激光传感器，数据处理单元以及控制单元；其中，

所述激光传感器通过所述数据处理单元与所述控制单元相连，扫描获取周围环境的点云信息，进行局部栅格地图构建；

所述数据处理单元与所述控制单元电连，运行地图构建算法，将局部栅格地图融合成全局栅格地图；所述数据控制单元还与所述激光传感器电连，为激光传感器供电并提供驱动控制其运行；

所述控制单元支持串口控制，提供速度位置双环PID控制，使得机器人工作在速度控制或者位置控制模式。

参阅附图2，基于ORB特征的多移动机器人地图融合方法，具体包括以下步骤：

(1)多机器人网络构建

实验系统包括2个全向机器人和1台笔记本电脑，每个全向机器人搭载树莓派2，安装Ubuntu14.04系统，ROS(Robot operating system)版本Indigo，笔记本电脑作为终端计算机，搭载AMDFX-8800P处理器，内存8G，同样安装Ubuntu14.04系统，ROS版本Indigo。使用终端计算机发布指令，独立控制每个全向机器人。首先建立wifi网络，系统才能相互通信，终端计算机通过SSH网络协议远程登录机器人上的数据处理单元树莓派2，终端计算机和每个机器人分别启动ROS，使得终端计算机可以独立的控制2个机器人，控制的机器人数量理论上不受限制，由于每个树莓派2有不同的IP地址，所以机器人相互之间不会受到影响，同时利用现有的master_discovery和master_sync两个软件包进行时间上的同步。

(2)局部栅格地图构建

全向机器人通过漫游扫描周围的环境，搭载激光雷达RPLIDAR实时获取环境点云信息，树莓派2运行HectorSLAM算法进行二维栅格地图构建，为了保证地图构建的效率，使用ROS中的rviz实时显示地图构建情况。

(3)ORB特征提取匹配

①ORB特征提取匹配。图像处理使用开源图像处理库OpenCV，版本为2.4.9。将机器人扫描得到的栅格地图分别提取ORB特征，得到关键点oFAST集合k_i＝{k_i|1≤i≤m,m∈N⁺}和k_j＝{k_j|1≤j≤n,n∈N⁺}，将每个特征点依据描述子rBRIEF的方向向量化，用坐标表示为

和

针对ORB的描述子为二进制的BRIEF描述子，故选择汉明距离作为描述子的度量手段。特征点匹配选用穷举法Brute Force，由于栅格地图特征分明，重复相似的区域较多，会出现很多的误匹配，需要进一步的提纯。

②剔除误匹配。先对Brute Force匹配后的特征点，使用基于阈值的方法，将描述子距离大于30的特征点剔除，进行第一步提纯；然后找到两幅地图中距离差Δd相同的特征点对，

将距离差大于5的点对剔除，进行第二步提纯；对剩余的特征点对利用RANSAC算法筛选，进行第三步提纯。

(4)融合参数计算

对经过提纯的二维特征点对计算偏仿射变换(Partial affine)，定义旋转矩阵R_φ、尺度变换S和平变换t：

φ代表旋转的角度，s代表缩放的比例，δ_x和δ_y代表x和y方向上的平移量。使用estimateRigidTransform函数计算特征点对的最优偏仿射变换矩阵。

偏仿射变换T表示如下：

(5)栅格地图融合

待融合的栅格地图设为m₁与m₂。由于T只有4个变量，只需要输入2组匹配点对即可求解，所以对输入的特征点利用最小二乘计算最优变换。得到单应矩阵T₀＝(R₀,t₀)后，作为裁剪ICP算法的初值，采用迭代方式计算最优解(R,t)，建立点集之间的对应关系时，使用基于k-d树的最近邻搜索算法。基于前一次迭代获得的转换关系(R_k-1,t_k-1)，建立点集p_i和q_j之间的对应关系：

在得到最优解(R,t)之后，T(m₂)与m₁的重叠区域可以进行较好的匹配，根据得到的最优匹配点对将两幅地图联系起来，最后设定融合比例，将m₁与m₂融合得到全局栅格地图。

本发明所提供的基于ORB特征的多移动机器人地图融合方法，将传统方法和ORB算法结合起来，应用到多机器人栅格地图融合，通过自主搭建的基于树莓派2的全向机器人平台，经过双机器人构图实验，成功得使用Hector SLAM算法构建了局部栅格地图并融合成全局栅格地图，同时选择公开的栅格地图数据集进行验证，本发明提出的融合算法在融合效率和融合精度上都有较好的表现。

请参阅附图3-4，本发明提供了基于ORB特征的多移动机器人地图融合平台，包括：平台基座、电池、驱动电机、电机驱动模块、激光传感器、数据处理单元、电压转换模块和控制单元。

平台基座包括：底板、驱动轮、联轴器和滚筒，滚筒和联轴器设置于底板顶端，并且滚筒通过联轴器与驱动轮连接，底板材质使用亚克力板，驱动轮由三个直径60mm铝合金全向轮组成。平台基座的相关参数见表1。

表1

电池为3S 2800mah的航模锂电池，支持B3充电器充电。

驱动电机，控制平台基座的移动，驱动电机选用永磁有刷直流电机，并且带减速器，能够更好的控制机器人运动。驱动电机的工作参数见表2。

表2

工作电压(V)	7～13，通常选择12
		额定功率(W)	7
堵转电流(A)	5.4
		堵转扭矩(kgf.cm)	15
额定电流(mA)	540
		额定扭矩(kgf.cm)	15
原始转速(rpm)	15000
		空载转速(rpm)	500
减速比	1：30

电机驱动模块通过AB相编码器驱动电机，实现驱动电机正反向的调速控制。

激光传感器采用激光雷达RPLIDAR，包括测距核心和安装平台，测距核心安装于安装平台顶端。激光雷达RPLIDAR作为二维激光传感器，可以扫描获取周围环境的点云信息，进行二维栅格地图构建。具体参数见表3。

表3

数据处理单元，数据处理单元是移动机器人平台运行构图算法以及实现控制的核心，选择数据处理单元需要考虑处理能力和运行速度，结合性价比及性能，选用树莓派2作为数据处理单元，能够满足正常条件下的实验需求。它的功能包括：通过输出PWM信号控制电机驱动模块；运行地图构建算法，进行栅格地图构建；为激光雷达RPLIDAR供电并提供驱动控制其运行；树莓派2通过USB转TTL串口线连接控制单元，并为控制单元提供5V电压。树莓派2硬件配置参数见表4。

表4

电压转换模块，选用了XL4015大功率直流可调降稳压电源模块，最大电流5A，功率75W，带电压显示带数显。电池通过电压转换模块，将12V降压为5V稳压，为树莓派2供电。

控制单元为主板，主板配置了0.96寸OLED显示屏，3个TB6612FNG电机驱动模块，传感器包括型号GY-521的MPU-6050模块，集成了三轴加速度计和三轴陀螺仪，共6DOF，提供Z轴陀螺仪校正，并通过CAN总线输出小车三轴姿态。支持串口控制，提供速度位置双环PID控制，机器人可以工作在速度控制或者位置控制模式。

本发明还提供了基于ORB特征的多移动机器人地图融合平台，采用低成本的激光雷达RPLIDAR传感器、树莓派2和三轮全向机器人基座，构建了三轮全向轮式移动机器人硬件平台，拥有速度位置双环控制控制模式，可以构建清晰的环境栅格地图，具备在室内进行自主漫游避障和导航的功能，同时可以作为多机器人地图融合的验证平台。

移动机器人的可扩展性强，软件系统选择ROS系统，硬件主体使用树莓派2，二者均为开源产品，可开发程度高，通过硬件的升级和程序的改进，本平台能够开发多机器人在地图融合方面的应用研究。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (1)

1.一种基于ORB特征的多移动机器人地图融合方法，其特征在于，包括如下步骤：

1)、建立网络通信实现多机器人网络构建；

包括2个全向机器人和1台笔记本电脑，每个全向机器人搭载树莓派2，安装Ubuntu14.04系统，Robot operating system版本即ROS版本Indigo，笔记本电脑作为终端计算机，搭载AMD FX-8800P处理器，内存8G，同样安装Ubuntu14.04系统，ROS版本Indigo；使用终端计算机发布指令，独立控制每个全向机器人：首先建立wifi网络，相互通信，终端计算机通过SSH网络协议远程登录机器人上的数据处理单元树莓派2，终端计算机和每个机器人分别启动ROS，使得终端计算机可以独立的控制2个机器人，控制的机器人数量不受限制，由于每个树莓派2有不同的IP地址，则机器人相互之间不会受到影响，同时利用现有的master_discovery软件包和master_sync软件包进行时间上的同步；

2)、机器人通过漫游扫描周围的环境，实时获取的环境点云信息进行局部栅格地图构建；

全向机器人通过漫游扫描周围的环境，搭载激光雷达RPLIDAR实时获取环境点云信息，树莓派2运行Hector SLAM算法进行二维栅格地图构建，为了保证地图构建的效率，使用ROS中的rviz实时显示地图构建情况；

3)、将机器人扫描得到的栅格地图分别提取ORB特征，并进行特征点匹配，将匹配后的特征点基于最小距离比例提纯和随机采样一致性RANSAC方法提纯；

3-1)、ORB特征提取匹配：图像处理使用开源图像处理库OpenCV，版本为2.4.9，将机器人扫描得到的栅格地图分别提取ORB特征，得到关键点oFAST集合k_i＝{k_i|1≤i≤m,m∈N⁺}和k_j＝{k_j|1≤j≤n,n∈N⁺}，将每个特征点依据描述子rBRIEF的方向向量化，用坐标表示为

和

针对ORB的描述子为二进制的BRIEF描述子，故选择汉明距离作为描述子的度量手段；特征点匹配选用穷举法Brute Force，由于栅格地图特征分明，重复相似的区域较多，会出现很多的误匹配，需要进一步的提纯；

3-2)、剔除误匹配：先对Brute Force匹配后的特征点，使用基于阈值的方法，将描述子距离大于30的特征点剔除，进行第一步提纯；然后找到两幅地图中距离差Δd相同的特征点对，

将距离差大于5的点对剔除，进行第二步提纯；对剩余的特征点对利用RANSAC算法筛选，进行第三步提纯；

4)、利用经过提纯的二维特征点对计算偏仿射变换，得到最优仿射变换矩阵；

对经过提纯的二维特征点对计算偏仿射变换Partial affine，定义旋转矩阵R_φ、尺度变换S和平变换t：

φ代表旋转的角度，s代表缩放的比例，δ_x和δ_y代表x和y方向上的平移量；使用estimateRigidTransform函数计算特征点对的最优偏仿射变换矩阵；

偏仿射变换T表示如下：

5)、对输入最优仿射变换矩阵的特征点计算最优变换，得到单应矩阵，通过对单应矩阵求解得到最优解；根据得到的最优匹配点对将多幅地图联系起来，设定融合比例得到全局栅格地图；

待融合的栅格地图设为m₁、m₂，T有4个变量，输入2组匹配点对即可求解，所以对输入的特征点利用最小二乘计算最优变换，得到单应矩阵T₀＝(R₀,t₀)后，作为裁剪ICP算法的初值，采用迭代方式计算最优解(R,t)，建立点集之间的对应关系时，使用基于k-d树的最近邻搜索算法，基于前一次迭代获得的转换关系(R_k-1,t_k-1)，建立点集p_i和q_j之间的对应关系：

在得到最优解(R,t)之后，T(m₂)与m₁的重叠区域可以进行较好的匹配，根据得到的最优匹配点对将两幅地图联系起来，最后设定融合比例，将m₁与m₂融合得到全局栅格地图。

Images