一种自移动装置定位导航回充校准方法及系统与流程

本发明涉及智能机器人及定位，具体涉及一种自移动装置定位导航回充校准方法及系统。

背景技术：

1、自移动设备，即具备自主移动能力和执行工作任务的智能机器人，随着人工智能和机器人技术的飞速发展，自移动设备的应用日益广泛，已经成为现代生活和产业领域的重要一员，例如清扫机器人、送餐机器人、割草机等等。然而，由于自移动设备所配备的移动电源容量有限，随之而来的问题之一就是续航管理与电量补给，为了保证自移动设备的全自动化运行，自移动设备通常需要具备自主回充能力，即当自移动设备电量较低或完成作业时，自移动设备可以定位充电座的位置，自动返航至充电座充电。

2、自移动设备返航至充电座这一回充过程中依据自移动设备离充电座的远近可以将该回充分为远距离回充和近距离回充两个阶段；在远距离回充阶段自移动设备需要标记出充电座的大致位置，规划路径回到充电座附近，对于定位精度要求低；而近距离回充阶段自移动设备需要定位充电座的精确位置，依照预设路径移动至充电座指定位置完成对接，对于定位精度的要求高。

3、目前存在利用视觉导航、惯性导航方案引导自主回充的技术方案，但由于视觉导航和惯性导航均存在随时间累积误差的问题，导致自移动设备长时间作业后，定位出现无法纠正的偏差，只能满足远距离回充阶段的要求。

4、基于rtk(real-time kinematic))的定位方案与上述方案结合可以一定程度上解决累计误差的问题，但更多是作为自移动设备定位引导、修正误差的作用，进而提升视觉等回充对齐方案的成功率，但是在一些未覆盖rtk的区域还需要额外部署rtk参考站，整体成本较高；现有某些方案中还引入了多基站uwb tof(time offlight)定位导航技术，多基站uwb tof定位导航的方案虽然精度满足要求，但需要在自移动设备的工作场所部署多个基站，且需要根据场景调整部署策略，成本较高不利于应用推广。

5、目前近距离回充阶段常用的低成本自主回充技术手段是基于红外线信号引导自移动设备返回充电座，但红外传感器的发射角度小、易受环境因素影响，自移动设备与充电座的角度偏差稍大就容易无法找到充电座的位置，回充速度慢、精度也有限。

技术实现思路

1、本发明的目的在于，提供一种自移动装置定位导航回充校准方法及系统，以较低成本的方案解决相关技术中存在的自移动装置定位导航累积误差校正问题，同时将视觉技术与测距、测角技术融合，解决回充精度有限，重复动态调整，导致回充速度慢、甚至回充失败需要人为调整的问题。

2、第一方面，本发明提供了一种自移动装置定位导航回充校准方法，包括步骤：

3、s1：自移动装置移动至回充起始区域，获取所述自移动装置当前的第一环境图像，对比所述第一环境图像与预存的标准回充起始图像，判断两者相似度是否大于等于预设的第一阈值，若是，则认为所述自移动装置已位于标准回充起点并处于标准回充姿态，进行步骤s7；若否，则进行步骤s2；

4、s2：通过所述第一测量模块和所述第二测量模块获取所述自移动装置相对于所述充电座的第一角度，根据所述第一角度所述自移动装置原地旋转，调整姿态至标准回充姿态；

5、s3：通过所述第一测量模块和所述第二测量模块获取所述自移动装置与所述充电座的第一距离以及所述自移动装置与所述充电座形成的第二角度；根据所述第二角度，向目标路径方向移动第一路径距离至回充过渡点；获取当前所述自移动装置与所述充电座第二距离，与所述充电座形成的第三角度；

6、s4：获取所述自移动装置当前的第二环境图像，判断所述第二环境图像与所述标准回充起始图像的相似度是否大于等于所述第一阈值，若是，则认为所述自移动装置已位于标准回充起点并处于标准回充姿态，进行步骤s7；若否，则进行步骤s5；

7、s5：获取所述第二角度和所述第三角度的置信度，判断所述置信度是否小于预设的第二阈值，若是，则返回步骤s2；若否，则进行步骤s6；

8、s6：获取所述自移动装置应向所述目标路径方向移动的第二路径距离，移动所述第二路径距离至所述目标路径上，通过所述第一测量模块和所述第二测量模块获取所述自移动装置相对于所述充电座形成的第四角度，根据所述第四角度所述自移动装置原地旋转，调整姿态至标准回充姿态；

9、s7：所述自移动装置沿所述目标路径移动至所述充电座进行回充。

10、可选的，所述第一测量模块包括第一测角单元和第一测距单元；所述第二测量模块包括第二测角单元和第二测距单元；所述述第二测量模块的测角法向方向与充电座的回充方向有确定的相对关系；通过所述第一测角单元和所述第二测角单元获取所述自移动装置相对于所述充电座的角度；通过所述第一测距单元和所述第二测距单元获取所述自移动装置与充电座之间的距离。

11、可选的，所述第一测量模块为第一uwb模块，包括至少一个第一uwb收发器；所述第二测量模块为第二uwb模块，包括至少一个第二uwb收发器

12、可选的，所述自移动装置原地旋转，调整姿态至标准回充姿态的步骤包括：所述自移动装置原地旋转调整姿态至朝向所述充电座。

13、可选的，所述自移动装置朝向充电座的姿态与所述标准回充姿态间存在角度偏差，则所述自移动装置原地旋转调整姿态至朝向所述充电座后，再根据所述角度偏差旋转使自移动装置处于标准回充姿态。

14、可选地，所述步骤s2包括：通过所述第一测量模块和所述第二测量模块获取所述第一角度的初始值，以顺时针旋转为正方向，逆时针旋转为负方向；所述自移动装置旋转角度δ得到所述第一角度的旋转值，若所述第一角度的旋转值和所述第一角度的初始值的差值绝对值变大，则负方向旋转，否则继续同方向旋转，重复旋转操作直至找到所述第一角度的极小值，则认为所述自移动装置已调整姿态至朝向充电座。

15、可选地，通过所述第一测量模块和所述第二测量模块获取所述第四角度的初始值，以顺时针旋转为正方向，逆时针旋转为负方向；自移动装置旋转角度δ后得到所述第四角度的旋转值，若所述第四角度的旋转值和所述第四角度的初始值的差值绝对值变大，则负方向旋转，否则继续同方向旋转，重复旋转操作直至找到所述第四角度的极小值，则认为所述自移动装置已调整姿态至朝向充电座。

16、可选的，所述获取所述第二角度和所述第三角度的置信度的步骤包括：

17、根据所述第一距离、第二距离以及第一路径距离，获取角度α’，公式为：

18、δd2＝d12+d02-2d1d0cosα’

19、

20、其中，d0为第一距离，d1为第二距离，δd为第一路径距离；

21、根据所述第二角度、第三角度，获取角度α”，公式为：

22、α”＝α0-α1

23、其中，α0为第二角度，α1为第三角度

24、根据所述角度α’和角度α”，获取所述第二角度的置信度c，公式为：

25、c＝|aα”-bα’|

26、其中，a为角度α”权重值、b为角度α’的权重值。

27、可选的，所述步骤s6还包括：

28、根据所述第一距离、所述第一路径距离、所述第二距离、所述第三角度以及所述角度α”获取所述第二路径距离，公式为：

29、

30、α3＝π-α1-α2

31、

32、

33、其中，d2表示第二路径距离。

34、可选的，所述步骤s6还包括：根据所述第一距离、所述第一路径距离、所述第二距离、所述第三角度以及所述角度α’获取所述第二路径距离，公式为：

35、

36、α3＝π-α1-α2

37、

38、

39、其中，d2表示第二路径距离。

40、可选的，所述自移动装置设有图像采集模块，所述步骤s1还包括：所述自移动装置移动至回充起始区域前，通过所述图像采集模块和所述第一测量模块进行所述自移动装置的定位和导航。

41、可选的，设定校正区域和校正阈值，所述步骤s1之前还包括步骤：

42、获取自移动装置当前累积移动距离、通过所述第一测量模块和所述第二测量模块获取所述自移动装置与充电座之间的校正起始距离；判断累积移动距离值和校正起始距离值之和是否大于所述校正阈值，若是，则所述自移动装置向所述校正区域移动；

43、判断所述自移动装置是否进入所述校正区域；若是，通过所述第一测量模块和所述第二测量模块多次获取所述自移动装置当前位置与所述充电座之间的校正距离和校正角度，根据多个所述校正距离和所述校正角度的平均值，获取所述自移动装置相对于所述充电座的位置坐标；

44、根据所述位置坐标校正所述自移动装置的位置，清除所述自移动装置当前累积误差。

45、可选的，所述校正区域的范围为：与所述充电座之间沿回充方向的距离小于等于d1及与所述充电座回充方向形成角度绝对值小于等于θ1的点形成的平面；所述判断所述自移动装置是否进入所述校正区域包括：获取所述自移动装置当前位置与充电座之间的角度和距离，若所述角度绝对值小于等于θ1，所述距离小于等于d1，则所述自移动装置位于校正区域内。

46、可选的，所述校正区域范围大于所述回充起始区域，所述回充起始区域内的点沿回充方向距离充电座长度小于等于d2，与充电座回充方向形成角度绝对值小于等于θ2；其中d2≤d1，θ2≤θ1。

47、可选的，所述校正区域和所述回充起始区域在uwb信号覆盖范围内。

48、第二方面，本发明还提供了一种自移动装置定位导航回充校准系统，所述系统包括自移动装置和充电座，其中，所述自移动装置包括：图像采集模块，第一测量模块以及主控模块；所述充电座包括第二测量模块；

49、所述图像采集模块用于获取所述自移动装置当前的第一环境图像和第二环境图像，并将所述第一环境图像和所述第二环境图像发送给所述主控模块；

50、所述第一测量模块，用于配合所述第二测量模块获取第一距离、第二距离，第一角度、第二角度、第三角度以及第四角度，并发送给所述主控模块；

51、所述第二测量模块，用于配合所述第一测量模块获取所述自移动装置与所述充电座的所述第一距离、所述第二距离、所述第一角度、所述第二角度、所述第三角度以及所述第四角度；；

52、所述主控模块预先存储有标准回充起始图像，用于对比判断所述第一环境图像、所述第二环境图像与所述标准回充起始图像的相似度，控制所述自移动装置旋转，移动，获取所述自移动装置向目标路径方向移动第一路径距离以及所述自移动装置应向所述目标路径方向移动的第二路径距离。

53、可选的，所述图像采集模块还用于获取自移动装置当前累积移动距离；

54、所述第一测量模块还用于，配合所述第二测量模块获取校正起始距离、校正距离以及校正角度，并发送给所述主控模块；

55、所述第二测量模块还用于，配合所述第一测量模块获取所述自移动装置与所述充电座之间的校正起始距离、校正距离以及校正角度；

56、所述主控单元还用于判断累积移动距离值和校正起始距离值之和是否大于所述校正阈值，若是，则控制所述自移动装置向校正区域移动；判断所述自移动装置是否进入所述校正区域，若是，根据多个所述校正距离和所述校正角度的平均值，获取所述自移动装置相对于所述充电座的位置坐标；根据所述位置坐标校正所述自移动装置的位置，清除所述自移动装置当前累积误差以及进行路径规划。

57、可选的，所述第一测量模块包括第一测距单元和第二测角单元；所述第二测量模块包括第二测距单元和第二测角单元；所述第一测距单元和第二测距单元用于配合测量所述自移动装置与所述充电座之间的距离，所述第一测角单元和所述第二测角单元用于配合测量自移动装置与充电座之间的角度。

58、可选的，所述第一测量模块为第一uwb模块，包括至少一个第一uwb收发器；所述第二测量模块为第二uwb模块，包括至少一个第二uwb收发器。

59、相比于现有技术，本发明所提供的自移动装置定位导航回充校准方法及系统在所述自移动装置设置所述第一测量模块，在所述充电座上设置所述第二测量模块；在所述自移动装置位于所述回充起始区域时，通过所述第一测量模块和所述第二测量模块实时获取所述自移动装置相对于所述充电座角度和距离，根据获取的所述自移动装置相对于所述充电座的角度，调整所述自移动装置的姿态至朝向充电座；根据获取的所述自移动装置与所述充电座的距离，控制所述自移动装置向标准回充起点移动，使所述自移动装置位于回充目标路径上，当所述自移动装置已位于标准回充起点并处于标准回充姿态则沿目标路径移动至充电座进行回充。本发明所提供的实施例仅通过第一测量模块和第二测量模块即可完成较高精度的回充，不用部署额外的基站，降低了回充工作场所的部署难度和成本。同时，通过判断自移动装置和充电座之间角度的置信度还可以简单判断自移动装置和充电座之间是否存在障碍物。

60、进一步的，通过设置校正区域和校正阈值，在定位导航的误差累积到一定门限时(超过校正阈值)，自移动装置能够多次运行到校正区域，只需要在进入校正区域之后即进行位置校正，进而可以更加有效的实现自移动装置整体的运动轨迹的规划，提高自移动装置整体工作的效率和速度。