JP5659890B2 - Robot trajectory planning system and trajectory planning method - Google Patents

Description

  The present invention relates to a trajectory planning system and a trajectory planning method for a robot.

  A robot is required to autonomously perform a trajectory plan of a robot arm reflecting environmental information even in various changing environments. Here, the trajectory plan of the robot arm refers to a series of joint axis information in which the position and orientation of the end effector of the robot arm reach from the initial posture to the final posture so that the robot itself and the environment do not interfere with each other. It is a problem.

  As one of solutions to this problem, a technique is known that uses a graph structure having a plurality of nodes that are joint axis information of a robot. In the graph structure, nodes are connected by edges, and a route that is a sequence of nodes from a start node that is an initial posture to a goal node that is a final posture is searched. A technique for performing trajectory planning using such a graph structure is disclosed in Patent Document 1, for example.

  In Patent Document 1, by setting a plurality of final positions, it is easy to set an appropriate trajectory by increasing the possible paths.

JP 2006-48372 A

In the method using the graph structure as described above, if the number of nodes is increased in order to perform the trajectory planning reliably, it takes time to search for the trajectory. The same applies to the case where a plurality of final positions are set as in the technique disclosed in Patent Document 1.
On the other hand, if the number of nodes is reduced in order to shorten the time required for trajectory search, trajectory search may fail.

  The present invention has been made to solve such a problem, and an object thereof is to provide a trajectory planning system and a trajectory planning method for a robot that can improve the success rate of trajectory planning with a small number of nodes. To do.

A trajectory planning system for a robot according to an aspect of the present invention includes:
A trajectory design system for a robot using a graph structure,
Environment recognition means for acquiring environment information of the environment around the robot;
Graph structure data storage means for storing a graph structure having a node indicating joint axis information of the hand and arm of the robot, and an edge connecting the nodes;
Wherein with respect to the graph structure stored in the graph structure data storage means, add start node indicating the initial posture of the hand and the arm, and the hand and the goal node indicating a final attitude of the arm, further wherein Before connecting each node from the start node to the goal node, a node in which the hand is arranged in the rear space of the hand in the final posture indicated by the goal node is added, and the added node A graph structure search processing means for connecting a node adjacent to the node with an edge and searching for a path connecting the start node to the goal node;
For a node included in a route searched by the graph structure search processing unit, it is determined whether or not the attitude indicated by the node interferes with the environment information acquired by the environment recognition unit. If determined, node validity determination means for excluding the node from the graph structure;
Is provided.

The graph structure search processing means preferably adds nodes for arranging the hands on the semicircular arc at substantially equal intervals.
The graph structure search processing means preferably adds a node for placing the hand in a virtual hemisphere.

A robot trajectory design method according to an aspect of the present invention includes:
A robot trajectory design method using a graph structure,
Obtaining environmental information of the environment around the robot;
For a graph structure having a node indicating joint axis information of the robot hand and arm and an edge connecting the nodes, a start node indicating an initial posture of the hand and the arm, and the hand and the Before adding the goal node indicating the final posture of the arm and connecting the nodes from the start node to the goal node, the hand is placed in the rear space of the hand in the final posture indicated by the goal node. Adding a node to be arranged , connecting the added node and a node adjacent to the node by an edge, and searching for a route connecting the start node to the goal node;
For a node included in the searched route, it is determined whether the attitude indicated by the node interferes with the acquired environment information. If it is determined that there is an interference, the node is represented by the graph structure. Steps to exclude from,
Is provided.

  As described above, according to the present invention, it is possible to provide a trajectory planning system and a trajectory planning method for a robot that can improve the success rate of trajectory planning with a small number of nodes.

1 is an overall view showing a schematic configuration of a robot according to an embodiment. 1 is a configuration diagram of a robot trajectory planning system according to an embodiment. It is a block diagram of the whole body trajectory plan part of the robot which concerns on embodiment. It is a conceptual diagram of the graph structure which concerns on embodiment. It is the schematic which shows the arrangement | positioning relationship between the node and hand to add. It is a flowchart figure of the trajectory plan process which concerns on embodiment. (A) is a top view which shows roughly the arrangement | positioning relationship between the added node and a hand. (B) is a side view schematically showing an arrangement relationship between an added node and a hand. It is the schematic which shows the different arrangement | positioning relationship between the added node and a hand. It is a conceptual diagram which shows the mode of an orbit search.

  Embodiments of the present invention will be described below with reference to the drawings. In the following, in the description in the text, the symbols described before are used as necessary.

FIG. 1 is an overall view showing a schematic configuration of the robot according to the present embodiment.
FIG. 1 illustrates an external configuration of the robot 100, and the robot 100 includes a body 1, an arm 2 connected to the body 1, and a hand 3 connected to the arm 2. The arm 2 includes a plurality of joints and a plurality of links connected via the joints. The hand 3 is connected to the end of the arm 2 via a wrist joint. In addition, although illustration about a detail is abbreviate | omitted, the hand 3 may be provided with the palm part and the several finger | toe connected to a palm part via a joint. Each finger may include a plurality of finger joints and a plurality of links connected via the finger joints.

  By driving an actuator (not shown) such as a motor provided in each joint, each joint operates at a desired angle and angular velocity. As a result, in each coordinate system, the position and posture of the whole body of the robot 100 and the position and posture of the hand can be controlled to a desired position and posture. Thus, the actuator corresponds to power generation means when the robot 100 operates.

  The robot 100 executes various tasks specified by the user (operations on objects existing in the environment). For example, the robot 100 grips an object existing in the environment with the hand 3. The robot 100 executes a trajectory plan of the arm 2 and the hand 3 in order to execute such a task, and drives and controls each joint so as to follow the planned trajectory. The trajectory is a series of joint angle vectors from the joint angle vector indicating the initial posture to the joint angle vector indicating the final posture, and is obtained using a graph structure described later.

  FIG. 2 is a configuration diagram of the robot trajectory planning system according to the present embodiment. The trajectory planning system 10 includes an environment recognition unit 11, a task management unit 12, a final whole body position / posture determination unit 13, an initial whole body position / posture acquisition unit 14, a whole body trajectory plan unit 15, an interpolation unit 16, And a motor drive unit 17.

  The environment recognition unit 11 acquires environment information about the environment around the robot 100. The environment recognition unit 11 is configured using various sensors, and acquires environment information such as the position and shape of an object present in the environment around the robot 100. For example, a stereo camera or a laser range finder can be used as the sensor, and the environment recognition unit 11 acquires three-dimensional point cloud information about an object in the environment and acquires distance information from the robot 100 to the object. be able to. The environment recognition unit 11 generates an object in the environment as, for example, a rectangular parallelepiped voxel information or a three-dimensional object model based on the acquired distance information of the object. Output to the whole body position / posture determination unit 13 and the whole body trajectory planning unit 15.

  The task management unit 12 manages tasks to be executed by the robot 100. The task management unit 12 acquires necessary information from the environment information acquired by the environment recognition unit 11 according to the content of the task. The contents of the task may be input by the user via an interface (not shown) provided in the trajectory planning system 10.

  For example, in the task of grasping an object, information indicating the object to be grasped by the robot 100, the grasping method thereof, and the like are included in the task, and the task management unit 12 can grasp the object to be grasped from the environment information acquired by the environment recognition unit 11. Information indicating the object to be obtained, information indicating its shape, position, posture, and the like are acquired.

  The final whole body position / posture determination unit 13 determines the final whole body position / posture of the robot 100 during task execution from the environment information and the task information output from the task management unit 12.

  The initial whole body position / posture acquisition unit 14 acquires the initial whole body position / posture of the robot 100 during task execution from the task information output from the task management unit 12. The initial whole body position / posture may be the same as the current position / posture of the robot 100.

  The whole body trajectory planning unit 15 performs trajectory planning for the whole body of the robot 100 based on the environment information, the final whole body position / posture, and the initial whole body position / posture. Trajectory planning is performed by searching the trajectory for each part, and a trajectory from the initial whole body position / posture to the final whole body position / posture that does not interfere with the environment is planned. The details of the whole body trajectory planning unit 15 will be described later.

  The interpolating unit 16 interpolates the whole body joint trajectory obtained by the whole body trajectory planning unit 15 so as to be appropriate according to the control cycle for driving the motor. A smoother trajectory is obtained by the interpolation unit 16 as compared with the trajectory obtained by the whole body trajectory planning unit 15. The interpolation unit 16 generates a motor command value for the actuator (motor) of the robot 100 based on the joint angle vector included in the trajectory.

  The motor drive unit 17 performs feedback control based on the motor command value obtained by the interpolation unit 16.

Next, the details of the whole body trajectory planning unit 15 will be described with reference to FIGS. 3 to 9.
FIG. 3 is a configuration diagram of the whole body trajectory planning unit 15. The whole body trajectory planning unit 15 includes a graph structure data storage unit 151, a search graph structure data holding unit 152, a graph structure generation processing unit 153, a graph structure search processing unit 154, a node validity determination processing unit 155, It has.

  The graph structure data storage unit 151 stores a plurality of graph structures, and stores a graph structure for each part in the present embodiment. In the present embodiment, instead of performing a trajectory plan using a graph structure representing all joint axes of the whole body of the robot 100, a trajectory for each part is obtained using a graph structure prepared for each part. To plan. For example, the graph structure stored in the graph structure data storage unit 151 includes the graph structures of the arm 2 and the hand 3.

  FIG. 4 is a conceptual diagram of the graph structure. FIG. 5 is a diagram illustrating a positional relationship between the hand 3 in the final posture and the added nodes a1 to a6. However, in FIG. 4, some nodes a <b> 3 to a <b> 6 illustrated as added nodes in FIG.

  The graph structure 20 includes a plurality of nodes q1 to q9 and a line segment (hereinafter sometimes referred to as an edge) connecting the nodes. Using the graph structure 20, a route connecting the start node S and the goal node G is searched. The node structure of the graph structure is set by the user in consideration of the contents of the task, and each node is carefully selected in advance by the user.

  The node is a joint axis θ1 of the hand and a joint axis θ2 of the arm (however, when the hand has a plurality of joint axes, θ1 indicates a vector, and similarly when the arm has a plurality of joint axes) , Θ2 represents a vector). An edge has distance information between nodes.

  The search graph structure data holding unit 152 temporarily holds a graph structure used for trajectory search. For example, the graph structure being used for the trajectory search is temporarily held in the search graph structure data holding unit 152. In addition, the searched graph structure data holding unit 152 holds the searched trajectory in the search graph structure data holding unit 152 and updates the held graph structure when the robot 100 moves. May be used.

  The graph structure generation processing unit 153 selects the graph structure of the part to be trajectory searched from the graph structure stored in the graph structure data storage unit 151 and holds the selected graph structure in the search graph structure data holding unit 152. To do. Then, the graph structure generation processing unit 153 adds the input start node S and goal node G to the graph structure held in the search graph structure data holding unit 152.

Further, as illustrated in FIGS. 4 and 5, the graph structure generation processing unit 153 adds nodes a1 to a6 in which the hand 3 is arranged in the peripheral space of the hand 3 in the final posture, which is indicated by the goal node G. That is, nodes a1 to a6 in which the hand 3 is arranged in the peripheral space around the hand position of the arm 2 indicated by the goal node G are added.

  Then, the graph structure generation processing unit 153 connects each added node with a neighboring node by an edge. Here, the start node S is a node indicating the position / posture of the trajectory search part in the initial whole body position / posture of the robot 100, and the goal node G is the position / posture of the trajectory search part in the final whole body posture of the robot 100. It is a node to show. Note that the start node S may be a node indicating only the posture of the trajectory search site.

  The graph structure search processing unit 154 uses the graph structure with the added nodes and edges held in the search graph structure data holding unit 152 to connect the path from the start node S to the goal node G (start node S To the goal node G, and may be referred to as a trajectory). Note that the search method by the graph structure search processing unit 154 is not limited, and a known method such as a Dijkstra method may be used.

  The node validity determination processing unit 155 checks whether or not the posture indicated by the node for the nodes included in the route searched by the graph structure search processing unit 154 does not interfere with an object in the environment or the robot 100 itself. If there is interference, the node is excluded from the graph structure of the search graph structure data holding unit 155.

FIG. 6 is an operation flowchart of the whole body trajectory planning unit 15.
First, the graph structure generation processing unit 153 selects a graph structure of a region to be trajectory searched from the graph structure stored in the graph structure data storage unit 151 (in this embodiment, the graph structure of the hand and the arm). (S101) The selected graph structure is held in the search graph structure data holding unit 152.

Next, the graph structure generation processing unit 153 adds the start node S and the goal node G to the graph structure stored in the search graph structure data holding unit 152 (S102). Further, the graph structure generation processing unit 153 adds nodes a1 to a6 in which the hand 3 is arranged in the peripheral space of the hand 3 in the final posture indicated by the goal node G (S103). Note that the number of nodes to be added is set as appropriate.

The graph structure generation processing unit 153 according to the present embodiment adds nodes a1 to a6 in which the hand 3 is arranged in the rear space of the hand 3 in the final posture. At this time, as shown in FIGS. 7A and 7B, the graph structure generation processing unit 153 places the hand 3 in the virtual hemispherical space formed in the rear space of the hand 3 in the final posture. Nodes a1 to a6 may be added. By specifying the space to which the node is added in this way, the number of nodes to be added can be reduced compared to the case where the node in which the hand 3 is arranged is randomly added to the rear space of the hand 3 in the final posture. it can. These nodes a1 to a6 are preferably added at random so that the interval between adjacent nodes is narrower than the interval between adjacent nodes in the graph structure 20. Note that the size of the virtual hemispherical space is appropriately set by the user.

Alternatively , as illustrated in FIG. 8, the graph structure generation processing unit 153 may add nodes a1 to a6 in which the hand 3 is arranged on an arc in the rear space of the hand 3 in the final posture. For example, a node that has a diameter in the vertical direction with respect to the hand 3 in the final posture and is disposed on concentric arcs R1 and R2 virtually drawn rearward from the coordinate O of the hand position. a1, a3, a5 and nodes a2, a4, a6 are added. At this time, the intervals of the nodes a1, a3, a5 arranged on the arc R1 are substantially equal, and the intervals of the nodes a2, a4, a6 arranged on the arc R2 are substantially equal. The nodes a1 and a2 are arranged on a line L1 extending from the coordinate O, the nodes a3 and a4 are arranged on a line L2 extending from the coordinate O, and the nodes a5 and a6 are L3 extending from the coordinate O. Placed on top. At this time, the angle θ3 formed by L1 and L2 and the angle θ4 formed by L1 and L3 are set substantially equal. Also in this case, since the space to which the node is added is specified, the number of nodes to be added can be reduced as compared with the case where the node in which the hand 3 is arranged is randomly added to the space behind the hand 3 in the final posture. . In addition, since nodes are added in the vertical direction, even if an obstacle exists, the object to be grasped can be approached from above or below so as to avoid the obstacle, improving the success rate of trajectory design. be able to. The diameter of the arc and the interval between nodes arranged on the arc are set as appropriate. Moreover, in this Embodiment, although the node was added to the up-down direction, the direction to add is not specifically limited. In short, it is only necessary to satisfactorily approach the object to be grasped so as to avoid the obstacle.

  Next, the graph structure generation processing unit 153 connects each added node with a neighboring node by an edge (S104). Then, the graph structure search processing unit 154 searches for a trajectory connecting the start node S to the goal node G using the graph structure to which nodes and edges are added (S105). Further, the graph structure search processing unit 154 determines whether or not a trajectory exists as a result of the trajectory search (S106). If the trajectory does not exist (in the case of “No” in S106), the trajectory cannot be searched and the processing is terminated.

Next, when there is a trajectory (when “Yes” in S106), the node validity determination processing unit 155 determines that the attitude indicated by the node for the node included in the searched route is an object in the environment ( It is checked whether or not there is interference with the obstacle or the robot 100 itself (S107), and the presence or absence of interference is detected (S108). Then, as a result of the interference detection, the node validity determination processing unit 155 excludes the node from the graph structure when any one of the nodes included in the route interferes (in the case of “NG” in S108). (S109). Thereafter, the node validity determination processing unit 155 performs the trajectory search again using the graph structure after the node is excluded. For example, as shown in FIG. 9, when the node q3 or the node q8 in the vicinity of the goal node G interferes, the node validity determination processing unit 155 excludes the node q3 or the node q8 and performs the trajectory search again. Run.
On the other hand, the node validity determination processing unit 155 outputs a trajectory when any of the nodes included in the route does not interfere as a result of the interference detection (in the case of “OK” in S108). In the present embodiment, start node S → node q1 → node a1 → node a2 → goal node G is output as a trajectory.

In general, obstacles that interfere with the hand or arm often exist around the object to be grasped, particularly in the space behind the object, which is the trajectory of the hand or arm. Therefore, in the present embodiment, a node where the hand 3 is arranged is added to the peripheral space of the hand 3 in the final posture. As described above, since the nodes a1 to a6 in which the hand 3 is arranged are added to the surrounding space of the object to be grasped where the presence of the obstacle is expected, particularly the rear space of the object, the obstacle exists. It is possible to locally increase the number of route candidates in the vicinity of the position where the position is expected. Therefore, for example, as shown in FIG. 9, even when the nodes q3 and q8 near the goal node G interfere, the success rate of the trajectory planning can be improved. In addition, it is possible to locally increase the number of route candidates near the position where an obstacle is expected to exist, so there is no need to add nodes to other spaces, and a graph structure with a relatively small number of nodes can be created. Can be used. Therefore, the amount of calculation can be reduced, which can contribute to reduction in calculation time and downsizing of the calculation device.

Other embodiments.
In the embodiment described above, the trajectory plan is performed in order to execute the task of gripping the object to be gripped by the hand, but when executing the task of moving the object already gripped by the hand to another position, A trajectory plan can be executed in substantially the same manner. In this case, a graph structure corresponding to the task may be used.

  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. For example, in the above-described embodiment, the robot 100 having the trunk is used, but the same can be applied to a robot having only an arm and a hand.

100 Robot 1 Body 2 Arm 3 Hand 10 Trajectory Planning System 11 Environment Recognition Unit 12 Task Management Unit 13 Final Whole Body Posture Determination Unit 14 Initial Whole Body Position / Posture Acquisition Unit 15 Whole Body Trajectory Planning Unit 16 Interpolation Unit 17 Motor Drive Unit 20 Graph Structure 151 Graph structure data storage unit 152 Search graph structure data storage unit 153 Graph structure generation processing unit 154 Graph structure search processing unit 155 Node validity determination processing units a1 to a6 Nodes q1 to q9 Node G Goal node S Start node

Claims (4)

A trajectory design system for a robot using a graph structure,
Environment recognition means for acquiring environment information of the environment around the robot;
Graph structure data storage means for storing a graph structure having a node indicating joint axis information of the hand and arm of the robot, and an edge connecting the nodes;
Wherein with respect to the graph structure stored in the graph structure data storage means, add start node indicating the initial posture of the hand and the arm, and the hand and the goal node indicating a final attitude of the arm, further wherein Before connecting each node from the start node to the goal node, a node in which the hand is arranged in the rear space of the hand in the final posture indicated by the goal node is added, and the added node A graph structure search processing means for connecting a node adjacent to the node with an edge and searching for a path connecting the start node to the goal node;
For a node included in a route searched by the graph structure search processing unit, it is determined whether or not the attitude indicated by the node interferes with the environment information acquired by the environment recognition unit. If determined, node validity determination means for excluding the node from the graph structure;
Robot trajectory design system equipped with. The robot trajectory design system according to claim 1, wherein the graph structure search processing unit adds nodes for arranging the hands on a semicircular arc at substantially equal intervals. The robot trajectory design system according to claim 1, wherein the graph structure search processing unit adds a node for placing the hand in a virtual hemisphere. A robot trajectory design method using a graph structure,
Obtaining environmental information of the environment around the robot;
For a graph structure having a node indicating joint axis information of the robot hand and arm and an edge connecting the nodes, a start node indicating an initial posture of the hand and the arm, and the hand and the Before adding the goal node indicating the final posture of the arm and connecting the nodes from the start node to the goal node, the hand is placed in the rear space of the hand in the final posture indicated by the goal node. Adding a node to be arranged , connecting the added node and a node adjacent to the node by an edge, and searching for a route connecting the start node to the goal node;
For a node included in the searched route, it is determined whether the attitude indicated by the node interferes with the acquired environment information. If it is determined that there is an interference, the node is represented by the graph structure. Steps to exclude from,
A robot trajectory design method comprising:

Images