一种应用于动态环境的多传感器融合SLAM方法及系统

本发明涉及一种应用于动态环境的多传感器融合slam方法及系统，属于自主导航领域。

背景技术：

1、slam (simultaneous localization and mapping，同步定位与地图构建) 是一种在未知环境中，实时构建地图并确定自身位置的技术。它是自主移动机器人、自动驾驶和增强现实(ar)等领域的关键技术之一。slam 的核心任务是机器人或传感器系统在未知环境中移动时，同时定位自身的位置并构建环境地图。这意味着系统需要不断地收集传感器数据，如激光雷达、相机、惯性传感器(imu)、轮速计，进而使用这些数据来估计机器人的位置，并创建一个地图，记录环境的特征和结构。

2、传统的单传感器slam系统中，使用视觉或激光信息在静态环境下进行位姿估计以及建图。视觉传感器的成本较低，同时能够采集纹理颜色等信息，其在低速运动以及静态环境下表现良好。但纯视觉slam系统对光照和纹理条件敏感，且在高速运动下，图片容易出现模糊、丢帧等情况，导致视觉slam前端跟踪失效。同时，纯视觉slam在处理环境中的动态物体时难度较大，快速运动的物体可能导致特征点的错误匹配，进而影响定位和建图的准确性。而激光雷达主要提供距离信息，具有高精度、测量范围大等优势。但其对于环境纹理和颜色信息的感知能力有限，在重复结构化场景中(如隧道、长走廊)，无法进行点云特征匹配。同时，激光雷达难以有效处理动态物体，尤其是快速移动的物体，动态物体的存在会导致定位误差大，也导致建图过程中有较多杂点和动态物体的拖影。综上，目前单传感器slam系统难以在复杂动态环境中发挥优势，传感器融合成为目前slam系统的主流趋势。

技术实现思路

1、本发明提供一种应用于动态环境的多传感器融合slam方法及系统，旨在至少解决现有技术中存在的技术问题之一。

2、本发明的技术方案涉及应用于动态环境的多传感器融合slam方法，应用于slam系统，所述slam系统设置有惯性传感器、轮速计、视觉相机和激光雷达，根据本发明的方法包括以下步骤：

3、s100、获取所述惯性传感器、所述轮速计、所述视觉相机和所述激光雷达的传感数据；

4、s200、对所述惯性传感器和所述轮速计的传感数据进行预积分后，通过松耦合位姿估计，采用几何方法检测动态物体并滤除动态点；

5、s300、对所述视觉相机的传感数据进行过语义分割，对检测结果进行聚类，以及根据重投影误差估计，以在位姿估计过程中滤除动态点，获得当前的最优位姿；

6、s400、从所述激光雷达的传感数据中提取静态点云，使用所述静态点云关联视觉特征深度；

7、s500、根据惯性传感器预积分残差、轮速计预积分残差、静态点重投影残差和边缘化残差，通过滑动窗口优化、回环检测以及位姿图优化，输出位姿图。

8、进一步，所述步骤s200中，所述预积分的公式表示如下：

9、

10、

11、

12、

13、式中，分别表示惯性传感器的加速度积分得到的位置预积分、速度预积分，表示惯性传感器的角速度积分得到的四元数预积分，表示轮速计速度积分得到的位置预积分；表示旋转矩阵，其上标表示参考坐标系，其下标表示目标坐标系，表示第k帧的机体坐标系到当前时间t机体系的变换；表示时间，其是积分量；上标表示第帧，上标k+1表示当前帧；表示第k帧机体坐标系；表示惯性传感器加速度偏置；表示惯性传感器角速度偏置；表示惯性传感器加速度测量值；表示惯性传感器角速度测量值；表示轮速计速度测量值。

14、进一步，所述步骤s200中，所述几何法滤除动态物体包括以下步骤：

15、s210、利用惯性传感器积分值与所述轮速计积分值作为两帧间粗略位姿估计值，将上一帧中的特征点通过帧间粗略位姿转换到当前帧下，得到上一帧特征点在当前帧坐标系下的三维坐标，以获得特征点的理论坐标；

16、s220、将从所述视觉数据的传感数据中提取到的当前帧中的特征点投影至当前帧相机坐标系下，得到特征点的测量坐标；

17、s230、求取特征点的理论坐标和测量坐标之间的距离，判断所述距离所否超过设定的距离阈值，若是则认为该特征点是动态特征点。

18、进一步，所述步骤s200中，

19、特征点的理论坐标表示如下：

20、

21、特征点的测量坐标表示如下：

22、

23、取特征点的理论坐标和测量坐标之间的距离表示如下：

24、

25、式中，下标c表示相机坐标系，下标w表示世界坐标系；上标k表示第k帧，上标k+1表示当前帧；p代表特征点的齐次坐标，t为4×4矩阵，表示坐标系之间的变换关系； 表示视觉相机投影模型，求逆则表示逆投影模型，将像素坐标投影至相机坐标系下；表示特征点的像素坐标，z表示特征点的深度。

26、进一步，所述步骤s300包括步骤：

27、s310、对所述视觉相机的传感数据进行过语义分割，以获得分割掩码内点和分割掩码外点；

28、s320、对所述分割掩码内点进行三角化处理，以获得先验静态点和先验动态点；

29、s320、根据所述分割掩码外点、惯性传感器预积分和轮速计预积分以及重投影误差，通过第一滑动窗口进行粗优化，估计出动态物体并滤除动态点，以及将误判为动态点的特征点恢复为静态点；

30、s330、根据步骤s320获得的两种静态点，以及根据惯性传感器预积分和轮速计预积分，通过第二滑动窗口进行第二次位姿估计，以共同优化得到当前的最优位姿。

31、进一步，所述步骤s320包括步骤：

32、s321、对所述分割掩码外点计算视觉测量误差，以及根据惯性传感器预积分和轮速计预积分，通过第一滑动窗口得到粗略位姿估计；

33、s322、根据所述粗略位姿估计计算所述先验静态点在第一滑动窗口所有重投影的误差之和；

34、s323、判断所述误差之和是否大于预设的误差阈值，若是则判定该特征点为动态点，以留下静态点；

35、s324、对所述第一滑动窗口的检测结果与所述先验动态点进行特征点一致性检测，以确定需要恢复的静态点。

36、进一步，所述步骤s300中，

37、静态特征点的重投影误差计算公式表示如下：

38、

39、用于求解最优状态变量的非线性优化方程表示如下：

40、

41、式中，表示特征点的像素坐标，表示相机内参矩阵，表示相机系到世界系的位姿变换，表示特征点在世界系下的三维坐标；第一部分残差为边缘化残差，第二部分残差为惯性传感器与轮速计预积分残差，第三部分残差为所有特征点的重投影误差；x表示状态向量；表示惯性传感器与轮速计的所有测量帧；表示边缘化残差；表示预积分残差的函数；表示视觉残差的计算函数；表示边缘化雅可比矩阵；表示惯性传感器与轮速计的测量值；表示第帧、第个特征点的测量值。

42、进一步，所述步骤s400包括步骤：

43、s410、根据语义分割后的所述视觉相机的传输数据，滤除所述激光雷达中的动态点云；

44、s420、将所述激光雷达的激光点云转换到相机坐标系下，构建局部点云地图，并进行降采样；

45、s430、将降采样后的局部点云地图中的点云投影至符合相机坐标系下的单位球体坐标系，以构建点云深度图；

46、s440、针对每个视觉特征点，在单位球坐标系下寻找距离其最近的三个激光点，判断上述三个激光点中任意两个点的距离大于预设的点云阈值；

47、s450、若是，则不关联此视觉特征的深度；若否，将该视觉特征点投影到三个激光点构成的平面上，得到的投影点即为该特征点在世界坐标系下的三维坐标。

48、本发明的技术方案还涉及计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施上述的方法。

49、本发明的技术方案还涉及应用于动态环境的多传感器融合slam系统，所述系统包括计算机系统，该计算机系统包含上述计算机可读存储介质。

50、本发明的有益效果如下：

51、通过融合具有不受动态物体干扰的惯性传感器(imu)以及轮速计，同时结合视觉感知算法，整合多个传感器的信息，提高实际复杂动态场景下，多传感器融合slam的定位精度，使机器人能够在不断变化的环境中进行定位和导航，提高系统对动态环境的适应性。

52、根据惯性传感器(imu)以及轮速计均具有高频特性，在短时间内的测量较为准确，且不受动态环境的影响，可以弥补视觉以及激光传感器的劣势，本发明建立融合了双目视觉、激光雷达、惯性传感器、轮速计的紧耦合slam系统，通过惯性传感器以及轮速计的短时间松耦合位姿估计，利用几何方法检测动态物体并滤除，以及应用yolov5语义分割，检测环境中可能存在的先验动态物体，对检测结果进行聚类，根据重投影误差估计出动态物体，在位姿估计过程中滤除。