Max György Advanced Nonlinear Control of Vehicles and Their Formations

(1)

Max György

Advanced Nonlinear Control of Vehicles and Their Formations

Doctoral thesis for the degree of PhD

Supervisor: Prof. emer. Dr. Lantos Béla, DSc.

Budapest University of Technology and Economics Faculty of Electrical Engineering and Informatics

Department of Control Engineering and Information Technology

Budapest, December 22, 2016

(2)

Budapest University of Technology and Economics

Doctoral thesis for the degree of PhD

Faculty of Electrical Engineering and Computer Science Department of Control Engineering and Information Technology

(3)

This thesis is dedicated to Victoria

(4)

(5)

Declaration

Undersigned, György Max, hereby state that this Ph.D. Thesis is my own work wherein I have used only the sources listed in the Bibliography. All parts taken from other works, either in a word for word citation or rewritten keeping the original contents, have been unambiguously marked by a reference to the source.

The reviews of this Ph.D. Thesis and the record of defense will be available later in the Dean Office of the Faculty of Electrical Engineering and Informatics of the Budapest University of Technology and Economics.

Nyilatkozat

Alulírott Max György kijelentem, hogy ezt a doktori értekezést magam készítettem és ab- ban csak a megadott forrásokat használtam fel. Minden olyan részt, amelyet szó szerint, vagy azonos tartalomban, de átfogalmazva más forrásból átvettem, egyértelműen, a forrás megadásával megjelöltem.

Az értekezésről készült bírálatok és a jegyzőkönyv a későbbiekben a Budapesti Műszaki és Gazdaságtudományi Egyetem Villamosmérnöki Karának Dékáni Hivatalában elérhetőek.

Budapest, December 22, 2016 Max György

(6)

(7)

Acknowledgments

First of all I would like to express my gratitude to my supervisor, Professor Béla Lantos, for his enthusiasm and exceptionally precise nature. His ability to encourage and inspire have been crucial for maintaining satisfactory progress in my work. His helpful guidance and valuable suggestions greatly improved the quality of this thesis.

I would like to thank my colleagues Dr. Bálint Kiss, Dr. Dániel Drexler, Loránd Lukács, Dr. István Harmati, Gábor Péter and Gábor Kovács at BUTE for providing a supportive working environment and for helping in my teaching related duties.

I am grateful for my family for their encouragement and support. A special thank goes to my brother Gyuszi for proof-reading this thesis in last minute.

I would also like to thank Dr. Ruth Bars and Professor József Tar for their helpful comments and suggestions that helped me to significantly increase the quality of this document.

The research was supported by the ÚNKP-16-3-III. New National Excellence Program of the Ministry of Human Capacities under project "Advanced Nonlinear Control of Au- tonomous Electric Ground Vehicles". I hereby wish to thank the program for the support.

Finally, but not least, I would like to thank my Victoria for her love and constant support in the past five years.

(8)

(9)

Összefoglaló

A földi járművek automatikus irányítása az autóipar egy nagy kihívást jelentő feladata, mely széles körben kutatott téma, és a jövő vezető nélküli járműveinek a szabályozását alapozza meg. A dolgozat célja nemlineáris autonóm jármű és járműcsoportok irányítási algoritmusainak a kifejlesztése nagy sebességű manőverek végrehajtásához.

A disszertáció első részében egy háromszintű hierarchikus szabályozórendszer kerül be- mutatásra négy-kerék meghajtású (4WD) járművek optimális pályatervezésére és irányítására bemenet illetve állapot korlátozások figyelembe vételével. A magas szintű szabályozó biz- tosítja a jármű egyszerűbb, két-kerék meghajtású modelljének globális idő-optimális és lokális idő-szuboptimális irányítását fix (ismert) pályára illetve tetszőleges (részben ismert) pályára. A beavatkozó jelek egy dinamikus, nem-lineáris optimális irányítási probléma megoldásaként állnak elő, mely közvetlen diszkretizálásra és többszörös tüzelés elvére épül, mozgó horizontú kiterjesztéssel. A középső szintű szabályozó a magas szintről kapott kerékerők legkisebb négyzetek értelmében vett optimális elosztását végzi a négykerekű jármű dinamikát figyelembe véve. Az alacsony szintű modell prediktív integráló szabály- ozó (MPC) zárt körben minimalizálja a magas szintű optimális trajektóriáktól való eltérést kezdeti állapot perturbációk jelenlétében, ezzel megvalósítva a nemlineáris 4WD jármű optimális irányítását.

A második részben egy kétszintű autonóm járműirányító rendszer kerül kidolgozásra, mely lehetővé teszi vezető nélküli földi járművek nagy sebességű (> 30 m/s) pályakövetését.

A rendszer figyelembe veszi az ismeretlen tömegközéppont okozta terhelés változásokat, amit a 16 szabadságfokú (16 DoF) többtest (multibody) járműmodell alapján számol. A jármű egy fa-struktúrájú mechanikai rendszernek tekinthető, melynek dinamikus modellje az Appell-féle robotikai formalizmus és Gibbs-függvény alkalmazásával kerül meghatározásra.

A magas szintű szabályozó a jármű kinematikai modellje alapján valamint véges horizontú, lineáris, időben változó LQ optimális irányítási stratégia mellett lokálisan stabilizálja a zárt rendszert és egyben minimalizálja a referencia jelektől való eltérést. Az állapotokat egy két-szintű Kalman Filter becsüli, mely realisztikus GPS és inerciális (IMU) szenzorjeleken illetve navigációs kinematikai differenciálegyenleteken alapul. A jármű dőlésének és bil- lenésének hatásait PID-típusú alacsony szintű szabályozók minimalizálják manőverezés közben, megválasztható hiba differenciálegyenletek segítségével. A szimulációk ígéretes eredményeket mutatnak a jármű fizikai korlátai közelében, kis oldalirányú pályaeltéréssel és agresszív manőverezés mellett.

A harmadik rész két decentralizált, vezető-követő struktúrájú, formációirányító rendszer kifejlesztését írja le alulaktuált szárazföldi járművek alakzatban való haladásához. Az első megközelítésben egy adaptív szabályozási törvény kerül kidolgozásra kinematikai egyen- letek alapján. A követő járműveknek korlátozott információ áll rendelkezésre a vezető jármű állapotairól. Az ismeretlen függvényeket, melyek a vezető ismeretlen sebesség infor- mációit tartalmazzák, neurális hálózatok becsülik online adaptív hangolási szabályokkal, így a zárt kör hibajelei garantáltan egyenletesen korlátossá tehetők. A második módszer a többtest rendszerek analitikus mechanikájára épül. A formáció koordinált mozgása korlá- tozások hatására alakítható ki, melyek a járművek közötti mechanikai kényszereket képezik le. Stabilizáló visszacsatolás mellett a zárt kör hibajelei globálisan exponenciálisan stabillá tehetők, alapot adva egy könnyen konfigurálható formációirányítási keretrendszernek.

(10)

(11)

Abstract

Automated control of ground vehicles is a challenging task of the automotive industry. It is an extensively researched topic that became the foundation of control of future driverless vehicles. The thesis takes a step further in the field of nonlinear autonomous vehicle and formation control that is capable of coping with high velocity cruising.

In the first part, a three-level hierarchical framework is proposed for the optimal control and trajectory planning of four wheel driven (4WD) ground vehicles considering state and input constraints. The high-level controller’s objective is to compute global time optimal and local time sub-optimal control based on the simplified two-wheel driven vehicle model for fixed or arbitrary paths. This is accomplished by solving a dynamic nonlinear optimal control problem based on direct discretization and multiple shooting method with receding horizon extension. The middle-level controller optimally distributes the driving forces to all four wheels in a least squares sense. The low-level controller keeps the deviation from the given trajectory at minimum while initial state perturbations are present. The closed loop utilizes a discrete low-level model predictive (MPC) integral controller resulting optimal trajectory tracking of the nonlinear 4WD vehicle.

In the second part, a two-level autonomous vehicle control system is developed. It allows high-speed (30 m/s) tracking control of unmanned ground vehicles despite of load variations. Changes in the load caused by the unknown center of mass point are considered based on the realistic 16 degree of freedom multibody vehicle model. The vehicle is modeled as a tree-structured mechanical system with moving base and its dynamics is obtained by utilizing Appell’s method and the Gibbs-function. The high-level controller incorporates the kinematics of the vehicle in a finite horizon linear time-varying quadratic optimal controller that locally stabilizes the closed loop system about the perturbations from the prescribed trajectories. The states of the vehicle are estimated with a two-stage Kalman Filter (KF) using realistic GPS and inertial (IMU) sensory information based on the kinematic differential equation of navigation.. Furthermore, the effect of rolling and pitching during the maneuver is minimized by low-level controller system which employs PID-type controllers based on selectable error differential equations. The simulations show promising results with small lateral path deviation for aggressive maneuvers near the physical limitations of the vehicle.

In the third part, two decentralized leader-follower formation structure is developed for underactuated ground vehicles. The first approach elaborates an adaptive control law based on kinematics equations. The follower vehicles have limited knowledge about the leader’s states and the unknown term containing the velocity information of the leader is estimated using neural networks with online adaptive weight tuning laws. All closed loop error signals are guaranteed to be uniformly ultimately bounded. The second approach exploits the principles of analytical mechanics. The group of vehicles are considered as a multibody system and coordinated motion is accomplished by imposing constraint forces. These forces maintain the prescribed formation and by stabilizing feedback all closed loop error signals can be rendered globally exponentially stable, giving rise for an easily configurable formation control framework.

(12)

(13)

List of Tables

3.1 Parameters of vehicle model . . . 19

3.2 Dimensions of the NLP problem . . . 25

4.1 Sampling times of vehicle control system . . . 79

4.2 Averages of the maximum lateral path errors (cm) . . . 82

B.1 Denavit-Hartenberg parameters of 16 DoF vehicle . . . 115

B.2 Dynamic parameters of the 16 DoF vehicle (SI units) . . . 116

(16)

(17)

List of Figures

2.1 Structure of SHL neural network . . . 9

3.1 Schematics of single-track vehicle . . . 15

3.2 Schematics of double-track vehicle . . . 18

3.3 Concept of three-level vehicle control system . . . 20

3.4 Path boundaries of the double lane change maneuver (∆=5 andd=0.84) . 22 3.5 Application of the Direct Multiple Shooting method to an optimal control problem using piecewise constant basis functions: IVP solutions violating the trajectory joining equations (left); optimal shooting nodes and control inputs are found by the NLP solver (right). . . 24

3.6 Subdivision of shooting interval for numerical integration . . . 29

3.7 Solution of NLP problem for grid pointsm=80 (left) andm=160 (right). 30 3.8 Optimal state trajectories for grid pointsm=80 (left) andm=160 (right) . 31 3.9 Optimal control inputs for grid pointsm=80 (left) andm=160 (right) . . 31

3.10 Resulting motion with the time optimal and the distributed controls. . . 34

3.11 Optimized global forces and torque with the reached RMSE errors . . . 35

3.12 LCQP distributed 4WD control signals. . . 35

3.13 Output trajectory of the MPC if the real initial lateral state differs from the optimal one . . . 41

3.14 Nominal and MPC control signals of the time optimal control problem . . . 41

3.15 Path design constraints . . . 45

3.16 The concept of Nonlinear Receding Horizon Control (N =8 andM=5). . 47

3.17 Optimal path constructed from the different LOCP solutions. Vertical lines indicate the shifting positions: local solution is accepted to the left (red) and discarded to the right (blue). . . 50

3.18 Optimal state trajectories computed by the RHPC algorithm. . . 50

3.19 Results of the optimal force distribution for the RHPC control . . . 51

3.20 Nominal and MPC control with RHPC high-level controller. . . 51

3.21 Comparison of output trajectories with the RHPC and the LCQP distibuted controls . . . 52

3.22 MPC controlled optimal path trajectories with initial perturbation for arbitrary path . . . 52

4.1 Modified Denavit-Hartenberg formalism . . . 58

4.2 Geometric model of 16 DoF ground vehicle . . . 59

4.3 Longitudinal and lateral tire forces by Pacejka model . . . 65

4.4 Derivative and smoothing filter (D/S) . . . 69

4.5 Performance of the LLC system and path errors in case of offsets in the vehicle’s CoG point: longitudinal motion (top row); lateral motion (bottom row) . . . 69

(18)

4.6 Comparison of passive and active suspension control for vehicle roll and

pitch: longitudinal motion (left column); lateral motion (right column) . . . 70

4.7 Simplified planar kinematics of the vehicle. . . 71

4.8 Tracking performance of the HLC system: longitudinal motion (left column); lateral motion (right column) . . . 74

4.9 Architecture of the vehicle control system . . . 78

4.10 Simulation result of high-speed motion maneuver with overlapping longitudinal and lateral acceleration . . . 80

5.1 Leader-follower configuration on thex–yplane . . . 88

5.2 Formation output trajectories (to scale) . . . 95

5.3 Closed loop signals of adaptive NN-based formation control . . . 97

5.4 Output trajectories during formation change (to scale) . . . 105

5.5 Closed loop signals of formation control based on constraint functions . . . 106

(19)

Nomenclature

The following list describes the symbols that will be later used within the thesis.

Abbreviation

2WD, 4WD Two-wheel drive, four-wheel drive

CAS Collision Avoidance System

CoG Center of Gravity

D/S Differentiating and Smoothing filter

DoF Degrees of Freedom

FNN Feedforward Neural Network

GPS Global Positioning System

HLC High-Level Controller

HOCP High-Level Optimal Control Problem

IMU Inertial Measuring Unit

INS Inertial Navigation System

IPOPT Interior Point OPTimizer

IVP Initial Value Problems

KF Kalman Filter

KKT Karush-Kuhn-Tucker theorem

LCQP Linearly Constrained Quadratic Program

LLC Low-Level Controller

LOCP Local Optimal Control Problem

LPV Linear Parameter Varying

LQR Linear Quadratic Regulator

LTI Linear Time-Invariant

LTV Linear Time-Varying

MPC Model Predictive Control

NLP Nonlinear Program

ODE Ordinary Differential Equation

OPTI OPTimization Interface toolbox for MATLAB PID Proportional-Integral-Derivative controller RHPC Receding Horizon Predictive Control RK4 Runge-Kutta method (4^thorder)

RMSE Root Mean Squared Error

RPY Roll-Pitch-Yaw

SGUUB Semi-Global Ultimate and Uniform Boundedness

SHL Single Hidden Layer

UGV Unmanned Ground Vehicle

General Notation

αx,αy longitudinal and lateral wheel slip

β side slip angle

(20)

a,ε acceleration and angular acceleration vector v,ω velocity and angular velocity vector

V, ˆV,W, ˆW ideal and approximated weight matrices of FNN

σ(·) activation function

∆ unknown nonlinear term to be approximated by FNN

λ_i Lagrange multiplier

A⁺ Moore-Penrose psuedoinverse ofA

Jj,Fj,Mj inertia matrix ofB_jexpressed inK_j, forces and moments acting onB_j

q joint variables

z_{i j},z_i,_1:_N jth and firstN element of vectorz_i, respectively

δ steering angle

k·k,k·k_F second and Frobenius norm, respectively

C(·) constraint function

G Gibbs function

J objective function

L Lyapunov function

M(q) generalized inertia matrix

R(ψ,z) 2D or 3D rotational matrix of angleψabout axisz ωδ, ˙δ steering rate

ωz, ˙ψ yaw rate

ϕ,θ,ψ roll, pitch, yaw angle

B_j jth body of the multibody system F_l,F_t lateral and longitudinal wheel forces F_r,F_a rolling and aerodynamical resistance force K_j jth coordinate-frame attached toB_j

l,d geometric parameters: base and track width mρ_{c j} first moment of body B_j expressed inK_j U_j control input: wheel traction and brake forces x,y,z coordinates of general position

iT_j homogeneous transformation betweenK_iandK_j, expressed inK_i tr(A) trace of matrixA

(21)

Chapter 1

Introduction

1.1 Motivation

The current trend in automotive industry is automation in driving on a wide range of scale, from basic cruise control (i.e. tempomats) to fully automated pilot systems. Almost every recently manufactured vehicle contains certain type of driving assistance system, and may also have a certain level of autonomy, i.e. automated parking functions, pedestrian collision avoidance systems, adaptive lane keeping control. Recent high-end products of the automotive industry are capable of executing more complex maneuvers, i.e. high-speed cruising on highway by employing autopilot control system. Tremendous effort is put by automotive companies and governments into having fully automated ground vehicles in the past few decades. However, automation of ground vehicles requires reliable models of the dynamics of the phenomena arising in the different maneuvers, along with accurate controllers, actuators and sensor instrumentations.

Designing the path to be followed for certain maneuvers is an important step towards full automation. However, computing the reference path that needs to be followed by the car requires the knowledge of the road on which the car travels. Provided that the whole track is known in advance, a path planning and time-optimal control can be suggested.

In practical situations it may happen that the entire path is not known apriori, only some sections of the track are available that can be detected by the sensors installed on the vehicle.

As a solution, a sub-optimal control algorithm with receding horizon scheme may be an appropriate approach to choose for handling the problem. Sub-optimality in this context refers to the difference between a feasible global time optimal and the actual motion of the vehicle. Another captivating problem is the tracking of these optimal reference signals.

A suitable control system is needed to overcome this task, i.e. an appropriate control input needs to be computed and applied to the vehicle in closed loop such that minimal deviation occurs from the desired trajectories. However, increasing the level of complexity by incorporating additional information is a challenge on its own, i.e. introducing obstacles, taking into account uncertainties and nonlinear dynamics.

Consequently, simulations are required in every sector and application related to vehicle automation. Simulators are used in the development process of the different control algorithms, and allow the developers to test different control design strategies without the danger of causing accidents on real physical systems. Additionally, simulations can be utilized for hardware-in-the-loop tests, i.e. the manufactured controllers’ hardware (and software) are tested in a real scenario, in which the signals that would come from the physical vehicle and the environment are substituted by a computer simulation. The models employed by these simulators have crucial effect on the validity of the simulation, thus analysis and further development of vehicle models used by the simulators play an important role in this area.

(22)

Automation of ground vehicles is realized through the application of hierarchically structured control systems. High-level controllers perform tasks related to preprocessing measurements from the interaction with the environment, decision support, energy man- agement and safety interference in case of emergencies. Low-level controllers are used for instance in steering control, velocity control and active suspension control. These controllers ensure that the suspension systems operate in the desired way, i.e. keeping the car in a horizontal plane by minimizing rolling and pitching effect arising from uneven road surface, and the vehicle is headed towards the prescribed direction with the given speed pro- file. The desired orientation and velocity may be given by the human driver, computed by high-level controllers or can be considered as command signals determined by the optimal path planner algorithms discussed previously.

Real-time control of ground vehicles requires real-time information about the car and its environment that acquired from measurements. Typical on-board sensors of a car are the global positioning system (GPS) giving real-time absolute position information with considerable sampling time and low accuracy. On the other hand, inertial measurement unit (IMU) can provide acceleration and velocity information with significantly lower sampling time and higher accuracy but it is exerted to a drift-type error. The accuracy of the measurements can be increased by employing other types of devices such as magnetometer or differential and carrier phase GPS. As a consequence, the incomplete data coming from separate sensory units can be incorporated into an effective inertial navigation system by sensor fusion, utilizing the strengths and suppressing the weaknesses of the different devices. However, even with fast IMU sensors, the control algorithms may need data from more frequent measurements in case of high-speed maneuvers. One approach to solve this problem is interpolation between measurements with high fidelity and the use of sophisticated and accurate estimation algorithms.

Automation of ground vehicles may provide a future transportation with increased safety, since automated systems can react to situations that cannot even be detected by humans, moreover they are lacking the possibility of human failures; additionally, the level of safety can be further increased by controlling multiple vehicles in groups. Once the automation of single vehicles is elaborated, the next step would be the formation control of these vehicles, i.e. control multiple autonomous (i.e. unmanned) vehicles in order to achieve a desired goal. These may include scenarios such as avoiding accidents in general transportation situations, minimizing the travel times and as well as the risk of traffic jams, employing more efficient energy usage, and decreasing the human interaction. Moreover, automated vehicles in traffic can react to prioritized vehicles (ambulance and police cars, fire trucks, etc.) in an aligned manner. Consequently, the level of complexity of controlling multiple vehicles increases in comparison to those elaborated for single vehicle system that needs to be handled by adequate high-level formation control system.

1.2 Main objectives

The main contributions of this thesis are summarized as follows:

• A hierarchical control system is proposed for four independently actuated vehicles.

Time optimal and time-suboptimal trajectory is computed either on fixed or arbitrary

(23)

1.3. Thesis outline 3

paths. A closed loop model predictive integral control is presented for minimizing the perturbations from the optimal trajectories.

• High-speed autonomous control framework is developed based on the realistic sixteen degree of freedom multibody model of the vehicle using nonlinear tire characteristics and dynamic load distribution. Precise low-level active type suspension, speed and steering control are employed and associated with a high-level linear optimal controller. The control system is capable of accurately compensating load changes arising from the unknown center of gravity point and it can be implemented for real-time applications.

• Adaptive formation control system is elaborated for unmanned ground vehicles (UGVs) in leader follower structure. The follower vehicles have limited knowledge about the leader’s states and these uncertainties are estimated online by adaptive neural networks making all closed loop signals ultimately bounded and increasing robustness.

• Coordinated motion of UGVs are proposed taking into account both kinematics and dynamics of the vehicle. The approach is based on multibody interpretation of mechanical systems and by imposing constraints forces on the group of the vehicles stable trajectory tracking is guaranteed.

1.3 Thesis outline

The remaining part of this thesis is structured as follows.

Chapter2briefly summarizes the mathematical preliminaries on which the thesis is based.

Namely, the concepts of optimal control, nonlinear optimization and stability of nonlinear systems.

Chapter 3 elaborates on the concept of optimal path planning and tracking control of four independently actuated vehicles equally suitable for fully electric vehicles, or vehicles equipped with other types of propulsions.

Chapter 4 develops a more complex and full vehicle model based on robotic formalism and describes a hierarchical control framework of autonomous vehicles for high-speed maneuvers using real-time sensor fusion and optimal control.

Chapter 5 deals with the topic of formation control in leader-follower structure and coordinated trajectory tracking of multiple ground vehicles based on two different methods:

nonlinear adaptive control taking into considerations uncertainties; and multibody moti- vated formation control using constraint forces.

Finally, Chapter6offers some conclusions along with possible outlook for future work.

(24)

(25)

Chapter 2

Mathematical Preliminaries

Contents

2.1 Optimal Control . . . . 5 2.1.1 Nonlinear Optimal Control Problem . . . . 5 2.1.2 LQR Control Problem . . . . 6 2.1.3 Nonlinear Optimization . . . . 7 2.2 Adaptive Nonlinear Control. . . . 8 2.2.1 Lyapunov-Stability . . . . 8 2.2.2 Feedforward Neural Networks . . . . 9 2.2.3 Tuning Laws of Adaptive NNs . . . . 10

2.1 Optimal Control

2.1.1 Nonlinear Optimal Control Problem

The general problem formulation of the nonlinear optimal control problem is

min_x,u ψ(x,u) (2.1)

s.t. x˙(t) = f(t,x(t),u(t)) (2.2)

0≥ c(t,x(t),u(t)) (2.3)

0≥ r(x(t_k)), k =1,2, . . . ,K (2.4) wheret∈[t₀,t_f] andt_k ∈[t₀,t_f] for eachk =1,2, . . . ,Kwitht₀being the initial time and t_f being the final time, see e.g. [1].

• Equation (2.1) expresses the minimization of the cost function by finding the control input functionu and the state space variable functionsx on the time interval [t₀,t_f] that minimize the functionalψ. The output of the nonlinear optimal control problem is the control input functionu.

• Equation (2.2) defines the dynamics of the system, given by ordinary differential equations. In the most general situation we assume that the function f is at least piece-wise Lipschitz, i.e. the alteration of the arguments of the function results in bounded alteration of the value of the function.

(26)

• Equation (2.3) gives inequality type constraints in a general form that can depend on the time, the internal states of the system and the control input. Inequality type constraints can be used i.e. to restrict the system trajectories into the admissible region, define boundary conditions, etc.

• Equation (2.4) defines (point-wise) K number of inequality type constraints that depend on the internal states in specific time instances only. This function defined point-wise constraints that can be used i.e. to impose conditions on the initial and final state of the system.

The optimization problem can be solved in many different ways. Indirect methods are based on Pontryagin’s maximum principle and involve symbolical calculation which hardens the problem for high dimension and makes algorithmic optimization almost impossible.

Direct methods can be implemented in an algorithmic way, however they only result in less accurate approximation of the solution compared to the indirect methods.

Dynamic programming [2,3] is an indirect method to calculate the optimal solution that can be done using recursive backwards cost-to-go function that solves the optimal control problem on intervals starting from the final state and advancing backwards in time to the initial state. Dynamic programming is able to find the global solution, however the complexity grows exponentially as the dimension of the problem increases, thus dynamic programming is not used in this thesis.

Direct methods discretize the search space and seek for the (sub)optimal solution in the discretized space. The direct simple shooting method uses discrete values for the control inputs based on piece-wise basis functions on fixed intervals, and find the suboptimal solution using nonlinear programming in the search space that can be achieved with such control inputs. Direct collocation methods (e.g. [4]) discretize the state space as well along with the control input function, resulting in a much larger nonlinear programming problem.

However, the nonlinear programming problem is sparse, thus it can be solved efficiently using sparse solvers. Direct multiple shooting methods [5,6,7] are the combination of the single shooting and collocation method, in the sense that they employ discretized search space for both the state variables and control inputs functions, however they still solve initial value problems to calculate the system trajectories during optimization. Direct multiple shooting methods can combine the advantages of the direct methods while suppressing the disadvantages of the individual algorithms.

2.1.2 LQR Control Problem

The linear quadratic regulator (LQR) problem [8] is an optimization problem that optimizes the quadratic functional in the time interval [i,N] defined as

J_i(x,u)= 1 2

N−1

X

k=i

x^T_kQ_kx_k+u^T_kR_ku_k + 1

2

x^T_NQ_Nx_N (2.5) for time-varying linear dynamic systems defined by the differential equation

xk+1= Akxk+Bkuk (2.6)

(27)

2.1. Optimal Control 7

with the assumptions thatR_k > 0 andQ_k ≥ 0 for allk =i, . . . ,N.

This is a finite-horizon optimization problem, a solution to the problem can be acquired using the state-feedback

u_k =−K_kx_k (2.7)

where the matricesK_kare calculated using backward recursion with initializationS_N =Q_N and the recursion starting withk = N−1 as

S_k = A^T_k

S_k+1−S_k+1B_k

B^T_kS_k+1B_k +R_k−1

B^T_kS_k+1

A_k+Q_k (2.8) K_k =

B^T_kS_k₊₁B_k+R_k−1

B^T_kS_k₊₁A_k (2.9)

Equation (2.8) is called the control Ricatti difference equation.

The infinite horizon alternative of the problem is formulated as the minimization of J∞ = 1

2

∞

X

k=0

x^T_kQx_k+ 1 2

u^T_kRu_k (2.10)

withQ≥0 andR> 0 for the system with the differential equation

xk+1= Axk+Buk (2.11)

The solution to the infinite horizon problem is the state feedback

u_k =−K∞x_k (2.12)

where K∞ (also called the steady-state Kalman–gain) can be acquired after solving the algebraic Ricatti equation

S = A^>

S−SB

B^>SB+R−1

B^>S

A+Q (2.13)

using the formula

K∞=

B^>SB+R−1

B^>S A (2.14)

The solutions to the finite and infinite horizon problems exist (are stable) if the underlying dynamic processes are stabilizable.

2.1.3 Nonlinear Optimization

Consider the following constrained nonlinear optimization program:

xmin∈Rⁿ

f(x) (2.15)

s.t. g(x) =0 (2.16)

h(x) ≤ 0 (2.17)

where f is the objective function, g ∈ Rⁿ^g vector valued function defining equality type constraints, whileh∈Rⁿ^h gives the inequality type constraints, all functions being at least once continuously differentiable. A point is called a feasible point if it satisfies (2.16) and (2.17), the set of feasible points is called the feasible set.

A necessary condition for a feasible point to be a local optimum of the objective function f is given by the Karush-Kuhn-Tucker (KKT) theorem [9].

(28)

Theorem 2.1 (Karush-Kuhn-Tucker). Define the Lagrangian function as the real-valued functional

L(x,λ,µ) = f(x)+λ^Tg(x)+µ^Th(x) (2.18) Letx^?∈Rⁿbe a local optimum of the functional f(x)and suppose that the gradients∂_xh and∂_xgare linearly independent at the pointx^?. Then there exist Lagrange multipliersλ^? andµ^?such that the following conditions hold

∂xL(x^?,λ^?,µ^?) =0 (2.19)

g(x^?) =0 (2.20)

h(x^?) ≤0 (2.21)

µ^?≥0 (2.22)

µ^?Th(x^?) =0 (2.23)

2.2 Adaptive Nonlinear Control

2.2.1 Lyapunov-Stability

Stability of nonlinear systems is mostly analyzed by the methods of Lyapunov [10]. How- ever, there are different stability properties for nonlinear systems. Consider the nonlinear nonautonomous (time-varying) system with the differential equation

x˙ = f(t,x(t)) (2.24) at time instantt≥t₀≥ 0 withx(t) ∈Rⁿ. The pointx^∗ ∈Rⁿis the equilibrium point of the system with dynamics governed by (2.24) if f(t,x^∗)=0 holds. The stability of equilibrium points can be found in many literature [11,12]. Some definitions are referred here.

Definition 2.1. An equilibrium pointx^∗∈Rⁿof a system with differential equation (2.24) is stable in the sense of Lyapunov, if∀ε > 0and∀t₀ ≥ 0 there exists a positive constant δ= δ(ε,t₀), such that∀t≥t₀the trajectories of the system stay close (inεdistance) tox^∗, if the initial condition is sufficiently close to the equilibriumx^∗(inδdistance), i.e.∀t ≥t₀, kx(t)−x^∗k ≤ε, ifkx(t₀)−x^∗k ≤δ, with some appropriately defined vector norm onRⁿ. Note that the parameterδis a constant in the definition, however its value may depend on the initial time and the positive constantε.

Definition 2.2. Consider Definition2.1. If the constantδis independent of the initial time t₀, i.e. δ=δ(ε) >0, then the equilibrium point is said to be uniformly stable. Note that for autonomous (time-invariant) systems, i.e. systems with differential equation in the form of x˙ = f(x), a stable equilibrium is always uniformly stable.

Definition 2.3. Consider Definition2.1. If the constantδis independent of the parameter ε, i.e. δ = δ(t₀), and x(t) → x^∗ as t → ∞ then the equilibrium point is said to be asymptotically stable.

Definition 2.4. Consider Definition2.1. If the conditions hold for anyx₀, then the equilib- rium point is said to be globally stable.

(29)

2.2. Adaptive Nonlinear Control 9

In some cases, it is enough to have bounded solutions, thus bounded systems do not necessarily converge or stay arbitrarily close to an equilibrium.

Definition 2.5. An autonomous system governed by x˙ = f(x) is said to be semi-globally uniformly ultimately bounded (SGUUB) [13], if for all initial conditions x(t₀) = x₀ ∈Ω, whereΩis a (nonempty) compact subset ofRⁿ, there exists a numberµ∈R(the boundary) and a numberT(µ,x₀) ≥ 0 (a time after the system gets into the boundary), such that

∀t≥t₀+Tit holds thatkx(t)k ≤ µ.

2.2.2 Feedforward Neural Networks

In this section the basic background and properties of feedforward neural networks (NNs) are introduced using the notations and results of [14] which will be used in Chapter5for adaptive controller design.

Consider a single hidden layered (SHL) NN as depicted in Fig. 2.1. Theith NN output for inputx =[x₁,x₂, . . . ,x_N₁]^T ∈R^N¹is defined by

y_i=

N₂

X

j=1





 w_{i j}σ_j*

,

N₁

X

k=1

v_jkx_k +θ_{v j}+ -

+θ_wi





(2.25) fori= 1, . . . ,N₃with activation functionσ(·) andvjk,wi j layer interconnection weights.

The input and output bias terms (thresholds) associated with the jth hidden layer and the ith output are denoted withθ_{v j} andθ_wi, respectively. ConstructV^T =[θ_v,v₁, . . . ,v_N₁] and

xN₁

y1

y2

yN₃

1

x1

x2

vN₁N₂

θv1

θw1

wN2N3

σ1

σN₂

1

σ2

Figure 2.1: Structure of SHL neural network

W^T = [θ_w,w₁, . . . ,w_N₂] weight matrices and define σ = [1, σ₁(·), . . . , σ_N₂(·)]^T ∈R^N²⁺¹ and the new input vector ¯x =[1,x^T]^T ∈R^N¹⁺¹to incorporate the bias terms, then the NN equation (2.25) can be expressed as

y=W^Tσ(V^Tx)¯ (2.26)

The universal approximation theorem [15] states that given a continuous real-valued function f : Ω → R^N³ over the compact set Ω ∈ R^N¹ and any ε^? > 0 precision, then for some sufficiently large N₂ there exist bounded V^T and W^T weights such that the approximation

f(x) =W^Tσ(V^Tx)¯ +ε(x) (2.27)

(30)

satisfiesε(x) ≤ ε^?. Typically, non-decreasing and bounded activation functions are chosen, such as the sigmoid function whose range is [0,1].

2.2.3 Tuning Laws of Adaptive NNs

The weight matrices in the universal approximation theorem can be approximated by tuning the NN. There exist several tuning algorithms for NNs, the most common is called the error backpropagation (see e.g. [13]). The backpropagation algorithm modifies the parameters based on the value of the tracking error of the NN. This error is weighted by the signed effect of the current parameter on the output error (i.e. the derivative of the error with respect to the current parameter). This technique modifies the parameters along the negative gradient of the tracking error, resulting in a local tracking error minimization in the parameter space in the neighborhood of the initial conditions.

Note that for adaptive NNs, the backpropagation algorithm is used online, i.e. if the adaptive NN is used as a controller, the closed-loop system is being modified during operation because of the backpropagation algorithm. Since a real control system may contain several disturbances and noises, increasing the robustness of the tuning method against these effects is desirable. The σ-modification method modifies the adaptation law for the parameters by adding a linear drift term to the differential equation of the parameters. For example, ifθis the parameter vector, and the adaptation law is ˙θ= f(θ,e) with e being the tracking error, then theσ-modification method modifies this adaptation law to ˙θ = f(θ,e)−σθ withσbeing a positive constant [16]. This method increases the robustness of the tuning algorithm, moreover it helps to ensure that the parameters remain bounded. Theσ-modification was developed further in [17], where the constantσhas been replaced by a term depending on the absolute value of the tracking error, i.e. the differential equation of the parameter has been modified to ˙θ = f(θ,e)−γ|e|θ, withγbeing a positive constant. This method is called thee-modification which further increases the robustness of the adaptation law [17].

Denote the approximation of V and W by Vˆ andWˆ respectively, (i.e. the current parameters used in the realized NN areVˆ andWˆ ). A robust adaptation law of the weight parameters can be given by [13] as follows:

˙ˆ

V =−Gf

x¯Wˆ^Tσ⁰(Vˆ^Tx¯)e+δ_ν(1+|e|^m)Vˆg

(2.28)

˙ˆ

W =−F( f

σ(Vˆ^Tx)¯ −σ⁰(Vˆ^Tx)¯ Vˆ^Tx¯g

e+δω(1+|e|^m)Wˆ )

(2.29) whereF,G,δ_ω,δ_ν,mare positive design parameters andσ⁰is the Jacobian of the activation functions. In both adaptation laws, the first term in the outermost brackets stands for the backpropagation, while the second term is the combination of theσ-modification and the e-modification that are incorporated into the adaptation law to increase the robustness.

(31)

Chapter 3

Optimal Control of Four In-Wheel Driven Vehicles

Contents

3.1 Introduction . . . . 11 3.2 Dynamic Modeling. . . . 14 3.2.1 Single-Track Vehicle . . . . 14 3.2.2 Double-Track Vehicle. . . . 17 3.3 Three-level Time Optimal Control of 4WD Vehicles . . . . 20 3.3.1 Test Path: Double Lane Change . . . . 21 3.3.2 High-level Time Optimal Control Problem . . . . 21 3.3.3 The Nonlinear Problem. . . . 22 3.3.4 Implementation Strategies . . . . 25 3.3.5 Optimal Wheel Force Distribution . . . . 32 3.3.6 Low-level Model Predictive Control . . . . 35

3.3.7 Collision Avoidance System . . . . 41 3.4 Time-Suboptimal Predictive Control in Arbitrary Paths . . . . 43 3.4.1 Path Constraints Design . . . . 44 3.4.2 Nonlinear Receding Horizon Predictive Control . . . . 46 3.4.3 Numerical Results . . . . 49 3.5 Summary . . . . 53

3.1 Introduction

Pursuit of optimal behavior on the improved functionality and overall system robustness in automotive industry is a common goal of recent research. The intention of these im- provements is to provide more reliable, more pleasant and, most importantly, safer driving experience. The key idea is to assist the driver in some way to be better able to handle difficult driving scenarios when the vehicle’s physical properties are pushed to their limits, i.e. avoiding unexpected obstacle on the road or driving in harsh environment. There are several existing systems that can be found in the majority of cars (such as Anti-lock Braking System (ABS), Traction Control (TC), Electronic Stability Control (ESC), etc.), which solve some of these problems and help the driver either passively or actively.

(32)

Naturally, an overall better performance regarding the passengers’ safety and comfort can be achieved in a number of different ways with the underline on the "hardware", i.e. by improving the vehicle’s agility, equipping better sensors etc. While these being valid and important aspects, they fall beyond the scope of this thesis.

One of the most captivating problem in transportation is autonomous vehicle control and consequently it is regarded as an objective of the near future. The composition of the main task is twofold: 1) a feasible path to be followed needs to be calculated with admissible dynamic prescriptions, 2) the corresponding control sequence needs to be applied that tracks these signals with minimal deviation as possible. However, incorporating additional information is a challenge on its own, i.e. introducing obstacles, taking into account road/air friction or its absence, etc. Being such complex problem, it can be categorized into several subsets focusing on different aspects.

Considerable efforts have been dedicated to intelligent vehicle systems that automate longitudinal driving tasks. These autonomous vehicle systems include Adaptive Cruise Control (ACC), connected vehicle systems, Cooperative Adaptive Cruise Control (CACC) systems. In [18, 19] the emphasis of vehicle control is driving on highways, utilizing the rather organized structure and relatively simple rules of highways. The authors of [18] demonstrated a two-level control system where path planning with multiple collision avoidance constraints generates collision free maneuvers, which can be tracked by a low- level vehicle controller. A fully automated Lane-changing and Car-following Control Systems (LCCS) is proposed in [19] where a possible future path is calculated based on the current position and velocity, and the expected behavior of the surrounding vehicles. The study focuses on mathematical problem of what strategy should be used in order to have a beneficial path, i.e. less change in the velocity while keeping safe distance from other vehicles.

Model Predictive Control (MPC) is widely used to solve optimal control problems with relatively high-fidelity. The advantage of this control lies within its robustness, i.e.

the same algorithm can be used on different system with minimal changes in the controller parameters. Implications vary on the type of problem; however, some transportation related topics include, amongst others, path correction of tractor-trailer system [20], designing a more efficient diesel engine with less toxic/harmful emission [21]. In [22] the proposed MPC systems (with different computational complexity) were tested through both simulations and experiments, showing that MPC systems can be implemented in real-life applications. An active steering system based on MPC is presented in [23] where only the steering angle of the front wheels can be modified. Simulations show that irregardless of the simplicity of the control input, a fairly good tracking of the reference path can be achieved at low speeds.

In [24] an MPC is proposed to integrate vehicle stability and slip control into one system.

Simulations and experimental results show that the system is capable of determining the inputs of a 4WD vehicle, resulting in a maneuver on slippery road without oversteering.

In [25] a nonlinear receding horizon control design combined with dynamic optimization is used to calculate an applicable path. This method is most likely the closest to human perception, namely it generates feasible routes based on changes in the path and/or in the available information of the environment. Experimental results show that receding horizon method can yield time-efficient optimal routes. It is worth noting that the desired paths in these experiments were calculated offline.

(33)

3.1. Introduction 13

Torque-vectoring is another possibility to enhance the overall stability of the vehicle, change yaw rate and control slide slip angle. Holistic cornering control (HCC), which is an optimal torque vectoring algorithm, has several advantages, namely, it helps tracking the desired path while providing real-time vehicle stability, has fault-tolerant capability, and is robust in terms of vehicle parameters. Experimental results show that HCC can be successfully implemented in real-life scenarios, yielding improved stability on slippery surfaces where uncontrolled vehicles would slide [26]. Quadratically constrained linear programming (QCLP) is proposed to optimize force distribution of individual wheel forces in [27]. The presented flexible algorithm is capable of calculating optimal force distribution for different input configurations such as rear, front or all driving vehicles.

Besides MPC, other approaches exist to tackle these problems. In [28, 29] a mixed- integer optimal control problem (MIOCP) is solved to drive a geared vehicle on a closed track. The papers propose a single-track vehicle model while keeping the numerical com- putation time in the minute range by employing fast numerical methods. The optimization yields open loop reference signals, i.e. there is no feedback from dynamical vehicle model;

as well as that the route is predetermined, meaning that all information about future segments of the path are available from the beginning. In [30] LQR controller is used to coordinate a four-wheel driving and four-wheel steering (4WD4WS) electric ground vehicle. Using this method helps to reduce the numerical computational effort. In [31] the authors utilized Linear Parameter Varying (LPV) approach to create an active driver assisting system and the feasibility of the method was tested by simulation. The results show increased maneu- verability and stability as opposed to the unassisted driver’s path. Similar technique in [32]

and [33] was employed to design fault-tolerant control systems.

In the majority of the mentioned papers and research, the proposed control method is a multi-level control system. The high level algorithm handles the task by setting a desired objective and the low-level system ensures that the deviation from this calculated optimum is minimal. The benefit of this approach is the unnecessity of using labor-intensive computations at low level and thus resulting in a faster online execution.

Even if the combination of the calculated path and the corresponding control inputs are both available and feasible, the vehicle may not be kept on the path due to the lack of information on the initial values, i.e. starting position, velocity, heading angle, etc.

To overcome this issue a path-following control is presented in [34], using hyperbolic projection. The concept proposes reduced overshoot of the lateral offset when the initial conditions are deficient or erroneous, and make the lateral offset compactly bounded.

It should be noted that, despite the best effort of the manufacturers/designers, faults may occur in the vehicle control, e.g. steering system failure, electric motor fault. Such faults may endanger the passengers’ safety that is why it is necessary to handle these situations.

A thorough discussion and possible solutions of the problem are given in [35,36,32,33], where fault tolerant controls are tested.

A possible solution to adequately control a four-wheeled vehicle is to actuate each wheel independently with in-wheel motors. This way similar results can be achieved as with torque vectoring while having total control over the actuated motors. Another advantage of being able to independently control the wheels is that when negative torque needs to be applied on a wheel it can be used to generate electricity (i.e. regenerative braking) and as a consequence increasing the travel distance of the car. The concept of regenerative braking already exists

(34)

using this strategy [37]; however, utilizing individual motors for this purpose is not common in the literature and is rather unexploited in real-life applications.

This chapter presents an MPC based control design strategy to preserve stability and improve handling of a four-wheel independently actuated ground vehicle. While the presented concept is related to, and more easily implementable for electric cars equipped with in-wheel motors, it is by no means excluding other types of propulsion, e.g. hydrogen, hybrid-hydrogen or ordinary combustion engines. The main contribution of this chapter is a hierarchic framework that is capable of producing and tracking an optimal reference trajectory despite of initial uncertainties for four-in-wheel driven vehicles. The presented method can handle fixed, predefined paths in which a time optimal control problem is solved by the high-level system, as well as unspecified paths where only a fraction of the total path is "visible" to the system. For the latter scenario, a time sub-optimal solution is given using nonlinear receding horizon control. The approach is based on accurate two-track dynamic vehicle model while considering the input and output constraints of the vehicle, i.e. max- imal velocity, steering, path boundaries etc. The presented method will be evaluated for a fixed double-lane change and an arbitrary path by means of simulation. The purpose is to provide the capability for online execution with the aid of the low-level subsystem.

3.2 Dynamic Modeling

This section provides a basic introduction to longitudinal and lateral vehicle dynamics as well as tire modeling [38]. These modeling approaches are required for the purpose of vehicle control. To reduce the computational complexity and execution time of the vehicle control algorithms, simplified vehicle models such as the single- and two-track models are introduced consecutively.

3.2.1 Single-Track Vehicle

Consider a car moving in a horizontal plane steered by the front wheels. It is assumed that the right and left side of the vehicle is symmetrical, thus the two halves can be merged to a single-track model as shown in Figure3.1.

Two important coordinate frames have to be highlighted. The body-fixed frameK₁is fixed to the center of gravity point (CoG) of the vehicle. The x₁-axis coincides with the heading angle of the carψand thez₁-axis is directed upwards. The origin ofK₁is expressed in the inertial (reference) frameK₀by coordinates(x,y).

The steering angle of the front wheel is denoted withδand angles α_F, α_R and β are the front and rear tire slips and vehicle side slip angle, respectively. The velocities of the front and rear tire-wheel contact points, usually referred as wheel velocities, arevFandvR, respectively. The local velocity magnitude of the vehicle is denoted withv.

Assume that the front wheel is steered to the left inK₁, then bothvandvF vectors are to the left of x₁, while v_R appears to the right of x₁. In this case, the direction of the tire slip and side slip angles are considered to be positive, that isδ >0,α_F >0 andα_R >0.

Taking into account the angular velocityωzof the car, thex₁andy₁components of the front wheel velocities can be expressed by

(35)

3.2. Dynamic Modeling 15

x0

y0

x1

y1

F_lR F_tR

CoG

l_F l_R

e_sp

F_ax

F_ay

v_R

v

v_F

ψ β

α_R

F_lF F_tF

δ αF

Figure 3.1: Schematics of single-track vehicle

vFsin(δ−αF) =l_Fωz+vsinβ, (3.1)

v_Fcos(δ−α_F) =vcosβ (3.2)

from which, it follows that

α_F =δ−arctan

l_Fω_z +vsinβ vcosβ

!

(3.3) Similarly, the rear tire slip can be given by

α_R =arctan

l_Fω_z−vsinβ vcosβ

!

(3.4) The lateral force generated at the contact point between the road and tire is transferred to the wheel. This mechanism is modeled by the empirical Magic Formula [39] that expresses the lateral forces in function of the tire slip angles. The general formula is given by

F_{t j}= D_jsin

(C_jarctan

fB_jα_j−E_j(B_jα_j−arctan(B_jα_j))g )

, j∈ {F,R} (3.5) with shape factorsB,C,DandE.

The longitudinal force is modeled with two parts: the traction/braking force and the rolling resistance. The former is considered to be an input of the vehicle, while the latter is computed from the friction as a velocity dependent function and the static load distribution as follows [40]:

f_r(v) =9·10⁻³+7.2·10⁻⁵v+5.038848·10⁻¹⁰v⁴ (3.6) F_{r F} = f_r(v) ml_Rg

l_F+l_R, F_{r R} = f_r(v) ml_Fg

l_F+l_R (3.7)