CN113223064A - 一种视觉惯性里程计尺度的估计方法和装置 - Google Patents

Abstract

本发明实施例公开了一种视觉惯性里程计尺度的估计方法和装置，该方法包括：当识别到当前感知图像中存在虚线段端点时，提取该虚线段端点，并将该虚线段端点与包含有虚线段端点的历史感知图像进行匹配，得到相匹配的目标虚线段端点；确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标；根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度；根据所述当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。通过采用上述技术方案，在GPS信号受到干扰或影响时，可准确估计出VIO的尺度。

Description

一种视觉惯性里程计尺度的估计方法和装置

技术领域

本发明涉及自动驾驶技术领域，具体涉及一种视觉惯性里程计尺度的估计方法和装置。

背景技术

基于相机与IMU融合的VIO(visual-inertialodometry，视觉惯性里程计)定位系统是目前非常流行的定位框架，IMU(Inertial measurement unit，惯性测量单元)保证了短时间内的轨迹演算精度，但是误差随时间累积很快；相机通过观察与追踪周围环境中的特征点，约束自身的位姿与速度方向，同时使得尺度的发散相比单靠IMU演算更慢。相机与IMU互补，构成良好的定位系统。

但是，当载体的运动缺乏激励，特别是对车载系统而言，大部分时候汽车主要做近似的匀速直线运动，尺度对VIO系统是不可观的，这时尺度会持续地发散，导致定位精度下降，甚至引起系统崩溃。通过引入GPS(Global Positioning System，全球定位系统)可以很好地解决这个问题，GPS提供绝对的位置与速度测量，使得绝对位置与尺度的误差始终不发散，可以解决大部分室外场景。但是当GPS信号受遮挡或干扰时，特别是长隧道场景，系统又恢复为普通的VIO系统，尺度发散很明显，导致定位精度不满足建图的要求。在硬件受限，不能获得汽车轮速，以及安装双目相机的情况下，需要从别的地方获得缺失的尺度信息。

发明内容

本发明实施例公开一种视觉惯性里程计尺度的估计方法和装置，在GPS信号受到干扰或影响时，可准确估计出VIO的尺度。

第一方面，本发明实施例公开了一种视觉惯性里程计尺度的估计方法，该方法包括：

当识别到当前感知图像中存在虚线段端点时，提取所述虚线段端点，并将所述虚线段端点与包含有虚线段端点的历史感知图像进行匹配，得到相匹配的目标虚线段端点；

确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标；

根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度；

根据所述当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标，包括：

根据相机拍摄当前图像对应的当前位姿，以及拍摄历史图像对应的历史位姿，确定所述目标虚线段端点在相机坐标系下，与所述当前位姿对应的当前第二坐标，以及与所述历史位姿对应的历史第二坐标；

将所述当前第二坐标和所述历史第二坐标均转换到世界坐标系中，得到对应的当前第一坐标和历史第一坐标。

可选的，根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度，包括：

按照如下公式，确定相机的当前对地高度：

其中，

表示目标虚线段端点在世界坐标系下，与历史感知图像对应的历史第一坐标；

表示目标虚线段端点在世界坐标系下，与当前感知图像对应的当前第一坐标；{R₁,t₁}表示相机拍摄历史图像对应的历史位姿，{R₂,t₂}表示相机拍摄当前图像对应的当前位姿；h₁表示相机拍摄历史图像时的历史对地高度，h₂表示相机拍摄当前图像时的当前对地高度；n表示是方向朝上的地面法向量；d₁表示与历史位姿对应的目标虚线段端点的观测方向；d₂表示与当前位姿对应的目标虚线段端点的观测方向。

可选的，在确定相机的当前对地高度之后，所述方法还包括：

对于每个包含有虚线段端点的历史感知图像，从该历史感知图像中提取与当前感知图像相匹配的目标虚线段端点，以得到与每个目标虚线段端点相对应的当前对地高度；

将多个当前对地高度通过最小二乘法进行估计，得到相机对应的第一对地高度；

如果所述第一对地高度在预设高度范围内，则将该第一对地高度确定为相机的预估对地高度；

相应的，根据所述预估对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，所述方法还包括：

将遍历所有包含虚线段端点的历史感知图像得到的多个预估对地高度，通过最小二乘法进行估计，得到相机对应的第二对地高度；

如果所述第二对地高度在预设高度范围内，则将该第二对地高度确定为相机的目标对地高度；

相应的，根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，当识别到当前感知图像中不存在虚线段端点时，确定当前车辆前方设定范围内的路面区域，并从所述路面区域中提取梯度值大于设定阈值的目标路面点；

根据所述目标路面点在当前感知图像的光度值，以及在历史感知图像中光度值之间的相等关系，通过调整相机的对地高度，使得以相机的对地高度为参数的光度误差函数的值达到最小，并将函数值最小时所对应的相机对地高度作为相机的当前对地高度；

其中，所述历史感知图像为与当前感知图像相邻的前一帧感知图像。

可选的，所述目标路面点在历史感知图像中的光度值通过如下方式来确定：

确定所述目标路面点在当前感知图像中的当前像素坐标；

根据相机拍摄当前图像对应的当前位姿，以及拍摄与当前图像相邻的前一帧图像对应的历史位姿，确定所述目标路面点在当前位姿对应的相机坐标系下，与其在历史位姿对应的相机坐标系下的坐标转换关系，所述坐标转换关系以相机的对地高度作为参数；

根据所述坐标转换关系和当前像素坐标，确定所述目标路面点在所述历史感知图像中的历史像素坐标，并根据所述历史像素坐标确定对应的光度值。

可选的，所述方法还包括：

基于高斯-牛顿法对所述当前对地高度进行迭代优化，在设定迭代次数内，如果迭代得到的第二对地高度在预设高度范围内，则将得到的第二对地高作为相机的目标对地高度；

相应的，根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

第二方面，本发明实施例还提供了一种视觉惯性里程计尺度的估计装置，该装置包括：

目标虚线段端点确定模块，被配置为当识别到当前感知图像中存在虚线段端点时，提取所述虚线段端点，并将所述虚线段端点与包含有虚线段端点的历史感知图像进行匹配，得到相匹配的目标虚线段端点；

坐标确定模块，被配置为确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标；

当前对地高度确定模块，被配置为根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度；

尺度估计模块，被配置为根据所述当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，所述坐标确定模块，具体被配置为：

根据相机拍摄当前图像对应的当前位姿，以及拍摄历史图像对应的历史位姿，确定所述目标虚线段端点在相机坐标系下，与所述当前位姿对应的当前第二坐标，以及与所述历史位姿对应的历史第二坐标；

将所述当前第二坐标和所述历史第二坐标均转换到世界坐标系中，得到对应的当前第一坐标和历史第一坐标。

可选的，所述当前对地高度确定模块，具体被配置为：

按照如下公式，确定相机的当前对地高度：

其中，

表示目标虚线段端点在世界坐标系下，与历史感知图像对应的历史第一坐标；

表示目标虚线段端点在世界坐标系下，与当前感知图像对应的当前第一坐标；{R₁,t₁}表示相机拍摄历史图像对应的历史位姿，{R₂,t₂}表示相机拍摄当前图像对应的当前位姿；h₁表示相机拍摄历史图像时的历史对地高度，h₂表示相机拍摄当前图像时的当前对地高度；n表示是方向朝上的地面法向量；d₁表示与历史位姿对应的目标虚线段端点的观测方向；d₂表示与当前位姿对应的目标虚线段端点的观测方向。

可选的，所述装置还包括：

第一对地高度确定模块，被配置为在确定相机的当前对地高度之后，对于每个包含有虚线段端点的历史感知图像，从该历史感知图像中提取与当前感知图像相匹配的目标虚线段端点，以得到与每个目标虚线段端点相对应的当前对地高度；将多个当前对地高度通过最小二乘法进行估计，得到相机对应的第一对地高度；

预估对地高度确定模块，被配置为如果所述第一对地高度在预设高度范围内，则将该第一对地高度确定为相机的预估对地高度；

相应的，所述尺度估计模块具体被配置为：根据所述预估对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，所述装置还包括：

第二对地高度确定模块，被配置为将遍历所有包含虚线段端点的历史感知图像得到的多个预估对地高度，通过最小二乘法进行估计，得到相机对应的第二对地高度；

目标对地高度确定模块，被配置为如果所述第二对地高度在预设高度范围内，则将该第二对地高度确定为相机的目标对地高度；

相应的，所述尺度估计模块具体被配置为：根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，所述装置还包括：

目标路面点提取模块，被配置为当识别到当前感知图像中不存在虚线段端点时，确定当前车辆前方设定范围内的路面区域，并从所述路面区域中提取梯度值大于设定阈值的目标路面点；

当前对地高度估计模块，被配置为根据所述目标路面点在当前感知图像的光度值，以及在历史感知图像中光度值之间的相等关系，通过调整相机的对地高度，使得以相机的对地高度为参数的光度误差函数的值达到最小，并将函数值最小时所对应的相机对地高度作为相机的当前对地高度；

其中，所述历史感知图像为与当前感知图像相邻的前一帧感知图像。

可选的，所述目标路面点在历史感知图像中的光度值通过如下方式来确定：

确定所述目标路面点在当前感知图像中的当前像素坐标；

根据相机拍摄当前图像对应的当前位姿，以及拍摄与当前图像相邻的前一帧图像对应的历史位姿，确定所述目标路面点在当前位姿对应的相机坐标系下，与其在历史位姿对应的相机坐标系下的坐标转换关系，所述坐标转换关系以相机的对地高度作为参数；

根据所述坐标转换关系和当前像素坐标，确定所述目标路面点在所述历史感知图像中的历史像素坐标，并根据所述历史像素坐标确定对应的光度值。

可选的，所述装置还包括：

迭代优化模块，被配置为基于高斯-牛顿法对所述当前对地高度进行迭代优化，在设定迭代次数内，如果迭代得到的第二对地高度在预设高度范围内，则将得到的第二对地高作为相机的目标对地高度；

相应的，所述尺度估计模块，具体被配置为根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

第三方面，本发明实施例还提供了一种车载终端，包括：

存储有可执行程序代码的存储器；

与所述存储器耦合的处理器；

所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明任意实施例所提供的视觉惯性里程计尺度的估计方法的部分或全部步骤。

第四方面，本发明实施例还提供了一种计算机可读存储介质，其存储计算机程序，所述计算机程序包括用于执行本发明任意实施例所提供的视觉惯性里程计尺度的估计方法的部分或全部步骤的指令。

第五方面，本发明实施例还提供了一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行本发明任意实施例所提供的视觉惯性里程计尺度的估计方法的部分或全部步骤。

本发明实施例提供的技术方案，在识别到感知图像中存在虚线段端点时，可将不同感知图像中相匹配的目标虚线段端点提取出来，并可根据不同感知图像中的同一虚线段端点在世界坐标系中的坐标相同，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，通过确定相机的当前对地高度来估计VIO的当前尺度。这种方法计算量较小，在不增加硬件的情况下，依然可以实时估计尺度，保持了单目VIO系统的便宜与计算量低的优势，解决了在GPS信号受到干扰或影响时，VIO尺度发散的问题。

本发明的发明点包括：

1、通过从不同感知图像中提取相匹配的目标虚线段端点，可根据不同感知图像中的同一虚线段端点在世界坐标系中的坐标相同，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，来确定相机的当前对地高度，从而可利用当前对地高度估计VIO的当前尺度，解决了在GPS信号受到干扰或影响时，VIO尺度发散的问题，是本发明的发明点之一。

2、在道路中不存在虚线段端点的应用场景下，通过充分利用地面上的如箭头、斑马线等标识信息，提取更多的梯度存在变化的路面点，以对光度误差函数进行优化，可使得估计出的VIO精度更加准确，解决了在GPS信号受到干扰或影响时，VIO尺度发散的问题，是本发明的发明点之一。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种视觉惯性里程计尺度的估计方法的流程示意图；

图2是本发明实施例提供的一种视觉惯性里程计尺度的估计方法的流程示意图；

图3是本发明实施例提供的一种视觉惯性里程计尺度的估计装置的结构框图；

图4是本发明实施例提供的一种车载终端的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例一

请参阅图1，图1是本发明实施例提供的一种视觉惯性里程计尺度的估计方法的流程示意图。该方法典型的是应用于自动驾驶过程中GPS信号受到干扰或影响的场景下，例如长隧道场景下。该方法可由视觉惯性里程计尺度的估计装置来执行，该装置可通过软件和/或硬件的方式实现，一般可集成在车载电脑、车载工业控制计算机(Industrial personalComputer，IPC)等车载终端中，本发明实施例不做限定。如图1所示，本实施例提供的方法具体包括：

110、当识别到当前感知图像中存在虚线段端点时，提取该虚线段端点，并将该虚线段端点与包含有虚线段端点的历史感知图像进行匹配，得到相匹配的目标虚线段端点。

其中，感知图像是利用预设感知模型对相机采集的包含道路信息的图像进行识别后得到的。预设感知模型可以预先采用大量标注有图像语义特征的道路样本图像对感知模型进行训练。其中，图像语义特征可包括交通牌、路灯杆、车道线、车道线虚线端点等。通过将包含有道路信息的道路图像输入至训练好的预设感知模型，基于预设感知模型的识别结果，即可得到道路图像中的图像语义特征。其中，预设感知模型可以通过以下方式得到：

构建训练样本集，该训练样本集包括多组训练样本数据，每组训练样本数据包括道路样本图像和对应的标注有图像语义特征的道路感知样本图像；基于训练样本集对搭建的初始神经网络进行训练得到预设感知模型，该预设感知模型使得每组训练样本数据中的道路样本图像与对应的标注有图像语义特征的道路感知样本图像相关联。模型输出的即可称之为感知图像。对于每一帧感知图像，可将其保存到MSCKF(Multi-State ConstraintKalman Filter，多状态约束卡尔曼滤波)框架下的历史帧中。当下一帧图像产生时，已保存的感知图像均可作为历史感知图像。

本实施例中，在识别到当前感知图像中存在虚线段端点时，可提取出该虚线段端点，并将该虚线段端点与历史感知图像中的虚线段端点进行匹配。其中，历史感知图像是采集当前图像之前的任意一帧感知图像。将当前感知图像与历史感知图像中的虚线段端点进行匹配方法有多种，例如，对于从当前感知图像中提取的任意一个虚线段端点，可将历史感知图像中与该虚线段端点的距离小于预设阈值的虚线段端点作为相匹配的目标虚线段端点。

120、确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标。

示例性的，可根据相机拍摄当前图像对应的当前位姿，以及拍摄历史图像对应的历史位姿，确定目标虚线段端点在相机坐标系下，与当前位姿对应的当前第二坐标，以及与历史位姿对应的历史第二坐标；将当前第二坐标和历史第二坐标均转换到世界坐标系中，得到对应的当前第一坐标和历史第一坐标。

具体的，在跟踪某个目标虚线段端点时，d表示目标虚线端点的观测方向；地面方程为：n^TX+h＝0，ρ表示目标虚线段端点的深度；n表示是方向朝上的地面法向量；h表示是相机对地高度，据此，可根据如下地面约束方程和相机观测方程：

得到该目标虚线段端点在相机坐标系下的坐标X：

在本实施例中，目标虚线段端点在相机坐标系下，与相机当前位姿对应的当前第二坐标为

以及与相机历史位姿对应的历史第二坐标为

其中，d₁表示历史位姿对应的目标虚线段端点的观测方向；d₂表示与当前位姿对应的目标虚线段端点的观测方向。

设相机拍摄历史图像对应的历史位姿为{R₁,t₁}；相机拍摄当前图像对应的当前位姿为{R₂,t₂}，据此，可将与相机当前位姿对应的，目标虚线段端点在相机坐标系中的当前第二坐标

转换到世界坐标系下，得到当前第一坐标：

也可将与相机历史位姿对应的，目标虚线段端点在相机坐标系中的历史第二坐标

转换到世界坐标系下，得到历史第一坐标：

其中，h₁表示相机拍摄历史图像时的历史对地高度，h₂表示相机拍摄当前图像时的当前对地高度，即本实施例中所要确定的相机对地高度。

130、根据当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度。

由于不同图像中的同一个虚线段端点在世界坐标系中的坐标相等，因此，可建立历史第一坐标与当前第一坐标之间的相等关系。相机在拍摄当前感知图像和历史感知图像时的高度增量可通过相机的旋转角R、位移t和地面法向量n来表示。具体的，可建立如下约束方程：

通过上述约束方程，可得到相机的当前对地高度h₂。

140、根据当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

其中，可将高度先验真值作为相机对地的参考高度，将该参考对地高度与上述步骤得到的当前对地高度做商，可得到VIO的估计尺度。

进一步的，对于每个包含有虚线段端点的历史感知图像，通过从该历史感知图像中提取与当前感知图像中的虚线段端点相匹配的目标虚线段端点，并通过执行上述步骤后，可以得到与多个目标虚线段端点相对应的多个当前对地高度。通过将这多个当前对地高度采用最小二乘法进行估计，可得到相机对应的第一对地高度；如果该第一对地高度在预设高度范围内，则可将该第一对地高度确定为相机的预估对地高度；相应的，根据预估对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。这样设置提高了VIO估计尺度的准确性。

进一步的，遍历所有保存的包含有虚线段端点的历史感知图像，对于所有历史感知图像各自对应的预估对地高度，再次采用最小二乘法进行估计，可得到相机对应的第二对地高度。如果第二对地高度在预设高度范围内，则将该第二对地高度确定为相机的目标对地高度。相应的，根据目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。这样设置，可进一步提升VIO估计尺度的准确性。

本实施例提供的技术方案，在识别到感知图像中存在虚线段端点时，可将不同感知图像中相匹配的目标虚线段端点提取出来，并可根据不同感知图像中的同一虚线段端点在世界坐标系中的坐标相同这一特点，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，通过确定相机的当前对地高度，来估计VIO的当前尺度。这种方法计算量较小，在不增加硬件的情况下，依然可以实时估计尺度，保持了单目VIO系统的便宜与计算量低的优势，解决了在GPS信号受到干扰或影响时，VIO尺度发散的问题。

实施例二

请参阅图2，图2是本发明实施例提供的一种视觉惯性里程计尺度的估计方法的流程示意图。本实施例在上述实施例的基础上进行了优化，主要应用于不存路面上不存在虚线段端点的路面场景下。如图2所示，该方法包括：

210、当识别到当前感知图像中不存在虚线段端点时，确定当前车辆前方设定范围内的路面区域，并从路面区域中提取梯度值大于设定阈值的目标路面点。

示例性的，在提取目标路面点时，可将路面区域划分网格，并从每个网格中提取梯度值大于设定阈值的特征点，从而保证整个路面区域中能够提取到足够多、分布均匀且梯度变化的路面特征点。具体的，在实际应用场景中，这些特征点可以为道路线、箭头、斑马线、建筑物的阴影与树荫或路面的污痕等。

220、根据目标路面点在当前感知图像的光度值，以及在历史感知图像中光度值之间的相等关系，通过调整相机的对地高度，使得以相机的对地高度为参数的光度误差函数的值达到最小，并将函数值最小时所对应的相机对地高度作为相机的当前对地高度。

可以理解的是，对于同一个像素，其在不同图片中的光度值是一样的。因此，目标路面点在当前感知图像的光度值，以及在历史感知图像中光度值之间存在相等关系。本实施例中所涉及到的历史感知图像为与当前感知图像相邻的前一帧感知图像。

示例性的，目标路面点在感知图像中光度值可通过目标路面点在该图像中的像素坐标来确定。对于当前感知图像，可提取出目标路面点在其中的像素坐标。对于历史感知图像，可根据相机拍摄当前图像对应的当前位姿，以及拍摄与当前图像相邻的前一帧图像对应的历史位姿，确定目标路面点在当前位姿对应的相机坐标系下，与其在历史位姿对应的相机坐标系下的坐标转换关系；根据该坐标转换关系和当前像素坐标，可确定出目标路面点在历史感知图像中的历史像素坐标，从而可根据历史像素坐标确定对应的光度值。

具体的，设目标路面点在相机当前位姿对应的相机坐标系下的坐标为P，在相机历史位姿对应的历史坐标系下的坐标为(RP+t)，其中，{R,t}是由MSCKF前端提供的相机在拍摄相邻两帧图像之间的相对旋转与位移。设目标路面点在相机拍摄前一帧图像时在相机坐标系下的方向向量为d₁，在相机拍摄当前图像时在相机坐标系下的方向向量为d₂，则根据相机在拍摄相邻两帧图像之间的相对旋转和位移，可得到目标路面点在当前位姿对应的相机坐标系下，与其在历史位姿对应的相机坐标系下的坐标转换关系为：

通过该坐标转换关系以及目标路面点在当前感知图像中的当前像素坐标x，可得到目标路面点在历史感知图像中的历史像素坐标W(x,n,h)，据此，可得到以相机的对地高度为参数的光度误差函数：

通过调整相机的对地高度h和地面法向量n，可使光度误差函数的值达到最小，此时，可将函数值最小时对应的相机对地高度可作为本实施例所要确定的相机的当前对地高度。

进一步的，在得到相机的当前对地高度后，可基于高斯-牛顿法对当前对地高度进行迭代优化，在设定迭代次数内，如果迭代得到的第二对地高度在预设高度范围内，则将得到的第二对地高作为相机的目标对地高度，这样设置提升了相机对地高度的准确性。相应的，可根据目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

230、根据当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

本实施例在上述实施例的基础上，在道路中不存在虚线段端点的应用场景下，充分利用了地面上的如箭头、斑马线等标识信息，可大大提高场景覆盖范围。通过提取更多梯度存在变化的路面点对光度误差函数进行优化，可使得估计出的VIO精度更加准确，解决了在GPS信号受到干扰或影响时，VIO尺度发散的问题。

实施例三

请参阅图3，图3是本发明实施例提供的一种视觉惯性里程计尺度的估计装置的结构框图。如图3所示，该装置具体包括：目标虚线段端点确定模块310、坐标确定模块320、当前对地高度确定模块330和尺度估计模块340；其中，

目标虚线段端点确定模块310，被配置为当识别到当前感知图像中存在虚线段端点时，提取所述虚线段端点，并将所述虚线段端点与包含有虚线段端点的历史感知图像进行匹配，得到相匹配的目标虚线段端点；

坐标确定模块320，被配置为确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标；

当前对地高度确定模块330，被配置为根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度；

尺度估计模块340，被配置为根据所述当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，所述坐标确定模块，具体被配置为：

根据相机拍摄当前图像对应的当前位姿，以及拍摄历史图像对应的历史位姿，确定所述目标虚线段端点在相机坐标系下，与所述当前位姿对应的当前第二坐标，以及与所述历史位姿对应的历史第二坐标；

将所述当前第二坐标和所述历史第二坐标均转换到世界坐标系中，得到对应的当前第一坐标和历史第一坐标。

可选的，所述当前对地高度确定模块，具体被配置为：

按照如下公式，确定相机的当前对地高度：

其中，

表示目标虚线段端点在世界坐标系下，与历史感知图像对应的历史第一坐标；

表示目标虚线段端点在世界坐标系下，与当前感知图像对应的当前第一坐标；{R₁,t₁}表示相机拍摄历史图像对应的历史位姿，{R₂,t₂}表示相机拍摄当前图像对应的当前位姿；h₁表示相机拍摄历史图像时的历史对地高度，h₂表示相机拍摄当前图像时的当前对地高度；n表示是方向朝上的地面法向量；d₁表示与历史位姿对应的目标虚线段端点的观测方向；d₂表示与当前位姿对应的目标虚线段端点的观测方向。

可选的，所述装置还包括：

第一对地高度确定模块，被配置为在确定相机的当前对地高度之后，对于每个包含有虚线段端点的历史感知图像，从该历史感知图像中提取与当前感知图像相匹配的目标虚线段端点，以得到与每个目标虚线段端点相对应的当前对地高度；将多个当前对地高度通过最小二乘法进行估计，得到相机对应的第一对地高度；

预估对地高度确定模块，被配置为如果所述第一对地高度在预设高度范围内，则将该第一对地高度确定为相机的预估对地高度；

相应的，所述尺度估计模块具体被配置为：根据所述预估对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，所述装置还包括：

第二对地高度确定模块，被配置为将遍历所有包含虚线段端点的历史感知图像得到的多个预估对地高度，通过最小二乘法进行估计，得到相机对应的第二对地高度；

目标对地高度确定模块，被配置为如果所述第二对地高度在预设高度范围内，则将该第二对地高度确定为相机的目标对地高度；

相应的，所述尺度估计模块具体被配置为：根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

可选的，所述装置还包括：

目标路面点提取模块，被配置为当识别到当前感知图像中不存在虚线段端点时，确定当前车辆前方设定范围内的路面区域，并从所述路面区域中提取梯度值大于设定阈值的目标路面点；

当前对地高度估计模块，被配置为根据所述目标路面点在当前感知图像的光度值，以及在历史感知图像中光度值之间的相等关系，通过调整相机的对地高度，使得以相机的对地高度为参数的光度误差函数的值达到最小，并将函数值最小时所对应的相机对地高度作为相机的当前对地高度；

其中，所述历史感知图像为与当前感知图像相邻的前一帧感知图像。

可选的，所述目标路面点在历史感知图像中的光度值通过如下方式来确定：

确定所述目标路面点在当前感知图像中的当前像素坐标；

根据相机拍摄当前图像对应的当前位姿，以及拍摄与当前图像相邻的前一帧图像对应的历史位姿，确定所述目标路面点在当前位姿对应的相机坐标系下，与其在历史位姿对应的相机坐标系下的坐标转换关系，所述坐标转换关系以相机的对地高度作为参数；

根据所述坐标转换关系和当前像素坐标，确定所述目标路面点在所述历史感知图像中的历史像素坐标，并根据所述历史像素坐标确定对应的光度值。

可选的，所述装置还包括：

迭代优化模块，被配置为基于高斯-牛顿法对所述当前对地高度进行迭代优化，在设定迭代次数内，如果迭代得到的第二对地高度在预设高度范围内，则将得到的第二对地高作为相机的目标对地高度；

相应的，所述尺度估计模块，具体被配置为根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

本发明实施例所提供的视觉惯性里程计尺度的估计装置可执行本发明任意实施例所提供的视觉惯性里程计尺度的估计方法，具备执行方法相应的功能模块和有益效果。未在上述实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的视觉惯性里程计尺度的估计方法。

实施例四

请参阅图4，图4是本发明实施例提供的一种车载终端的结构示意图。如图4所示，该车载终端可以包括：

存储有可执行程序代码的存储器701；

与存储器701耦合的处理器702；

其中，处理器702调用存储器701中存储的可执行程序代码，执行本发明任意实施例所提供的视觉惯性里程计尺度的估计方法。

本发明实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行本发明任意实施例所提供的视觉惯性里程计尺度的估计方法。

本发明实施例公开一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行本发明任意实施例所提供的视觉惯性里程计尺度的估计方法的部分或全部步骤。

在本发明的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

在本发明所提供的实施例中，应理解，“与A相应的B”表示B与A相关联，根据A可以确定B。但还应理解，根据A确定B并不意味着仅仅根据A确定B，还可以根据A和/或其他信息确定B。

另外，在本发明各实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分，可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。

以上对本发明实施例公开的一种视觉惯性里程计尺度的估计方法和装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种视觉惯性里程计尺度的估计方法，应用于自动驾驶，其特征在于，包括：

当识别到当前感知图像中存在虚线段端点时，提取所述虚线段端点，并将所述虚线段端点与包含有虚线段端点的历史感知图像进行匹配，得到相匹配的目标虚线段端点；

确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标；

根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度；

根据所述当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

2.根据权利要求1所述的方法，其特征在于，确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标，包括：

根据相机拍摄当前图像对应的当前位姿，以及拍摄历史图像对应的历史位姿，确定所述目标虚线段端点在相机坐标系下，与所述当前位姿对应的当前第二坐标，以及与所述历史位姿对应的历史第二坐标；

将所述当前第二坐标和所述历史第二坐标均转换到世界坐标系中，得到对应的当前第一坐标和历史第一坐标。

3.根据权利要求1所述的方法，其特征在于，根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度，包括：

按照如下公式，确定相机的当前对地高度：

其中，

表示目标虚线段端点在世界坐标系下，与历史感知图像对应的历史第一坐标；

表示目标虚线段端点在世界坐标系下，与当前感知图像对应的当前第一坐标；{R₁,t₁}表示相机拍摄历史图像对应的历史位姿，{R₂,t₂}表示相机拍摄当前图像对应的当前位姿；h₁表示相机拍摄历史图像时的历史对地高度，h₂表示相机拍摄当前图像时的当前对地高度；n表示是方向朝上的地面法向量；d₁表示与历史位姿对应的目标虚线段端点的观测方向；d₂表示与当前位姿对应的目标虚线段端点的观测方向。

4.根据权利要求1所述的方法，其特征在于，在确定相机的当前对地高度之后，所述方法还包括：

对于每个包含有虚线段端点的历史感知图像，从该历史感知图像中提取与当前感知图像相匹配的目标虚线段端点，以得到与每个目标虚线段端点相对应的当前对地高度；

将多个当前对地高度通过最小二乘法进行估计，得到相机对应的第一对地高度；

如果所述第一对地高度在预设高度范围内，则将该第一对地高度确定为相机的预估对地高度；

相应的，根据所述预估对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

5.根据权利要求4所述的方法，其特征在于，所述方法还包括：

将遍历所有包含虚线段端点的历史感知图像得到的多个预估对地高度，通过最小二乘法进行估计，得到相机对应的第二对地高度；

如果所述第二对地高度在预设高度范围内，则将该第二对地高度确定为相机的目标对地高度；

相应的，根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

6.根据权利要求1所述的方法，其特征在于，所述方法还包括：

当识别到当前感知图像中不存在虚线段端点时，确定当前车辆前方设定范围内的路面区域，并从所述路面区域中提取梯度值大于设定阈值的目标路面点；

根据所述目标路面点在当前感知图像的光度值，以及在历史感知图像中光度值之间的相等关系，通过调整相机的对地高度，使得以相机的对地高度为参数的光度误差函数的值达到最小，并将函数值最小时所对应的相机对地高度作为相机的当前对地高度；

其中，所述历史感知图像为与当前感知图像相邻的前一帧感知图像。

7.根据权利要求6所述的方法，其特征在于，所述目标路面点在历史感知图像中的光度值通过如下方式来确定：

确定所述目标路面点在当前感知图像中的当前像素坐标；

根据相机拍摄当前图像对应的当前位姿，以及拍摄与当前图像相邻的前一帧图像对应的历史位姿，确定所述目标路面点在当前位姿对应的相机坐标系下，与其在历史位姿对应的相机坐标系下的坐标转换关系，所述坐标转换关系以相机的对地高度作为参数；

根据所述坐标转换关系和当前像素坐标，确定所述目标路面点在所述历史感知图像中的历史像素坐标，并根据所述历史像素坐标确定对应的光度值。

8.根据权利要求7所述的方法，其特征在于，所述方法还包括：

基于高斯-牛顿法对所述当前对地高度进行迭代优化，在设定迭代次数内，如果迭代得到的第二对地高度在预设高度范围内，则将得到的第二对地高作为相机的目标对地高度；

相应的，根据所述目标对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

9.一种视觉惯性里程计尺度的估计装置，应用于自动驾驶，其特征在于，包括：

目标虚线段端点确定模块，被配置为当识别到当前感知图像中存在虚线段端点时，提取所述虚线段端点，并将所述虚线段端点与包含有虚线段端点的历史感知图像进行匹配，得到相匹配的目标虚线段端点；

坐标确定模块，被配置为确定目标虚线段端点在世界坐标系下，与当前感知图像和历史感知图像分别对应的当前第一坐标和历史第一坐标；

当前对地高度确定模块，被配置为根据所述当前第一坐标与历史第一坐标之间的相等关系，以及相机在拍摄当前感知图像和历史感知图像时的高度增量，确定相机的当前对地高度；

尺度估计模块，被配置为根据所述当前对地高度和相机对地的参考高度，确定视觉惯性里程计VIO的估计尺度。

10.根据权利要求9所述的装置，其特征在于，所述装置还包括：

目标路面点提取模块，被配置为当识别到当前感知图像中不存在虚线段端点时，确定当前车辆前方设定范围内的路面区域，并从所述路面区域中提取梯度值大于设定阈值的目标路面点；

当前对地高度估计模块，被配置为根据所述目标路面点在当前感知图像的光度值，以及在历史感知图像中光度值之间的相等关系，通过调整相机的对地高度，使得以相机的对地高度为参数的光度误差函数的值达到最小，并将函数值最小时所对应的相机对地高度作为相机的当前对地高度；

其中，所述历史感知图像为与当前感知图像相邻的前一帧感知图像。

Images