# Dynamic distributed lanes: motion planning for multiple autonomous vehicles

## Abstract

Unorganized traffic is a generalized form of travel wherein vehicles do not adhere to any predefined lanes and can travel in-between lanes. Such travel is visible in a number of countries e.g. India, wherein it enables a higher traffic bandwidth, more overtaking and more efficient travel. These advantages are visible when the vehicles vary considerably in size and speed, in the absence of which the predefined lanes are near-optimal. Motion planning for multiple autonomous vehicles in unorganized traffic deals with deciding on the manner in which every vehicle travels, ensuring no collision either with each other or with static obstacles. In this paper the notion of predefined lanes is generalized to model unorganized travel for the purpose of planning vehicles travel. A uniform cost search is used for finding the optimal motion strategy of a vehicle, amidst the known travel plans of the other vehicles. The aim is to maximize the separation between the vehicles and static obstacles. The search is responsible for defining an optimal lane distribution among vehicles in the planning scenario. Clothoid curves are used for maintaining a lane or changing lanes. Experiments are performed by simulation over a set of challenging scenarios with a complex grid of obstacles. Additionally behaviours of overtaking, waiting for a vehicle to cross and following another vehicle are exhibited.

### Keywords

Autonomous vehicles Robotics Graph search Planning Multi-robot path planning Multi-robotic systems## 1 Introduction

The advantages of autonomous vehicles over human-driven vehicles include safety, efficiency and personal comfort [4]. Autonomous vehicles, connected via an inter-vehicle communication framework [30, 37], may ‘talk’ and inform each other about the road scenario. They may further communicate with the road infrastructure [21], a processing unit fixed with the roads. The vehicles may hence make collaborative plans which are better than human or machine-based planning without communication wherein one has to ‘guess’ the intent of the other driver [43]. Advantages include traffic jam avoidance [7], collaborative obstacle avoidance [15], failure detection [42], etc. Attributed to the advantages of autonomous vehicles, it will become increasingly possible to have autonomous vehicle only traffic in the future for large sections of city/motorway traffic, once such autonomous vehicles are commercially available. This is hence a common assumption in the transportation literature.

Most countries follow organized traffic where the road width is divided into a number of lanes and vehicles are asked to drive within a lane, changing lanes if necessary. The task of planning is primarily to decide, at every instance of time, the lane and speed of travel [29, 40]. On the contrary some countries like India stick to unorganized traffic wherein a vehicle can travel anywhere on the road and sticking to a particular lane is not a requirement [23, 26, 39]. The width of a lane, in organized traffic, is kept just larger than the maximum width of a vehicle. Vehicles larger than a lane’s width are not allowed, while much smaller vehicles (excluding motorbikes) are very rare. If traffic is under the occupancy of vehicles of varying sizes, from very small to large, the unutilized width of lanes in occupancy with smaller vehicles would add up to a significant amount, enough to fit in a lot more vehicles. This is the main reason why unorganized Indian traffic shows a higher bandwidth as compared to that possible with organized traffic (restricted to the number of lanes), where the vehicles vary in sizes from cycles, motorbikes, 3-wheeled auto rickshaws, cars to big trucks.

Further, big differences in speed between vehicles mean that every overtaking manoeuvre must be completed as soon as possible, with the cooperation of the other vehicles involved. In unorganized traffic this is possible as soon as space is available, while in organized traffic one would have to wait for the overtaking lane to be clear. Unorganized Indian traffic shows multiple simultaneous overtakes, wherein cars overtake 3-wheeled auto rickshaws, which again overtake cycles; while motorbikes overtake or get overtaken depending upon the situation/driver. Each such overtake is useful as it is ‘painful’ to follow a slower vehicle at its speed. Further, if small obstacles lie on all lanes of travel, all the lanes are rendered un-navigable, while it may still be possible to navigate in between the obstacles, which is what many human drivers, even in organized traffic, tend to do.

Sticking to lanes is a good traffic rule if the majority of the vehicles in the traffic are wide enough to occupy the entire lane and the standard deviation in speed is small. However, as the size and speed diversity increases, the limitations of this traffic rule become increasingly apparent, which explains why some countries stick to unorganized traffic. Unorganized traffic is more generalized in nature and is capable of displaying a richer set of traffic behaviours which are restricted in organized traffic due to the lane-sticking traffic rule. Motion planning for unorganized traffic is important in order to get autonomous vehicles into countries following unorganized traffic. Further, fully autonomous vehicles not requiring a human inside may have speed and size designed to best cater for business requirements. These vehicles may bring large diversity to the traffic landscape, questioning the validity of lanes in the presently organized countries.

Interesting research has been done for planning a vehicle in organized traffic operating within lanes, which is not applicable for unorganized traffic. Furda and Vlacic [10] modelled the problem as a deterministic state automata problem based on raw sensor data, using which the authors exhibited robotic behaviours of lane keeping, maintaining a safe distance from other vehicles and avoiding any potential collisions. Multi-Criterion Decision Making was used for strategy selection. On the other hand, Valdes et al. [38] considered optimal routing strategies, particularly for cooperative multi-vehicle situations. Reveliotis and Roszkowska [31] meanwhile modelled the system as a resource allocation problem with lanes as virtual resources, which the vehicles attempted to acquire. Conflict resolution strategies for allocation prohibited collisions. Schubert et al. [34, 35] fed the distances from between the lane markings and the vehicles into an automaton to compute the optimal lane of travel.

Some attempts have been made for motion planning without necessarily the assumption of lanes. Kala and Warwick [12, 13, 15] studied the problem of planning and coordination of vehicles using a set of discrete behaviours. Kuwata et al. [17] used Rapidly-exploring Random Trees for the planning of a single vehicle using a biased sampling technique for obstacle avoidance. Chu et al. [8] constructed a number of trajectories for obstacle avoidance using a single manoeuver, out of which the best was used. Gehrig and Stein [11] used an elastic band attached to a leader vehicle to model the motion of the follower vehicles. These approaches can however all only exhibit simple behaviours of vehicle following, overtaking or obstacle avoidance; but cannot display the strong coordination strategies possible with the availability of communication.

The problem of planning in unorganized traffic is closer to the problem of multi-robot motion planning, for which a number of interesting approaches exist. The difference between wide maps used in mobile robotics, as opposed to the narrowly bounded roads in the case of vehicles does though question the validity of many approaches. Other differences include the notion of overtaking and vehicle following, terms which are largely baseless in robotics; the absence of a predefined goal in vehicles; the continuous emergence of vehicles in a road, unlike in mobile robotics where robot positions are initially known and any addition causes re-planning.

Centralized approaches [33] are non-implementable in real time, due to which planning needs to be performed in a decentralized manner [2, 20, 36]. Pure decentralized approaches cannot though model strong cooperation between the vehicles which is only possible with centralized approaches. Hence the use of a decentralized approach must ensure that the resultant system can showcase all types of desirable behaviours between a pool of vehicles, e.g. in prioritized approaches [3] lower priority vehicles always lose out, which can lead to overtaking becoming unfeasible due to a lack of cooperation.

Graph search based methods are widely used for robotic planning. However a centralized graph search approach for multi-robots is clearly computationally too expensive. For a single robot, approaches rely on the use of hierarchies for computational speedup. However in a road based system, it is impossible to make any decision (to overtake or not, side to avoid an obstacle, etc.) at a lower resolution map, since all such behaviours take place with small separations which must be precisely known. Kala et al. [16] presented an iterative graph search algorithm, where the map was represented at multiple resolutions. The resolution around the path was increased along with iterations to get better paths. Lu et al. [19] also used an iterative graph search, employing the Lifelong Planning A^{∗} algorithm for a dynamic environment. Chand and Carnegie [6] decomposed the problem at two levels, using the A^{∗} algorithm for both levels. A finer A^{∗} algorithm computed the connectivity cost between coarser level nodes. A modified A^{∗} algorithm was also used by Viet et al. [41] for cleaning robots. Cowlagi and Tsiotras [9] meanwhile studied the problem of different kinematics for different robots, and modelled a graph with multiple costs at the edges depending upon the kinematics and state. Model predictive control was then used for local trajectory tracking.

This paper is motivated by an earlier work by the authors [14] where a 4 layer planning algorithm was proposed consisting of route selection, an obstacle avoidance strategy, distribution of available (obstacle-free) road width amongst the vehicles, and trajectory generation. But a limitation of the work was that the different layers worked at different abstractions and the decisions of any particular layer were made based on abstract knowledge of the other vehicles available for that layer. However, as most decisions relating to overtaking, obstacle avoidance, and other manoeuvres take place with small separations, precise information is needed for optimal decision making. Hence instead of dividing the algorithm into hierarchies, here the intention is to stick to the finest hierarchy and use heuristics to limit the state expansions, thereby forming a completely different method of making graph searches faster.

The key contributions of this paper are: (a) A generalized notion of lanes is defined. (b) The generalized notion is used for planning and coordination of multiple vehicles. (c) A coordination technique is designed which uses the concepts of decentralized coordination for iteratively planning different vehicles but empowers a vehicle to pass other vehicles. The coordination is hence better in terms of optimality and completeness than purely decentralized approaches, while being somewhat computationally expensive in the worst cases. (d) The concept of one vehicle waiting for another vehicle coming from the other direction is introduced, when there may be space enough for only one vehicle to pass. (e) Heuristics are used for pruning the expansions of states, which result in a significant computational efficiency while leading to a slight loss of optimality.

Section 2 deals with the generalized notion of lanes. Section 3 presents the algorithm framework for a single vehicle, which is extended for the presence of multiple vehicles in Sect. 4. Results are given in Sect. 5. Discussion is included in Sects. 6 and 7 contains the concluding remarks.

## 2 Generalized notion of lanes

Inspired by organized traffic, we first address the question of what constitutes lanes in unorganized traffic, which is used as the basis of the planning algorithm. In other words the concept of lanes is generalized here from organized to unorganized traffic.

- (i)
*Distributed*—The number of lanes and their widths may change in different segments of the road. Consider a thin slice of road, which is segmented so as to only include obstacle-free regions. The number and width of lanes change as the slice is traversed along the length of the road. Both the number and widths of lanes at the road slice depend on the width of the road, presence of obstacles, demand of the road slice at any instance of time, and widths of vehicles demanding the road slice. - (ii)
*Dynamic*—The lanes change with time as vehicles pass by. The number and widths of lanes at a road slice change with time. For every vehicle the lane changes with time as the vehicle discovers more vehicles it may possibly overtake, completes an overtake, is overtaken, decides to travel aside some vehicle, and similar situations. - (iii)
*Single vehicle*—No two vehicles may occupy a lane side-by-side. - (iv)
*Variable Width*—Every lane has a different width that depends upon the width of the vehicle that uses it and the total available width (excluding obstacles).

## 3 Planning for a single vehicle

### 3.1 Problem and vehicle design

The first problem we consider is planning a single vehicle. A road segment (part of the entire road), characterized by its two boundaries, is given; which may consist of any number/type of static obstacles. Let *R*_{i} be the vehicle to be planned, initially located at position (\(x^{s}_{i},y^{s}_{i}\)) with orientation \(\theta^{s}_{i}\); and having current speed \(v^{s}_{i}\) (\({\leq} v_{\max}^{i}\), the maximum permissible speed). Further consider that the vehicle is a rectangular grid of known size *len*_{i}×*wid*_{i}. Non-rectangular vehicles are expanded to the nearest rectangular shape or handled as per the bounding box approach [5, 18]. For algorithmic purposes, the *X*-axis is taken as the longitudinal axis, along the direction of the road; and the *Y*-axis is taken as the lateral axis. Considering the possibility of irregular width roads, the *Y*-axis coordinate value is taken as a ratio to the current road width [14].

Let the free configuration space of the vehicle be given by \(\zeta_{\mathit{static}}^{\mathit{free}}\). The initial configuration of the vehicle *S* is known. The algorithm computes the trajectory *τ*_{i}(*t*) for the duration 0≤*t*≤*T*_{i}. Here *T*_{i} is the time for the vehicle to reach the planned end point. The objective of the algorithm is to make the vehicle travel as far as possible in the road segment (or \(\operatorname{maximize}\tau_{i}(T_{i})[X]\)). For trajectories that reach equally as far, the one that takes the shortest time is selected (or \(\operatorname{minimize}T_{i}\)). If the vehicle travels further, there is less chance of it being struck in an obstacle framework with no way out other than reversing, if there is no single collision free trajectory which can overcome all obstacles as per the limited view of the current road segment being planned.

*L*

_{i}〈

*x*

_{i},

*y*

_{i},

*θ*

_{i},

*v*

_{i},

*t*,

*τ*

_{i},

*lane*

_{i}〉 where (

*x*

_{i},

*y*

_{i}) denotes position,

*θ*

_{i}denotes orientation,

*v*

_{i}denotes speed (constant) and

*t*is the time of arrival at the state,

*τ*

_{i}denotes trajectory from the source that leads to the state, and

*lane*

_{i}denotes the generalized lane from the source to the state. To make discussions simple, in this text we use the term state to refer to both a state of the configuration space (consisting of 〈

*x*

_{i},

*y*

_{i},

*θ*

_{i}〉 only) as well as a node of the graph search. The cost of a state

*L*

_{i}is taken to be its time, or

*L*

_{i}[

*t*]. Once all the nodes are expanded, the best trajectory is selected. The general algorithm is given in Algorithm 1.

### 3.2 State reduction

For a rich set of possible actions, the graph search is computationally expensive and cannot be used. Hence we use heuristic means to carefully select the actions or states to generate in expansion. Two hypotheses are used regarding the motion strategy of the vehicle. (a) Every vehicle attempts to move in the road such that its relative position (on the lateral axis) remains the same. This is naturally seen while we drive on roads when no steering is performed while travelling on a straight road. Required turns are made in a manner so as to keep oneself at approximately the same relative position compared to others. (b) The vehicle always attempts to move such that its separation from obstacles or road boundaries is as large as possible [1]. However the attempt to maximize the separation is limited to a value of *sm*. This is again visible in everyday driving where in the presence of any obstacle we attempt to align the vehicle, maximizing the separation from the obstacle.

*L*

_{i}〈

*x*

_{i},

*y*

_{i},

*θ*

_{i},

*v*

_{i},

*t*,

*τ*

_{i},

*lane*

_{i}〉 of the graph search is done in a manner that the vehicle moves forward by a magnitude of

*Δ*(

*v*

_{i}) in the

*X*axis. The value in the

*Y*axis is chosen by the above mentioned hypotheses. The choice of new position for the vehicle needs to be done in a manner so as to maximize the separation of the vehicle in both the lateral (

*X*) and longitudinal (

*Y*) axis. Lateral separation is maintained by the hypothesis while longitudinal separation is maintained by

*Δ*(

*v*

_{i}). If an obstacle happens to lie just ahead of the vehicle it may not be able to further continue its journey without collision. The aim is to keep a minimal distance of

*Δ*(

*v*

_{i}) from the obstacle in front.

*Δ*(

*v*

_{i}) (Eq. (1)) must be large enough so that the vehicle can steer itself a significant amount laterally, while travelling longitudinally on the road. Higher speeds would naturally need larger longitudinal distance (along the

*X*axis) to steer across the

*Y*axis.

*c*is a constant called the

*longitudinal separation constant*.

*Δ*(

*v*

_{i}) from the current position (in order to guarantee a longitudinal separation of

*Δ*(

*v*

_{i}) from the expanded state). We consider the

*obstacle analysis line*

*Z*as the

*Y*axis at a longitudinal distance of 2

*Δ*(

*v*

_{i}) from

*x*

_{i}(Fig. 1). Vehicles mostly align themselves along the road, and hence while crossing the line

*Z*, the ideal vehicle’s position should be perpendicular to

*Z*. We find valid states

*P*given by Eq. (2).

*P*as the expanded states. The line

*Z*may cross obstacles, which means all possible states

*P*would be disjoint into smaller sets

*P*

_{a}(

*P*

_{a}⊂

*P*,

*P*

_{a}∩

*P*

_{b}=

*φ*∀

*P*

_{a},

*P*

_{b},

*P*

_{a}≠

*P*

_{b}) (Fig. 1). We reduce the states to be formed by expansion to one per disjoint set. Figure 1 also shows the importance of selecting states from each set, as two of the three sets are later discovered to be sub-optimal (or blocked).

### 3.3 State selection

*l*) in the set

*P*

_{a}is based on the set hypothesis. Let

*d*(

*l*

_{3}) be the distance of

*l*

_{3}lying on

*Z*measured from the road boundary. Let

*s*

_{1}and

*s*

_{2}be the points in

*Z*which have the least and the highest value of

*d*(.) such that the line

*s*

_{1}to

*s*

_{2}is collision free and includes

*P*

_{a}(Fig. 2). Going by the set hypothesis, disregarding the obstacles, the vehicle may attempt to keep itself in the same lateral position on the road which gives its preferred position

*pr*such that

*pr*[

*Y*]=

*y*(or

*L*

_{i}[

*Y*]) through which we may write Eq. (3)

*rl*is the road width at

*Z*.

*pr*may though be such that a vehicle placed at

*pr*is not feasible and within

*P*

_{a}or may not keep a distance of

*sm*from obstacles or road boundaries, which if introduced gives the preferred position

*l*as in Eq. (2).

*sm*on both sides. Equation (4c) states the condition when the vehicle as per

*d*(

*pr*) already maintains a wide margin at both sides. Equations (4b) and (4c) apply for conditions when the vehicle has a wide margin at one side but not on the other side, and needs to be moved so that a margin is produced on both sides. Equation (4d) is an attempt to maximize the separation as much as possible by placing the vehicle in the middle of the segment. Various cases that arise in selection of

*l*are shown in Fig. 3. For details refer to Eqs. (4a)–(4d).

*l*lies on

*Z*which may be quite close longitudinally to an obstacle, even though lateral separation has been maximized. The purpose was to find a valid state at a longitudinal distance of

*Δ*(

*v*

_{i}), say

*l*

_{2}. Let

*Z*

_{2}(

*vehicle placement line*) be the

*Y*axis at a distance of

*Δ*(

*v*

_{i}).

*l*

_{2}(Eq. (5)) is selected as a state such that the vehicle can steer itself and orient itself parallel to the road attaining the correct relative (lateral) position of

*l*[

*Y*] when it crosses the line

*Z*

_{2}. It may then later travel parallel to the road to state

*l*(Fig. 4).

*obstacle analysis line*(

*Z*) keeps the algorithm informed about the obstacles it may face in the future and the vehicle must receive its correct orientation well in advance. The motion takes place only till the

*vehicle placement line*(

*Z*

_{2}), after which (in the next expansion)

*Z*is further moved to keep the vehicle informed about still further obstacles. In this way

*Z*acts as a forerunner for the vehicle, while

*Z*

_{2}acts as the line up to which the vehicle can confidently travel.

### 3.4 Curve generation

The point *L*_{i} is connected to *l*_{2} using clothoid curves [22, 25] denoted by *connect*_{2}(*L*_{i},*l*_{2}). The purpose of clothoids is to generate a curve that makes the vehicle correct its angle of orientation, change its lateral position in the road and generate a curve for the vehicle to traverse on a curved road. *l*_{2} is connected to *l* by a curve parallel to the road denoted by *connect*(*l*_{2},*l*) (Fig. 4). The feasibility of the two curves is checked such that all intermediate points belong to \(\zeta_{\mathit{dynamic}}^{\mathit{free}}\). A small local search around *l* is performed to assess if either of the curves is infeasible.

*small obstacle problem*shown in Fig. 5. The line

*Z*completely misses out citing the small obstacle and hence the computations are erroneous. In such a case, feasibility may only be returned by

*Z*discovering the small obstacle for which

*Δ*(

*v*

_{i}) is avoided by a reduction of speed. Reduction of speed by a discrete amount at a time instant is assumed to be possible as in instantaneous models [27, 28].

*l*

_{2}becomes the expanded state with parent

*L*

_{i}, and is added in the processing queue of the uniform cost search. The region in which

*l*can be moved in

*Z*and correspondingly

*l*

_{2}can be moved in

*Z*

_{2}without causing a collision (under the threshold of

*sm*at both sides from

*l*) constitutes a vehicle’s lane at

*l*

_{2}. The lane definition for tentative motion from

*L*

_{i}to

*l*

_{2}can be visualized by connecting the lane definition at

*L*

_{i}to the lane definition at

*l*

_{2}along the longitudinal axis. The points

*L*

_{i}and

*l*

_{2}characterize the lane, and these are stored for algorithmic purposes. The expansion is given as a pseudo-code in Algorithm 2. The graph generated for a synthetic problem is shown in Fig. 6.

### 3.5 Experimental results

## 4 Planning with multiple vehicles

### 4.1 Problem and algorithm design

The problem consists of *N* vehicles, each vehicle *R*_{i} with its own time of emergence *e*_{i} in the planning scenario. The source *S*_{i} which is the state of the vehicle at *e*_{i} is known, but only after vehicle emergence. Each vehicle is planned upon its emergence. When *R*_{i} arrives, the trajectories *τ*_{j} of vehicles *R*_{j} are already known for *e*_{j}<*e*_{i}. The aim is to generate a plan *τ*_{i} for *R*_{i}. The plan *τ*_{i} is called admissible in the planning scenario *τ* if no collision occurs between any two vehicles or static obstacles. If *R*_{i} fails to generate an admissible plan, the plans of all vehicles are nullified and re-planning is done. The order of planning may be altered and any combination that results in a valid plan may be selected.

We make use of a uniform cost search. The first task is obstacle analysis using the *obstacle analysis line* (*Z*), which gives a set of states *P* in disjoint sets of *P*_{a}, disjoint by static obstacles. This is used to select the expanded state. Here however we need to take into account the other vehicles as well. The general algorithm is the same as Algorithm 1.

*R*_{i} may not be able to construct a feasible plan without moving the other vehicles by modifying their plans. *R*_{i} is allowed to alter the lanes of the other vehicles, which correspondingly changes their trajectories. The lane of any vehicle *R*_{j} is in the form of a set of points *lane*_{j}[*u*] while the trajectory *τ*_{j} is a result of connecting these points by a smooth curve. The points *lane*_{j}[*u*] act as controls available with *R*_{i}, which may alter any of these to produce a different lane for *R*_{j} and hence change the trajectory.

Every graph node (state) maintains trajectories *τ* detailing all alterations made to the trajectories of the vehicles in the path from the source to the state in the graph expansion; and *lane* detailing all alterations made to the lanes of vehicles. Hence every child takes the plan (trajectories and lanes) specified by the parent, makes modification to it as per its desires, and stores the resultant plan which becomes the plan specification given to all its children which may modify it further.

Consider any graph node or state *L*_{i}〈*x*_{i},*y*_{i},*v*_{i},*θ*_{i},*t*,*τ*,*lane*〉 which needs to be expanded using the search technique. We formulate three expansion strategies and any one or more of the strategies may be used for the expansion. These are *free state expansion*, *vehicle following* and *wait for vehicle*. Each of these is discussed in the following sub-sections.

### 4.2 Free state expansion

The *free state expansion* strategy attempts to place every vehicle laterally with *R*_{i} moving with the highest possible speed. In the absence of other vehicles, this strategy is similar to the expansion strategy discussed in Sect. 3. The expansion might lead the vehicle to overtake any slow moving vehicle if it comes on its way. Hence the strategy may also be termed as an *overtaking strategy*.

The problem is the expansion of state *L*_{i} for the valid states *P*_{a} returned by static obstacle analysis. Here we do not select a single position in *P*_{a} for the vehicle *R*_{i}, that was done in planning for a single vehicle. Rather we divide *P*_{a} (specifically, complete *obstacle analysis line* segment *Z* bounded by a road boundary or obstacle from both ends) between the vehicles which plan to move parallel to *R*_{i} in its motion till *Z*, and must hence occupy distinct lanes. Let there be *n* such vehicles. The task is the selection of states *l*^{1},*l*^{2},…,*l*^{n} where *l*^{j} denotes the state of *R*_{j} on crossing *Z*. The task is hence twofold, first to find the vehicles that need to occupy distinct lanes, and second to identify the lane for each of these vehicles (*l*^{j} is known as the lane for *R*_{j} at the particular state).

Imagine a thick slice of road with a vehicle travelling through the slice amidst other vehicles moving under their own plans. If there seems to be a likely collision between any two vehicles, one would like to alter the travel plans of the two vehicles such that they lie in different lanes. Carrying out and completing the alteration of the plan of the new vehicle can span the considered road slice and hence the slice may need to be widened. If the changed plan results in a different collision, one would like to include the new colliding vehicle, and assign all 3 vehicles different lanes. Alterations may result in further widening of the slice. The altered plan may further cause more collisions, in which case the participating vehicles are added. The addition is done till all vehicles can harmoniously cross the road slice. Once the vehicles are identified, the (obstacle free) road slice can be divided between the vehicles, such that each occupies an independent lane across the width of the road slice. This example illustrates the two tasks, to identify the vehicles requiring independent lanes, and to divide the road slice into different lanes between these vehicles.

#### 4.2.1 Computing the number of lanes required

The first task is calculating the vehicles that need separate lanes while *R*_{i} crosses *Z*. As per the hypotheses we need to maximize the lateral separation of *R*_{i} from any other vehicle or obstacle subjected to a maximum of *sm*, while longitudinal separation may preferably be *Δ*(*v*_{i}). As per the free state expansion strategy, all vehicles that as per their plan lie at a longitudinal separation of less than *Δ*(*v*_{i}) need an independent lane. This however excludes the *following a vehicle behaviour*, when the vehicle ahead moves with a greater or equal speed. Giving an independent lane assures that no collision happens as long as the vehicles stick to their allocated lanes, irrespective of what longitudinal separation they maintain. Suppose we select a vehicle *R*_{j} needing a separate lane, which would be required to alter its plan. Modification of the plan of *R*_{j} might make the longitudinal separation between *R*_{j} and any other vehicle *R*_{k} less than the required *Δ*(*v*_{j}), threatening a collision between *R*_{j} and *R*_{k}. In this manner we keep adding vehicles until we reach a point where modification of lanes does not affect the other vehicles.

*H*denote the vehicles (including

*R*

_{i}) requiring independent lanes. Initially the set

*H*contains only

*R*

_{i}. Suppose at time

*t*,

*R*

_{j}as per its planned trajectory is at position

*τ*

_{j}(

*t*). Let

*S*(

*S*

_{a},

*S*

_{b}) denote the segment of road under consideration for computing lanes, where

*S*

_{a}is the start position in the

*X*axis and

*S*

_{b}is the end position in the

*X*axis. Within the region

*S*vehicles must occupy independent lanes, unless a vehicle follows another. Since we are studying the prospective motion of

*R*

_{i}from its current position

*x*

_{i}to its final position at a longitudinal separation of 2

*Δ*(

*v*

_{i}), the initial value of the segment

*S*is (

*x*

_{i}−

*len*

_{i}/2,

*x*

_{i}+2

*Δ*(

*v*

_{i})+

*len*

_{i}/2), accounting for the entire coverage of

*R*

_{i}in this motion (Fig. 8).

*H*and

*S*are iteratively grown until no further vehicle qualifies to be added to

*H*. At any time, every

*R*

_{k}is checked such that motion of any

*R*

_{j}in

*H*within

*S*may result in a collision between

*R*

_{j}and

*R*

_{k}. This separation check is only done at points denoting lanes of

*R*

_{j}. If the separation is less than

*Δ*(

*v*

_{j}), there is a possible collision. In such a case,

*R*

_{k}is added to

*H*and accordingly

*S*is modified to account for the segment

*S*

_{2}that is the region affected by

*R*

_{k}being given an independent lane. The modified

*S*is given by

*S*∪

*S*

_{2}. The procedure is repeated until no vehicle in the planning scenario qualifies to be added in

*H*. The two sub-problems that arise are: calculate

*S*

_{2}if some

*R*

_{j}is given an independent lane, and calculate

*S*if some

*R*

_{j}in

*H*threatens some

*R*

_{k}, requiring it to have an independent lane, each of which is dealt with one after the other. The general algorithm is given in Algorithm 3.

*R*

_{j}needs to steer to get to the new lane, and later steer back to the original lane (Fig. 8); the positions when both these changes happen needs to be calculated. Both turns require a distance of

*Δ*(

*v*

_{j}) along the

*X*axis. New lanes are brought into effect immediately, or

*R*

_{j}is immediately asked to go to the new lane. Suppose

*R*

_{j}at time

*t*has left the point

*lane*

_{j}[

*a*] and moves towards the point

*lane*

_{j}[

*a*+1] (or its motion has been confirmed) as per its planned trajectory. In such a case we cannot alter the motion of the vehicle from

*lane*

_{j}[

*a*] to

*lane*

_{j}[

*a*+1]. Modification of

*lane*

_{j}[

*a*+1] would mean a different trajectory connecting

*lane*

_{j}[

*a*] to

*lane*

_{j}[

*a*+1]. The part of trajectory that vehicle has already travelled from

*lane*

_{j}[

*a*] to

*lane*

_{j}[

*a*+1] may not be the same as the one desired with an altered

*lane*

_{j}[

*a*+1]. However we can certainly modify lanes

*lane*

_{j}[

*a*+2] and onwards (Eq. (6)).

*lane*

_{j}[

*a*][

*T*] denotes the time

*R*

_{j}arrives at the lane, while

*lane*

_{j}[

*a*] denotes the position of the lane.

*L*

_{i}, the algorithm assumes that

*R*

_{i}suddenly disappears after it achieves the expanded state at the

*vehicle placement line*

*Z*

_{2}, while no collision would be recorded in moving from

*L*

_{i}to

*Z*. The subsequent graph expansions cater for the latter motion. Hence after

*R*

_{i}attains the expanded state, other vehicles must return to their original lanes.

*R*

_{i}ends its journey at a longitudinal distance

*Δ*(

*v*

_{i}) from

*x*

_{i}, after which vehicles must maintain an additional longitudinal safety distance of

*Δ*(

*v*

_{i}) before going to their original lanes (Fig. 8). Vehicles may be driving inbound or outbound. Considering both cases,

*R*

_{j}can return to its original lane after it is completely out of the region of considered motion of

*R*

_{i}(considering lengths of both vehicles contribute to their longitudinal occupancy) at

*lane*

_{j}[

*b*] (Eq. (7)). For

*lane*

_{j}[

*b*+1] onwards,

*R*

_{j}has normal lanes.

*lane*

_{j}[

*b*]⊗

*R*

_{j}[

*Y*] denotes the longitudinal coverage of

*R*

_{j}placed at

*lane*

_{j}[

*b*].

No vehicle must lie at a separation of *Δ*(*v*_{j}) from *lane*_{j}[*b*+1], which is the location of *lane*_{j}[*b*+2]. Hence *S*_{2} is given by (*lane*_{j}[*a*+1]−*len*_{j}/2,*lane*_{j}[*b*+2]+*len*_{j}/2). The notations are illustrated in Fig. 8. An assumption here is that after the vehicle *R*_{i} reaches the line *Z*, all vehicles may more or less simultaneously travel back to their original lanes. This may lead to collisions when one vehicle readily moves back to its original lane, while another vehicle takes a little time to start moving back. These are handled by optimization during trajectory generation.

The last task is to check if the changed dynamics of any *R*_{j} cause a potential threat with any general *R*_{k}, indicating the set *H* to be grown to include *R*_{k}. Since the changes in lanes of *R*_{j} are not yet computed (Sect. 4.2.2), the time when *R*_{j} crosses *lane*_{j}[*b*] is unknown. Due to this a threat to any *R*_{k} cannot be determined. This issue may be handled by heuristics. Knowing the positions of the vehicle at time *t* we aim to decide the separation of the vehicles. We divide the decision into two cases. These are whether *R*_{j} and *R*_{k} are travelling in the same directions (both inbound or outbound) or different directions (one inbound and the other outbound). As a general rule *R*_{k} needs to be included in *H* if any part of *R*_{k}, while at *τ*_{k}(*t*), lies within (*S*_{a}, *S*_{b}+*d*).

*d*is taken to be 0. The worst case is when

*R*

_{j}reaches its normal lane at

*lane*

_{j}[

*b*+1] and

*R*

_{k}has not moved much making the separation between

*R*

_{j}and

*R*

_{k}as small as possible. The inclusion of extra distance in computing

*S*

_{2}for

*R*

_{j}ensures this distance is wide enough. Any motion of

*R*

_{k}would make the separation more than the minimum amount of

*Δ*(

*v*

_{j}). The notations are shown in Fig. 9(a). In the case when vehicles are travelling in different directions,

*d*is taken to be 2

*Δ*(

*v*

_{k}). Here since

*R*

_{j}races towards

*R*

_{k}as

*R*

_{k}travels, the distances get shorter very quickly. The smallest distance would be recorded when

*R*

_{j}returns to its normal lane. Since we do not know the time required for the same, and hence the distance travelled by

*R*

_{k}in the process, we calculate an approximate value assuming longitudinal travel for both vehicles (minimum of 2

*Δ*(

*v*

_{k})), as the maximum distance that can be covered by

*R*

_{k}while

*R*

_{j}turns to a modified lane and again back to its normal lane. The notations are shown in Fig. 9(b).

#### 4.2.2 Lane distribution

*s*

_{1}and

*s*

_{2}be the points in

*Z*which have the least and highest value of

*d*(.) (distance from boundary) such that the line

*s*

_{1}to

*s*

_{2}is obstacle free and includes

*P*

_{a}. Let

*rl*be the road width at

*Z*. The preferred position of any

*R*

_{j}in

*H*is given by Eq. (8).

*pr*

_{j}may be infeasible or without maximum separations. We sort the vehicles in

*H*as per their

*d*(

*pr*

_{j}) values.

*pr*

_{i}may not be within the segment, which is corrected by giving it the highest or lowest possible value. Working with any

*R*

_{j}to create maximum separation is similar to the approach used in the single vehicle case, with the addition that

*R*

_{j}might move other vehicles to create extra space for itself. Let

*left*(

*pr*

_{j}) (Eq. (9)) be the point to which

*R*

_{j}placed at

*pr*

_{j}may be moved leftward without colliding with any other vehicle, road boundary or obstacle. The value of

*left*(

*pr*

_{j}) is the distance of that point from the boundary. Similarly for right we have

*right*(

*pr*

_{j}) given by Eq. (10). Notations are shown in Fig. 10.

*pr*

_{j}such that no movement of other vehicles is required and it would maintain a distance of

*sm*from both its ends. Equation (11) gives the precondition and Eq. (12) gives the placement.

*C*is not met, the task is to move the vehicles to have more space for accommodating

*R*

_{j}. A vehicle is only moved if the vehicle already has a distance excess of

*sm*from both its ends. This is the cooperation shown by the vehicles wherein they allow

*R*

_{j}to have sufficient separation by effectively lessening their own excessive separation. The total separation needed by

*R*

_{j}is

*s*(Eq. (13)). The spare separation that

*R*

_{k}has and is ready to offer is given by Eq. (14).

*R*

_{k}, one by one on either side of

*R*

_{i}are moved, till the required separation

*s*is created. Vehicles on the left of

*R*

_{j}are moved towards the left and those on the right of

*R*

_{j}are moved towards the right to get the necessary separation, that is movements are centred along

*R*

_{j}. Notations are shown in Fig. 10.

*s*

_{2}−

*s*

_{1})−∑

_{j}

*wid*(

*pr*

_{j})) is evenly distributed between the vehicles. If all vehicles get sufficient space without overlapping each other, the planned distribution may be used for the planned movement of the vehicles. The complete algorithm is given by Algorithm 4.

#### 4.2.3 Vehicle trajectory generation

In planning the trajectory for the expanded state the task is two-fold. First we need to make *R*_{i} move from the state *L*_{i} to the state \(l^{i}_{2}\) which is obtained from state *l*^{i} as per the terminology followed in planning for a single vehicle. We then need to modify all vehicles in *H*−{*R*_{i}} to occupy their modified lanes, and to further ensure that no collision is recorded between any two vehicles.

*R*

_{i}. We select state \(l^{i}_{2}\) at

*Z*

_{2}, with orientation perpendicular to

*Z*

_{2}and the same lateral position. The Clothoid curve is drawn from

*L*

_{i}to \(l^{i}_{2}\) and a curve parallel to the road is drawn from \(l^{i}_{2}\) to

*l*

^{i}. Both curves are checked such that all intermediate points in both curves belong to \(\zeta_{\mathit{dynamic}}^{\mathit{free}}( \tau^{2} )\). Here \(\zeta_{\mathit{dynamic}}^{\mathit{free}}( \tau^{2} )\) denotes the free configuration space considering the dynamics of vehicles specified in plan

*τ*

_{2}(Eq. (15)). At time

*t*, the space is given by Eq. (16).

*l*

^{i}along

*Z*in order to make the path feasible. On computing a feasible plan the plan vector

*τ*

^{2}is updated to account for the computed trajectory of

*R*

_{i}.

The next task is to plan each of the vehicles excluding *R*_{i} in *H*. For any vehicle *R*_{j} we first update the lane information *lane*_{j}, as computed, and then attempt to make a trajectory by using the updated lane. The change is made by altering the lateral axis values of lanes. All lanes from *lane*_{j}[*a*+2] to *lane*_{j}[*b*] are given a lateral value of *l*^{j}. This lane is then used for the motion of the vehicle. Vehicles may be taken in any order in *H*. However once the trajectory of a vehicle is computed, it is added in the plan *τ*_{2}. The complete algorithm is given by Algorithm 5. The expansion strategy is given in Algorithm 6. We simply count the number of lanes required and distribute the available space between the vehicles. This is followed by the generation of the necessary trajectories.

*R*

_{i}would not be as shown in Figs. 8(b) and 8(c). This is because presently we assume that

*R*

_{i}stops after the planned path at \(l^{i}_{2}\). When we further expand the state of

*R*

_{i},

*R*

_{j}this gives space for

*R*

_{i}to pass by, considering the extended motion as a result of subsequent expansions. In this manner

*R*

_{i}would keep moving with

*R*

_{j}to the side, as planning proceeds denoting motion of

*R*

_{i}along the road. This stops when

*R*

_{i}is sufficiently ahead of

*R*

_{j}.

### 4.3 Vehicle following

The free state expansion attempts to make a vehicle move at its highest possible speed, which may not always result in an optimal plan. Consider that a vehicle *R*_{i} capable of high speeds finds a vehicle *R*_{j} moving slowly in front. *R*_{i} would naturally attempt to overtake *R*_{j}. But overtaking may not be possible due to various reasons like narrowing of road width, discovery of another vehicle, obstacle discovery, etc. In all these cases *R*_{i} has to slow down by significant amounts to avoid a collision. Slowing the vehicle iteratively to compute a possible trajectory is itself a time consuming activity.

The ideal travel plan in the above presented scenario would be planning *R*_{i} to *follow**R*_{j} until the conditions become favourable for overtaking and to accommodate both vehicles in parallel for the required time. Keeping to the general algorithm methodology, we have a state *L*_{i} that we need to expand into state \(l^{i}_{2}\) using the vehicle following strategy. The implementation of this behaviour of the vehicle is simple. It involves two parts. Selecting the vehicle to follow and following the selected vehicle.

*Y*axis. It is however important that the selected vehicle

*R*

_{j}must be passing through

*P*

_{a}. Further the search for a potential

*R*

_{j}is always carried within a longitudinal distance 2

*Δ*(

*v*

_{i}). The other thing that we need to keep in mind in designing the vehicle following strategy is that vehicle

*R*

_{i}would ultimately be attempting to overtake

*R*

_{j}. In order for overtaking to be possible,

*R*

_{i}must have a sufficient longitudinal distance from

*R*

_{j}, which, as per our modelling, is \(\varDelta (v^{\max}_{i})\). Vehicle following is done by reducing the speed of the vehicle

*R*

_{i}to a value of \(v^{\mathit{red}}_{i}\) given by Eq. (17).

*δ*is a small factor maintaining the difference in speeds. The value of speed

*v*

_{j}can be found by looking at

*R*

_{j}’s travel plan at the current time

*t*.

The next task is making the vehicle move forward. We use exactly the same algorithm as in the free state expansion. The only difference is that the obstacle analysis line *Z* is constructed at a longitudinal distance of 2*Δ*(*v*_{i}). However the vehicle placement line *Z*_{2} is constructed at a longitudinal distance of \(\varDelta (v^{\mathit{red}}_{i})\). Since the motion of *R*_{i} is with a speed of \(v^{\mathit{red}}_{i}\), it is evident that all the steering needed would be possible. The task of obstacle analysis at *Z* is to ensure that *R*_{i} avoids all static obstacles and other moving vehicles other than *R*_{j}. Since the width of *R*_{i} is different from *R*_{j}, the manner of avoiding vehicles and obstacles would be different for the two vehicles. While we compute the vehicles for the lane requirements, *R*_{j} having greater or equal speed to *R*_{i} would not qualify for an independent lane. The possibility of a vehicle following another vehicle was accounted for in the lane computation.

*R*

_{i}also needs to take the option of following

*R*

_{j}. As at the time of planning the distance between

*R*

_{i}and

*R*

_{j}is less than \(\varDelta (v_{i}^{\max})\), the speed of

*R*

_{j}is taken to be slightly slower than the speed of

*R*

_{i}. In planning

*R*

_{i}does not consider

*R*

_{j}and travels as shown in Fig. 12. Figure 12 also shows the travel plan of

*R*

_{j}. No collisions happen since

*R*

_{i}is travelling at a lower speed than

*R*

_{j}. The general algorithm is given in Algorithm 7.

### 4.4 Wait for vehicle

The travel plan of multiple vehicles may not always involve all vehicles moving slowly or quickly in the road segment. It may in fact require a vehicle to *wait* for some time. As an example consider the situation of a narrow bridge. Since vehicles have variable width, the number of vehicles that may pass the bridge at same time without collisions is variable. Consider the case when a large vehicle *R*_{j} is already on the bridge, making it impossible for another vehicle *R*_{i} of decent width to fit if coming from the other side of the road. In such a case *R*_{i} needs to stop at a location before the bridge and wait for *R*_{j} to leave the bridge. The strategy is inspired by a general driving scenario in English traffic, wherein parked vehicles (obstacles) on small roads reduce the number of lanes from 2 to 1, requiring inbound and outbound vehicle drivers to coordinate between themselves deciding who goes first.

This stresses the need to make a vehicle wait at some point for some duration. It is computationally impossible to compute the time a vehicle must wait at every location by trying all possibilities in the search task. We make a simple heuristic that a vehicle must always wait for a vehicle to pass by. Using this heuristic the task of waiting may be broken down into steps: deciding the vehicle to wait for, adjusting the position for the other vehicle to smoothly pass by, modifying other vehicles’ trajectory to account for the vehicle waiting, and computing the state after wait which becomes the expanded state as per this expansion strategy.

The general expansion algorithm expects the expansion strategy to expand the state *L*_{i} towards *P*_{a}. In this approach we do not place any restriction on the selection of the vehicle and all vehicles *R*_{j} that are tentative to pass through *P*_{a} at a later stage in their travel plan are worked for. Consider that *R*_{i} is waiting for *R*_{j} to pass through. Consider the set *H* containing all vehicles *R*_{k} (including *R*_{j}) that pass through *P*_{a} from the other direction, earlier than *R*_{j}. The time when a vehicle *R*_{k} passes *Z* can be known by iterating through its travel plan.

#### 4.4.1 Calculating preferred positions

We know a stream of vehicles *H* would cross *P*_{a}, after which *R*_{i} may continue its journey. The next major issue is adjusting the position of *R*_{i}. Here we need to decide whether *R*_{i} would wait left or right of the stream of incoming vehicles. Similar situations in everyday life have vehicles waiting towards the left (countries having the left-side driving rule). However in certain cases of road and obstacles, there may be a need for vehicles to wait on the other side of the road, making the entire travel plan optimal. Assuming *R*_{i} would be able to maintain a distance of *sm*, for every *R*_{k} in *H*, we compute the ideal positions of *R*_{i} along the *Y* axis.

*inside bridge line*)

*Z*to lie in the region which was narrow and hence could not accommodate

*R*

_{i}. We further assume the vehicle placement line (here also called the

*vehicle waiting line*)

*Z*

_{2}to be lying in the region which is wide enough for

*R*

_{i}to wait while the other vehicles in

*H*pass by. Let

*l*

^{k}be the position of

*R*

_{k}when it crosses the line

*Z*as per its plan. We assume in the modified plan of any

*R*

_{k}in

*H*,

*R*

_{k}would like to stay in the same lateral position till it crosses the waiting

*R*

_{i}. This means that any interesting behaviour of

*R*

_{k}would be suspended till it crosses

*R*

_{i}and it would need to travel parallel to the road while overcoming the waiting

*R*

_{i}. Hence while

*R*

_{k}is crossing the line

*Z*

_{2}at a point \(l^{k}_{2}\) it is to be found at the lateral position

*l*

^{k}[

*Y*]. The preferred position of

*R*

_{i}considering only

*R*

_{k}is given by Eqs. (18) and (19). The values denote the distance measured from the road boundary along

*Z*

_{2}. Here

*rl*is the road width at

*Z*

_{2}.

*R*

_{i}attempts to wait on the left or right side of the stream, it needs to leave enough space for each vehicle in the stream to pass through, and hence the preferred points on both sides may be given by min(

*right*

_{k}) and max(

*left*

_{k}) ∀

*R*

_{k}∈

*H*, out of which

*R*

_{i}chooses the one which requires least steering (Eq. (20)).

*sm*. Local optimization may be performed to induce feasibility, which gives the state to be expanded

*l*

_{2}. The state

*l*

_{2}however does not guarantee a distance of less than

*Δ*(

*v*

_{i}) from the obstacle ahead. It means further expansion of

*l*

_{2}may require reducing the vehicle’s speed.

#### 4.4.2 Generating trajectories

The next task is modifying the trajectories of each vehicle in *H* to account for the waiting *R*_{i}. Consider the segment of road along the *X* axis *S*=(*x*_{i}+*Δ*(*v*_{i})−*len*_{i}/2−*Δ*(*v*_{k}),*x*_{i}+*Δ*(*v*_{i})+*len*_{i}/2+2*Δ*(*v*_{k})). This is the region, at a distance of *Δ*(*v*_{k}) on either side of \(l^{i}_{2}\), which is affected by *R*_{i} waiting at \(l^{i}_{2}\) including the length of the waiting vehicle. In other words we consider *R*_{i} as an obstacle waiting at \(l^{i}_{2}\) and try to avoid *R*_{k} at a distance of *Δ*(*v*_{k}). We keep extra space to correct the position of *R*_{k} before surpassing *R*_{i} at \(l^{i}_{2}\) and further space after it has crossed waiting *R*_{i} to avoid collisions due to the immediate correction of position. Any lane of *R*_{k} that lies within this region is modified to account for the waiting *R*_{i}. As per the calculations of \(l^{i}_{2}\), we expect *R*_{k} to have the same lateral position that it had when it crossed the line *Z*. Let *R*_{k} enter the region *S* at *lane*_{j}[*a*] and leave at *lane*_{j}[*b*]. The manner of changing the lane and then the trajectory is similar to free state expansion.

The order of planning of vehicles in *H* specifically takes planning of the vehicle being waited for *R*_{j} ahead of the other vehicles. This is because this planning gives us the time (*tw*) when *R*_{j} crosses its *lane*_{j}[*b*] and hence *R*_{i} can continue its journey from this time onwards. Other vehicles in *H* consider *R*_{i} to be waiting only till the time *tw*. Hence any of their lanes are only modified if the vehicle crosses the position before this time.

The next task is generating a trajectory for *R*_{i} from *L*_{i} to \(l^{i}_{2}\). The intention behind first generating trajectories of the vehicles in *H* and later *R*_{i} is that as we keep generating trajectories, they keep getting added to the travel plan \(\zeta_{\mathit{dynamic}}^{\mathit{free}}(\tau_{2})\) which the vehicles being panned later must avoid. Local optimizations are performed on all vehicles in *H* and *R*_{i}. By this scheme we make clear that the path of vehicles in *H* must be as unchanged as possible, with *R*_{i} compromising as much as possible in terms of separation availability. Local optimizations in trajectory generation of *R*_{i} ensures that any possibility of feasible curve generation of *R*_{i} returns a trajectory, in case there is no sufficient space. Computing the clothoid from *L*_{i} and \(l^{i}_{2}\) and adding it into the expanded state of *L*_{i} is similar to free state expansion. The major difference is that the time a vehicle reaches \(l^{i}_{2}\) is taken to be *tw* and not as computed by the general algorithm.

*R*

_{i}discovers the sudden change in the width of the road, which initiates this expansion technique as per the heuristic (Sect. 4.5). The region

*S*for

*R*

_{k}is shown in the same figure. Figure 13(b) meanwhile shows the modification of the lane of

*R*

_{k}. Since a large amount of space was available and the vehicle was travelling in a straight line, no change was recorded. The trajectories of

*R*

_{i}and

*R*

_{k}are shown in Fig. 13(c). The final waiting point for

*R*

_{i}becomes the initial state for further expansions. Note that in this figure the direction of motion of the vehicle is in the opposite direction as compared to the earlier figures. The general structure of the algorithm is similar to the free state expansion and is given as Algorithm 8.

### 4.5 Expansion strategy

An easy method of implementation would be to use all three expansion strategies in all the states of planning. However this results in a significantly large computational time. In order to reduce the complexity we decided to use heuristics to prune the expansions. Along with a natural driving analogy as well as general modelling of the planning scenario this helps us to mine out good scenarios. We use a heuristic rule common for the free state and vehicle following expansion, based on which either or both techniques may be used; and one for the wait for vehicle technique.

*H*) that have been encountered in the expansion of state

*L*

_{i}. The vehicles that were encountered in a similar region when the state

*L*

_{i}was the expanded state are known, say

*L*

_{i}[

*H*]. If the two sets

*L*

_{i}[

*H*] and

*L*

_{i}[

*H*] match, it means that planning is encountering the same vehicles again. However in case the two sets do not match, it means that a new vehicle has been encountered or

*R*

_{i}has completely passed

*R*

_{j}. Now consider a road segment. By natural driving we know that once a vehicle

*R*

_{i}decides to overtake another vehicle

*R*

_{j}, it continues to do the same till overtaking is successful. Similarly if

*R*

_{i}decides to follow

*R*

_{j}, it continues to do so, till some vehicle

*R*

_{k}leaves, which was not giving space for

*R*

_{i}to overtake

*R*

_{j}. We make three observations here. First the decision whether to overtake a vehicle or to follow it is made on its first emergence. Second, once the decision has been made, it is unaltered as the vehicles go forward. Third, a change of decision is always marked by a vehicle entering or leaving. Vehicles entering and leaving in our algorithm are marked by a difference in the two sets. We know the strategy, out of the two strategies that resulted in the expansion of state

*L*

_{i}, say

*strategy*(

*L*

_{i}). A small assumption here is that every

*R*

_{k}is recorded at the critical region set. Alarmingly high differences in speed may make

*R*

_{k}unnoticed anytime in the critical region. This assumption can be improved by increasing the size of the critical region. The selected strategy of expansion is given in Algorithm 9.

Similarly we place a heuristic rule for deciding whether the expansion by wait for vehicle technique needs to take place or not. Consider the obstacle analysis line *Z*. Let the length of obstacle-free line in *P*_{a} of state *L*_{i} be *width*(*P*_{a}). Let *width*(*L*_{i}) be the length of the obstacle free line when *L*_{i} was produced. The technique of wait for vehicle expansion is only used if there is a significant (and sudden) decrease in the width. Hence a drop in values between *width*(*L*_{i}) and *width*(*P*_{a}) triggers this expansion technique. This is given in Algorithm 9.

## 5 Results

The complete system discussed was experimented with for a variety of scenarios. We first deal with understanding the vehicle dynamics in the presence of multiple vehicles. Our focus is on the three strategies presented, and hence the map taken does not have any obstacle. We then deal with understanding the vehicle dynamics within an obstacle network. For this part it is important to ensure that the vehicles do not collide or lie close to obstacles while displaying any behaviour.

In our general experimental set up it is assumed that all vehicles have an initial travel plan however this is iteratively modified dependent on the travel plans of the other vehicles. The overall aim is for time and route optimality subject to complete cooperation between all of the vehicles involved. This means that all vehicles effectively become aware of the route requests of all other vehicles. A variety of road conditions are looked at in these experiments. In order to achieve this, lanes are not a fixed physical entity, as at present, but rather are adaptable to the vehicle flow requirements. Far from such an infrastructure being restrictive in a future practical scenario it is felt to be a very sensible method to take whereby a human who wishes to be transported merely needs to be clear on their start and end points along with the sort of time they wish to take and any specific route request they may have. The overall system will then deal with the rest, as is experimented on here.

### 5.1 Simple road experiments

The other behaviour was a vehicle following scenario which is also studied by similar experiments. Here again three vehicles are used. The oncoming vehicle allowed no possibility for the faster vehicle to overtake the slower vehicle. However, after the vehicle crossed, overtaking was possible. Figure 14(b) shows the situation where the oncoming vehicle crosses the two vehicles. Note that both vehicles drifted leftwards to ensure that no collision was recorded as well as a maximum separation being available. Soon after this vehicle crossed, the faster vehicle decided to overtake the slower vehicle. The intent to overtake is shown in Fig. 14(c).

The different vehicles may need different lanes. Though we intent to align them in a way such that after steering and attaining their orientations they distribute the available road width amongst themselves, it is important to see whether the distribution takes place neatly. The next scenario presented three vehicles, two originating from one side of the road while the third from the other. The speeds were arranged and emergence timed so that the three met at some point in between. The result is shown in Fig. 14(d). It can be seen that the separation between any two vehicles and road ends is kept large enough.

The last experiment was for the wait for vehicle strategy which requires build-up of a narrow bridge in the middle of the road. Here we chose three vehicles, two coming from one side and the other coming from the other side. The scenario and the results are shown in Figs. 14(e) to 14(g). The first vehicle emerges and moves almost straight to cross the bridge. The second vehicle pre-computes that it needs to wait to avoid collision, it moves forward to take its waiting position as shown in Fig. 14(e). Even before the first vehicle crosses, the third vehicle emerges into the planning scenario. It computes that it cannot cross the bridge before the first vehicle or just after the first vehicle, avoiding a collision with the second vehicle. So it also decides to wait for the second vehicle to cross the bridge by taking its position as shown in Fig. 14(f). After the first vehicle has crossed, the second vehicle starts and similarly after the second vehicle has crossed, the third vehicle starts. This is shown in Fig. 14(g).

### 5.2 Experiments of multi-vehicles in an obstacle network

In the experiments of Sect. 5.1 we discussed vehicle behaviours in the absence of obstacles. However it may be essential for overtaking behaviour to complete within some obstacle sub-network. If a collision is encountered, the plan may alternatively choose vehicle following behaviour which is collision free. The scenario was set up with a barrier dividing a curved road. The vehicles were generated from the same side of the road. Scenario and results are shown in Figs. 15(b) and 15(c). The first vehicle took the narrower segment and the second the broader one. The third vehicle had a higher speed and hence decided to overtake the slower vehicle in the broader segment. The intent to overtake is shown in Fig. 15(b). Soon another vehicle was added in the scenario. Even though it had a high speed, it could not construct any overtaking strategy that completed within the segment with all the pre-planned behaviours of the other vehicles intact. So it decided to follow the front vehicle, while the earlier vehicle had almost overtaken. The situation is given in Fig. 15(c).

The last scenario we take is to see if multiple behaviours can be simultaneously displayed. A curved road was taken with two vehicles of varying widths originated at different times from either side of the road. The scenario is displayed in Figs. 15(d) to 15(f). On the first part of the road the vehicle generated later was of high speed and low size and was generated when the two vehicles were reasonably close by. The intent to overtake is shown in Fig. 15(d). The vehicle is completely overtaken as shown in Fig. 15(e). However on the other side of the road the two vehicles were separated by a large margin and the difference in speeds was kept low. As a result the second vehicle followed the first vehicle at high speed for a while as shown in Fig. 15(e). Later it attempted to overtake it as displayed in Fig. 15(f).

## 6 Discussion

Giving effective results as early as possible is one of the prime requirements of any planning algorithm. It is hence important to analyze the complexity of the algorithm, based on which the road segment length considered for planning may be decided. Too large a length may make the algorithm unable to give results within time, and too small a length would make planning ineffective, wherein different vehicle behaviours cannot be displayed and the vehicle can possibly be trapped in an obstacle network.

For *n* static obstacles on the road, going left or right of the obstacle may result in two different plans, indicating the total number of plans possible in the worst case to be 2^{n} which the graph search explores. Consider a number of states produced by the graph search. A state may be called connecting state if it has one child or no children. These states do not multiply the search space or do not lead to new paths. Every obstacle approximately adds 1 non-connecting state corresponding to the state when it was first discovered in all possible paths. Hence for *n* obstacles, total paths in the worst case are 2^{n} each approximately of length *l*, which makes the complexity *O*(2^{n}*l*). The effect of factor \(\varDelta (v^{i}_{\max})\) and connecting states on complexity is together communicated by the factor *l*. If some of these obstacles are small obstacles, there would be additional cost due to the reduction in speed of a vehicle which is done in steps.

The algorithm may also have multiple vehicles. Let *a* be the number of vehicles travelling in the same direction (inbound or outbound) as the vehicle being planned and *b* the number of vehicles travelling in the opposite direction. For a vehicles going in the same direction, it may either follow it or overtake it. Both possibilities need to be tried. The total possibilities to travel are 2^{a} in the worst case. As per heuristics both possibilities were checked on encountering any vehicle. The possibilities are tried both when the vehicle arrives as well as when it departs. This may sometimes lead to a multiplicity of cases. The maximum number of possibilities in the worst case hence are 2^{a}(*a*+2*b*). In addition a vehicle may need to wait for another vehicle. Say the scenario has only one narrow bridge. At most the vehicle has to wait for all *b* vehicles coming, which means an additional *b* possibilities. However these possibilities are only tried at one particular state in the road when the width changes dramatically. Hence the addition of complexity is non-exponential. The complexity is hence *O*(2^{a}(*a*+2*b*)), which shows it is exponential for the number of vehicles in the same direction and obstacles. This number is usually very small considering the small road segment taken, or the vehicles would be simply following each other which is not accounted.

*O*(2

^{n+a}(

*a*+2

*b*)

*l*), which means it is exponential in both

*n*and

*a*. We experimented both factors of obstacles and vehicles by a more realistic experimental scenario. First we took a straight road map and experimented on the effect of the addition of obstacles. The intent was to expose the worst case that the algorithm may face, and hence all the obstacles were added one after the other. The road was significantly elongated so that we could fit in lots of obstacles. The effect of the increase in the number of connecting states

*s*and non-connecting states

*n*is shown in Fig. 16. Since the vehicle was intentionally kept too small to allow avoiding all obstacles to be possible; many times after aligning to overcome an obstacle, in the next expansion stage it could further go to the other side of the obstacle, making possibilities per obstacles 3 rather than 2.

*a*vehicles in front are arranged in increasing order of speeds with large gaps between them. The scenario was difficult to create even after prolonged increase in the road width. Clearly though theoretically possible, the situation is not practical. The practical worst case is to have a series of vehicles going at equal speeds separated by large gaps. For every vehicle, the vehicle being planned may decide to overtake, or to follow. If it overtakes, the choices repeat with the other vehicle in front. If it follows, no further choice is available. Hence the total number of cases possible are

*a*+1, where

*a*is the number of vehicles in front. Clearly the practical worst case is linear. The number of connecting and non-connecting states for an increasing number of vehicles is shown in Fig. 17. The connecting states are not linear, as the section in which the vehicle being overtaken is recorded, and the road length remaining makes the difference.

The other factor is the longitudinal separation constant. This factor was kept as per the present notion that a vehicle must be able to steer significant distance in the *Y* axis. This puts a lower threshold on the value of the factor. Increasing this factor decreases steering and increases smoothness. However on increasing the factor, a small obstacle problem becomes harder to detect and rectify, close overtaking cannot be performed, and vehicles start aligning early on seeing an obstacle or some other vehicle. All these factors affect optimality. Too small a value however leads to too sharp turns and large computational time.

From the software point of view one of the greatest tasks ahead is to change the data structure in which paths and lanes are represented internally. The data structures must cater for fast query response. Heuristics can be added in the expansion for computational speedup. Post processing techniques may be added to optimize the trajectories as a whole and remove any oscillations visible in overtaking, lane changes, and while travelling on a curved road. Displaying vehicle behaviour at traffic merging, diversions and crossing scenarios is still to be done. The simulations can then be extended to an entire transportation system. The simulations ultimately need to be validated on real vehicles and real roads in order to prove their effectiveness.

## 7 Conclusions

Having large diversities in traffic, be it in terms of vehicle size, speed capabilities or road infrastructure, necessitates sophisticated planning techniques for the motion of vehicles. In this paper we considered the traffic scenario, and the planning, of multiple autonomous vehicles. The planning performed was based on a prioritized Uniform Cost Search, affecting re-planning of vehicles of higher priority which ensured cooperation. The algorithm could try all possibilities and come up with an optimal plan. Experiments over complex grids of obstacles and other vehicles showcased the effectiveness of the algorithm. Further vehicle behaviours of overtaking, vehicle following, and waiting for vehicle ensured optimal performance for a variety of scenarios.

A major question that arises out of the research is the validity of the present traffic rules. In an autonomous scenario, should the vehicles be allowed to collaboratively construct optimal plans which do not adhere to static traffic rules, which was the case in this approach? This leads to possibilities of more driving behaviours. The algorithm needs to ensure completeness and absence of deadlocks. Pedestrians and humans are desirable as obstacles, since they intelligently clear up with time, causing no deadlocks with autonomous vehicles. In the case of autonomous vehicles only, deadlocks need to be handled algorithmically. We have taken a positive step towards identifying all possible scenarios to ensure an absence of deadlocks, as depicted by the experimental results.

One issue with this research is that it is looking ahead to a scenario in which vehicles are fully autonomous and communicate both with each other and a vehicle network. As a result many advantages arise. In order to get from the present situation with human drivers to the fully autonomous version it is recognised that a transitionary state will apply. It may well be the case therefore that some of the functions considered here are not immediately available, e.g. methods of overtaking, however it is not necessarily the case that autonomous vehicles will have to follow the same rules as human driven cars as different overall rules may well be apparent in the transitionary state—at this stage we simply cannot tell.

## Notes

### Acknowledgements

The authors wish to thank the Commonwealth Scholarship Commission in the United Kingdom and the British Council for their support of the first named author through the Commonwealth Scholarship and Fellowship Program (2010)—UK through award number INCS-2010-161.

### References

- 1.Alvarez-Sanchez J, de la Paz Lopez F, Troncoso J, de Santos Sierra D (2010) Reactive navigation in real environments using partial center of area method. Robot Auton Syst 58(12):1231–1237 CrossRefGoogle Scholar
- 2.Arai T, Ota J (1992) Motion planning of multiple mobile robots. In: Proceedings of the 1992 IEEE/RSJ international conference on intelligent robots and systems, Raleigh, North Carolina, USA, pp 1761–1768 CrossRefGoogle Scholar
- 3.Bennewitz M, Burgard W, Thrun S (2002) Finding and optimizing solvable priority schemes for decoupled path planning techniques for teams of mobile robots. Robot Auton Syst 41(2–3):89–99 CrossRefGoogle Scholar
- 4.Bishop R (2000) A survey of intelligent vehicle applications worldwide. In: Proceedings of the IEEE intelligent vehicles symposium, 2000. IV 2000, Oct 2000, pp 25–30 Google Scholar
- 5.Burchan Bayazit O, Lien J-M, Amato N (2002) Probabilistic roadmap motion planning for deformable objects. In: Proceedings of the IEEE international conference on robotics and automation. ICRA’02, vol 2, pp 2126–2133 Google Scholar
- 6.Chand P, Carnegie D (2012) A two-tiered global path planning strategy for limited memory mobile robots. Robot Auton Syst 60(3):309–321 CrossRefGoogle Scholar
- 7.Chang H-J, Kwak H-J, Park G-T (2009) Efficient dissemination method for traffic jams information sharing based on inter-vehicle communication. In: 2009 IEEE student conference on research and development (SCOReD), 16–18 Nov 2009, pp 61–64 CrossRefGoogle Scholar
- 8.Chu K, Lee M, Sunwoo M (2012) Local path planning for off-road autonomous driving with avoidance of static obstacles. IEEE Trans Intell Transp Syst 13(4):1599–1616 CrossRefGoogle Scholar
- 9.Cowlagi R, Tsiotras P (2012) Hierarchical motion planning with dynamical feasibility guarantees for mobile robotic vehicles. IEEE Trans Robot 28(2):379–395 CrossRefGoogle Scholar
- 10.Furda A, Vlacic L (2011) Enabling safe autonomous driving in real-world city traffic using multiple criteria decision making. IEEE Intell Transp Syst Mag 3(1):4–17 CrossRefGoogle Scholar
- 11.Gehrig S, Stein F (2007) Collision avoidance for vehicle-following systems. IEEE Trans Intell Transp Syst 8(2):233–244 CrossRefGoogle Scholar
- 12.Kala R, Warwick K (2012) Multi-vehicle planning using RRT-connect. Paladyn 2(3):134–144 CrossRefGoogle Scholar
- 13.Kala R, Warwick K (2013) Motion planning of autonomous vehicles in a non-autonomous vehicle environment without speed lanes. Eng Appl Artif Intell 26(5–6):1588–1601 CrossRefGoogle Scholar
- 14.Kala R, Warwick K (2013) Multi-level planning for semi-autonomous vehicles in traffic scenarios based on separation maximization. J Intell Robot Syst. doi:10.1007/s10846-013-9817-7 MATHGoogle Scholar
- 15.Kala R, Warwick K (2013) Planning autonomous vehicles in the absence of speed lanes using an elastic strip. IEEE Trans Intell Transp Syst. doi:10.1109/TITS.2013.2266355 MATHGoogle Scholar
- 16.Kala R, Shukla A, Tiwari R (2011) Robotic path planning in static environment using hierarchical multi-neuron heuristic search and probability based fitness. Neurocomputing 74(14–15):2314–2335 CrossRefGoogle Scholar
- 17.Kuwata Y, Karaman S, Teo J, Frazzoli E, How J, Fiore G (2009) Real-time motion planning with applications to autonomous urban driving. IEEE Trans Control Syst Technol 17(5):1105–1118 CrossRefGoogle Scholar
- 18.Leroy S, Laumond J, Siméon T (1999) Multiple path coordination for mobile robots: a geometric algorithm. In: Proceedings of the 16th international joint conf artificial intelligence, Stockholm, Sweden, pp 1118–1123 Google Scholar
- 19.Lu Y, Huo X, Arslan O, Tsiotras P (2011) Incremental multi-scale search algorithm for dynamic path planning with low worst-case complexity. IEEE Trans Syst Man Cybern, Part B, Cybern 41(6):1556–1570 CrossRefGoogle Scholar
- 20.Lumelsky V, Harinarayan K (1997) Decentralized motion planning for multiple mobile robots: the cocktail party model. Auton Robots 4(1):121–135 CrossRefGoogle Scholar
- 21.Ma Y, Chowdhury M, Sadek A, Jeihani M (2009) Real-time highway traffic condition assessment framework using vehicle–infrastructure integration (VII) with artificial intelligence (AI). IEEE Trans Intell Transp Syst 10(4):615–627 CrossRefGoogle Scholar
- 22.McCrae J, Singh K (2009) Sketching piecewise clothoid curves. Comput Graph 33(4):452–461 CrossRefGoogle Scholar
- 23.Mohan D, Bawa P (1985) An analysis of road traffic fatalities in Delhi, India. Accid Anal Prev 17(1):33–45 CrossRefGoogle Scholar
- 24.Nilsson N (1971) Problem solving methods in artificial intelligence. McGraw-Hill, New York Google Scholar
- 25.Nutbourne A, McLellan P, Kensit R (1972) Curvature profiles for plane curves. Comput Aided Des 4(4):176–184 CrossRefGoogle Scholar
- 26.Paruchuri P, Pullalarevu A, Karlapalem K (2002) Multi agent simulation of unorganized traffic. In: Proc ACM 1st intl jt conf autonomous agents and multiagent systems: part 1, New York, pp 176–183 Google Scholar
- 27.Peng J, Akella S (2003) Coordinating the motions of multiple robots with kinodynamic constraints. In: Proc IEEE international conference on robotics & automation, Taipei, Taiwan, pp 4066–4073 Google Scholar
- 28.Peng J, Akella S (2005) Coordinating multiple robots with kinodynamic constraints along specified paths. Int J Robot Res 24(4):295–310 CrossRefGoogle Scholar
- 29.Rajamani R, Tan H-S, Law BK, Zhang W-B (2000) Demonstration of integrated longitudinal and lateral control for the operation of automated vehicles in platoons. IEEE Trans Control Syst Technol 8(4):659–708 CrossRefGoogle Scholar
- 30.Reichardt D, Miglietta M, Moretti L, Morsink P, Schulz W (2002) CarTALK 2000: safe and comfortable driving based upon inter-vehicle-communication. In: IEEE intelligent vehicle symposium 2002, vol 2, pp 545–550. pp 17–21 CrossRefGoogle Scholar
- 31.Reveliotis SA, Roszkowska E (2011) Conflict resolution in free-ranging multivehicle systems: a resource allocation paradigm. IEEE Trans Robot 27(2):283–296 CrossRefGoogle Scholar
- 32.Russell S, Norvig P (2010) Artificial intelligence: a modern approach. Prentice Hall, New York Google Scholar
- 33.Sánchez-Ante G, Latombe J (2002) Using a PRM planner to compare centralized and decoupled planning for multi-robot systems. In: Proceedings of the IEEE international conference on robotics and automation, Washington, DC, pp 2112–2119 Google Scholar
- 34.Schubert R (2012) Evaluating the utility of driving: toward automated decision making under uncertainty. IEEE Trans Intell Transp Syst 13(1):354–364 CrossRefGoogle Scholar
- 35.Schubert R, Schulze K, Wanielik G (2010) Situation assessment for automatic lane-change manoeuvers. IEEE Trans Intell Transp Syst 11(3):607–616 CrossRefGoogle Scholar
- 36.Svestka P, Overmars M (1995) Coordinated motion planning for multiple car-like robots using probabilistic roadmaps. In: Proceedings of the 1995 IEEE international conference on robotics and automation, Nagoya, Japan, vol 2, pp 1631–1636 CrossRefGoogle Scholar
- 37.Tsugawa S (2002) Inter-vehicle communications and their applications to intelligent vehicles: an overview. In: IEEE intelligent vehicles symposium, 17–21 June 2002, vol 2, pp 564–569 Google Scholar
- 38.Valdes F, Iglesias R, Espinosa F, Rodriguez M (2012) An efficient algorithm for optimal routing applied to convoy merging manoeuvres in urban environments. Appl Intell 37(2):267–279 CrossRefGoogle Scholar
- 39.Vanajakshi L, Subramanian S, Sivanandan R (2009) Travel time prediction under heterogeneous traffic conditions using global positioning system data from buses. IET Intell Transp Syst 3(1):1–9 CrossRefGoogle Scholar
- 40.Varaiya P (1993) Smart cars on smart roads: problems of control. IEEE Trans Autom Control 38(2):195–207 CrossRefMathSciNetGoogle Scholar
- 41.Viet H, Dang V-H, Laskar M, Chung T-C (2013) BA
^{∗}: an online complete coverage algorithm for cleaning robots. Appl Intell 39(2):217–235 CrossRefGoogle Scholar - 42.Weber J, Wotawa F (2012) Diagnosis and repair of dependent failures in the control system of a mobile autonomous robot. Appl Intell 36(3):511–528 CrossRefGoogle Scholar
- 43.Zheng Y-J, Chen S-Y (2013) Cooperative particle swarm optimization for multiobjective transportation planning. Appl Intell 39(1):202–216 CrossRefGoogle Scholar