一种基于双换能器紧组合与航向自适应过程噪声的单信标水下定位方法

本发明涉及水下导航与定位，具体涉及一种基于双换能器紧组合与航向自适应过程噪声的单信标水下定位方法。

背景技术：

1、水下高精度导航是海洋探测、海底资源开发和水下自主作业的重要支撑技术。目前，水下导航面临的主要限制在于：全球导航卫星系统（gnss）信号进入水体后迅速衰减，难以为水下航行器提供连续、可靠的位置参考。捷联惯性导航系统（sins）虽可提供全自主的连续导航信息，但其位置误差会随时间持续累积并发散，难以独立满足长航时、高精度任务需求。为抑制惯性导航误差累积，通常需要引入外部辅助信息进行周期性校正，声学测距因作用距离较远、可在水下传播，是常用的外部辅助手段之一。

2、现有水声导航定位系统主要包括长基线（lbl）、短基线（sbl）和超短基线（usbl）。lbl通常需要布放多个海底应答器并进行基线标定，作业范围较大但部署、回收和维护成本较高；sbl通过多个具有已知间距的换能器形成阵列，安装和标定要求较高，且定位性能随作用距离与基线比增大而下降；usbl将基阵集成于紧凑单元内，安装方便，但其角度测量精度同样受到有效基线、作用距离和姿态误差影响。因此，在减少外部基准数量和部署复杂度的同时保持可用定位性能，是提高水下航行器自主作业能力的重要方向。

3、为降低系统部署成本，单信标水下导航定位受到广泛关注。单换能器在单个测量历元内仅提供一个斜距约束，即使利用深度信息约束垂向位置，水平位置仍位于以信标为中心的圆周上，单历元存在连续位置不确定性；垂直于当前视线方向的水平位移在单次测距中缺少直接约束，需要依赖后续运动、惯性先验或其他辅助信息逐步改善。部分几何解算方法还需要进行候选分支判别，在弱几何、噪声扰动或分支切换时可能产生较大的位置中间解误差。

4、此外，现有紧组合滤波方法通常采用固定的各向同性过程噪声，难以体现水下航行器不同运动方向上的模型不确定性差异。在典型巡航状态下，auv沿主要推进方向的运动通常具有较强可预测性，而水平横向更易受到海流、侧滑及未建模扰动影响。若对各方向采用相同的过程噪声强度，可能无法充分利用上述方向性运动特征。

5、综上所述，如何增强单信标条件下的单历元水平几何约束，避免弱几何条件下几何中间解的误差放大，并在滤波框架内利用auv运动不确定性的方向差异以提高定位精度和鲁棒性，是本领域需要解决的技术问题。

技术实现思路

1、为解决上述技术问题，本发明提出一种基于双换能器紧组合与航向自适应过程噪声的单信标水下定位方法。该方法至少包括以下技术特征：（1）在auv上设置两个具有已知非零基线的换能器，在同一历元获得两路斜距，使单换能器的连续水平位置不确定性被压缩为有限候选区域并增强单历元水平几何约束；（2）采用紧组合扩展卡尔曼滤波直接融合原始双斜距观测，不显式生成几何交会位置中间解；（3）依据当前航向在导航坐标系中重构方向性过程噪声，使沿航向、水平横向和垂向采用不同的过程噪声强度，并使方向分配随航向变化。

2、本发明所称“航向自适应过程噪声”是指过程噪声协方差的方向基于当前航向角逐历元重构；各方向噪声强度参数可根据平台运动特性预先设定或标定，并不限定为依据滤波新息在线估计噪声幅值。具体地，本发明在auv上固定设置两个具有已知杆臂和非零基线的声学换能器，利用姿态信息将杆臂转换至导航坐标系，建立两个换能器至同一信标的非线性斜距观测方程，并将原始双斜距直接作为扩展卡尔曼滤波器量测输入。

3、本发明采用如下方案：

4、一种基于双换能器紧组合与航向自适应过程噪声的单信标水下定位方法，包括：

5、s1：在自主水下航行器auv上以已知非零基线固定设置两个声学换能器，标定两个换能器相对于auv参考点在载体坐标系下的杆臂矢量；

6、s2：获取auv实时姿态信息，将两个换能器的杆臂矢量由载体坐标系转换至导航坐标系，得到两个换能器在导航坐标系下的位置；

7、s3：构建扩展卡尔曼滤波状态模型，建立两个换能器至同一声学信标的非线性斜距观测方程，并将两路原始斜距直接作为量测输入进行紧组合更新，不先求取双圆几何交会位置；

8、s4：根据auv当前航向角重构沿航向、水平横向和垂向具有不同噪声强度的方向性过程噪声协方差矩阵，并在预测阶段完成状态与协方差传播；

9、s5：基于预测状态和预测协方差，分别利用两路原始斜距进行扩展卡尔曼滤波量测更新，得到当前时刻的状态估计。

10、作为本发明的进一步改进，步骤s1的具体实现过程如下：

11、s11：定义载体坐标系frd，其中x轴指向auv前方、y轴指向auv右侧、z轴指向auv下方；

12、s12：两个换能器在frd坐标系下的杆臂矢量分别定义为:

13、 ，

14、式中，dsep为两个换能器之间的水平基线间距，取值为2.3 m；zrod为auv参考点至换能器阵列参考面的垂向杆臂长度，取值为0.2978 m。

15、作为本发明的进一步改进，步骤s2中，载体坐标系frd至导航坐标系enu的坐标转换通过两级旋转实现：

16、首先，利用欧拉角(φ, θ, ψ)将frd体坐标系旋转至ned坐标系，旋转矩阵为3×3正交矩阵；然后，将ned坐标系转换为enu坐标系：；换能器在enu坐标系下的全局位置为，其中pauv为auv质心在enu坐标系下的位置向量。

17、上述换能器安装方案使两个换能器在同一测量历元获得两路斜距。两路斜距与已知基线共同形成差分几何约束，可将单换能器对应的连续水平位置不确定性压缩为有限候选区域；当存在对称候选解时，可结合滤波先验、历史轨迹或运动连续性完成分支判别，由此在保留双换能器差分几何信息的同时，避免将单历元几何解的唯一性作为滤波成立条件。

18、作为本发明的进一步改进，步骤s3的具体实现过程如下：

19、s31：定义扩展卡尔曼滤波器的状态向量为x = [x, y, z, vx, vy, vz]ᵀ，包含三维位置和三维速度，状态转移矩阵f采用常速度模型：

20、 ，

21、s32：建立两个换能器到信标的斜距观测方程：

22、；

23、式中，li为第i个换能器到信标的斜距观测值，为第i个换能器在导航坐标系下的位置向量，b为信标位置向量，ηi为斜距测量噪声。两路斜距直接参与紧组合滤波更新，不生成几何交会位置中间解。

24、s33：深度观测由auv搭载的压力传感器提供，观测方程为：

25、 ，

26、式中，压力传感器输出的原始深度先依据选定的深度基准和垂向坐标正方向转换为与状态z一致的垂向位置观测zdepth，再按zdepth=z+νd进行量测更新，νd为深度观测噪声。

27、步骤s32中所述斜距观测方程的雅可比矩阵为：

28、 ，

29、式中，hi 为第 i 个换能器斜距观测方程关于状态向量的雅可比矩阵，为换能器到信标的视线向量在enu坐标系下的三轴分量，定义为：，rᵢ为换能器到信标的几何距离。该雅可比矩阵为换能器到信标方向的单位矢量，表明斜距观测仅提供沿视线方向的径向距离约束。

30、作为本发明的进一步改进，步骤s4的具体实现过程如下：

31、s41：以enu导航坐标系为例，规定航向角ψ以正北为0°并沿顺时针方向增加，根据当前航向角ψ定义三个正交方向单位向量：

32、沿航向方向：ealong = [sinψ, cosψ, 0]ᵀ ，横向方向：ecross = [cosψ, -sinψ, 0]ᵀ ，垂向方向：eup = [0, 0, 1]ᵀ 。

33、s42：设定各方向的加速度过程噪声标准差，构建加速度协方差矩阵：

34、 ，

35、式中，σalong为沿航向方向的加速度过程噪声标准差，σcross为水平横向方向的加速度过程噪声标准差，σup为垂向方向的加速度过程噪声标准差；ealong ealongᵀ、ecross ecrossᵀ和eup eupᵀ分别为对应方向的投影矩阵。

36、s43：对于常速度模型，将连续时间白噪声加速度模型离散化，得到离散时间过程噪声协方差矩阵q：

37、 ，

38、式中，δt为滤波器的采样时间间隔；为加速度过程噪声协方差矩阵；

39、s44：在离散过程噪声协方差矩阵的位置子块中加入最小位置过程噪声项：

40、q(1:3, 1:3) = q(1:3, 1:3) + qpos,min · i3 ，

41、式中，qpos,min为每历元附加至位置子块的最小位置过程噪声方差，i3为3×3单位矩阵。

42、s45：执行预测阶段的状态和协方差更新：

43、 ，

44、式中， 为第 k-1 时刻估计值对第 k 时刻状态的一步预测值；第 k-1时刻的状态后验估计值；为状态转移矩阵；第 k 时刻的一步预测协方差矩阵；第 k-1 时刻的后验协方差矩阵；为过程噪声协方差矩阵；

45、本实施例中，沿航向设置较小的加速度过程噪声标准差σalong=0.10 m/s2，水平横向设置σcross=0.30 m/s2，垂向设置σup=0.05 m/s2。上述参数仅为本实施例的优选配置，用于体现不同方向的运动不确定性差异；其中较小的σalong加强沿主要推进方向的模型约束，较大的σcross使横向状态保留更充分的量测修正能力。不同平台可根据运动特性、采样率及量测噪声重新标定。

46、本实施例在离散过程噪声协方差矩阵左上3×3位置块中加入qpos,min·i3，其中qpos,min=1×10-4 m2/step，对应每历元1 cm的最小位置过程标准差。该项用于避免高采样率下位置过程噪声过小，其数值可根据采样率和平台运动特性进行调整。

47、作为本发明的进一步改进，步骤s5的具体实现过程如下：

48、s51：利用当前姿态矩阵将第一换能器的杆臂转换至enu坐标系，计算第一换能器到信标的预测斜距h1及其雅可比矩阵h1，计算新息协方差，若满足则执行卡尔曼更新，否则跳过该观测；

49、s52：利用当前姿态矩阵将第二换能器的杆臂转换至enu坐标系，计算第二换能器到信标的预测斜距h2及其雅可比矩阵h2，计算新息协方差，若满足则执行卡尔曼更新，否则跳过该观测；

50、s53：以深度观测进行更新，深度观测矩阵为，计算卡尔曼增益，更新状态和协方差；

51、s54：每次更新后对协方差矩阵进行对称化处理p ← (p + pᵀ)/2。

52、作为本发明的进一步改进，步骤s51和s52中所述卡尔曼更新的具体过程为：

53、 ，

54、式中，ki为第 i 个斜距观测的卡尔曼增益向量；p为当前状态协方差矩阵；hi 为第 i 个换能器斜距观测方程的雅可比矩阵；si为第 i 个斜距观测的新息协方差；为当前状态估计向量；为基于当前状态估计预测的第 i 个斜距值； i为6×6单位矩阵。

55、本实施例对每一路斜距独立执行基于新息协方差的3σ门控：计算sᵢ=hᵢphᵢᵀ+σᵣ2，仅当新息绝对值小于3√sᵢ时接受该路观测；3σ为本实施例采用的门限系数，也可依据异常值统计特性采用其他预设门限。

56、为减小有限精度数值计算引入的协方差非对称误差，每次更新后可执行p←(p+pᵀ)/2的对称化处理。

57、顺序标量更新使两路斜距可以分别计算新息、卡尔曼增益和门控结果；其主要工程优势在于可以对两路声学观测进行独立异常检测。当其中一路未通过门控时，另一路有效斜距仍可继续更新状态，而不必因单路异常而同时舍弃全部声学观测。

58、与现有技术相比，本发明具有以下有益效果：

59、（1）双换能器在同一历元提供两路斜距，相对于单换能器单斜距增加了与载体-信标相对方位相关的差分几何信息，可将连续的水平位置不确定性压缩为有限候选区域，增强单历元水平约束并降低对长时间机动激励的依赖。

60、（2）紧组合扩展卡尔曼滤波直接利用原始双斜距进行非线性量测更新，避免将弱几何条件下易受测距噪声放大的几何交会位置作为中间观测量输入滤波器，从而提高弱几何条件下的定位稳定性；该技术效果来源于避免几何中间解误差放大，而非假定远距离几何信息本身消失。

61、（3）航向自适应过程噪声根据当前航向逐历元重构方向分配，使沿航向、水平横向和垂向采用不同的过程噪声强度，以利用auv运动不确定性的方向差异。在运动状态和声学几何条件适宜时，可改善预测模型与量测更新之间的匹配；在快速转向等工况下，仍由原始双斜距紧组合框架提供基础定位约束和鲁棒性。