CN110412596A - 一种基于图像信息和激光点云的机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于图像信息和激光点云的机器人定位方法，包括如下步骤：1)定位开始前，双目摄像机预先记录关键位置的图像；2)定位开始后，若机器人没有经过关键位置，则根据里程计运动模型采样，得到机器人位姿的提议分布；若机器人经过关键位置，则利用双目摄像机和激光雷达数据联合观测数据来得到机器人位姿概率最大的一点，在此点附近采样一组位姿点，计算这组位姿点的均值和方差，得到高斯分布来代替以往的提议分布；3)对得到的提议分布进行重采样，利用激光雷达观测模型来计算粒子的权值，保留权值大的粒子，舍弃权值小的粒子，为下一次预测阶段提供输入；4)经过步骤2)和3)阶段的反复迭代之后，来得到机器人的真实位姿，完成定位。

Description

一种基于图像信息和激光点云的机器人定位方法

技术领域

本发明涉及机器人技术领域，具体地说，特别涉及到一种基于图像信息和激光点云的机器人定位方法。

背景技术

目前，机器人定位技术广泛应用于园区巡检，仓储搬运等领域中，机器人自主定位导航技术的应用可以有效代替人完成部分作业，因此，机器人的定位导航技术式当前的研究热点。

目前，机器人定位分为局部定位和全局定位。局部定位是要求机器人在已知初始位置的条件下通过测量机器人相对于初始位置的距离和方向来确定当前位置，通常也称为航迹推算法。主要包括里程计法和惯性导航定位法。里程计法是在移动机器人的车轮上安装光电编码器，记录车轮转动圈数，来计算机器人的位移和偏转角度。惯性导航定位法是一种使用惯性导航传感器定位的方法。通过使用陀螺仪来测量机器人的角速度，用加速度计测量机器人的加速度。

全局定位是要求机器人在未知初始位置的情况下确定自己的位置。目前采用导航信标、主动或被动标识、地图匹配、卫星导航技术或概率方法进行定位。其中基于概率的全局定位成为了机器人定位研究的热点，最重要的是马尔可夫定位和蒙特卡洛定位。两者不仅能够实现全局定位和局部位置跟踪，而且能够解决机器人的“绑架”问题(是指由于机器人容易与外界发生碰撞而使机器人在不知情的情况下发生移动)。

在局部定位中，里程计法存在机器人与外界环境接触而引发的非系统的误差，导致机器人无法准确预测位姿情况。而在惯性导航定位法中，由于常量误差经积分运算会产生误差的累计，不适用于长时间的精确定位。

在基于概率的绝对定位中，当机器人获得编码器信息或者利用外界传感器感知环境后，马尔可夫定位算法必须对所有的栅格进行计算，因此需要大量的计算资源和内存，导致定位处理的实时性很差。蒙特卡洛定位则需要大量的样本才能较好地估计后验密度，任然需要较多时间实现机器人的位置更新，实时性不理想。

发明内容

本发明的目的在于针对现有技术中的不足，提供一种基于图像信息和激光点云的机器人定位方法，利用双目摄像机和激光雷达获取周围环境信息，对机器人实现高精度的定位。

以解决现有技术中存在的问题。

本发明所解决的技术问题可以采用以下技术方案来实现：

一种基于图像信息和激光点云的机器人定位方法，包括如下步骤：

1)定位开始前，双目摄像机预先记录关键位置的图像；

2)定位开始后，若机器人没有经过双目摄像机记录的关键位置，则根据里程计运动模型采样，得到机器人位姿的提议分布；

若机器人经过双目摄像机记录的关键位置，则利用双目摄像机和激光雷达数据联合观测数据来得到机器人位姿概率最大的一点，在此点附近采样一组位姿点，计算这组位姿点的均值和方差，得到高斯分布，以高斯分布来代替以往的提议分布；

3)对得到的提议分布进行重采样，利用激光雷达观测模型来计算粒子的权值，保留权值大的粒子，舍弃权值小的粒子，为下一次预测阶段提供输入；

4)经过步骤2)和步骤3)阶段的反复迭代之后，来得到机器人的真实位姿，完成定位。

进一步的，所述步骤2)为粒子滤波预测阶段，其过程如下：

2.1)当机器人没有经过双目摄像机记录的位置时，则获取最新时刻的里程计数据u_t-1，利用里程计运动模型，根据状态转移函数来对每一个粒子进行状态估计，以此来得到一个提议分布，根据此提议分布进行重采样，反复迭代之后得到机器人真实位姿；

2.2)当机器人经过双目摄像机记录的位置时，利用里程计，双目摄像机，激光雷达数据联合观测，得到提议分布；

2.3)根据公式在概率最大的位姿点附近进行采样，若两位姿点距离小于阈值Δ，则判断该位姿点有效，进行采样，共得到K个位姿点；

2.4)利用双目摄像头、激光雷达的融合观测概率和里程计判断的位姿概率来对K个位姿点进行估计，利用均值和方差来表示K个位姿点的高斯分布，以高斯分布来表示新的提议分布。

进一步的，所述步骤2.2)的过程如下：

2.2.1)u_t-1为最新时刻的里程计数据，根据最新时刻的里程计运动模型的数据，对机器人位姿进行估计，来得到新的机器人位姿集合

2.2.2)将双目摄像头最新时刻图像数据z_Vt和激光雷达最新时刻数据z_Lt进行数据融合，将相机坐标系下的扫描点的角度信息，经过旋转变换之后，转换到激光雷达下的坐标系,将角度信息进行融合；

2.2.3)输入双目摄像机和激光雷达的联合观测数据，包括扫描点云的融合角度信息和距离信息，根据公式来得出概率最大的一个位姿点

进一步的，所述步骤2.2.2)的过程如下：

设双目摄像机扫描到物体点特征的角度为以下形式：

Θ_V＝[θ_v,1θ_v,2···θ_v,n] (1)

激光雷达扫描到物体点云的角度为以下形式：

Θ_L＝[θ_l,1θ_l,2···θ_l,n] (2)

根据式(3)来求得旋转变换矩阵R，将相机扫描点的角度信息与激光雷达扫描的角度信息进行融合；

当时，求出的解为旋转矩阵R的最大似然估计，为激光雷达测量噪声的方差,为双目相机测量噪声的方差。根据奇异值分解，旋转矩阵R的求解如下：

R＝UV^T (5)

与现有技术相比，本发明的有益效果在于：

1)在现有定位方法中，主要是基于粒子滤波或者直方图滤波的框架下，利用激光雷达或者双目摄像机来对机器人进行定位导航，定位精度存在较大偏差，导航性能差。本发明是利用激光雷达和双目摄像机的联合观测数据进行定位导航。在机器人定位开始前，事先利用双目摄像机记录关键位置的图像，如路口，拐角处或障碍物附近等。若机器人经过关键位置时，调取先前记录的图像，将图像中的角度信息与激光雷达的角度信息进行融合，将融合后的角度信息与激光雷达的距离信息作为观测数据，对机器人的位姿进行矫正，可以有效提高定位精度。若机器人未经过关键位置，则利用激光雷达观测数据对机器人进行实时定位。

2)目前，在粒子滤波过程中的预测阶段，是将里程计模型判断后的粒子分布作为提议分布，为重采样阶段提供粒子分布群。此分布过于分散，估计机器人位姿效率不高。本发明是将双目摄像机和激光雷达的数据进行融合。具体是将相机坐标系的角度信息转换到激光雷达坐标系，将融合的角度信息和激光雷达的距离信息作为观测数据，来改善现有的提议分布，使提议分布更为集中，有效减少了机器人位姿估计的时间。

3)在现有方法中，是将里程计模型判断后的粒子分布作为提议分布。在本发明的粒子滤波预测阶段，利用双目摄像机和激光雷达数据联合观测来得到机器人位姿概率最大的一点，在此点附近采样一组位姿点，计算这组位姿点的均值和方差，得到新的高斯分布，以高斯分布来代替现有的里程计模型判断的提议分布，使估计位姿粒子群更接近于机器人真实位姿。

附图说明

图1为本发明所述基于图像信息和激光点云的机器人定位方法的示意图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

参见图1，本发明所述的一种基于图像信息和激光点云的机器人定位方法，包括如下步骤：

1)在定位开始前，双目摄像机事先记录关键位置的图像，如路口，障碍物附近或者楼道转弯处等。

2)定位开始后，若机器人没有经过双目摄像机记录的位置，则在粒子滤波的预测阶段，根据里程计运动模型采样，来得到机器人位姿的提议分布。若机器人经过双目摄像机记录的位置，则利用双目摄像机和激光雷达数据联合观测数据来得到机器人位姿概率最大的一点，在此点附近采样一组位姿点，计算这组位姿点的均值和方差，得到高斯分布，以高斯分布来代替以往的提议分布。

3)对得到的提议分布进行重采样，利用激光雷达观测模型来计算粒子的权值，保留权值大的粒子，舍弃权值小的粒子，为下一次预测阶段提供输入。

4)经过步骤2)和步骤3)阶段的反复迭代之后，来得到机器人的真实位姿，完成定位。

实施例

1)设初始时刻机器人采样样本为S_t-1，包括机器人位姿和地图信息

双目摄像机事先记录关键位置的图像，如路口，障碍物附近或者楼道转弯处等。

2)粒子滤波预测阶段

2.1)当机器人没有经过双目摄像机记录的位置时，则获取最新时刻的里程计数据u_t-1，利用里程计运动模型，根据状态转移函数来对每一个粒子进行状态估计，以此来得到一个提议分布，根据此提议分布进行重采样，反复迭代之后得到机器人真实位姿。

2.2)当机器人经过双目摄像机记录的位置时，利用里程计，双目摄像机，激光雷达数据联合观测，得到提议分布。

2.2.1)u_t-1为最新时刻的里程计数据，根据最新时刻的里程计运动模型的数据，对机器人位姿进行估计，来得到新的机器人位姿集合

2.2.2)将双目摄像头最新时刻图像数据z_Vt和激光雷达最新时刻数据z_Lt进行数据融合，将相机坐标系下的扫描点的角度信息，经过旋转变换之后，转换到激光雷达下的坐标系,将角度信息进行融合。

设双目摄像机扫描到物体点特征的角度为以下形式：

Θ_V＝[θ_v,1θ_v,2···θ_v,n] (1)

激光雷达扫描到物体点云的角度为以下形式：

Θ_L＝[θ_l,1θ_l,2···θ_l,n] (2)

根据式(3)来求得旋转变换矩阵R，将相机扫描点的角度信息与激光雷达扫描的角度信息进行融合。

当时，求出的解为旋转矩阵R的最大似然估计，为激光雷达测量噪声的方差,为双目相机测量噪声的方差。根据奇异值分解，旋转矩阵R的求解如下所示。

R＝UV^T (5)

2.2.3)输入双目摄像机和激光雷达的联合观测数据，包括扫描点云的融合角度信息和距离信息，根据公式来得出概率最大的一个位姿点

2.3)根据公式在概率最大的位姿点附近进行采样，若两位姿点距离小于阈值Δ，则判断该位姿点有效，进行采样，共得到K个位姿点。

2.4)利用双目摄像头、激光雷达的融合观测概率和里程计判断的位姿概率来对K个位姿点进行估计，利用均值和方差来表示K个位姿点的高斯分布，以高斯分布来表示新的提议分布。

3)对得到的提议分布进行重采样，根据激光雷达的观测模型，计算各个位姿点的权重。并且舍弃权重小的粒子，保留权重大的粒子。

4)经过重采样得到的粒子分布，作为下一次预测阶段的粒子输入，经过步骤2)和3)阶段的反复迭代，使大部分粒子都接近于机器人的真实位姿，完成机器人的定位。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (4)

1.一种基于图像信息和激光点云的机器人定位方法，其特征在于，包括如下步骤：

1)定位开始前，双目摄像机预先记录关键位置的图像；

2)定位开始后，若机器人没有经过双目摄像机记录的关键位置，则根据里程计运动模型采样，得到机器人位姿的提议分布；

若机器人经过双目摄像机记录的关键位置，则利用双目摄像机和激光雷达数据联合观测数据来得到机器人位姿概率最大的一点，在此点附近采样一组位姿点，计算这组位姿点的均值和方差，得到高斯分布，以高斯分布来代替以往的提议分布；

3)对得到的提议分布进行重采样，利用激光雷达观测模型来计算粒子的权值，保留权值大的粒子，舍弃权值小的粒子，为下一次预测阶段提供输入；

4)经过步骤2)和步骤3)阶段的反复迭代之后，来得到机器人的真实位姿，完成定位。

2.根据权利要求1所述的基于图像信息和激光点云的机器人定位方法，其特征在于，所述步骤2)为粒子滤波预测阶段，其过程如下：

2.1)当机器人没有经过双目摄像机记录的位置时，则获取最新时刻的里程计数据u_t-1，利用里程计运动模型，根据状态转移函数来对每一个粒子进行状态估计，以此来得到一个提议分布，根据此提议分布进行重采样，反复迭代之后得到机器人真实位姿；

2.2)当机器人经过双目摄像机记录的位置时，利用里程计，双目摄像机，激光雷达数据联合观测，得到提议分布；

2.3)根据公式在概率最大的位姿点附近进行采样，若两位姿点距离小于阈值Δ，则判断该位姿点有效，进行采样，共得到K个位姿点；

2.4)利用双目摄像头、激光雷达的融合观测概率和里程计判断的位姿概率来对K个位姿点进行估计，利用均值和方差来表示K个位姿点的高斯分布，以高斯分布来表示新的提议分布。

3.根据权利要求2所述的基于图像信息和激光点云的机器人定位方法，其特征在于，所述步骤2.2)的过程如下：

2.2.1)u_t-1为最新时刻的里程计数据，根据最新时刻的里程计运动模型的数据，对机器人位姿进行估计，来得到新的机器人位姿集合

2.2.2)将双目摄像头最新时刻图像数据z_Vt和激光雷达最新时刻数据z_Lt进行数据融合，将相机坐标系下的扫描点的角度信息，经过旋转变换之后，转换到激光雷达下的坐标系,将角度信息进行融合；

2.2.3)输入双目摄像机和激光雷达的联合观测数据，包括扫描点云的融合角度信息和距离信息，根据公式来得出概率最大的一个位姿点

4.根据权利要求3所述的基于图像信息和激光点云的机器人定位方法，其特征在于，所述步骤2.2.2)的过程如下：

设双目摄像机扫描到物体点特征的角度为以下形式：

Θ_V＝[θ_v,1 θ_v,2 ··· θ_v,n] (1)

激光雷达扫描到物体点云的角度为以下形式：

Θ_L＝[θ_l,1 θ_l,2 ··· θ_l,n] (2)

根据式(3)来求得旋转变换矩阵R，将相机扫描点的角度信息与激光雷达扫描的角度信息进行融合；

当时，求出的解为旋转矩阵R的最大似然估计，为激光雷达测量噪声的方差,为双目相机测量噪声的方差。根据奇异值分解，旋转矩阵R的求解如下：

R＝UV^T (5)。