CN113391300B - 一种基于imu的激光雷达三维点云实时运动补偿方法 - Google Patents

一种基于imu的激光雷达三维点云实时运动补偿方法 Download PDF

Info

Publication number
CN113391300B
CN113391300B CN202110559368.1A CN202110559368A CN113391300B CN 113391300 B CN113391300 B CN 113391300B CN 202110559368 A CN202110559368 A CN 202110559368A CN 113391300 B CN113391300 B CN 113391300B
Authority
CN
China
Prior art keywords
data
laser radar
point cloud
axis
imu
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110559368.1A
Other languages
English (en)
Other versions
CN113391300A (zh
Inventor
王军
赵子豪
李玉莲
申政文
潘在宇
刘欢
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China University of Mining and Technology CUMT
Original Assignee
China University of Mining and Technology CUMT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China University of Mining and Technology CUMT filed Critical China University of Mining and Technology CUMT
Priority to CN202110559368.1A priority Critical patent/CN113391300B/zh
Publication of CN113391300A publication Critical patent/CN113391300A/zh
Application granted granted Critical
Publication of CN113391300B publication Critical patent/CN113391300B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开了一种基于IMU的激光雷达三维点云实时运动补偿方法，首先，对用于本发明方法实现的激光雷达及IMU进行时间轴同步与空间坐标系统一，并按照时间戳顺序对激光雷达三维点云数据及IMU数据分别进行排序；其次，提出一种基于数据块划分的激光雷达三维点云旋转补偿方法，根据IMU输出激光雷达三维点云数据时序对每帧激光雷达三维点云数据进行数据块划分，根据以上数据块划分思想求得每个数据点相对于帧尾的三轴旋转变换矩阵R，并对激光雷达三维点云数据进行三轴旋转补偿；最后，根据旋转补偿后的点云数据帧估计出点云帧间运动量T，并对点云数据进行平移补偿。与传统方法相比，本方法具有更高的实时性与鲁棒性。

Description

一种基于IMU的激光雷达三维点云实时运动补偿方法
技术领域
本发明涉及到无人驾驶领域，具体为一种基于IMU的激光雷达三维点云实时运动补偿方法。
背景技术
在无人驾驶技术中，环境感知是其中重要的一环，大量传感器构成环境感知的重要组成部分。目前，广泛被应用环境感知的传感器主要是摄像头、毫米波雷达、激光雷达和超声波探测器。其中，激光雷达占有主导性的地位，主要通过激光头发射大量激光束，扫描到物体上反射回来再接收的原理，能够进行长距离高精度测量与静态、动态障碍物的检测任务。摄像机作为视觉传感器，能为无人车提供丰富的感知信息，缺点是不包含距离信息，无法做到精确测距。因此，相比于摄像头，激光雷达能够在无人驾驶环境感知中发挥重要作用。
常规的运动补偿方法都涉及到IMU的应用，但IMU仅仅在三轴角度变化估计上表现较好，三轴位置变化估计上却不尽人意，因此，设计一种实时性高又能对点云畸变进行各个方向补偿的方案变得尤为重要。
发明内容
本发明的目的是针对激光雷达在运动过程中产生的运动畸变的问题，提出了一种基于IMU的激光雷达三维点云实时运动补偿方法。
实现本发明目的的技术解决方案为：一种基于IMU的激光雷达三维点云实时运动补偿方法，具体步骤如下：
步骤S1，将激光雷达固定安装于无人车上，对激光雷达与IMU进行时间轴同步与空间坐标系统一，同步实时采集激光雷达三维点云数据与IMU数据，进入步骤S2。
步骤S2，按照时间戳顺序对采集的激光雷达三维点云数据与IMU数据分别进行排序，得到排序后的IMU数据与激光雷达三维点云数据，进入步骤S3。
步骤S3，根据排序后的IMU数据时序对每帧激光雷达三维点云数据进行数据块划分，通过IMU采样频率Fimu和激光雷达采样频率Flidar之比来计算数据块划分的分块数N＝Fimu/Flidar，且相邻数据块的时间间隔Δt为IMU采样周期，即Δt＝1/Fimu，默认同一数据块时间间隔内激光雷达三轴角速度与线性加速度不变，进入步骤S4。
步骤S4，利用上述划分后的数据块，计算每个数据块中三轴平均角速度、三轴旋转角度，结合每个数据块中激光雷达三维点云数据点相对于第一个点的时间戳的时间偏移，计算划分出的数据块与每个数据块的三轴旋转角度，然后求得三轴旋转变换矩阵R，实现对每个激光雷达数据点进行三轴旋转运动补偿，进入步骤S5。
步骤S5，利用得到的三轴旋转变换矩阵R对每帧激光雷达三维点云数据中的每一个点进行三轴旋转补偿，并将补偿后的点云坐标系统一变换到帧尾坐标系，输出旋转补偿后的数据，进入步骤S6。
步骤S6，将旋转运动补偿后的激光雷达三维点云数据分成两路输出，一路作为三轴平移运动补偿的源数据，另一路经过降采样、帧间运动量初步估计、扩展卡尔曼滤波完成三轴平移运动补偿；两路一起完成激光雷达三维点云数据的实时运动补偿。
作为对本发明的优选，步骤S1将激光雷达固定安装于无人车上，对激光雷达与IMU进行时间轴同步与空间坐标系统一，采集激光雷达三维点云数据与 IMU数据，按照如下步骤进行：
步骤S1.1，将IMU与激光雷达进行时间轴同步与空间坐标系统一，采集共同输入的数据，数据包括激光雷达三维点云、IMU三轴角速度、IMU三轴线性加速度、激光雷达与IMU数据时间戳等。
作为对本发明的优选，步骤S2按照时间戳顺序对采集到的激光雷达三维点云数据与IMU数据分别进行排序，得到排序后的IMU数据与激光雷达三维点云数据。
作为对本发明的优选，步骤S3根据排序后的IMU数据时序对每帧激光雷达三维点云数据进行数据块划分，通过IMU采样频率Fimu和激光雷达采样频率 Flidar之比来计算数据块划分的分块数N＝Fimu/Flidar，且相邻数据块的时间间隔Δt等于IMU采样周期，即Δt＝1/Fimu，默认同一数据块时间间隔内激光雷达三轴角速度与线性加速度不变，具体步骤如下：
步骤S3.1，利用排序后的激光雷达三维点云数据和IMU数据，记录激光雷达的采样频率Flidar和IMU的采样频率Fimu，计算每帧激光雷达三维点云数据的分块数N，且相邻数据块时间间隔Δt等于IMU采样周期1/Fimu，即Δt＝1/Fimu, 默认每个数据块内IMU三轴角速度与线性加速度不变：
Figure GDA0003404878100000031
其中：N为每帧激光雷达三维点云数据的分块数，Fimu为IMU的采集频率，Flidar为激光雷达的采样频率，进入步骤S3.2。
步骤S3.2，根据划分的数据块，计算输入的激光雷达三维点云数据中任一点 P所属的数据块位置：
Figure GDA0003404878100000032
其中：Blockn为点P所属数据块，Ptimestamp为当前点的时间戳，Ptimeheader为每帧中帧头时间戳，Δt为相邻数据块时间间隔。
作为对本发明的优选，步骤S4利用上述划分后的数据块，计算每个数据块中三轴平均角速度、三轴旋转角度，结合每个数据块中激光雷达三维点云数据点相对于第一个点的时间戳的时间偏移，计算划分出的数据块与每个数据块的三轴旋转角度，然后求得三轴旋转变换矩阵R，实现对每个激光雷达数据点进行三轴旋转运动补偿，具体步骤如下：
步骤S4.1，根据上一步骤S3.1得到划分后的数据块，计算每个数据块中的三轴平均角速度
Figure GDA0003404878100000033
Figure GDA0003404878100000034
其中：x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z 轴坐标值，
Figure GDA0003404878100000035
为每个数据块中的三轴平均角速度，ωn(x，y，z)为IMU 在tn时刻三轴旋转角速度，ωn+1(x，y，z)为IMU在tn+1时刻三轴旋转角速度，进入步骤S4.2。
步骤S4.2，根据三轴平均角速度计算三轴旋转角度三轴旋转角度θn：
Figure GDA0003404878100000036
其中：x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z 轴坐标值，
Figure GDA0003404878100000037
为每个数据块中的三轴平均角速度，Δt为相邻数据块时间间隔，进入步骤S4.3。
步骤S4.3，根据划分出的数据块与每个数据块的三轴旋转角度，对每个激光雷达三维点云数据点进行三轴旋转运动补偿，得到旋转矩阵R。
作为对本发明的优选，步骤S4.3，根据划分出的数据块，利用每个数据块的三轴旋转角度，对每个激光雷达三维点云数据点进行三轴旋转运动补偿，得到三轴旋转变换矩阵R，具体如下：
步骤S4.3.1，根据点P所属数据块的位置，计算点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和θsum；
θsum＝θn+1+θn+2+…+θend
其中：θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，θn+1、θn+2、…θend分别为点P所属激光雷达三维点云数据块到第n+1个数据块， n+2个数据块，…，最后一个数据块中三轴旋转角度，进入步骤S4.3.3。
步骤S4.3.2，计算点P相对于本数据块中最后一个点的三轴旋转角度
Figure GDA0003404878100000041
Figure GDA0003404878100000042
其中：
Figure GDA0003404878100000043
为点P相对于本数据块中最后一个点云数据的三轴旋转角度，ΔT 为点P到其所在的第n个数据块尾部的时间之差，
Figure GDA0003404878100000044
为每个数据的三轴平均角速度，Δt为相邻数据块的时间间隔，Ptimestamp为当前点的时间戳，进入步骤S4.3.3。
步骤S4.3.3，利用得到的θsum和
Figure GDA0003404878100000045
计算点P相对与帧尾三轴旋转角度θP：
Figure GDA0003404878100000046
其中：θP为点P到帧尾三轴旋转角度，θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，
Figure GDA0003404878100000047
为点P相对于本数据块中最后一个激光雷达三维点云数据的三轴旋转角度，进入步骤S4.3.4。
步骤S4.3.4，利用点P相对于帧尾三轴旋转角度，分别计算点P在x轴的旋转量Rroll、在y轴的旋转量Rpitch、在z轴的旋转量Ryaw：
Figure GDA0003404878100000048
Figure GDA0003404878100000049
Figure GDA00034048781000000410
其中：Ryaw为点P在z轴的旋转量，Rpitch为点P在y轴的旋转量，Rroll为点 P在x轴的旋转量，进入步骤S4.3.5。
步骤S4.3.5，计算点P相对于帧尾坐标系的旋转变换矩阵R：
R＝RyawRpitchRroll
作为本发明的优选，步骤S5利用得到的旋转变换矩阵R对每帧激光雷达三维点云数据中的每一个点进行三轴旋转补偿，并将补偿后的点云坐标系统一变换到帧尾坐标系，输出旋转补偿后的激光雷达三维点云数据，按照如下步骤进行；
步骤S5.1，利用旋转变换矩阵，将点P旋转变换到帧尾坐标系；
所述计算公式：
P′＝P·R
其中：P'为点P变换后的点云数据，点P为变换前的点云数据，R为旋转变换矩阵，进入步骤S5.2。
步骤S5.2，输出旋转补偿后的三维点云数据，即完成三轴旋转运动补偿。
作为对本发明的优选，步骤S6将旋转运动补偿后的激光雷达三维点云数据分成两路输出，一路作为三轴平移运动补偿的源数据，另一路经过降采样、帧间运动量初步估计、扩展卡尔曼滤波完成三轴平移运动补偿；两路一起完成激光雷达三维点云数据的实时运动补偿，按照如下步骤进行：
步骤S6.1，计算每个体素栅格内所有点云的重心，利用体素滤波对待配准点云进行降采样：
Figure GDA0003404878100000051
其中：cijk为体素内的重心，pi为体素内的数据点，k为体素内的点数，进入步骤S6.2。
步骤S6.2，利用Generalized-ICP算法进行帧间运动量初步估计，进入步骤 S6.3。
步骤S6.3，利用扩展卡尔曼滤波算法对配准结果进行优化，输出帧间位移矩阵T，进入步骤S6.4。
步骤S6.4，利用帧间位移矩阵T对每帧内所有激光雷达三维点云进行三轴平移运动补偿，最终将点云转换到帧尾坐标系：
P″＝P′+T
其中：P”为点P最终转换到帧尾坐标系对应的点，P'为点P经旋转矩阵变换的点，T为帧间位移矩阵。
本发明与现有技术相比，其显著优点在于：
现有方法多通过IMU直接测量角速度与线性加速度来降低运动畸变，默认 IMU在采样周期内的每帧激光雷达三维点云数据的角速度与线性加速度相等，并且假设IMU在匀速状态下很难实现，得到的旋转变换和平移变换会存在比较大的差。本发明是根据IMU输出数据时序对每帧激光雷达三维点云数据进行数据块划分，默认每个数据块中的三轴角速度与线性加速度相等，得到的三轴角速度与线性加速度更加准确，从而具备更高的实时性与鲁棒性。
不同于现有技术方案，本发明将旋转变换与平移变换分开单独计算，提出了一种基于IMU的激光雷达三维点云实时运动补偿方法。即先根据IMU输出数据时序对每帧激光雷达三维点云数据进行数据块划分，默认同一数据块时间间隔内激光雷达三轴角速度与线性加速度相等，计算每个数据点相对于帧尾的旋转矩阵 R，对激光雷达三维点云数据进行旋转运动补偿，再根据旋转补偿后的激光雷达三维点云数据帧估计出点云帧间运动量T，对激光雷达三维点云数据进行平移补偿。
附图说明
图1激光雷达在无人车安装方式示意图。
图2一种基于IMU的激光雷达三维点云实时运动补偿方法框图。
图3数据块划分示意图。
图4三轴旋转运动补偿方法流程图。
图5三维点云实时运动补偿实验场景图。
图6三维点云数据运动补偿前后效果对比图。
具体实施方式
下面结合具体实施方式对本发明做进一步详细的说明。
本发明采用的两台非重复扫描激光雷达(Livox Mid-100与Horizon)具有每次扫描轨迹不重复的特点，随着扫描时间的增加，所输出的三维点云视野覆盖率不断增大，静止扫描数秒后，视野覆盖率趋近100％，可充分反映出精确的环境细节信息。
如图2所示，本发明所述的基于IMU的激光雷达三维点云实时运动补偿方法，步骤如下：
步骤S1，将激光雷达固定安装于无人车上，对激光雷达与IMU进行时间轴同步与空间坐标系统一，同步实时采集激光雷达三维点云数据与IMU数据，进入步骤S2。
步骤S2，按照时间戳顺序对采集的激光雷达三维点云数据与IMU数据分别进行排序，得到排序后的IMU数据与激光雷达三维点云数据，进入步骤S3。
步骤S3，根据排序后的IMU数据时序对每帧激光雷达三维点云数据进行数据块划分，通过IMU采样频率Fimu和激光雷达采样频率Flidar之比来计算数据块划分的分块数N＝Fimu/Flidar，且相邻数据块的时间间隔Δt为IMU采样周期，即Δt＝1/Fimu，默认同一数据块时间间隔内激光雷达三轴角速度与线性加速度不变，进入步骤S4。
步骤S4，利用上述划分后的数据块，计算每个数据块中三轴平均角速度、三轴旋转角度，结合每个数据块中激光雷达三维点云数据点相对于第一个点的时间戳的时间偏移，计算划分出的数据块与每个数据块的三轴旋转角度，然后求得三轴旋转变换矩阵R，实现对每个激光雷达数据点进行三轴旋转运动补偿，进入步骤S5。
步骤S5，利用得到的三轴旋转变换矩阵R对每帧激光雷达三维点云数据中的每一个点进行三轴旋转补偿，并将补偿后的点云坐标系统一变换到帧尾坐标系，输出旋转补偿后的激光雷达三维点云数据，进入步骤S6。
步骤S6，将旋转运动补偿后的激光雷达三维点云数据分成两路输出，一路作为三轴平移运动补偿的源数据，另一路经过降采样、帧间运动量初步估计、扩展卡尔曼滤波完成三轴平移运动补偿；两路一起完成激光雷达三维点云数据的实时运动补偿。
作为对本发明的优选，步骤S1将激光雷达固定安装于无人车上，对激光雷达与IMU进行时间轴同步与空间坐标系统一，采集激光雷达三维点云数据与 IMU数据，按照如下步骤进行：
步骤S1.1，将IMU与激光雷达进行时间轴同步与空间坐标系统一，采集共同输入的数据，数据包括激光雷达三维点云、IMU三轴角速度、IMU三轴线性加速度、激光雷达与IMU数据时间戳等。
作为对本发明的优选，步骤S2按照时间戳顺序对采集到的激光雷达三维点云数据与IMU数据分别进行排序，得到排序后的IMU数据与激光雷达三维点云数据。
作为对本发明的优选，步骤S3根据排序后的IMU数据时序对每帧激光雷达三维点云数据进行数据块划分，通过IMU采样频率Fimu和激光雷达采样频率 Flidar之比来计算数据块划分的分块数N＝Fimu/Flidar，且相邻数据块的时间间隔Δt等于IMU采样周期，即Δt＝1/Fimu，默认同一数据块时间间隔内激光雷达三轴角速度与线性加速度不变，具体步骤如下：
步骤S3.1，利用排序后的激光雷达三维点云数据和IMU数据，记录激光雷达的采样频率Flidar和IMU的采样频率Fimu，计算每帧激光雷达三维点云数据的分块数N，且相邻数据块时间间隔Δt等于IMU采样周期1/Fimu，即Δt＝1/Fimu, 默认每个数据块内IMU三轴角速度与线性加速度不变：
Figure GDA0003404878100000081
其中：N为每帧激光雷达三维点云数据的分块数，Fimu为IMU的采集频率， Flidar为激光雷达的采样频率，进入步骤S3.2。
步骤S3.2，根据划分的数据块，计算输入的激光雷达三维点云数据中任一点 P所属的数据块位置：
Figure GDA0003404878100000082
其中：Blockn为点P所属数据块，Ptimestamp为当前点的时间戳，Ptimeheader为每帧中帧头时间戳，Δt为相邻数据块时间间隔。
作为对本发明的优选，步骤S4利用上述划分后的数据块，计算每个数据块中三轴平均角速度、三轴旋转角度，结合每个数据块中激光雷达三维点云数据点相对于第一个点的时间戳的时间偏移，计算划分出的数据块与每个数据块的三轴旋转角度，然后求得三轴旋转变换矩阵R，实现对每个激光雷达数据点进行三轴旋转运动补偿，具体步骤如下：
步骤S4.1，根据上一步骤S3.1得到划分后的数据块，计算每个数据块中的三轴平均角速度
Figure GDA0003404878100000091
Figure GDA0003404878100000092
其中：x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z 轴坐标值，
Figure GDA0003404878100000093
为每个数据块中的三轴平均角速度，ωn(x，y，z)为IMU 在tn时刻三轴旋转角速度，ωn+1(x，y，z)为IMU在tn+1时刻三轴旋转角速度，进入步骤S4.2。
步骤S4.2，根据三轴平均角速度计算三轴旋转角度三轴旋转角度θn：
Figure GDA0003404878100000094
其中：x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z 轴坐标值，
Figure GDA0003404878100000095
为每个数据块中的三轴平均角速度，Δt为相邻数据块时间间隔，进入步骤S4.3。
步骤S4.3，根据划分出的数据块与每个数据块的三轴旋转角度，对每个激光雷达三维点云数据点进行三轴旋转运动补偿，得到旋转矩阵R。
作为对本发明的优选，步骤S4.3，根据划分出的数据块，利用每个数据块的三轴旋转角度，对每个激光雷达三维点云数据点进行三轴旋转运动补偿，得到三轴旋转变换矩阵R，具体如下：
步骤S4.3.1，根据点P所属数据块的位置，计算点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和θsum；
θsum＝θn+1+θn+2+…+θend
其中：θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，θn+1、θn+2、…θend分别为点P所属激光雷达三维点云数据块到第n+1个数据块， n+2个数据块，…，最后一个数据块中三轴旋转角度，进入步骤S4.3.3。
步骤S4.3.2，计算点P相对于本数据块中最后一个点的三轴旋转角度
Figure GDA0003404878100000096
Figure GDA0003404878100000097
其中：
Figure GDA0003404878100000098
为点P相对于本数据块中最后一个点云数据的三轴旋转角度，ΔT 为点P到其所在的第n个数据块尾部的时间之差，
Figure GDA0003404878100000099
为每个数据的三轴平均角速度，Δt为相邻数据块的时间间隔，Ptimestamp为当前点的时间戳，进入步骤S4.3.3。
步骤S4.3.3，利用得到的θsum和
Figure GDA00034048781000000910
计算点P相对与帧尾三轴旋转角度θP：
Figure GDA0003404878100000101
其中：θP为点P到帧尾三轴旋转角度，θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，
Figure GDA0003404878100000102
为点P相对于本数据块中最后一个激光雷达三维点云数据的三轴旋转角度，进入步骤S4.3.4。
步骤S4.3.4，利用点P相对于帧尾三轴旋转角度，分别计算点P在x轴的旋转量Rroll、在y轴的旋转量Rpitch、在z轴的旋转量Ryaw：
Figure GDA0003404878100000103
Figure GDA0003404878100000104
Figure GDA0003404878100000105
其中：Ryaw为点P在z轴的旋转量，Rpitch为点P在y轴的旋转量，Rroll为点 P在x轴的旋转量，进入步骤S4.3.5。
步骤S4.3.5，计算点P相对于帧尾坐标系的旋转变换矩阵R：
R＝RyawRpitchRroll
作为本发明的优选，步骤S5利用得到的旋转变换矩阵R对每帧激光雷达三维点云数据中的每一个点进行三轴旋转补偿，并将补偿后的点云坐标系统一变换到帧尾坐标系，输出旋转补偿后的激光雷达三维点云数据，按照如下步骤进行；
步骤S5.1，利用旋转变换矩阵，将点P旋转变换到帧尾坐标系；
所述计算公式：
P′＝P·R
其中：P'为点P变换后的点云数据，点P为变换前的点云数据，R为旋转变换矩阵，进入步骤S5.2。
步骤S5.2，输出旋转补偿后的三维点云数据，即完成三轴旋转运动补偿。
作为对本发明的优选，步骤S6将旋转运动补偿后的激光雷达三维点云数据分成两路输出，一路作为三轴平移运动补偿的源数据，另一路经过降采样、帧间运动量初步估计、扩展卡尔曼滤波完成三轴平移运动补偿；两路一起完成激光雷达三维点云数据的实时运动补偿，按照如下步骤进行：
步骤S6.1，计算每个体素栅格内所有点云的重心，利用体素滤波对待配准点云进行降采样：
Figure GDA0003404878100000111
其中：cijk为体素内的重心，pi为体素内的数据点，k为体素内的点数，进入步骤S6.2。
步骤S6.2，利用Generalized-ICP算法进行帧间运动量初步估计，进入步骤 S6.3。
步骤S6.3，利用扩展卡尔曼滤波算法对配准结果进行优化，输出帧间位移矩阵T，进入步骤S6.4。
步骤S6.4，利用帧间位移矩阵T对每帧内所有激光雷达三维点云进行三轴平移运动补偿，最终将点云转换到帧尾坐标系：
P″＝P′+T
其中：P”为点P最终转换到帧尾坐标系对应的点，P'为点P经旋转矩阵变换的点，T为帧间位移矩阵。
实施例1
如图1所示，通过手册对两台激光雷达的参数分析，Horizon在视野范围内具有更高的点云覆盖率，将其安装于实验车辆正前方，由于Horizon的垂直视角为25.1°，所以将Horizon向下倾斜约12.5°安装，这样使得Horizon垂直视角上界接近于水平，能够有效应对低空及地面障碍物，也能保持较高的探测距离。相比于Horizon雷达，Mid-100雷达的视场角更大，尤其是垂直视场角达到了38.4°，故将其水平安装于实验车辆顶部，IMU为内置于Horizon，这样省去了IMU与Horizon标定的步骤。
按照IMU的采样频率进行激光雷达三维点云数据分块
如图3所示，步骤S3中根据IMU采样频率和激光雷达采样频率，对每帧激光雷达三维点云数据进行分块，分块数为IMU采样频率和激光雷达之比，相邻数据块时间间隔为IMU采样周期，默认每个数据块内IMU三轴角速度与线性加速度不变，按照如下步骤进行，
步骤S3.1，利用排序后的激光雷达点云数据和IMU数据，记录激光雷达的采样频率Flidar和IMU的采样频率Fimu，计算每帧数据的分块数N＝Fimu/Flidar，且相邻数据块的时间间隔等于IMU采样周期，默认每个数据块内IMU三轴角速度与线性加速度不变；
步骤S3.2，根据划分的数据块，计算输入的激光雷达三维点云数据中任一点 P所属的数据块位置：
Figure GDA0003404878100000121
其中：Blockn为点P所属数据块，Ptimestamp为当前点的时间戳，Ptimeheader为每帧激光雷达三维点云中帧头时间戳，Δt为相邻数据块时间间隔。
进一步，运用在本发明的分块过程，在IMU采样频率为200Hz情况下，IMU 每间隔100ms输出一次数据包，即每个IMU数据包中包含20个IMU数据点。激光雷达三维点云输出频率为10Hz，每个Mid-100单通道与Horizon数据包中分别包含10000与24000个激光雷达三维点云数据。因此，在激光雷达输出频率为10Hz时，每100ms输出一次数据包的数据包括四个通道激光雷达三维点云数据包和一个IMU数据包，即五路数据。本部分利用每帧中的20个IMU数据时序将每帧中的激光雷达三维点云数据分成若干数据块，这样得到每个数据块中分别包含500个Mid-100单通道数据块与1200个Horizon数据块，而且相邻数据块前后时间差约为5ms，默认每个数据块中的所有激光雷达三维点云数据块具有相同的三轴角速度与线性加速度。
在分块之前需要对激光雷达三维点云数据与IMU数据分别进行时间排序，即利用时间戳顺序对激光雷达三维点云数据与IMU数据进行排序操作。
如图4所示，利用分块的数据块计算旋转矩阵R，可按照以下步骤进行：
步骤S4.1，根据划分后的数据块，计算每个数据块中的三轴平均角速度
Figure GDA0003404878100000122
Figure GDA0003404878100000123
其中：x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z轴坐标值，
Figure GDA0003404878100000124
为每个数据块中的三轴平均角速度，ωn(x，y，z)为IMU 在tn时刻三轴旋转角速度，ωn+1(x，y，z)为IMU在tn+1时刻三轴旋转角速度，进入步骤S4.2；
步骤S4.2，根据三轴平均角速度计算三轴旋转角度θn：
Figure GDA0003404878100000131
其中：θn为三轴旋转角度，x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z轴坐标值，
Figure GDA0003404878100000132
为每个数据块中的三轴平均角速度，Δt为相邻数据块时间间隔，进入步骤S4.3；
步骤S4.3，根据划分出的数据块，利用每个数据块的三轴旋转角度，对每个激光雷达数据点进行三轴旋转运动补偿，得到三轴旋转变换矩阵R。最终把每帧所有激光雷达三维点云中的点统一变换到帧尾坐标系，具体方法如下步骤进行：
步骤S4.3.1，根据点P所属数据块的位置，计算点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和θsum；
θsum＝θn+1+θn+2+…+θend
其中：θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，θn+1、θn+2、…θend分别为点P所属激光雷达三维点云数据块到第n+1个数据块， n+2个数据块，…，最后一个数据块中三轴旋转角度，进入步骤S4.3.3；
步骤S4.3.2，计算点P相对于本数据块中最后一个点的三轴旋转角度
Figure GDA0003404878100000133
Figure GDA0003404878100000134
其中：
Figure GDA0003404878100000135
为点P相对于本数据块中最后一个激光雷达三维点云数据的三轴旋转角度，ΔT为点云P到其所在的第n个数据块尾部的时间之差，
Figure GDA0003404878100000136
为每个数据的三轴平均角速度，Δt为相邻数据块的时间间隔，Ptimestamp为当前点的时间戳，进入步骤S4.3.3；
步骤S4.3.3，利用得到的θsum和
Figure GDA0003404878100000137
计算点P相对与帧尾三轴旋转角度θP：
Figure GDA0003404878100000138
其中：θP为点P到帧尾三轴旋转角度，θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，
Figure GDA0003404878100000139
为点P相对于本数据块中最后一个点云数据的三轴旋转角度，进入步骤S4.3.4；
步骤S4.3.4，利用点P相对于帧尾三轴旋转角度，分别计算点P在x轴的旋转量Rroll、在y轴的旋转量Rpitch，在z轴的旋转量Ryaw：
Figure GDA0003404878100000141
Figure GDA0003404878100000142
Figure GDA0003404878100000143
其中：Ryaw为点P在z轴的旋转量，Rpitch为点P在y轴的旋转量，Rroll为点 P在x轴的旋转量，进入步骤S4.3.5；
步骤S4.3.5，计算点P相对于帧尾坐标系的旋转变换矩阵R：
R＝RyawRpitchRroll
进一步，在步骤S5中，根据计算出的旋转矩阵R将补偿后的点云坐标系统一变换到帧尾坐标系，输出旋转补偿后的数据，按照如下步骤进行，
步骤S5.1，利用旋转变换矩阵，将点P旋转变换到帧尾坐标系：
P′＝P·R
其中：P'为点P变换后的激光雷达三维点云数据，P为变换前的激光雷达三维点云数据，R为旋转变换矩阵，进入步骤S5.2；
步骤S5.2，得到旋转补偿后的激光雷达三维点云数据，即完成三轴旋转运动补偿。
计算平移矩阵T，进行平移运动补偿
在步骤S6中，将旋转运动补偿后的激光雷达三维点云数据分成两路输出，一路作为三轴平移运动补偿的源数据，另一路经过降采样、帧间运动量初步估计、扩展卡尔曼滤波完成三轴平移运动补偿；两路一起完成激光雷达三维点云数据的实时运动补偿。
图5为三维点云实时运动补偿实验场景图，在户外草坪这样的非结构化环境进行实验。
图6为激光雷达三维点云数据运动补偿前后效果对比图，车辆行驶在右转状态下，车速为从0开始，最高速度约30km/h。在车辆以较高速度右转行驶时，车载激光雷达所采集到的数据将会朝向车辆转弯方向倾斜，同时点云朝向车辆位移方向出现重影畸变，由于地面的起伏性，激光雷达三维点云数据也会存在其他方向畸变。由车辆行驶速度及转向等因素导致，补偿前的点云图像出现明显的倾斜与拉长现象，树木等物体轮廓变得模糊。经过运动补偿，倾斜与拉长现象得到明显改善，树木等物体轮廓变得更清晰。
上述实施例为本发明最佳的实施例及运用技术原理，但本发明的实施例方式并不受上述实施例的限制，其他不违背本发明的原理下所做的改变、修饰、替代、组合、简化，均应为等效的置换方式，都在本发明的保护范围之内。

Claims (3)

1.一种基于IMU的激光雷达三维点云实时运动补偿方法，其特征在于，步骤如下：
步骤S1，将激光雷达固定安装于无人车上，对激光雷达与IMU进行时间轴同步与空间坐标系统一，同步实时采集激光雷达三维点云数据与IMU数据，进入步骤S2；
步骤S2，按照时间戳顺序对采集的激光雷达三维点云数据与IMU数据分别进行排序，得到排序后的IMU数据与激光雷达三维点云数据，进入步骤S3；
步骤S3，根据排序后的IMU数据时序对每帧激光雷达三维点云数据进行数据块划分，通过IMU采样频率Fimu和激光雷达采样频率Flidar之比来计算数据块划分的分块数N＝Fimu/Flidar，且相邻数据块的时间间隔Δt为IMU采样周期，即Δt＝1/Fimu，默认同一数据块时间间隔内激光雷达三轴角速度与线性加速度不变，进入步骤S4；
步骤S4，利用上述划分后的数据块，计算每个数据块中三轴平均角速度、三轴旋转角度，结合每个数据块中激光雷达三维点云数据点相对于第一个点的时间戳的时间偏移，计算划分出的数据块与每个数据块的三轴旋转角度，然后求得三轴旋转变换矩阵R，实现对每个激光雷达数据点进行三轴旋转运动补偿，具体如下：
步骤S4.1，根据划分后的数据块，计算每个数据块中的三轴平均角速度
Figure FDA0003404878090000014
Figure FDA0003404878090000011
其中，x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z轴坐标值，
Figure FDA0003404878090000012
为每个激光雷达点云三维数据块中的三轴平均角速度，ωn(x，y，z)为IMU在tn时刻三轴旋转角速度，ωn+1(x，y，z)为IMU在tn+1时刻三轴旋转角速度，进入步骤S4.2；
步骤S4.2，根据三轴平均角速度计算三轴旋转角度θn：
Figure FDA0003404878090000013
其中，θn为三轴旋转角度，x、y、z分别为每个三维点云x轴坐标值、y轴坐标值、z轴坐标值，
Figure FDA0003404878090000021
为每个数据块中的三轴平均角速度，Δt为相邻数据块时间间隔，进入步骤S4.3；
步骤S4.3，根据划分出的数据块，利用每个数据块的三轴旋转角度，对每个雷达三维点云数据进行三轴旋转运动补偿，得到三轴旋转变换矩阵R；
进入步骤S5；
步骤S5，利用得到的三轴旋转变换矩阵R对每帧激光雷达三维点云数据中的每一个点进行三轴旋转补偿，并将补偿后的点云坐标系统一变换到帧尾坐标系，输出旋转补偿后的激光雷达三维点云数据，进入步骤S6；
步骤S6，将旋转运动补偿后的激光雷达三维点云数据分成两路输出，一路作为三轴平移运动补偿的源数据，另一路经过降采样、帧间运动量初步估计、扩展卡尔曼滤波完成三轴平移运动补偿；两路一起完成激光雷达三维点云数据的实时运动补偿。
2.根据权利要求1所述的一种基于IMU的激光雷达三维点云实时运动补偿方法，其特征在于，步骤S3根据IMU输出数据时序对每帧激光雷达点云数据进行数据块划分，具体如下：
步骤S3.1，利用排序后的激光雷达三维点云数据和IMU数据，记录激光雷达的采样频率Flidar和IMU的采样频率Fimu，计算每帧激光雷达三维点云数据的分块数N，且相邻数据块的时间间隔等于IMU采样周期，默认每个数据块内IMU三轴角速度与线性加速度不变；
步骤S3.2，根据划分的数据块，计算输入的激光雷达三维点云数据中任一点P所属的数据块位置：
Figure FDA0003404878090000022
其中：Blockn为点P所属数据块，Ptimestamp为当前点的时间戳，Ptimeheader为每帧激光雷达三维点云中帧头时间戳，Δt为相邻数据块时间间隔。
3.根据权利要求2所述的一种基于IMU的激光雷达三维点云实时运动补偿方法，其特征在于，步骤S4.3，根据划分出的数据块，利用每个数据块的三轴旋转角度，对每个激光雷达三维点云数据中的点进行三轴旋转运动补偿，得到三轴旋转变换矩阵R，具体如下：
步骤S4.3.1，根据点P所属数据块的位置，计算点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和θsum；
θsum＝θn+1+θn+2+…+θend
其中：θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，θn+1、θn+2、…θend分别为点P所属数据块到第n+1个数据块，n+2个数据块，…，最后一个数据块中三轴旋转角度，进入步骤S4.3.2；
步骤S4.3.2，计算点P相对于本数据块中最后一个点的三轴旋转角度
Figure FDA0003404878090000031
Figure FDA0003404878090000032
其中：
Figure FDA0003404878090000033
为点P相对于本数据块中最后一个激光雷达三维点云数据的三轴旋转角度，ΔT为点P到其所在的第n个数据块尾部的时间之差，
Figure FDA0003404878090000034
为每个激光雷达三维点云数据的三轴平均角速度，Δt为相邻数据块的时间间隔，Ptimestamp为当前点的时间戳，进入步骤S4.3.3；
步骤S4.3.3，利用得到的θsum和
Figure FDA0003404878090000035
计算点P相对与帧尾三轴旋转角度θP：
Figure FDA0003404878090000036
其中：θP为点P到帧尾三轴旋转角度，θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和，
Figure FDA0003404878090000037
为点P相对于本数据块中最后一个激光雷达三维点云数据的三轴旋转角度，进入步骤S4.3.4；
步骤S4.3.4，利用点P相对于帧尾三轴旋转角度，分别计算点P在x轴的旋转量Rroll、在y轴的旋转量Rpitch、在z轴的旋转量Ryaw：
Figure FDA0003404878090000038
Figure FDA0003404878090000039
Figure FDA00034048780900000310
其中：Ryaw为点P在z轴的旋转量，Rpitch为点P在y轴的旋转量，Rroll为点P在x轴的旋转量，进入步骤S4.3.5；
步骤S4.3.5，计算点P相对于帧尾坐标系的旋转变换矩阵R：
R＝RyawRpitchRroll 。
CN202110559368.1A 2021-05-21 2021-05-21 一种基于imu的激光雷达三维点云实时运动补偿方法 Active CN113391300B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110559368.1A CN113391300B (zh) 2021-05-21 2021-05-21 一种基于imu的激光雷达三维点云实时运动补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110559368.1A CN113391300B (zh) 2021-05-21 2021-05-21 一种基于imu的激光雷达三维点云实时运动补偿方法

Publications (2)

Publication Number Publication Date
CN113391300A CN113391300A (zh) 2021-09-14
CN113391300B true CN113391300B (zh) 2022-02-01

Family

ID=77618817

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110559368.1A Active CN113391300B (zh) 2021-05-21 2021-05-21 一种基于imu的激光雷达三维点云实时运动补偿方法

Country Status (1)

Country Link
CN (1) CN113391300B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116106864A (zh) * 2021-11-09 2023-05-12 深圳市速腾聚创科技有限公司 点云的运动补偿方法、装置、存储介质及激光雷达
CN114296057A (zh) * 2021-12-08 2022-04-08 深圳奥锐达科技有限公司 一种计算测距系统相对外参的方法、装置和存储介质
CN114897942B (zh) * 2022-07-15 2022-10-28 深圳元戎启行科技有限公司 点云地图的生成方法、设备及相关存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109975792A (zh) * 2019-04-24 2019-07-05 福州大学 基于多传感器融合矫正多线激光雷达点云运动畸变的方法
CN110703229A (zh) * 2019-09-25 2020-01-17 禾多科技（北京）有限公司 点云去畸变方法及车载激光雷达到imu的外参标定方法
CN111025331A (zh) * 2019-12-25 2020-04-17 湖北省国土资源研究院(湖北省国土资源厅不动产登记中心) 一种基于旋转结构的激光雷达建图方法及其扫描系统
CN111257853A (zh) * 2020-01-10 2020-06-09 清华大学 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN111708048A (zh) * 2020-08-19 2020-09-25 深圳市速腾聚创科技有限公司 点云的运动补偿方法、装置和系统
CN111982091A (zh) * 2020-07-09 2020-11-24 安徽博龙动力科技股份有限公司 一种基于同步imu的激光点云畸变纠正方法
CN112051591A (zh) * 2020-08-31 2020-12-08 广州文远知行科技有限公司 一种激光雷达与惯性测量单元的检测方法及相关装置
CN112781594A (zh) * 2021-01-11 2021-05-11 桂林电子科技大学 基于imu耦合的激光雷达迭代最近点改进算法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11181640B2 (en) * 2019-06-21 2021-11-23 Blackmore Sensors & Analytics, Llc Method and system for vehicle odometry using coherent range doppler optical sensors

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109975792A (zh) * 2019-04-24 2019-07-05 福州大学 基于多传感器融合矫正多线激光雷达点云运动畸变的方法
CN110703229A (zh) * 2019-09-25 2020-01-17 禾多科技（北京）有限公司 点云去畸变方法及车载激光雷达到imu的外参标定方法
CN111025331A (zh) * 2019-12-25 2020-04-17 湖北省国土资源研究院(湖北省国土资源厅不动产登记中心) 一种基于旋转结构的激光雷达建图方法及其扫描系统
CN111257853A (zh) * 2020-01-10 2020-06-09 清华大学 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN111982091A (zh) * 2020-07-09 2020-11-24 安徽博龙动力科技股份有限公司 一种基于同步imu的激光点云畸变纠正方法
CN111708048A (zh) * 2020-08-19 2020-09-25 深圳市速腾聚创科技有限公司 点云的运动补偿方法、装置和系统
CN112731450A (zh) * 2020-08-19 2021-04-30 深圳市速腾聚创科技有限公司 点云的运动补偿方法、装置和系统
CN112051591A (zh) * 2020-08-31 2020-12-08 广州文远知行科技有限公司 一种激光雷达与惯性测量单元的检测方法及相关装置
CN112781594A (zh) * 2021-01-11 2021-05-11 桂林电子科技大学 基于imu耦合的激光雷达迭代最近点改进算法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
3D Lidar-IMU Calibration based on Upsampled Preintegrated Measurements for Motion Distortion Correction;Cedric Le Gentil等;《2018 IEEE International Conference on Robotics and Automation (ICRA)》;20180525;第2149-2155页 *
A Point Cloud Distortion Removing and Mapping Algorithm based on Lidar and IMU UKF Fusion;Biao Zhang等;《Proceedings of the 2019 IEEE/ASME International Conference on Advanced Intelligent Mechatronics》;20190812;第966-971页 *
基于多线激光雷达建图的里程计优化及回环检测;李旭;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑(月刊)》;20200215(第2期);第I136-1991页 *
基于手眼模型的三维激光雷达外参数标定;韩栋斌等;《光电工程》;20170831;第44卷(第8期);第798-804页 *
激光雷达惯导耦合的里程计与建图方法研究;庞帆;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑(月刊)》;20200515(第8期);第I136-286页 *

Also Published As

Publication number Publication date
CN113391300A (zh) 2021-09-14

Similar Documents

Publication Publication Date Title
CN113391300B (zh) 一种基于imu的激光雷达三维点云实时运动补偿方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN111929699B (zh) 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
Zhu et al. The multivehicle stereo event camera dataset: An event camera dataset for 3D perception
CN110244772B (zh) 移动机器人的领航跟随系统和领航跟随控制方法
CN106887037B (zh) 一种基于gpu和深度相机的室内三维重建方法
CN107289910B (zh) 一种基于tof的光流定位系统
JP4446041B2 (ja) カメラベクトル演算装置と、このカメラベクトル演算装置に備えられる揺れ成分検出装置，画像安定化装置，位置姿勢安定化装置，目的対象物ロックオン装置及び実写対象物属性呼出装置
CN111292369B (zh) 激光雷达的伪点云数据生成方法
CN114674311B (zh) 一种室内定位与建图方法及系统
CN107831776A (zh) 基于九轴惯性传感器的无人机自主返航方法
CN115435784B (zh) 高空作业平台激光雷达与惯导融合定位建图装置及方法
CN117974766B (zh) 基于时空依据的分布式双红外传感器多目标同一性判定方法
CN113670301A (zh) 一种基于惯导系统参数的机载sar运动补偿方法
CN115046543A (zh) 一种基于多传感器的组合导航方法及系统
CN1064129C (zh) 遥感多维信息集成的装置和方法
CN116736322A (zh) 融合相机图像与机载激光雷达点云数据的速度预测方法
CN115328163B (zh) 一种巡检机器人雷达里程计的速度与精度优化方法
CN114785906A (zh) 一种航空异速像移补偿电路和方法
Lopez et al. Performance of passive ranging from image flow
CN107462244A (zh) 一种基于gps定位和航摄图像匹配的航空遥感平台姿态角高精度测量方法
Deng et al. Implementation and Optimization of LiDAR and Camera Fusion Mapping for Indoor Robots
Ainiwaer et al. P‐16.12: Simultaneous Localization and mapping technology based on solid‐state lidar
CN117128957B (zh) 一种多源信息融合的无人机火源定位方法及系统
CN113970753B (zh) 一种基于激光雷达和视觉检测的无人机定位控制方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant