Geometric Methods

几何方法—旋量理论补充

参考Lynch的《现代机器人学》（modern robotics）

角速度回顾

当坐标架$\hat{x},\hat{y},\hat{z}$绕着某一个轴$\hat{\omega}$（三维向量）以$\dot{\theta}$（标量）转动时，则$\omega = \hat{\omega}\dot{\theta}$代表角速度

根据几何就有$\dot{\hat{x}} = \omega\times \hat{x}$（$y$，$z$同理）。因此，选世界坐标系为参考系，表示出来就有$\dot{r_i} = \omega\times r_i$，合并成矩阵就是$\dot{R} = [\omega_s\times r_1\quad \omega_s\times r_2 \quad \omega_s\times r_3]$

我们可以直接定义一个新的记号$[\omega_s]$，他的作用就是使得$\dot{R}=[\omega_s] R$成立。这个式子就给出了$[\omega_s]$的定义，这里的$s$代表的是世界坐标系。而$[]$这个映射也就是$\mathbb{R}^3\to \mathfrak{so}(3)$的双射，具体怎么个映法，已经很熟悉了

因此，$[\omega_s] = \dot{R}R^{-1}$

定理 $R[\omega]R^\top = [R\omega]$

证明：略，可以参考任何书

下面来看$\omega_b$，其定义是“把刚体在世界坐标系下的角速度，转变到刚体坐标系下看后的向量”，因此他只不过是$\omega_s$的坐标变换：$\omega_b = R_{bs} \omega_s =R^\top_{sb} \omega_s$（原本的$R$无非是$R_{sb}$的简称嘛！$b$在$s$下看的orientation）

注：为啥要定义这个玩意？因为实际机器人中的IMU等，测的角速度究竟是什么？

1. 是刚体的旋转在自身坐标系下看的角速度吗？不是，因为自己看自己当然恒为静止的

2. 其实是相对世界系下的角速度，但是IMU是装在刚体上的，所以读数是世界系下的角速度变换到刚体系下看的向量，这就是$\omega_b$！

因此，$[\omega_b] = [R^\top \omega_s]$。利用刚才定理，得到$R^\top [\omega_s] R = R^{-1} \dot{R}$

有形式上对偶的关系式：$[\omega_b]= R^{-1} \dot{R}$以及$[\omega_s]= \dot{R} R^{-1}$，需要注意的是，这里的$R$指的都是$R_{sb}$，也就是在世界系${s}$下看的刚体系${b}$的orientation

值得注意的是：

• $\omega_s$与${b}$的选取无关，意思是如果选取另外一个固定在刚体上的坐标系${b’}$，得到的结论仍然一样

• $\omega_b$与${s}$的选取无关，意思是如果选取另外一个固定在大地上的坐标系${s’}$，得到的结论仍然一样

证明：比如只证明第一个，只用取$R_{sb’} = R_{sb}R_{bb’}$来代替原来的$R_{sb}$，然后代入即可$[\Omega_s] = \dot{R} _ {sb’} R _ {sb’}^{-1}$然后利用莱布尼茨律以及$R_{bb’}$是一个常数阵的事实，运算后就有$[\Omega_s] = [\omega_s]$的结果

指数表示

回顾轴角表示，有一个罗德里格斯公式，下面用这套新的语言推出罗德里格斯公式

由于一个三维向量$p$在空间中按照轴$\hat{\omega}$以恒定单位速度旋转时，可以写为方程$\dot{p} = \hat{\omega}\times p = [\hat{\omega}] p$，因此，仿照方程式$\dot{x}=Ax$的解，得知，$p(t) = e^{[\hat{\omega}]t}p(0)$。又由于是单位速度所以$t=\theta$，故$p(\theta) = e^{[\hat{\omega}]\theta}p(0)$

因此，把$e^{[\hat{\omega}]\theta}$用指数的定义式展开整理，即可得到罗德里格斯公式：
$p(\theta) = (I+\sin \theta[\hat{\omega}]+(1-\cos\theta) [\hat{\omega}]^2) p(0)$

这个$\hat{\omega}\theta$就是所谓的指数坐标。同样，给定一个旋转，找到其对应的反对称矩阵$[\hat{\omega}]\theta \in \mathfrak{so}(3)$也就被叫做“取对数”操作

Trace

在一般情况下（就是说“取对数”操作不会有太多解的情况）有$\text{tr}(R) = 1+2\cos\theta$，这也就能帮助理解为什么Taeyoung Lee的论文中用$\frac{1}{2}\text{tr}(I-R_d^T R)$作为error定义（代入就能发现其实是$1-\cos\theta_\text{error}$）

旋量理论

只用把刚才的一套东西，都从纯粹的“旋转”推广到“旋转+平动”即可：

我们都知道齐次阵$T_{sb}=\begin{pmatrix}
R & p \\
0 & 1 \
\end{pmatrix}$（此处的$R=R_{sb}$，$p$也是指的$s$下看的刚体系原点坐标），因此，直接模仿之前的做法，定义$[\mathcal{V}_b] = T^{-1}\dot{T}$，以及$[\mathcal{V}_s] = \dot{T} T^{-1}$

先来看$[\mathcal{V}_b]$，计算后就发现：
$$
[\mathcal{V}_b] =
\begin{pmatrix}
[\omega_b] & R^{\top}\dot{p} \\
0 & 0 \
\end{pmatrix}
$$
这就能够清晰的解释含义了，简单来说，这个就是实际工程中喜闻乐见的量，因为IMU等测的是机器人相对于世界的角速度与平动速度，但是是在刚体系下看的，故角速度是$\omega_b$，而测得的平动速度也是$R^{-1} \dot{p}$，把$R^{-1} \dot{p}$记为$v_b$，则$[\mathcal{V}_b] =
\begin{pmatrix}
[\omega_b] & v_b\\
0 & 0 \
\end{pmatrix}$

把$R^{\top}\dot{p}$记作$v_b$，定义物体运动旋量为$\mathcal{V}_b = \begin{pmatrix}
\omega_b \\ v_b
\end{pmatrix}$

再来看$[\mathcal{V}_s]$，有：

$$
[\mathcal{V}_s] =
\begin{pmatrix}
[\omega_s] & \dot{p} - \dot{R}R^{\top}p \\
0 & 0 \\
\end{pmatrix}
$$
同样可以把$\dot{p} - \dot{R}R^{\top}p$定义为$v_s$，但是含义并不直观，有兴趣可以自己查看教材等，我懒得写了。则$[\mathcal{V}_s] =
\begin{pmatrix}
[\omega_s] & v_s\\
0 & 0 \
\end{pmatrix}$

定义空间运动旋量为$\mathcal{V}_s = \begin{pmatrix}
\omega_s \\ v_s
\end{pmatrix}$

伴随

这样子，仅仅凭借定义就可以得到$[\mathcal{V}_b] = T^{-1} [\mathcal{V}_s] T$，以及，根据刚才给出的$\omega_s,\omega_b ,v_s ,v_b$的定义，就可以得到：

$$
\begin{pmatrix}
\omega_s \\
v_s
\end{pmatrix} = \begin{pmatrix}
R & 0 \\
[p]R & R \\
\end{pmatrix} \begin{pmatrix}
\omega_b \\
v_b
\end{pmatrix}
$$

现在，把中间这个矩阵叫做“伴随矩阵”，记为$[\text{Ad}_T]$，上式就是$\mathcal{V}_s = [\text{Ad}_T]\mathcal{V}_b$。总结一下：

同样的道理：

• $\mathcal{V}_s$与${b}$的选取无关，意思是如果选取另外一个固定在刚体上的坐标系${b’}$，得到的结论仍然一样

• $\mathcal{V}_b$与${s}$的选取无关，意思是如果选取另外一个固定在大地上的坐标系${s’}$，得到的结论仍然一样

“螺旋轴”

旋量刻画的就是拧螺丝时的螺旋运动，既有旋转又有平动。我们把“螺旋轴”记作$\mathcal{S}\in\mathbb{R}^6$，这个“螺旋轴”打引号是很重要的！因为他实际上是正则化的旋量，并不是说真的是物理上的那个螺旋轴。正则化的方法是：

考虑$\mathcal{V}_s = \begin{pmatrix}
\omega \\ v
\end{pmatrix}$，

1. 若$\Vert \omega\Vert \neq 0$，则取$\mathcal{S} = \begin{pmatrix}
   \omega / \Vert\omega\Vert \\ v / \Vert\omega\Vert
   \end{pmatrix}$

2. 若$\Vert \omega\Vert = 0$，则取$\mathcal{S} = \begin{pmatrix}
   0 \\ v / \Vert v \Vert
   \end{pmatrix}$

当然，由于这个$\mathcal{S}$无非是规范化的旋量，所以一切和旋量相关的知识都是延续下来的，比如$[\mathcal{S}] = \begin{pmatrix}
[\omega] & v \\
0 & 0 \
\end{pmatrix}\in \mathfrak{se}(3)$以及$\mathcal{S}_s = [\text{Ad}_T]\mathcal{S}_b $

可以看到，如果运动是纯平移的，那么$\mathcal{S}$就代表平动的方向。如果运动带点旋转，那么$\mathcal{S}$就代表旋转轴的方向

刚体运动的指数表示

同样的，我们定义齐次阵$T$的六维指数坐标为$\mathcal{S}\theta\in \mathbb{R}^6$，其中$\mathcal{S}$就是刚才那个东西，$\theta$就是一个参数而已，暂时不要赋予它物理含义

现在，我们可以证明，任何的齐次阵$T = \begin{pmatrix}
R & p \\
0 & 1 \
\end{pmatrix}$都可以被一组指数坐标通过指数运算给出，即$T = e^{[\mathcal{S}]\theta}$，这样的$\mathcal{S}$和$\theta$必定可以被找到，具体过程可以看Lynch的书的命题3.25以及后面的一段论述（简单来说就是，把这个指数先算出来闭式解，然后分类讨论+对照一下元素就完事了）

左不变的意义

left-invariant的含义就是，当更换固定坐标系（也就是换一个和地面固连的坐标系）不会变化。比如说，$\omega_b$和$\mathcal{V}_b$就是与${ s }$的选取无关的两个量

也就是说，把$R_{sb}$与$T_{sb}$换成$R_{s’b}$与$T_{s’b}$，无非是左乘一个$R_{s’s}$或者$T_{s’s}$，这就是“左”的来历