精华内容
参与话题
问答
  • Delta型并联机器人运动学正解 几何解法pdf,并联机器 人运 动学正 解的封闭解 问题到 目前 为止没有得 到全面 解决, 常用 的解决方 案是采 用基于 代 数万 程组 的数值解法 , 该 方 法 不 足 之 处是推导过程复杂 , ...
  • Delta Robot Kinematics 并联机器人 运动学 正解 逆解 本人自己总结 matlab亲测 正反解正确
  • 基于改进DH参数的机器人正解和逆解程序,逆解采用解析解形式,输出8组关节角度解
  • puma560的运动学正解和逆解的C++源码.cpp puma560的运动学正解和逆解的C++源码.cpp puma560的运动学正解和逆解的C++源码.cpp
  • 采用机器人D-H坐标变换法,建立了PUMA560机器人运动学正解的数学模型。借助AutoCAD平台对工件中心位置和姿态进行测量,使得数学模型和CAD运动仿真得到了较全面的相互验证,对工业机器人运动性能研究具有积极意义。
  • 6自由度机器人运动学正反C++程序

    热门讨论 2015-07-26 21:14:10
    6自由度机器人运动学正、反C++程序,可直接运行,简单易懂!
  • 六自由度机器人运动学正反求解过程,十分详细,机器人运动学
  • 牛顿迭代求解串联机器人运动学正解
  • 为分析3-PRS并联机构复杂的正向运动问题,判定出一组利用数值法求解出来的合适正解,建立了一种3-PRS并联机构的正向和逆向运动学数学模型,并对正向运动进行了解析,将各组正解所对应的3-PRS并联机构位形进行可视化分析...
  • 6轴机器人运动学正解,逆解1

    万次阅读 多人点赞 2017-12-14 21:27:35
    正解与逆解的求解需要相应的机器人运动方程,其中关键的就是DH参数表 DH参数表用来描述机器人各关节坐标系之间的关系,有了DH参数表就可以在机器人各关节之间进行坐标转换 求解正解就是从关节1到关节5的坐标转换 ...

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

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

    常见的工业机器人

    正解与逆解的求解需要相应的机器人运动方程,其中关键的就是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];
            }
        }
    }
    
    
    
    
    
    
    展开全文
  • Delta机构机器人算法资料 Delta机构机器人算法资料 Delta机构机器人算法资料
  • 6轴机器人运动学正解,逆解2

    千次阅读 2017-12-15 17:37:04
    机器人建模、规划与控制 西安交通大学出版社对于关节1,2,3可以从运动方程手工推导出各个关节旋转角度的计算公式 逆求解的结果并不是唯一的 可能有多组 /*计算逆 根据机器人坐标计算机器人关节角度 ...

    逆解
    逆解计算方法可以参考以下书籍
    机器人学导论——分析、系统及应用 电子工业出版社
    机器人学导论第3版 机械工业出版社
    机器人学建模、规划与控制 西安交通大学出版社

    对于关节1,2,3可以从运动方程手工推导出各个关节旋转角度的计算公式

    逆解求解的结果并不是唯一的 可能有多组解

    /*计算逆解 根据机器人坐标计算机器人关节角度 
     *关节参数在文件 param_table中
     *机器人坐标在文件 xyzrpy中
     *计算结果在屏幕输出 */
    
    
    
    #include <stdio.h>
    #include <math.h>
    #include <string.h>
    
    #define XYZ_F_3D "./xyzrpy"
    #define DESIGN_DT "./param_table"
    #define XYZ_F_TOOL "./tool_xyz"
    
    #define PI (3.1415926535898)
    #define ANG2RAD_EQU(N) (N *= (180.0/3.1415926535898) )
    #define ANG2RAD(N) ( (N) * (180.0/3.1415926535898) )
    #define RAD2ANG (3.1415926535898/180.0)
    #define IS_ZERO(var) if(var < 0.0000000001 && var > -0.0000000001){var = 0;} 
    // #define IS_ZERO(var) ( (var) < 0.0000000001 && (var) > -0.0000000001 )?0 :1 
    #define JUDGE_ZERO(var) ( (var) < 0.0000000001 && (var) > -0.0000000001 )
    
    #define MATRIX_1 1
    #define MATRIX_M 4
    #define MATRIX_N 4
    
    #define ANGLE_OFFSET_J2 90
    #define ANGLE_OFFSET_J3 90
    
    
    typedef struct {
        double joint_v;  //joint variable
        double length;
        double d;
        double angle;
    }param_t;
    
    param_t param_table[6] ={0};
    double worldx =0, worldy =0, worldz =0, 
           worldrr =0, worldrp =0, worldry =0;
    double z_offset=0;
    
    void printmatrix(double matrix[MATRIX_N][MATRIX_N], int m, int n);
    
    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 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 calculate_matrix_R(double worldrr, double worldrp, double worldry,
                double (*matrix_R)[MATRIX_N]);
    void calculate_matrix_A(double matrix[MATRIX_N][MATRIX_N], 
                param_t *p_param);
    int judge(double j1, double j2, double j3);
    void matrix_translate(double matrix[MATRIX_M][MATRIX_N], int m, int n);
    void fun_zyz(double matrix_R[MATRIX_N][MATRIX_N], 
                double *p_r,  double *p_p, double *p_y);
    int fun_j456(double  j1, double j2, double j3,
                param_t *p_table,double p_matrix_R[MATRIX_N][MATRIX_N],
                double *p_j4, double *p_j5, double *p_j6);
    
    int fun_j2(double j1, double *p_j2, 
                double a1, double a2, double a3, double d4,   
                double px, double py, double pz )
    {//计算关节2的角度 
        double v1_c, v1_s, v2_c, v2_s;
        double var_M, var_K, tmp;
        double var_sqrt[2] = {0};
    
        v1_c =cos(j1);
        IS_ZERO(v1_c);
        v1_s =sin(j1);
        IS_ZERO(v1_s);
        var_M = v1_c*px + v1_s*py - a1;
        var_K = (d4*d4 + a3*a3 - a2*a2 - pz*pz - var_M*var_M) / (-2 * a2);
        tmp = var_M*var_M + pz*pz - var_K*var_K;
        IS_ZERO(tmp);
        if( tmp >=0 ){
        //if( (var_M*var_M + pz*pz - var_K*var_K) >=0){
            //var_sqrt[0] = sqrt(var_M*var_M + pz*pz - var_K*var_K);
            var_sqrt[0] = sqrt(tmp);
            var_sqrt[1] = -var_sqrt[0];
        }else{
            printf("m^2 + z^2 - k^2 <0 : %lf\n", tmp);
            p_j2[0] =0, p_j2[1] =0;
            return 0;
        }
    
        p_j2[0] = -atan2(var_M, pz) + atan2(var_K, var_sqrt[0]);
        p_j2[1] = -atan2(var_M, pz) + atan2(var_K, var_sqrt[1]);
        return 1;
    }
    
    int fun_j3(double j1, double j2, double *p_j3,
                double a1, double a3, double d4,
                double px, double py, double pz)
    {//计算关节3的角度 
        double var_K, tmp;
        double var_sqrt[2];
        double v1_c, v1_s, v2_c, v2_s;
        v1_c = cos(j1);
        IS_ZERO(v1_c);
        v1_s = sin(j1);
        IS_ZERO(v1_s);
        v2_c = cos(j2);
        IS_ZERO(v2_c);
        v2_s = sin(j2);
        IS_ZERO(v2_s);
    
        var_K = -v2_s*v1_c*px - v1_s*v2_s*py + v2_c*pz + v2_s*a1;
        IS_ZERO(var_K);
    
        tmp = d4*d4 + a3*a3 - var_K*var_K;
        IS_ZERO(tmp);
    
        if( tmp >=0 ){
            var_sqrt[0] = sqrt(tmp);
            var_sqrt[1] = -var_sqrt[0];
            p_j3[0] = atan2(d4, a3) + atan2(var_K, var_sqrt[0]);
            p_j3[1] = atan2(d4, a3) + atan2(var_K, var_sqrt[1]);
        }else{
            printf("m^2 + z^2 - k^2 <0 : %lf\n", d4*d4 + a3*a3 - var_K*var_K);
            p_j3[0] =0; p_j3[1] = 0;
            return 0;
        }
        return 1;
    }
    
    /* 计算过程 根据运动方程 计算矩阵 列出等式 计算 j1 j2 j3
     * 计算旋转矩阵 根据 j1 j2 j3 计算T3 并转置 与旋转矩阵相乘 3*3
     * 计算zyz 就是 j4 j5 j6 */
    int main()
    {
        double matrix_R[MATRIX_N][MATRIX_N];
    
        double j1[2] = {0};  //元素值 >=360 度或 < -360 度 表示角度无效
        double j2[4] = {0};
        double j3[8] = {0};
        double j4[8] = {0};
        double j5[8] = {0};
        double j6[8] = {0};
    
        int i, j;
    //  double z_offset=0;
    //  memset(param_table, 0, sizeof(param_table) );
    
        FILE * fp=NULL;
        fp=fopen(XYZ_F_3D, "r");
        if(fp== NULL){
            perror("open xyzrpy file error\n");
            return 0;
        }
        fscanf(fp, "%lf%lf%lf%lf%lf%lf",
                    &worldx, &worldy, &worldz, &worldry, &worldrp, &worldrr);
        fclose(fp);
    
        printf("worldx: %lf worldy: %lf worldz: %lf\nworldry: %lf worldrp: %lf worldrr: %lf\n",
              worldx, worldy, worldz, worldry, worldrp, worldrr);
    
        fp=fopen(DESIGN_DT, "r");
        if( fp== NULL){
            perror("open param_table file error\n");
            return 0;
        }
    
        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;
    
        calculate_matrix_R(worldrr, worldrp, worldry, matrix_R);
        matrix_R[0][3] = worldx;
        matrix_R[1][3] = worldy;
        matrix_R[2][3] = worldz-z_offset;
        matrix_R[3][0] = 0;
        matrix_R[3][1] = 0;
        matrix_R[3][2] = 0;
        matrix_R[3][3] = 1;
        printmatrix(matrix_R, MATRIX_N, MATRIX_N);
    
        //double var_M, var_K;
        //double var_sqrt[2];
        double a1 = param_table[0].length;
        double a2 = param_table[1].length;
        double a3 = param_table[2].length;
        double d4 = param_table[3].d;
        double px = matrix_R[0][3];
        double py = matrix_R[1][3];
        double pz = matrix_R[2][3];
    
        double v1_c, v1_s, v2_c, v2_s;
    
        //计算 j1
        j1[0] = atan2(worldy, worldx); 
        IS_ZERO( j1[0] );
        //ANG2RAD_EQU(j1[0]);
        j1[1] = j1[0] +PI;
        JUDGE_ZERO(j1[1] -2*PI)? (j1[1] = 0) : 1;
        //j1[1] = JUDGE_ZERO(j1[1] -2*PI)? j1[1] = 0: 1;
        printf("j1: \n  %lf , %lf\n", ANG2RAD(j1[0]), ANG2RAD(j1[1]) );
    
        //计算 j2
        int v_bool;
        v_bool = fun_j2(j1[0], j2, a1, a2, a3, d4, px, py, pz);
        if(v_bool)
            printf("j2: %lf, %lf\n", ANG2RAD(j2[0])-90, ANG2RAD(j2[1])-90 );
        else{
            printf("this j2 invalid\n");
            j2[0] =2*PI; j2[1] =2*PI;
    //      j2[0]>0 ? (j2[0] += 2*PI): (j2[0] -= 2*PI) ;    
    //      j2[1]>0 ? (j2[1] += 2*PI): (j2[1] -= 2*PI) ;    
        }
    
        v_bool = fun_j2(j1[1], j2+2, a1, a2, a3, d4, px, py, pz);
        if(v_bool)
          printf("j2: %lf, %lf\n", ANG2RAD(j2[2])-90, ANG2RAD(j2[3])-90 );
        else{
            printf("this j2 invalid\n");
            j2[2] =2*PI; j2[3] =2*PI;
        }
    
        //计算 j3
        for(i=0; i<8; i+=2){
            v_bool = fun_j3(j1[i/4], j2[i/2], j3+i, a1, a3, d4, px, py, pz);
            if(v_bool)
              printf("j3: %lf, %lf\n", 
                          ANG2RAD(j3[i])-90, ANG2RAD(j3[i+1])-90 );
            else {
                printf("this j3 invalid\n");
                j3[i] =2*PI; j3[i+1] =2*PI;
                //j3[k]>0 ? (j3[k] += 2*PI): (j3[k] -= 2*PI) ;
                //j3[k+1]>0 ? (j3[k+1] += 2*PI): (j3[k+1] -= 2*PI) ;
            }
        }
    
    printf("judge\n");
        for(i=0; i<8; i++){
            printf("j1[%d]: %lf, j2[%d]: %lf, j3[%d]: %lf\n",
                        i/4, j1[i/4], i/2, j2[i/2], i, j3[i]);
    
            //if(j1[i/4]==2*PI || j2[i/2]==2*PI || j3[i]==2*PI) continue;
    
            if( !judge(j1[i/4], j2[i/2], j3[i]) ) { 
                j3[i]>=0 ? (j3[i] += 2*PI): (j3[i] -= 2*PI) ; }
        }
    
        printf("\nj1: %lf, %lf\nj2: %lf, %lf, %lf, %lf\n", 
                ANG2RAD(j1[0]), ANG2RAD(j1[1]),
                ANG2RAD(j2[0])-90, ANG2RAD(j2[1])-90, 
                ANG2RAD(j2[2])-90, ANG2RAD(j2[3])-90 );
        printf("j3:\n");
        for(i=0; i<8; i++){
            printf(" %lf ", ANG2RAD(j3[i])-90);
            if( (i+1)%4 ==0 )printf("\n");
        }
    
    //计算 j4 j5 j6   
        for(i=0, j=0; i<8; i++){
            if(j3[i] >= 2.0*PI || j3[i] < -2.0*PI) continue;
            printf("\n----j1[%d]: %lf j2[%d]: %lf j3[%d]: %lf\n", 
                        i/4, ANG2RAD(j1[i/4]), 
                        i/2, ANG2RAD(j2[i/2])-90, 
                        i, ANG2RAD(j3[i])-90 );
    
            fun_j456(j1[i/4], j2[i/2], j3[i], param_table, matrix_R, 
                        &j4[j], &j5[j], &j6[j]);
            printf("j4: %lf, %lf\nj5: %lf, %lf\nj6: %lf, %lf\n",
                        ANG2RAD(j4[j]), ANG2RAD(j4[j+1]),
                        ANG2RAD(j5[j]), ANG2RAD(j5[j+1]), 
                        ANG2RAD(j6[j]), ANG2RAD(j6[j+1]) );
            j +=2;
        }
    
    }
    
    void calculate_matrix_R(double angle_r, double angle_p, double angle_y,
                double (*matrix_R)[MATRIX_N])
    {
    /*计算旋转矩阵 */
        int i,j;
        double mtmp;
        double r_c, r_s, p_c, p_s, y_c, y_s;
    
        angle_r *= RAD2ANG;
        angle_p *= RAD2ANG;
        angle_y *= RAD2ANG;
    
        r_c = cos( angle_r );
        IS_ZERO(r_c);
        r_s = sin( angle_r );
        IS_ZERO(r_s);
        p_c = cos( angle_p );
        IS_ZERO(p_c);
        p_s = sin( angle_p );
        IS_ZERO(p_s);
        y_c = cos( angle_y );
        IS_ZERO(p_c);
        y_s = sin( angle_y );
        IS_ZERO(y_s);
    
        matrix_R[0][0] = r_c * p_c;
        matrix_R[0][1] = r_c * p_s * y_s - r_s * y_c;
        matrix_R[0][2] = r_c * p_s * y_c + r_s * y_s;
    
        matrix_R[1][0] = r_s * p_c;
        matrix_R[1][1] = r_s * p_s * y_s + r_c * y_c;
        matrix_R[1][2] = r_s * p_s * y_c - r_c * y_s;
    
        matrix_R[2][0] = -p_s;
        matrix_R[2][1] = p_c * y_s;
        matrix_R[2][2] = p_c * y_c;
    
    }
    
    int judge(double j1, double j2, double j3)
    {
        /* j1 j2 j3 是弧度 j2 j3 已加90度 */
        double x, y, z, tmp;
        j2 -= 0.5*PI;
        j3 -= 0.5*PI;
    
        //计算x
        tmp = -sin(j2);
        IS_ZERO(tmp);
        x = tmp * param_table[1].length;
    
        tmp = cos(j2+j3);
        IS_ZERO(tmp);
        x -= param_table[2].length * tmp;
    
        tmp = -sin(j2+j3);
        IS_ZERO(tmp);
        x +=tmp* param_table[3].d;
        x += param_table[0].length; 
        y = x;
    
        tmp =cos(j1);
        IS_ZERO(tmp);
        x *=tmp;
        //计算y
        tmp =sin(j1);
        IS_ZERO(tmp);
        y *=tmp;
        //计算z
        tmp = cos(j2);
        IS_ZERO(tmp);
        z = param_table[1].length*tmp;
    
        tmp = sin(j2+j3);
        IS_ZERO(tmp);
        z -=param_table[2].length*tmp;
    
        tmp = cos(j2+j3);
        IS_ZERO(tmp);
        z += param_table[3].d *tmp +z_offset;
    
        //printf("%lf %lf %lf\n", x, y, z);
        tmp = x - worldx;
        if( tmp > 0.0000000001 || tmp < -0.0000000001 ) return 0;
    //  if( !(tmp < 0.0000000001 && tmp > -0.0000000001) ) return 0;
        tmp = y - worldy;
        if( tmp > 0.0000000001 || tmp < -0.0000000001 ) return 0;
        tmp = z - worldz;
        if( tmp > 0.0000000001 || tmp < -0.0000000001 ) return 0;
        return 1;
    
    }
    
    int fun_j456(double  j1, double j2, double j3, 
                param_t *p_table, double p_matrix_R[MATRIX_N][MATRIX_N], 
                double *p_j4, double *p_j5, double *p_j6)
    {
        double matrix_a[MATRIX_N][MATRIX_N], matrix_b[MATRIX_N][MATRIX_N];
        double matrix_tmp[MATRIX_N][MATRIX_N];
    
        //printf("j1: %lf j2: %lf j3:  %lf\n", j1, j2, j3);
        p_table[0].joint_v = j1;
        p_table[1].joint_v = j2;
        p_table[2].joint_v = j3;
    
        calculate_matrix_A(matrix_a, p_table+0);
        calculate_matrix_A(matrix_b, p_table+1);
        matrix_mul(matrix_a, matrix_b, matrix_tmp, MATRIX_N, MATRIX_N);
    
        calculate_matrix_A(matrix_b, p_table+2);
        matrix_mul(matrix_tmp, matrix_b, matrix_a, MATRIX_N, MATRIX_N);
        matrix_translate(matrix_a, MATRIX_N-1, MATRIX_N-1);
    
        matrix_mul(matrix_a, p_matrix_R, matrix_b, MATRIX_N-1, MATRIX_N-1);
    
        fun_zyz(matrix_b, p_j4, p_j5, p_j6);    
    
    }
    
    void fun_zyz(double matrix_R[MATRIX_N][MATRIX_N], 
                double *p_r,  double *p_p, double *p_y)
    {
        double mtmp =sqrt(matrix_R[0][2]*matrix_R[0][2] + 
                    matrix_R[1][2]*matrix_R[1][2]);
    
    //  printf("ZYZ  \n--- > -pi  and < 0\n");
        p_r[0] = atan2( matrix_R[1][2], matrix_R[0][2]);
        p_p[0] = atan2( mtmp, matrix_R[2][2]);
        p_y[0] = atan2( matrix_R[2][1], -matrix_R[2][0] );
    
    //  printf("ZYZ  \n--- > -pi  and < 0\n");
        p_r[1] = atan2( -matrix_R[1][2], -matrix_R[0][2]);
        p_p[1] = atan2( -mtmp, matrix_R[2][2]);
        p_y[1] = atan2( -matrix_R[2][1],  matrix_R[2][0] );
    
    }
    
    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;
        *pmatrix++ = -var_s * angle_c;
        *pmatrix++ = var_s * angle_s;
        *pmatrix++ = p_param->length * var_c;
    
        *pmatrix++ = var_s;
        *pmatrix++ = var_c * angle_c;
        *pmatrix++ = -var_c *angle_s;
        *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 matrix_copy(double matrix_a[MATRIX_N][MATRIX_N],
                double matrix_b[MATRIX_N][MATRIX_N], int m, int n)
    {
        int i,j;
        for(i=0; i<m; i++){
            for(j=0; j<n; j++){
                matrix_b[i][j] = matrix_a[i][j];
            }
        }
    }
    
    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 m, int n)
    {
        int i,j,k;
        double sum;
        double matrix_tmp[MATRIX_N][MATRIX_N]={0};
    
        /*嵌套循环计算结果矩阵(m*p)的每个元素*/
        for(i=0; i<m; i++)
          for(j=0; j<n; j++)
          {
              /*按照矩阵乘法的规则计算结果矩阵的i*j元素*/
              sum=0;
              for(k=0; k<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;
    }
    
    void matrix_translate(double matrix[MATRIX_M][MATRIX_N], int m, int n)
    {//矩阵转置
        double m_tmp;
        int i, j, k;
    
        for(i=0, j=0; i<m; i++, j++){
            for(k=j; k<n; k++){
                if(i == k) continue;
                m_tmp = matrix[i][k];
                matrix[i][k] = matrix[k][i];
                matrix[k][i] = m_tmp;
            }
        }
    }
    
    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");
    }
    
    
    展开全文
  • 首相用OpenGL对机械臂进行事物仿真,添加了机械臂正反的算法研究。
  • 关于机器人运动学正解网上和机器人相关书籍上都是通过建立连杆坐标系和DH参数推导出来的,推导过程比较繁琐,本人不是从事机器人专业,我在推导机器人运动学正解的时候还不知道有DH参数一说,我的算法原理是运用...
    • 关于机器人运动学正解网上和机器人相关书籍上都是通过建立连杆坐标系和DH参数推导出来的,推导过程比较繁琐,本人不是从事机器人专业,我在推导机器人运动学正解的时候还不知道有DH参数一说,我的算法原理是运用计算机图形学中三维几何变换矩阵推导的,过程比较直观,通俗易懂。

    我们知道,三维空间中平移(tx,ty,tz)距离对应的齐次变换矩阵为:                                   

    绕x轴旋转θ角对应的矩阵为:

    绕y轴旋转θ角对应的矩阵为:                     

    绕z轴旋转θ角对应的矩阵为: 

    机器人各个关节的运动无非是绕某个轴线旋转,因此我们可以利用上面4个矩阵来推导机器人运动学正解。

    首先给出机器人的结构模型:

    我们发现,轴1其实绕z轴旋转,因此轴1对应的旋转矩阵为:

    轴2是绕平行于y轴的一个轴线旋转,这里我们需要用到UG或者其他的三维软件打开机器人3D模型查看旋转中心的坐标值,这里我们姑且用一个常量代替:(c2xc2yc2z),由于轴2的旋转轴不是y轴,因此不能直接套用绕y轴的旋转矩阵,但我们可以通过平移将其移动到与y轴重合的位置,然后再绕y轴旋转给定的关节角度,最后将其平移回原来的位置,这样可以通过3个变换操作得到轴2对应的旋转矩阵。

    首先将轴2平移使其与y轴重合,平移向量为(-c2x-c2y-c2z),对应的矩阵为:                                                       

    然后再绕y轴旋转给定的关节角度θ2,对应的矩阵为:             

    最后将其平移回原来的位置:

    因此最终轴2对应的旋转矩阵为上述3个矩阵的复合效果,即:

    注意这里是矩阵的左乘,从右往左读。

    轴3也是绕平行于y轴的轴线旋转,按照同样的方法,先平移,再旋转,最后再平移可以的得到轴3的旋转矩阵:

    轴4是绕平行于z轴的轴线旋转,按照上述方法可以得到轴4的旋转矩阵:

    轴5也是绕平行于y轴的轴线旋转:

    轴6是绕平行于z轴的轴线旋转:

     

    因此最终的所求的旋转矩阵为:

    在这里我们的机器人的初始姿态最好是零位状态,即每个关节角度为0的状态,方便我们分析问题,看看各个旋转轴是不是与坐标轴平行,如果不平行,此时还需要进行使旋转轴与某一选定坐标轴(x,y或z轴)对齐的旋转操作,再将此轴变回原来的位置。

     

    展开全文
  • Delta机器人:运动学正反分析

    千次阅读 2020-06-28 11:41:22
    Delta机器人:运动学正反分析 一、Delta机构简介 Delta机构是并联机构中的一种典型机构,起原始结构如图1所示。Delta机构由R.Clavel 博士在 1985年发明,是现在并联机器人中使用最多的一种,具备了并联机构所特有...

    Delta机器人:运动学正反解分析

    一、Delta机构简介

    Delta机构是并联机构中的一种典型机构,起原始结构如图1所示。Delta机构由R.Clavel 博士在 1985年发明,是现在并联机器人中使用最多的一种,具备了并联机构所特有的优点,负载能力强、效率高、末端执行器精度高、运动惯性小,可以高速稳定运动等。因此在机器人领域获得了越来越广泛的应用。以实现高速、精准、高效的运动。
    图1 R.Clavel 博士发明的Delta机构
                                                  1 R.ClavelDelta\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图1\ R.Clavel 博士发明的Delta机构

    二、数学模型建立

    建立Delta机构简化数学模型如图2所示,其中圆OΟ所在平面为定平台,圆pp所在平面为动平台,C1C2C3∆C_1C_2C_3A1A2A3∆A_1A_2A_3为等边三角形,点C1C2C3A1A2A3C_1、C_2、C_3、A_1、A_2、A_3分别为三个主动臂和三个从动臂与上下两个平台的连接点。
    在这里插入图片描述
                                                  2 Delta\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图2\ Delta机构简化数学模型
    如图2所示,以定平台中心OΟ建立坐标系OXYZΟ-XYZ,以动平台中心pp建立坐标系pxyzp-xyz。由Delta机构的设计原理可知,三条支链完全对称,因此不妨设第ii=1,2,3i(i=1,2,3)条支链的主动臂BiCi\left|B_iC_i\right|长度为LL,从动臂AiBi\left|A_iB_i\right|长度为ll,主动臂与定平台夹角为θi\theta_i,三条支链与X轴的夹角为φi=(2(i1)π/3)i=1,2,3\varphi_i=\left(2\left(i-1\right)\pi/3\right),i=1,2,3,定平台半径为R,动平台半径为rr

    三、运动学正解

    Delta机构的正解,是在已知三个主动臂转角的情况下求出动平台中心点pp在定平台所在坐标系中的坐标。Delta机构运动学正解的求法有很多种,此处采取几何解法,将AiBiA_iB_i分别沿AipA_ip平移使其交于点pp得到DipD_ip,连接D1D2D_1D_2D2D3D_2D_3D3D1D_3D_1得到四棱锥pD1D2D3p{-D}_1D_2D_3,如图3所示。
    在这里插入图片描述
                                        3 Delta\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图3\ Delta机构几何法正解简化模型
    根据上图不难得到,定平台三个铰接点C1C2C3C_1、C_2、C_3的坐标为
                                                    [xiyizi]=[RcosφiRsinφi0]i=1,2,3                    (1)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}R\cos{\varphi_i}\\R\sin{\varphi_i}\\0\\\end{matrix}\right],i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1)
    向量OBi\vec{OB_i}可表示为
                                                  OBi=OCi+CiBii=1,2,3                       (2)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{\ OB_i}=\vec{OC_i}+\vec{C_iB_i},i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2)
    其中CiBi\vec{C_iB_i}又可表示为
                            [xiyizi]=[LsinθicosφiLsinθisinφiLcosθi]i=1,2,3      (3)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}-L\sin{\theta_i}\cos{\varphi_i}\\-L\sin{\theta_i}\sin{\varphi_i}\\-L\cos{\theta_i}\\\end{matrix}\right],i=1,2,3\ \ \ \ \ \ (3)
    Aip\vec{A_ip}可表示为
                                                           [xiyizi]=[rcosφirsinφi0]i=1,2,3           (4)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}-r\cos{\varphi_i}\\-r\sin{\varphi_i}\\0\\\end{matrix}\right],i=1,2,3\ \ \ \ \ \ \ \ \ \ \ (4)
    ODi\vec{OD_i}可以表示为
                                                           ODi= OBi+ BiDi                               (5)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OD_i}=\vec{\ OB_i}+\vec{\ B_iD_i}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (5)
    其中由Delta机构的几何性质可知
                                                                   BiDi=Aip                                       (6)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{\ B_iD_i}=\vec{A_ip}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (6)
    所以
                                                         ODi=OCi+CiBi+Aip                         (7)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OD_i}=\vec{OC_i}+\vec{C_iB_i}+\vec{A_ip}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (7)
    综合式(1)—(7)可得,在坐标系Ο-XYZ中D_i的坐标为
                                            [xiyizi]=[(RrLsinθi)cosφi(RrLsinθi)sinφiLcosθi]i=1,2,3         (8)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}\left(R-r-L\sin{\theta_i}\right)\cos{\varphi_i}\\\left(R-r-L\sin{\theta_i}\right)\sin{\varphi_i}\\-L\cos{\theta_i}\\\end{matrix}\right],i=1,2,3\ \ \ \ \ \ \ \ \ (8)
    此时不难发现,Delta机构的正运动学解算已经转化为已知三个顶点坐标和各棱的长度求解另外一个顶点坐标的问题。将图2.4中的四棱锥pD1D2D3p{-D}_1D_2D_3取出单独分析,作垂线pEpE垂直于平面D1D2D3D_1D_2D_3于点EE,取D2D3D_2D_3中点FF,连接EFEFED2ED_2,如图3所示。
    在这里插入图片描述
                                                                4 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图4\ 等效四棱锥
    不难证明,点E为三角形D_1D_2D_3的外接圆圆心。
    则向量Op\vec{Op}可表示为
                                                              Op=OE+Ep                            (9)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{Op}=\vec{OE}+\vec{Ep}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (9)
    OE\vec{OE}可以表示为
                                                             OE=OF+FE                        (10)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OE}=\vec{OF}+\vec{FE}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (10)

    其中,OF=(OD2+OD3)/2\vec{OF}=\left(\vec{OD_2}+\vec{OD_3}\right)/2FE=FEnFE\vec{FE}=\left|\vec{FE}\right|\vec{n_{FE}}

    对于向量FE\vec{FE}其长度为
                                                      FE=D2E2D2F2                            (11)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|\vec{FE}\right|=\sqrt{\left|D_2E\right|^2-\left|D_2F\right|^2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (11)
    其中,D2E\left|D_2E\right|为三角形D1D2D3D_1D_2D_3的外接圆半径,可用公式(12)计算

                                                                 D2E=abc4S                              (12)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|D_2E\right|=\frac{abc}{4S}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left(12\right)

    其中

    S=p(pa)(pb)(pc)        (13)                       p=(a+b+c)2                       (14)S=\sqrt{p\left(p-a\right)\left(p-b\right)\left(p-c\right)}\ \ \ \ \ \ \ \ \left(13\right)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ p=\frac{\left(a+b+c\right)}{2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (14)

    abca、b、c是三角形D1D2D3D_1D_2D_3的边长。

    联立(11)—(14)可得

                                                            FE=(a2+b2c2)c8S                   (15)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|\vec{FE}\right|=\frac{\left(a^2+b^2-c^2\right)c}{8S}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (15)
    向量FE\vec{FE}的单位方向向量为
                                                      nFE=D2D1×D2D3×D3D2D2D1×D2D3×D3D2                  (16)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{n_{FE}}=\frac{\vec{D_2D_1}\times\vec{D_2D_3}\times\vec{D_3D_2}}{\left|\vec{D_2D_1}\times\vec{D_2D_3}\times\vec{D_3D_2}\right|}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (16)
    又向量Ep\vec{Ep}的方向向量
                                                                 nEp=D2D1×D2D3D2D1×D2D3                    (17)\ \ \ \ \ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{n_{Ep}}=\frac{\vec{D_2D_1}\times\vec{D_2D_3}}{\left|\vec{D_2D_1}\times\vec{D_2D_3}\right|}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (17)
    长度为
                                                      Ep=D1p2D1E2              (18)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{Ep}=\sqrt{\left|\vec{D_1p}\right|^2-\left|\vec{D_1E}\right|^2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ (18)
    其中D1p=B1A1=lD1E\left|\vec{D_1p}\right|=\left|\vec{B_1A_1}\right|=l,\vec{D_1E}为外接圆半径。
    将(10)—(18)式联立求解带入(9)式即可求得Delta机构正解。

    四、运动学反解

    运动学反解是在已知机器人pp点的位置(x,y,z)(x,y,z)的情况下求解三个主动臂需要转过的角度θ1\theta_1θ2\theta_2θ3\theta_3,与串联机器人不同,并联机器人的反解较易求得,此处只需要根据杆长进行约束即可很容易解出,求解过程在文献[3]中有详细的说明,此处不再推导,仅根据图5所示的单支链求解示意图给出最终的求解结果。
    在这里插入图片描述
                                                                5 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图5\ 单支链求解示意图

                                       θi=2arctan(BiBi24AiCi2Ai)i=1,2,3               (19)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \theta_i=2arctan\left(\frac{-B_i-\sqrt{B_i^2-4A_iC_i}}{2A_i}\right),i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (19)
    其中
    A1=x2+y2+z2+Rr2+L2l22x(Rr)2L(Rrx)A1=x2+y2+z2+R-r2+L2-l2-2x(R-r)2L-(R-r-x)
    B1=2zB1=2z
    C1=x2+y2+z2+Rr2+L2l22x(Rr)2L+(Rrx)C1=x2+y2+z2+R-r2+L2-l2-2x(R-r)2L+(R-r-x)
    A2=x2+y2+z2+Rr2+L2l2+(x3y)(Rr)L2Rr(x3y)A2=x2+y2+z2+R-r2+L2-l2+(x-3y)(R-r)L-2R-r-(x-3y)
    B2=4zB2=4z
    C2=x2+y2+z2+Rr2+L2l2+(x3y)(Rr)L+2Rr+(x3y)C2=x2+y2+z2+R-r2+L2-l2+(x-3y)(R-r)L+2R-r+(x-3y)
    A3=x2+y2+z2+Rr2+L2l2+(x+3y)(Rr)L2Rr(x+3y)A3=x2+y2+z2+R-r2+L2-l2+(x+3y)(R-r)L-2R-r-(x+3y)
    B3=4zB3=4z
    C3=x2+y2+z2+Rr2+L2l2+(x+3y)(Rr)L+2Rr+(x+3y)C3=x2+y2+z2+R-r2+L2-l2+(x+3y)(R-r)L+2R-r+(x+3y)
    至此,Delta机构的运动学正反解均以求解完毕。

    五、参考文献

    [1] Clavel R. Dispositif pour le deplacement et le positionnement d’un element dans l’espace[P].Switzerland: CH1985005348856,1985.
    [2] 赵杰,朱延河,蔡鹤皋.Delta型并联机器人运动学正解几何解法[J].哈尔滨工业大学学报,2003(01):25-27.
    [3] 伍经纹,徐世许,王鹏,宋婷婷.基于Adams的三自由度Delta机械手的运动学仿真分析[J].软件,2017,38(06):108-112.

    展开全文
  • 绘制3-SPR并联平台的中心点工作空间和姿态偏转工作空间,绘制时间和图像精度取决于计算步长的选择,可以自定义绘制平台的尺寸和约束限制
  • //正解 void forward(const double* q, double* T) { double s1 = sin(*q), c1 = cos(*q); q++; double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; double s3 = sin(*q), c3 = cos(*q); q234 += *q...
  • 牛顿法求解Stewart平台运动学正解

    千次阅读 2019-09-22 20:34:07
    function Newton() format long x0=[0;0;20;0;0;0]; x1=x0-inv(myJacobi(x0))*myfun(x0); while norm(x1-x0)>1e-3 x0=x1; x1=x0-inv(myJacobi(x0))*myfun(x0); end x1 end function f=myfun(x) syms x...
  • DH参数法建立机器人的运动学正解

    万次阅读 多人点赞 2016-04-21 23:32:21
    DH参数法建立机器人的运动学正解  运用DH参数法时坐标系建立的两个约定:  (1)x_i与z_(i-1)垂直  (2)x_i与z_(i-1)相交  坐标系i与坐标系i-1的其次变换矩阵为:  a为两z轴的距离,d为两x轴的距离。 ...
  • function STEWART_fsolve() format long x=fsolve(@myfun,[0;0;20;0;0;0]) %调用函数 end function f=myfun(x) syms x1 x2 x3 a b g X=[x1;x2;x3]; RX=[1 0 0;0 cos(a) -sin(a);...RY=[cos(b) 0 sin(b...
  • 下面展示一些 内联代码片。 #include <iostream> #include <Eigen/Dense> #include <math.h> ...//-0.0182128,-0.751325,-0.0162059,-2.33274,0.0310344,1.59808,0.751991 初始
  • 六轴机器人matlab写运动学正解函数(DH模型)

    万次阅读 多人点赞 2018-06-15 13:25:23
    1.分两个程序①主函数②function函数 2.main clear; clc; %建立机器人模型 % theta d a alpha offset SL1=Link([0 0 0.180 -pi/2 0 ],'standard'); SL2=Link([0 ...
  • 1.分两个程序①主函数②function函数 2.main clear; clc; %建立机器人模型 % theta d a alpha offset ML1=Link([0 0 0 0 0 ],'modified'); ML2=Link([0 0...

空空如也

1 2 3 4 5 ... 9
收藏数 163
精华内容 65
关键字:

运动学正解