本发明涉及机器人定位，更具体的说是涉及一种神经惯性网络辅助的激光雷达惯性里程计定位方法及系统。
背景技术：
1、近年来，自动驾驶车辆和自主移动机器人的快速发展对3d激光雷达的尺寸和价格提出了新的要求。其中固态激光雷达因其紧凑的结构设计和较低的制造成本成为领域内最受欢迎的传感器之一。然而，由于不需要通过旋转镜头或其他机械部件来扫描周围环境，固态激光雷达一般具有更小的视场角，其在近距离面对墙体、地面扫描或在长走廊等环境中，非常容易发生退化。
2、惯性传感器作为一种内部传感器，可以感知载体加速度和角速度的变化，不易受到外部环境的影响，因此被广泛应用于各种运动设备中。尤其可与激光雷达和视觉等外部传感器优势互补，从而提升多传感器融合系统的定位性能。然而，低成本的mems惯性传感器的测量不可避免地会受到各种误差源的影响，在传统惯性导航算法中双重积分会导致漂移不受限制，使惯性定位面临误差漂移问题。如果激光雷达-惯性导航系统长期处于退化场景中，系统仍然会面临定位失败的风险。
3、在理想条件下，惯性传感器误差足够小，可以通过利用具体的物理和数学规则设计导航算法获得准确可靠的姿态估计。传统基于运动学的惯性导航方法首先对角速度测量进行积分获得载体姿态，然后根据所获得的姿态将加速度测量值转换到导航坐标系。最后，通过对转换后的加速度进行双重积分获得位置。然而，现实应用中的低成本imu会受到如偏置误差、温度相关误差、随机传感器噪声和随机游走噪声等因素的影响，这些误差和噪声随着积分过程被放大，导航系统甚至会在几秒钟内失败。近年来，随着深度学习技术的发展，研究者们开始尝试采用数据驱动的方式来应对惯性定位的挑战。深度神经网络模型被应用于校准惯性传感器噪声、减少惯性导航系统的漂移，并将惯性数据与其他传感器信息融合。这些研究不依赖于具体的物理或数学模型，展示了数据驱动方法在惯性导航中的巨大潜力。
4、然而，当前对于数据驱动的神经惯性导航方法的研究还不够充分。大多数现有的数据驱动方法只能估计简单的二维平面运动，缺少对高度方向的运动估计，这极大限制了神经惯性导航方法的可应用场景。此外，大多数数据驱动的方法基于长短期记忆网络序列模型来学习时间相关性，但未能捕捉多变量时间序列之间的空间关系。惯性测量中的丰富上下文信息，包括空间和时间维度的特征表达还有待探索。
技术实现思路
1、有鉴于此，本发明提供了一种神经惯性网络辅助的激光雷达惯性里程计定位方法及系统，可有效解决激光雷达在退化场景中的定位漂移问题。
2、为了实现上述目的，本发明采用如下技术方案：
3、第一方面，本发明提供一种神经惯性网络辅助的激光雷达惯性里程计定位方法，包括以下步骤：
4、s1、实时获取激光雷达点云数据和对应的imu测量数据；基于所述imu测量数据预测载体当前时刻位姿；基于当前时刻位姿补偿所述激光雷达点云数据的运动畸变；将所述imu测量数据变换到世界坐标系，变换后的数据存储在imu缓冲区；
5、s2、将所述imu缓冲区的数据输入预先训练好的神经惯性网络模型进行推理，得到预测位置；
6、s3、将经过畸变矫正的激光雷达点云数据与局部地图进行配准，判断是否发生匹配退化；
7、s4、根据退化方向，将所述神经惯性网络模型的预测位置与激光雷达匹配估计位置进行融合，得到融合位姿；
8、s5、基于所述融合位姿更新里程计和局部地图，并进行下一轮的点云配准。
9、进一步的，s1中，将所述imu测量数据变换到世界坐标系的变换公式为：
10、
11、其中，ai和ωi分别表示imu坐标系下的加速度与角速度原始测量值；i∈[1,n]，表示imu缓冲区中的数据索引；rw表示从imu坐标系到世界坐标系的旋转矩阵，该值由激光惯性里程计系统的前向传播过程提供；ba表示加速度计的偏置，bg表示陀螺仪的漂移，ba和bg由激光惯性里程计系统实时估计；aw和ωw分别表示变换到世界坐标系后的加速度与角速度测量值。
12、进一步的，s2中，所述神经惯性网络模型的输入为imu缓冲区中的imu测量数据序列，输出为序列时间内的相对位移pos及对应的协方差cov，其数学模型表示为：
13、[pos,cov]＝fθ([aw,ωw]1:n)
14、其中，aw和ωw分别为世界坐标系下的加速度和角速度测量值，n表示序列长度。
15、进一步的，所述神经惯性网络模型包括resnet模块、时空嵌入模块、位置编码模块、注意力模块以及全连接层；
16、所述resnet模块用于提取imu测量数据序列的隐藏特征，生成初步的特征表示；
17、所述时空嵌入模块用于将时间信息和空间信息嵌入到初步的特征表示中，得到时空特征表示；
18、所述位置编码模块用于在时空特征表示中加入位置编码；
19、所述注意力模块用于采用概率稀疏注意力机制计算每个时空特征之间的相关性，强化重要特征的表示，输出特征图；
20、所述全连接层用于对特征图进行特征映射和维度压缩，生成最终的位置预测结果。
21、进一步的，通过概率稀疏自注意力机制提取主导注意力，表达式为：
22、
23、其中，q,k,v分别表示输入序列与参数矩阵相乘后所得到的查询矩阵、键矩阵和值矩阵；qu为q的子矩阵，仅包含能够主导注意力的前u个查询；d为输入特征的维度。
24、进一步的，s3包括：
25、计算激光雷达点云与局部地图配准的匹配残差：
26、
27、其中，hj为匹配残差；为激光雷达和惯性测量单元imu之间的已知外参；为待估计的载体位姿；为当前激光雷达点云扫描帧的第j个配准点，为该配准点的测量噪声，上角标l为激光雷达坐标系；xk为k时刻的系统状态量；为位于局部地图中与最近邻点所拟合平面，为该拟合平面的法向量；
28、令jj为的雅可比矩阵，当前帧参与配准点的个数为m，令j＝[j1t j2t …jm-1t jmt]t，t表示转置，将hessian矩阵排列为：
29、
30、其中，h′tt仅包含与平移向量相关的信息，是一个三行三列矩阵；j表示j的转置；
31、对ht′t进行特征值分解，得到由大到小排列的特征值λ1,λ2,λ3及对应的特征向量v1,v2,v3；jj与vk的夹角越小，则jj对vk方向所提供的约束程度越高；据此，计算每一个雅可比向量jj的平移相关分量与特征向量vk的内积，将结果累加得到特征向量的对齐分数：
32、
33、其中，为jj的前三维数据，仅包含与平移向量t相关的信息；jj[1:3]表示取jj的前三维数据；若小于预设阈值，则认为点云配准在vk方向发生了退化。
34、进一步的，s4中，若检测出退化，则将退化方向的位置估计结果替换为神经惯性网络的位置推理结果，而非退化方向则保留为激光雷达的配准结果，用公式表示为：
35、p1＝(pliot·v1)·v1＝(v1·v1t)·plio
36、p2＝(pliot·v2)·v2＝(v2·v2t)·plio
37、p3＝(pnett·v3)·v3＝(v3·v3t)·plio
38、其中，v3为退化方向的单位向量；v1与v2为非退化方向的单位向量；pnet为神经惯性网络的位置预测结果；plio为激光雷达-惯性里程计的状态估计结果；p1、p2分别表示激光雷达惯性里程计估计位置在非退化方向上的投影；p3表示神经惯性网络预测位置在退化方向上的投影。
39、进一步的，所述融合位姿表示为：
40、p＝p1+p2+p3
41、对应的协方差更新为：
42、
43、式中，vi表示特征向量，i∈{1,2,3}；表示投影矩阵；cov(plio)和cov(pnet)分别为plio和pnet所对应的协方差矩阵。
44、第二方面，本发明提供一种神经惯性网络辅助的激光雷达惯性里程计定位系统，其特征在于，包括：
45、数据获取及预处理模块，用于实时获取激光雷达点云数据和对应的imu测量数据；基于imu测量数据预测载体当前时刻位姿；基于当前时刻位姿补偿激光雷达点云数据的运动畸变；将imu测量数据变换到世界坐标系，变换后的数据存储在imu缓冲区；
46、网络推理模块，用于将缓冲区的数据输入预先训练好的神经惯性网络模型进行推理，得到预测位置；
47、退化检测模块，用于将经过畸变矫正的激光雷达点云数据与局部地图进行配准，判断是否发生匹配退化；
48、里程计和地图更新模块，用于根据退化方向，将神经惯性网络模型的预测位置与激光雷达匹配估计位置进行融合，得到融合位姿，并基于融合位姿更新里程计和局部地图，并进行下一轮的点云配准。
49、第三方面，本发明提供一种终端设备，包括存储器、处理器和存储在所述存储器上并可在所述处理器上运行的计算机程序；其特征在于，所述处理器执行所述计算机程序时，实现上述神经惯性网络辅助的激光雷达惯性里程计定位方法的步骤。
50、经由上述的技术方案可知，与现有技术相比，本发明具有以下有益效果：
51、(1)本发明基于概率注意力机制构建神经惯性网络模型，在保证计算效率的前提下，充分挖掘输入数据之间的时空相关性。
52、(2)本发明通过点云配准结果判断定位是否发生退化，若检测出退化，则将网络位置预测结果与点云配准的位置估计结果进行融合，可以有效提升激光雷达惯性里程计在退化环境中的鲁棒性，有效解决激光雷达在退化场景中的定位漂移问题。
53、(3)本发明可用于轮式、足式机器人平台和无人机定位，提升运动平台多情境感知能力。
1.一种神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，s1中，将所述imu测量数据变换到世界坐标系的变换公式为：
3.根据权利要求1所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，s2中，所述神经惯性网络模型的输入为imu缓冲区中的imu测量数据序列，输出为序列时间内的相对位移pos及对应的协方差cov，其数学模型表示为：
4.根据权利要求1所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，所述神经惯性网络模型包括resnet模块、时空嵌入模块、位置编码模块、注意力模块以及全连接层；
5.根据权利要求4所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，通过概率稀疏自注意力机制提取主导注意力，表达式为：
6.根据权利要求1所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，s3包括：
7.根据权利要求6所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，s4中，若检测出退化，则将退化方向的位置估计结果替换为神经惯性网络的位置推理结果，而非退化方向则保留为激光雷达的配准结果，用公式表示为：
8.根据权利要求7所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，其特征在于，所述融合位姿表示为：
9.一种神经惯性网络辅助的激光雷达惯性里程计定位系统，其特征在于，该系统适用于如权利要求1-8任一项所述的神经惯性网络辅助的激光雷达惯性里程计定位方法，包括：
10.一种终端设备，包括存储器、处理器和存储在所述存储器上并可在所述处理器上运行的计算机程序；其特征在于，所述处理器执行所述计算机程序时，实现如权利要求1-8任一项所述的神经惯性网络辅助的激光雷达惯性里程计定位方法的步骤。
