xyz-AC 五轴正反解推导过程
最编程
2024-03-06 21:35:28
...
xyz-AC 五轴正反解推导过程
1.前提条件
C:固结在C轴上的坐标系
A:固结在A轴上的坐标系
R:基坐标系
t:刀具末端坐标系
*A、C原点重合,原点在A\C轴轴线交点
*R在转台上,初始时与A\C同向,原点与AC原点有个z向偏移
*刀具坐标系和R同向
2.运动链
C-A-R
t-R
第一步:
求 RTc = RTa * aTc
第二步:
求 cTR,利用位姿矩阵求逆公式
第三步:
求 cTt = cTR * RTt
3.推导过程
4.代码
void CNC::MulMATRIX4(MATRIX4 T1, MATRIX4 T2, MATRIX4 *Td)
{
Td->item11=T1.item11*T2.item11+T1.item12*T2.item21+T1.item13*T2.item31+T1.item14*T2.item41;
Td->item12=T1.item11*T2.item12+T1.item12*T2.item22+T1.item13*T2.item32+T1.item14*T2.item42;
Td->item13=T1.item11*T2.item13+T1.item12*T2.item23+T1.item13*T2.item33+T1.item14*T2.item43;
Td->item14=T1.item11*T2.item14+T1.item12*T2.item24+T1.item13*T2.item34+T1.item14*T2.item44;
Td->item21=T1.item21*T2.item11+T1.item22*T2.item21+T1.item23*T2.item31+T1.item24*T2.item41;
Td->item22=T1.item21*T2.item12+T1.item22*T2.item22+T1.item23*T2.item32+T1.item24*T2.item42;
Td->item23=T1.item21*T2.item13+T1.item22*T2.item23+T1.item23*T2.item33+T1.item24*T2.item43;
Td->item24=T1.item21*T2.item14+T1.item22*T2.item24+T1.item23*T2.item34+T1.item24*T2.item44;
Td->item31=T1.item31*T2.item11+T1.item32*T2.item21+T1.item33*T2.item31+T1.item34*T2.item41;
Td->item32=T1.item31*T2.item12+T1.item32*T2.item22+T1.item33*T2.item32+T1.item34*T2.item42;
Td->item33=T1.item31*T2.item13+T1.item32*T2.item23+T1.item33*T2.item33+T1.item34*T2.item43;
Td->item34=T1.item31*T2.item14+T1.item32*T2.item24+T1.item33*T2.item34+T1.item34*T2.item44;
Td->item41=T1.item41*T2.item11+T1.item42*T2.item21+T1.item43*T2.item31+T1.item44*T2.item41;
Td->item42=T1.item41*T2.item12+T1.item42*T2.item22+T1.item43*T2.item32+T1.item44*T2.item42;
Td->item43=T1.item41*T2.item13+T1.item42*T2.item23+T1.item43*T2.item33+T1.item44*T2.item43;
Td->item44=T1.item41*T2.item14+T1.item42*T2.item24+T1.item43*T2.item34+T1.item44*T2.item44;
}
int CNC::XYZ_AC_Forward(AXISvalve theta, XYZ_AC_PARAM param , MATRIX4 &result)
{//只针对ac转台
double x=theta.x,y=theta.y,z=theta.z,a=theta.a,c=theta.c;
double d1 = param.lz;
//rTc = cTr-1 即参考坐标系倒置等于转换矩阵的逆
MATRIX4 RTa={0};
RTa.item11 = 1;
RTa.item14 = 0;
RTa.item24 = 0;
RTa.item34 = d1;
RTa.item44 = 1;
RTa.item22 = cos(a);
RTa.item23 = -sin(a);
RTa.item32 = sin(a);
RTa.item33 = cos(a);
MATRIX4 aTc = {0};
aTc.item33 = 1;
aTc.item44 = 1;
aTc.item11 = cos(c);
aTc.item12 = -sin(c);
aTc.item21 = sin(c);
aTc.item22 = cos(c);
MATRIX4 RTc = {0};
MulMATRIX4(RTa, aTc, &RTc);
MATRIX4 cTR = {0};
InvMATRIX4(RTc, &cTR);//cTR为R到c的转化
MATRIX4 RTt = {0};
RTt.item11 = 1;
RTt.item22 = 1;
RTt.item33 = 1;
RTt.item44 = 1;
RTt.item14 = x;
RTt.item24 = y;
RTt.item34 = z;
MATRIX4 cTt = {0};
MulMATRIX4(cTR,RTt,&cTt);
printf("\n");
MATRIX4 cTt_my = {0};//为了验证,算了两次,真正计算,系需要算cTt_my即可
cTt_my.item11 = cos(c);
cTt_my.item12 = cos(a)*sin(c);
cTt_my.item13 = sin(a)*sin(c);
cTt_my.item21 = -sin(c);
cTt_my.item22 = cos(a)*cos(c);
cTt_my.item23 = sin(a)*cos(c);
cTt_my.item32 = -sin(a);
cTt_my.item33 = cos(a);
cTt_my.item14 = x*cos(c)+y*cos(a)*sin(c)+(z-d1)*sin(a)*sin(c);
cTt_my.item24 = -x*sin(c)+y*cos(a)*cos(c)+(z-d1)*sin(a)*cos(c);
cTt_my.item34 = -y*sin(a)+(z-d1)*cos(a);
result = cTt_my;
return 0;
}
int CNC::XYZ_AC_Inverse(MATRIX4 result, XYZ_AC_PARAM param , AXISvalve &theta){
//a 正负90度
double d1 = param.lz;
double cosa = result.item33;
double sina = -result.item32;
double cosc = result.item11;
double sinc = -result.item21;
double a,c;
if(sina < -1){
a = -M_PI / 2;
}else if(sina > 1){
a = M_PI / 2;
}else{
a = asin(sina);
}
c = acos(cosc);
if(sinc >= 0){
c = c;
}else{
c = -c;
}
//计算x y z
MATRIX4 RTa={0};
RTa.item11 = 1;
RTa.item14 = 0;
RTa.item24 = 0;
RTa.item34 = d1;
RTa.item44 = 1;
RTa.item22 = cos(a);
RTa.item23 = -sin(a);
RTa.item32 = sin(a);
RTa.item33 = cos(a);
MATRIX4 aTc = {0};
aTc.item33 = 1;
aTc.item44 = 1;
aTc.item11 = cos(c);
aTc.item12 = -sin(c);
aTc.item21 = sin(c);
aTc.item22 = cos(c);
MATRIX4 RTc = {0};
MulMATRIX4(RTa, aTc, &RTc);
MATRIX4 RTt = {0};
MulMATRIX4(RTc, result, &RTt);
theta.a =a;
theta.c = c;
theta.x = param.lx + RTt.item14;
theta.y = param.ly + RTt.item24;
theta.z = param.lz + RTt.item34;
return 0;
}
5.位姿矩阵求逆
注意!
a,b,c,d,e,f,g,h,i用的是RMmat中对应的数值,不要用RMmat-1中计算的值,切记!
推荐阅读