Motion planning of robots and providing controls for them are among the popular topics that have received a lot of attention. These problems get more interesting when the robots, with detailed dynamical models, are moving in a complex environment. On one side of this concern different type of approaches have been proposed with focus on just capturing the complexity of the environment, such as potential fields [intro1], [intro2] and probabilistic roadmaps [intro3]; while on the other side some methods have just investigated the role of the robot’s dynamic model in the problem, such as input parameterizations [intro4].
There has been a noticeable amount of work recently with the idea of integrating these subjects and are focused on combining the continuous nature of robots’ dynamic models with a discretized version of the environment. In [intro8] by polygonal partition of the environment they propose an automatic control approach which is applicable over a specific region of robot’s dynamical system. In [intro9] they provide provably correct control laws for the robots, based on the discrete algorithms that are used for handling the complexity of the robot’s environment.
One of the challenging questions about providing control laws for robots is how to obtain a control, when both the safety and stability properties are important and may have conflict with each other. This question has been investigated recently in some works, such as [intro5], [intro6] and [intro7], where the problem of having conflict between safety and stability purposes is solved by mediating them through a quadratic program (QP).
Having that in mind, in this paper we propose a control algorithm in the context of optimization-based algorithms for robots in polygonal environments. In this regard we first decompose the environment into a set of convex sections and then for each section of the environment consider a set of points as landmarks. Then for satisfying the forward invariance property for each section, specified in terms of control barrier functions, and also moving in the stabilizing direction of the section, which is specified in terms of control Lyapunov functions, an optimization problem is solved to provide the control law for each section. This control law is based on combination of distance to the landmarks of each section, where the combination coefficient is obtained from the optimization problem. Among the previous works in this area, the work in [intro9] is mostly related to this paper.
The paper is structured as follows: In section II some necessary definitions are presented. Then the problem formulation for both first and second order systems, and our proposed method for solving them are given in sections III and IV. Simulation results for each of the systems are shown in section V, and then finally in section VI the concluding remarks and the future works of this research are stated.
Ii Notation and Preliminaries
In this section we review and extend definitions of Control Barrier Functions and Control Lyapunov Functions, and define our environment model.
Ii-a System dynamics
We start by considering a control-affine dynamical system111We review the concepts of CBFs and CLFs in the general nonlinear setting, although in our work we will assume linear time-invariant systems, and use only affine barrier functions.
where is the state of the agent, is the system input, is a convex set defining actuator limits, and , are locally Lipschitz continuous functions.
Ii-B Control Barrier Functions and Control Lyapunov Functions
Suppose we have a continuously differentiable function which defines a safe set such that
We say that the set is forward invariant if implies in , for all where is well defined [zcbf1].
The Lie derivative of a differentiable function with respect to a vector field
with respect to a vector fieldis defined as . The Lie derivative of order is denoted as , and is recursively defined by , with [zcbf2].
A function is said to have relative degree with respect to the dynamics (1) if for and ; equivalently, it is the minimum order of the time derivative of the system ( that explicitly depends on the inputs .
Given a sufficiently continuously differentiable function having relative degree for the dynamics (1), we define the transversal state
and the transversal dynamics
where and are defined as
and is a virtual input. These concepts are used below for defining CBFs and CLFs.
Definition 3 (Ecbf, [nguyen2016exponential]).
Consider the control system (1), and a continuously differentiable function with relative degree defining a forward invariant set as in (2). The function is an Exponential Control Barrier Function (ECBF) if there exist and control inputs such that
and such that stabilizes the transversal dynamics, i.e., the matrix has eigenvalues with negative real parts.
has eigenvalues with negative real parts.
The latter condition ensures that the set is forward invariant [nguyen2016exponential, Theorem 2]. If , then is a Zero Control Barrier Function (ZCBF,[zcbf1, zcbf2]).
Note that in this work, we will assume that is a linear function.
Next, we present an analogous definition for an extension of Control Lyapunov Functions [ames2014rapidly]
Consider the control system (1), and a continuously differentiable function , , with relative degree . The function is an Exponential Control Lyapunov Function (ECLF) if there exist constants such that,
where is a class- function, and if there exists and control inputs such that
where is a forward-invariant set, and stabilizes the transversal dynamics, i.e., the matrix has eigenvalues with negative real parts.
For , we recover the definition of Exponentially Stabilizing CLFs (ES-CLFs, [ames2014rapidly]). It is possible to use ECLF to design controllers that exponentially stabilize the original dynamics (1), as shown by the following:
The proof mirrors a simplified version of the ideas in [nguyen2016exponential]. Setting the virtual input in the transversal dynamics to
we have that with exponential convergence (since it is an LTI system and contains stabilizing feedback gains). If we pick , we obtain
in which the last element correspond to (8). Applying Gronwall’s comparison lemma, we then conclude that , which, in particular, implies . Using standard arguments from Lyapunov theory, we then have that the addition of condition (7) implies . ∎
Note that in this work, we assume is a piecewise linear function.
Ii-C Polygonal environment decomposition
We assume a polygonal environment, potentially non-simply-connected, and potentially with a large number of vertices; we decompose the polygonal environment to a finite number of convex cells.
A polygonal environment is a closed environment defined by a finite number of vertices where and is the number of vertices. We partition the environment in a set of convex sets , such that , and such that each set is a polytope defined by liner inequality constraints of the form .
Our goal is to design a different linear feedback controller for each cell . The feedback signal used by the controller will be based on linear relative measurements with respect to a set of landmarks.
A landmark is defined as a point whose location is known and fixed in the environment.
For each convex section, we have a finite number of landmarks. In this paper, we choose the landmarks as the vertices of the convex section, although this choice does not make any difference in terms of the actual method.
Ii-D High-level planning
We consider two overall objectives for the controller design:
Point stabilization: given the stabilization point in the environment and starting from any point, we aim to converge to the stabilization point (e.g. Fig. 8).
Patrolling: starting from any point, we aim to patrol the environment by converging to a path and then, traversing the same path (e.g. Fig. (a)a).
To specify the convergence objective for each controller , we first abstract the polygonal environment into a graph , where each vertex represents a cell in the partition of , and an edge if and only if cells corresponding to and have a face in common.
In the case of the point stabilization objective 1, the stabilization point is one of the vertices of the graph and if the stabilization point is in the middle of the cell, without loss of generality, we can decompose the cell into new convex cells such that the stabilization point is one of the vertices of the new cells. Then, we add one vertex to the set , which is the stabilization point and also, add edges between the new vertex and any cells which have a face in common with the cell includes the stabilization point to the set .
For each cell, we then select one exit edge (a pointer) such that, when considered together, all such edges provide a solution in the abstract graph to the high level objective. For instance, in the case of objective 1, the exit edge of each cell will point in the direction of the shortest path toward the vertex of the stabilization point. In the case of objective 2, following the exit edges will lead to a cyclic path in the graph.
To give an example, the polygonal environment in Fig. (a)a is converted to the connected graph in Fig. (c)c based on the cell decomposition of the environment in Fig. (b)b. Starting from the first node in Fig. (c)c which is shown by green point, we find the path from the start node to the equilibrium node shown by red point through the path planning algorithms (e.g. using Dijkstra’s algorithm). Regarding to that path, we define the exit face as the face of the convex section the path moves through and based on that we design the controller.
For each cell in the decomposition of the environment, we define an exit face or stabilization point to be the face or vertex corresponding to the exit edge in the abstract graph . The exit direction is an inward-facing normal of .
In this work we desire to design a controller for each convex section of the environment that drives the system in the exit direction toward the exit face, while avoiding the boundary of the environment.
Overall, thanks to the high level planning in the abstract graph , and the controller design in each cell (explained in the sections below) the system will traverse a sequence of cells to reach a given equilibrium point, or achieve a periodic steady state behavior (see examples in Section V) according to the desired objective.
Iii Problem Setup, First Order System
Assume we have an environment decomposed in a finite number of convex polytopic sections. For each section consider the control system,
where is the position of the agent in the environment and is the controller which is bounded by . We assume that these convex sets are polytopes defined by,
where , and and . Our goal is to find a feedback controller that, given the relative displacements between the robot’s position and the landmarks in the environment, provides an input that drives the system toward an exit face or vertex of while avoiding obstacles (non-exit faces of ). Note that the landmarks do not necessarily need to belong to . For this section, we choose a controller of the form:
where is a matrix where each column represents the coordinates of one of landmarks, , and is a vector of feedback gains that need to be designed. Note that gives a matrix where each column is the relative displacement between each landmark and the current position of the system; as such, we are looking for a controller that feeds back linear combinations of these displacements.
Iii-a Control Barrier Function
Let and , , represent individual columns and elements of the matrix and vector , i.e., individual faces of the cell, except the one associated to an exit face (or all of them in the case of a stabilization point). For each one of these constraints we define the following candidate ECBF:
where and is scalar, and is the number of equations needed to define the convex section and is a ZCBF.
Iii-B Control Lyapunov Function
To stabilize the system, we define the Lyapunov function as,
where is the exit direction for the cell (See Definition 7). Note that this Lyapunov function represents, up to a constant, the distance between the current system position and the exit face.
Iii-C Finding the Controller by Robust Optimization
Our goal is to find controllers (more precisely, control gains ) that maximize the motion of the robot toward the exit face, while avoiding the boundary of the environment. Using the CLF-CBF constraints reviewed in Section II, we encode our goal in the following optimization problem:
where and are positive scalars, since and all have relative degree . Note that the constraints in 17 need to be satisfied for all in the cell , i.e., the same control gains should satisfy the CLF-CBF constraints at every point in the cell. We handle this type of constraint by rewriting 17 using a min-max formulation: where (17) is equivalent to,
We relax the two constraints in (18) by introducing the slack variables and as,
In (20), we have a bi-level optimization problem with constraints that are given themselves by other optimization problems. As all constraints and objective function are linear and and are convex set, (20) and inner maximization problems are linear programming problem so we can change the min-max problem (20) to min-min problem by replacing the inner maximization problems with their dual forms. In addition, the first and third constraints in (20) can be merged to one constraint as,
where min-min problem (21) is equivalent to the minimization problem,
If a linear programming problem has an optimal solution, so does its dual, and the respective optimal costs are equal, this is known as the strong duality property [LP, Theorem 4.4].
This remark allows us to prove the following.
The optimization problems in (20) and (21) have the same objective functions. Constraints in (20) are linear programming problems and the constraints in (21) are the dual form of each. According to the Remark 1, the optimal cost of constraints in (20) and (21) are equal and result the same constraints with the same objective functions which implies (20) and (21) have the same optimal solution. ∎
Assume we have an optimal solution for (22), then the solution is also feasible for (21) and the objective costs are the same. In the same way, if we have an optimal solution for (21), so there must exist dual variables for inner optimization problem in (21) which are also feasible for (22) and result in the same objective cost[robust_opt]. ∎
Iv Problem Setup, Second Order System
where and and in the form of state space,
and and are convex sets and the controller defined as,
where . Matrix divided in two parts and such that,
if is one of the landmarks and , then , where , and , where
is the identity matrix.
For the second order system, the min-max optimization problem (20) changes to,
We change min-max problem (28) to min-min problem by forming the dual optimization problem of inner maximization problems,
and as discussed in section III it is equivalent to,
The resulting controller from (30) converges toward the exit face or stabilization point while avoiding the boundary of the environment.
Each convex section in the polygonal environment is defined by a set of half planes which specifies the the invariant set. The ECBF result given by [nguyen2016exponential, Theorem 1] ensure that each set is forward invariant, i.e., cannot leave except at exit face (if present). Similarly, Proposition 1 ensures that the distance is always decreasing, i.e., the system always progresses toward the exit face or stabilization point. Note that, in the case of a stabilization point, the combination of the ECLF and ECBF results imply that the stabilization point becomes an equilibrium of the system. ∎
V Numerical Examples
In this section, we apply our theory to both simple and non-simply connected environments to find a output-feedback controller, then we deform the environment and use the same controller to show the robustness of the controller. Given the environment in Fig 7, we decompose the environment to 8 cells. The agent starts from and moves to the . We show the controller is robust to the significant deformation of the environment.
V-1 First Order Controller
Given the decomposed environment, we design a controller for each cell individually and move from the start point. In this example, we assume the landmarks for each cell are equivalent to the vertices of the cell and we define as the exit direction of the cell. The control barrier function is defined as,
where is the equation of the line define the cell and is the number barrier lines which the agent cannot pass through. Solving the optimization problem (22) finds the optimal for each cell which implies the optimal controller. In Fig. 8 the first order controller moves the agent from different starting points without violating any constraints.
Then we deform the environment and the results are shown in Fig 11.
We cannot change the location of vertices of the environment to any desired location as the Control Lyapunov Function is independent of the location of vertices and landmarks.
V-a Second Order Controller
In this section, we design a controller for a second order system on a decomposed environment Fig. 8. Same as the first order system, we assume the landmarks are equivalent to the vertices of each cell and we define as the exit direction of the cell and ECBF is defined by (31).
Then, to see the robustness of the controller we deform the environment and use the same to compute the controller. The result of the simulation is shown on Fig. 15. In Fig. (a)a the scale of environment remains the same but the location of vertices changes; however in Fig. (b)b, the environment is stretched. In both cases the second order controller, which is designed for the environment Fig. 12 remains feasible for the deformed environments.
The controller defined by (14) is dependent on the landmarks, so when change the location of the landmarks, although the variable remains the same, the controller changes and causes the robustness of the controller for the deformed environment. However, the Control Lyapunov Function defined by (27) is independent of the landmarks or vertices of the environment, so when we change the location of vertices it does not affect the Control Lyapunov Function and causes the limitation of deforming of the environment.
Now, we apply our method to a non-simply connected environment. First we design a first and second order controller which controls an agent to move from the start point in Fig .18. Then, we enlarge the obstacle and apply the same controller to the agent in Fig. (a)a. Our method guarantees that the agent moves through the environment completely without violating safety and stability constraints and when obstacle rotates counterclockwise in Fig. (b)b, the agent moves through the feasible path to cover all the environment.
Vi Conclusions and Future Works
In this paper we proposed a novel approach to design a output-feedback controller with cell decomposition, through Linear Programming. We defined a controller such that it depends on the relative displacement measurements with respect to the landmarks of the convex cells and formed the min-max convex problem. Then we changed the min-max optimization problem to min-min optimization problem by forming the dual of the inner maximization problems and we found the controller which is robust to the significant changes of the environment. We validate our approach on different examples for the first and second order dynamic control systems. In the future, we will study the Control Lyapunov function to be dependent to the the landmarks of the environment to find the output-feedback controller which is robust to the noticeable changes of the exit direction of the cells.