JP5778467B2 - Trajectory planning method, trajectory control method, trajectory planning system, and trajectory planning / control system - Google Patents

Description

The present invention relates to a trajectory planning method, a trajectory planning system, a trajectory control method for controlling the object according to the created trajectory, and a trajectory planning / control system.

  The state of the machine (posture / speed) can be determined as a position on the phase space, and the movement of the machine can be determined as a path on the phase space. Sometimes you want to control the machine from its initial state to its target state. Specific examples are as follows.

The first specific example is a case where a biped robot loses its balance. The initial state in this case is a posture / speed immediately after the balance is lost, and the target state is a stable posture (an upright posture and a speed of 0, etc.).

The second specific example is a case where the aircraft loses balance and stalls. The initial state in this case is the posture / speed immediately after the balance is lost, and the target state is a state in which the wing and the body are leveled and the wing and the body move forward at a constant speed.
As other specific examples, there is a case where the balance is lost when the motorcycle hits a stone while traveling and the posture is to be corrected, or the vehicle slips.

  When controlling the machine from the initial state to the target state, if PID control or the potential method is used, there is a problem that the machine does not function properly depending on the state after the balance is lost. This problem is due to the fact that these control methods cannot take a wide range of initial states in the machine state transition to the target state in the phase space. Further, in the PID control and the potential method, it is difficult to define the time until the machine reaches the target state, and it is difficult to return the target state in a short time.

As a method of creating a state space trajectory plan, there is a method of describing a state transition in the state space as a search tree. In this method, it is conceivable to derive a predetermined number of branches from roots or branches and search for each branch. However, the number of branches to be derived is N, when the depth of the tree is M, the number of branches to be searched at the level of the depth M of the tree, the N ^M. Therefore, when searching for all branches to be derived, the calculation cost is enormous.

  Therefore, a method has been proposed in which an evaluation function for a branch of a search tree is determined and a branch is derived only from a branch selected according to the evaluation function (Non-Patent Document 1). According to this method, the calculation cost can be suppressed. However, the evaluation function needs to be determined by the designer for each target based on experience. Depending on the evaluation function, the search in the state space may not be sufficiently performed. That is, in this method, there may be restrictions on the search of the state space.

  The conventional method using the search tree including the above method has a calculation cost (calculation time) that is too large to be used for real-time trajectory planning and control, or there are restrictions on the search. Further, in the conventional method, it is difficult to determine the trajectory from the initial state of the object to the target state with high accuracy.

Pedro S. Huang: “Planning For Dynamic Motions Using a Search Tree”, a graduate thesis of Toronto Univ., (1996).

Therefore, a trajectory planning method, trajectory planning system, and trajectory obtained by which trajectories from various initial states to a target state of an object, that is, a machine, can be obtained with high calculation accuracy and high accuracy without any restriction on search. Therefore, there is a need for a trajectory control method and a trajectory planning / control system that can control an object so as to reach a target state.

A trajectory planning method according to a first aspect of the present invention is a trajectory planning method for obtaining a trajectory for controlling a state of an object to a target state by a trajectory planning system, wherein a search tree creation unit includes the target state as a root, and Luz step to a reverse search tree that aggregates branches by limiting the number of each of the branches included in the area node in advance a plurality of zones to split the state space, If the transition possible area determination unit is within that area with respect to a point on the backward search tree in the state space, a transition possible area that can reach the target state according to the backward search tree is determined. And determining a trajectory of the object from the point in the transitionable region to the root in the state space using the backward search tree.

  In the trajectory planning method of this aspect, there is no restriction on the search. In addition, since the backward search tree is aggregated using the area, the calculation cost is low compared to the conventional method. Furthermore, since the root of the backward search tree matches the target state, the accuracy of the trajectory having the target state as the end point is high. Therefore, according to the trajectory planning method of this aspect, the trajectory from the point in the region where the point on the backward search tree can be transferred to the target state is obtained with low calculation cost and high accuracy without any restriction on the search. be able to.

According to the first embodiment of the first aspect of the present invention, the search tree creating unit includes, in the state space, the branches included in the respective sections of the state space with the initial state of the object as a root. Conversely the absence steps to create a forward search tree that aggregates branches by limiting the number of nodes, the path generation unit, in any of the transition area, a track by the forward search tree the Connecting a trajectory by a direction search tree to determine a trajectory of the object from the initial state to the target state.

According to the present embodiment, the trajectory from the initial state to the target state of the object can be determined by connecting the trajectory based on the forward search tree and the trajectory based on the backward search tree in the transferable region. The backward search tree roots, equal to the target state, the forward search tree roots, so to match the initial state, the accuracy of the orbit you an initial state as a starting point to the goal state and the end point is high.

  According to the second embodiment of the first aspect of the present invention, in the step of determining the migratable area, the migratable area determining unit is configured for a plurality of points in the vicinity of the point on the backward search tree. Thus, by performing feedback control, it is determined whether or not the target state can be reached, and the shiftable region is determined based on the result.

  According to the present embodiment, it is possible to reliably determine the transferable region by performing the feedback control.

According to 3rd Embodiment of the 1st aspect of this invention, the said transferable area | region determination part is the most on the point on the said reverse search tree among the points which could not reach the said target state. The migratable area is determined based on the distance of a close point to a point on the backward search tree.

  According to the present embodiment, by using the distance to the point on the backward search tree that is closest to the point on the backward search tree among the points that could not reach the target state, The migratable area can be determined with high accuracy.

  The trajectory control method according to the second aspect of the present invention sets the state of the object to the target state by controlling the object according to the trajectory obtained by the trajectory planning method according to the first aspect of the present invention.

  According to the trajectory control method according to this aspect, the trajectory up to the target state can be obtained with a low calculation cost and high accuracy without any restriction on the search, and the state of the object can be set as the target state according to the trajectory.

  A trajectory planning system according to a third aspect of the present invention is a trajectory planning system for obtaining a trajectory for controlling the state of an object to a target state. A search tree creation unit that creates a backward search tree in which branches are aggregated by limiting the number of branch nodes included in each area of the divided state space; and on the backward search tree in the state space If the point is within that region, the transferable region determination unit that determines the transferable region that can reach the target state according to the backward search tree, and the search that stores the search tree and the transferable region A tree storage unit, and a trajectory creation unit that determines a trajectory of the object from a point in the transitionable region to the root in the state space using the backward search tree.

  In the trajectory planning system of this aspect, there are no restrictions on the search. In addition, since the backward search tree is aggregated using the area, the calculation cost is low compared to the conventional method. Furthermore, since the root of the backward search tree matches the target state, the accuracy of the trajectory having the target state as the end point is high. Therefore, according to the trajectory planning system of this aspect, the trajectory from the point in the region where the point on the backward search tree can be transferred to the target state is obtained with low calculation cost and high accuracy without any restriction on the search. be able to.

  In the trajectory planning system according to the first embodiment of the third aspect of the present invention, the search tree creation unit is included in each area of the state space, with the initial state of the object as a root in the state space. A forward search tree in which branches are aggregated by limiting the number of nodes of the branch to be generated, and the trajectory creation unit includes the trajectory and the backward search by the forward search tree in any of the transitionable regions. A trajectory by a tree is connected to determine a trajectory from the initial state to the target state of the object.

According to the present embodiment, the trajectory from the initial state to the target state of the object can be determined by connecting the trajectory based on the forward search tree and the trajectory based on the backward search tree in the transferable region. The backward search tree roots, equal to the target state, the forward search tree roots, so to match the initial state, the accuracy of the orbit you an initial state as a starting point to the goal state and the end point is high.

  A trajectory planning / control system according to a fourth aspect of the present invention includes a trajectory planning system according to the second aspect of the present invention and controlling the object according to the trajectory obtained by the trajectory planning system. A trajectory control unit that sets the state to the target state.

  According to the trajectory planning / control system of this aspect, the trajectory up to the target state can be obtained with a low calculation cost and high accuracy without restrictions on the search, and the state of the object can be set as the target state according to the trajectory. it can.

It is a figure which shows the structure of the track | orbit planning / control system by one Embodiment of this invention. It is a figure which shows the structure of a double inverted pendulum. It is a flowchart which shows the procedure in which a search tree preparation part produces a search tree on phase space. It is a figure for demonstrating operation | movement of step S1020 and step S1030 of FIG. It is a figure which shows the phase space divided | segmented into the several area. It is a figure for demonstrating operation | movement of step S1050 of FIG. 3, and step S1060. It is a flowchart for demonstrating the method in which a transferable area | region determination part determines a transferable area. It is a figure for demonstrating operation | movement of step S2020 of FIG. It is a figure for demonstrating operation | movement of step S2030 and step S2040 of FIG. It is a figure for demonstrating step S2060 of FIG. It is a flowchart for demonstrating the creation method of a track | orbit. It is a flowchart for demonstrating feedback control. It is a figure for demonstrating operation | movement of step S4010 thru | or step S4050 of FIG. It is a figure which shows the target state of the double inverted pendulum 200. FIG. It is a figure which shows the locus | trajectory by the reverse search tree calculated | required by the method shown in the flowchart of FIG. 3, and the transferable area | region created by the method shown in the flowchart of FIG. It is a figure which shows the state which connected the locus | trajectory by a reverse direction search tree shown in FIG. 15, and the locus | trajectory by a forward direction search tree in a transferable area | region. It is a figure which shows a locus | trajectory at the time of performing feedback control toward the target state from the transition point of the locus | trajectory by a forward direction search tree.

  FIG. 1 is a diagram showing a configuration of a trajectory planning / control system 100 according to an embodiment of the present invention. The trajectory planning / control system 100 includes a search tree creation unit 101 that creates a search tree having branches corresponding to the state of an object in the state space, and a migratable area determination unit that determines a migratable area for points on the search tree. 103, a search tree storage unit 104 that stores a search tree created by the search tree creation unit 101 and a migratable area, and a target state using the search tree and migratable area stored in the search tree memory unit 104 A trajectory creation unit 105 that creates the trajectory of the trajectory, and a trajectory control unit 107 that performs trajectory control using the trajectory created by the trajectory creation unit 105. The transferable area of points on the search tree will be described later. Details of the function of each component will be described later.

  FIG. 2 is a diagram showing a configuration of the double inverted pendulum 200. As shown in FIG. In the following embodiments, the description will be made with the double inverted pendulum 200 moving in a two-dimensional plane as an object. The double inverted pendulum 200 includes a first link 205, a second link 211, a first joint (joint) 201 that rotatably connects the first link 205 to a fixed point, a first link 205, and a second link 211. , And a second joint 207 that rotatably connects. The centers of gravity of the first link 205 and the second link 211 are indicated by reference numerals 203 and 209, respectively.

第２リンク２１１の第１リンク２０５に対する角度

及びこれらの角速度

によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採用する。

The motion of the double inverted pendulum 200 is determined by the torque applied to the first joint 201 and the second joint 207, and the angle of the first link 205 with respect to the X axis.

Angle of the second link 211 with respect to the first link 205

And their angular velocities

Is represented by Therefore, in the present embodiment, the following phase space is adopted as the state space.

  FIG. 3 is a flowchart showing a procedure for the search tree creation unit 101 to create a search tree in the phase space.

  In step S1010 of FIG. 3, the search tree creation unit 101 obtains the target state of the object.

  In step S1020 of FIG. 3, the search tree creating unit 101 determines the target state of the object as the root of the search tree in the phase space.

In step S1030 of FIG. 3, the search tree creating unit 101 determines a state before a predetermined time interval, that is, at time t−Δt, as a node of the search tree branch in the phase space. A branch node is a point at which a branch branches or a point at the tip of a branch. The state before the predetermined time interval is determined by an equation of motion or a dynamic simulator. As described above, since the motion of the double inverted pendulum 200 is determined by the torque applied to the first joint 201 and the second joint 207, the torque applied to the first joint 201 and the second joint 207 can be determined at a predetermined time interval. For example, the state of the double inverted pendulum 200 before a predetermined time interval is determined. Therefore, while changing the torque applied to the first joint 201 and the second joint 207 at a predetermined time interval between a maximum value and a minimum value in a predetermined increment, the state before the time interval corresponding to each case is changed. You may ask for it. In this case, an exhaustive search may be performed by a breadth-first search while changing the torque between a maximum value and a minimum value in a predetermined increment.

が与えられたとして、３個の異なる所定の時間間隔前の状態が計算される。探索木作成部１０１は、これらの状態を探索木の枝とする。さらに、生成された枝が示すそれぞれの状態に対して、３個の異なるトルク

が与えられたとして、３個の異なる、さらに所定の時間間隔前の状態が計算される。 FIG. 4 is a diagram for explaining the operations in steps S1020 and S1030 in FIG. In FIG. 4, a circle indicates a state, and a black circle indicates a target state. A dotted arrow indicates the order of calculation. FIG. 4 shows the phase space as a two-dimensional space for simplicity. The double inverted pendulum 200 in the target state indicated by the black circle has three different torques at a predetermined time interval until the target state is reached.

Is given, the state before three different predetermined time intervals is calculated. The search tree creation unit 101 sets these states as search tree branches. In addition, three different torques for each state indicated by the generated branch

, Three different states before a predetermined time interval are calculated.

  Here, the phase space is divided into a plurality of areas in advance.

  FIG. 5 is a diagram showing a phase space divided into a plurality of areas. The division may equally divide the assumed angle and angular velocity range. Hereinafter, each of the plurality of areas is referred to as a cell.

  In step S1040 of FIG. 3, the search tree creation unit 101 determines whether a node of a search tree branch is already registered in a cell to which a state before a predetermined time interval belongs. If the branch node of the search tree has already been registered, the process proceeds to step S1060. If no node of the branch of the search tree is registered, the process proceeds to step S1045.

In step S <b> 1045 of FIG. 3, the search tree creation unit 101 determines whether the state before a predetermined time interval represented by the branch derived by the search tree creation unit 101 satisfies the constraint condition on the phase space. The constraint condition in the phase space is a constraint condition corresponding to a region in the phase space where a state cannot exist due to the performance of a machine that is an object or an external obstacle. If the state before the predetermined time interval satisfies the constraints on the phase space, the process proceeds to step S 1 050. If the state before the predetermined time interval does not satisfy the constraints on the phase space, the process proceeds to step S 1 060.

  In step S1050 of FIG. 3, the search tree creation unit 101 registers a state before a predetermined time interval represented by the branch derived by the search tree creation unit 101 as a node of the search tree branch.

In step S1060 of FIG. 3, the search tree creating unit 101 discards the state before a predetermined time interval represented by the branch derived by the search tree creating unit 101.

  FIG. 6 is a diagram for explaining the operations in step S1050 and step S1060 in FIG. In FIG. 6, a circle indicates a state, and a black circle indicates a target state. A solid arrow indicates a state transition, and a dotted arrow indicates a calculation order. The direction of the state transition indicated by the solid line and the direction of the calculation order indicated by the dotted line are opposite. As a rule, there is only one node in the branch of the search tree in one cell. When the search tree creation unit 101 determines a new branch node in a cell in which a search tree branch node already exists, the branch (state) is discarded. That is, the state where the position in the phase space is close is aggregated for each cell. In this way, the calculation cost can be greatly reduced by aggregating the branches of the conventional search tree using cells.

  In step S1070 of FIG. 3, the search tree creation unit 101 determines whether the search is completed. The search tree creation unit 101 may determine that the search has ended when the search to a predetermined depth of the search tree is completed, or when the search is completed up to a predetermined time transition. If it is determined that the search has ended, the process ends. If it is determined that the search has not ended, the process returns to step S1030 and the search is continued.

  The search tree created by the procedure shown in FIG. 3 uses a target state as a root and a state at a past time as a branch node. Since the direction from the root of the search tree to the tip of the branch is opposite to the time progression direction, it is called a reverse search tree. The root of the search tree, that is, the target state can be reached by following the branches in the direction of time progression, that is, the forward direction, from a point on the backward search tree. Thus, the backward search tree provides a trajectory from the state represented by an arbitrary point on the search tree to the target state.

  Here, a transferable area of points on the search tree will be described. The migratable area is a set of migratable points on the state space. The transferable point is a point that can reach the target state along the trajectory of the search tree by appropriately controlling the state represented by the point.

  FIG. 7 is a flowchart for explaining a method by which the migratable area determination unit 103 determines the migratable area.

  In step S2010 of FIG. 7, the migratable area determination unit 103 sets the index j to 0.

In step S2020 in FIG. 7, the transferable area determination unit 103 creates S _i ′ in which an error is randomly added to the position and speed for the point S _i on the search tree.

としたとき、探索木上の点

に対して、ランダムな誤差（Ｒ_１，Ｒ_２）を加えて点

を生成する。ここで、

である。 FIG. 8 is a diagram for explaining the operation in step S2020 of FIG. In FIG. 8, S _k indicates a target state, and a solid line reaching the target state indicates a search tree. Position and speed

The point on the search tree

Random errors (R ₁ , R ₂ )

Is generated. here,

It is.

In step S2030 of FIG. 7, the transferable area determination unit 103 performs feedback control on S _i ′ and determines whether the target state S _k can be reached. If not reachable, the process proceeds to step S2040. If reachable, the process proceeds to step S2050. The feedback control will be described later.

In step S2040 of FIG. 7, the migratable area determination unit 103 registers S _i ′ in the non-migratable point list M _l .

FIG. 9 is a diagram for explaining the operations in steps S2030 and S2040 in FIG. If the distance between S ′ _igoal and S _k is greater than or equal to the threshold Const with respect to the point S ′ _igoal after feedback control is performed from S _i ′ to the target state S _k , S _i ′ cannot be transferred. A point is registered in the list M ₁ of points that cannot be transferred.

In step S2050 of FIG. 7, the migratable area determination unit 103 determines whether the index j has reached N. Here, N is the number of points S _i ′ where it is determined whether or not migration is possible. If the index j has reached N, the process proceeds to step S2060. If j has not reached N, the process returns to step S2020.

In step S2060 of FIG. 7, the transferable area determining unit 103 sets a hypersphere or a rectangular parallelepiped having a radius R _i bounded by a point closest to S _i in the non-transferable point list M _l as a transferable area. Determine.

FIG. 10 is a diagram for explaining step S2060 in FIG. Since a hypersphere having a radius R _i whose boundary is the point closest to S _i in M _l is defined, a point existing within the radius R _i of the supersphere reaches the target state S _k by applying feedback control. Can be made.

  In step S2070 of FIG. 7, the transferable area determination unit 103 determines whether or not the index i has reached K. Here, K is the number of points that define the transferable area. If the index i has reached K, the process is terminated. If i has not reached K, the process returns to step S2090.

  In step S2080 of FIG. 7, the transferable area determination unit 103 adds 1 to the index j.

  In step S2090 of FIG. 7, the migratable area determination unit 103 adds 1 to the index i and sets the index j to 0.

  Next, a method for creating a trajectory will be described.

  FIG. 11 is a flowchart for explaining an example of a trajectory creation method.

  In step S3010 of FIG. 11, the search tree creation unit 101 creates a backward search tree according to the method shown in the flowchart of FIG. Note that the backward search tree may be prepared in advance and stored in the search tree storage unit 104.

  In step S3020 of FIG. 11, the transferable area determining unit 103 determines the transferable area of each point on the backward search tree according to the method shown in the flowchart of FIG.

  In step S3025 of FIG. 11, the transferable area determination unit 103 determines whether the initial state of the object is included in any transferable area. If the initial state of the object is not included in any transferable area, the process proceeds to step S3030. If the initial state of the object is included in any of the transferable areas, the process proceeds to step S3055.

  In step S3030 of FIG. 11, the search tree creating unit 101 creates a forward search tree with a certain point on the backward search tree as a target state. The forward search tree is a search tree in which branches are grown as time progresses with the initial state of the object as a root. The forward search tree can be created by the same method as the backward search tree creation method shown in FIG. Similar to the backward search tree, branches are aggregated using cells.

  Instead of creating a forward search tree with a certain point on the backward search tree as a target state, a tree continuing near the target state from a previously created forward search tree may be searched.

  In step S3040 of FIG. 11, the trajectory creation unit 105 determines whether the target state of the forward search tree, that is, the end point of the trajectory is within the point transferable region on the reverse search tree. If it is not in the transferable area, the process returns to step S3030. If it is within the transferable area, the process proceeds to step S3050.

  In step S3050 in FIG. 11, the trajectory creation unit 105 connects the trajectory based on the forward search tree rooted in the initial state of the object and the trajectory based on the reverse search tree rooted in the target state of the object in the migratable region. To determine the overall trajectory.

  In step S3055 of FIG. 11, the trajectory creation unit 105 determines a trajectory based on a backward search tree rooted at the target state of the object.

  In step S3060 of FIG. 11, the trajectory control unit 107 generates a motion of the object by feedback control. Feedback control will be described later.

  The trajectory using the backward search tree rooted in the target state has high accuracy regarding the target state. On the other hand, the trajectory using the forward search tree rooted in the initial state (current state) has high accuracy with respect to the initial state. Therefore, the trajectory connecting the trajectory using the backward search tree and the trajectory using the forward search tree has high accuracy regarding the target state and the initial state.

  Here, feedback control will be described.

  FIG. 12 is a flowchart for explaining feedback control.

  FIG. 13 is a diagram for explaining the operation of the flowchart of FIG. In FIG. 13, A shows the trajectory of the search tree, and B shows the trajectory by feedback control.

  In step S4010 of FIG. 12, the trajectory control unit 107 acquires the short-term target state Splan (t + n) n steps ahead from the branch of the search tree.

  In step S4020 of FIG. 12, the trajectory control unit 107 applies a random torque (torque Tcomp selected from a uniform random number within a fixed section centered on the planned torque Tplan) from the current state Scur (t). The state Spred (t + n) after n steps has elapsed is obtained.

  In step S4030 of FIG. 12, the trajectory control unit 107 calculates an error between Spred (t + n) and Splan (t + n).

  In step S4040 of FIG. 12, the trajectory control unit 107 determines whether a predetermined number of error calculations have been completed. The predetermined number of times is 200 times as an example. If the predetermined number of error calculations have been completed, the process proceeds to step S4050. If the predetermined number of error calculations have not been completed, the process returns to step S4020.

  In step S4050 of FIG. 12, the trajectory control unit 107 obtains Tplan + Tcomp that minimizes the error, transmits it to the actuator of the machine as a torque target value, maintains the same torque until one step elapses, and ends the process.

  When the backward search tree of step S3010 in FIG. 11 is prepared in advance, the calculation of step S3030 is not necessary, and a tree that is close to the target state from the prepared (prepared) forward search tree is obtained. Find it.

  In the present specification, the trajectory planning method according to the present invention uses a double inverted pendulum for the purpose of simplifying the description. However, the idea of the present invention using the backward search tree and the migratable region can be applied to a trajectory planning method in the state space of an arbitrary object.

関節角度と重力の関係に対する理解を容易にするために、以下のように座標を変更した。

Next, a control experiment by simulation of the double inverted pendulum 200 shown in FIG. 2 will be described. The link length of the first link 205 of the double inverted pendulum 200 is l ₁ , the mass is m ₁ , and the center of gravity position 203 is the center of the link. Link length of the second link 211 of the double inverted pendulum 200 is _{l 2,} mass, _{m 2,} and the center of gravity position 209 is a central link. Also, let g be the acceleration of gravity. The equation of motion of the double inverted pendulum 200 is as follows.

In order to facilitate understanding of the relationship between joint angle and gravity, the coordinates were changed as follows.

Table 1 shows the experimental simulation settings. The time interval from the current state to the adjacent state corresponds to the torque switching time step in Table 1.

Table 2 shows the parameters of the first joint and the second joint. In Table 2, the maximum torque of the first joint is 2 [Nm], and the torque granularity is 10 when creating a search tree. When deriving a new branch of the search tree, from -2 [Nm] to 2 [Nm] For example, -1.8 [Nm], -1.4 [Nm], -1.0 [Nm] ... 1.0 [Nm], 1.4 [Nm], 1.8 [Nm] This means that the next state (angle and angular velocity) is obtained from the equation of motion. In the experiment, 10 different torques are applied to the first joint and the second joint, respectively, so that 100 different next states are derived. Here, the movement of the object can be limited by adjusting the value of the maximum torque.

  In the experiment, the state in which the double inverted pendulum 200 was completely inverted was set as the target state.

FIG. 14 is a diagram showing a target state of the double inverted pendulum 200. As shown in FIG. The coordinates of the target state of the double inverted pendulum 200 are as follows.

  FIG. 15 is a diagram showing a trajectory based on the inverse search tree obtained by the method shown in the flowchart of FIG. 3 and a migratable area created by the method shown in the flowchart of FIG. 7 for points on the trajectory. In FIG. 15, the target state is indicated by A and the transferable area is indicated by R. FIG. 15A shows the phase space of the first joint, and FIG. 15B shows the phase space of the second joint.

  FIG. 16 is a diagram illustrating a state in which the trajectory based on the backward search tree and the trajectory based on the forward search tree illustrated in FIG. 15 are connected in the transferable region. In FIG. 16, the trajectory by the backward search tree is indicated by a solid line, and the trajectory by the forward search tree is indicated by a dotted line. The target state on the trajectory by the backward search tree is indicated by A, the transition point is indicated by B, and the initial state on the trajectory by the forward search tree is indicated by C. The movement from the initial state to the target state can be executed by connecting the two trajectories in the transitionable region. FIG. 16A shows the phase space of the first joint, and FIG. 16B shows the phase space of the second joint.

  FIG. 17 is a diagram illustrating a trajectory when feedback control is performed toward the target state from the end point of the trajectory by the forward search tree. In FIG. 17, the locus by feedback control is indicated by a solid line, and the locus by a forward search tree is indicated by a dotted line. In FIG. 17, the target state is indicated by A, the end point of the trajectory by the forward search tree is indicated by B, and the initial state on the trajectory by the forward search tree is indicated by C. The target state could not be reached by feedback control from the end point B of the trajectory by the forward search tree.

  As described above, the method and apparatus of the present invention using the backward search tree and the migratable region function effectively compared to simple feedback control.

  The present invention can be applied to a case where a machine in a stable state is transited to a next stable state via a certain unstable state. As an example, a machine performing a pitching motion usually selects a stable state as an initial state. The moment of throwing is unstable for the machine. The machine returns to the original stable state via this unstable state. In this case, connect the trajectory by the forward search tree with the stable state before pitching as the initial state and the trajectory with the backward search tree with the stable state after pitching as the initial state to connect the machine in the stable state. A trajectory for transitioning to the next stable state via a certain unstable state can be obtained. In addition to the pitching motion, there are a number of motions, such as tennis swing motion and lifting a rebounded baggage, to move from the stable state to the stable state via the unstable state.

DESCRIPTION OF SYMBOLS 101 ... Search tree creation part, 103 ... Transferable area | region determination part, 104 ... Search tree memory | storage part, 105 ... Trajectory creation part, 107 ... Trajectory control part,

Claims (8)

A trajectory planning method for obtaining a trajectory for controlling an object state from an initial state to a target state by a trajectory planning system,
Search tree creation unit, on the basis of the kinetic conditions for the transition of the state, the target state as a root in the state space, the branches included in each of the zones in advance a plurality of zones to split the state space node a method that forms create a backward search tree aggregate branches by limiting the number of,
Transition area determination unit, in the state space, the can reach the backward search within a predetermined distance range centered on the target state in accordance with the tree, the range of conditions around the point on the reverse search tree Defining a migratable area composed of :
A trajectory creation unit determines a trajectory from the point representing the initial state in the state space to the target state using the backward search tree;
Including
The trajectory creation unit uses a trajectory determined by the backward search tree from the point in the transitionable region to the root as all or part of the trajectory from the initial state to the target state.
Trajectory planning method. In the state space, the search tree creation unit aggregates branches by limiting the number of branch nodes included in the respective sections of the state space, with the initial state of the object as a root, and the transition is possible and a step that forms create a forward search tree with a branch in the region,
Claim wherein the path generation unit, by connecting the track the track by front Symbol forward search tree by the backward search tree, further comprising the steps of: determining the trajectory from the initial state of the object to the target state The trajectory planning method according to 1.   In the step of determining the migratable area, the migratable area determining unit can reach the target state by performing feedback control on a plurality of points in the vicinity of the point on the backward search tree. The trajectory planning method according to claim 1 or 2, wherein the transition possible area is determined based on a result of the determination.   The transferable area determination unit is based on a distance from a point closest to the point on the backward search tree to a point on the backward search tree among points that could not reach the target state. The trajectory planning method according to claim 3, wherein the transferable area is determined. A trajectory control method for controlling the state of the object from the initial state to the target state by controlling the object according to the trajectory obtained by the trajectory planning method according to claim 1. A trajectory planning system for obtaining a trajectory for controlling an object state from an initial state to a target state,
Based on the kinetic conditions for the transition of the state, by the target state as a root in the state space, which limits the number of branches of the nodes included in each of the zones of pre-divided state space into a plurality of zones A search tree creation unit for creating a backward search tree in which branches are aggregated;
In the state space, a migratable area configured by a range of states centered on a point on the backward search tree, which can reach a predetermined distance range centered on the target state according to the backward search tree A migratable area determination unit that determines
A search tree storage unit for storing a search tree and a migratable area;
A trajectory creating unit for defining a trajectory from the point representing the initial state in the state space to the target state using the backward search tree;
Bei to give a,
The trajectory creation unit uses a trajectory determined by the backward search tree from the point in the transitionable region to the root as all or part of the trajectory from the initial state to the target state.
Trajectory planning system. The migratable region in which the search tree creation unit aggregates branches by limiting the number of branch nodes included in each section of the state space with the initial state of the object as a root in the state space. Create a forward search tree with branches in it,
The path generation unit, by connecting the track the track by front Symbol forward search tree by the backward search tree, path planning according from the initial state of the object to claim 6 for determining the trajectory of to the target state system. The trajectory planning system according to claim 6 or 7, and a trajectory control unit that controls the state of the object from the initial state to the target state by controlling the object according to the trajectory obtained by the trajectory planning system. Trajectory planning and control system with

Images