Patent ID: 12246753

Corresponding reference characters indicate corresponding elements among the view of the drawings. The headings used in the figures do not limit the scope of the claims.

DETAILED DESCRIPTION

Autonomous Vehicles (AVs) have the potential to make transportation safer by reducing the number of accidents that are caused due to human error. When AVs become connected (referred to as Connected Autonomous Vehicles (CAVs)), they can further improve road safety by sharing their information with each other such as position, velocity, future path, etc. In addition, CAVs are projected to improve fuel consumption, travel time, and passenger comfort through cooperative driving.

Achieving cooperative behaviors among robots is typically studied under multi-agent motion planning in the robotics domain. Existing techniques can be categorized into two groups: i) centralized; and ii) decentralized (distributed). In centralized approaches, it is assumed that a central planner exists that has access to all information and computes the trajectory for robots e.g. path-velocity decomposition technique, while in decentralized approaches, each robot is assumed to have incomplete information and autonomously determines a plan while avoiding static and moving obstacles as well as other robots. Although centralized approaches can find the optimal solution, they are computationally demanding and less tolerant of uncertainty. On the same lines, in the Intelligent Transportation System (ITS) domain, centralized and decentralized techniques are proposed where CAVs share their information with each other (through V2V) or the infrastructure (through V2I) to perform traffic management at intersections, merges.

In general, existing motion planning algorithms and traffic management techniques consider a safety buffer around each vehicle to cover for uncertainties in the localization and trajectory tracking, and then a reference trajectory is determined. A trajectory is considered to be safe if the safety buffer of the vehicle does not overlap with obstacles or other vehicles' safety buffer at any point in time. While reasonable, this definition may not provide absolute safety because it implicitly assumes that all vehicles will follow the plan (with small errors that are within the safety buffer). However, any disruption in the plan can cause an accident. For example, consider a scenario when two vehicles are driving on a street, one behind the other. If the front vehicle suddenly stops for any unplanned reason (e.g. yielding to a jaywalker), then the rear vehicle may hit the front car. In common driving parlance—the rear vehicle should not tail-gate the front vehicle.

Responsibility-Sensitive Safety (RSS) approach from Mobileye+Intel addresses the safety issue from the legal/blame perspective and allows vehicles that have the right-of-the-way according to the rules of the road to change their plans. RSS proposes a set of safety rules such that if a vehicle abides by these rules, then it cannot be blamed for an accident. In the scenario that is mentioned above, RSS rules are used to determine the minimum distance at which the rear vehicle should follow the front vehicle so that it will be able to stop without causing an accident even in the worst-case scenario. RSS uses a lane-based coordinate system to define lateral and longitudinal distances between vehicles depending on the driving scenario. For example, there is a longitudinal rule for the scenario when two vehicles are one in front of the other, and there is a lateral rule for the scenario in which two vehicles are driving in parallel to each other. The longitudinal direction is toward the center-line of the lane and the lateral direction is perpendicular to the center-line of the lane. The main shortcoming of RSS is that it is scenario-based and not all scenarios are covered because longitudinal and lateral distances cannot be computed for merges, intersections, and unstructured roads where lane markings are not provided. One aspect of the present disclosure is to provide a trajectory-based definition for RSS rules, that works in all situations, including merges, intersections, and unstructured roads.

When CAVs interact with each other in different scenarios, they may face a deadlock situation where CAVs yield to each other for an indefinite time. Researchers have proposed methods to detect and resolve deadlocks at intersections and roundabouts. In existing approaches, the intersection/roundabout area is divided into a grid of zones, and vehicles that intend to occupy the same zone are said to have a conflict. Then, the dependencies between CAVs (who should enter a conflict zone first and who enters second) are represented with a directed graph, and deadlocks are resolved by removing cycles in the graph. One of the limitations of existing approaches is that they use fixed zones to detect conflicts between vehicles and the size of each zone affects the efficiency and computational complexity of the conflict detection algorithm since using coarse grids makes the schedule pessimistic and using fine grids increases the number of checks. Furthermore, in existing approaches, the dependency graph is computed individually by each CAV, which is extremely inefficient because the same computing is done redundantly and the overhead grows as the number of vehicles increases. Another aspect of the present disclosure is to provide an efficient and decentralized approach to detect and resolve deadlock where each CAV determines only its own conflicts.

A CAV system100described herein provides a cooperative driving and deadlock resolution approach for a plurality of CAVs102. Instead of a lane-based coordinate system, the CAV system100uses future trajectories of CAVs102to represent their conflicts, which can be applied to any road geometries and situations. The CAV system100includes a new set of safety rules for CAVs102to guarantee that no accidents happen if each CAV102of the plurality of CAVs102abides by proposed RSS rules. The CAV system100also provides an efficient and decentralized deadlock detection and resolution mechanism for CAVs102. The integration of the proposed RSS safety rules and deadlock resolution algorithms with motion planning is also provided. Results from conducting experiments on a realistic simulator—that considers vehicle dynamics and network delay—demonstrate that all CAVs102remain safe even if one or more CAVs102slow down or stop at any point in time. The CAV system100was evaluated by comparing the average travel time of CAVs102with a case that vehicles are autonomous but not connected. Finally, a deadlock resolution mechanism for an intersection scenario is also disclosed. Examples of conflict scenarios are illustrated inFIGS.1-3and will be described in greater detail in a later section below.

Referring briefly toFIGS.4A and4B, the CAV system100is illustrated. As shown inFIG.4A, the CAV system100includes the plurality of CAVs102and is illustrated from the perspective of a single “ego” CAV102A relative to a plurality of additional CAVs102B-102N of the plurality of CAVs102. Each CAV102including the ego CAV102A includes an onboard computing system200in operative association with a mechanical system150of the respective CAV102. The computing system200includes a processor220in association with a memory240, which includes various modules including a CAV info module270that aggregates and stores operating information specific to the CAV102, a motion planner module280that plans a motion of the CAV102as will be discussed in greater detail herein, and a motion controller module285that controls the motion of the CAV102. The motion controller module285applies one or more electrical inputs to various components of the mechanical system150, including but not limited to a throttle152that controls an acceleration of the CAV102, a braking mechanism154that controls a deceleration of the CAV102, and a steering mechanism156that controls a trajectory angle of the CAV102. Additionally, the computing system200can include a GPS module230operable to determine a latitudinal “x” position, a longitudinal “y” position, an instant velocity, and also provide a clock time to the CAV102such that timestamps for all CAVs102are accurate to one another. The computing system200further includes a network interface module210operable to facilitate communication to and from other CAVs102.

Turning toFIG.4B, a data flow diagram is illustrated showing additional modules of the computing system200communicating with one another. Similarly,FIG.4Bis shown from the perspective of an ego CAV102A relative to additional CAVs102B-102N; however, any CAV102within the CAV system100can be considered the ego CAV102A. The ego CAV102A maintains various values related to a vehicle state in the CAV Info module270, including but not limited to ID, x and y position, velocity, timestamp, some of which are communicated to a future path module272that determines a future path of the ego CAV102A based on a known starting position and destination position of a world map. The future path determined by future path module272can be an array of x-y coordinates or “waypoints” stored within CAV Info module270. The information maintained by CAV info module270including the future path is periodically broadcast to additional CAVs102B-102N by the network interface module210, which also receives future paths from the additional CAVs102B-102N.

Future paths periodically received from additional CAVs102B-102N are passed to a Conflict Zone Detection Module274that compares the future path of the ego CAV102A with the future paths received from additional CAVs102B-102N to identify a set of conflict zones (C) where the future paths conflict with one another. The Conflict Zone Detection Module274additionally creates a Partial Dependency Graph (PDG) for the ego CAV102A that indicates conflict relationships between the future paths of the ego CAV102A and the additional CAVs102B-102N. However, the ego CAV102A is initially ignorant to conflict relationships between the additional CAVs102B-102N that the ego CAV102A is not directly involved with. To remedy this, the ego CAV102A broadcasts its PDG to the additional CAVs102B-102N through the network interface module210, and in turn receives a plurality of additional PDGs from the additional CAVs102B-102N that are each calculated from the perspectives of each respective additional CAV102B-102N. Using the PDG belonging to the ego CAV102A and the additional PDGs from the additional CAVs102B-102N, the ego CAV102A constructs a Complete Dependency Graph (CDG) at a CDG construction module276that indicates conflict relationships between all involved CAVs102including the ego CAV102A and the additional CAVs102B-102N. This reduces computational burden on each CAV102as they do not need to redundantly calculate relationships between all CAVs102involved in the CAV system100. Note that as each CAV102considers itself to be the ego CAV, each involved CAV102constructs its own CDG based on the PDGs it receives from other CAVs102including the example ego CAV102A.

A Deadlock Detection and Resolution module278of the ego CAV102A uses the constructed CDG and the set of conflict zones (C) to iteratively detect and resolve any “deadlocks” that are present within the CAV system100as will be described in greater detail below. Deadlock resolution results in an updated set of conflict zones (C) that are passed to the motion planner module280, which uses a world map graph to determine a planned motion of the ego CAV102including various waypoints and optimal velocities along an optimized future path of the ego CAV102A. Based on results from the Deadlock Detection and Resolution module278, the ego CAV102A can iteratively re-evaluate and update its future path; in some scenarios the “updated” future path can change if the ego CAV102A is expected to yield to another CAV102, or the “updated” future path can stay the same if the ego CAV102A is not expected to yield. These values are additionally used by the motion controller module285to interpret the planned motion into electrical inputs for the mechanical system150of the ego CAV102A, including the throttle152, the braking mechanism154, and the steering mechanism156.

Generic Formulation of RSS Rules

In this section, a trajectory-based formulation is introduced for RSS rules. The advantage of this approach is that the rules are generic and can be applied to all cases, including unstructured roads.

Given the future paths of CAVs102are known, each CAV102can determine a set of conflict zones C. A conflict zone Ci⊂C is defined as a convex contour that includes a subset of future paths (FP) of two CAVs102where a distance between the future paths is less than a threshold, dth. Since two CAVs102may have more than one conflict, only consecutive edges that have a distance of less than dthare considered to be a part of the same conflict zone Ci. The midpoints of the edges are used to calculate the distance between two edges from two future paths. To specify the boundaries of a conflict zone, midpoints of first and last edges are used.

Based on the road geometry and rules of the road, every pair of CAVs102can determine who has an “advantage” to enter the conflict zone first and who has a “disadvantage” of entering the conflict zone second. For simplicity, it is assumed that a CAV102with an earlier arrival time has the advantage. Without loss of generality, it is assumed that in any pair of CAVs102approaching a conflict zone, a first CAV102has the advantage, and a second CAV102has the disadvantage. The CAV system100represents the distance of the CAV102with the advantage from the beginning of the conflict zone and from the end of the conflict zone with abeginAand aendA, respectively. Similarly, the CAV system100represents the distance of the CAV102with disadvantage from the beginning of the conflict zone with abeginD.FIGS.1,2, and3show different scenarios for example CAVS102(where CAVS102with advantages are respectively labeled as10A,20A and30A, and where CAVS102with disadvantages are respectively labeled as10B,20B and30B) where the abeginA, aendAand abeginDare shown. It is assumed that Equation 1 represents the dynamics of each respective CAV102.

Vehicle dynamics will be discussed herein from the perspective of an “ego” CAV102A of the plurality of CAVs102relative to other “additional” CAVs102B-102N of the plurality of CAVs102, where N is a total number of involved CAVs102of the plurality of CAVs102including the “ego” CAV102A. For the examples provided herein and without loss of generality, the “ego” CAV102A can be any CAV102of the plurality of CAVs102and can be at an advantage or a disadvantage when approaching a conflict zone, depending on the scenario. Similarly, other “additional” CAVs102B-102N each respectively consider themselves to be an “ego” CAV with all other CAVs102involved in the system100being “additional” CAVs. The vehicle dynamics described herein apply individually to each CAV102of the plurality of CAVs102. The following vehicle dynamics from the perspective of the “ego” CAV102A are assumed:

{x.=v⁢cos⁡(ϕ)y.=v⁢sin⁡(ϕ)ϕ.=vL⁢tan⁡(ψ)v.=a(1)
where x and y respectively represent a latitudinal position and a longitudinal position of the ego CAV102A on the world map in Cartesian coordinates, ϕ is a heading angle of the ego CAV102A from the x-axis, v and a are linear velocity and acceleration of the ego CAV102A respectively, L is a wheelbase distance of the ego CAV102A and ψ is a steering angle of front wheels with respect to the heading of the ego CAV102A. In order to make the model more realistic, the CAV system100considers an upper bound and a lower bound on the acceleration rate and steering angle of the ego CAV102A as: ∈[amin,emax] and Ψ∈[Ψmin,Ψmax] where amaxand aminare the maximum acceleration and deceleration rates and Ψmaxand Ψminare the maximum and minimum steering angles of the ego CAV102A.

For simplicity, the trajectory of the ego CAV102A is projected onto its path and represented with a double-integrator model. As a result, a stop distance of the ego CAV102A in the case of being at an advantage when entering a conflict zone is calculated as:

dstopA=vA22⁢❘"\[LeftBracketingBar]"abrake❘"\[RightBracketingBar]"(2)

The CAV system100assumes that the ego CAV102A (and each of the additional CAVS102B-102N) broadcasts its information every T milliseconds and a worst-case end-to-end delay (p) is 2T. Taking into account the delay, a worst-case stop distance of the ego CAV102A in the case of being at a disadvantage when entering a conflict zone is calculated as:

dstopD=vD⁢ρ+⁢12⁢aACC⁢ρ2+(vD+aACC⁢ρ)2❘"\[LeftBracketingBar]"2⁢abrake❘"\[RightBracketingBar]"(3)

The first two terms (vDρ and ½aACCP2) indicate that the ego CAV102A with disadvantage may be accelerating in the worst-case scenario while waiting for broadcast information from an additional CAV102B with an advantage when entering a conflict zone. If a distance from the end of the conflict zone of the additional CAV102B with advantage is greater or equal to the stop distance of the additional CAV102B with advantage (dendA≥dstopA), there is a possibility that it may slow down and stop inside the conflict zone and block the ego CAV102A with the disadvantage. Otherwise, there is no conflict. Accordingly, the modified RSS rule is defined as:

DEFINITION 1. General RSS Rule: Given the entering order of an ego CAV102A and an additional CAV102B to a conflict zone is known, the minimum safe distance to maintain from the conflict zone (dSAFED) for an ego CAV102A having a disadvantage is:

dSAFED={dstopD-dscenarioA+VLA+VLD2if⁢dendA>dstopA0otherwise(4)
where dscenarioAis the scenario-dependent distance that the other CAV102B with advantage travels inside the conflict zone, VLDis the vehicle length for the ego CAV102A with the disadvantage and VLAis the vehicle length for the other CAV102B with the advantage. Since the distance values are calculated based on the centers of the CAVs102A and102B, the term

VLA+VLD2
is added. In addition, the following rule should be satisfied to make sure the travelled distance during the response time of the ego CAV102A with disadvantage is not greater than the safe distance:
dSAFED>vDρ+½aACCρ2(5)
Note that while the above explanation is in terms of the ego CAV102A being at a disadvantage relative to the other CAV102B, the above can also be applied vice-versa with the ego CAV102A being at an advantage and the other CAV102B being at a disadvantage.
LEMMA 3.1. If the CAV with disadvantage always maintains a distance of at least dSAFEDfrom its conflict zone, it will not hit the CAV with advantage even if it changes its plan and decelerates at any point in time.

PROOF. If the distance of a CAV102with the advantage from the end of the conflict zone is smaller than its stop distance, dendA<dstopA, the CAV102with the advantage will stop outside of the conflict zone even if it decelerates at a rate of smaller than or equal to abrake.

If the distance of the CAV102with the advantage from the end of the conflict zone is greater than its stop distance, dendA<dstopA, the CAV102with the advantage may stop inside the conflict zone if it decelerates. In this case, the CAV102with disadvantage will be notified after ρ milliseconds in the worst-case scenario. If the CAV102with disadvantage accelerates at a rate of smaller than or equal to aACCduring this time interval (ρ) and then decelerates at a rate of abrake, its stop distance will be equal to dstopD(Eq. (3)) and the CAV102with disadvantage will not enter the conflict zone and no accident will happen. For scenarios where the scenario-dependent distance is not zero, dscenarioA>0 (same lane and merge), the paths of the CAVs102overlap and if the CAV102with advantage decelerates, it will allow the other CAV102with disadvantage to travel through the conflict zone by dscenarioAand still be safe. As a result, the required safe distance is dstopD−dscenarioA.

Next, a few case studies will be discussed and show how the safe RSS distance is calculated for each scenario.

Same Lane

Let us consider an example scenario where two CAVs10including a first CAV10A and a second CAV10B are driving in the same lane as depicted inFIG.1. The first CAV10A has the advantage since its arrival time at the conflict zone is smaller than the second CAV10B. In this example, the ego CAV102A could be the first CAV10A (which would make the additional CAV102B the second CAV10B) or the second CAV10B (which would make the additional CAV102B the first CAV10A) depending on an arrival order of the ego CAV102A relative to the additional CAV102B.

Since the path of the first CAV10A overlaps with the path of the second CAV10B, dscenarioA=dstopA, which means the first CAV10A travels dstopAmeters inside the conflict zone before a complete stop and the second rear CAV second CAV10B. has dstopAmeters more to stop. According to Equation (4), the required safe distance for the second CAV10B (dSAFED) to maintain from the conflict zone/first CAV10A is:

dSAFED=dstopD-dstopA+VLD+VLA2
where dstopDand dstopAare calculated according to Equation 2 and 3.
Intersection

Now, let us consider a scenario where two CAVs20including a first CAV20A and a second CAV20B approach an intersection and their future path crosses inside the intersection area as it is depicted inFIG.2. The CAV system100assumes the arrival time of the first CAV20A to be earlier than the second CAV20B and therefore, the first CAV20A has the advantage. In this example, the ego CAV102A could be the first CAV20A (which would make the additional CAV102B the second CAV20B) or the second CAV20B (which would make the additional CAV102B the first CAV20A) depending on an arrival order of the ego CAV102A relative to the additional CAV102B.

If the first CAV20A stops anywhere inside the conflict zone, it is not safe for the second CAV20B to enter the conflict zone. Therefore, the scenario-dependent distance is zero, dscenarioA=0. As a result, the following is given:

dSAFED={dstopD+VLA+VLD2if⁢dendA≥dstopA0otherwise

If the distance of the first CAV20A from the end of the conflict zone is smaller than its stop distance, even in the worst-case (if the first CAV20A decelerates at the maximum rate), the first CAV20A will stop outside the conflict zone and does not cause a conflict for the second CAV20B. In this case, there will be no conflicts and dSAFED=0.

Merge

Next, the CAV system100considers a merge scenario where two CAVs30merge into the same lane as it is shown inFIG.3. Without loss of generality, the CAV system100assumes that a first CAV30A has the advantage and a second CAV30B has disadvantage respectively. In this example, the ego CAV102A could be the first CAV30A (which would make the additional CAV102B the second CAV30B) or the second CAV30B (which would make the additional CAV102B the first CAV30A) depending on an arrival order of the ego CAV102A relative to the additional CAV102B.

In this scenario, the scenario-dependent distance is dscenarioA=min(0,dstopA−dmergeA), where dmergeAis the distance of the first CAV30A with advantage from the merging point, which is indicated inFIG.3. As a result, the second CAV30B must maintain a minimum distance of:

dSAFED=dstopD-min⁡(0.dstopA-dmergeA)+VLD+VLA2
from the conflict zone. Note that once the second CAV30B reaches the merge point, the dscenarioAis changed. The lateral case in the original RSS rules (two CAVs driving on adjacent lanes) can be modeled like a merging case. If a first CAV30A attempts to merge into the other lane occupied by the second CAV30B, it is only allowed if the created conflict zone is far enough from the second CAV30B i.e. at least dmax.
Cooperative Driving System

An algorithm that runs on each CAV102assuming no deadlock situation happens will be discussed from the perspective of the ego CAV102A. It should be noted that each respective CAV102of the plurality of CAVs102considers itself to be an “ego” CAV102A relative to a plurality of additional CAVS102B-102N. Further, a deadlock resolution algorithm will also be discussed.

Main Algorithm

Given the initial position and final destination of the ego CAV102A are known, a motion planner module280onboard the ego CAV102A uses the world's map to determine the shortest route (R) that connects the current position of the ego CAV102A to the destination of the ego CAV102A. The CAV system100assumes that at least one feasible path exists that connects the current location of the ego CAV102A to its destination. The world map, M(N,E), is a directed graph where N is a set of nodes (referred to herein as “waypoints”) and E is a set of edges or connections between waypoints. Each edge of the map graph has a weight, w, which indicates the maximum velocity for that segment of the road. In the motion planner module280, it is assumed that the computation time and communication time for the ego CAV102A are bounded by T.

In a periodic manner, each CAV102broadcasts through network interface module210a set of CAV information which can in some embodiments be stored in a CAV info module270and can include ID, x and y position, velocity, timestamp, and its future path (FP), which is an array of x-y coordinates. The CAV system100assumes that all CAV102synchronize their clocks using GPS so that timestamps are captured with clocks that have almost the same notion of time. When the ego CAV102A receives sets of CAV information of other additional CAVs102B-102N, the ego CAV102A checks if their paths intersect or the distance between their paths is less than a threshold. If so, the ego CAV102A computes a set of conflict zones (C). For each conflict zone, the ego CAV102A determines which vehicle has the advantage to enter the conflict zone first based on who is expected to reach the conflict zone first. To detect possible deadlocks, the ego CAV102A computes a graph called Partial Dependency Graph (PDG), which represents the dependency among other additional CAVs102B-102N and itself (i.e. who should yield to who over a conflict zone). Next, the ego CAV102A broadcasts the computed PDG, and after receiving other PDGs from additional CAVs102B-102N, the ego CAV102A constructs a Complete Dependency Graph (CDG) to detect and resolve possible deadlocks. Finally, if the ego CAV102A has disadvantage over a conflict zone, the ego CAV102A computes a safe velocity so that it always maintains a safe distance from that conflict zone. Based on the determined velocity, the weights of some of the edges are updated to reflect the presence of the additional CAVs102B-102N and to make sure a safe distance is always maintained from the conflict zone. Then, the motion planner module280runs the shortest path algorithm again to check if a shorter path exists that does not cause a new conflict. Finally, the motion controller module285uses a subset of future waypoints of the future path and velocities of corresponding edges to determine desired velocity and control inputs (steering angle and acceleration) for the mechanical system150of the ego CAV102A. Algorithm 1 shows the pseudo-code of the algorithm that is executed on each CAV102. To have a better understanding of the algorithm, different components of the CAV system100and their relationships have been depicted inFIGS.4A and4B. Next, the functionality of each component of the algorithm will be discussed.

Future Path Computation

The ego CAV102A broadcasts the set of CAV information including its ID, position (x, y), velocity (v), and the corresponding timestamp (ts) as well as its future path ((x1, y1), . . . , (xn, yn)) to all additional CAVs102B-102N. Assuming the motion controller module285is tuned to have a short settling time, the ego CAV102A will track its path with a negligible error. As a result, the future position of the ego CAV102A is represented with a subset of its expected route (R). Given R⊂M(N,E) is the route of the ego CAV102A, the future path of the ego CAV102A, FP⊂R is calculated as follows which includes n points:

Algorithm 1: CAVs algorithmwhile has not reached the destination do| FP = compute_future_path( );| CAV_info = [x, y, v, ts, FP, ID];| broadcast(CAV_info);| others_info = receive_other_CAVs_info( );| for each member of other_CAVs_info do| | [C, PDG] = find_conflict_zones(CAV_info,| |  others_info);| end| broadcast(PDG);| others_PDG = receive_other_PDGs( );| CDG = construct_CDG(PDG,| other_PDGs);| C = deadlock_resolution(C, CDG);| if ego CAV has disadvantage over a conflict zone then| | [FP, velocity] = motion_planner(C, Map);| end| motionController(FP, Velocity);end

FP={(xi,yi)∈R⁢❘"\[LeftBracketingBar]"(∑i=2i=n(xi-xi-1)2+(yi-yi-1)2)<dmax}(6)
where dmaxis the fixed length of the future path calculated as:
dmax=vmax(ρ+tb)  (7)
where ρ represents the worst-case end-to-end delay from the ego CAV102A capturing its information and broadcasting it, to actuation of an additional CAV102B based on the received information (seeFIG.5) and tbis the worst-case brake time which can be calculated as

tb=vmax❘"\[LeftBracketingBar]"abrake❘"\[RightBracketingBar]".
FIG.5snows me execution profile of the algorithm on two CAVs102(i and j) (where CAV i can be considered to be the ego CAV102A and CAV j can be considered to be an additional CAV102B, or vice versa). Let it be assumed that CAVs i and j have a conflict and CAV i (top) has the advantage. If CAV i slows down due to any reason right after sensing and broadcasting its info, the CAV j will not be notified until receiving the next broadcast. As a result, the worst-case end-to-end delay (ρ) is bounded by 2T as depicted inFIG.5. By computing the dmaxbased on the worst-case info sharing delay and worst-case braking time, the CAV system100ensures that for the first time that two CAVs102A and102B detect that they have a conflict, the CAV102A or102B with the disadvantage will have enough distance to safely stop without entering the conflict zone, even in the worst-case scenario.
Conflict Zone Detection

Despite existing approaches that use fixed conflict zones, the CAV system100uses expected trajectories of the CAVS102to detect a conflict zone. As mentioned before, a future path (FP) is used to represent an expected future position of the ego CAV102A. First, the ego CAV102A computes the distance between the mid-point of edges on its path. All contiguous edges that have a distance less than dthare considered to be a part of the same conflict zone.

Two CAVs40A and40B may have multiple conflicts on their paths as depicted inFIG.6. In this example, the ego CAV102A could be the first CAV40A (which would make the additional CAV102B the second CAV40B) or the second CAV40B (which would make the additional CAV102B the first CAV40A) depending on an arrival order of the ego CAV102A relative to the additional CAV102B. Each conflict zone Ciis a data structure that includes waypoints that are inside the conflict zones, distances of CAVs40A and40B from the beginning and end of the conflict zone, their expected arrival time at the conflict zone (Equation 8) and the ID of the CAV40A that has the advantage. The CAV system100computes the arrival times for both CAVs40A and40B assuming the CAVs40A and40B drive at a constant velocity:

T⁢O⁢Ai=dbeginivi(8)
where dbeginiis the distance of the CAV i (40A or40B) from the conflict zone and viis the velocity of the CAV i (40A or40B). Since the algorithm is executed periodically (every T ms), the value of TOAiis updated as the velocity of the CAV i (40A or40B) changes. If a CAV40A or40B is stopped inside a conflict zone, its arrival time is set to zero. By default, the CAV40A with the earliest arrival time will have the advantage unless it is changed to resolve a deadlock (explained later) or the other CAV40B has a priority (e.g. opposite direction). If two CAVs40A and40B have the same arrival time, the CAV40A with the lower ID will have the advantage to break the tie. In addition, if the difference between the arrival times of two CAVs40A and40B is within the accuracy of the clock synchronization (±10 nanoseconds for GPS), they use the ID to determine who has the advantage.
Motion Planner

If the ego CAV102A has disadvantage over a conflict zone, it first checks if an alternative path exists such that it avoids all the conflicts. If such a path exists, the ego CAV102A selects that path and if not, the ego CAV102A calculates a safe velocity (vSAFE) to be maintained so that the ego CAV102A is always safe. The safe velocity, vSAFE, is determined based on the minimum safe distance that the ego CAV102A must maintain from the conflict zone given that the additional CAV102B—which has the advantage—may slow down at any point in time and stop inside the conflict zone.

Maximum Safe Velocity: For each segment of the road that has a distance of dCfrom the conflict zone, the maximum safe velocity for the ego CAV102A is computed using Equation 9.

vSAFE=-(2⁢ρ⁢aACC+2⁢❘"\[LeftBracketingBar]"abrake❘"\[RightBracketingBar]")+Δ2⁢where⁢Δ=4⁢(abrake2+2aACC⁢ρ⁢abrake-aACC⁢ρ2⁢abrake-2⁢dC⁢❘"\[LeftBracketingBar]"abrake❘"\[RightBracketingBar]".(9)
Equation 9 is determined by solving Equation 3 for vDwhen the distance from the conflict zone is dC. dCcan be calculated using Equation 12. Equation (9) ensures that the ego CAV102A with disadvantage has always a minimum distance of dSAFEDfrom the conflict zone.

Once the safe velocity is determined for each conflict zone (Ci), the motion planner module280of the ego CAV102A updates weights of the map M(N,E), to account for the presence of the additional CAVs102B-102N and generates safe velocities for the motion controller module285of the ego CAV102A. To account for the presence of the additional CAVs102B-102N, the motion planner module280determines, U the set of all edges (ei) that are connected to waypoints that are on the future path of other CAVs102B-102N:
U={ei∈E|ei∈connected(FP)}  (10)
where connected(FP) is the set of all edges that are connected to waypoints in the set FP. To account for the safe RSS distance, the motion planner module280of the ego CAV102A determines UD, the set of all edges that are connected to the waypoints that are on the future path of the ego CAV102A with disadvantage (FPD) and are either a member of the conflict zone set (C) or within the safe distance (dSAFED) of the conflict zone.
UD={ei∈E|ei∈connected(FPDC)}  (11)
where connected(FPDC) is the set of all edges that are connected to waypoints in the set (FPDC).

FIG.7shows a merge scenario and future paths of two CAVs50. In this example, the ego CAV102A could be the first CAV50A (which would make the additional CAV102B the second CAV50B) or the second CAV50B (which would make the additional CAV102B the first CAV50A) depending on an arrival order of the ego CAV102A relative to the additional CAV102B. Weights of all edges connected to nodes that are on the path of a first CAV50A with advantage and all edges that are on the path of the second CAV50B and are either within the safe distance of or inside the conflict zone are updated. The set UP and U are highlighted on the path. The subset of future point, FPDC, is determined as:
FPDC={ni∈N|ni∈FPDandni∈Corni∈within(Cj)}
within (Cj) is the set of all waypoints that where their distance from the conflict zone j is less than dSAFE. To calculate the distance between two waypoints, the CAV system100uses the following equation:

distance=∑i=2N(xi-xi-1)2+(yi-yi-1)2(12)
where N in Equation 12 is the number of waypoints including the first and last way-points. Finally, weights of each edge in set U and UDare updated based on their distance from the conflict zone using the safe velocity from Equation 9:

wi=lvSAFEi(13)
where i refers to each segment of the road, l is the length of the corresponding edge and vSAFEiis the safe velocity calculated for each segment of the road (edge). Since the weight of an edge may be updated multiple times—as it may be involved in more than one conflict—, the maximum weight is considered (the slowest safe velocity) for an edge. If the safe velocity vSAFEis equal to zero, instead of infinity, the weight is set to be a large constant number.

After updating the weights, the motion planner module280of the ego CAV102A uses the Dijkstra algorithm to find the shortest path to the destination. The summation of weights (Σwi) from the source to the destination corresponds to the travel duration.

Motion Controller

The motion controller module285uses the future waypoints and safe velocities to calculate the reference heading angle θrefand the safe velocity vreffor the ego CAV102A. For the desired heading angle (θref), the motion controller module285selects a look-ahead point similar to the pure pursuit algorithm and calculates the bearing angle from its current location (x, y) to the look-ahead point:
θref=a tan 2(x−yl,y−yl)  (14)
where xland y correspond to the x-y coordinate of the look-ahead point. It is assumed that each vehicle has an initial desired velocity of v0and never drives faster than that. The motion controller module285uses the weight of the next edge to determine the reference velocity

(vref=diwi).
If the calculated velocity is greater than the initial desired velocity (v0) of the ego CAV102A, the motion controller module285sets the reference velocity to be v0. If the reference velocity is close to zero, (v<ϵ), it is set to zero. Once the reference heading and velocity are calculated, they are passed to two Proportional Integral Derivative (PID) controllers to calculate a steering angle and acceleration for the ego CAV102A:

{ψ=kpeθ+kI⁢∫eθ+kD⁢e.θa=kp′⁢ev+kI′⁢∫ev+kD′⁢ev.(15)
where kP, kI, kDand k′P, k′I, k′Dare constant (controller gains) that are tuned to achieve a fast response while the overshoot is small (short settling time), eθ=θr−θ and ev=vSAFE−v, and evand eθare the derivative of evand eθ, respectively.
Deadlock Detection and Resolution

In order to detect and resolve deadlocks, all CAVs102create a directed graph called the dependency graph. Nodes of the dependency graph are vehicle IDs, and edges of the dependency graph indicate if a CAV102is yielding to another CAV102over a conflict zone. There will be a directed edge from node Vito node Vjif CAV Viis yielding to the CAV Vjover a conflict zone. Since the ego CAV102A determines only the conflicts between itself and other additional CAVs102B-102N—and not the conflicts between other additional CAVs102B-102N, the constructed dependency graph that the ego CAV102A creates is not complete. The dependency graph of the ego CAV102A is referred to as the “partial dependency graph” or PDG. To compute the complete graph, the ego CAV102A broadcasts its PDG to inform the additional CAVs102B-102N about its conflict zones with the additional CAVs102B-102N and to receive PDGs from each of the additional CAVs102B-102N (which are calculated by each respective additional CAVs102B-102N from their own perspectives as “ego” CAVs). From the received PDGs of additional CAVs102B-102N and the PDG of the ego CAV102A, the complete dependency graph (CDG) is constructed. To build the CDG, the PDG stored by the ego CAV102A is incrementally updated by adding nodes and edges for each received PDG. Finally, all edges between two nodes are merged into one.FIG.8shows a scenario with 5 CAVs60A-60E that have each determined their own PDGs and the final consensual CDG.

After constructing the CDG, the ego CAV102A checks if the CDG has a cycle. A Depth-First Search (DFS) algorithm is used to detect cycles. If a deadlock is detected, the ego CAV102A calculates a score for each CAV102that is involved in the cycle based on its average time of arrival at corresponding conflict zones. If an ego CAV102A has m conflicts, its score is calculated as:

S=∑i-1mT⁢O⁢Aim
where TOAiis the time of arrival of the ego CAV102A at its ithconflict zone. The CAV system100selects a CAV102A,102B, . . . or102N of the plurality of CAVs102with the least average time of arrival to have the advantage over all of its conflict zones because on average, it can reach its conflict zone earlier than others. This CAV102A,102B, . . . or102N, which may or may not be the ego CAV102A, is referred to as the leader. Once the leader is determined, the direction of all incoming edges to the leader's node is reversed. If multiple CAVs102have the same score, a CAV102A,102B, . . . or102N with the lower ID number will be selected as the leader. Since there can be more than one cycle in a graph, this process is iteratively repeated until all cycles are removed. The leader changes each time this is process is repeated (as the last leader has already left the conflict zone and is no longer considered to be a deadlocked CAV). Each time a leader CAV moves forward past the conflict zone, scores are calculated again for the remaining deadlocked CAVs and a new leader CAV is chosen.
LEMMA 5.1. If the CDG has no cycles, then there is no deadlock involving the ego CAV.

PROOF. Once the CDG is modified to be acyclic, there is no path (set of sequential edges) starting at node vegothat eventually loops back to node vegoagain, which means the ego CAV102A never yields to other CAVs102B-102N that are yielding to the ego CAV102A and therefore, there is no deadlock involving the ego CAV102A.

It takes some time to resolve a deadlock due to the vehicle's dynamic—CAVs102cannot change their velocity and expected arrival time instantly. As a result, CAVs102may face the same deadline again when they compute the CDG after T. It can be shown that the result of deadlock resolution will be the same (the same CAV102A,102B, . . . or102N will be selected as the leader) until the deadlock is resolved. Since the leader CAV102A,102B, . . . or102N has the least average time of arrival in the first iteration, it does not yield to any other CAV102while other CAVs102involved in the deadlock slow down to yield to at least one CAV102. Therefore, the average time of arrival of the leader CAV102A,102B, . . . or102N will be less than other CAVs102in the second iteration and so on.

Experimental Results

The algorithm was evaluated on a simulator that was developed in Matlab. The tool was created in Python to automatically extract a desired map from the OpenStreetMap (OSM format) and then generate the world map graph for it. Once the map was generated, a driving scenario was created where initial position and velocity and the destination of CAVs are randomly selected. The CAV system used differential equations represented in (1) to model vehicle's dynamics. The size of each vehicle is 5×2 meters, the lane width is 5 meters and the distance between waypoints of the map is 0.5 meters. Gains of the controller for both heading and velocity control are KP=5 and KD=0.1. Other parameters of the vehicle are listed in Table 1. CAVs communication delay is modeled by queuing the broadcast packets. InFIGS.9A-9C, a randomly generated map from openStreetMap, its corresponding map graph and a random scenario with 20 CAVs are depicted.

TABLE 1Parameters of the CAVs for simulation.vminvmaxaminamaxψminψmaxTρ0 m/s23 m/s−8 m/s25 m/s2−π/3 radπ/3 rad0.1 s0.2 s
Safety Evaluation

To demonstrate the safety of the proposed algorithm, a merge and an intersection scenario was created where two CAVs have a conflict on their future path as it is depicted inFIGS.10A and10B.

To verify that CAVs are always safe, the CAV with the advantage was forced to suddenly decelerate at different times. It was shown that no accident will happen regardless of the deceleration time and CAVs maintain a minimum safe distance of 5 meters. Using brute-force testing, the deceleration time of the CAV with the advantage varies in a 30-second interval with a 0.1 second increment that includes critical times that stops inside the conflict zone.FIGS.11and12show the distance between CAVs for the intersection and merge scenarios, respectively. In the intersection scenario, the CAV with the advantage may stop before, inside, or after the conflict zone where distances between CAVs are depicted in dark green, yellow, and blue colors, respectively. For cases that the CAV with advantage stops before or after the conflict zone, the CAV with disadvantage continues while in cases that the CAV with advantage stops inside the conflict zone, the CAV with advantage slows down and stops (depicted in yellow). In the merge scenario, the conflict zone moves with the CAV with the advantage after it reaches the merging point. As a result, the CAV with the advantage either stops before the conflict zone or inside it. For cases that the CAV with the advantage stops before the merging point, the CAV with the advantage continues and enters the merge (depicted in dark green) and for the rest of the cases, the CAV with the disadvantage slows down and stops (depicted in yellow).

Deadlock Resolution Demonstration

To evaluate the deadlock detection and resolution approach, a deadlock situation at the intersection (FIG.13A) was created.FIG.13Bshows the CCG for the scenario. The paths of CAVs were fixed to make a left turn at the intersection while having the same distance from the intersection and the same velocity. The CAVs' behavior were simulated both with and without deadlock detection.FIG.14shows the velocities of CAVs for both cases. For the case that no deadlock resolution is done, CAVs slow down to yield to other CAVs and eventually stop and will wait forever. For the case with deadlock resolution, CAVs slow down at first but speed up when their conflict zone is cleared. It was observed that after 7 seconds, all CAVs reach their desired velocity (10 m/s) while in no deadlock detection case, their velocity converges to zero.

Efficiency Evaluation

To evaluate the efficiency of the CAV system, the performance of the CAV system was compared with the case that vehicles are autonomous but not connected. For the non-connected case, the intersections are managed by stop signs and all other conflicts among CAVs are handled by the AV's perception system e.g. adaptive cruise control (ACC) system. A map was extracted from the OpenStreetMap (FIGS.9A-9C) and simulated three scenarios, i) light traffic with 5 vehicles, ii) moderate traffic with 10 vehicles, and iii) heavy traffic with 20 vehicles being present at the same time. When a vehicle exits the map boundary, a new vehicle is spawned. The average velocities of CAVs were measured and reported them in Table 2. The fuel consumption of CAVs was also computed using the following model and reported them in Table 2:

f={0if⁢PT>0fi3600+β1⁢PT+β2⁢aPIotherwise(16)

Where PT=min(P max, PC+PI) is the total tractive power (kW), PC=b1v+b2v3is the cruise component of total power (kW),

PC=b1v+b2v3
if the cruise component of total power (kW),

PI=mav1000
is the inertia component of the total power (kW), fi=888.8 mL/h is the instantaneous fuel consumption rate (mL/s), P max is the maximum engine power (kW), m is the vehicle mass, a and v are the instantaneous acceleration and velocity, b1 is rolling resistant factor (kN), and b2 is the aerodynamic drag factor (kN/(m/s)2), β1 and β2 are the efficiency factors for non-accelerating and accelerating cases.

TABLE 2Comparing the average velocity and fuel consumptionof vehicles when they navigate autonomously (non-connected) and cooperatively (connected).Light TrafficModerate TrafficHeavy TrafficAVsCAVsAVsCAVsAVsCAVsAverage10.5111.5510.9111.8311.2111.96Velocity (m/s)Average Fuel1.2710.4951.0890.4791.0170.485Consumption(mL/s)

With the help of shared information, CAVs not only drive at higher velocities, they drive smoother than non-connected case because they slow down and stop less frequently and therefore, their fuel consumption is less than the connected case.

Computing System

FIG.15is a schematic block diagram of an example device200that may be used with one or more embodiments described herein, e.g., as a component of CAV system100and/or as computing system200shown inFIG.4A.

Device200comprises one or more network interfaces210(e.g., wireless, PLC, etc.), at least one processor220, and the memory240interconnected by a system bus250, as well as a power supply260(e.g., battery, plug-in, etc.).

Network interface(s)210include the mechanical, electrical, and signaling circuitry for communicating data over the communication links coupled to a communication network. Network interfaces210are configured to transmit and/or receive data using a variety of different communication protocols. As illustrated, the box representing network interfaces210is shown for simplicity, and it is appreciated that such interfaces may represent different types of network connections such as wireless and wired (physical) connections. Network interfaces210are shown separately from power supply260, however it is appreciated that the interfaces that support PLC protocols may communicate through power supply260and/or may be an integral component coupled to power supply260.

Memory240includes a plurality of storage locations that are addressable by processor220and network interfaces210for storing software programs and data structures associated with the embodiments described herein. In some embodiments, device200may have limited memory or no memory (e.g., no memory for storage other than for programs/processes operating on the device and associated caches).

Processor220comprises hardware elements or logic adapted to execute the software programs (e.g., instructions) and manipulate data structures245. An operating system242, portions of which are typically resident in memory240and executed by the processor, functionally organizes device200by, inter alia, invoking operations in support of software processes and/or services executing on the device. These software processes and/or services may include CAV Control processes/services290described herein which can include the various modules stored in memory240and illustrated inFIGS.4A and4Bincluding CAV info module270, future path module272, conflict zone detection module274, deadlock detection and resolution module278, CDG construction module276, motion planner module280and motion controller module285. Note that while CAV Control processes/services290is illustrated in centralized memory240, alternative embodiments provide for the process to be operated within the network interfaces210, such as a component of a MAC layer, and/or as part of a distributed computing network environment.

It will be apparent to those skilled in the art that other processor and memory types, including various computer-readable media, may be used to store and execute program instructions pertaining to the techniques described herein. Also, while the description illustrates various processes, it is expressly contemplated that various processes may be embodied as modules or engines configured to operate in accordance with the techniques herein (e.g., according to the functionality of a similar process). In this context, the term module and engine may be interchangeable. In general, the term module or engine refers to model or an organization of interrelated software components/functions. Further, while the CAV Control processes/services290is shown as a standalone process, those skilled in the art will appreciate that this process may be executed as a routine or module within other processes.

It should be understood from the foregoing that, while particular embodiments have been illustrated and described, various modifications can be made thereto without departing from the spirit and scope of the invention as will be apparent to those skilled in the art. Such changes and modifications are within the scope and teachings of this invention as defined in the claims appended hereto.