JP2009166164A - Industrial robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To avoid a singularity of a manipulator by slightly correcting the position of an auxiliary device. <P>SOLUTION: When a singularity determining unit 26 determines the vicinity of a singularity, a correction moving device target value calculating unit 27 corrects an auxiliary device target value Jsta(t) to calculate a correction auxiliary device target value Jsta(t)' so that the posture of a manipulator at the next time becomes out of a range of the vicinity of the singularity while maintaining the position and posture of a tool front end to a work at the next time. A second coordinate converting/calculating unit 28 converts a tool target value worldPta(t) into a tool target value defined in a manipulator coordinate system Σbase, using the corrected auxiliary device target value Jsta(t)'. A correction target joint angle calculating unit 29 carries out inverse kinematics calculation on the converted tool target value worldPta(t)" defined in the manipulator coordinate system Σbase to calculate a correction target joint angle Jmta(t)". The correction target joint angle Jmta(t)" and the corrected auxiliary device target value Jsta(t)' are outputted to the manipulator 11 and the moving device 12. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to an industrial robot. In particular, the present invention relates to an industrial robot including a manipulator including a tool and an auxiliary device such as a moving device that moves the manipulator and a positioner that positions a workpiece.

  In general, manipulators for industrial robots have multiple degrees of freedom, and with this degree of freedom, tools (processing tools) such as welding torches and paint guns attached to the tip of the wrist are positioned at arbitrary positions and postures (workpieces to be processed). Processing). When operating the tip position of such an articulated manipulator in an orthogonal coordinate system, there is an inoperable point called a singular point. In the vicinity of this singular point, the solution of inverse kinematics calculation for obtaining the joint angle of the manipulator from the position and posture of the manipulator tip in the Cartesian coordinate system becomes indefinite. As a result, the command value for each joint motor cannot be obtained, and the manipulator stops operating as an error occurs.

  On the other hand, some industrial robots include a moving device that moves the manipulator and an auxiliary device such as a positioner that moves and rotates the workpiece in addition to the manipulator. By enlarging the work area with the moving device and appropriately positioning the work within the work area of the manipulator with the positioner, it becomes possible to process a wider variety of work. In general, the manipulator and the auxiliary device are operated simultaneously in synchronization with each other in order to shorten the work time and improve the work quality. For example, in the case of arc welding, by operating the moving device and positioner in synchronization with the manipulator, it becomes possible to continuously weld large workpieces where the work area is insufficient with the manipulator alone, shortening the welding work time and welding quality Can be improved.

  For example, Patent Documents 1 to 3 disclose a method for avoiding the above-described singular point. All of the singularity avoidance methods described in these patent documents are intended to smoothly pass the vicinity of the singularity by changing the speed and trajectory in the vicinity of the singularity on the wrist axis of the vertical articulated manipulator. ing. However, all disclosed in Patent Documents 1 to 3 are singular point avoidance methods using only a manipulator, and an auxiliary device is not considered for singular point avoidance. For this reason, the singular point avoidance methods disclosed in Patent Documents 1 to 3 are difficult to achieve both ensuring position accuracy and speed accuracy, and require complicated calculations.

JP2003-300193A JP-A-10-138180 Japanese Patent Laid-Open No. 10-329066

  The present invention relates to an industrial robot that operates a manipulator and an auxiliary device in synchronization with each other by finely correcting the position of the auxiliary device to ensure position accuracy and speed accuracy, while maintaining a singular point of the manipulator with relatively simple calculation. The problem is to avoid the problem.

  The present invention provides an articulated manipulator having a tool at a tip, a work machined by the tool and / or an auxiliary device for moving the manipulator, and a control device for operating the manipulator and the auxiliary device in synchronization. An industrial robot comprising: a tool target value calculation means for calculating a tool target value, which is a target value of a tool tip position and posture at a next time in a predetermined fixed orthogonal coordinate system; Auxiliary device target value calculating means for calculating an auxiliary device target value that is a target value of the position and orientation of the auxiliary device at the next time, and using the auxiliary device target value, the tool target value in the manipulator coordinate system A first coordinate transformation computing means for transformation, and an inverse kinematics computation of the tool target value transformed into the manipulator coordinate system. , A target joint angle calculation means for calculating a target joint angle that is a target value of a joint angle of each joint of the manipulator at the next time, and a singular point determination for determining whether or not the target joint angle is in the vicinity of a singular point When the means and the singular point discrimination means discriminate the vicinity of the singular point, the posture of the manipulator at the next time is out of the singular point vicinity range while maintaining the position and posture of the tool tip with respect to the workpiece at the next time. In the fixed orthogonal coordinate system of the next time using the correction auxiliary device target value calculating means for correcting the auxiliary device target value and calculating the correction auxiliary device target value, A second coordinate transformation calculation means for transforming the tool target value into a manipulator coordinate system; and a tool transformed into the manipulator coordinate system by the second coordinate transformation means. The corrected target joint angle calculating means for calculating the corrected target joint angle that is the target value of the joint angle of the manipulator at the next time by the inverse kinematics calculation of the target value, and the singular point determination means determine the vicinity of the singular point. If not, the manipulator and the auxiliary device are driven according to the target joint angle and the auxiliary device target value, while the corrected target joint angle and the correction auxiliary device are driven if the singularity discriminating means discriminates the vicinity of the singularity. An industrial robot comprising the manipulator and driving means for driving the auxiliary device according to a target value is provided.

  Specifically, the correction assist device target value calculation means calculates a preliminary correction target joint angle obtained by correcting the target joint angle so as to be outside a predetermined singular point vicinity range, and performs the preliminary correction. A preliminary correction tool target value in the manipulator coordinate system is calculated by forward kinematics calculation of a target joint angle, a correction value that is a difference between the preliminary correction tool target value and the tool target value in the manipulator coordinate system is calculated, and the correction is performed. The correction assist device target value is calculated from the assist device target value using a value.

  For example, the control device defines a tool path generation unit that generates a tool path that defines a time change in the position and posture of the tool tip in the fixed orthogonal coordinate system, and a time change in the position and posture of the auxiliary device. An auxiliary device path generating means for generating an auxiliary device path, wherein the tool target value calculating means calculates the tool target value at the next time based on the tool path, and the auxiliary device target value calculating means. Calculates the auxiliary device target value at the next time based on the auxiliary device path.

  The auxiliary device may be one of the moving device and the positioner, or may include both the moving device and the positioner.

  In addition, the auxiliary device only needs to have at least one linear joint or rotary joint.

  The manipulator has a wrist with a vertical articulated type and 6 axes, the fourth axis is a roll, the fifth axis is a vent, and the sixth axis is a roll. The auxiliary device includes only a positioner having only a rotary joint. In some cases, at least one of the rotary joints of the positioner needs to rotate in the same direction as the fifth axis of the manipulator.

  When the target joint angle at the next time of the manipulator (the posture of the manipulator at the next time) is in the vicinity of the singular point, correction assistance that corrects the auxiliary device target value so that the posture of the manipulator at the next time is outside the range near the singular point The apparatus target value is calculated, the correction target joint angle is calculated from the correction auxiliary apparatus target value, and the correction target joint angle and the correction auxiliary apparatus target value are output to the manipulator and the auxiliary apparatus. In other words, when the manipulator is in the vicinity of the singular point, the position of the auxiliary device is automatically corrected to make the manipulator posture out of the singular point vicinity range. Therefore, it is possible to avoid singular points while maintaining the position accuracy and speed accuracy without converting the position and posture of the tool tip with respect to the workpiece. This is effective in avoiding singularities in work that greatly affects Further, since the singular point is avoided by fine correction of the position of the auxiliary device, the singular point of the manipulator can be avoided by a relatively simple calculation.

  Next, embodiments of the present invention will be described in detail with reference to the accompanying drawings.

(First embodiment)
The first embodiment shown in FIGS. 1 and 2 is an example in which the present invention is applied to a PTP type industrial robot including a manipulator 11 and a moving device (slider) 12.

  Referring to FIG. 1, the industrial robot includes a 6-axis vertical articulated manipulator 11, a moving device 12, a control device 13, and a teaching device 14. In the present embodiment, the position and posture of the workpiece 17 itself are fixed.

The manipulator 11 includes a tool 16 at the tip, and changes the position and posture of the tool 16 in a three-dimensional space. Manipulator 11 has six rotary joints _{RJ m1, RJ m2, RJ m3} , RJ m4, RJ m5, RJ m6. The rotary joints RJ _{m1 to} RJ _m6 are connected by a link, and the rotary joint RJ _m1 on the most proximal side is attached to the base 11a. The rotary joints RJ _{m1 to} RJ _m6 include motors MO _{m1 to} MO _m6 for rotational driving and angle sensors SE _{m1 to} SE for detecting a joint angle Jm (J1, J2, J3, J4, J5, J6). _m6 . Further, the manipulator 11 is wrist three axes (rotary joint _RJ m4 _{~RJ m6)} roll bend roll type intersect at one point (fourth axis roll, fifth shaft vent, sixth axis roll) Wrist Have.

The moving device 12 includes three linear motion joints SJ ₁ , SJ ₂ , and SJ ₃ that linearly move in directions orthogonal to each other, and the base 11 a of the manipulator 11 is held by the linear motion joint SJ ₁ . The linear motion joints SJ _{1 to} SJ ₃ are respectively motors for driving MO _{s1 to} MO _s3 and position sensors SE _s1 to SEs for detecting a joint position Js (SX, SY, SZ) in a world coordinate system Σworld described later. SE _s3 is provided.

  Next, coordinates used for controlling the manipulator 11 and the moving device 12 will be described.

With respect to the manipulator 11, an orthogonal coordinate system (manipulator coordinate system Σbase) whose origin is set on the base 11a of the manipulator 11 and is fixed with respect to the three-dimensional space is set. Position and orientation of the manipulator 11 in the manipulator coordinate system Shigumabase (position and orientation of the tool tip 16a), i.e. baseP the rotation angle around the coordinate and the coordinate axes in manipulator coordinates _{Σbase (X b, Y b,} Z b, α b, [beta] _b , [gamma] _b ). α represents a roll angle, β represents a pitch angle, and γ represents a yaw angle.

In addition to the manipulator coordinate system Σbase, an orthogonal coordinate system (world coordinate system Σworld) fixed to the three-dimensional space is set. Position and orientation of the manipulator 11 in the world coordinate system Shigumaworld (position of the tool tip 16a and attitude), i.e. worldP coordinates and axes around rotation angle in the world coordinate system _{Σworld (X w, Y w,} Z w, α w, β _w , γ _w ). The world coordinate system Σworld and the manipulator coordinate system Σbase have the same posture (the directions of the XYZ axes) in the three-dimensional space. The directions of the linear motion joints SJ _{1 to} SJ ₃ of the moving device 12 coincide with the directions of the XYZ axes of the world coordinate system Σworld, and the origin of the world coordinate system Σworld is the joint position Js (0, 0, 0). The position is set.

  Referring to FIG. 2, the control device 13 includes a route generation unit 21, a tool target value calculation unit 22, a moving device target value calculation unit 23, a first coordinate conversion calculation unit 24, a target joint angle calculation unit 25, and a singular point determination unit. 26, a correction moving device target value calculation unit 27, a second coordinate calculation unit 28, a correction target joint angle calculation unit 29, and a drive unit 30. The control device 13 is constructed by a processing unit, various storage devices such as RAM and ROM, hardware such as input / output ports, and software installed therein.

  Next, control of the manipulator 11 and the moving device 12 executed by the control device 13 will be described with reference to the flowcharts of FIGS. 3A and 3B.

First, in step S3-1, the path generation unit 21 defines an equation (tool path equation worldP (t), sign) that defines a time-series change in the position and orientation of the tool tip 16a from the current position to the teaching position in the world coordinate system Σworld. t indicates time). In step S3-2, the path generation unit 21 teaches from the current position so as to be synchronized with the time series change of the position and orientation of the tool tip 16a in the world coordinate system Σworld defined by the tool path equation worldP (t). An equation (moving device path equation Js (t)) that defines a time-series change in the position and orientation of the moving device 12 up to the position is calculated. In order to calculate the tool path equation worldP (t) and the moving device path equation Js (t), the teaching position Pte (Jmte + Jste) is input from the teaching device 14 to the path generating unit 21, and the current position P0 (Jm0 + Js0) is input. Input from the angle sensors SE _{m1 to} SE _m6 and the position sensors SE _{s1 to} SE _s3 . Further, control conditions (such as the shape of the trajectory of the tool tip 16a and the moving speed) necessary for calculating the tool path equation worldP (t) and the moving device path equation Js (t) are input to the path generation unit 21. .

  After obtaining the tool path equation worldP (t) and the moving device path equation Js (t), the time t is initialized in step S3-3 (t = 0).

  The processing from step S3-4 to step S3-17 is repeated every fixed control cycle Tc. First, in step S3-4, time t is updated to time t + Tc (next time).

Next, in step S3-5, the tool target value calculation unit 22 uses the tool path equation worldP (t), and the tool is the target value of the position and orientation at the time t of the tool tip 16a in the world coordinate system Σworld. The target value worldPta (t) (X _wta , Y _wta , Z _wta , α _wta , β _wta , γ _wta ) is calculated. In step S3-6, the mobile device target value calculation unit 23 uses the mobile device path equation Js (t) to determine the position and orientation target values of the mobile device 12 at time t in the world coordinate system Σworld. A certain moving device target value Jsta (t) (SXta, SYta, SZta) is calculated.

  In step S3-7, the first coordinate conversion unit 24 uses the moving device target value Jsta (t) to obtain the tool target value worldPta (t) in the world coordinate system Σworld, and the tool target at time t in the manipulator coordinate system Σbase. Coordinates are converted to the value basePta (t) (Xbta, Ybta, Zbtz, αbta, βbta, γbta). Various coordinate conversion means for this type of coordinate conversion are known, but in this embodiment, the directions of the three orthogonal axes of the manipulator coordinate system Σbase and the movement direction of the moving device 12 coincide with each other. A tool target value basePta (t) in the manipulator coordinate system Σbase is obtained by simple subtraction of the value worldPta (t) and the moving device target value Jsta (t).

Next, in step S3-8, the target joint angle calculation unit 25 calculates the inverse kinematics of the tool target value basePta (t) in the manipulator coordinate system Σbase, and the time t of each rotary joint RJ _{m1 to} RJ _m6 of the manipulator 11. Is converted into a target joint angle Jmta (t) (J1ta, J2ta, J3ta, J4ta, J5ta, J6ta), which is the target value of the joint angle at.

Subsequently, in step S3-9, the singular point determination unit 26 determines whether or not the target joint angle Jmta (t) calculated by the target joint angle calculation unit 25 is in the vicinity of the singular point. In the manipulator 11 having a roll-bend-roll-type wrist in which the rotation center of the wrist 3 axes intersects at one point as in the present embodiment, the joint angle J5 of the rotary joint RJ _m5 that is the fifth axis is around 0 degrees. From the position and orientation basePta (t) of the torch tip 16a in the manipulator coordinate system Σbase, the joint angles J1 to J6 of the rotary joints RJ _{m1 to} RJ _m6 are “singular points” that cannot be uniquely determined. When the vicinity of the singular point is approached during the operation of the manipulator 11, the rotary joints RJ _{m4 to} RJ _{m6 that} are wrist axes change rapidly. Conventionally, when approaching the vicinity of the singular point, the operation of the manipulator 11 is stopped as an error, or the speed of the tool tip 16a is reduced so that the rotation speed of the wrist shaft does not exceed the maximum rotation speed, and the singular point is passed. It was.

  In the present embodiment, the singularity determination unit 26 determines that the target joint angle Jmta (t) (attitude of the manipulator 11) at the time t is in the vicinity of the singularity when the following expression (1) is satisfied. Conversely, when equation (1) does not hold, the singular point determination unit 26 determines that the target joint angle Jmta (t) at time t is not near the singular point.

In equation (1), J5ta is the target joint angle of the rotary joint RJ _m5 of the fifth axis at time t, and ΔJs is a discrimination threshold. This determination threshold ΔJs is determined by the allowable maximum speed of the rotary joints RJ _{m4 to} RJ _m6 that are wrist axes and the manipulator link length. If the rotary joint RJ _m5 of the fifth axis is operated so as to be out of the range of the expression (1), a singular point can be avoided and the manipulator 11 can be driven smoothly.

If it is determined in step S3-9 that the target joint angle Jmta (t) is not near the singular point, the drive unit 30 outputs the target joint angle Jmta (t) to the manipulator 11 in step S3-10, and the moving device target The value Jsta (t) is output to the moving device 12. By these commands, the motors MO _{m1 to} MO _m6 of the manipulator 11 and the motors MO _{s1 to} MO _s3 of the moving device 12 operate, and the manipulator 11 and the moving device 12 operate simultaneously in synchronization.

  On the other hand, if it is determined in step S3-9 that the target joint angle Jmta (t) is in the vicinity of the singular point, the target joint angle Jmta (t) and the moving device target value Jsta (t) are directly used as the manipulator 11 and the moving device. Rather than output to 12, the process of steps S3-11-1 to 17-17 is executed to obtain the corrected moving device target value Jsta (t) ′ and the corrected target joint angle Jmta (t) ″ for singularity avoidance. Calculate and output.

  The correction moving device target value calculation unit 27 executes the processes of steps S3-11 to S3-14. The corrected moving device target value calculation unit 27 maintains the position and posture of the tool tip 16a in the world coordinate system Σworld at time t (the position and posture of the tool tip 16a with respect to the workpiece 17), while the manipulator 11 at time t. The position and posture of the moving device 12 at time t are finely corrected so that the posture is out of the singular point vicinity range.

First, in step S3-11, a preliminary correction target joint angle Jmta (t) ′ is calculated. As described above, the singularity can be avoided and the manipulator 11 can be driven smoothly by operating the rotary joint RJ _m5 of the fifth axis so as to be out of the range of the expression (1). The preliminary correction target value joint angle Jmta (t) ′ is obtained by changing the target joint angle J5ta of the rotary joint RJ _m5 out of the range of the expression (1) out of the target joint angle Jmta (t) and rotating the remaining five axes. joint angle _RJ m1 _~RJ m4, the target joint angle J1ta~J4ta of _{RJ m6,} J6ta are those maintained without modification. FIG. 4 conceptually shows the relationship between the target joint angle Jmta and the preliminary correction target joint angle Jmta (t). When the path is moving in the state of J5> + ΔJs, when the target joint angle Jmta (t) at time t (next time) becomes J5ta <+ ΔJs, the joint angle of the rotary joint RJ _m5 of the fifth axis outside the vicinity of the singular point In order to limit J5, J5ta is set to J5ta ′ = + ΔJs. Further, during the path movement in the state of J5 <−ΔJs, when J5ta> −ΔJs at the target joint angle Jmta (t) at time t, J5ta is set to J5ta ′ = − ΔJs.

  Next, in step S3-12, the forward kinematics of the preliminary correction target joint angle Jmta (t) 'is calculated, and the preliminary correction tool target value basePta (t)' in the manipulator coordinate system Σbase is calculated.

  In step S3-12, a necessary correction amount ΔP in the base coordinate system Σbase for making the tool tip 16a out of the singular point vicinity range is calculated from the tool target value basePta (t) and the preliminary correction tool target value basePta (t) ′. calculate. Specifically, as shown in the following equation (2), the correction amount ΔP is given as a difference between the orthogonal component of the preliminary correction tool target value basePta (t) ′ and the orthogonal component of the tool target value basePta (t).

  Next, in order to realize the correction amount ΔP by finely correcting the position and orientation of the moving device 12, the correction device ΔP is used to correct the moving device target value Jsta (t) at time t, and the time of the moving device 12 is corrected. A new target position at t (corrected moving device target value Jsta (t) ′) is calculated. As shown in the following equation (3), the corrected moving device target value Jsta (t) ′ (SX ′, SY ′, SZ ′) is corrected with the moving device target value Jsta (t) (SX, SY, SZ). It is given as the sum of the quantities ΔP (ΔX, ΔY, ΔZ).

  Next, in step S3-15, the second coordinate transformation calculation unit 28 uses the corrected moving device target value Jsta (t) ′ calculated by the corrected moving device target value calculating unit 27 to use the tool in the world coordinate system Σworld. The target value worldPta (t) is corrected tool target value basePta (t) ″ (Xta ″, Yta ″, Zta ″, αta ″, βta ″, γta ′, which is a tool target value in the manipulator coordinate system Σbase. ') Convert coordinates. In the present embodiment, since the XYZ axes of the manipulator coordinate system and the moving direction of the moving device 12 coincide with each other, the XYZ component of the correction tool target value basePta (t) ″ is the tool as shown in the following equation (4). The αβγ component of the correction tool target value basePta (t) ″ is the same as the tool target value worldPta (t), calculated by simple subtraction of the target value worldPta (t) and the correction moving device target value Jsta (t) ′. .

  Next, in step S3-16, the correction target joint angle calculation unit 29 calculates the inverse kinematics of the correction tool target value basePt (t) ″ in the manipulator coordinate system Σbase, and each rotary joint RJm1 to RJm6 of the manipulator 11. To the corrected target joint angle Jmta (t) ″ (J1ta ″, J2ta ″, J3ta ″, J4ta ″, J5ta ″, J6ta ″), which is the correction value of the target value of the joint angle at time t. Convert.

Thereafter, in step S3-17, the drive unit 30 outputs the corrected target joint angle Jmta (t) ″ to the manipulator 11 and outputs the corrected moving device target value Jsta (t) ′ to the moving device 12. These commands actuate the motors MO _{m1 to} MO _m6 of the manipulator 11 and the motors MO _{s1 to} MO _s3 of the moving device 12, and the manipulator 11 and the moving device 12 move in synchronization. As shown in FIG. 5A, even if the posture of the manipulator 11 becomes a singular point at time t if the operation for avoiding the singular point is not executed, in this embodiment, as shown by the arrow A in FIG. By moving the entire manipulator 11 by 12, the position and posture of the torch tip 16a with respect to the work 17 are maintained, and the posture of the manipulator 11 is avoided from being in the vicinity of a singular point.

The processing from step S3-4 to step S3-17 is repeated until the tool tip 16a reaches the teaching position Pte in step S3-18.

  In the present embodiment, when the target joint angle Jmta (t) at the next time of the manipulator 11, that is, the posture of the manipulator at the next time is in the vicinity of the singular point, the posture of the manipulator 11 at the next time is out of the singular point vicinity range. Thus, the correction auxiliary device target value Jsta (t) ′ obtained by correcting the moving device target value Jsta (t) is calculated, and the corrected target joint angle Jmta (t) ″ is calculated from the corrected moving device target value Jsta (t) ′. The corrected target joint angle Jmta (t) ″ and the auxiliary moving device target value Jsta (t) ′ are output to the manipulator 11 and the moving device 12. In other words, as shown in FIG. 5B, when the manipulator 11 is in the vicinity of the singular point, the position of the moving device 12 is automatically corrected to make the manipulator 11 out of the singular point vicinity range. Therefore, it is possible to avoid singular points while maintaining the position accuracy and speed accuracy without converting the position and posture of the tool tip 16a with respect to the workpiece 17, and in particular, fluctuations in the position and posture of the processing tool such as arc welding. This is effective in avoiding singularities in operations that greatly affect machining quality. In addition, since the singular point is avoided by finely correcting the position of the moving device 12, the singular point of the manipulator 11 can be avoided by a relatively simple calculation.

(Second Embodiment)
The second embodiment shown in FIGS. 6 and 7 is an example in which the present invention is applied to a PTP industrial robot including the manipulator 11 and the positioner 32. Although the workpiece 17 is moved by the positioner 32, the position and posture of the manipulator 17 are fixed.

The positioner 32 in the present embodiment includes three linear motion joints PJ ₁ , PJ ₂ , PJ ₃ and one rotary joint PJ ₄ that linearly move in directions orthogonal to each other. Each of the linear motion joints PJ _{1 to} PJ ₃ includes driving motors MO _{P1 to} MO _P3 and position sensors SE _{p1 to} SE _p3 for detecting the joint position Jp in the world coordinate system Σworld. The directions of the linear motion joints PJ _{1 to} PJ ₃ coincide with the directions of the XYZ axes of the world coordinate system Σworld, and the origin of the world coordinate system is set to a position where the joint position Jp is (0, 0, 0). Yes. Also, rotary joints PJ ₄ of the positioner 32 rotates about _{Y w} axis of the world coordinate system Shigumaworld. The joint position Jp in the world coordinate system Σworld of the positioner 32 is expressed as (PX, PY, PZ, βp).

  The positioner target value calculation unit 33 and the correction positioner target value calculation unit 37 included in the control device 13 in the present embodiment are respectively the movement position target value calculation unit 23 and the correction movement device target value calculation included in the control device 13 in the first embodiment. It has the same function as the unit 27.

  8A and 8B and FIGS. 3A and 3B, the control device 13 in the present embodiment controls the manipulator 11 and the positioner 32 in the same manner as in the first embodiment. Hereinafter, an outline of the control of FIGS. 8A and 8B will be described.

  After the path generation unit 21 calculates the tool path equation worldP (t) and the positioner path equation Jp (t) (steps S8-1 and S8-2), the time t is initialized (step S8-3). Until the arrival at the teaching position Pte is detected in step S8-18, the processing from step S8-4 to step S8-17 is repeated every certain control cycle Tc.

  The tool target value calculation unit 22 calculates the tool target value worldPta (t) in the world coordinate system Σworld at time t (next time) from the tool path equation worldP (t) (step S8-5), and calculates the positioner target value. The unit 33 calculates a positioner target value Jpta (t) (PXta, PYta, PZta, βpta) at time t from the positioner path equation worldJp (t) (step S8-6).

Next, the first coordinate conversion unit 24 uses the positioner target value Jpta (t) to obtain the tool target value worldPta (t) in the world coordinate system Σworld, and the tool target value basePta (t) at time t in the manipulator coordinate system Σbase. ) (Step 8-7). In addition, the target joint angle calculation unit 25 calculates the tool target value basePta (t) in the manipulator coordinate system Σbase by inverse kinematics calculation to the target joint angle Jmta (t) at the time t of each of the rotary joints RJ _{m1 to} RJ _m6 of the manipulator 11. (Step S8-8).

If the singularity determination unit 26 determines that the posture of the manipulator 11 at the time t is not a singular point from the target joint angle J5ta of the rotary joint RJ _m5 of the fifth axis of the manipulator 11 (see Expression (1)). Outputs the target joint angle Jmta (t) to the manipulator 11 and outputs the positioner target value Jpta (t) to the positioner 32, and simultaneously drives the manipulator 11 and the positioner 32 synchronously (steps 8-9, S8-). 10).

  On the other hand, if the singularity determination unit 26 determines that the attitude of the manipulator 11 at time t is a singularity, the corrected positioner target value Jpta (t) ′ and the corrected target joint angle Jmta (t) ′ for avoiding the singularity. 'Is calculated and output to the positioner 32 and the manipulator 11 (steps S8-9, S8-11 to S817).

  Specifically, the correction positioner target value calculation unit 37 calculates a preliminary correction target joint angle Jmta (t) ′, and calculates the preliminary correction tool target value by forward kinematics calculation of the preliminary correction target joint angle Jmta (t) ′. basePta (t) ′ is calculated, and further, the necessary correction in the base coordinate system Σbase for making the manipulator 11 out of the singular point vicinity range from the tool target value basePta (t) and the preliminary correction tool target value basePta (t) ′ The amount ΔP (see equation (2)) is calculated (steps S8-11 to S8-13). The corrected positioner target value calculation unit 37 uses the correction amount ΔP to correct the positioner target value Jpta (t), which is a new target position at the time t of the positioner 32 from the positioner target value position Jpta (t) at the time t. 'Calculate. Specifically, the corrected positioner target value Jpta (t) ′ (PXta ′, PYta ′, PZta ′, βpta ′) is as shown in the following equation (5).

  Next, the second coordinate transformation calculation unit 28 uses the corrected positioner target value Jpta (t) ′ to obtain the tool target value worldPta (t) in the world coordinate system Σworld and the correction tool target value basePta (t in the manipulator coordinate system Σbase. ) '' (Step S8-15). Since the XYZ axes of the manipulator coordinate system coincide with the direction of linear movement of the positioner 32, the XYZ component of the correction tool target value basePta (t) '' is the tool target value worldPta as shown in the following equation (6). Calculated by simple subtraction of (t) and the corrected positioner target value Jpta (t) ′, the αβγ component of the corrected tool target value basePta (t) ″ is the same as the tool target value worldPta (t).

Further, the correction target joint angle calculation unit 29 calculates the inverse kinematics of the correction tool target value basePt (t) ″ in the manipulator coordinate system Σbase, and corrects the correction target joints for the rotary joints RJ _{m1 to} RJ _m6 of the manipulator 11. Convert to angle Jmta (t) ″.

  Also in the industrial robot of this embodiment, when the manipulator 11 is in the vicinity of the singular point, the position of the positioner 32 is automatically corrected to make the manipulator 11 out of the singular point vicinity range. Therefore, it is possible to avoid the singularity while maintaining the position accuracy and the velocity accuracy without changing the position and posture of the tool tip 16a with respect to the workpiece 17, and the calculation for avoiding the singularity is relatively simple. .

  Other configurations and operations of the second embodiment are the same as those of the first embodiment.

(Third embodiment)
In the first and second embodiments, the auxiliary device (the moving device 12 and the positioner 32) includes three linear motion joints, but the present invention only includes a positioner having only one or two-axis rotary joints as the auxiliary device. It can also be applied to industrial robots equipped with This embodiment is an example in which the present invention is applied to an industrial robot including only a positioner 42 including only a single-axis rotary joint as an auxiliary device.

  Referring to FIG. 9, the positioner 42 includes only one rotational joint PJ that rotates around a horizontal axis (Yw axis of the world coordinate system Σworld). The rotary joint PJ includes a driving motor MO and a position sensor SE. In FIG. 9, Σposi is positioner coordinates fixed to the mounting surface of the workpiece 17 of the positioner 42. Other mechanical configurations of the present embodiment are the same as those of the second embodiment. The configuration of the control device 13 is the same as that of the control device 13 (see FIG. 7) of the second embodiment.

  Control of the manipulator 11 and the positioner 42 shown in FIGS. 10A and 10B is the same as the control (FIGS. 8A and 8B) in the second embodiment. Specifically, steps S10-1 to S10-10 and S10-13 to S10-16 in FIGS. 10A and 10B are respectively performed as S8-1 to S8-10 and S8-14 to S8- in FIGS. 8A and 8B. 18 is the same.

  In the present embodiment, the method for obtaining the corrected positioner target value Jpta (t) ′ for avoiding the singular point, which is executed by the corrected positioner target value calculation unit 37, is different from that in the second embodiment. In the third embodiment, the preliminary correction target joint angle Jmta (t) ′ is calculated, the preliminary correction tool target position base Pta (t) ′ is obtained by forward kinematics, and the preliminary correction tool target position basePta (t) ′ and the tool are obtained. From the target value base Pta (t), a correction amount ΔP is obtained from equation (2) (steps S8-11 to S8-13 in FIG. 8B). On the other hand, in the present embodiment, the determination threshold value ΔJs as to whether or not it is near a singular point is set to the correction amount ΔP (step S10-11 in FIG. 10B). Specifically, during the path movement in the state of J5> + ΔJs, when J5ta <+ ΔJs at the target joint angle Jmta (t) at time t (next time), + ΔJs is set as the correction amount ΔP. Also, during the path movement in the state of J5 <−ΔJs, when J5ta> −ΔJs at the target joint angle Jmta (t) at time t, −ΔJs is set to the correction amount ΔP. The following equation (7) shows the correction amount ΔP in the present embodiment.

  By correcting the positioner target value Jpta (t) (= βp) with this correction amount Δ, the tool tip 16a at time t is out of the singular point vicinity range. The following equation (8) shows the corrected positioner target value Jpta (t) '.

  The calculation after the correction positioner target value Jpta (t) 'is calculated is the same as that in the second embodiment as described above (steps S10-12 to S10-15).

  As shown in FIG. 11A, even if the posture of the manipulator 11 becomes a singular point at time t unless the operation for avoiding the singular point is executed, in this embodiment, as shown by the arrow B in FIG. Thus, the posture of the manipulator 11 is avoided from being in the vicinity of the singular point by rotating the workpiece 17 by the above. Although the attitude angle of the torch tip 16a with respect to the direction of gravity (Zw direction of the world coordinate system world) changes by ΔJs due to the rotation correction of the positioner 42, this angle is several degrees or less, and there is no substantial problem in machining quality. It is a very small amount.

In the case of a manipulator having a roll-bend-roll-type wrist in which wrists 3 axes intersect at one point as in the present embodiment, when the present invention is implemented by a positioner having only a rotary joint, at least the rotary joint of the positioner One rotational component needs to be in an arrangement relationship that substantially coincides with the rotational component of the fifth axis (RJ _m5 ) of the manipulator. For example, when the manipulator has the wrist described above, the present invention cannot be implemented with a turntable type positioner having only one rotary joint that rotates around the weight direction (Zw axis of the world coordinate system Σworld).

  The present invention is not limited to the above embodiment, and various modifications can be made. For example, the present invention can be applied even when both a moving device and a positioner are combined with a manipulator. Further, the determination of whether or not it is in the vicinity of the singular point is not limited to the expression (1), and the vicinity of the singular point may be determined by another method according to the configuration of the manipulator. Furthermore, the present invention provides not only singularity avoidance in real-time control during industrial robot operation, but also a manipulator and auxiliary device so that no singularity is generated in the course of the route in the offline teaching of the PTP type industrial robot. It can also be applied when creating position data.

1 is a schematic configuration diagram showing an industrial robot according to a first embodiment of the present invention. The block diagram of the control apparatus in 1st Embodiment. The flowchart for demonstrating operation | movement of the industrial robot which concerns on 1st Embodiment in this invention. The flowchart for demonstrating operation | movement of the industrial robot which concerns on 1st Embodiment of this invention. The conceptual diagram which shows the relationship between target joint angle Jmta (t) and preliminary | backup correction target joint angle Jmta (t) '. The schematic diagram which shows the behavior of the manipulator when not performing singularity avoidance operation | movement. The schematic diagram which shows the behavior of the manipulator when performing the singularity avoidance operation | movement of this invention. The schematic block diagram which shows the industrial robot which concerns on 2nd Embodiment of this invention. The block diagram of the control apparatus in 2nd Embodiment. The flowchart for demonstrating operation | movement of the industrial robot which concerns on 2nd Embodiment of this invention. The flowchart for demonstrating operation | movement of the industrial robot which concerns on 2nd Embodiment of this invention. The schematic block diagram which shows the industrial robot which concerns on 3rd Embodiment of this invention. The block diagram of the control apparatus in 3rd Embodiment. The block diagram of the control apparatus in 3rd Embodiment. The flowchart for demonstrating operation | movement of the industrial robot which concerns on 3rd Embodiment of this invention. The flowchart for demonstrating operation | movement of the industrial robot which concerns on 3rd Embodiment of this invention.

Explanation of symbols

DESCRIPTION OF SYMBOLS 11 Manipulator 11a Pedestal 12 Moving apparatus 13 Control apparatus 14 Teaching apparatus 16 Tool 17 Work 21 Path | route production | generation part 22 Tool target value calculating part 23 Moving apparatus target value calculating part 24 1st coordinate conversion part 25 Target joint angle calculating part 26 Singular point discrimination | determination Unit 27 Corrected moving device target value calculating unit 28 Second coordinate converting unit 29 Corrected target joint angle calculating unit 30 Drive unit 32, 42 Positioner 33 Positioner target value calculating unit 37 Corrected positioner target value calculating unit

Claims (7)

Industrial use comprising an articulated manipulator having a tool at the tip, a work machined by the tool and / or an auxiliary device for moving the manipulator, and a control device for operating the manipulator and the auxiliary device in synchronization. A robot,
The controller is
Tool target value calculation means for calculating a tool target value which is a target value of the position and orientation of the tool tip at the next time in a predetermined fixed orthogonal coordinate system;
Auxiliary device target value calculating means for calculating an auxiliary device target value which is a target value of the position and posture of the auxiliary device at the next time;
First coordinate transformation calculation means for transforming the tool target value into a manipulator coordinate system using the auxiliary device target value;
A target joint angle calculation means for calculating a target joint angle that is a target value of a joint angle of each joint of the manipulator at the next time by inverse kinematics calculation of the tool target value converted into the manipulator coordinate system;
Singularity determination means for determining whether or not the target joint angle is near a singularity;
When the singular point determination means determines the vicinity of the singular point, while maintaining the position and posture of the tool tip at the next time relative to the workpiece, so that the manipulator posture at the next time is out of the singular point vicinity range, Correction auxiliary device target value calculating means for correcting the auxiliary device target value to calculate a correction auxiliary device target value;
Second coordinate conversion calculation means for converting the tool target value in the fixed orthogonal coordinate system at the next time into a manipulator coordinate system using the correction auxiliary device target value;
A corrected target joint that calculates a corrected target joint angle, which is a target value of the joint angle of the manipulator at the next time, by inverse kinematics calculation of the tool target value converted into the manipulator coordinate system by the second coordinate conversion means. An angle calculation means;
If the singular point discriminating means does not discriminate near the singular point, the manipulator and the auxiliary device are driven by the target joint angle and the auxiliary device target value, while the singular point discriminating unit discriminates the vicinity of the singular point. Then, an industrial robot comprising: the manipulator and driving means for driving the auxiliary device according to the correction target joint angle and the correction auxiliary device target value. The correction assist device target value calculation means includes:
Calculating a preliminary corrected target joint angle obtained by correcting the target joint angle to be outside a predetermined singular point vicinity range;
Calculate the preliminary correction tool target value in the manipulator coordinate system by forward kinematics calculation of the preliminary correction target joint angle,
Calculating a correction value that is a difference between the preliminary correction tool target value and the tool target value in a manipulator coordinate system;
The industrial robot according to claim 1, wherein the correction assist device target value is calculated from the assist device target value using the correction value. The controller is
Tool path generation means for generating a tool path that defines temporal changes in the position and orientation of the tool tip in the fixed orthogonal coordinate system;
An auxiliary device path generating means for generating an auxiliary device path for defining a time change in the position and posture of the auxiliary device; and
The tool target value calculation means calculates the tool target value at the next time based on the tool path, and the auxiliary device target value calculation means calculates the auxiliary device at the next time based on the auxiliary device path. The industrial robot according to claim 1 or 2, wherein a target value is calculated.   The industrial robot according to any one of claims 1 to 3, comprising at least one of a moving device and a positioner as the auxiliary device.   The industrial robot according to any one of claims 1 to 3, wherein the auxiliary device includes at least one linear motion joint.   The industrial robot according to any one of claims 1 to 3, wherein the auxiliary device includes at least one rotary joint. The manipulator has a wrist with a vertical articulated type and six axes, a fourth axis is a roll, a fifth axis is a vent, and a sixth axis is a roll.
Only the positioner having only the rotary joint as the auxiliary device,
The industrial robot according to any one of claims 1 to 3, wherein at least one of the rotary joints of the positioner rotates in the same direction as the fifth axis of the manipulator.

Images