[发明专利]基于位姿约束的激光雷达SLAM退化检测方法及系统

权利要求书

1.一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，包括：

通过激光雷达传感器采集连续帧点云；

对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；

根据所述点云约束关系，进行机器人位姿约束扰动识别，获得SLAM退化方向。

2.根据权利要求1所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，所述对连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系，包括：

将所述连续帧点云的局部表面近似为平面，每个测量点满足下列关系式：

其中，是雷达坐标系下激光光束的单位向量，用于指示光束方向，对应其光束长度，i∈{1,2,...,m}是激光点束的索引，是近似平面的法向量，p_i,0是近似平面上的一个点，分别描述了机器人的位置和姿态，R是旋转矩阵，t是平移距离；

将记为d_i，则获得机器人位姿抗扰动鲁棒性的点云约束关系为：

3.根据权利要求1所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，所述根据点云约束关系，进行机器人位姿约束扰动识别，包括：

在点云配准过程中，将机器人的位置和姿态(R,t)的变动视为扰动；

通过计算激光测量距离ρ_i对(R,t)扰动的敏感程度，判断激光雷达SLAM是否发生场景退化。

4.根据权利要求3所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，若机器人位姿受到扰动，而激光测量距离变化较大，那么当前描述机器人位姿约束较强；反之，若机器人位姿受到扰动，激光测量距离变化不大，那么当前描述机器人约束极弱，系统处于退化环境。

5.根据权利要求1所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，所述获得退化方向，包括：

基于机器人位姿中小角度θ变换的假设，使用R≈I+[θ]_×对求导问题线性化：

基于隐函数定理，建立等式：

推导得公式，

根据隐函数定理获得的所述等式和所述公式，构建F矩阵与T矩阵，分别表示激光约束对平移参数的敏感度与对旋转参数的敏感度：

对所述F矩阵和T矩阵进行特征值分解，获得一组特征向量，

其最小特征值对应的特征向量即为当前的退化方向：所述F矩阵的最小特征值用于判断平移量的退化方向；所述T矩阵的最小特征值用于判断旋转的退化方向。

6.根据权利要求5所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，矩阵在任一特征向量的投影，是特征向量自身固定的伸缩，伸缩比例是特征值，特征值反映矩阵在该特征向量方向的投影的比例大小，特征值越小，说明矩阵在该特征向量方向上的约束越小，约束小，说明在该方向上容易发生退化。

7.根据权利要求6所述的一种基于位姿约束的激光雷达SLAM退化检测方法，其特征在于，若λ_min小于所设定的阈值，代表当前场景发生退化；同时，最小特征值λ_min对应的特征向量即为当前的退化方向。

8.一种基于位姿约束的激光雷达SLAM退化检测系统，其特征在于，包括：

数据模块，该模块通过激光雷达传感器采集连续帧点云；

约束关系模块，该模块对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；

扰动识别模块，该模块根据所述点云约束关系，进行机器人位姿约束扰动识别。

9.一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时可用于执行权利要求1-7中任一项所述的方法，或，运行权利要求8所述的系统。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时可用于执行权利要求1-7中任一项所述的方法，或，运行权利要求8所述的系统。