空间某点
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=BATBp 式中，齐次坐标
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=BARBp+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=100001000010abc1xyzw=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θ00001Rot(y,θ)=cθ0−sθ00100sθ0cθ00001Rot(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′.
给定坐标系{
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=CBTCp
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=BATBp=BATCBTCp
定义复合变换
C
A
T
=
B
A
T
C
B
T
^A_C\boldsymbol{T}\ =\ ^A_B\boldsymbol{T}\ ^B_C\boldsymbol{T}
CAT=BATCBT 表示{
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=BATCBT=[BAR0ApBo1][CBR0BpCo1]=[BARCBR0BARBpCo+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)=ABRApBo+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=−ABRApBo=−BARTApBo 综上分析，并据式(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−BARTApBo1]=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个列矢量，分别称为原点矢量、法线矢量、方向矢量和接近矢量.
