视觉定位方法、设备、机器人、存储介质及程序产品

摘要

本发明实施例提供一种视觉定位方法、设备、机器人、存储介质及程序产品，该方法包括通过单目相机拍摄得到当前单目视觉图像，根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿，根据所述当前相机位姿进行定位。本发明实施例可以基于单目视觉图像快速恢复三维路面特征点，并通过将得到的三维路面特征点与高精地图匹配达到准确的定位，实现了在保证定位精度的前提下减少定位过程中的运算量。

权利要求

1.一种视觉定位方法，其特征在于，包括：

通过单目相机拍摄得到当前单目视觉图像；

根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点；

将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿；

根据所述当前相机位姿进行定位；

所述根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点之前，还包括：对所述单目相机近地平面方程进行实时自标定，得到标定后的单目相机近地平面方程；

所述根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，包括：根据所述当前单目视觉图像和所述标定后的单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

2.根据权利要求1所述的方法，其特征在于，所述根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，包括：从所述当前单目视觉图像中提取二维路面特征点；

将所述二维路面特征点，反投影到近地平面上，并结合所述单目相机近地平面方程确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

3.根据权利要求1所述的方法，其特征在于，所述将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿之前，还包括：获取所述单目相机的当前预测位姿；

所述将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿，包括：在所述当前预测位姿的指引下，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿。

4.根据权利要求3所述的方法，其特征在于，所述获取所述单目相机的当前预测位姿，包括：若所述当前预测位姿为初始位姿，则通过全球导航卫星系统获取所述单目相机的当前预测位姿；

若所述当前预测位姿为非初始位姿，则根据上一相机位姿和运动预测模型获取所述单目相机的当前预测位姿。

5.根据权利要求4所述的方法，其特征在于，所述对所述单目相机近地平面方程进行实时自标定，包括：按照预设方式获取近地平面点云；

在所述近地平面点云中加入调节系数，得到调节后的近地平面点云；

根据所述调节后的近地平面点云拟合得到调节后的近地平面方程；

根据所述当前单目视觉图像和所述调节后的近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的调节后的三维路面特征点；

获取所述高精地图中的路面特征点的先验几何信息；

对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，并根据所述最终的调节系数对所述单目相机近地平面方程进行标定。

6.根据权利要求5所述的方法，其特征在于，所述调节系数包括翻滚角调节系数、俯仰角调节系数和偏航角调节系数；所述先验几何信息包括左右车道线平行和多车道车道线等距；所述对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，包括：从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

对所述翻滚角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现左右车道线平行的特征，则将当前的翻滚角调节系数作为最终的翻滚角调节系数；

从所述当前单目视觉图像中过滤得到多车道车道线等距路段的三维路面特征点；

对所述俯仰角调节系数进行调节，直至所述多车道车道线等距路段的三维路面特征点呈现多车道车道线等距的特征，则将当前的俯仰角调节系数作为最终的俯仰角调节系数；

从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

对所述偏航角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现的左右车道线距离与实际距离一致，则将当前的偏航角调节系数作为最终的偏航角调节系数。

7.一种视觉定位设备，其特征在于，包括：

拍摄模块，用于通过单目相机拍摄得到当前单目视觉图像；

确定模块，用于根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点；

匹配模块，用于将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿；

定位模块，用于根据所述当前相机位姿进行定位；

所述设备，还包括：

自标定模块，用于对所述单目相机近地平面方程进行实时自标定，得到标定后的单目相机近地平面方程；

所述确定模块，具体用于：

根据所述当前单目视觉图像和所述标定后的单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

8.一种视觉定位设备，其特征在于，包括：单目相机、至少一个处理器和存储器；

所述单目相机，与所述处理器连接，用于拍摄得到当前单目视觉图像，并将所述当前单目视觉图像发送给所述处理器；

所述存储器存储计算机执行指令；

所述至少一个处理器执行所述存储器存储的计算机执行指令，使得所述至少一个处理器执行如权利要求1至6任一项所述的视觉定位方法。

9.一种机器人，其特征在于，包括：机器人主体和如权利要求8所述的视觉定位设备；

所述机器人主体，与所述视觉定位设备连接，用于根据所述视觉定位设备提供的定位信息在路面行驶。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机执行指令，当处理器执行所述计算机执行指令时，实现如权利要求1至6任一项所述的视觉定位方法。

11.一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至6任一项所述的视觉定位方法。

说明书

视觉定位方法、设备、机器人、存储介质及程序产品

技术领域

[0001] 本发明实施例涉及无人驾驶技术领域，尤其涉及一种视觉定位方法、设备、机器人、存储介质及程序产品。

背景技术

[0002] 定位技术是自动驾驶等机器人应用技术的基础和核心技术之一，为机器人提供位置和姿态，即位姿信息。准确的定位是机器人安全行驶的重要保障。

[0003] 在现有技术中，可以通过单目相机或双目相机进行视觉定位。

[0004] 然而，现有的单目相机定位，进行深度恢复得到三维信息时需要投入较大的计算开销和时间才能达到较高的精度，而采用双目相机恢复三维信息需要严格的外参数标定和大量的计算开销，并且随着机器人运动振动，外参数可能产生变化，需要重新进行标定校准。因此，如何在保证定位精度的前提下减少定位过程中的运算量是当前亟待解决的技术问题。

发明内容

[0005] 本发明实施例提供一种视觉定位方法、设备、机器人、存储介质及程序产品，以在保证定位精度的前提下减少定位过程中的运算量。

[0006] 第一方面，本发明实施例提供一种视觉定位方法，包括：

[0007] 通过单目相机拍摄得到当前单目视觉图像；

[0008] 根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点；

[0009] 将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿；

[0010] 根据所述当前相机位姿进行定位。

[0011] 在一种可能的设计中，所述根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，包括：

[0012] 从所述当前单目视觉图像中提取二维路面特征点；

[0013] 将所述二维路面特征点，反投影到近地平面上，并结合所述单目相机近地平面方程确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0014] 在一种可能的设计中，所述将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿之前，还包括：

[0015] 获取所述单目相机的当前预测位姿；

[0016] 所述将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿，包括：

[0017] 在所述当前预测位姿的指引下，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿。

[0018] 在一种可能的设计中，所述获取所述单目相机的当前预测位姿，包括：

[0019] 若所述当前预测位姿为初始位姿，则通过全球导航卫星系统获取所述单目相机的当前预测位姿；

[0020] 若所述当前预测位姿为非初始位姿，则根据上一相机位姿和运动预测模型获取所述单目相机的当前预测位姿。

[0021] 在一种可能的设计中，所述根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点之前，还包括：

[0022] 对所述单目相机近地平面方程进行实时自标定，得到标定后的单目相机近地平面方程；

[0023] 所述根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，包括：

[0024] 根据所述当前单目视觉图像和所述标定后的单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0025] 在一种可能的设计中，所述对所述单目相机近地平面方程进行实时自标定，包括：

[0026] 按照预设方式获取近地平面点云；

[0027] 在所述近地平面点云中加入调节系数，得到调节后的近地平面点云；

[0028] 根据所述调节后的近地平面点云拟合得到调节后的近地平面方程；

[0029] 根据所述当前单目视觉图像和所述调节后的近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的调节后的三维路面特征点；

[0030] 获取所述高精地图中的路面特征点的先验几何信息；

[0031] 对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，并根据所述最终的调节系数对所述单目相机近地平面方程进行标定。

[0032] 在一种可能的设计中，所述调节系数包括翻滚角调节系数、俯仰角调节系数和偏航角调节系数；所述先验几何信息包括左右车道线平行和多车道车道线等距；所述对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，包括：

[0033] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

[0034] 对所述翻滚角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现左右车道线平行的特征，则将当前的翻滚角调节系数作为最终的翻滚角调节系数；

[0035] 从所述当前单目视觉图像中过滤得到多车道车道线等距路段的三维路面特征点；

[0036] 对所述俯仰角调节系数进行调节，直至所述多车道车道线等距路段的三维路面特征点呈现多车道车道线等距的特征，则将当前的俯仰角调节系数作为最终的俯仰角调节系数；

[0037] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

[0038] 对所述偏航角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现的左右车道线距离与实际距离一致，则将当前的偏航角调节系数作为最终的偏航角调节系数。

[0039] 第二方面，本发明实施例提供一种视觉定位设备，包括：

[0040] 拍摄模块，用于通过单目相机拍摄得到当前单目视觉图像；

[0041] 确定模块，用于根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点；

[0042] 匹配模块，用于将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿；

[0043] 定位模块，用于根据所述当前相机位姿进行定位。

[0044] 在一种可能的设计中，所述确定模块，具体用于：

[0045] 从所述当前单目视觉图像中提取二维路面特征点；

[0046] 将所述二维路面特征点，反投影到近地平面上，并结合所述单目相机近地平面方程确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0047] 在一种可能的设计中，所述设备，还包括：

[0048] 获取模块，用于获取所述单目相机的当前预测位姿；

[0049] 所述匹配模块，具体用于：

[0050] 在所述当前预测位姿的指引下，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿。

[0051] 在一种可能的设计中，所述获取模块，具体用于：

[0052] 若所述当前预测位姿为初始位姿，则通过全球导航卫星系统获取所述单目相机的当前预测位姿；

[0053] 若所述当前预测位姿为非初始位姿，则根据上一相机位姿和运动预测模型获取所述单目相机的当前预测位姿。

[0054] 在一种可能的设计中，所述设备，还包括：

[0055] 自标定模块，用于对所述单目相机近地平面方程进行实时自标定，得到标定后的单目相机近地平面方程；

[0056] 所述确定模块，具体用于：

[0057] 根据所述当前单目视觉图像和所述标定后的单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0058] 在一种可能的设计中，所述自标定模块，具体用于：

[0059] 按照预设方式获取近地平面点云；

[0060] 在所述近地平面点云中加入调节系数，得到调节后的近地平面点云；

[0061] 根据所述调节后的近地平面点云拟合得到调节后的近地平面方程；

[0062] 根据所述当前单目视觉图像和所述调节后的近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的调节后的三维路面特征点；

[0063] 获取所述高精地图中的路面特征点的先验几何信息；

[0064] 对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，并根据所述最终的调节系数对所述单目相机近地平面方程进行标定。

[0065] 在一种可能的设计中，所述自标定模块，具体用于：

[0066] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

[0067] 对所述翻滚角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现左右车道线平行的特征，则将当前的翻滚角调节系数作为最终的翻滚角调节系数；

[0068] 从所述当前单目视觉图像中过滤得到多车道车道线等距路段的三维路面特征点；

[0069] 对所述俯仰角调节系数进行调节，直至所述多车道车道线等距路段的三维路面特征点呈现多车道车道线等距的特征，则将当前的俯仰角调节系数作为最终的俯仰角调节系数；

[0070] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

[0071] 对所述偏航角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现的左右车道线距离与实际距离一致，则将当前的偏航角调节系数作为最终的偏航角调节系数。

[0072] 第三方面，本发明实施例提供一种视觉定位设备，包括：单目相机、至少一个处理器和存储器；

[0073] 所述单目相机，与所述处理器连接，用于拍摄得到当前单目视觉图像，并将所述当前单目视觉图像发送给所述处理器；

[0074] 所述存储器存储计算机执行指令；

[0075] 所述至少一个处理器执行所述存储器存储的计算机执行指令，使得所述至少一个处理器执行如上第一方面以及第一方面各种可能的设计所述的方法。

[0076] 第四方面，本发明实施例提供一种机器人，包括：机器人主体和如上第三方面所述的视觉定位设备；

[0077] 所述机器人主体，与所述视觉定位设备连接，用于根据所述视觉定位设备提供的定位信息在路面行驶。

[0078] 第五方面，本发明实施例提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，当处理器执行所述计算机执行指令时，实现如上第一方面以及第一方面各种可能的设计所述的方法。

[0079] 第六方面，本发明实施例提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时，实现如上第一方面以及第一方面各种可能的设计所述的方法。

[0080] 本实施例提供的视觉定位方法、设备、机器人、存储介质及程序产品，该方法通过单目相机拍摄得到当前单目视觉图像，根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿，根据所述当前相机位姿进行定位，能够基于单目视觉图像快速恢复三维路面特征点，并通过将得到的三维路面特征点与高精地图匹配达到准确的定位，实现了在保证定位精度的前提下减少定位过程中的运算量。

附图说明

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

[0082] 图1为本发明一实施例提供的视觉定位原理图；

[0083] 图2为本发明一实施例提供的视觉定位方法的流程示意图；

[0084] 图3为本发明一实施例提供的高精地图的示意图；

[0085] 图4为本发明又一实施例提供的视觉定位方法的流程示意图；

[0086] 图5为本发明又一实施例提供的视觉定位方法的原理图；

[0087] 图6为本发明再一实施例提供的视觉定位方法的流程示意图；

[0088] 图7为本发明再一实施例提供的单目相机近地平面方程实时自标定的原理示意图；

[0089] 图8为本发明一实施例提供的视觉定位设备的结构示意图；

[0090] 图9为本发明一实施例提供的视觉定位设备的硬件结构示意图。

具体实施方式

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

[0092] 定位技术是自动驾驶等机器人应用技术的基础和核心技术之一，为机器人提供位置和姿态，即位姿信息。按定位原理，定位技术可以分为几何定位、航位推算、特征定位。

[0093] 几何定位是对已知位置的参考设备进行测距或者测角，再通过几何计算确定自身位置。包括全球导航卫星系统(Global Navigation Satellite System，GNSS)、室内定位技术(Ultra Wide Band，UWB)、蓝牙、5G等技术，提供绝对定位信息。在智能汽车应用中以GNSS技术应用最广。GNSS定位基于卫星定位技术，分为单点定位、差分GPS定位、实时动态定位(Real‑time kinematic，RTK)GPS定位，其中单点定位提供3～10米定位精度，差分GPS提供0.5～2米定位精度，RTK GPS提供厘米级定位精度。

[0094] 航位推算是从上一时刻位置出发，根据惯性测量单元(Inertial Measurement Unit，IMU)和轮速计等传感器的运动数据推算下一时刻的位置，提供的是相对定位信息。

[0095] 特征定位首先获取周围环境的若干特征，如基站ID、Wifi指纹、图像、Lidar点云等。然后将观测特征和事先建立的特征地图进行匹配，确定在特征地图中的位置，可提供绝对定位信息。

[0096] 视觉定位作为一种特征定位技术，通过相机采集获取环境特征，与视觉特征地图进行匹配，确定机器人位姿信息。深度相机通过特定的硬件和软件支持，可直接获取3D环境信息。普通光学相机采集获取的环境特征为2D信息。单目相机恢复3D信息存在尺度问题，需要更多的计算开销和时间。双目相机恢复3D信息需要严格的外参数标定和大量的计算开销，并且随着机器人运动振动，外参数可能产生变化，需要重新进行标定校准。因此，如何在保证定位精度的前提下减少定位过程中的运算量是当前亟待解决的技术问题。

[0097] 为了解决上述问题，发明人研究发现，可以基于近地平面假设根据单目相机拍摄得到的单目视觉图像快速得到三维(3Dimensional，3D)特征信息，并结合高精地图实现高精度的定位，从而能够在保证定位精度的前提下减少定位过程中的运算量。

[0098] 具体的，图1为本发明一实施例提供的视觉定位原理图。如图1所示，首先可以通过单目相机获得单目视觉图像，再对该单目视觉图像进行2D观测，也即二维路面特征点的提取，再基于近地平面假设，也即结合近地平面方程进行3D观测，获得三维路面特征点。实际路面中获得的三维路面特征点与建立的高精地图进行配准，进而得到高精度的位姿信息，实现了在保证定位精度的前提下减少定位过程中的运算量。

[0099] 另外，在主流同步定位与建图(Simultaneous Localization And Mapping，SLAM)中，视觉特征地图存储专用视觉特征信息，如加速稳健特征(Speeded Up Robust Feature，SURF)、定向快速旋转特征(Oriented Fast and Rotated Brief，ORB)等，其主要问题是地图占用空间大，且使用过程中需要专门建立单独的定位图层。而本申请中可以直接使用高精地图进行定位，避免了单独定位图层的存在，并且相较于传统的视觉特征地图，高精地图存储的是矢量元素信息，具有占用空间小的优点，因此更进一步的减少了运算量，提高了运算速度。

[0100] 此外，为了解决相机自身安装位置和姿态随着时间的推移会发生变动的问题，本申请还可以通过使用高精地图中的先验几何结构信息，在线实时标定单目相机近地平面方程，进一步提高定位的准确度。

[0101] 下面以具体地实施例对本发明的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。

[0102] 图2为本发明一实施例提供的视觉定位方法的流程示意图。如图2所示，该方法包括：

[0103] 201、通过单目相机拍摄得到当前单目视觉图像。

[0104] 本实施例的执行主体可以为在路面行驶的机器人，例如，无人驾驶汽车，扫地机器人等。

[0105] 本实施例中，单目相机设置在机器人上，在机器人行驶过程中，对周围环境进行拍摄，例如，可以对行驶的前方进行拍摄，获得当前单目视觉图像，该当前单目视觉图像中包含前方路面的路面特征信息，例如车道线、车道标识等。

[0106] 202、根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0107] 具体的，从所述当前单目视觉图像中提取二维路面特征点，即2D路面特征点；将所述二维路面特征点，反投影到近地平面上，并结合所述单目相机近地平面方程确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，即3D路面特征点。

[0108] 该过程基于近地平面假设，即假设在相机观测的一定范围内，地平面是水平的。

[0109] 对单目相机拍摄得到的当前单目视觉图像进行检测。检测提取地平面上的路面特征信息，特征提取的方式有多种，例如可以使用LineNet、LaneNet、PolyLaneNet等神经网络方法，也可以使用传统的路面特征检测方法，例如图像滤波方法，比如漫水填充、霍夫变换、边缘检测等。

[0110] 路面特征信息是2D路面特征点的集合，包括但不限于车道线、车道标识等。

[0111] 完成上述2D路面特征点检测后，在近地平面假设下，将像平面上的2D路面特征点，反投影到近地平面上，以获取相机坐标系中的3D路面特征点。

[0112] 相机归一化投影平面上的点Pcn＝(xcn,ycn,1)和像平面点Ps＝(xs,ys,zs)的关系满足以下表达式：

[0113] Ps＝K×Pcn            (1)

[0114] 其中，相机内部参数K为3x3的线性投影矩阵。通过表达式1逆推即可通过像平面2D路面特征点Ps获取归一化投影平面上的点Pcn。

[0115] 另外，设相机归一化投影平面上的点为Pcn与相机坐标系3D路面特征点Pc＝(xc,yc,zc)的关系满足以下表达式：

[0116] xc＝xcn×zc            (2)

[0117] yc＝ycn×zc            (3)

[0118] 通过表达式(2)和(3)可知，从2D路面特征点恢复到3D路面特征点，实际最关键的是恢复深度信息zc。在近地平面假设下，相机坐标系中的3D路面特征点在近地平面上。单目相机近地平面方程为一个平面方程，可表示为

[0119] a×x+b×y+c×z+d＝0              (4)

[0120] 其中，a、b、c、d为地平面方程系数。

[0121] 可见，通过该近地平面，可以将单目视觉图像中提取的2D路面特征点反投影为相机坐标系下的3D路面特征点。具体的，联合表达式(1)至(3)，将相机坐标系下的3D路面特征点代入表达式(4)中，得

[0122] zc＝‑d/(a×xcn+b×ycn+c)            (5)

[0123] 再将表达式(5)代入表达式(2)和(3)中，即可得到当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点Pc。

[0124] 203、将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿。

[0125] 本实施例中，高精度地图可以采用现有地图，也可以通过建立获得。具体的，可以通过高精度定位设备和传感器采集获取道路信息，可以根据该道路信息建立得到高精地图。高精地图以矢量信息的方式存储道路信息。包括但不限于以点、线、面等矢量信息存储路面上的灯杆、路牌、路沿等路面物体信息，以及实线、虚线、箭头、文字等路面标识信息。示例性的，可参照图3，图3为本发明一实施例提供的高精地图的示意图。

[0126] 204、根据所述当前相机位姿进行定位。

[0127] 实际应用中，单目相机可以进行连续拍摄，每次拍摄获得一帧图像，针对每次拍摄获得的该帧图像，进行相机位姿的确定，得到该时间点的定位信息，连续拍摄后，即可得到连续的定位信息，无人驾驶车辆等机器人可以根据该连续定位信息进行安全行驶。

[0128] 本实施例提供的视觉定位方法，通过单目相机拍摄得到当前单目视觉图像，根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿，根据所述当前相机位姿进行定位，能够基于单目视觉图像快速恢复三维路面特征点，并通过将得到的三维路面特征点与高精地图匹配达到准确的定位，实现了在保证定位精度的前提下减少定位过程中的运算量。

[0129] 图4为本发明又一实施例提供的视觉定位方法的流程示意图。如图4所示，在上述实施例的基础上，例如在图2所示实施例的基础上，为了能够快速的完成高精地图与三维路面特征点的配准，可以预先进行位姿的预测，本实施例中，该方法包括：

[0130] 401、通过单目相机拍摄得到当前单目视觉图像。

[0131] 本实施例中步骤401与上述实施例中步骤201相类似，此处不再赘述。

[0132] 402、根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0133] 本实施例中步骤403与上述实施例中步骤202相类似，此处不再赘述。

[0134] 403、获取所述单目相机的当前预测位姿。

[0135] 404、在所述当前预测位姿的指引下，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿。

[0136] 本实施例中，为了能够快速的完成高精地图与三维路面特征点的配准，可以预先进行位姿的预测，在预测的当前预测位姿的指引下，来缩小高精地图与三维路面特征点的匹配范围，从而能够快速完成位姿的确定。

[0137] 实际应用中，当前预测位姿的获取由多种方式，在一种可实现方式中，可以通过GNSS确定。针对每一帧单目视觉图像的处理，均采用GNSS来进获取预测位姿，以在该预测位姿所在的区域完成高精地图与三维路面特征点的快速匹配。

[0138] 在另一种可实现方式中，若所述当前预测位姿为初始位姿，则通过全球导航卫星系统获取所述单目相机的当前预测位姿；若所述当前预测位姿为非初始位姿，则根据上一相机位姿和运动预测模型获取所述单目相机的当前预测位姿。也就是说，可以在初始化中采用GNSS获取初始的预测位姿，在后续的处理中，通过运动预测模型来获取预测位姿。其中，该运动预测模型可以是简单的恒定速度模型，也可基于IMU、轮速等额外的传感器信息建立的运动预测模型，本实施例中对此不做限定。

[0139] 示例的，图5为本发明又一实施例提供的视觉定位方法的原理图，如图5所示，第一步，在初始时，以GNSS提供初始的预测位姿。通过3D特征观测和高精地图的匹配获取相机位姿。现有技术的配准方法包括但不限于贝叶斯方法等。从而完成定位初始化。第二步，通过运动预测模型，提供下一帧观测时的预测位姿。该运动预测模型可用简单的恒定速度模型，也可基于IMU、轮速等额外的传感器信息。第三步，在预测位姿的指引下，进行3D观测和地图的匹配获取位姿。第四步，循环进行第二步和第三步，实现持续定位。

[0140] 405、根据所述当前相机位姿进行定位。

[0141] 本实施例中步骤405与上述实施例中步骤204相类似，此处不再赘述。

[0142] 本实施例提供的视觉定位方法，通过进行预测位姿的获取，在预测的当前预测位姿的指引下，来缩小高精地图与三维路面特征点的匹配范围，从而能够快速的完成高精地图与三维路面特征点的配准，快速完成位姿的确定，提高了定位的效率。

[0143] 图6为本发明再一实施例提供的视觉定位方法的流程示意图。如图6所示，为了解决相机自身安装位置和姿态随着时间的推移会发生变动的问题，在上述实施例的基础上，例如在图2所示实施例的基础上，本实施例中增设了近地平面方程实时自标定的步骤，该方法包括：

[0144] 601、通过单目相机拍摄得到当前单目视觉图像。

[0145] 602、对所述单目相机近地平面方程进行实时自标定，得到标定后的单目相机近地平面方程。

[0146] 可选地，所述对所述单目相机近地平面方程进行实时自标定，包括：按照预设方式获取近地平面点云；在所述近地平面点云中加入调节系数，得到调节后的近地平面点云；根据所述调节后的近地平面点云拟合得到调节后的近地平面方程；根据所述当前单目视觉图像和所述调节后的近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的调节后的三维路面特征点；获取所述高精地图中的路面特征点的先验几何信息；对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，并根据所述最终的调节系数对所述单目相机近地平面方程进行标定。

[0147] 具体的，所述调节系数包括翻滚角调节系数、俯仰角调节系数和偏航角调节系数；所述先验几何信息包括左右车道线平行和多车道车道线等距；所述对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，包括：

[0148] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；对所述翻滚角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现左右车道线平行的特征，则将当前的翻滚角调节系数作为最终的翻滚角调节系数。

[0149] 从所述当前单目视觉图像中过滤得到多车道车道线等距路段的三维路面特征点；对所述俯仰角调节系数进行调节，直至所述多车道车道线等距路段的三维路面特征点呈现多车道车道线等距的特征，则将当前的俯仰角调节系数作为最终的俯仰角调节系数。

[0150] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；对所述偏航角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现的左右车道线距离与实际距离一致，则将当前的偏航角调节系数作为最终的偏航角调节系数。

[0151] 示例的，图7为本发明再一实施例提供的单目相机近地平面方程实时自标定的原理示意图。如图7所示，首先可以通过单目相机获得单目视觉图像，再对该单目视觉图像进行2D观测，也即二维路面特征点的提取，再基于近地平面假设，也即结合近地平面方程进行3D观测，获得三维路面特征点。其次，通过GNSS获取预测位姿，并在该预测位姿的指导下从高精地图中确定拍摄路面的几何先验特征，通过该几何先验特征，对三维路面特征点中的调节系数进行调节，从而获取使得三维路面特征点与几何先验特征一致的系数值，并根据该系数值进行单目相机近地平面方程系数的自标定。通过实时自标定，能够解决随机器人运行产生的相机位置移动导致的标定参数不准的问题。

[0152] 具体的，在实际使用中，随着时间的推移，相机自身安装位置和姿态会发生变动。因此需要每隔一段时间进行相机近地平面的重新标定。实际应用中再返回到专门标定场所进行标定是费时费力的，因此本申请提出一种通过实时采集的图像进行单目相机近地平面自标定方法。

[0153] 在初始时(即设备第一次装配时)，可以通过事先标定的方式确定方程系数的初始值。具体步骤包括：第一步，通过光学雷达LIDAR，获取LIDAR坐标系下的地平面点云；第二步，地平面点云通过LIDAR‑相机外参转换到相机坐标系下；第三步，拟合相机坐标系下的地平面点云，获取地平面方程系数。

[0154] 在实时自标定过程中，在相机坐标系中，如上述表达式(4)所示，地平面方程以a、b、c、d共4个参数表示。在本申请所述的方法中，利用的是地平面上的车道标识信息。假如地平面方程系数是带误差的，则地平面在相机坐标系中，表现为相比较于实际平面存在翻滚角roll、俯仰角pitch角方向以及偏航角z方向上的不一致。而这也最终导致反投影获得的3D车道标识信息的畸变失真。本实施例即利用这一关系来做近地平面的实时自标定。

[0155] 对近地平面方程系数的实时自标定过程，具体包括以下步骤：

[0156] 第一步，获取近地平面采样点云。以上述初始时所获得的近地平面方程系数作为初始参数，或者以上一次自标定所获得的地平面方程系数作为初始系数。在该近地平面上，以一定方式采样n个点。比如以任一近地平面上的点(x0，y0，z0)为起点，向x轴、y轴方向以10cm为等间隔采样100个点。

[0157] 第二步，在近地平面采样点云中加入调节系数。上述第一步中获得的采样点构成近地平面上的点云。对该点云分别施加roll角方向、pitch角方向和z方向的未知调节系数factor‑r、factor‑p和factor‑z。则点云分别沿着roll角方向、pitch角方向发生转动，沿着z方向发生平移。由该点云拟合得到调节后的近地平面方程。上述未知调节系数在下述过程中求解。

[0158] 第三步，通过如图2所示实施例的方法将单目相机获得的2D路面特征点恢复得到3D路面特征点。不同于如图2所示实施例的是，这里使用到的地平面方程系数是上述未知调节系数的函数，由此恢复得到的3D路面特征点也是上述未知调节系数的函数，设为Pcr。

[0159] 第四步，获取高精地图中的先验信息。以GNSS提供的预测位姿为指引，可以确定当前机器人/相机所在车道路段，从而可获取得到该段车道的先验几何信息，包括左右车道线是否平行、多车道车道线等距、车道线间距。

[0160] 第五步，在左右车道线平行路段。从单目相机获得的2D路面特征点过滤获取得到2D的左右车道线特征。调节上述第二步中的近地平面点云调节系数factor‑r，则由第三步获取的3D左右车道线特征也对应变化。具体而言，当经调节系数factor‑r调整后的近地平面点云与实际近地平面在roll角方向平行时，获取的3D左右车道线特征平行。据此，调节近地平面点云调节系数factor‑r，使得3D左右车道线平行，则此时的调节系数factor‑r即为正确的调节系数。

[0161] 在上述基础上，在多车道车道线等距路段。从单目相机获得的2D路面特征点过滤获取得到2D的多车道车道线。调节第二步中的近地平面点云调节系数factor‑p，则由第三步获取的3D多车道车道线特征也对应变化。具体而言，当经调节系数factor‑p调整后的近地平面点云与实际近地平面在pitch角方向平行时，获取的3D多车道车道线特征等距。据此，调节近地平面点云调节系数factor‑p，使得3D多车道车道线等距，则此时的调节系数factor‑p即为正确的调节系数。

[0162] 在上述基础上，在左右车道线平行路段。从单目相机获得的2D路面特征点过滤获取得到2D的左右车道线。调节第二步中的近地平面点云调节系数factor‑z，则由第三步获取的3D左右车道线特征也对应变化。具体而言，当经调节系数factor‑z调整后的近地平面点云与实际近地平面在z方向重合时，获取的3D左右车道线特征距离与实际左右车道乡距离相等。据此，调节近地平面点云调节系数factor‑z，使得3D左右车道线特征距离与实际左右车道乡距离相等，则此时的调节系数factor‑z即为正确的调节系数。

[0163] 有上述步骤获取到正确的调节系数factor‑r、factor‑p和factor‑z，将其作用于近地平面采样点云。对该点云进行拟合，即获取一次在线标定后的近地平面方程系数。

[0164] 603、根据所述当前单目视觉图像和所述标定后的单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0165] 604、将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿。

[0166] 605、根据所述当前相机位姿进行定位。

[0167] 本实施例中步骤603至步骤605与上述实施例中步骤202至步骤204相类似，此处不再赘述。

[0168] 本实施例提供的视觉定位方法，通过使用高精地图中的先验几何结构信息，可在线实时标定单目相机近地平面方程系数，能够解决随机器人运行产生的相机位置移动导致的标定参数不准的问题，从而进一步提高了定位精度。

[0169] 图8为本发明一实施例提供的视觉定位设备的结构示意图。如图8所示，该视觉定位设备80包括：拍摄模块801、确定模块802、匹配模块803以及定位模块804。

[0170] 拍摄模块801，用于通过单目相机拍摄得到当前单目视觉图像；

[0171] 确定模块802，用于根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点；

[0172] 匹配模块803，用于将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿；

[0173] 定位模块804，用于根据所述当前相机位姿进行定位。

[0174] 本发明实施例提供的视觉定位设备，拍摄模块801通过单目相机拍摄得到当前单目视觉图像，确定模块802根据所述当前单目视觉图像和单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点，匹配模块803将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿，定位模块804根据所述当前相机位姿进行定位，能够基于单目视觉图像快速恢复三维路面特征点，并通过将得到的三维路面特征点与高精地图匹配达到准确的定位，实现了在保证定位精度的前提下减少定位过程中的运算量。

[0175] 在一种可能的设计中，所述确定模块802，具体用于：

[0176] 从所述当前单目视觉图像中提取二维路面特征点；

[0177] 将所述二维路面特征点，反投影到近地平面上，并结合所述单目相机近地平面方程确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0178] 在一种可能的设计中，所述设备，还包括：

[0179] 获取模块，用于获取所述单目相机的当前预测位姿；

[0180] 所述匹配模块803，具体用于：

[0181] 在所述当前预测位姿的指引下，将所述当前三维路面特征点与高精地图进行匹配，得到当前相机位姿。

[0182] 在一种可能的设计中，所述获取模块，具体用于：

[0183] 若所述当前预测位姿为初始位姿，则通过全球导航卫星系统获取所述单目相机的当前预测位姿；

[0184] 若所述当前预测位姿为非初始位姿，则根据上一相机位姿和运动预测模型获取所述单目相机的当前预测位姿。

[0185] 在一种可能的设计中，所述设备，还包括：

[0186] 自标定模块，用于对所述单目相机近地平面方程进行实时自标定，得到标定后的单目相机近地平面方程；

[0187] 所述确定模块，具体用于：

[0188] 根据所述当前单目视觉图像和所述标定后的单目相机近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的当前三维路面特征点。

[0189] 在一种可能的设计中，所述自标定模块，具体用于：

[0190] 按照预设方式获取近地平面点云；

[0191] 在所述近地平面点云中加入调节系数，得到调节后的近地平面点云；

[0192] 根据所述调节后的近地平面点云拟合得到调节后的近地平面方程；

[0193] 根据所述当前单目视觉图像和所述调节后的近地平面方程，确定所述当前单目视觉图像在所述单目相机坐标系下的调节后的三维路面特征点；

[0194] 获取所述高精地图中的路面特征点的先验几何信息；

[0195] 对所述调节系数进行调节，直至所述调节后的三维路面特征点与所述先验几何信息相一致，得到最终的调节系数，并根据所述最终的调节系数对所述单目相机近地平面方程进行标定。

[0196] 在一种可能的设计中，所述自标定模块，具体用于：

[0197] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

[0198] 对所述翻滚角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现左右车道线平行的特征，则将当前的翻滚角调节系数作为最终的翻滚角调节系数；

[0199] 从所述当前单目视觉图像中过滤得到多车道车道线等距路段的三维路面特征点；

[0200] 对所述俯仰角调节系数进行调节，直至所述多车道车道线等距路段的三维路面特征点呈现多车道车道线等距的特征，则将当前的俯仰角调节系数作为最终的俯仰角调节系数；

[0201] 从所述当前单目视觉图像中过滤得到左右车道线平行路段的三维路面特征点；

[0202] 对所述偏航角调节系数进行调节，直至所述左右车道线平行路段的三维路面特征点呈现的左右车道线距离与实际距离一致，则将当前的偏航角调节系数作为最终的偏航角调节系数。

[0203] 本发明实施例提供的视觉定位设备，可用于执行上述的方法实施例，其实现原理和技术效果类似，本实施例此处不再赘述。

[0204] 图9为本发明一实施例提供的视觉定位设备的硬件结构示意图。如图9所示，本实施例提供的视觉定位设备90包括：单目相机900、至少一个处理器901和存储器902。该视觉定位设备90还包括通信部件903。其中，处理器901、存储器902以及通信部件903通过总线904连接。

[0205] 所述单目相机900，与所述处理器901连接，用于拍摄得到当前单目视觉图像，并将所述当前单目视觉图像发送给所述处理器901。

[0206] 所述存储器902存储计算机执行指令。

[0207] 所述至少一个处理器901执行所述存储器902存储的计算机执行指令，使得至少一个处理器901执行如上视觉定位设备90所执行的视觉定位方法。

[0208] 在具体实现过程中，单目相机900，拍摄得到当前单目视觉图像，并将所述当前单目视觉图像发送给所述处理器901。至少一个处理器901执行所述存储器902存储的计算机执行指令，使得至少一个处理器901执行如上视觉定位设备90所执行的视觉定位方法。

[0209] 当本实施例的单目视觉图像处理由服务器执行时，该通信部件903可以将单目视觉图像发送给服务器。

[0210] 处理器901的具体实现过程可参见上述方法实施例，其实现原理和技术效果类似，本实施例此处不再赘述。

[0211] 在上述的图9所示的实施例中，应理解，处理器可以是中央处理单元(英文：Central Processing Unit，简称：CPU)，还可以是其他通用处理器、数字信号处理器(英文：
Digital Signal Processor，简称：DSP)、专用集成电路(英文：Application Specific Integrated Circuit，简称：ASIC)等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合发明所公开的方法的步骤可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。

[0212] 存储器可能包含高速RAM存储器，也可能还包括非易失性存储NVM，例如至少一个磁盘存储器。

[0213] 总线可以是工业标准体系结构(Industry Standard Architecture，ISA)总线、外部设备互连(Peripheral Component，PCI)总线或扩展工业标准体系结构(Extended Industry Standard Architecture，EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，本申请附图中的总线并不限定仅有一根总线或一种类型的总线。

[0214] 本申请还提供一种机器人，包括：机器人主体和如上所述的视觉定位设备。所述机器人主体，与所述视觉定位设备连接，用于根据所述视觉定位设备提供的定位信息在路面行驶。

[0215] 本申请还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，当处理器执行所述计算机执行指令时，实现如上视觉定位设备执行的视觉定位方法。

[0216] 上述的计算机可读存储介质，上述可读存储介质可以是由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(SRAM)，电可擦除可编程只读存储器(EEPROM)，可擦除可编程只读存储器(EPROM)，可编程只读存储器(PROM)，只读存储器(ROM)，磁存储器，快闪存储器，磁盘或光盘。可读存储介质可以是通用或专用计算机能够存取的任何可用介质。

[0217] 一种示例性的可读存储介质耦合至处理器，从而使处理器能够从该可读存储介质读取信息，且可向该可读存储介质写入信息。当然，可读存储介质也可以是处理器的组成部分。处理器和可读存储介质可以位于专用集成电路(Application Specific Integrated Circuits，简称：ASIC)中。当然，处理器和可读存储介质也可以作为分立组件存在于设备中。

[0218] 本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

[0219] 最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。