最近在做刚体变换，查阅了许多相关的文献，发现许多文章中对于旋转矩阵的定义都不一致。笔者经过仔细比对终于发现了一些规律，现在总结如下：
 
第一种：
 
 
注意这里的 sin 角的正负号，使用这种方式定义的x,y,z旋转矩阵，得到最终的旋转矩阵为：
 
 
第二种：
 
 
同样的，注意这里的 sin 角的正负号，使用这种方式定义的x,y,z旋转矩阵，得到的旋转矩阵为：
 
 
第三种：
 
 
使用这种方式定义的x,y,z旋转矩阵，得到的旋转矩阵为：
 

 
三角形E经过 旋转(绕M点顺时针旋转45度)+平移(M=>N)==三角形F
 
其中 M=(Mx,My),N=(Nx,Ny)
 
则 3x3的旋转平移矩阵=
 
[1,0,Nx-Mx]    [1,0,Mx]    [cos(PI/4),-sin(PI/4),0]   [1,0,-Mx]  
 
[0,1,Ny-My] X [0,1,My] X[sin(PI/4),cos(PI/4),0] X [0,1,-My] 
 
[0,0,  1]           [0,0,  1]    [0,0,  1]                          [0,0,  1]
 
 

 
三维空间刚体运动2：旋转向量与罗德里格斯公式（最详推导）
 
1.旋转向量定义
2.罗德里格斯公式-向量转换为矩阵
2.1 定义
2.2 推导
2.2.1 推导一
2.2.2 推导二
2.2.3 推导向量$a$和$b$
  
  
3.旋转矩阵到旋转向量


 序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：
 
旋转矩阵和变换矩阵；
旋转向量表示旋转；
欧拉角表示旋转；
四元数包括以下部分：
 4-1. 四元数表示变换；
 4-2. 四元数线性插值方法：LinEuler/LinMat/Lerp/Nlerp/Slerp；
 4-3. 四元数多点插值方法：Squad；
 4-4. 四元数多点连续解析解插值方法：Spicv；
 4-5. 四元数多点离散数值解插值方法：Sping。
实践：SLAM中显示机器人运动轨迹及相机位姿。
 
1.旋转向量定义
 
对于矩阵表示方式至少有以下两个缺点：
 1.$SO(3)$的旋转矩阵有9个量，但一次旋转只有3个自由度，因此这种表达方式是冗余的，同理$SE(4)$也是。那么，是否有更紧凑的表示呢？
 2.旋转矩阵和变换矩阵自身带有约束：它必须是正交矩阵且行列式为1。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变的更困难。
 因此，希望有一种方式能够紧凑的描述旋转和平移。
 
旋转向量：事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画，于是，我们可以使用一个向量$u$（为方便表达，上下文统一采用符号$u$表示单位向量，其他符号$n,k$等也是可以的），其方向与旋转轴一致，其长度等于旋转角$\theta$，那么向量$\theta u$也可以描述这个旋转，这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。
 
在三维空间中定义一个方向只需要用到两个量就可以了（与任意两个坐标轴之间的夹角，比如地球的经纬度就可以确定方向），因此第三个量可以用来定义长度，表示旋转角度。同样，对于变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换，此时变量维数正好是六维。
 
2.罗德里格斯公式-向量转换为矩阵
 
2.1 定义
 
罗德里格斯公式：旋转向量和旋转矩阵有什么联系吗？事实上，从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigues’s Formula）表示。因为任意旋转都可以由一个旋转轴$u$和一个旋转角$\theta$刻画，故罗德里格斯公式具体形式如下：$$R=\cos \theta I+(1-\cos \theta )u{u}^T+\sin \theta u^{\wedge }\text{\hspace{1em}(2.1)}$$符号${}^{\wedge }$是向量到反对称矩阵的转换符，见上一篇博客《三维空间刚体运动1：旋转矩阵与变换矩阵》的公式（1.4）。公式还可以写为：$$R=I+(1-\cos \theta )U^2+\sin \theta U.\text{\hspace{1em}(2.2)}$$其中U代表向量$u$转换的反对称矩阵$u^{\wedge }$。假设单位旋转向量$u$的坐标为$u=\left[\begin{array}{c}{u}_x\\ u_y\\ u_z\end{array}\right]$，旋转角为$\theta$，则R的展开式为：$$\left[\begin{array}{ccc}\cos \theta +u_x^2(1-\cos \theta )& u_x{u}_y(1-\cos \theta )-u_z\sin \theta & u_y\sin \theta +u_x{u}_z(1-\cos \theta )\\ u_z\sin \theta +u_x{u}_y(1-\cos \theta )& \cos \theta +u_y^2(1-\cos \theta )& -u_x\sin \theta +u_y{u}_z(1-\cos \theta )\\ -u_y\sin \theta +u_x{u}_z(1-\cos \theta )& u_x\sin \theta +u_y{u}_z(1-\cos \theta )& \cos \theta +u_z^2(1-\cos \theta )\end{array}\right]\text{\hspace{1em}(2.3)}$$
 
2.2 推导
 
首先，理解下图。定义$u$是旋转轴的单位向量，$v$为旋转向量，$w$为$u\times v$方向上的单位向量。图中$v$绕$u$旋转角度$\theta$得到$v_{rot}$。将$v$分解为平行于旋转轴$u$以及正交于$u$的两个分量：$v_{\Vert }$和$v_{\perp }$。将$v_{rot}$分解为平行于旋转轴$u$以及正交于$u$的两个分量：$v_{rot||}$和$v_{rot\perp }$，其中$v_{rot||}=v_{||}$。向量$a$和$b$分别是$v_{rot\perp }$在$w$和$v_{\perp }$方向上的分量。
 图2.1：旋转向量3D图（数学绘图软件推荐geogebra）
 
 所谓推导旋转方程，实则求出一个旋转矩阵，使得$v_{rot}=Rv$，所以我们要做的就是找出$v_{rot}$与$v$，并用矩阵来表示。
 此公式有2种形式，故而也有2种推导方法，两者推导方法的不同主要在$v_{\perp }$的表示上。具体推导过程如下。
 
2.2.1 推导一
 
推导一推导过程如下：
 
$v$：对$v$进行向量分解：$v=v_{\perp }+v_{||}$，根据向量减法可得：$$v_{\perp }=v-v_{||}\text{\hspace{1em}(2.4)}$$对$v_{rot||}$进行向量分解$$v_{rot}=v_{rot||}+v_{rot\perp }\text{\hspace{1em}(2.5)}$$下边分别推导$v_{rot||}$和$v_{rot\perp }$。
$v_{rot||}$：由于旋转过程平行向量的不变性可得$v_{rot||}=v_{||}$，$v_{||}$其实就是$v$在$u$上的正交投影（Orthogonal Projection），根据正交投影公式：$$\begin{array}{rlrlr}{v}_{||}& =pro{j}_u(v)& =\frac{u\cdot v}{u\cdot u}u& =\frac{u\cdot v}{||u|{|}^2}u& =(u\cdot v)u\end{array}\text{\hspace{1em}(2.6)}$$其中$u\cdot u=||u|{|}^2$，$||u||=1$，点积$(u\cdot v)$为标量，所以再乘向量$u$得到一个矢量。
$v_{rot\perp }$：下面画出$v_{rot\perp }$的俯视图：
 图2.2：旋转向量俯视图
 
 
我们需要处理正交于$u$的$v_{\perp }$，因为这两个向量是正交的，这个旋转可以看做是平面内的一个旋转，因为旋转不改变$v_{\perp }$的长度，所以路径是一个圆。现在，3D的旋转被转化为了2D平面上的旋转。由于在这个平面上只有一个向量$v_{\perp }$，用它来表示一个旋转是不够的，我们还需要构造一个同时正交于$u$ 和$v_{\perp }$的向量$w$，这个可以通过叉乘来获得： $$w=u\times v_{\perp }\text{\hspace{1em}(2.7)}$$注意叉乘的顺序，因为我们使用的是右手坐标系统，按照右手定则你可以发现这 个新的向量$w$指向$v_{\perp }$逆时针旋转$(\pi /2)$后的方向，并且和$v_{\perp }$一样也处于正交于$u$ 的平面内。因为$\Vert u\Vert$ = 1，我们可以发现：$$\begin{array}{rl}||w||& =||u\times v_{\perp }||\\ & =||u||\cdot ||v_{\perp }||\cdot \sin (\frac{\pi }{2})\\ & =||v_{\perp }||\end{array}\text{\hspace{1em}(2.8)}$$也就是说，$w$和$v_{\perp }$的模长是相同的，所以，$w$也位于圆上。有了这个新的向量$w$， 就相当于在平面内有了两个坐标轴。我们现在可以把$v_{rot||}$投影到$w$和$v_{\perp }$上，将其分解为向量$a$和$b$，使用一些三角知识可以得到：$$\begin{array}{rl}{v}_{rot\perp }& =a+b\\ & =\sin \theta w+\cos \theta v_{\perp }\\ & =\sin \theta (u\times v_{\perp })+\cos \theta v_{\perp }\end{array}\text{\hspace{1em}(2.9)}$$因为叉乘遵守分配律，且$u$平行于$v_{||}$，故：$$\begin{array}{rl}u\times v_{\perp }& =u\times (v-v_{||})\\ & =u\times v-u\times v_{||}\\ & =u\times v\end{array}\text{\hspace{1em}(2.10)}$$另外，向量$a$和$b$还有另外一种证法，稍显繁琐，有兴趣的同学请参见后两小节。
 
综上可得：
 $$\begin{array}{rl}{v}_{rot}& =v_{rot\perp }+v_{rot||}\\ & =a+b+v_{||}\\ & =\sin \theta u\times v_{\perp }+\cos \theta v_{\perp }+(u\cdot v)u\\ & =\sin \theta u\times v+\cos \theta (v-v_{||})+(u\cdot v)u\\ & =\sin \theta u\times v+\cos \theta (v-(u\cdot v)u)+(u\cdot v)u\\ & =\cos \theta v+(1-\cos \theta )(u\cdot v)u+\sin \theta u\times v\end{array}\text{\hspace{1em}(2.11)}$$
显然：到此步，我们还无法将其用矩阵来表示，所以需要对$(u\cdot v)u$ 和 $u\times v$ 进行矩阵转换，由点积的交换律和结合律得：$$(u\cdot v)u=u\cdot (u\cdot v)=u\cdot (u^{T}v)=u\cdot u^{T}v\text{\hspace{1em}(2.12)}$$其中的向量都是列向量，点积展开规则为：$x\cdot y=[x,y]=x^{T}y$。
 对于$u\times v$可用叉乘矩阵来化简为$Uv$：$$u\times v=\left[\begin{array}{c}(u\times v{)}_x\\ (u\times v{)}_y\\ (u\times v{)}_z\end{array}\right]=\left[\begin{array}{c}{u}_y{v}_z-u_z{v}_y\\ u_z{v}_x-u_x{v}_z\\ u_x{v}_y-u_y{v}_x\end{array}\right]=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ u_y& u_x& 0\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]=Uv\text{\hspace{1em}(2.13)}$$其中$$U=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ u_y& u_x& 0\end{array}\right]\text{\hspace{1em}(2.14)}$$
将$(u\cdot v)u$ 和 $u\times v$ 转换的矩阵代入式（2.10）得：$$\begin{array}{rl}{v}_{rot}& =\cos \theta v+(1-\cos \theta )(v\cdot u)u+\sin \theta u\times v\\ & =\cos \theta v+(1-\cos \theta )u{u}^{T}v+\sin \theta Uv\\ & =(\cos \theta I+(1-\cos \theta )u{u}^T+\sin \theta U)v\end{array}\text{\hspace{1em}(2.15)}$$故旋转矩阵$$R=\cos \theta I+(1-\cos \theta )u{u}^T+\sin \theta U.\text{\hspace{1em}(2.16)}$$其中$I$为单位矩阵。
 
2.2.2 推导二
 
与推导一相比，推导二的不同主要在于用叉乘去表示一些数据。用叉乘来表示$v_{\perp }$：$$v_{\perp }=-u\times (u\times v)\text{\hspace{1em}(2.17)}$$联立推导一中各式得：
 $$\begin{array}{rl}{v}_{rot}& =v_{rot\perp }+v_{rot||}\\ & =a+b+v_{||}\\ & =\sin \theta u\times v+\cos \theta v_{\perp }+v-v_{\perp }\\ & =\sin \theta u\times v-\cos \theta u\times (u\times v)+v+u\times (u\times v)\\ & =v+(1-\cos \theta )u\times (u\times v)+\sin \theta u\times v\\ & =v+(1-\cos \theta )U^{2}v+\sin \theta Uv(叉乘矩阵表示)\\ & =(I+(1-\cos \theta )U^2+\sin \theta U)v\\ & =Rv\end{array}\text{\hspace{1em}(2.18)}$$
 从而得出第二种表达式$$R=I+(1-\cos \theta )U^2+\sin \theta U.\text{\hspace{1em}(2.19)}$$显然，第二种表达式更为简便，在计算的过程中涉及的参数更少，所以这也是在进行旋转操作时常用的公式。
 
2.2.3 推导向量$a$和$b$
 
此处单独推导罗德里格斯公式的向量$a$和$b$，仅做参考，也可以忽略不看。$a$和$b$是由$v_{rot\perp }$正交分解得到的矢量，既有大小又有方向，所以在求解时，我们要对其大小和方向分别求解。
 $a$大小
 设${\theta }_1=\pi -\theta$，${\theta }_2$是向量$v$和$u$的夹角，$u$为单位向量，则对于$a$的大小$|a|$有：
 $$\begin{array}{rl}|a|& =\sin {\theta }_1|v_{rot\perp }|\\ & =\sin {\theta }_1|v_{\perp }|\\ & =\sin (\pi -\theta )|v_{\perp }|\\ & =\sin \theta |v_{\perp }|\\ & =\sin \theta \sin {\theta }_2|v|\\ & =\sin \theta \sin {\theta }_2|v||u|\end{array}\text{\hspace{1em}(2.20)}$$由三角公式$|a\times b|=\sin \theta |a||b|$知：$\sin {\theta }_2|v||u|=|u\times v|$，所以：
 $$|a|=\sin \theta |u\times v|\text{\hspace{1em}(2.21)}$$
 
$a$方向
 由叉乘方向可得$a$的单位方向向量为：$$u\times v/|u\times v|\text{\hspace{1em}(2.22)}$$
 
综上可得：
 $$\begin{array}{rl}a& =(u\times v/|u\times v|)|a|\\ & =(u\times v/|u\times v|)\sin \theta |u\times v|\\ & =\sin \theta u\times v\end{array}\text{\hspace{1em}(2.23)}$$
 
$b$大小
 由图得，${\theta }_1$为$b$和$v_{rot\perp }$的夹角，则：
 $$\begin{array}{rl}|b|& =\cos {\theta }_1|v_{rot\perp }|\\ & =\cos (\pi -\theta )|v_{\perp }|\\ & =-\cos \theta |v_{\perp }|\end{array}\text{\hspace{1em}(2.24)}$$
 
$b$方向
 由于$b$的方向与$v_{\perp }$方向相反，可得$b$的单位方向向量为：$$-v_{\perp }/|v_{\perp }|\text{\hspace{1em}(2.25)}$$
 
综上可得：
 $$\begin{array}{rl}b& =(-v_{\perp }/|v_{\perp }|)|b|\\ & =\cos \theta v_{\perp }\end{array}\text{\hspace{1em}(2.26)}$$
 
至此，罗德里格斯公式的证明全部结束。此外，如果读者希望获得关于Oxyz坐标系的旋转变换关系，可以参考这篇博客：图形变换之旋转变换公式推导。
 罗德里格斯公式反应的是旋转向量到旋转矩阵的转换关系，如果已知旋转矩阵$R$，如何推导旋转向量$v$呢？下边给出旋转矩阵到旋转向量的反向转换关系。
 
3.旋转矩阵到旋转向量
 
这里计算从一个旋转矩阵到旋转向量的转换。对于旋转角$\theta$，取旋转矩阵$R$两边的迹，有：$$\begin{array}{rl}tr(R)& =\cos \theta tr(I)+(1-\cos \theta )tr(u{u}^T)+\sin \theta tr(u^{\wedge })\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta .\end{array}\text{\hspace{1em}(3.1)}$$因此：$$\theta =\arccos \frac{tr(R)-1}{2}.\text{\hspace{1em}(3.2)}$$关于转轴$u$，旋转轴上的向量在旋转后不发生改变，说明：$$Ru=u.\text{\hspace{1em}(3.3)}$$因此，转轴$u$是矩阵$R$特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴$u$。由$v=\theta u$得到旋转向量$v$。
 
至此，推导结束。实践部分代码放到第三部分一起演示。
 
本文基于《视觉SLAM十四讲：从理论到实践》和《Quaternions, Interpolation and Animation》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。
 
参考文献：
 1.《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
 2. 罗德里格斯公式推导
 3. 四元数与三维旋转

 向量叉乘（Cross Product）
 
 ----------------------------------------------------------------------------------------------------------------------------------------------------------
 
 
 
 
 
 
 
 
 刚体变换（Rigid Transformation）
 
  刚体变换 变换前后两点间的距离依旧保持不变则被称为刚体变换（Rigid Transform）。 刚体变换可分解为平移变换、旋转变换和反转（镜像）变换。
 
 
 ----------------------------------------------------------------------------------------------------------------------------------------------------------
 
 
 （1）让g（x）是三维向量空间到三维空间的一个映射函数。如果该函数满足下列性质，则被称为刚体变换（Rigid Transformation）:
 
 
 
 
 
 
 
 （2）刚体变化能够通过下式表示：
 
 
 
 
 在该式中，矩阵，R，被称为旋转矩阵（Rotation Matrix），并且满足下列特殊性质：
 
 
 
 
 
 
 
 （3）拥有下述性质的矩阵，连同矩阵乘法运算，构成一个组（Group）：SO(3)
 
 
 
 
 
 上式是一个旋转矩阵。
 
 性质：
 
 
 
 
 
 
 
 组（Group）
 
 ----------------------------------------------------------------------------------------------------------------------------------------------------------
 
 
 组（Group）是一种代数结构，由一个集合，G，和一组组上的操作组成，满足下列公理：
 
 （1）封闭性（Closure）:
 
 
                                           
 
 
 （2）结合性（Associativity）:
 
                                           
 
 
 （3）同一性和可逆性（Identity and Inverse）:
 
                                           
 
 
                                           
 
 
 
 
 
 罗德里格斯公式（Rodrigues Formula）
 
 ----------------------------------------------------------------------------------------------------------------------------------------------------------
 
 
 表示物体的旋转有三种方式：转轴和转角，旋转矩阵，四元素。
 
 罗德里格斯公式用来将转轴转角表示方式转换成旋转矩阵表示方式：
 
 
 
 
 其中theta是转角，omega是转轴的单位向量。
 
 
 
 
 四元素（Quaternion）
 
 ----------------------------------------------------------------------------------------------------------------------------------------------------------
 
 
 四元素可以被看做是由一个标量和一个矢量构成：
 
                                            
 
 
 四元素乘法计算如下：
 
                                          
 
 
 单位四元素是四个分量平方和为一的四元素，对于转轴和转角表示方式也可以用单位四元素方式表示：
 
                                           
 
 
 同样也可以从四元素得到旋转矩阵：
 
                                           
 
 
 四元数共轭（Quaternion Conjugate）
 
                                            
 
 
 值得注意的是：
 
 （1）如果一个四元素对应一个旋转矩阵，则该四元素的共轭对应于同一旋转矩阵的转置；
 
 
 （2）四元素乘法与对应旋转矩阵乘法的等价性：