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 }