Reactive Collision Avoidance For Autonomous Vehicles Considering Physical Constraints

Abstract

A first robot performs navigation to a predetermined first static or dynamic target location. A first set of velocity candidates is generated for the first robot based on the detection of a first set of one or more velocity obstacles. A first new velocity is selected from the first set of velocity candidates. The first robot is moved at a first velocity corresponding to the first new velocity. The first new velocity is a first desired velocity or a velocity closest to the first desired velocity when at least one velocity candidate of the first set of velocity candidates corresponds to a safe and reachable velocity for the first robot. The first new velocity is a minimum velocity possible by the first robot when each one of the first set of velocity candidates corresponds to a respective unsafe velocity for the first robot.

Claims

1. A method of navigation by a first robot to a predetermined first static or dynamic target location, comprising: generating, via a first computer processor of the first robot, a first set of velocity candidates for the first robot based on the detection of a first set of one or more velocity obstacles; determining, via the first computer processor of the first robot, whether each of the first set of velocity candidates is a safe and reachable velocity for the first robot or an unsafe velocity for the first robot; selecting, via the first computer processor, a first new velocity from the first set of velocity candidates; and moving the first robot at a first velocity corresponding to the first new velocity, wherein the first new velocity is a velocity candidate of the first set of velocity candidates corresponding to a first desired velocity or a velocity closest to the first desired velocity when the first new velocity corresponds to a determined safe and reachable velocity for the first robot, and wherein the first new velocity is a minimum velocity possible by the first robot when each one of the first set of velocity candidates are determined to be unsafe velocities for the first robot.

2. The method according to claim 1, further comprising detecting the presence of the first set of velocity obstacles with a first obstacle detection sensor of the first robot, wherein the first set of velocity candidates is generated when the first obstacle detection sensor detects the presence of the first set of velocity obstacles.

3. The method according to claim 2, wherein the first set of velocity candidates for the first robot is generated only when at least one velocity obstacle of the first set of velocity obstacles is detected to be within a preset distance from the first robot.

4. The method according to claim 2, wherein the first set of velocity candidates for the first robot is generated only when the first set of velocity obstacles detected includes more than one velocity obstacle.

5. The method according to claim 1, further comprising generating, via the first computer processor, a first set of reachable velocities for the first robot when each one of the first set of velocity candidates corresponds to a respective safe but unreachable velocity for the first robot, wherein the first new velocity is a velocity corresponding to a velocity candidate of the first set of velocity candidates, the corresponding velocity candidate being determined, via the first computer processor using a preset algorithm, to correspond to a velocity closest to the first desired velocity.

6. The method according to claim 1, further comprising: detecting the location of the first set of velocity obstacles; and determining, via the first computer processor, collision cones based on the location of the first set of velocity obstacles to ascertain the first set of velocity candidates.

7. The method according to claim 1, wherein the determination of whether each one of the first set of velocity candidates is safe and reachable by the first robot includes calculating, via the computer processor of the first robot, safety and reachability indices using preset algorithms.

8. The method according to claim 1, wherein the minimum velocity possible by the first robot is greater than zero.

9. The method according to claim 1, wherein at least one velocity obstacle of the first set of velocity obstacles of the velocity obstacles is a second robot identical or substantially identical to the first robot.

10. The method according to claim 1, wherein the first robot is an autonomous vehicle.

11. The method according to claim 10, wherein the autonomous vehicle is intended for travel on either one or both of roadways and other terrain.

12. The method according to claim 10, wherein the autonomous vehicle is intended for either one or both of air travel and sea travel.

13. The method according to claim 1, wherein the first robot is a first non-holonomic agent, and wherein the generating of the first set of velocity candidates comprises: determining candidate sets of kinematic values each comprising a respective candidate translational velocity value, a respective candidate rotational velocity value, and a respective candidate angular value providing a possible directional heading for the first robot, each of the sets of kinematic values being based on a kinematics model for the non-holonomic agent; and reducing the candidate sets of kinematic values to only such candidate sets of kinematic values reachable for the first robot if the first robot were a holonomic agent.

14. A robot collision avoidance system, the collision avoidance system comprising: a first robot configured for performing the method according to claim 1; and a second robot configured for performing a method of navigation by the second robot to a predetermined second static or dynamic target location, the second robot being one of the velocity obstacles of the first set of velocity obstacles and the method comprising: generating, via a second computer processor of the second robot, a second set of velocity candidates for the second robot based on the detection of a second set of one or more velocity obstacles, the second set of velocity obstacles including the first robot; selecting, via the second computer processor, a second new velocity from the second set of velocity candidates; and moving the second robot at a velocity corresponding to the second new velocity, wherein the second new velocity is a second desired velocity or a velocity closest to the second desired velocity when at least one velocity candidate of the second set of velocity candidates corresponds to a safe and reachable velocity for the second robot, and wherein the second new velocity is a minimum velocity possible by the second robot when each one of the second set of velocity candidates corresponds to a respective unsafe velocity for the second robot.

15. The robot according to claim 14, wherein the first robot is a first non-holonomic agent, and wherein the generating of the first set of velocity candidates comprises: determining candidate sets of kinematic values each comprising a respective candidate translational velocity value, a respective candidate rotational velocity value, and a respective candidate angular value providing a possible directional heading for the first robot, each of the sets of kinematic values being based on a kinematics model for the non-holonomic agent; and reducing the candidate sets of kinematic values to only such candidate sets of kinematic values reachable for the first robot if the first robot were a holonomic agent.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0024] By way of description only, embodiments of the present disclosure are described herein with reference to the accompanying figures, in which:

[0025] FIG. 1 is a process flow diagram for a velocity obstacle approach in accordance with the prior art;

[0026] FIG. 2 is a model of a global environment that includes a robot and static and dynamic obstacles;

[0027] FIG. 3A is model of a robot and a velocity obstacle along with associated parameters;

[0028] FIG. 3B are plots of velocity boundaries for two examples of acceleration models of non-holonomic agents;

[0029] FIG. 4 is a process flow diagram of a robot motion planning and control process in accordance with an embodiment;

[0030] FIG. 5A is a model of an initial setup of another global environment that includes a robot and velocity obstacles along with collision cones associated with the obstacles as determined by the robot in accordance with another embodiment in which the robot will collide with a single obstacle;

[0031] FIG. 5B is the model of FIG. 5A along with intersection points of colliding obstacle collision cone and the maximum reachable acceleration constraint as determined by the robot;

[0032] FIG. 6A is a model of an initial setup of another global environment that includes a robot and velocity obstacles along with collision cones associated with the obstacles as determined by the robot in accordance with another embodiment in which the robot will collide with multiple obstacles;

[0033] FIG. 6B is the model of FIG. 6A along with potential velocities based on intersection points of colliding obstacle collision cones;

[0034] FIG. 6C is the model of FIG. 6A along with potential velocities on the common edges of maximum acceleration and maximum velocity constraints;

[0035] FIG. 7A is the model of FIG. 5A showing an exactly one potential velocity at a next time step;

[0036] FIG. 7B is a modification of the model of FIG. 5A showing multiple potential velocities at a next time step;

[0037] FIG. 8 is a modification of the model of FIG. 5A showing a maximum acceleration constraint in which there are no potential velocities when the robot will collide with the single obstacle;

[0038] FIG. 9A is the model of FIG. 6A showing an exactly one potential velocity at a next time step which is safe and reachable;

[0039] FIG. 9B is a modification of the model of FIG. 6A showing multiple potential velocities at a next time step which are safe and reachable;

[0040] FIG. 10A is a modification of the model of FIG. 6A showing an exactly one potential velocity which is safe albeit not reachable;

[0041] FIG. 10B is a modification of the model of FIG. 6A showing multiple potential velocities which are safe albeit not reachable; and

[0042] FIG. 11 is a modification of the model of FIG. 6A showing multiple potential velocities which are reachable albeit not safe.

DETAILED DESCRIPTION

Modeling of the Environment

[0043] Referring now to the drawings, in one example model, a two-dimensional world space W is defined as having a radius such that W=.sup.2. As shown in FIG. 2, in this example, world space W includes robot A, which may be an autonomous robot, and a set of obstacles O.sub.1-O.sub.5 which are subsets of the world space and form highly dynamic dense environment. For simplicity, the robot is modeled geometrically and is controllable via a motion plan, and each obstacle is an object within world space W that either permanently occupies the world space, e.g., a building or other permanent or semi-permanent fixture, or temporarily occupies the world space, e.g., as in the examples of pedestrians, vehicles, or moving robots such as autonomous vehicles. In reality, obstacles come in all shapes and sizes, but in the interest of aiding understanding, any polygon obstacle may be replaced with a circle to simplify the geometry modeling.

[0044] Accordingly, In this example, robot A is modeled as a disc-shaped holonomic agent having p.sub.A, r.sub.A, and v.sub.A as its position, radius, and velocity, respectively, and each of obstacles O.sub.1-O.sub.5, which are either static or dynamic, are also modeled as disc-shaped agents having p.sub.B, r.sub.B, and v.sub.B as their positions, radii, and velocities, respectively, in which B is a variable representing the respective obstacle number and static obstacles are shown without a velocity. For robot A and a given obstacle B, the velocity obstacle VO.sub.B.sup.A(v.sub.B) is defined as the set of velocities for robot A that would result in a collision with B at time t?T in which T is the present time, i.e., at some time in the future. In order to define VO.sub.B.sup.A(v.sub.B), the concept of Minkowski for summing two objects A and B, A?B, is applied. See Hadwiger, Hugo, Minkowskische Addition und Subtraktion beliebiger Punktmengen und die Theoreme von Erhard Schmidt. Math. Z., 53 (3): 210-218, doi:10.1007/BF01175656. If ?A is the reflection of A in its reference point, and function ?(p, v)={p+vt|t?0} defines a ray starting at point p and heading in the direction of v, then VO.sub.B.sup.A(v.sub.B) can be expressed by Eq. 1 as follows:

[00001]$\begin{array}{cc}{\mathrm{VO}}_B^A\left(v_B\right)=\left\{v_A.Math.?\left(p_A,v_A-v_B\right).Math.B?-A\right\}??& \left(1\right)\end{array}$

[0045] Concisely, VO.sub.B.sup.A(v.sub.B) is a collision cone having an apex at v.sub.B as shown in FIG. 3A modeling relative positions and velocities of robot A and an obstacle B. In the modeling and while considering the size of obstacles, the radius of the obstacles is their original radius plus the radius of the robot such that in the final model, the robot is a point without sizes.

Physical Constraints

[0046] To avoid obstacles safely, robot A should select a safe motion that avoids of velocity obstacle VO.sub.B.sup.A(v.sub.B). The selected motion should be from a set of reachable and feasible safe motions. Specifically, robot A should move along under certain conditions set by the environment, e.g., road rules, and set by mechanical characteristics of the robot. Where robot A is a simple car-like robot in which (x, y) is a given position of the robot and ? is a given orientation of the robot at time t, the kinematic constraints for navigating the robot are expressed by Eq. 2 as follows:

[00002]$\begin{array}{cc}\{\begin{array}{c}{x}^{}\left(t\right)=v\left(t\right)\cos ?\left(t\right)\\ y^{}\left(t\right)=v\left(t\right)\sin ?\left(t\right)\\ {?}^{}\left(t\right)=v\left(t\right)\frac{\tan ?\left(t\right)}{L}\end{array}& \left(2\right)\end{array}$

according to LaValle, S.M., Planning Algorithms, Cambridge: Cambridge University Press 2006, the entirety of the disclosure of which hereby being incorporated by reference, where ?(t) is the angle between the velocity of the robot and the horizontal axis at time t; v and ? are the controls of the robot, i.e., the speed and rotation angle, respectively, of the robot; and L is the distance between the front and rear wheels of the robot. An expression for the position of the robot at time t under the assumption that the controls remain constant can be calculated by integrating Eq. 2. Based on the controls of the robot, a first physical constraint that will be imposed is the rotation angle ?. The robot A can be rotated clockwise or counterclockwise by some angle ??[(0, 2?) by mapping every (x.sub.A, y.sub.A)?A according to the correlation identified as (3) below:

[00003]$\begin{array}{cc}\left(x_A,y_A\right).fwdarw.\left(x_A\cos ?-y_A\sin ?,x_A\sin ?+y_A\cos ?\right)& \left(3\right)\end{array}$

[0047] A second physical constraint that will be imposed is a road speed limit imposed by the governing law for road vehicles which may be expressed by Eq. 4 as follows:

[00004]$\begin{array}{cc}{\left(x-x_A\right)}^2+{\left(y-y_A\right)}^2<{\left(V?t\right)}^2& \left(4\right)\end{array}$

wherein x.sub.A and y.sub.A are the respective coordinates of the current location of robot A and V is maximum allowed speed (V.sub.max) for a given time step ?t. A third physical constraint that will be imposed is a maximum reachable acceleration while robot A is seeking a next motion to be made in which the maximum reachable acceleration is expressed by Eq. 5 as follows:

[00005]$\begin{array}{cc}{\left(x-x_v\right)}^2+{\left(y-y_v\right)}^2<{\left(a_A\right)}^2& \left(5\right)\end{array}$

wherein x.sub.v and y.sub.v are the respective coordinates of the current velocity and a.sub.A is calculated based on a maximum acceleration of the robot a.sub.max as a.sub.A=?a.sub.max??t.sup.2.

[0048] Other physical constraints can be added which will essentially reduce the reachable space of the robot. These constraints include but are not limited to the following: a maximum angular acceleration constraint, a non-holonomic kinematics constraint, a driving comfort related constraint such as a change of the acceleration constraint, and any other traffic rules related constraint such as a driving within a lane constraint.

[0049] Unlike holonomic agents, non-holonomic agents have fewer controllable degrees of freedom than total degrees of freedom and therefore cannot move with equal freedom in all directions. A differential drive robot is one example of a non-holonomic agent. Analytical solutions for a differential drive robot are described herein by way of example, but similar analytic solutions could be applied to other non-holonomic agents. For a typical differential drive robot, the two controllable degrees of freedom are the left and right wheel rotation velocity, v.sub.r and v.sub.l, respectively. The kinematics model for such a robot may be expressed as the following:

[00006]$\{\begin{array}{c}v=\frac{v_r+v_l}{2}\\ ?=\frac{v_r-v_l}{L}\\ ?={?}_0+\mathrm{??}t\end{array},$

where v is the translational velocity of the robot, ? is the angular velocity of the robot, L is the distance between left and right wheels, ?.sub.0 is the previous heading direction of the agent, and ? is the updated heading direction after control cycle ?t. In this manner, a wheel velocity boundary may be mapped to an agent velocity boundary. The reachable velocity in x- and y-directions (i.e., v.sub.x and v.sub.y) of the non-holonomic agent is determined by as follows:

[00007]$\{\begin{array}{c}{v}_x=v*\cos \left({?}_0+\mathrm{??}t\right)\\ v_y=v*\sin \left({?}_0+\mathrm{??}t\right)\\ ?={?}_0+\mathrm{??}t\end{array}.$

? is the angular velocity of the robot, L is the distance between left and right wheels, ?.sub.0 is the previous heading direction of the agent, and ? is the updated heading direction after control cycle ?t. In this manner, a wheel velocity boundary may be mapped to an agent velocity boundary.

[0050] The range of possible directional velocities within a given amount of time, or velocity boundary, for a holonomic agent can be plotted, as illustrated in the examples of FIG. 3A, as a circle around the agent. Unlike holonomic agents, the velocity boundary for a non-holonomic agent has a non-circular boundary as shown in the further examples of FIG. 3B.

[0051] The non-circular shape of the velocity boundary for a differential drive robot is a function of the robot's turning capabilities. If the rotation angle is constant, the reachable velocity profile, i.e., will be a rhombus since the translational velocity boundary will be linear along each of its sides. However, if the reachable angular velocity is not constant, and instead depends on a current angular velocity, different rotation angles per unit time will be possible depending on the state of the agent. If such rotation angle difference is nontrivial, the reachable velocity profile would have a nonlinear boundary.

[0052] To apply the navigation solutions described elsewhere in this disclosure to non-holonomic agents, the movement of the non-holonomic agent can be constrained to force a circular velocity profile at a hardware or software level. As shown in FIG. 3B, a circular reduced velocity boundary can be determined within the non-holonomic agent's actual velocity boundary. Where the actual velocity boundary has linear or nearly linear sides as shown in the example presented on the left side of FIG. 3B, the radius of the reduced velocity boundary is determined by the shortest distance from the current velocity to any of the four sides of the actual velocity boundary. Where the lines are significantly non-linear, the radius of the reduced velocity boundary is determined iteratively with an optimization function as known to those skilled in the art.

[0053] By constraining the non-holonomic agent's movements to a circular reduced velocity boundary, the non-holonomic agent can be made functionally equivalent to a holonomic agent. Thus, approaches to navigation disclosed herein that assume that agents change their directional, i.e., x- and y-, velocities in a linearly proportional manner are equally applicable to holonomic agents and constrained non-holonomic agents.

New Velocity Setting

[0054] It is believed that only randomized motion planning may be adequate to provide collision avoidance capabilities within complex and/or highly dynamic environments, e.g., crowded crossings, in which controlled robot samples come arbitrarily close to any configuration of obstacles with a high probability as the complexity of the environment increases. As the computation time spent on collision checking of randomly selected obstacles is insufficient in such environments using the current velocity obstacle (VO) approach, robot motion planning and control process 100 as shown in FIG. 4 overcomes these limitations.

[0055] Referring now to FIG. 4, upon initialization or at an arbitrary start time t.sub.0 at a starting step 100A of robot A, at step 101 of process 100 at time t.sub.i, robot A may seek the location and velocity of any obstacles in the sensing range of sensors of the robot, which may be but are not limited to being one or more Light Detection and Ranging (LiDAR) sensors to develop respective collision cones and/or collision cone boundaries relating to each of the identified obstacles, such as obstacles O.sub.1-O.sub.5 in the example of FIG. 2, and within the given physical constraints of the robot. At step 102, the robot may determine whether there are any sufficiently close obstacles which may be based on a preset level of closeness. If no obstacles are identified as being sufficiently close, then at step 103 a desired velocity of robot A may be calculated or otherwise set, e.g., via a microprocessor or a microcontroller, and the robot then may be actuated to move at a velocity corresponding to the desired velocity. Such desired velocity towards the goal destination of robot A may be based on the current position of the robot and the location of the goal destination, and must be reachable and safe.

[0056] If there are sufficiently close obstacles, at step 104, robot A may determine, e.g., via a microprocessor or a microcontroller, whether there will be a collision with any the identified obstacles. If not, robot A may proceed to step 103. If yes, at step 105, robot A may determine, e.g., via a microprocessor or microcontroller, whether the robot will collide with multiple obstacles at the current planned motion of the robot. If no, then at step 106, robot A may calculate, e.g., via a microprocessor or microcontroller, new velocity NewV corresponding to a possible new velocity of the robot by applying an algorithm for use when a collision with a single obstacle is determined to be imminent at the current planned motion of the robot. In this instance, at step 107, robot A may determine whether the calculated new velocity NewV is safe and if so, the robot may be actuated to move at a new velocity corresponding to NewV. which would correspond to the closest velocity to the desired velocity, and if no, then at step 108, robot A may choose a new velocity which gives the largest time to collision (TTC) at that instant in time. For a single obstacle collision, the largest TTC will force the robot to reduce the current velocity of the robot as much as possible, in which the TTC is calculated based on the ratio of the distance between the robot and the single obstacle to the relative velocity between the robot and the single obstacle.

[0057] If robot A determines that there will be a collision with multiple obstacles, then at step 109, robot A may calculate, e.g., via a microprocessor or microcontroller, intersection points of the developed collision cone boundaries of the identified obstacles to determine a set of potentially acceptable new velocities NewV.sub.i, and at step 110, the robot may determine, via a microprocessor or microcontroller, whether any of the determined potentially acceptable new velocities NewV.sub.i are both safe and reachable by the robot by calculating safety and reachability indices. If any of the determined velocities NewV.sub.i are both safe and reachable to the goal destination by the robot, then at step 111, robot A, e.g., via a microprocessor or microcontroller, may select the closest velocity among such determined set of potentially acceptable new velocities NewV.sub.i to the desired velocity of the robot at that instant in time and accordingly be actuated to move at a velocity corresponding to the selected velocity among the determined set of potentially acceptable new velocities NewV.sub.i. If none of the determined possible velocities NewVi are both safe and reachable, then at step 112 a set of reachable velocities closest to the desired velocity may be determined by robot A, and at step 113, the robot may determine velocities among the set of reachable velocities closest to the desired velocity that are safe, if any. If velocities among the set of reachable velocities closest to the desired velocity are safe, then at step 114, the robot A may select the velocity among such velocities that are closest to the desired velocity and accordingly be actuated to move at a velocity corresponding to the selected velocities among the velocities that are closest to the desired velocity. If none of the velocities among the set of reachable velocities closest to the desired velocity are safe, then at step 115, robot A may select a new velocity which gives the largest TTC at that instant in time. For multiple obstacle collision, the largest TTC will force the robot to reduce the current velocity magnitude as much as possible while choosing the collision obstacle being the furthest distance from the robot.

[0058] At step 116, robot A updates the location of the robot based on a global positioning unit attached to the robot or via a calculation of the new coordinates of the robot based on the immediately preceding location of the robot and the preceding velocity of the robot over the respective preceding time interval. At step 117 occurs at time t.sub.i+1, robot A determines if its goal has been reached and the current iteration is over. If robot A has reached its goal, at step 120, then robot motion planning and control process 100 ends. Otherwise, robot A again may seek the location and velocity of any obstacles in the sensing range of the robot at step 101 and the remainder of the steps of process 100 are repeated as appropriate for the given conditions at that time and that position and velocity of the robot.

[0059] In the discussion noted above, the references to a microprocessor or microcontroller may be to the same or a different microprocessor or microcontroller. Additionally, the term goal destination may be a particular location or locations within a predetermined distance from or boundary around a particular location, e.g., in the neighborhood of a particular desired location.

[0060] Referring now to the particular examples shown FIGS. 5A-11, a robot moving towards a goal designated by an asterisk is shown by the white circle, obstacles to the robot are shown by the gray circles, and the dashed cone shapes are velocity obstacles. In the case of colliding with a single obstacle, the potential motions of the robot are the intersection points of colliding obstacle collision cone and maximum reachable acceleration constraint (Eq. 5) (FIG. 5B). However, if the robot will collide with multiple obstacles, its potential motions are Set-1 corresponding to the intersection points of colliding obstacles collision cones (FIG. 6B) and Set-2 corresponding to the velocities on the common edges of maximum acceleration and maximum velocity constraints (FIG. 6C).

[0061] Determining the new velocity has two major cases and some subcases. In both cases, the subcases discussed further herein may be useful to aid understanding.

[0062] In the first case, when the robot will collide with one obstacle, the subcases are as follows: [0063] Subcase-1) there are one or more potential velocities which are intersection point(s) of colliding obstacle collision cone and maximum reachable acceleration constraint: [0064] 1-1) There is one potential reachable velocity (FIG. 7A): If this velocity is safe, this velocity will be considered as the next time step velocity. Otherwise, Subcase-2 will be considered in determining the next time step velocity. [0065] 1-2) There are multiple potential reachable velocities (FIG. 7B): Firstly, any safe candidate(s) is considered. If there is one safe candidate, it will be considered as the next time step velocity. In the situation of having multiple safe potential velocities, firstly the degrees of deviation from the desired velocity toward each of these potential velocities are calculated and then, the next time step velocity is selected as the velocity with minimum deviation from the desired velocity. Otherwise, Subcase-2 will be considered in determining the next time step velocity. [0066] Subcase-2) There is no intersection point: If there is no potential velocity which is safe (FIG. 8), the speed will be reduced as much as possible in every time step until fully stop.

[0067] In the second case, when the robot will collide with multiple obstacles, the following subcases should be considered: [0068] Subcase-1) In Set-1, there are one or more potential velocities which are safe and reachable: [0069] 1-1) There is only one potential velocity which is safe and reachable: In such incident, this velocity will be considered as the next time step velocity (FIG. 9A). [0070] 1-2) There are multiple potential velocities which are safe and reachable: For this case, firstly the degrees of deviation from the desired velocity toward each of these potential velocities are calculated, and then, the next time step velocity is selected as the velocity with minimum deviation from the desired velocity (FIG. 9B). [0071] Subcase-2) In Set-1, there are one or more potential velocities which are safe but not reachable: [0072] 2-1) There is only one potential velocity which is safe but not reachable: In this incident, the algorithm will consider a potential velocity from Set-2 (i.e., a reachable velocity) which is closest to this safe but not reachable velocity (FIG. 10A). If there is no safe motion in Set-2, subcase 2-3 will be considered. [0073] 2-2) There are multiple potential velocities which are safe but not reachable: For this case, a potential velocity from Set-2 (i.e., a reachable velocity) which is safe and closest to the desired velocity will be selected (FIG. 10B). The result is the same as subcase 2-1 in that if there is no safe motion in Set-2, subcase 2-3 will be considered. [0074] 2-3) If there is no potential velocity in set-2 which is safe (FIG. 11), the speed will be reduced as much as possible in every time step until the robot is at a complete stop. However, if the robot has enough safety distance from the obstacles, the robot will consider the desirable velocity for the next time step. [0075] Subcase-3) In Set-1, there are one or more potential velocities which are not safe. If there is no potential velocity in set-2 which is safe, the speed will be reduced as much as possible in every time step until the robot is at a complete stop. Otherwise, the safe velocity in set-2 which is closest to the desired velocity will be selected.

[0076] It is to be understood that the disclosure set forth herein includes any possible combinations of the particular features set forth above, whether specifically disclosed herein or not. For example, where a particular feature is disclosed in the context of a particular aspect, arrangement, configuration, or embodiment, that feature can also be used, to the extent possible, in combination with and/or in the context of other particular aspects, arrangements, configurations, and embodiments of the disclosure. Furthermore, although the disclosure herein has referred to particular features, it is to be understood that these features are merely illustrative of the principles and applications of the present invention. It is therefore to be understood that numerous modifications may be made to the illustrative embodiments and that other arrangements may be devised without departing from the spirit and scope of the present disclosure.