CN116338652A - 激光雷达和imu系统的时空联合标定方法 - Google Patents
激光雷达和imu系统的时空联合标定方法 Download PDFInfo
- Publication number
- CN116338652A CN116338652A CN202310208025.XA CN202310208025A CN116338652A CN 116338652 A CN116338652 A CN 116338652A CN 202310208025 A CN202310208025 A CN 202310208025A CN 116338652 A CN116338652 A CN 116338652A
- Authority
- CN
- China
- Prior art keywords
- imu
- laser radar
- coordinate system
- time
- motion
- 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.)
- Pending
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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Manufacturing & Machinery (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本申请涉及激光雷达和IMU系统的时空联合标定方法,包括如下步骤:利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模,得到IMU相对于世界坐标系的运动轨迹;利用正态分布变换点云匹配算法估计激光雷达的运动,得到激光雷达相对于初始位置的运动轨迹;以局部轨迹之间的Hausdorff距离为代价函数,结合手眼标定方法求解激光雷达的坐标系与IMU的坐标系之间时空关系的初始值;利用激光雷达与IMU的相对位姿减小激光雷达的点云数据畸变;根据IMU预积分和激光雷达点云的点、线、面特征,构建相应的非线性优化目标函数,结合校正后的激光雷达的点云数据和时空关系的初始值,在非线性图优化框架下对目标函数进行优化,并完成激光雷达和IMU的相对位姿标定。
Description
技术领域
本申请属于机器人技术领域,涉及一种多传感器配置的导航系统时空标定方法。
背景技术
随着社会的发展,机器人在各种环境下应用的越来越普及。传感器在机器人的各种感知中发挥着中十分重要的作用,就像是人的器官一样。多传感器融合是解决复杂环境下机器人自主环境感知与导航的重要方式,其可以充分利用不同传感器的优点,相互补偿,实现相较于单一传感器更为精准与鲁棒的感知与定位性能,因此近年来受到了广泛的关注,如在无人驾驶汽车、野外机器人、高精度地图构建、目标跟踪等。
现有的多传感器融合技术主要分为基于外部设备的标定方法和自标定的方法。基于外部设备的标定方法主要是利用外部传感器、转台、标定板,通过这些外部设备提供的先验信息去估计传感器的空间变换参数,该方法的精度更多取决于外部校准设备,同时该方法对外部设备的精度要求很高,且校准麻烦,受到时间地点限制,无法大规模的使用,限制的应用范围。
另一种自校准方法由于不需要外部设备,所以也是目前最先进和应用最广泛的一种方法。该方法利用传感器采集的数据生成运动轨迹进行优化,但是也存在传感器数据的不同步等需要优化的问题,时间偏移也会影响到校准的精度。同时由于机器人自主运动所面临的环境是非常复杂多变的,而大多传感器会因为时间的积累产生偏移现象,所以一个多传感器时空自主在线标定方法是迫切需要的。
发明内容
本申请的目的是提供激光雷达和IMU系统的时空联合标定方法,能够提高激光雷达和IMU(Inertial Measurement Unit,即惯性测量单元)系统的标定精度和效率。
本申请的一种激光雷达和IMU系统的时空联合标定方法,包括如下步骤:
S1、利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模,得到IMU相对于世界坐标系的运动轨迹;
S2、利用正态分布变换点云匹配算法估计激光雷达的运动,得到激光雷达相对于初始位置的运动轨迹;
S3、以局部轨迹之间的Hausdorff距离为代价函数,结合手眼标定方法求解激光雷达的坐标系与IMU的坐标系之间时空关系的初始值;
S4、利用激光雷达与IMU的相对位姿减小激光雷达的点云数据畸变;
S5、根据IMU预积分和激光雷达点云的点、线、面特征,构建相应的非线性优化目标函数,结合校正后的激光雷达的点云数据和时空关系的初始值,在非线性图优化框架下对目标函数进行优化,并完成激光雷达和IMU的相对位姿标定。
本技术方案的方法能够实现激光雷达和IMU系统中高精度标定的要求,以满足机器人系统、无人驾驶汽车系统等定位和测绘高精度的要求。
应当理解的是,以上的一般描述和后文的细节描述仅是示例性的,本发明并不受限制。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本发明的实施例,并与说明书一起用于解释本发明的原理。
图1是本申请一实施例中对轨迹进行分段的原理示意图。
具体实施方式
这里将详细地对示例性实施例进行说明,以下示例性实施例中所描述的实施方式并不代表与本发明相一致的所有实施方式。相反,它们仅是与如所附权利要求书中所记载的、本发明的一些方面相一致的系统和方法的例子。
本申请实施例提供一种激光雷达和IMU系统的时空联合标定方法,包括如下步骤:
步骤S1、利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模,得到IMU相对于世界坐标系的运动轨迹,其具体过程如下:
当IMU的运动轨迹的控制点均匀分布时,采用以下矩阵形式的B-spline基函数进行建模,其公式为:
其中,d为B-spline基函数的阶数,即t∈[ti,ti+1)区间内的连续运动轨迹p(t),仅由时间ti,ti+1,…,ti+d对应的控制点pi,pi+1,…,pi+d决定,uT=[1,u,…,ud],u=(t-ti)/(ti+1-ti),表示样条矩阵M(d+1)的第j列;
因此,运动轨迹的累积可表示为:
对旋转群组SO(3)空间中的均匀分布单元四元数控制点qi进行建模:
其中Exp(·)表示李代数到李群的映射,Log(·)表示李群到李代数的映射。
本文采用三次样条基函数对运动轨迹进行建模,其中样条矩阵和累积样条矩阵可表示为:
步骤S2、利用正态分布变换(NDT)点云匹配算法估计激光雷达的运动,得到激光雷达相对于初始位置的运动轨迹;
具体的是采用正态分布变换(NDT)点云匹配算法进行估计,根据激光雷达的运动轨迹上各点的时间戳,得到激光雷达的相对于初始位置的相对运动轨迹LΓL。
步骤S3、以局部轨迹之间的Hausdorff距离(豪斯多夫距离)为代价函数,结合手眼标定方法求解激光雷达的雷达坐标系与IMU的IMU坐标系之间的时空关系的初始值;
由于Hausdorff距离可以描述两个离散轨迹之间的相似性,下式表示为Hausdorff距离,解决了激光雷达的运动轨迹IΓL(q)和MU的运动轨迹IΓI(q)之间的相似性:
Hx=max(h(xi,xj),h(xj,xi))
其中H表示轨迹之间的相似度。
为了消除里程计估计的失效区域,如图1所示对轨迹进行分段,并分别计算轨迹段内的Hausdorff距离。如果轨迹之间的相似度小于设定的阈值,则可以使用估计的轨迹进行初始参数标定。
上述获得的IMU运动姿态的连续时间运动轨迹IΓI(t)和激光雷达的相对运动轨迹LΓL,对两个传感器轨迹上各对应的时间戳进行变换,构造非线性最小二乘问题:
对满足要求的所有轨迹点,用上述公式计算代价,利用Levenberg-Marquardt(LM)非线性优化算法求解上述非线性最小二乘优化问题,得到的时空关系的统一初值参数qLtoI和Δt。
标定得到初始参数后,利用预积分运算对IMU测量进行积分,通过预积分运算得到激光雷达帧数据采集时刻的运动,对激光雷达数据采集的运动失真进行相应补偿,重复上述过程,得到最终的时空关系的初始值。
其中手眼标定方法的过程如下:
其中t1和t2分别代表IMU数据采集的时间戳和激光雷达数据采集的时间戳。时间偏差可以用Δt=t2-t1表示,qWtoI表示从IMU坐标系到世界坐标系的姿态旋转矩阵,pWinI表示IMU坐标系在世界坐标系中的平移向量,表示激光雷达坐标系到激光雷达初始坐标系的旋转矩阵,/>表示激光雷达坐标系在激光雷达零坐标系中的平移向量。
如果两个刚性连接的传感器的轨迹估计精度足够高,且激光雷达与IMU之间不存在时间偏差,则它们的姿态运动轨迹和的形状相似,那么可以任意选择两个轨迹上同一时刻的点和,计算轨迹之间的两个变换关系如下:
步骤S4、利用激光雷达与IMU的相对位姿减小激光雷达的点云数据畸变;
使用步骤S3中标定得到的相对位姿,即最终的时空关系的初始值,首先对一个激光帧扫描起始时刻和结束时刻之间的IMU测量值进行积分,然后利用每个激光测量点的时间戳对IMU积分结果进行线性插值,从而将每个测量点投影到激光帧的起始时刻,完成运动补偿,减小激光雷达在运动过程中产生的畸变。
步骤S5、根据IMU预积分和激光雷达点云的点、线、面特征,构建相应的非线性优化目标函数,结合校正后的激光雷达数据和两个传感器坐标系的时空校准初始值,在非线性图优化框架下对目标函数进行优化并完成雷达和IMU的相对位姿标定。
具体过程如下:
S51、计算IMU的预积分因子;
为了利用IMU测量数据来估计机器人身体在世界坐标系中的运动,根据IMU模型可以建立以下IMU的动力学模型:
为避免线性化点变化后的重复积分问题,利用一种预积分方法对区间内的运动状态进行积分。根据预积分公式,将IMU两态之间的预积分量作为约束因子,即IMU的预积分因子,约束两个力矩之间的导航状态:
其中qbw和分别表示世界坐标系与IMU坐标系之间的姿态变换四元数和平移向量;vw表示IMU坐标系在世界坐标系中的速度矢量;gw表示世界坐标系中的重力加速度;ba和bg分别表示加速度计和陀螺仪的偏置误差。
S52、提取激光雷达点云的点、线、面特征,并构造激光雷达因子;
利用结构张量提取点云的几何特征。它可以表示为:
其次,对结构张量进行矩阵分解,得到特征值和特征向量。
S=uΣVT,Σ=diag(λ1,λ2,λ3)
最后,根据特征值计算几何描述特征,
只保留线性度和平面度在一定阈值范围内的点云作为提取的线特征和平面特征。另一方面,考虑到激光雷达点云的稀疏性,只提取环境中点云的点、线、面特征。
对每一帧点云计算一定范围内三维点云的不变矩,并构造协方差矩阵Σ=diag(λ1,λ2,λ3)来描述该范围内三维点云的几何特征;获取点云的几何描述特征,提取点云的点、线、面特征。此外,通过评估相邻点的曲率提取平面特征,将曲率值较小的点分类为平面特征。
对上述方法提取的几何描述特征进行交叉验证,得到最终的点、线、面特征,根据点与点、线、面之间的距离构造以下约束因子:
其中epo→po、epo→li和epo→pl分别表示点、点到线和点到面之间的距离;Mvj和Mnk分别表示地图坐标系中特征线和特征平面上的方向向量和法向量。表示当前激光雷达坐标系与地图坐标系之间的位姿。
S53、构建相应的非线性优化目标函数;
在测量噪声和过程噪声服从高斯分布的条件下,构造了如下非线性优化问题:
最后,在非线性图优化框架下对目标进行优化并完成激光雷达和IMU的相对位姿标定。
Claims (10)
1.激光雷达和IMU系统的时空联合标定方法,其特征在于,包括如下步骤:
S1、利用B样条基函数对IMU运动姿态的连续时间下轨迹进行建模,得到IMU相对于世界坐标系的运动轨迹;
S2、利用正态分布变换点云匹配算法估计激光雷达的运动,得到激光雷达相对于初始位置的运动轨迹;
S3、以局部轨迹之间的Hausdorff距离为代价函数,结合手眼标定方法求解激光雷达的坐标系与IMU的坐标系之间时空关系的初始值;
S4、利用激光雷达与IMU的相对位姿减小激光雷达的点云数据畸变;
S5、根据IMU预积分和激光雷达点云的点、线、面特征,构建相应的非线性优化目标函数,结合校正后的激光雷达的点云数据和时空关系的初始值,在非线性图优化框架下对目标函数进行优化,并完成激光雷达和IMU的相对位姿标定。
2.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法,其特征在于,所述步骤S1包括:
S11、在IMU的运动轨迹的控制点均匀分布时,采用以下矩阵形式的B-spline基函数进行建模,其公式为:
其中,d为B-spline基函数的阶数,即t∈[ti,ti+1)区间内的连续运动轨迹p(t),仅由时间ti,ti+1,…,ti+d对应的控制点pi,pi+1,…,pi+d决定,uT=[1,u,…,ud],u=(t-ti)/(ti+1-ti),表示样条矩阵M(d+1)的第j列;
S12、运动轨迹的累积可表示为:
S13、对SO(3)空间中的均匀分布单元四元数控制点qi进行建模:
其中Exp(·)表示李代数到李群的映射,Log(·)表示李群到李代数的映射;
S14、采用三次样条基函数对运动轨迹进行建模,其中样条矩阵和累积样条矩阵可表示为:
3.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法,其特征在于,所述步骤S2包括:
采用正态分布变换点云匹配算法进行估计,根据激光雷达的运动轨迹上各点的时间戳,得到激光雷达的相对运动轨迹LΓL。
4.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法,其特征在于,所述步骤S3包括:
S31、利用Hausdorff距离描述激光雷达的运动轨迹IΓL(q)和IMU的运动轨迹IΓI(q)之间的相似性,下式表示为Hausdorff距离:
Hx=max(h(xi,xj),h(xj,xi))
其中H表示轨迹之间的相似度;
S32、IMU运动姿态的连续时间运动轨迹IΓI(t)和激光雷达的相对运动轨迹LΓL,对两个轨迹上各对应的时间戳进行变换,构造非线性最小二乘问题:
S33、对满足要求的所有轨迹点,用上述公式计算代价,利用Levenberg-Marquardt非线性优化算法求解上述非线性最小二乘优化问题,得到时空关系的初始参数qLtoI和Δt;
S34、得到初始参数后,利用预积分运算对IMU测量进行积分,得到激光雷达帧数据采集时刻的运动,对激光雷达数据采集的运动失真进行相应补偿,得到最终的时空关系的初始值。
6.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法,其特征在于,所述步骤S4包括:
根据得到时空关系的初始值,对一个激光帧扫描起始时刻和结束时刻之间的IMU测量值进行积分,利用每个激光测量点的时间戳对IMU积分结果进行线性插值,将每个测量点投影到激光帧的起始时刻,完成运动补偿,减小激光雷达在运动过程中产生的畸变。
7.如权利要求1所述的激光雷达和IMU系统的时空联合标定方法,其特征在于,所述步骤S5中,其中“根据IMU预积分和激光雷达点云的点、线、面特征,构建相应的非线性优化目标函数”包括:
S51、计算IMU的预积分因子;
S52、提取激光雷达点云的点、线、面特征,并构造激光雷达因子;
S53、构建相应的非线性优化目标函数。
8.如权利要求7所述的激光雷达和IMU系统的时空联合标定方法,其特征在于,所述S51、计算IMU的预积分因子的步骤包括:
S512、根据IMU模型建立以下IMU的动力学模型:
S513、利用预积分方法对区间内的运动状态进行积分,根据预积分公式,将IMU两态之间的预积分量作为约束因子,约束两个力矩之间的导航状态:
9.如权利要求7所述的激光雷达和IMU系统的时空联合标定方法,其特征在于,所述S52、“提取激光雷达点云的点、线、面特征,并构造激光雷达因子”的步骤包括:
S521、利用结构张量提取点云的几何特征,表示为:
S522、对结构张量进行矩阵分解,得到特征值和特征向量:
S523、根据特征值计算几何描述特征,
S524、对提取的几何描述特征进行交叉验证,得到最终的点、线、面特征,根据点与点、线、面之间的距离构造以下约束因子:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310208025.XA CN116338652A (zh) | 2023-03-06 | 2023-03-06 | 激光雷达和imu系统的时空联合标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310208025.XA CN116338652A (zh) | 2023-03-06 | 2023-03-06 | 激光雷达和imu系统的时空联合标定方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116338652A true CN116338652A (zh) | 2023-06-27 |
Family
ID=86881556
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310208025.XA Pending CN116338652A (zh) | 2023-03-06 | 2023-03-06 | 激光雷达和imu系统的时空联合标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116338652A (zh) |
-
2023
- 2023-03-06 CN CN202310208025.XA patent/CN116338652A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109709801B (zh) | 一种基于激光雷达的室内无人机定位系统及方法 | |
CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
CN111536967B (zh) | 一种基于ekf的多传感器融合温室巡检机器人跟踪方法 | |
CN113311411B (zh) | 一种用于移动机器人的激光雷达点云运动畸变校正方法 | |
CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN114136311B (zh) | 一种基于imu预积分的激光slam定位方法 | |
CN111260751B (zh) | 基于多传感器移动机器人的建图方法 | |
CN110702091A (zh) | 一种沿地铁轨道移动机器人的高精度定位方法 | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN113189613B (zh) | 一种基于粒子滤波的机器人定位方法 | |
CN112504261A (zh) | 一种基于视觉锚点的无人机降落位姿滤波估计方法及系统 | |
CN115326053A (zh) | 一种基于双层视觉的移动机器人多传感器融合定位方法 | |
CN112067007B (zh) | 地图生成方法、计算机存储介质及电子设备 | |
Jiang et al. | 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments | |
CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及系统 | |
CN117451032A (zh) | 一种低算力与松耦合的激光雷达和imu的slam方法及系统 | |
CN116338652A (zh) | 激光雷达和imu系统的时空联合标定方法 | |
Wang et al. | Micro aerial vehicle navigation with visual-inertial integration aided by structured light | |
Chen et al. | Multi-frequency data fusion for attitude estimation based on multi-layer perception and cubature kalman filter | |
CN116202487A (zh) | 一种基于三维建模的实时目标姿态测量方法 | |
CN116466712A (zh) | 基于cml-avg激光雷达小车的自主巡航方法及系统 | |
CN113689501B (zh) | 一种基于收敛点的双机协同目标机定位跟踪控制方法 | |
CN113093215B (zh) | 一种基于激光测距的移动平台跟踪方法 | |
CN114415655A (zh) | 一种基于改进slam的巡检机器人导航控制方法 |
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 |