CN114659688B - 双回路力矩感知***及其感知方法 - Google Patents
双回路力矩感知***及其感知方法 Download PDFInfo
- Publication number
- CN114659688B CN114659688B CN202111112421.XA CN202111112421A CN114659688B CN 114659688 B CN114659688 B CN 114659688B CN 202111112421 A CN202111112421 A CN 202111112421A CN 114659688 B CN114659688 B CN 114659688B
- Authority
- CN
- China
- Prior art keywords
- position sensor
- value
- absolute
- speed value
- absolute speed
- 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
- 238000000034 method Methods 0.000 title claims abstract description 39
- 230000002159 abnormal effect Effects 0.000 claims abstract description 57
- 238000001514 detection method Methods 0.000 claims abstract description 52
- 230000009977 dual effect Effects 0.000 claims description 14
- 230000005856 abnormality Effects 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 7
- 239000003638 chemical reducing agent Substances 0.000 claims description 5
- 230000002547 anomalous effect Effects 0.000 claims 1
- 238000004519 manufacturing process Methods 0.000 description 6
- 238000009434 installation Methods 0.000 description 4
- FFBHFFJDDLITSX-UHFFFAOYSA-N benzyl N-[2-hydroxy-4-(3-oxomorpholin-4-yl)phenyl]carbamate Chemical compound OC1=C(NC(=O)OCC2=CC=CC=C2)C=CC(=C1)N1CCOCC1=O FFBHFFJDDLITSX-UHFFFAOYSA-N 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000012423 maintenance Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L25/00—Testing or calibrating of apparatus for measuring force, torque, work, mechanical power, or mechanical efficiency
- G01L25/003—Testing or calibrating of apparatus for measuring force, torque, work, mechanical power, or mechanical efficiency for measuring torque
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
- G01L5/0061—Force sensors associated with industrial machines or actuators
- G01L5/0076—Force sensors associated with manufacturing machines
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L3/00—Measuring torque, work, mechanical power, or mechanical efficiency, in general
- G01L3/02—Rotary-transmission dynamometers
- G01L3/04—Rotary-transmission dynamometers wherein the torque-transmitting element comprises a torsionally-flexible shaft
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L3/00—Measuring torque, work, mechanical power, or mechanical efficiency, in general
- G01L3/02—Rotary-transmission dynamometers
- G01L3/04—Rotary-transmission dynamometers wherein the torque-transmitting element comprises a torsionally-flexible shaft
- G01L3/10—Rotary-transmission dynamometers wherein the torque-transmitting element comprises a torsionally-flexible shaft involving electric or magnetic means for indicating
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
- G01L5/0061—Force sensors associated with industrial machines or actuators
- G01L5/0071—Specific indicating arrangements, e.g. of overload
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Safety Devices In Control Systems (AREA)
- Force Measurement Appropriate To Specific Purposes (AREA)
- Retarders (AREA)
Abstract
一种双回路力矩感知***及其感知方法,在马达与减速机构设四位置传感器构成双回路进行检测而计算输出力矩,并利用位置传感器的检测,在回路或位置传感器产生异常时,发出失效的警报,以提升作业环境的安全性。
Description
技术领域
本发明涉及一种力矩感知***及其方法,尤其涉及双回路的力矩感知***及其感知方法,其为检测传感器与双回路是否正常运作的感知***及方法。
背景技术
机器手臂具有灵活移动、精确定位及连续性作业的特性,已成为产品生产线上制造组装的最佳利器。但是机器手臂是否正常的运作,直接影响到周边作业人员与环境的安全性,因此机器手臂作业的安全性已成为业界的重要课题。
现有技术一般获得机器手臂的力矩是在关节模块的马达1设置力矩传感器3,利用其内的应变规(strain gauge)测量马达1的力矩。或者,马达1的输出力矩传递至减速机构2,减速机构2减速输出力矩,并在减速机构2设置力矩传感器4,利用其内的应变规,进行输出力矩检测。但单一的力矩传感器,无法检测力矩传感器运作异常,导致影响作业安全。因此,如图1所示现有技术在马达1及减速机构2各设置力矩传感器3、4,各自感知力矩,一旦力矩数值不相符,即判断故障异常,将机器手臂停机,以确保作业安全性。
但对于力矩传感器的应用,在组装架设要求极为严格,且需经过复杂的校正程序与额外机构设计,避免其他方向的应力及力矩的干扰影响,方能正确检测各力矩,不仅导致机器手臂装设力矩传感器的结构复杂及成本提高,而且在力矩感知异常后,机器手臂即启动安全机制让机器手臂停机,造成生产中断,进而影响工厂的生产效率。此外,在力矩感知到异常时,无法判断何力矩传感器发生失效,因而必需停机后,一一检测才能找出故障的力矩传感器进行维修,相当耗费时间。因此,机器手臂在力矩感知***及感知方法上,仍有问题亟待解决。
发明内容
本发明的目的提供一种双回路力矩感知***及其感知方法,藉由在马达与减速机构设位置传感器,检测马达与减速机构的转动位置差异测量输出力矩,以简化力矩感知***的构造与安装。
本发明的另一目的提供一种双回路力矩感知***及其感知方法,利用马达与减速机构搭配四个位置传感器,构成双回路的力矩感知***,单一回路失效时交互替代,以提升作业环境的安全性。
本发明的再一目的提供一种双回路力矩感知***及其感知方法,利用四个位置传感器的检测,检知位置传感器是否发生故障,取代故障的位置传感器,避免停机,以提高生产效率。
为了达到前述发明的目的,本发明双回路力矩感知***,由马达连接减速机构减速输出力矩,减速机构连接第一位置传感器与第二位置传感器,检测减速机构转动状态,马达连接第三位置传感器与第四位置传感器,检测马达转动状态。在第一感知回路设置第一位置传感器与第三位置传感器,在第二感知回路设置第二位置传感器与第四位置传感器,控制装置耦接第一感知回路与第二感知回路。控制装置包含读取电路连接第一至第四位置传感器读取检测值,运算电路连接读取电路接收检测值进行力矩运算。比较电路连接运算电路接收运算电路的运算结果,并针对运算结果选择输出辨识信号或正常信号。搜寻电路,连接比较电路接收辨识信号,且根据辨识信号搜寻异常的位置传感器后,输出搜寻信号至力矩输出电路。力矩输出电路接收搜寻信号或正常信号,并依据第一感知回路的位置传感器的检测值输出第一力矩值或/和依据第二感知回路的位置传感器的检测值输出第二力矩值。
本发明双回路力矩感知方法,读取减速机构端两组位置传感器的检测值、及马达端两组位置传感器的检测值,在预设时间差T,第二次读取四组位置传感器的检测值,依据两次的检测值、预设时间差及减速比,运算四组位置传感器相对应的第一绝对速度值、第二绝对速度值、第三绝对速度值及第四绝对速度值。当比较绝对值的第一绝对速度值与第二绝对速度值的差小于判断阈值,且比较绝对值的第三绝对速度值与第四绝对速度值的差小于判断阈值,表示马达及减速机构侧的位置传感器正常,输出第一感知回路的第一力矩值及第二感知回路的第二力矩值。
本发明当绝对值的第一绝对速度值与第二绝对速度值的差,与绝对值的第三绝对速度值与第四绝对速度值的差,均大于判断阈值,表示第一位置传感器、第二位置传感器、第三位置传感器与第四位置传感器中至少两个位置传感器异常,并通知机器手臂停机。本发明当绝对值的第一绝对速度值与第二绝对速度值的差,或绝对值的第三绝对速度值与第四绝对速度值的差,大于判断阈值,表示第一位置传感器、第二位置传感器、第三位置传感器与第四位置传感器中有一个位置传感器异常,在机器手臂持续运作中发出维修警报。
附图说明
图1为已知马达、减速机构与力矩传感器的示意图。
图2为本发明双回路力矩感知***的方块图。
图3为本发明双回路力矩感知方法的流程图。
图4为本发明双回路力矩感知方法的搜寻异常编码器程序1的流程图。
图5为本发明双回路力矩感知方法的搜寻异常编码器程序2的流程图。
【符号说明】
10 马达
20 双回路力矩感知***
21 第一感知回路
22 第二感知回路
30 减速机构
41 第一位置传感器
42 第二位置传感器
43 第三位置传感器
44 第四位置传感器
50 控制装置
51 读取电路
52 运算电路
53 比较电路
54 搜寻电路
55 力矩输出电路
c 搜寻阈值
Cri 判断阈值
K 减速机参数
N 减速比
Sa 异常信号
Sd 搜寻信号
Si 辨识信号
Sn 正常信号
T 预设时间
F1 第一力矩值
F2 第二力矩值
具体实施方式
有关本发明为达成上述目的,所采用的技术手段及其功效,兹举实施例,并配合附图加以说明如下。
参阅图2为本发明双回路力矩感知***的方块图。本发明的双回路力矩感知***20的马达10连接减速机构30并产生输出力矩至减速机构30,减速机构30减速输出力矩并产生驱动力矩。减速机构30侧连接第一位置传感器41、第二位置传感器42,马达10侧连接第三位置传感器43与第四位置传感器44,在实施例中各位置传感器41-44可以例如为编码器,且只要各位置传感器41-44能对马达10与减速机构30进行检测即可,不限制各位置传感器41-44的设置位置。
本发明的双回路力矩感知***20将第一位置传感器41与第三位置传感器43设置于第一感知回路21,第三位置传感器43连接马达10端并检测其转动状态,第一位置传感器41连接减速机构30端并检测减速后转动状态。本发明另将第二位置传感器42与第四位置传感器44设置于第二感知回路22,第四位置传感器44连接马达10端并检测其转动状态,第二位置传感器42连接减速机构30端并检测减速后转动状态,其中转动状态可以是转动速度或转动角度等。
本发明的双回路力矩感知***20还包含控制装置50,控制装置50包含读取电路51、运算电路52、比较电路53、搜寻电路54与力矩输出电路55。其中读取电路51连接各位置传感器41-44并对各位置传感器41-44读取检测值P1、P2、P3、P4,读取电路51连接运算电路52并输出读取的检测值P1、P2、P3、P4至运算电路52进行运算,运算电路52连接比较电路53并输出运算结果至比较电路53。比较电路53可以针对不同运算结果选择输出辨识信号Si或正常信号Sn,比较电路53连接搜寻电路54与力矩输出电路55,并将辨识信号Si输出至搜寻电路54或将正常信号Sn输出至力矩输出电路55。搜寻电路54连接力矩输出电路55,并根据辨识信号Si在搜寻异常的位置传感器后输出搜寻信号Sd至力矩输出电路55。力矩输出电路55连接比较电路53与搜寻电路54,并依据各位置传感器41-44的状况输出第一力矩值F1或/和第二力矩值F2。
本发明的控制装置50耦接第一感知回路21与第二感知回路22,第一感知回路21的第一位置传感器41检测减速机构30减速后的驱动状态而产生第一检测值P1,第一感知回路21的第三位置传感器43检测马达10的转动状态而产生第三检测值P3。由于减速机构30的减速比为固定,第三位置传感器43检测马达10转动状态,与第一位置传感器41检测减速后转动状态(即驱动状态)具有减速比的固定比例关系,一旦经控制装置50利用读取电路51读取第一检测值P1及第三检测值P3传输至运算电路52计算,两者转动状态超出减速比的固定比例关系产生转动状态差异,例如转动位置的差异,就可利用位置差异比例算出由第一感知回路21检测后输出的第一力矩值F1,计算方法将在后段叙明。同理第二感知回路22的第二位置传感器42检测的第二检测值P2,及第四位置传感器44检测的第四检测值P4,也可利用位置差异比例算出由第二感知回路22检测后输出的第二力矩值F2。本发明双回路力矩感知***20就可利用各位置传感器41-44检测马达10的输出力矩变化,进行检测机器手臂在作业环境的碰撞,所以相当于检测机器手臂的运动状态,进而控制马达10的运作。因此,本发明在不使用力矩传感器下,就可藉由各位置传感器41-44检测马达10输出力矩与减速机构30驱动力矩,以取代设置力矩传感器,使本发明双回路力矩感知***20的构造与安装简单化。
此外,本发明的双回路力矩感知***20中第一位置传感器41、减速机构30、马达10与第三位置传感器43构成的第一感知回路21在失效时,第二位置传感器42、减速机构30、马达10与第四位置传感器44构成的第二感知回路22,仍可以正常运作保持作业环境的安全性,同理第二感知回路22失效时,第一感知回路21也可以正常运作保持作业环境的安全性。换句话说,本发明藉由马达10与减速机构30搭配四个位置传感器41-44,就可以双回路的力矩感知***,提升作业环境的安全性。
控制装置50辨识各位置传感器41-44、第一感知回路21与第二感知回路22正常与否时,控制装置50控制读取电路51在不同时间两次读取第一位置传感器41、第二位置传感器42、第三位置传感器43与第四位置传感器44,获得第一检测值P1、P1’、第二检测值P2、P2’、第三检测值P3、P3’与第四检测值P4、P4’所代表的两次不同转动位置,两次读取间的预设时间差T(或延迟时间)可以为数百毫秒或数十毫秒。再者,控制读取电路51输出读取的两次检测值P3、P3’、P4、P4’至运算电路52,运算电路52依据马达10的第三位置传感器43与第四位置传感器44的两次检测值P3、P3’、P4、P4’的转动位置差除以时间差T,分别运算出马达10转动的绝对速度,即第三位置传感器43检测而运算出的第三绝对速度值V3=(P3’-P3)/T,第四位置传感器44检测而运算出的第四绝对速度值V4=(P4’-P4)/T,因第三位置传感器43与第四位置传感器44检测同一马达10,第三绝对速度值V3与第四绝对速度值V4虽有运算误差但理论上应相等。同理,由减速机构30的第一位置传感器41与第二位置传感器42的两次检测值P1、P1’、P2、P2’,也可算出减速机构30减速后的绝对速度,但为与马达10的绝对速度进行比较,将其乘上减速比N,回算至马达10的绝对速度,即第一位置传感器41检测而运算出的第一绝对速度值V1=(P1’-P1)*N/T,第二位置传感器42检测而运算出的第二绝对速度值V2=(P2’-P2)*N/T,因第一位置传感器41与第二位置传感器42检测同一减速机构30,第一绝对速度值V1与第二绝对速度值V2虽有运算误差但理论上应相等,且应与第三绝对速度值V3与第四绝对速度值V4相等。
本发明的控制装置50控制运算电路52将运算结果传输至比较电路53,比较电路53依据接收的第一绝对速度值V1与第二绝对速度值V2,运算绝对值的第一绝对速度值V1与第二绝对速度值V2的差是否小于判断阈值Cri,如此本发明双回路力矩感知***20可以决定第一位置传感器41与第二位置传感器42是否异常。同理,运算绝对值的第三绝对速度值V3与第四绝对速度值V4的差是否小于判断阈值Cri,而决定第三位置传感器43与第四位置传感器44是否异常。
所以,本发明双回路力矩感知***20当绝对值的第一绝对速度值V1与第二绝对速度值V2的差,与绝对值的第三绝对速度值V3与第四绝对速度值V4的差,小于判断阈值Cri时,本发明双回路力矩感知***20决定第一位置传感器41、第二位置传感器42、第三位置传感器43与第四位置传感器44为正常位置传感器。因此,比较电路53将比对结果为正常的正常信号Sn传输至力矩输出电路55,力矩输出电路55接收后根据位置传感器的检测值P1、P2、P3、P4,开始计算第一感知回路21与第二感知回路22的第一力矩值F1与第二力矩值F2。例如将第一感知回路21中在马达10的第三位置传感器43的检测值P3,利用减速比N换算至减速后的状态,即P3*N/360,与减速机构30的第一位置传感器41的检测值P1形成转动差,再乘上减速机构30的减速机参数K,计算出第一感知回路21的输出力矩,即第一力矩值F1=K*(P1-P3*N/360)。同理,利用第二感知回路22中的第二位置传感器42的检测值P2与第四位置传感器44的检测值P4也可计算出第二感知回路22的输出力矩,即第二力矩值F2=K*(P2-P4*N/360)。
本发明双回路力矩感知***20当绝对值的第一绝对速度值V1与第二绝对速度值V2的差,与绝对值的第三绝对速度值V3与第四绝对速度值V4的差,均大于判断阈值Cri,本发明双回路力矩感知***20决定第一位置传感器41、第二位置传感器42、第三位置传感器43与第四位置传感器44中至少两个位置传感器异常,并通知机器手臂停机,且力矩输出电路55从比较电路53接收比对结果为异常的异常信号Sa而未进行力矩值计算及输出。本发明双回路力矩感知***20当绝对值的第一绝对速度值V1与第二绝对速度值V2的差,或绝对值的第三绝对速度值V3与第四绝对速度值V4的差,其中之一大于判断阈值Cri,双回路力矩感知***20检测出第一位置传感器41、第二位置传感器42、第三位置传感器43与第四位置传感器44中有一个位置传感器异常,并需进行异常位置传感器搜寻。
本发明双回路力矩感知***20的比较电路53通知搜寻电路54进行搜寻异常位置传感器,且当绝对值的第一绝对速度值V1与第二绝对速度值V2的差大于判断阈值Cri时,搜寻电路54将正常的第三绝对速度值V3或第四绝对速度值V4当作搜寻阈值c。将搜寻阈值c、第一绝对速度值V1与第二绝对速度值V2依大小排列,搜寻电路54搜寻离搜寻阈值c较远的位置传感器为异常。另当绝对值的第三绝对速度值V3与第四绝对速度值V4的差大于判断阈值Cri时,搜寻电路54将正常的第一绝对速度值V1或第二绝对速度值V2当作搜寻阈值c。将搜寻阈值c、第三绝对速度值V3与第四绝对速度值V4依大小排列,同样搜寻离搜寻阈值c较远的位置传感器为异常。
在搜寻电路54获得异常的位置传感器后,输出搜寻信号Sd通知力矩输出电路55。搜寻信号Sd可以用于表示异常的位置传感器,例如搜寻信号Sd的高低电平或正负值,如此,力矩输出电路55连接搜寻电路54而接收表示第一位置传感器41、第二位置传感器42、第三位置传感器43或第四位置传感器44为异常的搜寻信号Sd。当搜寻信号Sd表示第一位置传感器41或第三位置传感器43为异常时,力矩输出电路55输出第二力矩值F2,当搜寻信号Sd表示第二位置传感器42或第四位置传感器44为异常时,力矩输出电路55输出第一力矩值F1。
参阅图3,为本发明双回路力矩感知方法的流程图,本发明双回路力矩感知方法的详细步骤说明如下:步骤S10开始检测程序,首先步骤S11读取减速机构30端2组位置传感器的检测值P1、P2、及马达10端2组位置传感器的检测值P3、P4,并在步骤S12等待预设时间差T,而在步骤S13第二次读取4组位置传感器的检测值P1’、P2’、P3’、P4’。在步骤S14依据读取两次的检测值P1、P1’、P2、P2’、P3、P3’、P4、P4’、预设时间差T及减速比N,而运算4组位置传感器的绝对速度值,即
第一绝对速度值V1=|(P1’-P1)*N|/T
第二绝对速度值V2=|(P2’-P2)*N|/T
第三绝对速度值V3=|(P3’-P3)|/T
第四绝对速度值V4=|(P4’-P4)|/T。
步骤S15比较绝对值的第一绝对速度值V1与第二绝对速度值V2的差是否小于判断阈值Cri,若小于判断阈值Cri表示减速机构30端的位置传感器正常。接续步骤S16,比较绝对值的第三绝对速度值V3与第四绝对速度值V4的差是否小于判断阈值Cri,若判断小于判断阈值Cri表示马达10的位置传感器正常,如此在步骤S17检测4组位置传感器正常。再进入步骤S30,依据减速比N及减速机参数K,计算出第一感知回路21的第一力矩值F1=K*(P1-P3*N/360)及第二感知回路22的第二力矩值F2=K*(P2-P4*N/360),以双回路输出力矩。在步骤S15若判断不小于判断阈值Cri表示减速机构30的位置传感器异常,则进入步骤S18比较绝对值的第三绝对速度值V3与第四绝对速度值V4的差是否小于判断阈值Cri,若小于判断阈值Cri表示马达10的位置传感器正常,则进入步骤S40搜寻异常位置传感器程序1,否则表示马达10的位置传感器同为异常,需进入步骤S20表示两组以上位置传感器失效需停机维修。在步骤S16若判断不小于判断阈值Cri,表示马达10的位置传感器异常而减速机构30的位置传感器正常,因此需进入步骤S50搜寻异常位置传感器程序2。
参阅图4,为本发明双回路力矩感知***的搜寻异常位置传感器程序1的流程图。搜寻异常程序1开始搜寻异常位置传感器,当马达10的位置传感器正常而减速机构30的位置传感器异常时,搜寻的详细步骤如下:在步骤S40进入搜寻异常位置传感器的程序1,步骤S41编号减速机构30的2组位置传感器的绝对速度值V1及V2,为编号a=V1、编号b=V2,因马达10的位置传感器为正常,可以选择第三绝对速度值V3或第四绝对速度值V4之一设定为搜寻阈值c。进入步骤S42将绝对速度值由大到小排列。尔后,步骤S43开始判断,若三者数值的中间数值为编号a,进入步骤S44表示远离正常的搜寻阈值c的绝对速度值V2误差较大,相对应的第二位置传感器42为失效,第二位置传感器42构成的第二感知回路22为异常,进入步骤S34,计算出第一感知回路21的第一力矩值F1=K*(P1-P3*N/360),以第一感知回路21输出力矩,接着进入步骤60,警示第二位置传感器42失效。
在步骤43若判断三者数值的中间数值非为编号a,进入步骤S45开始判断三者数值的中间数值是否为编号b,若是则进入步骤S46表示远离正常值的搜寻阈值c的绝对速度值V1误差较大,相对应的第一位置传感器41为异常,第一位置传感器41构成的第一感知回路21为异常,进入步骤S32,计算出第二感知回路22的第二力矩值F2=K*(P2-P4*N/360),以第二感知回路22输出力矩,接着进入步骤62,警示第一位置传感器41失效。若搜寻阈值c位于编号a与编号b之间,则进入步骤S47,计算绝对值的搜寻阈值c与编号a的差异值并表示为Dx=|c-a|,及计算绝对值的搜寻阈值c与编号b的差异值并表示为Dy=|c-b|。运算后在步骤S48判断Dx是否大于Dy,若Dx较大则进入步骤S46,表示绝对速度值V1误差较大,相对应的第一位置传感器41为失效,第一位置传感器41构成的第一感知回路21异常,进入步骤S32,计算出第二感知回路22的第二力矩值F2=K*(P2-P4*N/360),以第二感知回路22输出力矩,接着进入步骤62,警示第一位置传感器41失效。若Dy较大则进入步骤S44,表示绝对速度值V2误差较大,相对应的第二位置传感器42为失效,第二位置传感器42构成的第二感知回路22异常,进入步骤S34,计算出第一感知回路21的第一力矩值F1=K*(P1-P3*N/360),以第一感知回路21输出力矩,接着进入步骤60,警示第二位置传感器42失效。
参阅图5,为本发明双回路力矩感知***的搜寻异常位置传感器程序2的流程图。搜寻异常程序2开始搜寻异常位置传感器,当减速机构30的位置传感器正常而马达10的位置传感器异常时,搜寻的详细步骤如下:在步骤S50进入搜寻异常位置传感器,步骤S51编号马达10的2组位置传感器的绝对速度值V3及V3,为编号a=V3、编号b=V4,因减速机构30的位置传感器为正常,可以选择第一绝对速度值V1或第二绝对速度值V2之一设定为搜寻阈值c。进入步骤S52将绝对速度值由大到小排列。尔后,步骤S53开始判断,若三者数值的中间数值为编号a,进入步骤S54表示远离正常的搜寻阈值c的绝对速度值V4误差较大,相对应的第四位置传感器44为异常,第四位置传感器44构成的第二感知回路22为异常,进入步骤S34,计算出第一感知回路21的第一力矩值F1=K*(P1-P3*N/360),以第一感知回路21输出力矩,接着进入步骤64,警示第四位置传感器44失效。
在步骤53若判断三者数值的中间数值非为编号a,进入步骤S55开始判断三者数值的中间数值是否为编号b,若是则进入步骤S56表示远离正常值的搜寻阈值c的绝对速度值V3误差较大,相对应的第三位置传感器43异常,第三位置传感器43构成的第一感知回路21为异常,进入步骤S32,计算出第二感知回路22的第二力矩值F2=K*(P2-P4*N/360),以第二感知回路22输出力矩,接着进入步骤66,警示第三位置传感器43失效。若搜寻阈值c位于编号a与编号b之间,则进入步骤S57,计算绝对值的搜寻阈值c与编号a的差异值并表示为Dx=|c-a|,及计算绝对值的搜寻阈值c与编号b的差异值并表示为Dy=|c-b|。运算后在步骤S58判断Dx是否大于Dy,若Dx较大则进入步骤S56,表示绝对速度值V3误差较大,相对应的第三位置传感器43失效,第三位置传感器43构成的第一感知回路21为异常,进入步骤S32,计算出第二感知回路22的第二力矩值F2=K*(P2-P4*N/360),以第二感知回路22输出力矩,接着进入步骤66,警示第三位置传感器43失效。若Dy较大则进入步骤S54,表示绝对速度值V4误差较大,相对应的第四位置传感器44失效,第四位置传感器44构成的第二感知回路22为异常,进入步骤S34,计算出第一感知回路21的第一力矩值F1=K*(P1-P3*N/360),以第一感知回路21输出力矩,接着进入步骤64,警示第四位置传感器44失效。
因此,本发明双回路力矩感知***就可藉由在马达与减速机构设位置传感器,取代设置高成本及设置条件严格的力矩传感器,而检测马达与减速机构的转动位置差异以测量输出力矩,达到简化力矩感知***的构造与安装的发明目的。另外,本发明利用马达与减速机构搭配四个位置传感器,构成双回路的力矩感知***,一回路失效时另一回路交互替代,达到提升作业环境的安全性。此外,利用四个位置传感器的检测,检测感知回路或位置传感器是否发生故障,而发出失效的警报待修,并另一感知回路持续运作,避免机器手臂停机,达到提高生产效率发明目的。
以上所述仅为用以方便说明本发明的实施例,本发明的范围不限于该等实施例,凡依本发明所做的任何变更,于不脱离本发明的精神下,皆属本发明申请专利的范围。
Claims (17)
1.一种双回路力矩感知***,其包含:
马达,连接减速机构减速输出力矩,减速机构连接第一位置传感器与第二位置传感器,检测减速机构转动状态,马达连接第三位置传感器与第四位置传感器,检测马达转动状态;
第一感知回路,设置第一位置传感器与第三位置传感器;
第二感知回路,设置第二位置传感器与第四位置传感器;及
控制装置,耦接第一感知回路与第二感知回路,控制装置包含:
读取电路,连接第一至第四位置传感器读取检测值;
运算电路,连接读取电路,接收检测值进行力矩运算;
比较电路,连接运算电路,接收运算电路的运算结果,并针对运算结果选择输出辨识信号或正常信号;
搜寻电路,连接比较电路,接收辨识信号,根据辨识信号搜寻异常的位置传感器,输出搜寻信号;
力矩输出电路,接收搜寻信号或正常信号,依据第一感知回路的位置传感器的检测值输出第一力矩值或/和依据第二感知回路的位置传感器的检测值输出第二力矩值。
2.如权利要求1所述的双回路力矩感知***,其中运算电路接收第一感知回路的第一位置传感器检测减速机构而产生的第一检测值P1,及第三位置传感器检测马达而产生的第三检测值P3,并依据减速机构的减速机参数K及减速比N,计算第一力矩值F1=K*(P1-P3*N/360)。
3.如权利要求1所述的双回路力矩感知***,其中运算电路接收第二感知回路的第二位置传感器检测减速机构而产生的第二检测值P2,及第四位置传感器检测马达而产生的第四检测值P4,并依据减速机构的减速机参数K及减速比N,计算第二力矩值F2=K*(P2-P4*N/360)。
4.如权利要求1所述的双回路力矩感知***,其中读取电路在预设时间差分别读取减速机构侧的第一位置传感器、第二位置传感器、马达侧的第三位置传感器与第四位置传感器的两次检测值,由运算电路依据两次检测值与预设时间差分别运算出减速机构侧的位置传感器相对应的第一绝对速度值、第二绝对速度值、马达侧的第三绝对速度值与第四绝对速度值,而比对减速比减速前后的绝对速度,判断各位置传感器的异常。
5.如权利要求4所述的双回路力矩感知***,其中比较电路接收第一绝对速度值与第二绝对速度值,并运算绝对值的第一绝对速度值与第二绝对速度值的差是否小于判断阈值,决定减速机构侧的位置传感器是否异常;比较电路接收第三绝对速度值与第四绝对速度值,并运算绝对值的第三绝对速度值与第四绝对速度值的差是否小于判断阈值,决定马达侧的位置传感器是否异常。
6.如权利要求5所述的双回路力矩感知***,其中比较电路判断第一位置传感器、第二位置传感器、第三位置传感器与第四位置传感器为正常位置传感器时,将正常信号传输至力矩输出电路。
7.如权利要求5所述的双回路力矩感知***,其中比较电路判断第一位置传感器、第二位置传感器、第三位置传感器或第四位置传感器为异常位置传感器时,将辨识信号传输至搜寻电路,搜寻异常的位置传感器。
8.一种双回路力矩感知方法,其步骤包含:
连接马达的输出至减速机构,连接减速机构的输出至第一位置传感器与第二位置传感器,以检测减速机构的转动状态;
连接第三位置传感器与第四位置传感器至马达,以检测马达的转动状态;读取减速机构的2组位置传感器的第一次检测值及马达的2组位置传感器的第一次检测值;
在预设时间差T,第二次读取4组位置传感器的检测值;
以第二次的检测值、第一次检测值、减速比与预设时间差,运算4组位置传感器相对应的第一绝对速度值、第二绝对速度值、第三绝对速度值及第四绝对速度值;
比较绝对值的第一绝对速度值与第二绝对速度值的差小于判断阈值,且比较绝对值的第三绝对速度值与第四绝对速度值的差小于判断阈值,表示马达及减速机构的位置传感器正常;及
输出第一力矩值及第二力矩值。
9.如权利要求8所述的双回路力矩感知方法,其中当绝对值的第一绝对速度值与第二绝对速度值的差,与绝对值的第三绝对速度值与第四绝对速度值的差,大于判断阈值,第一位置传感器、第二位置传感器、第三位置传感器与第四位置传感器中至少两个位置传感器异常,并通知机器手臂停机。
10.如权利要求8所述的双回路力矩感知方法,其中当绝对值的第一绝对速度值与第二绝对速度值的差,或绝对值的第三绝对速度值与第四绝对速度值的差,大于判断阈值,第一位置传感器、第二位置传感器、第三位置传感器与第四位置传感器中有一个位置传感器异常。
11.如权利要求10所述的双回路力矩感知方法,其中当运算绝对值的第一绝对速度值与第二绝对速度值的差大于判断阈值时,将第三绝对速度值或第四绝对速度值当作搜寻阈值,而搜寻第一位置传感器或第二位置传感器为异常。
12.如权利要求11所述的双回路力矩感知方法,其中当搜寻阈值位于第一绝对速度值与第二绝对速度值之间时,决定搜寻阈值与第一绝对速度值的差是否大于搜寻阈值与第二绝对速度值的差,而搜寻第一位置传感器或第二位置传感器为异常。
13.如权利要求11所述的双回路力矩感知方法,其中运算绝对值的第三绝对速度值与第四绝对速度值的差大于判断阈值时,将第一绝对速度值或第二绝对速度值当作搜寻阈值,而搜寻第三位置传感器或第四位置传感器为异常。
14.如权利要求13所述的双回路力矩感知方法,其中当搜寻阈值位于第三绝对速度值与第四绝对速度值之间时,决定搜寻阈值与第三绝对速度值的差是否大于搜寻阈值与第四绝对速度值的差,而搜寻第三位置传感器或第四位置传感器为异常。
15.如权利要求8所述的双回路力矩感知方法,其中控制装置包含力矩输出电路连接比较电路而接收表示全部位置传感器为正常的正常信号,并开始依据减速机参数、减速比与四个检测值计算第一感知回路与第二感知回路的第一力矩值与第二力矩值。
16.如权利要求9所述的双回路力矩感知方法,其中控制装置包含力矩输出电路连接比较电路而接收表示位置传感器为异常的异常信号,并未进行力矩值计算。
17.如权利要求10所述的双回路力矩感知方法,其中当搜寻信号表示第一位置传感器或第三位置传感器为异常时,力矩输出电路输出第二力矩值,当搜寻信号表示第二位置传感器或第四位置传感器为异常时,力矩输出电路输出第一力矩值。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW109145868A TWI805978B (zh) | 2020-12-22 | 2020-12-22 | 雙迴路力矩感知系統及其感知方法 |
TW109145868 | 2020-12-22 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114659688A CN114659688A (zh) | 2022-06-24 |
CN114659688B true CN114659688B (zh) | 2024-05-28 |
Family
ID=82022410
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111112421.XA Active CN114659688B (zh) | 2020-12-22 | 2021-09-23 | 双回路力矩感知***及其感知方法 |
Country Status (3)
Country | Link |
---|---|
US (1) | US11846556B2 (zh) |
CN (1) | CN114659688B (zh) |
TW (1) | TWI805978B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS63154088A (ja) * | 1986-12-15 | 1988-06-27 | Agency Of Ind Science & Technol | 減速機付モ−タの力・トルク制御方式 |
JP2005024509A (ja) * | 2003-07-03 | 2005-01-27 | Calsonic Kansei Corp | トルクセンサの故障診断方法 |
JP2005037334A (ja) * | 2003-07-18 | 2005-02-10 | Calsonic Kansei Corp | トルクセンサの故障診断方法 |
JP2006102889A (ja) * | 2004-10-06 | 2006-04-20 | Yaskawa Electric Corp | 減速機の異常判定装置及び減速機の異常判定方法 |
JP2007301680A (ja) * | 2006-05-11 | 2007-11-22 | Nachi Fujikoshi Corp | ロボットアーム診断装置 |
JP2007301691A (ja) * | 2006-05-12 | 2007-11-22 | Nachi Fujikoshi Corp | ロボット制御装置 |
KR20190056732A (ko) * | 2017-11-17 | 2019-05-27 | 주식회사 만도 | 액티브 롤 제어 시스템의 사용 전류 제한 시스템 및 방법 |
Family Cites Families (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3923053B2 (ja) * | 2004-03-31 | 2007-05-30 | ファナック株式会社 | ロボット教示装置 |
JP2006174690A (ja) * | 2004-11-18 | 2006-06-29 | Smc Corp | アクチュエータ制御システム |
TWI490513B (zh) * | 2006-12-29 | 2015-07-01 | Intest Corp | 用於使負載沿平移軸線平移之負載定位系統以及使負載達到平衡之方法 |
TWI492826B (zh) * | 2007-06-27 | 2015-07-21 | Brooks Automation Inc | 具有磁浮主軸軸承的自動機械驅動器 |
JP4335286B2 (ja) * | 2008-02-08 | 2009-09-30 | ファナック株式会社 | 部品保護機能を備えたロボット制御装置及びロボット制御方法 |
JP5365595B2 (ja) * | 2010-09-15 | 2013-12-11 | 株式会社安川電機 | 減速機の異常判定方法、異常判定装置、ロボット及びロボットシステム |
US8930128B2 (en) * | 2012-02-27 | 2015-01-06 | Lit Motors Corporation | Vehicle collision mitigation system |
KR101922495B1 (ko) * | 2013-04-25 | 2018-11-27 | 주식회사 만도 | 토크 신뢰성 제공 방법 및 제어 장치 |
JP6078444B2 (ja) * | 2013-09-20 | 2017-02-08 | 日立オートモティブシステムズ株式会社 | パワーステアリング装置および車両搭載機器の制御装置 |
WO2016184451A1 (de) * | 2015-05-21 | 2016-11-24 | Kastanienbaum GmbH | Verfahren und vorrichtung zur steuerung/regelung eines aktorisch angetriebenen robotergelenks |
AU2016342272A1 (en) * | 2015-10-23 | 2018-05-10 | Yamaha Hatsudoki Kabushiki Kaisha | Robot and robot control system |
US10562191B2 (en) * | 2015-12-29 | 2020-02-18 | Robomotive Laboratories LLC | Method of controlling devices with sensation of applied force |
TWI622865B (zh) * | 2016-04-29 | 2018-05-01 | 財團法人工業技術研究院 | 用於機器人的直覺式力量導引方法及其裝置 |
JP6692762B2 (ja) * | 2017-02-13 | 2020-05-13 | 日本電産コパル電子株式会社 | トルクセンサ |
US11659819B2 (en) * | 2018-10-05 | 2023-05-30 | X Development Llc | Sensor positioning system |
WO2020194597A1 (ja) * | 2019-03-27 | 2020-10-01 | 日産自動車株式会社 | 異常検知装置、及び異常検知方法 |
-
2020
- 2020-12-22 TW TW109145868A patent/TWI805978B/zh active
-
2021
- 2021-09-23 CN CN202111112421.XA patent/CN114659688B/zh active Active
- 2021-11-04 US US17/519,534 patent/US11846556B2/en active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS63154088A (ja) * | 1986-12-15 | 1988-06-27 | Agency Of Ind Science & Technol | 減速機付モ−タの力・トルク制御方式 |
JP2005024509A (ja) * | 2003-07-03 | 2005-01-27 | Calsonic Kansei Corp | トルクセンサの故障診断方法 |
JP2005037334A (ja) * | 2003-07-18 | 2005-02-10 | Calsonic Kansei Corp | トルクセンサの故障診断方法 |
JP2006102889A (ja) * | 2004-10-06 | 2006-04-20 | Yaskawa Electric Corp | 減速機の異常判定装置及び減速機の異常判定方法 |
JP2007301680A (ja) * | 2006-05-11 | 2007-11-22 | Nachi Fujikoshi Corp | ロボットアーム診断装置 |
JP2007301691A (ja) * | 2006-05-12 | 2007-11-22 | Nachi Fujikoshi Corp | ロボット制御装置 |
KR20190056732A (ko) * | 2017-11-17 | 2019-05-27 | 주식회사 만도 | 액티브 롤 제어 시스템의 사용 전류 제한 시스템 및 방법 |
Also Published As
Publication number | Publication date |
---|---|
TWI805978B (zh) | 2023-06-21 |
TW202225692A (zh) | 2022-07-01 |
CN114659688A (zh) | 2022-06-24 |
US20220196504A1 (en) | 2022-06-23 |
US11846556B2 (en) | 2023-12-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11824333B2 (en) | Systems for monitoring the operation of an energy chain | |
JP2015508367A (ja) | エレベータケージの移動動作を監視する方法および制御装置 | |
JP5367623B2 (ja) | サーボシステム、サーボモータ駆動装置、セーフティユニットおよびサーボシステムの制御方法 | |
US6237479B1 (en) | Servo press controller | |
CN108931261B (zh) | 编码器***和异常检测方法 | |
EP2216626B1 (en) | Sensor Apparatus | |
US20220152821A1 (en) | Robot safety weight compensation system and method capable of compensating weight of robot | |
CN106976771B (zh) | 用于多轿厢无绳电梯***的传感器故障检测和融合*** | |
US20080190166A1 (en) | Device and Method for Error-Protected Detection of Measured Values in a Control Unit | |
CN114659688B (zh) | 双回路力矩感知***及其感知方法 | |
KR101901127B1 (ko) | 모터 고장 판단 장치 및 방법 | |
CN110687522B (zh) | 室内配送车的红外***校准方法、电子设备及存储介质 | |
US10254742B2 (en) | Electric drive system | |
JP5215188B2 (ja) | 駆動装置のための監視装置 | |
CN111964883B (zh) | 一种摆动结构中运动零件位置的检测方法 | |
CN110757878B (zh) | 一种伺服压力机的压力故障检测***及方法 | |
US8618808B2 (en) | Field device | |
JP4881702B2 (ja) | モータ制御装置 | |
US7873484B2 (en) | Method for reliable position monitoring | |
CN115675580A (zh) | 一种轨道列车全冗余测速方法及*** | |
CN115135566A (zh) | 电子控制装置、电动动力转向装置以及电动动力转向装置的控制装置 | |
WO2021053717A1 (ja) | 異常判定装置及び異常判定方法 | |
US20030130816A1 (en) | Device for reliable position monitoring | |
CN113483802B (zh) | 一种防止装盒机机械传动异常的快速预警保护***及方法 | |
JP2000193489A (ja) | 位置検出装置およびそれに用いられるケ―ブル |
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 |