CN107015259A - 采用多普勒测速仪计算伪距/伪距率的紧组合方法 - Google Patents

Abstract

本发明公开了一种采用多普勒测速仪计算伪距/伪距率的紧组合方法，该方法分为以下步骤：步骤1、将捷联惯性导航系统、激光多普勒测速仪、北斗卫星导航系统安装到运动载体上，组合导航系统上电启动；步骤2、进行初始位置参数装订；步骤3、运动载体保持静止5分钟，进行初始对准，得到初始姿态角；步骤4、进入紧组合模式，启动运动载体；步骤5、计算得到伪距、伪距率值，与北斗卫星导航系统输出的伪距、伪距率值之差作为组合导航系统的观测量，进行紧组合导航。本发明采用激光多普勒测速仪计算的伪距、伪距率与北斗卫星导航系统进行紧组合，将二者功能互补，根据运动载体的位置和北斗卫星推算出运动载体的伪距和伪距率，实现高精度导航。

Description

采用多普勒测速仪计算伪距/伪距率的紧组合方法

技术领域

本发明涉及一种紧组合方法，尤其涉及一种采用多普勒测速仪计算伪距/伪距率的紧组合方法，属于导航技术领域。

背景技术

激光多普勒测速仪是根据多普勒频移可以实时测量运动运动载体相对地面移动的速度，且其测量误差不随时间积累，与惯导组合使用可以实现高精度导航。但随着时间的推移系统误差也会不断累积，组合导航系统导航的精度也会下降。北斗卫星导航系统是由我国自主研发的北斗卫星导航系统，可以提供高精度的实时位置、速度信息以及伪距和伪距率。但当其受到干扰时，会丧失导航能力。

发明内容

为了解决上述问题中的不足之处，本发明提供了一种采用多普勒测速仪计算伪距/伪距率的紧组合方法。

为解决以上固定问题，本发明采用的解决方案是：一种采用多普勒测速仪计算伪距/伪距率的紧组合方法，该方法分为以下步骤：

步骤1、将捷联惯性导航系统、激光多普勒测速仪、北斗卫星导航系统安装到运动载体上，形成运动载体的组合导航系统，组合导航系统上电启动；

步骤2、组合导航系统上电启动后，将北斗卫星导航系统接收到的运动载体初始位置的经度、维度、高度作为捷联惯性导航系统的初始位置参数，将初始位置参数装订至捷联惯性导航系统的导航计算机中；

步骤3、运动载体保持静止5分钟，根据装订的初始位置参数进行初始对准；得到最终运动载体的初始姿态角；

步骤4、完成初始对准后，组合导航系统进入紧组合模式，启动运动载体；

步骤5、由北斗卫星导航系统采集的北斗卫星位置、捷联惯性导航系统与激光多普勒测速仪解算的位置进行计算得到伪距、伪距率值；

然后根据得到的伪距、伪距率值与北斗卫星导航系统输出的伪距、伪距率值之差作为组合导航系统的观测量，进行紧组合导航。

其中，步骤3初始对准的具体方式为：组合导航系统对陀螺和加速度计采集到的数据进行处理，然后根据捷联捷联惯性导航系统误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成组合导航系统的粗对准，初步确定运动载体的姿态角；其中粗对准时间为2分钟；粗对准完成后，再利用卡尔曼滤波技术对精对准3分钟，得到最终运动载体的初始姿态角。

步骤5中得到的伪距、伪距率值，是采用捷联惯性导航系统输出的姿态信息与激光多普勒测速仪输出的速度信息解算出运动载体的位置；进行解算时需要将激光多普勒测速仪在运动载体坐标系下的速度转换到导航坐标系下，具体方法为：

将激光测速仪在运动载体坐标系下的速度定义为在导航坐标系下的速度定义为捷联惯性导航系统输出的姿态矩阵为运动载体坐标系下的速度转换到导航坐标系下的速度关系如下：

根据导航坐标系下的激光多普勒测速仪速度和航位推算位置计算公式可以得到运动载体的位置：

(2)式中，λ、L、h分别为计算所得经度、纬度、高度的值，λ₀、L₀、h₀分别为经度、纬度、高度的初值，R_M、R_N分别为子午圈曲率半径和卯酉圈曲率半径；要计算伪距、伪距率需要将经度、纬度、高度转换成空间直角坐标系，计算公式如下：

x＝(R_N+h)cosLcosλ

y＝(R_N+h)cosLsinλ

z＝[R_N(1-e²)+h]sinL (3)

设由北斗卫星星历确定的空间直角坐标系下北斗卫星位置为(x_s y_s z_s)^Τ，则可以计算得到相应于运动载体坐标系统所在位置的伪距ρ_I如下：

ρ_I＝[(x-x_s)²+(y-y_s)²+(z-z_s)²]^1/2 (4)

运动载体与北斗卫星之间的伪距变化率简称伪距率；其中，ρ_B，分别为北斗卫星导航系统接收到的伪距和伪距率；

当捷联惯性导航系统与北斗卫星导航系统组成的子系统选择伪距、伪距率进行组合时，系统的观测量包含两种：一种是伪距量测差值，由激光多普勒测速仪与捷联惯性导航系统形成的子系统解算出解算出的伪距值与北斗卫星导航系统给出的相应伪距值作差作为一种量测值；另一种是伪距率量测差值；将运动载体与北斗卫星间伪距的变化率表示成伪距率；伪距率量测差值由激光多普勒测速仪与捷联惯性导航系统形成的子系统的伪距变化率与北斗卫星导航系统给出的相应伪距率值作差得到；

建立组合导航系统状态误差模型如下：

(5)式中，X(t)为20维系统误差状态向量；W(t)为系统白噪声阵；F(t)为系统状态矩阵；

(6)式中φ_E,φ_N,φ_U为姿态误差，ε_x,ε_y,ε_z为陀螺漂移，▽_x,▽_y,▽_z为加速度计零偏，δL,δλ,δh分别为纬度、经度、高度误差，δv_E,δv_N,δv_U为东、北、天速度误差，δK_D激光测速仪标度误差，δα_ψ,δα_θ分别为激光测速仪航向安装角、俯仰安装角误差；δt_u为时钟误差等效的距离误差，δt_ru为时钟频率误差等效的距离率误差；

组合导航系统采用伪距、伪距率的组合方式，以激光多普勒测速仪、捷联惯性导航系统与北斗卫星导航系统两者伪距之差距之差和伪距率之差同时作为系统观测量；

组合导航系统中，ρ_B，分别为北斗卫星导航系统到北斗卫星的伪距和伪距率，可直接由北斗卫星导航系统给出；ρ_I，分别为激光多普勒测速仪/捷联惯性导航系统到北斗卫星的伪距和伪距率；

设组合导航系统的量测方程表示为：

Ζ(t)＝H(t)X(t)+V(t) (8)

(8)式中，Ζ(t)为H系(t)统X(t观)测V(t量)，H(t)为Ζ系(t统)量H测(t)矩X(阵t)，V(t)为量测噪声阵。

本发明采用激光多普勒测速仪计算的伪距、伪距率与北斗卫星导航系统进行紧组合，可以将二者功能互补，将激光多普勒测速仪与惯性导航系统结合解算出运动载体的位置，根据运动载体的位置和北斗卫星可以推算出运动载体的伪距和伪距率，实现高精度导航。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明紧组合方法的整体流程图。

图2为本发明多普勒测速仪伪距、伪距率计算流程图。

图3为紧组合导航的系统框图。

图2中符号说明如下：

L：由激光多普勒测速仪与惯导系统经过航位推算算法得到的纬度；

λ：由激光多普勒测速仪与惯导系统经过航位推算算法得到的经度；

h：由激光多普勒测速仪与惯导系统经过航位推算算法得到的高度；

(x y z)：激光多普勒测速仪/惯导系统在空间直角坐标系下的位置坐标；

(x_s y_s z_s)：北斗接收机输出的卫星位置坐标；

ρ_I：由激光多普勒测速仪/惯导系统位置和卫星位置计算得到的伪距；

由激光多普勒测速仪/惯导系统位置和卫星位置计算得到的伪距率；

图3中符号说明如下：

ρ_B：北斗接收机输出的伪距；

北斗接收机输出的伪距率；

ρ_I：由激光多普勒测速仪/惯导系统位置和卫星位置计算得到的伪距；

由激光多普勒测速仪/惯导系统位置和卫星位置计算得到的伪距率；

具体实施方式

如图1所示，本发明分为以下步骤：

步骤1、将捷联惯性导航系统、激光多普勒测速仪、北斗卫星导航系统安装到运动载体上，形成运动载体的组合导航系统，组合导航系统上电启动；

步骤2、组合导航系统上电启动后，将北斗卫星导航系统接收到的运动载体初始位置的经度、维度、高度作为捷联惯性导航系统的初始位置参数，将初始位置参数装订至捷联惯性导航系统的导航计算机中；

步骤3、运动载体保持静止5分钟，根据装订的初始位置参数进行初始对准；具体方式是：组合导航系统对陀螺和加速度计采集到的数据进行处理，然后根据捷联捷联惯性导航系统误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成组合导航系统的粗对准，初步确定运动载体的姿态角；其中粗对准时间为2分钟；粗对准完成后，再利用卡尔曼滤波技术对精对准3分钟，得到最终运动载体的初始姿态角；

步骤4、完成初始对准后，组合导航系统进入紧组合模式，启动运动载体；

步骤5、由北斗卫星导航系统采集的北斗卫星位置、捷联惯性导航系统与激光多普勒测速仪解算的位置进行计算得到伪距、伪距率值，然后根据得到的伪距、伪距率值与北斗卫星导航系统输出的伪距、伪距率值之差作为组合导航系统的观测量，进行紧组合导航。

其中，捷联惯性导航系统与激光多普勒测速仪解算的位置计算得到伪距、伪距率值，是采用捷联惯性导航系统输出的姿态信息与激光多普勒测速仪输出的速度信息解算出运动载体的位置；进行解算时需要将激光多普勒测速仪在运动载体坐标系下的速度转换到导航坐标系下。

本发明将激光测速仪在运动载体坐标系下的速度定义为在导航坐标系下的速度定义为捷联惯性导航系统输出的姿态矩阵为运动载体坐标系下的速度转换到导航坐标系下的速度关系如下：

根据导航坐标系下的激光多普勒测速仪速度和航位推算位置计算公式可以得到运动载体的位置：

(2)式中，λ、L、h分别为计算所得经度、纬度、高度的值，λ₀、L₀、h₀分别为经度、纬度、高度的初值，R_M、R_N分别为子午圈曲率半径和卯酉圈曲率半径。要计算伪距、伪距率需要将经度、纬度、高度转换成空间直角坐标系，计算公式如下：

x＝(R_N+h)cosLcosλ

y＝(R_N+h)cosLsinλ

z＝[R_N(1-e²)+h]sinL (3)

设由北斗卫星星历确定的空间直角坐标系下北斗卫星位置为(x_s y_s z_s)^Τ，则可以计算得到相应于运动载体坐标系统所在位置的伪距ρ_I如下：

ρ_I＝[(x-x_s)²+(y-y_s)²+(z-z_s)²]^1/2 (4)

运动载体与北斗卫星之间的伪距变化率简称伪距率；其中，ρ_B，分别为北斗卫星导航系统接收到的伪距和伪距率。

当捷联惯性导航系统与北斗卫星导航系统组成的子系统选择伪距、伪距率进行组合时，系统的观测量包含两种：一种是伪距量测差值，由激光多普勒测速仪与捷联惯性导航系统形成的子系统解算出解算出的伪距值与北斗卫星导航系统给出的相应伪距值作差作为一种量测值；另一种是伪距率量测差值，由于北斗卫星是运动的，激光多普勒测速仪与捷联惯性导航系统形成的子系统安装于运动载体上，运动载体相对于北斗卫星有相对运动，所以将运动载体与北斗卫星间伪距的变化率表示成伪距率；伪距率量测差值由激光多普勒测速仪与捷联惯性导航系统形成的子系统的伪距变化率与北斗卫星导航系统给出的相应伪距率值作差得到。

建立组合导航系统状态误差模型如下：

(5)式中，X(t)为20维系统误差状态向量；W(t)为系统白噪声阵；F(t)为系统状态矩阵；可以根据捷联惯性导航系统误差方程、激光多普勒测速仪误差方程、北斗系统误差方程计算得到。

(6)式中φ_E,φ_N,φ_U为姿态误差，ε_x,ε_y,ε_z为陀螺漂移，▽_x,▽_y,▽_z为加速度计零偏，δL,δλ,δh分别为纬度、经度、高度误差，δv_E,δv_N,δv_U为东、北、天速度误差，δK_D激光测速仪标度误差，δα_ψ,δα_θ分别为激光测速仪航向安装角、俯仰安装角误差。δt_u为时钟误差等效的距离误差，δt_ru为时钟频率误差等效的距离率误差。

组合导航系统采用伪距、伪距率的组合方式，以激光多普勒测速仪、捷联惯性导航系统与北斗卫星导航系统两者伪距之差距之差和伪距率之差同时作为系统观测量。

组合导航系统中，ρ_B，分别为北斗卫星导航系统到北斗卫星的伪距和伪距率，可直接由北斗卫星导航系统给出。ρ_I，分别为激光多普勒测速仪/捷联惯性导航系统到北斗卫星的伪距和伪距率。

设组合导航系统的量测方程表示为：

Ζ(t)＝H(t)X(t)+V(t) (8)

(8)式中，Ζ(t)为H(系t)X统(t)观测V(t量)，H(t)为系Ζ(t统)量H测(t)矩X(阵t)，V(t)为量测噪声阵，可以根据测速仪误差方程、伪距、伪距率误差方程计算得到。

本发明根据捷联惯性导航系统误差方程，激光多普勒测速仪误差方程，伪距、伪距率误差方程最终得到了系统误差状态方程、量测方程以及通过多普勒激光测速仪计算得到了伪距、伪距率，然后可以通过卡尔曼滤波实现紧组合导航计算。

上述实施方式并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的技术方案范围内所做出的变化、改型、添加或替换，也均属于本发明的保护范围。

Claims (3)

1.一种采用多普勒测速仪计算伪距/伪距率的紧组合方法，其特征在于：该方法分为以下步骤：

步骤1、将捷联惯性导航系统、激光多普勒测速仪、北斗卫星导航系统安装到运动载体上，形成运动载体的组合导航系统，组合导航系统上电启动；

步骤2、组合导航系统上电启动后，将北斗卫星导航系统接收到的运动载体初始位置的经度、维度、高度作为捷联惯性导航系统的初始位置参数，将初始位置参数装订至捷联惯性导航系统的导航计算机中；

步骤3、运动载体保持静止5分钟，根据装订的初始位置参数进行初始对准；得到最终运动载体的初始姿态角；

步骤4、完成初始对准后，组合导航系统进入紧组合模式，启动运动载体；

步骤5、由北斗卫星导航系统采集的北斗卫星位置、捷联惯性导航系统与激光多普勒测速仪解算的位置进行计算得到伪距、伪距率值；

然后根据得到的伪距、伪距率值与北斗卫星导航系统输出的伪距、伪距率值之差作为组合导航系统的观测量，进行紧组合导航。

2.根据权利要求1所述的采用多普勒测速仪计算伪距/伪距率的紧组合方法，其特征在于：所述步骤3初始对准的具体方式为：组合导航系统对陀螺和加速度计采集到的数据进行处理，然后根据捷联捷联惯性导航系统误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成组合导航系统的粗对准，初步确定运动载体的姿态角；其中粗对准时间为2分钟；粗对准完成后，再利用卡尔曼滤波技术对精对准3分钟，得到最终运动载体的初始姿态角。

3.根据权利要求1所述的采用多普勒测速仪计算伪距/伪距率的紧组合方法，其特征在于：所述步骤5中得到的伪距、伪距率值，是采用捷联惯性导航系统输出的姿态信息与激光多普勒测速仪输出的速度信息解算出运动载体的位置；进行解算时需要将激光多普勒测速仪在运动载体坐标系下的速度转换到导航坐标系下，具体方法为：

将激光测速仪在运动载体坐标系下的速度定义为在导航坐标系下的速度定义为捷联惯性导航系统输出的姿态矩阵为运动载体坐标系下的速度转换到导航坐标系下的速度关系如下：

根据导航坐标系下的激光多普勒测速仪速度和航位推算位置计算公式可以得到运动载体的位置：

(2)式中，λ、L、h分别为计算所得经度、纬度、高度的值，λ₀、L₀、h₀分别为经度、纬度、高度的初值，R_M、R_N分别为子午圈曲率半径和卯酉圈曲率半径；要计算伪距、伪距率需要将经度、纬度、高度转换成空间直角坐标系，计算公式如下：

x＝(R_N+h)cosLcosλ

y＝(R_N+h)cosLsinλ

z＝[R_N(1-e²)+h]sinL (3)

设由北斗卫星星历确定的空间直角坐标系下北斗卫星位置为(x_s y_s z_s)^Τ，则可以计算得到相应于运动载体坐标系统所在位置的伪距ρ_I如下：

ρ_I＝[(x-x_s)²+(y-y_s)²+(z-z_s)²]^1/2 (4)

运动载体与北斗卫星之间的伪距变化率简称伪距率；其中，ρ_B，分别为北斗卫星导航系统接收到的伪距和伪距率；

当捷联惯性导航系统与北斗卫星导航系统组成的子系统选择伪距、伪距率进行组合时，系统的观测量包含两种：一种是伪距量测差值，由激光多普勒测速仪与捷联惯性导航系统形成的子系统解算出解算出的伪距值与北斗卫星导航系统给出的相应伪距值作差作为一种量测值；另一种是伪距率量测差值；将运动载体与北斗卫星间伪距的变化率表示成伪距率；伪距率量测差值由激光多普勒测速仪与捷联惯性导航系统形成的子系统的伪距变化率与北斗卫星导航系统给出的相应伪距率值作差得到；

建立组合导航系统状态误差模型如下：

(5)式中，X(t)为20维系统误差状态向量；W(t)为系统白噪声阵；F(t)为系统状态矩阵；

(6)式中φ_E,φ_N,φ_U为姿态误差，ε_x,ε_y,ε_z为陀螺漂移，为加速度计零偏，δL,δλ,δh分别为纬度、经度、高度误差，δv_E,δv_N,δv_U为东、北、天速度误差，δK_D激光测速仪标度误差，δα_ψ,δα_θ分别为激光测速仪航向安装角、俯仰安装角误差；δt_u为时钟误差等效的距离误差，δt_ru为时钟频率误差等效的距离率误差；

组合导航系统采用伪距、伪距率的组合方式，以激光多普勒测速仪、捷联惯性导航系统与北斗卫星导航系统两者伪距之差距之差和伪距率之差同时作为系统观测量；

组合导航系统中，ρ_B，分别为北斗卫星导航系统到北斗卫星的伪距和伪距率，可直接由北斗卫星导航系统给出；ρ_I，分别为激光多普勒测速仪/捷联惯性导航系统到北斗卫星的伪距和伪距率；

设组合导航系统的量测方程表示为：

Ζ(t)＝H(t)X(t)+V(t) (8)

(8)式中，Ζ(t)为系统观测量，H(t)为系统量测矩阵，V(t)为量测噪声阵。