PS: 本篇文章为台大机器人学学习笔记，B站视频链接如下：
https://www.bilibili.com/video/BV1v4411H7ez?p=7

1. 刚体（rigid body）状态描述：

（DOF：degree of freedom，自由度）

（1）在大地坐标系（{W}， world frame）下：

	平面	空间
移动（DOF）	2	3
转动（DOF）	1	3

–> 在空间中，用6个参数表示刚体运动状态。

（2） 如何整合表示刚体状态：

进一步，在刚体的定点(多为质心)上建立坐标系（{B}，body frame）：

移动	转动
由{B}的原点位置判定	由{B}的姿态判定

（3） 刚体的连动状态如何描述：

利用各个（6个）DOF的微分，将位移（displacement）和姿态(orientation)**转换到速度（velocity）和加速度(acceleration)**等连动状态：

2. 刚体移动描述：

（1）移动：

用向量P$$\vec{P}=\left[\begin{array}{l}{P}_x\\ P_y\\ P_z\end{array}\right]$$描述原点的状态变化：

（2）向量可表达空间关系的两个方式：

① 表示body frame的原点

② 表示body frame 的方向

3. 刚体转动描述：

描述{B}相对{A}的姿态： rotation matrix
（由于对于机械臂来说关节多，因此这里先用A来表示固定的坐标系，而不用W）

公式描述：

左上角为基准：B在A坐标系下的描述；$R$的三个分量为由{A}看{B}的三个basis：${\hat{X}}_B,{\hat{Y}}_B,{\hat{Z}}_B$

投影的角度：将${\hat{X}}_B,{\hat{Y}}_B,{\hat{Z}}_B$分别分解到$A$坐标系下；

4 R的特性

4.1 R描述一个frame的姿态

（1）特性

①
${\hat{X}}_B\cdot {\hat{Y}}_A$为内积，内积前后互换无影响，结果为数字，因此可以对所有内积做前后互换，得到：

即：
$${}_B^{A}R={}_A^B{R}^T$$

②

即：
$${}_B^{A}R{=}_A^B{R}^T{=}_A^B{R}^{-1}$$

（2）特性总结

• 符合正交矩阵（orthogonal matrix）Q特性：
  $$Q{Q}^T=Q^{T}Q=I,Q^{-1}=Q^T$$
• 矩阵中的列向量：长度为1；两两互相垂直。
• $R$包含9个数字，但上述内容包含6个条件，即$R$中只包含了三个自由度，对应了空间中转动只具有3DOFs

4.2 R描述转换关系

向量$P$:
$${}^{B}P{=}^B{P}_x{\hat{X}}_B{+}^B{P}_y{\hat{Y}}_B{+}^B{P}_z{\hat{Z}}_B,$$
$${}^{A}P{=}^A{P}_x{\hat{X}}_A{+}^A{P}_y{\hat{Y}}_A{+}^A{P}_z{\hat{Z}}_A$$
其中，${}^A{P}_x{,}^A{P}_y$等表示在对应方向上的长度。
进一步，可以将${}^A{P}_x$展开为：(${}^{B}P$带入上式)
$${}^A{P}_x{=}^{B}P\cdot {\hat{X}}_A={\hat{X}}_B\cdot {\hat{X}}_A{}^B{P}_x+{\hat{Y}}_B\cdot {\hat{X}}_A{}^B{P}_y+{\hat{Z}}_B\cdot {\hat{X}}_A{}^B{P}_z$$

同理可得${}^A{P}_y{,}^A{P}_z$，

进一步有：

即有：
$${}^{A}P={}_B^{A}R{}^{B}P,$$
${}^{B}P$为$P$在{B}下的表达。

• 功能：使用$R$, 将某坐标点从某个frame转到另一个frame。

4.3 R描述物体转动状态

以${}_B^{A}R$等于${\hat{X}}_B,{\hat{Y}}_B,{\hat{Z}}_B$三个向量在$A$的投影得到的列向量拼接为基础，可以得到以${\hat{Z}}_A$为旋转轴，$\theta$为旋转角度的表示（角度由于常用所以进一步进行简写）：

绕${\hat{X}}_A$有：

绕${\hat{Y}}_A$有：

4.4 R的功能总结