[发明专利]一种面向园区自动驾驶的双滤波器融合定位系统

说明书

本发明公开了一种面向园区自动驾驶的双滤波器融合定位方法和系统，其目的是：设计一种面向园区自动驾驶的双滤波器融合定位方法和系统，通过扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，以获取车辆预测位姿。利用粒子滤波技术融合车辆预测位姿和激光里程计，以期提高园区环境下自动驾驶车辆定位精度与鲁棒性。

技术领域

本发明涉及自动驾驶领域，特别是关于一种面向园区自动驾驶的双滤波器融合定位系统。

背景技术

近年来，自动驾驶技术研究已成为热点和趋势。自动驾驶企业正尝试在园区场景中推进自动驾驶技术的商业化落地，准确、鲁棒的实时定位是自动驾驶车辆安全行驶基础，确定车辆在全局坐标系中的位置信息为规划和控制模块提供依据。

自动驾驶车辆常用定位技术分为两类：1)基于全球卫星导航系统定位。2)基于自主式传感器定位。全球卫星导航系统定位精度较高，但容易受到使用环境的遮蔽影响而失效。基于自主式传感器的定位主要利用激光雷达、惯性测量单元以及里程计。利用里程计或IMU传感器进行航迹推算是一种低成本、自主的定位方式，抗干扰能力强且短时间内车辆定位精度较高，但航迹推算定位算法的误差会随时间不断累积，不适合长时间单独定位。

自动驾驶面临的难题之一在于单一传感器难以满足自动驾驶车辆定位精度要求。目前已有基于滤波器的融合定位相关技术与专利。专利文件CN111207743A公布一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，采用紧耦合的方式融合编码器和惯导设备，实现车辆定位。但是两种定位均属于航迹推算定位算法，此类算法的误差会随时间不断累计，不适合长时间定位。因此，这种融合定位方式也会存在相同问题。此外。采用紧耦合方式会很大程度上增加算法复杂度，相比于松耦合来说，实际效果能否提升还值得商榷。专利文件CN109737957A公布一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统，将INS与激光雷达分别测量的位置信息之差作为观测量输入FIR滤波算法，预估INS计算误差，获取最优位置信息。但是这种方法缺少车辆的平面运动约束。专利CN111240331A公布基于激光雷达和里程计SLAM的智能小车定位导航方法及系统，通过编码器和IMU获取小车速度信息和位姿信息，形成里程计信息。通过激光雷达扫描小车的周围环境形成激光雷达坐标系中的环境点云位置信息。这种方法也没有考虑车辆的平面运动约束。此外，激光SLAM模块并没有采用效果更好的优化方式。自动驾驶车辆在园区内主要以平面运动为主。需要引入车辆平面运动约束条件，改善车辆位姿估计结果。因此，设计一种面向园区自动驾驶的双滤波器融合定位方法和系统，通过滤波技术融合多种传感器数据，以期提高园区场景中自动驾驶车辆定位精度和鲁棒性。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种面向园区自动驾驶的双滤波器融合定位方法和系统，通过扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，以获取车辆预测位姿。利用粒子滤波技术融合车辆预测位姿和激光里程计，以期提高园区环境下自动驾驶车辆定位精度与鲁棒性。

为实现上述目的，本发明提供了如下技术方案：一种面向园区自动驾驶的双滤波器融合定位系统，包括：

车辆运动学模型模块，在两帧激光点云数据之间，利用轮速传感器和方向盘转角传感器数据构建车辆运动学模型，根据车辆运动学模型预测车辆运动状态，该预测位姿为扩展卡尔曼滤波算法提供预测值；

IMU积分模块，该模块由陀螺仪和加速度计组成，测量角速度ω^b和加速度a^b，通过积分惯性测量值获取j时刻车辆预测位姿：

其中，g^w表示世界坐标系下的重力加速度，Δt表示相邻激光帧间的时间间隔，q表示四元数，表示i时刻惯性坐标系到世界坐标系的旋转，和表示只与IMU测量值有关的积分量，IMU积分量按下式计算：