arXiv:1708.04028v1 [cs.RO] 14 Aug 2017  Counterexample Guided Inductive Optimization Applied to Mobile Robots Path Planning (Extended Version)  Rodrigo F. Ara ́ ujo ∗ , Alexandre Ribeiro † , Iury V. Bessa † , Lucas C. Cordeiro ‡† , Jo ̃ ao E. C. Filho †  ∗ Federal Institute of Amazonas, Brazil  † Federal University of Amazonas, Brazil  ‡ University of Oxford, UK  Abstract —We describe and evaluate a novel optimization-based off-line path planning algorithm for mobile robots based on the Counterexample-Guided Inductive Optimization (CEGIO) technique. CEGIO iteratively employs counterexamples gener- ated from Boolean Satisfiability (SAT) and Satisfiability Modulo Theories   (SMT)   solvers,   in   order   to   guide   the   optimization process and to ensure global optimization. This paper marks the first application of those solvers for planning mobile robot path. In particular, CEGIO has been successfully applied to obtain optimal two-dimensional paths for autonomous mobile robots using off-the-shelf SAT and SMT solvers.  Index Terms —optimization, satisfiability modulo theory, path planning, mobile robots.  I. I NTRODUCTION  Recently, mobile robots have been employed for various tasks, replacing humans in dangerous and monotonous tasks with a certain degree of efficiency and safety [1]–[3]. For this specific reason, robot mobile navigation, and in particular, path planning has become an important research topic in recent days. The basic path planning problem can be described as the computation of robot positions and motions, which allow the robot to autonomously move from one starting point to a final desired position, performing a specific task and avoiding possible obstacles [4]. There are several methods in the literature that are usually employed for   path   planning [5]–[11].   For   instance,   Yang  et al.   [12] classify the path planning method in five main categories: sampling-based, node-based optimal, mathematical model-based, bioinspired, and multifusion-based algorithms. Additionally, path planning strategies can be classified   in two categories: on-line and off-line. In on-line mode, the path planning can be performed during the robot movement, while in off-line mode, the robot movement path planning is performed in advance ( i.e. , before the movement). Path planning task is often modeled as an optimization problem, where a decision variable represents a given path,  i.e. , the sequence of points (or movements) by which the robot must move; the cost function is a certain criteria or metric whose value is optimized ( e.g. , distance, energy con- sumption, and execution time). Thus, various optimization techniques have been applied to solve path planning problems,  e.g. , genetic algorithm (GA) [5]–[7], A* [8], particle swarm optimization (PSO) [9], nonlinear programming (NLP) [10], and ant colony [11]. However, these optimization techniques are unable to ensure the global optimality of the robot path, although they are able to provide results sufficiently fast for on-line path planning applications. We describe and evaluate a novel off-line path planning algorithm based on the counterexample guided inductive opti- mization (CEGIO) technique described by Araujo   et al.   [13], [14], which is a Boolean Satisfiability (SAT) and Satisfia- bility Modulo Theories (SMT) based optimization algorithm that executes iteratively to achieve global optimization via counterexamples produced by SAT and SMT solvers. Previous studies [13], [14], showed that CEGIO is able to ensure the global optimization of various non-trivial functions classes ( e.g. , convex, nonlinear, and nonlinear functions), with an accuracy rate better than traditional optimization techniques ( e.g. , GA,   PSO,   and   NLP).   Therefore, the   main   original contributions of this paper are:  •   Apply   the   CEGIO   algorithm   to   obtain   optimal   two- dimensional paths for autonomous mobile robots.  •   Evaluate the effect of the minimum improvement (step) on the cost function at each iteration of the CEGIO algorithm.  Outline : Section II provides a brief background about opti- mization problems, path planning for mobile robots, software model checking, and CEGIO. Section III describes the pro- posed methodology steps for efficiently solving optimization problems using the CEGIO approach. Section IV presents the objectives and experimental results of the proposed algorithm if applied to the path planning problem in bi-dimensional space for mobile robots. Finally, Section V presents the conclusions of this work and outlines future studies. II. B ACKGROUND  A. Optimization Problems  Let   f   :   X   →   R   be a cost function, such that   X   ∈   R n   represents the decision variables vector   x 1 ,   x 2 , ...,   x n   and   f   ( x 1 ,   x 2 , ...,   x n )   ≡  f   ( x ) . Let   Ω   ⊂   X   be a subset settled by a set of constraints.  Definition 1.   A multi-variable optimization problem consists in finding an optimal vector x, which minimizes f in   Ω .
According to Definition 1, an optimization problem can be written as   min  x   f   ( x ) ,  s.t.   x   ∈   Ω .   (1) In particular, this optimization problem can be classified in different ways with respect to constraints, decision variables domain, and nature of cost function   f   . All optimization prob- lems considered here are constrained,   i.e. , decision variables are constrained by the subset   Ω . The optimization problem domain   X   that contains   Ω   can be the set of   N ,   Z ,   Q , or   R . Depending on the domain and constraints, the optimization search-space can be small or large, which influence the opti- mization algorithm performance. The cost function can be classified as linear or non-linear; continuous, discontinuous or discrete; convex or non-convex. Depending on the cost function nature, the optimization prob- lem can be difficult to solve, given the time and memory constraints [15], even if we use SAT and SMT solvers [16]. Particularly, non-convex optimization problems are the most difficult ones with respect to the cost function nature. A non-convex cost function is a function whose epigraph is a non-convex set and consequently presents various inflexion points that can trap the optimization algorithm to a sub- optimal solution. A non-convex problem is necessarily a non- linear problem and it can also be discontinuous. Depending on that classification, some optimization techniques are unable to solve the optimization problem, and some algorithms usually point to suboptimal solutions,   i.e. , a solution that is not a global minimum of   f   , but it only locally minimizes   f   . Global optimal solutions of the function   f   , aforementioned, can be defined as:  Definition 2.   A vector x ∗   ∈   Ω   ia a global optimal solution of f in   Ω   iff f   ( x ∗ )   ≤   f   ( x ) ,   ∀ x   ∈   Ω . B. Path Planning for Mobile Robots  Path planning is one of the main robot navigation steps in which the robot must determine a safe and a collision free path from one starting point to a target point. Such path is a curve without time continuous consideration and it is composed of various segments, usually called by trajectories [12].  Definition 3.   A path is a set of straight segments successively connected to guide the mobile robot from one initial point to a target point.  A path planning algorithm can be evaluated, given some properties,   e.g. , completeness, optimally, correctness, robust- ness, and computational complexity [4]. In particular, path planning algorithms often seek to obtain not only a correct path,   i.e. , a safe path that meets the path specification ( e.g. , obstacle avoidance), but also an optimal path [12].  Definition 4.   An optimal path is a set of straight segments successively connected to guide the mobile robot from one initial point to a target point, which minimizes a cost function related to that path.  Thus, the main path planning algorithm objective is to find the   n   points, which form the path that connects the initial and target points of the mission, generating smallest displacement cost,   i.e. , the path planning is essentially a trajectory optimiza- tion problem. In the present work, this problem is restricted to the bi-dimensional environment.  C. Model Checking  Model checking is an automated verification procedure to exhaustively check all (reachable) systems states [17]. The model checking procedure typically consists of three steps: modeling, specification, and verification. Modeling is the first step, where it converts the system to a formalism that is accepted by a verifier. The second step is the specification, which describes the systems behavior and the property to be checked. Model checking provides ways to check whether a given specification satisfies a system property, but it is difficult to determine whether such specification covers all properties in which the system should satisfy. Finally, the verification step checks whether a given property is satisfied with respect to a given model,   i.e. , all relevant system states are checked to search for any state that violates the verified property. In case of a property violation, the verifier reports the system execution trace (counterexample), which contains all steps from the initial state to the bad state that leads to the property violation.  1) Bounded Model Checking (BMC):   BMC is an impor- tant verification technique, which is based on SAT [18] or SMT [19] solvers. BMC has been successfully applied to ver- ify single- and multi-threaded programs [20]–[23]. It checks the negation of a given property at a given depth over a transition system   M .  Definition 5.   Given a transition system M, a property  φ   , and a bound k; BMC unrolls the system k times and translates it into a verification condition (VC)  ψ , which is satisfiable iff  φ  has a counterexample of depth less than or equal to k [18].  In BMC, the associated problem is formulated by construct- ing the following logical formula  ψ k   =   I ( S 0 )   ∧  k ∨  i = 0  i − 1 ∧  j = 0  (  γ ( s   j   ,   s   j + 1 )   ∧ ¬  φ   ( s i ) )   ,   (2) where  φ   is a property and   S 0   is a set of initial states of   M , and  γ ( s   j   ,   s   j + 1 )   is the transition relation of   M   between time steps   j   and   j   +   1. Hence,   I ( S 0 )   ∧   ∧ i − 1  j = 0  γ ( s   j ,   s   j + 1 )   represents the executions of a transition system   M   of length   i . The above VC  ψ k   can be satisfied if and only if, for some   i   ≤   k  there exists a reachable state at time step   i   in which  φ   is violated. If the logical formula (2) is satisfiable ( i.e. , returns true), then the SMT solver provides a satisfying assignment (counterexample).  Definition 6.   A counterexample for a property  φ   is a sequence of states s 0 ,   s 1 , ...,   s k   with s 0   ∈   S 0 , s k   ∈   S k , and  γ ( s i ,   s i + 1 )   for  0   ≤   i   ≤   k that makes Eq.   (2)   satisfiable. If it is unsatisfiable
(i.e., returns false), then we can conclude that there is no error state in k steps or less. D. Counterexample Guided Inductive Optimization  This section presents a novel class of search-based opti- mization algorithm that employs non-deterministic represen- tation of decision variables and constrains the state-space search based on counterexamples produced by a SAT or SMT solver, in order to ensure the complete global optimization without employing randomness. This class of techniques is defined here as counterexample guided inductive optimization (CEGIO), which is inspired by the syntax-guided synthesis (SyGuS) to perform inductive generalization based on coun- terexamples provided by a verification oracle [24]. In particular, CEGIO relies on iterative executions to con- strain a verification procedure, in order to perform inductive generalization, based on counterexamples extracted from SAT and SMT solvers. CEGIO is able to successfully optimize a wide range of functions, including non-linear and non-convex optimization problems based on SAT and SMT solvers, in which data provided by counterexamples are employed to guide the verification engine, thus reducing the optimization domain [13]. The function evaluation and the search for the op- timal solution are performed by means of an iterative execution of successive verifications based on counterexamples extracted from SAT and SMT solvers. The counterexample provides new domain boundaries and new optimal candidates. In contrast to other heuristic methods ( e.g. , genetic algorithms), which are usually employed for optimizing this class of function, the present approach always finds the global optimal point. III. CEGIO- BASED   P ATH   P LANNING  In this section, a novel optimization method to solve the path planning problem of autonomous mobile robots is described. The main objective of a path planning algorithm is to generate points needed to guide the mobile robot in a environment with obstacles. As observed by other researchers, it is a challenge to find the optimal path in a region, since the number of activ- ities and obstacles types in the environment can substantially increase the path planning problem complexity [4]. In most cases, it requires substantial processing time, especially if there are many points to visit. Therefore, there is a growing need for developing techniques for optimal path planning, considering trajectory length and system energy consumption. For the path planning method proposed here, the following two steps are applied: (1) encode the environment, movement space, and static obstacles ( i.e. , the environment is assumed to be known and contains only static obstacles); (2) use a path search method that consists of points in the space and its respective orientations, to find a path that satisfies the constraints given by the problem. Ara ́ ujo   et al.   [14] proposed three different algorithm types based   on   CEGIO,   which   are   suitable   for   different   situa- tions: the Generalized Algorithm (CEGIO-G), the Simplified Algorithm (CEGIO-S), and the Fast Algorithm (CEGIO-F). CEGIO-G can to be applied to any function class ( i.e. , convex and non-convex ones). CEGIO-S is suitable for functions about which we have some prior knowledge ( e.g. , semi- and positive- definite functions). Finally, CEGIO-F can be applied to convex functions and uses its properties to restrict the associated state- space, according to the results presented by Ara ́ ujo   et al.   [14], which show considerable improvement regarding optimization times. The proposed method consists of modeling the path planning as an optimization problem and solving it by a CEGIO-F based algorithm.  A. Optimization Problem Formulation  For a complete path planning problem formulation as an op- timization problem, the cost function and problem constraints must be defined.  1) Cost Function:   Given the starting point ( S ) and the target point ( T ) defined as   S   =   P 1   and   T   =   P n , the objective is to find a decision variables matrix,   L   = [ P 1 ,   P 2 , ...,   P n − 1 ,   P n ] , such that,  J ( L )   is the length function. The cost function is defined by Eq. (3) as  J ( L ) =  n − 1  ∑  i = 1  ‖ P i + 1   −   P i ‖ 2   ,   (3) where   n   is the number of points that compose the path and for the bi-dimensional case,   P i   = ( x i ,   y i )   is a path vertex. We can see that the bi-dimensional path planning opti- mization problem is an optimization problem in 2 n   −   4-th dimension. Note that, if   n   →   ∞ , the path will be a smooth trajectory; furthermore, the optimization problem dimension will also tend to infinity. However, the trajectory smoothness should be provided by a trajectory planning algorithm, which uses the results of the path planning algorithm (the scope of this paper).  2) Constraints:   According to   Definition   3,   the   path   is formed by   n   −   1 straight segments, which connect the   n   points such that the   i -th straight segment is built from   P i   to   P i + 1 . Thus, constraints are about each point   p i  λ   that composes the   i -th straight segments, in such way that the   i -th must not intercept any obstacle. From Definition 1 and Eq. (3), the path planning optimization problem can be written as min  L   J ( L ) ,  p i  λ   ( L )   / ∈   O  s.t.   p i  λ   ( L )   ∈   E  i   =   1 , ...,   n   −   1 ,  (4) where   O   is the set of points defined by obstacles;   E   is the set of points defined by environment limits;   n   is the number of points that compose the path; and   p i  λ   ( L )   is all points belonging to the   i -th straight segment of the path defined by vector   L , each  p i  λ   ( L )   point is defined by Eq. (5) as  p i  λ   ( L ) = ( 1   −  λ   ) P i   +  λ   P i + 1 ,   ∀  λ   ∈   [ 0 ,   1 ] .   (5)  3) Environmental   Modeling:   Although   the   optimization problem is defined by Eq. (4), it is still necessary to model the environment of movement ( E ) and obstacles ( O ) in it. For simplicity, the movement environment is modeled as a
rectangle, which is defined by lower and upper limits and constrains the value that each coordinate can assume. Obstacles can be modeled by means of circles, such that the geometric center of the real obstacle shape defines the center of the circle; the circle radius is such that every point of the real obstacle shape is inside the circle. The constraints of the optimization problem (4) ensure that there is no intersection between the path segments and the obstacle. This constrained is described by Eq. (6).  ( x i  λ   −   x 0 ) 2   + ( y i  λ   −   y 0 ) 2   ≥   ( r   +  σ   ) 2   (6) where   p i  λ   = ( x i  λ   ,   y i  λ   ) , and  σ   is a safety margin.  B. Path Planning Algorithm  There are two code directive in C/C++ programming lan- guage, which can be used to model and control the verification process: ASSUME and ASSERT. The ASSUME directive is able to define constraints over (non-deterministic) variables, and the ASSERT directive is used to check system correctness with respect to a given property. Using these two directives, any off-the-shelf C/C++ model checker could be applied to check specific constraints in optimization problems. The verification process consists of three steps: modeling, especification, and verification [13]. Thus, the optimization problem described in section III-A is encoded as shown in Figure 1. This C code checks whether the literal   J optimal   given by Eq. 7 is satisfied for value   J c   that is a candidate to optimal, such that   J c   is randomly initialized with high values.  J optimal   ⇐⇒   J ( L )   >   J c   (7) A function   rest_points   inserts the constraints on   n   −   1 path straight segments, in such way that they satisfy the conditions discussed in the previous section; for this purpose, the ASSUME directive is used. Note that   rest_points  must be executed for each obstacle. Figure 2 illustrates the  rest_points   ANSI-C code. If   the   code   shown   in   Figure   1   returns   false,   i.e. ,   the negation of   J optimal   is satisfiable, then there is a   L ( i )   for which  J ( L ( i ) )   <   J c . Thus, the optimal candidate,   J c , can be updated with the returned value,   J c   =   J ( L ( i ) ) , for the new code instance execution; otherwise,   J optimal   is unsatisfiable,   i.e. ,   J ( L ( i − 1 ) )   is the optimal value for a given precision   p . Furthermore, the counterexample returns the matrix   L   that defines the optimal path with   n   points between start and target points. The number of points is automatically increased if it is not possible to find an optimal path,   i.e. ,   J optimal   is unsatisfiable. If  J optimal   is repeatedly unsatisfiable, then the precision must be improved. The precision   p   defines the path points coordinate precision, such that:  k   >   logp ,   (8) where   k   is the number of decimal places of the points coor- dinate values. The precision   p   is initialized by one,   i.e. ,   k   =   0 and coordinates are considered to be integers. The precision is increased by multiplying   p   by 10,   i.e. , by adding one decimal place in the coordinate values.  # d e f i n e   DIM   2   / /   s p a c e   d i m e n s i o n  # d e f i n e   n   1   / /   number   o f   p o i n t s   t h a t   compose   t h e   p a t h  # d e f i n e   p   1   / /   p r e c i s i o n   o f   p o i n t s   l o c a l i z a t i o n  # d e f i n e   J   c   25   / /   c a n d i d a t e   v a l u e   o f   c o s t   f u n c t i o n  # d e f i n e   no   1   / /   number   o f   o b s t a c l e s  / /   o b s t a c l e s   i n f o r m a t i o n  f l o a t   x0 [ no ]   =   { 5 } ;   / /   c o o r d i n a t e s   o f   c e n t e r   ’ x ’  f l o a t   y0 [ no ]   =   { 5 } ;   / /   c o o r d i n a t e s   o f   c e n t e r   ’ y ’  f l o a t   r [ no ]   =   {   2 . 5   }   ;   / /   o b s t a c l e s   r a d i u s  i n t   main ( )   {  i n t   i ,   j ;  i n t   A   [DIM ]   =   { 1 ∗ p ,   1 ∗ p   }   ;   / /   s t a r t   p o i n t  i n t   B   [DIM ]   =   { 9 ∗ p ,   9 ∗ p   }   ;   / /   t a r g e t   p o i n t  / /   e n v i r o n m e n t a l   l i m i t s  i n t   l i m   [DIM ] [ 2 ]   =   {   0 ∗ p ,   10 ∗ p ,   0 ∗ p ,   10 ∗   p   }   ;  / /   s t a t e s   d e c l a r a t i o n ,   x=x [ i ] [ 0 ]   and   y=x [ i ] [ 1 ]  / /   a s   non −   d e t e r m i n i s t i c  f o r   ( i = 0 ;   i < n ;   i ++)  f o r   ( j = 0 ;   j < DIM ;   j ++)  x [ i ] [ j ]   =   n o n d e t   i n t ( ) ;  / /   c o n s t r a i n t s   on   e n v i r o n m e n t   l i m i t s   and   o b s t a c l e s  f o r   ( i = 0 ;   i < n ;   i ++)   {  ESBMC   assume (   x [ i ] [ 0 ]   > =   l i m [ 0 ] [ 0 ]   ) ;  ESBMC   assume (   x [ i ] [ 0 ]   < =   l i m [ 0 ] [ 1 ]   ) ;  ESBMC   assume (   x [ i ] [ 1 ]   > =   l i m [ 1 ] [ 0 ]   ) ;  ESBMC   assume (   x [ i ] [ 1 ]   < =   l i m [ 1 ] [ 1 ]   ) ;  }  f o r   ( j = 0 ;   j < no ;   j ++)  r e s t   p o i n t s   (A,   0 ,   x0 [ j ] ∗   p ,   y0 [ j ] ∗   p ,   r [ j ] ∗   p ) ;  f o r   ( i = 1 ;   i < n ;   i ++)   {  f o r   ( j = 0 ;   j < no ;   j ++)  r e s t   p o i n t s ( x [ i   − 1] , i , x0 [ j ] ∗   p , y0 [ j ] ∗   p , r [ j ] ∗   p ) ;  }  f o r   ( j = 0 ;   j < no ;   j ++)  r e s t   p o i n t s   ( B ,   n   − 1 , x0 [ j ] ∗   p , y0 [ j ] ∗   p , r [ j ] ∗   p ) ;  / /   c o m p u t e   t h e   c o s t   f u n c t i o n  f l o a t   Aux1 [DIM ] ,   Aux2 [DIM ] ;  f l o a t   J   =   0 . 0 ;  f o r   ( j = 0 ;   j < DIM ;   j ++)  Aux1 [ j ]   = A[ j ] / p ;  f o r   ( i = 0 ;   i < n ;   i ++)   {  f o r   ( j = 0 ;   j < DIM ;   j ++)  Aux2 [ j ]   =   (   f l o a t   )   x [ i ] [ j ] / p ;  J   =   J   +   d i s t ( Aux1 , Aux2 ) ;  f o r   ( j = 0 ;   j < DIM ;   j ++)  Aux1 [ j ]   =   Aux2 [ j ] ;  }  f o r   ( j = 0 ;   j < DIM ;   j ++)  Aux2 [ j ]   = B[ j ] / p ;  J   =   J   +   d i s t ( Aux1 , Aux2 ) ;  ESBMC   assume (   J   <   J   c   ) ;  / /   t e s t   t h e   l i t e r a l   J   o p t i m a l ,   Eq .   ( 7 )  a s s e r t   (   J   >   J   c   ) ;  r e t u r n   0 ;  }  Fig. 1: C code for bi-dimensional path planning. The optimization problem (4) is solved by executing the code in Figure 1 iteratively, according to precision values and number of points that compose the path. Algorithm 1 summa- rizes the aforementioned steps of the proposed bi-dimensional path planning methodology. The algorithm is inspired by CEGIO-F algorithm presented by Ara ́ ujo   et al.   [14], since the cost function is convex. The algorithm efficiency depends on the number of points in the path   n . Naturally, a large value of   n   will generate smooth paths, but it will increase the complexity of the optimization problem, leading to a large execution time. The smoothness is not required from the path planning algorithm, but it is provided by a trajectory planning algorithm that computes the curves between the points of the path,   i.e. , a trajectory planning
v o i d   r e s t   p o i n t s (   i n t   P1   [DIM ] ,   i n t   i ,   f l o a t   x0 ,   f l o a t   y0 ,  f l o a t   r )   {  f l o a t   s i g ma   =   0 . 5 ;   / /   s a f e t y   m a r g i n  / /   c o n s t r a i n t   g i v e n   by   Eq .   ( 6 )  ESBMC   assume (   ( x [ i ][0 ]   −   x0 )   ∗   ( x [ i ][0 ]   −   x0 )   +  ( x [ i ][1 ]   −   y0 )   ∗   ( x [ i ][1 ]   −   y0 )   >   ( r + s i g ma )   ∗   ( r + s i g ma )   ) ;  f l o a t   a ,   b ,   c ;  i f   ( P1 [0]   −   x [ i ] [ 0 ] = = 0 )   {  a   =   1 ;  b   =   0 ;  c   =   − P1 [ 0 ] ;  }  e l s e   {  a   =   (   f l o a t   )   ( P1 [1]   −   x [ i ] [ 1 ] ) / ( P1 [0]   −   x [ i ] [ 0 ] ) ;  b   =   − 1;  c   =   (   f l o a t   )   − a ∗ P1 [ 0 ] + P1 [ 1 ] ;  }  f l o a t   Py   =   ( a ∗ a ∗ y0 − a ∗ b ∗ x0 − b ∗ c ) / ( a ∗ a+b ∗ b ) ;  i f   ( ( ( Py − x [ i ] [ 1 ] ) / ( P1 [1]   −   x [ i ] [ 1 ] )   >   = 0 ) &&  ( ( Py − x [ i ] [ 1 ] ) / ( P1 [1]   −   x [ i ] [ 1 ] )   <   = 1 ) ) )   {  f l o a t   d = (   f l o a t   )   a b s 2 ( a ∗ x0+b ∗ y0+ c ) / s q r t 2 ( a ∗ a +b ∗ b ) ;  ESBMC   assume (   d   >   r   ) ;  }  }  Fig. 2: C code for function   rest_points .  Algorithm 1:   Path planning algorithm based on satisfia- bility.  input :   Cost function   J ( L ) , is a set of obstacles constraints   O   and a set of environment constraints   E , which define   Ω   and a desired precision  η  output:   The optimal path   L ∗   and the optimal cost function value   J ( L ∗   )  1   Initialize J ( L ( 0 ) )   randomly ;  2   Initialize precision variable with p   =   1 , k   =   0   e i   =   1;  3   Initialize number of points, n   =   1;  4   Declare decision variables vector   L i   as non-deterministic integer variables ;  5   while   k   ≤  η   do  6   Define upper and lower limits of   L   with directive   ASSUME , such as L   ∈   Ω k   ;  7   Describe the objective function model J ( L ) ;  8   do  9   do  10   Define the constraint J ( L ( i ) )   <   J ( L ( i − 1 ) )   with directive   ASSUME ;  11   Verify the satisfiability of J optimal   given by Eq.   (7);  12   Update   L ∗   =   L ( i )   e J ( L ∗   ) =   J ( L ( i ) )   based on the counterexample ;  13   Do i   =   i   +   1;  14   while   ¬ J optimal   is satisfiable ;  15   if   ¬ J optimal   is not consecutively satisfiable   then  16   break  17   end  18   else  19   Update the number of points, n ;  20   end  21   while   TRUE ;  22   Do k   =   k   +   1;  23   Update the set   Ω k   ;  24   Update the precision variable, p ;  25   end  26   L ∗   =   L ( i )   e J ( L ∗   ) =   J ( L ( i ) ) ;  27   return   L ∗   e J ( L ∗   ) ;  algorithm is responsible for interpolating the points found by the path planning. IV. E XPERIMENTAL   E VALUATION  A. Experimental Objectives and Description  The path planning algorithm described in section III is suitable for a general autonomous vehicle, and the goal is to find points which compose the path. Two experiments were designed and executed to evaluate the application of the CEGIO-based path planning algorithm for an autonomous vehicle. The first experiment is performed with Setting 1, in which the autonomous vehicle is inserted in a bi-dimensional space with the goal of avoiding a single obstacle, as shown in Figure 3(a). The second experiment, with Setting 2, is similar to the first one, except that now there are two obstacles, as illustrated in Figure 3(b).  0   2   4   6   8   10  x (m)  0 2 4 6 8 10  y (m)   O S T Obstacle  (a) Setting 1  0   2   4   6   8   10  x (m)  0 2 4 6 8 10  y (m)  O 1  O 2  S T Obstacle  (b) Setting 2  Fig. 3: Settings for evaluating the proposed Algorithm 1. The objective in both experiments is to compute a path from  S   to   T . The precision on the points location that form the path is 10 cm . In Setting 1, the obstacle is centered in   O ( x 0 ,   y 0 ) = ( 5 ,   5 )   and its radius is   r   =   2 . 5. In Setting 2, the obstacles are centered in   O 1 ( x 0 ,   y 0 ) = ( 2 ,   4 )   and   O 2 ( x 0 ,   y 0 ) = ( 7 ,   8 ) , and their radius are   r 1   =   1   r 2   =   1 . 5, respectively. The safety margin for both settings is  σ   =   0 . 5. All previous distances are measured in meters. The maximum algorithm execution time for both employed verifiers CBMC 1   and ESBMC 2   is set to three days for Setting 1 and one week for Setting 2.  B. Experimental Setup  All experiments were conducted on an otherwise idle Intel Core i7   −   4790 3 . 60 GHz processor, with 16 GB of RAM, running Ubuntu 14.10 64-bits. Additionally, the time presented here is related to the average of 10 executions for each benchmark; the measuring unit is always in seconds based on the CPU time; we did not restrict the memory consumption for the experiments. The employed software verifiers versions are: CBMC v4.5 with support to the MiniSAT v2 . 2 . 0 solver and ESBMC v3 . 1 . 0 with support to the MathSAT v5 . 3 . 13 solver.  C. Experimental Results  The paths obtained in Algorithm 1 for both settings and also using the SAT and SMT solvers are illustrated in Figure 4. For Setting 1, a path with only five points was obtained ( n   =   5), and for Setting 2, the obtained path has six points ( n   =   6); both scenarios suffered the timeout of three days and one week, respectively. Figure 5 shows the cost function convergence trajectories to   obtain   those   paths   shown   in   Figure   4.   Note   that   the cost function value always decreases at each iteration and converges to the optimal solution. However, it does not reach  1 http://www.cprover.org/cbmc/  2 http://esbmc.org/
0   2   4   6   8   10  x (m)  0 2 4 6 8 10  y (m)  S O T SMT  SAT  (a) Path generated for Setting 1  0   2   4   6   8   10  x (m)  0 2 4 6 8 10  y (m)  O 1  O 2  S T SMT  SAT  (b) Path generated for Setting 2  Fig. 4: Paths obtained by Algorithm 1. Step of the cost function is 10 − 4 . the optimal value due to the timeout previously defined. SAT and SMT solvers obtained very similar solutions with the same number of points and even timeout; however, the SAT solver converges more quickly to the optimal solution if the number of points increases, as shown in Figure 5(b).  0   2   4   6   8   10   12  Iterations  12 14 16 18 20 22 24 26  J (m)  SMT  SAT  (a) Trajectory for Setting 1  0   5   10   15   20  Iterations  12 14 16 18 20 22 24 26  J (m)  SMT  SAT  (b) Trajectory for Setting 2  Fig. 5: Paths obtained by Algorithm 1. Step of the cost function is 10 − 4 . The timeout for both settings occurred due to the cost function step being 10 − 4 , which is the precision of values returned from the counterexamples. Thus, the proposed al- gorithm requires more iterations to converge to the optimal solution and as much close to that, each iteration does not significantly improve the value of the cost function, which substantially increases the convergence time. A solution to this problem is to increase this step, whereby the value of the cost function decreases. Thus, in order to evaluate the step influence in the execution time, the step was fixed to 10 − 2 ,   i.e. ,   J c   =   J ( L ( i ) )   −   10 − 2 , a hundred times larger than the previous step, and the Setting 1 experiment is repeated. However, the found solution will not be the best, but it will be at a distance of 10 − 2   from it and for most problems, it is still a satisfactory solution. Figure 6 shows a comparison between the obtained paths and the convergence trajectory of cost function for Setting 1, considering the steps previously mentioned, 10 − 4 , and the step fixed in 10 − 2 . Only the SAT solver was used to perform this specific experiment.  0   2   4   6   8   10  x (m)  0 2 4 6 8 10  y (m)  S O T 10 -2  10 -4  (a) Paths for Setting 1  0   5   10   15   20  Iterations  12 14 16 18 20 22 24 26  J (m)  10 -2  10 -4  (b) Trajectories for Setting 1  Fig. 6: Comparison of cost function paths and trajectories for steps 10 − 2   and 10 − 4 . The obtained path, considering the new step, has more points,   n   =   6; additionally, it was found in a much shorter time than the previous configuration,   i.e. , only four hours, which represents 5 . 5% of the previous time, 72 hours, although it took more iterations. This step can be further increased such that satisfactory solutions can be obtained in a shorter time. However, there is no guarantee that the optimal solution is found, as previously mentioned, it is only guaranteed that the solution found is at a distance relative to the chosen step of the optimal solution. V. C ONCLUSION  We have presented a novel path planning algorithm for mo- bile robots, where the optimal path planning problem is solved by the application of the CEGIO-F algorithm. The experimen- tal results indicate that CEGIO is able to provide optimal paths for mobile robots. However, the cumulative execution time is still high, if compared to traditional optimization-based path planning algorithms. Future studies consist in applying the CEGIO-based algorithm to tridimensional environments and in the context of the trajectory planning in order to obtain a smooth trajectory. R EFERENCES [1]   S. Karma, E. Zorba, G. Pallis, G. Statheropoulos, I. Balta, K. Mikedi, J.   Vamvakari,   A.   Pappa,   M.   Chalaris,   G.   Xanthopoulos,   and M. Statheropoulos, “Use of unmanned vehicles in search and rescue operations in forest fires: Advantages and limitations observed in a field trial,”   International Journal of Disaster Risk Reduction , vol. 13, pp. 307 – 312, 2015. [2]   G. Varela, P. Caamao, F. Orjales, lvaro Deibe, F. Lpez-Pea, and R. J. Duro, “Autonomous UAV based search operations using constrained sampling evolutionary algorithms,”   Neurocomputing , vol. 132, pp. 54 – 67, 2014. [3]   Y. Naidoo, R. Stopforth, and G. Bright, “Development of an UAV for search & rescue applications,” in   AFRICON , Sept 2011, pp. 1–6. [4]   J. J.   M.   Lunenburg,   S.   A. M.   Coenen,   G. J.   L. Naus,   M.   J. G. van de Molengraft, and M. Steinbuch, “Motion planning for mobile robots: A method for the selection of a combination of motion-planning algorithms,”   IEEE Robotics Automation Magazine , vol. 23, no. 4, pp. 107–117, Dec 2016. [5]   A.   Bakdi,   A.   Hentout,   H.   Boutami,   A.   Maoudj,   O.   Hachour,   and B. Bouzouia, “Optimal path planning and execution for mobile robots using genetic algorithm and adaptive fuzzy-logic control,”   Robotics and Autonomous Systems , vol. 89, pp. 95 – 109, 2017.
[6]   A. V. Topalov, D. D. Tsankova, M. G. Petrov, and T. P. Proychev, “Intelligent motion planning and control for a simulated mobile robot,”  { IFAC }   Proceedings Volumes , vol. 30, no. 21, pp. 351 – 356, 1997, 2nd  { IFAC }   Workshop on New Trends in Design of Control Systems 1997, Smolenice, Slovak Republic, 7-10 September. [7]   J. Da Silva Arantes, M. Da Silva Arantes, C. Toledo, O. Jnior, and B. Williams, “Heuristic and genetic algorithm approaches for UAV path planning under critical situation,”   International Journal on Artificial Intelligence Tools , vol. 26, no. 1, 2017. [8]   “Flight path planning for unmanned aerial vehicles with landmark-based visual navigation,”   Robotics and Autonomous Systems , vol. 62, no. 2, pp. 142 – 150, 2014. [9]   J. Han and Y. Seo, “Mobile robot path planning with surrounding point set and path improvement,”   Applied Soft Computing Journal , vol. 57, pp. 35–47, 2017. [10]   A. M. Lekkas, A. L. Roald, and M. Breivik, “Online path planning for surface vehicles exposed to unknown ocean currents using pseu- dospectral optimal control,”   IFAC-PapersOnLine , vol. 49, no. 23, pp. 1 – 7, 2016, 10th IFAC Conference on Control Applications in Marine SystemsCAMS 2016. [11]   M.-R. Zeng, L. Xi, and A.-M. Xiao, “The free step length ant colony algorithm in mobile robot path planning,”   Advanced Robotics , vol. 30, no. 23, pp. 1509–1514, 2016. [12]   L. Yang, J. Qi, D. Song, J. Xiao, J. Han, and Y. Xia, “Survey of robot 3D path planning algorithms,”   Journal of Control Science and Engineering , vol. 2016, 2016. [13]   R. Arajo, I. Bessa, L. C. Cordeiro, and J. E. C. Filho, “SMT-based verification applied to non-convex optimization problems,” in   2016 VI Brazilian Symposium on Computing Systems Engineering (SBESC) , Nov 2016, pp. 1–8. [14]   R. Ara ́ ujo, I. Bessa, L. Cordeiro, and J. E. C. Filho, “Counterexample guided   inductive   optimization,”   in   arXiv:1704.03738   [cs.AI] ,   April 2017, pp. 1–32. [Online]. Available: http://arxiv.org/abs/1704.03738 [15]   E. A. Galperin,   “Problem-method classification   in optimization   and control,”   Computers and Mathematics with Applications , vol. 21, no. 6, pp. 1 – 6, 1991. [16]   A. Trindade and L. C. Cordeiro, “Applying smt-based verification to hardware/software partitioning in embedded systems,”   Design Autom. for Emb. Sys. , vol. 20, no. 1, pp. 1–19, 2016. [Online]. Available: https://doi.org/10.1007/s10617-015-9163-z [17]   C. Baier and J. Katoen,   Principles of model checking .   MIT Press, 2008. [18]   A. Biere, “Bounded model checking,” in   Handbook of Satisfiability .   IOS Press, 2009, pp. 457–481. [19]   C. W. Barrett, R. Sebastiani, S. A. Seshia, and C. Tinelli, “Satisfiability modulo theories,” in   Handbook of Satisfiability .   IOS Press, 2009, p. 825?885. [20]   L. Cordeiro, B. Fischer, and J. Marques-Silva, “SMT-based bounded model checking for embedded ANSI-C software,”   IEEE Transactions on Software Engineering , vol. 38, no. 4, pp. 957–974, July 2012. [21]   M.   Y.   R.   Gadelha,   H.   I.   Ismail,   and   L.   C.   Cordeiro,   “Handling loops in bounded model checking of C programs via k-induction,”  STTT ,   vol.   19,   no.   1,   pp.   97–114,   2017.   [Online].   Available: https://doi.org/10.1007/s10009-015-0407-9 [22]   F. R. Monteiro, M. Garcia, L. C. Cordeiro, and E. B. de Lima Filho, “Bounded model checking of C++ programs based on the qt cross- platform framework,”   Softw. Test., Verif. Reliab. , vol. 27, no. 3, 2017. [Online]. Available: https://doi.org/10.1002/stvr.1632 [23]   L.   C.   Cordeiro,   “SMT-based   bounded   model   checking   of   multi- threaded   software   in   embedded   systems,”   Ph.D.   dissertation, University   of   Southampton,   UK,   2011.   [Online].   Available: http://eprints.soton.ac.uk/186011/ [24]   R. Alur, R. Bodik, G. Juniwal, M. M. K. Martin, M. Raghothaman, S. A. Seshia, R. Singh, A. Solar-Lezama, E. Torlak, and A. Udupa, “Syntax- guided synthesis,” in   Formal Methods in Computer-Aided Design , 2013, pp. 1–8.