CN114952870B - 用于高频接触物体消毒的四轴机械臂运动控制方法及系统 - Google Patents
用于高频接触物体消毒的四轴机械臂运动控制方法及系统 Download PDFInfo
- Publication number
- CN114952870B CN114952870B CN202210902349.9A CN202210902349A CN114952870B CN 114952870 B CN114952870 B CN 114952870B CN 202210902349 A CN202210902349 A CN 202210902349A CN 114952870 B CN114952870 B CN 114952870B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- target
- obstacle
- point
- angles
- 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.)
- Active
Links
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
- B25J11/00—Manipulators not otherwise provided for
-
- 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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- 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
- B25J9/1666—Avoiding collision or forbidden zones
-
- 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)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
Abstract
本发明提供了一种用于高频接触物体消毒的四轴机械臂运动控制方法及系统,属于机器人控制技术领域;将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂坐标系得出机械臂各轴角度;将目标位置各轴角度与当前角度作差求出各轴转角,将各轴转角转换成电机转角,根据电机转角大小按比例分配电机转速;进行运动学正解估计机械臂末端的笛卡尔坐标,比较笛卡尔坐标在水平面上的投影是否越过障碍物点构成的约束,如果是,则判断机械臂在运动过程中会碰到障碍物,进行路径规划以规避障碍物;否则,直接根据逆解出的各轴转角控制机械臂到达目标位置进行消毒;本发明实现了四轴机械臂的更精准控制,进而保证了高频接触物体的消杀精度和效率。
Description
技术领域
本发明涉及机器人控制技术领域,特别涉及一种用于高频接触物体消毒的四轴机械臂运动控制方法及系统。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术,并不必然构成现有技术。
在疫情流行期间,做好隔离消毒尤为重要。间接接触传播也是一个主要的传播途径,对于办公场合的门把手、电梯按键等高频的密接物体,人手可能携带病毒,在接触到门把手的时候就会将病毒留在门把手上,造成接触传播的风险。而现有的设备的消毒主要是针对空气或者是快递包裹,目前对于医院、办公场合高密接门把手以手动擦拭的方式进行消杀,这样势必会产生二次感染的风险,并且工作效率低。
发明人发现,现有的消毒机器人大多是对机器人的运动进行控制以实现喷洒消杀或者照射消杀,主要用于待消杀环境的整体性消杀,对高频接触物体采用普通消杀方式往往消杀不彻底,即现有的机器人的无法对机械臂进行有效的四轴联动控制以实现特定消杀目标的准确消杀控制。
发明内容
为了解决现有技术的不足,本发明提供了一种用于高频接触物体消毒的四轴机械臂运动控制方法及系统,实现了更精准的机器人消杀控制。
为了实现上述目的,本发明采用如下技术方案:
本发明第一方面提供了一种用于高频接触物体消毒的四轴机械臂运动控制方法。
一种用于高频接触物体消毒的四轴机械臂运动控制方法,包括以下过程:
获取消毒目标的中心位置的空间坐标;
将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂关节坐标系得出机械臂各轴角度;
将目标位置各轴角度与当前角度作差求出各轴转角,将各轴转角转换成电机转角,根据电机转角大小按比例分配电机转速;
进行运动学正解估计机械臂末端的笛卡尔坐标,比较笛卡尔坐标在水平面上的投影是否越过障碍物点构成的约束,如果是,则判断机械臂在运动过程中会碰到障碍物,进行路径规划以规避障碍物;否则,直接根据逆解出的各轴转角控制机械臂到达目标位置进行消毒。
作为可选的一种实现方式,所述路径规划,包括:
将栅格地图上的障碍物进行膨化得到膨化曲线S(虚线),作为机械臂末端的安全工作界限;
从原路径与膨化曲线交点X0开始沿着膨化曲线向着目标点的方向以一定的步长选取路径点X1,进行从X1点到目标点的碰撞检测,如不发生碰撞,就停止路径点的寻找,避障路径就是起始点到X0再到X1最后到目标点;
如果发生碰撞,就从X1重复步骤2,直到找到的路径点Xi到目标点的过程中直接不发生碰撞;
将选出来的点进行二次B样条曲线进行拟合,使路径平缓,然后进行插值选取新的路径点,对于有的路径点曲率比较大,则将该路径点舍去。
作为可选的一种实现方式,将各轴转角转换成电机转角,包括:
将各轴转角根据传动比转换成电机转角,将其中某个转轴电机需要转动的角度分割成多个子角度,计算出该轴电机转到该角度时其他轴电机的角度,根据比例关系得到机械臂其他各轴角度。
作为可选的一种实现方式,进行运动学正解估计机械臂末端的笛卡尔坐标,包括:
通过预建立的二维栅格地图将障碍物相对于世界坐标系XwYw的坐标信息以点云的方式记录在栅格地图中;实时获取机器人在栅格地图中的定位,得到世界坐标系与机器人坐标系之间的关系Twr,进而得到障碍物点云相对于自身的坐标信息XrYr;
根据机械臂和机器人中心的关系,得到机器人坐标系XrYr到机械臂坐标系XmYm之间的变换关系Trm,将障碍物点云的坐标转换到相对于机械臂原点的笛卡尔转标系上,得到机械臂末端的笛卡尔坐标。
作为可选的一种实现方式,根据电机速度到零之后的当前编码器值与下发的期望编码器值之前的差值是否在误差允许范围之内判断机械臂是否到达目标位置,在到达位置之后再进行消毒操作。
作为可选的一种实现方式,对于单目标点,没有碰撞直接执行当前位置到目标位置的连续转动,到达位置完成消毒动作之后原路返回,有碰撞则进行运动规划分割得到新的路径点,依次执行新的路径点,达到目标位置后原路返回。
作为可选的一种实现方式,对于多目标点,将各目标点进行运动学逆解之后,将各轴角度存储下来依次执行,直到执行完最后一个目标点的位置之后才回零,不需要原路返回,在当前位置以机械臂起始位置的各轴角度为目标位置,进行碰撞检测和运动规划。
作为可选的一种实现方式,对横式门把手内侧进行消毒时,将识别到的目标点的坐标向上偏移大于四轴的坐标,求逆解时将四轴机械臂运动简化为求三轴机械臂逆解,求出三轴末端到达目标点上方的逆解,到达位置后,将四轴旋转使四轴朝下进行消毒;
对高频接触物体进行消毒时,对目标位置坐标沿机械臂坐标系的Ym负方向偏移,然后求解三个轴的运动学逆解后,让机械臂四轴保持水平进行喷洒。
本发明第二方面提供了一种用于高频接触物体消毒的四轴机械臂运动控制系统。
一种用于高频接触物体消毒的四轴机械臂运动控制系统,包括以下过程:
数据获取模块,被配置为:获取消毒目标的中心位置的空间坐标;
坐标转换模块,被配置为:将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂坐标系得出机械臂各轴角度;
转角计算模块,被配置为:将目标位置各轴角度与当前角度作差求出各轴转角,将各轴转角转换成电机转角,根据电机转角大小按比例分配电机转速;
消毒控制模块,被配置为:进行运动学正解估计机械臂末端的笛卡尔坐标,比较笛卡尔坐标在水平面上的投影是否越过障碍物点构成的约束,如果是,则判断机械臂在运动过程中会碰到障碍物,进行路径规划以规避障碍物;否则,直接根据逆解出的各轴转角控制机械臂到达目标位置进行消毒。
作为可选的一种实现方式,所述路径规划,包括:
将栅格地图上的障碍物进行膨化得到膨化曲线,以膨化曲线作为机械臂末端的安全工作界限;
在障碍物膨化曲线、原路径非碰撞段、机械臂第一轴旋转中心与目标点连线线段位于膨化曲线与目标点之间的部分上选取点作为路径点;
用选出来的路径点进行二次B样条曲线拟合,进行插值选取新的路径点。
与现有技术相比,本发明的有益效果是:
1、本发明所述的用于高频接触物体消毒的四轴机械臂运动控制方法及系统,根据实际应用场景将四轴机械臂运动学逆解问题简化为三轴机械臂运动学逆解问题,降低了求解难度和求解时间。
2、本发明所述的用于高频接触物体消毒的四轴机械臂运动控制方法及系统,利用墙壁等障碍物形状的规则性,将三维空间的障碍信息转换到二维xy平面上,简化了障碍物检测和路径规划的复杂性。
3、本发明所述的用于高频接触物体消毒的四轴机械臂运动控制方法及系统,利用激光雷达建立的2D栅格地图结合机器人自身在地图中的定位以及机械臂坐标系与机器人坐标系之间的关系,将障碍物点的坐标信息从机器人坐标系XrYr转换到机械臂坐标系XmYm下,在二维空间下进行碰撞检测和路径规划。
4、本发明所述的用于高频接触物体消毒的四轴机械臂运动控制方法及系统,结合障碍物轮廓进行路径规划,相比于使用较多的基于贪婪策略的A*算法、RRT算法等,搜索次数低,计算量低,而且对进行了平滑处理,减少了机械臂的抖动。
5、本发明所述的用于高频接触物体消毒的四轴机械臂运动控制方法及系统,采用工业相机检测目标的坐标信息,充分利用自主移动机器人的优势,将需要检测的目标在环境中的位置存储下来,在机器人到达该位置时再利用视觉检测目标位置,待到消毒动作完成之后又关闭视觉检测,避免不必要的检测造成的资源消耗。
本发明附加方面的优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。
图1为本发明实施例1提供的四轴机械臂的结构示意图。
图2为本发明实施例1提供的用于高频接触物体消毒的四轴机械臂运动控制方法的流程示意图。
图3为本发明实施例1提供的各坐标系水平投影之间的变换关系示意图。
图4为本发明实施例1提供的机器人坐标与机械臂坐标系关系示意图。
图5为本发明实施例1提供的避障路径规划示意图。
其中,1-第一轴;2-第二轴;3-第三轴;4-第四轴;5-消毒液喷洒头。
具体实施方式
下面结合附图与实施例对本发明作进一步说明。
应该指出,以下详细说明都是示例性的,旨在对本发明提供进一步的说明。除非另有指明,本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是,这里所使用的术语仅是为了描述具体实施方式,而非意图限制根据本发明的示例性实施方式。如在这里所使用的,除非上下文另外明确指出,否则单数形式也意图包括复数形式,此外,还应当理解的是,当在本说明书中使用术语“包含”和/或“包括”时,其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
实施例1:
本发明实施例1提供了一种用于高频接触物体消毒的四轴机械臂运动控制方法,通过视觉识别得出目标进行运动学逆解的处目标位置,结合2D激光SLAM生成的栅格地图进行障碍物检测,进行避障路径规划,控制如图1所示的四轴机械臂达目标点,包括第一轴1、第二轴2、第三轴3、第四轴4和消毒液喷洒头5, 消毒液喷洒头5安装在第四轴4的末端,到达目标位置后喷洒消毒液进行消毒。
本发明结合视觉识别功能对需要消毒的目标进行识别并获取其空间坐标,将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂坐标系得出机械臂各轴的转角,通过伺服电机位置模式控制机械臂到达目标位置进行消毒。为了保证机械臂各轴同时到位,将各轴转角根据传动比等因素转换成电机转角,根据电机转角大小按比例分配电机转速。针对机械臂运动过程中的避障问题,由于本机械臂主要用于门把手、电梯按键等物品的消毒,障碍物形状比较规则,在竖直方向上形状变化不大,对障碍物的感知仅需获取水平面上的信息,本机械臂搭载于配备2D激光雷达的自主移动机器人上,可以通过2D激光雷达获取障碍物信息,通过一系列坐标转换将障碍物信息转换到机械臂坐标系上,将障碍物轮廓进行膨化,在膨化的轮廓上选取点进行插值得出路径点,机械臂沿着路径点循迹成功绕过障碍物到达目标点进行消毒。
具体的,如图2所示,包括以下过程:
S1:目标识别
本机械臂搭载于使用2D激光SLAM室内定位技术的自主移动机器人上,该类型机器人能实时获取自身在环境中的位置和位于机关雷达平面的环境障碍物轮廓信息,同时也能将整个环境的地图以点云的方式存储在栅格地图中,将需要进行消毒的地点的位置事先存储在栅格地图上,在机器人自身定位系统检测到机器人到达位置后,给视觉检测程序发送信息,开启视觉检测,视觉检测系统识别出要消毒的区域并解算出消毒目标的中心位置相对于机械臂原点的坐标,将坐标发送给机械臂控制程序,机械臂对该坐标进行运动学逆解,四轴机械臂逆解的结果有多组。
根据实际消毒场景,对四轴的姿态要求不一样;比如横式门把手内侧也要喷洒消毒液,需要让四轴的角度尽可能朝下,本实施例的处理办法就是将识别到的目标点的坐标向上偏移比四轴稍微大一点的数值,求逆解时将四轴机械臂运动问题简化为求三轴机械臂逆解问题,求出三轴末端到达目标点上方的逆解,到达位置后,将四轴旋转使四轴朝下,这样就可以喷洒到门把手内侧;
对于电梯按键等高频接触物体,不存在消毒液喷洒不到的死角,则按照消毒门把手的求解方法对目标位置坐标沿Ym负方向偏移然后求解三个轴的运动学逆解问题后,让机械臂四轴保持水平进行喷洒。
S2:碰撞预测
逆解出目标位置的各轴角度后,将目标位置各轴角度与当前角度作差求出各轴转角,为了保证机械臂各轴同时到位,将各轴转角根据传动比等因素转换成电机转角,根据电机转角大小按比例分配电机转速。
将其中某个转轴电机需要转动的角度分割成足够的小角度,计算出该轴电机转到该角度时其他电机的角度,根据比例关系也能得到机械臂其他各轴角度,进行运动学正解估计路径机械臂末端的笛卡尔坐标(xi,yi,zi)。
如图3、图4和图5所示,机器人通过提前建立二维栅格地图已经将障碍物相对于世界坐标系XwYw的坐标信息以点云的方式记录在栅格地图中,机器人实时获取自身在栅格地图中的定位,可以得到世界坐标系与机器人坐标系之间的关系Twr,从而得到障碍物点云相对于自身的坐标信息XrYr,又已知机械臂和机器人中心的关系,可知机器人坐标系XrYr到机械臂坐标系XmYm之间的变换关系Trm,可将障碍物点云的坐标转换到相对于机械臂原点的笛卡尔转标系上,比较(xi,yi,zi)在水平面上的投影是否越过障碍物点构成的约束,如果是,则说明机械臂在运动过程中会碰到障碍物,需要进行路径规划来避障。否则,直接执行逆解出的角度控制机械臂到达目标位置进行消毒。
S3:路径规划避障
预测到会发生碰撞之后,开始进行避障路径规划。根据实际应用场景,墙壁等障碍物在竖直方向上的尺寸不变,因此将三维空间的路径规划转化为二维水平面上的路径规划问题,路径规划方法为:
S3.1:将栅格地图上的障碍物进行膨化得到膨化曲线S,作为机械臂末端的安全工作界限。
S3.2:在障碍物膨化曲线、原路径非碰撞段、第一轴旋转中心与目标点连线线段位于膨化曲线与目标点之间的部分上选取点作为路径点。
S3.3:由于各路径点连线是折线段,折现拐角处的机械臂角度变化可能较大,所以用选出来的点进行二次B样条曲线进行拟合,使路径平缓,然后进行插值选取新的路径点.对于有的路径点曲率比较大,则将该路径点舍去。
S4:运动执行
包括目标位置对应的角度下发和实时检测是否到达目标位置,本实施例根据电机速度到零之后的当前编码器值与下发的期望编码器值之前的差值是否在误差允许范围之内判断机械臂是否到达目标位置到达位置之后再进行消毒操作。
对于实际应用场景,会碰到多目标点和单目标点的情况;
对于单目标点,没有碰撞直接执行当前位置到目标位置的连续转动,到达位置完成消毒动作之后原路返回,有碰撞则进行运动规划分割得到路径点,依次执行,达到目标位置后原路返回,返回后与其他相关部分交互信息,执行下一步操作;
对于多目标点,如大型长门把手、一个位置有多个门把手等一次消毒操作不够的情况,会一次收到多个目标点,将目标点进行运动学逆解之后,将各轴角度存储下来依次执行,只不过是直到执行完最后一个目标点的位置之后才回零,且不需要原路返回,而是在当前位置以机械臂起始位置的各轴角度为目标位置,进行碰撞检测和运动规划。
多个目标点本质上和避障路径规划中的路径点没有太大区别,仅有的区别是目标点需要停下来进行喷洒消毒液的操作。
实施例2:
本发明实施例2提供了一种用于高频接触物体消毒的四轴机械臂运动控制系统,包括以下过程:
数据获取模块,被配置为:获取消毒目标的中心位置的空间坐标;
坐标转换模块,被配置为:将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂坐标系得出机械臂各轴角度;
转角计算模块,被配置为:将目标位置各轴角度与当前角度作差求出各轴转角,将各轴转角转换成电机转角,根据电机转角大小按比例分配电机转速;
消毒控制模块,被配置为:进行运动学正解估计机械臂末端的笛卡尔坐标,比较笛卡尔坐标在水平面上的投影是否越过障碍物点构成的约束,如果是,则判断机械臂在运动过程中会碰到障碍物,进行路径规划以规避障碍物;否则,直接根据逆解出的各轴转角控制机械臂到达目标位置进行消毒。
转角计算模块,具体的,包括:逆解出目标位置的各轴角度后,将目标位置各轴角度与当前角度作差求出各轴转角,为了保证机械臂各轴同时到位,将各轴转角根据传动比等因素转换成电机转角,根据电机转角大小按比例分配电机转速;
或者,将其中某个转轴电机需要转动的角度分割成足够的小角度,计算出该轴电机转到该角度时其他电机的角度,根据比例关系也能得到机械臂其他各轴角度。
消毒控制模块,具体的,包括:
进行运动学正解估计路径机械臂末端的笛卡尔坐标(xi,yi,zi),如图4所示,机器人通过提前建立二维栅格地图已经将障碍物相对于世界坐标系XwYw的坐标信息以点云的方式记录在栅格地图中,机器人实时获取自身在栅格地图中的定位,可以得到世界坐标系与机器人坐标系之间的关系Twr,从而得到障碍物点云相对于自身的坐标信息XrYr,又已知机械臂和机器人中心的关系,可知机器人坐标系XrYr到机械臂坐标系XmYm之间的变换关系Trm,可将障碍物点云的坐标转换到相对于机械臂原点的笛卡尔转标系上,比较(xi,yi,zi)在水平面上的投影是否越过障碍物点构成的约束,如果是,则说明机械臂在运动过程中会碰到障碍物,需要进行路径规划来避障;否则,直接执行逆解出的角度控制机械臂到达目标位置进行消毒。
预测到会发生碰撞之后,开始进行避障路径规划。根据实际应用场景,墙壁等障碍物在竖直方向上的尺寸不变,因此将三维空间的路径规划转化为二维水平面上的路径规划问题,路径规划方法为:
将栅格地图上的障碍物进行膨化得到膨化曲线S(虚线),作为机械臂末端的安全工作界限;
从原路径与膨化曲线交点X0开始沿着膨化曲线向着目标点的方向以一定的步长选取路径点X1,进行从X1点到目标点的碰撞检测,如不发生碰撞,就停止路径点的寻找,避障路径就是起始点到X0再到X1最后到目标点;
如果发生碰撞,就从X1重复步骤2,直到找到的路径点Xi到目标点的过程中直接不发生碰撞;
将选出来的点进行二次B样条曲线进行拟合,使路径平缓,然后进行插值选取新的路径点,对于有的路径点曲率比较大,则将该路径点舍去
所述系统,还包括:
运动执行模块,被配置为:包括目标位置对应的角度下发和实时检测是否到达目标位置;
根据电机速度到零之后的当前编码器值与下发的期望编码器值之前的差值是否在误差允许范围之内判断机械臂是否到达目标位置到达位置之后再进行消毒操作;
对于实际应用场景,会碰到多目标点和单目标点的情况,对于单目标点,没有碰撞直接执行当前位置到目标位置的连续转动,到达位置完成消毒动作之后原路返回,有碰撞则进行运动规划分割得到路径点,依次执行,达到目标位置后原路返回,返回后与其他相关部分交互信息,执行下一步操作;
对于多目标点,如大型长门把手、一个位置有多个门把手等一次消毒操作不够的情况,会一次收到多个目标点,将目标点进行运动学逆解之后,将各轴角度存储下来依次执行,只不过是直到执行完最后一个目标点的位置之后才回零,且不需要原路返回,而是在当前位置以机械臂起始位置的各轴角度为目标位置,进行碰撞检测和运动规划;
多个目标点本质上和避障路径规划中的路径点没有太大区别,仅有的区别是目标点需要停下来进行喷洒消毒液的操作。
本领域内的技术人员应明白,本发明的实施例可提供为方法、系统、或计算机程序产品。因此,本发明可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (2)
1.一种用于高频接触物体消毒的四轴机械臂运动控制方法,其特征在于:
包括以下过程:
获取消毒目标的中心位置的空间坐标;
将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂坐标系得出机械臂各轴角度;
将目标位置各轴角度与当前角度作差求出各轴转角,将各轴转角转换成电机转角,根据电机转角大小按比例分配电机转速;
进行运动学正解估计机械臂末端的笛卡尔坐标,比较笛卡尔坐标在水平面上的投影是否越过障碍物点构成的约束,如果是,则判断机械臂在运动过程中会碰到障碍物,进行路径规划以规避障碍物;否则,直接根据逆解出的各轴转角控制机械臂到达目标位置进行消毒;
将栅格地图上的障碍物进行膨化得到膨化曲线,以膨化曲线作为机械臂末端的安全工作界限;
在障碍物膨化曲线、原路径非碰撞段、机械臂第一轴旋转中心与目标点连线线段位于膨化曲线与目标点之间的部分上选取点作为路径点,具体的:从原路径与膨化曲线交点X0开始沿着膨化曲线向着目标点的方向以一定的步长选取路径点X1,进行从X1点到目标点的碰撞检测,如不发生碰撞,就停止路径点的寻找,避障路径就是起始点到X0再到X1最后到目标点;如果发生碰撞,就从X1重复开始沿着膨化曲线向着目标点的方向以一定的步长选取路径点,直到找到的路径点Xi到目标点的过程中直接不发生碰撞;
用选出来的路径点进行二次B样条曲线拟合,进行插值选取新的路径点;
利用障碍物形状的规则性,将三维空间的障碍信息转换到二维xy平面上,对于多目标点,将各目标点进行运动学逆解之后,将各轴角度存储下来依次执行,直到执行完最后一个目标点的位置之后才回零,不需要原路返回,在当前位置以机械臂起始位置的各轴角度为目标位置,进行碰撞检测和运动规划;
对横式门把手内侧进行消毒时,将识别到的目标点的坐标向上偏移大于四轴的坐标,求逆解时将四轴机械臂运动简化为求三轴机械臂逆解,求出三轴末端到达目标点上方的逆解,到达位置后,将四轴旋转使四轴朝下进行消毒;
对高频接触物体进行消毒时,对目标位置坐标沿机械臂坐标系的Ym负方向偏移,然后求解三个轴的运动学逆解后,让机械臂四轴保持水平进行喷洒;
将各轴转角转换成电机转角,包括:
将各轴转角根据传动比转换成电机转角;或者,将其中某个转轴电机需要转动的角度分割成多个子角度,计算出该轴电机转到该子角度时其他电机的角度,根据比例关系得到机械臂其他各轴角度;
进行运动学正解估计机械臂末端的笛卡尔坐标,包括:
通过预建立的二维栅格地图将障碍物相对于世界坐标系XwYw的坐标信息以点云的方式记录在栅格地图中;实时获取机器人在栅格地图中的定位,得到世界坐标系与机器人坐标系之间的关系Twr,进而得到障碍物点云相对于自身的坐标信息XrYr;
根据机械臂和机器人中心的关系,得到机器人坐标系XrYr到机械臂坐标系XmYm之间的变换关系Trm,将障碍物点云的坐标转换到相对于机械臂原点的笛卡尔转标系上,得到机械臂末端的笛卡尔坐标;
根据电机速度到零之后的当前编码器值与下发的期望编码器值之前的差值是否在误差允许范围之内判断机械臂是否到达目标位置,在到达位置之后再进行消毒操作;
对于单目标点,没有碰撞直接执行当前位置到目标位置的连续转动,到达位置完成消毒动作之后原路返回,有碰撞则进行运动规划分割得到新的路径点,依次执行新的路径点,达到目标位置后原路返回。
2.一种用于高频接触物体消毒的四轴机械臂运动控制系统,其特征在于:
包括以下过程:
数据获取模块,被配置为:获取消毒目标的中心位置的空间坐标;
坐标转换模块,被配置为:将空间坐标通过运动学逆解由笛卡尔坐标转到机械臂坐标系得出机械臂各轴角度;
转角计算模块,被配置为:将目标位置各轴角度与当前角度作差求出各轴转角,将各轴转角转换成电机转角,根据电机转角大小按比例分配电机转速;
消毒控制模块,被配置为:进行运动学正解估计机械臂末端的笛卡尔坐标,比较笛卡尔坐标在水平面上的投影是否越过障碍物点构成的约束,如果是,则判断机械臂在运动过程中会碰到障碍物,进行路径规划以规避障碍物;否则,直接根据逆解出的各轴转角控制机械臂到达目标位置进行消毒;
将栅格地图上的障碍物进行膨化得到膨化曲线,以膨化曲线作为机械臂末端的安全工作界限;
在障碍物膨化曲线、原路径非碰撞段、机械臂第一轴旋转中心与目标点连线线段位于膨化曲线与目标点之间的部分上选取点作为路径点,具体的:从原路径与膨化曲线交点X0开始沿着膨化曲线向着目标点的方向以一定的步长选取路径点X1,进行从X1点到目标点的碰撞检测,如不发生碰撞,就停止路径点的寻找,避障路径就是起始点到X0再到X1最后到目标点;如果发生碰撞,就从X1重复开始沿着膨化曲线向着目标点的方向以一定的步长选取路径点,直到找到的路径点Xi到目标点的过程中直接不发生碰撞;
用选出来的路径点进行二次B样条曲线拟合,进行插值选取新的路径点;
利用障碍物形状的规则性,将三维空间的障碍信息转换到二维xy平面上,对于多目标点,将各目标点进行运动学逆解之后,将各轴角度存储下来依次执行,直到执行完最后一个目标点的位置之后才回零,不需要原路返回,在当前位置以机械臂起始位置的各轴角度为目标位置,进行碰撞检测和运动规划;
对横式门把手内侧进行消毒时,将识别到的目标点的坐标向上偏移大于四轴的坐标,求逆解时将四轴机械臂运动简化为求三轴机械臂逆解,求出三轴末端到达目标点上方的逆解,到达位置后,将四轴旋转使四轴朝下进行消毒;
对高频接触物体进行消毒时,对目标位置坐标沿机械臂坐标系的Ym负方向偏移,然后求解三个轴的运动学逆解后,让机械臂四轴保持水平进行喷洒;
将各轴转角转换成电机转角,包括:
将各轴转角根据传动比转换成电机转角;或者,将其中某个转轴电机需要转动的角度分割成多个子角度,计算出该轴电机转到该子角度时其他电机的角度,根据比例关系得到机械臂其他各轴角度;
进行运动学正解估计机械臂末端的笛卡尔坐标,包括:
通过预建立的二维栅格地图将障碍物相对于世界坐标系XwYw的坐标信息以点云的方式记录在栅格地图中;实时获取机器人在栅格地图中的定位,得到世界坐标系与机器人坐标系之间的关系Twr,进而得到障碍物点云相对于自身的坐标信息XrYr;
根据机械臂和机器人中心的关系,得到机器人坐标系XrYr到机械臂坐标系XmYm之间的变换关系Trm,将障碍物点云的坐标转换到相对于机械臂原点的笛卡尔转标系上,得到机械臂末端的笛卡尔坐标;
根据电机速度到零之后的当前编码器值与下发的期望编码器值之前的差值是否在误差允许范围之内判断机械臂是否到达目标位置,在到达位置之后再进行消毒操作;
对于单目标点,没有碰撞直接执行当前位置到目标位置的连续转动,到达位置完成消毒动作之后原路返回,有碰撞则进行运动规划分割得到新的路径点,依次执行新的路径点,达到目标位置后原路返回。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210902349.9A CN114952870B (zh) | 2022-07-29 | 2022-07-29 | 用于高频接触物体消毒的四轴机械臂运动控制方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210902349.9A CN114952870B (zh) | 2022-07-29 | 2022-07-29 | 用于高频接触物体消毒的四轴机械臂运动控制方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114952870A CN114952870A (zh) | 2022-08-30 |
CN114952870B true CN114952870B (zh) | 2023-09-29 |
Family
ID=82968749
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210902349.9A Active CN114952870B (zh) | 2022-07-29 | 2022-07-29 | 用于高频接触物体消毒的四轴机械臂运动控制方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114952870B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117140537B (zh) * | 2023-10-30 | 2024-01-16 | 中国标准化研究院 | 一种扫地机器人用机械臂控制系统及方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110228069A (zh) * | 2019-07-17 | 2019-09-13 | 东北大学 | 一种机械臂在线避障运动规划方法 |
CN111331612A (zh) * | 2020-03-11 | 2020-06-26 | 广东若铂智能机器人有限公司 | 一种基于视觉系统的智能长臂消毒机器人 |
CN112060109A (zh) * | 2020-09-29 | 2020-12-11 | 南京工程学院 | 一种室内拆弹机器人及其工作方法 |
CN112549016A (zh) * | 2020-10-21 | 2021-03-26 | 西安工程大学 | 一种机械臂运动规划方法 |
CN112828889A (zh) * | 2021-01-05 | 2021-05-25 | 佛山科学技术学院 | 一种六轴协作机械臂路径规划方法及系统 |
CN113211447A (zh) * | 2021-05-27 | 2021-08-06 | 山东大学 | 一种基于双向rrt*算法的机械臂实时感知规划方法及系统 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3953115A1 (en) * | 2019-04-06 | 2022-02-16 | Electric Sheep Robotic, Inc. | System, devices and methods for tele-operated robotics |
-
2022
- 2022-07-29 CN CN202210902349.9A patent/CN114952870B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110228069A (zh) * | 2019-07-17 | 2019-09-13 | 东北大学 | 一种机械臂在线避障运动规划方法 |
CN111331612A (zh) * | 2020-03-11 | 2020-06-26 | 广东若铂智能机器人有限公司 | 一种基于视觉系统的智能长臂消毒机器人 |
CN112060109A (zh) * | 2020-09-29 | 2020-12-11 | 南京工程学院 | 一种室内拆弹机器人及其工作方法 |
CN112549016A (zh) * | 2020-10-21 | 2021-03-26 | 西安工程大学 | 一种机械臂运动规划方法 |
CN112828889A (zh) * | 2021-01-05 | 2021-05-25 | 佛山科学技术学院 | 一种六轴协作机械臂路径规划方法及系统 |
CN113211447A (zh) * | 2021-05-27 | 2021-08-06 | 山东大学 | 一种基于双向rrt*算法的机械臂实时感知规划方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN114952870A (zh) | 2022-08-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP3410246B1 (en) | Robot obstacle avoidance control system and method, robot, and storage medium | |
CN108286949B (zh) | 一种可移动式三维检测机器人系统 | |
US11279033B2 (en) | Method and apparatus for collision-free motion planning of a manipulator | |
JP2023513841A (ja) | 産業ロボット用の衝突回避モーション計画方法 | |
Kaldestad et al. | Collision avoidance with potential fields based on parallel processing of 3D-point cloud data on the GPU | |
US20110196533A1 (en) | Method For Collision-Free Path Planning Of An Industrial Robot | |
CN114952870B (zh) | 用于高频接触物体消毒的四轴机械臂运动控制方法及系统 | |
Şucan et al. | Combining planning techniques for manipulation using realtime perception | |
CN112428274B (zh) | 一种多自由度机器人的空间运动规划方法 | |
Al-Sharo et al. | Generalized Procedure for Determining the Collision-Free Trajectory for a Robotic Arm | |
JP2023084115A (ja) | 点集合の干渉チェック | |
Nagai et al. | Three-dimensional obstacle avoidance online path-planning method for autonomous mobile overhead crane | |
Kimmel et al. | Scheduling pick-and-place tasks for dual-arm manipulators using incremental search on coordination diagrams | |
Li et al. | Hybrid trajectory replanning-based dynamic obstacle avoidance for physical human-robot interaction | |
Spencer et al. | Collision avoidance techniques for tele-operated and autonomous manipulators in overlapping workspaces | |
Vesentini et al. | Velocity obstacle-based trajectory planner for two-link planar manipulators | |
CN113146637B (zh) | 一种机器人笛卡尔空间的运动规划方法 | |
Richards et al. | Manipulator-based grasping pose selection by means of task-objective optimisation | |
CN111699079B (zh) | 协调系统、操作设备和方法 | |
Kuppan Chetty et al. | A heuristic approach towards path planning and obstacle avoidance control of planar manipulator | |
Pedrocchi et al. | Safe obstacle avoidance for industrial robot working without fences | |
Yang et al. | Collision avoidance trajectory planning for a dual-robot system: using a modified APF method | |
Kivelä et al. | Real-time distance query and collision avoidance for point clouds with heavy-duty redundant manipulator | |
Hujic et al. | An active prediction, planning and execution system for interception of moving objects | |
US20230109223A1 (en) | Intelligent clear path |
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 |