幻灯片2.PNG
幻灯片3.PNG
幻灯片4.PNG
幻灯片5.PNG

概述

本次机器人学项目二要求对如下图所示的四六轴机器人在 MATLAB 平台进行仿真。同时要仿真出相应的工作空间及相关正逆运动学，最终完成 P 及 P’ 点的三维空间描绘。

图1.1 图1.2

具体内容

四轴机器人工作空间

工作空间是当机械手的所有关节进行所有可能的运动时，末端执行器坐标系的原点所描述的区域。工作空间在习惯上被区分为可达工作空间和灵活工作空间。后者是可以从不同的方向到达时末端执行器坐标系原点所描述的区域，而前者是至少从一个方向可以到达时末端执行器坐标系原点所描述的区域。
因此我们绘制计算的是可达工作空间。而又因为本次的SCARA系列机器人的第四轴为长度200mm的在z轴方向伸缩的杆件，所以将其简化。
考虑如图2.1的平面机器人。设g=（x，y，φ）表示末端执行器的位姿。机构的运动学正解可用指数积公式推导，但根据片面几何关系推导将更容易。
这里取L1=250mm，L2=200mm。
满足以下关系：
$$ x=l_1\cos⁡\theta_1+l_2\cos⁡(\theta_1+\theta_2) $$
$$ y=l_1\sin\theta_1+l_2\sin(\theta_1+\theta_2) $$
$$ \phi=\theta_1+\theta_2 \tag{2.1} $$

图2.1.1 图2.1.2

图2.1.3

六轴机器人工作空间

四轴正向运动学

考虑到SCARA机器人由三个转动关节和一个移动关节组成。因此要求运动学正解，取$ \theta=0 $对应于图示位形，并建立基础坐标系和工具坐标系。
这里取L0=343mm，L1=250mm，L2=200mm。
具体计算步骤为：

  1. $ \theta=0 $时，计算基础坐标系和工具坐标系的变换g0
  2. 构造转动关节的运动旋量
  3. 取轴线上的点
  4. 计算由此产生的运动旋量
  5. 分别写出各个指数
  6. 计算用指数积公式展开
    具体公式及代码详见附录。

六轴逆向运动学

逆运动学是由给定的末端执行器位置和方向，确定相对应的关节变量。逆运动学可能遇到的问题包括：

  1. 要求解的方程通常是非线性的，因而并非总能找到一个闭合形式的解。
  2. 可能存在多重解。
  3. 可能存在无穷多解。
  4. 从机械手运动学结构的角度，可能存在可行解。
    （在一开始无从下手的时候参用了DH的方法以及机器人工具箱）
    得出Denavit-Hartenberg参数表
    坐标系确定：
    坐标系z轴，与各关节旋转中心轴线重合
    坐标系x轴，与沿着相邻z轴的公垂线重合
    坐标系y轴，通过右手定则确定
$ \theta_i $ $ d_i $ $ a_i $ $ \alpha_i $
$ l_0-l_1 $ 0 491 0 90
$ l_1-l_2 $ 90 0 350 0
$ l_2-l_3 $ 0 0 0 90
$ l_3-l_4 $ 0 350 0 90
$ l_4-l_5 $ 90 0 0 90
$ l_5-l_6 $ 0 -84 0 0

正解算法

正解算法
根据DH表得到六个连杆变换矩阵：
T1 = [c1 0 s1 0;s1 0 -c1 0;0 1 0 491;0 0 0 1];
T2 = [-s2 -c2 0 -350s2;c2 -s2 0 350c2;0 0 1 0;0 0 0 1];
T3 = [c3 0 s3 0;s3 0 -c3 0;0 1 0 0;0 0 0 1];
T4 = [c4 0 s4 0;s4 0 -c4 0;0 1 0 350;0 0 0 1];
T5 = [-s5 0 c5 0;c5 0 s5 0;0 1 0 0;0 0 0 1];
T6 = [c6 -s6 0 0;s6 c6 0 0;0 0 1 -84;0 0 0 1];

T = T1T2T3T4T5*T6

得到位置计算式：
p_x=350c1c2c3-84s5*(c1c2c3-c1s2s3)-84c5(s1s4-c4(c1c2s3+c1c3s2))- 350c1s2-350c1s2s3
p_y=84c5*(c1s4 + c4(c2s1s3 + c3s1s2)) - 84s5(c2c3s1 - s1s2s3) - 350s1s2+ 350c2c3s1 - 350s1s2s3
p_z=350c2+350c2s3+350c3s2-84s5*(c2s3+c3s2)-84c4c5*(c2c3-s2s3)+491

得到姿态计算式：
r_11=c6*(c5*(c1c2c3 - c1s2s3) - s5*(s1s4 - c4(c1c2s3 + c1c3s2))) - s6*(c4s1 + s4(c1c2s3 + c1c3s2))
r_12=- s6*(c5*(c1c2c3 - c1s2s3) - s5*(s1s4 - c4(c1c2s3 + c1c3s2))) - c6*(c4s1 + s4(c1c2s3 + c1c3s2))
r_13= s5*(c1c2c3 - c1s2s3) + c5*(s1s4 - c4(c1c2s3 + c1c3s2))
r_21=c6*(c5*(c2c3s1 - s1s2s3) + s5*(c1s4 + c4(c2s1s3 + c3s1s2))) + s6*(c1c4 - s4(c2s1s3 + c3s1s2))
r_22=c6*(c1c4 - s4(c2s1s3 + c3s1s2)) - s6*(c5*(c2c3s1 - s1s2s3) + s5*(c1s4 + c4(c2s1s3 + c3s1s2)))
r_23=s5*(c2c3s1 - s1s2s3) - c5*(c1s4 + c4(c2s1s3 + c3s1s2))
r_31=c6*(c5*(c2s3 + c3s2) - c4s5(c2c3 - s2s3)) + s4s6(c2c3 - s2s3)
r_32=c6s4(c2c3 - s2s3) - s6*(c5*(c2s3 + c3s2) - c4s5(c2c3 - s2s3))
r_33=s5*(c2s3 + c3s2) + c4c5(c2c3 - s2s3)

逆解算法

借助工具进行了建模：
image.png
后来根据PPT的引导以及网上关于牛顿迭代法的介绍，写了逆解的算法
image.png
列出了大致的框架，设定初始值、步长、精度、和迭代次数去运行，用正解得到的点位姿反推逆解，经过伴随矩阵转换算得雅可比矩阵以及计算雅可比伪逆矩阵，并在这中间不断迭代得到新的角，得到的数值解结果接近于给定的θ值。但并未对接到主程序所要求的Scara的点上。

六轴正向运动学

正运动学方程是建立关节变量与末端执行器位置和方向之间的函数关系。六轴机器人要求运动学正解，为了便于运算，取$ \theta=0 $，并建立基础坐标系和工具坐标系。这里取L0=491mm，L1=350mm，L2=350mm。
具体公式及代码详见附录。

四六轴奇异点

奇异点问题主要是由于机器人在某一特殊状态，速度方程的雅可比逆解矩阵无法得到准确解，造成失速的现象。机器人的奇异位形是机器人雅可比矩阵降秩时所处的位形。
对于四轴SCARA机器人的雅可比矩阵。考虑处于任意位形的SCARA机器人，其空间雅可比矩阵可以通过当前位形各关节的运动螺旋求得。对于SCARA机器人，运动螺旋的方向固定，只有各运动螺旋的轴线是所经过的点是关于θ的函数。
image.png
接着通过rank求秩函数，对雅可比矩阵求秩。
image.png

结果发现，当$ \theta1=\theta2=0 $，秩为3。而其他取值情况秩为4。此时机器人雅可比矩阵降秩并且逆运动学问题可能存在无穷多解，因此$ \theta1=\theta2=0 $时，会导致奇异点。
而对于六轴机器人的奇异点。根据书上的定义有以下三种情况：
一，共轴线的两个转动关节
其运动螺旋$ \xi4 $以及$ \xi6 $满足条件：

  1. 关节轴线平行：$ \omega4=\omega6 $.
  2. 关节轴线共线：$ \omega i\times(q1-q2) i=4,6 $
    因此当 4轴和6轴平行时，该六轴机器人的雅可比矩阵奇异，即找到奇异点。

二，轴线平行且共面的三个转动关节
其2轴3轴5轴满足条件：

  1. 关节轴线平行：$ \omega i=\omega j (i,j=2,3,5) $
  2. 关节轴线共面：存在一个平面，其单位法向量n满足 $ n^T\omega i=0 $
    和$n^t(qi-qj) (i,j=2,3,5) $
    因此当2轴3轴5轴轴线平行且共面时，该六轴机器人的雅可比矩阵奇异，即找到奇异点。

三，轴线交汇于一点的四个转动关节
其1轴4轴5轴6轴满足条件：
1.轴线交汇于一点：$ \omega i\times(qi-q) , i=1,4,5,6 $
因此当1轴4轴5轴6轴轴线交汇于一点时，该六轴机器人的雅可比矩阵奇异，即找到奇异点。

代码

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
function out = SixDoForwardKinematics(SixDoFAngle)
% 正运动学验证六轴机器人逆解结果准确性

    theta1 = SixDoFAngle(1); % theta1为轴1转动角度，以下类推
    theta2 = SixDoFAngle(2); 
    theta3 = SixDoFAngle(3); 
    theta4 = SixDoFAngle(4); 
    theta5 = SixDoFAngle(5); 
    theta6 = SixDoFAngle(6);

    I = [1 0 0; 0 1 0; 0 0 1]; % 三阶单位阵

    omega1 = [0 0 1]; % 轴1，以下类推
    omega2 = [-1 0 0];
    omega3 = omega2; 
    omega4 = [0 1 0];
    omega5 = omega2;
    omega6 = omega1;

    q1 = [0 0 0]';   % 轴1上的一个点，以下类推
    q2 = q1;
    q3 = q1;
    q4 = [0 0 841]'; 
    q5 = [0 350 841]';
    q6 = [0 350 841]';
    

    pA1 = ComputePoseArray(omega1,theta1,q1); % 轴1位形矩阵，以下类推
    pA2 = ComputePoseArray(omega2,theta2,q2);
    pA3 = ComputePoseArray(omega3,theta3,q3);
    pA4 = ComputePoseArray(omega4,theta4,q4);
    pA5 = ComputePoseArray(omega5,theta5,q5);
    pA6 = ComputePoseArray(omega6,theta6,q6);
    
    p0 = [0 350 757]'; % 初始状态下，机器人工具端在基础坐标系的坐标
    pA0 = [I p0; 0 0 0 1]; % 初始状态下，机器人工具端位形矩阵

    out = pA1*pA2*pA3*pA4*pA5*pA6*pA0;
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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
function out = SixDoFInverseKinematics(g_d, distance, ErrorThreshold, MaxIterationsNum, StepSize)
% 六轴机械臂逆运动学计算（数值解方式）
   
    % 进行位形矩阵转化，转化到基于六轴基座的空间坐标系下
    I = eye(3);
    DisVec = [0; distance; 0];
    TransMat = [I DisVec; 0 0 0 1];
    gTarget = TransMat * g_d; % 待拟合的位形矩阵

    % 初始化会用到的数据
    SixDoFAngle = zeros(6, 1);  % 将角度值初始化为0
    error = ErrorThreshold + 1; % 将误差初始化为一个大于阈值的数，确保正确进入循环
    LoopCounter = 1; % 计数器置1
    ErrorArray = []; % 初始化一个误差数组用来保存迭代过程中的误差
    
    % Newton-Raphson算法
    while (error > ErrorThreshold) && (LoopCounter < MaxIterationsNum) % 当误差进入误差带或者迭代次数达到最大时跳出循环
        % 计算收敛误差
        gCal = SixDoFForwardKinematics(SixDoFAngle, 'matrix');  % 当前角度值计算出来的矩阵
        Phi =  CalPhi(gTarget, gCal); % 6*1的矩阵
        error = norm(Phi); % 取模
        ErrorArray(LoopCounter) = error; % 保存绘图参数
        
        % 进行角度更新
        JacobianMat = CalJacobianMat(SixDoFAngle); % 计算雅可比矩阵
        jShape = pinv(JacobianMat); % 计算雅可比矩阵的伪逆矩阵
        SixDoFAngle = SixDoFAngle - StepSize * jShape * Phi; % 更新角度

        LoopCounter  =LoopCounter + 1; % 计数器累加
    end
    
    % 绘图
    PlotErrorCurve(ErrorArray, linspace(1, LoopCounter-1, LoopCounter-1));
    
    out = SixDoFAngle'; % 返回最终结果
end


function out = CalPhi(gTarget, gCal)
% 计算收敛误差

    LogMat = logm(gCal * inv(gTarget));
    out = vee(LogMat);

end


function out = CalJacobianMat(SixDoFAngle)
% 计算雅可比矩阵
    
    omega = [ 0 0 1;
                     -1 0 0;
                     -1 0 0;
                      0 1 0;
                     -1 0 0;
                      0 0 1];

    q = [0     0     0;
           0     0     0;
           0     0     0;
           0     0 841;
           0 350 841;
           0 350 841];
    
     JMat = zeros(6 ,6); % 初始化一个空矩阵，用于存放旋量
     ExpMat = eye(4); % 初始化指数矩阵，用于后面进行累乘
     
     % 利用循环计算伴随变换与雅可比矩阵
     for i = 1:6
        if i == 1 % 第一个旋量无需乘上伴随变换
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            JMat(:, i) = twist;
        else
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            ExpMat = ExpMat * ComputePoseArray(omega(i-1, :)', SixDoFAngle(i-1), q(i-1, :)'); % 计算指数矩阵
            Adg = CalAdjointTrans(ExpMat); % 计算伴随变换矩阵
            JMat(:, i) = Adg * twist;
        end  
     end
    
    out = JMat;
    
end

function PlotErrorCurve(y, x)
% 绘制误差曲线

    figure
    plot(x, y, 'bo-','LineWidth', 1, 'MarkerSize', 4)
    title('Convergence Rate')
    xlabel('Number of Iterations')
    ylabel('Norm of Phil')
end


function out = vee(m)
% 去帽操作

    out = [m(1:3,4); m(3,2); m(1,3);m(2,1)];

end 


function out = CalAdjointTrans(g)
% 计算伴随矩阵
        
    a1 = g(1, 4);
    a2 = g(2, 4);
    a3 = g(3, 4);
    
    pHat = [  0 -a3  a2;
                 a3    0 -a1;
                -a2   a1  0];
            
    R = g(1:3, 1:3);
    
    out = [R pHat*R; zeros(3, 3) R];
    
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
function out = SixDoFForwardKinematics(SixDoFAngle,resultForm)
% 正运动学验证六轴机器人逆解结果准确性

    theta1 = SixDoFAngle(1); % theta1为轴1转动角度，以下类推
    theta2 = SixDoFAngle(2); 
    theta3 = SixDoFAngle(3); 
    theta4 = SixDoFAngle(4); 
    theta5 = SixDoFAngle(5); 
    theta6 = SixDoFAngle(6);

    I = [1 0 0; 0 1 0; 0 0 1]; % 三阶单位阵

    omega1 = [0 0 1]; % 轴1，以下类推
    omega2 = [-1 0 0];
    omega3 = omega2; 
    omega4 = [0 1 0];
    omega5 = omega2;
    omega6 = omega1;

    q1 = [0 0 0]';   % 轴1上的一个点，以下类推
    q2 = q1;
    q3 = q1;
    q4 = [0 0 841]'; 
    q5 = [0 350 841]';
    q6 = [0 350 841]';
    q0 = [0 350 757]'; % 初始状态下，机器人工具端在基础坐标系的坐标

    pA1 = ComputePoseArray(omega1,theta1,q1); % 轴1位形矩阵，以下类推
    pA2 = ComputePoseArray(omega2,theta2,q2);
    pA3 = ComputePoseArray(omega3,theta3,q3);
    pA4 = ComputePoseArray(omega4,theta4,q4);
    pA5 = ComputePoseArray(omega5,theta5,q5);
    pA6 = ComputePoseArray(omega6,theta6,q6);
    pA0 = [I q0; 0 0 0 1]; % 初始状态下，机器人工具端位形矩阵

    impactPoseArraytool = pA1*pA2*pA3*pA4*pA5*pA6*pA0; % 运动后机器人工具端位形矩阵
    toolPoint = [impactPoseArraytool(1,4) impactPoseArraytool(2,4) impactPoseArraytool(3,4)];  % 运动后机器人工具端坐标点
    
    if resultForm == 'matrix'
        out = impactPoseArraytool;
    else
        out = toolPoint; % 选择输出的形式
    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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
clear;
clc;

r = 10;
StepSize = 0.1;
ErrorThreshold = 1;
MaxIterationsNum = 50;

g_d =[
   -0.0000   -0.5000    0.8660 -190.1372;
   -1.0000   -0.0000   -0.0000    0.0000;
    0.0000   -0.8660   -0.5000 -861.3274;
         0         0         0    1.0000]

theta1 = 10;theta2 = 10;theta3 = 20; 
theta4 = 10;theta5 = 20;theta6 = 30;

    I = [1 0 0; 0 1 0; 0 0 1]; % 三阶单位阵

    omega1 = [0 0 1]; % 轴1，以下类推
    omega2 = [-1 0 0];
    omega3 = [-1 0 0]; 
    omega4 = [0 1 0];
    omega5 = [-1 0 0];
    omega6 = [0 0 1];

    q1 = [0 0 0]';   % 轴1上的一个点，以下类推
    q2 = q1;
    q3 = q1;
    q4 = [0 0 841]'; 
    q5 = [0 350 841]';
    q6 = [0 350 841]';
    q0 = [0 350 757]'; % 初始状态下，机器人工具端在基础坐标系的坐标

    pA1 = ComputePoseArray(omega1,theta1,q1); % 轴1位形矩阵，以下类推
    pA2 = ComputePoseArray(omega2,theta2,q2);
    pA3 = ComputePoseArray(omega3,theta3,q3);
    pA4 = ComputePoseArray(omega4,theta4,q4);
    pA5 = ComputePoseArray(omega5,theta5,q5);
    pA6 = ComputePoseArray(omega6,theta6,q6);
    pA0 = [I q0; 0 0 0 1]; % 初始状态下，机器人工具端位形矩阵

    impactPoseArraytool = pA1*pA2*pA3*pA4*pA5*pA6*pA0; % 运动后机器人工具端位形矩阵
    toolPoint = [impactPoseArraytool(1,4) impactPoseArraytool(2,4) impactPoseArraytool(3,4)];  % 运动后机器人工具端坐标点

    
    
% 六轴机械臂逆运动学计算（数值解方式）
   
    % 进行位形矩阵转化，转化到基于六轴基座的空间坐标系下
    I = eye(3);
    DisVec = [0; r; 0];
    TransMat = [I DisVec; 0 0 0 1];
    gTarget = TransMat * g_d; % 待拟合的位形矩阵

    % 初始化会用到的数据
    SixDoFAngle = zeros(6, 1);  % 将角度值初始化为0
    error = ErrorThreshold + 1; % 将误差初始化为一个大于阈值的数，确保正确进入循环
    LoopCounter = 1; % 计数器置1
    ErrorArray = []; % 初始化一个误差数组用来保存迭代过程中的误差
    
    % Newton-Raphson算法
    while (error > ErrorThreshold) && (LoopCounter < MaxIterationsNum) % 当误差进入误差带或者迭代次数达到最大时跳出循环
        % 计算收敛误差
        gCal = SixDoFForwardKinematics(SixDoFAngle, 'matrix');  % 当前角度值计算出来的矩阵
        Phi =  CalPhi(gTarget, gCal); % 6*1的矩阵
        error = norm(Phi); % 取模
        ErrorArray(LoopCounter) = error; % 保存绘图参数
        
        % 进行角度更新
        JacobianMat = CalJacobianMat(SixDoFAngle); % 计算雅可比矩阵
        jShape = pinv(JacobianMat); % 计算雅可比矩阵的伪逆矩阵
        SixDoFAngle = SixDoFAngle - StepSize * jShape * Phi; % 更新角度

        LoopCounter  =LoopCounter + 1; % 计数器累加
    end
    
    % 绘图
    PlotErrorCurve(ErrorArray, linspace(1, LoopCounter-1, LoopCounter-1));
    
    SixInv = SixDoFAngle % 返回最终结果



function out = CalPhi(gTarget, gCal)
% 计算收敛误差

    LogMat = logm(gCal * inv(gTarget));
    out = vee(LogMat);

end


function out = CalJacobianMat(SixDoFAngle)
% 计算雅可比矩阵
    
    omega = [ 0 0 1;
                     -1 0 0;
                     -1 0 0;
                      0 1 0;
                     -1 0 0;
                      0 0 1];

    q = [0     0     0;
           0     0     0;
           0     0     0;
           0     0 841;
           0 350 841;
           0 350 841];
    
     JMat = zeros(6 ,6); % 初始化一个空矩阵，用于存放旋量
     ExpMat = eye(4); % 初始化指数矩阵，用于后面进行累乘
     
     % 利用循环计算伴随变换与雅可比矩阵
     for i = 1:6
        if i == 1 % 第一个旋量无需乘上伴随变换
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            JMat(:, i) = twist;
        else
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            ExpMat = ExpMat * ComputePoseArray(omega(i-1, :)', SixDoFAngle(i-1), q(i-1, :)'); % 计算指数矩阵
            Adg = CalAdjointTrans(ExpMat); % 计算伴随变换矩阵
            JMat(:, i) = Adg * twist;
        end  
     end
    
    out = JMat;
    
end
% 
function PlotErrorCurve(y, x)
% 绘制误差曲线

    figure
    plot(x, y, 'bo-','LineWidth', 1, 'MarkerSize', 4)
    title('Convergence Rate')
    xlabel('Number of Iterations')
    ylabel('Norm of Phil')
end

% 
function out = vee(m)
% 去帽操作

    out = [m(1:3,4); m(3,2); m(1,3);m(2,1)];

end 

% 
function out = CalAdjointTrans(g)
% 计算伴随矩阵
        
    a1 = g(1, 4);
    a2 = g(2, 4);
    a3 = g(3, 4);
    
    pHat = [  0 -a3  a2;
                 a3    0 -a1;
                -a2   a1  0];
            
    R = g(1:3, 1:3);
    
    out = [R pHat*R; zeros(3, 3) R];
    
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
function out = SCARAForwardKinematics(SCARAAngle,resultForm)
% 正运动学计算四轴机器人目标位置点

    theta1 = SCARAAngle(1); % theta1为轴1转动角度
    theta2 = SCARAAngle(2); % theta2为轴2转动角度
    theta3 = SCARAAngle(3); % theta3为轴3转动角度
    theta4 = SCARAAngle(4); % theta4为轴4移动距离

    I = [1 0 0; 0 1 0; 0 0 1]; % 三阶单位阵

    omega1 = [0 0 1]; % 轴1，以下类推
    omega2 = omega1;
    omega3 = omega2; 
    omega4 = [0 0 0]; % 轴4为纯移动

    q1 = [0 0 0]';   % 轴1上的一个点，以下类推
    q2 = [0 -250 395]';
    q3 = [0 -450 395]';
    q4 = [0 0 -theta4]'; % 轴4为纯移动
    q0 = [0 -450 325]'; % 初始状态下，机器人工具端在基础坐标系的坐标

    pA1 = ComputePoseArray(omega1,theta1,q1); % 轴1位形矩阵，以下类推
    pA2 = ComputePoseArray(omega2,theta2,q2);
    pA3 = ComputePoseArray(omega3,theta3,q3);
    pA4 = [I q4; 0 0 0 1];
    pA0 = [I q0; 0 0 0 1]; % 初始状态下，机器人工具端位形矩阵

    impactPoseArraytool = pA1*pA2*pA3*pA4*pA0; % 运动后机器人工具端位形矩阵
    toolPoint = [impactPoseArraytool(1,4) impactPoseArraytool(2,4) impactPoseArraytool(3,4)];  % 运动后机器人工具端坐标点
    
    if resultForm == 'matrix'
        out = impactPoseArraytool;
    else
        out = toolPoint; % 选择输出的形式
    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
function judgement = isIntArea(SCARAPoint,IntersecMatrix)
Intersectionmatrix = IntersecMatrix;
x = SCARAPoint(1);                    %用点法式方程求垂直于法线的平面，法线为由求解点到中心点的向量
y = SCARAPoint(2);
z = SCARAPoint(3);
CenterPoint = mean(Intersectionmatrix,1);  %求交集点的中心点
Cx = CenterPoint(1);
Cy = CenterPoint(2);
Cz = CenterPoint(3);
A = Cx - x;
B = Cy - z;
C = Cz - z;
    if(z>Cz)                                 %当求解点高于中点时，只要有一个交集点在平面上方，则求解点在交集内
        for j = 1:1:350000                    %否则，求解点不在交集内
        TestPoint = Intersectionmatrix(1,:);
        Tx = TestPoint(1);
        Ty = TestPoint(2);
        Tz = TestPoint(3);
            if(Tz > -(A*(Tx-x)+B*(Ty-y))/C)
                judgement = 1;
            break;
            else
                judgement = 0;
            end
        end
    else                                      %当求解点低于中点时，只要有一个交集点在平面下方，则求解点在交集内
                                                 %否则，求解点不在交集内
        for j = 1:1:350000
        TestPoint = Intersectionmatrix(1,:);
        Tx = TestPoint(1);
        Ty = TestPoint(2);
        Tz = TestPoint(3);
            if(Tz < -(A*(Tx-x)+B*(Ty-y))/C)
                judgement = 1;
            break;
            else
                judgement = 0;
            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
function FiniteStateMachine(state, distance, ErrorThreshold, MaxIterationsNum, StepSize)
% 有限状态机

    IntArea = CalIntArea(distance); % 计算工作空间的交集区域
    
    % 判断是否有交集区域
    if IntArea == 0
      warning('Two robotic arms have not intersection area of workspaces.') % 无交集区域，发出警告信息 
      return % 结束程序
    end
    
    % 进行状态机循环
    while 1
        switch  state
            case ProgramState.WaitForInput % 等待输入角度状态
                
                    SCARAAngle = input('Please input a array of angle in degrees: '); % 输入角度
                    
                    SCARAPoint = SCARAForwardKinematics(SCARAAngle, 'point'); % 正运动学计算四轴末端点位
                    judgement = isIntArea(SCARAPoint, distance); % 判断到达位置是否位于交集区域内
                    
                    if judgement == 1
                       state = ProgramState.ComputeAndPlot; % 输入有效，状态跳转
                    else
                       disp('Void input. Please input again.') % 输入无效，请求再次输入
                    end
                    
            case ProgramState.ComputeAndPlot % 机械臂计算并绘图状态
                
                SCARAgd = SCARAForwardKinematics(SCARAAngle, 'matrix'); % 计算基于SCARA基座空间坐标系的位形矩阵
                SixDoFAngle = SixDoFInverseKinematics(SCARAgd, distance, ErrorThreshold, MaxIterationsNum, StepSize); % 逆运动学计算六轴各关节角度
                PlotRoboticArm(SCARAAngle, SixDoFAngle, distance); % 绘制图像
                state = ProgramState.LoopEnquiry; % 状态跳转              
                
            case ProgramState.LoopEnquiry % 循环询问状态 
                
                answer = input('Would you continue to input a new array of  angle? Y/N: ','s'); % 输入Y/N
                
                if answer == 'Y'
                    state = ProgramState.WaitForInput; % 输入为Y，状态跳转
                elseif answer == 'N'
                    disp('Thanks for running our code!') % 输入为N，跳出循环，结束程序
                    break
                else
                    disp('Void input. Please input again.') % 输入无效，请求再次输入
                end
                
            otherwise
                warning('Unexpected state. Please set state to RobotState.WaitForInput.') % 状态无效，发出警告信息
                break
        end   
    end
end
1
2
3
4
5
6
7
8
9
10
11
12
13
function out = ComputePoseArray(omega,theta,q)
%求轴的位形矩阵

    I = [1 0 0; 0 1 0; 0 0 1]; % 三阶单位阵

    omegaHat = [0 -omega(3) omega(2);
                          omega(3) 0 -omega(1);
                         -omega(2) omega(1) 0;]; % 求ω^

    expm = I+omegaHat*sin(theta)+(omegaHat^2)*(1-cos(theta)); % 轴的矩阵指数

    out = [expm (I-expm)*q; 0 0 0 1];
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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
function [IntArea,IntersecMatrix] = CalIntArea(distance)
Intersectionmatrix = Intersec(distance);
if(Intersectionmatrix(1,1)~=0)
        a = 1;
    else
        a = 0;
    end
    IntArea = a;
    IntersecMatrix = Intersectionmatrix;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function IntersecM = Intersec(distance)               %输入两台机器人距离，输出交集矩阵的函数
Pi = 3.14;
i = 1;
Pointmatrix = zeros(350000,3);               %定义用来存储四轴坐标的矩阵，第一列为X，第二列为Y，第三列为Z
Intersectionmatrix = zeros(350000,3);
for theta1 = -135:5:135              %绘制SCARA工作空间
    for theta2 = -150:5:150
            for theta4 = 0:20:200
                theta3 = 2;
                Angle = [theta1*Pi/180,theta2*Pi/180,theta3*Pi/180,theta4];
                point = SCARAForwardKinematics(Angle,0);
                x = point(1);
                y = point(2);
                z = point(3);
               if ((x>-110)&&(x<110)&&(y>0)&&(y<450))
                else
                        Pointmatrix(i,:) = point;
                                                     
                end
                i = i + 1;
            end
     end
end
scatter3(Pointmatrix(:,1),Pointmatrix(:,2),Pointmatrix(:,3),'.blue');
i = 1;
figure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Pointmatrixfor6 = zeros(350000,3);
    for theta1 = -170:5:170              %绘制六轴工作空间
        for theta2 = -110:40:110
            for theta3 = -136:30:136
                for theta4 = -185:40:185
                       for theta5 = -210:40:30
                         theta6 = 0;
                        Angle = [theta1*Pi/180,theta2*Pi/180,theta3*Pi/180,theta4*Pi/180,theta5*Pi/180,theta6*Pi/180];
                        point = SixDoFForwardKinematics(Angle,0); 
                        Pointmatrixfor6(i,:) = point;
                        i = i + 1;
                       end
                end             
            end
        end
    end
scatter3(Pointmatrixfor6(:,1),Pointmatrixfor6(:,2),Pointmatrixfor6(:,3),'.red')
figure
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5555
                              %绘制交集区域，得出交集点矩阵
 Dy = distance;
                %机器人base之间的坐标差
 k = 1;                    
for i = 1:1:350000
     point1 = Pointmatrixfor6(i,:);
     x = point1(1);
     y = point1(2);
     z = point1(3);
   if((z>=125))&&(z<=325)
     if(x>80)
         if(((y-Dy)^2+x^2)<=202500)
             Intersectionmatrix(k,1)=x ;
             Intersectionmatrix(k,2)=y-Dy ;
             Intersectionmatrix(k,3)=z;
             k = k + 1;
         end
     elseif(x<-80)
         if(((y-Dy)^2+x^2)<=202500)
             Intersectionmatrix(k,1)=x ;
             Intersectionmatrix(k,2)=y-Dy ;
             Intersectionmatrix(k,3)=z;
             k = k + 1;
         end
     else
         if((((y-Dy)^2+x^2)<=202500)&&(((y-Dy)^2+x^2)>=6400)&&((y-Dy)<=0))
             Intersectionmatrix(k,1)=x ;
             Intersectionmatrix(k,2)=y-Dy ;
             Intersectionmatrix(k,3)=z;
             k = k + 1;
         end
         
     end
 
   end
end
 scatter3(Intersectionmatrix(:,1),Intersectionmatrix(:,2),Intersectionmatrix(:,3),'.green');
    IntersecM = Intersectionmatrix
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
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
Angle = [0 0   -90  0 0 0];
J = CalJacobianMat(Angle*pi/180)
d = det(J)
syms theta1 theta2 theta3 theta4;
SCARAAngle =[theta1,theta2,theta3,theta4]
S = CalJacobianMatforSCARA(SCARAAngle)

function out = CalJacobianMat(SixDoFAngle)
% 计算雅可比矩阵
    
    omega = [ 0 0 1;
                     -1 0 0;
                     -1 0 0;
                      0 1 0;
                     -1 0 0;
                      0 0 1];

    q = [0     0     0;
           0     0     491;
           0     0     841;
           0     0 841;
           0 350 841;
           0 350 841];
    
     JMat = zeros(6 ,6); % 初始化一个空矩阵，用于存放旋量
     ExpMat = eye(4); % 初始化指数矩阵，用于后面进行累乘
     
     % 利用循环计算伴随变换与雅可比矩阵
     for i = 1:6
        if i == 1 % 第一个旋量无需乘上伴随变换
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            JMat(:, i) = twist;
        else
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            ExpMat = ExpMat * ComputePoseArray(omega(i-1, :)', SixDoFAngle(i-1), q(i-1, :)'); % 计算指数矩阵
            Adg = CalAdjointTrans(ExpMat); % 计算伴随变换矩阵
            JMat(:, i) = Adg * twist;
        end  
     end
    
    out = JMat;
    
end
function out = CalJacobianMatforSCARA(SCARAAngle)
% 计算雅可比矩阵
   
    omega = [ 0 0 1;
                     0 0 1;
                     0 0 1;
                      0 0 0                     ];

    q = [0     0     0;
           0     -250     395;
           0     -450     395;
           0     0 -SCARAAngle(4)];
         
    
     JMat = sym(zeros(6 ,4)); % 初始化一个空矩阵，用于存放旋量
     ExpMat = eye(4); % 初始化指数矩阵，用于后面进行累乘
     
     % 利用循环计算伴随变换与雅可比矩阵
     for i = 1:4
        if i == 1 % 第一个旋量无需乘上伴随变换
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            JMat(:, i) = twist;
        else
            twist = [-cross(omega(i, :), q(i, :)) omega(i, :)]'; % 计算旋量
            ExpMat = ExpMat * ComputePoseArray(omega(i-1, :)', SCARAAngle(i-1), q(i-1, :)'); % 计算指数矩阵
            Adg = CalAdjointTrans(ExpMat); % 计算伴随变换矩阵
            JMat(:, i) = Adg * twist;
        end  
     end
    
    out = JMat;
    
end
function out = CalAdjointTrans(g)
% 计算伴随矩阵
        
    a1 = g(1, 4);
    a2 = g(2, 4);
    a3 = g(3, 4);
    
    pHat = [  0 -a3  a2;
                 a3    0 -a1;
                -a2   a1  0];
            
    R = g(1:3, 1:3);
    
    out = [R pHat*R; zeros(3, 3) R];
    
end
function out = ComputePoseArray(omega,theta,q)
%求轴的位形矩阵

    I = [1 0 0; 0 1 0; 0 0 1]; % 三阶单位阵

    omegaHat = [0 -omega(3) omega(2);
                omega(3) 0 -omega(1);
                -omega(2) omega(1) 0;]; % 求ω^

    expm = I+omegaHat*sin(theta)+(omegaHat^2)*(1-cos(theta)); % 轴的矩阵指数

    out = [expm (I-expm)*q; 0 0 0 1];
end