# การคำนวณหา Forward Kinematic และ Inverse Kinematic ของ Manipulator ด้วย MATLAB by Blogger2009

VIEWS: 551 PAGES: 14

• pg 1
```									                       Forward Kinematic Inverse
Kinematic F        MATLAB GUI

1             Assembly Part       Manipulator            F Home Position

Assembly               Manipulator Arm              F         F Home Position
Manipulator    F        F       45                        F   F       F            Solid work
F        F          F     F Link Parameter             Joint Parameter
F    F   F        D-H Parameter                            Transformation Matrix F
Dimension   F       F

2   Dimension   Base

2       Dimension       F (Shoulder)

4       Dimension    F     (Elbow)
Forward Kinematic Method

F     F                 Kinematic Diagram                  Frame          F

5      Kinematic Diagram

DH-Parameter
Link                      ai                αi                 θi                       d i (mm)
1                        0                 90                 0                             0
2                      456.7               0                 -45                        227.7
3                      502.9               0                  0                        -219.1

Transformation Matrix

Cθi    − Sθi Cθi    Sθi Sα i   ai Cθi 
 Sθ    Cθi Cα i    −Cθi Sα i   ai Sθi 
i −1
Ti =  i                                       .....................(1)
 0        Sα i       Cα i        di 
                                      
 0         0          0           1 

F           D-H parameter                  Transformation Matrix                 F
 cos(θ1 ) − sin(θ1 ) cos(θ1 ) sin(θ1 ) sin(α1 ) a1 cos(θ1 ) 
 sin(θ ) cos(θ ) cos(α ) − cos(θ ) sin(α ) a sin(θ ) 
0
T1 =       1          1        1          1        1  1      1 
.....................(2)
 0             sin(α1 )          cos(α1 )           d1      
                                                            
 0                 0                  0             1       

cos(θ 2 ) − sin(θ 2 ) cos(θ 2 ) sin(θ 2 ) sin(α 2 ) a2 cos(θ 2 ) 
 sin(θ ) cos(θ ) cos(α ) − cos(θ ) sin(α ) a sin(θ ) 
1
T2 =       2          2         2            2         2  2      2 
.....................(3)
 0             sin(α 2 )            cos(α 2 )           d2       
                                                                 
 0                 0                     0               1       

cos(θ3 ) − sin(θ3 ) cos(θ3 ) sin(θ3 )sin(α 3 ) a3 cos(θ3 ) 
 sin(θ ) cos(θ ) cos(α ) − cos(θ )sin(α ) a sin(θ ) 
2
T3 =       3         3        3          3        3  1      3 
.....................(4)
 0            sin(α 3 )          cos(α 3 )         d3      
                                                           
 0                0                  0             1       

F         D-H parameter                (2), (3), (4)       Transform         frame 3
frame 0            F

0
T3 = 0T1 1T2 2T3

F                     F                MATLAB GUI (                    Forward Kinematic)

Forward Kinematic
Inverse Kinematic: Geometric Approach

Arm                       F         F         F                                                F        Matrix Transformation                           F
function trigonometric                                                                        F            F Geometrics Approach                                          F
Inverse Kinematic                                                        F                     Manipulator                        Space F
F                F     F F θ1 , θ 2 , θ3            F                        F                       Manipulation               F            F            F

:                                  F               F     Space                          Manipulator                         Forward
Transformation Matrix                                        F                                                           Inverse Kinematic F
Geometrics Approach F                                            F          F

Forward Transformation Matrix                                                          F                   FF

 0.7071 0.7071 0 678.5397   nx                                                           ox           ax    px 
 0          0   −1   -8.6   n y                                                          oy           ay    py 
0
T3 =                             =                                                                                  
 -0.7071 0.7071 0 -678.5397   nz                                                         oz           az    pz 
                                                                                                               
 0          0   0     1      0                                                           0             0    1

F                    Manipulator                           F space                      F              F                   Px, Py, Pz

Px = 678.5397
Py = -8.6
Pz = -678.5397
F           Geometrics Approach                                                               F         θ1 , θ 2 , θ3              F
F                       Manipulator                     F Space F

6                     Kinematic Diagram                                   Manipulator
Z0       F              F θ1 F                    F
F         X 0 − Y0 plane

7               X 0 − Y0

 Py 
F F θ1 = tan −1        F                F                   Offset
 Px 
F (Shoulder)        F    (Elbow)              F                F F     F   F

8       Offset                Shoulder           Elbow
Drawing                 F F offset                F         8.6 mm                   F   F        F
d 2 + d3         D-H parameter                       FF      F           ( d 2 + d3 = 227.7 + (−219.1) = 8.6 mm. )

9                      F θ1                     X − Y plane

FF
θ1 = φ − α

φ = tan −1 ( Py Px )
 r2 − d 2 
α = tan      −1

    d      
           
 P 2 + P 2 − d2                     
−1   x      y

= tan                                             ; d = offset
       d                            
                                    

 Py         P 2 + P 2 − d2                        
−1   −1                                        
θ1 = tan   − tan
x      y

 Px               d                               
                                       

F                      F θ 2 , θ3          F                      F         Link 2         3   Manipulator
10                         θ 2 , θ3                       Z plane

F             θ 2 , θ3        F                F                       F       F       θ3

Px 2 + Py 2 − d 2 − Pz 2 − a2 2 − a32
cos θ3 =                                                         := D,
2a2 a3

 ± 1 − D2
−1

θ31,2 = tan                         
     D                 
                       
space                   F F         θ3       F          F 1 F                          F               Elbow up        Elbow down

11                            Elbow up             Elbow down                           F                   Link

F        θ2             F       F          θ3 F                           F                  θ3 F                    θ2 F
FF
 P 2 + P 2 − d2            a + a cos θ31 
θ 21 = tan 
−1                    − tan −1  2 3
x      y

      Pz                   a3 sin θ3    
                                    1    
 P2 + P 2 −d2              a + a cos θ32 
−1                  − tan −1  2 3
θ 22 = tan
x      y
         ; d = offset
      Pz                   a3 sin θ3     
                                     2   

F           θ1 , θ 2 , θ3                           F                   MATLAB GUI (
Inverse Kinematic)

Inverse Kinematic

F                               Forward           Inverse Kinematic

1.                 F D-H Parameter             F                 Kinematic Model             F D-H
Parameter     F F   F         F       F        Forward             Transformation Matrix
3                0

2.                            >>>                       F        Transformation Matrix       Forward
Kinematic         F Transformation Matrix                      F            F    F
F Px Py Pz
3.                       F    F Px Py Pz         F arm offset F ( F         F       F F
F       F      Inverse          F F θ1 , θ 2 , θ3      F F   F           D-H
Parameter      F F     F F F                   Inverse                 θ1 , θ 2 , θ3

:       Link 2      3        F         Elbow up   Elbow down
F Solution          F   θ 2 , θ3   F       2 F              F          2
Code (     F )     MATLAB GUI       F F GUI

Forward Kinematic Algorithm

% --- Executes on button press in forward.
function forward_Callback(hObject, eventdata, handles)
% hObject handle to forward (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% input = str2double(get(hObject,'String'));
% if(isempty(input))
% set(hObject,'String','0')
% end
a1 = str2double(get(handles.a1,'String'));
a2 = str2double(get(handles.a2,'String'));
a3 = str2double(get(handles.a3,'String'));
A = [a1 a2 a3];

alp1 = str2double(get(handles.alp1,'String'));
alp2 = str2double(get(handles.alp2,'String'));
alp3 = str2double(get(handles.alp3,'String'));
ALP = [alp1 alp2 alp3];

zth1 = str2double(get(handles.zth1,'String'));
zth2 = str2double(get(handles.zth2,'String'));
zth3 = str2double(get(handles.zth3,'String'));
ZTH = [zth1 zth2 zth3];

d1 = str2double(get(handles.d1,'String'));
d2 = str2double(get(handles.d2,'String'));
d3 = str2double(get(handles.d3,'String'));
D = [d1 d2 d3];

% function mat = genMatrix(a,alpha,zetha,d)
for i=1:1:3
switch i
case 1
T1 = genMatrix(A(i),ALP(i),ZTH(i),D(i));
case 2
T2 = genMatrix(A(i),ALP(i),ZTH(i),D(i));
case 3
T3 = genMatrix(A(i),ALP(i),ZTH(i),D(i));
end
end
T = zeros(4);
T = T1*T2*T3;
Inverse Kinematic Algorithm
% --- Executes on button press in reverse.
function reverse_Callback(hObject, eventdata, handles)
% hObject handle to reverse (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% r23 = str2double(get(handles.R23,'String'));
% r24 = str2double(get(handles.R24,'String'));
% r14 = str2double(get(handles.R14,'String'));
offset = str2double(get(handles.offset,'String'));

Px = str2double(get(handles.R14,'String'));
Py = str2double(get(handles.R24,'String'));
Pz = str2double(get(handles.R34,'String'));

%calculate zeta_1
phi = atan(Py/Px);
alpha = atan(sqrt(Px^2+Py^2-offset^2)/offset);
zth1 = rad2deg((pi/2)+ phi - alpha);
set(handles.ZTH1,'String',zth1);

%calculate zeta_3
a2 = str2double(get(handles.a2,'String'));
a3 = str2double(get(handles.a3,'String'));
D = (Px^2+Py^2+Pz^2-a2^2-a3^2-offset^2)/(2*a2*a3);
zth32 = atan(-sqrt(1-D^2)/D);
zth31 = atan(sqrt(1-D^2)/D);
set(handles.ZTH31,'String',zth31*180/pi);
set(handles.ZTH32,'String',zth32*180/pi);

%calculate zeta_2
zth21 = atan(sqrt(Px^2+Py^2-offset^2)/Pz) -
atan((a2+a3*cos(zth31))/(a3*sin(zth31)));
zth22 = atan(sqrt(Px^2+Py^2-offset^2)/Pz) -
atan((a2+a3*cos(zth32))/(a3*sin(zth32)));
set(handles.ZTH21,'String',zth21*180/pi);
set(handles.ZTH22,'String',zth22*180/pi);
MATLAB GUI

Reference

[1] RK Mittal, I J Nagrath “ROBOTICS AND CONTROL” New Delhi : Tata
McGraw Hill.

[2] Puma 560 Simulator [Online]. available :
http://www1bpt.bridgeport.edu/~risc/html/proj/damir/title.html

[3] Forward kinematics. [Online]. available :
http://virtual.cvut.cz/odl/partners/fuh/course_main/node17.html

[4] The homogeneous transformation matrix [Online]. available :
http://planning.cs.uiuc.edu/node111.html

```
To top