机械臂（5）– C#实现正解

按照机械臂系列第四篇中的公式，用C#实现机械臂的正向求解。

用到的矩阵乘法在http://blog.csdn.net/shaynerain/article/details/77692920中

1、首先对建立D-H表，上篇中建立的有些不太符合，在此处更正

1. public class D_H  
   
2. {  
   
3.     public  double[] theta = new double[4];  
   
4.     public  double[] d = { 3, 0, 0, 2.5 };  
   
5.     public  double[] a = { 0, 6.4, 5.5, 9.4 };  
   
6.     public  double[] alpha = { (Math.PI / 2), 0, -Math.PI, 0 };  
   
7. }  
   

2、求出每个关节相对前个关节的姿态

1. public class T : D_H  
   
2.     {  
   
3.         public double[] GetAngle(double[][]tt)  
   
4.         {  
   
5.             double[] theta = new double[3];  
   
6.             theta[0] = -Math.Atan2(tt[0][2], tt[1][2]);  
   
7.             //theta[1] = Math.Atan(tt[2][0]/(Math.Cos(theta[0]) * tt[0][3] + Math.Sin(theta[0]) * tt[1][3]));  
   
8.             //theta[0] = -Math.Atan(tt[1][3]/tt[0][3]);  
   
9.    
    
10.    
     
11.             return theta;  
    
12.         }  
    
13.         public double[][] GetTii_1(int i)  
    
14.         {  
    
15.             double[][] t = new double[4][]{  
    
16.             new double[4],new double[4],new double[4],new double[4]{0,0,0,1}  
    
17.             };  
    
18.    
     
19.             t[0][0] = Math.Cos(theta[i]);  
    
20.             t[0][1] = -Math.Cos(alpha[i]) * Math.Sin(theta[i]);  
    
21.             t[0][2] = Math.Sin(alpha[i]) * Math.Sin(theta[i]);  
    
22.             t[0][3] = a[i] * Math.Cos(theta[i]);  
    
23.    
     
24.             t[1][0] = Math.Sin(theta[i]);  
    
25.             t[1][1] = Math.Cos(alpha[i]) * Math.Cos(theta[i]);  
    
26.             t[1][2] = -Math.Sin(alpha[i]) * Math.Cos(theta[i]);  
    
27.             t[1][3] = a[i] * Math.Sin(theta[i]);  
    
28.    
     
29.             t[2][0] = 0;  
    
30.             t[2][1] = Math.Sin(alpha[i]);  
    
31.             t[2][2] = Math.Cos(alpha[i]);  
    
32.             t[2][3] = d[i];  
    
33.    
     
34.             return t;  
    
35.         }  
    
36.    
     
37.     }  
    

3、正解

定义以及初始化

1. T t = new T();  
   
2.             double[][] t0 = new double[4][]{  
   
3.             new double[4],new double[4],new double[4],new double[4]{0,0,0,1}  
   
4.             };  
   
5.             double[][] t1 = new double[4][]{  
   
6.             new double[4],new double[4],new double[4],new double[4]{0,0,0,1}  
   
7.             };  
   
8.             double[][] t2 = new double[4][]{  
   
9.             new double[4],new double[4],new double[4],new double[4]{0,0,0,1}  
   
10.             };  
    
11.             double[][] t3 = new double[4][]{  
    
12.             new double[4],new double[4],new double[4],new double[4]{0,0,0,1}  
    
13.             };  
    
14.             double[][] tt = new double[4][]{  
    
15.             new double[4],new double[4],new double[4],new double[4]{0,0,0,1}  
    
16.             };  
    

 

赋值：

t.theta[0]= 0;

t.theta[1]= 0;

t.theta[2]= 0;

t.theta[3]= 0;

求解：

1. t0 = t.GetTii_1(0);  
   
2.                      t1 = t.GetTii_1(1);  
   
3.                      t2 = t.GetTii_1(2);  
   
4.                      t3 = t.GetTii_1(3);  
   
5.   
    
6.                      tt = MatrixMult(t0, t1);  
   
7.                      tt = MatrixMult(tt, t2);  
   
8.                      tt = MatrixMult(tt, t3);  
   
9.                      //打印  
   
10.                      PrintMatrix(tt);