校正模块105,利用所述斜率数据对所述AUV的导航位置进行校正；
[0050] 为了进一步得到目标AUV更精确的导航位置，可以获取AUV距海底的斜率数据作为 校正数据，并利用该斜率数据来校正前面所述的目标AUV的实时导航位置。示例性地，所述 校正数据获取模块可以包括水声应答器，通过水声应答器来获取斜率数据和/或方位夹角 作为所述校正数据。
[0051] 全源导航计算模块106,根据校正后的所述AUV的导航位置、AUV所在位置的准确地 形数据进行全源导航，得到AUV全源导航数据。
[0052] 在得到所述AUV的经校正后的导航位置数据以及AUV所在位置的准确地形数据之 后，即可根据这些数据来进行全源导航计算，得到综合了多种传感器/探测器的感测数据的 全源导航数据，该全源导航数据比基于任何一种单一的或少数几种传感器/探测器所得到 的导航数据更加精确和全面，参考价值更高。
[0053] 进一步地，所述全源导航计算模块可以包括:状态控制单元，根据所述基本导航模 块、准确地形获取模块、以及导航位置校正模块所输出数据的有效性得到状态控制数据；以 及数据融合单元，根据所述状态控制数据对校正后的所述AUV的导航位置、AUV所在位置的 准确地形数据进行数据融合，得到AUV全源导航数据。
[0054] 更进一步地，所述数据融合单元可以包括三个子滤波器以及相连的主滤波器，其 中，所述三个子滤波器的状态量均为AUV的位置、速度和姿态，测量量分别为校正后的所述 AUV的导航位置、AUV所在位置的准确地形数据、以及所述校正数据;所述主滤波器根据所述 状态控制数据控制各子滤波器的状态，并对校正后的所述AUV的导航位置、AUV所在位置的 准确地形数据进行数据融合。
[0055] 本实施例提供了一种具有融合多种传感器信息能力的全源导航系统。该系统充分 利用水下环境中任意可用的传感器信息，实现AUV的全源导航技术。该技术融合多种传感器 信息，可以提高AUV导航定位的适应性和自主性，从而提高定位结果的精度和可信度。
[0056] 实施例二
[0057] 图3是本实施例所述的用于AUV的全源导航方法的流程图，如图3所示，本实施例所 述的全源导航方法，包括如下步骤：
[0058] S1、获取AUV的运动数据，并根据该运动数据计算AUV的导航位置；
[0059] 目标AUV的运动数据通常包括运动姿态和速度等信息。对于运动姿态，可参见图2 所示，此处不再赘述。可以通过多种传感或探测设备获取目标AUV的运动数据，例如，IMU、 DVL、CVL、罗盘、磁力计等。在得到目标AUV的运动数据后即可根据该运动数据计算出AUV的 实时导航位置。
[0060] S2、获取AUV所在位置的实时地形数据；
[0061] 可以通过以下设备获取某一位置的实时地形数据:多波束测深仪、高度计、深度计 等。例如，通过深度计和高度计作为水深测量设备来测量目标AUV所处的水深值;利用多波 束测深仪测量目标AUV测深剖面的水深数据等。
[0062] S3、根据所述AUV的导航位置和所述实时地形数据与参考地形数据进行匹配，得到 AUV所在位置的准确地形数据；
[0063] 通过所述步骤S2所得到的实时地形数据作为第一手探测数据不够准确，通常还需 要进一步处理才可用于后续的导航过程。可以根据前面获取的所述AUV的实时导航位置和 所述实时地形数据与参考地形数据进行匹配，从而得到AUV所在位置的准确地形数据。 [0064] S4、获取AUV距海底的斜率数据；
[0065] S5、利用该斜率数据对所述AUV的导航位置进行校正；
[0066]为了进一步得到目标AUV更精确的导航位置，可以获取AUV距海底的斜率数据作为 校正数据，并利用该斜率数据来校正前面所述的目标AUV的实时导航位置。示例性地，所述 校正数据获取模块可以包括水声应答器，通过水声应答器来获取斜率数据和/或方位夹角 作为所述校正数据。
[0067] S6、根据校正后的所述AUV的导航位置、AUV所在位置的准确地形数据进行全源导 航计算，得到AUV全源导航数据。
[0068]在得到所述AUV的经校正后的导航位置数据以及AUV所在位置的准确地形数据之 后，即可根据这些数据来进行全源导航计算，得到综合了多种传感器/探测器的感测数据的 全源导航数据。
[0069]进一步地，所述步骤S6还包括:根据所述步骤S1、S3、S5所输出数据的有效性得到 状态控制数据；以及，根据所述状态控制数据对校正后的所述AUV的导航位置、AUV所在位置 的准确地形数据进行数据融合，从而得到AUV全源导航数据。
[0070] 本实施例提供一种具有融合多种传感器信息能力的全源导航方法。该方法充分利 用水下环境中任意可用的传感器信息，实现AUV的全源导航技术。该技术融合多种传感器信 息，可以提高AUV导航定位的适应性和自主性，从而提高定位结果的精度和可信度。
[0071] 实施例三
[0072] 下面描述用于AUV的全源导航方法的算法及相应的数据模型，具体如下所示：
[0073] (1)建立目标AUV的动力学模型：
[0074] AUV的运动状态一般包括沿着三个方向的位移和绕着三个方向的旋转，如图2所 示，这六种状态分别为横向位移（Sway)、前向位移（Surge)、纵向位移（Heave)、纵倾角 (Pitch)、横滚角(Roll)和航向角(Yaw)。
[0075] 本发明主要采用通用动力学模型和常规导航状态，将MU测量值作为测量更新值， 待估基本导航状态量如下所示：
[0076] X - (| L A Alt (<|)T (a^f' 1 )
[0077] 其中，U
[0078] Alt分别是大地炜度、经度和高程，vn是东北天速度，ab是载体坐标加速度，q是基于 载体姿态的四元素数组，Wnb是载体框架描述的载体坐标到NED框架的角速率。所有的状态量 都以导航系统平台的某个选取中心点为基准。
[0079] 构建状态量转移公式，位置状态的转移方程如下： # sec L < ^ Λ = 颜=一，
[0081] 其中，VN/VE/VD分别是Χ、Υ、Ζ轴的速度，移动载体的速度ν Ν、加速度ab代表由载体运 动引起的速度和加速度。因此，重力、Coriolis、曲面运动加速度等因此在动态方程中不考 虑;其影响仅表现在测量更新方程中。根据牛顿定律，将载体加速度转换为NED框架后，速度 的转移矩阵表示为：
[0082] ^
[0083] 采用F0GM模型描述加速度的状态转移，其方程(单轴方程)如下所示： $ 1 是 ?
[0084] αν --二一(厂 +
[0085] 该模型能够有效描述目标平台的动态特征，并将物理特性、可预估的机动等考虑 在内。
[0086] 四元素姿态转移方程为： Η I _ I [ 0 ]
[0087] q i/2Q(q| | 叫 ' 、L 4 J
[0088] Q(q)矩阵具有如下形式： -?? I a ,、 m m. - m m I
[0089] Q|<|) ^ I % 龜 f i ~'il2
[#4 --?3 ft fi J
[0090] 根据平台，使用修正的FOGM角速率模型，从NED框架到本体框架的角速率(单轴)状 态转移方程如下； 、.|! ..I h
[0091] ^ ~~
[0092] 其中，时间常量Twx和噪声参数Wwx由用户自定义，并且可根据平台动态特性进行适 应性调整。
[0093] 将所有状态纳入统一状态转移方程，有以下转移方程形式：
[0094] xCl) ^ ffx(f),u(H^] i-G(〇w(i)
[0095] 状态方程中，G(t)是噪声分布矩阵，在基本动态模型下，其具体形式为：
[0;, 0, | I | XSV , ;：：I | w<s ：
[0096] G ? I | | I ?3 (? I | W\;… I h I
[0097] 方程的噪声能量方程Q的形式为： Γ 1 ,、丨 Qd % I
[0098] Q 二丨 ξ I 私 cl I lx J1
[0099] 其中，Qa、Qw可通过FOGM时间常量和噪声参数按照GM模型推导；以下仅给出单轴的 计算公式：
[0100] = ·〒····:·、{.l.、=' .··;·····'··.
[0101] (2)利用步骤(1)中的动力学模型建立状态空间模型，求取线性动态矩阵