《Modern Robotics》阅读笔记4——刚体的运动（四）

《Modern Robotics》阅读笔记4——刚体的运动（四）

1. 旋转的指数坐标表达法

上图中，我们假设，把$p(0)$向量，沿着$\hat{\omega }$轴旋转$\theta$，到达$p(\theta )$位置。（这里假设所有的量都是表达在固定坐标系下的）。我们假设旋转角速度是$1$rad/s，那么这个过程花费的时间就是从$t=0$到$t=\theta$。假设$p(t)$代表了向量的轨迹，那么我们得到$p(t)$的导数的表达：
$$\dot{p}=\hat{\omega }\times p$$
通过简单的证明可知上式的正确性：由于我们假设了旋转角速度为$1$rad/s，那么线速度就是角速度乘半径，为$||p||\sin \varphi$；而上式结果也为$||p||\sin \varphi$。

根据前文的介绍，上式可以写作：
$$\dot{p}=[\hat{\omega }]p$$
这是一个矩阵的线性微分方程，它的解为：
$$p(t)=e^{[\hat{\omega }]t}p(0)$$
因为旋转角速度为$1$rad/s，所以$t$和$\theta$是等价的：
$$p(\theta )=e^{[\hat{\omega }]\theta }p(0)$$
可以看出，$e^{[\hat{\omega }]\theta }$在这里充当了旋转矩阵的角色。
又根据矩阵指数的性质：
$$e^{[\hat{\omega }]\theta }=I+\sin \theta [\hat{\omega }]+(1-\cos \theta )[\hat{\omega }{]}^2$$
所以有：
$$\mathrm{Rot}(\hat{\omega },\theta )=e^{[\hat{\omega }]\theta }=I+\sin \theta [\hat{\omega }]+(1-\cos \theta )[\hat{\omega }{]}^2\in SO(3)$$
这也就是著名的罗德里得斯公式。也就是旋转的指数表达方法。

需要注意的是，这里也需要考虑$\hat{\omega }$是表示在哪个坐标系下。如果$\hat{\omega }$是表示在固定坐标系下的，那么是左乘$\mathrm{Rot}(\hat{\omega },\theta )\cdot R$；如果$\hat{\omega }$是表示在体坐标系下的，那么是右乘$R\cdot \mathrm{Rot}(\hat{\omega },\theta )$。

2. 指数坐标与旋转矩阵的相互转化

上文介绍了矩阵指数运算：$\exp :[\hat{\omega }]\theta \in so(3)\to R\in SO(3)$
这里新引入矩阵对数运算：$\log :R\in SO(3)\to [\hat{\omega }]\theta \in so(3)$

指数坐标通过矩阵的指数运算就可以转换成旋转矩阵了，也就是上文的罗德里得斯公式。但如何从旋转矩阵回到指数坐标呢?

不好意思，这个问题先空着。。。以后在更。。。

3. 刚体运动的表示

刚体运动的表示包括了刚体的旋转与刚体的平移。这是在旋转的基础上，进一步的扩展。

3.1 变换矩阵——特殊欧式群

很自然的选择就是，用刚体的旋转矩阵和平移矢量来表达刚体运动。我们常常会用到的一种形式就是：齐次变换矩阵（Transformation matrix）。他和特殊欧式群的关系密不可分，其定义如下：

特殊欧式群（Special Euclidean Group）$SE(3)$，就是以下$4\times 4$实数矩阵$T$的集合。
$$T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_1\\ r_{21}& r_{22}& r_{23}& p_2\\ r_{31}& r_{32}& r_{33}& p_3\\ 0& 0& 0& 1\end{array}\right]$$
其中$R\in SO(2),p\in {\mathbb{R}}^2$。

变换矩阵的性质：

• $T^{-1}={\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]}^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]$
• 两个变换矩阵相乘还是变换矩阵
• $\left(T_1{T}_2\right)T_3=T_1\left(T_2{T}_3\right)$
• $T_1{T}_2̸=T_2{T}_1$
• $\Vert Tx-Ty\Vert =\Vert x-y\Vert$，$⟨Tx-Tz,Ty-Tz⟩=⟨x-z,y-z⟩$，即保距保角（刚体变换）

变换矩阵的用法：
和旋转矩阵的三种用法类似，变换矩阵也有三种用法。

• 代表刚体的位姿（一种位姿表示方法）
• 改变一个向量或坐标系的参考坐标系（一个改变向量表达的参考坐标系的算子）
• 变换一个向量或坐标系的实际位置（一个转动和平移向量的操作）

3.2 Twists

简单的说，Twist是一个综合了刚体线速度和角速度（或称为spatial velocity）的六维向量。
在前文中，我们已经关于角速度和旋转矩阵的关系进行了分析。这部分中，我们要进一步考量Twist向量（spatial velocity）和变换矩阵的关系。

根据上文的介绍，我们定义{b}系(body系)在{s}系(space系或fixed系)下的表示为：
$$T_{sb}(t)=T(t)=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right]$$
其中，$R$代表$R_{sb}$，$p$代表$p_{sb}$。

参考前文中有$R^{-1}\dot{R}$，我们可以对$T^{-1}\dot{T}$进行分析：
$$\begin{array}{rl}{T}^{-1}\dot{T}& =\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\dot{R}& \dot{p}\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}{R}^T\dot{R}& R^T\dot{p}\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}\left[{\omega }_b\right]& v_b\\ 0& 0\end{array}\right]\end{array}$$
我们定义体坐标系下的Twist为：
$${\mathcal{V}}_b=\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]\in {\mathbb{R}}^6$$
因此，仿照$[\omega ]$的定义，我们运用twist进一步定义了如下记号：
$$T^{-1}\dot{T}=\left[{\mathcal{V}}_b\right]=\left[\begin{array}{cc}\left[{\omega }_b\right]& v_b\\ 0& 0\end{array}\right]\in se(3)$$

与之类似，我们可以反过来分析$\dot{T}{T}^{-1}$：
$$\begin{array}{rl}\dot{T}{T}^{-1}& =\left[\begin{array}{cc}\dot{R}& \dot{p}\\ 0& 0\end{array}\right]\left[\begin{array}{cc}{R}^T& -R^{T}p\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\dot{R}{R}^T& \dot{p}-\dot{R}{R}^{T}p\\ 0& 0\end{array}\right]\\ & =\left[\begin{array}{cc}\left[{\omega }_s\right]& v_s\\ 0& 0\end{array}\right]\end{array}$$
这里需要注意，为什么$v_s=\dot{p}-\dot{R}{R}^{T}p$成立呢？
先看下面这幅图:

$v_s$这个速度表达的含义可以通过这幅图说明。可以想象有一个边界无限大的运动刚体（即图中椭圆），它与{b}系固连。而$v_s$代表的就是，在某一个瞬时时刻，这个刚体上恰好与{s}系原点对应的那一点的瞬时速度在{s}系下的表示。所以$v_s$应包含两部分，一部分就是{b}系原点与{s}系原点的相对速度，以及{s}系原点相对于{b}系旋转引起的速度。
$$\begin{array}{rl}{v}_s& =\dot{p}+v_{rot}\\ & =\dot{p}+R^T\cdot ({\omega }_b\times p_b)\\ & =\dot{p}+R^T\cdot [{\omega }_b\times (-Rp)]\\ & =\dot{p}+R^T{\omega }_b\times (-R^{T}Rp)\\ & =\dot{p}-{\omega }_s\times p\\ & =\dot{p}-\dot{R}{R}^{T}p\end{array}$$
我们定义固定坐标系下的Twist为（spatial twist）：
$${\mathcal{V}}_s=\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]\in {\mathbb{R}}^6,\left[{\mathcal{V}}_s\right]=\left[\begin{array}{cc}\left[{\omega }_s\right]& v_s\\ 0& 0\end{array}\right]=\dot{T}{T}^{-1}\in se(3)$$

根据以上定义，进一步的，我们可以分析body twist ${\mathcal{V}}_b$和spatial twist ${\mathcal{V}}_s$的关系:
$$\begin{array}{rl}\left[{\mathcal{V}}_b\right]& =T^{-1}\dot{T}\\ & =T^{-1}\left[{\mathcal{V}}_s\right]T\end{array}$$
$$\left[{\mathcal{V}}_s\right]=T\left[{\mathcal{V}}_b\right]T^{-1}$$
$${\mathcal{V}}_s=\left[\begin{array}{cc}R\left[{\omega }_b\right]R^T& -R\left[{\omega }_b\right]R^{T}p+R{v}_b\\ 0& 0\end{array}\right]$$
最终，我们可以得到二者的关系为：
$$\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]$$
为了表示方便起见，这里又要引入一个新的定义：adjoint representation $\left[{\mathrm{A}\mathrm{d}}_T\right]$
$$\left[{\mathrm{A}\mathrm{d}}_T\right]=\left[\begin{array}{cc}R& 0\\ [p|R& R\end{array}\right]\in {\mathbb{R}}^{6\times 6}$$
$\left[{\mathrm{A}\mathrm{d}}_T\right]$的性质如下：

• ${\mathrm{Ad}}_{T_1}\left({\mathrm{Ad}}_{T_2}(\mathcal{V})\right)={\mathrm{Ad}}_{T_1{T}_2}(\mathcal{V})$
• $\left[{\mathrm{A}\mathrm{d}}_{T_1}\right]\left[{\mathrm{A}\mathrm{d}}_{T_2}\right]\mathcal{V}=\left[{\mathrm{A}\mathrm{d}}_{T_1{T}_2}\right]\mathcal{V}$
• ${\left[{\mathrm{A}\mathrm{d}}_T\right]}^{-1}=\left[{\mathrm{A}\mathrm{d}}_{T^{-1}}\right]$
• ${\mathrm{Ad}}_{T-1}\left({\mathrm{Ad}}_T(\mathcal{V})\right)={\mathrm{Ad}}_{T-1}T(\mathcal{V})={\mathrm{Ad}}_I(\mathcal{V}))=\mathcal{V}$

所以，最终，${\mathcal{V}}_b$和spatial twist ${\mathcal{V}}_s$的关系为：
$${\mathcal{V}}_s=\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]=\left[\begin{array}{cc}R& 0\\ [p]R& R\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]=\left[{\mathrm{A}\mathrm{d}}_{T_{sb}}\right]{\mathcal{V}}_b$$
$${\mathcal{V}}_b=\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]=\left[\begin{array}{cc}{R}^T& 0\\ -R^T[p]& R^T\end{array}\right]\left[\begin{array}{c}{\omega }_s\\ v_s\end{array}\right]=\left[{\mathrm{A}\mathrm{d}}_{T_{\mathrm{b}\mathrm{s}}}\right]{\mathcal{V}}_s$$

本篇文章结束。

注：为了更好的阅读体验，避免文章过长，原本一章的内容被拆分成了多篇文章。