视觉惯导里程计、位姿时序偏差估计方法和定位方法与流程

1.本公开涉及定位技术领域，具体涉及视觉惯导里程计、位姿时序偏差估计方法和定位方法。


背景技术：

2.目前高精地图的采集通常使用高精度的组合惯导和激光雷达设备。由于成本较高，高精度设备不能满足高频的大范围的高精地图采集，从而较难保证地图的高频更新。廉价的视觉惯导(visual inertial，也称为视觉惯性)设备可以大规模部署，通过vio(视觉惯导里程计或者视觉惯性里程计，visual inertial odometry)、gps和三维重建，完成对高精地图要素的采集和高频更新。
3.在机器人学和计算机视觉领域，视觉里程计的定义是通过相机采集到的图像计算其位置和姿态(位姿)的过程，被广泛应用在诸如机器人之类的移动设备领域，例如火星探测车。如果视觉里程计系统同时使用惯性测量元件，这种系统通常被称为视觉惯导里程计(vio)。为了保证以vio为基础的高精地图采集系统的精度，在估计vio位姿时需要修正vio位姿的时序偏差。时序偏差指的是测量值与真值之间的偏差形成的时间序列。然而，获取vio位姿的时序偏差是困难的。


技术实现要素：

4.为了解决相关技术中的问题，本公开实施例提供视觉惯导里程计位姿时序偏差估计方法、视觉惯导里程计定位方法、装置、电子设备、可读存储介质、计算机程序产品和视觉惯导里程计。
5.第一方面，本公开实施例中提供了一种视觉惯导里程计位姿时序偏差估计方法，所述方法包括：
6.从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；
7.获取所述视觉惯导里程计的先验位姿；
8.基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差。
9.结合第一方面，本公开在第一方面的第一种实现方式中，所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，包括：
10.基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，分别确定所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项。
11.结合第一方面的第一种实现方式，本公开在第一方面的第二种实现方式中，基于
所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：
12.根据所建立的视觉特征与高精地图的数据关联，以获取与视觉特征匹配的地图三维特征；
13.获取与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标；
14.基于所述时序位姿变量、所述地图三维特征和与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束。
15.结合第一方面的第一种实现方式，本公开在第一方面的第三种实现方式中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：
16.基于指定时刻的相机的时序位姿变量和所述视觉惯导里程计的位姿偏差，利用预设函数计算指定时刻的第一预测值；
17.基于获取的指定时刻的所述视觉惯导里程计的先验位姿和所述指定时刻的第一预测值，计算所述先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束。
18.结合第一方面的一种实现方式，本公开在第一方面的第四种实现方式中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：
19.基于预设的自回归位姿偏差估计模型，利用指定时刻之前的所述视觉惯导里程计的预设数量时刻的位姿偏差计算所述视觉惯导里程计的指定时刻的位姿偏差；
20.基于计算出的所述视觉惯导里程计的指定时刻的位姿偏差和指定时刻之前的预设数量时刻的时序偏差，计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束。
21.结合第一方面、第一方面的第一种实现方式至第一方面的第四种实现方式中的任一种，本公开在第一方面的第五种实现方式中，所述估计所述相机的位姿和视觉惯导里程计位姿时序偏差，包括：
22.基于所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差，计算所述视觉惯导里程计总误差；
23.利用预设非线性优化算法，通过使得所述视觉惯导里程计位姿的总误差最小化，估计所述相机的位姿和所述视觉惯导里程计的位姿时序偏差。
24.结合第一方面，本公开在第一方面的第六种实现方式中，所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，包括：
25.基于所建立的所述视觉特征与高精地图的数据关联，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征
对所述相机的位姿变量的约束；
26.基于指定时刻的相机的时序位姿变量，计算所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束；
27.基于预设的自回归位姿偏差估计模型，利用所述视觉惯导里程计的先验位姿的偏差计算所述视觉惯导里程计的指定时刻的位姿偏差，并且计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束。
28.第二方面，本公开实施例中提供了一种视觉惯导里程计，所述视觉惯导里程计包括视觉惯导里程计位姿时序偏差估计装置，所述视觉惯导里程计位姿时序偏差估计装置包括：
29.提取和建立模块，被配置为从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；
30.获取模块，被配置为获取所述视觉惯导里程计的先验位姿；
31.估计模块，被配置为基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差。
32.第三方面，本公开实施例中提供了一种视觉惯导里程计定位方法，所述方法包括：
33.从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；
34.获取所述视觉惯导里程计的先验位姿；
35.基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差；
36.基于估计出的所述相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位。
37.第四方面，本公开实施例中提供了一种计算机程序产品，包括计算机指令，该计算机指令被处理器执行时实现如第一方面、第一方面的第一种实现方式到第六种实现方式、第三方面所述的方法。
38.本公开实施例提供的技术方案可以包括以下有益效果：
39.根据本公开实施例提供的技术方案，通过从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取所述视觉惯导里程计的先验位姿；基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
40.根据本公开实施例提供的技术方案，通过所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的
估计模型，包括：基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，分别确定所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，可以进行视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
41.根据本公开实施例提供的技术方案，通过基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：根据所建立的视觉特征与高精地图的数据关联，以获取与视觉特征匹配的地图三维特征；获取与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标；基于所述时序位姿变量、所述地图三维特征和与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
42.根据本公开实施例提供的技术方案，通过基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：基于指定时刻的相机的时序位姿变量和所述视觉惯导里程计的位姿偏差，利用预设函数计算指定时刻的第一预测值；基于获取的指定时刻的所述视觉惯导里程计的先验位姿和所述指定时刻的第一预测值，计算所述先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
43.根据本公开实施例提供的技术方案，通过基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：基于预设的自回归位姿偏差估计模型，利用指定时刻之前的所述视觉惯导里程计的预设数量时刻的位姿偏差计算所述视觉惯导里程计的指定时刻的位姿偏差；基于计算出的所述视觉惯导里程计的指定时刻的位姿偏差和指定时刻之前的预设数量时刻的时序偏差，计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，进而可以利用自回归过程对视觉惯导里程计位姿时序偏差进行建模和估计。而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
44.根据本公开实施例提供的技术方案，通过所述估计所述相机的位姿和视觉惯导里程计位姿时序偏差，包括：基于所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差，计算所述视觉惯导里程计总误差；利用预设非线性优化算法，通过使得
所述视觉惯导里程计位姿的总误差最小化，估计所述相机的位姿和所述视觉惯导里程计的位姿时序偏差，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
45.根据本公开实施例提供的技术方案，通过所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，包括：基于所建立的所述视觉特征与高精地图的数据关联，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束；基于指定时刻的相机的时序位姿变量，计算所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束；基于预设的自回归位姿偏差估计模型，利用所述视觉惯导里程计的先验位姿的偏差计算所述视觉惯导里程计的指定时刻的位姿偏差，并且计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，进而可以利用自回归过程对视觉惯导里程计位姿时序偏差进行建模和估计。而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
46.根据本公开实施例提供的技术方案，通过所述视觉惯导里程计包括视觉惯导里程计位姿时序偏差估计装置，所述视觉惯导里程计位姿时序偏差估计装置包括：提取和建立模块，被配置为从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取模块，被配置为获取所述视觉惯导里程计的先验位姿；估计模块，被配置为基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
47.根据本公开实施例提供的技术方案，通过从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取所述视觉惯导里程计的先验位姿；基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差；基于估计出的所述相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
48.应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。
附图说明
49.结合附图，通过以下非限制性实施方式的详细描述，本公开的其它特征、目的和优点将变得更加明显。在附图中：
50.图1示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的流程图；
51.图2示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的实施过程的示例性示意图；
52.图3示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的算法的示例性因子图；
53.图4示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的算法的另一示例性因子图；
54.图5示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计装置的结构框图；
55.图6示出根据本公开一实施方式的视觉惯导里程计的结构框图；
56.图7示出根据本公开一实施方式的视觉惯导里程计定位方法的流程图；
57.图8示出根据本公开一实施方式的视觉惯导里程计定位装置的结构框图；
58.图9示出根据本公开一实施方式的电子设备的结构框图；
59.图10是适于用来实现根据本公开实施方式的方法的计算机系统的结构示意图。
具体实施方式
60.下文中，将参考附图详细描述本公开的示例性实施方式，以使本领域技术人员可容易地实现它们。此外，为了清楚起见，在附图中省略了与描述示例性实施方式无关的部分。
61.在本公开中，应理解，诸如“包括”或“具有”等的术语旨在指示本说明书中所公开的标签、数字、步骤、行为、部件、部分或其组合的存在，并且不欲排除一个或多个其他标签、数字、步骤、行为、部件、部分或其组合存在或被添加的可能性。
62.另外还需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的标签可以相互组合。下面将参考附图并结合实施例来详细说明本公开。
63.为了估计vio位姿，本公开发明人考虑了以下几种方案。
64.方案1:在位姿估计的状态空间中，只考虑视觉惯导里程计vio的相机的位姿变量{x1，x2，
…
，xn}，不把视觉惯导里程计的位姿偏差{bias1，bias2，
…
，biasn}作为变量，不使用图像帧间的相对运动约束。此方案的缺点：如果vio的先验位姿上有偏差，此方案只能通过与高精地图匹配把有视觉特征约束的图像帧定位，没有视觉特征的图像帧不能定位。
65.方案2:在位姿估计的状态空间中，只考虑视觉惯导里程计vio的相机的位姿变量{x1，x2，
…
，xn}，不把视觉惯导里程计的位姿偏差{bias1，bias2，
…
，biasn}作为变量，使用图像帧间的相对运动约束。此方案的缺点：相对于方案1，该方案能通过图像帧间的相对运动约束完成没有视觉特征的图像帧的定位，但是如果图像帧间的相对运动本身是有偏差的，通过图像帧间的相对运动推算出来的vio位姿也是有偏差的。
66.方案3:在位姿估计的状态空间中，考虑视觉惯导里程计vio的相机的位姿变量
{x1，x2，
…
，xn}，同时分段考虑视觉惯导里程计的位姿偏差，通过假设一段时间或距离内位姿偏差不变，完成该段时间或距离内视觉惯导里程计的位姿偏差的标定。此方案的缺点：视觉惯导里程计的位姿偏差在时间序列上是连续变化的，不容易在时间或空间上分出偏差不变的区段。
67.考虑以上方案的缺点，本公开发明人提出了新的方案：在位姿估计的状态空间中，同时考虑相机的位姿变量{x1，x2，
…
，xn}，和视觉惯导里程计的位姿偏差{bias1，bias2，
…
，biasn}作为变量。与方案1对比，能够解决缺少视觉特征(图像与高精先验地图匹配)约束的图像帧的定位。与方案2对比，能够避免图像帧的相对运动中存在偏差导致定位偏差的问题。与方案3对比，能够连续估计视觉惯导里程计的偏差。
68.在本公开实施例的方案中，修正视觉惯导里程计位姿时序偏差的过程需要加入与高精先验地图数据关联形成的视觉约束，并且进行对视觉惯导里程计位姿时序误差的建模和估计。而且，本公开实施例的方案通过视觉惯导里程计位姿时序误差的建模和估计，不但可以对视觉惯导里程计位姿时序误差进行估计，而且可以对视觉惯导里程计的相机位姿变量进行估计，进而可以实现视觉惯导里程计的位姿估计。
69.例如，在目前地图应用在高速场景的高精地图已经基本采集完毕的情况下，需要进行地图的更新流程，通常客户对地图的更新频率要求较高，需要一种能大规模部署的高精地图更新系统。通过本公开实施例的方案，在对高精地图进行更新时，可以采用单目相机、消费级惯导和gps进行高精地图的高频更新。例如，可以将包括vio的地图更新设备安装到各种车辆上，回收数据。
70.根据本公开实施例提供的技术方案，通过从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取所述视觉惯导里程计的先验位姿；基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
71.为了解决上述问题，本公开提出视觉惯导里程计位姿时序偏差估计方法、视觉惯导里程计定位方法、装置、电子设备、可读存储介质、计算机程序产品和视觉惯导里程计。
72.图1示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的流程图。如图1所示，视觉惯导里程计位姿时序偏差估计方法包括步骤s101、s102、s103。
73.在步骤s101中，从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的。在步骤s102中，获取所述视觉惯导里程计的先验位姿。在步骤s103中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差。
74.在本公开的一个实施例中，可以根据相关技术中已知的手段完成视觉特征的提取，本公开对具体的视觉特征提取方式不做限制。本领域技术人员可以理解，视觉特征提取重要的一个特性是“可重复性”：同一场景的不同图像所提取的视觉特征应该是相同的。在本公开的一个实施例中，从图像中提取视觉特征并非指的是可以从相机获取的每一幅图像
中都可以提取视觉特征，而是可能由于场景变化等原因导致仅可以从部分图像中提取到视觉特征。因此，在本公开的一个实施例中，可以解决缺少视觉特征(图像与高精先验地图匹配)约束的图像帧的定位的问题。
75.在本公开的一个实施例中，修正视觉惯导里程计位姿时序误差的过程需要加入视觉特征与高精(先验)地图数据关联形成的视觉约束。建立视觉特征与高精地图的数据关联的方式可以从相关技术中获取，本公开对此不做限制。
76.在本公开的一个实施例中，视觉惯导里程计的先验位姿指的是在进行视觉惯导里程计位姿时序偏差估计时以预设方式获取的作为先验值的视觉惯导里程计的位姿和位姿的偏差，其具体获取方式可以从相关技术中获取，本公开对此不做限制。
77.在本公开的一个实施例中，步骤s103包括：基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，分别确定所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项。
78.在本公开的一个实施例中，在视觉惯导里程计位姿时序偏差的估计模型中，视觉特征重投影误差、先验位姿误差、位姿偏差误差是为了完成视觉惯导里程计时序误差的连续估计而需要优化的总的误差的组成部分。
79.根据本公开实施例提供的技术方案，通过所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，包括：基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，分别确定所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，可以进行视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
80.以下参照图2描述根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的实施过程。图2示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的实施过程的示例性示意图。
81.如图2所示，步骤201、202和203是构建视觉特征重投影误差的过程。在步骤201，从相机获取的图像(图像帧)中进行图像视觉特征提取，例如，从图像中提取2d视觉特征。在步骤202，将视觉特征与高精地图的数据关联，例如，生成图像2d视觉特征与高精地图3d元素的匹配关系。高精地图3d元素也可以称为3d视觉特征，可包含点、线、面、边缘特征等。在步骤203，进行重投影误差构建。
82.在本公开的一个实施例中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：根据所建立的视觉特征与高精地图的数据关联，以获取与视觉特征匹配的地图三维特征；获取与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标；基于所述时序位姿变量、所述地图三维特征和与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特
征对所述相机的位姿变量的约束。
83.在本公开的一个实施例中，可以采用下式(1)构建视觉特征重投影误差error
reprojection
:
84.error
reprojection
＝proj(xk,landmarksk)-pixel
map,k
ꢀꢀ
(1)
85.其中，xk为时刻k的时序位姿变量；landmarksk为时刻k的视觉特征(二维视觉特征)与高精地图数据关联后，匹配好的地图三维特征(高精地图3d元素)；proj为可以从相关技术中获取的重投影函数；pixel
map,k
是时刻k的图像上与地图三维特征landmarksk匹配的视觉特征像素坐标。
86.根据本公开实施例提供的技术方案，通过基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：根据所建立的视觉特征与高精地图的数据关联，以获取与视觉特征匹配的地图三维特征；获取与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标；基于所述时序位姿变量、所述地图三维特征和与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
87.如图2所示，步骤204和205是构建视觉惯导里程计的先验位姿误差的过程。在步骤204，获取视觉惯导里程计vio先验位姿。在步骤205，构建视觉惯导里程计vio先验位姿误差。
88.在本公开的一个实施例中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：基于指定时刻的相机的时序位姿变量和所述视觉惯导里程计的位姿偏差，利用预设函数计算指定时刻的第一预测值；基于获取的指定时刻的所述视觉惯导里程计的先验位姿和所述指定时刻的第一预测值，计算所述先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束。
89.在本公开的一个实施例中，以采用如下式(2)构建视觉惯导里程计的先验位姿误差error
vio
：
90.error
vio
＝x
vio,k-compose(xk,biask)
ꢀꢀ
(2)
91.其中，x
vio,k
为视觉惯导里程计k时刻的先验位姿；xk为时刻k的相机位姿变量；biask为视觉惯导里程计k时刻的位姿偏差；compose是相关技术中已知的复合运算，例如，se(3)群元素的复合运算，可以得到se(3)状态的预测值，即，前述的第一预测值。在本公开的一个实施例中，位姿偏差biask可以是利用k时刻之前预设数量的连续时刻的位姿偏差通过自回归过程计算出的，其具体计算方式可以参照以下针对位姿偏差误差的计算过程。
92.根据本公开实施例提供的技术方案，通过基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：基于指定时刻的相
机的时序位姿变量和所述视觉惯导里程计的位姿偏差，利用预设函数计算指定时刻的第一预测值；基于获取的指定时刻的所述视觉惯导里程计的先验位姿和所述指定时刻的第一预测值，计算所述先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
93.如图2所示，步骤206和207是构建视觉惯导里程计的位姿偏差误差的过程，具体而言，利用自回归过程构建视觉惯导里程计的位姿偏差误差。在步骤206，识别自回归过程的参数。在步骤207，构建视觉惯导里程计的位姿偏差误差。
94.在本公开的一个实施例中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：基于预设的自回归位姿偏差估计模型，利用指定时刻之前的所述视觉惯导里程计的预设数量时刻的位姿偏差计算所述视觉惯导里程计的指定时刻的位姿偏差；基于计算出的所述视觉惯导里程计的指定时刻的位姿偏差和指定时刻之前的预设数量时刻的时序偏差，计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束。
95.在本公开的一个实施例中，可以基于以下式(3)中示出的预设的自回归位姿偏差估计模型，基于指定时刻之前的视觉惯导里程计的预设数量时刻的位姿偏差bias
k-i
估计视觉惯导里程计的指定时刻k的位姿偏差biask：
[0096][0097]
其中，式(3)中的自回归过程是离散化的p阶高斯马尔科夫过程。其中，ai是相关性系数；b0·
βk是自回归过程的驱动噪声，b0是驱动噪声的系数，βk是噪声，服从n(0,1)且是独立同分布的。阶数为p的马尔科夫性质使得k时刻的位姿偏差biask只依赖过去k-1到k-p时刻的状态。因此该高斯马尔科夫过程也叫做自回归过程，即k时刻的状态可以通过k时刻前的p个状态通过一个回归方程来预测。如果biask状态本身是一个向量，回归方程也可以写作一个向量化的回归过程，其具体形式可以从相关技术中获取，本公开在此不作赘述。
[0098]
在本公开的一个实施例中，可以通过以下式(4)，基于计算出的视觉惯导里程计的指定时刻k的位姿偏差biask和指定时刻之前的预设数量时刻的位姿偏差bias
k-i
，计算视觉惯导里程计的位姿偏差误差error
ar
作为自回归位姿偏差估计模型，即，式(3)的计算过程的约束：
[0099][0100]
其中，ai是相关性系数。
[0101]
从以上描述可知，利用式(3)估计指定时刻k的位姿偏差biask的自回归过程需要满足视觉惯导里程计的位姿偏差误差error
ar
的约束。从某种角度看，自回归过程的驱动噪
声b0·
βk是视觉惯导里程计的位姿偏差误差error
ar
的不确定性的体现。在本公开的一个实施例中，可以不考虑这种不确定性先将位姿偏差误差error
ar
算出来，然后再用这个不确定性去度量这个位姿偏差误差error
ar
。
[0102]
根据本公开实施例提供的技术方案，通过基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：基于预设的自回归位姿偏差估计模型，利用指定时刻之前的所述视觉惯导里程计的预设数量时刻的位姿偏差计算所述视觉惯导里程计的指定时刻的位姿偏差；基于计算出的所述视觉惯导里程计的指定时刻的位姿偏差和指定时刻之前的预设数量时刻的时序偏差，计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，进而可以利用自回归过程对视觉惯导里程计位姿时序偏差进行建模和估计。而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
[0103]
在本公开的一个实施例中，计算视觉特征重投影误差的过程、计算先验位姿误差的过程、计算位姿偏差误差的过程可以是在运行视觉惯导里程计位姿时序偏差的估计模型时并行执行(如图2所示)的，也可以根据实际情况按特定顺序执行，本公开对此不做限制。
[0104]
在本公开的一个实施例中，所述自回归位姿偏差估计模型的计算过程是高斯马尔科夫过程。
[0105]
根据本公开实施例提供的技术方案，通过所述自回归位姿偏差估计模型的计算过程是高斯马尔科夫过程，可以利用高斯马尔科夫过程对视觉惯导里程计位姿时序偏差进行建模和估计。而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
[0106]
如图2所示，可以基于计算出的视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差构建优化方程，该优化方程从整体上优化的视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差。
[0107]
在本公开的一个实施例中，步骤s103包括：基于所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差，计算所述视觉惯导里程计总误差；利用预设非线性优化算法，通过使得所述视觉惯导里程计位姿的总误差最小化，估计所述相机的位姿和所述视觉惯导里程计的位姿时序偏差。
[0108]
根据本公开实施例提供的技术方案，通过所述估计所述相机的位姿和视觉惯导里程计位姿时序偏差，包括：基于所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差，计算所述视觉惯导里程计总误差；利用预设非线性优化算法，通过使得所述视觉惯导里程计位姿的总误差最小化，估计所述相机的位姿和所述视觉惯导里程计的位姿时序偏差，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
[0109]
在本公开的一个实施例中，利用以下式(5)，基于视觉惯导里程计的视觉特征重投影误差error
reprojection
、先验位姿误差error
vio
、位姿偏差误差error
ar
，计算视觉惯导里程计
总误差error：
[0110][0111]
在本公开的一个实施例中，利用以下式(6)，通过进行使得视觉惯导里程计总误差error最小化的计算，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差{bias1，bias2，
…
，biasn}：
[0112]
{x1,x2,
…
,xn,bias1,bias2,
…
,biasn}＝argmin
x,bias∈se(3)
error
ꢀꢀ
(6)
[0113]
其中，argmin指的是在x,bias∈se(3)的情况下，使得视觉惯导里程计总误差error最小化(最优化)。在此情况下，求出的使得视觉惯导里程计总误差error最小化的视觉惯导里程计的相机的位姿变量{x1，x2，
…
，xn}和视觉惯导里程计的位姿时序偏差{bias1，bias2，
…
，biasn}即为所需的相机的位姿变量{x1，x2，
…
，xn}和视觉惯导里程计的位姿时序偏差{bias1，bias2，
…
，biasn}。即，计算出使得总误差error最小化(最优化)的视觉惯导里程计的相机的位姿变量和视觉惯导里程计的位姿时序偏差。尽管本公开实施例中提及的视觉惯导里程计位姿时序偏差的估计模型是为了求得视觉惯导里程计位姿时序偏差，但是求解时也会求解出相机的位姿变量。
[0114]
在本公开的一个实施例中，式(6)可以被认为是在使得所述视觉特征对所述相机的位姿变量的约束、所述视觉惯导里程计的先验位姿的约束、所述自回归位姿偏差估计模型的计算过程的约束整体上最优化的情况下，估计相机的位姿变量和视觉惯导里程计的位姿时序偏差两种变量。其中，通过对视觉惯导里程计位姿的偏差的建模并且估计(利用前述自回归过程)位姿时序偏差是本公开实施例方案能够实现视觉惯导里程计的位姿时序偏差估计的要点。
[0115]
如图2所示，利用式(6)，步骤209可以估计视觉惯导里程计的相机的位姿和视觉惯导里程计的位姿时序偏差。
[0116]
在本公开的一个实施例中，式(6)构建了一个非线性优化问题，解法包括但不限于：高斯牛顿法，dogleg最优化方法，共轭梯度下降法等优化算法。
[0117]
应该理解，本公开实施例中提及的视觉惯导里程计位姿时序偏差的估计模型不仅有视觉惯导里程计的位姿偏差作为变量，还有相机的位姿变量作为变量。在本公开的一个实施例中，视觉惯导里程计位姿时序偏差的估计模型可以被认为是由式(1)至(6)构建的一个方程组。式(1)构建所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束，其中包括相机的位姿变量作为变量。式(2)计算先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束，其中包括相机的位姿变量和视觉惯导里程计的位姿偏差作为变量。式(4)计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的式(3)所示的所述自回归位姿偏差估计模型的计算过程的约束，其中包括视觉惯导里程计的位姿偏差作为变量。在利用式(5)和(6)估计相机的位姿变量和视觉惯导里程计的位姿时序偏差两种变量时，要在整体上使得这些作为约束的误差最小化来进行求解。应该理解，前述具体的式(1)至(6)仅仅是构建视觉惯导里程计位姿时序偏差的估计模型的示例性算式，根据本公开实
施例的教导，本领域技术人员可以采取其他算式来计算与视觉惯导里程计的位姿相关的前述误差作为约束项。在此情况下，式(1)至(6)可以被其他算式替代。还应该理解，根据本公开实施例的教导，本领域技术人员可以采取其他算式来计算与视觉惯导里程计的位姿相关的其他误差作为约束项，只要建立将相机的位姿变量和视觉惯导里程计的位姿时序偏差两种变量作为变量视觉惯导里程计位姿时序偏差的估计模型即可。
[0118]
在本公开的一个实施例中，步骤s103包括：基于所建立的所述视觉特征与高精地图的数据关联，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束；基于指定时刻的相机的时序位姿变量，计算所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束；基于预设的自回归位姿偏差估计模型，利用所述视觉惯导里程计的先验位姿的偏差计算所述视觉惯导里程计的指定时刻的位姿偏差，并且计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束；估计视觉惯导里程计位姿时序偏差。
[0119]
在本公开的一个实施例中，虽然提及需要实现的是估计视觉惯导里程计位姿时序偏差，但是，重要的目的是为了实现视觉惯导里程计的位姿估计，或定位。因此，建立的视觉惯导里程计位姿时序偏差的估计模型不但可以估计出视觉惯导里程计位姿时序偏差，还可以估计出相机的时序位姿，进而估计视觉惯导里程计的位姿估计，即，对视觉惯导里程计定位。
[0120]
以下参照图3描述本公开一实施例中的算法的因子图。图3示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的算法的示例性因子图300。
[0121]
如图3所示，模块301为视觉特征重投影误差，作为视觉特征对相机的位姿变量的约束。模块302为相机的位姿变量。模块303为视觉惯导里程计的先验位姿误差，作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束。模块304代表视觉惯导里程计的时序误差。模块305代表视觉惯导里程计的位姿偏差误差，作为所述自回归位姿偏差估计模型的计算过程的自回归约束。
[0122]
根据本公开实施例提供的技术方案，通过所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，包括：基于所建立的所述视觉特征与高精地图的数据关联，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束；基于指定时刻的相机的时序位姿变量，计算所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束；基于预设的自回归位姿偏差估计模型，利用所述视觉惯导里程计的先验位姿的偏差计算所述视觉惯导里程计的指定时刻的位姿偏差，并且计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束；估计视觉惯导里程计位姿时序偏差，可以用于视觉惯导里程计位姿时序偏差的整体性误差优化，进而可以利用自回归过程对视觉惯导里程计位姿时序偏差进行建模和估计。而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进
而精确地估计视觉惯导里程计的位姿。
[0123]
在本公开的一个实施例中，步骤s103包括：对所述视觉惯导里程计位姿时序偏差设定时间窗口，使得特定时间窗口内的所述相机的位姿变量对应于同一个先验位姿的偏差。根据本公开实施例提供的技术方案，通过对所述视觉惯导里程计位姿时序偏差设定时间窗口，使得特定时间窗口内的所述相机的位姿变量对应于同一个先验位姿的偏差，可以降低利用视觉惯导里程计位姿时序偏差的估计模型估计出相机的位姿和视觉惯导里程计位姿时序偏差的计算量。
[0124]
以下参照图4描述本公开另一实施例中的算法的因子图。图4示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计方法的算法的另一示例性因子图400。
[0125]
如图4所示，模块401为视觉特征重投影误差，作为视觉特征对相机的位姿变量的约束。模块402为相机的位姿变量。模块403为视觉惯导里程计的先验位姿误差，作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束。模块404代表视觉惯导里程计的时序误差。模块405代表视觉惯导里程计的位姿偏差误差，作为所述自回归位姿偏差估计模型的计算过程的自回归约束。图4所示的算法为图3所示的算法的一个改进，区别在于图4中的算法可以对时序误差设定时间窗口406，使一定时间窗口406里的位姿变量共用同一个先验位姿的误差变量，减少优化问题的自由度。进而可以减小本公开实施例中的利用视觉惯导里程计位姿时序偏差的估计模型估计出相机的位姿和视觉惯导里程计位姿时序偏差的计算量。
[0126]
本公开实施例的方案可以处理但不限于处理视觉惯导里程计位姿的时序偏差，也可以用于处理其他位姿估计算法的时序偏差。本公开实施例的方案对位姿时序误差的建模可以从自回归过程扩展为自回归滑动平均过程，或者基于recurrent neural network的深度神经网络模型和其它对时序过程建模的数学方法。本公开实施例的方案的位姿估计算法可用于但不限于对相机位姿的估计，也可用于lidar、radar等其它传感器相关的位姿估计问题。
[0127]
本公开实施例的方案采用对视觉惯导里程计的位姿时序偏差建立时序模型，结合高精先验地图和视觉特征，可以实现对相机位姿的高精连续估计。
[0128]
以下参照图5描述根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计装置。图5示出根据本公开一实施方式的视觉惯导里程计位姿时序偏差估计装置500的结构框图。
[0129]
如图5所示，视觉惯导里程计位姿时序偏差估计装置500包括：提取和建立模块501、获取模块502、估计模块503。提取和建立模块501被配置为从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的。获取模块502被配置为获取所述视觉惯导里程计的先验位姿。估计模块503被配置为基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差。根据本公开实施例提供的技术方案，通过提取和建立模块，被配置为从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取模块，被配置为获取所述视觉惯导里程计的先验位姿；估计模块，被配置为基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉
惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
[0130]
本领域技术人员可以理解，参照图5描述的技术方案的可以与参照图1至图4描述的实施例结合，从而具备参照图1至图4描述的实施例所实现的技术效果。具体内容可以参照以上根据图1至图4进行的描述，其具体内容在此不再赘述。
[0131]
以下参照图6描述根据本公开一实施方式的视觉惯导里程计。图6示出根据本公开一实施方式的视觉惯导里程计600的结构框图。
[0132]
如图6所示，视觉惯导里程计600包括用于获取图像的相机601和视觉惯导里程计位姿时序偏差估计装置500，视觉惯导里程计位姿时序偏差估计装置500与图5所示的视觉惯导里程计位姿时序偏差估计装置500相同。
[0133]
根据本公开实施例提供的技术方案，通过所述视觉惯导里程计包括视觉惯导里程计位姿时序偏差估计装置，所述视觉惯导里程计位姿时序偏差估计装置包括：提取和建立模块，被配置为从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取模块，被配置为获取所述视觉惯导里程计的先验位姿；估计模块，被配置为基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
[0134]
本领域技术人员可以理解，参照图6描述的技术方案的可以与参照图1至图5描述的实施例结合，从而具备参照图1至图5描述的实施例所实现的技术效果。具体内容可以参照以上根据图1至图5进行的描述，其具体内容在此不再赘述。
[0135]
以下参照图7描述根据本公开一实施方式的视觉惯导里程计定位方法。图7示出根据本公开一实施方式的视觉惯导里程计定位方法的流程图。在本公开的一个实施例中，所述视觉惯导里程计包括用于获取图像的相机。如图7所示，视觉惯导里程计定位方法包括步骤s701、s702、s703、s704。
[0136]
在步骤s701中，从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的。在步骤s702中，获取所述视觉惯导里程计的先验位姿。在步骤s703中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差。在步骤s704中，基于估计出的所述相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位。
[0137]
在根据本公开实施例估计所述相机的位姿和所述视觉惯导里程计的位姿时序偏差的情况下，基于估计出的相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位。
[0138]
根据本公开实施例提供的技术方案，通过从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取所述视觉惯
导里程计的先验位姿；基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差；基于估计出的所述相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
[0139]
本公开实施例的方案在解决视觉惯导里程计定位问题时，可以将位姿估计系统(视觉惯导里程计)的位姿的偏差独立出来作为变量，并在时序上建立自回归模型，实现在缺少视觉特征时也能完成对视觉惯导里程计的定位。该定位方案可以用于高精地图更新的视觉惯导里程计定位算法。
[0140]
本领域技术人员可以理解，参照图7描述的技术方案的可以与参照图1至图6描述的实施例结合，从而具备参照图1至图6描述的实施例所实现的技术效果。具体内容可以参照以上根据图1至图6进行的描述，其具体内容在此不再赘述。
[0141]
以下参照图8描述根据本公开一实施方式的视觉惯导里程计定位装置。图8示出根据本公开一实施方式的视觉惯导里程计定位装置800的结构框图。
[0142]
如图8所示，视觉惯导里程计定位装置800包括：提取和建立模块801，被配置为从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取模块802，被配置为获取所述视觉惯导里程计的先验位姿；估计模块803，被配置为基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差；定位模块804，被配置为基于估计出的所述相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位。
[0143]
根据本公开实施例提供的技术方案，通过提取和建立模块，被配置为从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取模块，被配置为获取所述视觉惯导里程计的先验位姿；估计模块，被配置为基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差；定位模块，被配置为基于估计出的所述相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位，可以连续地对视觉惯导里程计的位姿偏差进行估计，而且利用视觉惯导里程计位姿时序偏差的估计模型一起估计出相机的位姿和视觉惯导里程计位姿时序偏差，进而精确地估计视觉惯导里程计的位姿。
[0144]
图9示出根据本公开一实施方式的电子设备的结构框图。
[0145]
本公开实施方式还提供了一种电子设备，如图9所示，包括至少一个处理器901；以及与至少一个处理器901通信连接的存储器902；其中，存储器902存储有可被至少一个处理器901执行的指令，指令被至少一个处理器901执行以实现以下步骤：
[0146]
从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取所述视觉惯导里程计的先验位姿；基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差。
[0147]
在本公开的一个实施例中，所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型包括：基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，分别确定所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项。
[0148]
在本公开的一个实施例中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：根据所建立的视觉特征与高精地图的数据关联，以获取与视觉特征匹配的地图三维特征；获取与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标；基于所述时序位姿变量、所述地图三维特征和与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束。
[0149]
在本公开的一个实施例中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：
[0150]
基于指定时刻的相机的时序位姿变量和所述视觉惯导里程计的位姿偏差，利用预设函数计算指定时刻的第一预测值；基于获取的指定时刻的所述视觉惯导里程计的先验位姿和所述指定时刻的第一预测值，计算所述先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束。
[0151]
在本公开的一个实施例中，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：基于预设的自回归位姿偏差估计模型，利用指定时刻之前的所述视觉惯导里程计的预设数量时刻的位姿偏差计算所述视觉惯导里程计的指定时刻的位姿偏差；基于计算出的所述视觉惯导里程计的指定时刻的位姿偏差和指定时刻之前的预设数量时刻的时序偏差，计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束。
[0152]
在本公开的一个实施例中，所述估计所述相机的位姿和视觉惯导里程计位姿时序偏差，包括：基于所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差，计算所述视觉惯导里程计总误差；利用预设非线性优化算法，通过使得所述视觉惯导里程计位姿的总误差最小化，估计所述相机的位姿和所述视觉惯导里程计的位姿时序偏差。
[0153]
在本公开的一个实施例中，所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，包括：基于所建立的所述视觉特征与高精地图的数据关联，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束；基于指定时刻的相机的时序位姿变量，计算所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束；基于预设的自回归位姿偏差估计模型，
利用所述视觉惯导里程计的先验位姿的偏差计算所述视觉惯导里程计的指定时刻的位姿偏差，并且计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束。
[0154]
本公开实施方式还提供了一种电子设备，如图9所示，包括至少一个处理器901；以及与至少一个处理器901通信连接的存储器902；其中，存储器902存储有可被至少一个处理器901执行的指令，指令被至少一个处理器901执行以实现以下步骤：从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；获取所述视觉惯导里程计的先验位姿；基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差；基于估计出的所述相机的位姿和视觉惯导里程计位姿时序偏差，对所述视觉惯导里程计进行定位。
[0155]
如图10所示，计算机系统1000包括处理单元1001，其可以根据存储在只读存储器(rom)1002中的程序或者从存储部分1008加载到随机访问存储器(ram)1003中的程序而执行上述附图所示的实施方式中的各种处理。在ram1003中，还存储有系统1000操作所需的各种程序和数据。cpu1001、rom1002以及ram1003通过总线1004彼此相连。输入/输出(i/o)接口1005也连接至总线1004。
[0156]
以下部件连接至i/o接口1005：包括键盘、鼠标等的输入部分1006；包括诸如阴极射线管(crt)、液晶显示器(lcd)等以及扬声器等的输出部分1007；包括硬盘等的存储部分1008；以及包括诸如lan卡、调制解调器等的网络接口卡的通信部分1009。通信部分1009经由诸如因特网的网络执行通信处理。驱动器1010也根据需要连接至i/o接口1005。可拆卸介质1011，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器1010上，以便于从其上读出的计算机程序根据需要被安装入存储部分1008。其中，所述处理单元1001可实现为cpu、gpu、tpu、fpga、npu等处理单元。
[0157]
特别地，根据本公开的实施方式，上文参考附图描述的方法可以被实现为计算机软件程序。例如，本公开的实施方式包括一种计算机程序产品，其包括有形地包含在及其可读介质上的计算机程序，所述计算机程序包含用于执行附图中的方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分1009从网络上被下载和安装，和/或从可拆卸介质1011被安装。例如，本公开的实施方式包括一种可读存储介质，其上存储有计算机指令，该计算机指令被处理器执行时实现用于执行附图中的方法的程序代码。
[0158]
附图中的流程图和框图，图示了按照本公开各种实施方式的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0159]
描述于本公开实施方式中所涉及到的单元或模块可以通过软件的方式实现，也可
以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定。
[0160]
作为另一方面，本公开还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施方式中所述节点中所包含的计算机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，所述程序被一个或者一个以上的处理器用来执行描述于本公开的方法。
[0161]
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。