MATLAB六自由度机械臂正逆运动_fun_dirsolu_mdh

实现效果

1、正运动学求解工作空间

2、逆运动学求解转角

定义数学函数

X轴旋转矩阵（fun_dirsolu_trotx.m）

function Rx = fun_dirsolu_trotx(x,y)
if nargin > 1 && strcmp(y, 'd')
    x = x *pi/180;
end
Rx=[1 0 0 0;0 cos(x) -sin(x) 0;0 sin(x) cos(x) 0;0 0 0 1];
end
1
2
3
4
5
6

Y轴旋转矩阵（fun_dirsolu_troty.m）

function Ry = fun_dirsolu_troty(y,x)
if nargin > 1 && strcmp(x, 'd')
    y = y *pi/180;
end
Ry=[cos(y) 0 sin(y) 0;0 1 0 0;-sin(y) 0 cos(y) 0;0 0 0 1];
end
1
2
3
4
5
6

Z轴旋转矩阵（fun_dirsolu_trotz.m）

function Rz = fun_dirsolu_trotz(z,x)
if nargin > 1 && strcmp(x, 'd')
    z = z *pi/180;
end
Rz=[cos(z) -sin(z) 0 0;sin(z) cos(z) 0 0;0 0 1 0;0 0 0 1];
end
1
2
3
4
5
6

平移矩阵（fun_dirsolu_transl.m）

function D = fun_dirsolu_transl(x,y,z)
D=[1 0 0 x;0 1 0 y;0 0 1 z;0 0 0 1];
end
1
2
3

定义DH变换矩阵函数

定义MDH变换矩阵（fun_dirsolu_mdh.m）

% % %主要实现：空间坐标变换，利用DH参数求解i-1到i绕自身按顺序的变化的矩阵。
% % %注意：2018版本MATLAB使用时，必须将pi定义为字符即包含syms pi的声明！！(有时候单独定义pi会报错，可以不加分号定义pi)

% % %例如仅仅只是定义了theta求变化矩阵时：
% % % syms theta;
% % % fun_dh(theta,0,0,pi/2)
% 结果为：(4967757600021511*sin(theta))/81129638414606681695789005144064,出错！！

% % %当把pi定义好后：
% % % syms theta;
% % % syms pi   %此时不加分号，防止报错。
% % % fun_dh(theta,0,0,pi/2)
% 结果为：[ cos(theta), -sin(theta),  0, 0]
% % % % % [          0,           0, -1, 0]
% % % % % [ sin(theta),  cos(theta),  0, 0]
% % % % % [          0,           0,  0, 1]
% % %----------------------------------------------------------------------------------

function T = fun_dirsolu_mdh(theta,d,a,alpha)
T = fun_dirsolu_trotx(alpha)*fun_dirsolu_transl(a,0,0)*fun_dirsolu_trotz(theta)*fun_dirsolu_transl(0,0,d);
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

定义SDH变换矩阵（fun_dirsolu_sdh.m）

function T = fun_dirsolu_sdh(theta,d,a,alpha)
T = fun_dirsolu_trotz(theta)*fun_dirsolu_transl(0,0,d)*fun_dirsolu_trotx(alpha)*fun_dirsolu_transl(a,0,0);
end
1
2
3

MDH和SDH的区别

参考连接Robotic toolbox10.2的fkine函数与机械臂MDH和DH变化矩阵的应用

定义辅助计算函数

矩阵化简函数（fun_round_matrix.m）

矩阵化简函数，保证矩阵最后效果是数值型的

function T = fun_round_matrix(A)
[a,b] = size(A);
for x = 1:a
    for y = 1:b
        T(x,y) = roundn(double(A(x,y)),-3);
    end
end
end
1
2
3
4
5
6
7
8

辅助sin函数（fun_revsolu_sin.m）

function s = fun_revsolu_sin(n)
d=sin(n);
if d<0.001
    s = 0;
else
    s=d;
end
end
1
2
3
4
5
6
7
8

辅助cos函数（fun_revsolu_cos.m）

function c = fun_revsolu_cos(n)
d=cos(n);
if d<0.001
    c = 0;
else
    c=d;
end
end
1
2
3
4
5
6
7
8

正运动学

1、定义DH参数
2、套用DH矩阵
3、求解工作空间

主文件（自命名.m）

1、用于求解关节角的旋转矩阵

2、用于求解正运动学工作空间

3、包括逆运动学的调用

4、示例的机械臂为KUKA的，DH参数为：

clear;clc;
syms theta1 theta2 theta3 theta4 theta5 theta6; 

% % 定义机械臂DH参数，其中L=[theta d a alpha]
L1 = [theta1,       1045,     0,       0];
L2 = [theta2,       0,        500,     -pi/2];
L3 = [theta3+pi/2,  0,        1300,    0];
L4 = [theta4,       1025,     55,      pi/2];
L5 = [theta5,       0,        0,       pi/2];
L6 = [theta6+pi,    0,        0,       -pi/2];

T01 = fun_dirsolu_mdh(L1(1),L1(2),L1(3),L1(4));
T12 = fun_dirsolu_mdh(L2(1),L2(2),L2(3),L2(4));
T23 = fun_dirsolu_mdh(L3(1),L3(2),L3(3),L3(4));
T34 = fun_dirsolu_mdh(L4(1),L4(2),L4(3),L4(4));
T45 = fun_dirsolu_mdh(L5(1),L5(2),L5(3),L5(4));
T56 = fun_dirsolu_mdh(L6(1),L6(2),L6(3),L6(4));

T06 = T01*T12*T23*T34*T45*T56;

% % 求解变换矩阵，T1赋值，round_matrix化简打印。
% T1=fun_round_matrix(subs(T06,{theta1,theta2,theta3,theta4,theta5,theta6},{1.2,0.3,1,0.2,1.5,0.8}));
% T2=fun_round_matrix(subs(T06,{theta1,theta2,theta3,theta4,theta5,theta6},{0,0,0,0,0,0}));

% % 求出八组逆解。
% [S,Q] = fun_revsolu_Kuka6D(T1);

% % % % % 求解工作空间，R赋值，dirsolu_6D_workspace.m绘制工作空间。
% R = [-pi pi;-(13/18)*pi (2/18)*pi;(-10/18)*pi (14/18)*pi;-pi pi;-pi pi;-pi pi];
% dirsolu_6D_workspace;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

正运动学求解工作空间（dirsolu_6D_workspace.m）

1、蒙特卡罗法求解工作空间

2、将记录的点放入数据中，不用hold on绘图 节省资源

3、开启并行计算

%主要实现：利用正运动学求解工作空间。
%注意：(不能直接运行！)
%1、随机函数N决定lim_theta角度的离散数值范围，rand(N,1)表示新建N个0~1的随机数，1表示1列，如果1改成10则
%是10列数，每列都是N个。如在theta1中用限制为[-pi,pi]那么为了使得离散的lim_theta1也在[-pi,pi]之间
%，因此其值为-pi+2*pi*rand(N,1)。得出结论X=[a,b]时，则离散范围X=a+(b-a)*rand(N,1)；

%2、for循环的n表示绘制点图时点的个数；

%3、subs函数对syms变量赋值，求解末端{6}的坐标系原点[0;0;0]相对基座坐标{0}的位置，由于T06是4X4矩阵，
%因此在{6}原点坐标后补1，用[0;0;0;1]表示其原点位置。
if exist('T06') && exist( 'R')  
    point=15000;
    lim_theta1=R(1,1)+(R(1,2)-R(1,1))*rand(point,1);
    lim_theta2=R(2,1)+(R(2,2)-R(2,1))*rand(point,1);
    lim_theta3=R(3,1)+(R(3,2)-R(3,1))*rand(point,1);
    lim_theta4=R(4,1)+(R(4,2)-R(4,1))*rand(point,1);
    lim_theta5=R(5,1)+(R(5,2)-R(5,1))*rand(point,1);
    lim_theta6=R(6,1)+(R(6,2)-R(6,1))*rand(point,1);
 
    parfor n=1:1:point
        p=subs(T06,{theta1 theta2 theta3 theta4 theta5 theta6},{lim_theta1(n),lim_theta2(n), ...
            lim_theta3(n),lim_theta4(n),lim_theta5(n),lim_theta6(n)})*[0;0;0;1];
        q(n,:) = p;
    end
    q = double(q);
    qx = q((1:point),1);
    qy = q((1:point),2);
    qz = q((1:point),3);
    
else
    disp('---------ERROR----------')
end
% % %----------------------------------------------------------------------------------
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

逆运动学

逆运动学数学推导

其余限制条件，见代码。

逆运动学函数（fun_revsolu_Kuka6D.m）

最后返回结果为逆解矩阵和符合条件的逆解个数。

function [S,Q] = fun_revsolu_Kuka6D(T)
%FUN_REVSOLU_KUKA6D 此处显示有关此函数的摘要
%   此处显示详细说明
r11=T(1,1);r12=T(1,2);r13=T(1,3);px=T(1,4);
r21=T(2,1);r22=T(2,2);r23=T(2,3);py=T(2,4);
r31=T(3,1);r32=T(3,2);r33=T(3,3);pz=T(3,4);

a1=500;a2=1300;a3=55;
d1=1045;d4=1025;

n=1;

theta1=atan(py/px);
c1=fun_revsolu_cos(theta1);s1=fun_revsolu_sin(theta1);

K=(px*px+py*py+(pz-d1)*(pz-d1)+a1*a1-a2*a2-a3*a3-d4*d4-2*a1*c1*px-2*a1*s1*py)/(2*a2);
KI = a3^2+d4^2-K^2;
if(KI <0 || abs(px)>2826 || abs(py)>2826 || pz >3700 || pz <-1045)
    S = 0;
    Q = [];
else
    for i=0:1
        theta3 = atan2(d4,a3)-atan2(K,(-1)^i*sqrt(abs(a3^2+d4^2-K^2)));
        c3=fun_revsolu_cos(theta3);s3=fun_revsolu_sin(theta3);

        ats23 = (a3-a2*s3)*(-c1*px-s1*py+a1)+(d4+a2*c3)*(-pz+d1);
        atc23 = (a3-a2*s3)*(-pz+d1)+(d4+a2*c3)*(c1*px+s1*py-a1);  
        theta23 = atan2(ats23,atc23);
        theta2 = theta23-theta3;
        c2=fun_revsolu_cos(theta2);s2=fun_revsolu_sin(theta2);
        s23=fun_revsolu_sin(theta2+theta3);c23=fun_revsolu_cos(theta2+theta3);

        ats4 = -r13*s1+r23*c1;
        atc4 = -r13*c1*s23-r23*s1*s23-r33*c23;
        for j = 0:1
            theta4 = atan2(ats4,atc4) + j*pi;
            c4=fun_revsolu_cos(theta4);s4=fun_revsolu_sin(theta4);

            ats5 = -r23*(c1*s4-c4*s1*s23)+r13*(s1*s4+c1*c4*s23)+c4*c23*r33;
            atc5 = c1*c23*r13-r33*s23+c23*r23*s1;
            for k =0:1
                theta5 =(-1)^k*atan2(ats5,atc5);
                c5=fun_revsolu_cos(theta5);s5=fun_revsolu_sin(theta5);

                ats6 = r22*(c5*(c1*s4-c4*s1*s23)+c23*s1*s5)-r12*(c5*(s1*s4+c1*c4*s23)-c1*c23*s5) - r32*(s5*s23 + c4*c5*c23);
                atc6 = r12*(c4*s1-c1*s4*s23)-r22*(c1*c4+s1*s4*s23)-c23*r32*s4;
                theta6 = atan2(ats6,atc6);

                Q(n,:) = fun_round_matrix([theta1 theta2 theta3 theta4 theta5 theta6]);
                n = n+1;
            end
        end
    end

    S = 0;

    for i = 1:8
        if(-3.14<Q(i,1) && Q(i,1)<3.14 && -2.267<Q(i,2) && Q(i,2)<0.349 && -1.744<Q(i,3) && Q(i,3)<2.512)
            if(-3.14<Q(i,4) && Q(i,4)<3.14 && -3.14<Q(i,5) && Q(i,5)<3.14 && -3.14<Q(i,6) && Q(i,6)<3.14)
                S = S+1;
            end 
        end
    end
end

end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

0积分资源

0积分资源连接