[发明专利]一种基于USBL软件接收机的SINS/USBL深组合导航定位方法

权利要求书

1.一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，该方法包括以下步骤：

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

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

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

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

所述各基元在声学基阵坐标系下的位置为：

其中：i为基元序号，i＝1,2,3,4，将基元1与3的间距表示为r₁，将基元2与4的间距表示为r₂，令r₁＝r₂＝r；为各基元在声学基阵坐标系的x_a轴方向上的位置，为各基元在声学基阵坐标系的y_a轴方向上的位置，为各基元在声学基阵坐标系的z_a轴方向上的位置，上角标T代表转置；

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

所述步骤三的具体过程为：

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

建立载体坐标系o_bx_by_bz_b，载体坐标系的坐标原点o_b位于水下航行器的质心，坐标轴x_b的正方向沿水下航行器的横轴指向右，坐标轴y_b的正方向沿水下航行器的纵轴指向前，坐标轴z_b的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；

声学基阵坐标系与载体坐标系三个坐标轴的角度安装偏差分别为α、β和γ，声学基阵坐标系原点相对于载体坐标系原点的位置偏差为：ΔX_b、ΔY_b和ΔZ_b为中的分量；

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

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

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

SINS计算的导航坐标系n'与真实导航坐标系n的三个坐标轴存在失准角误差φ＝[φ_xφ_y φ_z]^T，SINS计算的导航坐标系n'到真实导航坐标系n的转换矩阵为：

其中，I为单位矩阵；

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

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

记

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

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

所述步骤四的具体过程为：

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

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

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

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

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

所述各个基元相对于信标在径向方向的运动速度大小为：

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

其中，记中间变量cosζ_x、cosζ_y和cosζ_z为：

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

所述步骤六的具体过程为：

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

式中，f_aid为SINS辅助频率，c为有效声速，f₀为信标广播信号中心频率；

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

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

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

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

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

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

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

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

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

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