上篇:机器人数学基础
3 平移和旋转齐次坐标变换
3.1 齐次坐标
空间某点
p
p
p的直角坐标描述和齐次坐标描述如下:
p
=
[
x
y
z
]
⏟
直角坐标
=
[
x
y
z
1
]
=
[
w
x
w
y
w
z
w
]
⏟
齐次坐标
p\ =\ \underbrace{\left[\begin{array}{c}x \\ y \\ z \end{array}\right]}_{\rm{直角坐标}}\ =\ \underbrace{\left[\begin{array}{c}x \\ y \\ z \\ 1 \end{array}\right]\ =\ \left[\begin{array}{c}wx \\ wy \\ wz \\ w \end{array}\right]}_{\rm{齐次坐标}}
p = 直角坐标
xyz
= 齐次坐标
xyz1
=
wxwywzw
式中,
w
w
w为非零常数,是一坐标比例系数.
坐标原点的矢量,即零矢量表示为 [ 0 , 0 , 0 , 1 ] T [0,0,0,1]^{\rm{T}} [0,0,0,1]T.具有形如 [ a , b , c , 0 ] T [a,b,c,0]^{\rm{T}} [a,b,c,0]T的矢量表示无限远矢量,用来表示方向,即用 [ 1 , 0 , 0 , 0 ] T [1,0,0,0]^{\rm{T}} [1,0,0,0]T, [ 0 , 1 , 0 , 0 ] T [0,1,0,0]^{\rm{T}} [0,1,0,0]T, [ 0 , 0 , 1 , 0 ] T [0,0,1,0]^{\rm{T}} [0,0,1,0]T分别表示 x x x, y y y和 z z z轴的方向.
规定两矢量
a
\boldsymbol{a}
a和
b
\boldsymbol{b}
b的点积(数量积)为一标量,计算如下:
a
⋅
b
=
a
x
b
x
+
a
y
b
y
+
a
z
b
z
\boldsymbol{a}\cdot\boldsymbol{b}\ =\ a_xb_x+a_yb_y+a_zb_z
a⋅b = axbx+ayby+azbz
规定两矢量
a
\boldsymbol{a}
a和
b
\boldsymbol{b}
b的叉积(向量积)为与此两相乘矢量所决定的平面垂直的矢量,计算如下:
a
×
b
=
(
a
y
b
z
−
a
z
b
y
)
i
+
(
a
z
b
x
−
a
x
b
z
)
j
+
(
a
x
b
y
−
a
y
b
x
)
k
=
∣
i
j
k
a
x
a
y
a
z
b
x
b
y
b
z
∣
\begin{aligned} \boldsymbol{a} \times \boldsymbol{b}\ &= \ (a_yb_z-a_zb_y)\boldsymbol{i}+(a_zb_x-a_xb_z)\boldsymbol{j}+(a_xb_y-a_yb_x)\boldsymbol{k}\ \\ &=\ \left|\begin{array}{c}\boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k} \\ a_x & a_y & a_z \\ b_x & b_y & b_z \end{array}\right| \end{aligned}
a×b = (aybz−azby)i+(azbx−axbz)j+(axby−aybx)k =
iaxbxjaybykazbz
其中,
i
\boldsymbol{i}
i,
j
\boldsymbol{j}
j,
k
\boldsymbol{k}
k为轴
x
x
x,
y
y
y,
z
z
z上的单位矢量.
3.2 齐次变换
变换式(11)对于点
B
p
^B\boldsymbol{p}
Bp而言是非齐次的,但可将其表示为等价的齐次变换形式:
[
A
p
1
]
=
[
B
A
R
A
p
B
o
0
1
]
[
B
p
1
]
\left[\begin{array}{c}{ }^{A} \boldsymbol{p} \\ \mathbf{1}\end{array}\right]=\left[\begin{array}{cc}{ }_{B}^{A} \boldsymbol{R} & { }^{A} \boldsymbol{p}_{B_{o}} \\ \mathbf{0} & \mathbf{1}\end{array}\right]\left[\begin{array}{c}{ }^{B} \boldsymbol{p} \\ \mathbf{1}\end{array}\right]
[Ap1]=[BAR0ApBo1][Bp1]
其中,
4
×
1
4\times1
4×1的列向量表示三维空间的点,称为齐次坐标,仍然记为
A
p
^A\boldsymbol{p}
Ap或
B
p
^B\boldsymbol{p}
Bp.可把上式写成如下矩阵形式:
A
p
=
B
A
T
B
p
^A\boldsymbol{p}\ =\ ^A_B\boldsymbol{T}\ ^B\boldsymbol{p}
Ap = BAT Bp
式中,齐次坐标
A
p
^A\boldsymbol{p}
Ap和
B
p
^B\boldsymbol{p}
Bp是
4
×
1
4\times1
4×1的列向量,与式(14)中的维数不同,加入了第4个元素
1
\ \bf{1}
1.齐次变换矩阵
B
A
T
^A_B\boldsymbol{T}
BAT是
4
×
4
4\times4
4×4的方阵,具有如下形式:
B
A
T
=
[
B
A
R
A
p
B
o
0
1
]
^A_B\boldsymbol{T}\ =\ \left[\begin{array}{c}^A_B\boldsymbol{R} & ^A\boldsymbol{p}_{B_o} \\ \bf{0} & \bf{1} \end{array}\right]
BAT = [BAR0ApBo1]
B
A
T
^A_B{T}
BAT综合地表示了平移变换和旋转变换.变换式(14)和式(11)是等价的.实质上,式(14)可写成
A
p
=
B
A
R
B
p
+
A
p
B
o
1
=
1
{}^{A} \boldsymbol{p}={ }_{B}^{A} \boldsymbol{R}\ \ { }^{B} \boldsymbol{p}+{ }^{A} \boldsymbol{p}_{B_{o}}\\ \mathbf 1=\mathbf 1
Ap=BAR Bp+ApBo1=1
需要注意的是,位置矢量
A
p
^A\boldsymbol{p}
Ap和
B
p
^B\boldsymbol{p}
Bp需要根据实际情况来选择到底用直角坐标还是齐次坐标.
3.3 平移齐次变换
空间某点由矢量
a
i
+
b
j
+
c
k
a\boldsymbol{i}+b\boldsymbol{j}+c\boldsymbol{k}
ai+bj+ck描述.此点可用平移齐次变换表示为
T
r
a
n
s
(
a
,
b
,
c
)
=
[
1
0
0
a
0
1
0
b
0
0
1
c
0
0
0
1
]
\rm{Trans}(\it a,b,c)\ = \ \left[\begin{array}{c}1&0&0&a \\ 0&1&0&b \\ 0&0&1&c \\ 0&0&0&1\end{array}\right]
Trans(a,b,c) =
100001000010abc1
其中,
T
r
a
n
s
\rm Trans
Trans表示平移变换.
对已知矢量
u
=
[
x
,
y
,
z
,
w
]
T
\boldsymbol{u}=[x,y,z,w]^{\rm T}
u=[x,y,z,w]T进行平移变换所得矢量
v
\boldsymbol{v}
v为
v
=
T
r
a
n
s
(
a
,
b
,
c
)
u
=
[
1
0
0
a
0
1
0
b
0
0
1
c
0
0
0
1
]
[
x
y
z
w
]
=
[
x
+
a
w
y
+
b
w
z
+
c
w
w
]
=
[
x
/
w
+
a
y
/
w
+
b
z
/
w
+
c
1
]
\begin{aligned} \boldsymbol{v}\ =\ \rm{Trans}(\it{a,b,c}) \boldsymbol{u}\ &=\ \left[\begin{array}{cccc} 1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1 \end{array}\right] \left[\begin{aligned}{}x \\y \\z \\w \end{aligned}\right]\ \\ &=\ \left[\begin{aligned}{} x+aw \\ y+bw \\ z+cw \\ w \end{aligned}\right]\ =\ \left[\begin{aligned}{} x/w+a \\ y/w+b \\ z/w+c \\ 1 \end{aligned}\right] \end{aligned}
v = Trans(a,b,c)u =
100001000010abc1
xyzw
=
x+awy+bwz+cww
=
x/w+ay/w+bz/w+c1
即可把此变换看做矢量
(
x
/
w
)
i
+
(
y
/
w
)
j
+
(
z
/
w
)
k
(x/w)\boldsymbol{i}+(y/w)\boldsymbol{j}+(z/w)\boldsymbol{k}
(x/w)i+(y/w)j+(z/w)k与矢量
a
i
+
b
j
+
c
k
a\boldsymbol{i}+b\boldsymbol{j}+c\boldsymbol{k}
ai+bj+ck之和.
注意:用非零常数乘以此变换矩阵,不改变该变换矩阵的特性.
3.4 旋转齐次变换
对应于轴
x
x
x,
y
y
y或
z
z
z作转角为
θ
\theta
θ的旋转变换,齐次旋转矩阵分别如下:
R
o
t
(
x
,
θ
)
=
[
1
0
0
0
0
c
θ
−
s
θ
0
0
s
θ
c
θ
0
0
0
0
1
]
R
o
t
(
y
,
θ
)
=
[
c
θ
0
s
θ
0
0
1
0
0
−
s
θ
0
c
θ
0
0
0
0
1
]
R
o
t
(
z
,
θ
)
=
[
c
θ
−
s
θ
0
0
s
θ
c
θ
0
0
0
0
1
0
0
0
0
1
]
\rm{Rot}(\it x,\theta)=\left[\begin{array}{ cccc}1 & 0 & 0 & 0 \\ {0} & c\theta & -s\theta & 0 \\ 0 & s\theta & c\theta & 0 \\ 0 & 0 & 0 & 1\end{array}\right] \ \\\ \rm{Rot}(\it y,\theta)=\left[\begin{array}{ cccc}c\theta & 0 & s\theta & 0 \\ {0} & 1 & 0 & 0 \\ -s\theta & 0 & c\theta & 0 \\ 0 & 0 & 0 & 1\end{array}\right]\ \\\ \rm{Rot}(\it z,\theta)=\left[\begin{array}{ cccc}c\theta & -s\theta & 0 & 0 \\ s\theta & c\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]
Rot(x,θ)=
10000cθsθ00−sθcθ00001
Rot(y,θ)=
cθ0−sθ00100sθ0cθ00001
Rot(z,θ)=
cθsθ00−sθcθ0000100001
其中,
R
o
t
\rm Rot
Rot表示旋转变换.
设点
u
=
u
x
i
+
u
y
j
+
u
z
k
\boldsymbol{u}=u_x\boldsymbol{i}+u_y\boldsymbol{j}+u_z\boldsymbol{k}
u=uxi+uyj+uzk,先对它进行绕轴
z
z
z旋转
θ
1
\theta_1
θ1的变换后得到
v
\boldsymbol{v}
v,即
v
=
R
o
t
(
z
,
θ
1
)
u
\boldsymbol{v}=\rm Rot(\it z,\theta_{\rm 1})\boldsymbol{u}
v=Rot(z,θ1)u,再进行绕轴
y
y
y旋转
θ
2
\theta_2
θ2的变换后得到
w
\boldsymbol{w}
w,即
w
=
R
o
t
(
y
,
θ
2
)
v
\boldsymbol{w}=\rm Rot(\it y,\theta_{\rm 2})\boldsymbol{v}
w=Rot(y,θ2)v.将两个变换组合在一起可得下式:
w
=
R
o
t
(
y
,
θ
2
)
R
o
t
(
z
,
θ
1
)
u
\boldsymbol{w}=\rm Rot(\it y,\theta_{\rm 2})\ \rm Rot(\it z,\theta_{\rm 1})\boldsymbol{u}
w=Rot(y,θ2) Rot(z,θ1)u
值得注意的是,如果改变旋转次序,先对它进行绕轴
y
y
y旋转
θ
2
\theta_2
θ2的变换后,再进行绕轴
z
z
z旋转
θ
1
\theta_1
θ1的变换后得到
w
′
\boldsymbol{w}^\prime
w′,即:
w
′
=
R
o
t
(
z
,
θ
1
)
R
o
t
(
y
,
θ
2
)
u
\boldsymbol{w}^\prime=\rm Rot(\it z,\theta_{\rm 1})\ \rm Rot(\it y,\theta_{\rm 2})\boldsymbol{u}
w′=Rot(z,θ1) Rot(y,θ2)u
由于矩阵乘法不具备交换性,所以
w
≠
w
′
\boldsymbol{w}\neq\boldsymbol{w}^\prime
w=w′.
变换矩阵的左乘和右乘的运动解释是不同的:变换顺序“从右向左”,指明运动是相对固定坐标系而言的;变换顺序“从左向右”,指明运动是相对运动坐标系而言的.
4 齐次变换的逆变换
给定坐标系{
A
A
A},{
B
B
B}和{
C
C
C},若已知{
B
B
B}相对{
A
A
A}的描述为
B
A
T
^A_B\boldsymbol{T}
BAT,{
C
C
C}相对{
B
B
B}的描述为
C
B
T
^B_C\boldsymbol{T}
CBT,则
B
p
=
C
B
T
C
p
^B\boldsymbol{p}\ =\ ^B_C\boldsymbol{T}\ ^C\boldsymbol{p}
Bp = CBT Cp
A p = B A T B p = B A T C B T C p ^A\boldsymbol{p}\ =\ ^A_B\boldsymbol{T}\ ^B\boldsymbol{p}\ =\ ^A_B\boldsymbol{T}\ ^B_C\boldsymbol{T}\ ^C\boldsymbol{p} Ap = BAT Bp = BAT CBT Cp
定义复合变换
C
A
T
=
B
A
T
C
B
T
^A_C\boldsymbol{T}\ =\ ^A_B\boldsymbol{T}\ ^B_C\boldsymbol{T}
CAT = BAT CBT
表示{
C
C
C}相对于{
A
A
A}的描述.根据式(19)可得
C
A
T
=
B
A
T
C
B
T
=
[
B
A
R
A
p
B
o
0
1
]
[
C
B
R
B
p
C
o
0
1
]
=
[
B
A
R
C
B
R
B
A
R
B
p
C
o
+
A
p
B
o
0
1
]
\begin{aligned} { }_{C}^{A} \boldsymbol{T}={ }_{B}^{A} \boldsymbol{T}\ { }_{C}^{B} \boldsymbol{T}&=\left[\begin{array}{cc}{ }_{B}^{A} \boldsymbol{R} & { }^{A} \boldsymbol{p}_{B_{o}} \\ \mathbf{0} & \mathbf{1}\end{array}\right] \left[\begin{array}{cc}_{C}^{B}\boldsymbol{R} & ^{B}\boldsymbol{p}_{C_{o}} \\ \mathbf{0} & \mathbf{1}\end{array}\right] \\ &=\left[\begin{array}{cc}{ }_{B}^{A} \boldsymbol{R}\ {}_{C}^{B}\boldsymbol{R} & { }_{B}^{A} \boldsymbol{R}\ {}_{}^{B}\boldsymbol{p}_{C_{o}} +{ }^{A} \boldsymbol{p}_{B_{o}} \\ \mathbf{0} & \mathbf{1}\end{array}\right] \end{aligned}
CAT=BAT CBT=[BAR0ApBo1][CBR0BpCo1]=[BAR CBR0BAR BpCo+ApBo1]
从坐标系{
B
B
B}相对坐标系{
A
A
A}的描述
B
A
T
^A_B\boldsymbol{T}
BAT,求得{
A
A
A}相对于{
B
B
B}的描述
A
B
T
^B_A\boldsymbol{T}
ABT,式齐次变换求逆问题.有以下两种方法:
- 一种是利用齐次变换矩阵的特点,简化矩阵求逆运算.
- 另一种是直接对 4 × 4 4\times4 4×4的齐次变换矩阵 B A T ^A_B\boldsymbol{T} BAT求逆.
下面先介绍第一种方法.
对于给定的
B
A
T
^A_B\boldsymbol{T}
BAT,求
A
B
T
^B_A\boldsymbol{T}
ABT,等价于给定
B
A
R
^A_B\boldsymbol{R}
BAR和
A
p
B
o
^A\boldsymbol{p}_{B_o}
ApBo,计算
A
B
R
^B_A\boldsymbol{R}
ABR和
B
p
A
o
^B\boldsymbol{p}_{A_o}
BpAo.利用旋转矩阵的正交性,可得
A
B
R
=
B
A
R
−
1
=
B
A
R
T
^B_A\boldsymbol{R}\ =\ ^A_B\boldsymbol{R}^{-1}\ =\ ^A_B\boldsymbol{R}^{\rm T}
ABR = BAR−1 = BART
再根据式(11),求原点
A
p
B
o
^A\boldsymbol{p}_{B_o}
ApBo在坐标系{
B
B
B}中的描述
B
(
A
p
B
o
)
=
A
B
R
A
p
B
o
+
B
p
A
o
^B(^A\boldsymbol{p}_{B_o})\ =\ ^B_A\boldsymbol{R}\ ^A\boldsymbol{p}_{B_o}\ +\ ^B\boldsymbol{p}_{A_o}
B(ApBo) = ABR ApBo + BpAo
B
(
A
p
B
o
)
^B(^A\boldsymbol{p}_{B_o})
B(ApBo)表示{
B
B
B}的原点相对于{
B
B
B}的描述,为
0
\ \boldsymbol{0}
0矢量,因而上式为
0
\ \boldsymbol{0}
0,可得
B
p
A
o
=
−
A
B
R
A
p
B
o
=
−
B
A
R
T
A
p
B
o
^B\boldsymbol{p}_{A_o}\ =-{}^B_A\boldsymbol{R}\ ^A\boldsymbol{p}_{B_o}\ =\ -{}^A_B\boldsymbol{R}^{\rm T}\ ^A\boldsymbol{p}_{B_o}
BpAo =−ABR ApBo = −BART ApBo
综上分析,并据式(32)和(34)经推算可得
A
B
T
=
[
B
A
R
T
−
B
A
R
T
A
p
B
o
0
1
]
=
B
A
T
−
1
^B_A\boldsymbol{T}\ =\ \left[\begin{array}{cc}^A_B\boldsymbol{R}^{\rm T} & -{}^A_B\boldsymbol{R}^{\rm T}\ ^A\boldsymbol{p}_{B_o} \\ \bf{0} & \mathbf{1} \end{array}\right]\ =\ ^A_B\boldsymbol{T}^{-1}
ABT = [BART0−BART ApBo1] = BAT−1
下面介绍第二种方法,直接对
4
×
4
4\times4
4×4的齐次变换矩阵求逆.
实际上,逆变换是由被变换了的坐标系变回原坐标系的一种变换,也就是参考坐标系对于被变换了的坐标系的描述.
一般情况下,已知变换
T
\boldsymbol{T}
T的各元素
T
=
[
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
]
\boldsymbol{T}\ =\ \left[\begin{array}{cccc} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1\end{array}\right]
T =
nxnynz0oxoyoz0axayaz0pxpypz1
则其逆变换为
T
−
1
=
[
n
x
n
y
n
z
−
p
⋅
n
o
x
o
y
o
z
−
p
⋅
o
a
x
a
y
a
z
−
p
⋅
a
0
0
0
1
]
\boldsymbol{T}^{-1} \ =\ \left[ \begin{array}{cccc} n_x & n_y & n_z & -\boldsymbol{p\cdot n} \\ o_x & o_y & o_z & -\boldsymbol{p\cdot o} \\ a_x & a_y & a_z & -\boldsymbol{p\cdot a} \\ 0 & 0 & 0 & 1 \end{array} \right]
T−1 =
nxoxax0nyoyay0nzozaz0−p⋅n−p⋅o−p⋅a1
式中,
p
\boldsymbol{p}
p,
n
\boldsymbol{n}
n,
o
\boldsymbol{o}
o和
a
\boldsymbol{a}
a是4个列矢量,分别称为原点矢量、法线矢量、方向矢量和接近矢量.