6轴机器人运动学正解,逆解2
2024-04-20 15:57:18
逆解
逆解计算方法可以参考以下书籍
机器人学导论——分析、系统及应用 电子工业出版社
机器人学导论第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 90typedef struct {double joint_v; //joint variabledouble 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",¶m_table[i].length,¶m_table[i].d,¶m_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;//计算 j1j1[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]) );//计算 j2int 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;}//计算 j3for(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;//计算xtmp = -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;//计算ytmp =sin(j1);IS_ZERO(tmp);y *=tmp;//计算ztmp = 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");
}6轴机器人运动学正解,逆解2相关推荐
- 六轴机器人运动学正解
关于机器人运动学正解网上和机器人相关书籍上都是通过建立连杆坐标系和DH参数推导出来的,推导过程比较繁琐,本人不是从事机器人专业,我在推导机器人运动学正解的时候还不知道有DH参数一说,我的算法原理是运用 ...
- 6轴机器人运动学正解,逆解1
正解 给定机器人各关节的角度,计算出机器人末端的空间位置 逆解 已知机器人末端的位置和姿态,计算机器人各关节的角度值 常见的工业机器人 正解与逆解的求解需要相应的机器人运动方程,其中关键的就是DH参数 ...
- （6）六轴机械臂的运动学正、逆解
下面在前面的ur5机械臂的DH参数基础是对其正逆解进行求解,为了后面能在MATLAB中利用stl文件进行实际显示,这里以标准DH参数为例进行讲解.(修正DH参数在用plot3d函数是显示失败,不知道是 ...
- 工业机器人（4）-- Matlab Robot Toolbox运动学正、逆解
[Matlab Robotics Toolbox]robotics toolbox学习及使用记录,方便自己后面复习.改进. 基于Matlab R2019b 9.5; Peter Corke的Robot ...
- 6轴机器人运动学（正解）
理论部分 概念 运动学正解,简而言之,就是给出6个关节变量,求得机械臂末端的位置和姿态 即给出j1−j6j_1 - j_6j1−j6,求x,y,z,rx,ry,rzx,y,z,rx,ry,rzx, ...
- Puma560机器人运动学正逆解
puma560机器人D-H参数 puma560采用的是改进D-H参数,其DH参数表如下: i αi ai di θi 1 0 0 0 t1 2 -90 0 0 t2 3 0 r2 d3 t3 4 -9 ...
- 有关并联绳驱机器人运动学正解反解的学习（新手）
有关并联绳驱机器人运动学正解反解的学习 Preface(complain) Perface(start) Advantages of parallel robot Disdvantages of pa ...
- 机器人正解逆解-附记
距机器人正解逆解的3篇文章已经有一年多时间 不知不觉浏览已有一万 在评论里看到一些问题 在此尝试解答 建议还是先看看相关的书中的计算方法 不知道计算方法而通过程序去理解如何计算 是比较难的 代码里面的 ...
- IRB 1600-6/1.45 ABB 改进DH参数计算正解逆解
IRB 1600-6/1.45 ABB机器人正解逆解计算 一.参考文章 二.改进DH参数获取 三.fk推导 四.ik推导 五.测试验证 关节空间验证 附录-代码部分 一.参考文章 最近在项目中,需要通 ...
最新文章
- 测试udp端口状态 curl_检测TCP/UDP端口的连通性
- oracle分区exchange,oracle 分区表exchange原理
- Application package 'AndroidManifest.xml' must have a minimum of 2 segments.
- 虚拟机服务器被攻击,Linux服务器被攻击用来挖矿了
- 检验学习笔记-寄生虫
- 电机编码器调零步骤_什么是无刷直流电机换向的最有效方法？
- 关于图灵机的三个问题
- 【Antd】rawData.some is not a function 报错解决方法
- idea解決tomcat乱码问题
- java 实现ukey身份认证_一种基于ukey认证的单点登录实现方法与流程
- Hadoop 入门笔记
- 浏览器全球的书签都在这里了，看看有没有你的！
- 用计算机美化演讲稿过程,计算机应用基础第10章--PowerPoint应用—制作论文答辩演讲稿.ppt...
- win10中使用VS2017\VS2019编译MQTT(包含32位、64位；Debug版本\Release版本）（附示例demo）
- 提升技能，升级思维，你学知识我送T恤，订阅专栏免费领51CTO定制T恤
- 计算机开机选择用户界面,Windows10每次开机都会出现选择操作系统界面的解决方法...
- tosmana使用教程_20款最流行的免费定性数据分析工具
- 名片管理系统 #python项目 #演练 #增删改查CRUD #全程教程
- nginx 根据目录指定root_部署Nginx网站服务实现访问状态统计以及访问控制功能
- Qt 遇到的一些问题汇总(二)