一种耦合深度学习与物理优化的4D毫米波雷达自监督里程计方法

本发明涉及自动驾驶，尤其是涉及一种耦合深度学习与物理优化的4d毫米波雷达自监督里程计方法。

背景技术：

1、根据传感器采集的时间序列数据进行相对运动估计，是自动驾驶车辆实现定位的关键模块，也称为里程计，它为下游的车辆决策规划任务提供必需的信息。视觉相机和激光雷达是现有的里程计系统常常采用的传感器，尽管一些基于二者的里程计方法取得了一定的进展，然而考虑到天气、探测距离和成本等因素，上述两类传感器在里程计中的应用受到了较大的限制。

2、近年来，4d毫米波雷达作为一种新兴的传感器，已经越来越多地被汽车行业所关注。首先，4d毫米波雷达可以提供场景中点的3d位置、径向相对速度(rrv)和雷达散射截面积(rcs)等信息，更丰富的信息使得4d毫米波雷达具备天然的优势。其次，4d毫米波雷达可以在极端的天气下工作，具备全天候的特性。此外，相对较低的成本使得4d毫米波雷达更为工业界所欢迎。

3、尽管4d毫米波雷达具备突出的优势，然而从4d毫米波雷达点云中估计车辆相对运动仍旧面临一些挑战。同激光雷达点云相比，4d毫米波雷达点云稀疏得多，这种稀疏性使得提取鲁棒的点云特征变得困难。由于多径效应等原因，雷达点云也更为嘈杂，不利于进行里程计任务。

技术实现思路

1、本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种耦合深度学习与物理优化的4d毫米波雷达自监督里程计方法，能够有效提取稀疏嘈杂的4d毫米波雷达点云特征，并能获得良好的里程计性能。

2、本发明的目的可以通过以下技术方案来实现：一种耦合深度学习与物理优化的4d毫米波雷达自监督里程计方法，包括以下步骤：

3、s1、在训练阶段，采用基于point transformer的多层级特征提取网络，一方面从关联点云对中提取两帧点云对应的特征信息，另一方面从关联点云对中的前一帧的点云中提取上下文特征信息；

4、s2、根据两帧点云对应的特征信息，计算点云关联场；

5、s3、基于上下文特征和点云关联场，通过迭代优化模块，更新关联点云对之间的位姿变换，并根据每次迭代输出的位姿变换构建自监督信号，完成里程计网络的训练；

6、s4、在推理阶段，将待测试序列的点云数据输入里程计网络，初始化处理后依次计算各相邻帧的位姿变换，获得整个待测试序列对应的定位轨迹。

7、进一步地，所述多层级特征提取网络中，对于输入的某一帧点云st，将其3d位置信息、rrv特征信息和rcs特征信息分别通过多层感知机进行编码；

8、之后，将得到的三种特征在通道维度上进行拼接，输入基于point transformer的多层级网络中，其中，point transformer层基于向量自注意力机制，在每个点周围的局部邻域内应用自注意力，以提取逐点特征，同时通过注意力机制对嘈杂点赋予低注意力得分；

9、在不同层级对点云以不同的分辨率进行特征编码，并在不同层级特征之间采用跳跃连接，防止网络层次太深而导致训练时出现梯度消失的问题。

10、进一步地，所述基于point transformer的多层级网络包含两个层级，分别对256个点和128个点进行特征提取，首先用一层point transformer层对全部256个点进行特征编码，之后下采样层将得到的点云特征进行最远点采样，并利用k近邻方法通过多层感知机聚合采样点周围点的特征信息，得到点数减少为128后的特征；随后，利用一个多层感知机和一层point transformer层进一步编码高级特征，上采样层将输入特征经过线性映射后通过三线性插值将点数恢复为256；

11、之后通过跳跃连接将两个层级的特征拼接起来，利用point transformer层细化特征，最后通过一个多层感知机编码得到最终的点云特征。

12、进一步地，所述步骤s2中点云关联场的计算公式为：

13、

14、ft-1∈r256×d

15、ft∈r256×d

16、其中，ft-1为st-1帧点云提取的特征，ft为st帧点云提取的特征，d为特征维数。

17、进一步地，所述步骤s3中迭代优化模块具体是根据输入的上下文特征信息与点云关联场，在局部滑动窗口内构建残差方程进行优化。

18、进一步地，所述步骤s3中迭代优化模块完成一次迭代的过程包括：

19、首先根据第k-1次估计的位姿变换对点云st-1进行变换，得到点云其与点云st相对应，具体变换关系如下所示：

20、

21、根据点云对点云st进行k近邻查询，得到对应点云索引，据此在点云关联场中提取对应的关联特征；

22、之后由运动特征编码器利用多个卷积层将关联特征、第k-1次估计的场景流进行聚合，并与上下文特征进行拼接，得到运动特征，以作为门控循环单元gru的输入；

23、gru输出的特征经过一个场景流回归头估计点云与点云st-1之间的场景流场景流回归头由两层卷积层与中间的relu激活函数组成，同时利用置信度回归头估计场景流对应的置信度置信度回归头由两层卷积层、relu激活函数以及sigmoid激活函数组成，输出的置信度表示估计的场景流在三个维度上的可信程度；

24、利用得到的场景流，对点云进行扭曲，得到后续优化的目标

25、

26、给定网络估计的通过最小二乘优化的方式求取对应的第k次位姿变换

27、

28、其中，‖·‖∑t,t-1代表mahalanobis距离，它根据置信度权重对残差项进行加权，

29、对于最小二乘方程，利用高斯牛顿优化算法进行求解，将位姿变换用传感器相对于世界坐标系的位姿和表示：则有迭代优化的状态量考虑st-1中一点pi＝[x,y,z,1]t，在中对应点为记由高斯牛顿方程：

30、

31、其中，j为雅可比矩阵，推导过程基于李代数扰动求导方程，j＝[j1,j2]，具体展开如下：

32、

33、

34、其中，ad(·)表示伴随矩阵，ξt、ξt-1表示位姿对应的李代数表示；

35、基于高斯牛顿方程，迭代求解得到位姿变换并将此优化过程作为计算图的一部分实现，并且在训练期间通过推导的雅可比矩阵进行反向传播，实现物理优化与深度学习的深层耦合，之后，用第k次估计的位姿变换和场景流进行更新，如此迭代若干次，对每次得到的位姿变换构建自监督损失，将最终迭代得到的位姿变换作为网络输出。

36、进一步地，所述迭代优化模块利用滑动窗口将多对关联点云之间构建的最小二乘约束共同优化，滑动窗口内点云st不仅与相邻帧st-1、st+1构成约束，还与距离相差两帧的st-2、st+2构成约束，即在建立的最小二乘优化方程中引入多帧数据，以有效地减少里程计在长轨迹中的漂移，获得更为平滑的位姿变换估计。

37、进一步地，所述步骤s3中，具体是通过自监督损失函数构建起网络的自监督信号，所述自监督损失函数包括最近邻损失、点位移损失和倒角损失。

38、进一步地，所述自监督损失函数具体为：

39、

40、

41、

42、

43、其中，为最近邻损失，nn(·)代表寻找最近邻函数，为点位移损失，为倒角损失，s′t-1是指示函数，在满足条件时返回1、否则返回0，v(·)表示点的高斯概率密度，δ为预先设置的阈值常数，通过设置[·]+运算符表示max(·,0)使得计算时数值低于常量∈的误差被忽略，λ1、λ2、λ3分别为对应最近邻损失、点位移损失、倒角损失的权重，∑表示综合所有对关联帧的位姿变换估计损失。

44、进一步地，所述步骤s4中初始化处理具体是先累积前n帧点云的数据共同优化计算各帧间位姿变换，之后每得到一帧新的点云数据则计算其对应的位姿变换。

45、与现有技术相比，本发明具有以下优点：

46、本发明设计基于point transformer的多层级特征提取网络来提取稀疏嘈杂的4d毫米波雷达点云的特征，并设计迭代优化模块，用于更新关联点云对之间的位姿变换，并根据每次迭代输出的位姿变换构建自监督信号。由此实现一个耦合深度学习与物理优化的4d毫米波雷达里程计，既能通过数据驱动的方式获得良好的里程计性能，也能够通过几何约束提升方法的泛化性和鲁棒性，并减少里程计的长轨迹漂移现象。

47、本发明设计基于point transformer的多层级网络，point transformer层基于向量自注意力机制，在每个点周围的局部邻域内应用自注意力，能够有效地提取逐点特征，与此同时，通过注意力机制对一些嘈杂点赋予较低的注意力得分，从而缓解4d毫米波雷达点云嘈杂的问题，此外，为了充分提取稀疏点云的特征，本发明对网络采取多层级的设计，在不同层级对点云以不同的分辨率进行特征编码，以获取更丰富更有效的高级特征，并在不同层级特征之间采用跳跃连接，防止网络层次太深而导致训练时出现梯度消失的问题。

48、本发明设计基于gru(gate recurrent unit，门控循环单元)的迭代优化模块，该模块通过网络回归场景流，据此在滑动窗口内构建残差方程进行优化求解位姿变换，并迭代地进行更新获取最优解。通过物理意义上的几何约束来构建残差方程优化求解位姿变换，有利于提升方法的泛化性与鲁棒性，缓解深度学习方法泛化性差的问题。此外，在建立的最小二乘优化方程中引入多帧数据，能够充分利用多帧的历史信息，对于输入的序列点云数据，利用滑动窗口将多对关联点云之间构建的最小二乘约束共同优化，从而有效地减少里程计在长轨迹中的漂移，获得更为平滑的位姿变换估计。

49、本发明在构建自监督信号时，通过引入三种自监督损失函数，包括最近邻损失、点位移损失和倒角损失，这些损失从不同的方面共同调节网络学习，损失函数计算时考虑训练时滑动窗口内所有对关联帧之间估计的位姿变换，能够完全以自监督的方式训练，解决了依赖人工真值标签的问题，有效地实现了一种自监督学习的里程计框架。