一种基于USBL软件接收机的SINS/USBL深组合导航定位方法与流程

本发明属于组合导航及水声定位技术领域，具体涉及一种基于usbl(超短基线定位系统)软件接收机的sins(捷联惯导系统)/usbl深组合导航定位方法。

背景技术：

传统的sins/usbl组合导航建立在对sins和usbl位置输出信息或时延、时延差(斜距、斜距差)测量信息进行数据融合的基础上，通过建立sins误差方程和usbl定位误差方程或测距测向误差方程，实现对sins导航误差的修正，在usbl系统外部对定位结果或测距测向结果进行补偿，而忽略了利用sins动态信息来深入辅助usbl系统内部的信号跟踪环节。当usbl测量信息中断或者发生错误时，传统的组合方式都只能提供有限的容错能力，从而影响组合系统的性能。

技术实现要素：

本发明的目的是为解决当usbl测量信息中断或者发生错误时，传统组合方式的容错能力有限的问题，而提出了一种基于usbl软件接收机的sins/usbl深组合导航定位方法。

本发明为解决上述技术问题采取的技术方案是：一种基于usbl软件接收机的sins/usbl深组合导航定位方法，该方法包括以下步骤：

步骤一、将usbl的声学基阵倒置安装在水下航行器上，sins的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标；信标在导航坐标系下的位置通过gps获得；

所述导航坐标系是指“东北天”地理坐标系，“东北天”地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；

步骤二、建立声学基阵坐标系，并确定各基元在声学基阵坐标系下的位置；

所述声学基阵坐标系是指以usbl的声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于声学基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；

步骤三、根据水下航行器在sins计算的导航坐标系下的姿态信息，将基元在声学基阵坐标系下的位置转换到sins计算的导航坐标系下；

步骤四、根据基元在sins计算导航坐标系下的位置，计算声信号在各基元与信标间的传播时延以及传播时延差的预测值；

步骤五、将基元在sins计算导航坐标系下的速度沿径向分解，得到径向速度的预测值，即得到各个基元相对于信标在径向方向的运动速度大小；

步骤六、将步骤四计算出的传播时延、传播时延差的预测值以及步骤五计算出的径向速度的预测值输入至usbl软件接收机的信号跟踪环路，辅助usbl软件接收机对载波频率和码相位的跟踪，得到声信号在各基元与信标间的传播时延、传播时延差的测量值以及径向速度的测量值；

步骤七、建立以传播时延的预测值与测量值之差、传播时延差的测量值与预测值之差以及径向速度的预测值与测量值之差为观测量，以sins误差以及sins对usbl的辅助误差为状态变量的扩展卡尔曼滤波系统；

步骤八、根据扩展卡尔曼滤波系统输出的状态估计值修正sins输出，并重置扩展卡尔曼滤波系统输出的状态；

重置扩展卡尔曼滤波器的状态后，再重复步骤三至步骤八的过程，不断的对sins的输出进行修正。

本发明的有益效果是：本发明提出了一种基于usbl软件接收机的sins/usbl深组合导航定位方法，在不需要先验信息的情况下，本发明利用sins的动态信息辅助无源工作模式下的usbl载波跟踪环路和码跟踪环路，获得准确径向速度信息和时延信息，提高了usbl在干扰信号或者是弱信号下的系统容错能力，提高了组合系统的性能。

附图说明

图1为sins/usbl深组合导航系统结构图；

图2为sins辅助usbl载波跟踪环路原理图；

图3为sins辅助usbl码跟踪环路原理图；

图中，δτ是接收信号码相位对应的时延测量τ与本地参考信号码相位对应时延参考的时延差；

图4为声学基阵坐标系、载体坐标系和导航坐标系的转换原理图。

具体实施方式

具体实施方式一：如图1和图4所示，本实施方式所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法，该方法包括以下步骤：

步骤一、将usbl的声学基阵倒置安装在水下航行器上，sins的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标；信标在导航坐标系下的位置通过gps获得；

所述导航坐标系是指“东北天”(“enu”)地理坐标系，“东北天”地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；

步骤二、建立声学基阵坐标系，并确定各基元在声学基阵坐标系下的位置；

所述声学基阵坐标系是指以usbl的声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于声学基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；

步骤三、根据水下航行器在sins计算的导航坐标系下的姿态信息，将基元在声学基阵坐标系下的位置转换到sins计算的导航坐标系下；

步骤四、根据基元在sins计算导航坐标系下的位置，计算声信号在各基元与信标间的传播时延以及传播时延差的预测值；

步骤五、将基元在sins计算导航坐标系下的速度沿径向分解，得到径向速度的预测值，即得到各个基元相对于信标在径向方向的运动速度大小；

步骤六、将步骤四计算出的传播时延、传播时延差的预测值以及步骤五计算出的径向速度的预测值输入至usbl软件接收机的信号跟踪环路，辅助usbl软件接收机对载波频率和码相位的跟踪，得到声信号在各基元与信标间的传播时延、传播时延差的测量值以及径向速度的测量值；

步骤七、建立以传播时延的预测值与测量值之差、传播时延差的测量值与预测值之差以及径向速度的预测值与测量值之差为观测量，以sins误差以及sins对usbl的辅助误差为状态变量的扩展卡尔曼滤波系统；

步骤八、根据扩展卡尔曼滤波系统输出的状态估计值修正sins输出，并重置扩展卡尔曼滤波系统输出的状态；

重置扩展卡尔曼滤波器的状态后，再重复步骤三至步骤八的过程，不断的对sins的输出进行修正。

本发明中的导航坐标系包括sins计算的导航坐标系和真实导航坐标系(即步骤一建立的导航坐标系)，任何未经限定的导航坐标系均是指真实导航坐标系。sins计算的导航坐标系是指：根据sins计算的水下航行器姿态而复现的导航坐标系。

本实施方式中的声学基阵是由多个声信号接收换能器(基元)均匀分布的一个圆柱形设备。

在本发明的整个组合导航系统中，只有水声信号预处理和惯性器件是硬件部分，其他部分都由软件实现，能够将软件接收机的灵活性与sins辅助usbl的优势相结合，不仅便于系统的实现和测试，也为系统性能的仿真验证提供了便利。

本发明在usbl软件接收机的基础上，利用sins提供的动态信息辅助载波跟踪环路，可以降低多普勒的动态范围，减小跟踪环路的带宽，达到抑制噪声和信号干扰的目的；sins的动态辅助信息进入码跟踪环路能够提高对多路径效应的抑制作用和校正能力。在不需要先验信息的情况下，sins的动态信息能够辅助无源工作模式下的usbl码跟踪环路，获得准确时延信息，提高了usbl在干扰信号或者是弱信号下的系统容错能力。sins为usbl接收机提供的动态辅助信息能够增强usbl在远距离作业、高动态、强干扰和信号中断等恶劣环境中的鲁棒性，同时提高sins/usbl组合导航性能。

利用sins的动态信息辅助usbl水声定位系统对接收信号的跟踪，提高usbl定位系统的时延测量和径向速度测量精度，扩大usbl的有效作用范围。

具体实施方式二：如图2所示。本实施方式与具体实施方式一不同的是：所述步骤二中，各基元在声学基阵坐标系下的位置为：

其中：i为基元序号，i＝1,2,3,4，r为基元1与3的间距或基元2与4的间距(基元1与3的间距同基元2与4的间距相等)；为各基元在声学基阵坐标系的xa轴方向上的位置，为各基元在声学基阵坐标系的ya轴方向上的位置，为各基元在声学基阵坐标系的za轴方向上的位置，上角标t代表转置。

具体实施方式三：本实施方式与具体实施方式二不同的是：所述步骤三的具体过程为：

水下航行器在sins计算的导航坐标系下的姿态信息包括航向角a、俯仰角k和横滚角ψ；

建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；

声学基阵坐标系与载体坐标系三个坐标轴的角度安装偏差分别为α、β和γ，声学基阵坐标系原点相对于载体坐标系原点的位置偏差为：(δxb、δyb和δzb均为载体坐标系下分量)，δxb、δyb和δzb为中的分量；

载体坐标系到计算导航坐标系的转换矩阵和声学基阵坐标系到载体坐标系的转换矩阵分别为：

则第i个基元在sins计算的导航坐标系n′下的位置表示为

其中，是sins计算的水下航行器的位置在计算导航坐标系中的直角坐标形式，为载体坐标系到sins计算的导航坐标系n'的转换矩阵；

sins计算的导航坐标系n'与真实导航坐标系n的三个坐标轴存在失准角误差φ＝[φxφyφz]^t，sins计算的导航坐标系n'到真实导航坐标系n的转换矩阵可近似为：

其中，i为单位矩阵；

第i个基元在sins计算的导航坐标系下的位置与在真实导航坐标系(建立的导航坐标系)下位置之间的误差为

式中，δxi是与水下航行器在真实导航坐标系下位置的差；

记

是在计算导航坐标系中第i个基元相对水下航行器质心的位置，是沿计算导航坐标系xn′轴、yn′轴和zn′轴分量。

具体实施方式四：本实施方式与具体实施方式三不同的是：所述步骤四的具体过程为：

声信号在第i个基元与信标之间的传播时延预测值为：

式中，c是测量的有效声速，是信标在真实导航坐标系中的位置，由gps提供的位置信息转换得到，||·||是矩阵二范数的表示；

其中，为信标在真实导航坐标系的xn轴方向位置，为信标在真实导航坐标系的yn轴方向位置，为信标在真实导航坐标系的zn轴方向位置，为第i个基元在真实导航坐标系的xn轴方向位置，为第i个基元在真实导航坐标系的yn轴方向位置，为第i个基元在真实导航坐标系的zn轴方向位置；

声信号在基元3和基元1之间传播时延差的预测值为声信号在基元4和基元2之间传播时延差的预测值为

所述有效声速为信标和接收基阵的几何距离与声信号在两点间的传播时间的比值。

具体实施方式五：本实施方式与具体实施方式四不同的是：所述步骤五中，各个基元相对于信标在径向方向的运动速度大小为：

其中：为第i个基元相对于信标在径向方向的运动速度大小，为第i个基元在计算导航坐标系的xn′轴方向位置，为第i个基元在计算导航坐标系的yn′轴方向位置，为第i个基元在计算导航坐标系的zn′轴方向位置，为水下航行器在计算导航坐标系的xn′轴方向速度，为水下航行器在计算导航坐标系的yn′轴方向速度，为水下航行器在计算导航坐标系的zn′轴方向速度；

其中，记中间变量cosζx、cosζy和cosζz为：

具体实施方式六：结合图2说明本实施方式。本实施方式与具体实施方式五不同的是：所述步骤六的具体过程为：

sins对应usbl软件接收机第i个基元接收通道的载波跟踪环路动态辅助信息为：

式中，faid为sins辅助频率，c为有效声速，f0为信标广播信号中心频率；

结合图2，sins辅助下的usbl软件接收机第i个基元接收通道的载波环跟踪相位误差的导数和频率误差δf的导数表示为：

其中，是接收信号相位与本地参考信号相位的相位误差，为相位误差的导数，为低通滤波器输出的频率误差δf的导数，p和q是与具体跟踪环路有关的参数；δfp为δf与sins辅助频率faid误差δfaid之和，为δfp的导数，为δfaid的导数；在sins动态辅助usbl接收机时只需考虑一个基元接收通道的载波环跟踪过程。

结合图3，sins对应的usbl软件接收机第i个基元接收通道的码跟踪环路动态辅助信息为

其中，为sins的径向速度辅助信息，为sins的时延辅助信息；

结合图3，sins辅助下的usbl软件接收机第i个基元接收通道的码跟踪环路时延误差的导数表示为：

式中，为的导数，为sins的径向速度辅助误差，为的导数，qi是环路热噪声；

根据δf、faid和δτ，得到usbl软件接收机第i个基元接收通道的声信号在对应基元和信标间的传播时延测量值为传播时延差测量值为和径向速度测量值为

具体实施方式七：本实施方式与具体实施方式六不同的是：所述步骤七的具体过程为：

失准角误差φ为：φ＝[φxφyφz]^t，sins速度误差δν为：δν＝[δvxδvyδvz]^t(速度误差是由sins解算的计算导航坐标系下的水下航行器运动速度与真实导航坐标系下的速度真值的差)，sins位置误差δp为：δp＝[δlδλδh]^t(位置误差是sins解算的水下航行器在地球坐标系下的位置与真实位置的差值)，陀螺漂移误差ε为：ε＝[εxεyεz]^t，加速度计偏置误差为：其中，φx、φy和φz为φ中的分量；

则

其中，x为扩展卡尔曼滤波系统的状态变量；

扩展卡尔曼滤波系统的状态方程为

xk+1＝fk+1/kxk+wk+1(21)

其中，xk为k时刻的状态变量，xk+1为k+1时刻的状态变量，fk+1/k为状态转移矩阵，由捷联惯性导航系统的误差方程以及式(14)、式(15)、式(16)和式(19)获得，wk+1为扩展卡尔曼滤波系统过程噪声序列，通常为高斯白噪声形式；

扩展卡尔曼滤波系统的观测量z为：

观测方程为

zk+1＝hk+1xk+1+vk+1(23)

其中，zk+1为k+1时刻的观测量，vk+1为扩展卡尔曼滤波系统观测噪声序列，通常为高斯白噪声形式，hk+1为k+1时刻的观测矩阵。

sins解算的水下航行器位置表示为纬度l、经度λ和高度h的球坐标形式，再将纬度l、经度λ和高度h转换到地球直角坐标系下；

地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则

式中，x′e、y′e和z′e分别为sins解算的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，rn为地球卯酉圈主曲率半径，re为地球半径，e为地球偏心率，a和b分别是椭圆长轴和短轴半径；

上式的微分转换关系为：

地球直角坐标系到sins计算导航坐标系的转换矩阵为：

记中间变量矩阵a为：

结合式(7)和式(8)，得到式(9)的全微分形式：

式中，[dxn′dyn′dzn′]^t是sins计算的水下航行器位置与真实位置在三个轴方向的位置误差；是中的坐标分量。

记

式(12)的全微分为

式中，是sins计算的水下航行器在导航坐标系下三个方向的速度误差。

civv＝[cosζxcosζycosζz](32)

观测矩阵hk+1为

式(33)中，c1tt＝1000，c31tt＝-1010。

具体实施方式八：本实施方式与具体实施方式七不同的是：所述步骤八中，根据扩展卡尔曼滤波系统输出的状态估计值修正sins输出，并重置扩展卡尔曼滤波系统输出的状态，其具体过程为：

根据计算k+1时刻的状态预测值其中为k时刻的状态估计值；

根据pk+1/k＝fk+1/kpkf^tk+1/k+qk计算k+1时刻的状态预测误差协方差矩阵pk+1/k，其中pk为k时刻的状态估计误差协方差矩阵，qk为k时刻的系统过程噪声协方差矩阵；

根据kk+1＝pkh^tk+1(hk+1pkh^tk+1+rk+1)^-1计算k+1时刻的滤波器增益kk+1，其中rk+1和hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵；上角标-1代表矩阵的逆；

根据pk+1＝(i-kk+1hk+1)pk计算k+1时刻的状态估计误差协方差矩阵pk+1；

根据计算k+1时刻的状态估计值

根据修正sins的输出，并重置扩展卡尔曼滤波系统输出的状态。

所述扩展卡尔滤波系统是指在最小均方误差准则下，将非线性的状态方程和观测方程进行线性近似处理，对状态变量进行估计。

所述重置滤波状态变量是指，在修正捷联惯性导航输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。

本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。