1 /// <summary> 2 /// 六关节机器人雅可比矩阵 3 /// </summary> 4 /// <param name="AD">每个关节的a,d值构成的数组列表，如GSK RB8：{{150,0},{560,0},{155,0},{0,630},{0,0},{0,155}}</param> 5 /// <param name="Ang">每个关节的角度值数组</param> 6 /// <returns>6X6矩阵</returns> 7 public static double[,] Jacobian(List<double[]> AD, double[] Ang) 8 { 9 double A1 = AD[0][0]; 10 double D1 = AD[0][1]; 11 double A2 = AD[1][0]; 12 double A3 = AD[2][0]; 13 double D4 = AD[3][1]; 14 double A6 = AD[5][0]; 15 double D6 = AD[5][1]; 16 double C1 = Math.Cos(Ang[0]); 17 double S1 = Math.Sin(Ang[0]); 18 double C2 = Math.Cos(Ang[1]); 19 double S2 = Math.Sin(Ang[1]); 20 double C3 = Math.Cos(Ang[2]); 21 double S3 = Math.Sin(Ang[2]); 22 double S23 = Math.Sin(Ang[1] + Ang[2]); 23 double C23 = Math.Cos(Ang[1] + Ang[2]); 24 double C4 = Math.Cos(Ang[3]); 25 double S4 = Math.Sin(Ang[3]); 26 double C5 = Math.Cos(Ang[4]); 27 double S5 = Math.Sin(Ang[4]); 28 double C6 = Math.Cos(Ang[5]); 29 double S6 = Math.Sin(Ang[5]); 30 31 double J11 = ((-S1 * S23 * C4 + C1 * S4) * C5 - S1 * C23 * S5) * A6 * C6 + (S1 * S23 * S4 + C1 * C4) * A6 * S6 - ((-S1 * S23 * C4 + C1 * S4) * S5 + S1 * C23 * C5) * D6 - S1 * C23 * D4 - S1 * S23 * A3 - S1 * A2 * S2 - A1 * S1; 32 double J12 = (C1 * C23 * C4 * C5 - C1 * S23 * S5) * A6 * C6 - C1 * C23 * S4 * A6 * S6 - (C1 * C23 * C4 * S5 + C1 * S23 * C5) * D6 - C1 * S23 * D4 + C1 * C23 * A3 + C1 * A2 * C2; 33 double J13 = (C1 * C23 * C4 * C5 - C1 * S23 * S5) * A6 * C6 - C1 * C23 * S4 * A6 * S6 - (C1 * C23 * C4 * S5 + C1 * S23 * C5) * D6 - C1 * S23 * D4 + C1 * C23 * A3; 34 double J14 = (-C1 * S23 * S4 + S1 * C4) * C5 * A6 * C6 - (C1 * S23 * C4 + S1 * S4) * A6 * S6 + ((C1 * S23 * S4 - S1 * C4) * S5) * D6; 35 double J15 = ((C1 * S23 * C4 + S1 * S4) * (-S5) + C1 * C23 * C5) * A6 * C6 - ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * D6; 36 double J16 = ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * A6 * (-S6) - (C1 * S23 * S4 - S1 * C4) * A6 * C6; 37 38 double J21 = ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * A6 * C6 - (C1 * S23 * S4 - S1 * C4) * A6 * S6 + ((-C1 * S23 * C4 - S1 * S4) * S5 + C1 * C23 * C5) * D6 + C1 * C23 * D4 + C1 * S23 * A3 + C1 * A2 * S2 + A1 * C1; 39 double J22 = (S1 * C23 * C4 * C5 - S1 * S23 * S5) * A6 * C6 - S1 * C23 * S4 * A6 * S6 + (-S1 * C23 * C4 * S5 - S1 * S23 * C5) * D6 - S1 * S23 * D4 + S1 * C23 * A3 + S1 * A2 * C2; 40 double J23 = (S1 * C23 * C4 * C5 - S1 * S23 * S5) * A6 * C6 - S1 * C23 * S4 * A6 * S6 + (-S1 * C23 * C4 * S5 - S1 * S23 * C5) * D6 - S1 * S23 * D4 + S1 * C23 * A3; 41 double J24 = (-S1 * S23 * S4 - C1 * C4) * C5 * A6 * C6 - (S1 * S23 * C4 - C1 * S4) * A6 * S6 + (S1 * S23 * S4 + C1 * C4) * S5 * D6; 42 double J25 = ((S1 * S23 * C4 - C1 * S4) * (-S5) + S1 * C23 * C5) * A6 * C6 + ((-S1 * S23 * C4 + C1 * S4) * C5 - S1 * C23 * S5) * D6; 43 double J26 = ((S1 * S23 * C4 - C1 * S4) * C5 + S1 * C23 * S5) * A6 * (-S6) - (S1 * S23 * S4 + C1 * C4) * A6 * C6; 44 45 double J31 = 0; 46 double J32 = -(S23 * C4 * C5 + C23 * S5) * A6 * C6 + S23 * S4 * A6 * S6 + (S23 * C4 * S5 - C23 * C5) * D6 - C23 * D4 - S23 * A3 - A2 * S2; 47 double J33 = -(S23 * C4 * C5 + C23 * S5) * A6 * C6 + S23 * S4 * A6 * S6 + (S23 * C4 * S5 - C23 * C5) * D6 - C23 * D4 - S23 * A3; 48 double J34 = -(C23 * S4 * C5) * A6 * C6 - C23 * C4 * A6 * S6 + C23 * S4 * S5 * D6; 49 double J35 = -(C23 * C4 * S5 + S23 * C5) * A6 * C6 - (C23 * C4 * C5 - S23 * S5) * D6; 50 double J36 = (-C23 * C4 * C5 + S23 * S5) * A6 * S6 - C23 * S4 * A6 * C6; 51 52 double J41 = 0; 53 double J42 = -S1; 54 double J43 = -S1; 55 double J44 = C1 * C23; 56 double J45 = C1 * S23 * S4 - S1 * C4; 57 double J46 = (C1 * S23 * C4 + S1 * S4) * (-S5) + C1 * C23 * C5; 58 59 double J51 = 0; 60 double J52 = C1; 61 double J53 = C1; 62 double J54 = S1 * C23; 63 double J55 = S1 * S23 * S4 + C1 * C4; 64 double J56 = (S1 * S23 * C4 - C1 * S4) * (-S5) + S1 * C23 * C5; 65 66 double J61 = 1; 67 double J62 = 0; 68 double J63 = 0; 69 double J64 = -S23; 70 double J65 = C23 * S4; 71 double J66 = -(C23 * C4 * S5 + S23 * C5); 72 double[,] Mat = new double[,] { { J11, J12, J13, J14, J15, J16 }, { J21, J22, J23, J24, J25, J26 }, { J31, J32, J33, J34, J35, J36 }, { J41, J42, J43, J44, J45, J46 }, { J51, J52, J53, J54, J55, J56 }, { J61, J62, J63, J64, J65, J66 } }; 73 return Mat; 74 } 75 /// <summary> 76 /// 六关节机器人关节微分运算 77 /// </summary> 78 /// <param name="AD">每个关节的a,d值构成的数组列表，如GSK RB8：{{150,0},{560,0},{155,0},{0,630},{0,0},{0,155}}</param> 79 /// <param name="Ang">每个关节的角度值</param> 80 /// <param name="diffMat">末端空间位置姿态微分数组[dpx,dpy,dpz,δx,δy,δz]</param> 81 /// <returns>六关节角度微分数组[dӨ1,dӨ2,dӨ3,dӨ4,dӨ5,dӨ6]</returns> 82 public static double[] Differential(List<double[]> AD, double[] Ang,double[] diffMat) 83 { 84 double[] A = diffMat; 85 double[,] JacobianMat = Jacobian(AD, Ang); 86 for (int i = 0; i < 6; i++) 87 { 88 double m = JacobianMat[i, i]; 89 for (int j = i; j < 6; j++) 90 { 91 JacobianMat[i, j] /= m; 92 } 93 A[i] /= m; 94 for (int k = 0; k < 6; k++) 95 { 96 if (k != i) 97 { 98 m = JacobianMat[k, i]; 99 for (int l = i; l < 6; l++) 100 { 101 JacobianMat[k, l] -= m * JacobianMat[i, l]; 102 } 103 A[k] -= m * A[i]; 104 } 105 } 106 } 107 return A; 108 } 109 }