Collision-Free Dynamic Window Approach for Moving Obstacles

Abstract

A robot is navigated to a target location passively collision-free. An obstacle (21) is detected by sensors. An obstacle velocity dynamic window is calculated within a control cycle. An obstacle mobility boundary is determined and inflated to an inflated boundary that includes a collision threshold. Admissible velocities of the robot are identified as those in which the robot would be outside the inflated boundary at a next control cycle or the robot velocity is reduced if there is no admissible velocity. An optimal velocity among admissible velocities is selected. The position of the robot is updated, and the process is repeated until the target location is reached. Without the use of an inflated boundary, admissible velocities of the robot are identified as those that avoid projected obstacle boundaries for a preset number of possible obstacle trajectories.

Claims

1. A method of navigation by a robot to a predetermined first static or dynamic target location, comprising: obtaining, during a predetermined first time cycle, a first obstacle position of a first obstacle, a first obstacle velocity of the first obstacle at the first obstacle position, and a first obstacle acceleration of the first obstacle at the first obstacle position; determining, via one or more computer processors and during the first time cycle, a first obstacle first velocity dynamic window for the first obstacle based on the first obstacle velocity and the first obstacle acceleration; determining, via one or more computer processors and during the first time cycle, a first obstacle first mobility boundary defining a first set of subsequent obstacle positions reachable by the first obstacle during or at the completion of a predetermined second time cycle following the first time cycle from the first obstacle position based on the first obstacle first velocity dynamic window, the second time cycle being of equal duration to the first time cycle; selecting, via one or more computer processors and during the first time cycle, a first new robot velocity to be applied to the robot at the completion of the first time cycle from a set of first new robot velocity candidates for the robot, the first new robot velocity i) being one at which a first new robot position of the robot at the completion of the second time cycle is outside of a first obstacle first inflated boundary of the first obstacle spaced from the first obstacle first mobility boundary by a predetermined offset or ii) being a first reduced robot velocity when there is no reachable position for the robot outside of the first obstacle first inflated boundary at the completion of the second time cycle; and moving the robot at the first new robot velocity during the predetermined second time cycle and immediately following the first time cycle.

2. The method of claim 1, wherein the obtaining step comprises detecting, via at least a first sensor, any one or any combination of the first obstacle position, the first obstacle velocity at the first obstacle position, and the first obstacle acceleration at the first obstacle position.

3. The method of claim 1, further comprising determining, via the one or more computer processors, the first obstacle first inflated boundary by offsetting each of the obstacle positions of the first set of subsequent obstacle positions reachable by the first obstacle during or at the completion of a predetermined second time cycle by a fixed distance in radial directions from the first obstacle first inflated boundary.

4. The method of claim 1, further comprising determining the set of first new robot velocity candidates from a robot first velocity dynamic window based on a current robot velocity of the robot, and a current robot acceleration of the robot during the first time cycle.

5. The method of claim 1, wherein the selecting of the first new robot velocity comprises: determining a set of first new robot position candidates corresponding to the set of first new robot velocity candidates, the set of first new robot position candidates being at radial distances from the current position of the robot equal to a product of a length of the first time cycle and respective ones of the set of first new robot velocity candidates; and comparing the set of first new robot position candidates to the first obstacle first inflated boundary.

6. The method of claim 1, wherein the selecting of the first new robot velocity comprises selecting a first new robot velocity candidate of the set of first new robot velocity candidates having the highest value based on an objective function.

7. The method of claim 1, further comprising: 1) obtaining, at or after the completion of the moving of the robot and during a predetermined subsequent time cycle, a first obstacle actual position of the obstacle, a first obstacle actual velocity of the first obstacle at the first obstacle actual position, and a first obstacle actual acceleration of the first obstacle at the first obstacle actual position; determining, via one or more computer processors and during the subsequent time cycle, a first obstacle subsequent velocity dynamic window for the first obstacle based on the first obstacle actual velocity and the first obstacle actual acceleration; determining, via one or more computer processors and during the subsequent time cycle, a first obstacle subsequent mobility boundary defining a second set of subsequent obstacle positions reachable by the first obstacle during or at the completion of a predetermined further subsequent time cycle following the subsequent time cycle from the first obstacle actual position based on the first obstacle subsequent velocity dynamic window; 2) selecting, via one or more computer processors and during the subsequent time cycle, a subsequent new robot velocity to be applied to the robot at the completion of the subsequent time cycle from a set of subsequent new robot velocity candidates for the robot, the subsequent new robot velocity i) being one at which a subsequent new robot position of the robot at the completion of the further subsequent time cycle is outside of a first obstacle subsequent inflated boundary of the first obstacle spaced from the first obstacle subsequent mobility boundary by a predetermined offset or ii) being a subsequent reduced robot velocity when there is no reachable position for the robot outside of the first obstacle subsequent inflated boundary at the completion of the further subsequent time cycle, the subsequent and the further subsequent time cycles being of equal duration to the first time cycle; 3) moving the robot at the subsequent new robot velocity during the further subsequent time cycle; and 4) repeating steps 1-3 until the robot reaches the target location.

8. The method of claim 1, wherein a maximum first obstacle linear velocity is stored, via the computer processor, in a memory of the robot, and wherein the first obstacle velocity dynamic window is limited by the maximum first obstacle linear velocity.

9. The method of claim 1, further comprising wirelessly receiving, via a radio receiver, any one or any combination of the first obstacle velocity, the first obstacle acceleration, and a maximum first obstacle linear velocity from the first obstacle upon the detecting of the first obstacle; and storing, via the one or more computer processors, in a memory of the robot the one or combination of the first obstacle velocity, the first obstacle acceleration, and the maximum first obstacle linear velocity wirelessly received.

10. The method of claim 1, further comprising generating, via the computer processor, the set of first new robot velocity candidates in response to the detection of the first obstacle.

11. The method of claim 1, further comprising obtaining one or more additional obstacle positions of one or more respective additional obstacles, one or more respective additional obstacle velocities of the one or more additional obstacles at the one or more respective additional obstacle positions, and one or more respective additional obstacle accelerations of the one or more additional obstacles at the one or more respective additional obstacle positions during the first time cycle, wherein the first new robot velocity is selected as one at which the first new robot position of the robot at the completion of the second time cycle is outside of each of one or more respective additional obstacle first inflated boundaries of the respective one or more additional obstacles or is the first reduced robot velocity when there is no reachable position for the robot outside of every one of the first obstacle first inflated boundary and the one or more respective additional obstacle first inflated boundaries at the completion of the second time cycle, the one or more additional obstacle first inflated boundaries being spaced by respective predetermined offsets from one or more respective additional obstacle first mobility boundaries reachable by the one or more respective additional obstacles from the one or more respective additional obstacle positions at the completion of the second time cycle.

12. The method of claim 11, wherein the obtaining step comprises detecting, via at least the first sensor or at least one other sensor, any one or any combination of the one or more respective additional obstacle positions, the one or more respective additional obstacle velocities at the one or more respective additional obstacle positions, and the one or more respective additional obstacle accelerations at the one or more respective additional obstacle positions.

13. A method of navigation by a robot to a predetermined first static or dynamic target location within a dynamic environment, the dynamic environment having a predefined boundary, comprising: obtaining respective obstacle positions of a first set of obstacles within the predefined boundary, respective obstacle velocities of the first set of obstacles at the respective obstacle positions, and respective obstacle accelerations of the first set of obstacles at the respective obstacle positions; determining, via one or more computer processors and within a predetermined first time cycle, a predefined quantity of respective obstacle trajectories for each of the obstacles of the first set of obstacles based on the respective obstacle velocities of the first set of obstacles at the respective obstacle positions and the respective obstacle accelerations of the first set of obstacles at the respective obstacle positions; selecting, via the one or more computer processors, a first new robot velocity to be implemented for the robot in a subsequent time cycle from a first set of robot velocity candidates for the robot, the first new robot velocity i) being one at which a first robot position of the robot at the completion of the subsequent time cycle overlaps none of the determined respective obstacle trajectories at the completion of the subsequent time cycle or ii) being a first reduced robot velocity when there is no reachable position for the robot that does not overlap at least one of the determined respective obstacle trajectories at the completion of the subsequent time cycle; and moving the robot at the first new robot velocity during the subsequent time cycle.

14. The method of 13, wherein the obtaining step comprises detecting, via at least the first sensor or at least one other sensor, any one or any combination of the respective obstacle positions, the respective obstacle velocities at the respective obstacle positions, and the respective obstacle accelerations at the respective obstacle positions.

15. The method of claim 13, further comprising determining, via the one or more computer processors, the first set of robot velocity candidates from a robot first velocity dynamic window based on a current robot velocity of the robot and a current robot acceleration of the robot during the first time cycle.

16. The method of claim 13, further comprising: 1) obtaining respective subsequent obstacle positions of the first set of obstacles, respective subsequent obstacle velocities of the first set of obstacles at the respective subsequent obstacle positions, and respective subsequent obstacle accelerations of the first set of obstacles at the respective obstacle positions; 2) determining, via the one or more computer processors and within the subsequent time cycle, a predefined quantity of respective subsequent obstacle trajectories for each of the obstacles of the first set of obstacles based on the respective subsequent obstacle velocities of the first set of obstacles at the respective subsequent obstacle positions and the respective obstacle accelerations of the first set of obstacles at the respective subsequent obstacle positions; selecting, via the one or more computer processors, a subsequent new robot velocity to be implemented for the robot in a further subsequent time cycle from a subsequent set of robot velocity candidates for the robot, the subsequent new robot velocity i) being one at which a subsequent robot position of the robot at the completion of the further subsequent time cycle overlaps none of the determined respective obstacle trajectories at the completion of the further subsequent time cycle or ii) being a subsequent reduced robot velocity when there is no reachable position for the robot that does not overlap at least one of the determined respective obstacle trajectories at the completion of the further subsequent time cycle; 3) moving the robot at the subsequent new robot velocity during the further subsequent time cycle; and 4) repeating steps 1-3 until the robot reaches the target location.

17. The method of claim 13, further comprising determining the predefined quantity of the respective obstacle trajectories to be determined for each of the obstacles of the first set of obstacles within the first time cycle by, prior to obtaining the respective obstacle positions: 1) defining, via the one or more computer processors, a respective maximum translational velocity of each of obstacles of the first set of obstacles; 2) determining, via the computer processor, a obstacle velocity dynamic window for each of the obstacles of the first set of obstacles based on the respective maximum translational velocities of each of the obstacles of the first set of obstacles; 3) determining, via the computer processor, a final step obstacle mobility boundary for each of the obstacles of the first set of obstacles at a final time step of the time interval T based on the respective maximum translational velocities of each of the obstacles of the first set of obstacles; 4) generating, via the one or more computer processors, test quantities of obstacle velocity candidates for each of the obstacles of the first set of obstacles; 5) determining, via the one or more computer processors, respective test obstacle positions for each of the obstacles of the first set of obstacles, each of the respective test obstacle positions being based on an initial position of the largest obstacle and a respective one of the largest obstacle velocity candidates; 6) determining, via the one or more computer processors, respective test obstacle regions for each of the obstacles of the first set of obstacles, each of the respective test obstacle regions corresponding to a predefined collision threshold applied about the respective test obstacle positions; 7) repeating steps 4-6 by applying, in step 4, a larger test quantity of obstacle velocity candidates within the respective obstacle dynamic windows determined for each of the obstacles of the first set of obstacles than those generated during an immediately preceding performance of step 4 for any one or more of the obstacles of the first set of obstacles when the respective test obstacle regions of the one or more obstacles of the first set of obstacles do not cover the respective determined final step obstacle mobility boundary associated with the one or more obstacles of the first set of obstacles; and 8) storing in a memory of the robot, via the one or more computer processors, the test quantities of the obstacle velocity candidates for use as the predefined quantity of respective obstacle trajectories when the respective test obstacle regions of the one or more obstacles of the first set of obstacles cover the respective determined final step obstacle mobility boundary associated with the one or more obstacles of the first set of obstacles.

18. The method of claim 17, wherein the larger test quantity of largest obstacle velocity candidates within the largest obstacle dynamic window applied at step 6 is one greater than the one applied during the immediately preceding performance of step 3.

19. The method of claim 17, wherein the predefined collision threshold is a circle having a predefined radius.

20. A robot configured for navigation to a predetermined first static or dynamic target location, comprising: an object detection system configured for obtaining, during a predetermined first time cycle, a first obstacle position of a first obstacle, a first obstacle velocity of the first obstacle at the first obstacle position, and a first obstacle acceleration of the first obstacle at the first obstacle position; a computer system in communication with the object detection system and comprising a non-transitory computer-readable storage medium on which computer readable instructions of a program are stored and one or more computer processors, wherein the instructions, when executed by the one or more processors, cause the computer system to perform the following: (i) determine, during the first time cycle, a first obstacle first velocity dynamic window for the first obstacle based on the first obstacle velocity and the first obstacle acceleration; (ii) determine, during the first time cycle, a first obstacle first mobility boundary defining a first set of subsequent obstacle positions reachable by the first obstacle during or at the completion of a predetermined second time cycle following the first time cycle from the first obstacle position based on the first obstacle first velocity dynamic window, the second time cycle being of equal duration to the first time cycle; and (iii) select, during the first time cycle, a first new robot velocity to be applied to the robot at the completion of the first time cycle from a set of first new robot velocity candidates for the robot, the first new robot velocity i) being one at which a first new robot position of the robot at the completion of the second time cycle is outside of a first obstacle first inflated boundary of the first obstacle spaced from the first obstacle first mobility boundary by a predetermined offset or ii) being a first reduced robot velocity when there is no reachable position for the robot outside of the first obstacle first inflated boundary at the completion of the second time cycle; and a vehicle power system in communication with the computer system and configured for moving the robot at the first new robot velocity during the predetermined second time cycle and immediately following the first time cycle.

21. (canceled)

Description

BRIEF DESCRIPTION OF THE DRAWINGS

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

[0056] FIG. 1 is a schematic plot of admissible velocities of an AV agent within a static obstacle environment;

[0057] FIG. 2 is a process flow diagram for an AV agent to reach a goal destination using a process for identifying and applying an admissible velocity for the agent based on an obstacle's mobility samples;

[0058] FIG. 3 is a schematic plot of a predefined set of projected mobility positions of an obstacle of radius r.sub.obs to assess coverage by the projected mobility positions over an entire obstacle mobility boundary .sub.k at a final time step within a time interval T;

[0059] FIG. 4 is a process flow diagram for an AV agent to reach a goal destination using a process for identifying and applying an admissible velocity for the agent based on an obstacle's mobility regions; and

[0060] FIG. 5 is a schematic plot of an example position boundary .sub.k for an obstacle with a predefined offset of radius r=1 as a safety threshold for which an agent's position is inadmissible.

DETAILED DESCRIPTION

[0061] Referring now to FIG. 2, modified DWA approach process 10 identifies admissible velocities of an agent, e.g., a robot, based on mobility samples for obstacles within an operating environment of the agent. Upon the start of collision-free travel of an agent using approach 10, at step 11 that occurs during one control cycle t.sub.i of the agent and which may be said to begin at time to, the agent detects obstacles in a sensing range of the agent using sensors on the robot, which may be but are not limited to being one or more Light Detection and Ranging (LiDAR) sensors. In arrangements that employ LiDAR sensors, the detected light may be used to develop respective collision cones and/or collision cone boundaries relating to each of the identified obstacles. At step 12, a velocity DW within the agent's control cycle t.sub.i, e.g., 0.1 seconds, is calculated, e.g., via one or more microprocessors or microcontrollers of the agent, for each of the detected obstacles using Criterion 1. At step 13, a quantity of N trajectories for each obstacle at the end of preset time period T are projected by the agent by applying Criterion 2.1 or 2.2, as appropriate, via one or more microprocessors or microcontrollers of the agent, using k=T/t time steps. For example, for a time period T preset via one or more microprocessors or microcontrollers of the agent to three (3) seconds and an agent control cycle t.sub.i=0.1 sec, the agent would project each obstacle's trajectory 30 cycles out based on the respective current translational and angular velocities and current translational and angular accelerations or decelerations of each obstacle and assuming the angular accelerations or decelerations remain constant over the period T.

[0062] Still referring to FIG. 2, the value of N for the number of trajectories to be projected by the agent may be determined, in some arrangements, by trajectory sample count determination process 1, which may be conducted during an offline analysis prior to the travel of the agent using modified DWA approach process 10. At step 2 of trajectory sample count determination process 1, an obstacle velocity DW with the agent's control cycle t.sub.i is calculated, via one or more microprocessors or microcontrollers of one or more local or remote computers designated for conducting such analysis, for each of the obstacles planned for the operating environment of the agent during operation of the agent using modified DWA approach process 10. To obtain a worst-case scenario, the obstacle velocity DW for each of the obstacles preferably may be determined using a respective maximum translational velocity for each of the obstacles such that the obstacles' translational velocity as well as the obstacles' translational and rotational accelerations are not needed to determine the respective obstacle velocity DWs.

[0063] At step 4 of process 1, an obstacle mobility boundary .sub.k for each of the obstacles is determined, via the one or more microprocessors or microcontrollers of the one or more local or remote computers designated for conducting such analysis, using Criterion 2.1 or 2.2, as appropriate, at a final time step over the time period of travel T for the agent, i.e., the period (T.sub.t) to T. To obtain a worst-case scenario, i.e., a largest possible obstacle mobility boundary, the maximum translational velocity is used as the velocity in Criterion 2.1 or 2.2, as appropriate, and rotational velocity is not considered. The sample starting positions (x.sub.0.sup.obs, y.sub.0.sup.obs) for each of the obstacles may be determined, via the one or more microprocessors or microcontrollers of the one or more local or remote computers designated for conducting such analysis, by a known sampling scheme such as a full factorial sampling, and further may depend on the shape of the region .sub.k in order to obtain starting positions that lead to the largest possible obstacle mobility boundaries for each of the respective obstacles. As such, it is to be understood that the sample starting positions and thereby the chosen sampling scheme influences the minimum number of required samples.

[0064] At step 5 of process 1, regardless of the shape of the region .sub.k, a set of quantity Q obstacle velocity samples from the respective velocity spaces (v.sup.bs, .sup.obs) of each of the obstacles are selected for each of the obstacles, via the one or more microprocessors or microcontrollers of the one or more local or remote computers designated for conducting such analysis, by a known sampling scheme such as a fractional factorial sampling where Q=mn and m and n are the number of discrete levels for translational and angular velocity in V.sub.d.sup.obs, respectively. It is to be understood that the value of Q may be the same or may vary among the set of obstacles being evaluated.

[0065] At step 6 of process 1, each velocity sample for the set of quantity Q obstacle velocity samples of each of the obstacles are applied using Criterion 2.1 or 2.2, as appropriate, at the final time step, i.e., the period (T-d t) to T, via the one or more microprocessors or microcontrollers of the one or more local or remote computers designated for conducting such analysis, to obtain quantity Q, e.g., 3, 4, 5, . . . , of sample projected mobility positions for each of the obstacles. Again, it is to be understood that the value of Q may be the same or may vary among the set of obstacles being evaluated. In one example shown in FIG. 3, obstacle 21 having a maximum translational velocity v.sub.max.sup.obs and having a current position within test environment 20 (represented as a plot) as shown may have sample projected mobility positions at the centers of regions 22A-22I. As shown, Q=9 in this example.

[0066] At step 7 of process 1, for each of the obstacles, a collision threshold r is then applied, via the one or more microprocessors or microcontrollers of the one or more local or remote computers designated for conducting such analysis, to each of the sample mobility boundaries. The collision threshold r may be a radius dependent on the radius of each of the respective obstacles such that the collision threshold may vary from obstacle to obstacle. In some arrangements, the collision threshold may be defined by r.sub.agent+r.sub.obs (see Criterion 3.1 or 3.2, as appropriate). The collision threshold r applied to the sample mobility boundaries defines an inflated sample mobility boundary. In the example of FIG. 3, regions 22A-22I are defined by a collision threshold r.

[0067] At step 8, for each of the obstacles, a combination of the Q inflated sample mobility boundaries defined at step 7 and associated with the obstacles is compared, via the one or more microprocessors or microcontrollers of the one or more local or remote computers designated for conducting such analysis, to the respective obstacle mobility boundary .sub.k. In the example of FIG. 3, the combination of regions 22A-22I would be compared to the obstacle mobility boundary .sub.k as described previously herein with respect to step 4. If the combination of the inflated sample mobility boundaries is determined not to cover the entirety of the respective obstacle mobility boundary .sub.k, then, via step 9, steps 5-8 are repeated with Q=Q+1. If the combination of the inflated sample mobility boundaries is determined to cover the entirety of the respective obstacle mobility boundary .sub.k, then Q becomes N at step 13 of modified DWA approach process 10. Preferably, the initial value of Q would be low enough such that steps 5-8 must be repeated in order that a minimum value of Q that covers the entirety of the respective obstacle mobility boundary .sub.k may be determined as such minimum value of Q would provide the most efficient number of velocity samples, i.e., velocity candidates for the agent to evaluate while applying modified DWA approach process 10 during travel.

[0068] Still referring to FIG. 2 but now referring again to modified DWA approach process 10, at step 14, the agent's velocity DW, i.e., V.sub.d.sup.agent, is calculated, via the one or more microprocessors or microcontrollers of the agent, using Criterion 1. Step 14 may be performed at any time prior to step 15, including before, simultaneously with, or after any one or any combination of steps 11-13. Then, at step 15, the agent's velocity samples, i.e., candidate velocities, are generated, via the one or more microprocessors or microcontrollers of the agent. Such velocity samples, as described previously herein, are subject to the control resolution of the linear and angular velocities within V.sub.d.sup.agent that are achievable by the agent. For example, if the agent has six linear velocity settings and five angular velocity settings, then a total of thirty settings are available as candidate velocities. At this step 15, the portion of those candidate velocities that are determined, via the one or more microprocessors or microcontrollers of the agent, to be within V.sub.d.sup.agent, are then evaluated relative to a navigation objective function such as Criterion 4. At step 16, the candidate velocities within V.sub.d.sup.agent are then evaluated using Criterion 3.1 or 3.2, as appropriate, as to their admissibility over the time period T. In such evaluation, a candidate velocity is considered admissible when Criterion 3 is determined, via the one or more microprocessors or microcontrollers of the agent, to be satisfied based on such candidate velocity for every one of the respective N samples of obstacle trajectories for each one of the obstacles. For example, if there are two obstacles in which N=10 for the first obstacle and N=11 for the second obstacle, then Criterion 3.1 or 3.2, as appropriate, must be satisfied by a candidate velocity for all ten sample trajectories for the first obstacle and all eleven sample trajectories for the second obstacle for the candidate velocity to be considered admissible.

[0069] Where there are no admissible candidate velocities, the agent is configured to reduce its velocity, as may be directed via the one or more microprocessors or microcontrollers of the agent, as much as possible during the next control cycle t.sub.i+1, as shown at step 17A. Otherwise, at step 17B, an optimal agent velocity is selected, via the one or more microprocessors or microcontrollers of the agent, for the velocity of the agent during the next control cycle t.sub.i+1. The optimal velocity, in this example, is the admissible velocity with the highest value determined by the objective function.

[0070] In some arrangements, steps 15 and 16 may be reversed such that the determination as to the admissibility of candidate velocities is made prior to the evaluation of the objective functions. In this instance, to reduce the number of evaluations to be made against the objective functions calculated for each of the obstacles, only admissible candidate velocities may be evaluated against the objective function for each of the obstacles.

[0071] At step 18, after completion of either step 17A or step 17B, the agent updates, via the one or more microprocessors or microcontrollers of the agent, its current position and velocity, and in some arrangements, its pose or other such orientational characteristics. Then, at step 19, if the agent determines, via the one or more microprocessors or microcontrollers of the agent, that the agent has not reached its destination goal, then steps 11-19 are repeated. If, at step 19, the agent determines, via the one or more microprocessors or microcontrollers of the agent, that the agent has reached its destination goal, then the modified DWA approach process ends. At this point, the agent preferably stops its movement.

[0072] Referring now to FIG. 4, modified DWA approach process 30 identifies admissible velocities of an agent, e.g., a robot, based on mobility boundaries for obstacles within an operating environment of the agent. In approach process 30, steps 31 and 31 are the same as steps 11 and 12 for modified DWA approach process 10. At step 33, mobility boundaries .sub.i,j for each obstacle j at future time step i of future time period T are determined, via the one or more microprocessors or microcontrollers of the agent, using Criterion 2.1 or 2.2 as appropriate. One example of such a mobility boundary is boundary 51 within operating environment 50 (represented as a plot) in FIG. 5. Mobility boundary 51 is a mobility boundary at time step k for an obstacle.

[0073] Then, at step 34, each of the mobility boundaries .sub.i,j determined at step 33 are inflated to inflated boundaries using the polygon offset function .sub.offset(.sub.i, r) of Criterion 5.1 or 5.2, as appropriate. In such arrangement, offset constant r provides a safety factor to avoid a collision with the agent and preferably may take into account a size, e.g., a radius r.sub.agent, of the agent and a size, e.g., a radius r.sub.obs, of the obstacle. In the example of FIG. 5 for a 2D system, inflated boundary 52 is illustrated as the inflated boundary for an obstacle with the offset constant r=1. The mobility boundary for 3D systems is a 3D mobility boundary.

[0074] At step 35 of approach process 30, the agent's velocity DW, i.e., V.sub.d.sup.agent, is calculated in the same manner as in step 14 of modified DWA approach process 10. Step 35 may be performed at any time prior to step 36, including before, simultaneously with, or after any one or any combination of steps 31-34.

[0075] Then, at step 36, the agent's velocity samples, i.e., candidate velocities, are generated, via the one or more microprocessors or microcontrollers of the agent as described previously herein with respect to step 15 of process 10. At this same step, the projected positions (and in some instances the projected poses or other orientations) of the agent by the end of the next time step i+1 is determined, via the one or more microprocessors or microcontrollers of the agent, for each of the agent's velocity samples that are within V.sub.d.sup.agent based on the position and velocity (and respectively the pose when appropriate) of the agent at time step i (see Criterion 2.1 or 2.2, as appropriate). Isolation of the agent's velocity samples that are within V.sub.d.sup.agent could be performed by the agent, via the one or more microprocessors or microcontrollers of the agent, at a later step of approach 30, but such an order of steps would be inefficient.

[0076] Then, at step 37, the projected positions for each of the candidate velocities of the agent within V.sub.d.sup.agent are compared, via the one or more microprocessors or microcontrollers of the agent, to each of the inflated boundaries determined at step 34 and that are based on the mobility boundaries .sub.i, j determined at step 33 and described above. Each of the projected positions that do not overlap any of the determined inflated boundaries are admissible velocities, which the agent may be configured to identify as such via the one or more microprocessors or microcontrollers of the agent. Where there are no admissible candidate velocities within V.sub.d.sup.agent the agent is configured to reduce its velocity, as may be directed via the one or more microprocessors or microcontrollers of the agent, as much as possible during the next control cycle t.sub.i+1, as shown at step 38A. Otherwise, at step 38B, an optimal agent velocity is selected, via the one or more microprocessors or microcontrollers of the agent, for the velocity of the agent during the next control cycle t.sub.i+1. The optimal velocity may be the admissible velocity within V.sub.d.sup.agent with the highest value determined by an objective function such as Criterion 4.

[0077] At step 39, after completion of either step 38A or step 38B, the agent updates, via the one or more microprocessors or microcontrollers of the agent, its current position and velocity, and in some arrangements, its pose or other such orientational characteristics. Then, at step 40, if the agent determines, via the one or more microprocessors or microcontrollers of the agent, that the agent has not reached its destination goal, then steps 31-40 are repeated. If, at step 40, the agent determines, via the one or more microprocessors or microcontrollers of the agent, that the agent has reached its destination goal, then the modified DWA approach process ends. At this point, the agent preferably stops its movement.

[0078] 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 been described with reference to particular embodiments, it is to be understood that these embodiments 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 as defined by the appended claims.