CN103983270B - 一种声纳数据的图像化处理方法 - Google Patents

Abstract

本发明公开了一种声纳数据的图像化处理方法，能够有效地应用于移动机器人定位、地图创建及路径规划过程中。首先，对声纳数据进行降噪处理，提取数据中包含的角点信息，并基于误差椭圆、最大误差圆对角点范围加以扩展建立局部拓展地图；然后，将局部拓展地图映射至二值图像空间，建立二值图像空间中的局部拓展地图，通过旋转不变性匹配法，二维扫描匹配法以及SIFT特征匹配的方式实现当前和历史局部拓展地图的匹配。本发明提供了一种新的声纳距离数据处理方式，相对传统直接处理声纳数据的方法，声纳数据的图像化处理能够充分挖掘声纳数据包含的信息，丰富了声纳数据处理的方式，提高了声纳数据处理的精度和鲁棒性。

Description

一种声纳数据的图像化处理方法

技术领域

本发明涉及一种声纳距离数据的处理方法，通过将距离数据映射至图像空间的方式，实现了声纳数据的图像化处理。属于移动机器人导航领域。

背景技术

随着导航技术的发展进步，移动机器人被广泛地使用以辅助人类完成未知环境探测等任务，我国的“玉兔”登月车即为典型的例子。为完成未知环境探测的相关任务，机器人需具备自主导航的功能。一般，自主导航包括三个子问题：1.我在那？2.我要去哪？3.我如何去哪？分别对应机器人定位、地图创建及路径规划。机器人自身位姿及环境特征位置信息的感知是解决上述问题的前提。

目前，移动机器人主要使用视觉传感器、激光传感器以及超声波传感器等获取自身及环境特征的位置信息。其中，视觉传感器获取的信息丰富，但要求机器人具备快速的数据处理能力，另外，视觉传感器对光线、遮挡等干扰敏感，限制了其应用范围。激光传感器和超声波传感器均为距离传感器，通过测量机器人与环境特征间的距离，提供机器人导航所需的信息。激光传感器响应快，获取的信息精度高，但是，激光传感器安装精度要求高，价格昂贵。相对的，超声波传感器安装简单，价格相对较低，能够获取精度相对较高的信息，因此，超声波传感器仍被广泛使用。但是，由于波束角较大，超声波传感器获取的信息存在不确定性。概率理论、模糊理论及灰色***理论等均被用来表达、处理超声波信息，并最终实现机器人地图创建、定位及路径规划。

综观目前的声纳信息处理方法，一般是直接处理声纳获取的距离数据，并通过计算距离数据的统计信息，建立环境特征的形式化描述。典型的例子如环境的栅格化描述方法及特征地图创建方法。受波束角影响，声纳获取的距离信息不可避免的带有误差。目前的声纳距离数据处理方法最大限度的利用了数据包含的统计信息，并在移动机器人自主导航中取得了成功应用。但是，当误差较大时，统计信息的准确度会受到影响，同时，现存方法也很难进一步从原始数据中挖掘有用信息。为此，本文将声纳距离数据从距离空间映射至图像空间，利用图像处理的相关技术方法实现声纳距离信息的处理。该方法可有效应用于移动机器人定位、地图创建及路径规划过程中.

发明内容

本发明的目的是将声纳的距离数据映射至图像空间，利用图像处理技术实现声纳距离数据的处理，通过图像化的处理，最大限度的挖掘声纳数据包含的环境信息，本发明一方面提供了声纳数据处理的新思路，另一方面，通过多重声纳数据的图像化匹配技术，提高了声纳数据处理的精度及鲁棒性。

本发明提供一种声纳数据的图像化处理方法，通过将声纳距离信息映射至图像空间，利用图像处理的方法，实现声纳距离信息的处理，主要包括以下步骤：

步骤1、过滤由于测量盲区或超出声纳测量范围而产生的测量值(若记声纳数据为(x,y,θ，ra)，其中(x,y)表示目标的坐标，θ为目标相对机器人的方位，ra为目标到机器人的距离。不符合要求的数据标记为ra＝R，其中R为声纳传感器的最大测量距离，本发明中R＝5000mm)，记过滤后的数据集为S。

步骤2、过滤奇异值。本发明中，奇异值指不代表任何实际存在特征的、相对稀疏的测量值。给定数据集S，分别计算S中所有点两两之间的欧式距离，并根据坐标和距离的大小对S中的点进行分类。统计每个分类包含点的个数，当分类中点的个数小于阈值Num时，去掉相应类中的所有点，Num为事先设定的阈值。记过滤奇异值后的数据集为S₀，本发明称S₀为距离数据空间，简称为距离空间。

步骤3、提取角点。定义长度为N，滑动步长为s，滑动方向为的滑动窗口。沿方向，从S₀中依次取N个点，记其横纵坐标分别为X_t＝[x₁,x₂,...,x_N]和Y_t＝[y₁,y₂,...,y_N]，则X_t、Y_t的协方差矩阵为：

$C_t=\left[\begin{array}{cc}\mathrm{cov}(X_t,X_t)& \mathrm{cov}(X_t,Y_t)\\ \mathrm{cov}(Y_t,X_t)& \mathrm{cov}(Y_t,Y_t)\end{array}\right],---\left(1\right)$

其中： 和分别为X_t和Y_t元素的均值。记C_t的特征值为λ_max和λ_min，其比值为EVR＝λ_min/λ_max。随着窗口的滑动，可计算得到S₀的所有EVR值，得到当前的EVR曲线。在角点处，EVR会达到极值。本发明通过比较相邻EVR值的方式计算当前EVR曲线的峰值，每个峰值对应一个角点。

步骤4、计算误差椭圆。计算当前EVR曲线的峰值，本步骤以峰值O为例，详细阐述误差椭圆的计算过程如下：计算O对应的声纳数据点(x,y)，然后取以(x,y)为圆心，r'为半径范围内的n个数据点，其横纵坐标分别记为X＝[x₁,...,x_n]和Y＝[y₁,...,y_n]。若记X、Y的协方差阵为C，C的特征值为λ₁和λ₂(λ₁≥λ₂)，相应的特征向量为v₁和v₂，则误差椭圆为v₁和v₂组成的坐标系中以为圆心，λ₁和λ₂分别为长轴和短轴的椭圆，其中

步骤5、计算最大误差圆，建立局部拓展地图。以为圆心，λ₁为半径，建立误差椭圆对应的最大误差圆。根据角点的实际连接情况，计算实际相连角点对应的最大误差圆的公共外切线，进一步计算切线与最大误差圆的切点，连接实际连接角点对应最大误差圆的切点构成局部拓展地图M，所有最大误差圆的圆心、切点构成局部拓展地图M的关键点集im_point。

步骤6、映射至图像空间。记im_point包含点的横纵坐标分别为P＝(p₁,...,p_m)，Q＝(q₁,...,q_m)，p_max和q_max分别表示横纵坐标的最大值，p_min和q_min分别为横纵坐标的最小值。则im_point中的一点(p_i,q_i)通过公式(2)和公式(3)映射至图像空间中的一点(h_i,k_i)：

$h_i=\mathrm{int}[\mathrm{\η}(p_{\max }-p_{\min })\·\frac{(p_i-p_{\min })}{p_{\max }{-p}_{\min }}],---\left(2\right)$

$k_i=\mathrm{int}[\mathrm{\η}(q_{\max }-q_{\min })\·\frac{(q_i-q_{\min })}{q_{\max }-q_{\min }}],---\left(3\right)$

其中，η为比例系数，η与声纳数据及图像空间的尺度相关。

步骤7、记(h_i,k_i)为im_point中圆心的像，r_i为相应圆的半径，i＝1,...,j，j为圆心个数。以(h_i,k_i)为圆心，r_i为半径确定圆形区域，令该圆形区域以及im_point中切点的像确定的区域内的像素值为0，反之为1。记该二值图像空间中像素值为0的区域为图像空间中的局部拓展地图p_M。

步骤8、位置匹配。位置匹配主要包括两个方面：旋转不变性匹配和二维扫描匹配。

步骤A、旋转不变性匹配。

步骤A1、记p_M水平和垂直方向像素点之间距离的最大值分别为h_max和v_max，本发明定义P_c(int(h_max/2),int(v_max/2))为p_M的中心点。以P_c为坐标原点，沿p_M的水平和垂直方向建立坐标系Σ_c；

步骤A2、令D＝even((h_max/2)²+(v_max/2)²)^1/2，“even”表示((h_max/2)²+(v^max/2)²)^1/2向上取偶数。在坐标系Σ_c中，以P_c为圆心，分别作半径为r和r+Δr的同心圆，构成同心圆环R_r。R_r包含的像素个数记为N_r，其中被p_M占据的像素数记为U_r，其比值记为v(r)＝U_r/N_r，称为半径r对应的有效占空比。依此，计算得原点到D/2范围内所有的有效占空比，构成有效占空比向量V＝[v(0)，v(Δr)...v(r)...v(D/2)]^T。

步骤A3、设当前和历史p_M的有效占空比向量分别为V_l＝[v_l(0)，v_l(Δr)...v_l(r)...v_l(D/2)]^T和V_d＝[v_d(0)，v_d(Δr)...v_d(r)...v_d(D/2)]^T，则V_l和V_d的匹配率为：

$p_{\mathrm{rpt}}=\frac{{\mathrm{\Σ}}_{r=0}^{D/2}\left\{\right[v_l\left(r\right)-{\mathrm{\υ}}_l\left]\right[v_d\left(r\right)-{\mathrm{\υ}}_d\left]\right\}}{\sqrt{\left\{{\mathrm{\Σ}}_{r=0}^{D/2}{[v_l\left(r\right)-{\mathrm{\υ}}_l]}^2{\mathrm{\Σ}}_{r=0}^{D/2}{[v_d\left(r\right)-{\mathrm{\υ}}_d]}^2\right\}}}---\left(4\right)$

其中， ${\mathrm{\υ}}_l={\mathrm{\Σ}}_{r=0}^{D/2}{v}_l\left(r\right)/(1+D/2\mathrm{\Δr}),{\mathrm{\υ}}_d={\mathrm{\Σ}}_{r=0}^{D/2}{v}_d\left(r\right)/(1+D/2\mathrm{\Δr}).$

步骤B、二维扫描匹配。

步骤B1、依据步骤A1提到的坐标系建立方法，分别建立当前p_M和历史p_M的坐标系；

步骤B2、记当前二值图像空间中图片水平和垂直方向的像素数为N，当前p_M占据的第i行和第i列的像素数分别N_ri和N_ci，则第i行和列的有效占空比分别记为w_ri＝N_ri/N和w_ci＝N_ci/N。一般，由于当前p_M和历史p_M的朝向不同，相同序号行列的占空比一般不同。为此，本发明依据以下规则实现历史p_M和当前p_M的匹配：

2.1固定当前p_M，记其第i行和列的有效占空比为和计算其所有行列的有效占空比，生成当前p_M的有效占空比向量和 $W_c^l=[w_{c1}^l,w_{c2}^l,...,w_{\mathrm{cN}}^l{]}^T;$

2.2同2.1，记历史p_M的有效占空比向量分别为和 $W_c^d=[w_{c1}^d,w_{c2}^d,...,w_{\mathrm{cN}}^d{]}^T;$

2.3利用公式(4)分别计算两个维度的标准相关系数和。历史p_M逆时针旋转1度，重新计算当前p_M与历史p_M的标准相关系数，记为和。依次，分别计算两个维度一个周期内的360组标准相关系数，生成两个维度各自的相关系数向量：和 ${\mathrm{\λ}}_c={[{\mathrm{\λ}}_c^0,{\mathrm{\λ}}_c^1,...,{\mathrm{\λ}}_c^{359}]}^T.$

步骤B3、如果当前p_M和历史p_M表示相同的局部环境，则相关系数向量λ_r和λ_c中对应的若干连续元素将超过设定的阈值λ_th；反之，λ_r和λ_c的元素均小于λ_th或只有若干离散元素超过阈值。取λ_r和λ_c中包含的超过阈值λ_th的元素的均值，分别记为和则当前p_M与历史p_M的匹配率

步骤9、图像匹配。利用SIFT算子分别提取当前p_M和历史p_M的兴趣点，并进行图像匹配，记匹配率为P_p，本发明中P_p指正确匹配的兴趣点对数与所有匹配兴趣点对数的比值。

步骤10、匹配率融合。本发明中规定最终的匹配率为P_f＝αp_rpt+βp_2d+γp_p，其中α、β和γ分别为各个匹配率的权重，满足α+β+γ＝1。α、β和γ的取值根据实际应用中各个方法的匹配精度决定。如果P_f不满足阈值要求，则存储当前p_M；反之，则可利用匹配信息进行移动机器人定位、地图创建及路径规划等任务。

本发明具有以下有益效果

1、将声纳距离数据映射至图像空间，利用图像处理技术处理声纳距离数据，提供了一种新的声纳数据处理思路。

2、通过建立局部拓展地图，提高了声纳数据处理的鲁棒性。

3、提出了三种声纳数据图像化匹配方法，三种方法互相补充，提高了匹配精度。

4、声纳数据的图像化处理可以有效应用于移动机器人定位、地图创建及路径规划过程中，提高机器人导航任务的精度及鲁棒性。

附图说明

图1为声纳数据图像化处理算法流程图；

图2(a)、图2(b)分别为数据集S和S₀，图2(b)同时展示了误差椭圆和最大误差圆；

图3(a)、图3(b)分别为旋转不变性匹配方法示意图和旋转不变性匹配方法匹配结果示意图；

图4(a)、图4(b)分别为二维扫描匹配方法示意图和二维扫描匹配方法匹配结果示意图；

图5(a)、图5(b)分别为当前p_M和历史p_M兴趣点提取结果示意图；

图6(a)为当前和历史p_M SIFT兴趣点匹配效果的示意图，图6(b)为当前p_M兴趣点自匹配效果的示意图。

具体实施方式

本实施例在以发明技术方案为前提下进行实施，给出了详细的实施方案和过程，但本发明的实施范围不限于下述的实施例。

本实施例利用机器人Pioneer3-DX装备的16个声纳传感器在走廊-办公室环境下采集数据，利用Visual Studio2008，OpenCV-1.0.0以及Matlab R2009a混合编程实现声纳数据的图像化处理，算法流程如图1所示。本发明具体执行步骤如下：

(1)利用机器人声纳采集当前局部环境数据，分离ra＝5000mm的声纳数据点，分离结果如图2(a)所示，过滤声纳数据中的奇异值，建立距离空间S₀，如图2(b)所示；

(2)本实施例中取N＝10，s＝1，X_t＝[x₁,x₂,...,x₁₀]，Y_t＝[y₁,y₂,...,y₁₀]。首先，通过公式(1)计算X_t和Y_t的协方差矩阵C_t，然后，利用Matlab中提供的eig()函数，计算C_t的特征向量及相应特征值，进一步计算EVR＝λ_min/λ_max。

(3)沿以步长s滑动窗口，至窗口中数据小于N。重复步骤(2)，计算所有窗口对应的EVR，构成当前距离空间S₀的EVR曲线。

(4)比较相邻的EVR值，计算当前EVR曲线的峰值。假设O为其中的一个峰值，O对应的声纳数据点为(x,y)。取以(x,y)为圆心，r'为半径的圆内的n个数据点，其坐标记为X＝[x₁,...,x_n]和Y＝[y₁,...,y_n]。计算X、Y的协方差阵C，C的特征值λ₁和λ₂以及相应的特征向量v₁和v₂。在v₁和v₂构成的坐标系中，以为圆心，λ₁和λ₂为长轴和短轴确定误差椭圆，其中以为圆心，λ₁为半径，确定最大误差圆。

(5)根据步骤(4)计算当前EVR曲线所有峰值对应的最大误差圆。根据角点的实际连接情况，计算实际相连角点对应的最大误差圆的公共外切线，进一步计算切线与最大误差圆的切点，构建局部拓展地图M，确定M(如图3(a)和图4(a)所示)的关键点集im_point。

(6)记im_point包含点的横纵坐标分别为P＝(p₁,...,p_m)和Q＝(q₁,...,q_m)，确定p_max和q_max，以及p_min和q_min。通过公式(2)和公式(3)将im_point中的点(p_i,q_i)映射至图像空间中的点(h_i,k_i)，本发明中η＝0.02。

(7)确定(h_i,k_i)，r_i，i＝1,...,j。以(h_i,k_i)为圆心，r_i为半径确定圆形区域。令由该圆形区域以及im_point切点的像确定的区域内的像素值为0，反之为1，确定图像空间中的局部拓展地图p_M(如图5(a)和图5(b)所示)。

(8)位置匹配。

步骤A、旋转不变性匹配。

步骤A1、计算p_M水平和垂直方向像素点之间距离的最大值h_max和v_max，进一步计算p_M的中心点P_c(int(h_max/2),int(v_max/2))，建立坐标系Σ_c，如图3(a)所示；

步骤A2、首先计算D＝even((h_max/2)²+(v_max/2)²)^1/2，然后计算当前和历史p_M的有效占空比向量V_l＝[v_l(0)，v_l(Δr)...v_l(r)...v_l(D/2)]^T和V_d＝[v_d(0)，v_d(Δr)...v_d(r)...v_d(D/2)]^T，通过公式(4)计算V_l和V_d的匹配率p_rpt(如图3(b)所示)。

步骤B、二维扫描匹配。

步骤B1、依据步骤A1提出的坐标系建立方法，分别建立当前p_M和历史p_M的坐标系，如图4(a)所示；

步骤B2、计算当前p_M的有效占空比向量和

步骤B3、计算历史p_M的有效占空比向量和

步骤B4、利用公式(4)计算两个维度的标准相关系数向量和 ${\mathrm{\λ}}_c={[{\mathrm{\λ}}_c^0,{\mathrm{\λ}}_c^1,...,{\mathrm{\λ}}_c^{359}]}^T.$

步骤B5、计算P_2d(如图4(b)所示)。

(9)利用SIFT算子分别提取当前p_M和历史p_M的兴趣点(如图5(a)和图5(b)所示)，并进行图像匹配(如图6(a)和图6(b)所示)，记匹配率为P_p。

(10)计算最终的匹配率P_f＝αp_rpt+βp_2d+γp_p。本发明中多次试验结果表明，三种匹配算法的精度高低依次为：二维扫描匹配算法，旋转不变性匹配算法及图像匹配算法，因此本实验中取α＝0.33，β＝0.37，γ＝0.30。如果P_f不满足阈值要求，则存储当前p_M，反之，则可利用匹配信息进行移动机器人定位、地图创建及路径规划等任务。

Claims (11)

1.一种声纳数据的图像化处理方法，其特征在于具体包括以下步骤：

(1)过滤由于声纳盲区或超出声纳测量范围而产生的测量值；

(2)通过聚类的方法过滤声纳数据中的奇异值和离散点；

(3)计算声纳测量值的EVR曲线，通过检测当前EVR曲线峰值的方式获取声纳数据包含的环境角点信息；

(4)计算声纳数据包含的每个角点特征的误差椭圆；

(5)计算每个误差椭圆的最大误差圆，构建当前局部拓展地图M；

(6)建立当前局部拓展地图M与二值图像空间的映射关系；

(7)将M映射至二值图像空间，构建图像空间中的局部拓展地图p_M；

(8)利用旋转不变性匹配方法和二维扫描匹配方法实现局部拓展地图的匹配；

(9)利用SIFT算子提取兴趣点，并进行图像匹配；

(10)进行旋转不变性匹配方法、二维扫描匹配方法及图像匹配方法匹配结果的融合。

2.如权利要求1所述的方法，其特征在于步骤(1)包括：记声纳数据为(x,y,θ，ra)，其中(x,y)为目标坐标，θ为目标相对机器人的方位，ra为目标到机器人的距离，由于测量盲区或超出声纳测量范围的影响，会产生不符合要求的数据，标记为ra＝R，其中R为声纳传感器的最大测量距离，去除所有ra＝R的点，过滤后的数据集记为S。

3.如权利要求2所述的方法，其特征在于步骤(2)包括：计算声纳数据集S中所有点两两之间的欧式距离，并根据坐标及距离的大小对S中的点进行分类，统计每个分类包含点的个数，当某分类中点的个数小于阈值Num时，去掉相应类中的所有点，剩余点构成距离空间S₀。

4.如权利要求3所述的方法，其特征在于步骤(3)包括：定义长度为N，滑动步长为s，滑动方向为的滑动窗口，沿方向，从S₀中依次取N个点，其坐标分别记为X_t＝[x₁,x₂,…,x_N]和Y_t＝[y₁,y₂,…,y_N]，通过：

$C_t=\left[\begin{array}{cc}\mathrm{cov}(X_t,X_t)& \mathrm{cov}(X_t,Y_t)\\ \mathrm{cov}(Y_t,X_t)& \mathrm{cov}(Y_t,Y_t)\end{array}\right]$

计算X_t和Y_t的协方差矩阵C_t，其中 和分别为X_t和Y_t元素的均值，记C_t特征值的比值EVR＝λ_min/λ_max，λ_max和λ_min为C_t的特征值；沿以步长s滑动窗口，至窗口中数据小于N，计算得到当前的EVR曲线，通过比较相邻EVR值的方式计算当前EVR曲线的峰值，每个峰值对应一个角点。

5.如权利要求1所述的方法，其特征在于步骤(4)所述的计算误差椭圆具体包含如下内容：

假设O为其中的一个峰值，O对应的声纳数据点为(x,y)，取以(x,y)为圆心，r'为半径的圆内的n个数据点，其坐标记为X＝[x₁,…,x_n]和Y＝[y₁,…,y_n]，计算X和Y的协方差阵C，C的特征值为λ₁和λ₂(λ₁≥λ₂)，相应的特征向量为v₁和v₂，则误差椭圆为v₁和v₂组成的坐标系中以为圆心，λ₁、λ₂分别为长轴和短轴的椭圆，其中

6.如权利要求5所述的方法，其特征在于步骤(5)所述的构建当前局部拓展地图M，具体包含如下内容：

以为圆心，λ₁为半径，构建误差椭圆对应的最大误差圆，根据角点的实际连接情况，计算实际相连角点对应的最大误差圆的公共外切线，进一步计算切线与最大误差圆的切点，连接实际相连角点对应最大误差圆的切点构成局部拓展地图M，所有最大误差圆的圆心、切点构成局部拓展地图M的关键点集im_point。

7.如权利要求6所述的方法，其特征在于步骤(6)所述的建立M与二值图像空间的映射关系包括：

记im_point包含点的横坐标和纵坐标分别为P＝(p₁,…,p_m)和Q＝(q₁,…,q_m)，p_max和q_max分别为横纵坐标的最大值，p_min和q_min分别为横纵坐标的最小值，则im_point中的一点(p_i,q_i)通过公式和映射至图像空间中的一点(h_i,k_i)，其中，η为比例系数。

8.如权利要求7所述的方法，其特征在于步骤(7)所述的建立图像空间中的局部拓展地图包括以下内容：

记im_point中圆心的像和相应的半径分别为(h_i,k_i)和r_i，i＝1,…,j，j为圆心个数，以(h_i,k_i)为圆心，r_i为半径确定圆形区域，令该圆形区域以及im_point中切点的像确定的区域内的像素值为0，反之为1，记该二值图像空间中像素值为0的区域为图像空间中的局部拓展地图p_M。

9.如权利要求1所述的方法，其特征在于步骤(8)包括：

(8.1)利用旋转不变性匹配方法匹配当前p_M及历史p_M，具体包括如下内容：

(i)计算p_M水平和垂直方向像素点之间距离的最大值h_max和v_max，进一步计算p_M的中心点P_c(int(h_max/2),int(v_max/2))，沿水平和垂直方向建立坐标系∑_c，在坐标系∑_c中，以P_c为圆心，分别作半径为r和r+Δr的同心圆，构成同心圆环R_r，R_r包含的像素个数记为N_r，其中被p_M占据的像素数记为U_r，其比值记为v(r)＝U_r/N_r，称为半径r对应的有效占空比；

(ii)计算D＝even((h_max/2)²+(v_max/2)²)^1/2，“even”表示((h_max/2)²+(v_max/2)²)^1/2向上取偶数，然后计算当前和历史p_M的有效占空比向量V_l和V_d，并通过计算V_l和V_d的匹配率p_rpt，其中，V_l＝[v_l(0)，v_l(Δr)…v_l(r)…v_l(D/2)]^T，V_d＝[v_d(0)，v_d(Δr)…v_d(r)…v_d(D/2)]^T，

(8.2)利用二维扫描匹配方法匹配当前p_M及历史p_M，具体包括如下内容：

(i)依据(8.1)中的坐标系建立方法，分别建立当前p_M和历史p_M的坐标系；

(ii)记当前二值图像空间中图片水平和垂直方向的像素数为N，被p_M占据的第i行和第i列的像素数分别N_ri和N_ci，则第i行和列的有效占空比分别记为w_ri＝N_ri/N和w_ci＝N_ci/N，分别计算当前p_M所有行列的有效占空比构成有效占空比向量和同理，计算历史p_M的有效占空比向量和

(iii)分别计算当前和历史p_M行列有效占空比向量的标准相关系数及最终匹配度P_2d。

10.如权利要求9所述的方法，其特征在于步骤(9)包括如下内容：利用SIFT算子分别提取当前p_M和历史p_M的兴趣点，进行图像匹配，计算匹配率P_p。

11.如权利要求10所述的方法，其特征在于步骤(10)包括：

计算最终的匹配率P_f＝αp_rpt+βp_2d+γp_p，其中α+β+γ＝1，α，β及γ的取值根据各匹配方法的精度而定，如果P_f不满足阈值要求，则存储当前p_M，反之，则可利用匹配信息进行移动机器人定位、地图创建及路径规划任务。