定位方法、装置及计算机存储介质与流程

1.本技术实施例涉及视觉定位技术领域，尤其涉及一种定位方法、装置及计算机存储介质。


背景技术：

2.定位技术是增强现实(ar)、自动驾驶、机器人等领域普遍需要的关键技术。在定位技术中，视觉定位技术因其设备成本低、技术相对成熟而被广泛使用。
3.在视觉定位技术中，视觉同步定位与建图(v-simultaneous localization and mapping，v-slam)相比于卫星定位、网络定位等技术，可以摆脱对卫星信号或者网络通信的依赖，实现对处于未知环境中的设备的自身位置、姿态和环境中其他要素的位置等进行状态估计，实时获得定位结果及环境地图。
4.v-slam技术，需要依赖视觉传感器(如相机)、惯导和里程计等传感器输出的测量数据，当v-slam的当前观测与环境地图出现无法匹配的问题，在后续帧继续尝试与之前建立的地图进行匹配，并求解相机位姿，该过程称为重定位(re-localization)。若重定位匹配成功，则可在跟丢前的世界坐标系下继续对相机的位姿和环境地图进行状态估计。但是，现有的重定位方案存在鲁棒性和精度不高的问题。


技术实现要素：

5.有鉴于此，本技术实施例提供一种定位方案，以至少部分解决上述问题。
6.根据本技术实施例的第一方面，提供了一种定位方法，包括：根据异常检测结果确定发生视觉定位异常，如果发生，则确定异常发生前的常规图像帧，并获取异常发生后的异常图像帧；基于所述常规图像帧对常规地图点的观测数据、以及所述常规图像帧和所述异常图像帧中各相邻帧间的惯导观测数据，确定状态常量；根据所述状态常量和所述异常图像帧对异常地图点的观测数据，进行状态估计；根据状态估计的结果，进行视觉重定位。
7.根据本技术实施例的第二方面，提供了一种定位装置，包括：异常确定模块，用于根据异常检测结果确定发生视觉定位异常，如果发生，则确定异常发生前的常规图像帧，并获取异常发生后的异常图像帧；常量确定模块，用于基于所述常规图像帧对常规地图点的观测数据、以及所述常规图像帧和所述异常图像帧中各相邻帧间的惯导观测数据，确定状态常量；状态估计模块，用于根据所述状态常量和所述异常图像帧对异常地图点的观测数据，进行状态估计；重定位模块，用于根据状态估计的结果，进行视觉重定位。
8.根据本技术实施例的第三方面，提供了一种电子设备，包括：处理器、存储器、通信接口和通信总线，所述处理器、所述存储器和所述通信接口通过所述通信总线完成相互间的通信；所述存储器用于存放至少一可执行指令，所述可执行指令使所述处理器执行如第一方面所述的定位方法对应的操作。
9.根据本技术实施例的第四方面，提供了一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面所述的定位方法。
10.根据本技术实施例提供的定位方案，与常规出现视觉定位异常后，基于异常图像帧进行视觉重定位处理不同，本技术实施例中保留了异常发生前的常规图像帧，这部分常规图像帧被保留直至视觉重定位成功后且重定位帧达到预设数量。并且，基于这部分常规图像帧对应的相关数据以及各帧间的惯导观测数据，确定状态常量，以作为异常发生后进行状态估计时的重要参考因素，从而可以避免因视觉定位发生异常而导致的地图精度、惯导观测数据出现降低或偏差，而导致视觉重定位结果与真实情况偏差较大，鲁棒性和精度不高，无法满足相关应用的定位需求的现象。
附图说明
11.为了更清楚地说明本技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。
12.图1为根据本技术实施例一的一种重定位方法的步骤流程图；
13.图2a为根据本技术实施例二的一种重定位方法的步骤流程图；
14.图2b为图2a所示实施例中的一种重定位的流程架构示意图；
15.图2c为图2a所示实施例中的一种重定位第一阶段的示意图；
16.图2d为图2a所示实施例中的一种重定位第二阶段的示意图；
17.图3为根据本技术实施例三的一种重定位装置的结构框图；
18.图4为根据本技术实施例四的一种电子设备的结构示意图。
具体实施方式
19.为了使本领域的人员更好地理解本技术实施例中的技术方案，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本技术实施例一部分实施例，而不是全部的实施例。基于本技术实施例中的实施例，本领域普通技术人员所获得的所有其他实施例，都应当属于本技术实施例保护的范围。
20.下面结合本技术实施例附图进一步说明本技术实施例具体实现。
21.实施例一
22.参照图1，示出了根据本技术实施例一的一种定位方法的步骤流程图。
23.本实施例的定位方法包括以下步骤：
24.步骤s102：根据异常检测结果确定发生视觉定位异常，如果发生，则确定异常发生前的常规图像帧，并获取异常发生后的异常图像帧。
25.如前所述，在视觉定位中，经常可能因为图像采集设备如相机等因遮挡、抖动等导致视觉定位异常，因此，需要设定一定的规则对是否发生视觉定位异常进行检测，例如，实时检测、周期性检测、根据预设的触发条件检测等等。具体的异常检测规则可由本领域技术人员根据实际需求适当设定，如，相同响应阈值下当前图像帧提取到特征点数量是否少于某一阈值(如50个)、当前图像帧有效跟踪到地图中特征点的数量是否少于某一阈值(如30个)等等。
26.在确定发生了视觉定位异常的情况下，则需要进行相应的视觉重定位处理，以使
搭载有相应传感器(如相机、imu等)的仪器可以在异常情况下也能进行较为准确的状态估计和定位。
27.本实施例中，在确定发生了视觉定位异常，需要进行视觉重定位时，首先固定异常发生前的常规图像帧，并获取异常发生后的异常图像帧。
28.其中，固定异常发生前的常规图像帧可以是固定异常发生前预设时间段内的常规图像帧，或者，固定异常发生前预设帧数的常规图像帧，所述预设时间段和预设帧数均可由本领域技术人员根据实际需求适当设定。因滑动时间窗口法在视觉定位中广泛使用，因此，该预设时间段可以设定为等于或小于该时间窗口的时间段，或者，该预设帧数可以等于或小于该时间窗口对应的帧数。例如，以11帧为时间窗口长度，则随着图像帧的采集，时间窗口不断向前滑动，若a1-a11均为正常图像帧，也即常规图像帧，随着时间窗口移动a1移出时间窗口，新采集的a12移入时间窗口，若a12为异常图像帧，则此时，可固定a2-a10，并继续采集异常图像帧。此种情况下，时间窗口由固定长度变化为变长，其变长的长度可根据异常图像帧的数量确定。例如，若a12-a18为异常图像帧，则变长的时间窗口内容纳a2-a18共17个图像帧，也即，此时，该变长的时间窗口的长度在视觉定位异常情况下可变长为至少容纳17帧以上的长度。
29.需要说明的是，本技术实施例中，常规图像帧意指可以连续稳定地进行特征点跟踪，并实现视觉定位如实现v-slam的帧。而异常图像帧意指自异常情况发生起，至重定位成功或重定位失败止的图像帧。
30.步骤s104：基于常规图像帧对常规地图点的观测数据、以及常规图像帧和异常图像帧中各相邻帧间的惯导观测数据，确定状态常量。
31.本技术实施例中，使用相机 惯导(惯性导航)的视觉定位方式，惯导观测数据可通过imu(惯性测量单元)获得。
32.在视觉定位前期技术中，通常基于运动模型和观测模型来进行状态估计，这两个模型可抽象化表示为下述两个方程的形式：
[0033][0034]
其中，上面方程为运动方程，下面方程为观测方程。xk表示搭载有运动传感器如imu的仪器在第k时刻的位姿，x
k-1
表示所述仪器在第k-1时刻的位姿，uk表示运动传感器的读数或输入，wk表示噪声，f表示用于进行位姿估计的一般函数。z
k，j
表示所述仪器在xk位置上针对路标点yj产生的观测数据，v
k，j
表示观测里的噪声，h表示用于进行观测的一般函数。k表示时刻的集合，o也为一个集合，其记录有哪个时刻观测到了哪个路标的信息。状态估计即为基于上述方程，通过观测量(包括u、w、z、v)计算出最适当的x和y的过程。但随着视觉定位技术的发展，越来越多的方案采用优化方程或约束方程的形式来进行状态估计。例如，采用非线性最小二乘法方式、对极方式、icp方式、pnp方式等进行状态估计。这种方式可综合更多的信息，获得更为精准的状态估计结果。
[0035]
采用上述方式进行状态估计时，在视觉定位异常情况下，常规处理方式中基本都是基于异常图像帧进行状态估计，由此导致偏差较大，无法进行准确定位。本技术实施例中，则将异常情况下用于进行状态估计的数据分为两部分，一部分为常量部分即所述状态
常量，另一部分则为异常图像帧对应的数据部分。其中，所述状态常量根据常规图像帧对常规地图点的观测数据、以及常规图像帧和异常图像帧中各相邻帧间的惯导观测数据确定。
[0036]
因在视觉定位异常情况下，惯导观测数据以及地图精度等难免会出现偏差，为此，将视觉定位正常情况下的这部分数据作为异常情况下的常量数据，以尽可能地保证这部分数据的准确性。其中，常规图像帧对常规地图点的观测数据可以为原始观测数据，也可以为基于该观测数据获得的状态估计结果。而不管是常规图像帧之间、还是常规图像帧与异常图像帧之间、又或者异常图像帧之间，这些帧间的惯导观测数据均可以常规图像帧时的惯导观测数据为依据，将其设置为常量以作为异常情况下的惯导观测数据，尽可能保证其在异常情况下的准确性。
[0037]
步骤s106：根据状态常量和异常图像帧对异常地图点的观测数据，进行状态估计。
[0038]
因在异常情况发生时，仍有可能存在有效的视觉观测数据，因此在基于异常图像帧进行视觉定位时，除常量形式的惯导观测数据外，仍采用异常图像帧对异常地图点的观测数据，并以紧耦合的方式进行数据融合。
[0039]
其中，前述常规地图点意指异常情况发生前，可以稳定跟踪的地图点，而异常地图点则意指异常情况发生过程中，相机新跟踪到的地图点，其不包括异常情况发生前跟踪的地图点。
[0040]
进而，基于状态常量和异常图像帧对异常地图点的观测数据这两部分内容进行状态估计，获得状态估计结果。
[0041]
步骤s108：根据状态估计的结果，进行视觉重定位。
[0042]
获得的状态估计结果包括位置、速度、姿态等信息，因此，在获得了状态估计的结果后，即可较为准确地进行视觉定位异常情况下的视觉重定位。
[0043]
可见，通过本实施例，与常规出现视觉定位异常后，基于异常图像帧进行视觉重定位处理不同，本技术实施例中保留了异常发生前的常规图像帧，这部分常规图像帧被固定保留直至视觉重定位成功后且重定位帧达到预设数量。并且，基于这部分常规图像帧对应的相关数据以及各帧间的惯导观测数据，确定状态常量，以作为异常发生后进行状态估计时的重要参考因素，从而可以避免因视觉定位发生异常而导致的地图精度、惯导观测数据出现降低或偏差，而导致视觉重定位结果与真实情况偏差较大，鲁棒性和精度不高，无法满足相关应用的定位需求的现象。
[0044]
本实施例的重定位方法可以由任意适当的具有数据处理能力的电子设备执行，包括但不限于：移动终端(如手机、pad等)pc机、和服务器等。
[0045]
实施例二
[0046]
参照图2a，示出了根据本技术实施例二的一种重定位方法的步骤流程图。
[0047]
本实施例的重定位方法包括以下步骤：
[0048]
步骤s202：根据异常检测结果确定发生视觉定位异常。
[0049]
本实施例中，异常检测以检测是否发生图像采集装置如相机的遮挡/抖动为示例，但本领域技术人员应当明了的是，其它需进行视觉重定位的异常检测也同样适用于本技术实施例的方案。
[0050]
基于此，本实施例，对于新采集的图像帧，首先通过遮挡/抖动检测，判断仪器当前是否处于异常情况。检测条件包括但不限于以下至少之一：相同响应阈值下当前图像帧提
取到特征点数量是否少于某一阈值(如50个)、当前图像帧有效跟踪到地图中特征点的数量是否少于某一阈值(如30个)等。如符合上述检测条件，则进入后续的视觉重定位流程；否则，按照常规流程进行即时定位与建图，如类似vins-mono中基于滑动时间窗口进行特征点位置、相机姿态、imu偏置等状态的实时解算，进而定位并建图等。
[0051]
步骤s204：确定异常发生前的常规图像帧，并获取异常发生后的异常图像帧。
[0052]
若根据检测结果确定发生了视觉定位异常，则一方面对异常发生前的图像帧(即常规图像帧)进行确定并固定，本技术实施例中，对常规图像帧进行“固定”意指保留其相关数据并据此确定基于异常图像帧进行状态估计过程中使用的常量。另一方面，还同时继续进行图像帧采集，以获取异常发生后的异常图像帧。
[0053]
在使用滑动时间窗口法的情况下，异常发生前的时间窗口可以为定长时间窗口，而在异常发生后，该时间窗口转换为变长时间窗口，该变长时间窗口的长度随异常图像帧的采集而增加，直至视觉重定位成功且重定位帧达到一定数量后，该变长时间窗口将再次转换为与异常发生前相同长度的定长时间窗口。定长时间窗口可兼顾计算速度、效率与准确度之间的平衡，而变长时间窗口则既可保证异常情况下的数据准确性又考虑了异常情况下的部分数据有效性。
[0054]
例如，设原始定长时间窗口为11帧的长度，如a1-a20均为常规图像帧，a21-a25为异常图像帧，a26及以后为视觉重定位成功后的重定位帧。则，在异常发生即检测到a21为异常图像帧时，时间窗口内为第a11-a21帧，随着异常图像帧的采集，时间窗口内的图像帧为第a11-a25帧。假设，在视觉重定位成功后的第15个图像帧即a40帧时间窗口恢复定长，则在a40之前，时间窗口内将有a11-a39的图像帧。而当采集到第a40图像帧后，时间窗口长度为定长，在该时间窗口内将有a30-a40的图像帧。其中，时间窗口的长度变化的具体实现可由本领域技术人员根据实际情况采用适当手段实现，本技术实施例对此不作限制。
[0055]
步骤s206：基于常规图像帧对常规地图点的观测数据、以及常规图像帧和异常图像帧中各相邻帧间的惯导观测数据，确定状态常量。
[0056]
本实施例中，将基于常规图像帧对常规地图点的观测数据确定的状态估计结果，以及所述常规图像帧和异常图像帧中各相邻帧间的惯导观测数据均置为状态常量。虽然也可以将常规图像帧对常规地图点的观测数据确定为常量，后续使用时再进行状态估计计算获得结果，但因常规图像帧已对应有状态估计结果，因此，本实施例中采用该种方式，可以大大节约后续的计算成本，且简化了状态常量的设置。
[0057]
而在使用时间窗口的情况下，本步骤可以实现为：获得异常发生前的、时间窗口内的常规图像帧对常规地图点的观测数据确定的状态估计结果；根据异常发生前的所述状态估计结果和所述常规图像帧和异常图像帧中各相邻帧间的惯导观测数据，进行状态常量的设置。由此，使得状态常量的设置具有较为统一的标准，且可提升后续使用状态常量时的数据获取和处理效率。
[0058]
而对于惯导观测数据部分，则可以以异常发生前的惯导观测数据的初始值为常规图像帧对应的惯导观测数据常量，以异常图像帧对应的前序图像帧的惯导预测值为异常图像帧对应的惯导观测数据常量。进而，根据异常发生前的状态估计结果、常规图像帧对应的惯导观测数据常量和异常图像帧对应的惯导观测数据常量，进行状态常量的设置。
[0059]
其中，在异常图像帧中，当前异常图像帧对应的惯导观测数据以其之前的图像帧
为依据。对于首帧异常图像帧，其前序图像帧为常规图像帧；而对于其它异常图像帧，其前序图像帧也为异常图像帧。因在异常图像帧之前的常规图像帧对应有准确的惯导状态(包括但不限于偏置状态)预测值常量，基于此，异常图像帧对应的前序图像帧的惯导状态预测值具有较为准确的计算基础，因之前的惯导状态预测值为常量，因此，通过相应算法获得的惯导状态预测值也为常量。在一种可行方式中，可以通过随机游走模型获得惯导状态预测值。随机游走模型用来描述以不可预知的方式进行的不稳定的移动，具体到本实施例，其可描述惯导偏置bias这一状态量的变化状态。采用随机游走模型的方式，可以更为贴合实际情况中imu的状态变化。
[0060]
步骤s208：根据状态常量和异常图像帧对异常地图点的观测数据，进行状态估计。
[0061]
在状态常量及异常图像帧对异常地图点的观测数据都确定了的情况下，即可进行状态估计。
[0062]
当采用视觉 imu进行状态估计的情况下，可采用松耦合方式，也可采用紧耦合方式。松耦合方式是将imu和相机分别进行自身的运动估计，然后对其位姿估计结果进行整合；而紧耦合方式则是将imu的状态与相机的状态合并在一起，共同进行状态估计。本实施例中，采用紧耦合方式进行状态估计，相较于松耦合方式，紧耦合方式可以更好地利用和融合传感器数据，以获得更好的状态估计结果。
[0063]
基于此，本步骤可以实现为：基于状态常量，和基于异常图像帧对异常地图点的观测数据确定的状态估计结果，进行紧耦合状态估计。
[0064]
其中，如前所述，紧耦合状态估计的具体实现可由本领域技术人员根据实际需要采用适当方式实现，本技术实施例对此不作限制。
[0065]
步骤s210：根据状态估计的结果，进行视觉重定位。
[0066]
在v-slam中，若当前观测数据与地图出现无法匹配的情况后，可采用前述处理方式在后续图像帧继续尝试与之前建立的地图进行匹配，获得状态估计结果，包括相机位姿，从而实现重定位(re-localization，也称视觉重定位)过程。若重定位匹配成功，则可在跟丢前(即出现无法匹配前)的世界坐标系下继续对相机的位姿和环境地图进行状态估计。
[0067]
在重定位过程中，若当前图像帧与地图匹配得到足够的特征点(如匹配到的特征点大于或等于35个)关联，并可根据该关联解算出该时刻相机初始状态，即标志着重定位成功。本技术实施例中，将重定位匹配成功的帧及其之后临近帧称为重定位帧，将重定位匹配成功后相机跟踪到的地图点(不包括异常情况发生前跟踪的地图点)称为重定位后地图点。
[0068]
基于此，可选地，在步骤s210后还可执行下述可选步骤s212-s214。
[0069]
步骤s212：若确定视觉重定位成功，则根据常规图像帧对常规地图点的观测数据、常规图像帧中相邻帧间的惯导观测数据、视觉重定位成功后的重定位帧对重定位后的地图点的观测数据、重定位帧中相邻帧间的惯导观测数据，进行视觉重定位成功后的状态估计。
[0070]
虽然视觉重定位成功意味着图像帧与地图能够再次进行有效匹配，但其仍处于从异常至正常的过渡状态。因此，在此阶段还可以采用本步骤的方式进行处理，在保证视觉定位稳定性的基础上，进行状态估计。
[0071]
在该过程中，常规图像帧对常规地图点的观测数据、常规图像帧中相邻帧间的惯导观测数据仍如前，采用常量形式。而视觉重定位成功后的重定位帧对重定位后的地图点的观测数据、重定位帧中相邻帧间的惯导观测数据则可采用获得的实际数据。因异常情况
发生时异常图像帧对异常地图点的观测数据、imu的观测数据可能存在较大的噪声，因此在对重定位帧进行定位时，忽略这一部分观测数据。
[0072]
此外，在基于上述数据进行视觉重定位成功后的状态估计时，还考虑帧间约束，以在一系列图像帧之间的约束条件下进行状态估计。其中，所述帧间约束包括异常帧间、异常帧与常规帧、重定位帧间的偏置约束。基于此，在已知常规图像帧对常规地图点观测数据，常规图像帧间的imu观测数据，重定位帧对常规地图点观测数据，重定位帧对重定位后地图点观测数据，重定位帧间的imu观测数据，以及异常帧间、异常帧与常规帧、重定位帧间的偏置约束的情况下，对常规图像帧、常规地图点、重定位帧、重定位后地图点以及所有imu偏置状态进行状态估计。
[0073]
此外，对于图像采集设备如相机来说，其通常可分为全局相机和卷帘相机。全局相机采用同一时刻曝光所有像素的方式，而卷帘相机采用在一定时间间隔内逐行对像素进行曝光的方式。当卷帘相机在移动时,每一行的像素也在不同位姿下获取，因而影响状态估计。又考虑到重定位帧对常规地图点、重定位后地图点的观测约束较少，卷帘相机模型中时间补偿机制会将imu、视觉观测噪声传递给速度状态参数，因此，若图像采集设备采用卷帘相机，则在进行视觉重定位成功后的状态估计的过程中，将重定位帧对应的速度状态设置为常量，以避免重定位帧速度状态估计发散，提高系统鲁棒性。
[0074]
步骤s214：在确定重定位帧累积至预设数量后，基于重定位帧对重定位后的地图点的观测数据，及相邻重定位帧间的惯导观测数据，进行状态估计。
[0075]
在重定位帧累积至预设数量后，表明视觉定位恢复正常，因此，可基于重定位帧对重定位后的地图点的观测数据，及相邻重定位帧间的惯导观测数据，进行状态估计。在一种可行方式中，可以将滑动时间窗口中的常规图像帧、异常图像帧的状态估计结果及地图点、imu观测数据全部边缘化掉，仅保留重定位帧状态估计结果及对重定位后地图点、imu的观测数据，以避免imu偏置发散，提高系统鲁棒性。
[0076]
至此，可认为视觉重定位完全成功，时间窗口也可恢复原长度，进行常规视觉定位。
[0077]
以下，结合图2b-2d，以一个具体示例过程对上述视觉重定位的实现进行示例性说明。
[0078]
首先，如图2b所示，示出了视觉重定位的流程架构示意。
[0079]
由图2b可见，对于采集的新图像帧，首先通过遮挡/抖动检测，判断仪器当前是否处于异常情况。检测条件包括但不限于：相同响应阈值下当前图像帧提取到特征点数量是否少于某一阈值(如50个)、当前图像帧有效跟踪到地图中特征点的数量是否少于某一阈值(如30个)等。如符合遮挡/抖动检测条件，则进入重定位流程；否则，按照常规定位进行即时定位与建图，如类似vins-mono中基于滑动时间窗口进行特征点位置、相机姿态、imu偏置等状态的实时解算，最终进行位姿输出。
[0080]
而若进入重定位流程，则本示例中，将重定位流程分为重定位第一阶段(相当于步骤s204-s210阶段)、第二阶段(相当于步骤s212阶段)、第三阶段(相当于步骤s214阶段)以及重定位失败四部分。
[0081]
其中，根据当前图像帧提取到的特征点提取数量(如50个)、当前图像帧有效跟踪到地图中特征点的数量(如30个)及距离重定位流程开启的时间，从重定位流程开始至重定
位成功或失败(如，重定位流程开启30s后，如仍然没有重定位成功，则认为重定位失败，结束重定位第一阶段)之间的图像帧进入重定位第一阶段。该阶段主要根据基于图像帧的视觉观测数据和imu观测数据对相机当前位姿进行递推，并维护过程中地图、imu偏置等状态量不受异常情况的影响；尝试新图像帧与历史关键帧、地图进行匹配，如有效匹配的特征点数量超过某一阈值(如35个)，则重定位匹配成功，进入重定位第二阶段。
[0082]
在重定位第二阶段，通过紧耦合的数据融合方式，融合当前图像帧-地图约束、imu约束及历史的观测数据，并考虑当前图像帧-地图约束对于卷帘相机位姿约束的不完备性，进行重定位成功后初始几帧位姿的估计。当重定位成功后的图像帧积累到一定数量(如30帧)，则进入重定位第三阶段。
[0083]
在重定位第三阶段，将重定位成功前的历史观测数据剔除，恢复到常规的即时定位与建图，以控制被观测的观测数据和状态数据的维度，进而控制算力消耗。
[0084]
在重定位第一阶段中，如出现长时间无法重定位匹配的情况，则认为重定位失败。v-slam系统将进行重新初始化，以开启常规定位流程。
[0085]
在上述过程中，针对其中的重定位第一阶段，其状态估计示意图如图2c所示。该阶段为异常情况发生初期。考虑一个如符号7所示的由异常图像帧和常规图像帧组成的时间窗口内系统的状态估计问题：已知一段时间内常规图像帧(如符号1)对常规地图点(如符号3)的观测数据(如符号6)、异常图像帧(如符号2)对异常地图点(如符号4)的观测数据(如符号8)、所有帧之间的imu观测数据(如符号5)，对常规图像帧、异常图像帧、常规地图点、异常地图点以及imu偏置状态进行估计。
[0086]
本示例中，考虑到异常图像帧对异常地图点的观测噪声较大，可能会影响到异常情况发生前常规图像帧、常规地图点及imu偏置的状态估计。因此，在对图2c所示的状态估计问题进行求解时，将常规情况下的常规图像帧、常规地图点及滑动时间窗口中所有imu偏置状态量设为常量，直接取初始值作为估计结果。又考虑到在异常情况发生时，仍有可能存在有效的基于图像帧的视觉观测数据，因此在对异常图像帧进行定位时，除imu观测外，仍采用异常图像帧对异常地图点的观测数据，并以紧耦合的方式进行信息融合。
[0087]
本示例中，不指定信息融合具体的求解方法。在一种可行方式中，可以基于最小二乘法，进行最大后验状态的估计。在一定时间长度的时间窗口中构建优化问题，被优化的变量为：
[0088]
χ＝[x0,x1,
…
,xn,p0,p1,
…
,pm]
[0089]
其中，xk为k时刻异常图像帧的位置、姿态、速度等状态，n为时间窗口内异常图像帧时刻的个数。p
l
是第l个异常地图点的三维位置，m为时间窗口内所有异常图像帧观测到的全部异常特征点个数。根据最大后验构建的优化问题为：
[0090][0091]
其中，第一项||r
p-h
p
χ||2为时间窗口边缘化先验约束构成的残差，{r
p
，h
p
}表示边缘化的先验信息；为时间窗口内第k帧与k 1帧之间imu观测数据构成
的残差，b表示所有imu观测数据的数据集合；为第l个异常地图点在第j个异常图像帧中观测构成的视觉重投影残差，c表示当前时间窗口中被观测到的特征点的特征点集合。其中，每一项的具体实现可由本领域技术人员根据实际需求通过适当的方程或函数实现，本技术实施例对此不作限制。需要说明的是，在上述每一项的具体实现方程或函数中，imu的偏置、常规图像帧及常规地图点的状态量(即状态估计结果)直接取初始值，不进行优化更新。可选地，常规图像帧间imu偏置初始值为进入异常情况之前的状态估计结果；常规图像帧与异常图像帧，以及异常图像帧之间的imu偏置根据随机游走模型按前序帧取预测值；常规图像帧及常规地图点的状态量取进入异常情况之前的状态估计结果。
[0092]
上述重定位第一阶段中，在相机受遮挡、快速晃动等异常情况的过程中，相比于orb-slam2仅根据运动模型推测，本示例通过紧耦合的方式，融合该过程中的imu观测数据、视觉观测数据，在异常情况下仍然可以稳定输出状态估计结果(包括位置、速度、姿态)；在异常情况过程中，相比于vins-mono可能引入imu观测数据、视觉观测数据的噪声，本示例将异常情况前的地图点、相机状态估计结果(包括位置、速度、姿态)、imu偏置状态设为常量，避免了异常情况下地图精度、imu偏置状态变差。
[0093]
针对其中的重定位第二阶段，其状态估计示意图如图2d所示。该阶段从重定位帧出现起，至重定位后续帧累积到一定数量(如30帧)而恢复常规定位(即第三阶段)止。考虑到异常情况发生时异常图像帧对异常地图点观测数据、imu观测数据可能存在较大的噪声，在对重定位帧进行定位时，忽略这一部分观测数据。因此，重定位第二阶段的即时定位与建图问题可以归结为以下状态估计问题：已知常规图像帧对常规地图点观测数据(如符号18)，常规图像帧间imu观测数据(如符号13)，重定位帧对常规地图点观测数据(如符号17)，重定位帧对重定位后地图点观测数据(如符号16)，重定位帧间的imu观测数据(如符号15)，以及异常图像帧间、异常图像帧与常规图像帧、重定位帧间的偏置约束，对常规图像帧、常规地图点、重定位帧、重定位后地图点以及所有imu偏置状态进行状态估计。
[0094]
考虑到重定位帧对常规地图点、重定位后地图点的观测约束较少，卷帘相机模型中时间补偿机制会将imu、视觉观测噪声传递给速度状态参数，因此，本示例在重定位第二阶段将重定位帧的速度状态设为常量，直接取初始值作为估计结果，以避免重定位帧速度状态估计发散，提高系统鲁棒性。并且，以紧耦合的方式对前述状态估计问题中的观测数据进行信息融合。
[0095]
本示例中，不指定信息融合具体的求解方法。在一种可行方式中，基于最小二乘法，进行最大后验状态的估计。在一定时间长度的时间窗口中构建优化问题，被优化的变量为：
[0096]
χ＝[x0,x1,
…
,xn,p0,p1,
…
,pm,y0,y1,
…
,y
p
,b0,b1,
…
,bq]
[0097]
其中，xk为k时刻常规图像帧的位置、姿态、速度等状态，n为时间窗口内常规图像帧时刻的个数。p
l
是第l个常规地图点的三维位置，m为时间窗口内所有常规图像帧观测到的全部常规地图点个数。yk为k时刻重定位的位置、姿态、速度等状态(其中，重定位帧的速度状态采用初始值，不进行优化)，p为时间窗口内重定位帧时刻的个数。bk为时间窗口内第k个imu时刻的偏置状态，q表示时间窗口内所有的imu时刻。根据最大后验构建的优化问题为：
[0098][0099]
其中，第一项||r
p-h
p
χ||2为时间窗口边缘化先验约束构成的残差，{r
p
，h
p
}表示边缘化的先验信息；为时间窗口内常规图像帧或重定位帧的第k帧与k 1帧之间imu观测构成的残差，b表示所有常规帧和重定位帧对应的imu观测时刻的集合；为第l个常规地图点或重定位地图点在第j个常规图像帧或重定位帧中观测构成的视觉重投影残差，c表示当前时间窗口中被观测到的特征点的特征点集合；为两个异常图像帧之间，或异常图像帧与常规图像帧之间，或异常图像帧与重定位帧的第k帧与k 1帧之间的imu偏置连续性约束，其为两帧加速度偏置、角速度偏置之差，b’表示所有imu观测时刻的集合。
[0100]
上述重定位第二阶段，在重定位成功后，相比于orb-slam2仅依赖相机对地图特征点的观测数据进行重定位，本示例以紧耦合的方式融合异常情况前后的观测数据，同时舍弃异常情况过程中的观测数据，可以保证建图、定位的精度；相比于vins-mono以先验约束形式融合重定位约束，本示例以紧耦合的方式，对重定位帧-常规地图点的原始观测数据进行建模，避免建模过程中引起误差，保证重定位后相机位姿在重定位前的世界坐标系下的精度。
[0101]
而针对其中的重定位第三阶段，当第二阶段的重定位帧累积到一定数量(如30帧)，v-slam系统具备恢复常规定位的条件，即进入重定位第三阶段。在该阶段中，将滑动时间窗口的常规图像帧、异常图像帧的状态及视觉、imu观测数据全部边缘化掉，仅保留重定位帧状态及对重定位后地图点、imu的观测数据，并基于此进行状态估计。通过舍弃异常情况过程中的imu观测数据、视觉观测数据，而仅保留imu偏置连续性约束，避免imu偏置发散，提高系统鲁棒性。
[0102]
由上述过程可见，本示例中，整个重定位过程采用紧耦合的信息融合方式，对各部分传感器原始观测数据进行精确的建模，提高建图、定位精度。
[0103]
可见，通过本实施例，与常规出现视觉定位异常后，基于异常图像帧进行视觉重定位处理不同，本技术实施例中保留了异常发生前的常规图像帧，这部分常规图像帧被固定保留直至视觉重定位成功后且重定位帧达到预设数量。并且，基于这部分常规图像帧对应的相关数据以及各帧间的惯导观测数据，确定状态常量，以作为异常发生后进行状态估计时的重要参考因素，从而可以避免因视觉定位发生异常而导致的地图精度、惯导观测数据出现降低或偏差，而导致视觉重定位结果与真实情况偏差较大，鲁棒性和精度不高，无法满足相关应用的定位需求的现象。
[0104]
本实施例的定位方法可以由任意适当的具有数据处理能力的电子设备执行，包括
但不限于：移动终端(如手机、pad等)pc机、和服务器等。
[0105]
实施例三
[0106]
参照图3，示出了根据本技术实施例三的一种定位装置的结构框图。
[0107]
本实施例的定位装置包括：异常确定模块302，用于根据异常检测结果确定发生视觉定位异常，如果发生，则确定异常发生前的常规图像帧，并获取异常发生后的异常图像帧；常量确定模块304，用于基于所述常规图像帧对常规地图点的观测数据、以及所述常规图像帧和所述异常图像帧中各相邻帧间的惯导观测数据，确定状态常量；状态估计模块306，用于根据所述状态常量和所述异常图像帧对异常地图点的观测数据，进行状态估计；重定位模块308，用于根据状态估计的结果，进行视觉重定位。
[0108]
可选地，常量确定模块304，用于将基于所述常规图像帧对常规地图点的观测数据确定的状态估计结果，以及所述惯导观测数据均置为状态常量。
[0109]
可选地，常量确定模块304，用于获得异常发生前的、时间窗口内的所述常规图像帧对常规地图点的观测数据确定的状态估计结果；根据所述异常发生前的所述状态估计结果和所述惯导观测数据，进行所述状态常量的设置。
[0110]
可选地，常量确定模块304在根据所述异常发生前的所述状态估计结果和所述惯导观测数据，进行所述状态常量的设置时：以所述异常发生前的所述惯导观测数据的初始值为常规图像帧对应的惯导观测数据常量，以所述异常图像帧对应的前序图像帧的惯导预测值为所述异常图像帧对应的惯导观测数据常量；根据所述异常发生前的所述状态估计结果、所述常规图像帧对应的惯导观测数量常量和所述异常图像帧对应的惯导观测数量常量，进行所述状态常量的设置。
[0111]
可选地，状态估计模块306，用于基于所述状态常量，和基于所述异常图像帧对异常地图点的观测数据确定的状态估计结果，进行紧耦合状态估计。
[0112]
可选地，本实施例的重定位装置还包括：重定位成功后模块310，用于若确定视觉重定位成功，则根据所述常规图像帧对常规地图点的观测数据、所述常规图像帧中相邻帧间的惯导观测数据、视觉重定位成功后的重定位帧对重定位后的地图点的观测数据、重定位帧中相邻帧间的惯导观测数据，进行视觉重定位成功后的状态估计。
[0113]
可选地，重定位成功后模块310，还用于在进行所述视觉重定位成功后的状态估计的过程中，将重定位帧对应的速度状态设置为常量。
[0114]
可选地，重定位成功后模块310，还用于在确定所述重定位帧累积至预设数量后，基于重定位帧对重定位后的地图点的观测数据，及相邻重定位帧间的惯导观测数据，进行状态估计。
[0115]
本实施例的定位装置用于实现前述多个方法实施例中相应的定位方法，并具有相应的方法实施例的有益效果，在此不再赘述。此外，本实施例的定位装置中的各个模块的功能实现均可参照前述方法实施例中的相应部分的描述，在此亦不再赘述。
[0116]
实施例四
[0117]
参照图4，示出了根据本技术实施例四的一种电子设备的结构示意图，本技术具体实施例并不对电子设备的具体实现做限定。
[0118]
如图4所示，该电子设备可以包括：处理器(processor)402、通信接口(communications interface)404、存储器(memory)406、以及通信总线408。
[0119]
其中：
[0120]
处理器402、通信接口404、以及存储器406通过通信总线408完成相互间的通信。
[0121]
通信接口404，用于与其它电子设备或服务器进行通信。
[0122]
处理器402，用于执行程序410，具体可以执行上述定位方法实施例中的相关步骤。
[0123]
具体地，程序410可以包括程序代码，该程序代码包括计算机操作指令。
[0124]
处理器402可能是cpu，或者是特定集成电路asic(application specific integrated circuit)，或者是被配置成实施本技术实施例的一个或多个集成电路。智能设备包括的一个或多个处理器，可以是同一类型的处理器，如一个或多个cpu；也可以是不同类型的处理器，如一个或多个cpu以及一个或多个asic。
[0125]
存储器406，用于存放程序410。存储器406可能包含高速ram存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。
[0126]
程序410具体可以用于使得处理器402执行以下操作：根据异常检测结果确定发生视觉定位异常，如果发生，确定异常发生前的常规图像帧，并获取异常发生后的异常图像帧；基于所述常规图像帧对常规地图点的观测数据、以及所述常规图像帧和所述异常图像帧中各相邻帧间的惯导观测数据，确定状态常量；根据所述状态常量和所述异常图像帧对异常地图点的观测数据，进行状态估计；根据状态估计的结果，进行视觉重定位。
[0127]
在一种可选的实施方式中，程序410还用于使得处理器402在基于所述常规图像帧对常规地图点的观测数据、以及所述常规图像帧和所述异常图像帧中各相邻帧间的惯导观测数据，确定状态常量时：将基于所述常规图像帧对常规地图点的观测数据确定的状态估计结果，以及所述惯导观测数据均置为状态常量。
[0128]
在一种可选的实施方式中，程序410还用于使得处理器402在将基于所述常规图像帧对常规地图点的观测数据确定的状态估计结果，以及所述惯导观测数据均置为状态常量时：获得异常发生前的、时间窗口内的所述常规图像帧对常规地图点的观测数据确定的状态估计结果；根据所述异常发生前的所述状态估计结果和所述惯导观测数据，进行所述状态常量的设置。
[0129]
在一种可选的实施方式中，程序410还用于使得处理器402在根据所述异常发生前的所述状态估计结果和所述惯导观测数据，进行所述状态常量的设置时：以所述异常发生前的所述惯导观测数据的初始值为常规图像帧对应的惯导观测数据常量，以所述异常图像帧对应的前序图像帧的惯导预测值为所述异常图像帧对应的惯导观测数据常量；根据所述异常发生前的所述状态估计结果、所述常规图像帧对应的惯导观测数量常量和所述异常图像帧对应的惯导观测数量常量，进行所述状态常量的设置。
[0130]
在一种可选的实施方式中，程序410还用于使得处理器402在根据所述状态常量和所述异常图像帧对异常地图点的观测数据确定的状态估计结果，进行状态估计时：基于所述状态常量，和基于所述异常图像帧对异常地图点的观测数据确定的状态估计结果，进行紧耦合状态估计。
[0131]
在一种可选的实施方式中，程序410还用于使得处理器402在若确定视觉重定位成功，则根据所述常规图像帧对常规地图点的观测数据、所述常规图像帧中相邻帧间的惯导观测数据、视觉重定位成功后的重定位帧对重定位后的地图点的观测数据、重定位帧中相邻帧间的惯导观测数据，进行视觉重定位成功后的状态估计。
[0132]
在一种可选的实施方式中，程序410还用于使得处理器402在进行所述视觉重定位成功后的状态估计的过程中，将重定位帧对应的速度状态设置为常量。
[0133]
在一种可选的实施方式中，程序410还用于使得处理器402在确定所述重定位帧累积至预设数量后，基于重定位帧对重定位后的地图点的观测数据，及相邻重定位帧间的惯导观测数据，进行状态估计。
[0134]
程序410中各步骤的具体实现可以参见上述重定位方法实施例中的相应步骤和单元中对应的描述，在此不赘述。所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的设备和模块的具体工作过程，可以参考前述方法实施例中的对应过程描述，在此不再赘述。
[0135]
通过本实施例的电子设备，与常规出现视觉定位异常后，基于异常图像帧进行视觉重定位处理不同，本技术实施例中保留了异常发生前的常规图像帧，这部分常规图像帧被固定保留直至视觉重定位成功后且重定位帧达到预测数量。并且，基于这部分常规图像帧对应的相关数据以及各帧间的惯导观测数据，确定状态常量，以作为异常发生后进行状态估计时的重要参考因素，从而可以避免因视觉定位发生异常而导致的地图精度、惯导观测数据出现降低或偏差，而导致视觉重定位结果与真实情况偏差较大，鲁棒性和精度不高，无法满足相关应用的定位需求的现象。
[0136]
需要指出，根据实施的需要，可将本技术实施例中描述的各个部件/步骤拆分为更多部件/步骤，也可将两个或多个部件/步骤或者部件/步骤的部分操作组合成新的部件/步骤，以实现本技术实施例的目的。
[0137]
上述根据本技术实施例的方法可在硬件、固件中实现，或者被实现为可存储在记录介质(诸如cd rom、ram、软盘、硬盘或磁光盘)中的软件或计算机代码，或者被实现通过网络下载的原始存储在远程记录介质或非暂时机器可读介质中并将被存储在本地记录介质中的计算机代码，从而在此描述的方法可被存储在使用通用计算机、专用处理器或者可编程或专用硬件(诸如asic或fpga)的记录介质上的这样的软件处理。可以理解，计算机、处理器、微处理器控制器或可编程硬件包括可存储或接收软件或计算机代码的存储组件(例如，ram、rom、闪存等)，当所述软件或计算机代码被计算机、处理器或硬件访问且执行时，实现在此描述的重定位方法。此外，当通用计算机访问用于实现在此示出的重定位方法的代码时，代码的执行将通用计算机转换为用于执行在此示出的重定位方法的专用计算机。
[0138]
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及方法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本技术实施例的范围。
[0139]
以上实施方式仅用于说明本技术实施例，而并非对本技术实施例的限制，有关技术领域的普通技术人员，在不脱离本技术实施例的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本技术实施例的范畴，本技术实施例的专利保护范围应由权利要求限定。