[0056] 获取曲线段运动轨迹坐标和反解得到各臂转动角度的过程由工控机完成，工控机 与运动控制卡通过PCI总线进行通讯。
[0057] 具体过程如下：
[005引1)获取机械臂末端运动轨迹曲线，如图3给出了两种运动轨迹曲线，a)为向低处 放置物品，b)为向高处放置物品；在该运动轨迹曲线上，连续选取包含起始点和终止点的n 个坐标点，记为Pi (Xi, y;，Zi)，其中i = 1，…，n ;
[005引。对选取的每一坐标点Pi(Xi,y;，Zi)进行运动学反解计算，得到该坐标点对应的 各驱动关节处的转动角度信息；从而获得运动轨迹曲线与各臂转动角度之间的对应关系；
[0060] 码煤机械臂的运动反解依托于经规划得到的曲线段运动轨迹。对得到的曲线段运 动轨迹进行运动学反解计算，得到各驱动关节处的转动角度。如图4所示，具体过程如下：
[0061] 考虑到码煤机械臂的工作特征，即臂4在工作过程中始终保持与地面平行，故臂 4的坐标系{4}和末端执行器5的坐标系（引始终在同一平面上，因此坐标系{4}的坐标 (Xi4，yi4，Zi4)和坐标系巧}的坐标（Xi5，yi5，Zi5)之间存在如下的对应关系：
[0062]
[0063] 如此，可先将末端运动轨迹由坐标系间移动到坐标系{4}，在此基础上进行运动 学分析。
[0064] 对末端轨迹坐标点Pl(Xl，yl，Zl)，即坐标系{引的坐标为（Xl，yl，Zl)的点，根据巧） 式可知，其在坐标系{4}中的坐标为
[0065] (Xi+l4sin白 1,Yi-lACOs白 1,Zi)。
[0066] 由
其中 tana=Yi/xi，求解得到坐标点Pi(Xi,yi,Zi)相应的臂1的转动角度01;
[0067] 由
求解得 到坐标点Pi(Xi，Yi，Zi)相应的臂3的转动角度0 3;
[0068]由
[0069] 其中tar
求解得到坐标点Pi(Xi，yi，Zi)相应的臂2的转动 角度0 2;
[0070] 根据0 4= 0 3- 0 2,得到坐标点Pi(Xi，yi，Zi)相应的臂4的转动角度0 4;由
其中
，求解得到坐标点Pi(Xi,yi,Zi)相应的臂5的转动角 度05。
[0071] 对每个选取的点做运动学反解，获得运动轨迹曲线与各臂转动角度之间的对应关 系。
[0072] 3)将上述对应关系下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传 感器检测并反馈转动信号，各臂联动完成给定的运动轨迹。
[0073] 具体可采用六轴运动控制卡，实现对五台伺服直流电机的位置控制。运动控制卡 与工控机间采用PCI总线进行数据传输。工控机完成运动轨迹坐标反解后，将得到的各臂 转动角度信号下发给运动控制卡，经运动控制卡处理后，生成伺服直流电机驱动信号，通过 控制伺服驱动器进而控制电机的转动角度和速度。
[0074] 伺服驱动器接受来自运动控制卡的控制信号，并按照控制指令完成对伺服直流电 机转动角度和速度的控制。
[00巧]传感器用于检测各臂转动角度信号，并将信号反馈给运动控制卡，实现对轨迹的 精确控制。
[0076] 在伺服直流电机的作用下，各臂联动，使末端走出给定的运动轨迹。
【主权项】
1. 一种五自由度旋转链式码垛机械臂的运动控制方法，其特征在于，包括如下步骤： 1) 获取机械臂末端运动轨迹曲线，在该运动轨迹曲线上，连续选取包含起始点和终止 点的η个坐标点，记为Pi (Xi, y;，Zi)，其中i = 1，. . .，η ; 2) 对选取的每一坐标点PiUi, Yi, Zi)进行运动学反解计算，得到该坐标点对应的各驱 动关节处的转动角度信息；从而获得运动轨迹曲线与各臂转动角度之间的对应关系； 3) 将上述对应关系下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传感器 检测并反馈转动信号，各臂联动完成给定的运动轨迹。2. 根据权利要求1所述的五自由度旋转链式码垛机械臂的运动控制方法，其特征在 于，步骤2)中所述的对坐标点Pi (Xi, y;，Zi)进行运动学反解计算，具体包括如下步骤： 1) 将选取的坐标AP1(^y1J1)由末端执行器5所在的坐标系{5}移动到 臂4所在的坐标系{4}中，得到运动轨迹曲线上的点在坐标系{4}中相应的坐标 ( Xl+l4sin Θ JTi-I4C0s Θ Ζι);其中I4为臂4的长度，Θ i为臂1的转动角度； 2) 不考虑机械臂末端执行器转动角度时，坐标系{4}处的齐次变换矩阵可表示为：其中，n'、〇'、a'分别为法向向量、方位向量和接近向量，用于描述臂4的姿态，其中，12、13分别为臂2和臂3的长度，Θ 2、Θ 3分别为臂2、臂3的转动角度，d2、d3、d4 分别为臂2与1间的距离、臂3与2间的距离、臂4与3间的距离， 由⑵式可得，其中tan α =yi/xi，求解得到坐标点Pi (Xi, y;，Zi)相应的臂1的转动角度Θ 1;3)由步骤2)中（I)、（3) 式可得，，:求解得到坐标 点P1 (Xl，yi，Z1)相应的臂3的转动角度Θ 3; 4) 由步骤2)中⑶式可得，其中求解得到坐标点Pi (Xi, yi，Zi)相应的臂2的转动角度θ 2; 5) 根据θ 4= θ 3- Θ 2，得到坐标点Pi (Xi，yi, Zi)相应的臂4的转动角度Θ 4; 6) 考虑机械臂末端执行器的转动角度时，码垛机械臂末端执行器位姿的齐次变换矩阵 可表示为：其中n、〇、a分别为法向向量、方位向量和接近向量，用于描述末端执行器5的姿态； 根据与连杆变换方程联立，码垛机械臂的运动学方程可表示为得到：I 求解得到坐标点Pi (Xi, yi，Zi)相应的臂5的转动角度θ 5。
【专利摘要】本发明公开了一种五自由度旋转链式码垛机械臂的运动控制方法，适用于全旋转的链式机械臂，通过对给定的末端运动轨迹进行运动学反解，得到其对应的各驱动关节处的转动角度信息，并下发至运动控制卡，通过伺服驱动系统驱动各臂转动，进而完成给定的运动轨迹，通过传感器检测并反馈各臂转动角度，以提高末端控制器所走出轨迹的精度。本发明的方法针对五自由度链式码垛机械臂的工作特征，通过坐标系的移动，使得反解的过程变得简单。
【IPC分类】B25J9/16
【公开号】CN105171744
【申请号】
【发明人】史伟民, 葛宏伟, 杨亮亮, 许守金
【申请人】浙江理工大学
【公开日】2015年12月23日
【申请日】2015年7月29日