多介质复杂环境下高精度视觉/惯性组合导航方法
【专利说明】多介质复杂环境下高精度视觉/惯性组合导航方法 【技术领域】
[0001] 本发明为多介质复杂环境下高精度视觉/惯性组合导航方法，属于组合导航技术 领域。 【【背景技术】】
[0002] 近年来，在地面模拟空间的微重力环境是进行空间飞行器地面试验的重要组成部 分，而利用水下航行器模拟空间微重力环境中飞船卫星的交会对接，为空间交会对接提供 重要的测量数据和试验验证。西北工业大学飞行器设计国家重点实验室建立了相应的试验 系统，在地面进行水下航行器的自主交会对接，通过水的浮力和调整地面电磁场的方向和 磁场强度，使得水下航行器处于中性浮力水平，从而满足天地一致性的等效试验条件。而完 成交会对接任务的一个重要前提是高精度的导航系统提供精确的测量信息。
[0003] 目前，常用的导航方式有：诸如惯性导航、视觉导航、GPS的卫星导航、多普勒雷达 导航等单系统导航方式，以及里程计/INS组合导航、GNSS/INS组合导航、多普勒雷达/INS 组合导航系统等[1]。然而，在诸如室内、水下等环境中，GPS导航无法提供有效的导航信 息。在强电磁等复杂中，多普勒雷达等无线电导航方式也将失效。因此，当载体运动处于多 介质复杂环境下时，很多导航方法并不能实施。
[0004] 惯性导航系统（Inertial Navigation System, INS)由惯性测量单元（Inertial Measurement Unit, IMU)、采集装置、导航解算软件和导航计算机组成，具有自主性、全天候 工作、导航信息全等优点。但由于其导航方式为递推方式，误差积累会造成导航结果发散， 并且在复杂电磁场环境中会加剧这种发散，因此需尽量消除复杂电磁场环境对MU输出数 据的影响，但限于目前MU的材料和工艺，并不能完全隔离电磁场环境对其的影响，可以利 用算法补偿的方法消除电磁场的影响。在需要长航时导航时，除了隔离复杂电磁场环境影 响外，INS必须与误差不积累的导航系统进行组合才能提供高精度的导航信息。
[0005] 视觉导航系统（Vision Position System, VPS)由1个高速双目CCD相机、采集装 置、系统测量软件、工控机组成。通过双目CCD相机进行目标图像采集，通过采集装置将相 机采集到的图像传输到工控机，系统测量软件对相机的图像进行处理分析，构建各特征点 的成像光路图，利用多介质双目测量原理计算出目标航行器各特征点的三维空间坐标值， 进而计算出运动体的三维空间位姿测量值[2] [3]。
[0006] VPS系统的主要误差来源为图像处理误差和光噪声误差等，误差不随时间积累，在 室内、水下、陆上和空间等介质中均可应用，并且其测量不受电磁场等环境的影响，因此，将 VPS和INS进行组合导航，为多介质复杂环境下的载体提供高精度视觉/惯性组合导航方 法。
[0007] 参考文献
[0008] [1]张仁勇，罗建军，马卫华，苏二龙.交会对接视觉相对导航系统半物理仿真 [J] ·计算机仿真，2012, 06:75-79.
[0009] [2]王俊，朱战霞，贾国华，张旭阳.多介质下空间目标的视觉测量[J].计算机 应用，2011，05:1431-1434.
[0010] [3] Li M, Mourikis A 1.0 ptimization-Based Estimator Design for Vision-Aided Inertial Navigation[C]. Robotics: Science and Systems, 2012.
[0011] [4]张昊，石磊，涂俊峰，管乐鑫，解永春.基于交会对接C⑶光学成像敏感器 的双目测量算法[J].空间控制技术与应用，2011，06:66-71. 【
【发明内容】
】
[0012] 本发明的目的是提出一种多介质复杂环境下高精度视觉/惯性组合导航方法。本 方法适用于透明介质内复杂电磁环境中水下航行器的组合导航，为多介质复杂环境下的水 下航行器提供高精度的视觉/惯性导航方法。
[0013] 一种多介质复杂环境下高精度视觉/惯性组合导航装置，适用于透明介质内复杂 电磁环境中水下航行器的组合导航，包括水下航行器，该水下航行器放置在透明介质的容 器内，该容器底部放置有电磁铁阵列，水下航行器的四周均安装有一个推进器，在其中的一 个推进器前端安装有用于抓取的对接机构，所述水下航行器的正下方安装有一个永磁体， 用以调节水下航行器实现中性浮力水平；所述水下航行器内装有惯性导航系统，该惯性导 航系统内设置有惯性测量单元，所述惯性导航系统通过光纤完成数据和指令的传输；在所 述透明介质的外围设置有视觉导航系统，该视觉导航系统包括有双目CCD相机。
[0014] -种基于所述装置的多介质复杂环境下高精度视觉/惯性组合导航方法，利用水 下航行器模拟空间微重力环境中飞船卫星的交会对接，为空间交会对接提供重要的测量数 据和试验验证；整个实验系统在地面进行，通过水的浮力，地面电磁场的方向和磁场强度的 调整，使得水下航行器处于中性浮力水平，从而满足天地一致性的等效试验条件。
[0015] 所述组合导航系统，具体包括以下步骤：
[0016] (1)采用双目CCD相机对水下航行器进行图像采集，实现水下航行器位置的测量；
[0017] (2)建立电磁场强度与惯性测量单元中陀螺测得的角增量之间的关系；建立电磁 场强度与惯性测量单元中加速度计测得的速度增量之间的关系；
[0018] (3)根据实时测量的电磁场强度对陀螺和加速度计的测量数据进行补偿；
[0019] (4)对视觉导航系统的状态进行判断，若发生异常，则对视觉导航系统得到的位置 信息进行隔离，若正常，则对信息进行处理并将时钟信息传递给惯性导航系统；
[0020] (5)对惯性导航系统的状态进行判断，若异常，则对惯性导航系统得到的数据进 行隔离，若正常，则根据视觉导航系统的时标信息对惯性导航系统进行时间同步和数据拟 合；
[0021] (6)惯性导航系统根据步骤（5)数据拟合的结果进行自适应周期的导航解算，输 出水下航行器相对于当地地理坐标系下的姿态、速度、位置和加速度信息；
[0022] (7)视觉导航系统根据步骤（4)的结果输出水下航行器相对于视觉坐标系的位置 信息；
[0023] (8)若视觉导航系统和惯性导航系统的工作状态均正常，则将惯性导航系统和视 觉导航系统的结果统一到导航坐标系，然后转入步骤（9)进行组合导航模式；
[0024] (9)根据惯性导航系统的姿态误差角方程、速度误差方程、位置误差方程、陀螺和 加速度计误差模型以及视觉导航系统的位置误差方程建立组合导航卡尔曼滤波器的状态 方程，根据惯性导航系统与视觉导航系统的位置差建立组合导航系统卡尔曼滤波器的量测 方程，根据导航时间选择输出校正或反馈校正的组合导