一种无源模式下的SINS/USBL组合导航定位方法技术

一种无源模式下的SINS/USBL组合导航定位方法,涉及组合导航及水声定位技术领域,针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题，本发明专利技术引入深度计信息，设计USBL无源工作模式下的时钟误差的计算方法，补偿USBL单通道的时延误差，以提高组合导航定位的精度。本发明专利技术能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。

【技术实现步骤摘要】
一种无源模式下的SINS/USBL组合导航定位方法
本专利技术涉及组合导航及水声定位
，具体为一种无源模式下的SINS/USBL组合导航定位方法。
技术介绍
USBL水声定位系统的无源工作模式是指声源时钟和测量系统时钟不同步，使USBL声学接收基阵的各个通道测量的声信号传播时延均含有相同的时钟误差。在利用两两通道的时延差时，时钟误差的影响可以相互抵消，而单个通道的时延精度仍受时钟误差的影响，降低了USBL的测距和定位精度。传统的SINS/USBL紧组合导航都是建立在USBL有源工作模式下，以传播时延和时延差(斜距和斜距差)作为数据融合的基础，而对无源模式下的SINS/USBL紧组合导航研究较少。
技术实现思路
本专利技术的目的是：针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题，提出一种无源模式下的SINS/USBL组合导航定位方法。本专利技术为了解决上述技术问题采取的技术方案是：一种无源模式下的SINS/USBL组合导航定位方法，包括以下步骤：步骤一：将USBL声学基阵倒置安装在水下航行器上，并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上，水面布设一个非同步信标，并设置GPS获取信标在导航坐标系下的位置信息；步骤二：建立声学基阵坐标系，并确定基元在基阵坐标系下的位置；步骤三：利用USBL测量声信号在信标和基元之间的传播时延及时延差，获得USBL传播时延量测值及USBL传播时延差量测值，并结合有效声速，解算航行器在基阵坐标系下相对于信标的方向角余弦，所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值；步骤四：获取航行器的深度信息，并结合方向角余弦，判断航行器与信标的相对位置关系是否满足基阵开角条件，若满足，则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值，当出现连续两个采样时刻都满足基阵开角条件时，计算得到连续两个采样时刻的斜距预测值和传播时延预测值，并执行步骤五，若不满足基阵开角条件，则判断是否存在邻近时刻的时钟误差估计值，若存在，则沿用其估计值后执行步骤六，若不存在，则沿用步骤三中得到的USBL传播时延量测值，并执行步骤七，所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角；步骤五：建立时钟误差模型，利用连续两个采样时刻的传播时延预测值与对应的USBL测量的各基元传播时延均值之差，估计误差模型参数，计算时钟误差估计值；步骤六：利用时钟误差估计值，补偿USBL测量的传播时延，得到新的USBL传播时延测量值；步骤七：利用SINS计算水下航行器的姿态和位置，并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下；步骤八：根据SINS转换的基元在导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差，获得SINS传播时延量测值和传播时延差量测值；步骤九：根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值，建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器；步骤十：利用信息融合扩展卡尔曼滤波器计算SINS误差，修正导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。进一步的，所述步骤二中建立声学基阵坐标系的具体步骤为：首先以基阵中心为原点，然后以沿基阵平面指向水下航行器的艏向方向为y轴，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；所述步骤八中导航坐标系为东北天地理坐标系，所述东北天地理坐标系为以水下航行器质心为原点，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，构成右手坐标系。进一步的，所述信息融合扩展卡尔滤波器以SINS的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量，以USBL和SINS的传播时延之差、传播时延差之差作为观测量，在最小均方误差准则下，建立描述系统的状态方程和观测方程。进一步的，所述基元在基阵坐标系下的位置为：其中，i为基元序号，r为基元1，3或基元2，4的间距。进一步的，所述步骤三中方向角余弦为：其中，c为有效声速，d为对应基元间距，为USBL定位系统测量的声信号在各基元和信标间的传播时延，和为传播时延差。进一步的，所述步骤六中利用相邻时刻的传播时延预测值与对应的USBL水声定位系统测量的声信号传播时延均值之差，估计误差模型参数，计算时钟误差估计值，补偿USBL声学基阵的传播时延误差的具体步骤为：首先使USBL无源工作模式下的时钟误差δτ与USBL测量的声信号传播时延τ满足δτ＝aτ+b，a为时钟误差的变化速率，b为一常值误差，联立式式和式计算a和b的估计值和当基阵开角θz的绝对值小于60度时，和保持与邻近更新值不变，计算时钟误差估计值即补偿USBL声学基阵的传播时延误差公式为：其中，τi是基元i的时延测量真值，δτ是时钟误差真值，是补偿的时钟误差，ni是基元i的测量噪声，τ31是基元1,3的时延差真值，n31是基元1,3的时延差测量噪声，τ42是基元2,4的时延差真值，n42是基元4,2的时延差测量噪声。进一步的，所述步骤七的具体步骤为：首先结合基阵坐标系与载体坐标系的安装偏差校准结果，得到基元在导航坐标系下的位置，其中航行器姿态包括航向角A、俯仰角K和横滚角ψ，声学基阵与航行器对应的载体坐标系的角度安装偏差分别为α、β和γ，位置安装偏差为ΔX＝[ΔxΔyΔz]T，载体坐标系到导航坐标系的转换矩阵和声学基阵坐标系到载体坐标系的转换矩阵分别为：则第i个基元在导航坐标系下的位置表示为进一步的，所述步骤八中传播时延为：为SINS计算的基元位置，为基元真实位置，为和之间的误差，为信标位置，c为有效声速；传播时延差为：进一步的，所述步骤九的具体步骤为：SINS位置为纬度L、经度λ和高度h的形式，将其转换为地球直角坐标，则其中，RN为地球卯酉圈主曲率半径，Re为地球半径，e为地球偏心率，其中a,b分别是椭圆长轴和短轴半径；上式的微分形式为：记地球直角坐标系到SINS计算导航坐标系的转换矩阵为：扩展卡尔曼滤波状态变量由SINS姿态误差φ＝[φxφyφz]T，速度误差δν＝[δvxδvyδvz]T，位置误差δp＝[δLδλδh]T，陀螺漂移误差ε＝[εxεyεz]T，加速度计偏置误差组成，即状态方程为Xk+1＝Fk+1/kXk+wk+1其中，Fk+1/k为状态转移矩阵，由SINS的误差方程获得，wk+1为扩展卡尔滤波过程噪声矢量；扩展卡尔曼滤波的观测量为观测方程为Zk+1＝Hk+1Xk+1+vk+1其中，vk+1本文档来自技高网...

【技术保护点】
1.一种无源模式下的SINS/USBL组合导航定位方法，其特征在于包括以下步骤：/n步骤一：将USBL声学基阵倒置安装在水下航行器上，并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上，水面布设一个非同步信标，并设置GPS获取信标在导航坐标系下的位置信息；/n步骤二：建立声学基阵坐标系，并确定基元在基阵坐标系下的位置；/n步骤三：利用USBL测量声信号在信标和基元之间的传播时延及时延差，获得USBL传播时延量测值及USBL传播时延差量测值，并结合有效声速，解算航行器在基阵坐标系下相对于信标的方向角余弦，所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值；/n步骤四：获取航行器的深度信息，并结合方向角余弦，判断航行器与信标的相对位置关系是否满足基阵开角条件，若满足，则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值，当出现连续两个采样时刻都满足基阵开角条件时，计算得到连续两个采样时刻的斜距预测值和传播时延预测值，并执行步骤五，若不满足基阵开角条件，则判断是否存在邻近时刻的时钟误差估计值，若存在，则沿用其估计值后执行步骤六，若不存在，则沿用步骤三中得到的USBL传播时延量测值，并执行步骤七，所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角；/n步骤五：建立时钟误差模型，利用连续两个采样时刻的传播时延预测值与对应的USBL测量的各基元传播时延均值之差，估计误差模型参数，计算时钟误差估计值；/n步骤六：利用时钟误差估计值，补偿USBL测量的传播时延，得到新的USBL传播时延测量值；/n步骤七：利用SINS计算水下航行器的姿态和位置，并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下；/n步骤八：根据SINS转换的基元在导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差，获得SINS传播时延量测值和传播时延差量测值；/n步骤九：根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值，建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器；/n步骤十：利用信息融合扩展卡尔曼滤波器计算SINS误差，修正导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。/n...

【技术特征摘要】
1.一种无源模式下的SINS/USBL组合导航定位方法，其特征在于包括以下步骤：
步骤一：将USBL声学基阵倒置安装在水下航行器上，并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上，水面布设一个非同步信标，并设置GPS获取信标在导航坐标系下的位置信息；
步骤二：建立声学基阵坐标系，并确定基元在基阵坐标系下的位置；
步骤三：利用USBL测量声信号在信标和基元之间的传播时延及时延差，获得USBL传播时延量测值及USBL传播时延差量测值，并结合有效声速，解算航行器在基阵坐标系下相对于信标的方向角余弦，所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值；
步骤四：获取航行器的深度信息，并结合方向角余弦，判断航行器与信标的相对位置关系是否满足基阵开角条件，若满足，则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值，当出现连续两个采样时刻都满足基阵开角条件时，计算得到连续两个采样时刻的斜距预测值和传播时延预测值，并执行步骤五，若不满足基阵开角条件，则判断是否存在邻近时刻的时钟误差估计值，若存在，则沿用其估计值后执行步骤六，若不存在，则沿用步骤三中得到的USBL传播时延量测值，并执行步骤七，所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角；
步骤五：建立时钟误差模型，利用连续两个采样时刻的传播时延预测值与对应的USBL测量的各基元传播时延均值之差，估计误差模型参数，计算时钟误差估计值；
步骤六：利用时钟误差估计值，补偿USBL测量的传播时延，得到新的USBL传播时延测量值；
步骤七：利用SINS计算水下航行器的姿态和位置，并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下；
步骤八：根据SINS转换的基元在导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差，获得SINS传播时延量测值和传播时延差量测值；
步骤九：根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值，建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器；
步骤十：利用信息融合扩展卡尔曼滤波器计算SINS误差，修正导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。


2.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤二中建立声学基阵坐标系的具体步骤为：
首先以基阵中心为原点，然后以沿基阵平面指向水下航行器的艏向方向为y轴，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；
所述步骤八中导航坐标系为东北天地理坐标系，所述东北天地理坐标系为以水下航行器质心为原点，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，构成右手坐标系。


3.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述信息融合扩展卡尔滤波器以SINS的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量，以USBL和SINS的传播时延之差、传播时延差之差作为观测量，在最小均方误差准则下，建立描述系统的状态方程和观测方程。


4.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述基元在基阵坐标系下的位置为：



其中，i为基元序号，r为基元1，3或基元2，4的间距。


5.根据权利要求4所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤三中方向角余弦为：



其中，c为有效声速，d为对应基元间距，为USBL定位系统测量的声信号在各基元和信标间的传播时延，和为传播时延差。


6.根据权利要求5所述的一种无源模式下的SINS/USBL组合导航定位方法，其特征在于所述步骤六中利用相邻时刻的传播时延预测值与对应的USBL水声定位系统测量的声信号传播时延均值之差，估计误差模型参数，计算时钟误差估计值，...

【专利技术属性】
技术研发人员：郑翠娥，刘云，孙大军，张居成，韩云峰，崔宏宇，张殿伦，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23