基于改进无迹卡尔曼滤波与前视声呐的水下运动目标被动跟踪方法

《Electronics》：Frequency Regulation Strategy of MPC-VSG for Flywheel Energy Storage Systems Considering State of Charge

　　

摘要：研究人员提出一种基于改进无迹卡尔曼滤波(Unscented Kalman Filter, UKF)与前视声呐(Forward-Looking Sonar, FLS)图像信息的水下运动目标被动跟踪(passive tracking)方法。由于水下环境复杂、声呐量测存在噪声且仅有方位(bearing-only)信息可用，传统UKF在目标剧烈运动或初值误差较大时易出现发散或精度下降问题。本文通过引入渐消记忆因子(forgetting factor)与修正的缩放比例参数(scaling parameter)对标准UKF进行改进，将其应用于由前视声呐获取的方位序列对水下运动目标进行状态估计。研究人员建立了包含目标运动学模型与声呐观测模型的状态空间表达式，定义预测向量与协方差矩阵，构造了文中所用的SA、Se、Sm等求和矩阵及增广行向量Y?pc(k+1)、ΔP?m(k)用于n步预测与残差更新。实验采用水池环境及半实物仿真验证所提算法对水下水平面匀速与匀加速运动目标的跟踪性能。结果表明，改进UKF相比扩展卡尔曼滤波(Extended Kalman Filter, EKF)和标准UKF在方位估计误差、位置均方根误差(Root Mean Square Error, RMSE)及收敛稳定性方面均有提升，可满足水下机器人(ROV/AUV)对运动目标被动跟踪的工程应用需求。

【论文解读】发表于《Electronics》

研究背景与意义

水下运动目标（如潜水员、蛙人、其他自主水下航行器AUV）的探测与跟踪是水下机器人（Remotely Operated Vehicle, ROV / Autonomous Underwater Vehicle, AUV）执行协同作业、避碰及安防监视任务的关键技术。主动声呐虽测距精度高但易暴露自身位置且受混响干扰大；被动声呐或前视声呐(Forward-Looking Sonar, FLS)仅能提供目标的方位(bearing-only / angle-only)信息，属典型的方位仅跟踪(Bearing-Only Target Tracking, BOTTT)问题，状态不可观需载体机动方可求解。传统扩展卡尔曼滤波(Extended Kalman Filter, EKF)需计算非线性函数雅可比矩阵，线性化误差大；标准无迹卡尔曼滤波(Unscented Kalman Filter, UKF)通过无迹变换(Unscented Transform, UT)选取Sigma点逼近状态分布，精度优于EKF，但在水下高噪声、目标机动或初始误差大时仍可能滤波发散或收敛缓慢。针对上述问题，研究人员开展了基于改进UKF并利用FLS方位量测序列的水下运动目标被动跟踪方法研究，并验证其有效性，对提升水下机器人非接触式隐蔽目标跟踪能力具有重要意义。

关键技术方法

研究人员建立二维平面目标匀速(Constant Velocity, CV)或匀加速(Constant Acceleration, CA)运动模型作为状态方程，以前视声呐(Forward-Looking Sonar, FLS)量测的目标方位角(相对载体坐标系)为观测方程（仅含方位无距离）。采用n步增广状态向量拼接当前状态与过去n?1步输入/过程噪声，构造增广Sigma点；设计S_A= [A, Σ²_i=1Aⁱ, …, Σⁿ_i=1Aⁱ]_1×n（状态转移矩阵幂次求和行向量）、S_e= [B_e, Σ²_i=1A^i?1B_e, …, Σⁿ_i=1A^i?1B_e]_1×n（控制量作用求和行向量）、S_m（n×n阶下三角Toeplitz型过程噪声协方差加权块矩阵，对角块为B_m，第j行第i?j+1列为Σ^n?(i?j+1)+1_l=1A^l?1B_m），以及预测观测行向量?_pc(k+1) = ?(k)[?(k+1), ?(k+2), …, ?(k+n)]^T和残差协方差中使用的ΔP?_m(k), ΔP?_m(k+n?1)。在标准UKF框架基础上引入渐消记忆因子(forgetting factor)抑制旧量测权重，并自适应调整UT中的缩放比例参数κ与复合尺度参数λ=α2(n_a+κ)?n_a以提高对目标机动的鲁棒性。实验在水池环境利用ROV搭载FLS采集真实数据，并结合Monte Carlo半实物仿真对比EKF、标准UKF与改进UKF的性能，评价指标含方位角估计误差、位置均方根误差(Root Mean Square Error, RMSE)及轨迹收敛性。

研究结果

2.1. 状态空间模型与增广UKF (State-Space Model and Augmented UKF)

研究人员推导了将n步历史过程噪声纳入增广状态的UKF形式，给出状态一步预测X?(k+1|k)=A X?(k)+B U(k)与观测预测方程，明确了增广均值向量x?_a=[x?^T, 0, 0]^T及增广协方差阵P_a=blkdiag(P, Q, R)，通过UT生成2n_a+1个Sigma点χ_a⁽ⁱ⁾，经非线性观测函数h(·)传播得到预测观测均值?与自协方差P_zz、互协方差P_xz，最终完成Kalman增益K=P_xzP_zz^?1及状态更新。结论：所建增广形式可统一处理多步预测与FLS方位量测下的状态估计框架。

2.2. UKF改进策略 (Improved UKF Strategy)

研究人员提出在量测更新协方差中引入渐消记忆加权矩阵S(k)=β·I（β∈(0,1)为渐消因子），替代标准UKF中固定量测噪声R，使滤波器更关注近期声呐量测、削弱历史异常噪声影响；同时根据预测残差模值自适应调小UT参数α（默认10^?3~1）以压缩Sigma点散布范围，或调整κ保证半正定性。结论：改进策略可有效抑制水下高噪环境下标准UKF的滤波发散，加快对突变机动目标的重新收敛。

3. 实验设置 (Experimental Setup)

水池实验以ROV为载体搭载Mechanical Scanning Imaging Sonar (MSIS型FLS等效)，对模拟运动目标（拖曳反射体做直线/变向运动）采集方位序列；仿真中设定目标初始位置/速度随机扰动，对比三种算法在相同量测噪声(σ_θ)与采样周期(T_s)下100次Monte Carlo运行结果。结论：实验与仿真参数设置合理，可反映算法实际性能差异。

4. 结果与讨论 (Results and Discussion)

(1) 方位估计：改进UKF输出的目标方位角序列与真实方位吻合度最高，稳态误差小于标准UKF约30%~40%；(2) 位置RMSE：改进UKF在目标匀速段RMSE较EKF降低约45%，较标准UKF降低约20%；在目标突然加速/转向后，改进UKF重新收敛所需帧数少于标准UKF；(3) 稳定性：引入渐消因子后，在高仿真噪声(σ_θ=3°)下标准UKF出现数次协方差非正定告警导致发散，而改进UKF全程稳定。结论：所提改进UKF结合FLS方位量测能实现水下运动目标的有效被动跟踪，精度与鲁棒性均优于对照方法。

讨论与结论翻译总结

研究人员指出，标准UKF在方位仅跟踪问题中因声呐量测噪声大、信息不完全（无距离）导致可观性弱，易产生估计偏差甚至滤波崩溃；通过渐消记忆加权与自适应UT参数修正可显著增强对水下环境的适应性。该方法仅需FLS提供的方位角时间序列，无需目标配合或主动测距，适合ROV/AUV平台嵌入式部署。未来工作可考虑融合载体自身惯导(Inertial Navigation System, INS)位姿及多波束声呐点云以提升三维空间跟踪能力。最终研究结论为：本文提出的改进无迹卡尔曼滤波算法结合前视声呐方位量测，能有效完成水下运动目标被动跟踪任务，在估计精度、收敛速度与抗噪稳定性上均优于扩展卡尔曼滤波与标准无迹卡尔曼滤波，适用于水下机器人隐蔽监视与协同作业场景。