5.7 应用案例
在“作用力控制”中，可以用“状态变量”设置各组“控制模式/控制特性”，也可以用参数设置各组“控制模式/控制特性”。如表5-12所示。由“控制模式/控制特性”的组号决定是使用参数还是变量进行设置。
表5-12 使用参数或变量设置“控制模式/控制特性”
注：○—需要设定；△—根据需要设定；×—不需要设定。
5.7.1 应用案例1——作用力控制
（1）工作要求及过程
如图5-7所示，从PStart位置开始执行“作用力控制”。在Z方向上以10N力推入工件。
机器人运动到力觉工具坐标系+FZt方向上达到10N的反作用力的位置。
图5-7 作用力控制工程案例1
（2）样例程序
【设置控制模式组号=0的各技术参量（由于选择控制模式组号=0，所以使用“状态变量”设置各控制模式参量）】
P_FsStf0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——刚度系数（N/mm）（在本例中未设置）。
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——阻尼系数，先设置=0，根据运行状态进行调整。
P_FsMod0=（+0.00，+0.00，+1.00，+0.00，+0.00，+0.00）（0，0） '——作用力控制模式（设置Z轴=1——“作用力控制”）。
M_FsCod0=0 '——采用力觉工具坐标系。
'——【设置控制特性组号=0的各技术参量（由于选择控制特性组号=0，所以使用“状态变量”设置各控制模式参量）】
P_FsGn0=（+0.00，+0.00，+30.00，+0.00，+0.00，+0.00）（0，0） '——设置“增益”（根据运行状态进行调整）。
P_FsFLm0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力检测设定值（本例未设置）。
P_FsFCd0=（+0.00，+0.00，+10.00，+0.00，+0.00，+0.00）（0，0） '——作用力指令值（仅仅在FZt方向=10N，重要）。
P_FsSpd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置速度值（本例中未设置）。
P_FsSwF0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值（本例中未设置）。
'***<作用力控制（Z轴）程序>***
Servo On '——伺服ON。
Mov PStart '——移动到起始点PStart。
Dly 1'——暂停。
Fsc On，0，0，1 '——启动力觉控制ON（控制对象为“作用力”）。控制模式组号=0，控制特性组号=0，执行“清零”操作。在本例中，已经设置为“作用力控制模式”（在“作用力控制模式”下，Z轴开始下降，一直到Z轴受到的作用力=10N）。
Dly 10 '——暂停。
Fsc Off '——力觉控制OFF。
End '——结束。
（3）说明
①在程序上半部设置“控制模式”和“控制特性”（本例中使用状态变量设置，因为在Fsc On，0，0，1指令中，设置“控制模式组号=0”“控制特性组号=0”）。
②根据Fsc On指令，按照“控制模式组号=0”“控制特性组号=0”设定的条件开始力觉控制。为了正确使用传感器的零点数据，要执行“传感器数据清零”操作。
5.7.2 应用案例2——“速度优先模式”与“作用力模式”的切换
（1）工作要求及过程
工作要求及过程如图5-8所示。
①以5mm/s速度在Z向动作，接触工件后以10N推力推入。
②从位置PStart的位置启动作用力控制，在速度优先模式中Z轴以5mm/s的速度运动。
③检测到+FZt方向的力超过2.5N时，自动切换到“作用力优先模式”，以10N的力推入。
图5-8 应用案例2的工作要求及过程
（2）样例程序
'——【选择控制模式组号=0，使用变量设置下列技术参量】
P_FsStf0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——刚度系数（本例中未设置刚度系数）。
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——阻尼系数。根据实际工况进行调整。
P_FsMod0=（+0.00，+0.00，+1.00，+0.00，+0.00，+0.00）（0，0） '——设置Z轴为“作用力控制模式”。
M_FsCod0=0 '——采用力觉工具坐标系。
'——【选择控制特性组号=0，使用变量设置下列参量】
P_FsGn0=（+0.00，+0.00，+30.00，+0.00，+0.00，+0.00）（0，0） '——设置“Z轴增益=30”，根据实际工况进行调整。
P_FsFLm0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力检测设定值（本例中未设置）。
P_FsFCd0=（+0.00，+0.00，+10.00，+0.00，+0.00，+0.00）（0，0） '——设置“Z轴作用力指令=10N”。重要！
P_FsSpd0=（+0.00，+0.00，+5.00，+0.00，+0.00，+0.00）（0，0） '——设定Z轴速度=5mm/s。
P_FsSwF0=（+0.00，+0.00，+2.50，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值（+FZt方向：2.5N）。
'***<作用力控制（作用力—速度混合控制）程序>***
Servo On '——伺服ON。
Mov PStart '——移动到起始点PStart。
Dly 1 '——暂停。
Fsc On，0，0，1 '——启动力觉控制ON。控制模式组号=0，控制特性组号=0，执行“清零”操作。在本例中，已经设置为“作用力控制模式”。在“作用力控制模式”下，Z轴先进入“速度模式”，以5mm/s速度沿Z向下降，接触工件后继续下降，直到Z轴上作用力达到2.5N后，切换为“作用力控制模式”，继续沿Z向下降，直到Z轴受到的作用力=10N，停止并保持在该位置。
Dly 10 '——暂停。
Fsc Off '——力觉控制OFF。
End '——结束。
（3）说明
①执行Fsc On指令，按照“控制模式组号=0”“控制特性组号=0”设定的条件开始力觉控制。执行“传感器数据清零”操作。
②在Z轴上先以“速度优先模式”，按变量P_FsSpd0指定的速度动作。
③在Z轴上的作用力达到（FZt=2.5N）”时，切换为“作用力优先模式”，以P_FsFCd0指定的力推进。
5.7.3 刚度控制
“刚度控制”就是使机器人在力觉工具坐标系的±FXt、±FYt方向受到外力后如同装上弹簧一样工作。
（1）工作要求
要求在FXt、FYt轴执行刚度控制（其他轴上指定位置控制）。
在XY平面上受到外力时，沿外力的反方向做弹性动作。外力解除后回到原来的位置。如图5-9所示。
图5-9 刚度控制
（2）样例程序
'——【设置控制模式组号=0，使用变量设置下列参量】
P_FsStf0=（+0.10，+0.10，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置X轴、Y轴的刚度系数=0.1N/mm。重要！
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——根据现场状态设置阻尼系数。
P_FsMod0=（+2.00，+2.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置力觉控制模式（X，Y轴：刚度控制）。
M_FsCod0=0 '——采用力觉工具坐标系。
'——【设置控制特性组号=0，使用变量设置下列参量】
P_FsGn0=（+20.00，+20.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置X、Y轴增益=20μm/N，需要根据现场状态进行调整。
P_FsFLm0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力检测设定值（N）（本例中未设置）。
P_FsFCd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力指令（本例中未设置）。
P_FsSpd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设定速度控制模式的速度（本例中未设置）。
P_FsSwF0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设定模式切换判定值（本例中未设置）。
'***<刚度控制（X，Y轴）程序>***
Servo On '——伺服ON。
Mov PStart '——移动到起始点PStart。
Dly 1 '——暂停。
Fsc On，0，0，1 '——“力觉控制ON”。控制模式组号=0，控制特性组号=0，执行“清零”操作）。
Hlt '——暂停。
Fsc Off '——力觉控制OFF。
End '——结束。
（3）说明
①用状态变量P_FsStf0设定机器人的刚度。数值越小，刚度越小（弹簧常数小）。
②根据Fsc On指令，按照控制模式组号=0，控制特性组号=0，执行 “清零”操作的条件，开始力觉控制。
③通过Hlt指令暂停运行时，柔性控制机器人。重新运行时，若“力觉控制OFF”，则进入一般的控制模式。
5.7.4 控制特性变更1
（1）动作要求及内容
改变控制特性的工程案例1如图5-10所示。
①机器人在运动过程中能够改变作用力（控制特性）。
②在Z方向上以5.0N的力推压的同时，从P1向P2开始动作。
③在移动行程到达50%位置开始逐渐加大推力，最终以12.0N的力推压。
图5-10 改变控制特性的工程案例1
（2）样例程序
'——【设置控制模式组号=0，使用变量设置下列参量】
P_FsStf0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——刚度系数（本例未设置）。
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——阻尼系数，根据现场状态设置。
P_FsMod0=（+0.00，+0.00，+1.00，+0.00，+0.00，+0.00）（0，0） '——选择力觉控制模式（设置Z轴为“作用力控制”）。
M_FsCod0=0 '——采用力觉工具坐标系。
'——【设置控制特性组号=0，使用变量设置下列参量】
P_FsGn0=（+0.00，+0.00，+4.00，+0.00，+0.00，+0.00）（0，0） '——设置Z轴增益=4.0μm/N。
P_FsFLm0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力检测设定值（N）（本例未设置）。
P_FsFCd0=（+0.00，+0.00，+5.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力指令值（Z轴=5.0N）。
P_FsSpd0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置速度控制模式的速度（本例未设置）。
P_FsSwF0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值。
'——【设置控制模式组号=-1的技术参量】
P_FsGn1=（+0.00，+0.00，+4.00，+0.00，+0.00，+0.00）（0，0） '——设置Z轴增益=4.0μm/N。
P_FsFLm1=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用作用力检测设定值（N）（本例未设置）。
P_FsFCd1=（+0.00，+0.00，+12.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力指令值（Z轴=12N）。
P_FsSpd1=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置速度控制模式的速度（本例未设置）。
P_FsSwF1=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值（本例未设置）。
'***<作用力控制（变更控制特性）>***
Spd 10 '——设置运行速度。
Servo On '——伺服ON。
Mvs P1，-3 '——移动到P1点上方3mm位置。
Dly 1 '——暂停。
Fsc On，0，0，1 '——力觉控制ON。注意（控制模式组号=0，控制特性组=0，执行力觉传感器“清零”操作）。
Dly 3 '——暂停。
FsGChg 50，500，-1 '——切换“控制特性”指令。设置在从P1移动到P2点行程50%位置时，切换控制特性，控制特性组号变为-1（切换时间：500ms）。
Mvs P2 '——移动到P2点。
Fsc Off '——力觉控制“OFF”。
End '——结束。
（3）说明
①机器人运动到P1点上方3mm的位置后，机器人静止等待1s。
②通过Fsc On指令开始力觉控制（力控制）。
③使用FsGChg指令进行切换控制特性。本示例程序中为动作位置到达总行程50%时，切换控制特性。
5.7.5 控制特性变更2（Mo组合条件）
（1）工作要求
改变控制特性的工程案例2如图5-11所示，沿着导轨动作。
①在-X方向以5N的力推入的同时，在+Y方向上以10mm/s的速度开始动作。
②在位置A检测到与+Y方向的壁面接触时，切换为在+Y方向以5N的力推入同时在+X方向上以10mm/s的速度动作。
③在位置B检测到与+X方向的壁面接触后停止动作。
图5-11 改变控制特性的工程案例2
（2）程序样例
'——【选择控制模式组号=0，设置技术参量】
P_FsStf0=（+0.00，+0.10，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置Y轴刚度系数=0.1N/mm。
P_FsDmp0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置阻尼系数=0。
P_FsMod0=（+1.00，+1.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力控制模式（X，Y轴：作用力控制）。
M_FsCod0=1 '——采用“力觉直交坐标系”。
'——【设置“控制特性”组号=0的技术参量】
P_FsGn0=（+2.00，+2.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置X、Y轴增益=2.0（μm/N）。
P_FsFLm0=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力检测设定值（N）（未设置）。
P_FsFCd0=（-5.00，+5.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力指令值X轴=-5.0N，Y轴=5.0N。
P_FsSpd0=（+0.00，+10.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置速度。
P_FsSwF0=（+0.00，+3.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值。
'——【设置控制特性组号=-1的各技术参量】
P_FsGn1=（+2.00，+2.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置增益。
P_FsFLm1=（+0.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力检测设定值。
P_FsFCd1=（+5.00，+5.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置作用力指令值X轴=5.0（N），Y轴=5.0（N）。
P_FsSpd1=（+10.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设定速度控制模式的速度。
P_FsSwF1=（+3.00，+0.00，+0.00，+0.00，+0.00，+0.00）（0，0） '——设置模式切换判定值。
'***<作用力控制-改变作用力方向的程序>***
Def MoTrg 1，P_FsCurD.Y>4.5 '——定义#1Mo组合条件：力觉传感器数据Fy>4.5N。
Mvs PStart '——向初始位置点PStart移动。
Dly 1 '——暂停。等待直到完全静止。
Fsc On，0，0，1 '——力觉控制ON。
FsCTrg 1，100，-1，30，0，1 '——根据#1Mo组合条件，指定控制特性切换。切换时间为100ms。切换后的“控制特性组号=-1”“超时时间”设置为30s。超时不报警。
*LBL1：If P_FsCurD.X<4.5 Then GoTo *LBL1 '——如果Fx<4.5N，就一直等待。如果Fx>4.5N，就进入程序下一行。
Fsc Off '——力觉控制OFF。
End '——结束。
（3）说明
①FsCTrg指令根据Mo组合条件，切换“控制特性”。
②Mo组合条件根据Def MoTrg指令进行定义。本例中，当Fy>4.5N时，#1Mo组合条件条件成立。
③到达PStart后，在Fsc On指令下执行力觉控制。这时的作用力方向/动作方向如表5-13所示。
表5-13 作用力方向/动作方向
④根据FsCtrg指令，如果#1Mo条件成立时（到达位置A时），控制特性组号从“0”改变未“-1”。此时的动作方向如表5-14所示（FsCtrg指令中有超时监视功能，如果在指定时间内条件不成立，会发生报警）。
表5-14 动作方向
⑤使用If指令，等待机器人到达位置B（Fx≥4.5N）时，停止力觉控制。