JP2008194760A - Robot arm and control method therefor - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a robot arm and a control method therefore capable of shortening a driving time to a target position. <P>SOLUTION: The robot arm drives a plurality of joints to move a robot hand to the target position. The arm includes: a hand track generating unit which generates a hand track; a hand speed computing unit 33 which computes hand speed when a hand moves on the hand track; a driving speed calculating unit 34 which calculates the driving speed of the joints based on the hand speed; a driving speed estimating unit 35 which estimates whether the driving speed of the joints exceeds a limit value of the driving speed or not; a hand speed correcting unit which corrects the hand speed when the driving speed exceeds the limit value; and an actuator 28 which drives the joints corresponding to the driving speed calculated based on the modified hand speed. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

  The present invention relates to a robot arm and a control method thereof, and more particularly to a robot arm having a plurality of joints and a control method thereof.

  A robot arm used as an industrial robot is generally composed of a link and a plurality of joints connecting the links. Such a robot arm moves to a target position by driving a joint. Then, a predetermined operation is performed at the target position. For example, the robot hand provided at the tip of the robot arm is moved to the target position to perform a predetermined operation. Therefore, work efficiency can be improved by shortening the drive time of the robot arm to the target position.

A control method for shortening the driving time of the robot arm is disclosed (Patent Document 1). In this control method, when driving the robot arm, the joint axis that maximizes the moving speed in the moving path is selected. Then, after setting the maximum speed that can be generated by the joint axis as the target speed, other joint speeds are corrected. In this way, the moving speed of the robot arm is determined.
JP-A-11-198072

  However, the control method described in Patent Document 1 has the following problems. In this control method, the maximum speed is set without considering how much the vehicle can be accelerated to the next target point. Therefore, if sufficient acceleration time cannot be obtained, the set maximum speed will not be reached. Further, the joint axis that maximizes (distance from the hand to each joint) × (maximum joint speed−current joint speed) is selected as the joint axis that should be the maximum speed. Therefore, there is a problem that it is not known whether the joint axis that maximizes the maximum speed is optimal. Furthermore, since the calculation is based on the information (position, posture) of the target trajectory, it cannot be applied in real time when the target position changes dynamically every moment. Thus, the conventional control method has a problem that the drive time cannot be shortened.

  The present invention has been made to solve such a problem, and an object of the present invention is to provide a robot arm that can shorten the drive time to a target position, and a control method therefor.

  A robot arm according to a first aspect of the present invention is a robot arm that drives a plurality of joints to move a hand to a target position, and a hand trajectory generating unit that generates a hand trajectory to the target position; A hand speed calculator for calculating a hand speed when moving according to the hand trajectory generated by the hand trajectory generator; and a driving speed of the joint based on the hand speed calculated by the hand speed calculator A driving speed calculating unit for calculating the joint speed, a driving speed evaluating unit for evaluating whether or not the driving speed of the joint exceeds a limit value of the driving speed, and when the driving speed exceeds the limit value, A correction unit that corrects the hand speed calculated by the speed calculation unit and outputs the correction to the driving speed calculation unit, and a driving speed calculated by the driving speed calculation unit based on the hand speed corrected by the correction unit Depending on, those comprising, an actuator for driving the joint. Thereby, since the drive speed of each joint can be controlled appropriately, the drive time to the target position can be shortened.

  A robot arm according to a second aspect of the present invention is the above-described robot arm, wherein the plurality of joints include joints for changing the posture of the hand, and is generated by the front hand trajectory generation unit. A posture angular velocity calculating unit that calculates an angular velocity of the hand posture when moving according to the hand trajectory, and the drive speed calculating unit calculates the joint drive speed based on the hand speed and the angular velocity. The correction unit corrects the hand speed and the angular velocity according to the evaluation result in the driving speed evaluation unit, and the actuator is based on the corrected hand speed and the corrected angular velocity. The joint is driven according to the driving speed calculated by the joint driving unit. Thereby, the posture angle of the hand can be appropriately controlled.

  A robot arm according to a third aspect of the present invention is the robot arm described above, wherein the hand speed calculation unit calculates, as a hand speed, a speed when accelerating at a predetermined acceleration from the current speed of the hand. It is characterized by doing. Thereby, since the hand is accelerated at a predetermined acceleration, the driving time can be shortened.

  A robot arm control method according to a fourth aspect of the present invention is a robot arm control method that drives a plurality of joints to move a hand to a target position, and generates a trajectory of the hand to the target position. A step of calculating a hand speed when moving according to the hand trajectory, a step of calculating a driving speed of the joint based on the hand speed, and a driving speed of the joint is a limit of a driving speed. A step of evaluating whether or not the value exceeds a value; a step of correcting the hand speed when the drive speed exceeds a limit value; and a drive speed calculated based on the corrected hand speed. And a step of driving the joint. Thereby, since the drive speed of each joint can be controlled appropriately, the drive time to the target position can be shortened.

  The robot arm control method according to a fifth aspect of the present invention is the robot control method described above, wherein the plurality of joints include joints for changing the posture of the hand and moves according to the hand trajectory. A step of calculating an angular velocity of the hand posture of the step, and a step of calculating a pre-front drive speed calculates the joint drive speed based on the hand speed and the angular speed, and corrects the hand speed. In the step of driving, when the driving speed exceeds a limit value, the hand speed and the angular speed are corrected. In the step of driving the joint, the corrected hand speed and the corrected angular speed are corrected. The joint is driven according to the drive speed calculated based on the above. Thereby, since the drive speed of each joint can be controlled appropriately, the drive time to the target position can be shortened. Thereby, the posture angle of the hand can be appropriately controlled.

  In the robot arm control method according to the sixth aspect of the present invention, in the step of calculating the hand speed, the speed when accelerated at a predetermined acceleration is calculated from the current speed of the hand as the hand speed. Is. Thereby, since the hand is accelerated at a predetermined acceleration, the driving time can be shortened.

  ADVANTAGE OF THE INVENTION According to this invention, it becomes possible to provide the robot arm which can shorten the drive time to a target position, and its control method.

Embodiment 1 of the Invention
A robot arm (hereinafter simply referred to as a robot) according to a first embodiment of the present invention will be described below with reference to FIG. In the present embodiment, description will be made assuming that the robot 1 is a two-joint manipulator.

  FIG. 1 is a diagram schematically showing the robot 1. The robot 1 has a link mechanism provided with a link and a joint (joint) for connecting the link. Specifically, the robot 1 includes a first joint 21, a first link 22, a second joint 23, a second link 24, and a hand 26. The arm portion 20 is configured by the first joint 21, the first link 22, the second joint 23, and the second link 24. The first link 22 and the second link 24 are members formed of rigid bodies. A hand 26 is provided at the tip of the arm unit 20. That is, a hand 26 that performs a predetermined operation is attached to the tip of the second link 24. The hand 26 performs operations such as gripping, painting, and welding of an object, for example. That is, when the hand 26 moves to the target position, a predetermined operation is performed. As described above, the two-joint manipulator includes the arm unit 20 and the hand 26. The arm unit 20 is connected to the main body unit 10. The main body unit 10 houses a control unit 11 that controls the arm unit 20.

  The first joint 21 and the second joint 23 are provided with actuators such as motors and sensors such as encoders. For example, each joint is provided with a servo motor that generates power, a gear that transmits the power of the servo motor, and a rotary encoder that measures the rotational displacement of the servo motor. The first joint 21 connects the main body 10 and the first link 22. The second joint 23 connects the first link 22 and the second link 24. That is, the first joint 21 supports the first link 22 so as to be rotatable, and the second joint 23 supports the second link 24 so as to be rotatable.

  The arm unit 20 is attached to the main body unit 10. Specifically, the first joint 21 is fixed to the main body 10. The main body 10 is provided with a control unit 11 for driving the first joint 21 and the second joint 23. The control unit 11 is a computer having a storage area such as a CPU and a memory. The control unit 11 is a computer having an arithmetic processing unit such as a so-called CPU, and controls the operation of the manipulator according to a predetermined program. For example, the control unit 11 includes a CPU (Central Processing Unit) that is an arithmetic processing unit, a ROM (Read Only Memory) and RAM (Random Access Memory) that are storage areas, a communication interface, and the like. Control the behavior. For example, the ROM stores a control program for control, various setting data, and the like. Then, the CPU reads the control program stored in the ROM and develops it in the RAM. Then, the program is executed in accordance with the setting data and the output from the sensor or the like.

  The arm unit 20 operates autonomously when the control unit 11 controls the driving speed, the joint angle, and the like. The first joint 21 and the second joint 23 are driven at a predetermined driving speed by a driving signal from the control unit 11. Then, when the hand 26 provided at the tip of the arm unit 20 moves to the target position, the driving of each joint is stopped. The first joint 21 and the second joint 23 are rotary joints with one degree of freedom. Although FIG. 1 shows that the rotation axes of the first joint 21 and the second joint 23 are parallel, the present invention is not limited to this. The control unit 11 obtains the driving speed of each joint at a constant control cycle. And the control part 11 is outputting the drive signal corresponding to the drive speed of each joint to a motor etc. Thereby, the hand 26 can be quickly moved to the target position.

  For example, in FIG. 1, the first joint 21 and the second joint 23 are driven to rotate about a direction perpendicular to the paper surface as a rotation axis. By driving the first joint 21, the first link 22 rotates with respect to the main body 10. Furthermore, when the second joint 23 is driven, the second link 24 rotates with respect to the first link. Thereby, the hand 26 can be moved to a predetermined position. That is, the position of the hand 26 is determined by the joint angles of the first joint 21 and the second joint 23. When the first joint 21 and the second joint 23 are driven, the hand 26 moves to the target position. The hand 26 performs a predetermined operation at the target position. For example, the hand 26 performs an object gripping operation or the like at the target position. Thereby, a desired operation can be performed.

  Here, the operation of the arm unit 20 will be described with reference to FIG. Here, as shown in FIG. 2, the movement start position is point A and the target position is point B. That is, a case where the hand 26 stopped at the point A moves to the point B and stops at the point B will be described. Note that the position of the hand 26 provided at the tip of the arm unit 20 is a hand position. Further, the speed of the hand 26 is a hand speed V. For example, the position of the center of gravity of the hand 26 is defined as the hand position, and the speed of the center of gravity is defined as the hand speed. Further, the driving speed of the first joint 21 is v1, and the driving speed of the second joint 23 is v2. Note that the joint drive speed is a value corresponding to the angular speed of the rotating joint in the case of a rotating joint. Accordingly, the driving speed is a value based on the angular speed of the motor that drives the joint and the gear ratio. Then, the link 22 rotates at the driving speed v1, and the link 24 rotates at the driving speed v2.

  In FIG. 2, the manipulator at the point A is indicated by the first joint 21, the first link 22a, the second joint 23a, the second link 24a, and the hand 26a, and the manipulator at the point B is indicated by the first joint 21, the first link 22b, and the first link. Two joints 23b, a second link 24b, and a hand 26b are shown. Here, the hand trajectory from the point A to the point B is a straight trajectory. Therefore, the description will be made on the assumption that the hand moves according to a straight path. That is, the hand moves from point A to point B with the shortest distance.

  Next, the control unit 11 that performs control for moving the arm unit 20 from the point A to the point B will be described with reference to FIG. FIG. 3 is a block diagram schematically showing the configuration of the control unit 11. The control unit 11 includes a target position storage unit 31, a hand trajectory generation unit 32, a hand speed calculation unit 33, a drive speed calculation unit 34, a drive speed evaluation unit 35, an acceleration storage unit 36, a hand speed correction unit 37, and a deceleration process determination unit. 38 and a joint drive unit 39. For example, the control unit 11 calculates a driving speed for driving the joint at a constant control cycle, and outputs a driving signal corresponding to the driving speed to the actuator 28. Here, the processing in the deceleration processing determination unit 38 in the hand speed calculation unit 33, the driving speed calculation unit 34, the driving speed evaluation unit 35, and the hand speed correction unit 37 is repeatedly executed for each control cycle. Thereby, the driving speed of the joint can be obtained. The joint drive unit 39 is, for example, a motor amplifier, and outputs a drive signal corresponding to the drive speed to the actuator 28. The actuator 28 is driven based on the drive signal. As a result, each joint is driven at a driving speed. And the joint drive part 39 outputs a drive signal for every control period. As a result, each joint rotates at an appropriate driving speed.

  As shown in FIG. 3, the detection signal from the encoder 27 is input to the control unit 11. For example, the encoder 27 detects the rotation angles of the motors provided at the first joint 21 and the second joint 23. Then, the encoder 27 outputs the rotation angle of the motor as a detection signal. The current position of the hand can be obtained by the encoder 27 provided at each joint. For example, the control unit 11 obtains the current position of the hand as an xyz coordinate value. Further, the control unit 11 obtains the current speed of the hand based on the detection signal from the encoder 27. For example, the control unit 11 calculates the angular velocity of each joint from the change amount of the encoder 27 to obtain the current velocity of the hand.

  In the target position storage unit 31, for example, a target position input by a user is stored. As the target position, the position of the hand is stored as an xyz coordinate value. The hand trajectory generator 32 generates a hand trajectory from the current position to the target position. The hand trajectory is calculated as, for example, a three-dimensional trajectory data matrix. The control unit 11 determines the hand trajectory based on the current position of the hand and the target position. This hand trajectory becomes the movement path of the hand. Here, the hand trajectory can be obtained as a set of a plurality of points (coordinates). Therefore, a set of a plurality of coordinates becomes a trajectory data matrix indicating the hand trajectory. The coordinates of a plurality of points on the straight trajectory from point A to point B shown in FIG. 2 are calculated as a trajectory data matrix. For example, in the trajectory data matrix, the coordinates of a large number of points arranged at regular intervals on a straight trajectory are arranged in order. And a hand moves in order on the point contained on this orbit data matrix. Here, the position of the point on the hand trajectory included in the trajectory data matrix is defined as the trajectory position. The trajectory position is indicated by xyz coordinate values.

  The hand trajectory generated by the hand trajectory generating unit 32 is input to the hand speed calculating unit 33. That is, the coordinates of the trajectory position constituting the hand trajectory are input to the hand speed calculation unit 33. Here, the coordinates of the trajectory position included in the trajectory data matrix are sequentially input. Further, the current position of the hand calculated based on the detection signal of the encoder 27 is input to the hand speed calculation unit 33.

The hand speed calculator 33 calculates the hand speed based on the current hand speed and the hand trajectory. The hand speed calculator 33 calculates the hand speed using the acceleration stored in the acceleration storage 36. The acceleration storage unit 36 stores a plurality of hand accelerations. And the hand speed when accelerating at one of the accelerations is obtained. Here, the hand speed in the case of accelerating at the maximum acceleration toward the next trajectory position is calculated from the current speed of the hand. Here, the maximum acceleration is the maximum acceleration obtained by driving all joints in the same direction. The maximum acceleration is a constant value and is stored in the acceleration storage unit 36. The maximum acceleration is determined by the performance (torque, rotation speed, etc.) of the motor that drives the joint, the weight of the link or hand 26, the gear ratio, and the like. The hand speed calculator 33 calculates a speed when heading to the next orbital position on the hand orbit from the current position of the hand. By adding maximum acceleration to the current speed of the hand, the hand speed can be obtained. Here, it is assumed that the current speed of the hand is V0 and the maximum acceleration is a _max . The maximum acceleration _amax is a positive value. In FIG. 2, the hand trajectory is a straight trajectory. For this reason, if the hand speed calculated by the hand speed calculating unit 33 is V, the hand speed V = V0 + a _max t. Note that t is a time corresponding to a control cycle for calculating the hand speed. As described above, the hand speed calculation unit 33 sets the speed at the time of acceleration at the predetermined maximum acceleration a _max from the current speed V0 as the hand speed V. The hand speed calculator 33 calculates the hand speed V for each control period and outputs the calculated hand speed V to the drive speed calculator 34.

  The hand speed V calculated by the hand speed calculator 33 is input to the drive speed calculator 34. The drive speed calculation unit 34 calculates the drive speed of each joint based on the hand speed V. Specifically, a driving speed necessary to reach the hand speed V is calculated for each joint using a transformation matrix such as an inverse Jacobian. Using the inverse Jacobian matrix, the hand speed V is decomposed into driving speeds v1 and v2 of each joint. Thereby, the driving speeds v1 and v2 for reaching the hand speed V from the current hand speed V0 can be obtained in one control cycle. Of course, the driving speed of the joint may be obtained by other methods.

  The drive speeds v1 and v2 calculated by the drive speed calculation unit 34 are output to the drive speed evaluation unit 35. The drive speed evaluation unit 35 evaluates whether or not the drive speeds v1 and v2 exceed the limit value of the drive speed. Here, limit values of the drive speeds of the first joint 21 and the second joint 23 are defined as limit drive speeds v1_limit and v2_limit, respectively. Therefore, the drive speed calculation unit 34 compares the drive speed v1 of the first joint 21 with the limit drive speed v1_limit, and compares the drive speed v2 of the second joint 23 with the limit drive speed v2_limit of the second joint 23. The limit drive speed is set for both rotation directions. That is, positive and negative limit drive speeds are set. Here, the limit drive speed set with respect to both rotation directions is made the same magnitude. Therefore, it is determined whether the absolute values | v1 | and | v2 | of the drive speeds v1 and v2 are equal to or higher than the limit drive speeds v1_limit and v2_limit, respectively.

  The limit drive speed can be calculated from dynamics based on the motor specifications, gear ratio, and the like. In this case, the limit drive speed is a constant value. Further, the limit drive speed may be calculated from the dynamics based on the motor specifications, the gear ratio, the attitude of the arm unit 20, the speed, and the like. In this case, the limit drive speed changes according to the posture and speed of the arm unit 20. The drive speed evaluation unit 35 determines whether all joints exceed the limit drive speed.

  When the joint drive speed does not exceed the limit drive speed, the drive speed evaluation unit 35 outputs the drive speeds v1 and v2 to the deceleration processing determination unit 38 as they are. That is, the driving speed based on the hand speed V calculated by the hand speed calculating unit 33 is output. When the joint drive speed exceeds the limit drive speed, the drive speed evaluation unit 35 outputs an evaluation signal indicating that the joint drive speed exceeds the limit drive speed to the hand speed correction unit 37. When the evaluation signal is input to the hand speed correcting unit 37, the hand speed correcting unit 37 corrects the hand speed. That is, the hand speed correcting unit 37 corrects the hand speed V calculated by the hand speed calculating unit 33 according to the evaluation result in the drive speed evaluating unit 35. Here, the corrected hand speed is defined as a corrected speed Vmod. The correction speed Vmod is lower than the hand speed V.

The correction speed Vmod can be obtained based on the acceleration stored in the acceleration storage unit 36. For example, it is assumed that three accelerations a = a _max , 0, and −a _max are stored in the acceleration storage unit 36. If the driving speed obtained based on the hand speed V when accelerating at the maximum acceleration a _max exceeds the limit driving speed, the hand speed when accelerating with the acceleration a = 0 from V0 is set as the corrected speed Vmod. That is, since the hand moves at a constant speed, Vmod = V0. Then, the drive speed calculation unit 34 calculates the drive speed of each joint based on the correction speed Vmod. Since the calculation of the driving speed here is the same process as described above, the description thereof is omitted.

Then, the drive speed evaluation unit 35 evaluates the drive speeds v1 and v2 calculated based on the correction speed Vmod. That is, it is determined whether or not the drive speed calculated based on the correction speed Vmod exceeds the limit drive speed. Similarly to the above, when the drive speed calculated based on the correction speed Vmod exceeds the limit drive speed, the hand speed is corrected. Here, the speed when acceleration a = −a _max , that is, when the vehicle is decelerated at the maximum acceleration a _max is the corrected speed Vmod. Therefore, the correction speed Vmod = V0−a _max t. In this way, the accelerations stored in the acceleration storage unit 36 are used in order from the one that further accelerates the hand.

  As described above, when the drive speed calculated based on the corrected speed Vmod does not exceed the limit drive speed, the drive speed is output to the deceleration processing determination unit 38. Therefore, the drive speed evaluation unit 35 outputs a drive speed that does not exceed the limit drive speed to the deceleration process determination unit. That is, when the drive speed calculated based on the correction speed Vmod exceeds the limit drive speed, the drive speed evaluation unit 35, the hand speed correction unit 37, and the drive speed calculation until the drive speed does not exceed the limit drive speed. The processing in the unit 34 is repeated.

  The deceleration processing determination unit 38 determines whether deceleration processing for stopping the hand at the target position is necessary. That is, the deceleration process determination unit 38 performs a deceleration process for stopping at the target position when the current position of the hand is sufficiently close to the target position. As a result, the driving speed of each joint begins to decelerate, and the hand stops at the target position. In this way, by starting deceleration from the current position, the driving speed of each joint can be made zero at the target position. Specifically, the deceleration process determination unit 38 obtains a distance from the current position to the target position, for example. Then, by decelerating at the maximum acceleration from the current position, it is determined whether the hand speed becomes 0 at the target position. When the target position is sufficiently far away, the driving speed can be reduced to zero before reaching the target position. Accordingly, the deceleration process determination unit 38 determines that the deceleration process is unnecessary, and outputs the drive speeds v1 and v2 input from the drive speed evaluation unit 35 to the joint drive unit 39 as they are. The joint drive unit 39 is, for example, a motor amplifier. A drive signal corresponding to the drive speed is output to the actuator 28 which is a motor.

  If the vehicle is approaching the target position, the driving speed cannot be reduced to zero until the target position is reached unless deceleration is started. In this case, the deceleration processing determination unit 38 determines that deceleration processing is necessary. Thereby, the driving speed when the driving speed of each joint is decelerated at the maximum acceleration is output to the joint driving unit 39. The joint drive unit 39 then outputs a drive signal corresponding to the drive speed when the deceleration process is performed to the actuator 28. This accelerates at the maximum acceleration in the direction opposite to the rotational direction in which each joint is driven. That is, the driving speed starts to decrease at the maximum acceleration, and the driving speed at the target position can be made zero.

  In this way, the deceleration process determination unit 38 determines whether or not the hand cannot be stopped at the target position unless the deceleration process is started. If the hand cannot stop at the target position, the deceleration process is immediately started. That is, when the current position of the hand is sufficiently close to the target position to such an extent that the driving speed cannot be stopped at the target position, the deceleration process is started. Then, the drive speed when the current speed is decelerated at the maximum acceleration until the hand reaches the target position is output. Therefore, the vehicle decelerates at the maximum acceleration from the current speed. As a result, the driving speed at the target position becomes zero, and the hand can be stopped. That is, the speed of the hand can be reduced to zero at the target position by starting deceleration.

  The joint drive unit 39 outputs a drive signal based on the drive speed to the actuator 28. The actuator 28 drives each joint based on the drive signal. As a result, each joint is driven at the driving speed calculated by the control unit 11. And the control part 11 repeatedly performs said process with a fixed control period, and outputs a drive signal. For example, the coordinates of the next trajectory position on the hand trajectory generated by the hand trajectory generating unit 32 are input to the hand speed calculating unit 33. The hand speed calculator 33 similarly calculates the hand speed for the next trajectory position. And the process in the drive speed calculation part 34, the drive speed evaluation part 35, the hand speed correction part 37, and the deceleration process determination part 38 is performed similarly. In this way, the driving speed for the next orbital position is calculated, and a driving signal corresponding to the driving speed is output to the actuator 28. Therefore, the driving speed is updated at the control cycle. In this way, the driving speed of each joint is controlled.

  Here, when it is determined that the deceleration process is not necessary, the drive speed from the drive speed evaluation unit 35 is output to the joint drive unit 39. Accordingly, a driving speed that does not exceed the limit driving speed is input to each joint. As a result, the actuator 28 drives each joint at an appropriate driving speed. That is, since the limit drive speed can be set individually for each joint, an appropriate limit drive speed can be set for each joint. Therefore, the limit drive speed can be increased. The driving speed of each joint can be increased, and the driving time can be shortened.

Next, the hand speed and the driving speed when moving according to the trajectory shown in FIG. 2 will be described with reference to FIG. FIG. 4 is a diagram showing a control pattern of the hand speed and the driving speed, and shows the hand speed V, the driving speed v1 of the first joint 21, and the driving speed v2 of the second joint 23 in order from the top. In FIG. 4, the horizontal axis represents time, and the vertical axis represents the hand speed or the driving speed. Limit drive speeds v1_limit and v2_limit are set for the drive speed v1 and the drive speed v2, respectively. In the following description, it is assumed that the movement starts when t = 0 and the movement stops when t = t8. In addition, the triangular pattern shown with the dotted line in Fig.4 (a) has shown the control pattern when moving to a target position in the shortest time. That is, after gradually accelerated at the maximum acceleration a _max, control pattern is shown when gradually decelerated by the maximum acceleration a _max.

In the section from t = 0 to t1, the hand speed increases with a constant slope. That is, the hand is accelerating at a constant acceleration. In this section, the driving speed is sufficiently smaller than the limit driving speed. Therefore, since the hand speed V when accelerating at the maximum acceleration _amax does not exceed the limit speed, the acceleration is performed at the maximum acceleration _amax . That is, hand speed V = Vo + a _max t. Further, in this section, the driving speeds v1 and v2 are also increased with a constant slope. That is, the value of the driving speed of each joint gradually increases.

  Next, in the section of t = t1 to t2, the hand speed V is constant. That is, the hand speed V does not change in this section. As shown in FIG. 4, when t = t1, the driving speed v1 of the first joint 21 reaches the limit driving speed. Therefore, if the driving speeds v1 and v2 are calculated by accelerating from the hand speed at t = t1, v1 exceeds the limit driving speed v1_limit. Therefore, the hand speed is corrected with the acceleration a = 0. That is, the hand speed when the acceleration a = 0 is the correction speed Vmod. Then, driving speeds v1 and v2 are calculated based on the correction speed Vmod. In this section, the drive speed v1 is close to the limit drive speed v1_limit. For this reason, if the hand speed V is accelerated, the drive speed v1 exceeds the limit drive speed v1_limit. Therefore, the hand moves at a constant speed until t = t2. In this section, since the driving speed v2 does not exceed v2_limit, the driving speed v2 increases with a constant slope. That is, the driving speed v2 increases with a constant slope in the interval t = t0 to t2. On the other hand, the driving speed v1 decreases with a constant inclination. That is, if the hand speed is accelerated in this section, the driving speed v1 exceeds the limit driving speed. For this reason, the acceleration a = 0. In this section, the hand speed is constant as the driving speed v2 increases and the driving speed v1 decreases. As described above, in the section of t = t1 to t2, the hand speed is not accelerated but is made constant. At t = t2, the driving speed v2 reaches the limit driving speed v2_limit.

In the interval from t = t2 to t3, the hand speed V decreases with a constant slope. That is, the hand acceleration a = −a _max and the hand decelerates at the maximum acceleration a _max . In this section, even if the acceleration a = 0 and the hand speed is corrected, the driving speed exceeds the limit driving speed. Therefore, the hand speed when the acceleration a = −a _max is the correction speed Vmod. For example, at the hand speed at the hand acceleration a = a _max , the drive speeds v1 and v2 both exceed the limit drive speed. In addition, at the hand speed at the hand acceleration a = a _max , one of the drive speeds v1 and v2 exceeds the limit drive speed. Therefore, the hand speed is reduced. In this section, the drive speed v1 decreases with a constant slope, and the drive speed v2 is constant at the limit drive speed v2_limit.

In the section from t = t3 to t4, the hand speed V is constant. In this section, if the hand is accelerated at the maximum acceleration a _max , the driving speed v2 exceeds the limit driving speed. However, when the hand is driven at a constant speed, the drive speeds v1 and v2 do not exceed the limit drive speed. Therefore, the hand speed is corrected, and the hand speed when the acceleration a = 0 is set as the corrected speed Vmod. In this section, the drive speed v1 is constant, and the drive speed v2 is constant at the limit drive speed v2_limit.

In the section from t = t4 to t5, the hand speed V is constant. In this section, if the hand is accelerated at the maximum acceleration a _max , the driving speed v2 exceeds the limit driving speed. However, when the hand is driven at a constant speed, the drive speeds v1 and v2 do not exceed the limit drive speed. Therefore, the hand speed is corrected, and the speed when the acceleration a = 0 is set as the corrected speed Vmod. In this section, the driving speed v1 increases with a constant slope, and the driving speed v2 decreases with a constant slope. That is, the amount of change between the driving speed v1 and the driving speed v2 cancels out, and the hand moves at a constant speed. In this way, the increase in the drive speed v1 and the decrease in the drive speed v2 are controlled so that the hand speed V is constant. In this section, since the driving speed v1 is sufficiently lower than the limit driving speed, the driving speed v1 can be accelerated.

In the interval from t = t5 to t6, the hand speed V increases with a constant slope. That is, since the acceleration _{a =} drive speed calculated as _{a max} v1, v2 does not exceed the limit driving speed. There is no need to correct the hand speed. Therefore, the drive speed is determined based on the hand speed when accelerating at the maximum acceleration a _max . In this section, the driving speed v1 increases with a constant slope, and the driving speed v2 decreases with a constant slope. Further, the slope of the driving speed v1 in this section is larger than the slope at t4 to t5. In addition, the gradient of the driving speed v2 in this section is the same as the gradient at t4 to t5. Thus, in this section, the driving speed v1 increases and the driving speed v2 decreases, whereby the hand is accelerated.

In the section from t = t6 to t7, the hand speed V is constant. In this section, if the hand is accelerated at the maximum acceleration a _max , the driving speed v1 exceeds the limit driving speed. However, when the hand is driven at a constant speed, the drive speeds v1 and v2 do not exceed the limit drive speed. Therefore, the hand speed is corrected, and the speed when the acceleration a = 0 is set as the corrected speed Vmod. In this section, the driving speed v1 increases with a constant slope, and the driving speed v2 decreases with a constant slope. The inclination of the driving speed v1 in this section is smaller than the inclination at t5 to t6. Further, the gradient of the driving speed v2 in this section is the same as the gradient at t5 to t6. In this way, the increase in the drive speed v1 and the decrease in the drive speed v2 are controlled so that the hand speed V is constant.

In the section from t = t7 to t8, the hand speed V decreases. Here, since the target position is approached, deceleration processing is executed. That is, if deceleration is not started at the time t = t7, the target position will be exceeded. For this reason, deceleration is started at the maximum acceleration a _max . In this section, the vehicle decelerates at a constant acceleration regardless of the limit drive speed. When t = 8, the hand speed V = 0 and the movement stops. That is, the driving speed v1 is 0 and the driving speed v2 is 0.

As described above, the hand speed is in three stages of ± a _max and 0, but the driving speed and the inclination obtained thereby change in small increments. That is, since the driving speed of each joint can be finely controlled, the control can be performed at an appropriate driving speed. Furthermore, in the present embodiment, a limit value is set for the driving speed of each joint, not the hand speed. For this reason, it is possible to set the limit driving speed without considering complicated conditions. Therefore, the limit drive speed can be increased and the joint can be driven quickly. Thereby, compared with the case where a limit value is set for the hand speed, the hand can be moved faster and the driving time can be shortened. That is, the limit value of the hand speed may change depending on the posture of the arm unit 20. In this case, there are postures in which the hand speed can be increased and postures incapable of being on the hand trajectory. When moving on a trajectory in a posture where the hand speed cannot be increased, it is necessary to set the limit value of the hand speed low. When a limit value is set for the hand speed, a low limit value must be set for the entire track. In this case, since the hand speed cannot be increased, the hand cannot be moved in a short time. On the other hand, in the present embodiment, it is possible to set an appropriate limit driving speed (limit value) for each driving speed. For this reason, a hand can be moved at high speed. Thereby, drive time can be shortened.

Next, a control method according to the present embodiment will be described with reference to FIG. FIG. 5 is a flowchart showing a method for controlling the robot arm according to the present embodiment. FIG. 5 shows processing from the hand speed calculator 33. First, in order to calculate the hand speed, the current speed, the trajectory position, and the acceleration are obtained (step S101). The current speed of the hand can be obtained from the detection signal of the encoder 27, for example. The trajectory position corresponds to the coordinates of a point on the trajectory generated by the hand trajectory generator 32. The trajectory positions on the hand trajectory are sequentially output from the hand trajectory generation unit 32. That is, the trajectory position from the movement start position to the target position is sequentially output from the movement start position side to the target position side. The maximum acceleration a _max is used as the acceleration. That is, the maximum acceleration among the accelerations stored in the acceleration storage unit 36 is output so that the hand speed is the fastest. The acceleration direction of the maximum acceleration is the same direction as the current speed direction.

Next, the hand speed is calculated (step S102). That is, the hand speed calculator 33 calculates the hand speed V based on the current speed, the trajectory position, and the acceleration. Specifically, the hand speed V when the vehicle is accelerated at the maximum acceleration a _max from the current speed toward the orbit position is calculated. Then, the driving speed of each joint is calculated (step S103). That is, the drive speed calculation unit 34 calculates the drive speed for each joint. Here, as described above, the driving speed of each joint can be obtained using the inverse Jacobian.

  Then, it is determined whether or not the driving speed of each joint exceeds the limit (step S104). That is, the drive speed evaluation unit 35 compares the drive speed with the limit drive speed and evaluates whether the drive speed exceeds the limit drive speed. If the limit drive speed is exceeded, the hand speed is corrected (step S105). That is, the hand speed correction unit 37 changes the acceleration to obtain the correction speed Vmod. Here, the acceleration next to the maximum acceleration among the accelerations stored in the acceleration storage unit 36 is used. Accordingly, the hand speed when the acceleration a = 0 is set as the correction speed Vmod. Based on the correction speed Vmod, the drive speed of each joint is calculated (S104). Then, the hand speed is corrected until the drive speed does not exceed the limit (steps S104 and S105).

If a drive speed that does not exceed the limit drive speed is obtained, it is determined whether or not to start deceleration processing (step S106). That is, when the drive speed calculated based on the hand speed in step S102 does not exceed the limit drive speed, or the drive speed calculated based on the correction speed in step S105 does not exceed the limit drive speed. In this case, a deceleration process determination is performed.
If it is determined that the deceleration process is unnecessary, the joint drive speed is determined as it is (step S108). On the other hand, if it is determined that deceleration processing is necessary, deceleration processing is performed (step S107). That is, if the vehicle cannot stop at the target position unless it approaches the target position and starts deceleration, the vehicle immediately decelerates at the maximum acceleration. Then, deceleration processing is executed to determine the drive speed (step S108). That is, the driving speed obtained from the hand speed decelerated at the maximum acceleration is used. This drive speed is amplified by an amplifier and a drive signal is output (step S109). Thereby, the process in one control cycle is completed. Then, the process returns to step S101, the current speed and the trajectory position are updated, and the same process is performed again. In this way, the joint can be driven. By such a control method, the driving speed of each joint can be set appropriately, so that the driving time can be shortened.

  As described above, control is performed in consideration of the current arm state (current speed, current position of the hand, distance to the target position, etc.) for each control cycle. Can be controlled. As a result, even when the target position moves, it is possible to respond in real time. In this case, for example, a target trajectory from the current position is generated for the moved target position, and the same processing is performed. Therefore, it is possible to cope with real time. In the above description, it is assumed that all the joints are rotary joints, but some of the joints may be linear motion joints.

In the above description, the acceleration a is set at three levels, but may be set at two levels, or may be set at four or more levels. By increasing the number of accelerations a stored in the acceleration storage unit 36, finer control can be performed. That is, an appropriate acceleration can be selected from a plurality of accelerations stored in the acceleration storage unit 36. In this case, the drive speed at the maximum acceleration _{a max} is Once beyond the limit driving speed, accelerates at a maximum acceleration _{a max} low acceleration _{a 1} than. In this way, the hand speed can be set more finely. Therefore, the travel time can be further shortened. The calculation time increases by increasing the number of accelerations to be set. That is, there is a possibility that the number of executions of step S104, step S105, and step S103 increases. For this reason, it is preferable to increase the number of accelerations set to such an extent that the processing can be completed within an assumed control cycle.

Embodiment 2 of the Invention
In the present embodiment, the above control is applied to the control of the hand posture in the multi-degree-of-freedom manipulator. For example, in the robot 1 shown in FIG. 1, a joint that changes the posture of the hand such as a wrist joint is added. For example, a wrist joint that drives the hand 26 is provided between the second link 24 and the hand 26. Thereby, the posture (rolling, pitching, yawing) around the axis of the hand 26 can be changed. That is, the hand 26 rotates around the axis. For example, the hand 26 rotates with the direction in which the second link 24 is provided as the rotation axis. As a result, the axis around the hand 26 becomes the driving direction, and the hand posture changes. The control according to the present embodiment can also be applied to a manipulator having 6 degrees of freedom or 7 degrees of freedom. In such a manipulator, the position of the hand and the posture of the hand are changed by a plurality of joints. Further, the position and posture of the hand are changed by one joint.

  The robot according to this embodiment will be described with reference to FIG. FIG. 6 is a block diagram schematically showing the configuration of the control unit 11 of the robot 1 according to the present embodiment. Note that the basic configuration of the robot 1 is the same as that of the first embodiment, and a description thereof will be omitted. In addition, since the basic configuration of the control unit 11 is the same as that of the first embodiment, the following description will mainly describe differences from the first embodiment. That is, the control of the hand position is the same as that of the first embodiment, and thus the description thereof is omitted. In the present embodiment, for example, a wrist joint for changing the hand posture is provided. As the wrist joint rotates, the posture of the hand changes. That is, the hand tip rotates around the axis, and the hand posture changes. Note that the hand posture may be changed by a joint other than the wrist joint, and the hand posture may be changed by two or more joints. In the present embodiment, in order to control the hand posture, a posture angular velocity calculation unit 40 is provided in addition to the configuration of the control unit 11 shown in FIG. Further, a correction unit 41 is provided instead of the hand speed correction unit 37.

  In the present embodiment, the hand trajectory generating unit 32 calculates not only the hand trajectory position but also the hand posture on the hand trajectory. That is, the hand trajectory is generated by associating the hand posture angle (rotation angle around the hand axis) with the coordinates of the trajectory position. The trajectory of the hand position of the hand trajectory generating unit 32 is output to the hand speed calculating unit 33 as in the first embodiment. The hand speed calculator 33 calculates the hand speed as in the first embodiment. On the other hand, the trajectory of the hand posture is output to the posture angular velocity calculation unit 40 as a posture angle. Therefore, the posture angle up to the posture that becomes the target posture at the stop position is sequentially output to the posture angular velocity calculation unit 40. The posture angular velocity calculating unit 40 increases the angular velocity by a preset change amount from the angular velocity of the current posture toward the posture angle. As a result, the posture angular velocity ω of the hand can be calculated. Therefore, control is performed so as to follow the posture angle of the hand on the trajectory generated by the hand trajectory generating unit 32. The current posture angle of the hand can be obtained by the encoder 27.

  Then, the drive speed calculation unit 34 calculates a drive speed that satisfies the hand speed and the posture angular speed. Here, as in the first embodiment, the driving speed of each joint is calculated by a transformation matrix such as an inverse Jacobian. Of course, the joint drive speed for changing the posture angle of the hand is also calculated. Then, the drive speed evaluation unit 35 evaluates the drive speed of each joint. That is, it is determined whether the drive speed of the joint that changes the posture angle exceeds the limit drive speed. Of course, the driving speed of the joint that changes the position of the hand is also evaluated. When the driving speed of one or more joints exceeds the limit driving speed, an evaluation signal indicating that the driving speed exceeds the limit driving speed is output.

In the present embodiment, a correction unit 41 is provided instead of the hand speed correction unit 37 shown in the first embodiment. When the driving speed of one or more joints exceeds the limit driving speed, the correcting unit 41 corrects the hand speed and the posture angular speed ω. The posture angular velocity ω of the hand can be corrected similarly to the hand velocity. For example, if the posture angular velocity ω is increased, the posture angular velocity ω is made constant when the drive speed exceeds the limit drive speed. Even if the posture angular velocity ω is constant, the posture angular velocity ω is decreased when the drive speed exceeds the limit drive speed. In this way, the corrected angular velocity ω _mod can be obtained. Accordingly, when any one of the drive speeds exceeds the limit drive speed, the hand speed and the posture angular speed are corrected. Then, the driving speed of each joint is obtained based on the corrected hand speed and posture angular speed. This process is performed until the drive speed does not exceed the limit drive speed. Then, as in the first embodiment, a drive signal based on the drive speed is output. By repeating the above processing, each joint can be driven at an appropriate driving speed. Therefore, driving time can be shortened. In this way, by performing the above-described control on the manipulator whose hand position and posture are changed by driving the same joint, the driving time can be easily reduced.

1 is an overall schematic diagram schematically showing an entire robot according to a first embodiment of the present invention. It is a figure which shows the track | orbit of the robot which concerns on Embodiment 1 of this invention. It is a block diagram which shows the control part provided in the robot which concerns on Embodiment 1 of this invention. It is a figure which shows the control pattern in the control method of the robot which concerns on Embodiment 1 of this invention. It is a flowchart which shows the control method of the robot which concerns on Embodiment 1 of this invention. It is a block diagram which shows the control part provided in the robot which concerns on Embodiment 2 of this invention.

Explanation of symbols

1 robot, 10 main unit, 11 control unit, 20 arm unit,
21 1st joint, 22 1st link, 23 2nd joint, 24 2nd link, 26 hands,
27 Encoder, 28 Actuator,
31 target position storage unit, 32 hand trajectory generation unit, 33 hand speed calculation unit,
34 driving speed calculation unit, 35 driving speed evaluation unit, 36 acceleration storage unit,
37 Hand speed evaluation unit, 38 Deceleration process determination unit, 39 Joint drive unit,
40 posture angular velocity calculation unit, 41 correction unit

Claims (6)

A robot arm that drives a plurality of joints to move the hand to a target position,
A hand trajectory generator for generating a hand trajectory up to the target position;
A hand speed calculator that calculates the speed of the hand when moving according to the hand trajectory generated by the hand trajectory generator;
A driving speed calculation unit that calculates the driving speed of the joint based on the hand speed calculated by the hand speed calculation unit;
A driving speed evaluation unit for evaluating whether the driving speed of the joint exceeds a limit value of the driving speed;
When the driving speed exceeds a limit value, a correction unit that corrects the hand speed calculated by the hand speed calculation unit and outputs the correction to the driving speed calculation unit;
A robot arm comprising: an actuator that drives the joint according to the drive speed calculated by the drive speed calculation unit based on the hand speed corrected by the correction unit. The plurality of joints include joints for changing the posture of the hand,
A posture angular velocity calculating unit that calculates an angular velocity of the hand posture when moving according to the hand trajectory generated by the previous hand trajectory generating unit;
The drive speed calculation unit calculates the drive speed of the joint based on the hand speed and the angular speed,
The correction unit corrects the hand speed and the angular velocity according to the evaluation result in the drive speed evaluation unit,
The robot arm according to claim 1, wherein the actuator drives the joint according to a driving speed calculated by the joint driving unit based on the corrected hand speed and the corrected angular velocity.   3. The robot arm according to claim 1, wherein the hand speed calculation unit calculates a speed when accelerating at a predetermined acceleration from the current speed of the hand as a hand speed. 4. A control method of a robot arm that drives a plurality of joints and moves a hand to a target position,
Generating a trajectory of the hand to the target position;
Calculating the speed of the hand when moving according to the hand trajectory;
Calculating a driving speed of the joint based on the hand speed;
Evaluating whether the drive speed of the joint exceeds a limit value of the drive speed;
Correcting the hand speed when the driving speed exceeds a limit value; and
And a step of driving the joint according to a driving speed calculated based on the corrected hand speed. The plurality of joints include joints for changing the posture of the hand,
A step of calculating an angular velocity of the hand posture when moving according to the hand trajectory;
In the step of calculating the pre-front drive speed, the joint drive speed is calculated based on the hand speed and the angular speed,
In the step of correcting the hand speed, when the driving speed exceeds a limit value, the hand speed and the angular speed are corrected,
5. The robot arm control according to claim 4, wherein in the step of driving the joint, the joint is driven according to the corrected hand speed and the driving speed calculated based on the corrected angular velocity. 6. Method.   The robot arm control method according to claim 4 or 5, wherein in the step of calculating the hand speed, a speed when accelerating at a predetermined acceleration is calculated from the current speed of the hand as a hand speed.

Images