0

我有一个由 2 个伺服电机组成的机械臂。我正在尝试计算逆运动学，使手臂位于画布的中间，并且可以在两个方向（左右）上移动到所有可能的点。这是系统Image的图像。第一个伺服移动 0-180（逆时针）。第二个伺服器移动 0-180（顺时针）。

这是我的代码：

    int L1 = 170;
    int L2 = 230;
    Vector shoulderV;
    Vector targetV;
    shoulderV = new Vector(0,0);
    targetV = new Vector(0,400);


    Vector difference = Vector.Subtract(targetV, shoulderV);
    double L3 = difference.Length;
    if (L3 > 400) { L3 = 400; }
    if (L3 < 170) { L3 = 170; }

    // a + b is the equivelant of the shoulder angle
    double a = Math.Acos((L1 * L1 + L3 * L3 - L2 * L2) / (2 * L1 * L3));  
    double b = Math.Atan(difference.Y / difference.X);

   // S1 is the shoulder angle
   double S1 = a + b;
  // S2 is the elbow angle
  double S2 = Math.Acos((L1 * L1 + L2 * L2 - L3 * L3) / (2 * L1 * L2));

  int shoulderAngle = Convert.ToInt16(Math.Round(S1 * 180 / Math.PI));
  if (shoulderAngle < 0) { shoulderAngle = 180 - shoulderAngle; }
  if (shoulderAngle > 180) { shoulderAngle = 180; }
  int elbowAngle = Convert.ToInt16(Math.Round(S2 * 180 / Math.PI));

  elbowAngle = 180 - elbowAngle;

最初，当系统首次启动时，手臂伸直，肩=90，肘=0。当我给出正的 x 值时，我会在画布的左侧得到正确的结果。但是，我希望手臂也向右侧移动。当我输入负数时，我没有得到正确的值。我究竟做错了什么？我需要额外的伺服器才能到达右侧的点吗？

如果解释不好，请见谅。英语不是我的第一语言。

4

1 回答 1

0

我怀疑您在使用 Math.Atan() 时丢失了一个符号。我不知道这是什么编程语言或环境，但试试看你是否有这样的东西：

而不是这一行：

double b = Math.Atan(difference.Y / difference.X);

使用这样的东西：

double b = Math.Atan2(difference.Y, difference.X);

当difference.Y和difference.X具有相同的符号时，将它们相除会产生正值。这可以防止您区分它们既是积极的又是消极的情况。在这种情况下，例如，您无法区分 30 度和 210 度。

于 2016-07-26T08:11:09.317 回答