CN110567441B - 基于粒子滤波的定位方法、定位装置、建图及定位的方法 - Google Patents

Abstract

本公开涉及概率定位的技术领域，公开了一种基于粒子滤波的定位方法，包括以下步骤：基于视觉自定位，获得标签坐标及自定位坐标的信息；基于栅格地图数据，调用概率分布模型生成新的粒子群；基于所述粒子群的状态，对粒子的状态和分布进行更新；计算粒子评分并输出定位结果。相应地，还公开了一种定位装置以及建图及定位的方法。本公开的一些技术效果在于：基于视觉自定位获得自定位坐标，据此作为初始位姿来调节粒子群的初始分布形态，配合第一约束条件和第二约束条件对粒子群的分布进行约束，能更高效地、更准确地获得定位结果。

Description

基于粒子滤波的定位方法、定位装置、建图及定位的方法

技术领域

本公开涉及概率定位的技术领域，基于粒子滤波的定位方法、定位装置、建图及定位的方法。

背景技术

在利用蒙特卡罗定位算法(属于粒子滤波算法中的一种)进行定位时，一般会经历几个主要步骤：初始化粒子群、模拟粒子运动、计算粒子评分、粒子群重采样后再模拟粒子运动以及计算粒子评分。

在初始化粒子群的步骤，由于初始位置不确定，一般使用高斯分布随机采样的方法(randn函数)初始化粒子群，这使得需要经过较长时间的模拟粒子运动才能使粒子的分布逼近定位设备所在的真实位置。

上述主要步骤中，模拟粒子运动包括两个环节：根据运动数据以及运动模型更新粒子的状态、根据所述测量数据以及测量模型修正粒子的状态和分布。

其中，运动模型(或称概率运动模型、条件密度、基于地图的运动模型)可以表示为p(x_t|u_t，x_t-1，m)，它描述了对x_t-1执行运动控制u_t后，定位设备取得的运动学的后验分布，也可以理解为被放置在地图m所表示的环境里的定位设备在位姿x_t-1处执行控制量u_t到达位姿x_t的可能性。其中，x_t表示定位设备假想的后继位姿，x_t-1表示定位设备的初始位姿，属于模型中的输入项，也可以理解为前一时刻的位姿，它可以通过里程计等获取到的运动数据而被确定；m表示给定的、定位设备所在场景的对应的地图信息，属于模型中的输入项，一般来说这样的地图有多种类型，常用的是栅格地图。

其中，测量模型(或称环境测量模型、观测模型)可以表示为p(z_t|x_t，m)，它描述在客观世界生成传感器(如视觉相机、激光雷达、声呐传感器等)测量的过程。其中，z_t表示t时刻的位姿测量值，属于模型中的输入项，它可以通过例如视觉点云数据、激光点云数据等测量数据而被确定；x_t表示定位设备假想的后继位姿；m表示地图信息，属于模型中的输入项。

粒子滤波算法、蒙特卡罗算法及其改进的相关算法很多，但主要步骤一般会和前文描述所吻合，未详述之处，也可以参照书籍《概率机器人》(作者：塞巴斯蒂安·特龙、沃尔弗拉姆·比加尔、迪特尔·福克斯)的描述进行理解，也可以参照其他现有文献进行理解。

前文至少在一个方面表明，现有的定位设备在利用粒子滤波算法实现定位的方式，效率及效果有待提高。

发明内容

为解决前述的至少一个技术问题，本公开在一方面提出了一种基于粒子滤波的定位方法，其技术方案如下：

基于粒子滤波的定位方法，包括以下步骤：基于视觉自定位，获得标签坐标及自定位坐标的信息；基于栅格地图数据，调用概率分布模型生成新的粒子群；所述概率分布模型包括第一约束条件以及第二约束条件；以穿过所述自定位坐标与所述标签坐标之间的直线为基准线，以所述自定位坐标指向所述标签坐标的方向为基准线正方向，以粒子的数量及密度大于预设值的区间作为关注区域时，所述第一约束条件包括：在关注区域内，粒子围绕所述自定位坐标分布，在所述基准线的侧向上，粒子的密度递减；所述第二约束条件包括：在关注区域内，在基准线正方向上，粒子分布的区域单元的面积递减；基于所述粒子群的状态，对粒子的状态和分布进行更新；计算粒子评分并输出定位结果。

优选地，“在基准线的侧向上，粒子的密度递减”包括：所述粒子的数量以所述基准线为中心，沿侧向的分布遵循一元高斯分布规律；所述第二条件包括：以所述基准线为中心，所述区域单元内粒子分布对应的标准差，在基准线正方向上递减。

优选地，所述关注区域的边界形状包括以所述自定位坐标为重心而构成的图形；所述标签坐标不在所述关注区域内。

优选地，所述边界形状包括三角形、梯形或扇形。

优选地，还包括以下步骤：获取由相机生成的场景的图像，所述相机至少具有两个摄像头；对至少两个所述摄像头同步采集到的所述图像进行处理，获得图像视差数据；根据所述图像视差数据，生成像素深度数据；根据所述像素深度数据，生成视觉点云数据；所述栅格地图数据，由激光点云生成；以所述栅格地图数据格式为标准，对所述视觉点云数据进行格式转换，得到测量数据；“基于所述粒子群，对粒子的状态和分布进行更新”包括：根据运动数据以及运动模型更新粒子的状态；根据测量数据以及测量模型修正粒子的状态和分布。

优选地，还包括以下步骤：更新粒子的状态前，获取里程计提供的运动数据。

优选地，“基于视觉自定位，获得标签坐标及自定位坐标的信息”包括：

获取由相机生成的场景的图像；识别所述图像中的标签的特征信息，根据所述特征信息向标签信息库发送匹配请求；获得标签信息库响应所述匹配请求而返回的标签坐标的信息；根据所述标签坐标的信息获得自定位坐标的信息。

在一些方面，本公开还提出了一种定位装置，包括：自定位模块，用于基于视觉自定位，获得标签坐标及自定位坐标的信息；粒子生成模块，用于基于栅格地图数据，调用所述概率分布模型生成新的粒子群；粒子更新模块，用于基于所述粒子群的状态，对粒子的状态和分布进行更新；解算模块，用于计算粒子评分并输出定位结果。

在一些方面，本公开还提出了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如所述的基于粒子滤波的定位方法。

在一些方面，本公开还提出了建图及定位的方法，包括以下步骤：调用激光雷达对场景进行扫描，处理获得激光点云数据；对所述激光点云数据进行处理，生成栅格地图数据；调用相机对场景进行拍摄，获得场景的图像，所述相机至少具有两个摄像头；对所述图像进行处理，获得视觉点云数据；根据所述的定位方法，生成新的粒子群；以所述栅格地图数据格式为标准，对所述视觉点云数据进行格式转换，得到测量数据；根据获得的运动数据以及运动模型更新粒子的状态；根据所述测量数据以及测量模型修正粒子的状态和分布；计算粒子评分并输出定位结果。

本公开的一些技术效果在于：基于视觉自定位获得自定位坐标，据此作为初始位姿来调节粒子群的初始分布形态，配合第一约束条件和第二约束条件对粒子群的分布进行约束，能更高效地、更准确地获得定位结果。

附图说明

为更好地理解本公开的技术方案，可参考下列的、用于对现有技术或实施例进行辅助说明的附图。这些附图将对现有技术或本公开部分实施例中，涉及到的产品或方法有选择地进行展示。这些附图的基本信息如下：

图1为一个实施例，本公开采用的粒子滤波算法进行定位的主要步骤示意图；

图2为一种二维高斯分布的示意图；

图3为一个实施例中，采用高斯分布随机采样生成的粒子群分布示意图；

图4为一个实施例中，关注区域S内的粒子群分布示意图；

图5为一个实施例中，关注区域S的边界形状呈扇形的示意图；

图6为一个实施例中，关注区域S的边界形状呈梯形的示意图；

图7为一个实施例中，PnP投影关系示意图；

图8为一个实施例中，双目相机的成像示意图；

图9为一个实施例中，关注区域内的区域单元的排列示意以及部分的区域单元内的粒子分布的数量N的变化曲线示意图。

上述附图中，附图标记及其所对应的技术特征如下：

11-第一图像，12-第二图像。

具体实施方式

下文将对本公开涉及的技术手段或技术效果作进一步的展开描述，显然，所提供的实施例仅是本公开的部分实施方式，而并非全部。基于本公开中的实施例以及图文的明示或暗示，本领域技术人员在没有作出创造性劳动的前提下所能获得的所有其他实施例，都将在本公开保护的范围之内。

在总体思路上，本公开提出了一种基于粒子滤波的定位方法，包括以下步骤：基于视觉自定位，获得标签坐标及自定位坐标的信息；基于栅格地图数据，调用概率分布模型生成新的粒子群；所述概率分布模型包括第一约束条件以及第二约束条件；以穿过所述自定位坐标与所述标签坐标之间的直线为基准线，以所述自定位坐标指向所述标签坐标的方向为基准线正方向，以粒子的数量及密度大于预设值的区间作为关注区域时，所述第一约束条件包括：在关注区域内，粒子围绕所述自定位坐标分布，在所述基准线的侧向上，粒子的密度递减；所述第二约束条件包括：在关注区域内，在基准线正方向上，粒子分布的区域单元的面积递减；基于所述粒子群的状态，对粒子的状态和分布进行更新；计算粒子评分并输出定位结果。

如图1所示，应用本公开所述的总体思路，在定位设备进行第一次定位以及后续的每一时刻的定位时，均可重复执行“生成新的粒子群”“对粒子的状态和分布进行更新”“计算粒子评分”“输出当下时刻的定位结果”这四大步骤，当然在具体实施场合，也可以仅在一次定位的过程中执行前述的步骤。在“生成新的粒子群”步骤中，栅格地图数据、自定位坐标、标签坐标、概率分布模型作为输入项；在“对粒子的状态和分布进行更新”步骤中，可以包括两个进程，即“更新粒子状态”以及“修正粒子的状态和分布”前一进程的输入项是运动数据以及运动模型，后一进程的输入项是测量数据以及测量模型。“对粒子的状态和分布进行更新”步骤中，也可以只包括前述的后一进程，但包括两个进程的情况下，对于提高定位结果的精度具有更大的帮助。运动模型以及测量模型可以参照现有技术进行设计；运动数据可以利用诸如里程计等能读取定位设备运动参数的传感器进行获取；可以借助视觉相机、激光雷达等对场景的拍摄而生成点云数据，对点云数据与栅格地图数据进行匹配后得到测量数据。

在硬件配置上，前述四大步骤可以由一个或一个以上的处理器来执行，当处理器是多个的情况下，一些处理器可能只需执行步骤里面的某个环节的处理。一般来说，处理器执行前述四大步骤，需要定位设备其他硬件提供相关信息，如视觉相机(或称相机)、存储器等。这意味着，定位设备可以是多种多样的，仅举部分例子的情况下，定位设备可以是智能手机、电脑、机器人、车载导航仪等。

关于视觉自定位。本文所说的视觉自定位，属于基于特征的视觉定位技术，其原理主要包括：需要预先在场景中识别标签，这些标签应当具有与其旁边景物有所区别的特征，例如形状、色彩、位置等；标签可以直接采用场景里的一些景物，也可以通过人为的方式在场景中设置一些物体，如外观呈矩形或其他形状的标记物。利用相机在场景内的不同位置对场景进行拍摄，可以得到关于标签的系列图像，同时记录下相机拍摄时所在的位置，据此可以制作得到标签信息库(或称基于特征的场景地图数据库)，标签信息库中存储有标签的特征信息以及标签的位置信息。如此，在进行实时定位时，相机(一般是双目相机或更多目的相机)对场景进行拍摄，便可获得场景的图像，对图像进行特征识别，便可提取出图像中的标签的特征信息，据此可以到标签信息库中进行特征匹配，从而找到该标签在世界坐标系下的位置(即标签坐标)，据此可以推算出相机的实时位置(即自定位坐标)以及朝向，即实时位姿。一般来说，存在两个以上摄像头/摄像单元时，为提高数据处理效率，可以选取其中的一幅图像对标签的特征信息进行识别。当然，为了确保较高的定位精度，也可以选取多幅图像对标签的特征信息进行识别，然后求出自定位坐标。基于特征的视觉定位技术可见诸众多文献，在此不作进一步展开。

根据上述的视觉自定位的原理，在实施本公开所述方案的主要步骤前，对于图像纹理不够丰富的场景，如视野中存在大量白墙或玻璃墙，可以在这些物体上粘贴标记物(如矩形或者更多边形的标记物图案)，使得场景的多个位置、多个角度具有足够多、足够大识别度的标签，然后进行标签信息库的建立，这样能有助于进行更好的视觉自定位。

另外，采用视觉自定位为粒子滤波提供位置参考信息，能加快粒子收敛的速度。在现有技术中，对于二维的栅格地图，初始化粒子群时一般采用高斯分布模型，典型二维的高斯分布模型示意图如图2所示，对其模拟生成的粒子群的分布如图3所示，高斯分布模型默认粒子的分布密度在中间最高(以图中的O点为中心)，在四周最低，而且是各向同性的(但不表示实际生成的粒子密度分布是各向同性的)。而且，在第一次初始化粒子群时，由于定位设备或定位机器无法感知到自身位置，因此生成的粒子更是分散且随机的。不同于现有技术的是，本公开所述方案在第一次初始化粒子群时，利用自定位坐标作为位置参考信息，这样使得粒子的分布在一开始就得到有效的收敛信息，不存在过多的随机分散在定位设备自身位置较远位置的粒子，这样能大幅提高首次定位所需的时间。另外，本公开采用的生成粒子群的机制与现有技术的不一样，在一个实施例中，如图4所示，R点为视觉自定位得到的标签坐标(一般是标签的中心点坐标或者是标签的边角坐标)，O点为自定位坐标(一般是相机光心的坐标)，粒子群的分布受到以下条件的约束：

(1)在关注区域内(可参照一些实施例中，附图中的关注区域S进行理解)粒子围绕所述自定位坐标分布，以穿过自定位坐标与标签坐标之间的直线为基准线，在基准线的侧向上，粒子的密度递减；即以穿过RO点之间的直线作为基准线，在基准线的一个侧向(X轴正方向)上，粒子的密度递减，在基准线的另一个测向(X轴反方向)上，粒子的密度递减。设计该约束条件的作用在于，可以减少地图平面上远离O点的粒子的数量，使后续的粒子评分的结果能更符合真实情况。由于基准线的正方向极大概率地代表了相机的真实朝向，因此关注区域内基准线附近仍会分布大量的粒子。在基准线的侧向上的粒子分布，可以采用一元高斯分布的规律，使粒子的数量呈现中间多两边少的规律；当然，本领域技术人员也可以基于工作常识选用其他的公式/规律(例如线性递减)来配合第一约束条件。

(2)在关注区域内，在基准线正方向上，粒子分布的区域单元的面积递减；区域单元可以理解为组成所述关注区域的一个个单元，这些单元沿基准线正方向相接地排列，并具有单位宽度L(一种情况下如图9所示，基准线正方向即Y轴正方向)。设计该约束条件的作用在于，考虑到场景中的标签在成像时，投影到图像中可能会存在若干个像素的误差，这样会导致估计所得的标签与相机之间的距离也存在误差。由于与相机距离较远的标签，投影到图像中所占据的面积较小，即所占据的像素个数较少，因此根据这些像素的位置估计标签与相机之间的距离产生的误差也较大，而在垂直于基准线正方向的误差，也会因远离基准线而变大。所以考虑在基准线正方向上，对远离相机的且同时远离基准线的位置进行相对更少的采样，这样能对平衡系统误差作出更大的贡献。

在这样的基础上，根据后续的运动数据以及的测量数据的实时更新，不断地调整粒子的分布、位置和姿态，能得到更精准的位置信息，使定位设备能实现更高精度的定位。

在一个实施例中，“在基准线的侧向上，粒子的密度递减”包括：所述粒子的数量以所述基准线为中心，沿侧向的分布遵循一元高斯分布规律；所述第二条件包括：以所述基准线为中心，所述区域单元内粒子分布对应的标准差，在基准线正方向上递减。如图9所示，可以将关注区域S按Y轴正方向分成相接的多个区域单元，这些区域单元均具有单位宽度L。图9示出了部分的区域单元的单位宽度的标注，还示出了部分区域单元内粒子的分布曲线，在这些区域单元内，粒子的数量N以Y轴为对称轴，沿Y轴两边递减。同样地，粒子的密度也是沿Y轴两边递减的。但是，从图中的曲线可以看出，不同的区域单元内，粒子的密度分布是不一样的，即具有不同的标准差，在二维平面上，用公式表示，可以是这样的：

公式一：

公式二：

公式三：

上述公式式中，p(x)表示粒子出现的概率，x表示变量(一个向量)，μ表示平均向量，Σ表示协方差矩阵(Covariance Matrix，是一个方阵)，|Σ|表示Σ的行列式值，(x-μ)^T表示矩阵(x-μ)的转置，σ_x表示X方向上的标准差，σ_y表示Y方向上的标准差。公式一和公式二是二维高斯分布公式，公式三意味着，在关注区域内，随着y的增大，σ_x是递减的。

本领域技术人员可以理解的是，每一次生成的粒子群的状态分布不一定百分百遵循上述公式，但随着统计次数的增多，粒子群的状态分布总体上会趋向于前面公式揭示的规律。需要注意的是，上述的公式仅代表一种实施的情况，基于前文或者这些公式，本领域技术人员可以想到更多的其他实施例。

“粒子的数量及密度大于预设值”可以这样理解：一般而言，粒子的数量的预设值与总数的比例可以是85％-95％，当然也可以是其他的比例，主要看实际需要；同理地，粒子密度的预设值可以是粒子总体密度的2-5倍，当然也可以是其他比例。

需要说明的是，实施本公开所述的方案，并非一定需要构建关注区域及其边界函数(当然在一些实施例中也可以这样构建)，即，关注区域可以是假想的。本文提到的关注区域，仅是为了方便理解以及方便表达第一约束条件以及第二约束条件而构造出的一个概念。

在一个实施例中，所述关注区域的边界形状包括以所述自定位坐标为重心而构成的图形；所述标签坐标不在所述关注区域内。

在一个实施例中，所述边界形状包括三角形、梯形或扇形。

我方的概率分布模型主要适用于二维栅格地图，以图5作为例子，以O点(O点为自定位坐标所在栅格地图中的位置)和R点(R点为栅格地图中的标签坐标所在的位置)作为Y轴，O点指向R点的方向为Y轴正方向，X轴正方向为图中X轴箭头所指的方向。在该参考坐标系基础上，形成一关注区域S，图中该区域的边界形状为扇形，扇形的顶点为R点，扇形的夹角为图中角α的两倍。至少在该关注区域S内，粒子的分布以基准线(Y轴所在位置)为基准，在X轴上遵循一元高斯分布规律的约束，意味着远离基准线的区域粒子的数量较少，在Y轴方向上使粒子的数量递减(可以是线性递减、可以是指数递减也可以采用其他递减函数进行约束)，需要注意的是，O点应该与扇形的弧边具有一定距离，以确保O点被粒子所包围，一些情况下，O点可以位于扇形的重心所在位置。图6是另一个例子，示出了关注区域S的边界形状为梯形的情况。图4则是又一个例子，示出了关注区域S的边界形状为三角形的情况。

在一个实施例中，更新粒子的状态前，获取里程计提供的运动数据。

在一个实施例中，“基于视觉自定位，获得标签坐标及自定位坐标的信息”包括：获取由相机生成的场景的图像；识别所述图像中的标签的特征信息，根据所述特征信息向标签信息库发送匹配请求；获得标签信息库响应所述匹配请求而返回的标签坐标的信息；根据所述标签坐标的信息获得自定位坐标的信息。

一般来说，标签的特征信息包括了标签的边缘坐标，例如：当标签是矩形时，特征信息包括了矩形的四个角的相关像素的坐标(或称角点的像素坐标)；当标签是其他多边形时，特征信息包括了部分或全部的角、边的相关的像素坐标；当标签是具有不同色彩的图案时，特征信息包括了图案边沿或中心或其他部位的角、边的相关的像素坐标。总的来说，标签的特征信息的选取或者预设，应符合突出的特性(一般可以考虑色彩或形状特性)，使在进行图像识别时，能快速被区分于图像的背景或其他内容。

标签信息库不同于栅格地图数据库，前者用于存放标准的标签的特征信息以及该标签的位置信息的相关关系数据，后者用于存放栅格地图数据。

标签信息库预存了场景中的多个标签的特征信息与对应标签所在的(空间)位置信息(即标签坐标的数据)的对应关系，使得通过根据特征信息可以搜索(或匹配)到标签信息库中的预存的对应的标签坐标数据，该标签坐标数据可以包括标签整体或其相关部位(如某个边沿、某个角)的世界坐标，也可以包括标签整体或其相关部位在场景中的相对坐标。如此，根据成像原理，便可依据标签坐标计算出摄像模块或者摄像模块光心的位置(即自定位坐标)。例如，可以在图像中提取标签角点的像素坐标数据；根据所述标签角点的世界坐标数据以及像素坐标数据，运用PnP算法求解，获得所述自定位坐标的数据；所述自定位坐标的数据包括摄像模块的位置(包括坐标)以及姿态(包括朝向角度或航向角)数据。

PnP(Perspective-n-Point，透视n点投影)算法属于现有技术。PnP问题起源于1841年的摄影测量学领域，由Fishler和Bolles于1981年正式提出，是计算机视觉研究领域的一个经典问题。PnP问题具体指已知相机内部参数和n个3D/2D参考点分别在世界坐标系下的坐标和图像坐标系下的坐标，利用这n个参考点之间的对应关系，求解相机在拍摄图像时所处的位姿。例如，当n的取值为3时，如图7所示，场景中标签的三个角点A、B、C在图像中的投影点为a、b、c，根据几何关系即可求出摄像头或摄像单元的光心O点的位置坐标，这可以直接采用现有技术。

在一个实施例中，可以获取由相机生成的场景的图像，所述相机至少具有两个摄像头；对至少两个所述摄像头同步采集到的所述图像进行处理，获得图像视差数据；根据所述图像视差数据，生成像素深度数据；根据所述像素深度数据，生成视觉点云数据；所述栅格地图数据，由激光点云生成；以所述栅格地图数据格式为标准，对所述视觉点云数据进行格式转换，得到测量数据；“基于所述粒子群，对粒子的状态和分布进行更新”包括：根据运动数据以及运动模型更新粒子的状态；根据测量数据以及测量模型修正粒子的状态和分布。

如此，利用可实时处理而获得的视觉点云数据与栅格地图数据进行匹配，使得定位设备可以在不使用激光雷达进行测量的情况下而很好地利用了激光点云数据构建的栅格地图进行定位，同时由于不需要借助相对低精度的视觉地图进行定位，获得的定位结果具有较高精度。

一般来说，相机需要具备两个或更多的摄像头，这样能从不同角度对场景的物体的至少一个部位进行拍摄，在一个时刻同步地获得该物体的至少一个部位的两张以上的图像。如此，对不同摄像头在同一时刻拍摄到的物体的同一个部位的图像进行处理，结合摄像头之间的距离(可以理解为相机的基线)，利用现有技术即可算出图像中像素的深度，并处理得到视觉点云数据。

对于视觉点云数据，现有技术一般是采用预先生成的视觉点云地图作为匹配(或配准)的基础，通过匹配，计算得到摄像头在视觉点云地图的空间体系下的的位置，或者通过匹配，推算出定位设备在视觉点云地图的空间体系下的的位置，从而实现定位。

对于视觉点云数据，无需利用视觉点云地图作为匹配的基础，而是利用激光点云地图(即栅格地图数据)作为匹配的基础，这样的好处包括但不限于：

(1)提供了一种新的定位方法，使得相机获得的图像经过处理后，能适用于激光点云构建出来的栅格地图,即无视觉点云地图的情况下也可灵活地实现定位；

(2)一般来说，由于激光点云建图比视觉点云建图具有诸多优势(如可靠性更高、技术更成熟、精度高、不存在累计误差等)，因此栅格地图采用激光点云地图作为匹配基础，对于提高定位精度具有更可靠之处；

(3)激光雷达的成本较高(例如当前的价格从几万到十几万元不等)，用户的定位设备很有可能不适合配置激光雷达，去实现激光点云的实时定位，但本公开提出的方法，只需定位设备具有相机，适用范围极广；

(4)在无需对市面上大多数定位设备的硬件进行改造的基础上，保证了定位的精度。

视觉点云数据的格式、激光点云数据的格式、栅格地图数据的格式，这些格式本身都是现有技术，其种类在此不作展开。

作为补充而对部分的现有技术进行说明，视觉点云数据是根据对相机获得的图像进行处理，生成深度图(深度图由像素深度数据组成)而求得的。测量像素深度的原理类似人通过左右眼看到的景物差异(视差)来判断景物到人之间的距离。图8为双目相机的成像示意图，意味着该相机具有两个摄像头，两个摄像头(或摄像头)在一个时刻同步采集到的两幅图像为第一图像11和第二图像12(图像的格式通常是YUV或RGB格式，本文提及的图像或图像数据并不在格式上进行限制，格式只是图像的存储方式，并非重点，无论采取何种格式，最终计算时都会从对应图像格式转换或解码得到每个像素的像素值)，凭借这两幅图像可以计算得到该时刻的空间点Q在图像中的像素的深度值。图中，线段O₁O₂的长度，与两个摄像头间的距离一致，该距离为相机的基线(记为b)的长度，相机的焦距记为f，对于空间点Q的投影到第一图像11和第二图像12上的像素点Q₁、Q₂的横坐标分别记为UL和UR，Q点的视差(Disparity)记为d，像素的深度值记为Z，根据以下公式可求得Q点对应的像素深度Z：

Z＝(f·b)/d，d＝UL-UR；

视差的求解可通过第一图像11和第二图像12之间的立体匹配求得，常用的立体匹配算法有BM(Boyer-Moore)和SGBM(Semi-Global Block Matching)，由于计算量比较大，通常需要使用GPU或FPGA来计算。一个实施例中，使用海思3559的DPU来进行处理，得到视差图，并且视差值是亚像素的，处理速度可达到每秒30帧。

求得像素深度值Z后，还需通过以下公式计算每个像素点的3D坐标(X、Y、Z)：

即依据上式所展示的关系，可以通过深度图(多个像素的深度值的集合)进行坐标转换，进而得到点云数据；上式的K为相机的内参矩阵，u和v分别为像素点的横纵像素坐标，f_x、f_y简称为相机不同方向上的焦距(focal-length)，c_x、c_y为相对于成像平面的主点坐标(principal point)；求得每个像素对应的3D坐标后，就得到定位所用的视觉点云数据。

本领域技术人员可以理解的是，视差与距离成反比：视差越大，距离越近。由于视差的最小值为一个像素，于是相机的视觉深度存在一个理论最大值，由(f·b)的值所限制。所以当基线越长时，相机的两个摄像头具有较大的测量距离，反之，短基线的两个摄像头的测量距离也较短。

为了保障测量数据的精度，在一个实施例中，所述相机包括依次排列的第一摄像头、第二摄像头；以第一方式处理所述图像包括：处理后获得第一摄像头以及第二摄像头的视觉点云数据P₁；对视觉点云数据P₁进行筛选，得到点云集合P_a，使P_a＝{P₁≤T}，其中T为设定的距离阈值，{P₁≤T}为视觉点云数据P₁中，与所述相机的距离在T以内的景物对应的点云数据；以所述点云集合P_a作为用于生成测量数据的视觉点云数据。

距离阈值T的选取通常是根据相机在不同的距离(即拍摄时相机与景物的距离，也可以理解为物距)下的测距精度来确定的。具体可以通过选取场景里景物作为标记点，通过处理图像可以得到标记点对应的像素点，根据视差图数据可以求得深度值D₁，然后利用更精密的仪器或者方式进行测量(包括人工用尺子测量)，得出该标记点到相机的真实距离D₂，D₁和D₂之间数值的差距可以理解为测距误差或下文的标准精度误差值，据此可以得到在特定距离下的测距精度。同理，可以选取场景里不同位置、与相机具有不同距离的多个点(或多个景物)作为标记点，从而获得更多的关于测距精度的数据。

本领域技术人员可以根据实际的场合需求，灵活地对T进行赋值。例如，若测距误差小于0.5米可被认为符合精度要求，且相机进行拍摄后，得到的在小于10米的范围内的图像的部分，可以满足0.5米的测距精度要求，则可以对T赋值为10(米)。又例如，若测距误差小于0.3米可被认为符合测距精度要求，且相机进行拍摄后，得到的在小于5米的范围内的图像的部分，可以满足0.3米的测距精度要求，则可以对T赋值为5(米)。

考虑到对于空间较大、较空旷的场景，只用一个基线长度的相机无法满足不同物距下的均保持较高测距精度的要求，在一个实施例中，所述相机包括依次排列的第一摄像头、第二摄像头以及第三摄像头；以第一方式处理所述图像包括：处理后获得第一摄像头以及第二摄像头的视觉点云数据P₁；处理后获得第一摄像头以及第三摄像头的视觉点云数据P₂；进行点云数据融合，得到点云集合P_b，P_b＝{P₁≤T}∪{P₂＞T}；其中T为设定的距离阈值，{P₁≤T}为视觉点云数据P₁中，与所述相机的距离在T以内的景物对应的点云数据，{P₂＞T}为视觉点云数据P₂中，与所述相机的距离大于T的景物对应的点云数据；以所述点云集合P_b作为用于生成测量数据的视觉点云数据。如此，短基线得到的视觉点云数据P₁以及长基线得到的视觉点云数据P₂在图像质量上有互补之处，通过设置T值对两个点云数据进行有目的的筛选，将筛选出来的点云数据进行融合，得到的点云集合P_b能确保较高的精度，进一步地，利用点云集合P_b作为用于生成测量数据的视觉点云数据，与栅格地图数据进行匹配后，生成的视觉定位数据能更准确地反映出相机的位置信息。

一个实施例中，可以选取分辨率为1080*1920，内参f_x＝1174.548331631的相机，三个摄像头的水平间隔依次为10厘米，则短基线和长基线分别为10厘米和20厘米，此时可以令T＝10(米)。本领域技术人员可以根据实际的相机的性能以及对测距精度的要求，作其他的参数配置，在此不作过多展开。

事实上，相机的摄像头数量可以不限于两个或三个，还可以是更多的；摄像头之间的距离可以是相等的，也可以是不等的，这样多个摄像头的距离的组合可以形成多个不同长度的基线的组合，这样，对不同摄像头得到的图像，依据不同T值而取不同部分的像素点进行处理，最后得到具有更高精度的点云集合。

在一些方面，本公开还提出了一种定位装置，包括：自定位模块，用于基于视觉自定位，获得标签坐标及自定位坐标的信息；粒子生成模块，用于基于栅格地图数据，调用所述概率分布模型生成新的粒子群；粒子更新模块，用于基于所述粒子群的状态，对粒子的状态和分布进行更新；解算模块，用于计算粒子评分并输出定位结果。

在一些方面，本公开还提出了建图及定位的方法，包括以下步骤：调用激光雷达对场景进行扫描，处理获得激光点云数据；对所述激光点云数据进行处理，生成栅格地图数据；调用相机对场景进行拍摄，获得场景的图像，所述相机至少具有两个摄像头；对所述图像进行处理，获得视觉点云数据；根据所述的定位方法，生成新的粒子群；以所述栅格地图数据格式为标准，对所述视觉点云数据进行格式转换，得到测量数据；根据获得的运动数据以及运动模型更新粒子的状态；根据所述测量数据以及测量模型修正粒子的状态和分布；计算粒子评分并输出定位结果。

本文提及的各类定位或者图像处理方法，一般需要将不同坐标系下的对象，在同一参考坐标系下进行配准处理，但有关坐标系或者基准点的统一属于公知常识，且不属于本公开的技术方案的重点，在此不作过多展开。

本领域技术人员可以理解的是，实施例中的全部或部分步骤，可以通过计算机程序来指令相关的硬件实现，该程序可以存储于计算机可读介质中，可读介质可以包括闪存盘、移动硬盘、只读存储器、随机存取器、磁盘或光盘等各种可以存储程序代码的介质。

在一个实施例中，本公开还提出了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如所述的基于粒子滤波的定位方法。

在符合本领域技术人员的知识和能力水平范围内，本文提及的各种实施例或者技术特征在不冲突的情况下，可以相互组合而作为另外一些可选实施例，这些并未被一一罗列出来的、由有限数量的技术特征组合形成的有限数量的可选实施例，仍属于本公开揭露的技术范围内，亦是本领域技术人员结合附图和上文所能理解或推断而得出的。

另外，多数实施例的描述是基于不同的重点而展开的，未详述之处，可参见现有技术的内容或本文的其他相关描述进行理解。

再次强调，上文所列举的实施例，为本公开较为典型的、较佳实施例，仅用于详细说明、解释本公开的技术方案，以便于读者理解，并不用以限制本公开的保护范围或者应用。在本公开的精神和原则之内所作的任何修改、等同替换、改进等而获得的技术方案，都应被涵盖在本公开的保护范围之内。

Claims (10)

1.基于粒子滤波的定位方法，其特征在于，包括以下步骤：

基于视觉自定位，获得标签坐标及自定位坐标的信息；

基于栅格地图数据，调用概率分布模型生成新的粒子群；

所述概率分布模型包括第一约束条件以及第二约束条件；

以穿过所述自定位坐标与所述标签坐标之间的直线为基准线，以所述自定位坐标指向所述标签坐标的方向为基准线正方向，以粒子的数量及密度大于预设值的区间作为关注区域时，所述第一约束条件包括：在关注区域内，粒子围绕所述自定位坐标分布，在所述基准线的侧向上，粒子的密度递减；

所述第二约束条件包括：在关注区域内，在基准线正方向上，粒子分布的区域单元的面积递减；

基于所述粒子群的状态，对粒子的状态和分布进行更新；其步骤包括：根据运动数据以及运动模型更新所述粒子的状态；根据测量数据以及测量模型修正粒子的状态和分布；

计算粒子评分并输出定位结果。

2.根据权利要求1的定位方法，其特征在于：

所述“在基准线的侧向上，粒子的密度递减”包括：所述粒子的数量以所述基准线为中心，沿侧向的分布遵循一元高斯分布规律；

所述第二约束条件包括：以所述基准线为中心，所述区域单元内粒子分布对应的标准差，在基准线正方向上递减。

3.根据权利要求1的定位方法，其特征在于：

所述关注区域的边界形状包括以所述自定位坐标为重心而构成的图形；所述标签坐标不在所述关注区域内。

4.根据权利要求3的定位方法，其特征在于：

所述边界形状包括三角形、梯形或扇形。

5.根据权利要求1的定位方法，其特征在于，还包括以下步骤：

获取由相机生成的场景的图像，所述相机至少具有两个摄像头；

对至少两个所述摄像头同步采集到的所述图像进行处理，获得图像视差数据；

根据所述图像视差数据，生成像素深度数据；

根据所述像素深度数据，生成视觉点云数据；

所述栅格地图数据，由激光点云生成；

以所述栅格地图数据格式为标准，对所述视觉点云数据进行格式转换，得到测量数据。

6.根据权利要求5的定位方法，其特征在于，还包括以下步骤：

更新粒子的状态前，获取里程计提供的运动数据。

7.根据权利要求1的定位方法，其特征在于：

“基于视觉自定位，获得标签坐标及自定位坐标的信息”包括：

获取由相机生成的场景的图像；

识别所述图像中的标签的特征信息，根据所述特征信息向标签信息库发送匹配请求；

获得标签信息库响应所述匹配请求而返回的标签坐标的信息；

根据所述标签坐标的信息获得自定位坐标的信息。

8.执行权利要求1所述的定位方法的定位装置，其特征在于，包括：

自定位模块，用于基于视觉自定位，获得标签坐标及自定位坐标的信息；

粒子生成模块，用于基于栅格地图数据，调用所述概率分布模型生成新的粒子群；

粒子更新模块，用于基于所述粒子群的状态，对粒子的状态和分布进行更新；

解算模块，用于计算粒子评分并输出定位结果。

9.计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1至7中任一项所述的基于粒子滤波的定位方法。

10.建图及定位的方法，其特征在于，包括以下步骤：

调用激光雷达对场景进行扫描，处理获得激光点云数据；

对所述激光点云数据进行处理，生成栅格地图数据；

调用相机对场景进行拍摄，获得场景的图像，所述相机至少具有两个摄像头；

对所述图像进行处理，获得视觉点云数据；

根据权利要求1所述的定位方法，生成新的粒子群；

以所述栅格地图数据格式为标准，对所述视觉点云数据进行格式转换，得到测量数据；

根据获得的运动数据以及运动模型更新粒子的状态；

根据所述测量数据以及测量模型修正粒子的状态和分布；

计算粒子评分并输出定位结果。

Images