理论部分
概念
简而言之，运动学正解是给出6个关节变量，以获得机械臂末端的位置和姿势 即给出 j 1 ? j 6 j_1 - j_6 j1?j6，求 x , y , z , r x , r y , r z x,y,z,rx,ry,rz x,y,z,rx,ry,rz
DH参数
只给出关节值或直角坐标值，不能直接相互转换，但也与特定的机器人有关，这部分相关内容可以使用DH参数表显示了机器人关节坐标系之间的关系
连杆长度 (length) ：两个相邻关节轴之间的距离 连杆扭角 (angle) ：两个相邻关节轴之间的角度 连杆偏距 (d) ：两个关节坐标系X轴之间的距离
- DH参数表
| 关节编号 | legth(mm) | d(mm) | angle(deg) |
|---|---|---|---|
| 1 | 0 | 162.5 | 90 |
| 2 | -425 | 0 | 0 |
| 3 | -392.2 | 0 | 0 |
| 4 | 0 | 133.3 | 90 |
| 5 | 0 | 99.7 | -90 |
| 6 | 0 | 99.6 | 0 |
计算
根据DH参数表以及 j 1 ? j 6 j_1 - j_6 j1−j6，建立6个关节矩阵 A 1 − A 6 A_1-A_6 A1−A6，计算出转换矩阵 T 1 − T 6 T_1-T_6 T1−T6，计算 A 1 − A 6 A_1-A_6 A1−A6相乘得到矩阵R R = [ r o t 3 ∗ 3 P 3 ∗ 1 0 1 ∗ 3 1 ] R=\begin{bmatrix} {rot_{3*3}}&{P_{3*1}}\\ {0_{1*3}}&{1}\\ \end{bmatrix} R=[rot3∗301∗3P3∗11] P 3 ∗ 1 = ( x , y , z ) T P_{3*1}=(x,y,z)^T P3∗1=(x,y,z)T 则求出R即求出x,y,z 关节矩阵 A i A_i Ai由当前的关节的 j i j_i ji和DH参数导出 设当前 j i j_i ji为 β \beta β,legth为 l l l,d为 d d d,angle为 α \alpha α A i = [ c o s β − s i n β c o s α s i n β s i n α l c o s β s i n β c o s β c o s α − c o s β s i n α l s i n β 0 s i n α c o s α d 0 0 0 1 ] A_i= \left[ \begin{matrix} cos\beta & -sin\beta cos\alpha & sin\beta sin\alpha & lcos\beta \\ sin\beta & cos\beta cos\alpha & -cos\beta sin\alpha & lsin\beta \\ 0 & sin\alpha & cos\alpha & d \\ 0 & 0 & 0 & 1 \end{matrix} \right] Ai=⎣ ⎡cosβsinβ00−sinβcosαcosβcosαsinα0sinβsinα−cosβsinαcosα0lcosβlsinβd1⎦ ⎤ R = A 1 A 2 A 3 A 4 A 5 A 6 R=A_1A_2A_3A_4A_5A_6 R=A1A2A3A4A5A6
然后再求rx,ry,rz r o t 3 ∗ 3 = [ r 00 r 01 r 02 r 10 r 11 r 12 r 20 r 21 r 22 ] rot_{3*3}= \left[ \begin{matrix} r_{00} & r_{01} & r_{02} \\ r_{10} & r_{11} & r_{12} \\ r_{20} & r_{21} & r_{22} \\ \end{matrix} \right] rot3∗3=⎣ ⎡r00r10r20r01r11r21r02r12r22⎦ ⎤ r x = a r c t a n ( r [ 1 ] [ 2 ] , r [ 2 ] [ 2 ] ) rx = arctan(r[1][2], r[2][2]) rx=arctan(r[1][2],r[2][2]) r y = a r c t a n ( r [ 0 ] [ 2 ] , r [ 0 ] [ 0 ] 2 + r [ 0 ] [ 1 ] 2 ) ry = arctan(r[0][2], \sqrt{r[0][0] ^2 + r[0][1]^2}) ry=arctan(r[0][2],r[0][0]2+r[0][1]2 ) r z = a r c t a n ( r [ 0 ] [ 1 ] , r [ 0 ] [ 0 ] ) rz = arctan(r[0][1], r[0][0]) rz=arctan(r[0][1],r[0][0])
代码（C++）
/* 6轴机器人运动正解 * 关节角度在文件我放在"D:\\j.txt"中 * 机器人参数在文件"D:\\dh.txt"中 * x,y,z,rx,ry,rz在屏幕输出 * 原代码来自https://blog.csdn.net/weixin_37942267/article/details/78806448?spm=1001.2014.3001.5502*/ #include <stdio.h> #include<iostream> #include <math.h> #include <string.h> #include <math.h> using namespace std; #define XYZ_F_J "D:\\j.txt" #define DESIGN_DT "D:\\dh.txt" #define RAD2ANG (3.1415926535898/180.0) #define ANG2RAD (180.0/3.1415926535898) #define IS_ZERO(var) if(abs(var) < 1e-6 ){ var = 0;} #define MATRIX_N 4 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_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); void matrix_mul(double matrix_a[MATRIX_N][MATRIX_N], double matrix_b[MATRIX_N][MATRIX_N], double matrix_result[MATRIX_N][MATRIX_N]); void 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 标签：6187r20kl1单圈电位器