1 of 24

Safe Returning FaSTrack With Robust Control Lyapunov-Value Functions

Zheng Gong* , Boyang Li* , and Sylvia Herbert

Mechanical and Aerospace Engineering,

University of California at San Diego

1

2 of 24

Motivation

Safe navigation in a priori unknown environment, subject to sudden disturbances

2

3 of 24

Outline

3

Converge back to error bound after sudden disturbance

Robust Control Lyapunov Value Function (R-CLVF)

Reset planner state to guarantee safety

Maximum Safe Resetting Region: (sTEB)

Accelerate navigation

Introduce virtual disturbance

Background

Fast and Safe Tracking (FaSTrack)

OBS

4 of 24

FaSTrack: Models

Tracker Model

high dimensional, accurate
computationally intractable for real-time planning

Planner Model

low dimensional, simpler
computationally efficient for real-time planning

Relative Model

4

Chen*, Herbert* et al., "FaSTrack:A Modular Framework for Real-Time Motion Planning and Guaranteed Safe Tracking." IEEE Transactions on Automatic Control, 2021.

5 of 24

Error Function (user-defined)

Value function

5

TEB is the smallest level set corresponding to the value , which is defined as

Interpretation: the smallest possible tracking error of the system

When solved to convergence, any level set is a control invariant set.

Value Function and Tracking Error Bound (TEB)

6 of 24

FaSTrack (online)

6

Sense

OBS

Plan (RRT)

Track

OBS

7 of 24

Limitations of FaSTrack

7

OBS

8 of 24

Outline

8

Converge back to error bound after sudden disturbance

Robust Control Lyapunov Value Function (R-CLVF)

Reset planner state to guarantee safety

Maximum Safe Resetting Region: (sTEB)

Accelerate navigation

Introduce virtual disturbance

Background

Fast and Safe Tracking (FaSTrack)

OBS

9 of 24

Robust Control Lyapunov Value Function (R-CLVF)

9

Exponentially amplify the deviation to get a R-CLVF

The 0-sublevel set of R-CLVF is the TEB

The control can be obtained by

Zheng Gong, Sylvia Herbert, "Robust Control Lyapunov-Value Functions for Nonlinear Disturbed Systems" Preprint, 2024

10 of 24

Some properties of R-CLVF

10

Theorem: Exponential Stability� The R-CLVF exists on if and only if the system can be exponentially stabilized to its SRCIS from , regardless of the disturbance. The domain is called the Region of Exponential Stabilizability (ROES).

Lemma: Identical Smallest Robust Control Invariant Sets� , the 0-sublevel sets of R-CLVF are the same.

11 of 24

SR-F (offline)

Q1: Converge back to TEB

11

TEB

HJ Value Function

R-CLVF

Dstb

12 of 24

SR-F (offline)

12

OBS

Q1: Converge back to TEB

OBS

13 of 24

Outline

13

Converge back to error bound after sudden disturbance

Robust Control Lyapunov Value Function (R-CLVF)

Reset planner state to guarantee safety

Maximum Safe Resetting Region: (sTEB)

Accelerate navigation

Introduce virtual disturbance

Background

Fast and Safe Tracking (FaSTrack)

OBS

14 of 24

Safe Returning Mechanism

14

Obstacle free?

Sensing Block

Sense environment

Find sTEB

Augment obstacle

Dstb?

Set RF = 1

Yes

RF=1?

Plan new path

Track old path

Yes

No

Planning Block

Tracking

No

Safe returning mechanism

No

Yes

15 of 24

SR-F (online)

Q2: Choice of Planner State

15

Pick such that

it is the closest state to the target
it is in the augmented obstacle-free region

OBS

sTEB

Find sTEB based on the distance from the tracker to obstacles.

16 of 24

SR-F (online)

Q2: Choice of Planner State

16

OBS

Drone tracks the planner.
Call the path planning algorithm to replan a path to the goal.

Safety is guaranteed if the TEB doesn’t overlap with obstacles after the sudden disturbance.

OBS

17 of 24

SR-F (online)

Q2: Choice of Planner State

17

OBS

If the TEB overlaps with obstacles after the sudden disturbance, we cannot guarantee safety when tracking.

OBS

18 of 24

Outline

18

Converge back to error bound after sudden disturbance

Robust Control Lyapunov Value Function (R-CLVF)

Reset planner state to guarantee safety

Maximum Safe Resetting Region: (sTEB)

Accelerate navigation

Introduce virtual disturbance

Background

Fast and Safe Tracking (FaSTrack)

OBS

19 of 24

19

sTEB

SR-F (online)

Q3: Accelerate Navigation

OBS

Leveraging the convergence property of R-CLVF to allow planner to move further.

TEB

20 of 24

Results: 10D Quadrotor

20

Real disturbance

Virtual disturbance

Change of the tracker state after the sudden disturbance

Change of the planner state (safe resetting) after the sudden disturbance

Here’s one example of a 10D drone trying to reach the goal as denoted by the small black dot, while avoiding the obstacle denoted by the gray planes. The planning algorithm we used is the RRT.

The blue transparent box denotes the maximum safe resetting region. The green star denotes the plan state, and the trajectory is shown as the blue curve.

As you can see, once an obstacle is sensed, it turns to red. And since the system is moving towards the obstacle, the STEB is shrinking.

I’ll try to skip a little bit (jump to 00:15).

As you can see here, once it pass the first obstacle, we give an artificial disturbance that push it back. As shown in the dashed line. The planner state is reset in the sTEB, and the navigation continues.

Now (00:45), once there’s no obstacle in the sense range, the planner automatically jumps towards the target, and the drone converge to it, while the planner keeps running.

And here, another artificial disturbance that push it back. And another jump here.

In this video, we are visualizing the TEB around the planner. This is equivalent to show the augmented obstacle.

21 of 24

Summary of Contributions

21

Safe Returning FaSTrack can handle sudden disturbances and safely navigate the system to the target.
Safe Returning FaSTrack intentionally introduces a ‘beneficial disturbance’ to accelerate the navigation.

Reach out to us!

22 of 24

22

23 of 24

Results: 10D Quadrotor

23

Real disturbance

Virtual disturbance

Change of the tracker state after the sudden disturbance

Change of the planner state (safe resetting) after the sudden disturbance

Here’s one example of a 10D drone trying to reach the goal as denoted by the small black dot, while avoiding the obstacle denoted by the gray planes. The planning algorithm we used is the RRT.

The blue transparent box denotes the maximum safe resetting region. The green star denotes the plan state, and the trajectory is shown as the blue curve.

As you can see, once an obstacle is sensed, it turns to red. And since the system is moving towards the obstacle, the STEB is shrinking.

I’ll try to skip a little bit (jump to 00:15).

As you can see here, once it pass the first obstacle, we give an artificial disturbance that push it back. As shown in the dashed line. The planner state is reset in the sTEB, and the navigation continues.

Now (00:45), once there’s no obstacle in the sense range, the planner automatically jumps towards the target, and the drone converge to it, while the planner keeps running.

And here, another artificial disturbance that push it back. And another jump here.

In this video, we are visualizing the TEB around the planner. This is equivalent to show the augmented obstacle.

24 of 24

SR-F (online)

Q2: Choice of Planner State

24

Maximum Safe Resetting Region: (sTEB)

OBS