CN114088093A - 一种点云地图构建方法、装置、系统及存储介质 - Google Patents

Abstract

本发明实施例公开了一种点云地图构建方法、装置、系统及存储介质。该方法应用于点云地图构建系统，点云地图构建系统包括雷达里程计和至少一个预设传感器，该方法可包括：在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值；对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值；将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。本发明实施例的技术方案，通过至少两个传感器采集得到的传感器数据的融合，为点云地图的构建过程提供了更为准确的位姿初始值，由此提高了点云建图的构建精度。

Description

一种点云地图构建方法、装置、系统及存储介质

技术领域

本发明实施例涉及数据处理技术领域，尤其涉及一种点云地图构建方法、装置、系统及存储介质。

背景技术

自动驾驶技术的定位高度依赖于点云地图，因此如何准确高效地构建点云地图在自动驾驶技术中至关重要。

目前通常采用帧间匹配(scan to scan)技术得到当前点云帧的位姿初始值，并根据该位姿初始值将当前点云帧匹配到初步构建得到的中间点云地图上。

在实现本发明的过程中，发明人发现现有技术中存在以下技术问题：车辆快速运动尤其是快速旋转时，基于帧间匹配技术得到的当前点云帧的位姿初始值的准确性下降，这将直接影响点云地图的构建精度。

发明内容

本发明实施例提供了一种点云地图构建方法、装置、系统及存储介质，解决了因位姿初始值的确定精度较低而致使点云地图的构建精度较低的问题。

第一方面，本发明实施例提供了一种点云地图构建方法，该方法可应用于点云地图构建系统，点云地图构建系统包括雷达里程计和至少一个预设传感器，该方法可以包括：

在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值，其中，位姿预测值是根据预设传感器采集到的传感器数据预测出的当前点云帧的位姿数值；

对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值；

将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。

可选的，对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值，可以包括：

根据位姿观测值和位姿预测值得到位姿误差值；

根据位姿误差值确定目标误差值，并根据目标误差值和位姿预测值得到当前点云帧的下一点云帧的位姿初始值。

可选的，根据位姿观测值和位姿预测值得到位姿误差值，可以包括：

位姿观测值包括位置观测值P_lidar，位姿预测值包括位置预测值P_state，位姿误差值包括位置误差值ΔP，通过如下公式计算出ΔP：

ΔP＝P_lidar-P_state；

和/或，位姿观测值包括姿态观测值Q_lidar，位姿预测值包括姿态预测值Q_state，位姿误差值包括姿态误差值Δθ，通过如下公式计算出Δθ：

可选的，在此基础上，该方法还可以包括：

在接收到预设传感器发送的传感器数据时，根据传感器数据和位姿历史值预测出位姿预测值，其中，位姿历史值是在采集到传感器数据的采集时刻的上一时刻预测出的位姿数值。

可选的，根据传感器数据和位姿历史值预测出位姿预测值，可以包括：

传感器数据包括惯性测量单元在采集时刻采集得到的角速度w，位姿历史值包括姿态历史值Q’_state，位姿预测值包括姿态预测值Q_state，通过如下公式预测出Q_state：

其中，Δt是采集时刻和上一时刻间的时间差；

和/或，传感器数据包括轮速计在采集时刻采集到的速度v，位姿历史值包括Q’_state和位置历史值P’_state，位姿预测值包括位置预测值P_state，通过如下公式预测出P_state：

P_state＝P’_state+Q’_state*v*Δt。

可选的，位姿观测值是雷达里程计在对当前点云帧进行匹配后观测到的当前点云帧的位姿数值。

第二方面，本发明实施例还提供了一种点云地图构建装置，该装置配置于点云地图构建系统，点云地图构建系统包括雷达里程计和至少一个预设传感器，该装置可以包括：

位姿获取模块，用于在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值，其中，位姿预测值是根据预设传感器采集到的传感器数据预测出的当前点云帧的位姿数值；

位姿融合模块，用于对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值；

点云地图构建模块，用于将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。

第三方面，本发明实施例还提供了一种点云地图构建系统，该系统可包括：雷达里程计、至少一个预设传感器和卡尔曼滤波器，该卡尔曼滤波器可以包括：

一个或多个处理器；

存储器，用于存储一个或多个程序；

当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现本发明任意实施例所提供的点云地图构建方法。

可选的，卡尔曼滤波器可包括扩展卡尔曼滤波器或是误差卡尔曼滤波器。

第四方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现本发明任意实施例所提供的点云地图构建方法。

本发明实施例的技术方案，在接收到雷达里程计发送的当前点云帧的位姿观测值时，可以获取到与该位姿观测值对应的基于预设传感器采集到的传感器数据预测出的当前点云帧的位姿预测值，其中，位姿观测值和位姿预测值可以分别表示当前点云帧在同一时刻的不同的位姿数值；对位姿观测值和位姿预测值进行融合，由此得到的融合结果是当前点云帧在该同一时刻下的最佳的位姿数值，其可以作为当前点云帧的下一点云帧的位姿初始值，该位姿初始值是与下一点云帧的位姿观测值较为相似的位姿数值；由此，在将该位姿初始值发送给雷达里程计后，可以使得雷达里程计根据该位姿初始值将该下一点云帧准确匹配到中间点云地图上，得到已构建点云地图。上述技术方案，通过对基于预设传感器采集得到的传感器数据预测出的位姿预测值、以及雷达里程计反馈的位姿观测值进行融合，为点云地图的构建过程提供了更为准确的位姿初始值，由此提高了点云建图的构建精度。

附图说明

图1是本发明实施例一中的一种点云地图构建方法的流程图；

图2是本发明实施例一中的一种点云地图构建方法中可选示例的示意图；

图3是本发明实施例二中的一种点云地图构建方法的流程图；

图4是本发明实施例三中的一种点云地图构建方法的流程图；

图5是本发明实施例四中的一种点云地图构建装置的结构框图；

图6是本发明实施例五中的一种点云地图构建系统中卡尔曼滤波器的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

在介绍本发明实施例之前，先对本发明实施例的应用场景进行示例性说明：目前通常采用帧间匹配(scan to scan)技术得到当前点云帧的位姿初始值，该当前点云帧是在当前时刻最新获取到的点云帧，该位姿初始值是该当前点云帧的位姿的初始值，其可以通过对当前点云帧和该当前点云帧的上一点云帧进行匹配得到，该上一点云帧是在当前时刻的上一时刻获取到的点云帧。进一步，雷达里程计可以对该位姿初始值进行优化，并根据优化后的位姿优化值将当前点云帧匹配到初步构建得到的中间点云地图上，该中间点云地图可以是根据当前点云帧的上一点云帧的匹配结果构建得到的点云地图。后续可以将当前点云帧作为上一点云帧，并在再次获取到当前点云帧时，重复执行上述过程以完成点云地图的构建工作。但是，上述技术方案只是基于雷达里程计这一个传感器采集到的传感器数据进行帧间匹配，其在某些情况下匹配得到的位姿初始值的精确度较低，这将直接影响点云地图的构建精度。

为解决这一技术问题，本发明各实施例阐述的点云地图构建方法可应用于点云地图构建系统中，该点云地图构建系统可包括雷达里程计和至少一个预设传感器，该预设传感器的数量可以是一个、两个或是多个，该预设传感器可以是轮速计、惯性测量单元(Inertial Measurement Unit,IMU)、图像采集设备等等。下述点云地图构建方法可以将至少两个传感器采集到的传感器数据融合在一起，并基于融合结果确定位姿初始值，这一位姿初始值的确定过程可通过点云地图构建系统中的卡尔曼滤波器(Kalman Filter，KF)、粒子滤波器等等实现，当然，KF在本发明实施例涉及到的应用场景中的应用效果更好。其中，KF可延伸为拓展卡尔曼滤波器(Extended Kalman Filter，EKF)或是误差卡尔曼滤波器(Error State Kalman Filter，ESKF)，EKF和ESKF是KF的不同的表达方式。

需要说明的是，KF适合于线性系统，EKF适合于非线性系统，它们维护的均是当前时刻的状态量，该状态量在本发明各实施例中可以理解为位姿数值；ESKF不区分线性系统或是非线性系统，其维护的是当前时刻的状态量的误差值，EKF和ESKF本质上无明显区别，只是计算方式上的不同。位姿初始值的确定过程、点云地图的构建过程的具体实现如下所述。

实施例一

图1是本发明实施例一中提供的一种点云地图构建方法的流程图。本实施例可适用于基于位姿初始值构建点云地图的情况，该位姿初始值包括至少两个传感器采集得到的传感器数据融合后提供的位姿数值。该方法可以由本发明实施例提供的点云地图构建装置来执行，该装置可以由软件和/或硬件的方式实现，该装置可以集成在点云地图构建系统上。

参见图1，本发明实施例的方法具体包括如下步骤：

S110、在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值，其中，位姿预测值是根据预设传感器采集到的传感器数据预测出的当前点云帧的位姿数值。

其中，雷达里程计在将基于当前点云帧匹配到初步构建出的中间点云地图(帧和地图匹配，scan-to-map)后，可以将匹配结果更新为中间点云地图，且可以根据匹配结果确定当前点云帧的位姿观测值，该位姿观测值可以是雷达里程计观测到的当前点云帧的位姿数值。进而，雷达里程计可以将该位姿观测值发送出去，如将其发送给某种卡尔曼滤波器如EKF、ESKF等，以使该卡尔曼滤波器根据该位姿观测值确定当前点云帧的下一点云帧的位姿初始值。

具体的，在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与该位姿观测值对应的位姿预测值。需要说明的是，由于各个预设传感器可定时采集传感器数据，如IMU的更新频率是100Hz，其每0.01s采集到一个角速度；如轮速计的更新频率是50Hz，其每0.02s采集到一个速度；再如图像采集设备的更新频率是60Hz，其每1/60s采集到一帧图像，因此任一预设传感器在采集到一个传感器数据时，其可以将该传感器数据发送给卡尔曼滤波器，以使卡尔曼滤波器根据接收到的传感器数据预测出一个采集时刻下的位姿数值，该采集时刻可以是采集到该传感器数据的时刻。例如，假设预设传感器是设置在车辆中的传感器，可以根据该传感器数据预测出车辆在采集时刻时的位姿数值。

在此基础上，由于雷达里程计的更新频率是10Hz，那么在每接收到一位姿观测值前，卡尔曼滤波器已经对预测出的位姿数值进行多次更新，与位姿观测值对应的位姿预测值可以是最新预测出的位姿数值。由于预测出该最新预测出的位姿数值的时刻与扫描到该当前点云帧的当前时刻非常接近，因此可以将该最新预测出的位姿数值作为当前点云帧的位姿预测值。综上所述，以车辆为例，车辆在同一时刻的位姿数值可能存在两个状态量，一个预测得到的位姿预测值、和一个观测得到的位姿观测值，该位姿观测值还可以称为全局的状态量。

S120、对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值。

其中，对位姿观测值和位姿预测值进行融合的方式有多种，如基于EKF对二者进行融合时，由于EKF维护的是当前时刻的状态量(即当前点云帧的位姿数值)，则其可以对位姿观测值和位姿预测值直接进行融合，得到当前点云帧的位姿目标值，该位姿目标值可以认为是当前点云帧的最佳的位姿数值；再如基于ESKF对二者进行融合时，由于ESKF维护的是当前时刻的状态量的误差值(即当前点云帧的位姿数值的误差值)，则其可以先根据位姿观测值和位姿预测值得到位姿误差值，再根据该位姿误差值和位姿预测值得到当前点云帧的位姿目标值；等等，在此未做具体限定。

需要说明的是，基于激光雷达对物理空间中的同一实体进行扫描时，由于激光雷达在持续移动，则激光雷达扫描后连续得到的两个点云帧的位姿数值虽存在变化，但这一变化是有限的变化，这意味着当前点云帧的位姿目标值与该当前点云帧的下一点云帧的位姿真实值较为相似，该下一点云帧可以是在扫描到当前点云帧的当前时刻的下一时刻扫描到的点云帧，该位姿真实值是下一点云帧在中间点云地图中的真实的位姿数值，其也是下一点云帧的位姿观测值，该位姿观测值无法直接测量得到只能预估得到，因此可以将当前点云帧的位姿目标值作为下一点云帧的位姿初始值，与位姿真实值无限接近的位姿初始值是保证雷达里程计将下一点云帧成功且准确匹配到中间点云地图上的重要因素。

S130、将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。

其中，在得到下一点云帧的位姿初始值之后，可以将该位姿初始值发送给雷达里程计，以使雷达里程计根据该位姿初始值将下一点云帧匹配到中间点云地图上，如对该位姿初始值进行优化，并根据优化后的位姿优化值将下一点云帧匹配到中间点云地图上，该中间点云地图可以是根据当前点云帧的匹配结果构建得到的点云地图，该位姿优化值可以理解为下一点云帧的位姿观测值。这一步骤也可以认为是在接收到雷达里程计发送来的位姿初始值查询请求时，将与该位姿初始值查询请求对应的位姿初始值发送给雷达里程计，该位姿初始值查询请求可以是雷达里程计在接收到下一点云帧时发送出来的用于查询该下一点云帧的位姿初始值的请求。

在此基础上，可选的，可以将下一点云帧更新为当前点云帧，且将已构建点云地图更新为中间点云地图，重复执行上述各步骤，以便将依次扫描到的各点云帧匹配到中间点云地图上，最终得到已构建完成的点云地图。

为了更好地理解上述技术方案的实现过程，接下来以图2为例进行示例性说明。通常情况下，预设传感器的更新频率高于雷达里程计，该雷达里程计也是一种传感器，这意味着位姿预测值的更新频率高于位姿观测值，因此，雷达里程计在接收到某点云帧时，可以从EKF中查询到该点云帧的更为准确的位姿初始值，并基于该位姿初始值对该点云帧和中间点云地图进行匹配，且可以将匹配结果(即该点云帧的位姿观测值)反馈给EKF，以使EKF进行位姿初始值的更新，由此形成了一个紧耦合的点云地图构建系统。需要说明的是，图2中的状态预测是根据IMU采集到的角速度或是轮速计采集到的速度预测出位姿预测值的过程，状态更新是根据雷达里程计反馈的位姿观测值和EKF预测出的位姿预测值对位姿初始值进行更新的过程。

本发明实施例的技术方案，在接收到雷达里程计发送的当前点云帧的位姿观测值时，可以获取到与该位姿观测值对应的基于预设传感器采集到的传感器数据预测出的当前点云帧的位姿预测值，其中，位姿观测值和位姿预测值可以分别表示当前点云帧在同一时刻的不同的位姿数值；对位姿观测值和位姿预测值进行融合，由此得到的融合结果是当前点云帧在该同一时刻下的最佳的位姿数值，其可以作为当前点云帧的下一点云帧的位姿初始值，该位姿初始值是与下一点云帧的位姿观测值较为相似的位姿数值；由此，在将该位姿初始值发送给雷达里程计后，可以使得雷达里程计根据该位姿初始值将该下一点云帧准确匹配到中间点云地图上，得到已构建点云地图。上述技术方案，通过对基于预设传感器采集得到的传感器数据预测出的位姿预测值、以及雷达里程计反馈的位姿观测值进行融合，为点云地图的构建过程提供了更为准确的位姿初始值，由此提高了点云建图的构建精度。

实施例二

图3是本发明实施例二中提供的一种点云地图构建方法的流程图。本实施例以上述各技术方案为基础进行优化。在本实施例中，可选的，对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值，具体可包括：根据位姿观测值和位姿预测值得到位姿误差值；根据位姿误差值确定目标误差值，并根据目标误差值和位姿预测值得到当前点云帧的下一点云帧的位姿初始值。其中，与上述各实施例相同或相应的术语的解释在此不再赘述。参见图3，本实施例的方法具体可以包括如下步骤：

S210、在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值，其中，位姿预测值是根据预设传感器采集到的传感器数据预测出的当前点云帧的位姿数值。

S220、根据位姿观测值和位姿预测值得到位姿误差值，并根据位姿误差值确定目标误差值，并根据目标误差值和位姿预测值得到当前点云帧的下一点云帧的位姿初始值。

其中，在ESKF中，由于ESKF维护的是当前时刻的状态量的误差值(即当前点云帧的位姿数值的误差值)，且卡尔曼滤波器得到的位姿数值是最优值，该最优值的误差值是0，即ESKF认为预测出的误差值是0；另外，ESKF观测到的误差值是位姿观测值和位姿预测值间的误差值，因此可以根据位姿观测值和位姿预测值之间的误差值得到位姿误差值。进而，根据位姿误差值可以确定目标误差值，该目标误差值可以理解为当前点云帧的最佳的误差值，由此根据该目标误差值和位姿预测值可以得到当前点云帧的下一点云帧的位姿初始值，例如将目标误差值增加到该位姿预测值上，得到当前时刻的最佳的位姿数值，即该位姿初始值也是当前点云帧的最佳的位姿数值。

在此基础上，可选的，根据位姿观测值和位姿预测值得到位姿误差值，可包括：位姿观测值可包括位置观测值P_lidar，位姿预测值可包括位置预测值P_state，位姿误差值可包括位置误差值ΔP，通过如下公式计算出ΔP：

ΔP＝P_lidar-P_state；

和/或，位姿观测值可包括姿态观测值Q_lidar，位姿预测值可包括姿态预测值Q_state，位姿误差值可包括姿态误差值Δθ，通过如下公式计算出Δθ：

其中，四元数由四个数字表示，一个数字为实部且其余三个数字为虚部，xyz为四元数的虚部，这里表示只取出该四元数的虚部，即Δθ可以为四元数的虚部的系数，即李代数的数值。

再可选的，可以通过如下公式根据位姿误差值确定目标误差值，在卡尔曼滤波公式中，

其中，x_k-1表示k-1时刻的最佳状态，P_k-1表示x_k-1对应的协方差矩阵，

可表示k时刻的先验状态，

该先验状态的协方差矩阵，A是k-1时刻状态到k时刻状态的状态转移矩阵，H是观测矩阵，在本发明实施例中可以是单位矩阵，R是观测噪声矩阵，z为观测值。

考虑到本发明实施例可能涉及到的应用场景，k-1时刻可以是采集到当前点云帧的上一点云帧的时刻(即当前时刻的上一时刻)，k时刻可以是采集到当前点云帧的时刻(即当前时刻)，其中的状态可以是位姿数值。具体的，x可表示位置和姿态的误差值(即位姿误差值中的位置误差值和姿态误差值)，因此(1)中x_k-1为一个零向量，此时z即为上述ΔP和Δθ组成的误差向量，最终得到的x_k为k时刻的目标误差值。进一步，将该目标误差值叠加到k时刻的位姿预测值上，可以得到k时刻的最佳的位姿数值。

S230、将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。

本发明实施例的技术方案，通过位姿观测值和位姿预测值得到位姿误差值，该位姿误差值可以是ESKF观测到的误差值，进而根据该位姿误差值得到当前点云帧的最佳的误差值(即目标误差值)，并根据该目标误差值和该位姿预测值准确计算出当前点云帧的最佳的位姿数值，该位姿数值可以作为当前点云帧的下一点云帧的位姿初始值，由此达到了位姿初始值的准确计算的效果。

实施例三

图4是本发明实施例三中提供的一种点云地图构建方法的流程图。本实施例以上述各技术方案为基础进行优化。在本实施例中，上述点云地图构建方法，还可以包括：在接收到预设传感器发送的传感器数据时，根据传感器数据和位姿历史值预测出位姿预测值，其中，位姿历史值是在采集到传感器数据的采集时刻的上一时刻预测出的位姿数值。其中，与上述各实施例相同或相应的术语的解释在此不再赘述。

参见图4，本实施例的方法具体可以包括如下步骤：

S310、在接收到预设传感器发送的传感器数据时，根据传感器数据和位姿历史值预测出位姿预测值，其中，位姿历史值是在采集到传感器数据的采集时刻的上一时刻预测出的位姿数值。

其中，正如上文所述，某预设传感器在采集到一个传感器数据时，其可以将该传感器数据发送给卡尔曼滤波器，以使卡尔曼滤波器根据接收到的传感器数据可以预测出采集到该传感器数据的采集时刻的位姿数值。具体的，卡尔曼滤波器可以根据该传感器数据对位姿历史值进行调整，该位姿历史值可以是在采集时刻的上一时刻预测出的位姿数值，进而根据调整结果得到采集时刻下的位姿预测值。示例性的，以m-1时刻(采集时刻的上一时刻)和m时刻(采集时刻)为例，卡尔曼滤波器可以根据m-1时刻的位姿历史值和m时刻的传感器数据预测出m时刻的位姿预测值。

在此基础上，可选的，上述位姿预测值的预测过程可以如下所述：传感器数据可包括IMU在采集时刻采集得到的角速度w，位姿历史值包括在采集时刻的上一时刻预测的姿态历史值Q’_state，位姿预测值包括采集时刻下的姿态预测值Q_state，通过如下公式预测出Q_state：

其中，Δt是采集时刻和上一时刻间的时间差，即m时刻和m-1时刻间的时间差，Q_state和Q’_state可以采用Hamilton四元数表示；

和/或，传感器数据可包括轮速计在采集时刻采集到的速度v，位姿历史值可包括Q’_state和在采集时刻的上一时刻预测出的位置历史值P’_state，位姿预测值可包括位置预测值P_state，通过如下公式预测出P_state：

P_state＝P’_state+Q’_state*v*Δt。

S320、在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值，其中，位姿预测值是根据预设传感器采集到的传感器数据预测出的当前点云帧的位姿数值。

S330、对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值。

S340、将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。

本发明实施例的技术方案，在接收到预设传感器发送的传感器数据时，可根据传感器数据和在采集到该传感器数据的采集时刻的上一时刻预测出的位姿历史值预测出采集时刻的位姿预测值，达到了位姿预测值的实时更新的效果。

实施例四

图5为本发明实施例四提供的点云地图构建装置的结构框图，该装置配置于点云地图构建系统中，用于执行上述任意实施例所提供的点云地图构建方法。该装置与上述各实施例的点云地图构建方法属于同一个发明构思，在点云地图构建装置的实施例中未详尽描述的细节内容，可以参考上述点云地图构建方法的实施例。参见图5，该装置具体可包括：位姿获取模块410、位姿融合模块420和点云地图构建模块430。

其中，位姿获取模块410，用于在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值，其中，位姿预测值是根据预设传感器采集到的传感器数据预测出的当前点云帧的位姿数值；

位姿融合模块420，用于对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值；

点云地图构建模块430，用于将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。

可选的，位姿融合模块420，具体可以包括：

位姿误差值得到单元，用于根据位姿观测值和位姿预测值得到位姿误差值；

位姿融合单元，用于根据位姿误差值确定目标误差值，并根据目标误差值和位姿预测值得到当前点云帧的下一点云帧的位姿初始值。

可选的，位姿误差值得到单元，具体可以用于：

位姿观测值包括位置观测值P_lidar，位姿预测值包括位置预测值P_state，位姿误差值包括位置误差值ΔP，通过如下公式计算出ΔP：

ΔP＝P_lidar-P_state；

和/或，位姿观测值包括姿态观测值Q_lidar，位姿预测值包括姿态预测值Q_state，位姿误差值包括姿态误差值Δθ，通过如下公式计算出Δθ：

可选的，在上述装置的基础上，该装置还可包括：

位姿预测模块，用于在接收到预设传感器发送的传感器数据时，根据传感器数据和位姿历史值预测出位姿预测值，其中，位姿历史值是在采集到传感器数据的采集时刻的上一时刻预测出的位姿数值。

在此基础上，可选的，位姿预测模块，具体可以用于：

传感器数据包括惯性测量单元在采集时刻采集得到的角速度w，位姿历史值包括姿态历史值Q’_state，位姿预测值包括姿态预测值Q_state，通过如下公式预测出Q_state：

其中，Δt是采集时刻和上一时刻间的时间差；

和/或，传感器数据包括轮速计在采集时刻采集到的速度v，位姿历史值包括Q’_state和位置历史值P’_state，位姿预测值包括位置预测值P_state，通过如下公式预测出P_state：

P_state＝P’_state+Q’_state*v*Δt。

可选的，位姿观测值是雷达里程计在对当前点云帧进行匹配后观测到的当前点云帧的位姿数值。

本发明实施例四提供的点云地图构建装置，通过位姿获取模块在接收到雷达里程计发送的当前点云帧的位姿观测值时，可以获取到与该位姿观测值对应的基于预设传感器采集到的传感器数据预测出的当前点云帧的位姿预测值，位姿观测值和位姿预测值可以分别表示当前点云帧在同一时刻的不同的位姿数值；位姿融合模块对位姿观测值和位姿预测值进行融合，由此得到的融合结果是当前点云帧在该同一时刻下的最佳的位姿数值，其可以作为当前点云帧的下一点云帧的位姿初始值，该位姿初始值是与下一点云帧的位姿观测值较为相似的位姿数值；由此，点云地图构建模块在将该位姿初始值发送给雷达里程计后，可以使得雷达里程计根据该位姿初始值将该下一点云帧准确匹配到中间点云地图上，得到已构建点云地图。上述装置，通过对基于预设传感器采集得到的传感器数据预测出的位姿预测值、以及雷达里程计反馈的位姿观测值进行融合，为点云地图的构建过程提供了更为准确的位姿初始值，进一步提高了点云建图的构建精度。

本发明实施例所提供的点云地图构建装置可执行本发明任意实施例所提供的点云地图构建方法，具备执行方法相应的功能模块和有益效果。

值得注意的是，上述点云地图构建装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

实施例五

上文各个实施例所述的点云地图构建系统可以包括雷达里程计、至少一个预设传感器和卡尔曼滤波器，图6为本发明实施例五提供的一种卡尔曼滤波器的结构示意图，如图6所示，该卡尔曼滤波器包括存储器510、处理器520、输入装置530和输出装置540。卡尔曼滤波器中的处理器520的数量可以是一个或多个，图6中以一个处理器520为例；卡尔曼滤波器中的存储器510、处理器520、输入装置530和输出装置540可以通过总线或其它方式连接，图6中以通过总线550连接为例。

存储器510作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本发明实施例中的点云地图构建方法对应的程序指令/模块(例如，点云地图构建装置中的位姿获取模块410、位姿融合模块420和点云地图构建模块430)。处理器520通过运行存储在存储器510中的软件程序、指令以及模块，从而执行卡尔曼滤波器的各种功能应用以及数据处理，即实现上述的点云地图构建方法。

存储器510可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据卡尔曼滤波器的使用所创建的数据等。此外，存储器510可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中，存储器510可进一步包括相对于处理器520远程设置的存储器，这些远程存储器可以通过网络连接至设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

输入装置530可用于接收输入的数字或字符信息，以及产生与装置的用户设置以及功能控制有关的键信号输入。输出装置540可包括显示屏等显示设备。

在此基础上，可选的，该卡尔曼滤波器可以是扩展卡尔曼滤波器或是误差卡尔曼滤波器。

实施例六

本发明实施例六提供一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行一种点云地图构建方法，该方法应用于点云地图构建系统，点云地图构建系统包括雷达里程计和至少一个预设传感器，该方法可以包括：

在接收到雷达里程计发送的当前点云帧的位姿观测值时，获取与位姿观测值对应的位姿预测值，其中，位姿预测值是根据预设传感器采集到的传感器数据预测出的当前点云帧的位姿数值；

对位姿观测值和位姿预测值进行融合，并将融合结果作为当前点云帧的下一点云帧的位姿初始值；

将位姿初始值发送给雷达里程计，以使雷达里程计根据位姿初始值将下一点云帧匹配到中间点云地图上，得到已构建点云地图。

当然，本发明实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上所述的方法操作，还可以执行本发明任意实施例所提供的点云地图构建方法中的相关操作。

通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。依据这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (10)

1.一种点云地图构建方法，其特征在于，应用于点云地图构建系统，所述点云地图构建系统包括雷达里程计和至少一个预设传感器，所述方法包括：

在接收到所述雷达里程计发送的当前点云帧的位姿观测值时，获取与所述位姿观测值对应的位姿预测值，其中，所述位姿预测值是根据所述预设传感器采集到的传感器数据预测出的所述当前点云帧的位姿数值；

对所述位姿观测值和所述位姿预测值进行融合，并将融合结果作为所述当前点云帧的下一点云帧的位姿初始值；

将所述位姿初始值发送给所述雷达里程计，以使所述雷达里程计根据所述位姿初始值将所述下一点云帧匹配到中间点云地图上，得到已构建点云地图。

2.根据权利要求1所述的方法，其特征在于，所述对所述位姿观测值和所述位姿预测值进行融合，并将融合结果作为所述当前点云帧的下一点云帧的位姿初始值，包括：

根据所述位姿观测值和所述位姿预测值得到位姿误差值；

根据所述位姿误差值确定目标误差值，并根据所述目标误差值和所述位姿预测值得到所述当前点云帧的下一点云帧的位姿初始值。

3.根据权利要求2所述的方法，其特征在于，所述根据所述位姿观测值和所述位姿预测值得到位姿误差值，包括：

所述位姿观测值包括位置观测值P_lidar，所述位姿预测值包括位置预测值P_state，位姿误差值包括位置误差值ΔP，通过如下公式计算出ΔP：

ΔP＝P_lidar-P_state；

和/或，所述位姿观测值包括姿态观测值Q_lidar，所述位姿预测值包括姿态预测值Q_state，位姿误差值包括姿态误差值Δθ，通过如下公式计算出Δθ：

4.根据权利要求1-3中任一所述的方法，其特征在于，还包括：

在接收到所述预设传感器发送的所述传感器数据时，根据所述传感器数据和位姿历史值预测出所述位姿预测值，其中，所述位姿历史值是在采集到所述传感器数据的采集时刻的上一时刻预测出的位姿数值。

5.根据权利要求4所述的方法，其特征在于，所述根据所述传感器数据和位姿历史值预测出所述位姿预测值，包括：

所述传感器数据包括惯性测量单元在所述采集时刻采集得到的角速度w，所述位姿历史值包括姿态历史值Q’_state，所述位姿预测值包括姿态预测值Q_state，通过如下公式预测出Q_state：

其中，Δt是所述采集时刻和所述上一时刻间的时间差；

和/或，所述传感器数据包括轮速计在所述采集时刻采集到的速度v，所述位姿历史值包括Q’_state和位置历史值P’_state，所述位姿预测值包括位置预测值P_state，通过如下公式预测出P_state：

P_state＝P’_state+Q’_state*v*Δt。

6.根据权利要求1所述的方法，其特征在于，所述位姿观测值是所述雷达里程计在对所述当前点云帧进行匹配后观测到的所述当前点云帧的位姿数值。

7.一种点云地图构建装置，其特征在于，配置于点云地图构建系统，所述点云地图构建系统包括雷达里程计和至少一个预设传感器，所述装置包括：

位姿获取模块，用于在接收到所述雷达里程计发送的当前点云帧的位姿观测值时，获取与所述位姿观测值对应的位姿预测值，其中，所述位姿预测值是根据所述预设传感器采集到的传感器数据预测出的所述当前点云帧的位姿数值；

位姿融合模块，用于对所述位姿观测值和所述位姿预测值进行融合，并将融合结果作为所述当前点云帧的下一点云帧的位姿初始值；

点云地图构建模块，用于将所述位姿初始值发送给所述雷达里程计，以使所述雷达里程计根据所述位姿初始值将所述下一点云帧匹配到中间点云地图上，得到已构建点云地图。

8.一种点云地图构建系统，其特征在于，包括：

雷达里程计、至少一个预设传感器和卡尔曼滤波器；

所述卡尔曼滤波器包括：

一个或多个处理器；

存储器，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-6中任一所述的点云地图构建方法。

9.根据权利要求8所述的系统，其特征在于，所述卡尔曼滤波器包括扩展卡尔曼滤波器或是误差卡尔曼滤波器。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-6中任一所述的点云地图构建方法。

Images