机器人运动学
Kinematics of Robotics
3.1 机器人运动方程的表示
(姿态和方向角 \位置和坐标 \连杆变换矩阵 )
3.2 机械手运动方程的求解
(欧拉变换解 /滚仰偏变换解 /球面变换解 )
3.3 PUMA560机器人运动方程
(运动分析 /运动综合 )
3.4 机器人的雅可比公式
(微分运动 /雅可比矩阵 /计算实例 )
Robotics 运动学
3.1 机器人运动方程的表示
3.1.0 A矩阵和 T矩阵
机械手可以看成由一系列关节连接起来的连杆组构成,
用 A矩阵 描述连杆坐标系间相对平移和旋转的齐次变换,
A1表示第一连杆对基坐标的位姿
A2表示第二连杆对第一连杆位姿
则第二连杆对基坐标的位姿为
212 AAT ?
6543216 AAAAAAT ?
Robotics运动学
3.1 机器人运动方程的表示
3.1.1 运动姿态和方向角
1.运动方向
接近矢量 a:夹持器进入物体的方向 ;Z轴
方向矢量 o:指尖互相指向 ;Y轴
法线矢量 n:指尖互相指向 ;X轴
111 ??????
??
aoaaoo
aon
?
?
?
?
?
?
?
?
?
?
?
?
??
1000
6
zzzz
yyyy
xxxx
paon
paon
paon
TT
Robotics运动学
3.1 机器人运动方程的表示
3.1.1 运动姿态和方向角
2.用旋转系列表示运动姿态
欧拉角,绕 Z轴转 φ,再绕 新 Y轴转 θ,绕 最新 Z轴转 ψ,
(3-3)
注意,坐标变换是右乘,即后面的变
换乘在右边,(绕新轴转,连乘 )
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
0100
00
00
1000
00
0010
00
1000
0100
00
00
),(),(),(),,(
??
??
??
??
??
??
??????
cs
sc
cs
sc
cs
sc
zR o tyR o tzR o tE u l e r
Robotics运动学
3.1 机器人运动方程的表示
3.1.1 运动姿态和方向角
3.用滚 \仰 \偏转表示运动姿态
横滚,绕 Z轴转 φ,
俯仰,绕 Y轴转 θ,
偏转,绕 X轴转 ψ,
(3-5)
注意,左乘,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
00
00
0001
1000
00
0010
00
1000
0100
00
00
),(),(),(),,(
??
??
??
??
??
??
??????
cs
sc
cs
sc
cs
sc
xR o tyR o tzR o tRPY
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
1.用 柱面 坐标表示 末端 运动位置
由于上述绕 Z轴的旋转,使末端执行器的姿态出现变化,
若要执行器姿态不变,则需将其绕执行器 Z轴反向旋转,
(3-8) ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
100
010
001
1000
0100
00)()(
00)()(
1000
100
0
0
),,(
z
rs
rc
cs
sc
z
rscs
rcsc
rzC y l
?
?
??
??
???
???
?
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
2.用 球面 坐标表示 末端 运动位置
沿 Z平移 r,绕 Y轴转 β,绕 Z轴转 α,
(3-10) ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
1000
0
1000
100
0010
0001
1000
00
0010
00
1000
0100
00
00
),0,0(),(),(),,(
???
???????
???????
??
??
??
??
????
rccs
srsssccs
srcccscc
rcs
sc
cs
sc
rT r a n syR o tzR o trS p h
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
表示物体的位置,笛卡尔坐标、柱面坐标、球面坐标
1.用 柱面 坐标表示 末端 运动位置
沿 X平移 r,绕 Z轴转 α,沿 Z轴平移 z.
(绕原坐标系运动,左乘 )
(3-7) ?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
100
0
0
1000
0100
0010
001
1000
0100
00
00
1000
100
0010
0001
)0,0,(),(),0,0(),,(
z
rscs
rcsc
r
cs
sc
z
rT r a n szR o tzT r a n srzC yl
???
???
??
??
??
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
2.用 球面 坐标表示 末端 运动位置
沿 Z平移 r,绕 Y轴转 β,绕 Z轴转 α,
(3-10) ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
1000
0
1000
100
0010
0001
1000
00
0010
00
1000
0100
00
00
),0,0(),(),(),,(
???
???????
???????
??
??
??
??
????
rccs
srsssccs
srcccscc
rcs
sc
cs
sc
rT r a n syR o tzR o trS p h
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
2.用 球面 坐标表示 末端 运动位置
由于上述两个旋转,使执行器姿态发生变化,为保持姿
态,执行器要绕其自身 Y和 Z轴反向旋转,
(3-11)
?
?
?
?
?
?
?
?
?
?
?
?
?
???
1000
100
010
001
),(),(),0,0(),(),(),,(
?
??
??
??????
rc
srs
src
zR o tyR o trT r a n syR o tzR o trS p h
AA
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆 (D-H坐标 )
全为转动关节,
Zi坐标轴 ;
Xi坐标轴 ;
Yi坐标轴 ;
连杆长度 ai;
连杆扭角 α i;
两连杆距离 di;
两杆夹角 θ i
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆
全为转动关节,
Zi坐标轴,沿着 i+1关节的运动轴 ;
Xi坐标轴,沿着 Zi和 Zi-1的公法线,指向离开 Zi-1轴的方向 ;
Yi坐标轴,按右手直角坐标系法则制定 ;
连杆长度 ai; Zi和 Zi-1两轴心线的公法线长度 ;
连杆扭角 α i,Zi和 Zi-1两轴心线的夹角 ;
两连杆距离 di:相邻两杆三轴心线的两条公法线间的距离 ;
两杆夹角 θ i,Xi和 Xi-1两坐标轴的夹角 ;
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆 (D-H坐标 )
含移动关节,
Zi坐标轴 ;
Xi坐标轴 ;
Yi坐标轴 ;
连杆长度 ai=0;
连杆扭角 α i;
两连杆距离 di;
两杆夹角 θ i
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆
含移动关节,
Zi坐标轴,沿着 i+1关节的运动轴 ;
Xi坐标轴,沿着 Zi和 Zi-1的公法线,指向离开 Zi-1轴的方向 ;
Yi坐标轴,按右手直角坐标系法则制定 ;
连杆长度 ai; Zi和 Zi-1两轴心线的公法线长度 ;
连杆扭角 α i,Zi和 Zi-1两轴心线的夹角 ;
两连杆距离 di:相邻两杆三轴心线的两条公法线间的距离 ;
两杆夹角 θ i,Xi和 Xi-1两坐标轴的夹角 ;
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
2.广义变换矩阵
建立 D-H坐标系后,可通过两个旋转、两个平移建立相邻
连杆 i-1和 i间的相对关系。
1。绕 Zi-1轴转 θ i角，使 Xi-1转到与 Xi同一平面内；
2。沿 Zi-1轴平移 di，把 Xi-1移到与 Xi同一直线上；
3。沿 i轴平移 ai-1,把连杆 i-1的坐标系移到使其原点与
连杆 i的坐标系原点重合的位置；
4。绕 Xi-1轴转 α i-1角，使 Zi-1转到与 Zi同一直线上；
这四个齐次变换叫 Ai矩阵：
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
2.广义变换矩阵
对旋转关节,
(3-13)
对棱柱关节,
(3-14)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
???
1000
0
),()0,0,(),0,0(),(
11
111
111
iii
iiiiiii
iiiiiii
iiiii
dcs
sascccs
casscsc
xR o taT r a n sdT r a n szR o tA
??
??????
??????
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
1000
0
1
0
),()0,0,(),0,0(),(
11
11
11
iii
iiiii
iiiii
iiiii
dcs
scccs
sscsc
xR o taT r a n sdT r a n szR o tA
??
?????
?????
??
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
3.用 A矩阵表示 T矩阵
T6:机械手末端对其基座
Z:机械手基座对参考坐标系
E:端部工具对机械手末端
X:端部工具对参考坐标系
6161 AAAT iii ??? ?
11
6
6
???
?
XEZT
EZTX
Robotics运动学
3.2 机械手运动方程的求解
1)问题,已知手部位姿,求各关节位置
2)意义,是机械手控制的关键
3)没有一种算法可以通用,需要几何设置引导
本节介绍上节的几种特殊变换下的求解算法,
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
1.基本隐式方程的解
若上式中 T矩阵的各元素已知，即
（ 3-24）
对应项相等，有
),(),(),(),,( ?????? zR o tyR o tzR o tE u l e rT ??
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
0
0
1111
?????
????????????
????????????
csscs
ssccscsssccs
sccssccssccc
paon
paon
paon
zzzz
yyyy
xxxx
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
arccos:符号不定；
特殊点不准确；
0或 180时，后
（ 3-25/33） 两式没定义。
?
??
??
??
?????
?????
??
?????
?????
ca
ssa
sca
sso
ccscso
csscco
csn
scccsn
sscccn
z
y
x
z
y
x
z
y
x
?
?
?
?
???
???
??
??
??
)/a r c c o s (
)/a r c c o s (
)a r c c o s (
??
??
?
sn
sa
a
z
x
z
??
?
?
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
2.用显式方程求各角度
(3-37)
(3-39)
),(),(),(
),(),(),(
11
1
???
???
zR o tTzR o tyR o t
zR o tyR o tTzR o t
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
00
0
11111000
0100
00
00
?????
??
?????
??
??
csscs
cs
ssccc
paon
paon
paon
cs
sc
zzzz
yyyy
xxxx
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
其中 (3-40)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
00
0
1000
)()()()(
)()()()(
)()()()(
13131313
12121212
11111111
?????
??
?????
csscs
cs
ssccc
pfafofnf
pfafofnf
pfafofnf
paonu
uuf
ucusuf
usucuf
z
yx
yx
,,,
)(
)(
)(
13
12
11
?
?
???
??
??
??
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
由 有, 即
(3-42/43)
这样,由,得
(3-44)
再由,得
(3-45)
0)(12 ?af 0??? yx acas ??
)/t a n ()/t a n ( xyxy aaaaaa ???? ??
)()( 1311 afcafs ?? ??
)t a n(
z
yx
a
asaca ??? ??
)()( 1212 ofcnfs ?? ??
)t a n(
yx
yx
ocos
ncnsa
??
???
??
???
Robotics运动学
3.2 机械手运动方程的求解
3.2.2 滚、仰、偏变换解
由
（ 3-47）
f定义同前。
),(),(),( 1 ??? xR o tyR o tTzR o t ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
00
0
1000
)()()()(
)()()()(
)()()()(
13131313
12121212
11111111
?????
??
?????
ccscs
sc
csssc
pfafofnf
pfafofnf
pfafofnf
Robotics运动学
3.2 机械手运动方程的求解
3.2.2 滚、仰、偏变换解
由 得
（ 3-48）
这样，由
可得：
（ 3-50）
再由
得
（ 3-51）
0)(12 ???? yx ncnsnf ??
)t a n (
x
y
n
na??
???? snnfcnsncnf zyx ?????? )()( 1311
)t an (
yx
z
nsnc
na
??? ?
??
?????? sacasafcocosof yxyx ????????? )()( 1212
)t a n(
yx
yx
ocos
acasa
??
???
??
??
Robotics运动学
3.2 机械手运动方程的求解
3.2.3 球面变换解
右列相等,（ 3-53）
),0,0(),(),( 1 rT r a n syR o tTzR o t ?? ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
0010
0
10001000
0100
00
00
???
???
??
??
rccs
rssc
paon
paon
paon
cs
sc
zzzz
yyyy
xxxx
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
1
0
1
?
?
??
??
rc
rs
p
pcps
pspc
z
yx
yx
Robotics运动学
3.2 机械手运动方程的求解
3.2.3 球面变换解
由第二行有：
（ 3-54）
（ 3-56）
用 的右列相等，
可得：
（ 3-57）
)t a n (0
x
yyx ppapcps ???? ???
)t a n(
z
yx
zyx p
pspcarcprspspc ??????? ?????
),0,0(),(),( 11 rT r a n sTzR o tyR o t ??? ??
zyx pcpspcsr ???? ??? )(
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
已知转角，求各杆位姿
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
1。确定 D-H坐标系
全为转动关节,
Zi坐标轴,沿着 i+1关节的运动轴 ;
Xi坐标轴,沿着 Zi和 Zi-1的公法线,指向离开 Zi-1轴的方向 ;
Yi坐标轴,按右手直角坐标系法则制定 ;
连杆长度 ai; Zi和 Zi-1两轴心线的公法线长度 ;
连杆扭角 α i,Zi和 Zi-1两轴心线的夹角 ;
两连杆距离 di:相邻两杆三轴心线的两条公法线间的距离 ;
两杆夹角 θ i,Xi和 Xi-1两坐标轴的夹角 ;
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
2。确定各连杆 D-H参数和关节变量
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
3。求出两杆间的位姿矩阵
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
???
1000
0
),()0,0,(),0,0(),(
11
111
111
iii
iiiiiii
iiiiiii
iiiii
dcs
sascccs
casscsc
xR o taT r a n sdT r a n szR o tA
??
??????
??????
??
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
4。求末杆的位姿矩阵
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
（ 3-64）
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
5。验证
mmdmmdmmammdmma
oo
25.56,07.43 3,32.20,09.14 9,8.43 1
00,0,90,0,90
64322
654321
??????
?????? ??????
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
已知，
求：各转角
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
6
zzzz
yyyy
xxxx
paon
paon
paon
T
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
由于 交于一点 W,点 W在基础坐标系中的位置仅
与 有关。据此，可先解出,再分离出
，并逐一求解。
1.求 θ 1
654,,zzz
321,,??? 321,,???
654,,???
6
111
11
6
1
6
5
3
2
2
1
61
1
1
10001000
0100
00
00
)(
T
paon
paon
paon
cs
sc
TTTTTT
zzzz
yyyy
xxxx
oo
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
??
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
有两个可能的解。
其他角度可以类似方法求得。
211 dpcps yx ???
)t a n()t a n( 2
2
22
2
1 dpp
da
p
pa
yxx
y
???
???
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
解的多重性
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
在基系中的描述：
在坐标系 {T}中描述：
TdTIdfR o tdddT r a n s
TIdfR o tdddT r a n sdT
TdfR o tdddT r a n sdTT
zyx
zyx
zyx
?????
??
??
,),(),,(
]),(),,([
),(),,(
?
?
?
?????
??
??
T
zyx
T
zyx
zyx
TdTIdfR o tdddT r a ns
IdfR o tdddT r a nsdT
dfR o tdddT r a nsdTT
,),(),,(
]),(),,([
),(),,(
?
?
?
T
T
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
微分平移变换：
微分旋转变换：
因为,
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
100
010
001
),,(
z
y
x
zyx
d
d
d
dddT r a n s
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
1000
0
0
0
),(
??????
??????
??????
?
cv er sffsfv er sffsfv er sff
sfv er sffcv er sffsfv er sff
sfv er sffsfv er sffcv er sff
fR o t
zzxzyyzx
xyzyyzyx
yxzzxyxx
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
有：
0,1c o s,s i n limlimlim
000
???
???
????
???
v e r sd
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
01
01
01
),(
??
??
??
?
dfdf
dfdf
dfdf
dfR o t
xy
xz
yz
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
所以有
（ 3-87） ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
0000
0
0
0
1000
0100
0010
0001
1000
01
01
01
1000
100
010
001
),(),,(
zxy
yxz
xyz
xy
xz
yz
z
y
x
zyx
ddfdf
ddfdf
ddfdf
dfdf
dfdf
dfdf
d
d
d
IdfR o tdddT r a n s
??
??
??
??
??
??
?
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
因为：
(3-88) (3-89)
微分平移和旋转矢量：
zzyyxx dfdfdf ?????? ???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0000
0
0
0
,
0000
0
0
0
z
T
x
T
y
T
y
T
x
T
z
T
x
T
y
T
z
T
T
zxy
yxz
xyz
d
d
d
d
d
d
??
??
??
??
??
??
kjikdjdid zyxzyx ???? ??????d
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
记：
（ 3-90） （ 3-91）
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
z
T
y
T
x
T
z
T
y
T
x
T
T
T
T
z
y
x
z
y
x
d
d
d
d
d
d
?
?
??
?
?
??
d
D
d
D,
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
例 3.1:已知坐标系 {A}和其对基系的微分平移和旋转,
求微分变换 dA.
解,(3-88)
kji
kji
A
01.00
5.001
,
1000
0010
5001
10100
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
d
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0000
5.01.000
0000
101.00
1000
0010
5001
10100
0000
5.0001.0
0000
11.000
0000
5.0001.0
0000
11.000
AdA
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
坐标系 {A}的微分变化
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
目的：把一个坐标系内的位姿变换到另一坐标系内
由 有,????
TTdTTdT
TTT ??? ?1
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
1000
10000000
0
0
0
1
ap
op
np
zyx
zyx
zyx
zzzz
yyyy
xxxx
zxy
yxz
xyz
aaa
ooo
nnn
T
paon
paon
paon
d
d
d
T
??
??
??
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
与（ 3-89）元素对应相等，有
)943(
0000
)(0
)(0
)(0
1 ?
?
?
?
?
?
?
?
?
?
?
?
?
???????
???????
???????
???? ?
adapno
odopna
ndnpoa
TTT
???
???
???
)963(,,
)(
)953()(
)(
???????
?????
??????
?????
aon
adapd
odopd
ndnpd
z
T
y
T
x
T
z
T
y
T
x
T
??????
?
?
?
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
zyx
zyx
zyx
zyxzyx
zyxzyx
zyxzyx
z
T
y
T
x
T
z
T
y
T
x
T
d
d
d
aaa
ooo
nnn
apapapaaa
opopopooo
npnpnpnnn
d
d
d
?
?
?
?
?
?
000
000
000
)()()(
)()()(
)()()(
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
??
?
??
?
?
?
?
?
?
? ??
?
?
?
?
?
?
??
d
R
pSRRd
T
TT
T
T
0
)(
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0
0
0
)(,
xy
xz
yz
zzz
yyy
xxx
pp
pp
pp
pS
aon
aon
aon
R
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
例 3-2：在例 1中，求坐标系 {A}的等价微分平移和旋转
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
由上图，有
)1043()( 1111111 ????????? ??????? ABBAABABABAB BBB
)1053()()( 1111 ????????? ???? BBBBABBA BBABA
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
一摄像机，装在机械手的连杆 5上。这一连接及机械手的最后一
个连杆所处当前位置，分别由下式确定：
被观察的目标物体为 CAMO。要把机械手的末端引向目标物体，需
要知道的坐标系 {CAM}内的微分变化为：
求在坐标系 {T6}内所需要的微分变化。
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
8100
0001
0010
1000
10001
0010
5100
6AC A M
T
kjikjid C A MC A M 1.000,001 ??????? ?
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
例 3。 3：
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
Robotics运动学
3.4 机器人的雅可比公式
3.4.2 机器人的雅可比矩阵
1。定义
机械手的操作速度与关节速度间的线性变换定义
为机械手的 雅可比矩阵 。
)1 0 73()(
)1 0 63()(
??
??
qqJx
qxx
??
njiq qxqJ
j
i
ij,,2,1,6,,2,1,
)()( ?? ??
?
??
Robotics运动学
3.4 机器人的雅可比公式
3.4.2 机器人的雅可比矩阵
)1113()(
)1103()(limlim
)1093(
1
lim
00
0
??
??????
?
?
?
?
?
?
??
?
?
?
?
?
?
??
?
?
?
?
?
?
????
??
dqqJD
tqqJtx
d
D
d
tw
v
x
tt
t
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
n
wzwzwz
wywywy
wxwxwx
v z nvzvz
vynvyvy
vxnvxvx
z
y
x
z
y
x
q
q
q
JJJ
JJJ
JJJ
JJJ
JJJ
JJJ
w
w
w
v
v
v
?
?
?
?
?
?
?
?
?
?
2
1
321
321
321
21
21
21
Robotics运动学
3.4 机器人的雅可比公式
3.4.2 机器人的雅可比矩阵
2。雅可比矩阵的求法
（ 1）矢量积法 对移动关节
对转动关节
)1143(0,0 ??
?
?
??
??
??
?
??
??
??
?
??
? i
ii
i zJqz
w
v ?
)1153(,,??
?
?
??
? ??
??
?
??
? ??
??
?
??
?
i
o
n
i
i
ii
i
o
n
i
i
z
pzJq
z
pz
w
v ?
nioioni pRp ?
Kinematics of Robotics
3.1 机器人运动方程的表示
(姿态和方向角 \位置和坐标 \连杆变换矩阵 )
3.2 机械手运动方程的求解
(欧拉变换解 /滚仰偏变换解 /球面变换解 )
3.3 PUMA560机器人运动方程
(运动分析 /运动综合 )
3.4 机器人的雅可比公式
(微分运动 /雅可比矩阵 /计算实例 )
Robotics 运动学
3.1 机器人运动方程的表示
3.1.0 A矩阵和 T矩阵
机械手可以看成由一系列关节连接起来的连杆组构成,
用 A矩阵 描述连杆坐标系间相对平移和旋转的齐次变换,
A1表示第一连杆对基坐标的位姿
A2表示第二连杆对第一连杆位姿
则第二连杆对基坐标的位姿为
212 AAT ?
6543216 AAAAAAT ?
Robotics运动学
3.1 机器人运动方程的表示
3.1.1 运动姿态和方向角
1.运动方向
接近矢量 a:夹持器进入物体的方向 ;Z轴
方向矢量 o:指尖互相指向 ;Y轴
法线矢量 n:指尖互相指向 ;X轴
111 ??????
??
aoaaoo
aon
?
?
?
?
?
?
?
?
?
?
?
?
??
1000
6
zzzz
yyyy
xxxx
paon
paon
paon
TT
Robotics运动学
3.1 机器人运动方程的表示
3.1.1 运动姿态和方向角
2.用旋转系列表示运动姿态
欧拉角,绕 Z轴转 φ,再绕 新 Y轴转 θ,绕 最新 Z轴转 ψ,
(3-3)
注意,坐标变换是右乘,即后面的变
换乘在右边,(绕新轴转,连乘 )
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
0100
00
00
1000
00
0010
00
1000
0100
00
00
),(),(),(),,(
??
??
??
??
??
??
??????
cs
sc
cs
sc
cs
sc
zR o tyR o tzR o tE u l e r
Robotics运动学
3.1 机器人运动方程的表示
3.1.1 运动姿态和方向角
3.用滚 \仰 \偏转表示运动姿态
横滚,绕 Z轴转 φ,
俯仰,绕 Y轴转 θ,
偏转,绕 X轴转 ψ,
(3-5)
注意,左乘,
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
00
00
0001
1000
00
0010
00
1000
0100
00
00
),(),(),(),,(
??
??
??
??
??
??
??????
cs
sc
cs
sc
cs
sc
xR o tyR o tzR o tRPY
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
1.用 柱面 坐标表示 末端 运动位置
由于上述绕 Z轴的旋转,使末端执行器的姿态出现变化,
若要执行器姿态不变,则需将其绕执行器 Z轴反向旋转,
(3-8) ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1000
100
010
001
1000
0100
00)()(
00)()(
1000
100
0
0
),,(
z
rs
rc
cs
sc
z
rscs
rcsc
rzC y l
?
?
??
??
???
???
?
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
2.用 球面 坐标表示 末端 运动位置
沿 Z平移 r,绕 Y轴转 β,绕 Z轴转 α,
(3-10) ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
1000
0
1000
100
0010
0001
1000
00
0010
00
1000
0100
00
00
),0,0(),(),(),,(
???
???????
???????
??
??
??
??
????
rccs
srsssccs
srcccscc
rcs
sc
cs
sc
rT r a n syR o tzR o trS p h
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
表示物体的位置,笛卡尔坐标、柱面坐标、球面坐标
1.用 柱面 坐标表示 末端 运动位置
沿 X平移 r,绕 Z轴转 α,沿 Z轴平移 z.
(绕原坐标系运动,左乘 )
(3-7) ?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
100
0
0
1000
0100
0010
001
1000
0100
00
00
1000
100
0010
0001
)0,0,(),(),0,0(),,(
z
rscs
rcsc
r
cs
sc
z
rT r a n szR o tzT r a n srzC yl
???
???
??
??
??
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
2.用 球面 坐标表示 末端 运动位置
沿 Z平移 r,绕 Y轴转 β,绕 Z轴转 α,
(3-10) ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
1000
0
1000
100
0010
0001
1000
00
0010
00
1000
0100
00
00
),0,0(),(),(),,(
???
???????
???????
??
??
??
??
????
rccs
srsssccs
srcccscc
rcs
sc
cs
sc
rT r a n syR o tzR o trS p h
Robotics运动学
3.1 机器人运动方程的表示
3.1.2 运动位置和坐标
2.用 球面 坐标表示 末端 运动位置
由于上述两个旋转,使执行器姿态发生变化,为保持姿
态,执行器要绕其自身 Y和 Z轴反向旋转,
(3-11)
?
?
?
?
?
?
?
?
?
?
?
?
?
???
1000
100
010
001
),(),(),0,0(),(),(),,(
?
??
??
??????
rc
srs
src
zR o tyR o trT r a n syR o tzR o trS p h
AA
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆 (D-H坐标 )
全为转动关节,
Zi坐标轴 ;
Xi坐标轴 ;
Yi坐标轴 ;
连杆长度 ai;
连杆扭角 α i;
两连杆距离 di;
两杆夹角 θ i
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆
全为转动关节,
Zi坐标轴,沿着 i+1关节的运动轴 ;
Xi坐标轴,沿着 Zi和 Zi-1的公法线,指向离开 Zi-1轴的方向 ;
Yi坐标轴,按右手直角坐标系法则制定 ;
连杆长度 ai; Zi和 Zi-1两轴心线的公法线长度 ;
连杆扭角 α i,Zi和 Zi-1两轴心线的夹角 ;
两连杆距离 di:相邻两杆三轴心线的两条公法线间的距离 ;
两杆夹角 θ i,Xi和 Xi-1两坐标轴的夹角 ;
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆 (D-H坐标 )
含移动关节,
Zi坐标轴 ;
Xi坐标轴 ;
Yi坐标轴 ;
连杆长度 ai=0;
连杆扭角 α i;
两连杆距离 di;
两杆夹角 θ i
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
1.广义连杆
含移动关节,
Zi坐标轴,沿着 i+1关节的运动轴 ;
Xi坐标轴,沿着 Zi和 Zi-1的公法线,指向离开 Zi-1轴的方向 ;
Yi坐标轴,按右手直角坐标系法则制定 ;
连杆长度 ai; Zi和 Zi-1两轴心线的公法线长度 ;
连杆扭角 α i,Zi和 Zi-1两轴心线的夹角 ;
两连杆距离 di:相邻两杆三轴心线的两条公法线间的距离 ;
两杆夹角 θ i,Xi和 Xi-1两坐标轴的夹角 ;
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
2.广义变换矩阵
建立 D-H坐标系后,可通过两个旋转、两个平移建立相邻
连杆 i-1和 i间的相对关系。
1。绕 Zi-1轴转 θ i角，使 Xi-1转到与 Xi同一平面内；
2。沿 Zi-1轴平移 di，把 Xi-1移到与 Xi同一直线上；
3。沿 i轴平移 ai-1,把连杆 i-1的坐标系移到使其原点与
连杆 i的坐标系原点重合的位置；
4。绕 Xi-1轴转 α i-1角，使 Zi-1转到与 Zi同一直线上；
这四个齐次变换叫 Ai矩阵：
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
2.广义变换矩阵
对旋转关节,
(3-13)
对棱柱关节,
(3-14)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
???
1000
0
),()0,0,(),0,0(),(
11
111
111
iii
iiiiiii
iiiiiii
iiiii
dcs
sascccs
casscsc
xR o taT r a n sdT r a n szR o tA
??
??????
??????
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
1000
0
1
0
),()0,0,(),0,0(),(
11
11
11
iii
iiiii
iiiii
iiiii
dcs
scccs
sscsc
xR o taT r a n sdT r a n szR o tA
??
?????
?????
??
Robotics运动学
3.1 机器人运动方程的表示
3.1.3 连杆变换矩阵
3.用 A矩阵表示 T矩阵
T6:机械手末端对其基座
Z:机械手基座对参考坐标系
E:端部工具对机械手末端
X:端部工具对参考坐标系
6161 AAAT iii ??? ?
11
6
6
???
?
XEZT
EZTX
Robotics运动学
3.2 机械手运动方程的求解
1)问题,已知手部位姿,求各关节位置
2)意义,是机械手控制的关键
3)没有一种算法可以通用,需要几何设置引导
本节介绍上节的几种特殊变换下的求解算法,
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
1.基本隐式方程的解
若上式中 T矩阵的各元素已知，即
（ 3-24）
对应项相等，有
),(),(),(),,( ?????? zR o tyR o tzR o tE u l e rT ??
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
0
0
1111
?????
????????????
????????????
csscs
ssccscsssccs
sccssccssccc
paon
paon
paon
zzzz
yyyy
xxxx
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
arccos:符号不定；
特殊点不准确；
0或 180时，后
（ 3-25/33） 两式没定义。
?
??
??
??
?????
?????
??
?????
?????
ca
ssa
sca
sso
ccscso
csscco
csn
scccsn
sscccn
z
y
x
z
y
x
z
y
x
?
?
?
?
???
???
??
??
??
)/a r c c o s (
)/a r c c o s (
)a r c c o s (
??
??
?
sn
sa
a
z
x
z
??
?
?
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
2.用显式方程求各角度
(3-37)
(3-39)
),(),(),(
),(),(),(
11
1
???
???
zR o tTzR o tyR o t
zR o tyR o tTzR o t
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
00
0
11111000
0100
00
00
?????
??
?????
??
??
csscs
cs
ssccc
paon
paon
paon
cs
sc
zzzz
yyyy
xxxx
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
其中 (3-40)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
00
0
1000
)()()()(
)()()()(
)()()()(
13131313
12121212
11111111
?????
??
?????
csscs
cs
ssccc
pfafofnf
pfafofnf
pfafofnf
paonu
uuf
ucusuf
usucuf
z
yx
yx
,,,
)(
)(
)(
13
12
11
?
?
???
??
??
??
Robotics运动学
3.2 机械手运动方程的求解
3.2.1欧拉变换解
由 有, 即
(3-42/43)
这样,由,得
(3-44)
再由,得
(3-45)
0)(12 ?af 0??? yx acas ??
)/t a n ()/t a n ( xyxy aaaaaa ???? ??
)()( 1311 afcafs ?? ??
)t a n(
z
yx
a
asaca ??? ??
)()( 1212 ofcnfs ?? ??
)t a n(
yx
yx
ocos
ncnsa
??
???
??
???
Robotics运动学
3.2 机械手运动方程的求解
3.2.2 滚、仰、偏变换解
由
（ 3-47）
f定义同前。
),(),(),( 1 ??? xR o tyR o tTzR o t ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
00
0
1000
)()()()(
)()()()(
)()()()(
13131313
12121212
11111111
?????
??
?????
ccscs
sc
csssc
pfafofnf
pfafofnf
pfafofnf
Robotics运动学
3.2 机械手运动方程的求解
3.2.2 滚、仰、偏变换解
由 得
（ 3-48）
这样，由
可得：
（ 3-50）
再由
得
（ 3-51）
0)(12 ???? yx ncnsnf ??
)t a n (
x
y
n
na??
???? snnfcnsncnf zyx ?????? )()( 1311
)t an (
yx
z
nsnc
na
??? ?
??
?????? sacasafcocosof yxyx ????????? )()( 1212
)t a n(
yx
yx
ocos
acasa
??
???
??
??
Robotics运动学
3.2 机械手运动方程的求解
3.2.3 球面变换解
右列相等,（ 3-53）
),0,0(),(),( 1 rT r a n syR o tTzR o t ?? ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
0010
0
10001000
0100
00
00
???
???
??
??
rccs
rssc
paon
paon
paon
cs
sc
zzzz
yyyy
xxxx
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
1
0
1
?
?
??
??
rc
rs
p
pcps
pspc
z
yx
yx
Robotics运动学
3.2 机械手运动方程的求解
3.2.3 球面变换解
由第二行有：
（ 3-54）
（ 3-56）
用 的右列相等，
可得：
（ 3-57）
)t a n (0
x
yyx ppapcps ???? ???
)t a n(
z
yx
zyx p
pspcarcprspspc ??????? ?????
),0,0(),(),( 11 rT r a n sTzR o tyR o t ??? ??
zyx pcpspcsr ???? ??? )(
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
已知转角，求各杆位姿
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
1。确定 D-H坐标系
全为转动关节,
Zi坐标轴,沿着 i+1关节的运动轴 ;
Xi坐标轴,沿着 Zi和 Zi-1的公法线,指向离开 Zi-1轴的方向 ;
Yi坐标轴,按右手直角坐标系法则制定 ;
连杆长度 ai; Zi和 Zi-1两轴心线的公法线长度 ;
连杆扭角 α i,Zi和 Zi-1两轴心线的夹角 ;
两连杆距离 di:相邻两杆三轴心线的两条公法线间的距离 ;
两杆夹角 θ i,Xi和 Xi-1两坐标轴的夹角 ;
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
2。确定各连杆 D-H参数和关节变量
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
3。求出两杆间的位姿矩阵
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
???
1000
0
),()0,0,(),0,0(),(
11
111
111
iii
iiiiiii
iiiiiii
iiiii
dcs
sascccs
casscsc
xR o taT r a n sdT r a n szR o tA
??
??????
??????
??
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
4。求末杆的位姿矩阵
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
（ 3-64）
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.1 运动分析
5。验证
mmdmmdmmammdmma
oo
25.56,07.43 3,32.20,09.14 9,8.43 1
00,0,90,0,90
64322
654321
??????
?????? ??????
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
已知，
求：各转角
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
0
6
zzzz
yyyy
xxxx
paon
paon
paon
T
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
由于 交于一点 W,点 W在基础坐标系中的位置仅
与 有关。据此，可先解出,再分离出
，并逐一求解。
1.求 θ 1
654,,zzz
321,,??? 321,,???
654,,???
6
111
11
6
1
6
5
3
2
2
1
61
1
1
10001000
0100
00
00
)(
T
paon
paon
paon
cs
sc
TTTTTT
zzzz
yyyy
xxxx
oo
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
??
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
有两个可能的解。
其他角度可以类似方法求得。
211 dpcps yx ???
)t a n()t a n( 2
2
22
2
1 dpp
da
p
pa
yxx
y
???
???
Robotics运动学
3.3 PUMA600机器人运动方程
3.3.2 运动综合
解的多重性
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
在基系中的描述：
在坐标系 {T}中描述：
TdTIdfR o tdddT r a n s
TIdfR o tdddT r a n sdT
TdfR o tdddT r a n sdTT
zyx
zyx
zyx
?????
??
??
,),(),,(
]),(),,([
),(),,(
?
?
?
?????
??
??
T
zyx
T
zyx
zyx
TdTIdfR o tdddT r a ns
IdfR o tdddT r a nsdT
dfR o tdddT r a nsdTT
,),(),,(
]),(),,([
),(),,(
?
?
?
T
T
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
微分平移变换：
微分旋转变换：
因为,
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
100
010
001
),,(
z
y
x
zyx
d
d
d
dddT r a n s
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
1000
0
0
0
),(
??????
??????
??????
?
cv er sffsfv er sffsfv er sff
sfv er sffcv er sffsfv er sff
sfv er sffsfv er sffcv er sff
fR o t
zzxzyyzx
xyzyyzyx
yxzzxyxx
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
有：
0,1c o s,s i n limlimlim
000
???
???
????
???
v e r sd
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
01
01
01
),(
??
??
??
?
dfdf
dfdf
dfdf
dfR o t
xy
xz
yz
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
所以有
（ 3-87） ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
0000
0
0
0
1000
0100
0010
0001
1000
01
01
01
1000
100
010
001
),(),,(
zxy
yxz
xyz
xy
xz
yz
z
y
x
zyx
ddfdf
ddfdf
ddfdf
dfdf
dfdf
dfdf
d
d
d
IdfR o tdddT r a n s
??
??
??
??
??
??
?
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
因为：
(3-88) (3-89)
微分平移和旋转矢量：
zzyyxx dfdfdf ?????? ???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0000
0
0
0
,
0000
0
0
0
z
T
x
T
y
T
y
T
x
T
z
T
x
T
y
T
z
T
T
zxy
yxz
xyz
d
d
d
d
d
d
??
??
??
??
??
??
kjikdjdid zyxzyx ???? ??????d
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
记：
（ 3-90） （ 3-91）
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
z
T
y
T
x
T
z
T
y
T
x
T
T
T
T
z
y
x
z
y
x
d
d
d
d
d
d
?
?
??
?
?
??
d
D
d
D,
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
例 3.1:已知坐标系 {A}和其对基系的微分平移和旋转,
求微分变换 dA.
解,(3-88)
kji
kji
A
01.00
5.001
,
1000
0010
5001
10100
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
d
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0000
5.01.000
0000
101.00
1000
0010
5001
10100
0000
5.0001.0
0000
11.000
0000
5.0001.0
0000
11.000
AdA
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
1。 微分平移和旋转
坐标系 {A}的微分变化
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
目的：把一个坐标系内的位姿变换到另一坐标系内
由 有,????
TTdTTdT
TTT ??? ?1
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
1000
10000000
0
0
0
1
ap
op
np
zyx
zyx
zyx
zzzz
yyyy
xxxx
zxy
yxz
xyz
aaa
ooo
nnn
T
paon
paon
paon
d
d
d
T
??
??
??
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
与（ 3-89）元素对应相等，有
)943(
0000
)(0
)(0
)(0
1 ?
?
?
?
?
?
?
?
?
?
?
?
?
???????
???????
???????
???? ?
adapno
odopna
ndnpoa
TTT
???
???
???
)963(,,
)(
)953()(
)(
???????
?????
??????
?????
aon
adapd
odopd
ndnpd
z
T
y
T
x
T
z
T
y
T
x
T
??????
?
?
?
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
???
???
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
z
y
x
zyx
zyx
zyx
zyxzyx
zyxzyx
zyxzyx
z
T
y
T
x
T
z
T
y
T
x
T
d
d
d
aaa
ooo
nnn
apapapaaa
opopopooo
npnpnpnnn
d
d
d
?
?
?
?
?
?
000
000
000
)()()(
)()()(
)()()(
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
??
?
??
?
?
?
?
?
?
? ??
?
?
?
?
?
?
??
d
R
pSRRd
T
TT
T
T
0
)(
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0
0
0
)(,
xy
xz
yz
zzz
yyy
xxx
pp
pp
pp
pS
aon
aon
aon
R
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
2。 微分运动的等价变换
例 3-2：在例 1中，求坐标系 {A}的等价微分平移和旋转
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
由上图，有
)1043()( 1111111 ????????? ??????? ABBAABABABAB BBB
)1053()()( 1111 ????????? ???? BBBBABBA BBABA
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
一摄像机，装在机械手的连杆 5上。这一连接及机械手的最后一
个连杆所处当前位置，分别由下式确定：
被观察的目标物体为 CAMO。要把机械手的末端引向目标物体，需
要知道的坐标系 {CAM}内的微分变化为：
求在坐标系 {T6}内所需要的微分变化。
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
1000
8100
0001
0010
1000
10001
0010
5100
6AC A M
T
kjikjid C A MC A M 1.000,001 ??????? ?
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
例 3。 3：
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
Robotics运动学
3.4 机器人的雅可比公式
3.4.1 机器人的微分运动
3。变换式中的微分关系
Robotics运动学
3.4 机器人的雅可比公式
3.4.2 机器人的雅可比矩阵
1。定义
机械手的操作速度与关节速度间的线性变换定义
为机械手的 雅可比矩阵 。
)1 0 73()(
)1 0 63()(
??
??
qqJx
qxx
??
njiq qxqJ
j
i
ij,,2,1,6,,2,1,
)()( ?? ??
?
??
Robotics运动学
3.4 机器人的雅可比公式
3.4.2 机器人的雅可比矩阵
)1113()(
)1103()(limlim
)1093(
1
lim
00
0
??
??????
?
?
?
?
?
?
??
?
?
?
?
?
?
??
?
?
?
?
?
?
????
??
dqqJD
tqqJtx
d
D
d
tw
v
x
tt
t
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
n
wzwzwz
wywywy
wxwxwx
v z nvzvz
vynvyvy
vxnvxvx
z
y
x
z
y
x
q
q
q
JJJ
JJJ
JJJ
JJJ
JJJ
JJJ
w
w
w
v
v
v
?
?
?
?
?
?
?
?
?
?
2
1
321
321
321
21
21
21
Robotics运动学
3.4 机器人的雅可比公式
3.4.2 机器人的雅可比矩阵
2。雅可比矩阵的求法
（ 1）矢量积法 对移动关节
对转动关节
)1143(0,0 ??
?
?
??
??
??
?
??
??
??
?
??
? i
ii
i zJqz
w
v ?
)1153(,,??
?
?
??
? ??
??
?
??
? ??
??
?
??
?
i
o
n
i
i
ii
i
o
n
i
i
z
pzJq
z
pz
w
v ?
nioioni pRp ?
