matlab mdh模型,6轴机器人DH建模、仿真、正逆解代码
一.模型
二.MDH模型
使用修改DH模型、改进DH模型与标准DH模型主要区别在于末端，改进DH并没有建到工具坐标系，这里将4、5、6轴的坐标系都建在同一点，因此机器人连杆的参数只需要4个值a1、a2、a3、d4。a2、d4分别代表大臂与小臂的长度。要完全得到末端或者工具尖点的位姿还需要再右成一个工具坐标系6Te
DH图
1604040111741.jpg
DH参数表
θ
d
a
α
0
0
0
0
0
0
109
-90
0
0
250.5
0
0
239.5
115.5
-90
0
0
0
90
0
0
0
-90
MATLAB仿真效果
image.png
MATLAB代码
clear;
clc;
%建立机器人模型
% theta d a alpha offset
ML1=Link([0 0 0 0 0 ],'modified');
ML2=Link([0 0 109 -pi/2 0 ],'modified');
ML3=Link([0 0 250.5 0 0 ],'modified');
ML4=Link([0 239.5 115.5 -pi/2 0 ],'modified');
ML5=Link([0 0 0 pi/2 0 ],'modified');
ML6=Link([0 0 0 -pi/2 0 ],'modified');
modrobot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','6axis');
teach(modrobot)
一般6轴机器人初始状态为大臂垂直与地面，主要是通过给2轴一个初始角度-90°来实现，下图第一个模型为各轴0°时的姿态，第二图模型为一般回零后机器人保持的姿态。
1
2
三.6轴机器人运动学正解
代码
#include
/* 正解函数
* @joint 6轴各关节值结构体，单位弧度
* @return 返回一个4*4的位姿矩阵
*/
Eigen::Matrix4d IFKinematics::fKine(const Joint & joint)
{
double j1 = joint.j1,
j2 = joint.j2,
j3 = joint.j3,
j4 = joint.j4,
j5 = joint.j5,
j6 = joint.j6;
double s1 = sin(j1), c1 = cos(j1),
s2 = sin(j2), c2 = cos(j2),
s3 = sin(j3), c3 = cos(j3),
s4 = sin(j4), c4 = cos(j4),
s5 = sin(j5), c5 = cos(j5),
s6 = sin(j6), c6 = cos(j6);
Eigen::Matrix4d T01, T12, T23, T34, T45, T56, T0_6;
T01 << c1, 0, -s1, a1 * c1,
s1, 0, c1, a1 * s1,
0, -1, 0, 0,
0, 0, 0, 1;
T12 << c2, -s2, 0, a2 * c2,
s2, c2, 0, a2 * s2,
0, 0, 1, 0,
0, 0, 0, 1;
T23 << c3, 0, -s3, a3 * c3,
s3, 0, c3, a3 * s3,
0, -1, 0, 0,
0, 0, 0, 1;
T34 << c4, 0, s4, 0,
s4, 0, -c4, 0,
0, 1, 0, d4,
0, 0, 0, 1;
T45 << c5, 0, -s5, 0,
s5, 0, c5, 0,
0, -1, 0, 0,
0, 0, 0, 1;
T56 << c6, -s6, 0, 0,
s6, c6, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
T0_6 = T01 * T12 * T23 * T34 * T45 * T56;
return T0_6;
}
四.6轴机器人运动学逆解
代码
#include
#include
#define EPSINON 1e-4 // 精度，小于它就认为是0
#define ALLOW_MAX_DIFF (1 / 180 * pi) /* 距离上一轴角度的最大允许偏差 */
/* 各轴运动角度范围 */
const double MAX_JOINTS[6] = { 90, 30, 90, 135, 135, 180};
const double MIN_JOINTS[6] = { -90, -135, -135, -135, -135, -180};
const double pi = 3.1415926
/* 角度值转弧度值 */
inline double toRadian(double degree)
{
return degree * pi / 180;
}
/* 逆解函数
* @Joint 6轴各关节值结构体，单位弧度
* @lastJoints 上一次计算的关节值结构体
* @pose 要解逆解的位姿矩阵
*/
Joint IFKinematics::sixJointsIK(Eigen::Matrix4d pose, Joint lastJoints)
{
double nx = pose(0, 0), ox = pose(0, 1), ax = pose(0, 2),
ny = pose(1, 0), oy = pose(1, 1), ay = pose(1, 2),
nz = pose(2, 0), oz = pose(2, 1), az = pose(2, 2),
px = pose(0 ,3), py = pose(1, 3), pz = pose(2, 3);
/** joint 1 */
double j1[2] = {0, 0};
j1[0] = -atan2(-py, px);
j1[1] = -atan2(-py, px) + pi;
/** joint 3 */
double j3[4] = {0, 0, 0, 0};
/* temp, x, y, z, A are temp valiables */
double temp[2], A[2];
double x = a2 * d4,
y = a2 * a3,
z = a2 * a2 + a3 * a3 + d4 * d4;
for (int i = 0; i < 4; i+=2) {
temp[i/2] = a1 - px * cos(j1[i/2]) - py * sin(j1[i/2]);
A[i/2] = z - pz * pz - temp[i/2] * temp[i/2];
j3[i] = -atan2(-y, x) + atan2(A[i/2],sqrt( (4 * x * x + 4 * y * y - A[i/2] * A[i/2])));
j3[i+1] = -atan2(-y, x) + atan2(A[i/2],-sqrt((4 * x * x + 4 * y * y - A[i/2] * A[i/2])));
}
/** joint 2 */
double j2[4] = {0, 0, 0, 0};
double m[4] = {0, 0, 0, 0};
double n[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; ++i) {
m[i] = a2 + a3 * cos(j3[i]) - d4 * sin(j3[i]);
n[i] = a3 * sin(j3[i]) + d4 * cos(j3[i]);
j2[i] = atan2(-pz * m[i] + n[i] * temp[i/2],-pz * n[i] - m[i] * temp[i/2]);
}
/** joint 5 */
double j5[8] = {0, 0, 0, 0, 0, 0, 0, 0};
for (int i = 0; i < 4; ++i) {
m[i] = (ax * cos(j1[i / 2]) * cos(j2[i]) + ay * sin(j1[i / 2]) * cos(j2[i]) + az * -1 * sin(j2[i]));
n[i] = -(ax * cos(j1[i / 2]) * sin(j2[i]) + ay * sin(j1[i / 2]) * sin(j2[i]) - az * -1 * cos(j2[i]));
}
for (int i = 0; i < 8; ) {
x = n[i/2] * cos(j3[i/2]) - m[i/2] * sin(j3[i/2]);
y = sqrt(pow((ay * cos(j1[i / 4]) - ax * sin(j1[i / 4])),2) + pow((m[i/2] * cos(j3[i/2]) + n[i/2] * sin(j3[i/2])),2));
j5[i++] = atan2( y, x);
j5[i++] = atan2(-y, x);
}
/** joint 4 */
double j4[8] = {0, 0, 0, 0, 0, 0, 0, 0};
for (int i = 0; i < 8; ++i) {
if (fabs(sin(j5[i])) >= EPSINON) {
y = ((ay*cos(j1[i/4])-ax*sin(j1[i/4]))) / sin(j5[i]);
x= (-m[i/2]*cos(j3[i/2])-n[i/2]*sin(j3[i/2]))/(sin(j5[i]));
j4[i] = atan2(y , x);
} else {
j4[i] = 0;
}
}
/** joint 6 */
double j6[8] = {0, 0, 0, 0, 0, 0, 0 ,0};
for (int i = 0; i < 8; ++i) {
double u = nx * sin(j1[i/4]) - ny * cos(j1[i/4]);
double v = ox * sin(j1[i/4]) - oy * cos(j1[i/4]);
j6[i] = atan2(cos(j4[i]) * u - cos(j5[i]) * sin(j4[i]) * v, cos(j4[i]) * v + cos(j5[i]) * sin(j4[i]) * u);
}
/** 8组逆解结果 */
double ans[8][6];
for (int i = 0; i < 8; ++i) {
ans[i][0] = j1[i / 4];
ans[i][1] = j2[i / 2];
ans[i][2] = j3[i / 2];
ans[i][3] = j4[i];
ans[i][4] = j5[i];
ans[i][5] = j6[i];
}
/** 在选择8组解中的最优解，主要根据三个原则：连续原则、限位原则、最短路径原则 ，连续原则暂时不使用 */
double last[6] = {lastJoints.j1, lastJoints.j2, lastJoints.j3, lastJoints.j4, lastJoints.j5, lastJoints.j5};
bool isOutLimit[8] = {false, false, false, false, false, false, false, false};
//bool isContinuity[8] = {true, true, true, true, true, true, true, true};
/** 排除掉超过运动范围的解 */
for (int i = 0; i < 8; ++i) {
for (int j = 0; j < 6; ++j) {
if (ans[i][j] < toRadian(MIN_JOINTS[j]) || ans[i][j] > toRadian(MAX_JOINTS[j])) {
isOutLimit[i] = true;
break;
}
}
}
/** 运动连续：排除掉离上一轴角度过大的解，需要设定最大允许偏差值 ALLOW_MAX_DIFF */
// for (int i = 0; i < 8; ++i) {
// for (int j = 0; j < 6; ++j) {
// if (abs(ans[i][j] - last[j]) > ALLOW_MAX_DIFF) {
// isContinuity[i] = false;
// break;
// }
// }
// }
/** 最短路径原则：选出距离上一次姿态各轴所需最小的运动量，给各轴分配一个权重，使尽量动小轴或姿态轴 */
double weight[6] = {0.2, 0.25, 0.2, 0.15, 0.12, 0.08};
//double weight[6] = {1, 1, 1, 1, 1, 1}; // 各轴相同权重
double sum[8] = {0, 0, 0, 0, 0, 0, 0, 0};
double minSum = INT_MAX;
int opt = -1;
for (int i = 0; i < 8; ++i) {
if (isOutLimit[i]) {
//printf("第%d解超限\n", i + 1);
continue;
}
for (int j = 0; j < 6; ++j) {
sum[i] += fabs(ans[i][j] - last[j]) * weight[j];
}
if (sum[i] < minSum) {
minSum = sum[i];
opt = i;
}
}
/** 最优解 */
Joint res;
if (opt >= 0) {
/// 因为4轴和6轴平行,因此4轴转-x°和6轴转x°效果相同,因此如果遇到这种情况就将他们矫正
if (-ans[opt][3] - ans[opt][5] <= 1e-2) {
ans[opt][3] = 0;
ans[opt][5] = 0;
}
res = Joint(ans[opt][0], ans[opt][1], ans[opt][2],
ans[opt][3], ans[opt][4], ans[opt][5]);
} else {
printf("无合适逆解\n");
res = lastJoints;
}
return res;
}