坐标变换(5)—用旋转轴和旋转角表示旋转

任何旋转，都可以用一个旋转轴 $\hat \omega$ 和一个旋转角 $\theta$ 来描述。

1. 坐标系的线速度和角速度

如上图，在旋转的刚体上，附加一个body frame $b$ ，记为 $\hat{x},\hat{y},\hat{z}$ 。对于三个轴而言，绕着 $\hat \omega$ 旋转的轨迹为圆。当然，上述坐标轴 $\hat{x},\hat{y},\hat{z}$ 和 $\hat \omega$ 是在fixed frame $S$ 坐标系下的，下面将 $\hat \omega$ 记为 $\hat \omega_s$ ，

绕着轴 $\hat \omega$ 的角速度为，
$w_s=\hat{w}\dot{\theta} \tag{1}$
运动的线速度记为 $\dot{\hat{x}}$ ，三个轴的线速度则为，
$\begin{aligned} \dot{\hat{x}}&=w_s \times \hat{x} \\ \dot{\hat{y}}&=w_s \times \hat{y} \\ \dot{\hat{z}}&=w_s \times \hat{z} \end{aligned} \tag{2}$
将三个轴的线速度统一写为，
$\dot{R}= \begin{bmatrix} w_s \times \hat{x} & w_s \times \hat{y} & w_s \times \hat{z} \end{bmatrix}=w_s \times R \tag{3}$
为了简化公式(3)中的叉乘，特引入了 $[]$ 符号，将 $w \times R$ 可以记为矩阵的乘法 $[w]R$ ，其中 $[w]$ 的定义如下：
对于 $\mathbb{R}^3$ 中的向量 $x=\begin{bmatrix}x_1 & x_2 &x_3\end{bmatrix}$ ,定义 $[x]$ 为一个反对称矩阵，
$[x]=\left[\begin{array}{ccc} 0 & -x_{3} & x_{2} \\ x_{3} & 0 & -x_{1} \\ -x_{2} & x_{1} & 0 \end{array}\right]\tag{4}$
$[x]=-[x]^T \tag{5}$
上述所有 $3 \times 3$ 的反对称矩阵统称为 $so(3)$ ，小的。前面说过，旋转矩阵属于 $SO(3)$ ，大的。下面有一个两者结合起来有趣的性质，假定 $r_i^T$ 为 $R$ 的第 $i$ 行，即 $r_i$ 是 $R^T$ 的第 $i$ 列，则
$\begin{aligned} R[w]R^T &= R\begin{bmatrix} w_s\times r_1 & w_s\times r_2 &w_s\times r_3 \end{bmatrix}\\ &= \begin{bmatrix} r_1^{T}(w_s\times r_1) & r_1^T(w_s\times r_2) & r_1^T (w_s\times r_3) \\ r_2^{T}(w_s\times r_1) & r_2^T(w_s\times r_2) & r_2^T (w_s\times r_3) \\ r_3^{T}(w_s\times r_1) & r_3^T(w_s\times r_2) & r_3^T (w_s\times r_3) \end{bmatrix} \\ &= \begin{bmatrix} 0 & -r_3^Tw_s & r_2^Tw_s\\ r_3^Tw_s & 0 &-r_1^Tw_s\\ -r_2^Tw_s&r_1^Tw_s&0 \end{bmatrix}\\ &=[Rw_s] \end{aligned} \tag{6}$

对于(6)中矩阵中的 $r_1^{T}(w_s\times r_2)$ ，是三个向量 $r_1,w_s,r_2$ 的混合积，也就是三个向量组成的六面体的体积，而我们知道矩阵的行列式的值的物理意义就是体积。根据下面的混合积的图，很容易得到矩阵中对应元素的反对称的关系。

下面我们将三个轴的线速度表示为 $[w]$ 的写法，
$\dot{R}=[w_s]R \tag{7}$

$[w_s]=\dot{R}R^{-1} \tag{8}$
前面我们提到的所有的向量和 $R$ 都是在fixed frame $S$ 下描述的，下面我们将 $w_s$ 在body frame $b$ 下进行描述，易得，
$w_s=R_{sb}w_b\tag{9}$
则旋转轴在body frame $b$ 下，
$w_b=R_{sb}^{-1}w_s=R^{-1}w_s=R^{T}w_s \tag{10}$
因此可以得到，
$\begin{aligned} [w_b]&=[R^{T}w_s] \\ &= R^T[w_s]R \\ &= R^T(\dot{R}R^{T})R \\ &=R^T\dot{R} \\ &=R^{-1}\dot{R} \end{aligned}\tag{11}$

需要注意的是 $w_b$ 是在body frame $b$ 下的描述，所以它描述的角速度不是一个旋转的坐标系的角速度(例如 $b$ 相对于 $S$ 旋转)，而是在某一瞬时， $w_b$ 相对于body frame $b$ 的旋转。

2. 微分方程的解

给定下面一个简单的线性微分方程，其中 $x(t) \in \mathbb{R}$ ， $a \in \mathbb{R}$ ，初始状态 $x(t) =x(0)$ ，
$\dot x(t)=ax(t) \tag{12}$
易得上述的解为，
$x(t)=e^{at}x(0) \tag{13}$
对 $e^{at}$ 在 $0$ 附近进行泰勒展开，可得，
$e^{a t}=1+a t+\frac{(a t)^{2}}{2 !}+\frac{(a t)^{3}}{3 !}+\cdots \tag{14}$
同理，当 $a$ 为矩阵 $A$ 时， $x(t)$ 为列向量，
$\dot x(t)=Ax(t) \tag{15}$
可得解为,
$x(t)=e^{At}x(0) \tag{16}$
其中，
$e^{A t}=1+A t+\frac{(A t)^{2}}{2 !}+\frac{(A t)^{3}}{3 !}+\cdots \tag{17}$

3. 指数形式的旋转

任何旋转，都可以用一个旋转轴 $\hat \omega$ 和一个旋转角 $\theta$ 来描述。其中 $\hat \omega \in \mathbb{R^3},\Vert{\hat \omega}\Vert=1$ ， $\theta \in \mathbb{R^3}$ 。
下面我们来分析如何利用一根旋转轴和旋转角来描述旋转，

假设向量 $p(t)$ 从 $p(0)$ 绕着 $\hat{\omega}$ 以恒定的角速度 $1rad/s$ 旋转了 $\theta$ 秒，最终到 $p(\theta)$ ，定义 $p(t)$ 间断的线速度为，
$\dot{p}= \hat{\omega} \times p \tag{18}$
由前面的分析，引入 $[\hat{\omega}]$ ，则
$\dot{p}= [\hat{\omega}] p \tag{19}$
该微分方程如前面介绍为，
$p(t)=e^{[\hat{\omega}] t} p(0) \tag{20}$

则，
$p(\theta)=e^{[\hat{\omega}] \theta} p(0) \tag{21}$

容易得到 $[\hat{\omega}]$ 两个计算性质，如下，
$[\hat{\omega}][\hat{\omega}]=\hat{\omega}\hat{\omega}^{T}-I \tag{22}$

$[\hat{\omega}][\hat{\omega}][\hat{\omega}]= [\hat{\omega}]^{3}=-[\hat{\omega}] \tag{23}$
所以公式21可以化简为，
$\begin{aligned} e^{[\hat{\omega}] \theta} &=I+[\hat{\omega}] \theta+[\hat{\omega}]^{2} \frac{\theta^{2}}{2 !}+[\hat{\omega}]^{3} \frac{\theta^{3}}{3 !}+\cdots \\ &=I+\left(\theta-\frac{\theta^{3}}{3 !}+\frac{\theta^{5}}{5 !}-\cdots\right)[\hat{\omega}]+\left(\frac{\theta^{2}}{2 !}-\frac{\theta^{4}}{4 !}+\frac{\theta^{6}}{6 !}-\cdots\right)[\hat{\omega}]^{2} \\ &= I+sin(\theta)[\hat{\omega}]+(1-cos(\theta))[\hat{\omega}]^{2} \end{aligned} \tag{24}$
上式就是著名的罗德里格斯公式，即指数形式的旋转，
$\operatorname{Rot}(\hat{\omega}, \theta)=e^{[\hat{\omega}] \theta}=I+\sin \theta[\hat{\omega}]+(1-\cos \theta)[\hat{\omega}]^{2} \in S O(3) \tag{25}$
经过指数映射，将 $s o(3)$ 和旋转的角度 $\theta$ 通过指数映射为 $S O(3)$ ，即三维的旋转矩阵。

在前面文章中介绍过，旋转矩阵左乘和右乘的区别，这里也是类似的，假设body frame $b$ 在fixed frame $S$ 中的描述为 $R_{sb}$ ，则 $\operatorname{Rot}(\hat{\omega}, \theta)R_{sb}$ ，左乘，表示将 $R_{sb}$ 顺着 $S$ 中的 $\hat{\omega}$ 旋转 $\theta$ 。而 $R_{sb}\operatorname{Rot}(\hat{\omega}, \theta)$ ，右乘，表示将 $R_{sb}$ 顺着 $b$ 中的 $\hat{\omega}$ 旋转 $\theta$ 。

4. 旋转矩阵的对数

上面描述的是从 $\hat{\omega}\theta$ 到旋转矩阵 $R$ 的过程，下面介绍从旋转矩阵 $R$ 求 $\hat{\omega}\theta$ 的过程，也就是求得旋转向量和具体的旋转角度，求 $R$ 矩阵的“对数”。可以将两个对应的过程描述成下面的形式，

$\begin{array}{cll} \exp : & [\hat{\omega}] \theta \in s o(3) & \rightarrow & R \in S O(3) \\ \log : & R \in S O(3) & \rightarrow & [\hat{\omega}] \theta \in \operatorname{so}(3) \end{array}$

下面将公式(25)展开，如下，
$\left[\begin{array}{ccc} c_{\theta}+\hat{\omega}_{1}^{2}\left(1-c_{\theta}\right) & \hat{\omega}_{1} \hat{\omega}_{2}\left(1-c_{\theta}\right)-\hat{\omega}_{3} \mathrm{s}_{\theta} & \hat{\omega}_{1} \hat{\omega}_{3}\left(1-c_{\theta}\right)+\hat{\omega}_{2} \mathrm{s}_{\theta} \\ \hat{\omega}_{1} \hat{\omega}_{2}\left(1-\mathrm{c}_{\theta}\right)+\hat{\omega}_{3} \mathrm{S}_{\theta} & c_{\theta}+\hat{\omega}_{2}^{2}\left(1-c_{\theta}\right) & \hat{\omega}_{2} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)-\hat{\omega}_{1} \mathrm{s}_{\theta} \\ \hat{\omega}_{1} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)-\hat{\omega}_{2} \mathrm{S}_{\theta} & \hat{\omega}_{2} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)+\hat{\omega}_{1} \mathrm{s}_{\theta} & \mathrm{c}_{\theta}+\hat{\omega}_{3}^{2}\left(1-\mathrm{c}_{\theta}\right) \end{array}\right] \tag{26}$
其中， $\hat{\omega} = \begin{bmatrix} \hat{w_1} \\ \hat{w_2} \\ \hat{w_3} \end{bmatrix}$ ， $\mathrm{c}_{\theta}=cos(\theta)$ ， $\mathrm{s}_{\theta}=sin(\theta)$ 。
记旋转矩阵 $R$ 为 $r_{ij}$ ，则可以得到，
$\begin{aligned} r_{32}-r_{23}&=2 \hat{\omega}_{1}sin(\theta) \\ r_{13}-r_{31}&=2 \hat{\omega}_{2}sin(\theta) \\ r_{21}-r_{12}&=2 \hat{\omega}_{3}sin(\theta) \end{aligned} \tag{27}$
上式在 $sin(\theta)\ne 0$ 的情况下，可以得到，
$\begin{aligned} \hat{\omega}_{1}=\frac{1}{2sin(\theta)}(r_{32}-r_{23}) \\ \hat{\omega}_{2}=\frac{1}{2sin(\theta)}(r_{13}-r_{31}) \\ \hat{\omega}_{3}=\frac{1}{2sin(\theta)}(r_{21}-r_{12}) \end{aligned} \tag{28}$
上式也可以写成，
$[\hat{\omega}]=\left[\begin{array}{ccc} 0 & -\hat{\omega}_{3} & \hat{\omega}_{2} \\ \hat{\omega}_{3} & 0 & -\hat{\omega}_{1} \\ -\hat{\omega}_{2} & \hat{\omega}_{1} & 0 \end{array}\right]=\frac{1}{2 \sin \theta}\left(R-R^{\mathrm{T}}\right) \tag{29}$

此外，由式(26)可以得到另外一个计算 $\theta$ 的公式，
$\operatorname{tr} R=r_{11}+r_{22}+r_{33}=1+2 \cos \theta \tag{30}$
至此， $sin(\theta)\ne 0$ 的情况下，利用旋转矩阵 $R$ ，我们计算出了 $\hat{\omega}$ 和 $\theta$ 。接下来讨论 $sin(\theta) = 0$ 的情况：

当 $\theta=k\pi$ ，且 $k$ 是偶数的情况下，此时相当于没有旋转，回到了原位置， $R=I$ ；
当 $\theta=k\pi$ ，且 $k$ 是奇数的情况下，此时有，
$R=e^{[\hat{\omega}] \pi}=I+2[\hat{\omega}]^{2} \tag{31}$
因为式(31)三个矩阵都是对角矩阵，所以可以得到下面的结果(利用 $R$ 对角元素)
$\hat{\omega}_{i}=\pm \sqrt{\frac{r_{i i}+1}{2}}, \quad i=1,2,3 \tag{32}$
利用 $R$ 非对角元素，可得，
$\begin{aligned} 2 \hat{\omega}_{1} \hat{\omega}_{2} &=r_{12} \\ 2 \hat{\omega}_{2} \hat{\omega}_{3} &=r_{23} \\ 2 \hat{\omega}_{1} \hat{\omega}_{3} &=r_{13} \end{aligned} \tag{33}$
利用式(32)和式(33)我们就能计算出 $\hat{\omega}$ ，同时此时旋转的角为 $\theta=\pm \pi, \pm 3 \pi, \ldots$ 。
从上面的计算过程很容易看出来，旋转角度是以 $2\pi$ 为周期，其实也是符合物理意义的，旋转 $\pi$ 和旋转 $3\pi$ 的效果是一样的，因此我们可以将旋转的角度限定在 $[-\pi,\pi]$ 。此时计算的 $\hat{\omega}\theta$ 的长度是 $\le\pi$ 的。因此我们可以把 $S O (3)$ 想象为一个半径为 $\pi$ 的实心球，如下图所示，

当给定球中的一点 $r\in\mathbb{R}^3$ ，我们可以将 $\hat{\omega}=\frac{r}{\Vert r\Vert}$ 作为单位长度的旋转轴， $\Vert r\Vert$ 作为 $\theta$ 。和 $r$ 相对应的旋转矩阵 $R$ 可以被看作是绕着 $\hat{\omega}$ 旋转了 $\theta$ 角。对于 $R\in SO(3)$ ，同时 $trR \ne -1$ ，此时在实心球中总能找到一个唯一的 $r$ ，使得 $e^{[r]}=R$ 。当 $trR = -1$ 时，此时 $\Vert r\Vert=\pi$ ，在实心球的表面有一对正好相反的一对点，两者的效果是一样的， $r$ 和 $-r$ 都对应了同一个 $R$ 。

最后编辑于：2021.03.29 11:12:13

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 217,734评论 6赞 505
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 92,931评论 3赞 394
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 164,133评论 0赞 354
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 58,532评论 1赞 293
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 67,585评论 6赞 392
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 51,462评论 1赞 302
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 40,262评论 3赞 418
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 39,153评论 0赞 276
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 45,587评论 1赞 314
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 37,792评论 3赞 336
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 39,919评论 1赞 348
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 35,635评论 5赞 345
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 41,237评论 3赞 329
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 31,855评论 0赞 22
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,983评论 1赞 269
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 48,048评论 3赞 370
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 44,864评论 2赞 354

坐标变换(5)—用旋转轴和旋转角表示旋转

1. 坐标系的线速度和角速度

2. 微分方程的解

3. 指数形式的旋转

4. 旋转矩阵的对数

推荐阅读更多精彩内容