逆解
逆解计算方法可以参考以下书籍
机器人学导论——分析、系统及应用 电子工业出版社
机器人学导论第3版 机械工业出版社
机器人学建模、规划与控制 西安交通大学出版社
对于关节1,2,3可以从运动方程手工推导出各个关节旋转角度的计算公式
逆解求解的结果并不是唯一的 可能有多组解

/*计算逆解 根据机器人坐标计算机器人关节角度 *关节参数在文件 param_table中 *机器人坐标在文件 xyzrpy中 *计算结果在屏幕输出 */#include #include #include #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 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(doublej1, 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; //计算 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(doublej1, 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--- > -piand < 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--- > -piand < 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