机器人控制
Control of Robotics
6.1 机器人的基本控制原则
6.2 机械人的位置控制
6.3 机械人的柔顺控制
6.4 机器人的分解运动控制
6.5 机器人关节控制的模糊算法
Robotics 控制
6.1 机器人的基本控制原则
机器人控制特点：冗余的、多变量、本质非线性、耦合的
6.1.1 基本控制原则
1、控制器分类
结构形式,伺服、非伺服、位置反馈、速度反馈、力矩控制、
控制方式,非线性控制、分解加速度控制、最优控制、自适
应控制、滑模控制、模糊控制等
控制器选择,依工作任务，可选 PLC控制、普通计算机控制，
智能计算机控制等。
简单分类,单关节控制器：主要考虑稳态误差补偿；
多关节控制器：主要考虑耦合惯量补偿。
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
一般分类, PLC、单片机、小型计算机、多计算机分布控制
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
2、主要控制变量
任务轴 R0：描述工件位置的坐标系
X(t):末端执行器状态；
θ (t):关节变量；
C(t)：关节力矩矢量；
T(t),电机力矩矢量；
V(t)：电机电压矢量
本质是对下列双向方程的控制,
)()()()()( ttttt XCTV ???? ?
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
3、主要控制层次
分三个层次：人工智能级、控制模式级、伺服系统级
1）人工智能级
完成从机器人工作任务的语言描述 生成 X(t)；
仍处于研究阶段。
2）控制模式级
建立 X(t) T(t)之间的双向关系。
电机模型 传动模型 关节动力学模型 机器人模型
)()()()( tttt TCX ??? ?
)()()()( tttt XCT ?
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
3、伺服系统级
解决关节伺服控制问题
即 TV?
Robotics 控制
6.1 机器人的基本控制原则
6.1.2 伺服控制系统举例
1、液压缸伺服传动系统
优点：减少减速器等，消除了间隙和磨损误差，结构简单、
精度与电器传动相当。同样可以进行位置、速度、加速度及力
的反馈。
Robotics 控制
6.1 机器人的基本控制原则
6.1.2 伺服控制系统举例
2、典型的滑阀控制液压传动系统
Robotics 控制
6.2 机器人的位置控制
由于机器人系统具有高度非线性，且机械结构很复杂，因
此在研究其动态模型时，做如下假设：
（ 1）机器人各连杆是理想刚体，所有关节都是理想的，不存在
摩擦和间隙；
（ 2）相邻两连杆间只有一个自由度，或为旋转、或为平移。
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
伺服电机的参数：
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
（ 1）磁场型控制电机
222
2
2
,,
???
?
??
cc
m
c
m
m
mm
m
fmm
f
ffff
k
K
f
fF
J
JJ
K
dt
d
F
dt
d
JT
ikT
dt
di
lirv
?????
???
?
??
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
Laplace变换得：
)()()(
)()(
)()()(
2
sKFsJssT
sIksT
sIslrsV
mm
fmm
ffff
????
?
??
))(()(
)(
2 KFsJsslr
k
sV
s
ff
m
f
m
???
??
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
一般可取 K=0，则有等效框图
同时，传递函数变为
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
：电气时间常数；
：机械时间常数。
)1)(1(
)1)(1(
1
))(()(
)(
0
sss
k
s
F
J
s
r
l
s
Fr
k
FJsslrs
k
sV
s
me
f
ff
m
ff
m
f
m
??
?
??
?
??
?
??
?
m
e
?
?
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
由于,有时可以忽略，于是
而对角速度的传递函数为：
，因为
me ?? ??
)1()(
)( 0
ss
k
sV
s
mf
m
?
?
?
?
s
k
sV
s
mf
m
??
??
1)(
)( 0
dt
d m
m
?? ?
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
（ 2）电枢控制型电机
Ke:产生反电势。
222
2
2
,,
'
???
?
??
?
cc
m
c
m
m
mm
m
mmm
me
m
mmmm
k
K
f
fF
J
JJ
K
dt
d
F
dt
d
JT
ikT
k
dt
di
LiRV
?????
???
?
???
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
经拉氏变换、并设 K=0，有
? ?memm
m
m
m
kkJsFsLRs
k
sV
s
????
??
))(()(
)(?
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
2、直流电机的转速调整
误差信号：
)()()( 0 ttte i ?? ??
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
2、直流电机的转速调整
比例补偿：控制输出与 e(t)成比例；
微分补偿：控制输出与 de(t)/dt成比例；
积分补偿：控制输出与 ∫ e(t)dt成比例；
测速补偿：与输出位置的微分成比例。
比例微分 PD补偿：
比例积分 PI补偿：
比例微分积分 PID补偿：
测速补偿时：
)()()( sEsksE d????
)()()( sEsksE i????
)()()( sEsksE id ?? ????
)()1()()()()( 00 ssssssEsE tit ????? ??????
Robotics 控制
6.2 机器人的位置控制
6.2.2 位置控制的基本结构
1、基本控制结构
位置控制也称 位姿控制,或 轨迹控制 。分为：
点到点 PTP控制；如点焊； 连续路径 CP控制；如喷漆
期望的关节位置 期望的工具位置和姿态
? ?Tdnddd qqqq ?21? ],[ TdTdd pw ??
Robotics 控制
6.2 机器人的位置控制
6.2.2 位置控制的基本结构
2,PUMA机器人的伺服控制结构
1）机器人控制系统设计与一般计算机控制系统相似。
2）多数仍采用连续系统的设计方法设计控制器，然后再将
设计好的控制律离散化，用计算机实现。
3）现有的工业机器人大多数采用独立关节的 PID控制。
下图 PUMA机器人的伺服控制系统构成
Robotics 控制
6.2 机器人的位置控制
6.2.2 位置控制的基本结构
2,PUMA机器人的伺服控制结构
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
1、位置控制系统结构
具有力、位移、速度反馈
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
1、位置控制系统结构
控制器路径点的获取方式：
（ 1）以数字形式输入系统；若以直角坐标给出，须计算获
得其关节坐标位置。
（ 2）以示教方式输入系统；系统将直接获得关节坐标位置
允许机器人只移动一个关节，而锁住其他关节。
轨迹控制：
按关键点或轨迹进行定位控制。
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
对图示系统，有
J,等效转动惯量；
B,等效阻尼系数。
lm
lma
mmm
BBB
JJJJ
BJT
2
2
?
?
??
??
???
?? ???
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
因此可得其传递函数（同电枢控制直流伺服电机）
? ?)()()(
)(
2
Iemmmm
I
m
m
KkBRsBLJRJsLs
K
sV
s
?????
?
)]()([)(
)()(
)()()(
ttKtV
tt
ttte
sdm
ms
sd
??
???
??
? ??
?
??
)]()([)(
)()(
)()()(
ssKsV
ss
sssE
sdm
ms
sd
??
???
??
? ??
?
??
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
其开环传递函数为：
因为：,略去 Lm的项，简化上式为：
? ?)()()(
)(
2
Iemmmm
Is
KkBRsBLJRJsLs
KK
sE
s
?????
???
,mm RL ???
? ?)()(
)(
Iemm
Is
KkBRJsRs
KK
sE
s
???
???
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
则其闭环传递函数为：
这是一个典型的二阶系统闭环传递函数。
)()()(
1
)()(1
)()(
)(
)(
2
JRKKJRskKBRsJR
KK
sEs
sEs
s
s
mmeImm
I
s
s
d
s
?
?
?
?
?
?
?
???
?
?
?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
含有速度反馈的机械手单关节控制器的开环传递函数为
闭环传递函数为
sKKKKkBRJsR
KK
sE
s
tIIemm
Is
)()(
)(
1
2 ????
???
ItIeImm
I
s
s
d
s
KKsKKKkKBRJsR
KK
sEs
sEs
s
s
?
?
?
?
?
?
?
?
????
?
?
?
)(
)()(1
)()(
)(
)(
1
2
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
由上述闭环传递函数，得控制系统的特征方程为：
将其写为二阶系统标准形式
得
1/ KK?
0)( 12 ????? ItIeImm KKsKKKkKBRJsR ??
02 22 ??? nn ss ???
JRKKKKkKBR
JRKK
mIteIm
mIn
?
?
??
??
2/)]([
0/
1???
??
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
1/ KK?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
1/ KK?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
设结构的共振频率为,则为避免运动中发生共振，要求
同时要求系统阻尼大于 1，
J值随负载和位姿变化，应选可能的最大惯量。
r?
I
mrr
mIn K
JRKJRKK
?
????
?? 4,2/
2
???
tetIrm
mIteIm
KkKKBJRK
JRKKKKkKBR
//)/(
12/)]([
1
1
???
????
?
?? ?
1/ KK?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 2）稳态误差
根据控制理论，在控制系统框图中，计算得到 E(s),即可
得到系统的稳态位置误差、速度误差和加速度误差。
对于单位阶越位移 C0，其稳态误差为
)(lim)(lim 0 ssEtee stss ??? ??
ILgfmssp KKCCCRe ?? /)( ???
Robotics 控制
6.2 机器人的位置控制
6.2.4 多关节位置控制器
1）为快速运动，一般应采用多关节协调、同步运动。
2）这时各关节的位置和速度会互相作用，因此，必须进行附
加补偿。
1、动态拉格朗日公式
其他关节加速 自身加速 科式力 重力
且 D项皆与关节角有关。
i
j k
kjijkiai
i
jiji DqqDqJqDT ???? ? ??
? ??
6
1
6
1
6
1
??????
Robotics 控制
6.2 机器人的位置控制
6.2.4 多关节位置控制器
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
柔性无刚性作用的运动控制。 如擦玻璃、抓鸡蛋、装配等
一般应增加力反馈。
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
1、被动柔顺和主动柔顺
被动柔顺,通过弹簧、消振器等机械结构或通过改变机械操
作方式而使机器人与工作对象间产生相对柔性运
动的柔顺方式。（ 无须控制器参与 ）
如,1）海绵擦玻璃,2）把工件拉进孔取代推入
主动柔顺,通过改变控制器控制方式，增加力反馈等使机器
人与工作对象间无刚性运动的柔顺方式。（ 必须
控制器参与 ）
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
1、被动柔顺和主动柔顺
定义：
：在工作点的小位移；
：工作点的关节小位移
：正定对角刚度矩阵
：机器人雅可比矩阵
：回复力
：关节力矩
定义关节刚度矩阵：, 反应力矩与微位移关系
?
?
?
F
J
K
q
x
p
qJKJ
qJx
FJ
xKF
p
T
T
p
??
??
?
?
)(??
?
?
??
JKJK pTq ?
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
2、作业约束和力控制
自然约束与人为约束
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
2、作业约束和力控制
约束对机器人力控制的影响：
1）约束使自由度减少，限制了末端的运动方式；
2）约束给机器人增加了作用力，增加了控制的复杂程度；
3）上述两种情况会相互作用
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
3、柔顺控制的种类
1）阻抗控制
通过控制力和位置间的动态关系（阻抗），来实现柔顺
功能。即：通过控制使机械手末端呈现所需要的刚性和阻尼。
需要位置控制的自由度，需要大的刚性；
需要力控制的自由度，需要小的刚性。
2）力和位置混合控制
将控制分为一些自由度的位置控制，和另一些自由度的力
控制，通过计算，在关节空间合并，进行关节控制。
Robotics 控制
6.3 机器人的柔顺控制
6.3.2 主动阻抗控制
1、位置型阻抗控制
：估计重力矩
：雅可比矩阵
：机械手等效刚度比例系数
：机械手等效阻尼系数
教材中的稳定性讨论，内容不全，这里省略。
]~~)[()(?
~
xKxKqJqg
xxx
Dp
T
d
????
??
?
D
p
K
K
qJ
qg
)(
)(?
Robotics 控制
6.3 机器人的柔顺控制
6.3.2 主动阻抗控制
2、柔顺型阻抗控制
：接触引起的环境变形
：接触时
：不接触时
环境作用于机械手的力为：
0~
~
~
?
??
E
EE
E
x
xxx
x
EEE xKF ~??
Robotics 控制
6.3 机器人的柔顺控制
6.3.3 力和位置混合控制
Kp:刚性对角矩镇阵，可以通过对其元素的修改，适应不同方向
的刚性要求，达到柔顺目的。
Robotics 控制
6.3 机器人的柔顺控制
6.3.3 力和位置混合控制
Robotics 控制
6.3 机器人的柔顺控制
6.3.3 力和位置混合控制
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
将机械手末端运动分解为沿笛卡儿坐标的运动的形式，分
别用各关节的综合运动合成为沿笛卡儿坐标的运动。
主要是针对夹持器的坐标进行讨论。
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
10
)(
1000
)()()()(
)()()()(
)()()()(
)(
)(
6
0
6
0
tpR
tptatotn
tptatotn
tptatotn
tT
t
zzzz
yyyy
xxxx
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
用俯、仰、滚表示夹手的姿态：
可以定义夹手的位置、姿态、线速度、角速度矢量为：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
??
??
??
cs
sc
cs
sc
cs
sc
tptotn
tptotn
tptotn
tR
zzz
yyy
xxx
0
0
001
0
010
0
100
0
0
)()()(
)()()(
)()()(
)(
6
0
T
zyx
T
zyx
TT
zyx
tttttvtvtvtv
tttttptptptp
)](),(),([)()](),(),([)(
)](),(),([)()](),(),([)(
???
???
???
???
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
线速度：
角速度：
)()()( tp
dt
tdptv ???
?
?
?
?
?
?
?
?
?
?
???
??
????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0
0
0
0
0
0
??????????
???????
????????
??
??
??
????
???
????
sccccs
sccs
ccss
R
dt
dR
dt
dR
R
xy
xz
yz
T
T
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
即：
求逆，得
即
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
)(
)(
)(
10
0
0
)(
)(
)(
t
t
t
s
ccs
scc
t
t
t
z
y
x
?
?
?
?
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
)(
)(
)(
0
0
s ec
)(
)(
)(
t
t
t
csssc
cccs
sc
t
t
t
z
y
x
?
?
?
?????
????
??
?
?
?
?
?
?
?
)()]([)( tSt ?????
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
根据雅可比矩阵定义，有
若逆雅可比矩阵存在，则有，
这是分解速度控制的基础。
? ? ? ? )()()()()()()( )( 621 tqqJqJqJtqqJttv ??? ???????? ?
??
?
??
?
?
? ?
)(
)(
)()( 1
t
tv
qJtq?
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
对于加速度有：
从而有：
这是分解加速度控制的基础。
)()(
)(
)(
)(),(
)()()(),(
)(
)(
1
tqqJ
t
tv
qJqqJ
tqqJtqqqJ
t
tv
????
?????
?
?
??
?
?
?
?
?
?
?
???
?
?
?
?
?
?
?
??
?
??
?
????
?
??
?
??
???
)(
)()(),()(
)(
)()()( 111
t
tvqJqqJqJ
t
tvqJtq ??
?
???
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
分解运动速度控制，各关节电机联合运行，保证夹手沿笛
卡儿坐标稳定运动。
先把夹手运动分解为各关节的期望速度，然后对各关节实
行速度伺服控制。
? ?
? ?T
n
T
zyx
qqqtq
ppptx
tqftx
?21)(
)(
)]([)(
?
?
?
???
)()()()( tqqJtxdt tdx ?? ??
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
当 m=n，机械手非冗余时，有
当 m>n，机械手为冗余，需要用广义逆：
mjniqfJ
i
i
ij ?????
?? 1,1,
)()()( 1 txqJtq ?? ??
)()]()()[()( 111 txqJAqJqJAtq TT ?? ????
Robotics 控制
6.4 机器人的分解运动控制
6.4.3 分解运动加速度
分解运动加速度控制：首先计算出工具的笛卡儿坐标加速度，
然后将其分解为相应的各关节加速度，再按照动力学方程计算
出控制力矩。
实际位置和姿态 期望的位置和姿态
位置误差
姿态误差
??????? 1000 )()()()()( tptatotntH ??????? 1000 )()()()()( tptatotntH
dddd
d
?
?
?
?
?
?
?
?
?
?
?
?
?
???
)()(
)()(
)()(
)()()(
tptp
tptp
tptp
tptpte
z
d
z
y
d
y
x
d
x
d
p
? ?ddd ataotontnte ?????? )()()(21)(0
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
为减少位置和姿态误差，要求
因为
)(),()()()(
)()(
)(
)(
)(
tqqqJtqqJtx
tqqJ
t
tv
tx
???????
??
??
??
?
?
?
?
?
?
?
)()]()([)()(
)()]()([)()(
21
21
tekttktt
tektvtvktvtv
o
dd
p
dd
????
????
???? ??
??
??
?
??
??
?
?
?
?
?
??
)(
)()(,
)(
)()(
te
tete
t
tvtx
o
p
d
d
d
?
?
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
从而有：
代入
得：
因为这里考虑的是误差项，因此是闭环控制，精度高。
)()]()([)()( 21 tektxtxktxtx dd ???? ??????
)(),()()()( tqqqJtqqJtx ??????? ??
)](),()()()()[()()( 2111 tqqqJtektxktxqJtqktq dd ????????? ?????? ?
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.1 机器人关节控制的 PID算法
如果机器人的关节位置误差为：
则其 PID控制为
qqe d ??
})(
1
{
)()()()(
0
0
eTde
T
ek
tekdektektu
D
t
I
P
D
t
IP
?
?
???
???
?
?
??
??
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.1 机器人关节控制的 PID算法
,比例系数；控制量中，减小误差的直观比例部分
,微分系数；提高响应速度，相位超前；使系统趋于稳定；
,积分系数；提高稳态精度，相位滞后；使系统趋于不稳定
,积分时间；
,微分时间；D
I
D
I
P
T
T
k
k
k
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.1 机器人关节控制的 PID算法
微分超前型 PD控制,
积分项在使用中，经常出现饱和，有时会使系统不稳定，因此
常常使用 PD控制。其控制律为
xkekekektu DPDP ?? ????)(
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.2 模糊集合与模糊逻辑的基本概念
一、模糊集合与集合计算符
定义 1 模糊集合,设 U为若干事件的总和，如 U=Rn，我们称 U为
论域，一个定义在 U上的模糊集合 F，由隶属度函数
来表征，这里的 表示 在模糊集合 F上的隶属程度。
经典的集合（确定集合）的隶属度函数只取两个值 {0,1}。
要么属于，要么不属于。因此模糊集合是经典集合的推广。
? ?1,0,?UF?
)(uF? Uu?
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.2 模糊集合与模糊逻辑的基本概念
定义 2 交集、并集和补集,设 A和 B是 U上的两个模糊集合。对所
有的, A和 B的 交集 是定义在 U上的一个模糊集合，其隶属
度函数定义为：
对所有的, A和 B的 并集 是定义在 U上的一个模糊集合，其
隶属度函数定义为：
对所有的, A的补集是定义在 U上的一个模糊集合，其隶
属度函数为：
Uu?
)}(),(m in {)( uuu BABA ??? ??
Uu?
)}(),(m a x {)( uuu BABA ??? ??
Uu?
)(1)( uu AA ?? ??
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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[in f {),( vcucvu ABBA ??? ????? ?
)}()(]1,0[s u p {),( vcucvu BABA ??? ?????
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.3 模糊规则与模糊推理
模糊规则 是由如下形式的, 如果 ---则, 规则的总和组成
R（ l）,如果 x1为 F1l,且 …,且 xn为 Fnl，则 y为 Gl；
Fil, Gl为模糊集合,xi为模糊变量。
将变量 模糊化 后，经过按照模糊规则的运算，获得模糊结果，
这个过程称为一个 模糊推理 。模糊推理得到的模糊输出，再经
过反 模糊化,即可得到模糊推理的精确解。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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[in f {),( vcucvu ABBA ??? ????? ?
)}()(]1,0[s u p {),( vcucvu BABA ??? ?????
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.3 模糊规则与模糊推理
模糊规则 是由如下形式的, 如果 ---则, 规则的总和组成
R（ l）,如果 x1为 F1l,且 …,且 xn为 Fnl，则 y为 Gl；
Fil, Gl为模糊集合,xi为模糊变量。
将变量 模糊化 后，经过按照模糊规则的运算，获得模糊结果，
这个过程称为一个 模糊推理 。模糊推理得到的模糊输出，再经
过反 模糊化,即可得到模糊推理的精确解。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
一个典型的模糊控制系统
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
例：一热处理用的电热炉，按工艺要求须保持炉温 600O不变。由
于炉温受零件数量、体积、环境温度变化、电网电压波动等影
响，会出现波动所以要设计控制器。
控制方式,通过改变可控硅的导通角实现；也可以通过 PWM方式
调节。人工调节时，通过面板上的电位器实现，计
算机调节通过驱动线路实现。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
典型的温度控制电路如图
INT1,INT2过零检测,PB7触发控制,AN0传感器输入 A/D。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
人工操作时，根据经验，控制规则可以用语言描述如下：
如果炉稳低于 600OC则升压，低得越多升压越高；
如果炉稳高于 600OC则降压，高得越多降压越低；
如果炉稳等于 600OC则保持电压不变；
采用 模糊控制 时，其工作原理如下：
1、模糊控制器的输入变量与输出变量
设定炉温 t0=600，测量炉稳 t（ K），则将误差 e(K)=t0-t(K)
作为模糊控制的 输入变量 。
输出变量 为：控制电压 u，可通过改变可控硅的导通角或 PWM
比例实现。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
由此得模糊变量赋值表：
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
7、模糊控制表
上述的结果即可用于实际控制，但是，为了提高控制速度，
一般将各种情况预先计算出，存入表中，形成模糊控制表。
输入量和输出量的值，可以
自行定义，如定义 -3=550OC
e -3 -2 -1 0 1 2 3
u 3 2 1 0 -1 -2 -3
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.5 机器人关节位置的模糊控制
我们可以将上述温度变量换为机器人的关节角度，将误差
定义为期望转角与实测转角的差，输出定义为关节控制电压，
即可实现机器人关节角位置的模糊控制。
Control of Robotics
6.1 机器人的基本控制原则
6.2 机械人的位置控制
6.3 机械人的柔顺控制
6.4 机器人的分解运动控制
6.5 机器人关节控制的模糊算法
Robotics 控制
6.1 机器人的基本控制原则
机器人控制特点：冗余的、多变量、本质非线性、耦合的
6.1.1 基本控制原则
1、控制器分类
结构形式,伺服、非伺服、位置反馈、速度反馈、力矩控制、
控制方式,非线性控制、分解加速度控制、最优控制、自适
应控制、滑模控制、模糊控制等
控制器选择,依工作任务，可选 PLC控制、普通计算机控制，
智能计算机控制等。
简单分类,单关节控制器：主要考虑稳态误差补偿；
多关节控制器：主要考虑耦合惯量补偿。
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
一般分类, PLC、单片机、小型计算机、多计算机分布控制
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
2、主要控制变量
任务轴 R0：描述工件位置的坐标系
X(t):末端执行器状态；
θ (t):关节变量；
C(t)：关节力矩矢量；
T(t),电机力矩矢量；
V(t)：电机电压矢量
本质是对下列双向方程的控制,
)()()()()( ttttt XCTV ???? ?
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
3、主要控制层次
分三个层次：人工智能级、控制模式级、伺服系统级
1）人工智能级
完成从机器人工作任务的语言描述 生成 X(t)；
仍处于研究阶段。
2）控制模式级
建立 X(t) T(t)之间的双向关系。
电机模型 传动模型 关节动力学模型 机器人模型
)()()()( tttt TCX ??? ?
)()()()( tttt XCT ?
Robotics 控制
6.1 机器人的基本控制原则
6.1.1 基本控制原则
3、伺服系统级
解决关节伺服控制问题
即 TV?
Robotics 控制
6.1 机器人的基本控制原则
6.1.2 伺服控制系统举例
1、液压缸伺服传动系统
优点：减少减速器等，消除了间隙和磨损误差，结构简单、
精度与电器传动相当。同样可以进行位置、速度、加速度及力
的反馈。
Robotics 控制
6.1 机器人的基本控制原则
6.1.2 伺服控制系统举例
2、典型的滑阀控制液压传动系统
Robotics 控制
6.2 机器人的位置控制
由于机器人系统具有高度非线性，且机械结构很复杂，因
此在研究其动态模型时，做如下假设：
（ 1）机器人各连杆是理想刚体，所有关节都是理想的，不存在
摩擦和间隙；
（ 2）相邻两连杆间只有一个自由度，或为旋转、或为平移。
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
伺服电机的参数：
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
（ 1）磁场型控制电机
222
2
2
,,
???
?
??
cc
m
c
m
m
mm
m
fmm
f
ffff
k
K
f
fF
J
JJ
K
dt
d
F
dt
d
JT
ikT
dt
di
lirv
?????
???
?
??
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
Laplace变换得：
)()()(
)()(
)()()(
2
sKFsJssT
sIksT
sIslrsV
mm
fmm
ffff
????
?
??
))(()(
)(
2 KFsJsslr
k
sV
s
ff
m
f
m
???
??
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
一般可取 K=0，则有等效框图
同时，传递函数变为
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
：电气时间常数；
：机械时间常数。
)1)(1(
)1)(1(
1
))(()(
)(
0
sss
k
s
F
J
s
r
l
s
Fr
k
FJsslrs
k
sV
s
me
f
ff
m
ff
m
f
m
??
?
??
?
??
?
??
?
m
e
?
?
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
由于,有时可以忽略，于是
而对角速度的传递函数为：
，因为
me ?? ??
)1()(
)( 0
ss
k
sV
s
mf
m
?
?
?
?
s
k
sV
s
mf
m
??
??
1)(
)( 0
dt
d m
m
?? ?
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
（ 2）电枢控制型电机
Ke:产生反电势。
222
2
2
,,
'
???
?
??
?
cc
m
c
m
m
mm
m
mmm
me
m
mmmm
k
K
f
fF
J
JJ
K
dt
d
F
dt
d
JT
ikT
k
dt
di
LiRV
?????
???
?
???
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
1、传递函数与等效方框图
经拉氏变换、并设 K=0，有
? ?memm
m
m
m
kkJsFsLRs
k
sV
s
????
??
))(()(
)(?
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
2、直流电机的转速调整
误差信号：
)()()( 0 ttte i ?? ??
Robotics 控制
6.2 机器人的位置控制
6.2.1 直流传动系统的建模
2、直流电机的转速调整
比例补偿：控制输出与 e(t)成比例；
微分补偿：控制输出与 de(t)/dt成比例；
积分补偿：控制输出与 ∫ e(t)dt成比例；
测速补偿：与输出位置的微分成比例。
比例微分 PD补偿：
比例积分 PI补偿：
比例微分积分 PID补偿：
测速补偿时：
)()()( sEsksE d????
)()()( sEsksE i????
)()()( sEsksE id ?? ????
)()1()()()()( 00 ssssssEsE tit ????? ??????
Robotics 控制
6.2 机器人的位置控制
6.2.2 位置控制的基本结构
1、基本控制结构
位置控制也称 位姿控制,或 轨迹控制 。分为：
点到点 PTP控制；如点焊； 连续路径 CP控制；如喷漆
期望的关节位置 期望的工具位置和姿态
? ?Tdnddd qqqq ?21? ],[ TdTdd pw ??
Robotics 控制
6.2 机器人的位置控制
6.2.2 位置控制的基本结构
2,PUMA机器人的伺服控制结构
1）机器人控制系统设计与一般计算机控制系统相似。
2）多数仍采用连续系统的设计方法设计控制器，然后再将
设计好的控制律离散化，用计算机实现。
3）现有的工业机器人大多数采用独立关节的 PID控制。
下图 PUMA机器人的伺服控制系统构成
Robotics 控制
6.2 机器人的位置控制
6.2.2 位置控制的基本结构
2,PUMA机器人的伺服控制结构
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
1、位置控制系统结构
具有力、位移、速度反馈
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
1、位置控制系统结构
控制器路径点的获取方式：
（ 1）以数字形式输入系统；若以直角坐标给出，须计算获
得其关节坐标位置。
（ 2）以示教方式输入系统；系统将直接获得关节坐标位置
允许机器人只移动一个关节，而锁住其他关节。
轨迹控制：
按关键点或轨迹进行定位控制。
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
对图示系统，有
J,等效转动惯量；
B,等效阻尼系数。
lm
lma
mmm
BBB
JJJJ
BJT
2
2
?
?
??
??
???
?? ???
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
因此可得其传递函数（同电枢控制直流伺服电机）
? ?)()()(
)(
2
Iemmmm
I
m
m
KkBRsBLJRJsLs
K
sV
s
?????
?
)]()([)(
)()(
)()()(
ttKtV
tt
ttte
sdm
ms
sd
??
???
??
? ??
?
??
)]()([)(
)()(
)()()(
ssKsV
ss
sssE
sdm
ms
sd
??
???
??
? ??
?
??
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
其开环传递函数为：
因为：,略去 Lm的项，简化上式为：
? ?)()()(
)(
2
Iemmmm
Is
KkBRsBLJRJsLs
KK
sE
s
?????
???
,mm RL ???
? ?)()(
)(
Iemm
Is
KkBRJsRs
KK
sE
s
???
???
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
则其闭环传递函数为：
这是一个典型的二阶系统闭环传递函数。
)()()(
1
)()(1
)()(
)(
)(
2
JRKKJRskKBRsJR
KK
sEs
sEs
s
s
mmeImm
I
s
s
d
s
?
?
?
?
?
?
?
???
?
?
?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
2、单关节控制器的传递函数
含有速度反馈的机械手单关节控制器的开环传递函数为
闭环传递函数为
sKKKKkBRJsR
KK
sE
s
tIIemm
Is
)()(
)(
1
2 ????
???
ItIeImm
I
s
s
d
s
KKsKKKkKBRJsR
KK
sEs
sEs
s
s
?
?
?
?
?
?
?
?
????
?
?
?
)(
)()(1
)()(
)(
)(
1
2
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
由上述闭环传递函数，得控制系统的特征方程为：
将其写为二阶系统标准形式
得
1/ KK?
0)( 12 ????? ItIeImm KKsKKKkKBRJsR ??
02 22 ??? nn ss ???
JRKKKKkKBR
JRKK
mIteIm
mIn
?
?
??
??
2/)]([
0/
1???
??
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
1/ KK?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
1/ KK?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 1） 的确定
设结构的共振频率为,则为避免运动中发生共振，要求
同时要求系统阻尼大于 1，
J值随负载和位姿变化，应选可能的最大惯量。
r?
I
mrr
mIn K
JRKJRKK
?
????
?? 4,2/
2
???
tetIrm
mIteIm
KkKKBJRK
JRKKKKkKBR
//)/(
12/)]([
1
1
???
????
?
?? ?
1/ KK?
Robotics 控制
6.2 机器人的位置控制
6.2.3 单关节位置控制器
3、控制参数确定与稳态误差
（ 2）稳态误差
根据控制理论，在控制系统框图中，计算得到 E(s),即可
得到系统的稳态位置误差、速度误差和加速度误差。
对于单位阶越位移 C0，其稳态误差为
)(lim)(lim 0 ssEtee stss ??? ??
ILgfmssp KKCCCRe ?? /)( ???
Robotics 控制
6.2 机器人的位置控制
6.2.4 多关节位置控制器
1）为快速运动，一般应采用多关节协调、同步运动。
2）这时各关节的位置和速度会互相作用，因此，必须进行附
加补偿。
1、动态拉格朗日公式
其他关节加速 自身加速 科式力 重力
且 D项皆与关节角有关。
i
j k
kjijkiai
i
jiji DqqDqJqDT ???? ? ??
? ??
6
1
6
1
6
1
??????
Robotics 控制
6.2 机器人的位置控制
6.2.4 多关节位置控制器
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
柔性无刚性作用的运动控制。 如擦玻璃、抓鸡蛋、装配等
一般应增加力反馈。
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
1、被动柔顺和主动柔顺
被动柔顺,通过弹簧、消振器等机械结构或通过改变机械操
作方式而使机器人与工作对象间产生相对柔性运
动的柔顺方式。（ 无须控制器参与 ）
如,1）海绵擦玻璃,2）把工件拉进孔取代推入
主动柔顺,通过改变控制器控制方式，增加力反馈等使机器
人与工作对象间无刚性运动的柔顺方式。（ 必须
控制器参与 ）
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
1、被动柔顺和主动柔顺
定义：
：在工作点的小位移；
：工作点的关节小位移
：正定对角刚度矩阵
：机器人雅可比矩阵
：回复力
：关节力矩
定义关节刚度矩阵：, 反应力矩与微位移关系
?
?
?
F
J
K
q
x
p
qJKJ
qJx
FJ
xKF
p
T
T
p
??
??
?
?
)(??
?
?
??
JKJK pTq ?
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
2、作业约束和力控制
自然约束与人为约束
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
2、作业约束和力控制
约束对机器人力控制的影响：
1）约束使自由度减少，限制了末端的运动方式；
2）约束给机器人增加了作用力，增加了控制的复杂程度；
3）上述两种情况会相互作用
Robotics 控制
6.3 机器人的柔顺控制
6.3.1柔顺控制的基本概念
3、柔顺控制的种类
1）阻抗控制
通过控制力和位置间的动态关系（阻抗），来实现柔顺
功能。即：通过控制使机械手末端呈现所需要的刚性和阻尼。
需要位置控制的自由度，需要大的刚性；
需要力控制的自由度，需要小的刚性。
2）力和位置混合控制
将控制分为一些自由度的位置控制，和另一些自由度的力
控制，通过计算，在关节空间合并，进行关节控制。
Robotics 控制
6.3 机器人的柔顺控制
6.3.2 主动阻抗控制
1、位置型阻抗控制
：估计重力矩
：雅可比矩阵
：机械手等效刚度比例系数
：机械手等效阻尼系数
教材中的稳定性讨论，内容不全，这里省略。
]~~)[()(?
~
xKxKqJqg
xxx
Dp
T
d
????
??
?
D
p
K
K
qJ
qg
)(
)(?
Robotics 控制
6.3 机器人的柔顺控制
6.3.2 主动阻抗控制
2、柔顺型阻抗控制
：接触引起的环境变形
：接触时
：不接触时
环境作用于机械手的力为：
0~
~
~
?
??
E
EE
E
x
xxx
x
EEE xKF ~??
Robotics 控制
6.3 机器人的柔顺控制
6.3.3 力和位置混合控制
Kp:刚性对角矩镇阵，可以通过对其元素的修改，适应不同方向
的刚性要求，达到柔顺目的。
Robotics 控制
6.3 机器人的柔顺控制
6.3.3 力和位置混合控制
Robotics 控制
6.3 机器人的柔顺控制
6.3.3 力和位置混合控制
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
将机械手末端运动分解为沿笛卡儿坐标的运动的形式，分
别用各关节的综合运动合成为沿笛卡儿坐标的运动。
主要是针对夹持器的坐标进行讨论。
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
10
)(
1000
)()()()(
)()()()(
)()()()(
)(
)(
6
0
6
0
tpR
tptatotn
tptatotn
tptatotn
tT
t
zzzz
yyyy
xxxx
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
用俯、仰、滚表示夹手的姿态：
可以定义夹手的位置、姿态、线速度、角速度矢量为：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
??
??
??
cs
sc
cs
sc
cs
sc
tptotn
tptotn
tptotn
tR
zzz
yyy
xxx
0
0
001
0
010
0
100
0
0
)()()(
)()()(
)()()(
)(
6
0
T
zyx
T
zyx
TT
zyx
tttttvtvtvtv
tttttptptptp
)](),(),([)()](),(),([)(
)](),(),([)()](),(),([)(
???
???
???
???
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
线速度：
角速度：
)()()( tp
dt
tdptv ???
?
?
?
?
?
?
?
?
?
?
???
??
????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
????
0
0
0
0
0
0
??????????
???????
????????
??
??
??
????
???
????
sccccs
sccs
ccss
R
dt
dR
dt
dR
R
xy
xz
yz
T
T
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
即：
求逆，得
即
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
)(
)(
)(
10
0
0
)(
)(
)(
t
t
t
s
ccs
scc
t
t
t
z
y
x
?
?
?
?
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
)(
)(
)(
0
0
s ec
)(
)(
)(
t
t
t
csssc
cccs
sc
t
t
t
z
y
x
?
?
?
?????
????
??
?
?
?
?
?
?
?
)()]([)( tSt ?????
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
根据雅可比矩阵定义，有
若逆雅可比矩阵存在，则有，
这是分解速度控制的基础。
? ? ? ? )()()()()()()( )( 621 tqqJqJqJtqqJttv ??? ???????? ?
??
?
??
?
?
? ?
)(
)(
)()( 1
t
tv
qJtq?
Robotics 控制
6.4 机器人的分解运动控制
6.4.1 分解运动控制原理
对于加速度有：
从而有：
这是分解加速度控制的基础。
)()(
)(
)(
)(),(
)()()(),(
)(
)(
1
tqqJ
t
tv
qJqqJ
tqqJtqqqJ
t
tv
????
?????
?
?
??
?
?
?
?
?
?
?
???
?
?
?
?
?
?
?
??
?
??
?
????
?
??
?
??
???
)(
)()(),()(
)(
)()()( 111
t
tvqJqqJqJ
t
tvqJtq ??
?
???
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
分解运动速度控制，各关节电机联合运行，保证夹手沿笛
卡儿坐标稳定运动。
先把夹手运动分解为各关节的期望速度，然后对各关节实
行速度伺服控制。
? ?
? ?T
n
T
zyx
qqqtq
ppptx
tqftx
?21)(
)(
)]([)(
?
?
?
???
)()()()( tqqJtxdt tdx ?? ??
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
当 m=n，机械手非冗余时，有
当 m>n，机械手为冗余，需要用广义逆：
mjniqfJ
i
i
ij ?????
?? 1,1,
)()()( 1 txqJtq ?? ??
)()]()()[()( 111 txqJAqJqJAtq TT ?? ????
Robotics 控制
6.4 机器人的分解运动控制
6.4.3 分解运动加速度
分解运动加速度控制：首先计算出工具的笛卡儿坐标加速度，
然后将其分解为相应的各关节加速度，再按照动力学方程计算
出控制力矩。
实际位置和姿态 期望的位置和姿态
位置误差
姿态误差
??????? 1000 )()()()()( tptatotntH ??????? 1000 )()()()()( tptatotntH
dddd
d
?
?
?
?
?
?
?
?
?
?
?
?
?
???
)()(
)()(
)()(
)()()(
tptp
tptp
tptp
tptpte
z
d
z
y
d
y
x
d
x
d
p
? ?ddd ataotontnte ?????? )()()(21)(0
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
为减少位置和姿态误差，要求
因为
)(),()()()(
)()(
)(
)(
)(
tqqqJtqqJtx
tqqJ
t
tv
tx
???????
??
??
??
?
?
?
?
?
?
?
)()]()([)()(
)()]()([)()(
21
21
tekttktt
tektvtvktvtv
o
dd
p
dd
????
????
???? ??
??
??
?
??
??
?
?
?
?
?
??
)(
)()(,
)(
)()(
te
tete
t
tvtx
o
p
d
d
d
?
?
Robotics 控制
6.4 机器人的分解运动控制
6.4.2 分解运动速度控制
从而有：
代入
得：
因为这里考虑的是误差项，因此是闭环控制，精度高。
)()]()([)()( 21 tektxtxktxtx dd ???? ??????
)(),()()()( tqqqJtqqJtx ??????? ??
)](),()()()()[()()( 2111 tqqqJtektxktxqJtqktq dd ????????? ?????? ?
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.1 机器人关节控制的 PID算法
如果机器人的关节位置误差为：
则其 PID控制为
qqe d ??
})(
1
{
)()()()(
0
0
eTde
T
ek
tekdektektu
D
t
I
P
D
t
IP
?
?
???
???
?
?
??
??
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.1 机器人关节控制的 PID算法
,比例系数；控制量中，减小误差的直观比例部分
,微分系数；提高响应速度，相位超前；使系统趋于稳定；
,积分系数；提高稳态精度，相位滞后；使系统趋于不稳定
,积分时间；
,微分时间；D
I
D
I
P
T
T
k
k
k
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.1 机器人关节控制的 PID算法
微分超前型 PD控制,
积分项在使用中，经常出现饱和，有时会使系统不稳定，因此
常常使用 PD控制。其控制律为
xkekekektu DPDP ?? ????)(
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.2 模糊集合与模糊逻辑的基本概念
一、模糊集合与集合计算符
定义 1 模糊集合,设 U为若干事件的总和，如 U=Rn，我们称 U为
论域，一个定义在 U上的模糊集合 F，由隶属度函数
来表征，这里的 表示 在模糊集合 F上的隶属程度。
经典的集合（确定集合）的隶属度函数只取两个值 {0,1}。
要么属于，要么不属于。因此模糊集合是经典集合的推广。
? ?1,0,?UF?
)(uF? Uu?
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.2 模糊集合与模糊逻辑的基本概念
定义 2 交集、并集和补集,设 A和 B是 U上的两个模糊集合。对所
有的, A和 B的 交集 是定义在 U上的一个模糊集合，其隶属
度函数定义为：
对所有的, A和 B的 并集 是定义在 U上的一个模糊集合，其
隶属度函数定义为：
对所有的, A的补集是定义在 U上的一个模糊集合，其隶
属度函数为：
Uu?
)}(),(m in {)( uuu BABA ??? ??
Uu?
)}(),(m a x {)( uuu BABA ??? ??
Uu?
)(1)( uu AA ?? ??
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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[in f {),( vcucvu ABBA ??? ????? ?
)}()(]1,0[s u p {),( vcucvu BABA ??? ?????
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.3 模糊规则与模糊推理
模糊规则 是由如下形式的, 如果 ---则, 规则的总和组成
R（ l）,如果 x1为 F1l,且 …,且 xn为 Fnl，则 y为 Gl；
Fil, Gl为模糊集合,xi为模糊变量。
将变量 模糊化 后，经过按照模糊规则的运算，获得模糊结果，
这个过程称为一个 模糊推理 。模糊推理得到的模糊输出，再经
过反 模糊化,即可得到模糊推理的精确解。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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[in f {),( vcucvu ABBA ??? ????? ?
)}()(]1,0[s u p {),( vcucvu BABA ??? ?????
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.3 模糊规则与模糊推理
模糊规则 是由如下形式的, 如果 ---则, 规则的总和组成
R（ l）,如果 x1为 F1l,且 …,且 xn为 Fnl，则 y为 Gl；
Fil, Gl为模糊集合,xi为模糊变量。
将变量 模糊化 后，经过按照模糊规则的运算，获得模糊结果，
这个过程称为一个 模糊推理 。模糊推理得到的模糊输出，再经
过反 模糊化,即可得到模糊推理的精确解。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
一个典型的模糊控制系统
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
例：一热处理用的电热炉，按工艺要求须保持炉温 600O不变。由
于炉温受零件数量、体积、环境温度变化、电网电压波动等影
响，会出现波动所以要设计控制器。
控制方式,通过改变可控硅的导通角实现；也可以通过 PWM方式
调节。人工调节时，通过面板上的电位器实现，计
算机调节通过驱动线路实现。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
典型的温度控制电路如图
INT1,INT2过零检测,PB7触发控制,AN0传感器输入 A/D。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
人工操作时，根据经验，控制规则可以用语言描述如下：
如果炉稳低于 600OC则升压，低得越多升压越高；
如果炉稳高于 600OC则降压，高得越多降压越低；
如果炉稳等于 600OC则保持电压不变；
采用 模糊控制 时，其工作原理如下：
1、模糊控制器的输入变量与输出变量
设定炉温 t0=600，测量炉稳 t（ K），则将误差 e(K)=t0-t(K)
作为模糊控制的 输入变量 。
输出变量 为：控制电压 u，可通过改变可控硅的导通角或 PWM
比例实现。
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
由此得模糊变量赋值表：
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.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 控制
6.5 机器人关节控制的模糊算法
6.5.4 模糊控制工作原理
7、模糊控制表
上述的结果即可用于实际控制，但是，为了提高控制速度，
一般将各种情况预先计算出，存入表中，形成模糊控制表。
输入量和输出量的值，可以
自行定义，如定义 -3=550OC
e -3 -2 -1 0 1 2 3
u 3 2 1 0 -1 -2 -3
Robotics 控制
6.5 机器人关节控制的模糊算法
6.5.5 机器人关节位置的模糊控制
我们可以将上述温度变量换为机器人的关节角度，将误差
定义为期望转角与实测转角的差，输出定义为关节控制电压，
即可实现机器人关节角位置的模糊控制。
