CN109975792A

CN109975792A - 基于多传感器融合矫正多线激光雷达点云运动畸变的方法

Info

Publication number: CN109975792A
Application number: CN201910335648.7A
Authority: CN
Inventors: 彭育辉; 张剑锋
Original assignee: Fuzhou University
Current assignee: Fuzhou University
Priority date: 2019-04-24
Filing date: 2019-04-24
Publication date: 2019-07-05
Anticipated expiration: 2039-04-24
Also published as: CN109975792B

Abstract

本发明涉及汽车无人驾驶技术领域，具体为一种基于融合多种传感器矫正多线激光雷达点云运动畸变的方法，根据全局坐标系和激光雷达某帧起始点坐标系之间的旋转关系将位移畸变变换至起始点坐标系，再将激光雷达局部坐标系下的点变换至起始点坐标系，得到了在某帧起始点坐标系下的点云全部的点和相应的位移畸变。通过在三维点坐标上补偿位移畸变来矫正点云数据。

Description

基于多传感器融合矫正多线激光雷达点云运动畸变的方法

技术领域

本发明涉及汽车无人驾驶技术领域，具体为一种基于融合多种传感器矫正多线激光雷达点云运动畸变的方法。

背景技术

实现汽车无人驾驶上应用的传感器种类繁多，常见的有超声波雷达、毫米波雷达、单目相机、双目相机、激光雷达等。其中，激光雷达以其探测距离远，测量精度高，不易受外界光照变化影响等优势在各类传感器中脱颖而出。相对于超声波雷达，激光雷达能够获得更远的物体信息并且角度信息也更为精确；相对于相机传感器，激光雷达可以直接得到物体的距离信息且不受外界光照的影响，在室外环境下表现更加稳定。因此，激光雷达在智能驾驶领域中备受青睐。激光雷达可分为单线的二维激光雷达和多线的三维激光雷达，但二者的工作原理大同小异，都是通过向目标物体发射激光束，利用目标反射回来的信号和发射信号做适当处理获得目标距离、反射强度等信息。但是激光雷达作为汽车智能驾驶的传感器与汽车刚性连接，随着汽车的运动其坐标系不断变化，导致两次接收的数据不在同一个坐标系下，产生点云数据的偏移，因此需要对获得的点云数据进行运动畸变补偿。

目前，针对多线激光雷达因运动发生畸变的问题往往不做处理或者只使用惯性导航单元（IMU）计算激光雷达的运动，再对点云数据进行运动补偿。但是由于惯性导航单元无法直接获得位移信息，只能通过加速度的二次积分获得位移，所以在长时间工作下累计误差大。本发明针对汽车无人驾驶中激光雷达点云数据发生畸变的情况，提出了一种融合惯性导航单元和轮速传感器数据矫正激光雷达运动畸变的方法，可以稳定而有效地减小激光雷达点云数据的运动畸变。本发明为后续的目标跟踪、路径规划、地图构建、物体识别等算法提供更为精确的多线激光雷达的点云数据。

发明内容

本发明的目的是针对多线激光雷达在运动过程中不可避免地因为自身运动产生的运动畸变问题，提供一种基于融合多种传感器矫正多线激光雷达点云运动畸变的方法。

本发明的上述技术目的是通过以下技术方案得以实现的：基于多传感器融合矫正多线激光雷达点云运动畸变的方法，按如下步骤进行：

步骤S1，判断IMU的最新时间戳是否大于激光雷达当前帧某点的时间戳，若是，进入步骤S2，若否，进入步骤S3；

步骤S2，寻找激光雷达当前帧该点的时间戳附近两帧IMU的数据并利用IMU的数据进行插值获得激光雷达点在全局坐标系的运动信息，然后进入步骤S4；

步骤S3，将全局坐标系的IMU的运动信息赋予雷达激光点，然后进入步骤S4；

步骤S4，判断激光雷达当前帧该点是否是该帧点云的第一个点，若是，记录该点信息并建立起始点坐标系，然后进入步骤S5，若否，直接进入步骤S5；

步骤S5，将激光雷达当前帧点云各点在全局坐标系下的位移畸变变换至起始点坐标系，然后进入步骤S6；

步骤S6，将激光雷达该帧点云在局部坐标系下各点变换至全局坐标系再变换至起始点坐标系，然后进入步骤S7；

步骤S7，在三维点上矫正位移畸变。

作为对本发明的优选，步骤S3中IMU赋予雷达激光点的运动信息包括欧拉角、速度、位移。

作为对本发明的优选，步骤S1之前，对激光雷达点云数据进行预处理，预处理过程按如下步骤进行：

步骤S1.1，获取激光雷达点云数据；

步骤S1.2，去除激光雷达数据无效点；

步骤S1.3，确定感兴趣区域；

步骤S1.4，计算在局部坐标系下激光雷达起始扫描线和终止扫描线的水平角；

步骤S1.5，计算局部坐标系下激光雷达每个点的水平角；

步骤S1.6，判断是否收到IMU数据，若是，插值计算激光雷达每个点的时间戳，若否，则结束。

作为对本发明的优选，IMU的位移计算方式，按如下步骤进行：

步骤S2.1，判断IMU的时间戳是否大于轮速传感器的时间戳，若是，进入步骤S2.2，若否，进入步骤S2.3；

步骤S2.2，寻找IMU时间戳附近两帧轮速传感器的时间戳并将轮速传感器的速度进行插值，将结果作为IMU的观测值，然后进入步骤S2.4；

步骤S2.3，将轮速传感器的速度作为IMU的观测值，然后进入步骤S2.4；

步骤S2.4，将IMU的速度作为预测值，利用观测值和预测值做卡尔曼滤波，得到滤波后全局坐标系下IMU的速度，然后进入步骤S2.5；

步骤S2.5，再次积分获得全局坐标系下IMU的位移。

作为对本发明的优选，步骤S2.1之前，IMU数据中预处理车速度的计算方式按如下步骤进行：

步骤S3.1，判断是否是第一次获得IMU数据，若是，建立全局坐标系并结束，若否，进入步骤S3.2；

步骤S3.2，建立局部坐标系；

步骤S3.3，获取全局坐标系到局部坐标系的旋转矩阵；

步骤S3.4，去除局部坐标系下重力对IMU三轴加速度的影响；

步骤S3.5，将修正后的三轴加速度投影至全局坐标系；

步骤S3.6，判断IMU频率是否大于激光雷达频率，若是，积分获得全局坐标系下的预处理车速度并结束，若否，结束。

作为对本发明的优选，步骤S2.1之前，轮速传感器的车速计算方式按如下步骤进行：

步骤S4.1，获取轮速传感器信息；

步骤S4.2，设定轮速传感器阈值；

步骤S4.3，判断轮速传感器是否在阈值内，若是，进入步骤S4.4，若否，结束；

步骤S4.4，在单位时间内计算四个车轮的速度；

步骤S4.5，利用阿克曼转向原理得到汽车速度。

作为对本发明的优选，所述轮速传感器为霍尔式轮速传感器。

本发明的有益效果：通过各点的局部坐标系和起始点坐标系以及全局坐标系之间的变换关系，将位移畸变以及激光雷达点云变换至起始点坐标系，通过对运动畸变进行矫正得到更为精确的点云数据。

附图说明

图1为轮速传感器的车速计算流程图；

图2为惯性导航单元（IMU）的车速计算流程图；

图3为惯性导航单元（IMU）和轮速传感器数据融合图；

图4为激光雷达数据预处理流程图；

图5为激光雷达与惯性导航单元（IMU）数据融合流程图；

图6为数据流示意图；

图7为激光雷达点云运动畸变示意图。

具体实施方式

以下具体实施例仅仅是对本发明的解释，其并不是对本发明的限制，本领域技术人员在阅读完本说明书后可以根据需要对本实施例做出没有创造性贡献的修改，但只要在本发明的权利要求范围内都受到专利法的保护。

实施例，如图5所示，基于多传感器融合矫正多线激光雷达点云运动畸变的方法，按如下步骤进行：

步骤S3，将全局坐标系的IMU的运动信息赋予激光雷达点，然后进入步骤S4；

步骤S5，将激光雷达当前帧各点在全局坐标系下的位移畸变变换至起始点坐标系，然后进入步骤S6；

步骤S6，将激光雷达该帧点云在局部坐标系下的各点变换至全局坐标系再变换至起始点坐标系，然后进入步骤S7；

步骤S7，在三维点上矫正位移畸变。

因为惯性导航单元的数据获取频率高于激光雷达，所以需要进行时间对齐。如果惯性导航单元最新的时间戳小于激光雷达某帧某点的时间戳，则直接将惯性导航单元的欧拉角、速度、位移赋值给该点。如果惯性导航单元最新的时间戳大于激光雷达某帧某点的时间戳，则寻找该点时间戳前后两帧惯性导航单元的数据，利用三者之间的时间差对惯性导航单元运动信息进行插值并将结果赋予激光雷达点。如果赋值的点为该帧点云的第一个点时，则建立该帧的起始点坐标系。随后将各点位移和第一点做计算，得到在全局坐标下该帧点云的运动的畸变。通过各点的局部坐标系和起始点坐标系以及全局坐标系之间的变换关系，将位移畸变以及激光雷达点云变换至起始点坐标系，通过对运动畸变进行矫正得到更为精确的点云数据。

图6为数据处理的示意图，在工控机上利用ROS机器人操作系统(Robot OperatingSystem）获取激光雷达和惯性导航单元的数据。轮速传感器的数据先发送给整车控制器，再通过控制器局域网络(Controller Area Network, CAN)发送给工控机。

图7为运动畸变的示意图，如果激光雷达是静止的，它的靠下方的坐标系L_1不会变化，在一次旋转扫描后坐标系原点相对物体1和物体i的距离如左右两条长黑虚线所示，不会发生畸变。假设在一次扫描过程中，激光雷达由靠下方的坐标系L_1移动至靠上方的坐标系L_i，它扫描到物体1时在L_1坐标系下，距离为L1标注的左侧的长黑色虚线，它扫描到物体i时在L_i坐标系下，距离为Li标注的右侧的短黑色虚线，坐标系发生了偏移，不在同一个坐标系下，获得的物体坐标发生了畸变。

进一步，步骤S3中IMU赋予雷达激光点的运动信息可以包括欧拉角、速度、位移。又如图4所示，步骤S1之前，对激光雷达点云数据进行预处理，预处理过程可以按如下步骤进行：

步骤S1.1，获取激光雷达点云数据；

步骤S1.2，去除激光雷达数据无效点；

步骤S1.3，确定感兴趣区域；

步骤S1.5，计算局部坐标系下激光雷达每个点的水平角；

图4 为激光雷达数据预处理流程图，首先获取激光雷达数据，建立激光雷达局部坐标系，将雷达数据的无效点去除，并确定感兴趣区域。根据激光雷达扫描原理计算该帧点云的起始角和终止角。如果没有接收到惯性导航单元数据，则不作运动畸变矫正处理。如果接收到惯性导航单元数据，则将激光雷达视为匀速旋转，并计算该帧点云各点的旋转角，根据该帧点云的起始角、终止角、激光雷达的旋转周期以及该帧点云的时间戳，按照角度插值获得该帧各点的时间戳。

再进一步，IMU的位移计算方式，可以按如下步骤进行：

步骤S2.5，再次积分获得全局坐标系下IMU的位移。

图3为惯性导航单元和轮速传感器数据融合图，也即IMU的更准确稳定的位移计算方式，由于惯性导航单元无法直接获得速度信息，需要对加速度信息进行积分。随着运行时间的增长，惯性导航单元速度信息的误差逐渐增大，需要一个观测值来作矫正。而轮速传感器计算车速的原理决定了其不受运行时间长短的影响。此时选择使用惯性导航单元的速度信息作为估计值，轮速传感器的速度信息作为观测值，用卡尔曼滤波融合二者数据获得更加准确和稳定的速度信息。因为轮速传感器只能计算速率信息，没有方向，所以以惯性导航单元的三轴速度分配比例对轮速传感器速率进行分配，从而得到轮速传感器的三轴速度。因为惯性导航单元和轮速传感器的数据发布频率不同，需要进行时间上的对齐。当惯性导航单元的时间戳大于轮速传感器最新的时间戳时，直接以轮速传感器的速度作为观测值。反之则寻找在惯性导航单元时间戳前后的两帧轮速传感器速度，将通过时间插值获得的轮速传感器速度作为观测值，从而得到滤波后的三轴速度。对速度信息再次积分，获得全局坐标系下惯性导航单元的位移。

而在步骤S2.1之前，IMU数据中预处理车速度的计算方式可以按如下步骤进行：

步骤S3.2，建立局部坐标系；

步骤S3.3，获取全局坐标系到局部坐标系的旋转矩阵；

步骤S3.4，去除局部坐标系下重力对IMU三轴加速度的影响；

步骤S3.5，将修正后的三轴加速度投影至全局坐标系；

图2为惯性导航单元的车速计算流程图，首先获得惯性导航单元数据，包括时间戳、三轴加速度、四元数、角加速度。在第一次获取数据时建立全局坐标系，后续获得数据时建立局部坐标系。将惯性导航单元数据中的四元数转化为全局坐标系到局部坐标系的旋转矩阵。利用旋转矩阵去除重力加速度对三轴加速度的影响，得到局部坐标系下汽车运动的三轴加速度。再利用旋转矩阵将三轴加速度变换至全局坐标系下。我们的目的是利用惯性导航单元矫正激光雷达点云运动畸变，如果惯性导航单元的工作频率小于激光雷达，则无法实现目标，所以需要对惯性导航单元的时间间隔进行计算。当且仅当某一时刻相邻两帧惯性导航单元的时间间隔小于激光雷达工作的时间间隔时，才对惯性导航单元的三轴加速度进行积分，得到全局坐标系下的惯性导航单元的三轴速度信息。

步骤S2.1之前，轮速传感器的车速计算方式可以按如下步骤进行：

步骤S4.1，获取轮速传感器信息；

步骤S4.2，设定轮速传感器阈值；

步骤S4.4，在单位时间内计算四个车轮的速度；

步骤S4.5，利用阿克曼转向原理得到汽车速度。

图1为轮速传感器的车速计算流程图，轮速传感器分为两种，一种是磁电式轮速传感器，另一种为霍尔式轮速传感器。本发明选择霍尔式轮速传感器，因为霍尔式轮速传感器输出的电压信号幅值不受转速影响，响应频率高。首先获得轮速传感器信息，通过设置阈值防止轮速传感器信号异常。通过设置单位时间，利用车轮半径获得单位时间内单个车轮的速度，在获得四个车轮的速度后利用阿克曼转向原理获得汽车运动的速率。

本实施例针对汽车在无人行驶过程中由于激光雷达自身坐标系随着车体坐标系而移动，从而导致获得的点云发生运动畸变的情况，提出一种基于惯性导航单元、轮速传感器的多传感器融合矫正激光雷达点云运动畸变的方法，通过上述方法的运用，在具体实践中可以这样操作：

S1: 由轮速传感器获取汽车运动速率：利用霍尔式轮速传感器在设定的单位时间内获得的脉冲个数计算车轮的转速n，根据轮胎的半径r计算车轮速度v，在获取四个车轮的车速后，利用Ackermann-Jeantand转向模型计算整体汽车运动的速率。

S2：由惯性导航单元获得汽车运动速度：在第一次获得惯性导航单元数据时，建立全局坐标系W。在后续获得数据时，建立局部坐标系L_IMU_1~n。在建立坐标系时记录三轴加速度信息IMU_accx_1~n、IMU_accy_1~n、IMU_accz_1~n，四元数信息Q_IMU_1~n以及时间信息IMU_t_1~n，利用四元数获得此时局部坐标系相对于全局坐标系的旋转矩阵R_1~n，实现惯性导航单元重力加速度影响的去除，并将三轴加速度投影至全局坐标系下，得到惯性导航单元在全局坐标系下的加速度分量W_IMU_accx_1~n、W_IMU_accy_1~n、W_IMU_accz_1~n。惯性导航单元和激光雷达的工作频率不同，只有当惯性导航单元的频率大于激光雷达的频率时才能实现激光雷达运动畸变的矫正，所以计算连续两帧惯性导航单元数据的时间间隔(IMUt_n+1-IMUt_n)，当间隔小于激光雷达的间隔（由激光雷达的厂家设定）时，对惯性导航单元的三轴加速度进行积分W_IMU_v_n+1=W_IMU_v_n+W_IMU_accy_n*（t_n+1-t_n），从而获得全局坐标系下的三轴速度信息W_IMU_v_x、W_IMU_v_y、W_IMU_v_z。

S3：IMU速度信息的卡尔曼滤波：惯性导航单元只能提供三轴加速度信息，所以需要通过对时间积分才能获得速度信息。这就导致了在长时间工作下不可避免地发生误差发散。而轮速传感器的工作原理是在单位时间内计算脉冲数来获得车速，这表明其不受工作时间长短的影响。而惯性导航单元对加速度积分刚好可以转化为线性方程，所以使用卡尔曼滤波融合惯性导航单元和轮速传感器数据获得更为精确和稳定的速度信息。将惯性导航单元的速度作为估计值，加速度作为控制变量，测量精度作为估计值的高斯噪声，构造状态转移方程。由于轮速传感器得到的汽车速率没有方向信息，所以按照惯性导航单元的三轴速度比例分配轮速传感器所得的汽车速率，得到轮速传感器的三轴速度。将轮速传感器的速度作为观测值，轮速传感器的重复精度转化为观测值的高斯噪声，构造观测方程。在进行滤波前还需考虑两者工作频率不同的问题，如果惯性导航单元的时间戳大于轮速传感器最新的时间戳，直接将轮速传感器的速度作为卡尔曼滤波的观测值。如果惯性导航单元的时间戳小于轮速传感器最新的时间戳，则对轮速传感器的速度进行时间插值，将插值后的速度作为卡尔曼滤波的观测值。随后将滤波后的三轴速度再次积分，获得全局坐标系下惯性导航单元的的位移。

S4：激光雷达数据的预处理。在接收激光雷达数据后去除点云中的无效点。同时在全局坐标系下建立激光雷达的局部坐标系。因为激光雷达的点数较多，为了减小计算量，在激光雷达的局部坐标系内确定感兴趣区域。根据激光雷达扫描原理建立激光雷达的扫描模型，将激光雷达的旋转视为匀速，记录每帧数据的获取时间T_{_LINDAR_START}，根据点云三维坐标计算起始扫描线转角ORI_{_LINDAR_START}（- π， π）、终止扫描线转角ORI_{_LINDAR_END}（ π，3 π）以及每个点的转角ORI_{_LINDAR_I}（- π，3 π），通过转角得到点的相对时间，从而获得每个点的时间戳。

S5：激光雷达运动畸变的矫正：因为惯性导航单元的工作频率远远高于激光雷达，所以需要进行时间上的对齐。即记录一段时间内的惯性导航单元数据，当最新的惯性导航单元数据的时间戳T_{IMU_LAST}小于激光雷达某帧某点的时间戳T_LINDAR-i时，直接将惯性导航单元的运动信息赋值给激光雷达该帧该点。当激光雷达某帧某点的时间戳小于惯性导航单元最新的时间戳时，在已经保存的惯性导航单元数据中寻找激光雷达某帧某点时间戳的前后最近两帧数据的时间T_{IMU_BACK}和T_{IMU_FRONT}。利用T_{IMU_BACK}小于T_LINDAR-i小于T_{IMU_FRONT}的关系，通过插值法计算得到全局坐标系下激光雷达某帧某点的运动信息。如果计算的点是激光雷达该帧的第一个点，则记录这点的运动信息，将其建立的坐标系视为激光雷达该帧数据的起始点坐标系。随后将先前插值计算的全局坐标系下激光雷达某帧某点的位移和某帧第一点的位移做计算，得到相对于第一点的位移畸变。随后根据全局坐标系和激光雷达某帧起始点坐标系之间的旋转关系将位移畸变变换至起始点坐标系，再将激光雷达局部坐标系下的点变换至起始点坐标系，便得到了在起始点坐标系下的某帧点云全部的点和相应的位移畸变。通过在三维点坐标上加上位移畸变来矫正点云畸变。

Claims

1.基于多传感器融合矫正多线激光雷达点云运动畸变的方法，其特征在于，按如下步骤进行：

步骤S2，寻找激光雷达当前帧该点时间戳附近两帧IMU的数据并利用IMU的数据进行插值获得激光雷达点在全局坐标系的运动信息，然后进入步骤S4；

步骤S5，将激光雷达当前帧点云各个点在全局坐标系下的位移畸变变换至起始点坐标系，然后进入步骤S6；

步骤S6，将在局部坐标系下的激光雷达该帧点云数据变换至全局坐标系再变换至起始点坐标系，然后进入步骤S7；

步骤S7，在三维点上矫正位移畸变。

2.根据权利要求1所述的基于多传感器融合矫正多线激光雷达点云运动畸变的方法，其特征在于，步骤S3中IMU赋予雷达激光点的运动信息包括欧拉角、速度、位移。

3.根据权利要求1所述的基于多传感器融合矫正多线激光雷达点云运动畸变的方法，其特征在于，步骤S1之前，对激光雷达点云数据进行预处理，预处理过程按如下步骤进行：

步骤S1.1，获取激光雷达点云数据；

步骤S1.2，去除激光雷达数据无效点；

步骤S1.3，确定感兴趣区域；

步骤S1.5，计算局部坐标系下激光雷达每个点的水平角；

4.根据权利要求2所述的基于多传感器融合矫正多线激光雷达点云运动畸变的方法，其特征在于，IMU的位移计算方式，按如下步骤进行：

步骤S2.5，再次积分获得全局坐标系下IMU的位移。

5.根据权利要求4所述的基于多传感器融合矫正多线激光雷达点云运动畸变的方法，其特征在于，步骤S2.1之前，IMU数据中预处理车速度的计算方式按如下步骤进行：

步骤S3.2，建立局部坐标系；

步骤S3.3，获取全局坐标系到局部坐标系的旋转矩阵；

步骤S3.4，去除局部坐标系下重力对IMU三轴加速度的影响；

步骤S3.5，将修正后的三轴加速度投影至全局坐标系；

6.根据权利要求4所述的基于多传感器融合矫正多线激光雷达点云运动畸变的方法，其特征在于，步骤S2.1之前，轮速传感器的车速计算方式按如下步骤进行：

步骤S4.1，获取轮速传感器信息；

步骤S4.2，设定轮速传感器阈值；

步骤S4.4，在单位时间内计算四个车轮的速度；

步骤S4.5，利用阿克曼转向原理得到汽车速度。

7.根据权利要求6所述的基于多传感器融合矫正多线激光雷达点云运动畸变的方法，其特征在于，所述轮速传感器为霍尔式轮速传感器。