Delta Robot 正逆运动学推导与代码

Delta Robot Kinematics with code C++ Matlab

Forward Kinematics

c++

void robotForward(const double* q, double* TransVector, bool mconfig)
{

	double fi[3] = { 0,120,240 };
	double theta_tmp[6];
	for (int i = 0; i < 6; i++)
	{
		theta_tmp[i] = q[i];
		//cout << "theta：" << theta_tmp[i] << endl;
	}

	double  D[3][3] = { 0 };
	for (int i = 0; i < 3; i++)
	{
		D[i][0] = (R - r + L * cos_angle(theta_tmp[i])) * cos_angle(fi[i]);
		D[i][1] = (R - r + L * cos_angle(theta_tmp[i])) * sin_angle(fi[i]);
		D[i][2] = -L * sin_angle(theta_tmp[i]);

		//D[i][0] = (R - r - L * sin_angle(theta_tmp[i])) * cos_angle(fi[i]);
		//D[i][1] = (R - r - L * sin_angle(theta_tmp[i])) * sin_angle(fi[i]);
		//D[i][2] = -L*cos_angle(theta_tmp[i]);
	}


	double  a= PointDist(D[0],D[1]);
	double  b = PointDist(D[1], D[2]);
	double  c = PointDist(D[2], D[0]);
	
	double p = (a + b + c) / 2;
	double S = sqrt(p * (p-a) * (p-b) * (p-c));
	double FE_norm = (a * a + b * b - c * c) * c / (8 * S);
	double nFE;
	nFE = sqrt(pow(a * b * c / 4 / S, 2) - pow(b / 2, 2));
		double vD21[3], vD23[3], vD32[3];
		VectorSub(D[1], D[0], vD21);
		VectorSub(D[1], D[2], vD23);
		VectorSub(D[2], D[1], vD32);
		double  D21xD23[3];
		VectorCross(vD21, vD23, D21xD23);
		double  D21xD23xD32[3];
		VectorCross(D21xD23, vD32, D21xD23xD32);
		//double nFE_norm=VectorNorm(nFE);
		//VectorNumDivide(nFE, nFE_norm, nFE);

		double nD21xD23 = VectorNorm(D21xD23);
		double nD21xD23xD32 = VectorNorm(D21xD23xD32);

		double  R11 = a * b * c / 4 / S;

		for (int i = 0; i < 3; i++)
		{
			TransVector[(i+1)*4-1] = nFE * D21xD23xD32[i] / nD21xD23xD32 + sqrt(pow(l, 2) - pow(R11, 2)) * D21xD23[i] / nD21xD23 + (D[2-1][i] + D[3 - 1][i]) / 2;
		}

}

matlab

function [dis]=PointDist(p1, p2)
		dis = 0.0;
		for i=1:3
			dis = dis+(p1(i) - p2(i))^2;
        end
		dis=sqrt(dis);
end
function Position=forward_trans(theta_tmp)
        R  = 200;
        r  = 45;
        L = 350;
        l = 800;
        fi = [0,120,240];
        D=zeros(3,3);
        Position=zeros(1,3);
		for i=1:3
			D(i,1) = (R - r + L * cosd(theta_tmp(i))) * cosd(fi(i));
			D(i,2) = (R - r + L * cosd(theta_tmp(i))) * sind(fi(i));
			D(i,3) = -L * sind(theta_tmp(i));
        end
		a= PointDist(D(1,:),D(2,:));
		b = PointDist(D(2,:), D(3,:));
		c = PointDist(D(3,:), D(1,:));
		
		p = (a + b + c) / 2;
		S = sqrt(p * (p-a) * (p-b) * (p-c));
		FE_norm = (a * a + b * b - c * c) * c / (8 * S);
		nFE = sqrt((a * b * c / 4 / S)^2 - (b/2)^2);
		%vD21(3), vD23[3], vD32[3];
        vD21=D(2,:)-D(1,:);
        vD23=D(2,:)-D(3,:);
        vD32=D(3,:)-D(2,:);

         D21xD23=cross(vD21, vD23);
         D21xD23xD32=cross(D21xD23, vD32);
         
		 nD21xD23 = norm(D21xD23);
		 nD21xD23xD32 = norm(D21xD23xD32);

		 R11 = a * b * c / 4 / S;
		 for i=1:3
			 Position(i)= nFE * D21xD23xD32(i) / nD21xD23xD32 + sqrt(l^2 - R11^2) * D21xD23(i) / nD21xD23 + (D(2-1,i) + D(3 - 1,i)) / 2;
             %Position(i)=TransVector(i,4);
         end
end

Backward Kinematics

c++

void robotBackward(const double* TransVector, bool mconfig, double* theta)
{
	double A[3] = { 0 };
	double B[3] = { 0 };
	double C[3] = { 0 };

	double  x = TransVector[3];
	double  y = TransVector[7];
	double  z = TransVector[11];


	A[0] = (pow(x, 2) + pow(y, 2) + pow(z, 2) + pow(R - r, 2) + pow(L, 2) - pow(l, 2) - 2 * x * (R - r)) / 2 / L - (R - r - x);
	B[0] = 2 * z;
	C[0] = (pow(x, 2) + pow(y, 2) + pow(z, 2) + pow(R - r, 2) + pow(L, 2) - pow(l, 2) - 2 * x * (R - r)) / 2 / L + (R - r - x);
	A[1] = (pow(x, 2) + pow(y, 2) + pow(z, 2) + pow(R - r, 2) + pow(L, 2) - pow(l, 2) + (x - sqrt(3) * y) * (R - r)) / L / 2 - (R - r + x / 2 - sqrt(3) / 2 * y);
	B[1] = 2 * z;
	C[1] = (pow(x, 2) + pow(y, 2) + pow(z, 2) + pow(R - r, 2) + pow(L, 2) - pow(l, 2) + (x - sqrt(3) * y) * (R - r)) / L / 2 + (R - r + x / 2 - sqrt(3) / 2 * y);
	A[2] = (pow(x, 2) + pow(y, 2) + pow(z, 2) + pow(R - r, 2) + pow(L, 2) - pow(l, 2) + (x + sqrt(3) * y) * (R - r)) / L / 2 - (R - r + x / 2 + sqrt(3) / 2 * y);
	B[2] = 2 * z;
	C[2] = (pow(x, 2) + pow(y, 2) + pow(z, 2) + pow(R - r, 2) + pow(L, 2) - pow(l, 2) + (x + sqrt(3) * y) * (R - r)) / L / 2 + (R - r + x / 2 + sqrt(3) / 2 * y);//15

	double num = 0, den = 0;
	for (int i = 0; i < 3; i++)
	{
		if (pow(B[i], 2) - 4 * A[i] * C[i] >= 0)
			num = -B[i] - sqrt(pow(B[i], 2) - 4 * A[i] * C[i]);
		den = 2 * A[i];
		*(theta + i) = rad2angle(2 * atan(num / den));
	}
	*(theta + 3) = 202.19;
}

Application：

Reference：

参考链接

https://blog.csdn.net/qq413886183/article/details/106993725/

[1] Clavel R. Dispositif pour le deplacement et le positionnement d’un element dans l’espace[P].Switzerland: CH1985005348856,1985.
[2] 赵杰,朱延河,蔡鹤皋.Delta型并联机器人运动学正解几何解法[J].哈尔滨工业大学学报,2003(01):25-27.
[3] 伍经纹,徐世许,王鹏,宋婷婷.基于Adams的三自由度Delta机械手的运动学仿真分析[J].软件,2017,38(06):108-112.