一种结合地形和环境特征的水下导航定位方法
【专利摘要】本发明公开了一种结合地形和环境特征的水下导航定位方法，根据地形信息量判断水下潜器的规划航迹中各个子区域是否可匹配，若可匹配则采用地形辅助惯性导航系统实现定位，若不可匹配则采用同步定位与构图算法辅助主惯性导航系统实现定位。本发明通过对导航区域地形信息量的计算，将海底地形划分为地形可匹配区域及地形不可匹配区域。针对不同的区域，采用不同的导航算法实现对主惯性导航系统的位置误差修正，具有更高的自主性。
【专利说明】一种结合地形和环境特征的水下导航定位方法
【技术领域】
[0001]本发明涉及水下导航【技术领域】，具体地说是设计一种能够满足水下潜器长时间高精度自主导航定位要求的方法。
【背景技术】
[0002]惯性导航系统不需要任何外来信息，也不会向外辐射任何信息，仅靠惯性导航系统本身就能在全天候条件下，在全球范围内和任何介质环境里进行连续的三维定位和导航，这种同时具备自主性、隐蔽性和能获取载体完备运动信息的独特优点是诸如无线电导航、卫星导航和天文导航等其他导航系统无法比拟的。但是，惯性导航系统最大弱点是其系统误差随时间积累，时间越长，误差越大。为了确保水下潜器的安全航行和武器的准确发射，必须利用外界位置信息对惯性导航系统进行周期的调整与校正。
[0003]地形辅助导航系统(Terrain-Aided Navigat1n, TAN),实质是由惯性导航系统(给出实时图)与传感器(实时图与基准图的联系和纽带)和数字地图(给出基准图)构成的组合导航系统，它作为一种高精度导航定位技术，只需要地形信息含量足够丰富并增加存储器的容量就能将导航精度提高近一个数量级，达到十几米的定位精度。地形辅助导航具有自主、隐蔽、连续、全天候工作，导航定位误差不随时间积累的优点，是水下潜器理想的辅助导航定位手段。
[0004]典型的基于ICCP算法的地形辅助导航系统可以在具有先验地形图且地形变化明显的海域，修正主惯性导航系统的位置误差。然而，地形辅助导航需要有先验的地形图，且要求有明显的地形变化，对于那些尚未测绘或者地形变化过于平缓，地形特征不明显的海域，用地形辅助导航方法来降低主惯性导航系统随时间积累的误差是十分困难的。
[0005]此时，可利用对具有时间稳定性的其它水下特征诸如水下建筑、沉船等的观测，采用基于海洋环境特征的同步定位与地图构建(Simultaneous Localizat1n andMapping, SLAM)算法来作为地形辅助导航的替代方法，以实时修正水下潜器的位置，降低导航误差的积累，改善导航系统的定位精度。
【发明内容】
[0006]要解决的技术问题:针对现有技术的不足，本发明提出一种结合地形和环境特征的水下导航定位方法，针对海底先验地形图不可得或者是地形信息量不丰富的海域，无法利用地形辅助导航系统来降低主惯性导航系统随时间积累的位置误差的技术问题。
[0007]技术方案:为解决上述技术问题，本发明采用以下技术方案:
[0008]一种结合地形和环境特征的水下导航定位方法，根据地形信息量判断水下潜器的规划航迹中各个子区域是否可匹配，若可匹配则采用地形辅助惯性导航系统实现定位，若不可匹配则采用同步定位与构图算法辅助主惯性导航系统实现定位。
[0009]地形辅助导航方法具有自主、隐蔽等优点，在地形信息量丰富的可匹配海域，一般是基于ICCP算法，根据先验地形图来修正惯性导航系统的误差，修正方法和修正过程均为现有技术，可以很好的辅助主惯性导航系统实现水下潜器的导航定位；而在地形不可匹配区域，同步定位与构图算法，发挥了自身对具有时间稳定性的环境特征进行观测来实现定位，并修正主惯性导航系统位置误差的特点，有效地弥补了地形辅助导航系统的不足；两种方法配合使用，使得水下潜器的导航定位具有更好的自主性和精确性。
[0010]进一步的，在本发明中，判断水下潜器的规划航迹中的各个子区域是否可匹配的方法如下:
[0011]步骤1.1:将海底地形高程采用网格矩阵的方式划分成多块地形候选区域，水下潜器的规划航迹上经过其中L块地形候选区域，设定某一块地形候选区域的经纬度跨度为MXN网格，并且网格点坐标为(i, j)处的地形高程值为height (i，j)，这里i = 1，2，...，M，j = I, 2,..., N ；
[0012]步骤1.2:利用移动计算窗口法计算地形候选区域的参数，具体方法为:
[0013]定义一个大小为mXn的局部移动计算窗口，并计算局部移动计算窗口内的地形
高度平均值
【权利要求】
1.一种结合地形和环境特征的水下导航定位方法，其特征在于:根据地形信息量判断水下潜器的规划航迹中各个子区域是否可匹配，若可匹配则采用地形辅助惯性导航系统实现定位，若不可匹配则采用同步定位与构图算法辅助主惯性导航系统实现定位。
2.根据权利要求1所述的结合地形和环境特征的水下导航定位方法，其特征在于:判断水下潜器的规划航迹中的各个子区域是否可匹配的方法如下: 步骤1.1:将海底地形高程采用网格矩阵的方式划分成多块地形候选区域，水下潜器的规划航迹上经过其中L块地形候选区域，设定某一块地形候选区域的经纬度跨度为MXN网格，并且网格点坐标为(i, j)处的地形高程值为height (i, j),这里i = I, 2,..., M, j =I, 2,..., N ； 步骤1.2:利用移动计算窗口法计算地形候选区域的参数，具体方法为: 定义一个大小为mXn的局部移动计算窗口，并计算局部移动计算窗口内的地形高度平均值
，当局部移动计算窗口的中心在各地形候选区域的全部网格点上移动一遍后，可以得到各地形候选区域的地形标准差σ (I)、经度方向上的地形相关系数Rlmgitude⑴以及纬度方向上的地形相关系数Rlatitude (I)，其中I = 1，2，...，L，下标longitude表示经度，latitude表示纟韦度,地形候选区域的参数具体计算公式如下:
步骤1.3:针对规划航迹上的L块地形候选区域，分别判断每一块地形候选区域的地形标准差和经纬度方向地形相关系数是否同时满足σ⑴>4σ。且Rlmgitude⑴<0.7且Rlatitude(I) < 0.7，若满足，则对应的地形候选区域为可匹配区域，否则为不可匹配区域，其中，σ。为测深传感器测量误差的标准差。
3.根据权利要求1所述的结合地形和环境特征的水下导航定位方法，其特征在于:所述的同步定位与构图算法包括以下步骤: 步骤3.1:系统非线性过程模型的建立 步骤3.1.1:水下潜器状态方程的建立 选取东北天系为导航坐标系，载体坐标系X轴沿水下潜器横轴指向右舷，载体坐标系I轴沿水下潜器纵轴指向前，载体坐标系ζ轴垂直于X轴和y轴所确定的平面构成右手坐标系；则k时刻水下潜器状态方程如下:
其中: P°Svehicle (k)表不k时刻水下潜器的位置， veIvehicle (k)表示k时刻水下潜器的速度， quavehicle(k)表示k时刻水下潜器的姿态四元数， 上标G表示导航坐标系， 上标B表示载体坐标系， At表示离散采样时间间隔， CB2e表示从载体坐标系变换到导航坐标系的方向余弦矩阵， fB(k)表示k时刻加速度计输出， g表示重力加速度， (々)表示由陀螺仪测得的角速率构成的四元数， ?)表示四元数乘法； 步骤3.1.2:地图状态的建立 设定k时刻观测到了 Nm个新的环境特征，则k时刻地图状态为:
其中: (dm示k时刻第ml个环境特征的状态， 4^re_,?2(k)表示k时刻第m2个环境特征的状态，
衰示k时刻第mNm个环境特征的状态， 上标T表示转置； 将水下环境特征建模为点特征，则环境特征在导航坐标系下的位置如下:
其中:表示在k时刻得到的第mi个环境特征在导航坐标系中的位置，
表示在k-Ι时刻得到的第mi个环境特征在导航坐标系中的位置， 表示水下潜器上的方位/距离传感器到水下潜器中心的杆臂效应补偿量在载体坐标系的分量， 下标S表示方位/距离传感器坐标系， Cs2b表示方位/距离传感器坐标系变换到载体坐标系的方向余弦矩阵，
表示方位/距离传感器测得的第mi个环境特征与水下潜器之间的相对位置，其中，d表示方位/距离传感器到环境特征之间的距离，Phi表示环境特征与方位/距离传感器之间的方位角，theta表示环境特征与方位/距离传感器之间的高度角； 步骤3.2:系统非线性观测模型的建立 步骤3.2.1:根据方位/距离传感器获得环境特征与水下潜器的相对位置观测，从而建立系统非线性观测模型如下:
z (k) = h (X (k)) + V (k) 其中: z(k)表示对环境特征的观测值， h(.)为非线性观测函数， x(k)为系统状态矢量，包括水下潜器的位置、速度、姿态四元数以及环境特征的位置， V (k)为系统观测噪声； k时刻对第mi个环境特征的观测只与该环境特征和水下潜器状态相关，因此有:
zmi (k) = h (Xvehicle (k)，Xfeaturejli (k)) + V mi (k) 其中: Zffli (k)表示k时刻对第mi个环境特征的观测， XvehiCle (k)表示水下潜器在k时刻的状态， Xfeature_mi (k)表示第Hli个环境特征在k时刻的状态， vffli(k)是对第mi个环境特征的观测噪声； 步骤3.2.2:设定
则k时刻对第mi个环境特征的观测为:
其中: d表示方位/距离传感器到环境特征之间的距离， phi表示环境特征与方位/距离传感器之间的方位角， theta表示环境特征与方位/距离传感器之间的高度角； 步骤3.3:系统状态增广过程 水下潜器航行过程中，方位/距离传感器观测到环境特征，若该环境特征为已观测到过的特征，则直接执行步骤3.5 ;若该环境特征为新出现的特征，则对系统状态进行增广并顺序执行步骤3.4和步骤3.5，增广后的系统状态变为:
其中: XvehiCle (k)表示k时刻水下潜器状态，包括水下潜器位置、速度和姿态四元数， Xfeature(k)表示k时刻环境特征的状态， fvehicle (.)为非线性状态转换函数； 步骤3.4:同步定位与构图算法预测过程 通过步骤3.1建立的系统非线性过程模型和步骤3.2建立的系统非线性观测模型对k时刻的系统状态以及状态协方差矩阵进行预测，有:
X(kIk-1) = f(x(k-1Ik-1))
其中: X (k I k-1)是根据k-1时刻的系统状态对k时刻的一步预测状态量， X (k-11 k-Ι)是k-1时刻的系统状态估计值， Pcov (k I k-Ι)是根据k-Ι时刻的系统状态对k时刻的预测协方差， Pcov(k-1 I k-1)是k-1时刻的状态协方差估计值，
为系统状态方程的雅可比矩阵， τ代表转置， Q(k)为噪声ω (k)的协方差； 步骤3.5:同步定位与构图算法更新过程 对于观测得到的环境特征，结合步骤3.2和步骤3.4对系统状态值进行更新，具体公式如下:
新息:vimTOV (k) = z (k) -h (X (k I k-1)) 滤波增益:
状态估计值:x(k|k) = X (k I k-1)+Kgain (k).Vinnov (k)
状态协方差:
其中:
Vinnov (k)为新息， z (k)为k时刻观测值， h (x (k I k-1))为k时刻观测预测值， KgainOO为k时刻滤波增益，
为系统观测方程的雅可比矩阵， R(k)为V (k)的协方差， x(k|k)为k时刻的系统状态估计值， Pcov (k I k)为k时刻的系统状态协方差； 步骤3.6:从x(k|k)中得到的p0Svehic;le即为修正后的主惯性导航位置信息。
【文档编号】G01C21/16GK104075715SQ201410320791
【公开日】2014年10月1日 申请日期:2014年7月7日 优先权日:2014年7月7日
【发明者】徐晓苏, 汤郡郡, 李佩娟, 张涛, 岳增阳 申请人:东南大学