五自由度機械臂正逆運動學算法（Ｃ語言+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 <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_

#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 <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);
	// 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>
#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程序的正確性。