Robust Sequential Path Planning Under Disturbances and Adversarial Intruder
Provably safe and scalable multi-vehicle path planning is an important and urgent problem due to the expected increase of automation in civilian airspace in the near future. Although this problem has been studied in the past, there has not been a method that guarantees both goal satisfaction and safety for vehicles with general nonlinear dynamics while taking into account disturbances and potential adversarial agents, to the best of our knowledge. Hamilton-Jacobi (HJ) reachability is the ideal tool for guaranteeing goal satisfaction and safety under such scenarios, and has been successfully applied to many small-scale problems. However, a direct application of HJ reachability in most cases becomes intractable when there are more than two vehicles due to the exponentially scaling computational complexity with respect to system dimension. In this paper, we take advantage of the guarantees HJ reachability provides, and eliminate the computation burden by assigning a strict priority ordering to the vehicles under consideration. Under this sequential path planning (SPP) scheme, vehicles reserve “space-time” portions in the airspace, and the space-time portions guarantee dynamic feasibility, collision avoidance, and optimality of the paths given the priority ordering. With a computation complexity that scales quadratically when accounting for both disturbances and an intruder, and linearly when accounting for only disturbances, SPP can tractably solve the multi-vehicle path planning problem for vehicles with general nonlinear dynamics in a practical setting. We demonstrate our theory in representative simulations.
Recently, there has been an immense surge of interest in the use of unmanned aerial systems (UASs) in urban environments. UASs have great potential in civil applications such as package delivery, aerial surveillance, disaster response, among many others [1, 2, 3, 4, 5]. Unlike previous uses of UASs for military purposes, civil applications will involve unmanned aerial vehicles (UAVs) flying in urban environments, potentially in close proximity of humans, other UAVs, and other important assets. As a result, government agencies such as the Federal Aviation Administration (FAA) and National Aeronautics and Space Administration (NASA) of the United States are urgently trying to develop new scalable ways to organize an airspace in which potentially thousands of UAVs can fly simultaneously in the same region [6, 7].
One essential problem that needs to be addressed is the safe multi-vehicle path planning problem: how a group of vehicles in the same vicinity can reach their destinations while avoiding situations which are considered dangerous, such as collisions. In many previous studies that address this problem, specific control strategies for the vehicles are assumed, and approaches such as those involving induced velocity obstacles [8, 9, 10, 11] and involving virtual potential fields to maintain collision [12, 13] have been used. Other analyses of multi-vehicle systems include methods for real-time trajectory generation , for path planning for vehicles with linear dynamics in the presence of obstacles with known motion , and for cooperative path planning via waypoints which do not account for vehicle dynamics . Other related work include those which consider only the collision avoidance problem without path planning. These results include those that assume the system has a linear model [17, 18, 19], rely on a linearization of the system model [20, 21], assume a simple positional state space , and many others [23, 24, 25].
However, the capability to flexibly plan provably safe and dynamically feasible trajectories without making strong assumptions on the vehicles’ dynamics and other vehicles’ motion is essential for dense groups of UAVs to safely fly in each other’s vicinity. In addition, in a practical setting, any path planning scheme that also addresses collision avoidance must guarantee both goal satisfaction and safety of UAVs despite disturbances such as weather effects and communication faults . Furthermore, unexpected scenarios such as UAV malfunctions or even UAVs with malicious intent need to be accounted for.
The problem of trajectory planning and collision avoidance under disturbances in safety-critical systems has been studied using Hamilton-Jacobi (HJ) reachability analysis, which provides guarantees on goal satisfaction and safety of optimal system trajectories [26, 27, 28, 29, 30, 31]. Reachability-based methods are particular suitable in the context of UAVs because of the hard guarantees that are provided. In reachability analysis, one computes the reachable set, defined as the set of states from which the system can be driven to a target set. Many numerical tools are available for computing various definitions of reachable sets [32, 33, 34, 35], and reachability analysis has been successfully used in applications involving systems with no more than two vehicles, such as pairwise collision avoidance , automated in-flight refueling , and many others [37, 38].
One of the main challenges of managing the next generation of airspace is the density of vehicles that needs to be accommodated . Such a large-scale system has a high-dimensional joint state space, making a direct application of dynamic programming-based approaches such as reachability analysis intractable. In particular, reachable set computations involve solving a HJ partial differential equation (PDE) or variational inequality (VI) on a grid representing a discretization of the state space, causing computational complexity to scale exponentially with system dimension.
I-a Contributions and Outline
In this paper, we propose the sequential path planning (SPP) method to tackle the multi-vehicle path planning problem. Our approach is similar to the approaches of [39, 40], but provides hard guarantees on both the goal satisfaction and safety of all vehicles even in the presence of disturbances and a single intruder vehicle that could potentially be adversarial. In addition, our method scales only linearly with the number of vehicles when there is no intruder, and quadratically with the number of vehicles when there is a single intruder. On a high level, the SPP method assigns a strict priority ordering to the vehicles under consideration. Higher-priority vehicles plan their paths without taking into account the lower-priority vehicles. Lower-priority vehicles treat higher-priority vehicles as moving obstacles. Under this assumption, time-varying formulations of reachability [29, 31] can be used to obtain the optimal and provably safe paths for each vehicle, starting from the highest-priority vehicle. Thus, the curse of dimensionality is overcome for the multi-vehicle path planning problem at the cost of a mild structural assumption.
In a sense, the SPP method reserves a portion of “space-time” in the airspace for each vehicle. The reserved space-time portion is recorded so that lower-priority vehicles can take it into account. Besides planning around the reserved space-time portions of higher-priority vehicles, no other communication between the vehicles is needed at execution time, even when disturbances and an intruder are present.
In the absence of disturbances and intruders, and assuming each vehicle has perfect information about other vehicles’ positions, each vehicle may plan and commit to an exact trajectory, with the reserved space-time being the collision set around the trajectory at every point in time. This basic concept of SPP is formally presented in Section IV.
When the vehicles are affected by disturbances, exact trajectories cannot be known a priori, and thus the basic SPP algorithm cannot be directly applied. Fortunately, reachability analysis allows us to determine, at no additional computation cost, all possible states of each vehicle over time under the worst-case disturbance, given a control strategy. In addition, we can also determine suitable portions of space-time for each vehicle depending on the available information about the control strategies of higher-priority vehicles. SPP under disturbances and three different assumptions on the information available about the control strategy of other vehicles is formally presented in Section V.
In scenarios where there could potentially be single, possibly adversarial intruder in the airspace, each vehicle needs extra space around other vehicles in order to be able to perform avoidance maneuvers. Assuming the intruder may be present for some maximum duration, we use use reachability analysis to determine precisely the amount of space-time needed for each vehicle to be able to avoid the intruder under the presence of disturbances, making our proposed method sufficiently robust to most practical scenarios. SPP in the presence of a single intruder is formally presented in Section VI.
Ii Problem Formulation
Consider vehicles which participate in the SPP process and denote these vehicles as the SPP vehicles . We assume their dynamics are given by
where represents the state of vehicle , the control of , and the disturbance experienced by . For convenience, we partition the state into the position component and the non-position component : .
We assume that the control functions are drawn from the set of measurable functions111A function between two measurable spaces and is said to be measurable if the preimage of a measurable set in is a measurable set in , that is: , with -algebras on ,.. For convenience, we will use the sets to respectively denote the set of functions from which the control and disturbance functions are drawn.
In addition, we assume that the disturbances are drawn the set of non-anticipative strategies  , defined as follows:
Each vehicle has initial state , and aims to reach its target by some scheduled time of arrival . The target in general represents some set of desirable states, for example the destination of . In some situations, we may find that it is infeasible for to get to at or before . Whenever unsure, we may first determine the earliest feasible as described in Section VI.
On its way to , must avoid a set of static obstacles . The interpretation of could be a tall building, a region around an airport, or any set of states that are forbidden for each SPP vehicle.
In addition to the static obstacles, each vehicle must also avoid the danger zones with respect to every other vehicle . The danger zones in general can represent any joint configurations between and that are considered to be unsafe. In this paper, we define the danger zone of with respect to to be
whose interpretation is that and are considered to be in an unsafe configuration when they are within a distance of of each other. For concreteness, we will call the collision set, and if , then and are said to have collided.
Given the set of SPP vehicles, their targets , the static obstacles , and the vehicles’ danger zones with respect to each other , we would like, for each vehicle , to synthesize a controller which guarantees that reaches its target at or before the scheduled time of arrival , while avoiding the static obstacles as well as the danger zones with respect to all other vehicles . In addition, we would like to obtain the latest departure time such that can still arrive at on time.
In general, the above optimal path planning problem must be solved in the joint space of all SPP vehicles. However, due to the high joint dimensionality, a direct dynamic programming-based solution is intractable. Therefore, we propose to assign a priority to each vehicle, and perform SPP given the assigned priorities. Without loss of generality, let have a higher priority than if . Under the SPP scheme, higher-priority vehicles can ignore the presence of lower-priority vehicles, and perform path planning without taking into account the lower-priority vehicles’ danger zones. A lower-priority vehicle , on the other hand, must ensure that it does not enter the danger zones of the higher-priority vehicles ; each higher-priority vehicle induces a set of time-varying obstacles , which represents the possible states of such that a collision between and could occur.
It is straight-forward to see that if each vehicle is able to plan a trajectory that takes it to while avoiding the static obstacles and the danger zones of higher-priority vehicles , then the set of SPP vehicles would all be able to reach their targets safely. With the SPP scheme, the additional structure provided by the vehicle priorities allows us to reduce the complexity of the joint path planning problem. As we will see, under the SPP scheme, path planning can be done sequentially in descending order of vehicle priority in the state space of only a single vehicle. Thus, SPP provides a solution whose complexity scales linearly with the number of vehicles in the presence of disturbances, as opposed to exponentially with a direct application of dynamic programming approaches. In the presence of a single intruder, the computation complexity scaling becomes quadratic.
In the following sections, we will explore SPP under different assumptions. We begin with the basic SPP algorithm in which disturbances are ignored and perfect information of vehicles’ positions is assumed. This simplification allows us to clearly establish the basic SPP algorithm. Next, we show how the basic SPP approach can be made robust to disturbances as well as an imperfect knowledge of other vehicles’ positions. Finally, we further robustify the SPP approach by considering how the set of SPP vehicles may respond to the presence of an intruder vehicle which may be adversarial. All of our methods use time-varying reachability analysis to provide goal satisfaction and safety guarantees.
Iii Time-Varying Reachability Background
We will be using reachability analysis to compute either a backward reachable set (BRS) , a forward reachable set (FRS) , or a sequence of BRSs and FRSs, given some target set , time-varying obstacle , and the Hamiltonian function which captures the system dynamics as well as the roles of the control and disturbance. The BRS in a time interval or FRS in a time interval will be denoted by
In the SPP scheme, a lower-priority vehicle must avoid a set of moving obstacles on its way to the target. Several formulations of reachability are able to perform optimal path planning with hard guarantees on safety and performance under disturbances in such a scenario [29, 31]. For our application in SPP, we utilize the time-varying formulation in , which accounts for the time-varying nature of systems without requiring augmentation of the state space with the time variable. In the formulation in , a BRS is computed by solving the following final value double-obstacle HJ VI:
In a similar fashion, the FRS is computed by solving the following initial value HJ PDE:
In both (5) and (6), the function is the implicit surface function representing the target set . Similarly, the function is the implicit surface function representing the time-varying obstacles . The BRS and FRS are given by
Some of the reachability computations will not involve an obstacle set , in which case we can simply set which effectively means that the outside maximum is ignored in (5). Also, note that unlike in (5), there is no inner minimization in (6). As we will see later, we will be using the BRS to determine all states that can reach some target set within the time horizon , whereas we will be using the FRS to determine where a vehicle could be at some particular time . In addition, (6) has no outer maximum, since the FRSs that we will compute will not involve any obstacles.
The Hamiltonian, , depends on the system dynamics, and the role of control and disturbance. Whenever does not depend explicit on , we will drop the argument. In addition, the Hamiltonian is an optimization that produces the optimal control and optimal disturbance , once is determined. For BRSs, whenever the existence of a control (“”) or disturbance is sought, the optimization is a minimum over the set of controls or disturbance. Whenever a BRS characterizes the behavior of the system for all controls (“”) or disturbances, the optimization is a maximum. We will introduce precise definitions of reachable sets, expressions for the Hamiltonian, expressions for the optimal controls as needed for the many different reachability calculations we use.
Iv SPP Without Disturbances and With Perfect Information
In this section, we introduce the basic SPP algorithm assuming that there is no disturbance affecting the vehicles, and that each vehicle knows the exact position of higher-priority vehicles. Although in practice, such assumptions do not hold, the basic SPP algorithm can still serve as a useful approximation in certain situations. In addition, the description of the basic SPP algorithm will introduce the notation needed for describing the subsequent, more realistic versions of SPP. We also show simulation results for the basic SPP algorithm. The majority of the content in this section is taken from .
Recall that the SPP vehicles , are each assigned a strict priority, with having a higher priority than if . In the absence of disturbances, we can write the dynamics of the SPP vehicles as
In SPP, each vehicle plans the path to its target set while avoiding static obstacles and the obstacles induced by higher-priority vehicles . Path planning is done sequentially starting from the first vehicle and proceeding in descending priority, so that each of the path planning problems can be done in the state space of only one vehicle. During its path planning process, ignores the presence of lower-priority vehicles , and induces the obstacles for .
From the perspective of , each of the higher-priority vehicles induces a time-varying obstacle denoted that needs to avoid222Note that the index in denotes vehicles with lower priority than , and the index in denotes vehicles with higher priority than .. Therefore, each vehicle must plan its path to while avoiding the union of all the induced obstacles as well as the static obstacles. Let be the union of all the obstacles that must avoid on its way to :
With full position information of higher priority vehicles, the obstacle induced for by is simply
Each higher priority vehicle plans its path while ignoring . Since path planning is done sequentially in descending order or priority, the vehicles would have planned their paths before does. Thus, in the absence of disturbances, is a priori known, and therefore are known, deterministic moving obstacles, which means that is also known and deterministic. Therefore, the path planning problem for can be solved by first computing the BRS , defined as follows:
The BRS can be obtained by solving (5) with , , and the Hamiltonian
The optimal control for reaching while avoiding is then given by
from which the trajectory can be computed by integrating the system dynamics, which in this case are given by (8). In addition, the latest departure time can be obtained from the BRS as . In summary, the basic SPP algorithm is given as follows:
Basic SPP algorithm: Suppose we are given initial conditions , vehicle dynamics (8), target sets , and static obstacles . For each in ascending order starting from (which corresponds to descending order of priority),
Iv-B Numerical Simulations
We now illustrate the basic SPP algorithm using a four-vehicle example. In this example, we will use the following dynamics for each vehicle:
where is the state of vehicle , is the position, is the heading, is the speed, and is the turn rate. In this example, we assume that the vehicles have constant speed , and that the control of each vehicle is given by with . We have chosen these dynamics for clarity of illustration; the SPP algorithm can handle more general systems of the form in which the vehicles have different control bounds and dynamics.
For this example, the target sets of the vehicles are circles of radius in the position space; each vehicle is trying to reach some desired set of positions. In terms of the state space , the target sets are defined as
where are centers of the target circles. For the simulation of the basic SPP algorithm, we used . The vehicles have target centers , initial conditions , and scheduled times of arrivals as follows:
The setup for this example is shown in Fig. 1, which also shows the static obstacles as the black rectangles around the center of the domain.
The joint state space of this four-vehicle system is twelve-dimensional (12D), making the joint path planning and collision avoidance problem intractable for direct analysis using HJ reachability. Therefore, we apply the SPP algorithm described in Algorithm 1 and repeatedly solve the double-obstacle HJ VI in (5) to obtain the optimal control for each vehicle to reach its target while avoiding higher-priority vehicles. In addition, due to the flexibility of the HJ VI with respect to time-varying systems, the different scheduled times of arrival can be trivially incorporated.
Fig. 2, 3, and 4 show the simulation results. Since the state space of each vehicle is 3D, each of the BRSs is also 3D. To visualize the results, we slice the BRSs at the initial heading angles . Fig. 2 shows the 2D BRS slices for each vehicle at its latest departure times determined from our method. The obstacles in the domain and the obstacles induced by higher-priority vehicles inhibit the evolution of the BRSs, carving out thin “channels” that separate the BRSs into different “islands”. One can see how these “channels” and “islands” form by examining the time evolution of the BRS, shown in Fig. 3 for vehicle .
Finally, Fig. 4 shows the resulting trajectories of the four vehicles. Most interestingly, the subplot labeled shows all four vehicles in close proximity without collision: each vehicle is outside of the danger zone of all other vehicles (although the danger zones may overlap). This close proximity is an indication of the optimality of the basic SPP algorithm given the assigned priority ordering. Since no disturbances are present, getting as close to other vehicles’ danger zones as possible without entering the danger zones intuitively results in short transit times.
The actual arrival times of vehicles are , respectively. It is interesting to note that for some vehicles, the actual arrival times are earlier than the scheduled times of arrivals . This is because in order to arrive at the target by , these vehicles must depart early enough to avoid major delays resulting from the induced obstacles of other vehicles; these delays would have led to a late arrival if vehicle departed after .
V SPP With Disturbances and Incomplete Information
Disturbances and incomplete information significantly complicate the SPP scheme. The main difference is that the vehicle dynamics satisfy (1) as opposed to (8). Committing to exact trajectories is therefore no longer possible, since the disturbance is a priori unknown. Thus, the induced obstacles are no longer just the danger zones centered around positions.
We present three methods to address the above issues. The methods differ in terms of control policy information that is known to a lower-priority vehicle, and have their relative advantages and disadvantages depending on the situation. The three methods are as follows:
Centralized control: A specific control strategy is enforced upon a vehicle; this can be achieved, for example, by some central agent such as an air traffic controller.
Least restrictive control: A vehicle is required to arrive at its target on time, but has no other restrictions on its control policy. When the control policy of a vehicle is unknown, but its timely arrive at its target can be assumed, the least restrictive control can be safely assumed by lower-priority vehicles.
Robust trajectory tracking: A vehicle declares a nominal trajectory which can be robustly tracked under disturbances.
In general, the above methods can be used in combination in a single path planning problem, with each vehicle independently having different control policies. Lower-priority vehicles would then plan their paths while taking into account the control policy information known for each higher-priority vehicle. For clarity, we will present each method as if all vehicles are using the same method of path planning.
In addition, for simplicity of explanation, we will assume that no static obstacles exist. In the situations where static obstacles do exist, the time-varying obstacles simply become the union of the induced obstacles in (10) and the static obstacles. The material in this section is taken partially from .
V-A1 Centralized Control
The highest-priority vehicle first plans its path by computing the BRS (with )
Since we have assumed no static obstacles exist, we have that for , and thus the above BRS is well-defined. This BRS can be computed by solving the HJ VI (5) with the following Hamiltonian:
From the BRS, we can obtain the optimal control
Here, as well as in the other two methods, the latest departure time is then given by .
If there is a central agent directly controlling each of the vehicles, then the control law of each vehicle can be enforced. In this case, lower-priority vehicles can safely assume that higher-priority vehicles are applying the enforced control law. In particular, the optimal controller for getting to the target, , can be enforced. In this case, the dynamics of each vehicle becomes
where no longer appears explicitly in the dynamics.
From the perspective of a lower-priority vehicle , a higher-priority vehicle induces a time-varying obstacle that represents the positions that could possibly be within the collision radius of under the dynamics . Determining this obstacle involves computing an FRS of starting from333In practice, we define the target set to be a small region around the vehicle’s initial state for computational reasons. . The FRS is defined as follows:
This FRS can be computed using (6) with the Hamiltonian
The FRS represents the set of possible states at time of a higher-priority vehicle given all possible disturbances and given that uses the feedback controller . In order for a lower-priority vehicle to guarantee that it does not go within a distance of to , must stay a distance of at least away from the FRS for all possible values of the non-position states . This gives the obstacle induced by a higher-priority vehicle for a lower-priority vehicle as follows:
where the set is the set of states in the FRS projected onto the states representing position , and disregarding the non-position dimensions :
Finally, taking the union of the induced obstacles as in (9) gives us the time-varying obstacles needed to define and determine the BRS in (17). Repeating this process, all vehicles will be able to plan paths that guarantee the vehicles’ timely and safe arrival. The centralized control algorithm can be summarized as follows:
Centralized control algorithm: Given initial conditions , vehicle dynamics (1), target set , and static obstacles , for each ,
compute the BRS defined in (17). The latest departure time is then given by ;
V-A2 Least Restrictive Control
Here, we again begin with the highest-priority vehicle planning its path by computing the BRS in (17). However, if there is no centralized controller to enforce the control policy for higher-priority vehicles, weaker assumptions must be made by the lower-priority vehicles to ensure collision avoidance. One reasonable assumption is that all higher-priority vehicles follow the least restrictive control that would take them to their targets. This control would be given by
Such a controller allows each higher-priority vehicle to use any controller it desires, except when it is on the boundary of the BRS, , in which case the optimal control given by (19) must be used to get to the target safely and on time. This assumption is the weakest assumption that could be made by lower-priority vehicles given that the higher-priority vehicles will get to their targets on time.
Suppose a lower-priority vehicle assumes that higher-priority vehicles use the least restrictive control strategy in (26). From the perspective of the lower-priority vehicle , a higher-priority vehicle could be in any state that is reachable from ’s initial state and from which the target can be reached. Mathematically, this is defined by the intersection of an FRS from the initial state and the BRS defined in (17) from the target set , . In this situation, since cannot be assumed to be using any particular feedback control, is defined as
This FRS can be computed by solving (6) with the Hamiltonian
In turn, the obstacle induced by a higher-priority for a lower-priority vehicle is as follows:
The least restrictive control method can be summarized as follows:
Least restrictive control algorithm: Given initial conditions , vehicle dynamics (1), target set , and static obstacles , for each ,
The centralized control method described in the previous section can be thought of as the “most restrictive control” method, in which all vehicles must use the optimal controller at all times, while the least restrictive control method allows vehicles to use any suboptimal controller that allows them to arrive at the target on time. These two methods can be considered two extremes of a spectrum in which varying degrees of optimality is assumed for higher-priority vehicles. Vehicles can also choose a control strategy in the middle of the two extremes, and for example use a control within some range around the optimal control, or use the optimal control unless some condition is met. The induced obstacles and the BRS can then be similarly computed using the corresponding control strategy.
V-A3 Robust Trajectory Tracking
Even though it is impossible to commit to and track an exact trajectory in the presence of disturbances, it may still be possible to instead robustly track a feasible nominal trajectory with a bounded error at all times. If this can be done, then the tracking error bound can be used to determine the induced obstacles. Here, computation is done in two phases: the planning phase and the disturbance rejection phase. In the planning phase, we compute a nominal trajectory that is feasible in the absence of disturbances. In the disturbance rejection phase, we compute a bound on the tracking error.
It is important to note that the planning phase does not make full use of a vehicle’s control authority, as some margin is needed to reject unexpected disturbances while tracking the nominal trajectory. Therefore, in this method, planning is done for a reduced control set . The resulting trajectory reference will not utilize the vehicle’s full control capability; additional maneuverability is available at execution time to counteract external disturbances.
In the disturbance rejection phase, we determine the error bound independently of the nominal trajectory. To compute this error bound, we find a robust controlled-invariant set in the joint state space of the vehicle and a tracking reference that may “maneuver” arbitrarily in the presence of an unknown bounded disturbance. Taking a worst-case approach, the tracking reference can be viewed as a virtual evader vehicle that is optimally avoiding the actual vehicle to enlarge the tracking error. We therefore can model trajectory tracking as a pursuit-evasion game in which the actual vehicle is playing against the coordinated worst-case action of the virtual vehicle and the disturbance.
Let and denote the states of the actual vehicle and the virtual evader, respectively, and define the tracking error . When the error dynamics are independent of the absolute state as in (32) (and also (7) in ), we can obtain error dynamics of the form
To obtain bounds on the tracking error, we first conservatively estimate the error bound around any reference state , denoted :
where denotes the position coordinates of and is a design parameter. We next solve a reachability problem with its complement , the set of tracking errors violating the error bound, as the target in the space of the error dynamics. From , we compute the following BRS:
where the Hamiltonian to compute the BRS is given by:
Letting , we obtain the infinite-horizon control-invariant set . If is nonempty, then the tracking error at flight time is guaranteed to remain within provided that the vehicle starts inside and subsequently applies the feedback control law
The induced obstacles by each higher-priority vehicle can thus be obtained by:
where the “” in (37) denotes the Minkowski sum444The Minkowski sum of sets and is the set of all points that are the sum of any point in and .. Intuitively, if is tracking , then it will remain within the error bound around . This is precisely the set . The induced obstacles can then be obtained by augmenting a danger zone around this set. Finally, we can obtain the total obstacle set using (9).
Since each vehicle , , can only be guaranteed to stay within , we must make sure during the path planning of that at any given time, the error bounds of and , and , do not intersect. This can be done by augmenting the total obstacle set by :
Finally, given , we can guarantee that will reach its target if ; thus, in the path planning phase, we modify to be , and compute a BRS, with the control authority , that contains the initial state of the vehicle. Mathematically,
The BRS can be obtained by solving (5) using the Hamiltonian:
The corresponding optimal control for reaching is given by:
The nominal trajectory can thus be obtained by using vehicle dynamics (8), with the optimal control given by (41). From the resulting nominal trajectory , the overall control policy to reach can be obtained via (36). The robust trajectory tracking method can be summarized as follows:
Robust trajectory tracking algorithm: Given initial conditions , vehicle dynamics (1), target sets , and static obstacles , for each ,
decide on a reduced control authority for the planning phase, and choose a parameter to conservatively bound the tracking error;
compute the BRS using (34) and make sure that