CN109946701B - 一种点云坐标转换方法及装置 - Google Patents

Abstract

本申请实施例提供了一种点云坐标转换方法及装置。本申请根据从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中；获取从地面坐标系变换至所述导航坐标系之间的平面变换矩阵；基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。这样，通过先求解二维平面变换矩阵，再进行三维空间求解，可以简化计算步骤，提高计算效率以及转换的准确率。

Description

一种点云坐标转换方法及装置

技术领域

本申请涉及自动驾驶领域，尤其是涉及一种点云坐标转换方法及装置。

背景技术

随着计算机技术的快速发展，自动驾驶技术也随之发展，对于自动驾驶来讲，一个必不可少的工作就是计算自己的位置，以及自己与道路、其他车辆、行人等交通元素之间的相对位置关系，为了描述这些复杂的空间、时间关系，需要建立坐标系去描述，在自动驾驶领域可以建立多套坐标系对自己位置进行描述，所述多套坐标系需完成关联和转换，最终构建出统一的环境模型。激光雷达是自动驾驶领域的最重要传感器之一，激光雷达采集的数据均位于雷达坐标系下，需转换所述数据到其他坐标系下，完成统一的环境模型。车体坐标系用来描述车辆周围的物体和本车之间的相对位置关系，正确使用所述车体坐标系，即可完成对车身位姿的描述，以及确定周围物体和本车体之间的相对位置关系，对自动驾驶中车辆的定位具有重要意义。目前，基于惯性测量单元IMU的坐标定义是常用的车体坐标系定义方式之一。所以在多套坐标系下的雷达坐标系和IMU坐标系的转换具有重要意义。

现阶段，常用的雷达坐标系和IMU坐标系转换手段都是基于点云匹配的方式，这些方式都需要操作车辆走比较复杂的路线，如绕“8”字等以获取最佳质量的点云数据，然后把保存下来的数据用来整体解算激光雷达安装参数。这个过程一般计算繁琐，耗时较长，数据转换的精度方面出现偏差的概率较高。

发明内容

有鉴于此，本申请提供一种点云坐标转换方法及装置，可以简化计算步骤，提高计算效率以及转换的准确率。

本申请实施例提供了一种点云坐标转换方法，所述点云坐标转换方法包括：

基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；

基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，其中，所述拟合地平面为所述地面坐标系的二维水平面，所述地面坐标系的坐标原点位置为采集所述三维点云数据的采集器平移至所述拟合地平面上所在的位置，所述地面坐标系的横轴方向与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同；

获取从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵；

基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

进一步的，所述基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，包括：

基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵；

基于所述横坐标轴旋转矩阵和所述纵坐标轴旋转矩阵，将三维点云数据中的每个采集点校正至地面坐标系中。

进一步的，所述基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，包括：

使用所述拟合地平面的法向量，通过以下公式确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵：

(0,0,1)＝MX*MY*v_z；

其中，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，v_z为所述拟合地平面的法向量。

进一步的，通过以下方式确定从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵：

获取所述地面坐标系中至少两个计算靶标的坐标；

基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵；

确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

进一步的，当所述至少两个计算靶标包括第一计算靶标和第二计算靶标时，所述基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵，包括：

使用所述第一计算靶标的坐标和所述第二计算靶标的坐标，通过以下公式计算将所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵：

其中，I_T1为所述第一计算靶标在导航坐标系中的坐标，L_T1为所述第一计算靶标在所述地面坐标系中的坐标，I_T2为所述第二计算靶标在导航坐标系中的坐标，L_T2为所述第二计算靶标在所述地面坐标系中的坐标，M_xy*R_z为所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵。

进一步的，确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵之后，所述点云坐标转换方法还包括：

获取所述地面坐标系中至少一个测试靶标的坐标；

基于所述至少一个测试靶标的坐标，对所述平面变换矩阵进行校验。

进一步的，所述基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标，包括：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵；

基于所述变换矩阵将每个采集点变换至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

进一步的，所述基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵，包括：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，通过以下公式确定将每个采集点变换至所述导航坐标系的变换矩阵：

M_L2I＝M_xy*R_z*MX*MY；

其中，M_L2I为每个采集点变换至所述导航坐标系的变换矩阵，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，M_xy*R_z为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

本实施申请实施例提供了一种点云坐标转换装置，所述点云坐标转换装置包括：

第一确定模块，用于基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；

校正模块，用于基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，其中，所述拟合地平面为所述地面坐标系的二维水平面，所述地面坐标系的坐标原点位置为采集所述三维点云数据的采集器平移至所述拟合地平面上所在的位置，所述地面坐标系的横轴方向与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同；

第一获取模块，用于获取从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵；

第二确定模块，用于基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

进一步的，所述校正模块包括：

第一确定单元，用于基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵；

第一校正单元，用于基于所述横坐标轴旋转矩阵和所述纵坐标轴旋转矩阵，将三维点云数据中的每个采集点校正至地面坐标系中。

进一步的，所述第一确定单元具体用于：

使用所述拟合地平面的法向量，通过以下公式确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵：

(0,0,1)＝MX*MY*v_z；

其中，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，v_z为所述拟合地平面的法向量。

进一步的，所述第一获取模块包括：

第一获取单元，用于获取所述地面坐标系中至少两个计算靶标的坐标；

第一计算单元，用于基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵；

第二确定单元，用于确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

进一步的，当所述至少两个计算靶标包括第一靶标和第二靶标时，所述第一计算单元具体用于：

使用所述第一计算靶标的坐标和所述第二计算靶标的坐标，通过以下公式计算将所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵：

其中，I_T1为所述第一计算靶标在导航坐标系中的坐标，L_T1为所述第一计算靶标在所述地面坐标系中的坐标，I_T2为所述第二计算靶标在导航坐标系中的坐标，L_T2为所述第二计算靶标在所述地面坐标系中的坐标，M_xy*R_z为所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵。

进一步的，所述点云坐标转换装置还包括：

第二获取模块，用于获取所述地面坐标系中至少一个测试靶标的坐标；

第一校验模块，用于基于所述至少一个测试靶标的坐标，对所述平面变换矩阵进行校验。

进一步的，所述第二确定模块包括：

第三确定单元，用于基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵；

第四确定单元，用于基于所述变换矩阵将每个采集点变换至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

进一步的，所述第三确定单元具体用于：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，通过以下公式确定将每个采集点变换至所述导航坐标系的变换矩阵：

M_L2I＝M_xy*R_z*MX*MY；

其中，M_L2I为每个采集点变换至所述导航坐标系的变换矩阵，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，M_xy*R_z为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

本申请实施例还提供一种电子设备，包括：处理器、存储器和总线，所述存储器存储有所述处理器可执行的机器可读指令，当电子设备运行时，所述处理器与所述存储器之间通过总线通信，所述机器可读指令被所述处理器执行时执行如上述的点云坐标转换方法的步骤。

本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行如上述的点云坐标转换方法的步骤。

本申请实施例提供的点云坐标转换方法及装置，根据从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中；获取从地面坐标系变换至所述导航坐标系之间的平面变换矩阵；基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。这样，通过先求解二维平面变换矩阵，再进行三维空间求解，可以简化计算步骤，提高计算效率以及转换的准确率。

为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作说明如下。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为一种可能的应用场景下的系统架构图；

图2为本申请一实施例提供的点云坐标转换方法的流程图；

图3为本申请另一实施例提供的点云坐标转换方法的流程图；

图4为本申请一实施例提供的点云坐标转换装置的结构图之一；

图5为本申请一实施例提供的点云坐标转换装置的结构图之二；

图6为图4中所示的校正模块的结构图；

图7为图4中所示的第一获取模块的结构图；

图8为图4中所示的第二确定模块的结构图；

图9为本申请一实施例提供的电子设备的结构图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和出示的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围内，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

首先，对本申请可适用的应用场景进行介绍。本申请可以应用于自动驾驶的领域，通过对采集到的三维点云数据的坐标系转换，简化计算步骤，提高计算效率以及转换的准确率。请参阅图1，图1为一种在该场景下的系统构图。如图1中所示，所述系统包括数据采集装置和点云坐标转换装置。所述数据采集装置用于采集三维点云数据，得到待转换的点云数据，所述点云坐标转换装置将所述待转换的三维点云数据变换至导航坐标系下，从而构建出统一的环境模型，以完成自动驾驶中的导航定位任务。

现阶段，常用的雷达坐标系和导航坐标系转换手段都是基于点云匹配的方式，这些方式都需要操作车辆走比较复杂的路线，如绕“8”字等以获取最佳质量的点云数据，然后把保存下来的数据用来整体解算激光雷达安装参数。这个过程一般计算繁琐，耗时较长，数据转换的精度方面易出现偏差的概率较高。

基于此，本申请实施例提供一种点云坐标转换方法及装置，可以简化计算步骤，提高计算效率以及转换的准确率。

请参阅图2，图2为本申请一实施例提供的流程图。如图2中所示，本申请实施例提供的点云坐标转换方法，包括：

步骤201、基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面。

该步骤中，需要从采集到的大量的点云数据中，筛选出与所述点云数据要平移到的导航坐标系的二维平面处于同一平面的点云数据，将所述处于上述同一平面的点云数据拟合成地平面。

其中，采集点云数据的方法可以是使用雷达传感器进行采集。所采集到的点云数据位于雷达坐标系下，在自动驾驶领域中，车辆自动导航定位可以用多种坐标系统描述，例如车体坐标系统，世界坐标系统等。自动驾驶车辆安装有至少一个传感器。所述雷达传感器在每辆车中的安装位置不同，对同一物体的描述也不同，为了将不同传感器关联起来，将采集数据转换到同一坐标系下进行定位，是有必要的。

步骤202、基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中。

该步骤中，确定拟合成的地平面的法向量，将三维点云数据中的每个采集点校正到地面坐标系中。

其中，所述拟合地平面为所述地面坐标系的二维水平面，所述地面坐标系的坐标原点为采集所述三维点云数据的采集器平移至拟合地平面上所在的位置，所述地面坐标系的横轴方向与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同。

其中，所述坐标系的转换过程中，对于坐标系的描述应始终采用同一套坐标系，可以为左手坐标系也可以为右手坐标系。

其中，将数据校正到所述地面坐标系下，所述地面坐标系与所述导航坐标系的二维平面平行，地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同，计算转换矩阵的过程中，无需计算横轴和纵轴的旋转变换量，简化了计算步骤。

步骤203、获取从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

该步骤中，所述地面坐标系与所述导航坐标系的二维平面是平行的，二者之间的坐标变换是在二维基础上的平面变换，获取所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。先获取平面变换矩阵，在基于所述平面变换矩阵计算三维变换矩阵，简化了三维计算变换矩阵的计算步骤。

步骤204、基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

该步骤中，每个采集点在自己原始坐标系下有自己的坐标，基于所述平面变换矩阵，所述每个采集点的坐标与求解得到的平面变换矩阵相乘，得到的新坐标值即为每个采集点在所述导航坐标系下的坐标。

本申请实施例提供的点云坐标转换方法，基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中；获取从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵并根据所述平面变换矩确定每个采集点在所述导航坐标系下的坐标。

这样，本申请提供的一种点云坐标转换方法，通过先求解二维平面变换矩阵，再进行三维空间求解，可以简化计算步骤，提高计算效率以及转换的准确率。

请参阅图3，图3为本申请另一实施例提供的点云坐标转换方法的流程图。如图3中所示，本申请实施例提供的方法，包括：

步骤301、基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面。

步骤302、基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，其中，所述拟合地平面为所述地面坐标系的二维水平面，所述地面坐标系的坐标原点位置为采集所述三维点云数据的采集器平移至所述拟合地平面上所在的位置，所述地面坐标系的横轴方向与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同。

步骤303、获取所述地面坐标系中至少两个计算靶标的坐标。

该步骤中，得到所述地面坐标系中至少两个计算靶标的坐标，其中，所述至少两个计算靶标的坐标是指所述靶标圆筒的中轴的中点的坐标，它的坐标可以准确测量。通过将所述计算靶标平移至导航坐标系中，得到所述计算靶标在导航坐标系的坐标。获取采集的点云数据中所述计算靶标的点云数据，解算所述点云数据中计算靶标中点的坐标，从而得到地面坐标系中所述计算靶标的坐标。

步骤304、基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵。

本实施例中，是以两个计算靶标为例进行说明计算所述平移变换矩阵的，但并不局限于此，在其他实施例中，还可以是使用三个计算靶标、四个计算靶标或者五个计算靶标等，来搭配上述公式或者其他合适的公式以计算确定平移变换矩阵，例如可以使用三个计算标靶通过

来计算平移变换矩阵；

其中，I_T1为所述第一计算靶标在导航坐标系中的坐标，L_T1为所述第一计算靶标在所述地面坐标系中的坐标，I_T2为所述第二计算靶标在导航坐标系中的坐标，L_T2为所述第二计算靶标在所述地面坐标系中的坐标，I_T3为所述第三计算靶标在导航坐标系中的坐标，L_T3为所述第三计算靶标在所述地面坐标系中的坐标，M_xy*R_z为所述第一计算靶标，所述第二计算靶标和所述第三计算靶标转换至所述导航坐标系的平移变换矩阵。

其中所述平移变换矩阵的右手坐标系表达形式为：

和

在所述矩阵中z方向平移量d_z是已知量，选定一个参考平面，所述参考平面与所述拟合地平面平行，所述采集点到所述参考平面的垂直距离与所述地面坐标系二维平面的垂直距离作差，得到d_z。求解方程中的未知数x方向平移量d_x，y方向平移量d_y，和z轴旋转量r_z，即可确定所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵。

相应的左手坐标系的平移变换矩阵也可以通过上述方程进行求解。

步骤305、确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

该步骤中，基于至少两个计算靶标计算出来的平移变换矩阵是在所述地面坐标系下计算出来的，所述地面坐标系与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同，在实际应用中，所述采集点的横纵坐标轴的方向不固定，所以基于所述计算靶标计算的平移坐标系只是所述平移变换矩阵从所述地面坐标系变换至所述导航坐标系之间的变换矩阵中的平面变换矩阵。

步骤306、获取所述地面坐标系中至少一个测试靶标的坐标。

该步骤中，获取所述地面坐标系中的至少一个测试靶标的坐标值。其中，所述测试靶标与所述计算靶标一同放置在数据采集装置可以采集的范围内，所述测试靶标与所述计算靶标的相对位置不固定。所述测试靶标的坐标和所述计算靶标的坐标一样，都是可以经过测量，转换以及解算得到的。

步骤307、基于所述至少一个测试靶标的坐标，对所述平面变换矩阵进行校验。

该步骤中，基于所述测试靶标的地面坐标系坐标以及已经通过计算靶标求解得到的至少两个计算靶标转换至所述导航坐标系的平移变换矩阵即从所述地面坐标系、至所述导航坐标系之间的平面变换矩阵，计算所述测试靶标的导航坐标系的坐标值，与之前获取的测试靶标的导航坐标系坐标进行比对，基于比对的结果，对所述平面变换矩阵进行校验。也可以基于所述测试靶标的导航坐标系的坐标和所述平面变换矩阵求解所述测试靶标的地面坐标值，校验所述平面变换矩阵。

步骤308、基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

其中，步骤301，步骤302和步骤308的描述可以参照步骤201，步骤202和步骤204的描述，对此不做赘述。

进一步的，步骤302包括：

基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵。

该步骤中，基于拟合地平面的法向量，可以确定所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵。

其中，旋转矩阵中重要的参数为每个坐标轴的旋转角度，现给出右手坐标系下横坐标轴旋转矩阵

纵坐标轴旋转矩阵

求解横坐标轴旋转矩阵和纵坐标轴旋转矩阵就是求解横坐标轴旋转变换量r_x和横坐标轴旋转变换量r_y的过程，可以用公式(0,0,1)＝MX*MY*v_z求解。

其中，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，v_z为所述拟合地平面的法向量。

在计算中，为了便于计算和简化计算步骤，也可以将MX*MY看做一个整体进行求解，并求得MX*MY的整体结果，这样所述方程中只有一个未知量，可以简化求解过程，而且后续计算时，也可以直接使用整体的MX*MY进行计算。

所述求解过程在左手坐标系下也适用。

基于所述横坐标轴旋转矩阵和所述纵坐标轴旋转矩阵，将三维点云数据中的每个采集点校正至地面坐标系中。

该步骤中，求解出来的所述横坐标轴旋转矩阵和所述纵坐标轴旋转矩阵的旋转变换量是旋转到所述地面坐标系的旋转量，所有所述三维点云数据中的每个采集点与所述坐标轴旋转矩阵和所述纵坐标轴旋转矩阵相乘，得到在所述地面坐标系中的坐标值，将所述三维点云数据中的每个采集点校正至地面坐标系中。

进一步的，步骤308包括：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵。

通过以下公式确定将每个采集点变换至所述导航坐标系的变换矩阵：

M_L2IM＝M_xy*R_z*MX*MY；

其中，M_L2I为每个采集点变换至所述导航坐标系的变换矩阵，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，M_xy*R_z为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

该步骤中，确定地面坐标系变换至所述导航坐标系之间的平面变换矩阵M_xy*R_z以及MX*MY的整体结果之后，要确定将每个采集点变换至所述导航坐标系的变换矩阵，通过公式M_L2IM＝M_xy*R_z*MX*MY计算得到。

基于所述变换矩阵将每个采集点变换至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

该步骤中，基于求解的变换矩阵和已知原始坐标的采集点，将所有点云的数据与所述变换矩阵相乘，得到的新的坐标即为所述采集点在所述导航坐标系中的坐标。

本申请实施例提供的点云坐标转换方法，基于从采集到的三维点云数据筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中；获取计算靶标的坐标，并通过所述计算靶标计算出从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵，并通过已知坐标的测试靶标进行校验，得到准确的从所述地面坐标系至所述导航坐标系之间的平面变换矩阵；基于基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

这样，本申请提供的一种点云坐标转换方法，通过先求解二维平面变换矩阵，再进行三维空间求解，可以简化计算步骤，提高计算效率以及转换的准确率。

请参阅图4，图4为本申请一实施例提供的点云坐标转换装置的结构图之一，图4为本申请一实施例提供的点云坐标转换装置的结构图之一；图5为本申请一实施例提供的点云坐标转换装置的结构图之二；图6为图4中所示的校正模块的结构图；图7为图4中所示的第一获取模块的结构图；图8为图4中所示的第二确定模块的结构图；

如图4中所示，所述点云坐标转换装置400包括：

第一确定模块410，用于基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面。

校正模块420，用于基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，其中，所述拟合地平面为所述地面坐标系的二维水平面，所述地面坐标系的坐标原点位置为采集所述三维点云数据的采集器平移至所述拟合地平面上所在的位置，所述地面坐标系的横轴方向与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同。

第一获取模块430，用于获取从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

第二确定模块440，用于基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

如图5中所示，所述点云坐标转换装置400还包括：

第二获取模块450，用于获取所述地面坐标系中至少一个测试靶标的坐标。

第一校验模块460，用于基于所述至少一个测试靶标的坐标，对所述平面变换矩阵进行校验。

进一步的，如图6所示，所述校正模块420包括：

第一确定单元421，用于基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵。

第一校正单元422，用于基于所述横坐标轴旋转矩阵和所述纵坐标轴旋转矩阵，将三维点云数据中的每个采集点校正至地面坐标系中。

进一步的，所述第一确定单元421具体用于：

使用所述拟合地平面的法向量，通过以下公式确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵：

(0,0,1)＝MX*MY*v_z；

其中，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，v_z为所述拟合地平面的法向量。

进一步的，如图7所示，所述第一获取模块430包括：

第一获取单元431，用于获取所述地面坐标系中至少两个计算靶标的坐标。

第一计算单元432，用于基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵。

第二确定单元433，用于确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

进一步的，当所述至少两个计算靶标包括第一靶标和第二靶标时，所述第一计算单元432具体用于：

使用所述第一计算靶标的坐标和所述第二计算靶标的坐标，通过以下公式计算将所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵：

其中，I_T1为所述第一计算靶标在导航坐标系中的坐标，L_T1为所述第一计算靶标在所述地面坐标系中的坐标，I_T2为所述第二计算靶标在导航坐标系中的坐标，L_T2为所述第二计算靶标在所述地面坐标系中的坐标，M_xy*R_z为所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵。

进一步的，如图8所示，所述第二确定模块440包括：

第三确定单元441，用于基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵。

第四确定单元442，用于基于所述变换矩阵将每个采集点变换至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

进一步的，所述第三确定单元441具体用于：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，通过以下公式确定将每个采集点变换至所述导航坐标系的变换矩阵：

M_L2I＝M_xy*R_z*MX*MY；

其中，M_L2I为每个采集点变换至所述导航坐标系的变换矩阵，MX为地面坐标系中的横坐标轴旋转矩阵，MY为地面坐标系中的纵坐标轴旋转矩阵，M_xy*R_z为从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵。

本实施例中的点云坐标转换装置400，可以实现如图2和图3所示实施例中的路面提取方法的全部方法步骤，并可以达到相同的效果，在此不做赘述。

本申请实施例提供的点云坐标转换装置，基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中；获取从所述地面坐标系变换至所述导航坐标系之间的平面变换矩阵并根据所述平面变换矩确定每个采集点在所述导航坐标系下的坐标。

这样，本申请提供的一种点云坐标转换装置，通过先求解二维平面变换矩阵，再进行三维空间求解，可以简化计算步骤，提高计算效率以及转换的准确率。

请参阅图9，图9为本申请一实施例提供的电子设备的结构图。如图9中所示，所述电子设备900包括处理器910、存储器920和总线930。

所述存储器920存储有所述处理器910可执行的机器可读指令，当电子设备900运行时，所述处理器910与所述存储器920之间通过总线930通信，所述机器可读指令被所述处理器910执行时，可以执行如上述图2以及图3所示方法实施例中的点云坐标转换方法的步骤，具体实现方式可参见方法实施例，在此不再赘述。

本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时可以执行如上述图2以及图3所示方法实施例中的点云坐标转换方法的步骤，具体实现方式可参见方法实施例，在此不再赘述。

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

在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个处理器可执行的非易失的计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-OnlyMemory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上所述实施例，仅为本申请的具体实施方式，用以说明本申请的技术方案，而非对其限制，本申请的保护范围并不局限于此，尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本申请实施例技术方案的精神和范围，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。

Claims (16)

1.一种点云坐标转换方法，其特征在于，所述方法包括：

基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；

基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，其中，所述拟合地平面为所述地面坐标系的二维水平面，所述地面坐标系的坐标原点位置为采集所述三维点云数据的采集器平移至所述拟合地平面上所在的位置，所述地面坐标系的横轴方向与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同；

获取从所述地面坐标系变换至所述导航坐标系的平面变换矩阵；

基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

2.如权利要求1所述的点云坐标转换方法，其特征在于，所述基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，包括：

基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵；

基于所述横坐标轴旋转矩阵和所述纵坐标轴旋转矩阵，将三维点云数据中的每个采集点校正至地面坐标系中。

3.如权利要求2所述的点云坐标转换方法，其特征在于，所述基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，包括：

使用所述拟合地平面的法向量，通过以下公式确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵：

(0,0,1)＝MX*MY*v_z；

其中，MX为横坐标轴旋转矩阵，MY为纵坐标轴旋转矩阵，v_z为所述拟合地平面的法向量。

4.如权利要求1所述的点云坐标转换方法，其特征在于，通过以下方式确定从所述地面坐标系变换至所述导航坐标系的平面变换矩阵：

获取所述地面坐标系中至少两个计算靶标的坐标；

基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵；

确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系的平面变换矩阵。

5.如权利要求4所述的点云坐标转换方法，其特征在于，当所述至少两个计算靶标包括第一计算靶标和第二计算靶标时，所述基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵，包括：

使用所述第一计算靶标的坐标和所述第二计算靶标的坐标，通过以下公式计算将所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵：

其中，I_T1为所述第一计算靶标在导航坐标系中的坐标，L_T1为所述第一计算靶标在所述地面坐标系中的坐标，I_T2为所述第二计算靶标在导航坐标系中的坐标，L_T2为所述第二计算靶标在所述地面坐标系中的坐标，M_xy*R_z为所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵。

6.如权利要求4所述的点云坐标转换方法，其特征在于，确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系的平面变换矩阵之后，所述方法还包括：

获取所述地面坐标系中至少一个测试靶标的坐标；

基于所述至少一个测试靶标的坐标，对所述平面变换矩阵进行校验。

7.如权利要求1所述的点云坐标转换方法，其特征在于，所述基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标，包括：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵；

基于所述变换矩阵将每个采集点变换至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

8.如权利要求7所述的点云坐标转换方法，其特征在于，所述基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵，包括：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，通过以下公式确定将每个采集点变换至所述导航坐标系的变换矩阵：

M_L2I＝M_xy*R_z*MX*MY；

其中，M_L2I为每个采集点变换至所述导航坐标系的变换矩阵，MX为横坐标轴旋转矩阵，MY为纵坐标轴旋转矩阵，M_xy*R_z为从所述地面坐标系变换至所述导航坐标系的平面变换矩阵。

9.一种点云坐标转换装置，其特征在于，所述点云坐标转换装置包括：

第一确定模块，用于基于从采集到的三维点云数据中筛选出的与导航坐标系的水平二维平面处于同一平面的点云数据，确定拟合地平面；

校正模块，用于基于所述拟合地平面的法向量，将三维点云数据中的每个采集点校正至地面坐标系中，其中，所述拟合地平面为所述地面坐标系的二维水平面，所述地面坐标系的坐标原点位置为采集所述三维点云数据的采集器平移至所述拟合地平面上所在的位置，所述地面坐标系的横轴方向与所述导航坐标系的横轴方向相同，所述地面坐标系的纵轴方向与所述导航坐标系的纵轴方向相同；

第一获取模块，用于获取从所述地面坐标系变换至所述导航坐标系的平面变换矩阵；

第二确定模块，用于基于所述平面变换矩阵，将每个采集点平移至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

10.如权利要求9所述的点云坐标转换装置，其特征在于，所述校正模块包括：

第一确定单元，用于基于所述拟合地平面的法向量，确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵；

第一校正单元，用于基于所述横坐标轴旋转矩阵和所述纵坐标轴旋转矩阵，将三维点云数据中的每个采集点校正至地面坐标系中。

11.如权利要求10所述的点云坐标转换装置，其特征在于，所述第一确定单元具体用于：

使用所述拟合地平面的法向量，通过以下公式确定将所述三维点云数据中每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵：

(0,0,1)＝MX*MY*v_z；

其中，MX为横坐标轴旋转矩阵，MY为纵坐标轴旋转矩阵，v_z为所述拟合地平面的法向量。

12.如权利要求9所述的点云坐标转换装置，其特征在于，所述第一获取模块包括：

第一获取单元，用于获取所述地面坐标系中至少两个计算靶标的坐标；

第一计算单元，用于基于所述至少两个计算靶标的坐标，计算将所述至少两个计算靶标转换至所述导航坐标系的平移变换矩阵；

第二确定单元，用于确定所述平移变换矩阵为从所述地面坐标系变换至所述导航坐标系的平面变换矩阵。

13.如权利要求12所述的点云坐标转换装置，其特性在于，当所述至少两个计算靶标包括第一计算靶标和第二计算靶标时，所述第一计算单元具体用于：

使用所述第一计算靶标的坐标和所述第二计算靶标的坐标，通过以下公式计算将所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵：

其中，I_T1为所述第一计算靶标在导航坐标系中的坐标，L_T1为所述第一计算靶标在所述地面坐标系中的坐标，I_T2为所述第二计算靶标在导航坐标系中的坐标，L_T2为所述第二计算靶标在所述地面坐标系中的坐标，M_xy*R_z为所述第一计算靶标和所述第二计算靶标转换至所述导航坐标系的平移变换矩阵。

14.如权利要求12所述的点云坐标转换装置，其特性在于，所述点云坐标转换装置还包括：

第二获取模块，用于获取所述地面坐标系中至少一个测试靶标的坐标；

第一校验模块，用于基于所述至少一个测试靶标的坐标，对所述平面变换矩阵进行校验。

15.如权利要求9所述的点云坐标转换装置，其特征在于，所述第二确定模块包括：

第三确定单元，用于基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，确定将每个采集点变换至所述导航坐标系的变换矩阵；

第四确定单元，用于基于所述变换矩阵将每个采集点变换至所述导航坐标系中，并确定每个采集点在所述导航坐标系中的坐标。

16.如权利要求15所述的点云坐标转换装置，其特征在于，所述第三确定单元具体用于：

基于所述平面变换矩阵以及确定的将每个采集点校正至所述地面坐标系中的横坐标轴旋转矩阵和纵坐标轴旋转矩阵，通过以下公式确定将每个采集点变换至所述导航坐标系的变换矩阵：

M_L2I＝M_xy*R_z*MX*MY；

其中，M_L2I为每个采集点变换至所述导航坐标系的变换矩阵，MX为横坐标轴旋转矩阵，MY为纵坐标轴旋转矩阵，M_xy*R_z为从所述地面坐标系变换至所述导航坐标系的平面变换矩阵。

Images