一种基于激光扫描传感器的里程估计方法
【专利摘要】本发明公开了一种基于激光扫描传感器的里程估计方法，机器人的激光扫描传感器，可获得一定范围内的障碍物角度和距离信息，在每个位置可以得到局部环境地图，在相邻位置通过匹配两个局部地图可以得到一个旋转平移变换矩阵，根据变换矩阵对相邻的局部地图进行数据融合，最终可以得到一个全局的环境地图，然后机器人可利用激光扫描传感器扫描到的数据与全局环境地图匹配定位，从而得出一个准确的里程估计。本发明仅仅利用激光数据估计里程计信息，利用外部环境信息的迭代匹配进行估计里程，由于外部环境信息的相对稳定性，减少累积误差。
【专利说明】
一种基于激光扫描传感器的里程估计方法
技术领域
[0001]本发明属于机器人运动技术领域，特别涉及一种基于激光扫描传感器的里程估计方法。
【背景技术】
[0002]里程信息是机器人运动状态的最基本信息。现有的机器人里程估计方法需要对机器人在一段局部运行路径中各点的位姿变化来确定，这个位姿变化可由编码器和惯性导航传感器来得到，传统的编码器估计里程方法仅仅利用内部传感器的信息累积得到里程信息，但该位姿变化误差会不断累积，误差较大，最终得到的里程信息不准确。
【发明内容】
[0003]本发明的目的是为了解决上述问题，提供一种稳定且准确的基于激光扫描传感器的里程估计方法。
[0004]为此，本发明的技术方案是:一种基于激光扫描传感器的里程估计方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于:包括以下步骤:
1)对杂乱无规律的扫描数据点进行滤波处理，去除无用点；
2)将滤波后数据通过光线追踪方法计算局部栅格地图；局部栅格地图是指机器人当前位姿为坐标原点，将该位置观测到的数据构建的栅格地图，局部栅格地图创建通过激光原点所在位置到激光光束结束位置通过光束追踪计算得到，两者之间的连线经过的栅格点应为非障碍物占据，激光光束结束点为障碍物占据状态，在连线该点之后为未观测状态，即障碍物占据的概率等于非障碍物占据概率；
3)计算相邻两个位姿下的局部栅格地图的sift特征点对及其对应特征描述子;sift，即尺度不变特征变换，是用于图像处理领域的一种描述;这种描述具有尺度不变性，可在图像中检测出关键点，是一种局部特征描述子；
4)根据特征描述子的相关性对特征点对进行排序，滤除特征描述子相关性较小的特征点;在两幅相邻局部地图中分别提取特征点及其描述子，遍历该点集，寻找特征描述子距离满足阈值条件的特征点对，并根据距离由小到大对其排序，不匹配的特征点舍弃;选取匹配的特征点对的前百分之三十的特征点对进行ICP迭代计算变换矩阵；
5)将两个相邻位姿下的局部地图进行最近点迭代算法ICP，计算两个局部地图的变换矩阵；ICP迭代算法是利用与多条不平行直线的距离来确定一点的原理，通过计算直线相对机器人的距离变化信息得到机器人自身的相对位姿变化;利用两条不平行直线特征即可定位，当激光传感器获得多条不平行直线特征时，筛选线性相关系数较高的直线，两两直线进行定位解算，最后将结果舍去最大值和最小值求平均值；
6)重复步骤1)_5)，记录完成整个路径的变换矩阵，最后进行数据合并得到全局栅格地图； 7)利用激光传感器获得环境特征，将环境特征与全局地图匹配，从而计算出机器人的位姿变化，得出里程估计。
[0005]本发明仅仅利用激光数据估计里程计信息，不需要传统的编码器等传感器，适用于无法安装或不方便装配该类传感器的情况；利用外部环境信息的迭代匹配进行估计里程，由于外部环境信息的相对稳定性，减少累积误差;本发明计算的里程信息可以和传统的编码器或IMU得到的里程信息通过扩展卡尔曼滤波等方法进行融合，从而得到更稳定准确的里程信息。
【附图说明】
[0006]以下结合附图和本发明的实施方式来作进一步详细说明图1为本发明的流程框图；
图2为本发明的特征识别示意图；
图3为本发明的定位结算示意图；
图4为本发明的位姿校正示意图。
【具体实施方式】
[0007]参见附图。本实施例所述的里程估计方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用;机器人系统装有二维激光扫描传感器，可获得一定范围内的障碍物角度和距离信息，在每个位置可以得到局部环境地图，在相邻位置通过匹配两个局部地图可以得到一个旋转平移变换矩阵，根据变换矩阵对相邻的局部地图进行数据融合，最终我们可以得到一个全局的环境地图，然后机器人可利用激光扫描传感器扫描到的数据与全局环境地图匹配定位，从而得出一个准确的里程估计。
[0008]具体包括以下步骤:
1)对杂乱无规律的扫描数据点进行滤波处理，去除无用点；
2)将滤波后数据通过光线追踪方法计算局部栅格地图；局部栅格地图是指机器人当前位姿为坐标原点，将该位置观测到的数据构建的栅格地图，由于该地图是单次激光数据构建的，因而地图的估计误差主要为激光传感器的观测误差，不存在估计偏差。局部栅格地图创建通过激光原点所在位置到激光光束结束位置(障碍物位置)通过光束追踪计算得到，两者之间的连线经过的栅格点应为非障碍物占据，激光光束结束点为障碍物占据状态，在连线该点之后为未观测状态，即障碍物占据的概率等于非障碍物占据概率；
3)计算相邻两个位姿下的局部栅格地图的sift特征点对及其对应特征描述子;SIFT(Scale-1nvariant feature transform)是一种检测局部特征的算法，该算法通过求一幅图中的特征点(interest points ,or corner points)及其有关scale 和 orientat1n 的描述子，在尺度空间寻找极值点，提取位置，尺度，旋转不变量，SIFT特征对旋转、尺度缩放、亮度变化保持不变性，对视角变化、仿射变换、噪声也保持一定程度的稳定性；
4)根据特征描述子的相关性对特征点对进行排序，滤除特征描述子相关性较小的特征点;在两幅相邻局部地图中分别提取特征点及其描述子，遍历该点集，寻找特征描述子距离满足阈值条件的特征点对，并根据距离有小到大对其排序，不匹配的特征点舍弃;选取匹配的特征点对的前百分之三十的特征点对进行ICP迭代计算变换矩阵； 5)将两个相邻位姿下的局部地图进行最近点迭代算法ICP，计算两个局部地图的变换矩阵；利用与多条不平行直线的距离来确定一点的原理，通过计算直线相对机器人的距离变化信息得到机器人自身的相对位姿变化;利用两条不平行直线特征即可定位，当激光传感器获得多条不平行直线特征时，筛选线性相关系数较高的直线，两两直线进行定位解算，最后将结果舍去最大值和最小值求平均值(如图2所示)；如图3所示，已知两条不平行直线
11、12和与两条直线的距离(dl，d2)，可以确定点P;
6)重复步骤1)_5)，记录完成整个路径的变换矩阵，最后进行数据合并得到全局栅格地图；地图表示采用几何描述方式，在离线方式下获得，首先设定初始位姿，然后控制机器人按预定路径行进，在行进过程中，利用编码器和惯性传感器信息可以估计出机器人当前位姿，与此同时激光传感器不断扫描环境信息，通过定位过程所述方法，从激光传感器获得数据中提取自然环境特征得到特征参数；由于在一定范围内，同一特征能够一直被激光传感器所扫描获得，利用该特征的参数变化可计算出机器人运动的相对位姿变化，利用该位姿变化来校正编码器和惯性传感器的位姿信息；由校正后的位姿信息，将特征参数变换到起始位姿确定的全局坐标系下，然后将该特征添加到全局地图中；机器人不断前进扫描到新的环境特征，重复上述过程完成全局地图创建；
7)利用激光传感器获得环境特征，将环境特征与全局地图匹配，从而计算出机器人的位姿变化，得出里程估计。如图4所示，P1-P2-P3为机器人一段局部运行路径，在该局部范围内，激光传感器均能获得直线特征LI，L2，L3;从Pl至P3的位姿变化可由编码器和惯性导航传感器得到，但该位姿变化误差会不断累积，误差较大;所以利用直线特征也可计算出Pl至P3的位姿变化，该位姿变化没有累积误差，因而误差较小，通过比较位姿的变化，我们就得到了一个较为精准的里程估计。
【主权项】
1.一种基于激光扫描传感器的里程估计方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用;其特征在于:包括以下步骤: .1)对杂乱无规律的扫描数据点进行滤波处理，去除无用点； .2)将滤波后数据通过光线追踪方法计算局部栅格地图；局部栅格地图是指机器人当前位姿为坐标原点，将该位置观测到的数据构建的栅格地图，局部栅格地图创建通过激光原点所在位置到激光光束结束位置通过光束追踪计算得到，两者之间的连线经过的栅格点应为非障碍物占据，激光光束结束点为障碍物占据状态，在连线该点之后为未观测状态，即障碍物占据的概率等于非障碍物占据概率； .3 )计算相邻两个位姿下的局部栅格地图的s i f t特征点对及其对应特征描述子;s i f t，即尺度不变特征变换，是用于图像处理领域的一种描述;这种描述具有尺度不变性，可在图像中检测出关键点，是一种局部特征描述子； .4)根据特征描述子的相关性对特征点对进行排序，滤除特征描述子相关性较小的特征点;在两幅相邻局部地图中分别提取特征点及其描述子，遍历该点集，寻找特征描述子距离满足阈值条件的特征点对，并根据距离由小到大对其排序，不匹配的特征点舍弃;选取匹配的特征点对的前百分之三十的特征点对进行ICP迭代计算变换矩阵； .5)将两个相邻位姿下的局部地图进行最近点迭代算法ICP，计算两个局部地图的变换矩阵；ICP迭代算法是利用与多条不平行直线的距离来确定一点的原理，通过计算直线相对机器人的距离变化信息得到机器人自身的相对位姿变化;利用两条不平行直线特征即可定位，当激光传感器获得多条不平行直线特征时，筛选线性相关系数较高的直线，两两直线进行定位解算，最后将结果舍去最大值和最小值求平均值； .6)重复步骤1)_5)，记录完成整个路径的变换矩阵，最后进行数据合并得到全局栅格地图； .7)利用激光传感器获得环境特征，将环境特征与全局地图匹配，从而计算出机器人的位姿变化，得出里程估计。
【文档编号】G01C22/00GK105973265SQ201610332725
【公开日】2016年9月28日
【申请日】2016年5月19日
【发明人】王海瑞, 陈如申, 黎勇跃
【申请人】杭州申昊科技股份有限公司