3、机器人的位姿描述与坐标变换
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
Xj
Zj
P
Oj
Yj Zi
Oi P OiO j O j P
i
Oj i
P
P P P
Oj i j
Xi
Oi
Yi
沿着不同轴向的组合平移：
x 0 0 x 0 y 0 y Oj P i 0 0 z z
Zj
Zi
zi
P
zj
yj
Yj
xi
Xi
yi
Yi
xj
Xj
xi x j cos( X i , X j ) y j cos( X i , Y j ) z j cos( X i ,Z j ) i P yi x j cos( Yi , X j ) y j cos( Yi , Y j ) z j cos( Yi ,Z j ) z x cos( Z , X ) y cos( Z , Y ) z cos( Z ,Z ) j i j j i j j i j i
►绕多个坐标轴旋转的转动矩阵
1）、绕固定坐标系旋转
坐标系 ( X i , Yi , Z i )
坐标系( X m , Ym , Z m )
坐标系 ( X j , Y j , Z j )
Zm Zi Zj
R( X i , )
R( Z i ,q )
j i
R( ,q ) ?
q q
Yj Ym Yi
☺
9个元素，只有3个独立， 满足6个约束条件:
O' O O' O ' X .O OX O' O ' Y .O OY ' O' O O Z .O Z 1 ' O' O' O' O' X .O Y Y . Z O O O O Z .O X 0
' T R 1 O OR
☺
O' O
cos(X ' X ) cos(Y ' X ) cos(Z ' X ) cos(X ' Y ) cos(Y ' Y ) cos(Z ' Y ) O' O' O' O' R [ X Y Z ] O O O O 33 cos(X ' Z ) cos(Y ' Z ) cos(Z ' Z ) 姿态矩阵R的特点：
《机器人学》
第三章 机器人的位姿描述与坐标变换
战强
北京航空航天大学机器人研究所
第三章 机器人的位姿描述与坐标变换
Z Y X 机器人 的位姿
Zi Xi Zw Xw
连杆I的 位姿 Yi
Yw
3-1 刚体位姿的数学描述
￥ ￥假设机器人的连杆和关节都是刚体￥ ￥
x0 y ' 刚体位置： o P o 0 z0
证明： 1）绕运动坐标系旋转
R( Z i , )
坐标系 ( X i , Yi , Z i )
Z2 Zj Z i (Z1 )
R(Y1 ,q ) R( Z 2 , ) 坐标系 ( X 1 , Y1 , Z1 ) 坐标系 ( X 2 , Y2 , Z 2 )
坐标系 ( X j , Y j , Z j )
j i
R
j
P
►姿态矢量矩阵
cos(X ' X ) cos(Y ' X ) cos(Z ' X ) cos(X ' Y ) cos(Y ' Y ) cos(Z ' Y ) O' R O cos(X ' Z ) cos(Y ' Z ) cos(Z ' Z )
j i
R( ,q ) R(Z ,q ) R( X , )
Xi
Xm
q
Xj
cos q sinq j i R ( ,q ) 0
sinq cos q 0
0 1 0 0 cos 0 1 0 sin
0 cos q sinq sin cos 0
cos cosq sin sin cos sin cosq sin cos cos sin q sin
cos sin q sin sin q cosq
注意：多个旋转矩阵连乘时，次序不同则含义不同。
1）绕新的动坐标轴依次转动时，每个旋转矩阵要从左往右 乘，即旋转矩阵的相乘顺序与转动次序相同； 2）绕旧的固定坐标轴依次转动时，每个旋转矩阵要从右往 左乘，即旋转矩阵的相乘顺序与转动次序相反。
R是单位正交阵
O' O
R 1
刚体的位置和姿态：
' {O'} {O O R, O' O
P}
Zj
例：某刚体j在参考系i中的 位置 姿态
Oj Oi
oj oi
P?
R?
Xj Zi
Oj
Yj
6
10
Xi
Oi
Yi
3-2 坐标变换（点的映射）
1、坐标平移（坐标系方位相同）
已知点P在j坐标系的坐标，平移j至i，求 点P在i坐标系的坐标。
cos(X i , X j ) cos(X i , Y j ) cos(X i , Z j ) x j i P cos(Yi , X j ) cos(Yi , Y j ) cos(Yi , Z j ) y j cos(Z , X ) cos(Z , Y ) cos(Z , Z ) z i j i j i j j
j i
Yj （Y2 )
q
R?
1) P2 2j R Pj R( Z 2 , ) Pj
Y1
Yi
q
2 2) P 1 1R P 2 R (Y1 , q ) P 2
q
3) Pi i1R P 1 R( Z i , ) P 1 R( Z i , ) R(Y1 ,q ) P2 R( Z i , ) R(Y1 ,q ) R( Z 2 , ) Pj
X Z b Z' O' O
n
Y' t
X'
Y
刚体姿态：
O' O ' R [O OX O' O
Y
单位主矢量
cos(X ' X ) cos(Y ' X ) cos(Z ' X ) cos(X ' Y ) cos(Y ' Y ) cos(Z ' Y ) O' Z ] O 33 cos(X ' Z ) cos(Y ' Z ) cos(Z ' Z )
3）RZ
Z
Zi
j
q q
Yj Yi
Xi
Xj
cos(X i , X j ) cos(X i , Y j ) cos(X i , Z j ) j R cos( Y , X ) cos( Y , Y ) cos( Y , Z ) i i j i j i j cos(Zi , X j ) cos(Z i , Y j ) cos(Z i , Z j )
Xj Yi Yj
Xi
Zi
Zj Yj
cos q sinq j R ( Z , q ) i i 0
sinq cos q 0
0 0 1
q
Xi
q
Xj
Yi
ห้องสมุดไป่ตู้
0 1 0 cos q j R ( X , q ) i i 0 sinq
0 sin q cos q
Z2
q
Z i (Z1 )
R( Z i , )
j i
R(Y1 ,q )
R( Z 2 , )
Zj
R( ,q , ) R(Z , ) R(Y ,q ) R(Z , )
ZYZ欧拉角
q
Yj （Y2 )
q
Y1 Yi
Xi
X1 X 2 X j
cos j R( ,q , ) i sin 0
cos q 0 j R ( Y , q ) i i sinq
0 sinq 1 0 0 cos q
cos q sinq j R ( Z , q ) i i 0
sinq cos q 0
0 0 1
转动矩阵的特点： (1) 主对角线上有一个元素为1，其余均为转角的余弦/正弦； (2) 绕坐标轴转动的次序与元素1所在的行、列号对应； (3) 元素1所在的行、列，其它元素均为0； (4) 从元素1所在行起，自上而下，先出现的正弦为负，后出现 的为正，反之依然。
适用的机器人类型举例（有平移关节）
Z1 X1
Y1 Z2 X2
Y2
Z3 X3
Y3
三坐标的直角坐标机器人
Z
Y
X
Zi
Zj
例: Oi
Yi
P
Oj
Yj
Xi
Xj
15 已知
j
P 5 6 7
T
求 P点在i坐标系中的坐标。
T T
解答： i P j P OjP
i
5 21 7
5 6 7 0 15 0
0 1 0 cos q j R ( X , q ) i i 0 sinq 0 sin q cos q
Zj
Zi
cos q 0 j R ( Y , q ) i i sinq
0 sinq 1 0 0 cos q
q q
sinq cos cos q cos sin
sinq sin cos q sin cos
2）、绕运动坐标系旋转
坐标系 ( X i , Yi , Z i ) 坐标系 ( X 1 , Y1 , Z1 ) 坐标系 ( X 2 , Y2 , Z 2 ) 坐标系 ( X j , Y j , Z j )
Xi
Xm
q
Xj
适用的机器人类型举例（有旋转关节）
例1: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z 轴转30度, 假设点P在 坐标系B的描述为PB={3,7,0}T,求它在坐标 系A中的描述PA.
3、坐标变换综合（平移+旋转）
Zj Zc P
Pi RPj P
j i Oj i
Pj
Pi
Xj
Oj i
Yj Yc
旋转部分
平移部分
Zi
Xc
Oj
推导(建立中间坐标系C)：
I（旋转）: c与j 原点重合，c与i姿态相同
T
2、坐标旋转（坐标系原点相同）
Zj Zi P
坐标系j由坐标系i旋转而成 已知点P在j坐标系的坐标：
Yj
j
P [x j
yj
z j ]T
Yi
Xi Xj
求点P在i坐标系的坐标：
i
P [ xi
yi
zi ]T
Zj
Zi
zi
P
yj
zj
Yj
xi
Xi Xj
yi
Yi
xj
☺关于(Yi , X j )？
yi x j cos(Yi , X j ) yi x j cos(Yi , X j ) y j cos(Yi , Yj ) yi x j cos(Yi , X j ) y j cos(Yi , Yj ) z j cos(Yi , Z j )
X
Z b
Z'
O' Y' t
O
n
X'
Y
i
P R P
j i j
坐标系j相对 于i的方位
旋转矩阵
旋转矩阵的性质：
j i
R R R
i j 1 i j
T
►绕一个坐标轴旋转的转动矩阵
Z Zi
j
Zi
Zj
Yj
q q
q
Yi Y j
Yi Xi
q
Xi
Xj
X
j
1）RX
Zi Zj
2）RY
Yj
q q
Xi Xj Yi
Xi
X1 X 2 X j
2）、绕固定坐标系旋转
( X i , ) ( Zi , q)
坐标系 ( X i , Yi , Z i )
Zm Zi Zj
坐标系( X m , Ym , Z m )
j i
Yj Ym Yi
坐标系 ( X j , Y j , Z j )
q q
R?
证明与讨论:
1) Pm mjR Pj R( Z i , q) Pj 2) Pi m iR P m R ( X i , ) P m R( X i , ) R( Z i , q) Pj
sin cos 0
0 cos q 0 0 1 sinq
0 sinq cos sin 1 0 0 cos q 0
sin cos 0
0 0 1
cos cosq cos sin sin sin cosq cos cos sin sin q sin
Zj
P
Oj
Yj Zi
Oi P OiO j O j P
i
Oj i
P
P P P
Oj i j
Xi
Oi
Yi
沿着不同轴向的组合平移：
x 0 0 x 0 y 0 y Oj P i 0 0 z z
Zj
Zi
zi
P
zj
yj
Yj
xi
Xi
yi
Yi
xj
Xj
xi x j cos( X i , X j ) y j cos( X i , Y j ) z j cos( X i ,Z j ) i P yi x j cos( Yi , X j ) y j cos( Yi , Y j ) z j cos( Yi ,Z j ) z x cos( Z , X ) y cos( Z , Y ) z cos( Z ,Z ) j i j j i j j i j i
►绕多个坐标轴旋转的转动矩阵
1）、绕固定坐标系旋转
坐标系 ( X i , Yi , Z i )
坐标系( X m , Ym , Z m )
坐标系 ( X j , Y j , Z j )
Zm Zi Zj
R( X i , )
R( Z i ,q )
j i
R( ,q ) ?
q q
Yj Ym Yi
☺
9个元素，只有3个独立， 满足6个约束条件:
O' O O' O ' X .O OX O' O ' Y .O OY ' O' O O Z .O Z 1 ' O' O' O' O' X .O Y Y . Z O O O O Z .O X 0
' T R 1 O OR
☺
O' O
cos(X ' X ) cos(Y ' X ) cos(Z ' X ) cos(X ' Y ) cos(Y ' Y ) cos(Z ' Y ) O' O' O' O' R [ X Y Z ] O O O O 33 cos(X ' Z ) cos(Y ' Z ) cos(Z ' Z ) 姿态矩阵R的特点：
《机器人学》
第三章 机器人的位姿描述与坐标变换
战强
北京航空航天大学机器人研究所
第三章 机器人的位姿描述与坐标变换
Z Y X 机器人 的位姿
Zi Xi Zw Xw
连杆I的 位姿 Yi
Yw
3-1 刚体位姿的数学描述
￥ ￥假设机器人的连杆和关节都是刚体￥ ￥
x0 y ' 刚体位置： o P o 0 z0
证明： 1）绕运动坐标系旋转
R( Z i , )
坐标系 ( X i , Yi , Z i )
Z2 Zj Z i (Z1 )
R(Y1 ,q ) R( Z 2 , ) 坐标系 ( X 1 , Y1 , Z1 ) 坐标系 ( X 2 , Y2 , Z 2 )
坐标系 ( X j , Y j , Z j )
j i
R
j
P
►姿态矢量矩阵
cos(X ' X ) cos(Y ' X ) cos(Z ' X ) cos(X ' Y ) cos(Y ' Y ) cos(Z ' Y ) O' R O cos(X ' Z ) cos(Y ' Z ) cos(Z ' Z )
j i
R( ,q ) R(Z ,q ) R( X , )
Xi
Xm
q
Xj
cos q sinq j i R ( ,q ) 0
sinq cos q 0
0 1 0 0 cos 0 1 0 sin
0 cos q sinq sin cos 0
cos cosq sin sin cos sin cosq sin cos cos sin q sin
cos sin q sin sin q cosq
注意：多个旋转矩阵连乘时，次序不同则含义不同。
1）绕新的动坐标轴依次转动时，每个旋转矩阵要从左往右 乘，即旋转矩阵的相乘顺序与转动次序相同； 2）绕旧的固定坐标轴依次转动时，每个旋转矩阵要从右往 左乘，即旋转矩阵的相乘顺序与转动次序相反。
R是单位正交阵
O' O
R 1
刚体的位置和姿态：
' {O'} {O O R, O' O
P}
Zj
例：某刚体j在参考系i中的 位置 姿态
Oj Oi
oj oi
P?
R?
Xj Zi
Oj
Yj
6
10
Xi
Oi
Yi
3-2 坐标变换（点的映射）
1、坐标平移（坐标系方位相同）
已知点P在j坐标系的坐标，平移j至i，求 点P在i坐标系的坐标。
cos(X i , X j ) cos(X i , Y j ) cos(X i , Z j ) x j i P cos(Yi , X j ) cos(Yi , Y j ) cos(Yi , Z j ) y j cos(Z , X ) cos(Z , Y ) cos(Z , Z ) z i j i j i j j
j i
Yj （Y2 )
q
R?
1) P2 2j R Pj R( Z 2 , ) Pj
Y1
Yi
q
2 2) P 1 1R P 2 R (Y1 , q ) P 2
q
3) Pi i1R P 1 R( Z i , ) P 1 R( Z i , ) R(Y1 ,q ) P2 R( Z i , ) R(Y1 ,q ) R( Z 2 , ) Pj
X Z b Z' O' O
n
Y' t
X'
Y
刚体姿态：
O' O ' R [O OX O' O
Y
单位主矢量
cos(X ' X ) cos(Y ' X ) cos(Z ' X ) cos(X ' Y ) cos(Y ' Y ) cos(Z ' Y ) O' Z ] O 33 cos(X ' Z ) cos(Y ' Z ) cos(Z ' Z )
3）RZ
Z
Zi
j
q q
Yj Yi
Xi
Xj
cos(X i , X j ) cos(X i , Y j ) cos(X i , Z j ) j R cos( Y , X ) cos( Y , Y ) cos( Y , Z ) i i j i j i j cos(Zi , X j ) cos(Z i , Y j ) cos(Z i , Z j )
Xj Yi Yj
Xi
Zi
Zj Yj
cos q sinq j R ( Z , q ) i i 0
sinq cos q 0
0 0 1
q
Xi
q
Xj
Yi
ห้องสมุดไป่ตู้
0 1 0 cos q j R ( X , q ) i i 0 sinq
0 sin q cos q
Z2
q
Z i (Z1 )
R( Z i , )
j i
R(Y1 ,q )
R( Z 2 , )
Zj
R( ,q , ) R(Z , ) R(Y ,q ) R(Z , )
ZYZ欧拉角
q
Yj （Y2 )
q
Y1 Yi
Xi
X1 X 2 X j
cos j R( ,q , ) i sin 0
cos q 0 j R ( Y , q ) i i sinq
0 sinq 1 0 0 cos q
cos q sinq j R ( Z , q ) i i 0
sinq cos q 0
0 0 1
转动矩阵的特点： (1) 主对角线上有一个元素为1，其余均为转角的余弦/正弦； (2) 绕坐标轴转动的次序与元素1所在的行、列号对应； (3) 元素1所在的行、列，其它元素均为0； (4) 从元素1所在行起，自上而下，先出现的正弦为负，后出现 的为正，反之依然。
适用的机器人类型举例（有平移关节）
Z1 X1
Y1 Z2 X2
Y2
Z3 X3
Y3
三坐标的直角坐标机器人
Z
Y
X
Zi
Zj
例: Oi
Yi
P
Oj
Yj
Xi
Xj
15 已知
j
P 5 6 7
T
求 P点在i坐标系中的坐标。
T T
解答： i P j P OjP
i
5 21 7
5 6 7 0 15 0
0 1 0 cos q j R ( X , q ) i i 0 sinq 0 sin q cos q
Zj
Zi
cos q 0 j R ( Y , q ) i i sinq
0 sinq 1 0 0 cos q
q q
sinq cos cos q cos sin
sinq sin cos q sin cos
2）、绕运动坐标系旋转
坐标系 ( X i , Yi , Z i ) 坐标系 ( X 1 , Y1 , Z1 ) 坐标系 ( X 2 , Y2 , Z 2 ) 坐标系 ( X j , Y j , Z j )
Xi
Xm
q
Xj
适用的机器人类型举例（有旋转关节）
例1: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z 轴转30度, 假设点P在 坐标系B的描述为PB={3,7,0}T,求它在坐标 系A中的描述PA.
3、坐标变换综合（平移+旋转）
Zj Zc P
Pi RPj P
j i Oj i
Pj
Pi
Xj
Oj i
Yj Yc
旋转部分
平移部分
Zi
Xc
Oj
推导(建立中间坐标系C)：
I（旋转）: c与j 原点重合，c与i姿态相同
T
2、坐标旋转（坐标系原点相同）
Zj Zi P
坐标系j由坐标系i旋转而成 已知点P在j坐标系的坐标：
Yj
j
P [x j
yj
z j ]T
Yi
Xi Xj
求点P在i坐标系的坐标：
i
P [ xi
yi
zi ]T
Zj
Zi
zi
P
yj
zj
Yj
xi
Xi Xj
yi
Yi
xj
☺关于(Yi , X j )？
yi x j cos(Yi , X j ) yi x j cos(Yi , X j ) y j cos(Yi , Yj ) yi x j cos(Yi , X j ) y j cos(Yi , Yj ) z j cos(Yi , Z j )
X
Z b
Z'
O' Y' t
O
n
X'
Y
i
P R P
j i j
坐标系j相对 于i的方位
旋转矩阵
旋转矩阵的性质：
j i
R R R
i j 1 i j
T
►绕一个坐标轴旋转的转动矩阵
Z Zi
j
Zi
Zj
Yj
q q
q
Yi Y j
Yi Xi
q
Xi
Xj
X
j
1）RX
Zi Zj
2）RY
Yj
q q
Xi Xj Yi
Xi
X1 X 2 X j
2）、绕固定坐标系旋转
( X i , ) ( Zi , q)
坐标系 ( X i , Yi , Z i )
Zm Zi Zj
坐标系( X m , Ym , Z m )
j i
Yj Ym Yi
坐标系 ( X j , Y j , Z j )
q q
R?
证明与讨论:
1) Pm mjR Pj R( Z i , q) Pj 2) Pi m iR P m R ( X i , ) P m R( X i , ) R( Z i , q) Pj
sin cos 0
0 cos q 0 0 1 sinq
0 sinq cos sin 1 0 0 cos q 0
sin cos 0
0 0 1
cos cosq cos sin sin sin cosq cos cos sin sin q sin