CN111696159B - 激光里程计的特征存储方法、电子设备和存储介质 - Google Patents

Abstract

本发明提供了一种激光里程计的特征存储方法、电子设备和计算机可读存储介质。该方法包括：接收激光里程计的输入数据，输入数据至少包括时间连续的各帧激光点云；对各帧激光点云进行特征提取得到点云特征，点云特征包括多个激光特征点在直角坐标系下的直角坐标值；根据各激光特征点的直角坐标值计算各激光特征点的极坐标值；根据预设长度分辨率和预设角度分辨率对极坐标值进行离散化得到各激光特征点的离散极坐标值，从而得到包括多个激光特征点的离散极坐标值的转换后点云特征；将各帧激光点云的转换后点云特征作为激光里程计输出的各帧激光特征的至少一部分进行存储。本发明的方案存储更灵活，且大大减少激光特征存储空间的消耗。

Description

激光里程计的特征存储方法、电子设备和存储介质

技术领域

本发明涉及自动驾驶技术领域，特别是一种激光里程计的特征存储方法、电子设备和计算机可读存储介质。

背景技术

在自动驾驶领域，为了对车辆进行定位，需要构造环境地图。目前主要通过激光SLAM(Simultaneous Localization and Mapping，同步定位与建图)来构造环境地图。激光SLAM是一种基于激光雷达的实时定位和建图算法，分为前端和后端。前端称为激光里程计，用于估计两帧激光点云间的位姿。后端称为非线性优化，用于减少前端帧间配准的累积误差。前端输出的点云特征存储后用于后端进行非线性优化。

现有技术中，前端输出的点云特征主要有以下两种存储方式：第一种，基于UDP(User Datagram Protocol，用户数据报协议)包的格式存储。这种存储方式一般用于原始激光点云的存储，在存储时可以按照激光点云的扫描面和扫描线的配置信息有序存储。由于扫描面和扫描线的配置信息保存在激光雷达的配置文件中，而每种激光雷达的配置文件并不相同，因此，基于UDP包的格式存储需依赖相应的配置文件，使得这种存储方式不够灵活。并且，在激光里程计输出的点云特征不再是原始点云的情况下，这些输出的点云特征已失去了原始点云的结构，并不适于采用UDP包格式存储。第二种，基于PCD(Point CloudData，点云数据)的格式存储。PCD是一种点云存储格式，然而，在点云特征点数量众多的情况下，即使采用二进制的PCD格式保存点云特征，存储空间的消耗仍然很大。因此，亟需一种存储更灵活、节省存储空间的点云特征的存储方案。

发明内容

鉴于上述问题，提出了本发明以便提供一种克服上述问题或者至少部分地解决上述问题的激光里程计的特征存储方法、电子设备和计算机可读存储介质。

本发明的一个目的是提供一种存储更灵活、节省存储空间的激光里程计的特征存储方法。

特别地，根据本发明实施例的一方面，提供了一种激光里程计的特征存储方法，包括：

接收激光里程计的输入数据，所述输入数据至少包括时间连续的各帧激光点云；

对各帧激光点云进行特征提取得到点云特征，所述点云特征包括多个激光特征点在直角坐标系下的直角坐标值；

根据各所述激光特征点的直角坐标值计算各所述激光特征点的极坐标值；

根据预设长度分辨率和预设角度分辨率对所述极坐标值进行离散化得到各所述激光特征点的离散极坐标值，从而得到包括多个所述激光特征点的离散极坐标值的转换后点云特征；

将各帧激光点云的所述转换后点云特征作为所述激光里程计输出的各帧激光特征的至少一部分进行存储。

可选地，所述直角坐标值包括x轴坐标、y轴坐标和z轴坐标；

所述极坐标值包括长度值、垂直角和水平角；

所述根据各所述激光特征点的直角坐标值计算各所述激光特征点的极坐标值，包括：

根据各所述激光特征点的x轴坐标、y轴坐标和z轴坐标，按照下式(1)、(2)和(3)，计算各所述激光特征点的长度值l、垂直角β和水平角α：

其中，x、y和z分别表示各所述激光特征点的x轴坐标、y轴坐标和z轴坐标。

可选地，所述根据预设长度分辨率和预设角度分辨率对所述极坐标值进行离散化得到各所述激光特征点的离散极坐标值，包括：

将各所述激光特征点的长度值l与所述预设长度分辨率相除，并对相除得到的商进行取整，得到各所述激光特征点的离散长度值l_sampling；

将各所述激光特征点的垂直角β和水平角α分别与所述预设角度分辨率相除，并对相除得到的商进行取整，得到各所述激光特征点的离散垂直角β_sampling和离散水平角α_sampling。

可选地，在将各帧激光点云的所述转换后点云特征作为所述激光里程计输出的各帧激光特征的至少一部分进行存储之后，所述特征存储方法还包括：

读取所存储的各帧激光特征，根据各所述激光特征点的离散极坐标值、所述预设长度分辨率和所述预设角度分辨率，反向计算得到各所述激光特征点的直角坐标值，以用于对所述激光里程计输出的各帧激光特征进行非线性优化。

可选地，所述对各帧激光点云进行特征提取得到点云特征，包括：

使用指定边长的三维网格对所述各帧激光点云进行降采样；

将降采样得到的点云作为所述点云特征。

可选地，所述输入数据还包括惯性测量单元的测量数据、车轮转速和卫星定位数据；

在对各帧激光点云进行特征提取得到点云特征之后，所述特征存储方法还包括：

以所述输入数据中的时间上的首帧激光点云作为第一帧激光点云，并以所述首帧激光点云之后的各帧激光点云依次作为当前帧激光点云；

根据当前帧和前一相邻帧的激光点云、前一相邻帧激光点云的起始时刻与当前帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，计算得到当前帧激光点云相对于第一帧激光点云的位姿变化；

确定与当前帧激光点云的时间最近的卫星定位数据作为当前帧激光点云的绑定卫星定位数据；

所述将各帧激光点云的所述转换后点云特征作为所述激光里程计输出的各帧激光特征的至少一部分进行存储，包括：

将当前帧激光点云的所述转换后点云特征、当前帧激光点云相对于第一帧激光点云的位姿变化和当前帧激光点云的所述绑定卫星定位数据作为所述激光里程计输出的当前帧激光特征的至少一部分进行存储。

可选地，所述根据当前帧和前一相邻帧的激光点云、当前帧和前一相邻帧的惯性测量单元的测量数据以及当前帧和前一相邻帧的车轮转速以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，计算得到当前帧激光点云相对于第一帧激光点云的位姿变化，包括：

当当前帧激光点云为第二帧激光点云时，根据当前帧激光点云的起始时刻与第一帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速，基于第一帧激光点云的位姿预测当前帧激光点云的位姿；

以所预测的当前帧激光点云的位姿作为初值，通过通用迭代最近邻算法计算当前帧激光点云相对于第一帧激光点云的位姿变化；

当当前帧激光点云为所述第二帧激光点云后的其他帧激光点云时，根据当前帧激光点云的起始时刻与前一相邻帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速，基于前一相邻帧激光点云的位姿预测当前帧激光点云的位姿；

以所预测的当前帧激光点云的位姿作为初值，通过通用迭代最近邻算法计算当前帧激光点云与前一相邻帧激光点云之间的位姿变化；

根据当前帧激光点云与前一相邻帧激光点云之间的位姿变化以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，得到当前帧激光点云相对于第一帧激光点云的位姿变化。

可选地，所述各帧激光点云为剔除了移动物体点云的激光点云。

根据本发明实施例的另一方面，还提供了一种电子设备，包括：

处理器；以及

存储有计算机程序代码的存储器；

当所述计算机程序代码被所述处理器运行时，导致所述电子设备执行根据前文任一项所述的激光里程计的特征存储方法。

根据本发明实施例的再一方面，还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序代码，当所述计算机程序代码在电子设备上运行时，导致所述电子设备执行根据前文任一项所述的激光里程计的特征存储方法。

本发明实施例提出的激光里程计的特征存储方法中，将从各帧激光点云提取的点云特征中的各激光特征点的直角坐标值转化为极坐标值，并将得到的各激光特征点的极坐标值离散化得到各激光特征点的离散极坐标值，以作为激光里程计输出的各帧激光特征的至少一部分进行存储。由于离散极坐标值可以采用短整型数据类型，与采用浮点数据类型的直角坐标值相比，采用离散极坐标值表示各激光特征点，能够显著减少各激光特征点占用的字节长度，特别在激光特征点数量巨大的情况下，将大大减少激光里程计输出的激光特征所占用的存储空间。并且，本发明的特征存储方法不依赖于激光雷达的配置文件，存储更灵活。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。

附图说明

通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：

图1示出了根据本发明一实施例的激光里程计的特征存储方法的流程示意图；

图2示出了根据本发明一实施例的激光特征点的直角坐标值和极坐标值的示意图；

图3示出了根据本发明一实施例的反向计算激光特征点的直角坐标值后恢复的激光点云的示意图；

图4示出了根据本发明一实施例的当前帧激光特征的存储格式的示意图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

为解决上述技术问题，本发明实施例提出一种激光里程计的特征存储方法。

图1示出了根据本发明一实施例的激光里程计的特征存储方法的流程示意图。参见图1，该方法至少可以包括以下步骤S102至步骤S110。

步骤S102，接收激光里程计的输入数据，输入数据至少包括时间连续的各帧激光点云。

步骤S104，对各帧激光点云进行特征提取得到点云特征，点云特征包括多个激光特征点在直角坐标系下的直角坐标值。

步骤S106，根据各激光特征点的直角坐标值计算各激光特征点的极坐标值。

步骤S108，根据预设长度分辨率和预设角度分辨率对极坐标值进行离散化得到各激光特征点的离散极坐标值，从而得到包括多个激光特征点的离散极坐标值的转换后点云特征。

步骤S110，将各帧激光点云的转换后点云特征作为激光里程计输出的各帧激光特征的至少一部分进行存储。

本发明实施例提出的激光里程计的特征存储方法中，将从各帧激光点云提取的点云特征中的各激光特征点的直角坐标值转化为极坐标值，并将得到的各激光特征点的极坐标值离散化得到各激光特征点的离散极坐标值，以作为激光里程计输出的各帧激光特征的至少一部分进行存储。由于离散极坐标值可以采用短整型数据类型，与采用浮点数据类型的直角坐标值相比，采用离散极坐标值表示各激光特征点，能够显著减少各激光特征点占用的字节长度，特别在激光特征点数量巨大的情况下，将大大减少激光里程计输出的激光特征所占用的存储空间。并且，本发明的特征存储方法不依赖于激光雷达的配置文件，存储更灵活。

上文步骤S102中，所输入的激光点云的帧频率可以根据实际应用需求进行设置，例如，激光点云的帧频率可以是10Hz。

在一个实施例中，所输入的各帧激光点云可以是剔除了移动物体点云的激光点云。此处提及的移动物体可以包括行人、行驶车辆等。由于移动物体点云的特征点是不稳定的，会影响位姿配准的精度，因此，通过剔除移动物体点云，能够提高位姿变化计算的精准度。

上文步骤S104中，对各帧激光点云进行特征提取得到点云特征。具体地，可以使用指定边长的三维网格对各帧激光点云进行降采样，将降采样得到的点云作为各帧激光点云的点云特征。三维网格的指定边长可以根据实际应用需求进行设置，例如，可以设置为0.5m。

提取得到的各帧激光点云的点云特征包括多个激光特征点，各激光特征点由直角坐标系下的直角坐标值和强度值表示。此处的直角坐标值具体包括x轴坐标、y轴坐标和z轴坐标。

上文步骤S106中，根据各激光特征点的直角坐标值计算各激光特征点的极坐标值。

图2示出了根据本发明一实施例的激光特征点的直角坐标值和极坐标值的示意图。参见图2所示，激光特征点的极坐标值包括长度值l、垂直角β和水平角α，其中，长度值l为激光特征点P相对于直角坐标系的原点O的距离，垂直角β为激光特征点P和原点O的连线与直角坐标系的XOY平面的夹角，Q为激光特征点P在XOY平面的投影点，水平角α为激光特征点P在XOY平面的投影点Q和原点O之间的连线与X轴正方向的夹角。

具体地，可以根据各激光特征点的x轴坐标、y轴坐标和z轴坐标，按照以下的式(1)、(2)和(3)，计算各激光特征点的长度值l、垂直角β和水平角α：

在式(1)、(2)和(3)，x、y和z分别表示各激光特征点的x轴坐标、y轴坐标和z轴坐标。长度值l的单位与x轴坐标、y轴坐标和z轴坐标的单位相同，一般可设为米(m)。垂直角β和水平角α的单位设为度(°)，数值在-180°至180°范围内。

上文步骤S108中，根据预设长度分辨率和预设角度分辨率进一步对各激光特征点的极坐标值进行离散化，得到各激光特征点的离散极坐标值。

具体地，可以将各激光特征点的长度值l与预设长度分辨率相除，并对相除得到的商进行取整，得到各激光特征点的离散长度值l_sampling，如下面的式(4)所示：

在式(4)中，l_sampling表示激光特征点的离散长度值；l表示激光特征点的长度值；l_r表示预设长度分辨率，单位与长度值l相同。

并且，可以将各激光特征点的垂直角β和水平角α分别与预设角度分辨率相除，并对相除得到的商进行取整，得到各激光特征点的离散垂直角β_sampling和离散水平角α_sampling，如下面的式(5)和(6)所示：

在式(5)和(6)中，β_sampling和α_sampling分别表示激光特征点的离散垂直角和离散水平角；β和α分别表示激光特征点的垂直角和水平角；a_r表示预设角度分辨率，单位为度。

本发明的实施例中，离散长度值l_sampling可采用无符号短整型(unsignedshort)数据表示，即，离散长度值l_sampling可以0至65535范围内的整数表示。离散垂直角β_sampling和离散水平角α_sampling可采用短整型(short)数据表示，即，离散垂直角β_sampling和离散水平角α_sampling可以-32768至32767范围内的整数表示。在实际应用中，可根据激光特征点的最大长度值和误差要求设置该预设长度分辨率和预设角度分辨率。预设长度分辨率的值越大，可表示的最大长度值越大，离散长度值的表示误差也更大。类似地，预设角度分辨率越大，则离散垂直角和离散水平角的表示误差越大。举例来说，若激光特征点的最大长度值为130m左右，则为了保证所有的激光特征点的离散长度值可以无符号短整型数据表示，可以将预设长度分辨率设置为0.002m，同时，为了保证所有的激光特征点的离散垂直角和离散水平角均可以短整型数据表示，可以将预设角度分辨率设置为0.006度，如此，可以用无符号短整型的离散长度值以及短整型的离散垂直角和离散水平角表示出所有长度值小于或等于130m、任意朝向的激光特征点，且表示误差达到毫米级。当然，在激光特征点的最大长度值为超过130m的更大长度值的情况下，可将预设长度分辨率设置为比0.002m更大的值，但相应的表示误差也会增大。

在上文步骤S110中，将各帧激光点云的转换后点云特征作为激光里程计输出的各帧激光特征的至少一部分进行存储。

如前文所述，转换后点云特征中各激光特征点的离散极坐标值包括无符号短整型的离散长度值l_sampling、短整型的离散垂直角β_sampling和短整型的离散水平角α_sampling。另外，各激光特征点还包括强度值，强度值可以无符号字符型(unsigned char)数据表示。由于无符号短整型数据占用2个字节，短整型数据占用2个字节，无符号字符型数据占用1个字节，因此，采用离散极坐标值和强度值表示各激光特征点后，存储激光里程计输出的各帧激光特征时，每个激光特征点仅占用7个字节。而在二进制的PCD格式存储中，各激光特征点由x轴坐标、y轴坐标、z轴坐标和强度值表示，其中，x轴坐标、y轴坐标和z轴坐标均为单精度浮点型(float)数据，每个数据占用4个字节，因此每个激光特征点需占用13个字节。可见，与二进制的PCD格式存储方式相比，本发明的方案能够将每个激光特征点占用的存储空间从13个字节压缩到7个字节。而在毎帧激光特征中，激光特征点的数量通常能达到数万个(例如，对于帧频率为10Hz的激光点云，毎帧激光点云可具有5万个激光特征点)，由此，本发明的方案可大大减少激光里程计输出的激光特征的存储空间的消耗。特别地，与二进制的PCD格式存储方式相比，可节省将近一半的存储空间。

在存储激光里程计输出的各帧激光特征后，可以读取所存储的各帧激光特征提供给后端，以便后端借助回环检测等技术进行非线性优化，从而减少前端帧间配准的累积误差。在一个实施例中，在读取所存储的各帧激光特征后，可以反向恢复各激光特征点的直角坐标值，以用于后端对激光里程计输出的各帧激光特征进行非线性优化。

具体地，通过以下方式反向恢复各激光特征点的直角坐标值：根据各激光特征点的离散极坐标值、预设长度分辨率和预设角度分辨率，反向计算得到各激光特征点的直角坐标值，计算式如下面的式(7)至式(10)所示：

z＝l_sampling×l_r×sin((β_sampling×a_r×π)/180.0) (7)

Lxy＝l_sampling×l_r×cos((β_sampling×a_r×π)/180.0) (8)

x＝Lxy×cos((α_sampling×a_r×π)/180.0) (9)

y＝Lxy×sin((α_sampling×a_r×π)/180.0) (10)

在式(7)至式(10)中，x、y和z分别表示激光特征点的x轴坐标、y轴坐标和z轴坐标；l_sampling、β_sampling和α_sampling分别表示激光特征点的离散长度值、离散垂直角和离散水平角；Lxy表示激光特征点P在XOY平面的投影点Q与原点O之间的连线的长度；l_r表示预设长度分辨率；；a_r表示预设角度分辨率。

图3示出了根据本发明一实施例的反向计算激光特征点的直角坐标值后恢复的激光点云的示意图。由图3可见，本发明中在将各激光特征点的直角坐标值转化为离散极坐标值后，再反向计算各激光特征点的直角坐标值得到的恢复的激光点云的轮廓清晰。由此可见，本发明的激光里程计的特征存储方法可在显著节省激光特征的存储空间的同时，仍然保证激光点云特征的精度。

在一个实施例中，步骤S102中激光里程计的输入数据还可以包括惯性测量单元(Inertial measurement unit，IMU)的测量数据、车轮转速和卫星定位数据。

惯性测量单元是测量物体三轴姿态角及加速度的装置，一般包括三轴陀螺仪、三轴加速度计、三轴磁力计等。惯性测量单元的测量数据可包括三轴的加速度和三轴的角速度。惯性测量单元的测量数据和车轮转速的采集频率可与激光点云的帧频率相同或不同。一般而言，惯性测量单元的测量数据和车轮转速的采集频率可大于激光点云的帧频率，例如，对于帧频率10Hz的激光点云，惯性测量单元的测量数据和车轮转速的采集频率可以为50Hz。

卫星定位数据是通过卫星定位系统获得的车辆定位数据，可包括经度、纬度、高度、经度方程、纬度方程、高度方程、定位所用的卫星数量等。卫星定位系统例如可以是全球定位系统(Global Positioning System，GPS)、北斗卫星导航系统(BeiDou NavigationSatellite System，BDS)等。卫星定位数据的采集频率可与激光点云的帧频率相同或不同。一般而言，卫星定位数据的采集频率可低于激光点云的帧频率，例如，对于帧频率10Hz的激光点云，卫星定位数据的采集频率可以为1Hz。

相应地，在输入各帧激光点云、惯性测量单元的测量数据、车轮转速和卫星定位数据，并对各帧激光点云进行特征提取得到点云特征之后，还可以执行以下步骤：

以输入数据中的时间上的首帧激光点云作为第一帧激光点云，并以首帧激光点云之后的各帧激光点云依次作为当前帧激光点云。根据当前帧和前一相邻帧的激光点云、前一相邻帧激光点云的起始时刻与当前帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速、以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，计算得到当前帧激光点云相对于第一帧激光点云的位姿变化。并且，确定与当前帧激光点云的时间最近的卫星定位数据作为当前帧激光点云的绑定卫星定位数据。需要说明的是，计算得到当前帧激光点云相对于第一帧激光点云的位姿变化的步骤与确定当前帧激光点云的绑定卫星定位数据的步骤的顺序是可以互换的，或者可以同时进行。

进一步地，根据当前帧和前一相邻帧的激光点云、前一相邻帧激光点云的起始时刻与当前帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速、以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，计算得到当前帧激光点云相对于第一帧激光点云的位姿变化的步骤可以如下实施：

当当前帧激光点云为第二帧激光点云(即首帧激光点云的后一帧激光点云)时，根据当前帧激光点云的起始时刻与第一帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速，基于第一帧激光点云的位姿预测当前帧激光点云的位姿。然后，以所预测的当前帧激光点云的位姿作为初值，通过通用迭代最近邻算法(Generalized IterativeClosest Point，GICP)计算当前帧激光点云相对于第一帧激光点云的位姿变化。

当当前帧激光点云为第二帧激光点云后的其他帧激光点云时，根据当前帧激光点云的起始时刻与前一相邻帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速，基于前一相邻帧激光点云的位姿预测当前帧激光点云的位姿。然后，以所预测的当前帧激光点云的位姿作为初值，通过通用迭代最近邻算法计算当前帧激光点云与前一相邻帧激光点云之间的位姿变化。最后，根据当前帧激光点云与前一相邻帧激光点云之间的位姿变化以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，得到当前帧激光点云相对于第一帧激光点云的位姿变化。

举例来说，当当前帧为第三帧激光点云时，根据第三帧激光点云的起始时刻与第二帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速，基于第二帧激光点云的位姿预测第三帧激光点云的位姿。以所预测的第三帧激光点云的位姿作为初值，通过通用迭代最近邻算法计算第三帧激光点云与第二帧激光点云之间的位姿变化。进而，根据第三帧激光点云与第二帧激光点云之间的位姿变化以及已计算得到的第二帧激光点云相对于第一帧激光点云的位姿变化，得到第三帧激光点云相对于第一帧激光点云的位姿变化。以此类推，能够计算得到第二帧激光点云后的各帧激光点云相对于第一帧激光点云的位姿变化。本发明的实施例中，位姿变化以位姿变换矩阵表示。

在计算得到当前帧激光点云相对于第一帧激光点云的位姿变化并确定当前帧激光点云的绑定卫星定位数据后，步骤S110可以相应地执行为：将当前帧激光点云的转换后点云特征、当前帧激光点云相对于第一帧激光点云的位姿变化和当前帧激光点云的绑定卫星定位数据作为激光里程计输出的当前帧激光特征的至少一部分进行存储。

图4示出了根据本发明一实施例的当前帧激光特征的存储格式的示意图。参见图4所示，毎帧激光特征可以包括特征头(Feature Header)、当前帧与前一相邻帧之间用于运动补偿的位姿点数据(图4中简称为帧间运动补偿位姿点)、当前帧激光点云相对于第一帧激光点云的位姿变化(图4中简称为帧位姿)、当前帧激光点云的绑定卫星定位数据(图4中简称为卫星定位数据)以及当前帧激光点云的转换后点云特征(图4中简称为点云特征)五个部分。

特征头由时间戳、预设长度分辨率和预设角度分辨率组成，其中时间戳、预设长度分辨率和预设角度分辨率均可以是双精度浮点型(double)数据，各自占用8个字节。

当前帧与前一相邻帧之间用于运动补偿的位姿点数据包括位姿点数量和多个用于运动补偿的位姿点。各位姿点由前一相邻帧激光点云的的起始时刻到当前帧激光点云的起始时刻之间采集的惯性测量单元的测量数据获得，每一次采集的惯性测量单元的测量数据对应一个位姿点。各位姿点由时间戳、姿态和位置组成。其中，位姿点数量可以为整型(int)数据，占用4个字节。各位姿点的时间戳可以为双精度浮点型数据，占用8个字节。各位姿点的姿态可以由4个双精度浮点型数据表示，共占用32个字节。各位姿点的位置可以由3个双精度浮点型数据表示，共占用24个字节。本实施例中，用于运动补偿的位姿点的数量为10个，需要说明的是，这仅是示意性的，并不限制本发明。

当前帧激光点云相对于第一帧激光点云的位姿变化包括当前帧激光点云相对于第一帧激光点云的位姿变换矩阵的元素数量、位姿置信度和位姿变化。其中，元素数量可以为整型数据，占用4个字节。位姿置信度可以为双精度浮点型数据，占用8个字节。位姿变化由位姿变换矩阵的各元素表示，本实施例中位姿变换矩阵包含12个元素，每个元素也可以是双精度浮点型，从而位姿变化共占用96个字节。

当前帧激光点云的绑定卫星定位数据可由时间戳、经度、纬度、高度、经度方程、纬度方程、高度方程和定位所用的卫星数量组成。其中，定位所用的卫星数量可以是整型数据，时间戳、经度、纬度、高度、经度方程、纬度方程和高度方程均可以是双精度浮点型数据，从而绑定卫星定位数据共占用60个字节。

当前帧激光点云的转换后点云特征包括激光特征点数量和多个激光特征点。各激光特征点由离散长度值、离散垂直角、离散水平角和强度值表示。其中，激光特征点数量可以是整型数据，占用4个字节。各激光特征点的离散长度值为无符号短整型数据，占用2个字节。各激光特征点的离散垂直角和离散水平角均为短整型数据，各自占用2个字节。各激光特征点的强度值为无符号字符型数据，占用1个字节。

在图4所示实施例的激光特征的存储格式中，除了当前帧激光点云的转换后点云特征外，其他特征数据所占的存储空间仅为固定的836字节。而在毎帧激光点云的点云特征中，激光特征点的数量通常能达到数万个，以毎帧激光点云具有5万个激光特征点为例，与二进制的PCD格式存储相比，采用本发明的方案的存储格式，可节省50000×(13-7)＝300000个字节的存储空间，大大减少激光特征所占用的存储空间。

基于同一发明构思，本发明实施例还提供了一种电子设备。该电子设备包括：

处理器；以及

存储有计算机程序代码的存储器；

当该计算机程序代码被处理器运行时，导致该电子设备执行上述任意一个实施例或其组合所述的激光里程计的特征存储方法。

基于同一发明构思，本发明实施例还提供了一种计算机可读存储介质。该计算机可读存储介质存储有计算机程序代码，当计算机程序代码在电子设备上运行时，导致该电子设备执行上述任意一个实施例或其组合所述的激光里程计的特征存储方法。

根据上述任意一个可选实施例或多个可选实施例的组合，本发明实施例能够达到如下有益效果：

本发明实施例提出的激光里程计的特征存储方法中，将从各帧激光点云提取的点云特征中的各激光特征点的直角坐标值转化为极坐标值，并将得到的各激光特征点的极坐标值离散化得到各激光特征点的离散极坐标值，以作为激光里程计输出的各帧激光特征的至少一部分进行存储。由于离散极坐标值可以采用短整型数据类型，与采用浮点数据类型的直角坐标值相比，采用离散极坐标值表示各激光特征点，能够显著减少各激光特征点占用的字节长度，特别在激光特征点数量巨大的情况下，将大大减少激光里程计输出的激光特征所占用的存储空间。并且，本发明的特征存储方法不依赖于激光雷达的配置文件，存储更灵活。

所属领域的技术人员可以清楚地了解到，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，为简洁起见，在此不另赘述。

另外，在本发明各个实施例中的各功能单元可以物理上相互独立，也可以两个或两个以上功能单元集成在一起，还可以全部功能单元都集成在一个处理单元中。上述集成的功能单元既可以采用硬件的形式实现，也可以采用软件或者固件的形式实现。

本领域普通技术人员可以理解：所述集成的功能单元如果以软件的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，其包括若干指令，用以使得一台计算设备(例如个人计算机，服务器，或者网络设备等)在运行所述指令时执行本发明各实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM)、随机存取存储器(RAM)，磁碟或者光盘等各种可以存储程序代码的介质。

或者，实现前述方法实施例的全部或部分步骤可以通过程序指令相关的硬件(诸如个人计算机，服务器，或者网络设备等的计算设备)来完成，所述程序指令可以存储于一计算机可读取存储介质中，当所述程序指令被计算设备的处理器执行时，所述计算设备执行本发明各实施例所述方法的全部或部分步骤。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：在本发明的精神和原则之内，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案脱离本发明的保护范围。

Claims (9)

1.一种激光里程计的特征存储方法，其特征在于，包括：

接收激光里程计的输入数据，所述输入数据至少包括时间连续的各帧激光点云；

对各帧激光点云进行特征提取得到点云特征，所述点云特征包括多个激光特征点在直角坐标系下的直角坐标值；

根据各所述激光特征点的直角坐标值计算各所述激光特征点的极坐标值；

根据预设长度分辨率和预设角度分辨率对所述极坐标值进行离散化得到各所述激光特征点的离散极坐标值，从而得到包括多个所述激光特征点的离散极坐标值的转换后点云特征；

将各帧激光点云的所述转换后点云特征作为所述激光里程计输出的各帧激光特征的至少一部分进行存储；

其中，所述输入数据还包括惯性测量单元的测量数据、车轮转速和卫星定位数据；

在对各帧激光点云进行特征提取得到点云特征之后，所述特征存储方法还包括：

以所述输入数据中的时间上的首帧激光点云作为第一帧激光点云，并以所述首帧激光点云之后的各帧激光点云依次作为当前帧激光点云；

根据当前帧和前一相邻帧的激光点云、前一相邻帧激光点云的起始时刻与当前帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，计算得到当前帧激光点云相对于第一帧激光点云的位姿变化；

确定与当前帧激光点云的时间最近的卫星定位数据作为当前帧激光点云的绑定卫星定位数据；

所述将各帧激光点云的所述转换后点云特征作为所述激光里程计输出的各帧激光特征的至少一部分进行存储，包括：

将当前帧激光点云的所述转换后点云特征、当前帧激光点云相对于第一帧激光点云的位姿变化和当前帧激光点云的所述绑定卫星定位数据作为所述激光里程计输出的当前帧激光特征的至少一部分进行存储。

2.根据权利要求1所述的特征存储方法，其特征在于，所述直角坐标值包括x轴坐标、y轴坐标和z轴坐标；

所述极坐标值包括长度值、垂直角和水平角；

所述根据各所述激光特征点的直角坐标值计算各所述激光特征点的极坐标值，包括：

根据各所述激光特征点的x轴坐标、y轴坐标和z轴坐标，按照下式(1)、(2)和(3)，计算各所述激光特征点的长度值l、垂直角β和水平角α：

其中，x、y和z分别表示各所述激光特征点的x轴坐标、y轴坐标和z轴坐标。

3.根据权利要求2所述的特征存储方法，其特征在于，所述根据预设长度分辨率和预设角度分辨率对所述极坐标值进行离散化得到各所述激光特征点的离散极坐标值，包括：

将各所述激光特征点的长度值l与所述预设长度分辨率相除，并对相除得到的商进行取整，得到各所述激光特征点的离散长度值l_sampling；

将各所述激光特征点的垂直角β和水平角α分别与所述预设角度分辨率相除，并对相除得到的商进行取整，得到各所述激光特征点的离散垂直角β_sampling和离散水平角α_sampling。

4.根据权利要求3所述的特征存储方法，其特征在于，在将各帧激光点云的所述转换后点云特征作为所述激光里程计输出的各帧激光特征的至少一部分进行存储之后，还包括：

读取所存储的各帧激光特征，根据各所述激光特征点的离散极坐标值、所述预设长度分辨率和所述预设角度分辨率，反向计算得到各所述激光特征点的直角坐标值，以用于对所述激光里程计输出的各帧激光特征进行非线性优化。

5.根据权利要求1所述的特征存储方法，其特征在于，所述对各帧激光点云进行特征提取得到点云特征，包括：

使用指定边长的三维网格对所述各帧激光点云进行降采样；

将降采样得到的点云作为所述点云特征。

6.根据权利要求1所述的特征存储方法，其特征在于，所述根据当前帧和前一相邻帧的激光点云、当前帧和前一相邻帧的惯性测量单元的测量数据以及当前帧和前一相邻帧的车轮转速以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，计算得到当前帧激光点云相对于第一帧激光点云的位姿变化，包括：

当当前帧激光点云为第二帧激光点云时，根据当前帧激光点云的起始时刻与第一帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速，基于第一帧激光点云的位姿预测当前帧激光点云的位姿；

以所预测的当前帧激光点云的位姿作为初值，通过通用迭代最近邻算法计算当前帧激光点云相对于第一帧激光点云的位姿变化；

当当前帧激光点云为所述第二帧激光点云后的其他帧激光点云时，根据当前帧激光点云的起始时刻与前一相邻帧激光点云的起始时刻之间的惯性测量单元的测量数据和车轮转速，基于前一相邻帧激光点云的位姿预测当前帧激光点云的位姿；

以所预测的当前帧激光点云的位姿作为初值，通过通用迭代最近邻算法计算当前帧激光点云与前一相邻帧激光点云之间的位姿变化；

根据当前帧激光点云与前一相邻帧激光点云之间的位姿变化以及前一相邻帧激光点云相对于第一帧激光点云的位姿变化，得到当前帧激光点云相对于第一帧激光点云的位姿变化。

7.根据权利要求1所述的特征存储方法，其特征在于，所述各帧激光点云为剔除了移动物体点云的激光点云。

8.一种电子设备，其特征在于，包括：

处理器；以及

存储有计算机程序代码的存储器；

当所述计算机程序代码被所述处理器运行时，导致所述电子设备执行根据权利要求1-7中任一项所述的激光里程计的特征存储方法。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序代码，当所述计算机程序代码在电子设备上运行时，导致所述电子设备执行根据权利要求1-7中任一项所述的激光里程计的特征存储方法。

Images