本发明涉及slam、机器人及电子，尤其涉及一种多传感器融合的激光雷达slam系统及方法。
背景技术：
1、当前对激光雷达slam技术研究已经非常深入，但是针对地下长走廊这种特殊的环境仅仅依靠单一的传感器是不能满足实际需求的，因为激光雷达依靠特征来进行位姿估计，而长走廊的特征较少，所以非常容易出现退化和漂移的情况，尤其是固态激光雷达这种fov本身就比较小的雷达，虽然其点云更稠密，但fov仅是机械式激光雷达的约1/5，所以能够检测到的特征更少，更加容易出现漂移的情况。对此，多传感器融合的slam方法开始被推广。自动驾驶所采用的融合rtk/gps/gnss等高精度定位设备在地下环境中会失效，因此并不适用。
2、imu成为了绝大多数地下环境slam的解决方案，激光slam的缺陷可以通过融合imu信息来补偿，使其不受结构特征和环境巨大变化的影响，可以在短时间内提供高精度的姿态估计。imu与激光雷达融合的解决方案主要分为松耦合和紧耦合。松耦合的融合方案可以以最小的计算量为代价提供更加准确和稳定的工作，即imu与激光雷达数据是独立处理的，且一旦其中一种传感器失效，另一种传感器也会依然工作。机械式激光雷达与imu松耦合的解决方案较多，如loam、lego-loam、cartographer等，而以新兴的固态激光雷达为主要传感器与imu松耦合的解决方案中，loam-livox借鉴了loam算法，成为最具代表性的固态激光雷达slam算法。不过当前的研究中，紧耦合是研究者们更加青睐的一种研究方向，因为随着算力设备性能的不断提高，计算量的问题正在逐步被攻克，因此更高的精度和鲁棒性成为了追求。
3、imu的数据和激光雷达将会互相校准和纠正，imu的高频数据用以抵消激光雷达可能存在的误差和漂移，因此被广泛应用于需要更精细应用的场景中。在使用固态激光雷达与imu紧耦合的方案中，fast-lio是被提出的一个基于迭代卡尔曼滤波器的slam系统，其在大多数场景中表现的准确而鲁棒，但是其历史数据对当前状态的影响将会被丢弃，无法执行全局姿态校正。现有技术中还提出了一种迭代误差状态卡尔曼滤波算法来紧耦合激光雷达/imu，克服了扩展卡尔曼线性化过程中丢弃高阶误差导致的精度损失。但其准确性取决于结构化的环境。fast-lio2作为fast-lio的改进版本，取消了特征提取环节，并且通过增量式kd-tree来维护地图，提高了算法的鲁棒性和效率，适用于绝大多数场景，可以说是当前固态激光雷达与imu紧耦合最先进的方案，但正是由于算法有着更高的普适性，所以针对像矿区地下巷道、排洪涵洞这种特殊的场景来说，其并没有考虑到应用平台，即可能应用在轮式机器人上，或是腿足机器人上，亦或是无人机上，因此实际环境和其他可能用于融合来提升slam效果的传感器并未被纳入其中。
技术实现思路
1、为解决上述技术问题，本发明提出了一种多传感器融合的激光雷达slam系统及方法，不但发挥了轮式机器人自身轮子数据的作用，而且利用打滑检测手段避免了融合此数据导致系统不稳定的情况出现，做到因地、因物制宜，提高imu/编码器/固态激光雷达紧耦合slam的精度和鲁棒性。
2、一方面为实现上述目的，本发明提供了一种多传感器融合的激光雷达slam系统，包括：
3、数据输入模块：用于获取采集到的原始数据，所述原始数据包括imu数据、轮式编码器数据和固态激光雷达的点云数据；
4、预处理模块：用于对所述原始数据进行预处理；
5、位姿估计模块：用于进行imu/编码器和激光雷达的数据融合，利用紧耦合的方式以所述imu/编码器的高频数据抵消激光雷达的畸变，对imu误差状态进行更新；
6、输出模块：用于获取机器人的里程计输出，并且通过里程计将激光点云投影至正确区域，形成局部地图和全局地图；
7、其中，所述数据输入模块、所述预处理模块、所述位姿估计模块、所述输出模块依次连接。
8、优选地，所述数据输入模块包括：
9、imu单元：用于采集所述imu数据；
10、轮式编码单元：用于采集所述轮式编码器数据；
11、固态激光雷达单元：用于采集点云数据；
12、所述imu单元、所述轮式编码单元、所述固态激光雷达单元依次连接。
13、优选地，所述预处理模块包括：
14、imu预积分单元：用于通过预积分的方式获取以所述imu数据预测得到的机器人姿态变化；
15、轮式编码器里程计算单元：用于获取机器人的线性速度和偏航角；
16、打滑检测单元：用于判断所述机器人的轮子是打滑，提高所述机器人位姿预测的精度和鲁棒性；
17、其中，所述imu预积分单元与所述imu单元连接，所述轮式编码器里程计算单元与所述轮式编码单元连接，所述打滑检测单元与所述固态激光雷达单元连接。
18、优选地，所述预处理模块还包括特征提取与特征匹配单元，所述特征提取与特征匹配单元用于提取激光雷达点云数据的特征，通过特征值之间的关系进行平面点和边缘点的提取，采用基于icp的方法进行特征匹配。
19、另一方面，本发明还提供了一种多传感器融合的激光雷达slam系统的应用方法，包括：
20、基于数据输入模块获取原始数据，所述原始数据包括imu数据、轮式编码器数据和固态激光雷达的点云数据；
21、通过预处理模块分别对所述原始数据进行预积分处理、轮式编码器里程计算、打滑检测以及特征提取与匹配，获取处理后的原始数据；
22、通过位姿估计模块，利用误差状态卡尔曼滤波器进行imu/编码器和激光雷达的数据融合，利用紧耦合的方式以imu/编码器的高频数据抵消激光雷达的畸变，获取更新后的imu位姿；
23、通过eskf滤波输出后的机器人姿态估计，基于输出模块获得机器人的里程计输出，并且通过滤波输出的位姿变换将激光点云投影至正确区域，形成局部地图和全局地图。
24、优选地，进行所述预积分处理的方法为：
25、
26、
27、
28、其中，字母左上角的上标w表示在世界坐标系下进行的运算，为imu第i到第j帧的旋转增量，为imu第i到第j帧的速度变量，为imu第i到第j帧的位置变量，分别为在imu第i和第j帧的旋转增量，分别为在imu第i和第j帧的速度增量，g为重力加速度，δt为测量周期，分别为在imu第i和第j帧的位置增量，t为转置矩阵。
29、优选地，进行所述轮式编码器里程计算的方法为：
30、
31、其中，vl、vr分别为左右前轮的线速度，分别为左右前轮的转速，r为车轮半径，ψ为偏航角，为机器人的xyz三轴的线性速度，c为机器人左右轮的间距。
32、优选地，进行所述打滑检测，包括：
33、对比imu和轮式里程计在任一时间段内角度变化差异，判定轮子是否打滑，若打滑，则在此时间段内采用imu的线性速度数据，若未打滑，则使用所述轮式里程计计算线性速度数据。
34、优选地，获取所述更新后的imu位姿，包括：
35、对imu状态进行定义，将机器人在第n帧雷达帧时的位置定义为到第n+1帧的真实状态定义为误差状态为δx；然后进行状态传播，当新的imu测量帧到达后，误差状态进行更新，获得所述更新后的imu位姿。
36、与现有技术相比，本发明具有如下优点和技术效果：
37、本发明方法相比仅使用imu和激光雷达耦合算法的基础上利用误差状态卡尔曼滤波器融合了含打滑检测的轮式编码器信息，在线性速度上补偿了imu出现的累积误差，使提出的方法具有更高的精度和鲁棒性，适用于如矿山排洪涵洞、巷道等地下易发生退化的场景中。
1.一种多传感器融合的激光雷达slam系统，其特征在于，包括：
2.根据权利要求1所述的多传感器融合的激光雷达slam系统，其特征在于，所述数据输入模块包括：
3.根据权利要求2所述的多传感器融合的激光雷达slam系统，其特征在于，所述预处理模块包括：
4.根据权利要求3所述的多传感器融合的激光雷达slam系统，其特征在于，所述预处理模块还包括特征提取与特征匹配单元，所述特征提取与特征匹配单元用于提取激光雷达点云数据的特征，通过特征值之间的关系进行平面点和边缘点的提取，采用基于icp的方法进行特征匹配。
5.一种多传感器融合的激光雷达slam系统的应用方法，其特征在于，包括：
6.根据权利要求5所述的多传感器融合的激光雷达slam系统的应用方法，其特征在于，进行所述预积分处理的方法为：
7.根据权利要求5所述的多传感器融合的激光雷达slam系统的应用方法，其特征在于，进行所述轮式编码器里程计算的方法为：
8.根据权利要求5所述的多传感器融合的激光雷达slam系统的应用方法，其特征在于，进行所述打滑检测，包括：
9.根据权利要求5所述的多传感器融合的激光雷达slam系统的应用方法，其特征在于，获取所述更新后的imu位姿，包括：
