目录
一、概述
由于Codesys的Robotics模块有Delta并联机械手的运动模型，因此借助Codesys的机械手运动模型，研究自己的正解逆解算法。
二、结果截图
三、程序组织
四、关键点备注
1，Codesys模型中Delta并联机械手名称是 Tripod。
2，本程序用到了Codesys的多核授权，Task可以按需指定于多核CPU的某个核心进行运行。
3，程序的Delta正解算法基于几何法。
五、关键代码
Final_DirectTripod3A_Matrix
(*
#@Copyright:
#@License:
#@Birth: created by Along on 2023-05-25
#@Content: Final_DirectTripod3A_Matrix-正解Tripod机械手子程序FB
#@Version: 0.0.1
#@Revision: last revised by Along on 2023-05-25
#@brief 正解Tripod机械手子程序 ， Final不可继承
根据《赵杰,朱延河,蔡鹤皋.Delta型并联机器人运动学正解几何解法[J].哈尔滨工业大学学报,2003(01):25-27.》的方法进行计算。
即：
1,计算O->D1,O->D2,O->D3的向量
2,计算D1D2D3的外接圆心坐标E、半径
3,计算D1D2D3平面的法向量nEP
4,计算EP的模长，进而计算EP的向量
5,OP = OE + EP， P即为动平台中心坐标
*)
FUNCTION_BLOCK FINAL Final_DirectTripod3A_Matrix
VAR_INPUT
lr_j1 :LREAL; //Tripod机械手 A1轴角度
lr_j2 :LREAL; //Tripod机械手 A2轴角度
lr_j3 :LREAL; //Tripod机械手 A3轴角度
END_VAR
VAR_OUTPUT
lr_world_x :LREAL; //Tripod机械手 世界坐标 X
lr_world_y :LREAL; //Tripod机械手 世界坐标 Y
lr_world_z :LREAL; //Tripod机械手 世界坐标 Z
END_VAR
VAR
lr_j1_rap :LREAL; //轴1的弧度
lr_j2_rap :LREAL; //轴2的弧度
lr_j3_rap :LREAL; //轴3的弧度
v3d_od1 :Stru_V3D; //向量 O->D1
v3d_od2 :Stru_V3D; //向量 O->D2
v3d_od3 :Stru_V3D; //向量 O->D2
sci_c :Stru_3DCircleParam; //外接圆结构体
v3d_op :Stru_V3D; //向量 O->P
lr_ep :LREAL; //向量 E->P模长
v3d_ep :Stru_V3D; //向量 E->P
END_VAR
//角度转换弧度
lr_j1_rap := lr_j1 * glvars.const_lr_pi / 180;
lr_j2_rap := lr_j2 * glvars.const_lr_pi / 180;
lr_j3_rap := lr_j3 * glvars.const_lr_pi / 180;
//计算O->D1,O->D2,O->D3的向量
// Codesys的Tripod模型，轴0主动臂位于X轴负方向
// O->D1 向量
v3d_od1 := meth_ODi
(
lr_driven_r:= glvars.const_lr_driven_arm_r,
lr_passive_r:= glvars.const_lr_endplate_r,
lr_phi:= glvars.const_lr_pi, //180度的弧度
lr_theta:= lr_j1_rap,
lr_driven_armlen:= glvars.const_lr_driven_armlen,
lr_zerohei := glvars.const_lr_z_zerohei
);
// O->D2 向量
v3d_od2 := meth_ODi
(
lr_driven_r:= glvars.const_lr_driven_arm_r,
lr_passive_r:= glvars.const_lr_endplate_r,
lr_phi:= 1.047197551, //逆时针60度的弧度
lr_theta:= lr_j2_rap,
lr_driven_armlen:= glvars.const_lr_driven_armlen,
lr_zerohei := glvars.const_lr_z_zerohei
);
// O->D3 向量
v3d_od3 := meth_ODi
(
lr_driven_r:= glvars.const_lr_driven_arm_r,
lr_passive_r:= glvars.const_lr_endplate_r,
lr_phi:= -1.047197551, //逆时针-60度的弧度
lr_theta:= lr_j3_rap,
lr_driven_armlen:= glvars.const_lr_driven_armlen,
lr_zerohei := glvars.const_lr_z_zerohei
);
//计算外接圆信息
sci_c := meth_3D_CircleParam(v3d_a:= v3d_od1, v3d_b:= v3d_od2, v3d_c:= v3d_od3);
//根据外接圆半径和从动臂长，计算ep长度
lr_ep := SQRT(glvars.const_lr_passive_armlen * glvars.const_lr_passive_armlen - sci_c.radius * sci_c.radius );
//向量EP
v3d_ep := meth_Matrix_Mul_Number(v3d_left:= sci_c.v3d_norm, lr_k:= lr_ep);
// OP = OE + EP
v3d_op := meth_Matrix_Add(v3d_left:= sci_c.v3d_center, v3d_right:= v3d_ep);
//输出P点坐标
lr_world_x := v3d_op.x;
lr_world_y := v3d_op.y;
lr_world_z := v3d_op.z;
meth_3D_CircleParam
(*
#@Copyright:
#@License:
#@Birth: created by Along on 2023-05-25
#@Content: meth_3D_CircleParam-计算三维坐标系下圆参数 method
#@Version: 0.0.1
#@Revision: last revised by Along on 2023-05-25
#@brief 计算三维坐标系下圆参数 method
圆参数 有 圆心坐标、半径、圆所在平面的单位法向量
*)
METHOD meth_3D_CircleParam : Stru_3DCircleParam
VAR_INPUT
v3d_a :Stru_V3D;
v3d_b :Stru_V3D;
v3d_c :Stru_V3D;
END_VAR
VAR
a1 :LREAL;
a2 :LREAL;
a3 :LREAL;
b1 :LREAL;
b2 :LREAL;
b3 :LREAL;
c1 :LREAL;
c2 :LREAL;
c3 :LREAL;
d1 :LREAL;
d2 :LREAL;
d3 :LREAL;
x1 :LREAL;
y1 :LREAL;
z1 :LREAL;
x2 :LREAL;
y2 :LREAL;
z2 :LREAL;
x3 :LREAL;
y3 :LREAL;
z3 :LREAL;
x :LREAL;
y :LREAL;
z :LREAL;
lr_n :LREAL;
v3d_n :Stru_V3D;
END_VAR
//将输入的三点坐标赋值到中间变量
x1 := v3d_a.x;
y1 := v3d_a.y;
z1 := v3d_a.z;
x2 := v3d_b.x;
y2 := v3d_b.y;
z2 := v3d_b.z;
x3 := v3d_c.x;
y3 := v3d_c.y;
z3 := v3d_c.z;
// 已知三维空间中三个点求圆心坐标和半径 https://www.cnblogs.com/kongbursi-2292702937/p/15190825.html
a1 := (y1*z2 - y2*z1 - y1*z3 + y3*z1 + y2*z3 - y3*z2);
b1 := -(x1*z2 - x2*z1 - x1*z3 + x3*z1 + x2*z3 - x3*z2);
c1 := (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2);
d1 := -(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
a2 := 2 * (x2 - x1);
b2 := 2 * (y2 - y1);
c2 := 2 * (z2 - z1);
d2 := x1*x1 + y1*y1 + z1*z1 - x2*x2 - y2*y2 - z2*z2;
a3 := 2 * (x3 - x1);
b3 := 2 * (y3 - y1);
c3 := 2 * (z3 - z1);
d3 := x1*x1 + y1*y1 + z1*z1 - x3*x3 - y3*y3 - z3*z3;
//圆心坐标
x := -(b1*c2*d3 - b1*c3*d2 - b2*c1*d3 + b2*c3*d1 + b3*c1*d2 - b3*c2*d1) / (a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
y := (a1*c2*d3 - a1*c3*d2 - a2*c1*d3 + a2*c3*d1 + a3*c1*d2 - a3*c2*d1) / (a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
z := -(a1*b2*d3 - a1*b3*d2 - a2*b1*d3 + a2*b3*d1 + a3*b1*d2 - a3*b2*d1) / (a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
meth_3D_CircleParam.v3d_center.x := x;
meth_3D_CircleParam.v3d_center.y := y;
meth_3D_CircleParam.v3d_center.z := z;
//计算圆半径
meth_3D_CircleParam.radius := SQRT((x1 - x)*(x1 - x) + (y1 - y)*(y1 - y) + (z1 - z)*(z1 - z));
//根据3点2线，求法向量
v3d_n.x := ((y2 - y1)*(z3-z1) - (y3-y1)*(z2-z1));
v3d_n.y := ((z2 - z1)*(x3-x1) - (z3-z1)*(x2-x1));
v3d_n.z := ((x2 - x1)*(y3-y1) - (x3-x1)*(y2-y1));
//计算向量的模长
lr_n := meth_LineLen(v3d_line:= v3d_n);
//计算圆所在平面的单位法向量
meth_3D_CircleParam.v3d_norm := meth_Matrix_Div_Number(v3d_left:= v3d_n , lr_k:= lr_n);
Final_InverseTripod3A
(*
#@Copyright:
#@License:
#@Birth: created by Along on 2023-05-16
#@Content: Final_InverseTripod3A-Tripod机械手-三轴逆解函数fb
#@Version: 0.0.1
#@Revision: last revised by Along on 2023-05-16
#@brief Tripod机械手三轴逆解函数fb
#@requirement
*)
FUNCTION_BLOCK FINAL Final_InverseTripod3A
VAR_INPUT
lr_x_pos :LREAL; //Tripod机械手世界坐标 X
lr_y_pos :LREAL; //Tripod机械手世界坐标 Y
lr_z_pos :LREAL; //Tripod机械手世界坐标 Z
END_VAR
VAR_OUTPUT
lr_j1 :LREAL; //Tripod机械手 正解A1轴角度
lr_j2 :LREAL; //Tripod机械手 正解A2轴角度
lr_j3 :LREAL; //Tripod机械手 正解A3轴角度
END_VAR
VAR
fb_get_csr2d :getCoordinateSystemRotation2D; //2D坐标系转换函数fb
lr_tmp_x :LREAL; //中间变量-x
lr_tmp_y :LREAL; //中间变量-y
END_VAR
meth_InverseOneHand
(*
#@Copyright:
#@License:
#@Birth: created by Along on 2023-05-16
#@Content: meth_InverseOneHand-Tripod机械手-单轴逆解函数 method
#@Version: 0.0.1
#@Revision: last revised by Along on 2023-05-16
#@brief Tripod机械手-单轴逆解函数 method
#@requirement
*)
METHOD meth_InverseOneHand : LREAL
VAR_INPUT
lr_x_pos :LREAL; //以主动臂坐标系的，单轴 x坐标
lr_y_pos :LREAL; //以主动臂坐标系的，单轴 y坐标
lr_z_pos :LREAL; //以主动臂坐标系的，单轴 z坐标
END_VAR
VAR
lr_z :LREAL; //换算后z坐标
lr_x :LREAL; //换算后x坐标
l2_2 :LREAL; //从动臂投影长度的平方
f_2 :LREAL; //终端点与主动臂上连接点的距离的 f 的平方
f :LREAL; //终端点与主动臂上连接点的距离的 f
rag1 :LREAL; //弧度1
rag2 :LREAL; //弧度2
ag1 :LREAL; //角度1
ag2 :LREAL; //角度2
ag3 :LREAL; //角度3
tmp1 :LREAL; //浮点1
tmp2 :LREAL; //浮点2
tmp3 :LREAL; //浮点3
END_VAR
//转换X坐标和Z坐标
lr_x := -lr_x_pos ;
lr_z := glvars.const_lr_z_zerohei - lr_z_pos;
//从动臂投影长度l2, l2_2 = l2的平方
l2_2 := glvars.const_lr_passive_armlen*glvars.const_lr_passive_armlen - lr_y_pos*lr_y_pos;
//终端点与主动臂上连接点的距离的 f, f_2 = f的平方。
f_2 := (lr_x - glvars.const_lr_driven_passive_rgap)*(lr_x - glvars.const_lr_driven_passive_rgap) + lr_z*lr_z;
f := SQRT(f_2);
//根据余弦定理，求出 ∠终端-主动臂上连接点-主动臂下连接点 的弧度 rag1 , 再转为角度 ag1
tmp1 := f_2 + glvars.const_lr_driven_armlen*glvars.const_lr_driven_armlen - l2_2;
tmp2 := tmp1 / (2 * f*glvars.const_lr_driven_armlen) ;
rag1 := ACOS(tmp2);
ag1 := rag1 * 180 / glvars.const_lr_pi;
(*
1,∠ 主动臂平台圆心点-主动臂上连接点-终端点 是 (主动臂摆角+ ∠终端-主动臂上连接点-主动臂下连接点) 合角 的 补角。
2, (glvars.const_r_driven_passive_rgap - lr_x) 与 f 和 z形成直角三角形。
*)
tmp3 := (glvars.const_lr_driven_passive_rgap - lr_x) / f;
tmp3 := - tmp3;
rag2 := ACOS(tmp3);
ag2 := rag2 * 180 / glvars.const_lr_pi;
//主动臂摆角
ag3 := ag2 - ag1;
//电机方向与计算方向相反
meth_InverseOneHand := -ag3;
以上。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
