JP2009211571A - Course planning device, course planning method, and computer program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To find a course to avoid an obstacle and modify it into a smooth course easy to use for movement control and course planning. <P>SOLUTION: First, a Rapidly-exploring Random Tree (RRT) is used to rapidly acquire a course from a start to a goal. Then, requirements such as course smoothing and optimization and avoidance of inter-object penetration are expressed in forces and added to respective mass points in a multi-mass system dynamics simulation with respective nodes on the course found in the RRT as mass points, and the dynamics simulation with a position on the course found in the RRT as an initial condition is performed to generate a course meeting all the requirements. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a trajectory planning apparatus and a trajectory planning method for planning a movement path or a trajectory (Path) from a certain position (current position: Start) to a certain position (target position: Goal) on a two-dimensional or three-dimensional work space. In particular, the trajectory planning to find a trajectory that avoids obstacles and to correct it as a smooth trajectory so that it can be easily used for robot movement control and trajectory planning of the articulated arm hand, etc. The present invention relates to an apparatus, a trajectory planning method, and a computer program.

  For example, when autonomously controlling a mobile device such as a robot or a hand that is an end effector of an articulated arm from a certain position (current position: Start) on a two-dimensional or three-dimensional work space A moving path or path to a certain position (target position: Goal) must be planned. However, it is impossible to find an optimal solution from infinitely existing trajectories.

  Until now, the potential method (for example, see Non-Patent Document 1), the stochastic roadmap method (for example, see Non-Patent Document 1), Rapidly-exploring Random Tree (RRT) (for example, Non-Patent Document 1) Many trajectory planning methods have been proposed (see 2).

  Here, the potential method defines a potential field called “attraction potential” that brings the operation target closer to the target posture in the search space and “repulsive potential” that moves the operation target away from the obstacle, and the potential in the current posture of the operation target This is a method of planning the trajectory by repeating the procedure of calculating the field gradient and moving it slightly in the direction of decreasing potential.

  For example, an attraction potential based on the relative positional relationship between the moving object, the obstacle, and the target object using a map in the moving space generated based on the position and shape information of the moving object, the obstacle, and the target object, and A repulsive potential is calculated, a combined potential is generated, and a route search in the map is performed based on the combined potential to determine whether the convergence position of the route search is a local minimum. In the case of the minimum, a virtual potential is generated by increasing the potential at the convergence position by a predetermined value, and when the convergence position is the target position, a path generation that generates a moving path of the moving object based on the result of the path search is generated. Proposals have been made for devices (see, for example, Patent Document 1).

  Also, the probabilistic roadmap method randomly selects several postures from the arrangement space and connects them with branches if they can move to nearby postures. This is a method of performing a random walk from a certain position, extending the road map, and applying the Dijkstra method to find the shortest path in the road map.

  For example, a minimum cost route search apparatus that searches for a route from an ingress node to an egress node with a minimum cost in a network by the Dijkstra method, which is a mathematical programming method, has been proposed (see, for example, Patent Document 2). ).

  The RRT is a data structure of a tree that preferably covers the search space and a basic algorithm for generating the tree. In RRT, an initial position is first set on the search space, and then a random sample position is placed in the search space, and the nearest position from the initial position toward the sample position is searched. And add it to the tree. Thereafter, by repeating the arrangement of the sample positions for the newly added nearest neighbor position, further searching for the nearest neighbor position, and adding to the tree, a tree that covers the search space can be generated. Currently, RRT is used for real-time route planning, trajectory generation, and the like (see, for example, Non-Patent Document 3), and there are many research cases.

  Although the potential method is very intuitive, it is difficult to efficiently obtain a safe motion path because many postures with potentials other than the target posture may appear. The probabilistic roadmap method has a drawback that it is difficult to plan an action through a narrow space surrounded by many obstacles, that is, a large number of poses must be sampled. Although RRT has a drawback that the problem to be handled is not intuitive when it is multidimensional, intuition is not lost in a low-dimensional problem. Further, as another feature of RRT, it is known that relatively high-speed trajectory planning (Path Planning) is possible in a multidimensional configuration space.

  Thus, it can be seen that each conventional trajectory planning method has advantages and disadvantages. In addition, it is necessary to perform trajectory correction to smooth the trajectory generated by any of the methods or to avoid an obstacle. As methods for smoothing the trajectory, methods using minimum jerk, (for example, see Non-Patent Document 4), minimum torque, and polynomial (Spline) have been devised, but obtaining a smooth trajectory It is not possible to simultaneously satisfy the approach to the optimal trajectory and avoid permeation between objects. In other words, a satisfactory trajectory cannot be obtained for use in final control for robot movement control or trajectory planning of the articulated arm.

JP 2006-350776 A JP 2002-133351 A Jun Ota, "Introduction to Intelligent Robots-Solving Motion Planning Problems" (Corona, 2001) Steven M.M. LaVale, “Rapidly-Exploring Random Trees: A New Tool for Path Planning” (TR. 98-11, Computer Science Dpt., Iowa State University. Oct. 1998). James J.M. kuffner, Jr. , “Dynamically-stable Motion Planning for Humanoid Robots” (Autonomous Robots vol. 12, No. 1, pp. 105-118, 2002) T.A. Flash and N.C. Hogan, “The Coordination of Arm Movements: An Experimentally Consistent Mathematical Model” (The Journal of Neuroscience, Vol. 5, No. 7, 1988, No. 7, pp. 5, 1988, No. 7, pp. 88, No. 7, pp. 1988-No.

  An object of the present invention is to provide an excellent trajectory planning apparatus, trajectory planning method, and computer program capable of suitably planning a movement path or trajectory from a certain position to a certain position in a two-dimensional or three-dimensional work space. Is to provide.

  A further object of the present invention is to provide an excellent trajectory planning apparatus and trajectory planning method capable of finding a trajectory that avoids an obstacle and correcting it as a smooth trajectory so as to be easily used for movement control and trajectory planning, and To provide a computer program.

The present invention has been made in consideration of the above problems, and a first aspect thereof is a trajectory planning apparatus for planning a trajectory of an object in a search space,
Trajectory acquisition means for acquiring a trajectory composed of a plurality of nodes connecting between the current position of the object and the target position;
The trajectory acquired by the trajectory acquisition main stage is set as an initial condition, each node constituting the trajectory is treated as a mass point, and one or more required items imposed on the trajectory are expressed by force and added to each mass point. A trajectory correcting means for correcting the trajectory;
A trajectory planning apparatus.

  For example, when autonomously controlling a mobile device such as a robot or a hand that is an end effector of an articulated arm, it is necessary to plan a movement path or trajectory. It is impossible to find the optimal solution from Each conventionally proposed trajectory planning method has advantages and disadvantages. In addition, it is necessary to perform a trajectory correction for smoothing or avoiding an obstacle with respect to the trajectory generated by any method.

  On the other hand, in the trajectory planning apparatus according to the present invention, the trajectory acquisition means first acquires a trajectory composed of a plurality of nodes connecting the current position of the object and the target position using an existing trajectory planning method such as RRT. Subsequently, the trajectory correcting means treats the acquired trajectory as an initial condition, treats each node constituting the trajectory as a mass point, and expresses one or more required items imposed on the trajectory by force and adds them to each mass point. The trajectory is corrected. Specifically, a multi-mass point system with each node on the orbit discovered by RRT as a mass point expresses the demands of smoothing and optimizing the trajectory and avoiding penetration between objects by adding power to each mass point. The trajectory is corrected by executing a dynamic simulation using the position on the trajectory found by RRT as an initial condition, and a trajectory that satisfies all requirements is generated.

  Therefore, according to the present invention, one of the infinitely existing trajectories is found using RRT, and problems such as obstacle avoidance and trajectory smoothing are enhanced by using multi-mass system dynamic simulation. By simultaneously solving in the above dimensions, a trajectory that can be used in final control (that is, easy to use for movement control and trajectory planning) can be calculated with a low computational cost.

  Here, when the RRT is used, the trajectory acquisition unit calculates a trajectory between the current position and the target position based on a search tree that branches into a plurality of paths at a plurality of nodes and is expanded on the search space. Can be earned. Further, the trajectory acquisition means randomly arranges q_rand with respect to the initial position q_init in the search space, sets q_new at a constant distance in the direction from q_rand nearest node q_near to q_rand, and sets q_new to the nearest node q_new In addition, the process of adding q_new to the search tree by setting q_new at a constant distance in the direction from q_rand to q_rand in a random arrangement of q_rand and q_rand from the nearest node q_new to q_new is repeatedly executed. Thus, a search tree that suitably covers the search space can be generated.

  In addition, the trajectory discovered by the RRT described above is not necessarily the optimal solution. In order to generate a smooth trajectory from the current position to the target position, three requirements are required: obtaining a smooth trajectory, approaching the desired trajectory as much as possible, and avoiding penetration between objects. Is imposed. According to the trajectory planning apparatus according to the present invention, the trajectory correcting means has three requirements: obtaining a smooth trajectory, approaching the desired trajectory as much as possible, or avoiding penetration between objects. All of these required items are expressed as forces corresponding to each, and are integrated in the dimension of force to solve them comprehensively.

  Specifically, the trajectory correcting means connects adjacent nodes on the acquired trajectory with springs and dampers, and uses each spring and damper to satisfy the required item of acquiring a smooth trajectory. By generating the trajectory, the trajectory that satisfies the required item can be generated.

  Further, the trajectory correcting means connects each node on the acquired trajectory and a corresponding point on the desired trajectory as much as possible by using a spring and a damper, and uses each spring and damper so as to satisfy the requirement item to be as close as possible to the desired trajectory. By applying a virtual attractive force to each node, a trajectory that satisfies the required item can be generated.

  Further, the trajectory correcting means performs the shortest distance calculation between the object to be controlled and the obstacle existing in the search space, and obtains the artificial repulsive force corresponding to the repulsive potential by the calculated shortest distance. By acting on a node, the requirement item of avoiding penetration between objects is satisfied. At that time, a force due to viscous resistance for operating more stably may be applied to each node on the acquired trajectory.

According to a second aspect of the present invention, there is provided a computer program written in a computer-readable format so that a process for planning an object trajectory in a search space is executed on a computer.
Trajectory acquisition means for acquiring a trajectory composed of a plurality of nodes connecting between the current position of the object and the target position;
The trajectory acquired by the trajectory acquisition main stage is set as an initial condition, each node constituting the trajectory is treated as a mass point, and one or more required items imposed on the trajectory are expressed by force and added to each mass point. A trajectory correcting means for correcting the trajectory;
It is a computer program for making it function as.

  The computer program according to the second aspect of the present invention defines a computer program described in a computer-readable format so as to realize predetermined processing on the computer. In other words, by installing the computer program according to the second aspect of the present invention in the computer, a cooperative action is exhibited on the computer, and the same as the trajectory planning apparatus according to the first aspect of the present invention. An effect can be obtained.

  According to the present invention, an excellent trajectory planning apparatus, trajectory planning method, and computer program capable of suitably planning a movement path or trajectory from a certain position to a certain position in a two-dimensional or three-dimensional work space. Can be provided.

  In addition, according to the present invention, an excellent trajectory planning apparatus and trajectory planning method capable of finding a trajectory that avoids an obstacle and correcting it as a smooth trajectory so as to be easily used for movement control and trajectory planning, In addition, a computer program can be provided.

  The trajectory planning apparatus according to the present invention uses RRT to discover one of infinitely existing trajectories and uses a multi-mass point system dynamic simulation to solve problems such as obstacle avoidance and trajectory smoothness. By solving simultaneously in the dimension of force, a trajectory that can be used in final control (that is, easy to use for movement control and trajectory planning) can be calculated with a low computational cost. In addition, during the control period of robots and articulated arms, the trajectory planning device according to the present invention is used to generate trajectories as appropriate and always correct the trajectory so that not only trajectory planning but also real-time operation is possible. Obstacle avoidance can be realized.

  Other objects, features, and advantages of the present invention will become apparent from more detailed description based on embodiments of the present invention described later and the accompanying drawings.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

In order to achieve the object of generating a smooth trajectory from a certain position (current position: Start) to a certain position (target position: Goal) on the two-dimensional or three-dimensional work space, I think that 4 items are required.
(1) To obtain a trajectory for achieving a minimum objective (generating a trajectory from Start to Goal) at a high speed even if it is not an optimal solution.
(2) To acquire a smooth trajectory.
(3) As close as possible to the desired trajectory (optimal trajectory).
(4) Avoid penetration between objects.

  In the present embodiment, the requirement item (1) is solved by using an existing trajectory planning algorithm. Examples of the existing trajectory planning algorithm include a potential method, a probabilistic roadmap method, and a rapid-exploring random tree (RRT), each of which has advantages and disadvantages (described above). Of these, RRT has the disadvantage that it is not intuitive when the problem to be handled is multidimensional, but it does not lose its intuition in a low-dimensional problem. Further, as another feature of RRT, it is known that a relatively high-speed trajectory planning in a multidimensional configuration space is possible.

  Since the dimensions handled in this embodiment are low, relatively fast trajectory planning is possible as a means for obtaining a trajectory for achieving the minimum objective (1) among the above requirements. RRT is used. Therefore, in solving the other requirement items (2) to (4), it can be assumed that at least one of innumerable trajectories in the three-dimensional space is obtained.

  Here, a basic algorithm for generating an RRT tree will be described with reference to FIGS. 1A to 1G.

  First, an initial position q_init is set on the search space (see FIG. 1A).

  Next, q_rand is randomly placed on the search space (see FIG. 1B).

  Next, the node closest to q_rand in the tree (search tree) (that is, the nearest node) is set as q_near, and q_new is set to a constant distance in the direction from q_near to q_rand (see FIG. 1C). However, only q_init exists in the tree in the initial state (see FIG. 1A). Here, q_new is connected to q_new by connecting q_new and q_new (see FIG. 1D).

  Next, q_rand is re-arranged at random in the search space (see FIG. 1E), the nearest node of q_rand in Tree is set to q_near, q_new is generated from q_near and q_rand as before, and q_new is set to Tree. (See FIG. 1F).

  Then, by repeatedly performing the operations as described above, a Tree that suitably covers the search space can be generated (see FIG. 1G).

  According to RRT, a relatively high-speed trajectory plan is possible, and an appropriate trajectory can be generated even if it is not an optimal solution.

  FIG. 2 shows a trajectory when the cube to be controlled is moved from the upper right to the lower left in the figure while avoiding the central cube which is an obstacle. FIG. 3 shows a trajectory discovered by RRT with the control object hidden.

  In the execution of RRT, GJK (Gilbert-Johnson-Kerthidance algorithm) is assumed for the problem of penetration between objects when avoiding obstacles. The GJK algorithm is an iterative method for calculating the distance between convex figures, and outputs a collision representative point pair (nearest point pair or most penetrating point pair) as an interference detection result. For details of the GJK algorithm, see, for example, G.K. See van den Bergen, “Collision Detection in Interactive 3D Environments” (Morgan Kaufmann Publishers, 2004).

  As can be seen in FIGS. 2 and 3, the trajectory discovered by RRT is clearly wasteful. In other words, since the optimum solution is not obtained from the RRT, it is necessary to correct it as a smooth trajectory so that it can be easily used for the robot movement control, the trajectory planning of the hand of the articulated arm, and the like.

  In the present embodiment, a dynamic simulation of a multi-mass system is performed in order to make it as close as possible to the optimal trajectory and to avoid penetration between objects, that is, to satisfy the remaining three requirements (2) to (4). Is introduced. In other words, a multi-mass point system with each node on the trajectory discovered by RRT as a mass point expresses the demands of smoothing or optimizing the trajectory and avoiding penetration between objects, and adding them to each mass point. The trajectory that satisfies all the requirements is generated by executing the dynamic simulation using the position on the trajectory found in step 1 as the initial condition. That is, all the requirements listed as the requirement items (2) to (4) are expressed as forces corresponding to the requirements, and are integrated in the force dimension, so that they can be solved comprehensively. A method for converting the required items (2) to (4) into force will be described below.

  FIG. 4 shows a force constraint expression for obtaining a smooth trajectory of the requirement item (2). In the figure, both end points are nodes of a start position Start and a target position Goal, and a plurality of nodes connected to these are nodes on the trajectory between the start position and the target position calculated by RRT. Then, adjacent nodes are connected by a spring and a damper, and the attractive force on the smooth track is expressed using the spring and the damper. In other words, by applying an attractive force between the nodes using these springs and dampers so as to satisfy the required items, the track is converted into a lean track.

Here, the position of the n-th node Node (n) in the three-dimensional space is x _n = [x _n , y _n , z _n ] ^T , and the position of the n + 1-th node Node (n + 1) is x _{n + 1.} = [X _{n + 1} , y _{n + 1} , z _{n + 1} ] ^T , a vector from the node Node (n) to the node Node (n + 1) is represented by r _{n, n + 1} , the node Node (n) and the node Node ( n + 1), where the natural length of the spring is l _{n, n + 1} and the spring constant is k _{n, n + 1} , the spring force Fb _n, acting between the node Node (n) and the node Node (n + 1) _{n + 1} is expressed as the following formula (1).

  That is, the spring force acting between the node Node (n) and the node Node (n + 1) is represented by the product of the normalized spring force direction and the spring force.

If the damping coefficient is c _{n, n + 1} and the velocity vector is v _{n, n + 1} , the force Fd _{n, n + 1} due to the damper acting between the node Node (n) and the node Node (n + 1) is the same. Is expressed as the following formula (2).

  That is, the direction of the damping force is determined from the position between the two nodes, and the acting direction component of the relative speed determines the magnitude of the damping force and a positive / negative sign.

The acting force on the node Node (n + 1) is obtained by inverting the sign of the spring force Fb _{n, n + 1} obtained from the above equation (1) and the damping force Fdn _{, n + 1} obtained from the above equation (2). can get. Summarizing the above, the inter-node attractive force F _{a, n, n + 1} acting on the node Node (n) from the node Node (n + 1) is expressed by the following equation (3).

  FIG. 5 shows a constraint expression of force for bringing the required item (3) as close as possible to the desired trajectory. In the figure, both end points are nodes of a start position Start and a target position Goal, respectively, and a dotted line is a path that seems to be optimal from the start position Start to the target position Goal. Each point represented by x on the dotted line corresponds to a desired position of each node on the route, and is connected to the corresponding node by a spring and a damper, and optimal using the spring and the damper. Represents a virtual attractive force on the path. That is, the desired trajectory is converted as much as possible by applying a virtual attractive force to the optimum path to each node using these springs and dampers so as to satisfy the required items. In FIG. 4, the springs and dampers between the nodes shown in FIG. 4 representing the constraint of the force for obtaining a smooth trajectory are drawn with dotted lines.

Here, the optimum path position of the n-th node Node (n) in the three-dimensional space is expressed as x _{opt, n} = [x _{opt, n} , y _{opt, n} , z _{opt, n} ] ^T , node Node (n) R _{opt, n} , the relative velocity vector v _{opt, n} , the spring constant k _{opt, n} , the damping constant c _{opt, n} , the natural length of the spring and the speed of the optimal path position When both are set to 0, the virtual attractive force F _{opt, n} to the optimum route position acting on the node Node (n) is expressed as the following equation (4), similarly to the inter-node attractive force shown in the above equation (3). Is done.

  Next, a constraint expression of force imposed to avoid penetration between objects in the requirement item (4) will be described.

When executing dynamic simulation, the interference detection function is always working, and the shortest distance calculation between the controlled object and the obstacle is in operation. When the shortest distance calculated by the shortest distance calculation is ρ, the repulsive potential U ₀ (ρ) depending on the distance ρ between the controlled object and the obstacle is expressed by the following equation (5).

At this time, the artificial repulsive force F _{r, n} acting on the nth node Node (n) is expressed as in the following equation (6).

However, in the above equation (6), ρ ₀ is a threshold that represents a region where the artificial repulsive force acts, is P _b and P _a is the nearest neighbor representing the shortest distance, respectively controlled object and the obstacle. That is, d represented by the above formula (7) indicates the direction in which the normalized force is applied. The repulsive force obtained here is applied to the multi-mass point system.

In addition, in order to suppress the divergence of multi-mass system dynamics simulation and operate more stably, a force due to viscous resistance (like air resistance) is also introduced. Assuming that the speed of the n-th node Node (n) is v _n and the viscosity coefficient is γ _n , the force F _{dmp, n} due to the viscous resistance acting on the node Node (n) is expressed by the following equation (8). .

FIG. 6 shows a state in which all the external forces F _{a, n, n + 1} , F _{opt, n} and F _{r, n} are applied to the nth node Node (n), that is, the mass point n. Yes. The external force F _n acting on the node Node (n) is as shown in the following formula (9). However, in order from the left side of the right side of the equation, the viscous force, the inter-node attractive force acting on the node Node (n) from the node Node (n−1), the inter-node attractive force acting on the Node (n) from the node Node (n + 1), It represents the permeation avoidance potential repulsion and the optimum path attractive force.

All the force constraints described so far are added as the external force F of the multi-mass system, and the dynamic simulation is executed. That is, the equation of motion of the mass point is described by substituting the external force F _n shown in the above equation (9) for the equation of motion of each node (mass point) shown in the following equation (10). Then, a numerical simulation is executed by expressing a coupled motion consisting of n nodes as n simultaneous linear ordinary differential equations and performing numerical integration with the position obtained by RRT as an initial condition. As the numerical integration, for example, the Runge-Kutta method can be used. Note that the force is calculated for each numerical simulation step.

  7 and 8 show simulation examples in the case where the force constraint is applied to each node on the trajectory. 7A and 7B show paths discovered by the RRT, and FIGS. 8A and 8B show results of correcting the trajectory by applying force constraints to the trajectory. As can be seen from these figures, it can be confirmed that the initial route discovered by the RRT has been smoothed as required by the requirement items (2) to (4). The optimal route shown in the figure is set as a straight line connecting Start and Goal.

  FIG. 9 shows a system configuration for realizing the trajectory planning method described so far in the form of a functional block diagram. The illustrated system 10 includes an RRT execution unit 11, an interference detection unit 12, an external force calculation unit 13, and a multi-mass system dynamic simulation execution unit 14. For example, the functional configuration shown in the figure can be realized by executing a predetermined program on a general computer system such as a personal computer.

  Upon receiving the route planning command, the RRT execution unit 11 generates a route from the obstacle information on the three-dimensional search space, the current position (Start) and the target position (Goal) position of the controlled object. At that time, by using the interference detection unit 12 that performs the interference detection and the shortest distance calculation, the penetration at the time of generating the orbit (Path) is taken into consideration. That is, RRT performs trajectory generation without interference.

  The external force calculation unit 13 avoids infiltration of an inter-node attractive force calculation unit 13A that calculates an inter-node attractive force for facilitating the activation generated by the RRT execution unit 11, and an obstacle present in the space and the control target. An obstacle avoiding repulsive force calculating unit 13B, and an optimal route attractive force calculating unit 13C for calculating an optimal route attractive force to be applied to approach the desired route, and external forces such as attractive force and repulsive force calculated respectively. Is output.

  The multi-mass point system dynamic simulation execution unit 14 adds the external force at each node calculated by the external force calculation unit 13 as an external force constraint for each node. The multi-mass point system dynamics simulation execution unit 14 performs dynamics calculation for each simulation cycle.

  Even when executing a dynamic simulation of a multi-mass point system, the interference detection unit 12 is used for detecting penetration between objects or calculating the shortest distance between objects. The dynamic simulation of the multi-mass point system ends when each node converges to a set state (for example, the stationary state of an object), and becomes a smooth path obtained by the node group at that time.

  FIG. 10 shows the procedure of the trajectory planning process executed by the trajectory planning system 10 shown in FIG. 9 in the form of a flowchart.

  First, the RRT execution unit 11 receives a route planning command, and generates a route from the obstacle information on the three-dimensional search space, the current position (Start) and the target position (Goal) of the controlled object (Step S1). ). And the produced | generated path | route is handed over to the subsequent external force calculation process.

  Next, in the external force calculation process, external force added to each node on the path is calculated in each simulation cycle (step S2).

  Next, the multi-mass point system execution unit 14 executes a simulation (step S3).

  The multi-mass point system dynamic simulation described above is repeated until the node static conditions set by each node are satisfied (step S4).

  When the trajectory planning of the object is performed by the trajectory planning system 10 shown in FIG. 9, the results as shown in FIG. 7 and FIG. 8 can be obtained. That is, it is easy to acquire a moving route in space by RRT at high speed, smooth the acquired route, and approach the desired route (optimum route).

  In addition, the computation in the dynamic simulation of a multi-mass system can greatly reduce the amount of computation compared to the dynamic simulation of a rigid system. By always operating, it is possible to correct the trajectory such as obstacle avoidance in real time.

  FIG. 11 shows the result of obstacle avoidance performed in real time by always operating the dynamic simulation of the multi-mass point system when executing the object movement control. FIG. 11A shows a state in which a dynamic simulation of a multi-mass point system is executed until the node is settled after executing the RRT to acquire the start as an initial condition. Further, FIG. 11B shows a correction state of the trajectory when an obstacle in the space moves after the node is settled.

  The present invention has been described in detail above with reference to specific embodiments. However, it is obvious that those skilled in the art can make modifications and substitutions of the embodiment without departing from the gist of the present invention.

  The present invention can be applied to the movement control of a robot and the trajectory control of an end effector of an articulated arm.

  In short, the present invention has been disclosed in the form of exemplification, and the description of the present specification should not be interpreted in a limited manner. In order to determine the gist of the present invention, the claims should be taken into consideration.

FIG. 1A is a diagram for explaining a basic algorithm for generating an RRT tree. FIG. 1B is a diagram for explaining a basic algorithm for generating an RRT tree. FIG. 1C is a diagram for explaining a basic algorithm for generating an RRT tree. FIG. 1D is a diagram for explaining a basic algorithm for generating an RRT tree. FIG. 1E is a diagram for explaining a basic algorithm for generating an RRT tree. FIG. 1F is a diagram for explaining a basic algorithm for generating an RRT tree. FIG. 1G is a diagram for explaining a basic algorithm for generating an RRT tree. FIG. 2 is a diagram showing a state in which the cube to be controlled is moved from the upper right to the lower left in the same figure while avoiding the central cube which is an obstacle. FIG. 3 is a diagram showing a trajectory discovered by RRT with the control object hidden. FIG. 4 is a diagram for explaining a force constraint expression for obtaining a smooth trajectory of the requirement item (2). FIG. 5 is a diagram for explaining a force constraint expression for bringing the required item (3) as close as possible to the desired trajectory. FIG. 6 is a diagram illustrating a state in which all external forces F _{a, n, n + 1} , F _{opt, n} and F _{r, n} are applied to the nth node Node (n). FIG. 7A is a diagram showing a route discovered by the RRT. FIG. 7B is a diagram showing a route discovered by the RRT. FIG. 8A is a diagram showing a result of correcting the trajectory by applying a force constraint to the trajectory. FIG. 8B is a diagram illustrating a result of correcting the trajectory by applying a force constraint to the trajectory. FIG. 9 is a functional block diagram showing a system configuration for realizing the trajectory planning method according to the present invention. FIG. 10 is a flowchart showing the procedure of the trajectory planning process executed by the trajectory planning system 10 shown in FIG. FIG. 11A is a diagram showing a state in which a dynamic simulation of a multi-mass point system is executed until a node is settled after executing an RRT to acquire activation as an initial condition. FIG. 11B is a diagram illustrating a correction state of the trajectory when an obstacle in the space moves after the node is settled.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 ... Trajectory planning system 11 ... RRT execution part 12 ... Interference detection part 13 ... External force calculation part 13A ... Internode attractive force calculation part 13B ... Obstacle avoidance repulsive force calculation part 13C ... Optimal path attractive force calculation part 14 ... Multi-mass point system execution part

Claims (19)

A trajectory planning device for planning a trajectory of an object in a search space,
Trajectory acquisition means for acquiring a trajectory composed of a plurality of nodes connecting between the current position of the object and the target position;
The trajectory acquired by the trajectory acquisition main stage is set as an initial condition, each node constituting the trajectory is treated as a mass point, and one or more required items imposed on the trajectory are expressed by force and added to each mass point. A trajectory correcting means for correcting the trajectory;
A trajectory planning apparatus comprising: The trajectory acquisition means acquires a trajectory between the current position and the target position based on a search tree that branches into a plurality of paths at a plurality of nodes and is expanded on the search space.
The trajectory planning apparatus according to claim 1. The trajectory acquisition means randomly arranges q_rand with respect to the initial position q_init in the search space, sets q_new at a constant distance in the direction from q_rand's nearest node q_new to q_rand, and sets q_new to the nearest node q_new In addition, for q_new, q_new is randomly arranged and q_new is set to a constant distance in the direction from q_rand nearest node q_new to q_rand and added to the search tree repeatedly. To generate the search tree,
The trajectory planning apparatus according to claim 2. The trajectory correcting means adds a force expressing one or more requirement items imposed on the trajectory to a multi-mass point system having each node on the trajectory acquired by the trajectory acquiring means as a power. A trajectory that satisfies the above requirements is generated by performing a physical simulation.
The trajectory planning apparatus according to claim 1. The trajectory correcting means uses at least one of obtaining a smooth trajectory, approaching a desired trajectory as much as possible, or avoiding permeation between objects as a required item, and expressing a force expressing the required item. Added to each mass point corresponding to a node on the trajectory,
The trajectory planning apparatus according to claim 1. The trajectory correcting means connects adjacent nodes on the trajectory acquired by the trajectory acquiring means with springs and dampers, and uses the springs and dampers to satisfy the required items of acquiring a smooth trajectory between the nodes. By generating an orbit that satisfies the above requirements by applying an attractive force,
The trajectory planning apparatus according to claim 5. The trajectory correcting means connects each node on the trajectory acquired by the trajectory acquiring means and a corresponding point on the desired trajectory as much as possible by a spring and a damper, and satisfies each requirement item to make it as close as possible to the desired trajectory. And generating a trajectory that satisfies the required items by applying a virtual attractive force to each node using a damper.
The trajectory planning apparatus according to claim 5. The trajectory correcting means performs a shortest distance calculation between the object and an obstacle existing in the search space, and on the trajectory obtained by the trajectory acquiring means, an artificial repulsive force corresponding to the repulsive potential due to the calculated shortest distance. Satisfy the requirement of avoiding penetration between objects by acting on each node of
The trajectory planning apparatus according to claim 5. The trajectory correcting means acts on each node on the trajectory acquired by the trajectory acquiring means together with a force due to viscous resistance for operating more stably.
The trajectory planning apparatus according to claim 8. A trajectory planning method for planning a trajectory of an object in a search space,
A trajectory acquisition step for acquiring a trajectory composed of a plurality of nodes connecting between the current position of the object and the target position;
The trajectory acquired in the trajectory acquisition step is set as an initial condition, each node constituting the trajectory is treated as a mass point, one or more required items imposed on the trajectory are expressed by force, and the trajectory is added to each mass point. A trajectory correction step for correcting
A trajectory planning method comprising: In the trajectory acquisition step, a trajectory between the current position and the target position is acquired based on a search tree branched into a plurality of paths at a plurality of nodes and expanded on the search space.
The trajectory planning method according to claim 10. In the trajectory acquisition step, q_rand is randomly arranged with respect to the initial position q_init on the search space, q_new is set to a constant distance in the direction from q_nea nearest node q_near to q_rand, and q_new is set to the nearest node q_new In addition, for q_new, q_new is randomly arranged and q_new is set to a constant distance in the direction from q_rand nearest node q_new to q_rand and added to the search tree repeatedly. To generate the search tree,
The trajectory planning method according to claim 11. In the trajectory correction step, a force expressing one or more required items imposed on the trajectory is added to a multi-mass point system having each node on the trajectory obtained in the trajectory acquisition step as A trajectory that satisfies the above requirements is generated by performing a physical simulation.
The trajectory planning method according to claim 10. In the trajectory correcting step, the force expressing the required item is set as at least one of obtaining a smooth trajectory, approaching the desired trajectory as much as possible, or avoiding penetration between objects. Added to each mass point corresponding to a node on the trajectory,
The trajectory planning method according to claim 10. In the trajectory correction step, adjacent nodes on the trajectory acquired in the trajectory acquisition step are connected by springs and dampers, and between the nodes using each spring and damper so as to satisfy the requirement item of acquiring a smooth trajectory. By generating an orbit that satisfies the above requirements by applying an attractive force,
The trajectory planning method according to claim 14. In the trajectory correction step, each node on the trajectory acquired in the trajectory acquisition step is connected to a corresponding point on the desired trajectory as much as possible by a spring and a damper, and each spring is satisfied so as to satisfy the requirement item to be as close as possible to the desired trajectory. And generating a trajectory that satisfies the required items by applying a virtual attractive force to each node using a damper.
The trajectory planning method according to claim 14. In the trajectory correction step, a shortest distance calculation is performed between the object and an obstacle existing in the search space, and an artificial repulsive force corresponding to the repulsive potential due to the calculated shortest distance is acquired on the trajectory. Satisfy the requirement of avoiding penetration between objects by acting on each node of
The trajectory planning method according to claim 14. In the trajectory correction step, a force due to viscous resistance is applied to each node on the trajectory acquired in the trajectory acquisition step, in order to operate more stably.
The trajectory planning method according to claim 17. A computer program written in a computer-readable format to execute on a computer a process for planning a trajectory of an object in a search space, the computer comprising:
Trajectory acquisition means for acquiring a trajectory composed of a plurality of nodes connecting between the current position of the object and the target position;
The trajectory acquired by the trajectory acquisition main stage is set as an initial condition, each node constituting the trajectory is treated as a mass point, and one or more required items imposed on the trajectory are expressed by force and added to each mass point. A trajectory correcting means for correcting the trajectory;
Computer program to function as