一种视觉惯性系统的误差修正方法和装置与流程

本发明涉及导航技术领域，具体涉及一种视觉惯性系统的误差修正方法和装置。

背景技术：

vio(visual-inertialodometry，视觉惯性里程计)，有时也叫视觉惯性系统(vins，visual-inertialsystem)，该系统通过融合相机和imu(inertialmeasurementunit，惯性测量单元)数据来实现定位导航的算法。由于基于vio系统的单目视觉定位算法存在一些本身框架无法克服的缺陷，最突出的是尺度的问题，主要是单目vio处理的图像帧丢失了环境的深度信息，即使通过对极约束和三角化恢复了空间路标点的三维信息，但是这个过程的深度恢复的刻度是任意的，并不是实际的物理尺度，从而导致单目vio估计出的运动轨迹即使形状吻合但是尺寸大小却不是实际轨迹尺寸。

通过引入imu信息可以将imu估计的位姿序列和相机估计的位姿序列对齐，从而估计出相机轨迹的真实尺度，而且imu可以很好地预测出图像帧的位姿以及上一时刻特征点在下帧图像的位置，提高特征跟踪算法匹配速度和应对快速旋转的算法鲁棒性，最后imu中加速度计提供的重力向量可以将估计的位置转为实际导航需要的世界坐标系中。

虽然imu的加入解决了vio系统中尺度容易发散的问题，但是在某些特殊的退化运动中，单目vio无法通过视差而三角化特征点，即没有有效的观测去修正视觉误差或者imu的积分误差，最终导致位置漂移过大甚至出现整个系统的发散。

技术实现要素：

本发明实施例公开一种视觉惯性系统的误差修正方法和装置，防止视觉惯性系统出现位置过大的漂移或者发散，提高了系统精度和鲁棒性。

第一方面，本发明实施例公开了一种视觉惯性系统的误差修正方法，该方法包括：

根据统计数据判断当前视觉惯性系统是否处于静止状态，其中，所述统计数据包括下述一项或多项：所述当前视觉惯性系统中惯性测量单元输出的方差、预设时间内的标准速度或所述当前视觉惯性系统中的摄像头所采集图像中特征点的位移；

如果所述当前视觉惯性系统处于静止状态，则获取所述当前视觉惯性系统进入所述静止状态时各状态属性的参数值作为观测值，并获取各观测值分别所对应的预测值，其中，所述状态属性至少包括速度；

对于任意一个状态属性，利用该状态属性的观测值对其预测值进行修正。

可选的，所述根据统计数据判断当前视觉惯性系统是否处于静止状态，包括如下方式中的一项或多项：

判断当前视觉惯性系统中惯性测量单元输出的方差是否低于设定方差阈值；或者，

在设定时间内判断当前视觉惯性系统的最大速度是否不超过预设速度；或者，

判断当前视觉惯性系统中的摄像头所采集图像中特征点的位移是否超过设定的像素阈值。

可选的，对于任意一个状态属性，利用该状态属性的观测值对其预测值进行修正，包括：

对于任意一个状态属性，将该状态属性的观测值与其预测值作差，得到估计误差；

利用所述估计误差对该状态属性的预测值进行修正。

可选的，利用所述估计误差对该状态属性的预测值进行修正，包括：

基于卡尔曼滤波或非线性优化算法，利用所述估计误差对该状态属性的预测值进行修正。

可选的，如果所述当前视觉惯性系统处于静止状态，则将获取的所述当前视觉惯性系统进入所述静止状态时各状态属性的参数值作为观测值，包括：

如果所述当前视觉惯性系统第一次进入静止状态，则分别获取所述当前视觉惯性系统进入所述静止状态时的速度、位置和航向的参数值作为观测值。

第二方面，本发明实施例还提供了一种视觉惯性系统的误差修正装置，该装置包括：

静止状态判断模块，被配置为根据统计数据判断当前视觉惯性系统是否处于静止状态，其中，所述统计数据包括下述一项或多项：所述当前视觉惯性系统中惯性测量单元输出的方差、预设时间内的标准速度或所述当前视觉惯性系统中的摄像头所采集图像中特征点的位移；

观测值获取模块，被配置为如果所述当前视觉惯性系统处于静止状态，则获取所述当前视觉惯性系统进入所述静止状态时各状态属性的参数值作为观测值，并获取各观测值分别所对应的预测值，其中，所述状态属性至少包括速度；

修正模块，被配置为对于任意一个状态属性，利用该状态属性的观测值对其预测值进行修正。

可选的，所述静止状态判断模块，具体被配置为如下方式中的一项或多项：

判断当前视觉惯性系统中惯性测量单元输出的方差是否低于设定方差阈值；或者，

在设定时间内判断当前视觉惯性系统的最大速度是否不超过预设速度；或者，

判断当前视觉惯性系统中的摄像头所采集图像中特征点的位移是否超过设定的像素阈值。

可选的，所述修正模块，包括：

估计误差确定单元，被配置为对于任意一个状态属性，将该状态属性的观测值与其预测值作差，得到估计误差；

状态修正单元，被配置为利用所述估计误差对该状态属性的预测值进行修正。

可选的，所述状态修正单元，具体被配置为：

基于卡尔曼滤波或非线性优化算法，利用所述估计误差对该状态属性的预测值进行修正。

可选的，所述观测值获取模块，具体被配置为：

如果所述当前视觉惯性系统第一次进入静止状态，则分别将获取的所述当前视觉惯性系统进入所述静止状态时的速度、位置和航向的参数值作为观测值。

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

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

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

所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明任意实施例所提供的视觉惯性系统的误差修正方法的部分或全部步骤。

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

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

本实施例提供的技术方案，考虑到vio系统在静止状态下无法通过有效的观测去修正视觉误差或者imu的积分误差，容易导致位置漂移过大甚至出现整个系统的发散这一问题，当vio系统进入静止状态这一特殊的退出运动时，增加了速度不变、位置不变和航向不变的等假设条件，即在系统进入静止状态时提供了额外的观测。通过利用观测值对系统的预测值进行修正，可防止系统处于静止状态时出现位置过大的漂移或者发散问题，提高了系统精度及系统鲁棒性。

本发明的发明点包括：

1、增加了vio系统处于静止状态中特有的观测，防止系统出现位置过大的漂移或者发散，提高系统精度及系统鲁棒性，是本发明的发明点之一。

2、提出了如下任意一种或多种相结合的方式：判断imu输出的方差与设定方差阈值的大小、判断设定时间内vio系统的最大速度与预设速度的大小，以及vio系统的摄像头所采集图像中特征点的位移与设定像素阈值的大小，来检测vio系统是否处于静止状态，是本发明的发明点之一。

附图说明

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

图1是本发明实施例提供的一种视觉惯性系统的误差修正方法的流程示意图；

图2是本发明实施例提供的一种视觉惯性系统的误差修正装置的结构示意图；

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

具体实施方式

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

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

实施例一

请参阅图1，图1是本发明实施例提供的一种视觉惯性系统的误差修正方法的流程示意图。该方法可应用于自动驾驶汽车、无人机中，可由视觉惯性系统的误差修正方法装置来执行，该装置可通过软件和/或硬件的方式实现，典型的可集成在车载电脑、车载工业控制计算机(industrialpersonalcomputer，ipc)等车载终端中，本发明实施例不做限定。如图1所示，本实施例提供的方法具体包括：

110、根据统计数据判断当前视觉惯性系统是否处于静止状态。

其中，视觉惯性系统(以下简称vio系统)的静止状态是一种特殊的退化运动。在该状态下，单目vio系统无法通过视差而三角化特征点，即没有有效的观测去修正视觉误差或者imu的积分误差，该状态容易导致位置漂移过大甚至出现整个系统的发散。为了解决这一问题，本实施例增加了对vio系统是否处于静止状态的判定，并增加静止状态中特有的观测，从而提高系统精度及系统鲁棒性。

示例性的，可通过统计数据判断当前vio系统是否处于静止状态，其中，统计数据包括：当前vio系统中imu输出的方差、预设时间内的标准速度或当前vio系统中的摄像头所采集图像中特征点的位移。上述统计数据可以自由组合，也可以单独使用其中一项用于vio系统静止状态的判断。

示例性的，根据统计数据判断当前视觉惯性系统是否处于静止状态，可包括如下方式中的一项或多项：

判断当前vio系统中惯性测量单元输出的方差是否低于设定方差阈值，如果低于设定方法阈值，则确定当前vio系统处于静止状态；或者，

在设定时间内判断当前vio系统的最大速度是否不超过预设速度，如果不超过预设速度，例如0.1m/s，则确定当前vio系统处于静止状态；或者，

判断当前vio系统中的摄像头所采集图像中特征点的位移是否超过设定的像素阈值，如果超过1.5个像素的点数低于总数的80％，则确定当前vio系统处于静止状态。

具体的，可统计1s内imu输出方差是否低于器件手册给出的设定方差阈值，若低于，则判断出vio系统进入静止状态，反之则为非静止状态。

或者如果统计出1s内imu输出方差是否低于器件手册给出的设定方差阈值，并且判断出1s内vio系统的最大速度不超过0.1m/s，即可判定系统进入静止状态。

或者如果统计出1s内vio系统的最大速度不超过0.1m/s，且视野内特征点位移超过1.5个像素的点数低于总数的80％，即可判定系统进入静止状态。

120、如果当前视觉惯性系统处于静止状态，则获取当前视觉惯性系统进入静止状态时各状态属性的参数值作为观测值，并获取各观测值分别所对应的预测值。

其中，系统的状态属性至少包括系统的三向速度，还可包括但并不限于系统位置和航向。

本实施例在判断出当前vio系统处于静止状态时，设置有几个比较强的假设，即当vio系统进入静止状态时，三向速度为0；位置不变，即观测值具体为vio系统进入静止状态时的位置；航向不变，即观测值具体为vio系统进入静止状态时的航向。

另外，当vio系统处于静止状态时，vio系统还会估计系统的速度、位置和航向，具体估计方法可以为通过imu积分预测系统的姿态、位置和速度的变化，然后再通过图像之间的共视关系来修正imu积分带来的误差，得到各状态属性对应的预测值。

需要说明的是，本实施例中设置上述假设条件是为了对处于静止状态的vio系统的预测值进行修正，即进行观测融合，以防止系统出现位置过大的漂移或者发散。

130、对于任意一个状态属性，利用该状态属性的观测值对其预测值进行修正。

示例性的，利用该状态属性的观测值对其预测值进行修正，可通过如下方式来实现：

对于任意一个状态属性，将该状态属性的观测值与其预测值作差，得到估计误差，并利用估计误差对该状态属性的预测值进行修正。具体的修正方法可采用卡尔曼滤波或非线性优化算法，本实施例对此不做限定。

具体的，下面分别以速度、位置和航向为例进行说明：

当vio系统处于静止状态时，该系统所估计的状态量xk包括速度vk、位置pk和航向yk。其中，若vio系统估计系统速度，则其量测方程为hv(xk)＝vk，此时，速度估计误差若vio系统估计系统位置，则其量测方程为hp(xk)＝pk，此时，位置估计误差其中，pstatic为系统进入静止状态时记录下的位置；若vio系统估计系统速度，则其量测方程为hy(xk)＝yk，此时，航向估计误差为其中，ystatic为系统进入静止状态时记录下的航向。

需要说明的是，当vio系统第一次进入静止状态时，需结合上述三向速度为0、位置不变和航向不变这三项假设进行观测融合。但当vio系统脱离初始的静止状态，并且再次进入静止状态时，可只选用速度进行观测融合，或者也可将速度和其他两项假设条件中的任意一项或两项相结合进行观测融合。

本实施例提供的技术方案，考虑到vio系统在静止状态下无法通过有效的观测去修正视觉误差或者imu的积分误差，容易导致位置漂移过大甚至出现整个系统的发散这一问题，当vio系统进入静止状态这一特殊的退出运动时，增加了速度不变、位置不变和航向不变的等假设条件，即在系统进入静止状态时提供了额外的观测。通过利用观测值对系统的预测值进行修正，可防止系统处于静止状态时出现位置过大的漂移或者发散问题，提高了系统精度及系统鲁棒性。

实施例二

请参阅图2，图2是本发明实施例提供的一种视觉惯性系统的误差修正装置的结构示意图。如图2所示，该装置包括：静止状态判断模块210、观测值获取模块220和修正模块230。

其中，静止状态判断模块210，被配置为根据统计数据判断当前视觉惯性系统是否处于静止状态，其中，所述统计数据包括下述一项或多项：所述当前视觉惯性系统中惯性测量单元输出的方差、预设时间内的标准速度或所述当前视觉惯性系统中的摄像头所采集图像中特征点的位移；

观测值获取模块220，被配置为如果所述当前视觉惯性系统处于静止状态，则获取所述当前视觉惯性系统进入所述静止状态时各状态属性的参数值作为观测值，并获取各观测值分别所对应的预测值，其中，所述状态属性至少包括速度；

修正模块230，被配置为对于任意一个状态属性，利用该状态属性的观测值对其预测值进行修正。

本实施例提供的技术方案，考虑到vio系统在静止状态下无法通过有效的观测去修正视觉误差或者imu的积分误差，容易导致位置漂移过大甚至出现整个系统的发散这一问题，当vio系统进入静止状态这一特殊的退出运动时，增加了速度不变、位置不变和航向不变的等假设条件，即在系统进入静止状态时提供了额外的观测。通过利用观测值对系统的预测值进行修正，可防止系统处于静止状态时出现位置过大的漂移或者发散问题，提高了系统精度及系统鲁棒性。

可选的，所述静止状态判断模块，具体被配置为如下方式中的一项或多项：

判断当前视觉惯性系统中惯性测量单元输出的方差是否低于设定方差阈值；或者，

在设定时间内判断当前视觉惯性系统的最大速度是否不超过预设速度；或者，

判断当前视觉惯性系统中的摄像头所采集图像中特征点的位移是否超过设定的像素阈值。

可选的，所述修正模块，包括：

估计误差确定单元，被配置为对于任意一个状态属性，将该状态属性的观测值与其预测值作差，得到估计误差；

状态修正单元，被配置为利用所述估计误差对该状态属性的预测值进行修正。

可选的，所述状态修正单元，具体被配置为：

基于卡尔曼滤波或非线性优化算法，利用所述估计误差对该状态属性的预测值进行修正。

可选的，所述观测值获取模块，具体被配置为：

如果所述当前视觉惯性系统第一次进入静止状态，则分别将获取的所述当前视觉惯性系统进入所述静止状态时的速度、位置和航向的参数值作为观测值。

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

实施例三

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

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

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

其中，处理器702调用存储器701中存储的可执行程序代码，执行本发明任意实施例所提供的视觉惯性系统的误差修正方法。

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

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

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

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

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

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

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(read-onlymemory，rom)、随机存储器(randomaccessmemory，ram)、可编程只读存储器(programmableread-onlymemory，prom)、可擦除可编程只读存储器(erasableprogrammablereadonlymemory，eprom)、一次可编程只读存储器(one-timeprogrammableread-onlymemory，otprom)、电子抹除式可复写只读存储器(electrically-erasableprogrammableread-onlymemory，eeprom)、只读光盘(compactdiscread-onlymemory，cd-rom)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。

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