一种基于滑动窗口统计的自适应激光雷达SLAM退化检测方法

本发明属于移动机器人自主导航，具体涉及一种适用于激光雷达slam系统基于滑动窗口统计的自适应雷达退化检测方法。

背景技术：

1、激光雷达能够直接获取环境中的高精度三维几何结构信息，且对光照变化具有很强的鲁棒性，已经成为slam系统中重要的感知传感器之一。因此，以激光雷达作为主要传感器的同步定位与建图(lidar slam)技术已广泛应用于自动驾驶汽车和移动机器人等领域。但在长走廊、平面区域等场景，由于点云特征稀疏或者重复，容易导致雷达退化，这给slam算法的稳定性与可靠性构成了严峻的挑战。因此，在雷达slam运行过程中，及时检测到雷达退化状态可以让系统更好地在位姿估计过程中采取有效措施处理退化状态的点云，从而减轻雷达退化对定位和建图的不利影响。

2、当前的雷达退化检测方法主要依赖人工预设固定阈值的方式，当退化指标低于或超出对应的阈值时，则认为发生雷达退化。文献1(zhang j，kaess m，singh s.ondegeneracy of optimization-based state estimation problems[c]//2016 ieeeinternational conference on robotics and automation(icra).ieee，2016：809-816.)推导并证明了优化过程中海森矩阵的特征值可以决定退化。因此，作者基于特征值定义退化因子，并且通过实验确定退化阈值。当系统运行过程中，若某一帧点云的特征值小于阈值，则认为该帧点云退化。然而，阈值需要根据传感器类型和环境结构等因素通过实验反复调整。当应用场景发生变化时，固定阈值方法会限制退化检测方法的通用性和可扩展性。

3、此外，文献2(liao z，zhang x，zhang t，et al.a real-time degeneracysensing and compensation method for enhanced lidar slam[j].ieee transactionson intelligent transportation systems，2025.)通过条件数定义退化因子，提出一种基于dbscan聚类的检测方法，使得退化因子聚类为离群点和非离群点，将离群点视为退化点云帧来自动感知退化。然而，该方法只适用于检测突然的退化。在激光雷达长期退化时，发生退化时的退化因子不是离群点而是新的聚类，该方法不再适用。

4、针对现有退化检测方法的不足，本发明提出了一种基于滑动窗口统计的自适应雷达退化检测方法，通过滑动窗口机制与统计学方法，实现了对雷达自适应且细粒度的退化检测，从而摆脱了固定阈值的局限性。

技术实现思路

1、针对现有技术在长走廊、平面区域等场景下激光雷达退化检测方法依赖固定阈值，导致其通用性不足的问题，本发明提供了一种基于滑动窗口统计的自适应激光雷达slam退化检测方法。

2、为实现上述发明目的，本发明公开了下述技术方案：

3、一种基于滑动窗口统计的自适应激光雷达slam退化检测方法，包括如下步骤：

4、步骤1：激光雷达slam系统中当前帧点云与局部地图之间进行点云配准，从而获得点云配准残差，基于点云配准残差建立非线性最小二乘优化模型以求解位姿，并在优化求解线性化过程中构建海森矩阵。

5、步骤2：将海森矩阵进行分块处理，提取旋转海森子矩阵和平移海森子矩阵用于退化分析。旋转海森子矩阵用于分析姿态方向上的几何约束能力，平移海森子矩阵用于分析位置方向上的几何约束能力。

6、步骤3：分别对旋转海森子矩阵和平移海森子矩阵进行特征值分解，基于最大特征值与最小特征值的比值分别定义每帧点云旋转退化因子和平移退化因子。退化因子越大，说明最小特征值对应自由度方向上的约束信息越不均衡，激光雷达在该方向上的退化程度越高。

7、步骤4：分别基于连续帧点云计算出的旋转退化因子和平移退化因子构建基准窗、缓冲窗和检测窗三个滑动窗口，用于估计退化因子均值样本的统计分布参数和退化状态检测。

8、步骤5：根据检测窗均值样本相对于基准窗统计分布参数的偏离程度，对旋转自由度和平移自由度分别执行自适应退化状态判定，判定结果分为未退化、部分退化和退化三种类别并输出旋转和平移自由度对应的退化状态。

9、步骤6：根据退化状态判定结果选择性更新统计分布参数。当检测结果为未退化时，基准窗、缓冲窗和检测窗同步滑动，并结合遗忘因子更新统计分布参数；当检测结果为部分退化或退化时，不使用当前异常样本更新基准窗统计分布参数，从而避免退化样本污染正常状态基准。

10、与现有技术相比，本发明取得了以下突出的有益效果：

11、(1)本发明将点云配准优化过程中的海森矩阵按照旋转和平移自由度进行分块，并分别基于旋转海森子矩阵和平移海森子矩阵计算退化因子，能够独立量化激光雷达在姿态方向和位置方向上的退化程度。

12、(2)本发明采用基于基准窗、缓冲窗和检测窗的滑动窗口统计结构，根据历史未退化状态下的统计分布参数自适应生成检测标准，避免了传统固定阈值方法依赖人工经验调参的问题。

13、(3)本发明通过未退化、部分退化和退化三级判定机制对退化程度进行细粒度描述，相比二值退化检测方式能够更加平滑地刻画激光雷达约束能力由正常到退化的变化过程。

14、(4)本发明通过缓冲窗和遗忘因子更新机制使得统计分布参数能够随环境变化缓慢自适应更新，从而提升长期运行条件下退化检测的稳定性和鲁棒性。

1.一种基于滑动窗口统计的自适应激光雷达slam退化检测方法，其特征在于：包括如下步骤：

2.根据权利要求1所述的方法，其特征在于：步骤1中，所述点云配准残差构建非线性最小二乘优化模型为式中，fi(xk)为第i个特征点的配准残差，xk为求解的位姿。

3.根据权利要求1所述的方法，其特征在于：步骤1中，所述非线性最小二乘优化模型线性化为fi(xk+δxk)≈fi(xk)+ji(xk)δxk，所述雅可比计算方式为所述近似二阶海森矩阵计算方式为

4.根据权利要求1所述的方法，其特征在于：步骤2中，所述海森矩阵分快处理过程为所述仅包含旋转和平移信息的海森子矩阵分别为hrr和htt。

5.根据权利要求1所述的方法，其特征在于：步骤3中，所述海森子矩阵特征值分解为与式中∑r和σt分别表示仅包含旋转信息和平移信息的特征值矩阵，并且特征值满足λr1＞λr2＞λr3，λt1＞λt2＞λt3。

6.根据权利要求1所述的方法，其特征在于：步骤3中，所述基于条件数分别定义雷达旋转和平移两个自由度方向的退化因子分别为和

7.根据权利要求1所述的方法，其特征在于：步骤4中，所述均值样本μi计算过程为式中，dk表示旋转或平移方向的退化因子，m为滑动窗口长度。

8.根据权利要求1所述的方法，其特征在于：步骤4中，根据中心极限定理，原始退化因子的均值样本分布可近似服从正态分布；根据正态分布性质，样本落入1σs区间的概率为68.62％，落入2σs区间的概率为95.44％。因此，以σs和2σs作为阈值实现退化状态的自适应分级判定。

9.根据权利要求1所述的方法，其特征在于：步骤5中，所述退化状态自适应判定逻辑为，所述检测窗计算出包含最新退化因子dk的均值μi；根据μi与样本分布参数μs的偏离程度，自适应判定退化状态类别：若|μi-μs|落入2σs之外，则检测窗内最新的退化因子dk发生“退化”；若|μi-μs|落入σs与2σs之间，则检测窗内最新的退化因子dk发生“部分退化”；若|μi-μs|落入σs以内，则检测窗内最新的退化因子dk“未退化”。

10.根据权利要求1所述的方法，其特征在于：步骤6中，所述带有遗忘因子的统计分布参数更新方式为μs(k)＝λsμs(k-1)+(1-λs)μk-2m+2和式中，λs∈[0.8，1]为遗忘因子；当检测窗口均值样本落在|μi-μs|≤σs区间时，进行均值样本统计分布参数的更新。