Scara 机器人逆运动学

Question

我正在开发一个由 Arduino 控制的 Scara 机器人，并试图找出逆运动学。我编写了一个简单的示例草图来测试我的功能，但由于数学错误，它不起作用。有人可以告诉我出了什么问题吗？

const int L1 = 202.25;
const int L2 = 193.35;

int alpha = 0;
int beta = 0;

void calc_IK(int x, int y){
  beta = acos((x*x+y*y-L1*L1-L2*L2)/2*L1*L2); //I could also ise pow()
  alpha = atan((y/x)-atan(L2*sin(beta)/L1+L2*cos(beta)));
  Serial.println(alpha, beta);
}

void setup() {
  Serial.begin(9600);
}

void loop() {
  calc_IK(3, 5);
  delay(200);
}

Answer 1

我不确定错误在哪里，但我将一些变量更改为浮点型，将一些变量更改为双精度型。我还注意到结果是以弧度为单位的，所以我必须将其转换为度数。这是我现在运行的代码：

#include <math.h>

const double L1 = 202.25;
const double L2 = 193.35;

float alpha = 0; //radians
float beta = 0;

float beta_d; //degrees
float alpha_d;

void calc_IK(float x, float y){
  beta = acos((pow(x, 2)+pow(y, 2)-pow(L1, 2)-pow(L2, 2))/(2*pow(L1, 2)*pow(L2, 2)));
  alpha = atan((y/x)-atan((L2*sin(beta))/(L1+L2*cos(beta))));

  beta_d = beta * 180/M_PI;
  alpha_d = alpha * 180/M_PI;


  Serial.print("Beta: ");
  Serial.print(beta_d);
  Serial.print(" ");
  Serial.print("Alpha: "); 
  Serial.print(alpha_d);
  Serial.println(" ");

}

void setup() {
  Serial.begin(9600);
}

void loop() {
  calc_IK(3.0, 5.0);
}

它可能不是最好的，但它确实有效。