本文综合了几个相关的维基百科，加了点自己的理解，从比较基础的向量投影和叉积讲起，推导出罗德里格斯旋转公式。公式比较繁杂，如有错误，欢迎评论区指出。
对于向量的三维旋转问题，给定旋转轴和旋转角度，用罗德里格斯（Rodrigues）旋转公式可以得出旋转后的向量。另外，罗德里格斯旋转公式可以用旋转矩阵表示，即将三维旋转的轴-角（axis-angle）表示转变为旋转矩阵表示。
向量投影（Vector projection）
向量a在非零向量b上的向量投影指的是a在平行于向量b的直线上的正交投影。结果是一个平行于b的向量，定义为$\mathbf{a}_1=a_1\hat{\mathbf{b}}$，其中，$a_1$是一个标量，称为a在b上的标量投影，$\hat{\mathbf{b}}$是与b同向的单位向量。$a_1=\left\Vert\mathbf{a}\right\Vert\cos\theta=\mathbf{a}\cdot \hat{\mathbf{b}}=\mathbf{a}\cdot\frac{\mathbf{b}}{\left\Vert\mathbf{b}\right\Vert}$，其中$\cdot$表示点积，$\left\Vert\mathbf{a}\right\Vert$表示a的长度，$\theta$表示a和b的夹角。标量投影有正负，正负号与夹角$\theta$有关。
有了向量投影$\textbf{a}_1$，向量a可以表示为$\mathbf{a}=\mathbf{a}_1+\mathbf{a}_2$，其中$\mathbf{a}_2$称为a from b的vector rejection（没找到比较官方的翻译），也即a向正交于b的超平面的正交投影，$\mathbf{a}_2=\mathbf{a}-\mathbf{a}_1=\mathbf{a}-(\left\Vert\mathbf{a}\right\Vert\cos\theta)\hat{\mathbf{b}}$。下图比较清晰地表示出$\mathbf{a}$、$\mathbf{a}_1$、$\mathbf{a}_2$的关系。
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200214225341438-504414443.png" width = "200" alt="Projection of **a** on **b**, rejection of **a** from **b**"/> </div> <center> 图1 Projection of <b>a</b> on <b>b</b>(<b>a<sub>1</sub></b>), rejection of <b>a</b> from <b>b</b>(<b>a<sub>2</sub></b>) </center>
当$90^{\circ}<\theta\le180^{\circ}$时，向量投影示意图如图2所示：
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200214231032550-391133943.png" width = "200" alt="大于90°时的向量投影示意图"/> </div> <center> 图2 大于90°时的向量投影示意图，此时<b>a</b><sub>1</sub>与<b>b</b>的方向相反 </center>
记号
向量a在b上的向量投影用加粗的$\mathbf{a}1$表示，标量投影用不加粗的$a_1$。有时向量投影和vector rejection分别用$\mathbf{a}{\parallel\mathbf{b}}$和$\mathbf{a}_{\perp\mathbf{b}}$表示。
用a和b表示
当$\theta$未知时，可通过a和b计算得出，$\cos\theta = \frac{\mathbf{a}\cdot\mathbf{b}}{\left\Vert\mathbf{a}\right\Vert\left\Vert\mathbf{b}\right\Vert}$，从而标量投影、向量投影和vector rejection可以分别表示如下：
- 标量投影：
<p> \begin{equation} a_1=\left\Vert\mathbf{a}\right\Vert\cos\theta=\left\Vert\mathbf{a}\right\Vert\frac{\mathbf{a}\cdot\mathbf{b}}{\left\Vert\mathbf{a}\right\Vert\left\Vert\mathbf{b}\right\Vert}=\frac{\mathbf{a}\cdot\mathbf{b}}{\left\Vert\mathbf{b}\right\Vert} \end{equation} </p> - 向量投影： <p> \begin{equation} \mathbf{a}_1=a_1\hat{\mathbf{b}}=\frac{\mathbf{a}\cdot\mathbf{b}}{\left\Vert\mathbf{b}\right\Vert}\frac{\mathbf{b}}{\left\Vert\mathbf{b}\right\Vert}=\left(\mathbf{a}\cdot\hat{\mathbf{b}}\right)\hat{\mathbf{b}}=\frac{\mathbf{a}\cdot\mathbf{b}}{\mathbf{b}\cdot\mathbf{b}}\mathbf{b} \end{equation} </p> - vector rejection： <p> \begin{equation} \mathbf{a}_2=\mathbf{a}-\mathbf{a}_1=\mathbf{a}-\frac{\mathbf{a}\cdot\mathbf{b}}{\mathbf{b}\cdot\mathbf{b}}\mathbf{b} \end{equation} </p>
叉积
定义
叉积（又称向量积）是三维空间（$\mathbb{R}^3$）向量的二元操作，用符号$\times$表示，给定两个线性独立的向量a和b，叉积$\mathbf{a}\times\mathbf{b}$的结果是一个向量，这个向量与a、b都正交，也就是正交于a、b所在的平面。为什么要强调线性独立呢，因为非线性独立的两个向量（同向或反向）的叉积为$\mathbf{0}$。
叉积定义为：
<p> \begin{equation} \mathbf{a}\times\mathbf{b}=\left\Vert\mathbf{a}\right\Vert\left\Vert\mathbf{b}\right\Vert\sin(\theta)\mathbf{n} \end{equation}</p> 其中，$\theta$表示**a**、**b**的夹角，$0^\circ\le\theta\le180^\circ$，$\mathbf{n}$正交于**a**、**b**所在的平面，方向通常由右手法则确定，如下图所示： <div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200215222906413-1554311311.png" width = "200" alt="通过右手法则确定叉积方向"/> </div> <center> 图3 通过右手法则确定叉积方向 </center>
性质
右手法则决定了叉积不符合交换律，而符合反交换律，即$\mathbf{a}\times\mathbf{b}=-\mathbf{b}\times\mathbf{a}$，如图4所示：
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200215223314899-90292658.png" width = "150" alt="反交换律"/> </div> <center> 图4 反交换律</center>
由公式也可以看出当a、b的不线性独立时，即夹角为$0^\circ$或$180^\circ$时，叉积为零向量$\mathbf{0}$。叉积随夹角$\theta$的变化如图5所示。
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200215221723354-1206221458.gif" width = "300" alt="叉积随夹角变化示意图"/> </div> <center> 图5 叉积随夹角变化示意图</center>
另外，叉积符合分配律，即$\mathbf{a}\times(\mathbf{b}+\mathbf{c})=\mathbf{a}\times\mathbf{b}+\mathbf{a}\times\mathbf{c}$。如图6所示，左图向量b和c都被分解为vector projection和vector rejection两部分，右图则解释了分配律成立的原因，看图时要注意图中的平行四边形和正方形都表示了相等的关系。
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200217215810581-1926108251.png" width = "500" alt="叉积分配律示意图"/> </div> <center> 图6 叉积分配律示意图</center>
坐标表示
考虑右手法则定义的标准三维坐标系，三个坐标轴$\mathbf{i}$、$\mathbf{j}$、$\mathbf{{k}}$如图7所示，并满足以下等式关系： $$ \mathbf{i}\times\mathbf{j}=\mathbf{k}\ \mathbf{j}\times\mathbf{k}=\mathbf{i}\ \mathbf{k}\times\mathbf{i}=\mathbf{j} $$ 同样，由叉积的反交换律可得下面三个等式关系： $$ \mathbf{j}\times\mathbf{i}=-\mathbf{k}\ \mathbf{k}\times\mathbf{j}=-\mathbf{i}\ \mathbf{i}\times\mathbf{k}=-\mathbf{j} $$ 由平行向量的叉积为零向量可得：$\mathbf{i}\times\mathbf{i}=\mathbf{j}\times\mathbf{j}=\mathbf{k}\times\mathbf{k}=\mathbf{0}$。 由图7也可得，任意一个三维向量都可以表示为三个基向量的线性组合，例如： $$ \mathbf{a}=a_1\mathbf{i}+a_2\mathbf{j}+a_3\mathbf{k}\ \mathbf{b}=b_1\mathbf{i}+b_2\mathbf{j}+b_2\mathbf{k} $$
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200219161453680-1726751876.png" width = "300" alt="三维坐标系基向量与向量a的表示"/> </div> <center> 图7 三维坐标系基向量与向量<b>a</b>的表示</center> 进而，可以用坐标表示叉积运算如下：
<p> \begin{equation} \begin{split} \mathbf{a}\times\mathbf{b}&=(a_1\mathbf{i}+a_2\mathbf{j}+a_3\mathbf{k})\times(b_1\mathbf{i}+b_2\mathbf{j}+b_2\mathbf{k})\\ &=(a_2b_3-a_3b_2)\mathbf{i}+(a_3b_1-a_1b_3)\mathbf{j}+(a_1b_2-a_2b_1)\mathbf{k}\\ &=\left|\begin{array}{cccc} i & j & k \\ a_1 & a_2 & a_3\\ b_1 & b_2 & b_3 \end{array}\right| \end{split} \end{equation} </p>
上式中，将括号展开分别进行叉积推导出第二个等号，而第三个等号则可通过行列式计算得出。 进一步，可将叉积表示为矩阵与向量相乘的形式，由于$\mathbf{a}\times\mathbf{b}=(a_2b_3-a_3b_2, a_3b_1-a_1b_3,a_1b_2-a_2b_1)$，则叉积可表示为：
<p> \begin{equation} \begin{split} \mathbf{a}\times\mathbf{b}=\left[\mathbf{a}\right]_\times\mathbf{b}=\left[\begin{array}{cccc} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0 \end{array} \right]\left[ \begin{array}{cc} b_1\\b_2\\b_3 \end{array}\right]=\left[\mathbf{b}\right]^T_\times\mathbf{a}=\left[\begin{array}{cccc} 0 & b_3 & -b_2\\ -b_3 & 0 & b_1\\ b_2 & -b_1 & 0 \end{array} \right]\left[ \begin{array}{cc} a_1\\a_2\\a_3 \end{array}\right] \end{split} \end{equation} </p>
其中，$\left[\mathbf{a}\right]_\times$（slam14讲书上记为$\mathbf{a}^\wedge$）表示由向量$\mathbf{a}$得到的反对称矩阵，定义为：
<p> \begin{equation} \begin{split} \left[\mathbf{a}\right]_\times=\left[\begin{array}{cccc} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0 \end{array} \right] \end{split} \end{equation} </p>
通过该反对称矩阵的定义可以将叉积表示为矩阵与向量的乘法。
罗德里格斯旋转公式
考虑$\mathbf{v}\in\mathbb{R}^3$的三维旋转问题，旋转轴$\mathbf{k}$是单位向量，旋转角为$\theta$，按照右手法则（即逆时针）旋转。则可通过罗德里格斯旋转公式得出旋转后的向量$\mathbf{v}_{rot}$为：
<p> \begin{equation} \mathbf{v}_{rot}=\cos\theta\mathbf{v}+(1-\cos\theta)(\mathbf{k}\cdot\mathbf{v})\mathbf{k}+\sin\theta\mathbf{k}\times\mathbf{v} \end{equation} </p>
推导过程
由上文中向量投影部分的知识我们知道，一个向量$\mathbf{v}$可以分解为平行于$\mathbf{k}$的分量$\mathbf{v}\parallel$和正交于$\mathbf{k}$的分量$\mathbf{v}{\perp}$：
<p> \begin{equation} \mathbf{v}=\mathbf{v}_{\parallel}+\mathbf{v}_\perp \end{equation} </p>
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200217220414404-1389795950.png" width = "300" alt="向量的分解图示"/> </div> <center> 图8 向量的分解图示</center>
如图8所示，因为$\mathbf{k}$为单位向量，由向量投影部分知识可得
<p> \begin{equation} \mathbf{v}_\parallel=(\mathbf{v}\cdot\mathbf{k})\mathbf{k} \end{equation} </p>
<p> \begin{equation} \mathbf{v}_\perp=\mathbf{v}-\mathbf{v}_\parallel=\mathbf{v}-(\mathbf{k}\cdot\mathbf{v})\mathbf{k}=-\mathbf{k}\times(\mathbf{k}\times\mathbf{v}) \end{equation} </p>
关于上式最后一个等号的推导如下：
回顾叉积的知识，$\mathbf{k}\times\mathbf{v}=\mathbf{k}\times(\mathbf{v}{\parallel}+\mathbf{v}\perp)=\mathbf{0}+\mathbf{k}\times\mathbf{v}\perp=\mathbf{k}\times\mathbf{v}\perp$，$\mathbf{k}\times\mathbf{v}$可以看做将$\mathbf{v}\perp$以$\mathbf{k}$为旋转轴逆时针旋转了$90^\circ$。正如图9所示，$\mathbf{v}$分解为$\mathbf{v}\parallel$和$\mathbf{v}\perp$，用右手法则不难确定出$\mathbf{k}\times\mathbf{v}$的方向，进而不难发现，$\mathbf{k}\times(\mathbf{k}\times\mathbf{v})$可以看做将$\mathbf{v}\perp$以$\mathbf{k}$为旋转轴逆时针旋转了$180^\circ$，图9中的(椭)圆正反映了$\mathbf{k}\times(\mathbf{k}\times\mathbf{v})$、$\mathbf{k}\times\mathbf{v}$、$\mathbf{v}\perp$三者“大小相等”的关系。最终，可知$\mathbf{v}\perp=-\mathbf{k}\times(\mathbf{k}\times\mathbf{v})$。
<div align="center"> <img src="https://img2018.cnblogs.com/blog/1903168/202002/1903168-20200216012320436-1596896770.png" width = "600" alt="罗德里格斯旋转公式几何图示"/> </div> <center> 图9 罗德里格斯旋转公式几何图示</center>
从图8还可以看出，v的平行分量$\mathbf{v}\parallel$不会因为旋转而改变，旋转后的向量$\mathbf{v}{rot}$的平行分量依然等于$\mathbf{v}\parallel$，即$\mathbf{v}{\parallel rot}=\mathbf{v}\parallel$。
而v的正交分量$\mathbf{v}\perp$在旋转过程中大小不变，方向会发生变化，即
<p> \begin{equation} \begin{split} &|\mathbf{v}_{\perp rot}|=|\mathbf{v}_\perp|\\ &\mathbf{v}_{\perp rot}=\cos\theta\mathbf{v}_\perp+\sin\theta\mathbf{k}\times\mathbf{v}_\perp=\cos\theta\mathbf{v}_\perp+\sin\theta\mathbf{k}\times\mathbf{v} \end{split} \end{equation} </p>
上述第2个等式通过图9可以得出，将圆看做$xOy$坐标系平面，$\mathbf{v}\perp$所在的直线看做$x$轴，$\mathbf{k}\times\mathbf{v}$所在的直线看做$y$轴，结合三角函数，很容易用$\mathbf{v}\perp$和$\mathbf{k}\times\mathbf{v}$表示出$\mathbf{v}_\perp$。
到这已经得出罗德里格斯公式了：
<p> \begin{equation} \begin{split} \mathbf{v}_{rot}&=\mathbf{v}_{\parallel rot}+\mathbf{v}_{\perp rot}\\ &=\mathbf{v}_\parallel+\cos\theta\mathbf{v}_\perp+\sin\theta\mathbf{k}\times\mathbf{v}\\ &=\mathbf{v}_\parallel+\cos\theta(\mathbf{v}-\mathbf{v}_\parallel)+\sin\theta\mathbf{k}\times\mathbf{v}\\ &=\cos\theta\mathbf{v}+(1-\cos\theta)\mathbf{v}_\parallel+\sin\theta\mathbf{k}\times\mathbf{v}\\ &=\cos\theta\mathbf{v}+(1-\cos\theta)(\mathbf{k}\cdot\mathbf{v})\mathbf{k}+\sin\theta\mathbf{k}\times\mathbf{v} \end{split} \end{equation} </p>
矩阵形式
在叉积部分提到过叉积可以表示为矩阵乘向量的形式，类似地，罗德里格斯旋转公式可以表示为旋转矩阵乘以向量的形式，$\mathbf{v}_{rot}=\mathbf{R}\mathbf{v}$，其中$\mathbf{R}$是旋转矩阵。在slam14讲$^{[4]}$中的表示如下： \begin{equation} \mathbf{R}=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{k}\mathbf{k}^T+\sin\theta\mathbf{k}^\wedge \end{equation} 其中，$\mathbf{I}$表示单位矩阵，$\mathbf{k}$表示旋转向量（书中用$\mathbf{n}$表示旋转向量），$\mathbf{k}^\wedge$表示由$\mathbf{k}$得到的反对称矩阵。从式(13)不难看出上式，另外，结合式(13)还可以得到下面这个式子：
<p> \begin{equation} \begin{split} \mathbf{v}_{rot}&=\mathbf{v}_{\parallel rot}+\mathbf{v}_{\perp rot}\\ &=\mathbf{v}_\parallel+\cos\theta\mathbf{v}_\perp+\sin\theta\mathbf{k}\times\mathbf{v}\\ &=\mathbf{v}-\mathbf{v}_\perp+\cos\theta\mathbf{v}_\perp+\sin\theta\mathbf{k}\times\mathbf{v}\\ &=\mathbf{v}+(\sin\theta)\mathbf{k}\times\mathbf{v}+(1-\cos\theta)\mathbf{k}\times\mathbf{k}\times\mathbf{v} \end{split} \end{equation} </p>
从而，得出这个维基百科上的矩阵表示：
<p> \begin{equation} \begin{split} \mathbf{v}_{rot}=\mathbf{R}\mathbf{v}=\mathbf{v}+(\sin\theta)\mathbf{K}\mathbf{v}+(1-\cos\theta)\mathbf{K}^2\mathbf{v} \end{split} \end{equation} </p> 其中，$\mathbf{R}=\mathbf{I}+(\sin\theta)\mathbf{K}+(1-\cos\theta)\mathbf{K}^2$，$\mathbf{K}$表示由旋转向量$\mathbf{k}$生成的反对称矩阵。
参考：
[1] Rodrigues' rotation formula [2] Cross product [3] Vector projection [4] 视觉SLAM十四讲：从理论到实践
原文出处：https://www.cnblogs.com/wtyuan/p/12324495.html
