6轴机器人运动学正解,逆解1

最新推荐文章于 2022-07-27 15:48:32 发布

ACE2008888

最新推荐文章于 2022-07-27 15:48:32 发布

阅读量3.5k

点赞数

分类专栏：控制系统机器人数控底层算法文章标签：机器人运动学

控制系统机器人数控底层算法专栏收录该内容

4 篇文章 2 订阅

订阅专栏

正解
给定机器人各关节的角度，计算出机器人末端的空间位置

逆解
已知机器人末端的位置和姿态，计算机器人各关节的角度值

常见的工业机器人

正解与逆解的求解需要相应的机器人运动方程，其中关键的就是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

ACE2008888

关注

0
点赞
踩
21

收藏

觉得还不错? 一键收藏
1
评论
6轴机器人运动学正解,逆解1

正解给定机器人各关节的角度，计算出机器人末端的空间位置逆解已知机器人末端的位置和姿态，计算机器人各关节的角度值常见的工业机器人正解与逆解的求解需要相应的机器人运动方程，其中关键的就是DH参数表DH参数表用来描述机器人各关节坐标系之间的关系，有了DH参数表就可以在机器人各关节之间进行坐标转换求解正解就是从关节1到关节5的坐标转换基本知识关节：连接2个杆件的部分...
复制链接

扫一扫