双回路力矩感知系统及其感知方法与流程

1.本发明涉及一种力矩感知系统及其方法，尤其涉及双回路的力矩感知系统及其感知方法，其为检测传感器与双回路是否正常运作的感知系统及方法。


背景技术：

2.机器手臂具有灵活移动、精确定位及连续性作业的特性，已成为产品生产线上制造组装的最佳利器。但是机器手臂是否正常的运作，直接影响到周边作业人员与环境的安全性，因此机器手臂作业的安全性已成为业界的重要课题。
3.现有技术一般获得机器手臂的力矩是在关节模块的马达1设置力矩传感器3，利用其内的应变规(strain gauge)测量马达1的力矩。或者，马达1的输出力矩传递至减速机构2，减速机构2减速输出力矩，并在减速机构2设置力矩传感器4，利用其内的应变规，进行输出力矩检测。但单一的力矩传感器，无法检测力矩传感器运作异常，导致影响作业安全。因此，如图1所示现有技术在马达1及减速机构2各设置力矩传感器3、4，各自感知力矩，一旦力矩数值不相符，即判断故障异常，将机器手臂停机，以确保作业安全性。
4.但对于力矩传感器的应用，在组装架设要求极为严格，且需经过复杂的校正程序与额外机构设计，避免其他方向的应力及力矩的干扰影响，方能正确检测各力矩，不仅导致机器手臂装设力矩传感器的结构复杂及成本提高，而且在力矩感知异常后，机器手臂即启动安全机制让机器手臂停机，造成生产中断，进而影响工厂的生产效率。此外，在力矩感知到异常时，无法判断何力矩传感器发生失效，因而必需停机后，一一检测才能找出故障的力矩传感器进行维修，相当耗费时间。因此，机器手臂在力矩感知系统及感知方法上，仍有问题亟待解决。


技术实现要素：

5.本发明的目的提供一种双回路力矩感知系统及其感知方法，藉由在马达与减速机构设位置传感器，检测马达与减速机构的转动位置差异测量输出力矩，以简化力矩感知系统的构造与安装。
6.本发明的另一目的提供一种双回路力矩感知系统及其感知方法，利用马达与减速机构搭配四个位置传感器，构成双回路的力矩感知系统，单一回路失效时交互替代，以提升作业环境的安全性。
7.本发明的再一目的提供一种双回路力矩感知系统及其感知方法，利用四个位置传感器的检测，检知位置传感器是否发生故障，取代故障的位置传感器，避免停机，以提高生产效率。
8.为了达到前述发明的目的，本发明双回路力矩感知系统，由马达连接减速机构减速输出力矩，减速机构连接第一位置传感器与第二位置传感器，检测减速机构转动状态，马达连接第三位置传感器与第四位置传感器，检测马达转动状态。在第一感知回路设置第一位置传感器与第三位置传感器，在第二感知回路设置第二位置传感器与第四位置传感器，
控制装置耦接第一感知回路与第二感知回路。控制装置包含读取电路连接第一至第四位置传感器读取检测值，运算电路连接读取电路接收检测值进行力矩运算。比较电路连接运算电路接收运算电路的运算结果，并针对运算结果选择输出辨识信号或正常信号。搜寻电路，连接比较电路接收辨识信号，且根据辨识信号搜寻异常的位置传感器后，输出搜寻信号至力矩输出电路。力矩输出电路接收搜寻信号或正常信号，并依据第一感知回路的位置传感器的检测值输出第一力矩值或/和依据第二感知回路的位置传感器的检测值输出第二力矩值。
9.本发明双回路力矩感知方法，读取减速机构端两组位置传感器的检测值、及马达端两组位置传感器的检测值，在预设时间差t，第二次读取四组位置传感器的检测值，依据两次的检测值、预设时间差及减速比，运算四组位置传感器相对应的第一绝对速度值、第二绝对速度值、第三绝对速度值及第四绝对速度值。当比较绝对值的第一绝对速度值与第二绝对速度值的差小于判断阈值，且比较绝对值的第三绝对速度值与第四绝对速度值的差小于判断阈值，表示马达及减速机构侧的位置传感器正常，输出第一感知回路的第一力矩值及第二感知回路的第二力矩值。
10.本发明当绝对值的第一绝对速度值与第二绝对速度值的差，与绝对值的第三绝对速度值与第四绝对速度值的差，均大于判断阈值，表示第一位置传感器、第二位置传感器、第三位置传感器与第四位置传感器中至少两个位置传感器异常，并通知机器手臂停机。本发明当绝对值的第一绝对速度值与第二绝对速度值的差，或绝对值的第三绝对速度值与第四绝对速度值的差，大于判断阈值，表示第一位置传感器、第二位置传感器、第三位置传感器与第四位置传感器中有一个位置传感器异常，在机器手臂持续运作中发出维修警报。
附图说明
11.图1为已知马达、减速机构与力矩传感器的示意图。
12.图2为本发明双回路力矩感知系统的方块图。
13.图3为本发明双回路力矩感知方法的流程图。
14.图4为本发明双回路力矩感知方法的搜寻异常编码器程序1的流程图。
15.图5为本发明双回路力矩感知方法的搜寻异常编码器程序2的流程图。
16.【符号说明】
17.10
ꢀꢀꢀ
马达
18.20
ꢀꢀꢀ
双回路力矩感知系统
19.21
ꢀꢀꢀ
第一感知回路
20.22
ꢀꢀꢀ
第二感知回路
21.30
ꢀꢀꢀ
减速机构
22.41
ꢀꢀꢀ
第一位置传感器
23.42
ꢀꢀꢀ
第二位置传感器
24.43
ꢀꢀꢀ
第三位置传感器
25.44
ꢀꢀꢀ
第四位置传感器
26.50
ꢀꢀꢀ
控制装置
27.51
ꢀꢀꢀ
读取电路
28.52
ꢀꢀꢀ
运算电路
29.53
ꢀꢀꢀ
比较电路
30.54
ꢀꢀꢀ
搜寻电路
31.55
ꢀꢀꢀ
力矩输出电路
32.c
ꢀꢀꢀꢀ
搜寻阈值
33.cri
ꢀꢀ
判断阈值
34.k
ꢀꢀꢀꢀ
减速机参数
35.n
ꢀꢀꢀꢀ
减速比
36.sa
ꢀꢀꢀ
异常信号
37.sd
ꢀꢀꢀ
搜寻信号
38.si
ꢀꢀꢀ
辨识信号
39.sn
ꢀꢀꢀ
正常信号
40.t
ꢀꢀꢀꢀ
预设时间
41.f1
ꢀꢀꢀ
第一力矩值
42.f2
ꢀꢀꢀ
第二力矩值
具体实施方式
43.有关本发明为达成上述目的，所采用的技术手段及其功效，兹举实施例，并配合附图加以说明如下。
44.参阅图2为本发明双回路力矩感知系统的方块图。本发明的双回路力矩感知系统20的马达10连接减速机构30并产生输出力矩至减速机构30，减速机构30减速输出力矩并产生驱动力矩。减速机构30侧连接第一位置传感器41、第二位置传感器42，马达10侧连接第三位置传感器43与第四位置传感器44，在实施例中各位置传感器41-44可以例如为编码器，且只要各位置传感器41-44能对马达10与减速机构30进行检测即可，不限制各位置传感器41-44的设置位置。
45.本发明的双回路力矩感知系统20将第一位置传感器41与第三位置传感器43设置于第一感知回路21，第三位置传感器43连接马达10端并检测其转动状态，第一位置传感器41连接减速机构30端并检测减速后转动状态。本发明另将第二位置传感器42与第四位置传感器44设置于第二感知回路22，第四位置传感器44连接马达10端并检测其转动状态，第二位置传感器42连接减速机构30端并检测减速后转动状态，其中转动状态可以是转动速度或转动角度等。
46.本发明的双回路力矩感知系统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。
47.本发明的控制装置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的构造与安装简单化。
48.此外，本发明的双回路力矩感知系统20中第一位置传感器41、减速机构30、马达10与第三位置传感器43构成的第一感知回路21在失效时，第二位置传感器42、减速机构30、马达10与第四位置传感器44构成的第二感知回路22，仍可以正常运作保持作业环境的安全性，同理第二感知回路22失效时，第一感知回路21也可以正常运作保持作业环境的安全性。换句话说，本发明藉由马达10与减速机构30搭配四个位置传感器41-44，就可以双回路的力矩感知系统，提升作业环境的安全性。
49.控制装置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.本发明的控制装置50控制运算电路52将运算结果传输至比较电路53，比较电路53依据接收的第一绝对速度值v1与第二绝对速度值v2，运算绝对值的第一绝对速度值v1与第二绝对速度值v2的差是否小于判断阈值cri，如此本发明双回路力矩感知系统20可以决定第一位置传感器41与第二位置传感器42是否异常。同理，运算绝对值的第三绝对速度值v3与第四绝对速度值v4的差是否小于判断阈值cri，而决定第三位置传感器43与第四位置传感器44是否异常。
51.所以，本发明双回路力矩感知系统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)。
52.本发明双回路力矩感知系统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中有一个位置传感器异常，并需进行异常位置传感器搜寻。
53.本发明双回路力矩感知系统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.在搜寻电路54获得异常的位置传感器后，输出搜寻信号sd通知力矩输出电路55。搜寻信号sd可以用于表示异常的位置传感器，例如搜寻信号sd的高低电平或正负值，如此，力矩输出电路55连接搜寻电路54而接收表示第一位置传感器41、第二位置传感器42、第三
位置传感器43或第四位置传感器44为异常的搜寻信号sd。当搜寻信号sd表示第一位置传感器41或第三位置传感器43为异常时，力矩输出电路55输出第二力矩值f2，当搜寻信号sd表示第二位置传感器42或第四位置传感器44为异常时，力矩输出电路55输出第一力矩值f1。
55.参阅图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组位置传感器的绝对速度值，即
56.第一绝对速度值v1＝|(p1
’‑
p1)*n|/t
57.第二绝对速度值v2＝|(p2
’‑
p2)*n|/t
58.第三绝对速度值v3＝|(p3
’‑
p3)|/t
59.第四绝对速度值v4＝|(p4
’‑
p4)|/t。
60.步骤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。
61.参阅图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失效。
62.在步骤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失效。
63.参阅图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失效。
64.在步骤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失效。
65.因此，本发明双回路力矩感知系统就可藉由在马达与减速机构设位置传感器，取代设置高成本及设置条件严格的力矩传感器，而检测马达与减速机构的转动位置差异以测量输出力矩，达到简化力矩感知系统的构造与安装的发明目的。另外，本发明利用马达与减速机构搭配四个位置传感器，构成双回路的力矩感知系统，一回路失效时另一回路交互替代，达到提升作业环境的安全性。此外，利用四个位置传感器的检测，检测感知回路或位置
传感器是否发生故障，而发出失效的警报待修，并另一感知回路持续运作，避免机器手臂停机，达到提高生产效率发明目的。
66.以上所述仅为用以方便说明本发明的实施例，本发明的范围不限于该等实施例，凡依本发明所做的任何变更，于不脱离本发明的精神下，皆属本发明申请专利的范围。