[发明专利]一种基于反光板与SLAM融合的定位方法

说明书

本发明公开了一种基于反光板与SLAM融合的定位方法，S1：进入反光板扫描区域，进行反光板匹配；不为反光板定位模式，切换成反光板定位模式，再进行反光板匹配；S2：求解机器人位姿；S3：发布位姿；S4：判断是否离开反光板扫描区域，若是，切换到SLAM定位模式，并发布机器人位姿，若否，重复步骤S2和S3。机器人进入反光板扫描区域，当切换为反光板定位模式，求取机器人的位姿并发布，当激光雷达扫描不到反光板或扫描到反光板个数小于一定数量，切换到SLAM定位模式，在SLAM定位模式下继续发布机器人的位姿，适用于高动态环境还降低成本。

技术领域

本发明涉及定位技术领域，特别是一种基于反光板与SLAM融合的定位方法。

背景技术

随着中国经济的快速发展，agv搬运车越来越频繁使用于工厂各个角落，但是agv的工作环境是高动态变化的，有很多人与货物，包括其他机械设备经常变化，造成传统的slam定位不能适用于这种高动态环境，但是实际现场环境太大，而高动态区域估只占比一小部分，如果全场部署反光板的话，其成本就会较高。经过发明人长期研究，发明了一种基于反光板与SLAM融合的定位方法。

发明内容

本发明的目的在于克服现有技术的缺点，提供一种基于反光板与SLAM融合的定位方法。

本发明的目的通过以下技术方案来实现：一种基于反光板与SLAM融合的定位方法，包括以下步骤：

S1：进入反光板扫描区域，若为反光板定位模式，则进行反光板匹配；若不为反光板定位模式，则是用SLAM位姿作为初始化位姿并切换成反光板定位模式，再进行反光板匹配，在反光板定位模式下，SLAM定位模式继续运行求解连续位姿，但不作定位位姿发布；

S2：在反光板定位模式下，求解机器人位姿；

S3：发布位姿；

S4：判断是否离开反光板扫描区域，若是，则切换到SLAM定位模式，并发布机器人的位姿，若否，则重复步骤S2和S3。

优选的，步骤S1中，若不为反光板定位模式，则获取SLAM定位位姿作为初始位姿，其位姿矩阵为：

。

优选的，步骤S2中，还包括以下步骤：

S21：在反光板定位模式下，激光雷达扫描到高反物体，得到激光数据；

S22：根据扫描到的激光数据，按强度阈值过滤提取激光点云，计算出反光板上的点数，

；

其中为反光板半径，为激光雷达与反光板之间的距离，为雷达角度分辨率；

S23：提取的激光点云满足点数满足n时，提取的激光点云的坐标，可通过极坐标公式求得，公式如下：

；

；

其中，为激光雷达与反光板之间的距离，为雷达坐标系的扫描角度，当前反光板所有提取的激光点云通过加权平均得出反光板的质心位置，当前帧提取的所有反光板质心位置集合记为sets；

S24：根据提取的反光板质心集合sets，反光板在雷达坐标系的位姿矩阵，根据该反光板在雷达坐标系的位置，

；

根据已知雷达到机器人中心的相对位置，则反光板的当前预估位姿矩阵，

，

根据求解出反光板的位姿矩阵可以得到每个反光板当前位置，与标定好的反光板进行匹配，若匹配成功，则匹配成功的反光板记为一个集合mapsets。

优选的，步骤S24中，还包括以下步骤：

S241：匹配好的反光板，按两个反光板形成一个组合，分成若干组；

S242：遍历所有组合，其中每个组合包括两个反光板信息，反光板a的标定坐标为（，），当前雷达坐标系下的坐标为；反光板b的标定坐标为与当前雷达坐标系下的坐标为，从而计算出车当前位姿；