前言

研究旋转矩阵，旋转向量，单位四元数，都是为了表达机器人的姿态。欧拉角在SLAM的应用中不多，就不涉及了。

一、要点

1. 旋转矩阵

旋转矩阵是行列式为1的正交矩阵，由单位正交基组成。
正交矩阵的行列式为正负1
旋转矩阵是两个坐标系的过渡矩阵，其左乘坐标系1中的某向量的坐标，可以得到将此向量过渡到坐标系2后的坐标表示。
n维空间的旋转矩阵构成特殊正交群。
优点：是计算非常方便，矩阵乘法非常简单。
缺点：1.表达方式冗余，旋转只有3个自由度，却要用9个量表示。2.带有正交，行列式为1的约束，不利于优化。

2. 旋转向量

旋转向量又称为轴角，其方向与旋转轴n\mathbf nn一致，而长度等于旋转角θ\mathit \thetaθ。向量θn\mathit \theta \mathbf nθn即可表示旋转。
不严谨的说，这玩意就是李代数。
优点：表达紧凑；没有约束易于优化。
缺点：不能直接用来计算旋转一个向量，需要转换为旋转矩阵或四元数才能做这个计算；具有奇异性，模长大于2π2 \pi2π时具有周期性。
所谓奇异性，我理解的就是做不到一一对应，在这里是一个姿态可以对应多个旋转向量

3. 单位四元数

四元数的运算基本与复数的运算是类似的，唯一的区别是四元数的乘法不可交换（由于乘法公式的最后项有个叉乘）。
用四元数表示旋转的三个要点：
- 实部唯一表示了旋转角；
- 虚部所指方向即旋转轴的方向，（注意：虚部在R3\mathbb R^3R3里不需要是一个单位向量）；
- 实部和虚部一起要服从是单位四元数的约束，即模为1。
单位四元数的逆等于其共轭。
优点：没有奇异性，将要旋转的向量变成纯虚数后可以直接用四元数乘法计算。
缺点：不够直观。

二、旋转向量—>旋转矩阵（罗德里格斯公式）

在上图中，向量v\mathbf vv沿着旋转轴n\mathbf nn旋转θ\mathit \thetaθ角，变为v′\mathbf v'v′。现在我们要将v′\mathbf v'v′分解成向量v\mathbf vv，旋转轴n\mathbf nn，旋转角θ\mathit \thetaθ组成的量
v′=vk+vp′=vk+a+b=(v⋅n)n+∣vi∣sin⁡θj+vicos⁡θ=(v⋅n)n+∣(n×v)×n∣sin⁡θn×v∣n×v∣+((n×v)×n)cos⁡θ=(v⋅n)n+∣n∣∣v∣sin⁡α∣n∣sin⁡π2sin⁡θn×v∣n∣∣v∣sin⁡α+((n×v)×n)cos⁡θ=(v⋅n)n+(n×v)sin⁡θ+(v−(v⋅n)n)cos⁡θ=(1−cos⁡θ)(v⋅n)n+n∧vsin⁡θ+vcos⁡θ=(1−cos⁡θ)nnTv+sin⁡θn∧v+cos⁡θv=[cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧]v\begin{split} \mathbf v' & = \mathbf v_k + \mathbf v'_p \\ & = \mathbf v_k + \mathbf a + \mathbf b \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + | \mathbf v_i|\sin\theta \mathbf j + \mathbf v_i \cos \theta \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + |(\mathbf n \times \mathbf v) \times \mathbf n | \sin \theta \frac {\mathbf n \times \mathbf v}{|\mathbf n \times \mathbf v|} + ((\mathbf n \times \mathbf v) \times \mathbf n)\cos \theta \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + |\mathbf n| |\mathbf v|\sin \alpha |\mathbf n| \sin \frac{\pi}{2} \sin\theta \frac{\mathbf n \times \mathbf v}{|\mathbf n| |\mathbf v| \sin \alpha} + ((\mathbf n \times \mathbf v) \times \mathbf n)\cos \theta \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + (\mathbf n \times \mathbf v) \sin \theta + (\mathbf v - (\mathbf v \cdot \mathbf n ) \mathbf n) \cos \theta \\ & = (1 - \cos \theta) (\mathbf v \cdot \mathbf n) \mathbf n + \mathbf n^{\land} \mathbf v \sin \theta + \mathbf v \cos \theta \\ & = (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} \mathbf v + \sin \theta \mathbf n^{\land} \mathbf v + \cos \theta \mathbf v \\ & = [\cos \theta \mathbf I + (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} + \sin \theta \mathbf n^{\land}] \mathbf v \end{split} v′=vk+vp′=vk+a+b=(v⋅n)n+∣vi∣sinθj+vicosθ=(v⋅n)n+∣(n×v)×n∣sinθ∣n×v∣n×v+((n×v)×n)cosθ=(v⋅n)n+∣n∣∣v∣sinα∣n∣sin2πsinθ∣n∣∣v∣sinαn×v+((n×v)×n)cosθ=(v⋅n)n+(n×v)sinθ+(v−(v⋅n)n)cosθ=(1−cosθ)(v⋅n)n+n∧vsinθ+vcosθ=(1−cosθ)nnTv+sinθn∧v+cosθv=[cosθI+(1−cosθ)nnT+sinθn∧]v
要注意点乘得到的是标量；叉乘得到的是向量，且要用右手定则分析其方向。

(v⋅n)n=nnTv(\mathbf v \cdot \mathbf n) \mathbf n = \mathbf n \mathbf n^{\mathrm T} \mathbf v(v⋅n)n=nnTv可以从微观层面去证明，很简单。
上面的计算有很多细节值得思考，比如第三行的∣vi∣sin⁡θj| \mathbf v_i|\sin\theta \mathbf j∣vi∣sinθj为什么要乘单位向量j\mathbf jj？vicos⁡θ\mathbf v_i \cos \thetavicosθ为什么又不乘？

上式中中括号里的内容是一个矩阵，就是旋转矩阵R\mathbf RR：
R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧\begin{equation} \mathbf R = \cos \theta \mathbf I + (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} + \sin \theta \mathbf n^{\land} \end{equation} R=cosθI+(1−cosθ)nnT+sinθn∧

三、旋转矩阵—>旋转向量

将(1)式两边取迹：
tr(R)=3cos⁡θ+(1−cos⁡θ)nTn+sin⁡θ⋅0=3cos⁡θ+1−cos⁡θ=2cos⁡θ+1\begin{align} \mathrm tr(\mathbf R) & = 3 \cos \theta + (1 - \cos \theta) \mathbf n^{\mathrm T} \mathbf n + \sin \theta \cdot 0 \notag \\ & = 3 \cos \theta + 1 - \cos \theta \notag \\ & = 2 \cos \theta + 1 \end{align} tr(R)=3cosθ+(1−cosθ)nTn+sinθ⋅0=3cosθ+1−cosθ=2cosθ+1
从旋转矩阵中得到旋转角：
θ=arccos⁡tr(R)−12\theta = \arccos \frac {\mathrm tr(\mathbf R) - 1} {2} θ=arccos2tr(R)−1
至于转轴n\mathbf nn，把它看作普通的向量，用它对应的旋转矩阵R\mathbf RR去左乘旋转它，它不会发生任何变化，于是有下式：
Rn=n\mathbf R \mathbf n =\mathbf n Rn=n
因此，转轴n\mathbf nn是矩阵R\mathbf RR的特征值1对应的特征向量，用求特征向量的方法去求即可，再归一化可以得到转轴n\mathbf nn:
Rn=ns.t.nTn=1\mathbf R \mathbf n = \mathbf n \qquad \textit{s.t.} \qquad \mathbf n^{\mathrm T} \mathbf n = 1 Rn=ns.t.nTn=1

四、单位四元数—>旋转矩阵

用四元数q\mathbf qq对纯虚四元数数表示的空间点p=[0,vpT]T\mathbf p= [0,\mathbf v_p^{\mathrm T}]^{\mathrm T}p=[0,vpT]T进行旋转有：
p′=qpq−1=q+p+q−1=q+(q−1)⊕p=[s−vTvsI+v∧][svT−vsI+v∧][0vp]=[s2+vTvsvT−svT−vTv∧sv−sv−v∧vvvT+s2I+2sv∧+(v∧)2][0vp]=[1(v∧v)T0vvT+s2I+2sv∧+(v∧)2][0vp]=[10T0vvT+s2I+2sv∧+(v∧)2][0vp]=[0(vvT+s2I+2sv∧+(v∧)2)vp]\begin{split} \mathbf p^{'} & = \mathbf q \mathbf p \mathbf q^{-1} \\ & = \mathbf q^{+} \mathbf p^{+} \mathbf q^{-1} \\ & = \mathbf q^{+} (\mathbf q^{-1})^{\oplus} \mathbf p \\ & = \begin{bmatrix} s & -\mathbf v^{\mathrm T} \\ \mathbf v & s \mathbf I + \mathbf v^{\land} \end{bmatrix} \begin{bmatrix} s & \mathbf v^{\mathrm T} \\ -\mathbf v & s \mathbf I + \mathbf v^{\land} \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} s^2 + \mathbf v^{\mathrm T} \mathbf v & s\mathbf v^{\mathrm T} -s\mathbf v^{\mathrm T} - \mathbf v^{\mathrm T} \mathbf v^{\land} \\ s\mathbf v - s\mathbf v - \mathbf v^{\land} \mathbf v& \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} 1 & (\mathbf v^{\land} \mathbf v)^{\mathrm T} \\ \mathbf 0 & \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} 1 & \mathbf 0^{\mathrm T} \\ \mathbf 0 & \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} 0 \\ (\mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2) \mathbf v_{p}\end{bmatrix} \end{split} p′=qpq−1=q+p+q−1=q+(q−1)⊕p=[sv−vTsI+v∧][s−vvTsI+v∧][0vp]=[s2+vTvsv−sv−v∧vsvT−svT−vTv∧vvT+s2I+2sv∧+(v∧)2][0vp]=[10(v∧v)TvvT+s2I+2sv∧+(v∧)2][0vp]=[100TvvT+s2I+2sv∧+(v∧)2][0vp]=[0(vvT+s2I+2sv∧+(v∧)2)vp]
上面可以看出被单位四元数旋转后的坐标点(纯虚四元数)依然是一个纯虚四元数
由此可得单位四元数到旋转矩阵的变换：
R=vvT+s2I+2sv∧+(v∧)2\begin{equation} \mathbf R = \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{equation} R=vvT+s2I+2sv∧+(v∧)2

五、旋转矩阵—>单位四元数

将(3)式两边取迹：
tr(R)=vTv+3s2+0+tr((v∧)2)\mathrm tr(\mathbf R) = \mathbf v^{\mathrm T} \mathbf v + 3s^2 + 0 + \mathrm tr((\mathbf v^{\land})^2) tr(R)=vTv+3s2+0+tr((v∧)2)

其中： (v∧)2=[0−v3v2v30−v1−v2v10][0−v3v2v30−v1−v2v10]=[−v22−v32⋯⋯⋯−v12−v32⋯⋯⋯−v12−v22]\begin{split} (\mathbf v^{\land})^2 & = \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix} \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix} \\ & = \begin{bmatrix} -v_2^2 - v_3^2 & \cdots & \cdots \\ \cdots & -v_1^2-v_3^2 & \cdots \\ \cdots & \cdots & -v_1^2-v_2^2 \end{bmatrix} \end{split} (v∧)2=⎣⎡0v3−v2−v30v1v2−v10⎦⎤⎣⎡0v3−v2−v30v1v2−v10⎦⎤=⎣⎡−v22−v32⋯⋯⋯−v12−v32⋯⋯⋯−v12−v22⎦⎤

tr(R)=v12+v22+v32+3s2−2(v12+v22+v32)=3s2−(v12+v22+v32)=3s2−(1−s2)=4s2−1\begin{align} \mathrm tr(\mathbf R) & = v_1^2 + v_2^2 + v_3^2 + 3s^2 - 2(v_1^2 + v_2^2 + v_3^2) \notag \\ & = 3s^2 - (v_1^2 + v_2^2 + v_3^2) \notag \\ & = 3s^2 - (1 - s^2) \notag \\ & = 4s^2 - 1 \end{align} tr(R)=v12+v22+v32+3s2−2(v12+v22+v32)=3s2−(v12+v22+v32)=3s2−(1−s2)=4s2−1
稍作变换(4)式可以从旋转矩阵R\mathbf RR中求得单位四元数的实部：
s=tr(R)+14s = \sqrt {\frac {\mathrm tr(\mathbf R) +1}{4}} s=4tr(R)+1

下面开始求虚部，将虚部当作待旋转的坐标点带入(3)式有：
Rv=(vvT+s2I+2sv∧+(v∧)2)v=vvTv+s2v+0+0=(1−s2)v+s2v=v\begin{split} \mathbf R \mathbf v & = (\mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2) \mathbf v \\ & = \mathbf v \mathbf v^{\mathrm T} \mathbf v + s^2 \mathbf v + 0 + 0 \\ & = (1 - s^2) \mathbf v + s^2 \mathbf v \\ & = \mathbf v \end{split} Rv=(vvT+s2I+2sv∧+(v∧)2)v=vvTv+s2v+0+0=(1−s2)v+s2v=v

由此可见虚部也是矩阵R\mathbf RR的特征值1对应的特征向量，即与旋转向量θn\theta \mathbf nθn在R3\mathbb R^3R3中指向同一个方向，但是他们之间的模不一样，旋转向量的模是量转角θ\thetaθ，这里虚部的模由s2+vTv=1s^2 + \mathbf v^{\mathrm T} \mathbf v = 1s2+vTv=1确定。故由旋转矩阵求虚部为
Rv=vs.t.s2+vTv=1\mathbf R \mathbf v = \mathbf v \qquad \textit{s.t.} \qquad s^2 + \mathbf v^{\mathrm T} \mathbf v = 1 Rv=vs.t.s2+vTv=1

六、单位四元数<—>旋转向量

由式(2)与式(4)可得：
tr(R)=2cos⁡θ+1=4s2−1\mathrm tr(\mathbf R) = 2 \cos \theta +1 = 4s^2 -1 tr(R)=2cosθ+1=4s2−1
即
cos⁡θ=2s2−1\cos \theta = 2s^2 -1 cosθ=2s2−1
又有三角函数关系
cos⁡θ=2cos⁡2θ2−1\cos \theta = 2\cos ^2 \frac{\theta}{2} -1 cosθ=2cos22θ−1
于是有实部与转角的相互转换：
s=cos⁡θ2或θ=2arccos⁡ss = \cos \frac{\theta}{2} \quad \text{或} \quad\theta = 2 \arccos s s=cos2θ或θ=2arccoss
刚刚已经分析，单位四元数的虚部v\mathbf vv与旋转向量的转轴n\mathbf nn在R3\mathbb R^3R3中指向同一个方向，区别是他们的模服从不同的约束。单位四元数转换为转轴n\mathbf nn：
n=v∣v∣=v1−s2=v1−cos⁡2θ2=vsin⁡θ2\mathbf n = \frac {\mathbf v}{|\mathbf v|} = \frac {\mathbf v}{\sqrt{1 - s^2}} = \frac {\mathbf v}{\sqrt{1 - \cos^2 \frac {\theta}{2}}} = \frac {\mathbf v}{\sin \frac {\theta}{2}} n=∣v∣v=1−s2v=1−cos22θv=sin2θv
旋转向量转换为虚部v\mathbf vv：
v=sin⁡θ2n=1−s2n\mathbf v = \sin \frac {\theta}{2} \mathbf n = \sqrt{1 - s^2} \mathbf n v=sin2θn=1−s2n

总结

	旋转矩阵R\mathbf RR	旋转向量θn\theta \mathbf nθn	单位四元数q=[s,v]\mathbf q = [s, \mathbf v]q=[s,v]
旋转矩阵R\mathbf RR		R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧\mathbf R = \cos \theta \mathbf I + (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} + \sin \theta \mathbf n^{\land}R=cosθI+(1−cosθ)nnT+sinθn∧	R=vvT+s2I+2sv∧+(v∧)2\mathbf R = \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2R=vvT+s2I+2sv∧+(v∧)2
旋转向量θn\theta \mathbf nθn	θ=arccos⁡tr(R)−12Rn=ns.t.nTn=1\theta = \arccos \frac {\mathrm tr(\mathbf R) - 1} {2} \\ \mathbf R \mathbf n = \mathbf n \quad \textit{s.t.} \quad \mathbf n^{\mathrm T} \mathbf n = 1θ=arccos2tr(R)−1Rn=ns.t.nTn=1		θ=2arccos⁡sn=v∣v∣=v1−s2=vsin⁡θ2\quad\theta = 2 \arccos s \\ \mathbf n = \frac {\mathbf v}{\lvert \mathbf v \rvert} = \frac {\mathbf v}{\sqrt{1 - s^2}} = \frac {\mathbf v}{\sin \frac {\theta}{2}}θ=2arccossn=∣v∣v=1−s2v=sin2θv
单位四元数q=[s,v]\mathbf q = [s, \mathbf v]q=[s,v]	s=tr(R)+14Rv=vs.t.s2+vTv=1s = \sqrt {\frac {\mathrm tr(\mathbf R) +1}{4}} \\ \mathbf R \mathbf v = \mathbf v \quad \textit{s.t.} \quad s^2 + \mathbf v^{\mathrm T} \mathbf v = 1s=4tr(R)+1Rv=vs.t.s2+vTv=1	s=cos⁡θ2v=sin⁡θ2n=1−s2ns = \cos \frac{\theta}{2} \\ \mathbf v = \sin \frac {\theta}{2} \mathbf n = \sqrt{1 - s^2} \mathbf ns=cos2θv=sin2θn=1−s2n

一、旋转矩阵，旋转向量，单位四元数的相互转换总结相关推荐

【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）
矩阵的使用可参考系列博客:点击此处原文链接:基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换. 也可以参考另一篇博客:eigen 中四元数.欧拉角.旋转矩阵.旋转向量. ...
刚体在三维空间的旋转(关于旋转矩阵、DCM、旋转向量、四元数、欧拉角)
最近学习了一些关于三维空间旋转相关的知识,借此梳理一下备忘. 三维空间的旋转(3D Rotation)是一个很神奇的东东:如果对某个刚体在三维空间进行任意次的旋转,只要旋转中心保持不变,无论多少次的旋 ...
【自动驾驶】24.欧拉角、旋转向量、四元数、万向锁
原文链接:https://blog.csdn.net/shenxiaolu1984/article/details/50639298 原文链接:https://blog.csdn.net/lql071 ...
三维坐标变换（旋转矩阵旋转向量）
矩阵运算显然是计算机三维坐标变换最简单方便的计算方法,因此在 opencv.opengl.工业机器人等开发中,提到位姿旋转变换,多半用的是旋转矩阵.然而,用矩阵来表示一个旋转关系有两个缺点: 首先,通 ...
python二郎成长笔记（三）（matlab标定工具箱详解，旋转矩阵旋转向量，matlab标定数据传入opencv）
这一章由来:二郎之前研究matlab的双目立体视觉,已经得到了需要的信息,可是,二郎想要对代码进行修改使其更适宜自己的应用目的. 修改:标定不用修改--matlab内置的已经很强大了,而且没有必要用p ...
SLAM专题（5） -- 变换矩阵、旋转矩阵、欧拉角与四元数
目录坐标变换基础知识外积欧氏变换旋转矩阵旋转向量欧拉角四元数坐标变换基础知识外积外积大小为 ,可以表示两个向量张成四边形的有向面积,引入符号,定义a^表示为一个反对称矩阵,这样就把 ...
eigen 编译_头条 | 使用eigen实现四元数、欧拉角、旋转矩阵、旋转向量间的转换...
点击上方蓝字,关注本公众号,获得更多资源上一篇文章介绍了四元数.欧拉角.旋转矩阵.轴角如何相互转换,本篇文章介绍如何用eigen来实现. 旋转向量 1,初始化旋转向量:旋转角为alpha,旋转轴为(x ...
方向向量转欧拉角_【姿态表示】旋转向量、旋转矩阵、欧拉角、四元数
1. 旋转矩阵与旋转向量旋转矩阵(Rotation Matrix)用 9 个量描述旋转的3个自由度,有冗余: 9 个量是有约束的:必须是正交矩阵,且行列式为 1 旋转向量(Rotation Vecto ...
刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数
坐标变换及其方法 1.转化关系图 2 换算关系 3.1 旋转矩阵换算至其他 3.2 四元数换算至其他 3.3 旋转向量转换至旋转矩阵与四元数 3.3 欧拉角转换到旋转矩阵和四元数 3 坐标变换 4 坐 ...

一、旋转矩阵，旋转向量，单位四元数的相互转换总结

文章目录

前言