JP2012056063A - Smooth motion path generating device and smooth motion path generating method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To eliminate unnecessary motions from a motion path and regenerate a smooth motion path.SOLUTION: A smooth motion path generating device includes: a motion path storing means; a path waypoint setting means; a path waypoint storing means; a path waypoint pair adopting means for adopting a pair of path waypoints including a path waypoint and another waypoint to be paired among waypoints; a midpoint calculating means for obtaining a midpoint between a path waypoint and another path waypoint by a calculation formula, (position data of a path waypoint+position data of another path waypoint)/2; and a midpoint interference checking means for determining whether the midpoint interferes with an obstacle. When the midpoint interferes with the obstacle, a current motion path is adopted (S7). When the midpoint does not interfere with the obstacle, the midpoint is adopted as a new path waypoint, and a smooth path generating step of selecting a new motion path from a path waypoint to another path waypoint through the new path waypoint (S6).

Description

  The present invention relates to a smooth path generation apparatus and a smooth path generation method for regenerating a smooth operation path by reducing extra operations from an operation path generated so as to avoid an obstacle.

In general, when working in a place where human access is difficult, such as a nuclear reactor, remote control is performed by incorporating a CCD camera, distance sensor, contact sensor, etc., inspection of the reactor, measurement of the shape, repair work, etc. In many cases, a master / slave system is used.
In particular, in a nuclear reactor, work using an articulated robot that can automatically control the operation path selection is performed. As a method for realizing such automatic control and path generation, an operation path using a configuration space is used. An operation route generation method using a probabilistic method such as a generation method, a maze search method, or an RRT (Rapidly-exploring Random Trees) method is disclosed (Patent Documents 1 to 3).

JP-A-4-235606 JP 2008-105132 A JP 2010-32326 A

  However, since the robot motion path generated by these motion path planning algorithms prioritizes the avoidance of obstacles, the motion path may include vibrations and extra motion. For this reason, when the robot is operated on the trajectory generated in this way, there is a problem that an excessive force is applied to the robot body and the target object because of rapid acceleration and deceleration.

  For example, a robot motion path that is generated at a relatively high speed by a stochastic method such as the RRT method among motion path planning algorithms can be obtained randomly because the probabilistic method can only obtain the robot configuration in pieces. The robot trajectory is obtained by taking a configuration point (target position in the middle of the path) advanced by a certain distance toward the selected configuration point into the path search tree, growing the search tree, and following the target position incorporated in the search tree. To get.

  For this reason, in particular, the trajectory obtained by the RRT method is obtained by connecting the target positions at regular intervals based on random numbers and becomes a fragmented bent trajectory with few linear components. In some cases, vibration and extra motion are included, and there is a problem that a smooth motion path cannot be obtained.

  The present invention has been made in view of such a background, and reproduces a smooth operation path by reducing an operation path that induces vibrations and extra actions from an operation path generated so as to avoid an obstacle. It is an object of the present invention to provide a smooth path generation device and a smooth path generation method.

  The invention according to claim 1 of the present invention has a control device including an arithmetic processing device and a storage device, and smoothly operates the multi-axis robot arm generated so as to avoid an obstacle from the initial position to the target position. And a smooth path generating device that regenerates a smooth path, wherein the control device stores an operation path storage unit that stores an operation path from the initial position to the target position, and a plurality of intermediate points in the operation path. A route halfway point setting means for setting a plurality of route halfway points, a route halfway point storage means for storing the plurality of route halfway points, and one or more axes of the multi-axis robot arm as a pair of the route halfway points. A route intermediate point pair adopting means for adopting a route intermediate point pair consisting of the route intermediate point of the other route and another route intermediate point, the one route intermediate point adopted by the route intermediate point pair adopting means and the other route An intermediate point calculating means for obtaining an intermediate point from the intermediate point from a calculation formula (position data of one route intermediate point + position data of another route intermediate point) / 2, and an intermediate point obtained by the intermediate point calculating means Intermediate point interference check means for determining whether or not to interfere with the obstacle, and when the intermediate point interference check means determines that the intermediate point interferes with the obstacle, the intermediate point is When a current route maintaining step of adopting the current operation route without adopting as a new route intermediate point is executed, and the intermediate point interference check means determines that the intermediate point does not interfere with the obstacle Adopts the intermediate point as a new route halfway point, changes the operation route from the one route halfway point to a new route that passes through the new route halfway point to the other route halfway point, and Reroute And executes a smooth path generating step of forming.

  The present invention adopts a path halfway point pair consisting of one path halfway point and another path halfway point with respect to any one or more axes of the multi-axis robot arm. By obtaining an intermediate point, and when it is determined that the intermediate point does not interfere with the obstacle, the intermediate point is changed to a new operation route that is a new route intermediate point, and a smooth route is regenerated, An operation path from one route halfway point to the other route halfway point can be made smooth.

  That is, when it is determined that the intermediate point does not interfere with the obstacle, the operation route is changed from the one route intermediate point to a new operation route that passes through the intermediate point to the other route intermediate point. If there is an inappropriate route that induces vibrations or extra movement, such as a detoured route or a bent route, between the midpoint of the one route and the midpoint of the other route In this case, the route is changed from a detoured route or a bent route to a more linear route passing through the intermediate point, and the length of the route is shortened to make the operation route smooth.

  In this way, the smooth path generation device according to the present invention provides an inappropriate motion path that induces vibrations and extra motion included in the motion path from the midpoint of the one path to the midpoint of the other path. By changing to a straight path and smoothing it, it is possible to avoid a sudden acceleration and deceleration in the operation of the multi-axis robot arm and to ensure a smooth operation of the robot arm.

  For this reason, the smooth path generating device according to the present invention is, for example, a multi-joint robot arm in a narrow work space in which shrouds and a plurality of pipes coexist in nuclear equipment and the like where safety is important. Even when an operation path is generated, a smooth operation path can be generated while reliably avoiding an interference region, and therefore, high utility value as a path generation tool is expected.

  The invention according to claim 2 is the smooth route generation device according to claim 1, wherein the route halfway point pair adoption means adopts a number of the route halfway point pairs successively in a daisy chain, and the intermediate point The computing means obtains a large number of intermediate points in each of the numerous path halfway point pairs, and the intermediate point interference check means determines whether or not the numerous intermediate points interfere with the obstacle, respectively. In the route generation step, when the intermediate point interference check means determines that the intermediate point of the multiple intermediate points interferes with the obstacle, the intermediate point is adopted as a new route intermediate point. Without performing a current path maintenance step that adopts an operation path composed of a pair of midpoints of the path as it is without the current path. If it is determined not to interfere, the respective intermediate points are adopted as new route intermediate points, and the operation route passes from the one route intermediate point to each of the intermediate points through the respective new intermediate points. Then, a smooth path generation step is performed in which the smooth path is regenerated by changing to a new operation path that travels to the other path halfway point related to each of the intermediate points.

  According to such a configuration, the route halfway point pair selection means adopts a large number of the route halfway point pairs in a continuous manner and obtains a large number of intermediate points, thereby efficiently and smoothly from the initial position to the target position. The route can be regenerated.

  The invention according to claim 3 is the smooth path generation apparatus according to claim 1 or 2, wherein the control apparatus performs the path halfway point selection means after the smooth path generation step by the path halfway point selection means. A step of adopting a shift path halfway point pair step of adopting a pair of shift path halfway points consisting of one shift path halfway point and at least one of the other route halfway points and the other shift route halfway point And a shift intermediate point that is an intermediate point between the one shift path intermediate point and the other shift path intermediate point selected by the shift path intermediate point pair selecting unit is calculated by the calculation formula ( (Shift position midpoint position data + position data of another shift path midpoint) / 2) is executed, and the midpoint interference check means performs the shift point calculation step. If it is determined that the intermediate point is interfered with the obstacle, a current route maintaining step of adopting the current operation route without adopting the shift intermediate point as a new route intermediate point is executed, When the intermediate point interference check means determines that the shift intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is determined from the one shift route intermediate point. A shift smooth path generation step is performed in which the smooth path is regenerated by changing to a new operation path that passes through the new path midpoint to the other shift path midpoint, and thus the smooth path generation step The current path maintaining step or the shift smooth path generating step is executed after the current path maintaining step or the shift smooth path from the smooth path generating step. Characterized by smooth the motion path sequentially repeated production steps.

According to such a configuration, one shift path midpoint and at least one of the one path midpoint and the other path midpoint are different, and a pair of shift path midpoint pairs including another shift path midpoint is adopted. Thus, by determining the intermediate point of the shift path intermediate point pair, the intermediate point is determined while shifting the path intermediate point to be adopted as the shift path intermediate point pair from the path intermediate point pair.
When it is determined that the intermediate point does not interfere with the obstacle, the one shift is performed by changing the intermediate point to a new operation route having a new route halfway and regenerating a smooth route. The operation path from the midway point of the route to the midway point of the other shift route can be made smooth.

  The invention according to claim 4 is a smooth path generation method for regenerating a smooth path by smoothing an operation path of a multi-axis robot arm generated so as to avoid an obstacle from an initial position to a target position. A route halfway point setting in which a plurality of route halfway points in the operation route are set by a route halfway point setting means constituting the arithmetic processing unit using a smooth route generation device having a control device comprising a processing device and a storage device. One of the path halfway points and another path for any one or more axes of the multi-axis robot arm by the step and path halfway point adopting means constituting the arithmetic processing unit; A route halfway point pair adopting step that adopts a route halfway point pair consisting of halfway points, and a halfway point pair adopting step that constitutes the arithmetic processing unit. Intermediate point calculation for obtaining an intermediate point between the one route intermediate point and the other route intermediate point adopted in step 2 from a calculation formula (position data of one route intermediate point + position data of another route intermediate point) / 2. And an intermediate point interference check step for determining whether or not the intermediate point obtained in the intermediate point calculation step interferes with the obstacle by the intermediate point interference check means constituting the arithmetic processing unit. If the intermediate point interference check step determines that the intermediate point interferes with the obstacle, the current route maintaining step of adopting the current operation route without adopting the intermediate point as a new route intermediate point When the intermediate point interference check step determines that the intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is set as the previous route. And executes a smooth path generating step of regenerating the smooth path is changed to the new operation route proceeds from a path midway point to the other path midway point through the new path midway point.

  According to such a configuration, for any one or more axes of the multi-axis robot arm, a pair of route intermediate points consisting of one route intermediate point and another route intermediate point are adopted, and the route intermediate point is selected. A smooth route that obtains a midpoint of a pair and, when it is determined that the midpoint does not interfere with the obstacle, changes the midpoint to a new operation route with a new route halfway and regenerates a smooth route By executing the generating step, an operation path from the one way intermediate point to the other route intermediate point can be made smooth.

  According to the present invention, a smooth path generating device and a smooth path generating method for regenerating a smooth motion path by reducing motion paths that induce vibrations and extra motion from motion paths generated so as to avoid obstacles. Can be provided. Therefore, since the present invention can be applied smoothly even to an articulated robot having a high degree of freedom, it is particularly suitable when it is desired to perform a complex operation in a narrow work space in the nuclear power field or the like.

It is a perspective view which shows 6 axis | shaft structure in a robot arm. It is a figure which shows the relationship between the attitude | position of a robot arm in a biaxial robot arm, and an obstruction, (a) shows the relationship in real space, (b) shows the relationship in configuration space. It is a block diagram which shows the structure of the smooth path | route production | generation apparatus which concerns on embodiment of this invention. It is a figure for demonstrating operation | movement of the smooth path | route production | generation apparatus which concerns on embodiment of this invention, (a) is an operation | movement path | route before smoothing, (b) is from (c, 1) to (e, 4). (C) shows a current route maintenance step for the routes from (e, 4) to (h, 7). It is a figure for demonstrating operation | movement of the smooth path | route production | generation apparatus which concerns on embodiment of this invention, (a) is the smooth path | route production | generation step for the path | route from (h, 7) to (i, 9), ( b) shows the shift smooth path generation step, and (c) shows the first smooth path regenerated by the smooth path generation step and the shift smooth path generation step. It is a flowchart for demonstrating operation | movement of the route generation apparatus which concerns on embodiment of this invention. It is a figure for demonstrating operation | movement of the 1st Example in the smooth path | route production | generation apparatus which concerns on this invention. It is a figure for demonstrating operation | movement of the 2nd Example in the smooth path | route production | generation apparatus which concerns on this invention. It is a perspective view which shows the image of operation | movement from the initial position of a robot arm to a target position, (a) is an initial position, (b) is a target position, (c) shows the image of operation | movement from an initial position to a target position. .

  A smooth path generating apparatus 1 (see FIG. 3) according to an embodiment of the present invention will be described in detail with reference to the drawings as appropriate. The smooth path generation device 1 is an action path (an action path shown in FIG. 4A) generated in advance so as to avoid an obstacle in a robot arm 100 (see FIG. 1) having six axes that is a multi-axis robot arm. An example will be described in which the smooth operation path (the operation path shown in FIG. 4B (a)) is regenerated.

  4A to 4B, and FIGS. 6 and 7, a method for smoothing the operation path using the configuration space (C space) will be described. However, the operation path generated on the C space is described as an example. The present invention is not limited, and application in real space is also possible.

  In addition, the motion path (the motion path shown in FIG. 4A) generated in advance in the robot arm 100 (see FIG. 1) will be described as an example generated by the motion path generation method using the configuration space. However, it may be generated using a probabilistic method such as a maze search method or an RRT method, and is not limited to the operation route generation method, and thus detailed description thereof is omitted.

The configuration space (C space) is a virtual space having the same number of dimensions as the number of degrees of freedom of the manipulator (robot arm). On this C space, the configuration (posture) of the robot arm is the robot arm configuration. When the parameter representing the degree of freedom is a coordinate axis value, it can be represented by a point (configuration point).
For example, for all configurations of the robot arm, the presence or absence of interference with the obstacle is investigated, and a space (free space) in which the robot arm does not interfere with the obstacle is constructed on the C space, and the movement path of the robot arm in that space Is generated.

As shown in FIG. 1, the robot arm 100 includes six axes J1, J2, J3, J4, J5, and J6 including six joints (six degrees of freedom).
Assuming that a parameter representing the degree of freedom of the robot arm is a joint angle and the joint angles of the six axes J1, J2, J3, J4, J5, and J6 of the robot arm 100 are θ1, θ2, θ3, θ4, θ5, and θ6, The configuration (posture) of the robot arm 100 can be represented by θ1, θ2, θ3, θ4, θ5, and θ6.
In general, the robot arm 100 is not particularly limited as long as it is widely considered as a moving body in which the robot arm itself moves.

  As described above, when the parameters representing the degree of freedom of the robot arm 100 (for example, the joint angle and the expansion / contraction length) are coordinate axis values, the configuration points of the robot arm 100 can be expressed as points on the C space. .

  For example, as shown in FIG. 2, for simplification, for example, a two-axis (θx, θy) robot arm 100 ′ (see FIG. 2A) in which joint angles can be expressed by θx and θy in real space. If the horizontal axis represents θx and the vertical axis represents θy, the configuration (posture) of the robot arm 100 ′ can be represented as configuration points (θx, θy) on the C space.

On the other hand, a configuration point where the robot arm 100 ′ interferes (collises) with an obstacle Ob (Obscale) in the work space is defined as an interference point. Interference points are rarely present alone, and can generally be represented on the C space as an interference area CA (Collision Area) which is a set of continuous points (see FIG. 2B).
In this way, as shown in FIG. 2B, the interference between the robot arm and the obstacle in the operation path is checked by representing the configuration of the robot arm 100 ′ and the interference area CA on the C space. be able to.

  In FIG. 2, the two axes (θx, θy) have been described. However, the six axes (θ1, θ2, θ3, θ4, θ5, θ6) like the robot arm 100 (FIG. 1) can be used in the same way even if the calculation amount is different. It is also possible to develop and process. For this reason, as shown in FIG. 4A and FIG. 4B, it is easier to understand if only two axes are shown in a plan view, and therefore it is simplified for convenience.

As shown in FIGS. 3, 4A, and 4B, the smooth path generation device 1 is generated so as to avoid an obstacle from an initial configuration point qi that is an initial position to a target configuration point qg that is a target position. This is a device that smoothes the motion path of the robot arm 100 (referred to as “motion path before smooth processing”, see FIG. 4A (a)) and regenerates the smooth path (see FIG. 4B (c)) (FIG. 3, FIG. 8).
Here, as shown in FIG. 4A and FIG. 4B, the operation plan using the C space is an initial configuration point qi (initial) and a target configuration point qg (goal) avoiding the interference area CA on the C space. This is a method of generating a route connecting the, and searching for an operation route by replacing it with a route in the real space.

As shown in FIG. 3, the smooth path generating device 1 includes an operation panel 2 on which an operator (not shown) inputs data, an arithmetic processing device 3 including a CPU (not shown), and a memory for storing various data. And a device 4.
The operation panel 2 is provided with a display 21, and the robot arm 100 and the work space can be displayed in 3D (three-dimensional) graphics so that an operation path generated by an operator (not shown) can be viewed.

  As shown in FIG. 3, the arithmetic processing device 3 includes a route halfway point setting unit 31, a route halfway point pair selection unit 32, a middle point computation unit 33, and a middle point interference check unit 34.

  The storage device 4 includes an initial position storage unit 41, a target position storage unit 42, a robot shape storage unit 43, a work space storage unit 44, an operation path storage unit 45, and a path halfway point storage unit 46. ing.

Hereinafter, specific means constituting the smooth path generation device 1 and its operation and effects will be described along the order of operations of the smooth path generation device 1 mainly with reference to FIGS. 3 and 4A to 4B.
In FIG. 4A to FIG. 4B to be referred to, the vertical grid lines of the figures displayed as thin lines are indicated by a to k, the horizontal grid lines are indicated by 1 to 10, and the intersections of these grid lines are indicated. It is called a grid point and is displayed as (a, k) to (l, 10). However, the display of the grid line spacing and the like is schematic, and the aspect ratio and the like are simplified.

The lattice point is a concept for finely dividing the C space and handling it as a set of discretized configuration points. In this way, the C space is not recognized as a continuous space, but is discretized (quantized) with reference to the lattice point, and the new configuration point is adopted so as to pass through the quantized lattice point. By generating the route, the arithmetic processing on the C space can be simplified.
For example, the joint angles θ1 to θ3 of the base portion of the robot arm 100 (see FIG. 1) are set in increments of 1 and the joint angles θ4 to θ6 of the hand portion are set in increments of 5 degrees to set lattice points in the C space. A discretized C space can be constructed with reference to the grid points.

As shown in FIG. 3 and FIG. 4A (a), the initial position storage means 41 uses the initial posture of the robot arm 100 that is the initial configuration point qi (see FIG. 8A in real space) as the joint angle of each joint. Or the like, and an operator (not shown) inputs from the operation panel 2, or the arithmetic processing unit 3 detects the initial position with a detector such as an angle sensor and sets it as the initial configuration point qi. can do.
For example, the initial configuration point qi represents the joint angle as a parameter such that qi = (θ1, θ2, θ3, θ4, θ5, θ6) = (− 170, 90, 0, 25, −85, 75). Can do.

The target position storage means 42 is a means for storing the target posture of the robot arm 100 (see FIG. 8B in the real space) that is the target configuration point qg based on the joint angle of each joint. A target position (not shown) can be input, or the target position can be detected by the arithmetic processing unit 3 with a detector such as an angle sensor and set as the target configuration point qg.
For example, the target configuration point qg represents the joint angle as a parameter such that qg = (θ1, θ2, θ3, θ4, θ5, θ6) = (− 71, −75, 38, 80, −10, 25). be able to.

The robot shape storage means 43 stores the shape data of the robot arm 100 created from a CAD file or the like, but in order to simplify the data processing, for example, the shape data of the robot arm 100 is a minimum configuration such as a polygon called a polygon. Expressed as a set of units (for example, triangles), this polygon set can be expressed and stored as a shape on the C space.
With this configuration, the shape data of the robot arm 100 in the C space is simplified (discrete) by representing the shape data of the robot arm 100 in the real space as a set of polygons such as triangles and recognizing the shape as the shape on the C space. Calculation processing can be shortened.

The work space storage means 44 stores the shape data of the work space in which the obstacle Ob (see FIG. 8) is arranged. In order to simplify the data processing, for example, the shape data of the obstacle Ob is converted to a polygon (for example, a triangle). The set of triangles and the like can be expressed and stored as a shape on the C space.
With this configuration, it is possible to simplify (discretize) the shape data of the work space on the C space and shorten the arithmetic processing.

  The operation path storage means 45 is a storage device that stores an operation path from an initial configuration point qi that is an initial position to a target configuration point qg that is a target position, as shown in FIGS. 3 and 4A (a). Specifically, the operation path storage unit 45 stores configuration points constituting the operation path, and is updated each time a new operation path is generated.

  For example, in the case of the operation path before being smoothed in the state shown in FIG. 4A (a), the operation path storage means 45 changes from the initial configuration point qi (c, 1) to the configuration point (e, 3). , (E, 4), (h, 4), (h, 7), (i, 8) to store the configuration points constituting the target configuration point qg (i, 9) The operation path is stored.

  Thus, the operation path is from the initial configuration point qi (c, 1) to the configuration point (e, 3) and from the configuration point (e, 3) to the configuration point (e, 4). And a bending point that is a configuration point (e, 4) that connects straight paths having different directions.

  The route halfway point setting means 31 is a computing device that sets a plurality of route halfway points in the operation route, as shown in FIGS. 3 and 4A (a). Here, for convenience of explanation, the name “route halfway point” is used for simplification, but this is not intended to exclude the initial position qi and the target position qg.

  For example, in the operation route shown in FIG. 4A (a), the route halfway point setting means 31 sets the initial configuration point qi (c, 1) and the target configuration point qg (i, 9) as a plurality of route halfway points. In addition, configuration points (e, 3), (e, 4), (h, 4), (h, 7), (i, 8) that are bending points in the operation path are adopted and set. .

  The route halfway storage means 46 is a storage device that stores a plurality of route halfway points set by the route halfway point setting means 31. Specifically, in FIG. 4A (a), the initial configuration point qi (c , 1) and the target configuration point qg (i, 9), the bending point (e, 3), (e, 4), (h, 4), (h, 7), (i, 8) is stored.

  As shown in FIGS. 4A and 4C, the route halfway point selection means 32 is set by the route halfway point setting means 31 for any one or more axes of the robot arm 100 (see FIG. 1). Among a plurality of route intermediate points, a pair of route intermediate points and another route intermediate point pair q1, q2 (FIG. 4A (b)) and route intermediate point pairs q2, q3 (FIG. 4A ( c)), and a midway point pair q3, q4 (FIG. 4B (a)).

  Here, the way of selecting the route intermediate point pair by the route intermediate point pair selecting unit 32 is appropriately set according to the form and purpose of the operation route. For example, the route intermediate point pair selecting unit 32 may select the route intermediate point pair. Is selected continuously in three places, so that one of the bent points on both sides is one route halfway point, and the other Can be the other waypoint. When the route halfway point is set in this manner, a bending point qt (qt1 in FIG. 4A (b), qt2 in FIG. 4A (c), FIG. 4B (b) between one route halfway point and another route halfway point. Since qt3) of a) is sandwiched between a pair of intermediate points on the route, this bending point qt can be the target of smooth processing.

  In other words, in FIG. 4A (a), three route intermediate points of the initial configuration point qi (c, 1) and the bending points (e, 3), (e, 4) and the bending point (e, 4). ) Including continuous bending points (h, 4), (h, 7), and three continuous path points including bending points (h, 7). In addition, three sets of route halfway points that are connected in a sequence of three points each consisting of three route halfway points of the target configuration point qg (i, 9) are selected.

  Then, the initial configuration points qi (c, 1) and the bending points (e, 4), which are halfway points on both sides of each set, constitute the route halfway point pair q1, q2 in FIG. 4A (b). The inflection point (e, 4) and the inflection point (h, 7) constitute the path intermediate point pair q2 and q3 in FIG. 4A (c), and the inflection point (h, 7) and the target configuration point qg (i , 9) constitute the path halfway point pairs q3, q4 in FIG. 4B (a), respectively.

  At this time, there is a bending point qt1 (FIG. 4A (b)) between the route halfway point pair q1, q2, and between the route halfway point pair q2, q3, the bending point qt2. (FIG. 4A (c)) exists, and a bending point qt3 (FIG. 4B (a)) exists between the pair of intermediate points q3 and q4.

The intermediate point calculation means 33 calculates an intermediate point between one route intermediate point and another route intermediate point constituting the route intermediate point adopted by the route intermediate point pair selection means 32 by a calculation formula (one route intermediate point This is a calculation device obtained from (position data + position data of other route halfway point) / 2.
Specifically, as shown in FIG. 4A (b), when the initial configuration point qi (c, 1) and the bending point (e, 4) are adopted as the route intermediate point pair q1, q2, The intermediate point qm1 of
q1 = (θ11, θ12, θ13, θ14, θ15, θ16)
q2 = (θ21, θ22, θ23, θ24, θ25, θ26)
And when obtaining the midpoint qm1 for the six axes,
qm1 = (θm11, θm12, θm13, θm14, θm15, θm16)
Then,
θm11 = (θ11 + θ21) / 2
θm12 = (θ12 + θ22) / 2,
θm13 = (θ13 + θ23) / 2
θm14 = (θ14 + θ24) / 2
θm15 = (θ15 + θ25) / 2
θm16 = (θ16 + θ26) / 2
It is requested from. The same applies to the intermediate points qm2 and qm3.

  In the present embodiment, the intermediate points are obtained for all six axes of the robot arm 100 (see FIG. 1), but the present invention is not limited to this, and one or more of the six axes are included. An intermediate point may be obtained. Which of the six axes is the target is determined as appropriate according to, for example, the form and purpose of the operation path. For example, the one having a large dispersion width (grid spacing) can be selected. The arrangement of the obstacle Ob (see FIG. 8), the movable range of each axis, and the like can be taken into consideration.

As shown in FIGS. 3 and 4A, the intermediate point interference check unit 34 is an arithmetic unit that determines whether the intermediate point obtained by the intermediate point calculation unit 33 interferes with the obstacle Ob.
That is, the intermediate point interference check unit 34 is stored in the robot shape storage unit 43 from the joint angles (θ1, θ2, θ3, θ4, θ5, θ6) of the robot arm 100 (see FIG. 1) at the target intermediate point. Based on the shape data of the robot arm 100, the shape (posture) of the robot arm 100 at the target intermediate point is obtained, and the shape data of the obstacle Ob (interference area CA) stored in the work space storage means 44 Check if it interferes with.

  For example, when the CAD data of the robot arm 100 and the obstacle Ob is represented as a set of polygons (for example, polygons such as triangles), the intermediate point interference checking unit 34 is the robot at the configuration point adjacent to the target current position. It is determined by determining whether the polygon set of the arm 100 and the polygon set of the obstacle Ob are in geometric contact.

  Specifically, the intermediate point interference check unit 34 includes one polygon (polygon) constituting the shape of the robot arm 100 at the target intermediate point qm (qm1, qm2,...) On the C space. Whether contact with one polygon (polygon) constituting the shape of the obstacle Ob is determined, and a determination is similarly made for each set of all polygons (polygons), so that the robot arm 100 and the obstacle Ob interfere with each other. It can be determined and displayed on the display 21 (FIG. 3).

In this embodiment, it is determined whether or not a set of polygons is in contact with each other on the C space. However, the present invention is not limited to this, and according to the degree of freedom (N-dimensional) of the robot arm 100, The interference between the robot arm 100 and the interference area CA may be checked assuming this N-dimensional shape of the interference area CA.
Further, instead of polygons (polygonal shapes), the robot arm 100 and the obstacle Ob may be represented as a set of points, lines, or surfaces, and such a point-to-surface interference check or a line-to-surface interference check may be used. .
Furthermore, the present invention is not limited to the interference check on the C space, and an interference check using points, lines, or polygons in the CAD space or the real space can also be adopted.

Next, the operation of the smooth path generation device 1 according to the embodiment of the present invention will be described with reference to FIGS. 4A to 4B and the flowchart of FIG. 5 as appropriate.
[Store data]
An operator (not shown) refers to an operation path (see FIG. 4A (a)) of the robot arm 100 (FIG. 1) generated in advance so as to avoid the obstacle Ob from the initial configuration point qi to the target configuration point qg. ), The initial position data, the target position data, the shape data of the robot arm 100, and the shape data of the obstacle Ob (FIGS. 2A and 8) are input from the operation panel 2, the smooth path generation device 1 As shown in FIG. 5, in the C space, the initial position data of the robot arm 100 is stored in the initial position storage means 41 as the initial configuration point qi, and the target position data is stored in the target position storage means 42 as the target configuration point qg. The motion path (FIG. 4A (a)) is stored in the motion path storage means 45, and the robot arm 100 (FIG. Storing the shape data of the reference) to the robot shape storage section 43, the obstacle Ob (FIG. 2 (a), stores the shape data of FIG. 8) in the work space memory means 44 (see step S1 in FIG. 5).

[Set route halfway point]
As shown in FIG. 4A (a), the smooth path generation device 1 uses the path halfway point setting means 31 (FIG. 3) as an initial configuration point qi (c, 1), target configuration as a plurality of path halfway points. A point qg (i, 9) and configuration points (e, 3), (e, 4), (h, 4), (h, 7), (i, 8) which are bending points in the operation path are represented. Setting is made (step S2 in FIG. 5).

[Adoption of point pairs along the route]
As shown in FIG. 4A (b), the smooth route generation device 1 uses a route halfway point pair selection means 32 (FIG. 3) to form a pair of route halfway points q1 (c, 1), q2 (e, 4). As shown in FIG. 4A (c), a pair of route intermediate points q2 (e, 4) and q3 (h, 7) to be paired is adopted. Further, as shown in FIG. 4B (a), a pair of route halfway points q3 (e, 4) and q4 (h, 7) to be paired are adopted (see step S3 in FIG. 5).

[Calculation of midpoint]
As shown in FIG. 4A (b), the smooth route generation device 1 uses the intermediate point calculation means 33 (FIG. 3) to q1 (c, 1) which is one route intermediate point and q2 which is another route intermediate point. An intermediate point qm1 (d, 2-3) with (e, 4) is obtained from the calculation formula (position data of q1 + position data of q2) / 2 (see step S4 in FIG. 5). Here, for the sake of convenience of description in order to specify the position of the intermediate point on the drawing, the position between the horizontal grid line 2 and the grid line 3 is displayed as “2-3”.

  As shown in FIG. 4A (c), the smooth route generation device 1 uses the intermediate point calculation means 33 to make q2 (e, 4) which is one route intermediate point and q3 (h, 7) which is another route intermediate point. ) And the intermediate point qm2 (f−g, 5-6) is obtained from the calculation formula (position data of q2 + position data of q3) / 2. Here, for the convenience of explanation in order to specify the position of the intermediate point on the drawing, the position between the horizontal grid line f and the grid line g is displayed as “f−g”.

  Similarly, as shown in FIG. 4B (a), the smooth route generation device 1 uses the intermediate point calculation means 33 to generate q3 (h, 7) as one route intermediate point and q4 ( The intermediate point qm3 (hi, 8-9) with i, 9) is obtained from the calculation formula (position data of q3 + position data of q4) / 2.

[Interference check at midpoint]
As shown in FIGS. 4A (b) and 4 (c) and FIG. 4B (a), the smooth path generation device 1 uses the intermediate point calculation means 33 by the intermediate point interference check means 34 (FIG. 3). Whether qm1 (d, 2-3), intermediate point qm2 (f-g, 5-6), and intermediate point qm3 (hi, 8-9) interfere with obstacle Ob (interference area CA). Judgment is made (see step S5 in FIG. 5).

That is, since the intermediate point qm1 (d, 2-3) shown in FIG. 4A (b) is away from the interference point (d, 5) with the obstacle Ob, the intermediate point interference check means 34 is connected to the intermediate point qm1. It can be determined that (d, 2-3) is not included in the interference area CA and does not interfere with the obstacle Ob.
On the other hand, since the intermediate point qm2 (f−g, 5-6) shown in FIG. 4C is included in the interference area CA, the intermediate point interference check unit 34 determines the intermediate point qm2 (f−g, 5 It is determined that −6) interferes with the obstacle Ob.
Then, for the intermediate point qm3 (hi, 8-9) shown in FIG. 4B (a), the intermediate point interference checking means performs the intermediate point in the same manner as the intermediate point qm1 (d, 2-3). It is determined that qm3 (hi, 8-9) does not interfere with the obstacle Ob.

  Thus, after determining whether or not the intermediate point qm (qm1, qm2, qm3) interferes with the obstacle Ob (interference area CA), the smooth path generation device 1 uses the intermediate point interference check unit 34 to If it is determined that the intermediate point qm (qm2) interferes with the obstacle Ob, the current route maintaining step of adopting the current operation route without adopting the intermediate point qm (qm2) as the midpoint of the new route is performed. If it is determined that the intermediate point qm (qm1, qm3) does not interfere with the intermediate point qm (qm1, qm3), the intermediate point qm (qm1, qm3) is selected as the new route halfway point. Then, the new operation of moving the operation route from one route intermediate point (q1, q3) constituting the route intermediate point pair to the other route intermediate point (q2, q4) through the new route intermediate point (qm1, qm3). Change to route, smooth route Run smoothly path generating step for regenerating (FIG 4A (b), reference S6 in FIG. 5).

[Smooth path generation step]
Specifically, in FIG. 4A (b), since the intermediate point qm1 (d, 2-3) does not interfere with the obstacle Ob (interference area CA), the smooth path generation device 1 executes the smooth path generation step. .
That is, in the smooth route generation step, the intermediate point qm1 (d, 2-3) is adopted as a new route halfway point, and the configuration point q1 (c, 1) is passed through the configuration point qt1 (e, 3). The current operation path (indicated by a thick solid line) that travels to the point q2 (e, 4) passes from the configuration point q1 (c, 1) through the intermediate point qm1 (d, 2-3) that is the midpoint of the new path. The smooth route is regenerated by changing to a new operation route (indicated by a broken line) that goes to the configuration point q2 (e, 4) (see S6 in FIG. 5).

[Current route maintenance step]
On the other hand, in FIG. 4A (c), since the intermediate point qm2 (f-g, 5-6) interferes with the obstacle Ob (interference area CA), the smooth path generation device 1 executes the current path maintaining step. .
Specifically, the intermediate point qm2 (f-g, 5-6) is not adopted as the new route halfway point, and passes from the configuration point q2 (e, 4) to the route halfway point qt2 (h, 4). Adopt and maintain the current operating path that goes to configuration point q3 (h, 7).

  In FIG. 4B (a), since the intermediate point qm3 (hi, 8) does not interfere with the obstacle Ob (interference area CA), the smooth path generation device 1 executes the smooth path generation step, and the configuration point q3 A smooth path is reproduced by changing to a new operation path from (h, 7) to the configuration point q4 (i, 9), which is the target configuration point qg, through the new path halfway point qm3 (hi, 8). To do.

  In this way, as shown in FIG. 4B (b), the smooth path generation device 1 smoothes the motion path (see FIG. 4A (a)) of the robot arm 100 that has been generated in advance, and the initial configuration point qi. A smooth path that travels from (c, 1) to the target configuration point qg (i, 9) through intermediate points q2 (e, 4), qt2 (h, 4), q3 (h, 7) is regenerated. To do.

[Shift path halfway point pair selection step]
Next, the shift path midpoint pair selection step will be described with reference to FIGS. 3, 4B (c), and 5. FIG. The shift route halfway point adopting step includes, after the smooth route generating step, one route halfway point in which at least one of one route halfway point and another route halfway point is different by the route halfway point pair adopting unit 32, and A pair of shift path midpoint pairs consisting of other shift path midpoints is adopted (S8 in FIG. 5).

  That is, in the smooth route generation step, the smooth route generation device 1 uses the route intermediate point pair selection means 32 (see FIG. 3) to form a pair of route intermediate point pairs q1 (c, 1) in the operation route of FIG. 4A (b). ), Q2 (e, 4) are adopted, but qt2 (h, 4) at a position shifted from q2 (e, 4) in the operation route of FIG. Then, q1 (c, 1) and qt2 (h, 4) are adopted as the shift path halfway point pair.

  Similarly, in the smooth route generation step, the smooth route generation device 1 uses the route halfway point selection means 32 (see FIG. 3) to form a pair of route halfway points q3 (h) in the operation route of FIG. 4B (a). 7), q4 (i, 9) are adopted, but qt2 (h, 4) at a position shifted from q3 (h, 7) in the operation route of FIG. ) Is adopted, and qt2 (h, 4) and q4 (i, 9) are adopted as shift path midpoint pairs.

  In this embodiment, in the shift path midpoint pair selection step, a plurality of shift midpoint pairs are continuously selected. However, the present invention is not limited to this. First, one shift midpoint pair is selected. Then, the shift intermediate point interference check step, which will be described later, is executed, and other shift midpoint pairs are repeatedly adopted, so that the shift midpoint pair selection step and the shift intermediate point interference check step are performed after the smooth path generation step. It is also possible to generate a smooth shift path while repeatedly performing it sequentially.

  For example, three points q1 (c, 1), qt1 (e, 3), and q2 (e, 4) are adopted as route intermediate points, and qt1 (e, 3) to qm1 (d, After changing to 2-3) and smoothing (see FIG. 4A (b)), three route halfway points of qm1 (d, 2-3), q2 (e, 4), and qt2 (h, 4) Among them, qm1 (d, 2-3) and qt2 (h, 4) are adopted as shift path halfway point pairs in the shift path halfway point adoption step (see FIG. 4B (b)), and the shift middle described later is performed. In the middle of the shift path from the initial configuration point qi (c, 1) to the target configuration point qg (i, 9), such as generating a smooth shift path by checking for interference at the intermediate point in the point interference check step. Adopt while shifting the point pair It can be successively repeatedly perform the interference checking step of shifting the middle point pairs adoption steps and shift midpoint.

[Interference check step at shift midpoint]
Subsequently, the smooth route generation device 1 uses the intermediate point calculation means 33 (see FIG. 3) to determine q1 (c, 1) as one route intermediate point and qt2 (h, 4) as another route intermediate point. The shift intermediate point qm4 (ef, 2-3), which is the intermediate point of, is obtained from the calculation formula (position data of q1 + position data of qt2) / 2, and as shown in FIG. 4B (b) It is determined by the interference check means 34 (see FIG. 3) whether or not the shift intermediate point qm4 (ef, 2-3) obtained by the intermediate point calculation means 33 interferes with the obstacle Ob (S9 in FIG. 5). .

  Similarly, the smooth route generation apparatus 1 uses the intermediate point calculation means 33 (see FIG. 3) to qt2 (h, 4) that is one route intermediate point and q4 (i, 9) that is another route intermediate point. The shift intermediate point qm5 (hi, 6-7), which is the intermediate point between the two, is obtained from the calculation formula (position data of qt2 + position data of q4) / 2, and as shown in FIG. 4B (b) The point interference check means 34 (see FIG. 3) determines whether or not the shift intermediate point qm5 (hi, 6-7) obtained by the intermediate point calculation means 33 interferes with the obstacle Ob (S9 in FIG. 5). ).

  In this way, after determining whether or not the intermediate points qm4 and qm5 interfere with the obstacle Ob (interference area CA), if it is determined that the shifted intermediate points qm4 and qm5 interfere with the obstacle Ob, A current route maintenance step is adopted in which the current operation route is adopted without adopting the shift intermediate points qm4 and qm5 as new route intermediate points (S11 in FIG. 5), and it is determined that there is no interference with the obstacle Ob. In this case, the intermediate points qm4 and qm5 are adopted as the new route intermediate points, and the operation route is changed from one shift route intermediate point (q1, qt2) through the new route intermediate point (qm4, qm5) to the other shift route. A shift smooth path generation step is performed in which the smooth path is regenerated by changing to a new motion path that goes to the midpoint (qt2, q4) (S10 in FIG. 5).

  Here, since the shift intermediate points qm4 and qm5 do not interfere with the obstacle Ob (interference area CA) as shown in FIG. 4B (b), the smooth path generation device 1 is configured with the configuration points q1 (c, 1 ) To configuration point qt2 (e, 4) through configuration point q2 (e, 4) and to configuration point q4 (i, 9) through configuration point q3 (h, 7) From the configuration point q1 (c, 1) through the intermediate point qm4 (ef, 2-3), which is an intermediate point of the new route, and the new route from qt2 (h, 4). Change to a new motion path (indicated by a broken line) that goes to the target configuration point q4 (i, 9) through the intermediate point qm5 (hi, 6-7), which is an intermediate point, and a shift circle Regenerating pathway (FIG. 4B (c), reference S10 in FIG. 5).

  In this way, the smooth route generation apparatus 1 executes the current route maintenance step or the shift smooth route generation step after the smooth route generation step, and sequentially repeats the current route maintenance step or the shift smooth route generation step from the smooth route generation step. By smoothing the operation path, smoother operation paths can be sequentially generated from the smooth process in the smooth path generation step.

  For this reason, the smooth path generation device 1 includes an inappropriate path that induces vibrations and extra movements such as a detour path and a bent path in the operation path from the initial position qi to the target position qg. In this case, the path is changed from a detoured path or a bent path to a linear path successively, the path length is gradually shortened to make the motion path smooth, and a sudden change in acceleration in the motion of the robot arm 100 is caused. Can be suppressed.

Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and can be implemented with appropriate modifications.
For example, in the present embodiment, the bending point on the C space is selected by the route halfway point setting means 31, but the present invention is not limited to this. Arbitrarily divided points and the like can be set as appropriate according to the form and use of the operation path.

  Specifically, the first embodiment of the route halfway point setting means 31 according to the present invention will be described below with reference to FIG. FIG. 6 is a diagram for explaining another example of smoothing the operation path of FIG. 4B (c) according to the embodiment of the present invention, and FIG. 6 (a) is a diagram of the bending point qt2 (h, 4). Smooth processing, (b) shows smooth processing of the dividing point qm4 (ef, 2-3) and dividing point qm5 (hi, 6-7), and (c) shows the generated smooth path.

  As shown in FIG. 6 (a), the route halfway point setting means 31 (see FIG. 3) according to the first embodiment of the present invention uses qi (e, 1), qg (i, 9) as route halfway points. ), The bending point qt2 (h, 4), the dividing point qm4 (ef, 2-3) that bisects the straight path from qi (e, 1) to qt2 (h, 4), and qt2. A dividing point qm5 (hi, 6-7) that bisects a straight line route from (h, 4) to qg (i, 9) is set as a route halfway point.

Then, the route halfway point pair selection means 32 targets the bending point qt2 (h, 4) and divides the bending point qt2 (h, 4) so as to sandwich the dividing point qm4 (ef, 2-3). ) And the division point qm5 (hi, 6-7), the intermediate point calculation means 33 obtains the intermediate point qm6 (g, 4-5), and checks the interference with the obstacle Ob before the new route. Can be adopted as an intermediate point.
Thus, as shown by the thick solid line in FIG. 6B, the regenerated smooth path is divided from qi (e, 1) to the dividing point qm4 (ef, 2-3) and the intermediate point qm6. (G, 4-5), a smooth path that passes through the dividing point qm5 (hi, 6-7) to qg (i, 9) is formed.

  Here, as shown in FIG. 6 (b), the path halfway point pair selection means 32 continuously selects the bending points three by three so that the path middle point pairs are connected in a daisy chain, In the selected three bending points, one of the bending points on both sides is set as one route halfway point and the other is set as the other route halfway point, similarly to the case of FIG. 4B (a). , Qi (e, 1) to intermediate point qm7 (e, 3), intermediate point qm6 (g, 4-5), intermediate point qm8 (h, 7) to qg (i, 9) A route can be generated (see broken line in FIG. 6B, FIG. 6C).

  The dividing point qm4 (ef, 2-3) and the dividing point qm5 (hi, 6-7) in the first example are respectively the qi (e, 1) and qt2 ( h, 4) and qt2 (h, 4) and q4 (I, 9) are also intermediate points (see FIG. 4B (b)), and the intermediate point selected as the new route intermediate point is the route intermediate point setting means. The route halfway point additionally set by 31 (see FIG. 3) may be stored in the route halfway point storage means 46 and used.

Next, a second embodiment of the route halfway point setting unit 31 according to the present invention will be described with reference to FIG. As shown in FIG. 7A, the route halfway point setting unit 31 (see FIG. 3) according to the second embodiment pays attention to qt2 as a bending point to be smoothly processed, and q5 (f−g, 4). , Q6 (h, 5-6) are set as a route halfway point pair.
That is, the route halfway point setting means 31 passes the bending point qt2 (h, 4) located at the distance δ farthest from the line L connecting the initial configuration point qi (c, 1) and the target configuration point qg. And a dividing point q5 (f−g, 4) between the bending point q2 (e, 4) and the bending point qt2 (h, 4) so as to sandwich the bending point qt2 (h, 4), and A dividing point q6 (h, 5-6) between the bending point qt2 (h, 4) and the bending point q3 (h, 7) is further set as a midway point.

  Then, the route halfway point selection means 32 causes q5 (f−) of these three route halfway points qt2 (h, 4), q5 (f−g, 4), and q6 (h, 5-6). g, 4) and q6 (h, 5-6) are adopted as the route halfway point pairs q5 and q6, and the intermediate point calculation means 33 obtains the intermediate point qm9 (g−h, 4-5). After checking the interference with the object Ob, it is adopted as a new route halfway point.

In this way, the regenerated smooth path passes from qi (e, 1) to the dividing point q5 (f-g, 4) as shown by the thick solid line in FIG. Since the process proceeds to (h, 5-6), the bending point qt2 (h, 4) farthest from the line L connecting the initial configuration point qi (c, 1) and the target configuration point qg is eliminated. A route can be generated.
Further, in FIG. 7B, following the bending point q6 (h, 5-6) farthest from the line L, smooth processing is similarly performed for the bending point q5 (f-g, 4), A gradually smooth path can be generated.

DESCRIPTION OF SYMBOLS 1 Smooth path | route production | generation apparatus 2 Operation panel 3 Arithmetic processing apparatus 4 Memory | storage device 31 Path waypoint setting means 32 Path waypoint pair selection means 33 Intermediate point calculation means 34 Intermediate point interference check means 41 Initial position storage means 42 Target position storage means 43 Robot shape storage means 44 Work space storage means 45 Motion path storage means 46 Path halfway point storage means 100, 100 'Robot arm (multi-axis robot arm)
CA Interference area Ob Obstacle q1, q2 Path intermediate point pair q2, q3 Path intermediate point pair q3, q4 Path intermediate point pair qi (q1) Initial configuration point (initial position)
qg (q4) Target configuration point (target position)
qm (qm1, qm2, qm3 ...) Intermediate point qm4, qm5 Shift intermediate point

Claims (4)

Having a control unit comprising an arithmetic processing unit and a storage unit;
A smooth path generating device that smoothes an operation path of a multi-axis robot arm generated so as to avoid an obstacle from an initial position to a target position and regenerates the smooth path,
The controller is
Motion path storage means for storing a motion path from the initial position to the target position;
Route halfway point setting means for setting a plurality of route halfway points in the operation route;
Route halfway storage means for storing the plurality of route halfway points;
Route intermediate point pair selection means for selecting one of the route intermediate points as a pair and a route intermediate point pair consisting of another route intermediate point with respect to any one or more axes of the multi-axis robot arm. When,
An intermediate point between the one route intermediate point and the other route intermediate point adopted by the route intermediate point pair selection means is calculated as a formula (position data of one route intermediate point + position data of another route intermediate point). Intermediate point calculation means obtained from / 2,
An intermediate point interference check unit for determining whether the intermediate point obtained by the intermediate point calculation unit interferes with the obstacle, and
If the intermediate point interference check means determines that the intermediate point interferes with the obstacle, the current route is maintained without adopting the intermediate point as a new route halfway point. Perform steps,
When the intermediate point interference check means determines that the intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is changed from the one route intermediate point to the relevant point. A smooth route generating apparatus, wherein a smooth route generating step is performed in which a smooth route is regenerated by changing to a new motion route that passes through a new route halfway to the other route halfway. The route halfway point adopting means adopts a number of the route halfway point pairs successively in a daisy chain,
The intermediate point calculation means obtains a large number of intermediate points in each of the numerous path halfway point pairs,
The intermediate point interference check means determines whether each of the multiple intermediate points interferes with the obstacle,
In the smooth route generation step, when the intermediate point interference check unit determines that the intermediate point of the multiple intermediate points interferes with the obstacle, the intermediate point is determined as a new route intermediate point. Execute a current route maintenance step that adopts an operation route composed of a pair of intermediate points of each current route without adopting as,
If it is determined that the intermediate points do not interfere with the obstacles among the many intermediate points, the intermediate points are adopted as new route intermediate points, and the operation route is set to the respective intermediate points. Smooth path generation that regenerates the smooth path by changing from the one way intermediate point to the new operation route that passes through the new intermediate point to the other route intermediate point of the respective intermediate point The smooth path generation apparatus according to claim 1, wherein the step is executed. The control device, after the smooth route generation step, by the route halfway point selection means,
Adopting a pair of shift path midpoints consisting of one shift path midpoint in which at least one of the one path midpoint and the other path midpoint is different and another shift path midpoint pair is adopted Perform steps,
A shift intermediate point that is an intermediate point between the one shift path intermediate point and the other shift path intermediate point selected by the intermediate point calculation means and the shift path intermediate point pair selection means is calculated by a formula (one shift (2) Shift intermediate point calculation step obtained from the position data of the intermediate point of the route + position data of other intermediate point of the shift route) / 2
If the intermediate point interference check means determines that the shift intermediate point interferes with the obstacle, the current operation route is adopted without adopting the shift intermediate point as a new route intermediate point. Perform current route maintenance step,
When the intermediate point interference check means determines that the shift intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is set as the one shift route intermediate point. To perform a shift smooth path generation step for regenerating the smooth path by changing to a new operation path that travels from the midpoint of the new path to the other midpoint of the shift path,
In this way, the current route maintenance step or the shift smooth route generation step is executed after the smooth route generation step, and the current route maintenance step or the shift smooth route generation step is sequentially repeated from the smooth route generation step. The smooth path generating apparatus according to claim 1 or 2, wherein the operating path is smoothed. A smooth path generation method for smoothing an operation path of a multi-axis robot arm generated so as to avoid an obstacle from an initial position to a target position and regenerating the smooth path,
Using a smooth path generation device having a control device comprising an arithmetic processing device and a storage device,
A route halfway point setting step for setting a plurality of route halfway points in the operation route by a route halfway point setting means constituting the arithmetic processing unit;
By means of route halfway point adopting means constituting the arithmetic processing unit, from any one or more axes of the multi-axis robot arm from one route halfway point and another route halfway point that are paired among the route halfway points A path halfway point adopting step for adopting a path halfway point pair,
An intermediate point between the one route intermediate point and the other route intermediate point adopted in the route intermediate point pair selection step is calculated by an intermediate point calculating means (one route intermediate point) constituting the arithmetic processing unit. Position data + position data of other route intermediate points) / 2)
An intermediate point interference check step for determining whether the intermediate point obtained in the intermediate point calculation step interferes with the obstacle by the intermediate point interference check means constituting the arithmetic processing unit;
When the intermediate point interference check step determines that the intermediate point interferes with the obstacle, the current route is maintained without adopting the intermediate point as a new route intermediate point. Steps,
When the intermediate point interference check step determines that the intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is changed from the one route intermediate point to the relevant point. A smooth path generating step of regenerating the smooth path by changing to a new operation path that travels through a new path midpoint to the other path midpoint;
A smooth path generation method comprising:

Images