基于向量投影的立体阵定位算法

孙大军 蔡 珩 郑翠娥 程驰宇

(哈尔滨工程大学水声技术全国重点实验室 哈尔滨 150001)(海洋信息获取与安全工信部重点实验室(哈尔滨工程大学) 工业和信息化部 哈尔滨 150001)(哈尔滨工程大学水声工程学院 哈尔滨 150001)

1 引言

超短基线定位技术是水下航行器或潜水员进入深海、探测深海和开发深海的关键技术[1-5]。目前，超短基线定位技术在民用领域已实现商业化。法国iXblue公司的POSIDONIA II[6]、英国Sonardyne公司的HPT[7]、挪威Kongsberg公司的HiPAP[8]均为国际上知名的超短基线定位设备，代表着当今世界的最高水平。

超短基线通过测量目标到达声学基阵各个基元的传播时延来估计目标相对声学基阵的方位和距离从而确定目标的位置[9,10]。传统声学基阵大多为线阵和平面阵，文献[11]介绍了一种针对线阵的超短基线定位算法，此算法利用不同观测点测得的空间方位角建立非线性方程，通过拟牛顿法估计目标位置。针对线阵的算法需综合利用不同观测点的角度信息，且要迭代求解非线性方程，定位效率低，难以满足对运动目标的高帧率定位需求。平面阵能在每个观测点完成一次目标位置的解算，能提供高帧率位置信息。文献[12-14]介绍了几种常见平面阵阵型(L型平面阵、十字平面阵、正五边形平面阵)的定位解算方法。这些方法都存在一个特点，即能直接获得目标的水平定位结果，目标的垂向定位结果需通过斜距和水平结果间接获得，这就导致了目标垂向定位精度受水平定位精度影响较大。为使目标垂向定位结果与水平独立，需要采用立体阵以直接获得目标垂直方向的定位结果。现有适用于立体阵的定位算法较少；文献[15]介绍了针对4元立体阵的超短基线定位算法。此算法利用4元立体阵的对称结构，独立构造出目标垂直位置的观测方程，能直接获得目标的垂向位置。但此算法仅适用于特定几何构型的立体阵，有一定的局限性。为实现一个算法适应不同结构立体阵的解算；文献[16]介绍了三角分解算法。三角分解算法首先将立体阵分解成不同朝向的3元阵组合，每个组合可求解一个目标位置，其次对所有组合的定位结果进行平均，作为目标的最终定位结果。三角分解算法涉及大量坐标基转换解算步骤极其繁琐，且当阵元数量较多时，运算量将急剧增大。此外，针对立体阵这类复杂的阵型结构，三角分解算法的定位模型十分复杂，难以分析其误差特性。

对于三角分解算法存在的问题，本文从向量的角度提出了另一种适用于立体阵的算法，基于向量投影的立体阵定位算法。本文算法从向量投影的角度构建立体阵中各基线向量与目标方位之间的观测方程，通过解观测方程组实现对目标方向余弦向量的估计。相较于三角分解算法，本文提出的算法时间复杂度更小，且待估计参数与量测数据间的关系呈线性形式，易分析定位误差与各参量之间的影响关系。

2 三角分解算法及其缺陷

由引言部分可知，三角分解算法能适用于立体阵型的解算。算法的思想可以概括如下：(1)将立体阵分解为不同朝向的3元阵组合，每个组合定义一个本地坐标系；(2)确定目标在每个本地坐标系下的坐标；(3)通过坐标转换获得目标在基阵坐标系下[17]的坐标；(4)对所有组合的结果进行平均，获得目标的最终位置。

下面以某一个3元阵组合为例，给出具体定位原理。假设3元阵定义的本地坐标系为o-xLocalyLocalzLocal；目标在本地坐标系下的位置XLocal可由式(1)和式(2)获得。

其中，R代表目标到本地坐标系原点的距离。ν代表3 元阵中两基线的夹角。cosα=cτ1/d1,cosβ=cτ2/d2代表目标位置向量与3元阵中两基线夹角的余弦值。τ1,τ2分别代表声波到达两基线中各基元间的时延差，d1,d2代表基线长度，c代表声波在水中的传播速度。

由于本地坐标系与基阵坐标系并不一致，需通过坐标旋转获得目标在基阵坐标系下的位置XArray。

其中，RLA代表本地坐标系到基阵坐标系的旋转矩阵，θ代表两个坐标系之间的欧拉角矢量。

至此，可获得3元阵组合的定位结果。文献[16]给出了三角分解算法的详细推导，此处不再过多赘述。三角分解算法的主要量测数据为cτ1,cτ2，将其分别记为L1=cτ1,L2=cτ2，分析量测误差ΔL1, ΔL2对定位误差ΔXArray的影响程度。将式(3)写作XArray=RLA(θ)XLocal=f，对其两边同时求微分，有

式(4)的展开式十分复杂，从中看不到明显的关系。但由于f与cosα, cosβ有关，可知定位误差ΔXArray不仅与量测误差ΔL1, ΔL2有关，还与目标的方位有关。

综上，三角分解算法虽能适用于立体阵的解算，但也存在如下缺陷。(1)计算量大。对于一个N元立体阵，共能分解出种朝向的3元阵组合，每个组合都需要计算一次目标位置，计算量较大。(2)解算所需参数难以直观获取。三角分解算法需知道本地坐标系与基阵坐标系间的欧拉角，对立体阵这类复杂的阵型结构而言，该量难以直观获得。(3)误差分析困难。三角分解算法的定位误差解析式十分复杂，难以通过解析式直观获得定位误差与各参量间的关系。

3 基于向量投影的立体阵定位算法

针对三角分解算法的缺陷，本文提出基于向量投影的立体阵定位算法(以下简称向量投影算法)，从向量投影的角度构建立体阵中各基线向量与目标方位之间的观测方程，实现对三角分解算法定位模型的简化。

3.1 模型构建及参数求解

假设在基阵坐标系下，立体阵中任意两基元的坐标为Xj=[xj,yj,zj]T与Xi=[xi,yi,zi]T，此时基线i,j的方向向量可表示为Xji=Xj-Xi，如图1(a)所示。此外假设目标在基阵坐标系下的方向余弦为e=[cosα,cosβ,cosγ]T，方向余弦各角度的定义如图1(b)所示。

图1 基线与目标的方向向量

基于向量投影定理，可获得目标的方向余弦向量e与基线向量Xji夹角的余弦值为

其中， (·)代表向量点积，θ代表向量e与Xji间的夹角，dij代表基线i,j的长度。

此外，由二元测向定理[17]，可通过基线间的时延差，获得目标与基线i,j夹角的余弦值为

τij其中， 代表声波到达基元i,j的时延差。联立式(5)与式(6)可得基线向量与目标方向余弦向量的观测方程：

将不同基线对应的量测数据及待估计参数以矩阵形式表达为

其中

基于最小二乘定理，目标的方向余弦e可根据式(10)获得

假设基阵的几何中心为基阵坐标系的原点，可以根据式(11)获得目标到基阵坐标系原点的斜距R。

至此，目标的斜距与方向余弦均为已知量，目标的定位结果X可由式(12)获得

3.2 向量投影算法的误差分析

接下来对向量投影算法的估值进行误差分析。对式(10)两边同时取微分，有

由式(13)可以看出，量测误差ΔL传递到估值误差 Δe的程度与A有关，而A由基线向量组成，与立体阵的几何构型有关，为进一步分析估值误差与立体阵几何构型的关系，下面以一个普遍的4元立体阵为例进行推导。4元立体阵的构型如图2所示。

图2 4元立体阵

图2中，1-3基线与2-4基线的长度相同，到xoy平面的距离相等，投影到xoy平面上分别与x轴、y轴重合，并关于原点对称。定义立体阵4个基元的坐标为Xi=[xi,yi,zi]T(i=1,2,3,4)。为便于求解式(13)中ATA的逆，基于式(7)，以1-3基线向量，2-4基线向量，1-2加3-4基线向量构建观测方程组，有

将式(14)中A代入式(13)中，易求得估值误差与立体阵构型的关系

定义立体阵在x,y,z方向的孔径为|x3-x1|,|y4-y2|, |z2-z1|；则由式(1 5)可知，增加|x3-x1|, |y4-y2|, |z2-z1|，能分别减小cosα,cosβ, cosγ的估值误差。故增加立体阵3方向的孔径，能降低估值误差。

3.3 算法优势分析

从上面的分析可以得出，向量投影算法通过解线性方程组即可实现对目标方向余弦向量的估计。相比于三角分解算法，向量投影算法具有以下优势：

(1) 时间复杂度低。由文中的式(10)可知，向量投影算法的计算量主要集中在矩阵的运算上。对于一个N元立体阵，向量投影算法的独立量测量个数为N-1个，对应可列出N-1个观测方程，相应的A矩阵为(N-1)×3，L向量为(N-1)×1。根据矩阵运算的时间复杂度[18]，可计算出式(10)中的时间复杂度为O(21N+6)，根据时间复杂度的定义[18]，当N较大时，可忽略常数项及系数的影响，则向量投影算法的时间复杂度可简化为O(N)。同样对于一个N元立体阵，三角分解算法总共能分解出个朝向的3元阵组合，对于每个3元阵组合，都需计算一次目标位置，由于文中式(1)至式(3)与N无关，则三角分解算法的时间复杂度主要取决于3元组合的数量，即。忽略低阶项及系数，可将三角分解算法的时间复杂度简化为。由于的时间复杂度远小于，由此可得向量投影算法的时间复杂度更低。

(2) 解算所需的参数更易获取。相比于三角分解算法，向量投影算法无本地坐标系的定义，无需知道坐标系间的欧拉角，仅需输入各基元在基阵坐标系下的坐标、量测时延及声速，即可估计目标位置。

(3) 误差分析直观。针对立体阵这类复杂的阵型结构，传统三角分解算法难以通过解析式获得定位误差与各参量间的关系；而对于向量投影算法，能通过解析式获得定位误差与立体阵孔径间的关系。

4 仿真分析

为验证本文算法的性能，本部分通过算法运行时间与定位误差评价优劣。仿真中目标的位置采用球面坐标进行描述，如图3所示。图中R表示目标的斜距，E表示目标的俯仰角，定义目标位于xoy平面下方时为负，A表示目标的水平方位角，定义以x轴为起点，左手规则为正。

图3 水平方位角与俯仰角的定义

4.1 算法运行时间对比

此部分仿真对比不同基元数量下，两种算法的运行时间。仿真过程中，选择一个固定目标，俯仰角-45°，方位角45°，斜距50 m。仿真所用阵型为不同基元数量的圆锥阵，如图4(a)所示，顶基元距基阵水平面的距离为0.13 m，底面基元位于xoy平面上半径为0.13 m，以原点为中心的圆周上。底面基元的数量由4增大到10，仿真统计两种算法定位1 000次的运行时间，如图4(b)所示。

图4 算法运行时间对比

由图4可看出，三角分解算法的运行时间远大于向量投影算法，且随着基元数量变多增长斜率变大。

4.2 不同误差源下性能分析

此部分仿真分析不同量测误差源对三角分解算法及向量投影算法定位误差的影响。

由算法的观测方程可以看出，两种算法的定位误差由声速误差、基元位置误差及时延误差共同决定，3种误差中，基元位置误差在实际作业前会通过高精度阵型校准技术[19]进行修正，因此忽略基元位置误差的影响，重点分析声速误差和时延误差的影响。以下仿真所用的阵型参数为图2所示的4元立体阵。其中，x,y方向的孔径为0.26 m，z方向的孔径为0.13 m。目标位置设置与4.1节相同。

首先分析声速误差的影响。声速误差常以固定误差的形式存在，仿真过程中，声速误差从0 m/s均匀变化到10 m/s。两种算法的定位误差如图5所示。

图5 声速误差

由图5可看出随着声速误差的增大，两种算法的定位误差近似线性增大。在此斜距和方位下，10 m/s的声速误差对三角分解算法的影响最大不到2.4%R；对向量投影算法的影响最大不到0.52%R。

接下来分析时延误差的影响。时延误差常以随机误差的形式存在。假设立体阵各基元的时延误差统计独立，且都服从均值为0，标准差为σ的正态分布。仿真过程中，标准差σ的大小从0 μs均匀增加到5 μs。在每个标准差下进行1 000次蒙特卡罗实验。其他仿真条件与图5相同。仿真结果如图6所示。

图6 时延误差

由图6可看出随着时延误差的增大，向量投影算法的定位误差近似线性增大，但三角分解算法的定位误差呈非线性关系。在此斜距和方位下，5 μs的时延误差对两种算法的影响最大均超过了4%R。

从图5、图6的仿真结果可得出以下结论：声速误差对两种算法的定位误差影响较小，时延误差对两种算法的定位误差影响较大。在实际使用中，可通过提高信噪比和采样率提升时延量测精度。

4.3 不同目标方位下性能分析

为更普遍地分析算法性能，本部分仿真计算同一量测误差下，目标位于不同方位时两种算法的定位误差。仿真所用的阵型与4.2节一致，声速误差设置为5 m/s，时延误差的标准差设置为5 μs。

先分析不同水平方位角的影响。仿真过程中，固定目标的俯仰角为-45°，斜距为50 m。目标的水平方位角从-180°均匀变化到180°。两种算法的定位误差如图7所示。

图7 不同水平方位角下定位误差

由图7可以看出，三角分解算法的定位误差随水平方位角的变化上下震荡，而向量投影算法随水平方位角的变化不大。两种算法在不同水平方位角下的定位误差各有优劣。

接下来分析不同俯仰角的影响。固定目标的水平方位角为45°不变。目标的俯仰角从-90°均匀变化到0°，两种算法的定位误差如图8所示。

图8 不同俯仰角下定位误差

由图8可以看出，三角分解算法的定位误差受俯仰角的影响较大，而向量投影算法的几乎不随俯仰角变化。两种算法在不同俯仰角下的定位误差同样各有优劣。

由图7、图8可知：不同目标方位下，两种算法的定位误差在数值上各有优劣；但三角分解算法的定位误差受目标方位的影响较大，而向量投影算法的定位误差几乎不受目标方位的影响，性能更加稳定。

4.4 不同立体阵孔径下性能分析

本文在误差分析部分，得到了向量投影算法的估值误差与立体阵孔径成反比的结论。本节通过仿真验证此结论的正确性。仿真中，量测误差的设置与4.3节相同，目标位置的设置与4.1节相同。

基于图2所示的4元立体阵，定义x,y方向的孔径为水平孔径，定义z方向的孔径为垂直孔径。首先分析水平孔径的影响，仿真中，垂直孔径固定为0.26 m不变，水平孔径从0.26 m均匀增加到0.52 m。两种算法的定位误差如图9所示。

图9 不同水平孔径下定位误差

由图9可以看出，随着水平孔径的增大，向量投影算法的水平定位误差明显降低，垂直定位误差几乎不变；三角分解算法的水平与垂直定位误差随着水平孔径的增大，呈先增大后减小的趋势。

接下来分析垂直孔径的影响，水平孔径固定为0.26 m不变，垂直孔径从0.26 m均匀增加到0.52 m。两种算法的定位误差如图10所示。

图10 不同垂直孔径下定位误差

由图10可以看出，随着垂直孔径的增大，向量投影算法的水平定位误差变化不明显，垂直定位误差明显降低。三角分解算法的水平与垂直定位误差随着垂直孔径的增大而减小。

从图9、图10可得以下结论，随着基阵水平与垂直孔径的增大，向量投影算法的定位误差明显降低，验证了误差分析结论的正确性；但三角分解算法的定位误差与阵型孔径间无明显的单调规律。

5 湖试数据处理与分析

2022年2月27日，在湖北省丹江口开展了超短基线定位系统定位精度验证试验。试验期间平均水深约45 m。试验设备包括：哈尔滨工程大学自研的超短基线5元立体阵(阵型具体参数如图11所示)、声信标，法国iXsea公司的PHINS高精度姿态测量仪，中国华测公司P5型GNSS设备。试验中，姿态测量设备固定安装在5元立体阵的腔体内部；超短基线5元立体阵与GNSS天线通过4.5 m长的三脚架刚性连接，安装于船只右舷；5元立体阵入水深度约2 m；声信标布放在湖底，深度约40 m。

图11 湖试所用立体阵阵型

声信标在大地坐标系[20]下的位置为先验已知量，在试验前通过标定方法获得[20]。试验过程中，声信标布放在湖底不动，水面船只以声信标为中心，走半径约100 m的正反圆加正反叉航迹，如图12所示；期间5元立体阵利用1至4号基元在每个定位周期内完成对声信标的一次定位解算。

图12 水面船航迹

由于试验过程中，声信标在大地坐标系下的位置未曾改变，若将5元立体阵在基阵坐标系下的定位结果转到大地坐标系，则结果应基本聚合在一点。为对比三角分解算法、向量投影算法的定位精度，将两种算法的定位结果通过坐标转换公式转到大地坐标系下，以声信标的标定位置为参考，对比两种算法的离散程度，如图13的直方图所示。表1统计了图13中两种算法在3个方向的均方根误差及运行时间。

表1 算法均方根误差及运行时间统计结果

图13 与标定结果差值的直方图

由图13及表1的结果可以看出，向量投影算法与三角分解算法在3方向的均方根误差基本一致，差距在厘米级，向量投影算法略优；此外，三角分解算法的运行时间为0.122 5 s，远大于向量投影算法的0.013 1 s，是向量投影算法的9倍多。

湖试数据处理结果表明，向量投影算法能获得与三角分解算法几乎一致的定位精度，且计算效率更高。

6 结论

本文针对现有适用于立体阵型的三角分解算法计算量大、定位误差难以通过解析式精确表征的问题，从向量的角度提出了向量投影算法。与三角分解算法相比，向量投影算法的时间复杂度更低，解算所需的参数更易获取，且定位误差的解析式简洁、直观。通过对向量投影算法的误差分析，得到增大立体阵孔径能降低算法估值误差的结论，该结论对立体阵的阵型设计具备指导意义。仿真分析表明，在不同目标方位下，相较于三角分解算法，向量投影算法的定位误差几乎维持在同一数值，受目标方位影响小，算法性能更加稳定且运算时间更少；此外，基于误差分析的指导，增加基阵的水平与垂直孔径能明显减小向量投影算法的定位误差，验证了误差分析结论的正确性。湖试数据处理结果表明，向量投影算法能获得与三角分解算法几乎一致的定位精度，但运算时间少于三角分解算法的九分之一。