视觉定位方法、计算机存储介质及计算机程序产品

(19)国家知识产权局

(12)发明专利申请

4P(10)申请公布号CN115564839A

(43)申请公布日2023.01.03

(21)申请号202110749743.9

(22)申请日2021.07.02

(71)申请人阿里巴巴新加坡控股有限公司

地址新加坡珊顿道8号AXA大厦45层01室

(72)发明人黄帅张涛韩冰肖中阳

(74)专利代理机构北京合智同创知识产权代理

有限公司11545

专利代理师李杰兰淑铎

(51)Int.CI.

G06T7/80(2017.01)

G06T3/40(2006.01)

G06V70/44(2022.01)

权利要求书2页说明书18页附图6页

(54)发明名称

视觉定位方法、计算机存储介质及计算机程

序产品

(57)摘要

本申请实施例提供了一种视觉定位方法、计

算机存储介质及计算机程序产品，其中，所述视

觉定位方法包括:获取卷帘快门相机采集的图像

帧；获取图像帧用于视觉定位的特征点；获取I

MU相对位姿数据，所述IMU相对位姿数据是基于

从所述图像帧的曝光开始至曝光结束之间IMU

输出的IMU帧数据确定的；基于所述IMU相对位

姿数据，按照特征点所属的图像行,确定图像行I

MU相对位姿数据;根据所述图像行IMU相对位姿

数据、特征点在图像帧中的位置、根据对IMU帧

数据进行预积分确定的图像间测量项、和待估计

v状态量，进行视觉定位解算。通过本申请实施例，

会可以进行有效且准确的视觉定位。

00

S

g

g

r-H

g

CN115564839A权利要求书1/2页

1.一种视觉定位方法，其中，包括：

获取卷帘快门相机采集的图像帧；

获取所述图像帧用于视觉定位的特征点；

获取IMU相对位姿数据，所述IMU相对位姿数据是基于从所述图像帧的曝光开始至曝光

结束之间IMU输出的IMU帧数据确定的；

基于所述IMU相对位姿数据，按照特征点所属的图像行，确定图像行IMU相对位姿数据；

根据所述图像行IMU相对位姿数据、特征点在图像帧中的位置、根据对IMU帧数据进行

预积分确定的图像间测量项、和待估计状态量,进行视觉定位解算。

2.根据权利要求1所述的方法，其中，所述基于所述IMU相对位姿数据，按照特征点所属

的图像行,确定图像行IMU相对位姿数据，包括：

确定特征点所属的图像行的行时间，所述行时间用于指示该图像行的曝光时间；

根据各图像行的行时间对IMU相对位姿数据进行插值处理，获得图像行IMU相对位姿数

据。

3.根据权利要求1所述的方法，其中，在所述获取IMU相对位姿数据之前,所述方法还包

括：

采集IMU帧数据，并将IMU帧数据分别缓存在第一缓存和第二缓存中；

基于第一缓存中的IMU帧数据,确定每一帧IMU帧数据对应的IMU初始位姿；

基于第二缓存中的IMU帧数据进行预积分，得到每一帧IMU帧数据的预积分结果；

所述获取IMU相对位姿数据，包括：

根据每一帧IMU帧数据对应的IMU初始位姿和预积分结果，确定该帧IMU帧数据相对于

其所对应的图像帧的采集时刻的IMU相对位姿数据。

4.根据权利要求3所述的方法，其中，所述基于第二缓存中的IMU帧数据进行预积分，得

到每一帧IMU帧数据的预积分结果，包括：

确定所述IMU初始位姿相对于IMU偏置的雅可比矩阵；

根据所述IMU初始位姿和所述雅可比矩阵，对所述第二缓存中的IMU帧数据进行预积分

处理，获得每一帧IMU帧数据的预积分结果。

5.根据权利要求3所述的方法,其中，所述方法还包括：

在进行所述视觉定位解算的过程中，根据当前IMU帧数据的IMU偏置变化程度确定是否

需要更新当前IMU帧数据之前的历史IMU帧数据的相对位姿；

若需要更新,则对所述历史IMU帧数据对应的相对位姿进行更新。

6.根据权利要求5所述的方法，其中，对所述历史IMU帧数据对应的相对位姿进行更新,

包括：

使用IMU初始位姿相对于IMU偏置的雅可比矩阵，对所述历史IMU帧数据对应的相对位

姿进行一阶线性近似更新。

7.根据权利要求1-6所述的任意一项方法，其中，所述根据所述图像行IMU相对位姿数

据、特征点在图像帧中的位置、根据对IMU帧数据进行预积分确定的图像间测量项、和待估

计状态量，进行视觉定位解算，包括：

根据各特征点在图像帧中的位置、及各特征点所属的图像行的图像行IMU相对位姿数

据,确定各特征点的投影位置；

2

CN115564839A权利要求书2/2页

根据各特征点的观测位置和投影位置，确定各特征点的重投影误差；

根据所述重投影误差、所述图像间测量项、边缘化的先验信息和待估计状态量,进行视

觉定位解算。

8.根据权利要求1所述的方法，其中，

所述方法还包括:对各特征点所属的图像行进行行号去重；

所述基于所述IMU相对位姿数据，按照特征点所属的图像行，确定图像行IMU相对位姿

数据，包括:基于所述IMU相对位姿数据，按照进行了行号去重后的特征点所属的图像行，确

定图像行IMU相对位姿数据。

9.根据权利要求8所述的方法，其中，在所述确定图像行IMU相对位姿数据之后,所述方

法还包括：

根据各特征点所属图像行的行号和各特征点所属图像行对应的所述图像行IMU相对位

姿数据，生成用于记录行号与图像行IMU相对位姿数据对应关系的数据结构。

10.一种计算机存储介质，其上存储有计算机程序,该计算机程序被处理器执行时实现

如权利要求1-9中任一所述的视觉定位方法。

11.一种计算机程序产品，包括计算机指令,所述计算机指令指示计算设备执行如权利

要求1-9中任一所述的视觉定位方法对应的操作。

3

CN115564839A说明书1/18页

视觉定位方法'计算机存储介质及计算机程序产品

技术领域

[0001]本申请实施例涉及定位技术领域,尤其涉及一种视觉定位方法、计算机存储介质

及计算机程序产品。

背景技术

[0002]视觉定位是一种基于视觉传感器(如相机)获得图像,并对图像进行相应的计算和

处理后，获得搭载视觉传感器的设备的位姿等信息的定位技术。视觉惯性里程计(Visual

InertialOdometry,VI0)是实现视觉定位的技术方案之一，该方案通过相机、IMU获取到的

数据估计各帧图像/IMU的位姿，同时能够恢复出周围环境的三维数据。

[0003]现有的一种视觉里程计方案，使用了全局快门相机，但全局快门相机造价较高，一

般消费级设备例如手机,小型无人机等因成本考虑不会搭载全局快门相机，因此，这些设备

无法采用现有的视觉里程计技术，导致视觉里程计的应用可扩展性受限。

[0004]另一种视觉里程计方案使用了卷帘快门相机,卷帘快门相机的价格相比全局快门

相机低，但与全局快门相机在同一时刻曝光所有像素不同，卷帘快门相机是逐图像行(像素

行)曝光，图像的曝光时长称为读出时间(readouttime)。但当卷帘快门相机处于运动状态

时，同一图像曝光不同图像行时设备的位姿是不同。因此，虽然采用卷帘快门相机的VI0方

案成本较低，但由于曝光原理的差异,使得如何保证采用卷帘快门相机的VI0方案能够实现

准确的状态估计，是本领域技术人员需要解决一个问题。

发明内容

[0005]有鉴于此，本申请实施例提供一种视觉定位方案，以至少部分解决上述问题。

[0006]根据本申请实施例的第一方面，提供了一种视觉定位方法，包括:获取卷帘快门相

机采集的图像帧;获取图像帧用于视觉定位的特征点;获取IMU相对位姿数据，所述IMU相对

位姿数据是基于从所述图像帧的曝光开始至曝光结束之间IMU输出的IMU帧数据确定的；基

于所述IMU相对位姿数据，按照特征点所属的图像行，确定图像行IMU相对位姿数据;根据所

述图像行IMU相对位姿数据、特征点在图像帧中的位置、根据对IMU帧数据进行预积分确定

的图像间测量项、和待估计状态量,进行视觉定位解算。

[00。刀根据本申请实施例的第二方面,提供了一种计算机存储介质，其上存储有计算机

程序,该计算机程序被处理器执行时实现如第一方面所述的视觉定位方法。

[0008]根据本申请实施例的第三方面,提供了一种计算机程序产品，包括计算机指令，所

述计算机指令指示计算设备执行如第一方面所述的视觉定位方法对应的操作。

[0009]根据本申请实施例提供的方案,针对基于卷帘快门相机采集的图像进行视觉定位

的场景，为避免卷帘快门相机在移动时，在不同时刻按图像行曝光使得每一图像行的像素

在不同位姿下获取,按照现有全局相机定位方式造成状态估计不准确的问题，首先对卷帘

快门相机采集的图像帧进行特征点提取，然后，基于IMU的相对位姿数据和各特征点所属的

图像行确定各图像行对应的图像行IMU相对位姿数据，进而基于各图像行IMU相对位姿数

4

CN115564839A说明书2/18页

据、特征点在图像帧中的位置、图像间测量项以及待估计状态量进行视觉定位解算。因IMU

的高频特性,则基于高频采集到的IMU帧数据和卷帘快门相机采用的图像帧进行位姿解算,

可将及时获得的不同IMU帧数据对应的IMU相对位姿，作为确定图像帧中各图像行位姿的依

据，更有效地反映卷帘快门相机在按图像行曝光时的位姿。因此，本申请实施例的方案可以

进行有效且准确的视觉定位。

附图说明

[0010]为了更清楚地说明本申请实施例或现有技术中的技术方案,下面将对实施例或现

有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本

申请实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获

得其他的附图。

[0011]图1为根据本申请实施例一的一种视觉定位方法的步骤流程图；

[0012]图2为根据本申请实施例二的一种视觉定位方法的步骤流程图；

[0013]图3A为根据本申请实施例三的一种视觉定位方法的步骤流程图；

[0014]图3B为图3A所示实施例中的一种IMU帧相对位姿计算过程中涉及的各算法的示意

图；

[0015]图3c为图3A所示实施例中的一种重投影误差计算过程中涉及的各算法的示意图；

[0016]图4为根据本申请实施例四的一种视觉定位装置的结构框图；

[0017]图5A为根据本申请实施例五的一种视觉定位系统的整体处理过程示意图；

[0018]图5B为图5A所示实施例中的部分数据创建及更新示意图；

[0019]图6为根据本申请实施例六的一种电子设备的结构示意图。

具体实施方式

[0020]为了使本领域的人员更好地理解本申请实施例中的技术方案，下面将结合本申请

实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实

施例仅是本申请实施例一部分实施例，而不是全部的实施例。基于本申请实施例中的实施

例，本领域普通技术人员所获得的所有其他实施例，都应当属于本申请实施例保护的范围。

[0021]下面结合本申请实施例附图进一步说明本申请实施例具体实现。

[0022]实施例一

[0023]参照图1,示出了根据本申请实施例一的一种视觉定位方法的步骤流程图。

[0024]本实施例的视觉定位方法包括以下步骤：

[0025]步骤S100：获取卷帘快门相机采集的图像帧。

[0026]在使用视觉定位系统的设备中，需要有图像采集设备，本申请实施例中的图像采

集设备为卷帘快门相机。而在使用卷帘快门相机的情况下，如前所述,其逐图像行对像素进

行曝光，相机运动过程中，每一图像行的像素在设备的不同位姿下获取，并最终形成为一幅

完整的图像帧。

[0027]步骤S200：获取所述图像帧用于视觉定位的特征点。

[0028]在视觉定位中，特征点是图像帧里一些特别的地方,其在相机运动之后仍保持稳

定，如图像帧中具有这样特性的角点、边缘、区块等。在实际应用中，本领域技术人员可以根

5

CN115564839A说明书3/18页

据实际需求采用任意适当的方式对图像帧进行特征点提取，本申请实施例对此不作限制。

[0029]因卷帘快门相机是逐图像行进行曝光，针对某个图像行，其可能具有一个或多个

特征点，也可能不具有特征点。需要说明的是，本申请实施例中，若无特殊说明，“多个”、“多

种”等与“多”有关的数量均意指两个及两个以上。

[0030]步骤S300：获取IMU相对位姿数据。

[0031]其中，所述IMU相对位姿数据是基于从所述图像帧的曝光开始至曝光结束之间IMU

输出的IMU帧数据确定的。

[0032]对于卷帘快门相机来说，其在采集某帧图像时，是从图像的第一行开始曝光，至最

后一行曝光结束，采集的所有图像行形成图像帧。该曝光持续时间也可认为是卷帘快门相

机的读出时间。一般来说，对于承载卷帘快门相机和IMU的同一设备，其时钟同源。基于此，

可根据图像帧的曝光开始的时间至曝光结束的时间，来获取该时间段内相对应的IMU帧数

据。

[0033]相对于图像帧采集来说，IMU帧数据的采集较为高频，也即，在采集一个图像帧的

上述持续时间内通常会采集多帧IMU数据。进而，可以基于各IMU帧数据解算出对应的各IMU

帧数据对应的相对位姿，即IMU相对位姿数据。例如，在图像帧1和图像帧2的采集持续时间

中同时采集了4帧IMU数据，形成4个IMU帧数据，则可解算出该4个IMU帧数据的相对位姿。

[0034]步骤S400：基于所述IMU相对位姿数据，按照特征点所属的图像行，确定图像行IMU

相对位姿数据。

[0035]如前所述，某个图像行可能具有特征点，也可能不具有特征点。而对于具有特征点

的那些图像行，其可能具有一个特征点，也可能具有多个特征点。因一个图像行的所有特征

点均是在一个相机位姿下采集，因此，同一图像行的所有特征点的IMU相对位姿数据相同。

基于此，可以按照特征点所属的图像行来确定具有特征点的那些图像行的图像行IMU相对

位姿数据。

[0036]又因，IMU帧数据的采集相对于图像帧采集来说较为高频，因此，在确定图像行IMU

相对位姿数据时，可基于IMU帧数据与图像行的对应关系，来进行解算。例如，图像行1和2的

采集时间在第1个IMU帧数据采集之前，图像行3在第1个IMU帧数据和第2个IMU帧数据之间，

则图像行1和2对应的图像行IMU相对位姿数据可根据第1个IMU帧数据的相对位姿数据进行

处理后（如插值处理等）确定，而图像行3对应的图像行IMU相对位姿数据则可根据第2个IMU

帧数据的相对位姿数据进行处理后确定。

[0037]步骤S500：根据所述图像行IMU相对位姿数据、特征点在图像帧中的位置、根据对

IMU帧数据进行预积分确定的图像间测量项、和待估计状态量,进行视觉定位解算。

[0038]视觉定位的解算通常可通过用于进行状态估计的优化方程或约束方程等实现，在

该优化方程或约束方程中，除需要待估计状态量外,通常还需要相应的观测量和误差项。具

体到本申请实施例，该观测量和误差项可通过图像行IMU相对位姿数据、特征点在图像帧中

的位置、和所述图像间测量项体现。因此，本步骤中，通过上述多项体现的观测量和误差项、

待估计状态量，以及预设的优化方程或约束方程，即可进行视觉定位解算。

[0039]通过本实施例，针对基于卷帘快门相机采集的图像进行视觉定位的场景，为避免

卷帘快门相机在移动时，在不同时刻按图像行曝光使得每一图像行的像素在不同位姿下获

取，按照现有全局相机定位方式造成状态估计不准确的问题,首先对卷帘快门相机采集的

6

CN115564839A说明书4/18页

图像帧进行特征点提取，然后，基于IMU的相对位姿数据和各特征点所属的图像行确定各图

像行对应的图像行IMU相对位姿数据，进而基于各图像行IMU相对位姿数据、特征点在图像

帧中的位置、图像间测量项以及待估计状态量进行视觉定位解算。因IMU的高频特性，则基

于高频采集到的IMU帧数据和卷帘快门相机采用的图像帧进行位姿解算，可将及时获得的

不同IMU帧数据对应的IMU相对位姿，作为确定图像帧中各图像行位姿的依据，更有效地反

映卷帘快门相机在按图像行曝光时的位姿。因此，本实施例的方案可以进行有效且准确的

视觉定位。

[0040]实施例二

[0041]参照图2,示出了根据本申请实施例二的一种视觉定位方法的步骤流程图。

[0042]本实施例的视觉定位方法包括以下步骤：

[0043]步骤S102：获取卷帘快门相机采集的图像帧。

[0044]使用卷帘快门相机采集图像帧时，其逐图像行对像素进行曝光，每一图像行的像

素在设备的不同位姿下获取，并最终形成为一幅完整的图像帧。

[0045]步骤S104：获取所述图像帧中用于视觉定位的特征点。

[0046]在视觉定位中，特征点是图像帧里一些特别的地方,其在相机运动之后仍保持稳

定，如图像帧中具有这样特性的角点、边缘、区块等。在实际应用中，本领域技术人员可以根

据实际需求采用任意适当的方式对图像帧进行特征点提取。

[0047]步骤S106：确定各特征点所属的图像行，并对各特征点所属的图像行进行行号去

重。

[0048]逐图像行曝光的图像帧中，针对某个图像行，其可能具有一个或多个特征点，也可

能不具有特征点，本申请实施例中，获取具有特征点的那些图像行，并提取出的每个特征点

所属的图像行的行号。

[0049]针对不同的特征点，其可能位于同一图像行，也可能位于不同的图像行。例如，某

个图像行仅具有一个特征点，而另一图像行却具有五个特征点，则该五个特征点对应同样

的图像行的行号。通过确定特征点对应的图像行的行号，可为后续对相同图像行的特征点

进行统一处理提供依据。

[0050]因对于一个图像行来说，其中的像素点具有相同的曝光时间和对应的位姿，因此，

若该图像行中具有特征点，即可对该图像行中的特征点进行统一处理，以提高数据处理速

度，提升数据处理效率。为此，本申请实施例中，会对特征点对应的行号进行去重处理，以使

行号唯一。例如，特征点A对应的行号为3,特征点B对应的行号为5,特征点C对应的行号为3,

特征点D对应的行号为6,则经过行号去重处理，将得到行号3、5和6,其中，行号3同时对应有

特征点A和C。

[0051]步骤S108：获取IMU相对位姿数据。

[0052]其中，所述IMU相对位姿数据是基于从所述图像帧的曝光开始至曝光结束之间IMU

输出的IMU帧数据确定的。

[0053]相对于图像帧采集来说，IMU帧数据的采集较为高频，也即，在一个图像帧采集的

持续时间间隔内通常会采集多帧1皿数据。进而，可以基于各IMU帧数据解算出各IMU帧数据

对应的相对位姿，即IMU相对位姿数据。例如，在采集的图像帧1和图像帧2的时间间隔中同

时采集了4帧IMU数据，形成4个IMU帧数据，则可解算出该4个IMU帧数据的相对位姿。

7

CN115564839A说明书5/18页

[0054]此外，本实施例的一种可行方式中，在所述获取IMU相对位姿数据之前，还可以包

括:采集IMU帧数据，并将所述IMU帧数据分别缓存至第一缓存和第二缓存中（其中，第一缓

存和第二缓存可被称为双缓存）；基于第一缓存中的IMU帧数据，确定每一帧IMU帧数据对应

的IMU初始位姿;基于第二缓存中的IMU帧数据进行预积分，得到每一帧IMU帧数据的预积分

结果。在此情况下，所述获取IMU相对位姿数据可以包括:根据每一帧IMU帧数据对应的IMU

初始位姿和预积分结果，确定该帧IMU帧数据相对于其所对应的图像帧的采集时刻的IMU相

对位姿数据。

[0055]上述方式中，因为了确定IMU初始位姿而获取IMU帧数据的操作和IMU预积分运算

会使用同一缓存，可能会造成IMU数据冲突。为了不影响IMU预积分运算，该种方式中，当IMU

数据到达时缓存了两份，这样在进行IMU预积分运算对齐时就不会造成冲突。

[0056]在一种可行方式中，基于第二缓存中的IMU帧数据进行预积分，得到每一帧IMU帧

数据的预积分结果可以通过以下方式实现：确定IMU初始位姿相对于IMU偏置的雅可比矩

阵;根据所述IMU初始位姿和雅可比矩阵,对第二缓存中的IMU帧数据进行预积分处理,获得

每一帧IMU帧数据的预积分结果。通过对IMU帧数据进行预积分处理,可以获得两个图像帧

之间的图像间测量项，以对IMU初始位姿进行优化,获得更为准确的位姿。同时，为了节约当

IMU偏置变化时IMU预积分所需的算力消耗，在对IMU帧数据进行预积分时，可计算相对位姿

对IMU偏置的雅可比矩阵，基于该雅可比矩阵和初始位姿，即可在使用较小算力的情况下获

得准确的相对位姿。

[0057]步骤S110：基于所述IMU相对位姿数据，按照进行了行号去重后的特征点所属的图

像行，确定图像行IMU相对位姿数据。

[0058]本实施例中，因进行了行号去重处理，因此，在基于所述IMU相对位姿数据，按照特

征点所属的图像行，确定图像行IMU相对位姿数据时，可以针对去重后的图像行确定其对应

的图像行IMU相对位姿数据。如前所述，每个图像行中的所有特征点对应有相同的图像行

IMU相对位姿数据。

[0059]此外，在一种可行方式中，在确定了图像行IMU相对位姿数据之后，还可以根据各

特征点所属图像行的行号和各特征点所属图像行对应的图像行IMU相对位姿数据，生成用

于记录行号与图像行IMU相对位姿数据对应关系的数据结构。通过数据结构的形式,一方面

方便对各个特征点及其相对位姿进行管理，另一方面，也为后续使用特征点及其对应的相

对位姿的数据提供了极大的便利性。

[0060]例如，可以构建一种key-value形式的数据结构map,其中，key为行号（具体可示例

表示为imagerow）,value为图像行IMU相对位姿（具体可示例表示为pose）,形成的数据结

构示例如map〈imagerow,pose>o

[0061]步骤S112：根据所述图像行IMU相对位姿数据、特征点在图像帧中的位置、根据对

IMU帧数据进行预积分确定的图像间测量项、和待估计状态量,进行视觉定位解算。

[0062]在确定了所述图像行IMU相对位姿数据、特征点在图像帧中的位置、根据对IMU帧

数据进行预积分确定的图像间测量项、和待估计状态量后，即可确定用于视觉定位解算的

优化方程或约束方程中的各项，并基于此进行视觉定位解算。

[0063]以下，以一个具体场景示例的形式，对上述过程进行示例性说明。

[0064]本示例中，设定卷帘快门相机设置于自动驾驶车辆上，其每隔33ms采集一帧完整

8

CN115564839A说明书6/18页

的图像帧，而假设其每个图像行的曝光时长简单示例为3ms（上述33ms和3nls仅为简单示例，

本领域技术人员不应当将其理解为实际采集周期和行曝光时间），以33ms*3（3个图像帧采

集周期）期间会同时采集5帧IMU帧数据。首先对采集的图像帧A进行特征点提取并获得对应

的行号，为便于说明，本示例中将提取的特征点简化为10个,分别为特征点1T0。进而，对这

10个特征点对应的行号进行去重，得到去重后的图像行,假设为图像行1、3、7、11、23、27。在

上述对图像帧A进行处理的同时,还可分别针对5个IMU帧数据解算其对应的相对位姿，获得

5个IMU相对位姿数据。假设,5个IMU相对位姿数据分别为O、P、L、M和N（本示例中仅为简单示

意说明，本领域技术人员应当明了，位姿包括位置、速度和姿态等数据），且图像行1在图像

帧A的起始曝光时间和第1个IMU帧数据所在时间间隔内获取，图像行3和7在第1个IMU帧数

据和第2个IMU帧数据所在时间间隔内获取，图像行11在第2个IMU帧数据和第3个IMU帧数据

所在时间间隔内获取，图像行23在第3个IMU帧数据和第4个IMU帧数据所在时间间隔内获

取，图像行27在第4个IMU帧数据和第5个IMU帧数据所在时间间隔内获取。基于此，分别确定

图像行1、3、7、11、23、27对应的相对位姿即图像行IMU相对位姿数据，假设为O'、P1'、P2'、

L'、M'和N'。然后，可基于进行状态估计的方程，如基于最小二乘法进行最大后验估计的方

程,0'、P1'、P2'、L'、M'和N',特征点1-10在图像帧帧中的位置（如x、y坐标），以及在获取IMU

相对位姿数据时使用的IMU帧数据的预积分结果，来进行自动驾驶车辆的状态估计，获得状

态估计结果,该状态估计结果即为视觉定位结果，其可表征自动驾驶车辆的定位信息。

[0065]可见，通过本实施例,针对基于卷帘快门相机采集的图像进行视觉定位的场景，为

避免卷帘快门相机在移动时，在不同时刻按行曝光使得每一行的像素在不同位姿下获取，

按照现有全局相机定位方式造成状态估计不准确的问题，首先对卷帘快门相机采集的图像

帧进行特征点提取，并基于特征点的行号进行去重处理后，获得那些包含有特征点且行号

唯一的图像行;然后，基于IMU帧数据的相对位姿确定行号去重后的各图像行对应的相对位

姿,进而基于行号去重后的各图像行对应的相对位姿进行视觉定位。因IMU的高频特性，则

对高频采集到的IMU帧数据进行位姿解算，可及时地获得不同IMU帧数据对应的相对位姿,

以作为确定各图像行位姿的依据,更有效地反映卷帘快门相机在按图像行曝光时的位姿。

又因在进行了行号去重后，图像行的数量可以大大少于整个图像帧的图像行的数量,因此,

本实施例的方案不仅可以进行有效且准确地进行视觉定位，且可大大减少定位使用的数据

量,提高视觉定位效率。

[0066]实施例三

[0067]参照图3,示出了根据本申请实施例三的一种视觉定位方法的步骤流程图。

[0068]本实施例的视觉定位方法包括以下步骤：

[0069]步骤S202：获取卷帘快门相机采集的图像帧。

[0070]如前所述，卷帘快门相机逐图像行曝光,并最终形成图像帧。

[0071]步骤S204：获取图像帧用于视觉定位的特征点，并获得特征点对应的行号。

[0072]对图像帧进行特征点提取的具体实现可由本领域技术人员根据实际需求采用适

当方式实现,本申请实施例对此不作限制。在特征点确定后，特征点所在的图像行的行号即

可确定，由此，可获得各个特征点对应的行号。

[0073]步骤S206：对获得的行号进行去重，得到去重后的图像行。

[0074]不同的特征点可能对应有不同的行号，也可能对应相同的行号，基于此，可根据各

9

CN115564839A说明书7/18页

个特征点对应的行号进行行号去重，以使得最终具有特征点的图像行的行号唯一。

[0075]在视觉里程计处理过程中，为了保证优化的计算效率，所提取的特征点是比较稀

疏的，且提取或者追踪的特征点所对应的行号可能会发生重复，导致后续会进行大量的重

复运算。为此，本实施例中，对重复的行号进行剔除，即根据所提取的特征点的行号进行排

序，并剔除重复的行号，以在后续的处理中避免重复的运算，节约算力。

[0076]步骤S208：获取IMU相对位姿数据。

[0077]其中，所述IMU相对位姿数据是基于从所述图像帧的曝光开始至曝光结束之间IMU

输出的IMU帧数据确定的。

[0078]在一种可行方式中，可以根据整个图像帧的曝光开始时间和曝光结束时间（也可

认为是相机的读出时间），获取缓存中的该曝光开始时间和该曝光结束时间的IMU帧数据

（IMU帧数据具有与图像帧同源的时间信息），以实现对所述IMU帧数据的数据对齐处理，并

由此获得多个IMU帧数据;对每个IMU帧数据进行位姿估计，获得每个IMU帧数据对应的IMU

相对位姿数据。通过数据对齐,可以获得图像帧采集时间戳4到tj+r之间的IMU帧数据，其

中，％代表卷帘快门相机的读出时间。

[0079]此外，本实施例中，在所述获取IMU相对位姿数据之前，还可以包括:采集IMU帧数

据，并将所述IMU帧数据分别缓存至包括第一缓存和第二缓存的双缓存中；基于第一缓存中

的IMU帧数据，确定每一帧IMU帧数据对应的IMU初始位姿;基于第二缓存中的IMU帧数据进

行预积分，得到每一帧IMU帧数据的预积分结果；则所述获取IMU相对位姿数据包括:根据每

一帧IMU帧数据对应的IMU初始位姿和预积分结果，确定该帧IMU帧数据相对于其所对应的

图像帧的采集时刻的IMU相对位姿数据。因为在此阶段,IMU帧数据获取和IMU预积分运算会

使用同一缓存，可能会造成IMU帧数据冲突。为了不影响IMU预积分运算，该种方式中，当IMU

帧数据到达时缓存了两份，这样在进行IMU预积分运算对齐时就不会造成冲突。

[0080]在一种可行方式中，基于第二缓存中的IMU帧数据进行预积分，得到每一帧IMU帧

数据的预积分结果可以通过以下方式实现：确定IMU初始位姿相对于IMU偏置的雅可比矩

阵;根据IMU初始位姿和雅可比矩阵，对第二缓存中的IMU帧数据进行预积分处理，获得每一

帧IMU帧数据的预积分结果。通过对IMU帧数据进行预积分处理，可以获得预积分所涉及的

两个图像帧之间的测量项（即图像间测量项），以对IMU初始位姿进行优化，获得更为准确的

位姿。同时，为了节约当IMU偏置变化时IMU预积分所需的算力消耗，在对IMU帧数据进行预

积分时，可计算相对位姿对IMU偏置的雅可比矩阵，基于该雅可比矩阵和IMU初始位姿，即可

在使用较小算力的情况下获得准确的相对位姿。

[0081]一种IMU帧相对位姿计算过程中涉及的各算法如图3B所示，图3B中既涉及到每帧

IMU帧数据的IMU初始位姿的计算，也涉及到对IMU帧数据进行预积分使用到的雅可比矩阵。

该雅可比矩阵既用于结合IMU初始位姿来获得IMU相对位姿数据，也用于后续的IMU相对位

姿数据的更新。

[0082]具体地：

[0083]对于新的图像帧，首先需要计算从缓存中获取并经过数据对齐后的每一帧IMU帧

数据相对于图像帧时刻（图像采集时间）的相对位姿即IMU初始位姿.且，对于IMU的相对位

姿计算来说，还需要考虑图像帧时刻的IMU涉及的偏置状态，包括图像帧时刻的加速度计偏

置匕〃和图像帧时刻的陀螺仪偏置匕在。基于此，IMU帧数据的初始位姿的计算公式如图3B

10

CN115564839A说明书8/18页

中所示的：

1

2

[0084]Pci=Pli+%6t+^R（qu）（aci-ba.）8t

[0085]Vci=讳+R（况；）（%L%）%

1

[0086]*=琉吗抗）

[0087]其中，p%为当前IMU帧数据相对于图像帧时刻的位置，为当前IMU帧数据相对于

图像帧时刻的速度，为当前IMU帧数据相对于图像帧时刻的四元数旋转;P%为上--帧IMU

帧数据相对于图像帧时刻的位置，城为上一帧IMU帧数据相对于图像帧时刻的速度，碓为

上一帧IMU帧数据相对于图像帧时刻的四元数旋转,j为当前IMU帧数据时刻的加速度计观

测，6囚为图像帧时刻的加速度计偏置，为图像帧时刻的陀螺仪偏置，3记为当前口用帧

数据时刻的陀螺仪观测,机为两帧IMU之间的时间间隔,R(*)为四元数转为旋转矩阵操作，

③表示四元数乘法。由此，计算出每帧IMU数据的位置，速度，姿态，即：

p廉必&i={0,1,./}。对于新采集的一帧IMU帧数据来说,该位置、速度、姿态即为IMU初

始位姿。

[0088]又因IMU相对位姿数据还需要依赖预积分结果，可使用雅可比矩阵对其进行优化

和减轻数据计算量。图3B中，还示出了在对IMU进行预积分时需要计算相对位姿对偏置的雅

可比矩阵,计算公式如图3B中所示的：

8*

[0089]=-，R（q产）'及5t

8b3t

k=O

dVci

[0090]

西=(成烦

k=O

a必

[0091]=-%）八at

k=O

[0092]虹w新一（枷/

%k=o%

dPci_V加力a*八。必a

[0093]皈皈；&一以及)(以一%)西3t

k=O3I3(

其中，著表示旋转对加i的偏导数，“i为图像帧时刻的陀螺仪偏置，4表示so⑶

[0094]

的右雅可比矩阵,R(*)为四元数转为旋转矩阵操作，d+1表示第j个IMU帧数据相对于k+1时

11

CN115564839A说明书9/18页

刻的位姿；警表示速度对如的偏导数，q俵示第k个IMU帧数据相对于i时刻的位姿；学

表示速度对3j的偏导数，著表示位置对匕,・的偏导数，警表示位置对％i的偏导数,6t为

UDai

两帧IMU之间的时间间隔，j表示IMU帧的数量,ak表示第k个IMU数据帧的加速度计观测，匕

为图像帧时刻的加速度计偏置。

[0095]基于该雅可比矩阵即可对IMU帧数据进行预积分，基于预积分结果和之前的IMU初

始位姿，可获取优化后的IMU相对位姿。此外，雅可比矩阵雅还可节约当偏置变化时IMU预积

分所需的算力消耗。

[0096]步骤S210：基于IMU相对位姿数据，按照进行了行号去重后的特征点所属的图像

行，确定图像行IMU相对位姿数据。

[0097]在解算出各个IMU帧数据对应的IMU相对位姿数据后,每个图像行可以基于相对应

的IMU帧数据的IMU相对位姿数据，来确定行号去重后的各个图像行的相对位姿，即图像行

IMU相对位姿数据。

[0098]在一种可行方式中，因每个图像行的行时间（曝光时间）不同。基于此，可确定特征

点所属的图像行的行时间，所述行时间用于指示该图像行的曝光时间（也可认为是图像行

开始曝光的时间）；根据各图像行的行时间对IMU相对位姿数据进行插值处理，获得图像行

IMU相对位姿数据。

[0099]在进行了行号去重的情况下，则相应地，行号去重后的不同图像行的行时间也不

可能相同。基于此，可确定行号去重后的各图像行的行时间，所述行时间用于指示图像行的

曝光时间（也可认为是图像行开始曝光的时间）；根据各图像行的行时间对IMU相对位姿数

据进行插值处理,获得行号去重后的各图像行对应的图像行IMU相对位姿数据。

[0100]因IMU帧数据的采集时间间隔可能对应有多个图像行，而对于一个图像行来说，其

行时间通常会落入某个IMU帧数据的采集时间间隔,基于该IMU帧数据的相对位姿（即IMU相

对位姿数据）进行插值处理，可获得更为准确的图像行对应的相对位姿（即图像行IMU相对

位姿数据）。同理，若一个IMU采集时间间隔中存在多个图像行，则每个图像行基于自身的行

时间和IMU帧数据的相对位姿来确定图像行的相对位姿,也更为准确。

[0101]例如，首先根据图像帧采集时间当前图像行（行号去重后的）的行号n,图像帧

的总行数N,以及图像帧的读出时间tr,计算出当前图像行的时间=4+?*tr。然后，根

据对应的IMU帧数据的IMU相对位姿数据进行插值处理，计算出当前图像行的相对位姿（图

像行IMU相对位姿数据）%=[小欣小其中，哈表示相对姿态，p。表示相对位置。其中，

所述插值处理可以由本领域技术人员根据实际需求采用任意适当算法（包括线性插值和非

线性插值）实现，本申请实施例对此不作限制。

[0102]此外，在一种可行方式中，在确定了各图像行对应的图像行IMU相对位姿数据之

后，还可以根据各图像行的行号和各图像行对应的图像行IMU相对位姿数据，生成用于记录

行号与相对位姿对应关系的数据结构。通过数据结构的形式，一方面方便对各个特征点及

其相对位姿进行管理，另一方面,也为后续使用特征点及其对应的相对位姿的数据提供了

极大的便利性。

12

CN115564839A说明书10/18页

[0103]例如，可以构建一种key-value形式的数据结构map,其中，key为行号（具体可示例

表示为imagerow）,value为图像行IMU相对位姿（具体可示例表示为pose），形成的数据结

构示例如map〈imagerow,pose>o

[0104]步骤S212：根据所述图像行IMU相对位姿数据、特征点在图像帧中的位置、根据对

IMU帧数据进行预积分确定的图像间测量项、和待估计状态量,进行视觉定位解算。

[0105]在确定了上述各参数后，即可基于预设的优化方程或约束方程，确定与整个图像

帧对应的相对位姿,从而实现视觉定位。

[0106]一般来说,优化方程或约束方程通常需要重投影误差的信息（可根据图像行IMU相

对位姿数据和特征点在图像中的位置获得）、IMU预积分误差的信息（根据对IMU帧数据进行

预积分确定的图像间测量项）、以及边缘化的先验信息和待估计状态量。本实施例中，可选

地，可以对其中的重投影误差进行更能表征卷帘快门相机的重投影误差的改进，以获得更

为精准的视觉定位结果。

[0W7]具体地，可以根据各特征点在图像帧中的位置、及各特征点所属的图像行的图像

行IMU相对位姿数据，确定各特征点的投影位置;根据各特征点的观测位置和投影位置，确

定各特征点的重投影误差;根据重投影误差、图像间测量项和边缘化的先验信息，进行视觉

定位解算。与常规基于整个图像对应的相对位姿确定重投影误差不同，本实施例中，基于各

个特征点所属图像行的相对位姿（图像行IMU相对位姿数据）确定重投影误差，可有效避免

因卷帘快门相机逐图像行曝光带来的误差，获得更为准确的重投影误差。

[0108]在一种可行方式中，本实施例的重投影误差可通过下述公式计算：

[0109]和（端，幻=瓦与--4一

cjQ

[0113]其中瓦摩示特征点的观测位置帚表示特征点的投影位置;图表示第J帧图

像观测到第1个特征点（路标点）在图像帧的坐标，"1表示第i帧图像观测到第1个特征点

（路标点）在图像帧的坐标，[q）p目[qjpg分别表示i门两帧图像的特征点所在图像行相

对与图像帧时间（采集时间）的位姿，其中的q元素表示姿态,P元素表示位置，

出匕「口用。力分别为i,j两帧图像待估计的位姿,其中的R元素表示姿态,p元素表示位

13

CN115564839A说明书11/18页

置』传,P川为预先标定好的相机与IMU之间的外参，[R&p台表示[理,p对的逆，人]表示第1个

特征点逆深度状态，兀二（*）表示反向投影函数,用于将图像帧中的像素位置根据相机内参

参数反投影到相机的归一化平面,R（*）表示四元数和旋转矩阵之间的变换公式。

X=[%0，与,…,力，入0，入1，…，入瓶]表示全部待估计的状态，

Xk=\Pbk^k,q^,ba,bg],kC付速]为k时刻滑动时间窗口中图像帧待估计的位姿及运动状

态，C为时间窗口内至少被观测两帧以上的特征点的集合。

[0114]为了对上述重投影误差进行进一步说明，一种重投影误差计算过程中涉及的各算

法的示意如图3c所示。在计算重投影误差时，需要基于两帧图像帧对同一特征点的观测、该

两帧图像帧的待估计位姿、相机与IMU之间的外参、及两帧图像帧中的各特征点所属图像行

的图像行IMU相对位姿（图中简单示意为RSC计算出的相对位姿），使用预设的重投影误差计

算公式（图3c中圆角方框内所示）进行计算，由此获得各特征点27的重投影误差，进行获得

所有特征点的重投影误差二。7,%）。

[0115]在确定了重投影误差、IMU预积分误差确定的图像间测量项和边缘化的先验信息

后，即可基于预设的用于进行状态估计的约束方程或优化方程进行视觉定位。本实施例中，

采用的约束方程的形式如下所示：

2

[0116]argminx{||rm-HmX||+||rsfbk+2||足（染,汽）*}

keB"十1（ZJ）eC1

[0117]其中，X=…,工九,4）,入1，…,儿"表示全部待估计的状态，

Xk=[p£，u£，q£，bQ,bg],k词为k时刻滑动时间窗口中图像帧待估计的位姿及运动状

态，P牖为位置状态J同为速度状态,q4为姿态状态,ba,bg为偏置状态,A,1£[0,m]为第1

个特征点逆深度状态,m为特征点总数。％此}为边缘化的先验信息，%为IMU观

测模型的残差即预积分误差，3表示所有IMU观测数据的数据集。峰（靖，，K）为视觉重投影模

型残差即重投影误差，C为时间窗口内至少被观测两帧以上的特征点的集合。

[0118]基于以上方程可进行有效的状态估计，以获得基于卷帘快门相机的准确的视觉定

位。

[0119]此外，在一种可行方式中，为了消除偏置变化对IMU帧数据相对位姿的估算的影

响，还可以在进行所述视觉定位解算的过程中，根据当前IMU帧数据的IMU偏置变化程度确

定是否需要更新当前IMU帧数据之前的历史IMU帧数据的相对位姿;若需要更新，则对历史

IMU帧数据对应的相对位姿进行更新。具体地，可以使用IMU初始位姿相对于IMU偏置的雅可

比矩阵，对所述历史IMU帧数据对应的相对位姿进行一阶线性近似更新。

[0120]例如，对于某个IMU帧数据，可以首先根据优化后的偏置变化程度判断是否需要更

新该IMU帧数据之前的IMU帧数据（即历史IMU帧数据）对应的相对位姿，如计算优化前后加

速度偏置匕出与陀螺仪/i差的二范数值&ba.,8%丁当bb%或者b%]变化较大时，如，加速

14

CN115564839A说明书12/18页

度偏置变化b%]大于1*1。/或者陀螺仪偏置变化b/J%大于1*105,则可利用该【MU帧数据

之前的每帧IMU帧数据预积分时计算出的雅可比矩阵对该IMU帧数据之前的每帧IMU帧数据

预积分出的相对位姿进行一阶线性近似更新。

[0121]需要说明的是，上述对历史IMU帧数据进行相对位姿更新的过程，可与新的IMU帧

数据的相对位姿的解算过程并行进行。如，可以在基于当前IMU帧数据对应的IMU初始位姿

和预积分结果获得其IMU相对位姿数据的同时，对该当前IMU帧数据之前的历史IMU帧数据

进行相对位姿的一阶线性更新。

[0122]可见，通过本实施例,针对基于卷帘快门相机采集的图像进行视觉定位的场景，为

避免卷帘快门相机在移动时，在不同时刻按图像行曝光使得每一图像行的像素在不同位姿

下获取，按照现有全局相机定位方式造成状态估计不准确的问题，首先对卷帘快门相机采

集的图像帧进行特征点提取，并基于特征点的行号进行去重处理后，获得那些包含有特征

点且行号唯一的图像行;然后，基于IMU帧数据的相对位姿确定各图像行对应的相对位姿,

进而基于各图像行对应的相对位姿进行视觉定位。因IMU的高频特性，则对高频采集到的

IMU帧数据进行位姿解算，可及时地获得不同IMU帧数据对应的相对位姿，以作为确定各去

重后的图像行位姿的依据，更有效地反映卷帘快门相机在按图像行曝光时的位姿。又因在

进行了行号去重后，图像行的数量可以大大少于整个图像的图像行的数量，因此，本实施例

的方案不仅可以进行有效且准确地进行视觉定位，且可大大减少定位使用的数据量，提高

视觉定位效率。

[0123]实施例四

[0124]参照图4,示出了根据本申请实施例四的一种视觉定位装置的结构框图。

[0125]本实施例的视觉定位装置包括:行号模块302,用于获取卷帘快门相机采集的图像

帧；特征点模块304，用于获取所述图像帧用于视觉定位的特征点；IMU位姿模块306，用于获

取IMU相对位姿数据，其中，所述IMU相对位姿数据是基于从所述图像帧的曝光开始至曝光

结束之间IMU输出的IMU帧数据确定的；图像行位姿模块308,用于基于所述IMU相对位姿数

据，按照特征点所属的图像行，确定图像行IMU相对位姿数据;定位模块310,用于根据所述

图像行IMU相对位姿数据、特征点在图像帧中的位置、根据对IMU帧数据进行预积分确定的

图像间测量项、和待估计状态量,进行视觉定位解算。

[0126]可选地，图像行位姿模块308,用于确定特征点所属的图像行的行时间，所述行时

间用于指示该图像行的曝光时间；根据各图像行的行时间对IMU相对位姿数据进行插值处

理，获得图像行IMU相对位姿数据

[0127]可选地，IMU位姿模块306，还用于在所述获取IMU相对位姿数据之前，采集IMU帧数

据，并将IMU帧数据分别缓存在第一缓存和第二缓存中；基于第一缓存中的IMU帧数据，确定

每一帧IMU帧数据对应的IMU初始位姿;基于第二缓存中的IMU帧数据进行预积分，得到每一

帧IMU帧数据的预积分结果;进而，根据每一帧IMU帧数据对应的IMU初始位姿和预积分结

果，确定该帧IMU帧数据相对于其所对应的图像帧的采集时刻的IMU相对位姿数据。

[0128]可选地，IMU位姿模块306在基于第二缓存中的IMU帧数据进行预积分，得到每一帧

IMU帧数据的预积分结果时:确定所述IMU初始位姿相对于IMU偏置的雅可比矩阵;根据所述

IMU初始位姿和所述雅可比矩阵，对所述第二缓存中的IMU帧数据进行预积分处理,获得每

一帧IMU帧数据的预积分结果。

15

CN115564839A说明书13/18页

[0129]可选地，本实施例的视觉定位装置还包括：更新模块312,用于在进行所述视觉定

位解算的过程中，根据当前IMU帧数据的IMU偏置变化程度确定是否需要更新所述当前IMU

帧数据之前的历史IMU帧数据的相对位姿；若需要更新，则对所述历史IMU帧数据对应的相

对位姿进行更新。

[0130]可选地，更新模块312使用IMU初始位姿相对于IMU偏置的雅可比矩阵，对所述历史

IMU帧数据对应的相对位姿进行一阶线性近似更新。

[0131]可选地，定位模块310,用于根据各特征点在图像帧中的位置、及各特征点所属的

图