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

说明书

本发明提供一种基于位姿约束的激光雷达SLAM退化检测方法及系统，包括：通过激光雷达传感器采集连续帧点云；对所述连续帧点云进行分析，获取机器人位姿抗扰动鲁棒性的点云约束关系；根据所述点云约束关系，进行机器人位姿约束扰动识别，获得SLAM退化方向。本发明能解决激光雷达SLAM在退化场景下失效难以检测的问题，为激光SLAM的稳定运行提供预警，能实现较高的检测准确性，有利于实际应用。

技术领域

本发明涉及计算机视觉领域，具体地，涉及一种基于位姿约束的激光雷达SLAM退化检测方法及系统。

背景技术

激光雷达是目前一类应用广泛的传感器：发射模块基于光学原理，利用脉冲、调频、调幅等方式发射特定波长的激光；接收模块利用二极管元件对发射后反射回来的激光进行检测，通过光线飞行时间可以得到精确测距。激光雷达广泛应用于目标检测、场景建模、机器人导航等应用中，有较为广泛的商业与学术前景。

同步建图与定位算法(SLAM)是利用传感器提供的信息，建立所在环境场景的地图，并同时确定传感器在地图中的对应位置。目前SLAM技术广泛应用于机器人自主定位、导航任务中。而其中基于激光雷达的SLAM算法，因其高精度的测距信息与鲁棒的定位结果，广泛应用于无人物流、无人小巴等高难度定位导航任务中。

激光雷达SLAM算法的应用难题之一在于，面对特定场景的算法失效问题。这些场景包括：隧道、长廊、室外空旷停车场、高速公路等等，其特征在于场景中的结构性纹理较少，发生场景退化，定位时激光雷达提供的数据约束不足从而使算法失效。退化问题可以通过多传感器数据融合、状态空间建模补偿等方法处理，而如何对激光雷达SLAM的退化场景进行识别检测，成为算法优化的关键步骤。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于位姿约束的激光雷达SLAM退化检测方法及系统。

根据本发明的一个方面，提供一种基于位姿约束的激光雷达SLAM退化检测方法，包括：

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

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

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

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

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

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

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

优选地，所述根据所述点云约束关系，进行机器人位姿约束扰动识别，包括：

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

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

优选地，若机器人位姿受到扰动，而激光测量距离变化较大，那么当前描述机器人位姿约束较强；反之，若机器人位姿受到扰动，激光测量距离变化不大，那么当前描述机器人约束极弱，系统处于退化环境。

优选地，所述获得退化方向，包括：