一种基于反光板的移动机器人定位方法

本发明涉及机器人定位，具体涉及一种基于反光板的移动机器人定位方法。

背景技术：

1、随着计算机、5g通讯、传感器等前沿技术的快速发展，机器人技术从试验成长期大步跨越进生产应用期，进入了新的发展阶段，移动机器人作为机器人技术的重要分支，是机器人集群中较为灵活、具备自主运动和环境感知能力的一类应用于迎宾导览、机房巡检、环卫清洁、物流配送等多领域的智能化机器人，而在智能化机器人的实际应用中，智能化机器人的定位尤为重要，如中国专利申请号为202211630086.7，公布日为2023.04.25的专利文献，其公开了一种移动机器人可靠定位方法，包括基于种子区域生长的反光板直角特征提取模块、融合反光板直角特征的全局定位模块和融合相关性扫描匹配的局部定位模块；基于种子区域生长的反光板直角特征提取模块将根据环境点云信息、光强信息提取反光板直角特征后构建绝对位置下的特征词典数据库，融合反光板直角特征的全局定位模块匹配得出位姿置信度较高的机器人位姿粒子；融合相关性扫描匹配的局部定位模块，做最小化误差迭代输出高精度机器人实时位姿。

2、该文献中的方法是采用反光板高能量强度信息构建增强特征的方式来提高机器人在激光点云嘈杂无规则、有效点云稀疏的实际应用场景中的特征提取精度；但是并没有分析点云数据的连贯性以及强度，进而无法得到反光板的有效轮廓数据，使得反光板的定位误差较差。

技术实现思路

1、本发明的目的在于提供一种基于反光板的移动机器人定位方法，能够缩减反光板的定位误差，提高机器人的定位精度。

2、本发明为解决上述技术问题所采用的技术方案是：一种基于反光板的移动机器人定位方法，用于机器人的位姿进行定位，包括以下步骤：

3、（1）通过激光雷达扫描反光板轮廓采集点云数据；

4、（2）将采集得到的点云数据导入里程计参考坐标系，然后对点云数据进行多帧融合得到稳定的反光板轮廓；

5、（3）在所有多帧融合的点云数据中提取符合条件的点云数据组成连续轮廓{ p}；

6、（4）对步骤（3）中符合条件的连续轮廓{ p}进行初步匹配；

7、（5）通过标准反光板的外形相似度p对步骤（4）中初步匹配后的连续轮廓{ p}进行二次匹配，

8、（5.1）若匹配失败，则返回步骤（1）；

9、（5.2）若匹配成功，则进入步骤（6）；

10、（6）通过连续轮廓{ p}求解反光板拟合直线方程，然后构建反光板坐标系，取反光板中心点c在拟合直线方程上的投影点o作为反光板坐标系原点；

11、（7）通过反光板坐标系与里程计坐标系变换矩阵、里程计坐标系关系与机器人坐标系变换矩阵计算得到机器人坐标系在反光板坐标系下的位姿变换矩阵，并根据位姿变换矩阵确定机器人目前的位姿。

12、以上设置，通过激光雷达扫描反光板轮廓，能够采集点云数据；然后将点云数据导入里程计参考坐标系中并进行多帧融合，从而得到更加稳定的反光板轮廓；通过筛选符合条件的连续轮廓进行初步匹配，这样能够初步识别出与连续轮廓相对应的反光板轮廓，同时也能够筛选出反光板轮廓存在遮挡或无匹配的情况；而通过标准反光板的外形相似度p对初步匹配后的连续轮廓进行二次匹配，能够判断出连续轮廓对于反光板的整体形状，最后通过计算反光板拟合直线方程中的优化变量计算出反光板坐标系在里程计参考坐标系的位姿，最后通过反光板坐标系与里程计坐标系变换矩阵和里程计坐标系关系与机器人坐标系变换矩阵计算得到机器人坐标系在反光板坐标系下的位姿变换矩阵，并根据位姿变换矩阵确定机器人的位姿，即可确定好机器人目前的姿势从而实现定位控制。

13、进一步的，所述反光板包括反光板 b1 、反光板 b2 、反光板 b3，定义反光板 b1 、反光板 b2 、反光板 b3的宽度分别为 w1 、w2 、w3，反光板 b1和反光板 b2的理论中心距离为 d1,2，反光板 b2和反光板 b3的理论中心距离为 d2,3，反光板 b2和反光板 b3的理论中心距离为 d1 ,3，反光板 b1和反光板 b2的理论间距为 s1,2，反光板 b2和反光板 b3的理论间距为 s2,3。

14、以上设置，便于后续对反光板的宽度误差、中心距离误差、间距误差进行计算。

15、进一步的，所述步骤（2）中还包括：

16、通过变换矩阵将采集得到的点云数据从激光雷达坐标系转换到里程计参考坐标系中，其公式为：

17、，

18、合并短期时间段t内不同时刻的点云数据，并且只保留最新的n帧数据，然后进行多帧融合后得到点云数据。

19、以上设置，便于将点云数据转换到里程计参考坐标系中，然后对点云数据进行多帧融合。

20、进一步的，所述步骤（3）中还包括：

21、连续轮廓的条件设置为：

22、，

23、其中，x为点云的横坐标值，y为点云的纵坐标值，n为连续轮廓的数量，为两块反光板中心之间的错误距离为15mm， w1为第一块反光板的宽度， w2为第二块反光板的宽度， w3为第三块反光板的宽度， e1为第一块反光板的宽度误差， e2为第二块反光板的宽度误差， e3为第三块反光板的宽度误差。

24、以上设置，能够根据两块反光板中心之间的距离差是否超过预设值、以及轮廓的强度以及反光板上的轮廓宽度是否超出反光板的宽度这三个因素出发，筛选出符合条件的点云数据，并将符合条件的点云数据组成连续轮廓，其中轮廓的强度主要是考虑怕除了反光板等其他反光物质对机器人进行反射从而得到具体轮廓，而这些非反光板反射形成的轮廓与反光板形成的轮廓在距离以及平均强度上具有差别从而能通过这些筛选方式进行筛选出来。

25、进一步的，选取步骤（3）中符合条件的任意三组连续轮廓{ p}i、{ p}j、{ p}k进行组合，其中，然后分别计算反光板的宽度误差、中心距离误差、间距误差，

26、反光板的宽度误差计算公式为：

27、，

28、反光板的中心距离误差计算公式为：

29、，

30、反光板的间距误差计算公式为：

31、，

32、其中wi、wj、wk为任意三组反光板的宽度，di,j为第i块反光板和第j块反光板的实际测量中心距离，dj,k为第j块反光板和第k块反光板的实际测量中心距离，di,k为第i块反光板和第k块反光板的实际测量中心距离，si,j为第i块反光板和第j块反光板的实际测量间距，sj,k为为第j块反光板和第k块反光板的实际测量间距，

33、接着通过轮廓平均强度公式：

34、，

35、计算三组连续轮廓{ p}i、{ p}j、{ p}k相对应的轮廓平均强度分别为、和，

36、进而计算三组连续轮廓{ p}i、{ p}j、{ p}k的强度误差，

37、，

38、对上述的宽度误差、中心距离误差、间距误差以及强度误差分别匹配相对应的指定评价权重α、β、γ、σ，然后计算三组连续轮廓{ p}i、{ p}j、{ p}k的评价值g，

39、，

40、若g中最小值小于或等于预设最小阈值，则判断出三组连续轮廓{ p}i、{ p}j、{ p}k即为初步匹配得到的反光板轮廓；

41、若反光板轮廓存在遮挡或g中最小值大于预设最小阈值时，则重返步骤（1）。

42、以上设置，能够分别计算反光板的宽度误差、中心距离误差、间距误差以及强度误差，进而得到三组连续轮廓的评价值，然后将评价值与需要的最小阈值进行对比，若太大则表示其相差较大，从而舍弃掉，便能够通过评价值初步匹配反光板轮廓。

43、进一步的，所述步骤（5）中还包括：

44、预设以布尔值形式表示的栅格图，并以“”在栅格图中标记标准反光板形状，然后将初步匹配连续轮廓中数量为n的点云数据逐一代入栅格图中，

45、在步骤（5.1）中，若处于栅格图以外表示匹配失败，则返回步骤（1）；

46、在步骤（5.2）中，若处于栅格图以内，则进入以下步骤，其中m为点云命中栅格图中的标准反光板标记的数量，

47、（5.2.1）若命中栅格图中的“”标记，则命中数为：m=m+1，

48、（5.2.2）若未命中栅格图中的“”标记，则命中数为：m=m-1，

49、（5.2.3）计算连续轮廓 与标准反光板的外形相似度p，

50、，

51、当p大于或等于预设的阈值q时，则判断为匹配成功；否则判断为匹配失败，返回步骤（1）。

52、以上设置，能够进一步通过初步匹配后连续轮廓中的点云数据在栅格图中命中标记数来计算连续轮廓与标准反光板的外形相似度，进而判定二次匹配是否成功。

53、进一步的，所述步骤（6）中还包括：

54、在里程计参考坐标系内确定反光板拟合直线方程为：

55、，

56、其中参数a、b、c为ceres求解器的优化变量确定，然后反光板坐标系在里程计参考坐标系下的位姿，

57、，

58、其中 xc、 yc分别为反光板中心点c的坐标， xboard、 yboard分别为反光板坐标系原点坐标。

59、以上设置，通过在在里程计参考坐标系内通过ceres求解器获得反光板拟合直线方程，然后通过反光板拟合直线方程便能够计算出反光板坐标系在里程计参考坐标系的位姿。

60、进一步的，所述步骤（7）中还包括：

61、位姿变换矩阵的计算公式为：

62、。

63、以上设置，能够通过反光板坐标系与里程计坐标系变换矩阵和里程计坐标系关系与机器人坐标系变换矩阵计算得到机器人坐标系在反光板坐标系下的位姿变换矩阵。