六关节机器人的逆运动学计算

  1         /// <summary>
  2         /// 六关节机器人逆运算
  3         /// </summary>
  4         /// <param name="Mat6">末端姿态及位置</param>
  5         /// <param name="AD">每个关节的a,d值构成的数组列表，如GSK RB8：{{150,0},{560,0},{155,0},{0,630},{0,0},{0,155}}</param>
  6         /// <param name="A5minus">第5轴是否为负值</param>
  7         /// <returns>六关节角度数组</returns>
  8         public static double[] InverseCal(double[,] Mat6,List<double[]> AD,bool A5minus=false)
  9         {
 10             double A1 = AD[0][0];
 11             double D1 = AD[0][1];
 12             double A2 = AD[1][0];
 13             double A3 = AD[2][0];
 14             double D4 = AD[3][1];
 15             double A6 = AD[5][0];
 16             double D6 = AD[5][1];

 29             double Ө1 = 0, Ө2 = 0, Ө3 = 0, Ө4 = 0, Ө5 = 0, Ө6 = 0;
 30             double p5x = Mat6[0,3] - A6 * Mat6[0,0];
 31             double p5y = Mat6[1,3] - A6 * Mat6[1,0];
 32             double p5z = Mat6[2,3] - A6 * Mat6[2,0];
 33 
 34             double p4x = p5x - D6 * Mat6[0,2];
 35             double p4y = p5y - D6 * Mat6[1,2];
 36             double p4z = p5z - D6 * Mat6[2,2];
 37 
 38             Ө1 = Math.Atan2(p4y, p4x);
 39             double p2x = A1 * Math.Cos(Ө1);
 40             double p2y = A1 * Math.Sin(Ө1);
 41             double p2z = D1;
 42 
 43             double l422 = (p4x - p2x) * (p4x - p2x) + (p4y - p2y) * (p4y - p2y) + (p4z - p2z) * (p4z - p2z);
 44             double l42 = Math.Sqrt(l422);
 45             double l3 = Math.Sqrt(A3 * A3 + D4 * D4);
 46 
 47             double t21 = Math.Asin((p4z - D1) / l42); ///l42与xy平面夹角
 48 
 49             double l42_1 = (l422 - l3 * l3 + A2 * A2) / (2 * l42);//J2 - J3到L42垂线点距离
 50             double t22 = Math.Acos(l42_1 / A2);
 51             double t23 = Math.Acos((l42 - l42_1) / l3);
 52 
 53             Ө2 = -(t21 + t22) + Math.PI / 2;
 54 
 55             double t3a = Math.Atan2(D4, A3);
 56             Ө3 = t22 + t23 - t3a;
 57             double S1 = Math.Sin(Ө1);
 58             double C1 = Math.Cos(Ө1);
 59             double S2 = Math.Sin(Ө2);
 60             double C2 = Math.Cos(Ө2);
 61             double S3 = Math.Sin(Ө3);
 62             double C3 = Math.Cos(Ө3);
 63             double S23 = Math.Sin(Ө2 + Ө3);
 64             double C23 = Math.Cos(Ө2 + Ө3);
 65 
 66             double n3x = C1 * S23;
 67             double n3y = S1 * S23;
 68             double n3z = C23;
 69             double o3x = S1;
 70             double o3y = -C1;
 71             double o3z = 0;
 72             double a3x = C1 * C23;
 73             double a3y = S1 * C23;
 74             double a3z = -S23;
 80             double T02 = n3x * Mat6[0, 2] + n3y * Mat6[1, 2] + n3z * Mat6[2, 2];
 81             double T12 = o3x * Mat6[0, 2] + o3y * Mat6[1, 2] + o3z * Mat6[2, 2];
 82             double T22 = a3x * Mat6[0, 2] + a3y * Mat6[1, 2] + a3z * Mat6[2, 2];
 83             double T20 = a3x * Mat6[0, 0] + a3y * Mat6[1, 0] + a3z * Mat6[2, 0];
 84             double T21 = a3x * Mat6[0, 1] + a3y * Mat6[1, 1] + a3z * Mat6[2, 1];
 85 
 86             if (A5minus)
 87             {
 88                 Ө4 = Math.Atan2(T12, T02);
 89                 double v5y = Math.Sqrt(T12 * T12 + T02 * T02);
 90                 Ө5 = Math.Atan2(-v5y, T22);
 91                 Ө6 = Math.Atan2(T21, -T20);
 92             }
 93             else
 94             {
 95                 Ө4 = Math.Atan2(-T12, -T02);
 96                 double v5y = Math.Sqrt(T12 * T12 + T02 * T02);
 97                 Ө5 = Math.Atan2(v5y, T22);
 98                 Ө6 = Math.Atan2(-T21, T20);
 99             }

115             return new double[] { Ө1 * 57.2958,Ө2 * 57.2958 , Ө3 * 57.2958 , Ө4 * 57.2958, Ө5 * 57.2958, Ө6 * 57.2958 };
116         }