An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach

Available online at www.sciencedirect.com

Journal of the Franklin Institute ] (]]]]) ]]]–]]] www.elsevier.com/locate/jfranklin

An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach G. Franzèn, W. Lucia DIMES – Università della Calabria Rende (CS), 87036, Italy Received 14 October 2014; received in revised form 29 January 2015; accepted 12 March 2015

Abstract The paper addresses the obstacle avoidance motion planning problem for ground vehicles operating in uncertain environments. By resorting to set-theoretic ideas and sum of squares (SOS) decomposition techniques, a receding horizon control algorithm is proposed for robots modeled by polynomial systems subject to input, state and nonholonomic constraints. Sequences of inner ellipsoidal approximations of the exact one-step controllable sets are computed for all admissible obstacle scenarios and then on-line exploited to determine the more adequate control action to be applied in a receding horizon fashion. The results here proposed are a significant generalization of existing algorithms which are tailored only for linear time invariant plant descriptions. The resulting framework guarantees uniformly ultimate boundedness and constraints fulfilment regardless of any obstacle scenario occurrence. & 2015 The Franklin Institute. Published by Elsevier Ltd. All rights reserved.

1. Introduction Motion planning and autonomous vehicle control problems in uncertain environments are of paramount importance in several human applications where guaranteed real time feasibility, safety against uncertainties and disturbances are required. Despite extensive research, this problem still represents a real challenge that becomes more complex by taking into account the limited perception abilities and computational power resources of the robot, as well as restrictions on the vehicle mobility n

Corresponding author. E-mail addresses: [email protected] (G. Franzè), [email protected] (W. Lucia).

http://dx.doi.org/10.1016/j.jfranklin.2015.03.021 0016-0032/& 2015 The Franklin Institute. Published by Elsevier Ltd. All rights reserved.

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

2

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

and underactuation phenomena. Collision avoidance is a key component of safe navigation whose a typical objective is to reach a target through the obstacle-free part of the environment [1]. The existing algorithms can be generally classified into global and local path planners [2]. Global planning methods rely on a priori environment information and guarantee feasible or even optimal paths to the goal, but require huge computational resources. Local obstacle avoidance methods (also referred to as reactive methods) are mainly based on sensory data collected during operations that make them computationally efficient and hence suitable for real-time applications. However, most of the algorithms proposed in the literature only address the path planning aspect by leaving out the control phase, see [3,2] and references therein. This represents a serious drawback when the vehicle is subject to dynamical constraints because any tracking capability is de facto limited. Along these lines are also the recent contributions [4] and [5]. The first proposes a framework for path planning based on the use of homogeneous forms and linear matrix inequalities (LMIs), while in the second paper the attention has been focused on the formal definition of a navigation algorithm within dynamic cluttered environments. Moreover, a relevant aspect to be taken into account is that wheeled vehicles are typical examples of nonholonomic mechanical systems see e.g. [6,7]. These robots share strong controllability properties, but the nonholonomic kinematic constraints which characterize their motion render the associated control design problem quite challenging. In [8], this has been discussed in depth where the non-existence of pure-state feedbacks for the asymptotic stabilization of fixed configurations has been proved. This difficulty has had the effect of focusing the research on the feedback control of nonholonomic systems on two distinct subproblems, namely (i) fixed point asymptotic stabilization relying on highly nonlinear techniques, and (ii) asymptotic stabilization of feasible trajectories based on more classical linear and nonlinear techniques, see [9,10]. Of interest here are constrained Receding Horizon Control strategies which are an extremely appealing methodology for the obstacle avoidance motion planning problem due to their intrinsic capability to generate at each time instant feasible trajectories. Noticeable recent contributions are from [11,12]. In [11], the authors address the set-point regulation problem of a nonholonomic wheeled mobile robot with obstacle avoidance in a known dynamic environment populated with static and moving obstacles subject to robot kinematic and dynamic constraints by using the model predictive control (MPC) in polar coordinates. In [12], the proposed scheme is based on the combination of a model-based optimization scheme and a convergence-oriented potential field method. This is done by recasting the two approaches in an MPC and control Lyapunov function (CLF) framework so that a tractable version of the dynamic window approach (DWA) results. More recently, predictive and adaptive active steering control schemes for nonlinear model descriptions have been presented, see [13–16]. The first contribution essentially proposes an algorithm whose aim is to impose that the autonomous vehicle follows safe pre-defined reference trajectories which are assumed to be collision-free and achievable by the vehicle. In [14] a standard MPC strategy has been developed for dealing with a tracking problem while avoiding collision with cluttered obstacle scenarios. In [15] a tracking and stabilization adaptive control of damped mobile robots with unknown parameters and subject to input torque saturations has been proposed in order to compensate exogenous bounded disturbances. In [16] a two-mode MPC scheme which ensures feasibility retention despite time-varying state constraints is presented: the standard operative mode is based on the classical RHC philosophy, while the safe mode, activated under time-varying constraints scenarios, allows to confine the state trajectory within a robustly positively invariant set. An important aspect leaves out concerns with the definition of a procedure capable to determine new feasible paths when the standard mode cannot be recovered. Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

3

Moving from these considerations and thanking to the recent advances in semidefinite programming along with the use of semi-algebraic methods for developing efficient control strategies of polynomial systems [17], in this paper the basic SOS MPC strategy proposed in [18] is extended to deal with the solution of the obstacle avoidance problem under the presence of nonholonomic constraints. It is assumed that the set of all possible obstacle locations on the planar environment are known, but it is unpredictable which is the obstacle configuration within the working environment at each time instant. The latter gives rise to a certain level of uncertainty that if not properly treated can lead to collisions during the vehicle navigation. To deal with this problem the idea here proposed is to develop a set-theoretic based receding horizon approach so that the prescribed constraints are always fulfilled despite any obstacle scenario occurrence. The main ingredients of the proposed dual-mode predictive strategy can be summarized as follows:






a stabilizing state feedback law and a robust positively invariant ellipsoidal set centered at the goal location are first computed; according to the given obstacle scenario configurations, a procedure to off-line compute families of one-step ahead controllable sets in order to enlarge the domain of attraction is derived; at each sample time and given the current obstacle scenario, an on-line receding horizon strategy is obtained by deriving the smallest ellipsoidal set complying with the obstacle configuration. The control move is computed by minimizing a performance index such that the one-step ahead state prediction belongs to its successor.

A relevant feature of this scheme relies on its capability to move off-line most of computations and to ensure that at each time instant a feasible solution there always exists despite any admissible occurrence of the obstacle configurations. This leads to a substantial improvement in the computational efficiency and represents a key aspect when real-time applications are taken into consideration. Finally, theoretical results are illustrated by means of simulations on a dynamical nonholonomic mobile robot model whose the navigation within a planar environment is limited by unpredictable occurrences of two obstacle scenarios. Notations and preliminaries:





With R½x we denote the ring of multivariate scalar polynomials p A R½x in the unknown xA Rn : With Σ½x  R½x ( ) q
X  q
2 Σ½x≔ s A R½x
( qo1; ( pi i ¼ 1 ; pi A R½x; s:t: s ¼ pi i¼1




we denote the proper and closed subset of multivariate sum-of-squares (SOS) polynomials sA Σ½x in the unknown x A Rn . The polynomial pðxÞ, having degree 2d, belongs to Σ½x iff there exists a symmetric matrix Q ¼ QT Z 0 such that  T pðxÞ ¼ zT ðxÞQzðxÞ; zðxÞ ¼ 1; x1 ; x2 ; …; xn ; x1 x2 ; …; xdn ; ð1Þ with z(x) containing all monomials in the variables x1 ; …; xn of degree lower than or equal to d. The matrix Q is known as the Gram matrix and Eq. (1) as the Square Matricial Representation (SMR) of p(x), see [19–21].

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

4




n m Given a set  SDX  YDR  R , the projection of the set S onto X is defined as ProjX fSg≔ xA Xj (y A Y s:t: ðx; yÞA S .

Let us consider nonlinear discrete-time dynamical systems xðt þ 1Þ ¼ f ðxðtÞ; uðtÞÞ þ Gw wðtÞ

ð2Þ

and wðtÞ A W  R , with W≔fw A where t A Zþ ≔f0; 1; …g, xðtÞA R , uðtÞA R , Gw A R Rw jcw ðwÞr 1g a bounded exogenous disturbance including unmodeled dynamics and cw ðwÞA R½w a convex and compact polynomial function. Moreover, the following set-membership state and input constraints are prescribed: n

m

nm

w

xðtÞA X ; 8 t Z 0;

X ≔fx A Rn jcx ðxÞr 1g;

ð3Þ

uðtÞA U; 8 t Z 0;

U≔fuA Rm jcu ðuÞr 1g;

ð4Þ

where cx(x) and cu(u) are convex and compact polynomial scalar functions belonging to R½x and R½u, respectively. Definition 1. A set T DRn is robustly positively invariant (RPI) for Eq. (2) if there exists a control law uðtÞ A U such that once the closed-loop solution xCL(t) enters inside that set at any given time t0, it remains in it for all future instants, i.e. xCL ðt 0 ÞA T -xCL ðtÞA T ; 8 wðtÞA W; 8t Z t 0 . Definition 2. Given two sets A and B, the subtraction A  B≔faA A : a þ bA A; 8 b A Bg is said P-difference and  the P-difference operator [22].

ð5Þ □

Given the system (2), it is possible to determine the sets of states k-step controllable to Ξ via the following recursion (see [23]): Ξ 0 ≔Ξ Ξ k ≔fxA X : (u A U : f ðx; uÞ þ Gw w A Ξ k  1 ; 8 w A Wg ¼ fx A X : ( uA U : f ðx; uÞ A Ξ k  1  Gw Wg ¼ fx A X : ( uA U : f ðx; uÞ A Ξ~ k  1 g

ð6Þ

where Ξ0 is known as the terminal region, Ξk is the set of states that, irrespective of any admissible disturbance occurrence w A W, can be steered into Ξk-1 using a single control move and Ξ~ k  1 ≔Ξ k  1  Gw W. Furthermore, sets Ξk and Ξ~ k can be characterized in terms of appropriate polynomial functions Ψ k ðxÞ A R½x and Ψ~ k ðxÞA R½x as     Ξ k ≔ x A Rn : Ψ k ðxÞr 1 ; Ξ~ k ≔ xA Rn : Ψ~ k ðxÞr 1 An equivalent description of Ξi can be given in terms of appropriate polynomial level surface functions V k ðx; uÞA R½x; u (see [18]): V k ðx; uÞ40; 8 xA Rn of0g; u A Rm {f0g and V k ð0; 0Þ ¼ 0   x A Rn ; u A Rm jV k ðx; uÞr 1 \ fx A Rn ; uA Rm jcu ðuÞr 1g   \ fxA Rn ; uA Rm jcx ðxÞr 1gD xA Rn ; u A Rm jΨ~ k  1 ðf ðx; uÞÞ r 1

ð7Þ

ð8Þ

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

Ξ k ¼ Projx fx A X DRn ; u A UDRm j V k ðx; uÞ r 1g

5

ð9Þ

Note that Eqs. (7)–(8) are rephrasing the recursion (6) in the extended space ðx; uÞ. Proposition 1. Conditions (7)–(8) are satisfied if there exist V k ðx; uÞA Σ nþm ; si A Σ nþm ; i ¼ 1; …; 5; such that V k ðx; uÞ l1 ðx; uÞ A Σ nþm    ð1  V k ðx; uÞÞs1 þ ð1  cu ðuÞÞs2 þ ð1 cx ðxÞÞs3 þð1  V k ðx; uÞÞð1 cu ðuÞÞs4  þð1  V k ðx; uÞÞð1 cx ðxÞÞs5 þ ðΨ~ k  1 ðf ðx; uÞÞ  1Þ A Σ nþm

ð10Þ

ð11Þ

where l1 A R½x; u is a suitable chosen positive definite polynomial. Proof. By resorting to Positivstellensatz (P-satz) and S-procedure arguments, and by following the lines indicated in [24]. □ Definition 3. Let S be a neighborhood of the origin. The closed-loop trajectory of Eq. (2) is said to be Uniformly Ultimate Bounded in S if for all μ40 there exist TðμÞ40 and uðtÞA U such that, for every J xð0ÞJ r μ, xCL ðtÞA S for all t Z TðμÞ. Definition 4. Given a set S  Rn and a point x A Rn , the distance between them is defined as distðx; SÞ ¼ inf Jx  s J n ; sAS

where n is any relevant norm. Definition 5. Given two sets S; R  Rn the distance between them is defined as distðS; RÞ ¼ inff Js  r J n : s A S; r A Rg Definition 6. An oriented graph is an ordered pair G ¼ ðV; EÞ such that





V is the vertex set; E is a subset of ordered pairs of V known as the edge set, i.e. E  ffu; vgju; v A Vg

Definition 7. Let G ¼ ðV; EÞ an oriented graph, the reachable vertex set Vr is defined as Vr ≔fv A Vj ( u A V : fu; vgA Eg Definition 8. Let f(z) and g(z) be two differentiable vector fields. The Lie bracket of f(z) and g(z) is another vector field defined as ½f ; gðzÞ≔

∂g ∂f ðzÞf ðzÞ ðzÞgðzÞ ∂z ∂z

ð12Þ

2. Nonholonomic mobile robots The aim of this section is twofold: to provide detailed model descriptions of autonomous ground vehicles and to show that such nonlinear models can be recast as polynomial systems. To this end, two well-known model descriptions of the unicycle car-like mobile robot depicted in Fig. 1 are first presented and then a polynomial recasting procedure is outlined. Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

6

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

Y v

y

x

O

X

Fig. 1. Nonholonomic mobile robot.

2.1. Kinematic model A differential drive robot is a typical nonholonomic wheeled vehicle, which has two rear drive wheels and a front castor for body support. It is assumed that the motion of mobile robot cannot slip laterally so that the translational velocity is in the direction of heading, i.e. a pure rolling contact between the wheels and the ground. The velocity of the two rear wheels ðvl and vr Þ is used to impose the translation ðv ¼ ðvl þ vr Þ=2Þ and angular ðω ¼ ðvr  vl Þ=BÞ speeds of the robot (B is the wheelbase). The robot pose is described by its position ðx; yÞ, the midpoint of the rear axis of the robot, and its orientation (θ). Then, the kinematics equation is 2 3 3 2 3 2 x_ ðtÞ cos ðθðtÞÞ 0 6 7 7 6 y_ ðtÞ 7 6 ð13Þ 4 5 ¼ 4 sin ðθðtÞÞ 5vðtÞ þ 4 0 5ωðtÞ _θðtÞ 0 1 where maximum linear and angular velocities are prescribed: jvðtÞj r V MAX and jωðtÞj r W MAX ; 8t. Notice that the kinematic model (13) is nonintegratable and, as a consequence, kinematics constraints cannot be converted into geometrical requirements [25]. Moreover since the number of control variables is less than the number of state variables, a nonholonomic constraint holds and a continuous time-invariant feedback control law cannot be used [10,26]. On the other hand, since the accessibility rank condition is globally satisfied [27]: 02 3 2 331 3 2 3 22 cos ðθðtÞÞ cos ðθðtÞÞ 0 0 B6 7 6 77C 7 6 7 66 ð14Þ Rank@4 sin ðθðtÞÞ 5; 4 0 5; 44 sin ðθðtÞÞ 5; 4 0 55A ¼ 3; 0

1

0

1

the model plant (13) is controllable by means of a nonlinear or time-varying controller. Finally for the sake of completeness, the car-like plant of Fig. 1 can be equivalently described by means of a polar coordination frame. In fact, by defining the polar state components as follows: pffiffiffiffiffiffiffiffiffiffiffiffiffiffi e ¼ x2 þ y2 Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

ϕ ¼ α tan 2ð  y;  xÞ α ¼ ϕθ we have that Eq. (13) can be converted into the following polar form: 8 cos ðαðtÞÞ > > e_ ðt Þ ¼  vðt Þ > > eðtÞ > > > < sin ðαðtÞÞ ϕ_ ðt Þ ¼ vðt Þ eðtÞ > > > > > sin ðαðtÞÞ > > : α_ ðtÞ ¼  ωðt Þ þ vðt Þ eðtÞ

7

ð15Þ

ð16Þ

2.2. Dynamical model We consider that the motion and orientation are achieved by two independent actuators (e.g. DC motors) providing the necessary torques τ1 and τ2 to the driving wheels. The robot pose in the inertial Cartesian frame fO; X; Yg is completely specified by the 3-dimensional vector of generalized coordinates qðtÞ≔ðxðtÞ; yðtÞ; θðtÞÞ. Moreover, the autonomous vehicle is subject to an independent velocity constraint: x_ ðtÞ sin ðθðtÞÞ  y_ ðtÞ cos ðθðtÞÞ ¼ 0

ð17Þ

This represents a nonholonomic constraint and states that the robot can only move in the direction normal to the axis of the driving wheels: the mobile base satisfies the pure rolling and non-slipping conditions, see [7]. Then, the vehicle can be described by the following dynamical model: 8 cos ðθðtÞÞ > > x€ ðt Þ ¼  y_ ðt Þθ_ ðt Þ þ ðτ1 ðt Þ þ τ2 ðt ÞÞ > > mr > > < sin ðθðtÞÞ ðτ1 ðt Þ þ τ2 ðt ÞÞ y€ ðt Þ ¼ x_ ðt Þθ_ ðt Þ þ ð18Þ > mr > > > R > > : θ€ ðt Þ ¼ ðτ1 ðt Þ  τ2 ðt ÞÞ Ir where m, I, R are the robot mass, moment of inertia and length, respectively, and r is the wheels radius. Finally, notice that at each time instant the robot pose qðtÞ, as well as its  that it is assumed  _ T are available for feedback. _ ¼ x_ ðtÞ y_ ðtÞ θðtÞ derivative, qðtÞ 2.3. Non-polynomial to polynomial systems recasting procedure Since our subsequent developments will consider polynomial models, in this section we describe an algorithmic procedure useful to convert a non-polynomial system into its correspondent polynomial description, see [28]. Specifically, we consider non-polynomial systems whose vector field is a combination of sums and products of elementary functions of the following classes: exponential, power and trigonometric. Let us suppose that the nonlinear system is given in the following form: X ξ_ i ¼ λj ∏F ijk ðξÞ; i ¼ 1; …; s ð19Þ j

k

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

8

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

where λj A R, ξ ¼ ½ξ1 …ξs T and F ijk ðξÞ are elementary functions or some combinations of them. The recasting procedure is as follows: Recasting procedure (Papachristodoulou and Prajna [28]). 1. Let zi ¼ ξi ; i ¼ 1; …; s. 2. For each F ijk ðξÞ that is not in the form F ijk ðξÞ ¼ ξpl ; p A Zþ ≔f0; 1; …g and 1r l r s, introduce a new variable zr such that zr ¼ F ijk ðξÞ. 3. Compute the differential equation describing the time evolution of zr. 4. Replace all appearances of such F ijk ðξÞ in the system equations by zr. 5. Repeat steps 2–4, until a system of polynomial differential equations is achieved.

It is worth to note that the recasting process introduces additional and fictitious state variables with the unavoidable consequence of deriving a polynomial model with dimensions higher than the original plant. Moreover to guarantee the equivalence between the polynomial version and the original manifold, the following state constraints must be taken into account: zr ¼ Fðz1 ; …; zs Þ;

8r4s:

ð20Þ

Therefore the non-polynomial system (19) can be rewritten as ( z_ a ¼ f a ðza ; zb Þ z_ b ¼ f b ðza ; zb Þ

ð21Þ

where za ¼ ½z1 …zs  are the state variables of the original system (19), zb ¼ ½zsþ1 …zn  the additional state variables and f a ðza ; zb Þ , f b ðza ; zb Þ the polynomials forms. Moreover, besides the equality constraints (20) arising from the Recasting procedure, the following further equality/ inequality requirements: Ga ðza ; zb Þ ¼ 0

ð22Þ

Gb ðza ; zb ÞZ 0

ð23Þ

are necessary in order to obtain a polynomial form for f a ðza ; zb Þ and f b ðza ; zb Þ. Note that F; Ga and Gb are column vectors of functions with appropriate dimensions. For a detailed analysis and exhaustive examples, the interested reader can refer to [28]. Finally, it is worth to recall that different schemes can be used to recast non-polynomial systems into the polynomial field, see e.g. [29] where the nonlinear function is described by means of a truncated Taylor expansion with the remainder confined into a polytopic set. 3. Problem formulation We consider autonomous vehicles described by polynomial discrete-time systems zðt þ 1Þ ¼ f p ðzðtÞ;

uðtÞÞ þ Gw wðtÞ



ð24Þ

where f p : Rn  Rm -Rn is an array of multivariate polynomials, zðtÞ ¼ zqa ðt ÞT zva ðt ÞT zb ðt ÞT T A X  Rn the state with zqa A Rq and zva A Rv accounting for the robot pose and its derivative respectively, uðtÞ A U  Rm the control input, wðtÞA W  Rw a bounded exogenous disturbance including unmodeled dynamics and Gw A Rnw . Moreover, the vehicle is subject to Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

9

nonholonomic constraints of the following form: aTi ðzqa ðtÞÞzva ðtÞ ¼ 0;

i ¼ 1; …; na ; 8 t A Zþ ;

ð25Þ

with na being the number of nonholonomic constraints and ai : R -R ; i ¼ 1; …; na , nonlinear functions of the vehicle pose, see [6]. In the sequel, it is supposed that vehicles move within dynamic environments characterized by the following uncertain obstacle configuration: q





v

each moving object can occupy pre-specified positions so that a finite and known set of obstacle scenarios comes out; time instants at which the obstacle scenario changes are unknown.

This set-up concerns with partially known working areas where a specific set of obstacle scenarios may occur, e.g. production/assembly lines [30], restricted areas (buildings or rooms) where surveillance or housekeeping are required tasks [31] and so on. In order to formally describe this framework, the following definitions will be used: Definition 9. Let Obij be an object. Then an obstacle scenario Oi is defined as Oi ≔fObi1 ; …; Obini g

ð26Þ

where ni denotes the number of the involved objects. The obstacles Obij ; i ¼ 1; …; l; j ¼ 1; …; ni , have a polyhedral convex structure described as the intersection of lj half-spaces: 2 T 3 2  3 gij H ij 6 17 1 7 6 7 6 ⋮ 7p r 6 ⋮ 7 Obij : 6 ð27Þ 6 7 6 T 7 4 i 5 5 4 i gj Hj lj

lj

where p≔Bz A R2 are the planar components of the state space z A Rn and B AR2n a projection matrix. Definition 10. Let Oi be an obstacle scenario. Then the non-convex obstacle-free region pertaining to Oi is identified as follows: Oifree ≔fzA Rn : hi ðzÞ40g

ð28Þ

where hi : Rn -Rnf characterizes the admissibility state space region and nf is the number of component-wise inequalities. Then, the problem we want to solve can be stated as follows. Obstacle Avoidance Motion Planning (OAMP) Problem – Given a set of obstacle scenarios Oi ; i ¼ 1; …; l, determine a state-feedback control policy uðtÞ ¼ gðzðtÞÞ

ð29Þ

compatible with Eqs. (3), (4), (25) and (28), such that starting from an initial condition zð0Þ the robot trajectory z(t) is driven to a target position zf regardless of any obstacle scenario occurrence. Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

10

4. Set-theoretic based approach background Consider autonomous vehicles described by discrete-time LTI systems zðt þ 1Þ ¼ ΦzðtÞ þ GuðtÞ þ Gw wðtÞ

ð30Þ

In [32,33] a set-theoretic based solution to the OAMP problem has been recently obtained for models (30) by dealing with the following key aspects: (a) Given a set of obstacles (27), determine computationally tractable geometric conditions under which the admissibility region (28) is convexified. (b) Given an obstacle scenario Oi , determine a sequence of inner ellipsoidal approximations  i N i  N i such that there exists at least a T s s ¼ 0 of the exact one-step controllable sets Ξ is s ¼ 0 feasible path to the goal .  zf Ni (c) Given the sequences T is s ¼ 0 ; i ¼ 1; …; l, ensure viability properties under time-varying scenarios. Next sections will be devoted to summarize the main technical issues that allow to face (a)–(c). 4.1. Obstacle constraints convexification The non-convex constraints describing Eq. (28) can be rewritten as ( )

 lj T ni i n i i Ofree ≔ ⋂ zA R : ⋃ H j Bz4 gj j¼1

k¼1

k

ð31Þ

k

and recast into computationally tractable requirements by means of the convexification procedure (CP) developed in [33]

a

a

a o ni n  i a  i a  i a c ¼ c1 \ c2 \ ⋯ \ cini ¼ ⋂ zA Rn j H ij Bz4 gij ð32Þ j¼1

a where cij are the active constraint regions pertaining to each j-th obstacle of Oi . 4.2. One-step controllable sets Point (b) relies on the estimation of the domain of attraction (DoA), i.e. all the initial conditions zð0Þ for which there exists an admissible path to zf . Note that the basic construction (recursions (6)) of T ik may give rise to “small” DoA estimates, i.e. ⋃li ¼ 1 T iN i , because a saturation effect may occur on the one-step controllable sets growth. To overcome such a drawback, here we modify the construction of these sets as follows: One-step Controllable Set Procedure (OCSP) 1: Given the goal zf and chosen the initial condition zð0Þ as follows: zð0Þ≔argmax Jz  zf J 2 design the pair ðK 0 ; T 0 Þ, with T 0 a RPI region centered in zf where K0 is zAX

the stabilizing state-feedback gain complying with the constraints (4), (3), (28). Store the subscript m ¼ 0 into an index vector hereafter named IRi . Let T i0 ¼ T 0 and z0eq ¼ zf be the initial terminal region and equilibrium point, respectively;

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

11

 N m 2: Derive the sequence T ik k ¼ 1 by using recursions (6) under the additional state constrain (28). The integer Nm is the saturation level for the region growth; 3: Store the index Nm into an index vector denoted as LRi ; 4: if zð0Þ= 2 T iN m , then such that: 5: if there exists a candidate equilibrium zmþ1 eq mþ1 k ðaÞ zeq a zeq ; k ¼ 0; 1; …; m; ðbÞ zmþ1 eq ≔arg mini J zeq  zð0Þ J 2 zeq A T N m

then and K mþ1 satisfying 6: Design a new pair ðK mþ1 ; T iN m þ1 Þ, with T iN m þ1 centered in zmþ1 eq i Eqs. (4), (3), (28). Store the corresponding index N m þ 1 into IR ;  N m 7: Add T ik k ¼ 1 to the previous computed sequence; 8: m’m þ 1, T i0 ≔T iN m þ1 and goto Step 2; 9: else Stop; 10: end if 11: else Stop; 12: end if

4.3. Time-varying obstacle scenario occurrences The question (c) focuses on the difficulties arising when unpredictable obstacle scenario changes occur. Since the l controllable set sequences fT ik g; i ¼ 1; …; l, are computed under the hypothesis that a single obstacle scenario Oi takes place, the viability retention cannot be ensured 0 when Oi -Oi because a switching to a different set sequence must be imposed. Here, the idea is to design two further families of one-step controllable sets, hereafter named 0 Obstacle and Scenario Switching sequences, whose combined use allows to safely switch to fT ik g.

4.3.1. Obstacle controllable sequences Obi

0

The Obstacle sequences fT k j g; j ¼ 1; …; ni0 , have the aim to encircle the corresponding 0 obstacles Obij ; j ¼ 1; …; ni0 and they are introduced because of the following argument: 0

Let Oi be the current scenario, if at t the obstacle scenario change Oi -Oi occurs and the 0 2 T ik , for some k, then collisions could happen because the sequence fT ik g is no current state zðtÞ= longer admissible. In order to avoid such an undesired event, the Obstacle sets are designed such that when Obi

0

zðt^Þ A fT k j g; t^ Z t, the trajectory zðtÞ; 8t Z t^ remains confined into them until the switching to 0 the correct sequence fT ik g is made admissible. Obi

0

The computation of the sequences fT k j g; j ¼ 1; …; ni0 , is performed by using the OCSP procedure Obi

0

where both zð0Þ and zf are substituted with the equilibria zeq j ; j ¼ 1; …; ni0 . These equilibrium points 0 are selected as those at the maximum distance from the obstacles Obij ; j ¼ 1; …; ni0 , and satisfying the Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

12

following condition: Obi

0

0

0

distðBzeq j ; Obij ÞodistðT imaxj ; Obij Þ;

j ¼ 1; …; ni0 ;

ð33Þ

where T imaxj are the sets corresponding to the greatest indices s of fT ik g such that 0

T imaxj \ Obij ¼ ∅;

j ¼ 1; …; ni0

ð34Þ 0 Obij

The latter allows to ensure that the equilibrium xeq lies in the zone bounded by the obstacle 0 and the closer element of fT ik g which does not intersect Obij , i.e. T imaxj . 4.3.2. Scenario switching controllable sequences Although the Obstacle sequences have the important merit to avoid collisions when the 0 scenario Oi occurs, their use leads to the following drawback: 0 Let Oi -Oi be a generic scenario change occurrence, once the trajectory zðÞ enters inside 0

Obi

Obij

the region defined by fT k j g, it is by construction driven to the terminal region T 0 Obi zeq j

0

centered at

0

the equilibrium and it will be there confined. This situation clearly compromises any chance of the vehicle to reach the target zf and for this 0 reason the trajectory zðÞ must be driven towards the controllable sequence fT ik g. To this aim, the SW ij

Scenario Switching sequences fT k

0

g; j ¼ 1; …; ni , are introduced and designed as follows: SW i

0

0

1. for any admissible scenario change Oi -Oi , choose an equilibrium zeq j belonging to some 0 T ik ; 0 0 SW i Obi 2. apply the OCSP procedure with zeq j in place of zð0Þ and zeq j in place of zf .

0

Then, the switching fT ik g-fT ik g is accomplished as follows:




Obi

0

Let zðt^ÞA fT k j g be the current state. As soon as SW ij

zðtÞA T k^




0

for some k^ and for some t Z t^;

the set-membership to fT As soon as 0

zðtÞA T ik

SW ij k

ð35Þ

0

g is considered;

for some k and for some t Z t^:

ð36Þ

0

the set-membership to fT ik g can be used.

A relevant aspect leaved out in the above developments is to guarantee that under a scenario change the robot does not bump on any obstacle during the switching phase. To this end, the SW i

sequences fT ik g and fT k j g are derived under the following further condition: JBðzþ  zÞJ r ϵ;

ð37Þ

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

13

where zþ ¼ Φz þ Gu is the disturbance-free one-step evolution. The scalar ϵ can be determined by exploiting the Obstacle sequences properties as below detailed:




Obi

for each scenario Oi and for each j-th obstacle, consider the sequence fT k j g and determine IN i

the sequence fT m j g as follows: 8 IN ij Obij Obij > > < T m ≔T LRij ðmÞ ⋂ T LRij ðmþ1Þ ;

m ¼ 1; …; dimðLRij Þ 1

ð38Þ

IN j Obj Obj > > : T dimðLRij Þ ≔T LRij ð1Þ ⋂ T LRij ðdimðLRij ÞÞ i

i

i

Obi




where the index vectors LRij store the saturation regions for each sequence fT k j g; compute the minimum dij and maximum Dij Euclidean distances between the j-th obstacle




(polyhedron) and the indexed set sequence fT m j g; compute

IN i

d≔max i ¼ 1;…;l;

dij

ð39Þ

Dij

ð40Þ

j ¼ 1;…;pi

D≔ imin ¼ 1;…;l; j ¼ 1;…;pi

ϵ≔D  d

ð41Þ

4.3.3. One-step controllable families computation The consequence of all the above developments is that the set sequences must be computed by using the following scheme: Families Construction Procedure (FCP) 0. Let G ¼ ðV; EÞ be the oriented graph with V the l obstacle scenarios and E the set of ordered 0 pairs fi; i0 g; i; i0 A V, such that the switching Oi -Oi is admissible; 1. Generate the sequences fT ik g; i ¼ 1; …; l; Obi

2. Generate the Obstacle families fT k j g; 8 iA Vr ; j ¼ 1; …; ni , according to Eqs. (33)–(34); 3. Estimate the tolerance level ϵ by Eqs. (39)–(41); 4. Update the sequences fT ik g; i ¼ 1; …; l under the additional constraint (37); SW i

5. Generate the sequences fT k j g; 8 iA Vr ; j ¼ 1; …; ni under the additional constraint (37).

5. A SOS-based RHC algorithm As discussed in the Introduction, the aim of this paper is to present an MPC strategy for the class of nonlinear systems (24) subject to nonholonomic constraints (25). In this section, the settheoretic based scheme of [33] summarized in Section 4 will be extended to comply with the Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

14

polynomial plant description (24). To this end, the following points deserve an adequate consideration within the polynomial framework: (1) Computation of controllable set sequences fT ik g. (2) Sum-of-Squares characterization of the requirement (37) which limits the maximum displacement of the one-step state evolution so that obstacle constraints are satisfied with a tolerance level ϵ40. (3) Relaxation of the on-line optimization: uðtÞ ¼ argmin J kðtÞ ðzðtÞ; uÞ subject to

ð42Þ

s f p ðzðtÞ; uÞA Ξ~ kðtÞ  1 ; u A U

ð43Þ

5.1. One-step controllable families SOS characterization By resorting to Proposition 1 and under the further requirements imposed by the nonholonomic constraints (25), we have that the sets of states k-steps controllable to Ξ0 complying with Eqs. (3), (4) and (25) can be obtained by defining a collection of polynomial functions fV k ðz; uÞg each one satisfying the following condition and set containment: ð44Þ V k ðz; uÞ40; 8 zA Rn of0g; 8 u A Rm of0g and V k ð0; 0; 0Þ ¼ 0     z A Rn ; u A Rm : V k ðz; uÞr 1 \ z A Rn ; u A Rm : cx ðzÞr 1     \ zA Rn ; u A Rm : cu ðuÞr 1 \ zA Rn ; u A Rm : Ga ðza ; zb Þ ¼ 0     \ zA Rn ; u A Rm : zsþ1 ¼ Fðz1 ; …; zs Þ \ ⋯ \ z A Rn ; uA Rm : zn ¼ Fðz1 ; …; zs Þ na

⋂

i¼1



   zA Rn ; uA Rm : aTi ðzq ðtÞÞzv ðtÞ ¼ 0 D zA Rn ; u A Rm : Ψ~ k  1 ðf p ðz; uÞÞ r1 ð45Þ

The next developments summarize methodological requirements and computational tools under which one-step controllable regions can be obtained. Proposition 2. Conditions (44)–(45)Pare satisfied if there exist a candidate function V k ðz; uÞA P , polynomial multipliers si A nþm ; i ¼ 1; …; 5, a radially unbounded positive definite nþm function l0 , li ; i ¼ 1; …; na , and ζ i ðz; uÞ; i ¼ 1; …; n s, positive polynomials such that ð46Þ V k ðz; uÞ  l0 ðz; uÞA Σ nþm 8 ðð1  V k ðz; uÞÞs1 þ ð1 cu ðuÞÞs2 þ ð1 cx ðzÞÞs3 þ ð1 V k ðz; uÞÞð1  cu ðuÞÞs4 > > > > ns X > > > < þ ð1 V k ðz; uÞÞð1  cx ðzÞÞs5 þ ðΨ~ k  1 ðf p ðz; uÞÞ  1Þ þ ζ i ðz; uÞðzsþi ¼ Fðz1 ; …; zs ÞÞ > > > > > > > :

i¼1

þ

na X i¼1

li ðz; uÞðaTi ðzq ðtÞÞzv ðtÞÞÞ A Σ nþm ð47Þ

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

Proof. By resorting to Positivstellensatz Theorem and S-Procedure arguments.

15

□

Remark 1. An inner approximation of the one-step controllable region in the extended space (z,u) can be achieved by means of the following optimization problem with bilinear matrix inequality (BMI) constraints: n n ona o V^ k ðz; uÞ; fs^ i g5i ¼ 1 ; ^l i ≔arg minP Tr ðQk Þ i¼1

V k ðz;uÞ A

si A

P

nþm ;i ¼ 1;…;5;

nþm li A Rnþm ;i ¼ 1;…;na ; ζi ðz;uÞRnþm ;i ¼ 1;…;n  s

s:t:

ð46Þ–ð47Þ

ð48Þ

where Qk is the Gram matrix of the decomposition V k ðz; uÞ ¼ φT ðz; uÞQk φðz; uÞ and Qk can be viewed as a shaping matrix of an ellipsoid in the extended space ðz; uÞ. From a computational point view, the BMI optimization (48) can be solved by means of standard SDP packages, [34,35], which make use of standard bisection and/or moment procedures as also  shown in [36].  Once the set z AX ; uA U : V^ k ðz; uÞr 1 is obtained, the projected set T k along z can be easily computed by exploiting the quantifier elimination procedure detailed in [37]. Remark 2. Finally, notice that the additional constraint J Bðf p ðxðtÞ; uÞ zðtÞÞ J 22 r ϵ2 straightforwardly translates into a SOS requirement and, therefore, can be used within the polynomial version of the FCP procedure. 5.2. On-line optimization Here, we shall provide a convenient formulation of the optimization (42)–(43) so that it can be rephrased into a SDP problem. As detailed in [33], the On-line phase of the RHC algorithm (namely OA-MPC) prescribes the solution of the following optimization problem: uðtÞ ¼ argmin J kðtÞ ðzðtÞ; uÞ

ð49Þ

s:t:

u

J Bðf ðzðtÞ; uÞ xðtÞÞ J 22 r ϵ2

ð50Þ

f p ðzðtÞ; uÞA Ξ~ kðtÞ  1 ;

ð51Þ

uAU

where the running cost is defined as follows: J kðtÞ ðzðtÞ; uÞ≔α J f p ðzðtÞ; uÞ J 2~ ðÞ

Q kðtÞ  1

þ β J uJ 2Ru

ð52Þ

~ iðtÞ  1 the Gram matrix of Ξ~ iðtÞ  1 and α; β A ½0; 1. with Ru ¼ RTu 40, Q As it is evident the optimization (49) –(51) is computationally high demanding and, therefore, a relaxation procedure has to be considered. To this end, note first that the sets of states and inputs satisfying Eqs. (50)–(51) belong to ( )
Ψ~ ðf ðz; uÞÞ r 1 n m
kðtÞ  1 ~ S kðtÞ  1 ≔ zA R ; u A R :
ð53Þ J Bðf ðzðtÞ; uÞ zðtÞÞ J 22 r ϵ2 with Ψ~ kðtÞ  1 ðf ðz; uÞÞ and J Bðf ðzðtÞ; uÞ zðtÞÞ J 22 SOS polynomials. Then, the following inner Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

16

ellipsoidal approximation of S~ iðtÞ  1 is considered in the extended space ðz; uÞ : n o T  T T T 1 T~ kðtÞ  1 ≔ zT ; uT A Rnþm : ½zT ; uT PkðtÞ r 1 DS~ kðtÞ  1 1 z ; u

ð54Þ

Using Eq. (54) in place of S~ kðtÞ  1 does not ensure feasibility retention of the OA-MPC algorithm: such a property is simply recovered by computing the entire sequence fΞ k g under the satisfaction of the relaxation (54). The family of one-step controllable sets obtained by means of the following procedure is hereafter denotes with fT k g Polynomial Ellipsoidal Set (PES) procedure 1. Given the set T k  1 (T 0 Ξ 0 ), compute the inner ellipsoidal approximation T~ k  1 of S~ k  1 by solving an Eigenvalue LMI optimization problem (EVP) [38];  T 2. Solve the optimization problem (48) with½zT ; uT Pk11 zT ; uT r 1in place of Ψ~ k  1 ðf p ðz; uÞÞ r 1; 3. Compute the projection (9) (Ξ k -T k ) . Finally, the optimization (49)–(51) can be recast into a simple LMI problem. Proposition 3. The optimization (49)–(51) is recast into the following semi-definite programming problem: minαγ z þ βγ u

ð55Þ

u

" s:t: "

γu n

γz

½zðt ÞT uT 

n

PkðtÞ  1

# Z0

# uT Z0 Ru 1

ð56Þ

ð57Þ

Proof. An upper-bound to the cost (52) can be derived by means of the following arguments:





J uJ 2Ru r γ u ; γ u A Rþ ; the following set n o T    1  T T T T~ kðtÞ  1 ðγ z Þ≔ zT uT A Rnþm ; zT uT PkðtÞ r γ z ; γ z A Rþ 1 x u with γ z A Rþ a slack variable, can be considered in place of T~ kðtÞ  1 .

Then, by choosing two weighting scalars α; β A ½0; 1, the optimization problem (56)–(57) becomes

s:t:

min αγ z þ β γ u u 8 T u Ru u r γ u > > > > < ½zðtÞT uT T A T~ kðtÞ  1 ðγ Þ z T u uru > > > > : γ r1 u

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

17

that is straightforwardly converted into the LMI conditions (56)–(57) via Schur complements. □ 5.3. RHC algorithm By collecting the above developments, the Receding Horizon Control algorithm proposed in [33] is here adapted to comply with polynomial systems. In the sequel the following assumptions are made: Assumption 1. At each time instant t the robot is informed about the current obstacle scenario, i.e. scðtÞ. □ 0

Assumption 2. At each time instant t, an obstacle scenario change Oi -Oi can occur if the actual robot position zp ðtÞ ¼ BzðtÞ is such that ni0

0

0

zðtÞ= 2 ⋃ fz A Rn j H ij Bz r gij þ dg

ð58Þ

j¼1

Sum-of-Squares Obstacle Avoidance MPC (SOS-OA-MPC) Algorithm Off-line: 1 Given the obstacle scenarios Oi ; i ¼ 1; …; l, the initial condition zð0Þ and the goal zf , compute a non-empty robust invariant ellipsoidal region T 0  Rn and a stabilizing feedback gain K0 complying with the constraints (3)–(4), (25) and (32); 2 Jointly apply the FCP and PES procedures to generate the inner ellipsoidal approximations n Obi o  i Obi Ξ k -fT ik g; i ¼ 1; …; l, Ξ k j -fT k j g; 8 iA Vr ; j ¼ 1; …; ni , and n SW i o SW i Ξ k j -fT k j g; 8 iA Vr ; j ¼ 1; …; ni , such that 1 0 1 ! 0 ⋃ T ik

zð0ÞA

i ¼ 1;…;l k

i

i

SW Ob [ @ ⋃ T k jA [ @ ⋃ T k jA 8 i A Vr j ¼ 1;…;ni ; k

8 i A Vr j ¼ 1;…;ni ; k

3 Store the ellipsoidal sequences. On-line: 1: if !   ObjscðtÞ scðtÞ [ ⋃ Tk [ zðt ÞA ⋃T k j ¼ 1;…;ni ; k

k

⋃ T

j ¼ 1;…;ni ; k

SW jscðtÞ k

! then curr≔scðtÞ

2: else curr≔prec; 3: end if then 4: if zðtÞA ⋃ T curr k k candidate g; 5: goto Step 10 by considering fT curr k g as the candidate sequence, i.e. fT k 6: end if SW curr j

7: if zðt ÞA ⋃ T k j ¼ 1;…;ni ; k

SW curr j

then fT candidate g≔fT k k Obcurr j

8: else fT candidate g≔fT k k 9: end if

g

g

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

18

10 Find kðtÞ≔mink fzðtÞA T candidate g k 11: if kðtÞA IRcandidate then uðtÞ ¼ K kðtÞ zðtÞ 12: else 13: solve Eqs. (55)–(57) 14: end if 15: Apply uðtÞ; prec≔curr; t≔t þ 1; goto Step 1

The next proposition shows that the proposed SOS-OA-MPC algorithm enjoys the feasibility retention and closed-loop stability properties. Proposition 4. Let the sequences of sets






 i T k ; i ¼ 1; …; l; n Obi o T k j ; 8 iA Vr ; j ¼ 1; …; ni ; n SW i o T k j ; 8 iA Vr ; j ¼ 1; …; ni ,

be non-empty and ! zð0ÞA

⋃ T ik

i ¼ 1;…;l k

0

1

0

1

Obij

SW ij

[@ ⋃ Tk A[@ ⋃ Tk A 8 i A Vr j ¼ 1;…;ni ; k

8 i A Vr j ¼ 1;…;ni ; k

Then, the SOS-OA-MPC algorithm always satisfies the constraints and ensures Uniformly Ultimate Boundedness for all time-varying occurrences of Oi ; i ¼ 1; …; l. Proof. Note that existence of solutions at time t implies existence of solutions at time tþ1, because the optimization problem in Step 13 is always feasible. In fact, by construction there exists an input vector u satisfying the constraints (3), (4), (25) and (32) such that the setmembership requirement in Eq. (51) holds true. Then because each element of the sequence fT~ i g has been built up under the satisfaction of the additional constraint constraints (37), see Eq. (53), at the next time instant tþ1 the existence of a solution uðt þ 1Þ for the Step 13 is ensured. Finally, Uniformly Ultimate Boundedness of the strategy follows by noting that the trajectory is in the worst case confined to 1 0 1 ! 0 ⋃ T ik

i ¼ 1;…;l k

i

i

SW Ob [ @ ⋃ T k jA [ @ ⋃ T k jA 8 i A Vr j ¼ 1;…;ni ; k

8 i A Vr j ¼ 1;…;ni ; k

□

6. Simulations This section presents numerical results that illustrate the features of the proposed MPC strategy. We shall assume that the robot navigates within a planar dynamic environment where two obstacle scenarios are hypothesized: O1 (continuous lines) and O2 (dashed lines), see Fig. 3. Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

19

sc(t)

2

1 0

20

40

60

80

100

120

Time [sec]

Fig. 2. Obstacle scenario switchings. 30 t=73 sec

25

x

y[m]

20

f

2 Ob1 2 =Ob 2

15

t=69 sec

10

Ob 2 1

t=59 sec

5 0

Ob 1 1

0

5

t=56 sec

10

x(0)

15

20

25

x [m] Fig. 3. Robot path under time-varying obstacles.

The planar environment is described by 2 3 2 3 1 0 0 " # 6 1 7 6 7 z 0 7 1 6 6 25 7 r6 7 6 7 4 0 4 0 5  1 5 z2 0

1

ð59Þ

30

while the obstacles locations are below reported: 2 3 2 3 1 0  14 " # 6 1 6 15 7 0 7 6 7 z1 6 7 Ob11 : 6 r6 7 7; Ob21 4 0 4  13 5  1 5 z2 0

1 2

1 6 1 6 Ob12 ¼ Ob22 : 6 4 0 0

1

6 1 6 :6 4 0 0

14 3 2 3 5 0 " # 6 6 7 0 7 7 z1 6 7 r6 7 7 4  18 5  1 5 z2 1

2

0

3

"

#

2

 14

3

6 15 7 0 7 7 z1 6 7 r6 7 7 4  11 5  1 5 z2 1

ð60Þ

12

ð61Þ

19

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

20

The aim of the simulation is to drive the robot from the initial planar position p0 ¼ ½20 5T to the target pf ¼ ½5 22T for any obstacle scenario change (from O1 to O2 and viceversa), see Fig. 2. All the simulations have been carried out by using Matlab R2014a and the Ellipsoidal Toolbox [39] over a laptop PC equipped with a Intel Core(TM) 2 Duo CPU. For the sake of completeness, we have considered the dynamical model (18) that has been recast into a polynomial field by resorting to the Recasting procedure of Section 2.3. This prescribes that _ z1 ðtÞ ¼ xðtÞ, z2 ðtÞ ¼ yðtÞ, z3 ðtÞ ¼ θðtÞ, z4 ðtÞ ¼ x_ ðtÞ, z5 ðtÞ ¼ y_ ðtÞ, z6 ðtÞ ¼ θðtÞ, two auxiliary variables z7 ðtÞ ¼ sin θðtÞ and z8 ðtÞ ¼ cos θðtÞ are introduced and the following additional constraint imposed: z27 ðtÞ þ z28 ðtÞ ¼ 1

ð62Þ

0.4 0.35 [m/sec]

||V(t) ||

0.3 0.25 0.15

0.2 0.1 0.05 0

0

20

40

60

80

100

120

Time [sec]

1

τ (t) [Nm]

2

τ (t) [Nm]

Fig. 4. Linear velocity. 0,5 0.25 0 −0.25 −0,5 0,5 0.25 0 −0.25 −0,5

0

20

40

60

Time [sec]

80

120

100

Fig. 5. Applied torques.

180 T T T T

160 140

curr(t)

120

T

100

T

80 60 40 20 0

0

20

40

56 60

69 73 80

100

120

Time [sec]

Fig. 6. Set-membership signal. Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

21

Moreover, the nonholonomic constraint (17) translates into z4 ðtÞz8 ðtÞ  z5 ðtÞz7 ðtÞ ¼ 0

ð63Þ

Then, the extended state space description has been discretized by using Euler forward differences with the sampling time T¼ 0.5 s. Moreover, the following constraints are prescribed: jτ1 ðtÞj r 0:45 ½N m;

8t Z 0;

ð64Þ

jτ2 ðtÞj r 0:45 ½N m;

8t Z 0;

ð65Þ

J vðtÞJ 2 r 0:16 ½m=s;

8 t Z 0;

ð66Þ

where J vðtÞJ 2 ¼ z24 ðtÞ þ z25 ðtÞ Then, inner approximations of one-step controllable set sequences have been off-line derived:








167  150 T 1k k ¼ 1 and T 2k k ¼ 1 n o n o n o n o Ob1 114 Ob1 138 Ob2 137 Ob2 138 Tk 1 , Tk 2 , Tk 1 and T k 2 k¼1 k¼1 k¼1 k¼1 n o n o n o n o SW 11 47 SW 12 22 SW 21 36 SW 22 30 Tk , Tk , Tk and T k k¼1

k¼1

k¼1

k¼1

All the relevant results are summarized in Figs. 3–6. In Fig. 3 the robot trajectory under timevarying obstacles is depicted. It can be noted that the SOS-OA-MPC strategy is capable to achieve good control performance regardless of any obstacle change. Moreover as it results from Figs. 4 and 5, the prescribed constraints are always fulfilled. To appreciate the modus operandi of the proposed scheme, it is important to analyze the signal shown in Fig. 6, because it provides the smaller ellipsoid of the pre-computed families containing the current state zðtÞ. Note that the markers (□ and ■) and (○ and
) have been used to denote the set-membership to the same sequences fT 1k g and fT 2k g, respectively. The latter is instrumental to put in light that if the current obstacle scenario sc(t) is different from that identified by the SOS-OA-MPC algorithm (namely Steps 1–9), the robot trajectory proceeds along the sequence of the admissible scenario used at the previous time instant (Step 2: curr≔prec). Nonetheless, this does not comprise neither the constraints satisfaction nor the feasibility retention thanking to the one-step evolution bound (50). Let us consider the time interval ½56; 73 (the grey zone of Fig. 6) where the switching O1 -O2 occurs. At t ¼ 56 s the obstacle configuration changes while the current state is such that  150   137   138   36   30  SW 2 SW 2 Ob2 Ob2 zð56Þ= 2 ⋃ T 2k [ ⋃ T k 1 [ ⋃ T k 2 [ ⋃ T k 1 [ ⋃ T k 2 k¼1

k¼1

k¼1

k¼1

k¼1

therefore, as prescribed (Steps 1–3 of the On-line phase of the SOS-OA-MPC algorithm), the used scenario does not change (curr¼ 1), see the marker ■ in Fig. 6 and the membership to the red dashed ellipsoidal sequence in Fig. 3. Note that at 59 s, after the obstacle has been worked around, the robot trajectory direction quickly changes toward xf. This phenomenon fully complies with the nature of the ellipsoidal sequence T 1k which has been off-line built up to reach the target xf irrespective of obstacle occurrences. Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

22

Then at t ¼ 69 s. the correct scenario is recovered when the current state belongs to the SW 2

switching sequence (the blue dotted line ellipsoidal sequence of Fig. 3): zð69ÞA T 10 1 . Finally, at t ¼ 73 s the switching is accomplished and the current state belongs to the blue continuous line ellipsoidal sequence of Fig. 3, i.e. zð73ÞA T 229 Notice that the latter is clearly shown in Fig. 3, where the robot trajectory modifies its direction because a different one-step 2 controllable family is used, i.e. the following switching occurs fT SW 1 g-fT 2 g. The same reasoning applies to the other obstacle scenario occurrence. 7. Conclusions In this paper we have presented a receding horizon strategy for solving the obstacle avoidance motion planning problem for autonomous vehicles described by polynomial systems subject to nonholonomic constraints. Set-theoretic ideas and semialgebraic arguments have been used to take care of all admissible time-varying obstacle scenarios. As one of its main merits, most of the computations are moved in the off-line phase so that the on-line computational load pertaining to the control action computation is modest and the RHC control unit becomes usable in real-time frameworks.

References [1] J. Latombe, in: Robot Motion Planning, Kluwer Academic Publishers, London, 1991. [2] L. Lapierre, R. Zapata, P. Lepinay, Combined path-following and obstacle avoidance control of a wheeled robot, Int. J. Robot. Res. 26 (2007) 361–375. [3] J. Minguez, F. Lamiraux, J.-P. Laumond, Motion planning and obstacle avoidance, in: B. Siciliano, O. Khatib (Eds.), Springer Handbook of Robotics, Springer, Berlin, Heidelberg, 2008, pp. 827–852. [4] G. Chesi, Visual servoing path planning via homogeneous forms and LMI optimizations, IEEE Trans. Robot. 25 (2) (2009) 281–291. [5] A.V. Savkin, C. Wang, Seeking a path through the crowd: robot navigation in unknown dynamic environments with moving obstacles based on an integrated environment representation, Robotics and Autonomous Systems, ISSN 0921- 8890, 2014. [6] A. De Luca, G. Oriolo, Kinematics and dynamics of multi-body systems. CISM Courses and Lectures, in: J. Angeles, A. Kecskemethy (Eds.), Modelling and Control of Nonholonomic Mechanical Systems, vol. 360, Springer, Berlin, 1995, pp. 277–342. [7] R. Fierro, F.L. Lewis, Robust practical point stabilization of a nonholonomic robot using neural networks, J. Intell. Robot. Syst. 20 (1997) 295–317. [8] R.W. Brockett, Asymptotic stability and feedback stabilization, in: Differential Geometric Control Theory, Birkhäuser, Boston, 1983, pp. 181–191. [9] W. Ren, S.J. Sun, R.W. Beard, T.W. Mclain, Nonlinear tracking control for nonholonomic mobile robots with input constraints: an experimental study, in: American Control Conference, Portland, OR, USA, 2005, pp. 4923-4928. [10] D. Gu, H. Hu, Receding horizon tracking control of wheeled mobile robots, IEEE Trans. Control Syst. Technol. 14 (4) (2006) 743–749. [11] Chung-Han Hsieh, Jing-Sin Liu, Nonlinear model predictive control for wheeled mobile robot in dynamic environment, in: IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Kaohsiung, Taiwan, 2012. [12] P. Ögren, N.E. Leonard, A Convergent dynamic window approach to obstacle avoidance, IEEE Trans. Robot. Autom. 21 (2) (2005) 188–195. [13] P. Falcone, F. Borrelli, J. Asgari, H.E. Tseng, D. Hrovat, Predictive active steering control for autonomous vehicle systems, IEEE Trans. Control Syst. Technol. 15 (3) (2007) 566–580. [14] Y. Yoon, J. Shin, H.J. Kim, Y. Park, S. Sastry, Model-predictive active steering and obstacle avoidance for autonomous ground vehicles, Control Eng. Pract. 17 (2009) 741–750. Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021

G. Franzè, W. Lucia / Journal of the Franklin Institute ] (]]]]) ]]]–]]]

23

[15] J. Huang, C. Wen, W. Wang, Z.-P. Jiang, Adaptive stabilization and tracking control of a nonholonomic mobile robot with input saturation and disturbance, Syst. Control Lett. 62 (3) (2013) 234–241. [16] J.M. Carson III, B. Açikmeşe, R.M. Murray, D.G. MacMartin, A robust model predictive control algorithm augmented with a reactive safety mode, Automatica 49 (2012) 1251–1260. [17] G. Chesi, LMI techniques for optimization over polynomials in control: a survey, IEEE Trans. Autom. Control 55 (11) (2010) 2500–2510. [18] G. Franzè, A nonlinear sum-of-squares model predictive control approach, IEEE Trans. Autom. Control 55 (6) (2010) 1466–1471. [19] G. Chesi, A. Garulli, A. Tesi, A. Vicino, Homogeneous Lyapunov functions for systems with structured uncertainties, Automatica 39 (6) (2003) 1027–1035. [20] M. Choi, T. Lam, B. Reznick, Sum of squares of real polynomials, in: Proceedings of Symposia in Pure Mathematics, vol. 34 (2), 1995, pp. 103–126. [21] P. Parrillo, Structured semidefinite programs and semialgebraic geometry methods in robustness and optimization (Ph.D. thesis), California Institute of Technology, CA, USA, 2000. [22] A. Kurzhanski, I. Valyi, in: Ellipsoidal Calculus for Estimation and Control, Birkhäuser, Berlin, 1997. [23] F. Blanchini, S. Miani, in: Set-Theoretic Methods in Control, Birkäuser, Boston, 1997. [24] W. Tan, A. Packard, Stability region analysis using polynomial and composite polynomial Lyapunov functions and sum-of-squares programming, IEEE Trans. Autom. Control 53 (2008) 565–571. [25] R. Fierro, F.L. Lewis, Control of nonholonomic mobile robot: backstepping kinematics into dynamics, in: IEEE Conference on Decision and Control, New Orleans, LA, 1995, pp. 3805–3810. [26] J.M. Eklund, J. Sprinkle, S.S. Sastry, Switched and symmetric pursuit/evasion games using online model predictive control with application to autonomous aircraft, IEEE Trans. Control Syst. Technol. 20 (2011) 604–620. [27] G. Oriolo, A. de Luca, M. Vendittelli, WMR control via dynamic feedback linearization: design, implementation, and experimental validation, IEEE Trans. Control Syst. Technol. 10 (6) (2002) 835–852. [28] A. Papachristodoulou, S. Prajna, Analysis of non-polynomial systems using the sum of squares decomposition, positive polynomials in control, Lecture Notes in Control and Information Sciences, vol. 312, Springer,, Berlin, Heidelberg 23–43. [29] G. Chesi, Estimating the domain of attraction for non-polynomial systems via LMI optimizations, Automatica 45 (2009) 1536–1541. [30] T. Brogårdh, Present and future robot control development – an industrial perspective, Annu. Rev. Control 31 (2007) 69–79. [31] T. Fraichard, H. Asama, Inevitable collision states a step towards safer robot? in: IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, NV, USA, 2003, pp. 553–574. [32] G. Franzè, W. Lucìa, M. Muraca, An obstacle avoidance receding horizon control scheme for autonomous vehicles, in: IEEE American Control Conference, Washington, DC, USA, 2013. [33] G. Franzè, W. Lucia, The obstacle avoidance motion planning problem for autonomous vehicles: a low-demanding receding horizon control scheme, Syst. Control Lett. 77 (2015) 1–10. [34] J. Löfberg, A toolbox for modeling and optimization in MATLAB, in: Proceedings of the (CACSD) Conference, Taipei, Taiwan, 2004. [35] D. Henrion, J.B. Lasserre, Detecting global optimality and extracting solutions in gloptipoly, IEEE Control Syst. Mag. 24 (3) (2004) 72–83. [36] G. Franzè, A. Casavola, D. Famularo, E. Garone, An off-line MPC strategy for nonlinear systems based on SOS programming, Nonlinear Model Predictive Control (2009) 491–499. [37] C.W. Brown, QEPCAD quantifier elimination by partial cylindrical algebraic decomposition, 2003, Available from: 〈http://www.cs.usna.edu/qepcad/B/QEPCAD.html〉. [38] G. Chesi, Estimating the domain of attraction via union of continuous families of Lyapunov estimates, Syst. Control Lett. 56 (2007) 326–333. [39] A.A. Kurzhanskiy, P. Varaiya, Ellipsoidal toolbox (ET), in: 45st IEEE Conference on Decision and Control, San Diego, CA, USA, 2006, pp. 1498–1503.

Please cite this article as: G. Franzè, W. Lucia, An obstacle avoidance model predictive control scheme for mobile robots subject to nonholonomic constraints: A sum-of-squares approach, Journal of the Franklin Institute. (2015), http://dx.doi.org/10.1016/j.jfranklin.2015.03.021