xyz-AC 五轴正反解推导过程

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中计算的值，切记！