Orit Katz Seminar in CS (Robotics) 1

Orit Katz Seminar in CS (Robotics) 2 Statement of The Problem Given a set of vehicles moving in a dynamic world, we want to coordinate the motion of a subset of these vehicles. Given a set of vehicles moving in a dynamic world, we want to coordinate the motion of a subset of these vehicles. Each vehicle has its own goal and this goal is to be achieved according to: Each vehicle has its own goal and this goal is to be achieved according to: 1.optimality 2.Safety

Orit Katz Seminar in CS (Robotics) 3 Characteristics of The Problem 1. Vehicles A vehicle is an entity with motion capability. The goal of a vehicle is to reach a specific location in the world.

Orit Katz Seminar in CS (Robotics) 4 Characteristics of The Problem 2. The world It is cluttered with static but also dynamic obstacles. The static obstacles constitute the limits of the actual evolution space of the vehicles. The dynamic obstacles consist mainly of the vehicles.

Orit Katz Seminar in CS (Robotics) 5 Characteristics of The Problem - Cont. We can refer this problem in a multi-agent perspective. In this framework we have to deal with the following points: Plan recognition Plan recognition levels of interaction between the coordinated vehicles. levels of interaction between the coordinated vehicles. Generation of a plan in a multi-agent world. Generation of a plan in a multi-agent world. Execution of a plan in a dynamic and uncertain world. Execution of a plan in a dynamic and uncertain world.

Orit Katz Seminar in CS (Robotics) 6 1.“Independent” approach: The coordinated vehicle is able to solve its motion problem on its own. 2.“Controlled” approach: The coordinated vehicle has no capabilities of its own. 3.“Interdependent” approach: The coordinated vehicle has various capabilities but it is not able to solve its motion problem on its own. Possible Approaches one vehicle = one agent set of vehicles = one agent There is an external supervisor

Orit Katz Seminar in CS (Robotics) 7 The Framework The purpose of this application is to develop software tools in order to guide car-like vehicles moving on a particular subset of the road network - intersections.The purpose of this application is to develop software tools in order to guide car-like vehicles moving on a particular subset of the road network - intersections. Two types of vehicles have to be considered:Two types of vehicles have to be considered: A Specific Case

Orit Katz Seminar in CS (Robotics) 8 1.Those whose motion guided by the system. 2.The other vehicles. The assumption on the existence of a behavior code is instantiated here by the fact that the vehicles are supposed to respect the highway code. The assumption on the existence of a behavior code is instantiated here by the fact that the vehicles are supposed to respect the highway code. A Specific Case - Cont.

Orit Katz Seminar in CS (Robotics) 9 Indeed, the roadway turns out to be an organized set of lanes. Besides, intersections are designed in order to ease the traffic; thus a vehicle has to follow a particular lane and accordingly is not supposed to overtake. A Specific Case - Cont.

Orit Katz Seminar in CS (Robotics) 10 Choosing an approach “Controlled” approach “Controlled” approach The complexity of the problem prevents the use of a controlled approach. We are considering many vehicles moving in a dynamic and uncertain world. A Specific Case - Cont. System with high number of degrees of freedom ! 

Orit Katz Seminar in CS (Robotics) 11 “Independent” approach Each vehicle must reach its goal as quickly as possible. “Interdependent” approach Each coordinated vehicle is concerned with the realization of a predetermined reference trajectory. A Specific Case - Cont.  optimality isn’t saved! Optimality is saved !

Orit Katz Seminar in CS (Robotics) 12 The Intersection Manager The intersection manager A centralized system which is in charge of the coordinated vehicles within a specific intersection: A centralized system which is in charge of the coordinated vehicles within a specific intersection: - provides each coordinated vehicle with a reference trajectory. -deals with: 1. Plan recognition. 2. Plan generation in a dynamic world.

Orit Katz Seminar in CS (Robotics) 13 The Intersection Manager – Cont. The intersection manager has to provide each coordinated vehicle with a trajectory.has to provide each coordinated vehicle with a trajectory. operates on a planning horizon.operates on a planning horizon. makes use of a geometric description of the intersection and an estimation of the position/velocity parameters of the vehicles located at the crossroad area, at the beginning of the horizon.makes use of a geometric description of the intersection and an estimation of the position/velocity parameters of the vehicles located at the crossroad area, at the beginning of the horizon.

Orit Katz Seminar in CS (Robotics) 14 The Intersection Manager - Cont. Prioritization Approach Each coordinated vehicle is assigned a priority and the trajectories are planned in the priority order. This method consists first of planning a path avoiding the static obstacles and then of adjusting the velocity so as to avoid collision or give way to other vehicles.

Orit Katz Seminar in CS (Robotics) 15 The Intersection Manager - Cont. The intersection manager is composed of two main modules: A geometric plannerA geometric planner A temporal plannerA temporal planner

Orit Katz Seminar in CS (Robotics) 16 A Coordinated Vehicle A coordinated vehicle is periodically provided with a plan including both geometric and time constraints. The trajectory defines a valley of potential that the vehicle is supposed to track and the detected obstacles generate repulsive potential fields.

Orit Katz Seminar in CS (Robotics) 17 The World Model The EnvironmentThe Environment we distinguish between four types of regions: 1.Lanes 2.Crossing regions 3.Entry region 4.Exit region

Orit Katz Seminar in CS (Robotics) 18 The World Model - Cont.

Orit Katz Seminar in CS (Robotics) 19 The World Model - Cont.

Orit Katz Seminar in CS (Robotics) 20 The World Model - Cont. Our system deals with subsets of the road network. In order to represent the connectivity of the road network and the associated motion constraints, the regions obtained are structured using graph G.

Orit Katz Seminar in CS (Robotics) 21 The World Model - Cont. - a region. - a region. - it is possible to move from region r i to region r j. riri riri rjrj

Orit Katz Seminar in CS (Robotics) 22 The World Model - Cont.

Orit Katz Seminar in CS (Robotics) 23 The World Model - Cont. The VehiclesThe Vehicles Geometrically, they are represented by rectangles. Such a vehicle has two rear wheels and two directional front wheels.

Orit Katz Seminar in CS (Robotics) 24 The World Model - Cont.

Orit Katz Seminar in CS (Robotics) 25 The World Model - Cont. The three main constraints for such a vehicle are: 1.Dependence relation between its translational and its rotational degrees of freedom. 2.The curvature radius of its trajectory is lower bounded. 3.Its velocity is upper bounded.

Orit Katz Seminar in CS (Robotics) 26 The Geometric Planner The task of the geometric planner is to compute a geometric trajectory for each coordinated vehicle.

Orit Katz Seminar in CS (Robotics) 27 The Geometric Planner - Cont. Planning an executable trajectory is performed in two phases: 1.Generating a polygonal line which represents the “skeleton” of the trajectory searched. 2.“Smoothing” this polygonal line in order to obtain curved transitions which verifies the vehicle kinematic constraints.

Orit Katz Seminar in CS (Robotics) 28 The Geometric Planner - Cont. Determining the skeleton of the trajectory 1.Find a “path” In order to cross an intersection, a vehicle has to go through a specific sequence of regions. The sequence’s structure: entry region, ([lane], crossing region, [lane])*, exit region

Orit Katz Seminar in CS (Robotics) 29 The Geometric Planner - Cont. 2.Extract the skeleton in the current implementation we it is assumed that the lane axes are piecewise linear. It is then possible to build a polygonal line whose segments correspond to the axes of the regions crossed by the vehicle.

Orit Katz Seminar in CS (Robotics) 30 The Geometric Planner - Cont.

Orit Katz Seminar in CS (Robotics) 31 The Geometric Planner - Cont. Smoothing the skeleton This phase aims to introduce circular based transition in the skeleton in order to make the trajectory consistent with the kinematic constraints of the vehicle while avoiding the static obstacles.

Orit Katz Seminar in CS (Robotics) 32 The Geometric Planner - Cont.

Orit Katz Seminar in CS (Robotics) 33 The Temporal Planner The task of the temporal planner is to determine the velocity to apply to each coordinated vehicle in order to avoid collisions with the other vehicles. It operates periodically on the interval of time [0, horizon]. For each path determined by the geometric planner, it determines the entrance time to associate to the related regions.

Orit Katz Seminar in CS (Robotics) 34 Statement of the Problem Conflict Regions regions which cannot be simultaneously occupied by several vehicles. Since our world model ensures that each region is connected to a conflict region, it becomes possible to turn the velocity planning problem into an allocation problem, where the conflict regions are resources to be temporally allocated to the vehicles.

Orit Katz Seminar in CS (Robotics) 35 Statement of the Problem - Cont. V - set of vehicles present in the crossroad. V c - subset of the coordinated vehicles. OI (v,r) - occupation interval of the conflict region r by the vehicle v. The problem : find OI(v,r) for each vehicle V  Vc and for each conflict region r belonging to the path associated to v.

Orit Katz Seminar in CS (Robotics) 36 Statement of the Problem - Cont. 1.Monotonicity The velocity of a vehicle cannot be negative. 2.Maximum velocity A vehicle cannot go beyond a given velocity limit. 3.No collision  vi, vj  V, vi  vj, OI(vi,r)  OI(vj,r) =  4.No overtaking a vehicle cannot overtake when moving from one conflict region to the next.

Orit Katz Seminar in CS (Robotics) 37 Statement of the Problem - Cont. Non Coordinated Vehicles A non coordinated vehicle intending to cross a conflict region is subject to the same temporal constraints that coordinated vehicle is, therefor its kinematic behavior can be estimated by a conflict region allocation in the same way as for a coordinated vehicle. Consequently, the system has to estimate the goal of each non coordinated vehicle before applying the same planning process to all the vehicles.

Orit Katz Seminar in CS (Robotics) 38 Prioritization Approach This approach consists of allocating the conflict regions one vehicle at the time. There are two reasons that lead to this approach: 1.The planner efficiency. 2.It is worthless to try to generate a completely accurate temporal plan.

Orit Katz Seminar in CS (Robotics) 39 Prioritization Approach - Cont. The algorithm relies on a “natural” priority order: a vehicle v moving in a region r at time t=0, before processing a vehicle v’ having r in its path.

Orit Katz Seminar in CS (Robotics) 40 Prioritization Approach - Cont. The relation “BEFORE” This relation can generate cycles of the type “v before v’ “ and “v’ before v”.

Orit Katz Seminar in CS (Robotics) 41 Prioritization Approach - Cont.

Orit Katz Seminar in CS (Robotics) 42 Prioritization Approach - Cont. Prioritization Algorithm: 1.Build the graph associated to the relation BEFORE. 2.Turn the relation before in a partial ordering by moving an arbitrary edge in each cycle of this graph. 3.Topologically sort this new graph.

Orit Katz Seminar in CS (Robotics) 43 Prioritization Approach - Cont.

Orit Katz Seminar in CS (Robotics) 44 The Planning Algorithm Let v i be the i th vehicle in the prioritization order. The set of occupation intervals generated by the vehicles v 1, …, v i-1 on each conflict region is computed be the system before processing v i. Then the temporal allocation for v i is based on an analysis of intervals available associated to the conflict region belonging to the path of v i.

Orit Katz Seminar in CS (Robotics) 45 The Planning Algorithm - Cont. searching a solution for v i is achieved using a depth-first search (DFS) algorithm. This search considers the valid temporal intervals from the earliest to the latest and it terminates when it finds a valid temporal interval either associated to the last conflict region of the path o v i, or ending after horizon.

Orit Katz Seminar in CS (Robotics) 46 Experiments The Temporal Planning The world we consider is a basic intersection. The world contains both coordinated and non coordinated vehicles.

Orit Katz Seminar in CS (Robotics) 47 Experiments The first job of the intersection manager is to estimate the goal of each non coordinated vehicle. Therefor the intersection manager can determine their path.

Orit Katz Seminar in CS (Robotics) 48 Experiments The intersection manager is then able to apply the prioritization algorithm.

Orit Katz Seminar in CS (Robotics) 49 Experiments – Cont. The temporal planning process is then applied to every vehicle in the priority order.

Orit Katz Seminar in CS (Robotics) 50 Experiments – Cont. A temporal plan for a vehicle is a list of couples : (number of the region, entrance time in the region).

Orit Katz Seminar in CS (Robotics) 51 Experiments – Cont. For a non coordinated vehicle, this temporal plan represents an estimation of the vehicles behavior.

Orit Katz Seminar in CS (Robotics) 52 Conclusion A centralized “INTERSECTION MANAGER” periodically computes a reference trajectory for every coordinated vehicle.A centralized “INTERSECTION MANAGER” periodically computes a reference trajectory for every coordinated vehicle. The capabilities of such a vehicle include trajectory following and local obstacle avoidance.The capabilities of such a vehicle include trajectory following and local obstacle avoidance. The intersection manager deals with:The intersection manager deals with: 1.Reasoning the intentions of other vehicles. It makes use of the structure of the intersection both to estimate the behavior of the non coordinated vehicles and to plan the motion of the coordinated vehicles. 2.Motion planning in a dynamic and uncertain world.

Orit Katz Seminar in CS (Robotics) 53 Conclusion – Cont. The partial centralization enables to satisfy the optimality criterion efficiently.The partial centralization enables to satisfy the optimality criterion efficiently. The safety criterion is dealt with first by the intersection manager and then by each vehicle.The safety criterion is dealt with first by the intersection manager and then by each vehicle. In order to cope with the complexity of the problem, the intersection manager applies the principles of:In order to cope with the complexity of the problem, the intersection manager applies the principles of: 1.Prioritization. 2.Path / velocity decomposition.

Orit Katz Seminar in CS (Robotics) 54 Personal Thoughts