0

2 つのサーボ モーターで構成されるロボット アームがあります。アームがキャンバスの中央に配置され、両方向 (左右) のすべての可能なポイントに移動できるように、逆運動学を計算しようとしています。これはシステムImageのイメージです。最初のサーボは 0-180 (反時計回り) に移動します。2 番目のサーボは 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 に答える