CN116664698B - 车载双目相机与gnss/imu的自动标定方法 - Google Patents
车载双目相机与gnss/imu的自动标定方法 Download PDFInfo
- Publication number
- CN116664698B CN116664698B CN202310916924.5A CN202310916924A CN116664698B CN 116664698 B CN116664698 B CN 116664698B CN 202310916924 A CN202310916924 A CN 202310916924A CN 116664698 B CN116664698 B CN 116664698B
- Authority
- CN
- China
- Prior art keywords
- gnss
- imu
- binocular camera
- binocular
- initial
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 239000011159 matrix material Substances 0.000 claims abstract description 91
- 238000006243 chemical reaction Methods 0.000 claims abstract description 66
- 238000005259 measurement Methods 0.000 claims abstract description 34
- 238000005457 optimization Methods 0.000 claims description 26
- 230000009466 transformation Effects 0.000 claims description 22
- 238000013519 translation Methods 0.000 claims description 12
- 238000005516 engineering process Methods 0.000 claims description 8
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 230000001629 suppression Effects 0.000 claims description 4
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 239000003550 marker Substances 0.000 abstract description 3
- 230000008569 process Effects 0.000 description 7
- 238000004088 simulation Methods 0.000 description 7
- 238000012360 testing method Methods 0.000 description 4
- 230000000007 visual effect Effects 0.000 description 4
- 230000004044 response Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000012804 iterative process Methods 0.000 description 2
- 238000013139 quantization Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 241001270131 Agaricus moelleri Species 0.000 description 1
- 101100391182 Dictyostelium discoideum forI gene Proteins 0.000 description 1
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
- G06T7/85—Stereo camera calibration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T19/00—Manipulating 3D models or images for computer graphics
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10004—Still image; Photographic image
- G06T2207/10012—Stereo images
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Software Systems (AREA)
- Computer Graphics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Geometry (AREA)
- Remote Sensing (AREA)
- Multimedia (AREA)
- Computer Hardware Design (AREA)
- General Engineering & Computer Science (AREA)
- Navigation (AREA)
Abstract
本发明涉及自动驾驶技术领域,具体公开了一种车载双目相机与GNSS/IMU的自动标定方法,包括:确定GNSS/IMU到双目相机的初始转换矩阵;当GNSS/IMU和双目相机所在车辆沿第一方向行驶时,获取GNSS/IMU的第一测量值,并根据GNSS/IMU的第一测量值和初始转换矩阵确定双目相机位姿;根据双目相机位姿构建第一方向稀疏特征地图;当GNSS/IMU和双目相机所在车辆沿第二方向行驶时,获取GNSS/IMU的第二测量值,并根据第一方向稀疏特征地图获取双目相机全局位姿,其中第二方向与第一方向互为反方向;根据双目相机全局位姿和第二测量值构建运动约束以优化初始转换矩阵直至初始转换矩阵收敛,获得标定转换矩阵。本发明提供的车载双目相机与GNSS/IMU的自动标定方法无需标志物且标定精度高。
Description
技术领域
本发明涉及自动驾驶技术领域,尤其涉及一种车载双目相机与GNSS/IMU的自动标定方法。
背景技术
近年来,融合双目相机与GNSS(Global Navigation Satellite System,全球导航卫星系统)/IMU(INERTIAL MEASUREMEENT UNIT,惯性测量单元)的定位技术已成为自动驾驶技术中的研究热点,其可以克服GNSS在信号遮挡地区定位漂移、IMU的累计误差以及相机在特征稀疏场景定位失效等单一传感器的弊端,从而构建一个轻量、低成本且鲁棒的定位系统。双目相机与GNSS/IMU之间的精确标定矩阵可以将不同传感器数据转换到统一坐标系,是实现融合定位的前提条件。
目前的常用标定方案分为:基于标定板等先验标志物的标定方法和基于自然特征的标定方法。基于标定板等先验标志物的标定方法需搭建专用标定场地,人工铺设特定标志物,并测量标志物在场景中的位置信息。经过图像识别标志物获取相机位姿,以构建包含相机与GNSS/IMU转换矩阵的约束方程,进行外参求解。该方法常需布置标定板,引入了额外的人工成本,且标定过程也依赖专业人士操作,降低了标定的自动化程度。基于自然特征的标定方法通过提取自然特征、特征关联、建立重投影约束方程等视觉定位技术获取相机位姿,并结合GNSS/IMU观测值进行外参标定,但基于自然特征的标定结果常因受到视觉定位技术累计漂移影响导致标定精度不足。此外,该类方法常需GNSS/IMU与双目视觉相机进行全方向运动,以便对六自由度的标定转换矩阵产生良好约束,然而现实场景中的车辆往往在二维平面上行驶,难以提供符合要求的运动模式,这进一步限制了该标定方法在车辆上面的应用。
因此,如何提供一种无需标志物且精度高的标定方法成为本领域技术人员亟待解决的技术问题。
发明内容
本发明提供了一种车载双目相机与GNSS/IMU的自动标定方法,解决相关技术中存在的标定需要标志物且标定精度低的问题。
作为本发明的一个方面,提供一种车载双目相机与GNSS/IMU的自动标定方法,其中,包括:
确定GNSS/IMU到双目相机的初始转换矩阵;
当GNSS/IMU和双目相机所在车辆沿第一方向行驶时,获取GNSS/IMU的第一测量值,并根据所述GNSS/IMU的第一测量值和所述初始转换矩阵确定双目相机位姿;
根据所述双目相机位姿构建第一方向稀疏特征地图;
当GNSS/IMU和双目相机所在车辆沿第二方向行驶时,获取GNSS/IMU的第二测量值,并根据所述第一方向稀疏特征地图获取双目相机全局位姿,其中所述第二方向与所述第一方向互为反方向;
根据所述双目相机全局位姿和所述第二测量值构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛,获得标定转换矩阵。
进一步地,确定GNSS/IMU到双目相机的初始转换矩阵,包括:
当GNSS/IMU和双目相机所在车辆行驶时,分别获取双目相机的双目初始图像和GNSS/IMU的初始测量值;
确定每帧双目初始图像的位姿,以获得双目初始图像位姿数据;
对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量值进行时间同步,并构建相对运动约束以确定GNSS/IMU到双目相机的初始转换矩阵。
进一步地,对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量值进行时间同步,并构建相对运动约束以确定GNSS/IMU到双目相机的初始转换矩阵,包括:
根据四元数插值对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量进行时间同步,其中,所述四元数插值的公式为:
,
其中,表示旋转的单位四元数;/>表示根据所述双目初始图像位姿数据时间戳/>进行时间同步后的旋转四元数,由/>、/>两时刻的GNSS/IMU的初始测量值/>、/>插值获得; />表示插值因子,且;
满足
,GNSS/IMU的初始测量值的平移部分根据普通线性插值进行同步;
根据相对运动约束求解所述GNSS/IMU到双目相机的初始转换矩阵,其中,相对运动约束的公式为:
,
其中,表示待求解的所述GNSS/IMU到双目相机的初始转换矩阵;/>表示到/>时刻GNSS/IMU的相对运动变换;/>表示/>到/>时刻相机的相对运动变换;
根据列文伯格-马夸尔特非线性求解方法对优化函数进行求解,获得所述GNSS/IMU到双目相机的初始转换矩阵,其中所述优化函数的公式为:
。
进一步地,根据所述双目相机位姿构建第一方向稀疏特征地图,包括:
根据三角化技术获得特征点深度信息;
根据特征点深度信息构建第一方向稀疏特征地图。
进一步地,根据三角化技术获得特征点深度信息,包括:
当GNSS/IMU和双目相机所在车辆沿第一方向行驶时,获取双目相机的双目第一图像;
选取双目第一图像中的目标像素点,并确定该目标像素点的像素灰度值;
以该目标像素点为圆心,选取预设半径所形成的圆上的像素点;
若圆上有连续N个像素点的灰度值大于第一预设阈值,或者小于第二预设阈值,则该目标像素点被确认为特征点,其中N为大于1的自然数;
遍历双目第一图像中的所有目标像素点,以筛选获得特征点;
对筛选出的所有特征点进行进行非极大值抑制,以获得关联特征点;
对所述关联特征点进行三角化,以获得特征点深度信息。
进一步地,根据特征点深度信息构建第一方向稀疏特征地图,包括:
根据特征点深度信息与相机内参矩阵确定相机坐标系下特征点三维坐标;
根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标。
进一步地,根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标,包括:
根据所述GNSS/IMU的第一测量值以及所述GNSS/IMU到双目相机的初始转换矩阵将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标,其中,转换公式为:
,
其中,表示特征点对应的地图点坐标,/>表示所述GNSS/IMU的第一测量值,/>表示所述GNSS/IMU到双目相机的初始转换矩阵的逆,P表示特征点三维坐标。
进一步地,根据所述第一方向稀疏特征地图获取双目相机全局位姿,包括:
当GNSS/IMU和双目相机所在车辆沿第二方向行驶时,获取双目相机的双目第二图像;
提取双目第二图像中的特征点,并计算双目第二图像的特征点对应的描述子;
根据汉明距离进行所述第一方向稀疏特征地图的地图点与所述描述子的3D-2D关联;
根据所述3D-2D关联的结果找到所述第一方向稀疏特征地图和所述双目第二图像的特征点关联数最多的地图帧;
根据所述地图帧以及重定位算法对当前双目相机进行重定位,确定双目相机全局位姿。
进一步地,根据所述双目相机全局位姿和所述第二实时传感器数据构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛,获得标定转换矩阵,包括:
根据所述双目相机全局位姿构建运动约束,所述运动约束包括绝对运动约束和相对运动约束;
根据所述运动约束对所述初始化转换矩阵进行优化,以获得标定转换矩阵。
进一步地,根据所述运动约束对所述初始化转换矩阵进行优化,包括:
根据所述运动约束对所述初始化转换矩阵进行优化,获得优化标定结果;
判断所述优化标定结果是否收敛;
若所述优化标定结果不收敛,则根据所述优化标定结果重复构建第一方向稀疏特征地图、重复根据所述第一方向稀疏特征地图获取双目相机全局位姿以及重复获得优化标定结果的步骤;
若所述优化标定结果收敛,则将所述优化标定结果确定为标定转换矩阵。
本发明提供的车载双目相机与GNSS/IMU的自动标定方法,通过确定GNSS/IMU到双目相机的初始转换矩阵,进而在第一方向行驶时确定第一方向稀疏特征地图,在第二方向行驶时通过优化收敛确定标定转换矩阵。这种车载双目相机与GNSS/IMU的自动标定方法无需利用标志物,避免了手动布置、测量标志导致的额外人工费用。同时,不同于现有基于自然特征的标定方法,本发明方法对车辆运动形式无严苛要求。另外,本发明实施例由于采用互为反方向的行驶结合的方式能够有效消除误差,提升标定精度,且由于标定优化环节基于已有地图估算相机位姿,因此标定精度不受视觉里程计漂移影响。
附图说明
附图是用来提供对本发明的进一步理解,并且构成说明书的一部分,与下面的具体实施方式一起用于解释本发明,但并不构成对本发明的限制。
图1为本发明提供的车载双目相机与GNSS/IMU的自动标定方法的流程图。
图2为本发明提供的确定初始转换矩阵的流程图。
图3为本发明提供的构建第一方向稀疏特征地图的流程图。
图4为本发明提供的获得特征点深度信息的流程图。
图5为本发明提供的根据特征点深度信息构建第一方向稀疏特征地图的具体流程图。
图6为本发明提供的获取双目相机全局位姿的流程图。
图7为本发明提供的获得标定转换矩阵的流程图。
图8为本发明提供的车载双目相机与GNSS/IMU的自动标定方法的具体实现过程流程图。
图9为本发明提供的使用Gazebo搭建的仿真标定环境。
图10为本发明提供的标定参数在仿真环境下的迭代过程。
具体实施方式
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互结合。下面将参考附图并结合实施例来详细说明本发明。
为了使本领域技术人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
需要说明的是,本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本发明的实施例。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包括,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
在本实施例中提供了一种车载双目相机与GNSS/IMU的自动标定方法,图1是根据本发明实施例提供的车载双目相机与GNSS/IMU的自动标定方法的流程图,如图1所示,包括:
S100、确定GNSS/IMU到双目相机的初始转换矩阵;
在本发明实施例中,首先需要进行初始化,即确定初始转换矩阵,该初始转换矩阵的确定方式具体是将控制车辆行驶一端轨迹,通过采集到的双目相机的双目初始图像以及GNSS/IMU的初始测量值并借助运动约束确定GNSS/IMU到双目相机的初始转换矩阵。
具体地,如图2所示,确定GNSS/IMU到双目相机的初始转换矩阵,包括:
S110、当GNSS/IMU和双目相机所在车辆行驶时,分别获取双目相机的双目初始图像和GNSS/IMU的初始测量值;
S120、确定每帧双目初始图像的位姿,以获得双目初始图像位姿数据;
在本发明实施例中,可以通过ORB-SLAM3估计双目初始图像的每帧图像位姿。也可以使用SURF、SIFT等特征均进行图像特征提取,以获得双目初始图像位姿数据。
S130、对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量值进行时间同步,并构建相对运动约束以确定GNSS/IMU到双目相机的初始转换矩阵。
在本发明实施例中,具体地,对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量值进行时间同步,并构建相对运动约束以确定GNSS/IMU到双目相机的初始转换矩阵,包括:
根据四元数插值对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量进行时间同步,其中,所述四元数插值的公式为:
,
其中,表示旋转的单位四元数;/>表示根据所述双目初始图像位姿数据时间戳/>进行时间同步后的旋转四元数,由/>、/>两时刻的GNSS/IMU的初始测量值/>、/>插值获得; />表示插值因子,且/>;
满足
,GNSS/IMU的初始测量值的平移部分根据普通线性插值进行同步;
根据相对运动约束求解所述GNSS/IMU到双目相机的初始转换矩阵,其中,相对运动约束的公式为:
,
其中,表示待求解的所述GNSS/IMU到双目相机的初始转换矩阵;/>表示到/>时刻GNSS/IMU的相对运动变换;/>表示/>到/>时刻相机的相对运动变换;
根据列文伯格-马夸尔特非线性求解方法对优化函数进行求解,获得所述GNSS/IMU到双目相机的初始转换矩阵,其中所述优化函数的公式为:
。
在上述初始化完成之后,进入到下文所述的标定参数优化阶段。
S200、当GNSS/IMU和双目相机所在车辆沿第一方向行驶时,获取GNSS/IMU的第一测量值,并根据所述GNSS/IMU的第一测量值和所述初始转换矩阵确定双目相机位姿;
在本发明实施例中,首先控制车辆沿第一方向行驶,获取车辆在该方向行驶时GNSS/IMU的第一测量值以及双目相机的双目第一图像。
根据前文获取到的GNSS/IMU到双目相机的初始转换矩阵并结合当前的所述GNSS/IMU的第一测量值即可计算得双目相机位姿。
S300、根据所述双目相机位姿构建第一方向稀疏特征地图;
在本发明实施例中,如图3所示,为了能够进行标定结果的优化,根据所述双目相机位姿构建第一方向稀疏特征地图,包括:
S310、根据三角化技术获得特征点深度信息;
应当理解的是,根据三角化技术恢复特征点深度信息,进而构建稀疏特征地图。
具体地,如图4所示,根据三角化技术获得特征点深度信息,包括:
S311、当GNSS/IMU和双目相机所在车辆沿第一方向行驶时,获取双目相机的双目第一图像;
S312、选取双目第一图像中的目标像素点,并确定该目标像素点的像素灰度值;
在所述双目第一图像中选取一目标像素点,获取该目标像素点灰度值Ip。
S313、以该目标像素点为圆心,选取预设半径所形成的圆上的像素点;
具体地,以所述目标像素点为圆心,选取半径为的圆上的i个像素点,此处半径具体可以为3像素,像素点的个数j具体可以为16。
S314、若圆上有连续N个像素点的灰度值大于第一预设阈值,或者小于第二预设阈值,则该目标像素点被确认为特征点,其中N为大于1的自然数;
具体地,此处所述第一预设阈值具体可以为Ip + T,第二预设阈值具体可以为Ip+ T,T具体可以为0.2Ip。当该圆上有连续N个像素点的灰度值满足该阈值要求时,则该圆的圆心,即目标像素点被确认为特征点。
S315、遍历双目第一图像中的所有目标像素点,以筛选获得特征点;
S316、对筛选出的所有特征点进行进行非极大值抑制,以获得关联特征点;
需要说明的是,对筛选出的所有特征点进行非极大值抑制(NMS算法)的目的是搜索局部最大值,以保留互异性最强的代表性角点,即获得关联特征点。
首先依据下述公式计算特征点圆周上像素点与特征点/>灰度之差,从而获得特征点响应值;随后比较固定窗口内的多个特征点响应值,保留其中响应值最大的角点,
,
其中,Ipi表示像素点处的灰度值,Ip表示特征点/>处的灰度值。
完成双目第一图像的特征提取后,进行双目特征关联。特征点使用 BRIEF 算法进行表征,该算法通过在特征点p的周围以固定模式采样 128 对点对,把这128对点对的比较结果用 0、1 表示形成 128 维描述子。在进行特征点关联时,通过下述公式的汉明距离比较不同特征点的描述子距离以完成特征关联。
,
其中,表示第/>维描述子比较结果;/>为最终所计算的描述子距离;/>、/>为所比较的两特征点第/>维描述子数值。
S317、对所述关联特征点进行三角化,以获得特征点深度信息。
在本发明实施例中,利用下式对关联特征点进行三角化,以恢复特征点深度信息,
,
其中,表示特征点深度;/>示关联上的双目特征点视差;/>、/>分别表示左右特征点像素横坐标值;/>表示双目相机基线长度;相机内参矩阵表示为/>。
S320、根据特征点深度信息构建第一方向稀疏特征地图。
具体地,如图5所示,根据特征点深度信息构建第一方向稀疏特征地图,包括:
S321、根据特征点深度信息与相机内参矩阵确定相机坐标系下特征点三维坐标;
基于特征点深度信息与相机内参矩阵恢复左相机坐标系下特征点三维坐标,如下式所示:
。
S322、根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标。
进一步具体地,根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标,包括:
根据所述GNSS/IMU的第一测量值以及所述GNSS/IMU到双目相机的初始转换矩阵将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标,其中,转换公式为:
,
其中,表示特征点对应的地图点坐标,/>表示所述GNSS/IMU的第一测量值,/>表示所述GNSS/IMU到双目相机的初始转换矩阵的逆,P表示特征点三维坐标。
S400、当GNSS/IMU和双目相机所在车辆沿第二方向行驶时,获取GNSS/IMU的第二测量值,并根据所述第一方向稀疏特征地图获取双目相机全局位姿,其中所述第二方向与所述第一方向互为反方向;
如图6所示,根据所述第一方向稀疏特征地图获取双目相机全局位姿,包括:
S410、当GNSS/IMU和双目相机所在车辆沿第二方向行驶时,获取双目相机的双目第二图像;
S420、提取双目第二图像中的特征点,并计算双目第二图像的特征点对应的描述子;
S430、根据汉明距离进行所述第一方向稀疏特征地图的地图点与所述描述子的3D-2D关联;
S440、根据所述3D-2D关联的结果找到所述第一方向稀疏特征地图和所述双目第二图像的特征点关联数最多的地图帧;
S450、根据所述地图帧以及重定位算法对当前双目相机进行重定位,确定双目相机全局位姿。
在该第二方向行驶过程中,利用上文所述方法提取当前双目图像特征点并计算其对应描述子,利用汉明距离进行地图点与当前帧特征的3D-2D关联,并找到地图中和当前帧特征关联数最多的地图帧。随后基于PNP算法,对相机进行重定位,获取相机全局位姿后引入绝对运动约束:
,
其中,表示/>时刻经时间同步后的GNSS/IMU的测量值;/>为/>时刻经重定位恢复的相机位姿,/>为待优化的标定外参变换矩阵。
S500、根据所述双目相机全局位姿和所述第二测量值构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛,获得标定转换矩阵。
具体地,如图7所示,根据所述双目相机全局位姿和所述第二实时传感器数据构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛,获得标定转换矩阵,包括:
S510、根据所述双目相机全局位姿构建运动约束,所述运动约束包括绝对运动约束和相对运动约束;
S520、根据所述运动约束对所述初始化转换矩阵进行优化,以获得标定转换矩阵。
进一步具体地,根据所述运动约束对所述初始化转换矩阵进行优化,包括:
1)根据所述运动约束对所述初始化转换矩阵进行优化,获得优化标定结果;
2)判断所述优化标定结果是否收敛;
3)若所述优化标定结果不收敛,则根据所述优化标定结果重复构建第一方向稀疏特征地图、重复根据所述第一方向稀疏特征地图获取双目相机全局位姿以及重复获得优化标定结果的步骤;
4)若所述优化标定结果收敛,则将所述优化标定结果确定为标定转换矩阵。
需要说明的是,在对优化标定结果进行优化时,需要用到优化函数。优化函数由相对运动约束平移误差与绝对运动约束平移误差组成,相对运动平移误差项为:
,
其中,、/>分别表示/>的旋转分量与平移分量;/>表示/>的平移分量;/>、/>分别表示/>的旋转与平移分量。
绝对运动约束平移误差项为:
,
其中,、/>分别表示/>的平移分量与旋转分量;/>、/>分别表示的平移分量与旋转分量。
总优化函数为:
,
利用列文伯格-马夸尔特优化上述总优化函数,利用插值因子为0.5的四元数插值公式(前文中提到的四元数插值公式)对第k轮计算结果与第k-1轮优化结果进行插值,获得第k轮的迭代的最终优化结果。
综上,本发明提供的车载双目相机与GNSS/IMU的自动标定方法,通过确定GNSS/IMU到双目相机的初始转换矩阵,进而在第一方向行驶时确定第一方向稀疏特征地图,在第二方向行驶时通过优化收敛确定标定转换矩阵。这种车载双目相机与GNSS/IMU的自动标定方法无需利用标志物,避免了手动布置、测量标志导致的额外人工费用。同时,不同于现有基于自然特征的标定方法,本发明方法对车辆运动形式无严苛要求。另外,本发明实施例由于采用互为反方向的行驶结合的方式能够有效消除误差,提升标定精度,且由于标定优化环节基于已有地图估算相机位姿,因此标定精度不受视觉里程计漂移影响。
下面对本发明实施例提供的车载双目相机与GNSS/IMU的自动标定方法的具体标定过程进行详细描述。
由GNSS/IMU的第一测量值以及初始变换矩阵构建的第一方向稀疏特征地图能够有效避免后续里程计的漂移程度,而地图构建阶段与参数优化阶段车辆相反的行驶方向以及插值因子为0.5参数更新则保证了标定参数朝正确的方向收敛。
具体实现过程可以结合图8所示:
在第k轮迭代时,用第一方向稀疏特征地图计算的第i帧相机的位姿表示如式:
,
其中,表示第i帧相机在世界坐标系下的位姿;/>表示第i帧相机到地图关联帧m之间的位姿变换;/>为第k-1轮迭代的标定外参优化结果;/>为地图关联帧在世界坐标系下的位姿。
利用绝对位姿约束与上述位姿表示公式,第k轮的优化的标定参数可表示如:
,
其中,表示与地图关联帧m对应的GNSS/IMU到与第i帧相机对应的GNSS/IMU之间的位姿变换;剩余其他变量已在上文进行阐述,此处不再赘述。
为进一步分析标定误差,将分解为/>,其中/>表示标定参数真值,/>为标定误差,则第k轮的优化的标定参数公式可进一步表示为:
,
其中,平移部分分量可表示为:
,
其中,表示原始变换矩阵的平移分量;/>表示原始变换矩阵的旋转分量。地图构建阶段与参数优化阶段车辆相反的行驶方向使得/>。
利用插值因子为0.5的线性插值对第k轮计算结果与第k-1轮优化结果进行插值,则调整后的/>表示为:
,
由调整后的的公式可知,第k轮计算结果/>并不受前一轮误差标定误差影响,有效避免了累计漂移对标定精度的影响。假设地图帧/>与当前帧间的距离为1米,初始标定误差中旋转部分误差/>为1°(deg),则由该公式可知,第一轮参数优化即可将标定参数平移部分误差降低至0.0087m,同时,标定参数旋转部分也被进一步优化以满足绝对运动约束式与相对运动约束式。
另外,在本发明实施例中,使用Gazebo搭建了如图9所示的仿真标定环境。在车辆上安装了可以直接输出畸变矫正图像的双目相机以及GNSS/IMU传感器,数据按照10HZ的频率进行采集。为模拟真实环境中的随机因素影响,对GNSS/IMU测量值添加了高斯噪声。相机至GNSS/IMU的变换矩阵平移部分沿x、y、z轴的真实值分别为1.2m、1.2m、0m;旋转部分其偏航角(yaw)、俯仰角(pitch)、横滚角(roll)都为30°(deg)。
车辆行驶了三条不同的模拟测试(simulated test,ST)以对本发明实施例的标定方法性能进行评估,图10给出了标定参数在仿真环境下的迭代过程,其中横坐标表示迭代轮数,纵坐标从上倒下依次表示θx、θy、θz、tx和ty,其中θx、θy和θz的单位均为角度,tx和ty单位均为米。由图10可以看出,和证明中的预期一致,首轮迭代即可使标定误差快速下降,不同模拟测试的结果最终都使得标定参数收敛至真值。表1给出了本发明实施例标定精度量化结果,基于平均误差(Mean)、标准偏差(standard deviations,SD)和平均平方根误差(root mean squared error, RMSE)进行性能评估。不同模拟测试序列上,旋转和平移部分各量化指标误差都在数量级,表明本发明实施例的标定方法可以为不同形式的车辆运动提供高精度可重复的校准结果。
表1 仿真环境下标定精度结果
可以理解的是,以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式,然而本发明并不局限于此。对于本领域内的普通技术人员而言,在不脱离本发明的精神和实质的情况下,可以做出各种变型和改进,这些变型和改进也视为本发明的保护范围。
Claims (8)
1.一种车载双目相机与GNSS/IMU的自动标定方法,其特征在于,包括:
确定GNSS/IMU到双目相机的初始转换矩阵;
当GNSS/IMU和双目相机所在车辆沿第一方向行驶时,获取GNSS/IMU的第一测量值,并根据所述GNSS/IMU的第一测量值和所述初始转换矩阵确定双目相机位姿;
根据所述双目相机位姿构建第一方向稀疏特征地图;
当GNSS/IMU和双目相机所在车辆沿第二方向行驶时,获取GNSS/IMU的第二测量值,并根据所述第一方向稀疏特征地图获取双目相机全局位姿,其中所述第二方向与所述第一方向互为反方向;
根据所述双目相机全局位姿和所述第二测量值构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛,获得标定转换矩阵;
其中,确定GNSS/IMU到双目相机的初始转换矩阵,包括:
当GNSS/IMU和双目相机所在车辆行驶时,分别获取双目相机的双目初始图像和GNSS/IMU的初始测量值;
确定每帧双目初始图像的位姿,以获得双目初始图像位姿数据;
对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量值进行时间同步,并构建相对运动约束以确定GNSS/IMU到双目相机的初始转换矩阵;
对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量值进行时间同步,并构建相对运动约束以确定GNSS/IMU到双目相机的初始转换矩阵,包括:
根据四元数插值对所述双目初始图像位姿数据和所述GNSS/IMU的初始测量进行时间同步,其中,所述四元数插值的公式为:
,
其中,表示旋转的单位四元数;/>表示根据所述双目初始图像位姿数据时间戳/>进行时间同步后的旋转四元数,由/>、/>两时刻的GNSS/IMU的初始测量值/>、/>插值获得; />表示插值因子,且/>;
满足
,GNSS/IMU的初始测量值的平移部分根据普通线性插值进行同步;
根据相对运动约束求解所述GNSS/IMU到双目相机的初始转换矩阵,其中,相对运动约束的公式为:
,
其中,表示待求解的所述GNSS/IMU到双目相机的初始转换矩阵;/>表示/>到/>时刻GNSS/IMU的相对运动变换;/>表示/>到/>时刻相机的相对运动变换;
根据列文伯格-马夸尔特非线性求解方法对优化函数进行求解,获得所述GNSS/IMU到双目相机的初始转换矩阵,其中所述优化函数的公式为:
。
2.根据权利要求1所述的车载双目相机与GNSS/IMU的自动标定方法,其特征在于,根据所述双目相机位姿构建第一方向稀疏特征地图,包括:
根据三角化技术获得特征点深度信息;
根据特征点深度信息构建第一方向稀疏特征地图。
3.根据权利要求2所述的车载双目相机与GNSS/IMU的自动标定方法,其特征在于,根据三角化技术获得特征点深度信息,包括:
当GNSS/IMU和双目相机所在车辆沿第一方向行驶时,获取双目相机的双目第一图像;
选取双目第一图像中的目标像素点,并确定该目标像素点的像素灰度值;
以该目标像素点为圆心,选取预设半径所形成的圆上的像素点;
若圆上有连续N个像素点的灰度值大于第一预设阈值,或者小于第二预设阈值,则该目标像素点被确认为特征点,其中N为大于1的自然数;
遍历双目第一图像中的所有目标像素点,以筛选获得特征点;
对筛选出的所有特征点进行进行非极大值抑制,以获得关联特征点;
对所述关联特征点进行三角化,以获得特征点深度信息。
4.根据权利要求2所述的车载双目相机与GNSS/IMU的自动标定方法,其特征在于,根据特征点深度信息构建第一方向稀疏特征地图,包括:
根据特征点深度信息与相机内参矩阵确定相机坐标系下特征点三维坐标;
根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标。
5.根据权利要求4所述的车载双目相机与GNSS/IMU的自动标定方法,其特征在于,根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标,包括:
根据所述GNSS/IMU的第一测量值以及所述GNSS/IMU到双目相机的初始转换矩阵将所述相机坐标系下特征点三维坐标转换至世界坐标系下,以获得特征点对应的地图点坐标,其中,转换公式为:
,
其中,表示特征点对应的地图点坐标,/>表示所述GNSS/IMU的第一测量值,表示所述GNSS/IMU到双目相机的初始转换矩阵的逆,P表示特征点三维坐标。
6.根据权利要求1所述的车载双目相机与GNSS/IMU的自动标定方法,其特征在于,根据所述第一方向稀疏特征地图获取双目相机全局位姿,包括:
当GNSS/IMU和双目相机所在车辆沿第二方向行驶时,获取双目相机的双目第二图像;
提取双目第二图像中的特征点,并计算双目第二图像的特征点对应的描述子;
根据汉明距离进行所述第一方向稀疏特征地图的地图点与所述描述子的3D-2D关联;
根据所述3D-2D关联的结果找到所述第一方向稀疏特征地图和所述双目第二图像的特征点关联数最多的地图帧;
根据所述地图帧以及重定位算法对当前双目相机进行重定位,确定双目相机全局位姿。
7.根据权利要求1所述的车载双目相机与GNSS/IMU的自动标定方法,其特征在于,根据所述双目相机全局位姿和所述第二实时传感器数据构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛,获得标定转换矩阵,包括:
根据所述双目相机全局位姿构建运动约束,所述运动约束包括绝对运动约束和相对运动约束;
根据所述运动约束对所述初始转换矩阵进行优化,以获得标定转换矩阵。
8.根据权利要求7所述的车载双目相机与GNSS/IMU的自动标定方法,根据所述运动约束对所述初始转换矩阵进行优化,包括:
根据所述运动约束对所述初始转换矩阵进行优化,获得优化标定结果;
判断所述优化标定结果是否收敛;
若所述优化标定结果不收敛,则根据所述优化标定结果重复构建第一方向稀疏特征地图、重复根据所述第一方向稀疏特征地图获取双目相机全局位姿以及重复获得优化标定结果的步骤;
若所述优化标定结果收敛,则将所述优化标定结果确定为标定转换矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310916924.5A CN116664698B (zh) | 2023-07-25 | 2023-07-25 | 车载双目相机与gnss/imu的自动标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310916924.5A CN116664698B (zh) | 2023-07-25 | 2023-07-25 | 车载双目相机与gnss/imu的自动标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116664698A CN116664698A (zh) | 2023-08-29 |
CN116664698B true CN116664698B (zh) | 2023-12-22 |
Family
ID=87724387
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310916924.5A Active CN116664698B (zh) | 2023-07-25 | 2023-07-25 | 车载双目相机与gnss/imu的自动标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116664698B (zh) |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107747941A (zh) * | 2017-09-29 | 2018-03-02 | 歌尔股份有限公司 | 一种双目视觉定位方法、装置及系统 |
CN112683281A (zh) * | 2021-03-11 | 2021-04-20 | 之江实验室 | 一种基于车辆运动学的自动驾驶车辆联合定位方法 |
CN115330852A (zh) * | 2022-07-26 | 2022-11-11 | 哈尔滨工程大学 | 一种水下相机-imu-深度计的空间联合标定方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10371530B2 (en) * | 2017-01-04 | 2019-08-06 | Qualcomm Incorporated | Systems and methods for using a global positioning system velocity in visual-inertial odometry |
-
2023
- 2023-07-25 CN CN202310916924.5A patent/CN116664698B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107747941A (zh) * | 2017-09-29 | 2018-03-02 | 歌尔股份有限公司 | 一种双目视觉定位方法、装置及系统 |
CN112683281A (zh) * | 2021-03-11 | 2021-04-20 | 之江实验室 | 一种基于车辆运动学的自动驾驶车辆联合定位方法 |
CN115330852A (zh) * | 2022-07-26 | 2022-11-11 | 哈尔滨工程大学 | 一种水下相机-imu-深度计的空间联合标定方法 |
Non-Patent Citations (1)
Title |
---|
无人驾驶车辆单目视觉里程计的研究进展;马芳武;史津竹;葛林鹤;代凯;仲首任;吴量;;吉林大学学报(工学版)(第03期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN116664698A (zh) | 2023-08-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
CN110033489B (zh) | 一种车辆定位准确性的评估方法、装置及设备 | |
CN109658461B (zh) | 一种基于虚拟仿真环境的合作二维码的无人机定位方法 | |
CN112598757B (zh) | 一种多传感器时间空间标定方法及装置 | |
CN103578117B (zh) | 确定摄像头相对于环境的姿态的方法 | |
CN108592950B (zh) | 一种单目相机和惯性测量单元相对安装角标定方法 | |
CN112836737A (zh) | 一种基于车路数据融合的路侧组合感知设备在线标定方法 | |
CN108665499B (zh) | 一种基于视差法的近距飞机位姿测量方法 | |
CN110675453B (zh) | 一种已知场景中运动目标的自定位方法 | |
CN111220126A (zh) | 一种基于点特征和单目相机的空间物体位姿测量方法 | |
CN109724586B (zh) | 一种融合深度图和点云的航天器相对位姿测量方法 | |
CN110363758B (zh) | 一种光学遥感卫星成像质量确定方法及系统 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN106056121A (zh) | 基于sift图像特征匹配的卫星装配工件快速识别方法 | |
CN113763479A (zh) | 一种折反射全景相机与imu传感器的标定方法 | |
CN114488094A (zh) | 一种车载多线激光雷达与imu外参数自动标定方法及装置 | |
CN113137973A (zh) | 一种图像语义特征点真值确定方法及装置 | |
KR20230003803A (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 | |
CN117310627A (zh) | 一种应用于车路协同路侧感知系统的联合标定方法 | |
CN111862146B (zh) | 一种目标对象的定位方法及装置 | |
CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
CN116664698B (zh) | 车载双目相机与gnss/imu的自动标定方法 | |
CN116563377A (zh) | 一种基于半球投影模型的火星岩石测量方法 | |
CN115775242A (zh) | 一种基于匹配的点云地图质量评估方法 | |
Hrabar et al. | PTZ camera pose estimation by tracking a 3D target |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |