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.