Optimal Control For Autonomous UnderwaterVehicles

Monique Chyba - November 25, 2015Departments of Mathematics, University of Hawai‘i at Manoa

Elective in Robotics 2015/2016 - Control of Unmanned Vehicles

Literature

B.Bonnard and M.Chyba.Singular Trajectories and their Role inControl Theory. Springer-Verlag, 2003.

M. Chyba, T. Haberkorn, R.N. Smith, S.K. Choi. Design andimplementation of time efficient trajectories for an underwatervehicle. Ocean Engineering, 35/1, pp. 63-76, 2008.

M. Chyba, T. Haberkorn, R.N. Smith, S.K. Choi. AutonomousUnderwater Vehicles: Development and Implementation of time andEnergy Efficient Trajectories. Ship Technology Research, 55/2,pp.36-48, 2008.

M. Chyba, T. Haberkorn, R.N. Smith, G.R. Wilkens. A GeometricAnalysis of Trajectory Design for Underwater Vehicles. Discrete andContinuous Dynamical Systems-B, Volume: 11, Number: 2, 2009.

M. Chyba, T. Haberkorn, S.B. Singh, R.N. Smith, S.K. Choi. —itIncreasing Underwater Vehicle Autonomy by Reducing EnergyConsumption. Ocean Engineering, Special Issues on AutonomousUnderwater Vehicles, Vol 36/1, pp. 62-73, 2009.

Equations of Motion

Definition

Under some symmetry assumptions, the equations of motion in thebody-fixed frame for a rigid body submerged in a real fluid aregiven by:

M ν = Mν × Ω + Dν(ν)ν + ϕν

JΩ = JΩ× Ω + Mν × ν + DΩ(Ω)Ω− rCB× RtρgVk + τΩ

(1)

where M accounts for the mass and added mass, J accounts forthe body moments of inertia and the added moments of inertia.The matrices Dν(ν),DΩ(Ω) represent the drag force and dragmomentum, respectively. The term −rCB

× RtρgVk is the rightingmoment induced by the buoyancy force. Finally,ϕν = (ϕν1 , ϕν2 , ϕν3)t and τΩ = (τΩ1 , τΩ2 , τΩ3)t account for thecontrol. For a rigid body moving in ideal fluid (air), we neglect thedrag effects: Dν(ν) = DΩ(Ω) = 0.

Maximum Principle

Assume that there exists an admissible time-optimal controlσ = (ϕν , τΩ) : [0,T ]→ F × T , such that the correspondingtrajectory χ = (η, ν,Ω) steers the AUV from χ0 to χT . Then,there exists an absolutely continuous vectorλ = (λη, λν , λΩ) : [0,T ]→ R12, λ(t) 6= 0 for all t, such that thefollowing conditions hold almost everywhere:

η =∂H

∂λη, ν =

∂H

∂λν, Ω =

∂H

∂λΩ,

λη = −∂H∂η

, λν = −∂H∂ν

, λΩ = −∂H∂Ω

,

where the Hamiltonian function H is given by:

H(χ, λ, σ) = λtη(Rν,ΘΩ)t + λtνM−1[Mν × Ω + Dν(ν)ν + ϕν ]

+λtΩJ−1[JΩ× Ω + Mν × ν + DΩ(Ω)Ω− rB × RtρgVk + τΩ].

Maximum Principle

Furthermore, the maximum condition holds:

H(χ(t), λ(t), σ(t)) = maxσ∈F×T

H(χ(t), λ(t), σ)

The maximum of the Hamiltonian is constant along the solutionsand must satisfy H(χ(t), λ(t), σ(t)) = λ0, λ0 ≥ 0. A triple(χ, λ, σ) which satisfies the Maximum Principle is called anextremal, and the vector function λ(·) is called the adjoint vector.

The maximum condition (22), along with the control domainF × T , is equivalent almost everywhere to (M, J diagonal andpositive), i = 1, 2, 3:

ϕνi (t) = αminνi

if λνi (t) < 0 and ϕνi (t) = αmaxνi

if λνi (t) > 0 (2)

τΩi(t) = αmin

Ωiif λΩi

(t) < 0 and τΩi(t) = αmax

Ωiif λΩi

(t) > 0 (3)

Clearly, the zeros of the functions λνi determine the structure ofthe solutions to the Maximum Principle, and hence of thetime-optimal control.

Definition

We denote the i th switching function by:

δi (t) = λt(t)Yi , (4)

for i = 1, . . . , 6.

Bang-Bang and Singular Arcs

Definition

We say that a component σi of the control is bang-bang on agiven interval [t1, t2] if its corresponding switching function δi isnonzero for almost all t ∈ [t1, t2]. A bang-bang component of thecontrol only takes values in αmin

νj, αmax

νj if σi = ϕνj , and in

αminΩj

, αmaxΩj if σi = ϕΩj

for almost every t ∈ [t1, t2], i = 1, · · · , 6.

Definition

If there is a nontrivial interval [t1, t2] such that a switchingfunction is identically zero, the corresponding component of thecontrol is said to be singular on [t1, t2]. A singular componentcontrol is said to be strict if the other controls are bang.

Singular Arcs

An analysis of the singular arcs can be found in:M. Chyba, T. Haberkorn, R.N. Smith, G.R. Wilkens. A Geometric

Analysis of Trajectory Design for Underwater Vehicles. Discrete and

Continuous Dynamical Systems-B, Volume: 11, Number: 2, 2009.

It is also proved that the behavior of the solutions inside somemanifold is similar to that of the chattering arcs in the Fullerproblem. This is a consequence of the is a close relationshipbetween the existence of chattering arcs and singular extremals oforder two.

Numerical Simulations

η0 is set to be the origin, and ηT = (6, 4, 1, 0, 0, 0), with bothconfigurations being at rest.

0 10 20−10

0

10

γ ν1

0 10 20−10

0

10

γ ν2

0 10 20−20

0

20

40

γ ν3

t (s)

0 10 20−5

0

5

γ Ω1

0 10 20−5

0

5

γ Ω2

0 10 20−1

0

1

γ Ω3

t (s)

Comments on Time Optimal Trajectory

The time for this trajectory is ≈ 25.85s. The structure is mostlybang-bang, except for the τΩ3 control which contains singular arcs.These singular arcs depend on our choice of initial and finalconfigurations. In this case, orientation is the key to optimality;first orient, then move. We orient the vehicle such that we can usethe maximum available translational thrust to realize the motion,and the vehicle needs to maintain this orientation over the entiretrajectory. Singular arcs do not appear in τΩ1 and τΩ2 becausetheir full power is needed to offset the righting moments. Thetranslational controls ϕν1,ν2,ν3 are used to their full extent, as onewould expect for a time optimal translational displacement.

Concatenation of Pure Motions: Pure surge, pure swaythen pure heave ending at ηT = (6, 4, 1, 0, 0, 0)

0 20 40 60−10

0

10

γν

1

0 20 40 60−10

0

10

γν

2

0 20 40 60−20

0

20

40

γν

3

t (s)

0 20 40 600

5

10

b1

0 20 40 600

2

4

b2

0 20 40 600

0.5

1

b3

t (s)

Comments on Pure Motions Trajectory

Note that this trajectory is formed by a pure surge accelerationduring taccsurge ≈ 38.39 s, a deceleration for tdecsurge ≈ 3.74 s, a puresway acceleration for taccsway ≈ 25.89 s, a deceleration for

tdecsway ≈ 3.74 s, a pure heave acceleration for taccheave ≈ 2.92 s and a

deceleration for tdecheave ≈ 5.24 s. The non-symmetry of theacceleration and deceleration phases is due to drag forces andthruster asymmetry’s. The total transfer time for this trajectory istpure ≈ 79.92 s. The duration is more than triple the optimal time!This is actually not that surprising since the pure motion trajectoryuses only a fraction of the available thrust. A pure motion controlstrategy is attractive for our problem due to its piecewise constantstructure but it is far from time efficient.

Switching time parameterization algorithm: STTP

Goal : produce a time efficient which can be easily implementedonto a test-bed vehicle.By time efficient, we mean that thetrajectory duration is close in time to that of the time optimalsolution.Our idea is to impose the structure of the control strategy but notbasing it on the solution of the non linear problem. We fix thenumber of switching times along the trajectory, preferably to asmall number, and we numerically determine the optimal trajectoryfrom these candidates. We call this new optimization problemSwitching Time Parameterization Problem STTPp where p refersto the number of switching times. The unknowns are the timeperiods between two switching times along with the time periodbetween the last switch and the final time, and the values of theconstant thrust arcs. It is essential for convergence that the latervalues are introduced as parameters. Our construction does notnecessarily produce bang-bang trajectories.

The new optimization problem (STPP)p has the following form:

(STPP)p

minz∈D

tp+1

t0 = 0ti+1 = ti + ξi , i = 1, · · · , pχi+1 = χi +

∫ ti+1

tiχ(t, γi )dt

χp+1 = χT

z = (ξ1, · · · , ξp+1, γ1, · · · , γp+1)

D = R(p+1)+ × Up+1

where ξi , i = 1, · · · , p + 1 are the time arc-lengths andγi ∈ U , i = 1, · · · , p + 1 are the values of the constant thrust arcs.

Comparison of (NLP) and (STPP)

We analyze t fSTPPpfor different p and alternate final configurations.

The initial configuration is always η0 = 0 and ν0 = Ω0 = 0.

Final (NLP) (STPP)pConfiguration t fNLP # sw. Sing? t fSTPP2

t fSTPP3t fSTPP4

(6, 4, 1, 0, 0, 0) 26.58 s 21 Yes 28.72 s 28.10 s 28.02 s(6, 4, 0, 0, 0, 0) 28.42 s 28 Yes 34.43 s 29.83 s 29.01 s(6, 0, 0, 0, 0, 0) 25.40 s 23 Yes 31.52 s 28.45 s 26.55 s

(0, 6, 0, 0.2, 0.3, 0) 25.46 s 19 Yes 30.09 s 29.05 s 28.98 s

Energy minimization

Our ultimate goal is to minimize a combination of time andconsumption. While the time is a notion that is uniquely defined,the consumption criterion is largely dependent on the consideredmechanical system.Our test-bed AUV is powered solely by on board batteries, henceits autonomous abilities are directly related to the life-span of thispower supply. By virtue of design, a time efficient trajectory willrequire a high level of consumption. ODIN is controlled by eightexternal thrusters. These thrusters draw power from a bank of 20batteries. All other on-board electronics such as the computer andsensors run on a separate bank of four batteries which supplyenough power for ODIN to operate nearly indefinitely whencompared to the life-span of the thruster batteries. Thus,minimizing energy consumption for a given trajectory directlycorresponds to minimizing the amount of current pulled by thethrusters.

DOF controls versus real controls

γi , i = 1, 3, 5, 7 as the thrusts induced by the horizontal thrusters,γi , i = 2, 4, 6, 8 as the thrusts induced by the vertical thrusters.The horizontal thrusters contribute only to the forces ϕν1 (surge)and ϕν2 (sway) and to the torque τΩ3 (yaw). The vertical thrusterscontribute only to the force ϕν3 (heave) and to the torques τΩ1

(roll) and τΩ2 (pitch).

TCM =

−e1 0 e1 0 e1 0 −e1 0e1 0 e1 0 −e1 0 −e1 00 −1 0 −1 0 −1 0 −10 −e3 0 −e3 0 e3 0 e3

0 e3 0 −e3 0 −e3 0 e3

e2 0 −e2 0 e2 0 −e2 0

for e1 = 0.7071, e2 = 0.4816 and e3 = −0.2699, and

γ ∈ Γ = γ ∈ R8|γmin ≤ γi ≤ γmax, i = 1, · · · , 8.

Thruster calibration experiment

We supplied a known voltage function to each thruster whichcovered the operational range of voltage inputs used duringexperiments. As the voltage changed, we continuously recordedthe thrust output using a strain gauge, as well as the amps pulledby the thruster. Each thruster was tested four times through bothpositive and negative voltage ranges. The experimental data wasthen averaged over all tests and all thrusters to give the followingsimplified relation:

Amps(γi ) =

−0.4433γi = α−γi , if γi ≤ 00.2561γi = α+γi , if γi ≥ 0

,

where Amps(γi ) (A) is the current pulled when the thrust γi (N) isapplied by the thruster. Since we know the relationship betweeninput voltage and output thrust for each actuator from previouscalibration experiments, we can estimate the consumption basedon input voltage applied to the vehicle.

Energy Criterion

The lifespan of a battery is measured in A.h. Thus, we can writethe consumption criterion of the eight thrusters as:

C (γ) =

∫ T

0

8∑i=1

Amps(γi (t))dt,

where the final time T can either be fixed a priori or remain as afree parameter in the optimization problem. A fixed final time maybe chosen based upon specific mission constraints or on aconsumption minimizing trajectory duration. If we choose to leavethe final time as a free parameter, it is guaranteed to be finite sinethe vehicle is positively buoyant.

Energy Criterion

Since we will know the minimum time Tmin to connect twoterminal configurations, we have a lower bound on the trajectoryduration. This duration will clearly not be energy efficient. Webegin by considering a fixed duration T , which is a multiple ofTmin:

T = cT · Tmin, cT ≥ 1. (5)

Note that the way we consider the problem, for cT = 1 thesolution of the minimum time and minimum consumption problemsare the same.

Maximum Principle

For the consumption minimization problem we havel(χ, γ) =

∑8i=1 Amps(γi ) and T = cTTmin, cT ≥ 1. The

Hamiltonian for the energy minimization is:

H(χ, λ, γ) = −λ0l(χ, γ) + λtηJ(η)ν

+ λtνM−1(Mν × Ω + Dν(ν)ν + Rt(ρgV −mg)k + ϕν)

+ λtΩJ−1(JΩ× Ω + Mν × ν + DΩ(Ω)Ω

− rB × Rt(ρgV −mg)k + τΩ)

where λ0 is a constant that can be normalized to 0 or 1. Notice,for simplicity we express these equations with the 6-DOF controlbut for our minimization problems we will work with the8-dimensional controls.

Necessary Conditions

H(χ(t), λ(t), γ(t)) = supu∈Γ

H(χ(t), λ(t), u), a.e. on [0,T ].

In the case where the final time T is a free parameter, we mustadd the condition H(χ(t), λ(t), γ(t)) = 0. We define the functionsκi , i = 1, · · · , 8, as the multiplying coefficient of γi inH + λ0l(χ, γ). The function κi is then the i th component of thevector (λν , λΩ)tTCM(M−1, J−1) where TCM is the ThrustConversion Matrix: (ϕ, τ) = TCM · γ. For instance:

κ1 = −e1λν1

m1+

e1λν2

m2+

e2λΩ3

Ib3 + JΩ3f

.

Let us consider the maximum condition when λ0 = 1. The controlsγi , for i = 1, · · · , 8, are given by:

γi =

γmin , if κi < α−∈ [γmin, 0] , if κi = α−0 , if κi ∈ (α−, α+)∈ [0, γmax] , if κi = α+

γmax , if κi > α+

Here, if the function κi is not equal to α− or α+ on a nontrivialtime interval, the corresponding γi will be piecewise constant andassumes the value γmin, 0 or γmax. If the switching function κi isidentically equal to α− (or to α+) on a nontrivial time interval[t1, t2] we say that γi is singular on [t1, t2].

Results, Direct Method

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−10

0

10

20

γ1

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−10

0

10

20

γ2

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−10

0

10

20

γ3

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−15

−10

−5

0

γ4

t

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−10

0

10

20

γ5

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−15

−10

−5

0

γ6

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−10

0

10

20

γ7

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

−15

−10

−5

0

γ8

t

Figure: Minimum consumption control strategies for cT = 1.5 (plain), 2(dashed),2.75 (dotted) and for χT = (5, 4, 1, 0, · · · , 0).

Minimum Time

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 1

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 2

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 3

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 4

t (s)

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 5

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 6

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 7

0 2 4 6 8 10 12 14 16−20

−10

0

10

20

γ 8

t (s)

Figure: Minimum time control strategy for χT = (5, 4, 1, 0, · · · , 0).

State Trajectory, Energy Consumption

0 0.2 0.4 0.6 0.8 10

2

4

6b

1 (

m)

0 0.2 0.4 0.6 0.8 10

2

4

6

b2 (

m)

0 0.2 0.4 0.6 0.8 1−1

0

1

2

b3 (

m)

t

0 0.2 0.4 0.6 0.8 1−20

−10

0

10

Ω1 (

deg)

0 0.2 0.4 0.6 0.8 1−20

0

20

40

Ω2 (

deg)

0 0.2 0.4 0.6 0.8 1−20

−10

0

10

20

Ω3 (

deg)

t

The major difference between the three trajectories lies in the rolland pitch evolution Ω1,2. Indeed, we see that for a final time closeto the minimum one (cT = 1.5) the overall inclination of the rigidbody is much greater than when the trajectory duration islengthened. However, there is not much differences between thetrajectories where cT = 2 and cT = 2.75. This can be explained bythe fact that there is a much larger gain in consumption efficiencybetween ct = 1.5 and cT = 2.0 than between ct = 2.0 andcT = 2.5.

Consumption Evolution

Examine the relationship between cT and consumption asmentioned before. We vary the duration of the final time (for thesame initial and final configurations).

1 1.5 2 2.5 3 3.5 4150

200

250

300

350

400

450

500

550

600

650

cT

CT (

A.s

)

Figure: Minimum consumption vs. cT , for χT = (5, 4, 1, 0, · · · , 0).

We display the minimum consumption for 4 distinct finalconfigurations χT and 5 values of cT near 2. The initialconfiguration is always taken to be the origin.

χT Tmin(s) C1 C1.50 C2.0 C2.5 Copt

(5, 4, 1, 0, · · · , 0) 17.43 597.86 216.10 161.07 151.58 150.88(3, 3, 3, 0, · · · , 0) 13.66 488.37 189.06 147.14 138.39 137.54(5, 7, 0, · · · , 0) 22.22 753.53 273.53 196.61 184.12 182.92

(10, 8, 2, 0, · · · , 0) 31.48 1105.58 422.22 314.84 294.49 291.75

Table: Consumption for various χT and cT .

STTP for the Energy Consumption

(STPP)p

minz∈D

∑pi=1

∑8j=1(α−γ

−j ,i + α+γ

+j ,i )

t0 = 0, tp+1 = Tti+1 = ti + ξi , i = 1, · · · , pχi+1 = χi +

∫ ti+1

tiχ(t, γ−i + γ+

i )dt

χp+1 = χT

z = (ξ1, · · · , ξp+1, γ−1 , γ

+1 , · · · , γ

−p+1, γ

+p+1)

D = R(p+1)+ × (Γ− × Γ+)(p+1)

where ξi , i = 1, · · · , p + 1 are the time arclengths andγi ∈ U , i = 1, · · · , p + 1 are the values of the constant thrust arcs.

Results

(STPP)2 (STPP)3

χT C1.5 C2 C2.5 C1.5 C2 C2.5

1 228.43 163.89 150.64 228.72 164.91 157.512 221.53 155.56 148.45 205.79 155.09 146.173 292.92 203.51 189.72 284.82 202.09 200.104 437.00 323.09 436.44 322.28

Table: (STPP)2 and (STPP)3 consumptions for various χT and cT .

We see that the consumption calculated by the (STPP)2 and(STPP)3 approaches are of the same magnitude as seen previously.These values are exceptionally close to the optimal ones.

Computed thrust strategy corresponding to a (STPP)2

solution with cT = 2 and χT = (5, 4, 1, 0, · · · , 0).

0 10 20 300

0.2

0.4

γ1

0 10 20 30−4

−2

0

γ2

0 10 20 30−10

0

10

γ3

0 10 20 30−1

−0.5

0

γ4

0 10 20 300

2

4x 10

−6

γ5

0 10 20 30−4

−2

0

γ6

0 10 20 30−10

0

10

γ7

0 10 20 30−4

−2

0

γ8

Contrary to the optimal thrust strategy, the one presented above isimplementable onto a test-bed vehicle. There are only two times atwhich the actuators can switch during the trajectory, and moreovermany of the controls are zero for an extended period of time. Weuse the following section to present experimental results of theimplementation of the computed (STPP)2 strategy onto thetest-bed vehicle, ODIN.