《机器人学导论》学习笔记

第一章 概论

位姿 = 位置 + 姿态

(x, y, z) 可以确定空间中某一坐标系原点相对于基坐标系的位置，($\alpha$, $\beta$, $\gamma$) 可以确定
该坐标系的朝向。

• 一般来说，机器人具有6个自由度，可以在灵活工作空间内具有任意的位姿；但是对于FPS游戏来说，人物角色具有5个自由度，WASD控制x、y自由度，space控制z自由度，鼠标左旋右旋控制Yaw，鼠标前移后移控制Pitch，但是一般不能旋转，也就是没有Roll参数，这样会导致整个画面旋转，容易晕。

第二章 空间变换

末端执行器位姿的变化既可以认为是坐标系的变化，也可以认为是点的变化，这里的变化指的是旋转和平移，旋转是线性变换(线性变换指一个点变化到另一个点可以由左乘一个矩阵来得到，但是平移是相加)，但是平移是非线性变换，为了能够统一二者，引入了齐次坐标，在齐次坐标下的变换统称为仿射变换。

旋转和平移如下图所示：

• Rotation(绕Z轴旋转$\theta$)
  $$R_Z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

$$\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$

• Translation(三个分量分别平移$△x、△y、△z$)
  $$T(△x,△y,△z)=\left[\begin{array}{c}△x\\ △y\\ △z\end{array}\right]$$

$$\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ z^{{}^{\prime }}\end{array}\right]=\left[\begin{array}{c}△x\\ △y\\ △z\end{array}\right]+\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$

如何要进行连续变换，比如先旋转再平移，则不能写成一个矩阵乘积的形式，所以引入第四维齐次坐标。

• 齐次坐标下的旋转矩阵
  $$R_Z(\theta )=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& 0\\ \sin \theta & \cos \theta & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
• 齐次坐标下的平移矩阵
  $$T(△x,△y,△z)=\left[\begin{array}{cccc}1& 0& 0& △x\\ 0& 1& 0& △y\\ 0& 0& 1& △z\\ 0& 0& 0& 1\end{array}\right]$$

所以最终的变换矩阵可以写成如下形式
$$T_{final}=T(△x,△y,△z)R_Z(\theta )=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& △x\\ \sin \theta & \cos \theta & 0& △y\\ 0& 0& 1& △z\\ 0& 0& 0& 1\end{array}\right]$$

2.1绕基向量旋转

接下来我们从两个角度来推导旋转矩阵（以绕Z轴为例）

1. 从点变换的角度
   

$$\begin{gathered} x=\cos \alpha \\ y=\sin \alpha \\ z=constant \end{gathered}$$
旋转之后坐标变化
$$\begin{gathered} x^{\prime }=\cos (\alpha +\theta )=\cos \alpha \cos \theta -\sin \alpha \sin \theta =\cos \theta x-\sin \theta y \\ {y}^{\prime }=\sin (\alpha +\theta )=\sin \alpha \cos \theta +\cos \alpha \sin \theta =\sin \theta x+\cos \theta y \\ {z}^{\prime }=z \end{gathered}$$
然后写成矩阵乘积的形式就是
$$P^{\prime }=R_Z(\theta )P=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$
3. 从坐标系变换的角度

$\hat{X}$方向的基向量从 (1, 0, 0) 变换到 $(\cos \theta ,\sin \theta ,0)$，$\hat{Y}$方向的基向量从 (0, 1, 0) 变换到 $(-\sin \theta ,\cos \theta ,0)$， $\hat{Z}$方向的基向量不变，还是 (0, 0, 1)。然后把变化后的基向量填写到矩阵的每一列，可得出旋转矩阵
$${}_{B^{\prime }}^{B}R=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$
绕X、Y轴同理。

2.2绕任意轴旋转（Rodrigues’ rotation formula）

设$P$点绕$\hat{n}$旋转$\theta$到$P^{\prime }$（$\hat{n}、P$已知）。

旋转前：
$$P=P_{\perp }+P_{\parallel }$$
$$P_{\parallel }=(\hat{n}\cdot P)\hat{n}(projection)\text{\hspace{1em}(1)}$$
$$P_{\perp }=P-P_{\parallel }=P-(\hat{n}\cdot P)\hat{n}\text{\hspace{1em}(2)}$$
旋转后：
$$P^{\prime }=P_{\perp }^{\prime }+P_{\parallel }^{\prime }$$
又$\because$
$$P_{\parallel }^{\prime }=P_{\parallel }$$
$\therefore$
$$P_{\perp }^{\prime }=P^{\prime }-P_{\parallel }\text{\hspace{1em}(3)}$$
看向$-\hat{n}$可得到如下坐标系

$$\hat{n}\times P_{\perp }=\hat{n}\times (P-P_{\parallel })=\hat{n}\times P-\hat{n}\times P_{\parallel }=\hat{n}\times P$$
且
$$\Vert \hat{n}\times P\Vert =\Vert \hat{n}\times P_{\perp }\Vert =\Vert \hat{n}\Vert \cdot \Vert P_{\perp }\Vert \cdot \sin \frac{\pi }{2}=\Vert P_{\perp }\Vert$$
又$\because$
$$\Vert P_{\perp }^{\prime }\Vert =\Vert P_{\perp }\Vert$$
$\therefore$
$$P_{\perp }^{\prime }=\Vert P_{\perp }\Vert \cos \theta \frac{P_{\perp }}{\Vert P_{\perp }\Vert }+\Vert P_{\perp }\Vert \sin \theta \frac{\hat{n}\times P}{\Vert \hat{n}\times P\Vert }=\cos \theta P_{\perp }+\sin \theta (\hat{n}\times P)\text{\hspace{1em}(4)}$$
由(1)和(4)得
$$P^{\prime }=P_{\perp }^{\prime }+P_{\parallel }^{\prime }=P_{\perp }^{\prime }+P_{\parallel }=\cos \theta P+(1-\cos \theta )(\hat{n}\cdot P)\hat{n}+\sin \theta (\hat{n}\times P)\text{\hspace{1em}(5)}$$

第三章 正运动学

1. 连杆长度：两轴间公垂线的长度，记作$a_i$，沿$X_i$轴，从$Z_i$移动到$Z_{i+1}$的距离；
2. 连杆转角：两轴线形成的夹角，记作${\alpha }_i$，绕$X_i$轴，从$Z_i$旋转到$Z_{i+1}$的角度；
3. 连杆偏距：两相邻公垂线之间的长度，记作$d_i$，沿$Z_i$轴，从$X_{i-1}$移动到$X_i$的距离；
4. 关节角：两相邻公垂线之间的夹角，记作${\theta }_i$，绕$Z_i$轴，从$X_{i-1}$旋转到$X_i$的角度。

正运动学主要解决已知上述参数，求解出末端执行器的位置的问题，一般问题中会固定其中的一些参数。

机器臂系统中常用的五个坐标系定义如下：

1. 基座标系{B}；
2. 腕部坐标系{W}；
3. 工具坐标系{T}；
4. 工作台坐标系{S}；
5. 目标坐标系{G}。
   最终的目的就是让工具坐标系和目标坐标系重合。

第四章 逆运动学

逆运动学所要求解的方程是非线性方程，求解方法主要分为解析解和数值解两大类，数值解需要迭代，一般情况下比较慢；解析解又分为代数法和几何法两种。

v-rep中simIK工具包

在v-rep中想要求解FK/IK任务需要理解IK elements和IK groups这两个概念，一个IK task由一个IK group和一个或多个IK elements组成。

• IK element代表了一个简单的运动链（kinematics chain），一个运动链至少要有一个关节，一般还要有一个基座，一个末端操纵器和一个目标点。具体可以由以下组件组成：
  • 基座：代表运动链的开始
  • 一些连杆
  • 一些关节
  • 一个末端操纵器（end-effector）
  • 一个目标点
• IK group包含一个或多个IK element，在单次仿真中可以对多个运动链求解，且可以设置求解其、迭代次数等参数。

API

想要完成一个inverse kinematics问题求解，需要以下几个函数

1. int environmentHandle=simIK.createEnvironment(int flags=0)创建求解环境，相当于init
2. int ikGroupHandle=simIK.createGroup(int environmentHandle,string ikGroupName='')创建group
3. simIK.setGroupCalculation(int environmentHandle,int ikGroupHandle,int method,float damping,float maxIterations)设置求解参数（求解方法、迭代次数等）
4. int ikElement,map simToIkObjectMap,map ikToSimObjectMap=simIK.addElementFromScene(int environmentHandle,int ikGroup,int baseHandle,int tipHandle,int targetHandle,int constraints)从场景中加载模型，生成IK element
5. simIK.setElementFlags(int environmentHandle,int ikGroupHandle,int elementHandle,int flags)设置IK element属性
6. int result,int flags,float[2] precision=simIK.handleGroup(int environmentHandle,int ikGroupHandle,map options={})求解

1~5一般在脚本的sysCall_init()中设置，6在sysCall_actuation中设置。

生成IK element所依赖的模型不一定要从场景中获取真实的具有网格形体的模型，也可以在simIK单独创建虚拟的模型，具备相应的数据也可进行仿真求解。