CN115900705A - 一种紧耦合陆地组合导航方法、装置、计算机设备和介质 - Google Patents

一种紧耦合陆地组合导航方法、装置、计算机设备和介质 Download PDF

Info

Publication number
CN115900705A
CN115900705A CN202211725810.4A CN202211725810A CN115900705A CN 115900705 A CN115900705 A CN 115900705A CN 202211725810 A CN202211725810 A CN 202211725810A CN 115900705 A CN115900705 A CN 115900705A
Authority
CN
China
Prior art keywords
coordinate system
light beam
dimensional laser
laser doppler
inertial navigation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211725810.4A
Other languages
English (en)
Inventor
周健
向志毅
王�琦
聂晓明
金世龙
程吉利
黄荣
席崇宾
陈兰剑
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202211725810.4A priority Critical patent/CN115900705A/zh
Publication of CN115900705A publication Critical patent/CN115900705A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Navigation (AREA)

Abstract

本申请属于组合导航技术领域，涉及一种紧耦合陆地组合导航方法、装置、计算机设备和介质，方法包括：获取二维激光多普勒测速仪的相关参数，得到二维激光多普勒测速仪坐标系与光束坐标系的转换矩阵；获取捷联惯性导航系统在导航坐标系下的输出速度，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系的输出速度；根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系的输出速度，得到捷联惯性导航系统在光束坐标系的输出速度；构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向速度之差为滤波观测量，估计组合导航系统的状态向量并导航。本方法提高了组合导航系统的鲁棒性。

Description

一种紧耦合陆地组合导航方法、装置、计算机设备和介质
技术领域
本申请涉及组合导航技术领域，特别是涉及一种紧耦合陆地组合导航方法、装置、计算机设备和介质。
背景技术
组合导航系统需要将多个传感器的信息进行融合，以发挥各个传感器各自的优势。在陆地组合导航领域，使用最为广泛的辅助传感器是全球定位系统和里程计：全球定位系统虽然能直接提供精确的速度信息和位置信息，但是全球定位系统存在信号不稳定，抗干扰性较差且信号易被树木以及建筑物遮蔽等问题；与全球定位系统相比，里程计是完全自主的，里程计能提供载体坐标系下的速度信息和位移增量信息，但是里程计的测量精度与载体的行驶状态以及车轮的状态密切相关，车轮温度、气压、磨损状况以及车辆行驶过程中出现的跳动、打滑都将使里程计的测量精度降低。
现有技术中，可以使用一维激光多普勒测速仪，一维激光多普勒测速仪是一种新型的速度传感器，是完全自主的，且具有非接触测量、空间分辨率好、动态响应快、测速范围广、方向灵敏度高等优势，近几年已经在陆用组合导航领域得到了小范围运用，且取得了令人满意的效果。但是，和里程计一样，一维激光多普勒测速仪只能提供载体前向速度，这将使搭载一维激光多普勒测速仪和捷联惯性导航系统的组合导航系统在载体高度估计上不太理想。
因此，可以使用二维激光多普勒测速仪，二维激光多普勒测速仪有两个光束，并能根据这两个光束获得载体前向和天向的速度，这将使得组合导航系统获得更高的定位精度，尤其是在高度上。
但是，在二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统中，直接利用二维激光多普勒测速仪得到的二维速度是一种松耦合方式，这需要二维激光多普勒测速仪的两个光束测得的信号有效。在二维激光多普勒测速仪只有一个光束信号正常时，松耦合组合导航系统将不能正常工作，导航误差将很快累积。
发明内容
基于此，有必要针对上述技术问题，提供一种紧耦合陆地组合导航方法、装置、计算机设备和介质，在二维激光多普勒测速仪其中一个光束信号失效时，组合导航系统仍能够正常工作，大大提高了组合导航系统的鲁棒性。
一种紧耦合陆地组合导航方法，包括：
获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵；
获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度；
根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度；
获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
在一个实施例中，获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵包括：
获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移以及第二光束的多普勒频移，得到第一光束方向上的速度和第二光束方向上的速度：
Figure BDA0004029848800000035
Figure BDA0004029848800000036
式中，υ1为沿第一光束方向上的速度，υ2为沿第二光束方向上的速度，λ为二维激光多普勒测速仪的光束波长，
Figure BDA0004029848800000037
为第一光束所测得的多普勒频移和
Figure BDA0004029848800000038
为第二光束所测得的多普勒频移；
获取第一光束的实际倾角、第二光束的实际倾角以及二维激光多普勒测速仪在自身坐标系下的实际速度，根据第一光束的实际倾角、第二光束的实际倾角、二维激光多普勒测速仪在自身坐标系下的实际速度、第一光束方向上的速度以及第二光束方向上的速度，得到第一光束方向上的速度、第二光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的实际速度之间的关系：
Figure BDA0004029848800000031
υ1＝υycosθ1+υzsinθ1
υ2＝υycosθ2+υzsinθ2
υ1＝K1υy+K2υz
υ2＝K3υy+K4υz
Figure BDA0004029848800000032
式中，
Figure BDA0004029848800000033
为二维激光多普勒测速仪在自身坐标系下的实际速度，υy为二维激光多普勒测速仪的实际前向速度，υz为二维激光多普勒测速仪的实际天向速度，θ1为二维激光多普勒测速仪第一光束的实际倾角，θ2为二维激光多普勒测速仪第二光束的实际倾角，K1、K2、K3和K4为比例因子；
引入第三光束作为横向约束，得到第一光束方向上的速度、第二光束方向上的速度、第三光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的实际速度之间的第一关系：
Figure BDA0004029848800000034
考虑设计偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵。
在一个实施例中，考虑设计偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵包括：
考虑设计偏差，得到第一光束方向上的速度、第二光束方向上的速度、第三光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的输出速度之间的第二关系：
Figure BDA0004029848800000041
根据第二关系，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵：
Figure BDA0004029848800000042
式中，CT为转换矩阵。
在一个实施例中，获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度包括：
捷联惯性导航系统在导航坐标系下的输出速度为：
Figure BDA0004029848800000043
式中，n表示导航坐标系，
Figure BDA0004029848800000044
为捷联惯性导航系统在导航坐标系下的输出速度，
Figure BDA0004029848800000045
为捷联惯性导航系统在导航坐标系下的真实速度，
Figure BDA0004029848800000046
为捷联惯性导航系统的速度误差；
将捷联惯性导航系统在导航坐标系下的输出速度投影到载体坐标系，得到捷联惯性导航系统在载体坐标系下的输出速度：
Figure BDA0004029848800000047
式中，b表示载体坐标系，
Figure BDA0004029848800000048
为捷联惯性导航系统在载体坐标系下的输出速度，
Figure BDA0004029848800000051
为n系到b系的包含误差的姿态转换矩阵，
Figure BDA0004029848800000052
为n系到b系的姿态转换矩阵，I3为三阶单位矩阵，
Figure BDA0004029848800000053
为惯导的姿态误差；
将捷联惯性导航系统在载体坐标系下的输出速度转换到二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度：
Figure BDA0004029848800000054
φm＝[φmxφmyφmz]T
式中，
Figure BDA0004029848800000055
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，
Figure BDA0004029848800000056
为b系到m系的包含误差的姿态转换矩阵，
Figure BDA0004029848800000057
为b系到m系的姿态转换矩阵，φm为二维激光多普勒测速仪所在坐标系与载体坐标系之间的安装差角误差，φmx为俯仰安装差角误差，φmy为横滚安装差角误差，φmz为航向安装差角误差。
在一个实施例中，根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度包括：
根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度：
Figure BDA0004029848800000058
Figure BDA0004029848800000059
δK＝[δK1δK2δK3δK4]T
Figure BDA00040298488000000510
Figure BDA00040298488000000511
Figure BDA0004029848800000061
Figure BDA0004029848800000062
式中，
Figure BDA0004029848800000063
为捷联惯性导航系统在光束坐标系下的输出速度，
Figure BDA0004029848800000064
为捷联惯性导航系统在第一光束方向上的输出速度，
Figure BDA0004029848800000065
为捷联惯性导航系统在第二光束方向上的输出速度，
Figure BDA0004029848800000066
为捷联惯性导航系统在第三光束方向上的输出速度，
Figure BDA0004029848800000067
为捷联惯性导航系统在载体坐标系下的真实速度，
Figure BDA0004029848800000068
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的前向速度，
Figure BDA0004029848800000069
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的天向速度。
在一个实施例中，获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型包括：
Figure BDA00040298488000000610
Figure BDA00040298488000000611
Figure BDA00040298488000000612
Figure BDA00040298488000000613
Figure BDA00040298488000000614
Figure BDA00040298488000000615
Figure BDA00040298488000000616
其中：
υn＝[υEυNυU]T
δυn＝[δυEδυNδυU]T
Figure BDA00040298488000000617
Figure BDA00040298488000000618
Figure BDA00040298488000000619
式中，fn表示由加速度计输出得到的比力在导航坐标系中的投影，υn为捷联惯性导航系统的速度信息，υE、υN、υU分别表示东向速度、北向速度和天向速度，δυn表示捷联惯性导航系统的速度误差，L，λ，h分别表示地球纬度、地球经度以及高度，RM和RN分别表示载体所在位置的地球子午圈和卯酉圈曲率半径，ωie表示地球自转角速度。
在一个实施例中，根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计包括：
将二维激光多普勒测速仪的三个安装差角误差、四个比例因子误差、惯导姿态误差、速度误差、位置误差、陀螺测量误差以及加速度计测量误差共同构成组合导航系统中的状态向量：
Figure BDA0004029848800000071
式中，X(t)为组合导航系统的状态向量，
Figure BDA0004029848800000076
为惯导姿态误差，δυn为速度误差，δP为位置误差，
Figure BDA0004029848800000072
为陀螺测量误差，
Figure BDA0004029848800000073
为加速度计测量误差，φmx、φmy、φmz为二维激光多普勒测速仪的三个安装差角误差，δK1、δK2、δK3、δK4为四个比例因子误差；
令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对状态向量进行估计：
Figure BDA0004029848800000074
式中，z(t)为滤波观测量，H(t)为滤波器的量测矩阵，V(t)则为量测噪声向量，
Figure BDA0004029848800000075
为二维激光多普勒测速仪在三个光束方向上的速度。
一种紧耦合陆地组合导航装置，包括：
获取模块，用于获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵；
转换模块，用于获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度；
计算模块，用于根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度；
导航模块，用于获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵；
获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度；
根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度；
获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵；
获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度；
根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度；
获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
上述紧耦合陆地组合导航方法，建立二维激光多普勒测速仪自身坐标系下速度与光束坐标系下速度的关系，分析二维激光多普勒测速仪在组合导航过程中的误差项(包括四个比例因子误差和三个安装角误差)，并根据二维激光多普勒测速仪的误差项，得到捷联惯性导航系统在二维激光多普勒测速仪光束坐标系下的速度表达式，将二维激光多普勒测速仪的误差项以及捷联惯性导航系统的误差作为滤波器的状态向量，并将捷联惯性导航系统在二维激光多普勒测速仪光束坐标系下的速度与二维激光多普勒测速仪两束光束测量的速度之差作为滤波观测量，根据滤波得到结果对组合导航系统进行反馈修正。目前对激光多普勒测速仪的使用还停留在使用测速仪的合成速度上，这样使用固然方便，但需要足够的正常光束信号才能获得可信的合成速度。与现有方案相比，本申请在松耦合的基础上，将二维激光多普勒测速仪和惯性导航系统进行更深程度的紧耦合，没有直接使用二维激光多普勒测速仪的二维速度，而是直接使用二维激光多普勒测速仪两束光束所测量的原始速度，也就是直接使用二维激光多普勒测速仪的光束测量值而不将它们转换为二维速度，使得二维激光多普勒测速仪在其中一个光束信号失效时，组合导航系统仍能够正常工作，这大大提高了组合导航系统的鲁棒性。
附图说明
图1为一个实施例中紧耦合陆地组合导航方法的应用场景图；
图2为一个实施例中紧耦合陆地组合导航方法的流程示意图；
图3为一个实施例中二维激光多普勒测速仪在紧组合导航系统中的安装位置以及出射光路图；
图4为一个实施例中紧耦合陆地组合导航装置的结构框图；
图5为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
需要说明，本申请实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本申请中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多组”的含义是至少两组，例如两组，三组等，除非另有明确具体的限定。
在本申请中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。
另外，本申请各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本申请要求的保护范围之内。
本申请提供的紧耦合陆地组合导航方法，可以应用于如图1所示的应用环境中。其中，终端102通过网络与服务器104进行通信，终端102可以包括但不限于是各种个人计算机、笔记本电脑、智能手机、平板电脑和便携式可穿戴设备，服务器104可以是各类门户网站、工作系统后台对应的服务器等。
本申请提供了一种紧耦合陆地组合导航方法，如图2所示，在一个实施例中，以该方法应用于图1中的终端为例进行说明，包括：
步骤202，获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵。
具体的：
获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移以及第二光束的多普勒频移，得到二维激光多普勒测速仪在第一光束方向上的速度和第二光束方向上的速度：
Figure BDA0004029848800000111
Figure BDA0004029848800000112
式中，υ1为沿第一光束方向上的速度，υ2为沿第二光束方向上的速度，λ为二维激光多普勒测速仪的光束波长，
Figure BDA0004029848800000113
为第一光束所测得的多普勒频移和
Figure BDA0004029848800000114
为第二光束所测得的多普勒频移；
获取第一光束的实际倾角、第二光束的实际倾角以及二维激光多普勒测速仪在自身坐标系下的实际速度，根据第一光束的实际倾角、第二光束的实际倾角、二维激光多普勒测速仪在自身坐标系下的实际速度、第一光束方向上的速度以及第二光束方向上的速度，得到第一光束方向上的速度、第二光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的实际速度之间的关系：
二维激光多普勒测速仪在m系中的实际速度为，
Figure BDA0004029848800000121
υy和υz与υ1和υ2存在以下关系，
υ1＝υycosθ1+υzsinθ1(4)
υ2＝υycosθ2+υzsinθ2(5)
将(4)式和(5)式改写为如下形式，
υ1＝K1υy+K2υz(6)
υ2＝K3υy+K4υz(7)
其中，
K1＝cosθ1(8)
K2＝sinθ1(9)
K3＝cosθ2(10)
K4＝sinθ2(11)
根据(6)、(7)式，二维激光多普勒测速仪光束测量速度(即第一光束方向上的速度以及第二光束方向上的速度)与二维激光多普勒测速仪在自身坐标系下的速度之间的关系为：
Figure BDA0004029848800000122
式中，
Figure BDA0004029848800000123
为二维激光多普勒测速仪在自身坐标系下的实际速度，υy为二维激光多普勒测速仪在自身坐标系下的实际前向速度，υz为二维激光多普勒测速仪在自身坐标系下的实际天向速度，θ1为二维激光多普勒测速仪第一光束的实际倾角，θ2为二维激光多普勒测速仪第二光束的实际倾角，K1、K2、K3和K4为比例因子；
引入第三光束(第三光束为垂直于第一光束和第二光束所构成平面的虚拟光束)，作为横向约束，第三光束方向上的速度恒为0，即υ3＝0；
根据式(6)和式(7)，二维激光多普勒测速仪的光束测量速度与二维激光多普勒测速仪在自身坐标系即m系下的速度之间的关系，即第一光束方向上的速度、第二光束方向上的速度、第三光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的实际速度之间的第一关系为：
Figure BDA0004029848800000131
在工程应用中，真实的光束倾角θ1与θ2与设计值会存在偏差，因此根据倾角设计值得到的二维激光多普勒测速仪比例因子存在一定误差。
因此，在实际应用中，考虑设计倾角偏差，(13)式为以下形式，即根据第一关系，得到第一光束方向上的速度、第二光束方向上的速度、第三光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的输出速度之间的第二关系为：
Figure BDA0004029848800000132
其中，
Figure BDA0004029848800000133
Figure BDA0004029848800000134
Figure BDA0004029848800000135
Figure BDA0004029848800000136
式中，
Figure BDA0004029848800000137
和
Figure BDA0004029848800000138
分别为二维激光多普勒测速仪两束光束的设计倾角，
Figure BDA0004029848800000139
δKi为比例因子误差，
Figure BDA00040298488000001310
为二维激光多普勒测速仪在自身坐标系下的输出速度。
根据第二关系，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵：
Figure BDA0004029848800000141
式中，CT为转换矩阵。
步骤204，获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度。
具体的：
在实际应用中，捷联惯性导航系统所在的载体坐标系b系与二维激光多普勒测速仪所在的m系存在偏差，因此会存在安装角误差。安装角误差的精确标定与否将直接影响组合导航系统的导航精度。因此有必要考虑二维激光多普勒测速仪的安装角误差。
而且，在实际应用中，不可能在每次导航前都对二维激光多普勒测速仪进行精确标定，并且导航过程中较大的路面颠簸也会使二维激光多普勒测速仪的标定参数产生细微变化，因此有必要考虑二维激光多普勒测速仪的安装差角误差。
捷联惯性导航系统在导航坐标系下的输出速度为：
Figure BDA0004029848800000142
式中，n表示导航坐标系，
Figure BDA0004029848800000143
为捷联惯性导航系统在导航坐标系下的输出速度，
Figure BDA0004029848800000144
为捷联惯性导航系统在导航坐标系下的真实速度，
Figure BDA0004029848800000145
为捷联惯性导航系统在导航坐标系下的速度误差；
将捷联惯性导航系统在导航坐标系下的输出速度投影到载体坐标系，得到捷联惯性导航系统在载体坐标系下的输出速度：
Figure BDA0004029848800000146
式中，b表示载体坐标系，
Figure BDA0004029848800000151
为捷联惯性导航系统在载体坐标系下的输出速度，
Figure BDA0004029848800000152
为n系到b系的包含误差的姿态转换矩阵，
Figure BDA0004029848800000153
为n系到b系的姿态转换矩阵，I3为三阶单位矩阵，
Figure BDA0004029848800000154
为惯导的姿态误差；
将捷联惯性导航系统在载体坐标系下的输出速度转换到二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度：
Figure BDA0004029848800000155
φm＝[φmxφmyφmz]T
式中，
Figure BDA0004029848800000156
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，
Figure BDA0004029848800000157
为b系到m系的包含误差的姿态转换矩阵，
Figure BDA0004029848800000158
为b系到m系的姿态转换矩阵，φm为二维激光多普勒测速仪所在坐标系与载体坐标系之间的安装差角误差，φmx为俯仰安装差角误差，φmy为横滚安装差角误差，φmz为航向安装差角误差。
步骤206，根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度。
具体的：
根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，即根据式(19)和式(22)，得到捷联惯性导航系统在光束坐标系下的输出速度：
Figure BDA0004029848800000159
Figure BDA00040298488000001510
δK＝[δK1δK2δK3δK4]T(25)
Figure BDA0004029848800000161
Figure BDA0004029848800000162
Figure BDA0004029848800000163
Figure BDA0004029848800000164
式中，
Figure BDA0004029848800000165
为捷联惯性导航系统在光束坐标系下的输出速度，
Figure BDA0004029848800000166
为捷联惯性导航系统在第一光束方向上的输出速度，
Figure BDA0004029848800000167
为捷联惯性导航系统在第二光束方向上的输出速度，
Figure BDA0004029848800000168
为捷联惯性导航系统在第三光束方向上的输出速度，
Figure BDA0004029848800000169
为捷联惯性导航系统在载体坐标系下的真实速度，
Figure BDA00040298488000001610
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的前向速度，
Figure BDA00040298488000001611
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的天向速度，δK为二维激光多普勒测速仪的比例因子误差。
步骤208，获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
具体的：
获取二维激光多普勒测速仪的误差参数，在捷联惯性导航系统与二维激光多普勒测速仪构成的紧耦合组合导航系统中，构建捷联惯性导航系统的误差模型：
Figure BDA00040298488000001612
Figure BDA00040298488000001613
Figure BDA00040298488000001614
Figure BDA00040298488000001615
Figure BDA00040298488000001616
Figure BDA0004029848800000171
Figure BDA0004029848800000172
其中：
Figure BDA0004029848800000173
Figure BDA0004029848800000174
Figure BDA0004029848800000175
又有：
υn＝[υEυNυU]T
δυn＝[δυEδυNδυU]T
P＝[Lλh]T
δP＝[δLδλδh]T
式中，fn表示由加速度计输出得到的比力在导航坐标系n系中的投影，υn为捷联惯性导航系统的输出速度，υE、υN、υU分别表示东向速度、北向速度和天向速度，δυn表示捷联惯性导航系统的速度误差，L，λ，h分别表示地球纬度、地球经度以及高度，P表示捷联惯性导航系统的输出位置，δP表示捷联惯性导航系统的位置误差，RM和RN分别表示载体所在位置的地球子午圈和卯酉圈曲率半径，ωie表示地球自转角速度。
将二维激光多普勒测速仪的三个安装差角误差、四个比例因子误差、惯导姿态误差、速度误差、位置误差、陀螺测量误差以及加速度计测量误差共同构成紧耦合组合导航系统中的状态向量，卡尔曼滤波模型中的状态向量为22维，即：
Figure BDA0004029848800000176
式中，X(t)为组合导航系统的状态向量，
Figure BDA0004029848800000177
为惯导姿态误差，δυn为惯导速度误差(即上式中的δυ)，δP为惯导位置误差，
Figure BDA0004029848800000178
为陀螺测量误差，
Figure BDA0004029848800000179
为加速度计测量误差，φmx、φmy、φmz为二维激光多普勒测速仪的三个安装差角误差(分别是：俯仰安装角误差、横滚安装角误差以及航向安装角误差)，δK1、δK2、δK3、δK4为四个比例因子误差；
根据式(26)至式(32)表示的捷联惯性导航系统误差模型以及式(36)描述的状态变量，利用卡尔曼滤波器来进行状态估计，其状态方程可以表示为：
Figure BDA0004029848800000181
式中，F(t)为系统状态转移矩阵，G(t)为系统噪声矩阵，w(t)为系统噪声向量；
系统状态转移矩阵F(t)表示为：
Figure BDA0004029848800000182
其中，
Figure BDA0004029848800000183
Figure BDA0004029848800000184
Figure BDA0004029848800000185
Figure BDA0004029848800000191
Figure BDA0004029848800000192
Figure BDA0004029848800000193
系统噪声矩阵表示为：
Figure BDA0004029848800000194
系统噪声向量表示为：
Figure BDA0004029848800000195
式中，εwi表示陀螺的噪声，
Figure BDA0004029848800000196
表示加速度计的噪声；
令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，也就是说，以捷联惯性导航系统在第一光束方向上的速度与二维激光多普勒测速仪在第一光束方向上的速度之差为第一差值，以捷联惯性导航系统在第二光束方向上的速度与二维激光多普勒测速仪在第二光束方向上的速度之差为第二差值，以捷联惯性导航系统在第三光束方向上的速度与二维激光多普勒测速仪在第三光束方向上的速度之差为第三差值，以第一差值、第二差值和第三差值为滤波观测量，对状态向量进行估计，即卡尔曼滤波中的量测方程为：
Figure BDA0004029848800000201
式中，z(t)为滤波观测量，H(t)为滤波器的量测矩阵，V(t)则为量测噪声向量，
Figure BDA0004029848800000203
为二维激光多普勒测速仪在三个光束方向上的速度。
滤波器的量测矩阵H(t)表示为：
Figure BDA0004029848800000202
根据滤波得到的状态向量对惯导系统进行反馈校正。
在本步骤中，将捷联惯性导航的姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差，以及二维激光多普勒测速仪的比例因子误差、安装差角误差作为滤波状态向量(即：将二维激光多普勒测速仪的安装差角误差、以及比例因子误差与捷联惯性导航系统的误差一同作为状态向量)，将根据捷联惯性导航系统速度计算出的在三个光束方向上的速度与二维激光多普勒测速仪在三个光束的测量值之差作为滤波观测量(即：将捷联惯性导航系统在二维激光多普勒测速仪光束坐标系下的速度与二维激光多普勒测速仪两束光束测量的速度与差作为滤波观测量)，最后使用卡尔曼滤波对滤波状态量进行实时估计与反馈校正，并根据校正后的定位结果进行陆地组合导航。
在本实施例中，如图3所示，IMU表示惯性测量单元，LDV表示二维激光多普勒测速仪，XbYbZb表示惯性测量单元所在的载体坐标系，XmYmZm表示二维激光多普勒测速仪所在的自身坐标系。还需要说明，SINS表示捷联惯性导航系统。
捷联惯性导航系统在使用时，需要合理的安装惯性测量单元(IMU)和二维激光多普勒测速仪(LDV)，具体的，将惯性测量单元安装在车辆后轮轴的中央，将二维激光多普勒测速仪安装在车辆的侧面或者尾部合适的位置，使二维激光多普勒测速仪输出的光束能够有效打在地面上并获得较强的信号，以实时获得有效的载体速度信息。在捷联惯性导航系统使用前，需对惯性测量单元进行标定以获得陀螺和加速度计的刻度系数误差、安装误差角、零偏。在进入正式导航过程前还需要完成系统的初始化以获得初始姿态、速度与位置，包括获得载体的初始位置和速度信息以及完成初始对准。
上述紧耦合陆地组合导航方法，是一种捷联惯性导航系统与二维激光多普勒测速仪紧耦合的陆地组合导航方法，通过卡尔曼滤波来实现，具体地，建立二维激光多普勒测速仪自身坐标系下速度与光束坐标系下速度的关系，其中为了增加横向约束，引入了垂直于光束一和光束二所构成平面的虚拟光束作为第三束光束，且光束三方向上的速度恒为0，分析二维激光多普勒测速仪在组合导航过程中的误差项(包括四个比例因子误差和三个安装角误差)，并根据二维激光多普勒测速仪的误差项，得到捷联惯性导航系统在二维激光多普勒测速仪光束坐标系下的速度表达式，将二维激光多普勒测速仪的误差项(安装差角误差以及比例因子误差)以及捷联惯性导航系统的误差一起作为滤波器的状态向量，并将捷联惯性导航系统在二维激光多普勒测速仪光束坐标系下的速度与二维激光多普勒测速仪三束光束测量的速度之差作为滤波观测量，构建滤波模型，使用卡尔曼滤波对滤波状态量进行实时估计，根据滤波得到结果对组合导航系统进行反馈修正。目前对激光多普勒测速仪的使用还停留在使用测速仪的合成速度上，这样使用固然方便，但需要足够的正常光束信号才能获得可信的合成速度。与现有方案相比，本申请在松耦合的基础上，将二维激光多普勒测速仪和惯性导航系统进行更深程度的紧耦合，没有直接使用二维激光多普勒测速仪的二维速度，而是直接使用二维激光多普勒测速仪两束光束所测量的原始速度，而不是合成速度，也就是直接使用二维激光多普勒测速仪的光束测量值而不将它们转换为二维速度，使得二维激光多普勒测速仪在其中一个光束信号失效时，组合导航系统仍能够正常工作，这大大提高了组合导航系统的鲁棒性。
应该理解的是，虽然图2的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
本申请还提供了一种紧耦合陆地组合导航装置，如图4所示，在一个实施例中，包括：获取模块402、转移模块404、计算模块406和导航模块408，其中：
获取模块402，用于获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵；
转换模块404，用于获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度；
计算模块406，用于根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度；
导航模块408，用于获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
关于紧耦合陆地组合导航装置的具体限定可以参见上文中对于紧耦合陆地组合导航方法的限定，在此不再赘述。上述装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种紧耦合陆地组合导航方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图5中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述实施例中方法的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中方法的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种紧耦合陆地组合导航方法，其特征在于，包括：
获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵；
获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度；
根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度；
获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
2.根据权利要求1所述的方法，其特征在于，获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵包括：
获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移以及第二光束的多普勒频移，得到第一光束方向上的速度和第二光束方向上的速度：
Figure FDA0004029848790000011
Figure FDA0004029848790000012
式中，υ1为沿第一光束方向上的速度，υ2为沿第二光束方向上的速度，λ为二维激光多普勒测速仪的光束波长，
Figure FDA0004029848790000013
为第一光束所测得的多普勒频移和
Figure FDA0004029848790000014
为第二光束所测得的多普勒频移；
获取第一光束的实际倾角、第二光束的实际倾角以及二维激光多普勒测速仪在自身坐标系下的实际速度，根据第一光束的实际倾角、第二光束的实际倾角、二维激光多普勒测速仪在自身坐标系下的实际速度、第一光束方向上的速度以及第二光束方向上的速度，得到第一光束方向上的速度、第二光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的实际速度之间的关系：
Figure FDA0004029848790000021
υ1＝υy cosθ1+υz sinθ1
υ2＝υy cosθ2+υz sinθ2
υ1＝K1υy+K2υz
υ2＝K3υy+K4υz
Figure FDA0004029848790000022
式中，
Figure FDA0004029848790000023
为二维激光多普勒测速仪在自身坐标系下的实际速度，υy为二维激光多普勒测速仪的实际前向速度，υz为二维激光多普勒测速仪的实际天向速度，θ1为二维激光多普勒测速仪第一光束的实际倾角，θ2为二维激光多普勒测速仪第二光束的实际倾角，K1、K2、K3和K4为比例因子；
引入第三光束作为横向约束，得到第一光束方向上的速度、第二光束方向上的速度、第三光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的实际速度之间的第一关系：
Figure FDA0004029848790000024
考虑设计偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵。
3.根据权利要求2所述的方法，其特征在于，考虑设计偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵包括：
考虑设计偏差，得到第一光束方向上的速度、第二光束方向上的速度、第三光束方向上的速度以及二维激光多普勒测速仪在自身坐标系下的输出速度之间的第二关系：
Figure FDA0004029848790000031
根据第二关系，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵：
Figure FDA0004029848790000032
式中，CT为转换矩阵。
4.根据权利要求3所述的方法，其特征在于，获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度包括：
捷联惯性导航系统在导航坐标系下的输出速度为：
Figure FDA0004029848790000033
式中，n表示导航坐标系，
Figure FDA0004029848790000034
为捷联惯性导航系统在导航坐标系下的输出速度，
Figure FDA0004029848790000035
为捷联惯性导航系统在导航坐标系下的真实速度，
Figure FDA0004029848790000036
为捷联惯性导航系统的速度误差；
将捷联惯性导航系统在导航坐标系下的输出速度投影到载体坐标系，得到捷联惯性导航系统在载体坐标系下的输出速度：
Figure FDA0004029848790000037
式中，b表示载体坐标系，
Figure FDA0004029848790000038
为捷联惯性导航系统在载体坐标系下的输出速度，
Figure FDA0004029848790000039
为n系到b系的包含误差的姿态转换矩阵，
Figure FDA00040298487900000310
为n系到b系的姿态转换矩阵，I3为三阶单位矩阵，
Figure FDA00040298487900000311
为惯导的姿态误差；
将捷联惯性导航系统在载体坐标系下的输出速度转换到二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度：
Figure FDA00040298487900000312
φm＝[φmxφmyφmz]T
式中，
Figure FDA0004029848790000041
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，
Figure FDA0004029848790000042
为b系到m系的包含误差的姿态转换矩阵，
Figure FDA0004029848790000043
为b系到m系的姿态转换矩阵，φm为二维激光多普勒测速仪所在坐标系与载体坐标系之间的安装差角误差，φmx为俯仰安装差角误差，φmy为横滚安装差角误差，φmz为航向安装差角误差。
5.根据权利要求4所述的方法，其特征在于，根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度包括：
根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度：
Figure FDA0004029848790000044
Figure FDA0004029848790000045
δK＝[δK1δK2δK3δK4]T
Figure FDA0004029848790000046
Figure FDA0004029848790000047
Figure FDA0004029848790000048
Figure FDA0004029848790000049
式中，
Figure FDA00040298487900000410
为捷联惯性导航系统在光束坐标系下的输出速度，
Figure FDA00040298487900000411
为捷联惯性导航系统在第一光束方向上的输出速度，
Figure FDA00040298487900000412
为捷联惯性导航系统在第二光束方向上的输出速度，
Figure FDA00040298487900000413
为捷联惯性导航系统在第三光束方向上的输出速度，
Figure FDA0004029848790000051
为捷联惯性导航系统在载体坐标系下的真实速度，
Figure FDA0004029848790000052
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的前向速度，
Figure FDA0004029848790000053
为捷联惯性导航系统在二维激光多普勒测速仪坐标系下的天向速度。
6.根据权利要求1至5任一项所述的方法，其特征在于，获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型包括：
Figure FDA0004029848790000054
Figure FDA0004029848790000055
Figure FDA0004029848790000056
Figure FDA0004029848790000057
Figure FDA0004029848790000058
Figure FDA0004029848790000059
Figure FDA00040298487900000510
其中：
υn＝[υEυNυU]T
δυn＝[δυEδυNδυU]T
Figure FDA00040298487900000511
Figure FDA00040298487900000512
Figure FDA00040298487900000513
式中，fn表示由加速度计输出得到的比力在导航坐标系中的投影，υn为捷联惯性导航系统的速度信息，υE、υN、υU分别表示东向速度、北向速度和天向速度，δυn表示捷联惯性导航系统的速度误差，L，λ，h分别表示地球纬度、地球经度以及高度，RM和RN分别表示载体所在位置的地球子午圈和卯酉圈曲率半径，ωie表示地球自转角速度。
7.根据权利要求1至5任一项所述的方法，其特征在于，根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计包括：
将二维激光多普勒测速仪的三个安装差角误差、四个比例因子误差、惯导姿态误差、速度误差、位置误差、陀螺测量误差以及加速度计测量误差共同构成组合导航系统中的状态向量：
Figure FDA0004029848790000061
式中，X(t)为组合导航系统的状态向量，
Figure FDA0004029848790000062
为惯导姿态误差，δυn为速度误差，δP为位置误差，
Figure FDA0004029848790000063
为陀螺测量误差，
Figure FDA0004029848790000064
为加速度计测量误差，φmx、φmy、φmz为二维激光多普勒测速仪的三个安装差角误差，δK1、δK2、δK3、δK4为四个比例因子误差；
令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对状态向量进行估计：
Figure FDA0004029848790000065
式中，z(t)为滤波观测量，H(t)为滤波器的量测矩阵，V(t)则为量测噪声向量，
Figure FDA0004029848790000066
为二维激光多普勒测速仪在三个光束方向上的速度。
8.一种紧耦合陆地组合导航装置，其特征在于，包括：
获取模块，用于获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的实际倾角以及第二光束的实际倾角，引入第三光束，并考虑设计倾角偏差，得到二维激光多普勒测速仪坐标系与光束坐标系之间的转换矩阵；
转换模块，用于获取捷联惯性导航系统在导航坐标系下的输出速度，并先后转换到载体坐标系和二维激光多普勒测速仪坐标系，得到捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度；
计算模块，用于根据转换矩阵和捷联惯性导航系统在二维激光多普勒测速仪坐标系下的输出速度，得到捷联惯性导航系统在光束坐标系下的输出速度；
导航模块，用于获取二维激光多普勒测速仪的误差参数，并构建捷联惯性导航系统的误差模型；根据捷联惯性导航系统的误差模型，构建卡尔曼滤波器，令捷联惯性导航系统在光束坐标系下的输出速度与二维激光多普勒测速仪在光束方向上的速度之差为滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202211725810.4A 2022-12-30 2022-12-30 一种紧耦合陆地组合导航方法、装置、计算机设备和介质 Pending CN115900705A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211725810.4A CN115900705A (zh) 2022-12-30 2022-12-30 一种紧耦合陆地组合导航方法、装置、计算机设备和介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211725810.4A CN115900705A (zh) 2022-12-30 2022-12-30 一种紧耦合陆地组合导航方法、装置、计算机设备和介质

Publications (1)

Publication Number Publication Date
CN115900705A true CN115900705A (zh) 2023-04-04

Family

ID=86485936

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211725810.4A Pending CN115900705A (zh) 2022-12-30 2022-12-30 一种紧耦合陆地组合导航方法、装置、计算机设备和介质

Country Status (1)

Country Link
CN (1) CN115900705A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117249826A (zh) * 2023-11-14 2023-12-19 中国人民解放军国防科技大学 基于二维ldv和惯导系统的容错组合导航方法及装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117249826A (zh) * 2023-11-14 2023-12-19 中国人民解放军国防科技大学 基于二维ldv和惯导系统的容错组合导航方法及装置
CN117249826B (zh) * 2023-11-14 2024-01-30 中国人民解放军国防科技大学 基于二维ldv和惯导系统的容错组合导航方法及装置

Similar Documents

Publication Publication Date Title
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
Wagner et al. Integrating satellite and inertial navigation—conventional and new fusion approaches
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN112835085B (zh) 确定车辆位置的方法和装置
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN114812545A (zh) 基于双激光多普勒测速仪和惯导系统组合导航方法及装置
CN114413895B (zh) 光纤陀螺旋转惯导联合定位方法、装置、设备及介质
CN115585826A (zh) 多惯导旋转调制光纤陀螺标度因数误差自校正方法与装置
CN115900705A (zh) 一种紧耦合陆地组合导航方法、装置、计算机设备和介质
CN113092822B (zh) 一种基于惯组的激光多普勒测速仪的在线标定方法和装置
CN114413934A (zh) 一种车辆定位系统校正方法和装置
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN115856922A (zh) 一种松耦合陆地组合导航方法、装置、计算机设备和介质
CN115950450A (zh) 二维多普勒测速仪的标定方法、装置、计算机设备和介质
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
CN111141285B (zh) 一种航空重力测量装置
CN116594000A (zh) 基于位置观测的激光多普勒测速仪在线标定方法和装置
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN114895340A (zh) 双天线gnss/ins组合导航系统的定位方法和装置
CN114353791B (zh) 基于未知洋流流速的组合导航方法及装置
CN115291266B (zh) 基于信息滤波算法的卫星惯性紧组合实时导航定位方法
CN110986931A (zh) 全球混合导航方法、装置、计算机设备和可读存储介质
CN108957508A (zh) 车载pos离线组合估计方法和装置
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination