CN115421125B - 一种基于数据融合的雷达点云数据惯性校正方法 - Google Patents

一种基于数据融合的雷达点云数据惯性校正方法 Download PDF

Info

Publication number
CN115421125B
CN115421125B CN202211381367.3A CN202211381367A CN115421125B CN 115421125 B CN115421125 B CN 115421125B CN 202211381367 A CN202211381367 A CN 202211381367A CN 115421125 B CN115421125 B CN 115421125B
Authority
CN
China
Prior art keywords
data
imu
point cloud
laser radar
data set
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
CN202211381367.3A
Other languages
English (en)
Other versions
CN115421125A (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.)
Shandong Free Optics Technology Co ltd
Original Assignee
Shandong Free Optics Technology Co ltd
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 Shandong Free Optics Technology Co ltd filed Critical Shandong Free Optics Technology Co ltd
Priority to CN202211381367.3A priority Critical patent/CN115421125B/zh
Publication of CN115421125A publication Critical patent/CN115421125A/zh
Application granted granted Critical
Publication of CN115421125B publication Critical patent/CN115421125B/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/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/50—Image enhancement or restoration by the use of more than one image, e.g. averaging, subtraction
    • G06T5/80—
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging

Abstract

本发明涉及一种基于数据融合的雷达点云数据惯性校正方法及激光雷达；方法包括：步骤S1将IMU模块纳入激光雷达整体架构中，使IMU和激光雷达时间和空间同步；步骤S2基于IMU测量数据以及激光雷达初始位置得到与雷达点云数据一一对应的位姿数据记为数据集二；步骤S3利用数据集二进行雷达点云畸变消除和帧间数据匹配，得到表征雷达位姿的数据集三；步骤S4将数据集二和数据集三进行融合获得数据集四，计算用于判断融合效果的融合判据；步骤S5以数据集四替代数据集二，重复步骤S3‑S4更新数据集四，将对应融合判据为最小值的数据集四确定为数据集五；步骤S6依据数据集五中位姿数据对雷达点云数据进行惯性校正。本发明实现了雷达点云数据的惯性校正。

Description

一种基于数据融合的雷达点云数据惯性校正方法
技术领域
本发明属于激光雷达技术领域，具体涉及一种基于数据融合的雷达点云数据惯性校正方法及激光雷达。
背景技术
激光雷达已经被广泛地应用于AGV行业与机器人行业，激光雷达的功用依照其应用目的不同可分为定位和地图构建两类，SLAM(Simultaneous Localization AndMapping)同时包含两类应用。由于激光雷达采集到的物体信息表现为一系列分散的、具有准确角度和距离信息的点，因此被称为点云。传统扫描式激光雷达，其数据是以激光雷达中心为相对坐标系原点的极坐标模式输出。
搭载激光雷达的AGV/机器人系统通过对激光雷达相邻时刻两帧点云的匹配与比对，得到两帧时间间隔内物体移动的距离和转动的角度，即位姿的变化；一连串位姿变化的集合，便可以组成移动物体走过的路径，由此便可完成对AGV/机器人的定位，通过多帧点云数据的融合与处理，实现地图重构。点云数据与真实空间的符合程度决定了定位与地图重构的精度与准度。
在激光雷达跟随AGV/机器人运动过程中，由于AGV/机器人的运动与激光雷达扫描的运动同时发生，造成激光雷达不同时刻发出激光时，AGV/机器人的位置和角度是不同的，由此造成激光雷达点云数据产生畸变。随着技术的发展与提升运行效率要求的增多，AGV/机器人不仅出现了更快的平动速率，也出现了更为智能的转向需求与方式，高速的平动加转动应用场合越来越多，消除点云畸变的需求也越来越多。
在点云数据处理过程中，需对点云做匹配和消畸变两类操作，点云匹配是为了更好的确认不同帧间的位姿变换关系，消畸变是为了获取更真实的空间轮廓，以便于确定雷达运动过程中任意时刻的精确位置以及周边真实地图。消畸变有助于提升点云匹配精度。点云匹配确认出的位姿变换关系也可用于消畸变。
雷达自身点云匹配的方法有很多，主要包括ICP（PL-ICP、G-ICP、IMLS-ICP、VG-ICP），NDT、CSM、LOAM（Lego-LOAM）、SuMa（SuMa++）等。激光雷达对于特定条件下（隧道、长走廊等特征变化不显著的环境）的定位会产生较大误差，长时间运行也会因误差累积而导致定位精度下降。
集成其他传感器（如惯性测量单元IMU，Inertial Measurement Unit）可有效提升定位效果及消畸变。消畸变的原理是把一帧激光雷达数据的每个激光点对应的被测目标相对坐标值转换到绝对坐标系下的绝对坐标值。
将激光雷达点云与IMU数据融合，是一种经过实践验证的可有效提升定位精度与地图精度的方法。对于AGV/机器人厂商来说，激光雷达与IMU一般被置于AGV/机器人系统的不同位置，且激光雷达与IMU各自时钟独立。通过引入时间戳信号实现数据的同步，这种数据的同步率也存在着极大的不稳定性。同时，激光雷达与IMU分别处于空间不同位置，IMU数据与激光雷达数据坐标系不统一，不能用来直接表征激光雷达的位姿及运动信息，数据融合前还需额外的标定完成空间同步。实现对激光雷达点云数据与IMU惯导数据的时间同步与空间同步是二者数据融合的先决条件。
常见的激光雷达与IMU数据融合方法还存在以下缺点：
IMU自身数据不准，存在零偏及随机误差；
IMU与激光雷达的测量频率及初始测量时刻无对应关系，无法实现快速有效数据对齐，线性插值等计算为AGV/机器人系统引入附加的运算工作；
激光雷达的点云匹配及消畸变需消耗AGV/机器人系统大量的软硬件资源，极大地影响定位导航及地图重构的实时性指标。
发明内容
鉴于上述的分析，本发明旨在公开一种基于数据融合的雷达点云数据惯性校正方法及激光雷达，用于解决点云数据惯性校正问题。
本发明公开了一种基于数据融合的雷达点云数据惯性校正方法，包括：
步骤S1、将IMU模块纳入激光雷达整体架构中，使IMU和激光雷达在时间和空间同步条件下，进行惯性测量和距离测量；
步骤S2、基于IMU测量数据以及IMU和激光雷达的初始位置，得到与雷达点云数据一一对应的位姿数据记为数据集二；
步骤S3、利用数据集二进行雷达点云畸变消除，基于消除畸变的点云数据进行帧间数据匹配，得到表征雷达位姿的数据集三；
步骤S4、将数据集二和数据集三进行融合获得数据集四，根据数据集四计算用于判断融合效果的融合判据；
步骤S5、以数据集四替代数据集二，重复步骤S3-S4，迭代更新数据集四，将对应融合判据为最小值的数据集四确定为数据集五；
步骤S6、依据数据集五中的位姿数据对雷达点云数据进行惯性校正。
进一步地，所述步骤S2中包括：
利用数据集二将雷达点云数据每一帧内的所有数据投影至一个设定时刻，以消除点云畸变；
将投影后的雷达点云数据的临近帧间数据进行匹配，计算出帧间位姿变化信息，得到数据集三。
进一步地，用于判断融合效果的融合判据的确定方法为：
基于数据集四将激光雷达相对坐标下的所有点云位置信息转换成激光雷达载体运行初始位置对应绝对坐标系下的点云位置信息；计算出点云位置信息中每一点与其最近邻点之间的距离之和，以此距离和作为融合判据。
进一步地，步骤S6中，
依据数据集五，利用坐标转化矩阵计算激光雷达测量结果相对于初始点的绝对坐标；将绝对坐标数据替换雷达点云数据中的极坐标数据形成点云数据集一；所述点云数据集一为惯性校正后的雷达点云数据。
进一步地，IMU和激光雷达空间同步条件为：
所述IMU组件固定安装于激光雷达的内部，IMU组件中IMU芯片的中心与激光雷达的光学中心重合；IMU的一维坐标轴与激光雷达的一维坐标轴重合，IMU的另两个维度的坐标轴与激光雷达的坐标轴平行且方向一致。
进一步地，IMU和激光雷达时间同步为硬时钟同步，包括：
获得IMU的数据触发脉冲序列C1；
通过硬件变频电路将C1时钟进行变换，获得与激光雷达允许工作的触发脉冲序列C1’；
利用触发脉冲序列C1触发IMU进行角速度和加速度测量；
利用触发脉冲序列C1’取代雷达码盘信号直接触发激光雷达进行测距。
进一步地，IMU和激光雷达时间同步为硬时钟同步，包括：
获得激光雷达的数据触发脉冲序列C2；
通过硬件变频电路将C2时钟进行变换，获得IMU允许工作的触发脉冲序列C2’；
利用触发脉冲序列C2触发激光雷达进行测距；
利用触发脉冲序列C2’触发IMU进行角速度和加速度测量。
进一步地，IMU和激光雷达时间同步为软时钟同步，并根据软时钟同步获得步骤S2中的数据集二，具体包括：
在搭载了IMU的激光雷达系统中；
激光雷达触发时刻ti处于相邻的两个IMU触发时刻ta，tb之间；
提取的ta与tb时刻的惯性数据经积分运算转换为位姿矩阵；分别为A，B，其中A=[a1，a2，a3，a4，a5，a6]，B=[b1，b2，b3，b4，b5，b6]；矩阵中，前三项为位置信息，后三项为姿态信息；
采用线性插值为每个激光雷达数据触发时刻计算生成分配一个位姿矩阵；得到数据集二；
数据集二中触发时刻ti对应的位姿数据k i 为：
Figure 276104DEST_PATH_IMAGE001
。
本发明还公开了一种激光雷达，内部集成有IMU组件；所述IMU组件固定安装于激光雷达的内部；
通过如上所述的基于数据融合的雷达点云数据惯性校正方法，利用IMU数据对雷达点云数据进行惯性校正。
进一步地，所述激光雷达为旋转扫描类激光雷达；所述IMU组件固定安装于激光雷达中，IMU数据的一维坐标轴与激光雷达的一维坐标轴重合，IMU的另两个维度的坐标轴与激光雷达的坐标轴平行且方向一致。
本发明可实现以下有益效果：
本发明基于数据融合的雷达点云数据惯性校正方法及激光雷达，实现了激光雷达与IMU的时间同步、空间同步、数据匹配融合后的实时同步输出。
本发明将AGV/机器人系统所做的激光雷达与IMU数据对齐、融合处理的工作前置到激光雷达中，通过激光雷达的软硬件预先设定和配置，快速实现激光雷达与IMU的数据预处理和对齐融合，为AGV/机器人系统降低数据处理压力，将融合好的数据上传，便于系统快速实现定位导航及地图重构。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件；
图1为本发明实施例中的基于数据融合的雷达点云数据惯性校正方法流程图；
图2为本发明实施例中的激光雷达示意框图；
图3为本发明实施例中的IMU处于激光雷达内部位置的示意图；
图4为本发明实施例中的一种时钟同步组件组成连接示意框图；
图5为本发明实施例中的另一种时钟同步组件组成连接示意框图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
实施例一
本发明的一个实施例公开了一种基于数据融合的雷达点云数据惯性校正方法，如图1所示，包括：
步骤S1、将IMU模块纳入激光雷达整体架构中，使IMU和激光雷达在时间和空间同步条件下，进行惯性测量和距离测量；
步骤S2、基于IMU测量数据以及IMU和激光雷达的初始位置，得到与雷达点云数据一一对应的位姿数据记为数据集二；
步骤S3、利用数据集二进行雷达点云畸变消除，基于消除畸变的点云数据进行帧间数据匹配，得到表征雷达位姿的数据集三；
步骤S4、将数据集二和数据集三进行融合获得数据集四，根据数据集四计算用于判断融合效果的融合判据；
步骤S5、以数据集四替代数据集二，重复步骤S3-S4，迭代更新数据集四，将对应融合判据为最小值的数据集四确定为数据集五；
步骤S6、依据数据集五中的位姿数据对雷达点云数据进行惯性校正。
步骤S7、将数据集五与点云数据集一按激光雷达载体系统的数据使用需求整合打包，输出给激光雷达载体系统。
具体的，步骤S1中IMU和激光雷达空间同步采用以下措施实现：
将IMU组件固定安装于激光雷达的内部，IMU组件中IMU芯片的中心与激光雷达的光学中心重合；IMU的一维坐标轴与激光雷达的一维坐标轴重合，IMU其他两维坐标轴与激光雷达对应坐标轴平行且方向一致。
优选的，所述激光雷达为旋转扫描类激光雷达；所述IMU组件固定安装于激光雷达内部的隔离腔中，IMU的一维坐标轴与激光雷达的一维坐标轴重合，IMU的另两个维度的坐标轴与激光雷达的坐标轴平行且方向一致。
在本实施例中的以机械旋转扫描类激光雷达为例进行说明，但不将激光雷达的种类限定为机械旋转扫描类激光雷达，同样基于MEMS或光学相控阵（OPA）等其他扫描方式的激光雷达均适用于本实施例。
所述IMU组件置于隔离腔内，实现对非IMU组件产生的电磁信号的有效屏蔽。
由于，本实施例的空间同步，将IMU作为激光雷达的一个组件安装在雷达内部的特定位置，可直接利用IMU位姿表征激光雷达的位姿。并且，由于后续数据融合的过程中并不是将IMU数据直接引用，未经过位姿标定引入的偏差及IMU自身的测量误差通过反复迭代可得到有效消除。通过IMU与激光雷达空间相对位置的布置与迭代融合算法的引入，免去了IMU与激光雷达之间的位姿标定环节，也降低了对IMU测量数据精准度的要求。
具体的，步骤S1中IMU和激光雷达时间同步采用以下措施实现：
激光雷达将IMU组件内置在了激光雷达内部，由激光雷达控制IMU的工作状态，时间同步方法可采用硬时钟同步和软时钟同步两种方法。
其中硬时钟同步的实现可以但不限于以下方式：
1、用外部时钟同时触发IMU和激光雷达；
所述外部时钟优选为激光雷达载具（AGV/机器人）处理器所使用的时钟C0，通过时钟C0经适当频率变换，转化成符合IMU工作需求的时钟C1和符合激光雷达工作需求的时钟C2；可以使IMU数据、激光雷达数据与激光雷达载具的数据处理保持时间同步。
2、利用激光雷达时钟信号去触发IMU；
3、利用IMU时钟信号去触发激光雷达。
通过硬时钟同步后，激光雷达时钟信号与IMU时钟信号无初始相位差，激光雷达与IMU的测量被同步触发，且激光雷达时钟频率与IMU时钟频率相同或一方频率为另一方的整数倍。
其中，利用激光雷达时钟信号去触发IMU时，硬时钟同步，包括：
1）获得激光雷达的数据触发脉冲序列C2；
2）通过硬件变频电路将C2进行变频，获得IMU允许工作范围之内的脉冲序列C2’；
3）利用脉冲序列C2触发激光雷达进行测距；
4）利用脉冲序列C2’触发IMU进行角速度和加速度测量。
其中，利用IMU去触发激光雷达时钟信号时，硬时钟同步包括：
1）获得IMU的数据触发脉冲序列C1；
2）通过硬件变频电路将C1进行变频，获得激光雷达允许工作范围之内的脉冲序列C1’；
3）利用触发脉冲序列C1触发IMU进行角速度和加速度测量；
4）利用触发脉冲序列C1’直接触发激光雷达进行测距；
具体的，在步骤S2中，包括：
1）得到IMU位姿数据构成IMU数据集一；
依据IMU与激光雷达之间对应空间同步关系，将IMU测量数据做相应积分，结合激光雷达初始定位（AGV/机器人系统静止不动，雷达上电初始化获得初始定位），令其位姿信息表征激光雷达位姿，变换后的数据构成位姿数据集一；
2）基于激光雷达与IMU的时钟与相位的同步关系，在激光雷达时钟频率为IMU时钟频率整数倍时，数据集一中的每个位姿数据必然与一个雷达点云数据对应；由于雷达点云数据的时钟频率更高，则在数据集一中的相邻的两个位姿数据中，无位姿数据与之对应的雷达点云数据，通过对数据集一插值方法将缺失数据补齐，得到经过插值补齐后的与雷达点云数据一一对应的位姿数据记为数据集二。在IMU时钟频率为激光雷达时钟频率整数倍时，则每个雷达点云数据必然有一个位姿数据与之对应，通过删减IMU位姿数据即可得到数据集二。
优选的，在软时钟同步下，可通过软时钟同步的方法得到数据集二，
根据软时钟同步获得步骤S2中的数据集二，具体包括：
1）在搭载了IMU的激光雷达系统中；
2）激光雷达触发时刻ti处于两个IMU触发时刻ta，tb之间；
3）提取的ta与tb时刻的惯性数据经积分运算转换为位姿矩阵；分别为A，B，其中A=[a1，a2，a3，a4，a5，a6]，B=[b1，b2，b3，b4，b5，b6]；矩阵中，前三项为位置信息，后三项为姿态信息；
4）采用线性插值为每个激光雷达数据触发时刻计算生成分配一个位姿矩阵；得到数据集二；
数据集二中触发时刻ti对应的位姿数据k i 为：
Figure 458824DEST_PATH_IMAGE002
。
数据集二中包括根据IMU数据计算得出的与雷达点云数据一一对应的位姿数据。
具体的，所述步骤S3中包括：
1）利用数据集二将雷达点云数据每一帧内的所有数据投影至一个设定时刻，以消除点云畸变；
优选的，所述设定时刻可以为雷达点云数据的帧起始时刻。
2）将投影后的雷达点云数据的临近帧间数据进行匹配，计算出帧间位姿及运动信息，得到数据集三。帧数据匹配包括但不限于ICP（PL-ICP、G-ICP、IMLS-ICP、VG-ICP等），NDT、CSM、LOAM（Lego-LOAM）、SuMa（SuMa++）。
以ICP(迭代最近点算法)算法举例说明帧数据匹配：
点云匹配就是求解两帧点云之间的变换关系——也就是旋转关系R和平移关系t。
ICP算法的思路就是：找到两帧点云集合中距离最近的点对，根据估计的变换关系（R和t）来计算距离最近点对经过变换之后的误差，经过不断的迭代直至误差小于某一阈值或者达到迭代次数来确定最终的变换关系。
ICP算法的数学描述：
给定两帧点云集合：
X=（xx 1，2，x 3…x n）
P=（p 1，p 2，p 3…p n）
求解R和t，使下式最小；
Figure 910665DEST_PATH_IMAGE003
；
对于激光雷达的SLAM应用，我们可以根据特征匹配的方式找出两帧数据空间点的对应关系。
其中，特征匹配包括的具体步骤为：
步骤一：计算两帧点云的质心：
Figure 837033DEST_PATH_IMAGE004
Figure 57929DEST_PATH_IMAGE005
步骤二：两帧点云中的点以质心为原点的坐标：
Figure 411550DEST_PATH_IMAGE006
Figure 944163DEST_PATH_IMAGE007
步骤三：计算w并对其进行SVD分解
Figure 284009DEST_PATH_IMAGE008
步骤四：得出变换关系：
Figure 484046DEST_PATH_IMAGE009
Figure 742989DEST_PATH_IMAGE010
步骤五：利用变换关系及雷达初始位姿值计算获得任意帧时刻对应的位姿值，由此获得数据集三。
数据集三中包括根据雷达点云数据计算得出的与雷达点云数据一一对应的位姿数据。
具体的，在步骤S4中，将数据集二和数据集三进行互相融合。数据融合方法包括但不限于加权平均法、卡尔曼滤波法、多贝叶斯估计法、Dempster-Shafer（D-S）证据推理等。
具体的，在步骤S4中，用于判断融合效果的融合判据的确定方法为：
基于数据集四，将相对坐标下的所有点云位置信息转换成激光雷达载体运行初始位置对应绝对坐标系下的点云位置信息；计算出点云位置信息中每一点与其最近邻点之间的距离之和作为融合判据。
本实施例中给出的雷达点云数据相对于雷达当前位置的极坐标值转换成激光雷达载体运行初始位置对应绝对坐标系下的直角坐标值的示例如下：
已知条件：
1、雷达点云中的某点极坐标数据（L i ，α i ），其中L i 为测量目标距雷达的距离值，
Figure 638264DEST_PATH_IMAGE011
为测量目标的方位角。
2、极坐标数据（L i ，α i ）对应的激光雷达位姿信息（x i ，y i ，z i ，θ，γ，φ），其中（xi，yi，zi）对应雷达在绝对坐标系下的位置坐标，（θ，γ，φ）为雷达的欧拉角，θ为俯仰角，γ为翻滚角，φ为方位角。
3、激光雷达载体运行初始位置的位姿信息为（x 0 ，y 0 ，z 0 ,0,0,0）。
求解绝对坐标系下的点云坐标（x’ i ，y’ i ，z’ i ）
解算步骤如下：
1、将雷达点云中的极坐标数据转换为直角坐标数据，对应的相对坐标系下的坐标值为（L i cosα i ，L i sinα i ，z i ）；
2、依据雷达的位姿和绝对坐标系雷达初始状态的位姿计算坐标转换矩阵M，
Figure 906434DEST_PATH_IMAGE012
3、绝对坐标系下的点云坐标（x’ i ，y’ i ，z’ i ）与相对坐标系下的坐标（L i cosα i ，L i sinα i ，z i ）符合下列关系式
Figure 695398DEST_PATH_IMAGE013
通过矩阵解算，即可获得绝对坐标系下的点云坐标（x’ i ，y’ i ，z’ i ）。
具体的，步骤S6中，依据数据集五，利用坐标转化矩阵计算激光雷达测量结果相对于初始点的绝对坐标；将绝对坐标数据替换雷达点云数据中的极坐标数据形成点云数据集一；所述点云数据集一为惯性校正后的雷达点云数据。
具体的，在步骤S7中将数据集五与点云数据集一按激光雷达载体系统的数据使用需求整合打包，输出给激光雷达载体系统。保证了数据匹配融合后的实时同步输出。
综上所述，本发明实施例中的基于数据融合的雷达点云数据惯性校正方法，实现了激光雷达与IMU的时间同步、空间同步、数据匹配融合后的实时同步输出。本发明实施例中的将激光雷达载体系统所做的激光雷达与IMU数据对齐、融合处理的工作前置到激光雷达中，通过激光雷达的软硬件预先设定和配置，快速实现激光雷达与IMU的数据预处理和对齐融合，为激光雷达载体系统降低数据处理压力，将融合好的数据上传，便于系统快速实现定位导航及地图重构。
实施例二
本实施例公开了一种激光雷达，如图2所示，内部集成有IMU组件；所述IMU组件固定安装于激光雷达的内部；IMU组件中IMU芯片的中心与激光雷达的光学中心重合；IMU的一维坐标轴与激光雷达的一维坐标轴重合，IMU其他两维坐标轴与激光雷达对应坐标轴平行且方向一致。
具体的，所述激光雷达为旋转扫描类激光雷达；所述IMU组件固定安装于激光雷达内部；并采用金属隔离腔，所述IMU组件置于隔离腔内，实现对非IMU产生的电磁信号进行有效屏蔽。
图3为IMU在激光雷达内部位置示意图；如图3所示，IMU坐标系的坐标轴Z’与激光雷达坐标系的坐标轴Z’重合，IMU坐标系的坐标轴X’、Y’与激光雷达坐标系的坐标轴X、Y平行。
在本实施例中的方案，同样适用于基于MEMS或光学相控阵（OPA）等其他扫描方式的激光雷达。
并且，在硬时钟同步时，还包括时钟同步组件。
所述IMU组件与激光雷达处理器通过SPI总线电连接；IMU组件和激光雷达处理器分别与时钟同步组件连接，接受时钟同步组件发送的同源的时钟对齐并呈倍数关系的时钟信号。
所述时钟同步组件包括第一时钟输出端口和第二时钟输出端口；所述第一时钟输出端口与IMU组件的时钟输入端口电连接，向IMU组件输出第一时钟；所述第二时钟输出端口与激光雷达的控制电路板上的时钟输入端口电连接；向激光雷达的控制电路板输出第二时钟。
在一个具体的方案中，如图4所示，所述时钟同步组件还包括时钟生成器和变频器；
所述时钟生成器的输出端与第一时钟输出端口电连接；
所述时钟生成器的输出端还与变频器的输入端口电连接；所述变频器的输出端口与第二时钟输出端口电连接；
图4中附图标记1为第一时钟输出端口；附图标记2为第二时钟输出端口。
在另一个具体的方案中，如图5所示，所述时钟同步组件还包括时钟生成器和变频器；
所述时钟生成器的输出端与第二时钟输出端口电连接；
所述时钟生成器的输出端还与变频器的输入端口电连接；所述变频器的输出端口与第一时钟输出端口电连接。
图5中附图标记1为第一时钟输出端口；附图标记2为第二时钟输出端口。
具体的，在本实施例中的激光雷达采用实施例一中的基于数据融合的雷达点云数据惯性校正方法进行雷达点云数据的惯性校正得到关系校正后的整合打包数据；并通过串口/网线/spi总线输出到激光雷达载体主处理器中进行定位和地图构建。
所述激光雷达载体可以为AGV/机器人。
本实施例中的其他具体技术细节与有益的效果可参见上一实施例，在此就不一一赘述了。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种基于数据融合的雷达点云数据惯性校正方法，其特征在于，包括：
步骤S1、将IMU模块纳入激光雷达整体架构中，使IMU和激光雷达在时间和空间同步条件下，进行惯性测量和距离测量；
步骤S2、基于IMU测量数据以及IMU和激光雷达初始位置，得到与雷达点云数据一一对应的位姿数据记为数据集二；
步骤S3、利用数据集二进行雷达点云畸变消除，基于消除畸变的点云数据进行帧间数据匹配，得到表征雷达位姿的数据集三；
步骤S4、将数据集二和数据集三进行融合获得数据集四，根据数据集四计算用于判断融合效果的融合判据；
用于判断融合效果的融合判据的确定方法为：
基于数据集四将激光雷达相对坐标下的所有点云位置信息转换成激光雷达载体运行初始位置对应绝对坐标系下的点云位置信息；计算出点云位置信息中每一点与其最近邻点之间的距离之和，以此距离和作为融合判据；
步骤S5、以数据集四替代数据集二，重复步骤S3-S4，迭代更新数据集四，将对应融合判据为最小值的数据集四确定为数据集五；
步骤S6、依据数据集五中的位姿数据对雷达点云数据进行惯性校正；
步骤S6中，依据数据集五，利用坐标转化矩阵计算激光雷达测量结果相对于初始点的绝对坐标；将绝对坐标数据替换雷达点云数据中的极坐标数据形成点云数据集一；所述点云数据集一为惯性校正后的雷达点云数据。
2.根据权利要求1所述的雷达点云数据惯性校正方法，其特征在于，所述步骤S3中包括：
利用数据集二将雷达点云数据每一帧内的所有数据投影至一个设定时刻，以消除点云畸变；
将投影后的雷达点云数据的临近帧间数据进行匹配，计算出帧间位姿信息，得到数据集三。
3.根据权利要求1所述的雷达点云数据惯性校正方法，其特征在于，IMU和激光雷达空间同步条件为：
IMU组件固定安装于激光雷达的内部，IMU组件中IMU芯片的中心与激光雷达的光学中心重合；IMU测量数据的一维坐标轴与激光雷达的一维坐标轴重合，IMU其他两维坐标轴与激光雷达对应坐标轴平行且方向一致。
4.根据权利要求1所述的雷达点云数据惯性校正方法，其特征在于，IMU和激光雷达时间同步为硬时钟同步，包括：
获得IMU的数据触发脉冲序列C1；
通过硬件变频电路将C1时钟进行变换，获得与激光雷达允许工作的触发脉冲序列C1’；
利用触发脉冲序列C1触发IMU进行角速度和加速度测量；
利用触发脉冲序列C1’取代码盘信号直接触发激光雷达进行测距。
5.根据权利要求1所述的雷达点云数据惯性校正方法，其特征在于，IMU和激光雷达时间同步为硬时钟同步，包括：
获得激光雷达的数据触发脉冲序列C2；
通过硬件变频电路将C2时钟进行变换，获得IMU允许工作的触发脉冲序列C2’；
利用触发脉冲序列C2触发激光雷达进行测距；
利用触发脉冲序列C2’触发IMU进行角速度和加速度测量。
6.根据权利要求1所述的雷达点云数据惯性校正方法，其特征在于，
IMU和激光雷达时间同步为软时钟同步，并根据软时钟同步获得步骤S2中的数据集二，具体包括：
在搭载了IMU的激光雷达系统中；
激光雷达触发时刻ti处于相邻的两个IMU触发时刻ta，tb之间；
提取的ta与tb时刻的惯性数据经积分运算转换为位姿矩阵；分别为A，B，其中A=[a1，a2，a3，a4，a5，a6]，B=[b1，b2，b3，b4，b5，b6]；矩阵中，前三项为位置信息，后三项为姿态信息；
采用线性插值为每个激光雷达数据触发时刻计算生成分配一个位姿矩阵；得到数据集二；
数据集二中触发时刻ti对应的位姿数据k i 为：
Figure DEST_PATH_IMAGE001
。
7.一种激光雷达，其特征在于，内部集成有IMU组件；所述IMU组件固定安装于激光雷达的内部；
通过如权利要求1-6任一项所述的基于数据融合的雷达点云数据惯性校正方法，利用IMU数据对雷达点云数据进行惯性校正。
8.根据权利要求7所述的激光雷达，其特征在于，所述激光雷达为机械旋转扫描类激光雷达；所述IMU组件固定安装于激光雷达中，IMU测量数据的一维坐标轴与激光雷达的一维坐标轴重合，IMU的另两个维度的坐标轴与激光雷达的坐标轴平行且方向一致。
CN202211381367.3A 2022-11-07 2022-11-07 一种基于数据融合的雷达点云数据惯性校正方法 Active CN115421125B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211381367.3A CN115421125B (zh) 2022-11-07 2022-11-07 一种基于数据融合的雷达点云数据惯性校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211381367.3A CN115421125B (zh) 2022-11-07 2022-11-07 一种基于数据融合的雷达点云数据惯性校正方法

Publications (2)

Publication Number Publication Date
CN115421125A CN115421125A (zh) 2022-12-02
CN115421125B true CN115421125B (zh) 2023-01-10

Family

ID=84207962

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211381367.3A Active CN115421125B (zh) 2022-11-07 2022-11-07 一种基于数据融合的雷达点云数据惯性校正方法

Country Status (1)

Country Link
CN (1) CN115421125B (zh)

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109709801B (zh) * 2018-12-11 2024-02-02 智灵飞（北京）科技有限公司 一种基于激光雷达的室内无人机定位系统及方法
CN110221603B (zh) * 2019-05-13 2020-08-14 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN114585879A (zh) * 2020-09-27 2022-06-03 深圳市大疆创新科技有限公司 位姿估计的方法和装置
CN112230240A (zh) * 2020-09-30 2021-01-15 深兰人工智能(深圳)有限公司 激光雷达与相机数据的时空同步系统、装置及可读介质
CN112698302B (zh) * 2020-12-16 2023-11-07 南京航空航天大学 一种颠簸路况下的传感器融合目标检测方法
CN112764053B (zh) * 2020-12-29 2022-07-15 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质
CN113311411B (zh) * 2021-04-19 2022-07-12 杭州视熵科技有限公司 一种用于移动机器人的激光雷达点云运动畸变校正方法
CN113570715B (zh) * 2021-07-23 2023-10-13 东北大学 基于传感器融合的旋转激光实时定位建模系统及方法
CN113959437A (zh) * 2021-10-14 2022-01-21 重庆数字城市科技有限公司 一种用于移动测量设备的测量方法及系统
CN114419147A (zh) * 2021-11-16 2022-04-29 新兴际华集团有限公司 一种救援机器人智能化远程人机交互控制方法及系统

Also Published As

Publication number Publication date
CN115421125A (zh) 2022-12-02

Similar Documents

Publication Publication Date Title
JP5393318B2 (ja) 位置姿勢計測方法及び装置
CN111007530B (zh) 激光点云数据处理方法、装置及系统
US6438507B1 (en) Data processing method and processing device
CN110873883B (zh) 融合激光雷达和imu的定位方法、介质、终端和装置
CN110658530A (zh) 一种基于双激光雷达数据融合的地图构建方法、系统及地图
CN111121754A (zh) 移动机器人定位导航方法、装置、移动机器人及存储介质
JP2013187862A (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
CN110142805A (zh) 一种基于激光雷达的机器人末端校准方法
KR101390466B1 (ko) 이동로봇 및 레이저 스캐너가 고속으로 움직일 때 측정된 스캔 거리 데이터들의 동적 오차 보정 방법 및 이를 이용한 스캔 거리 데이터 측정 장치
CN111427061A (zh) 一种机器人建图方法、装置，机器人及存储介质
CN113311411A (zh) 一种用于移动机器人的激光雷达点云运动畸变校正方法
CN111366908B (zh) 一种激光雷达转台及其测量装置和测量方法
CN109856640B (zh) 一种基于反光柱或反光板的单线激光雷达二维定位方法
CN110187337B (zh) 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统
CN114296057A (zh) 一种计算测距系统相对外参的方法、装置和存储介质
CN114413958A (zh) 无人物流车的单目视觉测距测速方法
Boehm et al. Accuracy of exterior orientation for a range camera
CN112985415B (zh) 一种室内定位方法及系统
CN115421125B (zh) 一种基于数据融合的雷达点云数据惯性校正方法
Zhi et al. Multical: Spatiotemporal calibration for multiple IMUs, cameras and LiDARs
CN116047481A (zh) 矫正点云数据畸变方法、装置、设备及存储介质
CN115728753A (zh) 激光雷达与组合导航的外参标定方法、装置及智能车辆
CN110888123A (zh) 一种基于旋转矩阵的雷达坐标转换方法
CN115661252A (zh) 一种实时位姿估计方法、装置、电子设备以及存储介质
TW202137138A (zh) 含大地座標之3d點雲地圖的產生方法和3d點雲地圖產生系統

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