Patent Publication Number: US-11376734-B2

Title: Trajectory control device

Description:
CROSS-REFERENCE TO RELATED APPLICATION 
     This application is based upon and claims the benefit of priority from Japanese Patent Application No. 2019-211058 filed on Nov. 22, 2019, the contents of which are incorporated herein by reference. 
     BACKGROUND OF THE INVENTION 
     Field of the Invention 
     The present invention relates to a trajectory control device for moving a member such as a dispenser along a trajectory on a workpiece. 
     Description of the Related Art 
     Trajectory control in operations such as line application with dispensers, welding, and so on using industrial robots utilizes sensor technologies such as high-resolution cameras, image sensors, and so on, in order to detect the position of the trajectory and correct trajectory error caused by positional displacement of the workpiece. 
     For example, Japanese Laid-Open Patent Publication No. 2008-272814 describes a robot system that performs a tracking control of correcting the position of a welding torch while detecting the position of the welding line using a sensor capable of radiating laser slit light. 
     SUMMARY OF THE INVENTION 
     However, using a high-resolution camera or image sensor requires introducing an expensive and complicated system. 
     The present invention has been devised considering such a situation, and an object of the present invention is to provide a trajectory control device capable of performing trajectory correction by recognizing positional displacement and the amount of movement of a workpiece, without using a high-precision camera or image sensor. 
     An aspect of the present invention is directed to a trajectory control device for moving a trajectory tracking member along a trajectory on a workpiece that is placed in an arbitrary position. The trajectory control device includes: a contact sensor configured to contact side surfaces of the workpiece; an actuator configured to move the trajectory tracking member and the contact sensor; and a trajectory controller configured to calculate XY coordinates of the trajectory on the workpiece placed in the arbitrary position, by transforming XY coordinates of the trajectory on the workpiece in a reference position, based on positional information about the side surfaces of the workpiece in the reference position and positional information about the side surfaces of the workpiece placed in the arbitrary position. The positional information about the side surfaces of the workpiece placed in the arbitrary position is obtained by the contact sensor. 
     According to the trajectory control device, it is possible to obtain XY coordinates of a trajectory on a workpiece that is placed in an arbitrary position by means of a simple method without using a high-precision camera or image sensor. 
     The trajectory control device of the invention calculates the XY coordinates of the trajectory on the workpiece placed in an arbitrary position from known data indicating XY coordinates of the trajectory on the workpiece in a reference position, based on positional information about side surfaces of the workpiece that is obtained by the contact sensor. Accordingly, it is possible to obtain the XY coordinates of the trajectory on the workpiece placed in the arbitrary position by means of a simple method without using a high-precision camera or image sensor. 
     The above and other objects, features, and advantages of the present invention will become more apparent from the following description when taken in conjunction with the accompanying drawings, in which a preferred embodiment of the present invention is shown by way of illustrative example. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  is a conceptual diagram of a trajectory control device according to an embodiment of the present invention; 
         FIG. 2  is a diagram illustrating an XY coordinate system of the trajectory control device of  FIG. 1  and a workpiece placed therein; 
         FIG. 3  is a diagram illustrating data that is stored in a control table T 1  in a specific example in which a workpiece has moved without rotating; 
         FIG. 4  is a diagram illustrating data that is stored in a control table T 2  in the specific example of  FIG. 3 ; 
         FIG. 5  is a diagram illustrating data that is stored in control tables T 3  to T 5  in the specific example of  FIG. 3 ; 
         FIG. 6  is a diagram illustrating data that is stored in a control table T 6  in the specific example of  FIG. 3 ; 
         FIG. 7  is a diagram illustrating data that is stored in a control table T 7  in the specific example of  FIG. 3 ; 
         FIG. 8  is a diagram illustrating the results of a coordinate transformation of a workpiece alignment in the specific example of  FIG. 3 ; 
         FIG. 9  is a diagram illustrating the results of a coordinate transformation of trajectory data in the specific example of  FIG. 3 ; 
         FIG. 10  is a diagram illustrating data that is stored in the control table T 1  in a specific example in which a workpiece has moved with rotating; 
         FIG. 11  is a diagram illustrating data that is stored in the control table T 2  in the specific example of  FIG. 10 ; 
         FIG. 12  is a diagram illustrating data that is stored in the control tables T 3  to T 5  in the specific example of  FIG. 10 ; 
         FIG. 13  is a diagram illustrating data that is stored in the control table T 6  in the specific example of  FIG. 10 ; 
         FIG. 14  is a diagram illustrating data that is stored in the control table T 7  in the specific example of  FIG. 10 ; 
         FIG. 15  is a diagram illustrating the results of a coordinate transformation of a workpiece alignment in the specific example of  FIG. 10 ; and 
         FIG. 16  is a diagram illustrating the results of a coordinate transformation of trajectory data in the specific example of  FIG. 10 . 
     
    
    
     DESCRIPTION OF THE PREFERRED EMBODIMENTS 
     The trajectory control device of the invention will be described below in connection with preferred embodiments while referring to the accompanying drawings. 
     As shown in  FIG. 1 , a trajectory control device  10  includes a trajectory controller  12 , a contact sensor  26 , a trajectory tracking member  28 , and a group of electric actuators  24 . The trajectory controller  12  includes a trajectory control unit  14 , a motor control unit  16 , an amount-of-correction operating unit  18 , a coordinate transformation unit  20 , and an I/O unit (input/output unit)  22 . Each unit of the trajectory controller  12  is realized by one or multiple pieces of hardware and software. 
     The trajectory controller  12 , mainly the trajectory control unit  14 , performs a detection control to detect the position of a workpiece W by moving the contact sensor  26  by driving the group of electric actuators  24 , and also performs a trajectory control to move the trajectory tracking member  28  along a trajectory on the workpiece by driving the group of electric actuators  24 . 
     The contact sensor  26  detects the position of the workpiece W by mechanically contacting side surfaces of the workpiece W. The trajectory tracking member  28  is a member corresponding to the tip of a welding gun, or the tip of a dispenser nozzle for supplying adhesive or seal material, and is attached to a device installed on a welding line or application line. 
     The group of electric actuators  24  includes an X-axis actuator  24   a , a Y-axis actuator  24   b , and a Z-axis actuator  24   c . The contact sensor  26  and the trajectory tracking member  28  are both attached to the Z-axis actuator  24   c . The X-axis actuator  24   a  and the Y-axis actuator  24   b  serve to move the contact sensor  26  and the trajectory tracking member  28  to an arbitrary position in the plane of the XY coordinate system. 
     The Z-axis actuator  24   c  serves to adjust the positions of the contact sensor  26  and the trajectory tracking member  28  along the Z-axis direction (up-down direction). When the trajectory controller  12  performs the detection control, the contact sensor  26  moves downward to an operating position, while the trajectory tracking member  28  moves upward to a withdrawn position. On the other hand, when the trajectory controller  12  performs the trajectory control, the trajectory tracking member  28  moves downward to an operating position, while the contact sensor  26  moves upward to a withdrawn position. 
     The trajectory control unit  14  holds trajectory control data including data relating to the trajectory on the workpiece and motor control data. The trajectory data is electronic data such as DXF files created by CAD, and is formed of attribute commands such as Polyline and Arc and XY coordinate data. The motor control data is electronic data indicating the position, speed, acceleration, and so on of the X-axis actuator  24   a , the Y-axis actuator  24   b , and the Z-axis actuator  24   c.    
     The trajectory control unit  14  further holds XY coordinate data that represents a plane configuration of the workpiece W (which will be referred to as “workpiece alignment” hereinafter), as well as attribute data relating to side surfaces and a reference point of the workpiece W. The workpiece alignment at least includes data relating to two side surfaces that are adjacent to each other with the reference point therebetween. 
     The motor control unit  16  updates and holds, in real time, encoder values sent from motor encoders of the X-axis actuator  24   a  and the Y-axis actuator  24   b , as positional data of these actuators, and drives and controls the X-axis actuator  24   a  and the Y-axis actuator  24   b  on the basis of the trajectory control data from the trajectory control unit  14 . 
     The amount-of-correction operating unit  18  calculates the amount of correction (correction coefficients) of the trajectory on the workpiece, based on positional information about the side surfaces of the workpiece detected by the contact sensor  26 . The coordinate transformation unit  20  calculates coordinates of the trajectory on the workpiece that is placed in an arbitrary position, based on the amount of correction (correction coefficients) calculated by the amount-of-correction operating unit  18 . The I/O unit  22  serves to establish synchronization with peripheral devices, and the trajectory tracking member  28  and the contact sensor  26  are connected to the I/O unit  22 . 
     In the detection control, the trajectory controller  12  drives the X-axis actuator  24   a  and the Y-axis actuator  24   b  to move the contact sensor  26  in a direction toward a side surface of the workpiece W. When the contact sensor  26  comes in contact with the side surface of the workpiece W, then the contact sensor  26  notifies the trajectory controller  12  through the I/O unit  22  about the contact. When the trajectory controller  12  recognizes that the contact sensor  26  has come in contact with the side surface of the workpiece W, then the trajectory controller  12  stops the X-axis actuator  24   a  and the Y-axis actuator  24   b.    
     Next, the trajectory controller  12  reads the motor encoder value of the X-axis actuator  24   a  and the motor encoder value of the Y-axis actuator  24   b  from the motor control unit  16 , and holds the contact point of the contact sensor  26  on the side surface of the workpiece W as XY coordinate data. 
     This series of operations is repeated so as to obtain XY coordinate data of different two contact points on one side surface of the workpiece W. In the same way, this series of operations is repeated also for another side surface of the workpiece W so as to obtain XY coordinate data of different two contact points on this side surface. The position at which these two side surfaces of the workpiece W intersect is defined as the reference point. 
       FIG. 2  is a diagram illustrating the XY coordinate system of the trajectory control device  10  viewed from right above, and the position of the workpiece W placed thereon, and is used to explain operations of the amount-of-correction operating unit  18 . 
     In the XY coordinate system, the four side surfaces of the cuboidal workpiece W placed in a reference position is represented by the line connecting a point P 1  and a point P 2 , the line connecting the point P 2  and a point P 3 , the line connecting the point P 3  and a point P 4 , and the line connecting the point P 4  and the point P 1 . Among the four points P 1  to P 4 , the point P 1  is the reference point. Hereinafter, the side surface represented by the line connecting the point P 4  and the point P 1  is referred to as a workpiece side surface A, and the side surface represented by the line connecting the point P 1  and the point P 2  is referred to as a workpiece side surface B. 
     In this embodiment, the reference position is the position where the workpiece W is placed in such a manner that the workpiece side surface A is parallel to the Y axis and the workpiece side surface B is parallel to the X axis. However, for example, the reference position may be a position where the workpiece W is placed in such a manner that the workpiece side surface A and the workpiece side surface B are neither parallel to the X axis nor to the Y axis. 
     The plane configuration of the target workpiece W need not necessarily be rectangular, as long as the contour thereof has two straight line parts. In other words, it works if the side surfaces of the workpiece W have at least two flat surface portions. One or multiple pieces of trajectory data may exist in the rectangular range of the workpiece alignment, or may exist outside of the rectangular range of the workpiece alignment. Further, a plurality of workpieces W may be fixed to a single tray. 
     Now, referring to  FIG. 2 , the detection control will be described more specifically. Because a workpiece alignment WP (P 1 -P 2 -P 3 -P 4 ) placed in the reference position is previously held in the trajectory controller  12 , it is not necessary to detect the position of the side surfaces of the workpiece. However, here, the description will be given assuming that the workpiece W is first placed in the reference position. Accordingly, in practice, the detection of the side surfaces of the workpiece may be omitted for the workpiece W placed in the reference position. 
     The contact sensor  26  advances in a first movement direction  30  that is parallel to the X axis, stops when it reaches a point A 1  on the workpiece side surface A, and detects and holds the X coordinate and the Y coordinate of the point A 1 . In the same way, the contact sensor  26  advances in a second movement direction  32  that is parallel to the X axis, stops when it reaches a point A 2  on the workpiece side surface A, and detects and holds the X coordinate and the Y coordinate of the point A 2 . In this embodiment, the first movement direction  30  is a direction along the straight line that is expressed as y=40, and the second movement direction  32  is a direction along the straight line that is expressed as y=30. 
     Further, the contact sensor  26  advances in a third movement direction  34  that is parallel to the Y axis, stops when it reaches a point B 1  on the workpiece side surface B, and detects and holds the X coordinate and the Y coordinate of the point B 1 . In the same way, the contact sensor  26  advances in a fourth movement direction  36  that is parallel to the Y axis, stops when it reaches a point B 2  on the workpiece side surface B, and detects and holds the X coordinate and the Y coordinate of the point B 2 . In this embodiment, the third movement direction  34  is a direction along the straight line that is expressed as x=50, and the fourth movement direction  36  is a direction along the straight line that is expressed as x=40. 
     In this embodiment, the equation of the straight line passing through A 1  and A 2  is given as x=10, and the equation of the straight line passing through B 1  and B 2  is given as y=10. The intersection of these two straight lines is the reference point P 1 , and the XY coordinates of the reference point P 1  are (10, 10). As to these two straight lines, if a line passing through two points is expressed by a general expression y=ax+b, then a coefficient “a” and a coefficient “b” are both treated as zero (a=0, b=0). 
     Next, a workpiece alignment WQ that is placed in an arbitrary position will be described. The workpiece alignment WQ placed in an arbitrary position includes points Q 1  to Q 4  that correspond respectively to the four points P 1  to P 4  of the workpiece alignment WP placed in the reference position. Hereinafter, the side surface represented by the line connecting the point Q 4  and the point Q 1  is referred to as a workpiece side surface C, and the side surface represented by the line connecting the point Q 1  and the point Q 2  is referred to as a workpiece side surface D. 
     The contact sensor  26  advances in the aforementioned first movement direction  30  (y=40) that is parallel to the X axis, stops when it reaches a point C 1  on the workpiece side surface C corresponding to the workpiece side surface A in the reference position, and detects and holds the X coordinate and the Y coordinate of the point C 1 . In the same way, the contact sensor  26  advances in the aforementioned second movement direction  32  (y=30) that is parallel to the X axis, stops when it reaches a point C 2  on the workpiece side surface C, and detects and holds the X coordinate and the Y coordinate of the point C 2 . 
     Further, the contact sensor  26  advances in the aforementioned third movement direction  34  (x=50) that is parallel to the Y axis, stops when it reaches a point D 1  on the workpiece side surface D corresponding to the workpiece side surface B in the reference position, and detects and holds the X coordinate and the Y coordinate of the point D 1 . In the same way, the contact sensor  26  advances in the aforementioned fourth movement direction  36  (x=40) that is parallel to the Y axis, stops when it reaches a point D 2  on the workpiece side surface D, and detects and holds the X coordinate and the Y coordinate of the point D 2 . 
     The equation of the straight line passing through C 1  and C 2  is given as y=a 1 x+b 1 , and the equation of the straight line passing through D 1  and D 2  is given as y=a 2 x+b 2  (a 1 ≠a 2 ). The intersection of these two straight lines is a point Q 1  to which the reference point P 1  has moved. That is, the reference point P 1  moves to the point Q 1  when the workpiece W placed in the reference position moves to the arbitrary position. The XY coordinates (x 1 , y 1 ) of the point Q 1  can be calculated by the equations below.
 
 x 1=( b   2   −b   1 )/( a   1   −a   2 )
 
 y 1=( a   1   b   2   −a   2   b   1 )/( a   1   −a   2 )
 
     If the XY coordinates of the reference point P 1  are expressed as (x 0 , y 0 ) and the amount of movement from the reference point P 1  to the point Q 1  (x 1 , y 1 ) is expressed as an amount of movement Δx of the X coordinate and an amount of movement Δy of the Y coordinate, then Δx=x 1 −x 0  and Δy=y 1 −y 0 . In this embodiment, x 0 =10 and y 0 =10, and therefore Δx=x 1 −10 and Δy=y 1 −10. 
     If the straight line representing the workpiece side surface B placed in the reference position is given as y=a 0 +b 0 , then a gradient angle Δθ that is formed by the workpiece side surface D after movement and the workpiece side surface B in the reference position can be calculated by the equation below, based on this straight line and the straight line y=a 2 x+b 2  representing the workpiece side surface D placed in the arbitrary position.
 
Δθ=tan −1 (( a   2   −a   0 )/(1+ a   0   a   2 ))
 
     In this embodiment, a 0 =0 and therefore Δθ=tan −1 a 2 . 
     The trajectory data after movement can be obtained by calculation from the trajectory data in the reference position if the amount of movement Δx of the X coordinate of the reference point P 1 , the amount of movement Δy of the Y coordinate of the reference point P 1 , and the gradient angle Δθ have been detected. 
     Referring to  FIGS. 3 to 16 , a specific method for the trajectory control will be described separately in a case where the workpiece W has moved from the reference position without rotating (positional displacement) and a case where the workpiece W has moved from the reference position with rotating (positional displacement). 
     [Specific Example without Rotation] 
     First, referring to  FIGS. 3 to 9 , a specific example will be described in which the workpiece W has moved from a reference position without rotating. It is assumed that the reference position of the workpiece W is the same as that shown in  FIG. 2 . 
     As shown in  FIG. 3 , the control table T 1  stores XY coordinate data about the four points A 1 , A 2 , B 1  and B 2  on the workpiece alignment WP in the reference position, and XY coordinate data about the four points C 1 , C 2 , D 1  and D 2  on the workpiece alignment WQ after having moved. 
     Based on the XY coordinate data in the control table T 1 , the equation of the straight line passing through A 1  and A 2 , the equation of the straight line passing through B 1  and B 2 , the equation of the straight line passing through C 1  and C 2 , and the equation of the straight line passing through D 1  and D 2 , are obtained and stored in a control table T 2  as shown in  FIG. 4 . These four straight lines correspond to the workpiece side surface A, the workpiece side surface B, the workpiece side surface C, and the workpiece side surface D, respectively. 
     In this specific example, the straight line passing through A 1  and A 2  (A straight line) and the straight line passing through C 1  and C 2  (C straight line) are both parallel to the Y axis, and the straight line passing through B 1  and B 2  (B straight line) and the straight line passing through D 1  and D 2  (D straight line) are both parallel to the X axis. The workpiece side surface A (A surface) is stored as x=10.00, the workpiece side surface B (B surface) is stored as y=10.00, the workpiece side surface C (C surface) is stored as x=15.00, and the workpiece side surface D (D surface) is stored as y=17.00. Further, as to these four straight lines, the coefficient “a” and the coefficient “b” in the general expression of straight line y=ax+b are all stored as zero (0.00). 
     Based on the equations of the straight lines in the control table T 2 , the XY coordinates of the intersection of the workpiece side surface A and the workpiece side surface B, and the XY coordinates of the intersection of the workpiece side surface C and the workpiece side surface D, are obtained and stored in a control table T 3  as shown in  FIG. 5 . The data stored in the control table T 3  indicates that the X coordinate and Y coordinate of the intersection of the workpiece side surface A and the workpiece side surface B, i.e., the reference point P 1  (x 0 , y 0 ), are both 10.00. Also, the data indicates that the X coordinate and Y coordinate of the intersection of the workpiece side surface C and the workpiece side surface D, i.e., the point Q 1  (x 1 , y 1 ) to which the reference point P 1  has moved, are 15.00 and 17.00, respectively. 
     The amount of movement Δx of the X coordinate and the amount of movement Δy of the Y coordinate from the reference point P 1  (x 0 , y 0 ) to the point Q 1  (x 1 , y 1 ) are given as Δx=x 1 −x 0 =5.00 and Δy=y 1 −y 0 =7.00, respectively. As shown in  FIG. 5 , the value of the amount of movement Δx of the X coordinate is stored in a control table T 4  as an X-coordinate amount of movement (work_tx) of the workpiece W, and the value of the amount of movement Δy of the Y coordinate is stored in the control table T 4  as a Y-coordinate amount of movement (work_ty) of the workpiece W. 
     Further, if the straight line that represents the workpiece side surface B in the reference position is given as y=a 0 x+b 0  and the straight line that represents the workpiece side surface D after movement is given as y=a 2 x+b 2 , then, as stated earlier, the gradient angle Δθ of the workpiece side surface D with respect to the workpiece side surface B can be calculated by the equation below.
 
Δθ=tan −1 (( a   2   −a   0 )/(1+ a   0   a   2 ))
 
     In this specific example, a 0 =0 and a 2 =0, and therefore Δθ=0, which is, as shown in  FIG. 5 , stored in a control table T 5  as 0.00 radians and 0.00 degrees. 
     If the amount of movement Δx of the X coordinate of the reference point P 1 , the amount of movement Δy of the Y coordinate of the reference point P 1 , and the gradient angle Δθ of the workpiece side surface after movement with respect to the workpiece side surface in the reference position are known, then the XY coordinate data of the trajectory after having moved can be obtained by calculation from the XY coordinate data of the trajectory at the time when the workpiece W is in the reference position. As mentioned earlier, the XY coordinate data of the trajectory at the time when the workpiece W is in the reference position are held in the trajectory control unit  14 . 
     Specifically, the XY coordinates (x′, y′) of the trajectory after movement can be calculated by the following equations by performing a coordinate transformation called affine transformation on the XY coordinates (x, y) of the trajectory in the reference position.
 
 x ′=( p )( x )Cos(Δθ)−( q )( y )Sin(Δθ)+ tx  
 
 y ′=( p )( x )Sin(Δθ)+( q )( y )Cos(Δθ)+ ty  
 
     Now, p=1 and q=1, assuming that the workpiece size is not enlarged or reduced. Further, since the coordinate transformation is performed through rotation about the origin (0, 0) of the XY coordinate system, first, the amount of movement tx in the X direction and the amount of movement ty in the Y direction are set at zero in the affine transformation expressions (tx=0, ty=0), and then an amount of movement aff_tx of the X coordinate and an amount of movement aff_ty of the Y coordinate of the reference point by the affine transformation are calculated. 
     In this specific example, Δθ=0. Accordingly, x=10, y=10 and Δθ=0 are substituted into the following expressions, and then the X coordinate and Y coordinate of the reference point P 1  (10, 10) after movement by the affine transformation are both calculated to be 10.
 
 X  coordinate after movement=( x )Cos(Δθ)−( y )Sin(Δθ)
 
 Y  coordinate after movement=( x )Sin(Δθ)+( y )Cos(Δθ)
 
     The amount of movement aff_tx of the X coordinate and the amount of movement aff_ty of the Y coordinate of the reference point by the affine transformation are obtained as follows (the XY coordinates before movement are the XY coordinates of the reference point P 1 ).
 
aff_ tx=X  coordinate before movement− X  coordinate after movement=10−10=0
 
aff_ ty=Y  coordinate before movement− Y  coordinate after movement=10−10=0
 
As shown in  FIG. 6 , these values are stored in a control table T 6  as 0.00.
 
     Thus, a rotation angle θ=0.00 (degrees),
 
 X -direction amount of movement  tx =aff_ tx +work_ tx= 0.00+5.00=5.00
 
 Y -direction amount of movement  ty =aff_ ty +work_ ty= 0.00+7.00=7.00
 
In this way, the affine transformation (coordinate transformation) conditions for calculating the trajectory data after movement are obtained and stored in a control table T 7  as shown in  FIG. 7 .
 
       FIG. 8  shows the results of the coordinate transformation of the workpiece alignments. In  FIG. 8 , the dotted line shows the workpiece alignment WP in the reference position and the solid line shows the workpiece alignment WQ after movement. 
     Further,  FIG. 9  shows the results of the coordinate transformation of the trajectory data. In  FIG. 9 , the dotted line shows the trajectory in the reference position and the solid line shows the trajectory after movement. 
     [Specific Example with Rotation] 
     Next, referring to  FIGS. 10 to 16 , a specific example will be described in which the workpiece W has moved from a reference position with rotating. It is assumed that the reference position of the workpiece W is the same as that shown in  FIG. 2 . 
     As shown in  FIG. 10 , the control table T 1  stores the XY coordinate data about the four points A 1 , A 2 , B 1  and B 2  on the workpiece alignment WP in the reference position, and the XY coordinate data about the four points C 1 , C 2 , D 1  and D 2  on the workpiece alignment WQ after having moved. 
     Based on the XY coordinate data in the control table T 1 , the equation of the straight line passing through A 1  and A 2 , the equation of the straight line passing through B 1  and B 2 , the equation of the straight line passing through C 1  and C 2 , and the equation of the straight line passing through D 1  and D 2 , are obtained and stored in the control table T 2  as shown in  FIG. 11 . These four straight lines correspond to the workpiece side surface A, the workpiece side surface B, the workpiece side surface C, and the workpiece side surface D, respectively. 
     In this specific example, the straight line passing through A 1  and A 2  (A straight line) is parallel to the Y axis, and the straight line passing through B 1  and B 2  (B straight line) is parallel to the X axis. The workpiece side surface A (A surface) is stored as x=10.00 and the workpiece side surface B (B surface) is stored as y=10.00. Further, as to these two straight lines, the coefficient “a” and the coefficient “b” in the general expression of straight line y=ax+b are all stored as zero (0.00). 
     On the other hand, the straight line passing through C 1  and C 2  (C straight line) and the straight line passing through D 1  and D 2  (D straight line) are neither parallel to the X axis nor to the Y axis. The workpiece side surface C (C surface) is given as y=−3.33x+140, and the value of the coefficient “a” and the value of the coefficient “b” in the general expression of straight line y=ax+b are stored as −3.33 and 140.00, respectively. Further, the workpiece side surface D (D surface) is given as y=0.3x+8 and the value of the coefficient “a” is stored as 0.30 and the value of the coefficient “b” is stored as 8.00. 
     Based on the equations of the straight lines in the control table T 2 , the XY coordinates of the intersection of the workpiece side surface A and the workpiece side surface B, and the XY coordinates of the intersection of the workpiece side surface C and the workpiece side surface D, are obtained and stored in the control table T 3  as shown in  FIG. 12 . The data stored in the control table T 3  indicates that the X coordinate and Y coordinate of the intersection of the workpiece side surface A and the workpiece side surface B, i.e., the reference point P 1  (x 0 , y 0 ), are both 10.00. Also, the data indicates that the X coordinate and Y coordinate of the intersection of the workpiece side surface C and the workpiece side surface D, i.e., the point Q 1  (x 1 , y 1 ) to which the reference point P 1  has moved, are 36.33 and 18.90, respectively. 
     The amount of movement Δx of the X coordinate and the amount of movement Δy of the Y coordinate from the reference point P 1  (x 0 , y 0 ) to the point Q 1  (x 1 , y 1 ) are given as Δx=x 1 −x 0 =26.33 and Δy=y 1 −y 0 =8.90, respectively. As shown in  FIG. 12 , the value of the amount of movement Δx of the X coordinate is stored as work_tx in the control table T 4 , and the value of the amount of movement Δy of the Y coordinate is stored as work_ty in the control table T 4 . 
     Further, if the straight line that represents the workpiece side surface B in the reference position is given as y=a 0 x+b 0  and the straight line that represents the workpiece side surface D after movement is given as y=a 2 x+b 2 , then, as stated earlier, the gradient angle Δθ of the workpiece side surface D with respect to the workpiece side surface B can be calculated by the equation below.
 
Δθ=tan −1 (( a   2   −a   0 )/(1+ a   0   a   2 ))
 
     In this specific example, a 0 =0 and a 2 =0.30, and therefore Δθ=0.29, which is, as shown in  FIG. 12 , stored in the control table T 5  as 0.29 radians and 16.70 degrees. 
     As has been explained above, the XY coordinates (x′, y′) of the trajectory after movement can be calculated by the following equations by performing the affine transformation on the XY coordinates (x, y) of the trajectory in the reference position.
 
 x ′=( p )( x )Cos(Δθ)−( q )( y )Sin(Δθ)+ tx  
 
 y ′=( p )( x )Sin(Δθ)+( q )( y )Cos(Δθ)+ ty  
 
     Now, p=1 and q=1, assuming that the workpiece size is not enlarged or reduced. Further, since the coordinate transformation is performed through rotation about the origin (0, 0) of the XY coordinate system, the amount of movement tx in the X direction and the amount of movement ty in the Y direction are set at zero in the affine transformation expressions (tx=0, ty=0), and then the amount of movement aff_tx of the X coordinate and the amount of movement aff_ty of the Y coordinate of the reference point by the affine transformation are calculated. 
     In this specific example, Δθ=0.29 radians. Accordingly, x=10, y=10 and Δθ=0.29 are substituted into the following expressions, and then the X coordinate and Y coordinate of the reference point P 1  (10, 10) after movement by the affine transformation are calculated to be 6.70 and 12.45, respectively.
 
 X  coordinate after movement=( x )Cos(Δθ)−( y )Sin(Δθ)
 
 Y  coordinate after movement=( x )Sin(Δθ)+( y )Cos(Δθ)
 
     The amount of movement aff_tx of the X coordinate and the amount of movement aff_ty of the Y coordinate of the reference point by the affine transformation are obtained as follows (the XY coordinates before movement are the XY coordinates of the reference point P 1 ).
 
aff_ tx=X  coordinate before movement− X  coordinate after movement=10−6.70=3.30
 
aff_ ty=Y  coordinate before movement− Y  coordinate after movement=10−12.45=−2.45
 
As shown in  FIG. 13 , these values are stored in the control table T 6 .
 
     Seen from a different perspective, the amount of movement of the reference point by the affine transformation is for adjustment of the difference between the XY coordinates after rotation in the case where a coordinate point (x, y) in the XY coordinate system is rotated by Δθ about the origin of the XY coordinate system, and those in the case where it is rotated by Δθ about the reference point, and the amount of movement of the reference point by the affine transformation takes constant values corresponding to the value of Δθ and the values of the XY coordinates of the reference point, irrespective of the position of the coordinate point (x, y). Then, the amount of movement aff_tx of the X coordinate of the reference point by the affine transformation can be obtained by calculating the difference between the X coordinate of the reference point P 1  and the X coordinate of the reference point P 1  rotated by Δθ about the origin of the XY coordinate system. In the same way, the amount of movement aff_ty of the Y coordinate of the reference point by the affine transformation can be obtained by calculating the difference between the Y coordinate of the reference point P 1  and the Y coordinate of the reference point P 1  rotated by Δθ about the origin of the XY coordinate system. 
     Thus, the rotation angle θ=16.70 (degrees),
 
 X -direction amount of movement  tx =aff_ tx +work_ tx= 3.30+26.33=29.63
 
 Y -direction amount of movement  ty =aff_ ty +work_ ty=− 2.45+8.90=6.45
 
In this way, the affine transformation (coordinate transformation) conditions for calculating the trajectory data after movement are obtained and stored in the control table T 7  as shown in  FIG. 14 .
 
       FIG. 15  shows the results of the coordinate transformation of the workpiece alignments. In  FIG. 15 , the dotted line shows the workpiece alignment WP in the reference position and the solid line shows the workpiece alignment WQ after movement. 
     Further,  FIG. 16  shows the results of the coordinate transformation of the trajectory data. In  FIG. 16 , the dotted line shows the trajectory in the reference position and the solid line shows the trajectory after movement. 
     Thus, the trajectory control device  10  of the embodiment can obtain the amount of movement Δx of the X coordinate of the reference point P 1 , the amount of movement Δy of the Y coordinate of the reference point P 1 , and the gradient angle Δθ of a workpiece side surface after movement with respect to the workpiece side surface in the reference position, by means of a simple detection control using the contact sensor  26 , without using a high-precision camera or image sensor, and can correct the trajectory in real time on the basis of these pieces of data. 
     More specifically, if the XY coordinates of two contact points of the contact sensor  26  on the workpiece side surface C and the XY coordinates of two contact points of the contact sensor  26  on the workpiece side surface D are known, then the amount of movement Δx of the X coordinate, the amount of movement Δy of the Y coordinate, and the gradient angle Δθ can be obtained, and then the XY coordinates of the trajectory on the workpiece placed in an arbitrary position can be easily calculated by using the affine transformation. 
     Further, the contact sensor  26  and the trajectory tracking member  28  are both attached to the Z-axis actuator  24   c , and therefore the contact sensor  26  and the trajectory tracking member  28  can be driven by the same mechanism. 
     Furthermore, the X-axis actuator  24   a  and the Y-axis actuator  24   b  are stopped when the contact sensor  26  comes in contact with the workpiece side surface C or workpiece side surface D, which prevents the workpiece W placed in the arbitrary position from moving during the detection control. 
     The trajectory control device of the present invention is not limited to the embodiments described above, but can of course adopt various configurations without departing from the essence and gist of the present invention.