JP2015054360A - Speed control method for two-axis robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a speed control method that can exhibit a higher performance than convention with regards to a two-axis robot comprising a direct-acting shaft consisting of a ball screw shaft.SOLUTION: A two-axis XR robot enables a movable stage to move along a direct-acting shaft when a ball screw shaft is rotated by drive means. The movable stage is loaded with a joint and an arm rotationally driven and arranged so as to turn on the base point of a rotary shaft. When this XR robot is to be actuated, an upper limit speed of the movable stage is set to be variable with a movable stage position on the direct-acting shaft with regards to at least a partial range. Concretely, a dangerous rotation speed Nd of the ball screw shaft determined according to the movable stage position is compared with a DN value to determine it on the basis of an expression where the smaller value serves as a restraint requirement of the maximum speed.

Description

  The present invention relates to a method for controlling the speed of a biaxial robot having a linear motion shaft composed of a ball screw shaft fixed at both ends.

  Currently, a wide variety of industrial robots are used in production facilities and the like. As this industrial robot, there is a two-axis robot provided with a linear motion axis composed of a ball screw axis. This 2-axis robot is used for work such as welding, painting, parts assembly, and parts transportation. Due to the nature of the work it carries out, the operation time from the start point to the end point is shortened, that is, the operation speed is increased. Is required.

  In general, a trapezoidal speed pattern is often used as a basic operation speed pattern when controlling the operation speed of a robot (see, for example, Patent Documents 1 and 2). In the trapezoidal motion speed pattern, the upper side portion has a constant upper limit speed, and the acceleration is zero during the motion period corresponding to the upper side portion.

JP 2000-94371 A JP 2002-321178 A

  For the upper limit speed defined by the conventional trapezoidal operation speed pattern, the worst value is often used in consideration of safety. However, the upper limit speed is not necessarily set as a value close to an appropriate limit value based on the configuration adopted by the robot, and the original robot performance may not be fully exhibited. .

  The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a speed of a biaxial robot that can exhibit higher performance than the conventional one with respect to a biaxial robot having a linear motion shaft composed of a ball screw shaft. It is to provide a control method.

  According to the speed control method of the two-axis robot according to claim 1, the two-axis robot includes a linear motion shaft composed of a ball screw shaft fixed at both ends, and the ball screw shaft is rotated by the first driving means. Then, the moving body moves along the linear motion axis. The moving body is mounted with a second driving means and a rotating arm that is rotationally driven by the second driving means and is arranged so as to be rotatable about the rotation axis. When the two-axis robot is operated, the upper limit speed of the moving body is variably set according to the position of the moving body on the linear motion axis in at least a part of the range.

  That is, for the two-axis robot adopting the above configuration, the speed evaluated as the upper limit speed, the limit speed, or the dangerous speed varies depending on the position of the moving body on the linear motion axis. That is, in the state where the speed is limited as in the prior art, the performance of the robot cannot be sufficiently obtained. Therefore, the upper limit speed of the moving body is variably set in accordance with the position of the moving body on the linear motion axis in at least a part of the range, so that the upper limit speed is set higher depending on the position, and the moving body moves more than before. The working efficiency can be improved by reducing the time.

  According to the speed control method of the two-axis robot according to claim 2, the ball screw shaft critical rotation speed Nd and the DN value obtained according to the position of the moving body are compared, and the one showing the smaller value is maximized. This is a speed constraint. That is, for the structure in which the ball screw shaft is rotated to move the moving body linearly, the allowable rotational speed of the ball screw shaft is defined as described above. The DN value is a fixed value determined by the value K of the screw type on the ball screw shaft and the ball center diameter D. The dangerous rotational speed Nd is calculated using the position of the moving body on the linear motion shaft as a parameter. The maximum speed can be set according to the position.

  According to the speed control method of the biaxial robot according to claim 3, the position of the start point and the end point is generated when generating the hand trajectory for moving the hand located at the tip of the rotating arm from the start point to the end point. When the movement of the hand is started from the start point to the end point, a reaction that increases the acceleration force generated by the moving body by the action of the first driving means is caused by the action of the rotating arm. Further, when the hand is stopped at the end point, the speed pattern of the moving body and the rotating arm is generated so that the reaction of increasing the deceleration force generated by the moving body by the action of the first driving means is caused by the operation of the rotating arm. To generate the trajectory.

  In conventional multi-axis robot control, for example, when a simple work such as pick-and-place is performed, an axis whose movement amount does not change between the start point and the end point does not move. In this state, the pivot arm on the hand side is only a load of the first driving means on the base side, and the burden on the first driving means is heavy, and in order to increase the operating speed It is an obstacle.

In contrast, when the trajectory is generated as described above, the rotating arm operates so as to react against the movement of the moving body. Therefore, the acceleration force required at the time of starting the hand and the deceleration force required at the time of stopping are respectively increased, the speed at which the hand is moved from the start point to the end point can be improved, and the movement time can be shortened. here,
(Maximum joint acceleration) = (maximum joint torque + interference torque + gravity torque + friction torque)
/ Inertia (A) '
(Maximum joint speed) = (Maximum motor speed) / (Reduction ratio) (B)
It is. However, here, for simplification of explanation, only the horizontal operation is considered, and in the equation (A) ′, the gravity torque and the friction torque are ignored as the equation (A). For example, if only one shaft always outputs the maximum torque, and the upper limit of speed is not considered, as in Bang-Bang control, the arm is folded based on equation (A) to reduce the inertia. Therefore, shortening of the operation by improving the acceleration can be expected.

  However, most of the robots that are currently widely used have a high reduction ratio, and the maximum joint speed is lower than that in the formula (B), so the upper limit is reached soon. For this reason, the only way to shorten the operation time is to start up acceleration / deceleration steeply. In order to improve the acceleration / deceleration, it can be seen that it is effective to make the inertia smaller from the equation (A) and to make the sign of the interference torque term the same as the joint torque code (direction to assist). That is, the operation corresponds to swinging the rotating arm in the direction opposite to the moving direction of the moving body during acceleration / deceleration.

  At this time, the first driving means for rotating the linear motion shaft to be interfered can accelerate beyond the upper limit of the trapezoidal speed pattern described above. That is, the performance of the first driving means can be fully exhibited, and the acceleration can be increased more than before, so that the operation time can be shortened. In other words, if the operation is performed with the same movement time as the conventional method, less torque is required to output the same acceleration as the conventional method, so the capacity of the first driving means can be reduced, the robot can be reduced in size, Contributes to cost reduction. Since the above-described trajectory is generated as a result calculated based on the dynamics of the two-axis robot, a trajectory that maximizes the speed within a range that satisfies the constraint condition of the robot is obtained. Therefore, a trajectory that can be practically used is obtained.

  According to the speed control method of the biaxial robot according to claim 4, a plurality of initial via points are automatically set between the start point and the end point, and the speed pattern of the shortest time is set while updating the position of the initial via point. Since it is generated, it is possible to generate a trajectory of a speed pattern that minimizes the movement time from the start point to the end point.

  According to the two-axis robot speed control method of the fifth aspect, three initial waypoints are set. Assuming that the hand is moved from the start point to the end point, the time when acceleration is applied to the moving body immediately after startup, the time when the direction of acceleration is reversed during the operation, and the time when acceleration is zeroed immediately before stopping There is. In the course of optimizing the trajectory, it is considered important to optimize at least the waypoints corresponding to the above three points. Therefore, by setting the waypoints corresponding to the above three time points, it is possible to efficiently generate a trajectory that minimizes the travel time while suppressing the amount of calculation. As a result, in the conventional method, an optimized trajectory that can be practically used can be obtained while maximizing the potential of the motor and actuator as drive means that could not be extracted due to limitations of the method itself.

The figure which is 1st Embodiment and shows the result of having simulated the operation speed of the XR robot Plan view showing the configuration of the XR robot Schematic diagram explaining the effect of the trajectory generation method of the present application in an XR robot The figure explaining the mechanical numerical parameter of the XR robot Functional block diagram showing the configuration of the control device of the XR robot The figure which is 2nd Embodiment and shows roughly the process sequence for calculating | requiring the movement locus | trajectory of the shortest time of a robot. The figure explaining the principle of the method of controlling the movement of the arm of the robot compared with the conventional example Diagram showing physical parameters (dynamic parameters) of the 4-axis robot on the XY plane Flow chart for creating a speed pattern executed by the control unit 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 right and left hand systems of the robot arm The figure explaining the reason for calculating the speed pattern by converting the coordinate position of the hand to the angle of each axis Explanatory drawing explaining the constraint condition of acceleration The figure which shows the locus | trajectory in which the coordinate of a 1st axis | shaft, a 2nd axis | shaft, and a hand changes on XY plane. (A) is a diagram in which the five hand positions shown in FIGS. 14 (a) to (e) are plotted simultaneously, and (b) is the angle and angular velocity of each of the first and second axes while moving from the start point to the end point. , Diagram showing changes in angular acceleration and torque 1 equivalent diagram The figure explaining the case where each axis operation of an XR robot is synchronized FIG. 9 equivalent diagram showing the third embodiment. Fig. 10 equivalent

(First embodiment)
Hereinafter, the first embodiment will be described with reference to FIGS. 1 to 5. The XR robot shown in FIG. 2 includes a linear motion shaft J1 (X axis, first axis) having a ball screw shaft 1 and both ends fixed to a fixed base 3 by fixing means 2, and a nut on the linear motion shaft J1. A movable stage 5 movably held via a member 4 and a base side rotatably held via a joint 6 (including a joint and a motor: second driving means) arranged inside the movable stage 5 Arm 7 (rotating arm). The tip side of the arm 7 is a hand. The joint 6 forms a rotation axis (R axis, second axis) J2. The ball screw shaft 1 is rotationally driven by driving means 8 (first driving means) including a motor, which is disposed on the right end side in FIG. Thus, a biaxial XR robot 9 is configured.

  The operation of the XR robot 9 (joint 6 and driving means 8) is controlled by the control unit 11 of the robot controller RC based on a work process program, as shown in FIG. The control unit 11 (control device, control means) includes a CPU, a ROM, a RAM, and the like, and a teaching pendant 12 is connected thereto. The teaching pendant 12 is provided with a switch group 13 and a display unit 14 as display means. The arm rotation motor 6M is a motor constituting the joint 6, and the linear movement motor 8M is a motor built in the driving means 8.

  A single shaft such as a ball screw has an upper limit speed (referred to as a critical speed or a critical speed), and the value of this speed varies depending on the position from the fixed end of the shaft. In the case of driving with a conventional trapezoidal speed pattern, the upper side (upper limit speed) forming the trapezoid is a constant value and set to a value in consideration of the safety factor. That is, as shown in FIG. 3 as an example, the upper limit speed is given as a constant value less than a rotational speed of 2000 [rpm], for example.

However, the allowable rotational speed Np of a motor such as the actual drive means 8 is determined by the smaller of the critical speed N and DN value (rotation limit value) of the screw shaft (see, for example, JP-A-2006-198716). . That is,
Allowable rotational speed Np = Min (Nd, DN value) (1)
The DN value is calculated from the value (manufacturer value) K according to the screw type and the ball center diameter D as follows.
DN value = K / D (2)
The permissible rotational speed depends on the position of the robot is the critical speed Nd, which is calculated as follows. As shown in FIG. 4, the position of the robot travel axis (linear motion axis) is J1 [mm] (corresponding to the position P of the nut member 4 shown in FIG. 2), the mechanical stroke length is St [mm], and the movable range is Sf. Assuming [mm], the position La [mm] from the fixed end is
La = J1 + (St−Sf) / 2 (3)
It becomes. The inter-mounting distance Lb [mm] is given by the following equation using the one far from both ends.
Lb = Max (La, St−La) (4)
Therefore, assuming that the coefficient k and the screw shaft valley diameter d1 [mm] depend on the ball screw shaft mounting method (fixed / supported / free), the critical speed Nd is expressed by the following equation.
Nd = k × d1 × 10 ⁷ / Lb (5)
The upper limit speed defined based on the above equation (1) is shown in FIG. 3, and the portion where the upper limit speed is flat (about 3400 rpm) is defined by the DN value.

  Moreover, the result of having simulated the operation | movement of the XR robot according to the upper limit speed shown in FIG. 3 is shown to FIG. 1 (a), (c). FIG. 1B is a view corresponding to FIG. 3, and FIG. 1C is an enlarged view of FIG. In FIG. 1C, the alternate long and short dash line indicates that when the moving stage 5 (moving body) of the XR robot is moved (0 [mm] to 1200 [mm]), only the ball screw shaft 1 is rotationally driven as in the prior art. In this case (the upper limit speed is fixed), the movement time is about 907 ms.

  On the other hand, the broken line indicates that the moving stage 5 is moved only by the rotational drive of the ball screw shaft 1, but if the upper limit speed is varied based on the equation (1), the moving time is greatly reduced to about 596 ms. . Thereby, as schematically shown by the hatched area in FIG. 3, a speed area that has not been used up conventionally can be used effectively. That is, it is possible to move the hand with a shorter path and to save energy.

  As described above, according to the present embodiment, the biaxial XR robot 9 is such that when the ball screw shaft 1 is rotated by the driving means 8, the moving stage 5 moves along the linear movement axis. 5 is mounted with a joint 6 and an arm 7 that is rotationally driven by the joint 6 and is disposed so as to be rotatable about a rotation axis. When the XR robot 9 is operated, the upper limit speed of the moving stage 5 is variably set in accordance with the position of the moving stage 5 on the linear motion axis in at least a part of the range. Specifically, the critical rotational speed Nd and DN value of the ball screw shaft 1 obtained according to the position of the moving stage 5 are compared, and the one showing the smaller value is set as the maximum speed constraint condition (1). Determine based on the formula. Therefore, the upper limit speed can be set higher depending on the position, and the working time can be improved by shortening the movement time as compared with the prior art.

(Second Embodiment)
Hereinafter, a second embodiment will be described with reference to FIGS. In the second embodiment, a trajectory generation method for further improving the operation speed of the XR robot 9 will be described. First, the outline of the control method will be described with reference to FIGS. 6 and 7. First, the control method according to the second embodiment will be described in comparison with the conventional control method with reference to FIG.

  As shown in FIG. 7A, a simple two-axis (J1, J2) robot is assumed in which first and second arms (links) AM1, AM2 are movably coupled by two joints JT1, JT2. The arm end EF of the robot is moved from the angular position (J1, J2) = (0 °, 0 °) to the angular position (J1, J2) = (90 °, 0 °) on the XY plane. Assume that.

  As shown in FIG. 7B, the simplest method in this case is that the second arm AM2 (that is, the second axis J2) is not operated, and only the first arm AM1 (that is, the first axis J1) is operated. It is rotated 90 degrees. In this method, since only the first arm AM1 is operated, the load is unbalanced, the burden on the first arm AM1 is large, and the operating radius of the entire arm is always maximum and the inertia increases. For this reason, there exists a problem that operation speed becomes slow.

  FIG. 7C shows the operation by the above-described Bang-Bang control. In this case, the second arm AM2 is operated together with the first arm AM1 to fold the arm. Thereby, since inertia becomes small, it can be operated at high speed. However, since no constraint conditions such as speed constraints imposed on the robot are taken into consideration, it is impossible to actually reduce the operation time by operating in this way.

  In the second embodiment, an operation locus as schematically shown in FIG. 7D is realized. That is, when the movement of the hand EF is started, the first arm AM1 (first axis J1) is first rotated in the end point direction (see arrow A), and the second arm AM2 (second axis J2) is ended. Rotate in the opposite direction (see arrow A ′). At this time, the form of the arm is a so-called left-handed system.

  Thereafter, when the first arm AM1 is being rotated (see arrow A), when viewed from the first arm AM1, the hand position (the tip position of the second arm AM2) is the first arm AM1. The second arm AM2 is swung in the direction of rotation so as to approach the end point ahead of the tip position (see arrow B ′). At this time, the form of the arm is a so-called right-handed system.

  That is, in the case of the operation of FIG. 7B, the second arm AM2 and the hand EF are always located on the radius with the first arm AM1 as the rotation center. On the other hand, in the case of the operation shown in FIG. 7 (d), the second arm AM2 is initially rotated toward the start point direction (arrow A '), so that the rotation of the hand is delayed. However, after that, the second arm AM2 rotates in the direction of the end point ahead of the first arm AM1. That is, the hand EF is in a state preceding the first arm AM1.

  In this way, by cooperating the movements of the two arms AM1 and AM2 (the first axis and the second axis), the motors of the two joints JT1 and JT2 are applied as compared with the operation of FIG. 7B. The load can be distributed. Further, since the arm is slightly folded, the inertia becomes smaller and the load applied to the motor itself is reduced.

  In addition, the operation of the second arm AM2 generates a force associated with the reaction on the first arm AM1, that is, the first axis. That is, when the hand EF moves from the starting point, the urging force indicated by the arrow A ″ is applied to the first arm AM1 with the reaction of the temporary rotation of the second arm AM2 in the reverse direction (arrow A ′). This urging force A ″ is applied in a direction that urges the rotation of the first arm AM1 toward the end point. On the other hand, when the hand FE approaches the end point, the restraining force indicated by the arrow B ″ is increased due to the reaction of the preceding movement (arrow B ′) of the second arm AM2 in the previous rotation direction. That is, the restraining force B ″ acts in a direction to restrain the rotation (arrow A) of the first arm AM1 up to that time.

  As described above, when the hand EF starts to rotate and ends the rotation, the reaction force generated by the temporary operation of the second arm AM2, that is, the urging force and the suppressing force, is applied to the first arm. Actively assists in acceleration and deceleration of AM1 rotation. Therefore, the time required to move the hand EF from the angular position (J1, J2) = (0 °, 0 °) to the angular position (J1, J2) = (90 °, 0 °) is as described above. Compared to control without assist, the time is significantly shortened.

  Further, in order to perform the arm operation in this way, it is necessary to generate a speed pattern for rotating the arm, that is, the motor of each axis in advance. In generating this speed pattern, the most efficient motion trajectory is selected by the specific method described later on the premise that the constraint condition of the robot is satisfied.

  FIG. 6 schematically shows a processing procedure for realizing the motion trajectory as shown in FIG. First, a plurality of waypoints are temporarily set while moving the hand of the robot from the start point to the end point (FIG. 6A). Then, by using the Bobrow method to calculate the trajectory of the hand, a speed pattern that is the fastest for moving the hand while passing through each via point is calculated ((b) in the figure). 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.

  When the speed pattern is obtained, an operation time when the robot is operated from a predetermined start point to an end point according to the speed pattern is calculated ((c) in the figure). Then, the above-described process is repeatedly executed while updating the position of the via point ((d) in the figure), and the process is terminated when it is determined that the operation time has converged to the shortest. This series of processing is executed for each axis of the multi-axis robot. The trajectory obtained as a result becomes a trajectory that can move the hand from the start point to the end point in the shortest time after satisfying the robot constraint conditions (speed <speed limit value and torque <torque limit value). The information on the hand point and the velocity pattern of each axis (trajectory information) obtained in this way are stored in the robot controller and used for actual control of the multi-axis robot.

Next, with reference to FIGS. 8 to 17, an example of the method for generating the trajectory information of the multi-axis robot described above and an example of the control apparatus for the multi-axis robot using the trajectory information will be described in detail. FIG. 8 shows, on the XY plane, physical parameters (dynamic parameters) of a four-axis robot having a third axis and a fourth axis at the end of the arm 7 of the XR robot 9. M1, m2, and m3 shown in the figure are the mass [kg] of the first link, the mass [kg] of the second link, and the total mass [kg] of the “third and fourth links and load”, respectively. It is. lg2 [m] is the distance from the axial position of the first link to the center of gravity of the second link, and l2 [m] is the length of the second link. I2 [kgm ² ] is an inertia around the center of gravity of the second link, and I4 [kgm ² ] is an inertia around the total center of gravity of the third link and the fourth link. Θ1 is the joint position of the first axis, and θ2 is the joint angle of the second axis.

  The trajectory generation method and the control method according to the second embodiment can be performed on any two axes and a plurality of three or more axes among these four axes. Only the first and second axes will be described as calculation targets.

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

また、（６）式におけるＢは摩擦，ｇは重力であるが、以下ではこれらを無視して説明する。
尚、ロボットの運動方程式の詳細については、以下の文献（１）を参照できる。
・＜文献（１）＞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 Here, the following definitions are given for the physical quantities θ and λ used in the following description.
θ: joint angle of each axis indicating the position of the robot [rad]
θ ′: the joint angular velocity [rad / s] indicating the speed of the robot
θ ″: joint angular acceleration [rad / s ² ] of each axis indicating the acceleration of the robot
λ: Trajectory parameter indicating each position on the trajectory λ ′: Inclination at each position on the trajectory, indicating the speed of the trajectory.
λ ″: Indicates the acceleration of the trajectory indicating the curvature at each position on the trajectory.
The general notation of the equation of motion of the multi-axis XR robot 9 described above is expressed by equation (6).

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

Further, B in equation (6) is friction and g is gravity, but in the following description, these will be ignored.
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. 9 is a flowchart executed by the control unit 11 corresponding to the processing shown in FIG. First, when the control unit 11 reads the start point and end point of the hand of the XR robot 9 given by an operator (step S1, position specifying step, position specifying means), the control unit 11 sets three points between the start point and end point to an initial value. It is determined as a waypoint (interpolation point) (step 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 control unit 11 interpolates between the start point, the end point, and the above three initial via points by, for example, spline interpolation (step S3), and uses the Boblow method for the trajectory after the interpolation. A speed pattern that is the shortest is created (step S4). 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 control unit 11 calculates an operation time when the hand of the XR robot 9 (in this example, the second axis, that is, the tip of the second arm) is moved according to the speed pattern created for the interpolated trajectory ( Step S5), the difference between the operating time and the previously calculated operating time is obtained. Then, the control unit 11 determines whether or not the difference between the two is less than a predetermined threshold (for example, 0.001 s) (step S6), and if not (NO), the route point obtained in step S2 is determined. For example, it is updated using the downhill simplex method or the like (step S7). Of course, other methods such as a gradient method or a genetic algorithm may be used here.

  When the waypoint is updated, the process of the control unit 11 returns to step S3 and repeats the process up to step S7. In step S6, if the difference between the current operation time and the previous operation time is less than a predetermined threshold (YES), the process is terminated. Steps S3 to S7 correspond to a trajectory generation step (generation means).

Next, calculation processing using the Boblow method performed in step S5 will be described with reference to FIG. First, equation (8) indicating the trajectory of the hand determined in step S3 x = f (λ) (8)
Thus, the trajectory parameter λ is converted into the hand position x (vector) (step S11). For example, when the trajectory f (λ) is interpolated by a cubic spline, the following equation is obtained.
f (λ) = aλ ³ + bλ ² + cλ + d (9)
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 (step S12, conversion means).
q = p ⁻¹ (f (λ)) (10)
Where p is a function indicating forward kinematics. Here, the hand position x is converted into the angle q when the hand trajectory is obtained, as shown in FIG. 11, the hand system in the form of the arm of the XR robot 9 (FIG. 11 (a) is the left hand system, This is because FIG. 11B can be obtained without limiting the right hand system).

  That is, as described in FIG. 12 (however, the image of the robot is four horizontal axes), assuming that the calculation shown in FIG. 10 is performed using the coordinate position, it is assumed that the form of the arm is changed in the middle of the trajectory. Then (see FIGS. 12A and 12B), the constraint condition becomes discontinuous when the form changes, and the calculation result cannot be obtained. Therefore, when the calculation is performed using the coordinate position, it is necessary to perform the calculation without changing the form of the arm (in the reference (2) and (3), the form of the robot that is determined first is also used). To find the fastest speed pattern without changing from).

  On the other hand, if the calculation is performed after the coordinate position of the hand is converted into each axis angle in advance as in the second embodiment, the constraint condition is discontinuous even if the form changes in the middle of the trajectory. The calculation result can be obtained. Therefore, a speed pattern that is the fastest without being restricted by the arm configuration is required.

尚、Ｊはヤコビアンである。また、（１１）式において摩擦Ｂ，重力ｇを夫々含むパラメータａ３，ａ４の項は「ゼロ」となる。 Then, by substituting equation (10) into equation (6), equation (11) is obtained (step S13).

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

  Next, in step S14, the control unit 11 calculates the maximum acceleration λ′max and the maximum deceleration λ ″ min obtained from the equation (11) (these are (8) to (10) in the document (1). Note that the symbol “″” attached to “λ” replaces double dots for indicating acceleration in the document (1). The maximum acceleration λ ″ max and the maximum deceleration λ ″ min obtained here are constraint conditions regarding the acceleration of the operation of the XR robot 9.

Next, the control unit 11 calculates the next position λ and velocity λ ′ on the trajectory using the maximum acceleration λ ″ max (or the maximum deceleration λ ″ min) by the equations (12) and (13) ( Step 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 (12)
λ ′ = λ ″ maxt (13)

Next, the control unit 11 determines whether or not the speed λ ′ and the acceleration (deceleration) λ ″ of the trajectory parameter λ satisfy the respective constraint conditions (step S16).
λ′min ≦ λ ′ ≦ λ′max (14)
λ ″ min (λ, λ ′) ≦ λ ″ ≦ λ ″ max (λ, λ ′) (15)
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.

  FIG. 13 shows FIG. 2 schematically shows a range that satisfies the constraint condition regarding the acceleration (deceleration) λ ″ of the trajectory parameter λ for a biaxial robot. That is, each axis (motors arranged at each joint) The allowable range λ ″ min to λ ″ max taking the AND condition is set in the range of the maximum acceleration to the maximum deceleration defined in (1).

Further, the limit values λ′min and λ′max of the constraint conditions regarding the speed λ ′ of the orbital parameter λ are
λ′min = max (q′min / f ′ (λ)) (16)
λ′max = min (q′max / f ′ (λ)) (17)
It is. Here, f (λ) indicates the trajectory, and q′min and q′max are the minimum and maximum joint speeds determined by the specifications of the XR robot 9. 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 of the constraint conditions of the equations (14) and (15) (step S16, YES) in the above step S16, the control unit 11 Updates the position λ to the value calculated in step S15 (step S17) Next, the control unit 11 determines whether or not the position λ has reached the end point λend (step S18). (NO), the process returns to step S14, and if it has reached (YES), the process is terminated.

  On the other hand, in step S16, when the speed λ ′ and the acceleration (deceleration) λ ″ do not satisfy at least one of the constraint conditions of the equations (14) and (15) (NO in step S16), the control unit 11 In other words, the control unit 11 sets the maximum acceleration λ ″ max obtained by subtracting an arbitrary constant a as the new maximum acceleration λ ″ max during acceleration and the maximum during deceleration. A value obtained by subtracting an arbitrary constant a from the deceleration λ ″ 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 process shown in FIG. 10 is completed, the hand of the XR robot 9 moves on the trajectory obtained by interpolation in step S3 shown in FIG. 9 while satisfying the constraint conditions for speed and acceleration (= torque). In this case, a speed pattern with the fastest moving speed is obtained. That is, since the calculation process using the Boblow method is performed based on the kinematics of the XR robot 9, the constraint condition for the XR robot 9 can be satisfied.

  Although not specifically shown, in practice, the operation time according to steps S4 and S5 is calculated based on a reciprocating speed pattern from the start point to the end point. That is, the speed pattern from the start point to the end point is first calculated based on the algorithm described in step S4. Next, a return speed pattern from the end point to the start point is calculated. Then, the going speed pattern and the return speed pattern are synthesized, and the operation time is calculated for the synthesized speed pattern in step S5.

  Next, FIGS. 14 and 15 show an example in which the trajectory in which the moving speed of the hand of the XR robot is the shortest is obtained as described above. However, here, the critical speed of the ball screw shaft is not taken into consideration, and the calculation is performed at a constant upper limit speed. FIG. 14 (a) shows a locus in which the coordinates of the first axis, the second axis, and the hand on the XY plane change with solid lines, the start point coordinates are (−0.3, 0.3), and the end point. The coordinates are (0.3, 0.3). FIG. 15B shows changes in the angle, angular velocity, angular acceleration, and torque of each of the first axis and the second axis during the movement from the start point to the end point.

  The first axis moves linearly on the X axis from −0.3 [m] (see FIG. 14A) to 0.3 [m] (see FIG. 14E). That is, it corresponds to the movement stage moving along the linear movement rail. Further, the rotation angle θ2 of the second axis (rotation angle of the horizontal swing arm) is 0 [rad] at the start point and the end point, and the intermediate point on the X axis, respectively, but in FIG. Is rotated by 0.6 [rad] in the opposite direction. In this case, the X-axis coordinate of the hand position is on the negative side with respect to the coordinate of the moving stage, and the XR robot arm form is a right-handed system. In contrast, in FIG. 14D, the state is rotated to −0.6 [rad] (starting point direction). In this case, the X-axis coordinate of the hand position is on the positive side with respect to the coordinate of the moving stage, and the arm form of the XR robot is a left-handed system.

  In accordance with the above movement, the angular velocity of the second axis reaches the positive peak (10 [rad / s], peak point V2) immediately after moving from the starting point (V1), and from there, the negative peak at the intermediate point It changes linearly toward (−10 [rad / s], peak point V3). Moreover, it changes linearly from the intermediate point toward the positive peak (10 [rad / s], peak V4) immediately before the end point. That is, as shown in FIG. 15B, the speed pattern is symmetric with respect to the intermediate point.

The angular acceleration corresponding to the angular velocity pattern reaches a peak 400 [rad / s ² ] from 0 at the start point V1, and then has a negative value of about −50 [rad / s ² ] at the peak point V2. And the negative value of about −50 [rad / s ² ] is kept until the peak point V3 at the midpoint. Furthermore, when the peak value V3 of the intermediate point is switched to a positive value 50 [rad / s ² ], the value is kept until the peak point V4. The pattern reaches a negative peak of −400 [rad / s ² ] at the peak point V4 and returns to 0 at the end point V5.

  The following actions occur when the second axis operates as described above. The second axis changes the angle θ2 from 0 [rad] → 0.6 [rad] → 0 [rad] between the start of movement from the start point to the arrival of the intermediate point on the X axis. As a result, the hand of the XR robot moves so as to be swung back in the negative rotation direction from the state once swung in the positive rotation direction. In response to this, a reaction accompanying the movement occurs in the moving stage, and an accelerating force for accelerating the movement of the first arm when the movement is started is generated. This urging force is applied to the first and second arms. Assist (bias) the movement in the end direction.

  On the other hand, the second axis changes the angle θ2 from 0 [rad] → −0.6 [rad] → 0 [rad] until the end point is reached from the intermediate point on the X axis. The hand of the robot moves so as to be swung back in the positive rotation direction from the state once swung in the negative rotation direction. Also in this case, a reaction accompanying the movement occurs on the moving stage side and acts as a deceleration force. This deceleration force assists (suppresses) the deceleration movement until it reaches the end point and stops.

  As a result of the movement of the second axis as described above, the time required for the hand of the XR robot to move from the start point to the end point is shortened compared to the case where the simple operation is performed as in the conventional case. In FIG. 14, the trajectory when only the first axis is operated without rotating the second axis is indicated by a broken line. The travel time simulated for this trajectory is 536 [ms], whereas The travel time simulated for the trajectory indicated by the solid line was 514 [ms], and there was a shortening effect of over 4%. Therefore, if the movement time is the same as the conventional time, the XR robot can be operated with less energy, which can contribute to energy saving. Furthermore, for example, it is possible to reduce the wattage of the motor by reducing the output, and the robot can be reduced in size and cost.

  FIG. 15A is a plot of the five hand positions shown in FIGS. 14A to 14E simultaneously. These five points correspond to five points (V1 to V5) plotted on the angular velocity pattern of the second axis shown in FIG. 15B, that is, inflection points of angular velocity or angular acceleration. In step S2 shown in FIG. 9, three initial waypoints are set between the start point and the end point. However, setting the number of initial waypoints to 3 corresponds to the inflection point of the angular velocity or angular acceleration shown in FIG. The selection of what to do is based on the judgment that it is advantageous to improve the calculation efficiency.

For example, FIG. 4 and the related description, the Boblow method has an effect of shortening the operation time as the number of via points for searching for the trajectory is increased. However, increasing the number of waypoints directly increases the calculation time, so it is not practical to increase it indefinitely.
Therefore, in the second embodiment, the number of initial via points is set to three in consideration of the balance between shortening the operation time and suppressing the increase in calculation time. The selection of the initial via point does not need to be performed strictly, and may be determined by simply dividing the linear distance between the start point and the end point into four equal parts as described above. In the process of repeatedly executing the processing shown in FIG. 6 or FIG. 9, a new trajectory is obtained and optimized while the waypoint is changed.

  As a result, selecting a trajectory having the shortest operation time in an operation pattern such as pick-and-place means that three via points are angular velocities or angles of the second axis as shown in FIG. It can be said that the three inflection points of acceleration are determined by the coordinate position. Therefore, if the number of initial waypoints is 3, it is possible to obtain the trajectory with the shortest operation time while suppressing an increase in calculation time.

  FIG. 16 also shows an execution result obtained by adding the control of the second embodiment to FIG. 1 shown in the first embodiment. The two-dot chain line is calculated by using the Boblow method as in the second embodiment without performing the control of the first embodiment, and is caused by the reaction force generated by the arm 7 by rotating the rotation axis J2 during the movement. With the addition of assistance, the travel time is reduced to about 894 ms. A solid line is obtained by simultaneously performing the control of the first embodiment and the control of the second embodiment, and the moving time is shortened to about 574 ms. In comparison with the result shown by the alternate long and short dash line, this means that the movement time is shortened by about 37%.

In addition, as described in the first embodiment, by combining the control for varying the upper limit speed based on the expression (1) and the Boblow method, the following effects can be obtained. For example, as shown in FIG. 17A, for the XR robot 9, the first axis is moved from 0 [mm] to 1000 [mm], and the second axis is moved from 120 ° to 90 ° by −30 °. Is assumed. When such a multi-axis robot is operated in a coordinated manner, positioning is premised on simultaneous arrival and departure of all axes. As shown in FIG. 17 (b), the speed pattern (one-dot chain line) of the first axis in which the upper limit speed changes according to the position is aligned with the second axis shown in FIG. In general, it is difficult to correct and change.
However, if the Boblow method is used as in the second embodiment, the velocity pattern is determined in the orbital space, and as a result, the arrival and departure of the respective axes are automatically synchronized. Therefore, the user does not need to be particularly aware of synchronizing the operations of the axes.

  As described above, according to the second embodiment, when the control unit 11 moves the hand from the starting point, the reaction generated on the moving stage 5 side by the operation of the arm 7 is the ball screw that is the base axis of the moving stage 5. When the trajectory is generated so as to increase the acceleration force generated by operating along the axis 1 and the hand is stopped at the end point, the reaction that occurs on the moving stage 5 due to the movement of the arm 7 causes the moving stage 5 to move. The trajectory is generated so as to increase the deceleration force generated.

  More specifically, when starting the movement of the hand from the start point to the end point, the reaction that causes the moving stage 5 to increase the acceleration force generated by the ball screw shaft 1 and the moving stage 5 by the operation of the arm 7 is caused. When the hand is stopped at the end point, the speed of the moving stage 5 and the joint 6 is generated so that the reaction of increasing the deceleration force generated by the moving stage 5 is generated by the operation of the arm 7. Generate a trajectory for

  In conventional multi-axis robot control, for example, when a simple work such as pick-and-place is performed, an axis whose movement amount does not change between the start point and the end point does not move. That is, the operation pattern is indicated by a broken line in FIG. In such a state where the arm is fully extended, the second shaft continues to output useless holding torque so that the hand side arm does not move due to the inertia torque due to the high acceleration of the first shaft (traveling shaft). The load is only 8.

  On the other hand, when the trajectory is generated as described above, the arm 7 moves so as to react against the moving stage 5, and the acceleration force required when starting the hand and the deceleration force required when stopping the hand. And the speed of moving the hand from the start point to the end point can be improved to shorten the movement time. That is, by slightly rotating the arm 7 during the movement of the hand, the inertia when the moving stage 5 moves linearly is reduced, and the hand can be moved with less energy. Therefore, if the XR robot 9 is operated with the same movement time as the trajectory determined by the conventional method, the XR robot 9 can be operated with less energy consumption than in the past.

  Further, since the above-described trajectory is generated as a result calculated based on the dynamics of the XR robot 9, a trajectory that maximizes the speed within a range that satisfies the constraint condition of the robot is obtained. Thus, an optimized trajectory that can be used realistically is obtained. In addition, when calculating the speed pattern using the Boblow method, the position on the trajectory is converted into an angle, so the calculation is performed so that the arm form of the XR robot 9 is either left-handed or right-handed. Without being limited to the above, it is possible to obtain the speed pattern that is the fastest using both the left and right hand systems.

  Further, when the hand is moved from the starting point, the moving stage 5 is moved in the end point direction, and the arm 7 is rotated so that the tip thereof moves in the direction opposite to the end point direction. Thereafter, the tip of the arm 7 is rotated in the end point direction. At this time, the reaction caused by the operation of the arm 7 assists the acceleration force at the time of activation.

  Further, when the hand is stopped at the end point, a trajectory is generated so that the arm 7 is moved in the direction of the start point from a position where the hand, which is the tip, exceeds the end point. That is, the trajectory is generated and stopped so that the tip and the tip of the arm 7 are turned back toward the starting point. At this time, the reaction caused by the operation of the arm 7 assists the deceleration force at the stop. That is, by generating the trajectory in this way, the arm 7 can be operated so as to react against the moving stage 5.

  Then, three waypoints are set between the start point and the end point, and the movement time corresponding to each point is calculated while changing the position of the three points. That is, in order to generate a trajectory that minimizes the movement time of the hand, it is necessary to optimize the trajectory. In order to perform optimization, it is necessary to set one or more waypoints between the start point and the end point, and to compare the movement speeds required when the position of the waypoint is changed.

  Assuming that the arm 7 is operated as described above, in the ball screw shaft 1 and the moving stage 5 that are the base side axes of the arm 7, the acceleration is applied immediately after activation, and the acceleration is increased during the operation. There is a time point when the direction is reversed and a time point when the acceleration is made zero immediately before stopping. In the process of optimizing the trajectory, it is considered important to optimize at least the waypoints corresponding to the above three time points (see FIG. 6A). Therefore, by setting the waypoints corresponding to the above three time points, it is possible to efficiently generate a trajectory that minimizes the travel time while suppressing the amount of calculation. Thereby, in the conventional method, an optimized trajectory that can be practically used can be obtained while maximizing the potential of the motor and actuator that could not be extracted due to the limitations of the method itself.

(Third embodiment)
18 and 19 show the third embodiment, which corresponds to FIGS. 9 and 10. In the third embodiment, the execution timing of the step of converting the hand position x into the angle q of each axis is changed. That is, in the second embodiment, the inverse kinematics is solved to convert the hand position x into the angle q of each axis (see step S12). On the other hand, in the third embodiment, the conversion process to the angle q is performed at the stage before the creation of the shortest speed pattern, that is, at the stage of the interpolation process of the via point. As a result, computation of inverse kinematics can be avoided, and computation load is greatly reduced.

  In FIG. 18, the control unit 11 reads out information indicating a start point and an end point given by, for example, an operator (step S1), and generates three initial via points (step S2). Next, the control unit 11 converts the start point, end point, and initial via point in the hand space into corresponding angles in the joint space (step S2A). After this conversion, the control unit 11 proceeds to step S3, and generates a trajectory x = f (λ) by interpolating the start point, the end point, and the three initial via points. Next, in step S4A, as shown in FIG. 19, the processes of steps S13A and S14 to S19 are performed. Among these, in step S13A, the angle q is substituted into the motion equation of the robot.

と表される。上記以外の処理は第２実施形態と同様である。 At this time, the interpolated angle q is
q = f (λ), q ′ = f′λ ′, q ″ = f′λ ″ + f ″ λ ′ ² (18)
Therefore, the motion equation of the robot is

It is expressed. Processing other than the above is the same as in the second embodiment.

  Therefore, according to the third embodiment, the trajectory x is converted into the joint angle q before the trajectory x = f (λ) is interpolated. This eliminates the processing of solving the inverse kinematic equation described above, that is, the processing of steps S11 and S12 of FIG. That is, it is sufficient to solve the dynamics (forward kinematics) as it is in the joint angle space, and the calculation load is reduced accordingly.

The present invention is not limited to the embodiments described above or shown in the drawings, and the following modifications or expansions are possible.
Four or more initial waypoints may be set.
As a calculation method for obtaining a trajectory for maximizing the speed based on the dynamics of the multi-axis robot, a method other than the Boblow method may be used.

  1 is a ball screw shaft, 5 is a moving stage (moving body), 6 is a joint (second driving means), 7 is an arm, 8 is driving means (first driving means), 9XR robot (biaxial robot), 11 is A control unit (control device, position specifying means, creation means, conversion means, generation means, control means) is shown.

Claims (5)

A linear motion shaft consisting of a ball screw shaft with both ends fixed;
First driving means for rotationally driving the ball screw shaft;
A moving body arranged to be movable along the linear motion axis by rotating the ball screw shaft;
Two shafts that are mounted on the movable body and are configured to include second driving means, and a rotary arm that is rotationally driven by the second driving means and is arranged to be rotatable about the rotational axis. When operating the robot,
A speed control method for a two-axis robot, wherein an upper limit speed of the moving body is variably set in accordance with a position of the moving body on the linear motion axis in at least a part of a range.   2. The critical rotational speed Nd and DN value of the ball screw shaft determined according to the position of the moving body are compared, and the one showing a smaller value is set as the upper limit speed. Speed control method for a 2-axis robot. When generating the trajectory of the hand for moving the hand located at the tip of the rotating arm from the start point to the end point,
Specify the position of the start point and the end point,
When the movement of the hand is started from the start point toward the end point, the operation of the rotating arm causes a reaction that increases the acceleration force generated by the moving body by the action of the first driving means. ,
When the hand is stopped at the end point, the moving body and the rotating body are operated so as to cause a reaction to increase the deceleration force generated by the moving body by the action of the first driving means by the operation of the rotating arm. 3. The speed control method for a two-axis robot according to claim 1, wherein the trajectory is generated by generating a speed pattern of a moving arm. A plurality of initial via points are automatically set between the start point and the end point,
4. The speed control method for a two-axis robot according to claim 3, wherein the speed pattern having the shortest time is generated while updating the position of the initial waypoint.   5. The speed control method for a biaxial robot according to claim 4, wherein three initial waypoints are set.

Images