PMAC应用五-运动学
PMAC五-运动学运动学概念运动学PMAC运动学子程序机器人运动学分析基础运动学概念运动学关于运动学克雷格的《机器人学导论》是这样解释的：运动学研究操作臂的运动特性，而不考虑使操作臂产生运动时施加的力，在操作臂运动学中，将要研究操作臂的位置、速度、加速度、以及位置变量的所有高阶导数（包括对时间或其他变量的导数）。简单理解是不考虑动力的情况下，当工具坐标和机器人结构两个坐标系不一致时，要实现一种转换
运动学概念
运动学
关于运动学克雷格的《机器人学导论》是这样解释的：运动学研究操作臂的运动特性，而不考虑使操作臂产生运动时施加的力，在操作臂运动学中，将要研究操作臂的位置、速度、加速度、以及位置变量的所有高阶导数（包括对时间或其他变量的导数）。
简单理解是不考虑动力的情况下，当工具坐标和机器人结构两个坐标系不一致时，要实现一种转换，使得两个坐标系重合。
官方文档是这样介绍运动学子程序的：当工具坐标与执行机构(关节)的匹配位置之间存在非线性数学关系时，需要一个运动子学程序，子程序是实现输入转换电机位置到轴位置(正运动学)和反之(逆运动学)的方程(自己推到的方程)。
机器人运动学分析基础
对于机器人来说，最重要的是能准确的描述出机器人各个关节的位置、姿态，以及各个关节之间的关系。一个物体在空间的位姿可以通过在它上面建立一个坐标系，再将该坐标系通过一个4×4 矩阵R 表示。这个矩阵R 就是齐次变换矩阵。
R
=
(
i
0
R
i
0
P
0
1
)
=
(
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
)
R=\begin{pmatrix} {^0_i}R & {^0_i}P \\ 0 & 1 \\ \end{pmatrix}= \begin{pmatrix} n{_x} & o{_x} & a{_x}& p{_x} \\ n{_y} & o{_y} & a{_y}& p{_y} \\ n{_z} & o{_z} & a{_z}& p{_z} \\ 0 & 0& 0 & 1 \\ \end{pmatrix}
R=(i0R0i0P1)=⎝⎜⎜⎛nxnynz0oxoyoz0axayaz0pxpypz1⎠⎟⎟⎞
齐次矩阵中3×3 矩阵
i
0
R
{^0_i}R
i0R表示固连在物体i 上的坐标系{
O
i
O{_i}
Oi}在基座坐标系{
O
o
O{_o}
Oo} 中的姿态，3×1 矩阵
i
0
P
{^0_i}P
i0P表示固连在物体i 上的坐标系{
O
I
O{_I}
OI}在基座坐标系{
O
o
O{_o}
Oo}中的位置。坐标系之间的变换可以通过齐次变换矩阵表示，基本的变换矩阵有：
平移齐次变换：
T
r
a
n
s
(
x
,
y
,
z
)
=
(
1
0
0
x
0
1
0
y
0
0
1
z
0
0
0
1
)
Trans(x,y,z)=\begin{pmatrix} 1 & 0 & 0 & x \\ 0 & 1 & 0 & y \\ 0 & 0 & 1 & z \\0 & 0 & 0 & 1 \\ \end{pmatrix}
Trans(x,y,z)=⎝⎜⎜⎛100001000010xyz1⎠⎟⎟⎞
旋转齐次变换：
R
o
t
(
X
,
θ
)
=
(
1
0
0
0
0
c
θ
−
s
θ
0
0
s
θ
−
c
θ
0
0
0
0
1
)
Rot(X,θ)=\begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & cθ & -sθ & 0 \\ 0 & sθ & -cθ & 0 \\0 & 0 & 0 & 1 \\ \end{pmatrix}
Rot(X,θ)=⎝⎜⎜⎛10000cθsθ00−sθ−cθ00001⎠⎟⎟⎞
R
o
t
(
Y
,
θ
)
=
(
c
θ
0
s
θ
0
0
1
0
0
−
s
θ
0
c
θ
0
0
0
0
1
)
Rot(Y,θ)=\begin{pmatrix} cθ & 0 & sθ & 0 \\ 0 & 1 & 0 & 0 \\ -sθ & 0 & cθ & 0 \\0 & 0 & 0 & 1 \\ \end{pmatrix}
Rot(Y,θ)=⎝⎜⎜⎛cθ0−sθ00100sθ0cθ00001⎠⎟⎟⎞
R
o
t
(
Z
,
θ
)
=
(
c
θ
−
s
θ
0
0
s
θ
c
θ
0
0
0
0
1
0
0
0
0
1
)
Rot(Z,θ)=\begin{pmatrix} cθ & -sθ & 0 & 0 \\ sθ & cθ & 0 & 0 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1 \\ \end{pmatrix}
Rot(Z,θ)=⎝⎜⎜⎛cθsθ00−sθcθ0000100001⎠⎟⎟⎞
式中 Trans 表示平移，Rot 表示旋转，cθ 表示cosθ ， sθ 表示sinθ 。
标准DH参数齐次变换矩阵：
i
A
i
+
1
=
R
o
t
(
Z
,
θ
i
)
T
r
a
n
s
(
0
,
0
,
d
i
)
T
r
a
n
s
(
a
i
,
0
,
0
)
R
o
t
(
X
,
α
i
)
=
{^i}A{_i+_1}=Rot(Z,θ{_i})Trans(0,0,d{_i})Trans(a{_i},0,0)Rot(X,\alpha{_i})=
iAi+1=Rot(Z,θi)Trans(0,0,di)Trans(ai,0,0)Rot(X,αi)=
(
c
θ
i
−
c
α
i
s
θ
i
s
α
i
s
θ
i
a
i
c
θ
i
s
θ
i
c
α
i
c
θ
i
−
s
α
i
c
θ
i
a
i
s
θ
i
0
s
α
i
c
α
i
d
i
0
0
0
1
)
\begin{pmatrix} cθ{_i} & -c\alpha{_i}sθ{_i} & s\alpha{_i}sθ{_i} & a{_i}cθ{_i} \\ sθ{_i} &c\alpha{_i}cθ{_i} & -s\alpha{_i}cθ{_i} & a{_i}sθ{_i} \\0 & s\alpha{_i} & c\alpha{_i} & d{_i} \\0 & 0 & 0 & 1 \\ \end{pmatrix}
⎝⎜⎜⎛cθisθi00−cαisθicαicθisαi0sαisθi−sαicθicαi0aicθiaisθidi1⎠⎟⎟⎞
求出机构的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} px=KinPosAxisX、 p y p{_y} py=KinPosAxisY、 p z p{_z} pz=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)》
CSDN联合极客时间，共同打造面向开发者的精品内容学习社区，助力成长！
更多推荐
2
0- 0
已为社区贡献4条内容
所有评论(0)