[发明专利]基于三维点云的定位方法、装置、设备及存储介质

说明书

本申请提供了一种基于三维点云的定位方法、装置、设备及存储介质，涉及自动驾驶技术领域，该方法包括：通过分别获取组合导航数据和激光雷达测量的点云数据，并分别对点云数据和组合导航数据进行处理，先基于组合导航数据确定车辆的当前位姿信息，并确定点云定位中的变换矩阵和平移矩阵，再基于该变换矩阵和平移矩阵确定车辆的激光雷达位姿信息，然后基于该激光雷达位姿信息和车辆的姿态信息确定车辆的姿态计算法方程，然后采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，更加符合车辆的形式规律，保证车辆定位的准确性。

技术领域

本申请涉及自动驾驶技术领域，具体而言，本申请涉及一种基于三维点云的定位方法、装置、设备及存储介质。

背景技术

基于高精度地图的定位过程中，由于地图的非实时性与传感器噪声，实时采集的数据与地图之间不一致的存在问题。这一问题通常会给基于地图匹配的定位算法的精度带来影响，甚至造成局部最优或者定位无法收敛的情况。针对现有的问题，目前出现了许多融合不同传感器进行的定位，融合的方法大都基于KF(卡尔曼滤波)，然而在现实车辆运动过程中，并非属于线性系统，同时由于点云存在有噪声，使用经典的配准算法，如ICP或者NDT，存在有累计误差，造成在点云定位过程中存在定位不收敛，最终造成定位不准确。

发明内容

本申请的目的旨在至少能解决上述的技术缺陷之一，特别是现有技术存在有累计误差，造成在点云定位过程中存在定位不收敛，最终造成定位不准确的技术缺陷。

第一方面，提供了一种基于三维点云的定位方法，包括：

获取车辆的组合导航数据和激光雷达测量的点云数据；

对所述组合导航数据进行解析，得到所述车辆的当前组合导航位姿信息，基于所述当前组合导航位姿信息确定所述车辆在点云定位中点云变换矩阵和平移矩阵；

基于所述变换矩阵和所述平移矩阵将所述车辆的当前帧点云与预先建立的地图进行匹配，得到所述车辆的激光雷达位姿信息；

对所述组合导航数据进行解析得到所述车辆的当前姿态信息，将所述当前姿态信息与所述激光雷达位姿信息结合，确定所述车辆的位姿状态计算方程；

采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位。

作为本申请一种可能的实施方式，在该实施方式中，所述车辆的激光雷达位姿信息为：

X＝[L^T,W^T,V^T]^T

其中，L＝(l_x,l_y,l_z)^T用于表示所述激光雷达的位置信息，其中l_x为激光雷达x轴坐标信息，l_y为激光雷达y轴坐标信息，l_z为激光雷达z轴坐标信息；用于表示所述激光雷达的欧拉角，其中为俯仰角，θ为横滚角，ψ为航向角，V＝(v_x,v_y,v_z)^T用于表示所述激光雷达的速度，其中v_x为x轴速度，v_y为y轴速度，v_z为z轴速度。

作为本申请一种可能的实施方式，在该实施方式中，所述车辆的当前姿态信息为：

U＝[w^T,a^T]^T