一种基于多传感器融合的移动机器人定位方法

1.本发明涉及移动机器人定位领域，特别是一种基于多传感器融合的移动机器人定位方法。


背景技术：

2.随着社会现代化的迅速发展，机器人等无人系统越来越普及，自主定位作为移动机器人、搬运agv等完成工作调度的首要前提被广泛研究。全球定位系统凭借其定位范围广、定位精度高等优势被广泛应用于室外定位领域，但是在室内以及受建筑物遮挡环境下，全球定位系统由于信号衰减严重，无法为自主机器人提供精确的位置信息。
3.当前自主服务机器人应用最多的导航定位方式有磁条、激光雷达、视觉和无线定位等。磁条导航多用于室内，最为可靠普遍、成本适中且稳定性最好，但是铺设磁条诸多不便。视觉定位有两种方式，一种是视觉加辅助标识的方式，定位精度较高，但是易损坏且铺设场景受限；另一种是视觉slam技术，受光照影响较大，普适性和实时性也有待提升。激光雷达能够获取较高定位信息，但是价格昂贵，且存在定位盲区。超宽带是无线定位的主流，性价比高，但是受非视距和多径效应影响精度下降严重。在复杂环境下，传统定位方式都难以满足移动机器人对高精度、高可靠定位的需求。
4.目前服务机器人大多工作于室内外共存、非结构环境和大场景等环境下，以上定位方式单独使用都不能取得良好的效果，需要研究一种在上述环境下能够持续为自主服务机器人提供高精度、高可靠位置信息的定位方法。


技术实现要素：

5.针对上述问题，本发明提供一种多传感器融合定位方法，采用基于超宽带、2d激光雷达和可见光相机融合的定位方式，能够适应室内外共存、多障碍物等多种复杂环境，持续为自主服务机器人提供可靠的、高精度的位置信息，具有成本低、布设简单、实时性好、可靠性高等优点，可广泛应用于各种自主机器人。
6.实现本发明目的的技术解决方案为：一种融合2d激光雷达、可见光相机和超宽带的定位方法，包括以下步骤：
7.步骤一、在有限场景下布设包含真实空间坐标的人工标志物，通过2d激光雷达和可见光相机设备感知机器人周围环境，获取点云信息，进行激光雷达定位，获取图像信息辅助雷达定位。
8.步骤二、所述人工标志物包含真实空间点世界坐标、用以辅助识别的id、方向等信息，可以是apriltag、二维码等。
9.步骤三、所述获取点云信息与图像信息后，还包括：对所述点云信息和图像信息进行预处理。
10.步骤四、所述点云信息的预处理包括以下步骤：
11.1)利用统计离群点滤波算法进行雷达点云优化，剔除跳变点；
12.2)利用相机提取激光雷达所在平面特征点，三角化恢复特征点深度，生成二维局部相机点云；
13.3)所述相机点云与雷达点云融合并利用体素滤波器进行下采样，得到融合点云；
14.4)判断所述融合点云是否为点云关键帧。
15.步骤五、所述点云关键帧的选取包括以下步骤：
16.1)当前帧的地图点数量小于阈值，认定为关键帧；
17.2)当前帧提取的地图点中包含上一帧中特征点小于一定比例，认定当前帧为关键帧；
18.3)距离上一关键帧选取已超过预设时间，认定当前帧为关键帧。
19.步骤六、所述图像信息的预处理包括以下步骤：
20.1)所述可见光相机设备可选取单目相机等，将相机采集的彩色图像转换为灰度图；
21.2)将灰度图二值化并进行高斯平滑处理，减少噪声干扰；
22.3)使用边缘检测算法进行图像边缘检测，提取轮廓数据，并给定阈值剔除角点过于接近的轮廓；
23.4)拟合多边形轮廓并找寻与人工标志物相似轮廓，给定阈值限制边长大小，剔除与所述人工标志物大小差异较大四边形；
24.5)透视变换找寻灰度图中的人工标志物轮廓。
25.步骤七、所述激光雷达定位包括如下步骤：
26.1)对所述关键帧进行加窗处理，添加当前帧与先前多帧的约束关系，设置窗口大小，利用滑动窗口限制关键帧数目；
27.2)计算当前关键帧与滑窗内其他关键帧的位姿变换，分别得到一组雷达坐标与雷达姿态；
28.3)拟合所述雷达坐标的中心点坐标，删除与中心点坐标欧式距离最大、最小的雷达坐标后得到一组新的雷达定位坐标，减小偶然误差带来的影响；
29.4)分别根据滑窗内每一帧与当前帧的损失函数大小，为对应所述新的雷达定位坐标以及对应雷达姿态分配权重，且损失函数越大，分配权重越小；
30.5)每次新到关键帧计算损失函数，为所述雷达定位坐标及其对应的雷达姿态动态分配权重，根据权重分别对所述雷达定位坐标和雷达姿态进行融合，解算得到最终雷达位姿。
31.步骤八、所述获取图像信息辅助雷达定位包括以下步骤：
32.1)所述可见光相机设备扫描到人工标志物后根据3d-2d位置关系可解算得到相机坐标与姿态；
33.2)将所述相机姿态、所述雷达姿态和所述相机坐标、最终雷达定位结果分别转换到世界坐标系下后融合；
34.3)融合后的位姿作为先验位姿进行激光雷达初始位姿校准与累积误差抑制。
35.步骤九、获取超宽带精定位坐标与雷达定位坐标融合，在缺失人工标记物时辅助激光雷达提高定位精度并抑制累计误差。
36.步骤十、所述超宽带精定位结果获取包括以下步骤：
37.1)布设至少三个用于接收超宽带信号的基站，规定世界坐标系，其中各基站世界坐标已预设，时钟严格同步；
38.2)识别视距与非视距环境，将非视距环境下的定位问题转换到视距环境下求解；
39.3)超宽带信号发射源作为标签搭载于定位平台，向各个基站发射信号，基站接收超宽带信号经过滤波算法得出精定位坐标。
40.有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：
41.本发明实现了一种基于超宽带、视觉和2d激光雷达的多传感器融合定位方法，能够适应室内外共存、多障碍物等多种复杂场景。融合相机点云与雷达点云的方法可以充分利用机器人周围的环境信息，减少测量噪声带来的点云缺失。有限场景下的人工标志物可以消除雷达长期定位带来的累计误差并辅助雷达重置先验位姿进行点云校准。多传感器融合的定位方式确保位置信息时刻由两种定位方式提供，减小了定位失败的概率，更具有准确性和鲁棒性。
42.此外，将2d激光雷达与可见光相机融合，成本较低，体积较小，布设简单，维护容易。
43.此外，提供一种基于滑动窗口的权重自适应雷达位姿优化方法，增加了雷达当前位姿的约束关系，根据损失函数进行自适应加权融合的算法更能使求解的位姿趋近于真实值，提高算法运行速度，提高雷达帧间位姿匹配的精度，抑制累积误差。
44.此外，提供了一种多传感器动态融合的定位方法，充分发挥各传感器的优点，提高定位精度，减小盲目融合带来的误差，超宽带提供的全局位置信息可以解决移动机器人的迷航问题以及在移动机器人定位失败时为其提供绝对位置信息。
附图说明
45.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
46.图1是本发明提供的基于滑动窗口的权重自适应雷达位姿优化方法流程示意图；
47.图2是本发明提供的多传感器动态融合定位方法流程示意图；
48.图3是本发明提供的摘要附图。
具体实施方式
49.以下描述中，为了说明，但不限于此，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
50.下面结合实施例和说明书附图对本发明做进一步说明。
51.实施例1
52.一种基于权重自适应的滑窗雷达位姿优化方法，如图1所述，包括步骤如下：
53.1)激光雷达、相机联合标定，时间戳同步；
54.2)利用二维激光雷达获取点云信息，通过统计离群点滤波算法进行点云预处理，剔除跳变点，输出优化后的雷达点云，利用相机提取激光雷达所在平面特征点，恢复特征点深度，生成二维局部相机点云；
55.3)所述雷达点云与视觉点云融合得出具有丰富特征点的融合点云，利用体素滤波器进行下采样，去除重叠点；
56.4)对所述融合后的点云进行关键帧判断，利用滑动窗口增加当前关键帧与先前多帧关键帧的约束关系，限制关键帧数目，计算当前关键帧与滑窗内其他帧的位姿变换，分别得到定位坐标与姿态；
57.5)拟合所述雷达坐标的中心点坐标，删除与中心点坐标欧式距离最大、最小的雷达坐标后得到一组新的雷达定位坐标；
58.6)利用点线距离计算所述关键帧与滑窗内其他帧的损失函数，根据损失函数大小为对应所新的雷达定位坐标动态分配权重，损失函数越大则匹配越不精准，计算时权重应该更小，减小不精准匹配带来的真实值偏移；
59.7)根据所述权重分别对所述雷达定位坐标和对应雷达姿态进行融合，得到最终雷达位姿。
60.实施例2
61.一种基于多传感器动态融合的定位方法，如图2所述，包括步骤如下：
62.1)激光雷达扫描周围环境，获取雷达点云，视觉传感器进行图像采集，并行进行人工标志物(apriltag、二维码等)识别以及雷达所在平面特征点的深度估计和点云恢复；
63.2)所述人工标志物包含3d点的空间信息，根据3d-2d位置关系可解算得到相机位姿；
64.3)相机点云辅助雷达点云丰富特征，得到融合后的点云；
65.4)所述融合后的点云通过帧间位姿匹配得到雷达位姿；
66.5)检测到人工标志物时，将所述雷达位姿与所述相机位姿融合来提高雷达定位精度，融合后的位姿作为雷达先验位姿进行点云校准，抑制累计误差；
67.6)未检测到人工标志物时，进行视距与非视距环境识别，当超宽带处于视距环境下，直接将超宽带定位值与所述雷达定位值融合，当超宽带处于非视距环境下，将非视距环境下的定位问题转换为视距环境下的定位问题求解，得出定位值后再与所述雷达定位值融合，保证雷达定位误差一直被抑制。