五自由度机械臂建模
学习代码都记录在个人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、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程序的正确性。
1591
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
