CN109100731B - 一种基于激光雷达扫描匹配算法的移动机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达扫描匹配算法的移动机器人定位方法，包括1.分别对激光雷达相邻两采样周期的扫描数据进行角点特征提取，得到参考角点特征点集和待配准角点特征点集；2.对参考角点特征点集和待配准角点特征点集进行关联特征点配对得到初步配对点集；3.求解参考角点特征点集和待配准角点特征点集之间的整体匹配参数旋转矩阵和平移矩阵；4.对设置匹配阈值，滤除旋转矩阵和平移矩阵之间的无效关联特征；5.计算机器人在相邻两采样周期内的运动增量以及完成当前时刻机器人位姿估计。本发明通过对室内结构化特征进行分析，以环境中存在的角点特征代替传统ICP算法中的原始激光雷达扫描数据点作为改进型ICP算法的输入数据，提高匹配算法效率。

Description

一种基于激光雷达扫描匹配算法的移动机器人定位方法

技术领域

本发明属于移动机器人导航定位技术领域，具体为一种实现移动机器人自主定位的方法，适用室内结构化场景中根据环境特征匹配信息对移动机器人进行定位。

背景技术

定位技术是移动机器人实现自主导航中最基础的环节，是其实现路径规划、控制决策和执行任务的先决保证条件，对于运动目标跟踪、机器人导航、地图生成等具有重要意义。

定位问题可以描述为机器人通过传感器感知环境或自身运动状态，经过合理的数学模型和算法处理，最终得到自身在工作环境中的精确位置。在一般环境中，移动机器人的位姿通常使用三维向量来表示，即相对于全局坐标位置的横向、纵向平移分量和代表其朝向的旋转角度分量。

常用于定位中的传感器主要有惯性传感器、视觉传感器、激光雷达、超声波等，由于激光雷达具有测距范围大、精度高、环境适应能力强等特点，因此基于激光雷达扫描匹配算法的定位方法已逐渐成为移动机器人定位研究中的热点，该类方法通过匹配相邻两帧的激光雷达环境扫描距离数据估计出相邻采样周期间的相对位姿增量，对其进行叠加得到机器人当前位姿估计，其本质是搜索能够使相邻两帧激光雷达扫描数据之间的最佳匹配的变换关系。

在扫描匹配算法中，迭代最近点算法(ICP)在精度和鲁棒性方面都有着良好的表现，因此是激光雷达扫描匹配的主流算法，该类方法中比较典型的有：基于点-点特征、点-特征、特征-特征的关联匹配方法。

其中基于点-点的关联匹配方法输入数据简单，但数据量太大，算法收敛速度慢，算法实时性易受影响；基于点-特征的匹配方法利用点到直线的距离特征进行匹配，在室内结构化明显的简单环境中效果较好，但是在仓库通道等线段相似特征较多的复杂环境下易出现“一对多”特征误匹配现象；基于特征-特征的匹配方法是近年来比较流行的一种匹配方法，该类方法对特征提取的准确度和精度有较高的依赖性，但不同的方法又有各自的针对性和适用场所；除此之外，在特征关联匹配过程中，传统ICP算法默认待匹配的两组数据完全重合或一组扫描数据是另一组扫描数据的严格子集，而在实际应用中，由于机器人运动和激光传感器测量误差的存在，根本不存在完全符合上述条件的点云集合，即两组数据是近似关联的，从而求解出的两组数据的变换参数是某种准则下的整体最优匹配，并不是严格意义上的完全对应点对匹配，在这一过程中，由于部分数据点没有关联点存在，如果对所有数据进行关联匹配，则会把一些无效点当成有效点来匹配，上述问题的存在会影响到算法的精度。

因此，急需一种在室内环境中适应性好，实时性和精度都较高的移动机器人自主定位方法。

发明内容

有鉴于此，为了解决上述问题，本发明提供一种基于激光雷达扫描匹配算法的移动机器人定位方法。此方法为工程上实现移动机器人在室内环境中的自主定位提供了一种解决方案，方法简单易行、实时性好、精度高，室内环境下具有较好的适用性。

为实现上述目的及其他目的，本发明提供一种基于激光雷达扫描匹配算法的移动机器人定位方法，该方法包括以下步骤：

步骤1.分别对激光雷达相邻两采样周期的扫描数据进行角点特征提取，得到参考角点特征点集P和待配准角点特征点集Q；

步骤2.对参考角点特征点集P和待配准角点特征点集Q进行关联特征点配对得到初步配对点集M；

步骤3.求解参考角点特征点集P和待配准角点特征点集Q之间的整体匹配参数旋转矩阵R和平移矩阵T；

步骤4.对设置匹配阈值，滤除旋转矩阵R和平移矩阵T之间的无效关联特征；

步骤5.计算机器人在相邻两采样周期内的运动增量以及完成当前时刻机器人位姿估计。

优选地，所述对参考角点特征点集P和待配准角点特征点集Q进行关联特征点配对得到初步配对点集M具体为：

利用点到点的欧式距离，对于所有的点q_i∈Q，在参考角点特征点集P中找到距离点q_i最近的点p_i组成对应点对(q_i,p_i)，得到初步配对点集M＝{(mq_i,mp_i)},其中i＝1,2,...,n。

优选地，采用分步迭代的方法求解参考角点特征点集P和待配准角点特征点集Q之间的整体匹配参数旋转矩阵R和平移矩阵T。

优选地，所述步骤3具体包括以下步骤：

Step 31：对初步配对点集M中的集合MQ和集合MP分别进行去中心化处理：

mq_i′＝mq_i-C_mq

mp_i′＝mp_i-C_mq

其中，MQ＝{mq_i∈Q|i＝1,2,...,n}，MP＝{mp_i∈P|i＝1,2,...,n}，

Step 32：根据Step 31获得目标评价函数E′(R,T)：

当目标评价函数E′(R,T)的值最小时所对应的R为所求旋转矩阵R，其中：

上式中，Δθ表示在相邻采样周期内小车方向变化角度；

Step 33：将求解出的旋转矩阵R带入下式求出平移矩阵T：

T＝MP-R×MQ。

优选地，所述分步迭代的方法具体包括：

计算中间转换矩阵：将矩阵MQ′带入整体匹配参数旋转矩阵R计算得到MQ″＝R×MQ′，将整体匹配参数旋转矩阵R和平移矩阵T带入得集合MQ中得：MQ″′＝R×MQ+T；其中，MQ′代表去中心化的待配准角点特征集；

迭代终止条件设置：设最大迭代次数为num_max，旋转矩阵R和平移矩阵T最小判别阈值分别为thd_R和thd_T；设去中心化的参考角点集合MP′和经过一次旋转变换的中间待配准点集MQ″之间的最小二乘误差为

集合MP与MQ″′之间的最小二乘误差为

MQ″′表示初始待配准点集经过整体旋转和平移变换后的新的待配准点集；

迭代终止判断：

当迭代次数num_i<num_max时,若d_{min_R}＜thd_R并且d_{min_T}＜thd_T，返回(R,T)结束迭代；

否则，Q＝MQ″′，返回步骤2继续进行迭代；

当num_i≥num_max时，返回(R,T)结束迭代。

优选地，所述步骤4的具体方法为：

利用整体匹配时得到的参数(R,T)，对集合MQ进行转换得MQ1＝R×MQ+T，对集合MQ1和MP进行关联特征点配对，得到配对集合M1，对于(mq_i,mp_i)∈M1，若||mq_i-mp_i||＞D_max，将(mq_i,mp_i)从集合M1中剔除，其中D_max表示两关联点之间的最大距离阈值。

优选地，所述步骤5具体包括：

利用(R,T)可以计算出小车在相邻两采样周期内的运动增量[Δx,Δy,Δθ]^T：

其中，Δx,Δy表示相邻两采样周期内小车在全局坐标系下分别沿水平方向和竖直方向的运动增量，Δθ表示在该周期内机器人方向变化角度；

设当前时刻机器人在环境中的位姿为X_t＝[x_t,y_t,θ_t]^T，前一时刻位姿为X_t-1＝[x_t-1,y_t-1,θ_t-1]^T，结合

得到

优选地，该定位方法还包括：精确求取变换参数(R,T)。

由于采用了上述技术方案，本发明具有如下的优点：

1、本发明在数据输入方面，通过对室内结构化特征进行分析，以环境中存在的角点特征(即：墙角、物体轮廓直角)代替传统ICP算法中的原始激光雷达扫描数据点作为改进型ICP算法的输入数据，大大减少了输入数据量，提高匹配算法效率。

2、本发明在求解待匹配两组激光雷达扫描数据的变化参数和位移增量过程中，设置阈值，剔除没有关联点的无效特征数据，提高算法精度。

3、对两组扫描数据进行旋转矩阵参数R和平移矩阵T的求解过程中，对整体方程进行分解，采用分步迭代的方法分别对参数R和T进行求解，增强算法的鲁棒性。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述：

图1为机器人定位整体流程图；

图2为匹配参数R和T求解流程图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅图1至图2。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

本实施例的一种基于激光雷达扫描匹配算法的移动机器人定位方法方法，系统整体流程图如图1所示，包括如下步骤：

Step1：对激光雷达环境扫描数据进行角点特征提取。

设置激光雷达扫描匹配算法的采样周期为T,利用角点特征提取算法分别对激光雷达相邻两采样周期的扫描数据进行角点特征提取得到：

t+T-1时刻参考角点特征点集：P＝{(x_i,y_i)|i＝1,2,...,m}；

t+T时刻待配准角点特征点集：Q＝{(x_j,y_j)|j＝1,2,...,n}；

Step2：对相邻两帧扫描数据进行关联特征点配对。

利用点到点的欧式距离，对于所有的点q_i∈Q，i＝1,2,...,n，在参考角点特征点集P中找到距离其最近的点p_i组成对应点对(q_i,p_i)，得到初步配对点集M＝{(mq_i,mp_i)|i＝1,2,...,n}；

Step3：采用分步迭代的方法求解相邻两幅雷达扫描数据的整体匹配参数旋转矩阵R和平移矩阵T，主要包括以下3个部分：

Step 31：去中心化；设配对点集M中的集合MQ＝{mq_i∈Q|i＝1,2,...,n}和集合MP＝{mp_i∈P|i＝1,2,...,n}的中心点分别为C_mq和C_mp，其中：

对集合MQ和集合MP分别进行中心化处理：

mq_i′＝mq_i-C_mq (3)

mp_i′＝mp_i-C_mp (4)

Step 32：旋转矩阵R求解；在求取R和T参数的过程中，传统ICP算法中的目标评价函数为：

将式(1)-(4)带入上式得到新的目标评价函数为：

由式(6)可知，在对目标函数求取过程中，中心化的数据只需对旋转参数R进行求解，使评价函数值E′(R,T)最小的R值即为目标值；

对式(6)中的目标函数作进一步分解得：

从式(7)中可知，要想使E′(R,T)的值最小，只需使

的值最大，设：

令

设U和V分别为它的奇异值分解矩阵，根据已有证明可知，当且仅当R＝VU^T时，f(R)值最大，从而求解出旋转矩阵参数R；

Δθ表示在相邻采样周期内小车方向变化角度；

Step33：平移矩阵T求解；将求解出的矩阵参数R带入下式求出平移参数T：

T＝MP-R×MQ (9)

整体匹配迭代条件判断，主要包括以下3个部分：

计算中间转换矩阵；将矩阵MQ′带入整体匹配参数参数旋转矩阵R计算得到MQ″＝R×MQ′，将参数R和T带入得集合MQ中得：MQ″′＝R×MQ+T；

迭代终止条件设置；设最大迭代次数为num_max，旋转参数R和平移参数T最小判别阈值分别为thd_R和thd_T；设去中心化的参考角点集合MP′和经过一次旋转变换的中间待配准点集MQ″之间的最小二乘误差为

同理，集合MP与MQ″′之间的最小二乘误差为

MQ″′表示初始待配准点集经过整体旋转和平移变换后的新的待配准点集；

迭代终止判断；

1)迭代次数num_i<num_max时，若d_{min_R}＜thd_R并且d_{min_T}＜thd_T，返回(R,T)结束迭代；否则，Q＝MQ″′，返回步骤2继续进行迭代；

2)num_i≥num_max时，返回(R,T)结束迭代；

Step4：设置匹配阈值，滤除无效关联特征。

利用整体匹配时得到的最终参数(R,T)，对集合MQ进行转换得MQ1＝R×MQ+T，对集合MQ1和MP进行步骤2中关联特征点配对，得到配对集合M1，对于(mq_i,mp_i)∈M1，若||mq_i-mp_i||＞D_max，将(mq_i,mp_i)从集合M1中剔除，其中D_max表示两关联点之间的最大距离阈值；

Step 5：精确求取变换参数(R,T)。

通过步骤5中的无效关联点滤除，得到比较精准的配对点集M2，然后返回步骤2中，继续往下计算，最终得到比较精确的变换参数(R,T)，具体流程图如图2所示。

Step 6：计算机器人在相邻两采样周期内的运动增量[Δx,Δy,Δθ]^T，完成当前时刻机器人位姿估计。

利用(R,T)可以计算出小车在相邻两采样周期内的运动增量[Δx,Δy,Δθ]^T：

其中，Δx，Δy表示相邻两采样周期内小车在全局坐标系下分别沿水平方向和竖直方向的运动增量，Δθ表示在该周期内机器人方向变化角度。

设当前时刻机器人在环境中的位姿为X_t＝[x_t,y_t,θ_t]^T，前一时刻位姿为X_t-1＝[x_t-1,y_t-1,θ_t-1]^T，结合式(11)得到：

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的保护范围当中。

Claims (5)

1.一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，该方法包括以下步骤：

步骤1.分别对激光雷达相邻两采样周期的扫描数据进行角点特征提取，得到参考角点特征点集P和待配准角点特征点集Q；

步骤2.对参考角点特征点集P和待配准角点特征点集Q进行关联特征点配对得到初步配对点集M，具体为：利用点到点的欧式距离，对于所有的点q_i∈Q，在参考角点特征点集P中找到距离点q_i最近的点p_i组成对应点对(q_i,p_i)，得到初步配对点集M＝{(mq_i,mp_i)}，其中i＝1,2,...,n；

步骤3.采用分步迭代的方法求解参考角点特征点集P和待配准角点特征点集Q之间的整体匹配参数旋转矩阵R和平移矩阵T，具体包括以下步骤：

Step31：对初步配对点集M中的集合MQ和集合MP分别进行去中心化处理：

mq_i′＝mq_i-C_mq

mp_i′＝mp_i-C_mq

其中，MQ＝{mq_i∈Q|i＝1,2,...,n}，MP＝{mp_i∈P|i＝1,2,...,n}，

Step32：根据Step31获得目标评价函数E′(R,T)：

当目标评价函数E′(R,T)的值最小时所对应的R为所求旋转矩阵R，其中：

上式中，Δθ表示在相邻采样周期内小车方向变化角度；

Step33：将求解出的旋转矩阵R带入下式求出平移矩阵T：

T＝MP-R×MQ；

步骤4.对设置匹配阈值，滤除旋转矩阵R和平移矩阵T之间的无效关联特征；

步骤5.计算机器人在相邻两采样周期内的运动增量以及完成当前时刻机器人位姿估计。

2.根据权利要求1所述的一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，所述分步迭代的方法具体包括：

计算中间转换矩阵：将矩阵MQ′带入整体匹配参数旋转矩阵R计算得到MQ″＝R×MQ′，将整体匹配参数旋转矩阵R和平移矩阵T带入得集合MQ中得：MQ″′＝R×MQ+T；其中，MQ′代表去中心化的待配准角点特征集；

迭代终止条件设置：设最大迭代次数为num_max，旋转矩阵R和平移矩阵T最小判别阈值分别为thd_R和thd_T；设去中心化的参考角点集合MP′和经过一次旋转变换的中间待配准点集MQ″之间的最小二乘误差为

集合MP与MQ″′之间的最小二乘误差为

MQ″′表示初始待配准点集经过整体旋转和平移变换后的新的待配准点集；

迭代终止判断：

当迭代次数num_i＜num_max时,若d_{min_R}＜thd_R并且d_{min_T}＜thd_T，返回(R,T)结束迭代；

否则，Q＝MQ″′，返回步骤2继续进行迭代；

当num_i≥num_max时，返回(R,T)结束迭代。

3.根据权利要求1所述的一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，所述步骤4的具体方法为：

利用整体匹配时得到的参数(R,T)，对集合MQ进行转换得MQ1＝R×MQ+T，对集合MQ1和MP进行关联特征点配对，得到配对集合M1，对于(mq_i,mp_i)∈M1，若||mq_i-mp_i||＞D_max，将(mq_i,mp_i)从集合M1中剔除，其中D_max表示两关联点之间的最大距离阈值。

4.根据权利要求3所述的一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，所述步骤5具体包括：

利用(R,T)可以计算出小车在相邻两采样周期内的运动增量[Δx,Δy,Δθ]^T：

其中，Δx,Δy表示相邻两采样周期内小车在全局坐标系下分别沿水平方向和竖直方向的运动增量，Δθ表示在该周期内机器人方向变化角度；

设当前时刻机器人在环境中的位姿为X_t＝[x_t,y_t,θ_t]^T，前一时刻位姿为X_t-1＝[x_t-1,y_t-1,θ_t-1]^T，结合

得到

5.根据权利要求1所述的一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，该定位方法还包括：精确求取变换参数(R,T)。

Images