CN112083725A

CN112083725A - 一种面向自动驾驶车辆的结构共用多传感器融合定位***

Info

Publication number: CN112083725A
Application number: CN202010923469.8A
Authority: CN
Inventors: 秦晓辉; 庞涛; 徐彪; 谢国涛; 胡满江; 秦兆博; 秦洪懋; 王晓伟; 边有钢; 丁荣军; 常灯祥; 王哲文
Original assignee: Hunan University
Current assignee: Hunan University
Priority date: 2020-09-04
Filing date: 2020-09-04
Publication date: 2020-12-15
Anticipated expiration: 2040-09-04
Also published as: CN112083725B

Abstract

本发明公开了一种面向自动驾驶车辆的结构共用多传感器融合定位***，包括：车辆运动学模型模块，根据车辆运动学模型预测车辆运动状态；激光SLAM算法模块；扩展卡尔曼滤波模块，利用扩展卡尔曼滤波技术融合车辆运动学模型提供的车辆预测位姿和激光SLAM算法提供的车辆优化位姿，从而获取准确的车辆位姿信息。本发明的面向自动驾驶车辆的结构共用多传感器融合定位***，通过车辆运动学模型模块、激光SLAM算法模块和扩展卡尔曼滤波模块的设置，便可有效的实现进行定位了。

Description

一种面向自动驾驶车辆的结构共用多传感器融合定位***

技术领域

本发明涉及自动驾驶车辆定位方法，特别是关于一种面向自动驾驶车辆的结构共用多传感器融合定位***。

背景技术

近年来，自动驾驶技术研究已成为热点和趋势。自动驾驶技术包含环境感知、定位导航、路径规划和决策控制，其中定位导航是自动驾驶的关键。准确、鲁棒的实时定位是自动驾驶车辆安全行驶的基础，确定车辆在全局坐标系中的位置信息为规划和控制模块提供依据。自动驾驶车辆通过传感器获得自身运动状态和周围地形环境信息，实现车辆位姿的准确估计。

目前，自动驾驶车辆常用定位技术分为两类：1)基于全球卫星导航***(GNSS)定位。2)基于自主式传感器定位。GNSS定位精度较高，但容易受到高楼、隧道、高架和地下车库等使用环境的遮蔽影响而失效。基于自主式传感器的定位主要利用激光雷达、惯性测量单元或者里程计。利用里程计或IMU传感器进行航迹推算是一种低成本、自主的车辆定位方式，其优点在于抗干扰能力强，同时能根据传感器数据在短时间内提供较高精度的车辆定位信息，但航迹推算定位算法的误差会随时间不断累积，因此并不适合长时间单独定位。SLAM算法利用激光雷达或相机实现自动驾驶车辆实时定位。大多数情况下，自动驾驶车辆以平面运动为主，而SLAM算法并未考虑激光雷达受安装平台的约束影响，导致车辆优化位姿出现偏差。综上所述，使用单一传感器进行定位难以满足自动驾驶车辆的定位精度要求。因此，设计一种面向自动驾驶车辆的结构共用多传感器融合定位***，通过融合多种传感器数据提高自动驾驶车辆定位精度和鲁棒性。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种面向自动驾驶车辆的结构共用多传感器融合定位***，通过车辆运动学模型预测车辆运动状态，并利用激光SLAM算法优化车辆位姿。采用扩展卡尔曼滤波技术实现车辆运动学模型和激光SLAM算法的位姿融合，以期提高自动驾驶车辆定位精度与鲁棒性。为实现上述目的，本发明提供了如下技术方案：一种面向自动驾驶车辆的结构共用多传感器融合定位***，包括：

车辆运动学模型模块，以在新一帧激光点云数据还未到达之前，通过轮速传感器和方向盘转角传感器数据构建车辆运动学模型，根据车辆运动学模型预测车辆运动状态；

激光SLAM算法模块，分为前端匹配和后端图优化部分，前端匹配完成：基于车辆预测状态，矫正激光点云运动畸变，通过邻域点信息计算点云局部光滑度，采用双邻域策略提取角点和平面点特征，并与上一帧点云进行匹配，后端优化完成：基于点云强度和局部光滑度构建全局描述符，利用卡方检验计算扫描相似性并根据特征点进行验证，通过帧与局部地图匹配优化车辆位姿，基于当前帧与历史回环帧的位姿转换，构建位姿图优化，降低车辆位姿累计误差；

扩展卡尔曼滤波模块，利用扩展卡尔曼滤波技术融合车辆运动学模型提供的车辆预测位姿和激光SLAM算法提供的车辆优化位姿，从而获取准确的车辆位姿信息。

作为本发明的进一步改进，所述车辆运动模型模块构建车辆运动学模型和预测车辆状态的具体步骤如下：

步骤1，通过轮速传感器直接提供车辆纵向速度v和通过方向盘转角传感器提供车辆横摆角速度ω计算所需的方向盘转角δ_s；

步骤2，基于速度v、方向盘转角δ_s、转向器角传动比η以及轴距L计算车辆横摆角速度ω：

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

其中，δt表示相邻底层数据的时间间隔，

表示相对于上一激光帧对应时刻所累计的横摆角增量，

表示相邻时刻间车辆始终处于平面运动状态，即：以上一时刻车辆坐标系作为参考坐标系，Z轴方向无运动；

步骤4，基于上一时刻已优化的车辆位姿

计算j时刻自动驾驶车辆在世界坐标系下的位姿

其中，

作为本发明的进一步改进，所述步骤4中的平面上的旋转矩阵

计算公式为：

作为本发明的进一步改进，所述激光SLAM算法模块包括前端匹配模块、后端优化模块，其中前端匹配模块包括：

点云畸变矫正模块，接收最新时刻激光点云数据，根据车辆预测状态矫正点云运动畸变；

点云特征提取模块，根据邻域点信息计算点云局部光滑度，采用双邻域策略提取角点和平面点特征；

帧间匹配模块，基于车辆位姿初始估计，利用当前帧与上一帧点云匹配，获取车辆优化位姿；

所述后端优化模块包括：

描述符构建模块，利用点云强度和局部光滑度构建全局描述符；

相似性计算模块，利用卡方检验计算扫描相似性；

特征点验证，利用特征点匹配数量验证回环正确性；

回环帧匹配，利用回环帧对应位姿构建局部地图，进行帧与局部地图匹配，获取车辆优化位姿。

位姿图优化，根据当前帧与对应回环帧的位姿变换、回环中相邻两帧的位姿变换，构建车辆位姿图优化，降低车辆位姿的累计误差。

作为本发明的进一步改进，所述位姿优化模块根据激光里程计残差和激光回环残差构建代价函数，利用梯度下降法进行联合非线性优化以获得基于激光SLAM算法的车辆优化位姿。

作为本发明的进一步改进，所述扩展卡尔曼滤波模块中的算法分为两个阶段：预测阶段：根据上一时刻优化状态量和***协方差矩阵预测当前时刻车辆位姿和协方差矩阵，即：

其中，Σ表示***协方差矩阵，M_j表示***噪声协方差矩阵。g(·)表示状态方程，G表示状态转移矩阵，其推导公式如下：

其中，右乘BCH近似雅可比矩阵的逆

按下式计算：

其中，θa表示旋转矩阵R对应旋转向量，θ表示旋转角度，a表示旋转轴。∧为反对称符号，∨为解反对称符号；

更新阶段：计算卡尔曼增益并根据测量值和预测值更新车辆当前位姿和协方差矩阵，即：

其中，K_j表示卡尔曼增益。N_j表示观测噪声协方差矩阵。H_j表示***观测矩阵，该矩阵为6×6的单位矩阵。

本发明的有益效果，1)本发明提出一种面向自动驾驶车辆的结构共用多传感器融合定位方法，实现激光雷达、轮速传感器以及方向盘转角传感器三者的数据融合，解决单一传感器定位精度和鲁棒性难以满足自动驾驶车辆安全行驶要求的问题。2)本发明通过车辆运动学模型预测车辆运动状态，并利用激光SLAM算法优化车辆位姿，采用扩展卡尔曼滤波技术融合车辆运动学模型和激光SLAM算法，提高自动驾驶车辆定位精度与鲁棒性。3)本发明通过考虑车辆空间朝向实现三维空间下车辆运动学模型的使用，提高面向自动驾驶车辆的结构共用多传感器融合定位方法适用性。4)本发明通过车辆运动学模型状态预测为点云的运动畸变矫正提供依据，基于线性插值方法获取准确激光点云，利于激光点云数据关联的准确性。5)本发明通过邻域点信息计算点云局部光滑度，采用双邻域策略提取角点和平面点特征，降低邻域尺寸对局部光滑度计算的影响，提高特征提取的精度和鲁棒性。6)本发明实现激光雷达、轮速传感器以及方向盘转角传感器三者的数据融合，解决单一传感器定位精度和鲁棒性难以满足自动驾驶车辆安全行驶要求的问题。利用车辆运动学模型提供的车辆预测状态作为预测值，同时以激光SLAM算法提供的车辆优化位姿为测量值，采用扩展卡尔曼滤波技术融合两类车辆位姿。***计算复杂度较低，实时性强，且能保证自动驾驶车辆定位精度与鲁棒性。7)本发明基于点云强度和局部光滑度构建全局描述符并利用卡方检验计算描述符相似性，准确提取回环帧并构建位姿图优化，减小车辆定位累计误差。点云强度信息的直接利用和局部光滑度的一物两用提高特征利用效率并降低计算量，保证实时重定位。8)本发明实现共用原车传感器，无需增加额外传感器，实现成本节约、复杂度降低的同时提升可靠性。使用少量传感器完成数据融合，无需使用IMU、GNSS等复杂、昂贵传感器，易于工程实践的同时更利于通过车规级测试，9)本发明使用预积分技术同步多个信息源不同帧率的数据，避免了优化过程中的重复积分操作，节省计算量。

附图说明

图1是本发明面向自动驾驶车辆的结构共用多传感器融合定位算法架构图；

图2是本发明车辆运动学模型预积分示意图；

图3是本发明车辆运动学模型示意图；

图4是本发明激光SLAM算法架构图。

具体实施方式

下面将结合附图所给出的实施例对本发明做进一步的详述。

如图1所示，面向自动驾驶车辆的结构共用多传感器融合定位算法架构图包含三个模块：车辆运动学模型模块、激光SLAM算法模块和扩展卡尔曼滤波模块。

该定位***的输入包含三种传感器：多线激光雷达、轮速传感器和方向盘转角传感器。基于上一时刻扩展卡尔曼滤波模块提供的车辆优化位姿，利用激光点云数据和两帧点云对应时刻间的轮速传感器和方向盘转角传感器数据优化自动驾驶车辆位姿。在j时刻，待优化***状态量定义如下：

其中，下标w表示世界坐标系，b表示车辆坐标系。

表示j时刻车辆坐标系到世界坐标系的平移变换。

表示j时刻车辆坐标系与世界坐标系间旋转矩阵对应的李代数。旋转矩阵和李代数具有如下关系:

R＝exp(φ^∧)

1、车辆运动学模型模块

在新一帧激光点云数据还未到达之前，利用轮速传感器和方向盘转角传感器数据构建车辆运动学模型，根据车辆运动学模型预测车辆运动状态。车辆运动学模型包含两个输入：1)轮速传感器直接提供车辆纵向速度v。2)方向盘转角传感器用于提供车辆横摆角速度ω计算所需的方向盘转角δ_s。

车辆横摆角速度ω由速度v、方向盘转角δ_s、转向器角传动比η以及轴距L共同决定，即：

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

其中，δt表示相邻底层数据的时间间隔。

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

表示相邻时刻间车辆始终处于平面运动状态，即：以上一时刻车辆坐标系作为参考坐标系，Z轴方向无运动。

基于上一时刻已优化的车辆位姿

计算j时刻自动驾驶车辆在世界坐标系下的位姿

其中，

平面上的旋转矩阵

计算公式为：

基于车辆运动学模型的状态预测为扩展卡尔曼滤波算法提供预测值，同时兼作激光SLAM优化求解问题的初值。

2、激光SLAM算法模块

激光SLAM算法模块包含三个部分：激光里程计模块、激光回环检测模块和位姿优化模块。

激光里程计模块包含四个部分：点云畸变矫正、点云特征提取、局部地图构建和帧与地图匹配。这四个部分分别完成：1)接收最新时刻激光点云数据，根据车辆预测状态矫正点云运动畸变。2)根据邻域点信息计算点云局部光滑度，采用双邻域策略提取角点和平面点特征。3)基于上一时刻滤波模块提供的车辆优化位姿，构建局部点云地图。4)基于车辆位姿初始估计，利用帧与局部地图匹配算法构建用于联合优化的激光里程计残差。

激光回环检测模块包含四个部分：描述符构建、相似性计算、特征点验证和回环帧匹配。这四个部分分别完成：1)利用点云强度和局部光滑度构建全局描述符。2)利用卡方检验计算扫描相似性。3)利用特征点匹配数量验证回环正确性。4)利用回环帧对应位姿构建局部地图，进行帧与局部地图匹配。

位姿优化模块根据激光里程计残差和激光回环残差构建代价函数，利用梯度下降法进行联合非线性优化以获得基于激光SLAM算法的车辆优化位姿。自动驾驶车辆的联合优化位姿将传递给扩展卡尔曼滤波模块用作测量值。

3、扩展卡尔曼滤波模块

融合激光雷达、轮速传感器以及方向盘转角传感器三种测量数据，采用扩展卡尔曼滤波技术融合车辆运动学模型和激光SLAM算法，以期提高自动驾驶车辆定位精度和鲁棒性。扩展卡尔曼滤波模块利用车辆运动学模型的车辆状态预测作为预测值，而激光SLAM模块的车辆优化位姿作为观测值。

扩展卡尔曼滤波算法分为两个阶段：预测和更新。

预测阶段：根据上一时刻优化状态量和***协方差矩阵预测当前时刻车辆位姿和协方差矩阵，即：

其中，右乘BCH近似雅可比矩阵的逆

按下式计算：

其中，θa表示旋转矩阵R对应旋转向量，θ表示旋转角度，a表示旋转轴。∧为反对称符号，∨为解反对称符号。

通过滤波模块的多次迭代获取车辆准确位姿，用于下一时刻激光SLAM模块中的局部地图构建和车辆运动学模型模块中的车辆状态预测。

初次利用面向自动驾驶车辆的结构共用多传感器融合定位方法估计车辆位姿需进行初始化操作。其初始化分三个部分：1)通过线性插值实现激光雷达数据、轮速传感器数据以及方向盘转角传感器数据的时间戳对齐。2)通过测量更新激光雷达坐标系和车辆坐标系间的平移变换p_bl。3)利用旋转约束优化激光雷达坐标系和车辆坐标系间的旋转变换q_bl。4)设定初始***噪声协方差矩阵和观测噪声协方差矩阵，即：

相邻两时刻i,j之间有：车辆运动学模型旋转约束

激光里程计旋转约束

则有：

其中，q表示四元数。

推导可得：

其中，[·]_L，[·]_R分别表示四元数的左乘和右乘。

累计多个时刻线性方程并添加鲁棒核权重获取联合线性方程：

其中：

由旋转矩阵和轴角间变换关系tr(R)＝1+2cosθ，获得角度误差r的计算公式：

利用SVD分解进行联合线性方程的求解，最小奇异值对应奇异向量为激光雷达坐标系和车辆坐标系之间的旋转变换q_bl。

图2为车辆运动学模型预积分示意图。相对于激光雷达帧率，轮速传感器和方向盘转角传感器的频率较高，因此需进行车辆运动学模型预积分，以获取相邻激光帧之间的车辆运动状态预测。

将相邻激光帧之间的多个车辆底层数据进行预积分，以获取对应时刻间的车辆运动学模型预积分量。利用中值法计算车辆运动学模型预积分量，即利用相邻底层数据间平均测量值计算相对位姿变化：

基于车辆运动学模型模块已给出的计算公式，根据

和

计算车辆运动学模型预积分量

和

车辆运动学模型预积分量作为相邻两帧点云间的初始运动估计传递给激光SLAM算法模块。

图3为车辆运动学模型示意图。车辆运动学模型简化为两自由度自行车模型，前后轮均用单轮替代。以车辆后轴中心B为原点建立车辆坐标系，沿车辆前进方向为X_b轴方向，垂直X_b轴指向车体左侧为Y_b轴方向。

表示相邻时刻间车辆横摆角增量，L表示轴距，δ_f为前轮转角。为保证安全，自动驾驶***不会进入极限工况，因此质心侧偏角较小，可忽略不计。通常情况下，汽车后轮不可控，故可认为自行车模型中后轮转角控制输入δ_r＝0。利用方向盘转角δ_s和转向器角传动比η可获得前轮转角δ_f，即：δ_f＝δ_s/η。

车辆运动学模型建立原则是保证模型简单的同时应尽可能反映车辆真实运动特性。过于严格的车辆运动学模型不利于保证实时性。基于车辆结构和运动特点，前轮转向四轮车简化为两轮自行车模型。自行车模型基于以下假设：1)不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动。2)左右侧车轮转角一致，将左右侧轮胎合并为一个轮胎。3)车辆转向只由前轮控制。

车辆运动学模型有两个输入：轮速传感器提供的车辆速度v和方向盘转角传感器提供的前轮转角δ_f。以前一时刻车辆坐标系作为参考坐标系，当前时刻车辆运动学模型表达式如下：

其中，v_x和v_y分别表示自动驾驶车辆在参考坐标系下X轴方向的速度、Y轴方向的速度。

如图4所示，激光SLAM算法架构图包含三个模块：激光里程计模块、激光回环检测模块和位姿优化模块。

1、激光里程计模块

激光里程计模块包含四个部分：点云畸变矫正、点云特征提取、局部地图构建和帧与地图匹配。

(1)点云畸变矫正

为保证点云匹配准确性需矫正点云畸变。基于匀速运动假设，通过线性插值方法矫正激光点云的运动畸变。去畸变点云用于后续局部光滑度计算和点云匹配。

(2)点云特征提取

利用去畸变点云的角度信息进行点云线束划分。每条激光扫描线上点云按下式计算局部光滑度：

其中，s_j表示点云的局部光滑度。

表示点云测量值。A表示邻域点集合。

本实施例中的局部光滑度阈值定为0.1。排序点云光滑度并通过局部光滑度和邻域点分布提取两类特征点：1)角点：局部光滑度大于阈值的同时邻域点未发生突变。2)平面点：局部光滑度小于阈值的同时邻域点未发生突变。在双重邻域范围内计算局部光滑度并依此提取同时满足阈值的角点和平面点特征。双邻域特征提取算法避免邻域尺寸对局部光滑度的影响，提高特征提取的精度和鲁棒性。

为实现特征点云的均匀分布，将每条线束均分为4个独立区域。每个区域最多提供20个角点和40个平面点，构成角点集合

和平面点集合

在选择特征点时希望避免选取以下几类点：1)可能被遮挡的点。2)已经选过点周围的点。3)位于激光线接***行的平面点。

(3)局部地图构建

局部点云地图用于确定准确的特征对应关系。基于先前优化的车辆位姿，利用固定数量的历史激光帧构建稠密局部点云地图。将{j-m,…,j-1}时间段内对应角点特征

和平面点特征

根据不同时刻间相对位姿统一变换到关键帧k对应激光雷达坐标系，实现角点局部地图和平面点局部地图的构建。其中，

m为偶数。

(4)帧与地图匹配

基于已构建局部地图和车辆位姿初始估计，采用帧与局部地图匹配构建激光里程计残差。当前时刻特征点云集合

和

中的点云根据相对位姿关系实现点云投影，即，特征点云转换到k时刻对应激光雷达坐标系。基于两类局部地图中的特征点云，构建对应kd树并寻找角点对应的特征线和平面点对应的特征面，即，1)点线ICP：使用kd树算法快速寻找每个角点的两个最近点，利用最近点构造直线并计算点到直线的垂足坐标

2)点面ICP：使用kd树算法快速寻找每个平面点的三个最近点，利用最近点构造平面并计算点到平面的垂足坐标

对于j时刻激光点云中的特征点，将其投影到k时刻激光雷达坐标系下的值为：

其中，下标l表示激光雷达坐标系。

表示j时刻激光特征点在激光雷达坐标系下的三维坐标。

表示将j时刻激光特征点转换到k时刻激光雷达坐标系下的理论值。T表示两个坐标系间的变换矩阵，包含旋转矩阵和平移向量，即：

三维坐标形式为：

通过约束同一时刻激光雷达的测量值构建激光里程计残差。利用点到直线和点到平面的距离表示激光里程计残差r_l：

其中，

表示将j时刻激光测量点转换到k时刻激光雷达坐标系下的投影点。

表示投影点在k时刻激光雷达坐标系中的对应点。

即：

其中，c表示通过帧与局部地图匹配确定的特征点云对应关系。

通过激光里程计残差对***状态量求偏导构建雅克比矩阵J_l：

推导可得：

2、激光回环检测模块

激光回环检测模块包含四个部分：描述符构建、相似性计算、特征点验证和回环帧匹配。

(1)描述符构建

使用主成分分析法进行坐标轴划分。获得参考框架后，对齐边界和坐标轴，从而获取对应的划分坐标轴。基于点云强度和局部光滑度的全局描述符由m个不互相重叠区域组成，即，以激光雷达为中心，定义外球半径和内球半径，球体划分为上下两部分，每个半球体均分为四个区域，由此，m＝16。利用相应直方图进行区域描述，即，落入该区域的k个激光点对应激光点云强度和局部光滑度被计算成直方图，该直方图具有n个划分区域。

(2)相似性计算

以车辆位姿初始估计作为中心点，设定候选区域尺寸为40×40×10m。在满足时间间隔条件下，利用描述符匹配寻找满足相似度阈值且相似度最低的激光帧。利用卡方检验定义描述符A第i个区域与描述符B第i个区域的相似性度量，即：

利用区域相似性计算两个描述符的相似性，如下所示：

利用主成分分析法确定坐标轴存在歧义，为消除该过程造成的干扰，需通过分类讨论，实现坐标轴正确对齐。根据x轴和z轴的划分，共有4种情况。使用上式计算A和B的相似性时，出现四个不同的值，取最小值作为描述符匹配结果。即：

S_AB＝min{S_AB1,S_AB2,S_AB3,S_AB4}

(3)特征点验证

选择相似度最高且满足阈值的激光帧作为回环候选帧。为保证回环检测准确性，需利用特征点进一步验证。基于回环候选帧的对应车辆位姿，利用k近邻算法寻找k个历史帧，将历史帧对应特征点云投影到回环候选帧所对应激光雷达的坐标系，实现局部地图构建。基于车辆位姿初始估计，投影当前帧中的特征点云到回环候选帧。利用k近邻算法寻找角点对应直线和平面点对应平面，计算点到线以及点到面的距离。满足最小距离阈值的点云为匹配点，统计匹配点数量并计算匹配点与特征点数量的比值。若比值满足设定阈值，则回环检测正确。反之，回环检测错误，不构建激光回环残差。

(4)回环帧匹配

对于j时刻激光特征点云，将其投影到o时刻(回环帧对应时刻)激光雷达坐标系下的值为：

其中，

表示j时刻激光测量点在激光雷达坐标系下的三维坐标。

表示将j时刻激光测量点转换到n时刻激光雷达坐标系下的理论值。

三维坐标形式为：

对于角点特征，使用kd树算法快速寻找每个特征点的两个最近点。利用最近点构造直线并计算点到直线的垂足坐标

对于平面点特征，使用kd树算法快速寻找每个特征点的三个最近点，利用最近点构造平面并计算点到平面的垂足坐标

对于角点，利用点到线的距离表示激光回环残差。对于平面点，利用点到平面的距离表示激光回环残差。通过约束同一时刻激光雷达的测量值构建激光回环残差r_o：

其中，

表示将j时刻激光测量点转换到n时刻激光雷达坐标系下的投影点。

表示投影点在n时刻激光雷达坐标系中的对应点。

即：

其中，d表示通过帧与局部地图匹配确定的特征点云对应关系。

通过激光回环残差对***状态量求偏导构建雅克比矩阵J_o：

推导可得：

3、位姿优化模块

位姿优化模块根据激光里程计残差和激光回环残差构建代价函数，利用梯度下降法进行联合非线性优化以获得基于激光SLAM算法的车辆优化位姿。在梯度下降法执行过程中需要用到代价函数的雅克比矩阵，在前述内容中已进行相关推导和描述，此处不再赘述。自动驾驶车辆的联合优化位姿将传递给扩展卡尔曼滤波模块用作测量值。

通过计算***代价函数最小值获取待优化***状态量X的最大后验估计。面向自动驾驶车辆的结构共用多传感器融合定位***代价函数构建如下：

其中，两种残差均用马氏距离表示。r_l(c,X)表示激光里程计残差，r_o(d,X)表示激光回环残差,协方差矩阵Σ_L由激光雷达准确性决定。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。