一种视觉导航/惯性导航的松散组合导航方法与流程

本发明涉及导航技术领域，尤其涉及一种视觉导航/惯性导航的松散组合导航方法。

背景技术：

惯性导航系统(inertialnavigationsystemins)是利用惯性敏感器(陀螺仪和加速度计)测量得到的载体运动的角速率和加速度，依据牛顿力学定律即惯性定律，通过积分运算得到载体位置、速度、姿态等运动参数的装置或系统。惯性导航自主性、隐蔽性好，能连续提供多种较高精度的导航参数，抗干扰能力好，但缺点是误差随时间积累，因此长时间工作的导航精度较差，为了消除累计的误差，需要引入外部观测值来对惯导系统进行修正。目前尚没有完备的视觉导航/惯性导航的组合导航研究。

技术实现要素：

本发明的目的就是解决惯性导航误差随时间增长的缺陷而提供的一种视觉导航/惯性导航的松散组合导航方法。

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

一种视觉导航/惯性导航的松散组合导航方法，该方法包括以下步骤：

步骤一，惯性导航解算：选取当地东北天地理坐标系作为导航坐标系，陀螺仪敏感载体的角速率，加速度计来敏感载体的非引力即比力，通过姿态解算公式求出载体的姿态转换矩阵，从而求出载体的姿态信息；导航解算公式求出载体对地的速度和载体当前的位置。

步骤二，视觉导航参数解算：相机标定，求解出相机成像模型参数；室内导航环境中放置导航标志，相机对导航标志成像并训练成模版，并将模板存储在模版数据库中；通过固联在移动机器人前方垂直向下安置的车载摄像头，获取移动机器人在运动过程中所走过路面的图像序列，然后通过基于特征点的跟踪匹配方法，依据图像序列间的特征点的检测、跟踪及匹配，以及摄像机成像模型计算得到所跟踪的特征点在两图像间的像素位移、速度；固连在载体前方的相机，在载体运动过程中拍摄影像，并进行模版匹配，匹配成功执行步骤三，没有匹配成功时候利用纯惯导导航。

步骤三，视觉导航修正惯性导航：以卡尔曼滤波器为手段，取惯性导航的误差方程为滤波状态方程，利用视觉导航解算得到的载体位置、速度与惯性导航解算得到的位置、速度相减，得到的差值作为卡尔曼滤波的量测，从而估算出惯性导航参数误差和器件误差，从而对惯性导航进行反馈校正，得到惯导系统导航位置参数的最优估值，达到利用视觉系统来修正惯性导航系统位置参数的目的。

本发明的有益效果是：视觉导航修正惯导系统，通过卡尔曼滤波器不仅可以估计出惯导系统的导航参数误差(位置、速度、姿态误差)，还可以估计出器件误差(陀螺仪漂移、加速度计零偏)，并对导航参数误差进行补偿，将器件误差反馈到下一个周期的导航计算中，从而使得惯导系统保持长时间高精度。在视觉导航解算失败(模版匹配失败)的情况下，组合导航完全依靠惯导来提供载体的导航参数，由于之前对惯导的各种误差进行了估计和校正，所以在短时间内惯导系统依然保持高精度，不会因为视觉导航解算失败而影响其精度。

附图说明

下面结合附图对本发明作进一步的说明。

图1本发明的步骤流程示意图；

图2视觉导航系统原理框图；

图3视觉导航系统修正惯导系统原理框图；

图4ins与组合导航的位置误差对比图；

图5ins与组合导航的速度误差对比图；

具体实施方式

下面结合具体实施例对本发明作详细的说明。

如图1所示，该实施例的步骤如下：

步骤一，惯性导航解算；

具体是：

选取当地东北天地理坐标系作为导航坐标系，载体坐标系(b系)的三轴指向为“右、前、上”；

(1)初始对准：平台式惯导是物理平台和导航坐标系对准；捷联式惯导是给定初始姿态转换矩阵，即给定载体坐标系(b系)和导航坐标系(n系)之间的关系。

(2)姿态更新：根据陀螺仪敏感的角速率，采用四元数法作姿态更新，然后将更新后的四元数转化成姿态转换矩阵

(3)姿态角提取，由于载体的姿态就是b系相对于n系之间的方位关系，即可根据姿态转换矩阵中的元素值，计算载体的姿态(航向角、俯仰角、翻滚角)；

(4)计算载体位置参数：1)利用姿态变换矩阵将加速度计敏感的比力变化到导航坐标系中解算2)有害加速度的补偿：对fⁿ进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度a，a＝fⁿ-(2ωie+ωen)*vⁿ+gⁿ；4)积分求取载体的速度增量得到时刻载体速度；5)积分求取载体在地球表面的位置增量得到当前时刻载体的位置；

步骤二，视觉导航解算(如图2所示)；

具体是：

(1)摄像机标定：通过halcon的相机标定算法，通过标定板对相机标定；

(2)在导航环境中放置导航标志物并对其编号、标定导航标志的位置信息，相机依次对其成像并通过机器视觉算法模版训练，建立mysol数据库存储模版信息；通过固联在移动机器人前方垂直向下安置的车载摄像头，获取移动机器人在运动过程中所走过路面的图像序列，然后通过基于surf特征点的跟踪匹配方法，依据图像序列间的特征点的检测、跟踪及匹配，计算得到所跟踪的特征点在两图像间的像素位移，根据摄像机的成像模型计算得到移动机器人的速度以及位置信息。通过固连在载体前方的相机在载体运动过程中采集图像并进行模版匹配，当运动到导航标志附近时，模版匹配成功，执行步骤三，利用卡尔曼滤波器进行数据融合，估算出最优位置参数，否则利用纯惯导导航。

步骤三，视觉导航修正惯导系统(如图3所示)；

具体是：

如图3所示：视觉导航系统修正惯导系统：以卡尔曼滤波器为手段，取惯性导航的误差方程为滤波状态方程。从图中可以看出，系统的量测值时惯性导航计算得到的位置与利用视觉导航解算得到的载体位置、速度参数与惯性导航解算得到的参数相减，得到的差值作为卡尔曼滤波的量测，从而估算出惯性导航参数误差和器件误差，从而对惯性导航进行反馈校正；

(1)取惯导系统的误差方程作为卡尔曼滤波器的状态方程，本文选取六状态即速度误差、位置误差x＝f·x+g·ω，其中，x为误差状态向量，包含3个方向的位置误差σr，速度误差σv，x＝(σr，σv)；

(2)将视觉导航计算得到的位置、速度与惯性导航计算得到的位置、速度之差作为卡尔曼滤波的量测值，并建立量测方程如下：zr,za分别表示位置与速度量测值，hrha分别表示位置误差量测矩阵，速度误差量测矩阵；vr，va分别表示位置误差量测和速度误差量测的残差；其中，zr,＝rins-rvo；rins为惯性导航解算的载体位置坐标，rvo为视觉导航解算的位置坐标，hr＝[i0]，i为3*3单位矩阵，0为3*3零矩阵；速度误差za＝ains-avo，ains为惯性导航解算的速度avo为视觉导航解算的速度，速度误差量测矩阵ha＝[i0]，ha中0阵为3*3矩阵；至此，卡尔曼滤波的数学模型已经确定，通过滤波可以估计出惯导系统的导航参数误差和敏感器件误差，并进行补偿和反馈，从而使得惯导系统保持长时间高精度。

图4、图5：本次实验过程中，相机的图片采样率和惯性导航敏感器件的采样率均为50hz。载体平台的初始位置为32.0585on，118.7886oe，初始状态位置与速度误差都设置为0；控制移动机器人以大约0.5m/s左右的速度沿直线运动，共采集获得的数据量为890个，经过处理最后获得的东向以及北向速度与位置的误差曲线图，显示在视觉修正惯导系统的过程中，惯性导航的位置与速度误差。