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

权利要求书

1.一种面向园区自动驾驶的双滤波器融合定位系统，其特征在于：包括：

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

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

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

其中，表示t时刻惯性坐标系到i时刻惯性坐标系的旋转变化，表示四元数乘法；

扩展卡尔曼滤波模块，该模块融合IMU、轮速传感器以及方向盘转角传感器数据，采用扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，从而获取车辆预测位姿，其中通过车辆运动学模型模块提供预测值，而IMU积分模块提供测量值；

激光里程计模块，该模块包括：

点云畸变矫正模块，该点云畸变矫正模块基于匀速运动假设，利用扩展卡尔曼滤波模块提供的车辆预测位姿，通过线性插值方法矫正激光点云的运动畸变；

点云特征提取模块，该点云特征提取模块利用去畸变点云的角度信息进行点云线束划分，每条激光扫描线上点云按下式计算曲率：

其中，s_j表示点云的局部曲率，表示点云测量值，A表示邻域点集合；

点云特征匹配模块，该点云特征匹配模块利用扩展卡尔曼滤波模块获取的车辆预测位姿作为点云匹配初值，采用相邻两帧点云特征匹配构建激光里程计残差；车辆位姿优化模块，该车辆位姿优化模块根据激光里程计残差构建代价函数，利用梯度下降法进行非线性优化；

粒子滤波模块，该模块包括粒子采样模块、权重更新模块、重采样模块和状态估计模块。

2.根据权利要求1所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述车辆运动学模块包含两类输入：轮速传感器直接提供车辆纵向速度v和方向盘转角传感器提供车辆横摆角速度ω计算所需的方向盘转角δ_s，其中车辆横摆角速度ω由纵向速度v、方向盘转角δ_s、转向器角传动比η以及轴距d共同决定，即：

3.根据权利要求2所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述车辆运动学模型模块预测车辆运动状态的具体步骤为：

步骤一，以上一时刻车辆模型坐标系作为参考坐标系，两时刻间车辆始终运行在平面上，根据车辆运动学模型预测两时刻间车辆位姿变化量：

其中，表示相对于上一时刻的横摆角增量，δt表示相邻底层数据的时间间隔，下标v表示车辆模型坐标系，表示相邻时刻间车辆始终处于平面运动状态，即：以上一时刻车辆模型坐标系作为参考坐标系，Z轴方向无运动；

步骤二，基于上一时刻已优化的车辆位姿计算j时刻自动驾驶车辆在世界坐标系下的位姿

其中，R_bv表示车辆模型坐标系到惯性坐标系的旋转变换，平面上的旋转矩阵计算公式为：

其中，表示相邻激光帧间横摆角变化量。