图5 (a)工作空间三维图 图5 (b)工作空间侧视图
图5 (c)工作空间俯视图
3.4 分拣范围要求测试
由设计要求，要符合机器人的工作空间能够满足分拣工位的要求，所以为了测试本设计是否符合要求，将分拣系统中的篮子、传送带与工件放入工作空间中测试。测试结果显示机器人工作空间能够满足分拣工位范围的要求。具体测试效果如下图6所示。
图6 (a)分拣测试三维图 图6 (b)分拣测试主视图
图6 (c)分拣测试侧视图 图6 (d)分拣测试俯视图
4 机器人逆运动
6
逆运动学：如果要将机器人的末端放在一个期望的位姿，就必须知道机器人的每一个连杆的长度和关节的角度，才能将机器人末端定位在所期望的位姿，这就叫做逆运动学分析。
常用的机器人逆运动解法有解析法和数值法。解析法相比数值法计算量小，计算速度快，但由于解析解要求所有具有转动和移动关节的系统，在一个单一串联中总共有6个（或小于6个）自由度且设计时必须保证若干个关节轴为0 或 90°，才是可解的，并不符合本设计的七关节机器人。所以本设计机器人的逆运动所用方法是通过微分运动求数值解法。
4.1 微分运动
微分运动指机器人机构的微小运动，如果在一个小的时间段内测量或计算这个运动，则将得到速度。通过雅克比（Jacobian）矩阵将机械臂末端速度和各个关节速度联系起来，对于雅可比矩阵的求法本系统使用的是矢量积构造法。
由于末端的微分运动与各关节的微分运动存在如下关系（其中N=7）：
??Px???P??y???q1?????P???Pz???q2???W????W?=J???J[?Q]???x???W???q??y??N????Wz??
则有
??P?[?Q]=J?1????W?
本系统机器人微分运动主要过程为通过求取各个关节微分运动对末端微分运动之间速度关系表达式，利用速度矢量相加的原理便可以构造雅克比矩阵，所以末端的实际微小位移和微小转动就是各个关节造成的微位移和微转动的总和，即
N?j?pN???pN??j?1 ?Nj???N????N?j?1?7
??q1???0???q2? ??pN??Wa1?(pN?p1)Wa2?(pN?p2)???WaN?1?(pN?pN?1)????????WWWW??aaaa?N??12N?1N????q?N?然后通过以上建立的各关节间的齐次变换矩阵便可推导出本系统机器人雅克比矩阵为：
?Wa1?(p8?p1)?Wa1?Wa2?(p8?p2)Wa2Wa3?(p8?p3)Wa3Wa4?(p8?p4)Wa4Wa5?(p8?p5)Wa5Wa6?(p8?p6)Wa7? ? Wa60? 其中：
WWWa1?A0a1a2?A0A1a2a3?A0A1A2a3a4?A0A1A2A3a4a5?A0A1A2A3A4a5a6?A0A1A2A3A4A5a6a7?A0A1A2A3A4A5A6a7p1??A0??Pp2??A0A1??Pp3??A0A1A2??P
WWWW p4??A0A1A2A3??Pp5??A0A1A2A3A4??Pp6??A0A1A2A3A4A5??Pp8??A0A1A2A3A4A5A6A7??P
其中的pi?i(i?1,2,3,4,5,6,7)表示从机器人坐标原点指向当前关节的向量，这里的a1~a6指的是关节局部坐标系中转轴或者滑动轴的朝向。对于DH法，都是[0,0,1]，即z轴。
通过微分运动求得机器人各个关节的微分角度或滑动长度d?后，然后将得到的微分角度或长度对上一时刻各关节的角度或长度进行更新，更新后的角度或长度数据便可以使机器人通过正运动而向一定的方向上移动一定的微分距离，如果如此多次循环，则可以使机器人的末端在先前设定的方向上移动一定的距离。具体角度或长度的更新方式如下所示：
?1??1?d?1?2??2?d?2?3??3?d?3?4??4?d?4?5??5?d?5?6??6?d?6d7?d7?d?7
4.2 数值解法
由于单独微分运动是一个开环的正运动，所以每次微分运动都可能会存在一
8
定的微分误差，且误差经过多次累计后与目标位置就会造成较大误差，所以提出数值法来通过闭环的不断修正思想来解决。
数值解法：是采用闭环思想，通过机器人当前末端位姿与目标的位姿的差异来对机器人关节进行不断的迭代修正，以逐步趋近目标位姿的方法。具体原理如下图7所示（其中的修正量计算部分是通过4.1节中讲解的微分运动实现的）。
图7 数值解法原理
数值解法的主要过程为：首先给定各关节初始角度或长度（机器人初始状态）q、机器人末端的目标位置pref和姿态Rref；然后通过正运动计算机器人当前末端的位置p和姿态R，并与目标位置和姿态进行差运算得到当前误差；之后采用牛顿下山算法输出修正量?q更新关节变量，如此循环往复；直到误差达到设定的误差范围。
结合微分运动，可以得到各个关节的微分修正量为：
其中λ为下山因子，本系统λ选择的是0.8。总误差err小于0.001时，结束位姿的修正。
位置误差的计算公式如下：
角度误差的计算公式如下：
误差旋转矩阵对应的角速度误差为：
9
最后，总的误差可以定义为：
4.3 轨迹规划
轨迹规划可以分为机器人末端的轨迹规划和关节空间的轨迹规划。采用机器人末端位姿的运动来描述机器人的运动称为直角坐标空间描述。采用关节量来描述机器人的运动称为关节空间描述。
4.3.1 末端轨迹规划
本设计对机器人末端进行了两端不同类型的轨迹规划，一段是末端是直线的变速五次多项式轨迹规划，和一段末端是4-3-4轨迹的匀速轨迹规划。
五次多项式轨迹规划： 位置可描述为：
约束条件：
本系统中给定初始位置q0=0，末端位置q1=150，v0=v1=a0=a1=0，可规划出如下图8中的位置、速度、加速度曲线，根据规划出的位置曲线进行离散化选取
10
基于Matlab的七轴机器人工件搬运[路径规划]建模及仿真报告
