Java 如何正确计算 delta 机器人的直接运动学？
声明：本页面是StackOverFlow热门问题的中英对照翻译，遵循CC BY-SA 4.0协议，如果您需要使用它，必须同样遵循CC BY-SA许可，注明原文地址和作者信息，同时你必须将它归于原作者（不是我）：StackOverFlow
原文地址: http://stackoverflow.com/questions/18162880/
Warning: these are provided under cc-by-sa 4.0 license. You are free to use/share it, But you must attribute it to the original authors (not me):
StackOverFlow
How to correctly compute direct kinematics for a delta robot?
提问by George Profenza
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.
我正在尝试为 delta 机器人进行一个简单的模拟，我想使用正向运动学（直接运动学）通过传递 3 个角度来计算末端执行器在空间中的位置。
I've started with the Trossen Robotics Forum Delta Robot Tutorialand 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.
我从Trossen 机器人论坛 Delta 机器人教程开始，我可以理解大部分数学，但不是全部。在尝试计算 3 个球体的相交点时，我在前向运动学的最后一部分迷失了方向。我已经看过一般的球面坐标，但无法计算出用于找到旋转方向（到 E（x，y，z））的两个角度。我看到他们正在求解一个球体的方程，但那是我迷路的地方。
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.
Delta 机器人是并联机器人（意味着底座和末端执行器（头部）始终保持平行）。底部和末端执行器是等边三角形，腿（通常）放置在三角形边的中间。
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 rfand the lower side re.
Delta 机器人底座的侧面标有f。Delta 机器人的执行器一侧被标记为e。腿的上部有标记rf，下部有标记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.
原点 (O) 位于底三角形的中心。伺服电机位于三角形底边（F1、F2、F3）的中间。接头标有 J1、J2、J3。小腿在点 E1、E2、E3 处连接末端执行器，E 是末端执行器三角形的中心。
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).
我可以轻松计算点 F1、F2、F3 和 J1、J2、J3。我遇到了 E1、E2、E3 问题。从解释中，我了解到点 J1 向内平移一点（末端执行器的中位数的一半）到 J1'，它成为具有半径re（小腿长度）的球体的中心。对所有关节执行此操作将导致 3 个球体在同一位置相交：E(x,y,z)。通过求解球面方程，我们可以找到 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.
我还使用了提供的示例代码（如果您有启用 WebGL 的浏览器），您可以在此处运行。单击并拖动以旋转场景。要控制三个角度，请使用 q/Q、w/W、e/E 来减小/增加角度。
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.
问题是，在可视化时，下半部分改变了长度（正如您在打印的 message0 中看到的那样，它不应该，这进一步增加了我的困惑。
I've used the supplied C code in Java/Processing, but the programming language is least important.
我在 Java/Processing 中使用了提供的 C 代码，但编程语言最不重要。
[Edit by spektre]
[由 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
- 内衬的废话不是掌握运动学能力的最佳方式
- 据我所知，带有电机的底座位于上三角形平面上的此图像上
- 并且工具在底部三角形平面上
回答by Spektre
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
- 计算 F1、F2、F3；
求解系统
// 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- 其中 q 是关节距离 |Ei-E| 这是常数
[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
- 我不完全相信你应该驱动所有 3 个关节
- 很可能只驾驶其中的 2 个
- 并计算 3.th 使运动学保持在有效位置
[Edit1]
[编辑1]
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)
- Ti = 将 Ji 向 Z 轴平移 q（平行于 XY 平面）
现在，如果您只需要从 Ti 中找到 3 个球体的交点
- 这个点是E
所以 Ei 现在是 E 的简单翻译（与 Ji 翻译相反）
PS. I hope you know how to compute angles when you have all the points ...
附注。我希望你知道如何在拥有所有点后计算角度......
