一种基于特征和体素固定滞后平滑的激光SLAM方法

本发明属于自动驾驶的导航与定位领域，具体涉及一种基于特征和体素固定滞后平滑的激光slam方法。

背景技术：

1、持续可靠的定位和建图是自动驾驶车辆完成感知、决策、控制和协作任务的必要前提，为了在复杂场景中完成这些任务，自动驾驶汽车的定位和建图通常依赖于各种传感器，如gps/imu，激光雷达和摄像头。激光雷达作为一种不受环境光照影响的传感器，凭借精度的距离感知、大视角的特点，激光雷达slam能够在室外场景中实现准确的6自由度位姿估计。另一方面，激光slam在各种自动驾驶任务中也发挥着至关重要的作用，如物体的检测与跟踪，点云地图的构建和维护。因此，激光slam为自动驾驶平台寻求精确定位和地图构建提供了可行的解决方案。

2、在一些场景中，传统的基于曲率的特征提取方式受激光雷达视场的影响，导致特征点的提取不准确，将导致激光里程计前端位姿估计精度不高以及构建的点云地图模糊。因此，传统的基于曲率的特征提取面临着严峻的挑战，现有的特征提取解决方案大多采用主成分分析进行特征提取，虽然这类方法不受视场的影响，然而十分耗时，运算效率低。为了兼顾特征提取的精度与速度，迫切需要一种既能考虑激光雷达视点影响又能准确高效提取特征点的激光slam方法。此外，一些局部优化方案尽管能够优化前端里程计的位姿，但是大多数优化方案未能考虑局部范围内的位姿，当前帧点云发生退化时，将导致后端优化失效。因此，一种能够联合多帧细化前端位姿的局部优化方案是必要的。

技术实现思路

1、为解决上述问题，本发明公开了一种基于特征和体素固定滞后平滑的激光slam方法，能有效地提高退化场景下激光slam的特征提取和位姿估计的精度和速度。

2、为达到上述目的，本发明的技术方案如下：

3、一种基于特征和体素固定滞后平滑的激光slam方法，包括如下步骤：

4、s1：根据激光雷达点云等角测量特性利用相邻点夹角进行特征提??；

5、s2：采用点到直线和点到平面的配准方式进行帧到地图配准；

6、s3：根据帧间相对距离和角度提取关键??；

7、s4：将关键帧特征点云进行体素特征提取，划分为边体素特征和面体素特征。在固定滑动窗口内执行边体素和面体素特征匹配，联合多关键帧位姿执行固定滞后平滑求解最优的关键帧位姿；

8、s5：将关键帧位姿和回环检测传入位姿图优化?？?，当关键帧位姿与历史关键帧位姿满足距离和角度约束时，在位姿图优化中加入关键帧位姿与历史关键帧位姿匹配后的位姿约束进行全局优化，构建全局地图。

9、优选的，所述步骤s1具体包括以下过程：

10、根据激光雷达点云相邻间的夹角计算曲率，并作为边和面特征提取的准则，基于角度的曲率ci计算公式为：

11、

12、

13、其中，pi、pi-n、pi+n分别表示第i、i-n、i+n个点云，θ表示同一条线束下相邻点云之间的夹角，一般来说，旋转式激光雷达具有固定的水平角分辨率，n值表示计算相邻点云夹角所用的单边周围点云数量。为了减少计算消耗，本发明为每个点在顺时针和逆时针方向选择了n＝2个点。

14、本发明对扫描中的所有点进行迭代，得到基于角度计算的曲率大小排序，较大的曲率值表示边缘特征较小的曲率值表示平面特征

15、优选的，所述步骤s2具体包括以下过程：

16、s2.1点云地图特征管理，具体包括：首帧点云位姿作为全局坐标系基准，并将首帧点云的和直接存储在边和面特征子地图和中：

17、

18、

19、其中，和是世界坐标系下的单帧点云地图。通过直通滤波，更新当前地图内的边特征和面特征，并对特征进行降采样，生成待匹配的特征子地图。

20、s2.2对于第n(n≥2)帧点云，体素滤波降采样当前帧点云，并将特征与待匹配的特征子地图通过kd-tree关联，构建点到线和点到面的几何残差匹配关系。利用ceres优化求解几何残差构建的最小二乘问题，获取当前帧点云到全局坐标系的位姿估计。

21、为角点点云，则通过点到直线的配准方式对帧到地图进行配准，点到边特征的几何距离残差表示为：

22、

23、其中，表示激光雷达坐标系下第i个角点，表示激光雷达坐标系到全局坐标系的位姿变换，初值为单位矩阵，表示激光雷达坐标系下第i个角点在全局特征子地图中找到的两个距离最近点云；

24、为面点点云，则通过点到面的配准方式对帧到地图进行配准，点到面的几何距离残差表示为：

25、

26、其中，表示激光雷达坐标系下第i个面点，表示激光雷达坐标系到全局坐标系的变换矩阵，初值为单位矩阵，表示世界坐标系下与相距最近的点云，为其表面的法向量；

27、s2.3优化问题的代价函数为：

28、

29、采用高斯-牛顿迭代法求解非线性优化问题，得到最优位姿变换最后，利用该变换将当前帧的特征点转换为地图并更新地图数据。

30、优选的，所述步骤s3具体包括以下过程：

31、根据相邻帧间的位姿变换选取激光雷达的关键帧的准则为：

32、

33、其中，(δt)t、(δt)r、(δt)p、(δt)y分别表示位姿变换的平移增量、横滚角增量、俯仰角增量和偏航角增量，当四种增量任一种大于阈值时，当前帧将被选为关键帧。

34、优选的，所述步骤s4具体包括以下过程：

35、将关键帧特征点云进行体素特征提取，划分为边体素特征和面体素特征。在固定滑动窗口内执行边体素和面体素特征匹配，联合多关键帧位姿执行固定滞后平滑求解最优的关键帧位姿；

36、s4.1体素化当前帧上的边特征和面特征，将点云转化为更高级别的体素特征表示。体素v的属性可表示为：

37、

38、

39、

40、

41、

42、其中，表示体素内点云的数量；σi表示特征值λi的协方差，表示λi对应的特征向量。根据特征值的分布可区分体素的类别，如果最大特征值大于最小特征值的两倍，则体素v将被视为边体素特征；如果存在两个大特征值一个小特征值，则体素v将被视为面体素特征。在体素特征提取后，本发明将获得当前帧和地图的边体素特征集合和以及面体素集合和

43、

44、

45、其中，s、t表示当前帧和地图上边体素特征的数量，表示当前帧的第k个边体素特征，表示地图上的第j个边体素特征。p、q表示当前帧和地图上面体素特征的数量，表示当前帧的第k个面体素特征，表示地图上的第j个面体素特征。

46、s4.2根据体素地图上的边体素和面体素特征中心构建kd-tree，构建关键帧到关键帧地图之间的边体素特征关联和面体素特征关联

47、

48、

49、其中，和表示边体素到边体素的残差和面体素到面体素的残差。表示的体素中心，表示在体素地图上距离最近的点云，表示的体素中心，表示在体素地图上距离最近的体素。和根据kd-tree搜索获取。

50、s4.3为了保证体素匹配之间的正确紧密关联，边体素特征关联和面体素特征关联受以下约束：

51、

52、

53、其中，和是边体素和面体素之间的距离差，关联距离差和矢量差值。dp,df,dr为距离差，关联距离差和矢量差值的阈值。

54、s4.4对边体素和面体素特征匹配残差进行联合优化，通过求解最小化以下非线性最小二乘问题得到细化后的关键帧姿态

55、

56、s4.5采用固定滞后平滑方法在固定时间窗口内进行批量细化，只保留最近关键帧姿态的子集，同时边缘化旧姿态，对于一组历史姿态：

57、x1:k＝{t1,t2,…,tk}

58、固定滞后平滑可以表示为最小二乘最小化问题:

59、

60、其中，ep代表了边缘化因子，and代表边体素残差因子和面体素残差因子的协方差。在求解最小二乘问题后，为了保持固定的窗口大小，需要进行边缘化，将不再需要的关键帧及其对应的特征点从优化问题中移除，以降低计算复杂度并保持固定的窗口大小。

61、优选的，所述步骤s5具体包括以下过程：

62、判断当前关键帧位姿与历史关键帧位姿是否满足回环关系，当关键帧位姿与历史关键帧位姿满足距离和角度约束时，采用gicp求解当前关键帧与历史回环关键帧的位姿变换，将所有关键帧位姿和所有回环检测的位姿变换传入位姿图优化?？?，在位姿图优化中进行全局优化，构建全局地图。

63、本发明的有益效果是：

64、首先，本发明采用基于角度的特征提取策略，实现了边特征和面特征的准确提取，相比于传统的曲率特征提取，本发明不受激光雷达视点的影响，能够准确地提取特征点；此外，相比于基于主成分分析的特征提取方法，尽管这类方法也不受激光雷达视点的影响，但是十分耗时，而本发明基于角度的特征提取算法效率与基于曲率的特征提取算法相当，能够有效地提取特征点。

65、其次，本发明采用基于固定滞后平滑的局部优化算法进行后端优化，相比与传统的帧到地图的匹配和帧到关键帧的匹配，本发明提出的方法能够联合其他时刻的位姿，对当前帧位姿起到平滑作用，当前帧发生退化时，传统的帧到地图的匹配和帧到关键帧的匹配会发生失效情况，而本发明提出的固定滞后平滑方法能够联合其他时刻的位姿，即使当前帧退化，依靠相邻关键帧的位姿依然能够对当前帧进行较好的预测。