CN115502971A - 一种应对定位切换跳变的导航对接方法、系统以及设备 - Google Patents

一种应对定位切换跳变的导航对接方法、系统以及设备 Download PDF

Info

Publication number
CN115502971A
CN115502971A CN202211124837.8A CN202211124837A CN115502971A CN 115502971 A CN115502971 A CN 115502971A CN 202211124837 A CN202211124837 A CN 202211124837A CN 115502971 A CN115502971 A CN 115502971A
Authority
CN
China
Prior art keywords
coordinate system
robot
point
coordinate
coordinates
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202211124837.8A
Other languages
English (en)
Other versions
CN115502971B (zh
Inventor
娄诗烨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Lanxin Technology Co ltd
Original Assignee
Hangzhou Lanxin Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Lanxin Technology Co ltd filed Critical Hangzhou Lanxin Technology Co ltd
Priority to CN202211124837.8A priority Critical patent/CN115502971B/zh
Publication of CN115502971A publication Critical patent/CN115502971A/zh
Application granted granted Critical
Publication of CN115502971B publication Critical patent/CN115502971B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)

Abstract

本发明涉及一种应对定位切换跳变的导航对接方法、系统以及设备，其方法包括：在移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标；基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，控制所述移动机器人执行对接任务。本发明提出一种定位来源自动切换的方案，通过保证当前切换点坐标一致的方式，保证定位来源切换过程中，机器人定位无跳变。定位切换过程中，通过坐标补偿计算，无论定位模式是什么，机器人始终获得的是全局坐标系的定位值，借此可以实现更精准的机器人坐标上报，给机器人调度系统提供准确信息。

Description

一种应对定位切换跳变的导航对接方法、系统以及设备
技术领域
本发明涉及机器人定位技术领域，尤其涉及一种应对定位切换跳变的导航对接方法、系统以及设备。
背景技术
工业上使用的室内移动机器人按照用途大概有用于背负货架的潜伏式机器人，用于直接和码头自动化对接的辊筒式机器人，用于背负栈板的叉车机器人，以及各类针对具体行业和工艺设计的特种机器人，如软包上下料机器人，料箱机器人，复合机械臂机器人等。
一般来说，移动机器人在上下料过程中，都需要实现高精度对接工作。非特殊要求的情况下，潜伏式机器人进行货架对接时需要保证5mm左右的对接精度；辊筒式机器人在进行辊筒码头对接时，需要保证5mm左右的对接精度，同时保证机器人和码头间隙小于8cm；叉车机器人在进行栈板对接时，需要保证12mm左右的对接精度；软包上下料机器人机器人需要保证5mm左右的对接精度；料箱机器人需要保证前后5mm左右的对接精度等。
在移动机器人实际应用过程中，为实现高精度对接，需要在多种不同精度下的定位模式之间进行切换。又由于每个定位模式都有自己的坐标系定义，在定位切换过程中存在定位跳变问题。定位跳变如果发生在移动机器人的移动过程中，必然导致移动机器人的控制跳变，甚至失控。
发明内容
(一)要解决的技术问题
鉴于现有技术的上述缺点、不足，本发明提供一种应对定位切换跳变的导航对接方法、系统以及设备，其解决了现有移动机器人在定位模式切换时控制跳变与失控的技术问题。
(二)技术方案
为了达到上述目的，本发明采用的主要技术方案包括：
第一方面，本发明实施例提供一种应对定位切换跳变的导航对接方法，包括：
在依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标；
基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，以控制所述移动机器人执行对接任务；
其中，所述定位模式包括：定位精度为±2cm的全局定位模式、定位精度为±8mm的局部定位模式以及定位精度为±1mm的高精度定位模式。
可选地，在依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标包括：
获取包含对接预备点与对接点的对接任务；
在移动机器人接近对接任务指示的对接预备点时，将移动机器人的全局定位模式切换至局部定位模式，同时在局部定位模式下控制移动机器人抵达对接预备点；
将获取的局部定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人局部转换坐标与对接点局部转换坐标，并重新规划驶向对接点局部转换坐标的路径；
在移动机器人接近对接点局部转换坐标时，将移动机器人的局部定位模式切换至高精度定位模式；
将获取的高精度定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人高精度转换坐标与对接点高精度转换坐标。
可选地，在移动机器人接近对接任务指示的对接预备点时，将移动机器人的全局定位模式切换至局部定位模式，同时在局部定位模式下控制移动机器人抵达对接预备点包括：
控制移动机器人在全局定位模式下朝对接预备点行进；
在移动机器人距离对接预备点小于d1＝0.2m时，将移动机器人的全局定位模式切换至局部定位模式，并控制移动机器人抵达对接预备点；
获取局部定位模式的坐标系下的机器人坐标LP(xl yl θl)、对接预备点坐标LP1(xl1 yl1 θl1)以及对接点坐标LP2(xl2 yl2 θl2)，θ为角度值；
相应地，移动机器人接近对接点局部转换坐标的距离为d2＝0.1m。
可选地，将获取的局部定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人局部转换坐标与对接点局部转换坐标，并重新规划驶向对接点局部转换坐标的路径包括：
依据获取的全局定位模式的坐标系下的机器人当前坐标TMP(xtm ytm θtm)与局部定位模式的坐标系下的机器人当前坐标TLP(xtl ytl θtl)，通过式(1)将所述将局部定位模式的坐标系下的任意机器人坐标LP(xl yl θl)转换成全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)；
Figure BDA0003848098700000031
Figure BDA0003848098700000032
依据获取的全局定位模式的坐标系下的机器人当前坐标TMP(xtm ytm θtm)与局部定位模式的坐标系下的机器人当前坐标TLP(xtl ytl θtl)，通过式(2)将所述局部定位模式下的对接点坐标LP2(xl2 yl2 θl2)变换为全局定位模式的坐标系下的对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)：
Figure BDA0003848098700000041
Figure BDA0003848098700000042
式(1)与式(2)中，ΔLp为局部定位模式的坐标系下的机器人任意坐标相对于局部定位模式的坐标系下机器人当前坐标的偏差，xΔlp、yΔlp、θΔlp则分别为x、y、θ坐标下的偏差；
依据全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)与对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)，规划如下的第一5阶贝塞尔曲线B(t)；
Figure BDA0003848098700000043
Figure BDA0003848098700000051
Figure BDA0003848098700000052
Figure BDA0003848098700000053
Figure BDA0003848098700000054
Figure BDA0003848098700000055
Figure BDA0003848098700000056
式(3)与式(4)中，LPC(xlc ylc θlc)为当前出发点，LP2(xl2 yl2 θl2)为当前目标点，都是由外部输入，p1…p4为控制点由下述公式计算得出，pt表示目标点与出发点的坐标差值，pd表示转换到出发点坐标系下的pt表示，xt表示目标点和出发点在世界坐标系下的x轴方向差值，yt表示目标点和出发点在世界坐标系下的y轴方向差值，θt表示目标点和出发点在世界坐标系下的角度差值，xd表示出发点坐标系下的目标点x坐标值，yd表示出发点坐标系下的目标点y坐标值，θd表示出发点在世界坐标系下的角度值。
可选地，将获取的高精度定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人高精度转换坐标与对接点高精度转换坐标包括：
依据获取的全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)与高精度定位模式的坐标系下的机器人当前坐标TQP(xtq ytq θtq)，通过式(5)将任意高精度定位模式的坐标系下的机器人坐标QP(xq yq θq)转换成全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)；
Figure BDA0003848098700000061
Figure BDA0003848098700000062
依据获取的全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)与高精度定位模式的坐标系下的机器人当前坐标TQP(xtq ytq θtq)，通过式(6)将高精度定位模式的坐标系下的对接点坐标QP2(xq2 yq2 θq2)转换为全局定位模式的坐标系下的对接点高精度转换坐标QPC2(xqc2 yqc2 θqc2)；
Figure BDA0003848098700000063
Figure BDA0003848098700000064
式(5)与式(6)中，ΔQp为高精度定位模式的坐标系下的机器人任意坐标相对于高精度定位模式的坐标系下机器人当前坐标的偏差，xΔqp、yΔqp、θΔqp则分别为x、y、θ坐标下的偏差。
可选地，基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，以控制所述移动机器人执行对接任务包括：
依据全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)与对接点高精度转换坐标QPC2(xqc2 yqc2 θqc2)，通过式(7)得到对接点QPC3(xqc3 yqc3 θqc3)；
Figure BDA0003848098700000071
Figure BDA0003848098700000072
依据全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)与对接点QPC3(xqc3 yqc3 θqc3)，规划出如下的控制轨迹B2(t)：
B2(t)＝QPC+(QPC3-QPC)*t,t∈[0,1] (8)
在基于控制轨迹B2(t)控制移动机器人到达对接点QPC3(xqc3 yqc3 θqc3)时，依据高精度模式下的机器人实时坐标QP(xq yq θq)与对接点高精度转换坐标QP2(xq2 yq2 θq2)，得到如下机器人控制误差DP(xd yd θd)；
Figure BDA0003848098700000073
判断所述机器人控制误差是否满足如下阈值要求：
Figure BDA0003848098700000074
若机器人控制误差DP(xd yd θd)满足阈值要求，则将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作；
若机器人控制误差DP(xd yd θd)不满足阈值要求，则调整移动机器人的位置来满足阈值要求，再将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作。
可选地，若机器人控制误差不满足阈值要求，则调整移动机器人的位置来满足阈值要求，再将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作包括：
将高精度定位模式的坐标系下的当前机器人坐标跳变为局部定位模式的坐标系下的机器人当前跳变坐标LP3(xl3 yl3 θl3)，并以局部定位模式的坐标系下的对接预备点坐标LP1(xl1 yl1 θl1)为移动目标点；
通过式(11)将局部定位模式的坐标系下的对接坐标LP1(xl1 yl1 θl1)与机器人当前跳变坐标LP3(xl3 yl3 θl3)分别进行变换为LP4(xl4 yl4 θl4)与LP5(xl5 yl5 θl5)；
Figure BDA0003848098700000081
Figure BDA0003848098700000082
依据LP4(xl4 yl4 θl4)与LP5(xl5 yl5 θl5)规划第三5阶贝塞尔曲线B3(t)，并基于第三5阶贝塞尔曲线B3(t)调整移动机器人的位置来满足阈值要求；
Figure BDA0003848098700000083
Figure BDA0003848098700000091
Figure BDA0003848098700000092
Figure BDA0003848098700000093
Figure BDA0003848098700000094
Figure BDA0003848098700000095
Figure BDA0003848098700000096
式(13)与式(14)中，LP5(xl5 yl5 θl5)为当前出发点，LP4(xl4 yl4 θl4)为当前目标点，都是由外部输入，p31…p34为控制点由下述公式计算得出，P3t表示目标点与出发点的坐标差值，P3d表示转换到出发点坐标系下的P3t表示，x3t表示目标点和出发点在世界坐标系下的x轴方向差值，y3t表示目标点和出发点在世界坐标系下的y轴方向差值，θ3t表示目标点和出发点在世界坐标系下的角度差值，x3d表示出发点坐标系下的目标点x坐标值，y3d表示出发点坐标系下的目标点y坐标值，θ3d表示出发点在世界坐标系下的角度值；
在调整移动机器人的位置之后，依据全局定位模式的坐标系下的实时坐标LP6(xl6yl6 θl6)和全局定位模式的坐标系下的对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)规划第四5阶贝塞尔曲线B4(t)，控制移动机器人行进使移动机器人完成对接任务；
Figure BDA0003848098700000101
Figure BDA0003848098700000102
Figure BDA0003848098700000103
Figure BDA0003848098700000104
Figure BDA0003848098700000105
Figure BDA0003848098700000106
Figure BDA0003848098700000107
式(15)与式(16)中，LP6(xl6 yl6 θl6)为当前出发点，LPC2(xlc2 ylc2 θlc2)为当前目标点，都是由外部输入，p41…p44为控制点由下述公式计算得出，P4t表示目标点与出发点的坐标差值，P4d表示转换到出发点坐标系下的P4t表示，x4t表示目标点和出发点在世界坐标系下的x轴方向差值，y4t表示目标点和出发点在世界坐标系下的y轴方向差值，θ4t表示目标点和出发点在世界坐标系下的角度差值，x4d表示出发点坐标系下的目标点x坐标值，y4d表示出发点坐标系下的目标点y坐标值，θ4d表示出发点在世界坐标系下的角度值。
第二方面，本发明实施例提供一种应对定位切换跳变的导航对接系统，包括：
多模式定位模块，用于依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换；
坐标切换模块，用于在依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标；
轨迹规划模块，用于基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，以控制所述移动机器人执行对接任务。
第三方面，本发明实施例提供一种多定位模式自动切换的导航对接方法，包括：
在执行移动机器人的对接任务时，通过切换各种定位精度的定位模式来实现移动机器人的高精度对接；
其中，在进行定位模式切换时执行如上所述的一种应对定位切换跳变的导航对接方法。
第四方面，本发明实施例提供一种应对定位切换跳变的导航对接设备，其特征在于，包括：导航控制器，所述导航控制器用于对移动机器人执行如上所述的一种应对定位切换跳变的导航对接方法以及如上所述的一种多定位模式自动切换的导航对接方法。
(三)有益效果
本发明的有益效果是：本发明提出一种定位来源自动切换的方案，通过保证当前切换点坐标一致的方式，保证定位来源切换过程中，机器人定位无跳变。定位切换过程中，通过坐标补偿计算，无论定位模式是什么，机器人始终获得的是全局坐标系的定位值，可以实现更精准的机器人坐标上报，给机器人调度系统提供准确信息。
附图说明
图1为本发明实施例提供的一种应对定位切换跳变的导航对接方法的流程示意图；
图2为本发明实施例提供的一种应对定位切换跳变的导航对接方法的步骤S1的具体流程示意图；
图3为本发明实施例提供的一种应对定位切换跳变的导航对接方法的步骤S13的具体流程示意图；
图4中的(a)、(b)、(c)为本发明实施例提供的一种应对定位切换跳变的导航对接方法的全局定位坐标示意图、切换局部定位坐标示意图以及通过目标点坐标修改而保持定位不跳变示意图；
图5为本发明实施例提供的一种应对定位切换跳变的导航对接方法的步骤S15的具体流程示意图；
图6为本发明实施例提供的一种应对定位切换跳变的导航对接方法的步骤S2的具体流程示意图；
图7为本发明实施例提供的一种应对定位切换跳变的导航对接方法的步骤S225B的具体流程示意图；
图8为本发明实施例提供的一种应对定位切换跳变的导航对接方法的对接重试示意图；
图9中(a)、(b)、(c)为本发明实施例提供的一种应对定位切换跳变的导航对接方法的全局定位模式下的各坐标示意图、局部定位模式下的各坐标示意图以及其他定位模式下的坐标转换至全局定位模式的各坐标示意图。
具体实施方式
为了更好地解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。
移动机器人定位来源可以有很多，如激光定位，朝上视觉定位，朝下视觉定位，二维码定位，激光模板定位，激光货架腿定位等。按照用户用途来分，可以分为：
1、全局定位：部署快捷，只需要自动化或者半自动化的扫图作业后即可完成部署。对于环境依赖小，可以在任意自然环境下进行部署，同时对环境的部分变动不敏感。一般来说，常用的工业移动机器人室内定位方法有二维码导航，磁条导航，激光导航，视觉导航等，由于激光导航具有适用范围广，部署柔性且迅速，使用稳定性高，算力要求低，技术相对成熟等优点，被大量用于移动机器人的全局定位。但是目前的激光定位技术的定位精度在正负2cm左右，无法满足上述的各种对接精度要求。
2、局部定位：针对目标的定位，考虑机器人和目标之间的相对关系，对于目标所在全局位姿不敏感(例如货架腿对接，货架放歪了依旧可以完成机器人和货架相对精度较高的对接)。无法全局定位，只能目标在传感器视野范围内进行定位。对于目标有部署要求，目标模型需要提前人为编辑输入，假设目标模型变化后需要重新输入。一般来说，常用的工业移动机器人局部定位方法有，货架腿定位，激光模板定位，前向视觉TAG定位等。潜伏式车常用的是货架腿定位，辊筒车常用的是激光模板定位，这种定位方式通过预设目标模板，通过传感数据匹配目标模板的方式获取移动机器人相对于模板的相对位姿。一方面比全局定位的建图法具有更高的定位精度，另一方面可以适应可活动的目标模板。但是局部定位方法顾名思义是相对于对接目标点的局部坐标系定位方法，无法替代全局定位方法，也无法输出全局定位坐标结果。局部定位的精度中等，一般局部定位的产品化定位精度在±8mm左右。
3、高精度定位：需要张贴二维码等固定标签，因此需要一定的环境改造。一般来说高精度定位有效区域很小，只能通过增加二维码张贴的数量来提高定位有效区域。一般来说，常用的工业移动机器人高精度定位方法有二维码定位，向上视觉TAG定位等。高精度定位方法一般来说精度比较高，可以实现高精度的对接需求。但是定位范围比较小，对于初始偏差较大的场景，难以提供足够的调整距离。而且同样只能提供局部坐标系下的定位结果，无法输出全局定位结果。高精度的定位精度高，一般高精度定位的产品化定位精度在±1mm左右。
由于各自定位模式的优缺点，因此在移动机器人实际应用过程中，需要切换各种定位来源。一次典型的高精度对接过程，需要经历如下步骤
a)通过全局定位移动到预备对接点；
b)切换到局部定位，通过局部定位移动到目标点；
c)切换到高精度定位，通过高精度定位实现毫米级对接。
在以一次滚筒对接为例的具体对比例中，移动机器人使用全局定位导航到预备对接点，停车开启局部定位，坐标系从全局地图坐标系改成局部定位坐标系，使用预设的局部定位坐标对接点作为目标点开始导航，导航到局部定位坐标对接点，停车开启高精度定位，坐标系从局部定位坐标系改成高精度定位坐标系,使用预设的高精度定位坐标对接点作为目标点开始导航，导航到高精度定位坐标对接点，停车校验对接结果，成功继续，失败则报错。
如图1所示，本发明实施例提出的一种应对定位切换跳变的导航对接方法，包括：首先，在移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标；其次，基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，控制移动机器人执行对接任务。
为了更好地理解上述技术方案，下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更清楚、透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
具体地，本发明实施例提供一种应对定位切换跳变的导航对接方法，其包括：
S1、在移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标。
如图2所示，步骤S1包括：
S11、获取包含对接预备点与对接点的对接任务。
S12、在移动机器人接近对接任务指示的对接预备点时，将移动机器人的全局定位模式切换至局部定位模式，同时在局部定位模式下控制移动机器人抵达对接预备点。
步骤S12包括：
S121、控制移动机器人在全局定位模式下朝对接预备点行进。移动机器人在全局定位模式的坐标系下的坐标为MP(xm ym θm)，对接预备点坐标为MP1(xm1 ym1 θm1)，对接点坐标为MP2(xm2 ym2 θm2)。移动机器人收到一个精确对接任务后首先移动到对接预备点MP1(xm1ym1 θm1)。
S122、在移动机器人距离对接预备点小于d1＝0.2m时，将移动机器人的全局定位模式切换至局部定位模式，并控制移动机器人抵达对接预备点。
S123、获取局部定位模式的坐标系下的机器人坐标LP(xl yl θl)、对接预备点坐标LP1(xl1 yl1 θl1)以及对接点坐标LP2(xl2 yl2 θl2)，θ为角度值。
S13、将获取的局部定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人局部转换坐标与对接点局部转换坐标，并重新规划驶向新的对接点局部转换坐标的路径。
进一步地，如图3所示，步骤S13包括：
S131、机器人到达对接预备点后，得到全局定位模式的坐标系下的机器人当前坐标TMP(xtm ytm θtm)和局部定位模式的坐标系下的机器人当前坐标TLP(xtl ytl θtl)，据此通过式(1)将将局部定位模式的坐标系下的机器人坐标LP(xl yl θl)转换成全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)。
Figure BDA0003848098700000161
Figure BDA0003848098700000162
S132、依据获取的全局定位模式的坐标系下的机器人当前坐标TMP(xtm ytm θtm)与局部定位模式的坐标系下的机器人当前坐标TLP(xtl ytl θtl)，通过式(2)将局部定位模式下的对接点坐标LP2(xl2 yl2 θl2)变换为全局定位模式的坐标系下的对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)。
Figure BDA0003848098700000163
Figure BDA0003848098700000164
式(1)与式(2)中，ΔLp为局部定位模式的坐标系下的机器人任意坐标相对于局部定位模式的坐标系下机器人当前坐标的偏差，xΔlp、yΔlp、θΔlp则分别为x、y、θ坐标下的偏差。
S133、依据全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)与对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)，规划如下的第一5阶贝塞尔曲线B(t)。
Figure BDA0003848098700000171
Figure BDA0003848098700000181
Figure BDA0003848098700000182
Figure BDA0003848098700000183
Figure BDA0003848098700000184
Figure BDA0003848098700000185
Figure BDA0003848098700000186
式(3)与式(4)中，LPC(xlc ylc θlc)为当前出发点，LP2(xl2 yl2 θl2)为当前目标点，都是由外部输入，p1…p4为控制点由下述公式计算得出，pt表示目标点与出发点的坐标差值，pd表示转换到出发点坐标系下的pt表示，xt表示目标点和出发点在世界坐标系下的x轴方向差值，yt表示目标点和出发点在世界坐标系下的y轴方向差值，θt表示目标点和出发点在世界坐标系下的角度差值，xd表示出发点坐标系下的目标点x坐标值，yd表示出发点坐标系下的目标点y坐标值，θd表示出发点在世界坐标系下的角度值。
参考图4中的(a)所示，本发明实施例首先在全局定位模式下获得机器人当前坐标TMP(xtm ytm θtm)、对接点坐标为MP2(xm2 ym2 θm2)；其次，参考图4中的(b)，将全局定位模式切换至局部定位模式之后，获得坐标系下的机器人坐标LP(xl yl θl)、对接点坐标LP2(xl2yl2 θl2)；接着，参考图4中的(c),以保证LP＝TMP为目标，重新规划出第一5阶贝塞尔曲线B(t)。以此，完成了通过目标点坐标修改而保持定位不跳变。
S14、在移动机器人接近对接点局部转换坐标时，将移动机器人的局部定位模式切换至高精度定位模式。相应地，移动机器人接近对接点局部转换坐标的距离为d2＝0.1m。
S15、将获取的高精度定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人高精度转换坐标与对接点高精度转换坐标。
进一步地，如图5所示，步骤S15包括：
S151、依据获取的全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylcθlc)与高精度定位模式的坐标系下的机器人当前坐标TQP(xtq ytq θtq)，通过式(5)将高精度定位模式的坐标系下的机器人坐标QP(xq yq θq)转换成全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)。
Figure BDA0003848098700000191
Figure BDA0003848098700000192
S152、依据获取的全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylcθlc)与高精度定位模式的坐标系下的机器人当前坐标TQP(xtq ytq θtq)，通过式(6)将高精度定位模式的坐标系下的对接点坐标QP2(xq2 yq2 θq2)转换为全局定位模式的坐标系下的对接点高精度转换坐标QPC2(xqc2 yqc2 θqc2)。
Figure BDA0003848098700000201
Figure BDA0003848098700000202
式(5)与式(6)中，ΔQp为高精度定位模式的坐标系下的机器人任意坐标相对于高精度定位模式的坐标系下机器人当前坐标的偏差，xΔqp、yΔqp、θΔqp则分别为x、y、θ坐标下的偏差。
S2、基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，控制移动机器人执行对接任务。
如图6所示，步骤S2包括：
S21、依据全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)与对接点高精度转换坐标QPC2(xqc2 yqc2 θqc2)，通过式(7)得到对接点QPC3(xqc3 yqc3 θqc3)。由于高精度定位的控制距离非常短，为了避免临界问题，根据QPC(xqc yqc θqc)和QPC2(xqc2yqc2 θqc2)坐标，将目标点改为QPC3(xqc3 yqc3 θqc3)。
Figure BDA0003848098700000203
Figure BDA0003848098700000204
S22、依据全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)与对接点QPC3(xqc3 yqc3 θqc3)，规划出如下的呈直线的控制轨迹B2(t)。
B2(t)＝QPC+(QPC3-QPC)*t,t∈[0,1] (8)
S23、在控制机器人到达对接点QPC3(xqc3 yqc3 θqc3)时，依据高精度模式下的机器人实时坐标QP(xq yq θq)与对接点高精度转换坐标QP2(xq2 yq2 θq2)，得到如下机器人控制误差DP(xd yd θd)。
Figure BDA0003848098700000211
S24、判断机器人控制误差是否满足如下阈值要求：
Figure BDA0003848098700000212
S25A、若机器人控制误差DP(xd yd θd)满足阈值要求，则将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作。
S25B、若机器人控制误差DP(xd yd θd)不满足阈值要求，则调整移动机器人的位置来满足阈值要求，再将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作。在具体实施例中，可多次进行移动机器人位置的调整的操作直至机器人控制误差DP(xdyd θd)满足阈值要求，再共进行三次调整之后，如果依旧失败就直接报错，同时关闭局部定位模块和高精度定位模块。
进一步地，如图7所示，步骤S25B包括：
S25B-1、将高精度定位模式的坐标系下的当前机器人坐标跳变为局部定位模式的坐标系下的机器人当前跳变坐标LP3(xl3 yl3 θl3)，并以局部定位模式的坐标系下的对接预备点坐标LP1(xl1 yl1 θl1)为移动目标点。
S25B-2、通过式(11)将局部定位模式的坐标系下的对接坐标LP1(xl1 yl1 θl1)与机器人当前跳变坐标LP3(xl3 yl3 θl3)分别进行变换为LP4(xl4 yl4 θl4)与LP5(xl5 yl5 θl5)；
Figure BDA0003848098700000221
Figure BDA0003848098700000222
S25B-3、依据LP4(xl4 yl4 θl4)与LP5(xl5 yl5 θl5)规划第三5阶贝塞尔曲线B3(t)，并基于第三5阶贝塞尔曲线B3(t)调整移动机器人的位置来满足阈值要求。较佳地，调整移动机器人的位置在实际操作中一般为控制移动机器人后退。
Figure BDA0003848098700000223
Figure BDA0003848098700000231
Figure BDA0003848098700000232
Figure BDA0003848098700000233
Figure BDA0003848098700000234
Figure BDA0003848098700000235
Figure BDA0003848098700000236
式(13)与式(14)中，LP5(xl5 yl5 θl5)为当前出发点，LP4(xl4 yl4 θl4)为当前目标点，都是由外部输入，p31…p34为控制点由下述公式计算得出，P3t表示目标点与出发点的坐标差值，P3d表示转换到出发点坐标系下的P3t表示，x3t表示目标点和出发点在世界坐标系下的x轴方向差值，y3t表示目标点和出发点在世界坐标系下的y轴方向差值，θ3t表示目标点和出发点在世界坐标系下的角度差值，x3d表示出发点坐标系下的目标点x坐标值，y3d表示出发点坐标系下的目标点y坐标值，θ3d表示出发点在世界坐标系下的角度值。
基于上述的步骤，参考图8可知，本发明实施例提供的在机器人控制误差不满足阈值要求时的重试流程为：基于生成的第三5阶贝塞尔曲线调整移动机器人的位置，之后依据规划出的第四5阶贝塞尔曲线B4(t)，控制移动机器人往QPC3(xqc3 yqc3 θqc3)处的二维码行进，以使移动机器人完成对接任务。
S25B-4、在调整移动机器人的位置之后，局部定位模式的坐标系下的反馈坐标LP6(xl6 yl6 θl6)和全局定位模式的坐标系下的对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)规划第四5阶贝塞尔曲线B4(t)，控制移动机器人行进使移动机器人完成对接任务。
Figure BDA0003848098700000241
Figure BDA0003848098700000251
Figure BDA0003848098700000252
Figure BDA0003848098700000253
Figure BDA0003848098700000254
Figure BDA0003848098700000255
Figure BDA0003848098700000256
式(15)与式(16)中，LP6(xl6 yl6 θl6)为当前出发点，LPC2(xlc2 ylc2 θlc2)为当前目标点，都是由外部输入，p41…p44为控制点由下述公式计算得出，P4t表示目标点与出发点的坐标差值，P4d表示转换到出发点坐标系下的P4t表示，x4t表示目标点和出发点在世界坐标系下的x轴方向差值，y4t表示目标点和出发点在世界坐标系下的y轴方向差值，θ4t表示目标点和出发点在世界坐标系下的角度差值，x4d表示出发点坐标系下的目标点x坐标值，y4d表示出发点坐标系下的目标点y坐标值，θ4d表示出发点在世界坐标系下的角度值。
值得一提的是，上述各个坐标在参与具体运算时，均作了转置处理。
此外，本发明实施例还提供一种应对定位切换跳变的导航对接系统，包括：
多模式定位模块，用于对移动机器人实施不同定位精度的定位模式；
坐标切换模块，用于在移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标；
轨迹规划模块，用于基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，控制移动机器人执行对接任务。
同时，本发明实施例提供一种多定位模式自动切换的导航对接方法，包括：在执行移动机器人的对接任务时，通过切换各种定位精度的定位模式来实现移动机器人的高精度对接；其中，在进行定位模式切换时执行如上所述的一种应对定位切换跳变的导航对接方法；高精度在本实施例中为定位误差在±2cm。
以及，本发明实施例还提供一种应对定位切换跳变的导航对接设备，包括：存储有定位精度为±2cm的全局定位模式、定位精度为±8mm的局部定位模式以及定位精度为±1mm的高精度定位模式的导航控制器，导航控制器用于接收向移动机器人下发包含对接预备点与对接点的对接任务，并对移动机器人执行如上所述的方法以完成所述对接任务；再者，还提供一种计算机可读介质，其上存储有计算机可执行指令，其特征在于，所述可执行指令被处理器执行时实现如上所述的一种应对定位切换跳变的导航对接方法以及一种多定位模式自动切换的导航对接方法。
综上，本发明实施例提供一种应对定位切换跳变的导航对接方法、系统以及设备，如图9所示，其具体实现思路为：
a、首先，如图9(a)所示，移动机器人起始状态是在全局定位模式下行进的，在全局定位模式下获得全局定位模式的坐标系下的对接预备点坐标为MP1(xm1 ym1 θm1)、对接点坐标为MP2(xm2 ym2 θm2)以及全局定位模式的坐标系下的机器人当前坐标TMP(xtm ytm θtm)。
b、控制移动机器人到达相应的定位切换点后(适用于任何定位精度低到高的定位切换点，可以是全局定位切换到局部定位，也可以是局部定位切换到高精度定位，也可以是全局定位切换到高精度定位)，可以获取切换前后的两个坐标系的坐标值。其中，如图9(b)所示，由全局定位模式切换到局部定位模式时，获取此时的局部定位模式的坐标系下的机器人坐标LP(xl yl θl)、对接预备点坐标LP1(xl1 yl1 θl1)以及对接点坐标LP2(xl2 yl2 θl2)；如图9(c)所示，而将任意局部定位模式下的坐标转换回全局定位模式下的结果，得到转换后的全局坐标值LPC、转换后的对接预备点坐标LPC1以及对接点坐标LPC2。
c、为了保证切换过程没有跳变，需要在线计算坐标变换矩阵。通过坐标变换矩阵修改切换后坐标系原点的坐标值，保证当前两个坐标系输出的定位结果一致。换而言之，对切换后的坐标系整体进行坐标变换，保证变换后的坐标系的小车当前坐标值和切换前的坐标系输出的坐标值完全一致。
d、切换定位来源后，导航对接的目标点也需要从原目标点改成新的目标点。因此在切换后的坐标系原点修改后，新的目标点坐标也要根据坐标变换矩阵进行变换。
e、目标坐标点变换后，如果按照原有目标点导航，则无法到点对接点。因此需要按照新的目标点，重新规划导航轨迹。
f、a至e的步骤就是一次定位来源切换的思路，通过定位来源切换，保证了切换点无定位跳变。通过目标点坐标修改和导航轨迹的重新规划，保证了机器人可以很好的抵达目标点位置。
g、从全局定位到局部定位，从局部定位到高精度定位的定位来源切换流程都是一致的，区别只存在于导航轨迹规划方法上。
h、完成对接动作后，再将定位来源切换为全局定位，由于此时机器人为静止状态，可以接受定位切换导致的跳变。
基于上述描述可知，本发明解决了高速进行过程中的定位来源切换问题，实现真正的高速对接，保证切换点坐标无跳变，不需要停车进行坐标系切换，提高运行效率；通过切换坐标系后的导航目标点，同时进行合理的轨迹规划，可以提高对接成功率；且依靠动态的地图拼接，规避了多种定位方式的空间尺度不一致问题，由此本发明真正实现了所有的运动控制都由导航模块发出，解决了大量由于控制模块切换而产生的临界问题；再者，本发明通过管理定位来源，可以实现最优的重试逻辑，在对接空间狭窄时候可以通过重试完成对接。因此，所有需要一次或者多次定位来源切换的对接过程，都可以通过本发明方法实现不停车对接。
由于本发明上述实施例所描述的系统/装置，为实施本发明上述实施例的方法所采用的系统/装置，故而基于本发明上述实施例所描述的方法，本领域所属技术人员能够了解该系统/装置的具体结构及变形，因而在此不再赘述。凡是本发明上述实施例的方法所采用的系统/装置都属于本发明所欲保护的范围。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例，或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。
应当注意的是，在权利要求中，不应将位于括号之间的任何附图标记理解成对权利要求的限制。词语“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的词语“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的权利要求中，这些装置中的若干个可以是通过同一个硬件来具体体现。词语第一、第二、第三等的使用，仅是为了表述方便，而不表示任何顺序。可将这些词语理解为部件名称的一部分。
此外，需要说明的是，在本说明书的描述中，术语“一个实施例”、“一些实施例”、“实施例”、“示例”、“具体示例”或“一些示例”等的描述，是指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管已描述了本发明的优选实施例，但本领域的技术人员在得知了基本创造性概念后，则可对这些实施例作出另外的变更和修改。所以，权利要求应该解释为包括优选实施例以及落入本发明范围的所有变更和修改。
显然，本领域的技术人员可以对本发明进行各种修改和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也应该包含这些修改和变型在内。

Claims (10)

1.一种应对定位切换跳变的导航对接方法，其特征在于，包括：
在依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标；
基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，以控制所述移动机器人执行对接任务；
其中，所述定位模式包括：定位精度为±2cm的全局定位模式、定位精度为±8mm的局部定位模式以及定位精度为±1mm的高精度定位模式。
2.如权利要求1所述的一种应对定位切换跳变的导航对接方法，其特征在于，在依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标包括：
获取包含对接预备点与对接点的对接任务；
在移动机器人接近对接任务指示的对接预备点时，将移动机器人的全局定位模式切换至局部定位模式，同时在局部定位模式下控制移动机器人抵达对接预备点；
将获取的局部定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人局部转换坐标与对接点局部转换坐标，并重新规划驶向对接点局部转换坐标的路径；
在移动机器人接近对接点局部转换坐标时，将移动机器人的局部定位模式切换至高精度定位模式；
将获取的高精度定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人高精度转换坐标与对接点高精度转换坐标。
3.如权利要求2所述的一种应对定位切换跳变的导航对接方法，其特征在于，在移动机器人接近对接任务指示的对接预备点时，将移动机器人的全局定位模式切换至局部定位模式，同时在局部定位模式下控制移动机器人抵达对接预备点包括：
控制移动机器人在全局定位模式下朝对接预备点行进；
在移动机器人距离对接预备点小于d1＝0.2m时，将移动机器人的全局定位模式切换至局部定位模式，并控制移动机器人抵达对接预备点；
获取局部定位模式的坐标系下的机器人坐标LP(xl yl θl)、对接预备点坐标LP1(xl1 yl1θl1)以及对接点坐标LP2(xl2 yl2 θl2)，θ为角度值；
相应地，移动机器人接近对接点局部转换坐标的距离为d2＝0.1m。
4.如权利要求3所述的一种应对定位切换跳变的导航对接方法，其特征在于，将获取的局部定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人局部转换坐标与对接点局部转换坐标，并重新规划驶向对接点局部转换坐标的路径包括：
依据获取的全局定位模式的坐标系下的机器人当前坐标TMP(xtm ytm θtm)与局部定位模式的坐标系下的机器人当前坐标TLP(xtl ytl θtl)，通过式(1)将所述将局部定位模式的坐标系下的任意机器人坐标LP(xl yl θl)转换成全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)；
Figure FDA0003848098690000021
Figure FDA0003848098690000022
依据获取的全局定位模式的坐标系下的机器人当前坐标TMP(xtm ytm θtm)与局部定位模式的坐标系下的机器人当前坐标TLP(xtl ytl θtl)，通过式(2)将所述局部定位模式下的对接点坐标LP2(xl2 yl2 θl2)变换为全局定位模式的坐标系下的对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)：
Figure FDA0003848098690000031
Figure FDA0003848098690000032
式(1)与式(2)中，ΔLp为局部定位模式的坐标系下的机器人任意坐标相对于局部定位模式的坐标系下机器人当前坐标的偏差，xΔlp、yΔlp、θΔlp则分别为x、y、θ坐标下的偏差；
依据全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)与对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)，规划如下的第一5阶贝塞尔曲线B(t)；
Figure FDA0003848098690000033
Figure FDA0003848098690000041
Figure FDA0003848098690000042
Figure FDA0003848098690000043
Figure FDA0003848098690000044
Figure FDA0003848098690000045
Figure FDA0003848098690000046
式(3)与式(4)中，LPC(xlc ylc θlc)为当前出发点，LP2(xl2 yl2 θl2)为当前目标点，都是由外部输入，p1…p4为控制点由下述公式计算得出，pt表示目标点与出发点的坐标差值，pd表示转换到出发点坐标系下的pt表示，xt表示目标点和出发点在世界坐标系下的x轴方向差值，yt表示目标点和出发点在世界坐标系下的y轴方向差值，θt表示目标点和出发点在世界坐标系下的角度差值，xd表示出发点坐标系下的目标点x坐标值，yd表示出发点坐标系下的目标点y坐标值，θd表示出发点在世界坐标系下的角度值。
5.如权利要求4所述的一种应对定位切换跳变的导航对接方法，其特征在于，将获取的高精度定位模式的坐标系下的机器人坐标与对接点坐标变换为全局定位模式的坐标系下的机器人高精度转换坐标与对接点高精度转换坐标包括：
依据获取的全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)与高精度定位模式的坐标系下的机器人当前坐标TQP(xtq ytq θtq)，通过式(5)将任意高精度定位模式的坐标系下的机器人坐标QP(xq yq θq)转换成全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)；
Figure FDA0003848098690000051
Figure FDA0003848098690000052
依据获取的全局定位模式的坐标系下的机器人局部转换坐标LPC(xlc ylc θlc)与高精度定位模式的坐标系下的机器人当前坐标TQP(xtq ytq θtq)，通过式(6)将高精度定位模式的坐标系下的对接点坐标QP2(xq2 yq2 θq2)转换为全局定位模式的坐标系下的对接点高精度转换坐标QPC2(xqc2 yqc2 θqc2)；
Figure FDA0003848098690000053
Figure FDA0003848098690000054
式(5)与式(6)中，ΔQp为高精度定位模式的坐标系下的机器人任意坐标相对于高精度定位模式的坐标系下机器人当前坐标的偏差，xΔqp、yΔqp、θΔqp则分别为x、y、θ坐标下的偏差。
6.如权利要求5所述的一种应对定位切换跳变的导航对接方法，其特征在于，基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，以控制所述移动机器人执行对接任务包括：
依据全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)与对接点高精度转换坐标QPC2(xqc2 yqc2 θqc2)，通过式(7)得到对接点QPC3(xqc3 yqc3 θqc3)；
Figure FDA0003848098690000061
Figure FDA0003848098690000062
依据全局定位模式的坐标系下的机器人高精度转换坐标QPC(xqc yqc θqc)与对接点QPC3(xqc3 yqc3 θqc3)，规划出如下的控制轨迹B2(t)：
B2(t)＝QPC+(QPC3-QPC)*t,t∈[0,1] (8)
在基于控制轨迹B2(t)控制移动机器人到达对接点QPC3(xqc3 yqc3 θqc3)时，依据高精度模式下的机器人实时坐标QP(xq yq θq)与对接点高精度转换坐标QP2(xq2 yq2 θq2)，得到如下机器人控制误差DP(xd yd θd)；
Figure FDA0003848098690000063
判断所述机器人控制误差是否满足如下阈值要求：
Figure FDA0003848098690000064
若机器人控制误差DP(xd yd θd)满足阈值要求，则将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作；
若机器人控制误差DP(xd yd θd)不满足阈值要求，则调整移动机器人的位置来满足阈值要求，再将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作。
7.如权利要求6所述的一种应对定位切换跳变的导航对接方法，其特征在于，若机器人控制误差不满足阈值要求，则调整移动机器人的位置来满足阈值要求，再将定位模式跳变到全局定位模式，控制移动机器人完成相应的对接动作包括：
将高精度定位模式的坐标系下的当前机器人坐标跳变为局部定位模式的坐标系下的机器人当前跳变坐标LP3(xl3 yl3 θl3)，并以局部定位模式的坐标系下的对接预备点坐标LP1(xl1 yl1 θl1)为移动目标点；
通过式(11)将局部定位模式的坐标系下的对接坐标LP1(xl1 yl1 θl1)与机器人当前跳变坐标LP3(xl3 yl3 θl3)分别进行变换为LP4(xl4 yl4 θl4)与LP5(xl5 yl5 θl5)；
Figure FDA0003848098690000071
Figure FDA0003848098690000072
依据LP4(xl4 yl4 θl4)与LP5(xl5 yl5 θl5)规划第三5阶贝塞尔曲线B3(t)，并基于第三5阶贝塞尔曲线B3(t)调整移动机器人的位置来满足阈值要求；
Figure FDA0003848098690000081
Figure FDA0003848098690000082
Figure FDA0003848098690000083
Figure FDA0003848098690000084
Figure FDA0003848098690000085
Figure FDA0003848098690000086
Figure FDA0003848098690000087
式(13)与式(14)中，LP5(xl5 yl5 θl5)为当前出发点，LP4(xl4 yl4 θl4)为当前目标点，都是由外部输入，p31…p34为控制点由下述公式计算得出，P3t表示目标点与出发点的坐标差值，P3d表示转换到出发点坐标系下的P3t表示，x3t表示目标点和出发点在世界坐标系下的x轴方向差值，y3t表示目标点和出发点在世界坐标系下的y轴方向差值，θ3t表示目标点和出发点在世界坐标系下的角度差值，x3d表示出发点坐标系下的目标点x坐标值，y3d表示出发点坐标系下的目标点y坐标值，θ3d表示出发点在世界坐标系下的角度值；
在调整移动机器人的位置之后，依据全局定位模式的坐标系下的实时坐标LP6(xl6 yl6θl6)和全局定位模式的坐标系下的对接点局部转换坐标LPC2(xlc2 ylc2 θlc2)规划第四5阶贝塞尔曲线B4(t)，控制移动机器人行进使移动机器人完成对接任务；
Figure FDA0003848098690000091
Figure FDA0003848098690000101
Figure FDA0003848098690000102
Figure FDA0003848098690000103
Figure FDA0003848098690000104
Figure FDA0003848098690000105
Figure FDA0003848098690000106
式(15)与式(16)中，LP6(xl6 yl6 θl6)为当前出发点，LPC2(xlc2 ylc2 θlc2)为当前目标点，都是由外部输入，p41…p44为控制点由下述公式计算得出，P4t表示目标点与出发点的坐标差值，P4d表示转换到出发点坐标系下的P4t表示，x4t表示目标点和出发点在世界坐标系下的x轴方向差值，y4t表示目标点和出发点在世界坐标系下的y轴方向差值，θ4t表示目标点和出发点在世界坐标系下的角度差值，x4d表示出发点坐标系下的目标点x坐标值，y4d表示出发点坐标系下的目标点y坐标值，θ4d表示出发点在世界坐标系下的角度值。
8.一种应对定位切换跳变的导航对接系统，其特征在于，包括：
多模式定位模块，用于依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换；
坐标切换模块，用于在依据与目标点的实时距离对移动机器人进行不同定位精度下的定位模式切换时，通过对切换后的坐标系进行坐标变换以使切换前后的坐标系下的机器人坐标一致，同时通过坐标变换获得新的目标点坐标；
轨迹规划模块，用于基于切换后的坐标系下的机器人坐标与新的目标点坐标重新规划导航轨迹，以控制所述移动机器人执行对接任务。
9.一种多定位模式自动切换的导航对接方法，其特征在于，包括：
在执行移动机器人的对接任务时，通过切换各种定位精度的定位模式来实现移动机器人的高精度对接；
其中，在进行定位模式切换时执行如权利要求1-7任一项所述的一种应对定位切换跳变的导航对接方法。
10.一种应对定位切换跳变的导航对接设备，其特征在于，包括：导航控制器，所述导航控制器用于对移动机器人执行如权利要求1-7任一项所述的一种应对定位切换跳变的导航对接方法以及如权利要求9所述的一种多定位模式自动切换的导航对接方法。
CN202211124837.8A 2022-09-15 2022-09-15 一种应对定位切换跳变的导航对接方法、系统以及设备 Active CN115502971B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211124837.8A CN115502971B (zh) 2022-09-15 2022-09-15 一种应对定位切换跳变的导航对接方法、系统以及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211124837.8A CN115502971B (zh) 2022-09-15 2022-09-15 一种应对定位切换跳变的导航对接方法、系统以及设备

Publications (2)

Publication Number Publication Date
CN115502971A true CN115502971A (zh) 2022-12-23
CN115502971B CN115502971B (zh) 2023-06-27

Family

ID=84504781

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211124837.8A Active CN115502971B (zh) 2022-09-15 2022-09-15 一种应对定位切换跳变的导航对接方法、系统以及设备

Country Status (1)

Country Link
CN (1) CN115502971B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20030010057A (ko) * 2001-07-25 2003-02-05 주식회사 핸손테크놀로지 다중 지적정보 독취 시스템 및 이를 이용한 독취방법
CN110986920A (zh) * 2019-12-26 2020-04-10 武汉万集信息技术有限公司 定位导航方法、装置、设备及存储介质
CN111443713A (zh) * 2020-04-14 2020-07-24 三一机器人科技有限公司 融合定位导航系统和方法
CN111571618A (zh) * 2020-06-11 2020-08-25 深圳款款科技有限公司 一种基于视觉算法的自主捡拾机器人及其捡拾方法
CN112129282A (zh) * 2020-09-30 2020-12-25 杭州海康机器人技术有限公司 一种不同导航方式之间定位结果的转换方法、转换装置
CN113189977A (zh) * 2021-03-10 2021-07-30 新兴际华集团有限公司 一种用于机器人的智能导航路径规划系统及方法
CN113390411A (zh) * 2021-06-10 2021-09-14 中国北方车辆研究所 基于变构型感知装置的足式机器人导航及定位方法
CN114114367A (zh) * 2021-11-17 2022-03-01 中南大学 Agv室外定位切换方法、计算机装置及程序产品

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20030010057A (ko) * 2001-07-25 2003-02-05 주식회사 핸손테크놀로지 다중 지적정보 독취 시스템 및 이를 이용한 독취방법
CN110986920A (zh) * 2019-12-26 2020-04-10 武汉万集信息技术有限公司 定位导航方法、装置、设备及存储介质
CN111443713A (zh) * 2020-04-14 2020-07-24 三一机器人科技有限公司 融合定位导航系统和方法
CN111571618A (zh) * 2020-06-11 2020-08-25 深圳款款科技有限公司 一种基于视觉算法的自主捡拾机器人及其捡拾方法
CN112129282A (zh) * 2020-09-30 2020-12-25 杭州海康机器人技术有限公司 一种不同导航方式之间定位结果的转换方法、转换装置
CN113189977A (zh) * 2021-03-10 2021-07-30 新兴际华集团有限公司 一种用于机器人的智能导航路径规划系统及方法
CN113390411A (zh) * 2021-06-10 2021-09-14 中国北方车辆研究所 基于变构型感知装置的足式机器人导航及定位方法
CN114114367A (zh) * 2021-11-17 2022-03-01 中南大学 Agv室外定位切换方法、计算机装置及程序产品

Also Published As

Publication number Publication date
CN115502971B (zh) 2023-06-27

Similar Documents

Publication Publication Date Title
Schneier et al. Literature review of mobile robots for manufacturing
US8948913B2 (en) Method and apparatus for navigating robot
Hu et al. An efficient RRT-based framework for planning short and smooth wheeled robot motion under kinodynamic constraints
Seelinger et al. Automatic visual guidance of a forklift engaging a pallet
CN110789529B (zh) 车辆的控制方法、装置及计算机可读存储介质
Cronin et al. State-of-the-art review of autonomous intelligent vehicles (AIV) technologies for the automotive and manufacturing industry
CN112612273A (zh) 一种巡检机器人避障路径规划方法、系统、设备和介质
Kayhani et al. Tag-based visual-inertial localization of unmanned aerial vehicles in indoor construction environments using an on-manifold extended Kalman filter
Meng et al. A safe and efficient LIDAR-based navigation system for 4WS4WD mobile manipulators in manufacturing plants
Behrje et al. An autonomous forklift with 3d time-of-flight camera-based localization and navigation
JP6418359B1 (ja) ロボットプログラムの生成装置及び生成方法
Hüppi et al. T-prm: Temporal probabilistic roadmap for path planning in dynamic environments
CN115502971B (zh) 一种应对定位切换跳变的导航对接方法、系统以及设备
Li et al. Dynamic scene graph for mutual-cognition generation in proactive human-robot collaboration
CN114911226A (zh) 搬运车行驶控制方法、装置及搬运车
Wolf et al. Towards robotic laboratory automation Plug & play: Survey and concept proposal on teaching-free robot integration with the lapp digital twin
CN111290406B (zh) 一种路径规划的方法、机器人及存储介质
CN105415376B (zh) 一种离线编程装置
Lunenburg et al. Tech united eindhoven team description 2012
CN111360789A (zh) 工件加工的示教方法、控制方法及机器人示教系统
CN105415375A (zh) 一种离线编程装置
Schmitt et al. Global referencing systems and their contribution to a versatile production
Mantha et al. Investigating the fiducial marker network characteristics for autonomous mobile indoor robot navigation using ROS and Gazebo
Jiang et al. Research on RTLS-based coordinate guided vehicle (CGV) for material distribution in discrete manufacturing workshop
Aboul-Enein et al. Design and Implementation of a Closed-Loop Mobile Manipulator Control System

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant