JP2013132696A - Arm control device and arm control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a technology for replanning a trajectory of a robot arm as fast as possible by a simpler structure than before when a target position of the robot arm is changed while being moved to the target position.SOLUTION: An arm control device (a control part 10) for controlling the movement of the arm includes a trajectory planning part planning the trajectory of the arm by specifying an angular speed or angular acceleration of a joint, an arm control part moving the arm to the target position according to a trajectory plan, and a trajectory replanning part replanning the trajectory of the arm by keeping the magnitude of acceleration and deceleration of the joint as prescribed when the target position is changed while the arm is moved.

Description

  The present invention relates to an arm control device and an arm control method.

  Some industrial robots perform various operations by controlling a robot arm (for example, Patent Document 1).

In the control of the robot arm, for example,
(1) The target position of the robot arm is determined.
(2) Plan a trajectory for moving the robot arm from the current position to the target position.
(3) The robot arm is moved to the target position according to the trajectory plan.
The operations (1) to (3) are repeated.

  Of course, when the target position is changed during the movement of the robot arm, the above operations (2) and (3) must be executed again.

Japanese Patent Laid-Open No. 7-14015

  In that case, since the trajectory is re-planned while the robot arm is being moved, the trajectory must be quickly planned.

  The present invention provides a technique for replanning the trajectory of the robot arm as fast as possible with a simpler configuration than the conventional configuration when the target position is changed while the robot arm is being moved to the target position. The purpose is to do.

  The present application includes a plurality of means for solving the above-described problems, and examples thereof are as follows.

  A first aspect of the present invention for solving the above-described problem is an arm control device that controls the operation of an arm, and specifies a joint angular velocity or angular acceleration to perform a trajectory plan of the arm. And an arm control unit for moving the arm to a target position according to the trajectory plan, and when the target position is changed during the movement of the arm, the acceleration / deceleration magnitude of the joint is maintained at a predetermined level. A trajectory replanning unit for replanning the trajectory of the arm.

  According to said structure, it can reschedule about operation | movement of each joint with a simple calculation method. Therefore, even if the target position is changed during the movement of the arm toward the target position, the operation of each joint can be immediately replanned.

  The arm includes a plurality of joints, and the trajectory planning unit mainly selects a joint having the latest operation end timing among the plurality of joints to move the arm to a target position. The joint may be determined and the operation end timing of the joint other than the main joint may be adjusted to the main joint.

  According to said structure, the operation | movement of each joint can be complete | finished substantially simultaneously, and it seems that the arm is operating | moving smoothly.

Further, when the target position is changed during the movement of the arm, the trajectory replanning unit sets the acceleration (Amax) and deceleration (−Amax) of the joint as fixed values,
D = {(Vi + Vpeak) × Ta / 2} + {Vpeak × Tc / 2},
Ta = (Vpeak−Vi) / Amax,
Tc = Vpeak / (− Amax),
(Where D is the angle at which the joint is rotated before the arm reaches the target position, Vi is the initial angular velocity, Vpeak is the extreme angular velocity, Ta is the acceleration time, and Tc is the deceleration time)
When the extreme value (Vpeak) of the angular velocity of the joint is calculated using the following equation, the joint is rotated by the acceleration time (Ta) at the acceleration (Amax), and the angular velocity reaches the extreme value (Vpeak). Further, it is possible to re-plan to rotate the deceleration time (Tc) at the deceleration (−Amax).

  According to the above configuration, when the rotation direction of the joint can be maintained in the positive direction from the point where the target position of the arm is changed to the changed target position, the operation of the joint can be re-planned by a simple calculation method.

In addition, the trajectory replanning unit, when the calculated extreme value (Vpeak) of the angular velocity of the joint exceeds the upper limit value (Vmax) of the angular velocity,
D = {(Vi + Vmax) × Ta / 2} + {Vmax × Tb} + {Vmax × Tc / 2},
Ta = (Vmax−Vi) / Amax,
Tc = Vmax / (− Amax),
(Where D is the angle at which the joint is rotated before the arm reaches the target position, Vi is the initial angular velocity, Ta is the acceleration time, Tb is the constant velocity time, and Tc is the deceleration time)
When the constant velocity time (Tb) of the joint is calculated using the following formula, the joint is rotated by the acceleration time (Ta) at the acceleration (Amax), and the angular velocity reaches the upper limit value (Vmax), It is also possible to re-plan so that the constant speed time (Tb) is rotated at a constant angular velocity (Vmax) and then the deceleration time (Tc) is rotated at the deceleration (−Amax).

  According to the above configuration, the rotation direction of the joint can be maintained in the positive direction from the point where the target position of the arm is changed to the target position after the change, and the extreme value (Vpeak) of the angular velocity of the joint. However, when the angular velocity exceeds the upper limit value (Vmax), the joint operation can be re-planned by a simple calculation method.

  In addition, when the arm includes a plurality of joints, the trajectory replanning unit replans the operation for each joint and moves the arm to the changed target position. Of these joints, the joint with the latest operation end timing may be determined as the main joint, and the operation end timing may be adjusted to the main joint for joints other than the main joint.

  According to the above configuration, even when the joint motion plan is re-planned, the motion of the entire arm can be seen smoothly.

  A second aspect of the present invention for solving the above problem is an arm control method performed by an arm control device that controls the operation of an arm, wherein an angular velocity or an acceleration of a joint is designated, and the arm A trajectory planning step for performing trajectory planning, an arm control step for moving the arm to a target position according to the trajectory plan, and a magnitude of acceleration / deceleration of the joint when the target position is changed during the movement of the arm Is maintained at a predetermined value, and a trajectory replanning step of replanning the trajectory of the arm is performed.

  According to the second aspect of the present invention, the operation of each joint can be re-planned by a simple calculation method. Therefore, even if the target position is changed during the movement of the arm toward the target position, the operation of each joint can be immediately replanned.

  Problems, configurations, and effects other than those described above will be clarified by the following description of embodiments.

2 is a diagram illustrating an example of an appearance of a robot 1. FIG. 2 is a block diagram illustrating an example of a schematic configuration of a robot. FIG. It is a flowchart for demonstrating an example of an arm control task. (A) It is a figure which shows an example of the track | orbit plan about a 1st joint (1st axis | shaft). (B) It is a figure which shows an example of the track | orbit plan about a 2nd joint (2nd axis | shaft). (C) It is a figure which shows an example of the track | orbit plan about a 3rd joint (3rd axis | shaft). (D) It is a figure which shows the example in the case of adjusting the completion | finish timing of a 2nd joint according to a main joint. (E) It is a figure which shows the example in the case of adjusting the completion | finish timing of a 3rd joint according to a main joint. It is a figure explaining the method of calculating rotation amount D 'which a joint rotates until it stops after a target position is changed. (A) It is a figure which shows an example in the case of re-planning the track | orbit of a joint (however, when "D '<D""). (B) It is a figure which shows an example in the case of re-planning the track | orbit of a joint (however, when "D '<D""and" _Vpeak > _VMAX "). (A) It is a figure which shows an example in the case of re-planning the track | orbit of a joint (however, when "D '>D""). It is a diagram showing an example of a case of re-planning the trajectory (B) joint (if "D '>D""and" _V peak _{<-V MAX} "). (A) It is a figure which shows an example of the trajectory plan replanned about the 2nd joint (2nd axis | shaft). (B) It is a figure which shows an example of the trajectory plan re-planned about the 1st joint (1st axis | shaft). (C) It is a figure which shows an example of the trajectory plan replanned about the 3rd joint (3rd axis | shaft). (D) It is a figure which shows the example in the case of adjusting the completion | finish timing of a 1st joint according to a main joint when the track | orbit of each joint is re-planned. (E) It is a figure which shows the example in the case of adjusting the completion | finish timing of a 3rd joint according to a main joint, when the track | orbit of each joint is re-planned. It is a flowchart for demonstrating an example of a target position calculation task.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

  FIG. 1 is a diagram illustrating an appearance example of the robot 1 according to the present embodiment.

  As illustrated, the robot 1 is, for example, an arm type robot including a movable unit 30, an imaging unit 20, and a control unit 10.

  The movable part 30 is a movable part of the robot 1. In the illustrated example, a single robot arm is shown as the movable unit 30, and includes an arm unit 4 surrounded by a dotted line and a hand unit 5 attached to the tip of the arm unit 4. In addition, you may have two robot arms like what is called a double-arm robot.

  The arm unit 4 includes a plurality of joints (joints) 6 and a plurality of links 7.

  Each joint 6 connects the links 7 to each other and the base (body) of the robot to the link 7 so as to be rotatable (but rotatable within a predetermined movable range). Each joint 6 is, for example, a rotary joint, and is provided so that the angle between the links 7 can be changed or the links 7 can be rotated about the axis. Accordingly, by driving the joints 6 in conjunction with each other, the work point (for example, the hand portion 5) can be freely moved (but within a predetermined movable range) and directed in a free direction. You can also. In the example of this figure, the arm part 4 is a 3-axis arm provided with three joints.

  The hand unit 5 includes, for example, a plurality of fingers, can move the adjacent fingers in a direction approaching, and can grip the gripping object with at least two fingers.

  The imaging unit 20 is a unit that captures an external environment of the robot 1 (for example, the vicinity of the illustrated work table) and generates image data. The imaging unit 20 includes, for example, a camera and is provided on a ceiling or a wall. Under the control of the control unit 10, the imaging unit 20 shoots the movement destination of the work point (for example, the hand unit 5), that is, the target position, generates image data, and outputs the image data to the control unit 10.

  The control unit 10 performs processing for controlling the entire robot 1. For example, the control unit 10 may be built in the main body (movable unit 30) of the robot 1 or may be disposed at a location away from the main body of the robot 1 (remotely operable) as illustrated. It may be left.

  FIG. 2 is a block diagram illustrating an example of a schematic configuration of the robot 1. As illustrated, the control unit 10 includes a CPU 11 that is an arithmetic device, a RAM 12 that is a volatile storage device, a ROM 13 that is a nonvolatile storage device, and an interface (I) that connects the control unit 10 and other units. / F) A circuit 14 and a communication device 15 that communicates with a device external to the robot 1 are provided.

  Next, a functional configuration example of the control unit 10 will be described. A functional configuration example of the control unit 10 is shown in the lower part of FIG.

  As shown in the figure, the control unit 10 includes a central control unit 101, a storage unit 102, an imaging control unit 103, a target position calculation unit 104, a trajectory planning unit 105, an encoder reading unit 106, and an arm control unit 107. And a trajectory replanning unit 108.

  The central control unit 101 comprehensively controls the other units (102 to 107).

  The storage unit 102 stores various data and programs. For example, the storage unit 102 stores a program for performing a trajectory plan for the movable unit 30, a program for controlling the movable unit 30 in accordance with the trajectory plan, and the like.

  The imaging control unit 103 controls the imaging unit 20. For example, the imaging control unit 103 causes the imaging unit 20 to photograph the movement destination of the work point (for example, the hand unit 5), that is, the periphery of the target position. Then, the imaging control unit 103 acquires image data captured by the imaging unit 20.

  The target position calculation unit 104 performs image recognition on the image data captured by the imaging unit 20. Based on this image recognition, the target position calculation unit 104 calculates a target position (for example, three-dimensional coordinates) to which the work point is moved.

  The trajectory planning unit 105 plans a trajectory for the movable unit 30 when moving the work point (for example, the hand unit 5) to the target position.

  Specifically, the trajectory planning unit 105 specifies (determines) the temporal change in angular velocity or angular acceleration that rotates each joint 6.

  Further, the trajectory planning unit 105 determines the joint 6 having the latest operation end timing as the main joint among the plurality of joints 6 to move the work point (for example, the hand unit 5) to the target position. Then, the trajectory planning unit 105 matches the operation end timing with the main joint for the joints 6 other than the main joint. As a result, the operations of the joints 6 are finished almost at the same time, and it seems that the movable part 30 is operating smoothly.

  The encoder reading unit 106 reads the encoder value of the actuator 31 provided in each joint 6 of the arm unit 4. Such encoder values can be used to determine the angular velocity, angular acceleration, etc. of each joint 6 because the rotation angle of each joint 6 can be specified. Also, the position, moving direction, moving amount, speed, acceleration of the movable portion 30 are used. It is also used to grasp about.

  The arm control unit 107 performs control to move the work point toward the target position according to the trajectory plan by the trajectory plan unit 105. That is, the arm control unit 107 rotates each joint 6 at a designated angular velocity or angular acceleration.

  In order to perform such control, for example, the arm control unit 107 sets the work point to the target position (that is, the target value) based on the encoder value read by the encoder reading unit 106 and the output signals of the various sensors 32. The actuator 31 may be controlled so that the position and the posture are obtained.

  In addition, the target position may be changed while the work point is moved toward the target position.

  In this case, the trajectory replanning unit 108 replans the trajectory of the movable unit 30 while maintaining a predetermined acceleration / deceleration magnitude (ie, angular acceleration and angular deceleration) at which the joint 6 rotates.

  Further, even when re-planning the trajectory of the movable unit 30, the trajectory re-planning unit 108 moves the work point (for example, the hand unit 5) to the changed target position among the plurality of joints 6. The joint 6 with the latest operation end timing is determined as the main joint. Then, the trajectory replanning unit 108 matches the operation end timing with the main joint for the joints 6 other than the main joint.

  Each functional unit other than the above-described storage unit 102 is realized, for example, when the CPU 11 reads a predetermined program stored in the ROM 13 into the RAM 12 and executes it. The storage unit 102 is realized by, for example, the RAM 12 or the ROM 13. For example, the predetermined program may be installed in the ROM 13 in advance, or may be downloaded from the network via the communication device 15 and installed or updated.

  The configuration of the robot 1 described above is not limited to the above-described configuration because the main configuration has been described in describing the features of the present embodiment. As long as it has the movable part 30, what kind of structure may be sufficient. Further, the configuration of a general robot is not excluded.

  For example, although a three-axis arm is shown as the arm portion 4 in FIG. 1, the number of axes (number of joints) may be further increased or decreased. The number of links may be increased or decreased. In addition, the shape, size, arrangement, structure, and the like of various members such as the arm unit 4, the hand unit 5, the link, and the joint may be appropriately changed.

  In addition, each functional configuration of the control unit 100 described above is classified according to main processing contents in order to facilitate understanding of the configuration of the control unit 100. The present invention is not limited by the way of classification and names of the constituent elements. The configuration of the control unit 100 can be classified into more components depending on the processing content. Moreover, it can also classify | categorize so that one component may perform more processes. Further, the processing of each component may be executed by one hardware or may be executed by a plurality of hardware.

  Next, a characteristic operation of the robot 1 having the above configuration in the present embodiment will be described.

<Arm control task>
The task of moving the work point (for example, the hand unit 5) to the target position by controlling the arm unit 4 is hereinafter referred to as “arm control task”.

  FIG. 3 is a flowchart for explaining an example of the arm control task.

  The central control unit 101 starts an arm control task based on an instruction from the user.

  When the arm control task is started, first, the encoder reading unit 106 reads the encoder value of the actuator 31 provided in each joint 6 of the arm unit 4 (step S101). Here, the read encoder value corresponds to the rotation angle from the reference position of each joint 6, and for example, the angle formed by the two links 7 connected via the joint 6 can be specified.

  By the processing in step S101, the current state (initial) of the movable unit 30 (positions and postures of the joints 6 and the work points) can be known.

  Next, the target position calculation unit 104 starts a target position calculation task in order to calculate the target position of the work point (for example, the hand unit 5) (step S102). Details of the target position calculation task will be described later.

  When the target position (for example, three-dimensional coordinates) of the work point is determined by the target position calculation task, the trajectory planning unit 105 plans a trajectory for the movable unit 30 when moving the work point to the target position (step S103).

  Specifically, in step S103, the trajectory planning unit 105 performs an operation plan for each joint 6 by designating an angular velocity or angular acceleration to be rotated.

  FIG. 4A is a diagram illustrating an example of an operation plan for the first joint (first axis). FIG. 4B is a diagram illustrating an example of an operation plan for the second joint (second axis). FIG. 4C is a diagram illustrating an example of an operation plan for the third joint (third axis).

As shown in each drawing, in step S103, the trajectory planning unit 105 divides the acceleration section, the constant speed section, and the deceleration section into action plans for each joint 6. In the acceleration section, each joint 6 is rotated at the maximum angular acceleration (A _MAX ) that can be accelerated. Further, in the constant velocity section, the joint 6 is rotated at a constant velocity with the upper limit value (V _MAX ) of the angular velocity of each joint 6. Further, in the deceleration section, each joint 6 is rotated at the maximum angular deceleration (−A _MAX ) that can decelerate. In this way, the trajectory planning unit 105 performs an operation plan so that the angular velocity waveform of each joint 6 becomes a trapezoid.

  The trajectory planning unit 105 determines an acceleration time (acceleration section time) Ta, constant speed time (constant speed section time) Tb, according to the distance and direction in which the work point (for example, the hand unit 105) is moved. Deceleration time (deceleration section time) Tc is determined for each joint 6.

  For example, the trajectory planning unit 105 obtains the rotation amount D of each joint 6 necessary for moving the work point from the current position to the target position. The trajectory planning unit 105 then accelerates the time Ta, the constant velocity time Tb, and the deceleration time Tc so that the area of the angular velocity waveform (trapezoid) of each joint 6 matches the rotation angle D of each joint 6 obtained previously. To decide.

That is, the trajectory planning unit 105
D = (V _MAX × Ta) / 2 + (V _MAX × Tb) + (V _MAX × Tc) / 2,
A _MAX = V _MAX / Ta,
-A _MAX = -V _MAX / Tc,
The “acceleration time Ta”, “constant speed time Tb”, and “deceleration time Tc” that satisfy the following three equations are obtained for each joint 6.

Here, “V _MAX ” and “A _MAX ” are predetermined constants, for example, stored in the storage unit 102. In addition, when the value of “V _MAX ” is different for each joint 6, or when the value of “A _MAX ” is different for each joint 6, it may be stored for each joint 6.

  According to the planning method as described above, the total operation time (Ta + Tb + Tc) of each joint 6 is the shortest. However, since the operation end timings of the joints 6 are not uniform (T1, T2, T3 shown in the figure), the operation of the entire movable unit 30 may not be seen smoothly.

  Therefore, in order to make the operation of the entire movable unit 30 smooth, the trajectory planning unit 105 corrects the trajectory plan so that the operation end timings of the joints 6 are aligned in step S103.

  Specifically, the trajectory planning unit 105 determines the joint 6 having the latest operation end timing as the main joint among all the joints 6. Therefore, in the example shown in the figure, the operation of the first joint (first axis) takes the most time (T1> T2, T3), so the first joint is determined as the main joint.

  Then, the trajectory planning unit 105 matches the operation end timing with the main joint for the joints 6 other than the main joint. Accordingly, in the illustrated example, the trajectory planning unit 105 changes the operation end timing T2 of the second joint (second axis) to the operation end timing T1 of the main joint. Further, the operation end timing T3 of the third joint (third axis) is changed to the operation end timing T1 of the main joint.

  However, even when the operation end timing is changed for the joints 6 other than the main joint, the rotation angle D of each joint 6 when the work point is moved from the current position to the target position is changed. I can't help.

  Therefore, the trajectory planning unit 105 changes the operation end timing for the joints 6 other than the main joint while keeping the area (shaded portion) of the angular velocity waveform of each joint 6 constant.

  FIG. 4D is a diagram illustrating an example in which the operation end timing of the second joint is changed in accordance with the main joint. FIG. 4E is a diagram illustrating an example in which the operation end timing of the third joint is changed in accordance with the main joint.

As shown in each figure, the trajectory planning unit 105 performs the following operations on the joints 6 other than the main joint.
(1) D = ( _VMAX ′ × Ta ′) / 2+ ( _VMAX ′ × Tb ′) + ( _VMAX ′ × Tc ′) / 2,
(2) A _MAX = V _MAX '/ Ta',
(3) -A _MAX = -V _MAX '/ Tc',
(4) Ta ′ + Tb ′ + Tc ′ = T1
The “acceleration time Ta ′”, “constant speed time Tb ′”, “deceleration time Tc ′”, and “temporary upper limit value V _MAX ′” are obtained.

Here, “V _MAX ′” is a provisional upper limit value V _MAX ′ for the angular velocity of the joint 6, and “T1” is the operation end timing (time from the operation start to the operation end) of the main joint.

  By correcting the operation plan for the joints 6 other than the main joint as described above, the operation end timings of the joints 6 are aligned ("T1" shown in FIGS. 4D and 4E). Thus, the entire operation of the movable unit 30 can be seen smoothly.

  When the process of step S103 is completed, the arm control unit 107 controls the movable unit 30 according to the trajectory plan created in step S103 (step S104).

  Specifically, the arm control unit 107 rotates each joint 6 at the angular velocity or angular acceleration specified in the trajectory plan (FIGS. 4A, 4D, and 4E) created in step S103. Let

  After rotation for a predetermined time (for example, several seconds), the arm control unit 107 waits for a predetermined time (for example, 1 ms) (step S105), and the process proceeds to step S106.

  Meanwhile, in parallel with the control of this flow (arm control task), the target position calculation task started in step S102 is continuously executed. If the target position is changed while the work point (for example, the hand unit 5) is moved toward the target position, a target position change command is sent from the target position calculation unit 104. It is a mechanism to come.

  Therefore, when the process proceeds to step S106, the trajectory replanning unit 108 determines whether or not a target position change command is sent from the target position calculation unit 104 (step S106).

  The trajectory replanning unit 108 determines that the target position has not been changed when the target position change command has not been sent (step S106; No), and proceeds directly to step S108.

  On the other hand, when the target position change command is sent (step S106; Yes), the trajectory replanning unit 108 determines that the target position has been changed and replans the trajectory of the movable unit 30 ( Step S107).

Specifically, in step S107, the trajectory replanning unit 108 maintains the magnitude of acceleration / deceleration at which each joint 6 rotates, that is, angular acceleration (A _MAX ) and angular deceleration (−A _MAX ), at a predetermined level. (Without changing), redo the motion plan of each joint 6.

  Hereinafter, the process of step S107 will be described in detail.

First, when the process proceeds to step S107, the trajectory replanning unit 108 decelerates the angular velocity of the joint 6 from the current angular velocity (V _i ) by the maximum angular deceleration (−A _MAX ), and the rotation stops. The amount of rotation D ′ that is rotated up to is determined. The reason why the rotation amount D ′ is obtained here is that the method of redoing the operation plan of the joint 6 differs depending on the obtained rotation amount D ′ (replanning methods 1 to 4 described later).

  FIG. 5 is a diagram for explaining a method of calculating the rotation amount D ′.

  As illustrated, the trajectory replanning unit 108 calculates the rotation amount D ′ by obtaining the area of the vertical stripe portion.

That is, the trajectory replanning unit 108
(1) D ′ = V _i × T0 / 2
(2) -A _MAX = -V _i / T0
“Rotation amount D ′” that satisfies the following two equations is obtained.

Here, “T0” represents the time it takes for the rotation of the joint 6 to stop after the target position is changed. Further, “−A _MAX ” is a predetermined constant as described above. Further, “V _i ” is obtained from the encoder value of the actuator 31 provided in each joint 6.

  Next, the trajectory replanning unit 108 rotates each joint 6 necessary to move the work point from the current position (ie, the point where the target position has been changed) S to the changed target position G ′. The quantity D "is determined.

  Then, the trajectory replanning unit 108 compares the rotation amount D ″ obtained here with the rotation amount D ′.

(Replanning method 1)
At this time, if “D ′ <D ″”, the rotational direction of the joint 6 is in a positive direction between the current position S and the changed target position G ′ (that is, the angular velocity V> 0 of the joint 6). ) Is preferably maintained.

  FIG. 6A is a diagram illustrating an example of replanning the operation of the joint 6 when the condition “D ′ <D” ”is satisfied.

As shown in the drawing, the trajectory replanning unit 108 first replans the operation of each joint 6 by dividing into an acceleration section and a deceleration section. Specifically, the trajectory replanning unit 108 rotates each joint 6 from the initial speed (V _i ) with the angular acceleration (A _MAX ) (acceleration section), and when the angular velocity reaches the extreme value (V _peak ), Re-plan to rotate at the angular deceleration (-A _MAX ) (deceleration section). In this way, the trajectory replanning unit 108 _{replans the} operation so that the angular velocity waveform of each joint 6 becomes a quadrangle (trapezoid + triangle) having an extreme value (V _peak ) (thick line).

  The trajectory replanning unit 108 determines the acceleration time Ta and the deceleration time Tc according to the distance and the direction in which the work point is moved from the current position (point where the target position is changed) S to the changed target position G ′. , Determine.

  For example, the trajectory replanning unit 108 determines that the acceleration time Ta and the deceleration time so that the area (shaded portion) of the angular velocity waveform of each joint 6 matches the rotation amount D ″ of each joint 6 obtained previously. Determine Tc.

That is, the trajectory replanning unit 108 sets the angular acceleration (A _MAX ) and angular deceleration (−A _MAX ) of the joint 6 as fixed values,
(1) D ″ = {(V _i + V _peak ) × Ta / 2} + {V _peak × Tc / 2},
(2) Ta = (V _peak −V _i ) / A _MAX ,
(3) Tc = V _peak / (− A _MAX ),
The “acceleration time Ta”, “deceleration time Tc”, and “extreme value V _peak ” that satisfy the following three equations are obtained for each joint 6.

According to the method as described above, the angular acceleration (A _MAX ) and the angular deceleration (−A _MAX ) of the joint 6 are fixed values. You can re-plan. As a result, even if the target position is changed while the work point is moved toward the target position, the operation of each joint 6 can be immediately replanned.

(Replanning method 2)
Even if “D ′ <D ″”, if the obtained “extreme value V _peak ” exceeds the upper limit value V _{MAX of the} angular velocity, the re-planning method 1 cannot be adopted.

FIG. 6B is a diagram illustrating an example of replanning the operation of the joint 6 when the conditions “D ′ <D” ”and“ V _peak > V _MAX ”are satisfied.

As shown in the drawing, the trajectory replanning unit 108 replans the operation of each joint 6 in an acceleration section, a constant speed section, and a deceleration section. Specifically, the trajectory replanning unit 108 rotates each joint 6 from the initial speed (V _i ) at the angular acceleration (A _MAX ) (acceleration section), and when the angular velocity reaches the upper limit value (V _MAX ), It is re-planned to rotate at a constant angular velocity (V _MAX ) (constant velocity section) and then rotate at an angular deceleration (−A _MAX ) (deceleration section). In this manner, the trajectory replanning unit 108 replans the operation so that the angular velocity waveform of each joint 6 becomes a pentagon (trapezoid + rectangle + triangle) (thick line).

  The trajectory replanning unit 108 determines the acceleration time Ta and the constant speed time according to the distance and the direction in which the work point is moved from the current position (point where the target position has been changed) S to the changed target position G ′. Tb and deceleration time Tc are determined.

  For example, the trajectory replanning unit 108 determines that the acceleration time Ta and the constant velocity are set so that the area (shaded portion) of the angular velocity waveform of each joint 6 matches the rotation amount D ″ of each joint 6 previously obtained. Time Tb and deceleration time Tc are determined.

That is, the trajectory replanning unit 108 sets the angular acceleration (A _MAX ) and angular deceleration (−A _MAX ) of the joint 6 as fixed values,
(1) D ″ = {(V _i + V _MAX ) × Ta / 2} + {V _MAX × Tb} + {V _MAX × Tc / 2},
(2) Ta = (V _MAX −V _i ) / A _MAX ,
(3) Tc = _VMAX / ( _-AMAX ),
The “acceleration time Ta”, “deceleration time Tb”, and “deceleration time Tc” that satisfy the following three formulas are obtained for each joint 6.

By replanning the operation of each joint 6 in this way, even if “V _peak > V _MAX ”, _replanning can be performed without any problem, and the same effect as the replanning method 1 can be obtained.

(Replanning method 3)
When “D ′> D ″”, the rotation of the joint 6 is reversely rotated (that is, the angular velocity V <0 of the joint 6) in a partial section from the current position S to the changed target position G ′. )

  FIG. 7A is a diagram illustrating an example of replanning the operation of the joint 6 when the condition “D ′> D ″” is satisfied.

As illustrated, the trajectory replanning unit 108 first reschedules the operation of each joint 6 in a first deceleration zone, a second deceleration zone, and an acceleration zone. Specifically, the trajectory replanning unit 108 rotates each joint 6 from the initial speed (V _i ) at an angular deceleration (−A _MAX ) (first deceleration section), and the angular speed reaches zero. Is also rotated at angular deceleration (−A _MAX ) (second deceleration zone), and when the angular velocity reaches the extreme value (V _peak ), it is rotated at angular acceleration (A _MAX ) (acceleration zone). To plan. In this way, the trajectory replanning unit 108 replans the operation so that the angular velocity waveform of each joint 6 spans the positive and negative of the time axis (positive triangle + negative triangle) (thick line).

  The trajectory replanning unit 108 determines the first deceleration time Tc, depending on the distance and the direction in which the work point is moved from the current position (point where the target position is changed) S to the changed target position G ′. A second deceleration time Tc ′ and an acceleration time Ta are determined.

For example, each orbit replanning section 108, the area of the angular velocity waveform of each joint 6, i.e., the area obtained by subtracting the area D _L positive negative triangle from the area D 'of the triangle shown, had been obtained previously The first deceleration time Tc, the second deceleration time Tc ′, and the acceleration time Ta are determined so as to coincide with the rotation amount D ″ of the joint 6.

That is, the trajectory replanning unit 108 sets the angular acceleration (A _MAX ) and angular deceleration (−A _MAX ) of the joint 6 as fixed values,
(1) D ″ = {V _i × Tc / 2} − {| V _peak | × (Tc ′ + Ta) / 2},
(2) Tc = −V _i / (− A _MAX ),
(3) Tc ′ = − | V _peak | / (− A _MAX ),
(4) Ta = | V _peak | / A _MAX ,
The “first deceleration time Tc”, “second deceleration time Tc ′”, “acceleration time Ta”, and “extreme value V _peak ” that satisfy the following four equations are obtained for each joint 6.

  By replanning the operation of each joint 6 in this way, even if “D ′> D ″”, replanning can be performed without any problem, and the same effects as the replanning methods 1 and 2 can be obtained.

(Replanning method 4)
Further, when “D ′> D ″” and the obtained “extreme value V _peak ” is lower than the lower limit value (−V _MAX ) of the angular velocity, the above re-planning method 3 cannot be adopted.

FIG. 7B is a diagram illustrating an example of replanning the operation of the joint 6 when the conditions “D ′> D” ”and“ V _peak <−V _MAX ”are satisfied.

As shown in the figure, the trajectory replanning unit 108 replans the operation of each joint 6 in a first deceleration section, a second deceleration section, a constant speed section, and an acceleration section. Specifically, the trajectory replanning unit 108 rotates each joint 6 from the initial speed (V _i ) at an angular deceleration (−A _MAX ) (first deceleration section), and the angular speed reaches zero. Is also rotated at angular deceleration (−A _MAX ) (second deceleration zone), and when the angular velocity reaches the lower limit value (−V _MAX ), it is rotated at constant angular velocity (−V _MAX ) (constant velocity zone). ), And then re-plan to rotate (acceleration section) with angular acceleration (A _MAX ). As described above, the trajectory replanning unit 108 replans the operation so that the angular velocity waveform of each joint 6 spans the positive and negative of the time axis (positive triangle + negative trapezoid) (thick line).

  The trajectory replanning unit 108 determines the first deceleration time Tc, depending on the distance and the direction in which the work point is moved from the current position (point where the target position is changed) S to the changed target position G ′. A second deceleration time Tc ′, a constant velocity time Tb, and an acceleration time Ta are determined.

For example, each orbit replanning section 108, the area of the angular velocity waveform of each joint 6, i.e., the area obtained by subtracting the positive from the area D 'of the triangle negative trapezoidal area D _L shown, had been obtained previously The first deceleration time Tc, the second deceleration time Tc ′, the constant speed time Tb, and the acceleration time Ta are determined so as to coincide with the rotation amount D ″ of the joint 6.

That is, the trajectory replanning unit 108 sets the angular acceleration (A _MAX ) and angular deceleration (−A _MAX ) of the joint 6 as fixed values,
(1) D ″ = {V _i × Tc / 2} − {V _MAX × (Tc ′ + 2 × Tb + Ta) / 2},
(2) Tc = −V _i / (− A _MAX ),
(3) Tc ′ = − V _MAX / (− A _MAX ),
(4) Ta = V _MAX / A _MAX ,
The “first deceleration time Tc”, “second deceleration time Tc ′”, “constant speed time Tb”, and “acceleration time Ta” that satisfy the following four equations are obtained for each joint 6.

By replanning the operation of each joint 6 in this way, it is possible to _replan without problems even when “V _peak <−V _MAX ”, and the same effects as the above replanning methods 1 to 3 are obtained. .

In the re-planning methods 1 to 4 described above, when the current angular velocity V _i of the joint 6 matches the upper limit value V _MAX (ie, “V _i = V _MAX ”), “V _i = It may be calculated as “V _MAX ”.

  8A to 8C are diagrams illustrating an example of an operation plan after replanning for the first to third joints.

Regardless of which of the above replanning methods 1 to 4, the total operation time of each joint 6 (the time from TS to _{TG ′} shown in the figure) is the shortest. However, as shown in the figure, the operation end timings of the joints 6 are not uniform (T _{G1 ′} , T _{G2 ′} , T _{G3 ′} shown in the figure). Therefore, by replanning the operations of the joints, The operation may not look smooth.

  Therefore, even when re-planning the operation of each joint, the trajectory re-planning unit 108 corrects the operation plan so that the operation end timing of each joint 6 is aligned in order to make the entire operation of the movable unit 30 smooth. To do.

Specifically, the trajectory replanning unit 108 determines, as a main joint, the joint 6 having the latest operation end timing after the replanning among all the joints 6. Accordingly, in the example shown in the figure, the second joint (second axis) after the re-planning takes the most time (T _{G2 ′} > T _{G1 ′} , T _{G3 ′} ), so the second joint is used as the main joint. decide.

Then, the trajectory replanning unit 108 adjusts the operation end timing of the joints 6 other than the main joint to the main joint. Thus, in the illustrated example, the trajectory replanning section 108 _'and operation termination timing T _G2 of the main _joint' first joint operation end timing T _G1 (first axis) to change. Further, the operation end timing T _{G3 ′} of the third joint (third axis) is changed to the operation end timing T _{G2 ′} of the main joint.

  However, even when the operation end timing is changed for the joint 6 other than the main joint in this way, the work point is changed from the current position (that is, the point where the target position is changed) S to the target position G ′ after the change. The amount of rotation D ″ of each joint 6 when moving to the position cannot be changed.

  Therefore, the trajectory replanning unit 108 changes the operation end timing for the joints 6 other than the main joint while keeping the area D ″ of the angular velocity waveform of each joint 6 constant.

  FIG. 8D is a diagram illustrating an example of changing the operation end timing for the first joint. FIG. 8E is a diagram illustrating an example of changing the operation end timing for the third joint.

For example, as shown in FIG. 8D, for the joint 6 whose operation is replanned by the replanning methods 1 and 2, the trajectory replanning unit 108
(1) D ″ = (V _i + V _H ) × Tc / 2 + (V _H × Tb) + (V _H × Tc) / 2,
(2) -A _MAX = (V _H -V _i ) / Ta,
(3) -A _MAX = -V _H / Tc ',
(4) Tc + Tb + Tc ′ = T _{G2 ′}
The “first deceleration time Tc”, “constant speed time Tb”, “second deceleration time Tc ′”, and “equal acceleration V _H ” that satisfy the following four equations are obtained.

Here, “V _H ” is a value of the angular velocity when the joint 6 rotates at a constant angular velocity.

Further, as shown in FIG. 8E, the trajectory replanning unit 108 for the joint 6 whose operation is replanned by the replanning methods 3 and 4 described above,
(1) D ″ = V _i × Tc / 2− (| V _H | × (Tc ′ + 2 × Tb + Ta) / 2)
(2) -A _MAX = -V _i / Tc,
(3) -A _MAX =-| _VH | / Tc ',
(4) A _MAX = | V _H | / Ta,
(5) Tc + Tb + Tc ′ = T _{G2 ′}
The “first deceleration time Tc”, “second deceleration time Tc ′”, “constant speed time Tb”, “acceleration time Ta”, and “constant acceleration V _H ” that satisfy the following equation (5) are obtained.

Here, “V _H ” is a value of an angular velocity when the joint 6 rotates at a constant acceleration.

As described above, by correcting the operation plan for the joints 6 other than the main joint, the operation end timing of each joint 6 is aligned even in the operation plan after the re-planning (see FIGS. 8D and 8E). “T _{G2 ′} ”). Therefore, even when the operation plan is re-planned in step S107, the entire operation of the movable unit 30 can be seen smoothly.

  The above is the processing in step S107. As described above, after replanning the trajectory of the movable unit 30, the trajectory replanning unit 108 moves the process to step S108.

Returning to FIG. As shown in the drawing, when the process proceeds to step S108, the arm control unit 107 terminates the trajectory plan created in step S103 (for example, the main joint operation end timing T1 shown in FIGS. 4D and 4E). ) Is determined (step S108). Of course, when the trajectory plan is replanned in step S107, it is the end time after the replanning (for example, the operation end timing T _{G2 ′ of the} main joint shown in FIGS. _{8D and 8E} ). To determine.

  If the arm control unit 107 determines that it is not the end time (step S108; No), the process returns to step S104, and the remaining unexecuted control is continued.

  On the other hand, if the arm control unit 107 determines that it is the end time (step S108; Yes), the flow ends.

  The above control is the arm control task according to the present embodiment. As described above, when the arm control task according to the present embodiment is adopted, even if the target position is changed at any timing during the movement of the work point toward the target position, The operation of the joint 6 can be re-planned.

In the arm control task according to the present embodiment, the angular acceleration in the acceleration section and the angular deceleration in the deceleration section are fixed (A _MAX , -A _MAX ). The control is more stable than when variable control is performed.

  Further, in the arm control task according to the present embodiment, the control speed is changed according to the distance to the target position because the movable part 30 is controlled according to the trajectory plan regardless of the distance to the target position of the movable part 30. Compared with, the movable part 30 can be controlled at high speed.

<Target position calculation task>
Next, details of the target position calculation task (S102) will be described.

  FIG. 9 is a flowchart for explaining an example of the target position calculation task (S102).

  As described above, the target position calculation unit 104 starts this flow at the timing when the process proceeds to step S102.

  When this flow is started, the imaging control unit 103 first acquires an image of a target position for moving the work point (step S201). For example, the imaging control unit 103 issues an imaging instruction to the imaging unit 20 based on an instruction from the user. At this time, the imaging unit 20 images the target position direction designated by the user. In this way, the imaging control unit 103 can acquire an image of the target position.

  Next, the target position calculation unit 104 calculates the target position of the work point using the image acquired in step S201 (step S202). For example, the target position calculation unit 104 performs image recognition on the image acquired in step S201, and identifies an object (or a focal point) that can be specified as the target position. Then, the target position calculation unit 104 obtains the position of the identified object from the difference from the imaging direction and sets it as the target position of the work point.

  Then, the target position calculation unit 104 transmits the target position (for example, three-dimensional coordinates) of the work point calculated in step S202 to the control unit 10 as an initial setting of the target position or a target position change command ( Step S203).

  Next, the target position calculation unit 104 determines whether or not the arm control task has been completed (step S204).

  Here, when the target position calculation unit 104 and the arm control task have not ended (step S204; No), the process returns to step S201, and while the arm control task is being executed, an instruction to change the target position is given. Wait.

  On the other hand, when the target position calculation unit 104 and the arm control task are finished (step S204; No), this flow is finished.

  The above control is the target position calculation task according to the present embodiment. As described above, according to the target position calculation task according to the present embodiment, while the arm control task is being executed, it is possible to accept changes to the target position of the work point at any time.

  Each processing unit of each flow described above is divided according to the main processing contents in order to make the robot 1 easy to understand. The invention of the present application is not limited by the method of classification of the processing steps and the names thereof. The process performed by the robot 1 can be divided into more process steps. One processing step may execute more processes.

  Moreover, said embodiment intends to illustrate the summary of this invention, and does not limit this invention. Many alternatives, modifications, and variations will be apparent to those skilled in the art.

  For example, in the above embodiment, the target position of the work point is specified using the image captured by the imaging unit 20. However, the present invention is not limited to this, and the target position may be designated by any method. For example, the target position of the work point may be designated by the user manually inputting the target position (for example, three-dimensional coordinates).

  In the above embodiment, the trajectory is planned so that the work point reaches the target position in the shortest time. However, the moving speed of the movable unit 30 may be controlled to decrease as the work point approaches the target position.

In this case, for example, in step S203 described above, the target position calculation unit 104 transmits a speed coefficient Cv that varies according to the distance between the work point and the target position together with the target position (three-dimensional coordinates) of the work unit. You may do it. The trajectory replanning unit 108 may multiply the V _MAX by the speed coefficient Cv during the trajectory replanning in step S107.

  In this way, for example, the movable portion 30 can be moved at a higher speed as the work point is farther to the target position, and the movable portion 30 can be moved at a lower speed as the work point is closer to the target position.

Further, in the above-described embodiment, in the operation plan of each joint 6, the angular acceleration in the acceleration section and the angular deceleration in the deceleration section are the maximum values that each joint 6 can rotate (A _MAX , −A _MAX ). However, according to the present invention, in the operation plan of each joint 6, fixed values may be used for the angular acceleration and the angular deceleration, and it is not necessary to be the maximum value at which each joint 6 can rotate.

Further, V _MAX or A _MAX may be different for each joint 6.

DESCRIPTION OF SYMBOLS 1 ... Robot, 4 ... Arm part, 5 ... Hand part, 6 ... Joint, 7 ... Link, 10 ... Control part, 11 ... CPU, 12 ... RAM , 13 ... ROM, 14 ... I / F, 15 ... communication device, 20 ... imaging part, 30 ... movable part, 31 ... actuator, 32 ... sensor, 101. ..Central control unit, 102 ... Storage unit, 103 ... Imaging control unit, 104 ... Target position calculation unit, 105 ... Track planning unit, 106 ... Encoder reading unit, 107 ... Arm control unit, 108 ... orbit re-planning unit.

Claims (6)

An arm control device for controlling the operation of the arm,
A trajectory planning unit that performs trajectory planning of the arm by designating the angular velocity or angular acceleration of the joint,
An arm control unit for moving the arm to a target position according to the trajectory plan;
A trajectory replanning unit that replans the trajectory of the arm while maintaining a predetermined acceleration / deceleration magnitude of the joint when the target position is changed during the movement of the arm;
An arm control device. The arm control device according to claim 1,
The arm includes a plurality of joints,
The trajectory planning unit
In order to move the arm to the target position, the joint having the latest operation end timing among the plurality of joints is determined as the main joint,
For joints other than the main joint, adjust the end timing of the operation to the main joint.
An arm control device. The arm control device according to claim 1 or 2,
The trajectory replanning unit
When the target position is changed during the movement of the arm, the acceleration (Amax) and deceleration (−Amax) of the joint are fixed values,
D = {(Vi + Vpeak) × Ta / 2} + {Vpeak × Tc / 2},
Ta = (Vpeak−Vi) / Amax,
Tc = Vpeak / (− Amax),
(Where D is the angle at which the joint is rotated before the arm reaches the target position, Vi is the initial angular velocity, Vpeak is the extreme angular velocity, Ta is the acceleration time, and Tc is the deceleration time)
The extreme value (Vpeak) of the angular velocity of the joint is calculated using
The joint is rotated for the acceleration time (Ta) at the acceleration (Amax), and when the angular velocity reaches the extreme value (Vpeak), the deceleration time (Tc) is rotated at the deceleration (−Amax). Replan,
An arm control device. The arm control device according to claim 3,
The trajectory replanning unit
When the calculated extreme value (Vpeak) of the angular velocity of the joint exceeds the upper limit value (Vmax) of the angular velocity,
D = {(Vi + Vmax) × Ta / 2} + {Vmax × Tb} + {Vmax × Tc / 2},
Ta = (Vmax−Vi) / Amax,
Tc = Vmax / (− Amax),
(Where D is the angle at which the joint is rotated before the arm reaches the target position, Vi is the initial angular velocity, Ta is the acceleration time, Tb is the constant velocity time, and Tc is the deceleration time)
The constant velocity time (Tb) of the joint is calculated using
The joint is rotated for the acceleration time (Ta) at the acceleration (Amax), and when the angular velocity reaches the upper limit (Vmax), the joint is rotated at the constant velocity (Vmax) for the constant velocity time (Tb), and then Replanning to rotate the deceleration time (Tc) at the deceleration (−Amax),
An arm control device. The arm control device according to any one of claims 1 to 4,
The trajectory replanning unit
If the arm contains multiple joints, replan the movement for each joint,
In order to move the arm to the changed target position, among the plurality of joints, a joint having the latest operation end timing is determined as a main joint,
For joints other than the main joint, adjust the end timing of the operation to the main joint.
An arm control device. An arm control method performed by an arm control device that controls the operation of the arm,
A trajectory planning step for designating a trajectory of the arm by specifying an angular velocity or an angular acceleration of the joint; and
An arm control step for moving the arm to a target position according to the trajectory plan;
When the target position is changed during the movement of the arm, a trajectory replanning step of replanning the trajectory of the arm while maintaining a predetermined magnitude of acceleration / deceleration of the joint is performed.
An arm control method characterized by the above.

Images