如何正确计算三角机器人的直接运动学? [英] How to correctly compute direct kinematics for a delta robot?
问题描述
我正在尝试为一个三角机器人建立一个简单的模拟，我想使用正向运动学(直接运动学)通过传递3个角度来计算末端执行器在空间中的位置.
我从运行.单击并拖动以旋转场景.要控制这三个角度，请使用q/Q，w/W，e/E来减小/增大角度.
完整的代码清单:
//犀牛尺寸，以厘米为单位最终浮子e = 21;//末端执行器侧最终浮点f = 60.33;//底边最终浮标rf = 67.5;//大腿长度-上球半径最终浮点re = 95;//小腿长度-下球体的重做(偏移量将合并为E(x，y，z))最终浮点数sqrt3 = sqrt(3.0);最终浮点数sin120 = sqrt3/2.0;最终浮动cos120 = -0.5;最终浮点tan60 = sqrt3;最终浮动sin30 = 0.5;最终浮点tan30 = 1/sqrt3;最终浮点a120 = TWO_PI/3;最终浮点a60 = TWO_PI/6;//界限最终浮点数minX = -200;最终浮点数maxX = 200;最终浮点minY = -200;最终浮点数maxY = 200;最终浮点数minZ = -200;最终浮点maxZ = -10;最终浮点maxT = 54;最终浮点minT = -21;浮点xp = 0;浮点yp = 0;浮点zp = -45;浮点t1 = 0;//θ浮点t2 = 0;浮点t3 = 0;浮动prevX;上浮浮动prevZ;浮动prevT1;浮动prevT2;浮动prevT3;boolean validPosition;//便宜的弓箭PVector偏移量，cameraRotation =新的PVector()，cameraTargetRotation =新的PVector();void setup(){尺寸(900,600，P3D);}无效draw(){背景(192);pushMatrix();平移(宽度* .5，高度* .5,300);//rotateY(map(mouseX，0，width，-PI，PI));如果(mousePressed&&(mouseX> 300)){cameraTargetRotation.x + = -float(mouseY-pmouseY);cameraTargetRotation.y + = float(mouseX-pmouseX);}rotationX(radians(cameraRotation.x-=(cameraRotation.x-cameraTargetRotation.x)* .35));rotationY(radians(cameraRotation.y-=(cameraRotation.y-cameraTargetRotation.y)* .35));笔画(0);et(f，color(255));drawPoint(new PVector()，2，color(255,0,255));float [] t =新的float [] {t1，t2，t3};for(int i = 0; i< 3; i ++){浮点a = HALF_PI +(radians(120)* i);浮点数r1 = f/1.25 * tan(弧度(30));浮点数r2 = e/1.25 * tan(弧度(30));PVector F =新的PVector(cos(a)* r1，sin(a)* r1,0);PVector E =新的PVector(cos(a)* r2，sin(a)* r2,0);E.add(xp，yp，zp);//J = F * rxMatPMatrix3D m =新的PMatrix3D();m.translate(F.x，F.y，F.z);m.rotateZ(a);m.rotateY(radians(t [i]));m.translate(rf，0,0);PVector J =新的PVector();m.mult(new PVector()，J);行(F.x，F.y，F.z，J.x，J.y，J.z);行(E.x，E.y，E.z，J.x，J.y，J.z);drawPoint(F，2，color(255,0,0));drawPoint(J，2，color(255,255,0));drawPoint(E，2，color(0,255,0));//println(dist(F.x,F.y,F.z,J.x,J.y,J.z)+"\t"+rf);println(dist(E.x，E.y，E.z，J.x，J.y，J.z)+"\ t" + re);//长度不应改变}pushMatrix();翻译(xp，yp，zp);drawPoint(new PVector()，2，color(0,255,255));et(e，color(255));popMatrix();popMatrix();}void drawPoint(PVector p，float s，color c){pushMatrix();翻译(p.x，p.y，p.z);填写(c);盒子;popMatrix();}void et(float r，color c){//绘制等边三角形，r是半径(中位数)，c是颜色pushMatrix();rotationZ(-HALF_PI);填写(c);beginShape();for(int i = 0; i< 3; i ++)顶点(cos(a120 * i)* r，sin(a120 * i)* r，0);endShape(CLOSE);popMatrix();}无效keyPressed(){float amt = 3;if(key =='q')t1-= amt;if(key =='Q')t1 + = amt;if(key =='w')t2-= amt;if(key =='W')t2 + = amt;if(key =='e')t3-= amt;if(key =='E')t3 + = amt;t1 =约束(t1，minT，maxT);t2 =约束(t2，minT，maxT);t3 =约束(t3，minT，maxT);dk();}无效ik(){如果(xp< minX){xp = minX;}if(xp> maxX){xp = maxX;}if(yp< minX){yp = minX;}如果(yp> maxX){yp = maxX;}if(zp< minZ){zp = minZ;}if(zp> maxZ){zp = maxZ;}validPosition = true;//设置第一个角度浮点数theta1 = rotationYZ(xp，yp，zp);如果(theta1！= 999){浮点theta2 = rotateYZ(xp * cos120 + yp * sin120，yp * cos120-xp * sin120，zp);//将坐标旋转到+120度如果(theta2！= 999){float theta3 = rotationYZ(xp * cos120-yp * sin120，yp * cos120 + xp * sin120，zp);//将坐标旋转到-120度如果(theta3！= 999){//我们成功了-点存在if(theta1< = maxT& theta2< = maxT& theta3< = maxT& amp; theta1> = minT& amp; theta2> = minT& theta3>= minT){//边界检查t1 = theta1;t2 = theta2;t3 = theta3;} 别的 {validPosition = false;}} 别的 {validPosition = false;}} 别的 {validPosition = false;}} 别的 {validPosition = false;}//哦，哦，我们失败了，恢复到我们最后一次知道的好位置如果(！validPosition){xp = prevX;yp = prevY;zp = prevZ;}}无效dk(){validPosition = true;浮点t =(f-e)* tan30/2;浮点数dtr = PI/(float)180.0;浮点数theta1 = dtr * t1;浮点theta2 = dtr * t2;float theta3 = dtr * t3;浮点y1 =-(t + rf * cos(theta1));浮点z1 = -rf * sin(theta1);浮点y2 =(t + rf * cos(theta2))* sin30;浮点数x2 = y2 * tan60;浮点z2 = -rf * sin(theta2);浮点y3 =(t + rf * cos(theta3))* sin30;浮点数x3 = -y3 * tan60;浮点z3 = -rf * sin(theta3);浮点数dnm =(y2-y1)* x3-(y3-y1)* x2;浮点数w1 = y1 * y1 + z1 * z1;浮点数w2 = x2 * x2 + y2 * y2 + z2 * z2;浮点数w3 = x3 * x3 + y3 * y3 + z3 * z3;//x =(a1 * z + b1)/dnm浮点a1 =(z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);浮点b1 =-((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;//y =(a2 * z + b2)/dnm;浮点数a2 =-(z2-z1)* x3 +(z3-z1)* x2;浮点b2 =((w2-w1)* x3-(w3-w1)* x2)/2.0;//a * z ^ 2 + b * z + c = 0浮点a = a1 * a1 + a2 * a2 + dnm * dnm;浮点b = 2 *(a1 * b1 + a2 *(b2-y1 * dnm)-z1 * dnm * dnm);浮点数c =(b2-y1 * dnm)*(b2-y1 * dnm)+ b1 * b1 + dnm * dnm *(z1 * z1-re * re);//判别式浮点数d = b * b-(浮点数)4.0 * a * c;如果(d< 0){validPosition = false;}zp =-(float)0.5 *(b + sqrt(d))/a;xp =(a1 * zp + b1)/dnm;yp =(a2 * zp + b2)/dnm;如果(xp> = minX& xp< = maxX& yp> = minX& yp< = maxX&& zp> = minZ& zp< = maxZ){//边界检查} 别的 {validPosition = false;}如果(！validPosition){xp = prevX;yp = prevY;zp = prevZ;t1 = prevT1;t2 = prevT2;t3 = prevT3;}}无效的storePrev(){prevX = xp;prevY = yp;prevZ = zp;prevT1 = t1;prevT2 = t2;prevT3 = t3;}浮点数rotateYZ(浮点数x0，浮点数y0，浮点数z0){浮点y1 = -0.5 * 0.57735 * f;//f/2 * tg 30y0-= 0.5 * 0.57735 * e;//将中心移到边缘//z = a + b * y浮点a =(x0 * x0 + y0 * y0 + z0 * z0 + rf * rf-re * re-y1 * y1)/(2 * z0);浮点b =(y1-y0)/z0;//判别式浮点数d =-(a + b * y1)*(a + b * y1)+ rf *(b * b * rf + rf);如果(d< 0)返回999;//不存在的点浮点yj =(y1-a * b-sqrt(d))/(b * b +1);//选择外点浮点数zj = a + b * yj;返回180.0 * atan(-zj/(y1-yj))/PI +((yj> y1)?180.0:0.0);} 问题是，当可视化时，下部会改变长度(正如您在打印的message0中看到的那样，不应改变)，这进一步加剧了我的困惑.
我在Java/Processing中使用了提供的C代码，但是编程语言最不重要.
[由spektre编辑]
出于教学原因，我只需要添加这张图片.
- 胡说八道不是掌握运动学能力的最佳方法
- 据我所知，电动机的底座在此图像的上三角平面上
- 该工具在底部三角形平面上
我将按以下步骤进行操作(图形解决方案的代数表示):
- 计算F1，F2，F3;
-
解决系统
//球面从Ji到Ei ...平行四边形(使用较低的Z半球)(x1-J1.x)^ 2 +(y1-J1.y)^ 2 +(z1-J1.z)^ 2 = re ^ 2(x2-J2.x)^ 2 +(y2-J2.y)^ 2 +(z2-J2.z)^ 2 = re ^ 2(x3-J3.x)^ 2 +(y3-J3.y)^ 2 +(z3-J3.z)^ 2 = re ^ 2//Ei位于球体上E1 =(x1，y1，z1)E2 =(x2，y2，z2)E3 =(x3，y3，z3)//Ei与Fi平行...必须调整坐标系//因此基本三角形与XY平面平行z1 = z2z1 = z3z2 = z3//任何Ei Ej之间的距离必须始终为q//否则它是无效位置(运动被卡住甚至损坏)| E1-E2 | = q| E1-E3 | = q| E2-E3 | = q//中点只是Ei的平均值E =(E1 + E2 + E3)/3- 其中q是关节距离| Ei-E |这是恒定的
[注释]
请勿手动解决
- 使用导数或其他方法获得代数解
- 并仅使用有效的解决方案
- 其二次系统，因此很可能会有更多解决方案，因此您必须检查正确的解决方案
一个愚蠢的问题，为什么不解决逆运动学
- 很可能是您所需要的(如果您只是不进行可视化)
- 并且在这种情况下也更简单
同样，当您仅使用直接运动学时
- 我并不完全相信您应该推动所有3个关节
- 最有可能只开车2个
- 并计算第3个，使运动学保持在有效位置
[Edit1]
对我来说，有一种简化形式:
- Ti =将Ji向Z轴平移q(平行于XY平面)
-
现在是否只需要从Ti中查找3个球的交点
- 这就是E
-
所以Ei现在是E的简单翻译(与Ji翻译相反)
PS.我希望您知道所有点后如何计算角度...
I'm trying to put together a simple simulation for a delta robot and I'd like to use forward kinematics (direct kinematics) to compute the end effector's position in space by passing 3 angles.
I've started with the Trossen Robotics Forum Delta Robot Tutorial and I can understand most of the math, but not all. I'm lost at the last part in forward kinematics, when trying to compute the point where the 3 sphere's intersect. I've looked at spherical coordinates in general but couldn't work out the two angles used to find to rotate towards (to E(x,y,z)). I see they're solving the equation of a sphere, but that's where I get lost.
A delta robot is a parallel robot (meaning the base and the end effector(head) always stay parallel). The base and end effector are equilateral triangles and the legs are (typically) placed at the middle of the triangle's sides.
The side of the base of the delta robot is marked f.
The side of the effector of the delta robot is marked e.
The upper part of the leg is marked rf and the lower side re.
The origin(O) is at the centre of the base triangle. The servo motors are at the middle of the base triangle's sides (F1,F2,F3). The joints are marked J1,J2,J3. The lower legs join the end effector at points E1,E2,E3 and E is the centre of the end effector triangle.
I can easily compute points F1,F2,F3 and J1,J2,J3.
It's E1,E2,E3 I'm having issues with. From the explanations,
I understand that point J1 gets translate inwards a bit (by half the end effector's median)
to J1' and it becomes the centre of a sphere with radius re (lower leg length).
Doing this for all joints will result in 3 spheres intersecting in the same place: E(x,y,z). By solving the sphere equation we find E(x,y,z).
There is also a formula explained:
but this is where I get lost. My math skills aren't great. Could someone please explain those in a simpler manner, for the less math savvy of us ?
I've also used the sample code provided which (if you have a WebGL enabled browser) you can run here. Click and drag to rotate the scene. To control the three angles use q/Q, w/W,e/E to decrease/increase angles.
Full code listing:
//Rhino measurements in cm
final float e = 21;//end effector side
final float f = 60.33;//base side
final float rf = 67.5;//upper leg length - radius of upper sphere
final float re = 95;//lower leg length - redius of lower sphere (with offset will join in E(x,y,z))
final float sqrt3 = sqrt(3.0);
final float sin120 = sqrt3/2.0;
final float cos120 = -0.5;
final float tan60 = sqrt3;
final float sin30 = 0.5;
final float tan30 = 1/sqrt3;
final float a120 = TWO_PI/3;
final float a60 = TWO_PI/6;
//bounds
final float minX = -200;
final float maxX = 200;
final float minY = -200;
final float maxY = 200;
final float minZ = -200;
final float maxZ = -10;
final float maxT = 54;
final float minT = -21;
float xp = 0;
float yp = 0;
float zp =-45;
float t1 = 0;//theta
float t2 = 0;
float t3 = 0;
float prevX;
float prevY;
float prevZ;
float prevT1;
float prevT2;
float prevT3;
boolean validPosition;
//cheap arcball
PVector offset,cameraRotation = new PVector(),cameraTargetRotation = new PVector();
void setup() {
size(900,600,P3D);
}
void draw() {
background(192);
pushMatrix();
translate(width * .5,height * .5,300);
//rotateY(map(mouseX,0,width,-PI,PI));
if (mousePressed && (mouseX > 300)){
cameraTargetRotation.x += -float(mouseY-pmouseY);
cameraTargetRotation.y += float(mouseX-pmouseX);
}
rotateX(radians(cameraRotation.x -= (cameraRotation.x - cameraTargetRotation.x) * .35));
rotateY(radians(cameraRotation.y -= (cameraRotation.y - cameraTargetRotation.y) * .35));
stroke(0);
et(f,color(255));
drawPoint(new PVector(),2,color(255,0,255));
float[] t = new float[]{t1,t2,t3};
for(int i = 0 ; i < 3; i++){
float a = HALF_PI+(radians(120)*i);
float r1 = f / 1.25 * tan(radians(30));
float r2 = e / 1.25 * tan(radians(30));
PVector F = new PVector(cos(a) * r1,sin(a) * r1,0);
PVector E = new PVector(cos(a) * r2,sin(a) * r2,0);
E.add(xp,yp,zp);
//J = F * rxMat
PMatrix3D m = new PMatrix3D();
m.translate(F.x,F.y,F.z);
m.rotateZ(a);
m.rotateY(radians(t[i]));
m.translate(rf,0,0);
PVector J = new PVector();
m.mult(new PVector(),J);
line(F.x,F.y,F.z,J.x,J.y,J.z);
line(E.x,E.y,E.z,J.x,J.y,J.z);
drawPoint(F,2,color(255,0,0));
drawPoint(J,2,color(255,255,0));
drawPoint(E,2,color(0,255,0));
//println(dist(F.x,F.y,F.z,J.x,J.y,J.z)+"\t"+rf);
println(dist(E.x,E.y,E.z,J.x,J.y,J.z)+"\t"+re);//length should not change
}
pushMatrix();
translate(xp,yp,zp);
drawPoint(new PVector(),2,color(0,255,255));
et(e,color(255));
popMatrix();
popMatrix();
}
void drawPoint(PVector p,float s,color c){
pushMatrix();
translate(p.x,p.y,p.z);
fill(c);
box(s);
popMatrix();
}
void et(float r,color c){//draw equilateral triangle, r is radius ( median), c is colour
pushMatrix();
rotateZ(-HALF_PI);
fill(c);
beginShape();
for(int i = 0 ; i < 3; i++)
vertex(cos(a120*i) * r,sin(a120*i) * r,0);
endShape(CLOSE);
popMatrix();
}
void keyPressed(){
float amt = 3;
if(key == 'q') t1 -= amt;
if(key == 'Q') t1 += amt;
if(key == 'w') t2 -= amt;
if(key == 'W') t2 += amt;
if(key == 'e') t3 -= amt;
if(key == 'E') t3 += amt;
t1 = constrain(t1,minT,maxT);
t2 = constrain(t2,minT,maxT);
t3 = constrain(t3,minT,maxT);
dk();
}
void ik() {
if (xp < minX) { xp = minX; }
if (xp > maxX) { xp = maxX; }
if (yp < minX) { yp = minX; }
if (yp > maxX) { yp = maxX; }
if (zp < minZ) { zp = minZ; }
if (zp > maxZ) { zp = maxZ; }
validPosition = true;
//set the first angle
float theta1 = rotateYZ(xp, yp, zp);
if (theta1 != 999) {
float theta2 = rotateYZ(xp*cos120 + yp*sin120, yp*cos120-xp*sin120, zp); // rotate coords to +120 deg
if (theta2 != 999) {
float theta3 = rotateYZ(xp*cos120 - yp*sin120, yp*cos120+xp*sin120, zp); // rotate coords to -120 deg
if (theta3 != 999) {
//we succeeded - point exists
if (theta1 <= maxT && theta2 <= maxT && theta3 <= maxT && theta1 >= minT && theta2 >= minT && theta3 >= minT ) { //bounds check
t1 = theta1;
t2 = theta2;
t3 = theta3;
} else {
validPosition = false;
}
} else {
validPosition = false;
}
} else {
validPosition = false;
}
} else {
validPosition = false;
}
//uh oh, we failed, revert to our last known good positions
if ( !validPosition ) {
xp = prevX;
yp = prevY;
zp = prevZ;
}
}
void dk() {
validPosition = true;
float t = (f-e)*tan30/2;
float dtr = PI/(float)180.0;
float theta1 = dtr*t1;
float theta2 = dtr*t2;
float theta3 = dtr*t3;
float y1 = -(t + rf*cos(theta1));
float z1 = -rf*sin(theta1);
float y2 = (t + rf*cos(theta2))*sin30;
float x2 = y2*tan60;
float z2 = -rf*sin(theta2);
float y3 = (t + rf*cos(theta3))*sin30;
float x3 = -y3*tan60;
float z3 = -rf*sin(theta3);
float dnm = (y2-y1)*x3-(y3-y1)*x2;
float w1 = y1*y1 + z1*z1;
float w2 = x2*x2 + y2*y2 + z2*z2;
float w3 = x3*x3 + y3*y3 + z3*z3;
// x = (a1*z + b1)/dnm
float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;
// y = (a2*z + b2)/dnm;
float a2 = -(z2-z1)*x3+(z3-z1)*x2;
float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;
// a*z^2 + b*z + c = 0
float a = a1*a1 + a2*a2 + dnm*dnm;
float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);
// discriminant
float d = b*b - (float)4.0*a*c;
if (d < 0) { validPosition = false; }
zp = -(float)0.5*(b+sqrt(d))/a;
xp = (a1*zp + b1)/dnm;
yp = (a2*zp + b2)/dnm;
if (xp >= minX && xp <= maxX&& yp >= minX && yp <= maxX && zp >= minZ & zp <= maxZ) { //bounds check
} else {
validPosition = false;
}
if ( !validPosition ) {
xp = prevX;
yp = prevY;
zp = prevZ;
t1 = prevT1;
t2 = prevT2;
t3 = prevT3;
}
}
void storePrev() {
prevX = xp;
prevY = yp;
prevZ = zp;
prevT1 = t1;
prevT2 = t2;
prevT3 = t3;
}
float rotateYZ(float x0, float y0, float z0) {
float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
y0 -= 0.5 * 0.57735 * e; // shift center to edge
// z = a + b*y
float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
float b = (y1-y0)/z0;
// discriminant
float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
if (d < 0) return 999; // non-existing point
float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
float zj = a + b*yj;
return 180.0*atan(-zj/(y1 - yj))/PI + ((yj>y1)?180.0:0.0);
}
The problem is, when visualizing, the lower part changes length (as you can see in the printed message0 and it shouldn't, which further adds to my confusion.
I've used the supplied C code in Java/Processing, but the programming language is least important.
[Edit by spektre]
I just had to add this picture (for didactic reasons).
- the lined nonsense is not the best way for grasping the kinematics abilities
- as I understand the base with the motors is on this image on the upper triangle plane
- and the tool is on the bottom triangle plane
I would do it as follows (algebraic representation of graphic solution):
- compute F1,F2,F3;
solve system
// spheres from Ji to Ei ... parallelograms (use lower Z half sphere) (x1-J1.x)^2 + (y1-J1.y)^2 +(z1-J1.z)^2 = re^2 (x2-J2.x)^2 + (y2-J2.y)^2 +(z2-J2.z)^2 = re^2 (x3-J3.x)^2 + (y3-J3.y)^2 +(z3-J3.z)^2 = re^2 // Ei lies on the sphere E1=(x1,y1,z1) E2=(x2,y2,z2) E3=(x3,y3,z3) // Ei is parallel to Fi ... coordinate system must be adjusted // so base triangles are parallel with XY-plane z1=z2 z1=z3 z2=z3 // distance between any Ei Ej must be always q // else it is invalid position (kinematics get stuck or even damage) |E1-E2|=q |E1-E3|=q |E2-E3|=q // midpoint is just average of Ei E=(E1+E2+E3)/3- where q is the joint distance |Ei-E| which is constant
[Notes]
Do not solve it manually
- use derive or something to obtain algebraic solution
- and use only valid solution
- its quadratic system so there will be most likely more solutions so you have to check for the correct one
Just a silly question why don't you solve inverse kinematics
- it is most likely what you need (if you just don't do a visualization only)
- and also is a bit simpler in this case
Also when you use just direct kinematics
- I am not entirely convinced that you should drive all 3 joints
- most likely drive just 2 of them
- and compute the 3.th so the kinematics stay in valid position
[Edit1]
There is one simplification that just appear to me:
- Ti = translate Ji towards the Z axis by q (parallel to XY plane)
now if you just need to find intersection of 3 spheres from Ti
- this point is E
so Ei is now simple translation of E (inverse from the Ji translation)
PS. I hope you know how to compute angles when you have all the points ...
这篇关于如何正确计算三角机器人的直接运动学?的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！
