CN115655305A - 外参标定方法、装置、计算设备、存储介质和车辆 - Google Patents
外参标定方法、装置、计算设备、存储介质和车辆 Download PDFInfo
- Publication number
- CN115655305A CN115655305A CN202211112283.XA CN202211112283A CN115655305A CN 115655305 A CN115655305 A CN 115655305A CN 202211112283 A CN202211112283 A CN 202211112283A CN 115655305 A CN115655305 A CN 115655305A
- Authority
- CN
- China
- Prior art keywords
- rotation
- singular value
- data
- odometer
- increment
- 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
Landscapes
- Indication And Recording Devices For Special Purposes And Tariff Metering Devices (AREA)
Abstract
本公开实施例提供一种外参标定方法、装置、计算设备、存储介质和车辆。外参标定方法包括:获取第一里程计数据和第二里程计数据;基于第一里程计数据计算第一旋转增量,基于第二里程计数据计算第二旋转增量;基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于超定方程组进行分解得到奇异值矩阵;在奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据第四大奇异值对应的特征向量确定旋转外参的初值。前述方案利用了奇异值激励在第三维度和第四维度上的运动激励尺度的差异程度筛选退化运动,使得对包括退化运动特征的数据更具有鲁棒性,也就是使得最终获得的旋转外参初值更为准确。
Description
技术领域
本公开涉及传感器技术领域,具体涉及一种外参标定方法、装置、计算设备、存储介质和车辆。
背景技术
主流自动驾驶方案中配置有至少两种类型的里程计传感器,通过对前述里程计传感器检测的里程计数据进行融合,实现更为精准地环境特征感知和车辆运动特性感知。实现至少两种里程计传感器里程计数据正确融合的前提是确定不同传感器之间的空间坐标转换关系,也就是确定各个里程计传感器之间的外参。前述外参包括旋转外参和平移外参。
实际应用中,由于支撑件的材料老化变形和运动过程中的振动变形,里程计传感器的位姿实时地变化,基于传统离线标定方法确定的离线标定外参与传感器的实时位姿并不匹配,基于前述离线标定外参和里程计传感器里程计数据得到的融合数据的精度无法满足应用需求,也就是离线标定外参不再具有可用性。
为避免离线标定外参的不可用,已有里程计数据融合方案采用在线外参标定方法,在里程计数据融合过程中实时地确定在线标定外参。由于在线外参标定方法在里程计数据融合过程中实现外参的标定,为实现标定外参的快速准确收敛,需要较为准确的确定用于里程计数据融合时标定外参的初始值。但是,由于测量方式的不可靠性,在线外参标定初始值,特别是旋转外参的初始值设定并不准确。
发明内容
为了解决上述技术问题,本公开实施例提供一种外参标定方法、装置、计算设备、存储介质和车辆。
第一方面,本公开实施例提供一种外参标定方法,用于实现第一里程计传感器和第二里程计传感器之间的外参标定;所述方法包括:
获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在所述第一检测时窗内生成的第二里程计数据;
基于所述第一里程计数据计算多个第一旋转增量,以及基于所述第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于所述超定方程组进行奇异值分解得到奇异值矩阵;
在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定旋转外参的初值。
可选的,在根据所述第四大奇异值对应的特征向量确定旋转外参的初值之前,所述方法还包括:
判断所述第三大奇异值是否大于预设阈值;
所述根据所述第四大奇异值对应的特征向量确定旋转外参的初值,包括:
在所述第三大奇异值大于所述预设阈值的情况下,根据所述第四大奇异值对应的特征向量确定所述旋转外参的初始值。
可选的,在根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组之前,所述方法还包括:
判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值;
若是,删除小于所述旋转增量阈值的第一旋转增量和第二旋转增量。
可选的,所述方法还包括:
计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值;
在所述旋转增量差值小于差值阈值的情况下,删除对应的第一旋转增量和第二旋转增量。
可选的,在确定旋转外参的初值之后,所述方法还包括:
获取第一里程计传感器在在第二检测时窗内生成的第三里程计数据;
根据待标定的旋转外参对所述第三里程计数据进行坐标转换,得到第四里程计数据;
基于所述第四里程计数据计算目标残差;
基于所述初值对所述目标残差进行最小化优化,确定旋转外参的标定值。
可选的,所述基于所述第四里程计数据计算目标残差,包括:
基于所述第二检测时窗内第一时间戳对应的第四里程计数据确定检测特征点数据,以及基于所述第二检测时窗内第二时间戳对应的第四里程计数据确定检测特征线面数据,其中所述第二时间戳早于所述第一时间戳;
根据所述检测特征点和所述检测特征线面计算距离残差;
基于距离残差确定所述目标残差。
可选的,所述方法还包括:获取第二里程计传感器在所述第二检测时窗内生成的第五里程计数据;
根据所述第四里程计数据和第五里程计数据计算边缘化先验残差;
所述基于距离残差确定所述目标残差,包括:基于所述边缘化先验残差和所述距离残差确定所述目标残差。
可选的,所述方法还包括:根据所述第五里程计数据确定实际积分增量和预积分增量;
根据所述实际积分增量和所述预积分增量计算预积分残差;
所述基于所述边缘化先验残差和所述距离残差确定所述目标残差,包括:
基于所述边缘化先验残差、所述距离残差和所述预积分残差确定所述目标残差。
可选的,在基于所述旋转外参的初值对所述目标残差进行最小化优化之前,所述方法还包括:
计算所述距离残差和所述预积分残差的和值;
判断所述边缘化先验残差与所述和值的比值是否小于第二比值阈值;
若是,执行基于所述旋转外参的初值对所述目标残差进行最小化优化。
可选的,在基于所述旋转外参的初值对所述目标残差进行最小化优化之前,所述方法还包括:
判断承载刚体是否处在目标退化运动状态,所述承载刚体为承载所述第一里程计传感器和所述第二里程计传感器的刚体;
若否,基于所述旋转外参的初值对所述目标残差进行最小化优化。
第二方面,本公开实施例提供一种外参标定装置,用于实现第一里程计传感器和第二里程计传感器之间的外参标定;所述装置包括:
第一数据获取单元,用于获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在所述第一检测时窗内生成的第二里程计数据;
旋转增量计算单元,用于基于所述第一里程计数据计算多个第一旋转增量,以及基于所述第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
求解计算单元,用于基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于所述超定方程组进行奇异值分解得到奇异值矩阵;
第一判断单元,用于判断所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值是否大于第一比值阈值;
外参初值确定单元,用于在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定旋转外参的初值。
可选的,所述第一判断单元还用于判断所述第三大奇异值是否大于预设阈值;
所述外参初值确定单元在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值,并且所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定所述旋转外参的初始值。
可选的,所述装置还包括:第二判断单元,用于判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值;
删除单元,用于删除小于所述旋转增量阈值的第一旋转增量和第二旋转增量。
可选的,所述装置还包括:差值计算单元,用于计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值;
所述删除单元还用于在所述旋转增量差值小于差值阈值的情况下,删除对应的第一旋转增量和第二旋转增量。
可选的,所述第二里程计传感器为惯性测量单元或者差分定位传感器;在确定旋转外参的初值之后,所述装置还包括:
第二数据获取单元,用于获取第一里程计传感器在第二检测时窗内生成的第三里程计数据;
里程计数据转换单元,用于根据待标定的旋转外参对所述第三里程计数据进行坐标转换,得到第四里程计数据;
目标残差确定单元,用于基于所述第四里程计数据计算目标残差;
外参标定值确定单元,用于基于所述旋转外参的初值对所述目标残差进行最小化优化,确定旋转外参的标定值。
可选的,所述目标残差确定单元包括:
特征确定子单元,用于基于所述第二检测时窗内第一时间戳对应的第四里程计数据确定检测特征点数据,以及基于所述第二检测时窗内第二时间戳对应的第四里程计数据确定检测特征线面数据,其中所述第二时间戳早于所述第一时间戳;
距离残差计算子单元,用于根据所述检测特征点和所述检测特征线面计算距离残差;
目标残差确定子单元,用于基于距离残差确定所述目标残差。
可选的,所述第二数据获取单元还用于获取第二里程计传感器在所述第二检测时窗内生成的第五里程计数据;
所述目标残差确定单元还包括:先验残差确定子单元,用于根据所述第四里程计数据计算边缘化先验残差;
所述目标残差确定子单元基于所述边缘化先验残差和所述距离残差确定所述目标残差。
可选的,所述目标残差确定单元还包括:
积分增量计算子单元,用于根据所述第五里程计数据确定实际积分增量和预积分增量;
预积分残差计算子单元,用于根据所述实际积分增量和所述预积分增量计算预积分残差;
所述目标残差确定子单元基于所述边缘化先验残差、所述距离残差和所述预积分残差确定所述目标残差。
可选的,和值计算单元,用于计算所述距离残差和所述预积分残差的和值;
比值判断单元,用于判断所述边缘化先验残差与所述和值的比值是否小于第二比值阈值;
在所述比值判断单元判定所述边缘化先验残差与所述和值的比值小于第二比值阈值的情况下,所述外参标定值确定单元基于所述旋转外参的初值对所述目标残差进行最小化优化。
可选的,所述装置还包括:退化状态判定单元,用于判断承载刚体是否处在目标退化运动状态,所述承载刚体为承载所述第一里程计传感器和所述第二里程计传感器的刚体;
在所述退化状态判定单元判定承载刚体未处在目标退化运动状态的情况下,所述外参标定值确定单元基于所述旋转外参的初值对所述目标残差进行最小化优化。
第三方面,本公开实施例提供一种计算设备,包括处理器和存储器,所述存储器用于存储计算机程序;所述计算机程序在被所述处理器加载时,使所述处理器执行如前所述的外参标定方法。
第四方面,本公开实施例提供一种计算机可读存储介质,所述存储介质存储有计算机程序,当计算机程序被处理器执行时,使得处理器实现如前所述的外参标定方法。
第五方面,本公开实施例提供一种车辆,包括车载控制芯片、第一里程计传感器和第二里程计传感器,所述车载控制芯片用于执行如前所述的外参标定方法。
本公开实施例提供的技术方案与现有技术相比具有如下优点:
采用本公开实施例提供的方案,在基于第一里程计数据计算得到多个第一旋转你增量,基于第二里程计数据计算得到多个第二旋转增量,基于手眼标定模型、第一旋转增量和第二旋转增量构建超定方程组,并实现超定方程组的奇异值分解得到奇异值矩阵后,通过第三大奇异值和第四大奇异值比较得到奇异值比值,在奇异值比值大于第一比值的情况下,再将第四大奇异值对应的特征向量作为旋转外参的初值。前述方案利用了奇异值激励在第三维度和第四维度上的运动激励尺度的差异程度筛选退化运动,使得对包括退化运动特征的数据更具有鲁棒性,也就是使得最终获得的旋转外参初值更为准确。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本公开的实施例,并与说明书一起用于解释本公开的原理。
为了更清楚地说明本公开实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地,对于本领域普通技术人员而言,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图,其中:
图1是本公开实施例一些实施例提供的外参标定方法流程图;
图2是手眼标定模型实现的原理示意图;
图3是本公开一些实施例提供的外参标定方法流程图;
图4是本公开一些实施例提供的外参标定方法流程图;
图5是本公开实施例基于旋转外参的初值确定标定值的方法流程图;
图6是本公开实施例提供的外参标定装置的结构示意图;
图7是本公开实施例提供的计算设备的结构示意图。
具体实施方式
下面将参照附图更详细地描述本公开的实施例。虽然附图中显示了本公开的某些实施例,然而应当理解的是,本公开可以通过各种形式来实现,而且不应该被解释为限于这里阐述的实施例,相反提供这些实施例是为了更加透彻和完整地理解本公开。应当理解的是,本公开的附图及实施例仅用于示例性作用,并非用于限制本公开的保护范围。
本文使用的术语“包括”及其变形是开放性包括,即“包括但不限于”。术语“基于”是“至少部分地基于”。术语“一个实施例”表示“至少一个实施例”;术语“另一实施例”表示“至少一个另外的实施例”;术语“一些实施例”表示“至少一些实施例”。其他术语的相关定义将在下文描述中给出。需要注意,本公开中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分,并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。
需要注意,本公开中提及的“一个”、“多个”的修饰是示意性而非限制性的,本领域技术人员应当理解,除非在上下文另有明确指出,否则应该理解为“一个或多个”。
本公开实施例提供一种外参标定方法,用于实现两个里程计传感器之间外参的标定。在一些实施例中,实现两个里程计传感器之间外参的标定包括确定旋转外参的初值。在一些具体实施例中,在确定旋转外参初值的情况下,还可以实时地确定旋转外参的标定值。
里程计传感器是对承载刚体随时间变化的运动状态进行测量的传感器。前述的承载刚体是承载里程计传感器的刚体。前述的运动状态可以包括承载刚体的加速度、速度或者移动距离。具体实施中,里程计传感器可以是对承载刚体的运动特征进行测量,确定承载刚体运动状态的传感器,也可以是对承载刚体所在的环境特征进行测量,并根据环境特征间接确定承载刚体运动状态的传感器。
实际实施中,前述的两个里程计传感器均可以是惯性测量单元、相机、激光雷达、毫米波雷达、差分定位传感器(也就是诸如卫星导航定位传感器、场端导航定位传感器等)中的任意一种,本公开实施例不做特别地限定。
本公开实施例提供的外参标定方法可以应用于各种采用两个里程计传感器输出数据进行数据融合处理的场景中,前述场景可以是车辆驾驶场景,也可以是机器人控制场景,还可以是其他场景,本公开实施例也不做特别地限定。其中,在场景为车辆驾驶场景的情况下,承载刚体为车辆本体(具体多为车身);在场景为机器人控制场景的情况下,承载刚体为机器人自由端的运动臂。
还应注意的是,本公开实施例提供的外参标定方法由计算设备执行。计算设备可以是车辆的车机系统或者机器人的控制系统。在一些场景应用中,计算设备还可以是与前述车辆、机器人通信连接的远程服务器、边缘服务器或者场端服务器。
图1是本公开实施例一些实施例提供的外参标定方法流程图。如图1所示,本公开实施例提供的外参标定方法包括S110-S140。
S110:获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在第一检测时窗内生成的第二里程计数据。
第一检测时窗是预先设定的,用于圈定里程计传感器外参初值所需原始里程计数据的时窗。第一检测时窗的时窗长度根据第一里程计传感器和第二里程计传感器的采样频率确定。应当注意的是,第一检测时窗内获取的第一里程计数据和第二里程计数据的数据量应当满足后续步骤的执行,以保证可以确定旋转外参的初值。
计算设备获取第一里程计数据和第二里程计数据可以是在线实时地从第一里程计传感器和第二里程计传感器处获取,也可以是离线地从存储器中获取,本公开实施例不做特别地限定。也就是说,本公开实施例提供的外参标定方法可以是在线实时进行的,也可以是离线进行的。
S120:基于第一里程计数据计算多个第一旋转增量,以及基于第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量。
在获取到第一检测时窗内的第一里程计数据后,计算设备可以基于第一里程计数据确定多个第一旋转增量。
第一旋转增量是通过第一里程计数据确定的表征承载刚体在特定时间段(特定时间段为第一检测时窗内的较短时间段)内旋转角度的角度增量。前述的特定时间段可以用时间戳表示。在获取到第二检测时窗内的第二里程计数据后,计算设备可以基于第二里程计数据确定多个第二旋转增量。
第二旋转增量是通过第二里程计数据确定的表征承载刚体在特定时间段内旋转角度的角度增量。本公开实施例中,基于第二里程计数据计算第二旋转增量时,应当计算时间戳与各个第一旋转增量相同的第二旋转增量,也就是说需要计算体现相同时间段内承载刚体旋转程度第二旋转增量。
应当注意的是,前述第一旋转增量是在第一里程计传感器坐标系下的旋转增量,第二旋转增量是在第二里程计传感器坐标系下的旋转增量,并且第一旋转增量和第二旋转增量均为三个自由度的旋转增量。
具体实施中,如何基于第一里程计数据计算多个第一旋转增量,以及如何基于第二里程计数据计算多个第二旋转增量,需要根据第一里程计传感器和第二里程计传感器的类型确定,具体的计算方法请参见相关的技术文献,此处不再展开说明。
具体实施例中,第一里程计传感器和第二里程计传感器的数据生成频率可能不同,在此情况下,可以基于时间插值的方法求取与第一里程计数据时间戳相同的第二里程计数据,进而再根据计算得到的第二里程计数据计算得到时间戳与第一旋转增量相同的第二旋转增量。
S130:基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于超定方程组进行奇异值分解得到奇异值矩阵。
手眼标定模型是标定不同坐标系之间外参关系的模型。因为第一里程计传感器和第二里程计传感器是固定在承载刚体、相对承载刚体位姿并不发生变化,二者之间的外参保持不变,所以对应的手眼标定模型应当是Eye-In-Hand模型。
应当注意的是,前述的第一里程计传感器和第二里程计传感器在承载刚体上的位姿不发生变化仅是理论上不发生变化,实际应用中第一里程计传感器、第二里程计传感器相对于承载刚体的位姿可能由于承载刚体材料的老化、受力弹性变形而发生变化,进而使得第一里程计传感器和第二里程计传感器之间的旋转外参发生改变。
图2是手眼标定模型实现的原理示意图。如图2所示,手眼标定模型可以采用表示,其中表示时间戳为k和k+1的第一旋转增量(也就是从第k时刻到第k+1时刻的第一旋转增量),表示旋转外参,表示时间戳为k和k+1的第二旋转增量(也就是从第k时刻到第k+1时刻的第二旋转增量)。
根据前述的手眼标定模型,采用时间戳相同的第一旋转增量和第二旋转增量可以构建一个包括待求解外参的方程。在具有多个时间戳相同的第一旋转增量和第二旋转增量的情况下,可以采用多个时间戳相同的第一旋转增量和第二旋转增量构建多个包括待求解外参的方程,并利用多个方程构建超定方程组。实际应用中,因为旋转外参包括三个待求参数(也就是三个自由度),而超定方程组中方程数量多于待求参数,所以超定方程组中包括的待求解方程的数量至少为四个。
在本公开的一个实施例中,各个待求解方程均可以采用四元数表示,以方便超定方程组的求解。
在得到前述的四元数等式后,对其进行移项变换,可以得到其中Ql(q)和Qr(q)分别表示四元数的左乘和右乘形式,前述公式中四元数q表示为[qxqy qz qw]T,四元数q的虚部qv表示为[qx qy qz]T,[qv]×为虚部向量的反对称矩阵,
奇异值矩阵(也就是对角矩阵)Σ中的σi称为矩阵QN的奇异值,通过行列式变换可以使得奇异值满足σ1≥σ2≥σ3≥σ4。
S140:在奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据第四大奇异值对应的特征向量确定旋转外参的初值。
在得到奇异值矩阵和对应的右奇异向量后,计算设备随后对奇异值矩阵中的第三大奇异值和第四大奇异值进行比较,以确定奇异值比值。
在得到奇异值比值之后,计算设备将奇异值比值与第一比值阈值进行比较,判断奇异值比值是否大于第一比值阈值。如果奇异值比值大于第一比值阈值,则根据第四大奇异值对应的特征向量确定旋转外参的初值,也就是利用第四大奇异值σ4对应的右奇异向量v4作为旋转外参的初值。应当注意的是,为旋转外参的四元数表示。
在S140中,通过第三大奇异值和第四大奇异值比较得到奇异值比值,在奇异值比值大于第一比值的情况下,再将第四大奇异值σ4对应的右奇异向量v4作为旋转外参的初值,利用了奇异值激励在第三维度和第四维度上的运动激励尺度的差异程度筛选退化运动,使得对包括退化运动特征的数据更具有鲁棒性,也就是使得最终获得的旋转外参初值更为准确。
在旋转外参初值设定更为准确的前提下,基于旋转外参初值进行数据再处理确定旋转外参标定值时的数据运算量较小,最终确定的旋转外参标定值也就更为准确。
可选的,在公开的一些实施例中,S140中根据第四大奇异值对应的特征向量确定旋转外参的初值具体可以包括S141-S142。
S141:判断第三大奇异值是否大于预设阈值。若是,执行S142。
S142:根据第四大奇异值对应的特征向量确定旋转外参的初始值。
如果第三大奇异值大于预设阈值,则表明承载刚体在旋转增量最小的自由度上的运动激励尺寸已经较大,因此判定承载刚体处在退化运动中的可能性更小,继而将第四大奇异值对应的特征向量确定为旋转外参的初始值。
在执行前述S141和S142的基础上,在奇异值比值大于第一比值的情况下,再判断第三大奇异值是否大于预设阈值,使得求解得到的旋转外参初值更符合实际情况。也就是说,采用本公开实施例提供的方法,可以使得求解得到的旋转外参的初值更为准确。
图3是本公开一些实施例提供的外参标定方法流程图。如图3所示,在本公开的一些实施例中,外参标定方法包括S210-S260。
S210:获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在第一检测时窗内生成的第二里程计数据;
S220:基于第一里程计数据计算多个第一旋转增量,以及基于第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
S230:判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值;若是,执行S240。
S240:删除小于旋转增量阈值的第一旋转增量和第二旋转增量。
S250:基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于超定方程组进行奇异值分解得到奇异值矩阵;
S260:在奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据第四大奇异值对应的特征向量确定旋转外参的初值。
本公开实施例提供的外参标定方法,S210-S220与前文实施例中的S110-S120相同,S250-S260与前文实施例中的S130-S140相同,此处不再复述,具体可以参见前文表述。
与前文实施例不提供的是,本公开实施例中计算设备还会判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值。旋转增量阈值是用于排除不可用的第一旋转增量和第二旋转增量的数值阈值,旋转增量阈值根据大量数据测试确定。
实际应用中,承载刚体的运动状态可能处在直线运动状态。在此情况下,基于第一里程计数据确定的第一旋转增量为0,基于第二里程计数据确定的第二旋转增量为0。在此情况下构建的手眼标定模型中,和趋近于单位矩阵I,也就是手眼标定模型退化为
而根据单位矩阵的性质,任何旋转矩阵R均能够满足也就是说,在承载刚体的运动状态退化为直线运动状态或者近似直线运动状态时,并不利于旋转增量初值的求解。为了避免前述问题,通过执行S230判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值,可以确定对应的第一里程计数据和第二里程计数据是否是直线运动或者近似直线运动状态下获得的。
如果第一里程计数据和第二里程计数据是承载刚体在直线运动或者近似直线运动状态下获得的,则执行S240删除对应的第一旋转增量和第二旋转增量,避免采用前述数值较小的第一旋转增量和第二旋转增量构建待求解方程,进而避免造成的旋转外参与真实值差别较大的问题。
图4是本公开一些实施例提供的外参标定方法流程图。如图4所示,在本公开的一些实施例中,外参标定方法包括S310-S360。
S310:获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在第一检测时窗内生成的第二里程计数据;
S320:基于第一里程计数据计算多个第一旋转增量,以及基于第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
S330:计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值。
S340:在旋转增量差值小于差值阈值的情况下,删除对应的第一旋转增量和第二旋转增量。
S350:基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于超定方程组进行奇异值分解得到奇异值矩阵;
S360:在奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据第四大奇异值对应的特征向量确定旋转外参的初值。
本公开实施例提供的外参标定方法,S310-S320与前文实施例中的S110-S120相同,S350-S360与前文实施例中的S130-S140相同,此处不再复述,具体可以参见前文表述。
与前文实施例不同的是,本公开实施例中,计算设备还会计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值,并判定旋转增量差值是否小于差值阈值。差值阈值是用于排除不可用的第一旋转增量和第二旋转增量的数值阈值,差值阈值的数值根据大量数据测试确定。
如果旋转增量差值小于差值阈值,则表明第一里程计传感器输出的第一里程计数据和第二里程计传感器输出的第二里程计数据中的变化特性相同(均为随着承载刚体转动,相对于承载刚体的转动量较小),第一里程计传感器和第二里程计传感器没有发生较大的姿态变化,或者第一里程计传感器和第二里程计传感器检测的随机噪声较小。
而如果计算得到的旋转增量差值大于差值阈值,则可能出现情况:(1)第一里程计传感器和第二里程计传感器至少之一发生较大的姿态变化,此姿态变化可能是暂时的姿态变化;(2)第一里程计传感器或者第二里程计传感器之一受到随机噪声的影响,输出的第一里程计数据或者第二里程计数据无法表征承载刚体的旋转运动真实情况。
为了避免基于前述异常第一里程计数据和第二里程计数据的影响,本公开实施例中,如果旋转增量差值大于差值阈值,则删除对应的第一旋转轴增量和第二旋转增量,避免避基于异常的第一旋转增量和第二旋转增量计算旋转外参,造成的旋转外参求解误差与实际值差别较大的问题。
前述的S210-S260、S310-S360分别提供了一种确定旋转外参的方法。在一些实施例中,前述的S210-S260与S310-S360还可以组合,得到一种新的确定旋转外参的方法。具体是将S330-S340添加至S210-S260中,形成一种新的确定旋转外参的方法。实际实施例中,S330-S340可以在S230-S240之前执行,也可以在S230-S240之后执行,但是应当在S250之前执行。
为了更为直观的理解前述方法,以下以确定两个不同类型的里程计传感器对本发明实施例提供的旋转外参确定方法再做分析。两种不同类型的里程计传感器分别为激光雷达(Light Detection And Ranging,LiDAR)和惯性测量单元(Inertial MeasurementUnit,IMU)。
计算设备在获取到激光雷达输出的当前帧点云数据后(也就是第一里程计数据)后,根据承载刚体所处的运动状态对点云数据进行去畸变,得到去畸变后点云数据。具体实施中,可以根据惯性测量单元测量的惯性测量数据(也就是第二里程计数据)进行预积分运算,确定激光雷达的位姿变换矩阵随后使用位姿变换矩阵对点云数据进行去畸变,得到去畸变后的当前帧点云数据。
在得到去畸变后的当前帧点云数据后,计算设备对去畸变后的当前帧点云数据进行特征提取,获取特征点,特征点可以是当前帧点云数据中曲率变化最高或者最低的点,也就是点云数据中最平滑的点或者最边缘的点。
在获取到当前帧点云数据中的特征点后,随后将特征点和基于历史帧点云数据构建的局部地图中的特征线面进行匹配,使得去畸变后的当前点云数据能够融合至局部地图中。
在将当前点云数据融合到局部地图中后,可以根据局部地图中当前帧点云数据和在前帧点云数据确定雷达旋转增量(也就是第一旋转增量)。
此外,计算设备还会获取惯性测量单元输出的惯性测量数据(包括陀螺仪输出的角速度数据和加速度计输出的线速度数据等),根据惯性测量数据进行预积分计算,得到惯性测量旋转增量(也就是第二旋转增量)。
如前所述,在计算惯性测量旋转增量时,为使得其时间戳与雷达旋转增量的时间戳相同,还可以针对惯性测量数据进行插值计算,确定时间戳与当前帧点云数据相同的惯性测量数据,再基于此惯性测量数据计算第二旋转增量。
在得到第一旋转增量和第二旋转增量后,可以采用前述的S230-S240和S330-S340对第一旋转增量和第二旋转增量进行过滤,过滤掉不符合条件的旋转增量。随后可以采用剩余的第一旋转增量和第二旋转增量构建超定方程组,基于超定方程组进行奇异值分解,并根据奇异值分解结果采用S160确定是否获取到旋转外参的初值。
如前所述,通过采用前述的方法可以获取到旋转外参的初值。但是得到旋转外参的初值并不是目的,还需要基于旋转外参的初值确定旋转外参的标定值,也就是确定较为准确的旋转外参。为实现前述目的,本公开实施例中,在确定旋转外参的初值后还基于初值确定旋转外参的标定值。
图5是本公开实施例基于旋转外参的初值确定标定值的方法流程图。如图5所示,在本公开实施例中,基于旋转外参初值确定标定值的方法包括S410-S440。
应当注意的是,在本公开实施例中,基于外参初值确定标定值,是将第一里程计传感器获取的第一里程计数据进行坐标转换,转换至第二里程传感器的坐标系下,并基于第二里程计传感器坐标系下的数据进行计算。实际应用中,第二里程计数据优选为能够直接测量承载刚体运动状态的传感器,例如可以是惯性测量单元或者差分定位传感器。
S410:获取第一里程计传感器在第二检测时窗内生成的第三里程计数据。
步骤S410中的实现方法与前文中的S110获取第一里程计数据的方法相同,此处不再复述,具体可以参见前文表述;应当注意的是,第二检测时窗为一滑动时窗。
S420:根据待标定的旋转外参对第三里程计数据进行坐标转换,得到第四里程计数据。
计算设备根据待标定的旋转外参对第三里程计数据进行坐标转换,是基于待标定的旋转外参确定旋转矩阵,并基于旋转矩阵对第三里程计数据进行坐标转换。当然,实际应用中,也可以采用四元数方法基于待标定的旋转外参对第三里程计坐标进行转换,得到第四里程计数据。
前文根据待标定旋转外参对第三里程计数据进行坐标转换得到第四里程计数据,是将并不一定准确的旋转外参数值对第三里程计数据进行坐标转换,得到一些并一定准确的第四里程计数据。
应当注意的是,在进行坐标转换时,还需要平移外参。考虑到平移外参并不是本发明实施例的关注重点,此处不再展开表述。具体实施中,可以将平移外参作为待优化的数据,在执行后文采用目标残差确定旋转外参的标定值时,同时实现对平移外参的标定。
S430:基于第四里程计数据计算目标残差。
在得到第四里程计数据后,计算设备可以根据第四里程计数据计算目标残差。
具体如何计算目标残差,后文中再做分析。应当注意的是,对于不同的旋转外参的数值,执行前述S420-S430得到的目标残差的数值并不相同。
S440:基于旋转外参的初值对目标残差进行最小化优化,确定旋转外参的标定值。
基于初值对目标残差进行最小化优化,是将初值作为最先使用的并不一定准确的旋转外参,执行前述的S420-S440,得到目标残差。随后再采用高斯-牛顿算法等方法基于目标残差对待标定的旋转外参进行修正,并重复地执行S420-S440,直至目标残差能够达到最小化。在确定目标残差最小化后,用于获取最小化的目标残差所使用的旋转外参数值即为旋转外参的标定值。
利用采用前述的S410-S440,本公开实施例采用残差优化的方法(也可以称为非线性优化的方法),基于前文确定的旋转外参的初值,实现了旋转外参标定值的计算。
前文提及需要基于第四里程计数据计算目标残差,以下对如何基于第四里程计数据计算目标残差做说明。
在本公开的一些实施例中,S430基于第四里程计数据计算目标残差可以包括S431-S434。
S431:基于第二检测时窗内第一时间戳对应的第四里程计数据确定检测特征点,以及基于第二检测时窗内第二时间戳对应的第四里程计数据确定检测特征线面。
前述的第二时间戳早于第一时间戳。应当注意的是,前述的第一时间戳是一个时间点的时间戳,第二时间戳可以是一个时间点的时间戳,也可以是多个时间点的时间戳。并且,第一时间戳和第二时间戳可以是相邻的时间戳,也可以是不相邻的时间戳,本公开实施例不做特别地限定。
在一个具体实施中,第二时间戳为连续多个时间戳,基于第二时间戳对应的第四里程计数据确定检测特征线面,是基于多个时间戳对应的第四里程计数据确定检测特征线面。
本公开实施例中,计算设备可以对第四里程计数据中时间戳为第一时间戳的里程计数据进行特征提取,确定第二时间窗口内第一时间戳获取的里程计数据中,用于表征检测特征点的数据。前述的检测特征点优选是表征某一检测平面上的点。
本公开实施例中,计算设备可以对第四里程计数据中时间戳为第二时间戳的里程计数据进行特征提取,确定第二时间窗口内第二时间戳获取的里程计数据中表征特征线面的数据。
S432:根据检测特征点和检测特征线面计算距离残差。
根据检测特征点和检测特征线面计算距离残差,是首先确定检测特征点到匹配的检测特征线面的垂直距离,并将多个垂直距离的平均值作为距离残差。
S433:基于距离残差确定目标残差。
本公开实施例中,基于距离残差确定目标残差,可以是将距离残差作为目标残差。
可选的,在本公开的一些实施例中,目标残差的计算方法还可以包括S434-S435。
S434:获取第二里程计传感器在所述第二检测时窗内生成的第五里程计数据;
S435:根据所述第四里程计数据和所述第五里程计数计算边缘化先验残差。
本公开实施例中,边缘化先验残差用于将第四里程计数据中时间戳最早的数据删除,以及相关的第五里程计数据的删除,但保留前述数据对第一检测时窗内其他数据的约束关系而得到的残差。边缘化先验残差将前述的约束关系转换为一个约束项放入到目标残差中,以利于对旋转外参的优化和一些状态变量的优化。
在执行S434-S435的前提下,前述的S433包括:基于边缘化先验残差和距离残差确定目标残差。
S4311:基于边缘化先验残差和距离残差确定目标残差,是求取边缘化先验残差和距离残差的和值,并将和值作为目标残差。
通过将边缘化先验残差引入到目标残差的计算过程中,将第二检测时窗内得到的在前帧的第三里程计数据边缘化,并将与第三里程计数据计算得到的所有边缘化测量值转换为一个新的先验约束,并将先验约束加入到在后的优化约束中,使得在求解过程中,需要边缘化的变量可不被关注,进而减少计算量,也就是简化了后续利用目标残差确定旋转外参标定值的计算量。此外,通过引入边缘化先验残差,可以避免旋转外参标定量发生漂移的问题。
可选的,在本公开的一些实施例中,S430基于第四里程计数据计算目标残差还可以包括S436-S437。
S436:根据第五里程计数据计算实际积分增量和预积分增量。
S437:根据实际积分增量和预积分增量计算预积分残差。
本公开实施例中,根据第五里程计数据计算实际积分增量。
实际应用中,第二里程计传感器中的某些检测装置可能会出现偏置。例如,在第二里程计传感器为惯性测量单元的情况下,其中的加速度计和陀螺仪偏置会随机游走。而前述偏置的是在旋转外参标定过程中需要考虑的。而可以通过预积分残差可以表示前述偏置情况。
在执行S436-S437的基础上,S4311具体为S43111。
S43111:基于边缘化先验残差、距离残差和预积分残差确定目标残差。
基于边缘化先验残差、距离残差和预积分残差确定目标残差是将边缘化先验残差、距离残差和预积分残差相加的和值作为目标残差。
本公开实施例中,通过引入预计分残差考虑第二里程计传感器的偏置情况,使得残差计算过程中能够考虑第二里程计传感器自身的状态量,进而使得计算得到的旋转外参标定值更为准确。
以下以前文确定的激光雷达和惯性测量单元之间旋转外参的初值,对如何计算旋转外参的标定值再做说明。
激光雷达在第二检测时窗获得的第三里程计数据采用旋转外参和平移外参转换后,得到的第四里程计数据采用X表示, 其中p和j是第二检测时窗中间某一帧第三里程计数据的时间戳;n是第二检测时窗中的第三里程计数据的总数;Xk是第k帧第三里程计数据对应的第四里程计数据,它由速度方向IMU的加速度计偏差ba和陀螺仪偏差bg构成;Tl b表示激光雷达与惯性测量单元之间的六自由度外参,包含平移量和旋转量
在获得前述的目标残差后,可以采用高斯牛顿法按照Hδx=-b的形式进行求解,得到旋转外参的标定值。在得到旋转外参标定值的同时,还可以同时计算得到平移外参,激光雷达采集的第三里程计数据在惯性测量单元坐标系下的状态数据,以及惯性测量单元的加速度计偏差和陀螺仪偏差。
实际应用中,在得到前述的旋转外参的标定值后,可以利用前述的旋转外参的标定值作为新的旋转外参的初值,在下一个第二检测时窗内重新执行前述方法,进而得到新的旋转外参的初值。
在本公开的一些实施例中,计算设备在执行前述的S440之前,计算设备还可以执行S450-S460。
S450:计算距离残差和预积分残差的和值。
S460:判断边缘化先验残差与和值的比值是否小于第二比值阈值;若是,执行S440。
在一些具体实施中,第一里程计传感器是诸如激光雷达等传感器,在其初始化完成后的一段时间内,基于获取到的第三里程计数据确定的点云地图还未收敛,点云的匹配效果较差,导致边缘化先验残差过大,在这时利用初值迭代优化旋转外参会导致解析解偏离实际旋转外参。在此时,应当确保系统稳定后,再执行前述的S440。
根据各个残差涵义确定,边缘化先验残差可以用于判定作为系统是否稳定的依据。
为判断系统是否稳定,本公开实施例中,通过将边缘化先验残差与距离残差和预积分残差之和的比值和第二比值阈值比较。如果比值大于第二比值阈值,则认定系统并不稳定,因此此时不再执行S440。
可选的,在本公开的一些实施例中,第二里程计传感器为惯性测量单元。在此情况下计算设备在执行前述的S440之前,计算设备还可以执行S470。
S470:判断承载刚体是否处在目标退化运动状态;若否,执行S440。
如前,承载刚体为承载第一里程计传感器和第二里程计传感器的刚体。目标退化运动状态是无法使得惯性测量单元和其他里程计传感器之间旋转外参标定的承载刚体运动状态。在退化运动中采集得到的第一里程计数据和第二里程数据无法实现旋转外参的准确标定。
本公开实施例中,目标退化运动状态可以是承载刚体的静止、匀速运动状态。具体实施中,计算设备可以对第三里程计数据或者第五里程计数据进行分析,确定承载刚体是否处在目标退化运动状态。
如果承载刚体没有处在目标退化运动状态,则可以执行S440。而如果承载刚体处在目标退化运动状态,则不再执行S440。
采用本公开实施例提供的方法,在确定承载刚体处在目标退化运动状态的情况下,不再执行S440,可以避免采用旋转外参的初值进行旋转外参标定值优化的过程中,初值在迭代优化过程中陷入到局部极小值,而无法得到较为准确的旋转外参标定值的问题。
除了提供前述的外参标定方法外,本公开实施例还提供一种实现前述外参标定方法的外参标定装置。图6是本公开实施例提供的外参标定装置的结构示意图。如图6所示,外参标定装置600包括第一数据获取单元601、旋转增量计算单元602、求解计算单元603、第一判断单元604和外参初值确定单元605。
第一数据获取单元601用于获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在第一检测时窗内生成的第二里程计数据;旋转增量计算单元602用于基于第一里程计数据计算多个第一旋转增量,以及基于第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;求解计算单元603用于基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于超定方程组进行奇异值分解得到奇异值矩阵;第一判断单元604用于判断奇异值矩阵中的第三大奇异值与第四大奇异值的比值是否大于第一比值阈值;外参初值确定单元605用于在奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据第四大奇异值对应的特征向量确定旋转外参的初值。
在本公开的一些实施例中,第一判断单元604还用于判断第三大奇异值是否大于预设阈值;外参初值确定单元605在奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值,并且奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据第四大奇异值对应的特征向量确定旋转外参的初始值。
在本公开的一些实施例中,外参标定装置600还包括第二判断单元和删除单元。第二判断单元用于判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值;删除单元用于删除小于旋转增量阈值的第一旋转增量和第二旋转增量。
在在本公开的一些实施例中,外参标定装置600还包括差值计算单元。差值计算单元用于计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值;删除单元还用于在旋转增量差值小于差值阈值的情况下,删除对应的第一旋转增量和第二旋转增量。
在本公开的一些实施例中,第二里程计传感器为惯性测量单元或者差分定位传感器;在确定旋转外参的初值之后,外参标定装置600还包括第二数据获取单元、里程计数据转换单元、目标残差确定单元和外参标定值确定单元。
第二数据获取单元用于获取第一里程计传感器在第二检测时窗内生成的第三里程计数据;里程计数据转换单元用于根据待标定的旋转外参对第三里程计数据进行坐标转换,得到第四里程计数据;目标残差确定单元用于基于第四里程计数据计算目标残差;外参标定值确定单元用于基于初值对目标残差进行最小化优化,确定旋转外参的标定值。
在本公开的一些实施例中,目标残差确定单元包括特征确定子单元、距离残差计算子单元和目标残差确定子单元。特征确定子单元用于基于第二检测时窗内第一时间戳对应的第四里程计数据确定检测特征点数据,以及基于第二检测时窗内第二时间戳对应的第四里程计数据确定检测特征线面数据,其中第二时间戳早于第一时间戳;距离残差计算子单元用于根据检测特征点和检测特征线面计算距离残差;目标残差确定子单元用于基于距离残差确定目标残差。
在本公开的一些实施例中,第二数据获取单元还用于获取第二里程计传感器在第二检测时窗内生成的第五里程计数据目标残差确定单元还包括:先验残差确定子单元用于根据第四里程计数据和第五里程计数据计算边缘化先验残差;目标残差确定子单元基于边缘化先验残差和距离残差确定目标残差。
在本公开的一些实施例中,目标残差确定单元还包括:积分增量计算子单元用于根据第五里程计数据确定实际积分增量和预积分增量;预积分残差计算子单元,用于根据实际积分增量和预积分增量计算预积分残差;目标残差确定子单元基于边缘化先验残差、距离残差和预积分残差确定目标残差。
在本公开的一些实施例中,外参标定装置600还包括和值计算单元和比值判断单元。和值计算单元用于计算距离残差和预积分残差的和值;比值判断单元用于判断边缘化先验残差与和值的比值是否小于第二比值阈值;在比值判断单元判定边缘化先验残差与和值的比值小于第二比值阈值的情况下,外参标定值确定单元基于初值对目标残差进行最小化优化。
在本公开的一些实施例中,外参标定装置600还包括退化状态判定单元。退化状态判定单元用于判断承载刚体是否处在目标退化运动状态,承载刚体为承载第一里程计传感器和第二里程计传感器的刚体。在退化状态判定单元判定承载刚体未处在目标退化运动状态的情况下,外参标定值确定单元基于初值对目标残差进行最小化优化。
A1.一种外参标定方法,用于实现第一里程计传感器和第二里程计传感器之间的外参标定,所述方法包括:
获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在所述第一检测时窗内生成的第二里程计数据;
基于所述第一里程计数据计算多个第一旋转增量,以及基于所述第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于所述超定方程组进行奇异值分解得到奇异值矩阵;
在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定旋转外参的初值。
A2.根据权利要求1所述的方法,在根据所述第四大奇异值对应的特征向量确定旋转外参的初值之前,所述方法还包括:
判断所述第三大奇异值是否大于预设阈值;
所述根据所述第四大奇异值对应的特征向量确定旋转外参的初值,包括:
在所述第三大奇异值大于所述预设阈值的情况下,根据所述第四大奇异值对应的特征向量确定所述旋转外参的初始值。
A3.根据权利要求A1所述的方法,在根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组之前,所述方法还包括:
判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值;
若是,删除小于所述旋转增量阈值的第一旋转增量和第二旋转增量。
A4.根据权利要求A1所述的方法,所述方法还包括:
计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值;
在所述旋转增量差值小于差值阈值的情况下,删除对应的第一旋转增量和第二旋转增量。
A5.根据权利要求A1-A4任一项所述的方法,在确定旋转外参的初值之后,所述方法还包括:
获取第一里程计传感器在第二检测时窗内生成的第三里程计数据;
根据待标定的旋转外参对所述第三里程计数据进行坐标转换,得到第四里程计数据;
基于所述第四里程计数据计算目标残差;
基于所述旋转外参的初值对所述目标残差进行最小化优化,确定旋转外参的标定值。
A6.根据权利要求A5所述的方法,所述基于所述第四里程计数据计算目标残差,包括:
基于所述第二检测时窗内第一时间戳对应的第四里程计数据确定检测特征点数据,以及基于所述第二检测时窗内第二时间戳对应的第四里程计数据确定检测特征线面数据,其中所述第二时间戳早于所述第一时间戳;
根据所述检测特征点和所述检测特征线面计算距离残差;
基于距离残差确定所述目标残差。
A7.根据权利要求A6所述的方法,所述方法还包括:
获取第二里程计传感器在所述第二检测时窗内生成的第五里程计数据;
根据所述第四里程计数据和所述第五里程计数据计算边缘化先验残差;
所述基于距离残差确定所述目标残差,包括:基于所述边缘化先验残差和所述距离残差确定所述目标残差。
A8.根据权利要求A7所述的方法,所述方法还包括:
根据所述第五里程计数据确定实际积分增量和预积分增量;
根据所述实际积分增量和所述预积分增量计算预积分残差;
所述基于所述边缘化先验残差和所述距离残差确定所述目标残差,包括:
基于所述边缘化先验残差、所述距离残差和所述预积分残差确定所述目标残差。
A9.根据权利要求A8所述的方法,在基于所述旋转外参的初值对所述目标残差进行最小化优化之前,所述方法还包括:
计算所述距离残差和所述预积分残差的和值;
判断所述边缘化先验残差与所述和值的比值是否小于第二比值阈值;
若是,执行基于所述旋转外参的初值对所述目标残差进行最小化优化。
A10.根据权利要求A5所述的方法,在基于所述旋转外参的初值对所述目标残差进行最小化优化之前,所述方法还包括:
判断承载刚体是否处在目标退化运动状态,所述承载刚体为承载所述第一里程计传感器和所述第二里程计传感器的刚体;
若否,基于所述旋转外参的初值对所述目标残差进行最小化优化。
A11.一种外参标定装置,用于实现第一里程计传感器和第二里程计传感器之间的外参标定;所述装置包括:
第一数据获取单元,用于获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在所述第一检测时窗内生成的第二里程计数据;
旋转增量计算单元,用于基于所述第一里程计数据计算多个第一旋转增量,以及基于所述第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
求解计算单元,用于基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于所述超定方程组进行奇异值分解得到奇异值矩阵;
第一判断单元,用于判断所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值是否大于第一比值阈值;
外参初值确定单元,用于在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定旋转外参的初值。
A12.根据权利要求A11所述的装置,
所述第一判断单元还用于判断所述第三大奇异值是否大于预设阈值;
所述外参初值确定单元在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值,并且所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定所述旋转外参的初始值。
A13.根据权利要求A11所述的装置,还包括:
第二判断单元,用于判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值;
删除单元,用于删除小于所述旋转增量阈值的第一旋转增量和第二旋转增量。
A14.根据权利要求A11所述的装置,还包括:
差值计算单元,用于计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值;
所述删除单元还用于在所述旋转增量差值小于差值阈值的情况下,删除对应的第一旋转增量和第二旋转增量。
A15.根据权利要求A11-A14任一项所述的装置,所述第二里程计传感器为惯性测量单元或者差分定位传感器;在确定旋转外参的初值之后,所述装置还包括:
第二数据获取单元,用于获取第一里程计传感器在第二检测时窗内生成的第三里程计数据;
里程计数据转换单元,用于根据待标定的旋转外参对所述第三里程计数据进行坐标转换,得到第四里程计数据;
目标残差确定单元,用于基于所述第四里程计数据计算目标残差;
外参标定值确定单元,用于基于所述旋转外参的初值对所述目标残差进行最小化优化,确定旋转外参的标定值。
A16.根据权利要求A15所述的装置,所述目标残差确定单元包括:
特征确定子单元,用于基于所述第二检测时窗内第一时间戳对应的第四里程计数据确定检测特征点数据,以及基于所述第二检测时窗内第二时间戳对应的第四里程计数据确定检测特征线面数据,其中所述第二时间戳早于所述第一时间戳;
距离残差计算子单元,用于根据所述检测特征点和所述检测特征线面计算距离残差;
目标残差确定子单元,用于基于距离残差确定所述目标残差。
A17.根据权利要求A16所述的装置,
所述第二数据获取单元还用于获取第二里程计传感器在所述第二检测时窗内生成的第五里程计数据;
所述目标残差确定单元还包括:先验残差确定子单元,用于根据所述第四里程计数据和所述第五里程计数据计算边缘化先验残差;
所述目标残差确定子单元基于所述边缘化先验残差和所述距离残差确定所述目标残差。
A18.根据权利要求A16所述的装置,所述目标残差确定单元还包括:
积分增量计算子单元,用于根据所述第五里程计数据确定实际积分增量和预积分增量;
预积分残差计算子单元,用于根据所述实际积分增量和所述预积分增量计算预积分残差;
所述目标残差确定子单元基于所述边缘化先验残差、所述距离残差和所述预积分残差确定所述目标残差。
A19.根据权利要求A18所述的装置,还包括:
和值计算单元,用于计算所述距离残差和所述预积分残差的和值;
比值判断单元,用于判断所述边缘化先验残差与所述和值的比值是否小于第二比值阈值;
在所述比值判断单元判定所述边缘化先验残差与所述和值的比值小于第二比值阈值的情况下,所述外参标定值确定单元基于所述旋转外参的初值对所述目标残差进行最小化优化。
A20.根据权利要求A15所述的装置,还包括:
退化状态判定单元,用于判断承载刚体是否处在目标退化运动状态,所述承载刚体为承载所述第一里程计传感器和所述第二里程计传感器的刚体;
在所述退化状态判定单元判定承载刚体未处在目标退化运动状态的情况下,所述外参标定值确定单元基于所述旋转外参的初值对所述目标残差进行最小化优化。
本公开实施例还提供一种计算设备,该计算设备包括处理器和存储器,其中,存储器中存储有计算机程序,当计算机程序被处理器执行时可以实现上述任一实施例的外参标定方法。
图7是本公开实施例提供的计算设备的结构示意图。下面具体参考图7,其示出了适于用来实现本公开实施例中的计算设备700的结构示意图。图7示出的计算设备仅仅是一个示例,不应对本公开实施例的功能和使用范围带来任何限制。
如图7所示,计算设备700可以包括处理装置701(例如中央处理器、图形处理器等),其可以根据存储在只读存储器ROM702中的程序或者从存储装置708加载到随机访问存储器RAM703中的程序而执行各种适当的动作和处理。在RAM 703中,还存储有计算设备700操作所需的各种程序和数据。处理装置701、ROM 702以及RAM 703通过总线704彼此相连。输入/输出I/O接口705也连接至总线704。
通常,以下装置可以连接至I/O接口705:包括例如触摸屏、触摸板、摄像头、麦克风、加速度计、陀螺仪等的输入装置706;包括例如液晶显示器(LCD)、扬声器、振动器等的输出装置707;包括例如磁带、硬盘等的存储装置708;以及通信装置709。通信装置709可以允许计算设备700与其他设备进行无线或有线通信以交换数据。虽然图7示出了具有各种装置的计算设备700,但是应理解的是,并不要求实施或具备所有示出的装置。可以替代地实施或具备更多或更少的装置。
特别地,根据本公开的实施例,上文参考流程图描述的过程可以被实现为计算机软件程序。例如,本公开的实施例包括一种计算机程序产品,其包括承载在非暂态计算机可读介质上的计算机程序,该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中,该计算机程序可以通过通信装置709从网络上被下载和安装,或者从存储装置708被安装,或者从ROM 702被安装。在该计算机程序被处理装置701执行时,执行本公开实施例的方法中限定的上述功能。
需要说明的是,本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开中,计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于:电线、光缆、RF(射频)等等,或者上述的任意合适的组合。
在一些实施方式中,客户端、服务器可以利用诸如HTTP(HyperText TransferProtocol,超文本传输协议)之类的任何当前已知或未来研发的网络协议进行通信,并且可以与任意形式或介质的数字数据通信(例如,通信网络)互连。通信网络的示例包括局域网(“LAN”),广域网(“WAN”),网际网(例如,互联网)以及端对端网络(例如,ad hoc端对端网络),以及任何当前已知或未来研发的网络。
上述计算机可读介质可以是上述计算设备中所包含的;也可以是单独存在,而未装配入该计算设备中。
上述计算机可读介质承载有一个或者多个程序,当上述一个或者多个程序被该计算设备执行时,使得该计算设备:获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在第一检测时窗内生成的第二里程计数据;基于第一里程计数据计算多个第一旋转增量,以及基于第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于超定方程组进行奇异值分解得到奇异值矩阵;在奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据第四大奇异值对应的特征向量确定旋转外参的初值。
可以以一种或多种程序设计语言或其组合来编写用于执行本公开的操作的计算机程序代码,上述程序设计语言包括但不限于面向对象的程序设计语言—诸如Java、Smalltalk、C++,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中,远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机,或者,可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
附图中的流程图和框图,图示了按照本公开各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分,该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在有些作为替换的实现中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个接连地表示的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用执行规定的功能或操作的专用的根据硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施例中所涉及到的单元可以通过软件的方式实现,也可以通过硬件的方式来实现。其中,单元的名称在某种情况下并不构成对该单元本身的限定。
本文中以上描述的功能可以至少部分地由一个或多个硬件逻辑部件来执行。例如,非限制性地,可以使用的示范类型的硬件逻辑部件包括:现场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、片上系统(SOC)、复杂可编程逻辑设备(CPLD)等等。
在本公开的上下文中,机器可读介质可以是有形的介质,其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备,或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括根据一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
本公开实施例还提供一种计算机可读存储介质,存储介质中存储有计算机程序,当计算机程序被处理器执行时可以实现上述任一方法实施例的方法,其执行方式和有益效果类似,在这里不再赘述。
此外,第五方面,本公开实施例提供一种车辆,包括车载控制芯片和多个交互显示屏,所述车载控制芯片用于执行如前所述的文本输入方法,控制所述多个交互显示屏中的至少两个独立地显示文本输入界面。前述的车载控制芯片可以为车辆中的中控芯片,也可以是独立于中控芯片的娱乐系统控制芯片,也可以是其他芯片,本公开实施例并不做特别地限定;较为优选的,前述的车载控制芯片是车辆智能座舱系统中专门用于控制各个交互显示屏工作的控制芯片。
需要说明的是,在本文中,诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅是本公开的具体实施方式,使本领域技术人员能够理解或实现本公开。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的,本文中所定义的一般原理可以在不脱离本公开的精神或范围的情况下,在其它实施例中实现。因此,本公开将不会被限制于本文所述的这些实施例,而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
Claims (10)
1.一种外参标定方法,用于实现第一里程计传感器和第二里程计传感器之间的外参标定,其特征在于,所述方法包括:
获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在所述第一检测时窗内生成的第二里程计数据;
基于所述第一里程计数据计算多个第一旋转增量,以及基于所述第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于所述超定方程组进行奇异值分解得到奇异值矩阵;
在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定旋转外参的初值。
2.根据权利要求1所述的方法,其特征在于,在根据所述第四大奇异值对应的特征向量确定旋转外参的初值之前,所述方法还包括:
判断所述第三大奇异值是否大于预设阈值;
所述根据所述第四大奇异值对应的特征向量确定旋转外参的初值,包括:
在所述第三大奇异值大于所述预设阈值的情况下,根据所述第四大奇异值对应的特征向量确定所述旋转外参的初始值。
3.根据权利要求1所述的方法,其特征在于,在根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组之前,所述方法还包括:
判断第一旋转增量和第二旋转增量中的至少一个是否小于旋转增量阈值;
若是,删除小于所述旋转增量阈值的第一旋转增量和第二旋转增量。
4.根据权利要求1所述的方法,其特征在于,所述方法还包括:
计算时间戳相同的第一旋转增量和第二旋转增量的旋转增量差值;
在所述旋转增量差值小于差值阈值的情况下,删除对应的第一旋转增量和第二旋转增量。
5.根据权利要求1-4任一项所述的方法,其特征在于,在确定旋转外参的初值之后,所述方法还包括:
获取第一里程计传感器在第二检测时窗内生成的第三里程计数据;
根据待标定的旋转外参对所述第三里程计数据进行坐标转换,得到第四里程计数据;
基于所述第四里程计数据计算目标残差;
基于所述旋转外参的初值对所述目标残差进行最小化优化,确定旋转外参的标定值。
6.根据权利要求5所述的方法,其特征在于,在基于所述旋转外参的初值对所述目标残差进行最小化优化之前,所述方法还包括:
判断承载刚体是否处在目标退化运动状态,所述承载刚体为承载所述第一里程计传感器和所述第二里程计传感器的刚体;
若否,基于所述旋转外参的初值对所述目标残差进行最小化优化。
7.一种外参标定装置,用于实现第一里程计传感器和第二里程计传感器之间的外参标定;其特征在于,所述装置包括:
第一数据获取单元,用于获取第一里程计传感器在第一检测时窗内生成的第一里程计数据,以及第二里程计传感器在所述第一检测时窗内生成的第二里程计数据;
旋转增量计算单元,用于基于所述第一里程计数据计算多个第一旋转增量,以及基于所述第二里程计数据计算时间戳与各个第一旋转增量相同的第二旋转增量;
求解计算单元,用于基于手眼标定模型,根据时间戳相同的第一旋转增量和第二旋转增量构建超定方程组,并基于所述超定方程组进行奇异值分解得到奇异值矩阵;
第一判断单元,用于判断所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值是否大于第一比值阈值;
外参初值确定单元,用于在所述奇异值矩阵中的第三大奇异值与第四大奇异值的比值大于第一比值阈值的情况下,根据所述第四大奇异值对应的特征向量确定旋转外参的初值。
8.一种计算设备,其特征在于,包括处理器和存储器,所述存储器用于存储计算机程序;
所述计算机程序在被所述处理器加载时,使所述处理器执行如权利要求1-6任一项所述的外参标定方法。
9.一种计算机可读存储介质,其特征在于,所述存储介质存储有计算机程序,当计算机程序被处理器执行时,使得处理器实现如权利要求1-6任一项所述的外参标定方法。
10.一种车辆,其特征在于,包括车载控制芯片、第一里程计传感器和第二里程计传感器,所述车载控制芯片用于执行如权利要求1-6任一项所述的外参标定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211112283.XA CN115655305A (zh) | 2022-09-13 | 2022-09-13 | 外参标定方法、装置、计算设备、存储介质和车辆 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211112283.XA CN115655305A (zh) | 2022-09-13 | 2022-09-13 | 外参标定方法、装置、计算设备、存储介质和车辆 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115655305A true CN115655305A (zh) | 2023-01-31 |
Family
ID=84984144
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211112283.XA Pending CN115655305A (zh) | 2022-09-13 | 2022-09-13 | 外参标定方法、装置、计算设备、存储介质和车辆 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115655305A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116774195A (zh) * | 2023-08-22 | 2023-09-19 | 国网天津市电力公司滨海供电分公司 | 多传感器联合标定的激励判断与参数自调节方法及系统 |
-
2022
- 2022-09-13 CN CN202211112283.XA patent/CN115655305A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116774195A (zh) * | 2023-08-22 | 2023-09-19 | 国网天津市电力公司滨海供电分公司 | 多传感器联合标定的激励判断与参数自调节方法及系统 |
CN116774195B (zh) * | 2023-08-22 | 2023-12-08 | 国网天津市电力公司滨海供电分公司 | 多传感器联合标定的激励判断与参数自调节方法及系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111415387B (zh) | 相机位姿确定方法、装置、电子设备及存储介质 | |
CN111127563A (zh) | 联合标定方法、装置、电子设备及存储介质 | |
US10605925B2 (en) | Processing GPS drifting | |
CN114018274B (zh) | 车辆定位方法、装置及电子设备 | |
CN111060138B (zh) | 标定方法及装置、处理器、电子设备、存储介质 | |
CN110988849A (zh) | 雷达系统的标定方法、装置、电子设备及存储介质 | |
CN112285676A (zh) | 激光雷达与imu外参标定方法及装置 | |
CN113551665B (zh) | 一种用于运动载体的高动态运动状态感知系统及感知方法 | |
CN114111776B (zh) | 定位方法及相关装置 | |
CN112561990B (zh) | 定位信息生成方法、装置、设备和计算机可读介质 | |
CN116182878B (zh) | 道路曲面信息生成方法、装置、设备和计算机可读介质 | |
CN115655305A (zh) | 外参标定方法、装置、计算设备、存储介质和车辆 | |
CN115457152A (zh) | 外参标定方法、装置、电子设备及存储介质 | |
CN114442133A (zh) | 无人机定位方法、装置、设备及存储介质 | |
CN117392241A (zh) | 自动驾驶中的传感器标定方法、装置及电子设备 | |
CN117191080A (zh) | 相机与imu外参的标定方法、装置、设备及存储介质 | |
CN111998870B (zh) | 一种相机惯导系统的标定方法和装置 | |
WO2023185215A1 (zh) | 数据校准 | |
CN114140538B (zh) | 车载相机位姿调整方法、装置、设备和计算机可读介质 | |
CN114399587B (zh) | 三维车道线生成方法、装置、电子设备和计算机可读介质 | |
CN115727871A (zh) | 一种轨迹质量检测方法、装置、电子设备和存储介质 | |
CN114199220A (zh) | 一种无人机空中在线磁罗盘校准方法和装置 | |
CN111307176B (zh) | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 | |
CN114791610A (zh) | 一种转换参数的确定方法和装置 | |
CN113759384A (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 |