CN114019552A - 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 - Google Patents

Abstract

本发明公开了一种基于贝叶斯多传感器误差约束的定位置信度优化方法，属于导航定位及信息融合应用技术领域。本发明利用以GNSS\IMU\激光雷达\视觉相机组成的多源融合定位系统，在多源融合状态更新模型的基础上，提取各传感器误差约束；通过在时序的滑动窗口内引入贝叶斯决策方程，对各信源进行权重均衡；构建联合全局误差约束方程，优化自适应融合定位框架，获得多源融合最佳置信定位解。本发明方法简单易行，能够在城市峡谷、地质自然灾害等未知遮蔽环境为无人智能平台提供自适应定位技术手段。

Description

一种基于贝叶斯多传感器误差约束的定位置信度优化方法

技术领域

本发明涉及一种基于贝叶斯多传感器误差约束的定位置信度优化方法，属于导航定位及信息融合应用技术领域。

背景技术

随着无人物流，智慧农业，辅助驾驶等无人平台快速发展，高精度定位服务也逐步走向更加泛在的未知、复杂场景，实时连续、泛在无缝的高精度导航定位是保障各类无人平台安全行进、智能作业的重要基础，然而，城市峡谷、隧道、山区河谷等受限场景限制了无人平台的泛在无缝应用。鉴于视觉特征跟踪受限于大尺度、光照等空旷自然环境，激光雷达场景辨识能力差，难以闭环全局修正，低成本惯性导航累积误差时间漂移，GNSS信号抗干扰能力弱，易受遮挡等，当前，发展联合GNSS\IMU\视觉\激光雷达的多传感器松\紧耦合定位方法是解决受限环境下高精度导航定位难题的主流手段。

当前，已知的多源松\紧耦合技术尽管已取得了一定的效果，但实际技术应用仅限于城市道路，工业园区，机场等结构化环境，定位结果的优化依赖先验地图的强约束限制，技术难以在未知环境进行应用推广。基于多源融合的导航定位技术作为现阶段无人智能领域的研究热点，但导航定位在多类型遮蔽环境下的可用性层面鲜有成果发表，尚不具备完善的融合理论基础和统一的定位可用性评价方法，亟需发展面向复杂遮蔽环境的多源融合权重自适应调整及信源置信概率评估技术研究，但是，现有技术中尚没有相关应用。

发明内容

有鉴于此，本发明提出了一种基于贝叶斯多传感器误差约束的定位置信度优化方法，该方法突破了传统多源融合能力限制，实现了复杂遮蔽环境下自适应可信定位。

为了实现上述目的，本发明采用的技术方案为：

一种基于贝叶斯多传感器误差约束的定位置信度优化方法，包括如下步骤：

(1)在多源融合状态更新模型的基础上，提取各传感器误差约束；

(2)在时序的滑动窗口内引入贝叶斯决策方程，对各信源进行权重均衡；

(3)构建联合全局误差约束方程，优化自适应融合定位框架，获得多源融合最佳置信定位解。

进一步的，步骤(1)的具体方式如下：

建立信源融合框架，定义S,G,B,L,C分别表示多源全系统、GNSS传感器信源、IMU传感器信源、激光雷达、视觉相机：

GNSS量测通过PPP-RTK解算无人平台位置r_G，并手动测量天线杆臂值以对r_G进行补偿， GNSS天线杆臂是由IMU中心指向GNSS天线相位中心的向量，卫星天线相对于IMU处在前侧、右侧、上侧时，杆臂值l以该向量投影在IMU坐标系下的三个分量l＝(l_x,l_y,l_z)^T来表示，补偿后的r_G为：

r_G＝r_a-l

其中，r_a为天线杆测量值；

已知r^S为系统位置测量值，则GNSS量测误差约束f(G)为：

f(G)＝arg min{r^S-r_G}

其中，arg min{}表示使{}内关系式取最小值的参数因子解；

IMU量测计算无人平台t+Δt时刻运动状态速度v_t+Δt、位置r_t+Δt以及旋转q_t+Δt的估计模型为：

其中，a_t,ω_t分别为t时刻的IMU原始加速度和角速度量测；

为加速度量测偏差和随机白噪声，

为角速度量测偏差和随机白噪声；g是世界坐标系W的重力加速度，

为惯性本体坐标系到世界坐标系的旋转；

IMU预积分误差约束f(B)为：

δv＝R(v_j-v_i-gΔt_ij)

f(B)＝arg min{δv,δr,δq}

式中，Δt_ij为连续时刻i,j的时间间隔，v_i,v_j表示时刻i,j的速度，r_i,r_j表示时刻i,j的位置，q_i,q_j表示时刻i,j的位姿，R为惯性本体坐标系到世界坐标系的旋转，δv,δr,δq分别表示预积分速度偏差、预积分位置偏差、预积分位姿偏差；

视觉、激光雷达的状态转移模型均为相对位置测量，激光雷达里程计采用LOAM算法通过计算曲率提取边缘点、平面点特征，执行点到边缘特征、点到平面特征的连续扫描匹配估计传感器运动，d_e,d_h分别表示点到边缘特征、点到平面特征的距离，其中点到边缘的特征约束为距离arg min{d_e}，点到平面的特征约束为距离arg min{d_h}；

针对无人平台在高速运动时造成的激光雷达运动畸变，对运动估计进行补偿，状态转移通过线性插值，假设t时刻点i运动状态矩阵为

旋转矩阵为R、平移量为T，则有：

其中，τ,τ_i,τ_t分别表示完整扫描周期、扫描点i时刻、起始扫描时刻，

为

的估计值；

将上述状态转移过程使用6自由度[x,y,z,roll,pitch,yaw]表示，进行欧拉角到旋转矩阵的变换，使用罗德里格斯公式进行变换：

其中，欧拉角θ＝[roll,pitch,yaw]，

为旋转方向单位向量，T＝[x,y,z]为转动幅度；激光雷达匹配距离约束f(L)为：

f(L)＝arg min{∑d_e+∑d_h}

式中，∑d_e,∑d_h分别表示当前激光帧所有d_e,d_h的累积和；

基于视觉相机的定位技术，利用运动恢复结构获取连续图像帧相机姿态参数，假定存在连续可观测N关键图像帧的相机姿态

其中，

L,

为关键图像帧相机姿态参数，

L,

为连续关键图像帧所包含的特征点；

视觉重投影误差约束f(C)为：

其中，M_ij为关键帧i,j在世界坐标系下的共视坐标，s为特征深度尺度，K^C为相机内参数矩阵。

进一步的，步骤(2)的具体方式如下：

多源融合导航定位系统采用4类传感器：卫星导航GNSS、惯性导航IMU、视觉相机、激光雷达；

确定多源系统S中单一信源先验概率，

分别表征GNSS定位量测权重先验概率、IMU定位量测权重先验概率、激光雷达定位量测权重先验概率、视觉相机定位量测权重先验概率；无人平台行进环境的遮蔽程度影响GNSS、激光雷达和视觉相机的先验概率，IMU 先验概率仅和时间累积相关；其中，将遮蔽环境分为4类，模糊描述为空旷、半遮挡、严重遮挡、完全遮挡；

GNSS定位量测权重先验概率P_r ^G通过检测并剔除卫星周跳和当前卫星构型来描述，周跳越频繁表征遮挡越严重，当采样中无可用卫星时，为完全遮挡环境，概率P_r ^G恒为0；可用卫星数不超过3颗星时，为严重遮挡，概率P_r ^G恒为0；可用卫星数达到4颗卫星以上时，为半遮挡或开阔环境，此时卫星定位可解算；当卫星处于可解算状态时，通过PDOP位置精度因子描述卫星构型，PDOP位置精度因子小于等于3时可产生理想位置，概率P_r ^G初始为0.95； PDOP位置精度因子介于3到7之间时定位结果可接受，概率P_r ^G初始为0.5，PDOP位置精度因子大于等于7时定位结果不可接受，概率P_r ^G初始为0.05；

已知激光雷达定位量测权重先验概率P_r ^L和视觉相机定位量测权重先验概率P_r ^C分别与 GNSS定位量测权重先验概率P_r ^G负相关，则空旷、半遮挡、严重遮挡、完全遮挡环境下有：

P_r ^L＝1-λ_LP_r ^G，

P_r ^C＝1-λ_CP_r ^G，

λ_L,λ_C为系数；

IMU定位量测权重先验概率P_r ^B与环境的遮蔽情况无关，在一个滑动窗周期W内，IMU不仅维持GNSS信号阻塞时的导航，同时IMU预积分还用于优化GNSS收敛以及激光雷达、视觉相机的里程计；令P_r ^B＝1-λ_BP_r ^G，λ_B为系数，从而简化λ_L＝λ_C＝λ_B，对各单一信源先验概率进行归一化；

对于4类传感器和4类遮挡环境，选取自由度为9的卡方分布：

A＝G,B,L,C

其中，z_S,z_A分别表示世界坐标系下多源系统定位结果和单一源定位结果，卡方检验中理论值为单一信源世界坐标系定位结果，测量值为多源系统世界坐标系定位结果；

查询卡方检验表获取初始化多源融合系统定位条件概率P(S|G)、P(S|B)、P(S|L)、P(S|C)；

由贝叶斯公式获得单一信源的置信后验概率：

P(A|S)＝P(S|A)P_r ^A/P(S),A＝G,B,L,C

对各单一信源先验概率更新，归一化消去P(S)，得到：

P_r ^A＝P(A|S),A＝G,B,L,C。

进一步的，步骤(3)的具体方式如下：

利用集束调整进行全局约束优化的后验概率权重最小误差模型，已知4类传感器误差约束包含IMU预积分误差约束、GNSS量测定位误差约束、激光雷达匹配距离误差约束、视觉重投影误差约束；全局约束优化是将单一信源置信的后验概率作为误差约束的权重进行赋值，累计联合形成全局约束，利用Levenberg-Marquarelt算法进行非线性参数求解，优化各信源运动估计模型，获得多源融合最佳置信定位解：

min{∑P(A|S)f(A)},A＝G,B,L,C。

本发明与现有技术相比所取得的有益效果为：

1、本发明利用以GNSS\IMU\激光雷达\视觉相机组成的多源融合定位系统，在多源融合状态更新模型的基础上，提取各传感器误差约束；通过在时序的滑动窗口内引入贝叶斯决策方程，对各信源进行权重均衡；通过构建联合全局误差约束方程，优化自适应融合定位框架，获得多源融合最佳置信定位解。

2、本发明可实现不同遮挡环境下信源权重的自主调整，满足无人平台在复杂环境下自适应定位。

3、本发明基于贝叶斯联合全局误差约束，可优化提升多源耦合定位精度。

4、本发明方法简单易行，能够在城市峡谷、地质自然灾害等未知遮蔽环境中为无人智能平台提供自适应定位技术手段。

附图说明

图1本发明实施例方法的原理示意图。

具体实施方式

为了更好的说明本发明的目的和优点，下面对本发明的技术方案作进一步说明。

一种基于贝叶斯多传感器误差约束的定位置信度优化方法，该方法所需的设备包括 GNSS接收机、惯性导航IMU、激光雷达、光学相机。如图1所示，通过GNSS接收机获取卫星观测量，IMU获取加速计和角速度计，激光雷达获取点云，光学相机获取序列图像，提取多传感器状态更新模型误差约束，在时序的滑动窗口内引入多源权重均衡的贝叶斯决策方程，构建联合全局误差约束方程，优化自适应融合定位框架，获得多源融合最佳置信定位解。

该方法具体包括如下步骤：

(1)在多源融合状态更新模型的基础上，提取各传感器误差约束；

(2)通过在时序的滑动窗口内引入贝叶斯决策方程，对各信源进行权重均衡；

(3)构建联合全局误差约束方程，优化自适应融合定位框架。

进一步的，步骤(1)的具体方式如下：

建立信源融合框架，定义S,G,B,L,C分别表示多源全系统，GNSS传感器信源，IMU传感器信源，激光雷达和视觉相机：

GNSS量测通过PPP-RTK(Precise Point Positioning-Real Time Kinematic)解算无人平台位置r_G，并手动测量天线杆臂值以对r_G进行补偿，GNSS天线杆臂是由IMU中心指向GNSS 天线相位中心的向量，卫星天线相对于IMU处在前侧、右侧、上侧时，杆臂值l以该向量投影在IMU坐标系下的三个分量l＝(l_x,l_y,l_z)^T来表示，补偿后的r_G为：

r_G＝r_a-l

其中，r_a为天线杆测量值；已知r^S为系统位置测量值，GNSS量测误差约束f(G)：

f(G)＝arg min{r^S-r_G}

惯性导航IMU量测计算无人平台t+Δt时刻运动状态速度v_t+Δt、位置r_t+Δt以及旋转q_t+Δt估计模型：

其中，其中a_t,ω_t分别为t时刻的IMU原始加速度和角速度量测；

为加速度量测偏差和随机白噪声，

为角速度量测偏差和随机白噪声；g是世界坐标系W的重力加速度，

为惯性本体坐标系到世界坐标系的旋转；

IMU预积分误差约束f(B)：

δv＝R(v_j-v_i-gΔt_ij)

f(B)＝arg min{δv,δr,δq}

式中，Δt_ij为连续时刻i,j的时间间隔，v_i,v_j表示时刻i,j的速度，r_i,r_j表示时刻i,j的位置，q_i,q_j表示时刻i,j的位姿，R为惯性本体坐标系到世界坐标系的旋转，δv,δr,δq分别表示预积分速度偏差、预积分位置偏差、预积分位姿偏差。

不同于卫星导航的绝对定位，视觉\激光雷达的状态转移模型均为相对位置测量；激光雷达里程计采用LOAM算法通过计算曲率提取边缘点、平面点特征，执行点到边缘特征、点到平面特征的连续扫描匹配估计传感器运动，其中点到边缘的特征约束为距离arg min{d_e}，其中点到平面的特征约束为距离arg min{d_h}。

当无人平台在高速运动时会造成激光雷达运动畸变，因此需要对运动估计过程进行补偿，由于激光雷达扫描频率较高，一次扫描过程无人平台可近似为匀速运动，因此状态转移可通过线性插值，假设t时刻点i运动状态矩阵为

其中，旋转矩阵为R、平移量为T：

其中，τ,τ_i,τ_t分别表示完整扫描周期，扫描点i时刻，起始扫描时刻；将上述状态转移过程使用6自由度[x,y,z,roll,pitch,yaw]表示，进行欧拉角到旋转矩阵的变换，使用罗德里格斯公式进行变换：

其中，θ＝[roll,pitch,yaw]，

为旋转方向单位向量，T＝[x,y,z]为转动幅度；

激光雷达匹配距离约束f(L)：

f(L)＝arg min{∑d_e+∑d_h}

式中，∑d_e,∑d_h分别表示当前激光帧所有d_e,d_h的累积和；

基于视觉相机的定位技术利用运动恢复结构获取连续图像帧相机姿态参数，假定存在连续可观测N关键图像帧的相机姿态

其中，

L,

为关键图像帧相机姿态参数，

L,

为连续关键图像帧所包含的特征点；

视觉重投影误差约束f(C)：

其中，M_ij为关键帧i,j在世界坐标系下的共视坐标，s为特征深度尺度，K^C为相机内参数矩阵。

步骤(2)的具体方式如下：

多源融合导航定位系统采用4类传感器，卫星导航GNSS，惯性导航IMU，视觉相机与激光雷达，鉴于每一类传感器存在数据源特性和所适用的感知环境，当无人平台陆续通过空旷、半遮挡、遮挡环境时，每一类传感器的定位结果在系统中的融合权重概率必然存在差异。可以通过将这种概率定义为置信概率来描述多源融合系统的定位结果在当前状态时刻t下可靠性，本例选定滑动周期窗口W(取1秒)，构建多源贝叶斯滑动窗函数模型，累积各单一信源置信概率；

首先确定多源系统S中单一信源先验概率，P_r ^G,P_r ^B,P_r ^L,P_r ^C分别表征GNSS定位量测权重先验概率，IMU定位量测权重先验概率，激光雷达定位量测权重先验概率，视觉相机定位量测权重先验概率；无人平台行进环境的遮蔽程度将直接影响GNSS、激光雷达和视觉相机的先验概率，而IMU先验概率仅和时间累积相关，其中，将遮蔽程度模糊描述为空旷、半遮挡、严重遮挡、完全遮挡；

GNSS定位量测权重先验概率P_r ^G可以通过检测并剔除卫星周跳和当前卫星构型描述，周跳越频繁表征遮挡越严重，当采样中无可用卫星时，为完全遮挡环境(概率P_r ^G恒为0)，可用卫星数不超过3颗星时，为严重遮挡(概率P_r ^G恒为0)，可用卫星数达到4颗卫星以上时，为半遮挡或开阔环境，此时卫星定位可解算；当卫星处于可解算状态时，通过PDOP(Position Dilution ofPrecision)描述卫星构型，PDOP小于等于3时可产生理想位置(概率P_r ^G初始为 0.95)，3到7之间的PDOP定位可接受(概率P_r ^G初始为0.5)，大于等于7的PDOP较差定位结果不可接受(概率P_r ^G初始为0.05)；

已知激光雷达定位量测权重先验概率P_r ^L和视觉相机定位量测权重先验概率P_r ^C分别与GNSS定位量测权重先验概率P_r ^G负相关，则给出空旷、半遮挡、严重遮挡、完全遮挡环境下 P_r ^L＝1-λ_LP_r ^G，P_r ^C＝1-λ_CP_r ^G，λ_L,λ_C为系数；

IMU定位量测权重先验概率P_r ^B与环境的遮蔽情况无关，在一个滑动窗周期内，IMU不仅可以维持GNSS信号阻塞时的导航，同时IMU预积分可以优化GNSS收敛以及激光雷达\视觉相机里程计，令P_r ^B＝1-λ_BP_r ^G，λ_B为系数，可简化λ_L＝λ_C＝λ_B，对各单一信源先验概率进行归一化；

本例涉及4类传感器，4类遮挡环境，选取自由度为9的卡方分布，查询卡方检验表获取初始化多源融合系统定位条件概率，P(S|G)，P(S|B)，P(S|L)，P(S|C)；

A＝G,B,L,C

其中，z_S,z_A分别表示世界坐标系下多源系统定位结果和单一源定位结果，卡方检验中理论值为单一信源世界坐标系定位结果，测量值为多源系统世界坐标系定位结果。

由贝叶斯公式获得单一信源的置信后验概率：

P(A|S)＝P(S|A)P_r ^A/P(S),A＝G,B,L,C

对各单一信源先验概率更新，归一化消去P(S)：

P_r ^A＝P(A|S),A＝G,B,L,C

步骤(3)的具体方式如下：

利用集束调整进行全局约束优化的后验概率权重最小误差模型，已知4类传感器误差约束包含，IMU预积分误差约束，GNSS量测定位误差约束，激光雷达匹配距离误差约束，视觉重投影误差约束；全局约束优化方法是将单一信源置信的后验概率作为误差约束的权重进行赋值，累计联合形成全局约束，利用LM(Levenberg-Marquarelt)算法进行非线性参数求解，优化各信源运动估计模型，获得多源融合最佳置信定位解。

min{∑P(A|S)f(A)},A＝G,B,L,C

总之，实时连续的高精度导航定位是保障无人平台安全行进、智能作业的重要基础，然而，城市峡谷、隧道、山区等受限场景限制了无人平台的泛在无缝应用。鉴于视觉特征跟踪受限于大尺度、光照等空旷自然环境，激光雷达场景辨识能力差，难以闭环全局修正，低成本惯性导航累积误差时间漂移，GNSS信号抗干扰能力弱，易受遮挡等问题，当前，发展联合 GNSS\IMU\视觉\激光雷达的多传感器松\紧耦合定位方法是解决受限环境下高精度导航定位难题的主流手段。本发明提取多传感器状态更新模型误差约束，在时序的滑动窗口内引入多源权重均衡的贝叶斯决策方程，构建联合全局误差约束方程，优化自适应融合定位框架，获得多源融合最佳置信定位解。

Claims (4)

1.一种基于贝叶斯多传感器误差约束的定位置信度优化方法，其特征在于，包括如下步骤：

(1)在多源融合状态更新模型的基础上，提取各传感器误差约束；

(2)在时序的滑动窗口内引入贝叶斯决策方程，对各信源进行权重均衡；

(3)构建联合全局误差约束方程，优化自适应融合定位框架，获得多源融合最佳置信定位解。

2.根据权利要求1所述的一种基于贝叶斯多传感器误差约束的定位置信度优化方法，其特征在于，步骤(1)的具体方式如下：

建立信源融合框架，定义S,G,B,L,C分别表示多源全系统、GNSS传感器信源、IMU传感器信源、激光雷达、视觉相机：

GNSS量测通过PPP-RTK解算无人平台位置r_G，并手动测量天线杆臂值以对r_G进行补偿，GNSS天线杆臂是由IMU中心指向GNSS天线相位中心的向量，卫星天线相对于IMU处在前侧、右侧、上侧时，杆臂值l以该向量投影在IMU坐标系下的三个分量l＝(l_x,l_y,l_z)^T来表示，补偿后的r_G为：

r_G＝r_a-l

其中，r_a为天线杆测量值；

已知r^S为系统位置测量值，则GNSS量测误差约束f(G)为：

f(G)＝arg min{r^S-r_G}

其中，arg min{}表示使{}内关系式取最小值的参数因子解；

IMU量测计算无人平台t+Δt时刻运动状态速度v_t+Δt、位置r_t+Δt以及旋转q_t+Δt的估计模型为：

其中，a_t,ω_t分别为t时刻的IMU原始加速度和角速度量测；

为加速度量测偏差和随机白噪声，

为角速度量测偏差和随机白噪声；g是世界坐标系W的重力加速度，

为惯性本体坐标系到世界坐标系的旋转；

IMU预积分误差约束f(B)为：

δv＝R(v_j-v_i-gΔt_ij)

f(B)＝arg min{δv,δr,δq}

式中，Δt_ij为连续时刻i,j的时间间隔，v_i,v_j表示时刻i,j的速度，r_i,r_j表示时刻i,j的位置，q_i,q_j表示时刻i,j的位姿，R为惯性本体坐标系到世界坐标系的旋转，δv,δr,δq分别表示预积分速度偏差、预积分位置偏差、预积分位姿偏差；

视觉、激光雷达的状态转移模型均为相对位置测量，激光雷达里程计采用LOAM算法通过计算曲率提取边缘点、平面点特征，执行点到边缘特征、点到平面特征的连续扫描匹配估计传感器运动，d_e,d_h分别表示点到边缘特征、点到平面特征的距离，其中点到边缘的特征约束为距离arg min{d_e}，点到平面的特征约束为距离arg min{d_h}；

针对无人平台在高速运动时造成的激光雷达运动畸变，对运动估计进行补偿，状态转移通过线性插值，假设t时刻点i运动状态矩阵为

旋转矩阵为R、平移量为T，则有：

其中，τ,τ_i,τ_t分别表示完整扫描周期、扫描点i时刻、起始扫描时刻，

为

的估计值；

将上述状态转移过程使用6自由度[x,y,z,roll,pitch,yaw]表示，进行欧拉角到旋转矩阵的变换，使用罗德里格斯公式进行变换：

其中，欧拉角θ＝[roll,pitch,yaw]，

为旋转方向单位向量，T＝[x,y,z]为转动幅度；

激光雷达匹配距离约束f(L)为：

f(L)＝arg min{∑d_e+∑d_h}

式中，∑d_e,∑d_h分别表示当前激光帧所有d_e,d_h的累积和；

基于视觉相机的定位技术，利用运动恢复结构获取连续图像帧相机姿态参数，假定存在连续可观测N关键图像帧的相机姿态

其中，

为关键图像帧相机姿态参数，

为连续关键图像帧所包含的特征点；

视觉重投影误差约束f(C)为：

其中，M_ij为关键帧i,j在世界坐标系下的共视坐标，s为特征深度尺度，K^C为相机内参数矩阵。

3.根据权利要求1所述的一种基于贝叶斯多传感器误差约束的定位置信度优化方法，其特征在于，步骤(2)的具体方式如下：

多源融合导航定位系统采用4类传感器：卫星导航GNSS、惯性导航IMU、视觉相机、激光雷达；

确定多源系统S中单一信源先验概率，

分别表征GNSS定位量测权重先验概率、IMU定位量测权重先验概率、激光雷达定位量测权重先验概率、视觉相机定位量测权重先验概率；无人平台行进环境的遮蔽程度影响GNSS、激光雷达和视觉相机的先验概率，IMU先验概率仅和时间累积相关；其中，将遮蔽环境分为4类，模糊描述为空旷、半遮挡、严重遮挡、完全遮挡；

GNSS定位量测权重先验概率

通过检测并剔除卫星周跳和当前卫星构型来描述，周跳越频繁表征遮挡越严重，当采样中无可用卫星时，为完全遮挡环境，概率

恒为0；可用卫星数不超过3颗星时，为严重遮挡，概率

恒为0；可用卫星数达到4颗卫星以上时，为半遮挡或开阔环境，此时卫星定位可解算；当卫星处于可解算状态时，通过PDOP位置精度因子描述卫星构型，PDOP位置精度因子小于等于3时可产生理想位置，概率

初始为0.95；PDOP位置精度因子介于3到7之间时定位结果可接受，概率

初始为0.5，PDOP位置精度因子大于等于7时定位结果不可接受，概率

初始为0.05；

已知激光雷达定位量测权重先验概率

和视觉相机定位量测权重先验概率

分别与GNSS定位量测权重先验概率

负相关，则空旷、半遮挡、严重遮挡、完全遮挡环境下有：

λ_L,λ_C为系数；

IMU定位量测权重先验概率

与环境的遮蔽情况无关，在一个滑动窗周期W内，IMU不仅维持GNSS信号阻塞时的导航，同时IMU预积分还用于优化GNSS收敛以及激光雷达、视觉相机的里程计；令

λ_B为系数，从而简化λ_L＝λ_C＝λ_B，对各单一信源先验概率进行归一化；

对于4类传感器和4类遮挡环境，选取自由度为9的卡方分布：

其中，z_S,z_A分别表示世界坐标系下多源系统定位结果和单一源定位结果，卡方检验中理论值为单一信源世界坐标系定位结果，测量值为多源系统世界坐标系定位结果；

查询卡方检验表获取初始化多源融合系统定位条件概率P(S|G)、P(S|B)、P(S|L)、P(S|C)；

由贝叶斯公式获得单一信源的置信后验概率：

对各单一信源先验概率更新，归一化消去P(S)，得到：

4.根据权利要求1所述的一种基于贝叶斯多传感器误差约束的定位置信度优化方法，其特征在于，步骤(3)的具体方式如下：

利用集束调整进行全局约束优化的后验概率权重最小误差模型，已知4类传感器误差约束包含IMU预积分误差约束、GNSS量测定位误差约束、激光雷达匹配距离误差约束、视觉重投影误差约束；全局约束优化是将单一信源置信的后验概率作为误差约束的权重进行赋值，累计联合形成全局约束，利用Levenberg-Marquarelt算法进行非线性参数求解，优化各信源运动估计模型，获得多源融合最佳置信定位解：

min{∑P(A|S)f(A)},A＝G,B,L,C。

Images