代码收藏家 纯手工Codesys的6轴机械手控制程序记录
目录
一、概述
一直想学习6轴机械手的运动学，一直没有空闲。趁着12月疫情，闭门研究6轴机械手的运动并且用codesys简单实现。
机械手的控制我自己的理解是，机械手末端的位置和方向 -> 逆解出对应各关节的角度 ->控制关节旋转至目标角度。机械手的执行的运动品质，就转变为 机械手末端的位置和方向的大量的插补点，而且要考虑这些插补点逆解出的各关节的角度的运动趋势。特别是 各关节的运动趋势，因为运动趋势是最终关节电机如何运转和能不能运转的关键点。
本文利用codesys实现和验证6轴机械手的正解和逆解和简易的6轴机械手的插补运动（直线、阿基米德旋线+球面）。其中直线运动子程序，做了简单的梯形加减速，是固定方向下的位置直线运动轨迹；而阿基米德旋线+球面则只做了位置插补控制，做了变方向和变位置的运动轨迹。
二、程序的架构
3.1 截图
3.2 架构介绍
程序主要分了3个task。
MainTask(100ms,优先级8) := 按钮和界面的触发和更新。
Motion_Task(50ms,优先级2) := 运动的插补运算。
Servo_Task(1ms,优先级1) := 关节伺服运动控制功能块。(虽然是虚拟轴)
3.3 代码的展示
3.3.1机械手正解
//DirectKinematics 机械手正解函数，作者：AlongWu
FUNCTION DirectKinematics : HandJointPos
VAR_INPUT
HJointAg :HandJointAg; //机械手关节数据
END_VAR
VAR
T_Htf_Arr :ARRAY[0..5] OF ARRAY[0..3,0..3] OF LREAL; //齐次矩阵数组
T_Arr :ARRAY[0..5] OF ARRAY[0..3,0..3] OF LREAL; //机械手关节映射矩阵
i :INT;
END_VAR
//DH表格thet角赋值
GVL.DHTable.ItemArr[0].Thet := HJointAg.J1;
GVL.DHTable.ItemArr[1].Thet := HJointAg.J2;
GVL.DHTable.ItemArr[2].Thet := HJointAg.J3;
GVL.DHTable.ItemArr[3].Thet := HJointAg.J4;
GVL.DHTable.ItemArr[4].Thet := HJointAg.J5;
GVL.DHTable.ItemArr[5].Thet := HJointAg.J6;
//DH表格转齐次矩阵
FOR i:=0 TO 5 BY 1 DO
T_Htf_Arr[i] := HTF_MatrixSTD(GVL.DHTable.ItemArr[i]);
END_FOR
//右乘，获得全部关节的映射矩阵
T_Arr[0] := T_Htf_Arr[0];
T_Arr[1] :=Matrix44PointMul(T_Arr[0],T_Htf_Arr[1]);
T_Arr[2] :=Matrix44PointMul(T_Arr[1],T_Htf_Arr[2]);
T_Arr[3] :=Matrix44PointMul(T_Arr[2],T_Htf_Arr[3]);
T_Arr[4] :=Matrix44PointMul(T_Arr[3],T_Htf_Arr[4]);
T_Arr[5] :=Matrix44PointMul(T_Arr[4],T_Htf_Arr[5]);
//通过映射矩阵获取各关节的位置信息
DirectKinematics.J1 := RotationMatrix2pos(T_Arr[0]);
DirectKinematics.J2 := RotationMatrix2pos(T_Arr[1]);
DirectKinematics.J3 := RotationMatrix2pos(T_Arr[2]);
DirectKinematics.J4 := RotationMatrix2pos(T_Arr[3]);
DirectKinematics.J5 := RotationMatrix2pos(T_Arr[4]);
DirectKinematics.J6 := RotationMatrix2pos(T_Arr[5]);
3.3.2机械手逆解
//机械手逆解 InverseKinematics ，作者：AlongWu
FUNCTION InverseKinematics : HandJointResult
VAR CONSTANT
EmptyJointStru :ARRAY[0..3] OF HandJointAg:=[
(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0),
(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0),
(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0),
(J1Gap:=0,J2Gap:=0,J3Gap:=0,J4Gap:=0,J5Gap:=0,J6Gap:=0,J1:=0,J2:=0,J3:=0,J4:=0,J5:=0,J6:=0)
];
IptAgMax :LREAL:=500; //插补角度间距允许值
END_VAR
VAR_INPUT
KeyPosStru :PosStru; //目标坐标数据元
SortType :INT; //逆解结果的决策要求（0=总关节移动量最少，1=每个关节移动量不能大于"插补角度间距允许值"）
END_VAR
VAR
i :INT;
j :INT;
//k :INT;
T :ARRAY[0..3,0..3] OF LREAL; //目标坐标元的旋转矩阵
T_Arr :ARRAY[0..5] OF ARRAY[0..3,0..3] OF LREAL; //机械手关节映射矩阵
nx :LREAL;
ny :LREAL;
nz :LREAL;
ox :LREAL;
oy :LREAL;
oz :LREAL;
ax :LREAL;
ay :LREAL;
az :LREAL;
px :LREAL;
py :LREAL;
pz :LREAL;
tmp1 :LREAL;
tmp2 :LREAL;
tmp3 :LREAL;
tmp4 :LREAL;
tmp5 :LREAL;
tmp6 :LREAL;
d1 :LREAL;
d4 :LREAL;
d5 :LREAL;
d6 :LREAL;
a2 :LREAL;
a3 :LREAL;
sin1 :LREAL;
cos1 :LREAL;
sin2 :LREAL;
cos2 :LREAL;
sin3 :LREAL;
cos3 :LREAL;
sin4 :LREAL;
cos4 :LREAL;
sin5 :LREAL;
cos5 :LREAL;
sin6 :LREAL;
cos6 :LREAL;
th1 :LREAL;
th2 :LREAL;
th3 :LREAL;
th4 :LREAL;
th5 :LREAL;
th6 :LREAL;
m :LREAL;
n :LREAL;
c :BOOL;
Thet1 :ARRAY[0..1] OF Thet1Result; //逆解角度结果缓存
tmpJointStru :ARRAY[0..3] OF HandJointAg; //逆解结果缓存
tmpJointStruLen :INT;
tmpJs :HandJointAg;
CheckHandJointPos :HandJointPos;
END_VAR
//isNull的标志复位
InverseKinematics.IsNull :=FALSE;
//目标坐标数据元转旋转矩阵
T :=PosStur2RotationMatrix(KeyPosStru);
//旋转矩阵的值定位
nx:=T[0,0];
ny:=T[1,0];
nz:=T[2,0];
ox:=T[0,1];
oy:=T[1,1];
oz:=T[2,1];
ax:=T[0,2];
ay:=T[1,2];
az:=T[2,2];
px:=T[0,3];
py:=T[1,3];
pz:=T[2,3];
//DH表格赋值
d6 := GVL.DHTable.ItemArr[5].ZGap;
d4 := GVL.DHTable.ItemArr[3].ZGap;
d1 := GVL.DHTable.ItemArr[0].ZGap;
d5 := GVL.DHTable.ItemArr[4].ZGap;
a2 := GVL.DHTable.ItemArr[1].Bridge;
a3 := GVL.DHTable.ItemArr[2].Bridge;
//开始计算
tmp1 := d6 * ay - py;
tmp2 := px - d6 * ax;
tmp1 := tmp1 * tmp1 + tmp2 * tmp2;
IF tmp1 = 0 THEN
tmp2 :=0;
ELSE
tmp2 := d4 / SQRT(tmp1);
END_IF
IF tmp2 > 1 OR tmp2 <-1 THEN
//目标姿态不能实现
InverseKinematics.IsNull :=TRUE;
RETURN;
END_IF
tmp4 := ACOS(tmp2) * 180 / GVL.PI;
tmp5 := MATH.Atan2(px - d6 * ax, d6 * ay - py) * 180 / GVL.PI;
//thet1的2个解
Thet1[0].thet1 := tmp5 - tmp4;
Thet1[1].thet1 := tmp5 + tmp4;
FOR i:=0 TO 1 BY 1 DO
th1 := Thet1[i].thet1;
sin1 := SinAg(th1);
cos1 := CosAg(th1);
//计算th6
tmp1 := sin1*ox - cos1*oy;
tmp2 := sin1*nx - cos1*ny;
th6 := MATH.atan2(-tmp1,tmp2)*180 / GVL.PI;
sin6 := SinAg(th6);
cos6 := CosAg(th6);
Thet1[i].thet6 :=th6;
//计算th5
tmp1 := (sin1 * nx - cos1 * ny)*cos6-(sin1*ox-cos1*oy)*sin6;
IF tmp1>1 OR tmp1<-1 THEN
结束本次循环，进入下一个循环
CONTINUE;
END_IF
tmp2 := ASIN(tmp1) ;
tmp3 := sin1 * ax - cos1 * ay;
IF tmp3>1 OR tmp3<-1 THEN
结束本次循环，进入下一个循环
CONTINUE;
END_IF
tmp4 := ACOS(tmp3) * 180 / Math.PI;
IF tmp2>=0 THEN
Thet1[i].thet5 := tmp4;
ELSE
Thet1[i].thet5 := -tmp4;
END_IF
th5 := Thet1[i].thet5;
sin5 := SinAg(th5);
cos5 := CosAg(th5);
//计算th3
m := d5 * ((cos1 * nx + sin1 * ny) * sin6 + (cos1 * ox + sin1 * oy) * cos6) - d6 * (cos1 * ax + sin1 * ay) + cos1 * px + sin1 * py;
n := d5 * (nz * sin6 + oz * cos6) - d6 * az + pz - d1;
tmp1 := 2 * a2 * a3;
IF tmp1 = 0 THEN
tmp3 := 0;
ELSE
tmp3 := (m * m + n * n - a2 * a2 - a3 * a3) / tmp1;
END_IF
IF tmp3 > 1 OR tmp3 < -1 THEN
//结束本次循环，进入下一个循环
CONTINUE;
END_IF
tmp4 := ACOS(tmp3) * 180 / GVL.PI;
Thet1[i].thet3r[0].thet3 := -tmp4;
Thet1[i].thet3r[1].thet3 := tmp4;
FOR j:=0 TO 1 BY 1 DO
th3 := Thet1[i].thet3r[j].thet3;
cos3 := CosAg(th3);
sin3 := SinAg(th3);
//计算 th2
tmp1 := n * (a2 + a3 * cos3) - m * a3 * sin3;
tmp2 := m * (a2 + a3 * cos3) + n * a3 * sin3;
th2 := (MATH.Atan2(tmp1, tmp2) * 180 / Math.PI);
Thet1[i].thet3r[j].thet2 := th2;
//计算 th4
tmp1 := (nz * cos6 - oz * sin6) * cos5 - az * sin5;
tmp2 := ((cos1 * nx + sin1 * ny) * cos6 - (cos1 * ox + sin1 * oy) * sin6) * cos5 - (cos1 * ax + sin1 * ay) * sin5;
tmp3 := MATH.Atan2(tmp1, tmp2) * 180 / Math.PI;
th4 := tmp3 - th2 - th3;
Thet1[i].thet3r[j].thet4 := th4;
END_FOR
END_FOR
//角度超限检查并且 留下合规的解
//复位逆解结果缓存
tmpJointStru := EmptyJointStru;
tmpJointStruLen :=0;
FOR i:=0 TO 1 BY 1 DO
FOR j:=0 TO 1 BY 1 DO
c := LrealZooCmp(GVL.JointParamArr[1].maxAg,GVL.JointParamArr[1].minAg,Thet1[i].thet3r[j].thet2);
IF c THEN
c := LrealZooCmp(GVL.JointParamArr[2].maxAg,GVL.JointParamArr[2].minAg,Thet1[i].thet3r[j].thet3);
IF c THEN
c := LrealZooCmp(GVL.JointParamArr[3].maxAg,GVL.JointParamArr[3].minAg,Thet1[i].thet3r[j].thet4);
IF c THEN
c := LrealZooCmp(GVL.JointParamArr[4].maxAg,GVL.JointParamArr[4].minAg,Thet1[i].thet5);
IF c THEN
c := LrealZooCmp(GVL.JointParamArr[5].maxAg,GVL.JointParamArr[5].minAg,Thet1[i].thet6);
IF c THEN
tmpJointStru[tmpJointStruLen].J1 := Thet1[i].thet1;
tmpJointStru[tmpJointStruLen].J1Gap := Thet1[i].thet1 - GVL.LR_Hand_J1;
tmpJointStru[tmpJointStruLen].J2 := Thet1[i].thet3r[j].thet2;
tmpJointStru[tmpJointStruLen].J2Gap := Thet1[i].thet3r[j].thet2 - GVL.LR_Hand_J2;
tmpJointStru[tmpJointStruLen].J3 := Thet1[i].thet3r[j].thet3;
tmpJointStru[tmpJointStruLen].J3Gap := Thet1[i].thet3r[j].thet3 - GVL.LR_Hand_J3;
tmpJointStru[tmpJointStruLen].J4 := Thet1[i].thet3r[j].thet4;
tmpJointStru[tmpJointStruLen].J4Gap := Thet1[i].thet3r[j].thet4 - GVL.LR_Hand_J4;
tmpJointStru[tmpJointStruLen].J5 := Thet1[i].thet5;
tmpJointStru[tmpJointStruLen].J5Gap := Thet1[i].thet5 - GVL.LR_Hand_J5;
tmpJointStru[tmpJointStruLen].J6 := Thet1[i].thet6;
tmpJointStru[tmpJointStruLen].J6Gap := Thet1[i].thet6 - GVL.LR_Hand_J6;
tmpJointStruLen := tmpJointStruLen +1;
END_IF
END_IF
END_IF
END_IF
END_IF
END_FOR
END_FOR
//根据 SortType 挑选最优解并输出
IF tmpJointStruLen > 0 THEN
//先赋值第一个解
tmpJs :=tmpJointStru[0];
tmp1 := ABS(tmpJs.J1Gap)+ ABS(tmpJs.J2Gap)+ ABS(tmpJs.J3Gap)+ ABS(tmpJs.J4Gap)+ ABS(tmpJs.J5Gap)+ ABS(tmpJs.J6Gap);
//正解 逆解的结果 ，获取各关节的位置
CheckHandJointPos := DirectKinematics(tmpJs);
//区域限制
//各关节 不能低于 底面50
IF CheckHandJointPos.J1.pz <50 OR CheckHandJointPos.J2.pz <50 OR CheckHandJointPos.J3.pz <50 OR CheckHandJointPos.J4.pz <50 OR CheckHandJointPos.J5.pz <50 OR CheckHandJointPos.J6.pz <50 THEN
//不符合条件，剔除
InverseKinematics.IsNull := TRUE;
ELSE
//判断SortType
IF SortType =1 THEN
IF ABS(tmpJs.J1Gap)>IptAgMax OR ABS(tmpJs.J2Gap)>IptAgMax OR ABS(tmpJs.J3Gap)>IptAgMax OR ABS(tmpJs.J4Gap)>IptAgMax OR ABS(tmpJs.J5Gap)>IptAgMax OR ABS(tmpJs.J6Gap)>IptAgMax THEN
//不符合条件，剔除
InverseKinematics.IsNull := TRUE;
ELSE
//符合条件，赋值
InverseKinematics.HandJoint := tmpJs;
END_IF
ELSE
//符合条件，赋值
InverseKinematics.HandJoint := tmpJs;
END_IF
END_IF
IF tmpJointStruLen > 1 THEN
IF InverseKinematics.IsNull THEN
tmp1:= 1000000;
END_IF
FOR i:=1 TO (tmpJointStruLen-1) BY 1 DO
tmpJs :=tmpJointStru[i];
tmp2 := ABS(tmpJs.J1Gap)+ ABS(tmpJs.J2Gap)+ ABS(tmpJs.J3Gap)+ ABS(tmpJs.J4Gap)+ ABS(tmpJs.J5Gap)+ ABS(tmpJs.J6Gap);
//正解 逆解的结果 ，获取各关节的位置
CheckHandJointPos := DirectKinematics(tmpJs);
//区域限制
//各关节 不能低于 底面50
IF CheckHandJointPos.J1.pz <50 OR CheckHandJointPos.J2.pz <50 OR CheckHandJointPos.J3.pz <50 OR CheckHandJointPos.J4.pz <50 OR CheckHandJointPos.J5.pz <50 OR CheckHandJointPos.J6.pz <50 THEN
//不符合条件，剔除
CONTINUE;
ELSE
//判断SortType
IF SortType =1 THEN
IF ABS(tmpJs.J1Gap)>IptAgMax OR ABS(tmpJs.J2Gap)>IptAgMax OR ABS(tmpJs.J3Gap)>IptAgMax OR ABS(tmpJs.J4Gap)>IptAgMax OR ABS(tmpJs.J5Gap)>IptAgMax OR ABS(tmpJs.J6Gap)>IptAgMax THEN
//不符合条件，剔除
CONTINUE;
ELSE
//符合条件，赋值
InverseKinematics.IsNull := FALSE;
END_IF
ELSE
//符合条件，赋值
InverseKinematics.IsNull := FALSE;
END_IF
END_IF
//关节移动量最小
IF tmp1 > tmp2 THEN
tmp1 := tmp2;
InverseKinematics.HandJoint := tmpJs;
END_IF
END_FOR
END_IF
ELSE
//tmpJointStruLen 为0 ，没有解
InverseKinematics.IsNull := TRUE;
END_IF
3.3.3主程序
//作者：AlongWu
PROGRAM PLC_PRG
VAR
Point1_R_Trig :R_TRIG; //point1按钮上升沿
Point2_R_Trig :R_TRIG; //point2按钮上升沿
Line_R_Trig :R_TRIG; //Line按钮上升沿
Ball_R_Trig :R_TRIG; //Ball按钮上升沿
HJA :HandJointAg;
END_VAR
//按钮上升沿
Point1_R_Trig(CLK:=GVL.B_Point1Bt , Q=> );
Point2_R_Trig(CLK:=GVL.B_Point2Bt , Q=> );
Line_R_Trig(CLK:=GVL.B_LineBt , Q=> );
Ball_R_Trig(CLK:=GVL.B_BallBt , Q=> );
//point1运动触发
IF Point1_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
GVL.LR_TargetPX := 100;
GVL.LR_TargetPY := -100;
GVL.LR_TargetPZ := 100;
GVL.LR_TargetRX := 180;
GVL.LR_TargetRY := 0;
GVL.LR_TargetRZ := 0;
GVL.MotionType := MotionType_Enum.Point;
GVL.B_MotionAct := TRUE;
END_IF
//point2运动触发
IF Point2_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
GVL.LR_TargetPX := 200;
GVL.LR_TargetPY := -150;
GVL.LR_TargetPZ := 150;
GVL.LR_TargetRX := 90;
GVL.LR_TargetRY := 90;
GVL.LR_TargetRZ := 90;
GVL.MotionType := MotionType_Enum.Point;
GVL.B_MotionAct := TRUE;
END_IF
//line运动触发
IF Line_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
GVL.LR_TargetPX := 100;
GVL.LR_TargetPY := -100;
GVL.LR_TargetPZ := 100;
GVL.LR_TargetRX := 180;
GVL.LR_TargetRY := 0;
GVL.LR_TargetRZ := 0;
GVL.MotionType := MotionType_Enum.Line;
GVL.B_MotionAct := TRUE;
END_IF
//ball运动触发
IF Ball_R_Trig.Q AND GVL.B_MotionAct = FALSE THEN
GVL.LR_Ball_R :=10;
GVL.LR_Ball_Cx :=100;
GVL.LR_Ball_Cy :=-200;
GVL.LR_Ball_Cz :=200;
GVL.MotionType := MotionType_Enum.Ball;
GVL.B_MotionAct := TRUE;
END_IF
//计算所有关节的坐标
HJA.J1 := GVL.LR_Hand_J1;
HJA.J2 := GVL.LR_Hand_J2;
HJA.J3 := GVL.LR_Hand_J3;
HJA.J4 := GVL.LR_Hand_J4;
HJA.J5 := GVL.LR_Hand_J5;
HJA.J6 := GVL.LR_Hand_J6;
//正解，更新各关节的坐标
GVL.CurrentJointPos := DirectKinematics(HJointAg:= HJA);
3.3.4运动主程序
//作者：AlongWu
PROGRAM Motion_PRG
VAR
i :INT;
Act_R_Trig :R_TRIG; //Act上升沿
END_VAR
Act_R_Trig(CLK:=GVL.B_MotionAct , Q=> ); //Act上升沿
IF Act_R_Trig.Q THEN
//Act 上升沿，复位步骤和状态值
GVL.I_PointM_Step:=0;
GVL.I_LineM_Step:=0;
GVL.I_BallM_Step:=0;
GVL.B_InverseKineFalure :=FALSE;
END_IF
IF GVL.B_MotionAct THEN
CASE GVL.MotionType OF
MotionType_Enum.Point:
//启用 PointMotion的PRG
PointMotion_PRG();
MotionType_Enum.Line:
//启用 LineMotion的PRG
LineMotion_PRG();
MotionType_Enum.Ball:
//启用 BallMotion的PRG
BallMotion_PRG();
ELSE
//motion结束
GVL.B_MotionAct :=FALSE;
END_CASE
END_IF3.3.4.1直线子运动程序
//作者：AlongWu
PROGRAM LineMotion_PRG
VAR
tmpPs :PosStru; //目标坐标数据元
tmpHandJR :HandJointResult; //逆解结果缓存
HandJR :HandJointResult; //逆解结果
LR_MoveLen :LREAL; //机械手末端移动距离 mm
LR_MovingTime :LREAL; //运动时长 s
tmp1 :LREAL;
tmp2 :LREAL;
tmp3 :LREAL;
tmp4 :LREAL;
ALen :LREAL; //加速距离长 mm
ATm :LREAL; //加速时长 s
SpdLowPos :LREAL; //减速位置 mm
Xg :LREAL;
Yg :LREAL;
Zg :LREAL;
Xk :LREAL;
Yk :LREAL;
Zk :LREAL;
Xb :LREAL;
Yb :LREAL;
Zb :LREAL;
//6关节的 一元一次方程的 b 和 k。
j1b :LREAL;
j1k :LREAL;
j2b :LREAL;
j2k :LREAL;
j3b :LREAL;
j3k :LREAL;
j4b :LREAL;
j4k :LREAL;
j5b :LREAL;
j5k :LREAL;
j6b :LREAL;
j6k :LREAL;
slen :LREAL;
v :LREAL;
t :LREAL;
END_VAR
CASE GVL.I_LineM_Step OF
0:
//坐标赋值
tmpPs.px :=GVL.LR_TargetPX;
tmpPs.py :=GVL.LR_TargetPY;
tmpPs.pz :=GVL.LR_TargetPZ;
tmpPs.rx :=GVL.LR_TargetRX;
tmpPs.ry :=GVL.LR_TargetRY;
tmpPs.rz :=GVL.LR_TargetRZ;
GVL.I_LineM_Step := GVL.I_LineM_Step +1;
1:
//检查目标坐标是否有解
//逆解
tmpHandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
IF tmpHandJR.IsNull THEN
GVL.B_InverseKineFalure :=TRUE;
GVL.I_LineM_Step := 99;
RETURN;
ELSE
GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
END_IF
2:
//把角度摆好，再直线插补
//坐标赋值
tmpPs.px :=GVL.CurrentJointPos.J6.px;
tmpPs.py :=GVL.CurrentJointPos.J6.py;
tmpPs.pz :=GVL.CurrentJointPos.J6.pz;
tmpPs.rx :=GVL.LR_TargetRX;
tmpPs.ry :=GVL.LR_TargetRY;
tmpPs.rz :=GVL.LR_TargetRZ;
GVL.I_LineM_Step := GVL.I_LineM_Step +1;
3:
//逆解
HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
IF HandJR.IsNull THEN
GVL.B_InverseKineFalure :=TRUE;
GVL.I_LineM_Step := 99;
ELSE
GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
END_IF
4:
//按固定时长，机械手末端先移动至目标角度。
LR_MovingTime := 5;
j1b := GVL.LR_Hand_J1;
j2b := GVL.LR_Hand_J2;
j3b := GVL.LR_Hand_J3;
j4b := GVL.LR_Hand_J4;
j5b := GVL.LR_Hand_J5;
j6b := GVL.LR_Hand_J6;
j1k := HandJR.HandJoint.J1Gap / LR_MovingTime;
j2k := HandJR.HandJoint.J2Gap / LR_MovingTime;
j3k := HandJR.HandJoint.J3Gap / LR_MovingTime;
j4k := HandJR.HandJoint.J4Gap / LR_MovingTime;
j5k := HandJR.HandJoint.J5Gap / LR_MovingTime;
j6k := HandJR.HandJoint.J6Gap / LR_MovingTime;
t :=0;
GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
5:
GVL.LR_Hand_J1 := j1b + j1k * t;
GVL.LR_Hand_J2 := j2b + j2k * t;
GVL.LR_Hand_J3 := j3b + j3k * t;
GVL.LR_Hand_J4 := j4b + j4k * t;
GVL.LR_Hand_J5 := j5b + j5k * t;
GVL.LR_Hand_J6 := j6b + j6k * t;
t := t+ 0.05;
IF t > LR_MovingTime THEN
GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
END_IF
6:
GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
GVL.I_LineM_Step :=10;
10:
IF GVL.LR_MovingSpd = 0 OR GVL.LR_MovingAsp=0 THEN
GVL.I_LineM_Step := 99;
RETURN;
END_IF
//计算当前坐标到目标坐标的距离和时长
tmp1 :=GVL.LR_TargetPX - GVL.CurrentJointPos.J6.px;
tmp2 :=GVL.LR_TargetPY - GVL.CurrentJointPos.J6.py;
tmp3 :=GVL.LR_TargetPZ - GVL.CurrentJointPos.J6.pz;
LR_MoveLen := SQRT(tmp1*tmp1+tmp2*tmp2+tmp3*tmp3);
ATm := GVL.LR_MovingSpd / GVL.LR_MovingAsp;
ALen := GVL.LR_MovingAsp * ATm *ATm;
IF ALen > LR_MoveLen THEN
//未达到目标数独，已开始减速
LR_MovingTime := SQRT(LR_MoveLen/GVL.LR_MovingAsp);
SpdLowPos := LR_MoveLen * 0.5;
ELSE
LR_MovingTime :=ATm*2 + (LR_MoveLen-ALen)/GVL.LR_MovingSpd;
SpdLowPos := LR_MoveLen - ALen*0.5;
END_IF
IF LR_MovingTime = 0 THEN
GVL.I_LineM_Step := 99;
RETURN;
END_IF
//计算位移插补的一元一次方程参数， k 和 b。
Xg := GVL.LR_TargetPX -GVL.CurrentJointPos.J6.px;
Yg := GVL.LR_TargetPY -GVL.CurrentJointPos.J6.py;
Zg := GVL.LR_TargetPZ -GVL.CurrentJointPos.J6.pz;
Xk := Xg / LR_MoveLen;
Yk := Yg / LR_MoveLen;
Zk := Zg / LR_MoveLen;
Xb := GVL.CurrentJointPos.J6.px;
Yb := GVL.CurrentJointPos.J6.py;
Zb := GVL.CurrentJointPos.J6.pz;
slen :=0;
v :=0;
GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
11:
IF slen < LR_MoveLen THEN
IF slen < SpdLowPos THEN
IF v < GVL.LR_MovingSpd THEN
//加速阶段
v := v + GVL.LR_MovingAsp*0.05;
slen := slen + v*0.05;
ELSE
//匀速阶段
slen := slen + v*0.05;
END_IF
ELSE
//减速阶段
IF v<0.1 THEN
v :=0.1;
ELSE
v := v - GVL.LR_MovingAsp*0.05;
END_IF
slen := slen + v*0.05;
END_IF
tmpPs.px :=Xb + Xk*slen;
tmpPs.py :=Yb + Yk*slen;
tmpPs.pz :=Zb + Zk*slen;
//逆解
HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
IF HandJR.IsNull THEN
GVL.B_InverseKineFalure :=TRUE;
GVL.I_LineM_Step := 99;
RETURN;
END_IF
GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
ELSE
GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
END_IF
12:
GVL.LR_Hand_J1:=tmpHandJR.HandJoint.J1;
GVL.LR_Hand_J2:=tmpHandJR.HandJoint.J2;
GVL.LR_Hand_J3:=tmpHandJR.HandJoint.J3;
GVL.LR_Hand_J4:=tmpHandJR.HandJoint.J4;
GVL.LR_Hand_J5:=tmpHandJR.HandJoint.J5;
GVL.LR_Hand_J6:=tmpHandJR.HandJoint.J6;
GVL.I_LineM_Step :=GVL.I_LineM_Step + 1;
ELSE
GVL.MotionType := 3;
END_CASE
3.3.4.2阿基米德旋线+球面子运动程序
//作者：AlongWu
PROGRAM BallMotion_PRG
VAR
tmpPs :PosStru; //目标坐标数据元
tmpHandJR :HandJointResult; //逆解结果缓存
HandJR :HandJointResult; //逆解结果
LR_MoveLen :LREAL; //机械手末端移动距离 mm
LR_MovingTime :LREAL; //运动时长 s
BallR :LREAL;
BallCx :LREAL;
BallCy :LREAL;
BallCz :LREAL;
//6关节的 一元一次方程的 b 和 k。
j1b :LREAL;
j1k :LREAL;
j2b :LREAL;
j2k :LREAL;
j3b :LREAL;
j3k :LREAL;
j4b :LREAL;
j4k :LREAL;
j5b :LREAL;
j5k :LREAL;
j6b :LREAL;
j6k :LREAL;
t :LREAL;
Arch_B :LREAL; //螺旋线间距系数
tmpR :LREAL; //当前直径长度 mm
ag :LREAL; //旋线的极角度
Z :LREAL; //当前Z位置 mm
v3d :Vector3d; //3维向量
eg :EulerAngleStru; //欧拉角组
END_VAR
(*
球的方程
x = x0+R*sinA*cosB
y = y0+R*sinA*sinB
z = z0+R*cosA
阿基米德螺线方程
r = a+bB;
螺线距离 = 2* PI * b;
本动作的目标是 用阿基米德螺线包住球。 同时 每个点的J6的z方向 指向 球心。
测试过程，运动过程，会出现 多个 非连续姿态(下一个姿态的1个或多个关节，的跨度过大。)
测试用例：R=20, CX=100.CY=-200,CZ=100
*)
CASE GVL.I_BallM_Step OF
0:
//参数赋值
BallR :=GVL.LR_Ball_R;
BallCx :=GVL.LR_Ball_Cx;
BallCy :=GVL.LR_Ball_Cy;
BallCz :=GVL.LR_Ball_Cz;
GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
1:
//移动至球顶部，并调整姿态
//坐标赋值
tmpPs.px :=BallCx;
tmpPs.py :=BallCy;
tmpPs.pz :=BallCz + BallR;
tmpPs.rx :=0;
tmpPs.ry :=-180;
tmpPs.rz :=-180;
GVL.I_BallM_Step := GVL.I_BallM_Step +1;
2:
//逆解
HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=0 );
IF HandJR.IsNull THEN
GVL.B_InverseKineFalure :=TRUE;
GVL.I_BallM_Step := 99;
RETURN;
ELSE
GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
END_IF
3:
//按固定时长，机械手末端先移动至目标角度。
LR_MovingTime := 5;
j1b := GVL.LR_Hand_J1;
j2b := GVL.LR_Hand_J2;
j3b := GVL.LR_Hand_J3;
j4b := GVL.LR_Hand_J4;
j5b := GVL.LR_Hand_J5;
j6b := GVL.LR_Hand_J6;
j1k := HandJR.HandJoint.J1Gap / LR_MovingTime;
j2k := HandJR.HandJoint.J2Gap / LR_MovingTime;
j3k := HandJR.HandJoint.J3Gap / LR_MovingTime;
j4k := HandJR.HandJoint.J4Gap / LR_MovingTime;
j5k := HandJR.HandJoint.J5Gap / LR_MovingTime;
j6k := HandJR.HandJoint.J6Gap / LR_MovingTime;
t :=0;
GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
4:
GVL.LR_Hand_J1 := j1b + j1k * t;
GVL.LR_Hand_J2 := j2b + j2k * t;
GVL.LR_Hand_J3 := j3b + j3k * t;
GVL.LR_Hand_J4 := j4b + j4k * t;
GVL.LR_Hand_J5 := j5b + j5k * t;
GVL.LR_Hand_J6 := j6b + j6k * t;
t := t+ 0.05;
IF t > LR_MovingTime THEN
GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
END_IF
5:
GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
GVL.I_BallM_Step :=10;
10:
Arch_B :=(BallR / 360) /3; //螺旋线间距系数，3圈直径为目标直径
tmpR :=0;
ag :=0.1;
GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
11:
IF tmpR < BallR THEN
// 计算旋线的半径
tmpR := Arch_B * ag;
// 计算当前球面的坐标
tmpPs.px :=BallCx + tmpR*CosAg(ag);
tmpPs.py :=BallCy + tmpR*SinAg(ag);
Z := BallCz + BallR - tmpR;
tmpPs.pz := Z;
//计算当前的机械手末端指向的向量， 从球面指向球心
v3d.px := BallCx - tmpPs.px;
v3d.py := BallCy - tmpPs.py;
v3d.pz := BallCz - tmpPs.pz;
//计算当前的机械手末端指向的欧拉角
eg := v3d2Euler(v3d);
tmpPs.rx := eg.rx;
tmpPs.ry := eg.ry;
tmpPs.rz := eg.rz;
//逆解各关节角度
HandJR :=InverseKinematics(KeyPosStru:=tmpPs , SortType:=1 );
IF HandJR.IsNull THEN
GVL.B_InverseKineFalure :=TRUE;
GVL.I_BallM_Step := 99;
RETURN;
END_IF
GVL.LR_Hand_J1:=HandJR.HandJoint.J1;
GVL.LR_Hand_J2:=HandJR.HandJoint.J2;
GVL.LR_Hand_J3:=HandJR.HandJoint.J3;
GVL.LR_Hand_J4:=HandJR.HandJoint.J4;
GVL.LR_Hand_J5:=HandJR.HandJoint.J5;
GVL.LR_Hand_J6:=HandJR.HandJoint.J6;
//插补,增加角度
ag :=ag + 0.3;
ELSE
GVL.I_BallM_Step :=GVL.I_BallM_Step + 1;
END_IF
ELSE
GVL.MotionType := 3;
END_CASE三、总结
整个程序比较简单，没有用到特殊的函数或者功能块，只用了四则运算来裸写相关的计算。轴控制就不贴代码了，也没啥特点。只调用了MC_Power和SMC_ControlAxisByPos。由于机械手关节的电机控制是位置控制模式，只要把相应位置数值给到fSetPosition，轴即会根据速度参数自己转到位。
