逆解
逆解計算方法可以參考以下書籍
機器人學導論——分析、系統及應用 電子工業出版社
機器人學導論第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[] ={0};
double worldx =, worldy =, worldz =,
worldrr =, worldrp =, worldry =;
double z_offset=;
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 )
{//計算關節的角度
double v1_c, v1_s, v2_c, v2_s;
double var_M, var_K, tmp;
double var_sqrt[] = {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) / (- * a2);
tmp = var_M*var_M + pz*pz - var_K*var_K;
IS_ZERO(tmp);
if( tmp >= ){
//if( (var_M*var_M + pz*pz - var_K*var_K) >=){
//var_sqrt[] = sqrt(var_M*var_M + pz*pz - var_K*var_K);
var_sqrt[] = sqrt(tmp);
var_sqrt[] = -var_sqrt[];
}else{
printf("m^2 + z^2 - k^2 <0 : %lf\n", tmp);
p_j2[] =, p_j2[] =;
return ;
}
p_j2[] = -atan2(var_M, pz) + atan2(var_K, var_sqrt[]);
p_j2[] = -atan2(var_M, pz) + atan2(var_K, var_sqrt[]);
return ;
}
int fun_j3(double j1, double j2, double *p_j3,
double a1, double a3, double d4,
double px, double py, double pz)
{//計算關節的角度
double var_K, tmp;
double var_sqrt[];
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 >= ){
var_sqrt[] = sqrt(tmp);
var_sqrt[] = -var_sqrt[];
p_j3[] = atan2(d4, a3) + atan2(var_K, var_sqrt[]);
p_j3[] = atan2(d4, a3) + atan2(var_K, var_sqrt[]);
}else{
printf("m^2 + z^2 - k^2 <0 : %lf\n", d4*d4 + a3*a3 - var_K*var_K);
p_j3[] =; p_j3[] = ;
return ;
}
return ;
}
/* 計算過程 根據運動方程 計算矩陣 列出等式 計算 j1 j2 j3
* 計算旋轉矩陣 根據 j1 j2 j3 計算T3 并轉置 與旋轉矩陣相乘 *3
* 計算zyz 就是 j4 j5 j6 */
int main()
{
double matrix_R[MATRIX_N][MATRIX_N];
double j1[] = {0}; //元素值 >= 度或 < - 度 表示角度無效
double j2[] = {0};
double j3[] = {0};
double j4[] = {0};
double j5[] = {0};
double j6[] = {0};
int i, j;
// double z_offset=;
// memset(param_table, , sizeof(param_table) );
FILE * fp=NULL;
fp=fopen(XYZ_F_3D, "r");
if(fp== NULL){
perror("open xyzrpy file error\n");
return ;
}
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 ;
}
for(i=; i<; 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[].angle *= RAD2ANG;
param_table[].angle *= RAD2ANG;
param_table[].angle *= RAD2ANG;
param_table[].angle *= RAD2ANG;
param_table[].angle *= RAD2ANG;
param_table[].angle *= RAD2ANG;
calculate_matrix_R(worldrr, worldrp, worldry, matrix_R);
matrix_R[][] = worldx;
matrix_R[][] = worldy;
matrix_R[][] = worldz-z_offset;
matrix_R[][] = ;
matrix_R[][] = ;
matrix_R[][] = ;
matrix_R[][] = ;
printmatrix(matrix_R, MATRIX_N, MATRIX_N);
//double var_M, var_K;
//double var_sqrt[];
double a1 = param_table[].length;
double a2 = param_table[].length;
double a3 = param_table[].length;
double d4 = param_table[].d;
double px = matrix_R[][];
double py = matrix_R[][];
double pz = matrix_R[][];
double v1_c, v1_s, v2_c, v2_s;
//計算 j1
j1[] = atan2(worldy, worldx);
IS_ZERO( j1[] );
//ANG2RAD_EQU(j1[]);
j1[] = j1[] +PI;
JUDGE_ZERO(j1[] -*PI)? (j1[] = ) : ;
//j1[] = JUDGE_ZERO(j1[] -*PI)? j1[] = : ;
printf("j1: \n %lf , %lf\n", ANG2RAD(j1[]), ANG2RAD(j1[]) );
//計算 j2
int v_bool;
v_bool = fun_j2(j1[], j2, a1, a2, a3, d4, px, py, pz);
if(v_bool)
printf("j2: %lf, %lf\n", ANG2RAD(j2[])-, ANG2RAD(j2[])- );
else{
printf("this j2 invalid\n");
j2[] =*PI; j2[] =*PI;
// j2[]> ? (j2[] += *PI): (j2[] -= *PI) ;
// j2[]> ? (j2[] += *PI): (j2[] -= *PI) ;
}
v_bool = fun_j2(j1[], j2+, a1, a2, a3, d4, px, py, pz);
if(v_bool)
printf("j2: %lf, %lf\n", ANG2RAD(j2[])-, ANG2RAD(j2[])- );
else{
printf("this j2 invalid\n");
j2[] =*PI; j2[] =*PI;
}
//計算 j3
for(i=; i<; i+=){
v_bool = fun_j3(j1[i/], j2[i/], j3+i, a1, a3, d4, px, py, pz);
if(v_bool)
printf("j3: %lf, %lf\n",
ANG2RAD(j3[i])-, ANG2RAD(j3[i+])- );
else {
printf("this j3 invalid\n");
j3[i] =*PI; j3[i+] =*PI;
//j3[k]> ? (j3[k] += *PI): (j3[k] -= *PI) ;
//j3[k+]> ? (j3[k+] += *PI): (j3[k+] -= *PI) ;
}
}
printf("judge\n");
for(i=; i<; i++){
printf("j1[%d]: %lf, j2[%d]: %lf, j3[%d]: %lf\n",
i/, j1[i/], i/, j2[i/], i, j3[i]);
//if(j1[i/]==*PI || j2[i/]==*PI || j3[i]==*PI) continue;
if( !judge(j1[i/], j2[i/], j3[i]) ) {
j3[i]>= ? (j3[i] += *PI): (j3[i] -= *PI) ; }
}
printf("\nj1: %lf, %lf\nj2: %lf, %lf, %lf, %lf\n",
ANG2RAD(j1[]), ANG2RAD(j1[]),
ANG2RAD(j2[])-, ANG2RAD(j2[])-,
ANG2RAD(j2[])-, ANG2RAD(j2[])- );
printf("j3:\n");
for(i=; i<; i++){
printf(" %lf ", ANG2RAD(j3[i])-);
if( (i+)%4 == )printf("\n");
}
//計算 j4 j5 j6
for(i=, j=; i<; i++){
if(j3[i] >= *PI || j3[i] < -*PI) continue;
printf("\n----j1[%d]: %lf j2[%d]: %lf j3[%d]: %lf\n",
i/, ANG2RAD(j1[i/]),
i/, ANG2RAD(j2[i/])-,
i, ANG2RAD(j3[i])- );
fun_j456(j1[i/], j2[i/], 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+]),
ANG2RAD(j5[j]), ANG2RAD(j5[j+]),
ANG2RAD(j6[j]), ANG2RAD(j6[j+]) );
j +=;
}
}
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[][] = r_c * p_c;
matrix_R[][] = r_c * p_s * y_s - r_s * y_c;
matrix_R[][] = r_c * p_s * y_c + r_s * y_s;
matrix_R[][] = r_s * p_c;
matrix_R[][] = r_s * p_s * y_s + r_c * y_c;
matrix_R[][] = r_s * p_s * y_c - r_c * y_s;
matrix_R[][] = -p_s;
matrix_R[][] = p_c * y_s;
matrix_R[][] = p_c * y_c;
}
int judge(double j1, double j2, double j3)
{
/* j1 j2 j3 是弧度 j2 j3 已加90度 */
double x, y, z, tmp;
j2 -= .*PI;
j3 -= .*PI;
//計算x
tmp = -sin(j2);
IS_ZERO(tmp);
x = tmp * param_table[].length;
tmp = cos(j2+j3);
IS_ZERO(tmp);
x -= param_table[].length * tmp;
tmp = -sin(j2+j3);
IS_ZERO(tmp);
x +=tmp* param_table[].d;
x += param_table[].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[].length*tmp;
tmp = sin(j2+j3);
IS_ZERO(tmp);
z -=param_table[].length*tmp;
tmp = cos(j2+j3);
IS_ZERO(tmp);
z += param_table[].d *tmp +z_offset;
//printf("%lf %lf %lf\n", x, y, z);
tmp = x - worldx;
if( tmp > . || tmp < -. ) return ;
// if( !(tmp < . && tmp > -.) ) return ;
tmp = y - worldy;
if( tmp > . || tmp < -. ) return ;
tmp = z - worldz;
if( tmp > . || tmp < -. ) return ;
return ;
}
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[].joint_v = j1;
p_table[].joint_v = j2;
p_table[].joint_v = j3;
calculate_matrix_A(matrix_a, p_table+);
calculate_matrix_A(matrix_b, p_table+);
matrix_mul(matrix_a, matrix_b, matrix_tmp, MATRIX_N, MATRIX_N);
calculate_matrix_A(matrix_b, p_table+);
matrix_mul(matrix_tmp, matrix_b, matrix_a, MATRIX_N, MATRIX_N);
matrix_translate(matrix_a, MATRIX_N-, MATRIX_N-);
matrix_mul(matrix_a, p_matrix_R, matrix_b, MATRIX_N-, MATRIX_N-);
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[][]*matrix_R[][] +
matrix_R[][]*matrix_R[][]);
// printf("ZYZ \n--- > -pi and < 0\n");
p_r[] = atan2( matrix_R[][], matrix_R[][]);
p_p[] = atan2( mtmp, matrix_R[][]);
p_y[] = atan2( matrix_R[][], -matrix_R[][] );
// printf("ZYZ \n--- > -pi and < 0\n");
p_r[] = atan2( -matrix_R[][], -matrix_R[][]);
p_p[] = atan2( -mtmp, matrix_R[][]);
p_y[] = atan2( -matrix_R[][], matrix_R[][] );
}
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++ =;
*pmatrix++ = angle_s;
*pmatrix++ = angle_c;
*pmatrix++ = p_param->d;
*pmatrix++ =;
*pmatrix++ =;
*pmatrix++ =;
*pmatrix =;
}
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=; i<m; i++){
for(j=; 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=; i<m; i++)
for(j=; j<n; j++)
{
/*按照矩陣乘法的規則計算結果矩陣的i*j元素*/
sum=;
for(k=; 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 ;
}
void matrix_translate(double matrix[MATRIX_M][MATRIX_N], int m, int n)
{//矩陣轉置
double m_tmp;
int i, j, k;
for(i=, j=; 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=; i<m; i++){
for(j=; j<n; j++){
printf(" %lf ", matrix[i][j]);
}
printf("\n");
}
printf("\n");
}