JP2013146840A - Trajectory planning device and trajectory planning method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a technique for preferably reducing a time required for a trajectory planning of a robot arm more than before, irrespective of a target condition of the robot arm.SOLUTION: A trajectory planning device for planning a trajectory of an arm includes: a reference memory unit for storing a plurality of reference conditions of the arm; a reference selection unit for selecting one reference condition from the plurality of reference conditions according to the target condition of an operation point of the arm; a calculation unit for calculating a temporary joint angle to lead the operation point to the target condition based on the selected reference condition; and a conversion unit for converting the temporary joint angle into a joint angle on a real space.

Description

  The present invention relates to a trajectory planning apparatus and a trajectory planning 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 state (target position, target posture) of the robot arm is determined.
(2) Plan the trajectory of the robot arm from the current state to the target state.
(3) The robot arm is moved to the target state according to the trajectory plan.
The operations (1) to (3) are performed.

JP 2003-19682

  By the way, depending on the target state of the robot arm determined in (1) above, it takes time to plan the trajectory of the robot arm (that is, the processing in (2) above).

  An object of the present invention is to provide a technique for shortening the time required for trajectory planning of a robot arm as much as possible, regardless of the target state of the robot arm.

  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 a trajectory planning apparatus that performs trajectory planning of an arm, a reference storage unit that stores a plurality of reference states for the arm, A reference selection unit that selects one reference state from the plurality of reference states according to the target state of the work point, and a temporary joint for guiding the work point to the target state based on the selected reference state A calculation unit that calculates an angle and a conversion unit that converts the temporary joint angle into a joint angle in real space are provided.

  According to the above configuration, the time required for the trajectory planning of the robot arm can be shortened compared to the conventional case regardless of the target state of the robot arm.

  The reference selection unit may select a reference state closest to the target state of the work point.

  Further, the calculation unit may vary the Jacobian matrix used for calculating the temporary joint angle according to the selected reference state.

  According to the above configuration, it is possible to perform the trajectory planning of the robot arm regardless of what reference state is selected.

  The plurality of reference states may be arranged such that the positions of the arm working points in each reference state are dispersed.

  According to the above configuration, the probability that the number of iterative calculations performed at the time of trajectory planning can be reduced increases wherever the target state is specified.

  The plurality of reference states may be configured such that the positions of the arm working points in each reference state are concentrated in an area that is easily designated as the target state.

  According to the above configuration, the probability that the number of iterative calculations performed at the time of trajectory planning can be significantly reduced is increased as compared with the case where the positions of the arm working points in each reference state are dispersed.

  A second aspect of the present invention for solving the above problem is a trajectory planning method performed by a trajectory planning apparatus that performs trajectory planning of an arm, wherein the trajectory planning apparatus is configured to determine a plurality of reference states of the arm. A reference storage unit that stores the reference state, a reference selection step of selecting one reference state from the plurality of reference states according to a target state of the working point of the arm, and the selected reference state As a reference, a calculation step for calculating a temporary joint angle for guiding the work point to the target state, and a conversion step for converting the temporary joint angle into a joint angle in real space are performed. To do.

  According to the second aspect of the present invention described above, the time required for the trajectory planning of the robot arm can be shortened as compared with the conventional case regardless of the target state of the robot arm.

  A third aspect of the present invention for solving the above problem is a robot that includes an arm and performs a trajectory plan of the arm, and a reference storage unit that stores a plurality of reference states for the arm; A reference selection unit that selects one reference state from the plurality of reference states according to the target state of the work point of the arm, and for guiding the work point to the target state based on the selected reference state A calculation unit that calculates the temporary joint angle, a conversion unit that converts the temporary joint angle into a joint angle in real space, and an arm control unit that controls the arm based on the joint angle in real space; , Comprising.

  A fourth aspect of the present invention for solving the above-described problem is a robot system including a robot including an arm and a trajectory planning apparatus that performs trajectory planning of the arm, and the trajectory planning apparatus includes: A reference storage unit that stores a plurality of reference states for the arm; a reference selection unit that selects one reference state from the plurality of reference states according to a target state of the working point of the arm; A calculation unit that calculates a temporary joint angle for guiding the work point to the target state with a reference state as a reference, a conversion unit that converts the temporary joint angle into a joint angle in real space, and the real space And an arm control unit that controls the arm based on the joint angle.

  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 present state of the movable part 30. FIG. (B) It is a figure which shows an example of the target state of a work point. FIG. 3 is a diagram illustrating an example of a state (initial reference) serving as a reference for a movable unit 30. (A) It is a figure which shows an example of the state (1st temporary reference | standard) used as the 1st temporary reference | standard of the movable part 30. FIG. (B) It is a figure which shows an example of the state (2nd temporary reference | standard) used as the 2nd temporary reference | standard of the movable part 30. FIG. (C) It is a figure which shows an example of the state (3rd temporary reference | standard) used as the 3rd temporary reference | standard of the movable part 30. FIG. (D) It is a figure which shows an example of the state (4th temporary reference | standard) used as the 4th temporary reference | standard of the movable part 30. FIG. (E) It is a figure which shows an example of the state (5th temporary reference | standard) used as the 5th temporary reference | standard of the movable part 30. FIG. (A) It is a figure which shows the example which reproduces the present state of the movable part 30. FIG. (B) It is a figure which shows an example of the target state of the movable part 30 calculated | required by iterative calculation.

  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). In the example of this figure, the arm part 4 is a three-axis arm provided with three joints (a first joint J1, a second joint J2, and a third joint J3). Further, as illustrated, each link 7 is named as a first link L1, a second link L2, a third link L3, and the like.

  The hand unit 5 includes, for example, a plurality of fingers, and can move adjacent fingers so as to approach each other, and can sandwich the object to be grasped by at least two fingers. In the present embodiment, the position of the hand unit 5 is referred to as a “work point” of the movable unit 30 (so-called end effector).

  Moreover, the movable part 30 (specifically, each joint 6 and the hand part 5) is provided with, for example, an actuator 31 (shown in FIG. 2) for operating them. The actuator 31 includes, for example, a servo motor and an encoder. The encoder value output from the encoder is used for feedback control of the robot 1 by the control unit 10.

  The movable unit 30 includes a sensor 32 (shown in FIG. 2) in the hand unit 5 and the like. For example, the sensor 32 detects an external force applied to the movable unit 30.

  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 work table, ceiling, wall, or the like. The imaging unit 20 performs imaging in the target position direction of the work point of the movable unit 30 under the control of the control unit 10. Accordingly, the imaging unit 20 generates image data and outputs it 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. The robot mode is not limited to the above. For example, the movable unit 30 in FIG. 1 may be referred to as a “robot”, and the robot 1 in FIG. 1 may be referred to as a “robot system”. Also in this case, the control unit 10 may or may not be built in the robot. The imaging unit 20 may or may not be built in the robot.

  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 illustrated, the control unit 10 includes a central control unit 101, a storage unit 102, an imaging control unit 103, a trajectory planning unit 104, an encoder reading unit 105, and an arm control unit 106.

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

  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.

  In addition, the storage unit 102 stores information for specifying a state (hereinafter referred to as “initial reference”) that serves as a reference for the movable unit 30. In addition, the storage unit 102 includes a plurality of pieces of information (first provisional reference to fifth provisional reference described later) for specifying a state (hereinafter referred to as “temporary reference”) that is a provisional reference of the movable unit 30, I remember it. Note that information for specifying a provisional reference state is necessary for performing a trajectory plan for the movable unit 30 at a higher speed than in the past. Details of the initial standard and the temporary standard will be described later.

  The imaging control unit 103 controls the imaging unit 20. For example, the imaging control unit 103 causes the imaging unit 20 to capture the target position direction of the work point. Then, the imaging control unit 103 acquires image data captured by the imaging unit 20.

  The trajectory planning unit 104 plans a trajectory for the movable unit 30 when the working point of the movable unit 30 is guided from the current state (position and posture) to the target state (position and posture).

  Specifically, the trajectory planning unit 104 calculates the total rotation angle of each joint 6 necessary to guide the work point to the target state and the rotation angle of each joint 6 per unit time (for example, 1 ms). Ask.

  At this time, the trajectory planning unit 104 sets the rotation angle (total rotation angle and rotation angle per predetermined unit time) of each joint 6 using one temporary reference selected from the plurality of temporary references as reference coordinates. In the following, it is also referred to as “temporary joint angle”).

  Note that the temporary reference selected here is that the state (position, posture) of the work point of the movable unit 30 is closest to the target state (position, posture).

  In addition, the trajectory planning unit 104 actually converts the calculated temporary joint angle into a joint angle in real space (that is, an angle with the initial reference as a reference coordinate), so that the working point of the movable unit 30 is actually set. The rotation angle of each joint 6 required to guide the current state from the current state to the target state is obtained.

  By calculating the rotation angle of each joint 6 as described above, the amount of calculation can be reduced as compared with the conventional case (for example, as compared with the case where the initial reference is determined as the reference coordinate). As a result, trajectory planning can be performed at a higher speed than before.

  In the present embodiment, the “current state” of the work point includes at least the “current position” and the “current posture” of the work point. Similarly, the “target state” of the work point includes at least “target position” and “target posture” of the work point.

  The encoder reading unit 105 reads the encoder value of the actuator 31 provided in each joint 6 of the arm unit 4.

  The arm control unit 106 performs control for guiding the work point to the target state in accordance with the trajectory plan by the trajectory plan unit 104. That is, the arm control unit 106 rotates each joint 6 according to the rotation angle per unit time obtained by the trajectory planning unit 104 (however, the initial reference is the reference coordinate).

  The arm control unit 106 controls the movable unit 30 with reference to the encoder value of the actuator 31, the sensor value of the sensor 32, and the like. Thereby, the error of the trajectory can be corrected.

  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 and the control part 10, 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. Further, the shape, size, arrangement, structure, and the like of various members such as the arm unit 4, the hand unit 5, the joint 6, and the link 7 may be appropriately changed.

  In addition, each functional configuration of the control unit 10 described above is classified according to main processing contents in order to facilitate understanding of the configuration of the control unit 10. The present invention is not limited by the way of classification and names of the constituent elements. The configuration of the control unit 10 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 guiding the working point of the movable unit 30 to the target state 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 trajectory planning unit 104 specifies the current state (position, posture) of the movable unit 30 (step S101).

  Specifically, in step S <b> 101, the trajectory planning unit 104 first reads the encoder value of the actuator 31 provided in each joint 6. Then, the trajectory planning unit 104 obtains the current state (position, posture) of the work point from the joint angle of each joint 6 corresponding to the read encoder value (obtained by a method such as forward kinematics).

FIG. 4A is a diagram illustrating an example of the current state of the movable unit 30. As shown, in step S101, the trajectory planning unit 104, a joint angle .theta.c ₁ of the first joint J1, joint angle .theta.c ₂ of the second joint J2, based on the joint angle .theta.c ₃ of the third joint J3, the working point determine the current state ^{M 0.} However, the current state M ⁰ of the work point includes the current position (Xc, Yc) and the current posture (φc).

However, each joint angle (θc ₁ , θc ₂ , θc ₃ ), the current position of the work point (Xc, Yc), and the current posture of the work point (φc) are the coordinates and angle in real space, respectively. Specifically, the initial reference is the reference coordinate.

  Here, the X axis and the Y axis are axes orthogonal to each other as shown in the figure. A plane stretched between the X axis and the Y axis is, for example, a plane parallel to the ground.

  In this embodiment, a two-dimensional space is used to simplify the description, but a three-dimensional space spanned by the X axis, the Y axis, and the Z axis may be used.

  FIG. 5 is a diagram illustrating an example of a state (that is, an initial reference) that serves as a reference for the movable unit 30. As shown in the drawing, in this embodiment, the initial reference of the movable portion 30 is set so that each link 7 (L1, L2, L3) is a straight line in the positive direction of the X axis. In such an initial reference state, the joint angle of each joint 6 is set to “0”, and the position and orientation of the work point are “(X, Y) = (L1 + L2 + L3, 0)” and “φ = 0”, respectively. Become.

  In addition, the state used as the initial reference | standard of the movable part 30 is not limited to what is shown in FIG. For example, the initial reference of the movable unit 30 may be a state in which each link 7 (L1, L2, L3) is a straight line in the positive direction of the Y axis.

  When the current state (position, posture) of the work point is obtained in step S101, the trajectory planning unit 104 next determines the target state of the work point (step S102).

  Specifically, in step S <b> 102, the activation planning unit 104 acquires an image in the target position direction captured by the imaging unit 20. Then, the activation planning unit 104 performs image recognition on the image and identifies an object (or a focal point) that can be designated as the target position. Then, the activation planning unit 104 obtains a target state (position, posture) of the work point based on the identified position and angle of the target object.

  However, the process of step S102 may determine the target state (position, posture) of the work point by a user input, without depending on the method as described above.

  Further, the target state of the work point determined in step S102, that is, the target position (Xa, Ya) of the work point and the target posture (φa) of the work point, respectively, represent coordinates and angles in the real space. Specifically, the initial reference is the reference coordinate.

  FIG. 4B is a diagram illustrating an example of a target state of work points. As shown in the drawing, in step S102, the target state of the work point is determined.

  When the current state and the target state are determined as described above, the activation planning unit 104 makes a trajectory plan for guiding the movable unit 30 from the current state to the target state.

Therefore, the trajectory planning unit 104
(1) Θ ^{(k + 1)} = Θ ^(k) + J (Θ ^(k) ) ⁻¹ × (Ma−M ^(k) )
An operation plan is made for each joint 6 using the recurrence formula.

Here, Θ ^(k) represents the joint angle at the relay point k (the temporary joint angle described above). The trajectory planning unit 104 can obtain the temporary joint angle Θ ^{(k + 1)} of each joint 6 by repeatedly calculating the recurrence formula (1) while incrementing k.

J (Θ ^(k) ) ⁻¹ is a pseudo inverse matrix of the Jacobian matrix. Ma is the target state (may be a target position) of the work point determined in step S102, and M ^(k) is the state (or position) of the work point at the relay point k. However, k is an integer of 0 or more.

  By the way, in the conventional technique, the recurrence formula of (1) is generally repeatedly calculated based on the above-described initial reference state (FIG. 5) and the like.

  However, depending on the target state of the work point, iterative calculation may take time.

  Therefore, in the present embodiment, the trajectory planning unit 104 selects a temporary reference state according to the target state of the work point determined in step S102 in order to shorten the time required for the iterative calculation as compared with the conventional method. (Step S103) Using the selected temporary reference state as a reference (Step S104), iterative calculation is performed (Step S105).

  A plurality of provisional reference states that can be selected in step S103 are prepared as described above.

  FIGS. 6A to 6E are diagrams illustrating examples of states that serve as the first to fifth temporary references of the movable portion 30. FIG.

As shown in FIG. 6A, in the present embodiment, the first temporary reference of the movable unit 30 is set so that each link 7 (L1, L2, L3) is a straight line in the positive direction of the X axis. In such a first provisional reference state, the position and orientation of the work point are “(Xs, Ys) = (L1 + L2 + L3, 0)” and “φs = 0”, respectively. Further, the joint angle of each joint 6 is “(θs ₁ , θs ₂ , θs ₃ ) = (0, 0, 0)”.

In addition, as shown in FIG. 6B, in the present embodiment, the second temporary reference of the movable unit 30 is set such that each link 7 (L1, L2, L3) is a straight line in the positive direction of the Y axis. To do. In such a second provisional reference state, the position and orientation of the work point are “(Xs, Ys) = (0, L1 + L2 + L3)” and “φs = π / 2”, respectively. The joint angle of each joint 6 is “(θs ₁ , θs ₂ , θs ₃ ) = (π / ₂ , 0, 0)”.

Further, as shown in FIG. 6C, in the present embodiment, the third temporary reference of the movable portion 30 is set such that each link 7 (L1, L2, L3) is a straight line in the negative direction of the X axis. To do. In such a third provisional reference state, the position and orientation of the work point are “(Xs, Ys) = (− L1−L2−L3, 0)” and “φs = π”, respectively. Further, the joint angle of each joint 6 is “(θs ₁ , θs ₂ , θs ₃ ) = (π, 0, 0)”.

In addition, as shown in FIG. 6D, in the present embodiment, the fourth temporary reference of the movable unit 30 is set such that each link 7 (L1, L2, L3) is a straight line in the negative direction of the Y axis. To do. In such a fourth provisional reference state, the position and orientation of the work point are “(Xs, Ys) = (0, −L1−L2−L3)” and “φs = −π / 2”, respectively. . The joint angle of each joint 6 is “(θs ₁ , θs ₂ , θs ₃ ) = (− π / ₂ , 0, 0)”.

As shown in FIG. 6E, in the present embodiment, the fifth temporary reference of the movable portion 30 is set so that each link 7 (L1, L2, L3) is a broken line. In such a fifth provisional reference state, the position and orientation of the work point are “(Xs, Ys) = (L2, L1 + L3)” and “φs = π / 2”, respectively. The joint angle of each joint 6 is “(θs ₁ , θs ₂ , θs ₃ ) = (π / 2, −π / 2, π / 2)”.

  By the way, as a specific process of step S103, for example, the trajectory planning unit 104 may select a temporary reference state closest to the target state (Xa, Ya, φa) determined in step S102.

More specifically, the trajectory planning unit 104, in step S103,
(2) Δ = (Xa−Xs) ² + (Ya−Ys) ² + (φa−φs) ²
The state of the temporary reference that minimizes the value may be selected from the plurality of temporary references.

  In step S104, the trajectory planning unit 104 offsets by an appropriate joint angle in the coordinate system based on the temporary reference selected in step S103, so that the current state of the movable unit 30 specified in step S101 is obtained. To reproduce.

Specifically, the trajectory planning unit 104 uses the joint angles (θc ₁ , θc ₂ , θc ₃ ) identified in step S101 to realize the temporary reference state selected in step S103. A value obtained by subtracting (θs ₁ , θs ₂ , θs ₃ ) is set as an offset value. That is, the joint angles (Θ ₁ ⁰ , Θ ₂ ⁰ , Θ ₃ ⁰ ) that reproduce the current state with the temporary reference as a reference are (θc ₁ −θs ₁ , θc ₂ −θs ₂ , θc ₃ −θs _3). )

FIG. 7A is a diagram illustrating a case where the current state of the movable unit 30 is offset. In the illustrated example, the movable portion 30 in the current state shown in FIG. 4A is realized by offsetting the fifth temporary reference (within the dotted line) by an appropriate joint angle with reference coordinates. With respect to the state that realizes the fifth temporary reference, each state that realizes the state of the temporary reference selected in step S103 from the current joint angles (θc ₁ , θc ₂ , θc ₃ ) specified in step S101. Each joint angle (Θ ₁ ⁰ , Θ ₂ ⁰ , Θ ₃ ⁰ ) (= (θc ₁ −θs ₁ , θc ₂ −θs ₂ , θc) obtained by subtracting the joint angles (θs ₁ , θs ₂ , θs ₃ ) ₃₋ θs ₃ )) is added to reproduce the current state when the fifth temporary reference is the reference coordinate.

Then, in step S105, the trajectory planning unit 104 determines each joint angle (Θ ₁ ⁰ , Θ ₂ ⁰ , Θ ₃ ⁰ ) obtained here and the current state M ⁰ obtained in step S101 (1) above. Is substituted as an initial value of the recurrence formula (when k = 0), and iterative calculation is performed for each relay point k.

  The trajectory planning unit 104 changes the Jacobian matrix used in the iterative calculation of (1) above according to the provisional reference state selected in step S103.

  For example, when the first temporary reference is selected, the trajectory planning unit 104 performs iterative calculation using the following determinant 1 as the Jacobian matrix J.

  When the second temporary criterion is selected, the trajectory planning unit 104 performs iterative calculation using the following determinant 2 as the Jacobian matrix J.

  When the third temporary criterion is selected, the trajectory planning unit 104 performs iterative calculation using the following determinant 3 as the Jacobian matrix J.

  When the fourth temporary criterion is selected, the trajectory planning unit 104 performs iterative calculation using the following determinant 4 as the Jacobian matrix J.

  When the fifth temporary criterion is selected, the trajectory planning unit 104 performs iterative calculation using the following determinant 5 as the Jacobian matrix J.

  By preparing the Jacobian matrix J as described above, the recurrence formula (1) can be calculated regardless of which temporary criterion is selected in step S103.

The term (Ma−M ^(k) ) in (1) above indicates the distance (also referred to as “closeness” ⁾ between the target state Ma of the work point and the state M ^(k) at the relay point k. Represents.

  Then, the trajectory planning unit 104 determines that the work point has converged to the target state when the distance becomes equal to or smaller than a predetermined threshold (step S106).

Specifically, the trajectory planning unit 104
(3) (Ma-M ^(k) ) ² ≦ Err
Is satisfied, it is determined that the work point has converged to the target state. However, Err is a predetermined constant.

  Here, when the work point has not converged to the target state (step S106; No), the trajectory planning unit 104 increments the relay point k and returns the process to step S105.

As described above, the trajectory planning unit 104 repeatedly calculates the recurrence formula of the formula (1) until the formula (condition) of the above (3) is satisfied by repeating step S105. Thereby, the trajectory planning unit 104 makes provisional joint angles (Θ ₁ ^{(k + 1)} , Θ ₂ ^{(k + 1)} , Θ ₃ ^{(k + 1)} ) of each joint 6 every predetermined unit time (that is, every relay point k). Can be requested.

FIG. 7B is a diagram illustrating an example of a target state of the movable unit 30 obtained by iterative calculation. As shown in the figure, the temporary joint angles (Θ ₁ ^{(k + 1)} , Θ ₂ ^{(k + 1)} , Θ ₃ ^{(k + 1)} ) of each joint 6 are relatively small angles.

As described above, when the working point converges to the target state (step S106; Yes) and the temporary joint angles (Θ ₁ ^{(k + 1)} , Θ ₂ ^{(k + 1)} , Θ ₃ ^{(k + 1)} ) are obtained, the trajectory planning unit In step 104, the process proceeds to step S107.

By the way, the temporary joint angles (Θ ₁ ^{(k + 1)} , Θ ₂ ^{(k + 1)} , Θ ₃ ^{(k + 1)} ) obtained here are angles based on the temporary reference state selected in step S103.

  Therefore, the trajectory planning unit 104 converts the calculated temporary joint angle into a joint angle in real space (that is, an angle with the initial reference as a reference coordinate). Thereby, the trajectory planning unit 104 obtains the rotation angle θ of each joint 6 necessary for actually guiding the working point of the movable unit 30 from the current state to the relay point (k + 1) (step S107).

Specifically, the trajectory planning unit 104 obtains temporary joint angles (Θ ₁ ^{(k + 1) 1} , Θ ₂ ^{(k + 1)} ₂ , Θ ₃ ⁽ for each predetermined unit time (each relay point k)) obtained by iterative calculation in step S105. ^{k + 1)} ) may be added to each joint angle (θs ₁ , θs ₂ , θs ₃ ) that realizes the temporary reference state selected in step S103. That is, the rotation angle θ of each joint 6 required to guide the work point from the current state to the relay point k is expressed as “(θ ₁ ^{(k + 1)} , θ ₂ ^{(k + 1)} , θ ₃ ^{(k + 1)} ) = (Θ ₁ ^{(K + 1)} , Θ ₂ ^{(k + 1)} , Θ ₃ ^{(k + 1)} ) + (θs ₁ , θs ₂ , θs ₃ ) ”.

Further, if the relay point k + 1 is a value when the work point has converged (reached) to the target state, the rotation of each joint 6 required to actually guide the work point of the movable unit 30 from the current state to the target state. The angle θa can be obtained. That is, the trajectory planning unit 104 determines that “(θa ₁ , θa ₂ , θa ₃ ) = (Θ ₁ ^{(k + 1)} , Θ ₂ ^{(k + 1)} , Θ ₃ ^{(k + 1)} ) + (θs ₁ , θs ₂ , θs ₃ ) The rotation angle θa can be obtained by the calculation formula (where k + 1 is a value when the working point converges to the target state).

  As described above, when the operation plan for each joint 6 is established, the arm control unit 106 performs control for guiding the work point to the target state in accordance with the operation plan (step S108). That is, the arm control unit 106 rotates each joint 6 according to the rotation angle θ (initial reference) for each unit time obtained by the trajectory planning unit 104.

  Then, when the work point is led to the target state, the arm control unit 106 ends this flow (arm control task).

  The above processing is the arm control task according to the present embodiment. As described above, according to the arm control task according to the present embodiment, the time required for the trajectory planning can be shortened compared to the conventional case regardless of the target state of the movable unit 30 (robot arm).

  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, five types of temporary standards (first to fifth temporary standards) are prepared. However, the present invention is not limited to this, and may be further increased or decreased.

  In the first to fourth temporary references shown in the above embodiment, the positions of the work points are dispersed. In this way, by preparing a plurality of temporary references so that the positions of the work points are dispersed, it is possible to repeat the recurrence formula (1) regardless of where the target state is specified (determined). Probability of reducing the number of times increases.

  Further, the present invention is not limited to the above-described embodiment, and a plurality of temporary standards may be prepared so that work points are concentrated in an area that is easily designated as a target state. For example, the space in a range where the work point of the movable unit 30 can reach is divided into a plurality of small areas, and the position of each temporary reference work point is placed in the small area where the number of times designated in the past as the target state is (most). Keep it focused. This increases the probability that the number of iterations of the recurrence formula (1) can be significantly reduced as compared with the case where the positions of the temporary reference work points are dispersed.

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 unit, 30 ... movable unit, 101 ... central control unit, 102 ... storage unit , 103 ... imaging control unit, 104 ... trajectory planning unit, 105 ... encoder reading unit, 106 ... arm control unit.

Claims (8)

A trajectory planning device for trajectory planning of an arm,
A reference storage unit for storing a plurality of reference states for the arm;
A reference selection unit that selects one reference state from the plurality of reference states according to a target state of the working point of the arm;
A calculation unit that calculates a temporary joint angle for guiding the work point to the target state with the selected reference state as a reference;
A conversion unit that converts the temporary joint angle into a joint angle in real space, and
Trajectory planning device characterized by that. The trajectory planning apparatus according to claim 1,
The reference selection unit includes:
Selecting a reference state closest to the target state of the working point;
Trajectory planning device characterized by that. The trajectory planning apparatus according to claim 1 or 2,
The calculator is
Depending on the selected reference state, the Jacobian matrix used for the calculation of the temporary joint angle is varied.
Trajectory planning device characterized by that. The trajectory planning apparatus according to any one of claims 1 to 3,
The plurality of reference states are:
The positions of the arm working points in each reference state are dispersed,
Trajectory planning device characterized by that. The trajectory planning apparatus according to any one of claims 1 to 3,
The plurality of reference states are:
The position of the work point of the arm in each reference state is concentrated in an area that is easily designated as the target state,
Trajectory planning device characterized by that. A trajectory planning method performed by a trajectory planning apparatus that performs trajectory planning of an arm,
The trajectory planning device
A reference storage unit storing a plurality of reference states of the arm;
A reference selection step of selecting one reference state from the plurality of reference states according to a target state for the working point of the arm;
A calculation step of calculating a temporary joint angle for guiding the work point to the target state with respect to the selected reference state;
Converting the temporary joint angle into a joint angle in real space; and
A trajectory planning method characterized by that. A robot comprising an arm and performing trajectory planning of the arm,
A reference storage unit for storing a plurality of reference states for the arm;
A reference selection unit that selects one reference state from the plurality of reference states according to a target state of the working point of the arm;
A calculation unit that calculates a temporary joint angle for guiding the work point to the target state with the selected reference state as a reference;
A conversion unit that converts the temporary joint angle into a joint angle in real space;
An arm control unit for controlling the arm based on a joint angle in the real space,
A robot characterized by that. A robot system comprising: a robot including an arm; and a trajectory planning apparatus that performs trajectory planning of the arm,
The trajectory planning device
A reference storage unit for storing a plurality of reference states for the arm;
A reference selection unit that selects one reference state from the plurality of reference states according to a target state of the working point of the arm;
A calculation unit that calculates a temporary joint angle for guiding the work point to the target state with the selected reference state as a reference;
A conversion unit that converts the temporary joint angle into a joint angle in real space;
An arm control unit for controlling the arm based on a joint angle in the real space,
A robot system characterized by this.

Images