声学多普勒速度仪（DVL）是一种测量相对于水底速度的声纳设备。早年DVL一开始被设计主要是用于船舶的定位导航中，但是近年来随着水下机器人领域，尤其是民用领域的兴起（据有关机构测算这一市场规模已达100亿美元），DVL的作用显得愈发重要，因为水下机器人想要在水下实现自主导航有三样东西必不可少，一个是姿态测量单元，一个是速度测量单元，一个是位置测量单元。姿态测量单元通常是通过MEMS惯导元件或者光纤惯导元件等测量得到的，而MEMS通常价格非常便宜，非常容易获得，因此它可以被大量广泛的用在水下机器人中；位置测量单元可以由GPS、USBL等设备构成，获取的难度相对容易，成本也不算太高；最后一个速度测量单元目前最有效的手段就是通过DVL来获取，但其成本高昂，售价一般在十几万以上甚至更高，因此在目前民用的水下机器人上很少有它的身影。目前水下机器人市场还没有完全爆发，主要原因就是相关的传感器不够便宜，许多关键部件的成本没有降下来，国内外在这一领域还没有出现独角兽。笔者正是意识到了这一点，所以才下定决心来研究DVL，希望有志同道合的朋友能和笔者一起将DVL的成本降下来，让水下机器人市场彻底爆发的这一天来的更早一些。

1.多普勒测速原理

1.1多普勒效应

多普勒效应是由于声源与观测者之间存在着相对运动，使观测者听到的声音频率不同于声源所发出声音频率的现象。这是由奥地利物理学家Christian Doppler 于十九世纪在声学领域首先发现的物理学原理。

典型的多普勒效应发生于声源和观测者之间。在声源静止条件下，当观测者向着声源运动时，他收到的声波频率高于他相对声源静止时收到的声波频率；当观测者远离声源而去时，他收到的声波频率低于他相对声源静止时收到的声波频率。声源于观测者之间的相对运动速度越大，则所产生的声波频率变化也就越大。有关多普勒效应的研究不仅在声学领域可以看到，利用多普勒效应进行的研究还可扩展到电磁学，光学等其它领域。

下面推导任意方向上的多普勒效应公式。如图 1 所示，S 为声源，D为观测者， $\overrightarrow{S{S}'}$ 为声源的运动矢量， $\overrightarrow{D{D}'}$ 为观测者的运动矢量，这时从声源 S 到观测者 D 传播方向的矢量 SD 随时在变。设声源在时刻t=t0 和t0+dt 的位置分别为S 和 S′ ，相位分别为 $\varphi$ 和 ${\varphi }'$ ，其中相位的增量为 $d\varphi ={\varphi }'-\varphi =2\pi f_{t}dt$ 。在t0时刻，由声源 S 发出的相位为 $\varphi$ 的声波传播到 D,观测者的位置在 D ；t0+dt 时刻，声源到达 S′ ，由于观测者也在运动，所以，声源 S′ 发出的相位为 $\varphi$ 的声波只能到达观测者的新位置D′，观测者从D走到D′所用的时间dt′和他感受到的频率 $f_{r}$ ，与声源的 $dt$ 和 $f_{t}$ 是不一样的。在D接收的频率 $f_{t}$ ，在D′接收的频率 $f_{r}$ ，所接收声波的相位变化了 $d\varphi =2\pi f_{r}dt^{'}$ ，由于相位增量是一样的，即 $d\varphi =2\pi f_{t}dt=2\pi f_{r}dt^{'}$ 故

$f_{r}/f_{t} = dt/dt^{'}$ (1)

相位 $\varphi$ 从声源 S 传播到观测者的位置 D 的时刻为 $t=t_{0}+\left \| \vec{SD} \right \|/c$ ；而相位 $\varphi +d\varphi$ 由声源 S′ 传播到观测者的位置 D′ 的时刻为 $t=t_{0}+dt+\left \| \vec{{S}'{D}'} \right \|/c$ 。二者之差即为

$d{t}'=dt+\left \| \vec{{S}'{D}'} \right \|/c-\left \| \vec{SD} \right \|/c$ (2)

如图 1 所示，从 ${S}'$ 、 ${D}'$ 作SD的垂线，令相应的垂足分别为 $S_{0}$ 、 $D_{0}$ 。由于 $S_{0}D_{0}$ 与 ${S}'{D}'$ 的长度相差高阶无穷小量，可认为二者大小近似相等，于是

$\left \| \vec{SD} \right \|-\left \| \vec{{S}'{D}'} \right \|\approx \left \| \vec{SD} \right \|-\left \| \vec{S_{0}D_{0}} \right \|=\left \| \vec{SS_{0}} \right \|+\left \| \vec{DD_{0}} \right \|$

$=\left \| \vec{S{S}'} \right \|\cos \theta +\left \| \vec{D{D}'} \right \|\cos \psi$ (3)

式中，θ 是 $\vec{S{S}'}$ 与 $\vec{SD}$ 之间的夹角，ψ 则是 $\vec{S{D}'}$ 与 $\vec{DS}$ 之间的夹角。

由于声波在水体介质中的传播速度远大于声源和观测者的运动速度，可认为声源和观测者近似作匀速直线运动，因此 $\left \| \vec{S{S}'} \right \|=v_{t}dt$ ， $\left \| \vec{D{D}'} \right \|=v_{r}d{t}'$ 。由式(2)和式(2-3)解得

$d{t}'=dt-v_{t}dt\cos \theta /c-v_{r}d{t}'\cos \psi /c$ (4)

由此得到

$\frac{f_{r}}{f_{t}}=\frac{dt}{d{t}'}=\frac{c+v_{r}\cos \psi }{c-v_{t}\cos \theta }$ (5)

这便是多普勒效应的普遍公式。

1.2多普勒测速公式

不难看出，当θ 、ψ 都等于0 时，式(5)可以过渡到共线情形

$f_{r}=\frac{c+v_{r}}{c-v_{t}}f_{t}$ (6)

由式（6）可知，当声源与观测者相互靠近时， $f_{r}> f_{t}$ ；当声源与观测者相互远离时， $f_{r}< f_{t}$ 。

对于DVL而言，收发合制换能器。换能器首先作为声源，水底作为观测者，于是根据是（6）水底接收的声波频率为：

${f}'=\frac{c+v_{r}}{c-v_{t}}f_{t}$ (7)

之后水底作为声源，而换能器则作为观测者，于是换能器接收的声波频率为：
$f_{r}=\frac{c+v_{t}}{c-v_{r}}{f}'$ (8)

整合式（7）与式（8），得到：

$f_{r}=\frac{(c+v_{r})(c+v_{t})}{(c-v_{t})(c-v_{r})}f_{t}$ (9)

换能器相对静止、即当 $v_{t}=0$ 时， $v_{r}$ 就是水底相对换能器的径向流速。于是，式(9)经过整理得到：

$f_{r}=\frac{c+v_{r}}{c-v_{r}}f_{t}=\frac{1+2v_{r}/c+(v_{r}/c)^{2}}{1-(v_{r}/c)^{2}}f_{t}$ (10)

由此经过整理可以得到通用的径向测流基本公式

$v_{r}=\frac{f_{r}-f_{t}}{f_{r}+f_{t}}c=\frac{f_{d}}{2f_{t}+f_{d}}c$ (11)

其中 $f_{d}=f_{r}-f_{t}$ 为多普勒频移。由于水中声速 c 约为1500m/s，远大于水体流速 $v_{r}$ ，于是一般可以忽略式(10)中 $v_{r}/c$ 的二次项，再经过整理就可以得到近似的多普勒径向测流公式：

$v_{r}=\frac{c}{2f_{t}}(f_{r}-f_{t})=\frac{\lambda }{2}f_{d}$ (12)

式(2-12)中， $\lambda =c/f_{t}$ 为换能器发射的信号波长， $f_{d}$ 为收发信号之间的多普勒频移。由此通过测量 $f_{d}$ 就可以得到换能器相对径向水底速度 $v_{r}$ ，并可以进一步得到水平与垂直方向的速度。

2.DVL换能器阵型与坐标变换

2.1Janus阵型结构

本节介绍多普勒测流常用的四波束Janus阵型结构。具有该换能器阵型结构的DVL可以同时发射和同时接收四个窄波束声波，再经处理就可以得到相对于声学换能器的径向流速信息。在此定义DVL的右手坐标系D 。D 的原点位于 Janus 阵型结构的等效中心O，它的三个轴 $x_{d}$ 、 $y_{d}$ 和 $z_{d}$ 分别指向测流系统所规定的前、右和下的方向。DVL的四波束Janus阵型结构是由四个换能器构成，各换能器收发声波的轴线与测流系统坐标系z轴之间的夹角为α，如图 2 所示。

图2.四波束Janus阵型结构

另外，各换能器收发声波的轴线在基阵坐标系 xoy 平面的投影则形成两条相互垂直的直线，且波束 1 的投影与x 轴之间的夹角为 β，如图3 所示。通常测流系统成“X”字形式安装时的 β 等于 45°，成“十”字形式安装时的 β 等于 0°。配有四波束 Janus 阵型结构的DVL在工作时，换能器同时发射的窄波束声波，沿着各自换能器的轴线方向传播。在传播的过程中，每路声波被该路水体散射，会有散射回波被各换能器接收。DVL换能器和水体之间的径向运动使得收发声波频率之间存在多普勒频移。通过测量多普勒频移就可利用式(12)解算出径向流速 $v_{1}$ ， $v_{2}$ ， $v_{3}$ 和 $v_{4}$ 。

图3.四波束Janus阵型结构

2.2基阵坐标系下的速度

设水底在D下的速度矢量为 $\mathbf{V}_{d}=[v_{dx},v_{dy},v_{dz}]^{T}$ 。同时假设DVL所测水底是均匀水平的。配有四波束 Janus 阵型结构的DVL利用任意三个声波所测流速就可以转换为D下的流速矢量 $\mathbf{V}_{d}$ 。即使因为某一波束受到干扰不能正常测量时系统也可以工作，因此提高了测量的可靠性。以第 1、2 和 3 波束测得的流速列矢量 $\mathbf{V}_{b}=[v_{1},v_{2},v_{3}]^{T}$ 为例，得到 $\mathbf{V}_{b}$ 与 $\mathbf{V}_{d}$ 的关系为：

$\mathbf{V}_{b}=\mathbf{A}\mathbf{V}_{d}$ (13)

$\mathbf{V}_{d}=(\mathbf{A}^{T}\mathbf{A})^{-1}\mathbf{A}^{T}\mathbf{V}_{b}$ (14)

其中

$\mathbf{A}=\begin{bmatrix} \sin \alpha \cos \beta &\sin \alpha \sin \beta &\cos \alpha \\ -\sin \alpha \cos \beta&\sin \alpha \cos \beta &\cos \alpha \\ -\sin \alpha \cos \beta &-\sin \alpha \sin \beta & \cos \alpha \end{bmatrix}$ (15)

$\left ( \mathbf{A}^{T}\mathbf{A} \right )^{-1}\mathbf{A}^{T}=\begin{bmatrix} 1/\sin \alpha \cos \beta &-1/\sin \alpha \sin \beta &-1/\sin \alpha \cos \beta \\ 1/\sin \alpha \sin \beta&1/\sin \alpha \cos \beta &-1/\sin \alpha\sin \beta \\ 1/\cos \alpha &1/\cos \alpha & 1/\cos \alpha \end{bmatrix}$ (16)

实际上，所测水底很少是完全均匀的，即DVL在同一深度上所测相对水底的速度大小与方向往往不同。水底的不均匀性会引入不同程度的速度固有误差。四波束 Janus 阵型结构较三波束 Convex 阵型结构可以多获得第 4 波束的冗余信息，因此可以验证水底的均匀性情况。为了量化水底不均匀性造成的影响，可以引入流速固有误差公式，这是评估数据质量是否有效的重要因素。具体为：

$v_{error}=v_{1}-v_{2}+v_{3}-v_{4}$ (17)

通过分析可知，只要所测水底均匀，则无论系统怎样摇摆 $v_{error}$ 都基本趋近于零，且四路回波认为是有效数据；如果所测流场不均匀，也要看 $v_{error}$ 来确定不均匀的程度，用于判断四路回波数据的有效性。

四波束 Janus 阵型结构较三波束 Convex 阵型结构还有一个优势，就是可以更有效地减小由DVL基阵摇摆引入的流速测量误差，这是通过四个波束所测流速转化为D 下的流速矢量 $\mathbf{V}_{d}$ 来实现的。设水底的四个径向速度构成的列矢量为 $\mathbf{V_{b}}=[v_{1},v_{2},v_{3},v_{4}]^{T}$ ，由于水底在D下的流速列矢量为 $\mathbf{V_{d}}=[v_{dx},v_{dy},v_{dz}]^{T}$ ，于是得到 $\mathbf{V_{b}}$ 与 $\mathbf{V_{d}}$ 之间的关系为：

$\mathbf{V_{b}}=\mathbf{BV_{d}}$ (18)

$\mathbf{V_{d}}=(\mathbf{B^{T}B})^{-1}\mathbf{B^{T}V_{b}}$ (19)

式（18）和式（19）中的转换矩阵分别为：

$\mathbf{B}=\left ( \begin{matrix} \sin \alpha \cos \beta &\sin \alpha \sin \beta & \cos \alpha \\ -\sin \alpha \sin \beta &\sin \alpha \cos\beta & \cos \alpha\\ -\sin \alpha \cos \beta &-\sin \alpha \sin \beta & \cos \alpha\\ \sin \alpha \sin \beta &-\sin \alpha \cos\beta & \cos \alpha \end{matrix} \right )$ (20)

$\mathbf{(B^{T}B)^{-1}B^{T}}=\left ( \begin{matrix} 1/\sin\alpha\cos\beta &-1/\sin\alpha\sin\beta &-1/\sin\alpha\cos\beta &1/\sin\alpha\sin\beta \\ 1/\sin\alpha\sin\beta &1/\sin\alpha\cos\beta &-1/\sin\alpha\sin\beta &-1/\sin\alpha\cos\beta \\ 1/\cos \alpha & 1/\cos \alpha & 1/\cos \alpha & 1/\cos \alpha \end{matrix} \right )$ (21)

由式（14）和式（19）的关系，最后就可以将测得的径向速度 $\mathbf{V_{b}}$ 转换为测速系统坐标系D下的速度 $\mathbf{V_{d}}$ 。

2.3大地坐标系下的速度

相对于声学换能器的径向流速和基阵坐标系下的流速往往并不是用户想要得到的最终流速信息。在实际应用过程中，大地坐标系下的流速信息往往具有更好的实用价值，因此需要将所测换能器径向流速转换成基阵坐标系下的流速后，再进一步转换为大地坐标系下的流速，而这就涉及到了两个坐标系下的流速坐标转换问题。

图4. 坐标转换

在此定义大地的右手坐标系G。大地坐标系G的原点位于测速系统的等效中心O，它的三个轴 $\mathbf{x}_{g}$ 、 $\mathbf{y}_{g}$ 和 $\mathbf{z}_{g}$ 分别指向北、东和垂直向下。接下来，要将基阵坐标系D下的速度转换为大地坐标系G下的速度。如图4 所示设某一右手直角坐标系xyz就是基阵坐标系D，首先绕oz轴顺时针旋转一个角度ψ，则相应的方向余弦矩阵 $\mathbf{\Psi}$ 为：

$\mathbf{\Psi }=\left ( \begin{matrix} \cos \psi & \sin \psi &0 \\ -\sin \psi & \cos \psi &0 \\ 0 & 0 & 1 \end{matrix} \right )$ （22）

接下来绕oy轴顺时针旋转一个角度θ，则相应的方向余弦矩阵 $\mathbf{\Theta }$ 为：

$\mathbf{\Theta} =\left ( \begin{matrix} \cos \theta & 0 &-\sin \theta \\ 0 & 1 & 0\\ \sin \theta & 0 & \cos \theta \end{matrix} \right )$ （23）

最后绕ox 轴顺时针旋转一个角度 $\varphi$ ，则相应的方向余弦矩阵 $\mathbf{\Phi }$ 为：

$\mathbf{\Phi }=\left ( \begin{matrix} 1 & 0 &0 \\ 0 & \cos \varphi & \sin \varphi \\ 0 & -\sin \varphi & \cos \varphi \end{matrix} \right )$ (24)

此时ψ、θ与 $\varphi$ 为三个欧拉角，他们分别称为航向角、纵摇角和横摇角，这是通过DVL内嵌的姿态传感器提供的。

设经过以上三次旋转后的新右手直角坐标系 $\mathbf{{x}'{y}'{z}'}$ 为大地坐标系G，则 $\mathbf{xyz}$ 到 $\mathbf{{x}'{y}'{z}'}$ 的坐标转换矩阵 $\mathbf{R}$ 为：

$\mathbf{R}=\mathbf{\Psi \Theta \Phi }$ (25)

设速度矢量在D内坐标为 $\mathbf{V_{d}}$ ，在G内坐标为 $\mathbf{V_{g}}$ ，则有如下关系成立：

$\mathbf{V_{g}}=\mathbf{RV_{d}}$ (26)

$\mathbf{V_{d}}=\mathbf{R^{-1}V_{g}}=\mathbf{R^{T}V_{g}}$ (27)

由式（26）的关系，就可以将基阵坐标系D下的流速转换为大地坐标系G下的流速。最后由式(18)、(19)与式(26)、(27)，得到 $\mathbf{V_{b}}$ 与 $\mathbf{V_{g}}$ 的通用关系：

$\mathbf{V_{g}}=\mathbf{R(B^{T}B)^{-1}B^{T}V_{b}}$ (28)

$\mathbf{V_{b}}=\mathbf{BR^{-1}V_{g}}=\mathbf{BR^{T}V_{g}}$ (29)

由式（28）与式（29），就可以将波束所测径向速度直接转换为大地坐标系下的速度。

3.DVL的信号处理

3.1频率估计算法简述

如何从多普勒测速回波中提取流速信息是一个重要的问题。该问题涉及到一定空间范围内平均多普勒频移的测量。目前，常用的解决方法是将该问题简化为单频复正弦信号加上高斯白噪声的频率估计问题。

基于此观点，有很多可以利用的算法来估计频率。其中大部分是基于最大似然(ML)的方法。实际的 ML 估计器需要定位周期图频谱谱峰的位置，其中，Palmer 利用了 FFT 来估计谱峰位置，但性能并不理想。Rife 和 Boorstyn给出了最大似然估计器的公式，此时的该估计是无偏的且在信噪比(SNR)达到门限值以上时该估计的均方误差可以达到克拉美-罗下限(CLRB)。需要注意，以上基于频域的估计算法计算时较为复杂，目前并不适合DVL快速灵活的测量要求。

减小频域估计器计算量常用的方法是基于时域相位的估计思想。通过将加性高斯噪声近似为高斯相位噪声，Tretter给出了一种时域估计器。该近似在高 SNR 条件下是有效的，且作为一种最小二乘估计器其性能等价于 ML的性能。由于需要解决相位模糊问题，该估计器在低 SNR 时性能很难满足。Kay给出了一个延迟采样间隔的相位差分估计器，该估计器在高 SNR 条件下可以达到 CRLB。由于改变了提取相位与求和运算的顺序，该估计器可以被认为是由原来的一种相位平均估计器(PA)转变为一种线性预测估计器(LP)。以 Kay 的工作为基础，不同延迟和不同权重的时域频率估计器相继出现。这些估计器在性能上比 Kay 的估计器有所提高，但这是以限制频率估计范围或增加计算量为前提的。Brown 和 Wang，肖扬灿分别给出了各自的循环频率估计器。但这些循环形式的频率估计器性能的提高也是以进一步增加计算复杂性为代价的。

还有其它频率估计的典型算法，包括过零检测法，自适应法等。过零检测法是在一个过零点处开始以非常高的时钟脉冲计数，来确定 N 个信号周期所需的时间，从而进一步估计频率。该算法的运算量小且实现逻辑简单，但当信噪比较低时精度不能令人满意。自适应法是用最小均方算法(LMS)自适应调整基于自回归 AR 模型的 LP 的系数，然后通过对频率轴的扫描根据 LP谱峰值来确定信号的频率。自适应法可以实现高精度和连续调节的窄带频率估计，但该算法需要一定的自适应时间。由过零检测法和自适应法的以上特点可以看出，它们无法应用到宽带回波的多普勒频移估计中。

由波束散射模型可以看出，任意时刻的回波都是全部发射信号在对应某空间范围内的响应。这一情况使得回波与发射信号之间差异很大，特别是在发射信号形式较为复杂的时候。由于信号形式的不同情况，选择流速估计器的主要原则是在保证一定测速性能的基础上，考虑快速灵活的测量要求。可以看到，基于时域相位思想的复自相关算法因其快速灵活可控的特点而成为了流速估计的一个合适选择。

3.2复自相关算法

复自相关算法的主要思想是确定两段回波信号之间的幅值和相位关系，从而确定两段回波信号之间的频率。复自相关算法适合于不同信号下的流速的测量，因而是应用较为广泛的方法。从功率谱的观点出发，多普勒频移测量的问题就转化为确定观测信号功率谱密度的一阶矩。在此令回波的复数观测信号为：

$x(t)=s(t)+n(t), 0<t<T$ (30)

这里s(t)是含有多普勒信息的复数观测信号，n(t)为零均值独立平稳复高斯噪声。x(t) 对应的协方差函数与功率谱密度可以分别表示为： $R(\tau )=R_{s}(\tau )+R_{n}(\tau ),S(f)=S_{s}(f)+S_{n}(f)$ 。于是主要问题就是估计多普勒频率均值 $\mu_{s} (f)$ ，它可以用s(t)功率谱的一阶矩表示为

$\mu_{s} (f)=\frac{\int_{-\infty }^{\infty }fS_{s}(f)df}{\int_{-\infty }^{\infty }S_{s}(f)df}$ (31)

首先，将 x(t),s(t)与n(t)的自相关函数表示成极坐标形式

$R(\tau )=\left | R(\tau ) \right |\exp(j\phi (\tau ))$ (32)

$R_{s}(\tau )=\left | R_{s}(\tau ) \right |\exp(j\phi_{s} (\tau )),R_{n}(\tau )=\left | R_{n}(\tau ) \right |\exp(j\phi_{n} (\tau ))$ (33)

由于假定噪声是非零独立平稳高斯的，所以有

$R_{n}(\tau )=\left\{\begin{matrix} R_{n}(0), & \tau =0\\ 0, & \tau \neq 0 \end{matrix}\right.$ (34)

因此就有 $R(\tau )=R_{s}(\tau ),\tau \neq 0$ 。这说明 $\tau \neq 0$ 时对 $R(\tau )$ 的计算可得到 $R_{s}(\tau )$ ，即可以用 $R(\tau )$ 来代替 $R_{s}(\tau )$ 。

其次，对式（33）的 $R_{s}(\tau )$ 求导，有

${R_{s}}'(\tau )=\frac{dR_{s}(\tau )}{d\tau }=[{\left |R_{s}(\tau ) \right |}'+j\left | R_{s}(\tau ) \right |{\phi _{s}}'(\tau )]\exp (j\phi _{s}(\tau ))$ (35)

由于 $\left | R_{s}(\tau ) \right |$ 为偶函数， ${\left | R_{s}(\tau ) \right |}'$ 为奇函数，所以 ${\left | R_{s}(0 ) \right |}'=0$ 。因此

${R_{s}}'(0)=jR_{s}(0){\phi _{s}}'(0)$ (36)

另外根据维纳-辛钦定理

$R_{s}(\tau )=\int_{-\infty }^{\infty }S_{s}(f)\exp (j2\pi \tau f)df$ (37)

$S_{s}(f)=\int_{-\infty }^{\infty }S_{s}(\tau)\exp (-j2\pi f\tau)d\tau$ (38)

对式(37)的 $R_{s}(\tau )$ 求导，有

${R_{s}}'(\tau )=j2\pi \int_{-\infty }^{\infty }fS_{s}(f)\exp (j2\pi \tau f)df$ (39)

当 $\tau =0$ 时，式（37）与式（39）式变为

${R_{s}}'(0)=j2\pi \int_{-\infty }^{\infty }fS_{s}(f)df$ (40)

$R_{s}(0)=\int_{-\infty }^{\infty }S_{s}(f)df$ (41)

最后，将式（36）、式（40）与式（41）带入到式（31），得到

$\mu_{s} (f)=\frac{\int_{-\infty }^{\infty }fS_{s}(f)df}{\int_{-\infty }^{\infty }S_{s}(f)df}=\frac{-j{R_{s}}'(0)}{2\pi R_{s}(0)}=\frac{1}{2\pi }{\phi _{s}}'(0)$ (42)

由式（42）可以看出，多普勒频率均值 $\mu_{s} (f)$ 可以由回波复数观测信号自相关函数相位的导数来表示。于是进一步可以看出，对于线性相位情况，就有如下关系成立

$\mu_{s} (f)=\frac{\phi (\tau )}{2\pi \tau }$ (43)

式(43)就是得到的多普勒频率均值 $\mu_{s} (f)$ 的复自相关算法的估计公式，其中 $\phi (\tau )$ 是自相关函数的相位。由复自相关函数及其相位的公式

$R(\tau )=\frac{1}{T-\left | \tau \right |}\int_{0}^{T-\left | \tau \right |}x(t+\tau )x^{*}(t)dt,\phi (\tau )=\arctan \frac{Im{R(\tau )}}{Re{R(\tau )}}$ (44)

对于复数观测信号 $x(t)=x_{r}(t)+jx_{i}(t),\phi (\tau )$ 可以表示为

$\phi(\tau )=\arctan \frac{\int_{0}^{T-\left | \tau \right |}x_{r}(t)x_{i}(t+\tau )-x_{i}(t)x_{r}(t+\tau )dt}{\int_{0}^{T-\left | \tau \right |}x_{r}(t)x_{r}(t+\tau )+x_{i}(t)x_{i}(t+\tau )dt}$ (45)

以上的式(45)等价于 Kay 所给出的线性预测估计器。值得注意的是，由于反正切运算使得相位函数被限制在[−π,π]之间，而真实 $\phi (\tau )$ 的取值不应该被限制在这一范围。

由复自相关算法的估计公式可以看出，不论是发射单载频矩形脉冲信号，还是发射其它更为复杂的信号形式，只要具有时间间隔的两段回波信号之间存在着相干性，就都可以通过复自相关算法确定这两段回波信号之间的多普勒频率。因此基于复自相关算法的回波处理，多普勒速度测量将会变得非常灵活。

4.DVL系统仿真分析

4.1DVL系统CW回波信号的生成

根据前面的理论分析，下面给出的是基于CW信号的回波生成函数的M代码：

function Orignal = DVL_CW_echo_gen(f0,fs,tao,c,total_time,SNR,H,vg,attitude_angle,alpha,beta)
% ************************** DVL CW信号生成回波信号函数 **************************
% 假设海底是一个平面 
% 依据Janus阵型结构四子阵测得的径向多普勒速度,根据转换矩阵B转换得到基阵坐标系下
% 的速度,然后再依据转换矩阵R得到大地坐标系下的速度.
% 参考文献：[1] 刘德铸,声学多普勒流速测量关键技术研究,2.4节.
%                    ∧
%           子阵1    ||y    子阵2
%                    ||
% 行船方向 <-----x---||---------
%                    ||
%           子阵4    ||     子阵3
%                    ||
% **** 输入参数 ****
% **   f0                  信号频率                      单位：Hz
% **   fs                  采样率                        单位：Hz
% **   tao                 发射信号脉宽                   单位：s
% **   c                   声速                          单位：m/s
% **   total_time          采样信号总时间                 单位：s
% **   SNR                 信噪比                         单位：dB
% **   H                   DVL距离海底的垂直高度                                 单位：m
% **   vg                  DVL相对于大地坐标系的三维速度，各分量分别位Vx,Vy,Vz    单位：m/s
% **   attitude_angle      DVL的姿态角,各分量分别为Roll,Pitch,Yaw                单位：°
% **   alpha               DVL换能器轴线方向与换能器阵轴线方向夹角，即安装角，     单位：°
% **   beta                X型Janus阵型结构逆时针与坐标轴的夹角                   单位：°
% **** 输出参数 ****
% **   Orignal             各通道的原始数据      
%%
% 相关参数计算
alpha = alpha*pi./180;% 与z轴的夹角
beta = beta*pi./180;% X型Janus阵型结构逆时针与坐标轴的夹角
B = [sin(alpha)*cos(beta),sin(alpha)*sin(beta),cos(alpha);-sin(alpha)*sin(beta),sin(alpha)*cos(beta),cos(alpha);-sin(alpha)*cos(beta),-sin(alpha)*sin(beta),cos(alpha);sin(alpha)*sin(beta),-sin(alpha)*cos(beta),cos(alpha)];fai =   attitude_angle(1)*pi./180;% 横摇 roll 沿x轴顺时针旋转
theta = attitude_angle(2)*pi./180;% 纵摇 pitch 沿y轴顺时针旋转
pfai =  attitude_angle(3)*pi./180;% 艏摇 yaw 偏航 沿z轴顺时针旋转Fai = [1,0,0;0,cos(fai),sin(fai);0,-sin(fai),cos(fai)];
Theta = [cos(theta),0,-sin(theta);0,1,0;sin(theta),0,cos(theta)];
Pfai = [cos(pfai),sin(pfai),0;-sin(pfai),cos(pfai),0;0,0,1];
R = Pfai*Theta*Fai;% 坐标转换矩阵% vd = pinv(B)*vb;% 基阵坐标系下的速度
% vg = R*pinv(B)*vb;% 大地坐标系下的速度
vb = B*pinv(R)*vg;  % 由大地坐标系下的速度得到换能器的多普勒径向速度 DIRO1 = [sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];    %在换能器基阵坐标系中，1号换能器方向上的单位向量
DIRO2 = [-sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];   %在换能器基阵坐标系中，2号换能器方向上的单位向量
DIRO3 = [-sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];  %在换能器基阵坐标系中，3号换能器方向上的单位向量
DIRO4 = [sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];   %在换能器基阵坐标系中，4号换能器方向上的单位向量DIRO1_g = R*DIRO1;            %在大地坐标系中，1号换能器方向上的单位向量
DIRO2_g = R*DIRO2;            %在大地坐标系中，2号换能器方向上的单位向量
DIRO3_g = R*DIRO3;            %在大地坐标系中，3号换能器方向上的单位向量
DIRO4_g = R*DIRO4;            %在大地坐标系中，4号换能器方向上的单位向量
DIRZ_g = [0;0;1];             %大地坐标系中Z轴方向的单位向量dis1 = H/(DIRO1_g'*DIRZ_g);   %1号换能器的声波传播距离
dis2 = H/(DIRO2_g'*DIRZ_g);   %2号换能器的声波传播距离
dis3 = H/(DIRO3_g'*DIRZ_g);   %3号换能器的声波传播距离
dis4 = H/(DIRO4_g'*DIRZ_g);   %4号换能器的声波传播距离
dis = [dis1;dis2;dis3;dis4];  
%%
% 生成各基元回波信号
sig_points = fix(total_time*fs);  %总采样点数
Orignal = zeros(4,sig_points);
temp = 10^(-SNR/20);for i = 1:4tt = 2*dis(i)/c;             %计算回波的开始时刻 d_f = f0*2*vb(i)/c;          %计算多普勒频移f_r = f0 + d_f;              %接收的回波信号频率tao_compressd = tao/(1+2*vb(i)/c);     %计算压缩后的脉宽nTs = 0:1/fs:tao_compressd;            %被压缩的回波信号时刻Echo_points = length(nTs);             %被压缩的回波采样点数tt_num = fix(tt*fs)+1;                   %转换成时刻点phase = (tt_num/fs - tt)*f_r*2*pi;       %采样认为回波信号开始的时刻与真实的开始时刻相位差  Orignal(i,tt_num:(tt_num + Echo_points-1)) = cos(2*pi*f_r*nTs + phase);     %对回波信号采样noise = randn(1,sig_points);noise = noise * temp;Orignal(i,:) = Orignal(i,:) + noise;            %加入一定信噪比的噪声
end

4.2基于仿真信号的DVL系统仿真

由前面的理论分析，采用复自相关算法进行信号处理，编写的M代码如下：

% DVL信号处理
close all
clear all
clcc = 1500;
fs = 600e3;                %采样率
f0 = 400e3;                %CW信号频率
tao = 5.0e-3;
cw_with_ms = tao*1000;
total_time = 0.14;
H = 10;                                 %深度
alpha = 22.5;                    %换能器轴线方向与换能器阵轴线方向夹角，即安装角，单位：°
beta = 45;                       %X型Janus阵型结构逆时针与坐标轴的夹角           单位：°
attitude_angle = [2 5 4.5];      %姿态角，此时基阵坐标与大地坐标重合
vg1 = [1;-1;0.5];                   %DVL相对于大地坐标系速度
SNR = 40;                        %信噪比Orignal1 = DVL_CW_echo_gen(f0,fs,tao,c,total_time,SNR,H,vg1,attitude_angle,alpha,beta);discard_points = 1;                   %舍去一开始的点数
end_points = 30000;
ORI_Channel1 = Orignal1(1,discard_points:end_points);      %读取第一个通道的数据
ORI_Channel2 = Orignal1(2,discard_points:end_points);
ORI_Channel3 = Orignal1(3,discard_points:end_points);
ORI_Channel4 = Orignal1(4,discard_points:end_points);sample_points = length(ORI_Channel1);   %获取一个通道的采样点数
nTs = (0:sample_points-1)/fs + cw_with_ms/1000 + discard_points/fs;  %采样时刻
sig_points = fix(cw_with_ms*fs/1000*1)+1; %信号脉宽一半占的采样点数/2
delay_n = 1;            %延时点数fix(sig_points/2)/2  fix(sig_points*1/2)figure;
plot(ORI_Channel1,'r');
hold on;
grid on;
plot(ORI_Channel2,'b');
plot(ORI_Channel3,'c');
plot(ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('原始数据');
figure;
plot(nTs,ORI_Channel1,'r');
hold on;
grid on;
plot(nTs,ORI_Channel2,'b');
plot(nTs,ORI_Channel3,'c');
plot(nTs,ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
xlabel('时间/s');
title('原始数据');
%%
%正交变换
cos_table = cos(2*pi*f0*nTs);
sin_table = sin(2*pi*f0*nTs);cos_ORI_Channel1 = ORI_Channel1.*cos(2*pi*f0*nTs);
sin_ORI_Channel1 = ORI_Channel1.*sin(2*pi*f0*nTs);
cos_ORI_Channel2 = ORI_Channel2.*cos(2*pi*f0*nTs);
sin_ORI_Channel2 = ORI_Channel2.*sin(2*pi*f0*nTs);
cos_ORI_Channel3 = ORI_Channel3.*cos(2*pi*f0*nTs);
sin_ORI_Channel3 = ORI_Channel3.*sin(2*pi*f0*nTs);
cos_ORI_Channel4 = ORI_Channel4.*cos(2*pi*f0*nTs);
sin_ORI_Channel4 = ORI_Channel4.*sin(2*pi*f0*nTs);figure;
subplot(2,1,1);
plot(cos_ORI_Channel1,'r');
hold on;
grid on;
plot(cos_ORI_Channel2,'b');
plot(cos_ORI_Channel3,'c');
plot(cos_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*cos');
subplot(2,1,2);
plot(sin_ORI_Channel1,'r');
hold on;
grid on;
plot(sin_ORI_Channel2,'b');
plot(sin_ORI_Channel3,'c');
plot(sin_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*sin');
%*************** LPF Fpass = 16KHz  Fstop = 40kHz 64阶 *************************
hn = [0.0004    0.0004    0.0004    0.0002   -0.0001   -0.0007   -0.0014   -0.0020   -0.0023   -0.0021   -0.0012    0.0006    0.0029    0.0053...0.0073    0.0081    0.0072    0.0040   -0.0013   -0.0080   -0.0151   -0.0209   -0.0236   -0.0213   -0.0130    0.0019    0.0228    0.0480...0.0749    0.1006    0.1218    0.1358    0.1407    0.1358    0.1218    0.1006    0.0749    0.0480    0.0228    0.0019   -0.0130   -0.0213...-0.0236   -0.0209   -0.0151   -0.0080   -0.0013    0.0040    0.0072    0.0081    0.0073    0.0053    0.0029    0.0006   -0.0012   -0.0021...-0.0023   -0.0020   -0.0014   -0.0007   -0.0001    0.0002    0.0004    0.0004    0.0004];LPF_cos_ORI_Channel1 = filter(hn,1,cos_ORI_Channel1);
LPF_sin_ORI_Channel1 = filter(hn,1,sin_ORI_Channel1);
LPF_cos_ORI_Channel2 = filter(hn,1,cos_ORI_Channel2);
LPF_sin_ORI_Channel2 = filter(hn,1,sin_ORI_Channel2);
LPF_cos_ORI_Channel3 = filter(hn,1,cos_ORI_Channel3);
LPF_sin_ORI_Channel3 = filter(hn,1,sin_ORI_Channel3);
LPF_cos_ORI_Channel4 = filter(hn,1,cos_ORI_Channel4);
LPF_sin_ORI_Channel4 = filter(hn,1,sin_ORI_Channel4);figure;
subplot(2,1,1);
plot(LPF_cos_ORI_Channel1,'r');
hold on;
grid on;
plot(LPF_cos_ORI_Channel2,'b');
plot(LPF_cos_ORI_Channel3,'c');
plot(LPF_cos_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*cos经过LPF');
subplot(2,1,2);
plot(LPF_sin_ORI_Channel1,'r');
hold on;
grid on;
plot(LPF_sin_ORI_Channel2,'b');
plot(LPF_sin_ORI_Channel3,'c');
plot(LPF_sin_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*sin经过LPF');%% 
%生成复数
complex_ORI_Channel1 = LPF_cos_ORI_Channel1 + 1j*LPF_sin_ORI_Channel1;
complex_ORI_Channel2 = LPF_cos_ORI_Channel2 + 1j*LPF_sin_ORI_Channel2;
complex_ORI_Channel3 = LPF_cos_ORI_Channel3 + 1j*LPF_sin_ORI_Channel3;
complex_ORI_Channel4 = LPF_cos_ORI_Channel4 + 1j*LPF_sin_ORI_Channel4;figure;
subplot(2,1,1);
plot(abs(complex_ORI_Channel1),'r');
hold on;
grid on;
plot(abs(complex_ORI_Channel2),'b');
plot(abs(complex_ORI_Channel3),'c');
plot(abs(complex_ORI_Channel4),'g');
legend('通道1','通道2','通道3','通道4');
title('复信号幅度');
subplot(2,1,2);
plot(angle(complex_ORI_Channel1)/pi*180,'r');
hold on;
grid on;
plot(angle(complex_ORI_Channel2)/pi*180,'b');
plot(angle(complex_ORI_Channel3)/pi*180,'c');
plot(angle(complex_ORI_Channel4)/pi*180,'g');
legend('通道1','通道2','通道3','通道4');
title('相位°');%%
% 复数自相关
autocor_points = sig_points - delay_n;
for i = (autocor_points+delay_n):sample_pointsset = (1:autocor_points) + i - (autocor_points);R_complex_ORI_Channel1(i) = complex_ORI_Channel1(set-delay_n)*complex_ORI_Channel1(set)';R_complex_ORI_Channel2(i) = complex_ORI_Channel2(set-delay_n)*complex_ORI_Channel2(set)';R_complex_ORI_Channel3(i) = complex_ORI_Channel3(set-delay_n)*complex_ORI_Channel3(set)';R_complex_ORI_Channel4(i) = complex_ORI_Channel4(set-delay_n)*complex_ORI_Channel4(set)';
endAmp1 = abs(R_complex_ORI_Channel1);
Fai1 = angle(R_complex_ORI_Channel1);
Amp2 = abs(R_complex_ORI_Channel2);
Fai2 = angle(R_complex_ORI_Channel2);
Amp3 = abs(R_complex_ORI_Channel3);
Fai3 = angle(R_complex_ORI_Channel3);
Amp4 = abs(R_complex_ORI_Channel4);
Fai4 = angle(R_complex_ORI_Channel4);figure;
plot(Fai1/pi*180,'r');
hold on;
grid on;
plot(Fai2/pi*180,'b');
plot(Fai3/pi*180,'c');
plot(Fai4/pi*180,'g');
legend('通道1','通道2','通道3','通道4');
title('复自相关相位');%%norm_amp1 = Amp1/max(Amp1);
norm_amp2 = Amp2/max(Amp2);
norm_amp3 = Amp3/max(Amp3);
norm_amp4 = Amp4/max(Amp4);[echo_time1,v1,Fai_set1] = Echo_time_v_estimate(Amp1,sig_points,sample_points,Fai1,c,fs,f0,delay_n);
[echo_time2,v2,Fai_set2] = Echo_time_v_estimate(Amp2,sig_points,sample_points,Fai2,c,fs,f0,delay_n);
[echo_time3,v3,Fai_set3] = Echo_time_v_estimate(Amp3,sig_points,sample_points,Fai3,c,fs,f0,delay_n);
[echo_time4,v4,Fai_set4] = Echo_time_v_estimate(Amp4,sig_points,sample_points,Fai4,c,fs,f0,delay_n);Echo_time = [echo_time1,echo_time2,echo_time3,echo_time4];
Echo_time = Echo_time/fs - tao;
H1 = DVL_Hight_Estimate(Echo_time,c,attitude_angle,alpha,beta);figure;
plot(norm_amp1);
hold on;
grid on;
plot(Fai1/max(abs(Fai1)),'r');
plot(echo_time1,0,'*k');
plot(Fai_set1,Fai1(Fai_set1)/max(abs(Fai1)),'g*');
legend('复数自相关幅度','相位','估计的到达时刻','截取的相位区间');
string = ['估计的到达时刻 n=',num2str(echo_time1),',估计的速度 v1=',num2str(v1),'m/s'];
title(string);figure;
plot(norm_amp2);
hold on;
grid on;
plot(Fai2/max(abs(Fai2)),'r');
plot(echo_time2,0,'*k');
plot(Fai_set2,Fai2(Fai_set2)/max(abs(Fai2)),'g*');
legend('复数自相关幅度','相位','估计的到达时刻','截取的相位区间');
string = ['估计的到达时刻 n=',num2str(echo_time2),',估计的速度 v2=',num2str(v2),'m/s'];
title(string);figure;
plot(norm_amp3);
hold on;
grid on;
plot(Fai3/max(abs(Fai3)),'r');
plot(echo_time3,0,'*k');
plot(Fai_set3,Fai3(Fai_set3)/max(abs(Fai3)),'g*');
legend('复数自相关幅度','相位','估计的到达时刻','截取的相位区间');
string = ['估计的到达时刻 n=',num2str(echo_time3),',估计的速度 v3=',num2str(v3),'m/s'];
title(string);figure;
plot(norm_amp4);
hold on;
grid on;
plot(Fai4/max(abs(Fai4)),'r');
plot(echo_time4,0,'*k');
plot(Fai_set4,Fai4(Fai_set4)/max(abs(Fai4)),'g*');
legend('复数自相关幅度','相位','估计的到达时刻','截取的相位区间');
string = ['估计的到达时刻 n=',num2str(echo_time4),',估计的速度 v4=',num2str(v4),'m/s'];
title(string);vb = [v1 v2 v3 v4]';
[vg,vd] = Get_absolute_velocity(alpha,beta,vb,attitude_angle);   %相对大地速度，相对基阵速度

其中调用的三个子函数（Echo_time_v_estimate、DVL_Hight_Estimate、Get_absolute_velocity）代码分别如下：

function [echo_time,v,Fai_set] = Echo_time_v_estimate(Amp,sig_points,sample_points,Fai,c,fs,f0,delay_n)
%****************************** 回波时间与速度估计 *************************
% **** 输入参数
% **   Amp             复信号自相关后的幅度
% **   sig_points      发射信号脉宽占的采样信号点数
% **   sample_points   总采样点数，即信号总长度
% **   Fai             复信号自相关后的相位
% **   c               声速
% **   fs              采样率
% **   f0              载波频率
% **   delay_n         复自相关延迟的采样点数
% **** 输出参数
% **   echo_time       估计的回波时间
% **   v               估计的回波速度
% **   Fai_set         回波时刻对应的相位区间
%%
max_sum = sum(Amp(1:sig_points));
max_set = 1;
for i = 2:sample_points - sig_pointstemp = sum(Amp(i:(i+sig_points-1)));if temp > max_summax_sum = temp;    %寻找最大值max_set = i;       %记录最大值位置end
endX = max_set:(max_set+sig_points-1);
Y = Amp(X);
echo_time = sum(X.*Y)/sum(Y);      %质量中心法求最大值点
echo_time_int = fix(echo_time);Q_sig_points = fix(sig_points/4);
Fai_set = (echo_time_int-Q_sig_points):(echo_time_int+Q_sig_points);
Fai_avr = sum(Fai(Fai_set))/((2*Q_sig_points+1));v = Fai_avr*c*fs/(4*pi*delay_n*f0);    %估计速度

function H = DVL_Hight_Estimate(Echo_time,c,attitude_angle,alpha,beta)
% *************** DVL高度估计 **********************
% 依据Janus阵型结构四子阵测得的径向多普勒速度,根据转换矩阵B转换得到基阵坐标系下
% 的速度,然后再依据转换矩阵R得到大地坐标系下的速度.
% 参考文献：[1] 刘德铸,声学多普勒流速测量关键技术研究,2.4节.
%                    ∧
%           子阵1    ||y    子阵2
%                    ||
% 行船方向 <-----x---||---------
%                    ||
%           子阵4    ||     子阵3
%                    ||
% **** 输入参数 ****
% **   Echo_time           由各换能器估计的到达时刻       单位：s
% **   c                   声速                          单位：m/s
% **   attitude_angle      DVL的姿态角,各分量分别为Roll,Pitch,Yaw                单位：°
% **   alpha               DVL换能器轴线方向与换能器阵轴线方向夹角，即安装角，     单位：°
% **   beta                X型Janus阵型结构逆时针与坐标轴的夹角                   单位：°
% **** 输出参数 ****
% **   H                   估计的高度                     单位：m
%%
alpha = alpha*pi./180;% 与z轴的夹角
beta = beta*pi./180;% X型Janus阵型结构逆时针与坐标轴的夹角
fai =   attitude_angle(1)*pi./180;% 横摇 roll 沿x轴顺时针旋转
theta = attitude_angle(2)*pi./180;% 纵摇 pitch 沿y轴顺时针旋转
pfai =  attitude_angle(3)*pi./180;% 艏摇 yaw 偏航 沿z轴顺时针旋转Fai = [1,0,0;0,cos(fai),sin(fai);0,-sin(fai),cos(fai)];
Theta = [cos(theta),0,-sin(theta);0,1,0;sin(theta),0,cos(theta)];
Pfai = [cos(pfai),sin(pfai),0;-sin(pfai),cos(pfai),0;0,0,1];
R = Pfai*Theta*Fai;% 坐标转换矩阵DIRO1 = [sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];    %在换能器基阵坐标系中，1号换能器方向上的单位向量
DIRO2 = [-sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];   %在换能器基阵坐标系中，2号换能器方向上的单位向量
DIRO3 = [-sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];  %在换能器基阵坐标系中，3号换能器方向上的单位向量
DIRO4 = [sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];   %在换能器基阵坐标系中，4号换能器方向上的单位向量DIRO1_g = R*DIRO1;            %在大地坐标系中，1号换能器方向上的单位向量
DIRO2_g = R*DIRO2;            %在大地坐标系中，2号换能器方向上的单位向量
DIRO3_g = R*DIRO3;            %在大地坐标系中，3号换能器方向上的单位向量
DIRO4_g = R*DIRO4;            %在大地坐标系中，4号换能器方向上的单位向量
DIRZ_g = [0;0;1];             %大地坐标系中Z轴方向的单位向量dis = Echo_time*c/2;         %得到各换能器到地面的距离H1 = dis(1)*DIRO1_g'*DIRZ_g;  %由1号换能器估计的距离
H2 = dis(2)*DIRO2_g'*DIRZ_g;  %由2号换能器估计的距离
H3 = dis(3)*DIRO3_g'*DIRZ_g;  %由3号换能器估计的距离
H4 = dis(4)*DIRO4_g'*DIRZ_g;  %由4号换能器估计的距离H = (H1+H2+H3+H4)/4;       %得到最终的高度

function [vg,vd] = Get_absolute_velocity(alpha,beta,vb,attitude_angle)
% ************************ ADCP流速坐标系转换 ******************************
% 依据Janus阵型结构四子阵测得的径向多普勒速度,根据转换矩阵B转换得到基阵坐标系下
% 的速度,然后再依据转换矩阵R得到大地坐标系下的速度.
% 参考文献：[1] 刘德铸,声学多普勒流速测量关键技术研究,2.4节.
%                    ∧
%           子阵1    ||y    子阵2
%                    ||
% 行船方向 <-----x---||---------
%                    ||
%           子阵4    ||     子阵3
%                    ||
% **** 输入参数
% **   alpha           换能器轴线方向与换能器阵轴线方向夹角，即安装角，单位：°
% **   beta            X型Janus阵型结构逆时针与坐标轴的夹角           单位：°
% **   vb              各换能器的径向速度                            单位：m/s
% **   attitude_angle  姿态角                                        单位：°
% **** 输出参数
% **   vg              输出对地速度                                  单位：m/salpha = alpha*pi./180;% 与z轴的夹角
beta = beta*pi./180;% X型Janus阵型结构逆时针与坐标轴的夹角
B = [sin(alpha)*cos(beta),sin(alpha)*sin(beta),cos(alpha);-sin(alpha)*sin(beta),sin(alpha)*cos(beta),cos(alpha);-sin(alpha)*cos(beta),-sin(alpha)*sin(beta),cos(alpha);sin(alpha)*sin(beta),-sin(alpha)*cos(beta),cos(alpha)];
% vb = [1 1 1 1]';% 换能器测出多普勒径向速度 
vd = pinv(B)*vb;% 基阵坐标系下的速度fai =   attitude_angle(1)*pi./180;% 横摇 roll 沿x轴顺时针旋转
theta = attitude_angle(2)*pi./180;% 纵摇 pitch 沿y轴顺时针旋转
pfai =  attitude_angle(3)*pi./180;% 艏摇 yaw 偏航 沿z轴顺时针旋转Fai = [1,0,0;0,cos(fai),sin(fai);0,-sin(fai),cos(fai)];
Theta = [cos(theta),0,-sin(theta);0,1,0;sin(theta),0,cos(theta)];
Pfai = [cos(pfai),sin(pfai),0;-sin(pfai),cos(pfai),0;0,0,1];
R = Pfai*Theta*Fai;% 坐标转换矩阵vg = R*vd;% 大地坐标系下的速度

运行得到结果如下：

仿真输入的三维速度为[1;-1;0.5]，高度为10；估计的三维速度[0.9919;-0.9887;0.4948]，估计的高度为10.0365。

查看全文
如若内容造成侵权/违法违规/事实不符，请联系编程学习网邮箱：809451989@qq.com进行投诉反馈，一经查实，立即删除！

【转】C#编写NotifyIcon 将窗口最小化到托盘
1.设置窗体属性showinTask=false 2.加notifyicon控件notifyIcon1，为控件notifyIcon1的属性Icon添加一个icon图标。 3.添加窗体最小化事件(首先需要添加事件引用)： // this.SizeChanged += new System.EventHandler(this.Form1_SizeChanged); //上面一行是主窗体InitializeCom…...
2024/4/28 14:44:18
提高程序员生产力的代码生成器
码云 Gitee 项目推荐1. 项目名称：基于代码生成器的 J2EE 快速开发平台 jeecg项目简介：JEECG（J2EE Code Generation）是一款基于代码生成器的智能开发平台。引领新的开发模式( Online Coding -> 代码生成器 -> 手工 MERGE 智能开发)，可以帮助解决 Java 项目 60% 的重复…...
2024/4/27 23:57:42
Larbin 配置
ubuntu下1. 需要安装makedepend sudo apt-get install xutils-dev 2. 把adns文件夹下internal.h文件569-571 注释掉 3. 把所有#include<iostream.h> 替换为#include<iostream> 并且加上using namespace std;还有一个简单的方法，就是从/usr/include/c++里…...
2024/4/28 15:08:00
关于NotifyIcon的抱怨。。。
.NET中微软提供了NotifyIcon类来处理应用程序在托盘区的图标显示，并且该对象可以设置一个ContextMenu对象，方便应用程序进行快捷操作。但是，ContextMenu是以右键点击的方式弹出来的，而实际开发中用户经常要求左键单击的时候也能够弹出菜单进行操作，可是微软在NotifyIcon中…...
2024/4/28 6:23:40
“疯狂代码生成器” 正式发布
简单而言，DevHelper是一款为程序员开发的代码自动生成工具。中文名叫“疯狂代码生成器”。复杂来说，DevHelper是基于多种数据库多种开发语言的模板化代码代码生成器。它提供丰富的标准库以及简单的接口和逻辑结构，其目的是使开发人员更快速地进行项目框架搭建。目前支持数据…...
2024/4/28 10:29:44
Kinect 开发 —— 硬件设备解剖
Kinect for Xbox: 360 不支持“近景模式” 三只眼睛 —— 红外投影机，RGB摄像头，红外深度投影头 —— 色彩影像中的每个像素分别与深度影像中的一个像素对应四只耳朵 —— L形布局的麦克风阵列 —— 过滤背景噪声，定位声源 —— 根据麦克风阵列接听声音的时间差来判断声源…...
2024/4/28 5:56:17
Flash actionscrip视频教程
第一章基础 1 帧2 层3 时间轴4 元件5 动作面板6 强化练习——MTV的片头17 强化练习——MTV的片头28 强化练习——MTV的片头39 强化练习——MTV的片头410 强化练习——MTV的片头5第二章动作面板(已发布) 1 添加动作2 帧上添加动作3 强化练习——变动的文字4 影片剪辑上添加动…...
2024/4/28 4:44:14
SpringBoot集成MyBatis Generator代码生成器
在原有SpringBoot+gradle构建的项目上（项目地址chen1218chen/Springboot-Mybatis-Gradle）继续集成MyBatis Generator来自动生成model层、dao层代码，以实现快速开发。修改build.gradle文件首先，在gradle的配置文件中添加依赖：compile group: org.mybatis.generator, name: …...
2024/4/28 13:29:08
larbin的使用
一编译 1 sudo apt-get install xutils-dev 2 注释掉adns下internal.h中第568—571行代码 3 到/usr/include/c++/下CP一份iostream文件到larbin的src目录下。并将其的名改为iostream.h.打开此文件添加一句using namespace std; 4 ./configure && make二配置修改lar…...
2024/4/28 4:02:28
声音导引系统及信号采集处理电路设计
2009年参加了全国大学生电子竞赛，经过全队三人的努力，最终荣获全国一等奖（当年浙江省本科组全国一等奖好像一共5个队伍），在这里特别感谢武林老师，是他为我们创造了这么好的条件，才能够在电子竞赛的道路上走的更加顺畅。我也是在今天整理资料的时候忽然发现了这篇文章，这…...
2024/4/21 8:39:46
Jeesite使用自动代码生成器生成代码流程步骤
步骤一: 搭建Jeesite环境具体搭建不做赘述了,相信有很多帖子已经是写的很清楚了,我就不再做多余的解释了步骤二: 设计表格自行根据业务需要设计一张表格,然后通过你最喜欢的方式新建到jeesite数据库中,这里需要注意的是你的表格需有id字段且为不自增主键(因为Jeesite为做主键…...
2024/4/21 8:39:43
Flash资源加载
Flash资源加载Flash加载swf、图形资源使用的是loader类，下面贴代码更多内容请看:[url="http://www.yoewan.com"]yoewan分享[/url],[url="http://www.yoewan.com/flex"]Flash教程[/url]转载自:[url="http://www.yoewan.com/flash-resource-loading.ht…...
2024/4/20 10:59:04
聊聊麦克风阵列
首先我们简单聊聊什么是麦克风阵列。麦克风阵列 ≠ 一堆麦克风风吹麦浪，高山流水，蛙声虫鸣......人类能听到的声音大约有40多万种，频率在20至20000赫兹之间。而在嘈杂的环境下，机器想要听懂声音，它必须能够听的更远更清晰，人类可以带上助听器，机器需要什么呢?麦克风阵列…...
2024/4/21 8:39:41
larbin跑起来了，下一步就是阅读代码了
03年的代码就是不好编译通过，即使现在larbin跑起来了，还是出现很多的警告首先在下载larbin源代码。我下载的版本是larbin2.6.3第一步：注释adns目录下的internal.h的569到571行第二步：到iostream的目录下复制一份到同一目录，文件名iostream.h，也就是说，iostream在什么目…...
2024/4/21 8:39:41
jeesite代码生成器的使用（实例：报销表）
阅读本文前需要已经搭建好jeesite环境，具体请查看官方文档：http://www.jeesite.com/ PS.官方文档写得含糊且有不少错误，我焦头烂额地配置了两天才搭建好环境，不幸的是忘记记录了，如果有时间以后会写一份搭建环境的文档。本文将使用jeesite生成代码功能，设计并实现报销流程…...
2024/4/21 8:39:39
声道数
声道数是指支持能不同发声的音响的个数，它是衡量音响设备的重要指标之一。当人听到声音时，能对声源进行定位，那么通过在不同的位置设置声源，就可以造就出更好的听觉感受，如果配合影像进行音频位置的调整，则会得到更好的视听效果。常见的声道有：单声道的声道数为1个声道…...
2024/4/20 18:26:24
Go：微服务框架开发三（代码生成工具开发）
目录 共性抽象  代码生成工具设计  代码生成工具开发共性抽象package main import("log""net"pb "github.com/ibinarytree/koala/example/grpc_example/hello""golang.org/x/net/context""google.golang.org/grpc" ) …...
2024/4/20 18:26:24
较为详细的MUSIC算法原理及MATLAB实现
DOA估计算法 DOA（Direction Of Arrival）波达方向定位技术主要有ARMA谱分析、最大似然法、熵谱分析法和特征分解法，特征分解法主要有MUSIC算法、ESPRIT算法WSF算法等。 MUSIC (Multiple Signal Classification)算法，即多信号分类算法，由Schmidt等人于1979年提出。MUSIC算法…...
2024/4/27 9:05:19
NotifyIcon托盘组件的应用
 1、新建一个Form程序 2、拖拽一个NotifyIcon组件 订阅两个事件，代码如下： private void ServiceForm_SizeChanged(object sender, EventArgs e) { if (this.WindowS…...
2024/4/26 14:59:36
基于MybatisGenerator扩展的 Model & Mapper & Service & Controller代码生成器
新鲜出炉的基于数据库表自动生成Mybatis的 Model & Mapper & Service & Controller 等的代码的代码生成器，基于Mybatis Generator扩展其JavaMapper和XmlMapper的实现，一个普通业务网站的后端逻辑，点一下鼠标就实现了，项目链接:https://github.com/fenglibin/C…...
2024/4/21 8:39:38

DVL声学多普勒速度仪技术剖析（理论仿真篇）