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的间距;
3.根据权利要求2所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤三的具体过程为:
水下航行器在sins计算的导航坐标系下的姿态信息包括航向角a、俯仰角k和横滚角ψ;
建立载体坐标系obxbybzb,载体坐标系的坐标原点ob位于水下航行器的质心,坐标轴xb的正方向沿水下航行器的横轴指向右,坐标轴yb的正方向沿水下航行器的纵轴指向前,坐标轴zb的正方向沿水下航行器的立轴指向上,载体坐标系的定义满足右手定则;
声学基阵坐标系与载体坐标系三个坐标轴的角度安装偏差分别为α、β和γ,声学基阵坐标系原点相对于载体坐标系原点的位置偏差
载体坐标系到计算导航坐标系的转换矩阵
则第i个基元在sins计算的导航坐标系n′下的位置
其中,
sins计算的导航坐标系n'与真实导航坐标系n的三个坐标轴存在失准角误差φ=[φxφyφz]t,sins计算的导航坐标系n'到真实导航坐标系n的转换矩阵
其中,i为单位矩阵;
第i个基元在sins计算的导航坐标系下的位置
式中,δxi是
记
4.根据权利要求3所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤四的具体过程为:
声信号在第i个基元与信标之间的传播时延预测值
式中,c是测量的有效声速,
其中,
声信号在基元3和基元1之间传播时延差的预测值为
5.根据权利要求4所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤五中,各个基元相对于信标在径向方向的运动速度大小为:
其中:
其中,记中间变量cosζx、cosζy和cosζz为:
6.根据权利要求5所述的一种基于usbl软件接收机的sins/usbl深组合导航定位方法,其特征在于,所述步骤六的具体过程为:
sins对应usbl软件接收机第i个基元接收通道的载波跟踪环路动态辅助信息为:
式中,faid为sins辅助频率,c为有效声速,f0为信标广播信号中心频率;
sins辅助下的usbl软件接收机第i个基元接收通道的载波环跟踪相位误差
其中,
sins对应的usbl软件接收机第i个基元接收通道的码跟踪环路动态辅助信息为
其中,
sins辅助下的usbl软件接收机第i个基元接收通道的码跟踪环路时延误差
式中,
根据δf、
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为扩展卡尔曼滤波系统的状态变量;
扩展卡尔曼滤波系统的状态方程为
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输出,并重置扩展卡尔曼滤波系统输出的状态,其具体过程为:
根据
根据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;
根据
根据