CN107388967B - 一种车载三维激光传感器的外参数补偿方法及装置 - Google Patents

Abstract

本发明提供一种车载三维激光传感器的外参数补偿方法及装置，该方法包括：根据当前传感器外参数计算当前外参数旋转平移矩阵；当接收到车载三维激光传感器发送的当前帧点云数据以及惯性测量单元发送的当前车辆位姿数据时，根据当前外参数旋转平移矩阵将当前帧点云数据从传感器坐标系下变换至车体坐标系下，并提取当前帧地面点云数据；对当前帧地面点云数据进行平面拟合，确定当前路面补偿角；根据当前路面补偿角和当前车辆位姿数据对当前传感器外参数进行补偿。这就实现了利用单帧点云数据以及车辆位姿数据对传感器外参数进行补偿，从而降低传感器外参数的误差，使得变换到车体坐标系下的点云数据相对地面逐渐稳定，提高了后续环境感知的准确度。

Description

一种车载三维激光传感器的外参数补偿方法及装置

技术领域

本发明涉及自动驾驶技术领域，更具体地说，涉及一种车载三维激光传感器的外参数补偿方法及装置。

背景技术

随着ADAS(Advanced Driver Assistant System，高级驾驶辅助***)技术不断发展，自动驾驶汽车普遍安装有车载三维激光传感器，以感知车辆周围可通行状况。

在使用车载三维激光传感器之前，需要预先标定传感器外参数，其中，传感器外参数包括车载三维激光传感器在车体坐标系下的三维坐标、俯仰角、横滚角和偏航角，以确定传感器坐标系变换到车体坐标系的旋转平移矩阵，从而为后续环境感知提供条件，其中，标定时的车体坐标系采用的是地面坐标系。

车载三维激光传感器一般与车身固联，在车辆行驶过程中，由于车上乘员的载重分配与标定载重工况不一致或者行驶路面颠簸等原因常使车身与地面之间的相对位置发生变化，也就是，实际的车体坐标系与地面坐标系不一致，因此，这就造成标定的传感器外参数与实际所需传感器外参数存在误差，从而导致变换到车体坐标系下的点云数据相对地面不稳定，降低了后续环境感知结果的准确度。

发明内容

有鉴于此，本发明提供一种车载三维激光传感器的外参数补偿方法及装置，以解决车辆行驶过程中，实际车体坐标系与地面坐标系不一致造成的标定的传感器外参数存在误差的问题。技术方案如下：

一种车载三维激光传感器的外参数补偿方法，包括：

根据车载三维激光传感器的当前传感器外参数计算当前外参数旋转平移矩阵；

当接收到所述车载三维激光传感器发送的当前帧点云数据以及惯性测量单元发送的当前车辆位姿数据时，根据所述当前外参数旋转平移矩阵将所述当前帧点云数据从传感器坐标系变换至车体坐标系下；

从所述车体坐标系下的所述当前帧点云数据中提取当前帧地面点云数据；

对所述当前帧地面点云数据进行平面拟合，并根据拟合结果以及所述当前车辆位姿数据确定当前路面补偿角；

根据所述当前路面补偿角和所述当前车辆位姿数据对所述当前传感器外参数进行补偿。

优选的，所述从所述车体坐标系下的所述当前帧点云数据中提取当前帧地面点云数据，包括：

将所述车体坐标系下的所述当前帧点云数据中包含的各个数据点按照位置坐标投影到极坐标栅格地图上，所述极坐标栅格地图被预先划分为至少一个地图栅格；

对于每一个所述地图栅格，获取投影到该地图栅格的各个所述数据点的高度值，并计算最大高度值与最小高度值的高度差值；

判断所述高度差值是否小于障碍物判定阈值，所述障碍物判定阈值用于表征判定为障碍物的最小高度差值；

当所述高度差值小于所述障碍物判定阈值时，判断该地图栅格的中心与所述极坐标栅格地图中心的距离是否大于距离阈值；

若是，为该地图栅格添加地面标签；

若否，判断所述最小高度值是否小于地面判定阈值，所述地面判定阈值用于表征判定为地面的最小高度差值；

当所述最小高度值小于所述地面判定阈值时，为该地图栅格添加地面标签；

根据添加有所述地面标签的全部所述地图栅格内包含的数据点生成当前帧地面点云数据。

优选的，所述对所述当前帧地面点云数据进行平面拟合，并根据拟合结果以及所述当前车辆位姿数据确定当前路面补偿角，包括：

判断所述当前帧地面点云中包含数据点的数量是否大于拟合点阈值；

若否，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角；

若是，采用梯度下降算法对所述当前帧地面点云进行拟合，并计算拟合误差值；

判断所述拟合误差值是否小于拟合误差阈值；

当所述拟合误差值小于所述拟合误差阈值时，判定拟合成功，并计算当前拟合平面倾斜角度；

判断是否存在上一历史路面补偿角，并根据判断结果以及所述当前拟合平面倾斜角度生成相应当前路面补偿角；

当所述拟合误差值不小于所述拟合误差阈值时，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角。

优选的，所述判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角，包括：

判断是否存在上一历史路面补偿角；

若否，确定当前路面补偿角为0；

若是，判断当前车速是否为0；

当所述当前车速为0时，将所述历史路面补偿角确定为当前路面补偿角；

当所述当前车速不为0时，根据所述当前车辆位姿数据对所述历史路面补偿角进行衰减，得到当前路面补偿角。

优选的，所述判断是否存在上一历史路面补偿角，并根据判断结果以及所述当前拟合平面倾斜角度生成相应当前路面补偿角，包括：

判断是否存在上一历史路面补偿角；

若否，将所述当前拟合平面倾斜角度确定为当前路面补偿角；

若是，根据所述历史路面补偿角对所述当前拟合平面倾斜角度进行低通滤波，得到当前路面补偿角。

优选的，所述根据所述当前路面补偿角和所述当前车辆位姿数据对所述当前传感器外参数进行补偿，包括：

根据所述当前路面补偿角和所述当前车辆位姿数据计算外参数补偿量；

依据所述外参数补偿量对所述当前传感器外参数进行补偿。

一种车载三维激光传感器的外参数补偿装置，包括：计算模块、变换模块、提取模块、平面拟合模块和外参数补偿模块；

所述计算模块，用于根据车载三维激光传感器的当前传感器外参数计算当前外参数旋转平移矩阵；

所述变换模块，用于当接收到所述车载三维激光传感器发送的当前帧点云数据以及惯性测量单元发送的当前车辆位姿数据时，根据所述当前外参数旋转平移矩阵将所述当前帧点云数据从传感器坐标系变换至车体坐标系下；

所述提取模块，用于从所述车体坐标系下的所述当前帧点云数据中提取当前帧地面点云数据；

所述平面拟合模块，用于对所述当前帧地面点云数据进行平面拟合，并根据拟合结果以及所述当前车辆位姿数据确定当前路面补偿角；

所述外参数补偿模块，用于根据所述当前路面补偿角和所述当前车辆位姿数据对所述当前传感器外参数进行补偿。

优选的，所述提取模块，具体用于：

将所述车体坐标系下的所述当前帧点云数据中包含的各个数据点按照位置坐标投影到极坐标栅格地图上，所述极坐标栅格地图被预先划分为至少一个地图栅格，以及对于每一个所述地图栅格，获取投影到该地图栅格的各个所述数据点的高度值，并计算最大高度值与最小高度值的高度差值，以及判断所述高度差值是否小于障碍物判定阈值，所述障碍物判定阈值用于表征判定为障碍物的最小高度差值，以及当所述高度差值小于所述障碍物判定阈值时，判断该地图栅格的中心与所述极坐标栅格地图中心的距离是否大于距离阈值，以及若是，为该地图栅格添加地面标签，以及若否，判断所述最小高度值是否小于地面判定阈值，所述地面判定阈值用于表征判定为地面的最小高度差值，以及当所述最小高度值小于所述地面判定阈值时，为该地图栅格添加地面标签，以及根据添加有所述地面标签的全部所述地图栅格内包含的数据点生成当前帧地面点云数据。

优选的，所述平面拟合模块，具体用于：

判断所述当前帧地面点云中包含数据点的数量是否大于拟合点阈值，以及若否，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角，以及若是，采用梯度下降算法对所述当前帧地面点云进行拟合，并计算拟合误差值，以及判断所述拟合误差值是否小于拟合误差阈值，以及当所述拟合误差值小于所述拟合误差阈值时，判定拟合成功，并计算当前拟合平面倾斜角度，以及判断是否存在上一历史路面补偿角，并根据判断结果以及所述当前拟合平面倾斜角度生成相应当前路面补偿角，以及当所述拟合误差值不小于所述拟合误差阈值时，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角。

优选的，所述外参数补偿模块，具体用于：

根据所述当前路面补偿角和所述当前车辆位姿数据计算外参数补偿量，以及依据所述外参数补偿量对所述当前传感器外参数进行补偿。

相较于现有技术，本发明实现的有益效果为：

本发明公开一种车载三维激光传感器的外参数补偿方法及装置，该方法首先根据当前传感器外参数计算当前外参数旋转平移矩阵，然后利用当前外参数旋转平移矩阵将车载三维激光传感器发送的当前帧点云数据从传感器坐标系下变换至车体坐标系下，进而提取车体坐标系下的当前帧地面点云数据并进行平面拟合，从而根据拟合结果以及惯性测量单元发送的当前车辆位姿数据对当前传感器外参数进行补偿。

这就实现了利用单帧点云数据以及车辆位姿数据对传感器外参数进行补偿，从而降低传感器外参数的误差，使得变换到车体坐标系下的点云数据相对地面逐渐稳定，提高了后续环境感知的准确度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例提供的车载三维激光传感器的外参数补偿方法的方法流程图；

图2为本发明实施例提供的车载三维激光传感器的外参数补偿方法的部分方法流程图；

图3为本发明实施例提供的车载三维激光传感器的外参数补偿方法的又一部分方法流程图；

图4为本发明实施例提供的车载三维激光传感器的外参数补偿方法的再一部分方法流程图；

图5为本发明实施例提供的车载三维激光传感器的外参数补偿方法的再一部分方法流程图；

图6为本发明实施例提供的车载三维激光传感器的外参数补偿方法的再一部分方法流程图；

图7为本发明实施例提供的车载三维激光传感器的外参数补偿装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

车载三维激光传感器通常安装于车辆上部，惯性测量单元设置于车体内部，车载三维激光传感器和惯性测量单元均与车身固联，而车身通过悬架与车轮相连，车轮与路面接触，而惯性测量单元的测量对象为车身。

定义坐标系如下：

传感器坐标系oxyz：原点o位于车载三维激光传感器中心，x、y、z轴分别为车载三维激光传感器定义的遵循车载三维激光传感器底座的x、y、z方向；

车体坐标系OXYZ：原点O位于车体后轴中心在地面的投影点，X、Y、Z轴分别执行车身车头方向、车左侧方向以和垂直于车身平面竖直向上的方向。

涉及平面：

A平面：大地水平面，也就是惯性测量单元中所定义的俯仰角、横滚角以及偏航角均为0°的平面；

B平面：车辆当前处于位置的路面；

C平面：车身平面，也就是惯性测量单元的安装面。

涉及角度，以俯仰角为例进行说明，横滚角同理：

a：B平面与A平面的夹角；

b：C平面与B平面的夹角。

由于车辆行驶于路面，偏航角的标定对后续的旋转平移矩阵以及环境感知并不存在影响，因此，对于传感器外参数的补偿仅涉及俯仰角和横滚角，以俯仰角为例，横滚角同理：

按照标定的传感器外参数，可将车载三维激光传感器采集的单帧点云数据由传感器坐标系oxyz变换至车体坐标系OXYZ，而在此车体坐标系OXYZ中路面是C平面，而实际由于悬架变换或者路面颠簸等原因，车体坐标系OXYZ中的路面与实际路面并不重合，实际路面为B平面，因此，发生了角度b的偏差，惯性测量单元测定的车身角度为车身平面与大地水平面的绝对夹角，也就是A平面和C平面的夹角，可见角度b为需要计算和补偿的角度。

本发明中涉及的车载三维激光传感器在竖直方向以一定角度间隔排布多组激光发射接收器，每一组激光器可利用飞行时间测距原理测定激光路径前方障碍物距离。车载三维激光传感器固联于一个受电机驱动绕z轴旋转的结构上，电机转动带动激光发射接收器一同绕z轴转动并测量，以完成对周围环境的三维扫描，扫描结构以三维点云形式返回。

本发明实施例公开一种车载三维激光传感器的外参数补偿方法，方法流程图如图1所示，包括如下步骤：

S10，根据车载三维激光传感器的当前传感器外参数计算当前外参数旋转平移矩阵；

在执行步骤S101的过程中，在首次补偿车载三维激光传感器的外参数时，当前传感器外参数为预先标定的传感器外参数，而非首次补偿时，当前传感器外参数可为上一次补偿后的传感器外参数，也可为其他传感器外参数，具体可根据实际需要具体设置，本实施例不做任何限定；

例如，假设当前传感器外参数为(x_L,y_L,z_L,yaw_L,pitch_L,roll_L)，其中，x_L、y_L、z_L、yaw_L、pitch_L和roll_L分别表示车载三维激光传感器在车体坐标系下的三维坐标、偏航角、俯仰角和横滚角，可利用三维刚体旋转平移变换算法计算相应的当前外参数旋转平移矩阵，计算公式如公式(1)所示过程如下：

其中，为当前外参数旋转平移矩阵，R为3×3的旋转矩阵，T为3×1的平移矩阵，具体的：

R₀₀＝cos(yaw_L)*cos(pitch_L)

R₀₁＝cos(yaw_L)*sin(pitch_L)*sin(roll_L)-sin(yaw_L)*cos(roll_L)

R₀₂＝sin(yaw_L)*sin(roll_L)+cos(yaw_L)*sin(pitch_L)*cos(roll_L)

R₁₀＝sin(yaw_L)*cos(pitch_L)

R₁₁＝cos(yaw_L)*cos(roll_L)+sin(yaw_L)*sin(pitch_L)*sin(roll_L)

R₁₂＝sin(yaw_L)*sin(pitch_L)*cos(roll_L)-cos(yaw_L)*sin(roll_L)

R₂₀＝-sin(pitch_L)

R₂₁＝cos(pitch_L)*sin(roll_L)

R₂₂＝cos(pitch_L)*cos(roll_L)

T₀＝x_L

T₁＝y_L

T₂＝z_L。

S20，当接收到车载三维激光传感器发送的当前帧点云数据以及惯性测量单元发送的当前车辆位姿数据时，根据当前外参数旋转平移矩阵将当前帧点云数据从传感器坐标系变换至车体坐标系下；

在执行步骤S20的过程中，车载三维激光传感器发送的当前帧点云数据为一组包含有多个数据点坐标数据的序列，并且每个数据点的坐标数据包含有该数据点在传感器坐标系下的三维坐标，而惯性测量单元发送的当前车辆位姿数据用于表征车辆当前的位置和姿态角，所述位置包括经度、纬度和海拔高度，所述姿态角包括俯仰角、横滚角以及偏航角，本实施例中涉及姿态角，假设每个数据点在传感器坐标系下的三维坐标为(x_pi,y_pi,z_pi)，当前车辆位姿数据中的姿态角为(yaw_V,pitch_V,roll_V)，其中，yaw_V、pitch_V和roll_V分别为车辆相对于大地水平面，就是在大地坐标系中的偏航角、俯仰角和横滚角；

利用上述步骤S101计算得到的当前外参数旋转平移矩阵可按照如下公式(2)将当前帧点云数据中每个数据点从传感器坐标系变换至车体坐标系下：

其中，X_pi、Y_pi和Z_pi分别为数据点在车体坐标系下的三维坐标。

30，从车体坐标系下的当前帧点云数据中提取当前帧地面点云数据；

在执行步骤S103的过程中，单帧点云数据中包含单帧障碍物点云数据以及单帧非障碍物点云数据，其中，单帧非障碍物点云数据中包含单帧路面点云数据，因此，可从车体坐标系下的当前帧点云数据中提取当前帧路面点云数据；

在具体实现过程中，步骤S30“从车体坐标系下的当前帧点云数据中提取当前帧地面点云数据”可以具体采用以下步骤，方法流程图如图2所示：

S301，将车体坐标系下的当前帧点云数据中包含的各个数据点按照位置坐标投影到极坐标栅格地图上，所述极坐标栅格地图被预先划分为至少一个地图栅格；

S302，对于每一个地图栅格，获取投影到该地图栅格的各个数据点的高度值，并计算最大高度值与最小高度值的高度差值；

S303，判断高度差值是否小于障碍物判定阈值，所述障碍物判定阈值用于表征判定为障碍物的最小高度差值；若是，则执行步骤S304；

在执行步骤S303的过程中，每一个地图栅格可设置相应的障碍物判定阈值，并且，障碍物判定阈值与该地图栅格中心与极坐标栅格地图中心的距离以及该地图栅格的角度分辨率有关，其中，极坐标栅格地图中心也就是车体坐标系原点，障碍物判定阈值的计算可根据如下公式(3)进行计算：

TH_ob＝(0.15+Dist*DeltaAngle*0.06)m (3)

其中，TH_ob为障碍物判定阈值，Dist为该地图栅格中心与极坐标栅格地图中心的距离，DeltaAngle为该地图栅格的角度分辨率。

S304，判断该地图栅格的中心与极坐标栅格地图中心的距离是否大于距离阈值；若是，则执行步骤S305；若否，则执行步骤S306；

S305，为该地图栅格添加地面标签；

S306，判断最小高度值是否小于地面判定阈值，所述地面判定阈值用于表征判定为地面的最小高度差值；若是，则执行步骤S307；

在执行步骤S306的过程中，每一个地图栅格可设置相应的地面判定阈值，并且，与该地图栅格中心与极坐标栅格地图中心的距离，也就是车体坐标系原点有关，地面判定阈值的计算可根据如下公式(4)进行计算：

TH_gnd＝(0.15+Dist*0.04)m (4)

其中，TH_gnd为地面判定阈值，Dist为该地图栅格中心与极坐标栅格地图中心的距离。

S307，为该地图栅格添加地面标签；

S308，根据添加有地面标签的全部地图栅格内包含的数据点生成当前帧地面点云数据。

需要说明的是，当高度差值不小于障碍物判定阈值时，可为该地图栅格添加障碍物标签。

还需要说明的是，当最小高度值不小于地面判定阈值时，可为该地图栅格添加障碍物标签。

S40，对当前帧地面点云数据进行平面拟合，并根据拟合结果以及当前车辆位姿数据确定当前路面补偿角；

在具体实现过程中，步骤S40“对当前帧地面点云数据进行平面拟合，并根据拟合结果以及当前车辆位姿数据确定当前路面补偿角”可以具体采用以下步骤，方法流程图如图3所示：

S401，判断当前帧地面点云中包含数据点的数量是否大于拟合点阈值；若否，则执行步骤S402；若是，则执行步骤S403；

S402，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角；

在执行步骤S402的过程中，判断是否存在上一历史路面补偿角，也就是判断是本次拟合是否为第一次拟合，进一步根据判断结果生成相应的当前路面补偿角。

S403，采用梯度下降算法对当前帧地面点云进行拟合，并计算拟合误差值；

在执行步骤S403的过程中，预先定义拟合函数和代价函数，例如，定义拟合函数形式为z＝Ax+By+C，其中，拟合参数A和B的初始值均为0，C的初始值为当前帧地面点云中数据点的平均高度值，假设采用样本点到拟合平面的平均高度差平方作为代价函数，也就是拟合误差值

在一次拟合过程中，存在多种拟合可能，对于每一个拟合可能完成从而调整拟合参数值之后，可根据定义的代价函数计算任意数据点的拟合误差值其中，A'、B'、和C'为调整后的参数值，然后，选取拟合误差值最小的拟合可能，然后进行下一次拟合，直至拟合误差值达到最小，为并记录此时的拟合参数值A_F、B_F和C_F。

S404，判断拟合误差值是否小于拟合误差阈值；若是，则执行步骤S405；若否，则执行步骤S407；

S405，判定拟合成功，并计算当前拟合平面倾斜角度；

在执行步骤S405的过程中，可根据步骤S403得到的拟合参数值A_F、B_F和C_F计算拟合平面倾斜角度，由于车辆行驶于路面，偏航角的标定对后续的旋转平移矩阵以及环境感知并不存在影响，因此，对于传感器外参数的补偿仅涉及俯仰角和横滚角，可采用如下公式(5)分别计算当前拟合平面倾斜角度中的当前拟合平面倾斜俯仰角和当前拟合平面倾斜横滚角：

pitch_P＝arctan(A_F) (5)

roll_P＝-arctan(B_F)

其中，pitch_P为当前拟合平面倾斜俯仰角，roll_P为当前拟合平面倾斜横滚角。

S406，判断是否存在上一历史路面补偿角，并根据判断结果以及当前拟合平面倾斜角度生成相应当前路面补偿角；

在具体实现过程中，步骤S406中“判断是否存在上一历史路面补偿角，并根据判断结果以及当前拟合平面倾斜角度生成相应当前路面补偿角”可以具体采用以下步骤，方法流程图如图4所示：

S1001，判断是否存在上一历史路面补偿角；若否，则执行步骤S1002；若是，则执行步骤S1003；

S1002，将当前拟合平面倾斜角度确定为当前路面补偿角；

S1003，根据历史路面补偿角对当前拟合平面倾斜角度进行低通滤波，得到当前路面补偿角；

在执行步骤S1003的过程中，可利用一阶数字低通滤波器对当前拟合平面倾斜角度进行低通滤波，当前路面补偿角可根据如下公式(6)进行计算：

pitch_R＝pitch_R'*(1-β)+pitch_P*β (6)

roll_R＝pitch_R'*(1-β)+pitch_P*β

其中，pitch_R为当前路面俯仰补偿角，pitch_R'为上一历史路面俯仰补偿角，roll_R为当前路面横滚补偿角，pitch_R'为上一历史路面横滚补偿角，并且，β为预先设置的滤波系数。

S407，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角；

在具体实现过程中，步骤S402以及步骤S407中“判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角”可以具体采用以下步骤，方法流程图如图5所示：

S1004，判断是否存在上一历史路面补偿角；若否，则执行步骤S1005；若是，则执行步骤S1006；

S1005，确定当前路面补偿角为0；

S1006，判断当前车速是否为0；若是，则执行步骤S1007；若否，则执行步骤S1008；

S1007，将历史路面补偿角确定为当前路面补偿角；

S1008，根据当前车辆位姿数据对历史路面补偿角进行衰减，得到当前路面补偿角；

在执行步骤S1008的过程中，当前车辆位姿数据中包含车辆相对于大地水平面俯仰角pitch_V和横滚角roll_V，可采用如下公式(7)计算当前路面补偿角：

pitch_R＝pitch_R'+(pitch_V-pitch_R')*α (7)

roll_R＝roll_R'+(roll_V-roll_R')*α

其中，pitch_R为当前路面俯仰补偿角，pitch_R'为上一历史路面俯仰补偿角，roll_R为当前路面横滚补偿角，pitch_R'为上一历史路面横滚补偿角，并且，α为预先设置的衰减系数。

S50，根据当前路面补偿角和当前车辆位姿数据对当前传感器外参数进行补偿；

在具体实现过程中，步骤S50“根据当前路面补偿角和当前车辆位姿数据对当前传感器外参数进行补偿”可以具体采用以下步骤，方法流程图如图6所示：

S501，根据当前路面补偿角和当前车辆位姿数据计算外参数补偿量；

在执行步骤S501的过程中，可采用如下公式(8)计算外参数补偿量：

其中，pitch_C为俯仰角补偿量，roll_C为横滚角补偿量。

S502，依据外参数补偿量对当前传感器外参数进行补偿；

在执行步骤S502的过程中，可采用如下公式(9)对当前传感器外参数进行补偿：

其中，pitch_CL为补偿后的俯仰角，roll_CL为补偿后的横滚角。

以上步骤S301～步骤S308仅仅是本申请实施例公开的步骤S30“从车体坐标系下的当前帧点云数据中提取当前帧地面点云数据”过程的一种优选的实现方式，有关此过程的具体实现方式可根据自己的需求任意设置，在此不做限定。

以上步骤S401～步骤S407仅仅是本申请实施例公开的步骤S40“对当前帧地面点云数据进行平面拟合，并根据拟合结果以及当前车辆位姿数据确定当前路面补偿角”过程的一种优选的实现方式，有关此过程的具体实现方式可根据自己的需求任意设置，在此不做限定。

以上步骤S1001～步骤S1003仅仅是本申请实施例公开的步骤S406中“判断是否存在上一历史路面补偿角，并根据判断结果以及当前拟合平面倾斜角度生成相应当前路面补偿角”过程的一种优选的实现方式，有关此过程的具体实现方式可根据自己的需求任意设置，在此不做限定。

以上步骤S1004～步骤S1008仅仅是本申请实施例公开的步骤S402以及步骤S407中“判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角”过程的一种优选的实现方式，有关此过程的具体实现方式可根据自己的需求任意设置，在此不做限定。

以上步骤S501～步骤S502仅仅是本申请实施例公开的步骤S50“根据当前路面补偿角和当前车辆位姿数据对当前传感器外参数进行补偿”过程的一种优选的实现方式，有关此过程的具体实现方式可根据自己的需求任意设置，在此不做限定。

本发明实施例公开的车载三维激光传感器的外参数补偿方法，实现了利用单帧点云数据以及车辆位姿数据对传感器外参数进行补偿，从而降低传感器外参数的误差，使得变换到车体坐标系下的点云数据相对地面逐渐稳定，提高了后续环境感知的准确度。

基于上述实施例提供的车载三维激光传感器的外参数补偿方法，本发明又一实施例公开一种车载三维激光传感器的外参数补偿装置，结构示意图如图7所示，包括：计算模块10、变换模块20、提取模块30、平面拟合模块40和外参数补偿模块50；

计算模块10，用于根据车载三维激光传感器的当前传感器外参数计算当前外参数旋转平移矩阵；

变换模块20，用于当接收到车载三维激光传感器发送的当前帧点云数据以及惯性测量单元发送的当前车辆位姿数据时，根据当前外参数旋转平移矩阵将当前帧点云数据从传感器坐标系变换至车体坐标系下；

提取模块30，用于从车体坐标系下的当前帧点云数据中提取当前帧地面点云数据；

平面拟合模块40，用于对当前帧地面点云数据进行平面拟合，并根据拟合结果以及当前车辆位姿数据确定当前路面补偿角；

外参数补偿模块50，用于根据当前路面补偿角和当前车辆位姿数据对当前传感器外参数进行补偿。

可选的，提取模块30，具体用于：

将车体坐标系下的当前帧点云数据中包含的各个数据点按照位置坐标投影到极坐标栅格地图上，所述极坐标栅格地图被预先划分为至少一个地图栅格，以及对于每一个地图栅格，获取投影到该地图栅格的各个数据点的高度值，并计算最大高度值与最小高度值的高度差值，以及判断高度差值是否小于障碍物判定阈值，所述障碍物判定阈值用于表征判定为障碍物的最小高度差值，以及当高度差值小于障碍物判定阈值时，判断该地图栅格的中心与极坐标栅格地图中心的距离是否大于距离阈值，以及若是，为该地图栅格添加地面标签，以及若否，判断最小高度值是否小于地面判定阈值，所述地面判定阈值用于表征判定为地面的最小高度差值，以及当最小高度值小于地面判定阈值时，为该地图栅格添加地面标签，以及根据添加有地面标签的全部地图栅格内包含的数据点生成当前帧地面点云数据。

可选的，平面拟合模块40，具体用于：

判断当前帧地面点云中包含数据点的数量是否大于拟合点阈值，以及若否，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角，以及若是，采用梯度下降算法对当前帧地面点云进行拟合，并计算拟合误差值，以及判断拟合误差值是否小于拟合误差阈值，以及当拟合误差值小于拟合误差阈值时，判定拟合成功，并计算当前拟合平面倾斜角度，以及判断是否存在上一历史路面补偿角，并根据判断结果以及当前拟合平面倾斜角度生成相应当前路面补偿角，以及当拟合误差值不小于拟合误差阈值时，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角。

可选的，外参数补偿模块50，具体用于：

根据当前路面补偿角和当前车辆位姿数据计算外参数补偿量，以及依据外参数补偿量对当前传感器外参数进行补偿。

本发明实施例公开的车载三维激光传感器的外参数补偿装置，实现了利用单帧点云数据以及车辆位姿数据对传感器外参数进行补偿，从而降低传感器外参数的误差，使得变换到车体坐标系下的点云数据相对地面逐渐稳定，提高了后续环境感知的准确度。

以上对本发明所提供的一种车载三维激光传感器的外参数补偿方法及装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备所固有的要素，或者是还包括为这些过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (8)

1.一种车载三维激光传感器的外参数补偿方法，其特征在于，包括：

根据车载三维激光传感器的当前传感器外参数计算当前外参数旋转平移矩阵；

当接收到所述车载三维激光传感器发送的当前帧点云数据以及惯性测量单元发送的当前车辆位姿数据时，根据所述当前外参数旋转平移矩阵将所述当前帧点云数据从传感器坐标系变换至车体坐标系下；

从所述车体坐标系下的所述当前帧点云数据中提取当前帧地面点云数据；

对所述当前帧地面点云数据进行平面拟合，并根据拟合结果以及所述当前车辆位姿数据确定当前路面补偿角；

根据所述当前路面补偿角和所述当前车辆位姿数据对所述当前传感器外参数进行补偿；

其中，所述从所述车体坐标系下的所述当前帧点云数据中提取当前帧地面点云数据，包括：

将所述车体坐标系下的所述当前帧点云数据中包含的各个数据点按照位置坐标投影到极坐标栅格地图上，所述极坐标栅格地图被预先划分为至少一个地图栅格；

对于每一个所述地图栅格，获取投影到该地图栅格的各个所述数据点的高度值，并计算最大高度值与最小高度值的高度差值；

判断所述高度差值是否小于障碍物判定阈值，所述障碍物判定阈值用于表征判定为障碍物的最小高度差值；

当所述高度差值小于所述障碍物判定阈值时，判断该地图栅格的中心与所述极坐标栅格地图中心的距离是否大于距离阈值；

若是，为该地图栅格添加地面标签；

若否，判断所述最小高度值是否小于地面判定阈值，所述地面判定阈值用于表征判定为地面的最小高度差值；

当所述最小高度值小于所述地面判定阈值时，为该地图栅格添加地面标签；

根据添加有所述地面标签的全部所述地图栅格内包含的数据点生成当前帧地面点云数据。

2.根据权利要求1所述的方法，其特征在于，所述对所述当前帧地面点云数据进行平面拟合，并根据拟合结果以及所述当前车辆位姿数据确定当前路面补偿角，包括：

判断所述当前帧地面点云中包含数据点的数量是否大于拟合点阈值；

若否，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角；

若是，采用梯度下降算法对所述当前帧地面点云进行拟合，并计算拟合误差值；

判断所述拟合误差值是否小于拟合误差阈值；

当所述拟合误差值小于所述拟合误差阈值时，判定拟合成功，并计算当前拟合平面倾斜角度；

判断是否存在上一历史路面补偿角，并根据判断结果以及所述当前拟合平面倾斜角度生成相应当前路面补偿角；

当所述拟合误差值不小于所述拟合误差阈值时，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角。

3.根据权利要求2所述的方法，其特征在于，所述判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角，包括：

判断是否存在上一历史路面补偿角；

若否，确定当前路面补偿角为0；

若是，判断当前车速是否为0；

当所述当前车速为0时，将所述历史路面补偿角确定为当前路面补偿角；

当所述当前车速不为0时，根据所述当前车辆位姿数据对所述历史路面补偿角进行衰减，得到当前路面补偿角。

4.根据权利要求2所述的方法，其特征在于，所述判断是否存在上一历史路面补偿角，并根据判断结果以及所述当前拟合平面倾斜角度生成相应当前路面补偿角，包括：

判断是否存在上一历史路面补偿角；

若否，将所述当前拟合平面倾斜角度确定为当前路面补偿角；

若是，根据所述历史路面补偿角对所述当前拟合平面倾斜角度进行低通滤波，得到当前路面补偿角。

5.根据权利要求1所述的方法，其特征在于，所述根据所述当前路面补偿角和所述当前车辆位姿数据对所述当前传感器外参数进行补偿，包括：

根据所述当前路面补偿角和所述当前车辆位姿数据计算外参数补偿量；

依据所述外参数补偿量对所述当前传感器外参数进行补偿。

6.一种车载三维激光传感器的外参数补偿装置，其特征在于，包括：计算模块、变换模块、提取模块、平面拟合模块和外参数补偿模块；

所述计算模块，用于根据车载三维激光传感器的当前传感器外参数计算当前外参数旋转平移矩阵；

所述变换模块，用于当接收到所述车载三维激光传感器发送的当前帧点云数据以及惯性测量单元发送的当前车辆位姿数据时，根据所述当前外参数旋转平移矩阵将所述当前帧点云数据从传感器坐标系变换至车体坐标系下；

所述提取模块，用于从所述车体坐标系下的所述当前帧点云数据中提取当前帧地面点云数据；

所述平面拟合模块，用于对所述当前帧地面点云数据进行平面拟合，并根据拟合结果以及所述当前车辆位姿数据确定当前路面补偿角；

所述外参数补偿模块，用于根据所述当前路面补偿角和所述当前车辆位姿数据对所述当前传感器外参数进行补偿；

其中，所述提取模块，具体用于：

将所述车体坐标系下的所述当前帧点云数据中包含的各个数据点按照位置坐标投影到极坐标栅格地图上，所述极坐标栅格地图被预先划分为至少一个地图栅格，以及对于每一个所述地图栅格，获取投影到该地图栅格的各个所述数据点的高度值，并计算最大高度值与最小高度值的高度差值，以及判断所述高度差值是否小于障碍物判定阈值，所述障碍物判定阈值用于表征判定为障碍物的最小高度差值，以及当所述高度差值小于所述障碍物判定阈值时，判断该地图栅格的中心与所述极坐标栅格地图中心的距离是否大于距离阈值，以及若是，为该地图栅格添加地面标签，以及若否，判断所述最小高度值是否小于地面判定阈值，所述地面判定阈值用于表征判定为地面的最小高度差值，以及当所述最小高度值小于所述地面判定阈值时，为该地图栅格添加地面标签，以及根据添加有所述地面标签的全部所述地图栅格内包含的数据点生成当前帧地面点云数据。

7.根据权利要求6所述的装置，其特征在于，所述平面拟合模块，具体用于：

判断所述当前帧地面点云中包含数据点的数量是否大于拟合点阈值，以及若否，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角，以及若是，采用梯度下降算法对所述当前帧地面点云进行拟合，并计算拟合误差值，以及判断所述拟合误差值是否小于拟合误差阈值，以及当所述拟合误差值小于所述拟合误差阈值时，判定拟合成功，并计算当前拟合平面倾斜角度，以及判断是否存在上一历史路面补偿角，并根据判断结果以及所述当前拟合平面倾斜角度生成相应当前路面补偿角，以及当所述拟合误差值不小于所述拟合误差阈值时，判定拟合失败，判断是否存在上一历史路面补偿角，并根据判断结果生成相应当前路面补偿角。

8.根据权利要求6所述的装置，其特征在于，所述外参数补偿模块，具体用于：

根据所述当前路面补偿角和所述当前车辆位姿数据计算外参数补偿量，以及依据所述外参数补偿量对所述当前传感器外参数进行补偿。