CN115717901B - 基于滤波的惯性/视觉里程计安装误差估计方法 - Google Patents

Abstract

本发明提供一种基于滤波的惯性/视觉里程计安装误差估计方法，该方法包括：分别计算世界坐标系下视觉里程计解算的三个轴向位置与导航坐标系下惯导解算的三个轴向位置信息；将世界坐标系下视觉里程计解算的位置转换到导航坐标系下，获取视觉里程计在导航坐标系下的位置信息；根据导航坐标系下惯导解算的位置信息与视觉里程计在导航坐标系下的位置信息的差值构造观测量；并计算量测矩阵；确定误差模型；利用卡尔曼滤波方法，基于所述误差模型、观测量以及量测矩阵对系统误差进行估计，并根据滤波得到的误差估计结果对惯导系统误差进行修正。与现有技术相比大幅度提高了估计结果的精度，且安装误差估计方法具有便利性与普适性。

Description

基于滤波的惯性/视觉里程计安装误差估计方法

技术领域

本发明涉及视觉导航技术领域，尤其涉及一种基于滤波的惯性/视觉里程计安装误差估计方法。

背景技术

根据连续图像间特征变化进行载体位姿解算的视觉里程计是视觉导航研究中一个重要的领域。根据惯性导航信息输出频率高，与视觉里程计具有很好的互补性这一特点，惯性/视觉里程计导航方式有着更高的精度及鲁棒性。而二者之间的安装误差是其组合导航精度的重要影响因素，合理有效的安装误差估计方法在目标跟踪、路径规划、机器人避障等领域均有十分广阔的应用前景。

传统的机械里程计安装于无人车、机器人履带或车轮，通过测量车轮转动圈数计算载体系下前向里程，与惯导之间仅存在俯仰、航向两个方向的安装误差。而视觉里程计可以输出三个方向的位移量，因此传统误差模型无法应用于惯性/视觉里程计安装误差估计。

当前惯性/视觉里程计常采用离线标定方法，即通过采集大量不同旋转角度的标定板图像及惯导数据进行惯导与相机之间安装误差的计算，这种方法精度有限、无法估计视觉里程计的尺度误差。同时不具便利性，每次标定都需要提前离线采集大量数据，无法适用于批量系统的误差标定。

发明内容

本发明旨在至少解决现有技术中存在的问题之一。

为此，本发明提供了一种基于滤波的惯性/视觉里程计安装误差估计方法。

本发明的技术解决方案：本发明提供一种基于滤波的惯性/视觉里程计安装误差估计方法，其中，该方法包括：

分别计算世界坐标系下视觉里程计解算的三个轴向位置与导航坐标系下惯导解算的三个轴向位置信息；

将世界坐标系下视觉里程计解算的位置转换到导航坐标系下，获取视觉里程计在导航坐标系下的位置信息；

根据导航坐标系下惯导解算的位置信息与视觉里程计在导航坐标系下的位置信息的差值构造观测量Z_k；并计算量测矩阵H_k；

确定误差模型；

利用卡尔曼滤波方法，基于所述误差模型、观测量Z_k以及量测矩阵H_k对系统误差进行估计，并根据滤波得到的误差估计结果对惯导系统误差进行修正。

进一步地，通过下式获取视觉里程计在导航坐标系下的位置信息

其中，为地理坐标系到导航坐标系的姿态转移矩阵，/>为相机坐标系到地理坐标系的转换矩阵，/>为初始化成功时刻建立的世界坐标系与相机坐标系之间的姿态转移矩阵，/>为世界坐标系下视觉里程计解算的位置。

进一步地，通过下述根据导航坐标系下惯导解算的位置信息与视觉里程计在导航坐标系下的位置信息的差值构造观测量Z_k：

其中，pⁿ表示惯导位置真值，δpⁿ表示惯导的位置误差，表示视觉里程计在导航坐标系下的位置真值，/>表示视觉里程计的位置误差。

进一步地，通过下式基于误差方程系数关系构造量测矩阵H_k：

进一步地，确定误差模型包括：

确定惯导组合系统状态变量X_k；

确定惯导组合系统状态方程。

进一步地，采用如下式的系统状态变量X_k：

其中：

δVⁿ＝(δV_N,δV_U,δV_E)

δPⁿ＝(δL,δH,δλ)

ε^b＝(ε_bx,ε_by,ε_bz)

δV_N,δV_U,δV_E分别表示惯导系统在导航坐标系下北、天、东向的速度误差；

δL,δH,δλ分别表示惯导系统在导航坐标系下纬度、高度、经度误差；

分别表示惯导系统在导航坐标系下北、天、东向失准角；

ε_bx,ε_by,ε_bz分别表示惯导系统在载体坐标系下X、Y、Z方向的陀螺漂移；

分别表示惯导系统在载体坐标系下X、Y、Z方向的加速度计零偏；

分别表示视觉里程计在导航坐标系下北、天、东方向的位置误差；

δα_γ,δα_θ分别表示在载体坐标系下视觉里程计与惯导之间的安装误差角；

δS表示导航坐标系下视觉里程计的尺度误差；

进一步地，通过下式获视觉里程计的位置取

其中，

δα_γ、/>δα_θ分别为绕惯导纵轴x_b、竖轴y_b及横轴z_b存在的横滚误差角、航向误差偏角及俯仰误差角；δS为视觉里程计的尺度误差， 为视觉里程计在相机坐标系下的位置信息，/>分别代表视觉里程计在相机坐标系下x、y、z三个方向的位置输出。

进一步地，采用下式获取状态方程：

其中，为状态变量，/>为白噪声向量，F(t)为状态转移矩阵。

进一步地，所述状态转移矩阵采用下式获取：

其中，子矩阵F₁→F₁₀由捷联惯导误差方程建立，子矩阵F_O1→F_O2由视觉里程计误差方程建立，如下所示：

与现有技术相比，本发明的有益效果是：

通过惯导系统与视觉里程计在导航坐标系下的位置结果差值作为观测量，针对两种导航方式的所有误差源构建误差模型，通过基于卡尔曼滤波的方法实现对惯导系统误差、视觉里程计与惯导间的安装误差、视觉里程计尺度误差的精确估计，并对其进行实时修正。与现有技术相比，大幅度提高了估计结果的精度，且安装误差估计方法具有便利性与普适性。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了根据本发明实施例提供的一种基于速度积分的惯性/里程计组合导航方法的流程示意图。

具体实施方式

下面将结合附图对本发明的具体实施例进行详细说明。在下面的描述中，出于解释而非限制性的目的，阐述了具体细节，以帮助全面地理解本发明。然而，对本领域技术人员来说显而易见的是，也可以在脱离了这些具体细节的其它实施例中实践本发明。

在此需要说明的是，为了避免因不必要的细节而模糊了本发明，在附图中仅仅示出了与根据本发明的方案密切相关的设备结构和/或处理步骤，而省略了与本发明关系不大的其他细节。

图1示出了根据本发明实施例提供的一种基于滤波的惯性/视觉里程计安装误差估计方法的流程示意图。

如图1所示，提供一种基于滤波的惯性/视觉里程计安装误差估计方法，其中，该方法可以包括：

步骤一、分别计算世界坐标系下视觉里程计解算的三个轴向位置与导航坐标系下惯导解算的三个轴向位置信息；

步骤二、将世界坐标系下视觉里程计解算的位置转换到导航坐标系下，获取视觉里程计在导航坐标系下的位置信息；

步骤三、根据导航坐标系下惯导解算的位置信息与视觉里程计在导航坐标系下的位置信息的差值构造观测量Z_k；并计算量测矩阵H_k；

步骤四、确定误差模型；

步骤五、利用卡尔曼滤波方法，基于所述误差模型、观测量Z_k以及量测矩阵H_k对系统误差进行估计，并根据滤波得到的误差估计结果对惯导系统误差进行修正。

由此可见，相比于传统的离线采集数据进行标定的方法，本发明采用的是一种在线估计方法。通过惯导系统与视觉里程计的误差源构建误差模型，基于卡尔曼滤波算法对安装误差进行精确估计并完成实时修正，与现有技术相比提升了安装误差估计精度，并更加便利，适用于大批量装备标定。

举例来讲，本发明实施时涉及的坐标系如下：

a)载体坐标系(b系)

采用“前--上--右”坐标系。坐标原点O选取载体的质心，OX_b轴沿惯导系统纵轴方向，向前为正；OY_b轴沿惯导系统竖轴方向，向上为正；OZ_b轴沿惯导系统横轴方向，向右为正。

b)地理坐标系(g系)

采用“北--天--东”坐标系。坐标原点选取惯导系统的质心，OX_g轴沿地理南北方向，指北为正；OY_g轴沿地理天方向，指天为正；OZ_g轴沿地理东西方向，指东为正；

c)导航坐标系(n系)

选取地理坐标系为导航坐标系。

d)相机坐标系(c系)

采用“前--上--右”坐标系。坐标原点O选取相机质心，OX_c轴沿相机纵轴方向，向前为正；OY_c轴沿相机竖轴方向，向上为正；OZ_c轴沿相机横轴方向，向右为正。

e)世界坐标系(w系)

取视觉里程计初始化成功时刻相机坐标系为世界坐标系。

根据本发明的一种实施为例，通过惯导解算的由b系到n系的姿态转移矩阵惯导和相机之间的安装角度计算的/>初始化成功时刻建立的世界坐标系与相机坐标系之间的姿态阵/>将视觉里程计解算的世界坐标系下位置信息转换为导航坐标系：

下一步计算导航坐标下惯导系统与视觉里程计之间的位置误差δp：

其中，p_n、均表示不存在位置误差的惯导系统、视觉里程计位置真值，因此二者相等，即惯导系统与视觉里程计之间的位置差值为二者的位置误差之差。

因此，通过上式惯导系统与视觉里程计之间的位置差值构建观测量Z_k：

通过下式构造组合导航系统的量测矩阵H_k：

根据本发明一种实施为例，构建组合导航系统的状态变量X_k：

其中：

δVⁿ＝(δV_N,δV_U,δV_E)

δPⁿ＝(δL,δH,δλ)

ε^b＝(ε_bx,ε_by,ε_bz)

δV_N,δV_U,δV_E分别表示惯导系统在导航坐标系下北、天、东向的速度误差；

δL,δH,δλ分别表示惯导系统在导航坐标系下纬度、高度、经度误差；

分别表示惯导系统在导航坐标系下北、天、东向失准角；

ε_bx,ε_by,ε_bz分别表示惯导系统在载体坐标系下X、Y、Z方向的陀螺漂移；

分别表示惯导系统在载体坐标系下X、Y、Z方向的加速度计零偏；

分别表示视觉里程计在导航坐标系下北、天、东方向的位置误差；

δα_γ,δα_θ分别表示在载体坐标系下视觉里程计与惯导之间的安装误差角；

δS表示导航坐标系下视觉里程计的尺度误差；

根据本发明的一种实施为例，惯性/视觉里程计组合系统的卡尔曼滤波模型连续状态方程如下：

其中，为19维状态量，/>为白噪声向量，F(t)为状态转移矩阵。

其中F(t)由惯导误差方程、视觉里程计误差方程求取，表现形式具体如下：

其中：子矩阵F₁→F₁₀由捷联惯导误差方程建立：

其中：

表示惯导系统从载体坐标系到导航坐标系的姿态矩阵；

表示惯导系统从导航坐标系到载体坐标系的姿态矩阵；

V_n、V_u、V_e表示惯导系统计算得到的n系下北、天、东向速度；

L、h、λ表示惯导系统计算得到的n系下纬度、高度、经度；

表示惯导系统在b系下X、Y、Z三个方向的比力输出值转换到b系下北、天、东向的表示值；

R_M、R_N分别表示当前时刻惯导所在位置子午圈、卯酉圈曲率半径；

ω_ie表示地球的自转角速率。

子矩阵F_O1→F_O2由视觉里程计误差方程建立，具体如下：

在将视觉里程计在w系下的位置输出存在尺度误差，转换至n系下时引入了安装误差，举例如下：

视觉里程计测量的实际位移大小/>和理论位移大小p_O之间的关系为：

所以有：

实际测量中视觉里程计的位移输出在导航坐标系下表示为：

在实际系统中，惯性/视觉里程计系统相机与惯导的安装很难保证载体坐标系(b系)与相机坐标系(c系)完全重合，假设从c系至b系存在小量的安装误差角，即绕惯导纵轴x_b、竖轴y_b及横轴z_b分别存在横滚误差角δα_γ、航向误差偏角及俯仰误差角δα_θ，记为：

c系与b系间的安装误差矩阵记为所以视觉里程计在导航坐标系下的位置误差表示为：

其中，(δα)×为安装误差角向量的反对称矩阵：

δα与δS的乘积为高阶小量，忽略不计。

×为视觉里程计在相机坐标系下位置输出：

因此在状态转移矩阵F(t)中：

建立上述误差模型后，基于卡尔曼滤波方法进行状态估计。组合导航系统的可观测性与载体运动状态相关，载体的运动轨迹需要满足能够使安装误差与尺度误差快速可观测。

根据本发明一实施例：载体在具有前向、侧向速度和矢量方向变化的机动下，可实现安装误差与尺度误差可观测，仿照实现方式的简便性来看，可仿照“S型”轨迹运动。本领域技术人员应当理解，上述运动轨迹设计可以根据实际情况进行改动，本发明不限于此。

可见，现有的机械里程计安装误差标定都是只考虑了两个角度，而视觉里程计是输出三个方向位置，涉及到了俯仰方向的安装误差角，现有方法大多是采集数据离线标定，对于大型无人机等设备不具有便利性。本发明通过误差建模与卡尔曼滤波的方式实现了安装误差、尺度误差的在线估计，大幅度提高了估计结果的精度，且安装误差估计方法具有便利性与普适性。

如上针对一种实施例描述和/或示出的特征可以以相同或类似的方式在一个或更多个其它实施例中使用，和/或与其它实施例中的特征相结合或替代其它实施例中的特征使用。

应该强调，术语“包括/包含”在本文使用时指特征、整件、步骤或组件的存在，但并不排除一个或更多个其它特征、整件、步骤、组件或其组合的存在或附加。

本发明以上的方法可以由硬件实现，也可以由硬件结合软件实现。本发明涉及这样的计算机可读程序，当该程序被逻辑部件所执行时，能够使该逻辑部件实现上文所述的装置或构成部件，或使该逻辑部件实现上文所述的各种方法或步骤。本发明还涉及用于存储以上程序的存储介质，如硬盘、磁盘、光盘、DVD、flash存储器等。

这些实施例的许多特征和优点根据该详细描述是清楚的，因此所附权利要求旨在覆盖这些实施例的落入其真实精神和范围内的所有这些特征和优点。此外，由于本领域的技术人员容易想到很多修改和改变，因此不是要将本发明的实施例限于所例示和描述的精确结构和操作，而是可以涵盖落入其范围内的所有合适修改和等同物。

本发明未详细说明部分为本领域技术人员公知技术。

Claims (5)

1.一种基于滤波的惯性/视觉里程计安装误差估计方法，其特征在于，所述方法包括：

分别计算世界坐标系下视觉里程计解算的三个轴向位置与导航坐标系下惯导解算的三个轴向位置信息；

将世界坐标系下视觉里程计解算的位置转换到导航坐标系下，获取视觉里程计在导航坐标系下的位置信息；

根据导航坐标系下惯导解算的位置信息与视觉里程计在导航坐标系下的位置信息的差值构造观测量Z_k；并计算量测矩阵H_k；

确定误差模型；

利用卡尔曼滤波方法，基于所述误差模型、观测量Z_k以及量测矩阵H_k对系统误差进行估计，并根据滤波得到的误差估计结果对惯导系统误差进行修正；

其中：

通过下式根据导航坐标系下惯导解算的位置信息与视觉里程计在导航坐标系下的位置信息的差值构造观测量Z_k：

其中，p_n表示惯导位置真值，δp_n表示惯导的位置误差，表示视觉里程计在导航坐标系下的位置真值，/>表示视觉里程计的位置误差；

通过下式获取视觉里程计的位置误差

其中，

δα_γ、/>δα_θ分别为绕惯导纵轴x_b、竖轴y_b及横轴z_b存在的横滚误差角、航向误差偏角及俯仰误差角；δS为视觉里程计的尺度误差，/> 为视觉里程计在相机坐标系下的位置信息，/>分别代表视觉里程计在相机坐标系下x、y、z三个方向的位置输出，/>为地理坐标系到导航坐标系的姿态转移矩阵；

所述确定误差模型，包括：

确定惯导组合系统状态变量；

确定惯导组合系统状态方程；

采用如下式的系统状态变量：

其中：

δVⁿ＝(δV_N,δV_U,δV_E)

δPⁿ＝(δL,δH,δλ)

ε^b＝(ε_bx,ε_by,ε_bz)

δV_N,δV_U,δV_E分别表示惯导系统在导航坐标系下北、天、东向的速度误差；

δL,δH,δλ分别表示惯导系统在导航坐标系下纬度、高度、经度误差；

分别表示惯导系统在导航坐标系下北、天、东向失准角；

ε_bx,ε_by,ε_bz分别表示惯导系统在载体坐标系下X、Y、Z方向的陀螺漂移；

分别表示惯导系统在载体坐标系下X、Y、Z方向的加速度计零偏；

分别表示视觉里程计在导航坐标系下北、天、东方向的位置误差；

分别表示在载体坐标系下视觉里程计与惯导之间的安装误差角；

δS表示导航坐标系下视觉里程计的尺度误差。

2.根据权利要求1所述的方法，其特征在于，通过下式获取视觉里程计在导航坐标系下的位置真值

其中，为地理坐标系到导航坐标系的姿态转移矩阵，/>为相机坐标系到地理坐标系的转换矩阵，/>为初始化成功时刻建立的世界坐标系与相机坐标系之间的姿态转移矩阵，为世界坐标系下视觉里程计解算的位置。

3.根据权利要求1所述的方法，其特征在于，通过下式基于误差方程系数关系构造量测矩阵H_k：

4.根据权利要求1所述的方法，其特征在于，采用下式获取状态方程：

其中，为状态变量，/>为白噪声向量，F(t)为状态转移矩阵。

5.根据权利要求4所述的方法，其特征在于，所述状态转移矩阵采用下式获取：

其中，子矩阵F₁→F₁₀由捷联惯导误差方程建立，F_O1→F_O2由视觉里程计误差方程建立，如下所示：