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

Abstract

本申请属于组合导航技术领域，涉及一种松耦合陆地组合导航方法、装置、计算机设备和介质，方法包括：获取二维激光多普勒测速仪的光束波长、第一光束和第二光束各自的多普勒频移以及第一光束和第二光束各自的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的误差速度；将该误差速度转换到载体坐标系再转换到导航坐标系，得到导航坐标系下的误差速度，进而得到组合导航系统的量测模型；构建捷联惯性导航系统的误差模型；构建卡尔曼滤波器，估计组合导航系统的状态向量并进行导航。本方法基于二维激光多普勒测速仪与捷联惯性导航系统构成陆地组合导航系统，能提供载体的前向速度和天向速度。

Description

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

技术领域

本申请涉及组合导航技术领域，特别是涉及一种松耦合陆地组合导航方法、装置、计算机设备和介质。

背景技术

组合导航系统需要将多个传感器的信息进行融合，以发挥各个传感器各自的优势。在陆地组合导航领域，全球定位系统、里程计得到了广泛运用，但它们都有其各自的缺陷：全球定位系统信号不稳定，抗干扰性较差且信号易被树木以及建筑物遮蔽；里程计的精度与载体的行驶状态以及车轮的状态密切相关，车轮温度、气压、磨损状况以及车辆行驶过程中出现的跳动、打滑都将使里程计的测量精度降低。

现有技术中，一维激光多普勒测速仪在近几年小范围运用在陆用组合导航领域，并取得了不错的效果。虽然一维激光多普勒测速仪不能像全球定位系统那样提供导航系下的速度和位置信息，但一维激光多普勒测速仪是完全自主的，无需接受外界信号；与里程计相比，一维激光多普勒测速仪的测量精度更高，且具有非接触测量、空间分辨率好、动态响应快、测速范围广、方向灵敏度高等优势。

但是，美中不足的是，一维激光多普勒测速仪和里程计一样只能提供载体前向的速度，这往往使得一维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统在载体高度估计上不太理想。

发明内容

基于此，有必要针对上述技术问题，提供一种松耦合陆地组合导航方法、装置、计算机设备和介质，能够基于二维激光多普勒测速仪和捷联惯性导航系统构成陆地组合导航系统，不仅能提供载体的前向速度，还能提供载体的天向速度，且能够进一步提高组合导航系统定位精度，尤其是在高度上的精度。

一种松耦合陆地组合导航方法，包括：

获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度；

将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度；将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度；

构建捷联惯性导航系统的误差模型；获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型；

根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。

在一个实施例中，获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度包括：

获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移以及第二光束的多普勒频移，得到第一光束方向上的速度和第二光束方向上的速度：

式中，υ₁为沿第一光束方向上的速度，υ₂为沿第二光束方向上的速度，λ为二维激光多普勒测速仪的光束波长，

为第一光束所测得的多普勒频移和/>

为第二光束所测得的多普勒频移；

获取第一光束的设计倾角以及第二光束的设计倾角，并根据第一光束的设计倾角、第二光束的设计倾角、第一光束方向上的速度以及第二光束方向上的速度，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度：

式中，

为二维激光多普勒测速仪在自身坐标系下的误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的前向误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的天向误差速度，/>

δK_i为倾角设计值与实际倾角之间的偏差所引起的比例因子误差，/>

为二维激光多普勒测速仪第一光束的设计倾角，/>

为二维激光多普勒测速仪第二光束的设计倾角。

在一个实施例中，将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度包括：

φ_m＝[φ_mx φ_my φ_mz]^T

式中，

为二维激光多普勒测速仪在载体坐标系下的误差速度，I₃表示三阶单位矩阵，m表示二维激光多普勒测速仪所在的坐标系，b表示载体坐标系，φ_m表示二维激光多普勒测速仪所在坐标系与载体坐标系之间的安装差角误差，φ_mx为俯仰安装差角误差，φ_my为横滚安装差角误差，φ_mz为航向安装差角误差，×表示矩阵叉乘，/>

表示m系到b系的姿态转换矩阵。

在一个实施例中，将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度包括：

式中，

为二维激光多普勒测速仪在导航坐标系下的误差速度，n表示导航坐标系，φ表示惯导系统的姿态误差角，/>

表示b系到n系的姿态转换矩阵。

在一个实施例中，获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型包括：

由于：

δK＝[δK₁δK₂δK₃δK₄]^T

因此：

式中，

为捷联惯性导航系统的速度，/>

为捷联惯性导航系统的速度误差，

表示激光多普勒测速仪在n系中的真实速度，/>

表示激光多普勒测速仪在b系中的真实速度，/>

为二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型。

在一个实施例中，构建捷联惯性导航系统的误差模型包括：

其中：

υⁿ＝[υ_Eυ_Nυ_U]^T

δυⁿ＝[δυ_Eδυ_Nδυ_U]^T

式中，fⁿ表示由加速度计输出得到的比力在导航坐标系中的投影，υⁿ为捷联惯性导航系统的速度信息，υ_E、υ_N、υ_U分别表示东向速度、北向速度和天向速度，δυⁿ表示捷联惯性导航系统的速度误差，L，λ，h分别表示地球纬度、地球经度以及高度，R_M和R_N分别表示载体所在位置的地球子午圈和卯酉圈曲率半径，ω_ie表示地球自转角速度。

在一个实施例中，根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计包括：

将二维激光多普勒测速仪的三个安装差角误差、四个比例因子误差、惯导姿态误差、速度误差、位置误差、陀螺测量误差以及加速度计测量误差共同构成组合导航系统中的状态向量：

式中，X(t)为组合导航系统的状态向量，

为惯导姿态误差，δυⁿ为速度误差，δP为位置误差，/>

为陀螺测量误差，/>

为加速度计测量误差，φ_mx、φ_my、φ_mz为二维激光多普勒测速仪的三个安装差角误差，δK₁、δK₂、δK₃、δK₄为四个比例因子误差；

令二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型作为卡尔曼滤波器的滤波观测量，对状态向量进行估计：

/>

式中，z(t)为滤波观测量，H(t)为滤波器的量测矩阵，V(t)则为量测噪声向量。

一种松耦合陆地组合导航装置，包括：

获取模块，用于获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度；

转换模块，用于将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度；将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度；

建模模块，用于构建捷联惯性导航系统的误差模型；获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型；

导航模块，用于根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度；

将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度；将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度；

构建捷联惯性导航系统的误差模型；获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型；

根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度；

将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度；将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度；

构建捷联惯性导航系统的误差模型；获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型；

根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。

上述松耦合陆地组合导航方法，根据二维激光多普勒测速仪的两个输出得到二维激光多普勒测速仪在自身坐标系下前向和天向方向上的速度表达式，并分析二维激光多普勒测速仪在组合导航过程中的误差项(包括：四个比例因子误差δK₁、δK₂、δK₃、δK₄以及三个安装差角误差φ_mx、φ_my、φ_mz)，进而得到二维激光多普勒测速仪在导航坐标系下的误差速度表达式，将二维激光多普勒测速仪的误差项作为滤波器状态向量的一部分，并将二维激光多普勒测速仪与捷联惯性导航系统在导航系下的速度差作为滤波观测量，根据滤波得到结果对组合导航系统进行反馈修正；本申请能够基于二维激光多普勒测速仪和捷联惯性导航系统构成陆地组合导航系统，利用二维激光多普勒测速仪得到二维速度，即采用了一种松耦合方式，不仅能提供载体的前向速度，还能提供载体的天向速度，因此在高程测量上可以获得比现有技术更高的精度；而且，本申请将二维激光多普勒测速仪的七个误差项以及捷联惯性导航系统的误差项一起作为状态向量进行卡尔曼滤波并实时补偿，使得由二维激光多普勒测速仪得到的速度具有更高的精度，从而进一步提高组合导航系统的定位精度，尤其是在高度上的精度；再者，本申请考虑横滚安装差角误差，并根据二维激光多普勒测速仪的光路结构将比例因子误差增加至四个，因此二维激光多普勒测速仪在自身坐标系下的误差参数得到了充分考虑，有助于进一步提高组合导航的精度。

附图说明

图1为一个实施例中松耦合陆地组合导航方法的应用场景图；

图2为一个实施例中松耦合陆地组合导航方法的流程示意图；

图3为一个实施例中二维激光多普勒测速仪在组合导航系统中的安装位置以及出射光路图；

图4为一个实施例中松耦合陆地组合导航装置的结构框图；

图5为一个实施例中计算机设备的内部结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明，本申请实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本申请中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多组”的含义是至少两组，例如两组，三组等，除非另有明确具体的限定。

在本申请中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。

另外，本申请各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本申请要求的保护范围之内。

本申请提供的松耦合陆地组合导航方法，可以应用于如图1所示的应用环境中。其中，终端102通过网络与服务器104进行通信，终端102可以包括但不限于是各种个人计算机、笔记本电脑、智能手机、平板电脑和便携式可穿戴设备，服务器104可以是各类门户网站、工作系统后台对应的服务器等。

本申请提供了一种松耦合陆地组合导航方法，如图2所示，在一个实施例中，以该方法应用于图1中的终端为例进行说明，包括：

步骤202，获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度。

具体的：

获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移以及第二光束的多普勒频移，得到第一光束方向上的速度和第二光束方向上的速度：

式中，υ₁为沿第一光束方向上的速度，υ₂为沿第二光束方向上的速度，λ为二维激光多普勒测速仪的光束波长，

为第一光束所测得的多普勒频移和/>

为第二光束所测得的多普勒频移。

获取第一光束的实际倾角以及第二光束的实际倾角，并根据第一光束的实际倾角、第二光束的实际倾角、第一光束方向上的速度以及第二光束方向上的速度，得到二维激光多普勒测速仪在自身坐标系下的真实前向速度和真实天向速度，并得到二维激光多普勒测速仪在自身坐标系下的真实速度：

将(2)式和(3)式改写得：

υ_y＝K₁υ₂-K₂υ₁ (4)

υ_z＝K₃υ₁-K₄υ₂ (5)

其中：

式中，

为二维激光多普勒测速仪在自身坐标系下的真实速度，υ_y为二维激光多普勒测速仪在自身坐标系下的真实前向速度，υ_z为二维激光多普勒测速仪在自身坐标系下的真实天向速度，K₁、K₂、K₃和K₄为比例因子，θ₁为二维激光多普勒测速仪第一光束的实际倾角，θ₂为二维激光多普勒测速仪第二光束的实际倾角。

由于实际应用中光束的实际倾角θ₁和θ₂与光束设计值不相等，因此根据倾角设计值得到的载体前向速度和天向速度会和真实值存在一定偏差。

将倾角误差考虑在内，获取第一光束的设计倾角以及第二光束的设计倾角，并根据第一光束的设计倾角、第二光束的设计倾角、第一光束方向上的速度以及第二光束方向上的速度，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度：

/>

式中，

为二维激光多普勒测速仪在自身坐标系下的误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的前向误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的天向误差速度，/>

δK_i为倾角设计值与实际倾角之间的偏差所引起的比例因子误差，/>

为二维激光多普勒测速仪第一光束的设计倾角，/>

为二维激光多普勒测速仪第二光束的设计倾角。

步骤204，将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度；将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度。

具体的：

在实际应用中，由于安装差角的存在，二维激光多普勒测速仪所在的坐标系与惯性测量单元所在的载体坐标系并不一致，因此需要考虑两者之间的偏差。

其中，

φ_m＝[φ_mxφ_myφ_mz]^T

式中，

为二维激光多普勒测速仪在载体坐标系下的误差速度的投影，I₃表示三阶单位矩阵，m表示二维激光多普勒测速仪所在的坐标系，b表示载体坐标系，φ_m表示二维激光多普勒测速仪所在坐标系与载体坐标系之间的安装差角误差，φ_mx为俯仰安装差角误差，φ_my为横滚安装差角误差，φ_mz为航向安装差角误差，×表示矩阵叉乘，/>

表示m系到b系的姿态转换矩阵。

其中，

式中，

为二维激光多普勒测速仪在导航坐标系下的误差速度，n表示导航坐标系，φ表示惯导系统的姿态误差角，/>

表示b系到n系的姿态转换矩阵。

步骤206，构建捷联惯性导航系统的误差模型；获取捷联惯性导航系统的速度误差，并根据捷联惯性导航系统的速度误差、二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型。

具体的：

由于捷联惯性导航系统的速度可以表示为，

且联立(11)-(18)式有实际应用中导航坐标系下二维激光多普勒测速仪的速度表达式，

其中，

δK＝[δK₁δK₂δK₃δK₄]^T

将(19)式重写为，

则二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型

为，

式中，

为捷联惯性导航系统的速度，/>

为捷联惯性导航系统的速度误差，

表示激光多普勒测速仪在n系中的真实速度投影，/>

表示激光多普勒测速仪在b系中的真实速度。

需要说明，二维激光多普勒测速仪的误差参数包括：φ_m和δK，其中，φ_m＝[φ_mxφ_myφ_mz]^T，δK＝[δK₁δK₂δK₃δK₄]^T，也就是说，二维激光多普勒测速仪的误差参数包括七个参数，具体包括φ_mx、φ_my、φ_mz、这三个安装差角误差以及δK₁、δK₂、δK₃、δK₄这四个比例因子误差。

虽然二维激光多普勒测速仪在使用前是标定过的，但在实际中不会每次使用前都标定仪器，而之前的标定参数并不一定完全与当前时刻的二维激光多普勒测速仪的误差参数抑制，且路面的颠簸也会导致二维激光多普勒测速仪的误差参数出现细微变化。

因此在组合导航过程中需要考虑二维激光多普勒测速仪的误差项。

在基于二维激光多普勒测速仪与捷联惯性导航系统的组合导航系统中，建立捷联惯性导航系统的误差模型如下：

姿态误差：

速度误差：

位置误差：

陀螺误差：

加表误差：

其中：

又有：

υⁿ＝[υ_E υ_N υ_U]^T

δυⁿ＝[δυ_E δυ_N δυ_U]^T

式中，fⁿ表示由加速度计输出得到的比力在导航坐标系中的投影，υⁿ为捷联惯性导航系统的速度信息，υ_E、υ_N、υ_U分别表示东向速度、北向速度和天向速度，δυⁿ表示捷联惯性导航系统的速度误差，L，λ，h分别表示地球纬度、地球经度以及高度，P＝[L λ h]^T表示捷联惯性导航系统的位置信息，δP＝[δL δλ δh]^T表示捷联惯性导航系统的位置误差，R_M和R_N分别表示载体所在位置的地球子午圈和卯酉圈曲率半径，ω_ie表示地球自转角速度。

步骤208，根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。

具体的：

将二维激光多普勒测速仪的三个安装差角误差、四个比例因子误差、惯导姿态误差、速度误差、位置误差、陀螺测量误差以及加速度计测量误差共同构成组合导航系统中的状态向量，卡尔曼滤波模型中的状态向量为22维，即：

/>

式中，X(t)为组合导航系统的状态向量，

为惯导姿态误差，δυⁿ为惯导速度误差(式(20)中写为δυ)，δP为惯导位置误差，/>

为陀螺测量误差，/>

为加速度计测量误差，φ_mx、φ_my、φ_mz为二维激光多普勒测速仪的三个安装差角误差，分别是俯仰安装角误差、横滚安装角误差以及航向安装角误差，δK₁、δK₂、δK₃、δK₄为四个比例因子误差；

滤波器的状态方程表示为：

式中，F(t)表示系统状态转移矩阵，G(t)表示系统噪声矩阵，w(t)表示系统噪声向量；

系统状态转移矩阵F(t)表示为：

其中，

/>

系统噪声矩阵表示为：

系统噪声向量表示为：

式中，ε_wi表示陀螺的噪声，

表示加速度计的噪声。

令二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型作为卡尔曼滤波器的滤波观测量，对状态向量进行估计，卡尔曼滤波中的量测方程为：

即：

式中，z(t)为滤波观测量，H(t)为滤波器的量测矩阵，V(t)则为量测噪声向量；

量测矩阵H(t)表示为：

其中，

式中，

表示载体坐标系下二维激光多普勒测速仪的天向速度，/>

表示载体坐标系下二维激光多普勒测速仪的前向速度，/>

表示载体坐标系下二维激光多普勒测速仪的右向速度。

在本步骤中，构建卡尔曼滤波器，令二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型作为卡尔曼滤波器的滤波观测量，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，根据滤波得到的状态向量对二维激光多普勒测速仪和捷联惯性导航系统的相关项也就是滤波状态量进行反馈校正，并根据校正后的定位结果进行陆地组合导航。

在本实施例中，如图3所示，IMU表示惯性测量单元，LDV表示二维激光多普勒测速仪，X_bY_bZ_b表示惯性测量单元所在的载体坐标系，X_mY_mZ_m表示二维激光多普勒测速仪所在的自身坐标系。还需要说明，SINS表示捷联惯性导航系统。

捷联惯性导航系统在使用时，需要合理的安装惯性测量单元和二维激光多普勒测速仪，具体的，将惯性测量单元安装在车辆后轮轴的中央，将二维激光多普勒测速仪安装在车辆的侧面或者尾部合适的位置，使二维激光多普勒测速仪输出的光束能够有效打在地面上并获得较强的信号，以实时获得有效的载体速度信息。在捷联惯性导航系统使用前，需对惯性测量单元进行标定以获得陀螺和加速度计的刻度系数误差、安装误差角、零偏。在进入正式导航过程前还需要完成系统的初始化以获得初始姿态、速度与位置，包括获得载体的初始位置和速度信息以及完成初始对准。

上述松耦合陆地组合导航方法，根据二维激光多普勒测速仪的两个输出得到二维激光多普勒测速仪在自身坐标系下前向和天向方向上的速度表达式，并分析二维激光多普勒测速仪在组合导航过程中的误差项(包括：四个比例因子误差δK₁、δK₂、δK₃、δK₄以及三个安装差角误差φ_mx、φ_my、φ_mz)，进而得到二维激光多普勒测速仪在导航坐标系下的误差速度表达式，将二维激光多普勒测速仪的误差项作为滤波器状态向量的一部分，并将二维激光多普勒测速仪与捷联惯性导航系统在导航系下的速度差作为滤波观测量，根据滤波得到结果对组合导航系统进行反馈修正；本申请能够基于二维激光多普勒测速仪和捷联惯性导航系统构成陆地组合导航系统，利用二维激光多普勒测速仪得到二维速度，即采用了一种松耦合方式，不仅能提供载体的前向速度，还能提供载体的天向速度，因此在高程测量上可以获得比现有技术更高的精度；而且，本申请将二维激光多普勒测速仪的七个误差项以及捷联惯性导航系统的误差项一起作为状态向量进行卡尔曼滤波并实时补偿，使得由二维激光多普勒测速仪得到的速度具有更高的精度，从而进一步提高组合导航系统的定位精度，尤其是在高度上的精度；再者，本申请考虑横滚安装差角误差，并根据二维激光多普勒测速仪的光路结构将比例因子误差增加至四个，因此二维激光多普勒测速仪在自身坐标系下的误差参数得到了充分考虑，有助于进一步提高组合导航的精度。

应该理解的是，虽然图2的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

本申请还提供了一种松耦合陆地组合导航装置，如图4所示，在一个实施例中，包括：获取模块402、转移模块404、建模模块406和导航模块408，其中：

获取模块402，用于获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度；

转换模块404，用于将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度；将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度；

建模模块406，用于构建捷联惯性导航系统的误差模型；获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型；

导航模块408，用于根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。

在一个实施例中，获取模块402还用于获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度包括：

获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移以及第二光束的多普勒频移，得到第一光束方向上的速度和第二光束方向上的速度：

式中，υ₁为沿第一光束方向上的速度，υ₂为沿第二光束方向上的速度，λ为二维激光多普勒测速仪的光束波长，

为第一光束所测得的多普勒频移和/>

为第二光束所测得的多普勒频移；

获取第一光束的设计倾角以及第二光束的设计倾角，并根据第一光束的设计倾角、第二光束的设计倾角、第一光束方向上的速度以及第二光束方向上的速度，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度：

式中，

为二维激光多普勒测速仪在自身坐标系下的误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的前向误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的天向误差速度，/>

δK_i为倾角设计值与实际倾角之间的偏差所引起的比例因子误差，/>

为二维激光多普勒测速仪第一光束的设计倾角，/>

为二维激光多普勒测速仪第二光束的设计倾角。

在一个实施例中，转换模块404还用于将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度包括：

φ_m＝[φ_mxφ_myφ_mz]^T

式中，

为二维激光多普勒测速仪在载体坐标系下的误差速度，I₃表示三阶单位矩阵，m表示二维激光多普勒测速仪所在的坐标系，b表示载体坐标系，φ_m表示二维激光多普勒测速仪所在坐标系与载体坐标系之间的安装差角误差，φ_mx为俯仰安装差角误差，φ_my为横滚安装差角误差，φ_mz为航向安装差角误差，×表示矩阵叉乘，/>

表示m系到b系的姿态转换矩阵。

在一个实施例中，转换模块404还用于将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度包括：

式中，

为二维激光多普勒测速仪在导航坐标系下的误差速度，n表示导航坐标系，φ表示惯导系统的姿态误差角，/>

表示b系到n系的姿态转换矩阵。

在一个实施例中，建模模块406还用于获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型包括：

由于：

δK＝[δK₁δK₂δK₃δK₄]^T

因此：

式中，

为捷联惯性导航系统的速度，/>

为捷联惯性导航系统的速度误差，

表示激光多普勒测速仪在n系中的真实速度，/>

表示激光多普勒测速仪在b系中的真实速度，/>

为二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型。

在一个实施例中，建模模块406还用于构建捷联惯性导航系统的误差模型包括：

/>

其中：

υⁿ＝[υ_Eυ_Nυ_U]^T

δυⁿ＝[δυ_Eδυ_Nδυ_U]^T

式中，fⁿ表示由加速度计输出得到的比力在导航坐标系中的投影，υⁿ为捷联惯性导航系统的速度信息，υ_E、υ_N、υ_U分别表示东向速度、北向速度和天向速度，δυⁿ表示捷联惯性导航系统的速度误差，L，λ，h分别表示地球纬度、地球经度以及高度，R_M和R_N分别表示载体所在位置的地球子午圈和卯酉圈曲率半径，ω_ie表示地球自转角速度。

在一个实施例中，导航模块408还用于根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计包括：

将二维激光多普勒测速仪的三个安装差角误差、四个比例因子误差、惯导姿态误差、速度误差、位置误差、陀螺测量误差以及加速度计测量误差共同构成组合导航系统中的状态向量：

式中，X(t)为组合导航系统的状态向量，

为惯导姿态误差，δυⁿ为速度误差，δP为位置误差，/>

为陀螺测量误差，/>

为加速度计测量误差，φ_mx、φ_my、φ_mz为二维激光多普勒测速仪的三个安装差角误差，δK₁、δK₂、δK₃、δK₄为四个比例因子误差；

令二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型作为卡尔曼滤波器的滤波观测量，对状态向量进行估计：

式中，z(t)为滤波观测量，H(t)为滤波器的量测矩阵，V(t)则为量测噪声向量。

关于松耦合陆地组合导航装置的具体限定可以参见上文中对于松耦合陆地组合导航方法的限定，在此不再赘述。上述装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图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所述的方法，其特征在于，获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度包括：

获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移以及第二光束的多普勒频移，得到第一光束方向上的速度和第二光束方向上的速度：

式中，υ₁为沿第一光束方向上的速度，υ₂为沿第二光束方向上的速度，λ为二维激光多普勒测速仪的光束波长，

为第一光束所测得的多普勒频移和/>

为第二光束所测得的多普勒频移；

获取第一光束的设计倾角以及第二光束的设计倾角，并根据第一光束的设计倾角、第二光束的设计倾角、第一光束方向上的速度以及第二光束方向上的速度，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度：

/>

式中，

为二维激光多普勒测速仪在自身坐标系下的误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的前向误差速度，/>

为二维激光多普勒测速仪在自身坐标系下的天向误差速度，/>

δK_i为倾角设计值与实际倾角之间的偏差所引起的比例因子误差，/>

为二维激光多普勒测速仪第一光束的设计倾角，/>

为二维激光多普勒测速仪第二光束的设计倾角。

3.根据权利要求2所述的方法，其特征在于，将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度包括：

φ_m＝[φ_mx φ_my φ_mz]^T

式中，

为二维激光多普勒测速仪在载体坐标系下的误差速度，I₃表示三阶单位矩阵，m表示二维激光多普勒测速仪所在的坐标系，b表示载体坐标系，φ_m表示二维激光多普勒测速仪所在坐标系与载体坐标系之间的安装差角误差，φ_mx为俯仰安装差角误差，φ_my为横滚安装差角误差，φ_mz为航向安装差角误差，×表示矩阵叉乘，/>

表示m系到b系的姿态转换矩阵。

4.根据权利要求3所述的方法，其特征在于，将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度包括：

式中，

为二维激光多普勒测速仪在导航坐标系下的误差速度，n表示导航坐标系，φ表示惯导系统的姿态误差角，/>

表示b系到n系的姿态转换矩阵。

5.根据权利要求4所述的方法，其特征在于，获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型包括：

由于：

δK＝[δK₁δK₂δK₃δK₄]^T

因此：

式中，

为捷联惯性导航系统的速度，/>

为捷联惯性导航系统的速度误差，/>

表示激光多普勒测速仪在n系中的真实速度，/>

表示激光多普勒测速仪在b系中的真实速度，/>

为二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型。

6.根据权利要求1至5任一项所述的方法，其特征在于，构建捷联惯性导航系统的误差模型包括：

其中：

/>

式中，fⁿ表示由加速度计输出得到的比力在导航坐标系中的投影，υⁿ为捷联惯性导航系统的速度信息，υ_E、υ_N、υ_U分别表示东向速度、北向速度和天向速度，δυⁿ表示捷联惯性导航系统的速度误差，L，λ，h分别表示地球纬度、地球经度以及高度，R_M和R_N分别表示载体所在位置的地球子午圈和卯酉圈曲率半径，ω_ie表示地球自转角速度。

7.根据权利要求1至5任一项所述的方法，其特征在于，根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计包括：

将二维激光多普勒测速仪的三个安装差角误差、四个比例因子误差、惯导姿态误差、速度误差、位置误差、陀螺测量误差以及加速度计测量误差共同构成组合导航系统中的状态向量：

式中，X(t)为组合导航系统的状态向量，

为惯导姿态误差，δυⁿ为速度误差，δP为位置误差，/>

为陀螺测量误差，/>

为加速度计测量误差，φ_mx、φ_my、φ_mz为二维激光多普勒测速仪的三个安装差角误差，δK₁、δK₂、δK₃、δK₄为四个比例因子误差；

令二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型作为卡尔曼滤波器的滤波观测量，对状态向量进行估计：

式中，z(t)为滤波观测量，H(t)为滤波器的量测矩阵，V(t)则为量测噪声向量。

8.一种松耦合陆地组合导航装置，其特征在于，包括：

获取模块，用于获取二维激光多普勒测速仪的光束波长、第一光束的多普勒频移、第二光束的多普勒频移、第一光束的设计倾角以及第二光束的设计倾角，得到二维激光多普勒测速仪在自身坐标系下的前向误差速度和天向误差速度，并得到二维激光多普勒测速仪在自身坐标系下的误差速度；

转换模块，用于将二维激光多普勒测速仪在自身坐标系下的误差速度转换到载体坐标系，得到二维激光多普勒测速仪在载体坐标系下的误差速度；将二维激光多普勒测速仪在载体坐标系下的误差速度转换到导航坐标系，得到二维激光多普勒测速仪在导航坐标系下的误差速度；

建模模块，用于构建捷联惯性导航系统的误差模型；获取捷联惯性导航系统的速度误差，并根据二维激光多普勒测速仪的误差参数和二维激光多普勒测速仪在导航坐标系下的误差速度，得到二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的量测模型；

导航模块，用于根据组合导航系统的量测模型和捷联惯性导航系统的误差模型，构建卡尔曼滤波器，对二维激光多普勒测速仪与捷联惯性导航系统构成的组合导航系统的状态向量进行估计，并根据估计结果进行陆地组合导航。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。

Images