CN104501811A - 一种基于环境直线特征的地图匹配方法 - Google Patents

Abstract

本发明公开了一种基于环境直线特征的地图匹配方法，包括以下步骤：1）设置全局坐标系；2）机器人从起点开始不断扫描环境信息；3）对扫描数据点进行滤波处理；4）将滤波后数据点分割为不同的类别；5）采用IEP算法对每一个类别进行直线特征识别；6）从直线特征的数据点集中拟出直线，计算特征参数；7）将得到的直线特征从激光坐标系下变换到全局坐标系下，并添加到环境特征地图中；8）重复上述步骤，得到环境特征地图；9）机器人从起点开始运动，惯性导航输出当前位姿粗略值；10）按照上述步骤提取当前特征直线，在环境特征地图中查找相对应的直线特征；11）将对应的特征直线记录作为基础，结合当前扫描数据，计算机器人当前位姿。

Description

一种基于环境直线特征的地图匹配方法

技术领域

本发明属于移动机器人定位技术领域，特别涉及一种基于环境直线特征的地图匹配方法。

背景技术

在移动机器人的应用中，导航定位是机器人系统的关键功能。导航是指移动机器人通过传感器获得环境信息和自身位姿状态，实现在障碍物的环境中自主运动到目的地。导航的关键和前提是定位，定位是指实时确定机器人自身在环境中的位置和姿态。

目前较为常见的机器人定位技术主要是根据先验的环境信息，结合当前机器人位置信息和传感器获取的信息确定机器人位姿状态。定位技术主要分为绝对定位和相对定位；绝对定位常用的有卫星GPS定位、导航路标、地图匹配等，相对定位通过计算相对于初始位姿的位姿变化来确定当前的位姿状态，常见的有里程计、惯性导航系统。GPS定位误差较大，且容易受电磁干扰；导航路标定位需要建立导航路标改变已有环境，费时费力；而已有的地图匹配算法需要手动创建环境地图或者自主创建的地图精度不高无法形成闭环等；而里程计、惯性导航系统等长时间或大尺度下累积误差较大，定位精度较差。现有的导航定位方法存在的各种问题，无法准确、省力地为机器人提供位姿信息。

发明内容

本发明的目的是为了解决上述问题，提供一种无需改变环境即可完成机器人自主地图定位的基于环境直线特征的地图匹配方法。

为此，本发明的技术方案是：一种基于环境直线特征的地图匹配方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于：它包括以下步骤：

1）首先在新的未知环境下，设置起点位姿Z0，并以此为基准设置全局坐标系，即以起点位置为坐标系原点，以起点运动朝向为y轴正向，按右手法则确定x轴正向；

2）机器人从起点开始运动，机器人所载激光传感器不断扫描环境信息得到以激光传感器为原点的角度、距离极坐标点数据；机器人每运动一定距离就要处理一次激光扫描数据；

惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理,并将delta_d清0，从当前点重新开始计算delta_d；

3）对杂乱无规律的激光扫描数据点进行滤波处理，去除在有效计算距离范围之外的无用点；

4）将滤波后数据点根据其分布的连续特性聚类处理，分割为不同的类别；计算相邻两激光扫描点距离nd_laser，若该距离小于等于设定阈值thr_cate则判定为同一类别，同一类别代表该点集来自障碍物的连续表面；同时设置合适的阈值thr_cate，将数据点较少的类别剔除以加快数据处理速度；

5）采用IEP算法对每一个类别进行直线特征识别，在每个类别中找出构成直线特征的数据点集；设置合理的阈值thr_cate，将数据点较少的直线点集剔除；

6）利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数；

以机器人为中心坐标系与激光传感器为中心的坐标系重合，所述参数为在激光传感器坐标系下，直线到原点距离d、直线与坐标原点的垂线与极坐标轴夹角theta、直线端点坐标p1(x,y)，p2(x,y)和线性相关系数r；其中d，theta直线方程参数，利用线段端点p1，p2计算线段长度和直线匹配；利用线性系数r作为判别其直线特征相似程度，优先利用线性系数高的直线特征进行定位计算；

7）将上述步骤中得到的直线特征从激光坐标系下变换到全局坐标系下，并添加到环境特征地图中；所述环境特征地图为记录特征直线参数的数据库，每条记录为一特征直线，记录的属性分别为直线的特征参数d，theta，p1，p2；

8）使机器人在整个环境中运动重复上述步骤，以得到整个环境的特征地图；

9）机器人从起点开始运动，惯性导航输出当前位姿的粗略值；机器人所载激光传感器扫描当前环境，得到以激光传感器为原点的角度、距离极坐标点数据；

惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理,并将delta_d清0，从当前点重新开始计算delta_d；

10）将上述步骤中得到的直线特征变换到全局坐标系，在已创建的环境特征地图中查找相对应的直线特征，判别地图中是否已有直线与当前特征直线相对应；

11）将已创建的环境特征地图中可找到对应的特征直线记录作为基础，结合当前的扫描数据，计算出机器人当前位姿。

本发明以惯性导航的输出位姿为基础，利用激光扫描获取自然环境中的直线特征，创建高精度的环境特征地图，然后利用该创建的地图与实时扫描数据匹配进行定位。本发明无需改变环境即可完成机器人自主创建地图和定位，且精度较高。

附图说明

以下结合附图和本发明的实施方式来做进一步详细说明。

图1 本发明的直线特征提取流程图。

图2 本发明的地图匹配流程图。

具体实施方式

参见附图。本实施例在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；它包括以下步骤：

1）首先在新的未知环境下，设置起点位姿Z0，并以此为基准设置全局坐标系，即以起点位置为坐标系原点，以起点运动朝向为y轴正向，按右手法则确定x轴正向；

2）机器人从起点开始运动，机器人所载激光传感器不断扫描环境信息得到以激光传感器为原点的角度、距离极坐标点数据；机器人每运动一定距离就要处理一次激光扫描数据；

惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理,并将delta_d清0，从当前点重新开始计算delta_d；

3）如图1所示从激光扫描数据中提取直线特征过程：对杂乱无规律的激光扫描数据点进行滤波处理，去除在有效计算距离范围之外的无用点；

4）将滤波后数据点根据其分布的连续特性聚类处理，分割为不同的类别；方法为计算相邻两激光扫描点距离nd_laser，若该距离小于等于设定阈值thr_cate则判定为同一类别，同一类别代表该点集来自障碍物的连续表面；同时设置合适的阈值thr_cate，将数据点较少的类别剔除以加快数据处理速度；

5）采用IEP算法对每一个类别进行直线特征识别，在每个类别中找出构成直线特征的数据点集；设置合理的阈值thr_cate，将数据点较少的直线点集剔除；

6）利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数；

假设以机器人为中心坐标系与激光传感器为中心的坐标系重合，所述参数为在激光传感器坐标系下，直线到原点距离d、直线与坐标原点的垂线与极坐标轴夹角theta、直线端点坐标p1(x，y)，p2(x，y)和线性相关系数r；其中d、theta直线方程参数，利用线段端点p1，p2计算线段长度和直线匹配；利用线性系数r作为判别其直线特征相似程度，优先利用线性系数高的直线特征进行定位计算；

7）将上述步骤中得到的直线特征从激光坐标系下变换到全局坐标系下，并添加到环境特征地图中；所述环境特征地图为记录特征直线参数的数据库，每条记录为一特征直线，记录的属性分别为直线的特征参数d，theta，p1，p2；

8）使机器人在整个环境中运动重复上述步骤，以得到整个环境的特征地图；

9）机器人从起点开始运动，惯性导航输出当前位姿的粗略值；机器人所载激光传感器扫描当前环境，得到以激光传感器为原点的角度、距离极坐标点数据；

惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理,并将delta_d清0，从当前点重新开始计算delta_d；

10）将上述步骤中得到的直线特征变换到全局坐标系，在已创建的环境特征地图中查找相对应的直线特征，判别地图中是否已有直线与当前特征直线相对应；

11）将已创建的环境特征地图中可找到对应的特征直线记录作为基础，结合当前的扫描数据，计算出机器人当前位姿。

如图2所示为地图匹配定位过程，其中L1表示提取的特征直线在激光传感器坐标系下参数，{L1，L2...}表示特征直线集，{L1’， L2’...}表示将直线集{ L1，L2...}   变换到全局坐标系下，ML1为地图中所记录某一特征直线，{ML1，...}为地图记录的直线集；若在地图中可找到对应的特征直线记录假设为ML1，则以此记录ML1为基础和当前扫描数据L1计算当前位姿Z。若在地图中无法找到对应的特征直线记录，假设L2’，则判定当前扫描数据为一新特征，将该新特征ML2’添加到地图中以更新当前地图。

Claims (1)

1.一种基于环境直线特征的地图匹配方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于：它包括以下步骤：

1）首先在新的未知环境下，设置起点位姿Z0，并以此为基准设置全局坐标系，即以起点位置为坐标系原点，以起点运动朝向为y轴正向，按右手法则确定x轴正向；

2）机器人从起点开始运动，机器人所载激光传感器不断扫描环境信息得到以激光传感器为原点的角度、距离极坐标点数据；机器人每运动一定距离就要处理一次激光扫描数据；

惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理,并将delta_d清0，从当前点重新开始计算delta_d；

3）对杂乱无规律的激光扫描数据点进行滤波处理，去除在有效计算距离范围之外的无用点；

4）将滤波后数据点根据其分布的连续特性聚类处理，分割为不同的类别；计算相邻两激光扫描点距离nd_laser，若该距离小于等于设定阈值thr_cate则判定为同一类别，同一类别代表该点集来自障碍物的连续表面；同时设置合适的阈值thr_cate，将数据点较少的类别剔除以加快数据处理速度；

5）采用IEP算法对每一个类别进行直线特征识别，在每个类别中找出构成直线特征的数据点集；设置合理的阈值thr_cate，将数据点较少的直线点集剔除；

6）利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数；

以机器人为中心坐标系与激光传感器为中心的坐标系重合，所述参数为在激光传感器坐标系下，直线到原点距离d、直线与坐标原点的垂线与极坐标轴夹角theta、直线端点坐标p1(x,y)，p2(x,y)和线性相关系数r；其中d，theta直线方程参数，利用线段端点p1，p2计算线段长度和直线匹配；利用线性系数r作为判别其直线特征相似程度，优先利用线性系数高的直线特征进行定位计算；

7）将上述步骤中得到的直线特征从激光坐标系下变换到全局坐标系下，并添加到环境特征地图中；所述环境特征地图为记录特征直线参数的数据库，每条记录为一特征直线，记录的属性分别为直线的特征参数d，theta，p1，p2；

8）使机器人在整个环境中运动重复上述步骤，以得到整个环境的特征地图；

9）机器人从起点开始运动，惯性导航输出当前位姿的粗略值；机器人所载激光传感器扫描当前环境，得到以激光传感器为原点的角度、距离极坐标点数据；

惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理,并将delta_d清0，从当前点重新开始计算delta_d；

10）将上述步骤中得到的直线特征变换到全局坐标系，在已创建的环境特征地图中查找相对应的直线特征，判别地图中是否已有直线与当前特征直线相对应；

11）将已创建的环境特征地图中可找到对应的特征直线记录作为基础，结合当前的扫描数据，计算出机器人当前位姿。