一种基于惯性辅助的高效视觉里程计的制作方法

本发明涉及视觉导航技术领域，尤其涉及一种基于惯性辅助的高效视觉里程计。

背景技术

视觉里程计(vo,visualodometry)是视觉实时定位与构图(v-slam，visualsimultaneouslocalizationandmapping)的技术关键，也是导航技术领域中的一大研究热点，同样是无人驾驶等技术的关键。目前导航技术常用的方法是卫星导航，然而卫星导航易受高楼、隧道和室内等环境干扰。当卫星导航不可用时，视觉导航是常用的导航方式。视觉传感器可以直接的感知外界，不需要外部主动传感器，比如gps、uwb等的辅助，对外界依赖少，具有高自主性，且视觉传感器成本较低，可以较快的投入市场。v-slam方法是slam方法中应用主流且具有极大发展潜质的方法，vo作为v-slam技术的核心，同样具有广阔的发展前景。

目前大多v-slam都需要较好的光照以及高纹理环境，通常利用视觉传感器获取的图像数据提取特征点进行匹配或通过直接法在光照不变假设下进行求解位姿，而进行特征点的提取与匹配需要耗费大量的时间，对处理器具有极高的要求，尽管直接法相比较特征点法耗时较少，任需要一定的计算能力，继而对于处理平台的运算能力提出了要求，而嵌入式平台运算能力往往不能满足需求，进而无法实现实时运算。

技术实现要素：

本发明所要解决的技术问题在于提供一种基于惯性辅助的高效视觉里程计，通过将视觉里程计与惯性传感器融合，预测视觉传感器载体导航信息，从而降低视觉位姿解算的频率，提高算法实时性。

本发明是通过以下技术方案解决上述技术问题的：

一种基于惯性辅助的高效视觉里程计，包括固定在一载体上的视觉传感器和惯性传感器，通过惯性传感器的数值获得载体导航信息的预测值，根据预测值计算载体导航信息的变化量并对其进行判断，变化量满足条件时，输出预测值，变化量不满足条件时，利用视觉传感器数据对预测值进行修正，并输出修正值。

优选地，所述惯性传感器包括加速度计和陀螺仪，视觉里程计的工作方法包括以下步骤：

步骤1：间隔一定周期采集k时刻加速度计信息陀螺仪数据和视觉传感器数据s(k)；

步骤2：利用惯性传感器采集数据对载体导航信息进行预测得到预测值；

步骤3：根据步骤2的预测值确定载体导航信息的变化量；

步骤4：当导航信息变化量满足条件时，输出步骤2获得的载体导航信息预测值，并跳转至步骤1；当变化量不满足条件时，进行视觉数据解算；

步骤5：构建卡尔曼滤波器，根据解算的视觉数据结果，对k时刻惯性传感器预测的导航信息进行修正，并输出修正值；

步骤6：跳转至步骤1；

所述导航信息包括载体的位姿、速度、位置、误差信息。

优选地，对系统坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系，其中x轴、y轴与z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建导航坐标系，其中x轴、y轴与z轴分别与初始时刻载体的右向、前向和天向重合；

基于上述坐标系，步骤2中预测载体的导航信息的方法为：以ξ(k-1)作为k-1时刻载体的导航信息，则可根据下式递推载体k时刻的预测导航信息

上标t表示矩阵的转置，其中qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、和分别为k-1时刻载体的机体系相对于导航系的姿态四元数、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量；和分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量；

1)姿态四元数预测公式如下：

其中：

为k时刻预测的姿态四元数，上标t表示矩阵的转置；

q(k-1)＝[qo(k-1)q1(k-1)q2(k-1)q3(k-1)]^t

为k-1时刻的姿态四元数；

δt为离散采样周期；

其中通过下式计算：

其中，为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系x、y、z轴上的分量，为k时刻陀螺仪的零偏在机体系x、y、z轴上的分量；

2)速度预测公式如下：

其中：

为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系x、y、z轴上的分量；

为k时刻加速度计零偏在机体系x、y、z轴上的分量；

g＝[00g]^t，g为当地重力加速度值；

为k时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；

为k-1时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；

为机体系到导航系之间的姿态矩阵，通过下式计算：

3)位置预测公式如下：

其中：

为k时刻的位置，分别为载体k时刻在导航系x、y、z轴上的位置坐标；

为k-1时刻的位置，分别为飞行器k-1时刻在导航系x、y、z轴上的位置坐标；

4)加速度计零偏和陀螺仪零偏预测公式如下：

其中，为k-1时刻加速度计零偏在机体系x、y、z轴上的分量；为k-1时刻陀螺仪的零偏在机体系x、y、z轴上的分量。

优选地，步骤3中确定载体导航信息变化量的方法为：

1)从k-1时刻到k时间内，预测得到的载体导航信息的变化量：

即

其中，和分别为k-1时刻到k时刻时间内载体的机体系相对于导航系的姿态四元数、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量产生的变化量；

2)计算导航坐标系下k-1到k时刻时间内载体位移变化量的模长与角度变化量的模长

其中，为俯仰角的变化，为横滚角的变化，为航向角的变化。

优选地，步骤4中导航信息变化量是否满足条件的判断方法为：

如果导航信息变化量满足以下任一公式：

则认为导航信息变化量不满足条件，需要进行视觉数据解算；

如果k-1时刻到k时刻时间内与均未大于阈值，则将其累加，并同时将时间累加，再进行判断；具体判断公式如下：

如果导航信息变化量满足上面任一组公式，则认为不满足条件，需要进行视觉数据解算；

否则，认为导航信息变化量满足条件，直接输出预测值；

其中，δ为当前运算平台上需要计算一帧视觉里程计的耗时，

优选地，步骤4中利用视觉里程计算法进行图像匹配及位姿解算，具体方法为特征点法、直接法或半直接法中的任意一种。

优选地，视觉解算方法为：

通过视觉位姿解算得到变换矩阵t(k-1,k):

上式中t(k-1,k)表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵r，其中r包含9个元素，同时还包含一个平移矩阵t，其中包含三个平移元素；

令iγⁿ(k)、iθⁿ(k)、iψⁿ(k)、和分别为使用slam求得的k时刻载体机体系相对于导航系的横滚角、俯仰角、航向角、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量；

则姿态角计算公式如下：

位置计算公式如下：

slam输出的位姿公式如下：

取k时刻载体机体系相对于导航系的航向角、x轴坐标、y轴坐标和z轴坐标作为量测信息；

优选地，步骤5中构建卡尔曼滤波进行导航信息预测值修正正的方法为：

1)计算一步预测均方误差p(k|k-1)：

p(k|k-1)＝a(k,k-1)p(k-1|k-1)a(k,k-1)^t+g(k-1)w(k-1)g(k-1)^t

式中，

i3×3为3×3的单位矩阵，i4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵，a(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；p(k-1|k-1)为k-1时刻的状态估计均方差，p(k|k-1)为k-1时刻到k时刻的一步预测均方差；

g为滤波器k-1时刻的滤波器噪声系数矩阵；

w为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为的噪声标准差；

2)计算k时刻扩展卡尔曼滤波器滤波增益k(k)：

k(k)＝p(k|k-1)h(k)^t[h(k)p(k|k-1)h(k)^t+r(k)]^-1

式中，

其中，

h(k)为k时刻量测矩阵，k(k)为k时刻的滤波增益；

r(k)为k时刻的量测噪声，diag表示矩阵对角化，其中分别为iψⁿ的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；

3)计算k时扩展卡尔曼滤波器状态估计值

式中，

为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤4的预测公式计算得到k时刻的量测值；

4)计算k时刻扩展卡尔曼滤波器估计均方误差p(k|k)：

p(k|k)＝[i-k(k)h(k)]p(k|k-1)

式中，p(k|k)为k时刻估计均方误差，i为单位矩阵。

本发明提供的一种基于惯性辅助的高效视觉里程计的优点在于：通过惯性传感器对载体运动情况进行预测，降低视觉里程计的计算强度，提高算法实时性。

附图说明

图1是本发明的实施例所提供的一种基于惯性辅助的高效视觉里程计的原理框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。

一种基于惯性辅助的高效视觉里程计，包括固定在一载体上的视觉传感器和惯性传感器，惯性传感器包括加速度计和陀螺仪，视觉里程计固定于一载体上，参考图1，所述视觉里程计的具体工作方法包括以下步骤：

步骤1：间隔一定周期采集k时刻加速度计信息陀螺仪数据和视觉传感器数据s(k)；

同时对系统坐标系进行定义：以当前时刻载体的位置为原点构建机体坐标系，其中x轴、y轴与z轴分别与当前时刻载体的右向、前向和天向重合。以初始时刻载体的位置为原点构建导航坐标系，其中x轴、y轴与z轴分别与初始时刻载体的右向、前向和天向重合。

步骤2：利用惯性传感器采集数据对载体导航信息进行预测；所述导航信息包括载体的位姿、速度、位置、误差信息。

具体预测载体导航信息的方法为：以ξ(k-1)作为k-1时刻载体的导航信息，则可根据下式递推载体k时刻的预测导航信息

上标t表示矩阵的转置，其中qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、和分别为k-1时刻载体的机体系相对于导航系的姿态四元数、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量；和分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量；

1)姿态四元数预测公式如下：

其中：

为k时刻预测的姿态四元数，上标t表示矩阵的转置；

q(k-1)＝[qo(k-1)q1(k-1)q2(k-1)q3(k-1)]^t

为k-1时刻的姿态四元数；

δt为离散采样周期；

其中通过下式计算：

其中，为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系x、y、z轴上的分量，为k时刻陀螺仪的零偏在机体系x、y、z轴上的分量；

2)速度预测公式如下：

其中：

为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系x、y、z轴上的分量；

为k时刻加速度计零偏在机体系x、y、z轴上的分量；

g＝[00g]^t，g为当地重力加速度值；

为k时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；

为k-1时刻机体系相对于导航系的线速度在导航系x、y、z轴上的分量；

为机体系到导航系之间的姿态矩阵，通过下式计算：

3)位置预测公式如下：

其中：

为k时刻的位置，分别为载体k时刻在导航系x、y、z轴上的位置坐标；

为k-1时刻的位置，分别为飞行器k-1时刻在导航系x、y、z轴上的位置坐标；

4)加速度计零偏和陀螺仪零偏预测公式如下：

其中，为k-1时刻加速度计零偏在机体系x、y、z轴上的分量；为k-1时刻陀螺仪的零偏在机体系x、y、z轴上的分量。

步骤3：根据步骤2得到的载体导航信息预测值确定导航信息的变化量；具体方法为：

1)从k-1时刻到k时间内，预测得到的载体导航信息的变化量：

即

其中，和分别为k-1时刻到k时刻时间内载体的机体系相对于导航系的姿态四元数、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量产生的变化量；

2)计算导航坐标系下k-1到k时刻时间内载体位移变化量的模长与角度变化量的模长

其中，为俯仰角的变化，为横滚角的变化，为航向角的变化。

步骤4：对导航信息变化量进行判断，当导航信息变化量满足条件时，输出步骤2得到的载体导航信息预测值，并跳转至步骤1；当变化量不满足条件时，进行视觉数据解算；

上述导航信息变化量的判断方法为：

如果导航信息变化量满足以下任一公式：

则认为导航信息变化量不满足条件，需要进行视觉数据解算；

如果k-1时刻到k时刻时间内与均未大于阈值，则将其累加，并同时将时间累加，再进行判断；具体判断公式如下：

如果导航信息变化量满足上面任一公式，则认为不满足条件，需要进行视觉数据解算；否则，认为导航信息变化量满足条件，直接输出预测值，并跳转至步骤1；

其中β和δ为由传感器误差特性与载体的速度综合得到的常量；具体为：δ为当前运算平台上需要计算一帧视觉里程计的耗时，

进行视觉解算的方法可以是特征点法、直接法或半直接法中的任意一种，本实施例采用的具体方法为：

通过视觉位姿解算得到变换矩阵t(k-1,k):

上式中t(k-1,k)表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵r，其中r包含9个元素，同时还包含一个平移矩阵t，其中包含三个平移元素；

令iγⁿ(k)、iθⁿ(k)、iψⁿ(k)、和分别为使用slam求得的k时刻载体机体系相对于导航系的横滚角、俯仰角、航向角、x轴坐标、y轴坐标、z轴坐标、x轴速度分量、y轴速度分量和z轴速度分量；

则姿态角计算公式如下：

位置计算公式如下：

slam输出的位姿公式如下：

取k时刻载体机体系相对于导航系的航向角、x轴坐标、y轴坐标和z轴坐标作为量测信息；

步骤5：构建卡尔曼滤波器，根据解算的视觉数据结果，对k时刻惯性传感器预测的导航信息预测值进行修正正，并输出修正值；

构建卡尔曼滤波进行导航信息修正的具体方法为：

1)计算一步预测均方误差p(k|k-1)：

p(k|k-1)＝a(k,k-1)p(k-1|k-1)a(k,k-1)^t+g(k-1)w(k-1)g(k-1)^t

式中，

i3×3为3×3的单位矩阵，i4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵，a(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；p(k-1|k-1)为k-1时刻的状态估计均方差，p(k|k-1)为k-1时刻到k时刻的一步预测均方差；

g为滤波器k-1时刻的滤波器噪声系数矩阵；

w为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为的噪声标准差；

2)计算k时刻扩展卡尔曼滤波器滤波增益k(k)：

k(k)＝p(k|k-1)h(k)^t[h(k)p(k|k-1)h(k)^t+r(k)]^-1

式中，

其中，

h(k)为k时刻量测矩阵，k(k)为k时刻的滤波增益；

r(k)为k时刻的量测噪声，diag表示矩阵对角化，其中分别为iψⁿ的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；

3)计算k时扩展卡尔曼滤波器状态估计值

式中，

为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤4的预测公式计算得到k时刻的量测值；

4)计算k时刻扩展卡尔曼滤波器估计均方误差p(k|k)：

p(k|k)＝[i-k(k)h(k)]p(k|k-1)

式中，p(k|k)为k时刻估计均方误差，i为单位矩阵。

步骤6：跳转至步骤1。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，在不脱离本发明的精神和原则的前提下，本领域普通技术人员对本发明所做的任何修改、等同替换、改进等，均应落入本发明权利要求书确定的保护范围之内。