SECTION I

IN THE United States, vehicular collisions kill, on average, 116 and injure 7900 people per day [22]. In 2009, more than 33 800 people were killed in police-reported motor vehicle traffic crashes, and about 2.2 million people were injured [2], with an estimated economic cost of $230 billion. The situation in the European Union is similar, with about 43 000 deaths and 1.8 million people injured per year, for an estimated cost of ${{\hskip1pt}{\ssr C} {\hskip-7pt} \raise2.4pt \hbox{$\buildrel{\vrule depth0.5pt width5.5pt}\over{\vrule depth0.5pt width5pt}$}{\hskip2pt}}$160 billion [9]. In 2009, light vehicle crashes accounted for 68% of all motor vehicle fatalities in the United States, and, of those light vehicle fatalities, 26% were from side impacts [2], suggesting crashes at intersections or on roadways close to and leading to intersections. These statistics clearly indicate that crashes at intersections have a major impact on the total number of crashes and fatalities in the United States. Furthermore, unlike other high-percentage crashes, such as road departure and rear end, for which radar and camera-based forward collision systems are now available, there is currently no established technology to address side-impact collisions at intersections.

Vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communications are setting the basis for establishing this missing technology by having vehicles cooperate with each other and with the surrounding infrastructure, sharing information about the environment and improving overall situational awareness. Therefore, intelligent transportation systems for intervehicle cooperative (active) safety have been the subject of intense research worldwide in government and industry consortia, such as the Crash Avoidance Metrics Partnership and Vehicle Infrastructure Integration Consortium in the United States, the Car2Car Communications Consortium in Europe, and the Advanced Safety Vehicle project 3 in Japan.

Since cooperative active safety systems are life critical, ad hoc algorithms for preventing collisions are not acceptable. Instead, there is a compelling need for employing methodologies that provide formal safety guarantees, such as those found in the control theory and computer science literature [18], [24], [26]. Specifically, the collision avoidance problem can be addressed by computing the set of states, which are called the backward reachable set or the capture set, that lead to an unsafe configuration (a collision) independently of the input choice [26]. Then, a feedback map is computed that restricts the control inputs when necessary to prevent entrance in the capture set. While this approach is theoretically appealing because it ensures safety by construction and applies overrides only when necessary, its practical applicability is often limited by the complexity associated with the computation of the capture set [15], [27]. Researchers have been tackling computational issues by, among other approaches, focusing on restricted classes of systems [3], [11], [13], [14].

In this paper, we employ the techniques in [14], which lead to linear complexity algorithms that are implementable in real-time applications. Furthermore, the results in [14], as opposed to the others, guarantee safety in the presence of imperfect state information, due, for example, to sensor noise or communication delays, and only need a coarse model of the vehicle dynamics. We focus on a two-vehicle collision avoidance scenario at intersections and develop a decentralized control algorithm that uses V2V communication to determine whether automatic control is needed to prevent a collision. We prevent a collision through automatic control by only actuating the brake and throttle, but not steering, and assuming that drivers follow nominal paths as established by the driving lanes. In our intersection collision avoidance (ICA) application, the drivers retain full control of the vehicle until the system configuration hits the capture set. At this point, a control action is necessary to prevent a collision, and automatic throttle or brake is applied to both vehicles in a coordinated fashion so that one vehicle enters the intersection only after the other has exited. After the crash has been prevented, the driver regains control of the brake and throttle. We report on the implementation of our algorithms on two instrumented Lexus IS 250 test vehicles engaged in a collision avoidance scenario at a test intersection at the Toyota Technical Center, Ann Arbor, MI, USA.

The employment of formal methods in intelligent transportation has been previously applied by the California PATH project in the 1990s. The objective of the automated highway systems project was to deploy fully autonomous highway systems incorporating vehicle platoons to increase traffic throughput, safety, and fuel efficiency [4]. More recently, work that employs job-scheduling techniques [8], [17] and optimal control [19] for ICA has appeared. Collision warning algorithms have also been proposed for general traffic scenarios [7], [28] and for intersections [6], [12]. Although different in scope, research on collision mitigation through emergency braking [16] is also related to our work. Directly related to this paper are experimental works on full-scale vehicle test-beds focusing on collision avoidance/warning at intersections, which leverage V2V communication [20], [21]. Specifically, in [20], a fuzzy controller to manage vehicles crossing an intersection is proposed. In [21], onboard vehicle hazard detection that uses V2V is developed to warn the driver about dangerous situations. In these papers, formal safety guarantees are not provided, and cooperation between vehicles is not leveraged to provide least restrictive warnings/overrides. Here, we bridge the gap between formal methods and cooperative collision avoidance systems at intersections by developing/testing an experimental cooperative collision avoidance system based on formal control theoretic techniques.

SECTION II

We consider the intersection scenario shown in Fig. 1(a), in which two vehicles approach an intersection and can potentially collide in the indicated red shaded area. A collision may occur for a number of reasons, including a distracted driver not seeing the incoming vehicle, underestimating the vehicle speed, and violating red lights or stop signs. We seek to design controllers on board each vehicle that use V2V communication to negotiate the intersection and apply automatic control only when it is absolutely necessary to prevent a collision.

We assume that, after making high-level route decisions, drivers follow predefined (known) paths as established by driving lanes. Under this assumption, the methodology that we propose can be applied to any path geometry at an intersection. Here, we consider the specific intersection scenario in Fig. 1(a) to be consistent with the geometry of the test intersection employed in the experiments [see Fig. 1(d)]. Collisions between two vehicles are prevented only by controlling the longitudinal velocity and displacement of each vehicle along its path, never controlling vehicle steering. We assume that each vehicle is equipped with sensors for state measurement (absolute position, heading, velocity, acceleration, brake torque, and pedal position), V2V communication, and the ability to automatically actuate the throttle and brake. We assume that our collision avoidance system is active well before the vehicles approach the intersection, preventing initial vehicle configurations generating unavoidable collision. Under the given assumptions, the safety algorithms that we illustrate here guarantee that the vehicles will never collide.

The test vehicles used in this work are modified Lexus IS 250 (2007) test vehicles [see Fig. 1(c)]. The modifications include: computer running a Linux operating system; Differential Global Positioning System (DGPS) for position, absolute time, and heading measurement; Denso Wireless Safety Unit (WSU) capable of V2V and V2I dedicated short-range communications (DSRC); connection to the Controller Area Network (CAN) bus to read information from vehicle sensors (velocity, acceleration, brake pedal position, transmission state, etc.); and a CAN bus interface with brake and throttle actuators.

The computer system is well affixed inside the wheel. The purpose of this system is to interface with all onboard vehicle sensors and actuators, in a manner that allows for rapid development, deployment, and testing of software applications. The computer runs an Ubuntu Linux distribution and consists of an Intel Core-Duo 2.0-GHz processor, 1-GB random-access memory, a 150-GB hard drive, and a motherboard with onboard ethernet and Universal Serial Bus (USB) ports. A USB video card is connected to the vehicle navigation display unit and a wireless keyboard is used to control the computer from the passenger seat. The computer can read and write to the CAN bus via a USB adapter. To communicate between vehicles and interface with a DGPS unit, a Denso WSU is connected via ethernet, which is an after-market industry standard (planned) in communication and control for V2V and V2I safety systems [23].

The onboard DGPS unit is capable of 0.45-m accuracy for absolute position, 1.5^{o} accuracy for absolute heading, and 0.1-s accuracy for absolute time. The measurement update rate is 10 Hz. Other sensors include: 1) an accelerometer, which is based on microelectromechanical systems technology, capable of 0.5-$\hbox{m/s}^{2}$ accuracy; 2) a speedometer, measuring average speed at the wheel, capable of 0.5-m/s accuracy; 3) throttle pedal measurement, which is capable of 0.5% accuracy; and 4) brake torque applied at the wheel, which is capable of 0.5-Nm accuracy. The vehicle brake controller is modified to accept brake commands from the computer via CAN bus messages. The drive-by-wire [sends engine control unit (ECU) electric signals over the CAN bus] throttle pedal is modified to allow computer-issued commands via CAN bus messages to create throttle pedal signals to the ECU. Communication is carried out by the Denso WSU unit. The message standard is the DSRC, which is broadcast at the 5.9-GHz band, dedicated to V2V and V2I communication. The WSU is connected to a top mounted antenna [see Fig. 1(a)]. Communication is carried out with a broadcast network topology, that is, messages transmitted by a sender can be received by any listener in-range.

SECTION III

The general solution approach is based on formally encoding the requirement of no-collision into a bad set of vehicle speed and position configurations to be avoided. Then, based on the vehicle dynamical model, we calculate the capture set, which is the set of all vehicle configurations that enter the bad set independently of any throttle/brake control action. Once the capture set is computed, we determine a throttle/brake control map for both vehicles that keeps the system state outside of the capture set at all times. This control map applies throttle and brake inputs only when the system configuration hits the boundary of the capture set. Otherwise, no control action is applied, and the driver has full control of the vehicle.

The computations of the capture set and of the control map are usually very demanding, require an exact description of the system dynamics, and assume perfect information on the state of the system. Here, we illustrate the approach to compute the capture set and the control map developed in [14], which exploits the specific structure of the application domain to overcome these limitations. Specifically, it provides efficient algorithms, allows a coarser model obtained from suitable experiments, and is robust to imperfect state information due to sensor uncertainty and, particularly, to communication delays.

We model each vehicle as system $\Sigma^{i}$ for $i\in \{\hbox{1}, \hbox{2}\}$, describing the longitudinal dynamics of vehicle $i$ along its path. Each system $\Sigma^{i}$ is an input–output system, which is defined by the tuple $\Sigma^{i}: = \{X^{i}, {\cal O}^{i}, {\cal U}^{i}, {\cal D}^{i}, f^{i}, h^{i}\}$, where $X^{i} \subset \BBR^{2}$ is the state space describing position and speed; ${\cal O}^{i} \subset \BBR^{m}$ is the output measurement space; ${\cal U}^{i} := [u^{i}_{L}, u^{i}_{H}] \subset [\hbox{0}, \hbox{1}] \times [\hbox{0}, \hbox{1}]$ is the control input space representing the percentage the brake and throttle pedal are depressed; ${\cal D}^{i} := [d^{i}_{L}, d^{i}_{H}] \subset \BBR^{m}$ is the disturbance input space, which can be employed to account for unmodeled dynamics; $f^{i} : X^{i} \times {\cal U}^{i} \times {\cal D}^{i} \rightarrow X^{i}$ is the vector field modeling the dynamics of the vehicle; and $h^{i}: {\cal O}^{i} \rightrightarrows X^{i}$ is the output set-valued map that provides the set of states compatible with an output measurement. We let $x_{1}^{i}\in X^{i}_{1}$ denote the longitudinal displacement of vehicle $i$ along its fixed path and $x_{2}^{i}$ denote the longitudinal speed of vehicle $i$ along its path. We denote the continuous flow of system $\Sigma^{i}$ as $\phi^{i}(t, x^{i}, {\bf u}^{i}, {\bf d}^{i})$, where $t$ denotes the time, $x^{i}$ denotes the initial state, ${\bf u}^{i}$ denotes the control input signal, and ${\bf d}^{i}$ denotes the disturbance signal. In this paper, we will denote in bold signals, which are functions of time.

The two-vehicle system is modeled as the parallel composition of the two systems, denoted as $\Sigma = \Sigma^{1} \Vert \Sigma^{2}=\{X, {\cal O}, {\cal U}, {\cal D}, f, h\}$, in which $X = X^{1} \times X^{2}$, ${\cal O}= {\cal O}^{1} \times {\cal O}^{2}$, ${\cal U} = {\cal U}^{1} \times {\cal U}^{2}$, ${\cal D} = {\cal D}^{1} \times {\cal D}^{2}$, $f = (f^{1}, f^{2})$, and $h = (h^{1}, h^{2})$. Accordingly, we will let $x = (x^{1}, x^{2})$, $u = (u^{1}, u^{2})$, and $d = (d^{1}, d^{2})$. Furthermore, we let $x_{1} = (x_{1}^{1}, x_{1}^{2})\in X_{1}$ denote the pair of two-vehicle displacements. The safety specification for $\Sigma$ is described in terms of a subset of the state space that needs to be avoided to prevent a collision. Specifically, we call such a set the bad set ${\bf B}\subset X$, and we will say that the system is safe if the flow never enters the bad set ${\bf B}$. For some initial state $x_{o}$, the system is safe if there exists some control input signal ${\bf u}$, such that for all disturbance input signals ${\bf d}$ and time $t$, we have that $\phi(t, x_{o}, {\bf u}, {\bf d}) \not\in {\bf B}$.

From the construction of the state space and the fact that a collision between two vehicles results when they are both in the red shaded area in Fig. 1(a), ${\bf B}\subseteq X$ can be defined as TeX Source $${\bf B}: = \left\{x \in X\ \vert \left(x_{1}^{1}, x_{1}^{2}\right) \in \ ] L^{1}, H^{1} [\times] L^{2}, H^{2}[ \right\}\eqno{\hbox{(1)}}$$ where $L^{i} < H^{i}$ for $i \in \{\hbox{1}, \hbox{2}\}$ [see Fig. 1(a) and (b)]. We also denote $L = (L^{1}, L^{2})$ and $H = (H^{1}, H^{2})$.

The safe controller is based on computing a subset of the state space, which is called the capture set, denoted by ${\cal C} \subseteq X$. The capture set is the set of all initial conditions, such that no control input can prevent a collision. The mathematical definition is given by TeX Source $${\cal C}: = \left\{x \in X \ \vert \ \forall \ {\bf u}, \ \exists \ t, \ \ \exists \ {\bf d} \ \hbox{s.t.} \ \phi(t, x, {\bf u}, {\bf d}) \in {\bf B} \right\}.\eqno{\hbox{(2)}}$$

The approach of our solution to the safety control problem is to compute the capture set and, through the application of feedback control, prevent the flow from ever entering the capture set. By the definition of the capture set, safety is guaranteed if the flow never enters the capture set.

Computing the capture set is, in general, a difficult problem. In the following sections, we show how exploiting the structural features of the specific system under study allows us to compute this set and handle imperfect state information.

Here, we illustrate the main result in [14] to compute the capture set. This approach relies on 1) the state and input spaces of system $\Sigma^{i}$ being partially ordered and 2) the flow of system $\Sigma^{i}$ being an order preserving map. Specifically, for state space $X^{i}\subseteq \BBR^{2}$, we consider elements to be partially ordered according to component-wise ordering, that is, for $z^{i}, w^{i}\in X^{i}$, we have that $z^{i}\leq w^{i}$, provided $z_{1}^{i}\leq w_{1}^{i}$ and $z_{2}^{i}\leq w_{2}^{i}$. Further, we consider the partial ordering between input signals defined for signals ${\bf u}^{i}, {\bf v}^{i}$ as ${\bf u}^{i} \leq {\bf v}^{i} \Leftrightarrow {\bf u}^{i}(t) \leq {\bf v}^{i}(t)\ \hbox{for all}\ t$. The inequality ${\bf u}^{i}(t) \leq {\bf v}^{i}(t)$ is defined, such that ${\bf u}_{1}^{i}(t)\geq {\bf v}_{1}^{i}(t)$ and ${\bf u}_{2}^{i}(t)\leq {\bf v}_{2}^{i}(t)$. We assume that the flow of each system $\Sigma^{i}$ is an order preserving map. Mathematically, this means that for initial conditions $z^{i}, w^{i} \in X^{i}$, inputs ${\bf u}^{i}, {\bf v}^{i}$ and disturbances ${\bf d}^{i}, {\bf b}^{i}$, the following implication holds: TeX Source $$\displaylines{z^{i} \leq w^{i} \ \wedge\ {\bf u}^{i} \leq {\bf v}^{i} \ \wedge \ {\bf d}^{i} \leq {\bf b}^{i} \Rightarrow\hfill\cr\hfill \phi^{i} \left(t, z^{i}, {\bf u}^{i}, {\bf d}^{i} \right) \leq \phi^{i} \left(t, w^{i}, {\bf v}^{i}, {\bf b}^{i} \right) \ \forall \ t.\quad\hbox{(3)}}$$ In terms of the vehicle dynamics, this assumption implies that greater initial displacement, greater initial velocity, and greater inputs will lead to greater displacements and speeds at any time. The validity of this assumption for the vehicle dynamics is discussed in detail in Section IV, where the vehicle model is introduced. A liveliness condition is introduced by requiring that for at least one $i$ $f^{i}_{1}(x^{i}, u^{i}, d^{i}) > \hbox{0}$ for all $x^{i}$, $u^{i}$ and $d^{i}$. From a practical point of view, this requires that vehicle $i$ does not go in reverse and does not stop.

The order preserving property of the dynamics along with the structure of the bad set can be exploited to compute the capture set for system $\Sigma = \Sigma^{1}\Vert \Sigma^{2}$ with an algorithm that has linear complexity with respect to the state dimension. The algorithm is based on the restricted capture set, which, for a fixed input signal ${\bf u}$, is defined as ${\cal C}_{{\bf u}} := \{x \in X\ \vert\ \exists\ t \geq \hbox{0}, \ \exists\ {\bf d}\ \ \hbox{s.t.}\ \phi(t, x, {\bf u}, {\bf d}) \in {\bf B}\}$. This set represents the set of initial conditions that are taken into the bad set under the fixed input signal ${\bf u}$. Define the fixed input signals ${\bf u}_{\cal L}, {\bf u}_{\cal H}$, as ${\bf u}_{\cal L}(t) := (u^{1}_{H}, u^{2}_{L})$ and ${\bf u}_{\cal H}(t) := (u^{1}_{L}, u^{2}_{H})$ for all $t$. Then, we have [14] TeX Source $${\cal C} = {\cal C}_{\bf u_{\cal L}} \cap {\cal C}_{\bf u_{\cal H}}.\eqno{\hbox{(4)}}$$ the capture set can be computed by only computing the two restricted capture sets corresponding to maximum and minimum inputs. The restricted capture sets are simpler to compute, since they can be obtained by just integrating the dynamics under fixed control inputs. This is in contrast with capture set ${\cal C}$, whose computation requires the solution of a differential game between the control and the disturbance.

Based on the expression of the capture set given in (4), the feedback control map is given by TeX Source $$g(x): = \cases{\left(u_{H}^{1}, u_{L}^{2} \right), & \hbox{if} $x \in {\cal C}_{\bf u_{\cal L}} \ \hbox{and} \ x \in \partial {\cal C}_{\bf u_{\cal H}}$ \cr \left(u_{L}^{1}, u_{H}^{2}\right), & \hbox{if} $x \in \partial {\cal C}_{\bf u_{\cal L}} \ \hbox{and} \ x \in \overline{{\cal C}_{\bf u_{\cal H}}}$ \cr \quad{\cal U}, & {\hskip35pt}$\hbox{otherwise}$ }\eqno{\hbox{(5)}}$$ in which $\overline{{\cal C}_{{\bf u}_{\cal H}}}$ denotes the closure of ${{\cal C}_{{\bf u}_{\cal H}}}$. The controller allows the driver to choose any input until the flow hits the boundary of the capture set. The driver retains control once the flow no longer touches the boundary of the capture set. A visual interpretation of the feedback map is provided in Fig. 2.

In the presence of communication delays and/or uncertain sensor readings, the vehicles will not have access to the exact value of the system state but to a set of possible current system states. This can be easily incorporated in the previously described control strategy [14]. Let the set of possible current system states be denoted $\mathhat{x} \subset X$, which can be constructed using output measurement $z\in {\cal O}$, as explained in Section V-A. The safety specification is now posed in terms of preventing state uncertainty $\mathhat{x}$ from intersecting bad set ${\bf B}$. That is, the system is safe if $\mathhat{x}(t) \cap {\bf B} = \emptyset$ for all $t \in \BBR_{+}$. It has been shown that this is the case if and only if $\mathhat{x}(t)$ never intersects both ${\cal C}_{{\bf u}_{\cal L}}$ and ${\cal C}_{{\bf u}_{\cal H}}$ at the same time [14]. The feedback set-valued map $g$, as defined in (5), can still guarantee this as long as it is extended to set $\mathhat{x}$ as follows: TeX Source $${g}(\mathhat{x}): = \cases{\left(u_{H}^{1}, u_{L}^{2}\right), & if $\mathhat{x} \cap {\cal C}_{\bf u_{\cal H}} \neq \emptyset$ and \cr& $\mathhat{x} \cap \partial {\cal C}_{\bf u_{\cal L}} \neq \emptyset$ and $\mathhat{x} \cap {\cal C}_{\bf u_{\cal L}} = \emptyset$ \cr \left(u_{L}^{1}, u_{H}^{2}\right), & if $\mathhat{x} \cap \overline{{\cal C}_{\bf u_{\cal L}}} \neq \emptyset$ and\cr& $ \mathhat{x} \cap \partial {\cal C}_{\bf u_{\cal H}} \neq \emptyset$ and $\mathhat{x} \cap {\cal C}_{\bf u_{\cal H}} = \emptyset$ \cr \quad{\cal U}, & \quad otherwise.}\eqno{\hbox{(6)}}$$ If the set of admissible control inputs evaluated by ${g}(\mathhat{x})$ is ${\cal U}$, the driver is free to apply any input. The interpretation of this feedback set-valued map is that control is applied when the state uncertainty has a nonempty intersection with either ${\cal C}_{{\bf u}_{\cal L}}$ or ${\cal C}_{{\bf u}_{\cal H}}$ and, simultaneously, is touching the boundary of the other. We remark that by construction, feedback map $g$ is order reversing with respect to partial order established by set inclusion, that is, $A \subset B\ \rightarrow g(A) \supset g(B).$ This property implies that the larger the state uncertainty, the more conservative the controller will be.

Here, we provide a summary of the algorithms that compute the restricted capture set for the case in which the first component of vector fields $f^{i}$ do not depend on the $x^{i}_{1}$ coordinate (displacement) [14]. This assumption is satisfied by the vehicle dynamics considered in the following section. The algorithms are implemented onboard the vehicle computer; therefore, they must use a discrete-time model of the dynamics. For $n > \hbox{0}$ and step size $\Delta T > \hbox{0}$, the discrete-time flow of system $\Sigma$ is given by $\Phi(n, x, {\bf u}, {\bf d})$ and is generated by the forward Euler approximation of the continuous-time dynamics, mathematically given by $\Phi(n + \hbox{1}, x, {\bf u}, {\bf d}) = \Phi(n, x, {\bf u}, {\bf d}) + \Delta T f(\Phi(n, x, {\bf u}, {\bf d}), {\bf u}[n - \hbox{1}], {\bf d}[n - \hbox{1}])$, with initial condition $\Phi(\hbox{0}, x, {\bf u}, {\bf d}) = x$, and sampled signals ${\bf u}[n] := {\bf u}(n\Delta T)$ and ${\bf d}[n] := {\bf d}(n \Delta T)$.

Feedback map $g$ is implemented in discrete time, which requires an alternate definition of the capture set boundary. We will say that set $\mathhat{x}[n]\subset X$ intersects the boundary and not the interior of the restricted capture set ${\cal C}_{\bf u}$, provided $\mathhat{x}[n] \cap {\cal C}_{\bf u} = \emptyset\ \hbox{and}\ \mathhat{x}[n + \hbox{1}]\cap {\cal C}_{\bf u}\neq \emptyset$. This states that $\mathhat{x}[n]$ intersects the boundary and not the interior of the restricted capture set if it is currently outside of the set, but it will be inside the set at the next time step.

To compute capture set ${\cal C}_{\bf u}$, we can compute a slice of it in the displacement space, which is denoted by ${\cal C}_{\bf u} \subset X_{1}$, corresponding to the current two-vehicle velocity $(x_{2}^{1}, x_{2}^{2})$. Due to the order preserving properties of the dynamics with respect to state and input and the structure of bad set ${\bf B}$, the restricted capture set slice is computed through the back propagation of the upper and lower bounds of the bad set, i.e., $L, H \in X_{1}$. Specifically, define the sequences TeX Source $$\eqalignno{L(n, x, u): = &\, L + x_{1} - \Phi_{1}(n, x, {\bf u}, {\bf d}_{H}) \cr H(n, x, u): = &\, H + x_{1} - \Phi_{1}(n, x, {\bf u}, {\bf d}_{L})&\hbox{(7)}}$$ where ${\bf d}_{L}(k) := (d^{1}_{L}, d^{2}_{L})$ and ${\bf d}_{H}(k) := (d^{1}_{H}, d^{2}_{H})$ for all $k$. Given current state estimate set $\mathhat{x}$, the restricted capture set slice ${\cal C}_{{\bf u}}$ can be written as (Algorithm 1) TeX Source $${\cal C}_{\bf u} = \bigcup_{k \in \BBN}]L(n, \sup \mathhat{x}, {\bf u}), H(n, \inf \mathhat{x}, {\bf u})[.$$

We can determine the nonempty intersection of the capture set with the state uncertainty by using the equivalence $\mathhat{x}_{1} \cap {\cal C}_{\bf u} = \emptyset \Leftrightarrow \mathhat{x} \cap {\cal C}_{\bf u} = \emptyset$. The closed-loop implementation of the feedback map (6), in discrete time, is provided in Algorithm 2, where ${\bf u} =\hbox{FeedbackMap}(\mathhat{x}[n + \hbox{1}], \mathhat{x}[n])$.

Note that for evaluating the control map, we only need to calculate sequences $L(n, x, u)$ and $H(n, x, u)$ for two extremal constant inputs $u_{{\cal L}} = (u^{1}_{H}, u^{2}_{L})$ and $u_{{\cal H}} = (u^{1}_{L}, u^{2}_{H})$. Hence, we do not require the detailed model of system $\Sigma$, we just need to know how the system responds to these two extremal constant inputs. As we will see in Section IV, this can be achieved through a series of experiments where these constant inputs are applied for a set of different initial speeds.

SECTION IV

The vehicle dynamics, which take throttle and brake as inputs and provide longitudinal displacement as output, is the cascade of the powertrain system and the vehicle model [see Fig. 3(a)]. The powertrain system [see Fig. 3(b)] generates the wheel torque inputs in response to throttle and brake inputs. The vehicle model takes throttle and brake inputs and produces longitudinal displacement as output according to Newton's law. Here, we describe each of the two subsystems and illustrate how the cascade of the two generates a flow that is an order preserving map when throttle inputs do not change with time. Then, we perform a system identification procedure to determine the dynamics of the cascade system only in response to maximal throttle and maximal braking, which is sufficient for the implementation of the control map, as described in Section III.

The longitudinal displacement of the vehicle along its path is denoted by $p$, and the longitudinal velocity is denoted by $v \in [v_{\rm min}, v_{\rm max}]$, where $v_{\rm min} \geq \hbox{0}$. The controlled forces that act on the vehicle are the brake input $f_{b} \in {\cal F}_{b} = [f_{\rm min}, \hbox{0}]$ with $f_{\rm min} < \hbox{0}$ and engine input $f_{e} \in {\cal F}_{e} = [ \hbox{0}, f_{\rm max}]$ with $f_{\rm max} > \hbox{0}$. Brake force $f_{b}$ is controlled by the driver via the surjective-monotone map $\pi: {\cal U}_{1} \rightarrow {\cal F}_{b}$ That takes brake pedal percentage $u_{1}$ as an input, whereas engine force $f_{e}$ is supplied by the powertrain [see Fig. 3(a)]. The longitudinal dynamics are given by TeX Source $$\displaylines{{dv \over dt} = {{\cal R}^{2} \over J_{w} + {\cal M}{\cal R}^{2}} \left(f_{e} + f_{b} - {\rho_{\rm air} \over \hbox{2}} C_{D} A_{f} v^{2} \right.\hfill\cr\hfill - C_{rr}{\cal M}g) =: \mathtilde{f}(v, f_{b}, f_{e}) \quad\hbox{(8)}}$$ where ${\cal R}$ is the wheel radius, ${\cal M}$ is the vehicle mass, $\rho_{\rm air}$ is the air density, $C_{D}$ is the air drag coefficient, $A_{f}$ is the projected vehicle cross section, and $C_{rr}$ is the coefficient of rolling friction [29].

The longitudinal dynamics (8) generate a flow $(p(t, p_{o}, v_{o},\break {\bf f}_{\bf b}, {\bf f}_{\bf e}), v(t, v_{o}, {\bf f}_{\bf b}, {\bf f}_{\bf e}))$ That is an order preserving map with respect to brake force input signal ${\bf f}_{b}$, engine force signal ${\bf f}_{e}$, and initial conditions $(p_{o}, v_{o})$. That is, larger forces $f_{b}$ and $f_{e}$ will result in greater displacements and speeds; larger initial conditions $(p_{o}, v_{o})$ will also result in larger displacements and speeds. On the input space, we use the partial order defined by $u \leq v$, provided $u_{1} \geq v_{1}$ and $u_{2} \leq v_{2}$. Consequently, we have $u_{L} = (\hbox{1}, \hbox{0})$ and $u_{H} = (\hbox{0}, \hbox{1})$. Since brake force map $\pi: {\cal U}_{1} \rightarrow {\cal F}_{b}$ is monotone, the flow is an order preserving map also with respect to brake input ${\bf u}_{1}$. In the following section, we illustrate the components of the powertrain.

The dynamics of the powertrain take as control inputs $u = (u_{1}, u_{2}) \in [\hbox{0}, \hbox{1}] \times [\hbox{0}, \hbox{1}]$, where the first component $u_{1}$ denotes the brake pedal percent input, and the second component $u_{2}$ denotes the throttle pedal percent input [5]. In our application, these inputs can be administered either by the driver or by the automatic controller. The output of the system is assumed to be the torque applied at the wheel of vehicle $f_{e}$. An overview of the system is provided in Fig. 3(b).

The first component of the powertrain is the ECU. This subsystem determines the fuel injection rate $i\in [\hbox{0}, \hbox{1}]$ into the internal combustion engine (ICE) and the current gear $q \in \{\hbox{1}, \hbox{2}, \hbox{3}, \hbox{4}, \hbox{5}, \hbox{6}\}$ of the gearbox. The inputs to this block consist of the current velocity of vehicle $v$, throttle pedal input $u_{2}$, and brake pedal input $u_{1}$.

The second component of the powertrain is the ICE. The output of this system is torque $\tau$ applied by the flywheel and the input is the fuel injection rate administered by the ECU.

The third component of the powertrain is the gearbox. This module consists of the transmission with a fixed gear ratio. All switching logic is determined by the ECU, which sends reset input $R$ to the gearbox when a gear shift has been determined. The gearbox takes torque at the flywheel $\tau$ and converts it to torque $\tau_{q}$ based on the current gear.

The last component of the powertrain is the drivetrain. This component transfers torque at the gearbox $\tau_{q}$ to force applied at the wheel $f_{e}$. This module consists of the flywheel, torque converter, variable gear ratio transformer, propeller shaft, final drive, and drive shaft. (Details can be found, for example, in [29].)

For the powertrain model, the order preserving property of output $f_{e}$ with respect to throttle input $u_{2}$ does not hold in general. This is due to the complexity of the ECU, which controls the fuel injection rate in a manner that optimizes a set of performance metrics, such as emissions, engine thermodynamic efficiency, with transients that can be quite complex and nonmonotone [5]. By design, however, this is performed in a manner that generates monotone input–output behavior at steady state [10].

Therefore, the dynamics of the vehicle system that take brake $u_{1}$ and throttle $u_{2}$ commands as inputs and provide speed and displacement as output are order preserving with respect to constant throttle input at least after an initial transient. Hence, we restrict the control commands to be constant with time, so that the system dynamics generate an order preserving flow with respect to the inputs after an initial transient time $\epsilon$. In the following section, we illustrate how to identify the vehicle dynamics for the maximal braking and throttle inputs, which is the only knowledge on the model required by our algorithm.

To model how the powertrain responds to constant control inputs (maximal braking and maximal throttle), in principle, one should model the details of all the blocks in Fig. 3(b). Rather than modeling this level of detail, we exploit the fact that the approach illustrated in Section III allows for disturbance inputs, which we use here to account for unmodeled dynamics. For input signal ${\bf u}$ and velocity signal ${\bf v}$, define the nondeterministic engine force trajectories ${\bf F}_{e}({\bf u}, {\bf v})$ as the set of all possible output engine force trajectories applied at the wheel given an input signal and velocity signal.

When the powertrain model is combined with vehicle physics, vehicle velocity $v$ and engine force at the wheel $f_{e}$ are coupled through the longitudinal dynamics introduced in (8). To capture this dependence, we say that a system evolution is realizable if velocity trajectory ${\bf v}(t, v_{0}, {\bf u}_{1}, {\bf f}_{e})$ and engine torque trajectory ${\bf f}_{e}([\hbox{0}, t])$ satisfy (8) at all times and the inclusion TeX Source $${\bf f}_{e}([\hbox{0}, t]) \in {\bf F}_{e} \left ({\bf u}([\hbox{0}, t]), {\bf v} \left([\hbox{0}, t], v_{0}, \pi({\bf u}_{1}),{\bf f}_{e} \right) \right).\eqno{\hbox{(9)}}$$

Let $\epsilon \in \BBR_{+}$ denote the maximum delay between initial changes in driver input $u$ and steady-state vehicle acceleration $\mathdot{v}$. This is the consequence of delays in 1) software subsystems of the drive-by-wire throttle system, 2) delays in the powertrain due to chemical combustion, 3) gear shift delays, and 4) delays imposed by the ECU for filtering and environmental reasons. For a speed $x_{2}$, input $u^{\ast}$, and time-delay constant $\epsilon \geq \hbox{0}$, the permissible acceleration set, which is denoted by $\Upsilon(x_{2}, u^{\ast}, \epsilon) \subset \BBR$, is the collection of all accelerations given by TeX Source $$\eqalignno{& \Upsilon(x_{2}, u^{\ast}, \epsilon): = \cr& {\hskip25pt}\{\mathtilde{f} \left({\bf v} \left(t, v_{0}, \pi\left({\bf u}_{1}^{\ast}\right), {\bf f}_{e} \right), \pi \left({\bf u}_{1}^{\ast}(t) \right), {\bf f}_{e}(t) \right) \in \BBR \ \vert\cr& {\hskip25pt} \exists {\bf f}_{e}([\hbox{0}, t]) \in {\bf F}_{e} \left({\bf u}^{\ast}, {\bf v} \left([\hbox{0}, t], v_{0}, \pi({\bf u}_{1}^{\ast}), {\bf f}_{e} \right) \right) \cr& {\hskip25pt} \exists t \geq \epsilon, \ \exists v_{0}\ \hbox{s.t.}\ x_{2} = {\bf v} \left(t, v_{0}, \pi({\bf u}_{1}^{\ast}), {\bf f}_{e} \right)\}&\hbox{(10)}}$$ where ${\bf u}^{\ast}(t)=u^{\ast}$ for all $t$.

This is the set of all possible accelerations $\alpha = \mathtilde{f}(x_{2}, \pi(u_{1}^{\ast}),\break {\bf f}_{e}(t))$ achievable at velocity $x_{2}$ after $t \geq \epsilon$ s have elapsed under constant input signal ${\bf u}^{\ast}$. Letting $x_{1} = p$ and $x_{2} = v$, we construct vector field $f(x, u, d)$ in Section III-B for a fixed input $u = u^{\ast}$ as $f_{1}(x, u^{\ast}, d) := x_{2}, \ f_{2}(x, u^{\ast}, d_{H}) := \sup \Upsilon(x_{2},\break u^{\ast}, \epsilon), \ f_{2}(x, u^{\ast}, d_{L}) := \inf \Upsilon(x_{2}, u^{\ast}, \epsilon)$. For the case of maximum disturbance $d_{H}$ (minimum disturbance $d_{L}$), the interpretation of $f_{2}(x, u^{\ast}, d_{H})$ ($f_{2}(x, u^{\ast}, d_{L})$) is that it represents the greatest acceleration (least acceleration) that can possibly be achieved at velocity $x_{2}$ after constant input $u^{\ast}$ has been applied for at least $\epsilon \geq \hbox{0}$ s. If $\Upsilon(x, u^{\ast}, \epsilon) = \emptyset$, then find the minimizer $x^{\ast}_{2} := \arg\min_{y_{2} \in X_{2}}\ \{\Vert y_{2} - x_{2} \Vert\ \vert\ \Upsilon(y_{2}, u^{\ast}, \epsilon) \neq \emptyset \}$ and set $f(x, u^{\ast}, d) = f((x_{1}, x_{2}^{\ast}), u^{\ast}, d)$.

For implementing the feedback map in Section III-B, it is enough to experimentally identify $f_{2}(x, u_{L}, d_{H})$ and $f_{2}(x, u_{H}, d_{L})$. The identification procedure is as follows. To identify $f_{2}(x, u_{L}, d_{H})$, we conducted a set of experiments called braking trials, in which, starting from an initial constant velocity, maximal braking $u_{L} = (\hbox{1}, \hbox{0})$ is applied, and vehicle acceleration after $\epsilon = \hbox{0.7}\ \hbox{s}$ is recorded to provide data points for $\Upsilon(x_{2}, u_{L}, \epsilon)$ for the values of speed $x_{2}$ reached after $\epsilon$. The value of $\epsilon$ was chosen to be enough for the vehicle to reach a steady-state acceleration. Several trials for the same initial speed were performed, and the infimum of these data points for every speed $x_{2}$ was computed to provide the value of $f_{2}(x, u_{L}, d_{H})$. The set of initial velocities chosen is ${\cal V}_{0} := \{(\hbox{1}/\hbox{4})v_{\rm max}, (\hbox{1}/\hbox{2})v_{\rm max}, \{(\hbox{3}/\hbox{4})v_{\rm max}, v_{\rm max}\}$, in which $v_{\rm max} = \hbox{8}\ \hbox{m/s}$ for vehicle 1 (Blue IS 250) and $v_{\rm max} = \hbox{17}\ \hbox{m/s}$ for vehicle 2 (Grey IS 250). A brake trial consists of the following steps: 1) accelerate each vehicle to a nominal constant velocity $v_{0} \in {\cal V}_{0}$ on the vehicle path; 2) maintain velocity $v_{0}$ for at least 2 s, so transmission comes to a steady state; and 3) apply brake input $u_{L} := (\hbox{1}, \hbox{0})$ via a computer-issued command, driver does not override command until the vehicle reaches rest.

Similarly, to identify $f_{2}(x, u_{H}, d_{L})$, we conducted a set of experiments called throttle trials, in which, starting from an initial constant velocity, maximal throttle $u_{H} = (\hbox{0}, \hbox{1})$ for vehicle 1 and $u_{H} = (\hbox{0}, \hbox{0.5})$ for vehicle 2 was applied. The set of initial velocities are given by ${\cal V}_{0}:= \{\hbox{0}, (\hbox{1}/\hbox{4})v_{\rm max},\break (\hbox{1}/\hbox{2})v_{\rm max}, (\hbox{3}/ \hbox{4})v_{\rm max}\}$, in which $v_{\rm max} = \hbox{8}\ \hbox{m/s}$ for vehicle 1 and $v_{\rm max} = \hbox{17}\ \hbox{m/s}$ for vehicle 2. A throttle trial consists of the following steps: 1) accelerate each vehicle to a nominal constant velocity $v_{0} \in {\cal V}_{0}$ on vehicle path, if $v_{0} = \hbox{0}$, leave vehicle in idling state; 2) maintain velocity $v_{0}$ for at least 2 s, so transmission comes to steady state; and 3) apply acceleration input via a computer-issued command, driver does not override command until the vehicle reaches maximum velocity $v_{\rm max}$.

For vehicle 1, which has ${\cal U}^{1} = [\hbox{0}, \hbox{1}] \times [ \hbox{0}, \hbox{0.5}]$ and $x_{2}^{1}\in [\hbox{0}, \hbox{8.8}]$ m/s, along path 1 [as shown in Fig. 1(c)], we obtained $f^{1}_{2}(x_{2}^{1}, u^{1}_{L}, d^{1}_{H}) = -\hbox{3.1}$ and TeX Source $$f_{2}^{1}\left(x_{2}^{1}, u_{H}^{1}, d_{L}^{1}\right) = \cases{\hbox{3.0}, & $x_{2}^{1} \in [\hbox{0}, \hbox{7})$ \cr \hbox{1.75}, & $x_{2}^{1} \in [\hbox{7}, \infty).$} \eqno{\hbox{(11)}}$$

For vehicle 2, which has ${\cal U}^{2} = [\hbox{0}, \hbox{1}] \times [ \hbox{0}, \hbox{1}]$ and $x_{2}^{2}\in [\hbox{8.8}, \hbox{20}]$ m/s, along path 2 [as shown in Fig. 1(c)], we obtained $f^{2}_{2}(x_{2}^{2}, u^{2}_{L}, d^{2}_{H}) = -\hbox{3.1}$ and TeX Source $$f_{2}^{2}\left(x_{2}^{2}, u_{H}^{2}, d_{L}^{2}\right) = \cases{\hbox{3.9}, & $x_{2}^{2} \in [\hbox{0}, \hbox{13}) $\cr \hbox{2.5}, & $x_{2}^{2} \in [\hbox{13}, \infty).$} \eqno{\hbox{(12)}}$$ Fig. 4 shows the system identification results for vehicle 2. Similar plots were obtained for vehicle 1.

SECTION V

The major software components of the ICA application are estimation, communication, and control (see Fig. 5).

State estimation consists of several modules, i.e., longitudinal state measurement construction from raw measurements in UTM coordinates, calculation of the universal time, Kalman filter for local state prediction, and a full state estimator to construct the current state estimate set $\mathhat{x}(t) \subset X$ for the whole system. We denote with superscript “$L$” quantities computed on the local vehicle, whereas with superscript “ $R$” we denote quantities of the remote vehicle that the local vehicle receives through wireless communication. The measurement projection block is used to compute longitudinal state measurement $y_{k}$ from GPS and CAN measurements $y^{\rm UTM}$ (heading and position from GPS, velocity from CAN). The global time is computed by using local time measurement $t^{L}$ from the vehicle PC, and drift is removed by using universal time $t^{\rm UTM}$ from the GPS system. The Kalman filter combines longitudinal state measurement $y_{k}$ and pedal inputs $u^{L}$ to compute state estimate $x^{L}$ and acceleration profile ${\cal A}_{t}^{L}$. This information is sent both to the communication system and to the full state estimator. The full state estimator takes the current state estimate, time and acceleration profile $\{x^{L}, t^{L}, {\cal A}_{t}^{L}\}$, and combines this with remote state information $\{x^{R}, t^{R}, {\cal A}_{t}^{R}\}$ to construct full state estimate $\mathhat{x}[k]$ for use by the controller.

The time measurements available to each vehicle consist of global time $t^{\rm UTM}$, which is taken from the GPS system, and local time $t^{L}$ taken off the vehicle PC. Global time $t^{\rm UTM}$ is accurate but is only received at a rate of 10 Hz and can sometimes be unavailable due to message loss. Local time $t^{L}$ is available at a higher rate of 1.5 GHz to a precision of 1 ms; however, it is not globally accurate due to inherent drift in the crystal oscillator used to calculate time. To accurately compute a global time with an update rate that is equal to 1.5 GHz, we combine global time $t^{\rm UTM}$ with local time $t^{L}$ to produce time $t$ with using a simple moving average, where the moving average is updated every time a new global time $t^{\rm UTM}$ is made available.

The measurement projection block constructs a longitudinal state measurement from raw sensors onboard the vehicle. This involves projecting raw measurements onto the vehicle's path locally stored in ${\cal P}^{L}$. The source of absolute position and heading measurements is the GPS system, which provides updates at a fixed broadcast rate of 10 Hz.

For the Kalman filter, the longitudinal dynamics are assumed to be linear and hybrid, where transmission state $q \in \{\hbox{1}, \hbox{2}, \hbox{3}, \hbox{4}, \hbox{5}, \hbox{6}\}$ is assumed to be known at all times, as obtained from the CAN bus. To model rolling friction, we add a fictitious frictional input, which takes values based on the sign of velocity, which is given by $u_{3} = \hbox{sgn}(x_{2})$. Since we also seek to estimate acceleration, we add the engine torque at the wheels as a third state. Specifically, the Kalman filter state is $\mathhat{e}\in \BBR^{3}$, where the first component is longitudinal displacement, the second component is longitudinal velocity, and the third component is the engine torque applied at the wheels. The output measurement is $y_{k}\in \BBR^{3}$ and incorporates longitudinal displacement, longitudinal velocity, and acceleration measured from the onboard accelerometer. The output is a discrete-time signal indexed by $k \in \BBN$ with constant time step $\Delta T > \hbox{0}$, where the correspondence to time $t$ is given by $t = k \Delta T$. The process dynamics are given by TeX Source $$\eqalign{\dot{\mathhat{e}}(t) = &\, A \left(q(t) \right) \mathhat{e}(t) + B \left(q(t) \right) u(t) + w(t) \cr y_{k} = &\, C_{k} \mathhat{e}(k \Delta T) + D_{k} u(k \Delta T) + v_{k}}$$ where $w(t)\sim(\hbox{0}, Q)$ is continuous-time white noise with covariance $Q$, and $v_{k}\sim(\hbox{0}, R)$ is discrete-time white noise with covariance $R$.

Let matrix $P(t)$ denote the estimated state error covariance, which is initialized to the identity matrix. Then, the prediction step of the filter is given by the following update equations, which represent a forward Euler approximation of the continuous-time dynamics: TeX Source $$\eqalign{\mathhat{e}(t) = &\, \mathhat{e}(t^{-}) + t_{\Delta} \left(A \left(q(t) \right) \mathhat{e}(t^{-}) + B \left(q(t)\right) u(t) \right)\cr P(t) = &\, P(t^{-}) + t_{\Delta} \left(A \left(q(t) \right) P(t^{-}) \right.\cr&{\hskip70pt}\left. + P(t^{-}) A \left(q(t) \right)^{T} + Q \right)}$$ where $t^{-}$ is the time of the previous update, and $t_{\Delta} := t - t^{-}$. A prediction step is performed every time the software system updates the current state; therefore, in general, time step $t_{\Delta}$ is not constant. The correction step only occurs when a new longitudinal state measurement $y$ is available and consists of the following update equations: TeX Source $$\eqalign{K_{k} = &\, P(t^{-}) C^{T} \left(CP(t^{-}) C^{T} + R \right)^{-1}\cr \mathhat{e}(t) = &\, \mathhat{e}(t^{-}) + K_{k} \left(y_{k} - \left(C \mathhat{e}(t^{-}) + Du(t) \right)\right)\cr P(t) = &\, (I - K_{k}C) P(t^{-})(I - K_{k}C)^{T} + K_{k}RK_{k}^{T}.}$$ By nature of the fixed rate of measurements (discrete-time) and continuous-time inputs, the filter is said to be hybrid [25].

Matrices $A$, $B$, $C$, and $D$ have been identified from data for every gear $q$ employing the system identification toolbox within MATLAB. In particular, we used a gray-box technique, where the system identification determines a vector of parameters, given a matrix structure derived from first principles. In particular, we have a second-order system with rolling friction and inputs. We assume a multiplicative gear ratio from engine input to change in wheel torque. Therefore, the matrices are of the following form: TeX Source $$\eqalign{A(q) = &\, \left[\matrix{\hbox{0} & \hbox{1} & \hbox{0} \cr \hbox{0} & \hbox{0} & \hbox{1}\cr \hbox{0} & \hbox{0} & a(q)}\right]\cr B(q) =&\, \left[ \matrix{\hbox{0} & \hbox{0} & \hbox{0}\cr b_{1} & \hbox{0} & b_{2}\cr \hbox{0} & \alpha(q)b_{3}(q) & \hbox{0}} \right] \cr C(q) = &\, \left[\matrix{\hbox{1} & \hbox{0} & \hbox{0}\cr \hbox{0} & \hbox{1} & \hbox{0}\cr \hbox{0} & \hbox{0} & \hbox{1}} \right]\cr D(q) =&\, \left[ \matrix{\hbox{0} & \hbox{0} & \hbox{0}\cr \hbox{0} & \hbox{0} & \hbox{0}\cr b_{1} & \hbox{0} & \alpha(q)b_{3}(q)} \right].}$$ Data to preform this identification task were taken from four driving trials with varying input signals. The input signals were chosen by the driver to ensure an adequate sweep of the vehicles dynamic range under consideration. Each trial was taken on the path for which the vehicle normally drives on.

From the experimental data collected, we obtained for $q = \hbox{1}$ That $a(q)=-\hbox{2.5}$, $b_{1} = -\hbox{5}$, $b_{2} = -\hbox{0.1}$, $b_{3}(q) = \hbox{5}$, and $b_{1} = \hbox{0.002}$. For $q\in \{\hbox{2}, \hbox{3}, \hbox{4}, \hbox{5}, \hbox{6}\}$, we obtained that $a(q) = -\hbox{1}$, $b_{1} = -\hbox{5}$, $b_{2}=-\hbox{0.1}$, $b_{3}(q) = \hbox{5}$, and $b_{1} = \hbox{0.002}$. The gear ratios are given by $\alpha(\hbox{1}) = \hbox{3.5}, \ \alpha(\hbox{2}) = \hbox{2.0}, \ \alpha(\hbox{3}) = \hbox{1.5}, \ \alpha(\hbox{4}) = \hbox{1.2}, \ \alpha(\hbox{5}) = \hbox{1}$, and $\alpha(\hbox{6}) = \hbox{0.8}$, which were taken from a technical data sheet [1]. This model was validated by comparing simulations obtained with an experimental input signal with the experimental trajectories.

To implement the Kalman filter, we chose the process and output noise covariance matrices to maximize noise rejection while still maintaining satisfactory bandwidth. We assume that all noise processes are independent and identically distributed and have no mode dependence; therefore, the covariance matrices are all diagonal. The matrices are given as $R = {\rm diag}(\hbox{0.5}, \hbox{0.3}, \hbox{1})$ and $R = \ {\rm diag}(\hbox{0.5}, \hbox{1}, \hbox{1})$.

The Kalman filter is used to construct a state prediction. This is accomplished by computing acceleration profile ${\cal A}_{\bar{t}}$, which is a set-valued signal containing all possible acceleration trajectories for future times $t \geq \bar{t}$. This allows to predict the set of possible speeds $\mathhat{e}_{2}(t)$ for $t\geq \bar{t}$. Mathematically, this is given as $\mathhat{e}_{2}(t) \in \mathhat{e}_{2}(\bar{t}) + \int_{\bar{t}}^{t} {\cal A}_{\bar{t}}(\tau) d\tau$. As mentioned in Section III-C, Algorithm 2 requires a two-vehicle state prediction, which has a tunable time step $\Delta_{p}$, which can be chosen by the test engineer, assumed to be less than 1.5 s in total. With such a short time scale, it is reasonable to assume that the input stays constant, that is, $u(t) = u(\bar{t})$ for all $t \geq \bar{t}$. To account for the error of this assumption, we add a configurable window parametrized by parameter $\beta \in \BBR_{+}$ to the resulting acceleration. As $\beta$ is taken to 0, the prediction is assumed to be exact. The calculation is carried out, to obtain upper and lower bound sequences $[l_{k}, h_{k}]$, with the hybrid Kalman filter as TeX Source $$\eqalign{\mathhat{e}_{k} = &\, \mathhat{e}_{k - 1} + \Delta T \left(A \left(q(\bar{t}) \right) \mathhat{e}_{k - 1} + B \left(q(\bar{t}) \right) u(\bar{t}) \right) \cr [l_{k}, h_{k}] = &\, [\hbox{0}\ \hbox{0}\ \hbox{1}] \left(C \mathhat{e}_{k} + D u(\bar{t}) \right) + k [ - \beta, \beta]}$$ where set addition is understood in the sense of the Minkowski sum. Acceleration profile ${\cal A}_{\bar{t}}(t)$ is found by taking the zero-order hold approximation of sequence $[l_{k}, u_{k}]$.

The Kalman filter output is the estimate of position and speed, which are the first two components of $\mathhat{e}$, denoted by $x^{L}$ for the local vehicle and by $x^{R}$ for the remote vehicle, the estimate of global time $t$, and acceleration profile ${\cal A}_{\bar{t}}(t)$. The full state estimate is constructed by combining local state estimation from the Kalman filter with received remote vehicle state information. In accordance with feedback map $g(\mathhat{x})$, as defined in Algorithm 2, evaluating control involves discretizing the flow and constructing current state estimate $\mathhat{x}[n]$ and prediction $\mathhat{x}[n + \hbox{1}]$. We now define the algorithm for computing the full state estimate and prediction, with arguments local state information $(x^{L}, t, {\cal A}_{\bar{t}^{L}}^{L})$, remote state information $(x^{R}, t^{R}, {\cal A}_{\bar{t}^{R}}^{R})$, and prediction time step $\Delta_{P}$. The state estimate is found with $FullStateEstimate$, defined in Algorithm 3, which returns current state estimate $\mathhat{x}[n]$ and state prediction estimate $\mathhat{x}[n + \hbox{1}]$.

The state prediction performed by the estimator is necessary to account for communication delays and avoid control to be evaluated on old information. Communication delay comprises all delay experienced from the instant measurement data are populated onboard the local vehicle until the remote vehicle uses this state information to construct a capture set for control evaluation. This can be broken down into the following major components: 1) ICA application acquisition of state information from the local state estimator; 2) construction of a remote data message as commanded by the ICA application; 3) interface with communication layer Denso WSU radio; 4) physical delay in the wireless transmission of the information; 5) reception of the message from the remote vehicle communication layer; and 6) population of this state information into the ICA application for use in capture set construction and subsequent control evaluation. From experimental results, we have found that the worst case delay is 0.4 s. Hence, the multiple predictions performed to determine $\mathhat{x}[n + \hbox{1}]$ are such that the time $\Delta_{p}\approx \hbox{0.4}$ s.

The set-valued feedback map $g$ is locally computed on each vehicle. To accommodate delay in the system arising from communication, software, and actuators. (As previously discussed, we evaluate the feedback controller for a set of state estimate predictions.) Let state estimate $\mathhat{x}[n]_{i} \subset X$ represent the estimate onboard vehicle $i$ at time $t$. Algorithm 3 can be recursively used to construct more state estimate predictions. Define the prediction horizon count $N_{p} \in \BBN$, which is a configurable design parameter. We construct the state estimate predictions onboard vehicle $i$, given by $\mathhat{x}[n + j]_{i}$ for $\hbox{1} \leq j \leq N_{p}$, as follows: $(\mathhat{x}[n + j]_{i}, \mathhat{x}[n + j - \hbox{1}]_{i}) = \hbox{FullStateEstimate}(\mathhat{x}[n + j - \hbox{1}]_{i}, t + j \Delta_{p}, t^{R} + j \Delta_{p}, \Delta_{p}, {\cal A}_{\bar{t}^{L}}^{L}, {\cal A}_{\bar{t}^{R}}^{R})$, where the local vehicle refers to vehicle $i \in \{\hbox{1}, \hbox{2}\}$. We then use the set of predictions to evaluate feedback map $g$ onboard vehicle $i\in \{\hbox{1}, \hbox{2}\}$, which is implemented as $g(\mathhat{x}[n]_{i}): = \bigcap_{1 \leq j \leq N_{p}}\break \hbox{FeedbackMap}(\mathhat{x}[n + j]_{i}, \mathhat{x}[n]_{i})$.

Before applying control, the two vehicles should reach an agreement for the control commands to apply. In general, we have that $\mathhat{x}[n]_{1} \neq \mathhat{x}[n]_{2}$. However, both sets contain the true system state $x$ by construction. As a consequence, we have that $g(\mathhat{x}[n]_{i})\subseteq g(x)$ given the order reversing property of map $g$. As a consequence, we can take $g(\mathhat{x}[n]_{1})\cup g(\mathhat{x}[n]_{2})$ as the set of all possible safe control choices. In practice, we implement this with a handshake mechanism to guarantee that both vehicles choose the same actions. Specifically, the handshake module remains in the trivial initial state until a collision is predicted onboard the local vehicle. From Algorithm 2, a collision is predicted onboard vehicle $i$ when $g(\mathhat{x}[n]_{i}) \neq {\cal U}$, at which point a message is sent to the remote vehicle indicating a collision has been predicted. Vehicle $i$ Then waits for a message indicating a collision has been predicted onboard the second vehicle $j$. If no such message is received, the application sleeps for 10 ms and then resends the message denoting a collision has been predicted (in case the message was not received). This process continues until a message has been received from vehicle $j$ or it times out. If a message is received, then a consensus control is chosen and applied to the local actuator of both vehicles.

SECTION VI

Experiments were conducted at the test track of the Toyota Technical Center, Toyota Motor Engineering and Manufacturing North America, Inc., Ann Arbor, MI, USA, employing two modified Lexus IS 250 vehicles [see Fig. 1(c)]. Both vehicles run ICA as they approach the intersection. The velocity of approach is not fixed; however, it must be within safe limits. Each path is stored as a list of UTM coordinates on the respective vehicle. The speed limits for path 1 are $v_{\rm min} = \hbox{0}\ \hbox{m/s}$ and $v_{\rm max} = \hbox{8.8}\ \hbox{m/s}$, whereas the speed limits for path 2 are $v_{\rm min} = \hbox{8.8}\ \hbox{m/s}$ and $v_{\rm max} = \hbox{18}\ \hbox{m/s}$. The bad set parameters chosen are $L^{1} = \hbox{55} \ \hbox{m}$, $L^{2} = \hbox{75}\ \hbox{m}$, $H^{1} = \hbox{65}\ \hbox{m}$, and $H^{2} = \hbox{85}\ \hbox{m}$. These values can be changed as they are only input parameters to the algorithm. For the specific implementation, we chose them in such a way that sufficient separation would be maintained by the vehicles when crossing the intersection. The input sets are chosen to be ${\cal U}^{1} := [u^{1}_{L}, u^{1}_{H}] = [\hbox{0}, \hbox{0.3}] \times [ \hbox{0}, \hbox{0.5}]$ and ${\cal U}^{2} := [u^{2}_{L}, u^{2}_{H}] = [\hbox{0}, \hbox{0.3}] \times [\hbox{0}, \hbox{1}]$, which represent extremal inputs that maintain comfortable driving conditions. In general, these are design parameters that engineers have the freedom to change based on road surfaces, vehicle capabilities, and general intersection-dependent considerations. However, these need to remain fixed during the course of an experiment or implementation.

We consider two real-world scenarios, which we refer to as “use cases.” For use case A, we assume that a merging vehicle enters the intersection without properly surveying for oncoming traffic. Since the vehicle has already entered the intersection (or the speed is too high such that this is unavoidable), the only solution is for the merging vehicle to apply throttle and for the straight vehicle to brake. A visualization of this is provided in Fig. 6(a). For use case B, we assume that a merging vehicle is approaching an intersection at high speed and likely misjudging the speed of oncoming traffic. The solution in this case is for the merging vehicle to apply brake while the straight vehicle applies the throttle. A visualization of this is provided in Fig. 6(b). We performed a total of 28 trials, i.e., 15 for use case A and 13 for use case B.

All trajectories generated by the experiments are provided in Fig. 7 in the displacement plane. As is apparent from the plots, no trajectory ever entered the bad set; hence, all collisions were averted. In addition, the trajectories pass fairly close to the bad set, indicating that the control algorithm is nonconservative as expected from theory. To better quantify the performance, we calculated the distance of the trajectory of the system from the capture set, which is denoted by $\gamma$, and the distance of the trajectory from the bad set, which is denoted by $\zeta$. Table I provides the summary of the results. This table shows that the trajectory never entered the capture set nor the bad set in any trial, which follows from the nonzero values of $\wedge\zeta$ and $\wedge\gamma$. This is expected from theory as the controller guarantees that trajectories starting outside of the capture set remain outside of the capture set. Furthermore, the distances of the trajectories from the capture set are very small and can be decreased by decreasing prediction horizon $\Delta_{p}$ and removing state uncertainty $\beta$. Larger prediction horizons lead the system to override sooner, and as a consequence, the distances from the capture set and from the bad set are larger. With no state uncertainty ($\beta = \hbox{0}$), the trajectories pass closer to the capture set and to the bad set, indicating an aggressive and nonconservative controller. When uncertainty is introduced, the distances of the trajectory from the capture set and from the bad set increase because the algorithm applies control to keep an empty intersection between the predicted state uncertainty and the capture set. Hence, our algorithms also provide a number of design parameters to compromise how aggressive the controller is (measured by how close to the bad set the trajectories go) with control conservatism (the controller acts sooner than it could have). This tradeoff is relevant in practice because overriding the driver can be justified only if it is needed to keep the system safe.

Fig. 8 shows an experimental trial with perfect state information $(\beta = \hbox{0})$ and with use case a, whereas Fig. 9 shows a trial for use case b and imperfect state information ($\beta\neq \hbox{0}$). In use case A (see Fig. 8), the merging vehicle (vehicle 1) approached the intersection at a cruising speed of 6 m/s, whereas vehicle 2 approached the intersection at an accelerating speed of around 14 m/s. To avoid the collision, the drivers were overridden at 19.7 s when the state prediction hit the boundary of the capture set. At this time, automatic throttle was applied to vehicle 1, and automatic brake was applied to vehicle 2. This control results in vehicle 2 entering the intersection only (and immediately) after vehicle 1 has cleared the intersection. Vehicle 1 reached the speed limit $v^{1}_{\rm max}$ while applying throttle, after which time the controller held the speed constant. The test ended after the merging vehicle exited the intersection, after which time automatic control was deactivated and the driver retained control. While conducting this experiment, system trajectory $\mathhat{x}(t)$ was at least within 0.7 m of the capture set, while never actually entering it, which implies that safety was maintained and that the control actions were not conservative.

In use case B (see Fig. 9), imperfect state information was considered using $\beta = \hbox{0.2}\ \hbox{m/s}^{2}$. In this trial, the merging vehicle (vehicle 1) started at rest, whereas vehicle 2 approached the intersection at an accelerating speed of around 8 m/s. Vehicle 1 attempted to violently accelerate and enter the intersection. To avoid the collision, the drivers were overridden at 47.2 s when the set prediction hit the boundary of the capture set. In this case, automatic brake was applied to vehicle 1, and automatic throttle was applied to vehicle 2. This control results in vehicle 1 entering the intersection only (and immediately) after vehicle 2 has cleared the intersection. The merging vehicle reached the speed limit $v^{1}_{\rm min}$ while applying brake, after which time the controller held the vehicle at rest. The straight vehicle reached the speed limit $v^{2}_{\rm max}$ while applying throttle, after which time the controller held the vehicle at a constant speed. The test ended after the straight vehicle exited the intersection, after which time automatic control was deactivated and the driver retained longitudinal control. While conducting this experiment, system trajectory $\mathhat{x}(t)$ was within 0.6 m of the capture set, while never actually entering it, which implies that safety was maintained and that the control actions were not conservative.

SECTION VII

In this paper, we have presented algorithms and experimental validation on prototype vehicles for cooperative collision avoidance at intersections based on a formal control theoretic approach. Since the application considered is life critical, algorithms for collision avoidance should have safety certificates. The proposed approach provides these certificates, guaranteeing that the system stays collision free and that automatic control is not applied until absolutely necessary. This is achieved by keeping the system state always outside the capture set, which is the set of all states from which a collision is unavoidable given the vehicle dynamics and the limitations on the control efforts. A number of parameters can be chosen by the designer, including the maximal and minimal brake and throttle efforts for automatic control, maximal and minimal speeds, the size of the collision set (bad set), the bounds on the modeling uncertainty, the communication delay, and the bounds on the uncertainty on the driver control actions. For example, if acceleration is not considered suitable for preventing a collision, one can set the upper and lower bounds of the throttle input to zero in the calculation of the capture set and the control map, so that evasive maneuvers will only consider braking. Of course, the control action will be more conservative in this case as the capture set will be larger. Similarly, the size of the bad set is an input parameter to the algorithm, and it can be changed by the user depending on the specific intersection geometry. Experimentally, we have shown how to tune the prediction horizon and the number of prediction steps to adjust conservatism, that is, how soon the controller decides that automatic control is needed to prevent an imminent collision. The later the automatic control acts, the less conservative the algorithm is, but the closer the system trajectories come to a collision (while still averting it). This tradeoff can be decided depending on the system specifications. The experiments finally illustrate that the (linear complexity) algorithms for evaluating the capture set and control actions are fast enough for real-time implementation, which is a feature that is necessary for the practical applicability of our approach. A number of future research avenues are left to be explored. These include incorporating a warning phase that gives the opportunity to the driver to react before automatic control becomes necessary. Scalability to more than two vehicles needs to be studied, and initial results are promising [8]. Our approach can be applied where vehicles are on known crossing or merging paths, such as at intersections or when a vehicle merges onto a road from a parking lot or on the highway. Investigation should be carried out to extend the approach to road topologies other than intersections and merges and to situations where intended vehicle paths and collision zones cannot be identified a priori.

The Associate Editor for this paper was F.-Y. Wang.

M. R. Hafner is with the Systems Laboratory, University of Michigan, Ann Arbor, MI 48109 USA.

D. Cunningham and L. Caminiti are with the Integrated Vehicle Systems Department, Toyota Motor Engineering and Manufacturing North America, Inc., Erlanger, KY 41018 USA (e-mail: drew.cunningham@tema.toyota.com; lorenzo.caminiti@tema.toyota.com).

D. Del Vecchio is with the Department of Mechanical Engineering and the Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge, MA 02139-4307 USA.

Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org.

No Data Available

No Data Available

None

No Data Available

- This paper appears in:
- No Data Available
- Issue Date:
- No Data Available
- On page(s):
- No Data Available
- ISSN:
- None
- INSPEC Accession Number:
- None
- Digital Object Identifier:
- None
- Date of Current Version:
- No Data Available
- Date of Original Publication:
- No Data Available

Normal | Large

- Bookmark This Article
- Email to a Colleague
- Share
- Download Citation
- Download References
- Rights and Permissions