Fast and Safe Path-Following Control using a State-Dependent Directional Metric

This paper considers the problem of fast and safe autonomous navigation in partially known environments. Our main contribution is a control policy design based on ellipsoidal trajectory bounds obtained from a quadratic state-dependent distance metric. The ellipsoidal bounds are used to embed directional preference in the control design, leading to system behavior that is adapted to the local environment geometry, carefully considering medial obstacles while paying less attention to lateral ones. We use a virtual reference governor system to adaptively follow a desired navigation path, slowing down when system safety may be violated and speeding up otherwise. The resulting controller is able to navigate complex environments faster than common Euclidean-norm and Lyapunov-function-based designs, while retaining stability and collision avoidance guarantees.


page 1

page 6


Safe Robot Navigation in Cluttered Environments using Invariant Ellipsoids and a Reference Governor

This paper considers the problem of safe autonomous navigation in unknow...

Robust and Safe Autonomous Navigation for Systems with Learned SE(3) Hamiltonian Dynamics

Stability and safety are critical properties for successful deployment o...

Safe Autonomous Navigation for Systems with Learned SE(3) Hamiltonian Dynamics

Safe autonomous navigation in unknown environments is an important probl...

Sample Efficient Learning of Path Following and Obstacle Avoidance Behavior for Quadrotors

In this paper we propose an algorithm for the training of neural network...

Autonomous Obstacle Avoidance by Learning Policies for Reference Modification

The key problem for autonomous robots is how to navigate through complex...

A New Simulation Metric to Determine Safe Environments and Controllers for Systems with Unknown Dynamics

We consider the problem of extracting safe environments and controllers ...

I Introduction

Advances in embedded sensing and computation have enabled robot applications in unstructured environments and in close interaction with humans, including autonomous transportation, inspection and cleaning services, and medical robotics. Safe, yet efficient, robot navigation is important for these applications but is challenging due to partially known or rapidly changing operational conditions.

In motion planning, optimality guarantees have been achieved for geometric path planning [ARAstar, RRTstar] but incorporating robot dynamics without violating these guarantees remains an active area of research. To achieve efficient behavior for dynamical system, optimal control theory is used together with sampling-based or search-based motion planners. Sampling-based methods connect neighboring states using locally optimal control such as linear-quadratic-regulation (LQR) [lqr_rrt_star_perez2012] or fixed-final-state-free-final-time optimal control [kinodynamic_rrts_webb2013]. Locally optimal control, however, does not necessarily lead to global optimality [pacelli2018integration]. Search-based methods construct a safe corridor [SFC, SFC_FM] (a connected safe region in free space) and seek a composition of short motion primitives within. Depending on the primitive design, the resulting trajectory may already be dynamically feasible [Liu_DynamicTrajectoryPlanning_IROS17] or may be optimized locally using model predictive control (MPC) [SFC]. These techniques do not provide formal guarantees for joint collision avoidance and stability.

To guarantee safety formally, most existing works rely on Lyapunov theory and reachable set computations. A funnel is an outer approximation of the reachable set of a dynamical system in the presence of disturbances [funnel_idea]. Building on the seminal work of [burridge1999sequential], sequential composition of funnels offers effective means of guaranteeing safe navigation [lqr_tree_tedrake2009, Funnel_lib, gawron2017vfo, gawron_2018IROS_VFO]. Using sum-of-squares optimization [sos_stability], these techniques can deal with nonlinear systems, nonholonomic constraints, and bounded disturbances. Recently, control barrier functions methods [CBF_ames2014rapidly, CBF_wu2015safety, CBF_ames2016control, CBF_quadrotor] have received significant attention. While optimizing performance without sacrificing stability using control Lyapunov function (CLF), safety constraints are handle by a control barrier function (CBF). A virtual reference governor system [garone2016_ERG, kolmanovsky2014ref_cmd_gov] may also be used to enforce safety constraints as an add-on control scheme to pre-stabilized dynamical systems. Using a reference governor design, [Gov_ICRA17]

enables safe navigation for an acceleration-controlled robot among spherical obstacles by adaptive tracking of a first-order vector field.

Fig. 1: A robot equipped with a lidar scanner navigates in an unknown cluttered environment. A state-dependent metric that considers the robot’s direction of motion is designed to approximate the robot’s future trajectory (yellow ellipse) and quantify its safety (gray ellipse) with respect to surrounding obstacles. An adaptive controller guarantees safety and stability based on these measures.

The importance of considering configuration space geometry and system dynamics jointly when designing a steering function for sampling-based kinodynamic planning is considered in [pacelli2018integration]. Metrics based on Mahalanobis distance, linear-quadratic-regulator cost, and a Gram matrix derived from system linearization are considered. Inspired by this work, we observe that using a static distance metric to quantify the safety of a robot with respect to surrounding obstacles can significantly impact its performance in real applications. For example, an autonomous golf cart running on campus has to simultaneously maintain safe distance from pedestrians and, yet, be able to squeeze through narrow passages such as doors or road block pillars. Static safely measures do not take the system’s velocity direction into account, leading to overly cautious behavior even if the direction of travel is completely orthogonal to nearby obstacle surfaces. We refer to this limitation as the corridor effect and aim to design an adaptive path-following controller, mitigating this effect via new metric that takes the system’s state into account when quantifying safety. The two main contributions of this work are highlighted as follows. First, we propose a new state-dependent directional metric and develop accurate system trajectory bounds for linearized robot dynamics that take direction of motion into account. Second, we develop an adaptive feedback controller, based on the directional trajectory bounds, and prove that it ensures stable and collision-free navigation. The controller relies only on local obstacle information, easily obtainable from onboard sensors, and provides fast tracking performance in complex unknown environments. See Fig. 1 for an illustration.

Ii Notation

Let and denote the set of symmetric positive and semi-definite matrices Let and denote the generalized inequalities associated with and . Denote the Euclidean () norm by and the quadratic norm induced by as . Let and

be the maximum and minimum eigenvalues of

. Let denote the quadratic norm distance from a point to a set . Given and scaling , denote the associated ellipsoid centered at by .

Iii Problem Formulation

Consider a robot operating in an unknown environment . Denote the obstacle space by a closed set and the free space by an open set . Suppose that the robot dynamics are controllable, linear, and time-invariant:


where is the control input and is the robot state111Transpose operations are omitted when grouping vectors for conciseness., decomposed into constrained variables and free variables . Throughout this paper, represents the robot position, required to remain within for all , while denotes higher-order (velocity, acceleration, jerk, …) terms. Our objective is to design a closed-loop control policy , ensuring that the robot follows a given navigation path in the free space.

Definition 1.

A path is a piecewise-continuous function that maps a path-length parameter to the interior of free space. The start and the end of a navigation path is in the, i.e., .

A path may be provided by a geometric planning algorithm [lavalle2006planning, ARAstar, RRTstar]. We consider the following problem.

Problem 1.

Given a path , design a control policy so that the robot (1) is asymptotically steered from the start to the end of while remaining collision-free, i.e., for all .

Iv Technical Approach

In this section, we propose a novel state-dependent directional metric (SDDM) and show that closed-loop trajectories of (1) can be bounded in the SDDM by solving a convex optimization problem. We develop a feedback control law that exploits the trajectory bounds to stabilize the robot and follow the path adaptively, slowing down when safety may be endangered and speeding up otherwise.

Iv-a State-Dependent Directional Metric

As mentioned in the introduction, measuring safety using a static Euclidean norm may lead to system performance suffering from the corridor effect. We propose a quadratic distance measure that assigns priority to obstacles depending on the robot’s moving direction. The level sets of are ellipsoids whose shape and orientation are determined by the matrix . Our idea is to encode a desired directional preference in the distance metric via an appropriate choice of . Consider the example in Fig. 2. A quadratic norm, well-aligned with the local environment geometry, may provide a more accurate evaluation of safety than a static Euclidean norm. Based on this observation, we propose a general construction of a directional matrix , in the direction of vector , that defines a state-dependent directional metric.

Definition 2.

A directional matrix associated with vector and scalars is defined as


The unit ellipsoid centered at generated by a directional matrix is elongated in the direction of .

Fig. 2: Robot (black dot) moving in direction (green arrow) along a corridor. The distances, measured by a quadratic norm (left) and Euclidean norm (right), from the robot to the closest point (small blue square) on the wall (red line) are 1.41 and 0.71. The matrix is defined as a directional matrix .
Lemma 1.

For any vector , the directional matrix is symmetric positive definite.


Since is symmetric, . If , is positive definite. If and is arbitrary:

which follows from and the Cauchy-Schwarz inequality. The proof is completed by noting that . ∎

Iv-B Trajectory Bounds using SDDM

Using a directional matrix, one can define a SDDM to adaptively evaluate the risk of surrounding obstacles. We will show how to use an SDDM to obtain bounds on the closed-loop trajectory of the constrained state in (1). Assume that robot is stabilized by a feedback controller . The closed-loop dynamics are:


where is Hurwitz. Any initial state will converge exponentially to the equilibrium point at the origin. An output is introduced to consider the constrained state . We are interested in measuring the maximum deviation of for from the origin using a directional measure determined by the orientation of the initial state with respect to . Define an SDDM using the directional matrix:


and choose the output so that , where is the projection matrix from to . Note that . Thus, measuring the maximum deviation of in the SDDM is equivalent to finding the output peak along robot trajectory. :


We outline two approaches for solving this problem.

Iv-B1 Exact solution

The output peak can be computed exactly by comparing the values of at the boundary point and all critical points . Since the closed-loop system in (3) is linear time-invariant, can be obtained in closed form. Let be the Jordan decomposition of , where is block diagonal. The critical points satisfy:


In general, an exact solution may be hard to compute due to complicated expression of .

Iv-B2 Approximate solution

When an exact solution to (5) is hard to obtain, we may instead compute a tight upper bound on . Let be such that:


is an invariant ellipsoid for the robot dynamics (3), i.e., for all . Instead of finding the peak value of along the state trajectory, we can compute it over the invariant ellipsoid . Since contains the system trajectory, we have for all :


Obtaining the upper bound above is equivalent to solving the following semi-definite program [boyd_LMI_book, Ch.6]:

subject to
Lemma 2.

For any initial condition and associated constant directional matrix in (4), the trajectory under system dynamics (3) admits a tight ellipsoid bound, , for all , where is the solution to (5) and is the solution to (9).


By definition, is equivalent to . Since , inequality (8) yields . Hence, .∎

Now, we know how to find an accurate outer approximation of the system trajectory in the SDDM defined by (4). It is ready to develop a feedback controller that utilizes the trajectory bounds to quantify the safety of the system with respect to surrounding obstacles, while following the navigation path towards the goal.

Iv-C Structure of the Robot-Governor Controller

The problem of collision checking is simple for first-order kinematic systems since they can stop instantaneously to avoid collisions. We introduce a reference governor [garone2016_ERG, kolmanovsky2014ref_cmd_gov], a virtual first-order system: with state and control input , that will serve to simplify the conditions for maintaining stability and safety concurrently. Our proposed structure of a path-following control design is shown in Fig. 3. The reference governor behaves as a real-time reactive trajectory generator in that it continuously regulates a reference signal for the real robot dynamics depending on risk level evaluation using SDDM. More precisely, we choose the real system’s control input so that the robot tracks the governor state , while the governor state is regulated via to ensure collision avoidance and stability for the joint robot-governor system.

Fig. 3: Structure of the proposed controller. A virtual reference governor adaptively conveys global navigation information to a local feedback controller based on a prediction of the system trajectory. A local safe zone , depending only on the current system state , is constructed from an SDDM trajectory bound. A time-varying local goal is obtained by projecting the navigation path onto the local safe zone . The governor chases this local goal and continuously sends its updated stated as a reference signal to guide the local controller.

In detail, let be the system state with the first element changed from to to make an equilibrium point. Choose a local controller for (1) that tracks the governor state :


Consider the augmented robot-governor system with state , coupling the real states with the governor state:


Before proposing the design of the governor controller , we analyze the behavior of the robot-governor system in the case of a static governor.

Lemma 3.

If the governor is static, i.e., so that , then the robot-governor system in (11) is globally exponentially stable with respect to the equilibrium .


The subsystem has an equilibrium at , which is globally exponentially stable because is Hurwitz, while by assumption. ∎

In addition to guaranteeing stability for a static governor, we can use the ellipsoidal trajectory bounds from Lemma 2 to ensure safety.

Theorem 1.

Let be any initial state for the robot-governor system in (11) with . Suppose that so that . Let be a constant directional matrix and suppose that the following safety condition is satisfied:


where is an upper bound for obtained according to Lemma 2. Then, the robot-governor system is globally exponentially stable with respect to the equilibrium and, moreover, the robot trajectory is collision free, i.e., , for all .


Since is an equilibrium for , by Lemma 2, for all . From the safety condition in (12), for all . Stability is ensured by Lemma 3. ∎

Iv-D Local Projected Goal and Governor Control Policy

We established that the robot-governor system is stable and safe as long as the governor is static and (12) holds. Next, we consider how to move the governor without violating these properties. Based on (10), we know that the robot will attempt to track the governor state. The main idea is to choose a time varying directional matrix to measure the system’s safety. Since is positive definite (by Lemma 1), it can still be used to define a SDDM . Then, Lemma 2, can still provide an accurate robot trajectory bound , which takes the robot’s direction of motion into account. We need to design the governor control policy so that never violates a time-varying version of the safety condition in (12).

Our approach is to define an ellipsoid , called a local safe zone, centered at the governor state , and have the size of determine how fast the governor can move. In the worst case, if system safety or stability are endangered, should shrink to a point, forcing the governor to remain static. Once there is enough leeway in the safety conditions in (12), can grow, allowing the governor to move without endangering the safety or stability.

Definition 3.

A local safe zone is a time-varying set that at time depends on the robot-governor state as follows:


where is a directional matrix, determined by , and is a measure of leeway to safety violation, determined by an upper bound on for all and the directional distance from the governor to the nearest obstacle.

The term estimates the safety of the system based on the local environment geometry and the robot activeness. The requirement that only places a constraint on the magnitude , so

is a degree of freedom that can be utilized to make

asymptotically tend to a desired goal. We define a local goal for the governor.

Definition 4.

A local projected goal is the farthest point along the path contained in the local safe zone :


The informal notation will be used for the local projected goal to emphasize that is determined by projecting the path onto the local safe zone . The structure of the complete closed-loop control policy is illustrated in Fig. 3, while the definitions of a local safe zone and a local projected goal are visualized in Fig. 5. We are finally ready to define the governor control policy:


where is a control gain for the governor controller. We prove that the closed-loop system is stable, safe, and asymptotically reaches the goal specified by the path . We also informally claim that the path-following controller is fast due to the use of directional information for safety verification. This claim is supported empirically in Sec. V.

Theorem 2.

Given a path , the closed-loop robot-governor system (11) with move-to-projected-goal (15) is asymptotically steered from any safe initial state , i.e., and for some , to the goal state and the robot trajectory is collision-free for all time, i.e., for all .


From Thm. 1, we know that the robot-governor system (11) will asymptotically converge to the equilibrium point without collisions if the governor is static. The governor control policy in (15) allows the governor to move only when the interior of is nonempty. From Def. 3, this happens only if the safety condition is strictly satisfied, i.e., . Since approaches , eventually becomes strictly positive and the set becomes an ellipsoid in free space with non-empty interior. Since initially for some , the local projected goal in (14) will be well defined and when grows, the projected goal will move further along the path , i.e., the path length parameter will increase. Since the system dynamics are continuous, cannot suddenly become negative without crossing zero. If , the local energy zone shrinks to a point, i.e., , and hence the governor stops moving and waits until the robot catches up. When the governor is static, and since the safety condition in (12) is satisfied, Thm. 1 again guarantees that the robot can approach the governor without collisions, increasing in the process. Once goes above , the governor starts moving towards the goal again by chasing the projected goal. Note that the local projected goal always lies on the navigation path inside the free space, i.e., , so the robot-governor system cannot remain stuck at any configuration except . Using LaSalle’s Invariance Principle [khalil2002nonlinear], one can conclude that the largest invariant set is the point where both the robot and the governor are stationary at the goal location . ∎

Fig. 4: Trajectory bounds comparison between a Euclidean metric (left) and an SDDM (right). The governor is fixed at the origin while the robot’s initial conditions are and . The change of the trajectory bounds over time is illustrated via ellipsoids with different colors, starting from cold/blue and converging towards warm/red.
Fig. 5: Comparison of controller 1 (top) and 2 (bottom) in a corridor simulation. A snapshot is shown at the same time instant for both controllers. The local energy zone (yellow) resulting from the proposed SDDM trajectory bounds fits the corridor environment well, leading to fast, yet safe, movement.
Fig. 6: Simulation of the robot-governor system tracking a piecewise-linear path (black) in an environment with circular obstacles (dark gray circles). The start and end of the path are indicated by a red and green star, respectively. The two plots on the left show that the robot at around same location behaves differently due to different distance measures. The controller using Euclidean distance is overly cautious with respect to lateral obstacles resulting in conservative motion. The system employing SDDM trajectory bounds has a larger local safe zone, which helps the robot turn fast and smoothly. The two plots on the right show the trajectories followed by the systems employing the two controllers. The velocity profiles are shown as magenta arrows perpendicular to the robot path. The controller based on SDDM trajectory bounds (rightmost) results in higher velocities compared to the controller using Euclidean ball invariant sets. Observe that the path followed by the robot (green line) is also smoother, especially when turning, for the directional controller despite the higher velocity.
Fig. 7: Output peak from the trajectory followed in Fig. 6. The blue curve is computed from the SDP optimization in eq. (9). The red curve is obtained analytically from eq. (6). It is clear that is an upper bound for

, and the bound is tight at certain moments.

Fig. 8: Snapshot of the robot-governor system navigating the environment shown in Fig. 1. Streaming lidar scan measurements (red dots) are used to update an occupancy map (black lines and white regions in top plot) of the unknown environment. An acceleration-controlled robot (green dot) follows a virtual governor (blue dot) whose motion is modulated based on the local energy zone (yellow ellipse) and the directional distance to obstacles (gray ellipse). A navigation path (blue line) is periodically replanned using an planner and an inflated occupancy map (bottom plot).

V Evaluation

Consider an acceleration-controlled robot, stabilized by a proportional-derivative (PD) controller:


The closed-loop robot-governor system is:


Several experiments will be shown to compare the performance of two path-following controllers:

  • Controller 1 [Gov_ICRA17]: uses a Lyapunov function to ensure stability and collision avoidance. The robot’s kinetic and potential energy, i.e., is used to define a spherical local safe zone.

  • Controller 2 (ours): uses SDDM trajectory bounds (Lemma 2) to define an ellipsoidal local safe zone.

In the visualizations, the governor and robot positions are shown by a blue and green dot, respectively. A light-gray ellipse/ball indicates the distance from the governor to the nearest obstacles, while the local safe zone is indicated by a yellow ellipse/ball. The projected goal is shown as a small red dot at the boundary of . The controller parameters were , , .

Trajectory Prediction using Different Metrics.  First, we demonstrate that predicting the robot trajectory using our directional metric has some desirable properties for enforcing safety constraints. Fig. 4 compares trajectory bounds obtained from Lemma 2 for (16) using a Euclidean metric and an SDDM. Since the robot dynamics are simple, a tight directional trajectory bound can be obtained from an exact computation of the critical points according to (6). It is clear that the ellipsoid bounds on the system trajectory are less conservative (smaller volume) than the spherical bounds, as long as the damping is relatively large compared to . Unlike a Lyapunov function, the ellipse bounding the robot trajectory is not forward invariant. It can be shown that requiring invariance of directional ellipsoids () would need infinite damping unless for some , causing the metric to lose directionality. In contrast to control designs based on Lyapunov function invariance, we make the interesting observation that a safe and stable controller can be defined even if the sets containing the system trajectory are not strictly shrinking over time.

Corridor Environment.  We show that utilizing a directional metric in the control design alleviates the corridor effect discussed in Sec. I. We setup a simulation requiring a robot to navigate through a corridor (Fig. 5). The results show that controller 1, using a Lyapunov function with spherical level sets, suffers from the corridor effect while the proposed controller 2, making directional predictions about the system trajectory, does not.

Sparse Environment with Circular Obstacles.  This experiment compares the two controllers in a longer path-following task in an environment with circular obstacles. Snapshots illustrating how the two controllers judge distances to obstacles and define a local energy zone are shown in Fig. 6. It can be seen that the controller equipped with a directional sensing ability has a better understanding of the local environment geometry, leading to a larger, elongated local safe zone set. As a result, controller 2 does not need to slow down for low-risk lateral obstacles, leading to smoother and faster navigation. The directional bounds on the robot trajectory obtained analytically, according to eq. (6), and from the SDP in eq. (9) are compared in Fig. 7.

Unknown Environment with Arbitrary Obstacles.  This experiment demonstrates that our controller can work in a complex unknown environment, shown in Fig. 1, relying only on local onboard measurements. The directional distance from the governor to the obstacles is computed from the latest lidar scan. The path is re-planned from the current governor position to the goal using an occupancy map constructed from the lidar scans over time, as illustrated in Fig. 8.

Vi Conclusion

This paper presented a path-following controller relying on a state-dependent directional metric for trajectory prediction and safety quantification. The controller achieves fast tracking in unknown complex environments, mitigating the corridor effect, while providing safety and stability guarantees. The approach offers a promising direction for ensuring the safety of mobile autonomous systems operating in dynamically changing environments. Our design places very minimal requirements on the navigation path (piecewise-continuity) but the overall system behavior depends on the path quality. Future work will focus on incorporating safety and stability considerations in path planning, applying our results to complex robot dynamics, and demonstrating the effectiveness of our design in real experiments.