近年来，水下声源定位已经成为一个经典的估计问题，广泛应用于水下机器人探测、水下无线传感器网络和侦察目标等领域[1-4]．自主式水下潜器等设备在长时间水下工作中须要精确的位置估计，精确定位和保持时间同步是必须解决的难题．由于到达时间差(time difference of arrival，TDOA)的定位无须保持目标和传感器的时间同步，仅须保持传感器之间的时间同步，适用于复杂的水下信道环境[5]，因此到达时间差的定位作为水下定位的重要方法，广泛应用于声源定位领域[6-13]．经典的TDOA定位算法包括两阶段加权最小二乘(two-stage weighted least squares，TSWLS)算法[13]和最大似然估计(maximum likelihood estimation，MLE)算法[14]．最大似然估计算法是一种迭代算法，它需要一个接近真实目标位置的初始位置坐标，以确保算法的收敛性．这种算法具有较高的定位精度，但迭代次数可能达到几十或上百次，算法的复杂度过高．两阶段加权最小二乘算法很好地弥补了以上缺陷，它不需要初始位置，封闭形式的解决方案可以通过两次或三次迭代从而达到最优解．两阶段加权最小二乘算法可以扩展至到达时间差和到达频率差的联合定位[15]、到达时间差和到达角度的联合定位[16]．文献[17]针对到达时间差和到达频率差的联合定位问题，提出了一种约束加权最小二乘的闭式解决方案．文献[7]在此基础上，针对封闭算法受高噪声影响较大的问题，提出了一种基于半松弛技术的到达时间差和到达频率差的联合定位算法．由于海底洋流的影响，传感器可能随着洋流四处浮动，因此造成传感器的位置误差．文献[18]使用了位置精确已知的单个校准源，在存在随机传感器位置误差的情况下，进一步提高了TDOA定位精度．文献[19]则在传感器存在位置误差的情况下，采用时间差和频率差的联合方法对多个目标进行了定位．文献[20-21]分别在此基础上，改变了传感器位置误差变量，以进一步提升定位精度．位置不确定的传感器对时间差的测量带来了难度，基于到达时间差的定位精度有待进一步提升．在以上文献的基础上，本研究提出了一种传感器位置浮动情况下的水下TDOA定位算法．所提算法是一种两阶段加权最小二乘算法．第一阶段在改编的TDOA测量方程基础上，引入传感器位置误差，构造新的定位方程；第二阶段引入目标位置误差，并用加权最小二乘算法求解目标位置估计值，最后用第二阶段的目标位置误差估计值补偿第一阶段的定位结果．1 水下TDOA定位模型构建假设在水下三维空间中，目标的真实坐标为u=[x,y,z]T．有M个传感器参与定位，第i个传感器的真实坐标为si0=[xi0,yi0,zi0]T (i=1,2，⋯,M)．然而，传感器可能随着洋流四处浮动，造成的传感器位置误差设为Δsi=[Δxi,Δyi,Δzi]T．实际的水下环境中只有传感器存在误差的位置参数si=[xi,yi,zi]T，于是有si=si0+Δsi．考虑所有的传感器位置误差向量组成的矩阵Δs=[Δs1T,Δs2T,⋯,ΔsMT]T．当Δs为均值为零且方差为常数的高斯噪声向量时，Δs的协方差矩阵可以表示为Qs=E[ΔsΔsT]=σΔs12I3×303×303×3⋯03×303×3σΔs22I3×303×3⋯03×303×303×3σΔs32I3×3⋯03×3⋮03×303×303×3⋯σΔsM2I3×3，式中：I为一个对角线元素全为1，其余元素全为0的矩阵，下标为它的维度；σΔsi2为传感器位置噪声Δsi的方差；0为一个所有元素都为零的矩阵；E为期望函数．选取第一个传感器为参考传感器，则M个传感器可以通过广义互相关等时延估计方法，从M个接收声波信号中得到M-1个时延差τi,1．将时延差乘以水下声波传播的速度v后，可以得到TDOA的距离差为ri,10=vτi,1=ri0-r10    (i≠1)，(1)式中ri0表示目标和第i个传感器的真实距离，可以写为ri0=||u-si0||，(2)其中||*||为欧几里得范数．由于噪声和声速等因素的影响，实际得到的距离差通常伴随着测量噪声，因此表示为ri,1=ri-r1=ri,10+Δri,1，式中：ri,1为实际得到的距离差测量值，其中目标到第i个传感器的估计距离为ri=ri0+Δri，Δri为测量噪声；Δri,1为TDOA测量噪声,并服从均值为零且方差为常数的高斯分布．测量噪声向量Δγ=[Δr2,1,  Δr3,1,  ⋯,  ΔrM,1]T的协方差矩阵可以写为Qγ=E[ΔγΔγT]=σt20.5σt20.5σt2⋯0.5σt20.5σt2σt20.5σt2⋯0.5σt20.5σt20.5σt2σt2⋯0.5σt2⋮0.5σt20.5σt20.5σt2⋯σt2，式中σt2为TDOA测量噪声的方差．2 基于两阶段加权最小二乘算法的水下目标定位算法2.1　粗估计的目标位置考虑距离的一阶泰勒展开式，传感器位置误差的距离方程可以表示为ri0=||u-si0||=||u-si+Δsi||≈||u-si||+ρu,siTΔsi，式中：ρa,bT=(a-b)T/||a-b||，其中a和b为维度相同的向量．对式(1)进行移项和平方操作，再代入式(2)，最后整理可以得到TDOA测量方程为ri,102-||si0||2+||s10||2+2(si0-s10)Tu+2ri,10r10=0．研究表明，引入方程ri,10+2r10=ri0+r10后，新的测量方程可以避免在推导中因为忽视噪声平方项而造成的定位不精确问题[5]，有ri,10(ri0+r10)-||si0||2+||s10||2+2(si0-s10)Tu=0．(3)在此基础上，本研究引入Δγ和Δs，TDOA定位的伪线性方程可以表示为ri,1(||u1-si||+||u1-s1||)-||si||2+||s1||2+2(si-s1)Tu1≈(||u1-si||+||u1-s1||)Δri,1-[2(u1-s1)T+ri,1ρu1,s1T]Δs1+[2(u1-si)T-ri,1ρu1,siT]Δsi, (4)式中：||si0||2≈||si||2-2siTΔsi和u1为一个粗估计的目标位置，其中u1可以由传统的TSWLS算法的第一阶段计算．考虑所有的传感器，式(4)可以写成h1-A1u=B1Δγ+D1Δs，(5)式中： h1=r2,1(||u1-s2||+||u1-s1||)-||s2||2+||s1||2r3,1(||u1-s3||+||u1-s1||)-||s3||2+||s1||2⋮rM,1(||u1-sM||+||u1-s1||)-||sM||2+||s1||2；A1=-2[(s2-s1)T,  (s3-s1)T,⋅⋅⋅,  (sM-s1)T]T；B1=diag(||u1-s2||+||u1-s1||,||u1-s3||+||u1-s1||,…,||u1-sM||+||u1-s1||);D1=K2S201×301×3⋯01×3K301×3S301×3⋯01×3K401×301×3S4⋯01×3⋮KM01×301×301×3⋯SM，其中，Ki=-2(u1-s1)T-ri,1ρu1,s1T，Si=2(u1-si)T-ri,1ρu1,siT，这里i≠1．式(5)的加权最小二乘解为û=(A1TW1-1A1)-1A1TW1-1h1，式中：û=[x̂,ŷ,ẑ]T，其中x̂,ŷ,ẑ为目标估计位置三维坐标；W1为权值矩阵，有W1=E[(B1Δγ+D1Δs)(B1Δγ+D1Δs)T]=B1QγB1T+D1QsD1T.式(4)忽视了(u1-u)Δsi的系数项和Δs12-Δsi2，这会导致定位结果当传感器位置噪声很大时偏离克拉美罗下界，但本文算法无法避免这个问题．2.2　修正目标定位误差本文算法引入目标定位误差Δu=û-u，û为目标估计位置，则第i个传感器目标的距离可以表示为ri0=||u-si0||=||u-si+Δsi||≈||û-Δu-si||+ρu,siTΔsi≈||û-si||-ρû,siTΔu+ρû,siTΔsi, (6)式中忽略了Δu，Δsi的系数项和它们各自的高阶项．将式(6)代入TDOA测量方程(3)中，并加入测量值噪声，可得ri,1=(||û-si||+||u-s1||)-||si||2+||s1||2+2(si-s1)Tû-[ri,1(ρû,siT+ρû,s1T)+2(si-s1)T∙Δu≈(||û-si||+||u-s1||)Δri,1-2[(û-s1)+ri,1ρû,s1T]Δs1+[2(û-si)T-ri,1ρû,siT]Δsi, (7)式中和式(6)做了相同的近似．考虑所有的传感器组成的TDOA定位方程，式(7)的矩阵形式为h2-A2Δu=B2Δγ+D2Δs．(8)设Ri=ri,1(||û-si||+||û-s1||) (i≠1)，Hi=-2(û-s1)T-ri,1ρû,s1T (i≠1)，Ci=2(û-si)T-ri,1ρû,siT (i≠1)，于是有：h2=R2-||s2||2+||s1||2+2(s2-s1)TûR3-||s3||2+||s1||2+2(s3-s1)Tû⋮RM-||sM||2+||s1||2+2(sM-s1)Tû；A2=r2,1(ρû,s2T+ρû,s1T)+2(s2-s1)Tr3,1(ρû,s3T+ρû,s1T)+2(s3-s1)T⋮rM,1(ρû,sMT+ρû,s1T)+2(sM-s1)T；B2=diag(||û-s2||+||û-s1||,||û-s3||+||û-s1||,…,||û-sM||+||û-s1||);D2=H2C201×301×3⋯01×3H301×3C301×3⋯01×3H401×301×3C4⋯01×3⋮HM01×301×301×3⋯CM．式(8)的加权最小二乘解可以写为Δû=(A2TW2-1A2)-1A2TW2-1h2，式中：Δû为目标估计位置误差，Δû=[Δx̂,Δŷ,Δẑ]T；W2=B2QγB2T+D2QsD2T为权值矩阵．最终的定位结果可以表示为u¯=û-Δû．3 仿真结果与分析为了验证算法性能，开展了两组仿真，将本文算法与传统的TSWLS算法[13]、改进的TSWLS算法[21]和克拉美罗下界(Cramer-Rao lower bound，CRLB)作对比．这里以定位算法的均方根误差(root mean square error，RMSE)和偏差(bias)为基准来衡量算法的定位性能．均方根误差和偏差分别定义为 ζRMSE=∑j=1L||u¯(j)-u||2/L1/2，βbias=∑j=1L(u¯(j)-u)/L ， 其中：u¯j为第j次仿真的估计值；L为蒙特卡罗随机试验次数，若无特定说明，则L=1 000．3.1　目标与传感器位置接近的仿真对比在水下定位场景中，目标位置坐标为u=[300,200,300]T m．共有6个传感器参与定位，其中参考传感器位置坐标为s10=[300,100，150]T m，其余传感器位置坐标分别为s20=[400,150,100]T m，s30=[300,500,200]T m，s40=[350,200,100]T m，s50=[-100,-100,100]Tm和s60=[200,-300,200]T m．TDOA测量值的方差设置为σt2=1×10-4，传感器位置误差的协方差矩阵和文献[21]设置相同，为Qs=σs2diag(10，10，10，2，2，2，10，10，10，40，40，40，20，20，20，3，3，3)，其中σs2为传感器位置误差的方差．图1和2分别为所提算法和对比算法的RMSE曲线和Bias曲线，图中R为克拉美罗下界协方差矩阵的迹．从图1可以看出：所提算法远远优于传统的TSWLS算法，这是因为TSWLS算法没有考虑传感器位置误差；当σs2取值在-40~0 dB时，所提算法和文献[21]算法的RMSE误差相同；当σs2取值在0~10 dB时，所提算法略优于文献[21]算法．从图2可以看出：所提算法的Bias曲线与文献[21]相似，并都优于TSWLS的Bias曲线．10.13245/j.hust.211015.F001图1目标与传感器位置接近时算法的RMSE曲线10.13245/j.hust.211015.F002图2目标与传感器位置接近时算法的Bias曲线图3为所有算法在σs2=9处的累积分布函数(cumulative distribution function，CDF)，图中：ΔU为估计位置误差；μ为累积分布函数．从图3可以看出：本文算法的累积分布函数上升趋势略快于文献[21]算法的累积分布函数，但两种算法差距不大．这是因为两种算法都已经接近克拉美罗下界，定位精度都很高．没有考虑传感器位置误差的TSWLS算法的累积分布函数上升很慢．10.13245/j.hust.211015.F003图3目标与传感器位置接近时算法的累积分布函数3.2　目标远离传感器位置的仿真对比保持6个传感器的位置不变，改变目标位置为u=[500,500,500]T m．TDOA测量值和传感器位置误差的协方差矩阵都和第一组实验相同．图4和5分别为所有算法的RMSE曲线和Bias曲线．10.13245/j.hust.211015.F004图4目标远离传感器时算法的RMSE曲线从图4中可以看出：当σs2在9.45~9.55 dB时，本文算法的RMSE约等于克拉美罗下界，这表明本文算法的精度是最高的．图5显示本文算法的Bias要略低于文献[21]算法．图6为算法在σs2=9处的累积分布函数，由图可见：在高传感器位置噪声情况下，TSWLS算法的误差非常大．本文算法的累积分布函数上升趋势和文献[21]算法差不多，但在部分区域略优于文献[21]算法．这主要因为本文算法在文献[5]的TDOA测量方程基础上，引入了传感器位置误差构造新的方程，通过粗估计和精补偿两个阶段的求解，能够进一步提升算法的定位精度．10.13245/j.hust.211015.F005图5目标远离传感器时算法的Bias曲线10.13245/j.hust.211015.F006图6目标远离传感器时算法的累积分布函数4 结语考虑传感器位置会随着洋流四处浮动的问题，本研究提出了一种存在传感器位置误差的TDOA算法，所提算法是一种两阶段最小二乘算法．第一阶段引入测量值噪声和传感器位置噪声构造加权系数矩阵，并用加权最小二乘算法求解粗估计目标；第二阶段引入目标位置误差构造新的方程，用加权最小二乘算法求解目标位置误差，并用求解的目标位置误差调节上个阶段的粗估计位置．仿真结果表明本文算法是有效和可靠的．