zoukankan      html  css  js  c++  java
  • 六关节机器人的雅可比矩阵及微分运算

      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     }
  • 相关阅读:
    使用Fiddler抓取手机APP数据包--360WIFI
    mysql 查询数据库表信息,字段信息
    jQuery动态移除和绑定事件
    Topshelf+Quatz.Net的简单使用
    教你花30分钟时间搭建一个个人博客
    泛型接口的抗变和协变
    Action<T>和Func<T>
    DateTime和DateTimeOffset的区别
    Expression<Func<TObject, bool>>与Func<TObject, bool>的区别
    C#学习笔记(1) --简叙.net体系结构
  • 原文地址:https://www.cnblogs.com/xrll/p/5953008.html
Copyright © 2011-2022 走看看