Safe Returning FaSTrack With Robust Control Lyapunov-Value Functions

Real-time navigation in a priori unknown environment remains a challenging task, especially when an unexpected (unmodeled) disturbance occurs. In this letter, we propose the framework Safe Returning Fast and Safe Tracking (SR-F) that merges concepts from 1) Robust Control Lyapunov-Value Functions (R-CLVF) 1, and 2) the Fast and Safe Tracking (FaSTrack) framework 2. The SR-F computes an R-CLVF offline between a model of the true system and a simplified planning model. Online, a planning algorithm is used to generate a trajectory in the simplified planning space, and the R-CLVF is used to provide a tracking controller that exponentially stabilizes to the planning model. When an unexpected disturbance occurs, the proposed SR-F algorithm provides a means for the true system to recover to the planning model. We take advantage of this mechanism to induce an artificial disturbance by “jumping” the planning model in open environments, forcing faster navigation. Therefore, this algorithm can both reject unexpected true disturbances and accelerate navigation speed. We validate our framework using a 10D quadrotor system and show that SR-F is empirically 20% faster than the existing works while maintaining safety.

SECTION I. Introduction

Safe control for autonomous systems is a challenging task, particularly for dynamic systems navigating through a priori unknown environments. For computational efficiency, many algorithms use a simplified (often kinematic) model of the system to generate a path around obstacles to a goal. A more complex model representing the true robot is then used to track this path. Popular path planning algorithms include Dijkstra’s [3], A$^{*}$ [4], Rapidly Exploring Random Trees (RRT) [5] and heuristic-based methods [6], [7]. The tracking controller can be generated using, for example, model predictive control (MPC) [8], [9], or control Lyapunov functions (CLFs) [10], [11]. For safety, control barrier functions (CBFs) [12] or Hamilton Jacobi (HJ) reachability analysis [13], [14] can generate safety filters for the controller.

Since planning is typically done with a simplified model of the system, the path might not be feasible and safe for the actual robot to track. To address this issue, [15], [16] directly adds a CBF and CLF as a constraint in the path planning algorithm. Reference [17] uses a reference governor control design wherein a robot with a specific form of dynamics can safely stabilize to a moving equilibrium point. Integrated planning and control (IPC) uses the notion of a safe flight corridor (SFC) as a safety constraint in nonlinear MPC to guarantee safe navigation [18], [19], and shows empirical robustness to sudden disturbances.

Fast and Safe Tracking (FaSTrack) [2] is a modular framework that separates the navigation task into independent planning and tracking tasks (with corresponding planner and tracker models of the autonomous system). Offline, HJ reachability is used to precompute a tracking error bound (TEB) on the maximum deviation that the true tracker model may take from the planner model (Fig. 1). This is paired with an optimal tracking controller that maintains this error bound regardless of the planning algorithm used by the planner. Online, the obstacles are augmented with TEB and the planning algorithm provides a path in the low-dimensional planning space around the augmented obstacles. The tracking controller guarantees the distance between the tracker and the path is contained in the TEB, preserving safety.

Fig. 1.

Comparison of the relative trajectory using the original FaSTrack framework (left) and our proposed SR-F (right). The red dotted line denotes an unexpected disturbance that causes the relative state to leave the minimum tracking error bound (TEB). FaSTrack can only guarantee the relative state stays in the larger error bound, while the SR-F can stabilize the relative state back to the TEB.

While several of the above approaches can handle predefined disturbance bounds, they are not designed to maintain safety when experiencing a sudden disturbance beyond the expected bounds. In this letter, we modify the FaSTrack framework and propose the novel Safe Returning FaSTrack (SR-F) framework. The main contributions are as follows:

1. We introduce the SR-F, where a CLF-like function in the relative space between the tracker and planner is computed offline, and a new safe returning mechanism is used to accommodate unexpected disturbances. We prove (under mild assumptions) that the SR-F can maintain safety under unexpected disturbances.

2. We take advantage of this robustness to sudden disturbances by methodically introducing an artificial sudden disturbance by “jumping” the planner towards the goal, forcing the autonomous system to speed up in open environments while maintaining safety.

3. We compare SR-F with existing work [19], [20] on 8D and 10D quadrotor navigation tasks that are subjected to sudden high wind gusts. We show the SR-F can maintain safety when unexpected disturbance happens, and outperforms existing methods on navigation speed.

SECTION II. Background

We consider three models: (1) a tracker model that represents the true robot, (2) a planner model that is designed by the user for path planning, and (3) a relative model used to guarantee safety.

1) Tracker Model: The tracker model is given by the following nonlinear ordinary differential equation:\begin{equation*} \frac {d x}{d s} = \dot x = f\left ({{x,u,d}}\right ), x(t) = x_{0}, s\in \left [{{t,0}}\right ], \tag {1}\end{equation*} where s is the time, $x\in {\mathcal{X}}\subseteq \mathbb {R}^{n} $ is the tracker state, $u\in {\mathcal{U}}_{s}\subseteq \mathbb {R}^{m}$ is the control input, and $d\in {\mathcal{D}}\subseteq \mathbb {R}^{d}$ is the disturbance. Assume the dynamics $f~:~{\mathcal{X}}\times {\mathcal{U}}_{s}\times {\mathcal{D}}\mapsto {\mathcal{X}}$ is Lipschitz continuous in x for fixed $u, d$ . Assume the control and disturbance signal $u(\cdot)$ , $d(\cdot)$ are measurable functions:\begin{align*} u(\cdot ) \in \mathbb {U}_{s}\mathrel {\mathrel {\mathop :}\hspace {-0.0672em}=}& \{ u~:~\left [{{t,0}}\right ] \mapsto {\mathcal{U}}_{s}, u(\cdot )~\text {is measurable}\}, \\ d(\cdot ) \in \mathbb {D}\mathrel {\mathrel {\mathop :}\hspace {-0.0672em}=}& \{ d~:~\left [{{t,0}}\right ] \mapsto {\mathcal{D}}, d(\cdot )~\text {is measurable}\},\end{align*} where ${\mathcal{U}}_{s}$ and ${\mathcal{D}}$ are compact sets. Under these assumptions, we can solve for a unique solution of (1), denoted as $\xi _{f }(s;t,x,u(\cdot),d(\cdot))$ . Denote ${\mathcal{G}}\subset {\mathcal{X}}$ the goal set, and ${\mathcal{C}}\subset {\mathcal{X}}$ the constraint set, i.e., the set of states that we want to avoid.

2) Planner Model: The planner model is given by:\begin{equation*} \frac {d p}{d s} = \dot p = h\left ({{p, u_{p}}}\right ), p(t) = p_{0},\end{equation*} where $p\in {\mathcal{P}}\subseteq \mathbb {R}^{p}$ is the planner state, $u_{p}\in {\mathcal{U}}_{p}$ is the planner control. Further, assume that ${\mathcal{P}}$ is a subspace of ${\mathcal{X}}$ and we make analogous assumptions on the planner model dynamics as for (1) to guarantee a unique solution.

The goal and constraint sets in the planner space are denoted as ${\mathcal{G}}_{p}\subset {\mathcal{P}}$ , ${\mathcal{C}}_{p}\subset {\mathcal{P}}$ respectively.

3) Relative Dynamics: Define the relative state \begin{equation*} r= \Phi \left ({{x,p}}\right ) \left ({{x- Qp }}\right ), \tag {2}\end{equation*} where $r\in {\mathcal{R}}\in \mathbb {R}^{n}$ , Q augments the planner state and $\Phi $ is a linear map so that the dynamics can be written as \begin{equation*} \dot r = g\left ({{r,u,u_{p},d}}\right ). \tag {3}\end{equation*} The existence of Q and $\Phi $ are justified in [2]. From the assumption of the tracker and planner model, the relative dynamics also admits unique solution $\xi (s;t,r,u(\cdot),u_{p}(\cdot),d(\cdot))$ . Denote the error states between tracker and planner as e and the rest as $\eta $ , i.e., $r= [e,\eta]$ .

A. HJ Reachability and Fastrack

The FaSTrack framework contains two parts: offline computation and online execution. The offline part uses the HJ reachability to generate the TEB, which is a robust control invariant set. Online, it senses the environment, augments the obstacles with the TEB, and then plans and tracks a path around the augmented obstacles.

1) HJ Reachability (Offline):

HJ reachability can be formulated and solved as an optimal control problem. Specifically, the cost function $ \ell ~:~{\mathcal{R}}\mapsto \mathbb {R}^{+}$ is designed to measure distance (via the Euclidean norm) in the relative state space. The tracker control u tries its best to track the planner and minimize this cost, whereas the disturbance d and planner control $u_{p}$ try to escape the tracker as far as possible by maximizing this cost. Because the environment and planning algorithm are not necessarily known a priori, we assume the worst-case scenario, i.e., that the $u_{p},d$ can act optimally to u. We define their strategies as mappings $\lambda _{p}~:~{\mathcal{U}}_{s}\mapsto {\mathcal{U}}_{p}$ , $\lambda _{d}~:~ {\mathcal{U}}_{s}\mapsto {\mathcal{D}}$ . We further restrict them to be non-anticipative $\lambda _{p} \in \Lambda _{p}$ , $\lambda _{d} \in \Lambda _{d}$ [14]. The value function is given by \begin{align*}V\left ({{r, t}}\right )=& \max _{\lambda _{p} \in \Lambda _{p}, \lambda _{d} \in \Lambda _{d}} \min _{u\in \mathbb {U}_{s}} \left \{{{\vphantom {\max _{s\in \left [{{t, 0}}\right ]} \ell \left ({{ \xi ( s; t, r, u(\cdot ), \lambda _{p}(\cdot ),\lambda _{d}(\cdot )) }}\right )} }}\right. \\& \left. {{\max _{s\in \left [{{t, 0}}\right ]} \ell \left ({{ \xi ( s; t, r, u(\cdot ), \lambda _{p}(\cdot ),\lambda _{d}(\cdot )) }}\right ) }}\right \}.\end{align*} This value function captures the worst-case tracking error when the tracker is acting optimally and the disturbance and planner are acting adversarially. We assume the following limit exits on a compact set, i.e., it converges:\begin{equation*} V^{\infty } (r) = \lim _{t\rightarrow -\infty } V\left ({{r, t}}\right ). \tag {4}\end{equation*} The minimal value of (4) is denoted $\underline {V}^{\infty } $ , whose level set provides the TEB in the relative state space. For planning, we can project this bound into the error state space:\begin{equation*} {\mathcal{B}}_{e}\mathrel {\mathrel {\mathop :}\hspace {-0.0672em}=}\{e~:~\exists \eta \text {s.t.}~V^{\infty }\left ({{e,\eta }}\right ) \leq \underline {V}^{\infty }\}.\end{equation*} If ${\mathcal{C}}$ is known in advance, we could compute the inevitable backward reachable tube of the tracker to ${\mathcal{C}}$ , i.e., the set of states such that the collision must happen [14].

2) Online Execution:

The FaSTrack augments sensed obstacles by ${\mathcal{B}}_{e}$ and employs any planning algorithm to output the next state of the planner model. The gradients of the precomputed value function $V^{\infty } (r)$ inform a linear program to compute optimal control for the tracker model to pursue the planner model. This process is repeated until the tracker model reaches the goal. Safety is guaranteed as long as disturbances fall within the expected bounds [2].

Remark 1:

The value function is computed with a pre-specified disturbance bound ${\mathcal{D}}$ . A larger ${\mathcal{D}}$ corresponds to a larger TEB, which causes the augmented environment to be denser, impacting performance. However, this also makes the system more robust to the disturbance. On the other hand, a smaller ${\mathcal{D}}$ results in a smaller TEB, and therefore a sparser augmented environment, and better average performance, but is less robust to disturbances.

B. R-CLVF

Recently, [1] proposed the robust control Lyapunov value function (R-CLVF), defined as:

Definition 1 (R-CLVF$V_{\gamma } ^{\infty } $ ):

$D_{\gamma } \mapsto \mathbb {R} $ of (3) is \begin{equation*} V_{\gamma }^{\infty }(r) = \lim _{t\rightarrow -\infty } \max _{\lambda _{p} \in \Lambda _{p}, \lambda _{d} \in \Lambda _{d}} \min _{u_{s}\in \mathbb {U}_{s} } \{ \max _{s\in \left [{{t, 0}}\right ]} e^{\gamma (s-t)} \ell \left ({{ \xi (s)~}}\right ) \}.\end{equation*} Here, $ D_{\gamma } \subseteq \mathbb {R}^{n}$ is the domain, $\gamma $ is a user-specified parameter that represents the desired decay rate, and $\ell (x)= ||x|| - \underline {V}^{\infty } $ .

When $\gamma = 0$ , the R-CLVF is equivalent to the infinite-time HJ value function (4). [1] shows that for all $\gamma \geq 0$ , the R-CLVFs have the same zero-level set. In other words, for all $\gamma \geq 0$ , the zero-level set of the R-CLVFs is the TEB.

The R-CLVF value of r captures the largest exponentially amplified deviation of a trajectory starting from r to the TEB, under worst-case disturbance. If this value is finite, it means r can be exponentially stabilized to the TEB ([1, Lemma 7]).

Theorem 1:

The relative state can be exponentially stabilized to the TEB from ${\mathcal{D}}_{\gamma } \setminus {\mathcal{B}} $ , if the R-CLVF exists in ${\mathcal{D}}_{\gamma }$ .\begin{equation*} \min _ {a \in \partial {\mathcal{B}}}||\xi (s)-a|| \leq ke^{-\gamma (s-t)} \min _ {a \in \partial {\mathcal{B}}}||r-a||, \tag {5}\end{equation*} where $k \gt 0$ and $t\leq s\leq 0$ .

The R-CLVF can be computed by solving the following R-CLVF-VI until convergence \begin{align*} 0=& \max \Biggl\{ \ell (r) - V_{\gamma }^{\infty }(r), \\& \min _{u\in {\mathcal{U}}_{s}} \max _{u_{p}\in {\mathcal{U}}_{p},d\in {\mathcal{D}}} \frac { dV_{\gamma }^{\infty }}{d r} \cdot g \left ({{r,u,u_{p},d}}\right ) + \gamma V_{\gamma }^{\infty }\Biggr\}.\end{align*} The R-CLVF optimal controller is \begin{equation*} u^{*} = \mathrm {arg\,min}_{u\in {\mathcal{U}}_{s}}\max _{u_{p}\in {\mathcal{U}}_{p},d\in {\mathcal{D}}} \frac {d V_{\gamma }^{\infty }}{ d r} \cdot g \left ({{r,u,u_{p},d}}\right ). \tag {6}\end{equation*}

SECTION III. Safe Returning With Unexpected Disturbance

FaSTrack is robust to bounded pre-specified disturbances. However, unexpected and infrequent short-duration disturbances can happen because of communication delays, sudden external forces (e.g., a strong wind), or model mismatch. After a sudden unexpected disturbance event that causes the tracker to leave the TEB, the FaSTrack framework only guarantees that the tracker will not exit the current level set of the relative value function. This is visualized in Fig. 1, left. The corresponding error bound that must be used to augment obstacles is shown in blue, resulting in conservative plans.

We propose using the R-CLVF to guarantee that the relative states stabilize back to the TEB at the desired rate $\gamma $ . Alternatively, one can try to find a CBF that represents the TEB, and design a controller to stabilize the relative states to this TEB, which is hard for high-dimensional systems with disturbances and input constraints. We present the SR-F framework and highlight two important implications

1. After an unexpected disturbance event, the relative state will converge back to the TEB at an exponential rate $\gamma $ .

2. We can take advantage of this convergence property by introducing an artificial disturbance that “jumps” the planner forward towards the goal when safe to do so, speeding up navigation process.

A. SR-F Algorithm

The overall algorithm is shown in Alg. 1, with a flowchart shown in Fig. 2. We begin by explaining this algorithm at a high level. First the “sensing block” senses the environment and any unexpected disturbances, then augments obstacles by the maximum safe resetting region (sTEB).

Fig. 2.

Online flowchart for SR-F. The online algorithm contains three main blocks: the sensing block, the planning block, and the tracking block. The sensing block senses the environment, determines the sTEB and augments the obstacle, and checks if unexpected disturbances happen. The planning block checks if the sensed environment is free of obstacles, and use the safe returning function to determine the next plan state $p_{\text {next}}$ (and raw path). The tracking block takes in $p_{\text {next}}$ , determines the optimal controller, and updates the tracker state.

Next the “planning block” by default employs a planning algorithm to generate a path through the sensed environment that obeys the dynamics of the planner model. This planning block has modifications for two scenarios: 1) if a sudden disturbance has occurred, the planner may be moved in a way to ensure that the tracker will not hit an obstacle as it converges back to the TEB, 2) if there is an opportunity to do so safely, the planner will “jump” ahead towards the goal, forcing the tracker to converge back towards it at the rate $\gamma $ .

Finally, there is a “tracking block,” which updates the current relative state between the tracker and planner, and applies the pre-computed optimal controller to the tracker that minimizes the distance between itself and the planner.

B. Sensing Block

Initialization: Every iteration starts with checking if the tracker has experienced an unexpected disturbance, which we assume does not cause failure immediately.

Environment Sensing: The robot senses the environment, updates the constraint ${\mathcal{C}}_{\text {sensed}}$ (also in the planner space ${\mathcal{C}}_{\text {p,sensed}}$ ), and finds the distance from the tracker to the nearest obstacle within sensing range. This distance is given by \begin{align*} dst\left ({{ x; \mathcal C_{\text {sensed}} }}\right ) = \begin{cases} \mathcal R & \quad \text {no obstacle} \\ \min _{a\in \partial \mathcal C_{\text {sensed}}} ||x-a|| & \quad \text {otherwise}. \end{cases} \tag {7}\end{align*} If a new obstacle is sensed, we assign 1 to ReplanFlag (RF).

Computation of the Max Safe Resetting Region, sTEB: Since the planner model is a virtual model with no physical realization, the framework can reset the planner state arbitrarily if needed to ensure that the tracker does not collide with obstacles as it converges back to the planner. We provide a method to find the sTEB, which is denoted as ${\mathcal{S}}$ . Consider a hyperball in the relative state space with radius $dst(x)/2$ and centered at the origin: $B(0,dst(x)/2)$ . If the TEB ${\mathcal{B}}$ is contained in this ball $B(0,dst(x)/2)$ , the sTEB is the largest sub-level set of the R-CLVF contained in $B(0,dst(x)/2)$ . Otherwise, the sTEB is the TEB: \begin{align*} {\mathcal{S}}= \begin{cases} {\mathcal{B}}& \quad {\mathcal{B}} \nsubseteq B(0,dst/2) \\ \text {largest sub-level set} & \quad {\mathcal{B}} \subseteq B(0,dst/2). \end{cases} \tag {8}\end{align*} The sTEB in the planner space is given by \begin{equation*} {\mathcal{S}}_{e}\mathrel {\mathrel {\mathop :}\hspace {-0.0672em}=} \{e~:~\exists \eta \text {s.t.}~ \left [{{e,\eta }}\right ] \in {\mathcal{S}} \}. \tag {9}\end{equation*}

Augmentation of Obstacles: ${\mathcal{S}}_{e}$ is used to augment the obstacles and update the augmented constraint set ${\mathcal{C}}_{\text {p,aug}}$ . The outputs of the sensing block are the sensed and augmented obstacle map ${\mathcal{C}}_{\text {p,sense}}$ , ${\mathcal{C}}_{\text {p,aug}}$ , sTEB, and the RF.

Remark 2:

To guarantee safety, the consideration of the hyperball $B(0,dst(x)/2)$ is necessary, and its radius must be at least $dst(x)/2$ . The reason is that though exponential convergence to the TEB is guaranteed using R-CLVF, it is not necessary that for the next immediate time step, the norm of relative state decreases. This is because of the constant amplifier k in (5). We illustrate this issue in Fig. 1, right. With the hyperball $B(0,dst(x)/2)$ , we guarantee that the distance between the planner and tracker is always smaller than the distance between the planner and the obstacle.

C. Planning Block and the Safe Returning Function

Jump Evaluation: The planning block begins by evaluating whether the planner should “jump” from its current state. This occurs under two conditions. The first condition occurs when the relative state indicates that it is outside of the TEB (i.e., $V_{\gamma } ^{\infty } (r)~\gt 0)$ . In this case the planner must jump to ensure that the tracker does not collide with an obstacle as it converges back to the TEB. The second condition is when there are no obstacles within the sensing radius. In this case, the planner creates an artificial disturbance by intentionally “jumping” to a further point on its path, increasing the relative state r and forcing it to leave the TEB. This accelerates the navigation as the tracker works to converge back at an exponential rate while obeying its control bounds. If either of these conditions for jumping occurs, the JumpFlag (JF) is set to 1.

Safe Returning Function: If the JF ${=}1$ , the safe returning function sets $p_{\text {next}}$ as the state that is closest to the target, free of the augmented obstacles, and guarantee the relative state is in the sTEB (i.e., $\Phi (x,p_{\text {next}})(x- Q p_{\text {next}}) \in {\mathcal{S}})$ . We assign 1 to the RF, indicating that the planning algorithm should plan a new path from $p_{\text {next}}$ . The JF is reset to 0.

Replan: If the ReplanFlag has been activated, either from a jump or a new obstacle detected, the path planning algorithm is used to generate a new path for the planner. This path is processed by the function nextPlan, which converts the path into a trajectory that obeys the dynamics and control bounds of the planner. We then reset the RF to 0.

D. Tracking Block

We update the planner state using $p_{\text {next}}$ , and update the relative state r using (2). The tracking controller u is determined by (6), which is then sent to the tracker model and updates the tracker state. Note that we keep track of $r_{\text {old}}$ , which is used to check if disturbance happens in the next iteration (lines 18–20 of Algorithm 1).

Algorithm 1 SR-FaSTrack

Require:

$V_{\gamma } ^{\infty } $ , ${\mathcal{B}}$ , sense range ${\mathcal{R}}$ , initial states $x_{0}$ , $p_{0}$ .

1:

Initialization:

2:

$x\gets x_{0}$ , $x_{\text {old}} \gets x $ , $p \gets p_{0}$ , $t \gets 0$ , sTEB $\gets {\mathcal{B}} $ , JF $\gets 0$ , RF $\gets 1$

3:

while Goal not reached do

4:

Sensing Block

5:

If unexpected disturbance happens $(x\neq x_{\text {old}})$ , update relative state: $r\gets \Phi (x,p) (x- Qp)$

6:

Sense environment, update ${\mathcal{C}}_{\text {p, sense}}$ , and update distance from the tracker to the obstacle using (7)

7:

RF $\gets 1$ if new obstacle sensed

8:

Find safe resetting region ${\mathcal{S}}$ using (8), augment obstacle with ${\mathcal{S}}_{e}$ and update ${\mathcal{C}}_{\text {p, aug}}$

9:

Planning Block

10:

if $V_{\gamma } ^{\infty } (r)~\gt 0$ then JF $\gets 1$

11:

else if $V_{\gamma } ^{\infty } (r)~\leq 0$ then

12:

if ${\mathcal{C}}_{\text {p, sense}}$ is obstacle free then JF $\gets 1$

13:

else if Not obstacle free then JF $\gets 0$

14:

end if

15:

end if

16:

JF, RF, $p_{\text {next}}$ , $p_{\text {raw}} \gets $ SafeReturn($x,{\mathcal{U}}_{p}$ , JF, RF, $p_{\text {raw}}$ , ${\mathcal{C}}_{\text {p, aug}}$ )

17:

Tracking Block

18:

$p\gets p_{\text {next}}$ , $r\gets \Phi (x,p) (x- Qp)$ , $u\gets u^{*}$ using (6)

19:

Update tracker state: $x\gets \text {nextTrack} (x,u)$

20:

$r,r_{\text {old}} \gets \Phi (x,p) (x- Qp)$ , $ s\gets s+ \Delta s $

21:

end while

Theorem 2:

Safety is guaranteed using SR-F if the disturbance does not push the tracker in its inevitable backward reachable tube of ${\mathcal{C}}$ (as defined in [14]).

Proof: Assume the JF=0 for some time step, the SR-F works just like the FaSTrack, and safety is guaranteed [2].

Assume JF$\neq 0$ at some time step. After resetting the planner state and before tracking, denote planner, tracker, and relative states as $p_{\text {next}}$ , $x_{1}$ and $r_{1}$ . From line 3 of Algorithm 2, $p_{\text {next}}$ is chosen such that $p_{\text {next}}\notin {\mathcal{C}}_{\text {p,aug}}$ , which means the sTEB centered at $x_{1}$ is obstacle free. After applying controller (6), denote the new tracker and relative states as $x_{2}$ and $r_{2}$ . $r_{2}$ must be contained in a strict subset of the sTEB (by Theorem 1), which is also obstacle-free. This suggests that $x_{2}$ is free of obstacles, and safety is guaranteed for the next time step. The overall navigation process is a combination of JF ${=}1$ and JF ${=}0$ , and for both cases, immediate safety is guaranteed. We conclude that the whole navigation process is safe concerning modeled and unexpected disturbances.

Algorithm 2 Safe Returning Function

Require:

x, ${\mathcal{U}}_{p}$ , JF, RF, $p_{\text {raw}}$ , ${\mathcal{C}}_{\text {p,aug}}$

1:

Output: Next plan state $p_{\text {next}}$ , $p_{\text {raw}}$ , JF, RF

2:

if JF ${=}1$ then

3:

$p_{\text {next}}$ , $p_{\text {raw}} \gets $ the closest point to the target s.t. $\Phi (x,p_{\text {next}})(x-Qp) \in $ sTEB and $p \notin {\mathcal{C}}_{\text {p,aug}}$

4:

RF $\gets ~1$

5:

else if JF ${=}0$ then

6:

if RF ${=}1$ then

7:

$p_{\text {raw}} \gets $ PathPlanningAlgo$(p,{\mathcal{C}}_{\text {p,aug}})$

8:

end if

9:

$p_{\text {next}} \gets $ nextPlan($p_{\text {raw}}$ , ${\mathcal{U}}_{p}$ )

10:

remove $ p_{\text {next}} $ from $p_{\text {raw}}$ if $p_{\text {next}} \in p_{\text {raw}} $ , otherwise $p_{\text {raw}} \gets p_{\text {raw}}$

11:

RF $\gets ~0$

12:

end if

13:

JF $\gets 0$

14:

Return $p_{\text {next}}$ , $p_{\text {raw}}$ , JF, RF

Remark 3:

We provide two benefits compared with the FaSTrack. 1) SR-F is robust to unexpected disturbances, 2) in the obstacle-free region, we mimic a “beneficial disturbance” to make the planner jump, accelerating the navigation.

SECTION IV. Experiments

We demonstrate that SR-F can provide safety guarantees given unexpected disturbances, and accelerate the navigation process. We consider two examples: 1) an 8D quadrotor model tracking a 2D integrator planner model with the A* planner and 2) a 10D near-hover quadrotor tracking a 3D integrator planner model with the RRT planner. We compare our method with FaSTrack, Meta-FaSTrack (M-F) [20], and IPC [19]. All simulations are conducted in MATLAB. Code can be found at https://github.com/UCSD-SASLab/Safe-Returning-FThttps://github.com/UCSD-SASLab/Safe-Returning-FT.

A. Offline Computation

$\mathit {1) 10D-3D}$ : The system dynamics of the 10D quadrotor (tracker) and the 3D integrator (planner) are from [2, Example B]. The tracker states $(x,y,z)$ denote the position, $(v_{x}, v_{y}, v_{z})$ denote the velocity, $(\theta _{x}, \theta _{y})$ denote the pitch and roll, $(\omega _{x}, \omega _{y})$ denote the pitch and roll rates. The tracker has controls $(u_{x}, u_{y}, u_{z})$ , representing the desired pitch and roll angle and the vertical thrust. The planner has controls $(\hat {v}_{x}, \hat {v}_{y}, \hat {v}_{z})$ , representing the velocity in each positional dimension. The system parameters are set to be $d_{0} = 10, d_{1} = 8, n_{0} = 10, k_{T} = 0.91, g = 9.81$ , $|u_{x}|, |u_{y}| \leq \pi /9$ , $u_{z}\in [0, 1.5g]$ , $|\hat {v}_{x}|,| \hat {v}_{y}|,| \hat {v}_{z} | \leq 0.5$ , $d_{x}=d_{y}=d_{z}=0$ .

The relative dynamics can be obtained as \begin{align*} \dot {x}_{r}=& v_{x} - \hat {v}_{x} + d_{x}, \dot {v}_{x} = g\tan {\theta _{x}}, \dot {\theta }_{x} = -d_{1}\theta _{x} + \omega _{x}, \\ \dot {\omega }_{x}=& -d_{0}\theta _{x} + n_{0}u_{x}, \dot {y}_{r} = v_{y} - \hat {v}_{y} + d_{y}, \dot {v}_{y} = g\tan {\theta _{y}}, \\ \dot {\theta }_{y}=& -d_{1}\theta _{y} + \omega _{y}, \dot {\omega }_{y} = -d_{0}\theta _{y} + n_{0}u_{y}, \\ \dot {z}_{r}=& v_{z}- \hat {v}_{z} + d_{z}, \dot {v}_{z} = k_{T}u_{z} - g.\end{align*} This is decomposed into three independent subsystems $(x_{r}, v_{x}, \theta _{x}, \omega _{x})$ , $(y_{r}, v_{y}, \theta _{y}, \omega _{y})$ , $(z_{r}, v_{z})$ [21], allowing us to solve for the R-CLVF more tractably.

$\mathit {2) 8D-2D}$ : The relative dynamics of the 8D tracker and the 2D planner are the $x, y$ subsystems above.

B. Online Planning and Navigation

$\mathit {1) 10D-3D}$ : we compare SR-F with FaSTrack and M-F. The result is shown in Fig. 3 and Table I. We design three experiments with different disturbance settings: a) no disturbance, b) unexpected disturbance to the position states pushing the tracker to the obstacle (like a sudden wind), and c) unexpected disturbance to the position and velocity states that act in the worst-case. When no disturbance exists, safety is guaranteed for all three frameworks. When unexpected disturbances exist, both the FaSTrack and M-F collide for more than 80% of runs. Since the positional disturbances push the tracker to the obstacle, M-F and FaSTrack are prone to crash. However, the SR-F can survive these disturbances, showcasing the safe-returning property. When unexpected disturbances are generated by uniformly distributed noise, M-F and FaSTrack collide in less than 10% of experiments.

TABLE I Comparison of FaSTrack, M-F, and SR-F for the 10D-3D System. Each Row Is Averaged Across 40 Runs

Fig. 3.

10D-3D simulation using SR-F. The tracker tracks a RRT path when not obstacle-free (blue), and jumps ahead on the path (cyan) when obstacle-free. The planner’s position is the green star in the translucent blue box (representing sTEB). Both systems start on the left and navigate to a goal on the right. The three light grey rectangles are obstacles, and once sensed by the quadrotor they turn red. When the quadrotor is passing near an obstacle, it experiences an unexpected disturbance to its position (black dashed line), mimicking a sudden wind gust. The green dashed line shows the change of the planner’s position after replanning.

We highlight that SR-F guarantees safety under unexpected disturbances, though it takes more time to reach the goal. This is because FaSTrack and M-F do not consider the unexpected disturbance, and do not spend time to replan. However, it is preferable to sacrifice the navigation speed for the safety guarantee in most real-world applications. Also, note that the navigation speed is affected by the planning algorithm used and the environment.

$\mathit {2) 8D-2D}$ : we compare SR-F with IPC. We construct two scenarios: a) a relatively larger disturbance to the position states pushing the tracker to the obstacle (see Fig. 4 and column 1 of Table II), and b) uniformly distributed smaller position disturbances $(\Delta x, \Delta y\in [{-}0.2, 0.2])$ that are randomly added (Table II). The simulation time is 40s with a 0.1s time step. In all simulations, SR-F safely navigates the quadrotor to the goal. When the large disturbance is applied, IPC collides in 45% of runs. When 3 and 5 smaller random disturbances are applied, IPC fails to reach the goal within the simulation time for 20% and 75% of runs, though no collisions occur. When only one disturbance happens, the IPC is faster than SR-F, but as the occurrence of the disturbances increases, the navigation speed of IPC decreases significantly, and the SR-F outperforms it. The reason is that the IPC’s controller does not consider disturbance and the safety is guaranteed using the safe flight corridor (which works for the case where disturbance does not push the system out of it.). The presence of disturbances greatly impacts the performance of the MPC controller.

TABLE II Comparison of SR-F and IPC Frameworks for 8D-2D System. Each Row Is Averaged Across 40 Runs

Fig. 4.

Online simulations for an 8D quadrotor tracking a 2D planning model paired with an A* planner. Results using SR-F (ours) and IPC are shown. Left: the entire trajectory using SR-F. The quadrotor starts at (4, 4) and navigates to the goal at (16, 16) (red star). The obstacle (red) is augmented by the TEB. The system’s trajectory is shown in cyan (when jumping) and blue (when tracking). A position disturbance (labeled “Real Dstb”) is applied to the quadrotor, pushing it (blue dashed line) close to the obstacle. Right: the trajectory (blue) using IPC, with the same start and goal as SR-F. The SFC is shown as the light green region, and the path given by A* is shown in green.

SECTION V. Conclusion

In this letter, we introduced the SR-FaSTrack framework, which can be used to reject unexpected disturbances during navigation in a priori unknown environments. It also accelerates navigation by intentionally making the planner “jump” as a virtual disturbance in open environments.

Future work includes extending to multi-agent systems and dealing with moving obstacles, modifying the R-CLVF to get differently shaped TEBs, combining with deep neural networks to better accommodate different environments, and implementing hardware demonstrations.