[发明专利]二维激光雷达SLAM中探测环境发生突变的检测方法

权利要求书

1.二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，包括以下步骤：

(1)采集惯性传感器数据和激光雷达数据；

(2)根据惯性传感器数据递推载体的二维位置变化量；

(3)利用激光雷达数据进行匹配求解载体的二维位置变化量；

(4)对比步骤(2)惯性递推的二维位置变化量与步骤(3)匹配求解的二维位置变化量，若二者差值的绝对值大于预设阈值，则认为所探测的环境发生突变。

2.根据权利要求1所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，二维激光雷达SLAM中全局坐标系的建立方法为，以初始时刻载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别与初始时刻载体的右向和前向重合，Z轴为天向；航向角为载体的前向与全局坐标系的Y轴方向的夹角，以顺时针方向为正。

3.根据权利要求2所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，所述惯性传感器数据包括陀螺仪数据和加速度计数据，陀螺仪和加速度计固定在载体上，且其安装与载体坐标系重合。

4.根据权利要求3所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，步骤(2)的过程如下：

上式中，当前时刻记为k时刻，为k时刻载体的天向角速度，和为k时刻载体的右向和前向的加速度，和依次为从机器人组合导航系统中得到的k-1时刻载体在全局坐标系下X轴、Y轴方向的速度和航向角，和依次为k时刻载体在全局坐标系下X轴、Y轴方向的速度和航向角，Δt为从k-1时刻到k时刻的时间，和分别为k时刻载体在Δt内全局坐标系下X轴和Y轴方向的位置变化量，为递推出的载体从k-1时刻到k时刻的位置变化量。

5.根据权利要求1-4中任意一项所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，在步骤(3)中，设S_k和S_k-1分别为k时刻和k-1时刻激光雷达数据，根据ICP算法，先通过kd-tree进行最邻近点搜索，搜索S_k中每个激光点在S_k-1中的最近激光点，以此方式将S_k和S_k-1的激光点进行配对，然后利用奇异值分解法求解载体位置变化量。