太空垃圾回收-7自由度机器人MATLAB仿真

配套资源
摘要
I. 背景介绍
II. 机械臂构型设计
- A. 关节参数
- B. D-H建模与D-H参数表
III. 正运动学
- A. 齐次变换矩阵
- B. 工作空间分析
IV. 改进后的逆运动学数值解法
V. 路径规划
VI. 轨迹规划
- A. 路径段的时间分配
- B. 广义3次多项式
VII. 抓球策略
VIII. 量化评价
可以改进的地方

配套资源

仿真视频: https://www.bilibili.com/video/BV1qS4y1x7PZ?spm_id_from=333.999.list.card_archive.click&vd_source=240689d86a403b4d5d22137f38d4d71e
源码地址：https://github.com/OwenLee666/7DoF_robot
相关书籍：
- 机器人学导论——分析，控制及应用，Saeed B.Niku著
- 机器人学导论，John J. Craig著

摘要

本文设计了一个空间机械臂及其仿真程序，内容涉及正运动学，D-H建模，逆运动学数值解，只有位置约束的数值解法，路径规划，RRT,广义三次多项式轨迹规划等等。

I. 背景介绍

21世纪以来，我国太空探索领域取得极大进步，中国自己的空间站正在逐步成型，国际空间站即将在几年后退役，彼时中国空间站将是世界上唯一的空间站，这对我国的科学与工程技术提出了极大挑战。许多难题亟待解决，空间站机械臂就是其中之一。在这样的大背景下，开展了以下课题研究。
课题名为：空间站物品打扫，要求如下：

设计一个至少七自由度的机器人，至少包括一个伸缩关节，要求机器人工作空间能够满足左图工作空间的要求（自己定义机械臂安装位置要求能够满足抓取指定空间范围内上任意位置的漂浮物体）。
根据自己设计的机器人建立DH坐标系及齐次变换矩阵，并设置合理的关节转动极限，分析和绘制工作空间，关节必须要有合理的机构实现形式机械臂构型设计。
求取自己设计的机器人的逆解，实现末端制定位置的定位定姿，能够实现末端按照要求运动。
空间站物体均简化为球体，数量10个，直径范围（ 50mm-100mm ），物体位置（相对距离≤200mm），尺寸随机生成，抓取顺序从小到大进行。
使用RRT算法或其他算法实现机械臂无碰撞按顺序抓取物体，制作动画，并通过仿真对抓取策略进行量化评价。
机械臂旋转关节最大速度180°/s, 最大加速度180°/s², 直线运动关节最大速度1m/s, 最大加速度1m/s² 。

II. 机械臂构型设计

由于刚拿到这个课题时不知道如果着手设计机械臂，所以就仿照经典的工业六轴机械臂，在最头部加了一个线性滑动关机，组成了一个七轴机械臂，只改变了六轴机械臂的部分连杆长与关节偏移，以使其工作空间能覆盖整个空间站。做到后面越发感觉机械臂设计得不合理，没有考虑机械臂的占地与收纳问题，没有考虑空间站对于体积的要求，但是deadline临近使得没有时间修改了，这是很遗憾的地方，不过通过这条机械臂我完成了后续所有设计，收获还是颇丰的。机械臂模型如下所示

A. 关节参数

关节序号	关节类型	运动范围	最大速度	最大加速度
1	滑动	0~3600mm	1m/s	1m/s²
2	转动	-180°~180°	180°/s	180°/s²
3	转动	-150°~150°	180°/s	180°/s²
3	转动	-150°~150°	180°/s	180°/s²
4	转动	-150°~150°	180°/s	180°/s²
5	转动	-150°~150°	180°/s	180°/s²
6	转动	-150°~150°	180°/s	180°/s²
7	转动	-360°~360°	180°/s	180°/s²

由于没有深入到动力学部分，所以没有对于速度与加速度的设计约束，故严格按照设计要求的最大值给定。

B. D-H建模与D-H参数表

D-H建模非常重要，是后续开展一切分析的基础。按照D-H法建模如图2所示。

其中坐标系xyz为世界坐标系；x₀y₀z₀为基座坐标系，也是1号关节滑动轴所在的位置。在上图的基础上指定连杆长，关节偏移便得到了D-H表。D-H表如下所示。

		关节编号	关节变量	关节偏移	连杆长	扭角
A0	World to base		0	-1000	0	-90
A1	Base to 1	1	0	d1+200	0	90
A2	1 to 2	2	θ₂+90	580	0	-90
A3	2 to 3	3	θ₃+90	0	540	0
A4	3 to 4	4	θ₄	0	140	-90
A5	4 to 5	5	θ₅	604	0	90
A6	5 to 6	6	θ₆	0	0	-90
A7	6 to 7	7	θ₇	300	0	0

III. 正运动学

正运动学研究的是已知关节变量，求解机械臂末端以及各关节在笛卡尔坐标系中的位姿，也就是研究的是关节空间到三维空间的映射关系，位姿用齐次表换矩阵表示。

A. 齐次变换矩阵

根据上一小节得出的D-H表，可以很容易写出各个关节相对于上一关节的齐次变换矩阵，它表征的是此关节在上一级关节的坐标系里的位姿。

根据D-H表中的每一行可以写出下列四个矩阵：

由此可得到8个表示坐标系在上一级坐标系中位姿的齐次变换矩阵：

T=A0∗A1∗A2∗A3∗A4∗A5∗A6∗A7T = A0 * A1 * A2 *A3 * A4 *A5 * A6 * A7 T=A0∗A1∗A2∗A3∗A4∗A5∗A6∗A7
由上式可得机械臂末端在世界坐标系中的位姿。
同样，由T=A0∗A1∗⋯∗AiT=A0*A1*⋯*AiT=A0∗A1∗⋯∗Ai可以得到任一感兴趣的关节i在世界坐标系中的位置。

B. 工作空间分析

工作空间对于一个机械臂来说十分重要，在本课程设计中对于关节空间进行分析，能够保证机械臂的末端能够到达空间站任意位置。
工作空间又分为可达工作空间与灵巧工作空间，可达工作空间是灵巧工作空间的超集。机械臂只能以一种姿态到达可达工作空间的边缘，而对于灵巧工作空间内的点，机械臂能以任意姿态到达，对于本设计中的7自由度机械臂而言，甚至能做到末端保持一个固定的位姿，但可以做到有无穷多种构型，这为避障带来了便利。
下图是本设计中机械臂的可达工作空间侧视图边缘，通过它完全能够判断机械臂能够到达空间站的任意位置，而不需要再去遍历每个关节的每个点来画出点云进行判断。此可达工作空间边缘曲线是由关节2，关节3和关节6其一运动到极限位，另一关节旋转，交替运动产生。

由图可见，对于空间站的任一横截面，机械臂可以到达任一点，再加上滑动关机可以带着机械臂在空间站的z轴上移动，很容易分析出来空间站在机械臂工作空间的包络之下。

IV. 改进后的逆运动学数值解法

逆运动学研究的是已知机械臂末端位姿，反解各个关节变量的值，也就是说各关节变量是末端位姿的函数，有解析解与数值解两种解法。解析法通过代数法，几何法，三轴相交的pieper法等求得各个关节变量对于末端位姿的显示表达式。数值法借助雅可比矩阵通过牛顿下山法向解逼近。
刚开始时我认为数值解效率低下，执意要求出解析解来，解析解多么的优美简洁，高效快速，结果我求了整整两天半，尝试了各种办法，感觉就是解就在不远处，但就是抓不到。最后总算求出来一个，求出来的解效果极差，偏差极大，各关节变量之间耦合严重，甚至还出现复数，完全不能用。
无奈之下开始研究数值解，数值法看起来复杂，但实则可操作性比解析法强多了，计算机的求解时间也还能接受，误差小于1是基本能保证0.05秒以下。我还对数值解法进行了改进。对于本课程设计而言，机械臂抓球时的姿态并不重要，只要能到达球的位置，用任意姿态都是可以接受的。普通的数值解法需要完整的输入末端位姿，位置很好确定，就是球的位置，但姿态就麻烦了，不给又不行，好不容易给了倒显得机械臂愚笨，只能以固定姿态去到该位置。我改进了该算法，直接抛弃姿态的约束，甩掉确定姿态这个麻烦，只输入位置，雅可比矩阵也相应的从6x7变成了3x7，结果不仅计算量变小了，而且机械臂能自动以最合适的姿态从起点走到终点，这正是我要的结果，甚至连解析解都做不到这一点。
数值解法的框图如下：

算法步骤如下：

第一步：给定机器人末端（ Endeffector）目标位置p^ref。
第二步：定义机器人各关节的初始关节角 q。
第三步：由正运动学计算机器人末端的当前位置。
第四步：计算机器人末端位置的误差p^err = p^ref - p。
第五步：当误差 p^err 足够小时停止运算。
第六步：当误差 p^err 大于设定值时计算关节角的修正量δq。
第七步q=q+δqq = q + δqq=q+δq，返回第三步。

普通数值法的修正量计算是由下式给出的：

改进后的修正量计算式如下：

其中：

此改进算法可行性的严格数学证明我还没有来得及深入思考，但是从实践上证明是完全可行，而且效果很好的。

V. 路径规划

通过上一小节的数值逆解，我们可以获得机械臂末端到达球的位置时的各个关节变量，但是对于从起点到终点的中间这段该怎么走却还没有确定。路径规划研究的是怎样高效地规划一条通路来连接起点与终点，对于无障碍的情况，当然可以规划一条直线去连接起点与终点，这个直线可以是笛卡尔坐标系里的直线，也可以是关节空间里的广义直线；对于有障碍的情况，就需要设计各种各样的避障算法来找到一条通路，RRT算法就是其中之一。
对于本设计而言，路径规划又分为笛卡尔坐标系里的路径规划和关节空间里的路径规划，两种方法各有优劣。笛卡尔坐标系里的路径规划具有几何直观性，易于理解，但是缺点也很明显，因为路径点只有位置，所以规划出来的每一个路径点都需要给它指定一个姿态，然后运用逆运动学求出机械臂末端在该路径点时的各个关节变量，从而控制机器人运动。关节空间优缺点正好相反，本设计的机械臂有7个轴，所以关节空间为7维，没有几何直观可言，但是只需要知道起点和终点时的7个关节变量就可以规划一条关节空间里的路径，不需要再求逆解，因为每一个路径点本就是关节变量，可以直接用于正运动学控制机器人运动。
快速探索随机树（RRT）是一种通过随机构建空间填充树来有效搜索非凸，高维空间的算法。RRT算法是概率完备的：只要路径存在，且规划的时间足够长，就一定能确保找到一条路径解，但不是最优解。注意“且规划的时间足够长”这一前提条件，说明了如果规划器的参数设置不合理（如搜索次数限制太少、采样点过少等），就可能找不到解。
本设计采用关节空间里的RRT轨迹规划算法。起点的关节变量已知，终点的关节变量需要用上一小节的数值法求逆解，只需要这两个关节空间里的点就能规划路径。对于每一个新生成的节点，只需要运用简单的正运动学映射到笛卡尔坐标系里去判断有无碰撞，以此确定该点是否为可行点。算法流程图如下。

上图中判读新节点满足无碰撞约束的流程图如下所示：

以上流程图已经将本设计的路径规划算法表达得很好了，就不过多赘述，更多详细信息请查看源码。

VI. 轨迹规划

做完路径规划后，获得了一系列的从起点到终点路径点，但它们还只是一个个离散的位移点，与时间无关，所以还没有速度与加速度。下一步要做的就是对于每一个关节变量，要找一个以时间为自变量的函数将它们的位移点连续起来，要注意的是，虽然每一个关节都有自己的函数，但是也不能将它们完全割裂的看待，它们在时间上具有相关性，总之就是，每一个路径点的 7 个值将在 7 个函数的同一时刻出现，否则该路径点就不会在机械臂的运动中出现。那么如何保证路径点的 7 个值在 7 个函数的同一时刻出现？没想清楚的时候我认为这是解题的难点，想清楚后我发现这正是解题的突破点。对 N 个路径点的 N-1 个路径段分配时间，强行让 7 个函数在分配到的时刻上出现对应的值，这样时间就加入进去了，把所有路径点的值变成了以时间为函数上的离散点，下一步想想用什么函数来拟合它就可以了。
上面的问题想清楚后，就会面临接下来的两个问题，如何给路径段分配时间？用什么函数来拟合？

A. 路径段的时间分配

路径段的时间不能乱给，就某一段而言，如果给多了就会很慢，如果给少了就会导致速度与加速段超出要求的技术指标 180°/