一种惯性/视觉智能化组合导航方法与流程

本发明涉及一种导航方法，属于组合导航领域。

背景技术：

惯性导航系统具有完全自主性、高隐蔽性、强抗干扰性和信息连续性特点，因此惯性导航系统是载体实现自主导航控制的核心传感器之一。但惯性导航系统误差随时间增大而不断漂移，显然单一导航方式难以满足导航系统对精确度、可靠性及实时性的要求。随着视觉传感器、图像处理技术等不断进步，惯性/视觉组合系统是重要发展趋势之一，另外基于视觉导航对相对位姿等导航参数更为敏感，可以用于封闭或复杂环境，从而进一步完善自主定位导航能力。在载体长时间运动时，假设有场景地图，选用景象匹配进行导航，景象匹配在其匹配区域内定位具有较高的定位精度，但由于匹配区域的不连续性导致难以得到长时间连续的导航定位信息，惯性导航常作为辅助手段与惯性系统组合起来。

在现代化高技术战争条件下，战斗机面临日益严峻的挑战，对导航方面的要求也越来越高，要求既能在恶劣的环境下全天候工作，支持各种任务要求，又可以在任何情况下连续地提供最高精度的导航定位信息。对于视觉和惯性组合模式来说。每一种导航设备都具有各自的工作原理、定位精度及优缺点，它们的导航信息都是对平台部分状态的观测量，如果不加区分地对所有导航信息进行融合处理，所获得的结果势必不是最优的，甚至比不组合的结果还差。

技术实现要素：

为了克服现有技术的不足，本发明提供一种基于惯导/视觉智能组合导航方法，对视觉与惯性传感器进行智能化管理和决策，以提高系统的导航定位精度和可靠性，为各类无人系统提供导航参数。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤一，利用陀螺仪和加速度计生成惯性测量单元imu仿真数据，载体在飞行时输出的姿态角速率为载体在载体坐标系相对于地理坐标系下的角速度其中，为地理坐标系到载体坐标系的姿态转换矩阵；

陀螺仪的理想输出其中为地理系相对于惯性系的角速度在载体系的投影，

加速计的理想输出fn为地理系下的比力；

在理想陀螺仪的输出上附加随机误差，包括白噪声、一阶马尔科夫误差和随机常数误差，作为仿真的陀螺仪输出数据；

在理想加速度计的输出上附加随机误差，包括一阶马尔科夫误差，作为加速度计仿真输出数据；

步骤二，进行图像采集，从图像中提取特征并进行特征点跟踪计算，利用光速平差法计算相对坐标系视觉导航参数；通过预积分将两个相机帧之间的imu数据积分到一起，将视觉计算的结果与imu预积分后的结果进行配准，对齐时间戳；

步骤三，将惯性、视觉导航信息以及性能评估结果信息输入智能导航专家系统，由推理机控制知识库中的规则开始运行，读取当前各传感源的状态，生成中间事实并将其保存至综合数据库；然后推理机根据各传感源的实时状态，对知识库中的规则进行匹配，选择当前最佳的导航融合模式，激活并执行对应的规则，向用户输出当前选用决策依据；推理机根据输入的导航源传感器信息对知识库进行搜索，控制满足设定条件时相应的动作被激活，直至推理完成，得到最优的导航决策信息；

步骤四，把景象匹配结果和imu惯性数据进行无迹卡尔曼滤波组合。

本发明的有益效果是：在导航信息组合之前添加了智能决策模式，进行惯性/视觉组合导航工作模式选择策略和切换研究，使组合导航系统智能判断单一传感器及组合导航状态，根据传感器状态完成组合系统自主决策，保证整体导航定位性能，使组合导航获得最高精度。

附图说明

图1是专家系统的基本结构图；

图2是导航专家系统的推理过程图；

图3是本发明步骤流程示意图；

图4是智能决策流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明结合各导航源传感器的特点，提出了一种基于专家系统的智能化组合导航处理方法，探索利用基于规则的专家系统实现组合导航系统中的导航源辅助决策功能，以期实现定位定姿精度较高的导航源组合效果。

导航专家系统如图1所示，主要由综合数据库、推理机和知识库组成。当导航专家系统接收到导航信息以及异常检测结果等输入信息时，首先由推理机控制知识库中的规则开始运行，读取当前各测距源的状态，生成中间事实并将其保存至综合数据库，然后推理机根据各测距源的实时状态，对知识库中的规则进行匹配，选择当前最佳的导航融合模式，激活并执行对应的规则，选取导航源，解释机可以向用户解释并输出当前选用的导航融合模式等的决策依据，最后若导航融合模式切换，可对导航融合模式的性能变化趋势进行决策。

基于规则的专家系统最重要的构成要素为：知识库、推理机和综合数据库。

知识库：包含所有规则本文中的知识库采用框架作为知识表示方法，是基于产生式规则的启发式搜索，所以知识库中包含以规则形式存储的大量导航领域的专业知识。

推理机：对运行进行整体控制推理机是基于规则的专家系统的控制中心，决定哪些规则的前件满足条件，从而激活相应规则的后件去执行对应的操作，推理过程实际上是一个搜索和匹配的过程。推理机根据输入的导航源数据对知识库进行搜索，控制满足一定条件时相应的动作会被激活，直至推理完成，得到最优的导航决策信息。本文研究中的推理机选用正向链推理，采用rete模式匹配算法，从输入的导航源信息出发，使用规则进行启发式搜索，若规则前提匹配，则该规则选中，规则结论加入综合数据库，若问题未完全解出，继续推理，若完成，则退出。该算法极大地提高了推理的效率，为组合导航处理提供快速的决策反应。推理过程如图2所示。

综合数据库：包含推理所需的数据综合数据库用来存储推理过程中得到的各种中间状态、事实、数据、初始状态及目标等。它相当于工作存储器，用来存放用户回答的事实、已知的事实和由推理得到的事实，而且随着问题的不同，数据库的内容也可以是动态变化的。在推理过程中，推理机根据规则的执行情况对事实表进行相应操作，如删除事实、添加事实和修改事实等。

组合导航系统智能化主要研究惯性/视觉组合导航工作模式选择策略和切换研究，使组合导航系统智能判断单一传感器及组合导航状态，根据传感器状态完成组合系统自主决策，保证整体导航定位性能。建立了惯性、视觉传感器知识库，通过导航源工作健康状态、当前任务需求、外界干扰环境等因素进行综合的分析，完成工作模式选择策略和切换研究。

本发明提出智能化组合导航方法，具体步骤如下：

步骤一：生成imu仿真数据：

1.1)利用陀螺仪和加速度计测量原理生成惯性测量单元imu仿真数据，载体在飞行时输出的姿态角速率为根据欧拉角定理可以得到

其中为载体坐标系相对于地理坐标系下的角速度，为地理坐标系到载体坐标系的姿态转换矩阵。

陀螺仪的理想输出为

其中为地理系相对于惯性系的角速度在载体系的投影，

加速计的理想输出为

fn为地理系下的比力。

1.2)在理想陀螺仪的输出上附加随机误差(白噪声、一阶马尔科夫误差、随机常数误差)作为仿真的陀螺仪输出数据。

1.3)在理想加速度计的输出上附加随机误差(一阶马尔科夫误差)作为加速度计仿真输出数据。

步骤二：视觉数据获取，采用摄像机进行图像采集并存储，生成视觉导航数据。

步骤三：imu和视觉传感器初始化：

3.1)对视觉数据的处理是从图像中提取特征并进行特征点跟踪计算，基于structurefrommotion(sfm)的技术利用光速平差法可以计算相对坐标系视觉导航参数。

3.2)由于imu采集数据频率远高于相机帧率，为了实现数据融合，就需要考虑将时间戳对齐，预积分就是把两个相机帧之间的imu数据积分到一起。

3.3)利用经过预处理后的数据将视觉计算的结果与imu预积分后的结果进行配准，对齐其时间戳，并进行初始化。

步骤四：智能决策系统构建

惯性/视觉组合导航系统组合了惯性、视觉导航源传感器，这两种传感器的工作状态、实时性能和几何分布等都不相同，需要对各传感器进行管理和决策，利用当前最佳的融合模式进行组合，以提高系统的导航定位精度和可靠性。

4.1)针对导航工作模式选择策略和切换，通过导航源工作健康状态、当前任务需求、外界干扰环境进行综合的分析，给出对导航源管理、融合模式、信息输出模式的判断或决策结果，作为性能评估结果。智能决策流程图如图3所示。

4.2)将惯性、视觉导航信息以及性能评估结果(异常检测结果等)信息输入智能导航专家系统，

4.3)首先由推理机控制知识库中的规则开始运行，读取当前各传感源的状态，生成中间事实并将其保存至综合数据库，

4.4)然后推理机根据各传感源的实时状态，对知识库中的规则进行匹配，选择当前最佳的导航融合模式，激活并执行对应的规则，解释机可以向用户解释并输出当前选用决策依据，最后若导航融合模式切换，可对导航融合模式的性能变化趋势进行决策。

4.5)推理机根据输入的导航源传感器信息对知识库进行搜索，控制满足一定条件时相应的动作会被激活，直至推理完成，得到最优的导航决策信息，推理过程实际上是一个搜索和匹配的过程。

步骤五：惯性/视觉组合仿真：进行松耦合组合模式，即把景象匹配结果和imu惯性数据进行无迹卡尔曼滤波组合。

本发明的实施例利用imu工作原理生成需要的仿真数据，然后对imu、视觉传感器初始化，接着进行组合方式智能决策，最后进行了惯性/视觉组合仿真分析。

具体实施步骤如下：

步骤一：生成imu仿真数据：

1.1)利用陀螺仪和加速度计测量原理生成惯性测量单元imu仿真数据，导弹载体在飞行时输出的姿态角速率为根据欧拉角定理可以得到

其中为载体坐标系相对于地理坐标系下的角速度，为地理坐标系到载体坐标系的姿态转换矩阵。

陀螺仪的理想输出为

其中为地理系相对于惯性系的角速度在载体系的投影，

加速计的理想输出为

fn为地理系下的比力。

1.2)在理想陀螺仪的输出上附加随机误差(白噪声、一阶马尔科夫误差、随机常数误差)作为仿真的陀螺仪输出数据。

1.3)在理想加速度计的输出上附加随机误差(一阶马尔科夫误差)作为加速度计仿真输出数据。

步骤二：视觉数据获取，采用摄像机进行图像采集并存储，生成视觉导航数据。

步骤三：imu和视觉传感器初始化：

3.1)对视觉数据的处理是从图像中提取特征并进行特征点跟踪计算,基于structurefrommotion(sfm)的技术利用光速平差法可以计算相对坐标系视觉导航参数。

3.2)由于imu采集数据频率远高于相机帧率，为了实现数据融合，就需要考虑将时间戳对齐，预积分就是把两个相机帧之间的imu数据积分到一起。通过定义两个关键帧之间的姿态、速度、位置的相对变化量这几个量可用imu的测量数据表达，与最优估计的状态量无关，因此在迭代优化的过程中，上述预积分量可只计算一次，在更新状态时仅仅使用相对变化量来计算下一关键帧的状态量即可，因此能极大的降低计算量，从而提高在嵌入式系统上的运行速度。

3.3)利用经过预处理后的数据将视觉计算的结果与imu预积分后的结果进行配准，对齐其时间戳，并进行初始化。在配准同时能够从二维的图像序列中恢复出相应的三维信息，其中包括成像相机的运动参数以及场景的结构信息，从而得到一个很基本的状态估计。

步骤四：智能决策系统构建

惯性/视觉组合导航系统组合了惯性、视觉导航源传感器，这两种传感器的工作状态、实时性能和几何分布等都不相同，需要对各传感器进行管理和决策，利用当前最佳的融合模式进行组合，以提高系统的导航定位精度和可靠性。

4.1)由于惯性、视觉传感器导航定位特性不一样，需要对导航源进行抽象，建立各导航源的描述。针对导航工作模式选择策略和切换，通过导航源工作健康状态、当前任务需求、外界干扰环境等因素进行综合的分析，给出对导航源管理、融合模式、信息输出模式等判断或决策结果。智能决策流程图如图4所示。

4.2)将惯性、视觉导航信息以及性能评估结果(异常检测结果等)信息输入智能导航专家系统，

4.3)首先由推理机控制知识库中的规则开始运行，读取当前各传感源的状态，生成中间事实并将其保存至综合数据库，

4.4)然后推理机根据各传感源的实时状态，对知识库中的规则进行匹配，选择当前最佳的导航融合模式，激活并执行对应的规则，解释机可以向用户解释并输出当前选用决策依据，最后若导航融合模式切换，可对导航融合模式的性能变化趋势进行决策。

4.5)推理机根据输入的导航源传感器信息对知识库进行搜索，控制满足一定条件时相应的动作会被激活，直至推理完成，得到最优的导航决策信息，推理过程实际上是一个搜索和匹配的过程。推理机的主要功能是根据相关事实和规则，对各导航源传感器的实时状态进行推理和判断，选择当前可用的最佳融合模式，从而实现系统的融合模式决策。推理机制依赖知识进行。知识以特定的形式存入知识库中，存储的方式应便于用户分析和处理导航源传感器的工作状态，性能和几何分布等状态，实现导航源管理和决策推理。

步骤五：惯性/视觉组合仿真：进行松耦合组合模式，即把景象匹配结果和imu惯性数据进行无迹卡尔曼滤波组合。

(一)标定实施例

设定无人机飞行时间2小时，飞行速度300m/s，惯导定位精度1nmile/h(cep)，惯导更新频率200hz，景象可匹配区定位精度1m(cep)，考虑到景象匹配计算量大，景象匹配定位更新周期1s，在景象匹配失效区域，系统仅工作在纯惯性模式下，此时对系统误差阵进行递推运算，保证导航信息连续性，在景象匹配功能恢复后，整个导航系统又处于组合工作模式，景象匹配更新周期1s时，组合导航定位精度优于2.5m。

实际应用中景象匹配不是一直有效，因此需要考虑景象匹配多久不可用仍然能够保证组合导航定位精度，这为规划匹配区间隔长度、选择地面景象提供参考意义。

在满足组合定位指标要求下，调整景象匹配更新周期，通过参数调整使得惯性/视觉组合定位结果大于5m，此时的时间间隔可以认为是景象匹配的最大时间间隔，也就是景象匹配区间隔的最远距离。

以上述设置条件进行仿真，当组合周期小于10s时候，即飞行距离3km，组合导航定位精度优于5m(cep)，也就是说飞行3km时有景象匹配就可以保证整个过程组合定位精度。

以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。