机器人运动学
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
物机机器人末端操作器位姿的其它描述方法
用矩阵表示刚性体的转动简化了许多运算，
但它需要 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 （
解：
斯坦福机器人运动学逆问题解
6533211060 AAAAT
61T?
653321 AAA
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
211 dpcps yx由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA
式中第四列：
6362132 TTA
式中第三列：
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
物机机器人末端操作器位姿的其它描述方法
用矩阵表示刚性体的转动简化了许多运算，
但它需要 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 （
解：
斯坦福机器人运动学逆问题解
6533211060 AAAAT
61T?
653321 AAA
式中：
yx
yx
pCpSpf
zpf
pSpCpf
1113
12
1111
)(
)(
)(
211 dpcps yx由两端矩阵对应元素相等可得：
作三角变换：
式中：
得到：
即有：
（ ）
由 1,4和 2,4元素对应相等，得：
6261121 TTA
式中第四列：
6362132 TTA
式中第三列：
