I Introduction
Safe autonomous operation in unstructured, dynamic, and a priori unknown environments is a cornerstone problem of robotics. Achieving safe autonomous navigation with ground and aerial robots will have transformative impact on transportation, structure inspection, and environmental monitoring. Similarly, achieving safe autonomous manipulation may have transformative impact on product assembly, construction, and medical applications. Research in motion planning and dynamical system control has led to significant progress in these directions. Geometric motion planning algorithms, including graph search [ARAstar, MHA] and samplingbased techniques [RRT, RRTstar, BITstar], generate safe shortest paths in complex highdimensional problems. However, ensuring that the planned paths are dynamically feasible and remain safe when a realtime tracking controller is employed on the robot system is an active area of research. Instead of geometric paths, kinodynamic planning [kinodynamic_rrts_webb2013, SST]
aims to find highorder dynamically feasible trajectories directly simplifying the realtime control task. However, is challenging to plan trajectories over highorder variables such as angular acceleration or linear jerk, especially for robots with many degrees of freedom. Moreover, deciding the right trajectory clearance or time allocation during planning are major challenges in dynamically changing environments and in the presence of disturbances. These considerations make current techniques computationally challenging and, yet, safety guarantees when the planner is coupled with closedloop controller are difficult to obtain.
The focus of this paper is a control design for nonlinear controlaffine robot systems that provides theoretical guarantees for safe trajectory tracking when input disturbances are present, the environment is unknown and perceived only through local onboard sensing, and a only loworder geometric path is provided. Our approach relies on feedback linearization to determine invariant ellipsoid bounds on the closedloop robot trajectory under stabilizing control. In contrast to existing feedbacklinearization procedures, we examine the impact of disturbances carefully and develop semidefinite programming (SDP) and Lyapunovequation techniques to tightly and computationally efficiently bound the worstcase system behavior. Inspired by reference governor control techniques [garone2016_ERG, kolmanovsky2014ref_cmd_gov, Gov_ICRA17], we develop a firstorder virtual system to guide the real robot along a desired geometric path. The governor serves as a local equilibrium point for the robot that is able to move if safety is not endangered. In detail, the governor evaluates risk according to the volume of the local free space and the robot activeness, measured by the volume of the ellipsoid trajectory bounds, and adaptively selects its pathtracking speed to guarantee that the real robot remains safe and stable.
Ia Related Work
Safe corridor construction [SFC, SFC_FM] during geometric motion planning is a promising approach for generating highorder dynamically feasible trajectories. Existing approaches build the largest possible safe region around a geometric path using a series of connected convex sets (e.g., ellipsoids or polyhedra) and quadratic programming is used to generate safe dynamically feasible trajectories. A samplingbased Learning Model Predictive Controller is proposed in [rosolia2019sample] to build a safe set by iteratively refining a convex hull approximation of the reachable set of a linear system subject to bounded disturbance. The reachable set is approximated by finitehorizon closedloop trajectories with a sampled disturbance sequence. A Qvalue function, which behaves like a robust control Lyapunov function, and an associated control policy are obtained from the sampled trajectories.
To guarantee safety without sampling, most existing works rely on Lyapunov theory and reachable set overapproximations. Building on the seminal work of [burridge1999sequential], sequential composition of funnels [funnel_idea] (reachable set approximations) offers effective means of guaranteeing safe navigation [lqr_tree_tedrake2009, Funnel_lib, gawron2017vfo, gawron_2018IROS_VFO]. Using sumofsquares (SOS) optimization [sos_stability], these techniques can deal with nonlinear systems directly and may handle nonholonomic constraints and bounded disturbances. While funnels are accurate in representing the system dynamics, the design of a funnel library faces a tradeoff between the size of the library and computational complexity of composing the funnels online. A trajectory independent tracking controller was proposed by [singh2017robust] based on contraction theory. In the paper, a robust control invariant tube is constructed by minimizing a certain control contraction metric using SOS programming. Using HamiltonJacobi reachability, one can deal with nonlinear systems that are not controlaffine. Herbert et al. [herbert2017fastrack] proposed a hybrid controller, which consists of a regular trajectory tracking controller and a safety controller based on reachability analysis. The reachability analysis is very accurate leading to fast and safe tracking performance but too computationally demanding to be performed online, requiring an a priori known environment. Control barrier function methods [ADASCMEGMKSPT:19, CBF_ames2014rapidly, CBF_wu2015safety, CBF_ames2016control, CBF_quadrotor] have gained significant attention because they allow including safety constraints into a quadratic program optimization of the control input without requiring feedbacklinearization or reachability set approximations.
The reference governor framework [garone2016_ERG, kolmanovsky2014ref_cmd_gov, Gov_ICRA17] is a different approach to enforcing safety constraints that modulates the motion of a virtual system while the real prestabilized system tracks its motion. Arslan and Koditschek [Gov_ICRA17] developed a referencegovernor controller for a doubleintegrator system navigating among spherical obstacles. The key idea is to construct a Lyapunov function in the sumofsquares form and ensure safety by regulating the motion of the governor based on the size of the Lyapunov invariant set. Our work extends this idea significantly by applying it to general feedbacklinearizable controlaffine systems [fliess1995flatness, calvet1988feedforward_quasi_linSys, deluca1998feedback] in the presence of input disturbances.
IB Contributions
The two main contributions of this work are highlighted as follows. First, we develop two methods for obtaining tight bounds on the peak output of a feedbacklinearizable controlaffine system in the presence of disturbances: a semidefinite programming (SDP) and a Lyapunovequation approach. Second, we incorporate the output peak bounds in a governor control design, proving joint safety and stability for output trajectory tracking, and show an application of the complete approach on an Ackermanndrive robot. Our construction relies only on local obstacle information from onboard sensors and, hence, can be used in priori unknown environments.
Ii Notation
Define for , and let . Let denote the Lie derivative of a function
along a vector field
. We use to denote vertical stacking of scalars, vectors, or matrices. Let denote the space of positive definite matrices. The quadratic norm induced by will be denoted by and will be the Euclidean norm. Denote the distance from a point to a set as and let be the distance in the Euclidean norm.Iii Problem Statement
Consider a robot operating in an unknown environment with obstacle space denoted by . Denote the free space by and its interior by . The robot is modeled as a controlaffine nonlinear dynamical system,
(1)  
where denotes the state variables, represents the control input, is the input noise, are the system outputs^{1}^{1}1To simplify the presentation, we consider a system with the same number of inputs and outputs. Our approach relies on feedback linearization, which can generally be applied when fewer or more outputs are available. When more outputs are available, it is sufficient that the rectangular decoupling matrix defined in Def. 2 has full column rank and its leftinverse can be used to define the feedback linearization. If fewer outputs than inputs are available, new ones can be defined following the procedure in [sastry, Prop. 9.16]., and , , are smooth functions. We make the following assumptions.
Assumption 1.
The input noise is bounded, i.e., for some .
Assumption 2.
System (1) satisfies the conditions for feedback linearization in [isidori1995nonlinear, Lemma. 5.2.1] almost everywhere in .
Our approach relies on feedback linearization to transform the highorder nonlinear system dynamics (1) to a space defined by the system outputs , where the dynamics are simple. This is possible for many nonlinear systems, including differentially flat robots such as differentialdrive, Ackermanndrive, fixedwing aerial, and quadrotor robots [Franch2009_ECC, murray1995differential, sreenath2013geometric]. For example, a simple geometric variable such as position may be used as the output of an accelerationcontrolled Ackermanndrive as shown in Sec. VII. Our objective is to design a control policy so that the system outputs follow a desired path in free space without violating safety constraints, i.e, for all .
Definition 1.
A path is a piecewisecontinuous function mapping a pathlength parameter to the interior of the free space. The start of a path is ; the end of a path is .
Many control techniques require a reference trajectory of the same dimension as the original system, which places significant burden on planning systems to ensure dynamic feasibility. Moreover, this decoupling into kinodynamic planning and stable tracking cannot guarantee safety because the trajectory time allocation provided by the planner cannot be altered by the controller. Our approach relies on lowdimensional geometric paths that may be computed efficiently by standard planning algorithms [RRTstar, lavalle2006planning, ARAstar]. The problem considered in this paper is stated below.
Problem 1.
Given a path such that , design a control policy so that the constrained state of (1) is asymptotically steered to , while remaining safe, i.e., for all .
We use feedback linearization under Assumption 2 to find a diffeomorphism that transforms the nonlinear system in (1) to a linear one. Then, we examine the impact of the input noise on the linearized system under Assumption 1 and derive accurate, yet efficiently computable, bounds on the output under stabilizing control. Finally, we introduce a firstorder virtual governor system that enables safe and adaptive tracking of the desired path. The real system aims to converge locally to the governor, while the governor adapts itself using the system trajectory bounds to progress along the path without endangering safety. The structure of the proposed safe adaptive controller is illustrated in Fig 2. Our approach achieves safe navigation with an Ackermanndrive robot in an unknown environment, relying only on onboard depth sensing.
Iv Feedback Linearization
Definition 2.
A controlaffine nonlinear system (1) with outputs for has vector relative degree in a region if, for all :

for all , ,

the decoupling matrix below is invertible:
(2)
Since the relative degree satisfies , we can define a diffeomorphic transformation of the state.
Proposition 1 ([isidori1995nonlinear, Prop. 5.1.2]).
Assume the controlaffine nonlinear system (1) with outputs has vector relative degree in such that . Then, , where:
(3) 
defines a local diffeomorphism.
Proposition 1 allows us to express (1) in new coordinates , where the control input can be chosen to cancel the nonlinearities and obtain a linear system subject to statedependent noise.
Proposition 2.
Applying change of coordinates, , and control input:
(4) 
where is the decoupling matrix in (2) and , to the system in (1) leads to linear dynamics with statedependent noise:
(5) 
where , , are blockdiagonal matrices with blocks in Brunovsky Canonical Form and where is the th row of the decoupling matrix (2).
Proof.
See Appendix A. ∎
V Safe and Stable Regulation
In this section, we choose a stabilizing controller for the linearized system (5) and analyze its peak output in order to guarantee safety. The closedloop system is
(6)  
where is chosen so that is Hurwitz and, in the absence of disturbances, the origin is a globally exponentially stable (G.E.S.) equilibrium. To proceed, we also need a bound on the peak norm of the decoupling matrix.
Assumption 3.
There exists a finite upper bound, , on the norm of the decoupling matrix.
It will be convenient to reorder the states through a permutation matrix such that , with . Due to Prop. 1 such a permutation always exists. Note that
(7) 
and system (6) with reordered states becomes:
(8)  
where , , and . We are interested in finding the output peak of (8) along the system trajectory:
(9) 
for some to be specified later. Finding the exact output bound may be challenging [abedor1996linear]. Instead, we compute the tightest upper bound on over an ellipsoid outer approximation of all possible system trajectories of (8) starting at . Denote an ellipsoid in , centered at and defined by , as:
(10) 
Definition 3.
An ellipsoid is positively invariant for dynamical system (8) if implies , for all and every system trajectory .
Thus, if is a positively invariant ellipsoid for (8), we can obtain an upper bound on as follows:
(11) 
The following result allows us to find a positively invariant ellipsoid, centered at the equilibrium point of (8), in the presence of bounded disturbances.
Lemma 1 ([boyd_LMI_book]).
Consider system (8) with bounded disturbance . The ellipsoid is positively invariant if and only if there exists a real scalar such that
(12) 
Theorem 1.
Consider system (8) with bounded disturbance . The output of any trajectory, starting at , is bounded for all as follows:
(13) 
where and is the solution to the semidefinite program (SDP):
(14)  
subject to  
Moreover, is the ultimate bound for as .
Proof.
See Appendix B. ∎
Thm. 1 shows that an output peak bound may be obtained via a twolevel optimization. The lowerlevel is an parameterized SDP. The upperlevel is a scalar minimization of a scalar function over a bounded region , which may be approached with a global optimization method such as the Brent’s algorithm [brent_algorithm, Ch. 5]. While the bound computed by this optimization is very accurate, computation speed may be a concern for highfrequency control applications. An alternative looser bound on may be obtained by solving a series of algebraic Lyapunov equations.
Theorem 2.
Consider system (8) with bounded disturbance . The output of any trajectory, starting at , is bounded for all as follows:
(15) 
where and
(16) 
where is the solution of Lyapunov equation:
(17) 
Moreover, is an ultimate bound for as .
Proof.
See Appendix C ∎
Thm. 2 does not jointly consider the initial condition constraint while minimizing the output bound. As a result, . The two values are usually not the same. when is not negligible compared to . Albeit looser, the bound in Thm. 2 is computationally much cheaper to obtain than solving a series of SDPs, as required by Thm. 1.
Vi Safe and Stable Tracking using a Reference Governor
Instead of regulating the linearized system (8) to a fixed equilibrium, we will use the output trajectory bound provided by Thm. 1 or Thm. 2 to decide whether it is safe to move the equilibrium point along the desired path . We introduce a reference governor [garone2016_ERG, kolmanovsky2014ref_cmd_gov], a virtual firstorder system whose state behaves as a realtime adaptive output reference signal, slowing down if the output bound may violate safety and speeding up otherwise. The goal is to design a governor control policy that tracks the desired output path , while not allowing the output bound to exceed the distance to the obstacles . Consider an augmented robotgovernor system with state defined as:
(18)  
where the equilibrium point of (8) has been shifted to so that the output of the linearized system tracks . We define a local safe zone for the augmented system based on the output bound in (13).
Definition 4.
A local safe zone is a timevarying set that at time depends on the robotgovernor state as follows:
(19) 
where is a measure of leeway to safety violation, is a bound on the output peak of measured by a quadratic norm defined by , and is a small constant to accommodate numerical errors.
The first term, , in the definition of estimates the (quadratic) distance from the desired system equilibrium to the nearest obstacles. The second term, evaluates the maximum deviation of the system output from the equilibrium point . The value reflects the level of risk of the robotgovernor system at the current state . Intuitively, it is a measure of remaining energy that the system or the governor may use before safety is endangered. The requirement that only places a constraint on the magnitude of , so is a degree of freedom that can be utilized to make asymptotically tend toward a desired goal region. We use any available energy to move the governor along the desired path .
Definition 5.
A local projected goal is the furthest point along the path that intersects with the local safe zone :
(20) 
We use the informal notation because determining the local projected goal is equivalent to finding the metric projection of on the boundary of the local safe zone. We define the governor control policy to track the local projected goal:
(21) 
where is a control gain for the governor controller.
Note that the local safe zone set is an ellipsoid whose size is determined by the free energy and whose shape is determined by the positive definite matrix . If the volume of is large (no nearby obstacles), the local projected goal will be far along , leading to fast governor and, in turn, system motion. The shape of can be chosen via to obtain fast tracking even if there are nearby obstacles that do not interfere with the direction of motion. More precisely, may be chosen adaptively to be elongated in the direction from the system output to the equilibrium point .
Finally, we derive conditions that guarantee the robot remains safe and ultimately bounded around a static equilibrium and that the control policy (21) applied to the robotgovernor system (18) tracks the desired path safely, converging to a small region around the goal, determined by the input noise bound .
Theorem 3.
Let be any initial state for (18) with and let so that the governor remains static, i.e., . Suppose that the following safety condition is satisfied:
(22) 
where is an upper bound on , e.g., obtained from Thm. 1 or Thm. 2 for any . Then, output trajectory is collision free, i.e., for all , and ultimately bounded by .
Proof.
See Appendix D ∎
Definition 6.
The set of safe states of the robotgovernor system (18) includes all states with strictly positive free energy and associated local safe zone with nonempty intersection with the path :
(23) 
Definition 7.
Theorem 4.
Suppose that the path satisfies the following safety condition:
(25) 
for some constant and bound on the peak output of system (1) under the feedbacklinearizing controller in (6). Then, the closedloop robotgovernor system (18) with governor control policy in (21) is asymptotically steered from any safe initial state to a goal region and the robot output trajectory is collisionfree for all time, i.e., for all .
Proof.
See appendix E. ∎
Vii Application: AckermannDrive Robot
We demonstrate the proposed referencegovernor controller on a ground wheeled robot with an Ackermann steering mechanism. The state of the robot consists of its position , orientation and steering angle , while its input is the rearaxle driving speed and the steering angular velocity . The kinematic model [deluca1998feedback] describing the robot’s motion is:
(26) 
where is the distance between the front and rear axles. The model assumes that there is no wheel slip and the two front wheels have the same steering angle. In the literature, this model is also referred to as a bicycle model [deluca1998feedback], singletrack model [ackermann1993robust], or Ackermann vehicle model [Franch2009_ECC].
The system (26) is not static feedback linearizable but it is linearizable via dynamic extension [fliess1995flatness, isidori1995nonlinear]. Speed and acceleration are added to the system state , while jerk is added as a control input . The augmented system can be written in the controlaffine form (1) with bounded input disturbance :
(27) 
Choosing position as an output, , makes the system outputfeedback linearizable:
(28) 
Note that , so is nonsingular if and . Due to the geometry of the steering mechanism, the wheels can never be perpendicular to the vehicle body, while the nonzero speed requirement can be satisfied by initializing our controller just a small initial speed. Hence, Assumption 2 is satisfied. The system has vector relative degree and , which equals the number of states. By Prop. 1, the system is fullstate inputoutput linearizable with new coordinates . By Prop. 2, applying the control input leads to the linear system with statedependent noise in (5).
Instead of assuming a conservative bound on the input noise , we propose theres exist an operating profile that relates the allowable speed and steering angle of the car. The profile specifies that as the speed is increasing, the allowable steering angle is decreasing. Since , the operating envelope ensures that some upper bound exists on . From (28), we can compute:
(29) 
showing that Assumption 3 is satisfied with . Following the approach in Sec. V, we choose a stabilizing control gain for the linear system in (5) and reorder the states as , arriving at the system in (8). The reminder of the control design, including the local safe zone and projected goal computations in (19) and (20), is agnostic to the original system dynamics.
Viii Evaluation
This section evaluates the output peak bounds provided by Thm. 1 and Thm. 2 and demonstrates the complete control design for safe ackermanndrive robot navigation in a simulated environment.
Viiia Output Prediction for Stochastic Linear Systems
Consider the linearized system (8) corresponding to the Ackermanndrive robot described in Sec. VII. Fig. 3 compares the invariant ellipsoids containing the system output, predicted by Thm. 1 and Thm. 2 for two different choices of . We can observe that all four ellipsoids are valid outer approximations of the space of output trajectories. The SDPbased method (Thm. 1) leads to tighter bounds than the Lyapunovequationbased method (Thm. 2) regardless of how the quadratic output norm is defined. Using a quadratic norm aligned with the initial direction of motion of the system, , leads to a large improvement on the tightness of the SDPbased output peak bound. The Lyapunovequationbased method does not benefit significantly from a specific choice of because the bound estimation does not take the initial condition into account when minimizing the size of the invariant ellipsoid.
ViiiB Simulated AckermannDrive Navigation
Next, we demonstrate the performance of the complete control design on the Ackermanndrive robot in a simulated navigation tasks. The robot is operating in an unknown environment and relies on a simulated Lidar scanner to measure the Euclidean distance from the governor to the obstacles (see Fig. 5). The path is replanned using the algorithm [ARAstar] from the current governor position to a fixed goal location within an occupancy grid map [ProbabilisticRoboticsBook, Ch. 9] constructed from the lidar scans over time. It is important to note that the controller does not rely on the map of the environment. It only tracks the geometric path provided by , relying on the lidar scanner to evaluate the local safe zone in (19) and determine the the local projected goal in (20). The governor regulates the tracking speed using the output peak bounds proposed in Sec. V, slowing down when new obstacles appear to ensure that the real robot system remains safe and stable. Fig. 4 shows that the output trajectory bounds remain valid over time and that a safe distance from the obstacles is maintained. We can see that the bounds computed by the SDPbased and Lyapunovequationbased methods are quite similar and both are valid overapproximations of the space of paths from the robot position ot the governor position . As noted earlier, the Lyapunovequationbased method is much more computationally efficient, while the SDP method offers very high accuracy, especially with an appropriate choice of a (timevarying) quadratic norm scaling . Since throughout the simulation, the triangle inequality guarantees that and hence the robot is safe, , for all time .
Ix Conclusion
This paper presented a safe, stable, and fast pathfollowing control design for nonlinear systems subject to bounded disturbances. The controller relies on feedbacklinearization and requires only an output reference signal (rather than a highorder reference model), allowing the use of an efficient geometric path planner for autonomous navigation with differentially flat robots, such as cars or quadrotors. The design guarantees joint stability and safety using only local obstacle information, making it suitable for navigation in unknown environments. Future work will focus on additional hardware experiments pushing the performance limits of the controller in challenging environments and will incorporate ideas from controlbarrierfunctionbased designs which avoid the limitations of feedback linearization. The simplicity of the proposed safety criteria also creates a promising avenue for research on safe online learning of robot dynamics.
Appendix A
Applying the change of variables transforms the system (1) to normal form:
(30)  
From Def. 2, for all , , , leading to
(31)  
where and is the decoupling matrix in (2). Applying , leads to the following system in coordinates :
(32)  
where where is the th row of the decoupling matrix (2). The matrices , , are blockdiagonal, , , with elements , , in Brunovsky Canonical Form (BCF):
(33) 
Appendix B
From lemma 1, find a invariant ellipsoid is equivalent to an find a feasible solution for constraint (12) for some . Let represents the upper bound on over all possible realization of trajectories subject to starting with initial state at . Without loss of generality, consider . Let be an parameterized invariant ellipsoid. Without loss of generality, let us assume first, i.e., , we have:
change of variable  
Rayleigh Quotient 
Obtaining the smallest upper bound on is equivalent to solve the following optimization problem
(34)  
subject to  (35) 
Using Schur complement and Sylvester’s law of inertia, inequality (35) can be expressed as a LMI:
(36) 
Considering initial condition , we need to incorporate additional constraint . In order to find the invariant set with smallest upper bound , we need to solve the optimization problem over all . The upper bound of is obtained by presolving of LMI (12), which requires to be Hurwitz. The ultimate bound of can be obtained by solving this optimization problem without initial value constraint , since the linear system is G.E.S. the effect of initial condition will becomes negligible as . Since , replacing the with leads to bounds on straightforwardly. ∎
Appendix C
This result follows from the work of Abedor et al. [abedor1996linear] and Brockman and Corless [brockman1998quadratic]. If is positively invariant for (8) with , then Lemma 1 implies that there exists such that . Hence, the smallest invariant ellipsoids are generated by solutions to (17) as sweeps . Still assuming , from (11), the output peak satisfies
(37) 
Sec. 5 in [brockman1998quadratic] shows how to incorporate a nonzero initial condition in the bound to obtain the result in (16).∎
Appendix D
If the governor is static at and , the robotgovernor system (18) is G.E.S. with respect to the equilibrium since in the linear timeinvariant system: is Hurwitz. From Thm. 1 or Thm. 2, when the subsystem is subject to bounded disturbance , the output norm is bounded by for all . From the ellipsoid definition in (10), is equivalent to , which implies . In turn, the safety condition in (22) implies . Finally, by definition of , we can assure . As , the size of the ellipsoid will be decreasing and since the noisefree equilibrium point is G.E.S., will be the ultimate bound for .∎
Appendix E
From Thm. 3, we know that if the governor is static at , the robotgovernor system (18) will follow a collisionfree trajectory and will be ultimately bounded around the equilibrium point . The governor control policy in (21) allows the governor to move only when the interior of is nonempty. From Def. 4, this happens only if the safety condition is strictly satisfied, i.e.,