Nonlinear Formation Control of Unmanned Ground Vehicles

Contents

5.1 Introduction . . . . 85 5.2 Neural Network Based Adaptive Formation Control . . . . 88 5.2.1 Problem Statement . . . . 88 5.2.2 Adaptive Controller Design . . . . 90 5.2.3 Simulation Results . . . . 95 5.3 Formation Control of Constrained Multibody Systems . . . . 97 5.3.1 Fully Actuated Control . . . . 98 5.3.2 Formation Specifications and Topology . . . 100 5.3.3 Multibody Approach for Underactuated UGVs . . . 101 5.3.4 Simulation Results . . . 104 5.4 Summary . . . 106

5.1 Introduction

The importance and usefulness of moving in formation in order to fulfill a specific task is unquestionable. Various examples can be found in nature where the movement of each individual separately is underperformed as opposed to proceeding in formation: geese fly in the well-known V-shape to reduce the overall energy loss; preys tend to move as a herd to protect their young ones as well as to be less vulnerable to predators, etc. Clearly, deviating from the formation is punished and hence it is to be avoided whenever possible.

When human interaction is not needed or impossible yet a strict formation is necessary, unmanned vehicles can be used, let them be ground (UGV), aerial (UAV), surface (USV), marine (UMV) or even underwater (UUV) vehicles. Formation control of unmanned (autonomous) vehicles has obtained great importance over the past few years. It is due to the fact that many practical applications require a group of autonomous vehicles to follow a prescribed trajectory while maintaining a desired spatial configuration. Moving in formation has many advantages over single-vehicle systems, namely, increased robustness, more flexibility, efficient energy consumption and redundancy against failures can be achieved [115,116,117].

Regarding the type of control, it can be either centralized or decentralized. Centralized systems have the advantage of having only one overseer (the "master"), that holds informa-tion about the entire system, and it computes the instrucinforma-tions for others (the "slaves"). This way the slaves do not need to have such high computational power, they only need to be able to follow instructions. Decentralized systems on the other hand treat each vehicle equally, meaning that there is no distinguished instructor and every vehicle would react the same under similar conditions. This can be advantageous if storing or gathering information of the entire system is unnecessary, impractical or not realizable.

Further categorization of the type of formation control exists. Behavior-based systems make predictions of the surrounding vehicles’ movement based on their current state and possible behavior [19]. Related to this, the game theory approach of the problem is also well presented where each individual strives to achieve a state with the greatest benefit [118].

Another type of control includes consensus where the information state of the vehicles is updated on the basis of their local neighbors while the final information state of each vehicle converges to a common value [119]. Swarm or fleet control is used for handling large amounts vehicles [120,121]. Virtual leaders can be used in a decentralized system to mimic the presence of the leader [122,123]. The leader-follower strategy is also widely researched [115,124,125] which is preferred due to its simplicity and comprehensiveness.

As its name suggests, the followers follow a dedicated leader in the configuration.

Several application of formation control have already been implemented in various fields. Fuel replenishment of moving vehicles (e.g. aircrafts or ships) requires a stable and fixed position between tanker and vehicle, regardless of environmental effects. Formation flight control of constraint multi-body system [126] presents an approach where the aircraft model is of point-mass type and only position distance constraints are considered. Container ships traveling on icy seas need to closely follow an icebreaker, otherwise the water may refreeze and the container ship would stuck in place. The multi-body interpretation of the formation was successfully applied in the formation control of full-actuated surface ships [127]. Synchronized path following based on the fusion of backstepping control and passivity theory was suggested for surface ships [128]. Satellites have to orbit the planet in a predefined formation in order to provide the most coverage. A spacecraft formation controller is presented in [129] that provides less tracking error and less fuel consumption with regards to other control methods despite model inaccuracies and external disturbance.

In warehouses autonomous robots collect items based on customers’ orders which greatly reduces ordering times and delivery costs. Also, automated highway systems is a highly pursued goal that can have a great impact on safety, cost-effectiveness, and comfort. For stabilization of ground vehicles (robots) in formation the fusion of potential field method, passivity theory, dynamic inversion and LMI technique is a theoretically well founded approach if the inertia of the car-like vehicles has to be taken into consideration [130,38].

Optimal formation control for flexible driving conditions, such as narrow passages, is proposed in [131]. Control parameters are derived from car-like mobile robot’s dynamics to maintain a desired formation in a leader-follower configuration based on Lyapunov-function approach by incorporating adaptive neural network for estimating model uncertainties [132].

Naturally, these advantages attract problems with themselves. Most of the problems, however, arise from the nature of the control and not from the architecture. A well-functioning control system must be able to deal with the following problems: time-delays

5.1. Introduction 87

in the communication; disturbances and not-modeled dynamics; accurate state estimation;

collisions avoidance between two vehicles; the stability of the formation; controllability considerations due to underactuation. Several methods have been developed to overcome the mentioned issues. An appropriate Lyapunov-Krasovskii function was used to eliminate the unknown time delay while signals in the closed loop system being semi-globally uniformly ultimately bounded (SGUUB) [133]. One potential candidate for tackling disturbances is to employ feedforward neural networks (FNNs) as they represent a highly configurable tool for uncertainty estimation [14].

Two nonlinear approaches are proposed for the formation control of unmanned ground vehicles as the contribution of this work. First, an adaptive control is introduced that uses leader-follower approach using the kinematics of the vehicle and taking into consideration the model uncertainties based on earlier results for surface ships [134]. The underactuated control utilizes kinematic model and as such it is able to compute a reference track based on executable maneuver using the elaborated vehicle control system presented in Chapter 4. Furthermore, follower vehicles do not need to know the leader’s velocity, as it will be approximated by the followers, while a relative formation to the leader is maintained throughout the control process. Because of this, the intervehicle information exchange is reduced in the decentralized system which is desirable for non-ideal communication channel with high latency. The controller employs single hidden layer FNNs for compensating the nonlinear uncertainties whose parameters are updated in an online adaptive fashion such that the error signals in the closed loop system remain SGUUB. This is a sensible objective, since asymptotic stability may be too strong to achieve in the presence of uncertainties and disturbances. However, due to these uncertainties, stability issues may arise. By carefully choosing the control parameters, the ultimate bound can be decreased and thus the tracking error can be reduced. Another advantage of the proposed approach is its applicability to deal with time-varying high-speed velocity and overlapping time-varying orientation, making it possible to implement in fast systems (e.g. highway control). In addition to this, the structure of highways intends to minimize side-slip angle, however, this effect is non-negligible due to high velocities. As opposed to the previous work, the proposed modification will be evaluated accounting for the side slip angle as it will be illustrated by simulation results.

Second, only the kinematic model is utilized for formation control purposes until this point. Ground vehicles are underactuated and in many cases their mass/inertia cannot be neglected, thus the previous control design has to be modified to include vehicle dynam-ics. Due to these reasons, a formation control based on constraint forces and multibody interpretation is developed for the underactuated linear parameter varying (LPV) model of UGVs. The approach is based on well founded analytical principles of mechanics and the coordinated motion is achieved by introducing holonomic constraint functions that describe the interaction between the vehicles. These functions are interpreted as constraint forces that establish the coordinated movement at any time. The decentralized control law for each vehicle can be determined by Lagarange multiplier method. The motivation of the work is to elaborate the necessary modification of the theory for taking into account the underaction property of the UGV.

5.2 Neural Network Based Adaptive Formation Control

In this section a neural network based adaptive formation contol design is presented that is based on the kinematic model of the vehicle. At first, the formation control objective is formulated, then followed by the derivation of the adaptive formation control law. Finally, the tracking performance of the developed controller are investigated for a high-speed maneuver similar to the one presented in Section4.4.1.

5.2.1 Problem Statement Leader-Follower Structure

Consider a group ofi=1, . . . ,nunderactuated unmanned ground vehicles (UGVs) follow-ing a leader (L) vehicle in the horizontal plane given with velocity-controlled kinematics equation of

η˙_i=

R(ψi) 0

0 1

ν_i, R(ψ_i)=

cosψi −sinψi

sinψ_i cosψ_i

(5.1) whereηi =

x_i,yi, ψiT

∈R³is the pose of theith vehicle expressed in the reference frame andν_i =[u_i,vi,r_i]^T ∈R³contains the body-fixed linear and angular velocities. The control inputs of the vehicles are the velocities and the outputs are the position and orientation.

x , xi yi

ui ϕi

ϕiL

, xL yL

ψL

ψi

diL

Figure 5.1: Leader-follower configuration on thex–yplane

The geometry of the leader-follower configuration is presented in Fig. 5.1 where d_{i L} ∈R⁺is the distance andφ_{i L} ∈ (−π, π] is the relative bearing angle between the leader and theith follower formulated by

d_{i L} =q

(x_L−x_i)²+(y_L−y_i)² (5.2) φ_{i L} =arctan y_L −y_i

x_L −x_i

−ψ_L ,γ_{i L}−ψ_L (5.3)

Define φi , γi L −ψi and denote with fi ,

d_{i L}, φi LT

the formation specification vector, then its time derivative can be expressed by the body-fixed velocities using (5.1) and

5.2. Neural Network Based Adaptive Formation Control 89

trigonometric identities as follows:

f˙_i = B_ic_i+∆_i, B_i=−diag 1, 1 d_{i L}

, (5.4a)

ci =R^T(φi)

u_i v_i

, ∆_i=−B_iR^T(φi L)

u_L v_L

−

0 r_L

(5.4b) In order to facilitate the control objective and design, the following assumptions are made.

Assumption 5.1. The follower vehicles are underactuated in the sense that the lateral velocityvicannot be controlled directly, i.e. νi =[u_i,0,r_i]^T.

Assumption 5.2. Each vehicle can perfectly measure its own position and orientation. The leader’s pose, i.e. x_L, yL,ψ_L and therefore f_i = [d_{i L}, φ_{i L}]^T are available for formation control.

Assumption 5.3. The leader tracks a smooth and bounded reference path. The leader’s velocityν_L =[u_L,v_L,r_L]^T, and thus the nonlinear term (∆_i), are unknown to the followers.

Taking into consideration the underactuated property, it yields from (5.4b) that c¯_i= R^T(φ¯_i)

u¯_i 0

cos ¯φ_i

−sin ¯φ_i

u¯_i (5.5)

thus, choosing an appropriate control law ¯c_i = [ ¯c_i₁,c¯_i₂]^T for vehicle{i}, the longitudinal velocity and orientation can be determined according to

u¯_i ψ¯_i

= 



 q

c¯²_i

1+c¯²_i

atan2(c¯_i₂,c¯_i₁)+γ_{i L}







(5.6) These high-level signals ¯u_i and ¯ψ_i can be considered as reference signals for a lower level control system similar to the design presented in Section4.3.2.

Control Objective

Each follower vehicle must maintain a desired position relative to the leader while it only has knowledge about the leader’s position and orientation. Before establishing the control objective, first, introduce the tracking error as

e_i = f_id− f_i (5.7)

where f_id ∈R²is the desired formation specification prescribing the relative distance and bearing angle between the follower and the leader vehicle. Taking its derivative with respect to time yields

e˙i(t) = f˙id−Bici−∆_i (5.8) The control objective is to find the control input for the followers that for any ε > 0 ensureske_i(t)k ≤ εfor∀t > t₀, i.e. bounded tracking of the leader vehicle is required by satisfying the prescribed formation specification with imperfect knowledge of the leader’s actual states, and by guaranteeing that the tracking error can be made small.

Remark 5.1. The lateral velocity of the leader v_L does not necessarily have to be zero.

Generally, this implies that reference motions having nonzero side slip angles can be pre-scribed to the leader vehicle. From an application point of view, together with Assumption 5.3, this is more advantageous since a higher level of decentralization can be obtained with reduced communication effort between the vehicles.

Remark 5.2. Note that if∆_iwas known, asymptotic stability in sense of Lyapunov could be achieved for the closed loop error dynamics. It is easy to verify that by choosing

c_i= B⁻_i¹f

f˙_id+ke−∆_ig

, k >0 (5.9)

the closed loop system(5.8)is rendered asymptotic stable. On the other hand, by Assumption 5.3, the nonlinear function∆_ihave to be approximated.

5.2.2 Adaptive Controller Design Unknown Function Approximation

Multilayer neural network (MNN) is one of the most widely used neural network in modeling and control of nonlinear systems due to its universal function approximation property.

MNNs are static feedforward networks that can be described by several layers such as input, output and hidden layer. Each layer consists of a number of nodes (neurons) and each node in that layer is connected to all nodes of the following layer. Usually, at each neuron, the weighted sum of the incoming connections with a bias term is propagated through a nonlinear function, and the result is fed forward to the next layer. Clearly, a change in a single parameter at any layer will generally affect all the outputs in the following layers.

Consider now a single hidden layer (SHL) network according to (2.26) with the notation described in Section 2.2.2. In order to employ neural network approximators for control purposes, theunknownweight matrices (V,W) have to be estimated and also adapted in real-time such that the estimation error does not violate the stability of the closed loop system.

Before introducing the adaptation laws, some properties of the weight approximation is presented [14,13].

Define the weight estimation errors as

V˜ =Vˆ −V, W˜ =Wˆ −W, (5.10a)

σ˜ =σ−σˆ ,σ(V^Tx)¯ −σ(Vˆ^Tx)¯ (5.10b) where ˆV, ˆW are the estimations of the ideal V andW weights provided by the weight adaptation laws. The Taylor series expansion of the hidden-layer output about ˆV^Tx¯ can be given by

σ(V^Tx¯)= σ(Vˆ^Tx¯)+σ⁰(Vˆ^Tx¯)V˜^Tx¯+O(V˜^Tx)¯ ² (5.11) where O(V˜^Tx¯)²denotes the second order error terms and the derivativeσ⁰(ˆz) = ^dσ_dz |_z_=ˆ_z is determined by

σ⁰(Vˆ^Tx)¯ =







0 . . . 0

σ⁰

1(vˆ^T

1x)¯ . . . 0

... . . . ...

0 . . . σ⁰_N

2(vˆ^T_N

2x¯)







∈R^N²⁺¹^×N² (5.12)

5.2. Neural Network Based Adaptive Formation Control 91

Consequently, the function approximation error can be written as fˆ(x)− f(x)=Wˆ^Tσ(Vˆ^Tx)¯ −W^Tσ(V^Tx)¯

=W˜^T(σˆ −σˆ⁰Vˆ^Tx)¯ +Wˆ^Tσˆ⁰V˜^Tx¯ +ρ (5.13) with higher-order residual terms of

ρ=W˜^Tσ⁰Vˆ^Tx¯−W^TO(V˜^Tx)¯ ²

=−W^T(σ−σˆ)−W^Tσˆ⁰Vˆ^Tx¯+Wˆ^Tσˆ⁰V^Tx¯ (5.14) where ˆσ⁰ ,σ⁰(Vˆ^Tx)¯ is used for shorthand.

Lemma 5.1. [14]. The residual term(5.14)with sigmoid activation function can be upper bounded by

kρk ≤ kVk_Fkx¯Wˆ^Tσˆ⁰k_F +kWk kσˆ⁰Vˆ^Tx¯k+αkWk (5.15) whereα=√

N₂+1.

Proof. Using properties of norms (2-norm and Frobenius norm) and considering Wˆ^Tσˆ⁰V^Tx¯ =tr V^Tx¯Wˆ^Tσ⁰

≤ kVk_Fkx¯Wˆ^Tσ⁰k_F (5.16) yields

kρk ≤ kW^T(σ−σˆ)k+kW^Tσˆ⁰Vˆ^Tx¯k+kWˆ^Tσˆ⁰V^Tx¯k

≤ kWk kσ−σˆk+kWk kσˆ⁰Vˆ^Tx¯k+ktr V^Tx¯Wˆ^Tσ⁰ k

≤ αkWk+kWk kσˆ⁰Vˆ^Tx¯k+kVk_Fkx¯Wˆ^Tσˆ⁰k_F

Since σ ∈ R^N²⁺¹ and for each activation function σ_j(·) ∈ [0,1], therefore kσ− σˆk ≤

√N₂+1.

Lemma 5.2. The following identity withV˜ =Vˆ −V is true:

−tr ˜V^TVˆ= 1

2kVk²_F− 1 2

kV˜k²_F− 1 2

kVˆk_F² (5.17)

Proof. Substituting for ˜V^T and usingkAk²_F =tr A^TA =tr AA^T

, it follows

−tr ˜V^TVˆ=−1 2

kVˆk_F² − 1

2tr ˆV^TVˆ+1

2tr ˆV^TV+ 1

2tr V^TVˆ Adding and substracting the term ¹₂tr V^TV

yields the identity.

Stability Analysis

The leader-follower formation control algorithm is derived using Lyapunov-theory and em-ploys adaptive neural network with online weight tuning laws to approximate the unknown nonlinear function∆_i(t)of the leader’s velocity. The decentralized controller design follows [134] and the results are given by the following theorem.

Theorem 5.1. For vehicle {i} described by kinematics (5.1) with Assumptions 5.1–5.3 satisfied, let the control law be

c¯i= B⁻_i¹f

f˙id+k_{i N}ei−Wˆ_i^Tσ(Vˆ_i^Tx)¯ g

(5.18) and define the adaptive NN weight tuning laws as

V˙ˆ_i =−G_i

x e¯ ^T_iWˆ_i^Tσˆ⁰+k_{V i}Vˆ_i

(5.19a) W˙ˆ_i =−F_if

σˆ −σˆ⁰Vˆ_i^Tx¯

e^T_i +k_{W i}Wˆ_ig

(5.19b) whereF_i >0,G_i >0,k_{W i} >0,k_{V i} >0and the error gain is chosen as

k_{i N} = k_1i+k_2ikx¯Wˆ_i^Tσˆ⁰k²_F+k_3ikσˆ⁰Vˆ_i^Tx¯k², k_1i, k_2i, k_3i >0 (5.20) then all closed-loop signals are semi-globally uniformly ultimately bounded (SGUUB).

Proof. The proof follows [134]. Construct the Lyapunov-function candidate L_i= 1

e^T_i e_i+ 1

2tr W˜^TF⁻¹W˜+ 1

2tr ˜V^TG⁻¹V˜

(5.21) and suppress indices i onward for better readability. Differentiating (5.21) by time and using (5.8) and the fact that the idealW andV are constant matrices, henceW˙˜ = W˙ˆ and V˙˜ =V˙ˆ, it follows

L˙_i= e^T_i (f˙_id−B_ic_i−∆_i)+tr W˜^TF⁻¹W˙ˆ +tr ˜V^TG⁻¹V˙ˆ

(5.22) Each component of the unknown function∆_i can be approximated by a SHL NN to arbitrary accuracy by, thus

∆_i=W^Tσ(V^Tx)¯ +εi(x) (5.23) where||ε_i(x)|| ≤ε_ibis the bounded NN reconstruction error.

Substituting (5.18), (5.19) and (5.23) into (5.22) and using identity tr A+B =tr A+ tr B

yields

L˙_i= e^T_i f

Wˆ^Tσ(Vˆ^Tx)¯ −W^Tσ(V^Tx)¯ −ε_i(x)g

−tr W˜^T(σˆ −σˆ⁰Vˆ^Tx)¯ e^T_i

−tr ˜V^Tx e¯ ^T_iWˆ^Tσˆ⁰

−k_Vtr ˜V^TVˆ

−k_Wtr W˜^TWˆ

−k_{i N} ||ei||² (5.24) Furthermore, using tr AB =tr B A

and tr λ = λfor anyλ ∈R, it follows from (5.13) and (5.20) that

L˙_i= e^T_i ρ−ε_i(x)

−k_Vtr ˜V^TVˆ

−k_Wtr W˜^TWˆ

− ||e_i||²f

k₁+k₂kx¯Wˆ^Tσˆ⁰k²_F +k₃kσˆ⁰Vˆ^Txk¯ ²g

(5.25)

5.2. Neural Network Based Adaptive Formation Control 93

The derivative of the Lyapunov function can be upper bounded by applying (5.15) as L˙_i ≤ |e^T_i ρ|+|e^T_iεi(x)| −k_Vtr ˜V^TVˆ

−k_Wtr W˜^TWˆ

− ||e_i||²f

k₁+k₂kx¯Wˆ^Tσˆ⁰k_F² +k₃kσˆ⁰Vˆ^Txk¯ ²g

≤ keik(

kVk_Fkx¯Wˆ^Tσˆ⁰k_F +kWk kσˆ⁰Vˆ^Tx¯k+αkWk) +ke_ik kε_i(x)k −k_Vtr ˜V^TVˆ

−k_Wtr W˜^TWˆ

− ||e_i||²f

k₁+k₂kx¯Wˆ^Tσˆ⁰k_F² +k₃kσˆ⁰Vˆ^Txk¯ ²g

(5.26) Upper bounds can be given on the following terms according to Lemma5.2

−k_Vtr ˜V^TVˆ

≤ k_V 2

kVk_F² − k_V 2

kV˜k²_F

−k_Wtr W˜^TWˆ

≤ k_W

2 kWk_F² − k_W 2

kW˜ k_F² (5.27)

Furthermore, from the fact thatab ≤ ^a²⁺^b²

2 for anya,b ∈ R, it follows

√ kkak^√¹

kkbk ≤

2kak²+ ₂¹_kkbk²for anyk >0, hence the following inequalities hold keik kεi(x)k ≤ k₄

2 keik²+ 1

2k₄kεi(x)k² keik kVk_Fkx¯Wˆ^Tσˆ⁰k_F ≤ 1

4k₂kVk²_F +k₂keik²kx¯Wˆ^Tσˆ⁰k_F² ke_ik kWk kσˆ⁰Vˆ^Txk ≤¯ 1

4k₃kWk²+k₃ke_ik²kσˆ⁰Vˆ^Tx¯k² αke_ik kWk ≤ α²k₅

ke_ik²+ 1

2k₅kWk² (5.28)

Plugging inequalities (5.27) and (5.28) back into (5.26) and collecting the terms of different norms, one can obtain

L˙_i ≤ −(k₁−α²k₅ 2

− k₄

2)ke_ik²− k_V 2

kV˜k²_F− k_W 2

kW˜ k²_F + 1

2k₄kεi(x)k²+ k_V 2 + 1

4k₂

! kVk_F² +k_W

kW˜ k_F² + 1 4k₃ + 1

2k₅

kWk² (5.29)

Introducing the following notations η_i , min

(

k₁−α²k₅ 2

− k₄ 2;k_V

2 ;k_W 2

) µ_i , 1

2k₄kε_i(x)k²+ k_V 2 + 1

4k₂

kVk²_F + k_W 2

kW˜ k_F² + 1 4k₃ + 1

2k₅

! kWk² whereµ_iis bounded, then the upper bound on the Lyapunov candidate function becomes

L˙_i ≤ −η_if

ke_ik²+kV˜k_F² +kW˜ k_F²g

+ µ_i (5.30)

Consider the compact set Γ_i ,

(

(ei,V,ˆ Wˆ ) :keik²+kV˜k²_F+kW˜ k²_F ≤ µ_i η_i

)

(5.31) Clearly, ifke_ik²+kV˜k_F² +kW˜ k_F² > ^µ_ηⁱ

i, then L˙_i ≤ −ηi

keik²+kV˜k_F² +kW˜ k²_F− µi

η_i

<0 (5.32)

This means thatL_iis negative outside the compact set (5.31), i.e. any closed loop trajectory starting outsideΓ_iwill eventually reach and remain inside this set. In fact,ei(t), ˜Vi(t)and W˜_i(t) are semi-globally uniformly ultimately bounded.

Remark 5.3. The so-called σ-modification adaptation law(5.19) may be changed to e-modification law for more robust trajectory tracking by using

V˙ˆi=−Gi

x e¯ ^T_iWˆ_i^Tσˆ⁰+k_{V i}keikVˆi

(5.33) W˙ˆ_i=−F_i f

(σˆ −σˆ⁰Vˆ_i^Tx)¯ e^T_i +k_{W i}ke_ikWˆ_ig

(5.34) Further details can be found in [135,136,13].

Remark 5.4. For practical systems operating in the presence of uncertainties, SGUUB is a satisfactory objective since asymptotic stability may be too strong to achieve. The tracking performance can be improved by increasing the number of neurons in the NN hidden layer which results smaller approximation error and thus smaller Γ. Moreover, increasing the control parameters results larger η which means that Γ is reduced. On the other hand, larger µincreases the domain of attraction (semi-globally).

Implementation Strategies

The actual control input ¯u_i to system (5.1) is computed by (5.6). The yaw rate ¯r_i can be acquired by using filtered differentiation with time constantT_das follows:

r¯_i , ψ˙_{F i} = ψ¯_i−ψ_{F i}

T_d (5.35)

For practical reasons, it is important to take into account the limitations of the control efforts. Usually, the vehicles do not necessarily start close to the desired formation, thus large control signals may appear initially. A common approach is to use a command filter to influence the trajectories of the vehicles at the initial stage [137]. The following guidance law is suggested for determining a time-varying formation specification:

f˙id =T_c(fic− fid), fid(0)= fid0 (5.36) whereT_c is the time constant of the command filter and f_ic is the constant (steady-state) command of the desired relative distance and bearing angle. The output f_idcan be forced to stay close to the actual formation specification fiby properly settingT_c, thus making the error termeismall and therefore the control effort as well.

5.2. Neural Network Based Adaptive Formation Control 95

Another important consideration is the input choice and the weight initialization of the NNs. Generally, the input vectorxcan be constructed from the actual and previous control signals and system outputs [136]. For the formation controller presented in Section5.2.2, the NN inputs of vehicle{i}are selected using simulation step sizeT_sas follows:

x¯ = f

1 f_i^T(t) f_i^T(t−T_s) ψ_i(t)−ψ_L(t) c¯^T_i (t−T_s)gT

(5.37) Initializing the NN weights to zeros will not decrease the approximation performance [135]. However, from a practical point of view, it is preferred if the weight adaptation starts close to the accurate values in order to avoid large control signals at the initial stage. Thus, if feasible, a pre-initialization may be required before the actual control takes place.

5.2.3 Simulation Results

In this section the simulation results of the adaptive formation control system are presented and its effectiveness is investigated through a realistic high-speed maneuver.

A group of three follower vehicles and one leader is considered moving in thex–yplane.

The reference trajectories for the leader are generated by the 16 degree of freedom full-vehicle dynamic model using a prescribed steering rate and longitudinal velocity profile similarly as described in Chapter 4. The resulting motion consists of an overlapping acceleration/deceleration and turning phase at high speed ranging between 25-35m/s. The reference motion includes small lateral (vL < 2 m/s) velocity and therefore small side slip angle (less than 3 degrees) appears.

x(m)

0 100 200 300 400 500 600 700 800

y(m)

-200 -150 -100 -50 0

12.

trajectory reference leader desired command

t= 0.00 s 1.

t= 2.28 s 2.

t= 4.55 s 3.

t= 6.82 s 4.

t= 9.10 s 5.

t= 11.37 s 6.

t= 13.64 s 7.

t= 15.91 s 8.

t= 18.19 s 9.

t= 20.46 s 10.

t= 22.73 s 11.

t= 25.00 s 12.

Figure 5.2: Formation output trajectories (to scale)

For each vehicle, the neural network based adaptive formation controller are imple-mented separately using sigmoid activation functions in form of

σ(x) = 1

1+exp(−x) (5.38)

The number of hidden layer neurons are chosen experimentally, for satisfactory approxi-mation N₂ = 10 is sufficient. The NN weights are initially set to zeros, however, before enabling the actual control to the vehicles, the neural network adaptation process is activated for 0.5 s in order to decrease the weight errors and thus the initial control effort. During this initialization phase, all vehicles are considered to be moving with constant velocity on a straight path and the followers are orientated towards the leader.

A V-shape formation is commanded such that the first follower travelsd₁=10 m behind the leader and the rest of the vehicles are aligned in 45^◦angle andd₂=7 m distance relative to the first follower. Therefore, the stacked formation command vectors reads as

fic =

d₁ L L

0 −φ φ

, L=q

d²

1+d²

2+√

2d₁d₂≈15.75, φ=arcsin* ,

√ 2d₁ 2L +

-≈0.32 For high-speed maneuvering, simulation time T_s = 0.01 s is chosen and the filter time constants are set to T_d = 0.1 s and T_c = 0.5 s satisfying Shannon’s rule. The control parameters are selected for each vehicle as follows:

G_i =10⁻², F_i =2, k_{V i} =k_{W i}=0.3, k₁_i =3, k₂_i = k₃_i =10⁻³ (5.39) The output trajectories along with a few snapshots of the motion for different times are shown in Fig. 5.2. It can be observed that the followers are efficiently estimated the leader’s unknown velocity and the formation is well-established during the motion maneuver. This can also be verified by inspecting Fig. 5.3a where the formation specifications converge to the desired values after a short transient. Note that small lateral deviation may occur from the (real) reference trajectory obtained from the vehicle dynamics (Fig. 5.2), which can be explained by the spatial configuration of the formation and the presence ofvL > 0 due to the side slip angle.

Figure 5.3b shows the control signals for each vehicle and Fig. 5.3c demonstrates the adaptation of the unknown terms∆_i ∈ R² and their true values. Clearly, the leader’s uncertainties are efficiently estimated and compensated by the NN controller. The followers reached the commanded formation and dominant speed profiles after∼10 s. Note that the∆ terms are related to the transformed linear and angular velocities by (5.4). The longitudinal and lateral path errors from the commanded formation are illustrated in Fig. 5.3. It can be observed that all signals remain close to zero after the transient. The actual error deviations are less than±0.001 m (not shown in the figure).

In summary, the developed adaptive NN-based controller is capable of high-speed for-mation tracking in the presence of underactuation, i.e. no direct lateral velocity modification is possible, and approaching the unknown velocity information of the leader vehicle. The controller is based on leader-follower structure with kinematics equation and the resulting velocity signals can be utilized as command inputs at a lower control level. The drawback of the control architecture is that the dynamics of the vehicle are not considered. This issue and possible solutions will be addressed in the following Sections.

5.3. Formation Control of Constrained Multibody Systems 97

0 5 10 15 20 25

10 15 20

(m)

d_1L d_2L d_3L d_id

0 5 10 15 20 25

25 30 35

(m/s)

u₁ u¯₂ u¯₃ u_L

a) Convergence of formation specifications

0 5 10 15 20 25

-0.5 0 0.5

(rad)

φ1L φ2L φ3L φid

b) Control signals and reference velocity

0 5 10 15 20 25

-0.6 -0.4 -0.2 0 0.2

(rad/s)

r1 r¯2 r¯3 rL

0 5 10 15 20 25

20 30

(m/s)

NN11 NN21 NN31 ∆ⁱ¹

0 5 10 15 20 25

-2 0 2 4 6 8

x (m)

follower 1 follower 2 follower 3

t (s)

c) Approximation performance of NNs

0 5 10 15 20 25

-1 0 1

(rad/s)

NN12 NN22 NN32 ∆ⁱ²

t (s)

d) Formation x-y position errors

0 5 10 15 20 25

-10 0 10

y (m)

Figure 5.3: Closed loop signals of adaptive NN-based formation control

5.3 Formation Control of Constrained Multibody Systems

In this section, a different type of formation control design is presented. The problem is solved by constrained multibody interpretation motivated by analytical mechanics. In multibody systems, a description of the motion of each body is obtained as if the body had no restrictions on its movement in the configuration space. Naturally, the given constraints have to be fulfilled under all circumstances so the forces exerted on the bodies cannot be arbitrary. These forces, that also hold the different bodies of the system together, can be computed with the aid of the variational principle of analytical mechanics [138] and the Lagrange multiplier method. Since formation control can be regarded as a special type of constrained motion, therefore a natural idea is to use constraint forces which can be applied to the system through actuators.

Previous work of constrained multibody systems was presented in [127], where the problems of path tracking and formation are handled for fully actuated surface marine vehicles. In the sequel, the aim is to elaborate the necessary modification of the theory and illustrate its applicability for unmanned ground vehicles taking into account both underactuation and inertial properties. The decentralized control system is based on leader-follower structure and produces the necessary acceleration and steering angle inputs of the underactuated vehicles for handling realistic high-speed motion maneuvers.

5.3.1 Fully Actuated Control

Consider a group of fully actuated vehicles fori=1, . . . ,nmoving in the horizontal plane.

A single vehicle is modeled by kinematics (5.1) and dynamics equations of η˙ =







cosψ −sinψ 0 sinψ cosψ 0

0 0 1







ν, R(ψ)ν, M(η)ν˙ +n(η,η,˙ ν)=τ (5.40)

where R(ψ) is the 3 by 3 rotation matrix from body to the inertial frame,η = [x,y, ψ]^T is the vector of position and orientation and can be considered as generalized coordinates, ν =[u,v,r]^Tis the vector of the linear and angular velocities in the moving frame,M ∈R³^×³ denotes the positive definite system inertia, n ∈ R³ contains the centripetal, Coriolis, damping and gravity effects andτ ∈R³is the actuator torques expressed in the body frame.

Notice the similarity to robot control whereM(q)q¨+h(q,q)˙ = τ is the dynamic model of the robot in joint coordinates and the JacobianJ(q)plays the role of the rotation matrix according to ˙x= J(q)q˙.

In general, the motion of dynamic system (5.40) may be constrained, i.e. the motion is restricted to a subset of the configuration space. In Lagrangian mechanics, the equations of motion for a constrained system are modified as compared to the equations of unconstrained motion. These constraints exert additional forces on the system which have to be taken into account. Consider now a set of constraints functions expressed by generalized coordinates η, given in the form of

C(η) =0, C(η) ∈R^p (5.41)

and denote the constraints Jacobian with

W(η) , ∂C(η)

∂η ∈R^p×³ (5.42)

According to the results of [138], the forces that maintain kinematic constraints (5.41) add potential energy to the system. Hence, it follows that the modified motion equation becomes M(η)ν˙+n(η,η,˙ ν) =τ+τ_c, τ_c =−W(η)^Tλ (5.43) whereτ_c is the constraints force andλ ∈R^pis the Lagrange multiplier.

Expressingτin the reference frame and using properties of the rotational matrix τ_η , Rτ ⇒ τ =R^Tτ_η, R˙^T =−R^TRR˙ ^T, (5.44) the constrained system (5.43) can be transformed into the inertial frame

RM(η)R^T

| {z }

M_η>0

η¨+R

n− MR^TRR˙ ^Tη˙

| {z }

n_η(η,η,ν˙ )

=τη − RW(η)^Tλ (5.45)

The motion constraints are satisfied when there exist admissible kinematic velocity and acceleration functions in accordance with (5.41). These function can be obtained by differentiation with respect to time. Therefore, it follows fromC(η) =0that

C(η,˙ η)˙ = ∂C

∂ηη˙ =W(η)η˙ =0, (5.46)

C(η,¨ η,˙ η)¨ =W(η)η¨+W˙ (η)η˙ =0 (5.47)

In document Max György Advanced Nonlinear Control of Vehicles and Their Formations (Pldal 105-129)