Patent Publication Number: US-2019176325-A1

Title: An Error Modeling Method For End-Effector Space-Curve Trajectory Of Six Degree-of-Freedom Robots

Description:
CROSS REFERENCE TO RELATED APPLICATION 
     This application is a national stage application of International application number PCT/CN2017/103080, filed Sep. 25, 2017, titled “An Error Modeling Method For End-Effector Space-Curve Trajectory Of Six Degree-of-Freedom Robots”, which claims the priority benefit of Chinese Patent Application No. 201710226520.8, filed on Apr. 9, 2017, which is hereby incorporated by reference in its entirety. 
    
    
     TECHNICAL FIELD 
     This invention relates in general to the field of industrial robot end effector-tracking error analysis, and in particular, an end effector-tracking error model projecting the deviation between the planned trajectory and the ideal trajectory, simultaneously taking into account the influence of interpolation algorithm and joint linkage parameter error, providing a theoretical basis for controlling the robot end effector-tracking accuracy. 
     BACKGROUND 
     As one of the important performance indexes of industrial robots, end effector-tracking accuracy has become an important focus for research and development. Modern end effector error control mainly adopts a closed-loop control method. Although using a closed-loop control algorithm can effectively improve the positioning and repetitive positioning accuracy, closed-loop control algorithm heavily depends on the accuracies of joint sensors and end effector sensor. The closed-loop control algorithm also greatly complicates robot structure and makes controlling tracking accuracy of continuous trajectory extremely difficult. For the planning of the end effector continuous trajectory, there are two types of approach; one is to interpolate in the operation space, the other is to interpolate in the joint space. In order to ensure the movement flexibility of each joint, researchers normally interpolate inverse solutions of characteristic trajectory points into joint space, with the characteristic trajectory points reflecting ideal continuous curve trajectory. This results in a great influence by interpolation algorithm parameter selection on end effector-tracking accuracy. Secondly, in the actual industrial robot system, the linkage parameter error caused by manufacturing and assembly also exerts a great influence on end effector-tracking accuracy. Thus, to control the robot end effector-tracking accuracy, it is necessary to take into account of the two factors. In order to compensate for the end effector movement trajectory error, to improve the tracking accuracy, and avoid the complexity and uncertainty engendered by real-time measurement of and real-time compensation, it is necessary to perform offline prediction of the tracking error in the trajectory planning process. Therefore, it is important to establish the robot end effector-tracking error model. In the process of establishing the error model, since the general practice is to take points at equal time intervals at the end effector position during planning, it remains to be solved as to how to take the points on the ideal trajectory and calculate differential, in order to faithfully reflect the deviation between the planned trajectory and the ideal trajectory. This application aims to provide solution to this key issue. 
     SUMMARY 
     The invention discloses an error modeling method for end-effector space-curve trajectory of six degree-of-freedom (DOF) robot. The main feature of this method is that the interpolation algorithm and structural error are both taken into account in the modeling at the same time, and a concise and practical error model is provided for the continuous trajectory tracking of the robot end effector, so as to provide a theoretical basis for the tracking accuracy control. 
     The invention discloses an error modeling method for end effector space-curve trajectory of the six DOF robot, including the following steps: 
     1) selecting N trajectory points on the space-curve, wherein N is positive whole number and is determined by specific operational task, and obtaining displacement or angular displacement of each joint based on an inverse solution model; 
     2) selecting an interpolation algorithm and performing interpolation to obtain functional relationships between joint variables and time, wherein M joint variables are selected by taking a point every 20 milliseconds (ms), and wherein a total motion time obtained by the interpolation algorithm being T(s), M is T/0.02; 
     3) taking into account of the structural errors of each joint of the robot, obtaining positive solution to obtain M corresponding robot end effector trajectory points Q; 
     4) selecting point P on an ideal trajectory so that the points Q is on a normal line that passes the point P, therefore defining an trajectory error E as the distance between the point P and the point Q, so that the problem is transformed into solving the error E based on a known ideal space trajectory curve equation and coordinates of the point Q; wherein when E approaches infinitesimal, planned trajectory coincide with the ideal trajectory; 
     5) based on the curve equation, obtaining a tangent equation that passes the point P, calculating the coordinate of the point P, with the condition of PQ⊥PP 1  (P1 Is any point of the tangent line), so as to obtain the error E. 
       FIG. 1  shows the error diagram of the planned space-curve trajectory. 
     The invention is characterized by simultaneous adjustment to the interpolation algorithm operation and the error of each joint linkage structure. A closer to reality error model is established for the continuous trajectory tracking task of the end effector of the 6 DOF industrial robot, so as to provide a theoretical basis for the realization of trajectory tracking precision control. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  shows the error diagram of the planned space-curve trajectory. 
     
    
    
     DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 
     Embodiment I 
     The step (1) of obtaining displacement or angular displacement of each joint is performed by: 
     Setting robot end effector operational task space-curve equation as following: 
     
       
         
           
             { 
             
               
                 
                   
                     
                       
                         F 
                          
                         
                           ( 
                           
                             x 
                             , 
                             y 
                             , 
                             z 
                           
                           ) 
                         
                       
                       = 
                       0 
                     
                   
                 
                 
                   
                     
                       
                         G 
                          
                         
                           ( 
                           
                             x 
                             , 
                             y 
                             , 
                             z 
                           
                           ) 
                         
                       
                       = 
                       0 
                     
                   
                 
               
                 
             
           
         
       
     
     Selecting the N trajectory points evenly on the curve, and obtaining the angular displacement θ of each joint of the robot by inverse solution; 
     The step (2) of performing interpolation for each joint variables is performed by: using an interpolation algorithm to interpolate the joint variables, and obtaining the functional relationship between the i joint variable and the motion time as follows: 
       θ i   =f   i ( t )
 
     Taking a function value every 20 ms on a function curve obtained according to the above formula, so as to obtain M displacement values θ i  of each joint, and calculate M corresponding trajectory points Q through the forward kinematics model; 
     The step (3) of calculating corresponding robot end effector trajectory points is performed as follows: 
     The robot end effector position being related to the displacement θ i  of each joint, also being related to robot D-H linkage parameters, e.g., linkage length a i , linkage twist angle α i , joint distance d i  and joint angle θ i , therefore, the forward kinematics model of the robot is expressed as follows: 
       Pos= g   st (θ i   ,a   i ,α i   ,d   i ,θ i )
 
     Furthermore, there being the robot linkage parameter errors from the process of manufacturing and assembly, and such errors being able to affect robot end effector positioning accuracy, known linkage parameters thus can be expressed as: 
         a   i   +Δa   i ,α i +Δα i   ,d   i   +Δd   i ,θ i +Δθ i ,
 
     respectively; when the structural errors of each joint of the robot are taken into account, the position of the robot end effector is expressed as: 
       Pos(actual)= g   st (θ i   ,a   i   +Δa   i ,α i +Δα i   ,d   i   +Δd   i ,θ i +Δθ i )
 
     wherein the joint angle θ i  is obtained by interpolation, so that the actual position of the robot end effector is also affected by the interpolation algorithm; wherein by substituting the M joint angles θ i  into the above equation, the M corresponding robot end effector trajectory points are obtained; 
     The step (4) of calculating error E is performed as follows: 
     Setting the point P as a point on an ideal trajectory, and Q is on the normal line passing P, the point P 1  is on tangent line passing the point P, thus PQ⊥PP 1 , setting the spatial coordinates of each point as P (x 0 , y 0 , z 0 ) and P 1  (x 1 , y 1 , z 1 ), in order to faithfully reflect the deviation between the actual end effector trajectory and the ideal trajectory, the trajectory error E is defined as the distance between point P and point Q. As E approaches infinitesimal, the planned trajectory coincides with the ideal trajectory; 
     wherein the equation of the tangent line passing the point P is obtained from a space-curve function as follows: 
     
       
         
           
             { 
             
               
                 
                   
                     
                       
                         
                           
                             ∂ 
                             F 
                           
                           
                             ∂ 
                             x 
                           
                         
                          
                         
                            
                           
                             ( 
                             
                               
                                 x 
                                 0 
                               
                               , 
                               
                                 y 
                                 0 
                               
                               , 
                               
                                 z 
                                 0 
                               
                             
                             ) 
                           
                         
                          
                         
                           
                             
                               ( 
                               
                                 x 
                                 - 
                                 
                                   x 
                                   0 
                                 
                               
                               ) 
                             
                             + 
                             
                               
                                 ∂ 
                                 F 
                               
                               
                                 ∂ 
                                 y 
                               
                             
                           
                            
                           
                              
                             
                               ( 
                               
                                 
                                   x 
                                   0 
                                 
                                 , 
                                 
                                   y 
                                   0 
                                 
                                 , 
                                 
                                   z 
                                   0 
                                 
                               
                               ) 
                             
                           
                            
                           
                             
                               
                                 ( 
                                 
                                   y 
                                   - 
                                   
                                     y 
                                     0 
                                   
                                 
                                 ) 
                               
                               + 
                               
                                 
                                   ∂ 
                                   F 
                                 
                                 
                                   ∂ 
                                   z 
                                 
                               
                             
                              
                             
                                
                               
                                 ( 
                                 
                                   
                                     x 
                                     0 
                                   
                                   , 
                                   
                                     y 
                                     0 
                                   
                                   , 
                                   
                                     z 
                                     0 
                                   
                                 
                                 ) 
                               
                             
                              
                             
                               ( 
                               
                                 z 
                                 - 
                                 
                                   z 
                                   0 
                                 
                               
                               ) 
                             
                           
                         
                       
                       = 
                       0 
                     
                   
                 
                 
                   
                     
                       
                         
                           
                             ∂ 
                             G 
                           
                           
                             ∂ 
                             x 
                           
                         
                          
                         
                            
                           
                             ( 
                             
                               
                                 x 
                                 0 
                               
                               , 
                               
                                 y 
                                 0 
                               
                               , 
                               
                                 z 
                                 0 
                               
                             
                             ) 
                           
                         
                          
                         
                           
                             
                               ( 
                               
                                 x 
                                 - 
                                 
                                   x 
                                   0 
                                 
                               
                               ) 
                             
                             + 
                             
                               
                                 ∂ 
                                 G 
                               
                               
                                 ∂ 
                                 y 
                               
                             
                           
                            
                           
                              
                             
                               ( 
                               
                                 
                                   x 
                                   0 
                                 
                                 , 
                                 
                                   y 
                                   0 
                                 
                                 , 
                                 
                                   z 
                                   0 
                                 
                               
                               ) 
                             
                           
                            
                           
                             
                               
                                 ( 
                                 
                                   y 
                                   - 
                                   
                                     y 
                                     0 
                                   
                                 
                                 ) 
                               
                               + 
                               
                                 
                                   ∂ 
                                   G 
                                 
                                 
                                   ∂ 
                                   z 
                                 
                               
                             
                              
                             
                                
                               
                                 ( 
                                 
                                   
                                     x 
                                     0 
                                   
                                   , 
                                   
                                     y 
                                     0 
                                   
                                   , 
                                   
                                     z 
                                     0 
                                   
                                 
                                 ) 
                               
                             
                              
                             
                               ( 
                               
                                 z 
                                 - 
                                 
                                   z 
                                   0 
                                 
                               
                               ) 
                             
                           
                         
                       
                       = 
                       0 
                     
                   
                 
               
                 
             
           
         
       
     
     Take x−x 0 =Δx, y−y 0  and z−z 0  is obtained from the above formula, satisfying the following conditions: 
     
       
         
           
             { 
             
               
                 
                   
                     
                       
                         
                           F 
                            
                           
                             ( 
                             
                               x 
                               , 
                               y 
                               , 
                               z 
                             
                             ) 
                           
                         
                         = 
                         0 
                       
                        
                       
                           
                       
                     
                   
                 
                 
                   
                     
                       
                         
                           G 
                            
                           
                             ( 
                             
                               x 
                               , 
                               y 
                               , 
                               z 
                             
                             ) 
                           
                         
                         = 
                         0 
                       
                        
                       
                           
                       
                     
                   
                 
                 
                   
                     
                       
                         
                           
                             ( 
                             
                               x 
                               - 
                               
                                 x 
                                 0 
                               
                             
                             ) 
                           
                            
                           
                             ( 
                             
                               X 
                               - 
                               
                                 x 
                                 0 
                               
                             
                             ) 
                           
                         
                         + 
                         
                           
                             ( 
                             
                               y 
                               - 
                               
                                 y 
                                 0 
                               
                             
                             ) 
                           
                            
                           
                             ( 
                             
                               Y 
                               - 
                               
                                 y 
                                 0 
                               
                             
                             ) 
                           
                         
                         + 
                         
                           
                             ( 
                             
                               z 
                               - 
                               
                                 z 
                                 0 
                               
                             
                             ) 
                           
                            
                           
                             ( 
                             
                               Z 
                               - 
                               
                                 z 
                                 0 
                               
                             
                             ) 
                           
                         
                       
                       = 
                       0 
                     
                   
                 
               
                 
             
           
         
       
     
     Finally, solving the position of point P (x 0 ,y 0 ,z 0 ) from the above equations, obtaining the error E as follows: 
         E=|{right arrow over (PQ)} |=√{square root over (( X−x   0 ) 2 +( Y−y   0 ) 2 +( Z−z   0 ) 2 )}.