CN106908086A - 一种多普勒计程仪测速误差的修正方法 - Google Patents

Abstract

本发明公开了一种多普勒计程仪测速误差的修正方法。基于该修正方法的修正模型能够利用惯性导航系统(INS)提供的姿态信息，修正由载体俯仰角变化引起的多普勒计程仪(DVL)的测速误差，从而提高INS/DVL组合导航精度。在模型中不仅考虑了非零俯仰角对DVL测速误差的引入，还考虑了DVL发射信号时刻和接收信号时刻的时间差内载体俯仰角的变化量，以及俯仰角速率和DVL安装位置的杆臂引起的杆臂效应对DVL测速误差的引入，建立的误差修正模型更为准确。本发明选取导航系统提供的载体的姿态信息代入DVL测速误差的修正模型，数据选取简单、快速，实现对误差的有效修正。

Description

一种多普勒计程仪测速误差的修正方法

技术领域

本发明涉及导航系统及导航传感器技术领域，具体涉及一种多普勒计程仪测速误差的修正方法。

背景技术

在长航时导航中，由于惯性导航系统(INS)的误差随时间积累，因此，需要其他传感器来辅助来进行组合导航，从而抑制INS的误差积累。在水下潜器的长航时导航中，由于水下环境的特殊性，无线电信号在水中衰减迅速，致使以GPS等为代表的全球导航卫星系统(GNSS)无法在水下使用，需要寻找其他传感器来实现水下组合导航。

多普勒计程仪(DVL)作为一种精确的速度传感器，在水下组合导航中得到了广泛地应用，INS/DVL是目前实现水下自主导航的主要方式。DVL测速精度是INS/DVL组合导航系统精度的关键因素，在正常情况下，DVL在底跟踪模式下测速精度可以达到当前载体速度的1％，可以满足组合导航精度的需要。然而，当浪涌，或者载体的上浮、下潜运动等因素使载体水平姿态发生变化时，会给DVL的速度量测引入误差，从而影响组合导航精度。

发明内容

有鉴于此，本发明提供了一种多普勒计程仪测速误差的修正方法，基于该修正方法的修正模型能够利用惯性导航系统(INS)提供的姿态信息，修正由载体俯仰角变化引起的多普勒计程仪(DVL)的测速误差，从而提高INS/DVL组合导航精度。

为了解决上述技术问题，本发明采用的技术方案是：

利用公式(9)的修正模型对在Δt＝t₂-t₁时间内的多普勒计程仪量测速度进行修正：

其中V_h＝[V_hx V_hy V_hz]^T表示在Δt＝t₂-t₁时间内载体在水平坐标系h内的速度，为多普勒计程仪经测速误差修正后输出的速度；t₁为多普勒计程仪发射信号的时刻，t₂为多普勒计程仪接收到返回信号的时刻；V_{DVL_x}、V_{DVL_y}和V_{DVL_z}分别为多普勒计程仪量测得到的载体的右向、前向和向上的速度；θ为t₁时刻载体的俯仰角；Δθ为Δt时间内载体俯仰角的变化量；L_y和L_z分别为多普勒计程仪安装点在载体坐标系b Y^b和Z^b轴方向上偏离坐标系原点的距离；ω_θ1为t₁时刻载体的俯仰角变化速率，ω_θ2为t₂时刻载体的俯仰角变化速率。

其中，t₁时刻载体俯仰角变化速率ω_θ1、t₂时刻载体俯仰角变化速率ω_θ2、以及t₁时刻载体的俯仰角θ和t₂时刻载体的俯仰角(θ+Δθ)由载体的导航系统提供。较佳地，导航系统为SINS、INS或GNSS。

进一步地，修正模型的建立方法为：

步骤1，将多普勒计程仪在发射信号时刻t₁和接收信号时刻t₂沿前、右、后和左四条波束方向相对于水底的速度用含有俯仰角相关参数的表达式分别进行表达；

步骤2，依据步骤1得到的t₁和t₂时刻多普勒计程仪相对水底的速度的表达式，得到多普勒计程仪四条波束的频移[f_D1 f_D2 f_D3 f_D4]^T和载体坐标系b内的速度V_b的关系：

其中C为声波在水中的传播速度；f₀为多普勒计程仪发射信号的频率；α为多普勒计程仪发射信号方向与载体坐标系OX_bY_b平面之间的夹角；V_b＝[V_bx V_by V_bz]^T为载体坐标系b下的载体速度，V_bx、V_by和V_bz分别为载体坐标系b下载体的右向、前向和向上的速度；

步骤3，建立载体速度与量测速度在Δt＝t₂-t₁时间内的关系式，具体包括如下子步骤：

步骤3.1，实际采用的四波束Janus配置多普勒计程仪使用如下等式(5)计算载体速度，得到的载体速度为量测速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T：

步骤3.2，依据公式(3)，构建如下等式(4)：

步骤3.3，联立式(4)和式(5)，得到载体在载体坐标系b下的速度[V_bx V_by V_bz]^T与量测速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T在Δt时间内的关系式：

步骤4，将载体在载体坐标系b下的速度转换到水平坐标系h下；

步骤5，在Δt时间内，载体在水平坐标系h内的速度[V_hx V_hy V_hz]^T为：

式(9)即为多普勒计程仪测速误差的修正模型。

其中，步骤1中t时刻多普勒计程仪沿四条波束方向相对于水底的速度表达式为：

其中t＝t₁时，ω_θi＝ω_θ1，t＝t₂时，ω_θi＝ω_θ2。

其中，步骤4中，具体的坐标转换方法如下：t₁时刻，水平坐标系h到载体坐标系b的变换矩阵为：

t₂时刻，水平坐标系h到载体坐标系b的变换矩阵为：

依据式(6)载体在载体坐标系b下的速度，t₁时刻水平坐标系h到载体坐标系b的变换矩阵，在t₁时刻载体在水平坐标系h下的速度为：

依据式(6)载体在载体坐标系b下的速度，t₂时刻水平坐标系h到载体坐标系b的变换矩阵，在t₂时刻载体在水平坐标系h下的速度为：

有益效果：

(1)建立了载体俯仰角变化下多普勒计程仪测速误差的修正模型，在模型中不仅考虑了非零俯仰角对DVL测速误差的引入，还考虑了DVL发射信号时刻和接收信号时刻的时间差内载体俯仰角的变化量，以及俯仰角速率和DVL安装位置的杆臂引起的杆臂效应对DVL测速误差的引入，建立的误差修正模型更为准确。

(2)多普勒计程仪测速误差修正方法中利用所建立的误差修正模型，选取导航系统提供的载体的姿态信息代入DVL测速误差的修正模型，数据选取简单、快速，实现对误差的有效修正。

(3)多普勒计程仪测速误差修正方法应用在INS/DVL组合导航系统中，有效提高DVL测速精度，进而提高INS/DVL组合导航系统的精度。

附图说明

图1为本发明的修正方法流程图；

图2为俯仰角和杆臂效应影响下的DVL测速原理图；

图3为矫正前后的DVL速度误差对比图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明在建立修正模型时，全面考虑了俯仰角θ、DVL发射和接收信号时刻的时间差Δt内载体俯仰角的变化量Δθ以及DVL安装位置和载体摇摆中心间的杆臂L和载体俯仰角速率ω引起的杆臂效应对DVL测速误差的引入，使得建立的修正模型更为精确，完成DVL测速误差修正后得到更为精确的导航结果。

本发明中的坐标系及变量的定义：

导航坐标系n：原点O位于载体中心，Xⁿ轴和Yⁿ轴分别在当地水平面内指向东和北，Zⁿ轴垂直于当地水平面指向天向。

载体坐标系b：原点O位于载体中心，Y^b轴沿载体纵轴指向前，Z^b轴垂直于载体平面指向上，X^b与Y^b和Z^b构成右手坐标系。

水平坐标系h：原点O位于载体中心，X^h和Y^h是X^b与Y^b在水平面上的投影，Z^h垂直于水平面指向上。

t₁：DVL发射信号的时刻；

t₂：DVL接收到返回信号的时刻；

Δt：t₁时刻到t₂时刻的时间差；

θ：载体俯仰角；

Δθ：Δt时间内载体俯仰角的变化量；

O：载体的摇摆中心；

M：DVL的安装位置；

L_x L_y L_z：M在载体坐标系b内分别在X^b、Y^b和Z^b轴方向上偏离O点的距离；

L：M到O点的直线距离；

ω_θ1：t₁时刻载体的俯仰角变化速率；

ω_θ2：t₂时刻载体的俯仰角变化速率；

D₁：DVL前向波束在水底的反射位置点；

D₂：DVL右向波束在水底的反射位置点；

D₃：DVL后向波束在水底的反射位置点；

D₄：DVL左向波束在水底的反射位置点；

V_bx：载体坐标系b下的右向速度，在Δt内设为常值；

V_by：载体坐标系b下的前向速度，在Δt内设为常值；

V_bz：载体坐标系b下向上的速度，在Δt内设为常值；

V_hx：水平坐标系h下载体的右向速度；

V_hy：水平坐标系h下载体的前向速度；

V_hz：水平坐标系h下载体向上的速度；

V_{DVL_x}：DVL量测得到的载体的右向速度；

V_{DVL_y}：DVL量测得到的载体的前向速度；

V_{DVL_z}：DVL量测得到的载体向上的速度；

β：载体坐标系b下M点到O点的连线与Y^b轴之间的夹角，为定值；

C：声波在水中的传播速度，设为常数值。

α：多普勒计程仪发射信号方向与载体坐标系Y^b轴之间的夹角，为常数值；

f₀：DVL发射信号的频率；

前向波束发射信号和接收信号之间的频移；

右向波束发射信号和接收信号之间的频移；

后向波束发射信号和接收信号之间的频移；

左向波束发射信号和接收信号之间的频移。

根据DVL测速原理可知，安装在载体上的DVL测量的是载体相对地的速度，在载体无俯仰角和横滚角变化的情况下，载体坐标系b和水平坐标系h重合，DVL测量的是载体坐标系b系下的速度，然而实际上DVL测量的应该是水平坐标系h系下的速度，当载体存在俯仰角和横滚角变化情况下，载体坐标系b和水平坐标系h不再重合，此时如果再认为DVL测量的是载体坐标系b下的速度就会出现偏差。在原理上横滚角对DVL测速误差的影响和俯仰角相同，所以为了简化模型，在本发明中重点讨论俯仰角对DVL测速误差的影响及修正。

本发明提供了一种多普勒计程仪测速误差的修正方法，设定在自身驱动下，载体坐标系b下的速度在Δt时间内保持不变，而导航坐标系n和水平坐标系h系下的速度在俯仰角变化的影响下实时发生变化。

本发明中DVL采用詹纳斯(Janus)配置，在载体坐标系b下分别向前下、左下、后下、右下四个方向发射，四个波束相隔90度分布，图2所示为俯仰角和杆臂效应影响下的DVL测速原理图。图2为纵面剖图，为清晰表达各夹角，只绘出对应反射点为D₁和D₃的波束，横向两条波束及对应的反射点D₂和D₄未在图2中绘出；

在Δt时间内俯仰角和杆臂效应影响下的DVL测速误差的修正模型为公式(9)，将含有误差的DVL的量测值代入公式(9)，得到修正后的更为精确的载体速度[V_hx V_hy V_hz]^T：

其中，V_{DVL_x}为多普勒计程仪量测得到的载体的右向速度；V_{DVL_y}为多普勒计程仪量测得到的载体的前向速度；V_{DVL_z}为多普勒计程仪量测得到的载体向上的速度；θ为多普勒计程仪信号发射的时刻载体的俯仰角；Δθ为Δt时间内载体俯仰角的变化量；L_y和L_z为M点在载体坐标系b内分别在Y^b和Z^b轴方向上偏离O点的距离；ω_θ1为t₁时刻载体的俯仰角变化速率，ω_θ2为t₂时刻载体的俯仰角变化速率。

多普勒计程仪测速误差的修正模型建立方法如下：

根据四波束Janus配置DVL测速原理，DVL安装位置即M点沿四条波束发射方向相对于水底的速度的表达式为：

在t₁时刻由杆臂L和俯仰角速度ω_θ1引起杆臂速度为V_Lθ1＝L·ω_θ1，速度方向如图1所示，同理，在DVL信号接收的t₂时刻，M点沿四条波束返回方向相对于水底的速度的表达式为：

其中V_Lθ2＝L·ω_θ2，V_Lθ2为由杆臂L和t₂时刻载体的俯仰角变化速率ω_θ2引起杆臂速度。

根据V_trans和V_rec，得到多普勒计程仪四条波束发射信号和接收信号之间的频移[f_D1 f_D2 f_D3 f_D4]^T和V_b之间的关系式，其中V_b＝[V_bx V_by V_bz]^T：

依据公式(3)，可以构造如下等式(4)：

而在实际应用中，四波束Janus配置DVL使用如下等式(5)计算载体速度，得到的载体速度为量测速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T：

由于f_D1、f_D2、f_D3、f_D4为量测值，即式(4)和式(5)等号的左半部分都是通过量测得到的频移量，无论使用式(4)或式(5)计算速度，量测值f_D1、f_D2、f_D3、f_D4是不会变的，故式(4)和式(5)左半部分相等，联立两式并化简，可以得到V_b与V_DVL的关系式：

由式(6)可以从DVL测量值计算出Δt时间内载体在载体坐标系b下的速度。

当俯仰角为θ时，t₁时刻水平坐标系h到载体坐标系b的变换矩阵为：

t₂时刻水平坐标系h到载体坐标系b的变换矩阵为：

依据式(6)求得的载体在载体坐标系b下的速度，t₁时刻水平坐标系h到载体坐标系b的变换矩阵，得到在t₁时刻载体在水平坐标系h下的速度为：

依据式(6)载体在载体坐标系b下的速度，t₂时刻水平坐标系h到载体坐标系b的变换矩阵，得到在t₂时刻载体在水平坐标系h下的速度为：

在Δt内，载体在水平坐标系h内的速度[V_hx V_hy V_hz]^T近似等于t₁时刻载体在水平坐标系h下的速度与t₂时刻载体在水平坐标系h下的速度的平均值，表达式如下：

式(9)即为俯仰角和杆臂效应影响下的DVL测速误差的修正模型，将含有误差的DVL的量测值代入式(9)，得到修正后的更为精确的载体速度，[V_hx V_hy V_hz]^T为载体在水平坐标系h内的速度，为DVL信号发射时刻即t₁时刻，载体在水平坐标系h下的速度，为DVL信号接收时刻即t₂时刻，载体在水平坐标系h下的速度。

本发明多普勒计程仪测速误差的修正方法为：

步骤1，建立起在Δt＝t₂-t₁时间内如公式(9)的修正模型：

步骤2，利用INS提供的载体姿态信息修正DVL测速误差：

由于INS可以提供载体完整的姿态信息，故可以利用t₁时刻陀螺仪提供的俯仰角变化速率ω_{θ1_SINS}，t₂时刻陀螺仪提供的俯仰角变化速率ω_{θ2_SINS}，以及INS解算得到的俯仰角θ_SINS和(θ+Δθ)_SINS修正DVL测速误差，将其代入公式(9)，得到如下公式(10)的修正模型：

步骤3，利用由公式(10)得到的V_h代替DVL的直接量测值V_DVL进行组合导航，完成DVL测速误差的修正。

横滚角和杆臂效应影响下的DVL测速误差的修正模型构建方法与步骤1类似。本发明具体实施中更多地采用捷联惯性导航系统(SINS)来提供载体的姿态信息。在矫正效果仿真实验中：设L_y＝2.5m，L_z＝0.5m，其他条件如下：

(1)捷联惯性导航系统(SINS)：

陀螺常值漂移0.01°/h；随机漂移0.01°/h

加表常值零偏10^-4g；加表随机零偏5×10^-5g

(2)多普勒计程仪(DVL)：

测速常值误差1cm/s；随机误差1cm/s；更新频率1Hz

仿真时间设定为1200s，初始位置载体系下速度恒定为[0 2]，0-600s载体以姿态[0° 0° 45°]匀速直线运动，600-1200s在海浪的激励下，载体开始绕俯仰轴摇摆运动，摇摆运动模型为θ_msin(ω_θt+φ_θ)，摇摆幅值θ_m＝20°，摇摆周期T_θ＝50s，ω_θ＝2π/T_θ。

图3所示为多普勒计程仪测速误差修正前和修正后的对比图，由对比图可知本发明提出的修正方法是有效的。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种多普勒计程仪测速误差的修正方法，其特征在于，利用公式(9)的修正模型对在Δt＝t₂-t₁时间内的多普勒计程仪量测速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T进行修正：

$\left[\begin{array}{c}{V}_{hx}\\ V_{hy}\\ V_{hz}\end{array}\right]=\left[\begin{array}{c}{V}_{DVL\_x}\\ \frac{\cos \mathrm{\θ}+\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_y}-\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{2}{L}_z)-\frac{\sin \mathrm{\θ}+\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_z}+\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{4}{L}_y)\\ \frac{\sin \mathrm{\θ}+\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_y}-\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{2}{L}_z)+\frac{\cos \mathrm{\θ}+\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_z}+\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{4}{L}_y)\end{array}\right]---\left(9\right)$

其中V_h＝[V_hx V_hy V_hz]^T表示在Δt＝t₂-t₁时间内载体在水平坐标系h内的速度，为多普勒计程仪经测速误差修正后输出的速度；t₁为多普勒计程仪发射信号的时刻，t₂为多普勒计程仪接收到返回信号的时刻；V_{DVL_x}、V_{DVL_y}和V_{DVL_z}分别为多普勒计程仪量测得到的载体的右向、前向和向上的速度；θ为t₁时刻载体的俯仰角；Δθ为Δt时间内载体俯仰角的变化量；L_y和L_z分别为多普勒计程仪安装点在载体坐标系b的Y^b和Z^b轴方向上偏离坐标系原点的距离；ω_θ1为t₁时刻载体的俯仰角变化速率，ω_θ2为t₂时刻载体的俯仰角变化速率。

2.如权利要求1所述的一种多普勒计程仪测速误差的修正方法，其特征在于，t₁时刻载体俯仰角变化速率ω_θ1、t₂时刻载体俯仰角变化速率ω_θ2、以及t₁时刻载体的俯仰角θ和t₂时刻载体的俯仰角(θ+Δθ)由载体的导航系统提供。

3.如权利要求2所述的一种多普勒计程仪测速误差的修正方法，其特征在于，所述导航系统为SINS、INS或GNSS。

4.如权利要求1所述的一种多普勒计程仪测速误差的修正方法，其特征在于，所述修正模型的建立方法为：

步骤1，将多普勒计程仪在发射信号时刻t₁和接收信号时刻t₂沿前、右、后和左四条波束方向相对于水底的速度用含有俯仰角相关参数的表达式分别进行表达；

步骤2，依据步骤1得到的t₁和t₂时刻多普勒计程仪相对水底的速度的表达式，得到多普勒计程仪四条波束的频移[f_D1 f_D2 f_D3 f_D4]^T和载体坐标系b内的速度V_b的关系：

$\left[\begin{array}{c}{f}_{D1}\\ f_{D2}\\ f_{D3}\\ f_{D4}\end{array}\right]\≈\frac{2{\mathrm{cos\αf}}_0}{C}\left[\begin{array}{ccc}0& 1& -\tan \mathrm{\α}\\ 1& 0& -\tan \mathrm{\α}\\ 0& -1& -\tan \mathrm{\α}\\ -1& 0& -\tan \mathrm{\α}\end{array}\right]\left[\begin{array}{c}{V}_{bx}\\ V_{by}\\ V_{bz}\end{array}\right]+\frac{f_0}{C}\left[\begin{array}{c}({\mathrm{sin\αL}}_y+{\mathrm{cos\αL}}_z)({\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2})\\ 0\\ ({\mathrm{sin\αL}}_y-{\mathrm{cos\αL}}_z)({\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2})\\ 0\end{array}\right]---\left(3\right)$

其中f_D1、f_D2、f_D3和f_D4分别为多普勒计程仪四条波束发射信号和接收信号之间的频移；C为声波在水中的传播速度；f₀为多普勒计程仪发射信号的频率；α为多普勒计程仪发射信号方向与载体坐标系的OX_bY_b平面之间的夹角；V_b＝[V_bx V_by V_bz]^T为载体坐标系b下的载体速度，V_bx、V_by和V_bz分别为载体坐标系b下载体的右向、前向和向上的速度；

步骤3，建立载体速度[V_bx V_by V_bz]^T与量测速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T在Δt＝t₂-t₁时间内的关系式，具体包括如下子步骤：

步骤3.1，实际采用的四波束Janus配置多普勒计程仪的速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T满足如下等式(5)：

$\left[\begin{array}{c}{f}_{D1}-f_{D3}\\ f_{D2}-f_{D4}\\ f_{D1}+f_{D2}+f_{D3}+f_{D4}\end{array}\right]=\frac{4{\mathrm{cos\αf}}_0}{C}\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& -2tan\mathrm{\α}\end{array}\right]\left[\begin{array}{c}{V}_{DVL\_x}\\ V_{DVL\_y}\\ V_{DVL\_z}\end{array}\right]---\left(5\right)$

步骤3.2，依据公式(3)，构建如下等式(4)：

$\left[\begin{array}{c}{f}_{D1}-f_{D3}\\ f_{D2}-f_{D4}\\ f_{D1}+f_{D2}+f_{D3}+f_{D4}\end{array}\right]=\frac{4{\mathrm{cos\αf}}_0}{C}\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& -2tan\mathrm{\α}\end{array}\right]\left[\begin{array}{c}{V}_{bx}\\ V_{by}\\ V_{bz}\end{array}\right]+\frac{2{\mathrm{cos\αf}}_0}{C}\left[\begin{array}{c}{L}_z({\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2})\\ 0\\ {\mathrm{tan\αL}}_y({\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2})\end{array}\right]---\left(4\right)$

步骤3.3，联立式(4)和式(5)，得到载体在载体坐标系b下的速度[V_bx V_by V_bz]^T与与量测速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T在Δt时间内的关系式：

$\left[\begin{array}{c}{V}_{bx}\\ V_{by}\\ V_{bz}\end{array}\right]=\left[\begin{array}{c}{V}_{DVL\_x}\\ V_{DVL\_y}\\ V_{DVL\_z}\end{array}\right]-\left[\begin{array}{c}0\\ \frac{1}{2}{L}_z({\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2})\\ -\frac{1}{4}{L}_y({\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2})\end{array}\right]---\left(6\right)$

步骤4，将载体在载体坐标系b下的速度转换到水平坐标系h下；

步骤5，在Δt时间内，载体在水平坐标系h内的速度[V_hx V_hy V_hz]^T为：

$\left[\begin{array}{c}{V}_{hx}\\ V_{hy}\\ V_{hz}\end{array}\right]=\left[\begin{array}{c}{V}_{DVL\_x}\\ \frac{\cos \mathrm{\θ}+\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_y}-\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{2}{L}_z)-\frac{\sin \mathrm{\θ}+\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_z}+\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{4}{L}_y)\\ \frac{\sin \mathrm{\θ}+\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_y}-\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{2}{L}_z)+\frac{\cos \mathrm{\θ}+\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})}{2}(V_{DVL\_z}+\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{4}{L}_y)\end{array}\right]---\left(9\right)$

式(9)即为多普勒计程仪测速误差的修正模型。

5.如权利要求4所述的一种多普勒计程仪测速误差的修正方法，其特征在于，所述步骤1中t时刻多普勒计程仪沿四条波束方向相对于水底的速度表达式为：

$\left[\begin{array}{c}{V}_{D1,t}\\ V_{D2,t}\\ V_{D3,t}\\ V_{D4,t}\end{array}\right]=\left[\begin{array}{ccc}0& \cos \mathrm{\α}& -\sin \mathrm{\α}\\ \cos \mathrm{\α}& 0& -\sin \mathrm{\α}\\ 0& -\cos \mathrm{\α}& -\sin \mathrm{\α}\\ -\cos \mathrm{\α}& 0& -\sin \mathrm{\α}\end{array}\right]\left[\begin{array}{c}{V}_{bx}\\ V_{by}\\ V_{bz}\end{array}\right]+\left[\begin{array}{c}{\mathrm{sin\αL}}_y+{\mathrm{cos\αL}}_z\\ 0\\ {\mathrm{sin\αL}}_y-{\mathrm{cos\αL}}_z\\ 0\end{array}\right]{\mathrm{\ω}}_{\mathrm{\θ}i}---\left(1\right)$

其中t＝t₁时，ω_θi＝ω_θ1，t＝t₂时，ω_θi＝ω_θ2。

6.如权利要求4所述的一种多普勒计程仪测速误差的修正方法，其特征在于，所述步骤4中，具体的坐标转换方法如下：t₁时刻，水平坐标系h到载体坐标系b的变换矩阵为：

$C_h^{b_{t_1}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& sin\mathrm{\θ}\\ 0& -sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right];$

t₂时刻，水平坐标系h到载体坐标系b的变换矩阵为：

$C_h^{b_{t_2}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})& sin(\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})\\ 0& -sin(\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})& \cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})\end{array}\right];$

依据式(6)载体在载体坐标系b下的速度，t₁时刻水平坐标系h到载体坐标系b的变换矩阵，在t₁时刻载体在水平坐标系h下的速度为：

$\left[\begin{array}{c}{V}_{{\mathrm{hx}}_{t_1}}\\ V_{{\mathrm{hy}}_{t_1}}\\ V_{{\mathrm{hz}}_{t_1}}\end{array}\right]={\left(C_h^{b_{t_1}}\right)}^T\left[\begin{array}{c}{V}_{bx}\\ V_{by}\\ V_{bz}\end{array}\right]=\left[\begin{array}{c}{V}_{DVL\_x}\\ V_{DVL\_y}\cos \mathrm{\θ}-V_{DVL\_z}\sin \mathrm{\θ}\\ V_{DVL\_y}\sin \mathrm{\θ}+V_{DVL\_z}\cos \mathrm{\θ}\end{array}\right]-\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{2}\left[\begin{array}{c}0\\ L_z\cos \mathrm{\θ}+\frac{1}{2}{L}_y\sin \mathrm{\θ}\\ L_z\sin \mathrm{\θ}-\frac{1}{2}{L}_y\cos \mathrm{\θ}\end{array}\right]---\left(7\right)$

依据式(6)载体在载体坐标系b下的速度，t₂时刻水平坐标系h到载体坐标系b的变换矩阵，在t₂时刻载体在水平坐标系h下的速度为：

$\left[\begin{array}{c}{V}_{{\mathrm{hx}}_{t_2}}\\ V_{{\mathrm{hy}}_{t_2}}\\ V_{{\mathrm{hz}}_{t_2}}\end{array}\right]={\left(C_h^{b_{t_2}}\right)}^T\left[\begin{array}{c}{V}_{bx}\\ V_{by}\\ V_{bz}\end{array}\right]=\left[\begin{array}{c}{V}_{DVL\_x}\\ V_{DVL\_y}\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})-V_{DVL\_z}\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})\\ V_{DVL\_y}\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})+V_{DVL\_z}\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})\end{array}\right]-\frac{{\mathrm{\ω}}_{\mathrm{\θ}1}+{\mathrm{\ω}}_{\mathrm{\θ}2}}{2}\left[\begin{array}{c}0\\ L_z\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})+\frac{1}{2}{L}_y\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})\\ L_z\sin (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})-\frac{1}{2}{L}_y\cos (\mathrm{\θ}+\mathrm{\Δ}\mathrm{\θ})\end{array}\right]---\left(8\right).$