1. Articulated robots（关节型机器人）

Mechanisms（机构）

Def：A system of 2 or more linked machine parts with kinematic pairs（运动副）
- 运动副的元素：点、线、面
- 分类：
  - 按压强：高副（点、线）、低副（面）
  - 按自由度：平面副、空间副
- 机器人机械臂基本都是平面转动副
自由度（Degree of Freedom，DoF）
- 机器人的自由度认为是可被独立驱动器的数目
- n：可以动的零部件数目（结构数）
- L：低副数目
- H：高副数目
- $DoF = 3n-2L-H$

Robot Manipulators（机械臂）

Def： A sequence of rigid bodies ~（没抄完）

2. Rotation Geometry（旋转几何体）

Position & Orientation

Coordinate Frame（坐标系）变换的推导

$$
\[\begin{gather*} 对一个矢量r，在不同坐标系下的表达分别为：\\ \begin{cases} ^0r=X\dot I+Y\dot J+Z\dot K\\ ^1r=x\dot i+y\dot j+z\dot k\end{cases}\\ 根据点积的几何意义，可以写出三条表达式：\\ \begin{cases} X=\dot I\cdot \dot r=\dot I\cdot \dot {r^1}=\dot I\cdot x\dot i+\dot I\cdot y\dot j+\dot I\cdot z\dot k\\ Y=...\\Z=... \end{cases}\\ 得：^0r=\begin{bmatrix}X\\Y\\Z\end{bmatrix}= \,^0R_1\begin{bmatrix}x\\y\\z\end{bmatrix}=\,^0R_1\,^1r\\ 且：^0R_1=\,^1R_0^{(-1)}=\,^1R_0^T(即：旋转矩阵の逆=转置)\\ ^0R_1(1到0)=\begin{bmatrix} Ii&Ij&Ik\\Ji&Jj&Jk\\Ki&Kj&Kk \end{bmatrix}=\begin{bmatrix} i在OXYZ的向量&y在OXYZ的向量&z在OXYZ的向量 \end{bmatrix}\triangleq \begin{bmatrix} n&s&a \end{bmatrix}\\ 约束：|n|=|s|=|a|=1;\,\,l_1\times l_2=l_3(三条);\\因此还剩下三个自由度，正好对应旋转矩阵\\ 举个例子，1坐标系绕0坐标系z轴逆时针转动，旋转矩阵为：\\ ^0R_1=\begin{bmatrix} cos&sin&0\\-sin&cos&0\\0&0&1 \end{bmatrix} \end{gather*}\]
$$

多考虑几何意义，在手算的时候不需要手撕公式，直接看坐标系1三个轴在坐标系0中的坐标，搞成列向量的形式拼在一起就是旋转矩阵。
两次旋转的两种不同方式：

绕0系某坐标轴转动为1系，1系再绕0系某轴转动为2系：$^0R_2=R_{(1)}R_{(2)}$
绕0系某坐标轴转动为1系，绕1系(自身)某轴转动为2系：$^0R_2=\,^0R_1\,^1R_2$

结论：绕父坐标系(世界坐标系)旋转，左乘新矩阵；绕自身旋转，右乘新矩阵。

Representation of Orientation

固定角描述法

RPY Angels（RPY固定角描述法）：先X，再Y，再Z，全部都默认绕世界坐标系转动。
- R：Roll横滚；P：Pitch俯仰；Y：Yaw侧航。
- $^0R_1=R_Z\,R_Y\,R_X$

欧拉角描述法

Zxz Eular Angles：先Z，再x，再z。
- Z：Precession进动；x：Nutation章动；z：Spin自旋。
- $^0R_1=R_Z\,R_x\,R_z$
RPY Eular Angels：先X，再y，再z。

Triple-Angels

ABC（CBA）Type：$\beta\in(-\frac{\pi}2,\frac{\pi}2)$
ABA Type：$\beta\in(0,\pi)$

在$\beta\ne\pm\frac{\pi}2$时的RPY Angle下的转换公式：
\[ \beta=-arcsin(r_{31});\gamma=arctan2(r_{32},r_{33});\alpha=arctan2(r_{21},r_{11} ) \]
在$\beta\ne0 、\pi$时的Eular Zxz Angle下的转换公式：
\[ \beta=arccos(r_{33});\alpha=arctan2(r_{13},-r_{23});\gamma=arctan2(r_{32},r_{33}) \]
为了更好的完成后续内容，请不要让你的机器人出现$\beta=\pm\frac{\pi}2$的奇异性现象，否则将无法确认$\alpha,\gamma$的值。

其中，arctan2是双变量反正切函数，跟arctan基本一样，只是多规定了一下象限，能直接帮你判断象限和具体角度，一般用于编程。

3. Forward Kinematics（正运动学）

Position & Orientation

旋转矩阵+偏移矢量：$^0r={}^0R_1{}^1r+{}^0d$

旋转矩阵（非齐次式） ——> 转移矩阵（齐次式）：
\[ \begin{gather*} \begin{bmatrix}{}^0r\\1\end{bmatrix}= \begin{bmatrix}{}^0R_1&{}^0d\\0_{1\times3}&1\end{bmatrix}\begin{bmatrix}{}^1r\\1\end{bmatrix}= {}^0T_1\begin{bmatrix}{}^1r\\1\end{bmatrix} \end{gather*} \]
其中：
\[ \begin{gather*}^0d=d_{3\times1}=\begin{bmatrix}d_x\\d_y\\d_z\end{bmatrix}\end{gather*} \]
同样满足链式法则：$^0T_2={}^0T_1\cdot{}^1T_2$（绕自身坐标系右乘，绕世界坐标系左乘）

但是，与旋转矩阵不同的是，$^0T_1^{-1}={}^1T_0\ne{}^0T_1^T$

Denavit-Hartenberg Method

一种用于描述几乎所有机械臂运行工作方式的方法理论：find T between link i and link i-1for relative motions given (rotation and geometries of machanical parts，旋转和几何尺寸)

D-H Parameters：
1. 建立坐标系：将 link i 绕着 link i-1 旋转的转轴定义为Z_i轴，将 link i-1 指向 link i 的方向定义为X轴（$x _ i=\pm(z _ {i-1}\times z _ i)/||z _ {i _ 1}\times z _ i||$）
2. 连杆偏距d：前后两个X轴在Z_i-1轴上的距离。旋转角$\alpha$：z_i-1和z_i轴的夹角（z_i-1绕着x_i旋转）。连杆长度a：z_i-1经过旋转角后（与z_i平行）与z_i的距离。关节角$\theta$:前后两个X轴，绕Z_i-1旋转的夹角（可变）。（按照顺序一个一个变换）（如果X/Z轴共线的话，那么d/a长度就是0）

Transformation Matrix

\[ \begin{gather*} ^{i-1}T_i=\begin{bmatrix} c\theta_i&-c\alpha_is\theta_i&s\theta_is\alpha_i&a_ic\theta_i\\ s\theta_i&c\alpha_ic\theta_i&-s\alpha_ic\theta_i&a_is\theta_i\\ 0&s\alpha_i&c\alpha_i&d_i\\ 0&0&0&1 \end{bmatrix} \end{gather*} \]

注：同一个量平移和旋转可交换，两个旋转不可交换

4. Inverse Kinematics（逆运动学）

Inverse Kinematics

已知末端点的位姿，求解各个关节角的旋转角度。可能存在两个问题：1）可能没有解析解（但是一般的工程师不会有那么NT给自己设计没有解析解的机械臂）；2）可能有奇异性，即非单一解。

已知：$^0R_6$和$^0d_6$
\[ \begin{gather*} ^0T_6(数据)=\,^0T_3(参数)\,^3T_6(待求)\\ ^0d_6(数据)=\,^0d_3(参数)+\,^0R_3(参数+待求)\,^3r_6(参数)\Rightarrow\,^0R_3(\theta_{1,2,3})\\ ^3R_6(待求)=\,^0R_3^T(上面求出来了)\,^0R_6(已知数据)\Rightarrow\,^3R_6(\theta_{4,5,6}) \end{gather*} \]
最终要求的参数：所有$\theta$角。

5. Trajectory Planning（轨迹规划）

路径规划：
- 路径表示关节空间或操作空间中的点或位置，操纵者在执行指定的运动时必须遵循这些点。
- 路径规划的任务是找到一种可行的方法，将起始位置和姿势连接到目标位置和姿势。
- 路径是对运动的纯几何描述，不回答机器人关节如何移动到目标角度的问题。
轨迹规划：
- 轨迹是指定定时定律的路径，例如根据每个点的速度和（或）加速度。
- 轨迹规划的任务是生成变量的时间序列，这些变量描述末端执行器相对于所施加的约束随时间的位置和方向

Planning in joint space（关节空间）

使用三阶多项式：（常用）

如果使用五阶多项式，其结果为：（推导略，因为六个自由度，因此最多五阶）

可以参考Ex 5-2

Trapezoidal Velocity Trajectory（梯形速度轨迹）

通过设置特定的速度和加速度边界，可以用几个低阶多项式代替高阶多项式轨迹。
时间最优轨迹是让关节在所有约束条件下以最大速度工作。
从t0-t1-t2-tf，分别是匀加速、匀速（max speed）和匀减速

求解过程看PPT，这里提供结果：

Planning in Operational Space（笛卡尔坐标系）

考虑到直接运动学引入的非线性效应，关节空间中的规划导致末端执行器的运动不容易预测。
笛卡尔轨迹规划是规划最自然的应用。只要希望末端执行器运动在操作空间中遵循几何指定的路径，就有必要直接在同一空间中规划轨迹执行。
规划可以通过插值一系列规定的路径点来完成，也可以通过以准时的方式生成分析运动基元和相对轨迹来完成。
在这两种情况下，通过反向运动学算法实时利用由操作空间变量获得的值的时间序列来获得关节空间变量的相应值序列。

6. Different Kinematics（微分运动学）

Representation of Angular Velocity

把微分运算转化为代数运算

\[ \begin{gather*} v=\dot p=\omega\times p=\begin{bmatrix} 0&-\omega_z&\omega_y\\\omega_z&0&-\omega_x\\-\omega_y&\omega_x&0 \end{bmatrix}\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix} \end{gather*} \]

把这个ω的矩阵叫做S(ω)，根据一堆推导，反正最终有：
\[ (记\,^0p=R\,^1p)\,\,\,\,S(\omega)=\dot RR^T\,;\,\dot R=S(\omega)R\,;\,\dot RS(\omega)R^T=S(R\omega) \]
* 在XYZ固定角表示法中，ω与三个旋转角的关系：

罗德里德斯公式：

Forward Velocity Kinematics

已知第i-1个连杆的状态，求第i个连杆的角速度和线速度
\[ \begin{gather*} \omega_i=\omega_{i-1}+\omega_{i}^{i-1}=\omega_{i-1}+R_{i-1}\,^{i-1}\omega_i^{i-1}\\ v_i=v_{i-1}+v_i^{i-1}+\omega_i\times r_i^{i-1} \end{gather*} \]

直线移动关节和旋转关节，都可以做相应的简化，最后用$\sigma_i$把他们统一起来（$\sigma_i=1\,or\,0$）：
- 旋转关节$\sigma_i=0$，直线移动关节$\sigma_i=1$：

然后他就做了一个我看不懂但是看上去很神奇的操作：

Jacobian Matrix

（雅克比矩阵的含义大致就是全微分的偏导矩阵）：微分运动学给出了关节速度与相应末端执行器的线性和角速度之间的映射，该映射由一个矩阵描述，该矩阵称为几何雅可比，取决于操纵器配置。
\[ \begin{gather*} X=[x,y,z,\alpha,\beta,\gamma]^T=\dot T(q)=\begin{bmatrix}T_1(x,y,z,\alpha,\beta,\gamma)\\\dots\\T_6(x,y,z,\alpha,\beta,\gamma)\end{bmatrix}\Rightarrow\dot X=\frac{\partial T(q)}{\partial q}\dot q \end{gather*} \]
将其一分为二：