3轴舵机控制机械臂,成品的样子,用个夹子给夹在桌子边,就成了一个伴随工作的辅助工具。
最初的目标是做个5轴驱动的机械臂,是这个样子的:
但是我买的的舵机是20KG的,头太重,举起来很抖,很不稳定,然后就退而求其次,改为3舵机结构。
在这里分享个经验,玩金属多轴机械臂,50块钱以下的舵机就不要买了,真是一分钱一分货,可以买个30多块钱的做为机械夹的控制部分。
刚装好6轴结构,真的很酷,很重很有机械感,像头金属兽。但是驱动起来需要很大的动力,一个稍微好点的40KG舵机很贵,我一直想用最低成本做个耐用好用的机械臂,我尝试了多款舵机,低性能的舵机不仅控制精度不行,而且很容易坏,炸一次坏一个。
舵机尸体
所以就别浪费钱了,我最开始买的松甲舵机一直随我用到最后,炸过好几次,虽然轴已经歪了,但是还能用,不是做广告,良心推荐。
安装过程,耐心、细致点,然后一些必要的工具,尖嘴钳,镊子,螺丝刀。
前面已经写了必要的代码,方便调试,在这个基础上,继续添加所需的功能,自己写代码就是好,可以随便改。
再写代码之前先复习一下高中的数学知识,勾股定理:
正弦函数 sin a/c ∠A的对边比斜边
余弦函数 cos b/c ∠A的邻边比斜边
正切函数 tan a/b ∠A的对边比邻边
余切函数 cot b/a ∠A的邻边比对边
正割函数 sec c/b ∠A的斜边比邻边
余割函数 csc c/a ∠A的斜边比对边
三角函数最重要的作用是能通过角度算出边长,通过边长算出角度。
两条机械臂构成一个三角形,我们已知2条机械臂的长度,a = 后段机械臂的长度,b = 前段机械臂的长度,我们要到达的D点,设定D点相对于舵机的坐标dx,dy。
通过D点的坐标求出c边的长度,然后知道了c的长度和a、b的长度就能求出,A、B、C的角度。
我在这里列出一些公式算法:
// 已知直角三角形abc中 C=90度,斜边c、a, 求 A 角度
public int Solve_RT_AC_Angel(int al, int cl)
{
double l1 = al;
double l2 = cl;
double rad = Math.Sin(l1 / l2);
double ang = rad * (180 / pai);
int A = (int)Math.Round(ang);
return A;
}
// 已知直角三角形abc中 C=90度,斜边c、b, 求 A 角度
public int Solve_RT_BC_Angel(int bl, int cl)
{
double l1 = bl;
double l2 = cl;
double rad = Math.Cos(l1 / l2);
double ang = rad * (180 / pai);
int A = (int)Math.Round(ang);
return A;
}
// 已知直角三角形abc中 C=90度,斜边c、b, 求 A 角度
public int Solve_RT_AB_Angel(int al, int bl)
{
double l1 = al;
double l2 = bl;
double rad = Math.Tan(l1 / l2);
double ang = rad * (180 / pai);
int A = (int)Math.Round(ang);
return A;
}
// 已知直角三角形abc中 C=90度,斜边c、A , 求 b 长度
public int Solve_RT_B_Side(int cl, int A)
{
double rad = pai / 180 * A;
double len = cl * Math.Cos(rad);
int bl = (int)Math.Round(len);
return bl;
}
// 已知直角三角形abc中 C=90度,斜边c、A , 求 a 长度
public int Solve_RT_A_Side(int cl, int A)
{
double rad = pai / 180 * A;
double len = cl * Math.Sin(rad);
int al = (int)Math.Round(len);
return al;
}
// 已知直角三角形 abc 已知 a、b、长度, C = 90 , 求 c 的长度
public int Solve_RT_C_Side(int al, int bl)
{
double l1 = al;
double l2 = bl;
double len = Math.Sqrt(l1 * l1 + l2 * l2);
int cl = (int)Math.Round(len);
return cl;
}
// 任意三角形 abc 已知 a、b、C , 求 c 的长度
public int Solve_C_Length(int al, int bl, int C)
{
double rad = pai / 180 * C;
double len = Math.Sqrt(al * al + bl * bl - 2 * al * bl * Math.Cos(rad));
int cl = (int)Math.Round(len);
return cl;
}
// 任意三角形 abc 已知 a、b、c 的长度, 求 A 角度
public int Solve_A_Angel(int al, int bl, int cl)
{
double l1 = al;
double l2 = bl;
double l3 = cl;
double cos_a = (l2 * l2 + l3 * l3 - l1 * l1) / (2 * l2 * l3);
double rad_a = Math.Acos(cos_a);
double ang_a = rad_a * (180 / pai);
int A = (int)Math.Round(ang_a);
return A;
}
// 任意三角形 abc 已知 a、b、c 的长度, 求 B 角度
public int Solve_B_Angel(int al, int bl, int cl)
{
double l1 = al;
double l2 = bl;
double l3 = cl;
double cos_b = (l1 * l1 + l3 * l3 - l2 * l2) / (2 * l1 * l3);
double rad_b = Math.Acos(cos_b);
double ang_b = rad_b * (180 / pai);
int B = (int)Math.Round(ang_b);
return B;
}
// 任意三角形 abc 已知 a、b、c 的长度, 求 C 角度
public int Solve_C_Angel(int al, int bl, int cl)
{
double l1 = al;
double l2 = bl;
double l3 = cl;
double cos_c = (l1 * l1 + l2 * l2 - l3 * l3) / (2 * l1 * l2);
double rad_c = Math.Acos(cos_c);
double ang_c = rad_c * (180 / pai);
int C = (int)Math.Round(ang_c);
return C;
}
知识有限,只能通过一步一步推导出结果,想到最近很火的姜萍,如果团队里有这样一个人她就能帮我们简化成一条简单的公式。
side_x = e.X;
side_y = e.Y;
int rt_al;
int rt_bl;
int o_x = 200;
int o_y = s_height - h_len;
rt_al = Math.Abs(side_y - o_y);
rt_bl = Math.Abs(side_x - o_x);
if(side_x < o_x)
{
Print_Msg("输入超出范围");
return;
}
c_len = solve.Solve_RT_C_Side(rt_al, rt_bl);
int rt_an;
int rt_bn;
int rt_cn;
rt_an = solve.Solve_A_Angel(rt_al, rt_bl, c_len);
rt_bn = solve.Solve_B_Angel(rt_al, rt_bl, c_len);
rt_cn = solve.Solve_C_Angel(rt_al, rt_bl, c_len);
ang_a = solve.Solve_A_Angel(a_len, b_len, c_len);
ang_b = solve.Solve_B_Angel(a_len, b_len, c_len);
ang_c = solve.Solve_C_Angel(a_len, b_len, c_len);
if (ang_a < 0 || ang_b < 0 || ang_c < 0)
{
Print_Msg("输入不满足长度要求");
return;
}
int an;
int bn = 0;
if (side_y < o_y)
{
an = rt_an + ang_b;
if (an < 90)
{
bn = 90 - an;
c_dot_y = solve.Solve_RT_B_Side(a_len, bn);
c_dot_x = solve.Solve_RT_A_Side(a_len, bn);
c_dot_x = o_x + c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = 90 + bn;
}
if (an > 90 && an < 180)
{
bn = 180 - an;
c_dot_x = solve.Solve_RT_B_Side(a_len, bn);
c_dot_y = solve.Solve_RT_A_Side(a_len, bn);
c_dot_x = o_x - c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = bn;
}
if(an > 180)
{
Print_Msg("超出范围");
return;
}
}
if (side_y > o_y)
{
an = rt_bn + ang_b;
if (an < 90)
{
Print_Msg("超出范围");
return;
}
if (an > 90 && an < 180)
{
bn = 180 - an;
c_dot_x = solve.Solve_RT_A_Side(a_len, bn);
c_dot_y = solve.Solve_RT_B_Side(a_len, bn);
c_dot_x = o_x + c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = 90 + bn;
}
if (an > 180)
{
bn = 270 - an;
c_dot_x = solve.Solve_RT_B_Side(a_len, bn);
c_dot_y = solve.Solve_RT_A_Side(a_len, bn);
c_dot_x = o_x - c_dot_x;
c_dot_y = o_y - c_dot_y;
ang_d = bn;
}
}
label_len_c.Text = "c边:" + c_len.ToString();
if (ang_a > 0)
{
label_ang_a.Text = "a角:" + ang_a.ToString();
}
else
{
label_ang_a.Text = "a角:超范围";
}
if (ang_b > 0)
{
label_ang_b.Text = "b角:" + ang_d.ToString();
}
else
{
label_ang_b.Text = "b角:超范围";
}
if (ang_c > 0)
{
label_ang_c.Text = "c角:" + ang_c.ToString();
}
else
{
label_ang_c.Text = "c角:超范围";
}
图像化:
演示视频
舵机操作演示
最后一点总结:
我现在的控制依然是一步一步进行,炸机真的炸怕了,再往后,可以把单片机的控制部分全部放到上位机上运行,减少hi3861处理负担,可以把多个角度数据一次发到舵机控制板,同时运行。
舵机的精度问题,一般好的舵机控制精度单位都是几个微秒,500us到2500us范围,PCA9685的控制寄存器只有12位,最大4096个数值,所以它注定很难实现很高的控制精度,要换一个好的舵机控制板也很贵,毕竟PCA9685才10几块钱。PCA9685跟精度不高的SG90小舵机搭配的挺好的,建议入手几个,一个只有几块钱,先在小舵机机上调试程序,程序通了再上大舵机,炸坏一个就没一个。舵机的数值角度跟实际的角度有误差,我加了一个补偿,进行实际的微调。
外国的5轴机械臂运行的那么好,是因为有好多舵机,好的算法,还有经验的积累。我们也可以慢慢实现,未来都不是问题,天下大事莫做于细,天下难事莫做于易,把大问题化成一个个简单的小问题,一个个解决。