Robust Planning and Control For Polygonal Environments via Linear Programming
RRobust Planning and Control For Polygonal Environments via LinearProgramming
Mahroo Bahreinian , Erfan Aasi and Roberto Tron Abstract — In this paper we are concerned with the designof a set of controllers, on a cell decomposition of a polygonalenvironment through Linear Programming. The core of ourproposed method consists of a convex min-max formulationthat synthesizes an output-feedback controller, based on relativedisplacement measurements with respect to a set of landmarks.The optimization problem is formulated using piece-wise linearControl Lyapunov Function and Control Barrier Functionconstraints, to provide guarantees of stability and safety. Theinner maximization problem ensures that these constraints aremet by all the points in each cell, while the outer minimiza-tion problem balances the different constraints to optimizerobustness. We convert this min-max optimization problem toa regular Linear Programming problem, by forming the dualof the inner maximization problem. Although in principle ourapproach is applicable to any system with piecewise lineardynamics, in this paper as a proof of concept, we apply it to firstand second order integrators. We show through simulations thatthe resulting controllers are robust to significant deformationsof the environment.
I. INTRODUCTIONMotion planning of robots and providing controls for themare among the popular topics that have received a lot ofattention. These problems get more interesting when therobots, with detailed dynamical models, are moving in acomplex environment. On one side of this concern differenttype of approaches have been proposed with focus on justcapturing the complexity of the environment, such as potentialfields [9], [10] and probabilistic roadmaps [11]; while on theother side some methods have just investigated the role ofthe robot’s dynamic model in the problem, such as inputparameterizations [13].There has been a noticeable amount of work recentlywith the idea of integrating these subjects and are focusedon combining the continuous nature of robots’ dynamicmodels with a discretized version of the environment. In[7] by polygonal partition of the environment they proposean automatic control approach which is applicable over aspecific region of robot’s dynamical system. In [4] theyprovide provably correct control laws for the robots, basedon the discrete algorithms that are used for handling thecomplexity of the robot’s environment.
This work was supported by ONR MURI grant “Neuro-Autonomy:Neuroscience-Inspired Perception, Navigation, and Spatial Awareness” Mahroo Bahreinian is with Division of Systems Engineering at BostonUniversity, Boston, MA, 02215 USA. Email: [email protected] Erfan Aasi is with Department of Mechanical Engineering And Sys-tems Engineering at Boston University, Boston, MA, 02215 USA. Email: [email protected] Roberto Tron is with Faculty of Department of Mechanical Engineeringat Boston University, Boston, MA, 02215 USA. Email: [email protected]
One of the challenging questions about providing controllaws for robots is how to obtain a control, when both thesafety and stability properties are important and may haveconflict with each other. This question has been investigatedrecently in some works, such as [3], [8] and [1], wherethe problem of having conflict between safety and stabilitypurposes is solved by mediating them through a quadraticprogram (QP).Having that in mind, in this paper we propose a controlalgorithm in the context of optimization-based algorithmsfor robots in polygonal environments. In this regard we firstdecompose the environment into a set of convex sectionsand then for each section of the environment consider aset of points as landmarks. Then for satisfying the forwardinvariance property for each section, specified in terms ofcontrol barrier functions, and also moving in the stabilizingdirection of the section, which is specified in terms of controlLyapunov functions, an optimization problem is solved toprovide the control law for each section. This control law isbased on combination of distance to the landmarks of eachsection, where the combination coefficient is obtained fromthe optimization problem. Among the previous works in thisarea, the work in [4] is mostly related to this paper.The paper is structured as follows: In section II somenecessary definitions are presented. Then the problem for-mulation for both first and second order systems, and ourproposed method for solving them are given in sections IIIand IV. Simulation results for each of the systems are shownin section V, and then finally in section VI the concludingremarks and the future works of this research are stated.II. NOTATION AND PRELIMINARIESIn this section we review and extend definitions of ControlBarrier Functions and Control Lyapunov Functions, and defineour environment model.
A. System dynamics
We start by considering a control-affine dynamical system ˙ x = f ( x ) + g ( x ) u, (1)where x ∈ U x ⊂ R n is the state of the agent, u ∈ U a ⊂ R m is the system input, U a is a convex set defining actuator limits,and f ( x ) , g ( x ) are locally Lipschitz continuous functions. We review the concepts of CBFs and CLFs in the general nonlinearsetting, although in our work we will assume linear time-invariant systems,and use only affine barrier functions. a r X i v : . [ ee ss . S Y ] O c t . Control Barrier Functions and Control Lyapunov Func-tions Suppose we have a continuously differentiable function h ( x ) : R n → R which defines a safe set C such that C = { x ∈ R n | h ( x ) ≥ } ,∂C = { x ∈ R n | h ( x ) = 0 } ,Int ( C ) = { x ∈ R n | h ( x ) > } . (2)We say that the set C is forward invariant if x ( t ) ∈ C implies in x ( t ) ∈ C , for all t ∈ [ t , t max ) where x ( t ) is welldefined [14]. Definition 1:
The Lie derivative of a differentiable function h with respect to a vector field f ( x ) is defined as L f = ∂h∂x T f ( x ( t )) . The Lie derivative of order r is denoted as L rf , and is recursively defined by L rf h = L f ( L r − f h ) , with L f h = L f h [15]. Definition 2:
A function h is said to have relative degree r with respect to the dynamics (1) if L g L ( i ) f h = 0 for ≤ i ≤ r − and L g L r − f h (cid:54) = 0 ; equivalently, it is the minimumorder of the time derivative of the system ( h ( r ) ) that explicitlydepends on the inputs u .Given a sufficiently continuously differentiable function h ( x ) having relative degree r for the dynamics (1), we definethe transversal state ξ h ( x ) = h ( x )˙ h ( x ) ... h ( r − ( x ) = h ( x ) L f h ( x ) ... L ( r − f h ( x ) , (3)and the transversal dynamics ˙ ξ h ( x ) = A h ξ h ( x ) + B h µ h , (4)where A h ∈ R r × r and B h ∈ R r × are defined as A h = . . . ... ... . . . ... , B h = ... , (5)and µ h is a virtual input. These concepts are used below fordefining CBFs and CLFs. Definition 3 (ECBF, [12]):
Consider the control system(1), and a continuously differentiable function h ( x ) withrelative degree r ≥ defining a forward invariant set C asin (2). The function h ( x ) is an Exponential Control BarrierFunction (ECBF) if there exist K h ∈ R × r and control inputs u ∈ U such that L rf h ( x )+ L g L r − f h ( x ) u + K h ξ h ( x ) ≥ , ∀ x ∈ Int ( C ) , (6)and such that K h stabilizes the transversal dynamics, i.e., thematrix A h − B h K h has eigenvalues with negative real parts.The latter condition ensures that the set C is forward invariant[12, Theorem 2]. If r b = 1 , then h ( x ) is a Zero ControlBarrier Function (ZCBF, [14], [15]).Note that in this work, we will assume that h ( x ) is a linearfunction. Next, we present an analogous definition for an extensionof Control Lyapunov Functions [2] Definition 4:
Consider the control system (1), and a con-tinuously differentiable function V ( x ) : R n → R , V ( x ) ≥ ,with relative degree r ≥ . The function V ( x ) is an Exponential Control Lyapunov Function (ECLF) if there existconstants c , c ≥ such that, c α ( (cid:107) x (cid:107) ) ≤ V ( x ) ≤ c α ( (cid:107) x (cid:107) ) , (7)where α is a class- K function, and if there exists K V ∈ R × r and control inputs u ∈ U such that L rf V ( x ) + L g L r − f V ( x ) u + K V ξ V ( x ) ≤ , ∀ x ∈ Int ( C ) , (8)where C is a forward-invariant set, and K V stabilizes thetransversal dynamics, i.e., the matrix A V − B V K V haseigenvalues with negative real parts.For r = 1 , we recover the definition of ExponentiallyStabilizing CLFs (ES-CLFs, [2]). It is possible to use ECLFto design controllers that exponentially stabilize the originaldynamics (1), as shown by the following: Proposition 1:
Given a function V ( x ) ≥ and a control u ( x ) satisfying the inequality in (8), then lim t →∞ V ( x ( t )) =0 with exponential convergence. Moreover, if the function V ( x ) in addition satisfies (8), then V ( x ) is a ECLF and lim t →∞ = 0 with exponential convergence. Proof:
The proof mirrors a simplified version of theideas in [12]. Setting the virtual input µ V in the transversaldynamics to µ V = − K V ξ V , (9)we have that lim t →∞ ξ V = 0 with exponential convergence(since it is an LTI system and K V contains stabilizingfeedback gains). If we pick µ V ≤ − K V ξ V , we obtain ˙ ξ V ≤ ( A V − B V K V ) ξ V , (10)in which the last element correspond to (8). ApplyingGronwall’s comparison lemma, we then conclude that lim t →∞ ξ V = 0 , which, in particular, implies lim t →∞ V = 0 .Using standard arguments from Lyapunov theory, we thenhave that the addition of condition (7) implies lim t →∞ x = 0 .Note that in this work, we assume V ( x ) is a piecewiselinear function. C. Polygonal environment decomposition
We assume a polygonal environment, potentially non-simply-connected, and potentially with a large number ofvertices; we decompose the polygonal environment to a finitenumber of convex cells.
Definition 5:
A polygonal environment P is a closed envi-ronment defined by a finite number of vertices { p , . . . , p v } where p ∈ R n and v is the number of vertices. We partitionthe environment in a set of convex sets { U x,i } , such that (cid:83) i U x,i = P , and such that each set U x,i is a polytope definedby liner inequality constraints of the form A x x ≤ b x .Our goal is to design a different linear feedback controller u for each cell U x . The feedback signal used by the controllerill be based on linear relative measurements with respectto a set of landmarks . Definition 6:
A landmark is defined as a point y ∈ R n whose location is known and fixed in the environment.For each convex section, we have a finite number oflandmarks. In this paper, we choose the landmarks as thevertices of the convex section, although this choice does notmake any difference in terms of the actual method. D. High-level planning
We consider two overall objectives for the controller design:(O1) Point stabilization: given the stabilization point in theenvironment and starting from any point, we aim toconverge to the stabilization point (e.g. Fig. 3).(O2) Patrolling: starting from any point, we aim to patrol theenvironment by converging to a path and then, traversingthe same path (e.g. Fig. 7a).To specify the convergence objective for each controller u ,we first abstract the polygonal environment P into a graph G = ( V, E ) , where each vertex i ∈ V represents a cell U x in the partition of P , and an edge ( i, j ) ∈ E if and only ifcells corresponding to i and j have a face in common.In the case of the point stabilization objective (O1), thestabilization point is one of the vertices of the graph and ifthe stabilization point is in the middle of the cell, without lossof generality, we can decompose the cell into new convexcells such that the stabilization point is one of the vertices ofthe new cells. Then, we add one vertex to the set V , which isthe stabilization point and also, add edges between the newvertex and any cells which have a face in common with thecell includes the stabilization point to the set E .For each cell, we then select one exit edge (a pointer)such that, when considered together, all such edges providea solution in the abstract graph G to the high level objective.For instance, in the case of objective (O1), the exit edgeof each cell will point in the direction of the shortest pathtoward the vertex of the stabilization point. In the case ofobjective (O2), following the exit edges will lead to a cyclicpath in the graph.To give an example, the polygonal environment in Fig. 1ais converted to the connected graph in Fig. 1c based on thecell decomposition of the environment in Fig. 1b. Startingfrom the first node in Fig. 1c which is shown by green point,we find the path from the start node to the equilibrium nodeshown by red point through the path planning algorithms (e.g.using Dijkstra’s algorithm). Regarding to that path, we definethe exit face as the face of the convex section the path movesthrough and based on that we design the controller. Definition 7:
For each cell U x in the decomposition ofthe environment, we define an exit face or stabilization point P exit to be the face or vertex corresponding to the exit edge inthe abstract graph G . The exit direction n is an inward-facingnormal of P .In this work we desire to design a controller for eachconvex section of the environment that drives the system inthe exit direction toward the exit face, while avoiding theboundary of the environment. (a) (b) (c) Fig. 1: The polygonal environment in Fig .1a is decomposedto 8 convex sections Fig .1b and the corresponding graph isshown in Fig . 1cOverall, thanks to the high level planning in the abstractgraph G , and the controller design in each cell U x (explainedin the sections below) the system will traverse a sequence ofcells to reach a given equilibrium point, or achieve a periodicsteady state behavior (see examples in Section V) accordingto the desired objective.In the following sections III and IV we propose our methodand validate it through some examples in section V.III. PROBLEM SETUP, FIRST ORDER SYSTEMAssume we have an environment decomposed in a finitenumber of convex polytopic sections. For each sectionconsider the control system, ˙ x = u, x ∈ U x , u ∈ U a , (11)where x is the position of the agent in the environment U x ⊂ R n and u is the controller which is bounded by U a ⊂ R n .We assume that these convex sets are polytopes defined by, U x = { x | A Tx x ≤ b x } , (12) U a = { u | A Ta u ≤ b a } , (13)where A x ∈ R ( n × d ) , A a ∈ R ( n × q ) and b x ∈ R d and b a ∈ R q .Our goal is to find a feedback controller that, given therelative displacements between the robot’s position x and thelandmarks in the environment, provides an input u that drivesthe system toward an exit face or vertex of U x while avoidingobstacles (non-exit faces of U x ). Note that the landmarks donot necessarily need to belong to U x . For this section, wechoose a controller of the form: u ( k ) = ( y − x T ) k, (14)where y ∈ R ( n × m ) is a matrix where each column representsthe coordinates of one of m landmarks, ∈ R m , and k ∈ R m is a vector of feedback gains that need to be designed. Notethat y − x T gives a matrix where each column is the relativedisplacement between each landmark and the current positionof the system; as such, we are looking for a controller thatfeeds back linear combinations of these displacements. A. Control Barrier Function
Let A i ∈ R n and b i ∈ R , i ∈ { , . . . , d } , representindividual columns and elements of the matrix A x and vector b x , i.e., individual faces of the cell, except the one associatedto an exit face (or all of them in the case of a stabilizationoint). For each one of these constraints we define thefollowing candidate ECBF: h i ( x ) = A Ti x + b i ≥ , i ∈ d (15)where A i ∈ R n and b i is scalar, and d is the number ofequations needed to define the convex section and h i ( x ) is aZCBF. B. Control Lyapunov Function
To stabilize the system, we define the Lyapunov function V ( x ) as, V ( x ) = n T x, (16)where n ∈ R n is the exit direction for the cell (SeeDefinition 7). Note that this Lyapunov function represents,up to a constant, the distance d ( x, P exit ) between the currentsystem position and the exit face. C. Finding the Controller by Robust Optimization
Our goal is to find controllers u (more precisely, controlgains k ) that maximize the motion of the robot toward theexit face, while avoiding the boundary of the environment.Using the CLF-CBF constraints reviewed in Section II, weencode our goal in the following optimization problem: min k n T us.t. L f h i ( x ) + L g h i ( x ) u + K h h i ( x ) ≥ , L f V ( x ) + L g V ( x ) u + K l V ( x ) ≤ ,u ∈ U a , ∀ x ∈ U x , (17)where K h and K l are positive scalars, since h i and V all haverelative degree r = 1 . Note that the constraints in 17 needto be satisfied for all x in the cell U x , i.e., the same controlgains should satisfy the CLF-CBF constraints at every pointin the cell. We handle this type of constraint by rewriting 17using a min-max formulation: where (17) is equivalent to, min β,k βs.t n T u ≤ β, max x − ( L f h ( x ) + L g h ( x ) u + α ( h ( x ))) ≤ , max x L f V ( x ) + L g V ( x ) u + c V ( x )] ≤ ,x ∈ U x , u ∈ U a , (18)We relax the two constraints in (18) by introducing the slackvariables S b and S l as, min β,k,S l ,S b β + S l + S b s.t n T u ≤ β, (cid:20) max x − ( L f h ( x ) + L g h ( x ) u + α ( h ( x ))) s.t x ∈ U x , u ∈ U a (cid:21) ≤ S b , (cid:20) max x L f V ( x ) + L g V ( x ) u + c V ( x ) s.t x ∈ U x , u ∈ U a (cid:21) ≤ S l ,β, S l , S b ≤ (19) substituting V ( x ) from (16), h ( x ) from (15) and u from (14)results in, min β,k,S l ,S b β + S l + S b s.t n T ( y − x ) k ≤ β, (cid:20) max x A Ti x k − c A Ti xs.t A Tx x ≤ b x , i ∈ d (cid:21) ≤ S bi + c b i + A Ti yk, (cid:20) max x − n T x k + c n T xs.t A Tx x ≤ b x , (cid:21) ≤ S l − n T yk, (cid:20) max x − A Ta x ks.t A Tx x ≤ b x , (cid:21) ≤ b a − A Ta yk,β, S l , S b ≤ (20)In (20), we have a bi-level optimization problem withconstraints that are given themselves by other optimizationproblems. As all constraints and objective function are linearand U x and U a are convex set, (20) and inner maximizationproblems are linear programming problem so we can changethe min-max problem (20) to min-min problem by replacingthe inner maximization problems with their dual forms. Inaddition, the first and third constraints in (20) can be mergedto one constraint as, min β,k,S l ,S b β + S l + S b s.t. min λ h b Tx λ h ≤ S bi + c b i + A Ti yks.t : A Tx λ h = [ A Ti x ( n × t ) k − c A Ti ] λ h ≥ , i ∈ d, min λ l b Tx λ l ≤ S l − n T yks.t : A Tx λ l = [ − n T ( n × t ) k + c n T ] ,λ l ≥ , min λ a b Tx λ a ≤ b a − A Ta yks.t : A Tx λ a = [ − A Ta ( n × t ) k ] λ a ≥ ,β, S l , S b , S a ≤ , λ h , λ l , λ a ≥ . (21)where min-min problem (21) is equivalent to the minimiza-tion problem, min β,k,S l ,S b ,λ hi ,λ l ,λ a β + S l + S b s.t. b Tx λ hi ≤ S bi + c b i + A Ti ykA Tx λ hi = [ A Ti x ( n × t ) k − c A Ti ] b Tx λ l ≤ S l − n T ykA Tx λ l = [ − n T ( n × t ) k + c n T ] ,b Tx λ a ≤ b a − A Ta ykA Tx λ a = [ − A Ta ( n × t ) k ] β, S l , S b ≤ , λ hi , λ l , λ a ≥ , (22)In the following we prove that the feasible optimal solutionfor (20) is also the feasible optimal solution for (22). Remark 1:
If a linear programming problem has an optimalsolution, so does its dual, and the respective optimal costsre equal, this is known as the strong duality property [6,Theorem 4.4].This remark allows us to prove the following.
Lemma 1:
Optimization problems (20) and (21) have thesame feasible optimal solution.
Proof:
The optimization problems in (20) and (21) havethe same objective functions. Constraints in (20) are linearprogramming problems and the constraints in (21) are thedual form of each. According to the Remark 1, the optimalcost of constraints in (20) and (21) are equal and result thesame constraints with the same objective functions whichimplies (20) and (21) have the same optimal solution.
Lemma 2:
Optimization problems (21) and (22) have thesame feasible optimal solution.
Proof:
Assume we have an optimal solution for (22),then the solution is also feasible for (21) and the objectivecosts are the same. In the same way, if we have an optimalsolution for (21), so there must exist dual variables for inneroptimization problem in (21) which are also feasible for (22)and result in the same objective cost [5].From Lemma 1 and Lemma 2 we conclude that theoptimization problems (20), (21) and (22) are equivalent.IV. PROBLEM SETUP, SECOND ORDER SYSTEMIn section III we investigated the optimization problem tofind a controller for a first order system (11), in this sectionwe consider a second order system, ¨ x = u, x ∈ U x , ˙ x ∈ U a (23)where U x ⊂ R n and U a ⊂ R n and in the form of statespace, ˙ x = x ˙ x = u, x ∈ U x , x ∈ U a (24)and U x and U a are convex sets and the controller defined as, u = K vec ( Y − x , x ) , (25)where K ∈ R n × n ( m +1) . Matrix K divided in two parts K ∈ R n × nm and K ∈ R n such that, K vec ( Y − x , x ) = K ( Y − I x ) + K x , (26)if Y j ∈ R n is one of the landmarks and j ∈ t , then Y =[ Y T , . . . , Y Tt ] T , where y ∈ R ( mn ) , and I = [ I i , . . . , I m ] T ∈ R nm × n , where I i ∈ R n , i ∈ m is the identity matrix.To find the controller for the second order system (24) wedefine the same control barrier function and control Lyapunovfunction in (15) and (16), V ( x ) = n T x h i ( x ) = A i x + b i , i ∈ d, (27)however, h ( x ) and V ( x ) have the relative degree r b = 2 andneed to satisfy constraint (6) and (8). For the second order system, the min-max optimizationproblem (20) changes to, min β,k,S l ,S b β + S l + S b s.t. (cid:20) max x ( A Ti K I − c A Ti ) x + ( − A Ti K − c A Ti ) x s.t A Tx [ x , x ] T ≤ b x , i ∈ d (cid:21) ≤ S bi + c b i + A Ti K Y , (cid:20) max x ( − n T K I + c n T ) x + ( n T K + c n T ) x s.t A Tx [ x , x ] T ≤ b x , (cid:21) ≤ S l − n T K Y , (cid:20) max x A Ta K I x + ( A Ta K − c A Ta ) x s.t A Tx [ x , x ] T ≤ b x , (cid:21) ≤ c b a − A Ta K + Y ,β, S l , S b ≤ (28)We change min-max problem (28) to min-min problem byforming the dual optimization problem of inner maximizationproblems, min β,k,S l ,S b β + S l + S b s.t : β, S l , S b ≤ , min λ h b Tx λ h ≤ S bi + c b i + A Ti K Y s.t : A Tx λ h = [ A Ti K I − c A Ti , − A Ti K − c A Ti ] ,λ h ≥ , i ∈ d, min λ l b Tx λ l ≤ S l − n T K Y s.t : A Tx λ l = [ − n T K I + c n T , n T K + c n T ] ,λ l ≥ , min λ a b Tx λ a ≤ S a + c b a − A Ta K s.t : A Tx λ a = [ A Ta K I , A Ta K − c A Ta ] λ a ≥ , (29)and as discussed in section III it is equivalent to, min β,k,S l ,S b ,λ hi ,λ l ,λ a β + S l + S b s.t : b Tx λ h ≤ S bi + c b i + A Ti K Y A Tx λ h = [ A Ti K I − c A Ti , − A Ti K − c A Ti ] ,b Tx λ l ≤ S l − n T K Y A Tx λ l = [ − n T K I + c n T , n T K + c n T ] ,b Tx λ a ≤ S a + c b a − A Ta K Y s.t : A Tx λ a = [ A Ta K I , A Ta K − c A Ta ] β, S l , S b ≤ λ hi , λ l , λ a ≥ , i ∈ d, (30) a) Polygonal environment (b) Decomposed environment Fig. 2: Polygonal environment is decomposed to 8 convexsections.where the optimal feasible solution of (29) is the optimalfeasible solution for (28).
Proposition 2:
The resulting controller from (30) con-verges toward the exit face or stabilization point P exit whileavoiding the boundary of the environment. Proof:
Each convex section in the polygonal environ-ment is defined by a set of half planes which specifies thethe invariant set. The ECBF result given by [12, Theorem1] ensure that each set C i = { x ∈ R n | h i ( x ) > } is forwardinvariant, i.e., x cannot leave U x except at exit face (if present).Similarly, Proposition 1 ensures that the distance d ( x, P exit ) is always decreasing, i.e., the system always progresses towardthe exit face or stabilization point. Note that, in the caseof a stabilization point, the combination of the ECLF andECBF results imply that the stabilization point becomes anequilibrium of the system.V. NUMERICAL EXAMPLESIn this section, we apply our theory to both simple andnon-simply connected environments to find a output-feedbackcontroller, then we deform the environment and use the samecontroller to show the robustness of the controller. Giventhe environment in Fig 2, we decompose the environment to8 cells. The agent starts from x s = [10 . , . and movesto the x g = [85 , . We show the controller is robust to thesignificant deformation of the environment.
1) First Order Controller:
Given the decomposed environ-ment, we design a controller for each cell individually andmove from the start point. In this example, we assume thelandmarks for each cell are equivalent to the vertices of thecell and we define as n the exit direction of the cell. Thecontrol barrier function is defined as, h i ( x ) = A Ti x + b i ≥ , i ∈ d, (31)where h i ( x ) is the equation of the line define the cell and d isthe number barrier lines which the agent cannot pass through.Solving the optimization problem (22) finds the optimal k foreach cell which implies the optimal controller. In Fig. 3 thefirst order controller moves the agent from different startingpoints without violating any constraints.Then we deform the environment and the results are shownin Fig 4.We cannot change the location of vertices of the envi-ronment to any desired location as the Control Lyapunov Fig. 3: The first order controller moving form the startingpoints to the goal. (a) Example 1 (b) Example 2 Fig. 4: In this two examples we deform the environmentand apply the first order controller. The first order controllerof the original environment is feasible for the deformedenvironments.Fig. 5: The second order controller starts from differentstarting points.Function is independent of the location of vertices andlandmarks.
A. Second Order Controller
In this section, we design a controller for a second ordersystem on a decomposed environment Fig. 3. Same as thefirst order system, we assume the landmarks are equivalent tothe vertices of each cell and we define as n the exit directionof the cell and ECBF h ( x ) is defined by (31).Then, to see the robustness of the controller we deform theenvironment and use the same k to compute the controller.The result of the simulation is shown on Fig. 6. In Fig. 6a a) Example 1 (b) Example 2 Fig. 6: Second order controller starts from starting point andreaches the goal point. In Fig. 6a the scale of environmentremains the same but the location of vertices changes,however, in Fig. 6b, the environment is stretched, in bothcase the second order controller is feasible. (a) Non-simply connected environment(b) Changes of the states versus time
Fig. 7: Fig 7a is a non-simply connected environment and thegray polygon is an obstacle. In Fig. 7a an agent starts fromposition (10,40) and continuously moves through all sections.In Fig. 7b x and x variation versus time is shown.the scale of environment remains the same but the locationof vertices changes; however in Fig. 6b, the environment isstretched. In both cases the second order controller, which isdesigned for the environment Fig. 5 remains feasible for thedeformed environments.The controller defined by (14) is dependent on thelandmarks, so when change the location of the landmarks,although the variable k remains the same, the controllerchanges and causes the robustness of the controller forthe deformed environment. However, the Control LyapunovFunction defined by (27) is independent of the landmarks orvertices of the environment, so when we change the locationof vertices it does not affect the Control Lyapunov Functionand causes the limitation of deforming of the environment.Now, we apply our method to a non-simply connectedenvironment. First we design a first and second ordercontroller which controls an agent to move from the startpoint in Fig .7. Then, we enlarge the obstacle and apply thesame controller to the agent in Fig. 8a. Our method guaranteesthat the agent moves through the environment completelywithout violating safety and stability constraints and whenobstacle rotates π/ counterclockwise in Fig. 8b, the agentmoves through the feasible path to cover all the environment. (a) Enlarging the obstacle (b) Rotating the obstacle VI. CONCLUSIONS AND FUTURE WORKSIn this paper we proposed a novel approach to design aoutput-feedback controller with cell decomposition, throughLinear Programming. We defined a controller such that itdepends on the relative displacement measurements withrespect to the landmarks of the convex cells and formed themin-max convex problem. Then we changed the min-maxoptimization problem to min-min optimization problem byforming the dual of the inner maximization problems andwe found the controller which is robust to the significantchanges of the environment. We validate our approach ondifferent examples for the first and second order dynamiccontrol systems. In the future, we will study the ControlLyapunov function to be dependent to the the landmarks ofthe environment to find the output-feedback controller whichis robust to the noticeable changes of the exit direction ofthe cells. R
EFERENCES[1] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, andP. Tabuada. Control barrier functions: Theory and applications. arXivpreprint arXiv:1903.11199 , 2019.[2] A. D. Ames, K. Galloway, K. Sreenath, and J. W. Grizzle. Rapidlyexponentially stabilizing control lyapunov functions and hybrid zerodynamics.
IEEE Transactions on Automatic Control , 59(4):876–891,2014.[3] A. D. Ames, J. W. Grizzle, and P. Tabuada. Control barrier functionbased quadratic programs with application to adaptive cruise control.In , pages 6271–6278.IEEE, 2014.[4] C. Belta, V. Isler, and G. J. Pappas. Discrete abstractions forrobot motion planning and control in polygonal environments.
IEEETransactions on Robotics , 21(5):864–874, 2005.[5] A. Ben-Tal, L. El Ghaoui, and A. Nemirovski.
Robust optimization ,volume 28. Princeton University Press, 2009.[6] D. Bertsimas and J. N. Tsitsiklis.
Introduction to linear optimization ,volume 6. Athena Scientific Belmont, MA, 1997.[7] D. C. Conner, A. A. Rizzi, and H. Choset. Composition of localpotential functions for global robot control and navigation. In
Proceedings 2003 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS 2003)(Cat. No. 03CH37453) , volume 4,pages 3546–3551. IEEE, 2003.[8] S.-C. Hsu, X. Xu, and A. D. Ames. Control barrier function basedquadratic programs with application to bipedal robotic walking. In , pages 4542–4548. IEEE,2015.[9] O. Khatib. Real-time obstacle avoidance for manipulators and mobilerobots. In
Autonomous robot vehicles , pages 396–404. Springer, 1986.[10] J.-C. Latombe.
Robot motion planning , volume 124. Springer Science& Business Media, 2012.[11] S. M. LaValle, M. S. Branicky, and S. R. Lindemann. On therelationship between classical grid search and probabilistic roadmaps.
The International Journal of Robotics Research , 23(7-8):673–692, 2004.12] Q. Nguyen and K. Sreenath. Exponential control barrier functionsfor enforcing high relative-degree safety-critical constraints. In , pages 322–328. IEEE, 2016.[13] D. M. Tilbury and A. Chelouah.
Steering a three-input nonholonomicsystem using multi-rate controls . Electronics Research Laboratory,College of Engineering, University of , 1992.[14] X. Xu, P. Tabuada, J. W. Grizzle, and A. D. Ames. Robustness ofcontrol barrier functions for safety critical control.
IFAC-PapersOnLine ,48(27):54–61, 2015.[15] G. Yang, C. Belta, and R. Tron. Self-triggered control for safetycritical systems using control barrier functions. arXiv preprintarXiv:1903.03692arXiv preprintarXiv:1903.03692