CN114924260A - 一种多激光雷达点云配准方法 - Google Patents

Abstract

本发明实施例涉及一种多激光雷达点云配准方法，所述方法包括：获取多个第一激光雷达对应的多个第一点云帧序列；对各个第一点云帧的点进行干扰点删除和降采样处理；对各个第一点云帧的点进行分类处理；对各个第一点云帧序列的点进行动态点过滤处理；以雷达类型为主雷达类型的第一激光雷达的第一点云帧序列为参考对其他类型的第一激光雷达的第一点云帧序列进行点云配准处理。通过本发明，能在点云拼接前减少因位姿偏差造成的点云错位和分层等问题。

Description

一种多激光雷达点云配准方法

技术领域

本发明涉及数据处理技术领域，特别涉及一种多激光雷达点云配准方法。

背景技术

自动驾驶感知系统的精度提升依赖于数据标注质量的提升，若数据标注质量低下则会严重影响感知算法的真值获取，从而对算法训练造成影响。现有的数据在送人工标注之前一般先进行标注预处理，例如对多帧点云进行拼接。但是现有的基于原始位姿进行点云拼接的处理方式受定位精度影响，拼接后的点云容易出现点云错位、分层等现象。

发明内容

本发明的目的，就是针对现有技术的缺陷，提供一种多激光雷达点云配准方法、电子设备及计算机可读存储介质，在对各雷达点云进行一系列去干扰、降采样、分类、过滤动态点处理之后，以主雷达为基准对各个辅助雷达(一类雷达、二类雷达)的点云进行配准。通过本发明，能在点云拼接前减少因位姿偏差造成的点云错位和分层等问题。

为实现上述目的，本发明实施例第一方面提供了一种多激光雷达点云配准方法，所述方法包括：

获取多个第一激光雷达对应的多个第一点云帧序列；所述第一点云帧序列与所述第一激光雷达一一对应；所述第一点云帧序列包括多个第一点云帧，且所述第一点云帧按时间先后排序；

对各个所述第一点云帧的点进行干扰点删除和降采样处理；

对各个所述第一点云帧的点进行分类处理；

对各个所述第一点云帧序列的点进行动态点过滤处理；

以雷达类型为主雷达类型的所述第一激光雷达的所述第一点云帧序列为参考对其他类型的所述第一激光雷达的所述第一点云帧序列进行点云配准处理。

优选的，各个所述第一激光雷达对应一个所述雷达类型，所述雷达类型包括主雷达类型、一类雷达类型和二类雷达类型；各个所述第一激光雷达安装位置的水平高度保持一致；所述多个第一激光雷达中有且只有一个所述第一激光雷达的所述雷达类型为主雷达类型；所述雷达类型为一类雷达类型的所述第一激光雷达的扫描精度和视场角都大于所述雷达类型为二类雷达类型的所述第一激光雷达的扫描精度和视场角。

优选的，所述对各个所述第一点云帧的点进行干扰点删除和降采样处理，具体包括：

对所述第一点云帧的所有点进行遍历；遍历时，提取当前遍历点的点云通道数据(x，y，z，r)；并在反射强度值r超过预设的反射强度范围时将所述当前遍历点作为干扰点删除；x、y、z为点云坐标系的X、Y、Z轴坐标值，r为所述反射强度值；

在完成干扰点删除的所述第一点云帧中，对所有所述点云通道数据(x，y，z，r)中的X轴最大、最小坐标值进行统计生成对应的X轴最大坐标值x_max和X轴最小坐标值x_min；并对所有所述点云通道数据(x，y，z，r)中的Y轴最大、最小坐标值进行统计生成对应的Y轴最大坐标值y_max和Y轴最小坐标值y_min；并对所有所述点云通道数据(x，y，z，r)中的Z轴最小坐标值进行统计生成对应的Z轴最小坐标值z_min；

根据所述X轴最大坐标值x_max、所述X轴最小坐标值x_min、所述Y轴最大坐标值y_max、所述Y轴最小坐标值y_min、所述Z轴最小坐标值z_min和所述第一点云帧中各点的所述点云通道数据(x，y，z，r)计算各点对应的聚类特征值h,

α为预设的特征参数，所述特征参数α的取值为大于1的正整数；

将所述第一点云帧中所述聚类特征值h相同的多个点归入到同一个第一聚类点组合；并对各个所述第一聚类点组合的所有所述点云通道数据(x，y，z，r)按数据维度进行均值计算得到对应的采样点云通道数据(x_aver，y_aver，z_aver，r_aver)；并根据得到的所有所述采样点云通道数据(x_aver，y_aver，z_aver，r_aver)重构所述第一点云帧；重构后的所述第一点云帧中，点数量与所述采样点云通道数据(x_aver，y_aver，z_aver，r_aver)的数量一致，各点的点云通道数据分别对应一个所述采样点云通道数据(x_aver，y_aver，z_aver，r_aver)。

优选的，所述对各个所述第一点云帧的点进行分类处理，具体包括：

提取所述第一点云帧中各点的点云通道数据(x，y，z，r)构成点云通道数据集合；x、y、z为点云坐标系的X、Y、Z轴坐标值，r为所述反射强度值；

将所述点云通道数据集合中X-Y平面坐标(x，y)相同的多个点归入到同一个第二聚类点组合；并将各个所述第二聚类点组合中，最小Z轴坐标值对应的点作为第一参考点；

对得到的所有所述第一参考点进行遍历；遍历时，将当前遍历的所述第一参考点作为参考点U，并提取所述参考点U的所述点云通道数据作为对应的第一点云通道数据(x_U，y_U，z_U，r_U)；根据第一点云通道数据(x_U，y_U，z_U，r_U)和预设的雷达安装高度z_C计算对应的Z轴相对间距百分比a_CU,a_CU＝|z_C-z_U|/z_C；若所述Z轴相对间距百分比a_CU大于或等于预设的百分比阈值则将所述参考点U作为第二参考点；

对得到的所有所述第二参考点进行遍历；遍历时，将当前遍历的所述第二参考点作为参考点P，将与所述参考点P直线距离最近的所述第二参考点作为参考点Q；提取所述参考点P和所述参考点Q的所述点云通道数据作为对应的第二点云通道数据(x_P，y_P，z_P，r_P)和第三点云通道数据(x_Q，y_Q，z_Q，r_Q)；并根据所述第二、第三点云通道数据计算所述参考点P和所述参考点Q的X-Y平面间距生成对应的平面间距d_PQ，

并根据所述第二、第三点云通道数据计算所述参考点P和所述参考点Q的Z轴垂直间距生成对应的垂直间距h_PQ，h_PQ＝|z_P-z_Q|；并根据所述垂直间距h_PQ和所述平面间距d_PQ计算所述参考点P和所述参考点Q间的连接线与X-Y平面的夹角θ，

若所述夹角θ小于预设的角度阈值则将所述参考点P和所述参考点Q都标记为地面点；

统计所述地面点的数量作为第一数量n；并对所有所述地面点的点云通道数据(x_i，y_i，z_i，r_i)按数据维度进行均值计算得到地面中心点的点云通道数据(x_o，y_o，z_o，r_o)，1≤i≤n；根据所有所述点云通道数据(x_i，y_i，z_i，r_i)和所述点云通道数据(x_o，y_o，z_o，r_o)构建协方差矩阵C，

并将所述协方差矩阵C输入预设的PCA计算器进行主成分分析得到三个特征向量，从中选择向量长度最短的作为地面法向量；

将所述第一点云帧中除所述地面点之外的所有点记为非地面点；并对素有非地面点进行聚合得到多组非地面点组合；并为每组所述非地面点组合创建对应的协方差矩阵C’；并将所述协方差矩阵C’输入预设的PCA计算器进行主成分分析得到三个特征向量，从中选择向量长度最短的作为对应的第一法向量；若所述第一法向量与所述地面法向量垂直，则将对应的所述非地面点组合中的所有所述非地面点标记为一类点。

优选的，所述对各个所述第一点云帧序列的点进行动态点过滤处理，具体包括：

对当前第一点云帧序列中的各个所述第一点云帧逐个进行目标识别处理得到对应的多个目标识别框；并为所有所述第一点云帧中的各个所述目标识别框都分配一个初始化为空的目标标识；并对第一个所述第一点云帧中的各个所述目标识别框的所述目标标识进行顺序标识设置；所述目标识别框对应一组目标识别框参数；所述目标识别框参数包括所述目标标识、中心点坐标、框高度H、框宽度W和框深度L；

在所述当前第一点云帧序列的相邻两帧所述第一点云帧中，将时间靠前的作为上一帧、时间靠后的作为下一帧，并将所述上一帧中的所述目标识别框记为第一识别框、将所述下一帧中的所述目标识别框记为第二识别框；并使用卡尔曼滤波器的状态方程，以各个所述第一识别框的中心点坐标为上一时刻状态量，对所述第一识别框在下一时刻的中心点坐标进行预测得到对应的预测中心点坐标；并由各个所述预测中心点坐标结合所述框高度H、所述框宽度W和所述框深度L，在所述下一帧中构建对应的第三识别框；并对任一对所述第三、第二识别框的相似度进行计算生成对应的第一相似度；并根据得到的所有所述第一相似度，创建能够反映所有所述第二、第三识别框一一对应关系的关联矩阵；并基于深度匈牙利网络DHN对所述关联矩阵进行最佳匹配运算从而得到与任一所述第二识别框最匹配的所述第三识别框并记为匹配识别框；并使用各个所述匹配识别框对应的所述第一识别框的所述目标标识，对与当前匹配识别框对应的所述第二识别框的所述目标标识进行设置；

从所述当前第一点云帧序列的第一帧到最后一帧所述第一点云帧中，提取各个所述目标标识对应的所述目标识别框的所述中心点坐标构成对应的目标轨迹；所述目标轨迹包括多个轨迹点，每个轨迹点的坐标对应一个所述中心点坐标；

从各个所述目标轨迹中提取第一个所述轨迹点的坐标作为起始坐标，提取最后一个所述轨迹点的坐标作为结束坐标，对从所述起始坐标到所述结束坐标的相对速度进行计算得到对应的目标运动速度；若所述目标运动速度高于预设的运动速度阈值则将当前目标轨迹对应的所述目标标识记为运动目标标识；

将所述当前第一点云帧序列的各个所述第一点云帧中，所述目标标识为运动目标标识的所述目标识别框内的点记为动态点，并将所所有所述动态点删除。

优选的，所述以雷达类型为主雷达类型的所述第一激光雷达的所述第一点云帧序列为参考对其他类型的所述第一激光雷达的所述第一点云帧序列进行点云配准处理，具体包括：

将所述雷达类型为主雷达类型的所述第一激光雷达记为主雷达；并将所述雷达类型为一类雷达类型的所述第一激光雷达记为一类雷达；并将所述雷达类型为二类雷达类型的所述第一激光雷达记为二类雷达；

将所述主雷达对应的所述第一点云帧序列的所有所述第一点云帧的所有点都提取出来聚合成对应的主雷达点云；并将各个所述一类雷达对应的所述第一点云帧序列的所有所述第一点云帧的所有点都提取出来聚合成对应的一类雷达点云；并将各个所述二类雷达对应的所述第一点云帧序列的所有所述第一点云帧的所有点都提取出来聚合成对应的二类雷达点云；

基于任一所述一类、二类雷达与所述主雷达的坐标对应关系，将对应的所述一类、二类雷达点云中各点的点云通道数据(x，y，z，r)的点云坐标(x，y，z)统一转换为所述主雷达对应的点云坐标系的点云坐标；

以所述主雷达为中心，根据所述主雷达点云进行点云地图构建生成对应的第一点云地图；并在所述第一点云地图中对所述主雷达点云的各个地面点和一类点进行标记；

在所述第一点云地图与任一所述一类雷达点云中，按最小距离匹配原则选择多对第一匹配点对；所述第一匹配点对为(k_0,s,j，k_1,s,j)，k_0,s,j为所述第一点云地图的一类点，k_1,s,j为所述一类雷达点云中与k_0,s,j匹配的一类点，1≤s≤M，1≤j≤E，s为所述一类雷达的雷达索引，M为所述一类雷达的总数，j为所述第一匹配点对的点对索引，E为所述第一匹配点对的总数；

在所述第一点云地图与任一所述二类雷达点云中，按最小距离匹配原则选择多对第二匹配点对；所述第二匹配点对为(k_0,v,j，k_2,v,j)，k_0,v,j为所述第一点云地图的一类点，k_2,v,j为所述二类雷达点云中与k_0,v,j匹配的一类点，1≤v≤G，v为所述二类雷达的雷达索引，G为所述二类雷达的总数；任一所述二类雷达点云对应的所述第二匹配点对的数量与任一所述一类雷达点云对应的所述第一匹配点对的数量相同；

构建关于刚体运动旋转和平移的目标函数f，

其中，R_s、T_s为所述雷达索引s对应的所述一类雷达的刚体运动旋转矩阵和平移矩阵；R_v、T_v为所述雷达索引v对应的所述二类雷达的刚体运动旋转矩阵和平移矩阵；β为所述一类雷达的权重系数、γ为所述二类雷达的权重系数，β>γ且M*β+G*γ＝1；

基于迭代最近点算法对使所述目标函数f取得最小值的R_s、T_s、R_v、T_v进行求解得到一类雷达旋转和平移矩阵张量

和二类雷达旋转和平移矩阵张量

并根据所述一类雷达旋转和平移矩阵张量对各个所述一类雷达的所述第一点云帧序列中的所有所述第一点云帧进行旋转和平移；并根据所述二类雷达旋转和平移矩阵张量对各个所述二类雷达的所述第一点云帧序列中的所有所述第一点云帧进行旋转和平移。

本发明实施例第二方面提供了一种电子设备，包括：存储器、处理器和收发器；

所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现上述第一方面所述的方法步骤；

所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。

本发明实施例第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，当所述计算机指令被计算机执行时，使得所述计算机执行上述第一方面所述的方法的指令。

本发明实施例提供了一种多激光雷达点云配准方法、电子设备及计算机可读存储介质，在对各雷达点云进行一系列去干扰、降采样、分类、过滤动态点处理之后，以主雷达为基准对各个辅助雷达(一类雷达、二类雷达)的点云进行配准。通过本发明，在点云拼接前减少了因位姿偏差造成的点云错位和分层等问题，提高了点云拼接准确度。

附图说明

图1为本发明实施例一提供的一种多激光雷达点云配准方法示意图；

图2为本发明实施例二提供的一种电子设备的结构示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明实施例一提供一种多激光雷达点云配准方法，如图1为本发明实施例一提供的一种多激光雷达点云配准方法示意图所示，本方法主要包括如下步骤：

步骤1，获取多个第一激光雷达对应的多个第一点云帧序列；

其中，各个第一激光雷达对应一个雷达类型，雷达类型包括主雷达类型、一类雷达类型和二类雷达类型；各个第一激光雷达安装位置的水平高度保持一致；多个第一激光雷达中有且只有一个第一激光雷达的雷达类型为主雷达类型；雷达类型为一类雷达类型的第一激光雷达的扫描精度和视场角都大于雷达类型为二类雷达类型的第一激光雷达的扫描精度和视场角；第一点云帧序列与第一激光雷达一一对应；第一点云帧序列包括多个第一点云帧，且第一点云帧按时间先后排序。

这里，车辆感知系统会在车辆的一个水平位置上安装一组激光雷达也即是多个第一激光雷达用于对周围场景进行全景扫描，所以各个第一激光雷达安装位置的水平高度保持一致；常规情况下，这组激光雷达中有且只有一个是主雷达即多个第一激光雷达中有且只有一个第一激光雷达的雷达类型为主雷达类型；其他雷达都是辅助雷达，作为辅助雷达又分为两种类型：一类雷达类型和二类雷达类型；一类雷达类型对应的雷达分布在主雷达两侧，其具有扫描线数高(扫描精度高)视场角(Field of view，FOV)大等特点，主要用于对周围场景进行增强扫描；二类雷达类型对应的雷达分布在一类雷达侧面或一类雷达与主雷达之间，其扫描线数(扫描精度)和视场角都低于一类雷达，主要用于对主雷达或一类雷达的扫描盲区进行补充扫描；每个雷达经过一周期扫描都会生成多帧点云也就是第一点云帧，并按时间先后对第一点云帧排序就能得到第一点云帧序列；每个第一点云帧中都包括多个点，每个点对应一组点云数据(x，y，z，r)，其中，x、y、z为点云坐标系的X、Y、Z轴坐标值，r为反射强度值。

步骤2，对各个第一点云帧的点进行干扰点删除和降采样处理；

具体包括：步骤21，对第一点云帧的所有点进行遍历；遍历时，提取当前遍历点的点云通道数据(x，y，z，r)；并在反射强度值r超过预设的反射强度范围时将当前遍历点作为干扰点删除；

这里，反射强度范围是一个预设的系统参数由最小反射强度阈值与最大反射强度阈值构成；当前步骤实际就是对各个激光雷达的第一点云帧进行非正常干扰点过滤处理，过滤时若第一点云帧中某点的反射强度值r低于最小反射强度阈值或高于最大反射强度阈值则视为超过预设的反射强度范围，此时会将该点视为干扰点滤除；

步骤22，在完成干扰点删除的第一点云帧中，对所有点云通道数据(x，y，z，r)中的X轴最大、最小坐标值进行统计生成对应的X轴最大坐标值x_max和X轴最小坐标值x_min；并对所有点云通道数据(x，y，z，r)中的Y轴最大、最小坐标值进行统计生成对应的Y轴最大坐标值y_max和Y轴最小坐标值y_min；并对所有点云通道数据(x，y，z，r)中的Z轴最大、最小坐标值进行统计生成对应的Z轴最大坐标值z_max和Z轴最小坐标值z_min；

例如，第一点云帧A中有3个点，对应点云通道数据(x，y，z，r)的点云坐标分别为(11,12,13)、(22,23,24)、(33,34,35)，那么，得到的x_max、x_min、y_max、y_min、z_max、z_min为：

x_max＝max(11,22,33)＝33、x_min＝min(11,22,33)＝11，

y_max＝max(12,23,34)＝34、y_min＝min(12,23,34)＝12，

z_max＝max(13,24,35)＝35、z_min＝min(13,24,35)＝13；

步骤23，根据X轴最大坐标值x_max、X轴最小坐标值x_min、Y轴最大坐标值y_max、Y轴最小坐标值y_min、Z轴最小坐标值z_min和第一点云帧中各点的点云通道数据(x，y，z，r)计算各点对应的聚类特征值h；

其中，

α为预设的特征参数，特征参数α的取值为大于1的正整数；

例如，第一点云帧中A有3个点(11,12,13)、(22,23,24)、(33,34,35)，x_max＝33、x_min＝11、y_max＝34、y_min＝12、z_min＝13，那么就会得到3个不同的聚类特征值h；

步骤24，将第一点云帧中聚类特征值h相同的多个点归入到同一个第一聚类点组合；并对各个第一聚类点组合的所有点云通道数据(x，y，z，r)按数据维度进行均值计算得到对应的采样点云通道数据(x_aver，y_aver，z_aver，r_aver)；并根据得到的所有采样点云通道数据(x_aver，y_aver，z_aver，r_aver)重构第一点云帧；

其中，重构后的第一点云帧中，点数量与采样点云通道数据(x_aver，y_aver，z_aver，r_aver)的数量一致，各点的点云通道数据分别对应一个采样点云通道数据(x_aver，y_aver，z_aver，r_aver)。

例如，由第一点云帧中B有10个点P1-P10，对应的h为(1,1,2,2,3,4,5,6,7,8),那么，点P1、P2归入第一聚类点组合1，点P3、P4归入第一聚类点组合2，点P5-P10分别归入第一聚类点组合3-8；第一聚类点组合1对应的采样点云通道数据的x_aver＝(x₁+x₂)/2、y_aver＝(y₁+y₂)/2，z_aver＝(z₁+z₂)/2、r_aver＝(r₁+r₂)/2，第一聚类点组合2对应的x_aver＝(x₃+x₄)/2、y_aver＝(y₃+y₄)/2，z_aver＝(z₃+z₄)/2、r_aver＝(r₃+r₄)/2，而第一聚类点组合3-8因为各自里面只有一个点，所以各自对应的采样点云通道数据的x_aver、y_aver、z_aver、r_aver就是组合中那个点的原始通道数据；也就是说，通过聚类将第一点云帧中点总量从原来的10个降到了8个，且降采样后8个点中的2个点分别是取原来各自对应的2个点的通道数据平均值得到的。

这里，上述步骤22-24就是对滤除了干扰点的各个第一点云帧进行降采样处理，在降采样时以特征值h为依据进行聚类，并在各个聚类中采用均值法进行平均特征提取，这样既可以达到降采样的目的，又可以保证降采样后重构的第一点云帧与原始的第一点云帧特征之间没发生特征变异。

步骤3，对各个第一点云帧的点进行分类处理；

这里，实际是对完成步骤2处理的各个第一点云帧进行地面点和一类点的识别，所谓一类点就是现实场景中垂直地面的物体被由激光雷达扫描后得到的点；

具体包括：步骤31，提取第一点云帧中各点的点云通道数据(x，y，z，r)构成点云通道数据集合；

步骤32，将点云通道数据集合中X-Y平面坐标(x，y)相同的多个点归入到同一个第二聚类点组合；并将各个第二聚类点组合中，最小Z轴坐标值对应的点作为第一参考点；

这里，实际就是将第一点云帧的点云空间中沿Z轴方向的所有最低点都当做第一参考点；

步骤33，对得到的所有第一参考点进行遍历；遍历时，将当前遍历的第一参考点作为参考点U，并提取参考点U的点云通道数据作为对应的第一点云通道数据(x_U，y_U，z_U，r_U)；根据第一点云通道数据(x_U，y_U，z_U，r_U)和预设的雷达安装高度z_C计算对应的Z轴相对间距百分比a_CU,a_CU＝|z_C-z_U|/z_C；若Z轴相对间距百分比a_CU大于或等于预设的百分比阈值则将参考点U作为第二参考点；

这里，所有第一参考点并非都是地面点，例如，在高于地面上的大平台上的点；因此，当前步骤要基于一个预设的近地间距百分比参数来对第一参考点进行一轮非地面点过滤；上述百分比阈值就是这个近地间距百分比参数，这个值一般被设为一个偏大值；

例如，已知雷达安装高度z_C为2米，百分比阈值为98％，那么要满足Z轴相对间距百分比a_CU大于或等于98％，那这参考点U对应的第一参考点的高度就很小了、需要低于4厘米；这种情况下大部分类似高于地面的大平台上的点都会被过滤掉；

步骤34，对得到的所有第二参考点进行遍历；遍历时，将当前遍历的第二参考点作为参考点P，将与参考点P直线距离最近的第二参考点作为参考点Q；提取参考点P和参考点Q的点云通道数据作为对应的第二点云通道数据(x_P，y_P，z_P，r_P)和第三点云通道数据(x_Q，y_Q，z_Q，r_Q)；并根据第二、第三点云通道数据计算参考点P和参考点Q的X-Y平面间距生成对应的平面间距d_PQ，

并根据第二、第三点云通道数据计算参考点P和参考点Q的Z轴垂直间距生成对应的垂直间距h_PQ，h_PQ＝|z_P-z_Q|；并根据垂直间距h_PQ和平面间距d_PQ计算参考点P和参考点Q间的连接线与X-Y平面的夹角θ，

若夹角θ小于预设的角度阈值则将参考点P和参考点Q都标记为地面点；

这里，所有第二参考点也并非全是地面点，例如，地面上的一些小物体；对于真实的两个地面点之间二者连线与点云坐标的X-Y平面的夹角θ是很小的、接近于0，而地面上小物体的点与周围地面点间的连线与点云坐标的X-Y平面的夹角θ就会明显高于两个地面点之间产生夹角；当前步骤就是基于这个特点，预设一个角度阈值对两点间对应的夹角θ进行比对，只有低于该阈值的才能被保留作为真正的地面点；上述角度阈值常规可被设为5°，也可根据实际需求另行设定；

步骤35，统计地面点的数量作为第一数量n；并对所有地面点的点云通道数据(x_i，y_i，z_i，r_i)按数据维度进行均值计算得到地面中心点的点云通道数据(x_o，y_o，z_o，r_o)；1≤i≤n；

这里的均值计算方式与前文的均值计算方式一致，即：

步骤36，根据所有点云通道数据(x_i，y_i，z_i，r_i)和点云通道数据(x_o，y_o，z_o，r_o)构建协方差矩阵C，

并将协方差矩阵C输入预设的主成分分析(Principal Component Analysis，PCA)计算器进行主成分分析得到三个特征向量，从中选择向量长度最短的作为地面法向量；

这里，PCA计算器就是一个实现PCA算法的计算组件，而PCA算法的具体计算方式可参考公开的技术实现，在此不做进一步赘述；通过PCA计算，可获得与协方差矩阵C对应的所有地面点的2个平面向量和1个法向向量，其中法向向量的向量长度最短；

步骤37，将第一点云帧中除地面点之外的所有点记为非地面点；并对素有非地面点进行聚合得到多组非地面点组合；并为每组非地面点组合创建对应的协方差矩阵C’；并将协方差矩阵C’输入预设的PCA计算器进行主成分分析得到三个特征向量，从中选择向量长度最短的作为对应的第一法向量；若第一法向量与地面法向量垂直，则将对应的非地面点组合中的所有非地面点标记为一类点。

这里，先对所有非地面点进行聚合，聚合的实现过程类似点云目标识别中采用X-Y平面栅格聚类方式，也就是按约定的栅格形状将X-Y平面划分成多个平面栅格再对各个X平面栅格沿Z轴聚类，即将点云坐标(x，y)落入某个栅格中的不同高度z的点云都聚为一类；为每组非地面点组合创建对应的协方差矩阵C’并将协方差矩阵C’送入PCA计算器进行计算得到第一法向量的实现方式实际与步骤35-36近似，在此不做进一步赘述；此时得到的第一法向量可视为沿Z轴方向一个平面(与Z轴平行的平面或与Z轴有一定夹角的平面)的垂直向量。倘若第一法向量与地面法向量垂直，就意味着第一法向量对应的非地面点组合对应的物体应该是与地面呈垂直关系的物体，这里本发明实施例将这类与地面呈垂直关系的物体上的点即第一法向量与地面法向量垂直的非地面点组合下的点视为一类点。由后续步骤可知，一类点在点云配准时会成为配对点的筛选标准。

步骤4，对各个第一点云帧序列的点进行动态点过滤处理；

这里，实际就是在对各个第一点云帧完成去干扰、降采样和一类点识别之后，进一步按时序关系对各个第一点云帧上属于运动对象的点进行过滤；

具体包括：步骤41，对当前第一点云帧序列中的各个第一点云帧逐个进行目标识别处理得到对应的多个目标识别框；并为所有第一点云帧中的各个目标识别框都分配一个初始化为空的目标标识；并对第一个第一点云帧中的各个目标识别框的目标标识进行顺序标识设置；

其中，目标识别框对应一组目标识别框参数；目标识别框参数包括目标标识、中心点坐标、框高度H、框宽度W和框深度L；

步骤42，在当前第一点云帧序列的相邻两帧第一点云帧中，将时间靠前的作为上一帧、时间靠后的作为下一帧，并将上一帧中的目标识别框记为第一识别框、将下一帧中的目标识别框记为第二识别框；并使用卡尔曼滤波器的状态方程，以各个第一识别框的中心点坐标为上一时刻状态量，对第一识别框在下一时刻的中心点坐标进行预测得到对应的预测中心点坐标；并由各个预测中心点坐标结合框高度H、框宽度W和框深度L，在下一帧中构建对应的第三识别框；并对任一对第三、第二识别框的相似度进行计算生成对应的第一相似度；并根据得到的所有第一相似度，创建能够反映所有第二、第三识别框一一对应关系的关联矩阵；并基于深度匈牙利网络(Deep Hungarian Network，DHN)对关联矩阵进行最佳匹配运算从而得到与任一第二识别框最匹配的第三识别框并记为匹配识别框；并使用各个匹配识别框对应的第一识别框的目标标识，对与当前匹配识别框对应的第二识别框的目标标识进行设置；

这里，实际就是先对各个第一点云帧序列中各个第一点云帧进行目标检测得到多个第一识别框B_f，再按时间顺序以前一帧的第一识别框为上一个状态量，基于卡尔曼滤波器的运动方程对下一帧该目标识别框的位置进行预测从而得到下一帧的预测量也就是第三识别框

再将下一帧本身识别出的各个目标识别框作为观测量也就是第二识别框B_h；对任一对第三、第二识别框的相似度进行计算实际就是计算各个

与各个B_h之间的相似度并由此可以得到f*h个相似度ρ_f*h；由f*h个相似度ρ_f*h构建的一个形状为f*h的矩阵就是反映所有第二、第三识别框一一对应关系的关联矩阵；深度匈牙利网络(Deep HungarianNetwork，DHN)是一个基于改进匈牙利网络算法计算最匹配关系的神经网络，网络实现可参考公开的技术实现、在此不做一一赘述；通过深度匈牙利网络对关联矩阵的运算可以对各个

与各个B_h之间的最匹配关系进行确认，从而在相邻的两帧点云中建立起各个目标识别框之间的关联关系；一旦关联关系确认，就可通过用前帧点云中第一识别框B_f的目标标识对后帧点云中匹配的第二识别框B_h的目标标识进行设置的方式完成前后帧点云的目标数据关联；

步骤43，从当前第一点云帧序列的第一帧到最后一帧第一点云帧中，提取各个目标标识对应的目标识别框的中心点坐标构成对应的目标轨迹；

其中，目标轨迹包括多个轨迹点，每个轨迹点的坐标对应一个中心点坐标；

步骤44，从各个目标轨迹中提取第一个轨迹点的坐标作为起始坐标，提取最后一个轨迹点的坐标作为结束坐标，对从起始坐标到结束坐标的相对速度进行计算得到对应的目标运动速度；若目标运动速度高于预设的运动速度阈值则将当前目标轨迹对应的目标标识记为运动目标标识；

这里，目标运动速度的计算方式可先行计算X轴、Y轴的分量得到对应的V_x和V_y，再按目标

方式进行求解；

步骤45，将当前第一点云帧序列的各个第一点云帧中，目标标识为运动目标标识的目标识别框内的点记为动态点，并将所所有动态点删除。

步骤5，以雷达类型为主雷达类型的第一激光雷达的第一点云帧序列为参考对其他类型的第一激光雷达的第一点云帧序列进行点云配准处理；

具体包括：步骤51，将雷达类型为主雷达类型的第一激光雷达记为主雷达；并将雷达类型为一类雷达类型的第一激光雷达记为一类雷达；并将雷达类型为二类雷达类型的第一激光雷达记为二类雷达；

步骤52，将主雷达对应的第一点云帧序列的所有第一点云帧的所有点都提取出来聚合成对应的主雷达点云；并将各个一类雷达对应的第一点云帧序列的所有第一点云帧的所有点都提取出来聚合成对应的一类雷达点云；并将各个二类雷达对应的第一点云帧序列的所有第一点云帧的所有点都提取出来聚合成对应的二类雷达点云；

步骤53，基于任一一类、二类雷达与主雷达的坐标对应关系，将对应的一类、二类雷达点云中各点的点云通道数据(x，y，z，r)的点云坐标(x，y，z)统一转换为主雷达对应的点云坐标系的点云坐标；

这里，实际就是以主雷达点云为基准将所有一类、二类雷达的点云都转到与主雷达点云一致的坐标系下；

步骤54，以主雷达为中心，根据主雷达点云进行点云地图构建生成对应的第一点云地图；并在第一点云地图中对主雷达点云的各个地面点和一类点进行标记；

这里，以主雷达点云为基准构建点云地图也即是第一点云地图，第一点云地图将作为后续步骤配准的参考；

步骤55，在第一点云地图与任一一类雷达点云中，按最小距离匹配原则选择多对第一匹配点对；

其中，第一匹配点对为(k_0,s,j，k_1,s,j)，k_0,s,j为第一点云地图的一类点，k_1,s,j为一类雷达点云中与k_0,s,j匹配的一类点，1≤s≤M，1≤j≤E，s为一类雷达的雷达索引，M为一类雷达的总数，j为第一匹配点对的点对索引，E为第一匹配点对的总数；

这里，各雷达点云彼此之间存在重合点，本发明实施例按最小距离匹配原则从主雷达点云和一类雷达点云中选出匹配点对即第一匹配点对用于后续计算；

步骤56，在第一点云地图与任一二类雷达点云中，按最小距离匹配原则选择多对第二匹配点对；

其中，第二匹配点对为(k_0,v,j，k_2,v,j)，k_0,v,j为第一点云地图的一类点，k_2,v,j为二类雷达点云中与k_0,v,j匹配的一类点，1≤v≤G，v为二类雷达的雷达索引，G为二类雷达的总数；任一二类雷达点云对应的第二匹配点对的数量与任一一类雷达点云对应的第一匹配点对的数量相同；

这里同理，本发明实施例按最小距离匹配原则从主雷达点云和二类雷达点云中选出匹配点对即第二匹配点对用于后续计算；

步骤57，构建关于刚体运动旋转和平移的目标函数f，

其中，R_s、T_s为雷达索引s对应的一类雷达的刚体运动旋转矩阵和平移矩阵；R_v、T_v为雷达索引v对应的二类雷达的刚体运动旋转矩阵和平移矩阵；β为一类雷达的权重系数、γ为二类雷达的权重系数，β>γ且M*β+G*γ＝1；

这里，基于欧式变换构建对应的目标函数；

步骤58，基于迭代最近点(Iterative Closest Point，ICP)算法对使目标函数f取得最小值的R_s、T_s、R_v、T_v进行求解得到一类雷达旋转和平移矩阵张量

和二类雷达旋转和平移矩阵张量

并根据一类雷达旋转和平移矩阵张量对各个一类雷达的第一点云帧序列中的所有第一点云帧进行旋转和平移；并根据二类雷达旋转和平移矩阵张量对各个二类雷达的第一点云帧序列中的所有第一点云帧进行旋转和平移。

这里，ICP算法的求解实现可参考公开的实现，在此不做进一步赘述；基于ICP算法对使目标函数f取得最小值的R_s、T_s、R_v、T_v进行求解实际就是求argmin(f)_{Rs、Ts、Rv、Tv}。在得到一类、二类雷达旋转和平移矩阵张量之后，就可根据对应的雷达索引使用对应的R_s+T_s或R_v+T_v对一类、二类雷达对应的各个第一点云帧进行配准也即是进行对应的旋转和平移。

图2为本发明实施例二提供的一种电子设备的结构示意图。该电子设备可以为前述的终端设备或者服务器，也可以为与前述终端设备或者服务器连接的实现本发明实施例方法的终端设备或服务器。如图2所示，该电子设备可以包括：处理器301(例如CPU)、存储器302、收发器303；收发器303耦合至处理器301，处理器301控制收发器303的收发动作。存储器302中可以存储各种指令，以用于完成各种处理功能以及实现前述方法实施例描述的处理步骤。优选的，本发明实施例涉及的电子设备还包括：电源304、系统总线305以及通信端口306。系统总线305用于实现元件之间的通信连接。上述通信端口306用于电子设备与其他外设之间进行连接通信。

在图2中提到的系统总线305可以是外设部件互连标准(Peripheral ComponentInterconnect，PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，EISA)总线等。该系统总线可以分为地址总线、数据总线、控制总线等。为便于表示，图2中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。通信接口用于实现数据库访问装置与其他设备(例如客户端、读写库和只读库)之间的通信。存储器可能包含随机存取存储器(Random Access Memory，RAM)，也可能还包括非易失性存储器(Non-Volatile Memory)，例如至少一个磁盘存储器。

上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)、图形处理器(Graphics Processing Unit，GPU)等；还可以是数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(FieldProgrammable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。

需要说明的是，本发明实施例还提供一种计算机可读存储介质，该计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述实施例中提供的方法和处理过程。

本发明实施例还提供一种运行指令的芯片，该芯片用于执行前述方法实施例描述的处理步骤。

本发明实施例提供了一种多激光雷达点云配准方法、电子设备及计算机可读存储介质，在对各雷达点云进行一系列去干扰、降采样、分类、过滤动态点处理之后，以主雷达为基准对各个辅助雷达(一类雷达、二类雷达)的点云进行配准。通过本发明，在点云拼接前减少了因位姿偏差造成的点云错位和分层等问题，提高了点云拼接准确度。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种多激光雷达点云配准方法，其特征在于，所述方法包括：

获取多个第一激光雷达对应的多个第一点云帧序列；所述第一点云帧序列与所述第一激光雷达一一对应；所述第一点云帧序列包括多个第一点云帧，且所述第一点云帧按时间先后排序；

对各个所述第一点云帧的点进行干扰点删除和降采样处理；

对各个所述第一点云帧的点进行分类处理；

对各个所述第一点云帧序列的点进行动态点过滤处理；

以雷达类型为主雷达类型的所述第一激光雷达的所述第一点云帧序列为参考对其他类型的所述第一激光雷达的所述第一点云帧序列进行点云配准处理。

2.根据权利要求1所述的多激光雷达点云配准方法，其特征在于，

各个所述第一激光雷达对应一个所述雷达类型，所述雷达类型包括主雷达类型、一类雷达类型和二类雷达类型；各个所述第一激光雷达安装位置的水平高度保持一致；所述多个第一激光雷达中有且只有一个所述第一激光雷达的所述雷达类型为主雷达类型；所述雷达类型为一类雷达类型的所述第一激光雷达的扫描精度和视场角都大于所述雷达类型为二类雷达类型的所述第一激光雷达的扫描精度和视场角。

3.根据权利要求1所述的多激光雷达点云配准方法，其特征在于，所述对各个所述第一点云帧的点进行干扰点删除和降采样处理，具体包括：

对所述第一点云帧的所有点进行遍历；遍历时，提取当前遍历点的点云通道数据(x，y，z，r)；并在反射强度值r超过预设的反射强度范围时将所述当前遍历点作为干扰点删除；x、y、z为点云坐标系的X、Y、Z轴坐标值，r为所述反射强度值；

在完成干扰点删除的所述第一点云帧中，对所有所述点云通道数据(x，y，z，r)中的X轴最大、最小坐标值进行统计生成对应的X轴最大坐标值x_max和X轴最小坐标值x_min；并对所有所述点云通道数据(x，y，z，r)中的Y轴最大、最小坐标值进行统计生成对应的Y轴最大坐标值y_max和Y轴最小坐标值y_min；并对所有所述点云通道数据(x，y，z，r)中的Z轴最小坐标值进行统计生成对应的Z轴最小坐标值z_min；

根据所述X轴最大坐标值x_max、所述X轴最小坐标值x_min、所述Y轴最大坐标值y_max、所述Y轴最小坐标值y_min、所述Z轴最小坐标值z_min和所述第一点云帧中各点的所述点云通道数据(x，y，z，r)计算各点对应的聚类特征值h,

α为预设的特征参数，所述特征参数α的取值为大于1的正整数；

将所述第一点云帧中所述聚类特征值h相同的多个点归入到同一个第一聚类点组合；并对各个所述第一聚类点组合的所有所述点云通道数据(x，y，z，r)按数据维度进行均值计算得到对应的采样点云通道数据(x_aver，y_aver，z_aver，r_aver)；并根据得到的所有所述采样点云通道数据(x_aver，y_aver，z_aver，r_aver)重构所述第一点云帧；重构后的所述第一点云帧中，点数量与所述采样点云通道数据(x_aver，y_aver，z_aver，r_aver)的数量一致，各点的点云通道数据分别对应一个所述采样点云通道数据(x_aver，y_aver，z_aver，r_aver)。

4.根据权利要求1所述的多激光雷达点云配准方法，其特征在于，所述对各个所述第一点云帧的点进行分类处理，具体包括：

提取所述第一点云帧中各点的点云通道数据(x，y，z，r)构成点云通道数据集合；x、y、z为点云坐标系的X、Y、Z轴坐标值，r为所述反射强度值；

将所述点云通道数据集合中X-Y平面坐标(x，y)相同的多个点归入到同一个第二聚类点组合；并将各个所述第二聚类点组合中，最小Z轴坐标值对应的点作为第一参考点；

对得到的所有所述第一参考点进行遍历；遍历时，将当前遍历的所述第一参考点作为参考点U，并提取所述参考点U的所述点云通道数据作为对应的第一点云通道数据(x_U，y_U，z_U，r_U)；根据第一点云通道数据(x_U，y_U，z_U，r_U)和预设的雷达安装高度z_C计算对应的Z轴相对间距百分比a_CU,a_CU＝|z_C-z_U|/z_C；若所述Z轴相对间距百分比a_CU大于或等于预设的百分比阈值则将所述参考点U作为第二参考点；

对得到的所有所述第二参考点进行遍历；遍历时，将当前遍历的所述第二参考点作为参考点P，将与所述参考点P直线距离最近的所述第二参考点作为参考点Q；提取所述参考点P和所述参考点Q的所述点云通道数据作为对应的第二点云通道数据(x_P，y_P，z_P，r_P)和第三点云通道数据(x_Q，y_Q，z_Q，r_Q)；并根据所述第二、第三点云通道数据计算所述参考点P和所述参考点Q的X-Y平面间距生成对应的平面间距d_PQ，

并根据所述第二、第三点云通道数据计算所述参考点P和所述参考点Q的Z轴垂直间距生成对应的垂直间距h_PQ，h_PQ＝|z_P-z_Q|；并根据所述垂直间距h_PQ和所述平面间距d_PQ计算所述参考点P和所述参考点Q间的连接线与X-Y平面的夹角θ，

若所述夹角θ小于预设的角度阈值则将所述参考点P和所述参考点Q都标记为地面点；

统计所述地面点的数量作为第一数量n；并对所有所述地面点的点云通道数据(x_i，y_i，z_i，r_i)按数据维度进行均值计算得到地面中心点的点云通道数据(x_o，y_o，z_o，r_o)，1≤i≤n；根据所有所述点云通道数据(x_i，y_i，z_i，r_i)和所述点云通道数据(x_o，y_o，z_o，r_o)构建协方差矩阵C，

并将所述协方差矩阵C输入预设的PCA计算器进行主成分分析得到三个特征向量，从中选择向量长度最短的作为地面法向量；

将所述第一点云帧中除所述地面点之外的所有点记为非地面点；并对素有非地面点进行聚合得到多组非地面点组合；并为每组所述非地面点组合创建对应的协方差矩阵C’；并将所述协方差矩阵C’输入预设的PCA计算器进行主成分分析得到三个特征向量，从中选择向量长度最短的作为对应的第一法向量；若所述第一法向量与所述地面法向量垂直，则将对应的所述非地面点组合中的所有所述非地面点标记为一类点。

5.根据权利要求1所述的多激光雷达点云配准方法，其特征在于，所述对各个所述第一点云帧序列的点进行动态点过滤处理，具体包括：

对当前第一点云帧序列中的各个所述第一点云帧逐个进行目标识别处理得到对应的多个目标识别框；并为所有所述第一点云帧中的各个所述目标识别框都分配一个初始化为空的目标标识；并对第一个所述第一点云帧中的各个所述目标识别框的所述目标标识进行顺序标识设置；所述目标识别框对应一组目标识别框参数；所述目标识别框参数包括所述目标标识、中心点坐标、框高度H、框宽度W和框深度L；

在所述当前第一点云帧序列的相邻两帧所述第一点云帧中，将时间靠前的作为上一帧、时间靠后的作为下一帧，并将所述上一帧中的所述目标识别框记为第一识别框、将所述下一帧中的所述目标识别框记为第二识别框；并使用卡尔曼滤波器的状态方程，以各个所述第一识别框的中心点坐标为上一时刻状态量，对所述第一识别框在下一时刻的中心点坐标进行预测得到对应的预测中心点坐标；并由各个所述预测中心点坐标结合所述框高度H、所述框宽度W和所述框深度L，在所述下一帧中构建对应的第三识别框；并对任一对所述第三、第二识别框的相似度进行计算生成对应的第一相似度；并根据得到的所有所述第一相似度，创建能够反映所有所述第二、第三识别框一一对应关系的关联矩阵；并基于深度匈牙利网络DHN对所述关联矩阵进行最佳匹配运算从而得到与任一所述第二识别框最匹配的所述第三识别框并记为匹配识别框；并使用各个所述匹配识别框对应的所述第一识别框的所述目标标识，对与当前匹配识别框对应的所述第二识别框的所述目标标识进行设置；

从所述当前第一点云帧序列的第一帧到最后一帧所述第一点云帧中，提取各个所述目标标识对应的所述目标识别框的所述中心点坐标构成对应的目标轨迹；所述目标轨迹包括多个轨迹点，每个轨迹点的坐标对应一个所述中心点坐标；

从各个所述目标轨迹中提取第一个所述轨迹点的坐标作为起始坐标，提取最后一个所述轨迹点的坐标作为结束坐标，对从所述起始坐标到所述结束坐标的相对速度进行计算得到对应的目标运动速度；若所述目标运动速度高于预设的运动速度阈值则将当前目标轨迹对应的所述目标标识记为运动目标标识；

将所述当前第一点云帧序列的各个所述第一点云帧中，所述目标标识为运动目标标识的所述目标识别框内的点记为动态点，并将所所有所述动态点删除。

6.根据权利要求2所述的多激光雷达点云配准方法，其特征在于，所述以雷达类型为主雷达类型的所述第一激光雷达的所述第一点云帧序列为参考对其他类型的所述第一激光雷达的所述第一点云帧序列进行点云配准处理，具体包括：

将所述雷达类型为主雷达类型的所述第一激光雷达记为主雷达；并将所述雷达类型为一类雷达类型的所述第一激光雷达记为一类雷达；并将所述雷达类型为二类雷达类型的所述第一激光雷达记为二类雷达；

将所述主雷达对应的所述第一点云帧序列的所有所述第一点云帧的所有点都提取出来聚合成对应的主雷达点云；并将各个所述一类雷达对应的所述第一点云帧序列的所有所述第一点云帧的所有点都提取出来聚合成对应的一类雷达点云；并将各个所述二类雷达对应的所述第一点云帧序列的所有所述第一点云帧的所有点都提取出来聚合成对应的二类雷达点云；

基于任一所述一类、二类雷达与所述主雷达的坐标对应关系，将对应的所述一类、二类雷达点云中各点的点云通道数据(x，y，z，r)的点云坐标(x，y，z)统一转换为所述主雷达对应的点云坐标系的点云坐标；

以所述主雷达为中心，根据所述主雷达点云进行点云地图构建生成对应的第一点云地图；并在所述第一点云地图中对所述主雷达点云的各个地面点和一类点进行标记；

在所述第一点云地图与任一所述一类雷达点云中，按最小距离匹配原则选择多对第一匹配点对；所述第一匹配点对为(k_0,s,j，k_1,s,j)，k_0,s,j为所述第一点云地图的一类点，k_1,s,j为所述一类雷达点云中与k_0,s,j匹配的一类点，1≤s≤M，1≤j≤E，s为所述一类雷达的雷达索引，M为所述一类雷达的总数，j为所述第一匹配点对的点对索引，E为所述第一匹配点对的总数；

在所述第一点云地图与任一所述二类雷达点云中，按最小距离匹配原则选择多对第二匹配点对；所述第二匹配点对为(k_0,v,j，k_2,v,j)，k_0,v,j为所述第一点云地图的一类点，k_2,v,j为所述二类雷达点云中与k_0,v,j匹配的一类点，1≤v≤G，v为所述二类雷达的雷达索引，G为所述二类雷达的总数；任一所述二类雷达点云对应的所述第二匹配点对的数量与任一所述一类雷达点云对应的所述第一匹配点对的数量相同；

构建关于刚体运动旋转和平移的目标函数f，

其中，R_s、T_s为所述雷达索引s对应的所述一类雷达的刚体运动旋转矩阵和平移矩阵；R_v、T_v为所述雷达索引v对应的所述二类雷达的刚体运动旋转矩阵和平移矩阵；β为所述一类雷达的权重系数、γ为所述二类雷达的权重系数，β>γ且M*β+G*γ＝1；

基于迭代最近点算法对使所述目标函数f取得最小值的R_s、T_s、R_v、T_v进行求解得到一类雷达旋转和平移矩阵张量

和二类雷达旋转和平移矩阵张量

并根据所述一类雷达旋转和平移矩阵张量对各个所述一类雷达的所述第一点云帧序列中的所有所述第一点云帧进行旋转和平移；并根据所述二类雷达旋转和平移矩阵张量对各个所述二类雷达的所述第一点云帧序列中的所有所述第一点云帧进行旋转和平移。

7.一种电子设备，其特征在于，包括：存储器、处理器和收发器；

所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现权利要求1-6任一项所述的方法步骤；

所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。

8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，当所述计算机指令被计算机执行时，使得所述计算机执行权利要求1-6任一项所述的方法的指令。

Images