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

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

【最短路径Floyd算法详解推导过程】看完这篇，你还能不懂Floyd算法？还不会？...
简介 Floyd-Warshall算法(Floyd-Warshall algorithm),是一种利用动态规划的思想寻找给定的加权图中多源点之间最短路径的算法,与Dijkstra算法类似.该算法名称以 ...
LSTM公式详解推导
书籍简介 LSTM理解 LSTM流程简介算法及公式一些函数一些符号前向传播反向传播关于误差的定义公式推导总结书籍简介 <Surpervised Sequence Labelli ...
六轴机械臂运动学算法原理及其推导过程
网站上关于六轴机械臂piper算法的讲解有很多,但其腕点姿态的推到较为模糊,故此写一篇关于六轴机械臂piper算法的推导讲解,供有缘人参考,如果您觉得有用,可以点个赞,吾将不胜感激,若是推导过程存在错 ...
卡尔曼滤波的推导过程详解
在学习卡尔曼滤波的时候看了很多博客讲这方面的知识,感觉都讲得表面的东西,无法了解它五个公式真正代表的过程,这篇博客我想以我的理解讲讲卡尔曼滤波. 首先我先写出卡尔曼滤波的具体过程,首先针对如下状态空间 ...
一分钟详解「本质矩阵」推导过程
文章目录前言一本质矩阵如何推导二本质矩阵的意义三本质矩阵的求解四扩展--基本矩阵前言两幅视图存在两个关系:第一种,通过对极几何,一幅图像上的点可以确定另外一幅图像上的一条直线:另 ...
三个数差的平方公式推导过程_平方差公式证明推导过程及运用详解（数学简便计算方法之一）...
平方差公式是小学奥数计算中的常用公式. 通常写为:a²-b²=(a+b)x(a-b) 它的几何方法推导过程是这样的: 如下图所示,四边形ABCD和四边形DEFG为正方形,边长分别为a和b,求阴影部分 ...
几何分布的期望和方差公式推导_平方差公式证明推导过程及运用详解（数学简便计算方法之一）...
平方差公式是小学奥数计算中的常用公式. 通常写为:a²-b²=(a+b)x(a-b) 它的几何方法推导过程是这样的: 如下图所示,四边形ABCD和四边形DEFG为正方形,边长分别为a和b,求阴影部分 ...
二阶低通滤波器IIR的五个参数推导过程
最近在研究飞控代码看到了二阶低通滤波器IIR的软件代码,由于之前没有了解过二阶滤波器的原理,对代码十分懵逼,经过学习,特将学习成果发布,有兴趣的小伙伴看看,如果哪里不对,请提出来,一起学习进步哟,下面 ...
Delta机器人：运动学正反解分析
Delta机器人:运动学正反解分析一.Delta机构简介 Delta机构是并联机构中的一种典型机构,起原始结构如图1所示.Delta机构由R.Clavel 博士在 1985年发明,是现在并联机器人中 ...

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

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

1.前提条件

2.运动链

3.推导过程

4.代码

5.位姿矩阵求逆

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

最新文章

热门文章