|
|
本帖最后由 AimHigh 于 2018-9-14 10:25 编辑
教程一先教大家如何写出delta并联机器人的各个位置坐标程序， 上个学期实现了一个基于uno的delta的控制系统，作为arduino社区的支持者与爱好者，今天将作（wan)品（ju）其发到此处。
首先说一下我的整体方案设计见图一，它包括三个模块分别是舵机执行模块、电磁铁执行模块和串口通讯模块。其中舵机模块也是最重要的模块，需要三个舵机实现插补完成在空间走任意线段，定点搬运打孔等功能。
图1：整体方案图
[sf] 要实现上述功能，必须对Delta并联机器人进行机构位置分析。教程一先教大家如何求出位置坐标，因为我这里是舵机，功能要求只需要求出逆解，这需要一点数学基础。 先将Delta并联机器人机构做如图2所示的简化，并建立如图2所示的动、静两个坐标系，静坐标系O-XYZ的原点O位于静平台的几何中心，即等边三角形A1A2A3的几何中心，Z轴垂直于静平台，OX垂直于A1A3。动平台上建立O’-X’Y’Z’坐标系，O’为动平台的几何中心，Z’轴垂直于动平台，O’X’垂直于C1C3。其中AiBi=L，CiBi=l。设定静平台的外接圆半径为R ，即OAI=R；设定动平台外接圆半径为r ,即O’C’I=r。机构运动时主动臂与垂直方向之间的夹角为θi，其中i= 1,2,3。
Delta并联机器人通过三个分支链将上下平台连接起来,驱动臂在电机的驱动下作一定角度的反复摆动,再通过平行四边形闭环和转动副使动平台作平移运动，所以这里的位置逆解就是已知机器人动平台在静平台坐标系O-XYZ中的位置，求解主动臂AiBi与静平台平面之间的夹角（即（90°-θi）），然后建立
静平台结构图3， 具体计算过程见图4到7。
图2：delta简图
图3：静平台结构图
图4：逆解计算1
图5：逆解计算2
图6：逆解计算3
图7：逆解计算4
通过图7中的3-8式可知道，已知动平台坐标点时，每个ti会有两个解或者无解（不在工作空间内的点），于是主动臂与垂直方向的夹角θi也会有两个值。如果θi小的那个值时，主动臂会离静平台比较远；当θi取较大的那个值时，主动臂则会离静平台较近。由Delta的结构特点可以知道，主动臂与静平台之间的位置距离超出一定范围时可能会出现机构杆件之间的干涉，机构损坏，力学性能不稳定等问题。于是在选择θi的值时为了保证机构的良好工作性能，需要选取θi较大的那个值，于是可以得到 Delta机器人的位置逆解表达式，见图8。
图8：逆解
[/sf]
有了上面的基础就可以在arduino IDE中写上我们的代码如下： [sf]（这里要注意有三个舵机）
double nijie1(double R,double r,double L,double l,double x,double y,double z)
{
double K,M,N;
K=((-(x*x+y*y+z*z)+(y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L+2*z);
M=-2*(2*(R-r)-y-sqrt(3)*x);
N=((-(x*x+y*y+z*z)+(y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L-2*z);
double a=90-2*atan((-M-sqrt(M*M-4*K*N))/(2*K))*180/PI+30;
return(a);//返回角度
}
double nijie2(double R,double r,double L,double l,double x,double y,double z)
{
double K,M,N;
K=((-(x*x+y*y+z*z)-(-y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L+2*z);
M=-2*(2*(R-r)-y+sqrt(3)*x);
N=((-(x*x+y*y+z*z)-(-y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L-2*z);
double a=90-2*atan((-M-sqrt(M*M-4*K*N))/(2*K))*180/PI+30;
return(a);//返回角度
}
double nijie3(double R,double r,double L,double l,double x,double y,double z)
{
double K,M,N;
K=((-(x*x+y*y+z*z)-2*y*(R-r)-(R-r)*(R-r)-L*L+l*l)/(2*L)+z);
M=-2*(R-r+y);
N=((-(x*x+y*y+z*z)-2*y*(R-r)-(R-r)*(R-r)-L*L+l*l)/(2*L)-z);
double a=90-2*atan((-M-sqrt(M*M-4*K*N))/(2*K))*180/PI+30;
return(a);//返回角度
}[/sf]
|
|
发表于 2018-9-13 20:44
楼主
这里写错了