本发明属于紧耦合导航算法技术领域，具体涉及一种多应答器SINS/USBL组合导航算法。
背景技术：
捷联惯导系统(SINS)能够为水下载体提供完备导航信息，但其误差随时间积累；超短基线(USBL)定位系统操作简单、便携性强、成本较低，能够测量应答器相对于基阵的相对位置信息(包括角度信息和斜距信息)，可作为辅助导航系统，用来抑制惯导系统误差的发散。SINS/USBL组合导航能够集成二者的优点，实现互补。
现有USBL设备的普遍特点是测距精度高而测角精度相对较低。此外，常规的单应答器SINS/USBL组合导航系统使用前都要预先标定USBL到SINS的安装角，准备工作繁琐。国内外现有的单应答器SINS/USBL组合导航方案，其定位精度很大程度上受到USBL测角误差以及二者安装角误差的限制，不能满足部分水下高精度导航的需求。
因此，亟需研制一种SIN/SUSBL紧耦合算法，以解决SINS/USBL组合导航时存在的不足。
技术实现要素：
本发明要解决的技术问题是提供一种SINS/USBL紧耦合算法，以获得比现有单应答器SINS/USBL组合导航算法更高的定位精度。
为了实现这一目的，本发明采取的技术方案是：
一种SINSUSBL紧耦合算法，适用于多应答器SINS/USBL组合导航，包括 以下步骤：
(1)确定超短基线定位系统USBL原理
三应答器USBL的基本工作原理是以三个位置已知的应答器到基阵中心空间距离为半径的球面交会；i＝1,2,3表示三个应答器，其在地球直角坐标系中的位置为USBL测得的三个斜距值为Ri以及基阵位置为(Xe,Ye,Ze)，根据空间球面交会原理，得到如下的参数方程组：
式1
利用事先标定测量得到的应答器位置和USBL实时测量的斜距值，求解该方程组得到基阵的绝对位置；
(2)构建紧耦合导航算法滤波模型
选取惯导载体坐标系b为前上右坐标系，导航坐标系n为北天东坐标系；
选取传统15维惯导系统误差状态量，速度误差δVn＝[δVN δVU δVE]T，姿态误差φn＝[φN φU φE]T，位置误差陀螺漂移ε＝[εx εy εz]T，加表零位
相应捷联惯导系统SINS误差状态量为：
式2
捷联惯导系统误差状态方程为：
式3
将USBL测距精度误差扩展为系统状态：
XUSBL3＝[δKu1 δKu2 δKu3]T 式4
USBL测距精度误差δKui视为随机常数，i＝1、2、3，USBL部分对应的卡尔曼 滤波状态方程为：
式5
其中：
FUSBL3＝[03×3] 式6
18维的三应答器SINS/USBL紧耦合导航算法误差状态量为：
X＝[XSINS XUSBL3]T 式7
SINS/USBL组合导航的卡尔曼滤波状态方程为：
式8
其中，G为系统噪声矩阵，W为系统噪声，F为状态转移矩阵：
式9
公式(9)中，FSINS为惯导部分系统矩阵；
(3)构建紧耦合导航算法观测模型
以SINS相对三个应答器的距离信息和USBL直接测量得到的载体相对于三个应答器的斜距信息之差为观测量；
在地球直角坐标系中，利用惯导位置(Xs,Ys,Zs)和三个应答器位置构造相对距离为：
式10
对式10求偏微分：
式11
式12
应答器i的位置为惯导解算的位置为利用地球定位中球面坐标系到直角坐标系的转换公式，二者在地球直角坐标系的相对位置为
式13
其中，RN表示地球卯酉圈曲率半径，将式(13)化简得：
式14
联立式12和式14得：
式15
其中：
USBL实际测距表示USBL测量得到的惯导位置和第i个答器之间的距离，真实距离Ri表示实际的惯导位置和第i个答器之间的距离；USBL实际测距真实距离Ri和USBL测距精度误差δKui之间有如下关系：
式16
(4)确定三应答器SINS/USBL紧耦合导航算法的观测量为：
式17
三应答器SINS/USBL紧耦合导航算法观测方程为：
式18
三应答器SINS/USBL紧耦合导航算法观测矩阵为：
H＝[03×6 AsBs 03×6 diag[R1 R2 R3]] 式19
三应答器SINS/USBL紧耦合导航算法量测方程为：
Z＝HX+v 式20
(5)确定紧耦合导航算法滤波方程
对式(8)和式(20)进行离散化处理，得到SINS/USBL组合导航的卡尔曼滤波模型为：
其中k和k+1表示时刻。
进一步的，如上所述的一种SINSUSBL紧耦合算法，用于多应答器SINS/USBL组合导航，应答器数量大于3个时，i＝1,2,3…n，n＞3，采用步骤(1)～(5)的方法确定SINSUSBL紧耦合算法。
本发明技术方案的有益效果在于：通过采用标准卡尔曼滤波算法基本方程，选择合适的滤波初值(状态初始值，初始估计均方误差阵，系统噪声初始方差阵以及量测噪声方差阵)，进行滤波估计，并实时修正速度、姿态和位置信息，；设计了一种新型的多应答器SINS/USBL组合导航方案；并对多应答器SINS/USBL紧耦合导航算法进行误差建模与仿真分析。本发明能够避免USBL测角误差和USBL到SINS安装角误差等对定位精度带来的影响，且无需对USBL到SINS的安装角进行标定，使用更加方便。仿真结果表明，本发明能够获得比现有单应答器SINS/USBL组合导航算法更高的定位精度。
具体实施方式
下面结合具体实施例对本发明技术方案进行详细说明。
本发明一种SINSUSBL紧耦合算法，适用于多应答器SINS/USBL组合导航，包括以下步骤：
(1)确定超短基线定位系统USBL原理
三应答器USBL的基本工作原理是以三个位置已知的应答器到基阵中心空间距离为半径的球面交会；i＝1,2,3表示三个应答器，其在地球直角坐标系中的位置为USBL测得的三个斜距值为Ri以及基阵位置为(Xe,Ye,Ze)，根据空间球面交会原理，得到如下的参数方程组：
式(1)
利用事先标定测量得到的应答器位置和USBL实时测量的斜距值，求解该方程组得到基阵的绝对位置；
(2)构建紧耦合导航算法滤波模型
选取惯导载体坐标系b为前上右坐标系，导航坐标系n为北天东坐标系；
选取传统15维惯导系统误差状态量，速度误差δVn＝[δVN δVU δVE]T，姿态误差φn＝[φN φU φE]T，位置误差陀螺漂移ε＝[εx εy εz]T，加表零位
相应捷联惯导系统SINS误差状态量为：
式(2)
捷联惯导系统误差状态方程为：
式(3)
将USBL测距精度误差扩展为系统状态：
XUSBL3＝[δKu1 δKu2 δKu3]T 式(4)
USBL测距精度误差δKui视为随机常数，i＝1、2、3，USBL部分对应的卡尔曼滤波状态方程为：
式(5)
其中：
FUSBL3＝[03×3] 式(6)
18维的三应答器SINS/USBL紧耦合导航算法误差状态量为：
X＝[XSINS XUSBL3]T 式(7)
SINS/USBL组合导航的卡尔曼滤波状态方程为：
式(8)
其中，G为系统噪声矩阵，W为系统噪声，F为状态转移矩阵：
式(9)
公式(9)中，FSINS为惯导部分系统矩阵；
(3)构建紧耦合导航算法观测模型
以SINS相对三个应答器的距离信息和USBL直接测量得到的载体相对于三个应答器的斜距信息之差为观测量；
在地球直角坐标系中，利用惯导位置(Xs,Ys,Zs)和三个应答器位置构造相对距离为：
式(10)
对式(10)求偏微分：
式(11)
式(12)
应答器i的位置为惯导解算的位置为利用地球定位中球面坐标系到直角坐标系的转换公式，二者在地球直角坐标系的相对位置为
式(13)
其中，RN表示地球卯酉圈曲率半径，将式(13)化简得：
式(14)
联立式(12)和式(14)得：
式(15)
其中：
USBL实际测距表示USBL测量得到的惯导位置和第i个答器之间的距离，真实距离Ri表示实际的惯导位置和第i个答器之间的距离；USBL实际测距真实距离Ri和USBL测距精度误差δKui之间有如下关系：
式(16)
(4)确定三应答器SINS/USBL紧耦合导航算法的观测量为：
式(17)
三应答器SINS/USBL紧耦合导航算法观测方程为：
式(18)
三应答器SINS/USBL紧耦合导航算法观测矩阵为：
H＝[03×6 AsBs 03×6 diag[R1 R2 R3]] 式(19)
三应答器SINS/USBL紧耦合导航算法量测方程为：
Z＝HX+v 式(20)
(5)确定紧耦合导航算法滤波方程
对式(8)和式(20)进行离散化处理，得到SINS/USBL组合导航的卡尔曼滤波模型为：
其中k和k+1表示时刻。
本实施例中所述的一种SINSUSBL紧耦合算法，应答器数量为3个时，当应答器数量大于3个时，i＝1,2,3…n，n＞3，采用步骤(1)～(5)的方法确定SINSUSBL紧耦合算法同样适用。