一种多楼层粒子滤波定位方法、装置及机器人与流程

本发明涉及机器人，具体来说，涉及一种多楼层粒子滤波定位方法、装置及机器人。

背景技术：

1、目前有大量的巡检机器人在高层楼宇进行巡逻巡检工作，但之前室内巡检机器人基本上工作环境是单个平面层，不存在巡检过程中涉及到地图的切换问题。在高层楼宇巡检机器人当巡检完2楼，呼叫电梯，进入3楼巡检，那么就需要涉及到地图切换，重新给定机器人初始位置。

2、针对楼层间切换地图，当前的技术方案是在部署巡检机器人之前，会对每一层地图中提前录取电梯中间在地图中的位置，然后切换逻辑是机器人进入到电梯，识别当前电梯号，提取录取好的位置点，然后用这个位置点初始化机器人。

3、但目前的技术方案存在如下缺陷：第一是每一层都需要人工提前录点；第二个是机器人进入电梯由于电梯内可能有人，机器人在电梯里面停的位置可能每一次都不一样；第三机器人还得增加电梯号识别(两台电梯以上)，带来麻烦。

4、本文提供的背景描述用于总体上呈现本公开的上下文的目的。除非本文另外指示，在该章节中描述的资料不是该申请的权利要求的现有技术并且不要通过包括在该章节内来承认其成为现有技术。

技术实现思路

1、针对相关技术中的上述技术问题，本发明提出一种多楼层粒子滤波定位方法，其包括如下步骤：

2、s1，机器人进入电梯，切换到下一层导航地图；其中所机器人预先构建的地图的起始点均设置在电梯厢中心；

3、s2，在接收到下一层导航地图时，保存当前时刻机器人在第一地图的位姿p1；

4、s3，将激光扫描帧scan_c经机器人位姿p1转换到第一地图坐标系上获得所述激光扫描帧scan_c在第一地图上的投影地图scan_m；遍历scan_m里所有点，并找到与该点最近的地图上被占据的栅格点；并计算激光帧扫描帧在第一地图坐标系的投影scan_m与第一地图的重合度得分；

5、s4，计算用第一地图上的位姿p1平替当前时刻机器人在下一层地图上的位姿时存在的误差std；

6、s5，粒子重生成，以均值为位姿p1，方差为std的高斯分布，生成第一预设个位姿粒子；并使用生成的第一预设个位姿粒子替换定位程序里面当前的粒子。

7、具体的，所述预先构建地图的步骤如下：

8、s01，手动控制机器人到电梯厢内中心，机器人正朝向电梯内；

9、s02，开始扫描地图，手动操作机器人对当前楼层进行正常扫图；

10、s03，当前楼层地图扫描完成，保存当前楼层的地图；

11、s04，控制电梯进入下一层，按步骤s01-s03，将下一层地图扫描完成；

12、s05，循环执行步骤s04，直至所有楼层地图扫描完成。

13、具体的，所述计算重合度得分如下：

14、

15、n—scan_m里点的总数

16、[pj,mj]—对应最近点点对

17、其中p点为激光帧在第一地图坐标系的投影scan_m中任一点；m表示第一地图上的一点，此点最靠近p点，叫做p的地图上最近点，使用查最近点函数来获取p点的最近点m；(pjx，pjy)及(mjx，mjy)分别表示第j个p，m

18、点在地图上的x，y坐标。

19、具体的，所述误差std计算方法如下：

20、std＝(1-score_c)/r

21、其中r为地图栅格的分辨率大小。

22、第二方面，本发明的另一个实施例公开了一种多楼层粒子滤波定位装置，其包括如下单元：

23、导航地图切换单元，用于机器人进入电梯，切换到下一层导航地图；其中所机器人预先构建的地图的起始点均设置在电梯厢中心；

24、位姿获取单元，用于在接收到下一层导航地图时，保存当前时刻机器人在第一地图的位姿p1；

25、重合度计算单元，用于将激光扫描帧scan_c经机器人位姿p1转换到第一地图坐标系上获得所述激光扫描帧scan_c在第一地图上的投影地图scan_m；遍历scan_m里所有点，并找到与该点最近的地图上被占据的栅格点；并计算激光帧扫描帧在第一地图坐标系的投影scan_m与第一地图的重合度得分；

26、误差计算单元，用于计算用第一地图上的位姿p1平替当前时刻机器人在下一层地图上的位姿时存在的误差std；

27、粒子更新单元，用于粒子重生成，以均值为位姿p1，方差为std的高斯分布，生成第一预设个位姿粒子；并使用生成的第一预设个位姿粒子替换定位程序里面当前的粒子。

28、具体的，还包括：

29、机器人朝向设置单元，用于手动控制机器人到电梯厢内中心，机器人正朝向电梯内；

30、扫图单元，用于开始扫描地图，手动操作机器人对当前楼层进行正常扫图；

31、地图保存单元，用于当前楼层地图扫描完成，保存当前楼层的地图；

32、下一层地图扫描单元，用于控制电梯进入下一层，重复执行机器人朝向设置单元、扫图单元、地图保存单元，将下一层地图扫描完成。

33、具体的，所述计算重合度得分如下：

34、

35、n—scan_m里点的总数

36、[pj,mj]—对应最近点点对

37、其中p点为激光帧在第一地图坐标系的投影scan_m中任一点；m表示第一地图上的一点，此点最靠近p点，叫做p的地图上最近点，使用查最近点函数来获取p点的最近点m；(pjx，pjy)及(mjx，mjy)分别表示第j个p，m

38、点在地图上的x，y坐标。

39、具体的，所述误差std计算方法如下：

40、std＝(1-score_c)/r

41、其中r为地图栅格的分辨率大小。

42、第三方面，本发明的另一个实施例公开了一种非易失性存储器，所述存储器上存储有指令，所述指令被处理器执行时，用于实现如上述的一种多楼层粒子滤波定位方法。

43、第四方面，本发明的另一个实施例公开了一种机器人，所述机器人包括：一处理模块，一底盘，一存储模块，一激光雷达，所述存储模块存储有指令，在所述指令被执行时，用于实现如上述的一种多楼层粒子滤波定位方法。

44、本发明通过构建各楼层地图时起点和方向一致，致使各楼层地图理论上是在同一坐标系下表示，可以避免现有方案的三个缺陷，第一是每一层都需要人工提前录点；第二个是机器人进入电梯由于电梯内可能有人，机器人在电梯里面停的位置可能每一次都不一样；第三机器人还得增加电梯号识别(两台电梯以上)，带来麻烦。进一步的，但每一层地图在扫描构建过程中肯定存在随机误差，并不能直接平替定位信息，本发明引入了粒子重分布策略，保证定位更鲁棒。

技术特征：

1.一种多楼层粒子滤波定位方法，其包括如下步骤：

2.根据权利要求1所述的方法，所述预先构建地图的步骤如下：

3.根据权利要求1所述的方法，所述计算重合度得分如下：

4.根据权利要求3所述的方法，所述误差std计算方法如下：

5.一种多楼层粒子滤波定位装置，其包括如下单元：

6.根据权利要求5所述的装置，还包括：

7.根据权利要求5所述的装置，所述计算重合度得分如下：

8.根据权利要求7所述的装置，所述误差std计算方法如下：

9.一种非易失性存储器，所述存储器上存储有指令，所述指令被处理器执行时，用于实现如权利要求1-4中任一项所述的一种多楼层粒子滤波定位方法。

10.一种机器人，所述机器人包括：一处理模块，一底盘，一存储模块，一激光雷达，所述存储模块存储有指令，在所述指令被执行时，用于实现如权利要求1-4中任一项所述的一种多楼层粒子滤波定位方法。

技术总结
本发明提供了一种多楼层粒子滤波定位方法，S1，机器人进入电梯，切换到下一层导航地图；S2，保存当前时刻机器人在第一地图的位姿P1；S3，将激光扫描帧Scan_c经机器人位姿P1转换到第一地图坐标系上获得投影地图Scan_m；计算激光帧扫描帧在第一地图坐标系的投影Scan_m与第一地图的重合度得分；S4，计算用第一地图上的位姿P1平替当前时刻机器人在下一层地图上的位姿时存在的误差std；S5，粒子重生成，以均值为位姿P1，方差为std的高斯分布，生成第一预设个位姿粒子；并使用生成的第一预设个位姿粒子替换定位程序里面当前的粒子。

技术研发人员：袁国斌,柏林,刘彪,舒海燕,袁添厦,祝涛剑,沈创芸,王恒华,方映峰
受保护的技术使用者：广州高新兴机器人有限公司
技术研发日：
技术公布日：2024/1/14