Patent Publication Number: US-2022226992-A1

Title: Manipulator for finishing work, and control method therefor

Description:
TECHNICAL FIELD 
     The present disclosure relates to a manipulator for a finishing work, and a control method therefor. 
     BACKGROUND ART 
     Until now, finishing works (or surface finishing works) have been considered as manned works requiring long production times and high costs. As the demand for large molds and 3D printed products increases, automated finishing processes are becoming more and more important. However, robot finishing works have difficulty in employing the existing industrial robots. Industrial robots have a decisive disadvantage that their rigidity is remarkably low, and thus, particularly under machining force, significant path deviation is caused. In addition, since some industrial robots employ closed servo systems that limit a user&#39;s accessibility, special end-effectors are required to prevent disturbances in passive or active mechanisms. 
     DISCLOSURE OF THE INVENTION 
     Technical Problem 
     The present disclosure provides a manipulator for providing a stable torque to perform a stable finishing work, and a control method therefor. 
     Technical Solution 
     An embodiment of the present disclosure provides a manipulator for a finishing work, including: a base; an arm including a plurality of links, a plurality of joints connecting the plurality of links, and a plurality of actuators generating rotation of at least some of the plurality of joints; and a processor determining a driving torque of each of the plurality of actuators considering a self-weight effect of the manipulator and controlling the plurality of actuators based on the determined driving torque. 
     In addition, an embodiment of the present invention provides the manipulator wherein the arm further includes a parallelogram link set having one side fixed to the base and having a parallelogram structure. 
     In addition, an embodiment of the present disclosure provides the manipulator wherein the arm further includes a first link, a second link, a first actuator, a second actuator, a first joint, and a second joint, one side of the parallelogram link set is a double link, the double link comprises the first link and the second link parallel to each other, the first link is connected to the base through the first joint that is rotated by the first actuator, and the second link is connected to the base through the second joint that is rotated by the second actuator. 
     In addition, an embodiment of the present disclosure provides the manipulator wherein the first joint and the second joint have rotation axes parallel to a working plane, respectively, are disposed on a same straight line, and are dynamically decoupled. 
     An embodiment of the present disclosure provides the manipulator wherein the second joint provides a leverage effect on the arm by the parallelogram structure, and the processor performs feed forward torque control through the second actuator. 
     In addition, an embodiment of the present disclosure provides the manipulator wherein the feed forward torque control does not involve position feedback for the arm and force feedback for the arm. 
     In addition, an embodiment of the present disclosure provides the manipulator wherein the arm further comprises a third joint, a third actuator, and a swing arm link, the swing arm link is connected through the third joint at an end of a side not connected to the base of the parallelogram link set, and the third joint is rotated along a rotation axis in a direction perpendicular to the working plane by the third actuator. 
     In addition, an embodiment of the present disclosure provides the manipulator wherein the arm further comprises a fourth joint, a fifth joint, a fourth actuator, a fifth actuator, a hand link, and an end-effector, the hand link is connected through the fourth joint at an end of the swing arm link, the fourth joint is rotated along a rotation axis in a direction of the hand link by the fourth actuator, the end-effector is connected through the fifth joint at an end of the hand link, and the fifth joint is rotated along a rotation axis parallel to the working plane and perpendicular to the direction of the hand link by the fifth actuator. 
     In addition, an embodiment of the present disclosure provides the manipulator wherein the feed forward torque of the second actuator is 
     
       
         
           
             
               
                 τ 
                 2 
               
               = 
               
                 
                   
                     g 
                     2 
                   
                   ⁡ 
                   
                     ( 
                     q 
                     ) 
                   
                 
                 + 
                 
                   
                     λ 
                     z 
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         
                           h 
                           z 
                         
                         ⁡ 
                         
                           ( 
                           q 
                           ) 
                         
                       
                     
                     
                       ∂ 
                       
                         q 
                         2 
                       
                     
                   
                 
                 + 
                 
                   
                     λ 
                     x 
                   
                   ⁢ 
                   
                     
                       ∂ 
                       
                         
                           h 
                           x 
                         
                         ⁡ 
                         
                           ( 
                           q 
                           ) 
                         
                       
                     
                     
                       ∂ 
                       
                         q 
                         2 
                       
                     
                   
                 
               
             
             , 
           
         
       
     
     g 2  is a gravity of the manipulator at the second joint, q is a displacement vector of the first to fifth joints, q 2  is a displacement of the second joint, h is a constraint condition function of the manipulator, and λ is an external force vector in the end-effector. 
     In addition, an embodiment of the present disclosure provides the manipulator wherein the base is connected to a gantry structure. 
     Advantageous Effects 
     According to various embodiments of the present disclosure, compared with a compliance control method based on force feedback, uniform and smooth torque control can be achieved by performing feed forward torque control using a self-weight effect of a manipulator including a link having a parallelogram structure. 
     In addition, according to various embodiments of the present disclosure, a wider working range may be secured because the manipulator adopts a swing arm structure. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  is a view showing a finishing machine according to an embodiment of the present disclosure. 
         FIG. 2  is a block diagram showing a manipulator according to an embodiment of the present disclosure. 
         FIGS. 3 to 5  are views illustrating a 5-axis manipulator according to an embodiment of the present disclosure. 
         FIG. 6  is a view showing a method for controlling a manipulator based on an inverse kinematic model according to an embodiment of the present disclosure. 
         FIG. 7  is a view showing a simulation process for a manipulator according to an embodiment of the present disclosure. 
         FIG. 8  is a view for explaining a constrained motion of a manipulator according to an embodiment of the present disclosure. 
         FIG. 9  is a view for explaining feed forward torque control of a manipulator according to an embodiment of the present disclosure. 
         FIG. 10  is a control block diagram of a manipulator according to an embodiment of the present disclosure. 
         FIG. 11  is a diagram showing a force control result according to compliance control when an abrasive wheel is absent. 
         FIG. 12  is a view showing a force control result according to feed forward torque control in the case where an abrasive wheel is absent. 
         FIG. 13  is a view showing a comparison between conventional force feedback control and feed forward control proposed in the present disclosure. 
     
    
    
     MODE FOR CARRYING OUT THE INVENTION 
     Hereinafter, embodiments of the present disclosure are described in detail with reference to accompanying drawings and regardless of the reference symbols, same or similar components are assigned with the same reference numerals and thus overlapping descriptions for those are omitted. The suffixes ‘module’ and ‘unit’ for components used in the description below are assigned or mixed in consideration of easiness in writing the specification and do not have distinctive meanings or roles by themselves. In the following description, detailed descriptions of well-known functions or constructions will be omitted since they would obscure the invention in unnecessary detail. Additionally, the accompanying drawings are used to help easily understanding embodiments disclosed herein but the technical idea of the present disclosure is not limited thereto. It will be understood that the present disclosure includes all modifications, equivalents, and substitutes falling within the spirit and scope of various embodiments of the disclosure. 
     It will be understood that although the terms “first,” “second” etc. may be used herein to describe various elements, these elements should not be limited by these terms. These terms are only used to distinguish one element from another. 
     It will be understood that when an element is “connected” or “coupled” to another element, the element may be directly connected or coupled to the other element or may be “connected” or coupled” to the other element with an intervening element therebetween. On the other hand, it will be understood when an element is “directly connected” or “directly coupled” to another element, no intervening element is present therebetween. 
       FIG. 1  is a view showing a finishing machine  1  according to an embodiment of the present disclosure. 
     Referring to  FIG. 1 , a finishing machine  1  according to an embodiment of the present disclosure may include a manipulator  100 , a gantry structure  200 , and a first slide table  300 . 
     The gantry structure  200  shown in  FIG. 1  does not include a moving part moving in an x-axis direction, and instead, may include a second sliding table  400  that moves in the x-axis direction and holds a workpiece. However, the present disclosure is not limited. For example, in another embodiment, the gantry structure  200  of the finishing machine  1  may include a moving part (e.g., a wheel, etc.) moving in the x-axis direction. 
     The manipulator  100  may be connected to the gantry structure  200  through the first slide table  300 , and the height or z-axis position thereof may be adjusted according to the movement of the first slide table  300 . 
     The first slide table  300  may move in a y-axis direction in the gantry structure  200 . Compared with heavy industry robots, translational axes using the gantry structure  200  may have high stiffness and may provide a wider working space. 
     The manipulator  100  may perform a surface finishing work on the workpiece by using a rotary tool such as an electro-spindle or a pneumatic-spindle mounted on an end-effector. Since the manipulator  100  has different compliance characteristics depending on the position and orientation of the end-effector, the finishing work may be performed only in a limited area that produces an acceptable conformation in a given posture. 
     The manipulator  100  may be positioned in a desired working area in the x, y, and z axes so as to satisfy both the allowable compliance range and the working position of the workpiece. 
       FIG. 2  is a block diagram showing the manipulator  100  according to an embodiment of the present disclosure. 
     Referring to  FIG. 2 , the manipulator  100  according to an embodiment of the present disclosure may include a communication unit  110 , a memory  120 , an actuator  130 , a processor  190 , and the like. 
     The communication unit  110  may transmit/receive data to/from an external device (not shown) that controls the manipulator  100  or the finishing machine  1  by using wired/wireless communication technology. The communication unit  110  may receive a control signal or a control profile from an external device (not shown). 
     The communication technology used by the communication unit  110  includes Global System for Mobile communication (GSM), Code Division Multi Access (CDMA), Long Term Evolution (LTE), 5G, Wireless LAN (WLAN), Wireless-Fidelity (Wi-Fi), Bluetooth™, Radio Frequency Identification (RFID), Infrared Data Association (IrDA), ZigBee, Near Field Communication (NFC), and the like. 
     The memory  120  may store data supporting various functions of the manipulator  100 . In addition, the memory  120  may store various application programs driven by the manipulator  100 , data and instructions for the operation of the manipulator  100 , and the like. 
     The memory  120  may store firmware used to drive the manipulator  100 , an application program used to control the actuator  130 , a control profile, and the like. 
     The actuator  130  may generate the movement of the manipulator  100  and may include at least one actuator. The actuator  130  may be referred to as a motor. 
     The actuator  130  may generate a movement rotating about a predetermined axis, or may generate a translational movement along a predetermined path. 
     The actuator  130  may be operated considering a self-weight effect on the manipulator  100 . That is, the actuator  130  may be operated with the strength of torque determined considering the posture of the manipulator  100  and the weight thereof. 
     In one embodiment, the actuator  130  includes five actuators, and accordingly, the manipulator  100  may be a 5-axis manipulator. The 5-axis manipulator  100  according to an embodiment may have a structure shown in  FIGS. 3 and 4 . 
     The sensor unit  140  may obtain state information of the manipulator  100  and state information of the actuator  130  (e.g., motion information, force information, etc.) by using various sensors. For example, the sensor unit  140  may obtain an angle and a rotation speed by the operation of the actuator  130 , a rotation speed of a rotary tool mounted on the end-effector, and the like. 
     The sensor unit  140  may obtain the shape of the workpiece, the location of the workpiece, the state of the workpiece, surface information of the workpiece, and the like by using various sensors. 
     Sensors included in the sensor unit  140  include a proximity sensor, an illumination sensor, an acceleration sensor, a magnetic sensor, a gyro sensor, an inertial sensor, an RGB sensor, an IR sensor, a fingerprint recognition sensor, an ultrasonic sensor, an optical sensor, a microphone, a lidar, a radar, and the like. 
     The processor  190  may control the actuator  130  and the like by driving the application program stored in the memory  120 . 
     The processor  190  may control the actuator  130  based on the control signal. The processor  190  may receive the control signal from an external device through the communication unit  110 , or may receive the control signal through an input unit (not shown). 
     The processor  190  may generate the control signal for the actuator  130  considering the state information of the actuator  130 . 
     The processor  190  may adjust the set control signal considering the sensor information obtained through the sensor unit  140 . 
     The processor  190  may be referred to as a controller or a motion controller. 
       FIGS. 3 to 5  are views illustrating the 5-axis manipulator  100  according to an embodiment of the present disclosure. 
     Referring to  FIGS. 3 to 5 , the 5-axis manipulator  100  according to an embodiment of the present disclosure may include five motors or actuators. Therefore, various movements along five axes  311 ,  312 ,  313 ,  314 , and  315  may be possible. Hereinafter, the term “motor” and the term “actuator” may be used interchangeably. 
     A motor or an actuator that generates rotation with respect to a first joint  511  may be referred to as a first motor or a first actuator. Similarly, a motor or an actuator that generates rotation with respect to a second joint  512  may be referred to as a second motor or a second actuator, a motor or an actuator that generates rotation with respect to a third joint  513  may be referred to as a third motor or a third actuator, a motor or an actuator that generates rotation with respect to a fourth joint  514  may be referred to as a fourth motor or a fourth actuator, and a motor or an actuator that generates rotation with respect to a fifth joint  515  may be referred to as a fifth motor or a fifth actuator. In addition, a rotation axis of the first joint  511  may be referred to as the first axis  311 , a rotation axis of the second joint  512  may be referred to as the second axis  312 , a rotation axis of the third joint  513  may be referred to as the third axis  313 , a rotation axis of the fourth joint  514  may be referred to as the fourth axis  314 , and a rotation axis of the fifth joint  515  may be referred to as the fifth axis  315 . Hereinafter, the terms “axes  311  to  315 ” and the terms “joints  511  to  515 ” may be used interchangeably. 
     In the 5-axis manipulator  100 , only the first joint  511 , the second joint  512 , the third joint  513 , the fourth joint  514 , and the fifth joint  515  may be active joints in which actuators are provided and rotations are actively controlled, and the other joints  521 ,  522 , and  523  may be passive joints in which rotations are not actively controlled. The first passive joint  521 , the second passive joint  522 , and the third passive joint  523  play an important role in supporting a load and maintaining an accurate posture during the finishing work. 
     In one embodiment, all or part of the active joints  511 ,  512 ,  513 ,  514 , and  515  and the passive joints  521 ,  522 , and  523  may have an angular contact ball bearing structure. The angular contact ball bearing may have various angular contact ball bearing structures such as single row angular contact ball bearings, double row angular contact ball bearings, and the like, and may have a structure in which two or more angular contact ball bearings are assembled as a pair. The angular contact ball bearing may have high rigidity in the axial and radial directions, and may effectively reduce vibrations generated during machining. In particular, the first joint  511 , the second joint  512 , and the third passive joint  523  may have an angular contact ball bearing structure. 
     The 5-axis manipulator  100  may include links having a parallelogram structure (hereinafter referred to as a parallelogram link set  412 ), and two vertices of one side (z-axis direction) of the parallelogram structure (or one side of the parallelogram structure) may be fixed to predetermined positions of a base  411  of the 5-axis manipulator  100 . The first joint  511  and the second joint  512  may be disposed at a position corresponding to one vertex in the arm direction or the x-axis direction among the two vertices of the parallelogram link set  412  fixed to the base  411 . The arm direction may be the x-axis direction. The first actuator corresponding to the first joint  511  and the second actuator corresponding to the second joint  512  may be decoupled and mechanically separated from each other. The first axis  311  and the second axis  312  may be parallel to the y-axis direction. In addition, the first axis  311  and the second axis  312  may be positioned on the same straight line. That is, the first axis  311  and the second axis  312  may be in a direction parallel to a working plane. 
     The xy plane may refer to a plane parallel to the working plane, and the z-axis may refer to an axis in a direction perpendicular to the working plane or an axis in a vertical direction. The x-axis may refer to an arm direction, a working direction, or a front direction in a neutral state of the manipulator  100 . The arm direction may refer to a direction of a hand link  414  to be described below. 
     The parallelogram link set  412  includes a first link  531  connected to the first joint  511  and a second link  532  connected to the second joint  512 . The first link  531  and the second link  532  are parallel to each other and have a double link structure, and are located on the same side of the parallelogram in the parallelogram link set  412 . Only one end of each of the first link  531  and the second link  532  is connected to the base  411  by the first joint  511  and the second joint  512  corresponding to each other. 
     The second actuator corresponding to the second joint  512  may be connected to or disposed on the base  411  of the 5-axis manipulator  100 . Therefore, the load on the arm by the second actuator may be reduced and the driving torque of the second joint may be maximized. 
     Of the links included in the parallelogram link set  412 , the link not connected to the base  411  has a link structure protruding in a direction extending in the arm direction or the x-axis direction, and the third joint  513  may be disposed on the protruding link. That is, the third joint  513 , the second passive joint  522 , and the third manual joint  523  may be disposed on the link that is not connected to the base  411  among the links included in the parallelogram link set  412 . 
     A swing arm link  413  may be connected to the parallelogram link set  412  through the third joint  513 . The third axis  313  may be parallel to the z-axis direction. That is, the third axis  313  may be in a direction perpendicular to the work plane. 
     The fourth joint  514  may be disposed at the end of the swing arm link  413  in the arm direction. A hand link  414  may be connected to the swing arm link  413  through the fourth joint  514 . The fourth axis  314  may be parallel to the y-axis direction when the swing arm link  413  is in a neutral state. That is, the fourth axis  314  may be the direction of the hand link  414 . 
     The fifth joint  515  may be disposed on the hand link  414 . The end-effector may be connected to the hand link  414  through the fifth joint  515 . The fifth axis  315  may be parallel to the y-axis direction when the swing arm link  413  and the hand link  414  are in a neutral state. That is, the fifth axis  315  may be in a direction parallel to the working plane and perpendicular to the fourth axis  314 . 
     That is, the position of the end-effector in the z-axis direction (more specifically, the position in the xz plane) may be adjusted by rotation at the first joint  511  and the second joint  512 , the position of the end-effector in the xy plane may be adjusted by rotation at the third joint  513 , and the direction of the end-effector may be determined by rotation at the fourth joint  514  and the fifth joint  515 . 
     The following [Table 1] shows examples of the numerical values of the links of the 5-axis manipulator  100 , and the following [Table 2] shows the characteristics of the motor of the 5-axis manipulator  100 . The numerical values shown in [Table 1] and [Table 2] are only examples, and the values may vary according to various embodiments. 
     
       
         
           
               
               
             
               
                 TABLE 1 
               
             
            
               
                   
               
               
                   
                 Link 
               
            
           
           
               
               
               
               
               
               
               
               
            
               
                   
                 l 1   
                 l 2   
                 l 3   
                 l 4   
                 l 5   
                 l a   
                 l b   
               
               
                   
               
            
           
           
               
               
               
               
               
               
               
               
            
               
                 Length 
                 330 
                 210 
                 203.5 
                 96.5 
                 62.5 
                 130 
                 330 
               
               
                 (mm) 
                   
                   
                   
                   
                   
                   
                   
               
               
                 Mass 
                 5.67 
                 3.55 
                 1.57 
                 2.05 
                 — 
                 0.85 
                 1.53 
               
               
                 (kg) 
                   
                   
                   
                   
                   
                   
                   
               
               
                   
               
            
           
         
       
     
     
       
         
           
               
               
               
             
               
                   
                 TABLE 2 
               
             
            
               
                   
                   
               
               
                   
                   
                 Motor 
               
            
           
           
               
               
               
               
               
               
               
            
               
                   
                   
                 First 
                 Second 
                 Third 
                 Fourth 
                 Fifth 
               
               
                   
                   
                 motor 
                 motor 
                 motor 
                 motor 
                 motor 
               
               
                   
                   
               
            
           
           
               
               
               
               
               
               
               
            
               
                   
                 Power 
                 600 
                 200 
                 400 
                 200 
                 100 
               
               
                   
                 (W) 
                   
                   
                   
                   
                   
               
               
                   
                 Mass 
                 5.4 
                 1.8 
                 2.25 
                 1.8 
                 1.1 
               
               
                   
                 (kg) 
               
               
                   
                   
               
            
           
         
       
     
     The 5-axis manipulator  100  adopts a parallelogram mechanism so that the center of mass may be disposed near the base  411  of the manipulator  100 . This is because the center of mass is directly related to the performance of the actuator, and the load imposed on each joint decreases as the center of mass approaches the base  411 . In addition, this is because the parallelogram mechanism has a kinematically and dynamically separated structure. As described above, the first joint  511  and the second joint  512  may determine the positions in the xz plane. The 5-axis manipulator  100  may improve the dynamic response in planar operation by adopting a swing arm mechanism. The third motor or the third actuator may rotate the swing arm link  413  rotating about the third axis  313  parallel to the plane direction (xy plane). 
     The 5-axis manipulator  100  may employ a feed forward torque control function using a self-weight effect. When the finishing work is started, the function of the second motor corresponding to the second joint  512  in the 5-axis manipulator  100  may be switched to a feed forward torque control function rather than a position control function. A normal force that the end-effector presses against the workpiece may be constantly maintained according to the weight of the link and the set torque value. Therefore, the 5-axis manipulator  100  may overcome the limitations of robot feedback control, such as joint stiffness, response speed, and bandwidth, and may perform finishing processing. The 5-axis manipulator  100  may utilize the weight of the link or arm as a reaction force against the machining force by using the self-weight effect. 
     In one embodiment, the total weight of the manipulator  100  excluding the base may be about 25 kg. The base of the manipulator  100  is mounted on the z-axis slide of the finishing machine  1  or the first sliding table  300 , and may include a mounting jig. 
     The motion of the manipulator  100  may be determined by the driving torque of each actuator  130 , which may be calculated by a kinetic energy function and a potential energy function of the system. In order to increase linearization performance, an inverse kinematic model of the manipulator  100  may be applied to a motion control process. 
     The compliance of the manipulator  100  may be given by the following [Equation 1] to [Equation 4]. M may be a moment of inertia matrix, C may be a Coriolis and centrifugal force matrix, G may be a gravity vector, λ may be an external force vector including a Jacobian of the manipulator  100 , and T is a torque vector of a set of actuators  130  required to produce a particular movement. q may be a displacement vector of each axis or each joint, Kq may be a joint rotation stiffness matrix, and Δq may be a deformation value of each joint due to joint stiffness. 
     
       
         
           
             
               
                 
                   τ 
                   = 
                   
                     
                       
                         
                           M 
                           ⁡ 
                           
                             ( 
                             q 
                             ) 
                           
                         
                         ⁢ 
                         
                           q 
                           ¨ 
                         
                       
                       + 
                       
                         
                           C 
                           ⁡ 
                           
                             ( 
                             
                               q 
                               , 
                               
                                 q 
                                 . 
                               
                             
                             ) 
                           
                         
                         ⁢ 
                         
                           q 
                           . 
                         
                       
                       + 
                       
                         g 
                         ⁡ 
                         
                           ( 
                           q 
                           ) 
                         
                       
                       - 
                       
                         
                           
                             J 
                             T 
                           
                           ⁡ 
                           
                             ( 
                             q 
                             ) 
                           
                         
                         ⁢ 
                         λ 
                       
                     
                     = 
                     
                       
                         K 
                         q 
                       
                       ⁢ 
                       Δq 
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     1 
                   
                   ] 
                 
               
             
             
               
                 
                   
                     τ 
                     - 
                     
                       τ 
                       0 
                     
                   
                   = 
                   
                     
                       - 
                       
                         
                           J 
                           T 
                         
                         ⁡ 
                         
                           ( 
                           
                             λ 
                             - 
                             
                               λ 
                               0 
                             
                           
                           ) 
                         
                       
                     
                     = 
                     
                       
                         K 
                         q 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             Δ 
                             ⁢ 
                             q 
                           
                           - 
                           
                             Δ 
                             ⁢ 
                             
                               q 
                               0 
                             
                           
                         
                         ) 
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     2 
                   
                   ] 
                 
               
             
           
         
       
     
     [Equation 1] above may mean that the manipulator compliance is affected by the rotation displacement generated by the joint torque, and [Equation 2] may mean a difference value of torque depending on the presence or absence of an external force. 
     The Cartesian coordinates obtained from the joint coordinates may be expressed as [Equation 3] below. AX may mean a deformation vector of the end-effector, and CX may mean a Cartesian space compliance matrix. 
     
       
         
           
             
               
                 
                   
                     
                       Δ 
                       ⁢ 
                       x 
                     
                     - 
                     
                       Δ 
                       ⁢ 
                       
                         x 
                         0 
                       
                     
                   
                   = 
                   
                     
                       J 
                       ⁡ 
                       
                         ( 
                         
                           
                             Δ 
                             ⁢ 
                             q 
                           
                           - 
                           
                             Δ 
                             ⁢ 
                             
                               q 
                               0 
                             
                           
                         
                         ) 
                       
                     
                     = 
                     
                       J 
                       ⁢ 
                       
                         K 
                         q 
                         
                           - 
                           1 
                         
                       
                       ⁢ 
                       
                         
                           J 
                           T 
                         
                         ⁡ 
                         
                           ( 
                           
                             λ 
                             - 
                             
                               λ 
                               0 
                             
                           
                           ) 
                         
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     3 
                   
                   ] 
                 
               
             
           
         
       
     
     As a result, the displacement of the end-effector due to an external force may be defined as [Equation 4] and [Equation 5] below. 
     
       
         
           
             
               
                 
                   
                     Δ 
                     ⁢ 
                     x 
                   
                   = 
                   
                     
                       C 
                       X 
                     
                     ⁢ 
                     λ 
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     4 
                   
                   ] 
                 
               
             
             
               
                 
                   
                     C 
                     X 
                     ′ 
                   
                   = 
                   
                     
                       C 
                       X 
                     
                     + 
                     
                       C 
                       
                         w 
                         ⁢ 
                         h 
                         ⁢ 
                         e 
                         ⁢ 
                         e 
                         ⁢ 
                         l 
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     5 
                   
                   ] 
                 
               
             
           
         
       
     
     The overall compliance includes the manipulator and the wheel. If the wheel stiffness is high, the manipulator may be controlled by a position control method of producing a correct machine depth. The specified wheel may be recommended when removing tool marks while force control is performed so as not to damage a pre-machined original shape. [Table 3] below shows the z-axis overall compliance for each wheel use case. 
     
       
         
           
               
               
             
               
                 TABLE 3 
               
             
            
               
                   
               
               
                   
                 Case 
               
            
           
           
               
               
               
               
               
               
               
            
               
                   
                 w/o wheel 
                 #24 
                 #80 
                 #120 
                 #180 
                 #400 
               
               
                   
               
               
                 compliance 
                 4.8 
                 12.2 
                 35.6 
                 42.5 
                 83.8 
                 108.9 
               
               
                 c z  (μm/N) 
               
               
                   
               
            
           
         
       
     
     Referring to [Table 3], it can be seen that the compliance value is higher when the abrasive wheel is present than when the abrasive wheel is absent, and the compliance value is higher as the mesh number increases. 
       FIG. 6  is a view showing a method for controlling the manipulator  100  based on an inverse kinematic model according to an embodiment of the present disclosure. 
     Specifically,  FIG. 6  shows an example of a method for controlling the manipulator  100  involving feed forward torque control. 
     Referring to  FIG. 6 , in the method for controlling the manipulator  100  based on the inverse kinematic model according to an embodiment of the present disclosure, a separate PC  610  may transmit a position command and a torque command to a motion controller  620 . The motion controller  620  may refer to the processor  190  shown in  FIG. 2 . 
     Although  FIG. 6  shows an embodiment in which the manipulator  100  operates under the control of the separate PC  610 , the present disclosure is not limited thereto. That is, in one embodiment, the processor  180  may perform both the role of the PC  610  and the role of the motion controller  620  shown in  FIG. 6 . 
     The motion controller  620  may transmit a real-time torque value to the servo  630  based on motion information (or motion information  661 ) of the robot  650 , the position command, and the torque command. The robot  650  may refer to an end-effector of the manipulator  100  or a rotary tool mounted on the end-effector. The motion information  661  of the robot  650  may refer to posture information of the robot  650 , and the posture information of the robot  650  may refer to the posture of each motor or each joint. 
     The servo  630  may perform position control and torque control on the motor  640  based on the motion information  661  of the robot  650  and the real-time torque value received from the motion controller  620 . The robot  650  may be controlled by the operation of the motor  640 . 
     In one embodiment, the motor or the actuator may be used as a generic term for the servo  630  and the motor  640 . That is, even if simply referred to as the motor or the actuator, it may refer to the motor  640  including the servo  630 . 
     The PC  610  may perform calibration  681  for the control of the motor  640  by using the motion information  661  and the force information  671  of the robot  650 . The calibration for the control of the motor  640  may include position correction, dynamics correction, torsional compliance, bearing compliance, and the like. 
     The PC  610  may perform preprocessing  691  by using a kinematic model, a dynamic mode, a finishing condition, and a compliance model in the calibration process  681 . The PC  610  may improve the control process of the manipulator  100  based on the calibration  681  and the preprocessing  691 . 
     In one embodiment, the PC  610  may adjust K based on the force information  671  and may correct the torque. K may refer to mass and length parameters in the inverse kinematic model. 
     Although  FIG. 6  shows the method in which the processor  180  of the manipulator  100  controls the operation of the manipulator  100  by generating the control signal for the actuator  130 , the present disclosure is not limited thereto. That is, in another embodiment, an external device, for example, the PC  610 , may generate a control signal or a control command for the actuator  130  of the manipulator  100 , and the processor  180  of the manipulator  100  may control the actuator  130  based on the control signal or the control command generated by the external device. 
       FIG. 7  is a view showing a simulation process for the manipulator  100  according to an embodiment of the present disclosure. 
     Referring to  FIG. 7 , the simulation of the manipulator  100  may include an online calculation  710  and an offline calculation  720 . 
     The dynamic performance of the manipulator  100  may be estimated by employing a virtual controller, a servo drive, and hardware. The motion controller part may include a PID control loop and a filter for a frequency response, the servo drive part may include a motor mechanism, and the hardware part may include a rigid manipulator  100  and a transmission mechanism having a gear ratio and transmission efficiency. 
     The online calculation  710  may represent the actual communication between the manipulator  100  and the controller by encoder signals and currents. The offline calculation  720  may represent virtual communication. 
       FIG. 8  is a view for explaining a constrained motion of the manipulator according to an embodiment of the present disclosure. 
     Referring to  FIG. 8 , an abrasive wheel attached to an end-effector of the manipulator  100  moves while being constrained by a given work surface, and is affected by a reaction force generated by a cutting force. Therefore, it is necessary to calculate a driving torque for coping with a given constrained path rather than a forward kinematics motion based on surface shape data. 
     The surface constraint condition may be assumed as in [Equation 6] below. The constraint condition may be defined as [Equation 7] below through a relationship between a trajectory φ 811  shown in  FIG. 8  and a kinematic element of the manipulator  100 . h is a function representing the constraint condition for the manipulator  100 , and hj is a function representing the constraint condition when the j-axis direction is constrained. 
     
       
         
           
             
               
                 
                   
                     
                       h 
                       j 
                     
                     ⁡ 
                     
                       ( 
                       
                         
                           q 
                           1 
                         
                         , 
                         … 
                         ⁢ 
                         
                             
                         
                         , 
                         
                           q 
                           n 
                         
                       
                       ) 
                     
                   
                   = 
                   0 
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     6 
                   
                   ] 
                 
               
             
             
               
                 
                   
                     
                       
                         h 
                         j 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             
                               f 
                               1 
                             
                             ⁡ 
                             
                               ( 
                               ϕ 
                               ) 
                             
                           
                           , 
                           … 
                           ⁢ 
                           
                               
                           
                           , 
                           
                             
                               f 
                               n 
                             
                             ⁡ 
                             
                               ( 
                               ϕ 
                               ) 
                             
                           
                         
                         ) 
                       
                     
                     = 
                     0 
                   
                   , 
                   
                       
                   
                   ⁢ 
                   
                     
                       f 
                       ⁡ 
                       
                         ( 
                         ϕ 
                         ) 
                       
                     
                     = 
                     q 
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     7 
                   
                   ] 
                 
               
             
           
         
       
     
     Furthermore, a constraint condition in which elastic deformation is added due to a deviation between a nominal tool trajectory and an actual tool trajectory may be defined as [Equation 8] and [Equation 9] below. 
     
       
         
           
             
               
                 
                   
                     
                       h 
                       j 
                     
                     ⁡ 
                     
                       ( 
                       
                         
                           
                             q 
                             1 
                           
                           + 
                           
                             Δ 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               q 
                               1 
                             
                           
                         
                         , 
                         … 
                         ⁢ 
                         
                             
                         
                         , 
                         
                           
                             q 
                             n 
                           
                           + 
                           
                             Δ 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               q 
                               n 
                             
                           
                         
                       
                       ) 
                     
                   
                   = 
                   0 
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     8 
                   
                   ] 
                 
               
             
             
               
                 
                   
                     
                       
                         h 
                         f 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             
                               
                                 f 
                                 1 
                               
                               ⁡ 
                               
                                 ( 
                                 ϕ 
                                 ) 
                               
                             
                             + 
                             
                               
                                 c 
                                 1 
                               
                               ⁡ 
                               
                                 ( 
                                 Δϕ 
                                 ) 
                               
                             
                           
                           , 
                           … 
                           ⁢ 
                           
                               
                           
                           , 
                           
                             
                               
                                 f 
                                 n 
                               
                               ⁡ 
                               
                                 ( 
                                 ϕ 
                                 ) 
                               
                             
                             + 
                             
                               
                                 c 
                                 n 
                               
                               ⁡ 
                               
                                 ( 
                                 Δϕ 
                                 ) 
                               
                             
                           
                         
                         ) 
                       
                     
                     = 
                     0 
                   
                   , 
                   
                     
                       c 
                       ⁡ 
                       
                         ( 
                         
                           Δ 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           q 
                         
                         ) 
                       
                     
                     = 
                     Δϕ 
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     9 
                   
                   ] 
                 
               
             
           
         
       
     
     As a result, the constrained motion according to the constraint conditions in the x-direction and the z-direction may be expressed as [Equation 10] and [Equation 11] below. T may represent a transformation matrix from the base to the end-effector, t may represent a tangent vector of the end-effector, and n may represent a normal vector of the end-effector. 
     
       
         
           
             
               
                 
                   
                     
                       h 
                       x 
                     
                     ⁡ 
                     
                       ( 
                       
                         q 
                         + 
                         
                           Δ 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           q 
                         
                       
                       ) 
                     
                   
                   = 
                   
                     
                       [ 
                       
                         
                           
                             T 
                             tool 
                             base 
                           
                           ⁡ 
                           
                             ( 
                             
                               1 
                               , 
                               2 
                             
                             ) 
                           
                         
                         , 
                         
                           
                             T 
                             tool 
                             base 
                           
                           ⁡ 
                           
                             ( 
                             
                               2 
                               , 
                               2 
                             
                             ) 
                           
                         
                         , 
                         
                           
                             T 
                             tool 
                             base 
                           
                           ⁡ 
                           
                             ( 
                             
                               3 
                               , 
                               2 
                             
                             ) 
                           
                         
                       
                       ] 
                     
                     · 
                     
                       
                         [ 
                         
                           
                             t 
                             x 
                           
                           , 
                           
                             t 
                             y 
                           
                           , 
                           
                             t 
                             z 
                           
                         
                         ] 
                       
                       T 
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     10 
                   
                   ] 
                 
               
             
             
               
                 
                   
                     
                       h 
                       z 
                     
                     ⁡ 
                     
                       ( 
                       
                         q 
                         + 
                         
                           Δ 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           q 
                         
                       
                       ) 
                     
                   
                   = 
                   
                     
                       [ 
                       
                         
                           
                             T 
                             tool 
                             base 
                           
                           ⁡ 
                           
                             ( 
                             
                               1 
                               , 
                               1 
                             
                             ) 
                           
                         
                         , 
                         
                           
                             T 
                             tool 
                             base 
                           
                           ⁡ 
                           
                             ( 
                             
                               2 
                               , 
                               1 
                             
                             ) 
                           
                         
                         , 
                         
                           
                             T 
                             tool 
                             base 
                           
                           ⁡ 
                           
                             ( 
                             
                               3 
                               , 
                               1 
                             
                             ) 
                           
                         
                       
                       ] 
                     
                     · 
                     
                       
                         [ 
                         
                           
                             n 
                             x 
                           
                           , 
                           
                             n 
                             y 
                           
                           , 
                           
                             n 
                             z 
                           
                         
                         ] 
                       
                       T 
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     11 
                   
                   ] 
                 
               
             
           
         
       
     
       FIG. 9  is a view for explaining feed forward torque control of the manipulator  100  according to an embodiment of the present disclosure. 
     Referring to  FIG. 9 , the cutting force mainly depends on the second joint  512  due to the parallelogram structure of the manipulator  100 . A feed forward torque control method may be employed so as to maintain a constant reaction force in the constrained motion. In this case, the positions and forces of the respective joints  511  to  515  may be simultaneously controlled in accordance with the estimated driving torque for the respective joints  511  to  515  based on the force control rule. This control method has an advantage that force control without delay time is possible and the force error is not affected by dynamic motion in a non-constrained direction on the surface. Furthermore, in these operations in which surface roughness is important, such as lapping or polishing, generating smooth, uniform torque from the actuator is more appropriate than generating a frequent force control response by sensor feedback. 
     Consequently, feed forward torque control in the form of gravity feed may be employed for indirect force control, and the force of the end-effector may be determined by the deadweight of the manipulator  100  including the tool and the driving torque of the second joint  512 . 
     The dynamic relationship considering the motion constraint may be expressed as [Equation 12] below. τ φ  may refer to the driving torque of the unconstrained manipulator, and τ h  may refer to the driving torque only due to constraint. 
     
       
         
           
             
               
                 
                   τ 
                   = 
                   
                     
                       τ 
                       ϕ 
                     
                     + 
                     
                       τ 
                       h 
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     12 
                   
                   ] 
                 
               
             
           
         
       
     
     The driving torque of the unconstrained manipulator may be expressed as [Equation 13] based on [Equation 1], and this is because the kinetic energy term can be neglected because the joint motion is very slow. 
     
       
         
           
             
               
                 
                   
                     τ 
                     ϕ 
                   
                   = 
                   
                     
                       
                         M 
                         ⁡ 
                         
                           ( 
                           q 
                           ) 
                         
                       
                       ⁢ 
                       
                         q 
                         .. 
                       
                     
                     + 
                     
                       
                         C 
                         ⁡ 
                         
                           ( 
                           
                             q 
                             , 
                             
                               q 
                               . 
                             
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         q 
                         . 
                       
                     
                     + 
                     
                       g 
                       ⁡ 
                       
                         ( 
                         q 
                         ) 
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     13 
                   
                   ] 
                 
               
             
           
         
       
     
     Therefore, the driving torque of the lapping operation may generate a model as shown in [Equation 14] and [Equation 15], and the gradient term of the constraint condition may be obtained by [Equation 10] and [Equation 11]. 
     
       
         
           
             
               
                 
                   
                     τ 
                     h 
                   
                   = 
                   
                     ∑ 
                     
                       
                         λ 
                         j 
                       
                       ⁢ 
                       
                         ∇ 
                         
                           
                             h 
                             j 
                           
                           ⁡ 
                           
                             ( 
                             q 
                             ) 
                           
                         
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     14 
                   
                   ] 
                 
               
             
             
               
                 
                   τ 
                   = 
                   
                     
                       
                         τ 
                         ϕ 
                       
                       + 
                       
                         τ 
                         h 
                       
                     
                     = 
                     
                       
                         g 
                         ⁡ 
                         
                           ( 
                           q 
                           ) 
                         
                       
                       + 
                       
                         
                           λ 
                           z 
                         
                         ⁢ 
                         
                           ∇ 
                           
                             
                               h 
                               z 
                             
                             ⁡ 
                             
                               ( 
                               q 
                               ) 
                             
                           
                         
                       
                       + 
                       
                         
                           λ 
                           x 
                         
                         ⁢ 
                         
                           ∇ 
                           
                             
                               h 
                               x 
                             
                             ⁡ 
                             
                               ( 
                               q 
                               ) 
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     15 
                   
                   ] 
                 
               
             
           
         
       
     
     The 5-axis manipulator  100  according to an embodiment of the present disclosure is designed such that the second joint  512  provides a leverage effect. Therefore, the second joint  512  has a structure that is optimized to respond to a reaction force in a plane by using the weight of the manipulator  100 . Therefore, the feed forward torque control rule according to [Equation 16] below may be applied only to the second joint  512 . That is, only the second joint  512  or the second actuator corresponding to the second joint  512  may receive the control voltage converted by the wireless controller or the processor  190 , and may perform feed forward torque control. This feed forward torque control does not involve position feedback and force feedback. 
     
       
         
           
             
               
                 
                   
                     τ 
                     2 
                   
                   = 
                   
                     
                       
                         g 
                         2 
                       
                       ⁡ 
                       
                         ( 
                         q 
                         ) 
                       
                     
                     + 
                     
                       
                         λ 
                         z 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             
                               h 
                               z 
                             
                             ⁡ 
                             
                               ( 
                               q 
                               ) 
                             
                           
                         
                         
                           ∂ 
                           
                             q 
                             2 
                           
                         
                       
                     
                     + 
                     
                       
                         λ 
                         x 
                       
                       ⁢ 
                       
                         
                           ∂ 
                           
                             
                               h 
                               x 
                             
                             ⁡ 
                             
                               ( 
                               q 
                               ) 
                             
                           
                         
                         
                           ∂ 
                           
                             q 
                             2 
                           
                         
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     16 
                   
                   ] 
                 
               
             
           
         
       
     
     In the 5-axis manipulator  100 , as the first axis  311  and the second axis  312  have a dynamically decoupled parallelogram structure, the feed forward torque by the second joint  512  may be simplified as shown in [Equation 17] below. k 1  and k 2  are control gains. As the gain is higher, the actuator generates higher torque and the end-effector generates greater force. The gain values may be determined and adjusted by a preliminary experiment defining a relationship between the end-effector force and the gain value. 
     
       
         
           
             
               
                 
                   
                     τ 
                     2 
                   
                   = 
                   
                     
                       
                         k 
                         1 
                       
                       ⁢ 
                       cos 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         q 
                         2 
                       
                     
                     + 
                     
                       
                         k 
                         2 
                       
                       ⁢ 
                       
                         sin 
                         ⁡ 
                         
                           ( 
                           
                             
                               q 
                               
                                 2 
                                 - 
                               
                             
                             ⁢ 
                             
                               q 
                               5 
                             
                           
                           ) 
                         
                       
                       ⁢ 
                       
                         ( 
                         
                           
                             λ 
                             x 
                           
                           , 
                           
                             q 
                             3 
                           
                           , 
                           
                             
                               q 
                               4 
                             
                             = 
                             0 
                           
                         
                         ) 
                       
                     
                   
                 
               
               
                 
                   [ 
                   
                     Equation 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     17 
                   
                   ] 
                 
               
             
           
         
       
     
     Referring to [Equation 17] above, when the external force λ x  in the x-axis direction or the working direction is 0 and the displacement q 3  of the third actuator and the displacement q 4  of the fourth actuator are 0, the feed forward torque τ 2  of the second actuator may be determined based on the displacement q 2  of the second actuator and the displacement q 5  of the fifth actuator. 
       FIG. 10  is a control block diagram of the manipulator  100  according to an embodiment of the present disclosure. 
     Referring to  FIG. 10 , when a desired force is commanded, the driving torque of the second actuator corresponding to the second joint  512  is generated through the feed forward torque model, and the feedback control mode may be ended. When the force control is ended, the feed forward control mode is ended, and may be switched to the feedback control mode so as to move to a target point. The actuators other than the second actuator may maintain only the position control regardless of the control situation. 
       FIG. 11  is a diagram showing a force control result according to compliance control when an abrasive wheel is absent. 
     Specifically, (a) of  FIG. 11  shows the end-effector force, and (b) of  FIG. 11  shows the commanded voltage for motor control torque transmission. 
     Referring to (a) of  FIG. 11 , since the force generated by the torsional stiffness of the reduction gear or the compliant abrasive wheel is applied, the force response in the transient state immediately after the force command changes abruptly. 
     Referring to (b) of  FIG. 11 , since the performance of force control is determined by the physical rigidity of the passive system, the feedback of the motor adjusted based on the robot dynamics is unstable. 
       FIG. 12  is a view showing a force control result according to feed forward torque control in the case where an abrasive wheel is absent. 
     Specifically, (a) of  FIG. 12  shows the end-effector force, and (b) of  FIG. 12  shows the commanded voltage for motor control torque transmission. 
     Referring to (a) of  FIG. 12 , when the feed forward torque control is followed, the end-effector force is well controlled and maintained according to the command value. 
     Referring to (b) of  FIG. 12 , the driving voltage of the motor has a value between −6 V and +6 V, and generates 100% of the rated torque. As described above, since the force applied according to the feed forward torque control proposed in the present disclosure is determined by the driving torque of the second actuator, the driving torques of the first actuator (or the first motor) and the fifth actuator (or the fifth motor) may be kept constant by focusing only on the position control of the manipulator  100 . 
     Compared with the compliance control, the feed forward torque control has an advantage of helping to suppress the vibration of the manipulator  100  caused by the rapid dynamic cutting force. 
       FIG. 13  is a view showing a comparison between conventional force feedback control and feed forward control proposed in the present disclosure. 
     Referring to  FIG. 13 , the feed forward control proposed in the present disclosure shows an unrestricted and indirect force control aspect of feedback vibration. The end-effector of the manipulator  100  was moved from a position of 100 mm to a position of 200 mm in the x-axis according to a linear interpolation method. 
     In the feed forward control case, only the second motor may follow the calculated torque command, and the other motors may follow the servo control rule. In this case, the force error may occur in an unstable posture due to the compliance error due to the change in the posture of the manipulator  100 . 
     In the force feedback control case, it was assumed that all motors followed the servo control rule and the end-effector was a virtual force-controlled tool. In this case, although the compliance error is eliminated, it can be confirmed that feedback vibration has been generated due to the impedance condition between the end-effector and the reaction surface.