CN115265551B 一种无人驾驶复杂场景下的多传感器融合定位方法和系统 （北京理工大学）

一种无人驾驶复杂场景下的多传感器融合本发明涉及一种无人驾驶复杂场景下的多后验概率估计将由定位信息构建的因子图模型2根据定位信息构建因子图模型；所述定位信息包括：基于惯性测到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达里程将因子图优化过程输出的无人车位姿信息及其误差协方差矩阵作为自适应滤波噪声模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因数据的残差序列和所述观测数据的残差协方差矩阵均基于无人车的预对所述归一化处理结果进行加权处理得到所述故利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激光雷利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的协方3非线性问题转化模块，用于利用最大后验概率估计将所述定位结果确定模块，用于求解所述非线性问题得到无人车的定位将因子图优化过程输出的无人车位姿信息及其误差协方差矩阵作为自适应滤波噪声判断单元，用于根据所述故障检测指标判断全球导第一因子图模型构建单元，用于当所述判断结果为全球据所述基于惯性测量单元预积分模型得到的定位信息和所述基于激光雷达里程计得到的第二因子图模型构建单元，用于当所述判断结果为全球导航卫星系统未发生故障时，模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因故障检测函数构建子单元，用于利用观测数据的残差序列和观测数据的故障检测指标确定子单元，用于对所述归一化处理结果进行加权处理得修正模块，用于利用上一时刻激光雷达里程计的噪声均值和噪噪声均值协方差矩阵确定模块，用于利用自适应滤波确定修正后的激光雷达里程计4[0004]现有技术中，无人驾驶多传感器融合定位方法多以滤波方法进行多传感器融感知和车身控制提供基准数据；又如中国专利公布号为CN108692701A，公布日为[0006]本发明的目的是提供一种具有强鲁棒性的无人驾驶复杂场景下的多传感器融合5型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达积分模型得到的定位信息和所述基于激光雷达里程计得到的定位观测数据的残差序列和所述观测数据的残差协方差矩阵均基于无人车的预设观测数据确[0025]利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激[0026]利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的化方法，即采用最大后验概率估计将由定位信息构建的因子图模型转化为非线性问题后，6[0037]第一因子图模型构建单元，用于当所述判断结果为全球导航卫星系统发生故障定位模型得到的定位信息和所述基于激光雷达里程计得到的定位信息[0048]噪声均值协方差矩阵确定模块，用于利用自适应滤波确定修正后的激光雷达里[0049]因本发明提供的无人驾驶复杂场景下的多传感器融合定位系统实现的技术效果7[0054]图4为本发明实施例提供的无人驾驶复杂场景下的多传感器融合定位方法的整体[0055]图5为本发明提供的无人驾驶复杂场景下的多传感器融合定位系统的结构示意[0057]本发明的目的是提供一种具有强鲁棒性的无人驾驶复杂场景下的多传感器融合分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光得到传感器观测数据zi与对应状态变量Xi的非标准后验概8t,将公式带入公式中并取负对i测数据的残差序列和观测数据的残差协方差矩阵均基于无人车的预设观测数据确定。其TD2kTD2时，故障检测法出现误警的概率较大，将λk映射到区间[0,1]，进行归一化处理得到9预积分模型得到的定位信息和基于激光雷达里程计得到的定位信息构建航卫星系统子系统判定为故障后，只有当检验值小于解除故障阈值ε时，才能解除故障报k<ε。[0079]利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激[0080]利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的[0082]①利用当前时刻的激光雷达子定位系统的观测值zk和因子图优化输出的当前时[0084]③利用残差ek和因子图优化输出的当前时刻无人车位姿的误差协方差矩阵计[0093]非线性问题转化模块2用于利用最大后验概率估计将因子图模型转化为非线性问基于惯性测量单元预积分模型得到的定位信息和基于激光雷达里程计得到的定位信息构定位信息和基于激光雷达里程计得到的定位信息[0101]故障检测函数构建子单元用于利用观测数据的残差序列和观测数据的残差协方[0103]故障检测指标确定子单元用于对归一化处理结果进行加权处理得到故障检测指[0106]第二判断子单元用于当故障检测指标大于等于1时，判定全球导航卫星系统发生修正当前时