旋转矩阵、欧拉角、轴角、四元数
旋转矩阵、欧拉角、四元数
【如果有错的地方记得提醒我，感谢】
目录
- 旋转矩阵、欧拉角、四元数
- 【如果有错的地方记得提醒我，感谢】
-
- 一、左右手坐标系
- 二、旋转矩阵
-
- 2.1 什么是旋转矩阵？
- 2.2 旋转矩阵需要满足什么要求？
- 2.3 围绕x、y、z轴的旋转矩阵以及组合方式：
- 2.4 旋转矩阵的问题
- 三、欧拉角
-
- 3.1 什么是欧拉角？
- 3.2 欧拉角有不同的定义方式：
- 3.3 常用的轴顺序：
- 3.4 万向节死锁：
- 3.5 欧拉角和旋转矩阵转换：
- 3.6 code:
- 3.7 欧拉角的问题
- 四、轴角
-
- 4.1 什么是轴角？
- 4.2 轴角如何表示旋转？
- 4.3 轴角和旋转矩阵转换：
- 4.4 轴角的优缺点：
- 五、四元数
-
- 5.1 什么是四元数？
- 5.2 单位四元数
- 5.3 四元数和旋转矩阵之间的转换
-
- 5.3.1 四元数转旋转矩阵：
- 5.3.2 旋转矩阵转四元数
- 5.4 四元数优缺点
一、左右手坐标系
主要各种相机传感器坐标方向其实都不太一致，左手坐标系旋转90度还是左手坐标系，这里分左右手【关注的是三个坐标轴组合方式】。
可以对照以上两张图自己转一下，图一来自网络，图二来自《视觉slam十四讲》。
二、旋转矩阵
2.1 什么是旋转矩阵？
旋转矩阵：在乘以一个向量的时候有改变向量的方向但不改变大小的效果并保持了手性的矩阵。
刚体上的任意一个点P(x1, y1, z1)绕过原点的轴(i, j, k)旋转θ，旋转后的点为P’(x2, y2, z2)【这里用左乘，以绝对参考系为参照】:
2.2 旋转矩阵需要满足什么要求？
一个矩阵是旋转矩阵，当且仅当它是正交矩阵并且它的行列式是单位一。
正交矩阵的行列式是 ±1；如果行列式是 −1，则它包含了一个反射而不是真旋转矩阵。
2.3 围绕x、y、z轴的旋转矩阵以及组合方式：
绕x、y、z轴旋转矩阵如下：
围绕任意轴的旋转可以表示为：
R = R x ( − p ) ∗ R y ( − q ) ∗ R z ( r ) ∗ R y ( q ) ∗ R x ( p ) R = R_x(-p) * R_y(-q) * R_z(r) * R_y(q) * R_x(p) R=Rx(−p)∗Ry(−q)∗Rz(r)∗Ry(q)∗Rx(p)
绕x轴旋转角度p使指定的旋转轴在xz平面上
绕y轴旋转角度q使指定的旋转轴与z轴重合
绕z轴旋转角度θ
绕y轴旋转角度-q
绕x轴旋转角度-p
其中，p和q的值需要用i,j,k计算出来
参考：
旋转矩阵
百度百科
2.4 旋转矩阵的问题
用九个量描述三自由度的旋转，具有冗余性；
三、欧拉角
3.1 什么是欧拉角？
欧拉角：物体绕坐标系三个坐标轴(x,y,z轴）的旋转角度。
3.2 欧拉角有不同的定义方式：
轴顺序不同：
经典欧拉角(Proper Euler Angle)：一三旋转轴相同: zxz, xyx, yzy, zyz, xzx, yxy
泰特布莱恩角(Tait–Bryan angles)：三个旋转轴都不同:xyz, yzx, zxy, xzy, zyx, yxz
坐标轴不同：
外旋：绕世界坐标系的XYZ轴旋转，旋转过程中轴不会变；【左乘】
内旋：绕物体坐标系的XYZ轴旋转，旋转过程中轴会变化。【右乘】
3.3 常用的轴顺序：
常用ZYX 轴顺序表示，“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。
1)绕物体的 Z 轴旋转，得到偏航角 yaw；
2)绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
3)绕旋转之后的 X 轴旋转，得到滚转角 roll。
3.4 万向节死锁：
一段视频：万向节死锁演示
俯仰角为±90◦ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。
【只要用三个实数来表达三维旋转时，都有奇异性问题。】
由于这种原理，欧拉角【不适于插值和迭代】，往往只用于人机交互中。
3.5 欧拉角和旋转矩阵转换：
欧拉角转旋转矩阵：
R x ( θ ) = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R_{x}(\theta)=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta\end{array}\right] Rx(θ)=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤
R y ( θ ) = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_{y}(\theta)=\left[\begin{array}{ccc}\cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta\end{array}\right] Ry(θ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤
R z ( θ ) = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R_{z}(\theta)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1\end{array}\right] Rz(θ)=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤
R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] = [ c y c z c z s x s y − c x s z s x s z + c x c z s y c y s z c x c z + s x s y s z c x s y s z − c z s z − s y c y s x c x c y ] R=\left[\begin{array}{lll}r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33}\end{array}\right]=\left[\begin{array}{ccc}c_{y} c_{z} & c_{z} s_{x} s_{y}-c_{x} s_{z} & s_{x} s_{z}+c_{x} c_{z} s_{y} \\ c_{y} s_{z} & c_{x} c_{z}+s_{x} s_{y} s_{z} & c_{x} s_{y} s_{z}-c_{z} s_{z} \\ -s_{y} & c_{y} s_{x} & c_{x} c_{y}\end{array}\right] R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤=⎣⎡cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czszcxcy⎦⎤
旋转矩阵转欧拉角：
θ x = atan 2 ( r 32 , r 33 ) \theta_{x}=\operatorname{atan} 2\left(r_{32}, r_{33}\right) θx=atan2(r32,r33)
θ y = atan 2 ( − r 31 , r 32 2 + r 33 2 ) \theta_{y}=\operatorname{atan} 2\left(-r_{31}, \sqrt{r_{32}^{2}+r_{33}^{2}}\right) θy=atan2(−r31,r322+r332)
θ z = atan 2 ( r 21 , r 11 ) \theta_{z}=\operatorname{atan} 2\left(r_{21}, r_{11}\right) θz=atan2(r21,r11)
3.6 code:
代码1：基于numpy
def isRotationMatrix(R) :
Rt = np.transpose(R)
shouldBeIdentity = np.dot(Rt, R)
I = np.identity(3, dtype = R.dtype)
n = np.linalg.norm(I - shouldBeIdentity)
return n < 1e-6
def rotationMatrixToEulerAngles(R) :
assert(isRotationMatrix(R))
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular :
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
return np.array([x, y, z])
def eulerAnglesToRotationMatrix(theta, format='degree'):
"""
Calculates Rotation Matrix given euler angles.
:param theta: 1-by-3 list [rx, ry, rz] angle in degree
:return:
RPY角，是ZYX欧拉角，依次 绕定轴XYZ转动[rx, ry, rz]
"""
if format is 'degree':
theta = [i * math.pi / 180.0 for i in theta]
R_x = np.array([[1, 0, 0],
[0, math.cos(theta[0]), -math.sin(theta[0])],
[0, math.sin(theta[0]), math.cos(theta[0])]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
[0, 1, 0],
[-math.sin(theta[1]), 0, math.cos(theta[1])]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot(R_y, R_x))
return R
3.7 欧拉角的问题
欧拉角的表示方式不唯一；
万向锁问题，不适于插值和迭代；
运算时多乘两个矩阵好累的，转换成旋转矩阵计算量较大；
三角函数在电脑中运算也都只是近似解，误差可能会叠加。
四、轴角
4.1 什么是轴角？
用两个值参数化了旋转: 一个轴或直线，和描述绕这个轴的旋转量的一个角。它也叫做旋转的指数坐标。
4.2 轴角如何表示旋转？
【用4个参数就可以表示一个轴角】
假如你站在地面上，选取重力的方向为负 z 方向。如果你左转，你将绕 z 轴旋转弧度π/2 (或 90 度)。
轴角表示为：
< a x i s , a n g l e > = ( [ a x a y a z ] , θ ) = ( [ 0 0 1 ] , π 2 ) < axis, angle > =\left(\left[\begin{array}{l}a_{x} \\ a_{y} \\ a_{z}\end{array}\right], \theta\right)=\left(\left[\begin{array}{l}0 \\ 0 \\ 1\end{array}\right], \frac{\pi}{2}\right) <axis,angle>=⎝⎛⎣⎡axayaz⎦⎤,θ⎠⎞=⎝⎛⎣⎡001⎦⎤,2π⎠⎞
这可以表示为指示 z 方向的模为π/2 的旋转向量：
[ 0 0 x 2 ] \left[\begin{array}{l}0 \\ 0 \\ \frac{x}{2}\end{array}\right] ⎣⎡002x⎦⎤
4.3 轴角和旋转矩阵转换：
- 轴角转为旋转矩阵：
令r为参考坐标系O-xyz的旋转轴的单位向量。
绕轴r旋转角度θ的旋转矩阵为R(θ,r)，可以通过绕参考坐标系的三个坐标轴的基本旋转进行合成。
对应给定角度和轴的旋转矩阵为：
R ( θ , r ) = [ r x 2 ( 1 − cos θ ) + cos θ r x r y ( 1 − cos θ ) − r z sin θ r x r z ( 1 − cos θ ) + r y sin θ r x r y ( 1 − cos θ ) + r z sin θ r y 2 ( 1 − cos θ ) + cos θ r y r z ( 1 − cos θ ) − r x θ θ r x r z ( 1 − cos θ ) − r y sin θ r y r z ( 1 − cos θ ) + r x sin θ r z 2 ( 1 − cos θ ) + cos θ ] R(\theta, r)=\left[\begin{array}{ccc}r_{x}^{2}(1-\cos \theta)+\cos \theta & r_{x} r_{y}(1-\cos \theta)-r_{z} \sin \theta & r_{x} r_{z}(1-\cos \theta)+r_{y} \sin \theta \\ r_{x} r_{y}(1-\cos \theta)+r_{z} \sin \theta & r_{y}^{2}(1-\cos \theta)+\cos \theta & r_{y} r_{z}(1-\cos \theta)-r_{x} \theta \theta \\ r_{x} r_{z}(1-\cos \theta)-r_{y} \sin \theta & r_{y} r_{z}(1-\cos \theta)+r_{x} \sin \theta & r_{z}^{2}(1-\cos \theta)+\cos \theta\end{array}\right] R(θ,r)=⎣⎡rx2(1−cosθ)+cosθrxry(1−cosθ)+rzsinθrxrz(1−cosθ)−rysinθrxry(1−cosθ)−rzsinθry2(1−cosθ)+cosθryrz(1−cosθ)+rxsinθrxrz(1−cosθ)+rysinθryrz(1−cosθ)−rxθθrz2(1−cosθ)+cosθ⎦⎤
- 旋转矩阵转为轴角：
给定旋转矩阵R，将其转化为 θ \theta θ 和 r r r 的方式如下：
R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\left[\begin{array}{lll}r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33}\end{array}\right] R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
θ = arccos ( r 11 + r 22 + r 33 − 1 2 ) \theta=\arccos \left(\frac{r_{11}+r_{22}+r_{33}-1}{2}\right) θ=arccos(2r11+r22+r33−1)
r = 1 2 sin θ [ r 32 − r 23 r 13 − r 31 r 21 − r 12 ] r=\frac{1}{2 \sin \theta}\left[\begin{array}{l}r_{32}-r_{23} \\ r_{13}-r_{31} \\ r_{21}-r_{12}\end{array}\right] r=2sinθ1⎣⎡r32−r23r13−r31r21−r12⎦⎤
4.4 轴角的优缺点：
和欧拉角不同，轴角只需要找到一根旋转轴，并且只需要通过绕这根轴旋转一次，没有万向节死锁现象；
当 θ \theta θ 为 0 或者 π 的时候会出现奇点。
五、四元数
5.1 什么是四元数？
四元数是简单的超复数。 【复数是由实数加上虚数单位 i 组成，其中i^2 = -1】
四元数是 1、i、j 和 k 的线性组合，即是四元数一般可表示为a + bi+ cj + dk，其中a、b、c 、d是实数。
i 2 = j 2 = k 2 = − 1 i^2=j^2=k^2=-1 i2=j2=k2=−1
i 0 = j 0 = k 0 = 1 i^0 = j^0 = k^0 = 1 i0=j0=k0=1
i j = k 、 j i = − k 、 j k = i 、 k j = − i 、 k i = j 、 i k = − j ij=k、ji=-k、jk=i、kj=-i、ki=j、ik=-j ij=k、ji=−k、jk=i、kj=−i、ki=j、ik=−j
四元数的模：（a2+b2+c2+d2）的平方根.
对于i、j、k本身的几何意义可以理解为一种旋转;
其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转;
j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转;
k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转;
-i、-j、-k分别代表i、j、k旋转的反向旋转。
5.2 单位四元数
- 单位向量(x,y,z)旋转θ角度后的四元数:
( cos θ 2 , x ∗ sin θ 2 , y ∗ sin θ 2 , z ∗ sin θ 2 ) \left(\cos \frac{\theta}{2}, x * \sin \frac{\theta}{2}, y * \sin \frac{\theta}{2}, z * \sin \frac{\theta}{2}\right) (cos2θ,x∗sin2θ,y∗sin2θ,z∗sin2θ)
- 从单位四元数中计算出旋转轴和夹角：
θ = 2 arccos q 0 \theta=2 \arccos q_{0} θ=2arccosq0
[ x , y , z ] T = [ q 1 , q 2 , q 3 ] T / sin θ 2 \left[x, y, z\right]^{T}=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2} [x,y,z]T=[q1,q2,q3]T/sin2θ
5.3 四元数和旋转矩阵之间的转换
5.3.1 四元数转旋转矩阵：
-
假设四元数为 q = q 0 + q 1 ∗ i + q 2 ∗ j + q 3 ∗ k q=q_0+q_1*i+q_2*j+q_3*k q=q0+q1∗i+q2∗j+q3∗k，则对应旋转矩阵为
R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R=\left[\begin{array}{ccc}1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}-2 q_{0} q_{3} & 2 q_{1} q_{3}+2 q_{0} q_{2} \\ 2 q_{1} q_{2}+2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} \\ 2 q_{1} q_{3}-2 q_{0} q_{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2}\end{array}\right] R=⎣⎡1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22⎦⎤ -
code
def quaternion_to_rotation_matrix(quat):
q = quat.copy()
n = np.dot(q, q)
if n < np.finfo(q.dtype).eps:
return np.identity(4)
q = q * np.sqrt(2.0 / n)
q = np.outer(q, q)
rot_matrix = np.array(
[[1.0 - q[2, 2] - q[3, 3], q[1, 2] + q[3, 0], q[1, 3] - q[2, 0], 0.0],
[q[1, 2] - q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] + q[1, 0], 0.0],
[q[1, 3] + q[2, 0], q[2, 3] - q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
[0.0, 0.0, 0.0, 1.0]],
dtype=q.dtype)
return rot_matrix
5.3.2 旋转矩阵转四元数
5.4 四元数优缺点
Hamilton 找到的一种扩展的复数，它既是紧凑的，又没有奇异性。
四元数不够直观，其运算稍为复杂一些。
https://zhuanlan.zhihu.com/p/45404840
https://zhuanlan.zhihu.com/p/144032401
https://blog.csdn.net/lql0716/article/details/72597719
https://www.cnblogs.com/flyinggod/p/8144100.html
https://www.zhihu.com/question/23005815
https://blog.csdn.net/linuxheik/article/details/78842428
https://www.zhihu.com/question/47736315
https://blog.csdn.net/qq_21834027/article/details/85144454
https://blog.csdn.net/Sandy_WYM_/article/details/84309000
https://www.cnblogs.com/jins-note/p/9512728.html
《视觉slam十四讲》