CN104833352A - 多介质复杂环境下高精度视觉/惯性组合导航方法 - Google Patents

Abstract

本发明提供了一种多介质复杂环境下高精度的视觉/惯性组合导航方法，适用于透明介质内复杂电磁环境中水下航行器的组合导航，利用水下航行器模拟空间微重力环境中飞船卫星的交会对接，为空间交会对接提供重要的测量数据和试验验证。整个实验系统在地面进行，通过水的浮力，地面电磁场的方向和磁场强度的调整，使得水下航行器处于中性浮力水平，从而满足天地一致性的等效试验条件。本发明提出的多介质复杂环境下高精度视觉/惯性组合导航方法可以控制水下航行器模拟空间交会对接，方便对其姿态、速度、位置和加速度信息进行准确测量。

Description

多介质复杂环境下高精度视觉/惯性组合导航方法

【技术领域】

本发明为多介质复杂环境下高精度视觉/惯性组合导航方法，属于组合导航技术领域。

【背景技术】

近年来，在地面模拟空间的微重力环境是进行空间飞行器地面试验的重要组成部分，而利用水下航行器模拟空间微重力环境中飞船卫星的交会对接，为空间交会对接提供重要的测量数据和试验验证。西北工业大学飞行器设计国家重点实验室建立了相应的试验系统，在地面进行水下航行器的自主交会对接，通过水的浮力和调整地面电磁场的方向和磁场强度，使得水下航行器处于中性浮力水平，从而满足天地一致性的等效试验条件。而完成交会对接任务的一个重要前提是高精度的导航系统提供精确的测量信息。

目前，常用的导航方式有：诸如惯性导航、视觉导航、GPS的卫星导航、多普勒雷达导航等单系统导航方式，以及里程计/INS组合导航、GNSS/INS组合导航、多普勒雷达/INS组合导航系统等[1]。然而，在诸如室内、水下等环境中，GPS导航无法提供有效的导航信息。在强电磁等复杂中，多普勒雷达等无线电导航方式也将失效。因此，当载体运动处于多介质复杂环境下时，很多导航方法并不能实施。

惯性导航系统(Inertial Navigation System,INS)由惯性测量单元(Inertial Measurement Unit,IMU)、采集装置、导航解算软件和导航计算机组成，具有自主性、全天候工作、导航信息全等优点。但由于其导航方式为递推方式，误差积累会造成导航结果发散，并且在复杂电磁场环境中会加剧这种发散，因此需尽量消除复杂电磁场环境对IMU输出数据的影响，但限于目前IMU的材料和工艺，并不能完全隔离电磁场环境对其的影响，可以利用算法补偿的方法消除电磁场的影响。在需要长航时导航时，除了隔离复杂电磁场环境影响外，INS必须与误差不积 累的导航系统进行组合才能提供高精度的导航信息。

视觉导航系统(Vision Position System,VPS)由1个高速双目CCD相机、采集装置、系统测量软件、工控机组成。通过双目CCD相机进行目标图像采集，通过采集装置将相机采集到的图像传输到工控机，系统测量软件对相机的图像进行处理分析，构建各特征点的成像光路图，利用多介质双目测量原理计算出目标航行器各特征点的三维空间坐标值，进而计算出运动体的三维空间位姿测量值[2][3]。

VPS系统的主要误差来源为图像处理误差和光噪声误差等，误差不随时间积累，在室内、水下、陆上和空间等介质中均可应用，并且其测量不受电磁场等环境的影响，因此，将VPS和INS进行组合导航，为多介质复杂环境下的载体提供高精度视觉/惯性组合导航方法。

参考文献

[1]张仁勇,罗建军,马卫华,苏二龙.交会对接视觉相对导航系统半物理仿真[J].计算机仿真,2012,06:75-79.

[2]王俊,朱战霞,贾国华,张旭阳.多介质下空间目标的视觉测量[J].计算机应用,2011,05:1431-1434.

[3]Li M,Mourikis A I.Optimization-Based Estimator Design for Vision-Aided Inertial Navigation[C].Robotics:Science and Systems,2012. 

[4]张昊,石磊,涂俊峰,管乐鑫,解永春.基于交会对接CCD光学成像敏感器的双目测量算法[J].空间控制技术与应用,2011,06:66-71.

【发明内容】

本发明的目的是提出一种多介质复杂环境下高精度视觉/惯性组合导航方法。本方法适用于透明介质内复杂电磁环境中水下航行器的组合导航，为多介质复杂环境下的水下航行器提供高精度的视觉/惯性导航方法。

一种多介质复杂环境下高精度视觉/惯性组合导航装置，适用于透明介质内复杂电磁环境中水下航行器的组合导航，包括水下航行器，该水下航行器放置在透明介质的容器内，该容器底部放置有电磁铁阵列，水下航行器的四周均安装有一个推进器，在其中的一个推进器前端安装有用于抓取的对接机构，所述水下航行器的正下方安装有一个永磁体，用以调节水下航行器实现中性浮力水平；所述水下航行器内装有惯性导航系统，该惯性导航系统内设置有惯性测量单元，所述惯性导航系统通过光纤完成数据和指令的传输；在所述透明介质的外围设置有视觉导航系统，该视觉导航系统包括有双目CCD相机。

一种基于所述装置的多介质复杂环境下高精度视觉/惯性组合导航方法，利用水下航行器模拟空间微重力环境中飞船卫星的交会对接，为空间交会对接提供重要的测量数据和试验验证；整个实验系统在地面进行，通过水的浮力，地面电磁场的方向和磁场强度的调整，使得水下航行器处于中性浮力水平，从而满足天地一致性的等效试验条件。

所述组合导航系统，具体包括以下步骤：

(1)采用双目CCD相机对水下航行器进行图像采集，实现水下航行器位置的测量；

(2)建立电磁场强度与惯性测量单元中陀螺测得的角增量之间的关系；建立电磁场强度与惯性测量单元中加速度计测得的速度增量之间的关系；

(3)根据实时测量的电磁场强度对陀螺和加速度计的测量数据进行补偿；

(4)对视觉导航系统的状态进行判断，若发生异常，则对视觉导航系统得到的位置信息进行隔离，若正常，则对信息进行处理并将时钟信息传递给惯性导航系统；

(5)对惯性导航系统的状态进行判断，若异常，则对惯性导航系统得到的数据进行隔离，若正常，则根据视觉导航系统的时标信息对惯性导航系统进行时间同步和数据拟合；

(6)惯性导航系统根据步骤(5)数据拟合的结果进行自适应周期的导航解算，输出水下航行器相对于当地地理坐标系下的姿态、速度、位置和加速度信息；

(7)视觉导航系统根据步骤(4)的结果输出水下航行器相对于视觉坐标系的位置信息；

(8)若视觉导航系统和惯性导航系统的工作状态均正常，则将惯性导航系统和视觉导航系统的结果统一到导航坐标系，然后转入步骤(9)进行组合导航模式；

(9)根据惯性导航系统的姿态误差角方程、速度误差方程、位置误差方程、陀螺和加速度计误差模型以及视觉导航系统的位置误差方程建立组合导航卡尔曼滤波器的状态方程，根据惯性导航系统与视觉导航系统的位置差建立组合导航系统卡尔曼滤波器的量测方程，根据导航时间选择输出校正或反馈校正的组合导航方式。

所述步骤(1)的具体方法为：

(1.1)对双目CCD相机进行标定，建立左右两个相机的位置关系，建立视觉导航系统坐标系；

(1.2)采用双目CCD相机对水下航行器进行图像采集；

(1.3)根据光线在空气、透明介质、水中的折射率建立折射光线路径方程。

所述水下航行器在视觉导航坐标系下的位置坐标为p＝[x y z]^Τ，由以下公式计算得到：

$p=\frac{1}{2}(A\·s+{Q^{\′}}_L+{Q^{\′}}_R)$

其中，

A＝[w_L w_R]，w_L＝[w_L1 w_L2 w_L3]^Τ，w_R＝[w_R1 w_R2 w_R3]^Τ，s＝(A^Τ·A)^-1·A^Τ·(Q′_R-Q′_L)，Q′_L＝[x′_L y′_L z′_L]^Τ，Q′_R＝[x′_R y′_R z′_R]^Τ。

所述步骤(5)中所述的数据拟合方法为：视觉导航系统和惯性导航系统同时启示，当视觉导航系统采集到水下航行器的图像时，自动记录当前时间信息并传递给惯性导航系统；对惯性导航系统中陀螺和加速度计的输出，建立时间和输出的线性内插模型，拟合出陀螺和加速度计在视觉导航系统采样时刻的输出信息。

所述陀螺和加速度计的在VPS采样时刻的输出信息如下述公式：

$\left\{\begin{array}{c}\mathrm{\Δ\θ}\left(t\right)=\frac{t}{T}\·\mathrm{\Δ\θ}\\ \mathrm{\ΔV}\left(t\right)=\frac{t}{T}\·\mathrm{\ΔV}\end{array}\right.,0\≤t\≤T$

其中，Δθ(t)和ΔV(t)分别为陀螺和加速度计采样开始到VPS采样时刻的增量输出，Δθ和ΔV分别为陀螺和加速度计在IMU采样周期T内的增量信息，t为对VPS采样时刻对应到陀螺和加速度计采样周期内的时刻。

所述步骤(6)的自适应周期的导航解算方法为：根据初始姿态信息和经过时间同步拟合后陀螺输出的角增量信息进行姿态解算，根据初始速度信息和经过时间同步拟合后的加速度计输出的速度增量信息进行速度解算，根据初始位置信息和解算得到的速度信息进行位置解算。

在所述组合导航模式中，输出校正和反馈校正的原则是：根据惯性测量单元的精度设定时间阈值，导航时间小于时间阈值时，采用输出校正算法；导航时间大于时间阈值时，采用反馈校正算法。

与现有技术相比，本发明的有益效果：本发明多介质复杂环境下高精度视觉/惯性组合导航方法不受空气、透明介质和水多种介质和复杂电磁场环境的影响，可以连续的输出水下航行器的姿态、速度、位置和加速度导航信息；通过拟合算法完成时间同步，避免时间不同步造成的组合导航发散问题；在单系统出现故障时，可以进行故障隔离，避免单系统故障造成导航信息污染，从而尽最大能力为系统提供导航信息；根据导航系统工作时间，选择输出校正或反馈校正方式，保证组合导航系统工作在最佳方式。

【附图说明】

图1-1和图1-2是分别为本发明中多介质视觉导航系统结构示意图和原理图；

图2是多介质复杂环境下视觉/惯性组合导航方法原理图；

图3是多介质复杂环境下视觉/惯性组合导航方法时间同步原理图；

图4是应用多介质复杂环境下视觉/惯性组合导航方法的水下航行器交会对接轨迹图；

图5-1至图5-3为应用多介质复杂环境下视觉/惯性组合导航方法的水下航行器交会对接导航结果图。

其中，图1中，1为VPS系统，11为双目CCD相机，3为装有水和透明介质的容器，31为水下航行器，35为电磁铁阵列。

【具体实施方式】

下面结合附图对本发明方法的实施方式做详细说明。本发明的原理图如图2所示，其具体步骤包括：

步骤1，通过对高速双目CCD相机进行标定，建立左右相机的位置关系，建立VPS视觉导航坐标系(视觉导航坐标系以相机安装位置为原点，X轴指向西向、Y轴指向南向、Z轴指向天向构成的右手坐标系)，采用高速双目CCD相机对水下航行器的进行图像采集，根据光线在空气、透明介质、水中的折射率和建立多次折射光线路径方程，实现多介质传输路径补偿，消除空气、透明介质和水的折射对水下航行器位置观测的影响，实现水下航行器位置的精确测量，水下航行器的位置坐标为p＝[x y z]^Τ由公式(1)计算得到，结构图和原理图如1所示。

$p=\frac{1}{2}(A\·s+{Q^{\′}}_L+{Q^{\′}}_R)---\left(1\right)$

其中，A＝[w_L w_R]，w_L＝[w_L1 w_L2 w_L3]^Τ，w_R＝[w_R1 w_R2 w_R3]^Τ，s＝(A^Τ·A)^-1·A^Τ·(Q′_R-Q′_L)，Q′_L＝[x′_L y′_L z′_L]^Τ，Q′_R＝[x′_R y′_R z′_R]^Τ。

步骤2，IMU中陀螺和加速度计测量得到的水下航行器相对于惯性坐标系(以地心为原点，X轴和Y轴在赤道平面内，X轴指向春分点，Z轴指向地轴构成的右手坐标系)的角增量和速度增量信息，通过电磁场强度对IMU中陀螺和加速度计的输出进行标定测试(标定测试指建立电磁场强度与角增量之间的关系，建立电磁场强度与速度增量之间的关系)，完成电磁场强度对陀螺和加速度计的输出数据影响的建模，根据实时测量的电磁场强度对陀螺和加速度计数据进行补偿，隔离电磁场对陀螺和加速度计输出的影响。该步骤的另外一种表达方式是：根据 不同电磁场强度对IMU输出的影响进行标定，拟合出电磁场强度与陀螺和加速度计输出数据之间的模型，通过实时测量电磁场的强度，对陀螺和加速度计输出数据实时进行补偿，隔离电磁场对陀螺和加速度计数据的影响。

步骤3，对VPS的状态进行检查，若异常，则对VPS信息进行隔离(隔离指舍弃该信息)；若正常，则进行信息处理，得到水下航行器在视觉坐标下的位置信息，并将时钟信息传递给INS。对INS的状态进行判断，若异常，应进行信息隔离；若正常，根据VPS的时标信息进行时间同步和数据拟合，由于VPS数据输出频率不固定，因此需对根据VPS的时标信息对IMU进行时间同步，陀螺和加速度计的数据进行数据拟合。

所述拟合的具体过程如下：

VPS与INS同时启动，VPS应用计算机的时钟计时，而INS时钟由内部晶振提供，陀螺和加速度计固定频率输出。当VPS采集到水下航行器的图像时，自动记录当前时间信息并传递给INS。同步原理图见图3，将陀螺和加速度计的输出建立时间和输出的线性内插模型，拟合出陀螺和加速度计的在VPS采样时刻的输出信息。如公式(2)所示：

$\left\{\begin{array}{c}\mathrm{\Δ\θ}\left(t\right)=\frac{t}{T}\·\mathrm{\Δ\θ}\\ \mathrm{\ΔV}\left(t\right)=\frac{t}{T}\·\mathrm{\ΔV}\end{array}\right.,0\≤t\≤T---\left(2\right)$

其中，Δθ(t)和ΔV(t)分别为陀螺和加速度计采样开始到VPS采样时刻的增量输出，Δθ和ΔV分别为陀螺和加速度计在IMU采样周期T内的增量信息，t为对VPS采样时刻对应到陀螺和加速度计采样周期内的时刻。

步骤4，完成陀螺和加速度计信息的拟合后，INS进行自适应周期的导航解算，得到水下航行器在当地地理坐标系下的到姿态、速度、位置和加速度信息。自适应周期的导航解算方法为：根据初始姿态信息和经过时间同步拟合后陀螺输出的角增量信息进行姿态解算，根据初始速度信息和经过时间同步拟合后加速度计输出的速度增量信息进行速度解算，根据初始位置信 息和解算得到的速度信息进行位置解算。输出水下航行器相对于当地地理坐标系(以航行器质心为原点，X轴指向东向、Y轴指向北向、Z轴指向天向构成的右手坐标系)下的姿态、速度、位置和加速度信息。VPS进行信息处理，输出水下航行器相对于视觉坐标系的位置信息。

步骤5，若本发明中的两个系统(指VPS和INS两个系统)工作状态均正常，将INS和VPS导航结果经坐标变换后统一到导航坐标系(以视觉坐标系原点为原点，X轴指向西向、Y轴指向南向、Z轴指向天向构成的右手坐标系)。然后进入组合导航模式，由于VPS数据采集频率不固定，因此当VPS数据更新时进行组合导航，组合导航周期不固定。根据INS的姿态误差角方程、速度误差方程、位置误差方程、陀螺和加速度计误差模型以及VPS位置误差方程建立组合导航卡尔曼滤波器的状态方程，根据INS与VPS的位置差建立组合导航系统卡尔曼滤波器的量测方程。根据导航时间选择输出校正或反馈校正的组合导航方式，短时间导航时(指导航时间小于设定的时间阈值)，INS精度较高，采用输出校正算法；长时间导航时(指导航时间小于设定的时间阈值)，INS误差积累导致性能下降，采用反馈校正算法。所述时间阈值时根据惯性测量单元的精度设定的。

本发明所述的多介质指在空气、透明介质和水，复杂环境指复杂的电磁场环境。

水下航行器悬浮在透明介质内，易于实验人员对水下航行器的运行状态进行观测和操作。验证：

结合图4和图5说明本发明的实例验证，水下航行器放置在长宽高为4m×4m×3m的透明玻璃水缸中，水缸底部为均匀放置的电磁铁阵列，通过电磁控制系统调节电磁场强度和方向。水下航行器为金属六面体，四周各有一个推进器，并且在一个推进器的前端，安装有可完成抓取功能的对接机构。在水下航行器的正下方安装有一个永磁体，与电磁场进行配合调节水下航行器实现中性浮力水平。IMU安装于水下航行器中，控制系统通过一根光纤完成IMU数据和推进器的控制指令的传输。高速双目CCD相机安装于距水缸1.5m的地方，与工控机一起构成 VPS系统。目标星悬浮在固定位置，水下航行器按照设计的轨道进行交会对接，试验过程中的计算条件和技术参数如下：

(1)IMU测量频率为200Hz，陀螺的随机漂移为0.03°/h，随机游走为一阶马尔科夫相关时间为300s，加速度计的等效零偏为50μg；

(2)视觉导航系统输出频率为20Hz—40Hz，位置测量随机误差为0.05m，测量噪声方差为0.05；

(3)水下航行器初始位置在地球坐标系的投影为[34.21° 108.91° 415m]，初始速度为[0 0 0]m/s，初始姿态由INS自对准提供；

(4)基于本发明的视觉/惯性组合导航方法与上述的技术参数，水下航行器完成交会对接任务的试验轨迹如图4所示，试验时间为1800s。由图5可以看出，组合导航后的姿态误差最大为0.04°，组合导航后的速度误差和位置误差均收敛，并且速度误差收敛在0.03m/s以内，最大位置误差在0.05m以内；

(5)由试验结果可知，本发明方法能够利用视觉/惯性进行组合导航，在多介质复杂环境下提供高精度的导航信息。

Claims (9)

1.一种多介质复杂环境下高精度视觉/惯性组合导航装置，适用于透明介质内复杂电磁环境中水下航行器的组合导航，其特征在于：包括水下航行器，该水下航行器放置在透明介质的容器内，该容器底部放置有电磁铁阵列，水下航行器的四周均安装有一个推进器，在其中的一个推进器前端安装有用于抓取的对接机构，所述水下航行器的正下方安装有一个永磁体，用以调节水下航行器实现中性浮力水平；所述水下航行器内装有惯性导航系统，该惯性导航系统内设置有惯性测量单元，所述惯性导航系统通过光纤完成数据和指令的传输；在所述透明介质的外围设置有视觉导航系统，该视觉导航系统包括有双目CCD相机。

2.一种基于权利要求1所述装置的多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：组合导航方法利用水下航行器模拟空间微重力环境中飞船卫星的交会对接，为空间交会对接提供重要的测量数据和试验验证；整个实验系统在地面进行，通过水的浮力，地面电磁场的方向和磁场强度的调整，使得水下航行器处于中性浮力水平，从而满足天地一致性的等效试验条件。

3.根据权利要求2所述的一种多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：具体包括以下步骤：

(1)采用双目CCD相机对水下航行器进行图像采集，实现水下航行器位置的测量；

(2)建立电磁场强度与惯性测量单元中陀螺测得的角增量之间的关系；建立电磁场强度与惯性测量单元中加速度计测得的速度增量之间的关系；

(3)根据实时测量的电磁场强度对陀螺和加速度计的测量数据进行补偿；

(4)对视觉导航系统的状态进行判断，若发生异常，则对视觉导航系统得到的位置信息进行隔离，若正常，则对信息进行处理并将时钟信息传递给惯性导航系统；

(5)对惯性导航系统的状态进行判断，若异常，则对惯性导航系统得到的数据进行隔离，若正常，则根据视觉导航系统的时标信息对惯性导航系统进行时间同步和数据拟合；

(6)惯性导航系统根据步骤(5)数据拟合的结果进行自适应周期的导航解算，输出水下航行器相对于当地地理坐标系下的姿态、速度、位置和加速度信息；

(7)视觉导航系统根据步骤(4)的结果输出水下航行器相对于视觉坐标系的位置信息；

(8)若视觉导航系统和惯性导航系统的工作状态均正常，则将惯性导航系统和视觉导航系统的结果统一到导航坐标系，然后转入步骤(9)进行组合导航模式；

(9)根据惯性导航系统的姿态误差角方程、速度误差方程、位置误差方程、陀螺和加速度计误差模型以及视觉导航系统的位置误差方程建立组合导航卡尔曼滤波器的状态方程，根据惯性导航系统与视觉导航系统的位置差建立组合导航系统卡尔曼滤波器的量测方程，根据导航时间选择输出校正或反馈校正的组合导航方式。

4.根据权利要求3所述的一种多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：所述步骤(1)的具体方法为：

(1.1)对双目CCD相机进行标定，建立左右两个相机的位置关系，建立视觉导航系统坐标系；

(1.2)采用双目CCD相机对水下航行器进行图像采集；

(1.3)根据光线在空气、透明介质、水中的折射率建立折射光线路径方程。

5.根据权利要求3或4所述的一种多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：所述水下航行器在视觉导航坐标系下的的位置坐标为p＝[x y z]^Τ，由以下公式计算得到：

$p=\frac{1}{2}(A\·s+Q_L^{\′}+Q_R^{\′})$

其中，

A＝[w_L w_R]，w_L＝[w_L1 w_L2 w_L3]^Τ，w_R＝[w_R1 w_R2 w_R3]^Τ，s＝(A^Τ·A)^-1·A^Τ·(Q′_R-Q′_L)，Q′_L＝[x′_L y′_L z′_L]^Τ，Q′_R＝[x′_R y′_R z′_R]^Τ。

6.根据权利要求3所述的一种多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：所述步骤(5)中所述的数据拟合方法为：视觉导航系统和惯性导航系统同时启动，当视觉导航系统采集到水下航行器的图像时，自动记录当前时间信息并传递给惯性导航系统；对惯性导航系统中陀螺和加速度计的输出，建立时间和输出的线性内插模型，拟合出陀螺和加速度计在视觉导航系统采样时刻的输出信息。

7.根据权利要求6所述的一种多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：所述陀螺和加速度计的在VPS采样时刻的输出信息如下述公式：

$\left\{\begin{array}{c}\mathrm{\Δ\θ}\left(t\right)=\frac{t}{T}\·\mathrm{\Δ\θ}\\ \mathrm{\ΔV}\left(t\right)=\frac{t}{T}\·\mathrm{\ΔV}\end{array}\right.,0\≤t\≤T$

其中，Δθ(t)和ΔV(t)分别为陀螺和加速度计采样开始到VPS采样时刻的增量输出，Δθ和ΔV分别为陀螺和加速度计在IMU采样周期T内的增量信息，t为对VPS采样时刻对应到陀螺和加速度计采样周期内的时刻。

8.根据权利要求3所述的一种多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：所述步骤(6)的自适应周期的导航解算方法为：根据初始姿态信息和经过时间同步拟合后陀螺输出的角增量信息进行姿态解算，根据初始速度信息和经过时间同步拟合后加速度计输出的速度增量信息进行速度解算，根据初始位置信息和解算得到的速度信息进行位置解算。

9.根据权利要求3所述的一种多介质复杂环境下高精度视觉/惯性组合导航方法，其特征在于：在所述组合导航模式中，输出校正和反馈校正的原则是：根据惯性测量单元的精度设定时间阈值，导航时间小于时间阈值时，采用输出校正算法；导航时间大于时间阈值时，采用反馈校正算法。