JP2015058497A - Horizontal multi-axis robot trajectory forming method and control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a trajectory forming method capable of improving an operation speed with respect to a robot having a vertical movement shaft.SOLUTION: A horizontal four-axis type robot provided with a shaft is set as a control object. When forming a trajectory for moving a fingertip from a beginning point to an ending point, a controller determines an allowable bending moment Mmax from a bending moment M and a torsional moment T operating on the shaft (S21). Further, the largest acceleration Amax of the fingertip is determined (S22) in accordance with mass of a fingertip part, the allowance bending moment Mmax and a vertical position d of the fingertip, the maximum acceleration Amax is converted to an acceleration restraint λ"max on a trajectory of the fingertip and acceleration conditions are set (S23).

Description

  The present invention relates to a trajectory generation method and control device for a horizontal multi-axis robot having a vertical movement axis for moving a shaft in a vertical direction.

  FIG. 10 schematically shows the configuration of a horizontal four-axis robot (see, for example, Patent Document 1). In the robot 11, the base of the first arm 13 that forms the first axis is connected to the upper end of the base 12, and the root of the second arm 14 that forms the second axis is connected to the tip of the first arm 13. It is connected. A shaft 15 that forms the third axis and moves in the vertical direction (vertical direction, Z-axis direction) is disposed at the distal end of the second arm 14, and rotates on the lower end side of the shaft 15 by the fourth axis. A hand 16 (end effector) is arranged.

JP 2012-196717 A

The shaft 15 moves in the vertical direction with the hand 16 holding the workpiece 17. When the hand 16 is at a position close to the ground contact surface of the base portion 2, the length of the shaft 15 extending downward from the lower surface of the distal end portion of the second arm 14 becomes longer. In this case, since the shaft 15 is bent and easily vibrates due to the weight of the hand 16 and the work 17, a certain restriction is applied to the acceleration for moving the shaft 15 up and down in order to suppress the vibration. Yes. That is, the vibration generated when the lower end of the shaft 15 is lowered most is limited so as to be within an allowable range.
However, in practice, the amount of vibration generated according to the Z-axis position of the hand 16 should be different. Nevertheless, there has been a problem that the working efficiency of the robot cannot be improved because the movement acceleration of the third axis is limited by a certain upper limit value.

  The present invention has been made in view of the above circumstances, and an object thereof is to provide a trajectory generation method and control device for a horizontal multi-axis robot capable of improving the operation speed of a robot having a vertical movement axis. It is in.

  According to the trajectory generation method for a horizontal multi-axis robot according to claim 1, first, in the position specification step, the start point and end point positions of the hand are specified, and in the trajectory generation step, the constraint condition related to driving of each axis is taken into consideration. By calculating the velocity pattern of each axis, the trajectory is generated so that the movement time is the shortest. At this time, for the vertical movement axis, the allowable bending moment is obtained from the bending moment and the torsional moment acting on the shaft as the hand located on one end side of the shaft moves. Then, when the maximum acceleration of the hand is determined according to the mass of the hand part, the allowable bending moment, and the vertical position of the hand, the maximum acceleration is converted into an acceleration constraint on the hand orbit and the acceleration condition is set. To do. As a result, the acceleration condition for the vertical movement axis is variably set within a range that satisfies the allowable bending moment acting on the shaft in accordance with the vertical position of the hand. Therefore, the time for moving the hand in the vertical direction can be shortened compared to the conventional method, and the working efficiency of the robot can be improved.

The flowchart which shows one Embodiment and shows the process which determines the maximum acceleration for moving the shaft of a robot to a perpendicular direction The figure which shows an example of the maximum acceleration determined by (11) Formula Schematic configuration diagram of the robot system Diagram showing robot simulation conditions The figure which shows a simulation result about the case of this embodiment, and the case of a prior art The figure which shows the Y-axis direction acceleration according to the Z-axis position of a hand about the case of this embodiment, and the case of a prior art. Diagram showing the physical parameters (dynamic parameters) of the 2-axis robot on the XY plane Flow chart for creating speed pattern executed by controller The flowchart which shows the detail of the production | generation of the speed pattern by the Boblow method which considered the constraint condition performed as a part of process of FIG. Diagram explaining the prior art

  Hereinafter, an embodiment will be described with reference to FIGS. FIG. 3 shows a system configuration of a general industrial robot. The robot system 1 includes a robot 2, a controller 3 (control device, control means) that controls the robot 2, and a teaching pendant 4 connected to the controller 3.

  The robot 2 is configured as, for example, a 4-axis horizontal articulated robot. The robot 2 includes a base 5 fixed to the installation surface, a first arm 6 connected to the base 5 so as to be rotatable about a first axis J1 having an axis in the Z-axis (vertical axis) direction, A second arm 7 that is rotatably connected around a second axis J2 having an axis center in the Z-axis direction on the tip of the first arm 6 and can be moved up and down to the tip of the second arm 7 And a shaft 8 that is rotatably provided. The axis when moving the shaft 8 up and down is the third axis J3, and the axis when rotating the shaft 8 is the fourth axis J4. A flange 9 is positioned and attached to the tip (lower end) of the shaft 8 so as to be detachable.

The base 5, the first arm 6, the second arm 7, the shaft 8 and the flange 9 function as an arm of the robot 2. Although not shown, a tool such as an air chuck or a hand is attached to the flange 9 (corresponding to the hand) that is the tip of the arm. A plurality of axes (J1 to J4) provided in the robot 2 are driven by motors (not shown) provided corresponding to the respective axes. In the vicinity of each motor, a position detector (not shown) for detecting the rotation angle (rotation position) of each rotation shaft is provided.
In general, an industrial robot operates according to a predetermined operation program created by performing teaching or the like in advance. The controller 3 feedback-controls the driving of the motor based on the operation program, and controls the operation of the arm of the robot 2.

  The teaching pendant 4 is, for example, a size that can be operated by being carried by a user or carried by a hand, and is formed in, for example, a thin, substantially rectangular box shape. The teaching pendant 4 is provided with various key switches, and the user performs various input operations using these key switches. The teaching pendant 4 is connected to the controller 3 via a cable, and performs high-speed data transfer with the controller 3 via a communication interface. An operation input by operating a key switch Information such as signals is transmitted from the teaching pendant 4 to the controller 3.

  Here, the Bobrow method is used to calculate the trajectory of the hand, and the speed pattern that is the fastest to move the hand while passing through each waypoint is calculated. When performing calculations using this Boblow method, the equation of motion of the actual robot is taken into consideration, so the speed pattern that becomes the fastest after satisfying the constraint conditions of the robot, such as the operation speed of each axis and the torque limit, is Desired.

Next, an example of a method for generating the trajectory information of the multi-axis robot described above and an example of a control apparatus for the multi-axis robot using the trajectory information will be described in detail. FIG. 7 shows the physical parameters (dynamic parameters) of the robot 2 on the XY plane. The Y axis is orthogonal to the X axis, and the front side shown in FIG. Each parameter in the figure shows the following contents.
m ₁ : mass of the first link [kg]
m ₂ : Total mass of the second link and load [kg]
r _G1 : distance from the first axis to the center of gravity of the first link r _G2 : distance from the second axis to the center of gravity of the second link 11: length of the first link [m]
I1: Inertia around the link center of gravity of the first link [kgm ² ]
I2: Inertia around the link center of gravity of the second link [kgm ² ]
θ1: Joint angle of the first axis [deg]
θ2: Joint angle of the second axis [deg]
The multi-axis robot 2 described above has four axes, and the trajectory generation method and control method according to this embodiment can be performed on any two or more of these four axes. However, in the following description, only the first and second axes will be described for calculation in order to easily understand the principle of trajectory generation and control.

Here, the following definitions are given for the physical quantities θ and λ used in the following description.
θ is the joint angle [rad] of each axis indicating the position of the robot.
θ ′ is the joint angular velocity [rad / s] of each axis indicating the speed of the robot.
θ ″: joint angular acceleration [rad / s ² ] of each axis indicating the acceleration of the robot.
λ: A trajectory parameter indicating each position on the trajectory.
λ ′: an inclination at each position on the track, indicating the speed of the track.
λ ″: Indicates the acceleration of the trajectory indicating the curvature at each position on the trajectory.

但し、τはトルクベクトルである。そして、（１）式における慣性行列Ｈと、遠心力・コリオリ力を表す行列ｈとに、ロボット２の物理的パラメータが反映されている。ここで、

（２）式におけるＩ_１１〜Ｉ_２２，Ｃ_１，Ｃ_２は、図７中に示したパラメータである。また、（１）式におけるＢは摩擦，ｇは重力であるが、以下ではこれらを無視して説明する。 The general notation of the motion equation of the multi-axis robot 2 described above is expressed by equation (1).

Where τ is a torque vector. The physical parameters of the robot 2 are reflected in the inertia matrix H in the equation (1) and the matrix h representing the centrifugal force / Coriolis force. here,

In the formula (2), I _{11 to} I ₂₂ , C ₁ and C ₂ are parameters shown in FIG. Further, B in the equation (1) is friction and g is gravity.

For details of the equation of motion of the robot, the following document (1) can be referred to.
・ <Reference (1)> Proceedings of the 2006 IEEE / RSJ International Conference on Intelligent Robots and Systems October 9-15,2006, Beijing China “Time-Optimal Trajectory Generation of Fast-Motion Planar Parallel Manipulator” by Yanjie Liu, Chenqi Wang , Juan Li, Lining Sun

  FIG. 8 is a flowchart executed by the controller 3 corresponding to the processing shown in FIG. First, for example, when the controller 3 reads the start point and end point of the hand of the robot 2 given by an operator (S1, position specifying step, position specifying means), three points between the start point and end point are set as initial via points ( (Interpolation point) is determined (S2, creation means). For example, linear interpolation is used to determine these initial via points. For example, if the start point coordinates are (0, 0) and the end point coordinates are (30, 100), the initial via points are (7.5, 25), (15, 50), (22.5, 75). .

Next, the controller 3 interpolates between the start point, the end point, and the above-mentioned three initial transit points by, for example, spline interpolation (S3), and uses the Bobrow method for the trajectory after the interpolation to minimize the movement time. A speed pattern is created (S4, trajectory generation step). For details of the Boblow method, see the following references (2) and (3).
・ <Reference (2)> The International Journal of Robotics Research, Vol.4, No.3, Fall 1985 p3-17 “Time-Optimal Control of Robotic Manipulators Along Specified Paths” by JE Bobrow, S. Dubowsky, JS Gibson
・ <Reference (3)> IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL.4, NO.4, AUGUST 1988 P443-450 “Optimal Robot Path Planning Using the Minimum-Time Criterion” by JAMES E. BOBROW

Next, the calculation process using the Boblow method performed in step S4 will be described with reference to FIG. First, equation (3) indicating the trajectory of the hand determined in step S3 x = f (λ) (3)
Thus, the trajectory parameter λ is converted into the hand position x (vector) (S11). In addition, the number attached | subjected to a type | formula is united with the number of literature (1) about what is described in literature (1). For example, when the trajectory f (λ) is interpolated by a cubic spline, the following equation is obtained.
f (λ) = aλ ³ + bλ ² + cλ + d (3a)
However, λ is an incremental variable of coordinates from the start point to the end point.

尚、Ｊはヤコビアンである。また、（５）式において摩擦Ｂ，重力ｇを夫々含むパラメータａ３，ａ４の項は「ゼロ」となる。 Subsequently, the hand position x is converted into an angle q of each axis (arm) by inverse kinematics (S12, conversion means).
q = p ⁻¹ (f (λ)) (4)
Where p is a function indicating forward kinematics.
Substituting equation (4) into equation (1) yields equation (5) (S13).

J is Jacobian. Further, in the equation (5), the terms of the parameters a3 and a4 including the friction B and the gravity g are “zero”.

  Next, in step S14, the controller 3 calculates the maximum acceleration λ ″ max and the maximum deceleration λ ″ min obtained from the equation (5) (these are (8) to (10) in the document (1)). See formula). The symbol “″” attached to “λ” is replaced with a double dot for indicating acceleration in the document (1). The maximum acceleration λ ″ max and the maximum deceleration λ ″ min obtained here are the constraint conditions regarding the acceleration of the operation of the robot 2.

However, when the third axis: shaft 8 is the object, the maximum acceleration λ ″ max and the maximum deceleration λ ″ min are obtained by the process shown in FIG. 1 without using the equation (5). First, an allowable bending moment Mmax is obtained based on the mechanical specifications of the robot 2 (S21). The allowable bending moment Mmax is calculated as follows. When the allowable bending stress of the shaft 8 is σ [N / mm ² ] and the section modulus is Z [mm ³ ], the bending moment M [N · mm] is expressed by the following equation.
M = σ × Z (6)
Further, when the allowable torsional stress of the shaft 8 is Ta [N / mm ² ] and the polar section coefficient is Zp [mm ³ ], the torsional moment T [N · mm] is expressed by the following equation.
T = Ta × Zp (7)

The equivalent bending moment Me [kgf · mm] and the equivalent torsion moment Te [kgf · mm] when the bending moment M and the torsion moment T are applied to the shaft 8 are calculated by the following equations.
Me = {M + √ (M ² + T ² )} / 2 (8)
Te = √ (M ² + T ² ) (9)
Among these, the minimum value is the allowable bending moment Mmax.
Mmax = Min (Me, Te) (10)

Next, according to the Z-axis position d of the hand of the robot 2, the maximum acceleration Amax of the hand is obtained by the following equation (S22). Note that m is the mass of the hand portion, and includes the mass of the hand and, if the hand is holding the workpiece, the mass of the workpiece.
Amax = Mmax / (m × d) (11)
FIG. 2 shows an example of the maximum acceleration determined by the above equation.

尚、（１２）式は、（３）式の左辺ｘをＡに置き換えて２階微分を行うことで導出される。ｆは上述のように軌道の関数であるから、ｆ´は軌道の傾きであり、ｆ″は軌道の曲率である。シャフト８に関しては、ステップＳ２３で求められた最大加速度λ″maxをステップＳ１４で使用する。 Here, from the above maximum acceleration Amax, the maximum acceleration λ ″ max on the hand trajectory is obtained by the equation (12) (S23).

In addition, (12) Formula is derived | led-out by replacing the left side x of (3) Formula with A, and performing a 2nd-order differentiation. Since f is a function of the trajectory as described above, f ′ is the trajectory inclination, and f ″ is the curvature of the trajectory. For the shaft 8, the maximum acceleration λ ″ max obtained in step S23 is set in step S14. Used in.

Next, the controller 3 calculates the next position λ and velocity λ ′ on the trajectory from the equations (13) and (14) using the maximum acceleration λ ″ max (or the maximum deceleration λ ″ min) (S15). ). Here, time t is a sampling time (for example, 1 ms), and λ0 and λ′0 are current positions and velocities. Further, “′” added to “λ” represents a first-order differentiation, and is replaced with a dot for indicating speed in the document (1).
λ = 0.5λ ″ maxt ² + λ′t (13)
λ ′ = λ ″ maxt (14)

Next, the controller 3 determines whether or not the speed λ ′ and the acceleration (deceleration) λ ″ of the trajectory parameter λ satisfy the respective constraint conditions (S16).
λ′min ≦ λ ′ ≦ λ′max (15)
λ ″ min (λ, λ ′) ≦ λ ″ ≦ λ ″ max (λ, λ ′) (16)
It is determined whether or not both conditions are satisfied simultaneously. Note that λ′min or λ′max and λ ″ min or λ ″ max are used depending on whether the vehicle is accelerating or decelerating.

Further, the limit values λ′min and λ′max of the constraint conditions regarding the speed λ ′ of the orbital parameter λ are
λ′min = max (q′min / f ′ (λ)) (17)
λ′max = min (q′max / f ′ (λ)) (18)
It is. Here, q′min and q′max are the minimum and maximum joint speeds determined by the specifications of the robot 2. These speed conditions and acceleration (torque) conditions are the minimum conditions necessary as constraint conditions, and other conditions may be added depending on the situation.

  If the speed λ ′ and the acceleration (deceleration) λ ″ of the trajectory parameter λ satisfy both the constraint conditions of the equations (15) to (18) in the above step S16 (step S16, YES), the controller 3 The position λ is updated to the value calculated in step S15 (S17) Next, the controller 3 determines whether or not the position λ has reached the end point λend (S18). Is returned to step S14, and if it has reached (YES), the process is terminated.

  On the other hand, when the speed λ ′ and the acceleration (deceleration) λ ″ do not satisfy at least one of the constraint conditions of the expressions (13) and (14) in step S16 (S16, NO), the controller 3 That is, the controller 3 performs the processing of S19, that is, the controller 3 sets a new maximum acceleration λ ″ max that is obtained by subtracting an arbitrary constant a from the maximum acceleration λ ″ max during acceleration, and the maximum deceleration λ during deceleration. A value obtained by subtracting an arbitrary constant a from "min" is set as a new maximum deceleration λ "min. Thereafter, the process returns to step S15 to recalculate the position λ and the velocity λ 'as trajectory parameters.

  When the processing shown in FIG. 9 is completed, the hand of the robot 2 moves on the trajectory obtained by interpolation in step S3 shown in FIG. 8 while satisfying the constraints on speed and acceleration (= torque). In addition, a speed pattern with the fastest moving speed is obtained. In other words, since the calculation process using the Boblow method is performed based on the kinematics of the robot 2, the constraint condition for the robot 2 can be satisfied.

  As described above, according to the present embodiment, the horizontal four-axis robot 2 including the shaft 8 that is a vertical movement axis is set as a control target. When the controller 3 generates a trajectory for moving the hand from the starting point, the shaft 8 moving in the vertical direction acts on the shaft 8 as the hand located on one end side of the shaft 8 moves. An allowable bending moment Mmax is obtained from the bending moment M and the torsional moment T. Then, when the maximum acceleration Amax of the hand is determined according to the mass of the hand portion, the allowable bending moment Mmax, and the vertical position d of the hand, the maximum acceleration Amax is set to the acceleration constraint λ ″ max on the hand trajectory. By converting, the acceleration condition is set, whereby the acceleration condition for the vertical movement axis is variably set within the range satisfying Mmax of the allowable bending moment acting on the shaft 8 according to the vertical position of the hand. The time for moving the hand in the vertical direction can be shortened compared to the conventional case, and the working efficiency of the robot 2 can be improved.

The present invention is not limited to the embodiments described above or shown in the drawings, and the following modifications or expansions are possible.
The present invention is not limited to four axes, and can be applied to any articulated robot that has at least a vertical movement axis (vertical linear movement axis, Z axis) and operates in cooperation with the vertical movement axis.

  In the drawings, 2 is a robot, 3 is a controller (control device, position specifying means, trajectory generating means), 6 and 7 are arms, and 8 is a shaft.

Claims (2)

For a horizontal multi-axis robot having a vertical movement axis that moves a shaft in a vertical direction, a method for generating a trajectory of the hand for moving a hand located on one end side of the vertical movement axis from a start point to an end point,
A position specifying step for specifying the positions of the start point and the end point;
A trajectory generating step for generating the trajectory so that the movement time is the shortest by calculating the speed pattern of each axis in consideration of the constraint condition relating to the driving of each axis;
In the trajectory generation step, the constraint condition includes an acceleration condition set for each axis,
For the vertical movement axis, as the hand located on one end side of the shaft moves, the allowable bending moment is obtained from the bending moment and torsional moment acting on the shaft,
According to the mass of the hand portion, the allowable bending moment, and the vertical position of the hand, the maximum acceleration of the hand is determined.
A method for generating a trajectory for a horizontal multi-axis robot, wherein the acceleration condition is set by converting the maximum acceleration into an acceleration constraint on the trajectory of the hand. For a horizontal multi-axis robot having a vertical movement axis that moves a shaft in a vertical direction, in a control device that generates a trajectory of the hand for moving a hand located on one end side of the vertical movement axis from a start point to an end point ,
Position specifying means for specifying the position of the start point and the end point;
A trajectory generating means for generating the trajectory so that the movement time is the shortest by calculating the speed pattern of each axis in consideration of the constraint condition relating to the driving of each axis;
The trajectory generating means sets the constraint condition as an acceleration condition set for each axis,
For the vertical movement axis, as the hand located on one end side of the shaft moves, the allowable bending moment is obtained from the bending moment and torsional moment acting on the shaft,
According to the mass of the hand portion, the allowable bending moment, and the vertical position of the hand, the maximum acceleration of the hand is determined.
A control apparatus for a horizontal multi-axis robot, wherein the acceleration condition is set by converting the maximum acceleration into an acceleration constraint on the hand trajectory.

Images