机器人建模和控制
导论
机器人的数学模型
机器人的符号表示
机械臂：由一系列关节+连杆组成的运动链
关节：
- 回转 revolute，用 R 表示
- 平动 prismatic，用 P 表示
示例：RRR 机械臂，表示带有三个回转关节的机械臂；
常用关节变量及符号：
- z 表示旋转轴线；
- θ 表示回转关节的角度；
- d 表示平动关节的距离；
位形空间
位形：机械臂上各点位置的详细规范；
位形空间：所有位形的组合，称为位形空间；
示例：如果已知关节变量 θ 和 d 的值，就可以推算出机械臂上任意一点的位置；
因此，位形可由各关节变量值的集合来表示，例如 { q1, q2, q2, …, qn }
有多少个 q，即相当于有多少个自由度；自由度的个数，相当于位形空间的维度数量；
三维空间需要6个自由度，才能到达任意一点；但是为了规避障碍，有时需要增加一些冗余的自由度；
状态空间
位形是机械臂的一个瞬时状态，但机械臂通常处于运动的状态；因此，使用状态空间，来表示接下来一段时间内，机械臂的所有可能状态的组合；
关节的下一个时间点的状态，取决于两个变量，一个是关节的当前状态，一个是关节的移动速度；
因此，对于有 n 个自由度的机械臂来说，它的状态空间有 2n 个维度；
工作空间
工作空间表示机械臂在运动过程中，所有可能出现的位置的组合；如果站在这个工作空间内，有可能被打到；如果站在工作空间外，则一定不会被打到；
作为机械装置的机器人
机器臂毕竟也是一种机械装置，因此它有一些自己的物理特征；
机器臂的分类
动力源
电力
- 优点：便宜、干净、安静；
- 缺点：扭矩小；
液压
- 优点：响应速度和扭矩性能好，因此适合提取重物
- 缺点：漏油、外围设备多、噪音
气动
- 优点：成本低，结构简单；
- 缺点：难以精确控制；
控制方法
非伺服：non-servo，开环 open-loop 控制装置，运动范围取决于机械限位，主要用于物料传送；
伺服：servo，闭环 closed-loop 计算机控制；可编程，多功能；
早期伺服机器人的末端是点到点的，即通过示教器设置和存储一些离散点，之后末端执行器按顺序经过每个点的位置，最终到达目标点；
后来的机器人支持设置连续路径，以及速度和加速度，实现更精确的路径控制；
应用领域
装配机器人
- 特点：体型小，电力驱动，常采用回转关节或者 SCARA 类型设计；
非装配机器人
- 场景：焊接、喷漆、搬运、装卸等；
二者最大的区别便是精度要求；因为装配件之间存在相互作用力，如果精度不高，便有一定的概率造成破坏或失败；
几何结构
绝大多数机械臂的自由度不超过6个，有5种常见的几何结构：
- 关节链接型 RRR
- 球坐标型 RRP
- SCARA型 RRP
- 圆柱型 RPP
- 直角坐标型 PPP
以上五种都属于串联连杆，还有一种更复杂的并联机器人；
机器人系统
机械臂仅仅是整个机器人系统中的一个零件，所有零部件包括：
- 传感器；
- 计算机控制器；
- 动力源；
- 末端工具；
- 输入设备（例如示教器或示教软件）；
- 存储设备和网络；
- 机械臂；
精度和重复精度
精度：预期到达坐标和实际到达坐标之间的误差；
重复精度：到达示教点的误差；
分辨率：控制器可检测到的最小运动增量；
传统的误差测试方法是计算各关节状态变量，而不是直接计算末端位置和姿态；
这种方法会受到因素的影响，包括：
- 机械臂的加工精度；
- 计算误差；
- 机械臂的柔性变形；
- 齿轮间隙；
- 摩擦力等；
由于不是直接测量末端，因此为了减少误差，提高精度，机械臂只能做大刚性，以避免变形；
由于两点之间，直线最短。因此线性轴的误差会比旋转轴小一些。但旋转轴的好处是可以让结构变紧凑，占用空间小；
手腕和末端执行器
手腕几乎都是旋转关节，而且常用球形手腕；球形手腕有三个自由度，因此可以和机械臂的解耦；
可以根据不同的任务，开发不同的末端执行器，以便更高效的实现目标；
常见的运动学配置
关节型机械臂
RRR，也叫回转机械臂，由三部分构成，分成叫做腰、肩、肘；其中肩肘的 z 轴一般平行，并垂直于腰的 z 轴；
球坐标型机械臂
RRP，肘关节是平动的，垂直于肩关节；这样可使得末端执行器的坐标系和肩关节重合；
SCARA 机械臂
也是 RRP，主要用于装配场景，z1, z2, z3 三个轴是平行的；
圆柱型机械臂
RPP，腰是旋转的，肩肘是平动的；
笛卡尔的机械臂
PPP，例如 3D打印机，或者龙门吊机器人
并联机械臂
并联机械臂指多个连杆连接同一个起点，这样可以极大的提高刚性，减少变形，从而提高精度；
本书概要
机械臂
正运动学
正运动学：使用关节角度，来确定末端执行器的位置；
当关节较多，使用世界坐标系来计算较为复杂；更简单的方法是在每个关节处建立坐标系，然后不同关节之间，使用固定的变换函数进行变换坐标即可；
世界坐标系貌似也叫做惯性坐标系；
如果需要通过关节速度计算末端的速度，则需要对角度进行求导；据说此处会用一个雅可比矩阵来表示；
逆雅可比矩阵则可根据末端速度来计算关节的速度；
当中间关节的角度为 0 或者 180 度时，此时两个连杆处于一条直线状态。在这种状态下，机械臂无法向关节的 x 轴方向移动，雅可比矩阵不可逆，此时机械臂处于奇异位形，存在某些无法实现的无穷小运动。因此，在规划路径时，应避免让机械臂进入奇异位形的状态；
逆运动学
逆运动学：由目标位置的坐标，反向求解各关节角度；
逆运动学的求解，可能存在多种情况。有可能无解，因为不可达。也有可能存在两个解，即上肘位和下肘位。也可能存在无数个解；
动力学
- 拉格朗日动力学
- 牛顿欧拉递归方法
路径和轨迹规划
- 路径规划：在不触碰工作内其他物体的情况下，计算一条可到达指定位置的路径；
- 轨迹生成：基于时间序列的运动轨迹集合；
- 轨迹跟踪
独立关节控制
有了参考轨迹后，下一步是给关节控制器发送指令，让关节控制器干活；
控制器同时处理两个问题，一个是跟踪，一个是抗扰动；因为外部因素如噪音、摩擦等会带来干扰，需要在输入时，动态的计算这些偏差，并时不时调整自己，让误差尽可能的小；
因此涉及一些零部件如：补偿控制器、功率放大器、传感器等；
非线性和多变量控制
- 李亚普诺夫直接法
- 无源性控制；
力控制
当末端到达指定位置时，由于不可避免存在误差，有可能会给刚性的机械臂结构带来巨大的作用力，从而造成破坏；为避免悲剧发生，最好能够测量物体间的相互作用力，并作好应对措施，例如力控制或者柔顺控制；
基于视觉的控制
相机也可以视为一种传感器，因此可以基于图像进行伺服控制；
反馈线性化
将非线性反馈变换成线性的；适用于解决柔性关节机器人的场景；
欠驱动和移动机器人
欠驱动：驱动数量少于自由度的数量；
移动机器人：非完整系统的一种形式，需要新方法将非完整系统转变一种可控和稳定的形式；
刚体运动
建立各种坐标系，表示刚体的位置和状态，以及这些坐标系之间的转换；
齐次变换可在矩阵中同时包含旋转和平移的操作；
位置的表示
实体物理空间中的一个点 p，在不同的参考坐标系中，有不同的坐标值；
对于某个坐标系的原点来说，也是如此。它在自身的坐标系中是原点，坐标为 [0, 0]，但在其他坐标系中，它就不是这个值；
向量是独立于坐标系的，它用来表示方向；仅仅看向量，并不知道它实际上指向哪里，需要同时结合坐标系的原点后，才有办法判断；
如果两个坐标系平行，那么两个向量之间是可以进行计算的；但如果不平行，则不行，需要先进行变换，之后才可以计算；
旋转的表示
平面内的旋转
当知道坐标系 A 的原点，在坐标系 B 中的坐标时，就可以知道这两个坐标系之间的姿态关系了，进而还可以计算得出它们的旋转变换方程；其实也很简单，只需要将 B 坐标系的各轴，投影到 A 坐标系中，就知道如何变换了；
三维空间内的旋转
同二维，也是通过投影来计算；
旋转变换
旋转矩阵可用来表示坐标系 B 相对于另外一个坐标系 A 的姿态角，也可用来表示某个点从 B 到 A 的坐标变换；
相似变换
一个坐标系可以定义为一组基本向量，例如沿各坐标轴的单位向量；
旋转的叠加
相对于当前坐标系的旋转
当前坐标系：旋转发生时，所围绕的那个坐标系；
通过多个坐标系的链式变换，可得到最终坐标系的坐标，即”后乘“；
相对于固定坐标系的旋转
相对于当前坐标系，只需以相反的顺序叠加即可，即”前乘“；
旋转变换的叠加规则
基于当前坐标系，后乘；
基于固定坐标系，前乘；
旋转的参数化
一个刚体最多只有三个自由度，因为最多只需要三个变量，便可定义其姿态；
以下是用来表示自由旋转的三种方式：
欧拉角
按顺序分别绕 XYZ 三个轴旋转，用三个角度来表示旋转后的坐标系与原坐标系的姿态；每个旋转角度都是基于当前坐标系，而不是世界坐标系；
滚动-俯仰-偏航
按顺序分别绕 XYZ 三个轴旋转；三个轴的旋转角度是参照世界坐标系
转轴/角度
自定义一个旋转，让整个坐标系绕该轴旋转一定的角度即可；
该方法将涉及 4 个参数，其中前 3 个参数用来定义旋转轴，最后一个参数用来定义旋转角度；
任意一个旋转矩阵，都可以转换成转轴+角度的形式来描述；
转轴+角度的表示并不唯一，即相反方向的转轴+相反方向的角度，旋转后的结果是一样的的；
指数坐标
转轴+角度的另外一种描述形式，用 k 和 θ 两个变量来表示；
刚体运动的概念
刚体运动相对旋转变换的唯一区别是增加一个平移变量；
P0 = R10 P1 + d0
齐次变换
将平移也添加到矩阵中，这样可以简化计算过程；
一般刚体运动的指数坐标
跟旋转变换一样，齐次变换也可使用指数坐标来表示；
正运动学
正运动学：基于各关节的状态（如角度），计算出末端执行器的位置和姿态；
运动链
旋转关节或平动关节只有一个自由度，但球窝关节一般有两个自由度，球形腕关节有三个自由度；但后二者可等同视为多个单自由度组合且连杆长度为 0 的情形；所以在计算上面并没有本质的区别；
根据每个关节相对上一个关节的齐次变换矩阵，通过递归计算，可求解任意一点的位置；
Denavit 约定
为简化递归分析和计算的复杂性，引入了 Denavit-Hartenberg 约定；它将齐次变换矩阵，替换为四个基本矩阵的乘积。这四个基本矩阵，分别代表连杆长度、连杆扭曲、连杆位移、关节角度；
存在和唯一性问题
齐次变换矩阵中，有 6 个参数；按 DH 约定，只剩下了 4 个参数；显然这 4 个参数没有办法表示所有场景，但可以表示满足特殊条件下的所有场景；
特殊条件包括：
- 坐标轴 x1 垂直于坐标轴 x0
- 坐标轴 x1 与坐标轴 z0 相交
坐标系的配置
事实上，坐标系的原点是可以任意选择的，并不一定必须设置在连杆的末端；
不管中间的坐标系如何选择，T0n 都将会是一样的；并且，在一些特殊的情况下，齐次变换方程可以简化；
正运动学实例
此处作者举了以下几个例子来示范如何完成正运动学的计算：
- 平面肘型机械臂
- 三连杆圆柱型机械臂
- 球型手腕
- 带有球型手腕的圆柱型机械臂
- 斯坦福机械臂
- SCARA 型机械臂
本章小结
本章主要介绍如何通过关节变量 q，计算出末端的位置和姿态；
速度运动学
本章主要探讨关节的速度与末端的线速度和角速度之间的关系。这种关系，可用一个雅可比矩阵来表示；
末端执行器的线速度和角速度是一个 6 维向量，对于 n 个连杆的机械臂来说，这种变化关系，也叫雅可比矩阵，便是一个 6*n 的矩阵了；
角速度：固定转轴情况
对于围绕某个固定轴的情况，如果角速度是 w，半径是 r，那么线速度 v = w * r
此时刚体上任意一点的运动轨迹实际上是一个圆（平面），因此它的计算可以进行简化，该简化需要用到反对称矩阵；
反对称矩阵
对于 n * n 的矩阵 S，如果 ST + S = 0 时，那么 S 是一个反对称矩阵；
反对称矩阵在运算上有一些特性，这些特性会让计算变得方便；
奇点
奇点的一些特性：
- 在奇点处，某些方向的运动可能无法达到；
- 在奇点处，有限的末端速度，可能对应无限的关节速度；
- 在奇点处，有限的关节力矩，可能对应无限的末端处的力和力矩；
- 奇点通常对应工作空间的边界点；
- 受连杆参数微小变化的影响，奇点可能无法到达；
静态力/力矩的关系
当机械臂和环境相互作用时，会受到反作用力，该反作用力会传递给关节，从而在关节处产生力矩；
理论上来说，通过雅可比矩阵的转置，可以反向计算出关节的受力情况；
逆向速度和加速度
逆向速度：通过末端的速度，反向求解关节速度
加速度：对速度进行求导；
可操作性
对于特定的关节角度 q，根据雅可比矩阵，可计算出末端的速度；雅可比矩阵有点类似一个缩放系数；
在满足特定条件下，末端值的速度取值范围，构成了一个可操作性的椭球；我们可借助可操作性的取值范围，来确定执行特定任务的最佳位形。
另外，该可操作性，也可帮助我们设计满足特定条件的连杆长度组合；一般来说，当两根连杆的长度相等时，可操作性最大；
逆运动学
基于末端的位置和姿态，反向求解各关节的变量值，称为逆运动学。考虑到解并不唯一，因此它会更加困难一些；
一般的逆运动学问题
对于一般的逆运动学问题，末端的位置和姿态变量只有4个，但是待求解的关节变量要远远多于 4 个。这意味着很可能存在无数个解；
有两种求解方法：
- 闭式解：用解析表达式来表达的解，因此也叫做解析解；
- 数值方法求解：用数值来表示的解；
运动解耦
在特定条件下，我们可以将逆运动学问题拆解成以下两个相对简单的问题：
- 逆位置运动学
- 逆姿态运动学
逆位置求解：一种几何方法
考虑到机械臂的物理构造的特定形式和规律，我们可以通过几何方法来简化逆位置的求解过程。
事实上，正是因为逆运动学问题的求解太过复杂，才导致机械臂的构造形式要保持简单，以便能够求解；
所谓的几何方法，就是将机械臂投影到前一个关节的坐标系平面中，转换成求解一个三角学的问题；简单来说，就是降维求解，将三维空间的问题，降维到二维空间来解决；
逆姿态求解
当使用逆位置的几何方法求解后，我们就只剩下三个关节变量未知了；
逆运动学的数值方法
随着计算机能发的提升，数值方法变得可行了，有两种方法：
- 逆雅可比法；
- 雅可比转置法，一种梯度搜索算法
动力学
动力学方程的目标，是解决力和运动之间的关系；有两种常见的解决方案，一种是欧拉-拉格朗日方程，一种是牛顿-欧拉方法；
计算的目的，估计在于知道动力源如电机，需要以什么样的功率运行，才能够驱动机械臂实现预期中的运动；
欧拉-拉格朗日方程
欧拉-拉格朗日方程：一组用来描述系统如何随时间进行变化的微分方程；（前提条件：处于完整约束，且约束力满足虚功原理）；
这个方程可由虚功原理推导出来，也可使用最小作用量原理推导出来；
完整约束：在讨论物理的运动时，除了基于物理定律，通常还需要假设一些约束条件，减少需要考虑的因素，以便让计算变得简单和纯粹；所谓的完整约束是指，在任意的时刻，粒子的位置都符合对应此刻的确定几何关系；
虚功：根据给定的虚位移，乘以施加的外力，得到的机械功，称为虚功；
虚功原理：如果一个物理系统处于平衡状态，那么所有施加在该系统的外力，乘以虚位移，得到的虚功总和，等于零；
物体会受到两个外力影响，一个是天然存在的重力，一个是人工施加的动力，因此拉格朗日算子表示为动能和势能之差；
达朗贝尔定理：如果给每个质点施加一个虚构的反向附加力，那么每个质点将会处于平衡状态；
动能和势能
如果能使用广义坐标来表示系统的动能和重力势能，那么就能够用欧拉-拉格朗日方程来推导动力学方程；
对于一个 n 连杆机械臂，其总动能是一个 n * n 的惯性矩阵，这个矩阵是对称且正定的；
正定：总动能是非负的，并且仅在所有关节的速度都为零时，总动能才为零；
机械臂的唯一势能来源是重力，对于重力来说，它仅仅是广义坐标的函数，与速度（坐标的导数）无关；
运动方程
一些常见的位形
前述分析方法在几种机械臂位形中的应用：
- 双连杆直角坐标机械臂
- 平面肘型机械臂
- 带有远程驱动连杆的平面肘型机械臂
- 五杆机构
机器人动力学方程的性质
n 连杆机器人的动力学方程，包含一些重要的结构特性，这些特性有助于开发控制算法；
反对称性和无源性
反对称性：矩阵 N 中的元素，满足 njk = -nkj
无源元件：只能接收能量的电子元件；它可以吸收能量，耗散能量，或者存储能量；它发挥这些功能的时候，不需要施加外部电源；例如电阻、电容和电感等；
有源元件：需要电源才能正常工作的元件；
一个无源的机械系统可由阻尼、质量、弹簧等部分组成；
惯性矩阵的有界性
注：略
参数的线性化
机器人的运动方程由连杆质量、惯性矩阵等参数构成；不同的机械人，参数值不同；
一个给定的刚体，可通过 10 个参数来描述；对于一个 n 连杆的机器人，最多有 10n 个动力学参数；但由于连杆本身存在约束，关节之间相互连接和耦合，所以实际上独立参数的数量要少于 10n
对于不同的连杆结构，所需的参数数量不同，最少可以只需要 5 个参数；
牛顿-欧拉公式
拉格朗日方法，将机械臂当作一个整体来分析；牛顿欧拉方法则单独分析每一个连杆，最后再综合递归计算，可得出相同的结果；
有两种计算场景：
- 广义力已知，想知道运动轨迹；（此类型的问题，拉格朗日方法有优势）
- 运动轨迹已知，想知道广义力；（此类型的问题，牛顿欧拉方法有优势）
路径和轨迹规划
机械臂的空间有时候并不是空旷的，而是很可能存在障碍物的约束，包括自体的存在约束；为了避免让机器人碰到障碍物，有必要进行路径规划；
当机械臂的自由度增加时，路径规划的计算复杂度将以指数级上升。因此完整算法仅可能在低自由度的场景中实现；对于多自由度的场景，需要将其转换成搜索问题，使用启发式算法来解决；
虽然启发式算法不能覆盖全部答案，但常常已经够用，计算时间可控，能够在绝大部分场景中解决好问题；
位形空间
对于机械臂来说，关节变量组成的向量，可很方便的用来表示位形；
所有可能与障碍物发生碰撞的位形所组成的集合，称为位形空间障碍；无碰撞位形的集合，则称为自由位形空间；
平面路径规划
最终可转换成图结构的搜索问题；图由顶点和边组成；每条边由两个顶点构成；
三种构造碰撞路径的算法：
以上三种方法主要用于二维空间的场景，如在地面上移动行走的机器人；
可见性图
可见性图：顶点可以相互看到的图，每对顶点组成一条边，该边不与障碍物内部相交，即两个顶点之间没有东西遮挡；
这些顶点集合由障碍物的所有顶点，以及机械臂的起点和终点构成；
可见性图可用来计算起点和终点之间的最短路径，但由于存在一些不确定性，这种路径有些危险，容易出现碰撞；更好的做法或许是寻找离障碍物最远的路径，而不是最近的路径；
广义 Voronoi 图
Voronoi 单元：假设平面上有一系列的离散点 p1 到 pn , Voronoi 单元表示某个区域，在该区域内的所有点，都离该单元的离散点更近，而离其他离散点更远；
这些单元的界限，便是一条安全的路径了。虽然理论上可以计算出最理想的路径，但简单暴力的网格算法，其实效果也不错；
梯形分解
问：什么是梯形分解？
答：使用垂直线段扫描多边形，与多边形的边和顶点构成梯形，将多边形切分解很多个梯形；
对空间进行梯形分解，得到多个梯形后，使用连通图来表达梯形之间的邻接关系；
人工势场
将目标位置视为吸引力场，将障碍物体视为排斥力场，将路径规划问题，转换成寻找最小势场值的优化问题，使用梯度下降进行求解；
考虑到势场中通常会存在局部最小值，为避免卡在局部最小值，一般会引入随机化来逃离陷阱；
对于引力场，距离越远，引入越接近无穷大；对于斥力场，距离越近，斥力越接近无穷大；
基于采样的方法
先使用随机策略生成一系列的位形，然后从中筛选出可行的路径；有两种规划算法：
概率路线图
步骤：
- 在位形空间中，生成一组随机样本，并只保留无碰撞的样本；
- 使用简单的直线路径，将每个样本与其最近的节点相连；并只保留无碰撞的路径；
- 注：此时节点将分成几组，各组内部的节点相互连接，但组与组之间没有连接；
- 增强阶段：生成一些额外的节点，在无碰撞的情况下，将不同组连接在一起；
- 将起点和终点连接到路线图中，从中找到一条从起点通往终点的路径；
缺点：完全随机的情况下，有时候会出现死胡同；
快速探索随机树
从起始点处开始构建一棵随机树，直到某些叶子节点，达到目标位形为止；
步骤：
- 基于均匀概率分布，生成 sample；
- 从现有树中，找出离 sample 最近的顶点 near；
- 从 near 往 sample 的方向迈出一小步，得到 new；
- 如果 new 无碰撞，则将其添加到现有树中；
优点：快速探索随机数已被证明能够有效的应对复杂的路径规划场景，例如自动驾驶、无人机、卫星、航天器等；
轨迹规划
得到位形后，下一步就要基于这些位形，构建运动轨迹；
- 点到点运动的轨迹：单关节的情形；
- 通过中间点确定路径的轨迹；