前言
本文整理了一些关于《现代机器人：力学，规划，控制》（Modern Robotics: Mechanics, Planning, and Control）的运动学部分内容，为机器人学的部分的第二篇笔记，之前的机器人机构学部分在这里，手写的有些粗糙且不全面。本文整理的现代机器人学部分将涵盖部分其内容，并尽量结合我自己的理解通俗解释。
配置空间 Configuration Space（C Space）
配置空间和自由度
C空间是Configuration Space的缩写，如下是对C空间的定义。
机器人的配置是对机器人每个点位置的完整规范。表示配置所需的最小实值坐标数 n 是机器人的自由度数 （dof）。包含机器人所有可能配置的 n 维空间称为配置空间 （C-space）。机器人的配置由其 C 空间中的一个点表示。
常用自由度的求法：自由度 = 点的自由度之和 - 独立约束的数目
用系统变量与方程数表示：自由度 = 变量数 - 独立方程数
公式表示(格鲁布勒公式)： $dof = m(N-1) - \underset{i=1}{\overset{J}{\Sigma}}c_i$
拓扑学
一个球面上的点的空间是二维的，平面上的点也是二维的。
如果一个空间可以在不切割或粘合的情况下不断变形成另一个空间，那么这两个空间在拓扑上是等价的。说明：球面和平面不等价，因为球面经过切割和伸展才能变成平面。
拓扑上不同的一维空间包括圆、直线和直线的闭合区间。 圆被数学的描述为 $S$ 或 $S^1$ ,即一维球面，线被写作 $E$ 或 $E^1$ ，表示一维的欧几里德(或“平坦”)空间
在高维中，$R^n$ 是n维欧几里德空间，$S^n$ 是(n + 1)维空间中球面的n维曲面，例如， $S^2$ 是三维空间中球体的二维表面。
空间的拓扑是空间本身的一个基本属性，与我们如何选择坐标来表示空间中的点无关。
下面说明对于空间中点的描述存在奇点的问题。什么？你不知道奇点是什么？举个例子：对于一个球面空间，当用经纬坐标来表示点的位置的时候，南北极的位置就是奇点。在奇点处，一点点位移会导致很大的坐标变化，因此在奇点的位置速度可能会趋于无穷。那如何解决？
- 在空间上使用多个坐标图，其中每个坐标图都是只覆盖空间的一部分的显式参数化，这样在每个图中就没有奇点。
- 使用隐式表示，而不是显式参数化。 隐式表示法将n维空间视为嵌入在超过n维的欧几里德空间中，就像一个二维单位球可以被视为嵌入在三维欧几里德空间中的表面一样。 在之后的各个章节中将使用隐式表示。
这个隐式表示就是指用方程组来描述C空间。老子不用坐标表示就没奇点了很合理吧…同时，隐式表示也可以用方程组中的未知量组成的列向量表示，如四杆机构的C空间可表示为：
\[\theta = [\theta_1...\theta_n]^T\in R^n\]这种形式的k（k<=n）个独立方程的loop-closure equations,这种约束被称为完整约束，它能降低C空间的维数,C空间可以看作是一个维数为n-k的曲面
\[g(\theta) = \left[ \begin{matrix} g_1(\theta_1...\theta_n)\\ ...\\ g_k(\theta_1...\theta_n)\\ \end{matrix} \right] = 0\]假设闭链机器人的 loop-closure equations为$g(\theta) = 0$,$g:R^n\rightarrow R^k$,以$\theta(t)$运动，也满足$g(\theta(t)) = 0$就有：
\[\begin{align*} & \frac{d}{dt}g(\theta(t)) = 0\ \ \ & \frac{dg}{dt}(\theta)\dot \theta = 0 & && \end{align*}\]令$A(\theta)$为$g(\theta)$的积分则有：
\[A(\theta)\dot \theta = 0\]这种形式的速度约束称为Pfaffian-constraints，$g(\theta)$的完全约束也被称为可积约束（integrable constraints），即它们所提供的速度约束可以积分得到等效的结构约束。
同时，还有$g(\theta)$不可积的情况，被称为非完整约束（nonholonomic constraint）这种约束降低了系统速度的维数，但没有降低C空间的维数，例子如下：
考虑一枚半径为r的硬币在如图所示的平面上滚动：
硬币的外形由平面上的接触点(x, y)给出，转向角为$\phi$，旋转角为$\theta$，描述硬币的C空间就是$R^2\times T^2$(这里用了笛卡尔积，)，$T^2$是转向角$\phi$，旋转角$\theta$形成的二维环，整个C空间是四维的。 硬币滚动时没有滑动，做纯滚动，那么硬币向某个方向滚动的速度表示为：
\[\left( \begin{matrix} \dot x\\ \dot y \end{matrix} \right) = r\dot \theta \left( \begin{matrix} cos \phi\\ sin \phi \end{matrix} \right)\]把C空间中的四个坐标写成矢量：
\[q = [q_1\ q_2\ q_3\ q_4]^T = [x,y,\phi,\theta]\]上述纯滚动约束可表示为：
\[\left( \begin{matrix} 1 & 0 & 0 -rcos q_3\\ 0 & 1 & 0 -rsin q_3\\ \end{matrix} \right) \dot q= 0\]上式就是Pfaffian-constraints 的形式了,这些约束条件是不可积的
任务空间和工作空间
任务空间是机器人任务可以自然表达的空间。
工作区是机器人的末端执行器可以达到的配置的规范。
挺好理解的就不解释了
刚体运动 Rigid-Body Motions
任何刚体运动都可以被分解为对固定参考系的平动以及绕固定轴的转动，运动合成之后形成类似螺旋的运动，即绕同一固定轴旋转和平移运动的合成。
本章把线速度和角速度表达成一个在 $R^6$ 中的向量，力矩和力也一样，力矩和力组成的向量被称为spatial force or wrench。
坐标变换
（平面的坐标变换还是那个经典例子）空间中坐标变换有多种方法：旋转矩阵（必须是正交阵）+向量法、三参数欧拉角、指数坐标、RPY角roll–pitch–yaw angles、Cayley–Rodrigues parameters、四元数等
下面是一部分数学解释：对于正交旋转矩阵 $R\in SO(3)$,向量 $\omega_s = [\omega_1,\omega_2,\omega_3]$，令
\[[\omega_s]=\left( \begin{matrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 &-\omega_x \\ -\omega_y & \omega_x & 0 \end{matrix} \right)\in so(3)\]这里，SO(3) 是特殊正交群(Special Orthogonal Group)，它是所有三维旋转矩阵的集合；so(3) 是三维旋转的李代数 (Lie Algebra)，它是与 SO(3) 对应的李代数，是由 3×3 的反对称矩阵组成的线性空间，表示三维旋转的无穷小生成元。
旋转矩阵+向量表示法
旋转矩阵性质：互逆$^A_BP = ^B_AP^{-1} = ^B_AP^T$
旋转矩阵法描述的坐标变换：$^AP = ^A_BR^BP + ^AP_{B0}$
旋转矩阵表示为：
\[R=Rot(\hat x, \theta)=\left( \begin{matrix} c_\theta+\hat \omega_1^2(1-c_\theta) & \hat\omega_1 \hat\omega_2(1-c_\theta)-\hat\omega_3s_\theta & \hat\omega_1 \hat\omega_3(1-c_\theta)+\hat\omega_2s_\theta \\ \hat\omega_1 \hat\omega_2(1-c_\theta)+\hat\omega_3s_\theta & c_\theta+\hat \omega_1^2(1-c_\theta) & \hat\omega_2 \hat\omega_3(1-c_\theta)-\hat\omega_2s_\theta \\ \hat\omega_1 \hat\omega_3(1-c_\theta)-\hat\omega_2s_\theta & \hat\omega_2 \hat\omega_3(1-c_\theta)+\hat\omega_1s_\theta & c_\theta+\hat \omega_3^2(1-c_\theta) \\ \end{matrix} \right)\]下面求解角速度与线速度
角速度与线速度关系为：
\[\begin{align*} & \dot{\hat x} = \omega\times \hat x \ \ & \dot{\hat y} = \omega\times \hat y \ \ \ \ & \dot{\hat z} = \omega\times \hat z \ & && \end{align*}\]因此求得角速度就可以得到线速度
令R(t)为动系b相对于定系s的旋转矩阵，则它对时间的一阶导数为$\dot R(t)$
\[\dot R(t) = [\omega_s\times r_1,\omega_s\times r_2,\omega_s\times r_3] = \omega\times R\]其中，
\[[\omega_s] = \left( \begin{matrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 &-\omega_x \\ -\omega_y & \omega_x & 0 \end{matrix} \right)\]由斜对称矩阵性质：$R[\omega]R^T = [R\omega]\quad$ (向量$\omega$的斜对称矩阵为$[\omega]$)
可得：$\dot R = [\omega_s]R$
由此求出在定系中的角速度：
\[[\omega_s] = \dot RR^{-1}\]在动系中的角速度为：
\[[\omega_b] = [R_{sb}^{-1}\omega_s] = R_{sb}^T[\omega_s]R_{sb} = R^T\dot R = R^{-1}\dot R\]因此角速度在不同坐标之间变换和位置坐标变换相同
指数坐标表示法
指数坐标表示法只需两个参数：$[\hat \omega]$和$\theta$。分别为旋转的轴和角度，指数坐标和旋转矩阵作用相同
对于向量微分方程：$\dot x(t) = Ax(t),x(t)\in R^n,A\in R^{n\times n}$
方程的解为：$x(t) = e^{At}x_0$
泰勒展开：$e^{At} = I+At+\frac{At^2}{2!}+\frac{At^3}{3!}+……$
由于$Ae^{At} = e^{At}A$，如果$A = PDP^{-1},D\in R^{m\times n},P\in R^{n\times n}$
那么可得：
\[\begin{split} e^{At} &= I+At+\frac{At^2}{2\!}+\frac{At^3}{3\!}+......\\ &= I+(PDP^{-1})t+(PDP^{-1})(PDP^{-1})\frac{t^2}{2\!}+......\\ &= P(I+Dt+\frac{(Dt)^2}{2\!}+......)P^{-1}\\ &= Pd^{Dt}P^{-1} \end{split}\]矩阵指数有如下性质：$\frac{d(e^{At})}{dt} = Ae^{At} = e^{At}A$
准备工作结束，下面开始旋转指数坐标的推导，会利用上推出来的性质。
旋转指数坐标可以看成：
- 绕轴$\hat\omega(\hat\omega\in R^3,|\hat\omega| = 1)$旋转$\theta\in R$
- 将第一步中的两者相乘形成的三维向量$\hat\omega\in R^3$
假如向量p(0)绕轴旋转$\theta$为$p(\theta)$如下图
向量p的线速度为$\dot p$，可以表示为：$\dot p = \hat\omega\times p = [\hat\omega]p$
由上得其解为$p(t) = e^{[\hat\omega]t}p(0)$
旋转后的新位置为：$p(\theta) = e^{[\hat\omega]\theta}p(0)$
由于$[\hat\omega]$是斜对称矩阵，因此有性质：$[\hat\omega]^{n+2} = [-\hat\omega]^n, n\in Z$
将$e^{[\hat\omega]\theta}$泰勒展开，并奇偶项拆分，利用如上性质，并利用sin与cos的泰勒级数可得(有点类似于欧拉公式)
\[Rot([\hat\omega,\theta]) = e^{[\hat\omega]\theta} = I+\sin\theta[\hat\omega] + (1-\cos\theta)[\hat\omega]^2\in SO(3)\]上式就是Rodrigues’formula
上述推导用$[\hat\omega]\theta$表示旋转矩阵的指数矩阵，则反过来旋转矩阵R的对数矩阵（matrix logarithm），通过这种方式在旋转矩阵和向量之间搭建了桥梁（即通过对指映射完成了SO（3）与so（3）的转换），下面推导对数矩阵
对于对称的旋转矩阵，使其减去其转置可得：
\[\begin{split} & r_{32} - r_{23} = 2\hat\omega_1\sin\theta\\ & r_{13} - r_{31} = 2\hat\omega_2\sin\theta\\ & r_{21} - r_{12} = 2\hat\omega_3\sin\theta\\ \end{split}\]上式 $\sin\theta\neq0$则有：
\[\begin{split} & \hat\omega_1 = \frac{1}{2\sin\theta}(r_{32}-r_{23})\\ & \hat\omega_2 = \frac{1}{2\sin\theta}(r_{13}-r_{31})\\ & \hat\omega_3 = \frac{1}{2\sin\theta}(r_{21}-r_{12})\\ \end{split}\]即
\[[\hat\omega] = \frac{1}{2\sin\theta}(R-R^T)\]当$\sin\theta=0$时， $\theta = \pm k\pi,k\in Z$ 当k为偶数时，R=I,转轴$\hat\omega$无意义；当k为奇数时， $R = I\pm 2[\hat\omega]^2$
\[\hat\omega_i = \pm\sqrt{\frac{r_{ii}+1}{2}},\quad i = 1,2,3.\]齐次变换
就是讲了个矩阵，把旋转矩阵和平移向量整合到一起然后在下面加了一行凑方用的。
\[T = \left( \begin{matrix} R & p\\ 0 & 1 \end{matrix} \right)\]运动旋量和力旋量
运动旋量
动系[b]对于定系[s]的变换矩阵为：
\[T_{sb} = T(t) = \left( \begin{matrix} R(t) & p(t)\\ 0 & 1 \end{matrix} \right)\]由于
\[\begin{align*} & [\omega_s] = \dot R R^{-1} & \ [\omega_b] = R^{-1}\dot R && \end{align*}\]因此
\[\begin{split} T^{-1}\dot T &= \left( \begin{matrix} R^T & -R^Tp\\0 & 1 \end{matrix} \right)\left( \begin{matrix} \dot R & \dot p\\0 & 0 \end{matrix}\right)\\ &=\left( \begin{matrix} R^T\dot R & R^T\dot p\\ 0 & 0 \end{matrix} \right)\\ &= \left( \begin{matrix} [\omega_b] & v_b\\ 0 & 0 \end{matrix} \right)\\ \end{split}\]令$V_b = [\omega_b\ v_b]^T \in R^6$，称为 body twist
而
\[\dot T T^{-1} = \left( \begin{matrix} [\omega_s] & v_s\\ 0 & 0 \end{matrix} \right)\]令$V_s = [\omega_s\ v_s]^T \in R^6$，成为 spatial twist
则$V_b$和$V_s$的关系为：
\[\begin{align*} & [V_b] = T^{-1}[V_s]T & [V_s] = T[V-b]T^{-1} && \end{align*}\]有结论：
\[\left( \begin{matrix} \omega_s \\ v_s \end{matrix} \right) = \left( \begin{matrix} R & 0\\ [p]R & R \end{matrix} \right)\left( \begin{matrix} \omega_b \\ v_b \end{matrix} \right)\]令转换矩阵$T=(R,p)\in SE(3)$的伴随表示 adjoint representation为$[Ad_T]$
\[[Ad_T] = \left( \begin{matrix} R & 0\\ [p]R & R \end{matrix} \right)\in R^{6\times6}\]下面用twist来描述运动旋量 screw
旋转轴为$\hat s$（旋转轴的单位向量）,绕轴旋转角速度为$\dot \theta$，twist为V，q是轴上一个点的坐标，h为螺距（线速度与角速度之比）。
旋量表示有两个特殊情况：当螺距为0时为纯旋转，当螺距为无穷时为纯平移
规范化screw axix的定义为：
\[S = \left( \begin{matrix} \omega \\ v \end{matrix} \right)\in R^6\]其矩阵表示为：
\[[S] = \left( \begin{matrix} [\omega] & v\\ 0 & 0 \end{matrix} \right)\in se(3)\]ab两个坐标系的screw axix相对转换为：
\(S_a = [Ad_{T_{ab}}]S_b\) \(S_b = [Ad_{T_{ba}}]S_a\)
用旋量的指对矩阵与旋转矩阵的指对矩阵类似。
\[exp:se(3)\rightarrow SE(3)\] \[log:se(3)\rightarrow SE(3)\]运动旋量的指数矩阵为：
\[e^{[S]\theta} = \left( \begin{matrix} e^{[\omega]\theta}\ & (I\theta + (1-\cos\theta)[\omega] + (\theta-\sin\theta)[\omega]^2)v\\ 0 & 1 \end{matrix} \right)\]再结合角速度向量的指数矩阵
\[Rot([\hat\omega,\theta]) = e^{[\hat\omega]\theta} = I+\sin\theta[\hat\omega] + (1-\cos\theta)[\hat\omega]^2\in SO(3)\]力旋量
力旋量由力矩和力组成，与运动旋量类似。
\[F_b = \left( \begin{matrix} m_b\\ f_b \end{matrix} \right)\]正运动学
正运动学就是根据机器人的关节状态来计算工作爪的位置、速度等
位置关系
计算固定坐标系s（机器人基座）到动坐标系b（机器人爪）的转移矩阵$T(\theta)$的步骤：
- 令$M$为初始状态的转移矩阵，即当关节角都置零的时候的转移矩阵。
- 找到初始状态时，s坐标系中各关节的旋转轴（用旋量表示）$S_1,S_2,…,S_n$
- 对关节角赋值，按以下公式计算转移矩阵
此外，如果当运动旋量是在b坐标系的时候，即$B_1,B_2,…B_n$，转移矩阵公式应变为：
\[T(\theta) = Me^{[B_1]\theta_1}e^{[B_2]\theta_2}...e^{[B_n]\theta_n}\]并且，计算关节的旋转轴时，应按照如下顺序：
- 如果是在s坐标系中进行的话，应从最后一个关节（靠近b坐标系的）开始计算，并逐渐向内。因为这样上一个关节运动时的结果不会加到下一个关节上（上一个关节不在下一个关节与s坐标系之间）
- 如果是在b坐标系中进行的话，应从最前一个关节（靠近s坐标系的）开始计算，并逐渐向外。因为这样上一个关节运动时的结果不会加到下一个关节上（上一个关节不在下一个关节与b坐标系之间）
速度运动学
速度关系用雅可比矩阵描述。雅可比矩阵就是对运动方程 $x(\theta) = f(\theta(t))$ 求导，
\[\frac{\partial f}{\partial \theta} = \left( \begin{matrix} \frac{\partial f_1}{\partial \theta_1} & ... & \frac{\partial f_1}{\partial \theta_n}\\ &...\\ \frac{\partial f_m}{\partial \theta_1} & ...& \frac{\partial f_m}{\partial \theta_n} \end{matrix} \right)\in R^{m\times n}\]雅可比矩阵可以分析奇异位形的问题。
奇异位形：机器人在某些特定位置或姿态下，发生某种特殊情况，使得机器人的控制系统失去某些自由度或变得不稳定。
当雅可比矩阵为非满秩的情况下会出现奇异位形。
对于一个含有两个旋转关节的机器人，绘制其关节$\dot\theta_1 \dot\theta_2$坐标图。和工作空间$\dot x_1 \dot x_2$坐标图。则通过雅可比矩阵可以将关节坐标的一个圆映射为一个椭圆。但这个椭圆的形状取决于机器人的位形。通过这个椭圆可以看出什么方向可达到的速度最大。当处于奇异位形时，该椭圆退化为一条线段。
当关节数为3时，圆和椭圆进化为球和椭球
静力学
静力学同样用雅可比矩阵描述。下面是关节空间的力通过转置雅可比矩阵映射到工作空间的公式推导：
\[\begin{align*} P= &\dot\theta^T\tau = v^T_{tip}f_{tip} \\ &\dot\theta^T\tau = (J(\theta)\dot\theta)^T f_{tip} \\ & \dot\theta^T\tau = \dot\theta^TJ^T(\theta)f_{tip} \\ &\tau = J^T(\theta)f_{tip} && \end{align*}\]同样，关节力坐标图中的一个圆也可以通过转置雅可比矩阵映射到工作空间力坐标图中的一个椭圆。
末端力的极限在某种意义上与末端速度极限相反。在难以移动的方向上很容易施加力，在易于移动的方向上很难施加力。
空间雅可比矩阵 Space Jacobian
空间雅可比矩阵的每一列，就是改关节速度为1，而其他关节速度为0时的末端运动旋量
空间雅可比矩阵定义为：
\[v_s = J_S(\theta)\dot\theta\]其中，$J_S(\theta) = [J_{S1}J_{S2}(\theta)…J_{sn}(\theta)]\in R^{6\times n}$
且$J_s1 = S_1$
\[J_{si}(\theta) = [Ad_{e^{[S_1]\theta_1}...e^{[S_{i-1}]\theta_{i-1}}}]S_i\]物体雅可比矩阵 Body Jacobian
物体雅可比矩阵定义为
\[v_b = J_b(\theta)\dot\theta\]其中，$J_b = [J_{b1}(\theta)…J_{bn-1}(\theta)J_{bn}]\in R^{6\times n}$
且$J_{bn} = B_n$
\[J_{bi}(\theta) = [Ad_{e^{[-B_n]\theta_n}...e^{-[B_{i+1}]\theta_{i+1}}}]B_i\]物体雅可比矩阵与s坐标系选择无关
物体雅可比与空间雅可比关系为：
\[J_b(\theta) = [Ad_{T_{bs}}]J_s(\theta)\]