天天看點
傳回

6軸機器人運動學正解,逆解1

正解 

給定機器人各關節的角度，計算出機器人末端的空間位置

逆解 

已知機器人末端的位置和姿态，計算機器人各關節的角度值

常見的工業機器人 

正解與逆解的求解需要相應的機器人運動方程，其中關鍵的就是DH參數表 

DH參數表用來描述機器人各關節坐标系之間的關系，有了DH參數表就可以在機器人各關節之間進行坐标轉換 

求解正解就是從關節1到關節5的坐标轉換 

基本知識 

關節：連接配接2個杆件的部分 

連杆長度 (a) ：2個相鄰關節軸線之間的距離 

連杆扭角 (alpha α) ：2個相鄰關節軸線之間的角度 

連杆偏距 (d) ：2個關節坐标系的X軸之間的距離 

關節角度 (theta θ) ：關節變量 計算時需要加初始角度偏移

Z軸： 關節軸線 

X軸： 從Z(i)軸指向Z(i+1)軸 與a重合 

alpha： Z(i)軸繞X(i)軸旋轉到Z(i+1)軸的角度 

a： 相鄰的2個關節軸線之間的距離是a Z(i)軸到Z(i+1)軸 

d： 2個相鄰的a之間的距離是d a(i-1)到a(i) 相鄰X軸之間的距離

關節角度 從Z軸正向看原點 逆時針旋轉為正 

如果(a=0)，X(i)與X(i-1)方向相同 

如果(a!=0)，X(i)從Z(i)指向Z(i+1)

圖2 

圖3 

DH參數表

關節    a    d    α

1    100    0    90

2    270    0    0

3    60    0    90

4    0    270    -90

5    0    0    90

6    0    0    0

DH參數有多種表示方式，與坐标系及坐标軸的設定有關

正解的計算方法 

機器人從關節1到關節6進行坐标變換，結果就是末端的位置坐标 

根據DH參數表以及關節變量的值，建立6個關節矩陣A1-A6，計算出轉換矩陣T1-T6，T1-T6相乘得出結果 

6軸機器人4,5,6軸相交于1點 正解計算隻算到第5軸 

為簡化矩陣計算，關節1坐标系原點設在Z2軸的水準平面上，最終結果在Z方向需要再加上一個偏移值

/* 4階矩陣計算機器人正解 
 * 關節角度在檔案 J1_J6中
 * 機器人參數在檔案 param_table中
 * 坐标結果在螢幕輸出 */

#include <stdio.h>
#include <math.h>
#include <string.h>

#define XYZ_F_J "./J1_J6"
#define DESIGN_DT "./param_table"
#define XYZ_F_TOOL "./tool_xyz"
#define XYZ_F_WORLD "./world_xyz"


#define RAD2ANG (3.1415926535898/180.0)
#define IS_ZERO(var) if(var < 0.0000000001 && var > -0.0000000001){var = 0;} 

#define MATRIX_1 1
#define MATRIX_N 4

#define DEF_TOOLXYZ 0  
    /* 0 沒有工具坐标系 非零 有工具坐标系 */

/*角度偏移*/
#define ANGLE_OFFSET_J2 90
#define ANGLE_OFFSET_J3 90
//#define ANGLE_OFFSET_J4 -90

typedef struct {
    double joint_v;  //joint variable
    double length;
    double d;
    double angle;
}param_t;


    double matrix_A1[MATRIX_N][MATRIX_N];
    double matrix_A2[MATRIX_N][MATRIX_N];
    double matrix_A3[MATRIX_N][MATRIX_N];
    double matrix_A4[MATRIX_N][MATRIX_N];
    double matrix_A5[MATRIX_N][MATRIX_N];
    double matrix_A6[MATRIX_N][MATRIX_N];

double matrix_worldxyz[MATRIX_N][MATRIX_N];
double matrix_toolxyz[MATRIX_N][MATRIX_N];


void initmatrix_A(param_t *p_table);
void calculate_matrix_A(double matrix[MATRIX_N][MATRIX_N], param_t *p_param);

int matrix_mul(double matrix_a[MATRIX_N][MATRIX_N],
            double matrix_b[MATRIX_N][MATRIX_N],
            double matrix_result[MATRIX_N][MATRIX_N]);

int matrix_add(double matrix_a[MATRIX_N][MATRIX_N], 
            double matrix_b[MATRIX_N][MATRIX_N], 
            double matrix_sum[MATRIX_N][MATRIX_N], int m, int n);

void matrix_copy(double matrix_a[MATRIX_N][MATRIX_N], 
            double matrix_b[MATRIX_N][MATRIX_N], int m, int n);

void initmatrix_tool(double toolx, double tooly, double toolz);

void printmatrix(double matrix[MATRIX_N][MATRIX_N], int m, int n)
{
    int i, j;

    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            printf(" %lf ", matrix[i][j]);
        }
        printf("\n");
    }
    printf("\n");

}

void printmatrix_1(double matrix[MATRIX_N][1], int m, int n)
{
    int i, j;

    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            printf(" %lf ", matrix[i][j]);
        }
        printf("\n");
    }
    printf("\n");

}


int main()
{

    double matrix_T1[MATRIX_N][MATRIX_N];
    double matrix_T2[MATRIX_N][MATRIX_N];
    double matrix_T3[MATRIX_N][MATRIX_N];
    double matrix_T4[MATRIX_N][MATRIX_N];
    double matrix_T5[MATRIX_N][MATRIX_N];
    double matrix_T6[MATRIX_N][MATRIX_N];

    //double j1=0, j2=0, j3=0, j4=0, j5=0, j6=0;
    //double l1=0, l2=0, l3=0, d4=0, z_offset=0;
    double toolx=0, tooly=0, toolz=0, toolrx=0, toolry=0, toolrz=0;
    double worldx=0, worldy=0, worldz=0, worldrx=0, worldry=0, worldrz=0;
    double z_offset=0;

    param_t param_table[6] ={0};
    memset(param_table, 0, sizeof(param_table) );

    FILE * fp=NULL;

    fp=fopen(XYZ_F_J, "r");
    if(fp== NULL){
        perror("open J1_J6 file error\n");
        return 0;
    }
    fscanf(fp, "%lf%lf%lf%lf%lf%lf", 
                &param_table[0].joint_v, 
                &param_table[1].joint_v, 
                &param_table[2].joint_v, 
                &param_table[3].joint_v, 
                &param_table[4].joint_v, 
                &param_table[5].joint_v 
                );
    printf("j1...j6\n%lf %lf %lf %lf %lf %lf\n",
                param_table[0].joint_v, 
                param_table[1].joint_v, 
                param_table[2].joint_v, 
                param_table[3].joint_v, 
                param_table[4].joint_v, 
                param_table[5].joint_v 
                );


    //加初始角度偏移 j2 j3 
    param_table[1].joint_v += ANGLE_OFFSET_J2;
    param_table[2].joint_v += ANGLE_OFFSET_J3;

    //将機器人關節角度轉換成弧度
    param_table[0].joint_v *= RAD2ANG;
    param_table[1].joint_v *= RAD2ANG;
    param_table[2].joint_v *= RAD2ANG;
    param_table[3].joint_v *= RAD2ANG;
    param_table[4].joint_v *= RAD2ANG;
    param_table[5].joint_v *= RAD2ANG;

    printf("\nj1...j6 RAD2ANG\n%lf %lf %lf %lf %lf %lf\n", 
                param_table[0].joint_v, 
                param_table[1].joint_v, 
                param_table[2].joint_v, 
                param_table[3].joint_v, 
                param_table[4].joint_v, 
                param_table[5].joint_v 
          );

    fclose(fp);

    fp=fopen(DESIGN_DT, "r");
    if( fp== NULL){
        perror("open param_table file error\n");
        return 0;
    }

//讀入關節參數
    int i;
    for(i=0; i<6; i++){
        fscanf(fp, "%lf%lf%lf", 
                    &param_table[i].length, 
                    &param_table[i].d, 
                    &param_table[i].angle );
    }
    fscanf(fp, "%lf", &z_offset );
    fclose(fp);

    param_table[0].angle *= RAD2ANG;
    param_table[1].angle *= RAD2ANG;
    param_table[2].angle *= RAD2ANG;
    param_table[3].angle *= RAD2ANG;
    param_table[4].angle *= RAD2ANG;
    param_table[5].angle *= RAD2ANG;

    initmatrix_A(param_table);
/*
    //fscanf(fp, "%lf %lf %lf", &toolx, &tooly, &toolz);
    //printf("tool x y z\n%lf %lf %lf\n", toolx, tooly, toolz);

    fp=fopen(XYZ_F_TOOL, "r");
    if(fp== NULL || DEF_TOOLXYZ == 0){
        printf("no toolxyz \n");    
    }else{
        fscanf(fp, "%lf %lf %lf %lf %lf %lf", 
                    &toolx, &tooly, &toolz, &toolrx, &toolry, &toolrz);
        printf("\ntoolxyz\n%lf %lf %lf %lf %lf %lf\n", 
                    toolx, tooly, toolz, toolrx, toolry, toolrz);
        fclose(fp);
    }
    initmatrix_tool(toolx, tooly, toolz);
*/

    //計算變換矩陣 matrix T1---T6
    matrix_copy(matrix_A1, matrix_T1, MATRIX_N, MATRIX_N);
    printf("matrix_T1 =  \n");
    printmatrix(matrix_T1, MATRIX_N, MATRIX_N);

    matrix_mul(matrix_T1, matrix_A2, matrix_T2);
    printf("matrix_T2 =  \n");
    printmatrix(matrix_T2, MATRIX_N, MATRIX_N);

    matrix_mul(matrix_T2, matrix_A3, matrix_T3);
    printf("matrix_T3 =  \n");
    printmatrix(matrix_T3, MATRIX_N, MATRIX_N);

    matrix_mul(matrix_T3, matrix_A4, matrix_T4);
    printf("matrix_T4 =  \n");
    printmatrix(matrix_T4, MATRIX_N, MATRIX_N);

    matrix_mul(matrix_T4, matrix_A5, matrix_T5);
    printf("matrix_T5 =  \n");
    printmatrix(matrix_T5, MATRIX_N, MATRIX_N);

    matrix_mul(matrix_T5, matrix_A6, matrix_T6);
    printf("matrix_T6 =  \n");
    printmatrix(matrix_T6, MATRIX_N, MATRIX_N);

    //add();
    //matrix_worldxyz[2][0] +=z_offset;

    printf("\n----curent x, y, z-----\n%lf \n %lf\n %lf\n ", 
                matrix_T6[0][3], matrix_T6[1][3], 
                matrix_T6[2][3]+z_offset);


}

void initmatrix_A(param_t *p_table)
{//計算關節坐标矩陣 matrix A1--A6
    calculate_matrix_A(matrix_A1, p_table+0);
    printf("matrix_A1 =  \n");
    printmatrix(matrix_A1, MATRIX_N, MATRIX_N);

    calculate_matrix_A(matrix_A2, p_table+1);
    printf("matrix_A2 =  \n");
    printmatrix(matrix_A2, MATRIX_N, MATRIX_N);

    calculate_matrix_A(matrix_A3, p_table+2);
    printf("matrix_A3 =  \n");
    printmatrix(matrix_A3, MATRIX_N, MATRIX_N);

    calculate_matrix_A(matrix_A4, p_table+3);
    printf("matrix_A4 =  \n");
    printmatrix(matrix_A4, MATRIX_N, MATRIX_N);

    calculate_matrix_A(matrix_A5, p_table+4);
    printf("matrix_A5 =  \n");
    printmatrix(matrix_A5, MATRIX_N, MATRIX_N);

    calculate_matrix_A(matrix_A6, p_table+5);
    printf("matrix_A6 =  \n");
    printmatrix(matrix_A6, MATRIX_N, MATRIX_N);
}

void calculate_matrix_A(double matrix[MATRIX_N][MATRIX_N], param_t *p_param)
{//根據關節參數計算矩陣 
    double *pmatrix=(double *)matrix;
    double value, var_c, var_s, angle_c, angle_s;

    var_c = cos(p_param->joint_v);
    IS_ZERO(var_c);
    var_s = sin(p_param->joint_v);
    IS_ZERO(var_s);
    angle_c = cos(p_param->angle);
    IS_ZERO(angle_c);
    angle_s = sin(p_param->angle);
    IS_ZERO(angle_s);

    *pmatrix++ = var_c;
    //value = -var_s * angle_c;
    //IS_ZERO(value);
    *pmatrix++ = -var_s * angle_c;
    //value = var_s * angle_s;
    //IS_ZERO(value);
    *pmatrix++ = var_s * angle_s;
    //value = p_param->length * var_c;
    //IS_ZERO(value);
    *pmatrix++ = p_param->length * var_c;

    *pmatrix++ = var_s;
    //value = var_c * angle_c;
    //IS_ZERO(value);
    *pmatrix++ = var_c * angle_c;
    //value = -var_c *angle_s;
    //IS_ZERO(value);
    *pmatrix++ = -var_c *angle_s;
    //value = p_param->length * var_s;
    //IS_ZERO(value);
    *pmatrix++ = p_param->length * var_s;

    *pmatrix++ =0;
    *pmatrix++ = angle_s;
    *pmatrix++ = angle_c;
    *pmatrix++ = p_param->d;

    *pmatrix++ =0;
    *pmatrix++ =0;
    *pmatrix++ =0;
    *pmatrix =1;

}

void initmatrix_tool(double toolx, double tooly, double toolz)
{
    if(DEF_TOOLXYZ == 0){
        matrix_toolxyz[2][0] =1;
    }else{
        matrix_toolxyz[0][0] =toolx;
        matrix_toolxyz[1][0] =tooly;
        matrix_toolxyz[2][0] =toolz;

        {/* 初始化 toolrx, tooly, toolz */}
    }

}

int matrix_mul(double matrix_a[MATRIX_N][MATRIX_N], 
            double matrix_b[MATRIX_N][MATRIX_N], 
            double matrix_result[MATRIX_N][MATRIX_N])
{
    int i,j,k;
    double sum;
    double matrix_tmp[MATRIX_N][MATRIX_N]={0};

    /*嵌套循環計算結果矩陣（m*p）的每個元素*/
    for(i=0;i<MATRIX_N;i++)
      for(j=0;j<MATRIX_N;j++)
      {   
          /*按照矩陣乘法的規則計算結果矩陣的i*j元素*/
          sum=0;
          for(k=0;k<MATRIX_N;k++)
            sum += matrix_a[i][k] * matrix_b[k][j];
          matrix_tmp[i][j] = sum;
      }   
    matrix_copy(matrix_tmp, matrix_result, MATRIX_N, MATRIX_N);
    return 0;
}

int matrix_add(double matrix_a[MATRIX_N][MATRIX_N], 
            double matrix_b[MATRIX_N][MATRIX_N], 
            double matrix_sum[MATRIX_N][MATRIX_N], int m, int n)
{
    int i,j;
    double matrix_tmp[MATRIX_N][MATRIX_N]={0};

    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            matrix_tmp[i][j] = matrix_a[i][j] + matrix_b[i][j];
        }
    }
    matrix_copy(matrix_tmp, matrix_sum, MATRIX_N, MATRIX_N);

    return 0;
}


void matrix_copy(double matrix_src[MATRIX_N][MATRIX_N], 
            double matrix_des[MATRIX_N][MATRIX_N], int m, int n)
{
    int i,j;
    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            matrix_des[i][j] = matrix_src[i][j];
        }
    }
}

原文：https://blog.csdn.net/weixin_37942267/article/details/78806448 

控制系統機器人數控底層算法 機器人運動學
上一篇: 6軸機器人運動學正解,逆解2
下一篇: 機械臂正逆運動學-----SRS型機械臂解析解臂形角參數化原理講解源代碼

繼續閱讀