一种多传感器融合的SLAM定位和重建方法及系统与流程

本发明涉及机器人智能感知，具体涉及一种多传感器融合的slam定位和重建方法及系统。

背景技术：

1、同时定位与建图（simultaneous localizationand mapping, slam），是指搭载特定传感器的主体在没有环境先验的情况下，在运动过程中估计自己的运动同时建立环境地图。自主导航系统中通过slam可以使机器人能够在未知的环境中移动和定位，同时还能够建立环境地图。在增强现实中slam能过将虚拟对象叠加在真实世界中，通过使用相机和其他传感器来跟踪用户的移动并在场景中添加虚拟对象；在救援和巡检任务中，slam可以为无人机提供导航和定位功能；总的来说，slam技术具有广泛的应用前景和重要的研究意义，可以为自主导航、增强现实、无人机和安防等领域提供有效的解决方案。

2、从传感器类型上来看，slam可以分为视觉slam和激光slam，其中，激光slam使用激光传感器获取环境三维点云数据，而视觉slam使用相机获取环境图像数据。相比之下，激光slam获得的数据更加精确，视觉slam对于环境的要求更高，需要较好的光照条件和对比度，所以激光传感器和视觉传感器相比，激光传感器在环境三维信息获取上具有绝对优势。

3、由于激光雷达获取到的是稠密三维空间点的坐标信息，因此，可以直接通过坐标点的投影实现slam场景重建功能。但是激光雷达还有如下缺陷：1）在雨天场景中，激光雷达测量到的激光反射点可能会被雨滴反射而产生虚假点云，从而影响后续的数据处理和场景重建，2）激光雷达无法感知透明或反光的物体，如镜子、玻璃等，这会对机器人的感知和导航造成一定的限制，3）激光雷达并不能捕获环境的色彩信息，使得重建出来的三维模型视觉观感不理想。

技术实现思路

1、鉴于现有技术中的上述缺陷或不足，期望提供一种多传感器融合的slam定位和重建方法及系统。

2、第一方面，本技术实施例提供了一种多传感器融合的slam定位和重建方法，该方法包括：

3、s1：对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；

4、s2：计算激光雷达两次扫描间imu的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；

5、s3：通过卡尔曼滤波器更新imu先验信息得到lio最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；

6、s4：通过卡尔曼滤波器融合gps与s3中得到的运动轨迹；

7、s5：对步骤s4进行回环检测；

8、s6：将后验状态叠加到激光点云上，为每一次激光扫描结合gps信息赋予绝对的位置信息后得到三维重建图；

9、s7：将三维重建图发送到终端。

10、在其中一个实施例中，所述步骤s1包括：通过enlightengan对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色。

11、在其中一个实施例中，所述步骤s2包括：

12、计算激光雷达两次扫描间imu的相对运动量；

13、通过imu消除激光雷达的固有畸变；

14、通过卡尔曼滤波器耦合激光雷达和imu信息，通过二者信息联合估计机器人姿态信息。

15、在其中一个实施例中，在步骤s3中，所述通过卡尔曼滤波器更新imu先验信息得到lio最终后验状态量，包括：

16、s31：构造先验协方差矩阵，，其中，表示激光点误差，是时刻的后验协方差矩阵；是imu噪声系数矩阵，是噪声方差矩阵；

17、s32：定义卡尔曼增益矩阵为，，其中，是观测噪声矩阵，是观测噪声系数矩阵，是用于约束待优化状态量并随之迭代的矩阵，右上角表示矩阵的逆运算，右上角表示矩阵的转置运算，所以表示观测噪声的逆矩阵，表示的转置，符号同理；

18、第k次迭代可以表示为如下公式：

19、；

20、矩阵中的是第k次迭代imu姿态旋转分量与先验姿态间的变化量，是imu和激光雷达旋转外参在迭代中的变化关系，是3行3列的全零矩阵，是3行15列的全零矩阵，是3行3列的单位矩阵，是15行15列的单位矩阵，是的转置后求逆运算，同理；

21、s33：第k次迭代的后验状态如下：

22、，其中，表示i时刻第k次迭代的后验状态，表示i时刻第k次迭代之前的后验状态，运算符根据状态量域的不同，含义也有所不同，在李群域表示相乘，在李代数域表示相加。运算符是运算符的反过程，表示卡尔曼增益矩阵，表示第k次迭代的状态量残差矩阵，表示的单位矩阵，是观测噪声系数矩阵，表示i时刻迭代优化前的后验状态量与i时刻状态量迭代优化前的先验状态量之间的变化量；

23、s34：第k次迭代的后验方程可以表示为：

24、，其中表示的单位矩阵，表示卡尔曼增益矩阵，是观测噪声系数矩阵，是i时刻迭代优化前的先验方差，雅可比矩阵是用于约束待优化状态量并随之迭代的矩阵，右上角表示矩阵的转置运算。当迭代达到退出条件或者指定次数后即可退出迭代，得到最终的后验状态。

25、在其中一个实施例中，在步骤s4之前，该方法还包括：对融合的gps坐标进行转换。

26、在其中一个实施例中，所述步骤s5包括：采用scancontext对步骤s4进行闭环检测，当检测到回环之后，对回环区域进行全局优化。

27、在其中一个实施例中，所述步骤s6包括：将步骤s4得到的运动轨迹投影到世界坐标系；

28、投影后的坐标记为。表示第i个三维坐标点，左上角w表示该坐标点位于世界坐标系或全局坐标系，与之对应的还有激光雷达坐标系l、imu坐标系i及相机坐标系c，分别表示三个坐标轴的值。

29、在其中一个实施例中，所述步骤s7包括：通过互联网将三维重建图发送到终端。

30、第二方面，本技术实施例提供了一种多传感器融合的slam定位和重建系统，该系统包括：

31、预处理模块，用于对可见光相机数据进行预处理，将预处理后的可见光相机数据通过相机与激光雷达外参同步到激光点云中，为激光雷达每次扫描得到的激光点赋色；

32、计算模块，用于计算激光雷达两次扫描间imu的相对运动量，通过卡尔曼滤波器得到机器人姿态估计的先验信息；

33、更新模块，用于通过卡尔曼滤波器更新imu先验信息得到lio最终后验状态量，即两次激光雷达扫描帧间的相对运动信息，通过叠加相对运动信息得到机器人的运动轨迹；

34、融合模块，用于通过卡尔曼滤波器融合gps与s3中得到的运动轨迹；

35、检测模块，用于对步骤s4进行回环检测；

36、叠加模块，用于将后验状态叠加到激光点云上，为每一次激光扫描结合gps信息赋予绝对的位置信息后得到三维重建图；

37、发送模块，用于将三维重建图发送到终端。

38、本技术的有益效果包括：

39、本技术提供的多传感器融合的slam定位和重建方法，可以互补激光视觉传感器的优势，同时在缺少视觉数据的情况下系统依然能够正常运行，相比于仅耦合的激光视觉设计拥有更灵活的优势；而且本技术相比于目前主流的slam多传感器融合方案不但拥有更高的定位精度和更快的运行速度，而且拥有更好的综合性能。