一种激光雷达定位的方法、装置及系统与流程

本发明涉及雷达定位领域，尤其涉及一种激光雷达定位的方法、装置及系统。

背景技术：

在当今社会，随着科技的不断进步，自动驾驶及其相关技术成为当今社会研究的热点，而在上述热点技术之中，雷达定位技术又是其中重点研究的问题。由于激光雷达发射激光束的频率可以从每秒几个脉冲到每秒几万个脉冲，其接收器没分钟可以接收到几十万个点；所以激光雷达具有许多独特的优点，例如极高的距离分辨率和角分辨率、抗干扰能力强、体积与重量小等，激光雷达进行定位是当下定位技术的重要应用。激光雷达定位技术通常是使用在车辆、飞行器、潜水艇等移动平台之中，用于对当前移动平台进行定位。

在现有的激光雷达定位技术中，二维激光雷达定位技术已经日渐趋于成熟。二维激光雷达定位通常是使用激光在一个平面上获取周边环境的信息，并根据获取到的周边信息以及当前移动平台的数据，例如当前移动平台的速度、加速度等，计算得到当前移动平台的位置，对当前移动平台进行定位。

然而二维激光雷达定位在具体的使用过程中，由于其对于周边环境中固定参考物的依赖很大，当周边环境发生微小变化时，其测量精度就会显著降低。

技术实现要素：

有鉴于此，本发明的主要目的在于提供一种激光雷达定位的方法，可以减少由于周边环境发生微小变化所引起的定位误差；本发明的另一目的在于提供一种激光雷达定位的装置及系统，可以减少由于周边环境发生微小变化所引起的定位误差。

为了解决上述技术问题，本发明提供了一种激光雷达定位的方法，所述方法包括：

根据通过三维激光雷达装置获取到的移动平台周边环境的三维数据、通过惯性测量装置获取到的所述移动平台的惯性数据、通过里程计装置获取到的所述移动平台的里程数据，生成三维点云地图；

根据所述三维点云地图计算出当前移动平台的位姿。

可选的，所述根据所述三维点云地图计算出当前移动平台的位姿包括：

获取所述三维点云地图中初始图像的位置；

根据所述初始图像的位置、所述惯性数据和所述里程数据，计算出当前所述移动平台的位姿推测值，将所述位姿推测值作为当前所述移动平台的位姿。

可选的，所述根据所述三维点云地图计算出当前移动平台的位姿包括：

根据所述三维点云地图和所述三维数据，计算出当前移动平台的位姿观测值，将所述位姿观测值作为当前所述移动平台的位姿。

可选的，在计算出当前所述移动平台的位姿推测值之后，所述方法进一步包括：

根据所述三维点云地图和所述三维数据，计算出当前移动平台的位姿观测值；

通过卡尔曼滤波器，根据所述当前移动平台的位姿递推值和当前移动平台的位姿观测值，计算出当前移动平台的最优位姿，将所述最优位姿作为当前所述移动平台的位姿。

可选的，在生成所述三维点云地图之后，所述方法进一步包括：

对所述三维云点地图进行栅格化、去噪点和去杂点处理。

可选的，所述根据通过三维激光雷达装置获取到的移动平台周边环境的三维数据、通过惯性测量装置获取到的所述移动平台的惯性数据、通过里程计装置获取到的所述移动平台的里程数据，生成三维点云地图包括：

根据通过所述三维激光雷达装置获取到的所述移动平台周边环境的所述三维数据、通过六轴惯性测量装置获取到的所述移动平台的六维惯性数据、通过所述里程计装置获取到的所述移动平台的所述里程数据，生成所述三维点云地图，所述六维惯性数据包括x轴加速度、y轴加速度、z轴加速度、航向角、俯仰角和横滚角；

所述根据所述三维点云地图和所述三维数据，和/或根据所述三维点云地图、所述惯性数据和所述里程数据，计算出当前移动平台的位姿包括：

根据所述三维点云地图和所述三维数据，和/或根据所述三维点云地图、所述六维惯性数据和所述里程数据，计算出当前移动平台的六自由度位姿，所述六自由度位姿包括：x轴位置、y轴位置、z轴位置、航向角度数、俯仰角度数和横滚角度数。

本发明还提供了一种激光雷达定位的装置，所述装置包括：

地图采集构建模块：用于根据通过三维激光雷达装置获取到的移动平台周边环境的三维数据、通过惯性测量装置获取到的所述移动平台的惯性数据、通过里程计装置获取到的所述移动平台的里程数据，生成三维点云地图；

定位模块：用于根据所述三维点云地图计算出当前移动平台的位姿。

可选的，所述定位模块包括：

初始定位单元：用于获取所述三维点云地图中初始图像的位置；

位姿递推值计算单元：用于根据所述初始图像的位置、所述惯性数据和所述里程数据，计算出当前所述移动平台的位姿推测值；

位姿观测值计算单元：用于根据所述三维点云地图和所述三维数据，计算出当前移动平台的位姿观测值；

卡尔曼滤波单元：用于通过卡尔曼滤波器，根据所述当前移动平台的位姿递推值和当前移动平台的位姿观测值，计算出当前移动平台的最优位姿，将所述最优位姿作为当前所述移动平台的位姿。

可选的，在所述地图采集构建模块之后，所述装置进一步包括：

地图预处理模块：用于对所述三维云点地图进行栅格化、去噪点和去杂点处理。

本发明还提供了一种激光雷达定位的系统，所述系统包括：

三维激光雷达装置：用于获取移动平台周边环境的三维数据；

惯性测量装置：用于获取所述移动平台的惯性数据；

里程计装置：用于获取到的所述移动平台的里程数据；

处理器：用于根据所述三维数据、所述惯性数据和所述里程数据生成三维点云地图，并根据所述三维点云地图计算出当前移动平台的位姿。

本发明所提供的一种激光雷达定位的方法，可以获取移动平台周边环境的三维数据，并通过所述三维数据最终计算出当前移动平台的位姿。

由于本发明获取到的是移动平台周边环境的三维数据，相比于二维激光雷达获取的移动平台周边环境的二维数据，本发明提供的方法获取了更多有关移动平台周边环境的信息，当移动平台周边环境发生微小变化时，其对全局的影响就显著变小，从而减少了由于周边环境发生变化所引起的定位误差。本发明还提供了一种基于上述激光雷达定位方法的激光雷达定位装置及系统，同样具有上述有益效果，在此不再赘述。

附图说明

为了更清楚的说明本发明实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例所提供的第一种激光雷达定位方法的流程图；

图2为本发明实施例所提供的第二种激光雷达定位方法的流程图；

图3为本发明实施例提供的一种激光雷达定位装置的结构框图；

图4为本发明实施例提供的一种激光雷达定位系统的结构框图。

具体实施方式

本发明的核心是提供一种激光雷达定位的方法。在现有技术中，通常是使用二维激光雷达获取移动平台周边环境的二维数据，最终在计算出当前移动平台的位姿。但是由于二维激光雷达只能获取移动平台周边一个平面的二维数据，其对环境中固定参考物的依赖性非常大，当周变环境发生微小变动时，例如风吹草木移动等，就会严重影响到激光雷达定位的准确性。

而本发明所提供的激光雷达定位的方法，可以获取移动平台周边环境的三维数据，相比于二维激光雷达获取的二维数据，本发明提供的方法获取了更多有关移动平台周边环境的信息，当移动平台周边环境发生微小变化时，其对全局的影响就显著变小，从而降低了对环境中固定参考物的依赖性，减少了由于周边环境发生变化所引起的定位误差。

为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，图1为本发明实施例所提供的第一种激光雷达定位方法的流程图，该方法包括：

s101：根据通过三维激光雷达装置获取到的移动平台周边环境的三维数据、通过惯性测量装置获取到的所述移动平台的惯性数据、通过里程计装置获取到的所述移动平台的里程数据，生成三维点云地图。

在本发明实施例中，移动平台是可以移动的物体，例如飞行器、车辆、潜水器等，均可以是移动平台，不仅仅限于车辆，所以关于移动平台的构造在此不做具体限定。

在本步骤中，三维激光雷达装置获取移动平台周边环境的三维数据的方式，通常是向移动平台的周边环境发射一束或多束激光进行扫描，在经过光子接收部分收集发射的激光。发射出去的激光会与环境发生作用，在接收到激光之后，会根据该激光所携带的数据描绘出移动平台的周变环境。其中，所述携带的数据通常是激光从发射到接收所经过的时间，根据所述时间计算出周边环境中各个物体的距离；当然还可以获取其他数据，例如入射角与反射角、激光光强等，可以进一步计算出周边环境中各个物体的表面折射率和其他的数据。当然，三维激光雷达装置还可以根据其他的方式获取移动平台周边环境的三维数据，其具体方式在此不做具体限定。

当所述三维激光雷达装置对移动平台周边环境进行扫描时，作为优选的，可以进行闭环地图数据扫描。所述闭环地图数据扫描为所述扫描的起点和终点为同一个实际位置点，此时可以给后续生成三维点云地图时，提供变形修正数据，以便生成更加精准的三维点云地图。

在本发明实施例中，所述惯性测量装置可以获取到移动平台的惯性数据，其中惯性测量装置至少包括有加速度传感器，用于测量移动平台在x轴、y轴、z轴三个方向上的加速度。通常情况下，惯性测量装置除了加速度传感器，还会包括有陀螺仪，此时惯性传感器通常被称作是六维惯性测量装置。六维惯性测量装置除了可以测量移动平台在x轴、y轴、z轴三个方向上的加速度，还可以测量移动平台的航向角、俯仰角和横滚角，其中，根据x轴、y轴、z轴三个方向上的加速度可以进一步计算出当前移动平台所处的空间位置；航向角可以用来控制移动平台的姿态，使移动平台沿导航路径的切线角移动；俯仰角和横滚角可以用来递推当前移动平台所处的空间位置。当然，根据不同的实际情况，还可以使用不用的惯性测量装置，视具体的情况而定，在此不做具体限定。

在本发明实施例中，可以将所述里程计装置更换为高精度里程计装置，用于获取高精度里程数据。所述高精度里程数据比所述里程数据的精度更高。

在本发明中，由于移动平台是在移动的过程中实时获取当前移动平台的数据以及周边环境的数据，所以移动平台会获取大量移动帧图像，而生成三维点云地图的方法是将全部所述移动帧图像的数据通过匹配坐标变换统一折合到初始图像的坐标系里，从而生成三维点云地图。所述匹配坐标变换是根据测量各个移动帧图像的三维数据时移动平台的惯性数据和里程数据，来估算出各个移动帧图像关于周边环境中的物体的位姿，并加以三维数据共同优化修正得到所述三维点云地图。

s102：根据所述三维点云地图计算出当前移动平台的位姿。

在本步骤中，通常情况下是根据所述三维点云地图，并辅助以上述获取到的三维数据、惯性数据、里程数据来计算出当前移动平台的位姿。具体计算过程将在下述实施例中做详细描述，在此不再进行赘述。

当所述移动平台是使用六维惯性测量装置时，相应的也应该是计算出当前移动平台的六自由度位姿，所述六自由度位姿包括：x轴位置、y轴位置、z轴位置、航向角度数、俯仰角度数和横滚角度数。若使用其他惯性测量装置，对应的也应该生成与惯性测量装置相适应的位姿。

本发明实施例所提供的一种激光雷达定位的方法，可以获取到的是移动平台周边环境的三维数据，相比于二维激光雷达获取的移动平台周边环境的二维数据，本发明提供的方法获取了更多有关移动平台周边环境的信息，当移动平台周边环境发生微小变化时，其对全局的影响就显著变小，从而减少了由于周边环境发生变化所引起的定位误差。

在本发明中，根据三维点云地图计算出当前移动平台的位姿的方法有很多种，将在下述实施例中做详细描述。

请参考图2，图2为本发明实施例所提供的第二种激光雷达定位方法的流程图，该方法包括：

s201：根据通过三维激光雷达装置获取到的移动平台周边环境的三维数据、通过惯性测量装置获取到的所述移动平台的惯性数据、通过里程计装置获取到的所述移动平台的里程数据，生成三维点云地图。

本步骤与上述实施例中s101相同，其中具体细节已在上述实施例中做详细描述，在此不再赘述。

s202：对所述三维云点地图进行栅格化、去噪点和去杂点处理。

在本步骤中，可以对在s201中生成的三维点云地图进行栅格化、去噪点和去杂点处理。其目的是初步排除由于仪器精度所带来的误差，使三维点云地图更加接近于实际情况。当然，还可以进行其他的处理，使得三维点云地图更加接近于实际情况，在此不仅限于上述三种处理方法。

s203：获取所述三维点云地图中初始图像的位置。

在本步骤中，所述初始图像的位置可以是自动计算出来的，也可以是通过人工标定的，在此不做具体限定。所述初始图像通常是三维点云地图的第一帧图像，当然也可以是其他的移动帧图像，在此不做具体限定。通常情况下，三维点云地图中大量移动帧图像的数据也是通过匹配坐标变换统一折合到所述初始图像的坐标系里。

s204：根据所述初始图像的位置、所述惯性数据和所述里程数据，计算出当前所述移动平台的位姿推测值。

在本步骤中，会在s203中获取的初始图像的位置的基础上，根据所述惯性数据和所述里程数据递推得到当前所述移动平台的位姿推测值。此时位姿推测值就可以作为当前移动平台的位姿。

但是由于惯性测量装置与里程计装置自身在测量数据时就会存在误差，还有移动平台在移动过程中可能出现打滑等原因，位姿推测值会随着时间的推移越来越偏离实际位姿。

s205：根据所述三维点云地图和所述三维数据，计算出当前移动平台的位姿观测值。

在本步骤中，会先根据当前测量的三维数据与所述三维点云地图做最优匹配，以此来确定当前移动平台在三维点云地图中的位置，之后再通过大量计算，得到当前移动平台的位姿观测值，此时位姿观测值同样可以作为当前移动平台的位姿。

但是由于计算位姿观测值的数据主要来自三维激光雷达装置，当移动平台的周边环境大体相似或者相同时，就会极大的增加计算位姿观测值时出错的概率。

s206：通过卡尔曼滤波器，根据所述当前移动平台的位姿递推值和当前移动平台的位姿观测值，计算出当前移动平台的最优位姿，将所述最优位姿作为当前所述移动平台的位姿。

在本步骤中，会通过卡尔曼滤波器将s204中计算得到的位姿推测值与s205中计算得到的位姿观测值进行融合，即把上述位姿推测值与位姿观测值做高斯概率分布，通过线性最优递归位姿估计最终计算处当前移动平台的最优位姿，其中，最优位姿是一个概率收敛函数，其最终会在实际位置的坐标值的上下以微小的变化跳动。此时最优位姿就是当前移动平台的位姿。

除了上述计算当前移动平台的位姿的方法，还可以使用其他方法计算当前移动平台的位姿，本发明实施例仅提供一种具体的方法，但本发明并不仅限于使用此方法计算当前移动平台的位姿。

本发明实施例所提供的一种激光雷达定位的方法，可以计算出当前移动平台的位姿推测值与位姿观测值，并通过卡尔曼滤波器计算出当前移动平台的最优位姿。本发明实施例所提供的方法，可以极大的减少由于计算位姿方法本身所拥有的误差，可以更加精确的对当前移动平台进行定位。

下面对本发明实施例提供的一种激光雷达定位装置进行介绍，下文描述的激光雷达定位装置与上文描述的激光雷达定位方法可相互对应参照。

图3为本发明实施例提供的一种激光雷达定位装置的结构框图，参照图3激光雷达定位装置可以包括：

地图采集构建模块100：用于根据通过三维激光雷达装置获取到的移动平台周边环境的三维数据、通过惯性测量装置获取到的所述移动平台的惯性数据、通过里程计装置获取到的所述移动平台的里程数据，生成三维点云地图。

定位模块200：用于根据所述三维点云地图计算出当前移动平台的位姿。

在本发明实施例中，定位模块200可以包括：

初始定位单元201：用于获取所述三维点云地图中初始图像的位置。

位姿递推值计算单元202：用于根据所述初始图像的位置、所述惯性数据和所述里程数据，计算出当前所述移动平台的位姿推测值。

位姿观测值计算单元203：用于根据所述三维点云地图和所述三维数据，计算出当前移动平台的位姿观测值。

卡尔曼滤波单元204：用于通过卡尔曼滤波器，根据所述当前移动平台的位姿递推值和当前移动平台的位姿观测值，计算出当前移动平台的最优位姿，所述最优位姿为当前所述移动平台的位姿。

在本发明实施例中，在所述地图采集构建模块之后，激光雷达定位装置还可以进一步包括：

地图预处理模块300：用于对所述三维云点地图进行栅格化、去噪点和去杂点处理。

本实施例的激光雷达定位装置用于实现前述的激光雷达定位方法，因此激光雷达定位装置中的具体实施方式可见前文中的激光雷达定位方法的实施例部分，例如，地图采集构建模块100，定位模块200，地图预处理模块300，分别用于实现上述激光雷达定位方法中步骤s101，s102和s202，所以，其具体实施方式可以参照相应的各个部分实施例的描述，在此不再赘述。

下面对本发明实施例提供的一种激光雷达定位系统进行介绍，下文描述的激光雷达定位装置与上文描述的激光雷达定位方法和激光雷达定位装置可相互对应参照。

图4为本发明实施例提供的一种激光雷达定位系统的结构框图，参照图4激光雷达定位装置可以包括：

三维激光雷达装置400：用于获取移动平台周边环境的三维数据。

惯性测量装置500：用于获取所述移动平台的惯性数据。

里程计装置600：用于获取到的所述移动平台的里程数据。

处理器700：用于根据所述三维数据、所述惯性数据和所述里程数据生成三维点云地图，并根据所述三维点云地图计算出当前移动平台的位姿。

进一步的，本发明实施例中，所述系统可以进一步包括：

存储器800：用于存储所述三维数据、所述惯性数据、所述里程数据、所述三维点云地图和所述当前移动平台的位姿。

进一步的，本发明实施例中，所述惯性测量装置500可以具体为：

六轴惯性测量装置：用于获取所述移动平台的六维惯性数据，所述六维惯性数据包括x轴加速度、y轴加速度、z轴加速度、航向角、俯仰角和横滚角。

相应的，存储器应该存储所述六维惯性数据；处理器应该计算出当前移动平台的六自由度位姿，所述六自由度位姿包括：x轴位置、y轴位置、z轴位置、航向角度数、俯仰角度数和横滚角度数。

本实施例的激光雷达定位系统用于实现前述的激光雷达定位方法，其中处理器用于安装前述的激光雷达定位装置。因此激光雷达定位系统中的具体实施方式可见前文中的激光雷达定位方法的实施例部分和激光雷达定位装置的实施例部分，在此不再赘述。

本发明还提供了一种移动平台，其包含上述激光雷达定位系统，并实现了上述激光雷达定位方法；其他部分可以参照现有技术，本文不再展开。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其它形式的存储介质中。

以上对本发明所提供的一种激光雷达定位的方法以及装置、系统和移动平台进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。