一种结合地形和环境特征的水下导航定位方法.pdf

本发明公开了一种结合地形和环境特征的水下导航定位方法，根据地形信息量判断水下潜器的规划航迹中各个子区域是否可匹配，若可匹配则采用地形辅助惯性导航系统实现定位，若不可匹配则采用同步定位与构图算法辅助主惯性导航系统实现定位。本发明通过对导航区域地形信息量的计算，将海底地形划分为地形可匹配区域及地形不可匹配区域。针对不同的区域，采用不同的导航算法实现对主惯性导航系统的位置误差修正，具有更高的自主性。

1.  一种结合地形和环境特征的水下导航定位方法，其特征在于：根据地形信息量判断水下潜器的规划航迹中各个子区域是否可匹配，若可匹配则采用地形辅助惯性导航系统实现定位，若不可匹配则采用同步定位与构图算法辅助主惯性导航系统实现定位。

2.  根据权利要求1所述的结合地形和环境特征的水下导航定位方法，其特征在于：判断水下潜器的规划航迹中的各个子区域是否可匹配的方法如下：
步骤1.1：将海底地形高程采用网格矩阵的方式划分成多块地形候选区域，水下潜器的规划航迹上经过其中L块地形候选区域，设定某一块地形候选区域的经纬度跨度为M×N网格，并且网格点坐标为(i,j)处的地形高程值为height(i,j)，这里i＝1,2,...,M，j＝1,2,...,N；
步骤1.2：利用移动计算窗口法计算地形候选区域的参数，具体方法为：
定义一个大小为m×n的局部移动计算窗口，并计算局部移动计算窗口内的地形高度平均值当局部移动计算窗口的中心在各地形候选区域的全部网格点上移动一遍后，可以得到各地形候选区域的地形标准差σ(l)、经度方向上的地形相关系数R_longitude(l)以及纬度方向上的地形相关系数R_latitude(l)，其中l＝1,2,...,L，下标longitude表示经度，latitude表示纬度，地形候选区域的参数具体计算公式如下：
σ(l)=1m(n-1)Σi=1mΣj=1n[height(i,j)-height_ave]2]]>
Rlongitude(l)=1n(m-1)σ(l)2Σi=1m-1Σj=1n[height(i,j)-height_ave][height(i+1,j)-height_ave]]]>
Rlatitude(l)=1n(m-1)σ(l)2Σi=1mΣj=1n-1[height(i,j)-height_ave][height(i,j+1)-height_ave]]]>
步骤1.3：针对规划航迹上的L块地形候选区域，分别判断每一块地形候选区域的地形标准差和经纬度方向地形相关系数是否同时满足σ(l)＞4σ_c且 R_longitude(l)＜0.7且R_latitude(l)＜0.7，若满足，则对应的地形候选区域为可匹配区域，否则为不可匹配区域，其中，σ_c为测深传感器测量误差的标准差。

3.  根据权利要求1所述的结合地形和环境特征的水下导航定位方法，其特征在于：所述的同步定位与构图算法包括以下步骤：
步骤3.1：系统非线性过程模型的建立
步骤3.1.1：水下潜器状态方程的建立
选取东北天系为导航坐标系，载体坐标系x轴沿水下潜器横轴指向右舷，载体坐标系y轴沿水下潜器纵轴指向前，载体坐标系z轴垂直于x轴和y轴所确定的平面构成右手坐标系；则k时刻水下潜器状态方程如下：

其中：
pos_vehicle(k)表示k时刻水下潜器的位置，
vel_vehicle(k)表示k时刻水下潜器的速度，
qua_vehicle(k)表示k时刻水下潜器的姿态四元数，
上标G表示导航坐标系，
上标B表示载体坐标系，
Δt表示离散采样时间间隔，
C_B2G表示从载体坐标系变换到导航坐标系的方向余弦矩阵，
f^B(k)表示k时刻加速度计输出，
g表示重力加速度，
表示由陀螺仪测得的角速率构成的四元数，
表示四元数乘法；
步骤3.1.2：地图状态的建立
设定k时刻观测到了N_m个新的环境特征，则k时刻地图状态为：
xfeature(k)=xfeature_m1T(k)xfeature_m2T(k)...xfeature_mNmT(k)T]]>
其中：
表示k时刻第m1个环境特征的状态，
表示k时刻第m2个环境特征的状态，
表示k时刻第mN_m个环境特征的状态，
上标T表示转置；
将水下环境特征建模为点特征，则环境特征在导航坐标系下的位置如下：
xfeature_miG(k)=xfeature_miG(k-1)=posvehicleG(k)+CB2G(k)rBSB+CB2G(k)CS2BrSfeature_miS(k)]]>
其中：
表示在k时刻得到的第mi个环境特征在导航坐标系中的位置，
表示在k-1时刻得到的第mi个环境特征在导航坐标系中的位置，
表示水下潜器上的方位/距离传感器到水下潜器中心的杆臂效应补偿量在载体坐标系的分量，
下标S表示方位/距离传感器坐标系，
C_S2B表示方位/距离传感器坐标系变换到载体坐标系的方向余弦矩阵，
rSfeature_miS(k)=dcos(phi)cos(theta)dsin(phi)cos(theta)dcos(theta)T]]>表示方位/距离传感器测得的第mi个环境特征与水下潜器之间的相对位置，其中，d表示方位/距离传感器到环境特征之间的距离，phi表示环境特征与方位/距离传感器之间的方位角，theta表示环境特征与方位/距离传感器之间的高度角；
步骤3.2：系统非线性观测模型的建立
步骤3.2.1：根据方位/距离传感器获得环境特征与水下潜器的相对位置观测，从而建立系统非线性观测模型如下：
z(k)＝h(x(k))+ν(k)
其中：
z(k)表示对环境特征的观测值，
h(·)为非线性观测函数，
x(k)为系统状态矢量，包括水下潜器的位置、速度、姿态四元数以及环境特征的位置，
ν(k)为系统观测噪声；
k时刻对第mi个环境特征的观测只与该环境特征和水下潜器状态相关，因此有：
z_mi(k)＝h(x_vehicle(k),x_{feature_mi}(k))+ν_mi(k)
其中：
z_mi(k)表示k时刻对第mi个环境特征的观测，
x_vehicle(k)表示水下潜器在k时刻的状态，
x_{feature_mi}(k)表示第mi个环境特征在k时刻的状态，
ν_mi(k)是对第mi个环境特征的观测噪声；
步骤3.2.2：设定rSfeature_miS(k)=xyzT,]]>则k时刻对第mi个环境特征的观测为：
zmi(k)=dphitheta=x2+y2+z2arctan(yx)arctan(zx2+y2)]]>
其中：
d表示方位/距离传感器到环境特征之间的距离，
phi表示环境特征与方位/距离传感器之间的方位角，
theta表示环境特征与方位/距离传感器之间的高度角；
步骤3.3：系统状态增广过程
水下潜器航行过程中，方位/距离传感器观测到环境特征，若该环境特征为已观测到过的特征，则直接执行步骤3.5；若该环境特征为新出现的特征，则对系统状态进行增广并顺序执行步骤3.4和步骤3.5，增广后的系统状态变为：
x(k)=xvehicle(k)xfeature(k)=fvehicle(xvehicle(k-1),u(k),w(k))xfeature(k-1)]]>
其中：
x_vehicle(k)表示k时刻水下潜器状态，包括水下潜器位置、速度和姿态四元数，
x_feature(k)表示k时刻环境特征的状态，
f_vehicle(·)为非线性状态转换函数；
步骤3.4：同步定位与构图算法预测过程
通过步骤3.1建立的系统非线性过程模型和步骤3.2建立的系统非线性观测模型对k时刻的系统状态以及状态协方差矩阵进行预测，有：
x(k|k-1)＝f(x(k-1|k-1))
Pcov(k|k-1)=F‾(k)Pcov(k-1|k-1)F‾(k)T+Q(k)]]>
其中：
x(k|k-1)是根据k-1时刻的系统状态对k时刻的一步预测状态量，
x(k-1|k-1)是k-1时刻的系统状态估计值，
P_cov(k|k-1)是根据k-1时刻的系统状态对k时刻的预测协方差，
P_cov(k-1|k-1)是k-1时刻的状态协方差估计值，
为系统状态方程的雅可比矩阵，
T代表转置，
Q(k)为噪声ω(k)的协方差；
步骤3.5：同步定位与构图算法更新过程
对于观测得到的环境特征，结合步骤3.2和步骤3.4对系统状态值进行更新，具体公式如下：
新息：v_innov(k)＝z(k)-h(x(k|k-1))
滤波增益：Kgain(k)=Pcov(k|k-1)H‾(k)T(H‾(k)Pcov(k|k-1)H‾(k)T+R(k))-1]]>
状态估计值：x(k|k)＝x(k|k-1)+K_gain(k)·v_innov(k)
状态协方差：Pcov(k|k)=Pcov(k|k-1)-Kgain(k)H‾(k)Pcov(k|k-1)]]>
其中：
v_innov(k)为新息，
z(k)为k时刻观测值，
h(x(k|k-1))为k时刻观测预测值，
K_gain(k)为k时刻滤波增益，
为系统观测方程的雅可比矩阵，
R(k)为ν(k)的协方差，
x(k|k)为k时刻的系统状态估计值，
P_cov(k|k)为k时刻的系统状态协方差；
步骤3.6：从x(k|k)中得到的pos_vehicle即为修正后的主惯性导航位置信息。

一种结合地形和环境特征的水下导航定位方法
技术领域
本发明涉及水下导航技术领域，具体地说是设计一种能够满足水下潜器长时间高精度自主导航定位要求的方法。
背景技术
惯性导航系统不需要任何外来信息，也不会向外辐射任何信息，仅靠惯性导航系统本身就能在全天候条件下，在全球范围内和任何介质环境里进行连续的三维定位和导航，这种同时具备自主性、隐蔽性和能获取载体完备运动信息的独特优点是诸如无线电导航、卫星导航和天文导航等其他导航系统无法比拟的。但是，惯性导航系统最大弱点是其系统误差随时间积累，时间越长，误差越大。为了确保水下潜器的安全航行和武器的准确发射，必须利用外界位置信息对惯性导航系统进行周期的调整与校正。
地形辅助导航系统(Terrain-Aided Navigation,TAN)，实质是由惯性导航系统(给出实时图)与传感器(实时图与基准图的联系和纽带)和数字地图(给出基准图)构成的组合导航系统，它作为一种高精度导航定位技术，只需要地形信息含量足够丰富并增加存储器的容量就能将导航精度提高近一个数量级，达到十几米的定位精度。地形辅助导航具有自主、隐蔽、连续、全天候工作，导航定位误差不随时间积累的优点，是水下潜器理想的辅助导航定位手段。
典型的基于ICCP算法的地形辅助导航系统可以在具有先验地形图且地形变化明显的海域，修正主惯性导航系统的位置误差。然而，地形辅助导航需要有先验的地形图，且要求有明显的地形变化，对于那些尚未测绘或者地形变化过于平缓，地形特征不明显的海域，用地形辅助导航方法来降低主惯性导航系统随时间积累的误差是十分困难的。
此时，可利用对具有时间稳定性的其它水下特征诸如水下建筑、沉船等的观测，采用基于海洋环境特征的同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)算法来作为地形辅助导航的替代方法，以实时修正水下潜器的位置，降低导航误差的积累，改善导航系统的定位精度。
发明内容
要解决的技术问题：针对现有技术的不足，本发明提出一种结合地形和环境特征的水下导航定位方法，针对海底先验地形图不可得或者是地形信息量不丰富的海域，无法利用地形辅助导航系统来降低主惯性导航系统随时间积累的位置误差的技术问题。
技术方案：为解决上述技术问题，本发明采用以下技术方案：
一种结合地形和环境特征的水下导航定位方法，根据地形信息量判断水下潜器的规划航迹中各个子区域是否可匹配，若可匹配则采用地形辅助惯性导航系统实现定位，若不可匹配则采用同步定位与构图算法辅助主惯性导航系统实现定位。
地形辅助导航方法具有自主、隐蔽等优点，在地形信息量丰富的可匹配海域，一般是基于ICCP算法，根据先验地形图来修正惯性导航系统的误差，修正方法和修正过程均为现有技术，可以很好的辅助主惯性导航系统实现水下潜器的导航定位；而在地形不可匹配区域，同步定位与构图算法，发挥了自身对具有时间稳定性的环境特征进行观测来实现定位，并修正主惯性导航系统位置误差的特点，有效地弥补了地形辅助导航系统的不足；两种方法配合使用，使得水下潜器的导航定位具有更好的自主性和精确性。
进一步的，在本发明中，判断水下潜器的规划航迹中的各个子区域是否可匹配的方法如下：
步骤1.1：将海底地形高程采用网格矩阵的方式划分成多块地形候选区域，水下潜器的规划航迹上经过其中L块地形候选区域，设定某一块地形候选区域的经纬度跨度为M×N网格，并且网格点坐标为(i,j)处的地形高程值为height(i,j)，这里i＝1,2,...,M，j＝1,2,...,N；
步骤1.2：利用移动计算窗口法计算地形候选区域的参数，具体方法为：
定义一个大小为m×n的局部移动计算窗口，并计算局部移动计算窗口内的地形高度平均值当局部移动计算窗口的中心在各地形候选区域的全部网格点上移动一遍后，可以得到各地形候选区域的地形标准差σ(l)、经度方向上的地形相关系数R_longitude(l)以及纬度方向上的地形相关系数R_latitude(l)，其中l＝1,2,...,L，下标longitude表示经度，latitude表示纬度，地形 候选区域的参数具体计算公式如下：
σ(l)=1m(n-1)Σi=1mΣj=1n[height(i,j)-height_ave]2]]>
Rlongitude(l)=1n(m-1)σ(l)2Σi=1m-1Σj=1n[height(i,j)-height_ave][height(i+1,j)-height_ave]]]>
Rlatitude(l)=1n(m-1)σ(l)2Σi=1mΣj=1n-1[height(i,j)-height_ave][height(i,j+1)-height_ave]]]>
步骤1.3：针对规划航迹上的L块地形候选区域，分别判断每一块地形候选区域的地形标准差和经纬度方向地形相关系数是否同时满足σ(l)＞4σ_c且R_longitude(l)＜0.7且R_latitude(l)＜0.7，若满足，则对应的地形候选区域为可匹配区域，否则为不可匹配区域，其中，σ_c为测深传感器测量误差的标准差。
判断是否可匹配的方法是现有技术，针对不同的仿真地形计算与地形相关的各种参数，统计分析后确定参数的阈值来确定判定准则，具体的数值主要由计算机仿真得到的。
进一步的，在本发明中，针对地形不可匹配区域，所述同步定位与构图算法包括以下步骤：
步骤3.1：系统非线性过程模型的建立
步骤3.1.1：水下潜器状态方程的建立
选取东北天系为导航坐标系，载体坐标系x轴沿水下潜器横轴指向右舷，载体坐标系y轴沿水下潜器纵轴指向前，载体坐标系z轴垂直于x轴和y轴所确定的平面构成右手坐标系；则k时刻水下潜器状态方程如下：

其中：
pos_vehicle(k)表示k时刻水下潜器的位置，
vel_vehicle(k)表示k时刻水下潜器的速度，
qua_vehicle(k)表示k时刻水下潜器的姿态四元数，
上标G表示导航坐标系，
上标B表示载体坐标系，
Δt表示离散采样时间间隔，
C_B2G表示从载体坐标系变换到导航坐标系的方向余弦矩阵，
f^B(k)表示k时刻加速度计输出，
g表示重力加速度，
表示由陀螺仪测得的角速率构成的四元数，
表示四元数乘法；
步骤3.1.2：地图状态的建立
设定k时刻观测到了N_m个新的环境特征，则k时刻地图状态为：
xfeature(k)=xfeature_m1T(k)xfeature_m2T(k)...xfeature_mNmT(k)T]]>
其中：
表示k时刻第m1个环境特征的状态，
表示k时刻第m2个环境特征的状态，
表示k时刻第mN_m个环境特征的状态，
上标T表示转置；
将水下环境特征建模为点特征，则环境特征在导航坐标系下的位置如下：
xfeature_miG(k)=xfeature_miG(k-1)=posvehicleG(k)+CB2G(k)rBSB+CB2G(k)CS2BrSfeature_miS(k)]]>
其中：
表示在k时刻得到的第mi个环境特征在导航坐标系中的位置，
表示在k-1时刻得到的第mi个环境特征在导航坐标系中的位置，
表示水下潜器上的方位/距离传感器到水下潜器中心的杆臂效应补偿量在载体坐标系的分量，
下标S表示方位/距离传感器坐标系，方位/距离传感器都是固定在水下潜器上的，建立时一般将方位/距离传感器坐标系相对于载体坐标系x轴平移一段距离，y轴和z轴不变，
C_S2B表示方位/距离传感器坐标系变换到载体坐标系的方向余弦矩阵，
rSfeature_miS(k)=dcos(phi)cos(theta)dsin(phi)cos(theta)dcos(theta)T]]>表示传感器测得的第mi个环境特征与水下潜器之间的相对位置，其中，d表示传感器到环境特征之间的距离，phi表示环境特征与方位/距离传感器之间的方位角，theta表示环境特征与方位/距离传感器之间的高度角；
步骤3.2：系统非线性观测模型的建立
步骤3.2.1：根据方位/距离传感器获得环境特征与水下潜器的相对位置观测，从而建立系统非线性观测模型如下：
z(k)＝h(x(k))+ν(k)
其中：
z(k)表示对环境特征的观测值，
h(·)为非线性观测函数，
x(k)为系统状态矢量，包括水下潜器的位置、速度、姿态四元数以及环境特征的位置，
ν(k)为系统观测噪声；
k时刻对第mi个环境特征的观测只与该环境特征和水下潜器状态相关，因此有：
z_mi(k)＝h(x_vehicle(k),x_{feature_mi}(k))+ν_mi(k)
其中：
z_mi(k)表示k时刻对第mi个环境特征的观测，
x_vehicle(k)表示水下潜器在k时刻的状态，
x_{feature_mi}(k)表示第mi个环境特征在k时刻的状态，
ν_mi(k)是对第mi个环境特征的观测噪声；
步骤3.2.2：设定rSfeatrue_miS(k)=xyzT,]]>则k时刻对第mi个环境特征的观测为：
zmi(k)=dphitheta=x2+y2+z2arctan(yx)arctan(zx2+y2)]]>
其中：
d表示方位/距离传感器到环境特征之间的距离，
phi表示环境特征与方位/距离传感器之间的方位角，
theta表示环境特征与方位/距离传感器之间的高度角；
步骤3.3：系统状态增广过程
水下潜器航行过程中，方位/距离传感器观测到环境特征，若该环境特征为已观测到过的特征，则直接执行步骤3.5；若该环境特征为新出现的特征，则对系统状态进行增广并顺序执行步骤3.4和步骤3.5，增广后的系统状态变为：
x(k)=xvehicle(k)xfeature(k)=fvehicle(xvehicle(k-1),u(k),w(k))xfeature(k-1)]]>
其中：
x_vehicle(k)表示k时刻水下潜器状态，包括水下潜器位置、速度和姿态四元数，
x_feature(k)表示k时刻环境特征的状态，
f_vehicle(·)为非线性状态转换函数；
步骤3.4：同步定位与构图算法预测过程
通过步骤3.1建立的系统非线性过程模型和步骤3.2建立的系统非线性观测模型对k时刻的系统状态以及状态协方差矩阵进行预测，有：
x(k|k-1)＝f(x(k-1|k-1))
Pcov(k|k-1)=F‾(k)Pcov(k-1|k-1)F‾(k)T+Q(k)]]>
其中：
x(k|k-1)是根据k-1时刻的系统状态对k时刻的一步预测状态量，
x(k-1|k-1)是k-1时刻的系统状态估计值，
P_cov(k|k-1)是根据k-1时刻的系统状态对k时刻的预测协方差，
P_cov(k-1|k-1)是k-1时刻的状态协方差估计值，
为系统状态方程的雅可比矩阵，
T代表转置，
Q(k)为噪声ω(k)的协方差。
步骤3.5：依据扩展卡尔曼滤波算法对同步定位与构图算法更新过程
对于观测得到的环境特征，结合步骤3.2和步骤3.4对系统状态值进行更新，具体公式如下：
新息：v_innov(k)＝z(k)-h(x(k|k-1))
滤波增益：Kgain(k)=Pcov(k|k-1)H‾(k)T(H‾(k)Pcov(k|k-1)H‾(k)T+R(k))-1]]>
状态估计值：x(k|k)＝x(k|k-1)+K_gain(k)·v_innov(k)
状态协方差：Pcov(k|k)=Pcov(k|k-1)-Kgain(k)H‾(k)Pcov(k|k-1)]]>
其中：
v_innov(k)为新息，
z(k)为k时刻观测值，
h(x(k|k-1))为k时刻观测预测值，
K_gain(k)为k时刻滤波增益，
为系统观测方程的雅可比矩阵，
R(k)为ν(k)的协方差，
x(k|k)为k时刻的系统状态估计值，
P_cov(k|k)为k时刻的系统状态协方差；
步骤3.6：从x(k|k)中得到的pos_vehicle即为修正后的主惯性导航位置信息。
上述步骤3.1-3.6描述了同步定位与构图算法的主要过程，同步定位与构图算法接收来自惯性传感器和方位/距离传感器的原始数据，建立起基于惯性导航机械方程的水下潜器状态方程，以及由方位/距离传感器得到环境特征的状态；然后分别进行预测阶段、观测阶段和更新阶段，预测阶段以k-1时刻的系统状态估计值x(k-1|k-1)和协方差矩阵P_cov(k-1|k-1)为基础，对k时刻的系统状态及协方差进行预测；观测阶段对于已经存在的环境特征，用来对系统状态进行更新，对新的环境特征，则进行系统增广；更新阶段，利用观测值和预测值之间的信息来对状态值进行更新；整个过程采用扩展卡尔曼滤波器来对状态进行预测和更新，而不需要对对系统非线性过程模型和系统非线性观测模型做线性化处理，计算量更小且易于实现。
有益效果：
本发明通过对导航区域地形信息量的计算，将地形划分为地形可匹配区域及地形不可匹配区域。针对不同的导航区域，分别采用不同的导航算法来辅助主惯性导航系统，修正主惯性导航系统随时间积累的位置误差，具有更高的自主性。
本发明在地形可匹配区域，采用基于ICCP算法的地形辅助导航方法来修正主惯性导航系统的位置误差以得到较精确的潜器位置。当水下潜器进入地形不可匹配区域时，可以提供同步定位与构图算法的初始位置，以提高SLAM算法的精度和鲁棒性。
本发明在地形不可匹配区域，采用基于海洋环境特征的同步定位与构图算法来辅助主惯性导航系统，并且在计算过程中采用扩展卡尔曼滤波器来对状态进行预测和更新，而并非通过对系统非线性化过程模型和非线性化观测模型做线性化处理后采用卡尔曼滤波器实现状态预测和更新的方法，使得计算量小且更加易于实现。
仿真结果表明，该算法得到的水下潜器航迹比纯惯性导航系统得到的航迹更接近于理想航迹，可以克服主惯性导航误差随之间积累导致导航定位精度不高的问题。
附图说明
图1为本发明所描述结合地形和环境特征的水下导航定位系统原理图；
图2为同步定位与构图算法辅助惯性导航原理图；
图3为实施例中所采用的地形图1；
图4为实施例中地形1的等深线图；
图5为实施例中水下潜器经过地形可匹配区域时ICCP辅助主惯性导航系统的导航定位结果图；
图6为实施例中水下潜器经过地形不可匹配区域时SLAM辅助主惯性导航系统的导航定位结果图；
具体实施方式
下面结合附图对本发明作更进一步的说明。
一种结合地形和环境特征的水下导航定位方法，
第一步：根据地形信息量，判断地形是否可匹配
步骤1.1：海底地形高程采用格网矩阵的方式划分并存储为多块地形候选区域，水下潜器的规划航迹上经过其中L块地形候选区域，假设某一块地形候选区域的经纬度跨度为M×N网格，并且网格点坐标为(i,j)处的地形高程值为height(i,j)，这里i＝1,2,...,M，j＝1,2,...,N；
步骤1.2：利用移动计算窗口法计算地形候选区域的参数，具体方法为：
定义一个大小为m×n的局部移动计算窗口，并计算局部移动计算窗口内的地形高度平均值当局部移动计算窗口的中心在各地形候选区域的全部网格点上移动一遍后，可以得到各地形候选区域的地形标准差σ(l)、经度方向上的地形相关系数R_longitude(l)以及纬度方向上的地形相关系数R_latitude(l)，其中l＝1,2,...,L，下标longitude表示经度，latitude表示纬度，地形候选区域的参数具体计算公式如下：
σ(l)=1m(n-1)Σi=1mΣj=1n[height(i,j)-height_ave]2]]>
Rlongitude(l)=1n(m-1)σ(l)2Σi=1m-1Σj=1n[height(i,j)-height_ave][height(i+1,j)-height_ave]]]>
Rlatitude(l)=1n(m-1)σ(l)2Σi=1mΣj=1n-1[height(i,j)-height_ave][height(i,j+1)-height_ave]]]>
步骤1.3：针对规划航迹上的L块地形候选区域，分别判断每一块地形候选区域的地形标准差和经纬度方向地形相关系数是否同时满足σ(l)＞4σ_c且R_longitude(l)＜0.7且R_latitude(l)＜0.7，若满足，则对应的地形候选区域为可匹配区域，否则为不可匹配区域，其中，σ_c为测深传感器测量误差的标准差。
第二步：当水下潜器进入地形可匹配区域时，采用基于等值线最近点迭代(ICCP)算法的地形辅助导航系统来辅助修正主惯性导航系统随时间积累的位置误差，具体包括以下步骤：
步骤2.1：由惯性导航系统提供航迹测量的N_p个位置值序列，集合为Pos={posNi},]]>且posNi=latitudeNilongitudeNiT,]]>N_i＝1,2,...,N_p，表示第N_i个序列点的纬度，表示第N_i个序列点的经度；实时测深装置提供序列点相应的实测水深值，其中N_i＝1,2,...,N_p，并且从已知参考地图中抽取出相应的等值线，其中N_i＝1,2,...,N_p，并假设在相应等值线上的最近点为，所有构成序列集合；
步骤2.2：寻找包含旋转矩阵cosθ-sinθsinθcosθ]]>和平移矢量tlatitudetlongitude]]>的刚性变换T，其中，θ是随机旋转角，t_latitude和t_longitude分别是纬度和经度方向的平移量，使序列集合与集合之间的距离最小，也即使下面的目标函数最小
d(Y,TPos)=ΣNi=1NpωNi||yNi-TposNi||2]]>
ωNi=1-D(posNi,yNi)Dmax]]>
其中：
d为目标函数，
表示序列点的权值，
为和之间的距离，
D_max为和之间的距离的最大值；
求解T的过程就是求解θ、t_latitude和t_longitude的过程，可以通过四元数法并构造一个哈密尔顿矩阵获得；
步骤2.3：将集合变换到集合，即有e假设第iteration_k次迭代后得到第iteration_k+1次迭代后得到PosNiiteration_k+=1{posNiiteration k+},]]>计算d(iteration_k+1)=d(Y,TPosNiiteration_k+1)]]>和若d(iteration_k+1)-d(iteration_k)＞τ，τ的值为10^-6，则返回执行步骤2.2，若d(iteration_k+1)-d(iteration_k)≤τ，且迭代次数小于最大迭代次数，则判断满足最终的迭代终止条件，退出迭代，确定最终匹配结果为[latitude_ICCP longitude_ICCP]^T；
步骤2.4：将主惯性导航系统输出的位置信息latitude_INS、longitute_INS与ICCP匹配得到的位置信息latitude_ICCP、longitute_ICCP的差值latitude_INS-latitude_ICCP作为观测量进行Kalman滤波，并利用滤波得到的位置误差量反馈到主惯性导航系统中对主惯导输出的位置进行校正，得到校正后的位置为
第三步：当水下潜器进入地形不可匹配区域时，采用基于海洋环境特征的同步定位与构图(SLAM)算法来辅助修正主惯性导航系统随时间积累的位置误差，其基本思想如图2中所示，包括以下步骤：
步骤3.1：系统非线性过程模型的建立
步骤3.1.1：水下潜器状态方程的建立
选取东北天系为导航坐标系，载体坐标系x轴沿水下潜器横轴指向右舷，载体坐标系y轴沿水下潜器纵轴指向前，载体坐标系z轴垂直于x轴和y轴所确定的平 面构成右手坐标系；则k时刻水下潜器状态方程可由主惯性导航机械方程给出：

其中：
pos_vehicle(k)表示k时刻水下潜器的位置，
vel_vehicle(k)表示k时刻水下潜器的速度，
qua_vehicle(k)表示k时刻水下潜器的姿态四元数，
上标G表示导航坐标系，
上标B表示载体坐标系，
Δt表示离散采样时间间隔，
C_B2G表示从载体坐标系变换到导航坐标系的方向余弦矩阵，
f^B(k)表示k时刻加速度计输出，
g表示重力加速度，
表示由陀螺仪测得的角速率构成的四元数，
表示四元数乘法；
步骤3.1.2：地图状态的建立
设定k时刻观测到了N_m个新的环境特征，则k时刻地图状态为：
xfeature(k)=xfeature_m1T(k)xfeature_m2T(k)...xfeature_mNmT(k)T]]>
其中：
表示k时刻第m1个环境特征的状态，
表示k时刻第m2个环境特征的状态，
表示k时刻第mN_m个环境特征的状态，
上标T表示转置；
将水下环境特征建模为点特征，则环境特征在导航坐标系下的位置如下：
xfeature_miG(k)=xfeature_miG(k-1)=posvehicleG(k)+CB2G(k)rBSB+CB2G(k)CS2BrSfeature_miS(k)]]>
其中：
表示在k时刻得到的第mi个环境特征在导航坐标系中的位置，
表示在k-1时刻得到的第mi个环境特征在导航坐标系中的位置，
表示水下潜器上的方位/距离传感器到水下潜器中心的杆臂效应补偿量在载体坐标系的分量，
下标S表示方位/距离传感器坐标系，
C_S2B表示方位/距离传感器坐标系变换到载体坐标系的方向余弦矩阵，
rSfeature_miS(k)=dcos(phi)cos(theta)dsin(phi)cos(theta)dcos(theta)T]]>表示方位/距离传感器测得的第mi个环境特征与水下潜器之间的相对位置，其中，d表示方位/距离传感器到环境特征之间的距离，phi和theta分别表示环境特征与方位/距离传感器之间的方位角和高度角；
步骤3.2：系统非线性观测模型的建立
步骤3.2.1：水下潜器上的方位/距离传感器可以获得环境特征与水下潜器的相对位置观测，系统非线性观测模型可表示如下：
z(k)＝h(x(k))+ν(k)
其中：
z(k)表示对环境特征的观测值，
h(·)为非线性观测函数，
x(k)为系统状态矢量，包括水下潜器的位置、速度、姿态四元数以及环境特征的位置，
ν(k)为系统观测噪声；
k时刻对第mi个环境特征的观测只与该特征和水下潜器状态相关，因此有：
z_mi(k)＝h(x_vehicle(k),x_{feature_mi}(k))+ν_mi(k)
其中：
z_mi(k)表示k时刻对第mi个环境特征的观测，
x_vehicle(k)表示水下潜器在k时刻的状态，
x_{feature_mi}(k)表示第mi个环境特征在k时刻的状态，
ν_mi(k)是对第mi个环境特征的观测噪声；
步骤3.2.2：设定rSfeature_miS(k)=xyzT,]]>则k时刻对第mi个环境特征的观测为：
zmi(k)=dphitheta=x2+y2+z2arctan(yx)arctan(zx2+y2)]]>
其中：
d表示方位/距离传感器到环境特征之间的距离，
phi表示环境特征与方位/距离传感器之间的方位角，
theta表示环境特征与方位/距离传感器之间的高度角；
步骤3.3：系统状态增广过程
水下潜器航行过程中，方位/距离传感器观测到环境特征，若该环境特征为已观测到过的特征，则直接执行步骤3.5；若该环境特征为新出现的特征，则对系统状态进行增广并顺序执行步骤3.4和步骤3.5，增广后的系统状态变为：
x(k)=xvehicle(k)xfeature(k)=fvehicle(xvehicle(k-1),u(k),w(k))xfeature(k-1)]]>
其中：
x_vehicle(k)表示k时刻水下潜器状态，包括水下潜器位置、速度和姿态，
x_feature(k)表示环境特征的状态，由于环境特征的状态只包含环境特征的位置信息，固x_feature(k)实际表示环境特征的位置，
f_vehicle(·)为非线性状态转换函数；
步骤3.4：SLAM算法预测过程
通过过程模型和观测模型对k时刻的系统状态以及状态协方差矩阵进行预测， 有：
x(k|k-1)＝f(x(k-1|k-1))
Pcov(k|k-1)=F‾(k)Pcov(k-1|k-1)F‾(k)T+Q(k)]]>
其中：
x(k|k-1)是根据k-1时刻的系统状态对k时刻的一步预测状态量，
x(k-1|k-1)是k-1时刻的系统状态估计值，
P_cov(k|k-1)是根据k-1时刻的系统状态对k时刻的预测协方差，
P_cov(k-1|k-1)是k-1时刻的状态协方差估计值，
为系统状态方程的雅可比矩阵，
T代表转置，
Q(k)为噪声ω(k)的协方差；
步骤3.5：SLAM算法更新过程
对于观测得到的环境特征，结合步骤3.2和步骤3.4对系统状态值进行更新，具体公式如下：
新息：v_innov(k)＝z(k)-h(x(k|k-1))
滤波增益：Kgain(k)=Pcov(k|k-1)H‾(k)T(H‾(k)Pcov(k|k-1)H‾(k)T+R(k))-1]]>
状态估计值：x(k|k)＝x(k|k-1)+K_gain(k)·v_innov(k)
状态协方差：Pcov(k|k)=Pcov(k|k-1)-Kgain(k)H‾(k)Pcov(k|k-1)]]>
其中：
v_innov(k)为新息，
z(k)为k时刻观测值，
h(x(k|k-1))为k时刻观测预测值，
K_gain(k)为k时刻滤波增益，
为系统观测方程的雅可比矩阵，
R(k)为ν(k)的协方差，
x(k|k)为k时刻的系统状态估计值，
P_cov(k|k)为k时刻的系统状态协方差；
步骤3.6：SLAM算法中从x(k|k)中得到的pos_vehicle即为修正后的主惯性导航位置信息。
实施例：
仿真实验选取如图3所示的地形区域，其范围为(北纬38.0°，东经120.0°)至(北纬38.04°，东经120.05°)的矩形区域，根据移动计算窗口法，通过仿真分析得到如图4中左边的框形区域为地形匹配区域，右边的框形区域为地形不匹配区域。
在地形可匹配区域内设置一条含有10个点的理想导航路径，如图5中1#线，惯导指示航迹如图5中2#线，采用ICCP算法进行匹配定位后的匹配航迹如图5中3#线所示，可以看到，匹配航迹几乎与理想航迹重合。
在地形不可匹配区域，采用SLAM算法进行导航定位。如图6中，在环境中任意给出3个环境特征并设置一条以ICCP辅助惯性导航系统所得最终位置为起点的10个点的理想导航路径，如图6中1#线所示。惯导指示航迹如图6中2#线所示,采用SLAM算法获得的估计路径如图6中3#线所示。可以看到SLAM估计航迹比纯惯性导航系统指示航迹更接近于理想航迹。
从仿真结果来看，算法可以判断完地形是否可匹配后，自主选择合适的辅助导航手段，在地形可匹配区域，采用基于ICCP算法的地形辅助导航系统，来辅助惯性导航系统；在地形不可匹配区域，采用SLAM算法来辅助惯性导航系统，从而实现水下航行器长时间的自主导航。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。