WO2022118374A1 - スカラロボットの制御方法 - Google Patents

スカラロボットの制御方法 Download PDF

Info

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
Application number
PCT/JP2020/044678
Other languages
English (en)
French (fr)
Inventor
武史 藤城
Original Assignee
株式会社Fuji
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社Fuji filed Critical 株式会社Fuji
Priority to PCT/JP2020/044678 priority Critical patent/WO2022118374A1/ja
Priority to JP2022566526A priority patent/JPWO2022118374A1/ja
Publication of WO2022118374A1 publication Critical patent/WO2022118374A1/ja

Links

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

第２アームにカメラが取り付けられたスカラロボットの制御方法は、第１アーム，第２アーム，シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いてカメラの位置を制御する。この方法において、第３軸に対する光軸の傾きをカメラの取付角度として測定し、測定したカメラの取付角度に基づく補正値を第２リンクから第３リンクへの変換のための姿勢パラメータに登録する。

Description

スカラロボットの制御方法
　本明細書は、スカラロボットの制御方法について開示する。
　従来、作業座標に設けられた基準マークをアームに取り付けられたカメラにより計測し、ロボット座標による基準マークの計測位置と予め測定された作業座標における基準マークの位置との差から設置誤差を推定し、これを補正することによりロボットを制御するものが提案されている（例えば、特許文献１参照）。
特開平４－２１１８０７号公報
　上述した特許文献１には、移動体に搭載されたロボットのように、ロボットに設置誤差がある場合の対応ついては記載されているものの、カメラに取付誤差がある場合の対応については何ら言及されていない。
　本開示は、アームにカメラが取り付けられたスカラロボットにおいて、カメラに取付誤差が生じていてもより高い精度でカメラの位置を制御することができるスカラロボットの制御方法を提供することを主目的とする。
　本開示は、上述の主目的を達成するために以下の手段を採った。
　本開示のスカラロボットの制御方法は、
　基台と、
　前記基台に設けられ、第１軸周りに回動する第１アームと、
　前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
　前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
　光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、
　を備え、
　前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
　前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
　前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録する、
　ことを要旨とする。
　この本開示のスカラロボットの制御方法は、第２アームにカメラが取り付けられたスカラロボットにおいて、第１アーム，第２アーム，シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いてカメラの位置を制御するものである。この方法は、第３軸に対する光軸の傾きをカメラの取付角度として測定し、測定したカメラの取付角度に基づく補正値を第２リンクから第３リンクへの変換のための姿勢パラメータに登録する。このように、カメラの取付角度を測定して、測定値に基づく補正値を対応するリンクへの変換のための姿勢パラメータに反映させることで、カメラに取付角度に誤差が生じていても、カメラの位置制御を精度よく行なうことが可能となる。
スカラロボットの外観斜視図である。 ロボット本体の側面図である。 ロボット本体と制御装置との電気的な接続関係を示すブロック図である。 カメラ位置制御処理の一例を示すフローチャートである。 ロボットの座標系を説明する説明図である。 ロボットの座標系を説明する説明図である。 第１アームに対して生じるねじれを説明する説明図である。 第２アームが水平旋回した際のアーム部の重心位置のＹ1軸方向成分Ｌt’を説明する説明図である。 第１アームに生じるねじれモーメントを説明する説明図である。 第１アームおよび第２アームに対して生じるたわみを説明する説明図である。 第２アームが水平旋回した際のアーム部の重心位置のＸ1軸方向成分Ｌd1’を説明する説明図である。 第１アームに生じるたわみモーメントを説明する説明図である。 ワーク高さを説明する説明図である。 手先用リンクパラメータ表を示す説明図である。 カメラ用リンクパラメータ表を示す説明図である。 カメラの光軸がワークの置かれる基準面に対して傾いた状態で高さが異なるワークを撮像した場合に生じるワークに対する画像中心のズレを説明する説明図である。 カメラ用リンクパラメータ設定手順の一例を示す説明図である。 手先に測定用治具が取り付けられた状態を示す部分外観図である。 カメラ位置測定処理の一例を示すフローチャートである。 カメラ位置の測定の様子を示す説明図である。 カメラ傾き測定処理の一例を示すフローチャートである。 カメラ傾きの測定の様子を示す説明図である。
　次に、本開示を実施するための形態について図面を参照しながら説明する。
　図１は、スカラロボットの外観斜視図である。図２は、ロボット本体の側面図である。図３は、ロボット本体と制御装置との電気的な接続関係を示すブロック図である。
　スカラロボット１は、ワークＷに対して所定の作業（例えば、ワークＷをピックアップして搬送する搬送作業や、ワークＷを保持して組付対象物に組み付ける組付け作業など）を行なうものとして構成されている。スカラロボット１は、ロボット本体１０（図１および図２参照）と、ロボット本体１０を制御する制御装置７０（図３参照）と、を備える。
　ロボット本体１０は、基台１１とスカラアーム２０とカメラ６０とを備える。
　基台１１は、作業台２に固定されており、スカラアーム２０の基端側を支持する。スカラアーム２０は、第１アーム２１と第１アーム駆動部３０と第２アーム２２と第２アーム駆動部４０とシャフト２３とシャフト駆動部５０とを備える。第１アーム２１は、基端部が第１関節軸Ｊ１を介して基台１１に連結され、第１関節軸Ｊ１の回動により基台１１に対して水平面内で回動（水平旋回）可能に構成される。第２アーム２２は、基端部が第２関節軸Ｊ２を介して第１アーム２１の先端部に連結され、第２関節軸Ｊ２の回動により第１アーム２１に対して水平面内で回動（水平旋回）可能に構成される。シャフト２３は、第２アーム２２の先端部に第３関節軸Ｊ３を介して連結され、第２アーム２２に対して第３関節軸Ｊ３の軸周りに回転可能かつ第３関節軸Ｊ３の軸方向に沿って昇降可能に構成される。本実施形態のスカラロボットでは、シャフト２３の先端には、ワークＷをピックアップして保持するワーク保持部２４が設けられている。なお、ワーク保持部２４としては、例えば、負圧によりワークＷを吸着する吸着ノズルや、一対の爪によりワークＷを把持するメカニカルチャック、電磁石によりワークＷを吸着する電磁チャックなどを挙げることができる。
　第１アーム駆動部３０は、モータ３２とエンコーダ３４とを備える。モータ３２の回転軸は、図示しない減速機を介して第１関節軸Ｊ１に連結されている。第１アーム駆動部３０は、モータ３２を駆動することで減速機を介して第１関節軸Ｊ１に伝達されるトルクにより、第１関節軸Ｊ１を支点に第１アーム２１を回動させる。エンコーダ３４は、モータ３２の回転軸に取り付けられ、モータ３２の回転変位量を検出するロータリエンコーダとして構成される。
　第２アーム駆動部４０は、第１アーム駆動部３０と同様に、モータ４２とエンコーダ４４とを備える。モータ４２の回転軸は、図示しない減速機を介して第２関節軸Ｊ２に連結されている。第２アーム駆動部４０は、モータ４２を駆動することで減速機を介して第２関節軸Ｊ２に伝達されるトルクにより、第２関節軸Ｊ２を支点に第２アーム２２を回動させる。エンコーダ４４は、モータ４２の回転軸に取り付けられ、モータ４２の回転変位量を検出するロータリエンコーダとして構成される。
　シャフト駆動部５０は、図３に示すように、モータ５２ａ，５２ｂとエンコーダ５４ａ，５４ｂとを備える。モータ５２ａの回転軸は、図示しないベルトを介してシャフト２３に接続されている。シャフト駆動部５０は、モータ５２ａを駆動することによりシャフト２３を軸周りに回転させる。モータ５２ｂの回転軸は、図示しないボールねじ機構を介してシャフト２３に接続されている。シャフト駆動部５０は、モータ５２ｂを駆動することにより、ボールねじ機構によりモータ５２ｂの回転運動が直動運動に変換されることでシャフト２３を上下に昇降させる。エンコーダ５４ａは、シャフト２３の回転変位量を検出するロータリエンコーダとして構成される。エンコーダ５４ｂは、シャフト２３の昇降位置を検出するリニアエンコーダとして構成される。
　カメラ６０は、第２アーム２２の先端部側面に取り付けられている。カメラ６０は、作業対象物であるワークＷを撮像し、その撮像画像を制御装置７０へ出力する。制御装置７０は、撮像画像を処理することによりワークＷの位置や姿勢を認識することができる。なお、カメラ６０は第２アーム２２に取り付けられているから、本実施形態では、スカラアーム２０は、カメラ６０を上下方向に移動させることはできない。
　制御装置７０は、図３に示すように、ＣＰＵ７１と、処理プログラムを記憶するＲＯＭ７２と、ワークメモリとしてのＲＡＭ７３と、ＨＤＤやＳＳＤ等の記憶装置７４と、入出力インタフェース（図示せず）とを備える。制御装置７０には、エンコーダ３４，４４，５４ａ，５４ｂからの位置信号やカメラ６０からの画像信号などが入出力インタフェースを介して入力される。制御装置７０からは、モータ３２，４２，５２ａ，５２ｂへの駆動信号などが入出力インタフェースを介して出力される。
　次に、こうして構成されたスカラロボット１の動作について説明する。特に、作業台２上の被写体であるワークＷを撮像するためのカメラ６０の位置制御について説明する。図４は、制御装置７０のＣＰＵ７１により実行されるカメラ位置制御処理の一例を示すフローチャートである。
　カメラ位置制御処理が実行されると、制御装置７０のＣＰＵ７１は、まず、図示しないフィーダから供給されるワークＷを撮像するための基台１１（後述するベース座標系Σ0）から見たカメラ６０の画像中心の目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）を設定する（ステップＳ１００）。ステップＳ１００は、例えば、外部のコンピュータで作成されたプログラムのデータを読み込むことにより行なわれる。続いて、ＣＰＵ７１は、カメラ６０の画像中心の目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）に対して逆運動学を解くことにより、カメラ６０の画像中心を目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）に一致させるための第１関節軸Ｊ１の角度θJ1と第２関節軸Ｊ２の角度θJ2とを算出する（ステップＳ１１０）。
　次に、ＣＰＵ７１は、ステップＳ１１０で算出した第２関節軸Ｊ２の角度θJ2に基づいて第１アーム２１のねじれ角度θtとたわみ角度θd1とを算出すると共に（ステップＳ１２０）、第２アーム２２のたわみ角度θd2を算出する（ステップＳ１３０）。いま、図５および図６に示すように、基台１１（第０リンク）にベース座標系Σ0を設定し、第１アーム２１，第２アーム２２，シャフト２３，手先（ワーク保持部２４）を第１リンク，第２リンク，第３リンク，第４リンクとしてそれぞれリンク座標系を設定する場合を考える。ここで、リンク座標系Σ1は、第１アーム２１の根元（第１関節軸Ｊ１）に設定される。リンク座標系Σ2は、第２アーム２２の根元（第２関節軸Ｊ２）に設定される。リンク座標系Σ3は、第２アーム２２とシャフト２３との連結部分に設定される。リンク座標系（ハンド座標系）Σ4は、手先（ワーク保持部２４）に設定される。各座標系は、第１アーム２１および第２アーム２２が一直線状に伸びた状態をホームポジションとする。また、図中、Ｘ0，Ｘ1，Ｘ2，Ｘ3，Ｘ4は、それぞれの座標系のＸ軸である。Ｙ0，Ｙ1，Ｙ2，Ｙ3，Ｙ4は、それぞれの座標系のＹ軸である。Ｚ0，Ｚ1，Ｚ2，Ｚ3，Ｚ4は、それぞれの座標系のＺ軸である。
　第１アーム２１のねじれ角度θtは、リンク座標系Σ1において、第１関節軸Ｊ1の軸方向をＺ1軸方向とし、第１アーム２１の中心線に沿った方向をＸ1軸方向とし、Ｘ1軸およびＺ1軸に直交する方向をＹ1軸方向とした場合、図７に示すように、第１アーム２１のＸ1軸周りの変位角度として定義される。そして、第１アーム２１のねじれ角度θtは、第１アーム２１のねじれ角度θtと第２関節軸Ｊ２の角度θJ2とアーム質量Ｍ１との関係をあらわす次式（１）および（２）のねじれモデル式を用いて算出される。アーム質量Ｍ１は、スカラアーム２０の第１関節軸Ｊ１より先端側のアーム部の質量であり、ワーク保持部２４がワークＷを保持している場合には当該ワークＷの質量ｍも含み、ワークＷの質量ｍに応じた変数となる。「ｇ」は重力加速度である。「Ｌt・ｓｉｎθJ2」は、図８に示すように、アーム部の重心位置ＬtにおけるＹ1軸方向成分である。「Ｉt」は、図９に示すように、アーム部に作用する重力による荷重によって第１アーム２１をＸ1軸周りに回転させようとするモーメント（ねじれモーメント）であり、重心位置ＬtがＸ1軸上にあるときには値０であり、重心位置ＬtがＸ1軸からＹ1軸方向に離れるにつれて大きくなる。すなわち、ねじれモーメントＩtは、第２関節軸Ｊ２の角度θJ2が０度（基準角）のときに最小となり、第２関節軸Ｊ２の角度が－９０度のときと＋９０度のときに最大となる。「Ｋt」は、ねじれ剛性であり、予め実験などにより求められる。
Figure JPOXMLDOC01-appb-M000001
　第１アーム２１のたわみ角度θd1は、図１０に示すように、第１アーム２１のＹ1軸周りの変位角度として定義される。そして、第１アーム２１のたわみ角度θd1は、第１アーム２１のたわみ角度θd1と第２関節軸Ｊ２の角度θJ2とアーム部の質量である上述したアーム質量Ｍ１との関係をあらわす次式（３）および（４）のたわみモデル式を用いて算出される。式中、「Ｌd1・ｃｏｓθJ2」は、図１１に示すように、アーム部の重心位置ＬtにおけるＸ1軸方向成分である。「Ｉd1」は、図１２に示すように、アーム部に作用する重力による荷重によって第１アーム２１をＹ1軸周りに回転させようとするモーメント（たわみモーメント）であり、重心位置Ｌd1が第１関節軸Ｊ１からＸ1軸方向に離れるにつれて大きくなる。すなわち、たわみモーメントＩd1は、第２関節軸Ｊ２の角度が±１８０度のときに最小となり、第２関節軸Ｊ２の角度が０度（基準角）のときに最大となる。「Ｋd1」は、たわみ剛性であり、予め実験などにより求められる。
Figure JPOXMLDOC01-appb-M000002
　第２アーム２２のたわみ角度θd2は、図１０に示すように、第２アーム２２のＹ2軸周りの変位角度として定義される。そして、第２アーム２２のたわみ角度θd2は、第２アーム２２のたわみ角度θd2とアーム質量Ｍ２との関係をあらわす次式（５）および（６）のたわみモデル式を用いて算出される。ここで、アーム質量Ｍ２は、スカラアーム２０の第２関節軸Ｊ２より先端側のアーム部の質量であり、ワーク保持部２４がワークＷを保持している場合には当該ワークＷの質量ｍも含み、ワークＷの質量ｍに応じた変数となる。「Ｉd2」は、アーム部に作用する重力による荷重によって第２アーム２２をＹ2軸周りに回転させようとするモーメント（たわみモーメント）であり、第２関節軸Ｊ２の角度θd2とは無関係に、重心位置Ｌd2（第２関節軸Ｊ２の中心から重心位置までの距離）とアーム質量Ｍ２とによって定まる。「Ｋd2」は、たわみ剛性であり、予め実験などにより求められる。
Figure JPOXMLDOC01-appb-M000003
　これらのねじれモデル式およびたわみモデル式は、記憶装置７４に記憶されている。ステップＳ１１０，Ｓ１２０において、ＣＰＵ７１は、記憶装置７４から対応するモデル式を読み出すことにより、ねじれ角度θtおよびたわみ角度θd1，θd2を算出する。
　次に、ＣＰＵ７１は、第１関節軸Ｊ１の角度θJ1と第２関節軸Ｊ２の角度θJ2と第１アーム２１のねじれ角度θtおよびたわみ角度θd1と第２アーム２２のたわみ角度θd2とに基づいて順運動学を解くことにより、カメラ６０の画像中心の推定位置（Ｘ0，Ｙ0，Ｚ0）を算出する（ステップＳ１４０）。すなわち、ＣＰＵ７１は、画像中心が目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）と一致するように逆運動学により算出される第１および第２関節軸Ｊ１，Ｊ２の角度θJ1，θJ2を角度指令値として制御したと仮定した場合に、第１アーム２１のねじれ及びたわみや第２アーム２２のたわみに起因して目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）からズレる画像中心の推定位置（Ｘ0，Ｙ0，Ｚ0）を算出する。
　ＣＰＵ７１は、画像中心の推定位置（Ｘ0，Ｙ0，Ｚ0）を算出すると、算出した画像中心の推定位置（Ｘ0，Ｙ0，Ｚ0）とステップＳ１００で取得した画像中心の目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）との差分をとることにより、画像中心の位置ズレ量（ΔＸ0，ΔＹ0，ΔＺ0）を算出する（ステップＳ１５０）。続いて、ＣＰＵ７１は、画像中心の目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）を位置ズレ量（ΔＸ0，ΔＹ0，ΔＺ0）の分だけオフセットすることにより目標位置を補正し（ステップＳ１６０）、補正した目標位置に対して逆運動学を解くことによりカメラ６０の画像中心を目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）に一致させるための第１関節軸Ｊ１の角度指令値θJ1＊と第２関節軸Ｊ２の角度指令値θJ2＊とを算出する（ステップＳ１７０）。そして、ＣＰＵ７１は、エンコーダ３４により検出される第１関節軸Ｊ１の角度が算出した角度指令値θJ1＊に一致すると共にエンコーダ４４により検出される第２関節軸Ｊ２の角度が算出した角度指令値θJ2＊に一致するようフィードバック制御により対応するモータ３１，３２を制御して（ステップＳ１８０）、カメラ位置制御処理を終了する。
　ＣＰＵ７１は、こうしてカメラ位置制御処理を実行すると、図示しないフィーダから供給されたワークＷをカメラ６０により撮像し、撮像画像に画像処理を施すことにより、ワークＷの位置および姿勢を認識する。そして、ＣＰＵ７１は、ワークＷをピックアップするための手先（ワーク保持部２４）の目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）を設定し、手先を目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）に移動させるための手先位置制御処理を実行する。目標位置（Ｘ0＊，Ｙ0＊）の設定は、認識したワークＷの位置および姿勢に基づいて行なわれる。目標位置Ｚ0＊の設定は、ワークＷの高さ情報（ワーク高さＷＨ）に基づいて行なわれる。ワークＷの高さ情報は、上述した画像処理に用いられる画像処理データの一つとして予め登録されている。したがって、高さ情報の取得は、画像処理データの中からワーク高さＷＨを読み出すことにより行なうことができる。例えば、ワーク保持部２４として吸着ノズルや電磁チャックを用いてワークＷをピックアップする場合、ワークＷのピック位置はワークＷの上面（吸着面）となるため、目標位置Ｚ0＊は、図１３に示すように、ワークＷが置かれる基準面Ｓ（固定値）に対してワーク高さＷＨだけ高い位置に設定される。なお、ワーク保持部２４としてワークＷの両側面を把持するメカニカルチャックを用いてワークＷをピックアップする場合、ワークＷのピック位置は、ワークＷの上面よりも低い位置となる。この場合、手先の目標位置Ｚ0＊は、別途、入力されたピック位置を用いて設定される。
　手先位置制御処理は、シャフト駆動部５０によるシャフト２３の昇降が行なわれる点を除いて、基本的には、上述したカメラ位置制御処理と同様の処理により行なわれる。すなわち、手先位置制御処理では、ＣＰＵ７１は、まず、手先を設定した目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）に移動させるための第１関節軸Ｊ１の角度θJ1と第２関節軸Ｊ２の角度θJ2とシャフト２３の角度（シャフト角度）θJ4とシャフト２３の昇降位置Ｚsとを逆運動学により算出する。続いて、ＣＰＵ７１は、算出したθJ1，θJ2，θJ4，Ｚsと第１アーム２１のねじれ角度θtおよびたわみ角度θd1と第２アーム２２のねじれ角度θd2とに基づいてねじれやたわみを考慮した手先の推定位置（Ｘ0，Ｙ0，Ｚ0）を順運動学により算出する。次に、ＣＰＵ７１は、手先の推定位置（Ｘ0，Ｙ0，Ｚ0）と目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）との差分をとることにより位置ズレ量（ΔＸ0，ΔＹ0，ΔＺ0）を算出する。次に、ＣＰＵ７１は、目標位置（Ｘ0＊，Ｙ0＊，Ｚ0＊）を位置ズレ量（ΔＸ0，ΔＹ0，ΔＺ0）の分だけオフセットすることにより目標位置を補正する。そして、ＣＰＵ７１は、補正した目標位置に手先を移動させるための第１関節軸Ｊ１の角度指令値θJ1＊と第２関節軸Ｊ２の角度指令値θJ2＊とシャフト２３の角度指令値θJ4＊とシャフト２３の昇降位置指令値Ｚs＊とを逆運動学により算出し、各指令値に基づいて対応するモータ３１，３２，３３ａ，３３ｂをフィードバック制御により制御する。
　ここで、カメラ位置制御処理や手先位置制御処理において、第１アーム２１のねじれ及びたわみや第２アーム２２のたわみを考慮したカメラ６０の画像中心や手先（ワーク保持部２４）の推定位置を求めるための順運動学の計算は、変数θJ1，θJ2，θt，θd1，θd2を同次変換行列に与えることにより行なうことができる。同次変換行列は、ロボット本体１０に設定した各座標系のうち隣接する座標系間の関係i-1Ｔi（ｉ＝１，２，３，４）であるリンクパラメータを求め、リンクパラメータに基づいてアーム全体の変換行列0Ｔ4を求めることにより導き出すことができる。
　図１４は、手先用リンクパラメータ表の一例を示す説明図である。図１５は、カメラ用リンクパラメータ表の一例を示す説明図である。手先用リンクパラメータ表およびカメラ用リンクパラメータ表において、図１４および図１５中、第ｉリンク（ｉ＝１，２，３，４）の各リンクパラメータ（ｄｘ、ｄｙ、ｄｚ、ｒｘ、ｒｙ、ｒｚ、ｒｘ２、ｒｙ２）は、座標系Σi-1から座標系Σiへ変換するためのパラメータである。ｄｘ，ｄｙ，ｄｚは、Ｘ軸方向，Ｙ軸方向，Ｚ軸方向の並進量を示すパラメータ（位置パラメータ）である。ｒｘ（ｒｘ２），ｒｙ（ｒｙ２），ｒｚは、Ｘ軸周り，Ｙ軸周り，Ｚ軸周りの回転量を示すパラメータ（姿勢パラメータ）である。
　また、図５または図６に示すように、Ｌ1は、Ｚ1軸とＺ2軸とのＸ1軸方向の距離（第１アーム２１のアーム長）を示す。Ｌ2は、Ｚ2軸とＺ3軸とのＸ2軸方向の距離（第２アーム２２のアーム長）を示す。Ｌz1は、ベース座標系Σ0の原点からリンク座標系Σ1の原点までのＺ0軸方向の距離を示す。Ｌz2は、リンク座標系Σ1の原点からリンク座標系Σ2の原点までのＺ1軸方向の距離を示す。ｔｈ１は、リンク座標系Σ1のＺ1軸周りの回転角であり、第１関節軸Ｊ１の角度θJ1に相当する。ｔｈ２は、リンク座標系Σ2のＺ2軸周りの回転角であり、第２関節軸Ｊ２の角度θJ2に相当する。ｔｈ４は、リンク座標系Σ4のＺ4軸周りの回転角であり、シャフト２３の角度（シャフト角度）θJ4に相当する。ｄ３は、リンク座標系Σ3の原点から手先までの距離であり、昇降位置Ｚsに相当する。Ｊ１ｒｘは、リンク座標系Σ1のＸ1軸周りの回転角を示す。Ｊ１ｒｙは、リンク座標系Σ1のＹ1軸周りの回転角を示す。Ｊ１ｒｘ２は、リンク座標系Σ1がＺ1軸周りにｔｈ１だけ回転した後のＸ1軸周りの回転角を示し、第１アーム２１のねじれ角度θtに相当する。Ｊ１ｒｙ２は、リンク座標系Σ1がＺ1軸周りにｔｈ１だけ回転した後のＹ1軸周りの回転角を示し、第１アーム２１のたわみ角度θd1に相当する。Ｊ２ｒｘは、リンク座標系Σ2のＸ2軸周りの回転角を示す。Ｊ２ｒｙは、リンク座標系Σ2のＹ2軸周りの回転角を示す。Ｊ２ｒｙ２は、リンク座標系Σ2がＺ2軸周りにｔｈ２回転した後のＹ2軸周りの回転角を示し、第２アーム２２のたわみ角度θd2に相当する。Ｊ３ｒｘは、リンク座標系Σ3のＸ3軸周りの回転角を示す。Ｊ３ｒｙは、リンク座標系Σ3のＹ3軸周りの回転角を示す。
　アーム全体の変換行列0Ｔ4は、リンクパラメータ表による各座標系間の関係i-1Ｔi（ｉ＝１，２，３，４）を用いて次式（７）により求めることができる。各座標系間の関係（行列）i-1Ｔiは、次式（８）により求めることができる。なお、ｄｘ，ｄｙ，ｄｚ，ｒｘ，ｒｙ，ｒｚ，ｒｘ２，ｒｙ２の各行列Ｔdx，Ｔdy，Ｔdz，Ｔrx，Ｔry，Ｔrz，Ｔrx2，Ｔry2を次式（９）～（１６）に示す。
Figure JPOXMLDOC01-appb-M000004
Figure JPOXMLDOC01-appb-I000005
　手先位置制御処理において、手先用リンクパラメータ表のＪ１ｒｘ２に第１アーム２１のねじれ角度θtを与え、Ｊ１ｒｙ２に第１アーム２１のたわみ角度θd1を与え、Ｊ２ｒｙ２に第２アーム２２のたわみ角度θd2を与えることで、順運動学（変換行列0Ｔ4）により第１アーム２１のねじれ及びたわみと第２アーム２２のたわみとに起因した誤差を含む手先の推定位置を計算することができる。
　カメラ用リンクパラメータは、図１５に示すように、リンク座標系Σ2からリンク座標系Σ3への変換用パラメータｄｘ，ｄｙ，ｄｚ，ｒｘ，ｒｙに、Ｌ2＋ＨＣＸ，ＨＣＹ，０，ＨＣＲＡ，ＨＣＲＢが設定され、リンク座標系Σ3からハンド座標系Σ4への変換用パラメータのｄｚ，ｒｚにそれぞれＨＣＺ，０が設定されている点で、手先用リンクパラメータとは異なる。ＨＣＸは、Ｘ2軸方向におけるカメラ６０の位置を示し、ＨＣＹは、Ｙ2軸方向におけるカメラ６０の位置を示す。また、ＨＣＲＡは、Ｘ3軸周りのカメラ６０の傾きを示し、ＨＣＲＢは、Ｙ3軸回りのカメラ６０の傾きを示す。ＨＣＺは、カメラ６０（レンズ先端部）から被写体であるワークＷの上面までの距離（ワーキングディスタンスＷＤ）を示す。
　カメラ位置制御処理において、カメラ用リンクパラメータ表のＪ１ｒｘ２に第１アーム２１のねじれ角度θtを与え、Ｊ１ｒｙ２に第１アーム２１のたわみ角度θd1を与え、Ｊ２ｒｙ２に第２アーム２２のたわみ角度θd2を与えることで、順運動学により第１アーム２１のねじれ及びたわみと第２アーム２２のたわみとに起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。また、カメラ６０のＸ2軸方向における位置ＸおよびＹ2軸方向における位置Ｙを予め測定しておき、カメラ用リンクパラメータ表のＨＣＸに位置Ｘを与え、ＨＣＹに位置Ｙを与えることで、順運動学によりカメラ６０の取付位置に起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。さらに、カメラ６０のＸ3軸周りにおける傾きＲＡおよびＹ3軸周りにおける傾きＲＢを予め測定しておき、カメラ用リンクパラメータ表のＨＣＲＡに傾きＲＡを与え、ＨＣＲＢに傾きＲＢを与えることで、順運動学によりカメラ６０の取付角度に起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。
　さらに、本実施形態では、カメラ用リンクパラメータ表のＨＣＺに与えられるワーキングディスタンスＷＤは、カメラ６０（レンズ先端部）とワークＷが置かれる基準面Ｓとの距離（固定値）から上述したワーク高さＷＨを減じることにより算出される。図１６に示すように、ワークＷが置かれる基準面Ｓに対してカメラ６０の光軸（図中、破線参照）が傾いている状態で高さの異なるワークＷを撮像すると、高さの違いΔＷＨに応じてそれぞれのワークＷの撮像画像の画像中心がズレる現象が生じる。本実施形態では、画像処理データからワーク高さＷＨを取得し、ＨＣＺに反映させることで、ワークＷの高さの違いによる画像中心のズレを修正することが可能となる。
　次に、カメラ６０の位置Ｘ，Ｙや傾きＲＡ，ＲＢを測定してカメラ用リンクパラメータに反映させるための手順について説明する。図１７は、カメラ用リンクパラメータ設定手順の一例を示す説明図である。カメラ用リンクパラメータ設定手順では、ステップＳ２００～Ｓ２４０により実行される。ステップＳ２００は、手先（ワーク保持部２４）に測定用治具１００を取り付ける。図１８は、手先（ワーク保持部２４）に測定用治具が取り付けられた状態を示す部分外観図である。図示するように、測定用治具１００は、装着箇所を中心としてシャフト２３に直交する方向に所定距離だけ離間すると共に周方向に所定間隔（９０度の間隔）をおいて配置された複数（４つ）のマークＭＡを有する。測定用治具１００は、シャフト２３に対して位相合わせされた状態で取り付けられ、各マークＭＡは、カメラ６０が設計値通りに取り付けられている場合に、カメラ６０の画像中心がマークＭＡの中心と一致するように配置されている。
　ステップＳ２１０は、測定用治具１００を用いてカメラ６０の位置Ｘ，Ｙを測定する。このステップＳ２１０は、制御装置７０のＣＰＵ７１により図１９に例示するカメラ位置測定処理を実行することにより行なわれる。カメラ位置測定処理では、ＣＰＵ７１は、まず、手先（ワーク保持部２４）を測定位置に移動させる（ステップＳ３００）。この処理は、マークＭＡがカメラ６０の被写界深度内となる昇降位置指令値Ｚs＊を設定してモータ５２ｂを制御することにより行なわれる。続いて、ＣＰＵ７１は、シャフト２３が所定ピッチずつ回転するようにモータ５２ａを制御し、シャフト２３が所定ピッチずつ回転する毎に、カメラ６０によりマークＭＡを撮像する処理を繰り返す（ステップＳ３１０）。次に、ＣＰＵ７１は、撮像画像を処理することによりカメラ６０により測定されたマークＭＡの実測位置と設計上のマークＭＡの位置（設計位置）との位置ズレ量ΔＸ，ΔＹを算出する（ステップＳ３２０）。図２０は、カメラ位置の測定の様子を示す説明図である。図中、白丸は、カメラ６０の取付位置が設計値通りであった場合に撮像画像に写るマーク画像を示し、黒丸は、カメラ６０でマークＭＡを実際に撮像することにより撮像画像に写ったマーク画像を示す。位置ズレ量ΔＸは、設計位置の円弧と実測位置の円弧とのＸ軸方向の位置の差である。位置ズレ量ΔＹは、設計位置の円弧と実測位置の円弧とのＹ軸方向の位置の差である。そして、ＣＰＵ７１は、カメラ６０の設計位置Ｘｄｖ，ＹｄｖにステップＳ１３０で算出した位置ズレ量ΔＸ，ΔＹを加算することにより、実際のカメラ６０の位置Ｘ，Ｙを算出する（ステップＳ３３０）。
　ステップＳ２２０は、測定用治具１００を用いてカメラ６０の傾きＲＡ，ＲＢを測定する。このステップＳ２２０は、制御装置７０のＣＰＵ７１により図２１に例示するカメラ傾き測定処理を実行することにより行なわれる。カメラ傾き測定処理では、ＣＰＵ７１は、まず、マークＭＡをカメラ６０の正面（真下）に移動させる（ステップＳ４００）。この処理は、シャフト角度θJ4が予め定められた所定角度となるようにモータ５２ａを制御することにより行なわれる。続いて、ＣＰＵ７１は、カメラ６０の被写界深度内の所定量ｂだけマークＭＡを上下させ、下位置にあるマークＭＡと上位置にあるマークＭＡとをカメラ６０によりそれぞれ撮像する（ステップＳ４１０）。次に、ＣＰＵ７１は、撮像画像を処理することにより、下位置で撮像したマークＭＡの実測位置と設計位置とのＸＹ軸方向の位置ズレ量ΔＸａ，ΔＹａと、上位置で撮像したマークＭＡの実測位置と設計位置とのＸＹ軸方向の位置ズレ量ΔＸｃ，ΔＹｃとを算出する（ステップＳ４２０）。図２２は、カメラ傾きの測定の様子を示す説明図である。カメラ６０が取付角度が設計値通りであった場合に撮像画像に写るマーク画像の中心は、カメラ６０の画像中心と一致する。図示するように、位置ズレ量ΔＹａは、下位置でマークＭＡを撮像したときの撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＹ軸方向における差である。位置ズレ量ΔＹｃは、上位置でマークＭＡを撮像したときに撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＹ軸方向における差である。図示しないが、位置ズレ量ΔＸａは、下位置でマークＭＡを撮像したときの撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＸ軸方向における差である。位置ズレ量ΔＸｃは、上位置でマークＭＡを撮像したときに撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＹ軸方向における差である。そして、ＣＰＵ７１は、算出した位置ズレ量ΔＹａ，ΔＹｂと上述した所定量ｂとに基づいて次式（１７）によりＸ軸回りのカメラ６０の傾きＲＡを算出すると共に、算出した位置ズレ量ΔＸａ，ΔＸｂと所定量ｂとに基づいて次式（１８）によりＹ軸回りのカメラ６０の傾きＲＢを算出する（ステップＳ４３０）。なお、式（１７）は、図２２から容易に導き出すことができる。式（１８）についても同様である。
Figure JPOXMLDOC01-appb-M000006
　ステップ２３０は、算出した位置ＸをパラメータＨＣＸに設定し、位置ＹをパラメータＨＣＹに設定し、傾きＲＡをパラメータＨＣＲＡに設定し、傾きＲＢをパラメータＨＣＲＢに設定する。ステップＳ２４０は、各々設定したリンクパラメータＨＣＸ，ＨＣＹ，ＨＣＲＡ，ＨＣＲＢをカメラ用リンクパラメータとして登録する。これにより、カメラ６０が設計値通りに取り付けられていなかった場合に、カメラ位置制御処理において順運動学によりカメラ６０の取付位置や取付角度に起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。この推定位置に基づいて目標位置を補正して制御することで、カメラ６０の画像中心を精度よく目標位置に合わせることが可能となる。
　ここで、実施形態の主要な要素と請求の範囲に記載した本開示の主要な要素との対応関係について説明する。即ち、本実施形態では、第１アーム２１が第１アームに相当し、第１関節軸Ｊ１が第１軸に相当し、第２アーム２２が第２アームに相当し、第２関節軸Ｊ２が第２軸に相当し、シャフト２３がシャフトに相当し、第３関節軸Ｊ３が第３軸に相当し、カメラ６０がカメラに相当する。
　なお、本開示は上述した実施形態に何ら限定されることはなく、本開示の技術的範囲に属する限り種々の態様で実施し得ることはいうまでもない。
　例えば、上述した実施形態では、カメラ位置Ｘ，Ｙとカメラ傾きＲＡ，ＲＢとを測定し、それぞれカメラ用リンクパラメータＨＣＸ，ＨＣＹ，ＨＣＲＡ，ＨＣＲＢに反映させるものとした。しかし、カメラ位置Ｘ，Ｙとカメラ傾きＲＡ，ＲＢとのうちいずれか一方の測定を省略してもよい。
　以上説明したように、本開示のスカラロボットの制御方法は、基台と、前記基台に設けられ、第１軸周りに回動する第１アームと、前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、を備え、前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録することを要旨とする。
　この本開示のスカラロボットの制御方法によれば、カメラの取付角度を測定して、測定値に基づく補正値を対応するリンクへの変換のための姿勢パラメータに反映させることで、カメラに取付角度に誤差が生じていても、カメラの位置制御を精度よく行なうことが可能となる。
　こうした本開示のスカラロボットの制御方法において、前記第３軸と直交する方向における前記光軸の位置を前記カメラの取付位置として測定し、前記測定したカメラの取付位置に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記位置パラメータに登録するものとしてもよい。こうすれば、カメラの取付角度に加えて取付位置に誤差が生じていても、カメラの位置制御を精度よく行なうことができる。
　さらに、本開示のスカラロボットの制御方法において、前記第２アームの回動位置と前記第１アームのアーム周りのねじれ量との関係を記憶し、前記第２アームの回動位置に対応する前記第１アームのねじれ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録するものとしてもよい。こうすれば、第１アームのねじれが生じていても、カメラの位置制御を精度よく行なうことができる。
　また、本開示のスカラロボットの制御方法において、前記第２アームの回動位置に対応する前記第１アームのたわみ量と前記第２アームのたわみ量とを記憶し、前記第２アームの回動位置に対応する前記第１アームのたわみ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録すると共に、前記第２アームの回動位置に対応する前記第２アームのたわみ量に基づく補正値を、前記第１リンクから前記第２リンクへの変換のための前記姿勢パラメータに登録するものとしてもよい。こうすれば、第１アームや第２アームにたわみが生じていても、カメラの位置制御を精度よく行なうことができる。
　本開示は、スカラロボットの製造産業などに利用可能である。
　１　スカラロボットシステム、２　作業台、１０　ロボット本体、１１　基台、２０　スカラアーム、２１　第１アーム、２２　第２アーム、２３　シャフト、２４　ワーク保持部、３０　第１アーム駆動部、３２　モータ、３４　エンコーダ、４０　第２アーム駆動部、４２　モータ、４４　エンコーダ、５０　シャフト駆動部、５２ａ，５２ｂ　モータ、５４ａ，５４ｂ　エンコーダ、６０　カメラ、７０　制御装置、７１　ＣＰＵ、７２　ＲＯＭ、７３　ＲＡＭ、７４　記憶装置、１００　測定用治具、Ｊ１　第１関節軸、Ｊ２　第２関節軸、Ｊ３　第３関節軸、　ＭＡ　マーク、Ｗ　ワーク。

Claims (4)

  1. 　基台と、
    　前記基台に設けられ、第１軸周りに回動する第１アームと、
    　前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
    　前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
    　光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、
    　を備え、前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
    　前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
    　前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録する、
    　スカラロボットの制御方法。
  2. 　請求項１に記載のスカラロボットの制御方法であって、
    　前記第３軸と直交する方向における前記光軸の位置を前記カメラの取付位置として測定し、
    　前記測定したカメラの取付位置に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記位置パラメータに登録する、
    　スカラロボットの制御方法。
  3. 　請求項１または２に記載のスカラロボットの制御方法であって、
    　前記第２アームの回動位置と前記第１アームのアーム周りのねじれ量との関係を記憶し、
    　前記第２アームの回動位置に対応する前記第１アームのねじれ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録する、
    　スカラロボットの制御方法。
  4. 　請求項１ないし３いずれか１項に記載のスカラロボットの制御方法であって、
    　前記第２アームの回動位置に対応する前記第１アームのたわみ量と前記第２アームのたわみ量とを記憶し、
    　前記第２アームの回動位置に対応する前記第１アームのたわみ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録すると共に、前記第２アームの回動位置に対応する前記第２アームのたわみ量に基づく補正値を、前記第１リンクから前記第２リンクへの変換のための前記姿勢パラメータに登録する、
    　スカラロボットの制御方法。
PCT/JP2020/044678 2020-12-01 2020-12-01 スカラロボットの制御方法 WO2022118374A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
PCT/JP2020/044678 WO2022118374A1 (ja) 2020-12-01 2020-12-01 スカラロボットの制御方法
JP2022566526A JPWO2022118374A1 (ja) 2020-12-01 2020-12-01

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2020/044678 WO2022118374A1 (ja) 2020-12-01 2020-12-01 スカラロボットの制御方法

Publications (1)

Publication Number Publication Date
WO2022118374A1 true WO2022118374A1 (ja) 2022-06-09

Family

ID=81853003

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2020/044678 WO2022118374A1 (ja) 2020-12-01 2020-12-01 スカラロボットの制御方法

Country Status (2)

Country Link
JP (1) JPWO2022118374A1 (ja)
WO (1) WO2022118374A1 (ja)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6344206A (ja) * 1986-07-23 1988-02-25 アメリカン　テレフオン　アンド　テレグラフ　カムパニ− スカラ型ロボットを較正する方法
JPH05204423A (ja) * 1992-01-29 1993-08-13 Nec Home Electron Ltd 視覚装置付きロボット装置における座標較正方法
JPH0655485A (ja) * 1992-08-11 1994-03-01 Citizen Watch Co Ltd 多関節ロボットのアームたおれ量測定装置
JPH06206186A (ja) * 1993-01-12 1994-07-26 Citizen Watch Co Ltd 視覚センサ付き水平多関節型ロボットのハンドリング精度校正法
JP2012006125A (ja) * 2010-06-28 2012-01-12 Seiko Epson Corp 水平多関節ロボットの座標値補正方法
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 (ja) * 2016-12-09 2018-06-21 セイコーエプソン株式会社 制御装置、ロボットおよびロボットシステム

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6344206A (ja) * 1986-07-23 1988-02-25 アメリカン　テレフオン　アンド　テレグラフ　カムパニ− スカラ型ロボットを較正する方法
JPH05204423A (ja) * 1992-01-29 1993-08-13 Nec Home Electron Ltd 視覚装置付きロボット装置における座標較正方法
JPH0655485A (ja) * 1992-08-11 1994-03-01 Citizen Watch Co Ltd 多関節ロボットのアームたおれ量測定装置
JPH06206186A (ja) * 1993-01-12 1994-07-26 Citizen Watch Co Ltd 視覚センサ付き水平多関節型ロボットのハンドリング精度校正法
JP2012006125A (ja) * 2010-06-28 2012-01-12 Seiko Epson Corp 水平多関節ロボットの座標値補正方法
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 (ja) * 2016-12-09 2018-06-21 セイコーエプソン株式会社 制御装置、ロボットおよびロボットシステム

Also Published As

Publication number Publication date
JPWO2022118374A1 (ja) 2022-06-09

Similar Documents

Publication Publication Date Title
JP6468741B2 (ja) ロボットシステム及びロボットシステムの校正方法
JP6429473B2 (ja) ロボットシステム、ロボットシステムの校正方法、プログラム、およびコンピュータ読み取り可能な記録媒体
JP4267005B2 (ja) 計測装置及びキャリブレーション方法
CN107614195B (zh) 齿轮机构的组装装置及组装方法
CN113601158B (zh) 基于视觉定位的螺栓上料预拧紧***及控制方法
JP2009269110A (ja) 組立装置
JP5618770B2 (ja) ロボットの校正装置および校正方法
JP7306937B2 (ja) ロボットに支持された部材の位置を調整するロボット装置の制御装置
CN109952178B (zh) 作业机器人及作业位置修正方法
US11679508B2 (en) Robot device controller for controlling position of robot
JP4289619B2 (ja) 多関節ロボットのツール位置補正方法
US20180161983A1 (en) Control device, robot, and robot system
EP1886771B1 (en) Rotation center point calculating method, rotation axis calculating method, program creating method, operation method, and robot apparatus
JP2012223871A (ja) ロボットの関節の原点補正方法およびそのシステム
JP7281910B2 (ja) ロボット制御システム
KR20070122271A (ko) 로봇 자세 제어 시스템 및 로봇 자세 제어 방법
CN113043332B (zh) 绳驱柔性机器人的臂形测量***和方法
WO2022118374A1 (ja) スカラロボットの制御方法
KR102422990B1 (ko) 스캔을 이용한 로봇의 캘리브레이션 시스템 및 방법
JP7384653B2 (ja) ロボットの位置を制御するロボット装置の制御装置
JP6578671B2 (ja) ロボット、ロボットの制御方法、及びロボットの制御装置
JP4212783B2 (ja) ロボット座標校正システム
CN110977950B (zh) 一种机器人抓取定位方法
JP7145332B2 (ja) ロボット制御システム及びロボット制御方法
WO2022024243A1 (ja) スカラロボットシステム

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