机器人学笔记(1)变换矩阵
空间描述
如果要描述空间中确定的一点相对于空间中坐标系的位置,使用 A P ^{A}P AP表示点P相对坐标系A的位置矢量,然而,在实际的三维空间描述中,单单使用位置矢量是不足以描述位置信息的。
为了描述物体与参考坐标系之间的位置关系,一般选择在物体上建立坐标系,通过物体坐
标系与参考坐标系之间的关系来描述物体在空间中的位置,如图,坐标系之间的位置关系可以使用位置矢量
A
P
^{A}P
AP来表示,而两坐标系之间的旋转关系可以使用旋转矩阵来表示,一般地,我们可以将物体坐标系的三个坐标轴的单位矢量用向量
X
A
^
\hat{X_A}
XA^、
Y
A
^
\hat{Y_A}
YA^、
Z
A
^
\hat{Z_A}
ZA^表示,则其在参考坐标系上的表达为:
B
X
A
^
\hat{^{B}X_A}
BXA^、
B
Y
A
^
\hat{^{B}Y_A}
BYA^、
B
Z
A
^
\hat{^{B}Z_A}
BZA^。可有旋转矩阵为(B相对于A的表达):
B
A
R
^{A}_{B}R
BAR,有:
B
A
R
=
(
B
X
A
^
B
Y
A
^
B
Z
A
^
)
=
(
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
)
^{A}_{B}R = \begin{pmatrix} \hat{^{B}X_A}&\hat{^{B}Y_A}&\hat{^{B}Z_A} \end{pmatrix}=\begin{pmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33} \end{pmatrix}
BAR=(BXA^BYA^BZA^)=⎝
⎛r11r21r31r12r22r32r13r23r33⎠
⎞
对于以上3×3矩阵中的每一个元素分别为物体坐标系三轴在参考坐标系下的投影,即:
B
A
R
=
(
B
X
A
^
B
Y
A
^
B
Z
A
^
)
=
(
X
B
^
×
X
A
^
Y
B
^
×
X
A
^
Z
B
^
×
X
A
^
X
B
^
×
Y
A
^
Y
B
^
×
Y
A
^
Z
B
^
×
Y
A
^
X
B
^
×
Z
A
^
Y
B
^
×
Z
A
^
Z
B
^
×
Z
A
^
)
^{A}_{B}R = \begin{pmatrix}\hat{^{B}X_A}&\hat{^{B}Y_A}&\hat{^{B}Z_A}\end{pmatrix}= \begin{pmatrix} \hat{X_B}\times\ \hat{X_A}&\hat{Y_B}\times\ \hat{X_A}&\hat{Z_B}\times\ \hat{X_A}\\ \hat{X_B}\times\ \hat{Y_A}&\hat{Y_B}\times\ \hat{Y_A}&\hat{Z_B}\times\ \hat{Y_A}\\ \hat{X_B}\times\ \hat{Z_A}&\hat{Y_B}\times\ \hat{Z_A}&\hat{Z_B}\times\ \hat{Z_A} \end{pmatrix}
BAR=(BXA^BYA^BZA^)=⎝
⎛XB^× XA^XB^× YA^XB^× ZA^YB^× XA^YB^× YA^YB^× ZA^ZB^× XA^ZB^× YA^ZB^× ZA^⎠
⎞
在上式中,上标被省略,事实上,只需要在同一个坐标系下即可,结果是一样的。
观察上述矩阵,可有:
B
A
R
=
A
B
R
T
^{A}_{B}R = ^{B}_{A}R^T
BAR=ABRT
坐标系描述
为了完整描述下述机械手的位置,可将机械手上任选位置作为描述,为方便,将其设置为连体坐标系的原点,在机器人学中,位置和姿态常常成对出现,可将其描述为坐标系,使用四个矢量为一组描述位置和姿态信息,一个位置矢量描述坐标系的位置,另三个矢量描述坐标系的姿态,即为
[
B
]
=
[
B
A
R
,
P
B
O
R
G
]
[{B}]=[^{A}_{B}R,P_{BORG}]
[B]=[BAR,PBORG]
P为位置矢量,
从坐标系到坐标系的映射
平移坐标的映射
如果两个坐标姿态相同,而他们在空间中的位置是不同的,则两者之间只需做一个平移变换,如下图所示。
此时,只需要做简单的矢量加法:
A
P
=
A
P
B
O
G
R
+
B
P
^{A}P=^{A}P_{BOGR}+^{B}P
AP=APBOGR+BP
旋转坐标的映射
已知旋转矩阵每一列均为相互垂直的单位向量,有:
B
A
R
=
A
B
R
−
1
=
A
B
R
T
^{A}_{B}R =^{B}_AR^{-1}=^{B}_AR^{T}
BAR=ABR−1=ABRT
旋转矩阵是三个一组的列向量或三个一组的行向量。
B
A
R
=
(
A
X
^
B
A
Y
^
B
A
Z
^
B
)
^{A}_{B}R=\begin{pmatrix}^{A}\hat{X}_B&^{A}\hat{Y}_B &^{A}\hat{Z}_B\end{pmatrix}
BAR=(AX^BAY^BAZ^B)
此时,我们已知矢量相对于某坐标系B的定义,现在求一矢量相对于另一个坐标系A的关系,且两个坐标系原点重合,这个B对A姿态描述是可以用一个姿态矩阵
B
A
R
^A_BR
BAR来表示的,在下面这种情况中,我们已知矢量
B
P
^BP
BP在B坐标系下的表示,要求
A
P
^AP
AP,我们知道任一矢量的分量就是该矢量在参考系上单位矢量的投影,而这样的投影是由点乘得到的,即:
A
p
x
=
A
X
^
B
⋅
B
P
A
p
y
=
A
Y
^
B
⋅
B
P
A
p
z
=
A
Z
^
B
⋅
B
P
^Ap_x=^{A}\hat{X}_B\cdot^BP\\ ^Ap_y=^{A}\hat{Y}_B\cdot^BP\\ ^Ap_z=^{A}\hat{Z}_B\cdot^BP\\
Apx=AX^B⋅BPApy=AY^B⋅BPApz=AZ^B⋅BP
整理得:
A
P
=
B
A
R
⋅
B
P
^AP=^A_BR\cdot^BP
AP=BAR⋅BP
在这种case中,我们已知 B P ^{B}P BP,求 A P ^A{P} AP,考虑最一般的情况,B坐标系与A坐标系的原点不重合,有一个矢量偏移,确定B原点的矢量用 A P B O R G ^A{P}_{BORG} APBORG表示,B对A的旋转用旋转矩阵 B A R ^{A}_{B}R BAR表示。
他们之间的变换可以借助中间坐标系先进行一次旋转,再进行一次平移得到,即:
A
P
=
B
A
R
⋅
B
P
+
A
P
B
O
G
R
^{A}P=^{A}_{B}R\cdot^{B}P+^{A}P_{BOGR}
AP=BAR⋅BP+APBOGR
为了将旋转和平移统一表示,我们引入一种新的形式:
A
P
=
B
A
T
B
P
^{A}P=^{A}_{B}T^{B}P
AP=BATBP
即用一个矩阵形式的算子表示一个坐标系到另一个坐标系的映射,其中T是增广后的4×4矩阵。
(
A
P
1
)
=
(
B
A
R
A
P
B
O
G
R
0
1
)
(
B
P
1
)
\begin{pmatrix} ^{A}P\\1 \end{pmatrix}=\begin{pmatrix} ^{A}_BR&^{A}P_{BOGR}\\0&1 \end{pmatrix}\begin{pmatrix} ^{B}P\\1 \end{pmatrix}
(AP1)=(BAR0APBOGR1)(BP1)
算子:平移与旋转
平移算子:
平移指的是一个点沿一个矢量方向移动一定的距离,对于空间中一点来说,空间中点的平移与此点到另一坐标系的映射具有相同的描述,对于某一个向量在空间中的移动而言:
对于向量
A
P
1
^{A}P_1
AP1而言,
A
Q
^{A}Q
AQ描述了
A
P
1
^{A}P_1
AP1的位置变换,为:
A
P
2
=
A
Q
+
A
P
1
^{A}P_2 =^{A}Q+^{A}P_1
AP2=AQ+AP1
用矩阵算子写出平移变换,有:
A
P
2
=
D
Q
(
q
)
A
P
1
^{A}P_2=D_Q(q)^{A}P_1
AP2=DQ(q)AP1
式中,q是沿矢量
A
Q
^{A}Q
AQ平移的数量,其中算子
D
Q
(
q
)
D_Q(q)
DQ(q)为一种特殊的矢量变换
D
Q
(
q
)
=
(
1
0
0
q
1
0
1
0
q
2
0
0
1
q
3
0
0
0
1
)
D_Q(q)=\begin{pmatrix} 1&0&0&q_1 \\ 0&1&0&q_2 \\ 0&0&1&q_3 \\ 0&0&0&1 \end{pmatrix}
DQ(q)=⎝
⎛100001000010q1q2q31⎠
⎞
其中
q
1
q_1
q1、
q
2
q_2
q2和
q
3
q_3
q3是平移矢量
A
Q
^AQ
AQ的分量,且满足
q
1
2
+
q
2
2
+
q
3
2
=
q
\sqrt {q_1^2+q_2^2+q_3^2} = q
q12+q22+q32=q。
旋转算子
旋转变换算子还可以通过绕某轴转过的角度来表述,例如,绕z轴旋转的旋转矩阵为:
R
z
=
(
c
o
s
θ
−
s
i
n
θ
0
0
s
i
n
θ
c
o
s
θ
0
0
0
0
1
0
0
0
0
1
)
R_z=\begin{pmatrix} cos\theta&-sin\theta&0&0\\ sin\theta&cos\theta&0&0 \\ 0&0&1&0\\ 0&0&0&1 \end{pmatrix}
Rz=⎝
⎛cosθsinθ00−sinθcosθ0000100001⎠
⎞
总结
给出齐次变换矩阵的三个定义:
1.它是坐标系的描述。 B A T ^{A}_{B}T BAT表示相对于坐标系{A}的坐标系{B}。特别的, B A R ^{A}_BR BAR(纯旋转矩阵)的各列是B坐标系主轴上的单位矢量(在A坐标系下的表示), A P B O R G ^{A}P_{BORG} APBORG只起到确定B的原点的作用。
2.它是变换映射, B A T ^{A}_{B}T BAT可以将矢量从B坐标系变换到A坐标系。
3.它是变换算子。
变换算法
变换的乘法与变换的逆运算,两套变换构成了一套功能完备的变换算子。
混合变换
已知坐标系C相对于坐标系B的变换矩阵
C
B
T
^{B}_{C}T
CBT和坐标系B相对于坐标系A的变换矩阵
B
A
T
^{A}_{B}T
BAT,将矢量
C
P
^CP
CP变换为
A
P
^AP
AP,有:
A
P
=
B
A
T
C
B
T
C
P
^AP=^{A}_{B}T^{B}_{C}T^CP
AP=BATCBTCP
即:
C
A
T
=
B
A
T
C
B
T
^{A}_{C}T=^{A}_{B}T^{B}_{C}T
CAT=BATCBT
转为分块矩阵的形式:
C
A
T
=
(
B
A
R
C
B
R
B
A
R
B
P
C
O
R
G
+
A
P
B
O
R
G
0
1
)
^{A}_{C}T=\begin{pmatrix} ^A_BR^B_CR&^A_BR^BP_{CORG}+^AP_{BORG}\\ 0&1 \end{pmatrix}
CAT=(BARCBR0BARBPCORG+APBORG1)
逆变换
已知坐标系B相对于坐标系A——即已知变换矩阵 B A T ^{A}_{B}T BAT的值,为了得到坐标系A相对于坐标系B的描述,需要求解这个矩阵的逆。当然可以直接求矩阵的逆,但这样算比较繁琐,下面介绍如何利用变换矩阵的性质计算矩阵的逆。
要求
B
A
T
^A_BT
BAT,必须通过
A
P
B
O
R
G
^AP_{BORG}
APBORG和
B
A
R
^A_BR
BAR计算得到
B
P
A
O
R
G
^BP_{AORG}
BPAORG和
A
B
R
^B_AR
ABR,首先回顾一下,关于旋转矩阵,有:
A
B
R
=
B
A
R
T
^B_AR=^A_BR^T
ABR=BART
计算
A
P
B
O
R
G
^AP_{BORG}
APBORG在B坐标系下的表示,有
(
B
(
A
P
B
O
R
G
)
1
)
=
(
A
B
R
B
P
A
O
G
R
0
1
)
(
A
P
B
O
R
G
1
)
\begin{pmatrix}^B(^AP_{BORG})\\1\end{pmatrix}=\begin{pmatrix}^{B}_AR&^{B}P_{AOGR}\\0&1\end{pmatrix}\begin{pmatrix}^AP_{BORG}\\1\end{pmatrix}
(B(APBORG)1)=(ABR0BPAOGR1)(APBORG1)
注意,由于计算的是
A
P
B
O
R
G
^AP_{BORG}
APBORG在B坐标系下的表示,故
B
(
A
P
B
O
R
G
)
^B(^AP_{BORG})
B(APBORG)是零矢量,有:
A
B
R
A
P
B
O
R
G
+
B
P
A
O
G
R
=
0
^{B}_AR^AP_{BORG}+^{B}P_{AOGR}=0
ABRAPBORG+BPAOGR=0
即:
−
B
A
R
T
⋅
A
P
B
O
R
G
=
B
P
A
O
G
R
-^A_BR^T\cdot ^AP_{BORG}=^{B}P_{AOGR}
−BART⋅APBORG=BPAOGR
得到
A
B
T
^{B}_{A}T
ABT为:
A
B
T
=
(
B
A
R
T
−
B
A
R
T
⋅
A
P
B
O
R
G
0
1
)
^{B}_{A}T=\begin{pmatrix}^A_BR^T&-^A_BR^T\cdot ^AP_{BORG}\\0&1 \end{pmatrix}
ABT=(BART0−BART⋅APBORG1)
})$是零矢量,有:
A
B
R
A
P
B
O
R
G
+
B
P
A
O
G
R
=
0
^{B}_AR^AP_{BORG}+^{B}P_{AOGR}=0
ABRAPBORG+BPAOGR=0
即:
−
B
A
R
T
⋅
A
P
B
O
R
G
=
B
P
A
O
G
R
-^A_BR^T\cdot ^AP_{BORG}=^{B}P_{AOGR}
−BART⋅APBORG=BPAOGR
得到
A
B
T
^{B}_{A}T
ABT为:
A
B
T
=
(
B
A
R
T
−
B
A
R
T
⋅
A
P
B
O
R
G
0
1
)
^{B}_{A}T=\begin{pmatrix}^A_BR^T&-^A_BR^T\cdot ^AP_{BORG}\\0&1 \end{pmatrix}
ABT=(BART0−BART⋅APBORG1)