一种基于usbl辅助低成本sins系统的定位方法
一种基于usbl辅助低成本sins系统的定位方法
【技术领域】
[0001] 本发明设及一种基于US化辅助低成本SINS系统的定位方法，属于水下导航技术领 域。
【背景技术】
[0002] 在21世纪，人类将越来越多地向海洋获取物质资源和生存空间，水下航行器作为 一种可在水下移动、具有视觉和感知系统、通过遥控或自主操作方式的小型载体可W代替 或辅助人去完成某些水下作业勘探和侦察任务。水下航行器在配备探测、监视和环境传感 用的成套声学和非声学传感器W及水下高速火箭、防御武器等时，可用于潜艇战、水雷战、 侦察、监视和水下攻击等各方面。由于水下航行器的活动范围广、体积小、重量轻、噪音低、 隐蔽性高，现已成为各发达国家军事海洋技术研究的一个重要方向。
[0003] 水下高精度导航和定位技术是解决水下作业的前提和关键，为保证自主水下航行 器(AUV，Autonomous Underwater Vehicle)能够顺利的完成水下的相关任务，要求AUV导航 系统具有水下长期自主的导航定位和返航能力。目前已有的定位技术中，惯性导航系统 (INS)因其隐蔽性强和自主性在水下航行器中得到广泛应用，虽然惯导技术本身已日渐成 熟，但其误差会随着时间积累，因此，伴随惯导技术发展的同时，世界各国都一直在探索惯 导系统的校准方法。L化化ong Base Line,长基线)水声定位系统通常是由基线长度为几千 米的海底应答器阵和被定为载体上的问答机组成，利用水下目标与海底阵元之间的距离信 息来求解目标位置，但是水下布放、回收、校准基阵比较麻烦，且数据更新频率较低，作业复 杂。超短基线只有一个尺寸很小的基阵安装在载体上，无需布放基阵，使用比较灵活和方 便。
[0004] 对超短基线声学定位系统研究较早的是挪威的Kongsberg Simrad公司，该公司于 1997年推出了世界领先水平的高精度长程超短基线定位系统一化PAP350,作用距离可达 3000米，距离测量精度优于20cm，随后推出HiPAP500,作用水深达4000m，测距精度优于 20cm;新近推出的化PAPP700，作用水深达10000米，测距精度优于50cm，也是世界上唯一工 作水深上万米的长程超短基线定位系统。另外，法国0CEAN0 Technologies公司推出的 化sidonia 6000长程超短基线定位系统，工作水深6000m，最大作用距离8000m，在6000m水 深300度开角范围内，测距精度为0.5%，询问频率为8-14曲Z，应答频率为14-18曲Z，该系统 已经成功推向市场。
[000引由于国内对AUV技术开始研究的时间较晚，与欧美等发达国家相比，在很多方面都 存在着不足。但随着不断的投入也取得了很大的进展。国内具有代表性的水声定位系统是 哈尔冰工程大学水声工程学院研制出了四种基于声学定位的系统:深水重潜装潜水员超短 基线定位系统、"探索者"号水下机器人超短基线定位系统，灭雷具配套水声跟踪定位装置、 长程超短基线定位系统。此外，范欣等提出了一种基于多传感器信息融合的水下航行器组 合导航方法，建立了基于SINS\WL\L^\深度计\航向传感器的误差模型，推导了多传感器 信息融合模型，仿真结果表明该方法提高了远程自主水下航行器的导航精度；章大勇等提 出了一种基于SINS/PL化的AUV组合导航算法研究，分析了不同运动状态下的可观测性条 件，为后续的研究提供了依据。
[0006] 采用US化超短基线定位系统，利用宽带信号进行定位时，高精度时延差的获取是 水下定位精度的关键因素，但是水下环境复杂，有海洋环境噪声、舰船福射噪声、海洋混响 等因素影响，使得采用互相关函数的相关峰有模糊峰现象，使得时延差难W准确获取，造成 水下定位误差。
【发明内容】
[0007] 本发明的技术解决问题是:针对现有技术的不足，提出一种基于US化辅助低成本 SINS系统的定位方法。
[0008] 本发明的技术解决方案为:本发明提出的一种基于US化辅助低成本SINS系统的定 位方法，其特征在于，由捷联惯性导航系统SINS和超短基线系统US化组成，利用Klaman滤波 方法完成组合导航：
[0009] (1)超短基线由安装在AUV上的基阵和布放在海底的单应答器组成，基阵用于接收 单应答器的信号，接收信号两两进行广义加权二次相关，得到时延差；
[0010] (2)根据得到的时延差，进行USBL位置结算，得到AUV的位置时SBL;
[0011] (3)捷联惯性导航系统SINS通过捷联结算得到AUV的位置信息PsiNS;
[0012] (4)US化位置解算结果和SINS位置输出结果进行Kalman滤波，滤波输出校正SINS 位置输出。
[0013] 本发明还包括W下特征：
[0014] 1.广义加权二次相关获取时延差通过W下具体步骤实现：
[0015] (1)海底单应答器的应答信号为x(t)，第i个水听器接收到的信号为Xi(t)=aiX(t-1〇+山(1：)，第^'个水听器接收到的信号为刮（1：)=曰巧(1：-1如]1加），其中€4、曰功声信号在水 中传播的衰减系数，m(t)、n^t)为噪声信号，τι、τ^为传播时间；
[0016] (2)Xi(t)的自相关函数为
Xi(t)与Xj(t)的互相关 函数天
串中τ = τ厂τι，表示到达时间差，T表示观测时间；
[0017] (3)Rii(T)和Ru(T)看成新的信号函数，再次进行广义互相关，新的互相关函数
[001引（4 )根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：
是Rii(t)和Rij(t)之间的互功率谱；
[0019] (5)为了抑制噪声的影响，选择频域加权函数φυ(")对信号进行滤波，在傅里叶逆 变换到时域得到广义互相关函数，即：
[0020] (6)从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时 延差τ。
[0021] 2.根据时延差，进行US化位置解算通过W下具体步骤实现：
[0022] (1)声学基阵由互相垂直安装在基阵坐标X轴和Y轴上的4个水听器组成，海底单应 答器地理坐标已知.
[0023] (2)海底应答器在声学基阵坐标系（a系）下的位置为（x，y，z):
Τχ和Ty为时延差，C为水中的声速，d为阵元间距，R为应答器到AUV 的距离，R = 7评，T为往返时间；
[0024] (3)将求得的位置坐标(x，y，z)进行坐标变换，得到AUV相对于海底应答器的相对 位置，在根据已知的海底应答器的坐标得到水下AUV的位置。
[0025] 本发明与现有技术相比的优点在于：
[0026] 与现有技术相比，本发明通过将水听器的接收信号先进行自相关和互相关，然后 在进行广义互相关，在广义互相关的基础上添加了权重函数进行频域滤波，经过广义加权 二次相关后，增强了信号中信噪比比较高的频率成分，抑制了噪声的影响，锐化了峰值，在 低信噪比的情况下，依然能得到高精度时延差;US化定位输出校正了 SINS导航误差，弥补了 SINS长时间航行误差积累的问题，避免了无线电定位系统和长基线定位系统的使用，保证 了水下高精度导航的要求。
【附图说明】
[0027] 图1为US化辅助SINS定位示意图；
[002引图2为US化定位原理示意图；
[0029] 图3为同轴两个基元声线平行定位原理示意图；
[0030] 图4为广义加权二次相关时延估计原理图；
[0031 ] 图5为US化和SINS组合导航原理示意图
【具体实施方式】
[0032] 本发明提出的一种基于US化辅助低成本SINS系统的定位方法，采用W下具体方式 实现：
[0033] 1.如图1所示，本发明由安装在AUV上的低成本捷联惯性系统系统SINS和超短基线 系统US化组成。超短基线系统由安装在AUV上的声源、声学基阵和海底单应答器组成，已知 单应答器的地理坐标为昭，应答器发出的信号基阵原点的声线与X轴和Y轴的夹角分别为α 和β，应答器与基阵的距离是R = ^cr，T为往返时间，C为水中的声速，超短基线定位原理示 意图如图2所示，由图2可知：
[0034]
[003引由于超短基线系统基阵间距离较小，可W认为应答器信号到所有基元的声线平 行，如图3所示，有
d为阵元间距，所W可得
因此 测得时延差和距离R可W得出应答器在声学基阵坐标系(a系)下的坐标巧，
[0036] 2.上述时延差根据广义加权二次相关获得，广义加权二次相关时延估计原理图如 图4所示，具体实现方式如下：
[0037] (1)海底单应答器的应答信号为x(t)，第i个水听器接收到的信号为xi(t)=aix(t- 1〇+山(1：)，第^'个水听器接收到的信号为刮（1：)=曰巧(1：-1如]1加），其中€4、曰功声信号在水 中传播的衰减系数，m(t)、n^t)为噪声信号，τι、τ^为传播时间；
[003引（2)Xi(t)的自相关函数为
Xi(t)与Xj(t)的互相关 函数戈
其中τ = τ厂τι，表示到达时间差，T表示观测时间；
[0039] (3)Rii(T)和Ru(T)看成新的信号函数，再次进行广义互相关，新的互相关函数
[0040] (4)根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：R財=
Gi2( ω )是Rii(t)和Rij(t)之间的互功率谱；
[0041] (5)为了抑制噪声的影响，选择频域加权函数φ?2(ω)对信号进行滤波，在傅里叶逆 变换到时域得到广义互相关函数，即：
[0042] (6)从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时 延差τ;
[0043] 3.已知应答器在地理坐标系(η系）下位置为巧，通过坐标变换得到AUV在地理坐标 系下位置的具体实现方式如下：
[0044] (1)设时SBL为通过超短基线系统测得的AUV在地理坐标系的位置，Pnt为在地理坐标 系下AUV相对应答器的位置，则=谬+ Ik，巧为应答器在地理坐标系(η系)下位置； [004引（2)设地理坐标系为η系，声学基阵坐标系为a系，载体坐标系为b系，根据时延差求 得应答器在声学基阵坐标系(a系）下为位置为巧，由声学基阵坐标系到载体坐标系的坐标 转换可W得到应答器在载体坐标系下的位置巧二巧呼+邸，邸为基阵坐标系在载体坐标 系的安装误差，Cf为声学基阵坐标系到载体坐标系的变换矩阵；
[0046] (3)由载体坐标系到地理坐标系的坐标变换可W得到AUV相对应答器在大地坐标 系的位置Pnt二巧Pfb;
[0047] (4)因此超短基线系统测得AUV的位置为
[0048] 4.捷联惯 性导航系统SINS通过捷联结算得到AUV的位置信息PsiNS，具体实现方式 如下：
[0049] (1)计算姿态矩阵和姿态角
[0050] 采用四元数法计算姿态矩阵，根据欧拉定理，动坐标系相对参考坐标系的方位等 效于动坐标系绕某个等效转轴转动一个角度Θ，如果用U表示等效转轴方向的单位矢量，贝U 动坐标系的方位完全由U和Θ两个参数来确定。
[0051] 用U和Θ可构造一个四元数：
[0052]
[0053]对上式求导并化简可得四元数微分方程：
[0056]根据毕卡逼近法求解四元数微分方程得：
[0063] 令地球坐标系相对惯性坐标系的自转角速度为ω ie，（其值为15.04088° A)，L表示 当地缔度，λ表示当地经度，贝U
[0064] ω ie":地球坐标系相对惯性坐标系的自转角速度在地理坐标系中的矢量，为：
[0065]
[0066] ω leb:地球坐标系相对惯性坐标系的自转角速度在载体坐标系中的矢量，为：
[0067]
[0068] 式中的姿态矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时， 姿态矩阵跟着变化，由四元数即时修正后求得(下同）。
[0069] ω en":地理坐标相对地球坐标系转动角速度在地理坐标系中的矢量，为：
[0070]
[0071 ] Ve、Vn分别为载体运动的东向和北向速度；
[0072] Rn为参考楠球体子午面内的曲率半径，RN = Re(l-2e+3e sin2L);
[0073] Re为垂直子午面的法线平面内的曲率半径，RE = Re(l+e sin2L);
[0074] 其中Re为参考楠球体的长轴半径;e为楠球的楠圆度。
[007引又因为
则
[0076]
[0077] ω enb :地理坐标相对地球坐标系转动角速度在载体坐标系中的矢量，为：
[007 引
[0079] 巧螺输出角速度，记为
[0080]
[0081] ω nbb:载体坐标系相对地理坐标系的转动角速度在载体坐标系中的矢量，记为
[0085] 四元数即时修正后，根据下式可由四元数的元实时更新姿态矩阵C；；(t)
[0086]
[0087]从姿态阵中即可提取实时姿态角 [008引
[0089] (2)速度计算
[0090] 得到的载体坐标系中的比力矢量为fb，则地理坐标系中有：
[0091]
[0092] 式中的方向余弦矩阵q在载体静止时，由初始角度决定；当载体相对地理坐标系 转动时，方向余弦矩阵跟着变化，由四元数即时修正后求得。
[0093] 载体在惯导系内的比力方程为：
[0097]式中：严为载体加速度在导航坐标系上的投影，严二[fE fN fu]T;yn表示船体在导 航坐标系中的速度矢量，yn=[% Vn Vu]T;gn为重力加速度矢量，gn=[0 0 -g]T。
[0098] 积分上式，即可求得运载体在导航坐标系上的各个速度分量Ve、Vn、Vu。
[0099] (3)位置解算
[0100] 得到经缔度的微分方程可表示如下：
[0101]
[0102] 式中，h为高度。
[0103] 积分上式的经缔度的更新公式即可得到经缔度：
[0104]
[010引则得到位置Ρ(λ，^1ι)。
[0106] 5.US化位置解算结果时S化和SINS位置输出结果PsiNS进行Kalman滤波，滤波输出校 正SINS位置输出，USMJ日SINS组合导航原理示意图如图5所示，具体实现方式如下：
[0107] (1)建立的SINS系统误差模型及状态方程
[0108] 根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误 差、巧螺漂移和加速度计零偏作为状态量
[0109]
[0110] 式中，SVe、SVn分别是东向、北向速度误差；Φε、Φν、Φυ分别是东向、北向、天向失准 角；化、δλ分别是缔度、经度误差；V&、分别是x、y向的加速度计偏置;ebx、Eby、ebz分别是 x、y、z向的巧螺漂移。选取东北天坐标系作为导航坐标系，载体坐标系X轴沿水下潜器横轴 指向右艇，y轴沿航行器纵轴指向前，Z轴垂直于X与y轴所确定的平面构成右手坐标系。状态 方程为
[0111] 义二 FX + W
[0112] 系统噪声矩阵
[0113]
[0122]
[0123] (2)建立量测方程
[0124] Z = HX+V，其中H=[0 0000110000 0]，Z是每次测量水声定位得到的位 置信息与SINS定位信息差值。
[0125] (3)系统状态方程及量测方程的离散化
[0128] 式中，Xk为k时刻的状态向量，也就是被估计矢量;立为4时刻的测量序列;Wk-i为k-1 时刻的系统噪声;Vk为k时刻的测量噪声序列；Φk,k-i为k-1时刻到k时刻的一步状态转移矩 阵；Γ k-i是系统噪声输入矩阵，Hk为k时刻的测量矩阵，
[0129] (4)利用标准卡尔曼滤波方程计算状态的最优估计
[0130] 状态一步预测向量
[0131]
[0132] 状态估值计算
[0133]
[0134] 滤波增益
[0135]
[0136] -步预测均方误差矩阵
[0137]
[0138] 估计均方误差方程
[0139]
[0140] (5)校正 SINS
[0141] 利用当前的误差最优估计可W立即校正SINS每次的由测量数据得到的状态量。
[0142] 速度和位置校正可W通过惯导系统对运两个参数的估计值与估计误差简单相减 来修正：
[0143]
[0144] 式中，)(c是校正后的状态量。
【主权项】
1. 一种基于USBL辅助低成本SINS系统的定位方法，其特征在于，定位系统由捷联惯性 导航系统SINS和超短基线系统USBL组成，利用Klaman滤波方法完成组合导航： (1) 超短基线由安装在AUV(水下自主航行器)上的4个水听器基阵和布放在海底的单应 答器组成，基阵用于接收单应答器的信号，接收信号两两进行广义加权二次相关，得到时延 差； ⑵根据得到的时延差，进行USBL位置结算，得到AUV的位置Pusbl; (3) 捷联惯性导航系统SINS通过捷联结算得到AUV的位置信息Psins; (4) USBL位置解算结果和SINS位置输出结果进行Kalman滤波，滤波输出校正SINS位置 输出。2. 根据权利要求1所述的一种基于USBL辅助低成本SINS系统的定位方法，其特征在于， 广义加权二次相关获取时延差通过以下具体步骤实现： a)海底单应答器的应答信号为x(t)，第i个水听器接收到的信号为Χι(υ=αιΧα- τι)+ 1^(1：)，第」个水听器接收到的信号为1」(1：)=(^(1：-4)+11」(1：)，其中€4、€1」为声信号在水中传 播的衰减系数，1^(1:)、11」(1:)为噪声信号，^、4为传播时间 ； (2) Xl(t)的自相关函数为，:Xl(t)与Xj(t)的互相关函数 为其中τ = τ j-q，表示到达时间差，T表示观测时间； (3 )Rn (τ)和Rlj(T)看成新的信号函数，再次进行广义互相关，新的互相关函数(4 )根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：.Gi2( ω )是Rii(t)和Rij(t)之间的互功率谱，Gi2( ω ) =Rii( ω )Rij (ω ) U ω )和Rij( ω )分别由RiiU)和Rij(t)傅里叶变换得到； (5) 为了抑制噪声的影响，选择频域加权阐数φ12(ω)对信号进行滤波，在傅里叶逆变换 到时域得到广义互相关函数，即：(6) 从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时延差 τ〇3. 根据权利要求1所述的一种基于USBL辅助低成本SINS系统的定位方法，其特征在于， 根据时延差，进行USBL位置解算通过以下具体步骤实现： (1) 声学基阵由互相垂直安装在基阵坐标X轴和Y轴上的4个水听器组成，海底单应答器 地理坐标已知； (2) 海底应答器在声学基阵坐标系（a系）下的位置为（x，y，z);.TjPTy分别为X轴两个水听器和Y轴两个水听器之间的时延差，c为水中 的声速，d为阵元间距，R为应答器到声学基阵中心的距离，T为往返时间； (3) 将求得的位置坐标(x，y，z)进行坐标变换，得到AUV相对于海底应答器的相对位置， 在根据已知的海底应答器的绝对坐标得到水下AUV的位置。4.根据权利要求3所述的一种基于USBL辅助低成本SINS系统的定位方法，其特征在于， 其中所述的坐标变换的具体实现方式如下： (1) 已知应答器在地理坐标系(η系）下位置为，设Pusbl为通过超短基线系统测得的AUV 在地理坐标系的位置，Pnt为在地理坐标系下AUV相对应答器的位置，则为应答器在地理坐标系(η系）下位置； (2) 设地理坐标系为η系，声学基阵坐标系为a系，载体坐标系为b系，根据时延差求得应 答器在声学基阵坐标系(a系）下为位置为Pf，由声学基阵坐标系到载体坐标系的坐标转换 可以得到应答器在载体坐标系下的位置卟为基阵坐标系在载体坐标系的 安装误差，为声学基阵坐标系到载体坐标系的变换矩阵； (3) 由载体坐标系到地理坐标系的坐标变换可以得到AUV相对应答器在大地坐标系的 位置
【专利摘要】本发明提供了一种基于USBL辅助低成本SINS系统的定位方法。本发明由捷联惯性导航系统SINS和超短基线系统USBL组成，利用Klaman滤波方法完成组合导航。超短基线系统作为辅助导航系统，由安装在AUV上的基阵和布放在海底的单应答器组成，超短基线基阵接收的信号进行广义加权二次相关后，再进行位置解算，USBL的位置解算结果和SINS位置输出进行Kalman滤波，滤波器的输出再反馈校正SINS。本发明解决了采用低成本SINS系统长时间误差积累的问题，USBL采用广义加权二次相关提高了时延估计精度和抗噪声性能，避免了采用长基线系统基阵布放、校准、作业复杂的问题，同时保证了水下高精度定位与导航。
【IPC分类】G01C21/16, G01S5/18, G01C21/20
【公开号】CN105486313
【申请号】CN201610077647
【发明人】张涛, 胡贺庆, 徐晓苏, 王自强, 朱永云
【申请人】东南大学
【公开日】2016年4月13日
【申请日】2016年2月3日
【技术领域】
[0001] 本发明设及一种基于US化辅助低成本SINS系统的定位方法，属于水下导航技术领 域。
【背景技术】
[0002] 在21世纪，人类将越来越多地向海洋获取物质资源和生存空间，水下航行器作为 一种可在水下移动、具有视觉和感知系统、通过遥控或自主操作方式的小型载体可W代替 或辅助人去完成某些水下作业勘探和侦察任务。水下航行器在配备探测、监视和环境传感 用的成套声学和非声学传感器W及水下高速火箭、防御武器等时，可用于潜艇战、水雷战、 侦察、监视和水下攻击等各方面。由于水下航行器的活动范围广、体积小、重量轻、噪音低、 隐蔽性高，现已成为各发达国家军事海洋技术研究的一个重要方向。
[0003] 水下高精度导航和定位技术是解决水下作业的前提和关键，为保证自主水下航行 器(AUV，Autonomous Underwater Vehicle)能够顺利的完成水下的相关任务，要求AUV导航 系统具有水下长期自主的导航定位和返航能力。目前已有的定位技术中，惯性导航系统 (INS)因其隐蔽性强和自主性在水下航行器中得到广泛应用，虽然惯导技术本身已日渐成 熟，但其误差会随着时间积累，因此，伴随惯导技术发展的同时，世界各国都一直在探索惯 导系统的校准方法。L化化ong Base Line,长基线)水声定位系统通常是由基线长度为几千 米的海底应答器阵和被定为载体上的问答机组成，利用水下目标与海底阵元之间的距离信 息来求解目标位置，但是水下布放、回收、校准基阵比较麻烦，且数据更新频率较低，作业复 杂。超短基线只有一个尺寸很小的基阵安装在载体上，无需布放基阵，使用比较灵活和方 便。
[0004] 对超短基线声学定位系统研究较早的是挪威的Kongsberg Simrad公司，该公司于 1997年推出了世界领先水平的高精度长程超短基线定位系统一化PAP350,作用距离可达 3000米，距离测量精度优于20cm，随后推出HiPAP500,作用水深达4000m，测距精度优于 20cm;新近推出的化PAPP700，作用水深达10000米，测距精度优于50cm，也是世界上唯一工 作水深上万米的长程超短基线定位系统。另外，法国0CEAN0 Technologies公司推出的 化sidonia 6000长程超短基线定位系统，工作水深6000m，最大作用距离8000m，在6000m水 深300度开角范围内，测距精度为0.5%，询问频率为8-14曲Z，应答频率为14-18曲Z，该系统 已经成功推向市场。
[000引由于国内对AUV技术开始研究的时间较晚，与欧美等发达国家相比，在很多方面都 存在着不足。但随着不断的投入也取得了很大的进展。国内具有代表性的水声定位系统是 哈尔冰工程大学水声工程学院研制出了四种基于声学定位的系统:深水重潜装潜水员超短 基线定位系统、"探索者"号水下机器人超短基线定位系统，灭雷具配套水声跟踪定位装置、 长程超短基线定位系统。此外，范欣等提出了一种基于多传感器信息融合的水下航行器组 合导航方法，建立了基于SINS\WL\L^\深度计\航向传感器的误差模型，推导了多传感器 信息融合模型，仿真结果表明该方法提高了远程自主水下航行器的导航精度；章大勇等提 出了一种基于SINS/PL化的AUV组合导航算法研究，分析了不同运动状态下的可观测性条 件，为后续的研究提供了依据。
[0006] 采用US化超短基线定位系统，利用宽带信号进行定位时，高精度时延差的获取是 水下定位精度的关键因素，但是水下环境复杂，有海洋环境噪声、舰船福射噪声、海洋混响 等因素影响，使得采用互相关函数的相关峰有模糊峰现象，使得时延差难W准确获取，造成 水下定位误差。
【发明内容】
[0007] 本发明的技术解决问题是:针对现有技术的不足，提出一种基于US化辅助低成本 SINS系统的定位方法。
[0008] 本发明的技术解决方案为:本发明提出的一种基于US化辅助低成本SINS系统的定 位方法，其特征在于，由捷联惯性导航系统SINS和超短基线系统US化组成，利用Klaman滤波 方法完成组合导航：
[0009] (1)超短基线由安装在AUV上的基阵和布放在海底的单应答器组成，基阵用于接收 单应答器的信号，接收信号两两进行广义加权二次相关，得到时延差；
[0010] (2)根据得到的时延差，进行USBL位置结算，得到AUV的位置时SBL;
[0011] (3)捷联惯性导航系统SINS通过捷联结算得到AUV的位置信息PsiNS;
[0012] (4)US化位置解算结果和SINS位置输出结果进行Kalman滤波，滤波输出校正SINS 位置输出。
[0013] 本发明还包括W下特征：
[0014] 1.广义加权二次相关获取时延差通过W下具体步骤实现：
[0015] (1)海底单应答器的应答信号为x(t)，第i个水听器接收到的信号为Xi(t)=aiX(t-1〇+山(1：)，第^'个水听器接收到的信号为刮（1：)=曰巧(1：-1如]1加），其中€4、曰功声信号在水 中传播的衰减系数，m(t)、n^t)为噪声信号，τι、τ^为传播时间；
[0016] (2)Xi(t)的自相关函数为
Xi(t)与Xj(t)的互相关 函数天
串中τ = τ厂τι，表示到达时间差，T表示观测时间；
[0017] (3)Rii(T)和Ru(T)看成新的信号函数，再次进行广义互相关，新的互相关函数
[001引（4 )根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：
是Rii(t)和Rij(t)之间的互功率谱；
[0019] (5)为了抑制噪声的影响，选择频域加权函数φυ(")对信号进行滤波，在傅里叶逆 变换到时域得到广义互相关函数，即：
[0020] (6)从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时 延差τ。
[0021] 2.根据时延差，进行US化位置解算通过W下具体步骤实现：
[0022] (1)声学基阵由互相垂直安装在基阵坐标X轴和Y轴上的4个水听器组成，海底单应 答器地理坐标已知.
[0023] (2)海底应答器在声学基阵坐标系（a系）下的位置为（x，y，z):
Τχ和Ty为时延差，C为水中的声速，d为阵元间距，R为应答器到AUV 的距离，R = 7评，T为往返时间；
[0024] (3)将求得的位置坐标(x，y，z)进行坐标变换，得到AUV相对于海底应答器的相对 位置，在根据已知的海底应答器的坐标得到水下AUV的位置。
[0025] 本发明与现有技术相比的优点在于：
[0026] 与现有技术相比，本发明通过将水听器的接收信号先进行自相关和互相关，然后 在进行广义互相关，在广义互相关的基础上添加了权重函数进行频域滤波，经过广义加权 二次相关后，增强了信号中信噪比比较高的频率成分，抑制了噪声的影响，锐化了峰值，在 低信噪比的情况下，依然能得到高精度时延差;US化定位输出校正了 SINS导航误差，弥补了 SINS长时间航行误差积累的问题，避免了无线电定位系统和长基线定位系统的使用，保证 了水下高精度导航的要求。
【附图说明】
[0027] 图1为US化辅助SINS定位示意图；
[002引图2为US化定位原理示意图；
[0029] 图3为同轴两个基元声线平行定位原理示意图；
[0030] 图4为广义加权二次相关时延估计原理图；
[0031 ] 图5为US化和SINS组合导航原理示意图
【具体实施方式】
[0032] 本发明提出的一种基于US化辅助低成本SINS系统的定位方法，采用W下具体方式 实现：
[0033] 1.如图1所示，本发明由安装在AUV上的低成本捷联惯性系统系统SINS和超短基线 系统US化组成。超短基线系统由安装在AUV上的声源、声学基阵和海底单应答器组成，已知 单应答器的地理坐标为昭，应答器发出的信号基阵原点的声线与X轴和Y轴的夹角分别为α 和β，应答器与基阵的距离是R = ^cr，T为往返时间，C为水中的声速，超短基线定位原理示 意图如图2所示，由图2可知：
[0034]
[003引由于超短基线系统基阵间距离较小，可W认为应答器信号到所有基元的声线平 行，如图3所示，有
d为阵元间距，所W可得
因此 测得时延差和距离R可W得出应答器在声学基阵坐标系(a系)下的坐标巧，
[0036] 2.上述时延差根据广义加权二次相关获得，广义加权二次相关时延估计原理图如 图4所示，具体实现方式如下：
[0037] (1)海底单应答器的应答信号为x(t)，第i个水听器接收到的信号为xi(t)=aix(t- 1〇+山(1：)，第^'个水听器接收到的信号为刮（1：)=曰巧(1：-1如]1加），其中€4、曰功声信号在水 中传播的衰减系数，m(t)、n^t)为噪声信号，τι、τ^为传播时间；
[003引（2)Xi(t)的自相关函数为
Xi(t)与Xj(t)的互相关 函数戈
其中τ = τ厂τι，表示到达时间差，T表示观测时间；
[0039] (3)Rii(T)和Ru(T)看成新的信号函数，再次进行广义互相关，新的互相关函数
[0040] (4)根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：R財=
Gi2( ω )是Rii(t)和Rij(t)之间的互功率谱；
[0041] (5)为了抑制噪声的影响，选择频域加权函数φ?2(ω)对信号进行滤波，在傅里叶逆 变换到时域得到广义互相关函数，即：
[0042] (6)从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时 延差τ;
[0043] 3.已知应答器在地理坐标系(η系）下位置为巧，通过坐标变换得到AUV在地理坐标 系下位置的具体实现方式如下：
[0044] (1)设时SBL为通过超短基线系统测得的AUV在地理坐标系的位置，Pnt为在地理坐标 系下AUV相对应答器的位置，则=谬+ Ik，巧为应答器在地理坐标系(η系)下位置； [004引（2)设地理坐标系为η系，声学基阵坐标系为a系，载体坐标系为b系，根据时延差求 得应答器在声学基阵坐标系(a系）下为位置为巧，由声学基阵坐标系到载体坐标系的坐标 转换可W得到应答器在载体坐标系下的位置巧二巧呼+邸，邸为基阵坐标系在载体坐标 系的安装误差，Cf为声学基阵坐标系到载体坐标系的变换矩阵；
[0046] (3)由载体坐标系到地理坐标系的坐标变换可W得到AUV相对应答器在大地坐标 系的位置Pnt二巧Pfb;
[0047] (4)因此超短基线系统测得AUV的位置为
[0048] 4.捷联惯 性导航系统SINS通过捷联结算得到AUV的位置信息PsiNS，具体实现方式 如下：
[0049] (1)计算姿态矩阵和姿态角
[0050] 采用四元数法计算姿态矩阵，根据欧拉定理，动坐标系相对参考坐标系的方位等 效于动坐标系绕某个等效转轴转动一个角度Θ，如果用U表示等效转轴方向的单位矢量，贝U 动坐标系的方位完全由U和Θ两个参数来确定。
[0051] 用U和Θ可构造一个四元数：
[0052]
[0053]对上式求导并化简可得四元数微分方程：
[0056]根据毕卡逼近法求解四元数微分方程得：
[0063] 令地球坐标系相对惯性坐标系的自转角速度为ω ie，（其值为15.04088° A)，L表示 当地缔度，λ表示当地经度，贝U
[0064] ω ie":地球坐标系相对惯性坐标系的自转角速度在地理坐标系中的矢量，为：
[0065]
[0066] ω leb:地球坐标系相对惯性坐标系的自转角速度在载体坐标系中的矢量，为：
[0067]
[0068] 式中的姿态矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时， 姿态矩阵跟着变化，由四元数即时修正后求得(下同）。
[0069] ω en":地理坐标相对地球坐标系转动角速度在地理坐标系中的矢量，为：
[0070]
[0071 ] Ve、Vn分别为载体运动的东向和北向速度；
[0072] Rn为参考楠球体子午面内的曲率半径，RN = Re(l-2e+3e sin2L);
[0073] Re为垂直子午面的法线平面内的曲率半径，RE = Re(l+e sin2L);
[0074] 其中Re为参考楠球体的长轴半径;e为楠球的楠圆度。
[007引又因为
则
[0076]
[0077] ω enb :地理坐标相对地球坐标系转动角速度在载体坐标系中的矢量，为：
[007 引
[0079] 巧螺输出角速度，记为
[0080]
[0081] ω nbb:载体坐标系相对地理坐标系的转动角速度在载体坐标系中的矢量，记为
[0085] 四元数即时修正后，根据下式可由四元数的元实时更新姿态矩阵C；；(t)
[0086]
[0087]从姿态阵中即可提取实时姿态角 [008引
[0089] (2)速度计算
[0090] 得到的载体坐标系中的比力矢量为fb，则地理坐标系中有：
[0091]
[0092] 式中的方向余弦矩阵q在载体静止时，由初始角度决定；当载体相对地理坐标系 转动时，方向余弦矩阵跟着变化，由四元数即时修正后求得。
[0093] 载体在惯导系内的比力方程为：
[0097]式中：严为载体加速度在导航坐标系上的投影，严二[fE fN fu]T;yn表示船体在导 航坐标系中的速度矢量，yn=[% Vn Vu]T;gn为重力加速度矢量，gn=[0 0 -g]T。
[0098] 积分上式，即可求得运载体在导航坐标系上的各个速度分量Ve、Vn、Vu。
[0099] (3)位置解算
[0100] 得到经缔度的微分方程可表示如下：
[0101]
[0102] 式中，h为高度。
[0103] 积分上式的经缔度的更新公式即可得到经缔度：
[0104]
[010引则得到位置Ρ(λ，^1ι)。
[0106] 5.US化位置解算结果时S化和SINS位置输出结果PsiNS进行Kalman滤波，滤波输出校 正SINS位置输出，USMJ日SINS组合导航原理示意图如图5所示，具体实现方式如下：
[0107] (1)建立的SINS系统误差模型及状态方程
[0108] 根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误 差、巧螺漂移和加速度计零偏作为状态量
[0109]
[0110] 式中，SVe、SVn分别是东向、北向速度误差；Φε、Φν、Φυ分别是东向、北向、天向失准 角；化、δλ分别是缔度、经度误差；V&、分别是x、y向的加速度计偏置;ebx、Eby、ebz分别是 x、y、z向的巧螺漂移。选取东北天坐标系作为导航坐标系，载体坐标系X轴沿水下潜器横轴 指向右艇，y轴沿航行器纵轴指向前，Z轴垂直于X与y轴所确定的平面构成右手坐标系。状态 方程为
[0111] 义二 FX + W
[0112] 系统噪声矩阵
[0113]
[0122]
[0123] (2)建立量测方程
[0124] Z = HX+V，其中H=[0 0000110000 0]，Z是每次测量水声定位得到的位 置信息与SINS定位信息差值。
[0125] (3)系统状态方程及量测方程的离散化
[0128] 式中，Xk为k时刻的状态向量，也就是被估计矢量;立为4时刻的测量序列;Wk-i为k-1 时刻的系统噪声;Vk为k时刻的测量噪声序列；Φk,k-i为k-1时刻到k时刻的一步状态转移矩 阵；Γ k-i是系统噪声输入矩阵，Hk为k时刻的测量矩阵，
[0129] (4)利用标准卡尔曼滤波方程计算状态的最优估计
[0130] 状态一步预测向量
[0131]
[0132] 状态估值计算
[0133]
[0134] 滤波增益
[0135]
[0136] -步预测均方误差矩阵
[0137]
[0138] 估计均方误差方程
[0139]
[0140] (5)校正 SINS
[0141] 利用当前的误差最优估计可W立即校正SINS每次的由测量数据得到的状态量。
[0142] 速度和位置校正可W通过惯导系统对运两个参数的估计值与估计误差简单相减 来修正：
[0143]
[0144] 式中，)(c是校正后的状态量。
【主权项】
1. 一种基于USBL辅助低成本SINS系统的定位方法，其特征在于，定位系统由捷联惯性 导航系统SINS和超短基线系统USBL组成，利用Klaman滤波方法完成组合导航： (1) 超短基线由安装在AUV(水下自主航行器)上的4个水听器基阵和布放在海底的单应 答器组成，基阵用于接收单应答器的信号，接收信号两两进行广义加权二次相关，得到时延 差； ⑵根据得到的时延差，进行USBL位置结算，得到AUV的位置Pusbl; (3) 捷联惯性导航系统SINS通过捷联结算得到AUV的位置信息Psins; (4) USBL位置解算结果和SINS位置输出结果进行Kalman滤波，滤波输出校正SINS位置 输出。2. 根据权利要求1所述的一种基于USBL辅助低成本SINS系统的定位方法，其特征在于， 广义加权二次相关获取时延差通过以下具体步骤实现： a)海底单应答器的应答信号为x(t)，第i个水听器接收到的信号为Χι(υ=αιΧα- τι)+ 1^(1：)，第」个水听器接收到的信号为1」(1：)=(^(1：-4)+11」(1：)，其中€4、€1」为声信号在水中传 播的衰减系数，1^(1:)、11」(1:)为噪声信号，^、4为传播时间 ； (2) Xl(t)的自相关函数为，:Xl(t)与Xj(t)的互相关函数 为其中τ = τ j-q，表示到达时间差，T表示观测时间； (3 )Rn (τ)和Rlj(T)看成新的信号函数，再次进行广义互相关，新的互相关函数(4 )根据维纳-辛钦定理可知，互功率谱和互相关函数之间关系为：.Gi2( ω )是Rii(t)和Rij(t)之间的互功率谱，Gi2( ω ) =Rii( ω )Rij (ω ) U ω )和Rij( ω )分别由RiiU)和Rij(t)傅里叶变换得到； (5) 为了抑制噪声的影响，选择频域加权阐数φ12(ω)对信号进行滤波，在傅里叶逆变换 到时域得到广义互相关函数，即：(6) 从广义互相关函数的相关峰中选取峰值最大的相关峰，对应的横坐标值为时延差 τ〇3. 根据权利要求1所述的一种基于USBL辅助低成本SINS系统的定位方法，其特征在于， 根据时延差，进行USBL位置解算通过以下具体步骤实现： (1) 声学基阵由互相垂直安装在基阵坐标X轴和Y轴上的4个水听器组成，海底单应答器 地理坐标已知； (2) 海底应答器在声学基阵坐标系（a系）下的位置为（x，y，z);.TjPTy分别为X轴两个水听器和Y轴两个水听器之间的时延差，c为水中 的声速，d为阵元间距，R为应答器到声学基阵中心的距离，T为往返时间； (3) 将求得的位置坐标(x，y，z)进行坐标变换，得到AUV相对于海底应答器的相对位置， 在根据已知的海底应答器的绝对坐标得到水下AUV的位置。4.根据权利要求3所述的一种基于USBL辅助低成本SINS系统的定位方法，其特征在于， 其中所述的坐标变换的具体实现方式如下： (1) 已知应答器在地理坐标系(η系）下位置为，设Pusbl为通过超短基线系统测得的AUV 在地理坐标系的位置，Pnt为在地理坐标系下AUV相对应答器的位置，则为应答器在地理坐标系(η系）下位置； (2) 设地理坐标系为η系，声学基阵坐标系为a系，载体坐标系为b系，根据时延差求得应 答器在声学基阵坐标系(a系）下为位置为Pf，由声学基阵坐标系到载体坐标系的坐标转换 可以得到应答器在载体坐标系下的位置卟为基阵坐标系在载体坐标系的 安装误差，为声学基阵坐标系到载体坐标系的变换矩阵； (3) 由载体坐标系到地理坐标系的坐标变换可以得到AUV相对应答器在大地坐标系的 位置
【专利摘要】本发明提供了一种基于USBL辅助低成本SINS系统的定位方法。本发明由捷联惯性导航系统SINS和超短基线系统USBL组成，利用Klaman滤波方法完成组合导航。超短基线系统作为辅助导航系统，由安装在AUV上的基阵和布放在海底的单应答器组成，超短基线基阵接收的信号进行广义加权二次相关后，再进行位置解算，USBL的位置解算结果和SINS位置输出进行Kalman滤波，滤波器的输出再反馈校正SINS。本发明解决了采用低成本SINS系统长时间误差积累的问题，USBL采用广义加权二次相关提高了时延估计精度和抗噪声性能，避免了采用长基线系统基阵布放、校准、作业复杂的问题，同时保证了水下高精度定位与导航。
【IPC分类】G01C21/16, G01S5/18, G01C21/20
【公开号】CN105486313
【申请号】CN201610077647
【发明人】张涛, 胡贺庆, 徐晓苏, 王自强, 朱永云
【申请人】东南大学
【公开日】2016年4月13日
【申请日】2016年2月3日
最新回复(0)