东南大学远程教育
机器人技术
第 十三 讲
主讲教师：王兴松
机械手的控制
Control of Robotic Manipulator
3.1 机械人系统的构成
3.2 传递函数和方框图
3.3 PID控制
3.4 机械手的位置控制
3.5 机械手的力控制
3.6 其他控制方式简介
Robotics控制
3.1 机械人系统的组成
3.1.1 机械人系统示意
机器人的功能：
①动作和运动的控制
②末端操作器 /手爪的
轨迹和力的再现
③运动状态显示、
参数设定功能
Robotics控制
3.1 机械人系统的组成
3.1.2 机械人框图
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
0CeQ ?
10 eeRi ??
100 eeeRC ???
)()()( 100 sesesR C s e ??
1)(
)()(
1
0
?????
?
???
? ?
Ts
K
se
sesG
A
1,?? KRCT
??
?
?
??
)0(1
)0(0
1 t
te )1( /0 TteKe ???
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
fkxxdxm ??? ???
)()()()(2 sfskxsd s xsxms ???
22
2
2 2
1
)(
)()(
nn
n
B ss
K
kdsmssf
sxsG
???
?
?????????
?
???
? ?
kK
1?
mk
d
m
k
n 2,?? ??
??
?
?
??
)0(1
)0(0
t
tf
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
??
?
?
?
?
?
?
?
?
?
?
?
?
????
??
?
?
?
??
?
?
???
?
??
?
?
?
??
?
?
? ?
??
?
?
?
??
?
?
?
?
?
)1()1(
)1(
12
1
)1()}1(1{
)1()
1
t an1s i n (
1
1
12
12
2
2
12
2
2
2
???
??
?
??
?
?
?
??
?
??
??
??
?
??
t
t
t
n
t
n
t
n
n
n
n
n
e
e
e
K
teK
t
e
K
x
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
mmm qDqM ??? ??
iK tm ??
mb vvRidt
diL ???
mbb qKv ??
L=0
)(1 mbmm qKvRK ????
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
mmbm
t
vqKKRDqKRM ???
?
?
???
? ?? ?
1
)1()()(
)()(
1
2
1
?????? Tss
K
sKKRDRM
K
sv
sqsG
bsm
m
bKKRD
KK
1
1
?? bKKRD
RMT
1?
?
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
12 GGu
x
x
y
u
y ???
21
21
GG
u
uGuG
u
zx
u
y
??
?
?
?
?
GH
G
r
y
GrGHyy
HyrGGey
?
?
??
??
1
)( ?
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
Robotics控制
3.3 PID控制
3.3.1 PID控制的基本形式
? ??? 101 )()( eKdeKeKtu Dp ???
??
?
??
? ??? ? 1
0
)(1)( eKdeTeKtu Dp ???
???
?
???
? ??? sT
sTKsC Dp 1
11)(
Robotics控制
3.3 PID控制
3.3.1 PID控制的基本形式
Robotics控制
3.3 PID控制
3.3.1 PID控制的基本形式
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 1）微分超前型 PD控制
xKeKtu DP ???)(
)1(
)(
?
?
Tss
KsG
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 1）微分超前型 PD控制
xKeKtu DP ???)( )1()( ?? Tss KsG
)()1()( sVTss Ksq mm ??
)())()(()( ssqKsqsqKsV mDmmdpm ???
? ?)())()(()1()( ssqKsqsqKTss Ksq mDmmdPm ????
TKKsTKKs
TKK
sq
sqsG
PD
P
md
m
C /}/)1{(
/
)(
)()(
2 ???????
?
???
? ?
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 1）微分超前型 PD控制
TKKsTKKs
TKK
sq
sqsG
PD
P
md
m
C /}/)1{(
/
)(
)()(
2 ???????
?
???
? ?
T
KK P
n ?
2?
T
KK D
n
?? 12 ??
K
TK n
P
2?
? KTK nD 12 ?? ??
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 2） I-PD控制
xKxKe d tKtu Dp ???? ?1)(
Robotics控制
3.4 机械手的位置控制
3.4.1 手爪位置控制
（ 1）使用逆运动学和关节角控制的方法
qDdP ?KqqKτ ??? )(
)(1 dd f rq ??
Robotics控制
3.4 机械手的位置控制
3.4.1 手爪位置控制
（ 2）注重静力学关系的方法
FJτ T?
rKrrKF d ?Dp ??? )(
rr dtd??
)(qfr ?
Robotics控制
3.4 机械手的位置控制
3.4.2 动态控制
f?? ??? ),()( qqhqqM ??
fτqqhuqMτ ?),(?)(? ??? ?
)()( qqKqqKqu ????? dPdDd ????
Robotics控制
3.4 机械手的位置控制
3.4.2 动态控制
uq ???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
nn u
u
q
q
?
??
?
?? 11
qqeeKeKe ????? dPD,0???
),,(),2,,2( 21211 ?????? ?? d i a gd i a g PnD ?? KK
? ?Tnee,,1 ??e
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
ENqdqm ??? ?????
eedeqd Nqkqdqm ??? ???
de qqq ??
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
（ 1）外力矩可计测的场合
)(1 ededE
d
qkqdNmq ??? ???
E
d
ed
d
dd
dd
EdedededE
d
EededE
d
N
m
m
qd
m
m
qqk
m
m
q
m
m
d
NqdqqkqdqdN
m
m
NqdqkqdN
m
m
)1()(
|)(|
)(
1
?????
?
?
?
?
?
?
?
?
??
???????
?????
??
???
???
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
（ 2）关节角加速度可以检出的场合
? ?
dddddd
ddddd
qdqqkqddqmm
qqkqqdqmqdqm
???
???????
??????
???????
)()()(
)()(?
mmd ?
0?dq?
)()( qqkqdd ddd ???? ??
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
手爪位置,)(qr f?
手爪速度,雅克比矩阵）()(,
Tq
qfJqJr
?
??? ??
手爪加速度,qJqJr ?????? ??
静力学关系式：
ET FJτ F ?
控制对象：
ET FJhqM ??? ???
期望的动作：
Eededd FrKrDrM ??? ???
（ 3.65）
de rrr ??
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 1）外力可以计测的情况
|)(|
)(
11
1
qJrKrDFMJ
qJrJq
???
???
????
??
??
?
ededEd
hqJMJ
rKrDMMJFJMMJ
FJhqJrKrDFMMJ
FJhqMτ
??
????
??????
???
?
????
??
??
?
???
??
1
1111
11
)()(
|)(|
ededdE
T
d
E
T
ededEd
E
T
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 2）关节角加速度可以检出的情况
ededdE rKrDqJqJMF ???? ????? )(
? ?
hqJMJrKrDJqJMJM
rKrDqJqJMJhqMτ
??????
??????
?????
???????
d
T
eded
T
d
T
ededd
T
)()(
)(
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 3）既不需要检出外力也不需要检出关节角加速度
左乘
ET FJhqJrJM ????? ?)}({ 1 ????
ETrr FτJhrM ??? ???
111 )( ???? ?? TTr JJMMJJM
qJMhJh ?rTr ?? ?
)( 1???? MJJMM Trd
1)( ?? ? TT JJ
ETTTT FJhJqJMJJrMJJ ???? ?????? ????? 11
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
)( 1???? MJJMM Trd
MJJJMJJMJ ?? ? )( 1 TTdT
11 ??? ?? MJMJJJMJ TTdT
? ? hqJMJrrKrrDJ
hqJMJrKrDJτ
??????
?????
?
?
????
???
1
1
)()(
)(
dddd
T
eded
T
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 4）若手臂慢慢动作
0,00 ???? hqJrr ???? d
)( qqJrr ??? dd
)( qqKJτ ?? ddT
?
?
?
?
?
?
?????
??????
)()(
)()(
2112221122111
2112221122111
?????
?????
?????
??????
SLSLSL
CLCLCLJ
Robotics控制
3.5 机械手的力控制
3.5.3 机械手的混合控制
手爪偏差提取,)(,)}({ q
PdTP frerreΔr ???
力偏差提取：
FdTF eFFeΔF )( ??
位置控制规律：
qKΔqKτ P ?PDPP ??
力控制规律：
dtFI τΔKτ F ??
ΔFJΔτ T?
Robotics控制
3.5 机械手的力控制
3.5.3 机械手的混合控制
FP τττ ??
东南大学远程教育
机器人原理及应用
第 三十一 讲
主讲教师：王兴松
Robotics 控制
3.6 机械手的其他控制方式
3.6.1 机械手的模糊控制
如果机器人的关节位置误差为：
则其 PID控制为
qqe d ??
})(
1
{
)()()()(
0
0
eTde
T
ek
tekdektektu
D
t
I
P
D
t
IP
?
?
???
???
?
?
??
??
东南大学远程教育
机器人原理及应用
第 二十二 讲
主讲教师：王兴松
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
一、模糊集合与集合计算符
定义 1 模糊集合,设 U为若干事件的总和，如 U=Rn，我们称 U为
论域，一个定义在 U上的模糊集合 F，由隶属度函数
来表征，这里的 表示 在模糊集合 F上的隶属程度。
经典的集合（确定集合）的隶属度函数只取两个值 {0,1}。
要么属于，要么不属于。因此模糊集合是经典集合的推广。
? ?1,0,?UF?
)(uF? Uu?
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
定义 2 交集、并集和补集,设 A和 B是 U上的两个模糊集合。对所
有的, A和 B的 交集 是定义在 U上的一个模糊集合，其隶属
度函数定义为：
对所有的, A和 B的 并集 是定义在 U上的一个模糊集合，其
隶属度函数定义为：
对所有的, A的补集是定义在 U上的一个模糊集合，其隶
属度函数为：
Uu?
)}(),(m i n {)( uuu BABA ??? ??
Uu?
)}(),(m a x {)( uuu BABA ??? ??
Uu?
)(1)( uu AA ?? ??
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
定义 3 模糊关系 设 U和 V是两个论域。模糊关系 R是积空间 UxV上
的一个模糊集合，即当 时,R的隶属函数为,
定义 4 模糊蕴涵 设 A和 B分别为定义在 U和 V上的模糊集合,则由
所表示的模糊蕴涵是定义在 UxV上的一个特殊模糊关系,其
隶属度函数定义为,
模糊与,
模糊或,
实质蕴涵,
命题演算,
VvUu ??,),( vuR?
BA?
)()(),(
)()(),(
)()(),(
)()(),(
vuvu
vuvu
vuvu
vuvu
BAABA
BABA
BABA
BABA
??
?
?
?
??
??
??
??
???
???
???
???
?
?
?
东南大学远程教育
机器人原理及应用
第 二十三 讲
主讲教师：王兴松
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
定义 5 广义取式推理, 前提 1 x为 A’
前提 2 如果 x为 A,则 y为 B
结论 y为 B’
其中 A‘,A,B’,B为模糊集合,x,y为语言变量
定义 6 广义取式推理, 前提 1 y为 B’
前提 2 如果 x为 A,则 y为 B
结论 x为 A’
其中 A‘,A,B’,B为模糊集合,x,y为语言变量
)}()(]1,0[i n f {),( vcucvu ABBA ??? ????? ?
)}()(]1,0[s u p {),( vcucvu BABA ??? ?????
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.3 模糊规则与模糊推理
模糊规则 是由如下形式的, 如果 ---则, 规则的总和组成
R（ l）,如果 x1为 F1l,且 …,且 xn为 Fnl，则 y为 Gl；
Fil, Gl为模糊集合,xi为模糊变量。
将变量 模糊化 后，经过按照模糊规则的运算，获得模糊结果，
这个过程称为一个 模糊推理 。模糊推理得到的模糊输出，再经
过反 模糊化,即可得到模糊推理的精确解。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
一个典型的模糊控制系统
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
例：一热处理用的电热炉，按工艺要求须保持炉温 600O不变。由
于炉温受零件数量、体积、环境温度变化、电网电压波动等影
响，会出现波动所以要设计控制器。
控制方式,通过改变可控硅的导通角实现；也可以通过 PWM方式
调节。人工调节时，通过面板上的电位器实现，计
算机调节通过驱动线路实现。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
典型的温度控制电路如图
INT1,INT2过零检测,PB7触发控制,AN0传感器输入 A/D。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
人工操作时，根据经验，控制规则可以用语言描述如下：
如果炉稳低于 600OC则升压，低得越多升压越高；
如果炉稳高于 600OC则降压，高得越多降压越低；
如果炉稳等于 600OC则保持电压不变；
采用 模糊控制 时，其工作原理如下：
1、模糊控制器的输入变量与输出变量
设定炉温 t0=600，测量炉稳 t（ K），则将误差 e(K)=t0-t(K)
作为模糊控制的 输入变量 。
输出变量 为：控制电压 u，可通过改变可控硅的导通角或 PWM
比例实现。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
2、输入变量及输出变量的模糊语言描述（模糊化）
设描述输入及输出变量的语言值的模糊子集为：
{负大，负小,0，正小，正大 } 或记为 {NB,NS,0,PS,PB}
设误差 e的论域为 X，并将误差大小分为七个等级，为,-3,-2，
-1,0,1,2,3，则有,X={-3,-2,-1,0,1,2,3}
选控制变量 u的论域为 Y，并将其也分为七个等级，为,-3,-2，
-1,0,1,2,3，则有,Y={-3,-2,-1,0,1,2,3}
定义其隶属度函数如图：
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
由此得模糊变量赋值表：
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
3、模糊控制规则的语言描述
根据手动控制策略，规则为：
（ 1）若 e负大，则 u正大；
（ 2）若 e负小，则 u正小；
（ 3）若 e为零，则 u为零；
（ 4）若 e正小，则 u负小；
（ 5）若 e正大，则 u负大；
由此可得控制规则表：
e NB NS 0 PS PB
u PB PS 0 NS NB
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
4、模糊控制规则的矩阵形式
模糊控制规则实际上是从误差论域 X到控制量论域 Y的 模糊关
系 R，可记为：
从模糊赋制值表可得：
)()(
)()()(
ueue
ueueue
NBPBNSPS
OOPSNSPBNBR
????
??????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
0000000
0000000
0000000
0000000
0000000
5.05.000000
15.000000
)1,5.0,0,0,0,0,0()0,0,0,0,0,5.0,1(
ue
PBNB
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
通理，可以得到
将这些代入求和式，有
ueueueue NBPBNSPSOOPSNS ????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
000005.01
00005.05.05.0
005.05.015.00
005.015.000
05.015.05.000
5.05.05.00000
15.000000
R
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
5、模糊决策
当模糊输入为 e（ K）时，其对应的输出为
u（ K） =e（ K）。 R
例如，当 e（ K） =PS时，
6、模糊输出变为精确值
（ 1）隶属度最大原则，则 u=-1；
（ 2）平均法：
? ?005.05.015.05.0
)()(
?
? RKeKu ?
7
3
]001*5.00*5.0)1(*1)2(*5.0)3(*5.0[
7
1
??
??????????u
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
7、模糊控制表
上述的结果即可用于实际控制，但是，为了提高控制速度，
一般将各种情况预先计算出，存入表中，形成模糊控制表。
输入量和输出量的值，可以
自行定义，如定义 -3=550OC
e -3 -2 -1 0 1 2 3
u 3 2 1 0 -1 -2 -3
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.5 机器人关节位置的模糊控制
我们可以将上述温度变量换为机器人的关节角度，将误差
定义为期望转角与实测转角的差，输出定义为关节控制电压，
即可实现机器人关节角位置的模糊控制。
机器人技术
第 十三 讲
主讲教师：王兴松
机械手的控制
Control of Robotic Manipulator
3.1 机械人系统的构成
3.2 传递函数和方框图
3.3 PID控制
3.4 机械手的位置控制
3.5 机械手的力控制
3.6 其他控制方式简介
Robotics控制
3.1 机械人系统的组成
3.1.1 机械人系统示意
机器人的功能：
①动作和运动的控制
②末端操作器 /手爪的
轨迹和力的再现
③运动状态显示、
参数设定功能
Robotics控制
3.1 机械人系统的组成
3.1.2 机械人框图
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
0CeQ ?
10 eeRi ??
100 eeeRC ???
)()()( 100 sesesR C s e ??
1)(
)()(
1
0
?????
?
???
? ?
Ts
K
se
sesG
A
1,?? KRCT
??
?
?
??
)0(1
)0(0
1 t
te )1( /0 TteKe ???
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
fkxxdxm ??? ???
)()()()(2 sfskxsd s xsxms ???
22
2
2 2
1
)(
)()(
nn
n
B ss
K
kdsmssf
sxsG
???
?
?????????
?
???
? ?
kK
1?
mk
d
m
k
n 2,?? ??
??
?
?
??
)0(1
)0(0
t
tf
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
??
?
?
?
?
?
?
?
?
?
?
?
?
????
??
?
?
?
??
?
?
???
?
??
?
?
?
??
?
?
? ?
??
?
?
?
??
?
?
?
?
?
)1()1(
)1(
12
1
)1()}1(1{
)1()
1
t an1s i n (
1
1
12
12
2
2
12
2
2
2
???
??
?
??
?
?
?
??
?
??
??
??
?
??
t
t
t
n
t
n
t
n
n
n
n
n
e
e
e
K
teK
t
e
K
x
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
mmm qDqM ??? ??
iK tm ??
mb vvRidt
diL ???
mbb qKv ??
L=0
)(1 mbmm qKvRK ????
Robotics控制
3.2 传递函数和方框图
3.2.1 传递函数
mmbm
t
vqKKRDqKRM ???
?
?
???
? ?? ?
1
)1()()(
)()(
1
2
1
?????? Tss
K
sKKRDRM
K
sv
sqsG
bsm
m
bKKRD
KK
1
1
?? bKKRD
RMT
1?
?
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
12 GGu
x
x
y
u
y ???
21
21
GG
u
uGuG
u
zx
u
y
??
?
?
?
?
GH
G
r
y
GrGHyy
HyrGGey
?
?
??
??
1
)( ?
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
Robotics控制
3.2 传递函数和方框图
3.2.2 方框图
Robotics控制
3.3 PID控制
3.3.1 PID控制的基本形式
? ??? 101 )()( eKdeKeKtu Dp ???
??
?
??
? ??? ? 1
0
)(1)( eKdeTeKtu Dp ???
???
?
???
? ??? sT
sTKsC Dp 1
11)(
Robotics控制
3.3 PID控制
3.3.1 PID控制的基本形式
Robotics控制
3.3 PID控制
3.3.1 PID控制的基本形式
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 1）微分超前型 PD控制
xKeKtu DP ???)(
)1(
)(
?
?
Tss
KsG
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 1）微分超前型 PD控制
xKeKtu DP ???)( )1()( ?? Tss KsG
)()1()( sVTss Ksq mm ??
)())()(()( ssqKsqsqKsV mDmmdpm ???
? ?)())()(()1()( ssqKsqsqKTss Ksq mDmmdPm ????
TKKsTKKs
TKK
sq
sqsG
PD
P
md
m
C /}/)1{(
/
)(
)()(
2 ???????
?
???
? ?
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 1）微分超前型 PD控制
TKKsTKKs
TKK
sq
sqsG
PD
P
md
m
C /}/)1{(
/
)(
)()(
2 ???????
?
???
? ?
T
KK P
n ?
2?
T
KK D
n
?? 12 ??
K
TK n
P
2?
? KTK nD 12 ?? ??
Robotics控制
3.3 PID控制
3.3.2 实用的 PID控制
（ 2） I-PD控制
xKxKe d tKtu Dp ???? ?1)(
Robotics控制
3.4 机械手的位置控制
3.4.1 手爪位置控制
（ 1）使用逆运动学和关节角控制的方法
qDdP ?KqqKτ ??? )(
)(1 dd f rq ??
Robotics控制
3.4 机械手的位置控制
3.4.1 手爪位置控制
（ 2）注重静力学关系的方法
FJτ T?
rKrrKF d ?Dp ??? )(
rr dtd??
)(qfr ?
Robotics控制
3.4 机械手的位置控制
3.4.2 动态控制
f?? ??? ),()( qqhqqM ??
fτqqhuqMτ ?),(?)(? ??? ?
)()( qqKqqKqu ????? dPdDd ????
Robotics控制
3.4 机械手的位置控制
3.4.2 动态控制
uq ???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
nn u
u
q
q
?
??
?
?? 11
qqeeKeKe ????? dPD,0???
),,(),2,,2( 21211 ?????? ?? d i a gd i a g PnD ?? KK
? ?Tnee,,1 ??e
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
ENqdqm ??? ?????
eedeqd Nqkqdqm ??? ???
de qqq ??
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
（ 1）外力矩可计测的场合
)(1 ededE
d
qkqdNmq ??? ???
E
d
ed
d
dd
dd
EdedededE
d
EededE
d
N
m
m
qd
m
m
qqk
m
m
q
m
m
d
NqdqqkqdqdN
m
m
NqdqkqdN
m
m
)1()(
|)(|
)(
1
?????
?
?
?
?
?
?
?
?
??
???????
?????
??
???
???
Robotics控制
3.5 机械手的力控制
3.5.1 单自由度机械系统的阻抗控制
（ 2）关节角加速度可以检出的场合
? ?
dddddd
ddddd
qdqqkqddqmm
qqkqqdqmqdqm
???
???????
??????
???????
)()()(
)()(?
mmd ?
0?dq?
)()( qqkqdd ddd ???? ??
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
手爪位置,)(qr f?
手爪速度,雅克比矩阵）()(,
Tq
qfJqJr
?
??? ??
手爪加速度,qJqJr ?????? ??
静力学关系式：
ET FJτ F ?
控制对象：
ET FJhqM ??? ???
期望的动作：
Eededd FrKrDrM ??? ???
（ 3.65）
de rrr ??
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 1）外力可以计测的情况
|)(|
)(
11
1
qJrKrDFMJ
qJrJq
???
???
????
??
??
?
ededEd
hqJMJ
rKrDMMJFJMMJ
FJhqJrKrDFMMJ
FJhqMτ
??
????
??????
???
?
????
??
??
?
???
??
1
1111
11
)()(
|)(|
ededdE
T
d
E
T
ededEd
E
T
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 2）关节角加速度可以检出的情况
ededdE rKrDqJqJMF ???? ????? )(
? ?
hqJMJrKrDJqJMJM
rKrDqJqJMJhqMτ
??????
??????
?????
???????
d
T
eded
T
d
T
ededd
T
)()(
)(
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 3）既不需要检出外力也不需要检出关节角加速度
左乘
ET FJhqJrJM ????? ?)}({ 1 ????
ETrr FτJhrM ??? ???
111 )( ???? ?? TTr JJMMJJM
qJMhJh ?rTr ?? ?
)( 1???? MJJMM Trd
1)( ?? ? TT JJ
ETTTT FJhJqJMJJrMJJ ???? ?????? ????? 11
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
)( 1???? MJJMM Trd
MJJJMJJMJ ?? ? )( 1 TTdT
11 ??? ?? MJMJJJMJ TTdT
? ? hqJMJrrKrrDJ
hqJMJrKrDJτ
??????
?????
?
?
????
???
1
1
)()(
)(
dddd
T
eded
T
Robotics控制
3.5 机械手的力控制
3.5.2 机械手的阻抗控制
（ 4）若手臂慢慢动作
0,00 ???? hqJrr ???? d
)( qqJrr ??? dd
)( qqKJτ ?? ddT
?
?
?
?
?
?
?????
??????
)()(
)()(
2112221122111
2112221122111
?????
?????
?????
??????
SLSLSL
CLCLCLJ
Robotics控制
3.5 机械手的力控制
3.5.3 机械手的混合控制
手爪偏差提取,)(,)}({ q
PdTP frerreΔr ???
力偏差提取：
FdTF eFFeΔF )( ??
位置控制规律：
qKΔqKτ P ?PDPP ??
力控制规律：
dtFI τΔKτ F ??
ΔFJΔτ T?
Robotics控制
3.5 机械手的力控制
3.5.3 机械手的混合控制
FP τττ ??
东南大学远程教育
机器人原理及应用
第 三十一 讲
主讲教师：王兴松
Robotics 控制
3.6 机械手的其他控制方式
3.6.1 机械手的模糊控制
如果机器人的关节位置误差为：
则其 PID控制为
qqe d ??
})(
1
{
)()()()(
0
0
eTde
T
ek
tekdektektu
D
t
I
P
D
t
IP
?
?
???
???
?
?
??
??
东南大学远程教育
机器人原理及应用
第 二十二 讲
主讲教师：王兴松
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
一、模糊集合与集合计算符
定义 1 模糊集合,设 U为若干事件的总和，如 U=Rn，我们称 U为
论域，一个定义在 U上的模糊集合 F，由隶属度函数
来表征，这里的 表示 在模糊集合 F上的隶属程度。
经典的集合（确定集合）的隶属度函数只取两个值 {0,1}。
要么属于，要么不属于。因此模糊集合是经典集合的推广。
? ?1,0,?UF?
)(uF? Uu?
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
定义 2 交集、并集和补集,设 A和 B是 U上的两个模糊集合。对所
有的, A和 B的 交集 是定义在 U上的一个模糊集合，其隶属
度函数定义为：
对所有的, A和 B的 并集 是定义在 U上的一个模糊集合，其
隶属度函数定义为：
对所有的, A的补集是定义在 U上的一个模糊集合，其隶
属度函数为：
Uu?
)}(),(m i n {)( uuu BABA ??? ??
Uu?
)}(),(m a x {)( uuu BABA ??? ??
Uu?
)(1)( uu AA ?? ??
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
定义 3 模糊关系 设 U和 V是两个论域。模糊关系 R是积空间 UxV上
的一个模糊集合，即当 时,R的隶属函数为,
定义 4 模糊蕴涵 设 A和 B分别为定义在 U和 V上的模糊集合,则由
所表示的模糊蕴涵是定义在 UxV上的一个特殊模糊关系,其
隶属度函数定义为,
模糊与,
模糊或,
实质蕴涵,
命题演算,
VvUu ??,),( vuR?
BA?
)()(),(
)()(),(
)()(),(
)()(),(
vuvu
vuvu
vuvu
vuvu
BAABA
BABA
BABA
BABA
??
?
?
?
??
??
??
??
???
???
???
???
?
?
?
东南大学远程教育
机器人原理及应用
第 二十三 讲
主讲教师：王兴松
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.2 模糊集合与模糊逻辑的基本概念
定义 5 广义取式推理, 前提 1 x为 A’
前提 2 如果 x为 A,则 y为 B
结论 y为 B’
其中 A‘,A,B’,B为模糊集合,x,y为语言变量
定义 6 广义取式推理, 前提 1 y为 B’
前提 2 如果 x为 A,则 y为 B
结论 x为 A’
其中 A‘,A,B’,B为模糊集合,x,y为语言变量
)}()(]1,0[i n f {),( vcucvu ABBA ??? ????? ?
)}()(]1,0[s u p {),( vcucvu BABA ??? ?????
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.3 模糊规则与模糊推理
模糊规则 是由如下形式的, 如果 ---则, 规则的总和组成
R（ l）,如果 x1为 F1l,且 …,且 xn为 Fnl，则 y为 Gl；
Fil, Gl为模糊集合,xi为模糊变量。
将变量 模糊化 后，经过按照模糊规则的运算，获得模糊结果，
这个过程称为一个 模糊推理 。模糊推理得到的模糊输出，再经
过反 模糊化,即可得到模糊推理的精确解。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
一个典型的模糊控制系统
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
例：一热处理用的电热炉，按工艺要求须保持炉温 600O不变。由
于炉温受零件数量、体积、环境温度变化、电网电压波动等影
响，会出现波动所以要设计控制器。
控制方式,通过改变可控硅的导通角实现；也可以通过 PWM方式
调节。人工调节时，通过面板上的电位器实现，计
算机调节通过驱动线路实现。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
典型的温度控制电路如图
INT1,INT2过零检测,PB7触发控制,AN0传感器输入 A/D。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
人工操作时，根据经验，控制规则可以用语言描述如下：
如果炉稳低于 600OC则升压，低得越多升压越高；
如果炉稳高于 600OC则降压，高得越多降压越低；
如果炉稳等于 600OC则保持电压不变；
采用 模糊控制 时，其工作原理如下：
1、模糊控制器的输入变量与输出变量
设定炉温 t0=600，测量炉稳 t（ K），则将误差 e(K)=t0-t(K)
作为模糊控制的 输入变量 。
输出变量 为：控制电压 u，可通过改变可控硅的导通角或 PWM
比例实现。
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
2、输入变量及输出变量的模糊语言描述（模糊化）
设描述输入及输出变量的语言值的模糊子集为：
{负大，负小,0，正小，正大 } 或记为 {NB,NS,0,PS,PB}
设误差 e的论域为 X，并将误差大小分为七个等级，为,-3,-2，
-1,0,1,2,3，则有,X={-3,-2,-1,0,1,2,3}
选控制变量 u的论域为 Y，并将其也分为七个等级，为,-3,-2，
-1,0,1,2,3，则有,Y={-3,-2,-1,0,1,2,3}
定义其隶属度函数如图：
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
由此得模糊变量赋值表：
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
3、模糊控制规则的语言描述
根据手动控制策略，规则为：
（ 1）若 e负大，则 u正大；
（ 2）若 e负小，则 u正小；
（ 3）若 e为零，则 u为零；
（ 4）若 e正小，则 u负小；
（ 5）若 e正大，则 u负大；
由此可得控制规则表：
e NB NS 0 PS PB
u PB PS 0 NS NB
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
4、模糊控制规则的矩阵形式
模糊控制规则实际上是从误差论域 X到控制量论域 Y的 模糊关
系 R，可记为：
从模糊赋制值表可得：
)()(
)()()(
ueue
ueueue
NBPBNSPS
OOPSNSPBNBR
????
??????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
0000000
0000000
0000000
0000000
0000000
5.05.000000
15.000000
)1,5.0,0,0,0,0,0()0,0,0,0,0,5.0,1(
ue
PBNB
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
通理，可以得到
将这些代入求和式，有
ueueueue NBPBNSPSOOPSNS ????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
000005.01
00005.05.05.0
005.05.015.00
005.015.000
05.015.05.000
5.05.05.00000
15.000000
R
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
5、模糊决策
当模糊输入为 e（ K）时，其对应的输出为
u（ K） =e（ K）。 R
例如，当 e（ K） =PS时，
6、模糊输出变为精确值
（ 1）隶属度最大原则，则 u=-1；
（ 2）平均法：
? ?005.05.015.05.0
)()(
?
? RKeKu ?
7
3
]001*5.00*5.0)1(*1)2(*5.0)3(*5.0[
7
1
??
??????????u
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.4 模糊控制工作原理
7、模糊控制表
上述的结果即可用于实际控制，但是，为了提高控制速度，
一般将各种情况预先计算出，存入表中，形成模糊控制表。
输入量和输出量的值，可以
自行定义，如定义 -3=550OC
e -3 -2 -1 0 1 2 3
u 3 2 1 0 -1 -2 -3
Robotics 控制
3.6 机器人关节控制的模糊算法
3.6.5 机器人关节位置的模糊控制
我们可以将上述温度变量换为机器人的关节角度，将误差
定义为期望转角与实测转角的差，输出定义为关节控制电压，
即可实现机器人关节角位置的模糊控制。
