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.
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.
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.
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.
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.
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 .
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]:
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.
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.
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.
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 .
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.
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 makeasymptotically tend to a desired goal. We define a local goal for the governor.
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.
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 . ∎
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.
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.