16.323 Lecture 17

DIDO, A Pseudospectral Method for Solving Optimal Control Problems

• M. Ross, “User’s Manual For DIDO (Ver. PR.1β): A MATLABTM Application Package for Solving Optimal Control Problems,” Naval Postgraduate School, Monterey, CA, Tech. Rep. 0401.0, Feb. 2004. [Online]. Available: http://tomlab.biz/docs/DIDOManualPR1.pdf

• G. Elnagar, M. A. Kazemi and M. Razzaghi, “The Pseudospectral Legendre Method for Discretizing Optimal Control Problems,” IEEE Trans. Automatic Control, vol. 40, no. 10, pp. 17931796, Oct. 1995.

• M. Ross and F. Fahroo, “A Direct Method for Solving Nonsmooth Optimal Control Problems,” in Proc. 2002 IFAC World Congress, Barcelona, Spain, Jul. 2002, pp. 479484.

http://tomlab.biz/docs/DIDOManualPR1.pdf

Spr 2006 16.323 15–1 DIDO

• Many methods have been proposed over the last few decades to solve the general nonlinear Optimal Control Problem (OCP). Some of these have been developed to a sufficiently mature level, and made available as a self contained package:

– DIDO1 Ross et. al, Naval Postgraduate School

– Sparse Optimal Control Software (SOCS)2 Betts, Boeing

– Nonlinear Trajectory Generatory (NTG)3 Milam et. al, Caltech

– RIOTS 954 Schwartz, UC Berkeley

• Investigate one of the currently available solvers, DIDO5, chosen in part for its ability to solve very complex problems and most notably, the distinct theoretical foundation on which it is built.

• Common procedure for solving the OCP is to transform the problem into an equivalent parameter optimization problem that can be solved using well developed Nonlinear Program (NLP) solver.

– DIDO adopts the same procedure, and the NLP solver is based on Sequential Quadratic Programming (SQP) and named SNOPT6 .

– So how is DIDO different? Legendre Pseudospectral Method

1M. Ross, “User’s Manual For DIDO (Ver. PR.1β): A MATLABTM Application Package for Solving Optimal Control Problems,” Naval Postgraduate School, Monterey, CA, Tech. Rep. 0401.0, Feb. 2004. [Online]. Available: http://tomlab.biz/docs/ DIDOManualPR1.pdf

2K. Holmström, F. Martinsen and M. Edvall, “User’s Guide for TOMLAB/SOCS,” Tomlab Optimization Inc., Feb. 2005. [Online]. Avaliable: http://tomlab.biz/docs/TOMLAB_SOCS.pdf

3M. Milam, K. Mushambi and R. Murray, NTG A Library for RealTime Trajectory Generation. [Online]. Available: http: //www.cds.caltech.edu/~murray/software/2002a_ntg.html

4A. Schwartz, E. Polak and Y. Chen. (1997, May), RIOTS 95TM: A Matlab Toolbox for Solving Optimal Control Problems. Available: http://www.accesscom.com/~adam/RIOTS/manual.pdf

5Kirk pp 107, “Queen Dido of Carthage was apparently the first person to attack a problem that can readily be solved by using variational calculus. Dido, having been promised all of the land she could enclose with a bull’s hide, cleverly cut the hide into many lengths and tied the ends together. Having done this, her problem was to find the closed curve with a fixed perimeter that encloses the maximum area.”

6P. Gill, W. Murray and M. Saunders, “SNOPT: An SQP Algorithm for LargeScale Constrained Optimization,” SIAM J. Optimization, vol. 12, no. 4, pp. 9791006, 2002.

May 10, 2006

http://tomlab.biz/docs/DIDOManualPR1.pdfhttp://tomlab.biz/docs/DIDOManualPR1.pdfhttp://tomlab.biz/docs/TOMLAB_SOCS.pdfhttp://www.cds.caltech.edu/~murray/software/2002a_ntg.htmlhttp://www.cds.caltech.edu/~murray/software/2002a_ntg.htmlhttp://www.accesscom.com/~adam/RIOTS/manual.pdf

Spr 2006 16.323 15–2 Direct vs Indirect Methods

• Solution techniques to the OCP can be classified as either Direct or Indirect Methods7 8

– Direct and Indirect variants of Shooting, Multiple Shooting, Transcription/Collocation Methods are available

Indirect Methods •

– Aim is to find an approximate solution to the necessary conditions

– Pros

3 High accuracy

3 Solution satisfy necessary optimality conditions

– Cons

3 Necessary optimality conditions must be derived analytically (most likely hard!)

3 Small radius of convergence: Need a good initial guess

3 Need a guess for costates

3 Need to know a priori the constrained and unconstrained subarcs

Direct Methods •

– Transform the continuous OCP into a discrete NLP

– Pros: Circumvents the “Cons” listed under Indirect Methods

– Cons

3 Not as accurate as Indirect methods

3 Many Direct methods do not produce costate information

7J. Betts, “Survey of Numerical Methods for Trajectory Optimization,” J. Guidance, Control and Dynamics, vol. 21, no. 2, pp. 193207, Mar. 1998.

8D. Benson, “A Gauss Pseudospectral Transcription for Optimal Control,” Ph.D. dissertation, Dept. Aero. Astro., MIT, Cambridge, MA, 2005.

May 10, 2006

Spr 2006 16.323 15–3

• DIDO resembles the Direct Transcription/Collocation method, with the Legendre polynomials as the basis for function approximations

• It has been shown that the Legendre Pseudospectral Method (albeit a Direct Method) has the property that the solution will satisfy the necessary optimality conditions.

– This is not achieved for most other Direct Methods.9

9M. Ross and F. Fahroo, “A Perspective on Methods for Trajectory Optimization,” in Proc. AIAA/AAS Astrodynamics Specialist Conf., Monterey, 2002, AIAA 20024727.

May 10, 2006

Spr 2006 16.323 15–4

Spectral Methods

• DIDO uses the Legendre Pseudospectral Method, which have been used extensively in fluid flow modeling.

• DIDO most closely resembles the Direct Collocation Method.

– Main difference: traditional collocation methods use a fixed order polynomial (eg. cubic) to approximate the states and controls for each segment between nodes and DIDO uses the Legendre polynomials as the basis to approximate the variables over multiple nodes (over an entire segment).

– Now look at Spectral methods for function approximation.

• Suppose we want to approximate a periodic function u(t), which can be represented by the Fourier series: � 1 � 2π∞

u(t) = ûkeikt ûk = u(τ )e

−ikτdτ↔ 2π 0k=−∞

• For practical purposes, our approximation can at best be a truncated series. Furthermore, we need some way to calculate ûk using a finite number of points of u(τ ) within the interval of interest.

• The question then comes down to how many terms of the series we must include to get a “good enough” approximation to u(t), and how to approximate ûk with a finite number of points.

• Turns out that the Fourier method exhibits spectral accuracy. 10

“The kth coefficient of the expansion decays faster than any inverse power of k when the function is infinitely smooth and all its derivatives are periodic as well. In practice, this decay is not exhibited until there are enough coefficients to represent all the essential structures of the function. The subsequent rapid decay of the coefficients implies that the Fourier series truncated after just a few more terms represents an exceedingly good approximation of the function. This characteristic is usually referred to as “spectral accuracy” of the Fourier method.”

10C. Canuto, M. Hussaini, A. Quateroni and T. Zang, Spectral Methods in Fluid Dynamics. Berlin, Germany: SpringerVerlag, 1988, ch. 2.

May 10, 2006

Spr 2006 16.323 15–5

• As to the calculation of ûk, it can be shown that using Gauss type quadratures11 is optimal

– Produces best approximation with least number of data points in the interval of interest.

– Well known quadrature techniques (main differences are at the endpoints of the intervals.)

3 Gauss quadrature

3 GaussRadau

3 GaussLobatto

• For nonperiodic functions, can use orthogonal systems of polynomials

– Legendre Polynomials

– Chebyshev Polynomials

– Jacobi Polynomials encompasses both Legendre and Chebyshev

• In essence, spectral methods for approximating functions (including nonperiodic functions) can achieve much better accuracy with fewer terms in a truncated series.

• There is an extensive literature available on Spectral and Pseudospectral Methods.

– Free book is: J. Boyd, Chebyshev and Fourier Spectral Methods. New York: Dover, 2000.12

11Gauss quadrature is a method to approximate a definite integral by a finite sum

http://wwwpersonal.engin.umich.edu/~jpboyd/aaabook_9500may00.pdf

May 10, 2006

12

http://www-personal.engin.umich.edu/~jpboyd/aaabook_9500may00.pdf

Spr 2006 16.323 15–6 Legendre Pseudospectral Method

• DIDO is a powerful computational tool13 that can solve fairly complex OCPs including nonsmooth problems that have state/control discontinuities.

– These discontinuities arise naturally in OCPs as can be seen in the bangbang controls that results from minimum time problems.

• To capture the nonsmooth behavior, define an event as a discrete time point associated with a set of constraints that must be satisfied.

– Consider one interior event at time τe, but extends to more than one interior event.

– Boundary conditions at τ0 and τf are included in this definition.

– The event conditions are:

+ el≤ e(x0,xe−,xe ,xf , τ0, τe, τf) ≤ eu (1)

where x− = lim�↑0 x(τe + �) and xe− = lim� 0 x(τe + �)e ↓

• The general problem is to determine the statecontrol pair and possibly also the event times, τe, τf , that minimize the cost functional:

+J(x(.),u(.), τ0, τe, τf) = E(x0,xe−,xe ,xf , τ0, τe, τf)+� τf (2)

F (x(τ ),u(τ ), τ )dτ τ0

subject to the dynamic constraints:

ẋ(τ ) = f (x(τ ),u(τ ), τ )

event constraints (1), and mixed statecontrol path constraints:

gl≤ g(x(τ ),u(τ ), τ ) ≤ gu (3)

13M. Ross and F. Fahroo, “A Direct Method for Solving Nonsmooth Optimal Control Problems,” in Proc. 2002 IFAC World Congress, Barcelona, Spain, Jul. 2002, pp. 479484.

May 10, 2006

� � �

Spr 2006 16.323 15–7

• These functions can be nonsmooth at the event time τe but are continuously differentiable in the open intervals (τ0, τe), and (τe, τf ).

– If we use superscript 1 to denote the functions in the first interval and superscript 2 to denote functions in the second interval,

F 1(x(τ ), u(τ ), τ ) for τ ∈ [τ0, τe]F (.) = (4)

F 2(x(τ ), u(τ ), τ ) for τ ∈ [τe, τf ]

f 1(x(τ ), u(τ ), τ ) for τ ∈ [τ0, τe]f (.) = (5)

f 2(x(τ ), u(τ ), τ ) for τ ∈ [τe, τf ]

g1(x(τ ), u(τ ), τ ) for τ ∈ [τ0, τe] g(.) = (6)

g2(x(τ ), u(τ ), τ ) for τ ∈ [τe, τf ]

If there is more than one interior event, we simply partition the above functions in time accordingly.

• In the Legendre pseudospectral approximation, the node points are optimally chosen in the interval [−1, 1] by the LegendreGaussLobatto (LGL) quadrature.

– To map the time coordinate to the interval [−1, 1], define:

τ 1 (τe − τ0)t + (τe + τ0)

τ 2 (τf − τe)t + (τf + τe)

= = 2 2

• The LGL node points are closely related to the Legendre polynomials.

– LN (t) is the Legendre polynomial of deg N on the interval [−1, 1].

– LGL node points tl, l = 0, ..., N are t0 = −1, tN = 1 and for 1 ≤ l ≤ N − 1, tl are the zeros of L̇N (t), the derivative of LN (t).

– In the first segment [τ0, τe], there are N 1 + 1 LGL points, and in the second segment [τe, τf ], N 2 + 1 LGL points.

May 10, 2006

�

Spr 2006 16.323 15–8

• The unknown state and controls are then represented using the Lagrange interpolating polynomials, φl(t), as:

2N�1N� 1 2 xN (τ

1) = x 1(τl 1)φl(t) xN (τ

2) = x 2(τl 2)φl(t)

l=0 l=0 2N�1N�

1 2 uN (τ1) = u 1(τl

1)φl(t) uN (τ2) = u 2(τl

2)φl(t)

l=0 l=0

and φl(t) is related to the Legendre polynomials through:

1 (t2 − 1)L̇N i (t)φl(t) = Ni(Ni + 1)LN i (tl) t − tl

• This results in the following: +J(x(.), u(.), τ0, τe, τf ) = E(x0, xe

−, xe , xf , τ0, τe, τf )� 1 +

τe − τ0 F 1(x(t), u(t), τ (t))dt

2 �−11 (7) +

τf − τe F 1(x(t), u(t), τ (t))dt

2 −1 with the constraints:

τe−τ0 f 1(x(t), u(t), τ (t)) for τ ∈ [τ0, τe] ẋ(t) = 2 τf −τe f 2(x(t), u(t), τ (t)) for τ ∈ [τe, τf ]

(8) 2

+ el≤ e(x0, xe−, xe , xf , τ0, τe, τf ) ≤ eu (9) 1 1gl ≤ g (x(t), u(t), τ (t)) ≤ g 1 (10) u 2 2gl ≤ g (x(t), u(t), τ (t)) ≤ g 2 (11) u

• One of the key steps in this method is to impose the condition that the above approximations satisfy the differential constraints (8) at the LGL node points (collocation).

• The next step is to approximate the integrals in the cost functional (7)by a finite sum using the GaussLobatto quadrature.

May 10, 2006

Spr 2006 16.323 15–9

• The problem has now been transformed to an NLP where the unknowns are the coefficients x1(τi

1), u1(τi 1), x2(τj

2), and u2(τj 2) for

i = 0, ..., N 1 , j = 0, ..., N 2 .

– DIDO then uses the NLP solver SNOPT to solve the problem.

Note: •

– The concept of events and event constraints are introduced to solve nonsmooth OCPs. ⇒ The Legendre Pseudospectral Method alone can only solve smooth problems.

– The Legendre Pseudospectral Method is just another way of discretizing the problem to cast as an NLP.

3 However, it does this using Legendre polynomials and GaussLobatto quadratures (with very good reasons!)

– The constraints can only be enforced at the LGL node points and event times. Particularly, the constraints may not be satisfied in between the node points. This is a common trait of collocation methods.

May 10, 2006

�

�

�

Spr 2006 16.323 15–10 Example Solved using DIDO

• Consider a dropoff mission for a UAV. – Desired to drop off some supplies/cargo (via parachute) at a spec

ified location in minimum time and return to base with minimum fuel expenditure on the return leg.

• The simplified dynamics of the UAV flying in a horizontal plane can be modeled by:

ẋ(t) = V (t) cos θ(t)

ẏ(t) = V (t) sin θ(t)

θ̇(t) = ω(t)

V̇ (t) = −cdV 2(t) + T (t)/(m + m0) for t ≤ tD −cdV 2(t) + T (t)/m for t > tD

where θ(t) is the heading angle with respect to the x axis, V (t) is the speed of the UAV, cd is some scaled constant proportional to the drag coefficient, m is UAV mass, m0 is the supply/cargo mass, ω(t) and T (t) ≥ 0 are the controls, and tD is the dropoff time.

• Want to minimize the dropoff time, tD, and the fuel expended during the return leg to base, which is from tD to tf :

tf

min J = tD + λ T (t)dt tD

where λ is a suitable weighting factor. Both tD and tf are free.

The initial conditions are • x(0) = x0 y(0) = y0 θ(0) = θ0 v(0) = v0

• Desired dropoff point is specified as xD, yD, and we require the UAV to activate the dropoff within a circle of radius � m:

(x(tD) − xD)2 + (y(tD) − yD)2 ≤ � or equivalently (x(tD) − xD)2 + (y(tD) − yD)2 ≤ �2 which must be satisfied at t = tD.

May 10, 2006

Spr 2006 16.323 15–11

• Final target point is xf , yf , and the UAV is required to come within a circle of radius � to consider the mission accomplished at t = tf :

(x(tf )− xf )2 + (y(tf )− yf )2 ≤ �2

• Impose limits on the turning radius of the UAV: 1 ω(t) 1 −

rmin ≤

V (t) ≤

rmin

• For the UAV to fly, it must fly above the stall speed, vL. The upper limit beyond which the UAV cannot fly safely is vH .

vL ≤ V (t) ≤ vH Finally, there are limits on achievable thrust:

0 ≤ T (t) ≤ TH

• The numerical values of the parameters for this problem are :

x0 = 0m y0 = 0m θ0 = 0 deg v0 = 20m/s

xD = 100m yD = 100m xf = 0m yf = 0m

m = 50kg m0 = 25kg cd = 0.002m−1 � = 2m

rmin = 50m vL = 17m vH = 25m TH = 200N

λ = 0.001N−1

• As with any numerical method, scaling the problem appropriately can drastically improve its computational efficiency.

– The objective is to scale all variables such that their magnitudes are of order 1.

– One way to scale the variables is to define the length (lsc), velocity (vsc), time (tsc), acceleration (asc), mass (msc) and thrust (Tsc)

May 10, 2006

� �

�

Spr 2006 16.323 15–12

scaling factors as:

lsc = (xD − x0)2 + (yD − y0)2 + (xD − xf )2 + (yD − yf )2

vsc = vH tsc = lsc/vsc asc = vsc/tsc msc = m + m0 Tsc = mscasc

and use these to nondimensionalize all variables in the problem

• For example, to scale the objective function, define the variable τ = t/tsc so that dt = tscdτ , and the objective function is scaled as:

tf

min J = tD + λ T (t)dt � tD � tD

� τf T (τ ) = tsc + λTsc dτ

tsc τ0 Tsc

and the equivalent scaled objective function is: � τf min J̃ = t̃D + λ̃ T̃ (τ )dτ

τ0

T (τ )˜where t̃D = tD , λ = λTsc, and T̃ (τ ) = . Note that each scaled tsc Tsc

variable is dimensionless.

• The problem is particularly suited to be solved by DIDO using one internal “knot” to enforce the constraint at the dropoff point as well as to allow the minimization of the dropoff time, tD.

• The problem is solved using DIDO on a Pentium IV 2.2GHz PC with a computation time of 8.6 s.

– The time to reach the dropoff point (tD) is 6.1 s while the mission time (tf ) is 21.8 s.

– Suggests a limit on the type of problems where realtime optimization in the Model Predictive Control framework using DIDO can be applied.

May 10, 2006

Spr 2006 16.323 15–13

−100 −50 0 50 1000

50

100

150

x (m)

y (m

)Trajectory

Phase 1 − Min TimePhase 2 − Min FuelInitial Guess

Figure 1: Trajectory

0 5 10 15 20 250

200

400

θ (d

eg)

States

0 5 10 15 20 2515

20

25

t (s)

v (m

/s)

Figure 2: States History

May 10, 2006

Spr 2006 16.323 15–14

0 5 10 15 20 25

0

100

200

Thru

st (N

)Controls

0 5 10 15 20 25−0.5

0

0.5

t (s)

ω (r

ad/s

)

Figure 3: Controls History

0 5 10 15 20 25−0.005

0

0.005

0.01

0.015

0.02

0.025

t (s)

r (m

)

Path Constraints

Figure 4: Path Constraint

May 10, 2006

Spr 2006 16.323 15–15

Code for UAV Drop Problem

% Matlab script uavdrop.m % Note: This code required DIDO % toolbox.

close all; clear all;

m = 50; m0 = 25; x0 = 0; y0 = 0; theta0 = 0*pi/180; v0 = 20; xD = 100; yD = 100; xT = 0; yT = 0; rmin = 50; vL = 17; vH = 25; tL = 0; tH = (vH vL)/3*(m + m0); epssq = 4; Cd = 0.002; lambda = 0.001;

% Scaling constants

% Length scale

lsc = sqrt((xD x0)^2 + (yD

vsc = vH;

tsc = lsc/vsc;

asc = vsc/tsc;

msc = m + m0;

Tsc = msc*asc;

% Scaled constants

ms = m/msc;

m0s = m0/msc;

x0s = x0/lsc;

y0s = y0/lsc;

theta0s = theta0;

v0s = v0/vsc;

xDs = xD/lsc;

yDs = yD/lsc;

xTs = xT/lsc;

yTs = yT/lsc;

rmins = rmin/lsc;

vLs = vL/vsc;

vHs = vH/vsc;

tLs = 0/Tsc;

tHs = tH/Tsc;

epssqs = epssq/lsc/lsc;

global CONST;

CONST.ms

CONST.m0s

CONST.xDs

CONST.yDs

CONST.xTs

CONST.yTs

CONST.lambdas

= ms;

= m0s;

= xDs;

= yDs;

= xTs;

= yTs;

= lambda*Tsc;

to run on Matlab. DIDO is a third party Matlab

% UAV mass [kg]

% Payload mass [kg]

% Initial x position [m]

% Initial y position [m]

% Initial orientation [rad]

% Initial speed [m/s]

% Drop point x position [m]

% Drop point y position [m]

% Target point x position [m]

% Target point y position [m]

% Minimum turn radius [m]

% Minimum speed/ stall speed [m/s]

% Maximum speed [m/s]

% Minimum (negative) thrust [N]

% Maximum thrust [N]

% Square of distance tolerance [m^2]

% Equivalent drag coefficient [s^(1)]

% Cost weighting factor [N^(1)]

y0)^2) + sqrt((xD % Speed scale % Time scale % Acceleration scale % Mass scale % Thrust scale

xT)^2 + (yD yT)^2);

% UAV mass, scaled % Payload mass, scaled % Initial x position, scaled % Initial y position, scaled % Initial orientation [rad], scaled % Initial speed, scaled % Drop point x position, scaled % Drop point y position, scaled % Target point x position, scaled % Target point y position, scaled % Minimum turn radius, scaled % Minimum speed/ stall speed, scaled % Maximum speed, scaled % Minimum (negative) thrust, scaled % Maximum thrust, scaled % Square of distance tolerance, scaled

May 10, 2006

Spr 2006 16.323 15–16

CONST.Cd = Cd*lsc;

% Problem % problem specifies the Matlab functions that DIDO is supposed to call by their % filenames problem.cost = ’costfun’; problem.dynamics = ’dynamicsfun’; problem.events = ’eventsfun’; % Not needed if no events problem.path = ’pathfun’; % Not needed if no path constraints

% Knots % knots.locations = [t0 tf]: 1 * Nk, best guess of t0 and tf if unknown % knots.definitions = {’hard’, ’hard’}: 1 * Nk cell array ’hard’ or ’soft’ % knots.bounds.lower = [t0L tfL]: 1 * Nk, lower bounds for knots % knots.bounds.upper = [t0U tfU]: 1 * Nk, upper bounds for knots % knots.numNodes = [Nn] (recommended Nn >= 12): 1 * (Nk 1) % Notes % For bounds, can specify Inf % t0L

Spr 2006 16.323 15–17

guess.states(1,:) = xgs’;

guess.states(2,:) = ygs’;

guess.states(3,:) = thetag’;

guess.states(4,:) = v0s*ones(1,length(xgs));

guess.controls(1,:) = wgs’;

guess.controls(2,:) = zeros(1,length(xgs));

guess.time = tgs’;

knots.locations = tgs(1:10:length(xgs))’;

% Call DIDO

t0 = clock;

[cost, primal] = dido(problem, knots, bounds, guess);

ptime = etime(clock, t0);

x = primal.states(1,:)*lsc;

y = primal.states(2,:)*lsc;

theta = primal.states(3,:);

v = primal.states(4,:)*vsc;

omega = primal.controls(1,:)/tsc;

tr = primal.controls(2,:)*asc*msc;

t = primal.nodes*tsc;

tD = t(primal.indices.left);

tT = t(end);

dD = sqrt((x(primal.indices.left) xD)^2 + (y(primal.indices.left) yD)^2);

dT = sqrt((x(end) xT)^2 + (y(end) yT)^2);

disp([’Distance to (xD, yD): ’ num2str(dD) ’ m’]);

disp([’Distance to (xT, yT): ’ num2str(dT) ’ m’]);

disp([’Time to (xD, yD): ’ num2str(tD) ’ s’]);

disp([’Time to (xT, yT): ’ num2str(tT) ’ s’]);

disp([’Processing Time: ’ num2str(ptime) ’ s’]);

tmp = (0:1:360)*pi/180;

xc = sqrt(epssq)*cos(tmp);

yc = sqrt(epssq)*sin(tmp);

ind1 = 1:primal.indices.left;

ind2 = primal.indices.right:length(x);

plot(x(ind1), y(ind1), ’b’, x(ind2), y(ind2), ’r’);

hold on;

plot(xgs*lsc, ygs*lsc, ’m:’);

plot(xD + xc, yD + yc, ’m’);

plot(xT + xc, yT + yc, ’m’);

axis equal;

grid on;

xlabel(’x (m)’);

ylabel(’y (m)’);

title(’Trajectory’);

legend(’Phase 1 Min Time’, ’Phase 2 Min Fuel’, ’Initial Guess’, 0);

figure;

subplot(2,1,1);

plot(t(ind1), tr(ind1), ’b’, t(ind2), tr(ind2), ’r’);

grid on;

hold on;

plot([min(t) max(t)], tH*[1 1], ’m’);

plot([min(t) max(t)], tL*[1 1], ’m’);

ylabel(’Thrust (N)’);

title(’Controls’);

subplot(2,1,2);

plot(t(ind1), omega(ind1), ’b’, t(ind2), omega(ind2), ’r’);

May 10, 2006

Spr 2006 16.323 15–18

grid on;

xlabel(’t (s)’);

ylabel(’\omega (rad/s)’);

figure;

subplot(2,1,1);

plot(t(ind1), theta(ind1)*180/pi, ’b’, t(ind2), theta(ind2)*180/pi, ’r’);

grid on;

ylabel(’\theta (deg)’);

title(’States’);

subplot(2,1,2);

plot(t(ind1), v(ind1), ’b’, t(ind2), v(ind2), ’r’);

grid on;

hold on;

plot([min(t) max(t)], vH*[1 1], ’m’);

plot([min(t) max(t)], vL*[1 1], ’m’);

xlabel(’t (s)’);

ylabel(’v (m/s)’);

figure;

plot(t, omega./v);

grid on;

hold on;

plot([min(t) max(t)], 1/rmin*[1 1], ’m’);

xlabel(’t (s)’);

ylabel(’r (m)’);

title(’Path Constraints’);

% End uavdrop.m

function [Ed, Fd] = costfun(primal)

% Input parameter primal properties

% primal.states: Nx * Nn

% primal.controls: Nu * Nn

% primal.nodes: 1 * Nn

% primal.statedots: Nx * Nn

% Output properties

% Ed is the Mayer cost, Fd is the Lagrange cost

% Ed = f(x0, xf, t0, tf): 1 * 1

% x0 = primal.states(:,1)

% xf = primal.states(:,end)

% t0 = primal.nodes(1)

% tf = primal.nodes(end)

% Fd : 1 * Nn

% Notes

% If Ed not required, set Ed = 0, NOT Ed = []

% If Fd not required, set Fd = 0, NOT Fd = []

global CONST;

Ed = primal.nodes(primal.indices.left);

Fd = zeros(1,size(primal.states,2));

ind2 = primal.indices.right:size(primal.states,2);

Fd(ind2) = CONST.lambdas*abs(primal.controls(2,ind2));

% End costfun.m

function rd = dynamicsfun(primal)

% Input parameter primal properties

May 10, 2006

Spr 2006 16.323 15–19

% primal.states: Nx * Nn

% primal.controls: Nu * Nn

% primal.nodes: 1 * Nn

% primal.statedots: Nx * Nn

% Output properties

% rd is the discretized residuals of the differential equation

% r = xdot f(x, u, t)

% rd: Nx * Nn

dx = primal.statedots(1,:);

dy = primal.statedots(2,:);

dtheta = primal.statedots(3,:);

dv = primal.statedots(4,:);

x = primal.states(1,:);

y = primal.states(2,:);

theta = primal.states(3,:);

v = primal.states(4,:);

omega = primal.controls(1,:);

t = primal.controls(2,:);

global CONST;

rd = primal.states; % Allocate memory to improve efficiency

rd(1,:) = dx v.*cos(theta);

rd(2,:) = dy v.*sin(theta);

rd(3,:) = dtheta omega;

ind1 = 1:primal.indices.left;

ind2 = primal.indices.right:length(x);

rd(4,ind1) = dv(ind1) (CONST.Cd*(v(ind1).^2) + ...

t(ind1)/(CONST.ms + CONST.m0s)); rd(4,ind2) = dv(ind2) (CONST.Cd*(v(ind2).^2) + t(ind2)/CONST.ms);

% End dynamicsfun.m

function ed = eventsfun(primal)

% Input parameter primal properties

% primal.states: Nx * Nn

% primal.controls: Nu * Nn

% primal.nodes: 1 * Nn

% primal.statedots: Nx * Nn

% Output properties

% ed is the event function that provides the boundary conditions

% ed: Ne * 1

x0 = primal.states(1,1);

y0 = primal.states(2,1);

theta0 = primal.states(3,1);

v0 = primal.states(4,1);

xd = primal.states(1,primal.indices.left);

yd = primal.states(2,primal.indices.left);

xt = primal.states(1,end);

yt = primal.states(2,end);

thetat = primal.states(3,end);

ed = zeros(14,1); % Allocate memory to improve efficiency

global CONST;

% For continuity of states at knot points

May 10, 2006

Spr 2006 16.323 15–20

ed(1:4) = primal.states(:,primal.indices.left) ... primal.states(:,primal.indices.right);

ed(5:8) = primal.statedots(:, primal.indices.left) ... primal.statedots(:, primal.indices.right);

% For continuity of states at knot points

% Initial conditions constraints ed(9:12) = [x0; y0; theta0; v0]; % Drop point constraints ed(13) = (xd CONST.xDs)^2 + (yd CONST.yDs)^2; % Target point constraints ed(14) = (xt CONST.xTs)^2 + (yt CONST.yTs)^2;

% End eventsfun.m

function hd = pathfun(primal)

% Input parameter primal properties % primal.states: Nx * Nn % primal.controls: Nu * Nn % primal.nodes: 1 * Nn % primal.statedots: Nx * Nn

% Output properties % hd specifies the path constraints at the nodes % hd: Nh * Nn

v = primal.states(4,:); omega = primal.controls(1,:);

hd = zeros(1,size(v,2)); % Allocate memory to improve efficiency hd(1,:) = omega./v;

% End pathfun.m

function [x, y, t, w, theta] = calguess(xa, ya, theta0, v0, wgmax, numpoints)

if(numpoints < 1) disp(’numpoints must be >= 1’); return;

end

nwaypoints = length(xa); if(nwaypoints ~= length(ya))

disp(’length(xa) ~= length(ya)’); return;

end

x0 = xa(1);

y0 = ya(1);

xg = xa(2);

yg = ya(2);

x = zeros(numpoints*(nwaypoints 1) + 1, 1);

y = x;

t = x;

w = x;

theta = x;

t0 = 0;

for(ii = 1:(nwaypoints 1))

[x1, y1, t1, w1, theta1] = calguess1(x0, y0, theta0, v0, xg, yg, ... wgmax, numpoints);

index = (1:(numpoints + 1)) + (ii 1)*numpoints;

May 10, 2006

Spr 2006 16.323 15–21

x(index) = x1;

y(index) = y1;

t(index) = t1 + t0;

w(index) = w1;

theta(index) = theta1;

if(ii < (nwaypoints 1))

t0 = t(max(index));

x0 = x(max(index));

y0 = y(max(index));

theta0 = theta(max(index));

xg = xa(ii + 2);

yg = ya(ii + 2);

end end

theta = unwrap(theta);

% End calguess.m

function [x, y, t, w, theta] = calguess1(x0, y0, theta0, v0, xg, yg, wgmax, ... numpoints)

if(numpoints < 1) disp(’numpoints must be >= 1’); return;

end

dx = xg x0;

dy = yg y0;

len = sqrt(dx*dx + dy*dy);

costheta0 = cos(theta0);

sintheta0 = sin(theta0);

theta0 = mod(theta0 + pi, 2*pi) pi;

phi = mod(atan2(dy, dx) theta0 + pi, 2*pi) pi;

if(abs(phi) < 0.1*pi/180) % Goal lies straight ahead current heading

wg = 0;

thetag = theta0;

tg = len/v0;

xgr = x0 + v0*costheta0;

ygr = y0 + v0*sintheta0;

t = (0:(tg/numpoints):tg)’;

w = wg*ones(size(t));

theta = theta0 + w.*t;

x = x0 + v0*t*costheta0;

y = y0 + v0*t*sintheta0;

elseif(abs(phi) > 165*pi/180) % Goal lies right behind current heading if(phi >= 0)

wg = wgmax; else

wg = wgmax;

end

tg = pi/abs(wg);

thetag = theta0 + wg*tg;

npoints1 = floor(numpoints/2);

npoints2 = ceil(numpoints/2);

t = zeros(1 + numpoints,1); % Allocate memory

x = t; % Allocate memory

y = t; % Allocate memory

May 10, 2006

Spr 2006 16.323 15–22

w = t; % Allocate memory

theta = t; % Allocate memory

index = 1:(npoints1+1);

t(index) = (0:(tg/npoints1):tg)’;

w(index) = wg*ones(npoints1+1,1);

theta(index) = theta0 + w(index).*t(index);

x(index) = x0 + v0/wg*(sin(theta(index)) sintheta0);

y(index) = y0 v0/wg*(cos(theta(index)) costheta0);

x0 = x(npoints1+1);

y0 = y(npoints1+1);

theta0 = theta(npoints1+1);

[x1, y1, t1, w1, theta1] = calguess1(x0, y0, theta0, v0, xg, yg, ...

wgmax, npoints2);

index = (npoints1+1):(numpoints+1);

t(index) = tg + t1;

w(index) = w1;

theta(index) = theta0 + w1.*t1;

x(index) = x1;

y(index) = y1;

else % Intermediate range ang_diff = phi; thetag = mod(atan2(dy, dx) + ang_diff + pi, 2*pi) pi; if(abs(dx) > abs(dy))

wg = v0*(sin(thetag) sintheta0)/dx;

else

wg = v0*(cos(thetag) costheta0)/dy;

end

ang_diff = thetag theta0;

if(wg > 0)

while(ang_diff = 0)

ang_diff = ang_diff 2*pi;

end

end

tg = ang_diff/wg;

xgr = xg;

ygr = yg;

t = (0:(tg/numpoints):tg)’;

w = wg*ones(size(t));

theta = theta0 + w.*t;

x = x0 + v0/wg*(sin(theta) sintheta0);

y = y0 v0/wg*(cos(theta) costheta0);

end

% End calguess1.m

May 10, 2006

Lecture 17: DIDODIDODirect vs Indirect MethodsSpectral MethodsLegendre Pseudospectral MethodExample Solved using DIDOCode for UAV Drop Problem