CN105486313A - 一种基于usbl辅助低成本sins***的定位方法 - Google Patents

Abstract

本发明提供了一种基于USBL辅助低成本SINS***的定位方法。本发明由捷联惯性导航***SINS和超短基线***USBL组成，利用Klaman滤波方法完成组合导航。超短基线***作为辅助导航***，由安装在AUV上的基阵和布放在海底的单应答器组成，超短基线基阵接收的信号进行广义加权二次相关后，再进行位置解算，USBL的位置解算结果和SINS位置输出进行Kalman滤波，滤波器的输出再反馈校正SINS。本发明解决了采用低成本SINS***长时间误差积累的问题，USBL采用广义加权二次相关提高了时延估计精度和抗噪声性能，避免了采用长基线***基阵布放、校准、作业复杂的问题，同时保证了水下高精度定位与导航。

Description

一种基于USBL辅助低成本SINS***的定位方法

技术领域

本发明涉及一种基于USBL辅助低成本SINS***的定位方法，属于水下导航技术领域。

背景技术

在21世纪，人类将越来越多地向海洋获取物质资源和生存空间，水下航行器作为一种可在水下移动、具有视觉和感知***、通过遥控或自主操作方式的小型载体可以代替或辅助人去完成某些水下作业勘探和侦察任务。水下航行器在配备探测、监视和环境传感用的成套声学和非声学传感器以及水下高速火箭、防御武器等时，可用于潜艇战、水雷战、侦察、监视和水下攻击等各方面。由于水下航行器的活动范围广、体积小、重量轻、噪音低、隐蔽性高，现已成为各发达国家军事海洋技术研究的一个重要方向。

水下高精度导航和定位技术是解决水下作业的前提和关键，为保证自主水下航行器(AUV，AutonomousUnderwaterVehicle)能够顺利的完成水下的相关任务，要求AUV导航***具有水下长期自主的导航定位和返航能力。目前已有的定位技术中，惯性导航***(INS)因其隐蔽性强和自主性在水下航行器中得到广泛应用，虽然惯导技术本身已日渐成熟，但其误差会随着时间积累，因此，伴随惯导技术发展的同时，世界各国都一直在探索惯导***的校准方法。LBL(LongBaseLine，长基线)水声定位***通常是由基线长度为几千米的海底应答器阵和被定为载体上的问答机组成，利用水下目标与海底阵元之间的距离信息来求解目标位置，但是水下布放、回收、校准基阵比较麻烦，且数据更新频率较低，作业复杂。超短基线只有一个尺寸很小的基阵安装在载体上，无需布放基阵，使用比较灵活和方便。

对超短基线声学定位***研究较早的是挪威的KongsbergSimrad公司，该公司于1997年推出了世界领先水平的高精度长程超短基线定位***—HiPAP35O，作用距离可达3000米，距离测量精度优于20cm，随后推出HiPAP500，作用水深达4000m，测距精度优于20cm；新近推出的HiPAPP700，作用水深达10000米，测距精度优于50cm，也是世界上唯一工作水深上万米的长程超短基线定位***。另外，法国OCEANOTechnologies公司推出的Posidonia6000长程超短基线定位***，工作水深6000m，最大作用距离8000m，在6000m水深300度开角范围内，测距精度为0.5％，询问频率为8-14kHZ，应答频率为14-18kHz，该***已经成功推向市场。

由于国内对AUV技术开始研究的时间较晚，与欧美等发达国家相比，在很多方面都存在着不足。但随着不断的投入也取得了很大的进展。国内具有代表性的水声定位***是哈尔冰工程大学水声工程学院研制出了四种基于声学定位的***：深水重潜装潜水员超短基线定位***、“探索者”号水下机器人超短基线定位***，灭雷具配套水声跟踪定位装置、长程超短基线定位***。此外，范欣等提出了一种基于多传感器信息融合的水下航行器组合导航方法，建立了基于SINS\DVL\LBL\深度计\航向传感器的误差模型，推导了多传感器信息融合模型，仿真结果表明该方法提高了远程自主水下航行器的导航精度；章大勇等提出了一种基于SINS/PLBL的AUV组合导航算法研究，分析了不同运动状态下的可观测性条件，为后续的研究提供了依据。

采用USBL超短基线定位***，利用宽带信号进行定位时，高精度时延差的获取是水下定位精度的关键因素，但是水下环境复杂，有海洋环境噪声、舰船辐射噪声、海洋混响等因素影响，使得采用互相关函数的相关峰有模糊峰现象，使得时延差难以准确获取，造成水下定位误差。

发明内容

本发明的技术解决问题是：针对现有技术的不足，提出一种基于USBL辅助低成本SINS***的定位方法。

本发明的技术解决方案为：本发明提出的一种基于USBL辅助低成本SINS***的定位方法，其特征在于，由捷联惯性导航***SINS和超短基线***USBL组成，利用Klaman滤波方法完成组合导航:

(1)超短基线由安装在AUV上的基阵和布放在海底的单应答器组成，基阵用于接收单应答器的信号，接收信号两两进行广义加权二次相关，得到时延差；

(2)根据得到的时延差，进行USBL位置结算，得到AUV的位置P_USBL；

(3)捷联惯性导航***SINS通过捷联结算得到AUV的位置信息P_SINS；

(4)USBL位置解算结果和SINS位置输出结果进行Kalman滤波，滤波输出校正SINS位置输出。

本发明还包括以下特征：

1.广义加权二次相关获取时延差通过以下具体步骤实现：

(1)海底单应答器的应答信号为x(t),第i个水听器接收到的信号为x_i(t)＝α_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j(t)＝α_jx(t-τ_j)+n_j(t)，其中α_i、α_j为声信号在水中传播的衰减系数，n_i(t)、n_j(t)为噪声信号，τ_i、τ_j为传播时间；

(2)x_i(t)的自相关函数为x_i(t)与x_j(t)的互相关函数为其中τ＝τ_j-τ_i，表示到达时间差，T表示观测时间；

(3)R_ii(τ)和R_ij(τ)看成新的信号函数，再次进行广义互相关，新的互相关函数 $R\left(\mathrm{\τ}\right)=\frac{1}{T-\mathrm{\τ}}{\∫}_{\mathrm{\τ}}^T{R}_{ii}\left(t\right)R_{ij}(t-\mathrm{\τ})dt;$

(4)根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：G₁₂(ω)是R_ii(t)和R_ij(t)之间的互功率谱；

(5)为了抑制噪声的影响，选择频域加权函数对信号进行滤波，在傅里叶逆变换到时域得到广义互相关函数，即：

(6)从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时延差τ。

2.根据时延差，进行USBL位置解算通过以下具体步骤实现：

(1)声学基阵由互相垂直安装在基阵坐标X轴和Y轴上的4个水听器组成，海底单应答器地理坐标已知；

(2)海底应答器在声学基阵坐标系(a系)下的位置为(x,y,z)； τ_x和τ_y为时延差，c为水中的声速，d为阵元间距，R为应答器到AUV的距离，T为往返时间；

(3)将求得的位置坐标(x,y,z)进行坐标变换，得到AUV相对于海底应答器的相对位置，在根据已知的海底应答器的坐标得到水下AUV的位置。

本发明与现有技术相比的优点在于：

与现有技术相比，本发明通过将水听器的接收信号先进行自相关和互相关，然后在进行广义互相关，在广义互相关的基础上添加了权重函数进行频域滤波，经过广义加权二次相关后，增强了信号中信噪比比较高的频率成分，抑制了噪声的影响，锐化了峰值，在低信噪比的情况下，依然能得到高精度时延差；USBL定位输出校正了SINS导航误差，弥补了SINS长时间航行误差积累的问题，避免了无线电定位***和长基线定位***的使用，保证了水下高精度导航的要求。

附图说明

图1为USBL辅助SINS定位示意图；

图2为USBL定位原理示意图；

图3为同轴两个基元声线平行定位原理示意图；

图4为广义加权二次相关时延估计原理图；

图5为USBL和SINS组合导航原理示意图

具体实施方式

本发明提出的一种基于USBL辅助低成本SINS***的定位方法，采用以下具体方式实现：

1.如图1所示，本发明由安装在AUV上的低成本捷联惯性******SINS和超短基线***USBL组成。超短基线***由安装在AUV上的声源、声学基阵和海底单应答器组成，已知单应答器的地理坐标为应答器发出的信号基阵原点的声线与X轴和Y轴的夹角分别为α和β，应答器与基阵的距离是T为往返时间，c为水中的声速，超短基线定位原理示意图如图2所示，由图2可知：

$\left\{\begin{array}{c}x=Rcos\mathrm{\α},y=Rcos\mathrm{\β}\\ z=\sqrt{R^2-x^2-y^2}\end{array}\right.$

由于超短基线***基阵间距离较小，可以认为应答器信号到所有基元的声线平行，如图3所示，有d为阵元间距，所以可得因此测得时延差和距离R可以得出应答器在声学基阵坐标系(a系)下的坐标

2.上述时延差根据广义加权二次相关获得，广义加权二次相关时延估计原理图如图4所示，具体实现方式如下：

(1)海底单应答器的应答信号为x(t),第i个水听器接收到的信号为x_i(t)＝α_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j(t)＝α_jx(t-τ_j)+n_j(t)，其中α_i、α_j为声信号在水中传播的衰减系数，n_i(t)、n_j(t)为噪声信号，τ_i、τ_j为传播时间；

(2)x_i(t)的自相关函数为x_i(t)与x_j(t)的互相关函数为其中τ＝τ_j-τ_i，表示到达时间差，T表示观测时间；

(3)R_ii(τ)和R_ij(τ)看成新的信号函数，再次进行广义互相关，新的互相关函数 $R\left(\mathrm{\τ}\right)=\frac{1}{T-\mathrm{\τ}}{\∫}_{\mathrm{\τ}}^T{R}_{ii}\left(t\right)R_{ij}(t-\mathrm{\τ})dt;$

(4)根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为： G₁₂(ω)是R_ii(t)和R_ij(t)之间的互功率谱；

(5)为了抑制噪声的影响，选择频域加权函数对信号进行滤波，在傅里叶逆变换到时域得到广义互相关函数，即：

(6)从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时延差τ；

3.已知应答器在地理坐标系(n系)下位置为通过坐标变换得到AUV在地理坐标系下位置的具体实现方式如下：

(1)设P_USBL为通过超短基线***测得的AUV在地理坐标系的位置，P_nt为在地理坐标系下AUV相对应答器的位置，则为应答器在地理坐标系(n系)下位置；

(2)设地理坐标系为n系，声学基阵坐标系为a系，载体坐标系为b系，根据时延差求得应答器在声学基阵坐标系(a系)下为位置为由声学基阵坐标系到载体坐标系的坐标转换可以得到应答器在载体坐标系下的位置 δp为基阵坐标系在载体坐标系的安装误差，为声学基阵坐标系到载体坐标系的变换矩阵；

(3)由载体坐标系到地理坐标系的坐标变换可以得到AUV相对应答器在大地坐标系的位置

(4)因此超短基线***测得AUV的位置为

4.捷联惯性导航***SINS通过捷联结算得到AUV的位置信息P_SINS，具体实现方式如下：

(1)计算姿态矩阵和姿态角

采用四元数法计算姿态矩阵，根据欧拉定理,动坐标系相对参考坐标系的方位等效于动坐标系绕某个等效转轴转动一个角度θ，如果用u表示等效转轴方向的单位矢量，则动坐标系的方位完全由u和θ两个参数来确定。

用u和θ可构造一个四元数：

$Q=cos\frac{\mathrm{\θ}}{2}+usin\frac{\mathrm{\θ}}{2}$

对上式求导并化简可得四元数微分方程：

$Q\left(\stackrel{\·}{q}\right)=\frac{1}{2}M*\left(\mathrm{\ω}b\right)Q\left(q\right)$

式中 $M^{*}\left({\mathrm{\ω}}_b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nb}^{bx}& -{\mathrm{\ω}}_{nb}^{by}& -{\mathrm{\ω}}_{nb}^{bz}\\ {\mathrm{\ω}}_{nb}^{bx}& 0& {\mathrm{\ω}}_{nb}^{bz}& -{\mathrm{\ω}}_{nb}^{by}\\ {\mathrm{\ω}}_{nb}^{by}& -{\mathrm{\ω}}_{nb}^{bz}& 0& {\mathrm{\ω}}_{nb}^{bx}\\ {\mathrm{\ω}}_{nb}^{bz}& {\mathrm{\ω}}_{nb}^{by}& -{\mathrm{\ω}}_{nb}^{bx}& 0\end{array}\right]$

根据毕卡逼近法求解四元数微分方程得：

$q\left(t\right)=\{cos\frac{{\mathrm{\Δ\θ}}_0}{2}I+\frac{sin\frac{{\mathrm{\Δ\θ}}_0}{2}}{{\mathrm{\Δ\θ}}_0}\[\mathrm{\Δ}\mathrm{\θ}\]\}q\left(0\right)$

式中

${\mathrm{\Δ\θ}}_0=\sqrt{{\mathrm{\Δ\θ}}_x^2+{\mathrm{\Δ\θ}}_y^2+{\mathrm{\Δ\θ}}_z^2}$

$\[\mathrm{\Δ}\mathrm{\θ}\]={\∫}_{t_1}^{t_1+h}{M}^{*}\left({\mathrm{\ω}}_{nb}^b\right)dt=\left[\begin{array}{cccc}0& -{\mathrm{\Δ\θ}}_x& -{\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z\\ {\mathrm{\Δ\θ}}_x& 0& {\mathrm{\Δ\θ}}_z& -{\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z& 0& {\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_x& 0\end{array}\right]$

式中

${\mathrm{\Δ\θ}}_i={\∫}_t^{t+h}{\mathrm{\ω}}_{nb}^{bi}dt,i=x,y,z.$

令地球坐标系相对惯性坐标系的自转角速度为ω_ie，(其值为15.04088°/h)，L表示当地纬度,λ表示当地经度，则

ω_ie ⁿ：地球坐标系相对惯性坐标系的自转角速度在地理坐标系中的矢量，为：

${\mathrm{\ω}}_{ie}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\cos L& {\mathrm{\ω}}_{ie}\sin L\end{array}\right]}^T$

ω_ie ^b：地球坐标系相对惯性坐标系的自转角速度在载体坐标系中的矢量，为：

${{\mathrm{\ω}}_{ie}}^b=C_n^b{{\mathrm{\ω}}_{ie}}^n$

式中的姿态矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时，姿态矩阵跟着变化，由四元数即时修正后求得(下同)。

ω_en ⁿ:地理坐标相对地球坐标系转动角速度在地理坐标系中的矢量，为：

${\mathrm{\ω}}_{en}^n={\left[\begin{array}{ccc}-V_N/R_N& V_E/R_E& V_E\tan L/R_E\end{array}\right]}^T$

V_E、V_N分别为载体运动的东向和北向速度；

R_N为参考椭球体子午面内的曲率半径，R_N＝R_e(1-2e+3esin²L)；

R_E为垂直子午面的法线平面内的曲率半径，R_E＝R_e(1+esin²L)；

其中R_e为参考椭球体的长轴半径；e为椭球的椭圆度。

又因为,则

${\mathrm{\ω}}_{en}^n=\left[\begin{array}{ccc}-\stackrel{\·}{L}& \stackrel{\·}{\mathrm{\λ}}\cos L& \stackrel{\·}{\mathrm{\λ}}\sin L\end{array}\right]$

ω_en ^b:地理坐标相对地球坐标系转动角速度在载体坐标系中的矢量，为：

${{\mathrm{\ω}}_{en}}^b=C_n^b{{\mathrm{\ω}}_{en}}^n$

ω_ib ^b：陀螺输出角速度，记为

${\mathrm{\ω}}_{ib}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{ib}^{bx}& {\mathrm{\ω}}_{ib}^{by}& {\mathrm{\ω}}_{ib}^{bz}\end{array}\right]}^T$

ω_nb ^b：载体坐标系相对地理坐标系的转动角速度在载体坐标系中的矢量，记为

${\mathrm{\ω}}_{nb}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{nb}^{bx}& {\mathrm{\ω}}_{nb}^{by}& {\mathrm{\ω}}_{nb}^{bz}\end{array}\right]}^T$

则可得

ω_nb ^b＝ω_ib ^b-ω_ie ^b-ω_en ^b

四元数即时修正后，根据下式可由四元数的元实时更新姿态矩阵

$C_n^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

从姿态阵中即可提取实时姿态角

(2)速度计算

得到的载体坐标系中的比力矢量为f^b，则地理坐标系中有：

$f^n=C_b^n{f}^b$

式中的方向余弦矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时，方向余弦矩阵跟着变化，由四元数即时修正后求得。

载体在惯导系内的比力方程为：

${\stackrel{\·}{V}}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×V^n+g^n$

写成分量形式有：

$\begin{array}{c}\left[\begin{array}{c}{\stackrel{\·}{V}}_E\\ {\stackrel{\·}{V}}_N\\ {\stackrel{\·}{V}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]+\left[\begin{array}{ccc}0& (\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{ie}^n)\sin L& -\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{ie}^n)\cos L\\ -(\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{ie}^n)\sin L& 0& -\stackrel{\·}{L}\\ (\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{ie}^n)\cos L& \stackrel{\·}{L}& 0\end{array}\right]\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]\\ +\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]\end{array}$

式中：fⁿ为载体加速度在导航坐标系上的投影，fⁿ＝[f_Ef_Nf_U]^T；Vⁿ表示船体在导航坐标系中的速度矢量，Vⁿ＝[V_EV_NV_U]^T；gⁿ为重力加速度矢量，gⁿ＝[00-g]^T。

积分上式，即可求得运载体在导航坐标系上的各个速度分量V_E、V_N、V_U。

(3)位置解算

得到经纬度的微分方程可表示如下：

$\left\{\begin{array}{c}\stackrel{\·}{L}=\frac{V_N}{R_N+h}\\ \stackrel{\·}{\mathrm{\λ}}=\frac{V_E}{(R_N+h)\cos L}\\ \stackrel{\·}{h}=V_U\end{array}\right.$

式中，h为高度。

积分上式的经纬度的更新公式即可得到经纬度：

$\left\{\begin{array}{c}L=\∫\stackrel{\·}{L}dt+L\left(0\right)\\ \mathrm{\λ}=\∫\stackrel{\·}{\mathrm{\λ}}dt+\mathrm{\λ}\left(0\right)\\ h=\∫\stackrel{\·}{h}dt+h\left(0\right)\end{array}\right.$

则得到位置P(λ,L,h)。

5.USBL位置解算结果P_USBL和SINS位置输出结果P_SINS进行Kalman滤波，滤波输出校正SINS位置输出,USBL和SINS组合导航原理示意图如图5所示，具体实现方式如下：

(1)建立的SINS***误差模型及状态方程

根据捷联惯性导航***长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量

式中，δV_E、δV_N分别是东向、北向速度误差；φ_E、φ_N、φ_U分别是东向、北向、天向失准角；δL、δλ分别是纬度、经度误差；分别是x、y向的加速度计偏置；ε_bx、ε_by、ε_bz分别是x、y、z向的陀螺漂移。选取东北天坐标系作为导航坐标系，载体坐标系x轴沿水下潜器横轴指向右舷，y轴沿航行器纵轴指向前，z轴垂直于x与y轴所确定的平面构成右手坐标系。状态方程为

$\stackrel{\·}{X}=FX+W$

***噪声矩阵

$F=\left[\begin{array}{cccccccccccc}\frac{V_N}{R}tgL& F_{12}& 0& -f_U& f_N& F_{16}& 0& C_{11}& C_{21}& 0& 0& 0\\ F_{21}& 0& f_U& 0& -f_E& F_{26}& 0& C_{12}& C_{22}& 0& 0& 0\\ 0& -\frac{1}{R}& 0& F_{\mathrm{34}}& F_{35}& 0& 0& 0& 0& -C_{11}& -C_{21}& -C_{31}\\ \frac{1}{R}& 0& F_{\mathrm{43}}& 0& -\frac{V_N}{R}& F_{46}& 0& 0& 0& -C_{12}& -C_{22}& -C_{32}\\ \frac{tgL}{R}& 0& F_{\mathrm{53}}& \frac{V_N}{R}& 0& F_{56}& 0& 0& 0& -C_{13}& -C_{23}& -C_{33}\\ 0& \frac{1}{R}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ \frac{\sec L}{R}& 0& 0& 0& 0& F_{76}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中：

$\begin{array}{cc}{F}_{12}=2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L& F_{16}=(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L)V_N\end{array}$

$\begin{array}{cc}{F}_{21}=-2({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L)& F_{26}=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L)V_E\end{array}$

$\begin{array}{cc}{F}_{34}=-F_{43}={\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L& F_{35}=-F_{53}=-({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R})\end{array}$

F₄₆＝-ω_iesinL $F_{56}={\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}{\sec }^{2}L$

C_ij为姿态转移矩阵的元素

(2)建立量测方程

Z＝HX+V，其中H＝[000001100000]，Z是每次测量水声定位得到的位置信息与SINS定位信息差值。

(3)***状态方程及量测方程的离散化

X_k＝φ_k,k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

式中，X_k为k时刻的状态向量，也就是被估计矢量；Z_k为k时刻的测量序列；W_k-1为k-1时刻的***噪声；V_k为k时刻的测量噪声序列；Φ_k,k-1为k-1时刻到k时刻的一步状态转移矩阵；Γ_k-1是***噪声输入矩阵，H_k为k时刻的测量矩阵，

(4)利用标准卡尔曼滤波方程计算状态的最优估计

状态一步预测向量

X_k/k-1＝φ_k,k-1X_k-1

状态估值计算

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

滤波增益

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

一步预测均方误差矩阵

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

估计均方误差方程

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

(5)校正SINS

利用当前的误差最优估计可以立即校正SINS每次的由测量数据得到的状态量。

速度和位置校正可以通过惯导***对这两个参数的估计值与估计误差简单相减来修正：

$X_c=\hat{X}-X$

式中，X_c是校正后的状态量。

Claims (4)

1.一种基于USBL辅助低成本SINS***的定位方法,其特征在于，定位***由捷联惯性导航***SINS和超短基线***USBL组成，利用Klaman滤波方法完成组合导航:

(1)超短基线由安装在AUV(水下自主航行器)上的4个水听器基阵和布放在海底的单应答器组成，基阵用于接收单应答器的信号，接收信号两两进行广义加权二次相关，得到时延差；

(2)根据得到的时延差，进行USBL位置结算，得到AUV的位置P_USBL；

(3)捷联惯性导航***SINS通过捷联结算得到AUV的位置信息P_SINS；

(4)USBL位置解算结果和SINS位置输出结果进行Kalman滤波，滤波输出校正SINS位置输出。

2.根据权利要求1所述的一种基于USBL辅助低成本SINS***的定位方法，其特征在于，广义加权二次相关获取时延差通过以下具体步骤实现：

(1)海底单应答器的应答信号为x(t),第i个水听器接收到的信号为x_i(t)＝α_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j(t)＝α_jx(t-τ_j)+n_j(t)，其中α_i、α_j为声信号在水中传播的衰减系数，n_i(t)、n_j(t)为噪声信号，τ_i、τ_j为传播时间；

(2)x_i(t)的自相关函数为x_i(t)与x_j(t)的互相关函数为其中τ＝τ_j-τ_i，表示到达时间差，T表示观测时间；

(3)R_ii(τ)和R_ij(τ)看成新的信号函数，再次进行广义互相关，新的互相关函数 $R\left(\mathrm{\τ}\right)=\frac{1}{T-\mathrm{\τ}}{\∫}_{\mathrm{\τ}}^T{R}_{ii}\left(t\right)R_{ij}(t-\mathrm{\τ})dt;$

(4)根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：G₁₂(ω)是R_ii(t)和R_ij(t)之间的互功率谱，G₁₂(ω)＝R_ii(ω)R_ij(ω)，R_ii(ω)和R_ij(ω)分别由R_ii(t)和R_ij(t)傅里叶变换得到；

(5)为了抑制噪声的影响，选择频域加权函数对信号进行滤波，在傅里叶逆变换到时域得到广义互相关函数，即：

(6)从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时延差τ。

3.根据权利要求1所述的一种基于USBL辅助低成本SINS***的定位方法，其特征在于，根据时延差，进行USBL位置解算通过以下具体步骤实现：

(1)声学基阵由互相垂直安装在基阵坐标X轴和Y轴上的4个水听器组成，海底单应答器地理坐标已知；

(2)海底应答器在声学基阵坐标系(a系)下的位置为(x,y,z)； τ_x和τ_y分别为X轴两个水听器和Y轴两个水听器之间的时延差，c为水中的声速，d为阵元间距，R为应答器到声学基阵中心的距离，T为往返时间；

(3)将求得的位置坐标(x,y,z)进行坐标变换，得到AUV相对于海底应答器的相对位置，在根据已知的海底应答器的绝对坐标得到水下AUV的位置。

4.根据权利要求3所述的一种基于USBL辅助低成本SINS***的定位方法，其特征在于，其中所述的坐标变换的具体实现方式如下：

(1)已知应答器在地理坐标系(n系)下位置为设P_USBL为通过超短基线***测得的AUV在地理坐标系的位置，P_nt为在地理坐标系下AUV相对应答器的位置，则 为应答器在地理坐标系(n系)下位置；

(2)设地理坐标系为n系，声学基阵坐标系为a系，载体坐标系为b系，根据时延差求得应答器在声学基阵坐标系(a系)下为位置为由声学基阵坐标系到载体坐标系的坐标转换可以得到应答器在载体坐标系下的位置 δp为基阵坐标系在载体坐标系的安装误差，为声学基阵坐标系到载体坐标系的变换矩阵；

(3)由载体坐标系到地理坐标系的坐标变换可以得到AUV相对应答器在大地坐标系的位置 $P_{nt}=-C_b^n{P}_t^b.$