Java 如何正确计算delta机器人的正运动学?
我正试图为一个三角形机器人做一个简单的模拟,我想用正向运动学(直接运动学)通过3个角度来计算末端效应器在空间中的位置 我刚开始学数学,我能理解大部分数学,但不是全部。在正向运动学的最后一部分,当我试图计算3个球体相交的点时,我迷路了。一般来说,我看过球坐标,但无法计算出旋转方向(到E(x,y,z))的两个角度。我看到他们在解一个球体的方程,但这就是我迷路的地方 三角形机器人是一种并联机器人(意味着底座和末端执行器(头部)始终保持平行)。底部和末端效应器是等边三角形,腿(通常)放置在三角形边的中间 delta机器人底座的侧面标有Java 如何正确计算delta机器人的正运动学?,java,math,computational-geometry,robotics,kinematics,Java,Math,Computational Geometry,Robotics,Kinematics,我正试图为一个三角形机器人做一个简单的模拟,我想用正向运动学(直接运动学)通过3个角度来计算末端效应器在空间中的位置 我刚开始学数学,我能理解大部分数学,但不是全部。在正向运动学的最后一部分,当我试图计算3个球体相交的点时,我迷路了。一般来说,我看过球坐标,但无法计算出旋转方向(到E(x,y,z))的两个角度。我看到他们在解一个球体的方程,但这就是我迷路的地方 三角形机器人是一种并联机器人(意味着底座和末端执行器(头部)始终保持平行)。底部和末端效应器是等边三角形,腿(通常)放置在三角形边
f
。
delta机器人的效应器一侧标有e
。
腿部上部标记为rf
,下部标记为re
原点(O)位于基准三角形的中心。
伺服电机位于基本三角形边(F1、F2、F3)的中间。
接头标记为J1、J2、J3。小腿在点E1、E2、E3处连接末端效应器
E是末端执行器三角形的中心
我可以很容易地计算点F1、F2、F3和J1、J2、J3。
我和E1,E2,E3有问题。从解释上看,,
我知道点J1有点向内平移(通过末端效应器中值的一半)
到J1',它将成为半径为re的球体的中心(小腿长度)。
对所有关节执行此操作将导致3个球体在同一位置相交:E(x,y,z)。通过求解球面方程,我们得到了E(x,y,z)
还解释了一个公式:
但这就是我迷路的地方。我的数学能力不好。
有人能简单地解释一下吗,
对于我们这些不太懂数学的人
我还使用了提供的示例代码(如果您启用了WebGL)
浏览器)您可以运行。单击并拖动以旋转场景。要控制三个角度,请使用q/q、w/w、e/e减小/增大角度
完整代码列表:
//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);
}
//Rhino测量单位:厘米
最终浮点数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=2μPI/3;
最终浮点数a60=2μPI/6;
//界限
最终浮点数minX=-200;
最终浮点数maxX=200;
最终浮点数minY=-200;
最终浮动最大值=200;
最终浮点数minZ=-200;
最终浮点数maxZ=-10;
最终浮动最大值=54;
最终浮点数=-21;
浮点xp=0;
浮动yp=0;
浮动zp=-45;
浮点数t1=0//西塔
浮动t2=0;
浮动t3=0;
浮动prevX;
浮动汇率;
浮动prevZ;
浮动prevT1;
浮动prevT2;
浮动prevT3;
布尔有效位置;
//廉价电弧球
PVector偏移,cameraRotation=新PVector(),cameraTargetRotation=新PVector();
无效设置(){
尺寸(900600,P3D);
}
作废提款(){
背景(192);
pushMatrix();
平移(宽度*.5,高度*.5300);
//rotateY(map(mouseX,0,width,-PI,PI));
如果(鼠标按下&(鼠标>300)){
cameraTargetRotation.x+=-float(mouseY-pmouseY);
cameraTargetRotation.y+=浮动(mouseX-pmouseX);
}
rotateX(弧度(cameraRotation.x-=(cameraRotation.x-cameraTargetRotation.x)*.35));
rotateY(弧度(cameraRotation.y-=(cameraRotation.y-cameraTargetRotation.y)*.35));
冲程(0);
et(f,颜色(255));
绘图点(新PVector(),2,颜色(255,0255));
float[]t=新的float[]{t1,t2,t3};
对于(int i=0;i<3;i++){
浮点数a=半π+(弧度(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*rxMat
PMatrix3D m=新的PMatrix3D();
m、 翻译(F.x,F.y,F.z);
m、 rotateZ(a);
m、 rotateY(弧度(t[i]);
m、 翻译(rf,0,0);
PVector J=新的PVector();
m、 mult(新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);
绘制点(F,2,颜色(255,0,0));
绘图点(J,2,颜色(255255,0));
绘制点(E,2,颜色(0255,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);
绘图点(新PVector(),2,颜色(0255255));
et(e,颜色(255));
popMatrix();
popMatrix();
}
空心绘制点(PVector p、float s、颜色c){
pushMatrix();
翻译(p.x,p.y,p.z);
填充(c);
盒子(s);
popMatrix();
}
void et(float r,color c){//画等边三角形,r是半径(中位数),c是颜色
pushMatrix();
rotateZ(-HALF_PI);
填充(c);
beginShape();
对于(int i=0;i<3;i++)
顶点(cos(a120*i)*r,sin(a120*i)*r,0);
端形(闭合);
popMatrix();
}
按下void键(){
浮动金额=3;
如果(键=='q')t1-=金额;
如果(键=='Q')t1+=金额;
如果(键=='w')t2-=金额;
如果(键=='W')t2+=金额;
如果(键=='e')t3-=金额;
如果(键=='E')t3+=金额;
t1=约束(t1,minT,maxT);
t2=约束(t2,minT,maxT);
t3=约束(t3,minT,maxT);
dk();
}
void ik(){
如果(xpmaxX){xp=maxX;}
如果(ypmaxX){yp=maxX;}
如果(zpmaxZ){zp=maxZ;}
validPosition=true;
//设置第一个角度
浮动θ1=旋转z(xp,yp,zp);
如果(θ1!=999){
浮动θ2=旋转z(xp*cos120+yp*sin120,yp*co
// 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