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

五自由度机械臂建模

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

Matlab机器人工具箱版本9.10

机械臂还是原来的机械臂，之前用ROS做底层驱动，不需要写正逆运动学和相关算法就能得到一些简单的仿真轨迹，详情可见我之前的博客：
六自由度机械臂ROS+Rviz+Arbotix控制器仿真
使用MoveIt！+Arbotix控制六自由度机械臂
MoveIt！入门：RobotModel、RobotState
MoveIt!五自由度机械臂pick_and_place抓取规划演示
使用ROS MoveIt!控制真实五自由度机械臂
下面我搞一搞这个底层部分：

标准D-H法建模

由于该机械臂只有五个自由度，并且D-H法只能实现绕Z轴的旋转和沿X轴的位移，而该臂第四个关节和第五个关节坐标系必须先绕着Z轴旋转90度，然后再绕X轴旋转90度，这是常规D-H法无法实现的。这里可以在第四个关节和第五个关节中设置一个虚拟关节，以此来过渡一下，解决上述问题。建模如下：

i	αi	ai	di	θi
1	pi/2	0	0	θ1
2	0	0.104	0	θ2
3	0	0.096	0	θ3
4	0	0	0	θ4
5	pi/2	0	0	pi/2
6	0	0	0	θ5
7	0	0.028	0.163	0

正运动学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、d7nx = 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*/#ifndef HEADER_MYMATRIX_H_
#define HEADER_MYMATRIX_H_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 "../header/MyMatrix.h"
#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;elseE->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_#include "../header/MyMatrix.h"
#define PI 3.141592653
typedef struct DH_Node *PtrToDHNode;
struct DH_Node
{// thetafloat th1;float th2;float th3;float th4;float th5;float th6;float th7;// dfloat d1;float d2;float d3;float d4;float d5;float d6;float d7;// afloat a1;float a2;float a3;float a4;float a5;float a6;float a7;// alphafloat 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>
#include "../header/FiveDofKinematic.h"
#include "../header/MyMatrix.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);// theta1float 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);// theta3float 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);// theta2float 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);// theta4float 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>
#include "../header/FiveDofKinematic.h"
#include "../header/MyMatrix.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运算结果：

两者结果一致，证明c程序的正确性。