CN116105772A - 一种激光雷达与imu的标定方法、装置及存储介质 - Google Patents
一种激光雷达与imu的标定方法、装置及存储介质 Download PDFInfo
- Publication number
- CN116105772A CN116105772A CN202310159585.0A CN202310159585A CN116105772A CN 116105772 A CN116105772 A CN 116105772A CN 202310159585 A CN202310159585 A CN 202310159585A CN 116105772 A CN116105772 A CN 116105772A
- Authority
- CN
- China
- Prior art keywords
- imu
- laser radar
- calculating
- coordinate system
- plane
- 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
- 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
-
- 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)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manufacturing & Machinery (AREA)
- Computer Networks & Wireless Communication (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明公开了一种激光雷达与IMU的标定方法、装置及存储介质,本发明通过激光雷达获取特征物的第一点云数据并通过IMU获取第一IMU数据,变换预设次数载体相对于特征物的姿态,每一次变换后均通过激光雷达获取特征物的第二点云数据并通过IMU获取第二IMU数据,从而计算得到IMU坐标系中重力矢量的第一原始观测值以及第一变换观测值,激光雷达坐标系中重力矢量的第二原始观测值以及第二变换观测值,用于计算激光雷达与IMU之间的目标转换矩阵,以特征物作为特征获取点云数据并与IMU数据构建约束,不需要依赖额外的人工标靶和传感器,降低标定的条件要求;采用静态扫描的方式,不受激光雷达与IMU时间同步的影响、无需校准点云的运动畸变以及估计运动轨迹。
Description
技术领域
本发明涉及标定领域,尤其是一种激光雷达与IMU的标定方法、装置及存储介质。
背景技术
激光雷达(Light Detection and Ranging,LiDAR)与惯性测量单元(InertialMeasurement Unit,IMU)组合是一种常见的多源传感器融合方式,惯导的高频角速度、加速度信息可以为激光点云配准提供更好的先验值,惯性里程计与激光里程计紧耦合提升位姿估计的精度和鲁棒性。每个传感器都有各自的空间基准和时间基准,在融合两种传感器的数据进行定位、建图、感知之前,需先将不同传感器的测量值转换到同一坐标系下,即获取各传感器坐标系之间的旋转和平移关系实现标定。
现今,激光雷达与惯性测量单元的标定通常通过手眼标定法、在线估计法以及基于特征的方法等方法,手眼标定法依赖于IMU独立地估计轨迹,精度低;在线估计法需要有充足的线加速度和角速度激励,来使滤波收敛,若初始值的误差较大,可能致使滤波收敛缓慢或失败;基于特征的方法要求LiDAR和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和LiDAR的运动轨迹,简化了标定过程。
为了更好地理解和实施,下面结合附图详细说明本发明。
附图说明
图1为本发明激光雷达与IMU的标定方法的步骤流程示意图;
图2为本发明具体实施例变换载体相对于特征物的姿态的示意图;
图3为本发明具体实施例不同类型的特征物的示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分的实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本申请保护的范围。
本申请的说明书和权利要求书及所述附图中的术语“第一”、“第二”、“第三”和“第四”等是用于区别不同对象,而不是用于描述特定顺序。此外,术语“包括”和“具有”以及它们任何变形,意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、***、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单元,或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
在本文中提及“实施例”意味着,结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例,也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是,本文所描述的实施例可以与其它实施例相结合。
如图1所示,本发明实施例提供一种激光雷达与IMU的标定方法,包括步骤S100-S400:
S100、通过激光雷达获取特征物的第一点云数据并通过IMU获取第一IMU数据。
本发明实施例中,激光雷达与IMU固定于载体,特征物与铅垂线平行或垂直。需要说明的是,在本发明实施例中,激光雷达与IMU安装于载体的不同的位置,二者刚性固连,IMU具有IMU坐标系(b系),IMU坐标系与载体坐标系重合,定义X轴(第一坐标轴)指向右方,Y轴(第二坐标轴)指向前方,Z轴(第三坐标轴)指向上方;激光雷达具有扫描坐标系(记为激光雷达坐标系,即l系)同样为右手坐标系,以“东北天”地理坐标系定义为导航坐标系(n系)。
具体地,1)首先选择标定场地:选择具有以下特征的人工场景作为标定场地:
a、结构化场景中的水平地面,地面平整光滑,通过电子水平仪测量平面的水平度,选取水平度小于0.1°、大小1m2左右的平面区域作为特征物。在实际生产中,可采取人工布置标定板的方式,将标定板水平固定放置于四角高度可调节的底座上,通过电子水平仪将标定板精确调平,以获得更高的标定精度。
b、结构化场景中的墙角,具有两面非平行的相邻竖直墙面,要求墙面应与铅垂线近似平行。
c、结构化场景中的竖直杆/柱状地物,要求杆/柱状地物与铅垂线近似平行。
2)采集标定数据,将搭载激光雷达和IMU的载体固定于某一位置,要求特征目标在激光雷达的视场范围内,且距离不超过15m,启动激光雷达与IMU,静态测量,并记录激光雷达所获取的特征物的第一点云数据,以及通过IMU获取第一IMU数据。
S200、变换预设次数载体相对于特征物的姿态,每一次变换后均通过激光雷达获取特征物的第二点云数据并通过IMU获取第二IMU数据。
需要说明的是,本发明实施例中的第一IMU数据以及第二IMU数据指的是关于重力加速度的IMU数据。其中,预设次数根据实际需求设定,例如可以根据需要求解的矩阵的自由度确定,假设最终需要计算的目标转换矩阵的自由度为3,则预设次数≥3。
本发明实施例中,每一次变换载体相对于特征物的姿态Pose后,均通过激光雷达获取特征物的第二点云数据并通过IMU获取第二IMU数据,如图2所示,Z代表铅垂线方向,TAG代表特征物。
S300、根据第一IMU数据计算IMU坐标系中重力矢量的第一原始观测值,根据第一点云数据计算激光雷达坐标系中重力矢量的第二原始观测值,根据第二IMU数据计算IMU坐标系中重力矢量的第一变换观测值,根据第二点云数据计算激光雷达坐标系中重力矢量的第二变换观测值。
本发明实施例中,第一原始观测值以及第一变换观测值构成第一集合,第二原始观测值以及第二变换观测值构成第二集合。
可选地,步骤S300中根据第一IMU数据计算IMU坐标系中重力矢量的第一原始观测值,包括步骤S301-S303:
S301、根据第一IMU数据确定加速度计的观测值。
本发明实施例中,第一IMU数据包括若干组加速度计的数值,具体地:根据若干组加速度计的数值,计算加速度计的数值在IMU坐标系的第一坐标轴的均值、第二坐标轴的均值以及第三坐标轴的均值;第一坐标轴的均值、第二坐标轴的均值以及第三坐标轴的均值构成加速度计的观测值。
需要说明的是,当载体静止放置时,IMU仅受重力的影响,理论上IMU的加速度计质量块敏感到的加速度与重力加速度大小相等,方向相反,加速度计的观测值可视为重力加速度在IMU的X、Y、Z三个轴上的投影。加速度计的观测方程为:
式中:为加速度计的观测值,由IMU坐标系的第一坐标轴的均值第二坐标轴的均值以及第三坐标轴的均值构成,表示IMU坐标系相对于导航坐标系的方向余弦矩阵,gn=[0 0 g]T,T为转置,上标n代表导航坐标系(n系),g为重力加速度,ba为加速度计的预设零偏,可通过预先标定获得。
S302、计算加速度计的观测值与加速度计的预设零偏的第一差距。
S303、根据第一差距与第一差距的模的比值,得到IMU坐标系中重力矢量的第一原始观测值。
具体地,顾及旋转前后矢量的模长不变性,将式(1)中的矢量化单位化:
令
需要说明的是,fb-ba为第一差距,||||代表取模,||fb-ba||为第一差距的模,m为IMU坐标系中重力矢量的第一原始观测值。
可选地,步骤S300中根据第一点云数据计算激光雷达坐标系中重力矢量的第二原始观测值,包括步骤S311-S312或者S321、S322,其中S311、S312、S321不限定执行顺序:
S311、当特征物为与铅垂线垂直的第一平面:从第一点云数据提取位于第一平面的第一点集;根据待优化的第一平面的法向量以及第一点集,构建第一点集中每一点至第一平面的第一距离方程;对第一距离方程进行拟合处理,计算使得第一距离方程的结果之和最小的第一平面的第一目标法向量作为激光雷达坐标系中重力矢量的第二原始观测值。
本发明实施例中,当特征物为与铅垂线垂直的第一平面,如图3所示的第一平面101,从第一点云数据提取位于第一平面的第一点集P={p1,p2,p3,…,pk},pk为第一点集中的第k点。具体地,第一平面对应的平面方程为:
ax+by+cz-d=0 (5)
其中,n=[a,b,c]T表示待优化的第一平面的法向量,法向量具体为单位法向量,a,b,c代表不同坐标轴的分量,d表示LiDAR坐标系激光雷达坐标系)的原点到第一平面的距离,[x,y,z]为第一平面上任意一点在LiDAR坐标系下的三维坐标,即第一点集中任意一点在LiDAR坐标系下的三维坐标。
因此,第一点集中任意一点pi到平面的距离di的第一距离方程为:
最优的平面参数应使得以下目标函数值最小:
本发明实施例中,由于采集的第一点云数据中不可避免地存在噪声,为消除异常值对平面参数精度的影响,使用RANSAC法拟合平面参数,即使用RANSAC法对第一距离方程进行拟合处理,拟合处理如公式(7)所示,计算使得第一距离方程的结果之和最小的第一平面的第一目标法向量作为激光雷达坐标系中重力矢量的第二原始观测值,即满足公式(7)拟合结果,此时的n即为第一平面的第一目标法向量,作为激光雷达坐标系中重力矢量的第二原始观测值,该第一目标法向量与重力矢量平行,为重力方向在LiDAR坐标系中的表示。
S312、当特征物为与铅垂线平行的第二平面和第三平面,第二平面和第三平面相交:分别从第一点云数据提取位于第二平面的第二点集以及位于第三平面的第三点集;根据待优化的第二平面的法向量以及第二点集,构建第二点集中每一点至第二平面的第二距离方程,根据待优化的第三平面的法向量以及第三点集,构建第三点集中每一点至第三平面的第三距离方程;对第二距离方程进行拟合处理,计算使得第二距离方程的结果之和最小的第二平面的第二目标法向量,对第三距离方程进行拟合处理,计算使得第三距离方程的结果之和最小的第三平面的第三目标法向量;将第二目标法向量以及第三目标法向量的叉积作为激光雷达坐标系中重力矢量的第二原始观测值。
本发明实施例中,当特征物为与铅垂线平行的第二平面和第三平面,第二平面和第三平面相交,如图3中的102,包括相交的第二平面以及第三平面,分别从第一点云数据提取位于第二平面的第二点集以及位于第三平面的第三点集。然后,根据待优化的第二平面的法向量以及第二点集,构建第二点集中每一点至第二平面的第二距离方程,根据待优化的第三平面的法向量以及第三点集,构建第三点集中每一点至第三平面的第三距离方程。需要说明的是,第二距离方程以及第三距离方程的形式与公式(6)类似,不再赘述。
具体地,对第二距离方程进行拟合处理,计算使得第二距离方程的结果之和最小的第二平面的第二目标法向量,对第三距离方程进行拟合处理,计算使得第三距离方程的结果之和最小的第三平面的第三目标法向量。需要说明的是,拟合处理的原理如公式(7)所示,不再赘述。此时,可以得到第二目标法向量n1以及第三目标法向量n2。
然后,计算第二目标法向量n1以及第三目标法向量n2的叉积:
n=n1×n2 (8)
其中,公式(8)中的叉积n即为激光雷达坐标系中重力矢量的第二原始观测值。
S321、当特征物为物体,物体的轴线与铅垂线平行,从第一点云数据提取位于物体的第四点集。
可选地,从第一点云数据提取位于物体的第四点集,当特征物为物体,例如如图3所示的103,物体可以为杆状物、柱状物等物体。
S322、根据待优化的轴线的单位向量以及所述第四点集,构建所述第四点集中每一点至所述特征物的表面的第四距离方程;
对所述第四距离方程进行拟合处理,计算使得所述第四距离方程的结果之和最小的轴线的目标单位向量作为激光雷达坐标系中重力矢量的第二原始观测值。
本发明实施例中,以物体为圆柱为例,圆柱的轴线即圆柱的中心轴,特征物的表面即为圆柱的表面,例如圆柱面。轴线的方程可以表示为:
其中:[x,y,z]为第四点集上任意一点在LiDAR坐标系下的三维坐标,[x0,y0,z0]为轴线起点的坐标,λ表示比例因子,n′=[nx,ny,nz]T为沿轴线方向的单位向量,nx,nv,nz代表不同坐标轴的分量。
其中,第四点集上任意一点到圆柱面的距离di的第四距离方程为:
di=|μ×n′|-r (10)
其中,μ=[x-x0,y-y0,z-z0],r为圆柱的半径。然后,对第四距离方程进行拟合处理,最优物体参数应使得以下目标函数值最小:
其中,k为第四点集的点的数量。
本发明实施例中,使用RANSAC法拟合物体参数,即使用RANSAC法对第四距离方程进行拟合处理,拟合处理如公式(11)所示,计算使得第四距离方程的结果之和最小的轴线的目标单位向量作为激光雷达坐标系中重力矢量的第二原始观测值。
由上述三种类型特征(图3中的101、102、103)拟合的重力方向在激光雷达坐标系中的观测值与实际重力矢量的关系为:
S400、根据第一集合以及第二集合计算激光雷达与IMU之间的目标转换矩阵。
本发明实施例中,构建几何约束以后续求解目标转换矩阵。具体地:
以重力矢量作为IMU和激光雷达的共同观测值,构建约束,将式(4)和式(12)联立:
消去gn可得:
式中:q=[q0 q1 q2 q3]为单位四元数。
可选地,步骤S400包括步骤S410-S450:
S410、计算第一集合的第一均值以及第二集合的第二均值。
需要说明的是,本发明实施例待求解的预设转换矩阵的自由度为3,因此至少需要3组线性无关的观测值才能求解,因此预设次数≥3),假设根据式(15)可以列出N个方程,第一原始观测值以及第一变换观测值构成第一集合{m1,m2,…,mN},第二原始观测值以及第二变换观测值构成第二集合{n1,n2,…,nN},然后按照点的坐标转换方式求解旋转参数。
S420、计算第一集合中每一观测值与第一均值的差值,得到第一坐标信息,计算第二集合中每一观测值与第二均值的差值,得到第二坐标信息。
需要说明的是,第一集合中每一观测值mN指的是第一原始观测值或者第一变换观测值,第二集合中每一观测值nN指的是第二原始观测值或者第二变换观测值。具体地:
法向量重心化后的坐标n′i(第二坐标信息)、m′i(第一坐标信息):
S430、根据第一坐标信息、第二坐标信息以及预设求矩阵迹函数,构建对称矩阵方程。
具体地,有定义矩阵:
构造对称矩阵K的对称矩阵方程:
式中:Z=[B23-B32 B31-B13 B12-B21],Bij表示矩阵B的第i行第j列的元素,I3×3表示大小为3的单位矩阵,T为转置,tr表示预设求矩阵迹函数。
S440、对对称矩阵方程进行计算处理,确定使得对称矩阵方程的结果为最大值对应的目标特征向量。
具体地,根据四元数原理,确定使得对称矩阵K的对称矩阵方程的结果为最大值(最大特征值)λmax,λmax对应的目标特征向量q=[q0 q1 q2 q3]即为所求单位四元数。
S450、将目标特征向量代入至激光雷达与IMU之间的待求解的预设转换矩阵,得到激光雷达与IMU之间的目标转换矩阵。
可选地,本发明实施例的激光雷达与IMU的标定方法在步骤S400之前还包括步骤S330,步骤S330包括步骤S3301-S3303,其中S3302、S3303不限定执行顺序:
S3301、根据预设次数、第二变换观测值以及预设求矩阵迹函数,计算可观性得分。
需要说明的是,本发明实施例中,待求解的预设转换矩阵的自由度为3,因此至少需要3组线性无关的观测值才能求解。实际中,当N个载体姿态(假设变换载体相对于特征物的姿态的预设次数为N’,一共有N=N’+1个姿态)之间的变换较小时,可导致某一个方向的旋转不可观。为保证标定参数的可观性,定义可观性得分S:
其中:tr表示预设求矩阵迹函数,I3×3表示大小为3的单位矩阵,T为转置,mi为第i个姿态对应的观测值。需要说明的是,可观性得分与外参(待求解的预设转换矩阵)估值的协方差正相关。通常,方程数量越多、观测值间的夹角越大、则可观性得分越小,标定精度和可靠性也越高。
S3302、当可观性得分小于得分阈值,返回变换载体相对于特征物的姿态的步骤,直至可观性得分大于或等于得分阈值。
可选地,得分阈值可以根据实际需要设定,本发明实施例中以得分阈值为1进行举例说明。具体地,当S<1,认为可观性较差,返回变换载体相对于特征物的姿态的步骤,即再次变换载体相对于特征物的姿态,然后获取新的第二点云数据以及新的第二IMU数据,直至可观性得分大于或等于1。
S3303、当可观性得分大于或等于得分阈值,根据第一集合以及第二集合计算激光雷达与IMU之间的目标转换矩阵。
具体地,当可观性得分大于或等于1,执行步骤S400,根据第一集合以及第二集合计算激光雷达与IMU之间的目标转换矩阵。
相对于现有技术,本发明实施例的激光雷达与IMU的标定方法:
1)、通过选取场景中与重力方向垂直或平行的特征物,如面状地物、杆/柱状地物作为观测对象获取点云,以及获取IMU数据,拟合计算重力方向在激光雷达坐标系中的表示以及在IMU坐标系中的表示,基于共同的重力方向构建LiDAR-IMU的几何约束,基于特征值分解法求解旋转外参,不依赖GNSS接收机、相机等传感器,易于工程实现,降低对标定的条件要求,并且与基于滤波和图优化的方法相比,算法原理简单,无需外部参数的初始值
2)、采集数据时载体固定静止放置,记录第一点云数据、第二点云数据、第一IMU数据以及第二IMU数据,为保证标定的可观性,变换载体的位姿,从多个不同的角度观测特征目标,采用的为静态扫描的方式,不受激光雷达与IMU时间同步的影响、无需对点云进行运动补偿,也无需估计IMU和LiDAR的运动轨迹,简化了标定流程。
3)、定义了由观测值向量集合计算标定的可观性得分,通过设定阈值控制标定结果的精度和可靠性。
本发明实施例还提供一种激光雷达与IMU的标定装置,包括:
第一获取模块,用于通过激光雷达获取特征物的第一点云数据并通过IMU获取第一IMU数据;特征物与铅垂线平行或垂直,激光雷达与IMU固定于载体;
第二获取模块,用于变换预设次数载体相对于特征物的姿态,每一次变换后均通过激光雷达获取特征物的第二点云数据并通过IMU获取第二IMU数据;
第一计算模块,用于根据第一IMU数据计算IMU坐标系中重力矢量的第一原始观测值,根据第一点云数据计算激光雷达坐标系中重力矢量的第二原始观测值,根据第二IMU数据计算IMU坐标系中重力矢量的第一变换观测值,根据第二点云数据计算激光雷达坐标系中重力矢量的第二变换观测值;第一原始观测值以及第一变换观测值构成第一集合,第二原始观测值以及第二变换观测值构成第二集合;
第二计算模块,用于根据第一集合以及第二集合计算激光雷达与IMU之间的目标转换矩阵。
上述方法实施例中的内容均适用于本装置实施例中,本装置实施例所具体实现的功能与上述方法实施例相同,并且达到的有益效果与上述方法实施例所达到的有益效果也相同,不再赘述。
本发明实施例还提供了另一种激光雷达与IMU的标定装置,该激光雷达与IMU的标定装置包括处理器和存储器,存储器中存储有至少一条指令、至少一段程序、代码集或指令集,至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行以实现前述实施例的激光雷达与IMU的标定方法。可选地,该激光雷达与IMU的标定装置包括但不限于手机、平板电脑、电脑及车载电脑等。
上述方法实施例中的内容均适用于本装置实施例中,本装置实施例所具体实现的功能与上述方法实施例相同,并且达到的有益效果与上述方法实施例所达到的有益效果也相同,不再赘述。
本发明实施例还提供一种计算机可读存储介质,存储介质中存储有至少一条指令、至少一段程序、代码集或指令集,至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行以实现前述实施例的激光雷达与IMU的标定方法。
本发明实施例还提供一种计算机程序产品或计算机程序,该计算机程序产品或计算机程序包括计算机指令,该计算机指令存储在计算机可读存储介质中。计算机设备的处理器从计算机可读存储介质读取该计算机指令,处理器执行该计算机指令,使得该计算机设备执行前述实施例的激光雷达与IMU的标定方法。
本申请的说明书及上述附图中术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
应当理解,在本申请中,“至少一个(项)”是指一个或者多个,“多个”是指两个或两个以上。“和/或”,用于描述关联对象的关联关系,表示可以存在三种关系,例如,“A和/或B”可以表示:只存在A,只存在B以及同时存在A和B三种情况,其中A,B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项(个)”或其类似表达,是指这些项中的任意组合,包括单项(个)或复数项(个)的任意组合。例如,a,b或c中的至少一项(个),可以表示:a,b,c,“a和b”,“a和c”,“b和c”,或“a和b和c”,其中a,b,c可以是单个,也可以是多个。
在本申请所提供的几个实施例中,应该理解到,所揭露装置和方法,可以通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的,例如,单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个***,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,装置或单元的间接耦合或通信连接,可以是电性,机械或其它的形式。作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。另外,在本申请各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括多指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本申请各个实施例方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(Read-Only Memory,简称ROM)、随机存取存储器(Random Access Memory,简称RAM)、磁碟或者光盘等各种可以存储程序的介质。
以上,以上实施例仅用以说明本申请的技术方案,而非对其限制;尽管参照前述实施例对本申请进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。
Claims (10)
1.一种激光雷达与IMU的标定方法,其特征在于,包括:
通过激光雷达获取特征物的第一点云数据并通过IMU获取第一IMU数据;所述特征物与铅垂线平行或垂直,所述激光雷达与所述IMU固定于载体;
变换预设次数所述载体相对于特征物的姿态,每一次变换后均通过所述激光雷达获取特征物的第二点云数据并通过所述IMU获取第二IMU数据;
根据所述第一IMU数据计算IMU坐标系中重力矢量的第一原始观测值,根据所述第一点云数据计算激光雷达坐标系中重力矢量的第二原始观测值,根据所述第二IMU数据计算IMU坐标系中重力矢量的第一变换观测值,根据所述第二点云数据计算激光雷达坐标系中重力矢量的第二变换观测值;所述第一原始观测值以及所述第一变换观测值构成第一集合,所述第二原始观测值以及所述第二变换观测值构成第二集合;
根据所述第一集合以及所述第二集合计算所述激光雷达与所述IMU之间的目标转换矩阵。
2.根据权利要求1所述激光雷达与IMU的标定方法,其特征在于:所述根据所述第一IMU数据计算IMU坐标系中重力矢量的第一原始观测值,包括:
根据所述第一IMU数据确定加速度计的观测值;
计算所述加速度计的观测值与加速度计的预设零偏的第一差距;
根据所述第一差距与所述第一差距的模的比值,得到IMU坐标系中重力矢量的第一原始观测值。
3.根据权利要求2所述激光雷达与IMU的标定方法,其特征在于:所述第一IMU数据包括若干组加速度计的数值,所述根据所述第一IMU数据确定加速度计的观测值,包括:
根据若干组所述加速度计的数值,计算IMU坐标系的第一坐标轴的均值、第二坐标轴的均值以及第三坐标轴的均值;所述第一坐标轴的均值、所述第二坐标轴的均值以及所述第三坐标轴的均值构成所述加速度计的观测值。
4.根据权利要求1所述激光雷达与IMU的标定方法,其特征在于:所述根据所述第一点云数据计算激光雷达坐标系中重力矢量的第二原始观测值,包括:
当所述特征物为与所述铅垂线垂直的第一平面:从所述第一点云数据提取位于所述第一平面的第一点集;根据待优化的第一平面的法向量以及所述第一点集,构建所述第一点集中每一点至所述第一平面的第一距离方程;对所述第一距离方程进行拟合处理,计算使得所述第一距离方程的结果之和最小的第一平面的第一目标法向量作为激光雷达坐标系中重力矢量的第二原始观测值;
当所述特征物为与所述铅垂线平行的第二平面和第三平面,所述第二平面和所述第三平面相交:分别从所述第一点云数据提取位于所述第二平面的第二点集以及位于所述第三平面的第三点集;根据待优化的第二平面的法向量以及所述第二点集,构建所述第二点集中每一点至所述第二平面的第二距离方程,根据待优化的第三平面的法向量以及所述第三点集,构建所述第三点集中每一点至所述第三平面的第三距离方程;对所述第二距离方程进行拟合处理,计算使得所述第二距离方程的结果之和最小的第二平面的第二目标法向量,对所述第三距离方程进行拟合处理,计算使得所述第三距离方程的结果之和最小的第三平面的第三目标法向量;将所述第二目标法向量以及所述第三目标法向量的叉积作为激光雷达坐标系中重力矢量的第二原始观测值。
5.根据权利要求1所述激光雷达与IMU的标定方法,其特征在于:所述根据所述第一点云数据计算激光雷达坐标系中重力矢量的第二原始观测值,包括:
当所述特征物为物体,物体的轴线与所述铅垂线平行,从所述第一点云数据提取位于所述物体的第四点集:
根据待优化的轴线的单位向量以及所述第四点集,构建所述第四点集中每一点至所述特征物的表面的第四距离方程;
对所述第四距离方程进行拟合处理,计算使得所述第四距离方程的结果之和最小的轴线的目标单位向量作为激光雷达坐标系中重力矢量的第二原始观测值。
6.根据权利要求1-5任一项所述激光雷达与IMU的标定方法,其特征在于:所述根据所述第一集合以及所述第二集合计算所述激光雷达与所述IMU之间的目标转换矩阵,包括:
计算所述第一集合的第一均值以及所述第二集合的第二均值;
计算所述第一集合中每一观测值与所述第一均值的差值,得到第一坐标信息,计算所述第二集合中每一观测值与所述第二均值的差值,得到第二坐标信息;
根据所述第一坐标信息、所述第二坐标信息以及预设求矩阵迹函数,构建对称矩阵方程;
对所述对称矩阵方程进行计算处理,确定使得所述对称矩阵方程的结果为最大值对应的目标特征向量;
将所述目标特征向量代入至所述激光雷达与所述IMU之间的待求解的预设转换矩阵,得到所述激光雷达与所述IMU之间的目标转换矩阵。
7.根据权利要求1-5任一项所述激光雷达与IMU的标定方法,其特征在于:所述根据所述第一集合以及所述第二集合计算所述激光雷达与所述IMU之间的目标转换矩阵的步骤之前,还包括:
根据所述预设次数、所述第二变换观测值以及预设求矩阵迹函数,计算可观性得分;
当所述可观性得分小于得分阈值,返回所述变换预设次数所述载体相对于特征物的姿态的步骤,直至所述可观性得分大于或等于得分阈值;
当所述可观性得分大于或等于得分阈值,根据所述第一集合以及所述第二集合计算所述激光雷达与所述IMU之间的目标转换矩阵。
8.一种激光雷达与IMU的标定装置,其特征在于,包括:
第一获取模块,用于通过激光雷达获取特征物的第一点云数据并通过IMU获取第一IMU数据;所述特征物与铅垂线平行或垂直,所述激光雷达与所述IMU固定于载体;
第二获取模块,用于变换预设次数所述载体相对于特征物的姿态,每一次变换后均通过所述激光雷达获取特征物的第二点云数据并通过所述IMU获取第二IMU数据;
第一计算模块,用于根据所述第一IMU数据计算IMU坐标系中重力矢量的第一原始观测值,根据所述第一点云数据计算激光雷达坐标系中重力矢量的第二原始观测值,根据所述第二IMU数据计算IMU坐标系中重力矢量的第一变换观测值,根据所述第二点云数据计算激光雷达坐标系中重力矢量的第二变换观测值;所述第一原始观测值以及所述第一变换观测值构成第一集合,所述第二原始观测值以及所述第二变换观测值构成第二集合;
第二计算模块,用于根据所述第一集合以及所述第二集合计算所述激光雷达与所述IMU之间的目标转换矩阵。
9.一种激光雷达与IMU的标定装置,其特征在于:所述激光雷达与IMU的标定装置包括处理器和存储器,所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集,所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如权利要求1-7中任一项所述方法。
10.一种计算机可读存储介质,其特征在于:所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集,所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如权利要求1-7中任一项所述方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310159585.0A CN116105772A (zh) | 2023-02-22 | 2023-02-22 | 一种激光雷达与imu的标定方法、装置及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310159585.0A CN116105772A (zh) | 2023-02-22 | 2023-02-22 | 一种激光雷达与imu的标定方法、装置及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116105772A true CN116105772A (zh) | 2023-05-12 |
Family
ID=86258004
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310159585.0A Pending CN116105772A (zh) | 2023-02-22 | 2023-02-22 | 一种激光雷达与imu的标定方法、装置及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116105772A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116740197A (zh) * | 2023-08-11 | 2023-09-12 | 之江实验室 | 一种外参的标定方法、装置、存储介质及电子设备 |
-
2023
- 2023-02-22 CN CN202310159585.0A patent/CN116105772A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116740197A (zh) * | 2023-08-11 | 2023-09-12 | 之江实验室 | 一种外参的标定方法、装置、存储介质及电子设备 |
CN116740197B (zh) * | 2023-08-11 | 2023-11-21 | 之江实验室 | 一种外参的标定方法、装置、存储介质及电子设备 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP2990828B1 (en) | Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor | |
US10636168B2 (en) | Image processing apparatus, method, and program | |
CN112219087A (zh) | 位姿预测方法、地图构建方法、可移动平台及存储介质 | |
KR20210084622A (ko) | 시간 동기화 처리 방법, 전자 기기 및 저장 매체 | |
CN110268445A (zh) | 利用陀螺仪的相机自动校准 | |
CN110470333B (zh) | 传感器参数的标定方法及装置、存储介质和电子装置 | |
CN112815939B (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
CN113701745B (zh) | 一种外参变化检测方法、装置、电子设备及检测*** | |
WO2018107831A1 (zh) | 一种测量物体姿态角的方法和装置 | |
CN112781586A (zh) | 一种位姿数据的确定方法、装置、电子设备及车辆 | |
CN116105772A (zh) | 一种激光雷达与imu的标定方法、装置及存储介质 | |
CN113960622A (zh) | 融合激光雷达及imu传感器信息的实时定位方法及装置 | |
CN114111776A (zh) | 定位方法及相关装置 | |
CN111998870B (zh) | 一种相机惯导***的标定方法和装置 | |
CN112284381B (zh) | 视觉惯性实时初始化对准方法及*** | |
CN116721166B (zh) | 双目相机和imu旋转外参在线标定方法、装置及存储介质 | |
CN116182905A (zh) | 激光雷达和组合惯导的时空外参标定方法、装置及*** | |
EP3392748B1 (en) | System and method for position tracking in a virtual reality system | |
CN113405532B (zh) | 基于视觉***结构参数的前方交会测量方法及*** | |
CN114705223A (zh) | 多移动智能体在目标跟踪中的惯导误差补偿方法及*** | |
CN111307176A (zh) | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 | |
CN113494927A (zh) | 一种车辆多传感器标定方法、装置及车辆 | |
CN110082805A (zh) | 一种三维定位装置和方法 | |
CN115439561B (zh) | 机器人的传感器标定方法、机器人及存储介质 | |
CN110827323A (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 |