一种基于多传感器融合的复合环境下建图定位方法

本发明涉及移动机器人建图定位，尤其涉及一种基于多传感器融合的复合环境下建图定位方法。

背景技术：

1、当今社会，机器人已经广泛应用于各个领域，如工业制造、医疗卫生、物资运输等。机器人的建图定位一直是移动机器人研究过程中的一个重要问题，因为它关系到机器人的决策规划和控制等模块的正常运行。建图定位是指根据传感器获取的数据，估计机器人的位置、速度、姿态等状态参数，并同时建立周围的环境地图。

2、机器人的建图定位技术研究最早可以追溯到上世纪六十年代。当时，研究人员主要使用卡尔曼滤波器等方法对机器人进行位姿状态估计。随着计算机技术和传感器技术的不断发展，机器人位姿状态估计也得到了很大的发展，在单一的环境中已经有较为成熟的技术估计机器人位姿，例如在空旷场景中使用rtk定位技术一般精度可以达到厘米级。但是在环境复杂，天空视野有遮挡等复合环境下，rtk定位会退化到分米级或者完全失效。并且rtk定位技术一般只能估计机器人的位置和偏航角四个状态参数，缺少横滚角和俯仰角信息。

3、近年来基于同时定位与建图(slam)技术的发展，在卫星拒止情况下也可以得到较好的六自由度机器人位姿并且可以同时建立出机器人周围的环境地图，但是也存在着累计误差不断增大等问题。因此，在复合环境下使用多传感器的信息进行融合求解机器人的位姿状态，是一种很好的提高系统鲁棒性的方法。mit大学tixiao shan等人提出了一种基于因子图优化的融合激光雷达、imu、gps信息的机器人定位算法，但是其gps信息的处理较为粗糙，对gps的信息利用率较低。在室外复合环境下，如果有遮挡和高楼等情况，gps信息带来的跳变误差，对整个系统的稳定性影响较大，导致机器人位姿估计不准确，位姿跳变等问题，进而导致所建立的环境点云地图与真实世界偏差较大。

技术实现思路

1、本发明的目的在于针对现有技术中的不足，提供一种基于多传感器融合的复合环境下建图定位方法。

2、本发明的目的是通过以下技术方案来实现的：一种基于多传感器融合的复合环境下建图定位方法，包括以下步骤：

3、(1)通过激光雷达获取复合环境的原始点云信息，通过惯性测量单元获取惯性测量信息，通过对卫星报文进行解析获取卫星定位信息；

4、(2)对所述步骤(1)获取的原始点云信息、惯性测量信息和卫星定位信息进行预处理，其中，所述预处理包括imu预积分处理、点云信息预处理和utm坐标系投影；

5、(3)对所述步骤(2)中预处理后的原始点云信息、惯性测量信息和卫星定位信息进行全局优化，以获取机器人位姿状态、全局点云地图、机器人当前位置的局部地图，其中，所述全局优化包括点云配准和回环检测、卫星定位可信度检测以及全局因子图优化。

6、进一步地，所述步骤(2)具体为：对所述步骤(1)获取的惯性测量信息进行imu预积分处理，以获取相邻时间内的机器人位姿变化；对相邻时间内的机器人位姿变化和所述步骤(1)获取的原始点云信息进行点云信息预处理，以获取特征点云；对所述步骤(1)获取的卫星定位信息进行utm坐标系投影，以获取utm坐标系下的卫星定位信息。

7、进一步地，所述imu预积分处理具体为：将惯性测量信息在两帧之间的imu相对运动解算为固定量，以获取相邻时间内的机器人位姿变化。

8、进一步地，所述点云信息预处理具体为：首先将相邻时间内的机器人位姿变化和原始点云信息相结合进行点云噪点去除处理，以获取噪点去除后的点云信息；最后依据点云的局部曲率对噪点去除后的点云信息进行点云的特征提取，以获取特征点云。

9、进一步地，所述utm坐标系投影具体为：对卫星定位信息进行解算，以获取经纬高度信息，将经纬高度信息转化为机器人本地坐标系下的xyz三轴坐标信息，将全球卫星导航定位系统依赖的地心坐标系转化为utm坐标系，以获取utm坐标系下的卫星定位信息。

10、进一步地，所述点云配准和回环检测具体为：将相邻帧的点云信息预处理后的特征点云进行匹配校准，以优化相邻帧的机器人位姿，并输出相邻帧之间的机器人位姿变化加入全局因子图优化中作为点云配准约束；同时，将当前帧提取的特征点云与机器人所处位置的局部点云地图进行点云匹配和校准，以获取机器人在地图中的全局位姿变化，并将其加入全局因子图优化中作为回环检测约束。

11、进一步地，所述卫星定位可信度检测具体为：通过当前帧utm坐标系下的卫星定位信息和上一帧utm坐标系下的卫星定位信息作差获取第一相对位姿变化，根据第一相对位姿变化和相邻时间内的机器人位姿变化计算协方差矩阵，判断协方差矩阵中的值是否小于设置的协方差阈值，若协方差矩阵中的值小于设置的协方差阈值，则当前帧的卫星定位信息是可信的，并将该卫星定位信息加入全局因子图优化中作为卫星定位约束；否则，当前帧的卫星定位信息是不可信的，并将该卫星定位信息删除。

12、进一步地，所述全局因子图优化具体为：将经过imu预积分处理后得到的相邻时间内的机器人位姿变化加入全局因子图优化中作为imu预积分约束，根据imu预积分约束、点云配准约束和回环检测约束以及卫星定位约束的信息，选取关键帧，将每个时刻的机器人位姿作为顶点，将约束的信息作为边，构建因子图模型，并采用滑动窗口法实时解算窗口内的因子图模型，以获取机器人位姿状态和同时拼接出的全局点云地图，根据机器人当前所处位置，从全局点云地图中切取出机器人当前位置的局部地图。

13、本发明的有益效果是，本发明通过融合多传感器信息源，充分发挥了不同传感器的优势：本发明利用imu短时间测量精度较高的优点，为系统提供了相对位姿变化的初值信息，去除了点云运动畸变减少了激光里程计的误差，检测了卫星定位的可信度，剔除了卫星定位的跳变误差；本发明利用激光雷达的主动感知优点，提供了点云地图信息以及位姿重识别能力，引入了回环检测约束，校准了imu随时间产生的偏差；本发明利用了卫星定位的全局定位优点，提供了地球坐标系下的全局位置信息，将机器人本地局部坐标系与地球全局坐标系进行了关联，同时校准了由imu随时间带来的累计误差以及激光雷达随里程带来的累计误差；本发明在复合环境中有较高的鲁棒性，能够在单一传感器退化情况下仍保持高精度定位的能力。

技术特征：

1.一种基于多传感器融合的复合环境下建图定位方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的基于多传感器融合的复合环境下建图定位方法，其特征在于，所述步骤(2)具体为：对所述步骤(1)获取的惯性测量信息进行imu预积分处理，以获取相邻时间内的机器人位姿变化；对相邻时间内的机器人位姿变化和所述步骤(1)获取的原始点云信息进行点云信息预处理，以获取特征点云；对所述步骤(1)获取的卫星定位信息进行utm坐标系投影，以获取utm坐标系下的卫星定位信息。

3.根据权利要求1或2所述的基于多传感器融合的复合环境下建图定位方法，其特征在于，所述imu预积分处理具体为：将惯性测量信息在两帧之间的imu相对运动解算为固定量，以获取相邻时间内的机器人位姿变化。

4.根据权利要求1或2所述的基于多传感器融合的复合环境下建图定位方法，其特征在于，所述点云信息预处理具体为：首先将相邻时间内的机器人位姿变化和原始点云信息相结合进行点云噪点去除处理，以获取噪点去除后的点云信息；最后依据点云的局部曲率对噪点去除后的点云信息进行点云的特征提取，以获取特征点云。

5.根据权利要求1或2所述的基于多传感器融合的复合环境下建图定位方法，其特征在于，所述utm坐标系投影具体为：对卫星定位信息进行解算，以获取经纬高度信息，将经纬高度信息转化为机器人本地坐标系下的xyz三轴坐标信息，将全球卫星导航定位系统依赖的地心坐标系转化为utm坐标系，以获取utm坐标系下的卫星定位信息。

6.根据权利要求1所述的基于多传感器融合的复合环境下建图定位方法，其特征在于，所述点云配准和回环检测具体为：将相邻帧的点云信息预处理后的特征点云进行匹配校准，以优化相邻帧的机器人位姿，并输出相邻帧之间的机器人位姿变化加入全局因子图优化中作为点云配准约束；同时，将当前帧提取的特征点云与机器人所处位置的局部点云地图进行点云匹配和校准，以获取机器人在地图中的全局位姿变化，并将其加入全局因子图优化中作为回环检测约束。

7.根据权利要求1所述的基于多传感器融合的复合环境下建图定位方法，其特征在于，所述卫星定位可信度检测具体为：通过当前帧utm坐标系下的卫星定位信息和上一帧utm坐标系下的卫星定位信息作差获取第一相对位姿变化，根据第一相对位姿变化和相邻时间内的机器人位姿变化计算协方差矩阵，判断协方差矩阵中的值是否小于设置的协方差阈值，若协方差矩阵中的值小于设置的协方差阈值，则当前帧的卫星定位信息是可信的，并将该卫星定位信息加入全局因子图优化中作为卫星定位约束；否则，当前帧的卫星定位信息是不可信的，并将该卫星定位信息删除。

8.根据权利要求1所述的基于多传感器融合的复合环境下建图定位方法，其特征在于，所述全局因子图优化具体为：将经过imu预积分处理后得到的相邻时间内的机器人位姿变化加入全局因子图优化中作为imu预积分约束，根据imu预积分约束、点云配准约束和回环检测约束以及卫星定位约束的信息，选取关键帧，将每个时刻的机器人位姿作为顶点，将约束的信息作为边，构建因子图模型，并采用滑动窗口法实时解算窗口内的因子图模型，以获取机器人位姿状态和同时拼接出的全局点云地图，根据机器人当前所处位置，从全局点云地图中切取出机器人当前位置的局部地图。

技术总结
本发明公开了一种基于多传感器融合的复合环境下建图定位方法，该方法首先通过激光雷达获取复合环境的原始点云信息，通过惯性测量单元获取惯性测量信息，通过对卫星报文进行解析获取卫星定位信息；然后对原始点云信息、惯性测量信息和卫星定位信息进行预处理；最后对预处理后的原始点云信息、惯性测量信息和卫星定位信息进行全局优化，以获取机器人位姿状态、全局点云地图、机器人当前位置的局部地图。本发明在复合环境中有较高的鲁棒性，能够在单一传感器退化情况下仍保持高精度定位的能力，能够解决机器人位姿估计不准确、位姿跳变等问题，有利于减小建立的环境点云地图与真实世界的偏差。

技术研发人员：朱秋国,贾尚斌,苏昱竹,吴俊
受保护的技术使用者：浙江大学
技术研发日：
技术公布日：2024/1/14