JP2012190405A - Route information correcting device, track planning device, and robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To improve a calculation probability of non-interference route including no node interfered with an obstacle.SOLUTION: A track planning device 100 corrects routing information constituted by connecting nodes via an edge. The track planning device 100 includes: an interference determination unit 42 for determining an interference property of a node included in the routing information based on environment information; an additional node generating unit 70 for generating an additional node on a periphery of the node that is determined to have interference by the interference determination unit 42; and a node replacement execution unit 44 for replacing the node determined to have the interference, with the additional node according to the determination result of the interference property of the additional node by the interference determination section 42.

Description

  The present invention relates to a route information correction device, a trajectory planning device, and a robot.

  Advances in technology for calculating the movement path of a robot in a two-dimensional / three-dimensional space are remarkable.

  In Patent Document 1, as described in paragraph 0017 of the same document, information output by the corner route information generating unit 16 is added to the map information generated by the map generating unit 12, and map information in a graph representation is added. The points to be corrected are disclosed. Note that the map information is a graph expression composed of nodes and edges as described in paragraph 0015 of the same document. Further, in paragraph 0015 of the same document, the trajectory planning unit 13 obtains the route information to the destination based on the map information generated by the map generating unit 12 and the movement start point and the position information of the destination given by the user. It is described that a route sequence is generated by searching. Paragraph 0020 of the document describes that the node information includes corner environment information defined in FIG. Paragraph 0021 of the same document discloses that calculation processing is performed with reference to FIGS. 4 to 6 of the same document. In paragraph 0028 of the same document, it is described that the posture of the mobile robot at the pass area, the arc path center position, and the arc path end point obtained up to step 8 is registered in the node information. It is disclosed that a more optimal route can be obtained by these.

  Patent Document 2 discloses an example related to the calculation of the movement path of a robot. As shown in FIG. 4 of Patent Document 2, a plurality of nodes calculated by RRT (Rapidly-exploring Random Tree) are provided between the current position and the target position. As shown in FIG. 4 of Patent Document 2, the nodes are connected by springs and dampers, and an attractive force acts between the nodes, and the locus becomes smooth (for example, paragraph 0044 of Patent Document 1). reference).

JP 2007-328486 A JP 2009-211571 A

  When searching for a road map with a graph structure in which nodes are connected by edges, the paths prepared in advance intervene with obstacles depending on the presence of obstacles around the robot because the number of nodes prepared in advance is limited. It may not be easy to find a non-interfering path that does not include a node. Thus, it is strongly desired to increase the calculation probability of a non-interference path that does not include a node that interferes with an obstacle.

  A path information correction apparatus according to the present invention is a path information correction apparatus that corrects path information configured by connection between nodes via an edge, and includes interference of the node included in the path information based on environment information An interference determination unit that determines the nature of the additional node, an additional node generation unit that generates an additional node around the node determined to have interference by the interference determination unit, and a determination result of the coherence of the additional node by the interference determination unit And a node replacement execution unit that replaces the node determined to have interference with the additional node.

  The interference determination unit determines whether a node adjacent to the node determined to have interference through an edge is also determined to have interference, and the additional node generation unit interferes in a non-continuous manner. The additional node may be generated around the node that is present.

  The interference determination unit is the above-described route information correction device that determines whether or not a node adjacent to the node determined to have the interference via an edge is also determined to have interference, the route information correction When the interference determination unit determines that the node adjacent to the node determined to have interference is also determined to have interference by the interference determination unit, the device is connected to each node that is continuously interfering and the nodes. Instructs the deletion of the specified edge.

  The interference determination unit according to any one of the above-described route information correction devices, which determines whether or not a node adjacent to the node determined to have the interference via an edge is also determined to have interference. The information correction apparatus instructs an increase in the cost of the edge reaching the node determined to be interfering in a non-continuous manner by the interference determination unit.

  A comparison / determination unit that determines the match between the route information; and when the additional node generation unit determines that the route information searched continuously in time by the comparison / determination unit matches, The additional node may be generated based on the node determined to have interference by the interference determination unit.

  The interference determination unit further includes a comparison determination unit that determines matching between the route information, and the additional node generation unit determines that the route information that is temporally continuous matches by the comparison determination unit. The route information correction device according to any one of the above, wherein the additional node is generated based on the node determined to have interference by the additional node generation unit, wherein the additional node generation unit generates a predetermined number of the additional nodes, and The interference determination unit may sequentially determine the coherence of the additional nodes included in the predetermined number of the additional nodes.

  The additional node generation unit may generate a plurality of the additional nodes based on execution of a predetermined function. The node is preferably composed of a plurality of values. The image processing apparatus further includes an interpolation processing unit that performs a route interpolation process on the route information processed by the node replacement execution unit, and the interpolation processing unit sets discrete points at predetermined intervals with respect to the route included in the route information. It is preferable that the interference determination unit determine the coherence of the discrete points based on the environment information.

  The trajectory planning apparatus according to the present invention is based on a road map storage unit that stores road map information in which a plurality of nodes are connected via cost-set edges, and processing of cost information obtained from the road map information. A route search unit that searches for a route between the start and goal nodes, and the route information correction device according to any one of the above that corrects the route information supplied from the route search unit. It is good to further have an environment information acquisition part which acquires the environment information.

  The robot according to the present invention incorporates the trajectory planning apparatus described above.

  The path information correction apparatus according to the present invention is an operation method of a path information correction apparatus that corrects path information configured by connection between nodes via edges, and is included in the path information based on environment information. Determine the coherence of the node, generate an additional node around the node determined to have interference by the determination, and determine that there is interference according to the determination result of the interference of the additional node by the determination Replace the node with the additional node.

  A program according to the present invention causes a computer to execute the operation method described above.

  According to the present invention, it is possible to increase the calculation probability of a non-interference path that does not include a node that interferes with an obstacle.

1 is a schematic block diagram of a trajectory planning apparatus according to a first embodiment. 3 is a schematic flowchart showing an operation of the trajectory planning apparatus according to the first exemplary embodiment. 3 is a schematic flowchart showing an operation of the trajectory planning apparatus according to the first exemplary embodiment. It is a schematic diagram which shows the path | route searched in the road map concerning Embodiment 1. FIG. It is a schematic diagram which shows the path | route searched in the road map concerning Embodiment 1. FIG. It is a schematic diagram which shows the path | route searched in the road map concerning Embodiment 1. FIG. It is a schematic diagram which shows the path | route searched in the road map concerning Embodiment 1, and path | route data. It is a schematic diagram which shows the increase process of the cost added to the edge concerning Embodiment 1. FIG. FIG. 10 is a schematic diagram illustrating additional node generation / addition node interference confirmation processing according to the first exemplary embodiment; It is a schematic diagram which shows the process which replaces the interference node concerning Embodiment 1 with an additional node. FIG. 4 is a schematic diagram illustrating an interpolation process according to the first embodiment. FIG. 3 is a schematic diagram of a robot according to a second embodiment. FIG. 3 is a block diagram illustrating a schematic configuration of a robot according to a second embodiment. FIG. 10 is an explanatory diagram for explaining an operation of the robot according to the second embodiment.

  Hereinafter, embodiments will be described with reference to the drawings. Each embodiment is not individually independent but can be combined with each other, and an effect based on the combination can be claimed. The drawings are used for explanation and are simplified as appropriate.

Embodiment 1
The trajectory planning apparatus 100 (see FIG. 1) according to the present embodiment corrects route data configured by connection between nodes via edges, and interference of nodes included in the route data based on environment information. And generating an additional node around a node determined to have interference (hereinafter, simply referred to as an interference node), and depending on the determination result of the interference of the additional node, the original interference node is Replacement with a non-interfering additional node (see FIG. 10: the interfering node q3 is replaced with a non-interfering additional node qn5). By replacing the interfering node with a non-interfering additional node and providing an alternative path, it is possible to effectively increase the calculation probability of a non-interfering path that does not include the interfering node in the path. By setting an additional node around the interference node, it is possible to avoid the necessity of preparing a large number of nodes in advance. Even if the surrounding environment of the robot changes arbitrarily / as needed, and the degree of freedom of movement of the robot is large, the node arrangement state can be appropriately corrected according to the environment of the place. It is possible to effectively increase the calculation probability of the interference path. Note that the non-interfering node is a node in which the node does not interfere with the obstacle, if it is supplementarily described. The determination method / criteria for determining whether or not a node interferes with an obstacle is arbitrary, and is appropriately adjusted according to the apparatus in which the trajectory planning apparatus 100 is incorporated.

  Note that the probability of calculating a non-interfering path can also be increased by preparing a huge number of nodes in advance. However, in this case, since the number of nodes and the number of edges increase, the calculation cost required for the route search process may increase, and it may be difficult to improve the overall performance. The calculation cost indicates the degree of calculation processing required for obtaining a solution in route search. When the calculation cost is high, a large amount of calculation processing is required. When the calculation cost is small, a small amount of calculation processing is sufficient.

  Hereinafter, it demonstrates concretely, referring drawings. In the following, the description will be made in consideration of planning the trajectory of the articulated robot arm. However, the present invention is not limited to this, and is applicable to other trajectory plans (for example, the trajectory plan when the robot moves in space). You may do it. The calculated trajectory can be applied to either a two-dimensional / 3-dimensional space. The information indexed by the nodes on the trajectory is arbitrary and is set as appropriate according to the device to which the trajectory is applied. In the following description, information and data may be described without being distinguished.

  FIG. 1 is a schematic block diagram of a trajectory planning apparatus. 2 and 3 are schematic flowcharts showing the operation of the trajectory planning apparatus. 4 to 6 are schematic diagrams showing the searched routes. FIG. 7 is a schematic diagram showing the searched route and route data. FIG. 8 is a schematic diagram showing a process of increasing the cost added to the edge. FIG. 9 is a schematic diagram illustrating generation of an additional node / interference confirmation processing of the additional node. FIG. 10 is a schematic diagram illustrating a process of replacing an interference node with an additional node. FIG. 11 is a schematic diagram illustrating the interpolation processing.

  As shown in FIG. 1, the trajectory planning apparatus 100 includes a road map storage unit 10, a path supply unit 20, an environment information acquisition unit 30, a path correction unit 40, a trajectory interpolation unit 50, a path match determination unit 60, and additional node generation. Part 70 is included. The route supply unit 20 includes a road map temporary storage unit 22, a node search unit 24, and a route search unit 26. The route correction unit 40 includes an interference determination unit 42 and a node replacement execution unit 44. The route match determination unit 60 includes a route temporary storage unit 62 and a route comparison unit 64. Since the road map temporary storage unit 22 is equivalent to the road map storage unit 10 in the sense of storing a road map, the road map temporary storage unit 22 may be simply grasped as a road map storage unit.

  The trajectory planning apparatus 100 is incorporated in a robot. More specifically, the computer part 80 excluding the road map storage unit 10 and the environment information acquisition unit 30 are incorporated in the robot. The road map storage unit 10 is network-connected to the robot. Necessary road map data is downloaded to the robot road map temporary storage unit 22 via the network at an appropriate timing.

  The trajectory planning apparatus 100 is configured by a computer, and a predetermined function is exhibited by an arithmetic processor such as a CPU (Central Processing Unit) executing a program stored in a storage device such as a memory. In the process of executing the program, various data are written in a RAM (Random Access Memory) or the like and used for arithmetic processing by the CPU. The CPU and the RAM are connected via a bus or the like, and a memory controller such as a RAM for controlling memory access is also connected to the bus. The specific architecture of the computer is arbitrary. A program installed in the computer constituting the trajectory planning apparatus 100 is appropriately stored in an arbitrary storage unit such as a hard disk or an optical storage medium (CD, DVD, BD, etc.).

  The road map storage unit 10 is a storage unit that stores a road map having a graph structure. The road map having a graph structure is data in which a large number of nodes (sometimes called vertices) are connected via edges (sometimes called branches). A road map of the graph structure is shown in FIG. However, in FIG. 4, it is a partial road map downloaded from the road map storage unit 10 to the road map temporary storage unit 22. The road map stored in the road map storage unit 10 has more nodes and edges than the downloaded road map.

The node indicates the posture of the robot arm. When the number of motor axes of the robot arm is N, the node q _c is expressed by joint angle vectors q _{c1 to} q _cN as is apparent from the following equation.
q _c = (q _c1 , q _c2 ,..., q _cN ) (1)

  An edge connects nodes and indicates a connection relationship between nodes. When the norm between nodes (sometimes called distance) is less than or less than a threshold value, an edge is connected between those nodes. Cost is given to the edge. The route search is executed by calculating the cost given to the edge. For example, a route having the smallest cost addition value is calculated as a route candidate. The cost is given according to, for example, the amount of energy and time required for posture change.

  A method for generating a road map stored in the road map storage unit 10 is arbitrary. For example, it is automatically generated by a computer by inputting various conditions such as a node expression and an edge generation condition to the computer.

  The route supply unit 20 determines and supplies a route connecting between the initial posture and the final posture. Note that the initial posture and the final posture are calculated by any means and supplied to the route supply unit 20. The initial posture and the final posture are expressed by, for example, the expression (1) similarly to the node.

  The road map temporary storage unit 22 stores data in which the road map stored in the road map storage unit 10 is partially downloaded. The road map stored in the road map temporary storage unit 22 is supplied to the node search unit 24 and the route search unit 26. As will be apparent from the following description, the road map stored in the road map temporary storage unit 22 can be corrected by temporarily storing the road map in the road map temporary storage unit 22. For example, unnecessary nodes can be deleted and unnecessary edges can be deleted. As a result, the route search unit 26 can search for the route again under different conditions, and an alternative route can be quickly provided.

  The node search unit 24 searches the road map for a node corresponding to the initial posture. Similarly, a node corresponding to the final posture is searched from the road map. After the search process, the node search unit 24 outputs a start node corresponding to the initial posture and a goal node corresponding to the final posture to the route search unit 26.

  The route search unit 26 searches the road map for a route connecting the start node and the goal node. As described above, a cost is set for each edge included in the road map. The route search unit 26 obtains a route that minimizes the added value of the cost given to the edge, and outputs this route.

  The environment information acquisition unit 30 is a sensing function unit that acquires 3D data by performing 3D measurement of the surrounding environment of the robot arm. A specific means of three-dimensional measurement is arbitrary. The sensor included in the environment information acquisition unit 30 is, for example, a stereo camera, a laser range finder, or a depth measure. Based on the principle of TOF (Time Of Flight), a forward three-dimensional state may be detected. Note that the distance measurement method based on TOF is a method of measuring the depth of the inspection light in the emission direction by detecting the time interval between the light emission timing of the inspection light and the light reception timing of the reflected light. Like a stereo camera, depth information may be obtained based on processing of a plurality of images acquired by a plurality of imaging systems. The environment information acquisition unit 30 models the three-dimensional information acquired from the sensor output, and supplies the modeled data to the path correction unit 40. Note that the modeling is performed, for example, by applying a geometric model to the detected three-dimensional shape.

  As will be apparent from the following description, the route correction unit 40 executes various other processes in addition to the route correction process. For example, the route correction unit 40 determines whether an interference node is included in the route supplied from the route supply unit 20 based on the data supplied from the environment information acquisition unit 30. The route correction unit 40 instructs the road map temporary storage unit 22 to delete nodes and edges. The route correction unit 40 provides route data to the trajectory interpolation unit 50. The route correction unit 40 supplies route data to the route temporary storage unit 62 and instructs the writing thereof. The route correction unit 40 supplies route data to the route comparison unit 64 and instructs the route comparison unit 64 to compare the route data with the route data stored in the route temporary storage unit 62. The route correction unit 40 determines the coherency of the additional node included in the route data supplied from the additional node generation unit 70. The route correction unit 40 also executes a process of replacing the interference node with a non-interference additional node. Note that details of the operation of the route correction unit 40 will be apparent from the following description.

  The interference determination unit 42 determines whether a node included in the route supplied from the route supply unit 20 interferes with an obstacle detected from data supplied from the environment information acquisition unit 30. The interference determination unit 42 determines whether an additional node included in the route supplied from the additional node generation unit 70 interferes with an obstacle detected from data supplied from the environment information acquisition unit 30. .

  As will be apparent from the following description, the node replacement execution unit 44 executes a process of replacing the interference node that is the generation target of the additional node with the non-interference additional node. Details of the operation of the node replacement execution unit 44 will be apparent from the description below.

  The trajectory interpolation unit 50 performs a process for smoothing the route. The route match determination unit 60 determines whether or not the routes continuously supplied from the route supply unit 20 match. The route temporary storage unit 62 temporarily stores the route supplied last time. The route comparison unit 64 compares the route supplied this time with the route supplied last time, and determines whether or not they match. The route comparison unit 64 outputs the route data to the additional node generation unit 70 when the two match, and outputs the mismatch signal to the route correction unit 40 when the two are different. The additional node generation unit 70 generates an additional node around the target interference node, and supplies the route to which the additional node has been added to the route correction unit 40. The details of the operations of the trajectory interpolation unit 50 to the additional node generation unit 70 will be apparent from the following description.

  The operation procedure of the trajectory planning apparatus 100 will be described with reference to FIGS. This will be described with reference to FIGS. 4 to 11 as appropriate. In FIG. 4 to FIG. 10, a solid line indicates a searched route, and a broken line indicates a route that is not searched. A one-dot chain line indicates a region where an obstacle detected from the output of the environment information acquisition unit 30 exists.

  First, environmental information is acquired (S100). Specifically, the environmental information acquisition unit 30 drives the sensor system to acquire environmental information, and processes and outputs acquired data. Thereby, the position of the obstacle in the space in which the robot arm is movable is detected.

  Next, trajectory planning is performed (S101). Specifically, the route supply unit 20 searches for a route connecting the initial and final postures. The node search unit 24 first searches for a node corresponding to the initial posture, searches for a node corresponding to the final posture, and supplies the start and goal nodes obtained by these searches to the route search unit 26. The route search unit 26 searches for a route connecting the start and goal nodes based on the cost calculation. The route search unit 26 obtains a route with the lowest cost, and outputs this to the route correction unit 40. Note that the specific method of route search is arbitrary, and should not be limited to the method based on the cost calculation process.

  When the trajectory plan is successful (S102), an interference check is performed (S103).

  Whether or not the trajectory planning is successful is performed without considering the coherence of the node to the obstacle. For example, even when a route is searched as schematically shown in FIG. 4, it is determined that the trajectory plan is successful. Note that whether or not the trajectory plan is successful is determined based on whether or not the route search unit 26 outputs a route.

  When a route is supplied from the route search unit 26, the interference determination unit 42 determines the coherency of the constituent nodes of the route (S103).

  When the constituent node of the route searched this time does not interfere with the obstacle, an interpolation process (S104) is performed. Specifically, the trajectory interpolation unit 50 provides discrete points at a constant interval Δq with respect to the route supplied from the route correction unit 40 (see FIG. 11A).

  Next, a final confirmation is made (S105). Specifically, the interference determination unit 42 determines whether or not the nodes and discrete points on the route supplied from the trajectory interpolation unit 50 are interfering with an obstacle (see the point in FIG. 11B). By determining whether the discrete points interfere with the obstacle, it is possible to more reliably avoid the robot arm coming into contact with the obstacle. Further, the path correction unit 40 determines whether or not a condition allowed by the robot arm (for example, a movable angle of the joint of the robot arm) is satisfied. In cases other than the movable angle of the joint of the robot arm, the robot arm cannot move along the path, and therefore, such a meaningless path is excluded.

  When the final confirmation is OK, the route correction unit 40 outputs the final route as trajectory data. If the final confirmation is NG, unnecessary edges are deleted (S106). Unnecessary edges are, for example, edges that reach nodes that interfere with obstacles, edges that include discrete points that interfere with obstacles, and the like. The route correction unit 40 instructs the road map temporary storage unit 22 to delete the unnecessary edge. The road map temporary storage unit 22 deletes the edge specified by the instruction according to the instruction from the route correction unit 40. By correcting / changing the temporarily stored road map itself, it is possible to effectively increase the possibility that the route searched again by the trajectory plan (S101) becomes more accurate. When there is no path connecting the start node and the goal node (S102), the trajectory plan fails.

  When a node that interferes with an obstacle is included in the route (S103), the process proceeds to an additional process (S107). Through this addition process (S107), a final route is generated or the process returns to S101. Details of the addition processing (S107) are shown in FIG.

  As shown in FIG. 3, when the interference check is NG (S103), it is determined whether or not the node continuously interferes (S200). Specifically, the interference determination unit 42 determines whether or not each successive node via the edge interferes with an obstacle.

  For example, when the route shown in FIG. 4 is supplied, the interference determination unit 42 detects that the node q1 interferes with the obstacle (S103), and then the node q3 interferes with the obstacle. This is detected (S200).

  When the nodes are continuously interfering, the interfering node / related edge is deleted (S211). Specifically, the route correction unit 40 instructs the road map temporary storage unit 22 to remove the interference node and the edge connected thereto. In response to an instruction from the route correction unit 40, the road map temporary storage unit 22 removes nodes and edges specified by this instruction. In S101 of the next cycle, a route search is performed under the condition that there is no interference node and no edge leading to the interference node.

  For example, in the case illustrated in FIG. 4, the route correction unit 40 instructs the road map temporary storage unit 22 to remove the nodes q1 and q3 and the edges e10 to e15 connected thereto. The road map temporary storage unit 22 removes the designated node and edge in accordance with an instruction from the route correction unit 40.

  When the node does not continuously interfere, it is determined whether there is stored data (S201). Specifically, the route correction unit 40 determines whether there is route data temporarily stored in the route temporary storage unit 62. In the initial state, no route data is stored in the route temporary storage unit 62. Therefore, when it is detected for the first time that the nodes do not continuously interfere, the process proceeds to step 209.

  If the route is not saved, the route searched this time is temporarily saved (S209). Specifically, the route correction unit 40 stores the route data supplied from the route search unit 26 in the route temporary storage unit 62.

  For example, in the case shown in FIGS. 5 and 6, the nodes do not continuously interfere. Therefore, when such a route is searched for the first time, the route correction unit 40 temporarily stores the route data in the route temporary storage unit 62.

  In addition, although the preservation | save mode of route data is arbitrary, it is easy to memorize | store the node row | line | column contained in a route. As shown in FIG. 7A, when the route is formed in the order of the start node, the node q2, the node q3, the node q6, and the goal node, they are arranged in that order as shown in FIG. 7B. What is necessary is just to memorize | store a node name.

  Next, the movement cost is increased (S210). Specifically, the route correction unit 40 instructs the road map temporary storage unit 22 to increase the cost given to the edge connected to the interference node. In response to this, the road map temporary storage unit 22 increases the cost of the edge specified by the instruction. This increases the possibility that a route other than the route searched this time will be searched.

  For example, as shown in FIG. 8, when the node q3 is an interference node, the cost of the edge between the nodes q2 and q3 is increased. Specifically, the cost given to the edge between the nodes q3 and q2 is doubled. As is clear from the comparison between FIG. 8A and FIG. 8B, the cost given to the edge between the node q2 and the node q3 is 70 to 140. Thus, by increasing the cost of the edge reaching the interference node, it is possible to increase the probability that the interference node is not included in the route, and to increase the possibility of searching for another alternative route.

  When there is a saved route (S201), the route searched this time is compared with the previous route, and it is determined whether or not they match (S202). Specifically, the route comparison unit 64 compares the previously searched route stored in the route temporary storage unit 62 with the currently searched route. It should be noted that a specific method for determining the matching of the routes is arbitrary. For example, the match of the route is determined based on the total number of nodes included in the route and the node order. The route searched this time is supplied from the route correction unit 40 to the route comparison unit 64, and the route searched last time is supplied from the route temporary storage unit 62 to the route comparison unit 64.

  When the routes to be compared do not match, the route searched this time is temporarily saved (S209). Specifically, the route correction unit 40 stores the route searched this time in the route temporary storage unit 62. As a result, the latest route is stored in the route temporary storage unit 62. The route correction unit 40 instructs the route temporary storage unit 62 to update route data in response to the mismatch signal supplied from the route comparison unit 64.

  When the comparison target paths match, an additional node is generated (S203). Specifically, the route comparison unit 64 supplies route data determined to match to the additional node generation unit 70, and the additional node generation unit 70 includes a predetermined number of additional nodes around the node determined to be interference. Add Note that the number of additional nodes added by the additional node generation unit 70 is arbitrary, not limited to a plurality, and may be a single number. By limiting the number of additional nodes generated to a predetermined number, it is possible to avoid an extreme increase in calculation cost.

The method for generating the additional node is arbitrary. For example, the additional node defined by Expression (2) is calculated by substituting the configuration value of the interference node into Expression (2). Assume that the interference node is represented by q _c = (q _c1 , q _c2 ,..., Q _cN ). By substituting q _c1 into equation (3), q _i1 is calculated. q _i2 is calculated by substituting q _c2 into equation (3). By repeating this up to q _cN , q _iN is calculated, and the value of the additional node is determined. The function rand () included in Expression (3) is a function that randomly outputs a value that satisfies the condition of −1 <rand () <1.
q _i = (q _i1 , q _i2 ,..., q _iN ) (2)
q _ij = q _cj (1 + σ × rand ()) (3)

  In the case of reaching step 203 via steps S200 to 202, an alternative route is searched even if the interference node exists on the route in a non-continuous manner and the cost given to the edge leading to the interference node is increased. If not. In such a case, there is a possibility that the entire route can be validated by appropriately correcting the position of the interference node to make the node in a non-interference state. In the present embodiment, in view of this point, an additional node is arranged around the interference node, the coherence of the additional node is determined, and the interference node is replaced with a non-interference additional node. Thereby, it is possible to effectively increase the calculation probability of a non-interference route that does not include an interference node in the route.

  After the additional node is generated, the presence / absence of a non-interfering additional node is determined (S204). Specifically, the interference determination unit 42 determines whether the additional node generated by the additional node generation unit 70 interferes with an obstacle detected from the data supplied from the environment information acquisition unit 30. To do.

  For example, in the case illustrated in FIG. 9, the additional node generation unit 70 generates additional nodes qn1 to qnN around the node q3. The interference determination unit 42 determines the coherence in order from the node qn1, and detects that the node qn5 is a non-interfering node. FIG. 9A schematically shows that an additional node is generated around the node q3, and FIG. 9B schematically shows that other than qn5 around the node q3 is an interference node. Shown in

  When there is no non-interfering additional node, this corresponds to a case where obstacle avoidance is not possible even with the generation of the additional node. In this case, the interference node that is the generation target of the additional node and the edge connected thereto are deleted (step S211). The deletion of nodes and edges is instructed by the route correction unit 40 and executed by the road map temporary storage unit 22. Thereafter, the process returns to step S101.

  If there is a non-interference added node, a replacement process is executed (S205). Specifically, as schematically illustrated in FIG. 10, the path correction unit 40 replaces the interference node that is the target of generation of the additional node with a non-interfering additional node. First, as illustrated in FIG. 10A, the node replacement execution unit 44 deletes the interference node q3 and the edge connected thereto. Next, the node replacement execution unit 44 generates an edge for the additional node qn5 as shown in FIG. Specifically, the node q2 and the additional node qn5 are connected by an edge, and the additional node qn5 and the node q6 are connected by an edge. In this way, the node q3 is replaced with the additional node qn5. The route correction unit 40 supplies the corrected route to the trajectory interpolation unit 50.

  Next, interpolation processing is performed (S206). Specifically, the trajectory interpolation unit 50 provides discrete points at a predetermined interval Δq with respect to the route supplied from the route correction unit 40 (see FIG. 11A).

  Next, a final confirmation is made (S207). Specifically, the interference determination unit 42 determines whether or not nodes and discrete points on the route supplied from the trajectory interpolation unit 50 interfere with an obstacle (see FIG. 11B). By determining whether the discrete points interfere with the obstacle, it is possible to more reliably avoid the robot arm coming into contact with the obstacle. Further, the path correction unit 40 determines whether or not a condition allowed by the robot arm (for example, a movable angle of the joint of the robot arm) is satisfied. In cases other than the movable angle of the joint of the robot arm, the robot arm cannot move along the path, and therefore, such a meaningless path is excluded.

  In the case of final confirmation NG, the non-interfering additional node in which the interfering node is replaced is deleted (S208). Specifically, the route correction unit 40 deletes the additional node introduced by the node replacement execution unit 44. Next, the process proceeds to S204, where it is determined whether there is a non-interfering additional node among the remaining additional nodes. Subsequent processing is the same as described above.

  As is clear from the above description, in this embodiment, the route is selected based on whether or not the node continuously interferes. When nodes are continuously interfering, it can be estimated that the possibility of avoiding an obstacle is not high even if the position of the interfering node is adjusted. Therefore, in this case, it is possible to delete the interference node itself from the road map and search for another alternative route. As a result, it may be possible to search for a route in a relatively short time.

  Furthermore, in this embodiment, the consistency of the route searched continuously is determined, and when there is a match, the process proceeds to the route correction step based on the additional node (in short, S203). When the route searched continuously does not match, it is expected that another route will be searched by adjusting the value of the cost given to the edge and executing the route search again. As a result, it may be possible to search for an alternative route in a relatively short time.

  In the present embodiment, the additional node is calculated by substituting the configuration value of the interference node into a predetermined function. As a result, the additional node can be calculated without complicated calculation. The possibility of obstacle avoidance can be increased by generating a plurality of additional nodes in the form of a ring around an interference node that is a target for generating additional nodes. The generation mode of the additional node is arbitrary. For example, additional nodes may be generated in a spherical shape with the target interference node as the center. By limiting the number of additional nodes generated to a predetermined number, it is possible to avoid an extreme increase in calculation cost.

Embodiment 2
The second embodiment will be described with reference to FIGS. FIG. 12 is a schematic diagram of a robot. FIG. 13 is a block diagram illustrating a schematic configuration of the robot. FIG. 14 is an explanatory diagram for explaining the operation of the robot.

  The trajectory planning apparatus 100 shown in the first embodiment is built in, for example, the robot 300 shown in FIG. A robot 300 shown in FIG. 12 includes a trunk portion 301 provided with a head portion and a wheel portion, an arm portion 302 provided for the trunk portion 301, and a hand portion 303 provided at the tip of the arm portion 302. A drive unit such as an electric motor is provided at the joint of the arm unit 302, and the arm unit 302 is configured to be capable of joint motion. The movement of the arm unit 302 is controlled by a computer built in the body unit 301.

  A robot 300 shown in FIG. 12 includes the system configuration shown in FIG. As shown in FIG. 13, the output of the trajectory planning unit 100 corresponding to the trajectory measuring device 100 shown in FIG. 1 is supplied to the drive control unit 200, and the output of the drive control unit 200 is supplied to the drive unit 210. .

  The drive control unit 200 controls the drive unit 210 according to the route data supplied from the trajectory planning unit 10. When the path data shown in FIG. 11 is supplied, the drive control unit 200 sequentially controls the drive unit 210 so that the posture is specified by discrete points / nodes. The drive unit 210 is controlled by the drive control unit 200, whereby the movement of the arm unit 302 of the robot 300 is realized. As schematically shown in FIG. 14, the hand unit 303 of the robot 300 moves on the route and moves to a position specified by a goal node that can hold the holding object via the nodes qn5 and q6. To do. Also in this embodiment, the same effects as those described in Embodiment 1 can be obtained.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. The target specified by the node should not be limited to a three-dimensional posture, but may be a position in a two-dimensional space. It can be applied to various machines / devices other than robots.

100 trajectory planning device

10 road map storage unit 20 route supply unit 22 road map temporary storage unit 24 connection node search unit 26 route search unit 30 environment information acquisition unit 40 route correction unit 42 interference determination unit 44 node replacement execution unit 50 trajectory interpolation unit 60 path match determination Unit 62 Route temporary storage unit 64 Route comparison unit 70 Additional node generation unit 80 Computer part


200 Drive Control Unit 210 Drive Unit

300 Robot 301 Body 302 Arm 303 Hand

Claims (14)

A route information correction device for correcting route information configured by connection between nodes via an edge,
An interference determination unit that determines the coherence of the node included in the route information based on environment information;
An additional node generation unit that generates an additional node around the node determined to have interference by the interference determination unit;
A node replacement execution unit that replaces the node determined to have interference with the additional node in accordance with the determination result of the coherency of the additional node by the interference determination unit;
A route information correction apparatus comprising: The interference determination unit determines whether or not a node adjacent to the node determined to have the interference via an edge is also determined to have interference,
The route information correction apparatus according to claim 1, wherein the additional node generation unit generates the additional node around the nodes that interfere with each other in a discontinuous manner. The path information correction device according to claim 1, wherein the interference determination unit determines whether or not a node adjacent to the node determined to have the interference via an edge is also determined to have interference. And
When the interference determination unit determines that a node adjacent to the node determined to have interference through an edge is also determined to have interference by the interference determination unit, the path information correction device Instructs the nodes to delete the connected edge. The route according to any one of claims 1 to 3, wherein the interference determination unit determines whether or not a node adjacent to the node determined to have the interference via an edge is also determined to have interference. An information correction device,
The route information correction apparatus instructs the cost of an edge reaching the node determined to be interfering in a non-continuous manner by the interference determination unit. A comparison / determination unit that determines the match between the route information;
The additional node generation unit, based on the node determined to have interference by the interference determination unit when it is determined that the path information searched continuously in time by the comparison determination unit match. The route information correction apparatus according to claim 1, wherein an additional node is generated. A comparison / determination unit that determines the match between the route information;
The additional node generation unit, based on the node determined to have interference by the interference determination unit when it is determined that the path information searched continuously in time by the comparison determination unit match. The route information correction device according to any one of claims 1 to 5, wherein the route information correction device generates an additional node.
The additional node generation unit generates a predetermined number of the additional nodes,
The interference determination unit sequentially determines the coherence of the additional nodes included in a predetermined number of the additional nodes.   The path information correction apparatus according to claim 1, wherein the additional node generation unit generates a plurality of the additional nodes based on execution of a predetermined function.   The path information correcting apparatus according to claim 1, wherein the node includes a plurality of values. An interpolation processing unit for executing a route interpolation process on the route information processed by the node replacement execution unit;
The interpolation processing unit provides discrete points at predetermined intervals with respect to a route included in the route information,
The path information correction apparatus according to claim 1, wherein the interference determination unit determines the coherence of the discrete points based on the environment information. A roadmap storage unit that stores roadmap information in which a plurality of nodes are connected via cost-set edges;
A route search unit that searches for a route between the start and goal nodes based on the processing of the cost information obtained from the road map information;
The route information correction device according to any one of claims 1 to 9, wherein the route information supplied from the route search unit is corrected.
A trajectory planning device.   The trajectory planning apparatus according to claim 10, further comprising an environmental information acquisition unit that acquires the environmental information.   A robot incorporating the trajectory planning apparatus according to claim 11. An operation method of a route information correction device for correcting route information configured by connection between nodes via an edge,
Determining the coherence of the node included in the route information based on environmental information;
An additional node is generated around the node determined to have interference by the determination,
An operation method of a path information correction apparatus, wherein the node determined to be in interference is replaced with the additional node according to the determination result of the coherence of the additional node by the determination.   A program for causing a computer to execute the operation method according to claim 13.

Images