基于多传感器融合的定位方法、系统、设备及存储介质技术方案

本发明专利技术提供了一种基于多传感器融合的定位方法、系统、设备及存储介质，该方法包括：从激光雷达中获取载体的激光雷达测量数据，从所述激光雷达测量数据中提取激光雷达因子；从惯性测量单元获取惯性测量数据，从所述惯性测量数据中提取IMU预积分因子；从毫米波雷达获取毫米波雷达测量数据，从所述毫米波雷达测量数据中提取毫米波雷达因子；基于所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子进行因子图优化，获取所述载体的定位数据。本发明专利技术提高了各种场景中建图和定位的准确性。发明专利技术提高了各种场景中建图和定位的准确性。发明专利技术提高了各种场景中建图和定位的准确性。

【技术实现步骤摘要】
基于多传感器融合的定位方法、系统、设备及存储介质


[0001]本专利技术涉及数据处理
，尤其涉及一种基于多传感器融合的定位方法、系统、设备及存储介质。

技术介绍

[0002]在地铁防撞、小区巡检、公共道路基础设施巡检等方面应用中，需要实时获取传感器方案主体在环境中的位置与姿态信息，从而进一步结合感知算法方案确定目标物体在环境地图中的全局位置。
[0003]在地铁隧道场景中具有高相似度的长道路，墙面为水泥壁，会附有线缆、变电设备，截面呈截断圆形或方形，光照条件较差，多数物体呈现灰黑色，缺乏纹理特征，导致视觉惯性里程计(Visual
‑
Inertial Odometry，VO)在特征点提取方面很不稳定。又由于处于地下，无法接受到GPS(Global Positioning System，全球定位系统)信号，因此存在建图和定位上的很多困难。
[0004]需要说明的是，上述
技术介绍
部分公开的信息仅用于加强对本专利技术的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现思路

[0005]针对现有技术中的问题，本专利技术的目的在于提供一种基于多传感器融合的定位方法、系统、设备及存储介质，提高各种场景中建图和定位的准确性。
[0006]本专利技术实施例提供一种基于多传感器融合的定位方法，包括如下步骤：
[0007]从激光雷达中获取载体的激光雷达测量数据，从所述激光雷达测量数据中提取激光雷达因子；
[0008]从惯性测量单元获取惯性测量数据，从所述惯性测量数据中提取IMU预积分因子；
[0009]从毫米波雷达获取毫米波雷达测量数据，从所述毫米波雷达测量数据中提取毫米波雷达因子；
[0010]基于所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子进行因子图优化，获取所述载体的定位数据。
[0011]在一些实施例中，从所述毫米波雷达测量数据中提取毫米波雷达因子，包括如下步骤：
[0012]基于所述毫米波雷达测量数据计算直方图；
[0013]从所述直方图中查找计数值最大的速度区间；
[0014]将所述计数值最大的速度区间的速度均值作为毫米波测量速度，将所述毫米波测量速度作为毫米波雷达因子。
[0015]在一些实施例中，将所述计数值最大的速度区间的速度均值作为毫米波测量速度之后，还包括如下步骤：
[0016]将所述毫米波测量速度并入邻近激光雷达帧[0]点的x轴位置处。
[0017]在一些实施例中，所述进行因子图优化，获取所述载体的定位数据，包括如下步骤：
[0018]将所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子输入残差块，获取所述残差块输出的载体的定位数据。
[0019]在一些实施例中，将所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子输入残差块之后，还包括如下步骤：
[0020]基于所述毫米波测量速度、当前帧的惯性测量单元旋转矩阵和载体状态参数，计算代价函数。
[0021]在一些实施例中，所述计算代价函数，包括如下步骤：
[0022]将所述毫米波测量速度左乘当前帧的惯性测量单元旋转矩阵后，与载体状态参数中x轴、y轴和z轴方向的速度值分别进行残差计算，得到状态速度量残差，作为所述计算代价函数。
[0023]在一些实施例中，将所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子输入残差块时，还包括基于所述激光雷达测量数据和所述惯性测量数据获取载体状态参数并输入所述残差块。
[0024]在一些实施例中，从所述激光雷达测量数据中提取激光雷达因子，包括如下步骤：
[0025]从所述激光雷达测量数据中提取当前帧的激光雷达点云特征，所述激光雷达点云特征包括面特征点特征、线特征点特征和无规则点特征；
[0026]获取当前的点云地图；
[0027]基于所述当前帧的激光雷达点云特征和所述点云地图进行点云特征匹配，获取激光雷达预测位置，作为所述激光雷达因子。
[0028]在一些实施例中，还包括如下步骤：
[0029]判断当前是否为建图模式；
[0030]如果是建图模式，则基于预设时间窗口内的所述激光雷达测量数据构建点云地图。
[0031]在一些实施例中，基于预设时间窗口内的所述激光雷达测量数据构建点云地图时，基于一个与所述激光雷达测量数据并行的线程实现点云地图构建、保存和读取，并对保存的点云地图进行预先栅格化处理，得到多个预设尺寸的栅格；
[0032]基于所述激光雷达点云特征和所述点云地图进行点云特征匹配时，采用所述点云地图中载体位置前后预设数量栅格中的点云数据与当前帧的激光雷达点云特征进行点云特征匹配。
[0033]本专利技术实施例还提供一种基于多传感器融合的定位系统，用于实现所述的基于多传感器融合的定位方法，所述系统包括：
[0034]第一数据处理模块，用于从激光雷达中获取载体的激光雷达测量数据，从所述激光雷达测量数据中提取激光雷达因子；
[0035]第二数据处理模块，用于从惯性测量单元获取惯性测量数据，从所述惯性测量数据中提取IMU预积分因子；
[0036]第三数据处理模块，用于从毫米波雷达获取毫米波雷达测量数据，从所述毫米波雷达测量数据中提取毫米波雷达因子；
[0037]载体定位模块，用于基于所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子进行因子图优化，获取所述载体的定位数据。
[0038]本专利技术实施例还提供一种基于多传感器融合的定位设备，包括：
[0039]处理器；
[0040]存储器，其中存储有所述处理器的可执行指令；
[0041]其中，所述处理器配置为经由执行所述可执行指令来执行所述的基于多传感器融合的定位方法的步骤。
[0042]本专利技术实施例还提供一种计算机可读存储介质，用于存储程序，所述程序被处理器执行时实现所述的基于多传感器融合的定位方法的步骤。
[0043]应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。
[0044]本专利技术的基于多传感器融合的定位方法、系统、设备及存储介质具有如下有益效果：
[0045]本专利技术首先分别获取激光雷达因子、IMU预积分因子和毫米波雷达因子，然后将三种因子联合进行因子图优化，获取到载体的定位数据，通过毫米波雷达因子可以提供稳定速度约束，从而提高了建图和定位的稳定性和准确性。本专利技术可以应用于地铁隧道场景中，例如对地铁自身进行定位，并可用于防撞控制的基础，由毫米波雷达提供环境中静态物体的相对速度，本专利技术也可以应用于其他建图和定位场景中，例如街道、小区、封闭场所等。
附图说明
[0046]通过阅读参照以下附图对非限制性实施例所作的详细描述，本专利技术的其它特征、目的和优点将会变得更明显。
[0047]图1是本专利技术一实施例的基于多传感器融合的定位方法的流程图；...

【技术保护点】

【技术特征摘要】
1.一种基于多传感器融合的定位方法，其特征在于，包括如下步骤：从激光雷达中获取载体的激光雷达测量数据，从所述激光雷达测量数据中提取激光雷达因子；从惯性测量单元获取惯性测量数据，从所述惯性测量数据中提取IMU预积分因子；从毫米波雷达获取毫米波雷达测量数据，从所述毫米波雷达测量数据中提取毫米波雷达因子；基于所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子进行因子图优化，获取所述载体的定位数据。2.根据权利要求1所述的基于多传感器融合的定位方法，其特征在于，从所述毫米波雷达测量数据中提取毫米波雷达因子，包括如下步骤：基于所述毫米波雷达测量数据计算直方图；从所述直方图中查找计数值最大的速度区间；将所述计数值最大的速度区间的速度均值作为毫米波测量速度，将所述毫米波测量速度作为毫米波雷达因子。3.根据权利要求2所述的基于多传感器融合的定位方法，其特征在于，将所述计数值最大的速度区间的速度均值作为毫米波测量速度之后，还包括如下步骤：将所述毫米波测量速度并入邻近激光雷达帧[0]点的x轴位置处。4.根据权利要求2所述的基于多传感器融合的定位方法，其特征在于，所述进行因子图优化，获取所述载体的定位数据，包括如下步骤：将所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子输入残差块，获取所述残差块输出的载体的定位数据。5.根据权利要求4所述的基于多传感器融合的定位方法，其特征在于，将所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子输入残差块之后，还包括如下步骤：基于所述毫米波测量速度、当前帧的惯性测量单元旋转矩阵和载体状态参数，计算代价函数。6.根据权利要求5所述的基于多传感器融合的定位方法，其特征在于，所述计算代价函数，包括如下步骤：将所述毫米波测量速度左乘当前帧的惯性测量单元旋转矩阵后，与载体状态参数中x轴、y轴和z轴方向的速度值分别进行残差计算，得到状态速度量残差，作为所述计算代价函数。7.根据权利要求4所述的基于多传感器融合的定位方法，其特征在于，将所述激光雷达因子、所述IMU预积分因子和所述毫米波雷达因子输入残差块时，还包括基于所述激光雷达测量数据和所述惯性测量数据获取载体状态参数并输入所述残差块。8.根据权利要求1所述的基于...

【专利技术属性】
技术研发人员：谭黎敏，赵钊，纪豪，
申请(专利权)人：上海西井信息科技有限公司，
类型：发明
国别省市：