基于强跟踪的自适应PHD-SLAM算法

Adaptive PHD-SLAM Algorithm Based on Strong Tracking

同时定位与建图(Simultaneous Localization and Mapping,SLAM)技术使移动机器人在缺乏先验环境信息的条件下,能够在估计自身位姿的同时构建环境地图。然而,在海洋、矿洞等复杂环境中,移动机器人容易受到随机突变噪声的干扰,进而导致SLAM性能下降。现有的概率假设密度(Probability Hypothesis Density,PHD)SLAM算法未考虑随机突变噪声,受到干扰时在线自适应调整能力较弱。为解决移动机器人因随机突变噪声导致状态估计和建图精度降低的问题,本文结合强跟踪滤波器(Strong Tracking Filter,STF)与PHD滤波器,提出了一种基于强跟踪的自适应PHD-SLAM滤波算法(Strong Tracking Probability Hypothesis Density Simultaneous Localization and Mapping,STPHD-SLAM)。该算法以PHD-SLAM为框架,针对过程噪声协方差和量测噪声协方差随机突变问题,本文通过在特征预测协方差中引入STF中的渐消因子,实现了对特征预测的自适应修正和卡尔曼增益的动态调整,从而增强了算法的自适应能力。其中渐消因子根据量测新息递归更新,确保噪声突变时每个时刻的量测新息保持正交,从而充分利用量测信息,准确并且快速地跟踪突变噪声。针对渐消因子激增导致的滤波器发散问题,本文对渐消因子进行边界约束,提高算法的鲁棒性。仿真结果表明,在量测噪声协方差和过程噪声协方差随机突变的情况下,所提算法相较于PHD-SLAM 1.0和PHD-SLAM 2.0的定位和建图精度都得到了提高,同时保证了计算效率。

Simultaneous localization and mapping technology enables mobile robots to estimate their positions while constructing an environmental map in the absence of prior environmental information.This crucial capability has broad applications,including autonomous navigation,search and rescue operations,and exploration tasks.However,in complex environments,such as oceans,mines,and other challenging terrains,mobile robots are susceptible to interference from random,abrupt noise.This interference,in turn,resulted in a significant decline in SLAM performance as the robots struggled to estimate their positions and map the environment accurately.The probability hypothesis density SLAM algorithm did not adequately account for random abrupt noise,which led to weaker online adaptive adjustment capabilities when disturbed.This limitation hindered the ability of the robot to adapt to sudden changes in the environment,reducing the overall effectiveness of the SLAM process.This study proposed a novel strong tracking probability hypothesis density simultaneous localization and mapping filtering algorithm to address the critical issue of reduced state estimation and mapping accuracy in mobile robots due to random,abrupt noise.This innovative approach integrated the strong tracking filter with the PHD filter,leveraging the strengths of both methods.Built on the PHD-SLAM framework,the proposed algorithm specifically addresses the issue of random,abrupt changes in process noise covariance and measurement noise covariance.These abrupt changes could arise from various sources,including sensor malfunctions,sudden environmental changes,and unexpected dynamic obstacles.The study introduced the fading factor from the strong tracking filter into the feature prediction covariance,achieved adaptive correction of feature predictions,and dynamically adjusted the Kalman gains.This enhancement significantly improved the adaptability of the algorithm to changing conditions,enabling it to maintain accurate localization and mapping performance even in the presence of random,abrupt noise.The fading factor was recursively updated based on the measurement innovation.This approach ensured that the measurement innovation at each time step remained orthogonal when noise mutations occurred,thereby fully utilizing measurement information.Consequently,the algorithm could accurately and rapidly track abrupt noise changes,enhancing its robustness and reliability.Additionally,in response to the filter divergence issue caused by the sharp increase in the fading factor,this study imposed boundary constraints on the fading factor.These constraints were designed to prevent excessive values,which could otherwise lead to filter instability and divergence.Simulation results indicated that,in the scenario where random fluctuations occurred in both measurement noise covariance and process noise covariance,the proposed algorithm enhanced localization accuracy and significantly improved mapping precision when compared to the traditional PHD-SLAM 1.0 and the more advanced PHD-SLAM 2.0 algorithms.Additionally,this improvement in accuracy did not come at the expense of computational efficiency;the proposed algorithm maintained high levels of computational efficiency,ensuring it remained practical for real-time applications.These results demonstrated the robustness and effectiveness of the proposed algorithm in handling unpredictable noise variations while delivering superior performance in both localization and mapping tasks.