现在我们回到三维空间中相对位姿的表示,即两个坐标系之间位置和姿态的变化,如下图所示。在本文将介绍使用
4
×
4
4\times4
4×4齐次变换矩阵的方法表示旋转和平移。
使用一个齐次变换矩阵来表示旋转和转换,推导的方法与二维的类似在此基础上增加了
z
z
z轴,可写为:
(
A
x
A
y
A
z
1
)
=
(
A
R
B
t
0
1
×
3
1
)
(
B
x
B
y
B
z
1
)
\begin{pmatrix}{} {{}^Ax}\\ {{}^Ay}\\ {{}^Az}\\ 1 \end{pmatrix}=\begin{pmatrix}{} {{}^A{R_B}}&t\\ {{0_{1 \times 3}}}&1 \end{pmatrix}\begin{pmatrix}{} {{}^Bx}\\ {{}^By}\\ {{}^Bz}\\ 1 \end{pmatrix}
⎝⎜⎜⎛AxAyAz1⎠⎟⎟⎞=(ARB01×3t1)⎝⎜⎜⎛BxByBz1⎠⎟⎟⎞
坐标系原点之间的笛卡尔平移向量是
t
t
t,姿态的变化由一个
3
×
3
3 \times 3
3×3正交子矩阵
R
R
R表示,其余向量都表示为齐次形式,可以写成
A
p
~
=
(
A
R
B
t
0
1
×
3
1
)
B
p
~
=
A
T
B
B
p
~
{}^A\tilde p = {\begin{pmatrix}{} {{}^A{R_B}}&t\\ {{0_{1 \times 3}}}&1 \end{pmatrix}} {}^B\tilde p = {}^A{T_B}{}^B\tilde p
Ap~=(ARB01×3t1)Bp~=ATBBp~
A
T
B
{}^A{T_B}{}
ATB是一个
4
×
4
4 \times 4
4×4阶齐次变换矩阵。
>>T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)
>T= 1.0000 0 0 1.0000
0 0.0000 -1.0000 0.0000
0 1.0000 0.0000 1.0000
0 0 0 1.0000
函数 t r a n s l transl transl创建了一个有平移但无旋转的相对位姿,而函数 t r o t x trotx trotx则返回一个绕 x x x轴旋转 p i / 2 pi/2 pi/2的 4 × 4 4 \times 4 4×4齐次变换矩阵:旋转部分与 r o t x ( p i / 2 ) rotx(pi/2) rotx(pi/2)相同,平移部分为零。可以将以上函数组合对应的坐标系变化描述如下:首先沿着 x x x轴方向前进一个单位长度,然后绕 x x x轴旋转 9 0 ∘ {90^ \circ } 90∘,接着沿新的 y y y轴,也就是原来的 z z z轴前进一个单位长度。所得矩阵的最后一列,表示了沿原坐标系的 x x x轴和 z z z轴各平移一个单位长度的结果。从最终位姿矩阵的姿态部分,则可以看出是绕 x x x轴旋转的结果。可以用以下的函数绘制相应的坐标系:
>>trplot(T) #建立的坐标系
>>t2r(T) #旋转部分
>>transl(T)' #平移部分
所建立的坐标系如下图所示:
前面几节讲述了二维和三维世界中表示点和位姿,以及用正交旋转矩阵表示姿态,并用它的延伸——齐次变换矩阵表示姿态和平移。