五自由度旋转链式码垛机械臂的运动控制方法
【技术领域】
[0001] 本发明设及机器人的运动控制领域，尤其设及一种五自由度旋转链式码煤机械臂 的运动控制方法。
【背景技术】
[0002] 随着码煤机械臂在工业领域中的广泛应用，针对码煤机械臂控制系统的研究便具 有重要意义，其中运动反解问题是控制码煤机械臂运动的先决条件。通过反解运算，可将 机械臂空间位姿转换为各转动关节变量，进而通过控制转动关节的转动角度实现机构的运 动，完成给定的运动轨迹。对于全旋转的链式机械臂，可用单个连杆变换矩阵41可(巧_)左乘 末端的齐次变换矩阵F，依次得到各臂的转动角度值，但如此会出现许多角度的禪合，对计 算带来不便。
【发明内容】
[0003] 本发明提供了一种五自由度链式码煤机械臂的运动控制方法，W克服反解过程中 角度禪合给计算带来的困扰。
[0004] 本发明的五自由度旋转链式码煤机械臂的运动控制方法，包括如下步骤：
[0005] 1)获取机械臂末端运动轨迹曲线，在该运动轨迹曲线上，连续选取包含起始点和 终止点的n个坐标点，记为Pi (Xi, y;，Zi)，其中i = 1，…，n ;
[0006] 2)对选取的每一坐标点Pi(Xi,y;，Zi)进行运动学反解计算，得到该坐标点对应的 各驱动关节处的转动角度信息；从而获得运动轨迹曲线与各臂转动角度之间的对应关系；
[0007] 3)将上述对应关系下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传 感器检测并反馈转动信号，各臂联动完成给定的运动轨迹。
[0008] 对于五自由度旋转链式机械臂，采用D-H方法描述该机械臂，对所有的关节建立 各自的坐标系，如图1所示。
[0009] 其中，臂1可绕Z轴转动，其转动角度记为0 1，所在坐标系记为{1};
[0010] 臂2长度为12,与臂1间距离为d2,可相对臂1转动，其转动角度记为0 2,所在坐 标系记为{2};
[0011] 臂3长度为13,与臂2间距离为d3,可相对臂2转动，其转动角度记为0 3,所在坐 标系记为{3};
[0012] 臂4长度为14,与臂3间距离为d4,可相对臂3转动，其转动角度记为0 4,所在坐 标系记为{4};
[0013] 末端执行器5可相对臂4绕Z轴转动，其转动角度记为0y所在坐标系记为巧};
[0014] 末端n、0、a分别为法向向量、方位向量和接近向量，用于描述末端执行器5的姿 态。
[0015] 根据图1得到的码煤机械臂的连杆参数如表1所示。
[0016] 表1码煤机械臂的连杆参数
[0017]
[001*^~码煤机械臂中连杆间变换的通式为：I ' '
[0020]
[00川由通式（1)可知连杆变换"It依赖于四个参数aki，aki，dk和0k，其中3k康示 关节偏移，ak1表示关节扭角，dk表示臂k与k-1间的距离，0k表示臂k的旋转角度。对 于转动关节k而言/I了是0k的函数。
[0022] 将表1中的连杆参数代入通式（1)可得各连杆的变换矩阵：
[0026] 步骤2)中所述的对坐标点Pi(Xi，yi，Zi)进行运动学反解计算，具体包括如下步 骤：
[0027] 1)将选取的坐标点Pi(Xi，yi，Zi)由末端执行器5所在的坐标系间移动 到臂4所在的坐标系{4}中，得到运动轨迹曲线上的点在坐标系{4}中相应的坐标 (Xi+l4sin0 1，Yi-IaCos01，Zi);其中14为臂4的长度，0 1为臂1的转动角度；
[0028] 2)不考虑机械臂末端执行器转动角度时，坐标系{4}处的齐次变换矩阵可表示 为：
[0029]
[0030] 其中，n'、〇'、a'分别为法向向量、方位向量和接近向量，用于描述臂4的姿态， [00引]由背游)-呼'=江(爲朽T(巧) 34了(錢,)，;其中)为连杆k相对连杆k-1的变换矩阵， 得：
[0032]
[003引其中，12、13分别为臂2和臂3的长度，02、03分别为臂2、臂3的转动角度，cU、d3、d4分别为臂2与1间的距离、臂3与2间的距离、臂4与3间的距离，
[0034] 由（3)式可得，
[0035]
，其中 tana=yl/Xl，求解得到坐标点Pl(Xl，yl，Zl)相应的臂l的转动角度0l;リ由步骤リ中 似、（4)式可得，
[0036]
求解得到 坐标点Pi(Xi，yi，Zi)相应的臂3的转动角度0 3;
[0037] 4)由步骤。中（4)式可得，
[0040] 求解得到坐标点Pi(Xi，yi，Zi)相应的臂2的转动角度0 2;
[00川W根据白4=白3_白2,得到坐标点Pi(Xi，yi，Zi)相应的臂4的转动角度白4;
[0042] 6)考虑机械臂末端执行器的转动角度时，码煤机械臂末端执行器位姿的齐次变换 矩阵可表示为：
[0043]
[0044] 其中n、0、a分别为法向向量、方位向量和接近向量，用于描述末端执行器5的姿 态；
[0045] 根据与连杆变换方程联立，码煤机械臂的运动学方程可表示为 F=4T(0义T辟巧T(巧巧T的HT(巧)，
[0046] 得到：
[0048] 求解得到坐标点Pi (Xi，yi，Zi)相应的臂5的转动角度0 5。
[0049] 本发明方法的有益效果：对于全旋转的链式机械臂，可用单个连杆变换矩阵 了(巧）左乘末端的齐次变换矩阵F，依次得到各臂的转动角度值，但如此会出现许多角度 的禪合，对计算带来不便。本发明的方法针对五自由度链式码煤机械臂的工作特征，即臂4 始终处于水平工作状态，通过坐标系的移动，使得反解的过程变得简单。
【附图说明】
[0050] 图1是五自由度链式机械臂的D-H模型；
[0051] 图2是本发明的方法的流程。
[005引图3是两种具体的运动轨迹曲线，其中a)为向低处放置物品，b)为向高处放置物 品。
[0053] 图4是对运动轨迹进行运动学反解的流程。
【具体实施方式】
[0054] 下面结合附图对本发明作进一步说明。
[0055] 本发明的对五自由度旋转链式机械臂的运动控制方法，其流程如图2所示，码煤 机械臂的运动控制流程包括获取曲线段运动轨迹坐标、反解得到各臂转动角度、下发转动 角度信号至运动控制卡、伺服驱动系统、传感器检测、各臂联动完成运动轨迹等。