Patent Publication Number: US-8532825-B1

Title: Software compensation for kinematically singular machines

Description:
BACKGROUND 
     The aerospace industry uses numerically controlled (NC), multi-axis machine tools to fabricate high precision parts. A typical NC multi-axis machine tool includes a kinematic linkage for positioning and orienting a tool relative to a work piece. A 5-axis machine tool, for instance, can move a tool tip along x, y and z axes, and rotate the tool tip about the x and y axes. 
     These machine tools are now being specified and built to achieve a particular volumetric accuracy necessary to properly execute an intended process or job. For example, a machined part may have a surface location tolerance of ±0.010″. A multi-axis machine tool would be required to position a tool tip to better than ±0.010″ within a working volume. However, most machine tools do not have such mechanical accuracy built in. Therefore, software compensation is used to attain the required accuracy. 
     As the tool tip moves towards a singular region, the linkage might be commanded to produce large joint motions. This is problematic. The large joint motions might be physically unrealizable since machine joints have velocity and acceleration constraints. In addition, since the compensation is effectively unplanned motion, collisions with obstacles are risked. 
     Measures can be taken to avoid large joint motions. For example, the well known Linear-Quadratic Regulator (LQR) method of optimal control uses weighting matrices to minimize joint penalty far from the singular regions, and maximize joint and error penalty when close to a singular region, but only in the singular direction. 
     However, the weighting matrices are static. That is, they are designed ahead of time. Consequently, the LQR method can produce compensation that is heavily damped even though little damping is needed, or it can produce excessive joint motion when close to a singular region. 
     SUMMARY 
     According to an embodiment herein, a method of commanding a linkage to move an end body in a work space comprises computing a manipulator Jacobian given a nominal linkage command; computing a normalized Jacobian from the manipulator Jacobian, a joint space normalizing matrix Q, and an error space normalizing matrix E; and using the normalized Jacobian and the Q and E matrices to produce a software compensation command. 
     According to another embodiment herein, a method of commanding a linkage to move an end body in a space comprises computing a manipulator Jacobian J n  given a nominal linkage command q; computing a singular value decomposition of the Jacobian to produce a diagonal matrix of singular values; and calculating an approximation S† of the inverse of the S matrix, where 
                 S   †     =     diag   ⁡     (       σ   i   2         σ   i   3     +       σ   1   3     /   w         )         ,         
where σ i  is the i th  singular value and σ 1  is the largest singular value in the S matrix. The method further comprises using the approximation S† to produce a software compensation command.
 
     According to another embodiment herein, a machine comprises a kinematic linage for moving an end body to a pose; and a controller for generating a software compensation command Δq for the kinematic linkage given a nominal linkage command q. Generating the software compensation command Δq includes computing a manipulator Jacobian J n  given the nominal linkage command q; computing a normalized Jacobian   from the manipulator Jacobian J n , a joint space normalizing matrix Q, and an error space normalizing matrix E; identifying singular values, left singular vectors, and right singular vectors of the normalized Jacobian; and using the singular values, singular vectors, and the Q and E matrices to determine a software compensation command Δq for the linkage. 
     According to another embodiment herein, an article comprises a processor and memory encoded with data for causing the processor to generate a software compensation command given a nominal linkage command. Generating the software compensation command includes computing a manipulator Jacobian given a nominal linkage command; computing a normalized Jacobian from the manipulator Jacobian, a joint space normalizing matrix Q, and an error space normalizing matrix E; and using the normalized Jacobian and the Q and E matrices to produce a software compensation command. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  is an illustration of a kinematically singular machine. 
         FIG. 2  is an illustration of error as a function of a nominal linkage command. 
         FIG. 3  is an illustration of a method of generating a software compensation command. 
         FIG. 4  is an illustration of a method of computing an approximation of the inverse of the diagonal matrix of singular values. 
         FIG. 5  is an illustration of a kinematic linkage for a 5-axis machine tool. 
         FIG. 6  is an illustration of scaling of error motion as a function of the ratio of singular values. 
     
    
    
     DETAILED DESCRIPTION 
     Reference is made to  FIG. 1 , which illustrates a kinematically singular machine  110  including a kinematic linkage  120  that moves an end body in a work space. The end body may be a body at the end of the linkage  120  (e.g., a tool), or it may refer to the end of the linkage  120 . The kinematic linkage  120  has multiple degrees of freedom for changing the location and orientation (that is, pose) of the end body. 
     The machine  110  further includes a controller  130  for generating linkage commands q+Δq, which are sent to servo drives  140 . In response to the linkage commands q+Δq, the servo drives  140  cause the linkage  120  to move the end body to a commanded pose. The vector q is a nominal linkage command, which describes the desired pose of the linkage  120  at a given time. The vector Δq is a software compensation command for achieving the desired end body pose within a required volumetric accuracy. 
     The controller  130  is not limited to any particular construction. For example, the controller  130  may include a processor  132  and memory  134  encoded with data  136  for causing the processor  132  to generate the software compensation commands q+Δq. 
     For a machine  110  with n axes positioning a rigid end body in space, the forward kinematic equations can be modeled as a function mapping joint commands to tool pose:
 
 f ( q,e ( q )): R   q   n     T   x 
 
where e(q) denotes a vector of errors associated with the nominal linkage command q, R q   n  represents the linkage space, and x and T x  represents the pose of the end body.  FIG. 2  provides an example of error e as a function of the nominal linkage command q.
 
     The controller  130  determines a correction to the nominal linkage command that results in the machine  110  achieving a desired end body pose in the workspace of the end body. This correction is the software compensation command Δq. The workspace of the end body will be denoted by T t . The end body space T t  is always at the end body and the Z axis is aligned to the axis of the end body. The X and Y axes are rather arbitrary. For example, if the end body is a tool tip that is oriented with respect to a workpiece, the X and Y axes can be chosen to align with the X and Y axes of the workpiece coordinate system when the linkage  120  is commanded to q=0. 
     From this, the pose vector x of the end body fully describes the transform T x   T t . Thus, given a nominal pose of a linkage (x nom ) and a desired pose of the linkage (x act ), and representing x nom  and x act  as homogenous transform matrices X nom  and X act , the error transformation E is E=X nom   −1 X act . The error transformation E is expressed with respect to the end body workspace T t . Thus, the minimal set of error vectors e does not change with orientation of the end body. The error transformation E can be transformed back to a pose error vector e. 
     The controller  130  finds a compensation command Δq that maps to −e. The command q+Δq allows the machine  110  to achieve a particular volumetric accuracy. 
     Additional reference is made to  FIG. 2 . The compensation command Δq is derived from a linearized region L of the error e ( FIG. 2  shows the linearized region L in dash). The linearized region L may be represented by a manipulator Jacobian, which provides a linear function that maps joint motion to end body motion as Δx=J n (q)Δq and, therefore, maps the compensation command to −e (that is, J n (q)Δq=−e). 
     The controller  130  computes the manipulator Jacobian. However, one problem with the manipulator Jacobian is that it treats the same error values for rotation and translation as equivalent, and it also treats the same error values for different scales as equivalent. For example, the manipulator Jacobian treats errors of 0.001 mm, 0.001 inches and 0.001 degrees as equivalent errors. Consider a value of 0.010 mm, along with a value of 0.005 degrees. A minimization of the sum of the squares will push the compensation command Δq towards one which minimizes the linear value at the expense of the angular value (0.010 2 =0.0001; and 0.005 2 =0.000025). Thus, the linear value penalizes the sum of the squares by four times as much as the angular error. Consequently, an extremely large joint motion might result in the direction of a singularity. Moreover, the condition number of the Jacobian may be larger for a given pose of the end body, and the resulting compensation command Δq may be heavily damped when little damping is needed. 
     To overcome this problem, the controller  130  computes a normalized Jacobian   from the manipulator Jacobian J n . Normalizing the manipulator Jacobian J n  allows an accurate comparison of different types of joint motion (e.g., revolute versus prismatic). It also allows comparison of different units (e.g., millimeters vs. inches vs. degrees) and controls the relative accuracy of compensation against joint motion. 
     The controller  130  computes the compensation command Δq from the normalized Jacobian  . The compensation command Δq is based on the general problem
 
min Φ=( Δ q+e ) T   Q ( Δ Q+e )+(Δ q ) T   G (Δ q )
 
where Q and G represent cost penalties for error and joint motion. An error minimization algorithm (e.g., LQR) is used to minimize Φ.
 
     The controller  130  computes software compensation commands such that G and Q are adaptive, wherein damping is minimized when the end body is far from a singular region, and joint motion is minimized when the end body approaches a singular region. Contrast this to conventional LQR, where G and Q are static. 
     Reference is made to  FIG. 3 , which illustrates a method of computing a software compensation command Δq. At block  310 , a manipulator Jacobian J n  is computed given a nominal linkage command q. The subscript n indicates that J n  is the Jacobian of the body-space, not the work-space. 
     At block  320 , a normalized Jacobian   is computed from the manipulator Jacobian J n , a joint space normalizing matrix Q, and an error space normalizing matrix E. For example, the normalized Jacobian   is computed as  =EJnQ −1 . The Q matrix represents the smallest resolvable step for each axis. The E matrix represents error values that are normalized for the different error directions so that errors which create one linear unit of error in the end body have a normalized value of one. 
     At block  330 , singular values, left singular vectors, and right singular vectors of the normalized Jacobian   are identified. This step may include computing a singular value decomposition (SVD) of the normalized Jacobian   to produce USV T , where U and V are orthogonal matrices of the left and right singular vectors and where S is a diagonal matrix of singular values. This step orthogonalizes the software compensation. The end body motion is mapped into a space whose axes are scaled by the singular values of the S matrix. Thus, at a singularity σ i =0, the i th  axis in the orthogonal space will be annihilated. The right singular vectors are the orthogonal basis vectors for joint motion. The left singular vectors are the orthogonal basis set for end-body motion. 
     At block  340 , the singular values, singular vectors, and the Q and E matrices are used to determine the software compensation command Δq. The compensation command Δq may be computed as
 
Δ q=−Q   −1   VS   †   U   T   Ee, 
 
where e is the tool tip pose error and S †  is the approximation of the inverse of the S matrix. The approximation S †  may have inverse singular values that penalize singular directions in joint motion more heavily than singular directions in error. The inverse singular values may be adaptively weighted such that the joint motion along the right singular vectors decays along increasingly singular directions. The decay may be the inverse of a cubic function.
 
     Reference is now made to  FIG. 4 , which illustrates a method of computing the approximation S † . At block  410 , a manipulator Jacobian J n  is computed, given a nominal linkage command q. At block  420 , a singular value decomposition (SVD) of the Jacobian is computed to produce a diagonal matrix of singular values. 
     At block  430 , an approximation S †  to the inverse of the S matrix is computed. The approximation S† is computed as 
                 S   †     =     diag   ⁡     (       σ   i   2         σ   i   3     +     1   /   λ         )         ,         
where σ i  is the i th  singular value and σ 1  is the largest singular value in the S matrix. The deviation of S† from S is a function of the ratio of the i th  singular value and the largest singular value.
 
     In some embodiments, 
                 1   λ     =       σ   1   3     /   w       ,       whereby   ⁢           ⁢     S   †       =       diag   ⁡     (       σ   i   2         σ   i   3     +       σ   1   3     /   w         )       .             
The value w is a design constant. The advantage is that only a single design variable is required. This design variable can be chosen as an order of magnitude estimate. Different values of w can change the asymptotic decay in motion along increasingly singular directions. See, for example,  FIG. 6 .
 
     Scaling other than 1/λ=σ 1   3 /w may be used. In one embodiment, setting S † =1/σ i  a for all σ i  greater than some threshold will provide a numerically stable form of pseudo inverse (ordinary least squares). In another embodiment, setting 
               S   †     =     diag   ⁡     (       σ   i         σ   i   2     +   λ       )             
provides a form for damped least squares.
 
     In some embodiments of  FIG. 4 , the SVD of the non-normalized Jacobian may be taken. 
     There is no need to identify all the kinematic singularities of the machine  110  ahead of time because the singularities are those poses where one of the singular values becomes zero. The real-time implementation doesn&#39;t need a switching clause (if near singularity . . . else . . . ), as singular and non-singular poses are handled in exactly the same way. Thus, there is no need to identify the singular poses ahead of time for some if-else clause in the control. This is advantageous if the machine  110  has complicated singularities. 
     The machine  110  is not limited to any particular kinematic linkage. Some embodiments might include a single chain machine, while other embodiments might include a branched manipulator having multiple endpoints. No a priori knowledge of the machine&#39;s kinematic structure is required beyond the nominal axis motion stackup, as all components can be calculated at run time in a real-time environment. 
     The machine  110  is especially useful for multi-axis machine tools that are controlled in real time. For example, the machine could be adapted for a 5-axis machine tool. Volumetric accuracy is the positioning accuracy of the tool tip in a coordinate system that is fixed with respect to the workpiece. 
     Reference is now made to  FIG. 5 , which illustrates a linkage  510  for a 5-axis milling machine having an XYZCA geometry. The linkage  510  can move and orient a tool tip  520  relative to five degrees of freedom: translation in the X, Y and Z directions along three linear axes (x, y and z axes) and rotation in the A and C directions (about the y and z axes). The tool tip  520  is shown as being positioned at a singular point  530  since rotation of the tool tip  520  in the C rotational direction cannot change a position or orientation of the tool tip  520 . Accordingly, the five-axis assembly  510  loses a degree of freedom when the tool tip  520  is positioned at the singular point  530 . 
     The nominal linkage command q for the kinematic linkage  510  of  FIG. 5  is denoted by q=[q x , q y , q z , q a , q c ], where subscripts x, y and z denote translational motion and subscripts a and c denote rotational motion. The corresponding pose error for an asymmetric machine is denoted by e=[e x , e y , e z , e a , e c ] T . 
     The vectors Δq and e are scaled so that unit changes in each dimension have physical meaning. Let Δ{circumflex over (q)}={circumflex over (Q)}Δq and Δê=ÊΔe 
     In a normalized Δq, a one-unit change in compensation value is equal to the smallest resolvable step for that axis. In some embodiments, joint resolution and the smallest resolvable step are considered to be the same. In other embodiments, measurements of the smallest resolvable step are made. 
     Consider a machine that will be face milling with a 150 mm cutter. The X and Y directions have a resolution of 0.001/μm. The Z-direction has a resolution of 0.005 mm. The C and A directions have joint resolutions of 0.002 degrees. The normalizing matrix for Δq is: 
               Δ   ⁢           ⁢     q   ^       =       [         1000       0       0       0       0           0       1000       0       0       0           0       0       200       0       0           0       0       0       500       0           0       0       0       0       500         ]     ⁢   Δ   ⁢           ⁢   q           
Now joint motions equivalent to the motion resolution of the axis have a magnitude of 1.
 
     In a normalized error vector e, the error vector e is blended with distance. Consider milling with a face mill having a radius of 75 mm. An angular error of 1/75 radians will cause a mislocation of the edge of the tool equivalent to a translation error of 1. Thus, the relative scale between translation and rotation depends on the size of the object being controlled. 
     A single unit of error should in some way be comparable to the positioning uncertainty in the kinematic model. If the error is reduced to four times smaller than the model uncertainty, for instance, then further reductions are unlikely to make meaningful impact on the positioning accuracy of the machine. 
     To choose a normalizing matrix for the error vector e, suppose the machine with the 150 mm cutter has an estimated model uncertainty of about 0.004 mm. The normalizing matrix will be: 
               e   ^     =       [         1000       0       0       0       0           0       1000       0       0       0           0       0       1000       0       0           0       0       0       150000       0           0       0       0       0       150000         ]     ⁢   e           
so that errors which create 1/μm of error have a normalized value of 1.
 
     The Jacobian in normalized coordinates is defined by
 
 J   n ( {circumflex over (Q)}   −1   Δ{circumflex over (q)} )=− Ê   −1   ê 
 
 =( ÊJ   n   {circumflex over (Q)}   −1 )
 
   {circumflex over (q)}=−ê 
 
     Using the SVD of the normalized Jacobian
 
 = USV   T 
 
 S ( V   T   Δ{circumflex over (q)} )=−( U   T   ê )
 
     The minimization problem can now be expressed in normalized, orthogonalized coordinates:
 
Φ=( S ( V   T   Δ{circumflex over (q)} )+ U   T   ê ) T   Q ( S ( V   T   Δ{circumflex over (q)} )+ U   T   ê )+( V   T   Δ{circumflex over (q)}   T   G ( V   T   Δ{circumflex over (q)} )
 
The orthogonalized problem space implies that the elements of V T Δ{circumflex over (q)} and U T ê are independent of each other and meaningfully equal in value. That is, changes in the i th  element of V T Δ{circumflex over (q)} only cause changes in the i th  element of U T ê. Thus, Q and G are set to be diagonal.
 
     Let G=S −1 . The more singular the error direction, the larger the penalty on G, up to the point that when σ i =0 the penalty is effectively infinite. 
     Let Q=λl, where l is a scaled identity matrix. Motion in the orthogonal space is treated with equivalent penalty. 
     The following known minimal solution to Φ may be used:
 
Δ {circumflex over (q)}=−V ( SQS+G ) −1   SQU   T   ê 
 
     Substituting in the chosen values for Q and G, and recognizing that S, Q, and G are all diagonal gives: 
               Δ   ⁢           ⁢     q   ^       =       -     V   ⁡     (     diag   ⁡     (       σ   i   2         σ   i   3     +     1   /   λ         )       )         ⁢     U   T     ⁢     e   ^             
where diag (x i ) indicates a diagonal matrix with terms x i .
 
     Denormalizing the equation above gives 
     
       
         
           
             
               Δ 
               ⁢ 
               
                   
               
               ⁢ 
               q 
             
             = 
             
               
                 - 
                 
                   
                     Q 
                     ^ 
                   
                   
                     - 
                     1 
                   
                 
               
               ⁢ 
               
                 V 
                 ⁡ 
                 
                   ( 
                   
                     diag 
                     ⁡ 
                     
                       ( 
                       
                         
                           σ 
                           i 
                           2 
                         
                         
                           
                             σ 
                             i 
                             3 
                           
                           + 
                           
                             1 
                             / 
                             λ 
                           
                         
                       
                       ) 
                     
                   
                   ) 
                 
               
               ⁢ 
               
                 U 
                 T 
               
               ⁢ 
               
                 E 
                 ^ 
               
               ⁢ 
               e 
             
           
         
       
     
     Let λ=w/σ 1   3 . This leaves a single design variable, w. 
     Additional reference is made to  FIG. 6 , which shows an asymptotic decay in motion along increasingly singular directions. The decay changes as the inverse of a cubic function. The decay is shown for different values of w: 10, 100, 1000, and 10,000. For example, w=1000 can produce scaling of 50% when σ i /σ 1  is about 1/10. In other words, a singular value is preserved to at least 90% of its full compensation as long as σ i /σ 1 &gt;1/5. The choice of w=1000 has been found to provide a good balance between damping and accuracy far from a singularity.