一种无源模式下的sins/usbl组合导航定位方法
阅读说明：本技术 一种无源模式下的sins/usbl组合导航定位方法 (SINS/USBL combined navigation positioning method in passive mode ) 是由 郑翠娥 刘云 孙大军 张居成 韩云峰 崔宏宇 张殿伦 于 2020-03-04 设计创作，主要内容包括：一种无源模式下的SINS/USBL组合导航定位方法,涉及组合导航及水声定位技术领域,针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题,本发明引入深度计信息,设计USBL无源工作模式下的时钟误差的计算方法,补偿USBL单通道的时延误差,以提高组合导航定位的精度。本发明能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。(Aiming at the problem that SINS/USBL tightly combined navigation positioning precision is low in the prior art, the SINS/USBL combined navigation positioning method in the passive mode introduces depth meter information, designs a clock error calculation method in the USBL passive working mode, and compensates time delay error of a USBL single channel so as to improve the precision of combined navigation positioning. The invention can improve the SINS/USBL tight combination navigation positioning precision in the limited range of the array opening angle.)
技术领域
本发明涉及组合导航及水声定位技术领域，具体为一种无源模式下的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为一常值误差，联立式式和式
当基阵开角θz的绝对值小于60度时，和
其中，τ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位置为纬度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为高斯白噪声序列，
传播时延的全微分表示为：
记
观测矩阵Hk+1为
进一步的，所述步骤十的具体步骤为：
步骤十一：根据
步骤十二：根据Pk+1/k＝Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k，其中Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵；
步骤十三：根据Kk+1＝PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1，其中Rk+1和Hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵；
步骤十四：根据Pk+1＝(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1；
步骤十五：根据计算k+1时刻的状态估计值
步骤十六：根据
本发明的有益效果是：
本发明引入深度计信息，设计USBL无源工作模式下的时钟误差的计算方法，补偿USBL单通道的时延误差，以提高组合导航定位的精度。本发明能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。
附图说明
图1为无源模式下USBL时钟误差估计原理图；
图2为航行器与信标在基阵坐标系下的相对位置关系图；
图3为基阵坐标系、载体坐标系和导航坐标系的示意简图。
具体实施方式
具体实施方式一：参照图具体说明本实施方式，本实施方式所述的一种无源模式下的 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轴构成右手坐标系。
所述基阵开角是指在基阵坐标系原点与信标的连线与z轴正向的夹角。
所述导航坐标系是指“东北天(ENU)”地理坐标系。
所述“东北天(ENU)”地理坐标系是指以载体质心为原点，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，构成右手坐标系。
所述有效声速为信标和接收基阵的几何距离与声信号在两点间的传播时间的比值。
所述信息融合扩展卡尔滤波器是指以捷联惯性导航的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量，以超短基线和捷联惯性导航的传播时延之差、传播时延差之差作为观测量，在最小均方误差准则下，建立描述系统的状态方程和观测方程，对状态变量进行估计。
所述扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理。
所述重置滤波状态变量是指，在修正捷联惯性导航输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。
本发明中所述的USBL声学基阵为多个声信号接收换能器均匀设置，声信号接收换能器一般大于或等于3个。
本发明首次研究无源模式SINS/USBL紧组合导航中USBL时钟误差的估计与补偿方法，以提高USBL的时延测量精度和SINS/USBL紧组合导航定位精度。
本发明时钟误差的估计精度虽受USBL基阵开角的影响，但能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。
针对每个步骤的具体说明如下：
步骤一：将超短基线声学基阵和捷联惯性导航的陀螺组件、加速度计组件以及深度计安装在水下航行器上，其中信标可以在水下，USBL基阵可以不倒置。
步骤二：结合附图2，图2中基元1-4在基阵坐标系下的坐标为：
式(1)中，i为基元序号，r为基元1,3或基元2,4的间距。图中a为基阵坐标系，b 为载体坐标系，n为导航坐标系。
步骤三：超短基线定位系统测量的声信号在各基元和信标间的传播时延为
τi是基元i的时延测量真值，δτ是时钟误差真值，ni是基元i的测量噪声，τ31是基元1,3的时延差真值，n31是基元1,3的时延差测量噪声，τ42是基元2,4的时延差真值，n42是基元4,2的时延差测量噪声。
附图2的基阵坐标系中，航行器相对于信标的方向角余弦为
式(3)中，c为有效声速，d为对应基元间距。
步骤四：t1时刻深度计辅助下的预测斜距和预测传播时延分别为：
步骤五：重复步骤三，t2时刻深度计辅助下的预测斜距和预测传播时延分别为：
步骤六：USBL无源工作模式下的时钟误差δτ与USBL测量的声信号传播时延τ满足如下关系：
δτ＝aτ+b (8)
a为时钟误差的变化速率，b为一常值误差。联立式(2)、式(5)和式(7)计算a和 b的估计值
θz在60°内，采用深度计辅助预测的USBL斜距信息和时延信息才是有效的。当基阵开角θz不满足条件时，参数和保持与邻近更新值不变。
计算时钟误差估计值
步骤七：捷联惯性导航解算水下航行器在导航坐标系下的姿态、速度和位置。根据姿态信息，得到SINS对导航坐标系的复现，称为计算导航坐标系。结合基阵坐标系与载体坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置。
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
其中航行器姿态包括航向角A、俯仰角K和横滚角ψ，声学基阵坐标系与水下航行器载体坐标系对应的三个轴的角度安装偏差分别为α、β和γ，坐标系原点在载体坐标系下的位置安装偏差为ΔX＝[Δx Δy Δz]T。载体坐标系到计算导航坐标系的转换矩阵
则第i个基元在计算导航坐标系下的位置表示为
由于捷联惯性导航计算的导航坐标系n'与真实导航坐标系n存在角度误差φ＝[φx φy φz]T，二者之间的转换矩阵可近似为
其中
因此基元在计算导航坐标系下的位置与位置真值
ΔXi是
是在计算导航坐标系中第i个基元相对载体坐标系原点的位置，是沿计算导航坐标系xn′轴、yn′轴和zn′轴分量。
步骤八声信号在第i个基元与信标之间的传播时延为：
式(18)中，为SINS计算的基元位置，
||·||是矩阵二范数的表示，即
其中，为信标在真实导航坐标系的xn轴方向位置，为信标在真实导航坐标系的yn轴方向位置，为信标在真实导航坐标系的zn轴方向位置，
声信号的传播时延差为：
步骤九：建立基于传播时延、传播时延差融合的扩展卡尔曼滤波器状态方程和观测方程。
首先SINS位置为纬度L、经度λ和高度h的形式，将其转换为地球直角坐标，地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则
其中，RN为地球卯酉圈主曲率半径
上式的微分形式为：
记
地球直角坐标系到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 (24)
其中Fk+1/k为状态转移矩阵，由SINS的误差方程获得。wk+1为扩展卡尔滤波过程噪声矢量；
扩展卡尔曼滤波的观测量为
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (26)
其中vk+1为高斯白噪声序列。
结合式(16)，式(17)的全微分表示为：
观测矩阵Hk+1为
步骤十：计算捷联惯性导航的导航误差，滤波解算过程如下。
根据计算k+1时刻的状态预测值
根据Pk+1/k＝Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k，其中Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵。
根据Kk+1＝PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1，其中Rk+1和Hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵。
根据Pk+1＝(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1。
根据
根据的计算结果修正捷联惯性导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤三至步骤十。
需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：在自动代客泊车系统中用于估计位置的设备及方法