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

五自由度机械臂建模

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、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
 */

#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 
#include 
#include 

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_

#include "../header/MyMatrix.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 
#include 
#include 
#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);
	// 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 
#include 
#include 
#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程序的正确性。