机器人运动学
2005年 3月 24日运动学正问题
杆件参数的意义
坐标系的建立原则
杆件坐标系间的变换过程 -相邻关节坐标系的齐次变换
机器人的运动学方程杆件参数的意义 - 和
li 关节 Ai轴和 Ai+1轴线公法线的长度
关节 i轴线与 i+1
轴线在垂直于 li平面内的夹角串联关节,每个杆件最多与 2个杆件相连,如 Ai与 Ai-1和
Ai+1相连 。 由运动学的观点来看,杆件的作用仅在于它能保持其两端关节间的形态不变 。 这种形态由两个参数决定,一是杆件的长度 li（ ）,一个是杆件的扭转角
i?ia
Ai
Ai+
1
i?
il
i?
il i
杆件参数的意义 - 和
是从第 i-1坐标系的原点到 Zi－１ 轴和
Ｘ i轴的交点沿 Ｚ i-1
轴测量的距离
绕 Zi-1轴由 Ｘ i-1
轴转向 Ｘ i轴的关节角确定杆件相对位置关系,由另外 2个参数决定,一个是杆件的距离：,一个是杆件的回转角：
i?i
d
i?
id i?
Ai
Ai+
1
i?
il
id
1?il i?
Ai-1
id
坐标系的建立原则
Ai
Ai+
1
i?
il
id
1?il
i?
Ai-1
1?iz
1?ix
1?iy
1?io
iz
ix
iy
io
为右手坐标系
原点 Oi：设在 Li与
Ai+1轴线的交点上
Zi轴：与 Ai+1关节轴重合，指向任意
Xi轴：与公法线 Li
重合，指向沿 Li由
Ai轴线指向 Ai+1轴线
Yi轴：按右手定则
Li— 沿 xi 轴,zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴,由 zi-1转向 zi
di— 沿 zi-1 轴,zi-1轴和 xi 交点至 ∑0i–1坐标系原点的距离
θi— 绕 zi-1 轴,由 xi-1转向 xi
杆件坐标系间的变换过程
-相邻关节坐标系的齐次变换
将 xi-1轴绕 zi-1轴转?i 角度，将其与 xi轴平行；
沿 zi-1轴平移距离 di,使 zi-1轴与 zi轴重合；
沿 xi轴平移距离 Li，使两坐标系原点及 x轴重合；
绕 xi 轴转?i角度，两坐标系完全重合．
机器人的运动学方程
1
s i n
c o s
0
c o s
s i nc o s
s i ns i n
0
s i n
c o sc o s
c o ss i n
0
0
s i n
c o s
1
i
ii
ii
i
ii
ii
i
ii
ii
i
i
i
i
d
a
a
A
0 0 1 1
12
i
iiT A A A
D-H变换矩阵运动学逆问题
多解性，剔除多余解原则
根据关节运动空间合适的解
选择一个与前一采样时间最接近的解
根据避障要求得选择合适的解
逐级剔除多余解
可解性
所有具有转动和移动关节的系统，在一个单一串联中总共有 6个（或小于 6个）自由度时，是可解的，一般是数值解，它不是解析表达式，而是利用数值迭代原理求解，它的计算量要比解析解大
如若干个关节轴线相交和或多个关节轴线等于 0或 90°
的情况下，具有 6个自由度的机器人可得到解析解例题：
试求立方体中心在机座坐标系 ∑ 0中的位置
该手爪从上方把物体抓起，同时手爪的开合方向与物体的 Y轴同向，
那么，求手爪相对于 ∑ 0的姿态是什么？
在机器人工作台上加装一电视摄像机，摄像机可见到固联着 6DOF关节机器人的机座坐标系原点，它也可以见到被操作物体（立方体）的中心，如果在物体中心建一局部坐标系，则摄像机所见到的这个物体可由齐次变换矩阵 T1来表示，如果摄像机所见到的机座坐标系为矩阵 T2表示。
1000
101-00
2001-0
10-001
T
1000
91-00
10001
1010
T 21
xy z
解 1：
x
y z
z
机 y
机
z
物
y
物
x
物
o
O
机
O
物
T T 21 物机机摄物摄 求，，已知 TTT
TT 11-2 ）（有,物摄摄机物机 TTT
1000
91-00
10001
1010
1000
101-00
2001-0
10001
1000
1100
10001-
11010
∑O 物 根据 T 1 画出
∑O 机 根据 T 2 画出因此物体位于机座坐标系的（ 11,10,1） T
处，它的 X,Y,Z轴分别与机座坐标系的
-Y,X,Z轴平行。
解 2：
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物手爪机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
1000
向重合手爪开合方向与物体 ya,Ts ]001[有方向相反方向物体的从上向下抓，指出手爪 zab,Ta ]100[则有
Tkji
kji
asnc ]010[00
100
001,
1-00
001
010
因此：姿态矩阵为重合时与物体中心当手爪中心
1000
11-00
10001
11010
T
物机
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物手爪机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
1000
向重合手爪开合方向与物体 ya,Ts ]001[有方向相反方向物体的从上向下抓，指出手爪 zab,
Ta ]100[则有
O
s
n
a
y
z
x
特殊情况坐标系的建立原则
Oi— Ai与 Ai+1关节轴线的交点
Zi— Ai+1轴线
Xi— Zi和 Zi-1构成的面的法线
Yi— 右手定则
Ai
A
i + 1
oi
zi-1
zi
xi
yi
两个关节轴相交两个关节轴线平行
先建立
∑ 0i-1
然后建立
∑ 0i+1
最后建立
∑ 0i
A
i - 1 A
i
A
i + 1
A
i + 2
l
i - 1
o
i - 1
x
i - 1
y
i - 1
z
i - 1
A
B
C
D
o
i
（ x
i
）
（ y
i
）
z
i
x
i
y
i
o
i + 1
x
i + 1
y
i + 1
z
i + 1
d
i + 1
l
i + 1
d
i
举例,Stanford机器人
A1
A2A3
A4
A5
A6
d1
z1
x1y
1
O1
d2
z2
x2
y2
O2
z3
y3
x3
O3
y4
z4
x4
O4
z5 y5
x5
O5
345
45
,,0o o odd重 合
d3
z6
x6
y6
O6
d6
z0
y0 x0
O0
为右手坐标系
原点 Oi,Ai与 Ai+1
关节轴线的交点
Zi轴：与 Ai+1关节轴重合，指向任意
Xi轴,Zi和 Zi-1构成的面的法线
Yi轴：按右手定则
Li— 沿 xi 轴,zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴,由 zi-1转向 zi
di— 沿 zi-1 轴,zi-1轴和 xi 交点至 ∑0i–1坐标系原点的距离
θi— 绕 zi-1 轴,由 xi-1转向 xi
解：
用未知的逆变换逐次左乘，由乘得的矩阵方程的元素决定未知数，即用逆变换把一个未知数由矩阵方程的右边移到左边
求解这个未知数
把下一个未知数移到左边
重复上述过程，直到解出所有解运动学逆问题解法
Paul 等人提出的方法,
Paul 等人提出的方法
65544332211060 TTTTTTT?
TTTTTT T 6554433221601-10?）（ 1 q?
65544332601-101-21 TTTTTTT?）（）（ 2q?
65601-101-21132143154 TTTT)T()T()T( ）（）（5 q?
E?601-101-65 TT) T( ）（? 6 q?
1000
6
0
pzazsznz
pyaysyny
pxaxsxnx
T
机器人末端操作器位姿的其它描述方法
用矩阵表示刚性体的转动简化了许多运算，
但它需要 9个元素来完全描述旋转刚体的姿态，因此矩阵并不直接得出一组完备的广义坐标。
一组广义坐标应能描述转动刚体相对于参考坐标的方向，被称为欧拉角的三个角度,φ,
θ,ψ 就是这种广义坐标。
有几种不同的欧拉角表示方法，它们均可描述刚体相对于固定参考系的姿态。三种最常见的欧拉角类型列在表中
3种最常见的欧拉角类型步 1 步 2 步 3
类型 1 绕 OZ轴转 φ 角 绕当前 OU' 轴转 θ角 绕当前 OW″ 轴转 ψ 角类型 2 绕 OZ轴转 φ 角 绕当前 OV '轴转 θ角 绕当前 OW″ 轴转 ψ 角类型 3 绕 OX轴转 φ 角 绕 OY轴转 θ角 绕 OZ轴转 ψ 角
φ
φ
φ
u′
v′
w′
①
x(u)
y (v)
z (w)
o
θ
u"
v"
θ
w"
②
u
③ ψ
ψ
ψ
v
W
),(?ZR ),(R ),(?wRN0T
100
0
0
0
0
001
100
0
0
cs
sc
cs
sccs
sc
ccsss
sccccssscccs
ssccsscscscc
类型 1：表示法通常用于陀螺运动类型 2,所得的转动矩阵为右乘
100
0
0
c0s-
010
s0c
100
0
0
),(),v(),(R
cs
sc
cs
sc
wRRZR
1000
pz
pyR
px
T
ccsss
ssccscsscccs
sccssccssccc
类型 3,一般称此转动的欧拉角为横滚、俯仰和偏航角，这种形式主要用于航空工程中分析飞行器的运动，其旋转矩阵为（这种方法也叫做横滚、俯仰和偏航角表示方法）
ccscs
sccssccssscs
sscsccsssccc
cs
sc
cs
sc
cs
sc
xRyRz
0
0
001
0
010
0
100
0
0
),(),(),RR （
Z
Y
X
偏航俯仰横滚斯坦福机器人运动学逆问题解
6533211060 AAAAT
61T?
653321 AAA
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
211 dpcps yx由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA
式中第四列：
6362132 TTA
式中第三列：
微动矩阵和微动齐次变换
对象,微动矩阵主要是描述机器人在微动范围内各关节的位移运动关系
定义,各关节当角度移小于 5° 时，平移在 0.1mm左右时，微动矩阵大致可用设,有一机器人如图，末端执行器在机座坐标系中的齐次变换为 oTN,做微动,① 绕任意轴 w轴转 ； ② 绕各坐标轴平移 dx,dy,dz
求,在 中的位置和姿态,
定义 为微动齐次变换矩阵
N0? 00?
O 0
n
a
s
X 0
Y 0
Z 0
O n
NdT
Nr a n s TdwRdzdydxTT 0N0 ),(),,(变化后）（
Nr a n s TdwRdzdydxTdTT 0NN0 ),(),,(记为
TdwRdzdydxTdTT r an s ),(),,(
TEdwRdzdydxTdT r an s ]),(),,([
)T(
,),(),,(
的左边在注意，
称为微动率，令
TdT
EdwRdzdydxT r a n s?
在忽略高次项的情况下：微动齐次变换与次序无关
d
微动平移和微动旋转的齐次变换：
平移：
1000
100
010
001
dz
dy
dx
T r
旋转 R,绕任意轴 旋转 角,dw,wd
dd
dd
dd
dddd
dddd
dddd
dw
c os)c os(1r
s i nr)c os(1rr
s i nr)c os(1rr
s i nr)c os(1rrs i nr)c os(1rr
c os)c os(1rs i nr)c os(1rr
s i nr)c os(1rrc os)c os(1r
R
2
z
xzy
yzx
xzyyzx
2
yzyx
zyx
2
x
,
1000
01
01
01
s i n,1c o s
drdr
drdr
drdr
ddd
xy
xz
yz
在微动范围内绕经意轴转动 角,可以看作绕 x,y,z轴的微转动的合成 。 因此：
d
drzddryddrxd zyx,,
1000
01
01
01
,
xdyd
xdzd
ydzd
dwR
因此：
因此微动率△ = EdwRdzdydxT
r a n s,,,
0000
0
0
0
dzxdyd
dyxdzd
dxydzd
微动的齐次变换,dT= △?T
己知变换矩阵
1000
0001
3100
7010
T,001.0 kjid
kjidp 6.003.0
转动：
平移：求 d T
解：
0000
6.001.00
01.000
3.0000
1000
0100
0010
0001
1000
011.00
01.010
0001
1000
6.0100
0010
3.0001
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
1000
9.01.001
3101.0
3.7010
dTT
反过来：如果我们要求 Σ 在 Σ 中的齐次交换矩阵为
no oo
1000
0001
3100
7010
T 实际测得的为
1000
9.01.001
3101.0
3.7010
那么末端执行器坐标系要如何运动才能到达期望值？
1000
011.00
01.010
0001
转动：
1000
6.0100
0010
3.0001
平移：
等效微动位移的求解
前面研究的是动坐标系 ΣOn在 ΣOo中的 b
变换为 T，相对于基准坐标系作微平移和微转动，来求微动齐次交换。
现在我们研究动坐标系 Σ On相对于自身坐标系做了微位移或微转动，达到绕基准坐标同样的效果则如何求解。
dT=△?T （ 绕基准坐标系 ）
=T?△ T （ 绕动坐标系 ）
左乘,绕基准右乘,绕动坐标轴强调等效
TTT
TTT 1
1 TT
1000
zzzz
yyyy
xxxx
pasn
pasn
pasn
T
1000
1
Paaaa
Pssss
pnnnn
T
T
zyx
T
zyx
T
zyx
设,有：
0000
0
0
0
pdpdaasdand
pdpdssadsnd
pdpdnnadnsd
T
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
绕自身轴的微动率△ Τ 和绕固定坐标系坐标轴的微动率△之间的什么关系,举例说明：
例：一动坐标系相对于固定坐标系的齐次交换为
1000
0001
3100
7010
T
n s a p
己知相对固定坐标系的微动平移和转动
kjid
kjidp
001.0
6.003.0
求,① △ 与 △ Τ
② 求 dT
③ 求与之等效的绕动坐标系的微平移和微转动解,① △ =
0000
0
0
0
dzxdyd
dyxdzd
dxydyd
000
6.001.00
01.000
3.0000
0100001.0
1.0001001.0
0010001.0
kjikjind
kjikjisd
kjikjiad
kjikjikjipdpd 6.003.0037001.0
kji
kji
6.003.0
037
001.0
kji
kjikji
9.003.0
6.003.03.000
09.003.0010
3.09.003.0001
9.09.003.0100
kjikjipdpda
kjikjipdpds
kjikjipdpdn
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
T
0000
0001.0
3.0000
9.01.000
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
0000
9.01.000
0001.0
3.0000
0000
0001.0
3.0000
9.01.000
1000
0001
3100
7010
TTdT
解 ②,
解 ③,绕自身平移和转动
TTTT
TTTT
kjid
kjidp
01.00
03.09.0
其结果等于绕固定坐标系转动和旋转
kjid
kjidp
001.0
6.003.0
等效说明,如果我们发现末端操作器相对于基准坐标系有了微位移 （ 平移或转动 ）,我们可以认为末端操作器相对于自己的坐标系发生了微位移 。 只是微动率 △ 和 △ Τ不同而己 。 其结果是等效的 。
这些在进行误差补偿和微动时有用,如产生误差如何补偿？ 可以反向运动末端关节来补偿?
d
dp
微动齐次变换的意义误差及误差补偿
制造和检测误差
运算过程中圆整、插补、拟合造成的误差 — 原理性误差
构件承受的负载、加速度、重力的变形误差
传动误差
环境影响误差误差来源：
单关节补偿
多关节补偿误差补偿：
单关节补偿：
1116561111 iiiiii AATdAA
id?
11111656111211121 iiiiiiii AATTdAAA
6
6
6
5
6
5
6
5
6
5
6
5
6
5
6
5
2
2
2
1
2
1
2
1
2
1
2
1
2
1
2
1
1
1
1
0
1
0
1
0
1
0
1
0
1
0
1
0
AAAAAdA
AAAAAdA
AAAAAdA
6565212110106060 AdAAdAAdATdT
忽略高次项：
66651065322221106521111060 AAAAAAAAATd
绕自身
656521106521211065100060 AAAAAAAATd
绕 Σ i-1
多关节补偿：
并联机器人运动学
燕山大学 黄真
,并联机器人机构学理论及其控制,
11000
1
.,,0
.0,,,
.0),,(
)()()(
,
1000
T
00
2222
62,1
0
0
zib
yib
xib
pzazsznz
pyaysyny
pxaxsxnx
bi z
bi y
bi x
zibyibxib
bibi zbi ybi x
B i zB i yB i x
B i zbi zB i ybi yB i xbi xl
lll
pzazsznz
pyaysyny
pxaxsxnx
i
）是已知的中，（在中的位置在也就是）现在求（
中是已知的在易有：
求：
中的位置和姿态在即：已知
即为所求解
222
iziyixi llll
2005年 3月 24日运动学正问题
杆件参数的意义
坐标系的建立原则
杆件坐标系间的变换过程 -相邻关节坐标系的齐次变换
机器人的运动学方程杆件参数的意义 - 和
li 关节 Ai轴和 Ai+1轴线公法线的长度
关节 i轴线与 i+1
轴线在垂直于 li平面内的夹角串联关节,每个杆件最多与 2个杆件相连,如 Ai与 Ai-1和
Ai+1相连 。 由运动学的观点来看,杆件的作用仅在于它能保持其两端关节间的形态不变 。 这种形态由两个参数决定,一是杆件的长度 li（ ）,一个是杆件的扭转角
i?ia
Ai
Ai+
1
i?
il
i?
il i
杆件参数的意义 - 和
是从第 i-1坐标系的原点到 Zi－１ 轴和
Ｘ i轴的交点沿 Ｚ i-1
轴测量的距离
绕 Zi-1轴由 Ｘ i-1
轴转向 Ｘ i轴的关节角确定杆件相对位置关系,由另外 2个参数决定,一个是杆件的距离：,一个是杆件的回转角：
i?i
d
i?
id i?
Ai
Ai+
1
i?
il
id
1?il i?
Ai-1
id
坐标系的建立原则
Ai
Ai+
1
i?
il
id
1?il
i?
Ai-1
1?iz
1?ix
1?iy
1?io
iz
ix
iy
io
为右手坐标系
原点 Oi：设在 Li与
Ai+1轴线的交点上
Zi轴：与 Ai+1关节轴重合，指向任意
Xi轴：与公法线 Li
重合，指向沿 Li由
Ai轴线指向 Ai+1轴线
Yi轴：按右手定则
Li— 沿 xi 轴,zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴,由 zi-1转向 zi
di— 沿 zi-1 轴,zi-1轴和 xi 交点至 ∑0i–1坐标系原点的距离
θi— 绕 zi-1 轴,由 xi-1转向 xi
杆件坐标系间的变换过程
-相邻关节坐标系的齐次变换
将 xi-1轴绕 zi-1轴转?i 角度，将其与 xi轴平行；
沿 zi-1轴平移距离 di,使 zi-1轴与 zi轴重合；
沿 xi轴平移距离 Li，使两坐标系原点及 x轴重合；
绕 xi 轴转?i角度，两坐标系完全重合．
机器人的运动学方程
1
s i n
c o s
0
c o s
s i nc o s
s i ns i n
0
s i n
c o sc o s
c o ss i n
0
0
s i n
c o s
1
i
ii
ii
i
ii
ii
i
ii
ii
i
i
i
i
d
a
a
A
0 0 1 1
12
i
iiT A A A
D-H变换矩阵运动学逆问题
多解性，剔除多余解原则
根据关节运动空间合适的解
选择一个与前一采样时间最接近的解
根据避障要求得选择合适的解
逐级剔除多余解
可解性
所有具有转动和移动关节的系统，在一个单一串联中总共有 6个（或小于 6个）自由度时，是可解的，一般是数值解，它不是解析表达式，而是利用数值迭代原理求解，它的计算量要比解析解大
如若干个关节轴线相交和或多个关节轴线等于 0或 90°
的情况下，具有 6个自由度的机器人可得到解析解例题：
试求立方体中心在机座坐标系 ∑ 0中的位置
该手爪从上方把物体抓起，同时手爪的开合方向与物体的 Y轴同向，
那么，求手爪相对于 ∑ 0的姿态是什么？
在机器人工作台上加装一电视摄像机，摄像机可见到固联着 6DOF关节机器人的机座坐标系原点，它也可以见到被操作物体（立方体）的中心，如果在物体中心建一局部坐标系，则摄像机所见到的这个物体可由齐次变换矩阵 T1来表示，如果摄像机所见到的机座坐标系为矩阵 T2表示。
1000
101-00
2001-0
10-001
T
1000
91-00
10001
1010
T 21
xy z
解 1：
x
y z
z
机 y
机
z
物
y
物
x
物
o
O
机
O
物
T T 21 物机机摄物摄 求，，已知 TTT
TT 11-2 ）（有,物摄摄机物机 TTT
1000
91-00
10001
1010
1000
101-00
2001-0
10001
1000
1100
10001-
11010
∑O 物 根据 T 1 画出
∑O 机 根据 T 2 画出因此物体位于机座坐标系的（ 11,10,1） T
处，它的 X,Y,Z轴分别与机座坐标系的
-Y,X,Z轴平行。
解 2：
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物手爪机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
1000
向重合手爪开合方向与物体 ya,Ts ]001[有方向相反方向物体的从上向下抓，指出手爪 zab,Ta ]100[则有
Tkji
kji
asnc ]010[00
100
001,
1-00
001
010
因此：姿态矩阵为重合时与物体中心当手爪中心
1000
11-00
10001
11010
T
物机
x
y z
z 机 y
机
z 物
y 物
x 物
o
O 机
O 物手爪机实际要求 T
pzazsznz
pyaysyny
pxaxsxnx
1000
向重合手爪开合方向与物体 ya,Ts ]001[有方向相反方向物体的从上向下抓，指出手爪 zab,
Ta ]100[则有
O
s
n
a
y
z
x
特殊情况坐标系的建立原则
Oi— Ai与 Ai+1关节轴线的交点
Zi— Ai+1轴线
Xi— Zi和 Zi-1构成的面的法线
Yi— 右手定则
Ai
A
i + 1
oi
zi-1
zi
xi
yi
两个关节轴相交两个关节轴线平行
先建立
∑ 0i-1
然后建立
∑ 0i+1
最后建立
∑ 0i
A
i - 1 A
i
A
i + 1
A
i + 2
l
i - 1
o
i - 1
x
i - 1
y
i - 1
z
i - 1
A
B
C
D
o
i
（ x
i
）
（ y
i
）
z
i
x
i
y
i
o
i + 1
x
i + 1
y
i + 1
z
i + 1
d
i + 1
l
i + 1
d
i
举例,Stanford机器人
A1
A2A3
A4
A5
A6
d1
z1
x1y
1
O1
d2
z2
x2
y2
O2
z3
y3
x3
O3
y4
z4
x4
O4
z5 y5
x5
O5
345
45
,,0o o odd重 合
d3
z6
x6
y6
O6
d6
z0
y0 x0
O0
为右手坐标系
原点 Oi,Ai与 Ai+1
关节轴线的交点
Zi轴：与 Ai+1关节轴重合，指向任意
Xi轴,Zi和 Zi-1构成的面的法线
Yi轴：按右手定则
Li— 沿 xi 轴,zi-1 轴与 xi 轴交点到 0i 的距离
αi— 绕 xi 轴,由 zi-1转向 zi
di— 沿 zi-1 轴,zi-1轴和 xi 交点至 ∑0i–1坐标系原点的距离
θi— 绕 zi-1 轴,由 xi-1转向 xi
解：
用未知的逆变换逐次左乘，由乘得的矩阵方程的元素决定未知数，即用逆变换把一个未知数由矩阵方程的右边移到左边
求解这个未知数
把下一个未知数移到左边
重复上述过程，直到解出所有解运动学逆问题解法
Paul 等人提出的方法,
Paul 等人提出的方法
65544332211060 TTTTTTT?
TTTTTT T 6554433221601-10?）（ 1 q?
65544332601-101-21 TTTTTTT?）（）（ 2q?
65601-101-21132143154 TTTT)T()T()T( ）（）（5 q?
E?601-101-65 TT) T( ）（? 6 q?
1000
6
0
pzazsznz
pyaysyny
pxaxsxnx
T
机器人末端操作器位姿的其它描述方法
用矩阵表示刚性体的转动简化了许多运算，
但它需要 9个元素来完全描述旋转刚体的姿态，因此矩阵并不直接得出一组完备的广义坐标。
一组广义坐标应能描述转动刚体相对于参考坐标的方向，被称为欧拉角的三个角度,φ,
θ,ψ 就是这种广义坐标。
有几种不同的欧拉角表示方法，它们均可描述刚体相对于固定参考系的姿态。三种最常见的欧拉角类型列在表中
3种最常见的欧拉角类型步 1 步 2 步 3
类型 1 绕 OZ轴转 φ 角 绕当前 OU' 轴转 θ角 绕当前 OW″ 轴转 ψ 角类型 2 绕 OZ轴转 φ 角 绕当前 OV '轴转 θ角 绕当前 OW″ 轴转 ψ 角类型 3 绕 OX轴转 φ 角 绕 OY轴转 θ角 绕 OZ轴转 ψ 角
φ
φ
φ
u′
v′
w′
①
x(u)
y (v)
z (w)
o
θ
u"
v"
θ
w"
②
u
③ ψ
ψ
ψ
v
W
),(?ZR ),(R ),(?wRN0T
100
0
0
0
0
001
100
0
0
cs
sc
cs
sccs
sc
ccsss
sccccssscccs
ssccsscscscc
类型 1：表示法通常用于陀螺运动类型 2,所得的转动矩阵为右乘
100
0
0
c0s-
010
s0c
100
0
0
),(),v(),(R
cs
sc
cs
sc
wRRZR
1000
pz
pyR
px
T
ccsss
ssccscsscccs
sccssccssccc
类型 3,一般称此转动的欧拉角为横滚、俯仰和偏航角，这种形式主要用于航空工程中分析飞行器的运动，其旋转矩阵为（这种方法也叫做横滚、俯仰和偏航角表示方法）
ccscs
sccssccssscs
sscsccsssccc
cs
sc
cs
sc
cs
sc
xRyRz
0
0
001
0
010
0
100
0
0
),(),(),RR （
Z
Y
X
偏航俯仰横滚斯坦福机器人运动学逆问题解
6533211060 AAAAT
61T?
653321 AAA
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
211 dpcps yx由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA
式中第四列：
6362132 TTA
式中第三列：
微动矩阵和微动齐次变换
对象,微动矩阵主要是描述机器人在微动范围内各关节的位移运动关系
定义,各关节当角度移小于 5° 时，平移在 0.1mm左右时，微动矩阵大致可用设,有一机器人如图，末端执行器在机座坐标系中的齐次变换为 oTN,做微动,① 绕任意轴 w轴转 ； ② 绕各坐标轴平移 dx,dy,dz
求,在 中的位置和姿态,
定义 为微动齐次变换矩阵
N0? 00?
O 0
n
a
s
X 0
Y 0
Z 0
O n
NdT
Nr a n s TdwRdzdydxTT 0N0 ),(),,(变化后）（
Nr a n s TdwRdzdydxTdTT 0NN0 ),(),,(记为
TdwRdzdydxTdTT r an s ),(),,(
TEdwRdzdydxTdT r an s ]),(),,([
)T(
,),(),,(
的左边在注意，
称为微动率，令
TdT
EdwRdzdydxT r a n s?
在忽略高次项的情况下：微动齐次变换与次序无关
d
微动平移和微动旋转的齐次变换：
平移：
1000
100
010
001
dz
dy
dx
T r
旋转 R,绕任意轴 旋转 角,dw,wd
dd
dd
dd
dddd
dddd
dddd
dw
c os)c os(1r
s i nr)c os(1rr
s i nr)c os(1rr
s i nr)c os(1rrs i nr)c os(1rr
c os)c os(1rs i nr)c os(1rr
s i nr)c os(1rrc os)c os(1r
R
2
z
xzy
yzx
xzyyzx
2
yzyx
zyx
2
x
,
1000
01
01
01
s i n,1c o s
drdr
drdr
drdr
ddd
xy
xz
yz
在微动范围内绕经意轴转动 角,可以看作绕 x,y,z轴的微转动的合成 。 因此：
d
drzddryddrxd zyx,,
1000
01
01
01
,
xdyd
xdzd
ydzd
dwR
因此：
因此微动率△ = EdwRdzdydxT
r a n s,,,
0000
0
0
0
dzxdyd
dyxdzd
dxydzd
微动的齐次变换,dT= △?T
己知变换矩阵
1000
0001
3100
7010
T,001.0 kjid
kjidp 6.003.0
转动：
平移：求 d T
解：
0000
6.001.00
01.000
3.0000
1000
0100
0010
0001
1000
011.00
01.010
0001
1000
6.0100
0010
3.0001
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
1000
9.01.001
3101.0
3.7010
dTT
反过来：如果我们要求 Σ 在 Σ 中的齐次交换矩阵为
no oo
1000
0001
3100
7010
T 实际测得的为
1000
9.01.001
3101.0
3.7010
那么末端执行器坐标系要如何运动才能到达期望值？
1000
011.00
01.010
0001
转动：
1000
6.0100
0010
3.0001
平移：
等效微动位移的求解
前面研究的是动坐标系 ΣOn在 ΣOo中的 b
变换为 T，相对于基准坐标系作微平移和微转动，来求微动齐次交换。
现在我们研究动坐标系 Σ On相对于自身坐标系做了微位移或微转动，达到绕基准坐标同样的效果则如何求解。
dT=△?T （ 绕基准坐标系 ）
=T?△ T （ 绕动坐标系 ）
左乘,绕基准右乘,绕动坐标轴强调等效
TTT
TTT 1
1 TT
1000
zzzz
yyyy
xxxx
pasn
pasn
pasn
T
1000
1
Paaaa
Pssss
pnnnn
T
T
zyx
T
zyx
T
zyx
设,有：
0000
0
0
0
pdpdaasdand
pdpdssadsnd
pdpdnnadnsd
T
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
绕自身轴的微动率△ Τ 和绕固定坐标系坐标轴的微动率△之间的什么关系,举例说明：
例：一动坐标系相对于固定坐标系的齐次交换为
1000
0001
3100
7010
T
n s a p
己知相对固定坐标系的微动平移和转动
kjid
kjidp
001.0
6.003.0
求,① △ 与 △ Τ
② 求 dT
③ 求与之等效的绕动坐标系的微平移和微转动解,① △ =
0000
0
0
0
dzxdyd
dyxdzd
dxydyd
000
6.001.00
01.000
3.0000
0100001.0
1.0001001.0
0010001.0
kjikjind
kjikjisd
kjikjiad
kjikjikjipdpd 6.003.0037001.0
kji
kji
6.003.0
037
001.0
kji
kjikji
9.003.0
6.003.03.000
09.003.0010
3.09.003.0001
9.09.003.0100
kjikjipdpda
kjikjipdpds
kjikjipdpdn
0000
0
0
0
pdpdandsd
pdpdsndad
pdpdnsdad
T
0000
0001.0
3.0000
9.01.000
0000
9.01.000
0001.0
3.0000
1000
0001
3100
7010
0000
6.001.00
01.000
3.0000
TdT
0000
9.01.000
0001.0
3.0000
0000
0001.0
3.0000
9.01.000
1000
0001
3100
7010
TTdT
解 ②,
解 ③,绕自身平移和转动
TTTT
TTTT
kjid
kjidp
01.00
03.09.0
其结果等于绕固定坐标系转动和旋转
kjid
kjidp
001.0
6.003.0
等效说明,如果我们发现末端操作器相对于基准坐标系有了微位移 （ 平移或转动 ）,我们可以认为末端操作器相对于自己的坐标系发生了微位移 。 只是微动率 △ 和 △ Τ不同而己 。 其结果是等效的 。
这些在进行误差补偿和微动时有用,如产生误差如何补偿？ 可以反向运动末端关节来补偿?
d
dp
微动齐次变换的意义误差及误差补偿
制造和检测误差
运算过程中圆整、插补、拟合造成的误差 — 原理性误差
构件承受的负载、加速度、重力的变形误差
传动误差
环境影响误差误差来源：
单关节补偿
多关节补偿误差补偿：
单关节补偿：
1116561111 iiiiii AATdAA
id?
11111656111211121 iiiiiiii AATTdAAA
6
6
6
5
6
5
6
5
6
5
6
5
6
5
6
5
2
2
2
1
2
1
2
1
2
1
2
1
2
1
2
1
1
1
1
0
1
0
1
0
1
0
1
0
1
0
1
0
AAAAAdA
AAAAAdA
AAAAAdA
6565212110106060 AdAAdAAdATdT
忽略高次项：
66651065322221106521111060 AAAAAAAAATd
绕自身
656521106521211065100060 AAAAAAAATd
绕 Σ i-1
多关节补偿：
并联机器人运动学
燕山大学 黄真
,并联机器人机构学理论及其控制,
11000
1
.,,0
.0,,,
.0),,(
)()()(
,
1000
T
00
2222
62,1
0
0
zib
yib
xib
pzazsznz
pyaysyny
pxaxsxnx
bi z
bi y
bi x
zibyibxib
bibi zbi ybi x
B i zB i yB i x
B i zbi zB i ybi yB i xbi xl
lll
pzazsznz
pyaysyny
pxaxsxnx
i
）是已知的中，（在中的位置在也就是）现在求（
中是已知的在易有：
求：
中的位置和姿态在即：已知
即为所求解
222
iziyixi llll
