一种车辆4D毫米波雷达惯性里程计方法及计算机可读介质

本发明属于车辆状态估计，尤其涉及一种车辆4d毫米波雷达惯性里程计方法及计算机可读介质。

背景技术：

1、近年来，自动驾驶和自主机器人得到了迅速发展。基于感知传感器数据的定位导航技术一直是该领域的热门研究方向。目前，大多自动驾驶汽车使用lidar和光学相机作为主要传感器。这些传感器在良好天气条件情况下能保持不错的性能，但在雨雪、大雾等恶劣天气条件下难以工作。毫米波雷达的波长较长，fov更大，对灰尘、大雾、雨雪等小颗粒具有很强的抵抗力，可以在视觉环境恶化的情况下更好地工作，且对部分材料具有较强穿透能力。同时，毫米波雷达能提供多普勒速度观测，可以通过单帧数据估计机器人自我速度。

2、相比于3d毫米波radar，4d成像毫米波radar大幅提升了竖直方向的分辨率，增加了对目标高度的探测和解析，能够实现距离、方位、高度以及速度四个维度的信息感知，能更好地探测障碍物的轮廓、周围的环境信息、以及更好的路径规划和可通行空间检测功能。然而4d毫米波radar空间分辨率较低，点云稀疏性较大，且受到多径效应、谐波等噪声因素影响，难以实现稳定、连续、高精度的数据关联。因此，通过毫米波radar进行高精度三维空间定位和建图仍然是一项有挑战性的工作。

技术实现思路

1、针对现有技术方法的不足，本发明提出了一种车辆4d毫米波雷达惯性里程计方法及计算机可读介质。

2、本发明方法的技术方案为一种车辆4d毫米波雷达惯性里程计方法，具体包括以下步骤：

3、步骤1：按一定频率采集得到多个雷达采样时刻的4d radar观测数据序列，对每个雷达采样时刻的三维点云通过空间距离和基于局部点云统计特性的松弛滤波方法进行显著多径散射和散斑噪声点去除筛选得到每个时刻的松弛滤波后三维点云；将每个时刻松弛滤波后的三维点云以及点云中每个点对应的多普勒速度通过选权迭代的最小二范数方法估计每个时刻radar自身坐标系下的车辆速度，并剔除松弛滤波后点云中仍存在的动态干扰点得到每个时刻的radar预处理后数据；

4、步骤2：将相邻雷达采样时刻之间同步采集的imu观测子序列通过机械编排算法进行惯导数据的时间积分，得到下一个雷达采样时刻的预测状态；

5、步骤3：通过步骤1选权迭代的最小二范数方法估计得到的radar自身坐标系中的车辆速度和步骤2预测得到的下一个雷达采样时刻全局坐标系中的车辆速度构建的速度误差方程，并通过线性化误差方程得到下一个雷达采样时刻的残差初值、下一个雷达采样时刻的雅可比矩阵，利用卡尔曼滤波进行车辆状态初步更新，得到下一个雷达采样时刻的速度更新后的车辆状态；

6、步骤4：将过去多个雷达采样时刻的4d radar预处理后数据的三维点云通过ikdtree树策略构建局部地图，在局部地图中通过knn搜索得到下一个雷达采样时刻的radar预处理后数据的三维点云中每个三维点在局部地图中的n近邻点；将下一个雷达采样时刻的radar预处理后数据的三维点云中每个三维点到对应的n近邻点的平均欧式距离通过局部点云空间分布协方差方法进行加权计算，得到分布到多分布的平均加权距离，形成点云到局部地图的空间点匹配约束，以构建匹配误差方程以及线性化匹配误差方程，通过线性化匹配误差方程得到下一个雷达采样时刻的匹配残差初值和下一个雷达采样时刻的匹配残差雅可比矩阵，考虑所有4d radar预处理后数据对应的匹配约束构建最大后验估计问题，并进一步利用迭代卡尔曼滤波更新得到高精度的车辆状态；

7、步骤5：通过scancontext特征描述并检索历史radar点云帧中与当前时刻点云帧的最相似的帧；判断相似性是否大于既设阈值，若小于阈值，则舍弃该闭环，否则接受该闭环，并利用gicp算法计算两帧点云之间的三维投影变换关系，通过此空间变换关系作为闭环约束更新此前所有时刻的车辆状态。

8、作为优选，步骤1所述每个雷达采样时刻的4d radar观测数据由三维点云和三维点云中每个三维观测点的多普勒速度构成；

9、步骤1所述每个雷达采样时刻的4d radar观测数据，定义为：

10、

11、ρi,j＝(xi,j,yi,j,zi,j)

12、其中，datai表示第i个雷达采样时刻的4d radar观测数据，ρi,j表示第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的三维坐标，xi,j表示第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的x轴坐标，yi,j表示第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的y轴坐标，zi,j表示第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的z轴坐标，表示第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的多普勒速度观测，n表示每个雷达采样时刻的4d radar观测数据的三维点云中三维观测点的数量，nt为雷达采样时刻的数量；

13、步骤1所述选权迭代的最小二范数方法，具体如下：

14、构建每个雷达采样时刻的稳健估计问题，具体如下：

15、

16、其中，λi,j是第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的权重，表示第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的多普勒速度观测，ρi,j为第i个雷达采样时刻的4d radar观测数据的三维点云中第j个三维观测点的三维坐标，为第i个雷达采样时刻的radar自身坐标系下的车辆速度，n表示每个雷达采样时刻的4d radar观测数据的三维点云中三维观测点的数量；

17、在第一次迭代中，λi,j＝1，在随后的迭代中，∈表示迭代系数；

18、通过选权迭代最小二范数法进行求和，得到第i个雷达采样时刻的radar自身坐标系下的车辆速度以及第i个雷达采样时刻的协方差矩阵，将第i个雷达采样时刻的协方差矩阵的逆值作为第i个雷达采样时刻的radar自身坐标系下的车辆速度的观测协方差

19、步骤1所述剔除松弛滤波后点云中仍存在的动态干扰点得到每个时刻的radar预处理后数据，具体如下：

20、设置权重阈值λth；

21、若λi,j<λth时，ρi,j对应的三维观测点为动态噪声点，否则为静态内联点；

22、去除动态噪声点后得到静态内联观测点的集合，即为radar预处理后数据；

23、作为优选，步骤2所述相邻雷达采样时刻之间同步采集的imu观测子序列，定义为：

24、

25、其中，si表示第i雷达采样时刻与第i+1个雷达采样时刻之间的imu观测子序列，表示第i雷达采样时刻与第i+1个雷达采样时刻之间的imu观测子序列中第k个imu观测数据，为第i雷达采样时刻与第i+1个雷达采样时刻之间的imu观测子序列中第k个imu观测数据中加速度，为第i雷达采样时刻与第i+1个雷达采样时刻之间的imu观测子序列中第k个imu观测数据中角速度，表示imu子序列中观测的个数，nt为雷达采样时刻的数量；

26、步骤2所述下一个雷达采样时刻的预测状态，定义为：

27、

28、其中，表示第i+1个雷达采样时刻的预测状态，表示第i+1个雷达采样时刻的位置预测值，表示第i+1个雷达采样时刻的速度预测值，表示第i+1个雷达采样时刻的姿态预测值，表示第i+1个雷达采样时刻的加速度偏置预测值，表示第i+1个雷达采样时刻的角速度偏置预测值，为表示第i+1个雷达采样时刻的imu和雷达之间的外参姿态预测值，表示第i+1个雷达采样时刻的imu和雷达之间的外参距离预测值，表示第i+1个雷达采样时刻的全局坐标系下的重力向量预测值；

29、作为优选，步骤3中的速度误差方程，定义为：

30、

31、其中，为第i+1雷达采样时刻的速度残差，为第i+1个雷达采样时刻的radar自身坐标系下的车辆速度，rvi+1为第i+1个雷达采样时刻的radar自身坐标系下的速度真值，表示第i+1个雷达采样时刻的姿态，为表示第i+1个雷达采样时刻的imu和雷达之间的旋转外参，表示第i+1个雷达采样时刻的速度，ωi+1表示第i+1个雷达采样时刻的角速度测量值，表示第i+1个雷达采样时刻的角速度偏置，表示第i+1个雷达采样时刻的imu和雷达之间的平移外参，(·)×表示向量叉积，(·)t表示转置；

32、步骤3中线性化误差方程，定义为：

33、

34、

35、其中，表示第i+1个雷达采样时刻的速度残差，表示第i+1个雷达采样时刻的速度残差初值，为第i+1个雷达采样时刻的radar自身坐标系下速度的imu预测值，为第i+1个雷达采样时刻的雅可比矩阵，为第i+1个雷达采样时刻的误差状态；

36、作为优选，步骤4中的匹配误差方程，定义为

37、

38、

39、

40、其中，表示点云匹配残差，，ρi+1为第i+1雷达采样时刻radar观测点在radar坐标系中的坐标，gbτ为ρi+1局部地图中n近邻点在全局坐标系中的坐标，是全局坐标系到radar坐标系的变换矩阵，gi+1是第i+1雷达采样时刻的分布到多分布的距离加权矩阵，为radar自身坐标系到全局坐标系的变换矩阵，表示第i+1个雷达采样时刻的姿态，表示第i+1个雷达采样时刻的位置，为表示第i+1个雷达采样时刻的imu和雷达之间的旋转外参，为表示第i+1个雷达采样时刻的imu和雷达之间的旋转外参，01×3表示一行三列的0向量，ca为当前点ρi+1对应的空间分布协方差，为近邻点gbτ对应的空间分布协方差。

41、步骤4中线性化匹配误差方程，定义为：

42、

43、其中，为第i+1雷达采样时刻的匹配残差初值，为第i+1雷达采样时刻的匹配雅可比矩阵，为；

44、步骤4中的匹配约束最大后验估计问题，定义为：

45、

46、其中，为速度更新后得到的第i+1雷达采样时刻状态，为第i+1雷达采样时刻速度观测更新后的协方差矩阵，为第i+1雷达采样时刻后处理数据中第ζ个点的观测噪声，π为第i+1雷达采样时刻雷达点云帧中的有效观测个数。为第i+1雷达采样时刻第κ次迭代卡尔曼滤波状态更新值，为第i+1雷达采样时刻第κ次迭代卡尔曼滤波对应的误差状态，ji+1κ为对的线性化的雅可比矩阵，表示第i+1雷达采样时刻第κ次迭代第ζ个雷达观测点对应的匹配残差初值，表示第i+1雷达采样时刻第κ次迭代第ζ个雷达观测点对应的匹配雅可比矩阵，是状态量减法运算，其中旋转状态量符合右乘逆扰动，其他的符合向量减法运算，||·||表示欧式距离。

47、步骤4中的迭代卡尔曼更新系统状态定义如下：

48、

49、

50、其中：为第i+1雷达采样时刻第κ次迭代卡尔曼滤波得到的状态，为第i+1雷达采样时刻第κ次迭代滤波的匹配卡尔曼增益，表示点云匹配残差,i为单位矩阵，pi+1为第i+1雷达采样时刻匹配更新后的协方差矩阵，pi+1κ为第κ次迭代滤波更新后的协方差矩阵，pi+1κ+1为第κ+1次迭代滤波更新后的协方差矩阵，为第i+1雷达采样时刻第κ次迭代的匹配雅可比矩阵，田是状态量加法运算，其中旋转状态量符合右乘扰动，其他的符合向量减法运算,是状态量减法运算。

51、本发明还提供了一种计算机可读介质，所述计算机可读介质存储电子设备执行的计算机程序，当所述计算机程序在电子设备上运行时，使得所述电子设备执行所述车辆4d毫米波雷达惯性里程计方法的步骤。

52、本发明优点在于，创新地提出了一种基于4d radar的车辆3d位姿鲁棒估计方法，削弱了各类雷达噪声对定位结果的影响，并有效提高了稀疏雷达点云的匹配精度和鲁棒性。