车载双目相机与GNSS/IMU的自动标定方法

技术领域

本发明涉及自动驾驶技术领域，尤其涉及一种车载双目相机与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和双目相机所在车辆沿第一方向行驶时，获取双目相机的双目第一图像；

选取双目第一图像中的目标像素点，并确定该目标像素点的像素灰度值；

以该目标像素点为圆心，选取预设半径所形成的圆上的像素点；

若圆上有连续N个像素点的灰度值大于第一预设阈值，或者小于第二预设阈值，则该目标像素点被确认为特征点，其中N为大于1的自然数；

遍历双目第一图像中的所有目标像素点，以筛选获得特征点；

对筛选出的所有特征点进行进行非极大值抑制，以获得关联特征点；

对所述关联特征点进行三角化，以获得特征点深度信息。

进一步地，根据特征点深度信息构建第一方向稀疏特征地图，包括：

根据特征点深度信息与相机内参矩阵确定相机坐标系下特征点三维坐标；

根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下，以获得特征点对应的地图点坐标。

进一步地，根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下，以获得特征点对应的地图点坐标，包括：

根据所述GNSS/IMU的第一测量值以及所述GNSS/IMU到双目相机的初始转换矩阵将所述相机坐标系下特征点三维坐标转换至世界坐标系下，以获得特征点对应的地图点坐标，其中，转换公式为：

其中，

进一步地，根据所述第一方向稀疏特征地图获取双目相机全局位姿，包括：

当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到双目相机的初始转换矩阵，其中所述优化函数的公式为：

在上述初始化完成之后，进入到下文所述的标定参数优化阶段。

S200、当GNSS/IMU和双目相机所在车辆沿第一方向行驶时，获取GNSS/IMU的第一测量值，并根据所述GNSS/IMU的第一测量值和所述初始转换矩阵确定双目相机位姿；

在本发明实施例中，首先控制车辆沿第一方向行驶，获取车辆在该方向行驶时GNSS/IMU的第一测量值以及双目相机的双目第一图像。

根据前文获取到的GNSS/IMU到双目相机的初始转换矩阵并结合当前的所述GNSS/IMU的第一测量值即可计算得双目相机位姿。

S300、根据所述双目相机位姿构建第一方向稀疏特征地图；

在本发明实施例中，如图3所示，为了能够进行标定结果的优化，根据所述双目相机位姿构建第一方向稀疏特征地图，包括：

S310、根据三角化技术获得特征点深度信息；

应当理解的是，根据三角化技术恢复特征点深度信息，进而构建稀疏特征地图。

具体地，如图4所示，根据三角化技术获得特征点深度信息，包括：

S311、当GNSS/IMU和双目相机所在车辆沿第一方向行驶时，获取双目相机的双目第一图像；

S312、选取双目第一图像中的目标像素点，并确定该目标像素点的像素灰度值；

在所述双目第一图像中选取一目标像素点

S313、以该目标像素点为圆心，选取预设半径所形成的圆上的像素点；

具体地，以所述目标像素点

S314、若圆上有连续N个像素点的灰度值大于第一预设阈值，或者小于第二预设阈值，则该目标像素点被确认为特征点，其中N为大于1的自然数；

具体地，此处所述第一预设阈值具体可以为Ip + T，第二预设阈值具体可以为Ip+ T，T具体可以为0.2Ip。当该圆上有连续N个像素点的灰度值满足该阈值要求时，则该圆的圆心，即目标像素点被确认为特征点。

S315、遍历双目第一图像中的所有目标像素点，以筛选获得特征点；

S316、对筛选出的所有特征点进行进行非极大值抑制，以获得关联特征点；

需要说明的是，对筛选出的所有特征点进行非极大值抑制（NMS算法）的目的是搜索局部最大值，以保留互异性最强的代表性角点，即获得关联特征点。

首先依据下述公式计算特征点圆周上像素点

其中，I

完成双目第一图像的特征提取后，进行双目特征关联。特征点使用 BRIEF 算法进行表征，该算法通过在特征点p的周围以固定模式采样 128 对点对，把这128对点对的比较结果用 0、1 表示形成 128 维描述子。在进行特征点关联时，通过下述公式的汉明距离比较不同特征点的描述子距离以完成特征关联。

其中，

S317、对所述关联特征点进行三角化，以获得特征点深度信息。

在本发明实施例中，利用下式对关联特征点进行三角化，以恢复特征点深度信息，

其中，

S320、根据特征点深度信息构建第一方向稀疏特征地图。

具体地，如图5所示，根据特征点深度信息构建第一方向稀疏特征地图，包括：

S321、根据特征点深度信息与相机内参矩阵确定相机坐标系下特征点三维坐标；

基于特征点深度信息

S322、根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下，以获得特征点对应的地图点坐标。

进一步具体地，根据所述双目相机位姿将所述相机坐标系下特征点三维坐标转换至世界坐标系下，以获得特征点对应的地图点坐标，包括：

根据所述GNSS/IMU的第一测量值以及所述GNSS/IMU到双目相机的初始转换矩阵将所述相机坐标系下特征点三维坐标转换至世界坐标系下，以获得特征点对应的地图点坐标，其中，转换公式为：

其中，

S400、当GNSS/IMU和双目相机所在车辆沿第二方向行驶时，获取GNSS/IMU的第二测量值，并根据所述第一方向稀疏特征地图获取双目相机全局位姿，其中所述第二方向与所述第一方向互为反方向；

如图6所示，根据所述第一方向稀疏特征地图获取双目相机全局位姿，包括：

S410、当GNSS/IMU和双目相机所在车辆沿第二方向行驶时，获取双目相机的双目第二图像；

S420、提取双目第二图像中的特征点，并计算双目第二图像的特征点对应的描述子；

S430、根据汉明距离进行所述第一方向稀疏特征地图的地图点与所述描述子的3D-2D关联；

S440、根据所述3D-2D关联的结果找到所述第一方向稀疏特征地图和所述双目第二图像的特征点关联数最多的地图帧；

S450、根据所述地图帧以及重定位算法对当前双目相机进行重定位，确定双目相机全局位姿。

在该第二方向行驶过程中，利用上文所述方法提取当前双目图像特征点并计算其对应描述子，利用汉明距离进行地图点与当前帧特征的3D-2D关联，并找到地图中和当前帧特征关联数最多的地图帧。随后基于PNP算法，对相机进行重定位，获取相机全局位姿后引入绝对运动约束：

其中，

S500、根据所述双目相机全局位姿和所述第二测量值构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛，获得标定转换矩阵。

具体地，如图7所示，根据所述双目相机全局位姿和所述第二实时传感器数据构建运动约束以优化所述初始转换矩阵直至所述初始转换矩阵收敛，获得标定转换矩阵，包括：

S510、根据所述双目相机全局位姿构建运动约束，所述运动约束包括绝对运动约束和相对运动约束；

S520、根据所述运动约束对所述初始化转换矩阵进行优化，以获得标定转换矩阵。

进一步具体地，根据所述运动约束对所述初始化转换矩阵进行优化，包括：

1）根据所述运动约束对所述初始化转换矩阵进行优化，获得优化标定结果；

2）判断所述优化标定结果是否收敛；

3）若所述优化标定结果不收敛，则根据所述优化标定结果重复构建第一方向稀疏特征地图、重复根据所述第一方向稀疏特征地图获取双目相机全局位姿以及重复获得优化标定结果的步骤；

4）若所述优化标定结果收敛，则将所述优化标定结果确定为标定转换矩阵。

需要说明的是，在对优化标定结果进行优化时，需要用到优化函数。优化函数由相对运动约束平移误差与绝对运动约束平移误差组成，相对运动平移误差项为：

其中，

绝对运动约束平移误差项为：

其中，

总优化函数为：

利用列文伯格-马夸尔特优化上述总优化函数，利用插值因子为0.5的四元数插值公式（前文中提到的四元数插值公式）对第k轮计算结果

综上，本发明提供的车载双目相机与GNSS/IMU的自动标定方法，通过确定GNSS/IMU到双目相机的初始转换矩阵，进而在第一方向行驶时确定第一方向稀疏特征地图，在第二方向行驶时通过优化收敛确定标定转换矩阵。这种车载双目相机与GNSS/IMU的自动标定方法无需利用标志物，避免了手动布置、测量标志导致的额外人工费用。同时，不同于现有基于自然特征的标定方法，本发明方法对车辆运动形式无严苛要求。另外，本发明实施例由于采用互为反方向的行驶结合的方式能够有效消除误差，提升标定精度，且由于标定优化环节基于已有地图估算相机位姿，因此标定精度不受视觉里程计漂移影响。

下面对本发明实施例提供的车载双目相机与GNSS/IMU的自动标定方法的具体标定过程进行详细描述。

由GNSS/IMU的第一测量值以及初始变换矩阵构建的第一方向稀疏特征地图能够有效避免后续里程计的漂移程度，而地图构建阶段与参数优化阶段车辆相反的行驶方向以及插值因子为0.5参数更新则保证了标定参数朝正确的方向收敛。

具体实现过程可以结合图8所示：

在第k轮迭代时，用第一方向稀疏特征地图计算的第i帧相机的位姿表示如式：

其中，

利用绝对位姿约束

其中，

为进一步分析标定误差，将

其中，平移部分分量可表示为：

其中，

利用插值因子为0.5的线性插值对第k轮计算结果

由调整后的

另外，在本发明实施例中，使用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给出了标定参数在仿真环境下的迭代过程，其中横坐标表示迭代轮数，纵坐标从上倒下依次表示θ

表1 仿真环境下标定精度结果

可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。