Potential-field constructions in an MPC framework: application for safe navigation in a variable coastal environment

6th IFAC IFAC Conference Conference on on Nonlinear Nonlinear Model Model Predictive Predictive Control Control 6th 6th IFAC Conference on Nonlinear2018 Model Predictive Control Madison, Madison, WI, WI, USA, USA, August August 19-22, 19-22, 2018Available online at www.sciencedirect.com Madison, WI, USA, August 19-22, 2018 6th IFAC Conference on Nonlinear Model Predictive Control Madison, WI, USA, August 19-22, 2018

ScienceDirect

IFAC PapersOnLine 51-20 (2018) 307–312

Potential-field constructions in an MPC Potential-field Potential-field constructions constructions in in an an MPC MPC framework: application for safe navigation Potential-field constructions in an MPC framework: application for safe navigation framework: application for safe navigation framework: application for safe navigation in in a a variable variable coastal coastal environment environment in a variable coastal environment ∗ ∗ ∗∗ ∗ ∗ ∗ ∗∗ ∗

N.Q.H. Tran ∗ I. Prodan ∗ E. I. Grøtli L. Lef` e vre ∗ N.Q.H. e ∗∗ L. N.Q.H. Tran Tran ∗ I. I. Prodan Prodan ∗ E. E. I. I. Grøtli Grøtli ∗∗ L. Lef` Lef` evre vre ∗ ∗ ∗ ∗∗ N.Q.H. Tran I. Prodan E. I. Grøtli L. Lef` e vre ∗ ∗ ∗ Univ. Grenoble Alpes, Grenoble INP, LCIS, F-26000, Valence, Univ. Grenoble Alpes, Grenoble INP, LCIS, F-26000, Valence, ∗ ∗ Univ. Grenoble Alpes, Grenoble INP, LCIS, F-26000, Valence, France (ngo-quoc-huy.tran, ionela.prodan, France (ngo-quoc-huy.tran, ∗ Univ. Grenoble Alpes, Grenoble INP, ionela.prodan, LCIS, F-26000, Valence, France (ngo-quoc-huy.tran, ionela.prodan, [email protected]). [email protected]). France (ngo-quoc-huy.tran, ionela.prodan, ∗∗ [email protected]). ∗∗ Mathematics and Cybernetics, SINTEF Digital, Norway (Esten and SINTEF ∗∗ ∗∗ Mathematics [email protected]). and Cybernetics, Cybernetics, SINTEF Digital, Digital, Norway Norway (Esten (Esten [email protected]). [email protected]). ∗∗ Mathematics and Cybernetics, SINTEF Digital, Norway (Esten [email protected]). [email protected]). Abstract: This This work work presents presents an an NMPC NMPC (Nonlinear (Nonlinear Model Model Predictive Predictive Control)-based Control)-based algorithm algorithm Abstract: Abstract: This work an NMPC (Nonlinear Model Control)-based for safe safe navigation navigation of presents multi-agent dynamical systems in aa Predictive variable environment environment withalgorithm fixed or or for of multi-agent dynamical systems in variable with fixed Abstract: This work an NMPC (Nonlinear Model Control)-based algorithm for safe obstacles. navigation of presents multi-agent dynamical systems in a Predictive variable environment with fixed or moving The contribution lies in the construction of repulsive potential fields for the moving obstacles. The contribution lies in construction of potential fields for for navigation of multi-agent dynamical a variable environment with fixed or moving obstacles. The contribution liesintroduced in the thesystems construction of repulsive repulsive potential fieldsproblem for the the fixedsafe and moving obstacles obstacles which are are in aa in predictive control optimization fixed and moving which introduced in predictive control optimization problem moving obstacles. The contribution lies in the construction of repulsive potential fields for the fixed and moving obstacles which are introduced in a predictive control optimization problem to penalize penalize collision. collision. The The constraints constraints are are activated activated only only in in the view view range range of of the the agent agent using using to fixed and moving obstacles which aremotion introduced in only a of predictive control optimization problem to penalize collision. The constraints are activated in the the range of theharbor agent using on-off barrier functions. Collision-free planning ships in view Trondheim fjord serves on-off barrier functions. Collision-free motion planning of ships in Trondheim fjord harbor serves to The constraints are activated only inare the view range theharbor agent using on-off barriercollision. functions. Collision-free motion planning of ships insimulated Trondheim fjord serves as aapenalize benchmark for testing testing our algorithm. algorithm. Some scenarios in of order to prove the as benchmark for our Some scenarios are simulated in order to prove the on-off barrier functions. Collision-free motion planning of ships in Trondheim fjord harbor serves as a benchmark for testing our algorithm. Some scenarios are simulated in order to prove the efficiency of the algorithm and its feasibility to run in real-time. efficiency of the algorithm feasibilitySome to run in real-time. as a benchmark for testingand ourits algorithm. scenarios are simulated in order to prove the efficiency of the algorithm and its feasibility to run in real-time. efficiency of the algorithmFederation and its feasibility to run in real-time. © 2018, IFAC (International of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Fixed/Moving Fixed/Moving obstacles obstacles avoidance, avoidance, NMPC NMPC (Nonlinear (Nonlinear Model Model Predictive Control), Control), Keywords: Keywords: Fixed/Moving obstacles avoidance, NMPC ASV (Nonlinear Model Predictive Predictive Control), Potential field constructions, On-off barrier functions, (Autonomous surface vehicles). Potential constructions, On-off barrier (Autonomous surface Keywords:field Fixed/Moving obstacles NMPC ASV (Nonlinear Model Predictive Control), Potential field constructions, On-offavoidance, barrier functions, functions, ASV (Autonomous surface vehicles). vehicles). Potential field constructions, On-off barrier functions, ASV (Autonomous surface vehicles). agent cannot cannot enter enter (Song (Song et et al., al., 2017). 2017). Various Various works works using using 1 INTRODUCTION INTRODUCTION agent agent (Song et al., 2017). Various works 11 INTRODUCTION these cannot types of ofenter algorithms can be enumerated: enumerated: someusing use these types algorithms can be some use agent cannot (Song etcan al., 2017). Various works using 1 INTRODUCTION these types ofenter algorithms be enumerated: some use collision cone concept (Chakravarthy and Ghose, 2012), Autonomous systems, often included under the paradigm collision coneof concept concept (Chakravarthy and Ghose, Ghose, 2012), 2012), Autonomous systems, systems, often often included included under under the paradigm paradigm collision these types algorithms enumerated: use cone and Autonomous velocity obstacles (Lee(Chakravarthy et can al., be 2017), artificial some potential of multi-agent multi-agent dynamical dynamical systems, systems, are are proving provingthe to be be worthworth- velocity obstacles (Lee et al., 2017), artificial potential of to collision cone concept (Chakravarthy and Ghose, 2012), velocity obstacles (Lee et al., 2017), artificial potential Autonomous systems, often included under the paradigm of multi-agent dynamical systems, are proving to be worthfield (Tran et al., 2017), evolutionary method (Shin et al., while in in places places where where man man cannot cannot reach reach or or is is unable unable field (Tran et al., 2017), evolutionary method (Shin et al., while obstacles (Lee et al., using 2017),method artificial potential field (Tran et al., 2017), evolutionary (Shin et al., of multi-agent dynamical systems, are proving to In be particworth- velocity while in places where man cannot reach or is unable 2017), MPC-based approaches nonlinear programto perform in a timely and efficient manner. 2017), MPC-based approaches using method nonlinear programto perform perform in aa where timely man and efficient efficient manner. In particfield et al., 2017), evolutionary (Shin et al., MPC-based approaches using nonlinear programwhile in autonomous places cannot or In is particunable to in timely and manner. ming(Tran (Breivik et al., al., 2017). ular, for for marine vehiclesreach it is is important important to 2017), ming (Breivik et 2017). ular, autonomous marine vehicles it to 2017), MPC-based approaches using nonlinear programming (Breivik et al., 2017). to perform in a timely and efficient manner. In particular, for autonomous marine vehicles it is important to generate collision-free collision-free paths paths with with fixed fixed obstacles obstacles (e.g., (e.g., isis- This paper proposes a collision avoidance algorithm via a generate (Breivik et al., a2017). ular, autonomous marine vehicles is important to ming This paper paper proposes collision avoidance avoidance algorithm a generate collision-free paths with fixed (e.g., islands,for shorelines or ships ships anchoring) oritobstacles moving obstacles This proposes a between collision algorithm via via lands, shorelines or anchoring) or moving obstacles coherent combination potential field-based field-based con-a generate collision-free paths with fixed obstacles (e.g., iscoherent combination between potential conlands, shorelines orare ships anchoring) orcollision moving avoidance obstacles This (e.g., another ships operating). The paper proposes a between collision avoidance algorithm via coherent combination potential field-based con-a (e.g., another another ships ships are are operating). Theorcollision collision avoidance structions and NMPC. It is is applied applied over a benchmark benchmark lands, ships anchoring) moving obstacles structions and NMPC. It over (e.g., operating). The avoidance actionsshorelines for both both or manned and unmanned unmanned surface vessels coherent combination between potential field-based constructions and NMPC. It is the applied over aa benchmark actions for manned and surface vessels using real numerical data for safe navigation of ships (e.g., another ships are operating). Thebeen collision avoidance using real real numerical numerical dataItfor for the safe navigation navigation of ships ships actions for both manned and unmanned surface vessels must respect the rules which have given by the structions and NMPC. is applied over a benchmark using data the safe of must respect respect themanned rules which which have been been surface given by by the in in Trondheim Trondheim fjord fjord complying complying to to COLREGS COLREGS rules. rules. More More actions for both andPreventing unmanned vessels must the rules have given the International Regulations for Collisions at Sea Sea using real numerical data for of the safe navigation of More ships in Trondheim to COLREGS rules. International Regulations for Preventing Collisions at fjord complying specifically the contributions this paper are: must respect the rules which have been given by the specifically thefjord contributions ofto this paper are: are:rules. More International for 1999). Preventing Collisions at Sea in (COLREGS) Regulations (Commandant, 1999). Trondheim complyingof COLREGS specifically the contributions this paper (COLREGS) (Commandant, International for 1999). Preventing Collisions at Sea specifically (COLREGS) Regulations (Commandant, (1) construction construction of on-off barrier functions which activate the contributions of this paper are: of on-off on-off barrier barrier functions which activate activate (1) From aa control control point of of view, view,1999). we may may classify classify the the methmeth(COLREGS) (Commandant, (1) construction of functions which From point we the associated repulsive potential for fixed and movthe associated repulsive potential for fixed and movFrom a control point ofcollision view, weavoidance may classify the methods which tackle the problem with (1) the construction of repulsive on-off barrier functions whichand activate associated potential for fixed movods which which tackle the ofcollision collision avoidance problem with ing obstacles; obstacles; From a control point view, we may classify the mething ods tackle the avoidance problem with fixed/moving obstacles obstacles in in explicit explicit and and implicit implicit methods. methods. associatedofrepulsive potential foralgorithm fixed andwhich moving obstacles; fixed/moving (2) the development an NMPC-based NMPC-based ods which tackle themethods collision avoidance problem with (2) development of an an algorithm which which fixed/moving obstacles in explicit andtoimplicit methods. Usually the explicit reduce the use of MIP ing obstacles; (2) development of NMPC-based algorithm of Usually the the explicit explicit methods reduce toimplicit the use usemethods. activates the the constraints constraints in in the the view view range range of of the the fixed/moving obstacles in explicit andthe activates Usually methods reduce to the of MIP MIP (Mixed-Integer Programming) where constraints are (2) activates development an NMPC-based algorithm which the of constraints in the view range of the (Mixed-Integer Programming) where the constraints are agent; Usually the explicit methods reduce to the use of MIP agent; (Mixed-Integer where thethe constraints are explicitly taken taken Programming) into account account but but with cost of of high high constraints the view range simulaof the agent; explicitly into with the cost (3) activates validation the of the the proposed in algorithm through (Mixed-Integer Programming) where thethe constraints are (3) validation of proposed algorithm through simulaexplicitly taken into account but with cost of when high computation time (Prodan et al., 2015). Moreover, agent; (3) validation of thebenchmark proposed algorithm through simulacomputation time (Prodan et al., 2015). Moreover, when tions over a real of safe navigation of ships explicitly account but 2015). with nonlinear the cost of high tions over over aaofreal real benchmark of safe safe navigation navigation of ships computation timeinto (Prodan etdynamics, al., Moreover, when using the the taken nonlinear vehicle mixed(3) validation thebenchmark proposed algorithm through of simulations of ships using nonlinear vehicle dynamics, nonlinear mixedin the Trondheim fjord. computation time (Prodan al., 2015). Moreover, when in the theover Trondheim fjord. using nonlinear vehicleetdynamics, mixedintegerthe optimization programs need to be benonlinear solved (Abichantions a real benchmark of safe navigation of ships in Trondheim fjord. integer optimization programs need to solved (Abichanusing the nonlinear vehicle dynamics, mixedinteger optimization programs need to benonlinear solved (AbichanThe paper is organized as follows. follows. Section Section 22 formulates formulates dani et al., 2015). The implicit methods usually reduce to in the Trondheim fjord. The paper is organized as dani et al., 2015). The implicit methods usually reduce to integer programs need tofield-based be usually solved (Abichanpaper is and organized as follows. Sectionpotential 2 formulates dani et optimization al., 2015). The implicit methods reduce to The the problem introduces the repulsive conthe use of various types of potential constructhe problem problem and introduces the repulsive repulsive potential conthe use use al., of various various typesimplicit of potential potential field-based construcpaper Section is and organized as follows. Sectionpotential 2 formulates dani 2015). The methods usually reduce to The the introduces the conthe of types of field-based construcstructions. 3 introduces introduces the construction construction of on-off on-off tions et which prove to to be efficient efficient in many many real applications applications of structions. Section 3 the tions which prove be in real problem and in introduces thethe repulsive potential conthe use of various types of Furthermore, potential field-based construc- the structions. Section 3 conjunction introduces construction of on-off tions which prove to be efficient in many realthe applications barrier functions with repulsive potential. (Rasekhipour et al., al., 2017). for particular barrier functions in3 conjunction conjunction with repulsive potential. (Rasekhipour et 2017). Furthermore, for the particular structions. Section introduces the construction of on-off tions which prove to be efficient in many real applications barrier functions in with repulsive potential. (Rasekhipour et al., 2017). Furthermore, for the particular After that, that, all all the the elements elements presented presented are are gathered gathered into into a case of of motion motion planning planning with with collision collision avoidance avoidance at at sea sea After case barrier functions inelements conjunction with repulsive potential. (Rasekhipour al., 2017). Furthermore, for the particular After that, all the presented are gathered into aa case of methods motionet planning with collision avoidance at sea nonlinear model predictive control framework for motion several are employed: i) use sensor data providnonlinear model predictive control framework for motion motion several methods are employed: employed: i) use use sensor sensor data providprovidthat,with all the elements presented are gathered into a case of methods motion planning with avoidance at sea After nonlinear model predictive control framework several are i) data planning collision avoidance. Section 4 for shows the ing sub-optimal sub-optimal solutions with collision low computation computation demand planning with collision avoidance. Section 4 shows the ing solutions with low demand nonlinear model predictive control framework for motion several methods are employed: i) use sensor data providplanning with collision avoidance. Section 4 shows the ing sub-optimal solutions with low computation demand simulation results over a real benchmark. Section 5 draws (Dunbabin and Lament, 2017) or ii) build a 2D (twosimulationwith results over aa avoidance. real benchmark. benchmark. Section draws (Dunbabin and solutions Lament, 2017) 2017) low or computation ii) build build aa 2D 2D (two- planning collision Section 4 shows the ing sub-optimal demand simulation results over real Section 55 draws (Dunbabin and Lament, or ii) (twothe conclusions conclusions and presents the future future work. dimentional) obstacle map with from a priori information, each the and presents the work. a priori information, each dimentional) obstacle map from simulation results over a real benchmark. Section 5 draws (Dunbabin and Lament, 2017) or ii) build a 2D (twothe conclusions and presents the future work. dimentional) obstacle map from a priori information, each obstacle being being represented represented by by a cell-location cell-location in in which which the the obstacle dimentional) map from priori information, each obstacle beingobstacle represented by aa acell-location in which the the conclusions and presents the future work. obstacle by a cell-location in whichControl) the Hosting by Elsevier Ltd. All rights reserved. 2405-8963 being © 2018,represented IFAC (International Federation of Automatic

Copyright 2018 343 Copyright ©under 2018 IFAC IFAC 343Control. Peer review© of International Federation of Automatic Copyright © 2018 responsibility IFAC 343 10.1016/j.ifacol.2018.11.049 Copyright © 2018 IFAC 343

2018 IFAC NMPC 308 Madison, WI, USA, August 19-22, 2018

2

N.Q.H. Tran et al. / IFAC PapersOnLine 51-20 (2018) 307–312

PRELIMINARIES

This paper is inspired by the practical application of collision-free motion planning of unmanned surface vessels in the Trondheim fjord harbor, Norway. Fig. 1 illustrates the operating region of the ships which need to navigate from three different harbors (Orkanger, Stjørdal and Skogn) to Trondheim harbor while avoiding the shore or small islands and other ships (moving obstacles). The COLREGS rules 1 we consider hereafter are: • Rule 8 - Action to avoid collision: avoidance action must be applied timely, before other vessel approaches. Any alterations of course and/or speed must be large enough to clear the approaching vessels. • Rule 14 - Head on situation: when two power-driven vessels are meeting on nearly reciprocal courses so as to involve risk of collision, then alter course to starboard so that each pass on the port side of each other. • Rule 15 - Crossing situation: when two power-driven vessels are crossing so as to involve risk of collision, the vessel which has the other on her own starboard side shall keep out of the way. • Rule 16 - Actions by give-way vessel: take early and substantial action to keep well clear. The Automatic Identification Systems (AIS) provided real numerical data related to position, velocity of ships, time of navigation between harbors. In order to efficiently describe the non-convex feasible region for the dynamical (mobile) agent, we briefly recall here the system dynamics and various notions which involve polyhedral/ellipsoidal sets and repulsive potential constructions.

agent. A polytope is a bounded polyhedron and has a dual representation in terms of intersection of half-spaces or convex hull of extreme points: P = {x ∈ Rn : Sx ≤ K} =   {x ∈ Rn : x = αi vi , αi = 1, αi ≥ 0}. An ellipsoid is denoted by E(c, W, r) = {x ∈ Rn : (x − c)T W (x − c) ≤ r2 }, with W = W T  0 and r > 0. 2.1

Model Description

A classical model of an ASV (Pavlov et al., 2009) is used:  η˙ = R(ψ)ν, (1) M ν˙ + C(ν)ν + Dν = Bu. 

where the state vector, x ∈ R6 , with x = [η ν] includes  the vector η ∈ R3 , with η = [x y ψ] containing the system position and the yaw angle, ψ, in the inertial  frame. The vector, ν ∈ R3 , with ν = [u v r] is surge, sway and yaw rate. The input vector, u ∈ R2 , with  u = [τu τr ] is the control input corresponding to surge thrust and rudder deflection. Also, in (1) B ∈ R3×2 is the actuator configuration matrix, R(ψ), M , C(ν) and D ∈ R3×3 are the rotation, mass, Coriolis and damping matrices, respectively. 2.2

Repulsive potential for the fixed obstacles

Let us define the collection of forbidden cells (as also illustrated in Fig. 1) as a union of bounded polyhedral given by their half-space description: N cell Pi . (2) P= i=1

where Pi = {x ∈ Rn : ak x ≤ bk , k = 1, ..., nh }, ak ∈ R1×n , bk ∈ R, nh and Ncell are the number of half-spaces and forbidden cells respectively.

Next, for the bounded convex set Pi we define the piecewise linear function (Camacho and Bordons, 2004): nh  θ(x) = (ak (x−xs )−bk +ak xs + | ak (x−xs )−bk +ak xs |) k=1

Fig. 1. Map of Trodheim obtained from real data: feasible space and considered forbidden cells as in (2). The following notation will be used throughout the paper. For a vector x ∈ Rn and a positive definite√matrix P ∈ Rn×n , ||x||P denotes the weighted norm xT P x, ||x|| the Euclidean norm and |x| the absolute value of the vector x or an array x. We denote by ∆pi , i = 1...Ncell , the Euclidean distance calculated from Chebyshev center, the largest inscribed ball of ith polytope and current position of an Autonomous Surface Vehicle (ASV). Similarly, ∆pj , j = 1...Nmov is the Euclidean distance calculated from current position of an agent and j th moving obstacle. The safe distance between the agent and a forbidden cell i is denoted by Dsi . Also, ρview denotes the view range of the 1

http://astat.autonomous-ship.org/

344

(3) with xs ∈ Rn the analytic center 2 of the polytope Pi . The function (3) is zero in the interior of the convex set Pi (2) and a non-zero value which grows linearly as the distance from convex region increases. θ(x) will be used for the construction of repulsive potential which takes into account the shape of the fixed obstacles (2). Considering (3) we define the collection of repulsive potentials of the forbidden cells in (4) as: V=

N cell

Vifix (θi (xi )).

(4)

i=1

with xi ∈ Pi , i = 1...Ncell and Vifix (θi (xi )) given as:  c1 (c2 +θi (xi ))2 , if xi ∈ Pi , fix Vi (θi (xi )) = 0, if xi ∈ / Pi .

(5)

where c1 and c2 are positive parameters representing the strength and effect ranges of repulsive potential. 2 The function (3) is described in general form, in case that the origin does not belong to the strict interior of the polytope.

2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018

N.Q.H. Tran et al. / IFAC PapersOnLine 51-20 (2018) 307–312

309

F(x)

1 β=1 β=2 β=4

0.5

0

0

50

100

150

200

x

Fig. 3. On-off barrier function as in (8).

(a) For fixed obstacles (the ground).

(b) For the ASV.

Fig. 2. Repulsive potential field. Fig. 2a illustrates in 3D the total repulsive potential of shore which has high value inside the bounded convex sets and zero outside them. Note that the yellow zone in Fig. 2a denotes the overlap among the bounded polyhedra. The parameters in (5) used for illustration are c1 = 0.01 and c2 = 5.10−4 . 2.3

Repulsive potential for the agent

In here we consider Nmov moving obstacles which are represented by other agents defined by the dynamical system of an ASV given in (1). To avoid collision between the ASV and other moving obstacles we construct a repulsive potential for the ASV (which is updated at each step) in order to ensure that the j th moving obstacle cannot enter: λj V mov (Ej (x, y)) = . (6) (γj + Ej (x, y))2 where λj and γj represent the strength and effect range of ship and Ej (x, y) is an ellipsoid around the ship which are defined w.r.t. the j th moving obstacle: (xj − x)2 (yj − y)2 + . (7) Ej (x, y) = 2 rx ry2 with (xj , yj ) the current position of j th moving obstacle, (x, y) is the center of shifted ellipse but also the current position of the ASV and (rx , ry ) denote the ellipsoid axis and are chosen such that the shape of ASV is covered.

Gathering all the elements presented above we will concentrate next on the NMPC implementation for motion planning with collision avoidance. Particular attention is given to the formulation of the cost function through which we penalize the collision avoidance constraints with fixed and moving obstacles. 3

NMPC implementation

A typical approach when using potential field methods is to construct the total potential (attractive and repulsive) and add it into an optimization problem, possibly looking over a prediction horizon. However, this approach is computational cumbersome, especially for difficult applications when the number of fixed and moving obstacles is large. This may lead to infeasible solutions for the optimization problem usually due to local minima. Therefore, this section presents first the combined use of the above constructed repulsive potentials (5), (6) and the on-off barrier function (8) so that the terms of the objective function will be activated only when needed. Next, the NMPC optimization problem and the implementation algorithm are delineated. 3.1

On-off repulsive potential

The safe distance Dsi of ith cell w.r.t. the agent is calculated by: Dsi = eρi + ρview . (9) where e is an expansion coefficient, ρi is the radius of the circle defined from Chebyshev center of the ith forbidden cell, ρview is the view range of the ship. ·105 1.01

Shape of ship ith cell Shifted ellipse Ship’s view field Chebyshev circle expansion circle jth moving obstacle

1.01 1.01 1.01

y [m]

Fig. 2b shows the repulsive potential around a ship. The high value inside the ellipsoid prevents the moving obstacles from colliding with it.

⇢i

1.01 1.01 1.01

|| pi||

1.01

2.4

e⇢i

1.01

On-off barrier function

1

Let us define next an on-off barrier function which will be instrumental in the formulation of the forthcoming motion planning algorithm which activates the constraints only in the field of view of the agent. This function, also called a logistic function (Kyurkchiev and Markov, 2016) was first applied for the study of population growth. It is denoted by:    0, if x > xo , L  x−x  ≈ L2 , if x = xo , F (x) = (8) o   L, if x < xo . 1 + e β2

where xo is the x-value of the sigmoid’s midpoint, L is the curve’s maximum value and β, the steepness of the curve. Fig. 3 illustrates the on-off barrier function for L = 1, xo = 100 and varying β.

345

|| pj||

⇢view

1

6,650

6,700

6,750

6,800

6,850

6,900

6,950

7,000

7,050

7,100

7,150

7,200

7,250

7,300

7,350

7,400

7,450

7,500

x [m]

Fig. 4. Description of the safe distance of the ship w.r.t. the ith cell and j th moving obstacle. Given the set of repulsive potentials, V, as in (5) the agent will take into account the ith repulsive potential Vi (θi (xi )) if and only if the safe distance Dsi of ith cell w.r.t. the agent is greater than or equal to ∆pi . The on-off barrier function presented here for the ith cell as:    0, if L L Fi (xi ) =  ∆p −Di  ≈ 2 , if i s   2 β L, if 1+e

in (8) is redefined

∆pi  > Dsi , ∆pi  = Dsi , ∆pi  < Dsi . (10)

2018 IFAC NMPC 310 Madison, WI, USA, August 19-22, 2018

N.Q.H. Tran et al. / IFAC PapersOnLine 51-20 (2018) 307–312

Consequently, the on-off repulsive potential for the fixed obstacles is described as follows: N cell  Ωcell (xi ) = Fi (xi )Vifix (xi ), (11)

Lc (x, u) = Ωcell (xi ) + Ωmov (xj ) + x(t) − xref (t)2Q +u(t)2R . (17)

i=1

where Vifix (xi ) is given in (5), Fi (xi ) in (10) with L = 1. If safe distance Dsi between agent and ith repulsive potential is guaranteed, ith repulsive potential will be relaxed. If the agent approaches the safe distance Dsi , the value of ith repulsive potential will be increased to Vifix (xi ). Given the set of moving obstacles, M, the on-off repulsive potential of the agent w.r.t. j th moving obstacle will be taken into account if and only if the view range around this agent is entered by the j th moving obstacle. Similarly, the on-off barrier function presented in (8) is redefined here for the j th moving obstacle:    0, if ∆pj  > ρview , L  ∆pj −ρview  ≈ L2 , if ∆pj  = ρview , Fj (xj ) =   L, if ∆pj  < ρview . β2 1+e (12) Hence, the on-off repulsive potential of the agent w.r.t. the moving obstacles is described as follows: Ωmov (xj ) =

N mov

Fj (xj )Vjmov (xj ),

(13)

is given in (6) and Fj (xj ) in (12) with where L = 1. If the j moving obstacle is not in the agent’s view range ρview , the agent’s repulsive potential will be relaxed. If the j th moving obstacle is in the agent’s view range ρview , the agent’s repulsive potential will be activated. 3.2

NMPC optimization problem

Let us first write in a general form the continuous-time dynamical model of the ship given in detail in (1): x(t) ˙ = f (x(t), u(t)). (14) n n n with f : R × R , x ∈ R is the state vector and u ∈ Rm represents the control input. The motion planning optimal control problem is described by (see also Algorithm 1 which describes our proposed approach): min

T f

x(.),u(.) T

subject to:

Lc (t, x(t), u(t))dt + E(x(Tf ))

z,u k=0

s.t. (20); end if ∆pi  > Dsi and ∆pj  ≤ ρview then N −1 Jmov = J + (Vjmov (zk )) s.t. (20); k=0

end if ∆pi  ≤ Dsi and ∆pj  > ρview then N −1 Jfix = J + (Vifix (zk )) s.t. (20); k=0

j=1

Vjmov (xj ) th

Algorithm 1 NMPC implementation. Input: Consider model (1), the set of repulsive potentials of the forbidden cells (5) and of the ship (6), the safe distance (9) and the attractive potential (18). 1. Measure the current position z(k) of the ship and predict the position over the prediction horizon Tf . 2. All the information of position of forbidden cells and moving obstacles will be collected by the sensors if they are inside the ship’s view range. 3. Solve the NMPC problem: if ∆pi  > Dsi and ∆pj  > ρview then N −1 J = min (zk − zkref 2Q + uk 2R ) + EN (zN )

(15)

0

x(t0 ) = x0 ,

(16a)

x(t) ˙ = f (x(t), u(t)),

(16b)

umin ≤ u(t) ≤ umax ,

(16c)

(16d) xmin ≤ x(t) ≤ xmax , ∀t ∈ [T0 , Tf ]. n with x0 ∈ R the initial state of the agent, xmin , xmax ∈ Rn and umin , umax ∈ Rm . Bounds on the input variations can also be considered. The stage cost Lc (.) is a composition of multiple objectives with the following expression: 346

end if ∆pi  ≤ Dsi and ∆pj  ≤ ρview then N −1 fix Jmov =J+ (Vifix (zk ) + Vjmov (zk )) s.t. (20); k=0

end 4. Apply only the first component of the control sequence for system (14). 5. Continue to the next sampling instance, return step 1.

In (17) the term x(t) − xref (t)2Q represents the attractive potential which has the lowest value at the desired target, xref (in this particular application, the Trondheim harbor). This term also appears in the terminal cost of (15): E(x(Tf )) = (x(Tf ) − xref (Tf )2P ,

(18)

where Q, R, P are (semi-)positive definite weighting matrices of appropriate dimensions. Note that the continuous-time optimization problem (15) is non-convex and obtaining the global minimum is not trivial. Therefore, (15) will be discretized using the direct multiple shooting method (Bock and Plitt, 1984). The time horizon [T0 , Tf ] is partitioned into N finite subintervals [tk , tk+1 ], k = 0, ..., N − 1, where the state and the control input are defined as explicit optimization variables. Hence, (15) is rewritten in the following discrete-time optimization problem: min

N −1

z,u k=0

subject to:

(Lk (zk , uk )) + EN (zN )

(19)

2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018

N.Q.H. Tran et al. / IFAC PapersOnLine 51-20 (2018) 307–312

z0 = x 0 ,

311

(20a)

zk+1 = F(tk+1 ; tk , zk , uk ), umin ≤ uk ≤ umax , xmin ≤ zk ≤ xmax ,

k = 0, ..., N − 1, k = 0, ..., N − 1, k = 0, ..., N.

(20b) (20c) (20d)

The function F(.) as in (20b) is discretized by using the explicit Runge-Kutta integrator of order four (RK4), it shows that step tk+1 depends on the previous step tk . The prediction horizon, Tf , is chosen by multiplying the sampling time Ts and the number of shooting nodes N (Tf = Ts N ). A control sequence, uk (k = 0, ..., N − 1), over the prediction horizon Tf is obtained by solving (19). Only the first control input over the Ts time-step is applied to the system (14). This optimization problem is implemented by using the interior-point method with Hessian approximation and barrier strategy (ensures a progress towards a local minimum quickly) as in (Zanelli et al., 2017). In here we use FORCES PRO to solve the NMPC optimization problem generating high performance C code (used for real-time implementation) and Matlab executables (used for Matlab simulation). 4

SIMULATION RESULTS

This section continues the application example started at the beginning of the paper. Here we combine all the earlier elements and prove the effectiveness of the proposed algorithm for the safe navigation of a ship in the Trondheim fjord. We use the ASV model (1) with the value of parameters taken from (Pavlov et al., 2009). For simplicity, the Coriolis matrix, wind and wave forces are neglected. We consider constraints on the actuation force τu ∈ [−2, 2] [N], on the yaw moment τr ∈ [−1.5, 1.5] [Nm], the surge velocity u ∈ [−0.5, 0.5] [m/s], the sway velocity v ∈ [−0.1, 0.1] [m/s] and yaw rate r ∈ [−0.2, 0.2] [rad/s].

(a) From Skogn to Trondheim. (b) From Orkanger to Trondheim.

Fig. 5. Comparison of trajectories between Scenario 1&2. For Scenarios 1 and 2 we consider only fixed obstacles represented by the cells given in Fig. 1. For Scenario 3 moving obstacles are also considered. Fig. 5a illustrates the actual motion of the agent under Scenario 1 (blue dashed line) which gets stuck in a local minima because that is a critical area surrounded by many repulsive potentials and their interaction creates a nullpotential field. This is the main drawback of the classical approach. This is not the case in Scenario 2 (red dashed line) which is also represented in Fig. 5a and it can be observed that the ship converges towards the Trondheim harbor without getting stuck in a local minima, this is due to the fact that the number of non-convex constraints (coming from the repulsive potential) are reduced since they are not in the view range of the agent. Notice that the circle around the ship represents its view range.

The parameters used in (9) are: the ship’s view range ρview = 20 [m] and the expansion coefficient e ∈ [1.2, 1.5].

The steepness of the curve used as in (10) and (12) is β = 1.2. Other parameters of the NMPC optimization problem in (19) are: the prediction horizon is Tf = 20s over N = 21 shooting nodes, the simulation time is 1300s, weighting matrices are Q = 0.1I6 , R = 0.1I2 , P = [0.5I2 02 02 ; 02 I2 02 ; 02 02 I2 ]. Finally, the number of cells considered from partitioning the map as in Fig. 1 is Ncell = 22. Simulations are realized by using FORCE PRO via Matlab R2016a on a computer with following configuration: Intel Core i7-4790CPU, 3.60GHz, 8GB RAM. The travel from Orkanger and Skogn to Trondheim harbor while avoiding collision with static and moving obstacles is considered under the following scenarios: • Scenario 1: use the classical approach where the total potential field appearing in the MPC’s cost is active for the entire simulation; • Scenario 2: use Algorithm 1 where the constraints are activated when inside the view range of the agent; • Scenario 3: use Algorithm 1 under the COLGRES rules 8 and 16 given in Section 2. 347

Fig. 6. Ship’s collision avoidance complies COLREGS rules 8 and 16 in Scenario 3. Fig. 5b presents a comparison between the agent’s trajectory under Scenario 1 (blue dashed line) and Scenario 2 (red dashed line). Although the classical approach converges towards Trondheim harbour, evaluation criteria such as smoothness, length of trajectory, number of steps or computation time show that the proposed algorithm is superior to the classical approach. Table 1 gives some details for the two approaches. Table 1. Fig. 5b

Scenario 1

Scenario 2

Number of steps CPU time [s]/step Length of trajectory [m]

1163 0.0297 562.3

1081 0.0279 512

Fig. 6 shows the effectiveness of algorithms proposed. The trajectory generated following Scenario 3 can avoid both fixed and three moving obstacles (other ships). Notice that moving obstacles are represented in white colour because they do not belong to the ship’s view range. When

2018 IFAC NMPC 312 Madison, WI, USA, August 19-22, 2018

N.Q.H. Tran et al. / IFAC PapersOnLine 51-20 (2018) 307–312

moving obstacles are inside the ship’s view range, they are presented by green colour and the repulsive potential of the ship is activated in order to implement collision avoidance. Note that in Fig. 5a, Fig. 5b and Fig. 6, we also illustrate the repulsive and attractive potentials projected in 2D (the repulsive potentials appear around the forbidden region preventing ship’s collison with the fixed and moving obstacles and the attractive potential is represented by circles around the destination harbor).

Relative distance [m]

Fig. 7 shows the relative distances between the ship and the three moving obstacles in Scenario 3. The relative distances between them depend on strength (λ) and effect range (γ) of the ship’s repulsive potential as in (6). In this case, we assume that their safe thresholds are not less than 10 [m], λ and γ are 1000 and 1, respectively. 25

safe threshold avoidance threshold ship&moving obst1 (Scenario 3) ship&moving obst2 (Scenario 3) ship&moving obst3 (Scenario 3)

20

15

10

5

0 50

100

150

200

250

300

350

400

450

500

550

600

650

700

750

800

850

900

950 1,000 1,050 1,100 1,150 1,200 1,250 1,300

Time [s]

Fig. 7. Relative distances between ship and moving obstalces in Scenario 3 (Fig. 6). [rad/s]

0.4

0.2

0

Scenario 2 Scenario 3 lower and upper bounds

0.2

0

50

100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 900 950 1,000 1,050 1,100 1,150 1,200 1,250 1,300

Time [s]

⌧u [N]

Fig. 8. Yaw rate in Scenario 2 and 3 (Fig. 5b and 6). 2 0 2

0

50

100

150

200

250

300

350

400

450

500

550

600

650

700

750

800

850

900

950 1,000 1,050 1,100 1,150 1,200 1,250 1,300

⌧ r [Nm]

Scenario 2 Scenario 3 upper and lower bounds 1 0 1

0

50

100

150

200

250

300

350

400

450

500

550

600

650

700

750

800

850

900

950 1,000 1,050 1,100 1,150 1,200 1,250 1,300

Time [s]

Fig. 9. Control inputs under Scenario 2 and 3 (of the trajectories presented in Fig. 5b and 6). Observe in Fig. 8 and Fig. 9, that in the range of 700 to 900 seconds, yaw rate and control input of the proposed algorithm are changed in order to avoid the three moving obstacles that enter the ship’s view range obeying rules 8 and 16 of COLREGS. 5

CONCLUSION

This paper proposed an NMPC algorithm for safe navigation of multi-agent systems in an environment with fixed and moving obstacles. The focus was on a real application of motion planning for ships in the Trondheim fjord. Classical non-convex collision avoidance constraints were added in the cost function of an NMPC problem as penalty terms using repulsive potential constructions. These terms were activated only in the view field of the agent. Proof of concept, illustrations, simulations and comparison results over a real benchmark prove the efficiency of the proposed algorithm. Future work will consider other COLREGS rules for this particular application and distributed/decentralized control for multi-agents system. Of further interest is the efficient description of the forbidden areas such that the complexity is reduced and the description is close enough. 348

Acknowledgements This work was partly funded by the Research Council of Norway under grant no. 269557. References Abichandani, P., Torabi, S., Basu, S., and Benson, H. (2015). Mixed integer nonlinear programming framework for fixed path coordination of multiple underwater vehicles under acoustic communication constraints. IEEE Journal of Oceanic Engineering, 40(4), 864–873. Bock, H.G. and Plitt, K.J. (1984). A multiple shooting algorithm for direct solution of optimal control problems. IFAC Proceedings Volumes, 17(2), 1603–1608. Breivik, M. et al. (2017). Mpc-based mid-level collision avoidance for asvs using nonlinear programming. In the IEEE Conf. on Control Technol. and Apps., 766–772. Camacho, E.F. and Bordons, C. (2004). Model predictive control. advanced textbooks in control and signal processing. Springer-Verlag, London. Chakravarthy, A. and Ghose, D. (2012). Generalization of the collision cone approach for motion safety in 3-d environments. Autonomous Robots, 32(3), 243–266. Commandant, U. (1999). International regulations for prevention of collisions at sea, 1972 (72 colregs). US Department of Transportation, US Coast Guard, 16672. Dunbabin, M. and Lament, R. (2017). Adaptive receding horizon control for a high-speed autonomous surface vehicle in narrow waterways. In the IEEE Conference on Control Technology and Applications, 235–240. Kyurkchiev, N. and Markov, S. (2016). Approximation of the cut function by some generic logistic functions and applications. Advances in Applied Sciences, 1(2), 24–29. Lee, B.H., Jeon, J.D., and Oh, J.H. (2017). Velocity obstacle based local collision avoidance for a holonomic elliptic robot. Autonomous Robots, 41(6), 1347–1363. Pavlov, A., Nordahl, H., and Breivik, M. (2009). Mpcbased optimal path following for underactuated vessels. IFAC Proceedings Volumes, 42(18), 340–345. Prodan, I., Stoican, F., Olaru, S., and Niculescu, S.I. (2015). Mixed-integer representations in control design: Mathematical foundations and applications. Springer. Rasekhipour, Y., Khajepour, A., Chen, S.K., and Litkouhi, B. (2017). A potential field-based model predictive pathplanning controller for autonomous road vehicles. IEEE Trans. on Intelligent Transp. Systems, 18(5), 1255–1267. Shin, J., Kwak, D.J., and Lee, Y.i. (2017). Adaptive pathfollowing control for an unmanned surface vessel using an identified dynamic model. IEEE/ASME Transactions on Mechatronics, 22(3), 1143–1153. Song, R., Liu, Y., and Bucknall, R. (2017). A multi-layered fast marching method for unmanned surface vehicle path planning in a time-variant maritime environment. Ocean Engineering, 129, 301–317. Tran, N.Q.H., Prodan, I., and Lef`evre, L. (2017). Nonlinear optimization for multi-agent motion planning in a multi-obstacle environment. In 21st IEEE Int. Conf. on System Theory, Control and Computing, 488–493. Zanelli, A., Domahidi, A., Jerez, J., and Morari, M. (2017). Forces nlp: an efficient implementation of interior-point methods for multistage nonlinear nonconvex programs. International Journal of Control, 1–17.