Method and system for controlling a vehicle

Abstract

Method and system for controlling a vehicle that includes using velocity vectors of obstacles in the vehicle's environment to determining boundaries of the one or more obstacles and thereby generate a velocity space that may include velocity vectors for the vehicle and which are represented as collision cones. The using an identified velocity vector in combination with the velocity vectors of the one or more obstacles to produce a maximum motion in of the vehicle towards a target location while avoiding all determined collision cones.

Claims

1. A method of controlling a vehicle, the method comprising: (a) receiving a user input indicating a vehicle target location; (b) receiving sensor information from at least one sensor included in the vehicle for one or more obstacles within a predefined radius r around the vehicle; (c) determining, using processing circuitry included in the vehicle, velocity vectors of the one or more obstacles; (d) determining boundaries of the one or more obstacles, (e) generating a possible velocity space, the possible velocity space including possible velocity vectors for the vehicle; (f) determining a collision cone for each obstacle from the one or more obstacles based on at least the boundaries of the one or more obstacles; (g) identifying a velocity vector from the possible velocity space based on at least velocity vectors of the one or more obstacles in accordance with a greedy algorithm that is subject to a constraint that a difference between the direction of current velocity vector and a direction of the identified velocity vector is below a predetermined velocity direction threshold, the velocity vector being identified as to produce a maximum motion in the direction of the vehicle target location from a current location of the vehicle and being outside all determined collision cones for each obstacle within the radius r, wherein the greedy algorithm determines a solution to a sparse structured constraint matrix; (h) updating the current location of the vehicle to an updated location; (i) controlling the vehicle to move in the space based on the identified velocity vector for a predetermined period; and (j) repeating steps (b) through (i) until the updated location of the vehicle corresponds with the vehicle target location.

2. The method of claim 1, wherein each obstacle has a velocity direction indicating a movement towards the vehicle.

3. The method of claim 1, further comprising: wherein when the greedy algorithm does not find a solution to the sparse structured constraint matrix due to the presence of obstacles surrounding the vehicle, removing an obstacle from the one or more obstacles and repeating greedy algorithm in order to identify a velocity vector, the removed obstacle being the farthest from the vehicle.

4. The method of claim 1, wherein the obstacles includes one or more vehicles.

5. The method of claim 1, wherein the vehicle is an autonomous unmanned vehicle.

6. The method of claim 1, wherein determining a boundary of an obstacle includes determining an edge vector including a right edge vector and a left edge vector.

7. The method of claim 6, further comprising: sorting the possible velocity vectors in a first order; sorting edge vectors in the first order; selecting a first edge vector from the sorted edge vectors; and selecting a second edge vector when the first edge vector is associated with a difference above the predetermined threshold, the second edge vector being a next vector in the sorted edge vectors.

8. The method of claim 1, wherein the velocity vector is based on kinodynamic constraints, the kinodynamic constraints including the vehicle velocity vector and a vehicle acceleration.

9. The method of claim 8, wherein the kinodynamic constraints includes bounding the vehicle velocity vector and the vehicle acceleration by predefined upper and lower bounds.

10. The method of claim 1, wherein the collision cone includes determining a collision space.

11. The method of claim 10, wherein determining the collision space includes: determining a size of the vehicle; determining a first relative position of the vehicle to a first edge of each obstacle; determining a second relative position of the vehicle to a second edge of each obstacle; and forming the collision space based on first relative positions of the one or more obstacles, second relative positions of the one or more obstacles, and the size of the vehicle.

12. The method of claim 11, wherein the size of the vehicle is estimated as a maximum width.

13. A vehicle, comprising: at least one sensor configured to detect obstacles within a predefined range; processing circuitry configured to: (a) receive a user input indicating a vehicle target location; (b) receive sensor information from the at least one sensor for the detected obstacles; (c) determine velocity vectors of one or more obstacles present in a space; (d) determine boundaries of the one or more obstacles; (e) generate a possible velocity space, the possible velocity space including possible velocity vectors for the vehicle; (f) determine a collision cone for each obstacle from the one or more obstacles based on at least the boundaries of the one or more obstacles; (g) identify a velocity vector from the possible velocity space based on at least velocity vectors of the one or more obstacles in accordance with a greedy algorithm that is subject to a constraint that a difference between the direction of current velocity vector and a direction of the identified velocity vector is below a predetermined velocity direction threshold, the velocity vector being identified as to produce a maximum motion in the direction of the vehicle target location from a current location of the vehicle and being outside all determined collision cones for each detected obstacle, wherein the greedy algorithm determines a solution to a sparse structured constraint matrix; (h) update the current location of the vehicle to an updated location; (i) control the vehicle to move in the space based on the identified velocity vector for a predetermined period; and (j) repeat steps (b) through (i) until the updated location of the vehicle corresponds with the vehicle target location.

14. The vehicle of claim 13, wherein determining a boundary of an obstacle includes determining an edge vector including a right edge vector and a left edge vector.

15. The vehicle of claim 14, wherein the processing circuitry is further configured to: sort the possible velocity vectors in a first order; sort edge vectors in the first order; select a first edge vector from the sorted edge vectors; and select a second edge vector when the first edge vector is associated with a difference above the predetermined threshold, the second edge vector being a next vector in the sorted edge vectors.

16. A non-transitory computer readable medium storing computer-readable instructions therein which when executed by a computer cause the computer to perform a method for controlling a vehicle, the method comprising: (a) receiving a user input indicating a vehicle target location; (b) receiving sensor information from at least one sensor included in the vehicle for one or more obstacles within a predefined radius r around the vehicle; (c) determining velocity vectors of the one or more obstacles; (d) determining boundaries of the one or more obstacles; (e) generating a possible velocity space, the possible velocity space including possible velocity vectors for the vehicle; (f) determining a collision cone for each obstacle from the one or more obstacles based on at least the boundaries of the one or more obstacles; (g) identifying a velocity vector from the possible velocity space based on at least velocity vectors the one or more obstacles in accordance with a greedy algorithm that is subject to a constraint that a difference between the direction of current velocity vector and a direction of the identified velocity vector is below a predetermined velocity direction threshold, the velocity vector being identified as to produce a maximum motion in the direction of the vehicle target location from a current location of the vehicle and being outside all determined collision cones for each obstacle within the radius r, wherein the greedy algorithm determines a solution to a sparse structured constraint matrix; (h) update the current location of the vehicle to an updated location; (i) controlling the vehicle to move in the space based on the identified velocity vector for a predetermined period; and (j) repeating steps (b) through (i) until the updated location of the vehicle corresponds with the vehicle target location.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) A more complete appreciation of the disclosure and many of the attendant advantages thereof will be readily obtained as the same becomes better understood by reference to the following detailed description when considered in connection with the accompanying drawings, wherein:

(2) FIG. 1 is an environment model for single agent multiple obstacles, where w.sub.j is the current velocity of j.sup.th obstacle;

(3) FIG. 2 illustrates collision spaces represented by the region enclosed by dotted circles around each obstacle;

(4) FIG. 3 illustrates construction of the collision cones using simple geometry;

(5) FIG. 4 illustrates the set of hyperspaces creating feasible space formed by the two collision cone vectors of Obstacle-1 and vector c;

(6) FIG. 5 illustrates obstacles moving with velocity, w.sub.j, and CCs formed by their respective vectors;

(7) FIG. 6 illustrates stalling in a single agent-multi static obstacle case;

(8) FIG. 7 is an agent found the trajectory to navigate outside the spiral maze;

(9) FIG. 8 is an agent navigating to the target located inside the complicated maze structure;

(10) FIG. 9 is an agent navigating to the target located inside the complicated maze structure;

(11) FIG. 10 shows four agents initially located at corners of a square of sides 100 units each in a dense environment with 80 randomly placed and randomly moving obstacles; and

(12) FIG. 11 shows 50 agents initially located symmetrically on the periphery of a circle with their targets located at the antipodal position of their initial location.

(13) FIG. 12 is a flowchart of a method for controlling a vehicle according to an exemplary embodiment of the present disclosure; and

(14) FIG. 13 is a hardware description of a device and/system that is capable of carrying out the method through for example circuitry, according to an exemplary embodiment of the present disclosure.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(15) Embodiments of the present disclosure will now be described more fully hereinafter with reference to the accompanying drawings, in which some, but not all embodiments of the disclosure are shown. The present disclosure will be better understood with reference to the definitions, examples and descriptions provided herein.

(16) A typical cluttered environment in which the method and system of the invention are used, includes N number of agents present and k number of obstacles. The agents are assumed to be spherical in shape with radius r.sub.i of the i.sup.th agent and are capable of holonomic motion. However, other shapes including parallelepiped, irregular, trapezoidal, cubic, cylindrical may be used with dimensional corresponding to the longest dimension. The agents preferably have the information of their respective target locations for example by receiving the location from another source such as a server but most preferably by submission from a passenger or autonomously by a system such as GPS. The obstacles present in the environment may be static or dynamic.

(17) Representation of the obstacle and/or the vehicle may include uncertainty related to one or more aspects of its identity, dimensional characteristics or motion. The uncertainty is preferably represented by a separate parameter that describes the uncertainty in term relative to one or more of the characteristics or aspects of motion of the obstacle. For example, the uncertainty may be expressed mathematically as a percentage of, for example, the motion characteristics of the obstacle. The uncertainty may be represented as a fixed number (absolute), for example ±5 units of speed (e.g., km/hr) or as a percentage of the value (e.g., ±5%). The uncertainty conveys that a particular value associated with an obstacle may have a range of expected values. This is reflective of a practical application in which an obstacle undergoes changing velocity (e.g., for example when an automobile brakes or accelerates) or changes in size (e.g., when the angle of approach to an obstacle changes such that the visible cross-section of the obstacle changes with the angle of approach). In another aspect uncertainty can represent inherent measurement error or uncertainty in the sensors that used to detect obstacles and/or calculate/determine their characteristics. In this sense an initial value may be represented by a degree of confidence or tolerance that is representative of or includes measurement uncertainty/error. Further, a cumulative uncertainty may be associated with errors that arise when a plurality of sensors are used together. The uncertainty can be cumulative or may propagate non-linearly.

(18) In a particular embodiment the method of the invention includes taking into account or separately calculating the uncertainty related to boundaries of one or more obstacles. This is especially the case when one or more edge vectors of a boundary of an obstacle are determined. The edge vectors may be associated with uncertainty related to dimensional characteristics of the obstacle or, characteristics of the angle of approach or angle of observation from which the obstacle is observed or calculated.

(19) The boundary characteristics of the obstacle can be expressed dimensionally as a radius and/or diameter if the obstacle is represented by, for example, a spherical shape. However, if the obstacle is representative of a vehicle with dimensional characteristics that are not spherical, it may be represented by one or more conditions such as a width, height or length. In order to account for the greatest dimensional cross-section of the obstacle, preferably the vehicle is represented by a maximum dimension such as a maximum width.

(20) The invention includes representation and control of vehicle in a 3-dimensional space. While often the control of a vehicle is sufficiently represented in two dimensions, there is no restriction that an additional dimension of control or boundary condition calculation can be included. This may be of particular importance when representing obstacles that are present in or moving in a dimension outside of the two-dimensional that is representative of the plane (road) in which the vehicle is being controlled. Examples of obstacles that are preferentially represented in three dimensions include power lines or low-flying aircraft. Likewise, obstacles such as bridges and overpasses are preferably represented in three dimensions in order to take into account clearance dimensions. Preferably the vehicle under control is represented on a map-type substrate that can be adequately represented in two dimensions. Obstacles, however, are preferably represented in three dimensions to take into account protrusions outside of the plane representing the substrate on which the vehicle is represented. Preferably the three-dimensional characteristics of the obstacle relate to the obstacle's cross-sectional area. Representation of the obstacle in three dimensions (especially when the obstacle is a vehicle) such that the depth of the obstacle is calculated is of lesser value at least in so far as determining collision and/or avoidance measures.

(21) The agents may be equipped with some range sensors like sonars, cameras, infrared detectors, lasers or other devices which may have limited sensory capabilities enabling them to only recognize obstacles that are in some vicinity of radius s.sub.i around the i.sup.th agent. Additionally, each agent may only recognize the surface edges of obstacles and other agents exposed to it in its vicinity or may recognize a cross section perspective of the obstacle that may change as a function of the agents angle to the obstacle. The agents can also obtain velocity information v.sub.i and w.sub.j of every i.sup.th agent and j.sup.th obstacle respectively that is present in its sensor's range or this information may be provided by an external source such as a drone that is in wireless communication with the agent. The agents preferably are able to communicate with each other.

(22) The agents plan their motion onboard while moving through the dynamic environment. The agents must be capable of recognizing and avoiding obstacles even without static or dynamic global information of the environment. The onboard capability preferably includes an algorithm or model, or circuitry programmed with instructions including said algorithm or model, capable of finding, calculating or identifying one or more collision free paths for each agent. The algorithm should be fast and simple enough to suit the limited onboard computational capability of the agents.

(23) In an embodiment the method and system may rely on circuitry having instructions that include a mathematical formulation presented for a static (e.g., map) or dynamic (e.g., agent or obstacle location tracking) 2-D environment where the obstacles are assumed to be circular in shape but may be of any shape the same or different from the agent. Further, a search heuristic is preferably included to determine the agent's velocity and/or that generates collision free trajectory. The model can also be used for multi agent environments, where each agent needs to find collision free trajectories without any communication among them.

(24) In a scenario where an agent is present in a multi obstacle dynamic environment, the agent is preferably capable of gathering or receiving boundary/edge information of the obstacles present in some vicinity of its sensors. FIG. 1) represents a scenario where the agent has obtained the velocities and surface edges of the obstacles present in its vicinity. Each j.sup.th obstacle present in the vicinity the agent forms two vectors, R.sub.j and L.sub.j, showing relative position of the right and left edges of its surface from the center of the agent respectively, where the agent's center is considered as the origin of the vector space. In order to avoid collision with an obstacle, the center of the agent preferably remains at some distance away from the obstacles. The set of all minimum distances, thus, form Collision Spaces (CS) which depend on the dimensions of the agent and each obstacle. The collision spaces with respect to each obstacle can be conveniently formed by taking the following Minkowski sum (See FIG. 2)):
A⊕P.sub.j={a+p.sub.j|a∈A,p.sub.j∈P.sub.j} (2)
where A and P.sub.j are the vector spaces representing the shape of the agent and the j.sup.th obstacle respectively. It is important to point out here that the agent may not be able to find the minkowski sum due to the sensory limitations, however, the model of the problem presented here does not require the agent to calculate all the CSs for collision avoidance. Each pair of vectors, R.sub.j and L.sub.j for each obstacle as illustrated in FIG. 2, can be used in combination with the sum as obtained in Equation (2) to form the collision cone (CC). Preferably the agent will not collide with any of the obstacles in a future time if and preferably only if, its current relative velocity vector is outside all the collision cones (CCs), formed by the edge vectors of all the surrounding obstacle. A smaller rectangular region from FIG. 2) has been chosen to show the formation of the CC w.r.t Obstacle-1 (see FIG. 3)). Vectors R.sub.1 and L.sub.1 can be used together with the agent's radius, r, to find vectors a.sub.R.sub.1 and a.sub.L.sub.1 respectively in the right angled triangles shown in FIG. 3). The CC thus formed by these two vectors is convex. Similar CCs can be constructed with respect to all the obstacles detected by the agent. The agent's relative velocity, c.sub.j, w.r.t to each of the j.sup.th obstacle should exist in a non-convex space outside of all the CCs in order to avoid collision with any of the obstacles, which is shown mathematically as follows:
c.sub.j=c−w.sub.j∪CC (3)
where,
CC={x:x=λ.sub.R.sub.ja.sub.R.sub.j+λ.sub.L.sub.ja.sub.L.sub.j|λ.sub.R.sub.j,λ.sub.L.sub.j∈[0,∞]}∀j. (4)
Using Equations (3) and (4), following set of constraints can be written for the agent's relative velocity w.r.t Obstacle-1:
A.sub.1λ.sub.1≠c.sub.1 (5)
where A.sub.1=[a.sub.R.sub.1a.sub.L.sub.1],

(25) $λ_{1} = [\begin{matrix} λ_{R 1} \\ λ_{L 1} \end{matrix}]$
such that λ.sub.1∈R.sup.m and the column vectors a.sub.R.sub.1, a.sub.L.sub.1∈R.sup.n. The above system holds true if and preferably only if the following system does not have a solution:
A.sub.1λ.sub.1=c.sub.1,λ.sub.1≥0 (6)

(26) According to the Farkas's lemma, if the system in Equation (6) does not have a solution, then the following system must have a solution (and vice versa):
A.sub.1.sup.TX.sub.1≤0
c.sub.1.sup.TX.sub.1>0 (7)
The feasible space created by the above system of inequalities for Obstacle-1 is shown in FIG. 4). Also, it can be seen that the new relative velocity vector of the agent, c1, will generate a collision free trajectory with respect to Obstacle-1 if and only if, the above set of constraints in Equation (7) has a solution. The vector c1 in FIG. 4) is outside the collision cone and therefore, the hyperspaces formed by the vectors have a solution, i.e., they form a feasible space.

(27) Equation (7) forms a block of set of constraints required to check the feasibility of agent's new velocity with respect to one obstacle. Similar CCs can also be formed for the other obstacles as shown in FIG. 5). Each CC can then be used to form the set of half spaces for each j.sup.th obstacle and then formulated in a blocked structured LP as presented in Formulation (8). The LP will be feasible if and only if all cj are outside their respective collision cones.

(28) $\begin{matrix} \min c_{i}^{T} X_{1} + .Math. c_{j}^{T} X_{j} + .Math. c_{k}^{T} X_{k} s . t . : \begin{matrix} a_{R_{1}}^{T} X_{1} & \leq 0 \\ a_{L_{1}}^{T} X_{1} & \leq 0 \\ c_{1}^{T} X_{1} & \geq .Math. \\ ⋱ & .Math. \\ a_{R_{j}}^{T} X_{j} & \leq 0 \\ a_{L_{j}}^{T} X_{j} & \leq 0 \\ c_{j}^{T} X_{j} & \geq .Math. \\ ⋱ & .Math. \\ a_{R_{k}}^{T} X_{k} & \leq 0 \\ a_{L_{k}}^{T} X_{k} & \leq 0 \\ c_{k}^{T} X_{k} & \geq .Math. \end{matrix} & (8) \end{matrix}$
where, c.sub.j=c−w.sub.j, ∀.sub.j, and ε is a small positive scalar. The above LP is used to check the feasibility of the new velocity vector c in a non-convex space in order to obtain a collision free trajectory. The search for c is done through a heuristic method presented in IV-C.

(29) Although the size of the constraint matrix formed in the above LP increases with the increase in number of obstacles, the sparse structure of the constraint matrix significantly reduces the solution time. Improving the above LP is not the prime intention rather it is preferably just to check its feasibility, which is usually done in the pre-processing step by the solver and preferably requires no simplex iterations. So, the objective function in Equation (8) is chosen only for convenience.

(30) The agent's new velocity vector c is preferably chosen such that it satisfies the agent's kinodynamic constraints. The agent's current acceleration, g.sub.a, and its current velocity, v.sub.a, determines its future velocity, c, in the next iteration after τ time step. Both g.sub.a and v.sub.a are bounded and the kinodynamic constraints on acceleration and velocity can be written as follows:
v.sub.min≤∥c∥.sub.2≤v.sub.max (9)
a.sub.min≤∥g.sub.a∥.sub.2≤a.sub.max (10)
c=τg.sub.a+v.sub.a (11)

(31) a.sub.min and v.sub.min are the lower bounds, while a.sub.max and v.sub.max are the upper bounds on agent's current acceleration and future velocity after τ time step, respectively.

(32) Method/model (1) utilizes a greedy approach to find the feasible velocity vector c and may create stalling in some scenarios. A randomized greedy algorithm is presented wherein a smart approach is used to avoid stalling while keeping intact the speed of the solution.

(33) 1) Greedy Algorithm: The working of Formulation (8) can be seen in method/model (1). All the edge vectors, a.sub.R.sub.j and a.sub.L.sub.j are obtained for each j.sup.th obstacle in the agent's vicinity. According to Equations (9), (10) and (11), a uniform random sample of the Possible Velocity Space (PVS) is generated. These possible velocity vectors are then stored according to the descending order of their dot products with the relative target vector of the agent, Z.sub.a. Algorithm (1) selects the velocity that produces maximum motion in the direction of the target while avoiding collision with the obstacles in its vicinity in any future time. As shown in Algorithm (1), the sorted velocity vectors from the sampled PVS are tested for feasibility and the first feasible is chosen as c.

(34) TABLE-US-00001 Algorithm 1: Greedy Algorithm Input: r.sub.a ← Agent's radius; p.sub.a ← Agent's current location; T.sub.a ← Agent's target location; S.sub.a ← Agent's velocity sample size; 1 while ||T.sub.a − p.sub.a||.sub.2 ≥ ξ do | /* ξ is a small positive scalar */ 2 | Z.sub.a ← T.sub.a − p.sub.a; | Data: Obtain the edge vectors a.sub.R.sub.j, a.sub.L.sub.j and the | velocity vector w.sub.j for each j.sup.th obstacle | present in the agent's vicinity such that, | ||q.sub.j − p.sub.a||.sub.2 < ||Z.sub.a||.sub.2 ∀.sub.j | /* Generate uniform random sample of | possible velocities according to | Eq. (9), (10) and (11) */ 3 | PVS ←rand(S.sub.a,2); 4 | PVS ← sort(PVS,dot(PVS[i],Z.sub.a)); 5 | for i=1 to rows of PVS do 6 | | c ← PVS[i]; 7 | | Formulate LP as in Eq. (8); 8 | | Solve LP; 9 | | if LP is feasible then 10 | | | Solution for c is found; 11 | | | break out of for loop; 12 | | else 13 | └ └ c ← 0 14 | v.sub.a ← c; 15 | φ ← runtime of current while loop; 16 | Wait(τ − φ); /* where τ > φ */ | /* Update agent's position */ 17 └ p.sub.a ← τv.sub.a + p.sub.a;

(35) 2) Stalling: The phenomenon of stalling can be explained with the example as shown in FIG. 6). In order to move as close as possible to the target from position O, the only two best possibilities are to move in either of the direction of the edge vectors, a.sub.R and a.sub.L. Let's suppose that using Algorithm (1), the agent finds a.sub.R as its most feasible direction of motion. Hence, O and O′ show the agent's initial location and the location it moved in one iteration of T time-step respectively. The target in this case is closer to the agent w.r.t the center of an imaginary circle formed by the two tangents which are represented by the two extreme edge vectors, a.sub.R and a.sub.L, of the obstacles. In such a scenario, following conditions always hold:
a′.sub.R.sup.TZ′.sub.a<a.sub.R.sup.TZ.sub.a (12)
a′.sub.L.sup.TZ′.sub.a>a.sub.L.sup.TZ.sub.a (13)
where Z.sub.a and Z′.sub.a are vectors from the agents locations O and O′ to the target respectively. As per the above results, the model/method has identified, calculated or determined a better direction to move and the greedy approach as explained earlier will results in the agent to change its direction abruptly in the very next iteration. The criterion for the selection of c in model/method (1) will stall the agent's motion. Stalling may be avoided by enforcing the agent not to change the direction of its velocity very rapidly as compared to its previous velocity. Constraining the agent's velocity in this manner may also help to reduce mechanical jerks on the system and produce smooth trajectories.

(36) Thus, the following additional constraint is put on agent's velocity to avoid stalling and rapid changes in the direction of its velocity:
c.sup.Tv.sub.a≥0 (14)

(37) 3) Model/method: For static environments, it can be easily seen that if there exists a collision free path, the c vector in the direction of at least one of the edge vectors of the obstacles must be feasible. Therefore, all the possibilities of the edge vector directions are first tested for feasibility in the PVS. For this, the agent's acceleration constraints as given by Equation (10) are assumed to be such that a nominal velocity with the magnitude of at least, μ.sub.a, is possible to achieve in any direction from the current velocity, v.sub.a.

(38) The edge vectors are sorted, similar to other possible velocity vectors from PVS, in descending order of their dot product with the target vector. If there comes a situation when the new velocity c in the direction of some edge vector is feasible but does not satisfy the constraint in Equation (14), it is neglected and the search continues. The next vector from the sorted edge vectors stack is chosen for feasibility test as given in Algorithm (2).

(39) The model/method not only finds collision free trajectories in an efficient manner but also avoids stalling situations. However, it is preferable not to use the edge vectors created by the obstacles that are moving away from the agent, in a direction opposite to the direction of the target and will never be come into the path of the agent with their current velocities. Similarly, for cases where no solution is found due to the presence of obstacles all around the agent, the edge vectors of the farthest obstacle may be neglected and the algorithm be repeated until a solution is found.

(40) Same strategy can be applied for the multi-agent scenario where each agent considers all the agents around it as obstacles, since the agents are assumed to have no communication protocol between them. Most of the recent trajectory planning methods in dynamic environments are computationally very expensive and may require global information to effectively plan the trajectories. The model/method of the present disclosure is designed to effectively deal with dynamic situations where global information may not be available. Such methods are practical in scenarios where acquiring global information is difficult and expensive, and may require huge onboard computational resources. For cases where global information is available, it may be incorporated in the proposed model/method by simply updating the target value for each agent that may be obtained from the global path planning.

Examples

(41) Having generally described this disclosure, a further understanding can be obtained by reference to certain specific examples in which the method/mode of the invention is tested is different environments simulated to represent multi-agent and multi-obstacle environments encountered in ordinary daily life. The examples below are provided herein for purposes of illustration only and are not intended to limit the scope of the claims.

(42) The model/method was tested for numerous single and multi agent situations with obstacles. Results of four scenarios are presented to show the efficiency of the model/method:

(43) 1) Static complicated scenarios requiring exploration.

(44) 2) U-shaped scenarios with Non-Linear Velocity Obstacles (NLVO).

(45) 3) Multi agent dense scenarios with all randomly located and randomly moving obstacles.

(46) 4) Multi agent scenario with evenly placed agents on the periphery of a circle and the agents have to navigate to their antipodal positions on the circle.

(47) Situations in which the agent is surrounded by the obstacles generate infeasible space and the model/method may not find any solution. However, there may exist a solution if the obstacles are not evenly located around the agent. In such cases, the CC of the farthest obstacle is neglected to enable the agent to explore the environment. A spiral maze scenario is shown in FIG. 7). The model/method may not find a solution initially in a particular time instant but as the constraints on CC of the farthest obstacles are relaxed, the agent finds its way out of the spiral maze to its target.

(48) A more complicated maze scenario is shown in FIG. 8) where the agent's sensing vicinity is set to a small number similar to the width of the passage ways of the maze. Reducing the agents sensing radius results in tracking of the wall which enables the agent to navigate to its target. However, such scenarios typically require global path planning methods and since the model/method is preferably a local planning method, it may not be able to find a feasible path in all similar scenarios.

(49) Local planning methods are typically greedy in nature and may face phenomenon of stalling. However, the inclusion of an additional constraint given by Equation (14) in the model/method permits it to handle situations, similar to the one shown in FIG. 9) where some static obstacles are positioned in a U-Shaped structure. Additionally, there are 2 moving obstacles having non-linear velocity profiles. Initially, Obstacle-1 is outside while Obstacle-2 and the agent are inside the U-shaped structure. The y-component of the velocity of Obstacle-1 is constant and is in negative y direction while its x-component accelerates constantly in the positive x direction. Obstacle-2 has a sinusoidal velocity profile which moves it out of the U-shaped structure. The instances of the moving obstacles and the agent are printed with the index number of each iteration to show their motion w.r.t time.

(50) The agent first tries to avoid collision with Obstacle-1 by changing its direction of motion in the positive x direction. However, as the agent proceeds with its motion after iteration number 5, it slightly changes its direction of motion and reduces the speed to avoid a potential future collision with both Obstacle-1 and Obstacle-2 (see iteration 9-13 in FIG. (9)) and reaches its target.

(51) The model/method also performs well in dense and dynamic scenarios. One such scenario is presented in FIG. 10), where 4 agents are present among 80 randomly positioned, randomly sized (radius=3.5-4.5 units) and randomly moving obstacles inside the area formed by a square of sides 100 units each. The agents are positioned at the corner of the square while each of their targets are located at diagonally opposite corners. The agents locally find collision free trajectories and reach to their targets as shown in FIG. (10).

(52) Another dense scenario is shown in FIG. 11) where 50 agents are symmetrically placed at the periphery of a circle and they have to navigate to their antipodal positions on the circle. The agents are first seen to converge at around the center of the circle and then effectively avoid collision to navigate to their respective targets.

(53) FIG. 12 describes a flowchart representing an embodiment of the present disclosure that includes controlling a vehicle. In process circuitry that may be located in the vehicle, or alternately another vehicle or in a location wirelessly connected to but physically separate from the vehicle being controlled, a vehicle target location, vehicle predicted location and/or a vehicle present location is first obtained or received 1201. The target location and/or present location of the vehicle is used to generate one or more velocity spaces representing a path and/or a velocity space of the vehicle 1202. Concurrently, before or after the velocity space is generated, a velocity vector of one or more obstacles may be obtained, determined or calculated 1203. The velocity vector of the obstacle, stationary or moving, is used to determine one or more boundary conditions of the obstacle 1204. The boundary condition determination may occur concurrently, before or after the velocity space is obtained for the vehicle. Subsequently 1205 a collision cone is determined which, in some embodiments, represents a risk factor of collision or overlap between the trajectory of the vehicle and the trajectory or location of the obstacle. The collision cone may be determined utilizing one or more of the equations and methods described the present disclosure 1205. A vehicle velocity vector is subsequently calculated, determined or identified 1206. The vehicle velocity vector may be determined by further reference to the vehicle target location in present time, in future time or as a prediction based upon the vehicle velocity and trajectory. Subsequently one or more control instructions is dispatched to the vehicle 1208. The control instructions are dispatched regularly to accommodate changes in the space environment of the vehicle and/or to accommodate changes in the velocity trajectory of the obstacles.

(54) Motion planning has diverse applications in real life problems. Improvements in drone technology have created a number of possibilities for inclusion in the model/method of the invention. For instance, one key area is logistics which is usually the most critical and expensive component of any supply chain. With the expansion of internet, a lot of retail business has shifted online. Enterprises like Amazon and DHL plan to invest huge resources in the research of AUVs capable of planning their route to numerous target locations and thus managing the delivery of goods to their customers. Likewise, autonomous drones may also be used for surveillance purposes where they have to operate in open environment with limited resources and sensing capabilities. Flying machines can also be used in emergency situations like fire-fighting, rescue operations, bomb disposal, first-aid delivery, fog and smog prevention etc.

(55) Similarly, the progression in nanotechnology, nano-fabrication and microelectronics has significantly reduced the size of onboard embedded computing systems, permitting the development of miniature robots and mechanisms capable of making decisions and performing complicated tasks in the areas that cannot be accessed otherwise. One such application is in the field of medicine where such miniature agents can plan their motion while moving with the blood stream or in the intestinal canal of the human body to reach the assigned target location and perform a certain task like operating a tumor.

(56) In all of the above discussed areas, the onboard motion planning specially performed in dynamic environment is a complicated task and conventional model/methods are either computationally very expensive or compromise too much on the optimality.

(57) Of course, most of the above applications are available with only local knowledge of the environment and, therefore, global optimal in these situations may not always occur. The model/method of the invention, thus, can be utilized to navigate towards target in such difficult situations.

(58) Next, with reference to FIG. 13, a hardware description of a device implementing a method/model of the invention, according to exemplary embodiments, is described. In FIG. 13, the device includes a CPU 1300 which performs the processes described above/below. The process data and instructions may be stored in memory 1302. These processes and instructions may also be stored on a storage medium disk 1304 such as a hard drive (HDD) or portable storage medium or may be stored remotely. Further, the claimed advancements are not limited by the form of the computer-readable media on which the instructions of the inventive process are stored. For example, the instructions may be stored on CDs, DVDs, in FLASH memory, RAM, ROM, PROM, EPROM, EEPROM, hard disk or any other information processing device with which the device communicates, such as a server or computer.

(59) In embodiments of the invention the CPU 1300 is preferably present in the vehicle or agent which is being controlled. For example, CPU 1300 may be installed directly in the control system of an automobile. Likewise, the memory 1302 and/or the storage medium disk 1304 may be, and preferably are, directly installed on the vehicle being controlled. In other embodiments of the invention the CPU 1300, the memory 1302 and flash or the storage medium disk 1304 may each independently be located or stored remotely from the vehicle which is being controlled by the method/model and/or system of the invention. For example, any of these components may be connected wirelessly to the vehicle in one or more locations remote from the vehicle.

(60) Further, the claimed advancements may be provided together at least partially as a utility application, background daemon, or component of an operating system, or combination thereof, executing in conjunction with CPU 1300 and an operating system such as Microsoft Windows 7, UNIX, Solaris, LINUX, Apple MAC-OS and other systems known to those skilled in the art.

(61) The hardware elements in order to achieve the device may be realized by various circuitry elements, known to those skilled in the art. For example, CPU 1300 may be a Xenon or Core processor from Intel of America or an Opteron processor from AMD of America, or may be other processor types that would be recognized by one of ordinary skill in the art. Alternatively, the CPU 1300 may be implemented on an FPGA, ASIC, PLD or using discrete logic circuits, as one of ordinary skill in the art would recognize. Further, CPU 1300 may be implemented as multiple processors cooperatively working in parallel to perform the instructions of the inventive processes described above.

(62) As noted herein certain components of the device may be wirelessly communicating with the vehicle. Such wireless communication may take place over a device such as shown in FIG. 13. The device in FIG. 13 also includes a network controller 1306, such as an Intel Ethernet PRO network interface card from Intel Corporation of America, for interfacing with network 1328. As can be appreciated, the network 1328 can be a public network, such as the Internet, or a private network such as an LAN or WAN network, or any combination thereof and can also include PSTN or ISDN sub-networks. The network 1328 can also be wired, such as an Ethernet network, or can be wireless such as a cellular network including EDGE, 3G and 4G wireless cellular systems. The wireless network can also be WiFi, Bluetooth, or any other wireless form of communication that is known.

(63) The device further includes a display controller 1308, such as a NVIDIA GeForce GTX or Quadro graphics adaptor from NVIDIA Corporation of America for interfacing with display 1310, such as a Hewlett Packard HPL2445w LCD monitor. A general purpose I/O interface 1312 interfaces with a keyboard and/or mouse 1314 as well as a touch screen panel 1316 on or separate from display 1310. General purpose I/O interface also connects to a variety of peripherals 1318 including printers and scanners, such as an OfficeJet or DeskJet from Hewlett Packard.

(64) A sound controller 1320 is also provided in the device, such as Sound Blaster X-Fi Titanium from Creative, to interface with speakers/microphone 1322 thereby providing sounds and/or music. The general purpose storage controller 1324 connects the storage medium disk 1304 with communication bus 1326, which may be an ISA, EISA, VESA, PCI, or similar, for interconnecting all of the components of the device. A description of the general features and functionality of the display 1310, keyboard and/or mouse 1314, as well as the display controller 1308, storage controller 1324, network controller 1306, sound controller 1320, and general purpose I/O interface 1312 is omitted herein for brevity as these features are known.

Method and system for controlling a vehicle

Assignee

Inventors

Cpc classification

Classification Explorer

G01S13/589

PHYSICS

Classification Explorer

B60W30/0956

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

G01S13/56

PHYSICS

Classification Explorer

G01S13/726

PHYSICS

Classification Explorer

G06T2207/30252

PHYSICS

Classification Explorer

G05B2219/40519

PHYSICS

Classification Explorer

B25J9/1676

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

G05D1/0214

PHYSICS

Classification Explorer

G05D2201/0213

PHYSICS

Classification Explorer

B60W60/0011

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B60W50/0097

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

G05D1/0289

PHYSICS

International classification

Classification Explorer

G05D1/02

PHYSICS

Classification Explorer

B60W30/095

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B60W50/00

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

G01S13/56

PHYSICS

Classification Explorer

B25J9/16

PERFORMING OPERATIONS; TRANSPORTING

Abstract

Claims

Description