Docstoc

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

Document Sample
การคำนวณหา Forward Kinematic และ Inverse Kinematic ของ Manipulator ด้วย MATLAB Powered By Docstoc
					                       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)
    3    Dimension       Arm Link




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

                                 Inverse Kinematic                              Manipulation                       offset                                          Link
   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
Link       2    project                       Z plane
                                                 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                 
                                                                                              
  F                 Link 2                   3                 F    F 1 solution                                Link   F
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

				
DOCUMENT INFO
Shared By:
Categories:
Stats:
views:551
posted:11/25/2011
language:English
pages:14
Description: รายงาน โปรแกรมคำนวณ Forward Kinematic และ Inverse Kinematic เป็นโปรแกรมที่สร้างขึ้นด้วยโปรแกรม MATLAB เวอร์ชั่น 2009