虚拟圆球模型下极区惯性导航相位调制阻尼方法及系统与流程

1.本发明涉及极区惯性导航技术领域，尤其是指虚拟圆球模型下极区惯性导航相位调制阻尼方法及系统。


背景技术：

2.当前极区惯性导航算法多采用横向坐标系或者格网坐标系以解决导航坐标系极区解算奇异问题。由于地球的椭球特性，横向坐标系和格网坐标系在计算过程中均会产生原理性误差。为此，需要设计椭球模型下的极区惯性导航算法，这也带来了计算复杂度高的缺点。相较于横向椭球模型和格网椭球模型，虚拟圆球模型则较好的利用了圆球模型计算简便及减小原理误差的特点，有利于实现高精度极区导航。但上述方法目前使用过程中依然存在舒拉震荡等误差，使得惯性导航结果出现振荡现象，从而影响导航精度。


技术实现要素：

3.为此，本发明所要解决的技术问题在于克服现有技术中利用虚拟圆球模型实现极区导航存在舒拉震荡等误差，使得惯性导航结果出现振荡现象的问题。
4.为解决上述问题，本发明提供一种虚拟圆球模型下极区惯性导航相位调制阻尼方法，包括如下步骤：
5.步骤s1：采集惯性传感器的实时数据，根据实时数据得到横向导航系姿态；
6.步骤s2：根据实时数据和横向导航系姿态更新横向导航系速度并利用虚拟圆球模型对横向导航系速度进行扩展，得到虚拟圆球模型下横向导航系速度，根据虚拟圆球模型下横向导航系速度，得到横向导航系下载体的第一位置；
7.同时，根据实时数据、设定的速度阻尼系数和横向导航系姿态得到横向导航系相位调制后的速度，利用虚拟圆球模型对横向导航系相位调制后的速度进行扩展，得到虚拟圆球模型下横向导航系相位调制后的速度，根据虚拟圆球模型下横向导航系相位调制后的速度，得到相位调制后横向导航系下载体的第二位置；
8.步骤s3：对横向导航系姿态进行相位调制，得到相位调制后的横向导航系姿态，根据横向导航系姿态和相位调制后的横向导航系姿态得到惯性导航输出姿态，根据虚拟圆球模型下横向导航系速度和虚拟圆球模型下横向导航系相位调制后的速度得到惯性导航输出速度，以及根据横向导航系下载体的第一位置和相位调制后横向导航系下载体的第二位置得到惯性导航输出载体的输出位置，根据所述惯性导航输出姿态、惯性导航输出速度、惯性导航输出载体的输出位置进行导航。
9.在本发明的一种实施方式中，步骤s2中，所述根据实时数据和横向导航系姿态更新横向导航系速度的方法包括：对横向导航系的速度进行更新，其方程为：
[0010][0011]
式中，v
t
表示横向导航系的速度矢量；表示载体系到横向系的方向余弦矩阵；fb表示加速度计测量比力；表示地球系相对惯性系的旋转角速度在横向系的映射；表示横向系相对于地球系的旋转角速度在横向系的映射；g
t
表示重力矢量在横向系的映射。
[0012]
在本发明的一种实施方式中，步骤s1中，所述横向导航系姿态由下式得到：
[0013][0014]
其中，表示载体坐标系b到横向导航坐标系t的方向余弦矩阵；表示载体系相对于横向系的旋转角速度在载体系上的映射，[
·
x]为矢量变斜对称矩阵。
[0015]
在本发明的一种实施方式中，步骤s2中，所述利用虚拟圆球模型对横向导航系速度进行扩展，得到虚拟圆球模型下横向导航系速度的方法为：根据下式得到虚拟圆球模型下横向导航系速度：
[0016][0017]
式中，v
t
表示横向导航系的速度；表示横向系到导航系的方向余弦矩阵；表示导航系到横向系的方向余弦矩阵；k表示扩展系数矩阵，由下式计算：
[0018][0019]
式中，rm表示导航系子午圈曲率半径；rn表示导航系卯酉圈曲率半径；h
t
表示横向系高度。
[0020]
在本发明的一种实施方式中，步骤s2中，所述根据虚拟圆球模型下横向导航系速度，得到横向导航系下载体的第一位置的方法为：对虚拟圆球模型下横向导航系速度利用转换矩阵获得横向导航系下载体的位置p
t
：
[0021][0022]
式中，p
t
表示载体位置矢量在横向系下的映射；vv表示虚拟圆球模型下横向导航系速度；rc表示转换矩阵，表示为：
[0023][0024]
式中，表示横向卯酉圈曲率半径；h
t
表示横向系下高度；l
t
表示横向系纬度。
[0025]
在本发明的一种实施方式中，步骤s2中，所述根据实时数据、设定的速度阻尼系数和横向导航系姿态得到横向导航系相位调制后的速度的方法为：利用下式得到横向导航系相位调制后的速度：
[0026][0027]
其中，表示横向导航系相位调制后的速度矢量；表示载体系到横向系的方向余弦矩阵；fb表示加速度计测量比力；表示地球系相对惯性系的旋转角速度在横向系
的映射；表示横向系相对于地球系的旋转角速度在横向系的映射；g
t
表示重力矢量在横向系的映射；k表示速度阻尼系数。
[0028]
在本发明的一种实施方式中，步骤s3中，所述根据横向导航系姿态和相位调制后的横向导航系姿态得到惯性导航输出姿态的方法为：由下式得到惯性导航输出姿态：
[0029][0030]
其中，am()表示姿态角转姿态矩阵；ma()表示姿态矩阵转姿态角；表示载体系到横向系的方向余弦矩阵；表示相位调制之后的姿态矩阵，由下式得到
[0031][0032]
表示利用相位调制速度计算的载体系相对横向系旋转角速度在载体系下的映射，[
·
×
]为矢量变斜对称矩阵。
[0033]
在本发明的一种实施方式中，步骤s3中，所述根据虚拟圆球模型下横向导航系速度和虚拟圆球模型下横向导航系相位调制后的速度得到惯性导航输出速度的方法为：由下式得到惯性导航输出速度：
[0034][0035]
式中，表示采用虚拟圆球进行扩展之后横向坐标系下相位调制的速度矢量；vv表示采用虚拟圆球进行扩展之后横向坐标系下的速度矢量。
[0036]
在本发明的一种实施方式中，步骤s3中，所述根据横向导航系下载体的第一位置和相位调制后横向导航系下载体的第二位置得到惯性导航输出载体的输出位置的方法为：由下式得到惯性导航输出载体的最终位置：
[0037][0038]
式中，表示调制之后的载体位置矢量；p
t
表示载体位置矢量在横向系下的映射。
[0039]
本发明还提供一种虚拟圆球模型下极区惯性导航相位调制阻尼系统，包括：
[0040]
数据采集模块，用于采集惯性传感器的实时数据，根据实时数据得到横向导航系姿态；
[0041]
速度获取模块，用于根据实时数据和横向导航系姿态更新横向导航系速度并利用虚拟圆球模型对横向导航系速度进行扩展，得到虚拟圆球模型下横向导航系速度；
[0042]
载体的第一位置获取模块，用于根据虚拟圆球模型下横向导航系速度，得到横向导航系下载体的第一位置；
[0043]
相位调制速度获取模块，用于根据实时数据、设定的速度阻尼系数和横向导航系姿态得到横向导航系相位调制后的速度，利用虚拟圆球模型对横向导航系相位调制后的速度进行扩展，得到虚拟圆球模型下横向导航系相位调制后的速度；
[0044]
载体的第二位置获取模块，用于根据虚拟圆球模型下横向导航系相位调制后的速度，得到相位调制后横向导航系下载体的第二位置；
[0045]
惯性导航输出模块，用于对横向导航系姿态进行相位调制，得到相位调制后的横向导航系姿态，根据横向导航系姿态和相位调制后的横向导航系姿态得到惯性导航输出姿态，根据虚拟圆球模型下横向导航系速度和虚拟圆球模型下横向导航系相位调制后的速度得到惯性导航输出速度，以及根据横向导航系下载体的第一位置和相位调制后横向导航系下载体的第二位置得到惯性导航输出载体的输出位置，根据所述惯性导航输出姿态、惯性导航输出速度、惯性导航输出载体的输出位置进行导航。
[0046]
本发明的上述技术方案相比现有技术具有以下优点：
[0047]
本发明为克服传统极区惯性导航方法存在的问题，采用一种虚拟圆球模型极区惯性导航相位调制阻尼方法，在对虚拟圆球模型下极区惯性导航的基础上，利用阻尼系数对惯性导航结果进行相位调制，并对双环路进行融合，减小惯性导航舒拉震荡。
附图说明
[0048]
为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中
[0049]
图1是本发明的虚拟圆球模型下极区惯性导航相位调制阻尼方法流程图；
[0050]
图2是本发明的惯性导航相位调制结构框图；
[0051]
图3本发明惯性导航的纵摇误差图；
[0052]
图4本发明惯性导航的横摇误差图；
[0053]
图5本发明惯性导航的东向速度误差图；
[0054]
图6本发明惯性导航的北向速度误差图。
具体实施方式
[0055]
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
[0056]
实施例一
[0057]
参照图1所示，本实施例提供虚拟圆球模型下极区惯性导航相位调制阻尼方法，包括如下步骤：
[0058]
步骤s1：采采集惯性传感器的实时数据，根据实时数据得到横向导航系姿态；
[0059]
由惯性传感器测量模型可知：
[0060][0061]
式中，表示加速度计测量值；fb表示加速度计测量真实比力；表示加速度计零偏；ωa表示加速度计随机噪声；表示陀螺仪测量值；ωb表示真实角速度；εb表示陀螺仪零偏；wg表示陀螺仪测量噪声。
[0062]
具体地，步骤s1中，陀螺仪量测常值漂移为εb=[0.02 0.02 0.02]
to
/h，t表示矢量转置，
°
/h表示度每小时；陀螺仪量测随机游走噪声系数为加速度计量测常值漂移为
μ表示微，g表示当地重力加速度；加速度计量测随机游走噪声系数为hz表示赫兹；陀螺仪和加速度计的更新频率为200hz。
[0063]
步骤s2：根据实时数据和横向导航系姿态更新横向导航系速度并利用虚拟圆球模型对横向导航系速度进行扩展，得到虚拟圆球模型下横向导航系速度，根据虚拟圆球模型下横向导航系速度，得到横向导航系下载体的第一位置；同时，根据实时数据、设定的速度阻尼系数和横向导航系姿态得到横向导航系相位调制后的速度，利用虚拟圆球模型对横向导航系相位调制后的速度进行扩展，得到虚拟圆球模型下横向导航系相位调制后的速度，根据虚拟圆球模型下横向导航系相位调制后的速度，得到相位调制后横向导航系下载体的第二位置；
[0064]
具体地，步骤s2中，由横向导航坐标系姿态更新可知：
[0065][0066]
式中，表示载体坐标系b到横向导航坐标系t的方向余弦矩阵；(由惯性传感器获得计算)表示载体系相对于横向系的旋转角速度在载体系上的映射，[
·
×
]为矢量变斜对称矩阵；
[0067]
横向导航坐标系下速度矢量更新方程可以表示为：
[0068][0069]
式中，v
t
表示横向坐标系下的速度矢量；表示载体系到横向系的方向余弦矩阵；fb表示加速度计测量值；表示地球系相对惯性系的旋转角速度在横向系的映射；表示横向系相对于地球系的旋转角速度在横向系的映射；g
t
表示重力矢量在横向系的映射。
[0070]
考虑地球是一个椭球体，且基本呈现一个轻微压扁的球体，因此，直接采用圆球进行速度更新会导致北向速度误差增大的现象。为此，利用虚拟圆球方法，对横向导航坐标系的北向速度进行扩展，得到虚拟圆球模型下的扩展之后的速度：
[0071][0072]
式中，vv表示采用虚拟圆球进行扩展之后横向坐标系下的速度矢量；v
t
表示横向坐标系下的速度矢量；表示横向系到导航系的方向余弦矩阵；表示导航系到横向系的方向余弦矩阵；k表示扩展系数矩阵，可由下式计算：
[0073][0074]
式中，rm表示导航系子午圈曲率半径；rn表示导航系卯酉圈曲率半径；h
t
表示横向系高度；
[0075]
利用上述虚拟圆球速度vv，则横向导航坐标系下载体的位置更新方程可以表示
为：
[0076][0077]
式中，p
t
表示位置矢量在横向系下的映射；vv表示采用虚拟圆球进行扩展之后横向坐标系下的速度矢量；rc表示转换矩阵，可以表示为：
[0078][0079]
式中，表示横向卯酉圈曲率半径；h
t
表示横向系下高度；l
t
表示横向系纬度；
[0080]
相位调制姿态更新：
[0081][0082]
式中，表示相位调制之后的姿态矩阵；表示利用相位调制速度计算的载体系相对横向系旋转角速度在载体系下的映射。
[0083]
惯性导航算法受仪表误差及初始误差会出现舒拉震荡，利用惯性导航算法超前计算，可以有效的消除舒拉震荡影响，提高系统稳定性。在算法执行过程中，分别采用两个环路进行惯导计算，如图2所示，实际更新就是正常的导航计算，虚拟扩展更新就是利用阻尼对正常导航进行计算，分为环路1和环路2，两个环路是同时进行的，进入阻尼阶段后，两个环路将各自的导航信息融合进行导航输出。当载体处于静止状态时，设定速度阻尼系数使得惯导速度更新如下式所示：
[0084][0085]
式中，表示横向坐标系下相位调制计算的速度矢量，由迭代计算得到的，初始值与横向导航坐标系下的速度一致；表示载体系到横向系的方向余弦矩阵；fb表示加速度计量测；表示地球系相对惯性系的旋转角速度在横向系的映射；表示横向系相对于地球系的旋转角速度在横向系的映射；g
t
表示重力矢量在横向系的映射；k表示阻尼系数；
[0086]
同理，采用虚拟圆球对相位调制速度进行转换：
[0087][0088]
式中，表示采用虚拟圆球进行扩展之后横向坐标系下相位调制的速度矢量；表示横向系到导航系的方向余弦矩阵；表示导航系到横向系的方向余弦矩阵；k表示扩展系数矩阵，表示横向坐标系下相位调制计算的速度矢量；
[0089]
相位调制位置更新可以表示为：
[0090][0091]
式中，表示相位调制之后的横向坐标系下的位置矢量；k表示阻尼系数；表示采用虚拟圆球进行扩展之后横向坐标系下相位调制的速度矢量；rc表示转换矩阵。
[0092]
步骤s3：对横向导航系姿态进行相位调制，得到相位调制后的横向导航系姿态，根据横向导航系姿态和相位调制后的横向导航系姿态得到惯性导航输出姿态，根据虚拟圆球模型下横向导航系速度和虚拟圆球模型下横向导航系相位调制后的速度得到惯性导航输出速度，以及根据横向导航系下载体的第一位置和相位调制后横向导航系下载体的第二位置得到惯性导航输出载体的输出位置；
[0093]
经过相位调制之后，惯性导航输出可以表示为：
[0094][0095]
式中，表示惯性导航输出姿态矩阵；表示惯性导航输出速度矢量；表示惯性导航输出位置矢量；表示载体系到横向系的方向余弦矩阵；表示相位调制之后的姿态矩阵；ma()表示姿态矩阵转姿态角；am()表示姿态角转姿态矩阵；表示采用虚拟圆球进行扩展之后横向坐标系下相位调制的速度矢量；vv表示采用虚拟圆球进行扩展之后横向坐标系下的速度矢量；表示相位调制之后的位置矢量；p
t
表示位置矢量在横向系下的映射；
[0096]
步骤s4：根据惯性导航输出，设定极区惯性导航定位时长为m，本实施例设定m＝3小时，若导航时间k＝m，完成惯性导航定位，若k《m，表示惯性导航定位未完成，则重复上述步骤一至步骤五，直至惯性导航过程结束。
[0097]
本实施例将本发明提出的一种虚拟圆球模型极区惯性导航相位调制阻尼方法，通过matlab仿真软件进行仿真验证。仿真硬件环境均为intel(r)core(tm)t9600 cpu 2.80ghz，4g ram，windows 7操作系统。如图3和图4、图5和图6所示，从图中可以看出，采用虚拟圆球模型进行相位调制算法，可以减小舒拉震荡。
[0098]
实施例二
[0099]
基于同一发明构思，本实施例提供了虚拟圆球模型下极区惯性导航相位调制阻尼系统，其解决问题的原理与所述虚拟圆球模型下极区惯性导航相位调制阻尼方法类似，重复之处不再赘述。
[0100]
本实施例提供了虚拟圆球模型下极区惯性导航相位调制阻尼系统，包括：
[0101]
数据采集模块，用于采集惯性传感器的实时数据，根据实时数据得到横向导航系姿态；
[0102]
速度获取模块，用于根据实时数据和横向导航系姿态更新横向导航系速度并利用虚拟圆球模型对横向导航系速度进行扩展，得到虚拟圆球模型下横向导航系速度；
[0103]
载体的第一位置获取模块，用于根据虚拟圆球模型下横向导航系速度，得到横向导航系下载体的第一位置；
[0104]
相位调制速度获取模块，用于根据实时数据、设定的速度阻尼系数和横向导航系姿态得到横向导航系相位调制后的速度，利用虚拟圆球模型对横向导航系相位调制后的速度进行扩展，得到虚拟圆球模型下横向导航系相位调制后的速度；
[0105]
载体的第二位置获取模块，用于根据虚拟圆球模型下横向导航系相位调制后的速度，得到相位调制后横向导航系下载体的第二位置；
[0106]
惯性导航输出模块，用于对横向导航系姿态进行相位调制，得到相位调制后的横向导航系姿态，根据横向导航系姿态和相位调制后的横向导航系姿态得到惯性导航输出姿态，根据虚拟圆球模型下横向导航系速度和虚拟圆球模型下横向导航系相位调制后的速度得到惯性导航输出速度，以及根据横向导航系下载体的第一位置和相位调制后横向导航系下载体的第二位置得到惯性导航输出载体的输出位置，根据所述惯性导航输出姿态、惯性导航输出速度、惯性导航输出载体的输出位置进行导航。
[0107]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0108]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0109]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0110]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0111]
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。