【技术实现步骤摘要】
本专利技术涉及多传感器融合技术，同时定位与建图技术，属于服务机器人，具体涉及一种服务机器人的多传感器融合定位与建图方法及系统。
技术介绍
1、同时定位与建图（slam）技术是机器人在未知环境中作业的基础需求。现有的单一传感器slam技术较为成熟，如基于激光雷达的loam，基于相机的orb-slam等，但它们仍无法处理由于传感器自身能力限制而带来的退化和误差，例如激光雷达无法处理玻璃面、缺少结构化信息的场景，相机在黑暗中无法提取有效信息，惯性测量单元（imu）能够在短时间内解决激光雷达和相机信息退化的问题，但无法避免长期工作时的漂移问题。不同的传感器各有优势和缺陷，并且能够相互补偿，故而融合多传感器的信息在实际应用中是十分必要的。
2、现有的slam技术通常假设环境为静态，对于服务机器人经常面对的动态障碍物繁多的场景鲁棒性较差，一旦定位和建图结果被动态障碍物所干扰，机器人难以恢复到正确的位姿，而语义信息能够较好地减少动态障碍物的影响，同时能够处理纹理匮乏的场景，并且相较于传统方法单纯的建图，将语义信息投影到环境地图中能够提升机器人的场景理解能力。但由于场景中相同标签的物体众多，单纯的语义信息的定位精度较差，仍旧需要传统方法的辅助。
3、当机器人搭载多传感器时，对多模态信息进行松耦合虽然能够减少运算消耗，但是降低了整个系统的精度和信息的利用率，紧耦合的信息融合方式能够更高效和充分地利用多模态信息。
技术实现思路
1、有鉴于此，本专利技术提供了一种服务机器人的多传
2、本专利技术为达到上述专利技术目的，采用如下技术方案：
3、一种服务机器人的多传感器融合定位与建图方法，包括：
4、步骤s1，对服务机器人搭载的多个传感器采集的多模态数据进行初步处理，包括：
5、s101，对服务机器人搭载的单目相机、激光雷达和惯性测量单元imu进行内、外参标定以及时间对齐；
6、s102，经过设定的时间间隔获得单目相机采集的rgb图像数据和激光雷达采集的点云数据关键帧；
7、s103，通过惯性测量单元imu对点云数据去畸变，实现运动补偿；
8、s104，以惯性测量单元imu初始时间段测量的加速度和角速度数据积分作为服务机器人位姿初值，初始化系统；
9、步骤s2，针对rgb图像数据和点云数据分别提取语义特征，并对rgb图像数据和点云数据语义特征进行融合并输出融合图像，再预测融合图像域中的语义信息；
10、步骤s3，对点云数据进行几何特征提取；
11、步骤s4，优化因子设计，具体包括：
12、根据步骤s2预测的语义信息获得多模态语义特征因子；
13、根据点云数据的几何特征得到激光雷达里程计因子；
14、利用rgb图像数据相邻关键帧获得图像光度信息因子；
15、利用相邻关键帧之间的相对运动获取imu预积分残差因子；
16、步骤s5，根据步骤s4获得的五个因子，得到如下优化方程：
17、
18、其中，表示关键帧的序号；为滑动窗口大小；为选取的滑动窗口内所有关键帧对应的位姿的集合；、、、分别为语义、imu、相机、激光雷达测量值对应的协方差；表示通过协方差矩阵的逆对不同测量的残差进行加权求和；
19、通过gtsam优化方法解算所述优化方程得到最优的机器人的位姿信息，并根据里程计结果将语义信息投影到地图中，建立带有语义信息的环境地图。
20、较佳的，所述s103中，运动补偿的方法包括：通过球面线性插值法得到在不同激光雷达点云对应时间戳处imu测量的位姿，并依此将激光雷达点云投影到所对应的点云帧中。
21、较佳的，所述步骤s2具体包括如下步骤：
22、s201，根据s1中标定的内、外参矩阵将三维激光雷达点云投影到二维图像平面，实现点云数据和rgb图像中像素点的关联；
23、s202，对所述二维图像和所述rgb图像数据分别通过cnn提取语义特征得到特征向量和；
24、s203，分别对特征向量和进行查询、键和值的编码，经过第一个transformer层后得到特征向量和，再将和串联并拉平为，对其进行查询、键和值的编码，再经第二个transformer层后得到融合后的特征向量；
25、s204，将输入decoder模块，预测每个像素点的语义标签，及其该标签的相应概率，即像素点在位置处的语义观测为类的概率。
26、较佳的，所述s204中，移除设定类别的像素，减少动态障碍物的影响。
27、较佳的，所述s4中多模态语义特征因子的计算方法包括：
28、令语义观测似然函数表示像素点在位置处的语义观测为类为真的概率，其中为第帧在语义空间中的投影，为第帧的位姿；表示第帧像素点在语义域中的投影到语义标记为的区域的投影距离，与的大小成反比，具体表示如下：
29、
30、根据s2中所得到的多模态语义分类结果计算多模态语义特征因子，具体表示如下：
31、
32、其中为语义标签集合，为第帧的像素点集合。
33、较佳的，所述步骤s3中，对点云数据进行传统几何特征提取的方法包括：采用loam中通过曲率提取特征的方法，计算当前点云前后个点连接的曲率，曲率高于0.1的为边缘点，其余为平面点，具体计算公式如下：
34、
35、其中，为计算的曲率；表示第帧；表示在雷达坐标系下；和分别表示第帧上点云及其邻近点云的坐标值，对二者坐标的差值求和，除以所取的邻近点总数，得到曲率；取，。
36、较佳的，所述s4中，激光雷达里程计因子的计算方法包括：
37、首先根据惯性测量单元imu在第帧测量的位姿，将s3中提取的雷达坐标系下的边缘特征和平面特征投影到世界坐标系，分别得到和，与相邻10帧关键帧的特征所组成的局部特征地图做残差，和分别为局部特征地图中的边缘特征和平面特征，用特征匹配残差表示激光雷达里程计因子，具体表示如下：
38、
39、其中为huber损失函数。
40、较佳的，所述图像光度信息因子的计算方法包括：
41、设一个空间点在第帧和第帧中成像的像素点分别为和；假设相邻关键帧具有光度不变性，则用相同像素间的光度误差表示图像光度信息因子，具体表示如下：
42、
43、其中、分别为空间点在第帧和第帧中成像的像素点对应的灰度值；为第帧的像素点集合；；为第帧本文档来自技高网...
【技术保护点】
1.一种服务机器人的多传感器融合定位与建图方法，其特征在于，包括：
2.如权利要求1中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述S103中，运动补偿的方法包括：通过球面线性插值法得到在不同激光雷达点云对应时间戳处IMU测量的位姿，并依此将激光雷达点云投影到所对应的点云帧中。
3.如权利要求1中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述步骤S2具体包括如下步骤：
4.如权利要求3中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述S204中，移除设定类别的像素，减少动态障碍物的影响。
5.如权利要求3中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述S4中多模态语义特征因子的计算方法包括：
6.如权利要求1中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述步骤S3中，对点云数据进行传统几何特征提取的方法包括：采用LOAM中通过曲率提取特征的方法，计算当前点云前后个点连接的曲率，曲率高于0.1的为边缘点，其余为平面点，具体计算
7.如权利要求6中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述S4中，激光雷达里程计因子的计算方法包括：
8.如权利要求1中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述图像光度信息因子的计算方法包括：
9.如权利要求1中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述IMU预积分残差因子的计算方法包括：
10.一种实现权利要求1至9任一个所述的服务机器人的多传感器融合定位与建图方法的系统，其特征在于，用于实现步骤S1至S5的方法。
...【技术特征摘要】
1.一种服务机器人的多传感器融合定位与建图方法，其特征在于，包括：
2.如权利要求1中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述s103中，运动补偿的方法包括：通过球面线性插值法得到在不同激光雷达点云对应时间戳处imu测量的位姿，并依此将激光雷达点云投影到所对应的点云帧中。
3.如权利要求1中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述步骤s2具体包括如下步骤：
4.如权利要求3中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述s204中，移除设定类别的像素，减少动态障碍物的影响。
5.如权利要求3中所述的一种服务机器人的多传感器融合定位与建图方法，其特征在于，所述s4中多模态语义特征因子的计算方法包括：
6.如权利要求1中所述的一种服务机器人...
【专利技术属性】
技术研发人员：邓方，樊铮，张乐乐，董伟，石翔，赵佳晨，陈晨，吕茂斌，支涛，李洪波，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。