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

配套资源

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

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

III. 正运动学

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

A. 齐次变换矩阵

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

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


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

$$T=A0\ast A1\ast A2\ast A3\ast A4\ast A5\ast A6\ast A7$$
由上式可得机械臂末端在世界坐标系中的位姿。
同样，由$T=A0\ast A1\ast \cdots \ast 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+\delta q$，返回第三步。

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

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

其中：

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

V. 路径规划

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

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

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

VI. 轨迹规划

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

A. 路径段的时间分配

路径段的时间不能乱给，就某一段而言，如果给多了就会很慢，如果给少了就会导致速度与加速段超出要求的技术指标 180°/𝑠, 180°/𝑠²。为了保证速度不会超标，我去遍历了 7 个关节变量的所有路径段中变化最大的那一段，令其速度为 180/𝑠,有角位移和速度，很容易就求出了时间𝑡_𝑚𝑖𝑛，容易验证，变化率最大的段给𝑡_𝑚𝑖𝑛的时间速度都不会超标，那其他变化率更小的段给𝑡_𝑚𝑖𝑛的时间就更不会超标了，所以得到了总时间由下式求得：

$$t_{sum}=\alpha \times (N-1)\times t_{min},\alpha \in [1,+\infty )$$
其中：
$$t_{min}=\frac{\mathbf{max}(\delta \theta )}{180}$$

式中N为路径点的数量，α为安全系数，因为上面是基于每一段的平均速度来分析的，但是拟合的曲线还是有很大可能在中间某处速度极大，所以给一个富裕量是一个合理的做法。
有了总时间，如何来分配每一路径段的时间呢？为了让最终拟合出来的轨迹有一个加减速过程，我希望找到一个函数能够给两头分配多一点时间，中间少一点。最终选定了二次多项式函数。
$$令：f(n)=ax+b{x}^2+c{x}^3,n\in [1,N-1]$$
给定下面三个约束条件：

根据这三个约束条件写出解的矩阵公式：

这样就得到了路径段的时间分配函数，带入某段的序号就可以得到分配的时间。

B. 广义3次多项式

到这里路径点已经成为一个以时间为自变量的函数上的离散点，也就是说有了路径点处的位移和时间。还需要找个函数把它们连续起来，经过大量思考我选定了广义三次多项式。
一个三次多项式需要4个边界条件，可以是两个位移的，两个速度的。但是我们有(N-1)×7个三次多项式，需要(N-1)×7×4个边界条件。位移已经有了，就是路径点的值，能够给出(N-1)×7×2个边界条件，可是速度只有起点终点为零， 7×2个，要想给中间点全部确定速度不是个容易的事，所以令中间点的速度与加速度连续，有了(N-2)×7×2，刚刚好满足求解要求。
令广义三次多项式如下：
$$s_{ij}(x)=a_{ij}+b_{ij}x+c_{ij}{x}^2+d_{ij}{x}^3$$
其中：
$$i=1\dots 7,j=1\dots N-1$$
i为轴的序号，j为路径段的序号。
将上面的(N-1)×7×4个边界条件可以得到一堆齐次方程。
经过计算，我发现按照特定规律排布时，我发现了这堆齐次方程组的参数矩阵呈现出一定的规律（规律如下图所示），可以利用这个规律用程序去构造参数矩阵，求逆后乘上起点速度，位移，速度为零，加速度为零构造的矩阵，就求出了a_ij，b_ij，c_ij，d_ij构造的矩阵。

上图是路径点为4，即路径段为3时的参数矩阵，对于七个关节变量而言，它们的参数矩阵是一样的。
到这里就得到了轨迹，可以在上面打点来做动画了。

VII. 抓球策略

抓球策略如图8所示，非常清晰明了，不再赘述。

VIII. 量化评价

原路径：

角位移：

角速度：

角加速度：

由上几图可见，确实有了一个加减速过程，但是，速度还是超标了，加速度更是严重超标。有时间再来排查这个问题。
就抓球而言，除非有球挡在了必经之路上，否则都是可以按顺序抓球的。

可以改进的地方

• 机器人构型可以重新设计
• RRT路径规划得到的路径都是锯齿状的，可以将其平滑处理后再进行后续的路径规划，这个应该不难，MATLAB好像有可以直接用的函数。由于我没有这样处理，导致视频中机器人在有些路径中有点抖动