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 sampling-based techniques [RRT, RRTstar, BITstar], generate safe shortest paths in complex high-dimensional problems. However, ensuring that the planned paths are dynamically feasible and remain safe when a real-time 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 high-order dynamically feasible trajectories directly simplifying the real-time control task. However, is challenging to plan trajectories over high-order 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 closed-loop controller are difficult to obtain.
The focus of this paper is a control design for nonlinear control-affine 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 low-order geometric path is provided. Our approach relies on feedback linearization to determine invariant ellipsoid bounds on the closed-loop robot trajectory under stabilizing control. In contrast to existing feedback-linearization procedures, we examine the impact of disturbances carefully and develop semi-definite programming (SDP) and Lyapunov-equation techniques to tightly and computationally efficiently bound the worst-case system behavior. Inspired by reference governor control techniques [garone2016_ERG, kolmanovsky2014ref_cmd_gov, Gov_ICRA17], we develop a first-order 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 path-tracking speed to guarantee that the real robot remains safe and stable.
I-a Related Work
Safe corridor construction [SFC, SFC_FM] during geometric motion planning is a promising approach for generating high-order 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 sampling-based 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 finite-horizon closed-loop trajectories with a sampled disturbance sequence. A Q-value 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 over-approximations. 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 sum-of-squares (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 trade-off 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 Hamilton-Jacobi reachability, one can deal with nonlinear systems that are not control-affine. 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 [ADA-SC-ME-GM-KS-PT: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 feedback-linearization 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 pre-stabilized system tracks its motion. Arslan and Koditschek [Gov_ICRA17] developed a reference-governor controller for a double-integrator system navigating among spherical obstacles. The key idea is to construct a Lyapunov function in the sum-of-squares 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 feedback-linearizable control-affine systems [fliess1995flatness, calvet1988feedforward_quasi_linSys, deluca1998feedback] in the presence of input disturbances.
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 feedback-linearizable control-affine system in the presence of disturbances: a semi-definite programming (SDP) and a Lyapunov-equation 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 Ackermann-drive robot. Our construction relies only on local obstacle information from onboard sensors and, hence, can be used in priori unknown environments.
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 control-affine nonlinear dynamical system,
where denotes the state variables, represents the control input, is the input noise, are the system outputs111To 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 left-inverse 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.
The input noise is bounded, i.e., for some .
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 high-order 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 differential-drive, Ackermann-drive, fixed-wing 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 acceleration-controlled Ackermann-drive 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 .
A path is a piecewise-continuous function mapping a path-length 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 low-dimensional geometric paths that may be computed efficiently by standard planning algorithms [RRTstar, lavalle2006planning, ARAstar]. The problem considered in this paper is stated below.
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 first-order 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 Ackermann-drive robot in an unknown environment, relying only on onboard depth sensing.
Iv Feedback Linearization
A control-affine nonlinear system (1) with outputs for has vector relative degree in a region if, for all :
for all , ,
the decoupling matrix below is invertible:
Since the relative degree satisfies , we can define a diffeomorphic transformation of the state.
Proposition 1 ([isidori1995nonlinear, Prop. 5.1.2]).
Assume the control-affine nonlinear system (1) with outputs has vector relative degree in such that . Then, , where:
defines a local diffeomorphism.
Applying change of coordinates, , and control input:
where , , are block-diagonal matrices with blocks in Brunovsky Canonical Form and where is the -th row of the decoupling matrix (2).
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 closed-loop system is
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.
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
and system (6) with reordered states becomes:
where , , and . We are interested in finding the output peak of (8) along the system trajectory:
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:
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:
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
Consider system (8) with bounded disturbance . The output of any trajectory, starting at , is bounded for all as follows:
where and is the solution to the semi-definite program (SDP):
Moreover, is the ultimate bound for as .
See Appendix B. ∎
Thm. 1 shows that an output peak bound may be obtained via a two-level optimization. The lower-level is an -parameterized SDP. The upper-level 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 high-frequency control applications. An alternative looser bound on may be obtained by solving a series of algebraic Lyapunov equations.
Consider system (8) with bounded disturbance . The output of any trajectory, starting at , is bounded for all as follows:
where is the solution of Lyapunov equation:
Moreover, is an ultimate bound for as .
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 first-order system whose state behaves as a real-time 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 robot-governor system with state defined as:
A local safe zone is a time-varying set that at time depends on the robot-governor state as follows:
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 robot-governor 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 .
A local projected goal is the furthest point along the path that intersects with the local safe zone :
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:
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 robot-governor system (18) tracks the desired path safely, converging to a small region around the goal, determined by the input noise bound .
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:
See Appendix D ∎
The set of safe states of the robot-governor system (18) includes all states with strictly positive free energy and associated local safe zone with non-empty intersection with the path :
Suppose that the path satisfies the following safety condition:
for some constant and bound on the peak output of system (1) under the feedback-linearizing controller in (6). Then, the closed-loop robot-governor 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 collision-free for all time, i.e., for all .
See appendix E. ∎
Vii Application: Ackermann-Drive Robot
We demonstrate the proposed reference-governor 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 rear-axle driving speed and the steering angular velocity . The kinematic model [deluca1998feedback] describing the robot’s motion is:
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], single-track 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 control-affine form (1) with bounded input disturbance :
Choosing position as an output, , makes the system output-feedback linearizable:
Note that , so is non-singular if and . Due to the geometry of the steering mechanism, the wheels can never be perpendicular to the vehicle body, while the non-zero 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 full-state input-output linearizable with new coordinates . By Prop. 2, applying the control input leads to the linear system with state-dependent 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:
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 re-order 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-a Output Prediction for Stochastic Linear Systems
Consider the linearized system (8) corresponding to the Ackermann-drive 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 SDP-based method (Thm. 1) leads to tighter bounds than the Lyapunov-equation-based 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 SDP-based output peak bound. The Lyapunov-equation-based 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.
Viii-B Simulated Ackermann-Drive Navigation
Next, we demonstrate the performance of the complete control design on the Ackermann-drive 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 re-planned 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 SDP-based and Lyapunov-equation-based 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 Lyapunov-equation-based method is much more computationally efficient, while the SDP method offers very high accuracy, especially with an appropriate choice of a (time-varying) quadratic norm scaling . Since throughout the simulation, the triangle inequality guarantees that and hence the robot is safe, , for all time .
This paper presented a safe, stable, and fast path-following control design for non-linear systems subject to bounded disturbances. The controller relies on feedback-linearization and requires only an output reference signal (rather than a high-order 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 control-barrier-function-based 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.
Applying the change of variables transforms the system (1) to normal form:
From Def. 2, for all , , , leading to
where and is the decoupling matrix in (2). Applying , leads to the following system in coordinates :
where where is the -th row of the decoupling matrix (2). The matrices , , are block-diagonal, , , with elements , , in Brunovsky Canonical Form (BCF):
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|
Obtaining the smallest upper bound on is equivalent to solve the following optimization problem
Using Schur complement and Sylvester’s law of inertia, inequality (35) can be expressed as a LMI:
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 pre-solving 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. ∎
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
Sec. 5 in [brockman1998quadratic] shows how to incorporate a non-zero initial condition in the bound to obtain the result in (16).∎
If the governor is static at and , the robot-governor system (18) is G.E.S. with respect to the equilibrium since in the linear time-invariant 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 noise-free equilibrium point is G.E.S., will be the ultimate bound for .∎
From Thm. 3, we know that if the governor is static at , the robot-governor system (18) will follow a collision-free 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.,