CN114518111A - 一种激光雷达与惯性测量单元标定方法及系统 - Google Patents
一种激光雷达与惯性测量单元标定方法及系统 Download PDFInfo
- Publication number
- CN114518111A CN114518111A CN202210241914.1A CN202210241914A CN114518111A CN 114518111 A CN114518111 A CN 114518111A CN 202210241914 A CN202210241914 A CN 202210241914A CN 114518111 A CN114518111 A CN 114518111A
- Authority
- CN
- China
- Prior art keywords
- acceleration
- angular velocity
- moment
- coordinate system
- measurement unit
- 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
- 238000005259 measurement Methods 0.000 title claims abstract description 135
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000001133 acceleration Effects 0.000 claims abstract description 150
- 239000013598 vector Substances 0.000 claims description 112
- 238000013519 translation Methods 0.000 claims description 41
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000012545 processing Methods 0.000 claims description 19
- 230000009466 transformation Effects 0.000 claims description 12
- 230000005484 gravity Effects 0.000 claims description 5
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 36
- 230000008569 process Effects 0.000 description 9
- 230000010354 integration Effects 0.000 description 7
- 230000008859 change Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- LTXREWYXXSTFRX-QGZVFWFLSA-N Linagliptin Chemical compound N=1C=2N(C)C(=O)N(CC=3N=C4C=CC=CC4=C(C)N=3)C(=O)C=2N(CC#CC)C=1N1CCC[C@@H](N)C1 LTXREWYXXSTFRX-QGZVFWFLSA-N 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000002156 mixing Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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
-
- 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
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本申请提供一种激光雷达与惯性测量单元标定方法及系统，所述标定方法包括：获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度；将所述点云数据转换为第二角速度、第二角加速度和第二加速度；基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值。本申请的技术方案通过激光雷达测量的点云数据推导惯性测量单元的测量数据，再用推导的测量数据和实际的测量数据计算偏差，从而得到标定值，可以更准确的对激光雷达与惯性测量单元的相对位姿进行标定。
Description
技术领域
本申请涉及车辆自动驾驶领域，尤其涉及一种激光雷达与惯性测量单元标定方法及系统。
背景技术
多线激光雷达和惯性测量单元(Inertial Measurement Unit，IMU)已经是自动驾驶领域定位和建图必不可少的传感器。IMU数据具有累积误差，但是频率很高，短时间内累积误差很小。多线激光雷达点云在运动过程中会产生畸变，但是多线激光雷达点云与3D点云地图匹配不会产生累积误差。在多线雷达的扫描周期内用IMU数据校正雷达点云的畸变可以提高定位和建图的精度。为了保证定位和建图的精度，需要对多线激光雷达和IMU的相对位姿做精确标定。
现有的标定方法大多是基于IMU预积分计算多线激光雷达点云的重投影误差，然后构造优化函数最小化重投影误差，从而求出激光雷达坐标系和IMU坐标系的转换关系。IMU可以测量角速度和加速度，对角速度积分一次可以得到角度变化，而加速度需要积分两次才能得到位姿变化，积分两次会引入更多的积分误差。而且IMU测量的加速度包含重力加速度，随着IMU姿态的变化，测出的3维重力加速度向量也会变化。在对加速度积分求位置变化之前，需要先从加速度测量值中去除重力加速度，IMU测量的重力加速度向量值与IMU的姿态有关，而IMU的姿态需要用IMU角速度积分来估计，所以角速度的积分误差也会影响加速度的积分误差。这些因素导致加速度积分得到的位姿误差较大，难以保证标定精度。
因此，有必要提供更有效、更可靠的技术方案。
发明内容
本申请提供一种激光雷达与惯性测量单元标定方法及系统，可以更准确的对激光雷达与惯性测量单元的相对位姿进行标定。
本申请的一个方面提供一种激光雷达与惯性测量单元标定方法，包括：获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度，其中，所述N为大于等于1的整数；将所述点云数据转换为第二角速度、第二角加速度和第二加速度；基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值，所述标定值包括激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量。
在本申请的一些实施例中，将所述点云数据转换为第二角速度、第二角加速度和第二加速度的方法包括：将所述激光雷达在第1时刻至第i时刻采集到的的点云数据分别记为P1至Pi，并获取P1到Pi的变换矩阵，所述i为大于1小于等于N的整数；将所述变换矩阵转换为6维平移旋转向量，所述6维平移旋转向量包括3维平移向量和3维旋转向量；将第1时刻到第N时刻之间的6维平移旋转向量拟合为B样条曲线；将所述B样条曲线上的旋转向量求一阶导得到第二角速度，将所述B样条曲线上的旋转向量求二阶导得到第二角加速度，将所述B样条曲线上的平移向量求二阶导得到第二加速度。
在本申请的一些实施例中，所述旋转向量的旋转角度范围为[-π，π]。
在本申请的一些实施例中，基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值的方法包括：基于角速度误差函数计算得出第1时刻到第N时刻间每个时刻的角速度误差项；基于加速度误差函数计算得出第1时刻到第N时刻间每个时刻的加速度误差项；将所有的误差项相加得到总误差函数，所述总误差函数中仅有所述激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量为变量，使所述总误差函数达到最小值的所述激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量为所述标定值。
在本申请的一些实施例中，定义函数f(R，T)表示3维旋转向量R转换为3×3的旋转矩阵后与3维向量T相乘，所述角速度误差函数为：
Errgyro＝f(RL_to_I，f(RBj，gyroBj))-gyroIj
其中，RL_to_I为激光雷达坐标系到惯性测量单元坐标系的旋转向量，RBj为所述B样条曲线上第j时刻的旋转向量，gyroBj为所述B样条曲线上第j时刻的第二角速度，gyroIj为所述惯性测量单元在第j时刻采集到的第一角速度，所述j为大于等于1小于等于N的整数。
在本申请的一些实施例中，所述B样条曲线在第j时刻的第二角速度产生的法向加速度为：
accBj_n＝gyroBj×(gyroBj×f(RBj，TL_to_I))
其中，gyroBj为所述B样条曲线上第j时刻的第二角速度，TL_to_I为激光雷达坐标系到惯性测量单元坐标系的平移向量，RBj为所述B样条曲线上第j时刻的旋转向量。
在本申请的一些实施例中，所述B样条曲线在第j时刻的第二角加速度产生的切向加速度为：
accBj_τ＝alphaBj×f(RBj，TL_to_I)
其中，alphaBj为所述B样条曲线上第j时刻的第二角加速度，TL_to_I为激光雷达坐标系到惯性测量单元坐标系的平移向量，RBj为所述B样条曲线上第j时刻的旋转向量。
在本申请的一些实施例中，所述加速度误差函数为：
Erracc＝f(RL_to_I，f(RBj，(accBj+accBj_τ+accBj_n+G)))-accIj
其中，RL_to_I为激光雷达坐标系到惯性测量单元坐标系的旋转向量，RBj为所述B样条曲线上第j时刻的旋转向量，accBj为所述B样条曲线上第j时刻的第二加速度，G为所述激光雷达坐标系下第1时刻的重力加速度，accIj为所述惯性测量单元在第j时刻采集到的第一加速度。
在本申请的一些实施例中，所述标定方法还包括：对所述点云数据进行预处理以去除噪点；对预处理后的点云数据进行降采样处理，得到处理后的点云数据，所述处理后的点云数据用于转换为第二角速度、第二角加速度和第二加速度。
本申请的另一个方面还提供一种激光雷达与惯性测量单元标定系统，用于执行如上述所述的激光雷达与惯性测量单元标定方法，包括：数据获取单元，被配置为获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度，其中，所述N为大于等于1的整数；第一数据处理单元，被配置为将所述点云数据转换为第二角速度、第二角加速度和第二加速度；第二数据处理单元，被配置为基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值，所述标定值包括激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量；输出单元，被配置为输出所述标定值。
本申请提供一种激光雷达与惯性测量单元标定方法及系统，通过激光雷达测量的点云数据推导惯性测量单元的测量数据，再用推导的测量数据和实际的测量数据计算偏差，从而得到标定值，可以更准确的对激光雷达与惯性测量单元的相对位姿进行标定。
附图说明
以下附图详细描述了本申请中披露的示例性实施例。其中相同的附图标记在附图的若干视图中表示类似的结构。本领域的一般技术人员将理解这些实施例是非限制性的、示例性的实施例，附图仅用于说明和描述的目的，并不旨在限制本申请的范围，其他方式的实施例也可能同样的完成本申请中的发明意图。应当理解，附图未按比例绘制。其中：
图1为本申请实施例所述的激光雷达与惯性测量单元标定方法的流程图；
图2为本申请实施例所述的激光雷达与惯性测量单元标定系统的结构示意图。
具体实施方式
以下描述提供了本申请的特定应用场景和要求，目的是使本领域技术人员能够制造和使用本申请中的内容。对于本领域技术人员来说，对所公开的实施例的各种局部修改是显而易见的，并且在不脱离本申请的精神和范围的情况下，可以将这里定义的一般原理应用于其他实施例和应用。因此，本申请不限于所示的实施例，而是与权利要求一致的最宽范围。
下面结合实施例和附图对本发明技术方案进行详细说明。
图1为本申请实施例所述的激光雷达与惯性测量单元标定方法的流程图。
参考图1所示，本申请的实施例所述的一种激光雷达与惯性测量单元标定方法包括：
步骤S1：获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度，其中，所述N为大于等于1的整数；
步骤S2：将所述点云数据转换为第二角速度、第二角加速度和第二加速度；
步骤S3：基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值，所述标定值包括激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量。
需要说明的是，本申请实施例仅以自动驾驶车辆作为示范的应用场景，但这并不是限制，本申请的标定方法可以应用于其他任何适用的场景中。所述激光雷达和所述惯性测量单元安装于车辆上，在车辆行驶过程中，所述激光雷达一直获取车辆周围的环境点云数据，而所述惯性测量单元一直测量车辆的运行数据。
参考图1所示，步骤S1，获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度，其中，所述N为大于等于1的整数。
激光雷达是一种综合的光探测与测量系统，通过发射接受激光束，分析激光遇到目标对象后的折返时间，计算出目标对象与车辆的相对距离。目前常见的有16线、32线、64线激光雷达。激光雷达线束越多，测量精度越高，安全性越高。激光雷达测量的数据为点云数据。激光雷达通过发射激光和接收反射激光来获取周围环境的特征，激光雷达的点云数据通常会包括(x，y，z)和一些其他的信息，x，y，z是环境中的某一点在激光雷达坐标系中的位置。
惯性测量单元是一种测量物体三轴姿态角和加速度的装置，可以直接测量出车辆运行时的加速度以及转弯时的角速度(在本申请的实施例中，所述惯性测量单元测量的数据被标记为第一角速度和第一加速度)，从而推算出车辆在一段时间的运动姿态，可以应用在车辆定位中。其中，惯性测量数据包括但不限于加速度值和角速度值。当然，惯性测量数据还可以包括其它参数，本实施例在此不再一一列举。
在一些常规的标定方法中，需要将惯性测量单元测量的测量数据预积分处理成惯性测量单元的位姿信息，而预积分的过程则会引入一些误差。而本申请的技术方案中，不需要预先处理惯性测量单元测量的测量数据，后续可以直接使用测量到的第一角速度数据和第一加速度数据，避免了预积分带来的误差，可以使标定结果更准确。
在本申请的一些实施例中，所述第1时刻至第N时刻可以指的是第1秒至第N秒或者第1帧至第N帧等，其用于表现时间的变化。因为所述激光雷达和所述惯性测量单元是随时间变化而一直获取数据的，获取的数据是随时间变化的。
在本申请的一些实施例中，所述标定方法还包括：对所述点云数据进行预处理以去除噪点；对预处理后的点云数据进行降采样处理，得到处理后的点云数据，所述处理后的点云数据用于转换为第二角速度、第二角加速度和第二加速度。所述激光雷达采集到的点云数据不直接使用，而是先经过预处理，得到更精确的点云数据。
继续参考图1所示，步骤S2，将所述点云数据转换为第二角速度、第二角加速度和第二加速度。在对惯性测量单元和激光雷达的标定过程中，需要将激光雷达采集的点云数据以及惯性测量单元采集的惯性测量数据进行统一处理来计算标定值，而点云数据和惯性测量数据属于完全不同的数据种类，不能直接统一处理，因此需要将其中一种数据转换成与另一种数据相同种类的数据，才能统一进行处理。而在本申请的技术方案中，为了避免引入预积分误差，没有处理惯性测量数据，因此需要将点云数据转换成与惯性测量数据同类的数据，也就是角速度、角加速度和加速度。具体地，所述点云数据转换后的数据分别标记为第二角速度、第二角加速度和第二加速度。
下面详细说明本申请的技术方案的一个实施例中是如何处理点云数据的。
第一步，将所述激光雷达在第1时刻至第i时刻采集到的的点云数据分别记为P1至Pi，并获取P1到Pi的变换矩阵，所述i为大于1小于等于N的整数。
在本申请的一些实施例中，获取P1到Pi的变换矩阵的方法为：使用ICP算法进行计算。迭代最近点算法(Iterative closest point，ICP)通过迭代、寻找来不断搜索最近点，定义一个阀值最终完成多视图的拼合。当激光雷达运动时，环境中同一个点在激光雷达坐标系中的坐标值(x，y，z)会发生变化。假设环境中某一点P在1时刻的激光雷达坐标系下的坐标为(x1，y1，z1)，如果已知激光雷达从1时刻到j时刻的变换矩阵为4x4的矩阵Matrixj，可以求出P点在j时刻的激光雷达坐标系下的坐标(xj，yj，zj)。同理，如果已知环境点在1时刻激光雷达坐标系的坐标为(x1，y1，z1)，在j时刻的激光雷达坐标系下的坐标为(xj，yj，zj)，则(x1，y1，z1)通过一个变换矩阵转换为j时刻的激光雷达坐标系下的坐标与实际测量的坐标(xj，yj，zj)偏差最小时，该变换矩阵即为激光雷达从1时刻到j时刻的变换矩阵Matrixj。ICP算法通过迭代和搜索最近点来不断逼近实际的变换矩阵Matrixj。
第二步，将所述变换矩阵转换为6维平移旋转向量，所述6维平移旋转向量包括3维平移向量和3维旋转向量。
通过ICP算法可以得到4x4的变换矩阵，该矩阵的左上角3x3的子矩阵为3维旋转矩阵，该矩阵的右上角3x1的子矩阵为3维平移向量。假设3维旋转向量为(u＊θ，v＊θ，w＊θ)，(u，v，w)为旋转轴的单位向量(即u2+v2+w2＝1，θ为绕旋转轴(u，v，w)旋转的角度，则该3维旋转向量对应的3维旋转矩阵如下：
用M[i，j]表示3维旋转矩阵第i行第j列的值，可以得到如下等式：
M[0，0]+M[1，1]+M[2，2]＝1+2cosθ
M[2，1]-M[1，2]＝2usinθ
M[0，2]-M[2，0]＝2vsinθ
M[1，0]-M[0，1]＝2wsinθ
解上述方程组可以得到u，v，w，θ的值，从而求出3维旋转向量(u＊θ，v＊θ，w＊θ)。
在本申请的一些实施例中，所述旋转向量的旋转角度范围为[-π，π]。由于相邻两个旋转向量的旋转角度相差很小，如果旋转向量i+1与旋转向量i的角度之差大于π，则旋转向量i+1的旋转角度加上-2π；如果旋转向量i+1与旋转向量i的角度之差小于-π，则旋转向量i+1的旋转角度加上2π。旋转向量经过这样的处理后再拟合B样条曲线，可以保证B样条曲线的连续性。
第三步，将第1时刻到第N时刻之间的6维平移旋转向量拟合为一个B样条曲线。激光雷达在第1时刻到第N时刻之间的任意时刻的位姿都可以用所述B样条曲线上该时刻的6维平移旋转向量表示。
B样条曲线(B-spline curve)是指在数学的子学科数值分析里的一种特殊的表示形式。它是B样条基曲线的线性组合。B样条曲线曲面具有几何不变性、凸包性、保凸性、变差减小性、局部支撑性等许多优良性质。
第四步，将所述B样条曲线上的旋转向量求一阶导得到第二角速度，将所述B样条曲线上的旋转向量求二阶导得到第二角加速度，将所述B样条曲线上的平移向量求二阶导得到第二加速度。
通过上述四个步骤，则可以将点云数据处理成第二角速度、第二角加速度和第二加速度，能够在后续确定标定值的过程中使用。当然，在其他实施例中，也可以使用其他合适的方法来将点云数据处理成第二角速度、第二角加速度和第二加速度。
继续参考图1所示，步骤S3，基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值，所述标定值包括激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量。
下面结合公式详细说明确定所述标定值的方法。
步骤A，基于角速度误差函数计算得出第1时刻到第N时刻间每个时刻的角速度误差项。
定义函数f(R，T)表示3维旋转向量R转换为3×3的旋转矩阵后与3维向量T相乘，所述角速度误差函数为：
Errgyro＝f(RL_to_I，f(RBj，gyroBj))-gyroIj
其中，RL_to_I为激光雷达坐标系到惯性测量单元坐标系的旋转向量，RBj为所述B样条曲线上第j时刻的旋转向量，gyroBj为所述B样条曲线上第j时刻的第二角速度，gyroIj为所述惯性测量单元在第j时刻采集到的第一角速度，所述j为大于等于1小于等于N的整数。
在上述角速度误差函数中，仅有激光雷达坐标系到惯性测量单元坐标系的旋转向量RL_to_I为变量，其他参数都是已知的，因此计算得出的角速度误差项只有激光雷达坐标系到惯性测量单元坐标系的旋转向量RL_to_I一个变量。
步骤B，基于加速度误差函数计算得出第1时刻到第N时刻间每个时刻的加速度误差项。
所述B样条曲线在第j时刻的第二角速度产生的法向加速度为：
accBj_n＝gyroBj×(gyroBj×f(RBj，TL_to_I))
其中，gyroBj为所述B样条曲线上第j时刻的第二角速度，TL_to_I为激光雷达坐标系到惯性测量单元坐标系的平移向量，RBj为所述B样条曲线上第j时刻的旋转向量。
所述B样条曲线在第j时刻的第二角加速度产生的切向加速度为：
accBj_τ＝alphaBj×f(RBj，TL_to_I)
其中，alphaBj为所述B样条曲线上第j时刻的第二角加速度，TL_to_I为激光雷达坐标系到惯性测量单元坐标系的平移向量，RBj为所述B样条曲线上第j时刻的旋转向量。
根据上面两个公式可以分别计算出所述B样条曲线在第j时刻的第二角速度产生的法向加速度和所述B样条曲线在第j时刻的第二角加速度产生的切向加速度。然后代入下面的加速度误差函数中。
所述加速度误差函数为：
Erracc＝f(RL_to_I，f(RBj，(accBj+accBj_τ+accBj_n+G)))-accIj
其中，RL_to_I为激光雷达坐标系到惯性测量单元坐标系的旋转向量，RBj为所述B样条曲线上第j时刻的旋转向量，accBj为所述B样条曲线上第j时刻的第二加速度，G为所述激光雷达坐标系下第1时刻的重力加速度，accIj为所述惯性测量单元在第j时刻采集到的第一加速度。
在上述加速度误差函数中，仅有激光雷达坐标系到惯性测量单元坐标系的旋转向量RL_to_I和激光雷达坐标系到惯性测量单元坐标系的平移向量TL_to_I为变量，其他参数都是已知的，因此计算得出的加速度误差项只有激光雷达坐标系到惯性测量单元坐标系的旋转向量RL_to_I和激光雷达坐标系到惯性测量单元坐标系的平移向量TL_to_I两个变量。
步骤C，将所有的误差项相加得到总误差函数，所述总误差函数中仅有所述激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量为变量，使所述总误差函数达到最小值的所述激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量为所述标定值。
所述总误差函数也是由加速度误差项和角速度误差项相加得到，因此，所述总误差函数中只有激光雷达坐标系到惯性测量单元坐标系的旋转向量RL_to_I和激光雷达坐标系到惯性测量单元坐标系的平移向量TL_to_I两个变量。而所述总误差函数达到最小值时，说明此时激光雷达和惯性测量单元的相对位姿最准确，因此此时所述激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量为所述标定值。
在本申请的一些实施例中，所述步骤A和所述步骤B的顺序可以交换。
在本申请的技术方案中，对激光雷达采集的点云数据进行ICP匹配得到第1时刻到第i时刻点云数据的变换矩阵，由于每一次ICP匹配都是独立的，所以不会引入累积误差，一共得到N个变换矩阵。再将变换矩阵转换为6维平移旋转向量，用N个6维平移旋转向量构造一条B样条曲线。对于每一时刻的IMU数据，都可以找到B样条曲线上对应时刻的6维平移旋转向量，由于B样条曲线是连续且光滑的，可以对B样条曲线求导再计算与IMU测量数据的偏差。目前常用的IMU预积分的方法是通过对IMU的测量数据积分来预测激光雷达的位姿，而本申请采用的B样条曲线拟合则是对拟合的激光雷达位姿求导来预测单帧的IMU测量数据，可以很好地避免积分引入的累积误差。本申请采用的标定方法要求在采集数据的过程中，激光雷达和IMU的运动轨迹要尽量保证二次光滑，以达到更好的拟合效果。
本申请提供一种激光雷达与惯性测量单元标定方法，通过激光雷达测量的点云数据推导惯性测量单元的测量数据，再用推导的测量数据和实际的测量数据计算偏差，从而得到标定值，可以更准确的对激光雷达与惯性测量单元的相对位姿进行标定。
图2为本申请实施例所述的激光雷达与惯性测量单元标定系统的结构示意图。
本申请的实施例还提供一种激光雷达与惯性测量单元标定系统，用于执行如上述所述的激光雷达与惯性测量单元标定方法参考图2所示，包括：数据获取单元110，被配置为获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度，其中，所述N为大于等于1的整数；第一数据处理单元120，被配置为将所述点云数据转换为第二角速度、第二角加速度和第二加速度；第二数据处理单元130，被配置为基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值，所述标定值包括激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量；输出单元140，被配置为输出所述标定值。
在本申请的一些实施例中，所述标定系统可以安装于车辆中，或者是与车辆相独立的设备，本申请不对标定系统的实现方式作限定。所述标定装置与当前车辆中激光雷达和IMU通信连接，可以获取到激光雷达采集的点云数据以及IMU采集的惯性测量数据。
在本申请的一些实施例中，所述数据获取单元110与车辆上的激光雷达和IMU通信连接，可以获取到激光雷达采集的点云数据以及IMU采集的惯性测量数据。在本申请的另一些实施例中，也可以由工作人员手动向所述数据获取单元110输入激光雷达采集的点云数据以及IMU采集的惯性测量数据。
所述第一数据处理单元120与所述数据获取单元110通信连接，被配置为将所述点云数据转换为第二角速度、第二角加速度和第二加速度。具体地数据处理方法前文已经详细描述过，在此不做赘述。
所述第二数据处理单元130与所述数据获取单元110以及所述第一数据处理单元120通信连接，被配置为基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值。其中，所述第二数据处理单元130从所述数据获取单元110获取所述第一角速度和第一加速度；所述第二数据处理单元130从所述第一数据处理单元120获取所述第二角速度、第二角加速度和第二加速度。确定所述标定值的计算过程前文已经详细描述过，在此不做赘述。
所述输出单元140与所述第二数据处理单元130通信连接，被配置为输出所述标定值。
所述数据获取单元110可以是一个存储器，所述第一数据处理单元120和第二数据处理单元130可以分别是处理器。
本申请提供一种激光雷达与惯性测量单元标定方法及系统，通过激光雷达测量的点云数据推导惯性测量单元的测量数据，再用推导的测量数据和实际的测量数据计算偏差，从而得到标定值，可以更准确的对激光雷达与惯性测量单元的相对位姿进行标定。
综上所述，在阅读本申请内容之后，本领域技术人员可以明白，前述申请内容可以仅以示例的方式呈现，并且可以不是限制性的。尽管这里没有明确说明，本领域技术人员可以理解本申请意图囊括对实施例的各种合理改变，改进和修改。这些改变，改进和修改都在本申请的示例性实施例的精神和范围内。
应当理解，本实施例使用的术语″和/或″包括相关联的列出项目中的一个或多个的任意或全部组合。应当理解，当一个元件被称作″连接″或″耦接″至另一个元件时，其可以直接地连接或耦接至另一个元件，或者也可以存在中间元件。
还应当理解，术语″包含″、″包含着″、″包括″或者″包括着″，在本申请文件中使用时，指明存在所记载的特征、整体、步骤、操作、元件和/或组件，但并不排除存在或附加一个或多个其他特征、整体、步骤、操作、元件、组件和/或它们的组。
还应当理解，尽管术语第一、第二、第三等可以在此用于描述各种元件，但是这些元件不应当被这些术语所限制。这些术语仅用于将一个元件与另一个元件区分开。因此，在没有脱离本申请的教导的情况下，在一些实施例中的第一元件在其他实施例中可以被称为第二元件。相同的参考标号或相同的参考标记符在整个说明书中表示相同的元件。
此外，本申请说明书通过参考理想化的示例性截面图和/或平面图和/或立体图来描述示例性实施例。因此，由于例如制造技术和/或容差导致的与图示的形状的不同是可预见的。因此，不应当将示例性实施例解释为限于在此所示出的区域的形状，而是应当包括由例如制造所导致的形状中的偏差。因此，在图中示出的区域实质上是示意性的，其形状不是为了示出器件的区域的实际形状也不是为了限制示例性实施例的范围。
Claims (10)
1.一种激光雷达与惯性测量单元标定方法，其特征在于，包括：
获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度，其中，所述N为大于等于1的整数；
将所述点云数据转换为第二角速度、第二角加速度和第二加速度；
基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值，所述标定值包括激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量。
2.如权利要求1所述的标定方法，其特征在于，将所述点云数据转换为第二角速度、第二角加速度和第二加速度的方法包括：
将所述激光雷达在第1时刻至第i时刻采集到的的点云数据分别记为P1至Pi，并获取P1到Pi的变换矩阵，所述i为大于1小于等于N的整数；
将所述变换矩阵转换为6维平移旋转向量，所述6维平移旋转向量包括3维平移向量和3维旋转向量；
将第1时刻到第N时刻之间的6维平移旋转向量拟合为B样条曲线；
将所述B样条曲线上的旋转向量求一阶导得到第二角速度，将所述B样条曲线上的旋转向量求二阶导得到第二角加速度，将所述B样条曲线上的平移向量求二阶导得到第二加速度。
3.如权利要求2所述的标定方法，其特征在于，所述旋转向量的旋转角度范围为[-π，π]。
4.如权利要求2所述的标定方法，其特征在于，基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值的方法包括：
基于角速度误差函数计算得出第1时刻到第N时刻间每个时刻的角速度误差项；
基于加速度误差函数计算得出第1时刻到第N时刻间每个时刻的加速度误差项；
将所有的误差项相加得到总误差函数，所述总误差函数中仅有所述激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量为变量，使所述总误差函数达到最小值的所述激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量为所述标定值。
5.如权利要求4所述的标定方法，其特征在于，定义函数f(R，T)表示3维旋转向量R转换为3×3的旋转矩阵后与3维向量T相乘，所述角速度误差函数为：
Errgyro＝f(RL_to_I，f(RBj，gyroBj))-gyroIj
其中，RL_to_I为激光雷达坐标系到惯性测量单元坐标系的旋转向量，RBj为所述B样条曲线上第j时刻的旋转向量，gyroBj为所述B样条曲线上第j时刻的第二角速度，gyroIj为所述惯性测量单元在第j时刻采集到的第一角速度，所述j为大于等于1小于等于N的整数。
6.如权利要求5所述的标定方法，其特征在于，所述B样条曲线在第j时刻的第二角速度产生的法向加速度为：
accBj_n＝gyroBj×(gyroBj×f(RBj，TL_to_I))
其中，gyroBj为所述B样条曲线上第j时刻的第二角速度，TL_to_I为激光雷达坐标系到惯性测量单元坐标系的平移向量，RBj为所述B样条曲线上第j时刻的旋转向量。
7.如权利要求6所述的标定方法，其特征在于，所述B样条曲线在第j时刻的第二角加速度产生的切向加速度为：
accBj_τ＝alphaBj×f(RBj，TL_to_I)
其中，alphaBj为所述B样条曲线上第j时刻的第二角加速度，TL_to_I为激光雷达坐标系到惯性测量单元坐标系的平移向量，RBj为所述B样条曲线上第j时刻的旋转向量。
8.如权利要求7所述的标定方法，其特征在于，所述加速度误差函数为：
Erracc＝f(RL_to_I，f(RBj，(accBj+accBj_τ+accBj_n+G)))-accIj
其中，RL_to_I为激光雷达坐标系到惯性测量单元坐标系的旋转向量，RBj为所述B样条曲线上第j时刻的旋转向量，accBj为所述B样条曲线上第j时刻的第二加速度，G为所述激光雷达坐标系下第1时刻的重力加速度，accIj为所述惯性测量单元在第j时刻采集到的第一加速度。
9.如权利要求1所述的标定方法，其特征在于，还包括：对所述点云数据进行预处理以去除噪点；对预处理后的点云数据进行降采样处理，得到处理后的点云数据，所述处理后的点云数据用于转换为第二角速度、第二角加速度和第二加速度。
10.一种激光雷达与惯性测量单元标定系统，用于执行如权利要求1至9所述的激光雷达与惯性测量单元标定方法，其特征在于，包括：
数据获取单元，被配置为获取激光雷达在第1时刻到第N时刻之间采集到的点云数据和惯性测量单元在第1时刻到第N时刻之间采集到的惯性测量数据，所述惯性测量数据包括第一角速度和第一加速度，其中，所述N为大于等于1的整数；
第一数据处理单元，被配置为将所述点云数据转换为第二角速度、第二角加速度和第二加速度；
第二数据处理单元，被配置为基于角速度误差函数、加速度误差函数、所述第二角速度、第二角加速度和第二加速度以及所述第一角速度和第一加速度确定标定值，所述标定值包括激光雷达坐标系到惯性测量单元坐标系的平移向量和旋转向量；
输出单元，被配置为输出所述标定值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210241914.1A CN114518111A (zh) | 2022-03-11 | 2022-03-11 | 一种激光雷达与惯性测量单元标定方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210241914.1A CN114518111A (zh) | 2022-03-11 | 2022-03-11 | 一种激光雷达与惯性测量单元标定方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114518111A true CN114518111A (zh) | 2022-05-20 |
Family
ID=81600019
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210241914.1A Pending CN114518111A (zh) | 2022-03-11 | 2022-03-11 | 一种激光雷达与惯性测量单元标定方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114518111A (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
| CN112147599A (zh) * | 2019-06-28 | 2020-12-29 | 浙江大学 | 一种基于样条函数的连续时间上3d激光雷达和惯性传感器外参标定方法 |
| CN112577517A (zh) * | 2020-11-13 | 2021-03-30 | 上汽大众汽车有限公司 | 一种多元定位传感器联合标定方法和系统 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN113340295A (zh) * | 2021-06-16 | 2021-09-03 | 广东工业大学 | 一种多个测距传感器的无人艇近岸实时定位与建图方法 |
| CN113759384A (zh) * | 2020-09-22 | 2021-12-07 | 北京京东乾石科技有限公司 | 一种传感器位姿转换关系确定方法、装置、设备和介质 |
-
2022
- 2022-03-11 CN CN202210241914.1A patent/CN114518111A/zh active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112147599A (zh) * | 2019-06-28 | 2020-12-29 | 浙江大学 | 一种基于样条函数的连续时间上3d激光雷达和惯性传感器外参标定方法 |
| CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
| CN113759384A (zh) * | 2020-09-22 | 2021-12-07 | 北京京东乾石科技有限公司 | 一种传感器位姿转换关系确定方法、装置、设备和介质 |
| CN112577517A (zh) * | 2020-11-13 | 2021-03-30 | 上汽大众汽车有限公司 | 一种多元定位传感器联合标定方法和系统 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN113340295A (zh) * | 2021-06-16 | 2021-09-03 | 广东工业大学 | 一种多个测距传感器的无人艇近岸实时定位与建图方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107063228B (zh) | 基于双目视觉的目标姿态解算方法 | |
| CN105021124B (zh) | 一种基于深度图的平面零件三维位置和法向量计算方法 | |
| CN111795686B (zh) | 一种移动机器人定位与建图的方法 | |
| JP2018124787A (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
| CN111123242B (zh) | 一种基于激光雷达和相机的联合标定方法及计算机可读存储介质 | |
| CN112017248B (zh) | 一种基于点线特征的2d激光雷达相机多帧单步标定方法 | |
| CN107765244B (zh) | 基于机载双天线InSAR基线测量方法和装置 | |
| KR102490521B1 (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 | |
| CN114972532B (zh) | 激光雷达之间的外参标定方法、装置、设备及存储介质 | |
| CN115685160A (zh) | 基于标靶的激光雷达与相机标定方法、系统、电子设备 | |
| Lourenço et al. | A globally exponentially stable filter for bearing-only simultaneous localization and mapping with monocular vision | |
| CN117308982A (zh) | 一种核电站水下清洁机器人的定位方法及装置 | |
| CN113538699A (zh) | 基于三维点云的定位方法、装置、设备及存储介质 | |
| CN114518111A (zh) | 一种激光雷达与惯性测量单元标定方法及系统 | |
| CN116413706A (zh) | 一种移动载体上激光雷达同时建图和内参标定方法 | |
| CN113405532B (zh) | 基于视觉系统结构参数的前方交会测量方法及系统 | |
| CN114879168A (zh) | 一种激光雷达与imu标定方法及系统 | |
| US20230059883A1 (en) | Identification of planar points in lidar point cloud obtained with vehicle lidar system | |
| CN112230194B (zh) | 一种基于平移阵列的解模糊方法、设备及存储介质 | |
| CN114397642A (zh) | 一种基于图优化的三维激光雷达与imu外参标定方法 | |
| CN114419118A (zh) | 三维点云配准方法、移动设备及存储介质 | |
| CN114076946A (zh) | 一种运动估计方法及装置 | |
| CN113504385A (zh) | 复数相机测速方法及测速装置 | |
| CN111596309A (zh) | 一种基于激光雷达的车辆排队测量方法 | |
| CN117433511B (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 |