# 五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）

## 五自由度机械臂建模

### 学习代码都记录在个人github上，欢迎关注~

Matlab机器人工具箱版本9.10

MoveIt！入门：RobotModel、RobotState
MoveIt!五自由度机械臂pick_and_place抓取规划演示

iαiaidiθi
1pi/200θ1
200.1040θ2
300.0960θ3
4000θ4
5pi/200pi/2
6000θ5
700.0280.1630

### 正运动学Matlab

% Standard DH
% five_dof robot
% 在关节4和关节5之间增加一个虚拟关节，便于逆运动学计算
clear;
clc;
th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = pi/2;
th(2) = 0; d(2) = 0; a(2) = 0.104;alp(2) = 0;
th(3) = 0; d(3) = 0; a(3) = 0.096; alp(3) = 0;
th(4) = 0; d(4) = 0; a(4) = 0; alp(4) = 0;
th(5) = pi/2; d(5) = 0; a(5) = 0; alp(5) = pi/2;
th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = 0;
th(7) = 0; d(7) = 0.163; a(7) = 0.028; alp(7) = 0;
% DH parameters  th     d    a    alpha  sigma
L1 = Link([th(1), d(1), a(1), alp(1), 0]);
L2 = Link([th(2), d(2), a(2), alp(2), 0]);
L3 = Link([th(3), d(3), a(3), alp(3), 0]);
L4 = Link([th(4), d(4), a(4), alp(4), 0]);
L5 = Link([th(5), d(5), a(5), alp(5), 0]);
L6 = Link([th(6), d(6), a(6), alp(6), 0]);
L7 = Link([th(7), d(7), a(7), alp(7), 0]);
robot = SerialLink([L1, L2, L3, L4, L5, L6, L7]);
robot.name='MyRobot-5-dof';
robot.display()
theta = [0 0 0 0 90 0 0]*pi/180;
robot.teach();
robot.plot(theta);
t = robot.fkine(theta)    %末端执行器位姿
ik_T = five_dof_ikine(t)


### 逆运动学Matlab

function ik_T = five_dof_ikine(fk_T)
a2 = 0.104; a3 = 0.096; ae = 0.028; de = 0.163; % ae和de即为a7、d7

nx = fk_T(1, 1); ox = fk_T(1, 2); ax = fk_T(1, 3); px = fk_T(1, 4);
ny = fk_T(2, 1); oy = fk_T(2, 2); ay = fk_T(2, 3); py = fk_T(2, 4);
nz = fk_T(3, 1); oz = fk_T(3, 2); az = fk_T(3, 3); pz = fk_T(3, 4);

% theta1
theta1 = atan2(py - ny*ae - ay*de, px - nx*ae - ax*de);
% theta5
theta5 = atan2(sin(theta1)*nx - cos(theta1)*ny, sin(theta1)*ox - cos(theta1)*oy);
% theta3有两个解
m = px - nx*ae - ax*de;
n = py - ny*ae - ay*de;
t = pz - nz*ae - az*de;
c3 = (power(cos(theta1), 2)*power(m, 2) + power(sin(theta1), 2)*power(n, 2) + 2*sin(theta1)*cos(theta1)*m*n + power(t, 2) - power(a2, 2) - power(a3, 2)) / (2*a2*a3);
theta3_1 = atan2(sqrt(1-power(c3, 2)), c3);
theta3_2 = atan2(-sqrt(1-power(c3, 2)), c3);
% theta2有两个解
% 对应theta3_1
c2_1 = ((a3*cos(theta3_1) + a2)*(cos(theta1)*m + sin(theta1)*n) + a3*sin(theta3_1)*t) / (power(a3*cos(theta3_1) + a2, 2) + power(a3, 2)*power(sin(theta3_1), 2));
s2_1 = ((a3*cos(theta3_1) + a2)*t - a3*sin(theta3_1)*(cos(theta1)*m + sin(theta1)*n)) / (power(a3*cos(theta3_1) + a2, 2) + power(a3, 2)*power(sin(theta3_1), 2));
% 对应theta3_2
c2_2 = ((a3*cos(theta3_2) + a2)*(cos(theta1)*m + sin(theta1)*n) + a3*sin(theta3_2)*t) / (power(a3*cos(theta3_2) + a2, 2) + power(a3, 2)*power(sin(theta3_2), 2));
s2_2 = ((a3*cos(theta3_2) + a2)*t - a3*sin(theta3_2)*(cos(theta1)*m + sin(theta1)*n)) / (power(a3*cos(theta3_2) + a2, 2) + power(a3, 2)*power(sin(theta3_2), 2));
theta2_1 = atan2(s2_1, c2_1);
theta2_2 = atan2(s2_2, c2_2);
% theta4有两个解
theta4_1 = atan2(az, cos(theta1)*ax + sin(theta1)*ay) - theta3_1 - theta2_1;
theta4_2 = atan2(az, cos(theta1)*ax + sin(theta1)*ay) - theta3_2 - theta2_2;

ik_T = [theta1, theta2_1, theta3_1, theta4_1, theta5;
theta1, theta2_2, theta3_2, theta4_2, theta5];
end


### 验证

theta = [0 45 120 60 90 45 0]*pi/180;

theta = [3.141592653589793 0.400142386223488 2.094395102393196 -3.279935652014131 pi/2 -2.356194490192346 0];

theta = [3.141592653589793 2.356194490192346 -2.094395102393196 -1.047197551196598 pi/2 -2.356194490192346 0];

## 下面是C程序实现代码

/*
* MyMatrix.h
*
*  Created on: Jul 13, 2019
*      Author: xuuyann
*/

typedef struct MNode *PtrToMNode;
struct MNode
{
int row;
int column;
float **data;
};
typedef PtrToMNode Matrix;
// 创建一个矩阵
Matrix Create_Matrix(int row, int column);
// 初始化矩阵
void Init_Matrix(Matrix mat);
// 给矩阵每个元素赋值
void SetData_Matrix(Matrix mat, float data[]);
// 打印矩阵
void Show_Matrix(Matrix mat);
// 矩阵加减法
Matrix AddorSub_Matrix(Matrix mat_1, Matrix mat_2, int flag);
// 转置
Matrix Trans_Matrix(Matrix mat);
// 矩阵乘法
Matrix Mult_Matrix(Matrix mat_1, Matrix mat_2);
// 单位矩阵
Matrix eye(int n);
// 取出矩阵某行某列的元素
float PickInMat(Matrix mat, int r, int c);

#endif /* HEADER_MYMATRIX_H_ */


/*
* MyMatrix.c
*
*  Created on: Jul 13, 2019
*      Author: xuuyann
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

void Init_Matrix(Matrix mat)
{
int i, j;
for (i = 0; i < mat->row; i++){
for (j = 0; j < mat->column; j++){
mat->data[i][j] = 0;
}
}
}

Matrix Create_Matrix(int row, int col)
{
Matrix mat;
mat = (Matrix)malloc(sizeof(struct MNode));
if (row <= 0 || col <= 0){
printf("ERROR, in creat_Matrix the row or col <= 0\n");
exit(1);
}
if (row > 0 && col >0){
mat->row = row;
mat->column = col;
mat->data = (float **)malloc(row*sizeof(float *));
if (mat->data == NULL){
printf("ERROR, in creat_Matrix the mat->data == NULL\n");
exit(1);
}
int i;
for (i = 0; i < row; i++ ){
*(mat->data + i) = (float *)malloc(col*sizeof(float));
if (mat->data[i] == NULL){
printf("ERROR, in create_Matrix the mat->data[i] == NULL\n");
exit(1);
}
}
Init_Matrix(mat);
}
return mat;
}

void Show_Matrix(Matrix mat)
{
int i, j;
for (i = 0; i < mat->row; i++){
for (j = 0; j < mat->column; j++)
printf("%.6f\t", mat->data[i][j]);
printf("\n");
}
}

void SetData_Matrix(Matrix mat, float data[])
{
int i, j;
for (i = 0; i < mat->row; i++){
for (j = 0; j < mat->column; j++){
mat->data[i][j] = data[i*mat->column + j];
}
}
}

//flag = 0, add; flag = 1, sub
Matrix AddorSub_Matrix(Matrix mat_1, Matrix mat_2, int flag)
{
Matrix rst_mat;
if (mat_1->column != mat_2->column){
printf("ERROR in AddorSub, column !=\n");
exit(1);
}
if (mat_1->row != mat_2->row){
printf("ERROR in AddorSub, row !=\n");
exit(1);
}
int i, j;
rst_mat = Create_Matrix(mat_1->row, mat_1->column);
for (i = 0; i < mat_1->row; i++){
for (j = 0; j < mat_1->column; j++)
rst_mat->data[i][j] = mat_1->data[i][j] + pow(-1, flag)*mat_2->data[i][j];
}
return rst_mat;
}

//转置
Matrix Trans_Matrix(Matrix mat)
{
Matrix mat_;
int i, j;
mat_ = Create_Matrix(mat->row, mat->column);
for (i = 0; i < mat->row; i ++){
for (j = 0; j < mat->column; j++)
mat_->data[i][j] = mat->data[i][j];
}
return mat_;
}

Matrix Mult_Matrix(Matrix mat_1, Matrix mat_2)
{
Matrix rst_mat;
int i, j, m;
if (mat_1->column != mat_2->row){
printf("ERROR in Mult_Matrix, column != row\n");
exit(1);
}else{
rst_mat = Create_Matrix(mat_1->row, mat_2->column);
for (i = 0; i < mat_1->row; i++){
for (j = 0; j < mat_2->column; j++){
for (m = 0; m < mat_1->column; m++)
rst_mat->data[i][j] += mat_1->data[i][m] * mat_2->data[m][j];
}
}
}
return rst_mat;
}

Matrix eye(int n)
{
Matrix E;
int i, j;
if (n <= 0){
printf("ERROR in eye\n");
exit(1);
}
E = Create_Matrix(n, n);
for (i = 0; i < n; i++){
for (j = 0; j < n; j++){
if (i == j)
E->data[i][j] = 1;
else
E->data[i][j] = 0;
}
}
return E;
}

float PickInMat(Matrix mat, int r, int c)
{
float rst;
rst = mat->data[r - 1][c - 1];
return rst;
}

/*
* five_dof_kinematic.h
*
*  Created on: Jul 13, 2019
*      Author: xuuyann
*/

#ifndef FIVEDOFKINEMATIC_H_
#define FIVEDOFKINEMATIC_H_

#define PI 3.141592653
typedef struct DH_Node *PtrToDHNode;
struct DH_Node
{
// theta
float th1;
float th2;
float th3;
float th4;
float th5;
float th6;
float th7;
// d
float d1;
float d2;
float d3;
float d4;
float d5;
float d6;
float d7;
// a
float a1;
float a2;
float a3;
float a4;
float a5;
float a6;
float a7;
// alpha
float alp1;
float alp2;
float alp3;
float alp4;
float alp5;
float alp6;
float alp7;
};
typedef PtrToDHNode Input_data;

// 初始化ＤＨ参数
void Init_DH(Input_data DH_para);
// 正运动学推导
Matrix five_dof_fkine(Input_data DH_para, float theta[]);
// 逆运动学推导
Matrix five_dof_ikine(Input_data DH_para, Matrix fk_T);

#endif /* HEADER_FIVE_DOF_KINEMATIC_H_ */


/*
* FiveDofKinemate.c
*
*  Created on: Jul 13, 2019
*      Author: xuuyann
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*               theta,   d,     a,    alpha                     */
float DH[7][4] = {{0,     0,     0,     PI/2},
{0,     0,     0.104, 0},
{0,     0,     0.096, 0},
{0,     0,     0,     0},
{PI/2,  0,     0,     PI/2},
{0,     0,     0,     0},
{0,     0.163, 0.028, 0}};

void Init_DH(Input_data DH_para)
{
DH_para->th1 = DH[0][0];
DH_para->th2 = DH[1][0];
DH_para->th3 = DH[2][0];
DH_para->th4 = DH[3][0];
DH_para->th5 = DH[4][0];
DH_para->th6 = DH[5][0];
DH_para->th7 = DH[6][0];
DH_para->d1 = DH[0][1];
DH_para->d2 = DH[1][1];
DH_para->d3 = DH[2][1];
DH_para->d4 = DH[3][1];
DH_para->d5 = DH[4][1];
DH_para->d6 = DH[5][1];
DH_para->d7 = DH[6][1];
DH_para->a1 = DH[0][2];
DH_para->a2 = DH[1][2];
DH_para->a3 = DH[2][2];
DH_para->a4 = DH[3][2];
DH_para->a5 = DH[4][2];
DH_para->a6 = DH[5][2];
DH_para->a7 = DH[6][2];
DH_para->alp1 = DH[0][3];
DH_para->alp2 = DH[1][3];
DH_para->alp3 = DH[2][3];
DH_para->alp4 = DH[3][3];
DH_para->alp5 = DH[4][3];
DH_para->alp6 = DH[5][3];
DH_para->alp7 = DH[6][3];
}

// 正运动学推导
Matrix five_dof_fkine(Input_data DH_para, float theta[])
{
Matrix rst, Ti;
rst = eye(4);
Ti = Create_Matrix(4, 4);
float a[7] = {DH_para->a1, DH_para->a2, DH_para->a3, DH_para->a4, DH_para->a5, DH_para->a6, DH_para->a7};
float d[7] = {DH_para->d1, DH_para->d2, DH_para->d3, DH_para->d4, DH_para->d5, DH_para->d6, DH_para->d7};
float alp[7] = {DH_para->alp1, DH_para->alp2, DH_para->alp3, DH_para->alp4, DH_para->alp5, DH_para->alp6, DH_para->alp7};
float th[7] = {theta[0], theta[1], theta[2], theta[3], DH_para->th5, theta[4], DH_para->th7};
for (int i = 0; i < 7; i++){
Ti->data[0][0] = cos(th[i]);
Ti->data[0][1] = -sin(th[i]) * cos(alp[i]);
Ti->data[0][2] = sin(th[i]) * sin(alp[i]);
Ti->data[0][3] = a[i] * cos(th[i]);
Ti->data[1][0] = sin(th[i]);
Ti->data[1][1] = cos(th[i]) * cos(alp[i]);
Ti->data[1][2] = -cos(th[i]) * sin(alp[i]);
Ti->data[1][3] = a[i] * sin(th[i]);
Ti->data[2][0] = 0;
Ti->data[2][1] = sin(alp[i]);
Ti->data[2][2] = cos(alp[i]);
Ti->data[2][3] = d[i];
Ti->data[3][0] = 0;
Ti->data[3][1] = 0;
Ti->data[3][2] = 0;
Ti->data[3][3] = 1;
//Show_Matrix(Ti);
//printf("\n");
// Matrix Mult_Matrix(Matrix mat_1, Matrix mat_2);
rst = Mult_Matrix(rst, Ti);
//Show_Matrix(rst);
}
return rst;
}

Matrix five_dof_ikine(Input_data DH_para, Matrix fk_T)
{
Matrix ik_T;
ik_T = Create_Matrix(2, 5);
float a2 = DH_para->a2;
float a3 = DH_para->a3;
float ae = DH_para->a7;
float de = DH_para->d7;
float nx, ny, nz;
float ox, oy, oz;
float ax, ay, az;
float px, py, pz;
nx = PickInMat(fk_T, 1, 1);
ny = PickInMat(fk_T, 2, 1);
nz = PickInMat(fk_T, 3, 1);
ox = PickInMat(fk_T, 1, 2);
oy = PickInMat(fk_T, 2, 2);
oz = PickInMat(fk_T, 3, 2);
ax = PickInMat(fk_T, 1, 3);
ay = PickInMat(fk_T, 2, 3);
az = PickInMat(fk_T, 3, 3);
px = PickInMat(fk_T, 1, 4);
py = PickInMat(fk_T, 2, 4);
pz = PickInMat(fk_T, 3, 4);
// theta1
float theta1;
theta1 = atan2(py - ny*ae - ay*de, px - nx*ae - ax*de);
// theta5;
float theta5;
theta5 = atan2(sin(theta1)*nx - cos(theta1)*ny, sin(theta1)*ox - cos(theta1)*oy);
// theta3
float m = px - nx*ae - ax*de;
float n = py - ny*ae - ay*de;
float t = pz - nz*ae - az*de;
float c3;
c3 = (pow(cos(theta1), 2)*pow(m, 2) + pow(sin(theta1), 2)*pow(n, 2)
+ 2*sin(theta1)*cos(theta1)*m*n + pow(t, 2) - pow(a2, 2) - pow(a3, 2)) / (2*a2*a3);
float theta3_1 = atan2(sqrt(1-pow(c3, 2)), c3);
float theta3_2 = atan2(-sqrt(1-pow(c3, 2)), c3);
// theta2
float c2_1, s2_1, c2_2, s2_2;
c2_1 = ((a3*cos(theta3_1) + a2)*(cos(theta1)*m + sin(theta1)*n) + a3*sin(theta3_1)*t)
/ (pow(a3*cos(theta3_1) + a2, 2) + pow(a3, 2)*pow(sin(theta3_1), 2));
s2_1 = ((a3*cos(theta3_1) + a2)*t - a3*sin(theta3_1)*(cos(theta1)*m + sin(theta1)*n))
/ (pow(a3*cos(theta3_1) + a2, 2) + pow(a3, 2)*pow(sin(theta3_1), 2));
c2_2 = ((a3*cos(theta3_2) + a2)*(cos(theta1)*m + sin(theta1)*n) + a3*sin(theta3_2)*t)
/ (pow(a3*cos(theta3_2) + a2, 2) + pow(a3, 2)*pow(sin(theta3_2), 2));
s2_2 = ((a3*cos(theta3_2) + a2)*t - a3*sin(theta3_2)*(cos(theta1)*m + sin(theta1)*n))
/ (pow(a3*cos(theta3_2) + a2, 2) + pow(a3, 2)*pow(sin(theta3_2), 2));
float theta2_1 = atan2(s2_1, c2_1);
float theta2_2 = atan2(s2_2, c2_2);
// theta4
float theta4_1 = atan2(az, cos(theta1)*ax + sin(theta1)*ay) - theta3_1 - theta2_1;
float theta4_2 = atan2(az, cos(theta1)*ax + sin(theta1)*ay) - theta3_2 - theta2_2;

float th[10] = {theta1, theta2_1, theta3_1, theta4_1, theta5,
theta1, theta2_2, theta3_2, theta4_2, theta5};
SetData_Matrix(ik_T, th);

return ik_T;
}

/*
* main.c

*
*  Created on: Jul 13, 2019
*      Author: xuuyann
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

int main()
{
Matrix fk_T, ik_T;
fk_T = Create_Matrix(4, 4);
ik_T = Create_Matrix(2, 5);
float theta[5] = {0, 45*PI/180, 120*PI/180, 60*PI/180, 45*PI/180};
Input_data DH_para;
DH_para = (Input_data)malloc(sizeof(struct DH_Node));
Init_DH(DH_para);
// Matrix five_dof_fkine(Input_data DH_para, float theta[])
fk_T = five_dof_fkine(DH_para, theta);
printf("fk_T:\n");
Show_Matrix(fk_T);
printf("\n");
// Matrix five_dof_ikine(Input_data DH_para, Matrix fk_T);
printf("ik_T:\n");
ik_T = five_dof_ikine(DH_para, fk_T);
Show_Matrix(ik_T);

return 0;
}


theta = [0, 45, 120, 60, 45]
Ｃ语言运算结果：

matlab运算结果：

06-06 5109
07-21 3万+
03-28 1万+
01-15
07-31
05-31 4460
03-29 1410
08-14 1万+
07-25 4105
©️2020 CSDN 皮肤主题: 技术黑板 设计师:CSDN官方博客