第二章位姿描述和齐次变换
合集下载
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
17
2.3、齐次坐标和齐次变换(续)
A 齐次变换矩阵 BT的另一种变形：
A A BR BT 0 0 0
A
p B0 1
平移变换矩阵
旋转变换矩阵
I3x 3 0 0 0
A
p B0 A BR 1 0 0 0 1
T rans ( A pB0 ) Rot(k, )
A 4X4的方阵 设为： BT
B A A p A B R p p B0 1 1
— — 齐次变换。 称A BT为齐次变换矩阵。
坐标{B}相对于{A}的 旋转矩阵（3X3） 坐标{B}的原点在 {A}坐标系中的坐 标。（3X1） 标示符，“1”位置； “0”方向
16
A B
T的特点：
7
必须注意,齐次坐标的表示不是唯一的。我 们将其各元素同乘一非零因子w后,仍然代表同
一点P,即:
式中：a=w px; b=w py; c=w pz
8
2.1、刚体位姿描述(续)
二、方位的描述（旋转矩阵）
z
z1
0
y
为了表示刚体的方位， 用一直角坐标系 {B} { x1、y1、z1}与刚体固接。设 {B}中单位主矢量为 x B、
方式1： 方式2：
A 1 B 直接对矩阵A T 求逆变换 T （ B B AT）。
A BT 0
A
B A
A B
R 0 0
A
p B0 1
A B
A R、 pB0 是已知的。
pB0 ：表示 {B}坐标系中的原点在 {A}中的坐标位置。
p B0 ：表示 {A}坐标系中的一点 ({ B}的原点 )在{B}中的坐标位置 [0， 0， 0]T 。
cos 0 sin R ( y , ) 0 1 0 sin 0 cos
三、位姿的描述（固接坐标系）
cos si n 0 R ( z , ) si n cos 0 0 1 0
机器人的一个连杆可以看作一个刚体。若给定了刚体上某 一点的位置和该刚体在空间的姿态,则这个
yB
xB zB
zA
解释如下： {B}的坐标原点相对于 {A}的位置为 [1， 3， 4， 1]T
{B}的三个坐标轴相对于 {A}的方向分别为：
yA xA
{B}的x轴相对于 {A}的方向矢量 [0， 1 ， 0， 0]T {B}的x轴与{A}的y轴同向。 {B}的y轴相对于 {A}的方向矢量 [0， 0， 1 ， 0]T {B}的y轴与{A}的z轴同向。 {B}的z轴相对于 {A}的方向矢量 [1 ， 0， 0,0]T {B}的z轴与{A}的x轴同向。
n o
i
a
2
运动学研究的问题
Where is my hand?
运动学正问题
Direct Kinematics HERE!
How do I put my hand here?
运动学逆问题
Inverse Kinematics: Choose these angles!
3
• 丹纳维特（Denavit）和哈顿贝格（Hartenberg） 于1955年提出了一种矩阵代数方法解决机器人的 运动学问题—D-H方法 • 具有直观的几何意义 • 能表达动力学、计算机视觉和比例变换问题 • 其数学基础即是齐次变换
20
2.4、齐次变换矩阵的运算(续)
例2.5：已知 A {B}相对{A}绕其z轴转30，再沿{A}的x轴移动4， BT 表示 沿{A}的y轴移动3，求 B AT 。
解： 相对于固定坐标系运动
左乘：A (4,3,0) Rot(z,30) BT T rans
即
A B
T 0
A B
B C pB T p C A B C B A C T T p p A T p T p B C B C
A
A C
B 复合变换（ {C}相对于{A}的描述） T A T B CT
这种变换的另一种解释 ： 坐标系{C}是这样得到的：最初 {C}与{A}重合，首先相对于 {A}作运动
pB0 [10 ， 5， 0]T
(3)求A p
0.866 0.5 0 3 10 9.098 A B A pA R p pB0 0.5 0.866 0 7 5 12.562 B 0 1 0 0 0 0
A B
T，到达{B}，然后相对于{B}作运动 B {C}。 CT，到达最终
“从右到左” ( )：运动相对于固定坐标 系而言的。 左乘 变换顺序 “从左到右” ( )：运动相对于运动坐标 系而言的。 右乘
19
2.4、齐次变换矩阵的运算(续)
二、变换矩阵求逆
如何求？ B 已知坐标系 {B}相对{A}的描述为A T { A } 相对 { B } 的描述为 B AT。
13
2.2、坐标变换(续)
三、一般变换
zB zA
p
已知：
yB
B
p、A B R，求：
B
A
p
pB*p*
z
* B
p*
y* B
又有：
A
0
p A p* A pB0
A B*
xB
而 A p*
Biblioteka Baidu
R B* p*
0
yA
Ap
A B* A B
R B* p* A pB0 R B p A pB0
15
2.3、齐次坐标和齐次变换
因为在一般变换过程中 ，复合变换方程为：
A B A p A B R p p B0 — — 非齐次的。
用另外一种等价方式来 表示：
4X1的列向量
A
A p A BR 1 0 0 0
可简化为：
A B p A BT p
p B0 B p 1 1
一、坐标平移
坐标平移方程：
A
pB p A p Bo
zA
zB
p
{B}坐标系的原点在{A} 坐标系中的坐标
oB oA xA yA xB
yB
12
2.2、坐标变换(续)
二、坐标旋转
zA
zB
p
A A 已知B p、 R 、求 p? B
yA xB xA
A
0
yB
B z A 系(坐标旋转方程)为： p、 p 在两坐标系中的变换关 A B p A R p B
A y B、z B；则旋转矩阵的表达式 B R为：
x1
r12 r22 r32 r13 r23 r33
y1
A B
R [ A xB
A
yB
r11 r A z B ];或A R B 21 r31
x
A A A 因为单位主矢量 x B、 yB、 zB两两垂直 旋转矩阵A B R是正交矩阵。
A A BR BT 0 0 0
A
p B0 1
A 即 ： {B}相对于 {A}的位置和方位。 BT描述了坐标系
2.3、齐次坐标和齐次变换(续)
0 1 A 例如：试解释齐次变换 矩阵： BT 0 0 { A}坐标的位姿。 1 0 0 3 所描述的{ B}坐标相对于 1 0 4 0 0 1 0 1
0 4 4.964 0.866 0.5 3 0.598 0 . 5 0 . 866 0 0 1 0 0 0
T A A B R p B0 1
A 1 A T 即： B R BR ； A B
R 1
绕x轴旋转角的旋转矩阵为：
0 0 1 R ( x , ) 0 cos sin 0 sin cos
z
z1
x1
0
y1
y
x
9
2.1、刚体位姿描述(续)
同理，可求出：绕 y、z轴旋转角的旋转矩阵分别为：
手部的中心点为原点OB:关节轴为 ZB轴, ZB轴的单位方向矢量O称为
接近矢量,指向朝外;二手指的连线
为yB后轴,yB轴的单位方向矢量称 为姿态矢量,指向可任意选定,xB轴
与yB后铀及ZB轴垂直,X后轴的单
位方向矢量n为法向矢量,且 n=o×a,指向符合右手法则。
11
2.2、坐标变换 描述的是：空间任一点p在不同坐标系中表示方法的一种 联系。即：已知两坐标系{A}、{B}之间的关系，空间任一点p 在{A}中坐标表示，求p在{B}中的坐标表示？
解：
(1)求旋转矩阵A BR
A B
R R(z,30)
cos sin 0 而 R ( z , ) sin cos 0 0 1 0
A (2)求位置矢量 pB0 A
cos30 sin30 0 0.866 0.5 0 sin30 cos30 0 0.5 0.866 0 A R B 0 1 0 1 0 0
x A x* B
zA 复合变换方程为：
A B A p A R p p B0 B
14
2.2、坐标变换(续)
例2.1 ：已知坐标系 {B}的初始位姿与 {A}重合，首先 {B}相对于坐标系 {A}的z A 轴旋转30，再沿{A}的x A 轴移动10单元，同时沿 {A}的y A 轴移动5
B 单元, 求位置矢量A p B0和旋转矩阵A ， 7， 0]T ,求A p。 B R;若 p [ 3
4
位姿的定义：
刚体参考点的位置和刚体的姿态。
姿态的描述方法： 齐次变换法、矢量法、旋量法、四元数法等等。
x
z
z
y
0
y
2.1、刚体位姿描述
x
x 、 y 、 z 、 x 、y、z 在三维空间内，刚体有6个自由度：
一、位置的描述（位置矢量） 对于直角坐标系{A}，空间任一点p的位置可用3x1的列矢量Ap表示：
zB
p
表示绕过坐标原点的轴 k旋转θ角度。
z
* B
zA
yB
p*
y* B
0
xB
0
yA
* B
“左乘”的概念！！
18
xA x
2.4、齐次变换矩阵的运算
一、变换矩阵相乘
已知三维空间中的三个 坐标系{A}、 {B}、 {C}； {B}相对于{A}的描述为A BT， {C}相对于{B}的描述为B p，有： CT，则对于空间任一点
p x A p p y p z
5
式中PX,PY,PZ是点P在坐标系{A}中的三个位 置坐标分量,如图2-1所示。
6
齐次坐标表示： 如用四个数组成的(4×1)列阵。
表示三维空间直角坐标系{A}中点p,则列阵 [Px Py Pz 1]T称为三维空间点P的齐次坐标。
机器人运动学
1
引言
机械臂抓举或 搬运零件.AVI
• 机器人可以用一个开环关节链来建模 • 由数个驱动器驱动的转动或移动关节串联而成 • 一端固定在基座上，另一端是自由的，安装工具，用以 操纵物体 • 人们感兴趣的是操作机末端执行器 相对于固定参考坐标数的空间几何 描述，也就是机器人的运动学问题。 • 机器人的运动学即是研究机器人手 臂末端执行器位置和姿态与关节变 量空间之间的关系。
R 0
A
0
p B0 1
0 0.866 0.5 B T A R A 0.866 0 B R 0. 5 0 1 0
T A 又 A B R p B0
0.866 0.5 0 4 0.5 0.866 0 3 0 0 1 0 0 0 0 1
刚体在空间上是完全确定的。
刚体B在空间坐标系 {A}中的位姿 (位置、姿态)：
xB
zA
0
zB
{B} { R
A B
A
pBo }
位置矢量
0
yB yA
旋转矢量
xA
10
2.1、刚体位姿描述(续)
四、手爪坐标系
机器人手部的位置和姿态也可以用固连于手部的坐标系{B} 的位姿来表示,如图2-6所示。
坐标系{B}可以这样来确定:取
B A
A T A B B p B0 A R A p B0 B p A 0 0 B pA0 A R A pB0 B R pB0
B B AR AT 0 0 0
B
T A T A R R p B0 pA0 A B B 0 0 0 1 1
2.3、齐次坐标和齐次变换(续)
A 齐次变换矩阵 BT的另一种变形：
A A BR BT 0 0 0
A
p B0 1
平移变换矩阵
旋转变换矩阵
I3x 3 0 0 0
A
p B0 A BR 1 0 0 0 1
T rans ( A pB0 ) Rot(k, )
A 4X4的方阵 设为： BT
B A A p A B R p p B0 1 1
— — 齐次变换。 称A BT为齐次变换矩阵。
坐标{B}相对于{A}的 旋转矩阵（3X3） 坐标{B}的原点在 {A}坐标系中的坐 标。（3X1） 标示符，“1”位置； “0”方向
16
A B
T的特点：
7
必须注意,齐次坐标的表示不是唯一的。我 们将其各元素同乘一非零因子w后,仍然代表同
一点P,即:
式中：a=w px; b=w py; c=w pz
8
2.1、刚体位姿描述(续)
二、方位的描述（旋转矩阵）
z
z1
0
y
为了表示刚体的方位， 用一直角坐标系 {B} { x1、y1、z1}与刚体固接。设 {B}中单位主矢量为 x B、
方式1： 方式2：
A 1 B 直接对矩阵A T 求逆变换 T （ B B AT）。
A BT 0
A
B A
A B
R 0 0
A
p B0 1
A B
A R、 pB0 是已知的。
pB0 ：表示 {B}坐标系中的原点在 {A}中的坐标位置。
p B0 ：表示 {A}坐标系中的一点 ({ B}的原点 )在{B}中的坐标位置 [0， 0， 0]T 。
cos 0 sin R ( y , ) 0 1 0 sin 0 cos
三、位姿的描述（固接坐标系）
cos si n 0 R ( z , ) si n cos 0 0 1 0
机器人的一个连杆可以看作一个刚体。若给定了刚体上某 一点的位置和该刚体在空间的姿态,则这个
yB
xB zB
zA
解释如下： {B}的坐标原点相对于 {A}的位置为 [1， 3， 4， 1]T
{B}的三个坐标轴相对于 {A}的方向分别为：
yA xA
{B}的x轴相对于 {A}的方向矢量 [0， 1 ， 0， 0]T {B}的x轴与{A}的y轴同向。 {B}的y轴相对于 {A}的方向矢量 [0， 0， 1 ， 0]T {B}的y轴与{A}的z轴同向。 {B}的z轴相对于 {A}的方向矢量 [1 ， 0， 0,0]T {B}的z轴与{A}的x轴同向。
n o
i
a
2
运动学研究的问题
Where is my hand?
运动学正问题
Direct Kinematics HERE!
How do I put my hand here?
运动学逆问题
Inverse Kinematics: Choose these angles!
3
• 丹纳维特（Denavit）和哈顿贝格（Hartenberg） 于1955年提出了一种矩阵代数方法解决机器人的 运动学问题—D-H方法 • 具有直观的几何意义 • 能表达动力学、计算机视觉和比例变换问题 • 其数学基础即是齐次变换
20
2.4、齐次变换矩阵的运算(续)
例2.5：已知 A {B}相对{A}绕其z轴转30，再沿{A}的x轴移动4， BT 表示 沿{A}的y轴移动3，求 B AT 。
解： 相对于固定坐标系运动
左乘：A (4,3,0) Rot(z,30) BT T rans
即
A B
T 0
A B
B C pB T p C A B C B A C T T p p A T p T p B C B C
A
A C
B 复合变换（ {C}相对于{A}的描述） T A T B CT
这种变换的另一种解释 ： 坐标系{C}是这样得到的：最初 {C}与{A}重合，首先相对于 {A}作运动
pB0 [10 ， 5， 0]T
(3)求A p
0.866 0.5 0 3 10 9.098 A B A pA R p pB0 0.5 0.866 0 7 5 12.562 B 0 1 0 0 0 0
A B
T，到达{B}，然后相对于{B}作运动 B {C}。 CT，到达最终
“从右到左” ( )：运动相对于固定坐标 系而言的。 左乘 变换顺序 “从左到右” ( )：运动相对于运动坐标 系而言的。 右乘
19
2.4、齐次变换矩阵的运算(续)
二、变换矩阵求逆
如何求？ B 已知坐标系 {B}相对{A}的描述为A T { A } 相对 { B } 的描述为 B AT。
13
2.2、坐标变换(续)
三、一般变换
zB zA
p
已知：
yB
B
p、A B R，求：
B
A
p
pB*p*
z
* B
p*
y* B
又有：
A
0
p A p* A pB0
A B*
xB
而 A p*
Biblioteka Baidu
R B* p*
0
yA
Ap
A B* A B
R B* p* A pB0 R B p A pB0
15
2.3、齐次坐标和齐次变换
因为在一般变换过程中 ，复合变换方程为：
A B A p A B R p p B0 — — 非齐次的。
用另外一种等价方式来 表示：
4X1的列向量
A
A p A BR 1 0 0 0
可简化为：
A B p A BT p
p B0 B p 1 1
一、坐标平移
坐标平移方程：
A
pB p A p Bo
zA
zB
p
{B}坐标系的原点在{A} 坐标系中的坐标
oB oA xA yA xB
yB
12
2.2、坐标变换(续)
二、坐标旋转
zA
zB
p
A A 已知B p、 R 、求 p? B
yA xB xA
A
0
yB
B z A 系(坐标旋转方程)为： p、 p 在两坐标系中的变换关 A B p A R p B
A y B、z B；则旋转矩阵的表达式 B R为：
x1
r12 r22 r32 r13 r23 r33
y1
A B
R [ A xB
A
yB
r11 r A z B ];或A R B 21 r31
x
A A A 因为单位主矢量 x B、 yB、 zB两两垂直 旋转矩阵A B R是正交矩阵。
A A BR BT 0 0 0
A
p B0 1
A 即 ： {B}相对于 {A}的位置和方位。 BT描述了坐标系
2.3、齐次坐标和齐次变换(续)
0 1 A 例如：试解释齐次变换 矩阵： BT 0 0 { A}坐标的位姿。 1 0 0 3 所描述的{ B}坐标相对于 1 0 4 0 0 1 0 1
0 4 4.964 0.866 0.5 3 0.598 0 . 5 0 . 866 0 0 1 0 0 0
T A A B R p B0 1
A 1 A T 即： B R BR ； A B
R 1
绕x轴旋转角的旋转矩阵为：
0 0 1 R ( x , ) 0 cos sin 0 sin cos
z
z1
x1
0
y1
y
x
9
2.1、刚体位姿描述(续)
同理，可求出：绕 y、z轴旋转角的旋转矩阵分别为：
手部的中心点为原点OB:关节轴为 ZB轴, ZB轴的单位方向矢量O称为
接近矢量,指向朝外;二手指的连线
为yB后轴,yB轴的单位方向矢量称 为姿态矢量,指向可任意选定,xB轴
与yB后铀及ZB轴垂直,X后轴的单
位方向矢量n为法向矢量,且 n=o×a,指向符合右手法则。
11
2.2、坐标变换 描述的是：空间任一点p在不同坐标系中表示方法的一种 联系。即：已知两坐标系{A}、{B}之间的关系，空间任一点p 在{A}中坐标表示，求p在{B}中的坐标表示？
解：
(1)求旋转矩阵A BR
A B
R R(z,30)
cos sin 0 而 R ( z , ) sin cos 0 0 1 0
A (2)求位置矢量 pB0 A
cos30 sin30 0 0.866 0.5 0 sin30 cos30 0 0.5 0.866 0 A R B 0 1 0 1 0 0
x A x* B
zA 复合变换方程为：
A B A p A R p p B0 B
14
2.2、坐标变换(续)
例2.1 ：已知坐标系 {B}的初始位姿与 {A}重合，首先 {B}相对于坐标系 {A}的z A 轴旋转30，再沿{A}的x A 轴移动10单元，同时沿 {A}的y A 轴移动5
B 单元, 求位置矢量A p B0和旋转矩阵A ， 7， 0]T ,求A p。 B R;若 p [ 3
4
位姿的定义：
刚体参考点的位置和刚体的姿态。
姿态的描述方法： 齐次变换法、矢量法、旋量法、四元数法等等。
x
z
z
y
0
y
2.1、刚体位姿描述
x
x 、 y 、 z 、 x 、y、z 在三维空间内，刚体有6个自由度：
一、位置的描述（位置矢量） 对于直角坐标系{A}，空间任一点p的位置可用3x1的列矢量Ap表示：
zB
p
表示绕过坐标原点的轴 k旋转θ角度。
z
* B
zA
yB
p*
y* B
0
xB
0
yA
* B
“左乘”的概念！！
18
xA x
2.4、齐次变换矩阵的运算
一、变换矩阵相乘
已知三维空间中的三个 坐标系{A}、 {B}、 {C}； {B}相对于{A}的描述为A BT， {C}相对于{B}的描述为B p，有： CT，则对于空间任一点
p x A p p y p z
5
式中PX,PY,PZ是点P在坐标系{A}中的三个位 置坐标分量,如图2-1所示。
6
齐次坐标表示： 如用四个数组成的(4×1)列阵。
表示三维空间直角坐标系{A}中点p,则列阵 [Px Py Pz 1]T称为三维空间点P的齐次坐标。
机器人运动学
1
引言
机械臂抓举或 搬运零件.AVI
• 机器人可以用一个开环关节链来建模 • 由数个驱动器驱动的转动或移动关节串联而成 • 一端固定在基座上，另一端是自由的，安装工具，用以 操纵物体 • 人们感兴趣的是操作机末端执行器 相对于固定参考坐标数的空间几何 描述，也就是机器人的运动学问题。 • 机器人的运动学即是研究机器人手 臂末端执行器位置和姿态与关节变 量空间之间的关系。
R 0
A
0
p B0 1
0 0.866 0.5 B T A R A 0.866 0 B R 0. 5 0 1 0
T A 又 A B R p B0
0.866 0.5 0 4 0.5 0.866 0 3 0 0 1 0 0 0 0 1
刚体在空间上是完全确定的。
刚体B在空间坐标系 {A}中的位姿 (位置、姿态)：
xB
zA
0
zB
{B} { R
A B
A
pBo }
位置矢量
0
yB yA
旋转矢量
xA
10
2.1、刚体位姿描述(续)
四、手爪坐标系
机器人手部的位置和姿态也可以用固连于手部的坐标系{B} 的位姿来表示,如图2-6所示。
坐标系{B}可以这样来确定:取
B A
A T A B B p B0 A R A p B0 B p A 0 0 B pA0 A R A pB0 B R pB0
B B AR AT 0 0 0
B
T A T A R R p B0 pA0 A B B 0 0 0 1 1