An Optimization Based Approach for Relative ...

Preprint Accepted for Publication in Journal of Intelligent and Robotic Systems Related media can be found here:https://www.youtube.com/watch?v=foR7bJAHiOA The final publication is available at Springer via http://dx.doi.org/10.1007/s10846-016-0408-2

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems Mohamed W. Mehrez · George K. I. Mann · Raymond G. Gosine

Received: date / Accepted: date

Categories (3), (4). Abstract In this paper, an optimization based method is used for relative localization and relative trajectory tracking control in Multi-Robot Systems (MRS’s). In this framework, one or more robots are located and commanded to follow time varying trajectories with respect to another (possibly moving) robot reference frame. Such systems are suitable for a considerable number of applications, e.g. patrolling missions, searching operations, perimeter surveillance, and area coverage. Here, the nonlinear and constrained motion and measurement models in an MRS are incorporated to achieve an accurate state estimation algorithm based on nonlinear Moving Horizon Estimation (MHE) and a tracking control method based on Nonlinear Model Predictive Control (NMPC). In order to fulfill the real-time requirements, a fast and efficient algorithm based on a Real Time Iteration (RTI) scheme and automatic Ccode generation, is adopted. Numerical simulations are conducted to: first, compare the performance of MHE against the traditional estimator used for relative localization, i.e. extended Kalman filter (EKF); second, evaluate the utilized relative localization and tracking Mohamed W. Mehrez · George K. I. Mann · Raymond G. Gosine Intelligent Systems Lab Faculty of Engineering and Applied Science Memorial University of Newfoundland St. Johns, NL, Canada A1B 3X9 Mohamed W. Mehrez E-mail: [email protected] George K. I. Mann E-mail: [email protected] Raymond G. Gosine E-mail: [email protected]

control algorithm when applied to a team of multiple robots; finally, laboratory experiments are performed, for real-time performance evaluation. The conducted simulations validated the adopted algorithm and the experiments demonstrated its practical applicability. Keywords Multi-robot systems · Relative localization · Relative trajectory tracking control · Fast MHE and NMPC · Code generation

List of Abbreviations and Symbols

MRS

Multi-Robot System.

RL

Relative Localization.

MHE

Moving Horizon Estimation.

EKF

Extended Kalman Filter.

PF UKF NMPC

Particle Filter. Unscented Kalman Filter. Nonlinear Model Predictive Control.

DOF

Degrees of Freedom.

LSQ

Least Squares.

RTI

Real-Time Iteration.

RMSE TCP/IP

Root Mean Square Error. Transfer Control Protocol/Internet Protocol.

NEES R R≥0

Normalized Estimation Error Squared. Denotes real numbers. Denotes non-negative real numbers, where R>0 = {R≥0 \ 0}

N

Denotes natural numbers.

2

Mohamed W. Mehrez et al.

N0

Denotes non-negative natural numbers, i.e. N0 := N ∪ {0}

2 kxkM

.

A short notation for xT Mx

M

Number of observed robots.

L

Number of observing robots.

δt

Sampling time.

(·)c

A subscript used for variables related to observed robots.

(·)p

A subscript used for variables related to observing robots.

x

Relative state vector.

u

Observed robot local speeds’ vector.

ξ

Observing robot local speeds’ vector.

y

Relative measurements vector.

f (·)

An analytic vector mapping.

h(·)

An output function.

ν

Additive Gaussian noise.

σ

Standard deviation.

nx

Number of states in x.

nuc

Number of speeds in u.

nup

Number of speeds in ξ.

ny ˜ (·)

Number of measurements in y.

ˆ (·)

An estimation of a variable.

A measurement for a variable.

r

A superscript used to denote references.

(·)a

A subscript denoting augmented vectors

(·)

and matrices. rc

An Observed robot radius.

rp

An Observing robot radius.

nmc

Number of Monte-Carlo simulations.

NE

Estimation horizon length.

JNE (·) A, V, W S NC JNC (·)

MHE cost function. MHE tuning matrices. Estimation error covariance matrix. Prediction horizon length. NMPC cost function.

P, Q, R

NMPC tuning matrices.

Ω

Region of feasible states.

?

(·)

µ χ2{·}

A minimizing sequence. Feedback control law. Chi-square distribution.

1 Introduction The increased number of applications of autonomous robotics in the recent decade led to a rapidly growing interest in multi-robot systems (MRS’s). This is primarily because some given tasks may not be feasible by a single working robot, or multiple robots need a shorter time to execute the same task with a possibly higher performance. Potential application areas of MRS include: area coverage and patrolling missions [2], aerial/perimeter surveillance [1, 10, 29], search and rescue missions [11], searching operations [33], and situational awareness [24]. In the previously highlighted applications, the precise localization of the robotic members is a necessity in order to guarantee the success of such missions. Indeed, highly accurate localization can be achieved by equipping all the involved robots with the state-of-theart sensory means. This, however, increases the overall cost of such systems. Moreover, some robotic members, e.g. flying/hovering robots may not have enough payload/computational capacity to operate such means. Therefore, Relative Localization (RL) has been developed as a practical solution for effective and accurate execution of multi-robot collaborative tasks. The objective of relative localization is to detect and locate robots with limited sensory capabilities (observed robots) with respect to another robots with accurate localization means (observing robots). This is achieved by using the relative observation between the two robots categories, see [14, 36, 41, 48, 50], for more details. The most commonly used inter-robot relative measurements between a pair of robots in an MRS are the range and bearing measurements. Moreover, the extended Kalman filter (EKF) is the most commonly adopted nonlinear estimator for achieving the RL [14]. However, inappropriate initialization of EKF can generally lead to instability of the estimator as well as longer estimation settling time, i.e. the time required to reach acceptable estimation error levels. As a result, relative pose estimates become misleading. Indeed, this erroneous localization causes undesirable behaviour and possibly failure in collaborative missions. In order to avoid these issues, the majority of the past work assumed a known initial relative pose between two arbitrary robots, see, e.g. [17]. In addition to EKF, particle filter (PF) [12], unscented Kalman filter (UKF) [54], and pseudolinear Kalman filter [38, 48] have been also used to achieve RL in collaborative tasks. The previously highlighted estimators, except the PF, use the Gaussian probability distribution to approximate a given state noise; the PF instead approximates the distribution via Monte Carlo sampling [40].

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

Moreover, in order to reduce the computational complexity, these filters employ Markov property, i.e the current state estimate is based only on the most recent measurement and the previous state estimate. In contrast to the previous methods, it is proposed in this paper to employ a moving horizon estimation (MHE) to solve the RL problem. MHE considers the evolution of a constrained and possibly nonlinear model on a fixed time horizon, and minimizes the deviation of the model from a past window of measurements [39]. Therefore, MHE relaxes the Markov assumption shown above. Although MHE does not generally rely on any specific error distribution, tuning the estimator becomes easier when a probabilistic insight is considered, see [18, 55] for details, and [51] for a relative localization example using MHE in autonomous underwater vehicles. Once the RL problem is accurately solved, the control objectives within a given MRS are classified under virtual structure, behaviour based, and leader follower approaches. In the virtual structure approach, the individual robots in the MRS formation are treated as points on a rigid body where their individual motion are determined based on the formation overall motion. In behaviour-based control, several desired behaviours are assigned to each robot in the MRS; the behaviours include formation keeping (stabilization), goal seeking, and obstacle avoidance. Finally, in the approach of the leader follower structure, a follower robot is assigned to follow the pose of a leader robot with an offset, see the overview articles [16, 35, 42] for more details. The solution to the virtual structure approach is presented in [30], where authors designed an iterative algorithm that fits the virtual structure to the positions of the individual robots in the formation, then displaces the virtual structure in a prescribed direction and then updates the robot positions. Solutions to the behaviour based approach include motor scheme control [7], nullspace-based behaviour control [5], and social potential fields control [8]. The leader-follower control approach is achieved using feedback linearization [32], backstepping control [31], sliding mode control [43], and model predictive control [20]. In this paper, based on relative localization achieved using MHE, it is proposed to use a centralized Nonlinear Model Predictive Control (NMPC) to perform the relative trajectory tracking task where one or more robots in a given MRS are commanded to follow time varying trajectories with respect to another robot reference frame. In addition, inter-robot possible collisions must be avoided using the adopted controller. As this control problem can be hardly seen in the literature, it can be classified under the leader follower structure, with the exception that the relative references of the

3

following robots are time varying. In NMPC, a cost function is formulated to minimize the tracking error for each of the following robots. Then, a locally-optimal control is obtained by solving a discrete nonlinear optimization problem over a pre-set prediction horizon [19]. Practical implementation of MHE and NMPC requires online solutions of dynamic optimization problems, which can form a computational burden especially for systems with fast dynamics such as MRS’s. In addition, classical implementation techniques of MHE and NMPC to fast systems can cause a considerable time delay between taking measurements and applying new control commands. In order to overcome these problems, the Real-Time Iteration (RTI) scheme originally proposed in [15] and implemented in an opensource software, ACADOtoolkit [22], has been used in this study; The used software exports tailored, efficient, and self contained C-code implementing fast MHE and NMPC routines [18]. Contribution of the paper: The majority of the work considering MRS in the literature treats the relative localization and control problems separately. In contrast, in this paper, we combine the two objectives and adapt an optimization based solution for the relative localization and tracking control in multi-robot systems using fast MHE and NMPC, respectively. Therefore, in this study, we give a particular attention to the practical implementation and evaluation of the adopted method to the considered system. Herein, the advantages of using MHE against EKF are presented first via numerical simulations. Then, the the adopted estimationcontrol algorithm is evaluated when applied to a given MRS. Finally, laboratory experiments are conducted to demonstrate a practical proof-of-concept of the used algorithm. The paper is organized as follows: Section 2 presents an overview of multi-robot systems and details the relative localization and relative tracking control objectives. A brief description of the employed estimationcontrol algorithm is provided in Section 3. Section 4 shows numerical simulation results. Section 5 presents the experimental setup and real-time validation results. Finally, conclusions are drawn in Section 6. 2 Problem Setup This section provides an overview of the constructing elements in a typical multi-robot system, and introduces a detailed description of motion and measurement models involved in such systems. Moreover, an insightful explanation of relative localization and relative tracking control objectives is provided.

4

Mohamed W. Mehrez et al. Rc4

2.1 Multi-robot systems (MRS) Rc3

Rc2

(r ,  ,  ) Rcp32,k R

In a given MRS, robotic members, which include both aerial and ground robots, are classified into two classes, i.e. observing robots and observed robots. At a given time, a robot that takes inter-robot relative measurements of an arbitrary robot is defined as an observing robot, while the robot that comes into the measurement range of the observing robot is referred to as an observed robot. For the considered MRS, the robotic members are assumed to possess the following characteristics [36, 48–50]: i All robots in the MRS are equipped with deadreckoning measurement means. ii The observing robot is equipped with exteroceptive sensors in order to uniquely identify each of the observed robots in its field of view and measure their relative range and bearing. iii The observing robot uses high precision sensors such that it can accurately perform its selflocalization task while the observed robots have limited sensory/computational capabilities. iv Robots can share data and information via communication devices and protocols. v MRS’ ground robots are assumed to navigate on flat surfaces. Moreover, the aerial robots perform hovering control such that the pitch and roll angles are kept stable via low velocity manoeuvres, i.e. their 6-DOF are reduced to 4DOF for simplicity. Assume at a given time instant k ∈ N0 , a team of robots contains M ∈ N observed robots and L ∈ N observing robots where M and L are unknown to each robot, i.e. observing or observed, and also may change with time. In general, 6-DOF are required to describe a robot navigating a 3 dimensional space, see [46]; nonetheless, assumption (v) given above allows for the simplification of the 6-DOF into a 4-DOF [48]. Therefore, the relative pose of the ith observed robot in the j th observing robot’s local Cartesian coordinate frame at a given time step k is represented by the state vector j j xji,k ∈ Rnx , nx = 4, xji,k = [xji,k , yi,k , zi,k , φji,k ]T where j j j T [xi,k , yi,k , zi,k ] is the relative position and φji,k is the relative orientation, i.e. the relative yaw angle. Fig. 1 shows a sample MRS configuration space at a given time instant k ∈ N0 . Label Rpm , m ∈ {1, 2} represents observing robots, while label Rcn , n ∈ {1, 2, 3, 4} represents observed robots. In this configuration, the relative measurements are indicated by the measurej j j j j T ments vector yi,k ∈ Rny , ny = 3, yi,k = [ri,k , θi,k , αi,k ] which contains the relative range r, the relative azimuth angle θ, and the relative elevation angle α of the ith

(r ,  ,  ) Rcp42,k R

(r ,  ,  ) Rcp21,k R

(r ,  ,  ) Rcp31,k R

Rp2

Rc1

(r ,  ,  ) R pp12 ,k R

(r ,  ,  ) Rcp11,k R

Rp1

Observing Robots Observed Robots Observing and Observed Robots Relative Measurements

Fig. 1 Configuration space of a multi-robot system at a given time instant.

robot measured by the j th robot1 . As can be deduced from Fig. 1, a robot can act simultaneously as an observing robot and an observed robot, e.g. Rp2 .

2.2 Motion and Measurement Models At a given time t ∈ R≥0 , the noise free continuous time model of the relative-state xi (t) = [x, y, z, φ]Ti of the ith observed robot is given by [36, 48] x˙ i (t) = fi (xi (t), ui (t), ξ i (t))     x˙ vx cos(φ) − vy sin(φ) − v¯x + y ω ¯z   y˙  ωz    = vy cos(φ) + vx sin(φ) − v¯y − x¯    z˙   vz − v¯z ωz − ω ¯z φ˙ i i

(1)

where fi (·) : Rnx × Rnuc × Rnup → Rnx , nx = nuc = nup = 4, is the analytic vector mapping function. ui (t) = [vx , vy , vz , ωz ]Ti ∈ Rnuc is the control input velocities vector, of an observed robot, which contains the linear velocity components (vx , vy , vz )i and the angular velocity component ωz,i . ξ i (t) = [¯ vx , v¯y , v¯z , ω ¯ z ]Ti ∈ Rnup is the local velocities vector of an observing robot containing its linear and angular velocities, respectively. Since the rest of the paper is based on the discrete time formulation, we define the disturbed Euler discrete model of (1), for a given sampling period δt ∈ R>0 and a sampling instant k ∈ N0 , as xi,k+1 = fi,δt (xi,k , ui,k , ξ i,k ) + ν x

(2)

where fi,δt (·) : Rnx ×Rnuc ×Rnup → Rnx , is the discrete time dynamics given by     x vx cos(φ)−vy sin(φ)−¯ vx+y ω ¯z y  vy cos(φ)+vx sin(φ)−¯ vy−x¯ ωz     . (3) fi,δt (·)=  z  + δt  vz − v¯z φ i,k ωz − ω ¯z i,k 1

To keep the presentation simple later in the paper, all terms denoted by (·)ji will be replaced by (·)i .

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

Moreover, ν x is a zero mean additive white Gaussian noise vector which represents model uncertainties. To this end, ground observed robots are modeled by setting z = vz = 0 in (3). In terms of the relative Cartesian coordinates of an observed robot (x, y, z)i , the general 3-D relative observation model is defined by h(xi,k ) and given in (4). yi,k = h(xi,k ) + ν y i,k  p  x2 + y2 +z 2     y   νr r   arctan  +  νθ  θ  =  x !    να α i,k arctan p z 2 2 x +y i,k

(4)

where ri , θi and αi are the relative range, relative azimuth angle, and relative elevation angle, respectively. Parameters νr , νθ and να are zero mean additive Gaussian noises with standard deviations σr , σθ , and σα , respectively. As can be noticed from the measurement model (4), the relative position components (x, y, z) are indirectly measured by the model; however, the relative orientation φ is neither directly nor indirectly involved in the measurement model. The full observability of the motion-measurement models pair, i.e. (2) and (4), is guaranteed provided that the observed robot linear velocity vx is non-zero, see [46, 48], for details.

2.3 Estimation-Control Objective The main objectives for the MRS considered in this study can be summarized by two iterative tasks for all k ∈ N0 : a) Localize: using the odometery measurements ˜ i,k ∈ Rnuc and ξ˜i,k ∈ Rnup , as well as the sensory u ˜ i,k ∈ Rny , the localization task finds the best readings y ˆ i,k ∈ Rnx of the relative-pose state vector xi,k estimate x for each observed robot, i.e. for all i ∈ {1, 2, . . . , M }. b) Control: based on the first task, the control task is to calculate a feedback control law µi,k (ˆ xi,k ) : Rnx → Rnuc , i ∈ {1, 2, . . . , M }, such that each observed robot is tracking a reference trajectory with respect to another observing robot with the reference relative state vector xri,k ∈ Rnx and reference speeds uri,k ∈ Rnuc . Moreover, while robots are following their relative reference trajectories, they must not be jeopardized by collision with other robots in the given MRS. Traditional techniques solving the localization task are based on EKF. However, due to linearizations of

5

models (2) and (4) involved in EKF implementation, loss of information and bias problems arise [38]. Moreover, proper initialization of the estimated state in EKF is crucial because any misleading initialization can lead to unstable estimation and/or longer estimation settling time, see, e.g., [14, 49], for more details. To overcome these problems, MHE is proposed as the main estimator to satisfy the first task (RL). To achieve the second task (control), NMPC is proposed. The main reason for choosing NMPC is its ability to handle constrained nonlinear systems, e.g. (2), in a straight forward manner. Thus, the collision avoidance task of the controller is implicitly included in its associated optimal control problem as well be seen in the following section, see, e.g. [16, 35, 37] for more details.

3 Estimator-Controller Synthesis In This section, the mathematical formulation of MHE and NMPC for the localization and tracking control objectives is presented. In addition, a brief description of the code generation toolkit used is given.

3.1 MHE Formulation For each observed robot in a given MRS, the MHE relative-state estimator is formulated as a least squares (LSQ) cost function JNE : Rnx × Rnuc × Rnup → R≥0 shown in (5). Hence, using (5), the MHE repeatedly (k = 0, 1, · · · ) solves the constrained nonlinear dynamic optimization problem (6) over a fixed estimation horizon of length NE ∈ N [18]. 2 JNE (xi , ui , ξ i ) = kxi,q−NE − xest i,q−NE kAi

2 q q−1 X X

ui,k − u ˜ i,k 2

˜ i,k kVi + + kh(xi,k ) − y

ξ − ξ˜ i,k i,k W k=q−NE

k=q−NE

i

(5)

min

xi,k ,ui,k ,ξi,k

JNE (xi , ui , ξ i )

(6)

subject to: xi,k+1 = fi,δt (xi,k , ui,k , ξ i,k ) xlower ≤ i  lower  ui ≤ ξ lower i

xi,k ≤   ui,k ≤ ξ i,k

xupper i  upper  ui ξ upper i

∀ q − NE ≤ k ≤ q − 1 ∀ q − NE ≤ k ≤ q ∀ q − NE ≤ k ≤ q − 1

˜ i,k , u ˜ i,k , and ξ˜i,k denote the actually measured In (5), y system outputs and inputs at instant k, respectively. The first term in (5) is known as the arrival cost and it penalizes the deviation of the first state in the moving

6

Mohamed W. Mehrez et al.

horizon window and its priori estimate xest i,q−NE by the diagonal positive definite matrix Ai ∈ Rnx ×nx . Normally, the estimate xest i,q−NE is adopted from the previous MHE estimation step. Moreover, the weighting matrix Ai is chosen as a smoothed EKF-update based on sensitivity information gathered while solving the most recent MHE step. Therefore, xest i,q−NE and Ai have to be only initialized, see [18, 27] for more details. The second term in (5) penalizes the change in the system predicted outputs h(xi,k ) from the actually mea˜ i,k by the diagonal positive-definite masured outputs y ny ×ny trix Vi ∈ R . Similarly, the change in the applied control inputs (ui,k , and ξ i,k ) from the measured inputs (˜ ui,k , and ξ˜i,k ) is penalized using the diagonal positivedefinite matrix Wi ∈ R(nuc +nup )×(nuc +nup ) . The latter term is included in the cost function (5) to account for actuator noise and/or inaccuracy, see [55] for details. Vi and Wi are chosen to match the applied motion and measurement noise covariances. Furthermore, all estimated quantities in (6) may be subject to lower and upper bounds which signify the system physical limitations. The solution of the optimization problem (6) leads mainly to the relative-state estimate sequence ˆ i,k , k = (q − NE , · · · , q) where x ˆ i,0 := x ˆ i,q denotes the x current estimate of the relative state vector of a given ˆ i,0 is later used as a feedback to the observed robot; x NMPC controller presented in the following subsection. Moreover, estimates sequences for the actually applied ˆ i,k and ξˆi,k , are byproducts of the control inputs, i.e. u MHE estimator.

3.2 NMPC Formulation Motivated by the assumptions presented in Subsection 2.1, in particular that the observing robots have higher computational capabilities than the observed robots, a centralized NMPC controller is proposed to achieve the relative trajectory tracking task. To this end, an MRS overall augmented nominal model, for a sampling instant k ∈ N0 , is defined as xa,k+1 = fa,δt (xa,k , ua,k , ξ a,k )

(7)

where xa = [xT1 , xT2 , . . . , xTM ]T ∈ Rnx ·M is the augmented relative state vector of M observed robots; ua = [uT1 , uT2 , . . . , uTM ]T ∈ Rnuc ·M is the observed robots’ augmented controls; ξ a = [ξ T1 , ξ T2 , . . . , ξ TL ]T ∈ Rnup ·L is the augmented controls of the observing robots; and fa,δt : Rnx ·M ×nuc ·M ×nup ·L → Rnx ·M is the augmented nonlinear mapping given as T T T fa,δt = [f1,δt , f2,δt , . . . , fM,δt ]T

where fi,δt , i ∈ {1, 2, . . . , M } are given by (3).

To this end, we define the augmented reference trajectories and the augmented reference speeds for the obT  ∈ Rnx ·M served robots as xra = xr1 T , xr2 T , . . . , xrM T   T ∈ Rnuc ·M , respecand ura = ur1 T , ur2 T , . . . , urM T tively. Similar to the MHE formulation, the receding horizon NMPC controller is formulated using a least squares (LSQ) cost function JNC : Rnx ·M × Rnuc ·M → R≥0 penalizing the future deviation of the observed robots relative state vectors and control inputs from their time varying references as shown in (8).

2

JNC (xa , ua ) = xa,NC − xra,NC P

a

+

NX c −1 





xa,k − xra,k 2 + ua,k − ura,k 2 (8) R Q a

a

k=0

min JNC (xa , ua )

(9)

xa,k ,ua,k

subject to: ˆ a,0 xa,0 = x xa,k+1 = fa,δt (xa,k , ua,k , ξ a,k ) ∀ 0 ≤ k ≤ NC − 1 xa,k ∈ Ωa ulower a

≤ ua,k ≤

∀ 0 ≤ k ≤ NC uupper a

∀ 0 ≤ k ≤ NC − 1

For each sampling instant (k = 0, 1, · · · ), NMPC solves repeatedly the nonlinear dynamic optimization problem (9) over a future prediction horizon specified by NC ∈ N [47]. The first term of the cost function (8) accounts for the terminal state penalty. This is achieved by using the diagonal positive-definite matrix Pa ∈ Rnx ·M ×nx ·M , which penalizes the norm of the augmented terminal state error. This matrix is defined as Pa = diag(P1 , P2 , . . . , PM ) where Pi ∈ Rnx ×nx is the terminal state penalty of the ith observed robot for all i ∈ {1, 2, . . . , M }. The remaining terms in (8) account for the running costs of the future deviation between the robot states and controls, and their references. Indeed, the reference control values, i.e. ur{·} , represent the desired speeds along the reference trajectories. Moreover, penalizing the deviation between the control actions and their corresponding nominal values, in (8), is mainly for regularization purposes, i.e. to reduce/avoid the bang-bang behaviour of the optimal solution; this behaviour is undesirable especially in experiments. Therefore, this term leads to solutions that are less stressful to robots’ actuators and are close to their reference values. Additionally, as highlighted in the MPC literature, penalizing the control action may lead to easier optimization problems to solve, see, e.g. [19, Chapter 3, Section 3.1], for details. As a result,

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

the formulation (8) became common in studies considering autonomous applications, see, e.g. [26–28, 47, 55]. The norms of the running costs are weighted with the positive definite matrices Qa ∈ Rnx ·M ×nx ·M and Ra ∈ Rnuc ·M ×nuc ·M where Qa = diag(Q1 , Q2 , . . . , QM ) and Ra = diag(R1 , R2 , . . . , RM ), respectively. Qi ∈ Rnx ×nx and Ri ∈ Rnuc ×nuc , i ∈ {1, 2, . . . , M } are the individual running costs weighing matrices defined for each observed robot. The weighting matrices Pa , Qa , and Ra are chosen based on a simulation tuning procedure [27, 55]. As can be noticed from (9), the initial condition of the prediction of xa is set to the augmented estimate ˆ a,0 = [ˆ ˆ T2,0 , . . . , x ˆ TM,0 ]T ∈ Rnx ·M resulting vector x xT1,0 , x from solving problem (6) for all observed robots. Moreover, the augmented feasible region of xa , i.e. Ωa , shown in (9) accounts for the upper and the lower limits on the state xa , i.e. xlower and xupper , as well as the inter-robot a a collision avoidance which can be formulated by the following inequality constraints for all k ∈ {0, 1, . . . , NC } xlower ≤ xa,k ≤ xupper , a a

(10)

7

Algorithm 1 Estimation-control scheme in MRS based on MHE and NMPC 1: Set the number of the observed robots M 2: for every sampling instant tk , k = 0, 1, 2, . . . do 3: for each observed robot Rci , i = 1, 2, . . . , M do 4: Get the past NE odometery measurements, i.e. ˜ . ˜ i,k and ξ u i,k 5: Get the past NE + 1 relative measurements, ˜ i,k . i.e. y 6: Solve the optimization problem (6) over estimation horizon NE , and find the best current ˆ i,0 |k . estimate of the relative state xi,0 |k , i.e. x 7: end for T ˆ a,0 |k = [ˆ ˆT ˆT 8: Set the vector x xT 1,0 , x 2,0 , . . . , x M,0 ]k . 9: Solve the optimization problem (9) over prediction horizon NC , and find  the minimizing control se ? ? ? . quence u? a,NC = ua,0 , ua,1 , · · · , ua,NC −1 k h iT k ? ? T ? T ? T 10: Extract ua,0 k = u1,0 , u2,0 , . . . , uM,0 k from u? a,NC . k 11: Define the NMPC-feedback control law µi,k (ˆ xi,k ) = u? i,0 , i ∈ {1, 2, . . . , M } . k

12: for each observed robot Rci , i = 1, 2, . . . , M do 13: Apply µi,k (ˆ xi,k ) to the observed robot Rci . 14: end for 15: end for

2

1−

k(x, y, z)l,k − (x, y, z)m,k k ≤ 0 ∀ l, m : l 6= m, (11) 4rc2

3.3 ACADO Code Generation

and 2

k(x, y, z)l,k − (x, y, z)n,k k 1− ≤0 (rc + rp )2

∀ l, n

(12)

where l, m ∈ {1, 2, . . . , M }, n ∈ {1, 2, . . . , L}, rc is the minimum Euclidean distance that all the robots in the given MRS must maintain from any observed robot, and rp is the minimum Euclidean distance that all the robots in the given MRS must maintain from any observing robot. The solution of problem (9), at a sampling instant k ∈ N0 , leads to a minimizing (locally optimal) control sequence
u?a,NC k = u?a,0 , u?a,1 , · · · , u?a,NC −1 k ∈ Rnuc ·M ×NC iT h ∈ Rnuc ·M . where u?a,0 k = u?1,0 T , u?2,0 T , . . . , u?M,0 T k Thus, the feedback control law, for each observed robot in the given MRS, is defined by µi,k (ˆ xi,k ) = u?i,0 k , i ∈ {1, 2, . . . , M }. It has to be mentioned here that the observing robot speeds ξ k are considered as constant parameters when solving (9). Moreover, the units of the all weighting matrices appearing in the cost functions (5) and (8) are chosen such that these functions are dimensionless. Algorithm 1 summarizes the overall estimation-control scheme, for a given MRS, in which the term estimation-control refers to the main two iterative steps of the algorithm.

The open-source ACADO Code Generation toolkit [23, 47] is used in the implementation of the adopted algorithm. Based on a symbolic representation of MHE and NMPC problems, ACADO generates an optimized and self-contained C code, which limits the associated computations to the most essential steps. This is done by examining the structure of the defined optimization problems, and avoiding the dynamic memory allocation; thus, eliminating any unnecessary and/or redundant computations [22, 23]. The exported C code is based on the Direct Multiple Shooting method, utilizing a condensing algorithm, and a Real Time Iteration (RTI) scheme. The RTI scheme reduces the time delay of the optimization problems (6) and (9) by preparing the associated computations in advance even before the measurements/feedback become available, see [22, 23, 47], for details. Using ACADO code generation tool has been recently reported in several studies for different applications, e.g. autonomous driving [55], autonomous tractortrailer systems [26–28], and wind energy systems [56, 57]. In this paper, the use of fast MHE and NMPC schemes is extended to MRS’s relative localization and tracking control. In the following two sections, the generated code has been tailored to solve the minimization problems (6) and (9) based on the Gauss Newton method, a real time

8

Mohamed W. Mehrez et al.

iteration (RTI) scheme, a multiple-shooting discretization, and an Euler integrator for state prediction. The number of Gauss Newton iterations was limited to 10 while the Karush-Kuhn-Tucker (KKT) tolerance (a first order optimality condition) was set to 10−4 . This particular choice provided a compromise between the execution time and the convergence of Algorithm 1 when implemented to the considered system, see [23, 47], for details. Indeed, an integrator with more than one integration step can be used, e.g. Runge-Kutta integrator, but the Euler integrator was chosen mainly because it is faster in state integration. Moreover, its performance was satisfactory for the considered problem. It has to be mentioned here that the same state integrator was used in the implementation of EKF in subsection 4.1.

4 Simulation Results In this section, two simulation studies are presented. First, a comparison simulation between MHE and EKF, which shows the main advantages of using MHE over EKF in RL. Second, a closed-loop simulation to evaluate Algorithm 1 when applied to a given MRS.

4.1 Comparison between MHE and EKF for RL The performance of MHE against EKF is presented by a series of Monte Carlo numerical simulations. At a given sampling instant k ∈ N0 , EKF finds the current ˆ i,k , i ∈ {1, 2, . . . , M } estimate of an observed robot x through two successive steps, i.e. a prediction step and a measurement update step, see [45], for details. The conducted simulations here compromised an MRS with a ground observing robot and three aerial observed robots, i.e. L = 1, M = 3. The observed robots were commanded to navigate along trajectories having different elevations; thus, a 3D navigation space of dimensions 10 × 10 × 3 m3 was selected. In order to examine the effect of the sensory precision on the accuracy of the estimation, three noise configurations for ν y in (4) and shown in Table 1, were used [14, 48]. Under all sensory noise configurations, the odometery measurements noise standard deviations specified by (13) were also used for all robots in the MRS. σvx = σvy = σvz = 0.01 m/s σv¯x = σv¯y = σv¯z = 0.01 m/s σωz = σω¯ z = 0.0873 rad/s

(13)

Following [18, 27], the estimation horizon length of MHE was chosen as (NE = 30) using a trial and error

method with the following tuning matrices V = diag(σr , σθ , σα )−1 , and W = diag(σvx , σvy , σvz , σωz , σv¯x , σv¯y , σv¯z , σω¯ z )−1 . The comparison between MHE and EKF was investigated through 15 Monte-Carlo simulations running at 10 Hz, i.e. δt = 0.1 (seconds). The simulations presented here were conducted using the code generation tool of ACADO for MHE [18]. The generated C code has been then compiled into a MEX function and all simulations were run using Matlabr . The performance of the two estimators was evaluated for all observed robots by computing the rootmean-square-error (RMSE) of the relative position and relative orientation; thus, the conducted simulations are equivalent to 45 Monte-Carlo simulations. In order to meet the practical requirements of state estimation, the adopted estimators do not assume any knowledge of observed robots initial positions and/or orientations. Nonetheless, by inspecting the measurement model (4), the initial guess of the observed robots positions can be set using (14) for all i ∈ {1, 2, 3}. x− ˜i,o cos(α ˜ i,o ) cos(θ˜i,o ) i,o = r − yi,o = r˜i,o cos(α ˜ i,o ) sin(θ˜i,o ) − zi,o = r˜i,o sin(˜ αi,o )

(14)

− − where x− i,o , yi,o and zi,o are the initial guess of an observed robot position, and r˜i,o , α ˜ i,o and θ˜i,o are the corresponding initial relative measurements. Additionally, the initial guess of an observed robot orientation is set to zero. To this end, the comparison between MHE and EKF presented here extends our early study of implementing MHE to the relative localization problem in MRS, where a single measurement noise configuration has been used without any practical filter initialization, see [36], for details. The simulation results of the two estimators are summarized in Fig. 2. Under the sensory noise configuration of Table 1 (case 2), the RMSE of the relative Euclidean position and relative orientation are presented in Fig. 2 (top); the RMSE of the relative position is calculated by: ! 21 MX ·nmc 1 (ˆ x2e + yˆe2 + zˆe2 ) M · nmc i=1

Table 1 Sensory noise configurations. Case

Relative measurements noise configuration

1 2 3

σr = 0.0068 (m), σθ = σα = 0.0036 (rad) σr = 0.0167 (m), σθ = σα = 0.0175 (rad) σr = 0.1466 (m), σθ = σα = 0.1000 (rad)

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems 100

RMSE (m)

MHE EKF EKF Convergence Instants

RMSE (rad)

10-2

0

10

20

30

40

50

60

70

80

90

0

10

20

30

40

50

60

70

80

90

0

10

Average RMSE (rad) Average RMSE (m)

Simulation Time(seconds) 0.15

MHE EKF

0.1 0.05

9

The conducted simulations showed MHE advantages over EKF in achieving a more accurate state estimation with fast convergence. Although the slow convergence of EKF can be treated by designing open-loop controls for the initial period of the control process to safely wait for the estimator convergence, EKF has additionally shown, in the literature, unsatisfactory performance in the existence of measurements’ outliers when compared with MHE, see, e.g. [4, 6], for several numerical examples. Therefore, the high accuracy and the fast convergence of MHE, as well as the easy incorporation of state constraints in MHE formulation as shown in (6), suggests its use as a solution for relative state estimation in multi-robot systems.

0 1

2

3

4.2 Estimation and Control using MHE and NMPC

0.15 0.1 0.05 0 1

2

3

Case Study

Fig. 2 MHE and EKF performance for RL: relative position and relative orientation estimation RMSE for case 2 shown in Table 1 (top). Relative position and relative orientation estimation average RMSE for all sensory noise configurations cases shown in Table 1 (bottom).

where x ˆe , yˆe and zˆe are the relative state position error Cartesian components for all observed robots, and nmc = 15 is the number of Monte-Carlo simulations. As can be noticed from Fig 2 (top), MHE converges to acceptable estimation error limits immediately after the first estimation step is performed; however, EKF takes longer number of iterations until it reaches the same estimation error, i.e. 30 iterations (3 seconds) for relative position estimation error convergence and 45 iterations (4.5 seconds) for relative orientation estimation error convergence. Moreover, a more accurate relative positional estimation is observed under MHE while the relative orientation estimation accuracy of the two estimators are comparable. Indeed, it is crucial to achieve the state estimation with acceptable error limits as fast as possible in order to guarantee a satisfactory controller performance; thus, for RL, MHE outperforms EKF in this regard. Additionally, under the three cases of noise configurations shown in Table 1, the average estimation RMSE for relative position and orientation are presented in Fig. 2 (bottom) which shows an increase in the estimation error as the uncertainty of the relative measurements increases. Nonetheless, in all cases, MHE showed more accurate estimation results when compared with EKF.

Algorithm 1 was evaluated numerically when applied to a team of nonholonomic robots with a ground observing robot and three aerial observed robots, i.e. L = 1, M = 3 and vyi = v¯y = 0 for all i ∈ {1, 2, 3}. Here, the observed robots are required to track reference trajectories in the observing robot frame of reference which can be moving; such a mission can be seen in a number applications, e.g. area coverage, patrolling missions, searching operations, and perimeter surveillance, see, e.g. [2, 21, 29, 33], for more details. To this end, two case studies have been considered depending on the observing robot speeds: (a) the observing robot is moving in a straight line, i.e. v¯x = 0.1 (m/s) and w ¯z = 0 (rad/s), (b) The observing robot is moving in a circle, i.e. v¯x = 0.1 (m/s) and w ¯z = −0.05 (rad/s). See Table 2 for the detailed relative reference trajectories, of the observed robots, under these two cases. The simulations running time was set to 75 (seconds) with a sampling time δt = 0.2 (seconds). Similar to the simulations presented in Section 4.1, the observed robots have odometery measurements noise standard deviations given by (13). In addition, the observing robot is adopting a relative measurement means with sensory noise configuration specified by Table 1 (case 2). Motivated by the experimental requirements presented in the following Section, the saturation limits of the control inputs for all observed robots have been set to (15) for all i ∈ {1, 2, 3}. −0.25 ≤ vx,i ≤ 0.6 m/s −0.25 ≤ vz,i ≤ 0.6 m/s −0.7 ≤ ωz,i ≤ 0.7 rad/s

(15)

Indeed, the selected relative reference trajectories (Table 2) were chosen such that the corresponding reference speeds are not violating the limits (15). Moreover, to inspect the controller ability in fulfilling the

10

Mohamed W. Mehrez et al.

Rci

Case 1: v¯x = 0.1 (m/s), ω ¯ z = 0.0 (rad/s).

Case 2: v¯x = 0.1 (m/s), ω ¯ z = −0.05 (rad/s).

Rc1

xr1 = 4 + 1.5 sin(0.22t) y1r = 1.5 cos(0.22t) z1r = 0.5

xr1 = 4 + 1.5 sin(0.2t) y1r = 1.5 cos(0.2t) z1r = 0.5

Rc2

xr2 = 2 + 1.5 sin(0.13t) y2r = −1 + 1.5 cos(0.13t) z2r = 0.5

xr2 = 2 + 1.5 sin(0.1t) y2r = −1 + 1.5 cos(0.1t) z2r = 0.5

Rc3

xr3 = 2 + 1.5 sin(0.14t) y3r = 0.5 + 1.5 cos(0.14t) z3r = 0.5

xr3 = 2 + 1.5 sin(0.12t) y3r = 0.5 + 1.5 cos(0.12t) z3r = 0.5

collision avoidance task, the selected reference trajectories were designed such that there are locations along the mission where at least two observed robots are sharing the same reference position at the same time. The initial postures of all observed robots were also set such that a large initial tracking error is imposed on the controller. For MHE, the estimation horizon was selected as (NE = 20); estimator initialization and the selection of the associated tuning matrices were chosen as presented in Section 4.1. Additionally, following [27], NMPC prediction horizon was set to (NC = 20) using a trial and error method with the associated LSQ weighting matrices selected, for all i ∈ {1, 2, 3}, as Qi = diag(10, 10, 10, 0.1) Ri = diag(25, 25, 25)

robot were initialized by the corresponding reference speeds along the reference trajectories. All the simulations presented here were conducted using the code generation tool of ACADO for MHE and NMPC [18,23] which resembles the necessary real-time implementation requirements. The generated C code has been compiled into MEX functions and all simulations were conducted using Matlabr . Fig. 3 and Fig. 4 (best viewed in colors) summarize the performance of Algorithm 1 when applied to the considered MRS under the two considered scenarios shown in Table 2, and show the 3D exhibited trajectories of the observed robots with respect to the observing robot frame of reference. The overall motion of the MRS considered here can be seen in the supporting media files of this article. Fig. 3 and Fig. 4 (top) illustrate the performance of the estimator by depicting the the estimated trajectories against their ground truth data. As can be noticed, despite the exhibited trajectories by the observed robots, MHE provided an accurate state estima-

Rc1 Position Estimate Rc2 Position Estimate Rc3 Position Estimate Ground Truth

0.7 0.6

z-position (m)

Table 2 Simulation scenarios reference trajectories

0.4

0.2

(16) 0

Pi = diag(50, 50, 50, 0.5)

2

6

1 0

4

y-position (m) -1

2

-2

x-position (m)

0

Rc1 Actual Position Rc2 Actual Position Rc3 Actual Position Reference

0.7 0.6

z-position (m)

As can be noticed from (16), the weights that correspond to the robots position in Q are chosen larger than that for the orientation, to give more freedom in steering the robots to reach their desired positions. Moreover, due to the experimental requirements, the elements of the weighting matrix R were chosen larger than that for Q to obtain a well damped closed-loop response and compensate for the unmodeled dynamics of the robots in the experiments. Since the terminal value of the tracking error shown in (8) is the most necessary future error value that needs to be minimized, elements of P are chosen 5 times larger than that for Q, see, e.g. [27] for more details on NMPC tuning guidelines. The radii shown in the inequality constraints (11) and (12) were chosen as rc = 0.225 (m) and rp = 0.3 (m). It has to be mentioned here that the initialization of NMPC optimization problem (9), which is subjected to non-convex constraints, was necessary to obtain a non-empty solution, i.e. the control actions for each

0.4

0.2

0 2

6

1 0

4

y-position (m) -1

2

-2

x-position (m)

0

Fig. 3 Algorithm 1 performance for case 1 given by Table 2: estimator performance (top); controller performance (bottom). Observing robot is highlighted by a black color filled triangle at the origin.

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

11

0.5

0.1 0 -0.1

-0.3

0.1 0 -0.1

-0.5 0

0.2

0.3

-0.3

-0.2 0.4

20

40

60

0

20

40

60

20

40

60

z e-error(m)

0.4

0 2

6

1 0

4

y-position (m) -1

2

-2

x-position (m)

0

0.2 0.1 0 -0.1 -0.2

φ e-error(rad)

z-position (m)

0.6

0.2

y e-error(m)

0.7

x e-error (m)

Rc1 Position Estimate Rc2 Position Estimate Rc3 Position Estimate Ground Truth

0.5 0.2 0 -0.2 -0.5

-0.4 0

20

40

60

0

Simulation Time (sec.) Rc1 Actual Position Rc2 Actual Position Rc3 Actual Position Reference

0.7

Simulation Time (sec.)

Fig. 5 Relative state estimation error for (Rc1 ) in Case 1: estimation error (solid black), 3σ boundaries (solid blue).

0.2

0 2

6

1 0

4

y-position (m) -1

2

-2

1

2.5

0.4

2

0.2

1.5

0

1

20

40

60

0.5

Tracking Error(rad)

3 0.4

Tracking Error(m)

z-position (m)

0.6

0.5 0 -0.5

Rc1 Rc2 Rc3

-1

x-position (m) 0

0

-1.5 0

20

40

60

0

Simulation Time (sec.)

2 The actual trajectories of the observed robots in Fig. 3 and Fig. 4 (bottom) are the ground truth data in the same figures (top).

3

60

1

2.5

0.3

Tracking Error(rad)

tion along all trajectories. Fig. 5 shows the estimation error components and the 3σ error boundaries (where σ is the estimation error standard deviation) for the aerial observed robot Rc1 under case (1) of Table 2. It can be seen from Fig. 5 that the estimation errors did not go beyond the 3σ error boundaries. This indicates that the estimation process is conservative, i.e. no overconfident estimation has occurred. This observation was identical to the other observed robots. Moreover, the MHE estimator achieved the relative localization task with ∼ 10 (cm) positional accuracy and ∼ 0.18 (rad) orientation accuracy. Fig. 3 and Fig. 4 (bottom) show the performance of the controller by depicting the actual trajectories of the observed robots and their references2 . The position and orientation of the robots are indicated by colorfilled triangles. These figures show that the NMPC controller provided an accurate trajectory tracking regard-

40

Fig. 6 Relative tracking error for all observed robots (case 1 given by Table 2): relative position error (left), relative orientation error (right). Collision avoidance periods are highlighted in gray.

Tracking Error(m)

Fig. 4 Algorithm 1 performance for case 2 given by Table 2: estimator performance (top); controller performance (bottom). Observing robot is highlighted by a black color filled triangle at the origin.

20

Simulation Time (sec.)

0.2

2

0.1

1.5

0 1

20

40

60

0.5

0.5 0 -0.5

Rc1 Rc2 Rc3

-1 -1.5

0 0

20

40

60

Simulation Time (sec.)

0

20

40

60

Simulation Time (sec.)

Fig. 7 Relative tracking error for all observed robots (case 2 given by Table 2): relative position error (left), relative orientation error (right). Collision avoidance periods are highlighted in gray.

less the initially imposed error. Moreover, at the moments where the observed robots were about collision, the controller provided collision free maneuvers in which the observed robots were commanded to deviate from their references to fulfill this task. Fig. 6 and Fig. 7 show the relative tracking errors of all observed robots

12

Mohamed W. Mehrez et al.

vx,1 (m/s) vz,1 (m/s) ωz,1 (rad/s) Reference

Robot Speeds (m/s), (rad/s)

0.8 0.6

vx,2 (m/s) vz,2 (m/s) ωz,2 (rad/s) Reference

0.8 0.6

0.6

0.4

0.4

0.4

0.2

0.2

0.2

0

0

0

-0.2

-0.2

-0.2

-0.4

-0.4 0

20

40

60

vx,3 (m/s) vz,3 (m/s) ωz,3 (rad/s) Reference

0.8

-0.4 0

Simulation Time (sec.)

20

40

60

0

20

Simulation Time (sec.)

40

60

Simulation Time (sec.)

Fig. 8 Control actions for all observed robots case 1. Collision avoidance periods are highlighted in gray.

Robot Speeds (m/s), (rad/s)

1.2

vx,1 (m/s) vz,1 (m/s) ωz,1 (rad/s) Reference

1 0.8 0.6

1.2

1.2

vx,2 (m/s) vz,2 (m/s) ωz,2 (rad/s) Reference

1 0.8 0.6

0.8 0.6

0.4

0.4

0.4

0.2

0.2

0.2

0

0

0

-0.2

-0.2

-0.2

-0.4

-0.4

-0.4

-0.6

-0.6 0

20

40

60

Simulation Time (sec.)

vx,3 (m/s) vz,3 (m/s) ωz,3 (rad/s) Reference

1

-0.6 0

20

40

60

0

20

Simulation Time (sec.)

40

60

Simulation Time (sec.)

Fig. 9 Control actions for all observed robots case 2. Collision avoidance periods are highlighted in gray.

In order to check that the inequality constraints (10) are fulfilled, the inter-robot Euclidean distances are presented in Fig. 10, where it can be noticed that these distances did not violate their margin. Indeed, it is already clear from Fig. 3 and Fig. 4 that the observed robots are far enough from the observing robot such that the inequality constraints (11) are satisfied. Since the adopted estimator-controller algorithm will be implemented experimentally, the associated computational requirements must be satisfied. The conducted simulations here have been run on a standard PC with a quad-core 2.5 GHz processor and 8 GB of memory.

4 3 2 1 0

Rc1 − Rc2 Rc1 − Rc3 Rc2 − Rc3 2 · rc (m)

5

Inter Robot Distance (m)

Rc1 − Rc2 Rc1 − Rc3 Rc2 − Rc3 2 · rc (m)

5

Inter Robot Distance (m)

in terms of the Euclidean relative position, and the relative orientation; the steady state relative tracking errors were observed to be within ∼ 12 (cm) for the relative position and ∼ 0.16 (rad) for the relative orientation. However, during two time intervals (highlighted in gray in Fig. 6 and Fig. 7) positional tracking errors deviated largely from the steady state values for collision avoidance purposes. This can be also seen in Fig. 8 and Fig. 9, where the control commands of all observed robots deviate also from their references to achieve collision free maneuvers. Moreover, these figures show that the control actions never went beyond their physical limits (15).

4 3 2 1 0

0

10

20

30

40

50

60

70

0

10

Simulation Time (sec.)

20

30

40

50

60

70

Simulation Time (sec.)

Fig. 10 Inter-robot Euclidean distances: case 1 (left), case 2 (right). Table 3 Algorithm 1 computational times (case 1). Computational Time

Rc1

Rc2

Rc3

3.4 3.1

3.3 3.1

3.3 3.1

MHE

maximum (ms) average (ms)

NMPC

maximum (ms) average (ms)

20 15

Total

maximum (ms) average (ms)

30 19.3

MHE and NMPC computational times, for case 1 given by Table 2, are presented in Table 3 where it can be noticed that the adopted auto-generated C-code provided a fast implementation of the utilized algorithm, i.e. the

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

feedback control actions µi (ˆ xi ), i ∈ {1, 2, 3} were always ready to be sent to the observed robots within a maximum delay of ∼ 30 (milliseconds) from the time ˜ i , i ∈ {1, 2, 3} were available. Simithe measurements y lar computational requirements were observed for case 2 in Table 2.

Relative Measurements

~ y i  [ri  i ]T

Odometery Measurements

i  {1,2}

Localization Data x i i  {1,2} OptiTrack ® Motion Capture System

Algorithm 1 was validated experimentally using a team of three ground nonholonomic robots, i.e. an Irobotr which served as an observing robot and two Pioneer 3ATr research platforms which served as observed robots; thus, L = 1 and M = 2, see, e.g. [34], for the technical details of Pioneer 3-AT mobile robots. To achieve high accuracy generation of the ground truth data, all robots were localized using the commercial motion capture system OptiTrack r with a setup of 4 cameras. Motivated by [13,48], the ground truth data has been used ˜ i , i ∈ {1, 2} by to synthesis the relative measurements y computing the relative range ri and relative bearing θi using (4). Although synthesizing the relative measurements relaxes assumption (ii) presented in Section 2.1 in the conducted experiments, it provides a mean to simulate the behaviour of a relative measurement sensor and the freedom to choose its level of accuracy from Table 1, see, e.g. [44] for a possible relative measurement sensor that can be utilized here. The conducted experiments adopted a remote host 3 running a TCP/IP Matlab client which communicates wirelessly with the robotic team to acquire the odometery measurements while remotely communicating with OptiTrack ’s optical motion capture software (Motive r ) in order to stream the ground truth data. Moreover, the same remote host sends the feedback control commands to servers running on the observed robots’ platforms through the same TCP/IP connection. It has to be mentioned here that, in order to meet the real-time communication requirements, the client-server module of Pioneer-3AT mobile robots is based on an unacknowledged communication packet interface with simple means for dealing with ignored/lost packets, see Pioneer-3AT manual for more technical details [3, Chapter 6]. Moreover, the communication network used in the experiment was tested, for sending and receiving data, and the transmission delays were observed to be within the order of magnitude of ones of milliseconds and, thus, suitable for the sampling time used in the experiment. The client performs the estimation-control associated computations by using the same MEX func-

Observed Robots’ Speed Controllers

The used remote host in the experiments is the same PC used in simulations of subsection 4.2.

MHE State Estimator

Odometery Readings ~ ,~ u i ξ1 i  {1,2}

Team

5 Experimental Results

3

Remote Host

TCP/IP Interface

MRS

13

Ground Truth and Relative Measurement Generation

x leader

Feedback Control Law

Relative State Estimate xˆ i i  {1,2}

Centralized NMPC Tracking Controller

i (xˆ i ) i  {1,2}

Reference Trajectory Generator

x ra , u ra

Fig. 11 Block diagram of the experimental setup used to validate Algorithm 1.

tions generated in Section 4 for MHE and NMPC. The block diagram of the architecture used to validate the estimator-controller design is illustrated in Fig. 11. In the experiment, the observed robots were required to track the relative reference trajectories shown in Table 4, while the observing robot is stationary. Similar to subsection 4.2, these trajectories were designed such that there is at least an instant at which the two references share the same location. The total experiment time was set to 65 (seconds) with a sampling rate of 10 Hz, i.e. δt = 0.1 (seconds). Furthermore, the measurement noise configuration given in Table 1 (case 2) was used. MHE and NMPC tuning matrices were chosen following the method presented in subsection 4.2 with NE = NC = 30. Additionally, the radii shown in the inequality constraints (11) and (12) were chosen as rc = 0.5 (m) and rp = 0.3 (m). Fig. 12 summarizes the performance of MHE and NMPC; to increase the readability of the figure, the performance has been visualized over two consequent time intervals. See the supporting media files of this article for the experiment video. As can be concluded from Fig. 12, the utilized estimation-control design provided an accurate relative localization as well as relative tracking control. Fig. 13 depicts the relative state estimation error of the observed robots poses where it can be noticed that MHE provided an accurate state estiTable 4 Reference trajectories in experiments. Rci

Reference

Rc1

xr1 y1r

Rc1

xr1 = 2 + 1.2 sin(0.2t) y1r = −0.9 + 1.2 cos(0.2t)

= 2 + 1.2 sin(0.15t) = 1.2 cos(0.15t)

Mohamed W. Mehrez et al.

(17)

2

y-position (m)

1

1

0 -0.05 -0.1 0

10

20

30

40

50

60

0

10

20

30

40

50

60

0

10

20

30

40

50

60

0.1 0.05 0 -0.05 -0.1

0.2 0.1 0 -0.1

Experiment Time (sec.)

Fig. 13 Relative state estimation error components.

8

Rc1 Rc2 χ2nx ,δ

6 4 2

20

40

Experiment Time (sec.) 0

Rc1 − Rc2 Rc1 − Rp1 Rc2 − Rp1 2 · rc (m) rc + rp (m)

6 5 4 3 2 1 0

0 0

60

0

20

40

60

Experiment Time (sec.)

0

Fig. 14 NEES values of the estimation results(left). Interrobot Euclidean distances (right). -1

-1

-2

-2 0

y-position (m)

Rc1 Position Estimate Rc2 Position Estimate Ground Truth

2

0.05

-0.2

NEES

with δ specifying the significance level, e.g. a 95% acceptable region for a distribution of dimension nx = 3 (x, y, φ) is bounded by χ23,0.95 = 7.8147. Fig. 14 (left) shows the NEES values calculated for the relative localization of the observed robots where all of the values

φ e-error (rad)

ˆ i,k ] is the estimation error, Si,k ∈ Rnx ×nx where [xi,k − x is the estimation error covariance matrix, and Si,k is a scalar value that follows Chi-square distribution with dimension nx . In order for the estimator to be consistent under a single run, the following inequality should hold: Sk ≤ χ2nx ,δ

Rc1 Estimation Error Rc2 Estimation Error

0.1

Inter Robot Distance (m)

ˆ i,k ]T Si,k [xi,k − x ˆ i,k ] Si,k = [xi,k − x

y e-error (m)

mation from the first iteration, i.e. the relative localization accuracy has been observed to be within ∼ 15 (cm) for the relative position estimation and within ∼ 0.23 (rad) for the relative orientation estimation. Additionally, the consistency of MHE estimation has been evaluated by computing the Normalized Estimation Error Squared (NEES), i.e. Si,k , via (17) for all k ∈ N0 and i ∈ {1, 2}, see, e.g. [9], for details.

x e-error (m)

14

1

2

3

0

2

2

1

1

0

0

-1

-1

-2

-2

0

1

2

x-position (m)

3

4

1

2

are within the 95% acceptable region; thus, a consistent behaviour of the MHE estimator was demonstrated.

3

Rc1 Actual Position Rc2 Actual Position Reference

0

1

2

3

4

x-position (m)

Fig. 12 Experimental performance of Algorithm 1: relative localization results (top), relative tracking results (bottom). Left column subfigures show the performance during the time interval 0 to 30 (seconds) while right column subfigures are for the time interval 30.1 to 65 (seconds). The Observing robot is highlighted by a black color-filler triangle at the origin. Subfigures shown at the bottom highlight snapshots taken for the observed robots, at the same time instants, and illustrated by arrows with dotted circles indicating their minimum radius rc ; the first snapshot for each subfigure is highlighted by a black color-filled arrow.

Fig. 15 shows the relative tracking errors of the observed robots in terms of the Euclidean relative position, and the relative orientation; the steady state relative tracking errors were observed to be within ∼ 11 (cm) for the relative position and ∼ 0.12 (rad) for the relative orientation. In addition, it can be seen that during the obstacle avoidance period (highlighted in gray), the two robots deviated largely from their references. This period is again highlighted in Fig. 16 which shows the observed robots’ speeds deviation from their references. Indeed, these deviations were necessary to satisfy the inequality constraints (11) and (12) which are visualized in Fig. 14 (right). As can be noticed in Fig. 16, the speed vx,1 went slightly beyond its saturation limit (0.6 m/s). This is due an error in the internal low-level control loop (speed control loop) of the Pioneer robot which applies the sent control commands from the highlevel controller, i.e. NMPC.

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

Rc1 Rc2

0.6

0.8

Tracking Error(rad)

Tracking Error(m)

1

0.6 0.4 0.2

0.4 0.2 0 -0.2 -0.4

15

the real-time experiment. Moreover, by inspecting the computational times needed in the experiment, see Table 5, we notice that there is already allowance for additional unforeseen delays due to computations and/or communication. In summary, Algorithm 1 was demonstrated to be practically successful.

-0.6

0

-0.8 0

20

40

60

0

20

Experiment Time (sec.)

40

60

Experiment Time (sec.)

Robot Speeds (m/s), (rad/s)

Fig. 15 Relative trajectory tracking errors of the observed robots. Collision avoidance period is highlighted in gray. 0.8

vx,1 (m/s) ωz,1 (rad/s) Reference

0.6 0.4 0.2

0.8

vx,2 (m/s) ωz,2 (rad/s) Reference

0.6 0.4 0.2

0

0

-0.2

-0.2

-0.4

-0.4

-0.6

-0.6 0

20

40

60

Experiment Time (sec.)

0

20

40

60

Experiment Time (sec.)

Fig. 16 Speeds of the observed robots. Collision avoidance period is highlighted in gray.

Algorithm 1 computational times needed in the experiments are summarized in Table 5. As can be noticed, the feedback control actions, i.e. µi (ˆ xi ), i ∈ {1, 2}, were always ready to be sent to the observed robots speed controllers within a maximum time of ∼ 10.4 (mil˜i, i ∈ liseconds) from the time the relative measurement y {1, 2} were available. Indeed, there is a number of factors controlling the choice the sampling rate in the adopted algorithm. First, the computational demand of the adopted algorithm that’s directly related to the nonlinearities of the motion and measurement models as well as the number of robots considered in the system. Second, the delays in transmission of control commands and ground truth data of the adopted communication network. Finally, the processing power of the used computing means. For the considered experiment, a sampling frequency of 10 Hz was observed to be adequate to demonstrate that the adopted algorithm, i.e. Algorithm 1, is successful in Table 5 Algorithm 1 computational times in experiments. Computational Time

Rc1

Rc2

1.8 1.6

2.2 1.6

MHE

maximum (ms) average (ms)

NMPC

maximum (ms) average (ms)

6.4 4.4

Total

maximum (ms) average (ms)

10.4 7.6

6 Conclusions and Future Work In this research, we utilized an optimization based estimation and control algorithm using MHE and NMPC for relative localization and relative trajectory tracking control in multi-robots systems. Although MHE and NMPC are known in the literature to be computationally intense, we adopted an auto-generated C-code implementing fast and efficient algorithms based on a Real Time Iteration (RTI) scheme. The used algorithm is evaluated through a series of simulations and realtime experiments. Firstly, a comparison between MHE and EKF has been conducted. This comparison showed the main advantages of the adopted relative localization scheme, which lie in the high accuracy and fast convergence of MHE. Secondly, the algorithm has been evaluated on an MRS mission and showed acceptable performance in fulfilling the mission requirements, as well as the real-time implementation requirements. Finally, an experimental evaluation of the presented algorithm has been conducted in which the practical requirements for estimation, control, and computations have been fulfilled. To the best of the authors’ knowledge, this study is the first attempt that combines fast MHE and NMPC to achieve relative localization and tracking control in multi-robot systems with real-time demonstration. The estimation-control design is applied here in a centralized fashion in which the algorithm was shown to be well applicable–in terms of performance measures and real-time requirements–to systems with small numbers of observed robots. Although the centralized implementation has a limited applicability to systems with larger number of robots, this study provides benchmark results which can be used to verify decentralized/ distributed implementations, which are upper bounded by the centralized performance, see, e.g. [25]. Moreover, when considering decentralized/ distributed implementations the computational power of the observed robots has to be improved so that the computational effort is distributed among them–a requirement that is avoided in this study. While the simulations and experimental results always show convergence of the observed robots to their references, the stability of the NMPC controller needs to be further analyzed. To this end, our recent studies

16

on stability of NMPC without stabilizing constraints or costs for nonholonomic robots will be extended to the MRS control problem presented here, see [52, 53], for more details. Future work of this study includes also the experimental implementation of the proposed algorithm to MRS adopting aerial robots in addition to ground robots. Furthermore, independent range and bearing measuring sensors will be also used. Acknowledgment This work is supported by Natural Sciences and Engineering Research Council of Canada (NSERC), the Research and Development Corporation (RDC), C-CORE J.I. Clark Chair, and Memorial University of Newfoundland. References 1. Acevedo, J., Arrue, B., Maza, I., Ollero, A.: Cooperative large area surveillance with a team of aerial mobile robots for long endurance missions. Journal of Intelligent and Robotic Systems 70(1-4), 329–345 (2013) 2. Acevedo, J., Arrue, B., Maza, I., Ollero, A.: Distributed approach for coverage and patrolling missions with a team of heterogeneous aerial robots under communication constraints. International Journal of Advanced Robotic Systems 10(28), 1–13 (2013) 3. Adept mobilerobots: Pioneer-3at operation manual. www.mobilerobots.com/ResearchRobots/P3AT.aspx. Accessed: 2016-03-18 4. Alessandri, A., Awawdeh, M.: Moving-horizon estimation for discrete-time linear systems with measurements subject to outliers. In: Decision and Control (CDC), 2014 IEEE 53rd Annual Conference on, pp. 2591–2596 (2014) 5. Antonelli, G., Arrichiello, F., Chiaverini, S.: Flocking for multi-robot systems via the null-space-based behavioral control. In: Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on, pp. 1409– 1414 (2008) 6. Awawdeh, M.J.: Moving-horizon estimation for outliers detection and data mining applications. Ph.D. thesis, Universita degli Studi di Genova (2015) 7. Balch, T., Arkin, R.: Behavior-based formation control for multirobot teams. Robotics and Automation, IEEE Transactions on 14(6), 926–939 (1998) 8. Balch, T., Hybinette, M.: Social potentials for scalable multi-robot formations. In: Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE International Conference on, vol. 1, pp. 73–80 vol.1 (2000) 9. Bar-Shalom, Y., Kirubarajan, T., Li, X.R.: Estimation with Applications to Tracking and Navigation. John Wiley & Sons, Inc., New York, NY, USA (2002) 10. Beard, R., McLain, T., Nelson, D., Kingston, D., Johanson, D.: Decentralized cooperative aerial surveillance using fixed-wing miniature uavs. Proceedings of the IEEE 94(7), 1306–1324 (2006) 11. Bernard, M., Kondak, K., Maza, I., Ollero, A.: Autonomous transportation and deployment with aerial robots for search and rescue missions. Journal of Field Robotics 28(6), 914–931 (2011)

Mohamed W. Mehrez et al. 12. Carlone, L., Kaouk Ng, M., Du, J., Bona, B., Indri, M.: Simultaneous localization and mapping using raoblackwellized particle filters in multi robot systems. Journal of Intelligent and Robotic Systems 63(2), 283–307 (2011) 13. Cognetti, M., Stegagno, P., Franchi, A., Oriolo, G., Bulthoff, H.: 3-d mutual localization with anonymous bearing measurements. In: Robotics and Automation (ICRA), 2012 IEEE International Conference on, pp. 791–798 (2012) 14. De Silva, O., Mann, G., Gosine, R.: Development of a relative localization scheme for ground-aerial multi-robot systems. In: Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on, pp. 870– 875 (2012) 15. Diehl, M., Bock, H., Schlder, J.P., Findeisen, R., Nagy, Z., Allgwer, F.: Real-time optimization and nonlinear model predictive control of processes governed by differentialalgebraic equations. Journal of Process Control 12(4), 577 – 585 (2002) 16. Dunbar, W., Murray, R.: Model predictive control of coordinated multi-vehicle formations. In: Decision and Control, 2002, Proceedings of the 41st IEEE Conference on, vol. 4, pp. 4631–4636 vol.4 (2002) 17. Fenwick, J., Newman, P., Leonard, J.: Cooperative concurrent mapping and localization. In: Robotics and Automation, 2002. Proceedings. ICRA ’02. IEEE International Conference on, vol. 2, pp. 1810–1817 (2002) 18. Ferreau, H., Kraus, T., Vukov, M., Saeys, W., Diehl, M.: High-speed moving horizon estimation based on automatic code generation. In: Decision and Control (CDC), 2012 IEEE 51st Annual Conference on, pp. 687–692 (2012) 19. Gr¨ une, L., Pannek, J.: Nonlinear Model Predictive Control: Theory and Algorithms. Communications and Control Engineering. Springer London Dordrecht Heidelberg New York (2011) 20. Gu, D., Hu, H.: A model predictive controller for robots to follow a virtual leader. Robotica 27, 905–913 (2009) 21. Hazon, N., Mieli, F., Kaminka, G.: Towards robust online multi-robot coverage. In: Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International Conference on, pp. 1710–1715 (2006) 22. Houska, B., Ferreau, H., Diehl, M.: ACADO Toolkit – An Open Source Framework for Automatic Control and Dynamic Optimization. Optimal Control Applications and Methods 32(3), 298–312 (2011) 23. Houska, B., Ferreau, H.J., Diehl, M.: An auto-generated real-time iteration algorithm for nonlinear {MPC} in the microsecond range. Automatica 47(10), 2279 – 2285 (2011) 24. Hsieh, M.A., Cowley, A., Keller, J.F., Chaimowicz, L., Grocholsky, B., Kumar, V., Taylor, C.J., Endo, Y., Arkin, R.C., Jung, B., Wolf, D.F., Sukhatme, G.S., MacKenzie, D.C.: Adaptive teams of autonomous aerial and ground robots for situational awareness. Journal of Field Robotics 24(11-12), 991–1014 (2007) 25. Kanjanawanishkul, K.: Coordinated path following control and formation control of mobile robots. Ph.D. thesis, University of T¨ ubingen (2010) 26. Kayacan, E., Kayacan, E., Ramon, H., Saeys, W.: Distributed nonlinear model predictive control of an autonomous tractortrailer system. Mechatronics 24(8), 926 – 933 (2014) 27. Kayacan, E., Kayacan, E., Ramon, H., Saeys, W.: Learning in centralized nonlinear model predictive control: Application to an autonomous tractor-trailer system. Con-

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

28.

29.

30.

31.

32.

33.

34.

35.

36.

37.

38.

39.

40.

41.

42.

trol Systems Technology, IEEE Transactions on 23(1), 197–205 (2015) Kayacan, E., Kayacan, E., Ramon, H., Saeys, W.: Robust tube-based decentralized nonlinear model predictive control of an autonomous tractor-trailer system. Mechatronics, IEEE/ASME Transactions on 20(1), 447–456 (2015) Kingston, D., Beard, R., Holt, R.: Decentralized perimeter surveillance using a team of uavs. Robotics, IEEE Transactions on 24(6), 1394–1404 (2008) Lewis, M., Tan, K.H.: High precision formation control of mobile robots using virtual structures. Autonomous Robots 4(4), 387–403 (1997) Li, X., Xiao, J., Cai, Z.: Backstepping based multiple mobile robots formation control. In: Intelligent Robots and Systems, 2005. (IROS 2005). 2005 IEEE/RSJ International Conference on, pp. 887–892 (2005) Mariottini, G., Morbidi, F., Prattichizzo, D., Pappas, G., Daniilidis, K.: Leader-follower formations: Uncalibrated vision-based localization and control. In: Robotics and Automation, 2007 IEEE International Conference on, pp. 2403–2408 (2007) Maza, I., Ollero, A.: Multiple uav cooperative searching operation using polygon area decomposition and efficient coverage algorithms. In: R. Alami, R. Chatila, H. Asama (eds.) Distributed Autonomous Robotic Systems 6, pp. 221–230. Birkhuser Basel (2007) Mehrez, M.W., Mann, G.K., Gosine, R.G.: Stabilizing nmpc of wheeled mobile robots using open-source realtime software. In: Advanced Robotics (ICAR), 2013 16th International Conference on, pp. 1–6 (2013) Mehrez, M.W., Mann, G.K., Gosine, R.G.: Formation stabilization of nonholonomic robots using nonlinear model predictive control. In: Electrical and Computer Engineering (CCECE), 2014 IEEE 27th Canadian Conference on, pp. 1–6 (2014) Mehrez, M.W., Mann, G.K., Gosine, R.G.: Nonlinear moving horizon state estimation for multi-robot relative localization. In: Electrical and Computer Engineering (CCECE), 2014 IEEE 27th Canadian Conference on, pp. 1–5 (2014) Nascimento, T.P., Moreira, A.P., Conceio, A.G.S.: Multirobot nonlinear model predictive formation control: Moving target and target absence. Robotics and Autonomous Systems 61(12), 1502 – 1515 (2013) Pathiranage, C., Watanabe, K., Jayasekara, B., Izumi, K.: Simultaneous localization and mapping: A pseudolinear kalman filter (plkf) approach. In: Information and Automation for Sustainability, 2008. ICIAFS 2008. 4th International Conference on, pp. 61–66 (2008) Rao, C., Rawlings, J.: Nonlinear moving horizon state estimation. In: F. Allgwer, A. Zheng (eds.) Nonlinear Model Predictive Control, Progress in Systems and Control Theory, vol. 26, pp. 45–69. Birkhuser Basel (2000) Rawlings, J.B., Bakshi, B.R.: Particle filtering and moving horizon estimation. Computers and Chemical Engineering 30(1012), 1529 – 1541 (2006) Rivard, F., Bisson, J., Michaud, F., Letourneau, D.: Ultrasonic relative positioning for multi-robot systems. In: Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on, pp. 323–328 (2008) Saffarian, M., Fahimi, F.: Non-iterative nonlinear model predictive approach applied to the control of helicopters group formation. Robotics and Autonomous Systems 57(67), 749 – 757 (2009)

17

43. Sanchez, J., Fierro, R.: Sliding mode control for robot formations. In: Intelligent Control. 2003 IEEE International Symposium on, pp. 438–443 (2003) 44. Silva, O.D., Mann, G.K.I., Gosine, R.G.: An ultrasonic and vision-based relative positioning sensor for multirobot localization. IEEE Sensors Journal 15(3), 1716– 1726 (2015) 45. Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The MIT Press (2005) 46. Trawny, N., Zhou, X., Zhou, K., Roumeliotis, S.: Interrobot transformations in 3-d. Robotics, IEEE Transactions on 26(2), 226–243 (2010) 47. Vukov, M., Van Loock, W., Houska, B., Ferreau, H., Swevers, J., Diehl, M.: Experimental validation of nonlinear mpc on an overhead crane using automatic code generation. In: American Control Conference (ACC), 2012, pp. 6264–6269 (2012) 48. Wanasinghe, T., I. Mann, G., Gosine, R.: Relative localization approach for combined aerial and ground robotic system. Journal of Intelligent and Robotic Systems 77(1), 113–133 (2015) 49. Wanasinghe, T., Mann, G., Gosine, R.: Pseudo-linear measurement approach for heterogeneous multi-robot relative localization. In: Advanced Robotics (ICAR), 2013 16th International Conference on, pp. 1–6 (2013) 50. Wanasinghe, T., Mann, G., Gosine, R.: Distributed leader-assistive localization method for a heterogeneous multirobotic system. Automation Science and Engineering, IEEE Transactions on 12(3), 795–809 (2015) 51. Wang, S., Chen, L., Gu, D., Hu, H.: An optimization based moving horizon estimation with application to localization of autonomous underwater vehicles. Robotics and Autonomous Systems 62(10), 1581 – 1596 (2014) 52. Worthmann, K., Mehrez, M.W., Zanon, M., Mann, G.K., Gosine, R.G., Diehl, M.: Regulation of differential drive robots using continuous time mpc without stabilizing constraints or costs. In: Proceedings of the 5th IFAC Conference on Nonlinear Model Predictive Control (NPMC15), Sevilla, Spain, pp. 129–135 (2015) 53. Worthmann, K., Mehrez, M.W., Zanon, M., Mann, G.K.I., Gosine, R.G., Diehl, M.: Model predictive control of nonholonomic mobile robots without stabilizing constraints and costs. IEEE Transactions on Control Systems Technology 24(4), 1394–1406 (2016) 54. Xingxi, S., Tiesheng, W., Bo, H., Chunxia, Z.: Cooperative multi-robot localization based on distributed ukf. In: Computer Science and Information Technology (ICCSIT), 2010 3rd IEEE International Conference on, vol. 6, pp. 590–593 (2010) 55. Zanon, M., Frasch, J., Diehl, M.: Nonlinear moving horizon estimation for combined state and friction coefficient estimation in autonomous driving. In: Control Conference (ECC), 2013 European, pp. 4130–4135 (2013) 56. Zanon, M., Gros, S., Diehl, M.: Rotational start-up of tethered airplanes based on nonlinear mpc and mhe. In: Control Conference (ECC), 2013 European, pp. 1023– 1028 (2013) 57. Zanon, M., Horn, G., Gros, S., Diehl, M.: Control of dualairfoil airborne wind energy systems based on nonlinear mpc and mhe. In: Control Conference (ECC), 2014 European, pp. 1801–1806 (2014)