PMAC应用五-运动学
运动学概念
运动学
机器人运动学分析基础
PMAC运动学子程序
子程序类型
案例介绍
编写运动学子程序
正运动学
逆运动学
位置报告PLC
结后语
运动学概念
运动学
关于运动学克雷格的《机器人学导论》是这样解释的：运动学研究操作臂的运动特性，而不考虑使操作臂产生运动时施加的力，在操作臂运动学中，将要研究操作臂的位置、速度、加速度、以及位置变量的所有高阶导数（包括对时间或其他变量的导数）。
简单理解是不考虑动力的情况下，当工具坐标和机器人结构两个坐标系不一致时，要实现一种转换，使得两个坐标系重合。
官方文档是这样介绍运动学子程序的：当工具坐标与执行机构(关节)的匹配位置之间存在非线性数学关系时，需要一个运动子学程序，子程序是实现输入转换电机位置到轴位置(正运动学)和反之(逆运动学)的方程(自己推到的方程)。

机器人运动学分析基础
对于机器人来说，最重要的是能准确的描述出机器人各个关节的位置、姿态，以及各个关节之间的关系。一个物体在空间的位姿可以通过在它上面建立一个坐标系，再将该坐标系通过一个4×4 矩阵R 表示。这个矩阵R 就是齐次变换矩阵。

求出机构的DH参数后，把相应的参数出入，然后把所有变化矩阵相乘，就能得到该机构的齐次变换矩阵。关于运动学的求解可以查看相关资料，这里不做讲述。

PMAC运动学子程序
子程序类型
正运动学
              输入：关节/电机位置               输出：刀尖/轴坐标系
逆运动学
              输入：刀尖/轴坐标系               输出：关节/电机位置
官方图解：
在这里插入图片描述

案例介绍

我以哈尔滨工业大学朱铭的《面向鞋业点胶五自由度机器人机构设计》里已经求解出来的运动学为例，介绍如何在PMAC运动学子程序里实现。

在这里插入图片描述


正解

在这里插入图片描述

逆解

在这里插入图片描述

由此，我们得到了一个从关节1到关节5其次变换矩阵，这是一个正运动学。

编写运动学子程序

正运动学

在Kinematic Routines文件夹下新建一个Forward Kinematic程序。

在这里插入图片描述

轴的设置和相关变量(以下两者都是运动学所需要的)，可在坐标系配置里设置好。

#x->I    //使用电机x的运动学方程
Coord[x].SegMoveTime > 0    //细分模式，单位ms

输入：

电机位置在局部变量Lx中表示电机x

x受Sys.MaxMotors的限制

在IDE中，变量KinPosMotorx自动用于Lx

输出：

轴位置在局部变量C0 - C31中表示，这些变量被D0覆盖，作为变量KinAxisUsed
e.g.如果使用X, Y, Z和C轴,KinAxisUsed = $40+$80+$100+$4 = $1C4,在IDE中，变量KinPosAxis axis会自动用于Ci。
在这里插入图片描述

常用的几个终端查询命令

&xp  //在线查询轴位
&xv  //查询轴速度
&xf  //查询轴跟随错误

从DH参数中可以看到，d1、d2、d3分别为移动轴的变量，θ4和θ5为两个旋转轴的变量，d1=KinPosMotor1、d2=KinPosMotor2、d3=KinPosMotor3、d4=KinPosMotor4、d5=KinPosMotor5。 p x p{_x} p x=KinPosAxisX、 p y p{_y} p y=KinPosAxisY、 p z p{_z} p z=KinPosAxisZ、θ4=KinPosAxisB、θ5=KinPosAxisC。

Open forward // 为指定坐标系打开正向运动学缓冲区
if (KinVelEna > 0) callsub 100; // 检查是否需要双通道，否则转到第100行。D0作为输入
KinAxisUsed =KinEnaAxisX+KinEnaAxisY+KinEnaAxisZ+KinEnaAxisB+KinEnaAxisC; // D0作为输出，用于指定反向运动学中使用的轴
n100: // 标记为第100行
if (Coord[1].HomeComplete) // 判断是否回原点？
{
	KinPosAxisB=KinPosMotor4;
	KinPosAxisC=KinPosMotor5;
	KinPosAxisX=a5+sind(KinPosMotor5)-d4-KinPosMotor3;
	KinPosAxisY=a5*cosd(KinPosMotor4)*cosd(KinPosMotor5);
	KinPosAxisZ=-a5*sind(KinPosMotor4)*cosd(KinPosMotor5)+KinPosMotor2;
}
else //无效;停止操作
{
	if (Ldata.Status & $40) // 从运动程序启动?
	{
	Coord[1].ErrorStatus = 255; // 用户设置中止错误
	}
	else // 从轴查询中调用
	{
	KinPosAxisX = sqrt(-1); //X轴返回非法值
	KinPosAxisY = sqrt(-1); //Y轴返回非法值
	KinPosAxisZ = sqrt(-1); //Y轴返回非法值
	KinPosAxisB = sqrt(-1); //Y轴返回非法值
	KinPosAxisC = sqrt(-1); //Y轴返回非法值
	}
}
return; // 返回调用程序
close // 关闭正向运动学缓冲区

到此正运动学编写完毕！

逆运动学

在Kinematic Routines文件夹下新建一个inverse Kinematic程序。

在这里插入图片描述

输入：

  • 轴位置在局部变量C0 - C31中
  • 变量KinPosAxisi自动用于Ci(用户单位)

输出：

  • 电机位置在局部变量Lx中，用#x->I语句表示电机x。
  • 变量KinPosMotorx自动用于Lx(电机计数)

在这里插入图片描述

open inverse (1)      // Put Coordinate System number inside "(cs)"
// --------------------User Code Goes Here------------------------
KinPosMotor4=KinPosAxisB;
KinPosMotor5=KinPosAxisC;
KinPosMotor3=a5*sind(KinPosAxisC)-d4-KinPosAxisX;
KinPosMotor2=KinPosAxisY+a5*sind(KinPosAxisB)*cosd(KinPosAxisC);
KinPosMotor1=a5*cosd(KinPosAxisB)*cosd(KinPosAxisC)+KinPosAxisZ;
close

逆运动学编写完毕！

位置报告PLC

global ReportActPosX,ReportActPosY,ReportActPosZ,ReportActPosB,ReportActPosC
global ReportDesPosX,ReportDesPosY,ReportDesPosZ,ReportDesPosB,ReportDesPosC
global ReportActVelX,ReportActVelY,ReportActVelZ,ReportActVelB,ReportActVelC

open plc PositionReportingPLC
// --------------------User Code Goes Here------------------------
Ldata.coord = 1 // Select coordinate system 1
PREAD
ReportActPosX = D6 // Actual X
ReportActPosY = D7 // Actual Y
ReportActPosZ = D8 // Actual Z
ReportActPosB = D1 // Actual B
ReportActPosC = D3 // Actual C

DREAD
ReportDesPosX = D6 // Desired X
ReportDesPosY = D7 // Desired Y
ReportDesPosZ = D8 // Desired Z
ReportDesPosB = D1 // Desired B
ReportActPosC = D2 // Desired C

vread
ReportActVelX = D6 // actual velocities X
ReportActVelY = D7 // actual velocities Y
ReportActVelZ = D8 // actual velocities Z
ReportActVelB = D1 // actual velocities B
ReportActVelC = D2 // actual velocities C

close

结后语
只要算出了正反解，PMAC的正反解子程序编写并不难，新手一定要多看文档，文档中都给出了相信程序的模板，下一节简单讲讲前瞻。

John J.Craig《机器人学导论》
朱铭《面向鞋业点胶五自由度机器人机构设计》
OMRON《Power PMAC 5-Day Training (Plus Opt ECAT Training)》