[发明专利]一种基于惯性辅助的高效视觉里程计

权利要求书

1.一种基于惯性辅助的高效视觉里程计，其特征在于：包括固定在一载体上的视觉传感器和惯性传感器，通过惯性传感器的数值计算载体导航信息的预测值，根据预测值计算载体导航信息的变化量并对其进行判断，变化量满足条件时，输出预测值，变化量不满足条件时，利用视觉传感器数据对预测值进行修正，并输出修正值；所述惯性传感器包括加速度计和陀螺仪，视觉里程计的工作方法包括以下步骤：

步骤1：间隔一定时间采集加速度计信息陀螺仪数据和视觉传感器数据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表示矩阵的转置，其中q_o(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)、和分别为k-1时刻载体的机体系相对于导航系的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；和分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；

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

其中：

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

Q(k-1)＝[q_o(k-1) q₁(k-1) q₂(k-1) q₃(k-1)]^T

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

ΔT为离散采样周期；

其中通过下式计算：

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

2)速度预测公式如下：

其中：

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

为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；

g＝[0 0 g]^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时刻时间内与均未大于阈值，则将其累加，并同时将时间累加，再进行判断；具体判断公式如下：

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

否则，认为导航信息变化量满足条件，直接输出预测值，并跳转至步骤1；

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