Obstacle avoiding method in state-time space, recording medium storing program for executing same, and computer program stored in recording medium for executing same

Abstract

The present invention relates to an obstacle avoiding method in a state-time space, a recording medium storing a program for employing the same, and a computer program stored in a medium for employing the same. More particularly, the present invention relates to an obstacle avoiding method in a state-time space, a recording medium storing a program for employing the same, and a computer program stored in a medium for employing the same, wherein a forward trajectory is calculated on the basis of a safe timed configuration region, when collision is expected while calculating the forward trajectory, a part of the calculated forward trajectory is canceled, and a forward trajectory passing through an interim target is calculated, and thus a time required for calculating is reduced and a trajectory and success rate to the destination target state is ensured so as to obtain improvement in performance.

Claims

1. An obstacle avoiding method in a state-time space, wherein the method is employed in a program form executed by an arithmetic processing means including a computer, the method comprising: calculating a forward trajectory in a state-time space from a current location of a robot or a last calculation point of a previous forward trajectory to a current target waypoint by taking into account a state value (s) and a time value (time index n), wherein a state value (s.sub.n) in a state-time space, which is used when calculating the forward trajectory, includes a configuration value of a location and a bearing, and includes any one or a plurality of pieces of information selected from a steering angle, a velocity, and acceleration, the forward trajectory is calculated on the basis of a safe timed configuration region Q.sub.n.sup.safe (m) that is a safe space within a configuration region where collisions with static obstacles and dynamic obstacles are not present, wherein the obstacle avoiding method is performed: in response to detection of the safe timed configuration region Q.sub.n.sup.safe (m) not being presented so that collision is inevitable, determining it is determined that an inevitable collision state occurs, and in response to detection of the inevitable collision state being occurred, canceling a part of the forward trajectory, and calculating a forward trajectory passing through an interim target so as to avoid the collision.

2. The method of claim 1, wherein the forward trajectory is calculated by: setting a waypoint in which a state value (gw) of a robot is calculated when the robot arrives at the current target waypoint; calculating the forward trajectory; and generating a command for controlling the robot when the calculating the forward trajectory is repeated a predetermined number of times.

3. The method of claim 2, wherein in the setting of the waypoint, the state value (gw) in the current target waypoint is set as the equation below so as to minimize a cost-to-go, $\left[g_w{\tilde{g}}_{w+1}\right]=\underset{\left[g\tilde{g}\right]\in G_w\times G_{w+1}}{\mathrm{argmin}}\left(H\left(s_f,g\right)+H\left(g,\tilde{g}\right)\right)$ wherein, g.sub.w represents a state value in the current target waypoint, {tilde over (g)}.sub.w+1 represents a state value in a subsequent target waypoint while setting g.sub.w, G.sub.W represents a state range in the current target waypoint, G.sub.w+1 represents a state range in the subsequent target waypoint, H(a,b) represents a cost-to-go function from a to b, S.sub.f represents the current location of the robot, g represents the current target waypoint, and {tilde over (g)}represents the subsequent target waypoint.

4. The method of claim 2, wherein in the setting of the waypoint, a time parameter of the forward trajectory is initialized to a predetermined value when calculating an initial trajectory, and when calculating a trajectory after the initial trajectory, and an inevitable collision state occurs as a configuration value (q.sub.n) of the robot at a time index (n) is not included in the safe timed configuration region Q.sub.n.sup.safe (m), the time parameter of the forward trajectory is set by calculating the same to a greatest value capable of avoiding collision.

5. The method of claim 2, wherein calculating of the forward trajectory includes: incrementally calculating (one step) the forward trajectory when the safe timed configuration region Q.sub.n.sup.safe (m) is present; and when the inevitable collision state occurs, modifying a trajectory in partial, in which the forward trajectory is canceled from a last calculation point of the forward trajectory to a point at which the safe timed configuration region Q.sub.n.sup.safe (m) is present, and an interim target is set within the safe timed configuration region Q.sub.n.sup.safe (m) on the basis of the last calculation point of the forward trajectory in which the safe timed configuration region Q.sub.n.sup.safe (m) is present, and a trajectory passing through the interim target is planned so as to avoid collision.

6. The method of claim 5, wherein modifying the trajectory in partial, a state value of the interim target is determined as the equation below,
z.sub.f(−d)=N(s.sub.f−d,s.sub.−B,d) wherein, z.sub.f (−d) represents a state value in the interim target, N(a, b, c) represents a near-optimal timed state function calculating a trajectory from a to b during a time step c, s.sub.f−d represents a state value of the forward trajectory in which a time step d at which the forward trajectory is canceled is subtracted from a time step f of the forward trajectory at which collision is expected, s.sub.−B represents a state value in a step B of a backward trajectory and represents a state value of the current target waypoint when the backward trajectory is not present, and d represents a time step at which the forward trajectory is canceled.

7. The method of claim 5, wherein modifying of the trajectory in partial, a trajectory including the safe timed configuration region Q.sub.n.sup.safe (m) is determined by increasing a time index corresponding to the last calculation point of the forward trajectory in which the safe timed configuration region Q.sub.n.sup.safe (m) is present.

8. The method of claim 5, wherein modifying of the trajectory in partial, an identical input value is calculated for the robot.

9. A non-transitory computer-readable recording medium storing a program for employing an obstacle avoiding method in a state-time space of claim 1.

10. A computer-executable program stored in a non-transitory computer-readable recording medium, when executed by a computer of a robot, causes the computer of the robot to instruct the following arithmetic processing steps: calculating a forward trajectory in a state-time space from a current location of a robot or a last calculation point of a previous forward trajectory to a current target waypoint by taking into account a state value (s) and a time value (time index n), wherein a state value (s.sub.n) in a state-time space, which is used when calculating the forward trajectory, includes a configuration value of a location and a bearing, and includes any one or a plurality of pieces of information selected from a steering angle, a velocity, and acceleration, the forward trajectory is calculated on the basis of a safe timed configuration region Q.sub.n.sup.safe (m) that is a safe space within a configuration region where collisions with static obstacles and dynamic obstacles are not present, wherein the obstacle avoiding method is performed by the computer: in response to detection of the safe timed configuration region Q.sub.n.sup.safe (m) not being presented so that collision is inevitable, determining that an inevitable collision state occurs, and in response to detection of the inevitable collision state being occurred, canceling a part of the forward trajectory, and calculating a forward trajectory passing through an interim target so as to avoid the collision.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) The above and other objects, features and other advantages of the present invention will be more clearly understood from the following detailed description when taken in conjunction with the accompanying drawings, in which:

(2) FIG. 1 is a view of a flowchart showing an embodiment of a method of avoiding an obstacle in a state-time space according to the present invention;

(3) FIG. 2 is a view of a flowchart showing another embodiment of a method of avoiding an obstacle in a state-time space according to the present invention; and

(4) FIG. 3 is a view showing an example of modifying a trajectory in partial.

DETAILED DESCRIPTION OF THE INVENTION

(5) The present invention can be modified in various manners and have various forms. Therefore, specific embodiments will be described in detail with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments, but may include all modifications, equivalents and substitutions within the scope of the present invention.

(6) It will be understood that when an element is referred to as being “connected” or “coupled” to another element, it can be directly connected or coupled to the other element or intervening elements may be present.

(7) In contrast, when an element is referred to as being “directly connected” or “directly coupled” to another element, there are no intervening elements present.

(8) The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used herein, the singular forms “a”, “an”, and “the” are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms “comprises”, “comprising”, “includes”, and/or “including” when used herein, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.

(9) Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. It will be further understood that terms used herein should be interpreted as having a meaning that is consistent with their meaning in the context of this specification and the relevant art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein.

(10) Herein, with reference to the accompanying drawings, embodiments of the present invention will be described in detail. Prior to the description, it should be understood that the terms used in the specification and the appended claims should not be construed as limited to general and dictionary meanings, but interpreted based on the meanings and concepts corresponding to technical aspects of the present disclosure on the basis of the principle that the inventor is allowed to define terms appropriately for the best explanation. In addition, technical terms and scientific terms used in the present specification have the general meaning understood by those skilled in the art to which the present invention pertains unless otherwise defined, and a description for the known function and configuration obscuring the present invention will be omitted in the following description and the accompanying drawings. The drawings to be provided below are provided by way of example so that the idea of the present invention can be sufficiently delivered to a person skilled in the art to which the present invention pertains. Therefore, the present invention is not limited to the drawings provided below but may be modified in many different forms. In addition, like reference numerals designate like elements throughout the specification. In the drawings, same reference numerals denote same components throughout the disclosure.

(11) FIG. 1 is a view of a flowchart showing an embodiment of a method of avoiding an obstacle in a state-time space according to the present invention, FIG. 2 is a view of a flowchart showing another embodiment of a method of avoiding an obstacle in a state-time space according to the present invention, and FIG. 3 is a view showing an example of modifying a trajectory in partial.

(12) Prior to the description, terms used in the present specification (and claims) will be briefly described.

(13) A “state-time space” refers to a set of state values taking into account of time.

(14) A “state value” refers to a value including a configuration including a location and a bearing (heading angle), and includes any one or a plurality of pieces of information selected from a steering angle, a velocity (linear velocity, angular velocity), and an acceleration. For example, the same may be (x coordinate, y coordinate, bearing, velocity, acceleration), etc.

(15) A “timed configuration region” means a set of configuration values taking into account time.

(16) A “configuration value” refers to a value including a location and a bearing (heading angle). For example, the same may be (x coordinate, y coordinate, bearing), etc.

(17) A “waypoint” refers to a designated location trough which a robot passes when calculating a driving trajectory.

(18) For the description, various parameters and terms are designated and described. The variables and terms are as below.

(19) A state value of a current location of the robot is represented as “s.sub.k”.

(20) A state value at an f step calculated in a forward trajectory from a current location of the robot is represented as “s.sub.f”.

(21) A state value of the robot when the robot arrives at a current target waypoint is represented as “g.sub.w”.

(22) A state value calculated for a b step in a backward trajectory from the current target waypoint is represented as “s.sub.−b”.

(23) A time index representing to which step corresponds the calculation is represented as “n”.

(24) A time interval given for calculating one step is represented as “T”.

(25) A time at a time index n is represented as “t” (t=nT).

(26) A state value of the robot is represented as “s” (lowercase s).

(27) A state space that is a set of state values of the robot is represented as “S” (capital letter S).

(28) A timed state value that is a state value at a time step n is represented as s.sub.n (lowercase s).

(29) A state-time space is represented as “S.sub.n” (s.sub.n∈S.sub.n).

(30) A configuration value of the robot is represented as “q” (lowercase q).

(31) A configuration region that is a set of configuration values of the robot is represented as “Q” (capital letter Q).

(32) A timed configuration value that is a configuration value at a time step n is represented as q.sub.n (lowercase q).

(33) A timed configuration region is represented as “Q.sub.n” (q.sub.n∈Q.sub.n).

(34) A configuration region where collision with static obstacles is present is represented as Q.sup.env.

(35) A free static configuration region where collision with static obstacles is not present is represented as Q.sup.free (Q.sup.free=Q−Q.sup.env).

(36) A configuration region where collision with dynamic obstacle is present at a time index n is represented as Q.sub.n.sup.obs.

(37) A free dynamic configuration region where collision with static obstacles and dynamic obstacles is not present is represented as Q.sub.n.sup.free (Q.sub.n.sup.free=Q.sup.free−Q.sub.n.sup.obs).

(38) A state where collision is inevitable is referred to an “inevitable state”.

(39) Fundamental parameters and terms are summarized, but not all of the parameters and terms necessary for the description are listed. Parameters and terms that are used in addition to the parameters and terms defined above will be described later.

(40) Hereinafter, in the description, description will be made assuming that the robot moves from a current state point corresponding to a state value s.sub.k to a target point (current target waypoint) corresponding to a state value g.sub.w.

(41) In an obstacle avoiding method in a state-time space according to an embodiment of the present invention, for an obstacle avoiding method in a state-time space, wherein the method is employed in a program form executed by an arithmetic processing means including a computer, a forward trajectory in a state-time space taking into account a state value s and a time value (time index n) is calculated from a current location of a robot or a last calculation point of a forward trajectory to a current target waypoint.

(42) Herein, a state value s.sub.n in a state-time space used for calculating the forward trajectory includes a configuration value of a location and a bearing (heading angle), and includes any one or a plurality of pieces of information selected from a steering angle, a velocity (linear velocity, angular velocity), and acceleration.

(43) The forward trajectory is calculated on the basis of a safe timed configuration region Q.sub.n.sup.safe (m) that is a safe space in a configuration region where collisions with static obstacles and dynamic obstacles are not present. When the safe timed configuration region Q.sub.n.sup.safe (m) is not present so that collision is inevitable, it is determined that an inevitable collision state (ICS) occurs.

(44) When the inevitable collision state occurs, a part of the planned forward trajectory is canceled, and a forward trajectory passing an interim target is planned so as to avoid the collision.

(45) Herein, the time value is a value through which a future time from the current time is confirmed. For the same, a time index n may be used, but various methods may be used when an actual progress time can be confirmed such as multiplying a calculation period that calculates one step with a time index n (calculation period*time index n), etc.

(46) In the forward trajectory calculation, a forward trajectory in a state-time space is calculated from an initial current location of the robot to a current target waypoint. Subsequently, a subsequent forward trajectory in a state-time space may be calculated from a last calculation point of the forward trajectory to the target waypoint.

(47) In other words, in a an obstacle avoiding method in a state-time space according to an embodiment of the present invention, a forward trajectory is sequentially calculated, and when an inevitable collision state occurs while planning the forward trajectory, a part of the forward trajectory that is planned (calculated) in the meantime is canceled, an interim target capable of avoiding the inevitable collision state is set, and then a forward trajectory passing through the interim target is re-planned so as to avoid the collision.

(48) Herein, whether or not the inevitable collision state (ICS) occurs may be determined by whether or not a safe timed configuration region Q.sub.n.sup.safe (m) is present.

(49) In an obstacle avoiding method in a state-time space according to the present invention, in forward trajectory calculation, a cost-to-go function H is decomposited, and a priority r (r is a natural number) is assigned to the decomposited cost-to-go function H.sup.r.

(50) The decomposited cost-to-go functions H.sup.r are applied to a near-optimal timed state function N according to a preset reference which calculates a trajectory where a value of summing the decomposited cost-to-go function H.sup.r becomes minimum (an r* value may be set or calculated).

(51) Herein, a state value s.sub.n+m in a step where the number m of steps to be calculated is progressed from the given point of the state value s.sub.n toward the target point s.sup.to is calculated on the basis of a given state value s.sub.n in an n (n is natural number) step, a state value s.sup.to at a target point, and a number m of steps to be calculated.

(52) Herein, a weighting factor is assigned to each decomposited cost function H.sup.r, and a near-optimal timed state function N may be applied thereto so that a trajectory is calculated where the sum of values obtained by respectively multiplying the weight and decomposited cost functions H.sup.r becomes minimum.

(53) Alternatively, in forward trajectory calculation and backward calculation, a cost-to-go function H is decomposited, and a priority r (r is a natural number)) is assigned to the decomposited cost-to-go function H.sup.r.

(54) The decomposited cost-to-go functions H.sup.r are sequentially applied to a near-optimal timed state function N according to the priority (an r* value may be set or calculated) which calculates a trajectory where each value of the decomposited cost functions H.sup.r becomes minimum.

(55) Herein, a state value s.sub.n+m in a step where the number m of steps to be calculated is progressed from the given point of the state value s.sub.n toward the target point s.sup.to is calculated on the basis of a state value s.sub.n given in an n step, a state value s.sup.to at a target point and a number m of steps to be calculated.

(56) When planning the state value s.sub.n+m including the given state value sn, a cost-to-go from the state value s.sub.n+m to a point corresponding to the target state value s.sup.to is calculated to be minimum.

(57) In other words, the forward trajectory calculation may be defined as below.

(58) $s_{n+m}=\underset{s\in S_n^{\mathrm{safe}}\left(n\right)}{\mathrm{argmin}}H\left(s,s^{\mathrm{to}}\right)$

(59) Herein, s.sub.n+m is a state value at a time n+m, H is a cost-to-go function, s is a given state value, s.sup.to is a state value at a target point, and m is a number of steps to be calculated.

(60) When a cost-to-go function (H) is used, long time is required for calculation, and thus applying in real time to trajectory calculation is difficult.

(61) An input value u.sub.n for the robot may be obtained as below.

(62) $u_n=\underset{u\in {\stackrel{\_}{U}}_n\left(m\right)}{\mathrm{argmin}}{H}_{\mathrm{Input}}\left(s_n,s_{n+m}\right)$${\stackrel{\_}{U}}_n\left(m\right)=\left\{u\in U|s_{n+m}=g\left(s_n,u,m\right)s_{n+m}=\underset{s\in S_n^{\mathrm{safe}}\left(m\right)}{\mathrm{argmin}}H\left(s,s^{\mathrm{to}}\right)\right\}$

(63) Herein, H.sub.input is a given cost cost-to-go function about input for the least effort.

(64) The available input is always present since planning is performed for a s.sub.n+m within a safe space that is bounded by the available input space of the available robot.

(65) When the cost-to-go function for minimizing the cost-to-go is too complex to calculate a trajectory in real time, a function decomposition-based optimization method may be used.

(66) A function decomposition-based optimization method decomposes a function into simple parts, and minimizes a dominant part first. It is assumed that H is configured with R parts as below.

(67) $H=\underset{r=1,2,\mathrm{.Math.},R}{.Math.}{H}^r$

(68) Herein H.sup.rs are arranged in order of dominance. In other words, H.sup.r with smaller r is more dominant. When minimizing is performed sequentially one by one from H.sup.1 to H.sup.R, an available safe space becomes smaller. The decomposited cost-to-go function (H.sup.r) is minimized by increasing the r value unit a single state value is induced.

(69) A safe timed state space Ŝ.sub.n.sup.r(m) that minimizes r parts from H.sup.1 to H.sup.r may be defined as below.

(70) ${\hat{S}}_n^{r*}\left(m\right)=\left\{s.Math.\underset{s\in {\hat{S}}_n^{r-1}\left(m\right)}{\mathrm{argmin}}\left(H^r\left(s,s^{\mathrm{to}}\right)\right)\right\}$

(71) A safe timed state space Ŝ.sub.n.sup.o(m) calculated by using a cost-to-go function that is not decomposited is identical to a safe timed state space S.sub.n.sup.safe (m).

(72) An r value from which one state value is induced is referred to as r*, and minimizing is performed for the decomposited cost-to-go function until r* is obtained. Herein, a usable safe timed state space is reduced to a single state as below.
{s.sub.n+m}=Ŝ.sub.n.sup.r*(m)

(73) Herein,
r*=min{r∈{1,2, . . . ,R}∥Ŝ.sub.n.sup.r(m)|=1}

(74) Even though the timed state value obtained from function decomposition-based minimization does not minimize H, the result is close to minimum as dominant parts are minimized.

(75) In addition, real time execution is available, and calculation is efficient.

(76) Accordingly, when a near-optimal timed state function N is applied, the forward trajectory calculation may be performed as below.
s.sub.n+m=N(s.sub.n,s.sup.to,m)

(77) Herein, s.sub.n+m is a state value in an n+m step, N is a near-optimal timed state value calculation function, s.sub.n is a given state value in an n step, and s.sup.to is a state value at a target point, and m is a number of steps to be calculated.

(78) In case of an omnidirectional movable robot, for example, the near-optimal timed state function N may be calculated by using the equation below.

(79) $H\left(s^{\mathrm{from}},s^{\mathrm{to}}\right)=H^1+H^2+H^3$$H^1=.Math.p^{\mathrm{from}}-p^{\mathrm{to}}.Math.$$H^2=.Math.{\varphi }^{\mathrm{from}}-\phi .Math.+.Math.{\varphi }^{\mathrm{to}}-\phi .Math.$$H^3=.Math.\frac{v^{\mathrm{from}}D}{v_{\max }\min \left(D,.Math.p^{\mathrm{from}}|-p^{\mathrm{to}}.Math.\right)}-\frac{\pi -.Math.{\varphi }^{\mathrm{from}}-\phi .Math.}{\pi }.Math.+.Math.\frac{v^{\mathrm{to}}D}{v_{\max }\min \left(D,.Math.p^{\mathrm{from}}-p^{\mathrm{to}}.Math.\right)}-\frac{\pi -.Math.{\varphi }^{\mathrm{to}}-\phi .Math.}{\pi }.Math.$

(80) Herein, H.sup.1 is a velocity-independent cost-to-go function, H.sup.2 is a moving direction part cost-to-go function, and H.sup.3 is a velocity magnitude part cost-to-go function.

(81) In order to induce the robot to move toward the target fast and pass through waypoints effectively, a cost-to-go function H(s.sup.from, s.sup.to) may be configured with three parts, and D is a predefined threshold distance.

(82) Each location in a reachable region is matched 1:1 with an admissible velocity space, and thus each timed state is determined on the basis of the velocity-independent cost-to-go function H.sup.1.

(83) The moving direction part cost-to-go function H.sup.2 and the velocity magnitude part cost-to-go function H.sup.3 are used for setting a state of a current target waypoint.

(84) On the basis of H.sup.1, each timed configuration is determined to be the closest one to the target.

(85) H.sup.2 and H.sup.3 are designed for setting a state of the current target waypoint.

(86) A velocity at the current target waypoint is determined by taking account into a robot location, a location of a target waypoint, and a location of a subsequent waypoint.

(87) A direction is determined as an average direction of two vectors which are from a robot location to a location of a target waypoint, and from a location of a waypoint to a location of a subsequent waypoint.

(88) In addition, a magnitude of a velocity at a target waypoint is determined according to an interior angle of a trajectory at the target waypoint.

(89) When the angle is large, the velocity becomes fast, or vice versa.

(90) When an Euclidean distance between a robot location and a location of a target waypoint or between a location of the target waypoint and a location of a subsequent waypoint is smaller than D, a magnitude of a velocity of the current target waypoint is set proportional to the distance in order to not oscillate near the waypoint.

(91) In case of a robot moving through wheels, for example, the near-optimal timed state function N may be calculated by using the equation below.

(92) $H\left(s^{\mathrm{from}},s^{\mathrm{to}}\right)=H^1+H^2+H^3$$H^1=\alpha \left(.Math.p^{\mathrm{from}}-p^{\mathrm{to}}.Math.\right)+\left(1-\alpha \right)\left(.Math.{\theta }^{\mathrm{from}}-\phi .Math.+.Math.{\theta }^{\mathrm{to}}-\phi .Math.\right)$$H^2=.Math.\frac{v^{\mathrm{from}}D}{v_{\max }\min \left(D,.Math.q^{\mathrm{from}}-q^{\mathrm{to}}.Math.\right)}-\frac{\pi -.Math.{\theta }^{\mathrm{from}}-\phi .Math.}{\pi }.Math.+.Math.\frac{v^{\mathrm{to}}D}{v_{\max }\min \left(D,.Math.q^{\mathrm{from}}-q^{\mathrm{to}}.Math.\right)}-\frac{\pi -.Math.{\theta }^{\mathrm{to}}-\phi .Math.}{\pi }.Math.$$H^3=.Math.\frac{{\omega }^{\mathrm{from}}}{{\omega }_{\max }}-\frac{\phi -{\theta }^{\mathrm{from}}}{\pi }.Math.$

(93) Herein, H.sup.1 is a velocity-independent cost-to-go function, H.sup.2 is a moving direction pat cost-to-go function, and H.sup.3 is a velocity magnitude part cost-to-go function.

(94) As the example of the omnidirectional movable robot, each timed state is determined on the basis of the velocity-independent cost-to-go function H.sup.1.

(95) In order to take into account both of a location close to a target location and a direction to the target location, H.sup.1 is configured with two parts with a predefined weighting factor α.

(96) The moving direction pat cost-to-go function H.sup.2 and the velocity magnitude part cost-to-go function H.sup.3 are used for setting a linear and angular velocity of a current target waypoint similar to the case of the omnidirectional movable robot.

(97) In an obstacle avoiding method in a state-time space according to an embodiment of the present invention, in forward trajectory calculation, when a safe timed configuration region Q.sub.n.sup.safe (m) that is a safe space within a configuration region where collisions with static obstacles and dynamic obstacles are not present is present, a forward trajectory is calculated on the basis of a configuration value q.sub.n of the robot at a time index n, and the safe timed configuration region Q.sub.n.sup.safe (m) at a time index n.

(98) When a set of configuration values where collision with static environmental obstacles such as wall is present is referred to as Q.sup.env and as “environmental obstacle configuration region” (Q.sup.env⊂Q), difference of sets obtained by subtracting the environmental obstacle configuration region Q.sup.env from the configuration region Q may be referred to as a “free static configuration region” Q.sup.free.

(99) The robot may predict a trajectory of each dynamic obstacle such as people and other robots on the basis of a tracking algorithm using sensor data.

(100) Accordingly, when a set of configuration values predicted to collide with any dynamic obstacles such as people at a time index n is referred to as Q.sub.n.sup.obs and as a dynamic obstacle space “(Q.sub.n.sup.obs⊂Q), difference of sets obtained by subtracting the dynamic obstacle space Q.sub.n.sup.obs from the free static configuration region Q.sup.free may be referred to as a “free dynamic configuration region” Q.sub.n.sup.free.

(101) In other words, the free dynamic configuration region Q.sub.n.sup.free at a time index n may be a safe space where the robot does not collide with obstacles including dynamic obstacles and static obstacles at a time index n.

(102) A motion of a robot system with an input value u.sub.n (u.sub.n∈U) in an input space U of the robot at a time index n may be defined as below.
s.sub.n+1=f(s.sub.n,u.sub.n)

(103) Herein, f is a motion model function of a robot.

(104) A state space S of the robot and an input space U of the robot are bounded, and thus robot motion is constrained which is referred to a motion constraint.

(105) Environment detectors (sensors) detect and track obstacles on the basis of a sensory system using sensors, such as vision sensors or laser range finders (LRF), etc., and classify the same into static obstacles (environmental obstacles) and dynamic obstacles (non-environmental obstacles) to obtain Q.sup.env and Q.sub.n.sup.obs.

(106) Global trajectory planner (global path planner) planning a trajectory by assigning a waypoint plans a sequence of waypoints and a target inside a free static configuration region Q.sup.free.

(107) At every time instant t=kT, k=0, 1, . . . , an algorithm plans a trajectory by using a reference input k.

(108) By the above input, the robot passes through waypoints sequentially and arrive the target without any collision by using both of Q.sup.env and Q.sub.n.sup.obs.

(109) When a timed state value s.sub.n is given, a reachable state-time region at a time index n+m may be defined. Herein, a reachable timed configuration region may be also defined.

(110) In order to plan a trajectory in real time, it is preferable to reduce an amount of calculation by approximating the reachable state-time region or timed configuration region.

(111) It may be difficult to perform real time calculation when the reachable state-time region or timed configuration region is calculated accurately and used.

(112) In order to simplify the reachable region, the robot may be controlled for m time steps by using a uniform robot input value.

(113) A reachable state-time region that is reachable by the uniform robot input value may be defined as below.
S.sub.n(m)={s.sub.n+m∈S|∀u.sub.n∈U,s.sub.n+m=g(s.sub.n,u.sub.n,m)}

(114) Herein, g is an approximated motion model function of a robot.

(115) Herein, a reachable timed configuration region that is reachable by the uniform robot input value may be defined as below.

(116) $\begin{array}{c}{Q}_n\left(m\right)=\{q_{n+m}\in Q|\forall u_n\in U,s_{n+m}\\ =g\left(s_n,u_n,m\right),s_{n+m}\in S\}\end{array}$

(117) S.sub.n (m) and Q.sub.n (m) do not cover the whole actual reachable region, but calculation load is very small rather than calculating the whole actual reachable region, and thus the same are useful for trajectory planning.

(118) When a state value s.sub.n is given, a safe timed state space S.sub.n.sup.safe (m) at a time index n+m may be defined as below.
S.sub.n.sup.safe(m)=S.sub.n(m)∩S.sub.n+m.sup.free,(n>0,m>0)

(119) In addition, when a state value s.sub.n is given, a safe timed configuration region Q.sub.n.sup.safe (m) at a time n+m may be defined as below.
Q.sub.n.sup.safe(m)=Q.sub.n(m)∩Q.sub.n+m.sup.free,(n>0,m>0)

(120) Q.sub.n.sup.safe (m) is planned within S.sub.n.sup.safe (m), but using Q.sub.n.sup.safe (m) may be enough by checking predicted collision and by check whether or not an available safe space is present.

(121) When calculating the safe timed configuration region Q.sub.n.sup.safe (m) of a backward trajectory at a current target waypoint, Q.sub.n+m.sup.free is replaced with Q.sup.free.

(122) This is because, an actual time of Q.sub.n+m.sup.free is not given, and thus data of dynamic obstacles is not obtained.

(123) An obstacle avoiding method in a state-time space according to an embodiment of the present invention may include S20 of setting a waypoint, S50 of calculating a forward trajectory, and S60 of generating a robot control command.

(124) The S20 of setting a waypoint is setting a state value g.sub.w of the robot when the robot arrives a current target waypoint.

(125) The current target waypoint is a waypoint where a forward trajectory is heading to pass through.

(126) The S20 of setting a waypoint is setting a robot state when the robot passes the current target waypoint. It is preferable to set the robot state when the robot passes the current target waypoint by taking into account of a robot location, a location of a target waypoint, and a location of a subsequent waypoint.

(127) The S50 of calculating a forward trajectory calculates a forward trajectory.

(128) In the S50 of calculating a forward trajectory, a forward trajectory is sequentially planned from a current location of the robot to a target waypoint.

(129) The forward trajectory may be calculated by using the equation below,

(130) $s_{n+m}=\underset{s\in S_n^{\mathrm{safe}}\left(n\right)}{\mathrm{argmin}}H\left(s,s^{\mathrm{to}}\right)$

(131) or by using the equation below,
s.sub.n+m=N(s.sub.n,s.sup.to,m).

(132) The S60 of generating a robot control command generates a command for controlling the robot according to the forward trajectory when the S50 of calculating a forward trajectory is repeated a predetermined number of times.

(133) An input value for the robot generated in the S60 of generating a robot control command may be calculated as below.

(134) 0$u_k=\underset{u\in {\stackrel{\_}{U}}_k\left(1\right)}{\mathrm{argmin}}{H}_{\mathrm{Input}}\left(s_k,s_{k+1}\right)$

(135) Herein, u.sub.k is an input value for a robot at a time k, u∈Ū.sub.k(1) is an input value for the robot which is included in an input space of the robot at a k-th step where one step may be run, H.sub.input is a minimum cost-to-go function for an input value for the robot, S.sub.k is a state value of the robot at a time k, and s.sub.k+1 is a state value of the robot at a time k+1.

(136) In an obstacle avoiding method in a state-time space according to the present invention, the S20 of setting a waypoint is setting a state value g.sub.w of a current target waypoint so as to minimize a cost-to-go, and is set as the equation below.

(137) $\left[g_w{\tilde{g}}_{w+1}\right]=\underset{\left[g\tilde{g}\right]\in G_w\times G_{w+1}}{\mathrm{argmin}}\left(H\left(s_f,g\right)+H\left(g,\tilde{g}\right)\right)$

(138) Herein, g.sub.w represents a state value of a current target waypoint, {tilde over (g)}.sub.w+1 represents a state value of a subsequent target waypoint while setting g.sub.w, G.sub.w represents a state range of the current target waypoint, G.sub.w+1 represents a state range of the subsequent target waypoint, H(a,b) represents a cost-to-go function from a to b, S.sub.f represents a current location of a robot, g represents the current target waypoint, and {tilde over (g)} represents a subsequent waypoint of the current target waypoint.

(139) In an obstacle avoiding method in a state-time space according to an embodiment of the present invention, in the S20 of setting a waypoint, when calculating an initial trajectory, a time parameter of a forward trajectory is initialized to a preset value. Subsequently, when calculating a trajectory after a final trajectory, and an inevitable collision state occurs as a configuration value q.sub.n of the robot at a time index n is not included in a safe timed configuration region Q.sub.n.sup.safe (m), a time parameter of the forward trajectory is set by calculating the same to a greatest value capable of avoiding the collision.

(140) It is preferable to operate the robot when the forward trajectory is calculated for a predetermined number of steps.

(141) Calculating the entire forward trajectory causes increase in an amount of calculation when an inevitable collision state occurs due to unpredictable situation of dynamic obstacles so that trajectory modification is required.

(142) Accordingly, it is preferable to operate the robot when a forward trajectory is calculated for one set that is a preset number of steps of the forward trajectory.

(143) Subsequently, when collision is not expected in a subsequent set of forward trajectory calculation, calculating again the calculated the forward trajectory in the meantime may be performing unnecessary calculation.

(144) Accordingly, when calculating a trajectory after the initial trajectory calculation and an inevitable collision state does not occur, it is preferable to calculate a forward trajectory for a section that does not overlap among a predetermined number of steps of a subsequent forward trajectory when calculating a subsequent set of the subsequent forward trajectory.

(145) In addition, when calculating a trajectory after the initial trajectory calculation and an inevitable collision state occurs, it is preferable to calculate the entire set that is a predetermined number of steps of the forward trajectory by calculating a time parameter of the forward trajectory to be greater enough to avoid collision.

(146) In an obstacle avoiding method in a state-time space according to an embodiment of the present invention, the S50 of calculating a forward trajectory includes S51 of incrementally planning a trajectory, and S52 of modifying a trajectory in partial.

(147) In S51 of incrementally planning a trajectory, a forward trajectory is calculated when a safe timed configuration region Q.sub.n.sup.safe (m) is present.

(148) When an available safe timed configuration region Q.sub.n.sup.safe (m) is present, a state where each time is designated is planned within the safe timed configuration region Q.sub.n.sup.safe (m).

(149) In order to derive the robot to move toward s.sub.−B without collision, a timed state value s.sub.f may be calculated for one step toward s.sub.−B within the safe timed configuration region Q.sub.n.sup.safe (m) by using the equation below.
s.sub.f=N(s.sub.f−1,s.sub.−B,1)

(150) When the safe timed configuration region Q.sub.n.sup.safe (m) is not present Q.sub.f−1.sup.safe (1)=∅), a timed state without collision is not obtained by using the above equation, and thus a part of the forward trajectory has to be modified.

(151) When the safe timed configuration region Q.sub.n.sup.safe (m) is not present Q.sub.f−1.sup.safe (1)=∅), a part of the planned forward trajectory is modified through S52 of modifying a trajectory in partial which will be described later.

(152) In S52 of modifying a trajectory in partial, the planned forward trajectory is canceled from a last calculation point of the forward trajectory to a point at which the safe timed configuration region Q.sub.n.sup.safe (m) is present when an inevitable collision state occurs, and an interim target is set within the safe timed configuration region Q.sub.n.sup.safe (m) on the basis of the last calculation point of the forward trajectory in which the safe timed configuration region Q.sub.n.sup.safe (m) is present, and then a trajectory passing through the interim target is planned so as to avoid collision.

(153) First, a number of time steps, P.sub.min, required for modification is calculated by using the equation below.
P.sub.min=min{p∈{2,3, . . . ,f−k}|Q.sub.f−p.sup.safe(p)≠∅}

(154) When an available safe timed configuration region Q.sub.f−p.sup.safe (p) is present, the robot may move by a uniform input without collision at a time index f during [(f−p)T,fT].

(155) s.sub.f−P.sub.min is a state space where an inevitable collision state is expected not to occur.

(156) P.sub.min may be obtained by following the precedent planned forward trajectory in backwards.

(157) Setting the time parameter of the forward trajectory to the greatest value (f−p) capable of avoiding collision is to minimize a trajectory that is canceled from the calculated trajectory.

(158) It is possible to check an inevitable collision state for each time step, but it may be determined that an inevitable collision state occurs when a safe timed configuration region Q.sub.n.sup.safe (m) is not present.

(159) Herein, a part of the calculated forward trajectory is modified in advance so that an inevitable collision state does not occur in the forward trajectory so as to avoid collision.

(160) Taking into account that a point at which the robot collides with each obstacle has to be calculated in real time, the robot takes more flexible and appropriate actions even when many obstacles are present.

(161) Herein, within the safe timed configuration region Q.sub.n.sup.safe (m), a state value closest to the current target waypoint may be set to an interim target.

(162) This is because, even though a forward trajectory capable of avoiding a target object is replanned, an optimum forward trajectory capable of avoiding a target object is replanned by minimizing the difference with the initial planned forward trajectory.

(163) In an obstacle avoiding method in a state-time space according to an embodiment of the present invention, in the S52 of modifying a trajectory in partial, a state value of the interim target is determined as the equation below.
z.sub.f(−d)=N(s.sub.f−d,s.sub.−B,d)

(164) Herein, z.sub.f(−d) represents a state value of an interim target, N(a, b, c) represents a near-optimal timed state function calculating a trajectory from a to b during a time step c, s.sub.f−d represents a state value of a forward trajectory where a time step d at which the forward trajectory is canceled is subtracted from a time step f of the forward trajectory at which collision is expected, s.sub.−B represents a state value at a step B of a backward trajectory and represents a state value of the current target waypoint when the backward trajectory is not present, and d represents a time step at which the forward trajectory is canceled.

(165) In an obstacle avoiding method in a state-time space according to an embodiment of the present invention, in the S52 of modifying a trajectory in partial, a trajectory where a safe timed configuration region Q.sub.n.sup.safe (m) is present is planned by increasing a time index from a time index corresponding to a last calculation point of the forward trajectory where a safe timed configuration region Q.sub.n.sup.safe (m) is present.

(166) The modified part of the trajectory is at least P.sub.min and at most (f−k) that is a time interval where a time step k at which the robot is currently present is subtracted from a time step fat which the maximum forward trajectory is calculated.

(167) A trajectory where collision is not present is determined by increasing a number J of time steps to be modified from P.sub.min to f−k until the time index becomes f.

(168) When the trajectory where collision is not present is not determined until J becomes f−k, the robot is expected to collide with an obstacle.

(169) Calculation is performed by using the forward trajectory s.sub.f−d+1, d=J, J−1, . . . , 1, the interim target z.sub.f(−d), and an interim timed state s.sub.f′(−d+1).

(170) An index (subscript) d is a number of time intervals from fin a reverse direction.

(171) In an obstacle avoiding method in a state-time space according to an embodiment of the present invention, in the S52 of modifying a trajectory in partial, an identical input value ((constant, uniform) is calculated for the robot.

(172) This is to simplify the calculation.

(173) The interim target z.sub.f(−d) is a temporary target, and is an expected state of the robot moving by using the uniform input at time index f during [(f−p)T, fT].

(174) In order to derive the robot toward s.sub.−B safely, z.sub.f(−d) may be calculated as below.
z.sub.f(−d)=N(s.sub.f−d,s.sub.−B,d)

(175) After determining the interim target, a temporary timed state s.sub.f′(−d+1) that is a timed state at a time index f−d+1 on a trajectory where the robot moves with a temporary input state u.sub.f′(−d) may be calculated by using the equation below.
s′.sub.f(−d+1)=f(s.sub.f−d,u′.sub.f(−d))

(176) Herein, an input value u.sub.f′(−d) for the robot may be obtained by using the equation below.

(177) $u_f^{\prime }\left(-d\right)=\underset{u\in {\stackrel{\_}{U}}_{f-d}\left(d\right)}{\arg \min }{H}_{\mathrm{Input}}\left(s_{f-d},z_f\left(-d\right)\right)$

(178) Finally, a timed state s.sub.f−d+1 that is modified toward s.sub.f′(−d+1) within S.sub.f−p.sup.safe (1) may be determined as the equation below.
s.sub.f−d+1=N(s.sub.f−d,s′.sub.f(−d+1),1)

(179) When s.sub.f−d=s′.sub.f(−d), u′.sub.f(−d) is identical to a previous value, and thus calculation of z.sub.f(−d) is not necessary.

(180) In other words, z.sub.f(−d)=z.sub.f(−d−1) and u.sub.f′(−d)=u.sub.f′(−d−1).

(181) The S52 of modifying a trajectory in partial will be described in detail with reference to FIG. 3.

(182) In FIG. 3, a configuration region is represented in one dimension (vertical axis) in a vertical direction for easy understanding.

(183) In other words, a vertical axis of FIG. 3 represents a configuration region, and a horizontal axis represents a time space.

(184) Before modifying a trajectory, P.sub.min is obtained as 4(Q.sub.f−4.sup.safe(4)=∅), and J is set to P.sub.min.

(185) Subsequently, a modified trajectory s.sub.f−d+1, d=J, J−1, . . . 1 is obtained.

(186) For modification, first, a midterm target z.sub.f(−4) is set toward s.sub.−B within S.sub.f−4.sup.safe(4).

(187) Subsequently, s.sub.f−3 is determined as a timed state value s.sub.f′(−3) since there is no disturbing obstacle.

(188) However, s.sub.f−2 is not determined as s.sub.f′(−2) due to disturbing obstacles, but is determined to a state closest to s.sub.f′(−2) within S.sub.f−3.sup.safe(1) in which disturbing obstacles are not present.

(189) Subsequently, a midterm target z.sub.f(−2) is set as a trajectory for z.sub.f(−4) is modified during determining s.sub.f−2.

(190) Subsequently, during d=2, 1, s.sub.f−1 and sf are determined on the basis of z.sub.f(−2) since disturbing obstacles are not present.

(191) This means that the robot is supposed to pass through z.sub.f(−2) at a time index f at which the robot moves by the uniform input during [(f−2)T, fT].

(192) When planning a state at each time step, modifying a trajectory in partial is performed, and when P.sub.min is determined as f−k, (that is, when the entire trajectory is modified) the most calculation occurs.

(193) As above, an obstacle avoiding method in a state-time space according to an embodiment of the present invention has been described. However, a computer-readable recording medium on which a program for employing an obstacle avoiding method in a state-time space, and a program stored on a computer-readable recording medium for employing an obstacle avoiding method in a state-time space may be also feasible.

(194) In other words, those skilled in the art will readily understand that an obstacle avoiding method in a state-time space described above may be provided in a recording medium that can be read by a computer by tangibly embodying in the form of program instructions executable through diverse computing means and may be recorded in computer readable media. In other words, the method may be employed in the form of a program command that can be executed through various computer means, and can be recorded on a computer-readable recording medium. The computer readable media may include independently or associatively program instructions, data files, data structures, and so on. Program instructions recorded in the media may be specially designed and configured for embodiments, or may be generally known by those skilled in the computer software art. Computer readable recording media may include magnetic media such as hard disks and floppy disks, optical media such as CD-ROMs and DVDs, magneto-optical media such as floptical disks, and hardware units, such as ROM, RAM, flash memory, and so on, which are intentionally formed to store and perform program instructions. Program instructions may include high-class language codes executable by computers using interpreters, as well as machine language codes likely made by compilers. The hardware units may be configured to function as one or more software modules for performing operations according to embodiments of the present disclosure, and vice versa.

(195) Although a preferred embodiment of the present invention has been described for illustrative purposes, those skilled in the art will appreciate that various modifications, additions and substitutions are possible, without departing from the scope and spirit of the invention as disclosed in the accompanying claims.