声学多普勒速度仪（DVL）是一种测量相对于水底速度的声纳设备。早年DVL一开始被设计主要是用于船舶的定位导航中，但是近年来随着水下机器人领域，尤其是民用领域的兴起（据有关机构测算这一市场规模已达100亿美元），DVL的作用显得愈发重要，因为水下机器人想要在水下实现自主导航有三样东西必不可少，一个是姿态测量单元，一个是速度测量单元，一个是位置测量单元。姿态测量单元通常是通过MEMS惯导元件或者光纤惯导元件等测量得到的，而MEMS通常价格非常便宜，非常容易获得，因此它可以被大量广泛的用在水下机器人中；位置测量单元可以由GPS、USBL等设备构成，获取的难度相对容易，成本也不算太高；最后一个速度测量单元目前最有效的手段就是通过DVL来获取，但其成本高昂，售价一般在十几万以上甚至更高，因此在目前民用的水下机器人上很少有它的身影。目前水下机器人市场还没有完全爆发，主要原因就是相关的传感器不够便宜，许多关键部件的成本没有降下来，国内外在这一领域还没有出现独角兽。笔者正是意识到了这一点，所以才下定决心来研究DVL，希望有志同道合的朋友能和笔者一起将DVL的成本降下来，让水下机器人市场彻底爆发的这一天来的更早一些。
1.多普勒测速原理
1.1多普勒效应
多普勒效应是由于声源与观测者之间存在着相对运动，使观测者听到的声音频率不同于声源所发出声音频率的现象。这是由奥地利物理学家Christian Doppler 于十九世纪在声学领域首先发现的物理学原理。
典型的多普勒效应发生于声源和观测者之间。在声源静止条件下，当观测者向着声源运动时，他收到的声波频率高于他相对声源静止时收到的声波频率；当观测者远离声源而去时，他收到的声波频率低于他相对声源静止时收到的声波频率。声源于观测者之间的相对运动速度越大，则所产生的声波频率变化也就越大。有关多普勒效应的研究不仅在声学领域可以看到，利用多普勒效应进行的研究还可扩展到电磁学，光学等其它领域。
下面推导任意方向上的多普勒效应公式。如图 1 所示，S 为声源，D为观测者，为声源的运动矢量，为观测者的运动矢量，这时从声源 S 到观测者 D 传播方向的矢量 SD 随时在变。设声源在时刻t=t0 和t0+dt 的位置分别为S 和 S′ ，相位分别为和 ，其中相位的增量为。在t0时刻，由声源 S 发出的相位为 的声波传播到 D,观测者的位置在 D ；t0+dt 时刻，声源到达 S′ ，由于观测者也在运动，所以，声源 S′ 发出的相位为的声波只能到达观测者的新位置D′，观测者从D走到D′所用的时间dt′和他感受到的频率，与声源的和是不一样的。在D接收的频率，在D′接收的频率，所接收声波的相位变化了，由于相位增量是一样的，即故
(1)
相位从声源 S 传播到观测者的位置 D 的时刻为 ；而相位由声源 S′ 传播到观测者的位置 D′ 的时刻为。二者之差即为
(2)
如图 1 所示，从 、作SD的垂线，令相应的垂足分别为、。由于 与的长度相差高阶无穷小量，可认为二者大小近似相等，于是
(3)
式中，θ 是 与之间的夹角，ψ 则是与 之间的夹角。
由于声波在水体介质中的传播速度远大于声源和观测者的运动速度，可认为声源和观测者近似作匀速直线运动，因此，。由式(2)和式(2-3)解得
(4)
由此得到
(5)
这便是多普勒效应的普遍公式。
1.2多普勒测速公式
不难看出，当θ 、ψ 都等于0 时，式(5)可以过渡到共线情形
(6)
由式（6）可知，当声源与观测者相互靠近时，；当声源与观测者相互远离时，。
对于DVL而言，收发合制换能器。换能器首先作为声源，水底作为观测者，于是根据是（6）水底接收的声波频率为：
(7)
之后水底作为声源，而换能器则作为观测者，于是换能器接收的声波频率为：
(8)
整合式（7）与式（8），得到：
(9)
换能器相对静止、即当时，就是水底相对换能器的径向流速。于是，式(9)经过整理得到：
(10)
由此经过整理可以得到通用的径向测流基本公式
(11)
其中为多普勒频移。由于水中声速 c 约为1500m/s，远大于水体流速，于是一般可以忽略式(10)中的二次项，再经过整理就可以得到近似的多普勒径向测流公式：
(12)
式(2-12)中，为换能器发射的信号波长，为收发信号之间的多普勒频移。由此通过测量就可以得到换能器相对径向水底速度，并可以进一步得到水平与垂直方向的速度。
2.DVL换能器阵型与坐标变换
2.1Janus阵型结构
本节介绍多普勒测流常用的四波束Janus阵型结构。具有该换能器阵型结构的DVL可以同时发射和同时接收四个窄波束声波，再经处理就可以得到相对于声学换能器的径向流速信息。在此定义DVL的右手坐标系D 。D 的原点位于 Janus 阵型结构的等效中心O，它的三个轴、和分别指向测流系统所规定的前、右和下的方向。DVL的四波束Janus阵型结构是由四个换能器构成，各换能器收发声波的轴线与测流系统坐标系z轴之间的夹角为α，如图 2 所示。
图2.四波束Janus阵型结构
另外，各换能器收发声波的轴线在基阵坐标系 xoy 平面的投影则形成两条相互垂直的直线，且波束 1 的投影与x 轴之间的夹角为 β，如图3 所示。通常测流系统成“X”字形式安装时的 β 等于 45°，成“十”字形式安装时的 β 等于 0°。配有四波束 Janus 阵型结构的DVL在工作时，换能器同时发射的窄波束声波，沿着各自换能器的轴线方向传播。在传播的过程中，每路声波被该路水体散射，会有散射回波被各换能器接收。DVL换能器和水体之间的径向运动使得收发声波频率之间存在多普勒频移。通过测量多普勒频移就可利用式(12)解算出径向流速，，和。
图3.四波束Janus阵型结构
2.2基阵坐标系下的速度
设水底在D下的速度矢量为。同时假设DVL所测水底是均匀水平的。配有四波束 Janus 阵型结构的DVL利用任意三个声波所测流速就可以转换为D下的流速矢量。即使因为某一波束受到干扰不能正常测量时系统也可以工作，因此提高了测量的可靠性。以第 1、2 和 3 波束测得的流速列矢量 为例，得到与的关系为：
(13)
(14)
其中
(15)
(16)
实际上，所测水底很少是完全均匀的，即DVL在同一深度上所测相对水底的速度大小与方向往往不同。水底的不均匀性会引入不同程度的速度固有误差。四波束 Janus 阵型结构较三波束 Convex 阵型结构可以多获得第 4 波束的冗余信息，因此可以验证水底的均匀性情况。为了量化水底不均匀性造成的影响，可以引入流速固有误差公式，这是评估数据质量是否有效的重要因素。具体为：
(17)
通过分析可知，只要所测水底均匀，则无论系统怎样摇摆都基本趋近于零，且四路回波认为是有效数据；如果所测流场不均匀，也要看来确定不均匀的程度，用于判断四路回波数据的有效性。
四波束 Janus 阵型结构较三波束 Convex 阵型结构还有一个优势，就是可以更有效地减小由DVL基阵摇摆引入的流速测量误差，这是通过四个波束所测流速转化为D 下的流速矢量来实现的。设水底的四个径向速度构成的列矢量为，由于水底在D下的流速列矢量为，于是得到与之间的关系为：
(18)
(19)
式（18）和式（19）中的转换矩阵分别为：
(20)
(21)
由式（14）和式（19）的关系，最后就可以将测得的径向速度转换为测速系统坐标系D下的速度。
2.3大地坐标系下的速度
相对于声学换能器的径向流速和基阵坐标系下的流速往往并不是用户想要得到的最终流速信息。在实际应用过程中，大地坐标系下的流速信息往往具有更好的实用价值，因此需要将所测换能器径向流速转换成基阵坐标系下的流速后，再进一步转换为大地坐标系下的流速，而这就涉及到了两个坐标系下的流速坐标转换问题。
图4. 坐标转换
在此定义大地的右手坐标系G。大地坐标系G的原点位于测速系统的等效中心O，它的三个轴、和分别指向北、东和垂直向下。接下来，要将基阵坐标系D下的速度转换为大地坐标系G下的速度。如图4 所示设某一右手直角坐标系xyz就是基阵坐标系D，首先绕oz轴顺时针旋转一个角度ψ，则相应的方向余弦矩阵为：
（22）
接下来绕oy轴顺时针旋转一个角度θ，则相应的方向余弦矩阵为：
（23）
最后绕ox 轴顺时针旋转一个角度，则相应的方向余弦矩阵为：
(24)
此时ψ、θ与为三个欧拉角，他们分别称为航向角、纵摇角和横摇角，这是通过DVL内嵌的姿态传感器提供的。
设经过以上三次旋转后的新右手直角坐标系为大地坐标系G，则到的坐标转换矩阵为：
(25)
设速度矢量在D内坐标为，在G内坐标为，则有如下关系成立：
(26)
(27)
由式（26）的关系，就可以将基阵坐标系D下的流速转换为大地坐标系G下的流速。最后由式(18)、(19)与式(26)、(27)，得到与的通用关系：
(28)
(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复自相关算法
复自相关算法的主要思想是确定两段回波信号之间的幅值和相位关系，从而确定两段回波信号之间的频率。复自相关算法适合于不同信号下的流速的测量，因而是应用较为广泛的方法。从功率谱的观点出发，多普勒频移测量的问题就转化为确定观测信号功率谱密度的一阶矩。在此令回波的复数观测信号为：
(30)
这里s(t)是含有多普勒信息的复数观测信号，n(t)为零均值独立平稳复高斯噪声。x(t) 对 应 的 协 方 差 函 数 与 功 率 谱 密 度 可 以 分 别 表 示 为 ：。于是主要问题就是估计多普勒频率均值 ，它可以用s(t)功率谱的一阶矩表示为
(31)
首先，将 x(t),s(t)与n(t)的自相关函数表示成极坐标形式
(32)
(33)
由于假定噪声是非零独立平稳高斯的，所以有
(34)
因此就有。这说明时对的计算可得到，即可以用来代替。
其次，对式（33）的求导，有
(35)
由于为偶函数，为奇函数，所以。因此
(36)
另外根据维纳-辛钦定理
(37)
(38)
对式(37)的求导，有
(39)
当时，式（37）与式（39）式变为
(40)
(41)
最后，将式（36）、式（40）与式（41）带入到式（31），得到
(42)
由式（42）可以看出，多普勒频率均值可以由回波复数观测信号自相关函数相位的导数来表示。于是进一步可以看出，对于线性相位情况，就有如下关系成立
(43)
式(43)就是得到的多普勒频率均值的复自相关算法的估计公式，其中是自相关函数的相位。由复自相关函数及其相位的公式
(44)
对于复数观测信号可以表示为
(45)
以上的式(45)等价于 Kay 所给出的线性预测估计器。值得注意的是，由于反正切运算使得相位函数被限制在[−π,π]之间，而真实的取值不应该被限制在这一范围。
由复自相关算法的估计公式可以看出，不论是发射单载频矩形脉冲信号，还是发射其它更为复杂的信号形式，只要具有时间间隔的两段回波信号之间存在着相干性，就都可以通过复自相关算法确定这两段回波信号之间的多普勒频率。因此基于复自相关算法的回波处理，多普勒速度测量将会变得非常灵活。
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:4
tt = 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
clc
c = 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_points
set = (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)';
end
Amp1 = 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_points
temp = sum(Amp(i:(i+sig_points-1)));
if temp > max_sum
max_sum = temp; %寻找最大值
max_set = i; %记录最大值位置
end
end
X = 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/s
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)];
% 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。