[发明专利]结合鱼眼相机实现车端4D毫米波雷达SLAM处理的方法、装置、处理器及其存储介质

权利要求书

1.一种结合鱼眼相机实现车端4D毫米波雷达SLAM处理的方法，其特征在于，所述的方法包括以下步骤：

(1)智能驾驶车辆实时采集4D毫米波雷达的点云数据以及鱼眼相机的图像数据，并对二者的数据进行预处理；

(2)对经过预处理后的点云数据以及图像数据同时进行位姿估计处理，分别得到基于4D毫米波雷达的车辆当前位姿以及基于鱼眼图像的车辆当前位姿；

(3)将两个位姿进行EKF滤波处理，并得到滤波后更为精准的位姿；

(4)将车辆的精准位姿与4D毫米波雷达的点云关键帧进行变换处理，得到世界坐标系下的关键帧点云，并将数帧所述的关键帧点云进行拼接，得到点云地图；

(5)将上述得到的精准位姿以及点云地图作为SLAM进行输出。

2.根据权利要求1所述的结合鱼眼相机实现车端4D毫米波雷达SLAM处理的方法，其特征在于，所述的步骤(1)具体包括以下步骤：

(1.1)对采集到的各个4D毫米波雷达的数据进行时间同步，并按照先后顺序的融合方式拼接成车辆周围360度的点云；

(1.2)将4D毫米波雷达的点云数据和鱼眼相机的图像数据进行时间对齐同步，并按照4D毫米波雷达点云的时间进行对齐。

3.根据权利要求2所述的结合鱼眼相机实现车端4D毫米波雷达SLAM处理的方法，其特征在于，所述的步骤(2)进行基于4D毫米波雷达的位姿估计具体包括以下步骤：

(2.1.a)提取点云关键帧：根据车辆的运动信息，即距离上一次提取关键帧车辆运动的距离Δd或者旋转角度Δα，如果Δd或者Δα大于一定阈值θ则将当前的4D毫米波雷达的点云P_i{x₀，x₁…x_m}添加为关键帧，记为K_i；

(2.2.a)点云关键帧间匹配：即将关键帧K_i和上一个关键帧K_i-1进行正态分布变换ndt匹配，将关键帧K_i的点云P_i{x₀，x₁…x_m}和关键帧K_i-1的点云P_i-1{x₀，x₁...x_n}划分为一个个体素单元格voxel，估计出每个体素单元格内所有点服从的正态分布p_voxel～N(μ，σ)，其中，μ为正态分布的均值，σ为正态分布的协方差；并求解出一个变换T_i(R_i，t_i)使得关键帧K_i的点经过变换落到关键帧K_i-1点所在的体素单元格voxel的概率值p_voxel最大，求解方式如下：

最后得到4D毫米波雷达帧间的相对平移变换和相对旋转变换

(2.3.a)利用以下公式计算基于4D毫米波雷达的车辆当前位姿

其中，为上一帧车辆的位姿估计。

4.根据权利要求3所述的结合鱼眼相机实现车端4D毫米波雷达SLAM处理的方法，其特征在于，所述的步骤(2)进行基于鱼眼图像的位姿估计具体包括以下步骤：

(2.1.b)提取视觉特征：在每一帧鱼眼图像上提取视觉ORB特征，并将当前时刻图像帧提取的特征记为F_i，上一时刻图像帧提取到的特征记为F_i-1，记鱼眼图像当前帧的位姿为上一时刻图像帧的位姿为

(2.2.b)视觉特征匹配：根据特征F_i和F_i-1之间的汉明距离找到两帧之间的对应匹配点，并利用PnP计算出当前时刻帧相对于上一时刻帧的相对平移变换和相对旋转变换

(2.3.b)利用以下公式计算基于鱼眼图像的得到的当前位姿

并从鱼眼图像位姿中取出旋转部分得到