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

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

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

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

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

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

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

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

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

3. 刚体运动的表示

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

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

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

特殊欧式群（Special Euclidean Group）$SE(3)$SE(3)，就是以下$4\times4$4×4实数矩阵$T$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]$T=[R0​p1​]=⎣⎢⎢⎡​r11​r21​r31​0​r12​r22​r32​0​r13​r23​r33​0​p1​p2​p3​1​⎦⎥⎥⎤​
其中$R \in S O(2), p \in \mathbb{R}^{2}$R∈SO(2),p∈R2。

变换矩阵的性质：

• $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]$T−1=[R0​p1​]−1=[RT0​−RTp1​]
• 两个变换矩阵相乘还是变换矩阵
• $\left(T_{1} T_{2}\right) T_{3}=T_{1}\left(T_{2} T_{3}\right)$(T1​T2​)T3​=T1​(T2​T3​)
• $T_{1} T_{2} \neq T_{2} T_{1}$T1​T2​̸​=T2​T1​
• $\|T x-T y\|=\|x-y\|$∥Tx−Ty∥=∥x−y∥，$\langle T x-T z, T y-T z\rangle=\langle x-z, y-z\rangle$⟨Tx−Tz,Ty−Tz⟩=⟨x−z,y−z⟩，即保距保角（刚体变换）

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

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

3.2 Twists

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

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

参考前文中有$R^{-1} \dot{R}$R−1R˙，我们可以对$T^{-1} \dot{T}$T−1T˙进行分析：
$\begin{aligned} 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}{ccc}{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{aligned}$T−1T˙​=[RT0​−RTp1​][R˙0​p˙​0​]=[RTR˙0​RTp˙​0​]=[[ωb​]0​vb​0​]​
我们定义体坐标系下的Twist为：
$\mathcal{V}_{b}=\left[ \begin{array}{c}{\omega_{b}} \\ {v_{b}}\end{array}\right] \in \mathbb{R}^{6}$Vb​=[ωb​vb​​]∈R6
因此，仿照$[\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)$T−1T˙=[Vb​]=[[ωb​]0​vb​0​]∈se(3)

与之类似，我们可以反过来分析$\dot{T} T^{-1}$T˙T−1：
$\begin{aligned} \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{aligned}$T˙T−1​=[R˙0​p˙​0​][RT0​−RTp1​]=[R˙RT0​p˙​−R˙RTp0​]=[[ωs​]0​vs​0​]​
这里需要注意，为什么$v_s=\dot{p}-\dot{R} R^{T} p$vs​=p˙​−R˙RTp成立呢？
先看下面这幅图:

$v_s$vs​这个速度表达的含义可以通过这幅图说明。可以想象有一个边界无限大的运动刚体（即图中椭圆），它与{b}系固连。而$v_s$vs​代表的就是，在某一个瞬时时刻，这个刚体上恰好与{s}系原点对应的那一点的瞬时速度在{s}系下的表示。所以$v_s$vs​应包含两部分，一部分就是{b}系原点与{s}系原点的相对速度，以及{s}系原点相对于{b}系旋转引起的速度。
$\begin{aligned} 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^TRp) \\ &= \dot{p} - \omega_s \times p \\ &= \dot{p} - \dot{R} R^{T} p \\ \end{aligned}$vs​​=p˙​+vrot​=p˙​+RT⋅(ωb​×pb​)=p˙​+RT⋅[ωb​×(−Rp)]=p˙​+RTωb​×(−RTRp)=p˙​−ωs​×p=p˙​−R˙RTp​
我们定义固定坐标系下的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)$Vs​=[ωs​vs​​]∈R6,[Vs​]=[[ωs​]0​vs​0​]=T˙T−1∈se(3)

根据以上定义，进一步的，我们可以分析body twist $\mathcal{V}_{b}$Vb​和spatial twist $\mathcal{V}_{s}$Vs​的关系:
$\begin{aligned}\left[\mathcal{V}_{b}\right] &=T^{-1} \dot{T} \\ &=T^{-1}\left[\mathcal{V}_{s}\right] T \end{aligned}$[Vb​]​=T−1T˙=T−1[Vs​]T​
$\left[\mathcal{V}_{s}\right]=T\left[\mathcal{V}_{b}\right] T^{-1}$[Vs​]=T[Vb​]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]$Vs​=[R[ωb​]RT0​−R[ωb​]RTp+Rvb​0​]
最终，我们可以得到二者的关系为：
$\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]$[ωs​vs​​]=[R[p]R​0R​][ωb​vb​​]
为了表示方便起见，这里又要引入一个新的定义：adjoint representation $\left[\mathrm{Ad}_{T}\right]$[AdT​]
$\left[\mathrm{Ad}_{T}\right]=\left[ \begin{array}{cc}{R} & {0} \\ {[p | R} & {R}\end{array}\right] \in \mathbb{R}^{6 \times 6}$[AdT​]=[R[p∣R​0R​]∈R6×6
$\left[\mathrm{Ad}_{T}\right]$[AdT​]的性质如下：

• $\operatorname{Ad}_{T_{1}}\left(\operatorname{Ad}_{T_{2}}(\mathcal{V})\right)=\operatorname{Ad}_{T_{1} T_{2}}(\mathcal{V})$AdT1​​(AdT2​​(V))=AdT1​T2​​(V)
• $\left[\mathrm{Ad}_{T_{1}}\right]\left[\mathrm{Ad}_{T_{2}}\right] \mathcal{V}=\left[\mathrm{Ad}_{T_{1} T_{2}}\right] \mathcal{V}$[AdT1​​][AdT2​​]V=[AdT1​T2​​]V
• $\left[\mathrm{Ad}_{T}\right]^{-1}=\left[\mathrm{Ad}_{T^{-1}}\right]$[AdT​]−1=[AdT−1​]
• $\operatorname{Ad}_{T-1}\left(\operatorname{Ad}_{T}(\mathcal{V})\right)=\operatorname{Ad}_{T-1} T(\mathcal{V})=\operatorname{Ad}_{I}(\mathcal{V}))=\mathcal{V}$AdT−1​(AdT​(V))=AdT−1​T(V)=AdI​(V))=V

所以，最终，$\mathcal{V}_{b}$Vb​和spatial twist $\mathcal{V}_{s}$Vs​的关系为：
$\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{Ad}_{T_{s b}}\right] \mathcal{V}_{b}$Vs​=[ωs​vs​​]=[R[p]R​0R​][ωb​vb​​]=[AdTsb​​]Vb​
$\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{Ad}_{T_{\mathrm{bs}}}\right] \mathcal{V}_{s}$Vb​=[ωb​vb​​]=[RT−RT[p]​0RT​][ωs​vs​​]=[AdTbs​​]Vs​

本篇文章结束。

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