4 Star 18 Fork 7

harryzhang / XY串联型机械臂算法实现

加入 Gitee
与超过 1200万 开发者一起发现、参与优秀开源项目,私有仓库也完全免费 :)
免费加入
克隆/下载
贡献代码
同步代码
取消
提示: 由于 Git 不支持空文件夾,创建文件夹后会生成空的 .keep 文件
Loading...
README

双联杆型画图XY机械臂

联系方式

Harryzhang 503433013@qq.com/harryzhangpro@gmail.com

硬件实现代码

传送链接

介绍

突然有了一个想法:
如果不用第三个电机,只用两个电机、两根硬联杆实现画正弦波、三角波和任意图形?(不用第三个电机也能做出来)
于是,有了以下思路:

  • 在两个电机上安装两个活动联杆,然后联杆之间插入一支笔,如图所示: 结构图片
    A点:放笔的地方
    L1、L2:硬质联杆
    绿色框框:A点的执行范围,笛卡尔坐标系,H、L点的Y已知,分别为H和0,这个是由你机器决定的。
    H:电机控制的滑块位置A(可以通过编码器得到) L:电机控制的滑块位置B(可以通过编码器得到) A:输入的坐标值 根据上图描述,我们要通过输入A点目标值(L1、L2已知,设定固定L1原点),解算出H、L的位置。

那么,通过这个思路,我只要输入A的值,然后解算出H、L的坐标,不就可以画正玄波、画三角波、画方波、画任意图形了吗!

机械臂正解思路

思路图片

正解算法设计

个人感觉挺麻烦的,请对着上图看代码。

static solution invx(solution solu2)
{	
	solution solu = solu2;
	float L1_U,L2_U;	

	if(solu.CACL_A > solu.CACL_B){
		L1_U = L1;
		L2_U = L2;
	}else{
		L1_U = L2;
		L2_U = L1;
	}

	float ax = solu.CACL_A-solu.CACL_B;

	float A = pow(H,2)+pow(L1_U,2)-pow(ax,2)-pow(L2_U,2);
	float B = 2*H*L1_U;
	float C = 2*ax*L2_U;

	float a = pow(B,2)*pow(L2_U,2)+pow(C,2)*pow(L1_U,2);
	float b = -2*L1_U*A*B*pow(L2_U,2)-2*H*pow(L1_U,2)*pow(C,2);
	float c = pow(A,2)*pow(L1_U,2)*pow(L2_U,2)-pow(C,2)*pow(L1_U,2)*pow(L2_U,2)+pow(C,2)*pow(L1_U,2)*pow(H,2);

	solution solu3 = caclduo(a,b,c);
	
	if(solu.CACL_A > solu.CACL_B){
		solu.solved.x = solu.CACL_B + sqrt(pow(L1,2)-pow(solu3.DeltaX,2));
		solu.solved.y = solu3.DeltaX;
	}else{
		solu.solved.x = solu.CACL_A + sqrt(pow(L2,2)-pow(solu3.DeltaX,2));
		solu.solved.y = H-solu3.DeltaX;
	}

	// cout << sqrt(pow(L2,2)-pow(solu.DeltaX,2)) << "," << sqrt(pow(L1,2)-pow(solu.DeltaX,2)) << "," << org_a << "," << org_b << "," << ax << endl;

	return solu;
}

机械臂逆解思路

思路图片

逆解算法设计

假设,我们第一步输入了A(x,y)值,这个值是不能直接带入公式计算的,因为L1固定到原点以后,A点的活动范围只有以L1为半径的半圆,A(x,y)不能保证直接落到半圆上,否则会无解。此时我们需要把它转换成思路图片的相对坐标系,也就是让这个xy落到这个半圆上,才能够进行逆解计算。那么怎么才能找到这个"差值"呢?,这里我们设L点为(X0,0),则向量LA=(x-X0,y);则该向量的模为L1。利用这个关系,我们可以列出一个包含X0/x/y/L1的一个二元一次方程,然后计算它的根,并取较小(靠近原点)那个。

实现代码:

static solution getx(float x,float y)   //计算
{
	solution solu = caclduo(1,-2*x,pow(x,2)+pow(y,2)-pow(L1,2));    //计算二元一次函数,并取较小的那个根

	if(solu.success)
	{
		x -= solu.DeltaX;   //在这一步我们得到了最小的那个根,也就是L(X0,0),然后把他转换到相对坐标轴上,把原点通过X轴平移到L点。
		float a1 = atan(y/x);   //请结合上图,理解下面的代码
		float sina2 = (H-L1*sin(a1))/L2;    //请结合上图,理解下面的代码 
		float cosa2 = sqrt(1-pow(sina2,2));      //请结合上图,理解下面的代码
		float ax = L2*cosa2;

	    // cout << "a1:" << a1 << "  sina1:" << sin(a1) << "  sina2:" << sina2 << "  cosa2:" << cosa2 << "  ax:" << ax <<"  DeltaX:" << solu.DeltaX << endl;

		solu.CACL_A = solu.DeltaX + (x-ax);     //解算完成,由于是在相对坐标系上计算了,需要把原点移回绝对坐标系
		solu.CACL_B = solu.DeltaX;
	}
	return solu;
}

//ax^2+bx+c=0,返回较小方程的根
static solution caclduo(float a,float b,float c)    //二元一次方程计算函数,输入a,b,c
{
	solution solu;
	float root = pow(b,2)-4*a*c;    //计算根
	solu.success = true;
	if(root>=0 || a!=0) //二次函数不存在
	{
		if(root == 0)   //二次函数只有一个解
		{
			solu.DeltaX = -b/2*a;   //返回解
			solu.success = true;
			goto end;
		}
		float x1 = (-b+sqrt(root))/(2*a);   //通过求根公式计算方程的解
		float x2 = (-b-sqrt(root))/(2*a);   

		// cout << "a:" << a << "  b:" << b << "  c:" << c << "  root:" << root << "  Dx1:" << x1 << "  Dx2:" << x2 << endl;

		if(x1>x2)
			solu.DeltaX = x2;          //由于安装方式,取最靠近原点的值作为增量值。
		else
			solu.DeltaX = x1;

	}
	else 
		solu.success = false;
	end:return solu;
}

算法编译与使用

输入坐标

...(200行左右)
#define Ax 218
#define Ay 32
...

根据你自己的机器改L1、L2、H参数

...(160行左右)
const float L1 = 110.0;
const float L2 = 110.0;
const float H = 110.0;
...

编译代码

g++ plot.c -lgdi32 -o plot.exe
./plot.exe

算法效果

按照上面的方法,输入A(218,32)并执行:

效果图1

按照上面的方法,输入A(218,97)并执行:

效果图2

效果图2

效果图2

结语

至此,我们实现了输入一个坐标,就能够自动计算出相对于的H、L的坐标值,通过加入直线插补、圆弧插补算法,然后通过编码器和电机去执行就可以实现只用俩个电机画图的功能。

算法纯原创!请点个赞亲们!

空文件

简介

只使用两个电机实现在纸上画任意图形 展开 收起
C++
取消

发行版

暂无发行版

贡献者

全部

近期动态

加载更多
不能加载更多了
C++
1
https://gitee.com/harryzhangabc/harry-xy-robot.git
git@gitee.com:harryzhangabc/harry-xy-robot.git
harryzhangabc
harry-xy-robot
XY串联型机械臂算法实现
master

搜索帮助