一种奇异点区域减速保护系统和工业机器人的制作方法
【专利摘要】本申请公开了一种奇异点区域减速保护系统和工业机器人，该系统应用于具有多个关节的工业机器人，具体为根据工业机器人的关节的实时状态数据和目标状态数据计算该关节的目标运动速度，实时状态数据包括关节的实时运动角度和实时时间，目标状态数据包括预设的运动任务所对应的目标运动角度和目标运动时间；然后在所述关节的位置处于奇异点过渡区域内时，计算关节的允许位置和允许速度；最后根据目标运动速度和允许速度对关节进行减速保护，即当关节的运动速度在奇异点过渡区域内的运动速度超出允许速度后及时控制器进行减速，以避免超速的发生，从而能够解决工业机器人因为在奇异点区域因速度变大而导致的停机或生产安全的问题。
【专利说明】
-种奇异点区域减速保护系统和工业机器人
技术领域
[0001] 本申请设及机器人技术领域，更具体地说，设及一种奇异点区域减速保护系统和 工业机器人。
【背景技术】
[0002] 工业机器人是工业自动化系统中重要的部件之一，当前，多个国家在工业机器人 技术上已经树立了一定的领先优势，尤其是在机器人本体的设计上，相关的基础技术已经 比较成熟，因而众多的机器人厂家在机器人应用上投入了大量精力。对工业机器人的奇异 点处理方法是机器人应用技术中一个主要的技术难点，是工业机器人必然遇到的技术问 题，工业机器人一旦遇到或接近奇异点，就会发生关节运动速度变大的现象，从而导致机器 人停机甚至带来生产安全问题。 【实用新型内容】
[0003] 有鉴于此，本申请提供一种奇异点区域减速保护系统和工业机器人，用于解决工 业机器人因为在奇异点区域因速度变大导致停机或生产安全的问题。
[0004] 为了实现上述目的，现提出的方案如下：
[0005] -种奇异点区域减速保护系统，应用于具有多个关节的工业机器人，包括：
[0006] 实时检测模块，用于根据所述工业机器人的关节的实时状态数据和目标状态数据 计算所述关节的目标运动速度，所述实时状态数据包括所述关节的实时运动角度和实时时 间，所述目标状态数据包括预设的运动任务所对应的目标运动角度和目标运动时间；
[0007] 奇异点区域计算模块，用于当所述关节的位置处于奇异点过渡区域内时，计算所 述关节的允许速度；
[000引奇异点处理模块，用于根据所述目标运动速度和所述允许速度对所述关节进行减 速保护。
[0009] 可选的，所述实时检测模块包括：
[0010] 本体信息单元，用于获取所述实时运动角度和所述实时时间；
[0011] 模型处理单元，用于根据所述实时运动角度、所述实时时间、所述目标运动角度和 所述目标运动时间计算所述目标运动速度。
[0012] 可选的，所述模型处理单元包括：
[0013] 正逆解子单元，用于根据所述运动任务计算所述关节的目标位置和目标姿态，并 根据所述目标位置和目标姿态计算出所述目标运动角度和所述目标运动时间；
[0014] 速度计算子单元，用于根据所述实时运动角度、所述实时时间、所述目标运动角度 和所述目标运动时间计算所述目标运动速度。
[0015] 可选的，所述实时检测模块还包括：
[0016] 警报单元，用于当所述关节的运动速度和位置处于异常状态时发出预设的控制指 令。
[0017] 可选的，所述警报单元包括：
[0018] 超速警报子单元，用于当所述运动速度超出预设的速度阔值时发出第一控制指 令，所述第一控制指令用于控制所述工业机器人停机；
[0019] 工作区域警报子单元，用于当所述工业机器人的末端的位置超出正常工作区域时 发出第二控制指令，所述第二控制指令用于控制所述工业机器人停机；
[0020] 奇异点区域警报子单元，用于根据对所述工业机器人的运动位置的判断发出运动 状态提不?目息。
[0021] -种工业机器人，设置有如上所述的奇异点区域减速保护系统。
[0022] 从上述的技术方案可W看出，本申请公开了一种奇异点区域减速保护系统和工业 机器人，该系统应用于具有多个关节的工业机器人，具体为根据工业机器人的关节的实时 状态数据和目标状态数据计算该关节的目标运动速度，实时状态数据包括关节的实时运动 角度和实时时间，目标状态数据包括预设的运动任务所对应的目标运动角度和目标运动 时间；然后在所述关节的位置处于奇异点过渡区域内时，计算关节的允许位置和允许速度； 最后根据目标运动速度和允许速度对关节进行减速保护，即当关节的运动速度在奇异点过 渡区域内的运动速度超出允许速度后及时控制器进行减速，W避免超速的发生，从而能够 解决工业机器人因为在奇异点区域因速度变大而导致的停机或生产安全的问题。
【附图说明】
[0023] 为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现 有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本 申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可W 根据运些附图获得其他的附图。
[0024] 图1为本申请实施例提供的一种奇异点区域减速保护方法的流程图
[0025] 图2为本申请提供的一种工业机器人的D-H模型示意图；
[0026] 图3为本申请另一实施例提供的一种奇异点区域减速保护系统的结构框图；
[0027] 图4为本申请例提供另一种奇异点区域减速保护系统的结构框图；
[0028] 图5为本申请例提供又一种奇异点区域减速保护系统的结构框图。
【具体实施方式】
[0029] 下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于 本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他 实施例，都属于本申请保护的范围。
[0030] 实施例一
[0031] 图1为本申请实施例提供的一种奇异点区域减速保护方法的流程图。
[0032] 如图1所示，本实施例提供的奇异点区域减速保护方法应用于具有多个关节的工 业机器人，为了方便描述，本实施例所选用的工业机器人具有6个关节。
[0033] 工业机器人通常是用D-H模型进行机器人的结构建模，如图2所示，每两个关节之 间的位置及姿态变换都可W使用四个参数的D-H矩阵进行模拟计算。最常见的6关节的工业 机器人结构可W用六个D-H关节变换矩阵来表示，分别为：
[0036] 其中，ci = cos(0i) ,si = sin(0i) ,1 = 1,2, . . .，5,6，θ?为第i关节的关节转动角度， 曰1为关节i到关节i+1之间的连杆长度，di为关节i-巧Ij关节i之间的连杆偏距。
[0037] 通过计算矩阵乘法可W得到机器人基座到机器人末端的变换矩阵：
[00；3 引
[0039] 基于上述分析，本实施例提供的奇异点区域减速保护方法包括下列步骤：
[0040] S101:根据工业机器人的关节的实时状态数据和目标状态数据计算关节的目标运 动速度。
[0041] 其中实时状态数据包括关节的实时运动角度和实时时间，目标状态数据包括预设 的运动任务所对应的目标运动角度和目标运动时间。目标运动速度的计算过程包括：
[0042] 步骤1:获取机器人关节的实时关节转动角度θιΚ及实时时间t。
[0043] 步骤2:将运动任务的目标位置和目标姿态采用封闭形式解法进行计算，解出关节 需要转动的目标运动角度θ'ι，并根据插补时间计算出运动任务的目标运动时间t'。然根 据后关节的实时运动角度θιΚ及需要转动的目标运动角度θ'ι，实时时间tW及运动任务的 目标运动时间t'，求解出运动任务的关节的目标运动速度：
[0044]
[0045] S102:当关节的位置处于奇异点过渡区域内时，计算关节的允许速度。
[0046] 首先，采用化S(阻尼最小方差)算法进行奇异值控制：
[0047]
[004引其中，J为机器人雅克比矩阵J'为加入阻尼因子后的雅克比矩阵，P2为阻尼因子。
[0049] 对阻尼因子的控制中加入奇异值控制：
[(K)加]
[0051]其中，δ为雅克比矩阵J的最小奇异值，ε为δ的最大阔值。
[0052] 当Tw2《I目日I <Twi时，设定运动速度阔值k。
[0化3]计算机器人在阔值Twi处的运动速度作为运动速度最大值(? ,1)然后对阔值 Twl处的运动速度进行归一化处理，未使用减速算法的运动速度与使用减速算法的运动速度
之比为 5.
[0054]使用关节5角度值作为减速算法的变量，计算得到的距离变量为
[0化5]经过减速算法修正过后的关节的允许速度为：
[0化6]
[0057] S103:根据目标运动速度和允许速度对关节进行减速保护。
[005引当关节为执行运动任务而达到的目标运动速度超出允许速度时，及时控制该关节 降低速度，使之不超出允许速度，从而避免出现超速。
[0059] 从上述技术方案可W看出，本实施例提供了一种奇异点区域减速保护方法，该方 法应用于具有多个关节的工业机器人，具体为根据工业机器人的关节的实时状态数据和目 标状态数据计算该关节的目标运动速度，实时状态数据包括关节的实时运动角度和实时时 间，目标状态数据包括预设的运动任务所对应的目标运动角度和目标运动时间；然后在关 节的位置处于奇异点过渡区域内时，计算关节的允许位置和允许速度;最后根据目标运动 速度和允许速度对关节进行减速保护，即当关节的运动速度在奇异点过渡区域内的运动速 度超出允许速度后及时控制器进行减速，W避免超速的发生，从而能够解决工业机器人因 为在奇异点区域因速度变大而导致的停机或生产安全的问题。
[0060] 本实施例中的根据工业机器人的关节的实时状态数据和目标状态数据计算关节 的目标运动速度的过程中，还包括步骤3,该步骤为警报步骤，用于在工业机器人的关节的 运动速度和位置处于异常状态时发出预设的控制指令。
[0061] 首先，针对6个关节的运动速度分别选取阔值Ti，i = l，2, . . .，5,6,进行判断：
[0062]
[0063] 对flagi的值件检测，当flagi = 0时，机器人运动超速，设备发出第一控制指令，该 第一控制指令用于控制工业机器人停机。
[0064] 同时，设定机器人的工作区域zone,然后对工业机器人的末端的位置p(x，y，z)进 行工作区域判断。
[00 化]
[0066] 对flagzone的值进行检测，当flagzone = 0时，机器人超出正常工作区域，运时发出第 二控制指令，该第二控制指令同样用于控制工业机器人停机，W避免因超出正常工作区域 造成安全事故。
[0067]最后，还对工业机器人的关节信号进行阔值判断，首先获得腕部奇异点判断值05。 针对奇异点分别选取奇异点阔值Tw，那么进行判断：
[006引
[0069] 对flagw的值进行检测，当flagw为0时此时，机器人运动至奇异点过渡区域，设备发 出奇异点过渡警报信息；当flagw为1时此时，机器人运动至奇异点减速区域，设备发出奇异 点减速警报信息。当flagw为2时此时，机器人正常运动。
[0070] 实施例二
[0071] 图3为本申请实施例提供的一种奇异点区域减速保护系统的结构框图。
[0072] 如图3所示，本实施例提供的奇异点区域减速保护系统应用于具有多个关节的工 业机器人，同样为了方便描述，本实施例所选用的工业机器人具有6个关节，该系统包括实 时检测模块10、奇异点区域计算模块20和奇异点处理模块30。
[0073] 实时检测模块10用于根据工业机器人的关节的实时状态数据和目标状态数据计 算关节的目标运动速度。
[0074] 其中实时状态数据包括关节的实时运动角度和实时时间，目标状态数据包括预设 的运动任务所对应的目标运动角度和目标运动时间。实时检测模块10包括本体信息单元11 和模型处理单元12,如图4所示。
[0075] 本体信息单元11用于获取机器人关节的实时关节转动角度θιΚ及实时时间t。
[0076] 模型处理单元12包括正逆解子单元121和速度计算子单元122。
[0077] 正逆解子单元121用于将运动任务的目标位置和目标姿态采用封闭形式解法进行 计算，解出关节需要转动的目标运动角度θ'ι，并根据插补时间计算出运动任务的目标运动 时间t'。
[0078] 速度计算子单元122用于根据关节的实时运动角度θιΚ及需要转动的目标运动角 度9 ' i，实时时间t W及运动任务的目标运动时间t '，求解出运动任务的关节的目标运动速 度：
[0079]
[0080] 奇异点区域计算模块20用于当关节的位置处于奇异点过渡区域内时，计算关节的 允许速度。
[0081] 首先，采用化S(阻尼最小方差)算法进行奇异值控制：
[0082] J'=jT(jjT+p2。-!
[0083] 其中，J为机器人雅克比矩阵，J'为加入阻尼因子后的雅克比矩阵，P2为阻尼因子。
[0084] 对阻尼因子的控制中加入奇异值控制：
[0085]
[0086] 其中，δ为雅克比矩阵J的最小奇异值，ε为δ的最大阔值。
[0087] 当Tw2《I目日I <Twi时，设定运动速度阔值k。
[008引计算机器人在阔值Twi处的运动速度作为运动速度最大值1然后对阔值 Twl处的运动速度进行归一化处理，未使用减速算法的运动速度与使用减速算法的运动速度 之比天
[0089] 使用关节5角度值作为减速算法的变量，计算得到的距离变量为
[0090] 经过减速算法修正过后的关节的允许速度为：
[0091]
'
[0092] 奇异点处理模块30用于根据目标运动速度和允许速度对关节进行减速保护。
[0093] 当关节为执行运动任务而达到的目标运动速度超出允许速度时，及时控制该关节 降低速度，使之不超出允许速度，从而避免出现超速。
[0094] 从上述技术方案可W看出，本实施例提供了一种奇异点区域减速保护系统，该系 统应用于具有多个关节的工业机器人，具体为根据工业机器人的关节的实时状态数据和目 标状态数据计算该关节的目标运动速度，实时状态数据包括关节的实时运动角度和实时时 间，目标状态数据包括预设的运动任务所对应的目标运动角度和目标运动时间；然后在关 节的位置处于奇异点过渡区域内时，计算关节的允许位置和允许速度;最后根据目标运动 速度和允许速度对关节进行减速保护，即当关节的运动速度在奇异点过渡区域内的运动速 度超出允许速度后及时控制器进行减速，W避免超速的发生，从而能够解决工业机器人因 为在奇异点区域因速度变大而导致的停机或生产安全的问题。
[00M]本实施例中的实时检测模块10中还包括警报单元13,该警报单元用于在工业机器 人的关节的运动速度和位置处于异常状态时发出预设的控制指令，具体包括超速警报子单 元131、工作区域警报子单元132和奇异点区域警报子单元133,如图5所示。
[0096] 超速警报子单元131针对6个关节的运动速度分别选取阔值Ti，i = l，2，. . .，5,6， 进行判断：
[0097]
[0098] 然后对flagi的值件检测，当flagi = 0时，机器人运动超速，发出第一控制指令，该 第一控制指令用于控制工业机器人停机。
[0099] 工作区域警报子单元132用于设定机器人的工作区域zone,然后对工业机器人的 末端的位置P (X，y，Z)进行工作区域判断。
[0100]
[0101] 对flagzone的值进行检测，当flagzone = 0时，机器人超出正常工作区域，运时发出第 二控制指令，该第二控制指令同样用于控制工业机器人停机，W避免因超出正常工作区域 造成安全事故。
[0102] 奇异点区域警报子单元133用于对工业机器人的关节信号进行阔值判断，首先获 得腕部奇异点判断值θ5。针对奇异点分别选取奇异点阔值Tw进行判断：
[0103]
[0104] 对flagw的值进行检测，当flagw为0时此时，机器人运动至奇异点过渡区域，设备发 出奇异点过渡警报信息；当flagw为1时此时，机器人运动至奇异点减速区域，设备发出奇异 点减速警报信息。当flagw为2时此时，机器人正常运动。
[0105] 实施例Ξ
[0106] 本申请还提供了一种工业机器人，该工业机器人设置有上面实施例所提供的奇异 点区域减速保护系统，用于使该工业机器人在奇异点区域不会出现超速现象，能够避免因 超速出现的停机或者安全事故。
[0107] 本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他 实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对所公开的实施例的上 述说明，使本领域专业技术人员能够实现或使用本申请。对运些实施例的多种修改对本领 域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可W在不脱离本申请的 精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的运些 实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
【主权项】
1. 一种奇异点区域减速保护系统，应用于具有多个关节的工业机器人，其特征在于，包 括： 实时检测模块，用于根据所述工业机器人的关节的实时状态数据和目标状态数据计算 所述关节的目标运动速度，所述实时状态数据包括所述关节的实时运动角度和实时时间， 所述目标状态数据包括预设的运动任务所对应的目标运动角度和目标运动时间； 奇异点区域计算模块，用于当所述关节的位置处于奇异点过渡区域内时，计算所述关 节的允许速度； 奇异点处理模块，用于根据所述目标运动速度、所述允许速度对所述关节进行减速保 护。2. 如权利要求1所述的奇异点区域减速保护系统，其特征在于，所述实时检测模块包 括： 本体信息单元，用于获取所述实时运动角度和所述实时时间； 模型处理单元，用于根据所述实时运动角度、所述实时时间、所述目标运动角度和所述 目标运动时间计算所述目标运动速度。3. 如权利要求2所述的奇异点区域减速保护系统，其特征在于，所述模型处理单元包 括： 正逆解子单元，用于根据所述运动任务计算所述关节的目标位置和目标姿态，并根据 所述目标位置和目标姿态计算出所述目标运动角度和所述目标运动时间； 速度计算子单元，用于根据所述实时运动角度、所述实时时间、所述目标运动角度和所 述目标运动时间计算所述目标运动速度。4. 如权利要求2或3所述的奇异点区域减速保护系统，其特征在于，所述实时检测模块 还包括： 警报单元，用于当所述关节的运动速度和位置处于异常状态时发出预设的控制指令。5. 如权利要求4所述的奇异点区域减速保护系统，其特征在于，所述警报单元包括： 超速警报子单元，用于当所述运动速度超出预设的速度阈值时发出第一控制指令，所 述第一控制指令用于控制所述工业机器人停机； 工作区域警报子单元，用于当所述工业机器人的末端的位置超出正常工作区域时发出 第二控制指令，所述第二控制指令用于控制所述工业机器人停机； 奇异点区域警报子单元，用于根据对所述工业机器人的运动位置的判断发出运动状态 提不?目息。6. -种工业机器人，其特征在于，设置有如权利要求1~5任一项所述的奇异点区域减 速保护系统。
【文档编号】B25J9/16GK205614678SQ201620076120
【公开日】2016年10月5日
【申请日】2016年1月25日
【发明人】王业率, 边慧杰, 赵天光
【申请人】珠海格力电器股份有限公司