首页 > 代码库 > 六关节机器人的正运动学计算

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

网上关于六关节机器人运动学的资料很多,部分资料在坐标系法则上或是计算数据上存在一些错误,下面整理已通过程序验证。

根据Denavit-Hartenberg方法:(Rot(x,θ)Trans(a,0,d)Rot(z,α)

技术分享技术分享

另一种计算形式:

技术分享

由第一种方式得到的六关节矩阵:

技术分享

技术分享

技术分享

则:

技术分享

技术分享

  1         /// <summary>  2         /// 关节末端的空间姿态-位置向量[px,py,pz]  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>[px,py,pz]</returns>  7         public static double[] R6P(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 px = ((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; 32             double py = ((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; 33             double pz = (C23 * C4 * C5 - S23 * S5) * A6 * C6 - C23 * S4 * A6 * S6 - (C23 * C4 * S5 + S23 * C5) * D6 - S23 * D4 + C23 * A3 + A2 * C2 + D1; 34             return new double[] { px, py, pz }; 35         } 36         /// <summary> 37         /// 关节末端的空间姿态-n向量[nx,ny,nz] 38         /// </summary> 39         /// <param name="Ang">每个关节的角度值</param> 40         /// <returns>[nx,ny,nz]</returns> 41         public static double[] R6N(double[] Ang) 42         { 43             double C1 = Math.Cos(Ang[0]); 44             double S1 = Math.Sin(Ang[0]); 47             double C3 = Math.Cos(Ang[2]); 48             double S3 = Math.Sin(Ang[2]); 49             double S23 = Math.Sin(Ang[1] + Ang[2]); 50             double C23 = Math.Cos(Ang[1] + Ang[2]); 51             double C4 = Math.Cos(Ang[3]); 52             double S4 = Math.Sin(Ang[3]); 53             double C5 = Math.Cos(Ang[4]); 54             double S5 = Math.Sin(Ang[4]); 55             double C6 = Math.Cos(Ang[5]); 56             double S6 = Math.Sin(Ang[5]); 57             double nx = ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * C6 - (C1 * S23 * S4 - S1 * C4) * S6; 58             double ny = ((S1 * S23 * C4 - C1 * S4) * C5 + S1 * C23 * S5) * C6 - (S1 * S23 * S4 + C1 * C4) * S6; 59             double nz = (C23 * C4 * C5 - S23 * S5) * C6 - C23 * S4 * S6; 66             return new double[] { nx, ny, nz }; 67         } 68         /// <summary> 69         /// 关节末端的空间姿态-o向量[ox,oy,oz] 70         /// </summary> 71         /// <param name="Ang">每个关节的角度值</param> 72         /// <returns>[ox,oy,oz]</returns> 73         public static double[] R6O(double[] Ang) 74         { 75             double C1 = Math.Cos(Ang[0]); 76             double S1 = Math.Sin(Ang[0]); 79             double C3 = Math.Cos(Ang[2]); 80             double S3 = Math.Sin(Ang[2]); 81             double S23 = Math.Sin(Ang[1] + Ang[2]); 82             double C23 = Math.Cos(Ang[1] + Ang[2]); 83             double C4 = Math.Cos(Ang[3]); 84             double S4 = Math.Sin(Ang[3]); 85             double C5 = Math.Cos(Ang[4]); 86             double S5 = Math.Sin(Ang[4]); 87             double C6 = Math.Cos(Ang[5]); 88             double S6 = Math.Sin(Ang[5]); 89             double ox = ((C1 * S23 * C4 + S1 * S4) * C5 + C1 * C23 * S5) * (-S6) - (C1 * S23 * S4 - S1 * C4) * C6; 90             double oy = ((S1 * S23 * C4 - C1 * S4) * C5 + S1 * C23 * S5) * (-S6) - (S1 * S23 * S4 + C1 * C4) * C6; 91             double oz = (C23 * C4 * C5 - S23 * S5) * (-S6) - C23 * S4 * C6; 97             return new double[] { ox, oy, oz }; 98         } 99         /// <summary>100         /// 关节末端的空间姿态-a向量[ax,ay,az]101         /// </summary>102         /// <param name="Ang">每个关节的角度值</param>103         /// <returns>[ax,ay,az]</returns>104         public static double[] R6A(double[] Ang)105         {106             double C1 = Math.Cos(Ang[0]);107             double S1 = Math.Sin(Ang[0]);108             double C3 = Math.Cos(Ang[2]);111             double S3 = Math.Sin(Ang[2]);112             double S23 = Math.Sin(Ang[1] + Ang[2]);113             double C23 = Math.Cos(Ang[1] + Ang[2]);114             double C4 = Math.Cos(Ang[3]);115             double S4 = Math.Sin(Ang[3]);116             double C5 = Math.Cos(Ang[4]);117             double S5 = Math.Sin(Ang[4]);118             double C6 = Math.Cos(Ang[5]);119             double S6 = Math.Sin(Ang[5]);120             double ax = (C1 * S23 * C4 + S1 * S4) * (-S5) + C1 * C23 * C5;121             double ay = (S1 * S23 * C4 - C1 * S4) * (-S5) + S1 * C23 * C5;122             double az = C23 * C4 * (-S5) - S23 * C5;128             return new double[] { ax, ay, az };129         }

 

 

 

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