基于量测模型优化的惯性/视觉里程计组合导航定位方法与流程

本发明涉及一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，属于惯性/视觉里程计组合导航技术领域。

背景技术

近年来，随着无人机、无人车和机器人等智能运动体的研制发展，对导航系统性能提出更高要求。

在现有技术下，广泛应用于智能运动体的导航系统多采用惯性器件(imu-加速度计和陀螺仪)/卫星组合导航系统实现。但考虑到在城市、丛林、室内等卫星信号受到干扰或失效的环境，imu/卫星组合导航系统将难以提供准确的导航信息。视觉里程计技术是一种基于相关图像序列处理的六自由度运动估计方法，由于其仅依赖于被动测量的相机，具有自主性好、隐蔽性好、结构简单、成本低等优点，必将成为未来智能运动体导航系统的重要信息单元。

惯性导航系统的误差主要由惯性传感器(imu-加速度计和陀螺仪)测量误差引起，惯性导航系统的误差随时间累积，但是其测量精度不会受到突然机动的影响，而视觉里程计由于采用递推的方式求解位姿，视觉里程计不可避免地存在累积误差，且这种累积误差和基于光电编码器的里程计类似，是空间相关而非时间相关的；并且当运动体机动过大时，会由于图像场景变化过大而造成匹配失效进而影响定位精度。视觉里程计根据对图像信息利用的不同，主要分为利用特征点进行跟踪匹配的特征点法，以libviso、ptam等开源算法为代表，和直接利用像素灰度信息匹配的直接法，以svo、lsd-slam等开源算法为代表。各种代表性的算法运动估计方法不同，并且运动体的机动与输出位姿噪声的关系难以准确建模。考虑到两者的互补性，基于惯性导航和视觉里程计的组合导航受到广泛的重视，但现有的imu和视觉里程计的组合导航大多直接使用姿态和位置组合，忽视了视觉里程计定位误差累积和与机动相关的特点。如何实现惯性导航和视觉里程计导航信息的有效融合，具有重要的研究意义。

技术实现要素：

为解决现有技术的不足，本发明的目的在于提供一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，在载体运动过程中有效利用视觉里程计的角速度、线速度和位置信息，实现与惯性导航的最优融合，提高组合导航系统的精度。

为了实现上述目标，本发明采用如下的技术方案：

一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，

步骤1)建立组合导航系统的状态方程，将惯性传感器误差扩展为系统状态变量，包括陀螺的随机常数漂移、陀螺仪的一阶马尔科夫过程漂移和加速度计的一阶马尔科夫过程漂移；

步骤2)结合视觉里程计输出的相对位姿，得到相对位姿到导航系下的姿态和位置的转换关系；

步骤3)选择组合导航系统的量测变量，构建惯性/视觉里程计组合卡尔曼滤波量测方程；

步骤4)对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对系统状态量进行反馈校正。

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤1)中系统的状态方程为：

其中，x为状态向量，为捷联惯导系统中的数学平台误差角，分别为x、y、z的三轴数学平台误差角；δvⁿ为载体的速度误差，载体在地理系下x、y、z轴上的速度误差；δpⁿ为载体的位置误差，δl、δλ、δh分别为经度、纬度、高度误差；ε^b为陀螺仪的随机常数漂移，分别为陀螺仪x、y、z轴的随机常数漂移；为陀螺仪的一阶马尔科夫过程漂移，分别为陀螺仪x、y、z轴的一阶马尔科夫过程漂移；为加速度计的一阶马尔科夫过程漂移，为加速度计x、y、z轴的一阶马尔科夫过程漂移；

上标n表示地理系，上标b表示载体系，下标i表示惯导系统解算值，下标v表示视觉里程计解算值；

其中，为地理系相对于惯性系的角速率误差，为地理系相对于惯性系的角速率，表示从载体系到地理系的坐标变换矩阵，fⁿ为地理系下加速度计的输出比力，为地球系相对于惯性系的角速率误差，为地理系相对于地球系的角速率误差，为地球系相对于惯性系的角速率，为地理系相对于地球系的角速率，δgⁿ为重力加速度误差，vⁿ为载体在地理系下的速度，为载体在地理系下x轴上的速度，rm为卯酉圈半径，rn为子午圈半径，h为当地海拔高度，l为当地纬度，tg和ta分别为一阶马尔科夫过程的相关时间，wr和wa分别为相关的驱动白噪声。

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤2)中相对位姿到导航系下的姿态和位置的转换关系为：其中，为k帧图像时刻的载体系到地理系的坐标变换矩阵，为相机系到载体系的坐标变换矩阵，和为视觉里程计测得k+1帧图像时刻相对于k帧图像时刻的旋转矩阵和平移向量，为k帧图像时刻载体在地理系下的位置，上标t表示矩阵的转置，下标k和k+1表示对应帧时刻的物理量。

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤3)的量测方程为：

其中，为载体相对于地理系的角速度误差，δvⁿ为载体的速度误差，δpⁿ为载体的位置误差，下标i表示惯导系统解算值，下标v表示视觉里程计解算值，h为系统的量测系数矩阵，v为系统的量测噪声阵；

其中，表示从地理系到载体系的坐标变换矩阵，mv和mp分别是与速度和位置相关的系数，vx和vy分别为载体速度在x轴和y轴上的分量，为的模值，

i为单位矩阵。

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤4)的具体过程为：

401)将系统状态方程和量测方程离散化处理：

xk＝φk,k-1xk-1＋γk,k-1wk-1，zk＝hkxk+vk，其中，xk为tk时刻系统状态量,xk-1为tk-1时刻系统状态量，φk,k-1为tk-1时刻至tk时刻系统的状态转移矩阵，γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，wk-1为tk-1时刻系统的噪声矩阵，zk为tk时刻系统的姿态量测矩阵,hk为tk时刻的姿态量测系数矩阵，vk为tk时刻的姿态观测量的噪声矩阵；

402)对离散化处理得到的公式加入控制项uk-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：其中，bk,k-1为tk-1时刻至tk时刻系统的控制项系数矩阵；

403)得到系统的线性化卡尔曼滤波器方程：

其中，为tk-1时刻至tk时刻一步预测状态量；为tk-1时刻滤波状态估计量；为tk时刻滤波状态估计量；kk为tk时刻滤波增益矩阵；zk为tk时刻的新息序列；pk,k-1为tk-1时刻至tk时刻一步预测协方差矩阵；pk-1为tk-1时刻滤波状态估计协方差矩阵；φk,k-1^t为φk,k-1的转置矩阵；γk-1为tk-1时刻系统噪声系数矩阵；qk-1为tk-1时刻的系统观测噪声估计协方差阵；γk-1^t为γk-1的转置矩阵；为hk的转置矩阵；rk为tk时刻的量测噪声估计协方差阵；pk为tk时刻滤波状态估计协方差矩阵；i为单位矩阵；

404)根据步骤403)得到的线性化卡尔曼滤波器方程估计系统导航误差值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于量测模型优化的惯性/视觉里程计组合导航修正值。

本发明所达到的有益效果：本发明方法将视觉里程计作为角速度、线速度以及位置传感器以获取量测数据进而构建量测方程，符合视觉里程计输出的噪声特性，保证了对视觉里程计输出信息的有效利用，具有广泛的适用性；本发明方法应用卡尔曼滤波方法对惯性/视觉里程计组合导航系统的状态量实现了最优估计，利用惯性导航和视觉里程计的互补性，有效地提高组合导航系统的精度，适合工程应用。

附图说明

图1是本发明的架构图；

图2是本发明所使用的kitti数据集车辆真实运动轨迹示意图；

图3(a)-(c)是本发明的导航结果与使用姿态位置组合的导航结果比较图，图3(a)是位置误差比较曲线，图3(b)是姿态误差比较曲线，图3(c)是速度误差比较曲线。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

如图1所示，本发明所述的惯基于量测模型优化的惯性/视觉里程计组合导航方法的原理是：视觉里程计模块对双目相机输出的原始图像进行畸变校正，随后对序列图像进行特征提取与立体匹配，再进行特征点的跟踪，从而估计出相对位姿。卡尔曼滤波器模块通过建立卡尔曼滤波状态方程和量测方程，利用陀螺仪和加速度计原始输出进行捷联惯导解算，当到了滤波周期后，分别进行时间更新和量测更新，实现对组合导航系统状态量的最优估计，以提高组合导航系统性能。

本发明的具体实施方式如下：

步骤1)建立惯性/视觉里程计组合导航算法的状态方程：

首先定义坐标系：地理坐标系选取“东-北-天”为导航系(n)，载体坐标系(b)选取为“右-前-上”。需要估计的状态量如式所示：

式中，x为状态向量，为捷联惯导系统中的数学平台误差角，分别为x、y、z的三轴数学平台误差角；δvⁿ为载体的速度误差，载体在地理系下x、y、z轴上的速度误差；δpⁿ为载体的位置误差，δl、δλ、δh分别为经度、纬度、高度误差；ε^b为陀螺仪的随机常数漂移，分别为陀螺仪x、y、z轴的随机常数漂移；为陀螺仪的一阶马尔科夫过程漂移，分别为陀螺仪x、y、z轴的一阶马尔科夫过程漂移；为加速度计的一阶马尔科夫过程漂移，为加速度计x、y、z轴的一阶马尔科夫过程漂移。上标n表示地理系，上标b表示载体系；

imu的测量值作为状态方程的输入变量，状态方程表示为：

式中，为地理系相对于惯性系的角速率误差，为地理系相对于惯性系的角速率，表示从载体系到地理系的坐标变换矩阵，fⁿ为地理系下加速度计的输出比力，为地球系相对于惯性系的角速率误差，为地理系相对于地球系的角速率误差，为地球系相对于惯性系的角速率，为地理系相对于地球系的角速率，δgⁿ为重力加速度误差，vⁿ为载体在地理系下的速度，为载体在地理系下x轴上的速度，rm为卯酉圈半径，rn为子午圈半径，h为当地海拔高度，l为当地纬度，tg和ta分别为一阶马尔科夫过程的相关时间，wr和wa分别为相关的驱动白噪声。

步骤2)建立基于量测模型优化的惯性/视觉里程计组合导航算法的量测方程

视觉里程计通过对序列图像信息的匹配和跟踪估算相邻图像间相机的运动，它的输出是前后两个连续采样时刻的相对位姿，用旋转矩阵和平移向量来表示，k与k+1表示图像的帧数。

下式表示了相对位姿到导航系下的姿态和位置的转换关系。

式中，表示从相机系到载体系的坐标变换矩阵，一般认为在实验前已经标定好且在一次实验中保持不变。下标k和k+1表示载体在对应帧数图像的时刻的相应物理量的大小。

由于基于不同解算原理和方法的视觉里程计噪声特性不同，因此不失一般性，将视觉里程计视为一个黑箱，直接观测得到的相对位姿的噪声不会累积，近似于白噪声，因此噪声体现在姿态角增量和平移向量上，即：

其中，δγ，δθ，δφ表示从k和k+1时刻横滚角、俯仰角、航向角的变化量，表示带误差的横滚角、俯仰角、航向角测量值，wγ，wθ，wφ，wt分别表示横滚角、俯仰角、航向角、平移向量的白噪声。

基于以上分析的视觉里程计噪声特性，选取惯导系统和视觉里程计解算出的角速度误差的差值、线速度误差差值和位置误差差值作为组合系统的观测量，如下式所示：

其中，表示载体相对于地理系的角速度误差，δvⁿ为载体的速度误差，δpⁿ为载体的位置误差。下标i表示惯导系统解算值，下标v表示视觉里程计解算值。h为系统的量测矩阵。

的值可以由陀螺仪输出计算得到，如式所示：

其中，为陀螺仪输出，为地球自转角速率，为载体运动引起的角速度。

因此，忽略二阶小项，可以表示为：

的值可以根据欧拉角微分方程，由姿态变化角速率转换得到，如下式所示：

其中，为在三轴上的分量。

为系统的状态量，可以由k到k+1时刻的视觉位置估计变换计算。根据式(5)～(7)，系统的量测矩阵h如下式所示：

其中，符号×是向量叉乘的简化表示，mv和mp分别是与速度和位置相关的系数，为的模值，i为单位矩阵。

步骤3)建立基于量测模型优化的惯性/视觉里程计组合导航卡尔曼滤波模型

31)将系统状态方程和量测方程离散化处理：

xk＝φk,k-1xk-1+γk,k-1wk-1(9)

zk＝hkxk+vk(10)

其中，xk为tk时刻系统状态量,xk-1为tk-1时刻系统状态量，φk,k-1为tk-1时刻至tk时刻系统的状态转移矩阵，γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，wk-1为tk-1时刻系统的噪声矩阵，zk为tk时刻系统的姿态量测矩阵,hk为tk时刻的姿态量测系数矩阵，vk为tk时刻的姿态观测量的噪声矩阵。

32)对离散化处理得到的公式加入控制项uk-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：

其中，bk,k-1为tk-1时刻至tk时刻系统的控制项系数矩阵。

33)得到系统的线性化卡尔曼滤波器方程：

其中，为tk-1时刻至tk时刻一步预测状态量；为tk-1时刻滤波状态估计量；为tk时刻滤波状态估计量；kk为tk时刻滤波增益矩阵；zk为tk时刻的新息序列；pk,k-1为tk-1时刻至tk时刻一步预测协方差矩阵；pk-1为tk-1时刻滤波状态估计协方差矩阵；φk,k-1^t为φk,k-1的转置矩阵；γk-1为tk-1时刻系统噪声系数矩阵；qk-1为tk-1时刻的系统观测噪声估计协方差阵；γk-1^t为γk-1的转置矩阵；为hk的转置矩阵；rk为tk时刻的量测噪声估计协方差阵；pk为tk时刻滤波状态估计协方差矩阵；i为单位矩阵。

为了验证发明所提出的基于量测模型优化的惯性/视觉里程计组合导航方法的正确性和有效性，采用本发明方法建立模型，利用kitti数据集进行半物理仿真验证。测试车辆运行的真实轨迹如图2所示。

基于所选取的数据集，基于本发明所述的基于量测模型优化的惯性/视觉里程计组合导航方法进行验证，并与姿态位置直接组合的卡尔曼滤波进行比较，导航误差比较曲线如图3(a)-图3(c)所示。

图3(a)-图3(c)中实线代表本发明误差曲线，虚线代表姿态位置直接组合的卡尔曼滤波误差曲线。从图3(a)-图3(c)导航误差比较曲线可以看出，采用本发明提出的基于量测模型优化的惯性/视觉里程计组合导航方法，惯性导航系统精度相较于姿态位置直接组合的卡尔曼滤波有明显提高，具有有益的工程应用价值。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。