WO2022118374A1 - Method for controlling scara robot - Google Patents
Method for controlling scara robot Download PDFInfo
- Publication number
- WO2022118374A1 WO2022118374A1 PCT/JP2020/044678 JP2020044678W WO2022118374A1 WO 2022118374 A1 WO2022118374 A1 WO 2022118374A1 JP 2020044678 W JP2020044678 W JP 2020044678W WO 2022118374 A1 WO2022118374 A1 WO 2022118374A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- arm
- link
- camera
- axis
- angle
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000012937 correction Methods 0.000 claims abstract description 15
- 230000003287 optical effect Effects 0.000 claims abstract description 12
- 238000006243 chemical reaction Methods 0.000 claims description 14
- 230000009466 transformation Effects 0.000 abstract description 6
- 238000013461 design Methods 0.000 description 13
- 238000005259 measurement Methods 0.000 description 13
- 230000005484 gravity Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 7
- 238000006073 displacement reaction Methods 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 6
- 230000003028 elevating effect Effects 0.000 description 5
- 239000003638 chemical reducing agent Substances 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 3
- 238000009434 installation Methods 0.000 description 2
- 101710168624 Thioredoxin 2 Proteins 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 210000000078 claw Anatomy 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 108060008226 thioredoxin Proteins 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
Definitions
- This specification discloses a control method for a SCARA robot.
- the reference mark provided in the work coordinates is measured by a camera attached to the arm, and the installation error is estimated from the difference between the measurement position of the reference mark by the robot coordinates and the position of the reference mark in the pre-measured work coordinates.
- a robot that controls a robot by correcting this has been proposed (see, for example, Patent Document 1).
- Patent Document 1 describes the measures to be taken when the robot has an installation error, such as a robot mounted on a moving body, there is no mention of the measures to be taken when the camera has a mounting error. do not have.
- the main object of the present disclosure is to provide a control method for a SCARA robot in which a camera is mounted on an arm, which can control the position of the camera with higher accuracy even if a mounting error occurs in the camera. do.
- the method of controlling the SCARA robot disclosed in the present disclosure is as follows. Base and A first arm provided on the base and rotating around a first axis, A second arm provided on the first arm and rotating around a second axis parallel to the first axis, A shaft that moves in the axial direction of the third axis parallel to the second arm with respect to the second arm, A camera attached to the second arm so that the optical axis is parallel to the third axis, Equipped with The first arm, the second arm, and the shaft are regarded as the first link, the second link, and the third link, and the position parameter showing the positional relationship between the links and the posture parameter showing the posture relationship between the links. It is a control method of a SCARA robot that controls the position of the camera by using and.
- the inclination of the optical axis with respect to the third axis is measured as the mounting angle of the camera.
- a correction value based on the measured mounting angle of the camera is registered in the posture parameter for conversion from the second link to the third link.
- the gist is that.
- the control method of the SCARA robot of the present disclosure is that in the SCARA robot in which the camera is attached to the second arm, the first arm, the second arm, and the shaft are set as the first link, the second link, and the third link between the links.
- the position of the camera is controlled by using the position parameter showing the relationship of the position and the posture parameter showing the relationship of the posture between each link.
- the inclination of the optical axis with respect to the third axis is measured as the camera mounting angle, and the correction value based on the measured camera mounting angle is registered in the posture parameter for conversion from the second link to the third link. ..
- FIG. 1 is an external perspective view of a SCARA robot.
- FIG. 2 is a side view of the robot body.
- FIG. 3 is a block diagram showing an electrical connection relationship between the robot body and the control device.
- the SCARA robot 1 is configured to perform predetermined work on the work W (for example, a transport work of picking up and transporting the work W, an assembling work of holding the work W and assembling it to an object to be assembled, etc.). Has been done.
- the SCARA robot 1 includes a robot main body 10 (see FIGS. 1 and 2) and a control device 70 (see FIG. 3) that controls the robot main body 10.
- the robot body 10 includes a base 11, a scalar arm 20, and a camera 60.
- the base 11 is fixed to the workbench 2 and supports the base end side of the scalar arm 20.
- the scalar arm 20 includes a first arm 21, a first arm drive unit 30, a second arm 22, a second arm drive unit 40, a shaft 23, and a shaft drive unit 50.
- the base end of the first arm 21 is connected to the base 11 via the first joint axis J1 and can rotate (horizontally turn) with respect to the base 11 by the rotation of the first joint axis J1. It is composed of.
- the base end of the second arm 22 is connected to the tip of the first arm 21 via the second joint axis J2, and the second arm 22 rotates in a horizontal plane with respect to the first arm 21 due to the rotation of the second joint axis J2.
- the shaft 23 is connected to the tip of the second arm 22 via the third joint axis J3, is rotatable about the axis of the third joint axis J3 with respect to the second arm 22, and is in the axial direction of the third joint axis J3. It is configured to be able to move up and down along.
- a work holding portion 24 for picking up and holding the work W is provided at the tip of the shaft 23. Examples of the work holding portion 24 include a suction nozzle that sucks the work W by a negative pressure, a mechanical chuck that grips the work W by a pair of claws, and an electromagnetic chuck that sucks the work W by an electromagnet. ..
- the first arm drive unit 30 includes a motor 32 and an encoder 34.
- the rotation shaft of the motor 32 is connected to the first joint shaft J1 via a speed reducer (not shown).
- the first arm drive unit 30 rotates the first arm 21 with the first joint shaft J1 as a fulcrum by the torque transmitted to the first joint shaft J1 via the speed reducer by driving the motor 32.
- the encoder 34 is attached to the rotation shaft of the motor 32 and is configured as a rotary encoder that detects the amount of rotational displacement of the motor 32.
- the second arm drive unit 40 includes a motor 42 and an encoder 44, similarly to the first arm drive unit 30.
- the rotation shaft of the motor 42 is connected to the second joint shaft J2 via a speed reducer (not shown).
- the second arm drive unit 40 rotates the second arm 22 with the second joint shaft J2 as a fulcrum by the torque transmitted to the second joint shaft J2 via the speed reducer by driving the motor 42.
- the encoder 44 is attached to the rotation shaft of the motor 42 and is configured as a rotary encoder that detects the amount of rotational displacement of the motor 42.
- the shaft drive unit 50 includes motors 52a and 52b and encoders 54a and 54b.
- the rotation shaft of the motor 52a is connected to the shaft 23 via a belt (not shown).
- the shaft drive unit 50 rotates the shaft 23 around an axis by driving the motor 52a.
- the rotation shaft of the motor 52b is connected to the shaft 23 via a ball screw mechanism (not shown).
- By driving the motor 52b the shaft drive unit 50 raises and lowers the shaft 23 up and down by converting the rotational motion of the motor 52b into a linear motion by the ball screw mechanism.
- the encoder 54a is configured as a rotary encoder that detects the amount of rotational displacement of the shaft 23.
- the encoder 54b is configured as a linear encoder that detects the elevating position of the shaft 23.
- the camera 60 is attached to the side surface of the tip of the second arm 22.
- the camera 60 captures an image of the work W, which is a work object, and outputs the captured image to the control device 70.
- the control device 70 can recognize the position and posture of the work W by processing the captured image. Since the camera 60 is attached to the second arm 22, in the present embodiment, the scalar arm 20 cannot move the camera 60 in the vertical direction.
- the control device 70 includes a CPU 71, a ROM 72 for storing a processing program, a RAM 73 as a work memory, a storage device 74 such as an HDD or SSD, and an input / output interface (not shown). Be prepared. Position signals from the encoders 34, 44, 54a, 54b, image signals from the camera 60, and the like are input to the control device 70 via the input / output interface. From the control device 70, drive signals to the motors 32, 42, 52a, 52b and the like are output via the input / output interface.
- FIG. 4 is a flowchart showing an example of the camera position control process executed by the CPU 71 of the control device 70.
- the CPU 71 of the control device 70 first sees the camera 60 as seen from the base 11 (base coordinate system ⁇ 0 described later) for imaging the work W supplied from the feeder (not shown).
- the target position (X 0 *, Y 0 *, Z 0 *) at the center of the image is set (step S100).
- Step S100 is performed, for example, by reading the data of a program created by an external computer.
- the CPU 71 sets the image center of the camera 60 to the target position (X 0 *) by solving the inverse kinematics with respect to the target position (X 0 *, Y 0 *, Z 0 *) of the image center of the camera 60.
- Y 0 *, Z 0 *) the angle ⁇ J1 of the first joint axis J1 and the angle ⁇ J2 of the second joint axis J2 are calculated (step S110).
- the CPU 71 calculates the twist angle ⁇ t and the deflection angle ⁇ d1 of the first arm 21 based on the angle ⁇ J2 of the second joint axis J2 calculated in step S110 (step S120), and the second arm.
- the deflection angle ⁇ d2 of 22 is calculated (step S130).
- the base coordinate system ⁇ 0 is set on the base 11 (0th link), and the first arm 21, the second arm 22, the shaft 23, and the hand (work holding portion 24) are set.
- the link coordinate system is set as the first link, the second link, the third link, and the fourth link, respectively.
- the link coordinate system ⁇ 1 is set at the base of the first arm 21 (first joint axis J1).
- the link coordinate system ⁇ 2 is set at the base of the second arm 22 (second joint axis J2).
- the link coordinate system ⁇ 3 is set at the connecting portion between the second arm 22 and the shaft 23.
- the link coordinate system (hand coordinate system) ⁇ 4 is set in the hand (work holding unit 24).
- the home position is a state in which the first arm 21 and the second arm 22 extend in a straight line.
- X 0 , X 1 , X 2 , X 3 , and X 4 are the X axes of the respective coordinate systems.
- Y 0 , Y 1 , Y 2 , Y 3 , and Y 4 are the Y axes of the respective coordinate systems.
- Z 0 , Z 1 , Z 2 , Z 3 , and Z 4 are the Z axes of their respective coordinate systems.
- the twist angle ⁇ t of the first arm 21 is such that the axial direction of the first joint axis J 1 is the Z 1 axis direction and the direction along the center line of the first arm 21 is the X 1 axis direction. Assuming that the direction orthogonal to the X 1 axis and the Z 1 axis is the Y 1 axis direction, it is defined as the displacement angle of the first arm 21 around the X 1 axis as shown in FIG.
- the twist angle ⁇ t of the first arm 21 represents the relationship between the twist angle ⁇ t of the first arm 21, the angle ⁇ J2 of the second joint axis J2, and the arm mass M1, the following equations (1) and (2).
- the arm mass M1 is the mass of the arm portion on the tip side of the first joint axis J1 of the scalar arm 20, and when the work holding portion 24 holds the work W, the arm mass M includes the mass m of the work W and the work. It is a variable according to the mass m of W. "G” is the gravitational acceleration. As shown in FIG. 8, “L t ⁇ sin ⁇ J2 ” is a component in the Y1 axis direction at the position L t of the center of gravity of the arm portion. As shown in FIG.
- It is a moment (twisting moment ) that attempts to rotate the first arm 21 around the X1 axis due to the load due to gravity acting on the arm portion, and the center of gravity position L t is The value is 0 when it is on the X 1 axis, and increases as the center of gravity L t moves away from the X 1 axis in the Y 1 axis direction. That is, the twisting moment It is the minimum when the angle ⁇ J2 of the second joint axis J2 is 0 degrees (reference angle), and is maximum when the angle of the second joint axis J2 is ⁇ 90 degrees and +90 degrees. It becomes. "K t " has torsional rigidity and is obtained in advance by experiments or the like.
- the deflection angle ⁇ d1 of the first arm 21 is defined as the displacement angle of the first arm 21 around the Y 1 axis, as shown in FIG.
- the deflection angle ⁇ d1 of the first arm 21 represents the relationship between the deflection angle ⁇ d1 of the first arm 21, the angle ⁇ J2 of the second joint axis J2, and the above-mentioned arm mass M1 which is the mass of the arm portion. It is calculated using the deflection model equations of the equations (3) and (4).
- “L d1 ⁇ cos ⁇ J2 ” is an X1 axial component at the center of gravity position Lt of the arm portion, as shown in FIG. As shown in FIG.
- I d1 is a moment (deflection moment) that attempts to rotate the first arm 21 around the Y 1 axis due to the load due to gravity acting on the arm portion, and the center of gravity position L d1 is set. It increases as the distance from the first joint axis J1 increases in the X1 axis direction. That is, the deflection moment I d1 is the minimum when the angle of the second joint axis J2 is ⁇ 180 degrees, and is maximum when the angle of the second joint axis J2 is 0 degrees (reference angle). "K d1 " has a deflection rigidity and is obtained in advance by experiments or the like.
- the deflection angle ⁇ d2 of the second arm 22 is defined as the displacement angle of the second arm 22 around the Y 2 axis, as shown in FIG.
- the deflection angle ⁇ d2 of the second arm 22 is calculated using the deflection model equations (5) and (6) below, which express the relationship between the deflection angle ⁇ d2 of the second arm 22 and the arm mass M2. .
- the arm mass M2 is the mass of the arm portion on the tip side of the second joint axis J2 of the scalar arm 20, and when the work holding portion 24 holds the work W, the mass m of the work W is also It is included and becomes a variable according to the mass m of the work W.
- I d2 is a moment (deflection moment) that tries to rotate the second arm 22 around the Y 2 axis by the load due to the gravity acting on the arm portion, and is irrelevant to the angle ⁇ d2 of the second joint axis J2. It is determined by the center of gravity position L d2 (distance from the center of the second joint axis J2 to the center of gravity position) and the arm mass M2.
- K d2 has a deflection rigidity and is obtained in advance by experiments or the like.
- twist model formulas and deflection model formulas are stored in the storage device 74.
- the CPU 71 reads the corresponding model equation from the storage device 74 to calculate the twist angle ⁇ t and the deflection angles ⁇ d1 and ⁇ d2 .
- the CPU 71 has an angle ⁇ J1 of the first joint axis J1, an angle ⁇ J2 of the second joint axis J2, a twist angle ⁇ t of the first arm 21, a deflection angle ⁇ d1 , and a deflection angle ⁇ d2 of the second arm 22.
- the estimated position (X 0 , Y 0 , Z 0 ) of the image center of the camera 60 is calculated (step S140).
- the CPU 71 has an angle ⁇ J1 of the first and second joint axes J1 and J2 calculated by inverse kinematics so that the center of the image coincides with the target position (X 0 *, Y 0 *, Z 0 *).
- ⁇ J2 is controlled as an angle command value, from the target position (X 0 *, Y 0 *, Z 0 *) due to the twist and deflection of the first arm 21 and the deflection of the second arm 22.
- the estimated position (X 0 , Y 0 , Z 0 ) of the center of the image to be displaced is calculated.
- the CPU 71 calculates the estimated position of the image center (X 0 , Y 0 , Z 0 ), the calculated estimated position of the image center (X 0 , Y 0 , Z 0 ) and the target position of the image center acquired in step S100.
- the amount of positional deviation ( ⁇ X 0 , ⁇ Y 0 , ⁇ Z 0 ) at the center of the image is calculated (step S150).
- the CPU 71 corrects the target position by offsetting the target position (X 0 *, Y 0 *, Z 0 *) at the center of the image by the amount of the positional deviation ( ⁇ X 0 , ⁇ Y 0 , ⁇ Z 0 ).
- the angle command value ⁇ J1 * of the second joint axis J2 and the angle command value ⁇ J2 * of the second joint axis J2 are calculated (step S170).
- the CPU 71 matches the angle command value ⁇ J1 * calculated by the angle of the first joint axis J1 detected by the encoder 34, and the angle command value calculated by the angle of the second joint axis J2 detected by the encoder 44.
- the corresponding motors 31 and 32 are controlled by feedback control so as to match ⁇ J2 * (step S180), and the camera position control process is terminated.
- the CPU 71 takes an image of the work W supplied from a feeder (not shown) by the camera 60, and performs image processing on the captured image to recognize the position and posture of the work W. Then, the CPU 71 sets a target position (X 0 *, Y 0 *, Z 0 *) of the hand (work holding unit 24) for picking up the work W, and sets the hand to the target position (X 0 *, Y 0 ). *, Z 0 *) is executed to perform the hand position control process for moving to.
- the target position (X 0 *, Y 0 *) is set based on the recognized position and posture of the work W.
- the target position Z 0 * is set based on the height information of the work W (work height WH).
- the height information of the work W is registered in advance as one of the image processing data used for the above-mentioned image processing. Therefore, the height information can be acquired by reading the work height WH from the image processing data. For example, when the work W is picked up by using the suction nozzle or the electromagnetic chuck as the work holding portion 24, the pick position of the work W is the upper surface (suction surface) of the work W, so the target position Z 0 * is shown in FIG. As shown, the work height WH is set higher than the reference surface S (fixed value) on which the work W is placed.
- the pick position of the work W is lower than the upper surface of the work W.
- the target position Z 0 * of the hand is set separately using the input pick position.
- the hand position control process is basically performed by the same process as the camera position control process described above, except that the shaft 23 is moved up and down by the shaft drive unit 50. That is, in the hand position control process, the CPU 71 first has an angle ⁇ J1 of the first joint axis J1 and a second joint for moving the hand to the set target position (X 0 *, Y 0 *, Z 0 *).
- the angle ⁇ J2 of the axis J2 and the angle (shaft angle) ⁇ J2 of the shaft 23 and the elevating position Z s of the shaft 23 are calculated by inverse kinematics.
- the CPU 71 twists and twists based on the calculated ⁇ J1 , ⁇ J2 , ⁇ J4 , Z s , the twist angle ⁇ t of the first arm 21, the deflection angle ⁇ d1 , and the twist angle ⁇ d 2 of the second arm 22.
- the estimated position of the hand (X 0 , Y 0 , Z 0 ) considering the deflection is calculated by forward kinematics.
- the CPU 71 takes a difference between the estimated position of the hand (X 0 , Y 0 , Z 0 ) and the target position (X 0 *, Y 0 *, Z 0 *) to obtain a position shift amount ( ⁇ X 0 , ⁇ Y 0 , ⁇ Z 0 ) is calculated.
- the CPU 71 corrects the target position by offsetting the target position (X 0 *, Y 0 *, Z 0 *) by the amount of the positional deviation ( ⁇ X 0 , ⁇ Y 0 , ⁇ Z 0 ).
- the CPU 71 has an angle command value ⁇ J1 * of the first joint axis J1 for moving the hand to the corrected target position, an angle command value ⁇ J2 * of the second joint axis J2, and an angle command value ⁇ J4 of the shaft 23.
- the elevating position command value Z s * of the shaft 23 are calculated by inverse kinematics, and the corresponding motors 31, 32, 33a, 33b are controlled by feedback control based on each command value.
- the estimated position of the image center and the hand (work holding portion 24) of the camera 60 in consideration of the twist and deflection of the first arm 21 and the deflection of the second arm 22 is obtained.
- the forward kinematics calculation for this can be done by giving the variables ⁇ J1 , ⁇ J2 , ⁇ t , ⁇ d1 , and ⁇ d 2 to the homologous transformation matrix.
- FIG. 14 is an explanatory diagram showing an example of a hand link parameter table.
- FIG. 15 is an explanatory diagram showing an example of a camera link parameter table.
- dx, dy, and dz are parameters (positional parameters) indicating translational amounts in the X-axis direction, the Y-axis direction, and the Z-axis direction.
- rx (rx2), ry (ry2), and rz are parameters (posture parameters) indicating the amount of rotation around the X-axis, around the Y-axis, and around the Z-axis.
- L 1 indicates the distance between the Z 1 axis and the Z 2 axis in the X 1 axis direction (arm length of the first arm 21).
- L 2 indicates the distance between the Z 2 axis and the Z 3 axis in the X 2 axis direction (arm length of the second arm 22).
- L z 1 indicates the distance in the Z 0 axis direction from the origin of the base coordinate system ⁇ 0 to the origin of the link coordinate system ⁇ 1 .
- L z 2 indicates the distance in the Z 1 axis direction from the origin of the link coordinate system ⁇ 1 to the origin of the link coordinate system ⁇ 2 .
- th1 is the rotation angle of the link coordinate system ⁇ 1 around the Z 1 axis, and corresponds to the angle ⁇ J1 of the first joint axis J1.
- th2 is the angle of rotation around the Z2 axis of the link coordinate system ⁇ 2, and corresponds to the angle ⁇ J2 of the second joint axis J2.
- th4 is the rotation angle around the Z4 axis of the link coordinate system ⁇ 4 , and corresponds to the angle ( shaft angle) ⁇ J4 of the shaft 23.
- d3 is the distance from the origin of the link coordinate system ⁇ 3 to the hand, and corresponds to the elevating position Z s .
- J1rx indicates the rotation angle of the link coordinate system ⁇ 1 around the X 1 axis.
- J1ry indicates the rotation angle of the link coordinate system ⁇ 1 around the Y 1 axis.
- J1rx2 indicates the rotation angle around the X 1 axis after the link coordinate system ⁇ 1 is rotated about the Z 1 axis by th1 and corresponds to the twist angle ⁇ t of the first arm 21.
- J1ry2 indicates the rotation angle around the Y 1 axis after the link coordinate system ⁇ 1 is rotated about the Z 1 axis by th1 and corresponds to the deflection angle ⁇ d1 of the first arm 21.
- J2rx indicates the angle of rotation of the link coordinate system ⁇ 2 around the X 2 axis.
- J2ry indicates the rotation angle of the link coordinate system ⁇ 2 around the Y 2 axis.
- J2ry2 indicates the rotation angle around the Y 2 axis after the link coordinate system ⁇ 2 rotates th2 about the Z 2 axis, and corresponds to the deflection angle ⁇ d2 of the second arm 22.
- J3rx indicates the angle of rotation of the link coordinate system ⁇ 3 around the X3 axis.
- J3ry indicates the rotation angle of the link coordinate system ⁇ 3 around the Y 3 axis.
- the relationship (matrix) i -1 Ti between each coordinate system can be obtained by the following equation (8).
- the matrices T dx , T dy , T dz , Trx, Tri, Trz, Trx2, and Tri2 of the dx, dy, dz, rx , ry , rz , rx2 , and ry2 are given by the following equations (9) to (16).
- the twist angle ⁇ t of the first arm 21 is given to J1rx2 in the hand link parameter table
- the deflection angle ⁇ d1 of the first arm 21 is given to J1ry2
- the deflection angle of the second arm 22 is given to J2ry2.
- the link parameters for the camera are the parameters dx, dy, dz, rx, ry for conversion from the link coordinate system ⁇ 2 to the link coordinate system ⁇ 3 , and L 2 + HCX, HCY, 0, HCRA, It differs from the hand link parameter in that the HCRB is set and the HCZ and 0 are set in the dz and rz of the conversion parameters from the link coordinate system ⁇ 3 to the hand coordinate system ⁇ 4 , respectively.
- HCX indicates the position of the camera 60 in the X 2 -axis direction
- HCY indicates the position of the camera 60 in the Y 2 -axis direction.
- HCRA indicates the inclination of the camera 60 around the X3 axis
- HCRB indicates the inclination of the camera 60 around the Y3 axis.
- the HCZ indicates the distance (working distance WD) from the camera 60 (the tip of the lens) to the upper surface of the work W which is the subject.
- the twist angle ⁇ t of the first arm 21 is given to J1rx2 in the link parameter table for the camera, the deflection angle ⁇ d1 of the first arm 21 is given to J1ry2, and the deflection angle ⁇ of the second arm 22 is given to J2ry2.
- d2 it is possible to calculate the estimated position of the image center of the camera 60 including the error caused by the twist and deflection of the first arm 21 and the deflection of the second arm 22 by forward kinematics.
- the position X in the X 2 -axis direction of the camera 60 and the position Y in the Y 2 -axis direction are measured in advance, the position X is given to the HCX in the link parameter table for the camera, and the position Y is given to the HCY.
- Kinematics can be used to calculate the estimated position of the image center of the camera 60, including errors due to the mounting position of the camera 60.
- the tilt RA around the X3 axis and the tilt RB around the Y3 axis of the camera 60 are measured in advance, the tilt RA is given to the HCRA in the link parameter table for the camera, and the tilt RB is given to the HCRB.
- the kinematics can calculate the estimated position of the image center of the camera 60 including the error due to the mounting angle of the camera 60.
- the working distance WD given to the HCZ in the link parameter table for the camera is the work height described above from the distance (fixed value) between the camera 60 (lens tip) and the reference surface S on which the work W is placed. It is calculated by subtracting the WH.
- the difference in height is obtained.
- a phenomenon occurs in which the image center of the captured image of each work W shifts according to ⁇ WH.
- by acquiring the work height WH from the image processing data and reflecting it in the HCZ it is possible to correct the deviation of the image center due to the difference in the height of the work W.
- FIG. 17 is an explanatory diagram showing an example of a camera link parameter setting procedure.
- steps S200 to S240 are executed.
- step S200 the measuring jig 100 is attached to the hand (work holding portion 24).
- FIG. 18 is a partial external view showing a state in which the measuring jig is attached to the hand (work holding portion 24).
- the measuring jigs 100 are spaced apart by a predetermined distance in the direction orthogonal to the shaft 23 with the mounting location as the center, and are arranged at predetermined intervals (intervals of 90 degrees) in the circumferential direction (4). It has the mark MA of one).
- the measuring jig 100 is attached to the shaft 23 in a phase-aligned state, and each mark MA has the image center of the camera 60 as the center of the mark MA when the camera 60 is attached according to the design value. It is arranged to match with.
- step S210 the positions X and Y of the camera 60 are measured using the measuring jig 100.
- This step S210 is performed by executing the camera position measurement process illustrated in FIG. 19 by the CPU 71 of the control device 70.
- the CPU 71 first moves the hand (work holding unit 24) to the measurement position (step S300).
- This process is performed by controlling the motor 52b by setting the elevating position command value Z s * within the depth of field of the camera 60 by the mark MA.
- the CPU 71 controls the motor 52a so that the shaft 23 rotates by a predetermined pitch, and repeats the process of capturing the mark MA by the camera 60 each time the shaft 23 rotates by a predetermined pitch (step S310).
- FIG. 20 is an explanatory diagram showing a state of measurement of the camera position.
- white circles indicate mark images that appear in the captured image when the mounting position of the camera 60 is as designed, and black circles indicate marks that appear in the captured image by actually capturing the mark MA with the camera 60.
- the image is shown.
- the position deviation amount ⁇ X is the difference in position in the X-axis direction between the arc at the design position and the arc at the measured position.
- the position deviation amount ⁇ Y is the difference in position in the Y-axis direction between the arc at the design position and the arc at the measured position. Then, the CPU 71 calculates the actual camera 60 positions X and Y by adding the position deviation amounts ⁇ X and ⁇ Y calculated in step S130 to the design positions Xdv and Ydv of the camera 60 (step S330).
- step S220 the tilt RA and RB of the camera 60 are measured by using the measuring jig 100.
- This step S220 is performed by executing the camera tilt measurement process illustrated in FIG. 21 by the CPU 71 of the control device 70.
- the CPU 71 first moves the mark MA to the front (directly below) of the camera 60 (step S400). This process is performed by controlling the motor 52a so that the shaft angle ⁇ J4 becomes a predetermined predetermined angle. Subsequently, the CPU 71 raises and lowers the mark MA by a predetermined amount b within the depth of field of the camera 60, and images the mark MA at the lower position and the mark MA at the upper position by the camera 60 (step S410). ..
- the CPU 71 processes the captured image to measure the amount of positional deviation ⁇ Xa, ⁇ Ya in the XY-axis direction between the actually measured position of the mark MA imaged at the lower position and the design position, and the actual measurement of the mark MA captured at the upper position.
- the amount of positional deviation between the position and the design position in the XY axis directions ⁇ Xc and ⁇ Yc are calculated (step S420).
- FIG. 22 is an explanatory diagram showing a state of measuring the tilt of the camera. When the mounting angle of the camera 60 is as designed, the center of the mark image captured in the captured image coincides with the center of the image of the camera 60.
- the position deviation amount ⁇ Ya is the difference between the center position (measured position) of the mark image captured in the captured image when the mark MA is imaged at the lower position and the design position in the Y-axis direction.
- the position deviation amount ⁇ Yc is the difference between the center position (measured position) of the mark image captured in the captured image and the design position in the Y-axis direction when the mark MA is imaged at the upper position.
- the position deviation amount ⁇ Xa is the difference between the center position (measured position) of the mark image captured in the captured image when the mark MA is imaged at the lower position and the design position in the X-axis direction.
- the position deviation amount ⁇ Xc is the difference between the center position (measured position) of the mark image captured in the captured image and the design position in the Y-axis direction when the mark MA is imaged at the upper position.
- the CPU 71 calculates the tilt RA of the camera 60 around the X axis by the following equation (17) based on the calculated positional deviation amounts ⁇ Ya and ⁇ Yb and the predetermined quantity b described above, and the calculated positional deviation amount ⁇ Xa
- the inclination RB of the camera 60 around the Y axis is calculated by the following equation (18) based on ⁇ Xb and the predetermined amount b (step S430).
- the equation (17) can be easily derived from FIG. 22. The same applies to equation (18).
- step 230 the calculated position X is set to the parameter HCX, the position Y is set to the parameter HCY, the inclination RA is set to the parameter HCRA, and the inclination RB is set to the parameter HCRB.
- step S240 the set link parameters HCX, HCY, HCRA, and HCRB are registered as camera link parameters.
- the estimated position of the image center of the camera 60 including an error due to the mounting position and mounting angle of the camera 60 is obtained by forward kinematics in the camera position control process. Can be calculated. By correcting and controlling the target position based on this estimated position, it is possible to accurately align the image center of the camera 60 with the target position.
- the first arm 21 corresponds to the first arm
- the first joint axis J1 corresponds to the first axis
- the second arm 22 corresponds to the second arm
- the second joint axis J2 corresponds to the first arm.
- the shaft 23 corresponds to the shaft
- the third joint axis J3 corresponds to the third axis
- the camera 60 corresponds to the camera.
- the camera positions X and Y and the camera tilts RA and RB are measured and reflected in the camera link parameters HCX, HCY, HCRA and HCRB, respectively.
- the measurement of either the camera positions X and Y and the camera tilt RA and RB may be omitted.
- the control method of the scalar robot of the present disclosure is provided on the base, the first arm provided on the base and rotating around the first axis, and the first arm provided on the first arm.
- a second arm that rotates around a second axis that is parallel to one axis, a shaft that moves in the axial direction of the third axis that is parallel to the second axis with respect to the second arm, and an optical axis that is the third.
- a camera attached to the second arm so as to be parallel to the axis is provided, and the first arm, the second arm, and the shaft are used as the first link, the second link, and the third link between the links.
- the gist is to measure the tilt as the mounting angle of the camera and register the correction value based on the measured mounting angle of the camera in the posture parameter for conversion from the second link to the third link.
- the mounting angle of the camera is measured and the correction value based on the measured value is reflected in the posture parameter for conversion to the corresponding link. Even if there is an error in the camera, it is possible to control the position of the camera with high accuracy.
- the position of the optical axis in the direction orthogonal to the third axis is measured as the mounting position of the camera, and the correction value based on the measured mounting position of the camera is the second. It may be registered in the position parameter for conversion from the link to the third link. By doing so, even if there is an error in the mounting position in addition to the mounting angle of the camera, the position control of the camera can be performed accurately.
- the relationship between the rotation position of the second arm and the twist amount around the arm of the first arm is memorized, and the rotation position of the second arm corresponds to the above-mentioned.
- the correction value based on the twist amount of the first arm may be registered in the posture parameter for conversion from the 0th link to the 1st link with the base as the 0th link. By doing so, even if the first arm is twisted, the position of the camera can be controlled accurately.
- the deflection amount of the first arm and the deflection amount of the second arm corresponding to the rotation position of the second arm are stored, and the rotation of the second arm is stored.
- the correction value based on the amount of deflection of the first arm corresponding to the position is registered in the posture parameter for conversion from the 0th link to the 1st link with the base as the 0th link, and the first A correction value based on the amount of deflection of the second arm corresponding to the rotation position of the two arms may be registered in the posture parameter for conversion from the first link to the second link.
- This disclosure can be used in the SCARA robot manufacturing industry and the like.
- 1 SCARA robot system 2 work table, 10 robot body, 11 base, 20 scalar arm, 21 1st arm, 22 2nd arm, 23 shaft, 24 work holding unit, 30 1st arm drive unit, 32 motor, 34 Encoder, 40 2nd arm drive unit, 42 motor, 44 encoder, 50 shaft drive unit, 52a, 52b motor, 54a, 54b encoder, 60 camera, 70 control device, 71 CPU, 72 ROM, 73 RAM, 74 storage device, 100 Measuring jig, J1 1st joint axis, J2 2nd joint axis, J3 3rd joint axis, MA mark, W work.
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Manipulator (AREA)
Abstract
The method for controlling a SCARA robot having a camera attached to a second arm, involves controlling the position of the camera using position parameters that represent positional relationships between links, and orientation parameters that represent orientation relationships between the links with a first arm, a second arm, and a shaft serving as a first link, a second link, and a third link, respectively. In the method, the inclination of an optical axis with respect to a third axis is measured as a camera attachment angle, and a correction value, which is based on the measured camera attachment angle, is registered as an orientation parameter for transformation from the second link to the third link.
Description
本明細書は、スカラロボットの制御方法について開示する。
This specification discloses a control method for a SCARA robot.
従来、作業座標に設けられた基準マークをアームに取り付けられたカメラにより計測し、ロボット座標による基準マークの計測位置と予め測定された作業座標における基準マークの位置との差から設置誤差を推定し、これを補正することによりロボットを制御するものが提案されている(例えば、特許文献1参照)。
Conventionally, the reference mark provided in the work coordinates is measured by a camera attached to the arm, and the installation error is estimated from the difference between the measurement position of the reference mark by the robot coordinates and the position of the reference mark in the pre-measured work coordinates. , A robot that controls a robot by correcting this has been proposed (see, for example, Patent Document 1).
上述した特許文献1には、移動体に搭載されたロボットのように、ロボットに設置誤差がある場合の対応ついては記載されているものの、カメラに取付誤差がある場合の対応については何ら言及されていない。
Although the above-mentioned Patent Document 1 describes the measures to be taken when the robot has an installation error, such as a robot mounted on a moving body, there is no mention of the measures to be taken when the camera has a mounting error. do not have.
本開示は、アームにカメラが取り付けられたスカラロボットにおいて、カメラに取付誤差が生じていてもより高い精度でカメラの位置を制御することができるスカラロボットの制御方法を提供することを主目的とする。
The main object of the present disclosure is to provide a control method for a SCARA robot in which a camera is mounted on an arm, which can control the position of the camera with higher accuracy even if a mounting error occurs in the camera. do.
本開示は、上述の主目的を達成するために以下の手段を採った。
This disclosure has taken the following steps to achieve the above-mentioned main objectives.
本開示のスカラロボットの制御方法は、
基台と、
前記基台に設けられ、第1軸周りに回動する第1アームと、
前記第1アームに設けられ、前記第1軸と平行な第2軸周りに回動する第2アームと、
前記第2アームに対して前記第2軸と平行な第3軸の軸方向に移動するシャフトと、
光軸が前記第3軸と平行となるように前記第2アームに取り付けられたカメラと、
を備え、
前記第1アーム,前記第2アーム,前記シャフトを第1リンク,第2リンク,第3リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
前記第3軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
前記測定したカメラの取付角度に基づく補正値を前記第2リンクから前記第3リンクへの変換のための前記姿勢パラメータに登録する、
ことを要旨とする。 The method of controlling the SCARA robot disclosed in the present disclosure is as follows.
Base and
A first arm provided on the base and rotating around a first axis,
A second arm provided on the first arm and rotating around a second axis parallel to the first axis,
A shaft that moves in the axial direction of the third axis parallel to the second arm with respect to the second arm,
A camera attached to the second arm so that the optical axis is parallel to the third axis,
Equipped with
The first arm, the second arm, and the shaft are regarded as the first link, the second link, and the third link, and the position parameter showing the positional relationship between the links and the posture parameter showing the posture relationship between the links. It is a control method of a SCARA robot that controls the position of the camera by using and.
The inclination of the optical axis with respect to the third axis is measured as the mounting angle of the camera.
A correction value based on the measured mounting angle of the camera is registered in the posture parameter for conversion from the second link to the third link.
The gist is that.
基台と、
前記基台に設けられ、第1軸周りに回動する第1アームと、
前記第1アームに設けられ、前記第1軸と平行な第2軸周りに回動する第2アームと、
前記第2アームに対して前記第2軸と平行な第3軸の軸方向に移動するシャフトと、
光軸が前記第3軸と平行となるように前記第2アームに取り付けられたカメラと、
を備え、
前記第1アーム,前記第2アーム,前記シャフトを第1リンク,第2リンク,第3リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
前記第3軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
前記測定したカメラの取付角度に基づく補正値を前記第2リンクから前記第3リンクへの変換のための前記姿勢パラメータに登録する、
ことを要旨とする。 The method of controlling the SCARA robot disclosed in the present disclosure is as follows.
Base and
A first arm provided on the base and rotating around a first axis,
A second arm provided on the first arm and rotating around a second axis parallel to the first axis,
A shaft that moves in the axial direction of the third axis parallel to the second arm with respect to the second arm,
A camera attached to the second arm so that the optical axis is parallel to the third axis,
Equipped with
The first arm, the second arm, and the shaft are regarded as the first link, the second link, and the third link, and the position parameter showing the positional relationship between the links and the posture parameter showing the posture relationship between the links. It is a control method of a SCARA robot that controls the position of the camera by using and.
The inclination of the optical axis with respect to the third axis is measured as the mounting angle of the camera.
A correction value based on the measured mounting angle of the camera is registered in the posture parameter for conversion from the second link to the third link.
The gist is that.
この本開示のスカラロボットの制御方法は、第2アームにカメラが取り付けられたスカラロボットにおいて、第1アーム,第2アーム,シャフトを第1リンク,第2リンク,第3リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いてカメラの位置を制御するものである。この方法は、第3軸に対する光軸の傾きをカメラの取付角度として測定し、測定したカメラの取付角度に基づく補正値を第2リンクから第3リンクへの変換のための姿勢パラメータに登録する。このように、カメラの取付角度を測定して、測定値に基づく補正値を対応するリンクへの変換のための姿勢パラメータに反映させることで、カメラに取付角度に誤差が生じていても、カメラの位置制御を精度よく行なうことが可能となる。
The control method of the SCARA robot of the present disclosure is that in the SCARA robot in which the camera is attached to the second arm, the first arm, the second arm, and the shaft are set as the first link, the second link, and the third link between the links. The position of the camera is controlled by using the position parameter showing the relationship of the position and the posture parameter showing the relationship of the posture between each link. In this method, the inclination of the optical axis with respect to the third axis is measured as the camera mounting angle, and the correction value based on the measured camera mounting angle is registered in the posture parameter for conversion from the second link to the third link. .. In this way, by measuring the mounting angle of the camera and reflecting the correction value based on the measured value in the posture parameter for conversion to the corresponding link, even if there is an error in the mounting angle of the camera, the camera It is possible to control the position of the camera with high accuracy.
次に、本開示を実施するための形態について図面を参照しながら説明する。
Next, the mode for carrying out the present disclosure will be described with reference to the drawings.
図1は、スカラロボットの外観斜視図である。図2は、ロボット本体の側面図である。図3は、ロボット本体と制御装置との電気的な接続関係を示すブロック図である。
FIG. 1 is an external perspective view of a SCARA robot. FIG. 2 is a side view of the robot body. FIG. 3 is a block diagram showing an electrical connection relationship between the robot body and the control device.
スカラロボット1は、ワークWに対して所定の作業(例えば、ワークWをピックアップして搬送する搬送作業や、ワークWを保持して組付対象物に組み付ける組付け作業など)を行なうものとして構成されている。スカラロボット1は、ロボット本体10(図1および図2参照)と、ロボット本体10を制御する制御装置70(図3参照)と、を備える。
The SCARA robot 1 is configured to perform predetermined work on the work W (for example, a transport work of picking up and transporting the work W, an assembling work of holding the work W and assembling it to an object to be assembled, etc.). Has been done. The SCARA robot 1 includes a robot main body 10 (see FIGS. 1 and 2) and a control device 70 (see FIG. 3) that controls the robot main body 10.
ロボット本体10は、基台11とスカラアーム20とカメラ60とを備える。
The robot body 10 includes a base 11, a scalar arm 20, and a camera 60.
基台11は、作業台2に固定されており、スカラアーム20の基端側を支持する。スカラアーム20は、第1アーム21と第1アーム駆動部30と第2アーム22と第2アーム駆動部40とシャフト23とシャフト駆動部50とを備える。第1アーム21は、基端部が第1関節軸J1を介して基台11に連結され、第1関節軸J1の回動により基台11に対して水平面内で回動(水平旋回)可能に構成される。第2アーム22は、基端部が第2関節軸J2を介して第1アーム21の先端部に連結され、第2関節軸J2の回動により第1アーム21に対して水平面内で回動(水平旋回)可能に構成される。シャフト23は、第2アーム22の先端部に第3関節軸J3を介して連結され、第2アーム22に対して第3関節軸J3の軸周りに回転可能かつ第3関節軸J3の軸方向に沿って昇降可能に構成される。本実施形態のスカラロボットでは、シャフト23の先端には、ワークWをピックアップして保持するワーク保持部24が設けられている。なお、ワーク保持部24としては、例えば、負圧によりワークWを吸着する吸着ノズルや、一対の爪によりワークWを把持するメカニカルチャック、電磁石によりワークWを吸着する電磁チャックなどを挙げることができる。
The base 11 is fixed to the workbench 2 and supports the base end side of the scalar arm 20. The scalar arm 20 includes a first arm 21, a first arm drive unit 30, a second arm 22, a second arm drive unit 40, a shaft 23, and a shaft drive unit 50. The base end of the first arm 21 is connected to the base 11 via the first joint axis J1 and can rotate (horizontally turn) with respect to the base 11 by the rotation of the first joint axis J1. It is composed of. The base end of the second arm 22 is connected to the tip of the first arm 21 via the second joint axis J2, and the second arm 22 rotates in a horizontal plane with respect to the first arm 21 due to the rotation of the second joint axis J2. (Horizontal turning) is possible. The shaft 23 is connected to the tip of the second arm 22 via the third joint axis J3, is rotatable about the axis of the third joint axis J3 with respect to the second arm 22, and is in the axial direction of the third joint axis J3. It is configured to be able to move up and down along. In the SCARA robot of the present embodiment, a work holding portion 24 for picking up and holding the work W is provided at the tip of the shaft 23. Examples of the work holding portion 24 include a suction nozzle that sucks the work W by a negative pressure, a mechanical chuck that grips the work W by a pair of claws, and an electromagnetic chuck that sucks the work W by an electromagnet. ..
第1アーム駆動部30は、モータ32とエンコーダ34とを備える。モータ32の回転軸は、図示しない減速機を介して第1関節軸J1に連結されている。第1アーム駆動部30は、モータ32を駆動することで減速機を介して第1関節軸J1に伝達されるトルクにより、第1関節軸J1を支点に第1アーム21を回動させる。エンコーダ34は、モータ32の回転軸に取り付けられ、モータ32の回転変位量を検出するロータリエンコーダとして構成される。
The first arm drive unit 30 includes a motor 32 and an encoder 34. The rotation shaft of the motor 32 is connected to the first joint shaft J1 via a speed reducer (not shown). The first arm drive unit 30 rotates the first arm 21 with the first joint shaft J1 as a fulcrum by the torque transmitted to the first joint shaft J1 via the speed reducer by driving the motor 32. The encoder 34 is attached to the rotation shaft of the motor 32 and is configured as a rotary encoder that detects the amount of rotational displacement of the motor 32.
第2アーム駆動部40は、第1アーム駆動部30と同様に、モータ42とエンコーダ44とを備える。モータ42の回転軸は、図示しない減速機を介して第2関節軸J2に連結されている。第2アーム駆動部40は、モータ42を駆動することで減速機を介して第2関節軸J2に伝達されるトルクにより、第2関節軸J2を支点に第2アーム22を回動させる。エンコーダ44は、モータ42の回転軸に取り付けられ、モータ42の回転変位量を検出するロータリエンコーダとして構成される。
The second arm drive unit 40 includes a motor 42 and an encoder 44, similarly to the first arm drive unit 30. The rotation shaft of the motor 42 is connected to the second joint shaft J2 via a speed reducer (not shown). The second arm drive unit 40 rotates the second arm 22 with the second joint shaft J2 as a fulcrum by the torque transmitted to the second joint shaft J2 via the speed reducer by driving the motor 42. The encoder 44 is attached to the rotation shaft of the motor 42 and is configured as a rotary encoder that detects the amount of rotational displacement of the motor 42.
シャフト駆動部50は、図3に示すように、モータ52a,52bとエンコーダ54a,54bとを備える。モータ52aの回転軸は、図示しないベルトを介してシャフト23に接続されている。シャフト駆動部50は、モータ52aを駆動することによりシャフト23を軸周りに回転させる。モータ52bの回転軸は、図示しないボールねじ機構を介してシャフト23に接続されている。シャフト駆動部50は、モータ52bを駆動することにより、ボールねじ機構によりモータ52bの回転運動が直動運動に変換されることでシャフト23を上下に昇降させる。エンコーダ54aは、シャフト23の回転変位量を検出するロータリエンコーダとして構成される。エンコーダ54bは、シャフト23の昇降位置を検出するリニアエンコーダとして構成される。
As shown in FIG. 3, the shaft drive unit 50 includes motors 52a and 52b and encoders 54a and 54b. The rotation shaft of the motor 52a is connected to the shaft 23 via a belt (not shown). The shaft drive unit 50 rotates the shaft 23 around an axis by driving the motor 52a. The rotation shaft of the motor 52b is connected to the shaft 23 via a ball screw mechanism (not shown). By driving the motor 52b, the shaft drive unit 50 raises and lowers the shaft 23 up and down by converting the rotational motion of the motor 52b into a linear motion by the ball screw mechanism. The encoder 54a is configured as a rotary encoder that detects the amount of rotational displacement of the shaft 23. The encoder 54b is configured as a linear encoder that detects the elevating position of the shaft 23.
カメラ60は、第2アーム22の先端部側面に取り付けられている。カメラ60は、作業対象物であるワークWを撮像し、その撮像画像を制御装置70へ出力する。制御装置70は、撮像画像を処理することによりワークWの位置や姿勢を認識することができる。なお、カメラ60は第2アーム22に取り付けられているから、本実施形態では、スカラアーム20は、カメラ60を上下方向に移動させることはできない。
The camera 60 is attached to the side surface of the tip of the second arm 22. The camera 60 captures an image of the work W, which is a work object, and outputs the captured image to the control device 70. The control device 70 can recognize the position and posture of the work W by processing the captured image. Since the camera 60 is attached to the second arm 22, in the present embodiment, the scalar arm 20 cannot move the camera 60 in the vertical direction.
制御装置70は、図3に示すように、CPU71と、処理プログラムを記憶するROM72と、ワークメモリとしてのRAM73と、HDDやSSD等の記憶装置74と、入出力インタフェース(図示せず)とを備える。制御装置70には、エンコーダ34,44,54a,54bからの位置信号やカメラ60からの画像信号などが入出力インタフェースを介して入力される。制御装置70からは、モータ32,42,52a,52bへの駆動信号などが入出力インタフェースを介して出力される。
As shown in FIG. 3, the control device 70 includes a CPU 71, a ROM 72 for storing a processing program, a RAM 73 as a work memory, a storage device 74 such as an HDD or SSD, and an input / output interface (not shown). Be prepared. Position signals from the encoders 34, 44, 54a, 54b, image signals from the camera 60, and the like are input to the control device 70 via the input / output interface. From the control device 70, drive signals to the motors 32, 42, 52a, 52b and the like are output via the input / output interface.
次に、こうして構成されたスカラロボット1の動作について説明する。特に、作業台2上の被写体であるワークWを撮像するためのカメラ60の位置制御について説明する。図4は、制御装置70のCPU71により実行されるカメラ位置制御処理の一例を示すフローチャートである。
Next, the operation of the SCARA robot 1 configured in this way will be described. In particular, the position control of the camera 60 for photographing the work W which is the subject on the workbench 2 will be described. FIG. 4 is a flowchart showing an example of the camera position control process executed by the CPU 71 of the control device 70.
カメラ位置制御処理が実行されると、制御装置70のCPU71は、まず、図示しないフィーダから供給されるワークWを撮像するための基台11(後述するベース座標系Σ0)から見たカメラ60の画像中心の目標位置(X0*,Y0*,Z0*)を設定する(ステップS100)。ステップS100は、例えば、外部のコンピュータで作成されたプログラムのデータを読み込むことにより行なわれる。続いて、CPU71は、カメラ60の画像中心の目標位置(X0*,Y0*,Z0*)に対して逆運動学を解くことにより、カメラ60の画像中心を目標位置(X0*,Y0*,Z0*)に一致させるための第1関節軸J1の角度θJ1と第2関節軸J2の角度θJ2とを算出する(ステップS110)。
When the camera position control process is executed, the CPU 71 of the control device 70 first sees the camera 60 as seen from the base 11 (base coordinate system Σ 0 described later) for imaging the work W supplied from the feeder (not shown). The target position (X 0 *, Y 0 *, Z 0 *) at the center of the image is set (step S100). Step S100 is performed, for example, by reading the data of a program created by an external computer. Subsequently, the CPU 71 sets the image center of the camera 60 to the target position (X 0 *) by solving the inverse kinematics with respect to the target position (X 0 *, Y 0 *, Z 0 *) of the image center of the camera 60. , Y 0 *, Z 0 *), the angle θ J1 of the first joint axis J1 and the angle θ J2 of the second joint axis J2 are calculated (step S110).
次に、CPU71は、ステップS110で算出した第2関節軸J2の角度θJ2に基づいて第1アーム21のねじれ角度θtとたわみ角度θd1とを算出すると共に(ステップS120)、第2アーム22のたわみ角度θd2を算出する(ステップS130)。いま、図5および図6に示すように、基台11(第0リンク)にベース座標系Σ0を設定し、第1アーム21,第2アーム22,シャフト23,手先(ワーク保持部24)を第1リンク,第2リンク,第3リンク,第4リンクとしてそれぞれリンク座標系を設定する場合を考える。ここで、リンク座標系Σ1は、第1アーム21の根元(第1関節軸J1)に設定される。リンク座標系Σ2は、第2アーム22の根元(第2関節軸J2)に設定される。リンク座標系Σ3は、第2アーム22とシャフト23との連結部分に設定される。リンク座標系(ハンド座標系)Σ4は、手先(ワーク保持部24)に設定される。各座標系は、第1アーム21および第2アーム22が一直線状に伸びた状態をホームポジションとする。また、図中、X0,X1,X2,X3,X4は、それぞれの座標系のX軸である。Y0,Y1,Y2,Y3,Y4は、それぞれの座標系のY軸である。Z0,Z1,Z2,Z3,Z4は、それぞれの座標系のZ軸である。
Next, the CPU 71 calculates the twist angle θ t and the deflection angle θ d1 of the first arm 21 based on the angle θ J2 of the second joint axis J2 calculated in step S110 (step S120), and the second arm. The deflection angle θ d2 of 22 is calculated (step S130). Now, as shown in FIGS. 5 and 6, the base coordinate system Σ 0 is set on the base 11 (0th link), and the first arm 21, the second arm 22, the shaft 23, and the hand (work holding portion 24) are set. Consider the case where the link coordinate system is set as the first link, the second link, the third link, and the fourth link, respectively. Here, the link coordinate system Σ 1 is set at the base of the first arm 21 (first joint axis J1). The link coordinate system Σ 2 is set at the base of the second arm 22 (second joint axis J2). The link coordinate system Σ 3 is set at the connecting portion between the second arm 22 and the shaft 23. The link coordinate system (hand coordinate system) Σ 4 is set in the hand (work holding unit 24). In each coordinate system, the home position is a state in which the first arm 21 and the second arm 22 extend in a straight line. Further, in the figure, X 0 , X 1 , X 2 , X 3 , and X 4 are the X axes of the respective coordinate systems. Y 0 , Y 1 , Y 2 , Y 3 , and Y 4 are the Y axes of the respective coordinate systems. Z 0 , Z 1 , Z 2 , Z 3 , and Z 4 are the Z axes of their respective coordinate systems.
第1アーム21のねじれ角度θtは、リンク座標系Σ1において、第1関節軸J1の軸方向をZ1軸方向とし、第1アーム21の中心線に沿った方向をX1軸方向とし、X1軸およびZ1軸に直交する方向をY1軸方向とした場合、図7に示すように、第1アーム21のX1軸周りの変位角度として定義される。そして、第1アーム21のねじれ角度θtは、第1アーム21のねじれ角度θtと第2関節軸J2の角度θJ2とアーム質量M1との関係をあらわす次式(1)および(2)のねじれモデル式を用いて算出される。アーム質量M1は、スカラアーム20の第1関節軸J1より先端側のアーム部の質量であり、ワーク保持部24がワークWを保持している場合には当該ワークWの質量mも含み、ワークWの質量mに応じた変数となる。「g」は重力加速度である。「Lt・sinθJ2」は、図8に示すように、アーム部の重心位置LtにおけるY1軸方向成分である。「It」は、図9に示すように、アーム部に作用する重力による荷重によって第1アーム21をX1軸周りに回転させようとするモーメント(ねじれモーメント)であり、重心位置LtがX1軸上にあるときには値0であり、重心位置LtがX1軸からY1軸方向に離れるにつれて大きくなる。すなわち、ねじれモーメントItは、第2関節軸J2の角度θJ2が0度(基準角)のときに最小となり、第2関節軸J2の角度が-90度のときと+90度のときに最大となる。「Kt」は、ねじれ剛性であり、予め実験などにより求められる。
In the link coordinate system Σ 1 , the twist angle θ t of the first arm 21 is such that the axial direction of the first joint axis J 1 is the Z 1 axis direction and the direction along the center line of the first arm 21 is the X 1 axis direction. Assuming that the direction orthogonal to the X 1 axis and the Z 1 axis is the Y 1 axis direction, it is defined as the displacement angle of the first arm 21 around the X 1 axis as shown in FIG. The twist angle θ t of the first arm 21 represents the relationship between the twist angle θ t of the first arm 21, the angle θ J2 of the second joint axis J2, and the arm mass M1, the following equations (1) and (2). It is calculated using the twist model formula of. The arm mass M1 is the mass of the arm portion on the tip side of the first joint axis J1 of the scalar arm 20, and when the work holding portion 24 holds the work W, the arm mass M includes the mass m of the work W and the work. It is a variable according to the mass m of W. "G" is the gravitational acceleration. As shown in FIG. 8, “L t · sin θ J2 ” is a component in the Y1 axis direction at the position L t of the center of gravity of the arm portion. As shown in FIG. 9, “It” is a moment (twisting moment ) that attempts to rotate the first arm 21 around the X1 axis due to the load due to gravity acting on the arm portion, and the center of gravity position L t is The value is 0 when it is on the X 1 axis, and increases as the center of gravity L t moves away from the X 1 axis in the Y 1 axis direction. That is, the twisting moment It is the minimum when the angle θ J2 of the second joint axis J2 is 0 degrees (reference angle), and is maximum when the angle of the second joint axis J2 is −90 degrees and +90 degrees. It becomes. "K t " has torsional rigidity and is obtained in advance by experiments or the like.
第1アーム21のたわみ角度θd1は、図10に示すように、第1アーム21のY1軸周りの変位角度として定義される。そして、第1アーム21のたわみ角度θd1は、第1アーム21のたわみ角度θd1と第2関節軸J2の角度θJ2とアーム部の質量である上述したアーム質量M1との関係をあらわす次式(3)および(4)のたわみモデル式を用いて算出される。式中、「Ld1・cosθJ2」は、図11に示すように、アーム部の重心位置LtにおけるX1軸方向成分である。「Id1」は、図12に示すように、アーム部に作用する重力による荷重によって第1アーム21をY1軸周りに回転させようとするモーメント(たわみモーメント)であり、重心位置Ld1が第1関節軸J1からX1軸方向に離れるにつれて大きくなる。すなわち、たわみモーメントId1は、第2関節軸J2の角度が±180度のときに最小となり、第2関節軸J2の角度が0度(基準角)のときに最大となる。「Kd1」は、たわみ剛性であり、予め実験などにより求められる。
The deflection angle θ d1 of the first arm 21 is defined as the displacement angle of the first arm 21 around the Y 1 axis, as shown in FIG. The deflection angle θ d1 of the first arm 21 represents the relationship between the deflection angle θ d1 of the first arm 21, the angle θ J2 of the second joint axis J2, and the above-mentioned arm mass M1 which is the mass of the arm portion. It is calculated using the deflection model equations of the equations (3) and (4). In the equation, “L d1 · cosθ J2 ” is an X1 axial component at the center of gravity position Lt of the arm portion, as shown in FIG. As shown in FIG. 12, “I d1 ” is a moment (deflection moment) that attempts to rotate the first arm 21 around the Y 1 axis due to the load due to gravity acting on the arm portion, and the center of gravity position L d1 is set. It increases as the distance from the first joint axis J1 increases in the X1 axis direction. That is, the deflection moment I d1 is the minimum when the angle of the second joint axis J2 is ± 180 degrees, and is maximum when the angle of the second joint axis J2 is 0 degrees (reference angle). "K d1 " has a deflection rigidity and is obtained in advance by experiments or the like.
第2アーム22のたわみ角度θd2は、図10に示すように、第2アーム22のY2軸周りの変位角度として定義される。そして、第2アーム22のたわみ角度θd2は、第2アーム22のたわみ角度θd2とアーム質量M2との関係をあらわす次式(5)および(6)のたわみモデル式を用いて算出される。ここで、アーム質量M2は、スカラアーム20の第2関節軸J2より先端側のアーム部の質量であり、ワーク保持部24がワークWを保持している場合には当該ワークWの質量mも含み、ワークWの質量mに応じた変数となる。「Id2」は、アーム部に作用する重力による荷重によって第2アーム22をY2軸周りに回転させようとするモーメント(たわみモーメント)であり、第2関節軸J2の角度θd2とは無関係に、重心位置Ld2(第2関節軸J2の中心から重心位置までの距離)とアーム質量M2とによって定まる。「Kd2」は、たわみ剛性であり、予め実験などにより求められる。
The deflection angle θ d2 of the second arm 22 is defined as the displacement angle of the second arm 22 around the Y 2 axis, as shown in FIG. The deflection angle θ d2 of the second arm 22 is calculated using the deflection model equations (5) and (6) below, which express the relationship between the deflection angle θ d2 of the second arm 22 and the arm mass M2. .. Here, the arm mass M2 is the mass of the arm portion on the tip side of the second joint axis J2 of the scalar arm 20, and when the work holding portion 24 holds the work W, the mass m of the work W is also It is included and becomes a variable according to the mass m of the work W. "I d2 " is a moment (deflection moment) that tries to rotate the second arm 22 around the Y 2 axis by the load due to the gravity acting on the arm portion, and is irrelevant to the angle θ d2 of the second joint axis J2. It is determined by the center of gravity position L d2 (distance from the center of the second joint axis J2 to the center of gravity position) and the arm mass M2. "K d2 " has a deflection rigidity and is obtained in advance by experiments or the like.
これらのねじれモデル式およびたわみモデル式は、記憶装置74に記憶されている。ステップS110,S120において、CPU71は、記憶装置74から対応するモデル式を読み出すことにより、ねじれ角度θtおよびたわみ角度θd1,θd2を算出する。
These twist model formulas and deflection model formulas are stored in the storage device 74. In steps S110 and S120, the CPU 71 reads the corresponding model equation from the storage device 74 to calculate the twist angle θ t and the deflection angles θ d1 and θ d2 .
次に、CPU71は、第1関節軸J1の角度θJ1と第2関節軸J2の角度θJ2と第1アーム21のねじれ角度θtおよびたわみ角度θd1と第2アーム22のたわみ角度θd2とに基づいて順運動学を解くことにより、カメラ60の画像中心の推定位置(X0,Y0,Z0)を算出する(ステップS140)。すなわち、CPU71は、画像中心が目標位置(X0*,Y0*,Z0*)と一致するように逆運動学により算出される第1および第2関節軸J1,J2の角度θJ1,θJ2を角度指令値として制御したと仮定した場合に、第1アーム21のねじれ及びたわみや第2アーム22のたわみに起因して目標位置(X0*,Y0*,Z0*)からズレる画像中心の推定位置(X0,Y0,Z0)を算出する。
Next, the CPU 71 has an angle θ J1 of the first joint axis J1, an angle θ J2 of the second joint axis J2, a twist angle θ t of the first arm 21, a deflection angle θ d1 , and a deflection angle θ d2 of the second arm 22. By solving the forward kinematics based on the above, the estimated position (X 0 , Y 0 , Z 0 ) of the image center of the camera 60 is calculated (step S140). That is, the CPU 71 has an angle θ J1 of the first and second joint axes J1 and J2 calculated by inverse kinematics so that the center of the image coincides with the target position (X 0 *, Y 0 *, Z 0 *). Assuming that θ J2 is controlled as an angle command value, from the target position (X 0 *, Y 0 *, Z 0 *) due to the twist and deflection of the first arm 21 and the deflection of the second arm 22. The estimated position (X 0 , Y 0 , Z 0 ) of the center of the image to be displaced is calculated.
CPU71は、画像中心の推定位置(X0,Y0,Z0)を算出すると、算出した画像中心の推定位置(X0,Y0,Z0)とステップS100で取得した画像中心の目標位置(X0*,Y0*,Z0*)との差分をとることにより、画像中心の位置ズレ量(ΔX0,ΔY0,ΔZ0)を算出する(ステップS150)。続いて、CPU71は、画像中心の目標位置(X0*,Y0*,Z0*)を位置ズレ量(ΔX0,ΔY0,ΔZ0)の分だけオフセットすることにより目標位置を補正し(ステップS160)、補正した目標位置に対して逆運動学を解くことによりカメラ60の画像中心を目標位置(X0*,Y0*,Z0*)に一致させるための第1関節軸J1の角度指令値θJ1*と第2関節軸J2の角度指令値θJ2*とを算出する(ステップS170)。そして、CPU71は、エンコーダ34により検出される第1関節軸J1の角度が算出した角度指令値θJ1*に一致すると共にエンコーダ44により検出される第2関節軸J2の角度が算出した角度指令値θJ2*に一致するようフィードバック制御により対応するモータ31,32を制御して(ステップS180)、カメラ位置制御処理を終了する。
When the CPU 71 calculates the estimated position of the image center (X 0 , Y 0 , Z 0 ), the calculated estimated position of the image center (X 0 , Y 0 , Z 0 ) and the target position of the image center acquired in step S100. By taking the difference from (X 0 *, Y 0 *, Z 0 *), the amount of positional deviation (ΔX 0 , ΔY 0 , ΔZ 0 ) at the center of the image is calculated (step S150). Subsequently, the CPU 71 corrects the target position by offsetting the target position (X 0 *, Y 0 *, Z 0 *) at the center of the image by the amount of the positional deviation (ΔX 0 , ΔY 0 , ΔZ 0 ). (Step S160), the first joint axis J1 for aligning the image center of the camera 60 with the target position (X 0 *, Y 0 *, Z 0 *) by solving the inverse kinematics with respect to the corrected target position. The angle command value θ J1 * of the second joint axis J2 and the angle command value θ J2 * of the second joint axis J2 are calculated (step S170). Then, the CPU 71 matches the angle command value θ J1 * calculated by the angle of the first joint axis J1 detected by the encoder 34, and the angle command value calculated by the angle of the second joint axis J2 detected by the encoder 44. The corresponding motors 31 and 32 are controlled by feedback control so as to match θ J2 * (step S180), and the camera position control process is terminated.
CPU71は、こうしてカメラ位置制御処理を実行すると、図示しないフィーダから供給されたワークWをカメラ60により撮像し、撮像画像に画像処理を施すことにより、ワークWの位置および姿勢を認識する。そして、CPU71は、ワークWをピックアップするための手先(ワーク保持部24)の目標位置(X0*,Y0*,Z0*)を設定し、手先を目標位置(X0*,Y0*,Z0*)に移動させるための手先位置制御処理を実行する。目標位置(X0*,Y0*)の設定は、認識したワークWの位置および姿勢に基づいて行なわれる。目標位置Z0*の設定は、ワークWの高さ情報(ワーク高さWH)に基づいて行なわれる。ワークWの高さ情報は、上述した画像処理に用いられる画像処理データの一つとして予め登録されている。したがって、高さ情報の取得は、画像処理データの中からワーク高さWHを読み出すことにより行なうことができる。例えば、ワーク保持部24として吸着ノズルや電磁チャックを用いてワークWをピックアップする場合、ワークWのピック位置はワークWの上面(吸着面)となるため、目標位置Z0*は、図13に示すように、ワークWが置かれる基準面S(固定値)に対してワーク高さWHだけ高い位置に設定される。なお、ワーク保持部24としてワークWの両側面を把持するメカニカルチャックを用いてワークWをピックアップする場合、ワークWのピック位置は、ワークWの上面よりも低い位置となる。この場合、手先の目標位置Z0*は、別途、入力されたピック位置を用いて設定される。
When the camera position control process is executed in this way, the CPU 71 takes an image of the work W supplied from a feeder (not shown) by the camera 60, and performs image processing on the captured image to recognize the position and posture of the work W. Then, the CPU 71 sets a target position (X 0 *, Y 0 *, Z 0 *) of the hand (work holding unit 24) for picking up the work W, and sets the hand to the target position (X 0 *, Y 0 ). *, Z 0 *) is executed to perform the hand position control process for moving to. The target position (X 0 *, Y 0 *) is set based on the recognized position and posture of the work W. The target position Z 0 * is set based on the height information of the work W (work height WH). The height information of the work W is registered in advance as one of the image processing data used for the above-mentioned image processing. Therefore, the height information can be acquired by reading the work height WH from the image processing data. For example, when the work W is picked up by using the suction nozzle or the electromagnetic chuck as the work holding portion 24, the pick position of the work W is the upper surface (suction surface) of the work W, so the target position Z 0 * is shown in FIG. As shown, the work height WH is set higher than the reference surface S (fixed value) on which the work W is placed. When the work W is picked up by using a mechanical chuck that grips both side surfaces of the work W as the work holding portion 24, the pick position of the work W is lower than the upper surface of the work W. In this case, the target position Z 0 * of the hand is set separately using the input pick position.
手先位置制御処理は、シャフト駆動部50によるシャフト23の昇降が行なわれる点を除いて、基本的には、上述したカメラ位置制御処理と同様の処理により行なわれる。すなわち、手先位置制御処理では、CPU71は、まず、手先を設定した目標位置(X0*,Y0*,Z0*)に移動させるための第1関節軸J1の角度θJ1と第2関節軸J2の角度θJ2とシャフト23の角度(シャフト角度)θJ4とシャフト23の昇降位置Zsとを逆運動学により算出する。続いて、CPU71は、算出したθJ1,θJ2,θJ4,Zsと第1アーム21のねじれ角度θtおよびたわみ角度θd1と第2アーム22のねじれ角度θd2とに基づいてねじれやたわみを考慮した手先の推定位置(X0,Y0,Z0)を順運動学により算出する。次に、CPU71は、手先の推定位置(X0,Y0,Z0)と目標位置(X0*,Y0*,Z0*)との差分をとることにより位置ズレ量(ΔX0,ΔY0,ΔZ0)を算出する。次に、CPU71は、目標位置(X0*,Y0*,Z0*)を位置ズレ量(ΔX0,ΔY0,ΔZ0)の分だけオフセットすることにより目標位置を補正する。そして、CPU71は、補正した目標位置に手先を移動させるための第1関節軸J1の角度指令値θJ1*と第2関節軸J2の角度指令値θJ2*とシャフト23の角度指令値θJ4*とシャフト23の昇降位置指令値Zs*とを逆運動学により算出し、各指令値に基づいて対応するモータ31,32,33a,33bをフィードバック制御により制御する。
The hand position control process is basically performed by the same process as the camera position control process described above, except that the shaft 23 is moved up and down by the shaft drive unit 50. That is, in the hand position control process, the CPU 71 first has an angle θ J1 of the first joint axis J1 and a second joint for moving the hand to the set target position (X 0 *, Y 0 *, Z 0 *). The angle θ J2 of the axis J2 and the angle (shaft angle) θ J2 of the shaft 23 and the elevating position Z s of the shaft 23 are calculated by inverse kinematics. Subsequently, the CPU 71 twists and twists based on the calculated θ J1 , θ J2 , θ J4 , Z s , the twist angle θ t of the first arm 21, the deflection angle θ d1 , and the twist angle θ d 2 of the second arm 22. The estimated position of the hand (X 0 , Y 0 , Z 0 ) considering the deflection is calculated by forward kinematics. Next, the CPU 71 takes a difference between the estimated position of the hand (X 0 , Y 0 , Z 0 ) and the target position (X 0 *, Y 0 *, Z 0 *) to obtain a position shift amount (ΔX 0 , ΔY 0 , ΔZ 0 ) is calculated. Next, the CPU 71 corrects the target position by offsetting the target position (X 0 *, Y 0 *, Z 0 *) by the amount of the positional deviation (ΔX 0 , ΔY 0 , ΔZ 0 ). Then, the CPU 71 has an angle command value θ J1 * of the first joint axis J1 for moving the hand to the corrected target position, an angle command value θ J2 * of the second joint axis J2, and an angle command value θ J4 of the shaft 23. * And the elevating position command value Z s * of the shaft 23 are calculated by inverse kinematics, and the corresponding motors 31, 32, 33a, 33b are controlled by feedback control based on each command value.
ここで、カメラ位置制御処理や手先位置制御処理において、第1アーム21のねじれ及びたわみや第2アーム22のたわみを考慮したカメラ60の画像中心や手先(ワーク保持部24)の推定位置を求めるための順運動学の計算は、変数θJ1,θJ2,θt,θd1,θd2を同次変換行列に与えることにより行なうことができる。同次変換行列は、ロボット本体10に設定した各座標系のうち隣接する座標系間の関係i-1Ti(i=1,2,3,4)であるリンクパラメータを求め、リンクパラメータに基づいてアーム全体の変換行列0T4を求めることにより導き出すことができる。
Here, in the camera position control process and the hand position control process, the estimated position of the image center and the hand (work holding portion 24) of the camera 60 in consideration of the twist and deflection of the first arm 21 and the deflection of the second arm 22 is obtained. The forward kinematics calculation for this can be done by giving the variables θ J1 , θ J2 , θ t , θ d1 , and θ d 2 to the homologous transformation matrix. The homogeneous transformation matrix obtains the link parameter, which is the relationship i-1 Ti ( i = 1, 2, 3, 4) between the adjacent coordinate systems in each coordinate system set in the robot main body 10, and is used as the link parameter. It can be derived by finding the transformation matrix 0 T 4 for the entire arm based on this.
図14は、手先用リンクパラメータ表の一例を示す説明図である。図15は、カメラ用リンクパラメータ表の一例を示す説明図である。手先用リンクパラメータ表およびカメラ用リンクパラメータ表において、図14および図15中、第iリンク(i=1,2,3,4)の各リンクパラメータ(dx、dy、dz、rx、ry、rz、rx2、ry2)は、座標系Σi-1から座標系Σiへ変換するためのパラメータである。dx,dy,dzは、X軸方向,Y軸方向,Z軸方向の並進量を示すパラメータ(位置パラメータ)である。rx(rx2),ry(ry2),rzは、X軸周り,Y軸周り,Z軸周りの回転量を示すパラメータ(姿勢パラメータ)である。
FIG. 14 is an explanatory diagram showing an example of a hand link parameter table. FIG. 15 is an explanatory diagram showing an example of a camera link parameter table. In the hand link parameter table and the camera link parameter table, each link parameter (dx, dy, dz, rx, ry,) of the i-th link (i = 1, 2, 3, 4) in FIGS. 14 and 15 rz, rx2, ry2) are parameters for converting from the coordinate system Σ i-1 to the coordinate system Σ i . dx, dy, and dz are parameters (positional parameters) indicating translational amounts in the X-axis direction, the Y-axis direction, and the Z-axis direction. rx (rx2), ry (ry2), and rz are parameters (posture parameters) indicating the amount of rotation around the X-axis, around the Y-axis, and around the Z-axis.
また、図5または図6に示すように、L1は、Z1軸とZ2軸とのX1軸方向の距離(第1アーム21のアーム長)を示す。L2は、Z2軸とZ3軸とのX2軸方向の距離(第2アーム22のアーム長)を示す。Lz1は、ベース座標系Σ0の原点からリンク座標系Σ1の原点までのZ0軸方向の距離を示す。Lz2は、リンク座標系Σ1の原点からリンク座標系Σ2の原点までのZ1軸方向の距離を示す。th1は、リンク座標系Σ1のZ1軸周りの回転角であり、第1関節軸J1の角度θJ1に相当する。th2は、リンク座標系Σ2のZ2軸周りの回転角であり、第2関節軸J2の角度θJ2に相当する。th4は、リンク座標系Σ4のZ4軸周りの回転角であり、シャフト23の角度(シャフト角度)θJ4に相当する。d3は、リンク座標系Σ3の原点から手先までの距離であり、昇降位置Zsに相当する。J1rxは、リンク座標系Σ1のX1軸周りの回転角を示す。J1ryは、リンク座標系Σ1のY1軸周りの回転角を示す。J1rx2は、リンク座標系Σ1がZ1軸周りにth1だけ回転した後のX1軸周りの回転角を示し、第1アーム21のねじれ角度θtに相当する。J1ry2は、リンク座標系Σ1がZ1軸周りにth1だけ回転した後のY1軸周りの回転角を示し、第1アーム21のたわみ角度θd1に相当する。J2rxは、リンク座標系Σ2のX2軸周りの回転角を示す。J2ryは、リンク座標系Σ2のY2軸周りの回転角を示す。J2ry2は、リンク座標系Σ2がZ2軸周りにth2回転した後のY2軸周りの回転角を示し、第2アーム22のたわみ角度θd2に相当する。J3rxは、リンク座標系Σ3のX3軸周りの回転角を示す。J3ryは、リンク座標系Σ3のY3軸周りの回転角を示す。
Further, as shown in FIG. 5 or 6, L 1 indicates the distance between the Z 1 axis and the Z 2 axis in the X 1 axis direction (arm length of the first arm 21). L 2 indicates the distance between the Z 2 axis and the Z 3 axis in the X 2 axis direction (arm length of the second arm 22). L z 1 indicates the distance in the Z 0 axis direction from the origin of the base coordinate system Σ 0 to the origin of the link coordinate system Σ 1 . L z 2 indicates the distance in the Z 1 axis direction from the origin of the link coordinate system Σ 1 to the origin of the link coordinate system Σ 2 . th1 is the rotation angle of the link coordinate system Σ 1 around the Z 1 axis, and corresponds to the angle θ J1 of the first joint axis J1. th2 is the angle of rotation around the Z2 axis of the link coordinate system Σ2, and corresponds to the angle θ J2 of the second joint axis J2. th4 is the rotation angle around the Z4 axis of the link coordinate system Σ4 , and corresponds to the angle ( shaft angle) θ J4 of the shaft 23. d3 is the distance from the origin of the link coordinate system Σ 3 to the hand, and corresponds to the elevating position Z s . J1rx indicates the rotation angle of the link coordinate system Σ 1 around the X 1 axis. J1ry indicates the rotation angle of the link coordinate system Σ 1 around the Y 1 axis. J1rx2 indicates the rotation angle around the X 1 axis after the link coordinate system Σ 1 is rotated about the Z 1 axis by th1 and corresponds to the twist angle θ t of the first arm 21. J1ry2 indicates the rotation angle around the Y 1 axis after the link coordinate system Σ 1 is rotated about the Z 1 axis by th1 and corresponds to the deflection angle θ d1 of the first arm 21. J2rx indicates the angle of rotation of the link coordinate system Σ 2 around the X 2 axis. J2ry indicates the rotation angle of the link coordinate system Σ 2 around the Y 2 axis. J2ry2 indicates the rotation angle around the Y 2 axis after the link coordinate system Σ 2 rotates th2 about the Z 2 axis, and corresponds to the deflection angle θ d2 of the second arm 22. J3rx indicates the angle of rotation of the link coordinate system Σ 3 around the X3 axis. J3ry indicates the rotation angle of the link coordinate system Σ 3 around the Y 3 axis.
アーム全体の変換行列0T4は、リンクパラメータ表による各座標系間の関係i-1Ti(i=1,2,3,4)を用いて次式(7)により求めることができる。各座標系間の関係(行列)i-1Tiは、次式(8)により求めることができる。なお、dx,dy,dz,rx,ry,rz,rx2,ry2の各行列Tdx,Tdy,Tdz,Trx,Try,Trz,Trx2,Try2を次式(9)~(16)に示す。
The transformation matrix 0 T 4 for the entire arm can be obtained by the following equation (7) using the relationship i-1 Ti (i = 1, 2, 3, 4) between the coordinate systems according to the link parameter table. The relationship (matrix) i -1 Ti between each coordinate system can be obtained by the following equation (8). The matrices T dx , T dy , T dz , Trx, Tri, Trz, Trx2, and Tri2 of the dx, dy, dz, rx , ry , rz , rx2 , and ry2 are given by the following equations (9) to (16).
手先位置制御処理において、手先用リンクパラメータ表のJ1rx2に第1アーム21のねじれ角度θtを与え、J1ry2に第1アーム21のたわみ角度θd1を与え、J2ry2に第2アーム22のたわみ角度θd2を与えることで、順運動学(変換行列0T4)により第1アーム21のねじれ及びたわみと第2アーム22のたわみとに起因した誤差を含む手先の推定位置を計算することができる。
In the hand position control process, the twist angle θ t of the first arm 21 is given to J1rx2 in the hand link parameter table, the deflection angle θ d1 of the first arm 21 is given to J1ry2, and the deflection angle of the second arm 22 is given to J2ry2. By giving θ d2 , it is possible to calculate the estimated position of the hand including the error caused by the twist and deflection of the first arm 21 and the deflection of the second arm 22 by forward kinematics (transformation matrix 0 T 4 ). ..
カメラ用リンクパラメータは、図15に示すように、リンク座標系Σ2からリンク座標系Σ3への変換用パラメータdx,dy,dz,rx,ryに、L2+HCX,HCY,0,HCRA,HCRBが設定され、リンク座標系Σ3からハンド座標系Σ4への変換用パラメータのdz,rzにそれぞれHCZ,0が設定されている点で、手先用リンクパラメータとは異なる。HCXは、X2軸方向におけるカメラ60の位置を示し、HCYは、Y2軸方向におけるカメラ60の位置を示す。また、HCRAは、X3軸周りのカメラ60の傾きを示し、HCRBは、Y3軸回りのカメラ60の傾きを示す。HCZは、カメラ60(レンズ先端部)から被写体であるワークWの上面までの距離(ワーキングディスタンスWD)を示す。
As shown in FIG. 15, the link parameters for the camera are the parameters dx, dy, dz, rx, ry for conversion from the link coordinate system Σ 2 to the link coordinate system Σ 3 , and L 2 + HCX, HCY, 0, HCRA, It differs from the hand link parameter in that the HCRB is set and the HCZ and 0 are set in the dz and rz of the conversion parameters from the link coordinate system Σ 3 to the hand coordinate system Σ 4 , respectively. HCX indicates the position of the camera 60 in the X 2 -axis direction, and HCY indicates the position of the camera 60 in the Y 2 -axis direction. Further, HCRA indicates the inclination of the camera 60 around the X3 axis, and HCRB indicates the inclination of the camera 60 around the Y3 axis. The HCZ indicates the distance (working distance WD) from the camera 60 (the tip of the lens) to the upper surface of the work W which is the subject.
カメラ位置制御処理において、カメラ用リンクパラメータ表のJ1rx2に第1アーム21のねじれ角度θtを与え、J1ry2に第1アーム21のたわみ角度θd1を与え、J2ry2に第2アーム22のたわみ角度θd2を与えることで、順運動学により第1アーム21のねじれ及びたわみと第2アーム22のたわみとに起因した誤差を含むカメラ60の画像中心の推定位置を計算することができる。また、カメラ60のX2軸方向における位置XおよびY2軸方向における位置Yを予め測定しておき、カメラ用リンクパラメータ表のHCXに位置Xを与え、HCYに位置Yを与えることで、順運動学によりカメラ60の取付位置に起因した誤差を含むカメラ60の画像中心の推定位置を計算することができる。さらに、カメラ60のX3軸周りにおける傾きRAおよびY3軸周りにおける傾きRBを予め測定しておき、カメラ用リンクパラメータ表のHCRAに傾きRAを与え、HCRBに傾きRBを与えることで、順運動学によりカメラ60の取付角度に起因した誤差を含むカメラ60の画像中心の推定位置を計算することができる。
In the camera position control process, the twist angle θ t of the first arm 21 is given to J1rx2 in the link parameter table for the camera, the deflection angle θ d1 of the first arm 21 is given to J1ry2, and the deflection angle θ of the second arm 22 is given to J2ry2. By giving d2 , it is possible to calculate the estimated position of the image center of the camera 60 including the error caused by the twist and deflection of the first arm 21 and the deflection of the second arm 22 by forward kinematics. Further, the position X in the X 2 -axis direction of the camera 60 and the position Y in the Y 2 -axis direction are measured in advance, the position X is given to the HCX in the link parameter table for the camera, and the position Y is given to the HCY. Kinematics can be used to calculate the estimated position of the image center of the camera 60, including errors due to the mounting position of the camera 60. Further , the tilt RA around the X3 axis and the tilt RB around the Y3 axis of the camera 60 are measured in advance, the tilt RA is given to the HCRA in the link parameter table for the camera, and the tilt RB is given to the HCRB. The kinematics can calculate the estimated position of the image center of the camera 60 including the error due to the mounting angle of the camera 60.
さらに、本実施形態では、カメラ用リンクパラメータ表のHCZに与えられるワーキングディスタンスWDは、カメラ60(レンズ先端部)とワークWが置かれる基準面Sとの距離(固定値)から上述したワーク高さWHを減じることにより算出される。図16に示すように、ワークWが置かれる基準面Sに対してカメラ60の光軸(図中、破線参照)が傾いている状態で高さの異なるワークWを撮像すると、高さの違いΔWHに応じてそれぞれのワークWの撮像画像の画像中心がズレる現象が生じる。本実施形態では、画像処理データからワーク高さWHを取得し、HCZに反映させることで、ワークWの高さの違いによる画像中心のズレを修正することが可能となる。
Further, in the present embodiment, the working distance WD given to the HCZ in the link parameter table for the camera is the work height described above from the distance (fixed value) between the camera 60 (lens tip) and the reference surface S on which the work W is placed. It is calculated by subtracting the WH. As shown in FIG. 16, when images of work W having different heights are taken while the optical axis of the camera 60 (see the broken line in the figure) is tilted with respect to the reference plane S on which the work W is placed, the difference in height is obtained. A phenomenon occurs in which the image center of the captured image of each work W shifts according to ΔWH. In the present embodiment, by acquiring the work height WH from the image processing data and reflecting it in the HCZ, it is possible to correct the deviation of the image center due to the difference in the height of the work W.
次に、カメラ60の位置X,Yや傾きRA,RBを測定してカメラ用リンクパラメータに反映させるための手順について説明する。図17は、カメラ用リンクパラメータ設定手順の一例を示す説明図である。カメラ用リンクパラメータ設定手順では、ステップS200~S240により実行される。ステップS200は、手先(ワーク保持部24)に測定用治具100を取り付ける。図18は、手先(ワーク保持部24)に測定用治具が取り付けられた状態を示す部分外観図である。図示するように、測定用治具100は、装着箇所を中心としてシャフト23に直交する方向に所定距離だけ離間すると共に周方向に所定間隔(90度の間隔)をおいて配置された複数(4つ)のマークMAを有する。測定用治具100は、シャフト23に対して位相合わせされた状態で取り付けられ、各マークMAは、カメラ60が設計値通りに取り付けられている場合に、カメラ60の画像中心がマークMAの中心と一致するように配置されている。
Next, the procedure for measuring the positions X and Y of the camera 60 and the tilts RA and RB and reflecting them in the link parameters for the camera will be described. FIG. 17 is an explanatory diagram showing an example of a camera link parameter setting procedure. In the camera link parameter setting procedure, steps S200 to S240 are executed. In step S200, the measuring jig 100 is attached to the hand (work holding portion 24). FIG. 18 is a partial external view showing a state in which the measuring jig is attached to the hand (work holding portion 24). As shown in the figure, the measuring jigs 100 are spaced apart by a predetermined distance in the direction orthogonal to the shaft 23 with the mounting location as the center, and are arranged at predetermined intervals (intervals of 90 degrees) in the circumferential direction (4). It has the mark MA of one). The measuring jig 100 is attached to the shaft 23 in a phase-aligned state, and each mark MA has the image center of the camera 60 as the center of the mark MA when the camera 60 is attached according to the design value. It is arranged to match with.
ステップS210は、測定用治具100を用いてカメラ60の位置X,Yを測定する。このステップS210は、制御装置70のCPU71により図19に例示するカメラ位置測定処理を実行することにより行なわれる。カメラ位置測定処理では、CPU71は、まず、手先(ワーク保持部24)を測定位置に移動させる(ステップS300)。この処理は、マークMAがカメラ60の被写界深度内となる昇降位置指令値Zs*を設定してモータ52bを制御することにより行なわれる。続いて、CPU71は、シャフト23が所定ピッチずつ回転するようにモータ52aを制御し、シャフト23が所定ピッチずつ回転する毎に、カメラ60によりマークMAを撮像する処理を繰り返す(ステップS310)。次に、CPU71は、撮像画像を処理することによりカメラ60により測定されたマークMAの実測位置と設計上のマークMAの位置(設計位置)との位置ズレ量ΔX,ΔYを算出する(ステップS320)。図20は、カメラ位置の測定の様子を示す説明図である。図中、白丸は、カメラ60の取付位置が設計値通りであった場合に撮像画像に写るマーク画像を示し、黒丸は、カメラ60でマークMAを実際に撮像することにより撮像画像に写ったマーク画像を示す。位置ズレ量ΔXは、設計位置の円弧と実測位置の円弧とのX軸方向の位置の差である。位置ズレ量ΔYは、設計位置の円弧と実測位置の円弧とのY軸方向の位置の差である。そして、CPU71は、カメラ60の設計位置Xdv,YdvにステップS130で算出した位置ズレ量ΔX,ΔYを加算することにより、実際のカメラ60の位置X,Yを算出する(ステップS330)。
In step S210, the positions X and Y of the camera 60 are measured using the measuring jig 100. This step S210 is performed by executing the camera position measurement process illustrated in FIG. 19 by the CPU 71 of the control device 70. In the camera position measurement process, the CPU 71 first moves the hand (work holding unit 24) to the measurement position (step S300). This process is performed by controlling the motor 52b by setting the elevating position command value Z s * within the depth of field of the camera 60 by the mark MA. Subsequently, the CPU 71 controls the motor 52a so that the shaft 23 rotates by a predetermined pitch, and repeats the process of capturing the mark MA by the camera 60 each time the shaft 23 rotates by a predetermined pitch (step S310). Next, the CPU 71 calculates the amount of positional deviation ΔX, ΔY between the actually measured position of the mark MA measured by the camera 60 and the position (design position) of the mark MA on the design by processing the captured image (step S320). ). FIG. 20 is an explanatory diagram showing a state of measurement of the camera position. In the figure, white circles indicate mark images that appear in the captured image when the mounting position of the camera 60 is as designed, and black circles indicate marks that appear in the captured image by actually capturing the mark MA with the camera 60. The image is shown. The position deviation amount ΔX is the difference in position in the X-axis direction between the arc at the design position and the arc at the measured position. The position deviation amount ΔY is the difference in position in the Y-axis direction between the arc at the design position and the arc at the measured position. Then, the CPU 71 calculates the actual camera 60 positions X and Y by adding the position deviation amounts ΔX and ΔY calculated in step S130 to the design positions Xdv and Ydv of the camera 60 (step S330).
ステップS220は、測定用治具100を用いてカメラ60の傾きRA,RBを測定する。このステップS220は、制御装置70のCPU71により図21に例示するカメラ傾き測定処理を実行することにより行なわれる。カメラ傾き測定処理では、CPU71は、まず、マークMAをカメラ60の正面(真下)に移動させる(ステップS400)。この処理は、シャフト角度θJ4が予め定められた所定角度となるようにモータ52aを制御することにより行なわれる。続いて、CPU71は、カメラ60の被写界深度内の所定量bだけマークMAを上下させ、下位置にあるマークMAと上位置にあるマークMAとをカメラ60によりそれぞれ撮像する(ステップS410)。次に、CPU71は、撮像画像を処理することにより、下位置で撮像したマークMAの実測位置と設計位置とのXY軸方向の位置ズレ量ΔXa,ΔYaと、上位置で撮像したマークMAの実測位置と設計位置とのXY軸方向の位置ズレ量ΔXc,ΔYcとを算出する(ステップS420)。図22は、カメラ傾きの測定の様子を示す説明図である。カメラ60が取付角度が設計値通りであった場合に撮像画像に写るマーク画像の中心は、カメラ60の画像中心と一致する。図示するように、位置ズレ量ΔYaは、下位置でマークMAを撮像したときの撮像画像に写ったマーク画像の中心位置(実測位置)と設計位置とのY軸方向における差である。位置ズレ量ΔYcは、上位置でマークMAを撮像したときに撮像画像に写ったマーク画像の中心位置(実測位置)と設計位置とのY軸方向における差である。図示しないが、位置ズレ量ΔXaは、下位置でマークMAを撮像したときの撮像画像に写ったマーク画像の中心位置(実測位置)と設計位置とのX軸方向における差である。位置ズレ量ΔXcは、上位置でマークMAを撮像したときに撮像画像に写ったマーク画像の中心位置(実測位置)と設計位置とのY軸方向における差である。そして、CPU71は、算出した位置ズレ量ΔYa,ΔYbと上述した所定量bとに基づいて次式(17)によりX軸回りのカメラ60の傾きRAを算出すると共に、算出した位置ズレ量ΔXa,ΔXbと所定量bとに基づいて次式(18)によりY軸回りのカメラ60の傾きRBを算出する(ステップS430)。なお、式(17)は、図22から容易に導き出すことができる。式(18)についても同様である。
In step S220, the tilt RA and RB of the camera 60 are measured by using the measuring jig 100. This step S220 is performed by executing the camera tilt measurement process illustrated in FIG. 21 by the CPU 71 of the control device 70. In the camera tilt measurement process, the CPU 71 first moves the mark MA to the front (directly below) of the camera 60 (step S400). This process is performed by controlling the motor 52a so that the shaft angle θ J4 becomes a predetermined predetermined angle. Subsequently, the CPU 71 raises and lowers the mark MA by a predetermined amount b within the depth of field of the camera 60, and images the mark MA at the lower position and the mark MA at the upper position by the camera 60 (step S410). .. Next, the CPU 71 processes the captured image to measure the amount of positional deviation ΔXa, ΔYa in the XY-axis direction between the actually measured position of the mark MA imaged at the lower position and the design position, and the actual measurement of the mark MA captured at the upper position. The amount of positional deviation between the position and the design position in the XY axis directions ΔXc and ΔYc are calculated (step S420). FIG. 22 is an explanatory diagram showing a state of measuring the tilt of the camera. When the mounting angle of the camera 60 is as designed, the center of the mark image captured in the captured image coincides with the center of the image of the camera 60. As shown in the figure, the position deviation amount ΔYa is the difference between the center position (measured position) of the mark image captured in the captured image when the mark MA is imaged at the lower position and the design position in the Y-axis direction. The position deviation amount ΔYc is the difference between the center position (measured position) of the mark image captured in the captured image and the design position in the Y-axis direction when the mark MA is imaged at the upper position. Although not shown, the position deviation amount ΔXa is the difference between the center position (measured position) of the mark image captured in the captured image when the mark MA is imaged at the lower position and the design position in the X-axis direction. The position deviation amount ΔXc is the difference between the center position (measured position) of the mark image captured in the captured image and the design position in the Y-axis direction when the mark MA is imaged at the upper position. Then, the CPU 71 calculates the tilt RA of the camera 60 around the X axis by the following equation (17) based on the calculated positional deviation amounts ΔYa and ΔYb and the predetermined quantity b described above, and the calculated positional deviation amount ΔXa, The inclination RB of the camera 60 around the Y axis is calculated by the following equation (18) based on ΔXb and the predetermined amount b (step S430). The equation (17) can be easily derived from FIG. 22. The same applies to equation (18).
ステップ230は、算出した位置XをパラメータHCXに設定し、位置YをパラメータHCYに設定し、傾きRAをパラメータHCRAに設定し、傾きRBをパラメータHCRBに設定する。ステップS240は、各々設定したリンクパラメータHCX,HCY,HCRA,HCRBをカメラ用リンクパラメータとして登録する。これにより、カメラ60が設計値通りに取り付けられていなかった場合に、カメラ位置制御処理において順運動学によりカメラ60の取付位置や取付角度に起因した誤差を含むカメラ60の画像中心の推定位置を計算することができる。この推定位置に基づいて目標位置を補正して制御することで、カメラ60の画像中心を精度よく目標位置に合わせることが可能となる。
In step 230, the calculated position X is set to the parameter HCX, the position Y is set to the parameter HCY, the inclination RA is set to the parameter HCRA, and the inclination RB is set to the parameter HCRB. In step S240, the set link parameters HCX, HCY, HCRA, and HCRB are registered as camera link parameters. As a result, when the camera 60 is not mounted according to the design value, the estimated position of the image center of the camera 60 including an error due to the mounting position and mounting angle of the camera 60 is obtained by forward kinematics in the camera position control process. Can be calculated. By correcting and controlling the target position based on this estimated position, it is possible to accurately align the image center of the camera 60 with the target position.
ここで、実施形態の主要な要素と請求の範囲に記載した本開示の主要な要素との対応関係について説明する。即ち、本実施形態では、第1アーム21が第1アームに相当し、第1関節軸J1が第1軸に相当し、第2アーム22が第2アームに相当し、第2関節軸J2が第2軸に相当し、シャフト23がシャフトに相当し、第3関節軸J3が第3軸に相当し、カメラ60がカメラに相当する。
Here, the correspondence between the main elements of the embodiment and the main elements of the present disclosure described in the claims will be described. That is, in the present embodiment, the first arm 21 corresponds to the first arm, the first joint axis J1 corresponds to the first axis, the second arm 22 corresponds to the second arm, and the second joint axis J2 corresponds to the first arm. The shaft 23 corresponds to the shaft, the third joint axis J3 corresponds to the third axis, and the camera 60 corresponds to the camera.
なお、本開示は上述した実施形態に何ら限定されることはなく、本開示の技術的範囲に属する限り種々の態様で実施し得ることはいうまでもない。
It should be noted that the present disclosure is not limited to the above-described embodiment, and it goes without saying that the present disclosure can be carried out in various embodiments as long as it belongs to the technical scope of the present disclosure.
例えば、上述した実施形態では、カメラ位置X,Yとカメラ傾きRA,RBとを測定し、それぞれカメラ用リンクパラメータHCX,HCY,HCRA,HCRBに反映させるものとした。しかし、カメラ位置X,Yとカメラ傾きRA,RBとのうちいずれか一方の測定を省略してもよい。
For example, in the above-described embodiment, the camera positions X and Y and the camera tilts RA and RB are measured and reflected in the camera link parameters HCX, HCY, HCRA and HCRB, respectively. However, the measurement of either the camera positions X and Y and the camera tilt RA and RB may be omitted.
以上説明したように、本開示のスカラロボットの制御方法は、基台と、前記基台に設けられ、第1軸周りに回動する第1アームと、前記第1アームに設けられ、前記第1軸と平行な第2軸周りに回動する第2アームと、前記第2アームに対して前記第2軸と平行な第3軸の軸方向に移動するシャフトと、光軸が前記第3軸と平行となるように前記第2アームに取り付けられたカメラと、を備え、前記第1アーム,前記第2アーム,前記シャフトを第1リンク,第2リンク,第3リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、前記第3軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、前記測定したカメラの取付角度に基づく補正値を前記第2リンクから前記第3リンクへの変換のための前記姿勢パラメータに登録することを要旨とする。
As described above, the control method of the scalar robot of the present disclosure is provided on the base, the first arm provided on the base and rotating around the first axis, and the first arm provided on the first arm. A second arm that rotates around a second axis that is parallel to one axis, a shaft that moves in the axial direction of the third axis that is parallel to the second axis with respect to the second arm, and an optical axis that is the third. A camera attached to the second arm so as to be parallel to the axis is provided, and the first arm, the second arm, and the shaft are used as the first link, the second link, and the third link between the links. It is a control method of a scalar robot that controls the position of the camera by using a position parameter showing the relationship of position and a posture parameter showing the relationship of posture between each link, and is a control method of the optical axis with respect to the third axis. The gist is to measure the tilt as the mounting angle of the camera and register the correction value based on the measured mounting angle of the camera in the posture parameter for conversion from the second link to the third link.
この本開示のスカラロボットの制御方法によれば、カメラの取付角度を測定して、測定値に基づく補正値を対応するリンクへの変換のための姿勢パラメータに反映させることで、カメラに取付角度に誤差が生じていても、カメラの位置制御を精度よく行なうことが可能となる。
According to the control method of the scalar robot of the present disclosure, the mounting angle of the camera is measured and the correction value based on the measured value is reflected in the posture parameter for conversion to the corresponding link. Even if there is an error in the camera, it is possible to control the position of the camera with high accuracy.
こうした本開示のスカラロボットの制御方法において、前記第3軸と直交する方向における前記光軸の位置を前記カメラの取付位置として測定し、前記測定したカメラの取付位置に基づく補正値を前記第2リンクから前記第3リンクへの変換のための前記位置パラメータに登録するものとしてもよい。こうすれば、カメラの取付角度に加えて取付位置に誤差が生じていても、カメラの位置制御を精度よく行なうことができる。
In the control method of the SCARA robot of the present disclosure, the position of the optical axis in the direction orthogonal to the third axis is measured as the mounting position of the camera, and the correction value based on the measured mounting position of the camera is the second. It may be registered in the position parameter for conversion from the link to the third link. By doing so, even if there is an error in the mounting position in addition to the mounting angle of the camera, the position control of the camera can be performed accurately.
さらに、本開示のスカラロボットの制御方法において、前記第2アームの回動位置と前記第1アームのアーム周りのねじれ量との関係を記憶し、前記第2アームの回動位置に対応する前記第1アームのねじれ量に基づく補正値を、前記基台を第0リンクとして該第0リンクから前記第1リンクへの変換のための前記姿勢パラメータに登録するものとしてもよい。こうすれば、第1アームのねじれが生じていても、カメラの位置制御を精度よく行なうことができる。
Further, in the SCARA robot control method of the present disclosure, the relationship between the rotation position of the second arm and the twist amount around the arm of the first arm is memorized, and the rotation position of the second arm corresponds to the above-mentioned. The correction value based on the twist amount of the first arm may be registered in the posture parameter for conversion from the 0th link to the 1st link with the base as the 0th link. By doing so, even if the first arm is twisted, the position of the camera can be controlled accurately.
また、本開示のスカラロボットの制御方法において、前記第2アームの回動位置に対応する前記第1アームのたわみ量と前記第2アームのたわみ量とを記憶し、前記第2アームの回動位置に対応する前記第1アームのたわみ量に基づく補正値を、前記基台を第0リンクとして該第0リンクから前記第1リンクへの変換のための前記姿勢パラメータに登録すると共に、前記第2アームの回動位置に対応する前記第2アームのたわみ量に基づく補正値を、前記第1リンクから前記第2リンクへの変換のための前記姿勢パラメータに登録するものとしてもよい。こうすれば、第1アームや第2アームにたわみが生じていても、カメラの位置制御を精度よく行なうことができる。
Further, in the SCARA robot control method of the present disclosure, the deflection amount of the first arm and the deflection amount of the second arm corresponding to the rotation position of the second arm are stored, and the rotation of the second arm is stored. The correction value based on the amount of deflection of the first arm corresponding to the position is registered in the posture parameter for conversion from the 0th link to the 1st link with the base as the 0th link, and the first A correction value based on the amount of deflection of the second arm corresponding to the rotation position of the two arms may be registered in the posture parameter for conversion from the first link to the second link. By doing so, even if the first arm and the second arm are bent, the position of the camera can be controlled accurately.
本開示は、スカラロボットの製造産業などに利用可能である。
This disclosure can be used in the SCARA robot manufacturing industry and the like.
1 スカラロボットシステム、2 作業台、10 ロボット本体、11 基台、20 スカラアーム、21 第1アーム、22 第2アーム、23 シャフト、24 ワーク保持部、30 第1アーム駆動部、32 モータ、34 エンコーダ、40 第2アーム駆動部、42 モータ、44 エンコーダ、50 シャフト駆動部、52a,52b モータ、54a,54b エンコーダ、60 カメラ、70 制御装置、71 CPU、72 ROM、73 RAM、74 記憶装置、100 測定用治具、J1 第1関節軸、J2 第2関節軸、J3 第3関節軸、 MA マーク、W ワーク。
1 SCARA robot system, 2 work table, 10 robot body, 11 base, 20 scalar arm, 21 1st arm, 22 2nd arm, 23 shaft, 24 work holding unit, 30 1st arm drive unit, 32 motor, 34 Encoder, 40 2nd arm drive unit, 42 motor, 44 encoder, 50 shaft drive unit, 52a, 52b motor, 54a, 54b encoder, 60 camera, 70 control device, 71 CPU, 72 ROM, 73 RAM, 74 storage device, 100 Measuring jig, J1 1st joint axis, J2 2nd joint axis, J3 3rd joint axis, MA mark, W work.
Claims (4)
- 基台と、
前記基台に設けられ、第1軸周りに回動する第1アームと、
前記第1アームに設けられ、前記第1軸と平行な第2軸周りに回動する第2アームと、
前記第2アームに対して前記第2軸と平行な第3軸の軸方向に移動するシャフトと、
光軸が前記第3軸と平行となるように前記第2アームに取り付けられたカメラと、
を備え、前記第1アーム,前記第2アーム,前記シャフトを第1リンク,第2リンク,第3リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
前記第3軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
前記測定したカメラの取付角度に基づく補正値を前記第2リンクから前記第3リンクへの変換のための前記姿勢パラメータに登録する、
スカラロボットの制御方法。 Base and
A first arm provided on the base and rotating around a first axis,
A second arm provided on the first arm and rotating around a second axis parallel to the first axis,
A shaft that moves in the axial direction of the third axis parallel to the second arm with respect to the second arm,
A camera attached to the second arm so that the optical axis is parallel to the third axis,
The first arm, the second arm, and the shaft are designated as the first link, the second link, and the third link, and the positional parameter indicating the positional relationship between the links and the posture relationship between the links are represented. It is a control method of a SCARA robot that controls the position of the camera using the posture parameters.
The inclination of the optical axis with respect to the third axis is measured as the mounting angle of the camera.
A correction value based on the measured mounting angle of the camera is registered in the posture parameter for conversion from the second link to the third link.
How to control a SCARA robot. - 請求項1に記載のスカラロボットの制御方法であって、
前記第3軸と直交する方向における前記光軸の位置を前記カメラの取付位置として測定し、
前記測定したカメラの取付位置に基づく補正値を前記第2リンクから前記第3リンクへの変換のための前記位置パラメータに登録する、
スカラロボットの制御方法。 The method for controlling a SCARA robot according to claim 1.
The position of the optical axis in the direction orthogonal to the third axis is measured as the mounting position of the camera.
A correction value based on the measured mounting position of the camera is registered in the position parameter for conversion from the second link to the third link.
How to control a SCARA robot. - 請求項1または2に記載のスカラロボットの制御方法であって、
前記第2アームの回動位置と前記第1アームのアーム周りのねじれ量との関係を記憶し、
前記第2アームの回動位置に対応する前記第1アームのねじれ量に基づく補正値を、前記基台を第0リンクとして該第0リンクから前記第1リンクへの変換のための前記姿勢パラメータに登録する、
スカラロボットの制御方法。 The method for controlling a SCARA robot according to claim 1 or 2.
The relationship between the rotation position of the second arm and the amount of twist around the arm of the first arm is memorized.
The posture parameter for converting the correction value based on the twist amount of the first arm corresponding to the rotation position of the second arm from the 0th link to the 1st link with the base as the 0th link. To register with
How to control a SCARA robot. - 請求項1ないし3いずれか1項に記載のスカラロボットの制御方法であって、
前記第2アームの回動位置に対応する前記第1アームのたわみ量と前記第2アームのたわみ量とを記憶し、
前記第2アームの回動位置に対応する前記第1アームのたわみ量に基づく補正値を、前記基台を第0リンクとして該第0リンクから前記第1リンクへの変換のための前記姿勢パラメータに登録すると共に、前記第2アームの回動位置に対応する前記第2アームのたわみ量に基づく補正値を、前記第1リンクから前記第2リンクへの変換のための前記姿勢パラメータに登録する、
スカラロボットの制御方法。 The method for controlling a SCARA robot according to any one of claims 1 to 3.
The amount of deflection of the first arm corresponding to the rotation position of the second arm and the amount of deflection of the second arm are stored.
The posture parameter for converting the correction value based on the amount of deflection of the first arm corresponding to the rotation position of the second arm from the 0th link to the 1st link with the base as the 0th link. The correction value based on the amount of deflection of the second arm corresponding to the rotation position of the second arm is registered in the posture parameter for conversion from the first link to the second link. ,
How to control a SCARA robot.
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2022566526A JP7524350B2 (en) | 2020-12-01 | Method for controlling a SCARA robot | |
PCT/JP2020/044678 WO2022118374A1 (en) | 2020-12-01 | 2020-12-01 | Method for controlling scara robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/JP2020/044678 WO2022118374A1 (en) | 2020-12-01 | 2020-12-01 | Method for controlling scara robot |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2022118374A1 true WO2022118374A1 (en) | 2022-06-09 |
Family
ID=81853003
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2020/044678 WO2022118374A1 (en) | 2020-12-01 | 2020-12-01 | Method for controlling scara robot |
Country Status (1)
Country | Link |
---|---|
WO (1) | WO2022118374A1 (en) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6344206A (en) * | 1986-07-23 | 1988-02-25 | アメリカン テレフオン アンド テレグラフ カムパニ− | Calibration of scalar type robot |
JPH05204423A (en) * | 1992-01-29 | 1993-08-13 | Nec Home Electron Ltd | Coordinate correcting method for robot device provided with visual device |
JPH0655485A (en) * | 1992-08-11 | 1994-03-01 | Citizen Watch Co Ltd | Arm inclination amount measuring device of articulated robot |
JPH06206186A (en) * | 1993-01-12 | 1994-07-26 | Citizen Watch Co Ltd | Calibration of handling precision of horizontal articulated type robot equipped with visual sensor |
JP2012006125A (en) * | 2010-06-28 | 2012-01-12 | Seiko Epson Corp | Method for correcting coordinate value of horizontal articulated robot |
US20150142171A1 (en) * | 2011-08-11 | 2015-05-21 | Siemens Healthcare Diagnostics Inc. | Methods and apparatus to calibrate an orientation between a robot gripper and a camera |
JP2018094653A (en) * | 2016-12-09 | 2018-06-21 | セイコーエプソン株式会社 | Control device, robot, and robot system |
-
2020
- 2020-12-01 WO PCT/JP2020/044678 patent/WO2022118374A1/en active Application Filing
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6344206A (en) * | 1986-07-23 | 1988-02-25 | アメリカン テレフオン アンド テレグラフ カムパニ− | Calibration of scalar type robot |
JPH05204423A (en) * | 1992-01-29 | 1993-08-13 | Nec Home Electron Ltd | Coordinate correcting method for robot device provided with visual device |
JPH0655485A (en) * | 1992-08-11 | 1994-03-01 | Citizen Watch Co Ltd | Arm inclination amount measuring device of articulated robot |
JPH06206186A (en) * | 1993-01-12 | 1994-07-26 | Citizen Watch Co Ltd | Calibration of handling precision of horizontal articulated type robot equipped with visual sensor |
JP2012006125A (en) * | 2010-06-28 | 2012-01-12 | Seiko Epson Corp | Method for correcting coordinate value of horizontal articulated robot |
US20150142171A1 (en) * | 2011-08-11 | 2015-05-21 | Siemens Healthcare Diagnostics Inc. | Methods and apparatus to calibrate an orientation between a robot gripper and a camera |
JP2018094653A (en) * | 2016-12-09 | 2018-06-21 | セイコーエプソン株式会社 | Control device, robot, and robot system |
Also Published As
Publication number | Publication date |
---|---|
JPWO2022118374A1 (en) | 2022-06-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6468741B2 (en) | Robot system and robot system calibration method | |
JP6429473B2 (en) | Robot system, robot system calibration method, program, and computer-readable recording medium | |
JP4267005B2 (en) | Measuring apparatus and calibration method | |
CN107614195B (en) | Assembling device and assembling method for gear mechanism | |
CN113601158B (en) | Bolt feeding pre-tightening system based on visual positioning and control method | |
JP2009269110A (en) | Assembly equipment | |
JP5618770B2 (en) | Robot calibration apparatus and calibration method | |
US20180161983A1 (en) | Control device, robot, and robot system | |
CN109952178B (en) | Working robot and working position correction method | |
US11679508B2 (en) | Robot device controller for controlling position of robot | |
JP2023108062A (en) | Control device, robot device, control method, and program | |
US7957834B2 (en) | Method for calculating rotation center point and axis of rotation, method for generating program, method for moving manipulator and positioning device, and robotic system | |
JP2006082171A (en) | Tool location correcting method for articulated robot | |
JP2012223871A (en) | Origin correction method and system of robot joint | |
JP7281910B2 (en) | robot control system | |
KR20070122271A (en) | System for controlling position of robot and method for controlling thereof | |
WO2022118374A1 (en) | Method for controlling scara robot | |
CN113043332B (en) | Arm shape measuring system and method of rope-driven flexible robot | |
JP7524350B2 (en) | Method for controlling a SCARA robot | |
JP4212783B2 (en) | Robot coordinate calibration system | |
JP7384653B2 (en) | Control device for robot equipment that controls the position of the robot | |
JP6578671B2 (en) | ROBOT, ROBOT CONTROL METHOD, AND ROBOT CONTROL DEVICE | |
CN110977950B (en) | Robot grabbing and positioning method | |
JP7145332B2 (en) | Robot control system and robot control method | |
WO2022024243A1 (en) | Scara robot system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 20964228 Country of ref document: EP Kind code of ref document: A1 |
|
ENP | Entry into the national phase |
Ref document number: 2022566526 Country of ref document: JP Kind code of ref document: A |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 20964228 Country of ref document: EP Kind code of ref document: A1 |