1.本发明属于组合导航系统技术，特别适用于水下组合导航领域，具体涉及一种基于格网水流速度辅助的惯性/多普勒中水层导航方法。
背景技术：2.为利用丰富的海洋资源，海洋探索成为人类发展的必修课题。海洋探索依托水下航行器，其导航定位能力是保证任务有效性的前提与基础。在众多水下导航系统中， sins(即：捷联惯性导航系统)/dvl(即：多普勒测速仪)组合导航系统以其自主性、高精度、部署简单等特点成为了水下组合导航系统核心模块。但现有sins/dvl组合导航系统要求dvl持续工作在底跟踪(dvl-bt)模式，其在中水层区域的导航解决方案有待探索研究。
3.在较深海域，sins/dvl系统在水下航行器下潜、上浮过程中均会经历gnss及 dvl-bt均无效的中水层区域，在此区域dvl只能得到对水测速(dvl-wt)观测数据。为了提高在中水层区域的导航精度，亟需提出一种基于水流速度辅助sins/dvl的中水层导航方法。
技术实现要素：4.为了提高载体在中水层的导航精度，本发明提出了一种基于格网水流速度辅助的惯性/多普勒中水层导航方法。
5.为实现上述目的，本发明采取的技术方案是：
6.一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，包括以下步骤：
7.s1：基于组合导航系统为sins/gnss或者sins/dvl-bt系统，并初始化载体姿态、速度、位置信息；对航行区域进行交错格网划分；
8.s2：获取到有效dvl-wt量测时，根据当前载体解算位置与dvl-wt各水层量测深度判断各dvl-wt量测对应的格网区域；
9.s3:判断各dvl-wt量测对应格网区域水速是否已被估计，若已估计，利用该层对水测速结果进行量测更新；若未被估计，不进行量测更新，同时，利用组合后结果及当前各 dvl-wt量测，进行相应格网水速估计；
10.s4：在dvl-wt量测之间，进行惯性捷联解算，更新姿态速度位置微分方程，持续获得载体姿态速度位置信息；
11.s5：重复步骤s2到s4直至组合过程结束。
12.作为本发明进一步改进，所述步骤s1具体包括以下过程：
13.当载体起始于水面时，以sins/gnss组合结果作为载体初始状态信息；当载体起始于 dvl-bt有效水域时，初以sins/dvl-bt组合结果作为载体初始状态信息；
14.对即将航行的水域进行交错格网划分，根据dvl设备对水测速分层特性，将水域从深度0m处从上至下以bh作为层高进行分层。在一层内，以东向be、北向bn结合层高bh进行长方体划分，形成连续扩展格网拼接，在不同层之间，下一层相较于上一层，格网交错点向东
向移动ae、北向移动an，以此类推，形成交错格网水域划分。
15.作为本发明进一步改进，所述步骤s2具体包括以下过程：
16.获取到有效dvl-wt观测时，不同水层dvl-wt观测所对应的格网号(i，j，k)通过以下步骤确定；
17.以某一水层为例：假设该水层为当前时刻dvl设备对水测速的第n层，
[0018][0019][0020][0021]
λk＝λ1+(k-1)ae[0022]
lk＝l1+(k-1)an[0023]
其中为向下取整，λ、l和h为当前载体所在位置的经度、纬度和高度表示，λk和lk为该水域第k层格网的格网号计算起始经纬度，rm和rn分别为子午圈与卯酉圈半径。
[0024]
作为本发明进一步改进，所述步骤s3具体包括以下过程：
[0025]
s3.1获取到有效dvl-wt观测时，判断各dvl-wt量测对应格网区域水速是否已被估计，若已估计，建立dvl对水测速误差模型：
[0026][0027]
其中为dvl-wt对格网(i，j，k)测速观测值，为相应测速真值，w
d_c(i，j，k)
为相应观测噪声，δk为dvl标度因子误差；
[0028]
利用该层对水测速结果构建量测方程：
[0029][0030][0031]zwt
＝[z
wt(i1，j1，k1)
ꢀ…ꢀzwt(in，jn，kn
)]
t
[0032]
其中n为导航坐标系，以地理坐标系作为导航坐标系，b为载体坐标系，为sins捷联解算的速度，为载体姿态角对应旋转矩阵，n系至b系，为格网(i，j，k)水速估计值，为失准角对应旋转矩阵，φ
x
，φy，φz为各轴向的失准角，δ为相关量误差，vn为n系下载体速度，为相应速度真值，υ
c(i，j，k)
为格网(i，j，k)水速估计噪声，φn为失准角， (
×
)为对应斜对称矩阵；
[0033]
根据sins捷联解算姿态、速度、位置微分方程及误差模型，以及dvl对水测速误差模型建立sins/dvl-wt系统方程，选取状态变量：
[0034][0035]
其中δve，δvn，δvu为东北天方向的速度误差，δl，δλ，δh为各轴向的位置误差，为各轴向的加速度误差，ε
x
，εy，εz为各轴向的陀螺仪误差；
[0036]
构建系统状态方程：
[0037][0038]
f和g根据sins及dvl-wt误差模型获得；
[0039]
基于量测方程与系统方程，进行滤波估计，获得状态量x估计结果，补偿至sins系统形成反馈校正；
[0040]
s3.2利用组合后高精度结果及当前各dvl-wt量测，进行相应格网水速估计；
[0041]
构造格网水流速度状态向量xc：
[0042][0043]
其中为各格网水流待估计值，n1，n2，n3为三个方向格网最大值；
[0044]
根据sins/gnss或sins/dvl-bt高精度组合导航信息，以及dvl-wt多水层量测值，构建格网水速观测方程：
[0045][0046][0047]
其中观测矩阵h
c(i，j，k)
为单位阵，hc由h
c(i，j，k)
组成，m为此次观测到的有效格网水速的数量，(im，jm，km)为第m个观测对应的格网号(i，j，k)。
[0048]
根据格网内水速在短时间内变化不大的假设，计算卡尔曼滤波状态矢量及其协方差的一步预测：
[0049][0050]
p
c(k，k-1)
＝p
c(k-1))
[0051]
计算卡尔曼增益矩阵：
[0052][0053]
其中rc为测量噪声协方差矩阵；
[0054]
计算卡尔曼滤波残差：
[0055]
ζ
c(k)
＝z
c(k)-h
c(k)
x
c(k，k-1)
[0056]
计算卡尔曼滤波更新状态估计及其协方差：
[0057][0058]
p
c(k)
＝(i-k
c(k)hc(k)
)p
c(k，k-1
)。
[0059]
作为本发明进一步改进，所述步骤s4具体包括以下过程：
[0060]
建立sins姿态、速度、位置微分方程：
[0061][0062][0063][0064][0065][0066][0067][0068][0069]
其中gn为导航坐标系下重力矢量的投影，为在载体坐标系下从载体坐标系到导航坐标系的角速度，为导航坐标系下地球旋转矢量的投影，为导航坐标系下载体从地球坐标系到导航坐标系的角速度，和fb为惯性器件，包括陀螺和加速度计的输出，代表相对于惯性系的角速度与比力信息，ω为地球旋转角速度，vn、ve和vu分别为北向、东向和天向速度。
[0070]
作为本发明进一步改进，所述步骤s5具体包括以下过程：
[0071]
步骤s5中重复步骤s2到s4直至组合过程结束，过程中如有gnss信息或dvl-bt信息，与sins/dvl-wt系统构成多信息融合系统，这样可以进一步提高精度。
[0072]
有益效果：
[0073]
本发明构建的水流速度辅助sins/dvl中水层导航方法，充分利用中水层dvl-wt信息，提升了中水层区域sins/dvl导航定位精度。
附图说明
[0074]
图1为本发明的基于格网水流速度辅助的惯性/多普勒中水层导航方法系统导航方案图。
具体实施方式
[0075]
下面结合附图与具体实施方式对本发明进步一详细描述：
[0076]
下面结合附图，对本发明的技术方案作进一步说明。应当了解，以下提供的实例仅是为了详尽地且完全地公开本发明，并且向所属技术领域的技术人员充分传达本发明的技术构思，本发明还可以用许多不同的形式来实施，并且不局限于此处描述的实施例。对于表示在附图1中的示例性实施方式中的术语并不是对本发明的限定。
[0077]
本发明提供的基于格网水流速度辅助的惯性/多普勒中水层导航方法，其流程主要包括以下步骤：
[0078]
步骤s1，基于高精度组合导航系统(例如sins/gnss或者sins/dvl-bt系统)初始化载体姿态、速度、位置信息；对航行区域进行交错格网划分。具体包括以下过程：
[0079]
当载体起始于水面时，以sins/gnss组合结果作为载体初始状态信息；当载体起始于 dvl-bt有效水域时，初以sins/dvl-bt组合结果作为载体初始状态信息。
[0080]
对即将航行的水域进行交错格网划分，根据dvl设备对水测速分层特性，将水域从深度0m处从上至下以bh作为层高进行分层。在一层内，以东向be、北向bn结合层高bh进行长方体划分，形成连续扩展格网拼接。在不同层之间，下一层相较于上一层，格网交错点向东向移动ae、北向移动an，以此类推，形成交错格网水域划分。
[0081]
步骤s2，获取到有效dvl-wt量测时，根据当前载体解算位置与dvl-wt各水层量测深度判断各dvl-wt量测对应的格网区域。具体包括以下过程：
[0082]
获取到有效dvl-wt观测时，不同水层dvl-wt观测所对应的格网号(i，j，k)通过以下步骤确定(以某一水层为例)：假设该水层为当前时刻dvl设备对水测速的第n层，
[0083][0084][0085][0086]
λk＝λ1+(k-1)ae[0087]
lk＝l1+(k-1)an[0088]
其中为向下取整，λ、l和h为当前载体所在位置的经度、纬度和高度表示，λk和lk为该水域第k层格网的格网号计算起始经纬度。rm和rn分别为子午圈与卯酉圈半径。
[0089]
步骤s3，判断各dvl-wt量测对应格网区域水速是否已被估计，若已估计，利用该层对水测速结果进行量测更新；若未被估计，不进行量测更新。同时，利用组合后高精度结果及当前各dvl-wt量测，进行相应格网水速估计。具体包括以下过程：
[0090]
s3.1获取到有效dvl-wt观测时，判断各dvl-wt量测对应格网区域水速是否已被估计。若已估计，建立dvl对水测速误差模型：
[0091][0092]
其中为dvl-wt对格网(i，j，k)测速观测值，为相应测速真值，w
d_c(i，j，k)
为相应观测噪声，δk为dvl标度因子误差。
[0093]
利用该层对水测速结果构建量测方程：
[0094][0095][0096]zwt
＝[z
wt(i1，j1，k1)
ꢀ…ꢀzwt(in，jn，kn
)]
t
[0097]
其中n为导航坐标系(以地理坐标系作为导航坐标系)，b为载体坐标系，为s1ns捷联解算的速度，为载体姿态角对应旋转矩阵(n系至b系)，为格网(i，j，k)水速估计值，为失准角对应旋转矩阵，φ
x
，φy，φz为各轴向的失准角，δ为相关量误差，vn为n系下载体速度，为相应速度真值，υ
c(i，j，k)
为格网(i，j，k)水速估计噪声，φn为失准角，(
×
)为对应斜对称矩阵。
[0098]
同时构建sins误差模型：
[0099][0100][0101][0102][0103][0104]
其中为导航坐标系下地球旋转矢量的投影，为导航坐标系下载体从地球坐标系到导航坐标系的角速度，εb为陀螺仪误差，δ为相关量误差，为加速度计误差，fb为加速度计输出，代表相对于惯性系的比力信息，δve，δvn，δvu为东北天方向的速度误差，δl，δλ，δh为各轴向的位置误差。
[0105]
根据sins捷联解算姿态、速度、位置微分方程及误差模型，以及dvl对水测速误差模型建立sins/dvl-wt系统方程。选取状态变量：
[0106][0107]
其中为各轴向的加速度误差，ε
x
，εy，εz为各轴向的陀螺仪误差。构建系统状态方程：
[0108][0109]
f和g根据sins及dvl-wt误差模型获得。
[0110]
基于量测方程与系统方程，进行滤波估计，获得状态量x估计结果，补偿至sins系统形成反馈校正。
[0111]
s3.2利用组合后高精度结果及当前各dvl-wt量测，进行相应格网水速估计。
[0112]
构造格网水流速度状态向量xc：
[0113][0114]
其中为各格网水流待估计值，n1，n2，n3为三个方向格网最大值。
[0115]
根据sins/gnss或sins/dvl-bt高精度组合导航信息，以及dvl-wt多水层量测值，构建格网水速观测方程：
[0116]
[0117][0118]
其中观测矩阵h
c(i，j，k)
为单位阵，hc由h
c(i，j，k)
组成。m为此次观测到的有效格网水速的数量。(im，jm，km)为第m个观测对应的格网号(i，j，k)。
[0119]
根据格网内水速在短时间内变化不大的假设，计算卡尔曼滤波状态矢量及其协方差的一步预测：
[0120][0121]
p
c(k，k-1)
＝p
c(k-1)
[0122]
计算卡尔曼增益矩阵：
[0123][0124]
其中rc为测量噪声协方差矩阵；
[0125]
计算卡尔曼滤波残差：
[0126]
ζ
c(k)
＝zc(
k)-h
c(k)
x
c(k，k-1)
[0127]
计算卡尔曼滤波更新状态估计及其协方差：
[0128][0129]
p
c(k)
＝(i-k
c(k)hc(k)
)p
c(k，k-1)
[0130]
步骤s4，在dvl-wt量测之间，进行惯性捷联解算，更新姿态速度位置微分方程，持续获得载体姿态速度位置信息。具体包括以下过程：
[0131]
建立sins姿态、速度、位置微分方程：
[0132][0133][0134][0135][0136][0137][0138][0139][0140]
其中gn为导航坐标系下重力矢量的投影，为在载体坐标系下从载体坐标系到导航坐标系的角速度，为陀螺仪输出，代表相对于惯性系的角速度信息，ω为地球旋转角速度，vn、ve和vu分别为北向、东向和天向速度。
[0141]
步骤s5，重复步骤s2到s4直至组合过程结束。组合导航过程中如有gnss、dvl-bt 观测，可与sins/dvl-wt子滤波器组合使用，进一步提高精度。具体包括以下过程：
[0142]
重复步骤s2到s4直至组合过程结束。组合导航过程中如有gnss、dvl-bt观测，可与sins/dvl-wt子滤波器组合使用，进一步提高精度。
[0143]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。
技术特征：1.一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，包括以下步骤：s1：基于组合导航系统为sins/gnss或者sins/dvl-bt系统初始化载体姿态、速度、位置信息；对航行区域进行交错格网划分；s2：获取到有效dvl-wt量测时，根据当前载体解算位置与dvl-wt各水层量测深度判断各dvl-wt量测对应的格网区域；s3：判断各dvl-wt量测对应格网区域水速是否已被估计，若已估计，利用该层对水测速结果进行量测更新；若未被估计，不进行量测更新，同时，利用组合后结果及当前各dvl-wt量测，进行相应格网水速估计；s4：在dvl-wt量测之间，进行惯性捷联解算，更新姿态速度位置微分方程，持续获得载体姿态速度位置信息；s5：重复步骤s2到s4直至组合过程结束。2.根据权利要求1所述的一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，所述步骤s1具体包括以下过程：当载体起始于水面时，以sins/gnss组合结果作为载体初始状态信息；当载体起始于dvl-bt有效水域时，初以sins/dvl-bt组合结果作为载体初始状态信息；对即将航行的水域进行交错格网划分，根据dvl设备对水测速分层特性，将水域从深度0m处从上至下以b
h
作为层高进行分层。在一层内，以东向b
e
、北向b
n
结合层高b
h
进行长方体划分，形成连续扩展格网拼接，在不同层之间，下一层相较于上一层，格网交错点向东向移动a
e
、北向移动a
n
，以此类推，形成交错格网水域划分。3.根据权利要求1所述的一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，所述步骤s2具体包括以下过程：获取到有效dvl-wt观测时，不同水层dvl-wt观测所对应的格网号(i，j，k)通过以下步骤确定；以某一水层为例：假设该水层为当前时刻dvl设备对水测速的第n层，以某一水层为例：假设该水层为当前时刻dvl设备对水测速的第n层，以某一水层为例：假设该水层为当前时刻dvl设备对水测速的第n层，λ
k
＝λ1+(k-1)a
e
l
k
＝l1+(k-1)a
n
其中为向下取整，λ、l和h为当前载体所在位置的经度、纬度和高度表示，λ
k
和l
k
为该水域第k层格网的格网号计算起始经纬度，r
m
和r
n
分别为子午圈与卯酉圈半径。4.根据权利要求1所述的一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，所述步骤s3具体包括以下过程：s3.1获取到有效dvl-wt观测时，判断各dvl-wt量测对应格网区域水速是否已被估计，若已估计，建立dvl对水测速误差模型：
其中为dvl-wt对格网(i，j，k)测速观测值，为相应测速真值，w
d_c(i，j，k)
为相应观测噪声，δk为dvl标度因子误差；利用该层对水测速结果构建量测方程：利用该层对水测速结果构建量测方程：z
wt
＝[z
wt(i1，j1，k1)
…
z
wt(in，jn，kn)
]
t
其中n为导航坐标系，以地理坐标系作为导航坐标系，b为载体坐标系，为sins捷联解算的速度，为载体姿态角对应旋转矩阵，n系至b系，为格网(i，j，k)水速估计值，为失准角对应旋转矩阵，φ
x
，φ
y
，φ
z
为各轴向的失准角，δ为相关量误差，v
n
为n系下载体速度，为相应速度真值，υ
c(i，j，k)
为格网(i，j，k)水速估计噪声，φ
n
为失准角，(
×
)为对应斜对称矩阵；根据sins捷联解算姿态、速度、位置微分方程及误差模型，以及dvl对水测速误差模型建立sins/dvl-wt系统方程，选取状态变量：其中δv
e
，δv
n
，δv
u
为东北天方向的速度误差，δl，δλ，δh为各轴向的位置误差，为各轴向的加速度误差，ε
x
，ε
y
，ε
z
为各轴向的陀螺仪误差；构建系统状态方程：f和g根据sins及dvl-wt误差模型获得；基于量测方程与系统方程，进行滤波估计，获得状态量x估计结果，补偿至sins系统形成反馈校正；s3.2利用组合后高精度结果及当前各dvl-wt量测，进行相应格网水速估计；构造格网水流速度状态向量x
c
：其中为各格网水流待估计值，n1，n2，n3为三个方向格网最大值；根据sins/gnss或sins/dvl-bt高精度组合导航信息，以及dvl-wt多水层量测值，构建
格网水速观测方程：格网水速观测方程：其中观测矩阵h
c(i，j，k)
为单位阵，h
c
由h
c(i，j，k)
组成，m为此次观测到的有效格网水速的数量，(im，jm，km)为第m个观测对应的格网号(i，j，k)。根据格网内水速在短时间内变化不大的假设，计算卡尔曼滤波状态矢量及其协方差的一步预测：p
c(k，k-1)
＝p
c(k-1)
计算卡尔曼增益矩阵：其中r
c
为测量噪声协方差矩阵；计算卡尔曼滤波残差：ζ
c(k)
＝z
c(k)-h
c(k)
x
c(k，k-1)
计算卡尔曼滤波更新状态估计及其协方差：p
c(k)
＝(i-k
c(k)
h
c(k)
)p
c(k，k-1)
。5.根据权利要求1所述的一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，其特征在于，所述步骤s4具体包括以下过程：建立sins姿态、速度、位置微分方程：建立sins姿态、速度、位置微分方程：建立sins姿态、速度、位置微分方程：建立sins姿态、速度、位置微分方程：建立sins姿态、速度、位置微分方程：建立sins姿态、速度、位置微分方程：建立sins姿态、速度、位置微分方程：建立sins姿态、速度、位置微分方程：其中g
n
为导航坐标系下重力矢量的投影，为在载体坐标系下从载体坐标系到导航坐标系的角速度，为导航坐标系下地球旋转矢量的投影，为导航坐标系下载体从地球坐标系到导航坐标系的角速度，和f
b
分别为陀螺和加速度计的输出，代表相对于惯性系的角速度与比力信息，ω为地球旋转角速度，v
n
、v
e
和v
u
分别为北向、东向和天向速度。
6.所述的一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，所述步骤s5具体包括以下过程：步骤s5中重复步骤s2到s4直至组合过程结束，过程中如有gnss信息或dvl-bt信息，与sins/dvl-wt系统构成多信息融合系统。
技术总结一种基于格网水流速度辅助的惯性/多普勒中水层导航方法，包括：初始化载体姿态、速度、位置信息并对航行区域进行交错格网划分；获取到有效多普勒测速仪对水观测(DVL-WT)时，根据当前载体解算位置与DVL-WT各水层量测深度判断各DVL-WT量测对应的格网区域；判断各DVL-WT量测对应格网区域水速是否已被估计，若已估计，利用该层对水测速结果进行量测更新，并利用组合后高精度结果及当前各DVL-WT量测进行相应格网水速估计；在DVL-WT量测之间，进行惯性捷联解算，更新姿态速度位置微分方程，持续获得载体姿态速度位置信息；重复步骤上述过程直至组合过程结束。本发明能够充分利用中水层DVL-WT信息，提升了中水层区域SINS/DVL导航定位精度。位精度。位精度。
技术研发人员：姚逸卿 李浪 徐晓苏 潘绍华
受保护的技术使用者：东南大学
技术研发日：2022.05.11
技术公布日：2022/7/5
