1.一种面向自动驾驶车辆的结构共用多传感器融合定位系统,其特征在于:包括:车辆运动学模型模块,以在新一帧激光点云数据还未到达之前,通过轮速传感器和方向盘转角传感器数据构建车辆运动学模型,根据车辆运动学模型预测车辆运动状态;
激光SLAM算法模块,分为前端匹配和后端图优化部分,前端匹配完成:基于车辆预测状态,矫正激光点云运动畸变,通过邻域点信息计算点云局部光滑度,采用双邻域策略提取角点和平面点特征,并与上一帧点云进行匹配,后端优化完成:基于点云强度和局部光滑度构建全局描述符,利用卡方检验计算扫描相似性并根据特征点进行验证,通过帧与局部地图匹配优化车辆位姿,基于当前帧与历史回环帧的位姿转换,构建位姿图优化,降低车辆位姿累计误差;
扩展卡尔曼滤波模块,利用扩展卡尔曼滤波技术融合车辆运动学模型提供的车辆预测位姿和激光SLAM算法提供的车辆优化位姿,从而获取准确的车辆位姿信息。
2.根据权利要求1所述的面向自动驾驶车辆的结构共用多传感器融合定位系统,其特征在于:所述车辆运动模型模块构建车辆运动学模型和预测车辆状态的具体步骤如下:步骤1,通过轮速传感器直接提供车辆纵向速度v和通过方向盘转角传感器提供车辆横摆角速度ω计算所需的方向盘转角δs;
步骤2,基于速度v、方向盘转角δs、转向器角传动比η以及轴距L计算车辆横摆角速度ω:步骤3,以上一时刻车辆坐标系作为参考坐标系,两时刻间车辆始终运行在平面上,根据车辆运动学模型预测两时刻间车辆位姿变化量:其中,δt表示相邻底层数据的时间间隔, 表示相对于上一激光帧对应时刻所累计的横摆角增量, 表示相邻时刻间车辆始终处于平面运动状态,即:以上一时刻车辆坐标系作为参考坐标系,Z轴方向无运动;
步骤4,基于上一时刻已优化的车辆位姿 计算j时刻自动驾驶车辆在世界坐标系下的位姿其中,
3.根据权利要求2所述的面向自动驾驶车辆的结构共用多传感器融合定位系统,其特征在于:所述步骤4中的平面上的旋转矩阵 计算公式为:
4.根据权利要求1至3任意一项所述的面向自动驾驶车辆的结构共用多传感器融合定位系统,其特征在于:所述激光SLAM算法模块包括前端匹配模块、后端优化模块,其中前端匹配模块包括:点云畸变矫正模块,接收最新时刻激光点云数据,根据车辆预测状态矫正点云运动畸变;
点云特征提取模块,根据邻域点信息计算点云局部光滑度,采用双邻域策略提取角点和平面点特征;
帧间匹配模块,基于车辆位姿初始估计,利用当前帧与上一帧点云匹配,获取车辆优化位姿;
所述后端优化模块包括:
描述符构建模块,利用点云强度和局部光滑度构建全局描述符;
相似性计算模块,利用卡方检验计算扫描相似性;
特征点验证,利用特征点匹配数量验证回环正确性;
回环帧匹配,利用回环帧对应位姿构建局部地图,进行帧与局部地图匹配,获取车辆优化位姿。
位姿图优化,根据当前帧与对应回环帧的位姿变换、回环中相邻两帧的位姿变换,构建车辆位姿图优化,降低车辆位姿的累计误差。
5.根据权利要求4所述的面向自动驾驶车辆的结构供用多传感器融合定位系统,其特征在于:所述位姿优化模块根据激光里程计残差和激光回环残差构建代价函数,利用梯度下降法进行联合非线性优化以获得基于激光SLAM算法的车辆优化位姿。
6.根据权利要求1至3任意一项所述的面向自动驾驶车辆的结构供用多传感器融合定位系统,其特征在于:所述扩展卡尔曼滤波模块中的算法分为两个阶段:预测阶段:根据上一时刻优化状态量和系统协方差矩阵预测当前时刻车辆位姿和协方差矩阵,即:其中,Σ表示系统协方差矩阵,Mj表示系统噪声协方差矩阵。g(·)表示状态方程,G表示状态转移矩阵,其推导公式如下:其中,右乘BCH近似雅可比矩阵的逆 按下式计算:
其中,θa表示旋转矩阵R对应旋转向量,θ表示旋转角度,a表示旋转轴。∧为反对称符号,∨为解反对称符号;
更新阶段:计算卡尔曼增益并根据测量值和预测值更新车辆当前位姿和协方差矩阵,即:其中,Kj表示卡尔曼增益。Nj表示观测噪声协方差矩阵。Hj表示系统观测矩阵,该矩阵为
6×6的单位矩阵。