Trajectory planning method, trajectory planning system and robot

摘要

A trajectory planning system obtains a trajectory for controlling a state of an object toward a goal state. The system includes a search tree generating section which registers a state of the object as a root of a search tree in a state space, registers a next state of the object after a lapse of a predetermined time interval obtained through dynamical relationships during the time interval as a branch of the search tree in the state space. The system further includes a known-state registration tree storing section which stores a known-state registration tree and a known-state registration tree generating section which determines a cell to which the next state belongs among a plurality of cells previously prepared by segmenting the state space, determines whether or not a state which belongs to the cell has already been registered as a branch of the known-state registration tree, discards the next state when a state which belongs to the cell has been registered, and registers the next step as a branch of the known-state registration tree when a state which belongs to the cell has not been registered. The system further includes a trajectory generating section which selects a state whose distance to the goal state is minimum among states registered as branches of the known-state registration tree and obtains a trajectory using a sequence of states in a backward direction from the state toward the root of the known-state registration tree.

主权项

1. A trajectory planning method for determining a trajectory for controlling a state of a target device from an initial state toward a goal state, the method performed by a computer in a trajectory planning system, the computer comprising hardware, the computer configured to perform the method comprising steps of:
a. dividing a state space for representing states of the target device into cells; b. determining the initial state of the target device as a root of a search tree in the state space; c. extending the search tree starting from the root, by determining, as a new branch of the search tree, an edge connecting one node indicative of one state of the target device with another node indicative of a next state which the target device reaches after a predetermined time period as a result of that one of possible inputs is applied to the target device within the predetermined time period; d. finding out a cell containing the next state, and determining whether the cell containing the next state contains any other state which is already stored as a branch of the search tree; e. discarding the new branch determined at step c if the cell containing the next state contains any other state already stored as a branch of the search tree, and, if the cell containing the next state does not contain any other state already stored as a branch of the search tree, storing the new branch determined at step c; f. executing step c through step e repeatedly until a predefined search termination condition is met; g. selecting, after the predefined search termination condition is met, one state as a best state from the states each indicated by the respective stored node and having a distance in the state space to the goal state less than a predetermined value, and determining one set of the stored branches of the search tree connecting the initial state with the best state; and h. determining a trajectory of the target device from the initial state towards the goal state using a sequence of states indicated by the determined one set of the stored branches of the search tree.