申请/专利权人：运城学院

申请日：2020-08-21

公开（公告）日：2023-10-24

公开（公告）号：CN111982105B

主分类号：G01C21/16

分类号：G01C21/16;G01S5/24;G01L19/00

优先权：

专利状态码：有效-授权

法律状态：2023.10.24#授权;2020.12.11#实质审查的生效;2020.11.24#公开

摘要：本发明公开一种基于SINSLBL紧组合的水下导航定位方法及系统，针对现有技术中存在的水下导航定位误差收敛速度慢的问题，本发明提供一种基于SINSLBL紧组合的水下导航定位方法，基于该方法的系统由捷联惯导系统、长基线水声定位系统、压力传感器和数据处理单元组成，将捷联惯导系统输出的斜距差、斜距率差和深度值分别与长基线水声定位系统输出的斜距差和斜距率差、压力传感器测量的潜器深度作差后进行卡尔曼滤波，滤波输出的导航误差估计值反馈修正捷联惯导系统。本发明方法利用斜距差之差、斜距率差之差进行捷联惯导系统长基线水声定位系统紧组合，有效提高了水下潜器的导航定位精度。

主权项：1.一种基于SINSLBL紧组合的水下导航定位方法，其特征在于，该方法步骤如下：步骤1，初始化水下导航定位系统：获取潜器的初始位置参数，获取潜器的角速率和比力信息，进行捷联惯导系统初始对准，获取潜器的初始姿态矩阵和初始速度信息；步骤2，判断声学基阵工作区水听器的可用个数，当水听器的可用个数大于两个时，迭代循环步骤3至步骤5，否则停止迭代，继续判断水听器的可用个数；步骤3，利用声学基阵和捷联惯导系统解算的潜器位置信息，计算出潜器与声学基阵之间的斜距差和斜距率差；捷联惯导系统获取斜距差和斜距率差的具体过程如下：将解算的潜器位置由球面坐标系转换到直角坐标系为xI,yI,zI，计算潜器与声学基阵中各水听器的斜距差为： 式中，xa,ya,za为组成声学基阵的水听器位置坐标，为潜器相对于第j号水听器的斜距与其相对于第k号水听器的斜距之差，其中j、k为水听器编号，且j≠k；将斜距差在潜器位置真值x,y,z处泰勒级数展开，取其一次项得： 式中，为第j号水听器到潜器的真实距离，为第k号水听器到潜器的真实距离，分别为水听器在直角坐标三个轴向的方向余弦之差，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差；对斜距差求导，得到潜器相对于声学基阵中各水听器的斜距率差为： 式中，为潜器相对于第j号水听器的斜距率与其相对于第k号水听器的斜距率之差，变量上方·表示变量的导数；步骤4，利用长基线水声定位系统计算出潜器与声学基阵之间的斜距差和斜距率差；长基线水声定位系统获取斜距差和斜距率差的具体过程如下：潜器与声学基阵中各水听器的斜距差为： 式中，c为水中声速，τjk为潜器上声源到达第j号水听器的时延与其到达第k号水听器的时延之差，δtjk为斜距时延差误差，υδρ为斜距差观测噪声；对上述斜距差求导，得到斜距率差为： 式中，为声波多普勒频移差，λ为声波相位波长，δfjk为斜距时延差变化率误差，为斜距率差观测噪声；步骤5，将所述步骤3中捷联惯导系统获取的斜距差和斜距率差以及捷联惯导系统解算的潜器深度值分别与所述步骤4中长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值作差后进行卡尔曼滤波，并将滤波后输出的导航误差估计值反馈修正捷联惯导系统的导航参数，并输出相应的潜器位置、姿态和速度；作差、滤波及修正的具体步骤如下：步骤501，分别建立长基线水声定位系统和压力传感器状态方程及整个水下导航定位系统状态方程：长基线水声定位系统状态参数选取与时间相关的两个误差δtjk和δfjk，其状态表示为： 式中，δtjk为斜距时延差误差，δfjk为斜距时延差变化率误差，wδt为驱动噪声，τδf和wδf分别为一阶马尔科夫过程的相关时间和驱动噪声；压力传感器深度测量误差δhp状态方程为： 式中，τp为深度误差相关时间，wp为深度误差噪声；整个水下导航定位系统的状态方程描述为： 式中，XI、XL、XP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态变量，FI、FL、FP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态转移矩阵，WI、WL、WP分别为捷联惯导系统、长基线水声定位系统和压力传感器的系统噪声；XI表达式为： 式中，φE、φN和φU为“数学平台”失准角，δvE、δvN和δvU分别表示东北天速度误差，δL、δλ和δh分别表示纬度误差、经度误差和深度误差，εx、εy和εz为陀螺常值漂移，和为加速度计常值偏置，上标T表示矩阵转置，FI可以由捷联惯导系统误差方程得到；XL表达式为：XL＝[δtjkδfjk]T XP表达式为：XP＝δhp 步骤502，建立整个水下导航定位系统观测方程；斜距差之差观测方程为： 是将位置误差由球面坐标系转换到直角坐标系的转换矩阵，表达式为： 式中，RM表示地球卯酉圈曲率半径，L、λ、h分别表示潜器纬度、经度和深度，e表示地球偏心率；将转换矩阵代入斜距差之差观测方程，得斜距差之差观测方程为：Zδρ＝HδρX+Vδρ式中，Zδρ＝[δρjk]N-1×1，Hδρ＝[0N-1×6Hδρ10N-1×6Hδρ20N-1×1]，X＝[XIXLXP]T，Hδρ2＝[-c0]N-1×2，Vδρ＝[-υδρ]N-1×1，N为潜器进入声学基阵工作区水听器的可用个数，N2；斜距率差之差观测方程为： 是速度误差由东北天坐标系转换到直角坐标系的转换矩阵，表达式为： 将转换矩阵代入斜距率差之差观测方程，得斜距率差之差观测方程为： 式中，X＝[XIXLXP]T，深度之差观测方程为：ZP＝HPX+VPhI-hp＝h+δh-h+δhp+υp＝δh-δhp-υp式中，ZP＝hI-hP，HP＝[01×8101×8-1]，X＝[XIXLXP]T，VP＝-υP，hI为捷联惯导系统解算的潜器深度值，hp为压力传感器测量的潜器深度，h为潜器的深度真值，υp为深度观测噪声；整个水下导航定位系统的观测方程为：Z＝HX+V式中，观测量Zδρ、和ZP分别为捷联惯导系统获取的斜距差、斜距率差、潜器深度值与长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵X＝[XIXLXP]T，观测噪声步骤503，将上述得到的斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波，并利用滤波器输出的当前误差最优估计修正捷联惯导系统中的状态量位置校正通过捷联惯导系统的位置解算值与位置误差估计值相减来修正； 式中，Xc是修正后的状态量。

全文数据：

权利要求：

百度查询： 运城学院 一种基于SINS/LBL紧组合的水下导航定位方法及系统

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD