CN114879168A - 一种激光雷达与imu标定方法及系统 - Google Patents
一种激光雷达与imu标定方法及系统 Download PDFInfo
- Publication number
- CN114879168A CN114879168A CN202210433144.0A CN202210433144A CN114879168A CN 114879168 A CN114879168 A CN 114879168A CN 202210433144 A CN202210433144 A CN 202210433144A CN 114879168 A CN114879168 A CN 114879168A
- Authority
- CN
- China
- Prior art keywords
- transformation matrix
- imu
- main
- calibration plate
- laser radar
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manufacturing & Machinery (AREA)
- Computer Networks & Wireless Communication (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本申请提供一种激光雷达、IMU和相机标定方法及系统,所述标定方法包括:设置激光雷达、IMU、主相机和主标定板;获取所述激光雷达采集到的所述主标定板的点云数据、所述主相机采集到的所述主标定板的图像数据和所述IMU采集到的惯性测量数据;构建图优化模型,以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项;通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。本申请所述的激光雷达与IMU标定方法及系统,能够实现相机、激光雷达、IMU三种传感器的同时标定。
Description
技术领域
本申请涉及车辆自动驾驶领域,尤其涉及一种激光雷达与IMU标定方法及系统。
背景技术
当前,自动驾驶技术飞速发展,市场需求越来越大。高精度定位是车辆实现自动驾驶的关键技术,这需要使用多种传感器优势互补。市场上多依赖相机、激光雷达、IMU等多类型传感器进行融合定位。实现多传感器融合定位的前提是提前获知传感器之间精确的位姿关系,这需要通过标定技术对其位姿关系进行标定。
然而,市场上多以激光雷达和IMU、相机和激光雷达、相机和IMU等两类传感器之间的标定方法为主,当需要获知三个传感器的位姿关系时,就需要使用两种以上的方法进行标定计算,通过位姿转换,获得相机、激光雷达、IMU之间的位姿关系,面临多个传感器之间的标定结果精度不一致、标定场景要求多以及位姿转换等多种不利因素,导致整体标定精度较低。同时,自动驾驶场景越来越丰富,其使用的相机、激光雷达的数目往往不止一个,一般安装有多个相机、多个激光雷达,需要同时对其进行高精度标定,常规标定方法的局限性越来越大,已经难以满足现实工程的需要,这要求标定方法在激光雷达、相机各自的数目多于一个的情况下,不但能够同时标定激光雷达、相机和IMU的位姿关系,且需要保证良好的精度。
因此,有必要提供更可靠、更有效的技术方案。
发明内容
本申请提供一种激光雷达与IMU标定方法及系统,能够实现相机、激光雷达、IMU三种传感器的同时标定,且参与同时标定的激光雷达、相机各自的数目可以多于一个。
本申请的一个方面提供一种激光雷达、IMU和相机标定方法,包括:设置激光雷达、IMU、主相机和主标定板,并使所述主标定板位于所述激光雷达和主相机的采集范围内,获取所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵;获取所述激光雷达采集到的所述主标定板的点云数据、所述主相机采集到的所述主标定板的图像数据和所述IMU采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度;构建图优化模型,使用所述图优化模型获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,包括:以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项;通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵;根据所述图像数据获取所述主标定板和所述主相机之间的变换矩阵。
在本申请的一些实施例中,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵的方法包括:在所述主标定板上沿边缘设置相互垂直的两条高反射率条;提取位于所述高反射率条上的所有激光雷达点云,并拟合出两条直线;基于所述两条直线确定所述主标定板的平面位置,通过阈值聚类方法提取位于所述主标定板平面上的点云;基于所述主标定板平面上的点云,获取所述主标定板和所述激光雷达之间的变换矩阵。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:设置参考坐标系,以所述主标定板与所述参考坐标系之间的初始变换矩阵作为误差项,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:获取所述主相机和所述参考坐标系之间的变换矩阵的样条曲线;在所述样条曲线时间范围内,将所述IMU的角速度测量值和观测值作为误差项,其中,所述测量值由IMU采集得到,所述观测值为所述主相机和所述IMU之间的初始变换矩阵与所述样条曲线中的角速度的乘积。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:以所述主相机和所述激光雷达之间的变换矩阵的预测值与观测值作为误差项,其中,所述预测值中的旋转矩阵设置为单位矩阵,所述预测值中的位姿向量取零向量,所述观测值为所述参考坐标系和所述激光雷达之间的变换矩阵与所述主相机和所述参考坐标系之间的变换矩阵的乘积。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:基于所述IMU和所述参考坐标系之间的变换矩阵构建IMU样条曲线,所述IMU样条曲线作为误差项,其中,所述IMU和所述参考坐标系之间的变换矩阵为所述主标定板和所述参考坐标系之间的变换矩阵与所述IMU和所述主标定板之间的变换矩阵的乘积,所述IMU和所述主标定板之间的变换矩阵为所述IMU和所述主相机之间的变换矩阵与所述主相机和所述主标定板之间的变换矩阵的乘积。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括在所述IMU样条曲线的时间范围内初始化IMU加速度和陀螺仪样条曲线,并将所述IMU加速度和陀螺仪样条曲线作为误差项。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:根据所述图像数据获取所述主标定板到所述主相机的重投影误差,将所述重投影误差作为误差项。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:根据所述激光雷达和所述IMU之间的变换矩阵将激光雷达点云转换到IMU坐标系;根据所述IMU和所述参考坐标系之间的变换矩阵将激光雷达点云从IMU坐标系转换到参考坐标系;根据所述主标定板和所述参考坐标系之间的变换矩阵将激光雷达点云从参考坐标系转换到主标定板坐标系;将所述激光雷达与所述主标定板之间的位移投影到主标定板平面垂直方向,提取所述激光雷达与所述主标定板平面距离,所述激光雷达和所述主标定板的变换矩阵由参考坐标系和主标定板之间的变换矩阵与激光雷达和参考坐标系之间的变换矩阵的乘积求得,其中,激光雷达和参考坐标系之间的变换矩阵由IMU和参考坐标系之间的变换矩阵与激光雷达和IMU之间的变换矩阵乘积求得;基于主标定板上激光雷达点云的坐标,获取由激光雷达到主标定板上点云方向的单位向量,将该单位向量转换到标定板坐标系后,投影到标定板平面垂直方向上,提取所述激光雷达与所述主标定板单位距离;使用所述激光雷达与所述主标定板平面距离除以所述激光雷达与所述主标定板单位距离,得到激光雷达到主标定板距离的预测值,所述预测值作为误差项;将主标定板上激光雷达点云三维坐标所确定的与激光雷达的距离作为观测值,所述观测值作为误差项。
在本申请的一些实施例中,所述标定方法还包括:设置若干副标定板,以所有标定板之间的变换矩阵的观测值和测量值作为误差项,其中,所述观测值基于所有标定板与主相机之间的测量值求得,所述预测值为参考坐标系与所有标定板之间的初始变换矩阵,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
在本申请的一些实施例中,所述标定方法还包括:设置若干副相机,以所述主相机和每个副相机之间的变换矩阵的预测值和观测值为误差项,其中,所述预测值为各个相机之间的初始变换矩阵,所述初始变换矩阵中的旋转矩阵设置为单位矩阵,所述初始变换矩阵中的位姿向量取零向量,所述测量值为参考坐标系和每个相机之间的变换矩阵与主相机和参考坐标系之间的变换矩阵的乘积,所述参考坐标系和每个相机之间的变换矩阵中,旋转矩阵设置为单位矩阵,位姿向量取零向量。
本申请的另一个方面还提供一种激光雷达、IMU和相机标定系统,包括:激光雷达、IMU、主相机和主标定板,其中,所述主标定板位于所述激光雷达和主相机的采集范围内;数据获取模块,被配置为获取所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵以及获取所述激光雷达采集到的所述主标定板的点云数据、所述主相机采集到的所述主标定板的图像数据和所述IMU采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度;图优化模型,被配置为获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,所述图优化模型工作时:以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵;根据所述图像数据获取所述主标定板和所述主相机之间的变换矩阵。
在本申请的一些实施例中,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵的方法包括:在所述主标定板上沿边缘设置相互垂直的两条高反射率条;提取位于所述高反射率条上的所有激光雷达点云,并拟合出两条直线;基于所述两条直线确定所述主标定板的平面位置,通过阈值聚类方法提取位于所述主标定板平面上的点云;基于所述主标定板平面上的点云,获取所述主标定板和所述激光雷达之间的变换矩阵。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:设置参考坐标系,以所述主标定板与所述参考坐标系之间的初始变换矩阵作为误差项,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:获取所述主相机和所述参考坐标系之间的变换矩阵的样条曲线;在所述样条曲线时间范围内,将所述IMU的角速度测量值和观测值作为误差项,其中,所述测量值由IMU采集得到,所述观测值为所述主相机和所述IMU之间的初始变换矩阵与所述样条曲线中的角速度的乘积。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:以所述主相机和所述激光雷达之间的变换矩阵的预测值与观测值作为误差项,其中,所述预测值中的旋转矩阵设置为单位矩阵,所述预测值中的位姿向量取零向量,所述观测值为所述参考坐标系和所述激光雷达之间的变换矩阵与所述主相机和所述参考坐标系之间的变换矩阵的乘积。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:基于所述IMU和所述参考坐标系之间的变换矩阵构建IMU样条曲线,所述IMU样条曲线作为误差项,其中,所述IMU和所述参考坐标系之间的变换矩阵为所述主标定板和所述参考坐标系之间的变换矩阵与所述IMU和所述主标定板之间的变换矩阵的乘积,所述IMU和所述主标定板之间的变换矩阵为所述IMU和所述主相机之间的变换矩阵与所述主相机和所述主标定板之间的变换矩阵的乘积。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:在所述IMU样条曲线的时间范围内,初始化IMU加速度和陀螺仪样条曲线,并将所述IMU加速度和陀螺仪样条曲线作为误差项。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:根据所述图像数据获取所述主标定板到所述主相机的重投影误差,将所述重投影误差作为误差项。
在本申请的一些实施例中,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:根据所述激光雷达和所述IMU之间的变换矩阵将激光雷达点云转换到IMU坐标系;根据所述IMU和所述参考坐标系之间的变换矩阵将激光雷达点云从IMU坐标系转换到参考坐标系;根据所述主标定板和所述参考坐标系之间的变换矩阵将激光雷达点云从参考坐标系转换到主标定板坐标系;将所述激光雷达与所述主标定板之间的位移投影到主标定板平面垂直方向,提取所述激光雷达与所述主标定板平面距离,所述激光雷达和所述主标定板的变换矩阵由参考坐标系和主标定板之间的变换矩阵与激光雷达和参考坐标系之间的变换矩阵的乘积求得,其中,激光雷达和参考坐标系之间的变换矩阵由IMU和参考坐标系之间的变换矩阵与激光雷达和IMU之间的变换矩阵乘积求得;基于主标定板上激光雷达点云的坐标,获取由激光雷达到主标定板上点云方向的单位向量,将该单位向量转换到标定板坐标系后,投影到标定板平面垂直方向上,提取所述激光雷达与所述主标定板单位距离;使用所述激光雷达与所述主标定板平面距离除以所述激光雷达与所述主标定板单位距离,得到激光雷达到主标定板距离的预测值,所述预测值作为误差项;将主标定板上激光雷达点云三维坐标所确定的与激光雷达的距离作为观测值,所述观测值作为误差项。
在本申请的一些实施例中,所述标定系统还包括:若干副标定板,以所有标定板之间的变换矩阵的观测值和测量值作为误差项,其中,所述观测值基于所有标定板与主相机之间的测量值求得,所述预测值为参考坐标系与所有标定板之间的初始变换矩阵,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
在本申请的一些实施例中,所述标定系统还包括:若干副相机,以所述主相机和每个副相机之间的变换矩阵的预测值和观测值为误差项,其中,所述预测值为各个相机之间的初始变换矩阵,所述初始变换矩阵中的旋转矩阵设置为单位矩阵,所述初始变换矩阵中的位姿向量取零向量,所述测量值为参考坐标系和每个相机之间的变换矩阵与主相机和参考坐标系之间的变换矩阵的乘积,所述参考坐标系和每个相机之间的变换矩阵中,旋转矩阵设置为单位矩阵,位姿向量取零向量。
本申请提供一种激光雷达与IMU标定方法及系统,可以实现相机、激光雷达、IMU三种传感器外参的同时高精度标定,并且,相机、激光雷达的数目均可以多于一个,不但统一了标定场景,而且有助于节省标定成本和容易实现,有利于工程化落地。
附图说明
以下附图详细描述了本申请中披露的示例性实施例。其中相同的附图标记在附图的若干视图中表示类似的结构。本领域的一般技术人员将理解这些实施例是非限制性的、示例性的实施例,附图仅用于说明和描述的目的,并不旨在限制本申请的范围,其他方式的实施例也可能同样的完成本申请中的发明意图。应当理解,附图未按比例绘制。其中:
图1为本申请实施例所述的激光雷达、IMU和相机标定方法的流程图;
图2为本申请实施例所述的激光雷达、IMU和相机标定系统的结构示意图。
具体实施方式
以下描述提供了本申请的特定应用场景和要求,目的是使本领域技术人员能够制造和使用本申请中的内容。对于本领域技术人员来说,对所公开的实施例的各种局部修改是显而易见的,并且在不脱离本申请的精神和范围的情况下,可以将这里定义的一般原理应用于其他实施例和应用。因此,本申请不限于所示的实施例,而是与权利要求一致的最宽范围。
下面结合实施例和附图对本发明技术方案进行详细说明。
图1为本申请实施例所述的激光雷达、IMU和相机标定方法的流程图。
本申请的实施例提供一种激光雷达、IMU和相机标定方法,参考图1所示,包括:
步骤S1:设置激光雷达、IMU、主相机和主标定板,并使所述主标定板位于所述激光雷达和主相机的采集范围内,获取所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵;
步骤S2:获取所述激光雷达采集到的所述主标定板的点云数据、所述主相机采集到的所述主标定板的图像数据和所述IMU采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度;
步骤S3:构建图优化模型,使用所述图优化模型获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
下面结合附图1对本申请实施例所述的激光雷达、IMU和相机标定方法进行详细说明。
参考图1所示,步骤S1,设置激光雷达、IMU、主相机和主标定板,并使所述主标定板位于所述激光雷达和主相机的采集范围内,获取所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵。
在对激光雷达、IMU、主相机之间的位姿关系进行标定的方法中,需要构建一个模拟系统来获取当激光雷达、IMU、主相机处于移动状态时的相互位姿关系,然后利用移动状态时的相互位姿关系和所述模拟系统的初始相互位姿关系进行对比,标定出最佳的激光雷达、IMU、主相机的相互位姿关系。
因此,在本申请的技术方案中,首先构建一个用于测试模拟的模拟系统。所述模拟系统包括需要标定的激光雷达、IMU、主相机。为了提高模拟真实性,在一些实施例中,所述激光雷达、IMU、主相机设置于汽车上,然后在所述汽车的周围设置主标定板,在模拟过程中,控制所述汽车万向移动,并通过激光雷达、IMU、主相机采集相应的数据。在另一些实施例中,为了简便,不使用真实汽车,而是将所述激光雷达、IMU、主相机设置于支架上,模拟所述激光雷达、IMU、主相机在汽车上的真实安装情况,然后在支架周围设置主标定板,在模拟过程中,控制所述支架万向移动,并通过激光雷达、IMU、主相机采集相应的数据。
所述主标定板上设置有网格,用于模拟激光雷达、IMU、主相机的周围环境,辅助测量激光雷达、IMU、主相机的相互位姿关系。因此,所述主标定板必须位于激光雷达、IMU、主相机的测量范围内。
设置好激光雷达、IMU、主相机后,记录所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵,也就是所述激光雷达、IMU、主相机和主标定板之间的初始位姿关系。所述初始变换矩阵指的是设置所述激光雷达、IMU、主相机和主标定板时的设计位姿关系。
在本申请的另一些实施例中,所述激光雷达的数量可以是一个或多个。相机的数量也可以是一个或多个,当相机的数量为一个时,所述一个相机为主相机,当相机的数量为多个时,所述多个相机包括一个主相机和至少一个副相机。标定板的数量也可以是一个或多个,当标定板的数量为一个时,所述一个标定板为主标定板,当标定板的数量为多个时,所述多个标定板包括一个主标定板和至少一个副标定板。当有多个激光雷达、相机和标定板时,要记录所有激光雷达、所有相机和所有标定板的初始变换矩阵。
在本申请的一些实施例中,激光雷达、IMU、相机和标定板的内参数据可以采用现有标定方法进行标定,并将所有内参数据保存于数据获取模块的存储器中,以供后续步骤使用。
具体地,相机的内参数据包括:相机类型、内参矩阵、畸变模型、行径向畸变、切向畸变、图像分辨率等参数。IMU的内参数据包括:加速度计和陀螺仪的噪声强度值与随机游走值,采样频率等。激光雷达的内参数据包括:降采样后每帧激光雷达需要保留的点云数等。标定板的内参数据包括:标定板数目与类型,标定板棋盘格参数等。
在本申请的一些实施例中,标定板采用Aprilgrid标定板,其中,在主标定板边缘,沿着x,y两个轴贴有高反条,以方便通过反射的激光雷达点云提取该标定板位姿矩阵,且标定板数目不小于3个,以确保每个传感器(激光雷达、IMU、相机)视场范围内都能有效探测到标定板。
继续参考图1所示,步骤S2,获取所述激光雷达采集到的所述主标定板的点云数据、所述主相机采集到的所述主标定板的图像数据和所述IMU采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度。
具体地,控制所述汽车或者支架进行万向运动,在运动过程中,通过设置于所述汽车或者支架上的传感器(激光雷达、IMU、相机)获取相应的数据。
激光雷达是一种综合的光探测与测量系统,通过发射接受激光束,分析激光遇到目标对象后的折返时间,计算出目标对象与车辆的相对距离。目前常见的有16线、32线、64线激光雷达。激光雷达线束越多,测量精度越高,安全性越高。激光雷达测量的数据为点云数据。激光雷达通过发射激光和接收反射激光来获取周围环境的特征,激光雷达的点云数据通常会包括(x,y,z)和一些其他的信息,x,y,z是环境中的某一点在激光雷达坐标系中的位置。
惯性测量单元(IMU)是一种测量物体三轴姿态角和加速度的装置,可以直接测量出车辆运行时的加速度以及转弯时的角速度(在本申请的实施例中,所述惯性测量单元测量的数据被标记为第一角速度和第一加速度),从而推算出车辆在一段时间的运动姿态,可以应用在车辆定位中。其中,惯性测量数据包括但不限于加速度值和角速度值。
相机用于拍摄周围环境的图像,并根据图像数据获取图像中的物体与相机的距离等位姿关系。
继续参考图1所示,步骤S3,构建图优化模型,使用所述图优化模型获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
具体地,使用所述图优化模型获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵的方法包括:以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项;通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
图优化是一种数学建模的工具,图优化模型是数学建模的结果。传感器外参标定问题本质上是传感器之间位置关系的最优估计问题,图优化建模就是把外参标定这个实际工程问题转化为数学上的最优化问题。通过求解这个数学上的最优化问题就可以得到传感器之间最可信的相对位置关系。图优化模型的构建过程就是将实际工程问题首先构建为数学上的图模型,然后由图模型转化出最小二乘优化问题。构建出的图模型中节点表示系统中传感器及标定工具在不同的采样时间的物理空间位置,图模型中连接节点的边表示传感器的实际测量值。当系统中所有涉及的对象的空间位置已知时,我们可以通过传感器的功能模型计算出传感器的计算测量值,计算测量值与实际测量值之差就是最优化问题中的误差项,优化问题的求解过程就是不断调节所有节点的空间位置使所有误差项的平方和不断减小。所有误差项的平方和达到最小时的空间位置是系统最优空间位置,传感器之间的最优空间位置之差就是传感器之间的最优外参估计。
构建好图优化模型后,向所述图优化模型中输入各种误差项,利用这些误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,也就是最终的标定数据。具体地,所述误差项的种类以及如何获取误差项如下所述。
首先,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵(在后续计算中使用);根据所述图像数据获取所述主标定板和所述主相机之间的变换矩阵(在后续计算中使用)。其中,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵的方法包括:在所述主标定板上沿边缘设置相互垂直的两条高反射率条;提取位于所述高反射率条上的所有激光雷达点云,并拟合出两条直线;基于所述两条直线确定所述主标定板的平面位置,通过阈值聚类方法提取位于所述主标定板平面上的点云;基于所述主标定板平面上的点云,获取所述主标定板和所述激光雷达之间的变换矩阵。
在一些实施例中,设置参考坐标系,以所述主标定板与所述参考坐标系之间的初始变换矩阵作为误差项,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。所述参考坐标系用于作为不同传感器的共同参考关联不同传感器的坐标系和标定板的坐标系。
在一些实施例中,获取所述主相机和所述参考坐标系之间的变换矩阵的样条曲线;在所述样条曲线时间范围内,将所述IMU的角速度测量值和通过对样条曲线微分计算出的观测值作为误差项,其中,所述测量值由IMU采集得到,所述观测值为所述主相机和所述IMU之间的初始变换矩阵与所述样条曲线中的角速度的乘积。将所述主相机和所述参考坐标系之间的变换矩阵拟合成样条曲线的方法为常规方法。
在一些实施例中,以所述主相机和所述激光雷达之间的变换矩阵的预测值(初始设定值)与观测值(通过共视反推)作为误差项,其中,所述预测值中的旋转矩阵设置为单位矩阵,所述预测值中的位姿向量取零向量,所述观测值为所述参考坐标系和所述激光雷达之间的变换矩阵与所述主相机和所述参考坐标系之间的变换矩阵的乘积。
在一些实施例中,基于所述IMU和所述参考坐标系之间的变换矩阵构建IMU样条曲线,其中,所述IMU和所述参考坐标系之间的变换矩阵为所述主标定板和所述参考坐标系之间的变换矩阵与所述IMU和所述主标定板之间的变换矩阵的乘积,所述IMU和所述主标定板之间的变换矩阵为所述IMU和所述主相机之间的变换矩阵与所述主相机和所述主标定板之间的变换矩阵的乘积。将所述IMU和所述参考坐标系之间的变换矩阵拟合成IMU样条曲线的方法为常规方法。
在一些实施例中,在所述IMU样条曲线的时间范围内,初始化IMU加速度和陀螺仪样条曲线,并将所述IMU加速度和陀螺仪实际输出与样条曲线微分计算出的计算值之差作为误差项。
在一些实施例中,根据所述图像数据获取所述主标定板到所述主相机的误差项,获取方法为根据标定板与相机的空间位置利用相机的投影模型计算出标定板在相机成像平面成像,将计算出的成像与相机实际输出的成像做差即计算出标定板在相机成像平面的重投影误差,将所述重投影误差作为误差项。
在一些实施例中,根据所述激光雷达和所述IMU之间的变换矩阵将激光雷达点云转换到IMU坐标系;根据所述IMU和所述参考坐标系之间的变换矩阵将激光雷达点云从IMU坐标系转换到参考坐标系;根据所述主标定板和所述参考坐标系之间的变换矩阵将激光雷达点云从参考坐标系转换到主标定板坐标系;将所述激光雷达与所述主标定板之间的位移投影到主标定板平面垂直方向,提取所述激光雷达与所述主标定板平面距离,所述激光雷达和所述主标定板的变换矩阵由参考坐标系和主标定板之间的变换矩阵与激光雷达和参考坐标系之间的变换矩阵的乘积求得,其中,激光雷达和参考坐标系之间的变换矩阵由IMU和参考坐标系之间的变换矩阵与激光雷达和IMU之间的变换矩阵乘积求得;基于主标定板上激光雷达点云的坐标,获取由激光雷达到主标定板上点云方向的单位向量,将该单位向量转换到标定板坐标系后,投影到标定板平面垂直方向上,提取所述激光雷达与所述主标定板单位距离;使用所述激光雷达与所述主标定板平面距离除以所述激光雷达与所述主标定板单位距离,得到激光雷达到主标定板距离的预测值,所述预测值与激光雷达的实测值之差作为误差项。
在一些实施例中,还设置若干副标定板,以所有标定板之间的变换矩阵的观测值和测量值作为误差项,其中,所述观测值基于所有标定板与主相机之间的测量值求得,所述预测值为参考坐标系与所有标定板之间的初始变换矩阵,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
在一些实施例中,还设置若干副相机,以所述主相机和每个副相机之间的变换矩阵的预测值和观测值为误差项,其中,所述预测值为各个相机之间的初始变换矩阵,所述初始变换矩阵中的旋转矩阵设置为单位矩阵,所述初始变换矩阵中的位姿向量取零向量,所述测量值为参考坐标系和每个相机之间的变换矩阵与主相机和参考坐标系之间的变换矩阵的乘积,所述参考坐标系和每个相机之间的变换矩阵中,旋转矩阵设置为单位矩阵,位姿向量取零向量。
上述若干实施例中列举了一些用于输入图优化模型中进行优化的误差项,以及这些误差项的获取方法。在本申请的实施例中,上述若干误差项可以部分或全部输入图优化模型中。进行优化的误差项越多,优化时间越长,但优化得到的结果越精确。
本申请所述的一种激光雷达与IMU标定方法,利用图优化模型对相机、激光雷达、IMU三种传感器之间的位姿关系进行优化,可以实现相机、激光雷达、IMU三种传感器外参的同时高精度标定,并且,相机、激光雷达的数目均可以多于一个,不但统一了标定场景,且有助于节省标定成本,有利于工程化落地。
图2为本申请实施例所述的激光雷达、IMU和相机标定系统的结构示意图。
本申请的实施例还提供一种激光雷达、IMU和相机标定系统100,参考图2所示,所述标定系统100包括:激光雷达110、IMU120、主相机130和主标定板(图中未示出),其中,所述主标定板位于所述激光雷达110和主相机130的采集范围内;数据获取模块140,被配置为获取所述激光雷达110、IMU120、主相机130和主标定板之间的初始变换矩阵以及获取所述激光雷达110采集到的所述主标定板的点云数据、所述主相机130采集到的所述主标定板的图像数据和所述IMU120采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度;图优化模型150,被配置为获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,所述图优化模型150工作时:以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
下面结合附图2对本申请实施例所述的激光雷达、IMU和相机标定系统进行详细说明。
参考图2所示,其中,所述标定系统100包括第一部分101和第二部分102,所述第一部分101指的是硬件部分,也就是用于进行测试的模拟系统部分,包括激光雷达110、IMU120、主相机130和主标定板(图中未示出)。所述第二部分102指的是软件部分,也就是用于进行数据处理的部分,包括数据获取模块140和图优化模型150。
参考图2所示,所述标定系统100包括:激光雷达110、IMU120、主相机130和主标定板(图中未示出),其中,所述主标定板位于所述激光雷达110和主相机130的采集范围内。
在对激光雷达、IMU、主相机之间的位姿关系进行标定的方法中,需要构建一个模拟系统来获取当激光雷达、IMU、主相机处于移动状态时的相互位姿关系,然后利用移动状态时的相互位姿关系和所述模拟系统的初始相互位姿关系进行对比,标定出最佳的激光雷达、IMU、主相机的相互位姿关系。
因此,在本申请的技术方案中,首先构建一个用于测试模拟的模拟系统(也就是所述第一部分101)。所述模拟系统包括需要标定的激光雷达、IMU、主相机。为了提高模拟真实性,在一些实施例中,所述激光雷达、IMU、主相机设置于汽车上,然后在所述汽车的周围设置主标定板,在模拟过程中,控制所述汽车万向移动,并通过激光雷达、IMU、主相机采集相应的数据。在另一些实施例中,为了简便,不使用真实汽车,而是将所述激光雷达、IMU、主相机设置于支架上,模拟所述激光雷达、IMU、主相机在汽车上的真实安装情况,然后在支架周围设置主标定板,在模拟过程中,控制所述支架万向移动,并通过激光雷达、IMU、主相机采集相应的数据。
所述主标定板上设置有网格,用于模拟激光雷达、IMU、主相机的周围环境,辅助测量激光雷达、IMU、主相机的相互位姿关系。因此,所述主标定板必须位于激光雷达、IMU、主相机的测量范围内。
在本申请的另一些实施例中,所述激光雷达110的数量可以是一个或多个。相机的数量也可以是一个或多个,当相机的数量为一个时,所述一个相机为主相机,当相机的数量为多个时,所述多个相机包括一个主相机和至少一个副相机。标定板的数量也可以是一个或多个,当标定板的数量为一个时,所述一个标定板为主标定板,当标定板的数量为多个时,所述多个标定板包括一个主标定板和至少一个副标定板。当有多个激光雷达、相机和标定板时,要记录所有激光雷达、所有相机和所有标定板的初始变换矩阵。
在本申请的一些实施例中,所述激光雷达、IMU、相机和标定板的内参数据保存于数据获取模块140的存储器中,以供后续步骤使用。
具体地,相机的内参数据包括:相机类型、内参矩阵、畸变模型、行径向畸变、切向畸变、图像分辨率等参数。IMU的内参数据包括:加速度计和陀螺仪的噪声强度值与随机游走值,采样频率等。激光雷达的内参数据包括:降采样后每帧激光雷达需要保留的点云数等。标定板的内参数据包括:标定板数目与类型,标定板棋盘格参数等。
在本申请的一些实施例中,标定板采用Aprilgrid标定板,其中,在主标定板边缘,沿着x,y两个轴贴有高反条,以方便通过反射的激光雷达点云提取该标定板位姿矩阵,且标定板数目不小于3个,以确保每个传感器(激光雷达、IMU、相机)视场范围内都能有效探测到标定板。
继续参考图2所示,所述标定系统100还包括:数据获取模块140,被配置为获取所述激光雷达110、IMU120、主相机130和主标定板之间的初始变换矩阵以及获取所述激光雷达110采集到的所述主标定板的点云数据、所述主相机130采集到的所述主标定板的图像数据和所述IMU120采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度。
其中,所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵是预先设计好的,预先存储于所述数据获取模块140的存储器中以供后续步骤使用。
在控制所述汽车或者支架进行万向运动的过程中,通过设置于所述汽车或者支架上的传感器(激光雷达、IMU、相机)获取相应的数据并将这些数据传送到所述数据获取模块140中存储以供后续步骤使用。
激光雷达是一种综合的光探测与测量系统,通过发射接受激光束,分析激光遇到目标对象后的折返时间,计算出目标对象与车辆的相对距离。目前常见的有16线、32线、64线激光雷达。激光雷达线束越多,测量精度越高,安全性越高。激光雷达测量的数据为点云数据。激光雷达通过发射激光和接收反射激光来获取周围环境的特征,激光雷达的点云数据通常会包括(x,y,z)和一些其他的信息,x,y,z是环境中的某一点在激光雷达坐标系中的位置。
惯性测量单元是一种测量物体三轴姿态角和加速度的装置,可以直接测量出车辆运行时的加速度以及转弯时的角速度(在本申请的实施例中,所述惯性测量单元测量的数据被标记为第一角速度和第一加速度),从而推算出车辆在一段时间的运动姿态,可以应用在车辆定位中。其中,惯性测量数据包括但不限于加速度值和角速度值。
相机用于拍摄周围环境的图像,并根据图像数据获取图像中的物体与相机的距离等位姿关系。
继续参考图2所示,所述标定系统100还包括:图优化模型150,被配置为获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,所述图优化模型150工作时:以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
图优化是一种数学建模的工具,图优化模型是数学建模的结果。传感器外参标定问题本质上是传感器之间位置关系的最优估计问题,图优化建模就是把外参标定这个实际工程问题转化为数学上的最优化问题。通过求解这个数学上的最优化问题就可以得到传感器之间最可信的相对位置关系。图优化模型的构建过程就是将实际工程问题首先构建为数学上的图模型,然后由图模型转化出最小二乘优化问题。构建出的图模型中节点表示系统中传感器及标定工具在不同的采样时间的物理空间位置,图模型中连接节点的边表示传感器的实际测量值。当系统中所有涉及的对象的空间位置已知时,我们可以通过传感器的功能模型计算出传感器的计算测量值,计算测量值与实际测量值之差就是最优化问题中的误差项,优化问题的求解过程就是不断调节所有节点的空间位置使所有误差项的平方和不断减小。所有误差项的平方和达到最小时的空间位置是系统最优空间位置,传感器之间的最优空间位置之差就是传感器之间的最优外参估计。
构建好图优化模型后,向所述图优化模型中输入各种误差项,利用这些误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,也就是最终的标定数据。具体地,所述误差项的种类以及如何获取误差项如下所述。
首先,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵(在后续计算中使用);根据所述图像数据获取所述主标定板和所述主相机之间的变换矩阵(在后续计算中使用)。其中,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵的方法包括:在所述主标定板上沿边缘设置相互垂直的两条高反射率条;提取位于所述高反射率条上的所有激光雷达点云,并拟合出两条直线;基于所述两条直线确定所述主标定板的平面位置,通过阈值聚类方法提取位于所述主标定板平面上的点云;基于所述主标定板平面上的点云,获取所述主标定板和所述激光雷达之间的变换矩阵。
在一些实施例中,设置参考坐标系,以所述主标定板与所述参考坐标系之间的初始变换矩阵作为误差项,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。所述参考坐标系用于作为不同传感器的共同参考关联不同传感器的坐标系和标定板的坐标系。
在一些实施例中,获取所述主相机和所述参考坐标系之间的变换矩阵的样条曲线;在所述样条曲线时间范围内,将所述IMU的角速度测量值和通过对样条曲线微分计算出的观测值作为误差项,其中,所述测量值由IMU采集得到,所述观测值为所述主相机和所述IMU之间的初始变换矩阵与所述样条曲线中的角速度的乘积。将所述主相机和所述参考坐标系之间的变换矩阵拟合成样条曲线的方法为常规方法。
在一些实施例中,以所述主相机和所述激光雷达之间的变换矩阵的预测值(初始设定值)与观测值(通过共视反推)作为误差项,其中,所述预测值中的旋转矩阵设置为单位矩阵,所述预测值中的位姿向量取零向量,所述观测值为所述参考坐标系和所述激光雷达之间的变换矩阵与所述主相机和所述参考坐标系之间的变换矩阵的乘积。
在一些实施例中,基于所述IMU和所述参考坐标系之间的变换矩阵构建IMU样条曲线,其中,所述IMU和所述参考坐标系之间的变换矩阵为所述主标定板和所述参考坐标系之间的变换矩阵与所述IMU和所述主标定板之间的变换矩阵的乘积,所述IMU和所述主标定板之间的变换矩阵为所述IMU和所述主相机之间的变换矩阵与所述主相机和所述主标定板之间的变换矩阵的乘积。将所述IMU和所述参考坐标系之间的变换矩阵拟合成IMU样条曲线的方法为常规方法。
在一些实施例中,在所述IMU样条曲线的时间范围内,初始化IMU加速度和陀螺仪样条曲线,并将所述IMU加速度和陀螺仪实际输出与样条曲线微分计算出的计算值之差作为误差项。
在一些实施例中,根据所述图像数据获取所述主标定板到所述主相机的误差项,获取方法为根据标定板与相机的空间位置利用相机的投影模型计算出标定板在相机成像平面成像,将计算出的成像与相机实际输出的成像做差即计算出标定板在相机成像平面的重投影误差,将所述重投影误差作为误差项。
在一些实施例中,根据所述激光雷达和所述IMU之间的变换矩阵将激光雷达点云转换到IMU坐标系;根据所述IMU和所述参考坐标系之间的变换矩阵将激光雷达点云从IMU坐标系转换到参考坐标系;根据所述主标定板和所述参考坐标系之间的变换矩阵将激光雷达点云从参考坐标系转换到主标定板坐标系;将所述激光雷达与所述主标定板之间的位移投影到主标定板平面垂直方向,提取所述激光雷达与所述主标定板平面距离,所述激光雷达和所述主标定板的变换矩阵由参考坐标系和主标定板之间的变换矩阵与激光雷达和参考坐标系之间的变换矩阵的乘积求得,其中,激光雷达和参考坐标系之间的变换矩阵由IMU和参考坐标系之间的变换矩阵与激光雷达和IMU之间的变换矩阵乘积求得;基于主标定板上激光雷达点云的坐标,获取由激光雷达到主标定板上点云方向的单位向量,将该单位向量转换到标定板坐标系后,投影到标定板平面垂直方向上,提取所述激光雷达与所述主标定板单位距离;使用所述激光雷达与所述主标定板平面距离除以所述激光雷达与所述主标定板单位距离,得到激光雷达到主标定板距离的预测值,所述预测值与激光雷达的实测值之差作为误差项。
在一些实施例中,还设置有若干副标定板,以所有标定板之间的变换矩阵的观测值和测量值作为误差项,其中,所述观测值基于所有标定板与主相机之间的测量值求得,所述预测值为参考坐标系与所有标定板之间的初始变换矩阵,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
在一些实施例中,还设置有若干副相机,以所述主相机和每个副相机之间的变换矩阵的预测值和观测值为误差项,其中,所述预测值为各个相机之间的初始变换矩阵,所述初始变换矩阵中的旋转矩阵设置为单位矩阵,所述初始变换矩阵中的位姿向量取零向量,所述测量值为参考坐标系和每个相机之间的变换矩阵与主相机和参考坐标系之间的变换矩阵的乘积,所述参考坐标系和每个相机之间的变换矩阵中,旋转矩阵设置为单位矩阵,位姿向量取零向量。
上述若干实施例中列举了一些用于输入图优化模型中进行优化的误差项,以及这些误差项的获取方法。在本申请的实施例中,上述若干误差项可以部分或全部输入图优化模型中。进行优化的误差项越多,优化时间越长,但优化得到的结果越精确。
本申请提供一种激光雷达与IMU标定方法及系统,可以实现相机、激光雷达、IMU三种传感器外参的同时高精度标定,并且,相机、激光雷达的数目均可以多于一个,不但统一了标定场景,且有助于节省标定成本,有利于工程化落地。
综上所述,在阅读本申请内容之后,本领域技术人员可以明白,前述申请内容可以仅以示例的方式呈现,并且可以不是限制性的。尽管这里没有明确说明,本领域技术人员可以理解本申请意图囊括对实施例的各种合理改变,改进和修改。这些改变,改进和修改都在本申请的示例性实施例的精神和范围内。
应当理解,本实施例使用的术语″和/或″包括相关联的列出项目中的一个或多个的任意或全部组合。应当理解,当一个元件被称作″连接″或″耦接″至另一个元件时,其可以直接地连接或耦接至另一个元件,或者也可以存在中间元件。
还应当理解,术语″包含″、″包含着″、″包括″或者″包括着″,在本申请文件中使用时,指明存在所记载的特征、整体、步骤、操作、元件和/或组件,但并不排除存在或附加一个或多个其他特征、整体、步骤、操作、元件、组件和/或它们的组。
还应当理解,尽管术语第一、第二、第三等可以在此用于描述各种元件,但是这些元件不应当被这些术语所限制。这些术语仅用于将一个元件与另一个元件区分开。因此,在没有脱离本申请的教导的情况下,在一些实施例中的第一元件在其他实施例中可以被称为第二元件。相同的参考标号或相同的参考标记符在整个说明书中表示相同的元件。
Claims (24)
1.一种激光雷达、IMU和相机标定方法,其特征在于,包括:
设置激光雷达、IMU、主相机和主标定板,并使所述主标定板位于所述激光雷达和主相机的采集范围内,获取所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵;
获取所述激光雷达采集到的所述主标定板的点云数据、所述主相机采集到的所述主标定板的图像数据和所述IMU采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度;
构建图优化模型,使用所述图优化模型获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,包括:
以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;
根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项;
通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
2.如权利要求1所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵;
根据所述图像数据获取所述主标定板和所述主相机之间的变换矩阵。
3.如权利要求2所述的标定方法,其特征在于,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵的方法包括:
在所述主标定板上沿边缘设置相互垂直的两条高反射率条;
提取位于所述高反射率条上的所有激光雷达点云,并拟合出两条直线;
基于所述两条直线确定所述主标定板的平面位置,通过阈值聚类方法提取位于所述主标定板平面上的点云;
基于所述主标定板平面上的点云,获取所述主标定板和所述激光雷达之间的变换矩阵。
4.如权利要求2所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:设置参考坐标系,以所述主标定板与所述参考坐标系之间的初始变换矩阵作为误差项,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
5.如权利要求4所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
获取所述主相机和所述参考坐标系之间的变换矩阵的样条曲线;
在所述样条曲线时间范围内,将所述IMU的角速度测量值和观测值作为误差项,其中,所述测量值由IMU采集得到,所述观测值为所述主相机和所述IMU之间的初始变换矩阵与所述样条曲线中的角速度的乘积。
6.如权利要求5所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
以所述主相机和所述激光雷达之间的变换矩阵的预测值与观测值作为误差项,其中,所述预测值中的旋转矩阵设置为单位矩阵,所述预测值中的位姿向量取零向量,所述观测值为所述参考坐标系和所述激光雷达之间的变换矩阵与所述主相机和所述参考坐标系之间的变换矩阵的乘积。
7.如权利要求6所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
基于所述IMU和所述参考坐标系之间的变换矩阵构建IMU样条曲线,所述IMU样条曲线作为误差项,其中,所述IMU和所述参考坐标系之间的变换矩阵为所述主标定板和所述参考坐标系之间的变换矩阵与所述IMU和所述主标定板之间的变换矩阵的乘积,所述IMU和所述主标定板之间的变换矩阵为所述IMU和所述主相机之间的变换矩阵与所述主相机和所述主标定板之间的变换矩阵的乘积。
8.如权利要求7所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
在所述IMU样条曲线的时间范围内,初始化IMU加速度和陀螺仪样条曲线,并将所述IMU加速度和陀螺仪样条曲线作为误差项。
9.如权利要求8所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
根据所述图像数据获取所述主标定板到所述主相机的重投影误差,将所述重投影误差作为误差项。
10.如权利要求9所述的标定方法,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
根据所述激光雷达和所述IMU之间的变换矩阵将激光雷达点云转换到IMU坐标系;
根据所述IMU和所述参考坐标系之间的变换矩阵将激光雷达点云从IMU坐标系转换到参考坐标系;
根据所述主标定板和所述参考坐标系之间的变换矩阵将激光雷达点云从参考坐标系转换到主标定板坐标系;
将所述激光雷达与所述主标定板之间的位移投影到主标定板平面垂直方向,提取所述激光雷达与所述主标定板平面距离,所述激光雷达和所述主标定板的变换矩阵由参考坐标系和主标定板之间的变换矩阵与激光雷达和参考坐标系之间的变换矩阵的乘积求得,其中,激光雷达和参考坐标系之间的变换矩阵由IMU和参考坐标系之间的变换矩阵与激光雷达和IMU之间的变换矩阵乘积求得;
基于主标定板上激光雷达点云的坐标,获取由激光雷达到主标定板上点云方向的单位向量,将该单位向量转换到标定板坐标系后,投影到标定板平面垂直方向上,提取所述激光雷达与所述主标定板单位距离;
使用所述激光雷达与所述主标定板平面距离除以所述激光雷达与所述主标定板单位距离,得到激光雷达到主标定板距离的预测值,所述预测值作为误差项;
将主标定板上激光雷达点云三维坐标所确定的与激光雷达的距离作为观测值,所述观测值作为误差项。
11.如权利要求10所述的标定方法,其特征在于,还包括:设置若干副标定板,以所有标定板之间的变换矩阵的观测值和测量值作为误差项,其中,所述观测值基于所有标定板与主相机之间的测量值求得,所述预测值为参考坐标系与所有标定板之间的初始变换矩阵,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
12.如权利要求10所述的标定方法,其特征在于,还包括:设置若干副相机,以所述主相机和每个副相机之间的变换矩阵的预测值和观测值为误差项,其中,所述预测值为各个相机之间的初始变换矩阵,所述初始变换矩阵中的旋转矩阵设置为单位矩阵,所述初始变换矩阵中的位姿向量取零向量,所述测量值为参考坐标系和每个相机之间的变换矩阵与主相机和参考坐标系之间的变换矩阵的乘积,所述参考坐标系和每个相机之间的变换矩阵中,旋转矩阵设置为单位矩阵,位姿向量取零向量。
13.一种激光雷达、IMU和相机标定系统,其特征在于,包括:
激光雷达、IMU、主相机和主标定板,其中,所述主标定板位于所述激光雷达和主相机的采集范围内;
数据获取模块,被配置为获取所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵以及获取所述激光雷达采集到的所述主标定板的点云数据、所述主相机采集到的所述主标定板的图像数据和所述IMU采集到的惯性测量数据,所述惯性测量数据包括角速度和加速度;
图优化模型,被配置为获取所述激光雷达、IMU、主相机和主标定板之间的变换矩阵,所述图优化模型工作时:
以所述激光雷达、IMU、主相机和主标定板之间的变换矩阵作为待优化值;
根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项
通过所述误差项优化所述待优化值,得到优化后的所述激光雷达、IMU、主相机和主标定板之间的变换矩阵。
14.如权利要求13所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵;
根据所述图像数据获取所述主标定板和所述主相机之间的变换矩阵。
15.如权利要求14所述的标定系统,其特征在于,根据所述点云数据获取所述主标定板和所述激光雷达之间的变换矩阵的方法包括:
在所述主标定板上沿边缘设置相互垂直的两条高反射率条;
提取位于所述高反射率条上的所有激光雷达点云,并拟合出两条直线;
基于所述两条直线确定所述主标定板的平面位置,通过阈值聚类方法提取位于所述主标定板平面上的点云;
基于所述主标定板平面上的点云,获取所述主标定板和所述激光雷达之间的变换矩阵。
16.如权利要求14所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:设置参考坐标系,以所述主标定板与所述参考坐标系之间的初始变换矩阵作为误差项,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
17.如权利要求16所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
获取所述主相机和所述参考坐标系之间的变换矩阵的样条曲线;
在所述样条曲线时间范围内,将所述IMU的角速度测量值和观测值作为误差项,其中,所述测量值由IMU采集得到,所述观测值为所述主相机和所述IMU之间的初始变换矩阵与所述样条曲线中的角速度的乘积。
18.如权利要求17所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
以所述主相机和所述激光雷达之间的变换矩阵的预测值与观测值作为误差项,其中,所述预测值中的旋转矩阵设置为单位矩阵,所述预测值中的位姿向量取零向量,所述观测值为所述参考坐标系和所述激光雷达之间的变换矩阵与所述主相机和所述参考坐标系之间的变换矩阵的乘积。
19.如权利要求18所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
基于所述IMU和所述参考坐标系之间的变换矩阵构建IMU样条曲线,所述IMU样条曲线作为误差项,其中,所述IMU和所述参考坐标系之间的变换矩阵为所述主标定板和所述参考坐标系之间的变换矩阵与所述IMU和所述主标定板之间的变换矩阵的乘积,所述IMU和所述主标定板之间的变换矩阵为所述IMU和所述主相机之间的变换矩阵与所述主相机和所述主标定板之间的变换矩阵的乘积。
20.如权利要求19所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
在所述IMU样条曲线的时间范围内,初始化IMU加速度和陀螺仪样条曲线,并将所述IMU加速度和陀螺仪样条曲线作为误差项。
21.如权利要求20所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
根据所述图像数据获取所述主标定板到所述主相机的重投影误差,将所述重投影误差作为误差项。
22.如权利要求21所述的标定系统,其特征在于,根据所述激光雷达、IMU、主相机和主标定板之间的初始变换矩阵、所述点云数据、所述图像数据和所述惯性测量数据获取误差项的方法包括:
根据所述激光雷达和所述IMU之间的变换矩阵将激光雷达点云转换到IMU坐标系;
根据所述IMU和所述参考坐标系之间的变换矩阵将激光雷达点云从IMU坐标系转换到参考坐标系;
根据所述主标定板和所述参考坐标系之间的变换矩阵将激光雷达点云从参考坐标系转换到主标定板坐标系;
将所述激光雷达与所述主标定板之间的位移投影到主标定板平面垂直方向,提取所述激光雷达与所述主标定板平面距离,所述激光雷达和所述主标定板的变换矩阵由参考坐标系和主标定板之间的变换矩阵与激光雷达和参考坐标系之间的变换矩阵的乘积求得,其中,激光雷达和参考坐标系之间的变换矩阵由IMU和参考坐标系之间的变换矩阵与激光雷达和IMU之间的变换矩阵乘积求得;
基于主标定板上激光雷达点云的坐标,获取由激光雷达到主标定板上点云方向的单位向量,将该单位向量转换到标定板坐标系后,投影到标定板平面垂直方向上,提取所述激光雷达与所述主标定板单位距离;
使用所述激光雷达与所述主标定板平面距离除以所述激光雷达与所述主标定板单位距离,得到激光雷达到主标定板距离的预测值,所述预测值作为误差项;
将主标定板上激光雷达点云三维坐标所确定的与激光雷达的距离作为观测值,所述观测值作为误差项。
23.如权利要求22所述的标定系统,其特征在于,还包括:若干副标定板,以所有标定板之间的变换矩阵的观测值和测量值作为误差项,其中,所述观测值基于所有标定板与主相机之间的测量值求得,所述预测值为参考坐标系与所有标定板之间的初始变换矩阵,所述初始变换矩阵中的初始位移设置为零,所述初始变换矩阵中的初始旋转设置为单位矩阵。
24.如权利要求22所述的标定系统,其特征在于,还包括:若干副相机,以所述主相机和每个副相机之间的变换矩阵的预测值和观测值为误差项,其中,所述预测值为各个相机之间的初始变换矩阵,所述初始变换矩阵中的旋转矩阵设置为单位矩阵,所述初始变换矩阵中的位姿向量取零向量,所述测量值为参考坐标系和每个相机之间的变换矩阵与主相机和参考坐标系之间的变换矩阵的乘积,所述参考坐标系和每个相机之间的变换矩阵中,旋转矩阵设置为单位矩阵,位姿向量取零向量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210433144.0A CN114879168A (zh) | 2022-04-24 | 2022-04-24 | 一种激光雷达与imu标定方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210433144.0A CN114879168A (zh) | 2022-04-24 | 2022-04-24 | 一种激光雷达与imu标定方法及系统 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114879168A true CN114879168A (zh) | 2022-08-09 |
Family
ID=82672326
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210433144.0A Pending CN114879168A (zh) | 2022-04-24 | 2022-04-24 | 一种激光雷达与imu标定方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114879168A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117554937A (zh) * | 2024-01-08 | 2024-02-13 | 安徽中科星驰自动驾驶技术有限公司 | 一种误差可控的激光雷达和组合惯导外参标定方法和系统 |
-
2022
- 2022-04-24 CN CN202210433144.0A patent/CN114879168A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117554937A (zh) * | 2024-01-08 | 2024-02-13 | 安徽中科星驰自动驾驶技术有限公司 | 一种误差可控的激光雷达和组合惯导外参标定方法和系统 |
CN117554937B (zh) * | 2024-01-08 | 2024-04-26 | 安徽中科星驰自动驾驶技术有限公司 | 一种误差可控的激光雷达和组合惯导外参标定方法和系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
Li et al. | NRLI-UAV: Non-rigid registration of sequential raw laser scans and images for low-cost UAV LiDAR point cloud quality improvement | |
Pandey et al. | Extrinsic calibration of a 3d laser scanner and an omnidirectional camera | |
JP5992184B2 (ja) | 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム | |
US8295547B1 (en) | Model-based feature tracking in 3-D and 2-D imagery | |
WO2018142900A1 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
CN108734742B (zh) | 摄像头参数组算出方法、程序及装置 | |
US10645365B2 (en) | Camera parameter set calculation apparatus, camera parameter set calculation method, and recording medium | |
KR102627453B1 (ko) | 위치 추정 장치 및 방법 | |
CN110470333B (zh) | 传感器参数的标定方法及装置、存储介质和电子装置 | |
JP6321202B2 (ja) | モバイルプラットフォームの運動を決定する方法、装置、及びシステム | |
CN102788572B (zh) | 一种工程机械吊钩姿态的测量方法、装置及系统 | |
CN113269840A (zh) | 一种用于相机和多激光雷达的联合标定方法及电子设备 | |
EP3155369B1 (en) | System and method for measuring a displacement of a mobile platform | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN114608554B (zh) | 一种手持slam设备以及机器人即时定位与建图方法 | |
CN114413958A (zh) | 无人物流车的单目视觉测距测速方法 | |
KR102050995B1 (ko) | 공간좌표의 신뢰성 평가 장치 및 방법 | |
CN111308415B (zh) | 一种基于时间延迟的在线估计位姿的方法和设备 | |
CN106405581A (zh) | 卫星结构变形引起的多类型载荷间耦合指向精度评估方法 | |
Yu et al. | Displacement measurement of large structures using nonoverlapping field of view multi‐camera systems under six degrees of freedom ego‐motion | |
CN114879168A (zh) | 一种激光雷达与imu标定方法及系统 | |
CN114777768A (zh) | 一种卫星拒止环境高精度定位方法、系统及电子设备 | |
KR102490521B1 (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 |
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 |