一种引入径向速度的SINS/USBL紧组合导航定位方法与流程

本发明涉及sins/usbl紧组合导航定位方法，属于组合导航及水声定位技术领域。

背景技术：

传统的sins/usbl紧组合建立在以传播时延(斜距)之差和时延差(斜距差)之差为观测量的基础上。径向速度是水下航行器与参考信标在视在方向的运动速度，描述了水下航行器与参考信标之间的距离变化率大小，径向速度与航行器的运动速度直接相关。usbl能够通过计算基元接收信号与发射信号间的多普勒频率，获得基元与信标之间的径向速度。通过引入径向速度既能够提高滤波系统对速度误差的估计精度，又能保证大开角下紧组合导航的稳定性，改善整体导航性能，而传统的sins/usbl紧组合导航忽略了usbl计算的径向速度这一冗余测量，导致在传统的紧组合滤波系统中，速度误差的可观测性较弱，数据融合结果对sins的速度误差修正效果相对较差，且降低了在大开角情况下，sins/usbl紧组合的导航精度和稳定性。

技术实现要素：

本发明的目的是为了提高现有传统sins/usbl紧组合的速度误差估计精度和大开角情况下的sins/usbl紧组合稳定性，而提出一种引入径向速度的sins/usbl紧组合导航定位方法。

一种引入径向速度的sins/usbl紧组合导航定位方法具体过程为：

步骤一、将usbl声学基阵倒置安装在水下航行器上，sins的陀螺组件和加速度计组件固联在水下航行器上；

所述usbl为超短基线定位系统；sins为捷联惯性导航系统；

水面布放一个同步信标，信标在导航坐标系n下的位置通过gps获得；

所述声学基阵是由多个基元均匀分布的一个圆柱形设备；

所述多个为大于等于3个；

所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；

步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；

步骤三、usbl测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,i′，各基元在径向方向相对信标的运动速度为

步骤四、根据sins计算水下航行器的姿态、速度和位置；

结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置；

步骤五、结合声信号在航行器与信标之间传播的有效声速，根据sins转换的基元在计算导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差；

步骤六、将sins解算的航行器运动速度沿径向分解；

步骤七、建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程；

步骤八、计算sins误差，修正sins输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。

本发明的有益效果为：

本发明将usbl定位系统计算的水下航行器相对声学信标在径向方向的运动速度引入传统的sins/usbl紧组合中，增强滤波系统中速度误差的可观测性，提高速度误差的估计精度，最终实现组合导航速度和位置精度的提高；同时提高sins/usbl紧组合在大开角情况下的导航精度和稳定性。

附图说明

图1为sins/usbl紧组合导航定位数据融合原理图；

图2为基阵坐标系、载体坐标系和导航坐标系的示意简图。

具体实施方式

具体实施方式一：本实施方式一种引入径向速度的sins/usbl紧组合导航定位方法具体过程为：

步骤一、将usbl声学基阵倒置安装在水下航行器上，sins的陀螺组件和加速度计组件固联在水下航行器上；

所述usbl为超短基线定位系统；sins为捷联惯性导航系统；

水面布放一个同步信标，信标在导航坐标系n下的位置通过gps获得；

所述声学基阵是由多个基元(声信号接收换能器)均匀分布的一个圆柱形设备；

所述多个为大于等于3个；

所述导航坐标系是指“东北天”(“enu”)地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；

步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；

步骤三、usbl测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,i′，各基元在径向方向相对信标的运动速度为

步骤四、根据sins计算水下航行器的姿态、速度和位置(在这里把sins当成一个黑匣子，它输出水下航行器的姿态、速度和位置)；

结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果(和)，得到基元在计算导航坐标系下的位置；

步骤五、结合声信号在航行器与信标之间传播的有效声速(声速剖面仪(svp)测量)，根据sins转换的基元在计算导航坐标系中的位置信息(式5)，计算声信号在各基元与信标间的传播时延和传播时延差；

步骤六、将sins解算的航行器运动速度(sins的输出)沿径向分解；

步骤七、建立基于传播时延、传播时延差和径向速度信息(公式9、11、12)融合的扩展卡尔曼滤波器状态方程和观测方程；

步骤八、计算sins误差，修正sins输出，重置扩展卡尔曼滤波器状态为零(所述重置滤波状态变量是指，在修正sins输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。)，重新执行步骤三～步骤八，直至导航结束。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤二中建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；具体过程为：

声学基阵坐标系是指以基阵中心为原点，y轴沿基阵平面指向水下航行器的艏向方向，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；

基元在声学基阵坐标系下的坐标为：

式(1)中，i为基元序号，i′为基元总数，为基元i在声学基阵坐标系x轴下的坐标，r为基线长度，为基元i在声学基阵坐标系y轴下的坐标，为基元i在声学基阵坐标系z轴下的坐标。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤三中usbl测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,i′，各基元在径向方向相对信标的运动速度为具体过程为：

将和表示为：

式中，τi为第i个基元测量的传播时延真值，ni为第i个基元的测量噪声，τ1j为第j个基元测量的传播时延与1号基元测量的传播时延差真值，n1j为时延差测量噪声，vri为第i个基元测量的径向速度真值，nri为第i个基元的径向速度测量噪声。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤四中根据sins计算水下航行器的姿态、速度和位置(在这里把sins当成一个黑匣子，它输出水下航行器的姿态、速度和位置)；

结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果(和)，得到基元在计算导航坐标系下的位置；具体过程为：

建立水下航行器坐标系obxbybzb，水下航行器坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，水下航行器坐标系的定义满足右手定则；

所述航行器姿态包括航向角a、俯仰角k和横滚角ψ，根据航行器姿态复现(对真实导航系的一个跟踪)的导航坐标系称计算导航坐标系n'。

声学基阵坐标系相对于水下航行器坐标系的角度安装偏差分别为α、β和γ(基阵坐标系的三个坐标轴与水下航行器坐标系的三个坐标轴的角度偏差)，声学基阵坐标系原点相对于水下航行器坐标系原点的位置偏差为：

其中，δxb、δyb和δzb为中的分量，上角标t代表转置；

水下航行器坐标系到计算导航坐标系n'的转换矩阵和声学基阵坐标系到水下航行器坐标系的转换矩阵分别为：

则第i个基元在计算导航坐标系下的位置表示为

式中，为第i个基元在声学基阵坐标系下的位置，为sins计算的水下航行器位置；

由于sins的计算导航坐标系n'与导航坐标系n的三个坐标轴之间存在角度误差φ＝[φxφyφz]^t，称失准角误差，计算导航坐标系n'与导航坐标系n之间的转换矩阵可近似为

其中

式中，φ＝[φxφyφz]^t为失准角误差(sins姿态误差)，i为三阶单位阵；φx、φy、φz为φ中的分量；

第i个基元在sins计算导航坐标系下的位置与在真实导航坐标系(建立的导航坐标系)下位置值之间的误差为

式中，δxi是与水下航行器在真实导航坐标系下位置的差；记

是在计算导航坐标系中第i个基元相对水下航行器质心的位置。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤五中结合声信号在航行器与信标之间传播的有效声速(声速剖面仪(svp)测量)，根据sins转换的基元在计算导航坐标系中的位置信息(式5)，计算声信号在各基元与信标间的传播时延和传播时延差；具体过程为：

声信号在第i个基元与信标之间的传播时延预测值为：

式中，c是测量的有效声速，是信标在真实导航坐标系中的位置，由gps提供的位置信息转换得到，||·||是矩阵二范数的表示；

其中，为信标在真实导航坐标系的xn轴方向位置，为信标在真实导航坐标系的yn轴方向位置，为信标在真实导航坐标系的zn轴方向位置，为第i个基元在计算导航坐标系的xn轴方向位置，为第i个基元在计算导航坐标系的yn轴方向位置，为第i个基元在计算导航坐标系的zn轴方向位置；

声信号的传播时延差为：

式中，为sins计算的第j个基元传播时延预测值，为sins计算的第1个基元传播时延预测值。

所述有效声速为信标和接收基阵的几何距离与声信号在两点(信标和接收基阵)间的传播时间的比值。

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述步骤六中将sins解算的航行器运动速度(sins的输出)沿径向分解；具体过程为：

将sins解算的航行器运动速度沿径向分解为：

其中：为第i个基元相对于信标在径向方向的运动速度大小，为第i个基元在计算导航坐标系xn′轴方向位置，为第i个基元在计算导航坐标系yn′轴方向位置，为第i个基元在计算导航坐标系zn′轴方向位置，为水下航行器在计算导航坐标系的xn′轴方向速度，为水下航行器在计算导航坐标系的yn′轴方向速度，为水下航行器在计算导航坐标系的zn′轴方向速度，

式中，cosζx、cosζy、cosζz为中间变量。

其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：所述步骤七中建立基于传播时延、传播时延差和径向速度信息(公式9、11、12)融合的扩展卡尔曼滤波器状态方程和观测方程；具体过程为：

所述信息融合扩展卡尔滤波器是指以失准角误差、sins的速度误差、位置误差、陀螺漂移、加速度计偏置为状态变量，以sins和usbl的传播时延之差、传播时延差之差以及径向速度之差作为观测量，建立描述系统的状态方程和观测方程。

组合导航一般有反馈校正的闭环方式和输出修正的开环方式，前者工程实现复杂，当滤波器有故障时会直接影响sins的输出，而输出修正不涉及独立的导航系统内部，容错能力较强，因此本发明采用输出修正方式；

失准角误差φ＝[φxφyφz]^t，sins速度误差δν＝[δvxδvyδvz]^t(速度误差是由sins解算的计算导航坐标系下的水下航行器运动速度与真实速度的差)，sins位置误差δp＝[δlδλδh]^t(位置误差是sins解算的水下航行器在地球坐标系下的位置与真实位置的差值)，陀螺漂移误差ε＝[εxεyεz]^t，加速度计偏置误差

则

其中，x为扩展卡尔曼滤波系统的状态变量；

扩展卡尔曼滤波系统的状态方程为

xk+1＝fk+1/kxk+wk+1(15)

其中，xk为k时刻的状态变量，xk+1为k+1时刻的状态变量，fk+1/k为状态转移矩阵，由sins的误差方程获得，wk+1为扩展卡尔曼滤波系统过程噪声序列，通常为高斯白噪声形式；

扩展卡尔曼滤波系统的观测量z为：

观测方程为

zk+1＝hk+1xk+1+vk+1(17)

其中，zk+1为k+1时刻的观测量，vk+1为扩展卡尔曼滤波系统观测噪声序列，通常为高斯白噪声形式，hk+1为k时刻的观测矩阵，为声信号在第i个基元与信标之间的传播时延预测值；

观测矩阵hk+1为

式中，cipφ为中间变量，01×3为3维零向量，cipp为中间变量，为地球直角坐标系到sins计算导航坐标系的转换矩阵，a为中间变量矩阵，01×6为6维零向量，cjpφ为第j个基元对应的中间变量，j≠1，c1pφ为第1个基元对应的中间变量，cjpp为第j个基元对应的中间变量，j≠1；c1pp为第1个基元对应的中间变量，civv为中间变量，cipv为中间变量。

其它步骤及参数与具体实施方式一至六之一相同。

具体实施方式八：本实施方式与具体实施方式一至七之一不同的是：所述观测矩阵hk+1具体推算过程为：

sins解算的水下航行器位置表示为纬度l、经度λ和高度h的球坐标形式，再将纬度l、经度λ和高度h转换到地球直角坐标系下；

地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则

式中，x′e、y′e和z′e分别为sins解算的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，rn为地球卯酉圈主曲率半径，re为地球半径，e为地球偏心率，a和b分别是地球椭圆长轴和短轴半径；

式(18)的微分转换关系为：

地球直角坐标系到sins计算导航坐标系的转换矩阵为：

记中间变量矩阵a为：

结合式(7)和式(8)，得到式(9)的全微分形式：

式中，[dxn′dyn′dzn′]^t是sins计算的水下航行器位置与真实位置在三个轴方向的位置误差；是中的坐标分量；为的微分，c为有效声速，是沿计算导航坐标系xn′轴、yn′轴和zn′轴分量；

记

式中，cipp、cipφ为中间变量；

式(12)的全微分为

式中，是sins计算的水下航行器在导航坐标系下三个方向的速度误差；civv、cipv为中间变量；

civv＝[cosζxcosζycosζz](26)

式中，为水下航行器在计算导航坐标系下三个方向的速度；

观测矩阵hk+1为

式中，cipφ为中间变量，01×3为3维零向量，cipp为中间变量，为地球直角坐标系到sins计算导航坐标系的转换矩阵，a为中间变量矩阵，01×6为6维零向量，cjpφ为第j个基元对应的中间变量，j≠1，c1pφ为第1个基元对应的中间变量，cjpp为第j个基元对应的中间变量，j≠1；c1pp为第1个基元对应的中间变量，civv为中间变量，cipv为中间变量。

其它步骤及参数与具体实施方式一至七之一相同。

具体实施方式九：本实施方式与具体实施方式一至八之一不同的是：所述步骤八中计算sins误差，修正sins输出，重置扩展卡尔曼滤波器状态为零(所述重置滤波状态变量是指，在修正sins输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。)，重新执行步骤三～步骤八，直至导航结束；具体过程为：

计算sins的导航误差，具体过程为：

根据计算k+1时刻的状态预测值

式中，为k时刻的滤波状态变量估计值；

根据pk+1/k＝fk+1/kpkf^tk+1/k+qk，计算k+1时刻的状态预测误差协方差矩阵pk+1/k；

式中，pk为k时刻的状态估计误差协方差矩阵，qk为k时刻的系统过程噪声协方差矩阵；

根据kk+1＝pkh^tk+1(hk+1pkh^tk+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；

式中，i为三阶单位阵；

根据计算k+1时刻的状态估计值

根据修正sins输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。

其它步骤及参数与具体实施方式一至八之一相同。

所述扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理；

所述卡尔滤波器是指在最小均方误差准则下的线性贝叶斯估计；

所述修正sins输出是指在sins输出的vⁿ'、上减去状态变量的对应的估计值(δv、δp)，在航向角a、俯仰角k和横滚角ψ上，将φ结合姿态算法进行修正，得到新的惯性导航输出；

所述sins输出为航向角a、俯仰角k和横滚角ψ，航行器运动速度v^n'、航行器位置

状态变量的估计值为

所述重置扩展卡尔曼滤波器状态变量是指在修正sins输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。

所述信息融合扩展卡尔滤波器是指以sins的姿态误差、速度误差、位置误差、陀螺漂移、加速度计偏置为状态变量，以usbl和sins的传播时延之差、传播时延差之差以及径向速度之差作为观测量，建立描述系统的状态方程和观测方程。

本发明首次将usbl测量的水下航行器相对信标在径向方向的运动速度引入sins/usbl紧组合，以增强组合导航滤波器中速度误差的可观测性，进一步提高组合导航精度。

采用以下实施例验证本发明的有益效果：

实施例一：

本实施例具体是按照以下步骤制备的：

1.将超短基线声学基阵和捷联惯性导航的陀螺组件、加速度计组件安装在水下航行器上。

2.附图2中基元1-5在基阵坐标系下的坐标为：

式(29)中，i为基元序号，为基元i在声学基阵坐标系x轴下的坐标，r为基线长度，为基元i在声学基阵坐标系y轴下的坐标，为基元i在声学基阵坐标系z轴下的坐标；

3.超短基线定位系统测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,3,4,5，径向速度为

将和表示为：

式中，τi为第i个基元测量的传播时延真值，ni为第i个基元的测量噪声，τ1j为第j个基元测量的传播时延与1号基元测量的传播时延差真值，n1j为时延差测量噪声，vri为第i个基元测量的径向速度真值，nri为第i个基元的径向速度测量噪声；

4.航行器姿态包括航向角a、俯仰角k和横滚角ψ

声学基阵坐标系相对于水下航行器坐标系的角度安装偏差分别为α、β和γ(基阵坐标系的三个坐标轴与载体坐标系的三个坐标轴的角度偏差)，声学基阵坐标系原点相对于水下航行器坐标系原点的位置偏差为：

其中，δxb、δyb和δzb为中的分量，上角标t代表转置；

水下航行器坐标系到计算导航坐标系n'的转换矩阵和声学基阵坐标系到水下航行器坐标系的转换矩阵分别为：

则第i个基元在计算导航坐标系下的位置表示为

由于捷联惯性导航计算的导航坐标系n'与真实导航坐标系n存在角度误差φ＝[φxφyφz]^t，二者之间的转换矩阵可近似为

其中

因此基元在导航坐标系下的位置误差为

5.第i个基元在sins计算导航坐标系下的位置与在真实导航坐标系(建立的导航坐标系)下位置之间的误差为：

式(37)中，c是有效声速，||·||是矩阵二范数的表示，即

声信号的传播时延差预测值为：

6.捷联惯性导航计算的航行器运动速度在径向的分解为：

记

7.建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程。

惯性导航位置表示为纬度l、经度λ和高度h的形式，需要将其转换为直角坐标，则

式(42)中，rn为地球卯酉圈主曲率半径其中re为地球半径；e为地球偏心率，其中a,b分别是椭圆长轴和短轴半径。

组合导航一般有反馈校正的闭环方式和输出修正的开环方式，前者工程实现复杂，当滤波器有故障时会直接影响捷联惯性导航的输出，而输出修正不涉及独立的导航系统内部，容错能力较强，因此本发明采用输出修正方式。

扩展卡尔曼滤波状态变量由捷联惯性导航姿态误差φ＝[φxφyφz]^t，速度误差δν＝[δvxδvyδvz]^t，位置误差δp＝[δlδλδh]^t，陀螺漂移误差ε＝[εxεyεz]^t，加速度计偏置误差组成，简记为

状态方程为

xk+1＝fk+1/kxk+wk+1(44)

其中fk+1/k为状态转移矩阵，由捷联惯性导航系统的误差方程获得。wk+1为高斯白噪声序列。

导航坐标系与地球坐标系的微分转换关系如下所示

记

地球直角坐标系到sins计算导航坐标系的转换矩阵为：

扩展卡尔曼滤波的观测量为

观测方程为

zk+1＝hk+1xk+1+vk+1(49)

其中vk+1为高斯白噪声序列。

结合式(36)，式(37)的全微分表示为：

记

式(40)的全微分为

其中

civv＝[cosζxcosζycosζz](54)

观测矩阵hk+1为

8.计算捷联惯性导航的导航误差，滤波解算过程如下。

根据计算k+1时刻的状态预测值

根据pk+1/k＝fk+1/kpkf^tk+1/k+qk计算k+1时刻的状态预测误差协方差矩阵pk+1/k。

其中pk为k时刻的状态估计误差协方差矩阵，qk为k时刻的系统过程噪声协方差矩阵。

根据kk+1＝pkh^tk+1(hk+1pkh^tk+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时刻的状态估计值

根据的计算结果修正捷联惯性导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤3～步骤8，直至导航结束。

实施例2

本发明中，扩展卡尔曼滤波器的状态方程是由捷联惯性导航系统的误差方程构成，但不仅限于此，如在状态方程中加入超短基线的测时误差和安装偏差等。

本发明中，时延和时延差观测信息与斜距、斜距差信息意义等价。

本发明是基于一个同步信标，但不仅限于此，多个同步信标或应答器均可采用上述方法。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。