terrain avoidance model predictive control for autonomous ... · terrain avoidance model predictive...

DSORdynamical systems andocean robotics LAB

IntroductionModel

ControllerResults

Terrain Avoidance Model Predictive Control for

Autonomous Rotorcraft

B. Guerreiro C. Silvestre R. Cunha

Instituto Superior TecnicoInstitute for Systems and Robotics

Av. Rovisco Pais, 11049-001 Lisboa, Portugal

(email: {bguerreiro,cjs,rita}@isr.ist.utl.pt )

October 7th, 2008

Guerreiro, Silvestre and Cunha Terrain Avoidance MPC for Autonomous Rotorcraft 1/22


IntroductionModel

ControllerResults

Outline

1 IntroductionAutonomous RotorcraftModel Predictive Control Approach

2 ModelState Space EquationForces and Moments

3 ControllerMPC Control ProblemUnconstrained Optimization ProblemAlgorithm

4 ResultsSimulation ResultsSummary



IntroductionModel

ControllerResults

Autonomous RotorcraftModel Predictive Control Approach

Outline







IntroductionModel

ControllerResults


IntroductionAutonomous Rotorcraft

Applications:

Low altitude aerial surveillance;Automatic infrastructure inspection;3D mapping of unknown environments.

The platform:

High precision 3D maneuvers;Hover and VTOL capabilities;Carry multiple sensors.

Challenging Control problem:

Highly nonlinear and coupled modelWide parameter variations over the flightenvelope



IntroductionModel

ControllerResults


IntroductionModel Predictive Control Approach

Helicopter nonlinear model;

Input and state saturation constraints;

Terrain avoidance constraint:

Virtual repulsive field around vehicle.

Reformulated into unconstrained problem;

Solved online at each sampling instant;

Optimization algorithm:

Quasi-Newton search direction;Line search using Wolfe rule.



IntroductionModel

ControllerResults

State Space EquationForces and Moments

Outline







IntroductionModel

ControllerResults


Helicopter Model

Helicopter model:

6 DoF Rigid body dynamics;Parameterized for the Vario X-treme R/Chelicopter

Actuation u = [ θ0 , θ1c , θ1s , θ0t]T :

θ0 is the main rotor collective inputθ1c , θ1s are the main rotor cyclic inputsθ0t

is the tail rotor collective input

State variables:

v = [ u v w ]T – Body-fixed linear velocityω = [ p q r ]T – Body-fixed angular velocityp = [ x y z ]T – Positionλ = [ φ θ ψ ]T – ZYX Euler angles

State Vector:

x =

v

ω

p

λ



IntroductionModel

ControllerResults


Helicopter ModelState Space Equation

State Equation:

x = fc(x,u) =

−ω × v + 1m

[fh (v, ω,u) + fg (φ, θ)]

−I−1 (ω × I ω) + nh (v, ω,u)

R (λ) v

Q (φ, θ) ω

where x ∈ X and u ∈ U .



IntroductionModel

ControllerResults


Helicopter ModelForces and Moments

Resultant force and moment vectors:

fh = fmr + ftr + ffus + ftp + ffn

nh = nmr + ntr + nfus + ntp + nfn

Main rotor:

1st order pitching dynamics;Steady state flapping dynamics;Lag dynamic neglected;

Tail rotor pitch, flap and lag dynamics neglected;

Fuselage modeled as a function of flow velocity, incidenceangle and sideslip angle;

Horizontal tailplane and vertical fin modeled as regular wings.



IntroductionModel

ControllerResults

MPC Control ProblemUnconstrained Optimization ProblemAlgorithm

Outline







IntroductionModel

ControllerResults


MPC Control Problem

Discrete Model:

xk+1 ≈ f (xk,uk) = xk + Ts fc (xk,uk)

Prediction Horizon N;

State Sequence: Xk = {xk, . . . ,xk+N};

Input Sequence: Uk = {uk, . . . ,uk+N−1};

Reference State Sequence: Xk = {xk, . . . , xk+N};

Reference Input Sequence: Uk = {uk, . . . , uk+N};

Where Xk, Xk ∈ XN and Uk, Uk ∈ UN withX N = {Xk : xi ∈ X ,∀i=k,...,k+N}UN = {Uk : ui ∈ U ,∀i=k,...,k+N−1}



IntroductionModel

ControllerResults


MPC Control Problem

MPC Control Problem

Find, at each iteration k, the optimal control sequence U∗k with

horizon N , such that the resulting state sequence X∗k follows the

state reference Xk without violating the state and inputconstraints and avoiding collisions, i.e.,

U∗k = arg min

Uk

Jk , s.t. Xk ∈ X N , Uk ∈ UN

FM (Xk, Uk) = 0 , FT (Xk) = 0

Jk = Fk+N +k+N−1∑

i=k

Li , Fi = 12 x′

i P xi

Li = 12 (x′

iQ xi + u′iR ui) with xi = xi − xi and ui = ui − ui



IntroductionModel

ControllerResults


Unconstrained Optimization ProblemSaturation Constraint

Constraint Sets:

X = {x ∈ Rnx : |x(j)| ≤ x(j)

max ∀j=1,...,nx}

U = {u ∈ Rnu : |u(l)| ≤ u(l)

max ∀l=1,...,nu}

Penalty function:

fS (x,u) =1

2

nx∑

j=1

h(|x(j)|−x(j)max)

2w(j)x +

1

2

nu∑

l=1

h(|u(l)|−u(l)max)

2w(l)u

where w(j)x , w

(l)u ∈ R

+ and h(a) =

{

a , if a > 0

0 , otherwise.



IntroductionModel

ControllerResults


Unconstrained Optimization ProblemTerrain Constraint

Weight minimum distance betweenhelicopter position p and closest terrainpoint pm: pe = p − pm

Distance between the terrain and thesphere of radius rS :

g (x) = p′e pe − r2S

Terrain avoidance constraint function:

fT (x) = e−g(x)



IntroductionModel

ControllerResults


Unconstrained Optimization ProblemNew Optimization Problem

Saturation and Terrain constraints incorporated in costfunctional using penalty methods;

MPC Control Problem

The equivalent optimization problem is given by

U∗k = arg min

Uk

Jk , s.t. FM (Xk, Uk) = 0

where

Jk = Fk+N +k+N−1∑

i=k

Li

Fi = Fi + fS(xi, 0) + fT (xi) , Li = Li + fS(xi,ui) + fT (xi)



IntroductionModel

ControllerResults


Unconstrained Optimization ProblemModel Constraint

Model Constraint solved using Lagrange multipliers λi:

Hi = Li + λ′i+1 fd (xi,ui)

Jk = Fk+N − λ′k+N xk+N +

k+N−1∑

i=k+1

[

Hi − λ′i xi

]

+Hk

Choose:

λk+N =∂ Fk+N

∂xk+N

, λi =∂ Hi

∂xi

, ∀i=k+1,...,k+N−1

Then, 1st order condition of optimality reduced to:

∂ Jk

∂ui=∂ Hi

∂ui= 0 , ∀i=k,...,k+N−1



IntroductionModel

ControllerResults


AlgorithmMinimization

Minimization Algorithm

1 Initialize X(0)k , Xk, U

(0)k and Uk and set j = 0;

2 Compute {λi} and{

∂ Hi

∂ui

}

;

3 Compute the search direction ∆(j)k ;

4 Compute the step size s(j) using Wolfe’s rule;

5 Compute U(j+1)k = U

(j)k + s(j) ∆

(j)k and X

(j+1)k ;

6 Test stop condition: if false set j = j + 1 and go to step (2);

if true apply u(j+1)k to system.

Quasi-Newton search direction: ∆(j)k = −D(j) ∂ H

(j)k

∂U(j)k

.



IntroductionModel

ControllerResults


AlgorithmLine Search – Wolfe conditions

Step size optimization subproblem:

s∗ = arg mins≥0

φ(s)

Cost Functional:

φ(s) = Jk

(

X(j+1)k , U

(j+1)k

)

, φ′(s) =dφ(s)

ds

Wolfe sets (σ and λ are tuning constants):

A = {s > 0 : φ(s) ≤ φ(0) + σ φ′(0) s ∧ φ′(s) ≥ λφ′(0)}L = {s > 0 : φ(s) > φ(0) + σ φ′(0) s}R = {s > 0 : φ(s) ≤ φ(0) + σ φ′(0) s ∧ φ′(s) < λφ′(0)}

Algorithm finds an estimate of s∗ by selecting s ∈ A.



IntroductionModel

ControllerResults

Simulation ResultsSummary

Outline







IntroductionModel

ControllerResults


Simulation ResultsImplementation

Horizon N = 70;

Sample time Ts = 0.02s;

Simulation trajectory:

Hover at initial position;Straight line until final position;Hover at final position;

Terrain simulates a river bed;

−100

−50

0

50

100

−100

−50

0

50

100

20

40

XY

−Z

Reference trajectory colides twice with terrain;

Simplified nonlinear model used in MPC;

Full nonlinear model used as the plant.



IntroductionModel

ControllerResults


Simulation Results

0 10 20 30 40 50 60 70 80 90 100 110−200

−100

0

100

200

0 10 20 30 40 50 60 70 80 90 100 110−50

0

50

Pos

ition

[m]

0 10 20 30 40 50 60 70 80 90 100 110−40

−35

−30

−25

Time[s]

xReference x

yReference y

zReference z

Position

0 10 20 30 40 50 60 70 80 90 100 1100.04

0.05

0.06

θ c 0 [rad

]

0 10 20 30 40 50 60 70 80 90 100 1100

0.01

0.02

θ c 1c

[rad

]

0 10 20 30 40 50 60 70 80 90 100 110−0.02

0

0.02

θ c 1s

[rad

]

0 10 20 30 40 50 60 70 80 90 100 110

0.06

0.08

0.1

θ c 0t

[rad

]

Time[s]

Actuation



IntroductionModel

ControllerResults


Simulation ResultsHelicopter Trajectory

−100

−50

0

50

100

−100

−50

0

50

100

20

40

X

Simulating: t = 000.00/110.00 seg.

Y

−Z



IntroductionModel

ControllerResults


Summary

The results indicate that the presented methodology canachieve effective terrain avoidance while steering the vehiclealong a reference trajectory

Full nonlinear model of the helicopter;Repulsive field constraint for terrain avoidance;Input and State saturation constraints;Quasi-Newton Algorithm for minimization;Line search algorithm reduces computational effort.

Further research:

Nonlinear model simplification;Efficient computation of the closest terrain point;Tuning sampling frequency and prediction horizon of the MPCcontroller.



The end

Thank you for your time.



Short Bibliography

R. Cunha and C. Silvestre.Dynamic modeling and stability analysis of model-scale helicopters.Austin, Texas, August 2003.

D. Mayne, J. Rawlings, C. Rao, and P. Scokaert.Constrained model predictive control: Stability and optimality.Automatica, 36:790–814, 2000.Survey Paper.

Jorge Nocedal and Stephen Wright.Numerical Optimization.Springer Series in Operation Reasearch. Springer, 1999.

G. Sutton and R. Bitmead.Computational implementation of nonlinear model predictive control to nonlinearsubmarine.In F. Allgower and A. Zheng, editors, Nonlinear Model Predictive Control,volume 26 of Progress in Systems and Control Theory, pages 461–471.Birkhauser Verlag, Basel-Boston-Berlin, 2000.



Terrain Avoidance Model Predictive Control for

Autonomous Rotorcraft

B. Guerreiro C. Silvestre R. Cunha

Instituto Superior TecnicoInstitute for Systems and Robotics

Av. Rovisco Pais, 11049-001 Lisboa, Portugal

(email: {bguerreiro,cjs,rita}@isr.ist.utl.pt )

October 7th, 2008


terrain avoidance model predictive control for autonomous ... · terrain avoidance model predictive...

Documents