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

最新推荐文章于 2025-06-05 14:55:15 发布

原创

最新推荐文章于 2025-06-05 14:55:15 发布 · 5.3w 阅读

325 ·

CC 4.0 BY-SA版权

文章标签：

#机器人 #运动学正解 #DH参数 #6轴机器人坐标计算

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

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

常见的工业机器人

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