codesys 6轴机器人正解程序

——————————————————————————————————————

标准DH方法

——————————————————————————————————————

FUNCTION FUN_standard_DH_method : ARRAY[0..3,0..3] OF LREAL
VAR_INPUT
	theta:LREAL;
	d:LREAL;
	a:LREAL;
	alpha:LREAL;
END_VAR
VAR
END_VAR

FUN_standard_DH_method[0,0]:=COS(theta);
FUN_standard_DH_method[0,1]:=-SIN(theta) * COS(alpha);
FUN_standard_DH_method[0,2]:=SIN(theta) * SIN(alpha);
FUN_standard_DH_method[0,3]:=a * COS(theta);
 
FUN_standard_DH_method[1,0]:=SIN(theta);
FUN_standard_DH_method[1,1]:=COS(theta) * COS(alpha);
FUN_standard_DH_method[1,2]:=-COS(theta) * SIN(alpha);
FUN_standard_DH_method[1,3]:=a * SIN(theta);
 
FUN_standard_DH_method[2,0]:=0;
FUN_standard_DH_method[2,1]:=SIN(alpha);
FUN_standard_DH_method[2,2]:=COS(alpha);
FUN_standard_DH_method[2,3]:=d;
 
FUN_standard_DH_method[3,0]:=0;
FUN_standard_DH_method[3,1]:=0;
FUN_standard_DH_method[3,2]:=0;
FUN_standard_DH_method[3,3]:=1;

——————————————————————————————————————

矩阵相乘

——————————————————————————————————————

FUNCTION FUN_matrix_44_MUL : ARRAY[0..3,0..3] OF LREAL
VAR_INPUT
	matrix1:ARRAY[0..3,0..3] OF LREAL;
	matrix2:ARRAY[0..3,0..3] OF LREAL;
END_VAR
VAR
	i:INT;
	j:INT;
END_VAR

FOR i:=0 TO 3 BY 1 DO
	FOR j:=0 TO 3 BY 1 DO
		FUN_matrix_44_MUL[i,j]:=matrix1[i,0] * matrix2[0,j] +
								matrix1[i,1] * matrix2[1,j] +
								matrix1[i,2] * matrix2[2,j] +
								matrix1[i,3] * matrix2[3,j];
		
	END_FOR
END_FOR

——————————————————————————————————————

旋转矩阵变换RPY角

——————————————————————————————————————

FUNCTION FUN_matrix_33_to_RPY : BOOL
VAR_INPUT
	matrix:ARRAY[0..3,0..3] OF LREAL;
END_VAR
VAR_IN_OUT
	alpha:LREAL;
	bata:LREAL;
	gama:LREAL;
END_VAR
VAR
END_VAR

bata:=SM3M.SMC_atan2(SQRT(EXPT(matrix[2,0],2) + EXPT(matrix[2,1],2)), matrix[2,2]);
alpha:=SM3M.SMC_atan2(matrix[1,2] / SIN(bata), matrix[0,2] / SIN(bata));
gama:=SM3M.SMC_atan2(matrix[2,1] / SIN(bata), -matrix[2,0] / SIN(bata));

——————————————————————————————————————

程序

——————————————————————————————————————

VAR_GLOBAL
	theta:ARRAY[1..6] OF LREAL;
	d:ARRAY[1..6] OF LREAL;
	a:ARRAY[1..6] OF LREAL;
	alpha:ARRAY[1..6] OF LREAL;
	offset:ARRAY[1..6] OF LREAL;
	
	matrix:ARRAY [1..6] OF ARRAY [0..3,0..3] OF LREAL;
	endMatrix:ARRAY [0..3,0..3] OF LREAL;
END_VAR

PROGRAM PLC_PRG
VAR	
	i:INT;
	x:LREAL;
	y:LREAL;
	z:LREAL;
	rx:LREAL;
	ry:LREAL;
	rz:LREAL;
END_VAR

P_axisGroup();
 
theta[1]:=0+SM_Drive_Virtual_1.fActPosition/180*SM3M.SMC_PI;
theta[2]:=SM3M.SMC_PI/2+SM_Drive_Virtual_2.fActPosition/180*SM3M.SMC_PI;
theta[3]:=0+SM_Drive_Virtual_3.fActPosition/180*SM3M.SMC_PI;
theta[4]:=0+SM_Drive_Virtual_4.fActPosition/180*SM3M.SMC_PI;
theta[5]:=0+SM_Drive_Virtual_5.fActPosition/180*SM3M.SMC_PI;
theta[6]:=0+SM_Drive_Virtual_6.fActPosition/180*SM3M.SMC_PI;
 
alpha[1]:=SM3M.SMC_PI/2;
alpha[2]:=0;
alpha[3]:=SM3M.SMC_PI/2;
alpha[4]:=SM3M.SMC_PI/2;
alpha[5]:=-SM3M.SMC_PI/2;
alpha[6]:=0;
 
d[1]:=80;
d[2]:=0;
d[3]:=20;
d[4]:=70;
d[5]:=0;
d[6]:=10;
 
a[1]:=10;
a[2]:=100;
a[3]:=30;
a[4]:=0;
a[5]:=0;
a[6]:=0;
 
FOR i:=1 TO 6 BY 1 DO
	matrix[i]:=FUN_standard_DH_method(theta:=theta[i] ,d:=d[i] ,a:=a[i] ,alpha:=alpha[i]);
END_FOR
 
endMatrix:=matrix[1];
FOR i:=2 TO 6 BY 1 DO
	endMatrix:=FUN_matrix_44_MUL(endMatrix,matrix[i]);
END_FOR
 
x:=endMatrix[0,3];
y:=endMatrix[1,3];
z:=endMatrix[2,3];
 
FUN_matrix_33_to_RPY(matrix:=endMatrix , alpha:=rx , bata:=ry , gama:=rz );
rx:=rx/SM3M.SMC_PI*180;
ry:=ry/SM3M.SMC_PI*180;
rz:=rz/SM3M.SMC_PI*180;

——————————————————————————————————————

机器人轴组程序查看之前的文章

——————————————————————————————————————