CN105701771A - 一种基于射频识别锚点的数字地图拼接方法 - Google Patents

Abstract

本发明公开了一种基于射频识别锚点的数字地图拼接方法，包括以下步骤：首先建立带有射频识别锚点的数字地图；在各自地图中提取射频识别锚点为特征点，并建立对应关系；再借助RANSAC算法计算初始拼接参数，作为ICP算法的初值；然后使用ICP算法计算刚体变换参数，对地图进行刚体变换；最后，提出针对栅格地图的拼接规则，对两幅地图进行拼接。本发明的地图拼接方法计算量小准确度高，能做到快捷无误地提取特征点。

Description

一种基于射频识别锚点的数字地图拼接方法

技术领域

本发明涉及一种数字地图拼接方法，具体涉及一种基于射频识别锚点的栅格地图拼接方法，属于多移动机器人协同创建数字地图技术领域。

背景技术

地图拼接是多移动机器人协同创建环境地图中的关键技术，随着环境规模的不断扩大，单移动机器人创建地图的方式在精度、鲁棒性以及计算效率等性能方面受到了严重的制约。与单移动机器人相比，多移动机器人系统在执行指定任务时，在精度、鲁棒性和效率等方面具有十分优越的性能。但多移动机器人协同创建地图的过程是非常复杂的，其所面临的一个重要问题是如何实现地图拼接，即多个移动机器人在同一环境中分别探索不同区域，并创建独立的环境地图，最终将各自所创建的地图拼接成大规模的环境地图。

现阶段的地图拼接方法流程一般为：首先对两幅地图进行特征提取得到特征点；通过进行相似性度量找到匹配的特征点对；然后通过匹配的特征点对得到图像空间坐标变换参数；最后由坐标变换参数进行地图拼接。而特征提取是地图拼接技术中的关键，准确的特征提取为特征匹配的成功进行提供了保障。因此，寻求具有良好不变性和准确性的特征提取方法，对于匹配精度至关重要。

射频识别技术基本工作原理是将电子标签附着在被识别物体的表面或内部中，当被标记的对象处于读写器的可读取范围内时，电子标签和读写器之间将自动建立起无线电磁波或交变磁场方式的通信链路，读写器根据控制器需求或自动的以无接触的方式获取电子标签中所存储的识别信息(如编号、位置数据等等)，接收后读写器对信息进行解码并将结果传至后台控制器进行综合处理，从而完成整个信息的读取过程。由于射频识别标签易于存储位置数据，在机器人建立地图时以这些位置数据为基础建立锚点，成为一种具有良好不变性和准确性的特征提取方法。

发明内容

本发明的目的是为解决上述现有技术存在的问题和不足，提供一种基于射频识别锚点的数字地图拼接方法，本发明的方法能够有效提高特征点提取的不变性和准确性，并且减少计算量。

为达到上述目的，本发明采用以下技术方案：

一种基于射频识别锚点的数字地图拼接方法，包括以下步骤：

步骤(1)：建立带有射频识别锚点的数字地图；

步骤(2)：在各自地图中提取特征点，并建立对应关系；

步骤(3)：使用RANSAC算法挑选出有效的特征对，计算初始拼接参数；

步骤(4)：以初始拼接参数为基础，使用ICP算法计算刚体变换参数，完成对地图的刚体变换；

步骤(5)：提出针对栅格地图的拼接规则，对刚体变换后的地图进行拼接。

上述步骤(1)所述的建立带有射频识别锚点的数字地图的具体步骤为：

步骤①：启动机器人并加载激光雷达和射频识别模块；

步骤②：机器人使用激光雷达进行即时定位和地图创建；

步骤③：在地图中加入射频识别锚点。

上述步骤(2)所述的特征点为射频识别锚点。

上述步骤(3)所述的计算初始拼接参数,方法如下：设步骤(1)中建立的地图为P和Q，两幅地图中的射频识别锚点集合分别为和设P_ξ为P的子集，它代表地图P与Q重叠区域的射频识别锚点集合,其中ξ定义为点集P的重叠百分比，从地图P和Q中提取并建立特征对为其中第i组特征对中的F_i，P和F_i，Q分别表示地图P和Q中的射频识别锚点，这些射频识别锚点在各自地图中的位置分别为f_i，P和f_i，Q，设T＝{R，t}为拼接地图的近似解，其中R是由旋转角θ所确定的二维旋转矩阵,t为二维的列向量,设{F_i，P，F_i，Q}_i为一组有效特征对，一组特征对中的元素对应空间中的同一个位置，则||Rf_i，P+t-f_i，Q||₂≈0，设变量N₀和r的初值为0，根据RANSAC算法挑选有效特征对并计算初始拼接参数的步骤如下：

步骤①：设置r＝r+1，并随机选取两组特征对i＝m，i＝n；

步骤②：将{F_i，P，F_i,Q}_i＝m，n带入下式

$(R_r,t_r)=\underset{R,t}{\arg \min }\underset{p_i\∈p_{{\mathrm{\ξ}}_k}}{\Σ}\left|\right|{\mathrm{Rp}}_i+t-q_{ck\left(i\right)}|{|}_2^2$

计算获得一组拼接参数T_r＝{R_r，t_r}；

步骤③：根据最新获得的参数T_r，将地图P中所有的特征点位置转换到地图Q的坐标系下并计算d_i＝||R_rf_i，P+t_r-f_i，Q||₂；

步骤④：设定阈值为d_thr，统计满足d_i≤d_thr的锚点特征对的数目N_r，当N_r＞N₀，则设置N₀＝N_r；

步骤⑤：重复步骤①-④，直到达到r次，最后用找到的N₀个有效特征对来计算初始拼接参数T₀＝{R₀，t₀}。

上述步骤(4)所述的使用ICP算法计算刚体变换参数,其具体步骤如下：

在得出初始拼接参数T₀＝{R₀，t₀}后运用ICP算法中的迭代思想来求解刚体变换参数。求解步骤如下：

步骤①：基于前一次迭代获得的刚体变换(R_k-1，t_k-1),建立特征点之间的对应关系：

$c_k\left(i\right)=\arg \underset{j\∈(1,2,\mathrm{...},N_q)}{min}\left|\right|R_{k-1}{p}_i+t_{k-1}-q_j|{|}_2,i=1,2,\mathrm{...},N_p$

选择最近特征点建立特征点对保留特征点p_i与对应特征点之间的欧式距离

步骤②：根据当前建立的特征点对应关系，计算重叠百分比ξ_k，并更新相应的子集p_ξk：

$({\mathrm{\&xi;}}_k,p_{{\mathrm{\&xi;}}_k})=\arg \underset{{\mathrm{\&xi;}}_{\min }<\mathrm{\&xi;}\&le;1}{\min }\frac{{\mathrm{\&Sigma;}}_{p_i\&Element;p_{\mathrm{\&xi;}}}\left|\right|R_{k-1}{p}_i+t_{k-1}-q_{c_k\left(i\right)}|{|}_2^2}{\left|p_{\mathrm{\&xi;}}\right|{\mathrm{\&xi;}}^{1+\mathrm{\&lambda;}}}$

为求解上式，采用如下的方式：根据步骤①所保留的欧氏距离按升序方式对所有的特征点对进行排序，获得排序队列；采用累加方式，逐个遍历排序后的特征点对并计算步骤①中的目标函数值c_k(i)；待遍历完所有的特征点对后，确定最小目标函数值所对应的特征点对在队列中的位置标号I_k以计算当前最优的重叠百分比ξ_k＝(I_k/N_p)，并使用排序队列中的前I_k个特征点对更新子集P_ξk，其中，ξ_k代表待拼接栅格地图的重叠百分比；

步骤③：利用更新后的子集P_ξk，使用下式计算刚体变换参数T_k＝{R_k，t_k}

$(R_k,t_k)=\underset{R,t}{\arg \min }\underset{p_i\&Element;p_{{\mathrm{\&xi;}}_k}}{\&Sigma;}\left|\right|{\mathrm{Rp}}_i+t-q_{ck\left(i\right)}|{|}_2^2$

重复执行步骤①-③，直到满足或者迭代次数k达到最大次数K，其中待ICP算法满足迭代停止条件后,即获得精确的刚体变换参数T_k＝{R_k，t_k}，通过刚体变换参数对地图P进行相应的旋转和平移，获得刚体变换后的地图为T(P)。

上述步骤(5)所述的提出针对栅格地图的拼接规则是以像素为单位进行拼接。

本发明的优点和有益效果：

1、本发明使用射频识别锚点作为特征点，代替了以往的对图像进行预处理从而提取特征的过程，具有良好的不变性和准确性，并且更加的快速；

2、本发明所述的地图拼接方法对地图精度的依赖性很小，对于精度不高的地图同样可以完成良好的拼接，而且不受外界环境的影响，在复杂多变、有噪声干扰的环境下一样可以得出良好的结果；

3、本发明所述的地图拼接方法适用于多机器人协作构建环境地图，计算量小、准确度高，能提高多机器人协作构图的精度、鲁棒性和效率；

附图说明

图1为本发明基于射频识别锚点的数字地图拼接方法的步骤流程图，

图2为机器人创建含有射频识别锚点的数字地图的步骤流程图，

图3为两幅栅格地图拼接示意图，

图4为两幅栅格地图融合规则。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。

如图1所示，为本发明的基于射频识别锚点的数字地图拼接方法，步骤如下：

步骤(1)：建立带有射频识别锚点的数字地图；

步骤(2)：在各自地图中提取特征点，并建立对应关系；

步骤(3)：使用RANSAC算法挑选出有效的特征对，计算初始拼接参数；

步骤(4)：以初始拼接参数为基础，使用ICP算法计算刚体变换参数，完成对地图的刚体变换；

步骤(5)：提出针对栅格地图的拼接规则，对刚体变换后的地图进行拼接。

根据上述步骤(1)所述，如图2所示，建立带有射频识别锚点的数字地图步骤如下：

步骤①：启动移动机器人A和B，并加载相应的模块，在每个机器人所在环境中布置若干个射频识别标签，标签中存储着全局坐标；

步骤②：控制移动机器人个体A和B在各自所在的房间旋转一周，采用激光雷达获取环境的障碍物数据信息，一次扫描可以得到1080个环境点数据(范围270°，间隔0.25°)，这些数据是以激光雷达中心为极点的极坐标系表示的局部坐标(d_i,θ_i),其中θ_i＝i×0.25π÷270，第i个点在全局坐标下的坐标为：

x_i＝x_k+d_icosθ_icosα_k+d_isinθ_isinα_k(1)

y_i＝y_k+d_icosθ_isinα_k+d_isinθ_icosα_k(2)

其中(x_k，y_k)是k时刻激光雷达在全局坐标系中的坐标，α_k为k时刻激光雷达主轴与全局坐标系X轴的夹角,d_i表示k时刻激光雷达与第i个特征点之间的距离。

步骤③：将所得的激光雷达数据进行滤波处理，使用ROS机器人操作系统中的Gmapping来分析和生成环境地图。

步骤④：以事先在环境中布置好的射频识别标签为依据，在生成的地图中加入射频识别锚点，这些射频识别锚点作为以后地图拼接中的不变特征。

为了实现地图拼接，需要计算两幅地图之间的刚体变换T＝{R，t}，其中R是由旋转角θ所确定的二维旋转矩阵,t为二维的列向量，R和t的表达式为：给定同一环境下不同区域的栅格地图P和Q，地图拼接的目标就是准确的计算出拼接参数T＝{R，t}，地图P通过刚体变换后的地图为T(P)，使之与地图Q进行匹配，然后再完成拼接。

为了保证地图拼接取得较好的结果，需要从每张地图中提取特征点，通过建立不同地图之间特征点对应关系的方式，计算初始拼接参数，本发明中特征点是前面步骤中建立的射频识别锚点，每个射频识别锚点中存储着全局坐标，利用这些坐标建立特征点的对应关系。

根据上述步骤(3)所述，接下来采用RANSAC算法挑选出有效的特征对，计算初始拼接参数。两幅地图的锚点集合分别为和设P_ξ为P的子集，它代表地图P与Q重叠区域的锚点集合,其中ξ定义为数据点集P的重叠百分比。从地图P和Q中提取并建立特征对为其中第i组特征对中的F_i，P和f_i，Q分别表示地图P和Q中的锚点，其在各自地图中的位置分别为f_i，P和f_i，Q。假设T＝{R，t}为拼接地图的近似解，且{F_i，P，f_i，Q}_i为一组有效特征对，即他们对应空间中的同一个位置，则||Rf_i，P+t-f_i,Q‖₂≈0。设变量N₀和r的初值为0，根据RANSAC算法计算可靠初始拼接数据的步骤如下：

步骤①：设置r＝r+1，并随机选取两组特征对i＝m，i＝n；

步骤②：将(F_i，P,f_i,Q}_i＝m，n带入下式

$(R_r,t_r)=\underset{R,t}{\arg \min }{\mathrm{\&Sigma;}}_{p_i\&Element;p_{{\mathrm{\&xi;}}_k}}\left|\right|{\mathrm{Rp}}_i+t-q_{ck\left(i\right)}|{|}_2^2---\left(3\right)$

计算获得一组新的拼接参数T_r＝{R_r，t_r}；

步骤③：根据获得的参数T_r，将地图P中所有的特征点位置转换到地图Q的坐标系下并计算d_i＝||R_rf_i，P+t_r-f_i，Q‖₂；

步骤④：设定阈值为d_thr＝0.6，统计满足d_r≤d_thr的锚点特征对的数目N_r，当N_r＞N₀，则设置N₀＝N_r；

步骤⑤：重复步骤①-④，直到达到r次，最后用找到的N₀个有效特征对来计算初始拼接参数T₀＝{R₀，t₀}。

根据上述步骤(4)所述，使用ICP算法计算刚体变换参数,其具体步骤如下：

在得出初始拼接参数T₀＝{R₀，t₀}后运用ICP算法中的迭代思想来求解刚体变换参数。求解步骤如下：

步骤①：基于前一次迭代获得的刚体变换(R_k-1，t_k-1)，建立特征点之间的对应关系：

$c_k\left(i\right)=\arg \underset{j\&Element;\{1,2,\mathrm{...},N_q\}}{min}\left|\right|R_{k-1}{p}_i+t_{k-1}-q_j|{|}_2,i=1,2,...,N_p---\left(4\right)$

选择最近特征点建立特征点对保留特征点p_i与对应特征点之间的欧式距离

步骤②：根据当前建立的特征点对应关系，计算重叠百分比ξ_k，并更新相应的子集p_ξk：

$({\mathrm{\&xi;}}_k,p_{{\mathrm{\&xi;}}_k})=\arg \underset{{\mathrm{\&xi;}}_{\min }<\mathrm{\&xi;}\&le;1}{min}\frac{{\mathrm{\&Sigma;}}_{p_i\&Element;p_{\mathrm{\&xi;}}}\left|\right|R_{k-1}{p}_i+t_{k-1}-q_{c_k\left(i\right)}|{|}_2^2}{\left|p_{\mathrm{\&xi;}}\right|{\mathrm{\&xi;}}^{1+\mathrm{\&lambda;}}}---\left(5\right)$

为求解上式，采用如下的方式：根据步骤①所保留的欧氏距离按升序方式对所有的特征点对进行排序，获得排序队列；采用累加方式，逐个遍历排序后的特征点对并计算步骤①中的目标函数值c_k(i)；待遍历完所有的特征点对后，确定最小目标函数值所对应的特征点对在队列中的位置标号I_k以计算当前最优的重叠百分比ξ_k＝(I_k/N_p)并使用排序队列中的前I_k个特征点对更新子集P_ξk，其中，ξ_k代表待拼接栅格地图的重叠百分比；

步骤③：利用更新后的子集P_ξk，使用下式计算刚体变换参数T_k＝{R_k，t_k}

$(R_k,t_k)=\underset{R,t}{\arg \min }{\mathrm{\&Sigma;}}_{p_i\&Element;p_{{\mathrm{\&xi;}}_k}}\left|\right|{\mathrm{Rp}}_i+t-q_{ck\left(i\right)}|{|}_2^2---\left(6\right)$

使用最小二乘法来计算上式，设p_i＝(x_i，y_i)^T， 公式(6)对参数(θ，t_x，t_y)求偏导数，可获得3个方程求解：

$\left(\begin{array}{c}{\mathrm{\&theta;}}_k\\ t_{k,x}\\ t_{k,y}\end{array}\right)=\left(\begin{array}{c}\frac{arc\cot (S_{{\mathrm{xx}}^{\&prime;}}+S_{{\mathrm{yy}}^{\&prime;}})}{S_{{\mathrm{xy}}^{\&prime;}}+S_{{\mathrm{yx}}^{\&prime;}}}\\ {\stackrel{\&OverBar;}{x}}^{\&prime;}-(\stackrel{\&OverBar;}{x}{\mathrm{cos\&theta;}}_k-\stackrel{\&OverBar;}{y}{\mathrm{sin\&theta;}}_k)\\ {\stackrel{\&OverBar;}{y}}^{\&prime;}-(\stackrel{\&OverBar;}{x}{\mathrm{sin\&theta;}}_k+\stackrel{\&OverBar;}{y}{\mathrm{con\&theta;}}_k)\end{array}\right)---\left(7\right)$

重复执行步骤①-③，直到满足|ε_k-ε_k-1|＜ε或者迭代次数k达到最大次数K，其中待ICP算法满足迭代停止条件后,即可获得精确的刚体变换参数T_k＝{R_k，t_k}，通过刚体变换参数对地图P进行相应的旋转和平移，获得刚体变换后的地图为T(P)。上述ICP算法任然采用了原始ICP中的迭代思想和原理，所以该算法也具有局部收敛性。

步骤(5)所述的提出针对栅格地图的拼接规则是逐个遍历并确定地图中每个像素的方式来进行，计算出精确的拼接参数后，可对栅格地图P通过刚体变换进行对应的旋转和平移，把变换之后的地图记为T(P)，如图3所示。如果直接将变换之后的地图T(P)叠加到地图Q上，便会出现遮挡的现象，不能得到正确的拼接结果。因此，必须通过融合地图Q和T(P)，才能获得正确的拼接结果。为了准备地图融合，定义一幅空白地图M，用它来存放两幅地图的融合结果。地图M的大小可由等待拼接的两幅地图的大小决定。在定义了空白地图M后，选择基准点，把地图Q的坐标原点与基准点重合。然后通过逐个遍历的方法确定地图M中每个栅格中的内容，因为空白地图M中每个栅格的内容由地图T(P)和地图Q中的值来确定，为了确定M中每一个栅格的值需要根据步骤(4)中得出的拼接参数；T＝{R，t}确定P中的对应栅格，设[i，j]为地图M中的栅格，由下式来计算：

其中，代表向下取整符号。此时，可按下式确定M中第[i，j]个栅格的融合结果：

M[i，j]＝h(Q[i，j]，P[i'，j'])(9)

其中，h(·)代表融合函数，其对应的融合规则如图4所示。最后，当遍历及计算结束后即可获得精确的栅格地图拼接结果。

以上所述，仅为本发明较佳的具体实施方式。当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，任何熟悉本技术领域的技术人员，当可根据本发明作出各种相应的等效改变和变形，都应属于本发明所附的权利要求的保护范围。

Claims (6)

1.一种基于射频识别锚点的数字地图拼接方法，其特征在于，包括以下步骤：

步骤(1)：建立带有射频识别锚点的数字地图；

步骤(2)：在各自地图中提取特征点，并建立对应关系；

步骤(3)：使用RANSAC算法挑选有效的特征对，计算初始拼接参数；

步骤(4)：以初始拼接参数为基础，使用ICP算法计算刚体变换参数，完成对地图的刚体变换；

步骤(5)：提出针对栅格地图的拼接规则，对刚体变换后的地图进行拼接。

2.根据权利要求书1所述的一种基于射频识别锚点的数字地图拼接方法，其特征在于，步骤(1)所述的建立带有射频识别锚点的数字地图的具体步骤为：

步骤(21)：启动机器人并加载激光雷达和射频识别模块；

步骤(22)：机器人使用激光雷达进行即时定位和地图创建；

步骤(23)：在地图中加入射频识别锚点。

3.根据权利要求书1所述的一种基于射频识别锚点的数字地图拼接方法，其特征在于，步骤(2)所述的特征点为射频识别锚点。

4.根据权利要求书1所述的一种基于射频识别锚点的数字地图拼接方法，其特征在于，步骤(3)所述的计算初始拼接参数的方法是：设步骤(1)中建立的地图为P和Q，两幅地图中的射频识别锚点集合分别为和设P_ξ为P的子集，代表地图P与Q重叠区域的射频识别锚点集合,其中ξ定义为点集P的重叠百分比，从地图P和Q中提取并建立特征对为其中第i组特征对中的F_i,P和F_i,Q分别表示地图P和Q中的射频识别锚点，所述射频识别锚点在各自地图中的位置分别为f_i，P和f_i，Q，设T＝{R，t}为拼接地图的近似解，其中R是由旋转角θ所确定的二维旋转矩阵,t为二维的列向量,设{F_i，P，F_i，Q}_i为一组有效特征对，一组特征对中的元素对应空间中的同一个位置，则||Rf_i，P+t-f_i，Q||₂≈0，设变量N₀和r的初值为0，根据RANSAC挑选有效特征对并计算初始拼接参数的步骤如下：

步骤(41)：设置r＝r+1，并随机选取两组特征对i＝m,i＝n；

步骤(42)：将{F_i,P,F_i,Q}_i＝m,n带入下式

$(R_r,t_r)=\underset{R,t}{\arg \min }\underset{p_i\&Element;p_{{\mathrm{\&xi;}}_k}}{\&Sigma;}\left|\right|{\mathrm{Rp}}_i+t-q_{ck\left(i\right)}|{|}_2^2$

计算获得一组新的拼接参数T_r＝{R_r，t_r}；

步骤(43)：根据获得的参数T_r，将地图P中所有的特征点位置转换到地图Q的坐标系下并计算d_i＝||R_rf_i，P+t_r-f_i，Q||₂；

步骤(44)：设定阈值为d_thr，统计满足d_i≤d_thr的锚点特征对的数目N_r，当N_r＞N₀，则设置N₀＝N_r；

步骤(45)：重复步骤(41)-(44)，直到达到r次，最后用找到的N₀个有效特征对来计算初始拼接参数T₀＝{R₀，t₀}。

5.根据权利要求书1所述的一种基于射频识别锚点的数字地图拼接方法，其特征在于，步骤(4)所述的使用ICP算法计算刚体变换参数,其具体步骤如下：

将初始拼接参数T₀＝{R₀，t₀}运用ICP算法中的迭代思想求解刚体变换参数；求解步骤如下：

步骤(51)：基于前一次迭代获得的刚体变换(R_k-1，t_k-1),建立特征点之间的对应关系：

$c_k\left(i\right)=\arg \underset{j\&Element;\{1,2,...,N_q\}}{min}\left|\right|R_{k-1}{p}_i+t_{k-1}-q_j|{|}_2,i=1,2,...,N_p$

选择最近特征点建立特征点对保留特征点p_i与对应特征点之间的欧式距离

步骤(52)：根据当前建立的特征点对应关系，计算重叠百分比ξ_k，并更新相应的子集

$({\mathrm{\&xi;}}_k,p_{{\mathrm{\&xi;}}_k})=\arg \underset{{\mathrm{\&xi;}}_{\min }<\mathrm{\&xi;}\&le;1}{\min }\frac{{\mathrm{\&Sigma;}}_{p_i\&Element;p_{\mathrm{\&xi;}}}\left|\right|R_{k-1}{p}_i+t_{k-1}-q_{c_k\left(i\right)}|{|}_2^2}{\left|p_{\mathrm{\&xi;}}\right|{\mathrm{\&xi;}}^{1+\mathrm{\&lambda;}}}$

为求解上式，采用如下的方式：根据步骤(51)所保留的欧氏距离按升序方式对所有的特征点对进行排序，获得排序队列；采用累加方式，逐个遍历排序后的特征点对并计算步骤(51)中的目标函数值C_k(i)；待遍历完所有的特征点对后，确定最小目标函数值所对应的特征点对在队列中的位置标号I_k以计算当前最优的重叠百分比ξ_k＝(I_k/N_p)，并使用排序队列中的前I_k个特征点对更新子集其中，ξ_k代表待拼接栅格地图的重叠百分比；

步骤(53)：利用更新后的子集使用下式计算刚体变换参数T_k＝{R_k，t_k}

$(R_k,t_k)=\underset{R,t}{\arg \min }\underset{p_i\&Element;p_{{\mathrm{\&xi;}}_k}}{\&Sigma;}\left|\right|{\mathrm{Rp}}_i+t-q_{ck\left(i\right)}|{|}_2^2$

重复执行步骤(51)-(53)，直到满足|ε_k-ε_k-1|＜ε或者迭代次数k达到最大次数K，其中待ICP算法满足迭代停止条件后,即获得精确的刚体变换参数T_k＝{R_k，t_k}，通过刚体变换参数对地图P进行相应的旋转和平移，获得刚体变换后的地图为T(P)。

6.根据权利要求书1所述的一种基于射频识别锚点的数字地图拼接方法，其特征在于，步骤(5)所述的提出针对栅格地图的拼接规则是以像素为单位进行拼接。