具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

如图1中所示，本发明涉及一种二维声呐基阵运动姿态自校准方法，包含以下步骤：

步骤(1)：选取二维基阵的三顶点位置处的接收基元，对接收到的回波信号进行脉冲压缩处理，选取探测区域的三个“声亮点”目标，分别估计目标至顶点位置处的整数倍时延以及小数倍时延，计算目标斜距。

步骤(2)：利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组并构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解。

步骤(3)：根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差。

步骤(4)：利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差，从而联合估计系统六自由度运动误差。

步骤(1)中，选取二维基阵的三顶点位置处的接收基元，对接收到的回波信号进行脉冲压缩处理，选取探测区域的三个“声亮点”目标，如图1所示。在三维空间内，选取三个亮点位置，估计这些亮点与二维基阵的其中三个顶点之间的距离，从而确定基阵位置。基于“声亮点”的六自由度运动误差联合估计方法利用上一次基阵所在位置估计当前基阵所在位置，因此首先估计独立亮点目标到基阵三顶点位置的时延。设定探测区域存在三个独立亮点E(x₁,y₁,z₁)，F(x₂,y₂,z₂)和G(x₃,y₃,z₃)，载体初始位置P₀处选取三个位置已知的顶点A₀(x₀₁,y₀₁,z₀₁)，B₀(x₀₂,y₀₂,z₀₂)和C₀(x₀₃,y₀₃,z₀₃)，载体偏移位置P₁处选取三个位置需要估计的顶点A₁(x₁₁,y₁₁,z₁₁)，B₁(x₁₂,y₁₂,z₁₂)和C₁(x₁₃,y₁₃,z₁₃)，二维基阵航迹向实孔径d_y和水平向实孔径d_x已知。对于离散线性调频信号，发射信号可以表示为下式：

其中，N为发射信号脉宽，f₀为信号中心频率，f_s为系统采样率，n为采样序号。当发射信号经目标反射回接收基元时，其时延点数可以表示为t+Δt，其中整数倍时延点数t可以由相关峰计算得到，因此只考虑其时延点数估计残差Δt，信号形式为：

对接收信号进行匹配滤波处理，可以得到离散信号表达式如下所示，可以观察到当时刻m＝N+Δt时，R(m)取得幅值最大值且此时的相位项为0。由此，可以利用信号相关峰处的相位值求解时延点数残差Δt，由于点数残差Δt恒小于一个采样点对应的时间门，因此其相位变化处于主值区间内不存在相位模糊问题。

对互相关信号R(m)提取出其相位P，则在m＝N时刻的时延点数估计残差有如下关系：

求解一元二次方程根可得时延估计残差点数Δt：

方程具有两个平方根，根据约束条件|Δt|＜1可以确定其中的有效解，因此利用相关峰及其时延点数估计残差可以计算得到精确的目标与阵元之间的时延值τ，根据声速可以计算获得独立亮点至接收阵元之间的斜距l。

步骤(2)中，利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组并构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解。

如图1所示，在载体初始位置P₀处，可以根据亮点E至A₀、B₀和C₀三顶点处的距离唯一确定其坐标位置，根据相关函数可以确定载体在偏移位置P₁处时亮点E至A₁、B₁和C₁三顶点处的距离和并且有距离关系式：

类似的，对于亮点E和F也具有关系式：

此外，对于二维面阵结构的接收阵，其任意三顶点位于直角三角形上，有如下关系：

为了计算方便，将上述方程组改写成如下形式：

上述方程组具有三个顶点坐标的9个未知数和12个方程的限定条件，并且限定条件为二次方程组，因此其解析解可能不存在，需要通过求解非线性超定方程组最小二乘解的方式，求取三顶点坐标。对于非线性超定方程组的解法，首先需要将其转换成恰定方程组，之后再通过迭代法求解未知数。构造目标函数g(x)，利用梯度法求解函数的最小二乘解：

从某一点出发，将函数在该点下降最快的方向作为搜索方向，则其负梯度方向为该点下降最快的方向。由此可以将问题转化为一组沿负梯度方向搜索目标函数最优解的问题。对于目标函数G(ξ)，寻找使其达到最小值的ξ，即为所寻找的最小二乘解，其中未知数ξ＝(x₁₁,y₁₁,z₁₁,x₁₂,y₁₂,z₁₂,x₁₃,y₁₃,z₁₃)。设定迭代门限ε，当进行第k此迭代时，该组解ξ^k能够满足G(ξ^k)＜ε，则终止迭代。

梯度法迭代按照以下步骤进行：

(1)给定初始位置处的基阵三顶点坐标ξ⁰＝(x₀₁,y₀₁,z₀₁,x₀₂,y₀₂,z₀₂,x₀₃,y₀₃,z₀₃)，作为迭代初始值，设定ξ^k为第k次迭代获得的估计值。

(2)计算目标函数G(ξ^k)的值，如果G(ξ^k)＜ε则认为此时的ξ^k为所求最小二乘解，否则需要对迭代解进行如下修正：

对于目标函数G(ξ^k)的梯度可以通过对ξ求偏导获得：

其中，H控制收敛常数。

(3)重复修正迭代解直到满足收敛精度(G₁ ^k+1,G₂ ^k+1,…,G₁₂ ^k+1)＜ε，终止计算。

步骤(3)中，根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差。通过上述步骤中的梯度迭代方法，可以估计出载体在P₁位置处的三顶点坐标和并且可以计算得到基阵中心和第四个顶点位置：

步骤(4)中，利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差。对于载体中心，其位置不受姿态变化的影响，因此其位置的变化反映了载体的平移误差，由此可以获得载体的位置误差(Δx,Δy,Δz)。根据二维基阵的四个顶点位置，结合设计的基阵阵元间距，可以步进求取所有接收基元的实际空间位置。此外，还可以通过三维空间变换逆矩阵求取载体的姿态误差，其中P_r0 ^-1为初始位置P_r0的广义逆。

M_{＜α,β,γ＞}＝(P_r1-P_Δ)×P_r0 ^-1

通过旋转矩阵M_{＜α,β,γ＞}＝M_αN_βQ_γ的定义，可以获得约束关系式：

上述约束条件为具有3个未知数和9个方程的非线性超定方程组，同样可以用上述梯度法迭代搜索最小二乘解，由此可以估计出载体的姿态误差(Δα,Δβ,Δγ)。

下面结合本申请方法的一种具体应用仿真对本申请进行更详细的阐述：选取二维多波束合成孔径声呐系统基阵结构，系统信号中心频率f₀＝150kHz，信号带宽B＝10kHz，脉冲宽度τ＝10ms，发射阵元实孔径尺寸D＝10cm。水平向阵元数目32，阵元间距5mm，航迹向阵元数目4，阵元间距11cm。设定载体运动速度为0.2m/s，理论运动轨迹应为沿航迹向的匀速直线运动。在理论运动轨迹上叠加误差曲线，设定载体的位置误差按照幅值为1m的正弦波变化，姿态误差按照幅值为10°的正弦波变化。利用本申请提出的二维基阵姿态自校准方法估计载体的运动轨迹及运动偏差，载体运动轨迹如图2所示，以基阵中心点位置作为载体的运动轨迹。可以观察到，经过最小二乘估计后的载体中心位置与预设存在运动误差时的载体中心位置吻合度较好，能够准确的反映载体的实际位置。

利用本申请提出的二维基阵姿态自校准方法联合估计六个自由度的位置误差和姿态误差，算法估计结果与预设结果之间的偏差如图3和图4所示。六自由度误差随着航迹向采样位置变化，位置误差的估计偏差保持在厘米量级，姿态误差的估计偏差保持在0.05°范围，算法仿真结果证明了基于六自由度联合误差估计算法的有效性。此外，还利用具有8个运算核心的DSP器件TMS320C6678进行了迭代算法仿真，CPU工作在单核、主频1GHz情况下，运动误差估计算法运行时间约为毫秒量级，经器件并行化处理后会获得一定的加速度比，因此计算时间会进一步减少，保证了联合估计算法在工程应用条件下的运算实时性。

图4和图5分别展示了仿真硬件姿态估计方法精度不足时的成像效果和经本申请中提出的自校准方法成像结果，给定仿真条件，载体以0.5m/s的速度航行，在理论运动轨迹上叠加误差曲线，设定载体的位置误差按照幅值为1m的正弦波变化，姿态误差按照幅值为10°的正弦波变化。设定30个独立点目标处于深度值15m的平面上，目标间距20cm，利用多波束合成孔径声呐成像算法对探测区域进行三维成像。首先，模拟硬件补偿精度不足情况下的成像结果，将预设的载体位置精度范围控制在2cm范围内，将姿态精度控制在0.2°范围内，对姿态信息进行截断。图4与图5对比可以发现，虽然目标所在区域成像效果基本正确，未发生严重的图像散焦，但目标个数和位置发生混叠，成像效果未达到理论效果。经本申请中提出的自校准方法联合估计六自由度运动误差，对获得的运动误差补偿后的成像效果显示目标位置和间距与理论预设值相符，并且目标位置的能量聚集程度较好，达到了预期效果，证明了本申请提出方法的有效性。

本申使用最小二乘估计方法计算二维基阵的六自由度联合误差，相对于单自由度的误差估计具有更好的工程适用性。通过整数倍估计和小数倍估计结合的方式，提高目标至基阵的时延估计精度，从而提升六自由度联合估计的准确性。采用针对目标回波的软件运动误差估计方法，利用二维基阵接收到的回波信号及其几何关系，实现载体运动姿态的自校准过程。避免了硬件估计带来的高成本和对于装载空间的要求限制，适用于水下无人小空间的工程化应用场景。

综上，本发明公开了一种二维声呐基阵运动姿态自校准方法。首先对声呐基阵接收到的回波信号进行脉冲压缩处理，分别估计“声亮点”距离基阵各顶点的整数倍时延及小数倍时延，结合二维基阵孔径建立非线性超定方程组。针对获得的非线性超定方程组构造目标函数，利用梯度法迭代求解方程组的最小二乘解，从而获得当前采样位置处的二维基阵中心、平移误差以及四顶点位置。利用系统旋转矩阵再次建立非线性超定方程组，迭代求解获得姿态误差。本方法通过对回波信号的时延估计和基阵结构建立几何方程，能够实现二维基阵运动姿态的六自由度运动误差联合自校准过程，具有不依赖于昂贵的外部辅助设备、时延估计精确高、算法实时性良好等技术优势。