基于PLC控制的两关节机械手运动仿真 - 图文

表 7-1 机械手连杆参数

7.3 两关节机械手运动方程的建立

当已知各关节变量的值（

?1、?2），求机械手末端连杆的位姿（n，o，a，p），称

为解机械手运动方程。当机械手各连杆参数确定后，根据机械手学的知识，可以直接写.出相邻两坐标系间的变换矩阵：

?c?i?s?c?i?1?ii?1iT??s?is?i?1?0??s?ic?ic?i?1c?is?i?100?s?i?1c?i?10ai?1??dis?i?1??dic?i?1??1?

代入表2-1给出的各连杆参数，求得个连杆的变换矩阵后将各个连杆变换矩阵相乘，便得到 “手臂变换矩阵”

20T，

0212T=01T（?1）2T（?2）3T

它是关节变量

?1、?2的函数。

最后，求出连杆变换之积：

?nx?n2?y0T=?nz??0oxoyoz0axayaz0px?py??pz??1?

7.4 机械手逆向运动方程的建立

运动学逆问题，或叫做运动学反解，即已知机械手末端连杆的位姿（n，o，a，p），求各关节变量（

?1、?2）的值。

28

运动学反解有很多方法，例如Paul等人提出的反变换法，Lee和Ziegler提出的几何法和Pieper解法等，利用反变换法（也称代数法）求解得：

将运动方程写成

?nx?n0?y2T=?nz??0oxoyoz0axayaz0px?py?12?=01T（?1）2T（?2）3T?pz?1?

?1?Atan2(py,px)?Atan2(d2,?px2?py2?d22)式中

k?222222px?py?pz2?a2?a3?d2?d4

2a2

至此，机械手逆向运动方程参数已求出。

7.5 Matlab仿真

按照三次多项式插值法，用MATLAB软件编制程序（程序见附录（2）），输入机械手各关节起始点、路径点、和终止点的关节值，分别求出各关节变量的位移方程，并用MATLAB绘图功能分别绘出机械手关节1、2、3的位移、角速度、角加速度曲线[15]，分别如图 7-1、图 7-2和错误！未找到引用源。所示：

图 7-1 关节1的位移、角速度和角加速度曲线

29

由图 7-1可以看出机械手关节1的位移、角速度和角加速度曲线都是连续的，并且角速度在起始点和终止点都为零，满足轨迹规划的动力学约束要求。同理，下面的图 7-2和错误！未找到引用源。也都满足要求。

图 7-2 关节2的位移角速度和角加速度变化曲线

用MATLAB]中Robotic toolbox（机械手工具箱）建立的运动仿真如下： 图 7-3是机械手运动到终止点的三维截图，具体的运动过程动画可以用MATLAB运行附录中的运动仿真程序（见附录）

图 7-3 运动仿真终止点截图

30

由仿真的结果可以看出，各关节的位移、速度、和加速度均连续，且在初始点和终止点的速度均为零，说明此次过路径点的避障轨迹规划满足要求。而且容易知道，可以选取多个路径点，在每相邻两个路径点之间用三次多项式插值法进行路径规划，不仅可以满足位移、角速度和角加速度要求，而且能通过路径点以满足避障要求。

31