JP2022059740A - Robot and robot control program - Google Patents

Abstract

To reduce a calculation load of an operation path of a multi-joint robot, compared to when using a joint space.SOLUTION: A robot (S) comprises: an arm member (1) having a plurality of arm parts (2), and a joint part (3) arranged at a connection site between two arm parts (2); first posture deriving means (C3) for deriving a posture after the arm member (1) has operated, by solving inverse kinematics with regard to a position of the joint part (3) in an operation of moving a free end (2k) of the arm member (1) to a predetermined target (21); and a second posture deriving means (C6) for deriving an intermediate posture of the arm member (1), by solving inverse kinematics using, as a target position, an intermediate position (22) that is a candidate position through which the free end (2k) passes during operation, in the case of interfering with an obstacle (20) in the middle of the operation, on the basis of a position before operation and a position after operation of each joint part (3) derived by the first posture deriving means (C3).SELECTED DRAWING: Figure 5

Description

The present invention relates to a robot having an arm portion and a joint portion and a robot control program for controlling the robot, and in particular, a robot and robot control using inverse kinematics when controlling the movement of the free end of the robot to a target. Regarding the program.

The robot is controlled by calculating the trajectory of the posture of the robot so as to move a specific part such as a grip portion of the robot to a target position. As such a technique, the techniques described in the following Patent Documents 1 to 4 are known.

In Patent Document 1 (Japanese Unexamined Patent Publication No. 2020-93364), when deriving an orbit for moving the grip portion (20) of a moving body (1) such as a robot, first, as many orbital candidates (71a to 71e) as possible are derived. ) Is generated, the orbital candidates (71a to 71e) are evaluated, and the orbital (71b) having a high evaluation is output.

In Patent Document 2 (Japanese Unexamined Patent Publication No. 2007-313592), when a robot moves, an obstacle and an obstacle are found every unit time based on the current position, speed, acceleration, traveling direction, shape and size of the obstacle and the robot. It describes that the relative distance, relative velocity, and relative acceleration of an object and a robot are calculated, and based on the calculated information, an action model is selected from the action candidate models stored in the database to create an action path. .. Patent Document 2 describes that a repulsive force potential that increases as the distance to an obstacle gets closer is calculated to evaluate the distance to the obstacle, and a behavior model that does not get too close to the obstacle is selected.

In Patent Document 3 (Japanese Unexamined Patent Publication No. 2007-237334), when searching for a route from a start position to a target position in an articulated robot, the route is first searched by a random search algorithm and then searched. If there is an obstacle on the route, the route to the end position closest to the obstacle is derived, and then the route is searched without the obstacle assuming that the obstacle has been removed. It is stated that.

In Patent Document 4 (Japanese Unexamined Patent Publication No. 2000-20117), the geometric shape of the robot and the working environment is expressed by a geometric model (C space) in a technique for generating an operation path plan of a robot having two degrees of freedom. , Searching the route from the start to the goal on the geometric model. In Patent Document 4, the route interference is inspected and the distance is evaluated, and the route having the shortest sum of distances is selected and output.

Japanese Unexamined Patent Publication No. 2020-93364 ("0050"-"0056", FIGS. 3, 6 to 9) JP-A-2007-131592 ("0023"-"0042") JP-A-2007-237334 ("0023"-"0050") Japanese Unexamined Patent Publication No. 2000-20117 ("0009"-"0011")

(Problems of conventional technology)
In the techniques described in Patent Documents 1 and 3, trajectory candidates and random searches are performed. If the number of joints of the robot is as small as 1 to 3, it is possible to calculate, but the joints As the number increases, the amount of calculation becomes enormous and the calculation load becomes a problem. When the calculation load becomes large, there is also a problem that real-time control becomes difficult. The techniques described in Patent Documents 2 and 4 also calculate many parameters every unit time and perform evaluation each time, and there is a problem that the calculation load becomes enormous as the number of joints increases.

Currently, when controlling an articulated robot such as an industrial robot arm, it is known to solve the inverse kinematics to control the articulated robot. When the free end of the robot (the hand of the robot) reaches the target, it is forward kinematics to calculate the position (result) of the free end from the angle and orientation (cause) of each joint. Inverse kinematics is the calculation (back calculation) of the angle and orientation (cause) of each joint from the position (result) of the part.
As a method of obtaining a result in inverse kinematics, an analytical solution method that solves analytically using an equation or the like and a numerical solution method that solves approximately by applying a numerical value are known. The analytical solution has the limitation that it is limited to the case where the structure of the articulated robot meets special conditions, and the numerical solution has the advantage that it can be used regardless of the structure of the robot.

FIG. 7 is an explanatory diagram of a conventional solution of inverse kinematics, FIG. 7A is an explanatory diagram of an example of a robot model, FIG. 7B is an explanatory diagram of a joint space, and FIG. 7C is an enlarged view of a main part of FIG. 7B.
In FIG. 7, when the joint portions 01 are two arms 02, the displacement amounts of the joint portions 01 are set to q ₀ and q ₁ , and each displacement amount q ₀ in consideration of obstacles, the range of motion of the joint portions 01, and the like. , Q ₁ can be represented by a plane as shown in FIG. 7B. In FIG. 7B, if both displacements q ₀ and q ₁ can take the value, they are represented by white circles, and if one or both displacements q ₀ and q ₁ cannot take the value, they are black circles. It is represented by. Therefore, when deriving the path 06 that moves from the initial position 03 to the target position 04 using the joint space, the distance of the obstacle or the like is evaluated by the evaluation function (the closer to the obstacle, the lower the evaluation, and the longer the distance, the better the evaluation. The path 06 indicated by the arrow in FIG. 7C is selected by evaluation with the function).

When the conventional joint space and the evaluation function shown in FIG. 7 are used, it is necessary to calculate the entire range of values that each displacement amount q ₀ and q ₁ can take, which causes a problem that the calculation amount is large.
In the example shown in FIGS. 7B and 7C, since the number of joints 01 was two, the value that the displacement amount could take could be expressed in a two-dimensional plane, but when the number of joints 01 was n. Is an n-dimensional space. This n-dimensional space is called "joint space", "configuration space", or "configuration space (C space, see Patent Document 4)". Therefore, as the number of joint portions 01 increases, the n-dimensional dimension increases, the calculation of the joint space becomes exponentially enormous, and there is a problem that it takes time to derive the path 06.
Especially if the obstacle is stationary, the calculation in the joint space may still be practical if it takes time, but if the obstacle moves, the joint space is recalculated every time the obstacle moves. There is also a problem that the calculation load becomes even larger.

It is a technical subject of the present invention to reduce the calculation load of the motion path of the articulated robot as compared with the case of using the joint space.

In order to solve the technical problem, the robot according to claim 1 is the robot of the invention.
An arm member having a plurality of arm portions and a joint portion arranged at a connecting portion between the two arm portions and capable of displacing the other arm portion with respect to one arm portion.
A first posture deriving means for deriving the post-motion posture of the arm member by solving the inverse kinematics for the position of the joint in the motion of moving the free end of the arm member to a predetermined target.
Based on the pre-motion position and post-motion position of each joint derived by the first posture derivation means, if an obstacle is interfered with during the motion, the candidate for which the free end passes during the motion. A second posture deriving means for deriving the intermediate posture of the arm member by solving the reverse kinematics with the intermediate position as the target position as the target position.
When the arm member interferes with an obstacle during the operation derived by the first posture deriving means, the arm member is free to pass through the intermediate posture derived by the second posture deriving means. Arm control means that controls each joint so that the end is moved to the target,
It is characterized by being equipped with.

The invention according to claim 2 is the robot according to claim 1.
In the posture of the arm member after the operation derived by the first posture deriving means, the position of the joint portion on the base end side of the free end is set as the intermediate position, and the intermediate posture is derived. Posture derivation means,
It is characterized by being equipped with.

The invention according to claim 3 is the robot according to claim 2.
When the intermediate posture is derived with the position of the joint portion on the base end side of the free end as the intermediate position in the posture of the arm member after the operation, an obstacle is formed while moving from the posture before the operation to the intermediate posture. In the case of interference with, in the posture of the arm member before movement, the intermediate position is targeted from the second intermediate posture in which the position of the joint portion on the base end side of the free end is the position of the free end. The second posture derivation means for solving reverse kinetics,
It is characterized by being equipped with.

In order to solve the technical problem, the robot control program of the invention according to claim 4 is used.
Controls a robot having an arm member having a plurality of arm portions and a joint portion arranged at a connecting portion between the two arm portions and capable of displacing the other arm portion with respect to one arm portion. It ’s a robot control program.
Computer,
A first posture deriving means for deriving a post-motion posture of the arm member by solving inverse kinematics for the position of the joint in an motion of moving the free end of the arm member to a predetermined target.
Based on the pre-motion position and post-motion position of each joint derived by the first posture derivation means, if an obstacle is interfered with during the motion, the candidate for which the free end passes during the motion. A second posture deriving means for deriving the intermediate posture of the arm member by solving the inverse kinematics with the intermediate position as the target position as the target position.
When the arm member interferes with an obstacle during the operation derived by the first posture deriving means, the arm member is free to pass through the intermediate posture derived by the second posture deriving means. Arm control means that controls each joint to move the end to the target,
It is characterized by functioning as.

According to the inventions of claims 1 and 4, the calculation load of the motion path of the articulated robot can be reduced as compared with the case of using the joint space.
According to the second aspect of the present invention, it is possible to derive a path based on the final posture by deriving a path having a joint portion on the base end side of the free end as an intermediate position. can.
According to the third aspect of the present invention, when the interference cannot be eliminated only by the intermediate posture, the second intermediate posture is set and the path is derived, as compared with the case where the second intermediate posture is not set. , Interference is easy to eliminate.

FIG. 1 is a schematic explanatory view of the robot according to the first embodiment of the present invention. FIG. 2 is an explanatory diagram showing the control unit of the first embodiment as a functional block diagram. FIG. 3 is an explanatory diagram of a state before the operation of the arm member of the first embodiment. FIG. 4 is an explanatory diagram of an example of a posture after an operation in which the inverse kinematics of the arm member of the first embodiment is solved, FIG. 4A is an explanatory diagram of an example in the case of solving by a numerical solution method of the inverse kinematics, and FIG. 4B is an explanatory diagram. It is explanatory drawing of an example when it was solved by the inverse kinematics with priority. FIG. 5 is an explanatory diagram when the intermediate position is set as the target position, FIG. 5A is an explanatory diagram when inverse kinematics is solved with the first intermediate position as the target position, and FIG. 5B is an explanatory diagram when the second intermediate position is set. It is explanatory drawing of the state which was done. FIG. 6 is an explanatory diagram of a flowchart of the operation plan output process of the first embodiment. FIG. 7 is an explanatory diagram of a conventional solution of inverse kinematics, FIG. 7A is an explanatory diagram of an example of a robot model, FIG. 7B is an explanatory diagram of a joint space, and FIG. 7C is an enlarged view of a main part of FIG. 7B.

Next, specific examples of embodiments of the present invention (hereinafter referred to as Examples) will be described with reference to the drawings, but the present invention is not limited to the following examples.
In addition, in the explanation using the following drawings, the illustrations other than the members necessary for the explanation are omitted as appropriate for the sake of easy understanding.

FIG. 1 is a schematic explanatory view of the robot according to the first embodiment of the present invention.
In FIG. 1, the robot arm system S of the first embodiment as an example of the robot of the present invention has an arm member 1. The arm member 1 has a plurality of arm portions 2 and a joint portion 3 that connects the arm portions 2 to each other. The arm member 1 of the first embodiment has a first arm portion 2a whose one end is supported by the pedestal portion 4. A first joint portion 3a is supported at the other end of the first arm portion 2a. Similarly, the second arm portion 2b, the second joint portion 3b, ..., The seventh joint portion 3g, and the eighth arm portion 2h are connected. A robot hand portion 2k capable of gripping and releasing an object is formed at the other end (tip) of the eighth arm portion 2h.
Therefore, in the arm member 1 of the first embodiment, as a whole, one end of the first arm portion 2a is a base end portion, and the robot hand portion 2k is a free end portion.

In Example 1, a configuration having seven joint portions 3a to 3g and eight arm portions 2a to 2h has been exemplified, but the present invention is not limited to this. Further, in the first embodiment, the joint type is only a rotary joint, but a robot arm including a linear motion joint may be used. Hereinafter, as an example of the "displacement amount", it may be simply referred to as the "rotation amount" of the rotary joint, but it may also mean the "movement amount" of the translational movement of the linear joint, and can be read as appropriate. ..
The number of arm portions 2 and joint portions 3 can be increased or decreased to an arbitrary number depending on the use, design, specifications, and the like of the arm member 1. In particular, in inverse kinematics, in the X-axis, Y-axis, and Z-axis Cartesian coordinate systems, the positions x, y, and z of the joint portion 3 and the rotation angles θx, θy around the X-axis, Y-axis, and Z-axis, If the number of joints 3 is 6 or less for the 6 variables of θz, the number of equations that hold for each joint is smaller than the variables, and if certain conditions are met, it can be solved analytically. It may be possible. On the other hand, when the number of joints 3 is 7 or more, it is basically difficult to solve analytically, and since a numerical solution is used in principle, the number of joints 3 is 7. In the above cases, the invention of the present application can be suitably used.

The joint portions 3 (3a to 3g) are configured to be able to rotate (displace) the other arm portions 2b to 2h with respect to one arm portion 2a to 2g about the rotation axis. Each joint portion 3 has a built-in motor (not shown), and is configured to be able to control the amount of rotation by controlling the rotation drive and rotation stop of the motor. In the first embodiment, the motor of each joint portion 3 is configured to receive and drive a control signal by a wireless communication chip (not shown), but the motor is not limited to this. It is also possible to control the motor as a configuration that can send and receive control signals by wire such as a communication cable. The wireless communication or wired communication method can be any conventionally known communication method, and any wireless communication method such as a mobile phone line, Bluetooth (registered trademark), wireless LAN, etc. can be adopted. It is possible to adopt any wired communication method such as a serial method such as USB (Universal Serial Bus) or a parallel method.

The arm member 1 is controlled by a computer device 11 as an example of an information processing device. In the first embodiment, the arm member 1 is configured to enable wireless communication with the computer device 11. The computer device 11 includes a computer main body 12, a display 13 as an example of a display unit, and a keyboard 14 and a mouse 15 as an example of an input unit. In the first embodiment, the desktop type computer device is exemplified as the computer device 11, but the present invention is not limited to this, and a laptop type (notebook type) computer device can also be used. Further, the computer device 11 is not limited to the personal computer device, and may be in any form such as a server type, a microcomputer type, and a chip type.

(Explanation of control unit)
FIG. 2 is an explanatory diagram showing the control unit of the first embodiment as a functional block diagram.
The control unit (controller) C of the computer main body 12 of the first embodiment is an I / O (input / output interface) for inputting / outputting signals to / from the outside and adjusting the input / output signal level, and for performing necessary activation processing. ROM (read-only memory) in which programs and data are stored, RAM (random access memory) for temporarily storing necessary data and programs, CPU that performs processing according to the startup program stored in ROM etc. It is composed of a (central processing unit) and a computer device having a clock oscillator and the like, and various functions can be realized by executing a program stored in the ROM, RAM and the like.
The computer main body 12 stores basic software for controlling basic operations, a so-called operating system, a robot control program as an example of an application program, and other software (not shown).
The robot control program of the first embodiment has the following functions (means, functional modules) C1 to C7.

FIG. 3 is an explanatory diagram of a state before the operation of the arm member of the first embodiment.
The target position storage means C1 stores the target position of the robot hand portion 2k of the arm member 1. In the first embodiment, the target position 21 is input by the user through the keyboard 14 and the mouse 15 of the computer device 11.
The initial state storage means C2 stores the initial state of the arm member 1, that is, the state before the operation. In FIG. 3, the initial state storage means C2 of the first embodiment stores, as an example, a state in which the arm member 1 stands upright from the base end to the robot hand portion 2k, which is a free end, as an initial state. There is. In FIG. 3, an obstacle 20 is set for the arm member 1.

The first attitude deriving means C3 has an inverse kinematics analysis means C3a and a prioritized inverse kinematics analysis means C3b, and moves the free end (robot hand portion 2k) of the arm member 1 to the target position 21. Inverse kinematics is solved for the positions and displacements of the joint portions 3a to 3g in the motion, and the posture of the arm member 1 after the motion is derived.
The inverse kinematics analysis means C3a solves the inverse kinematics for calculating the angle (displacement amount) and direction of each joint portion 3a to 3g from the target position 21 of the robot hand portion 2k of the arm member 1, and obtains the arm member 1. The amount of displacement from the initial state until the robot hand unit 2k reaches the target position 21 is derived. In Example 1, the numerical solution method of inverse kinematics is used, but since the numerical solution method of inverse kinematics is conventionally known (see, for example, Japanese Patent No. 5586015), detailed description thereof will be omitted.

When solving the reverse kinematics, the prioritized reverse kinematics analysis means C3b not only brings the robot hand portion 2k closer to the target position 21, but also avoids the obstacle 20 at a specific position in the middle of the arm member 1. Priority is given to bringing the robot hand unit 2k closer to the target position 21 than to bring the specific position on the way closer to the second target position while approaching the second target position different from the target position 21. The amount of displacement of the arm member 1 from the initial state until the robot hand portion 2k reaches the target position 21 is derived by solving the "reverse kinematics".
Inverse kinematics with priority is described, for example, in Non-Patent Document 1 “T. Sugihara:“ Robust Solution of Prioritized Inverse Kinematics Based on Hestenes-Powell Multiplier Method, ”IEEE / RSJ International Conference on Intelligent Robots and Systems, pp. 510-515, 2014. ”and Non-Patent Document 2“ Yoshinori Sekiguchi, Naoyuki Takei: “Numerical Solution of Priority Inverse Kinematics by Cutting Method,” Journal of the Institute of Electrical and Electronics Engineers, vol.37, no.8, pp .711-717, 2019. ”, Non-Patent Document 3“ M. Sekiguchi and N. Takesue: “Fast and Robust Numerical Method for Inverse Kinematics with Prioritized Multiple Targets for Redundant Robots,” Advanced Robotics, vol.34, no.16 , Pp.1068-1078, 2020. ”, etc., and since it is publicly known, detailed explanation is omitted.

The interference determination means C4 determines whether or not the arm member 1 is interfering with the obstacle 20. The interference determining means C4 determines whether or not a part of the arm member 1 exists (enters) in the area of the obstacle 20 in the space where the arm member 1 is arranged, thereby causing interference. Determine if it is.

FIG. 4 is an explanatory diagram of an example of a posture after an operation in which the inverse kinematics of the arm member of the first embodiment is solved, FIG. 4A is an explanatory diagram of an example in the case of solving by a numerical solution method of the inverse kinematics, and FIG. 4B is an explanatory diagram. It is explanatory drawing of an example when it was solved by the inverse kinematics with priority.
When deriving the motion plan for moving the arm member 1 to the target position 21, the first posture deriving means C3 of the first embodiment first derives the motion plan by the inverse kinematics analysis means C3a and then the interference determining means C4. Determine the interference. If the arm member 1 does not interfere with the obstacle 20 in the post-motion state, the derivation result of the inverse kinematics analysis means C3a is adopted as the post-motion posture of the motion plan. In the derivation by the inverse kinematics analysis means C3a, as shown in FIG. 4A, when the arm member 1 interferes with the obstacle 20, the derivation is performed by the prioritized inverse kinematics analysis means C3b. Then, the interference discrimination means C4 discriminates the interference with respect to the posture after the motion derived by the prioritized inverse kinematics. As shown in FIG. 4B, when the arm member 1 does not interfere with the obstacle in the post-motion state, the derivation result of the prioritized inverse kinematics analysis means C3b is adopted as the post-motion posture of the motion plan. do. If the result that does not interfere with the inverse kinematics with priority cannot be obtained, an error is output as the motion plan cannot be generated.

The trajectory generating means C5 as an example of the path generating means generates a trajectory (path) of the arm member 1 from the initial state (start point) to the adopted posture (end point) after the operation. The trajectory generating means C5 linearly interpolates the start point and the end point to generate a joint trajectory. Specifically, the linear interpolation of the first embodiment is calculated as follows. First, let qs be the displacement vector for each of the joint portions 3 (3a to 3g) that are the starting points, and qg be the joint displacement vector that is the ending point. Further, the time required for transitioning from the start point to the end point is τ, and the operating time t (0 ≦ t ≦ τ). The joint trajectory q (t) obtained by linearly interpolating the start point and the end point is expressed by the following equation (1).
q (t) ＝ qs ＋ (t / τ) ・ (qg－qs)… Equation (1)
In the orbit generated by the orbit generating means C5, the interference determining means C4 determines whether or not the obstacle 20 is interfered with during the operation. That is, it is determined whether or not the linear orbit represented by the equation (1) passes through the region of the obstacle 20. If the track does not interfere with the obstacle 20, the derived track is adopted as the solution of the motion plan.

The second posture deriving means C6 has an intermediate position setting means C6a, and operates based on the pre-operation position and the post-operation position of each of the joint portions 3a to 3g derived by the first posture deriving means C3. In the case of interfering with the obstacle 20 in the middle of the operation, the reverse kinematics is solved with the intermediate position 22 which is the candidate position through which the free end passes during operation as the target position, and the intermediate posture of the arm member 1 is derived. The second posture deriving means C6 of the first embodiment sets the intermediate position 22 as a new target position when it is determined that the orbit generated by the orbit generating means C5 interferes with the obstacle 20, and the first posture deriving means is derived. It is solved by the numerical solution method of inverse kinematics as in the means C3.

FIG. 5 is an explanatory diagram when the intermediate position is set as the target position, FIG. 5A is an explanatory diagram when inverse kinematics is solved with the first intermediate position as the target position, and FIG. 5B is an explanatory diagram when the second intermediate position is set. It is explanatory drawing of the state which was done.
The intermediate position setting means C6a sets the intermediate position 22. In the first embodiment, as an example of the intermediate position 22, in the posture of the arm member 1 after the operation (state in FIG. 4B), the position of the joint portion 3g on the base end side of the free end (robot hand portion 2k) is set. , Set to the intermediate position (first intermediate position) 22. So to speak, the position of the wrist (joint portion 3g) with respect to the hand (robot hand portion 2k) in the posture after movement is set to the intermediate position 22, and the intermediate position 22 is set as a new target. Therefore, as shown in FIG. 5A, the second posture deriving means C6 derives the first intermediate posture targeting the intermediate position 22 as a state in front of the final target position 21 instead of the final target position 21. The motion plan (first intermediate posture and trajectory) is derived.
Obstacle 20 in the trajectory derived with the position of the joint portion 3g after the movement as the intermediate position (first intermediate position) 22, that is, the trajectory from the initial state (FIG. 3) to the first intermediate posture (FIG. 5A). When it no longer interferes with, the final trajectory passes through the trajectory from the initial state (start point) to the first intermediate position and the trajectory from the first intermediate position to the post-operation state (end point). Output as an operation plan.

Further, when the second posture deriving means C6 interferes even in the trajectory derived by setting the position of the joint portion 3g as the intermediate position 22, in the posture (see FIG. 3) before the operation (initial state), the second posture deriving means C6 interferes. The second intermediate posture (Fig.) With the position (second intermediate position 23) of the joint portion 3g on the base end side of the free end (robot hand portion 2k) as the target position of the free end (robot hand portion 2k). 5B) is derived, and an operation plan (trajectory from the second intermediate position to the first intermediate position) in which the robot hand unit 2k moves from the second intermediate position 23 to the first intermediate position 22 is derived. That is, the intermediate position is increased. If the trajectory moving from the second intermediate posture (FIG. 5B) to the first intermediate posture (FIG. 5A) does not interfere with the obstacle 20, the initial state (starting point, FIG. 3) to the second intermediate position (FIG. 3). Orbit to 5B), orbit from the second intermediate position (FIG. 5B) to the first intermediate position (FIG. 5A), orbit from the first intermediate position (FIG. 5A) to the post-motion position (FIG. 4B). The trajectory that passes through in order is output as the final motion plan.
In the first embodiment, when the obstacle 20 is interfered with even if the second intermediate posture is set, the position of the joint portion 3f on the two proximal ends side from the tip in the posture after the movement is set as the third intermediate position. The position of the joint portion 3f on the two proximal ends side from the tip in the posture before the operation is the fourth intermediate position, and the position of the joint portion 3e on the three proximal ends sides from the tip in the posture after the operation is the fifth intermediate position. Set the positions of the joints 3e on the three base end sides from the tip in the posture before movement in the order of the sixth intermediate position, ... (Increase the intermediate position), and each intermediate position until there is no interference. We will derive the orbit between them.

The arm control means C7 controls the arm member 1 to move the free end (robot hand portion 2k) to the target. The arm control means C7 of the first embodiment controls the displacement amount of the joint portions 3a to 3g of the arm member 1 according to the motion plan output from any of the posture derivation means C3 and C6, and controls the robot hand portion 2k. Move to the target. Therefore, if the motion plan is output by the first posture deriving means C3, the arm control means C7 of the first embodiment moves according to the motion plan derived by the first posture deriving means C3. Further, if the operation plan is output by the second posture deriving means C6, the arm control means C7 of the first embodiment follows the operation plan via the intermediate posture derived by the second posture deriving means C6. The robot hand portion 2k of the arm member 1 is moved to the target.

(Explanation of flowchart)
Next, the flow of control in the robot arm system S of the first embodiment will be described using a flow chart, a so-called flowchart.
FIG. 6 is an explanatory diagram of a flowchart of the operation plan output process of the first embodiment.
The processing of each step ST in the flowchart of FIG. 6 is performed according to the program stored in the control unit C. Further, this process is executed in parallel with other various processes of the computer main body 12. When the user's control start input is input, each joint portion 3 is controlled based on the rotation amount of each joint portion 3 output in the operation plan output process of FIG. 6, and the robot hand portion 2k is targeted. The flowchart of the control process to move to is omitted.
The flowchart shown in FIG. 6 is started by starting the robot control program.

In ST1 of FIG. 6, an image for inputting an initial position / posture and a target position / posture is displayed on the display 13. Then proceed to ST2.
In ST2, it is determined whether or not the initial position and the like are input by the keyboard 14 and the mouse 15 and the start of derivation of the motion plan is input. If yes (Y), proceed to ST3, and if no (N), repeat ST2.
In ST3, the inverse kinematics is solved from the initial state and the target position 21 to derive the posture after the movement. Then proceed to ST4.

In ST4, it is determined whether or not the posture after the operation interferes with the obstacle 20. If no (N), proceed to ST5, and if yes (Y), proceed to ST7.
In ST5, the trajectory between the initial state and the derived posture after the operation is derived by linear interpolation. Then, proceed to ST6.
In ST6, it is determined whether or not the derived trajectory interferes with the obstacle 20. If yes (Y), proceed to ST11, and if no (N), proceed to ST22.

In ST7, the posture after movement is derived by solving the prioritized inverse kinematics from the initial state and the target position 21. Then, proceed to ST8.
In ST8, it is determined whether or not the derived posture after the operation interferes with the obstacle 20. If no (N), the process proceeds to ST9, and if yes (Y), the process ends with an error.
In ST9, the trajectory between the initial state and the derived posture after the operation is derived by linear interpolation. Then, proceed to ST10.
In ST10, it is determined whether or not the derived trajectory interferes with the obstacle 20. If yes (Y), proceed to ST11, and if no (N), proceed to ST22.

In ST11, the first intermediate position 22 is set. Then, proceed to ST12.
In ST12, the inverse kinematics is solved from the initial state and the intermediate position (when the intermediate position increases, the intermediate positions are used) to derive the posture after the movement. Then, proceed to ST13.
In ST13, it is determined whether or not the posture after the operation interferes with the obstacle 20. If no (N), proceed to ST14, and if yes (Y), proceed to ST17.
In ST14, the trajectory between the initial state and the derived posture after the operation (if the intermediate position increases, the trajectory between the intermediate positions) is derived by linear interpolation. Then, proceed to ST15.
In ST15, it is determined whether or not the derived trajectory interferes with the obstacle 20. If yes (Y), proceed to ST16, and if no (N), proceed to ST22.
In ST16, set the next intermediate position (increase the intermediate position). Then, it returns to ST12.

In ST17, the reverse kinematics with priority is solved from the initial state and the intermediate position 22 (when the intermediate positions increase, the intermediate positions are connected to each other), and the posture after the movement is derived. Then, proceed to ST18.
In ST18, it is determined whether or not the derived posture after the operation interferes with the obstacle 20. If no (N), the process proceeds to ST19, and if yes (Y), the process ends with an error.
In ST19, the trajectory between the initial state and the derived posture after the operation (if the intermediate position increases, the trajectory between the intermediate positions) is derived by linear interpolation. Then, proceed to ST20.
In ST20, it is determined whether or not the derived trajectory interferes with the obstacle 20. If yes (Y), proceed to ST21, and if no (N), proceed to ST22.
In ST21, set the next intermediate position (increase the intermediate position). Then, it returns to ST17.
In ST22, the derivation result is output as an operation plan, and the operation plan output process is terminated.

(Action of Example 1)
In the robot arm system S of the first embodiment having the above configuration, the trajectory of the arm member 1 is derived by solving the numerical solution method of the inverse kinematics and the inverse kinematics with priority. When the derived orbit interferes with the obstacle 20, the intermediate position is set until the orbit that does not interfere with the obstacle 20 is derived, and the orbit is derived. Therefore, in the first embodiment, it is not necessary to use the joint space or the evaluation function when deriving the trajectory. Therefore, it is possible to reduce the calculation load of the motion path of the articulated robot as compared with the case of using the n-dimensional joint space (7-dimensional joint space in the example of Example 1) and the evaluation function.
In particular, in Example 1, even if the number of joints 3 increases, only one parameter in the calculation of inverse kinematics increases, and the calculation is performed as compared with the case where the dimension of the space increases as in the case of the joint space. The increase in load is suppressed. Further, in the first embodiment, even when the obstacle 20 moves (in the case of a dynamic obstacle), only the calculation of the numerical solution method of the inverse kinematics according to the position of the obstacle 20 after the movement is performed. The calculation load is greatly reduced as compared with the case of recalculating the n-dimensional joint space and the evaluation function.
Further, in the first embodiment, even if the trajectory that does not interfere with the obstacle 20 is not derived only by the first intermediate position 22, the second intermediate position, the third intermediate position, and so on are set in order. A non-interfering orbital search is performed.

(Change example)
Although the embodiments of the present invention have been described in detail above, the present invention is not limited to the above embodiments, and various modifications are made within the scope of the gist of the present invention described in the claims. It is possible. Examples of modifications (H01) to (H05) of the present invention are illustrated below.
(H01) In the above embodiment, the number of joints 3, the length and number of arms, the shape, and the like are not limited to the exemplified forms. Therefore, the number, length, shape, etc. can be arbitrarily changed according to the application, design, specifications, etc. of the arm member.
(H02) In the above embodiment, the case where the intermediate position is set to the position of the joint portion 3 is exemplified, but the present invention is not limited to this. For example, it can be set at an arbitrary position of the arm member 1, such as setting it at the intermediate portion of the arm portion 2 or setting it at a position moved in parallel by a certain distance from the arm portion 2 and the joint portion 3. ..

(H03) In the above embodiment, the type of the joint portion 3 is composed of only the rotary joint, but the type is not limited to the rotary joint. That is, it is also applicable when a linear motion joint is included.
(H04) In the above embodiment, the intermediate position is set based on the posture after the operation derived by the first posture deriving means C3, but the present invention is not limited to this. For example, it is also possible to set the position of the wrist joint portion 3g in the first intermediate posture as the next intermediate position with reference to the first intermediate posture derived with the target of the first intermediate position.
(H05) In the above embodiment, the case where the intermediate position is set in the order of after operation, before operation, after operation, and so on is illustrated, but the present invention is not limited to this. It is also possible to change the order before, after, and so on.

1 ... Arm member,
2k ... free end,
2 ... Arm part,
3 ... Joints,
3g ... One joint on the base end side of the free end,
11 ... Computer,
20 ... Obstacles,
21 ... Goal,
22 ... Intermediate position,
C ... Control unit,
C3 ... First posture derivation means,
C6 ... Second posture derivation means,
C7 ... Arm control means,
S ... Robot.

Claims (4)

An arm member having a plurality of arm portions and a joint portion arranged at a connecting portion between the two arm portions and capable of displacing the other arm portion with respect to one arm portion.
A first posture deriving means for deriving the post-motion posture of the arm member by solving the inverse kinematics for the position of the joint in the motion of moving the free end of the arm member to a predetermined target.
Based on the pre-motion position and post-motion position of each joint derived by the first posture derivation means, if an obstacle is interfered with during the motion, the candidate for which the free end passes during the motion. A second posture deriving means for deriving the intermediate posture of the arm member by solving the reverse kinematics with the intermediate position as the target position as the target position.
When the arm member interferes with an obstacle during the operation derived by the first posture deriving means, the arm member is free to pass through the intermediate posture derived by the second posture deriving means. Arm control means that controls each joint so that the end is moved to the target,
A robot characterized by being equipped with. In the posture of the arm member after the operation derived by the first posture deriving means, the position of the joint portion on the base end side of the free end is set as the intermediate position, and the intermediate posture is derived. Posture derivation means,
The robot according to claim 1, wherein the robot is provided with. When the intermediate posture is derived with the position of the joint portion on the base end side of the free end as the intermediate position in the posture of the arm member after the operation, an obstacle is formed while moving from the posture before the operation to the intermediate posture. In the case of interference with, in the posture of the arm member before movement, the intermediate position is targeted from the second intermediate posture in which the position of the joint portion on the base end side of the free end is the position of the free end. The second posture derivation means for solving reverse kinetics,
The robot according to claim 2, wherein the robot is provided with. Controls a robot having an arm member having a plurality of arm portions and a joint portion arranged at a connecting portion between the two arm portions and capable of displacing the other arm portion with respect to one arm portion. It ’s a robot control program.
Computer,
A first posture deriving means for deriving a post-motion posture of the arm member by solving inverse kinematics for the position of the joint in an motion of moving the free end of the arm member to a predetermined target.
Based on the pre-motion position and post-motion position of each joint derived by the first posture derivation means, if an obstacle is interfered with during the motion, the candidate for which the free end passes during the motion. A second posture deriving means for deriving the intermediate posture of the arm member by solving the inverse kinematics with the intermediate position as the target position as the target position.
When the arm member interferes with an obstacle during the operation derived by the first posture deriving means, the arm member is free to pass through the intermediate posture derived by the second posture deriving means. Arm control means that controls each joint to move the end to the target,
A robot-controlled program characterized by functioning as.

Images