三维情况是上一节的二维情况（二维空间位姿描述）的延伸。在二维坐标系上增加一个额外的坐标轴即可，通常用z表示，它同时与x轴和y轴正交。z轴的方向服从右手规则，并构成右手坐标系。与各坐标轴平行的单位向量记作
x
^
\hat{x}
x^、
y
^
\hat{y}
y^和
z
^
\hat{z}
z^：
z
^
=
x
^
×
y
^
,
x
^
=
y
^
×
z
^
,
y
^
=
z
^
×
x
^
\hat{z}=\hat{x}\times\hat{y},\hat{x}=\hat{y}\times\hat{z},\hat{y}=\hat{z}\times\hat{x}
z^=x^×y^,x^=y^×z^,y^=z^×x^
坐标系中的一个点P可用其x，y和z的坐标值（x，y，z）或者一个约束向量表示：
P
=
x
x
^
+
y
y
^
+
z
z
^
P=x\hat{x}+y\hat{y}+z\hat{z}
P=xx^+yy^+zz^
图1 展示了一个相对于参考坐标系{A}的坐标系{B}。显然，{B}的原点通过向量t=（x,y,z）进行平移，然后再通过某种复杂的方式进行旋转。做法还是同前面一样，从任意一点P出发，然后再确定 A p {^A}p Ap和 B p {^B}p Bp之间的关系。仍然是两方面考虑：旋转和平移。
1.2.1 三维空间姿态描述
图1中显示了两个右手坐标系，其方向差异很大，将坐标系{A}旋转，直到与坐标系{B}方向一致。首先考虑绕单个坐标轴的旋转。下图所示显示了一个右手坐标系，以及它绕不同坐标轴旋转不同角度的情形。
但绕坐标轴连续旋转就没那么简单了，下图演示了一个坐标系按不同的顺序旋转的情形，可以看到虽然旋转角度相同，但最终的坐标系完全不同，它取决于旋转的顺序。
目前有很多种表示旋转的方法，后面将会介绍几种：正交旋转矩阵，欧拉和卡尔丹角，旋转轴与角度，以及单元四位数。他们都可以表示为向量或矩阵，即MATLAB的自然数据类型，或者表示为一个工具箱自定义的类。
1.2.1.1正交旋转矩阵
用相对于参考坐标系的坐标轴单位向量表示它们所在坐标系的方向。每个单位向量有三个元素，他们组成了
3
×
3
3\times3
3×3阶正交矩阵
A
R
B
{^A}R_B
ARB:
(
A
x
A
y
A
z
)
=
A
R
B
(
B
x
B
y
B
z
)
\begin{pmatrix} {^A}x \\ {^A}y \\ {^A}z\\ \end{pmatrix}={^A}R_B\begin{pmatrix} {^B}x \\ {^B}y \\ {^B}z\\ \end{pmatrix}
⎝⎛AxAyAz⎠⎞=ARB⎝⎛BxByBz⎠⎞
上式将一个相对坐标系{B}的向量旋转为相对于坐标系{A}的向量。矩阵R属于属于特殊三维正交群。它具有正交矩阵的性质，如
R
T
=
R
−
1
R^T=R^{-1}
RT=R−1以及
d
e
t
(
R
)
=
1
det(R)=1
det(R)=1。
分别绕x,y,z轴旋转
θ
\theta
θ角后的标准正交矩阵可表示为：
R
x
(
θ
)
=
(
1
0
0
0
cos
θ
−
sin
θ
0
sin
θ
cos
θ
)
R_x(\theta)=\begin{pmatrix} 1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta \end{pmatrix}
Rx(θ)=⎝⎛1000cosθsinθ0−sinθcosθ⎠⎞
R
y
(
θ
)
=
(
cos
θ
0
sin
θ
0
1
0
−
sin
θ
0
cos
θ
)
R_y(\theta)=\begin{pmatrix}\cos\theta &0 &\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{pmatrix}
Ry(θ)=⎝⎛cosθ0−sinθ010sinθ0cosθ⎠⎞
R
y
(
θ
)
=
(
cos
θ
−
sin
θ
0
sin
θ
cos
θ
0
0
0
1
)
R_y(\theta)=\begin{pmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1&\end{pmatrix}
Ry(θ)=⎝⎛cosθsinθ0−sinθcosθ0001⎠⎞
- MATLAB程序实现：
- 绕x轴旋转
>>R = rotx(pi/2); #绕x轴旋转π/2
>>trplot(R); #绘制相应的坐标系
>>tranimate(R); #旋转动画，展示了世界坐标系旋转到指定坐标系的过程
R =
1.0000 0 0
0 0.0000 -1.0000
0 1.0000 0.0000
2. 先绕x轴旋转，再绕y轴旋转。
>>R = rotx(pi/2) * roty(pi/2)
>>trplot(R)
R =
0.0000 0 1.0000
1.0000 0.0000 -0.0000
-0.0000 1.0000 0.0000
3. 如果颠倒以上旋转顺序，可以看出旋转的不可交换性
>>R = roty(pi/2) * rotx(pi/2)
R =
0.0000 1.0000 0.0000
0 0.0000 -1.0000
-1.0000 0.0000 0.0000
得到的结果完全不同
1.2.1.2 三角度表示法
欧拉旋转定理要求绕3个轴依次旋转，但不能绕同一轴连续旋转两次。旋转顺序分为两种：欧拉式和卡尔丹式。
- 欧拉式：绕一个特定的轴重复旋转，但不是连续的：XYX、XZX、YXY、YZY、ZXZ、或ZYZ。
- 卡尔丹式：绕三个不同的轴旋转：XYZ、XZY、YZX、YXZ、ZXY或ZYX。
一般来说，所有这些序列均被称为欧拉角，共有12种形式可供选择。
ZYZ序列的欧拉角表示为
R
=
R
z
(
ϕ
)
R
y
(
θ
)
R
z
(
ψ
)
(1)
R=R_z(\phi)R_y(\theta)R_z(\psi)\tag1
R=Rz(ϕ)Ry(θ)Rz(ψ)(1)
欧拉角是一个三维向量
Γ
=
(
ϕ
,
θ
,
ψ
)
\Gamma=(\phi,\theta,\psi)
Γ=(ϕ,θ,ψ)。
例 1：要计算
Γ
=
(
0.1
,
0.2
,
0.3
）
\Gamma=(0.1,0.2,0.3）
Γ=(0.1,0.2,0.3）的等价旋转矩阵，可以表示为：
>>R = rotz(0.1) * roty(0.2) * rotz(0.3)
>>R = eu12r(0.1, 0.2, 0.3) #两个表达都可
R =
0.9021 -0.3836 0.1977
0.3875 0.9216 0.0198
-0.1898 0.0587 0.9801
上述问题的逆命题就是找到给定旋转矩阵的欧拉角：
>>gamma = tr2eul(R)
gamma =
0.1000 0.2000 0.3000
但是，如果 θ \theta θ为负时，其反函数结果与原来的数值不同，它返回了一个正的 θ \theta θ值和两个不同的 ϕ 、 ψ \phi、\psi ϕ、ψ值，但这一组欧拉角对应的旋转矩阵仍与前一组是相同的，两组不同的欧拉角对应同一个旋转矩阵，说明从旋转矩阵到欧拉角的映射不是唯一的，而工具箱函数返回的角度始终为正。
>>R = eu12r(0.1, -0.2, 0.3)
R =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
#反函数结果为
>>tr2eul(R)
ans =
-3.0416 0.2000 -2.8416
>>eu12r(ans)
ans =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
>>R = eul2r(0.1, 0, 0.3)
R =
0.9211 -0.3894 0
0.3894 0.9211 0
0 0 1.0000
>>tr2eul(R)
ans =
0 0 0.4000
对于
θ
=
0
\theta=0
θ=0的情况，反函数返回的角度值与原值完全不同。在这种情况下，从式（1）得出的旋转矩阵是
R
=
R
z
(
ϕ
)
R
z
(
ψ
)
=
R
z
(
ϕ
+
ψ
)
R=R_z(\phi)R_z(\psi)=R_z(\phi+\psi)
R=Rz(ϕ)Rz(ψ)=Rz(ϕ+ψ)
由于
R
y
=
I
R_y=I
Ry=I，所以可简化为只是
ϕ
+
ψ
\phi+\psi
ϕ+ψ的一个函数。对于逆运算而言，只能确定这个和的值。要想得到其中每个值，只能按惯例取
ϕ
=
0
\phi=0
ϕ=0。
θ
=
0
\theta=0
θ=0的情况实际上是一个奇异点。
另一种广泛使用的旋转角顺序是横滚-俯仰-偏航角，即
R
=
R
x
(
θ
r
)
R
y
(
θ
p
)
R
z
(
θ
y
)
R=R_x(\theta_r)R_y(\theta_p)R_z(\theta_y)
R=Rx(θr)Ry(θp)Rz(θy)
它用于描述船舶、飞机和车辆的姿态时非常直观。横滚、俯仰和偏航（也称为倾斜、姿态和航向）是指分别绕x、y、z轴的旋转。这个xyz角序列，即专业上的卡尔丹角，也被称为泰特-布莱恩角或导航角。
例2：对于航空及地面车辆而言，通常定义x轴为前进的方向、z轴垂直向下、y轴指向右手方向。
>>R = rpy2r(0.1, 0.2, 0.3)
R =
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
>>gamma = tr2rpy(R) #反函数结果
gamma =
0.1000 0.2000 0.3000
1.2.1.3绕任意向量旋转
对于空间中的两个任意姿态的坐标系，总是可以找到一个旋转轴，使其中一个坐标系绕这个轴旋转可以和另一个坐标系的姿态重合。
>>R = rpy2r(0.1, 0.2, 0.3)
R =
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
>>[theta, v] = tr2angvec(R) #theta是旋转的角度大小，v是绕其旋转的向量
theta =
0.3655
v =
0.1886 0.5834 0.7900
>>[v, lambda] = eig(R) #求取R矩阵的特征值和特征向量
v =
0.6944 + 0.0000i 0.6944 + 0.0000i 0.1886 + 0.0000i
-0.0792 - 0.5688i -0.0792 + 0.5688i 0.5834 + 0.0000i
-0.1073 + 0.4200i -0.1073 - 0.4200i 0.7900 + 0.0000i
lambda =
0.9339 + 0.3574i 0.0000 + 0.0000i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.9339 - 0.3574i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i
一个正交矩阵总有一个实特征值
λ
=
1
\lambda=1
λ=1，以及一对共轭复特征值
λ
=
cos
θ
±
i
sin
θ
\lambda=\cos\theta\pm{i}\sin\theta
λ=cosθ±isinθ,其中
θ
\theta
θ代表旋转的角度。根据特征值和特征向量的定义有
R
v
=
λ
v
Rv=\lambda{v}
Rv=λv
其中，
v
v
v是对应于
λ
\lambda
λ的特征向量。当
λ
=
1
\lambda=1
λ=1时：
R
v
=
v
Rv=v
Rv=v
意味着对应的这个特征向量
v
v
v是不随旋转而改变的。这样的向量只有一个，而旋转就是以这个轴为向量发生的。上面的例子中第三个特征值等于1，所以旋转轴应是矩阵v中的第三列。
扫一扫
1134
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
