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

文档序号:21400786发布日期:2020-07-07 14:33阅读:来源:国知局

技术特征:

1.一种基于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的输出进行修正。

2.根据权利要求1所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤二中,各基元在声学基阵坐标系下的位置为:

其中:i为基元序号,i=1,2,3,4,r为基元1与3的间距或基元2与4的间距;为各基元在声学基阵坐标系的xa轴方向上的位置,为各基元在声学基阵坐标系的ya轴方向上的位置,为各基元在声学基阵坐标系的za轴方向上的位置,上角标t代表转置。

3.根据权利要求2所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤三的具体过程为:

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

建立载体坐标系obxbybzb,载体坐标系的坐标原点ob位于水下航行器的质心,坐标轴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′轴分量。

4.根据权利要求3所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤四的具体过程为:

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

式中,c是测量的有效声速,是信标在真实导航坐标系中的位置,||·||是矩阵二范数的表示;

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

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

5.根据权利要求4所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤五中,各个基元相对于信标在径向方向的运动速度大小为:

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

其中,记中间变量cosζx、cosζy和cosζz为:

6.根据权利要求5所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤六的具体过程为:

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

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

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

其中,是接收信号相位与本地参考信号相位的相位误差,为相位误差的导数,为低通滤波器输出的频率误差δf的导数,p和q是与具体跟踪环路有关的参数;δfp为δf与sins辅助频率faid误差δfaid之和,为δfp的导数,为δfaid的导数;

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

其中,为sins的径向速度辅助信息,为sins的时延辅助信息;

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

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

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

7.根据权利要求6所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤七的具体过程为:

失准角误差φ为:φ=[φxφyφz]t,sins速度误差δν为:δν=[δvxδvyδvz]t,sins位置误差δp为:δp=[δlδλδh]t,陀螺漂移误差ε为:ε=[ε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为状态转移矩阵,wk+1为扩展卡尔曼滤波系统过程噪声序列;

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

观测方程为

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

其中,zk+1为k+1时刻的观测量,vk+1为扩展卡尔曼滤波系统观测噪声序列,hk+1为k+1时刻的观测矩阵。

8.根据权利要求7所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤八中,根据扩展卡尔曼滤波系统输出的状态估计值修正sins输出,并重置扩展卡尔曼滤波系统输出的状态,其具体过程为:

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

根据pk+1/k=fk+1/kpkftk+1/k+qk计算k+1时刻的状态预测误差协方差矩阵pk+1/k,其中pk为k时刻的状态估计误差协方差矩阵,qk为k时刻的系统过程噪声协方差矩阵;

根据kk+1=pkhtk+1(hk+1pkhtk+1+rk+1)-1计算k+1时刻的滤波器增益kk+1,其中rk+1和hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵;

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

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

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


技术总结
一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,它属于组合导航及水声定位技术领域。本发明解决了当USBL测量信息中断或者发生错误时,传统组合方式的容错能力有限的问题。在不需要先验信息的情况下,本发明利用SINS的动态信息辅助无源工作模式下的USBL载波跟踪环路和码跟踪环路,获得准确径向速度信息和时延信息,提高了USBL在干扰信号或者是弱信号下的系统容错能力,提高了组合系统的性能。本发明可以应用于组合导航及水声定位领域。

技术研发人员:孙大军;郑翠娥;张居成;韩云峰;崔宏宇;张殿伦
受保护的技术使用者:哈尔滨工程大学
技术研发日:2020.03.04
技术公布日:2020.07.07
当前第2页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1