虚拟圆球模型下SINS/DVL极区组合导航方法及系统
文献发布时间:2023-06-19 11:52:33
技术领域
本发明涉及导航技术领域,尤其是指一种虚拟圆球模型下SINS/DVL极区组合导航方法及系统。
背景技术
当前极区组合导航算法多采用横向坐标系或格网坐标系进行构建,而在地球模型上通常有圆球模型和椭球模型两种。其中圆球模型具有计算简单的优点,但是由于地球并非严格意义上的圆球,因此采用圆球模型进行组合导航存在原理性误差。椭球模型虽然可以消除原理性误差,但是却存在计算复杂度高的缺陷,难以在很多场合进行应用。
发明内容
为此,本发明所要解决的技术问题在于克服现有技术中圆球模型具有原理性误差,椭球模型计算复杂度高的技术缺陷。
为解决上述技术问题,本发明提供了一种虚拟圆球模型下SINS/DVL极区组合导航方法,包括以下步骤:
S1、获取惯性传感器和声学多普勒测速仪的采集的实时数据;
S2、构建虚拟圆球模型下惯性导航力学编排;
S3、基于虚拟圆球模型下惯性导航力学编排,构造虚拟圆球模型下SINS/DVL组合定位系统误差模型;
S4、将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中,利用迭代优化算法实现的误差参数估计;
S5、定义极区长航时惯性导航定位时长为M,若导航时间k<M,则标定过程未完成,重复S1-S4,直至导航定位结束。
作为优选的,所述S1具体包括:
由惯性传感器测量模型可知:
其中,
DVL测量模型为:
其中,
作为优选的,所述S2具体包括:
由横向导航定位方法可知:
其中,
其中,p表示导航系与横向导航系之间的夹角;L
其中,
虚拟圆球模型下惯性导航力学编排为:
其中,
作为优选的,所述S3中,虚拟圆球模型下SINS/DVL组合定位系统误差模型,包括虚拟圆球模型下姿态误差方程、虚拟圆球模型下速度误差方程和虚拟圆球模型下位置误差方程。
作为优选的,所述虚拟圆球模型下姿态误差方程,具体包括:
由姿态误差可知:
其中,
对上式两边进行求导并整理可得虚拟圆球模型下姿态误差方程:
其中,
其中,R
作为优选的,所述虚拟圆球模型下速度误差方程,具体包括:
由比力方程可知虚拟圆球模型下比力方程为:
其中,
由虚拟圆球模型下的比力方程可以计算速度误差为:
其中,
作为优选的,所述虚拟圆球模型下位置误差方程,具体包括:
基于速度误差,位置误差为:
其中,
其中,L
作为优选的,所述S3中,构建虚拟圆球模型下SINS/DVL组合定位系统误差模型,具体包括:
构造组合导航系统方程:
其中,
由DVL测速方程可以构造系统量测方程:
其中,y表示量测量;
作为优选的,所述S4具体包括:
P
其中,
本发明还公开了一种虚拟圆球模型下SINS/DVL极区组合导航系统,包括:
数据采集模块,所述数据采集模块用于获取惯性传感器和声学多普勒测速仪的采集的实时数据;
惯性导航力学编排模块,所述惯性导航力学编排模块用于构建虚拟圆球模型下惯性导航力学编排;
定位系统误差模型构建模块,所述定位系统误差模型构建模块基于虚拟圆球模型下惯性导航力学编排,构造虚拟圆球模型下SINS/DVL组合定位系统误差模型;
迭代优化模块,所述导航定位模块将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中,利用迭代优化算法实现的误差参数估计;
导航定位模块,所述导航定位模块用于定义极区长航时惯性导航定位时长为M,若导航时间k<M,则标定过程未完成,重复上述操作,直至导航定位结束。
本发明的上述技术方案相比现有技术具有以下优点:
1、本发明采用虚拟圆球模型进行极区惯性导航更新,减小了算法复杂度。
2、本发明通过虚拟圆球模型构造极区组合导航算法误差方程,减小了原理性误差,提高了惯导定位精度。
附图说明
图1是虚拟圆球模型下SINS/DVL极区组合导航算法流程图;
图2是组合导航算法姿态误差图,其中,图2(a)为纵摇误差,图2(b)为横摇误差,图2(c)为航向误差;
图3是组合导航算法速度误差图,其中,图3(a)为东向速度误差,图3(b)为北向速度误差;
图4是组合导航算法位置误差图,其中,图4(a)为维度误差,图4(b)为经度误差;
图5是三种算法计算时长统计图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明,以使本领域的技术人员可以更好地理解本发明并能予以实施,但所举实施例不作为对本发明的限定。
参照图1所示,本发明的虚拟圆球模型下SINS/DVL极区组合导航方法,其特征在于,包括以下步骤:
步骤一、获取惯性传感器和声学多普勒测速仪的采集的实时数据,具体包括:
由惯性传感器测量模型可知:
其中,
DVL测量模型为:
其中,
步骤二、构建虚拟圆球模型下惯性导航力学编排,具体包括:
由横向导航定位方法可知:
其中,
其中,p表示导航系与横向导航系之间的夹角;L
其中,
虚拟圆球模型下惯性导航力学编排为:
其中,
步骤三、基于虚拟圆球模型下惯性导航力学编排,构造虚拟圆球模型下SINS/DVL组合定位系统误差模型。
其中,虚拟圆球模型下SINS/DVL组合定位系统误差模型,包括虚拟圆球模型下姿态误差方程、虚拟圆球模型下速度误差方程和虚拟圆球模型下位置误差方程。
由姿态误差可知:
其中,
对上式两边进行求导并整理可得虚拟圆球模型下姿态误差方程:
其中,
其中,R
由比力方程可知虚拟圆球模型下比力方程为:
其中,
由虚拟圆球模型下的比力方程可以计算速度误差为:
其中,
基于速度误差,位置误差为:
其中,
其中,L
构造组合导航系统方程:
其中,
由DVL测速方程可以构造系统量测方程:
其中,y表示量测量;
步骤四、将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中,利用迭代优化算法实现的误差参数估计,具体包括:
P
其中,
步骤五、定义极区长航时惯性导航定位时长为M,若导航时间k<M,则标定过程未完成,重复步骤一至步骤四,直至导航定位结束。
MATLAB仿真实验,在以下的仿真条件下,对该方法进行仿真实验:陀螺仪量测常值漂移误差为ε
本实施例将本发明提出的一种虚拟圆球模型下SINS/DVL极区组合导航方法通过Matlab仿真软件进行仿真验证。仿真硬件环境均为Intel(R)Core(TM)T9600 CPU 2.80GHz,4G RAM,Windows 7操作系统。如图2所示,为组合导航算法姿态误差图,图3为组合导航算法速度误差图,图4为组合导航算法位置误差图,图5是三种算法计算时长统计图。从图中可以看出,采用虚拟圆球模型进行组合导航算法,可以达到与椭球模型相同的定位精度,而计算量却小于椭球模型。
本发明公开了一种虚拟圆球模型下SINS/DVL极区组合导航系统,包括数据采集模块、惯性导航力学编排模块、定位系统误差模型构建模块、迭代优化模块和导航定位模块。
所述数据采集模块用于获取惯性传感器和声学多普勒测速仪的采集的实时数据。
所述惯性导航力学编排模块用于构建虚拟圆球模型下惯性导航力学编排。
所述定位系统误差模型构建模块基于虚拟圆球模型下惯性导航力学编排,构造虚拟圆球模型下SINS/DVL组合定位系统误差模型。
所述导航定位模块将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中,利用迭代优化算法实现的误差参数估计。
所述导航定位模块用于定义极区长航时惯性导航定位时长为M,若导航时间k<M,则标定过程未完成,重复上述操作,直至导航定位结束。
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然,上述实施例仅仅是为清楚地说明所作的举例,并非对实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。
- 虚拟圆球模型下SINS/DVL极区组合导航方法及系统
- 虚拟圆球模型下极区惯性导航相位调制阻尼方法及系统