一种多传感器数据融合定位系统技术方案

本发明专利技术提供了一种多传感器数据融合定位系统。该多传感器数据融合定位系统包括组合定位子单元，包括GNSS模块、RTK服务模块和IMU模块；二维状态误差卡尔曼滤波单元，包括高精地图模块、视觉车道线检测模块、车道线匹配模块、轮速里程计解算模块和第一状态误差卡尔曼滤波模块；激光雷达点云匹配定位单元，包括点云模块、点云地图模块、定位初始化模块和NDT匹配定位模块；三维状态误差卡尔曼滤波单元，包括卡尔曼滤波初始化模块、IMU位姿状态量解算模块、判断模块、第二状态误差卡尔曼滤波模块和后验位姿模块。本发明专利技术提出的一种多传感器数据融合定位系统能增强全局定位的鲁棒性，具有较强的场景适应能力。强的场景适应能力。强的场景适应能力。

【技术实现步骤摘要】
一种多传感器数据融合定位系统


[0001]本专利技术涉及智能网联汽车智能驾驶
，尤其涉及一种多传感器数据融合定位系统。

技术介绍

[0002]对于高级自动驾驶系统(比如L4级)而言，定位模块是自动驾驶最核心的模块之一，定位又包括全局定位和局部定位，对于自动驾驶，其精度需要达到厘米级别，此外还要求定位系统具有强鲁棒性，也就是在任一种传感器数据退化的情况下，仍然能够输出准确的实时定位结果。定位模块通常会融合GNSS、IMU、轮速里程计(车辆底盘编码器航迹推演输出)、视觉检测信息、视觉里程计以及激光雷达里程计(LiDAR Odometry)等多种传感器的测量数据，使用滤波算法(ESKF、EKF、UKF等)以获得平滑、厘米级别的绝对定位，其中基于点云地图和激光雷达的配准定位(Lidar Odometry)因其精度高、场景适应性强，鲁棒性与可靠性好，在整个融合定位中通常占很大的权重，是自动驾驶定位系统中相对可靠的“绝对定位”数据来源。
[0003]全局定位是通过全球卫星定位系统(GNSS)获取车辆在地球全局世界坐标下的位姿。全局定位必然依赖GNSS,然而在自动驾驶部署工作的复杂场景下，比如城市峡谷、高架桥、隧道、林荫大道等场景，不可避免地存在没有GNSS信号，或者时有时无GNSS的情况。因此自动驾驶定位系统如果仅仅采用必须依赖GNSS的全局定位方案，是不具有复杂场景下鲁棒的定位能力的。这对全天候自动驾驶系统来说是最致命的问题。
[0004]车端局部定位则是要获得车辆在局部场景(比如室内或园区等)或局部地图坐标系下的位姿。传统的自主移动机器人(比如AGV)时常采用局部定位技术来实现既定场景下的路径规划等任务，比如，自主移动机器人时常采用一类同步定位与建图(SLAM，Simultaneous Localization and Mapping)方法进行在线定位。该方法的优点是在无GNSS信号的情况下采用较低成本的各类传感器，结合先进的多传感器数据融合算法即可实现精度可观的局部定位结果输出。但该方法存在大场景定位里程计误差累加增大和定位延时等问题、难以直接应用到复杂的自动驾驶场景。因为自动驾驶车辆通常行驶速度较快，距离较远，场景非常复杂，使得建图算法实时构建的地图偏移过大，定位算法输出的里程计误差累积，定位精度下降。

技术实现思路

[0005]针对现有技术的上述问题，本专利技术提出了一种多传感器数据融合定位系统，能增强全局定位的鲁棒性，具有较强的场景适应能力。
[0006]具体地，本专利技术提出了一种多传感器数据融合定位系统，适用于车辆，包括：
[0007]组合定位子单元，包括GNSS模块、RTK服务模块和IMU模块，所述GNSS模块用于获取GNSS数据，所述RTK服务模块用于提供实时运动差分增强解算服务，所述IMU模块用于获取车辆的IMU数据；
[0008]二维状态误差卡尔曼滤波单元，包括高精地图模块、视觉车道线检测模块、车道线匹配模块、轮速里程计解算模块和第一状态误差卡尔曼滤波模块，所述高精地图模块基于高精地图和GNSS数据获取所述车辆所在车道线的垂直坐标，所述视觉车道线检测模块通过所述车辆的视觉相机获取所述车辆所在车道线的最近点和最远点坐标，所述车道线匹配模块基于所述高精地图模块和视觉车道线检测模块的输出结果获取所述车辆的横向更新量，所述轮速里程计解算模块用于获取所述车辆的纵向更新量，所述第一状态误差卡尔曼滤波模块用于融合所述车辆的横向更新量、纵向更新量、GNSS数据及IMU数据以获取所述车辆的二维定位结果；
[0009]激光雷达点云匹配定位单元，包括点云模块、点云地图模块、定位初始化模块和NDT匹配定位模块，所述点云模块通过所述车辆的激光雷达获取所述车辆的实时点云数据，所述点云地图模块用于获取离线构建的点云地图，所述定位初始化模块用于获取所述车辆的全局坐标，所述NDT匹配定位模块基于所述实时点云数据和点云地图获取所述车辆的匹配定位位姿；
[0010]三维状态误差卡尔曼滤波单元，包括卡尔曼滤波初始化模块、IMU位姿状态量解算模块、判断模块、第二状态误差卡尔曼滤波模块和后验位姿模块，所述卡尔曼滤波初始化模块基于全局坐标获取全局位姿状态量初始值，所述IMU位姿状态量解算模块基于全局位姿状态量初始值和IMU数据计算获得预估状态量，所述判断模块用于判断所述激光雷达点云匹配定位单元是否输出有效的匹配定位位姿，若是，则所述第二状态误差卡尔曼滤波模块基于匹配定位位姿来更新所述预估状态量，所述后验位姿模块基于更新后的预估状态量计算所述车辆的三维定位结果；若否，则所述IMU位姿状态量解算模块基于所述预估状态量直接输出三维定位结果；
[0011]若所述GNSS模块能正常接收GNSS数据且RTK服务模块能提供实时运动差分增强解算服务，则所述多传感器数据融合定位系统通过所述组合定位子单元输出由GNSS数据和IMU数据融合后的定位结果；
[0012]若所述GNSS模块能正常接收GNSS数据且RTK服务模块不能提供实时运动差分增强解算服务，则所述多传感器数据融合定位系统通过所述二维状态误差卡尔曼滤波单元获取所述车辆的二维定位结果；
[0013]若所述GNSS模块不能接收完整的GNSS数据，则所述多传感器数据融合定位系统通过激光雷达点云匹配定位单元和三维状态误差卡尔曼滤波单元获取所述车辆的三维定位结果。
[0014]根据本专利技术的一个实施例，所述第一状态误差卡尔曼滤波模块获取的GNSS数据包括所述车辆的经纬度数据，所述第一状态误差卡尔曼滤波模块基于所述经纬度数据获得所述车辆的横向状态量和纵向状态量，并基于所述横向更新量和纵向更新量来更新校正所述横向状态量和纵向状态量；所述第一状态误差卡尔曼滤波模块基于所述GNSS数据和IMU数据融合后获得所述车辆的航向角。
[0015]根据本专利技术的一个实施例，在二维状态误差卡尔曼滤波单元中，所述高精地图模块基于高精地图和GNSS数据获取所述车辆距离最近的左右侧车道线的垂直点坐标，所述视觉车道线检测模块通过所述车辆的视觉相机获取所述视觉相机前端距离最近的左右侧车道线的最近点和最远点坐标，所述车道线匹配模块基于所述高精地图模块和视觉车道线检
测模块的输出结果获取所述车辆的横向更新量。
[0016]根据本专利技术的一个实施例，所述定位初始化模块获取所述车辆的全局坐标的过程包括：
[0017]基于所述GNSS数据变换后获取的全局坐标以实现定位初始化，基于GNSS数据持续更新所述全局坐标；
[0018]和/或基于所述IMU数据经过短暂时间间隔数值积分的位姿计算获取全局坐标以实现定位初始化，基于IMU数据持续更新所述全局坐标。
[0019]根据本专利技术的一个实施例，在所述激光雷达点云匹配定位单元的点云模块中，通过所述车辆的激光雷达获取所述车辆的点云数据经过体素栅格降采样滤波之后形成实时点云数据；所述点云地图是由所述车辆上的多传感器获取的数据经基于优化的融合建图算法离线构建生成。
[0020]根据本专利技术的一个实施例，所述体素栅本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多传感器数据融合定位系统，适用于车辆，包括：组合定位子单元，包括GNSS模块、RTK服务模块和IMU模块，所述GNSS模块用于获取GNSS数据，所述RTK服务模块用于提供实时运动差分增强解算服务，所述IMU模块用于获取车辆的IMU数据；二维状态误差卡尔曼滤波单元，包括高精地图模块、视觉车道线检测模块、车道线匹配模块、轮速里程计解算模块和第一状态误差卡尔曼滤波模块，所述高精地图模块基于高精地图和GNSS数据获取所述车辆所在车道线的垂直坐标，所述视觉车道线检测模块通过所述车辆的视觉相机获取所述车辆所在车道线的最近点和最远点坐标，所述车道线匹配模块基于所述高精地图模块和视觉车道线检测模块的输出结果获取所述车辆的横向更新量，所述轮速里程计解算模块用于获取所述车辆的纵向更新量，所述第一状态误差卡尔曼滤波模块用于融合所述车辆的横向更新量、纵向更新量、GNSS数据及IMU数据以获取所述车辆的二维定位结果；激光雷达点云匹配定位单元，包括点云模块、点云地图模块、定位初始化模块和NDT匹配定位模块，所述点云模块通过所述车辆的激光雷达获取所述车辆的实时点云数据，所述点云地图模块用于获取离线构建的点云地图，所述定位初始化模块用于获取所述车辆的全局坐标，所述NDT匹配定位模块基于所述实时点云数据和点云地图获取所述车辆的匹配定位位姿；三维状态误差卡尔曼滤波单元，包括卡尔曼滤波初始化模块、IMU位姿状态量解算模块、判断模块、第二状态误差卡尔曼滤波模块和后验位姿模块，所述卡尔曼滤波初始化模块基于全局坐标获取全局位姿状态量初始值，所述IMU位姿状态量解算模块基于全局位姿状态量初始值和IMU数据计算获得预估状态量，所述判断模块用于判断所述激光雷达点云匹配定位单元是否输出有效的匹配定位位姿，若是，则所述第二状态误差卡尔曼滤波模块基于匹配定位位姿来更新所述预估状态量，所述后验位姿模块基于更新后的预估状态量计算所述车辆的三维定位结果；若否，则所述IMU位姿状态量解算模块基于所述预估状态量直接输出三维定位结果；若所述GNSS模块能正常接收GNSS数据且RTK服务模块能提供实时运动差分增强解算服务，则所述多传感器数据融合定位系统通过所述组合定位子单元输出由GNSS数据和IMU数据融合后的定位结果；若所述GNSS模块能正常接收GNSS数据且RTK服务模块不能提供实时运动差分增强解算服务，则所述多传感器数据融合定位系统通过所述二维状态误差卡尔曼滤波单元获取所述车辆的二维定位结果；若所述GNSS模块不能接收完整的GNSS数据，则所述多传感器数据融合定位系统通过激光雷达点云匹配定位单元和三维状态误差卡尔曼滤波单元获取所述车辆的三维定位结果。2.如权利要求1所述的多传感器数据融合定位系统，其特征在于，所述第一状态误差卡尔曼滤波模块获取的GNSS数据包括所述车辆的...

【专利技术属性】
技术研发人员：柯希俊，施亮，
申请(专利权)人：上汽大众汽车有限公司，
类型：发明
国别省市：