得到d图像：
图8 (a)五次多项式位置曲线
图8 (b)五次多项式速度曲线
图8 (c)五次多项式加速度曲线
图8 (d)五次多项式位置离散曲线
4-3-4轨迹规划： 位置可描述为：
约束条件转换为矩阵形式可描述为：
11
本系统中给定起始点q0=30，中间点1 q1=50，中间点2 q2=90,末端点q3=70,v0=v1=a0=a1=0，可规划出如下图9中的位置、速度、加速度曲线：
图9 4-3-4多项式位置、速度、加速度曲线
得到的机器人末端效果如下图10所示（直线是末端的变速五次多项式轨迹规划，曲线是末端4-3-4轨迹的匀速轨迹规划）：
图10 末端规划轨迹效果图
对应得到的机器人各关节的角度变化如下图11（a）和11（b）所示：
12
图11 (a)直线轨迹各关节角度（或长度）变化图
图11 (b)曲线轨迹各关节角度（或长度）变化图
4.3.2 关节空间轨迹规划
关节空间轨迹规划的基本思想是在关节运动的起始段和终止段设计抛物线轨迹进行过渡实现，使机器人关节位置平滑地从初始位置运动到终点位置。本系统对于关节空间的轨迹规划使用的依旧是4-3-4轨迹规划，首先通过逆解求出起始点、中间点1、中间点2和末端点位置时的角度（或长度）逆解，然后将这些逆解数据进行角度（或长度）的轨迹规划。具体实现的各关节角度（或长度）变化的效果如下图12所示。
图12关节空间规划各关节角度（或长度）变化图
5 流程图
首先，对机器人的初始位置和各关节姿态和分拣系统的环境（工件、传送带、
13
工件和篮子）进行初始化，然后工件以要求以1.5m/s的速度开始运动，当到达目的地时，机器人末端开始进行拾取工件，并沿着预先规划好的直线轨迹进行末端五次多项式的变速运动，直线轨迹结束后，便进行末端4-3-4曲线轨迹的匀速运动，判断当到达目的地时，放下工件，完成工件的设定轨迹分拣任务，之后机器人便沿着关节空间的规划从滑动带到篮子四个角的运动，运行完成后，程序结束。具体工件分拣流程图如下图13所示。
开始初始机器人及分拣系统环境状态工件以1.5m/s的速度运动否到达目的地？是拾取工件沿直线轨迹变速运动沿曲线轨迹匀速运动否到达目的地？是放下工件关节空间规划到篮子四个角的运动结束
图13 工件分拣系统流程图
6 总结
经过这次机器人学课程，我学到了很多很多，当然发的时间也很多，非常感谢黄老师的辛勤指导和严格要求。在这次学习中，碰到了很多很多问题，不停地修正、找资料和同学探讨学习。比如说关于轨迹规划的问题，我开始一直没有理解怎么去规划路径，一直以为是规划角度，后来知道是规划执行器末端后，又在想，规划的末端是要走出一条具体设定轨迹呢，还是要在一条直线（或曲线）上让末端走出不同的速度和加速度呢，所以这次课设也是索性就把这几种情况都实现了下，来都对比看看效果。末端规划虽然可以让末端沿着设定的轨迹匀速或
14
变速运动，但是求逆解的量很大，耗费时间，而且按照这个轨迹走后，每个关节的变化不一定是平滑的，可能出现多解和奇异值的情况。对于角度空间规划，虽然相比只需要求2个或4个逆解，计算量小，而且由于规划的是各个关节的角度（或长度），所以各个角度（或长度）整个过程都是很光滑的，速度也很快，但是缺点是并不能知道在这个运动过程中末端的移动轨迹。所以我觉得这两种方法各有优缺点，应该根据具体应用场景来选择。当然这次课设也有碰到些难题，比如原先随意规划的一条轨迹，在末端走直线变速轨迹规划中，有一小段距离过程的误差有点大，而且是随着运动误差先变大后变小，开始我以为是因为机器人在走那段距离时，每次机器人末端走的距离变大导致的，就多取了些离散点，虽然效果好了一点，但终究没有解决有误差的问题，之后便有考虑到是不是由于奇异值的情况，便把每次求解后的奇异值求出来，看是不是不满秩，结果依旧是白忙活一场，后来在外散步时突然想到是不是因为机器人到不了那些点的缘故呢，可又一想，但是那些点都是在工作空间里啊，后来终于弄明白了，原来是姿态的问题，虽然机器人可以到达那些点的位置，但同时满足姿态是做不到的，因为我一直给的都是世界坐标系的姿态，所以为了做到更好的效果，就往机器人侧稍微移动了一点，便成功解决了这个问题。
总之，这次课程让我获益匪浅，虽然与很多其他同学相比仍有很大差距，而且缺点也确实存在，不能奢求什么都能做到尽善尽美，但是自己有尽力认真去完成就感觉是非常不错的了，很感谢老师给了我走进机器人学的课堂，让我走进了一个全新的世界，以后定将不负所望，砥砺前行！
15
基于Matlab的七轴机器人工件搬运[路径规划]建模及仿真报告
