Saroj Saimek∗∗Perry Y. Li∗∗∗Department of Mechanical Engineering,University of Minnesota,111 Church St. SEMinneapolis MN 55455, [email protected]@me.umn.edu
Motion Planning andControl of a SwimmingMachine∗
Abstract
We propose a practical maneuvering control strategy for an aquaticvehicle (AV) that uses an oscillating foil as a propulsor. The challengeof this problem lies in the need to consider the hydrodynamic interac-tion as well as the underactuated and non-minimum phase natures ofthe AV system. The control task is decomposed into the off-line step ofmotion planning and the on-line step of feedback tracking. Optimalcontrol techniques are used to compute a repertoire of time-scalableand concatenablemotion primitives. The complete motion plan isobtained by concatenating time-scaled copies of the primitives. Thecomputed optimal motion plans are regulated by a controller thatconsists of a cascade of linear quadratic regulator, input–outputfeedback linearization and sliding mode control. Time-varying lin-ear quadratic controllers can also be time-scaled and concatenated.Therefore, they can be computed beforehand. The proposed strategyhas been experimentally validated for both constrained longitudinalonly maneuvers and unconstrained longitudinal/lateral maneuvers.
KEY WORDS—aquatic vehicles, oscillating foil, swimmingmachine, optimal control, time-scaling, hydrodynamics, mo-tion planning, motion primitives, linear quadratic control
1. Introduction
Study of fish locomotion can be traced back as early as the 4thcentury BC by Aristotle (Hoar and Randall 1978). Nearly 70years ago, Sir James Gray put forward the so-called Gray’sparadox which stimulated much research and controversy(Gray 1936; Hoar and Randall 1978; Triantafyllou and Tri-antafyllou 1995). The paradox suggests that if the resistance
∗A portion of this paper was presented at the 2001 American ControlConference.∗∗Current address: Department of Mechanical Engineering, King Mongkut’sUniversity Of Technology, Thonburi, Bangkok, Thailand∗∗∗Author to whom all correspondence should be addressed.
of an actively swimming dolphin is equal to that of a simi-larly shaped rigid model towed at the same speed, the dol-phin’s muscles must be capable of generating at least seventimes more power than is typical of mammalian muscle (Gray1936).
In recent years, Triantafyllou and Triantafyllou (1995),Barrett, Grosenbaugh, and Triantafyllou (1996) and Barrettet al. (1999) have proposed an explanation of the Gray para-dox. They propose that fish are able to utilize the energy thatexists in the eddies of an oncoming flow by repositioning thevorticities. In addition, the oscillating swimming motion ofthe fish induces flow relaminization which serves to reducethe body drag (Techet and Triantafyllou 1999). Using an artic-ulated robotic fish, they were able to demonstrate experimen-tally that the power required to propel an actively swimmingfish-like body is indeed lower than the power required to towa rigid fish-shaped object (Barrett et al. 1999).
Inspired by the swimming motion of an aquatic animal,oscillating foil has therefore been proposed as an alternativepropulsor to the conventional screw propeller for aquatic vehi-cles (AVs; Triantafyllou and Triantafyllou 1995; Liu and Bose1999; Czarnowski, Cleary, and Kreamer 1997). These mimicCarangiform swimming in which only the last third of thebody participates in the undulatory motion. In addition to be-ing potentially more efficient (Barrett, Grosenbaugh, and Tri-antafyllou 1996), an oscillating foil may also be more stealthy.Also, as thrust can be generated in all directions, vehiclespropelled this way may also be more maneuverable. Harper,Berkemeir, and Grace (1998) proposed the design of an opti-mal spring constant to actuate the oscillating foil. Kelly et al.(1998) and Kelly and Murray (2000) derived the equationsof motion of a submerged foil in standard control-affine formvia the method of a reduced Lagrangian in a geometric frame-work. The experimental setup was later modified by Masonand Burdick (1999, 2000). Lateral and rotational degrees offreedom and a rectangular plate to simulate a submerged bodyare added. They also experimentally verified a mathematicalmodel based on quasi-steady uniform flow. Morgansen et al.
28 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
Fig. 1. The University of Minnesota Aquatic Vehicle Setup.
(2001) proposed geometric methods to compute the gait forforward and turning motions, and recently used averagingtheory to design stabilizing control laws (Vela, Morgansen,and Burdick 2002; Morgansen, Vela, and Burdick 2002). Ya-maguchi and Bose (1994) compared the propulsion perfor-mances of a tanker propelled by an oscillating foil and by aconventional screw propeller. They found that the flexible os-cillating foil has a higher quasi-propulsive efficiency than aconventional screw propeller, but a conventional screw pro-peller has a higher quasi-propulsive efficiency than a rigidoscillating foil. Kato (2000) considered the control of a pec-toral fin-like mechanism as a propulsor to achieve rendezvousand docking with an underwater post in water currents. BesideCarangiform swimming robots, there is also research that fo-cuses on Anguilliform swimming (in which the whole bodyundulates) such as eel-like robots (McIsaac and Ostrowski2002).
In this paper we consider the maneuvering control of an AVpropelled by an oscillating foil (Figure 1). The challenges liein the need to consider the hydrodynamic interaction with thestructure, and the under-actuated, and non-minimum phasenatures of the AV system. In our approach, a two-dimensional(2D) potential flow model is assumed (Streitlien 1994). Freevortices and central vortices are initially ignored and treated asdisturbances. Because of the non-holonomy associated withthe under-actuation, the linearized system at any equilibriumpoint is not controllable. Also, conventional nonlinear con-trol approaches, such as input–output feedback linearization,cannot be applied directly, because of the unstable internaldynamics. Emerging geometric control techniques for under-actuated and non-holonomic systems are typically mathemat-ically involved and use various forms of bracketing of vectorfields; see Morgansen et al. (2001) and Morgansen, Vela, andBurdick (2002) for a flavor. Instead, our strategy is to decom-pose the control task into two stages: motion planning and
tracking control. By explicitly planning the complete motionof the AV, feasible and well-behaved state trajectories are en-sured. The design of state trajectory tracking controllers canthen be carried out relatively easily, since the linearized sys-tem about the time-varying trajectory will generally be con-trollable. Our approach is driven by the desire for strategiesthat are practical, theoretically sound and at the same timeeasy for a practitioner to understand.
The motion planning problem is solved off-line via the op-timal control method. While it is theoretically possible to useoptimal control to determine the motion plan for every singlemaneuver, this is impractical because solving the optimal con-trol problem is computationally expensive and cannot be doneon-line. To reduce the complexity of the problem, maneuverswill be constructed by concatenating time-scaled copies of afiniteset of basic maneuvers, called motion primitives. By al-lowing the motion primitives to be time-scaled, time-shifted,and concatenated, it will be possible to accomplish maneu-vers at different speeds and at different ranges. Thus, the needto calculate the motion primitives for each basic maneuver atevery speed is avoided.
To track the planned motion, a cascade control which con-sists of a finite-time linear quadratic regulator (LQR), aninput–output linearization of the foil dynamics, and a slidingmode control is proposed. LQR determines the required foilheaving and pitching acceleration signals to achieve tracking,while I/O feedback linearization linearizes the foil dynamicsand determines the actual motor inputs to achieve the requiredfoil accelerations. Sliding mode control enhances the robust-ness to model uncertainty and disturbances. It will be shownthat the LQR controllers computed for motion primitives canalso be time-scaled for use with the time-scaled motions. Theabove motion planning and control strategy has been imple-mented and tested on an experimental AV system (Figure 1).
The LQR technique is used as the baseline controller be-cause of its ease of computation and its ability to handle timevariation that results from Jacobian linearization about a time-varying trajectory. Other linear control techniques usuallycannot handle possibly large time variation easily. Genericnonlinear control schemes for state tracking such as input-state linearization would require finding solutions for theFrobenius partial differential equations, which is a relativelydifficult task and yields an uncontrollable system at an equi-librium point.
The rest of the paper is organized as follows. In Section 2we describe the experimental AV system. In Section 3 wepresent the dynamic model of the AV and the hydrodynamicinteraction. In Sections 4 and 5, we give motion planning ex-amples for both laterally constrained, longitudinal only ma-neuvers, and unconstrained lateral/longitudinal motions. InSection 6 we present the tracking control strategy to ensurethat the planned motion is tracked. In Section 7 we presentsome experimental results, and Section 8 contains some con-cluding remarks.
Saimek and Li / Swimming Machine 29
α
Rotating hub
(unactuated)
R
7cm
85cm
Peduncle(Heaving link)
15cm
Floating mechanism
Joukowski foil
10cm
6cm
Pitchingmotor
Heavingmotor (center)
Pitchingpulley/belts
φpitch angle
Heave angle φ
Platform
Fig. 2. Top view of the swimming machine (AV) at theUniversity of Minnesota. Only the 30 cm spanned Joukowskifoil is submerged in water. The system has four degrees offreedom with only the heaving and pitching motions of theJoukowski foil being directly actuated. Thefloating mecha-nism enables the pitching pulley to move independently ofthe heaving motor.
2. Experimental Setup
Figures 1 and 2 are a picture and a schematic diagram of ourexperimental AV setup. This consists of a two-link actuatedmechanism mounted on a platform. The second link is thepropulsor which is a submerged symmetric Joukowski foil(Milne-Thomson 1968) with a cord length of 0.06 m, a max-imum thickness of 0.008 m, and a vertical span of 0.3 m. Theheaving (φ) motion of the Joukowski foil is directly actuatedby a MicroMo 3863 series DC motor (max torque 177oz-in)with a 43:1 gearhead. The pitching (θ ) motion is actuated by aMicroMo 3557 series DC motor (max torque 74.2oz-in) witha 25:1 gearhead via two 1:1 belt/pulley sets. The mechanismand pulley sets are designed around afloating mechanism suchthat the pitching and heaving motions can be independentlydriven by their respective DC motors so that the rotation ofeach motor corresponds exactly to the heaving and pitchingangles. The heaving and pitching angles are monitored by op-tical encoders with resolutions of 512× 43/(2π) lines rad−1,and 512× 25/(2π) lines rad−1, respectively.
The platform is above water and can slide along a pair ofrails on a set of linear bearings, simulating the lateral motion(R). The rails are connected to a rotating hub at the center ofa 1.75 m diameter water tank. The rotation of the platformsimulates the longitudinal motion (α) of the vehicle. The lon-gitudinal and radial motions are monitored by rotary and lin-ear optical encoders with resolutions of 6144/(2π) lines rad−1
and 1024 lines in−1, respectively. Notice that lateral and longi-tudinal motions of the platform are not directly actuated. Thelateral motion can be locked in place, if desired, to performlaterally constrained, longitudinal only motions.
The heaving and pitching motors are under torque (cur-rent) control mode so that, as far as the control design is con-cerned, torque is the control input. Controller implementa-tion is carried out using Matlab Realtime Toobox (Humusoft,CZ). This allows us to conveniently code our controller asMatlab M-files at the expense of a relatively long samplingtime (8 ms) which translates ultimately to limitation in controlperformance.
Our experimental setup is inspired by the Caltech design(Kelly et al. 1998; Kelly and Murray 2000) except that it ismuch smaller and uses rotational motion to simulate longi-tudinal motion. The smaller scale accentuates unmodeled ef-fects such as uneven gravitational effects, friction in the longi-tudinal and lateral motions, waves andfiniteness of the watertank. Our control system must therefore be robust enough tomitigate these effects. Moreover, because the system rotateson a hub, the dynamics are not symmetric with respect to lat-eral translation. This artifact of our experimental setup alsocomplicates the design of motion plans when motions involv-ing different lateral positions are required. We remark that inan actual AV, such an artifact will not exist.
3. System Model
We assume that thefluid flow is 2D, inviscid, incompressible,and irrotational and also that thefluid is at rest at infinity. Un-der these assumptions, theflow field and the hydrodynamicforces and torque that act on the foil are determined using po-tential flow theory (Milne-Thomson 1968; Streitlien 1994).The presence and effects of free and central vortices will ul-timately be ignored for the purpose of motion planning andcontroller design, and are treated as disturbances. This sim-plifies the motion planning and control significantly. The esti-mation and the purposeful incorporation of the vortices in thecontrol design is an ongoing research (Li and Saimek 1999).
Consider an elliptical Joukowski foil whose profile is de-scribed by a set of points in the complex planeS ∈ C = {z =x + iy} with x ∈ in the direction of the cord length, andy ∈ in the direction normal to it.S is the image of theunit circleC = {ζ = eiθ , θ ∈ [0,2π)} under the JoukowskimappingF : ζ → z given by
z = F(ζ ) := ζ + ζc + a2
ζ(1)
wherea andζc determine the shape of the foil. The complexpotential for theflow around the Joukowski foil is given by
ω(z) = U(t)ω1(ζ )+ V (t)ω2(ζ )+�(t)ω3(ζ )
+γc(t)ω4(ζ )+∑k
γkω5(ζ ; ζk(t)). (2)
Here,ζ = F−1(z), U(t) andV (t) are, respectively, the abso-lute velocities in the directions parallel and perpendicular to
30 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
the foil’s cord, and�(t) is the angular velocity of the foil.γcis the strength of the central vortex, andγk andF(ζk(t)) withk = 1, . . . , nk, are the strengths and locations of thenk freevortices. The functionsωi(ζ ), i = 1, · · · ,4 andω5(ζ, ζk(t))
are given in Appendix A.1. Once the complex potential hasbeen defined, the velocity(u, v)of thefluid particle at locationz is given by (Streitlien 1994; Li and Saimek 1999)
dω
dz= dω
dζ
dF
dζ
−1
= u− iv, (3)
which, using eq. (2) can be written as
dω
dz= Q1(ζ, ζ1(t), . . . , ζnk (t))
U
V
�
γcγ1
...
γnk
.
for someQ1(ζ, ζ1(t), . . . , ζnk (t)) ∈ 1×(nk+4).Let q = [α,R, φ, θ] ∈ 4 be the generalized coordinate
vector of the AV consisting of the longitudinal, radial, heavingand pitching coordinates (Figure 2). LetM(q) ∈ 4×4 be thepositive definite inertia matrix of the AV in these coordinates.
The sum of the kinetic energies of the AV and of the sur-rounding water is then given by
T = L
2ρ
∫Sv
dω
dz
(dω
dz
)dA+ 1
2qTM(q)q (4)
whereρ is the density of the water, andL is the out-of-planedimension (the height) of the foil. The integral in thefirstterm is taken over the complete water area outside the foiland outside any free vortices on the 2D plane.
Notice that the foil’s velocities,U , V and�, can be ex-pressed linearly with respect to the velocities of the general-ized coordinatesq. Therefore,in the case when central andfree vortices are ignored(i.e.,γk ≡ 0 andγc = 0), the kineticenergy (4) can be written as
T = 1
2qTMm(q)q. (5)
The exact expression and the derivation forMm(q), which isthe sum of the inertia of the AV and the so-called added massdue to thefluid, are presented in Appendix A.1.
Using the total kinetic energyT in eq. (5) as the Lagrangianand applying the Euler–Lagrange principle, we obtain theequations of motion of the system in the form
Mm(q)q+ Cm(q, q)q = 02×1
τhτp
=
(02×1
τττ
), (6)
where Cm(q, q)q ∈ 4 is the Coriolis force of the AVcombined with thefluid induced virtual damping, andτττ =(τh, τp)
T are the heaving and pitching motor torques. Just as foran ordinary robotic mechanism, the Coriolis matrixCm(q, q)is linear inq. Moreover, the matrix
Mm(q)− 2Cm(q, q)
is skew symmetric. Henceforth, we often use the notation
ψψψT :=(φ
θ
), τττ =
(τhτp
)
to denote the foil displacements and foil input torques.If the free and central vortices,γi , i = 1, . . . , nk, and
γc(t) are not ignored, the right-hand side of eq. (6) must beaugmented by additional hydrodynamic forces. They gener-ally will act on all four degrees of freedom. As mentioned,in this paper, these vortex generated forces are treated as dis-turbances and we aim to design our closed-loop controller tosuppress their effects on the AV.
The hydrodynamic forces acting on the foil are consideredto be internal forces when the Euler–Lagrange method in Ap-pendix A.1 is used. An alternative method of deriving thesystem model that considers the hydrodynamic force explic-itly is given in Appendix A.2. In this case, the hydrodynamicforce and moment formulae in Streitlien (1994) and Milne-Thomson (1968) can be used. This method will be especiallyuseful when the effects of vortices are to be included.
In this paper, we develop a motion control strategy that isapplicable to the unconstrained AV as well as to the AV lat-erally constrained atR = 0.43 m. When the lateral directionis fixed, i.e.,R = R ≡ 0, eq. (6) can be reduced to a three-degree-of-freedom system by eliminating its second row, set-tingR = 0.46 m,R = R ≡ 0, and usingq = (
α φ θ)T
asits generalized coordinates. Thus, we obtain the constrainedsystem
Mm(q) ¨q+ Cm(q, ˙q) ˙q =(
0τττ
)(7)
whereMm(q) ∈ 3×3,Cm(q, ˙q) ∈ 3×3.Clearly, both the original system (6) and the constrained
system (7) are under-actuated. It can be shown, using numer-ical simulation, that both systems are in factnon-minimumphasewith the AV’s body motionsα andR as outputs, andeither the heaving and pitching accelerationsψ = (φ, θ )T, orthe heaving and pitching torquesτττ = (τh, τp)T as the inputs.
The dynamics of the AV has properties useful for con-structing time-scaled maneuvers. These properties are validfor both the unconstrained system (6) and the laterally con-strained system (7).
PROPOSITION1. (Symmetry) The equation of motion (6) isinvariant with respect to the longitudinal displacementα.
Proof. This is a direct consequence of the kinetic energy func-tion (5) not being dependent onα. �
Saimek and Li / Swimming Machine 31
Notice that if the AV does not have to rotate on a hub, theequation of motion will also be symmetric to lateral displace-mentsR.
PROPOSITION 2. (Energy conservation)In the case whencentral and free vortices are ignored, the total kinetic energyT of the system in eq. (5) satisfies:
d
dtT (t) = (
φ θ) (τhτp
)= ψψψT
τττ . (8)
Notice that the right-hand side of eq. (8) is exactly the me-chanical power exerted by the motors. In other words, thesystem is passive with respect to the supply rate being themotor power input, i.e., there existsc2 > 0, such that for allt ≥ 0, and for any motor inputτh(·), τp(·),
t∫0
(φτh + θ τp)dτ ≥ −c2.
Furthermore if,τττ = (τh, τp)T = (0, 0)T, the kinetic energyof the system is conserved.
Proof. This result is a direct consequence that eq. (6) is aEuler–Lagrange system derived using its kinetic energy asthe Lagrangian. �
The energy conservation property enables us to use energyto categorize maneuvers. Proposition 2 is valid under our as-sumption of ideal potentialflow, which ignores any dissipa-tive effects. This result does not strictly hold in the presenceof drag, vortex shedding and surface wave effects, etc. Weconsider these as disturbances in this paper.
T, t ∈ and the initial conditionsq(0) andq(0)result in the motionq(t). Then the input heaving and pitch-ing torque trajectoriesλ2τττ(λt) and initial conditionsq(0) andλq(0) result in the time-scaled motionq(λt).
The same scaling results apply to the laterally constrainedsystem eq. (7).
Proof. Notice that time-scaled motionq(λt) induces time-scalings for the velocity and acceleration of the form:
q(t)← q(λt)
q(t)← λq(λt)
q(t)← λ2q(λt).
Since the proposed scalings for the initial conditions satisfythe above, to show that the proposed scalings of the torquetrajectoriesτττ(t)← λ2τττ(λt) generates the desired time-scaledmotion, we need only show that
Mm(q(λt))[λ2q
]+ Cm(q(λt), λq(λt))[λq(λt)
] = λ2τττ(λt)
which is indeed the case becauseCm(q, q) is linear inq. Thecondition is equivalent to
λ2[Mm(q(λt))q+ Cm(q(λt), q(λt))q(λt)
] = λ2τττ(λt)
which is clearly satisfied since the torque inputsτττ(t)generatesthe motionq(t). The same argument applies to the constrainedsystem eq. (7). �
Notice that Proposition 3 is only valid for special mechan-ical systems. It is not true for generic dynamical systems. Thetime-scaling property allows for a motion plan determinedfor one time-scale to be easily modified for use in anothertime-scale.
4. Motion Planning
Given the system’s models (6) it is tempting to design aninput–output linearizing controller withy = (α, R)T as thedesired output and the motor torques as input, to control ar-bitrary longitudinal motions. Unfortunately, this will lead toinstability of the internal dynamics (associated with the mo-tions of the foil,ψψψ) since, as mentioned before, such a systemcan be numerically shown to be non-minimum phase. Thesame is true for the constrained system eq. (7) withy = α.To overcome this difficulty, the maneuver control problem isdecomposed into
• the off-line motion planning for the maneuver, and
• the on-line tracking control of the system to follow theplanned motion.
Motion planning ensures that the internal states are all wellbehaved. Because the system is under-actuated, not every mo-tion trajectory,q(t) ∈ 4, is feasible.
To resolve this difficulty, an optimal control approach tomotion planning is adopted (Bryson 1999). Other motionplanning approaches for under-actuated systems include us-ing bang-bang control (Godhavn et al. 1999), geometric con-trol techniques and special coordinate transformations (Rey-hanoglu et al. 1999; Morgensen et al. 2001). The optimal con-trol approach to motion planning consists of the following:
1. specifying a set of constraints for a basic maneuver(these constraints include the satisfaction of basic re-quirement for the maneuvers, the equation of motion(7), as well as the physical constraints to the system);
2. specifying a performance index for each feasiblemotion;
3. computing the feasible motionq(t) that maximizesthe performance index while satisfying the specifiedconstraints.
32 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
One possible formulation of the optimal path planning is tocompute the optimal heaving and pitching torque trajectoriesτττ(t) directly. To compute the performance index, this methodrequires solving the four coupled second-order differentialequation (6) for the AV system (three coupled second-ordersystem for the constrained system (7)). Moreover, the torquetrajectories do not provide an intuitive understanding of theswimming motion. Because of these reasons, we formulatethe problem such that the optimal heaving and pitching dis-placement profilesψψψ(t) are the inputs to be computed instead.This alternative approach has two advantages:
• For the unconstrained system (6), only two coupledsecond-order differential equations (namely the differ-ential equations forα andR) need to be integrated toget (α(t), R(t)) in order to compute the performanceindex (one single second-order equation for the later-ally constrained system).
• The resulting heaving and pitching profiles have betterintuitive interpretations.
Once the heaving and pitching profiles are found, the pro-files for heaving and pitching torquesτττ(t) are computed fromeq. (7) after differentiatingψψψ(t) with respect to time twice.The optimal control problem isfirst converted into a parame-ter optimization problem byfinitely parameterizing, not nec-essarily linearly, the heaving and pitching trajectoriesψψψ(t)
similar to Yen and Nagurka (1990). Then, the“fmin” func-tion in Matlab’s (Mathworks) Optimization Toolbox is usedto compute the coefficients that optimizes the performanceindex defined for that maneuver.
4.1. Longitudinal Motion Primitives
In order to illustrate the motion planning approach, in this sec-tion we considerfirst the longitudinal-only motion, i.e., for thelaterally constrained system eq. (7). A very similar approachwill be applied to the unconstrained case in Section 5.
Since it is impractical to compute the optimal plan for ev-ery maneuver, optimal solutions for afinite set of basic mo-tion primitives are computed. Basic types of longitudinal mo-tion primitives arecruise, accelerate, decelerate,cruise-to-cruise. Each type of primitive has severalmembers parameterized by the operating speed or the en-ergy level. The primitives are designed so that they can beconcatenable according to the state diagram in Figure 9. Thisimposes equality constraints (boundary conditions) on the op-timal control problem. Additional inequality constraints, suchas heaving and pitching ranges of motion, are also imposed toensure that physical limitations of our setup are not exceeded.
4.1.1.cruise
Thecruise primitive is the most common basic longitudi-nal maneuver, intended for sustained travel. The objective forcruise is to travel far using little effort.
For cruise to be concatenable with itself, we imposethe boundary conditionsqcruise(0) = ˙qcruise(T cruise
f), and
ψψψcruise(0) = ψψψcruise(Tcruisef
). Eachcruise motion primitiveis characterized by its energy levelE in (9):
E = 1
2˙qT
cruise(T cruise
f)Mm(qcruise(T cruise
f)) ˙qcruise(T cruise
f), (9)
whereT cruisef
is thefinal time of the maneuver.The performance index to be maximized is
J = αcruise(T cruise
f)−
T cruisef∫0
(wh τ2h+ wp τ
2p)dt (10)
whereαcruise(T cruisef
) is thefinal longitudinal displacement atthefinal timeT cruise
f, andwp andwh are positive weights.
Heaving and pitching profiles are parameterized by aFourier series
φcruise(t) =n∑i=0
[Aφi sin
(2π it
T
)+ Bφi cos
(2π it
T
)]
θcruise(t) =n∑i=0
[Aθi sin
(2π it
T
)+ Bθi cos
(2π it
T
)]
whereAφi , Bφi , Aθi andBθi are constant coefficients to bedetermined andT = T cruise
f. By ensuring thatT cruise
fis an
integer multiple of the periodT , the foil motions are alsoconcatenable and T-periodic. The Fourier parameterizationis inspired by biological motions being also periodic duringsteady swimming. It is also consistent with the principle ofusing periodic inputs to achieve rectified motions for non-holonomic systems (Murray and Sastry 1993).
Since the system dynamics (7) are invariant to longitudinaltranslationα (Proposition 1), the kinetic energy of the system(5) at the beginning and the end of the maneuver are the same.By Proposition 2, nonetenergy input from the motor is neededto execute thecruise maneuver, assuming that the motorcan recuperate any braking energy (e.g., using the concept ofsprings in Harper, Berkemeir, and Grace 1998). The optimalcruise maneuver is therefore the one that uses the energymost effectively for longitudinal translation.
Figure 4 shows profiles of thecruise1 maneuver whichis computed at energy levelE = 0.05.
As expected, Figure 4 shows that thecruise1 maneuverprovides a higher average speed at the same specified energylevel than when there is no heaving and pitching motions.Notice also that there is a positive bias in the heaving. Thishas a tendency to point the foil outwards. Because the foil is15 cm behind the platform, and the platform is constrained inthis case, this creates a larger moment about the rotating hub.
Figure 5 shows the profiles of thecruise2 maneuverwhich is computed at energy levelE = 0.1. Notice that boththe longitudinal speed as well as the heaving amplitude are
Saimek and Li / Swimming Machine 33
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50
0.1
0.2
0.3
0.4
0.5
0.6
0.7
time(sec)
Dis
plac
emen
t(m
)
Longitudinal motion
Static motion
Periodic motion
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50.08
0.1
0.12
0.14
0.16
0.18
time(sec)
Vel
ocity
(m/s
)
Longitudinal velocity
Fig. 3. Longitudinal displacement and velocity profiles ofcruise at energy levelE = 0.05.
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−0.4
−0.2
0
0.2
0.4
0.6
time(sec)
Ang
le(r
ad)
Heaving and pitching displacement
Heaving Pitching
−0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 0.5 0.6−0.5
0
0.5
Heaving(rad)
Pitc
hing
(rad
)
Phase portrait of heaving and pitching angle
Fig. 4.cruise1 atE = 0.05: top, longitudinal displacementand velocity profiles (the trajectory label“static” refers tothe case when the foil is not moving); bottom, heaving andpitching profiles.
larger in this case, compared tocruise1 atE = 0.05. Noticealso that, in bothE = 0.1 andE = 0.05 cases, the pitchingmotion leads the heaving motion by about 90◦, leading toa roughly circular counterclockwise trajectory in the phase-diagram (Figure 5).
4.1.2.accelerate
Eachacceleratemotion primitive is intended for acceler-ating the AV from rest to acruise primitive. The boundary
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50
0.2
0.4
0.6
0.8
time(sec)
Dis
plac
emen
t(m
)
Longitudinal motion
Static motion
Periodic motion
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50.1
0.12
0.14
0.16
0.18
0.2
0.22
time(sec)
Vel
ocity
(m/s
)
Longitudinal velocity
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−0.5
0
0.5
time(sec)
Ang
le(r
ad)
Heaving and pitching displacement
Heaving Pitching
−0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 0.5−0.5
0
0.5
Heaving(rad)
Pitc
hing
(rad
)
Phase portrait of heaving and pitching angle
Fig. 5.cruise2 atE = 0.1: top, longitudinal displacementand velocity profiles; bottom, heaving and pitching profiles.
conditions are thereforeqacc(0) = 0, ˙qacc(T accf) = ˙qcruise(0),
ψψψacc(Taccf) = ψψψcruise(0), whereqcruise(·) andψψψcruise(·) are the
full configuration trajectory the AV and the foil-only mo-tion for the particularcruise primitive with which theaccelerate is intended to be concatenated.
The performance index similar to eq. (10) as incruiseis maximized
J = αacc(T acc
f)−
T accf∫
0
(wh τ2h+ wp τ
2p)dt
whereαacc(T accf) is thefinal longitudinal displacement at the
final timeT accf
, andwp andwh are positive weights.In this case, the heaving and pitching profiles are parame-
terized as“sinusoids” with time-varying amplitudes and fre-
34 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
whereA1φ(t), A2φ(t), B1θ (t) andB2θ (t) are polynomials intime, e.g.,A1φ(t) = a1 + a2t + · · · + antn. This parameter-ization is inspired by the observation that when tropicalfishaccelerate from rest, theirfins modulate both their amplitudesand frequencies.
As it turns out, aT accf= 4 s longaccelerate primitive,
parameterized using nine parameters, computed directly asabove, is not very effective in terms of the torque levels and thedistance traveled. Better performances are theoretically possi-ble if a richer parameterization is used. However, the optimiza-tion becomes very difficult. To improve on this, an improved,
but suboptimal primitive is obtained by concatenating two 2 ssegments of the form (11) (each using nine parameters). Thetwo segments are concatenated at an unspecified intermediateset of configurations. For practical reasons, the velocity of thefoil motion is not constrained to be continuous at the interme-diate point (see Section 4.3 for a justification). The compositeaccelerate primitive uses a lower torque level and trav-els farther (hence achieving a higher performance index) thanthe directly computed nine-parameteraccelerate primi-tive. Optimal profile computed using 18 parameters directlyshould theoretically be more optimal. However, the optimiza-tion does not converge.
Figure 6 shows the profiles of a 4 saccelerate1 (con-catenable tocruise1 atE = 0.05). Notice that, in contrastto thecruise, the heaving and pitching motions tend to beof opposite phase with each other, as indicated by the pre-dominant motions between II and IV quadrants in the phasediagram in Figure 6.
4.1.3.decelerate
The decelerate motion primitive is intended to bringthe AV to stop from acruise primitive. Boundary con-ditions for deceleration from cruise are therefore˙qdec(0) =˙qcruise(T cruise
f),ψψψdec(0) = ψψψcruise(T
cruisef
), and ˙qdec(T decf) = 0.
The performance index to be maximized is
J = −αdec(T dec
f)−
T decf∫t0
(whτ2h+ wpτ
2p)dt. (12)
The negative sign in thefirst term reflects the fact that wewould like the vehicle to decelerate without translating toomuch. An additional constraint
αdec(t) ≥ vmin,
where vmin is the minimum allowable speed for allt ∈[0, T dec
f], is also imposed so that the AV does not move back-
ward too much. Time-varying“sinusoids” as in eq. (11) areused to parameterize the heaving and pitching profiles.
Figure 7 shows the profiles of adecelerate1 (concaten-able fromcruise1 atE = 0.05). In contrast tocruise andaccelerate, the heaving and pitching motions are in-phasewith each other.
4.1.4.cruise-to-cruise
The cruise-to-cruise motion primitives are used toaccelerate or decelerate the AV between twocruise primi-tives at different energy levels. Therefore, the boundary condi-tions are˙qc−c(0) = ˙qcruise1(T cruise1
whereαc−c(Tc−cf ) is thefinal longitudinal displacement at the
final timeT c−cf , andwp andwh are positive weights. If the
intended initial speed is lower than thefinal speed, the“+”sign is used in eq. (13). If the intended initial speed is higherthan thefinal speed,“−” sign is used.
Time-varying“sinusoids” with frequency and amplitudebeing time polynomials, as in eq. (11), are used to parameter-ize the heaving and pitching profiles.
Figure 8 shows the profiles of the cruise− to−cruise1,2 primitive which transitions fromcruise1 (at en-ergy levelE = 0.05) tocruise2 (atE = 0.1).
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
0.05
0.1
0.15
0.2
0.25
0.3
0.35
time(sec)
Dis
plac
emen
t(m
)
Longitudinal motion planning using gradually acceleration
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20.08
0.1
0.12
0.14
0.16
0.18
0.2
time(sec)
Vel
ocity
(m/s
)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−1
−0.5
0
0.5
1
time(sec)
Ang
le(r
ad)
Heaving and pitching displacement
Heaving Pitching
−0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Heaving(rad)
Pitc
hing
(rad
)
Phase portrait of heaving and pitching angle
Fig. 8. cruise− to− cruise1,2: top, longitudinal dis-placement and velocity profiles; bottom, heaving and pitchingprofiles.
4.2. Time-Scaling and Concatenation
A complete AV maneuver can be obtained by concatenatingcompatible motion primitives according to the state transitiondiagram in Figure 9. The state trajectory for the completemaneuver is obtained from concatenating the state trajectoriesfor each motion primitive, with a proper translation of thelongitudinal motion. For example, for the maneuver
{accelerate,cruise,cruise · · · },the state trajectory is
α(t)
=
αacc(t) 0 ≤ t ≤ T acc
f
αacc(Taccf)+ αcruise(t − T acc
f) T acc
f< t ≤ T acc
f+ T cruise
f
...
36 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
Speed 1
Rest
Speed 2
accelerate_1
decelerate_2
accelerate_2
Cruise−to−cruise_2,1 Cruise−to−cruise_1,2
Cruise_1
Cruise 2
decelerate_1
Fig. 9. State transition diagram with two cruise speeds. Eachtransition corresponds to a motion primitive.
ψψψ(t) =
ψψψacc(t) 0 ≤ t < T acc
f
ψψψcruise(t − T accf) T acc
f≤ t < T acc
f+ T cruise
f
......
.
Since the motion primitives are computed at only a smallnumber of energy levels (in our case, only two –E =0.05,0.1) time-scaling is used to generate maneuvers at dif-ferent velocities. This exploits the time-scaling property inProposition 3.
The motion plan with a time-scaling factorλ > 0 isgiven by
q(t)← q(λt), ˙q(t)← λ ˙q(λt), τττ (t)← λ2τττ(λt),
whereλ > 1 represents time truncation (faster), andλ < 1represents time dilation (slower).
Although we can theoretically use time-scaling to achieveany arbitrary speed, it is not realistic with scalings very differ-ent fromλ = 1. This is because scaling byλ requires that thetorque level be scaled by a factor ofλ2. Thus the torque limitson the motors can easily be exceeded, forλ > 1, especiallyfor the acceleration and deceleration primitives. Forλ < 1,the fact that time-scaled motions are feasible but not optimalmay be an issue. For this reason, several motion primitivesof the same types but associated with different speeds are ob-tained. In this case, we choose to perform time-scaling on theset of motion primitives nearest to the desired speed.
By concatenating the motion primitives together, a com-plete maneuver is obtained. Figure 10 shows the longitudinalprofile for the maneuver:
0 5 10 15 20 25 300
0.5
1
1.5
2
2.5
3Longitudinal motion planning using gradual acceleration
time(sec)
Dis
plac
emen
t(m
)
0 5 10 15 20 25 30−0.05
0
0.05
0.1
0.15
0.2
time(sec)
Vel
ocity
(m/s
)
Fig. 10. Longitudinal displacement and velocity of a con-catenated motion plan.
Here the subscript indicates the specific primitive thatis used. For example,cruise1 is computed at energylevel E = 0.05, cruise2 is computed atE = 0.1,andcruise− to− cruise1,2 is the maneuver betweencruise1 andcruise2.
Figure 11 is the same maneuver with a time-scaling factorof λ = 1.2. Notice that the complete maneuver takes approx-imately 20% less time.
4.3. Practical Considerations
1. Theoretically, the quality of the motion primitives willbe improved if a large number of parameters is used toparameterize the heaving and pitching profiles. In thispaper, we have only experimented with limited ways ofparameterization (Fourier series, sinusoids with poly-nomial frequencies and amplitudes) with a handful ofparameters to the extent that usable and reasonable mo-tion primitives can be obtained so as to demonstrate theproposed methodology. The issue with increasing thenumber of parameters is that generic optimization pro-grams (such as the Matlabfmin function which usesa gradient search) may not converge, or converges toa poor local minimum. Such difficulties may be allevi-ated using optimization programs specific for trajectoryoptimization problems rather than generic ones.
2. In some cases, the number of boundary conditions makefinding a valid motion primitive difficult. This gener-
Saimek and Li / Swimming Machine 37
0 5 10 15 20 250
0.5
1
1.5
2
2.5
3Longitudinal motion planning using gradual acceleration
time(sec)
Dis
plac
emen
t(m
)
0 5 10 15 20 25−0.05
0
0.05
0.1
0.15
0.2
0.25
0.3
time(sec)
Vel
ocity
(m/s
)
Fig. 11. Time-scaling (λ = 1.2) of the longitudinal motionplan in Figure 10.
ally requires increasing the number of parameters forthe foil trajectory profiles. We find that by giving upcontinuity in the velocities of the foil motion, i.e.,ψψψat the concatenation point, the primitive motions canbe obtained much more easily. Since the foil inertia isrelatively small, it is not too difficult to change the foilvelocities rapidly. So this is not a significant problem.Nevertheless, in actual implementation, the concate-nated motion plans are splined so that˙q(t) is contin-uous at the expense that the splined motion plan doesnot strictly respect the equations of motion (7).
3. Similarly,decelerate motion primitive should ide-ally be concatenable toaccelerate. In actual im-plementation, we do not impose this constraint whencomputing thedecelerate primitives for simplic-ity. The discrepancies with the initial conditions arehandled by feedback control.
4. Theoretically, the time-scaling property is valid onlyfor constantλ. This means that each motion primitivein the complete maneuver should be scaled by the sameλ. This is quite restrictive since it prevents the motionplan to be updated during run time. In actuality, a slowlytime-varying time-scalingλ(t) can be used:
q(t)← q
t∫0
λ(τ)dτ
,
˙q(t)← λ(t) ˙q
t∫0
λ(τ)dτ
,
τττ (t)← λ2(t)τττ
t∫0
λ(τ)dτ
.
Generally, the equations of motion (7) will not strictlybe satisfied by these non-uniformly time-scaled trajec-tories because of the extra termλ(t)Mm(q) ˙q that wouldappear in the dynamics. However, these deviations areexpected to be small for smallλ to be dealt with byfeedback control.
The above considerations are also applicable to the motionplanning for the unconstrained AV system.
5. Motion Planning for Unconstrained AV
The motion planning approach illustrated for the laterally con-strained system (7) in Section 4 can also be applied to theunconstrained system (6). For motions with unconstrainedbut constant lateral position, the same basic types of motionprimitives, i.e., cruise, accelerate, decelerate,cruise-to-cruise, can also be applied. Motion plan-ning that requires non-constant lateral position is unfortu-nately more complicated because Proposition 1 states that theAV system dynamics are invariant to translation in the lon-gitudinal (α) translations, but not to lateral (R) translations.This means that motion plans derived at one lateral positioncannot be reused at other lateral positions. This limitation isan artifact of our AV setup which rotates about a hub. For anuntethered AV or one that runs on a linear track, such as theCaltech system in Kelly et al. (1998) and Kelly and Murray(2000), such limitation does not exist.
5.1. Lane-Based Motion Planning
To develop motion plans for our experimental AV sys-tem, we adopt the idea of lanes which are a finitenumber of imaginary tracks at discrete lateral posi-tions. In-lane motion primitives,cruise, accelerate,decelerate, cruise-to-cruise, are then developedfor each lane similar to the unconstrained case. In addition,new cruise− to− cruise motion primitives are de-signed to joincruises at different lanes thus enabling lanechanges. The notationcruise− to− cruise(Ri ,Ei ),(Rj ,Ej )denotes lane change from lane atRi , energyEi to lane atRj ,energyEj .
Using the lane concept, the number of primitives is signifi-cantly increased for the laterally unconstrained AV. Figure 12shows a state transition diagram for a moderate two-lane(R1 = 0.43 m,R2 = 0.48 m) two-energy-level (E1 = 0.1,E2 = 0.05) case. As the number of lanes or energy levels in-crease, the state transition diagram becomes very complicatedvery quickly. Note that the complexity will be significantly al-leviated for AVs whose dynamics are invariant to the lateraltranslations.
38 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
Fig. 12. State transition diagram for the two-lane, two-energy-level laterally unconstrained AV system. Each transitioncorresponds to a motion primitive. To avoid clutter, the“Rest” state is duplicated in two places. The top and bottomhalves correspond to motions on the lanes at R1 and R2,respectively; the left and right sides correspond to motions atenergy levels E1 and E2, respectively.
As an example of a motion plan generated using Fig-ure 12, consider starting at lateral positionR1 and the “Rest”state on the left. We can accelerateR1,1 to energy E1,cruiseR1,1 at E1 for a number of times, and then changelane using cruise− to− cruise(R1,1),(R2,1) to lateral po-sition R2, and cruiseR2,2 at energy level E2 for a number oftimes, before we decelerateR2,2 to “Rest” . This motioncan be time-scaled similarly as in the unconstrained case.
5.2. In-Lane and Lane-Changing Motion Primitives
The in-lane motion primitives cruise, accelerate,decelerate, and same lane cruise-to-cruise arecomputed similarly as in Section 4. To ensure that the mo-tion is kept in the lane, the initial and final lateral positionsare imposed R(0) = R(Tf ) = Rdesired . Other methods forconstraining the motion to be close to the desired lane arepossible (e.g., including a penalty term in the performanceindex). Using a relatively short maneuver time (1 s) for eachprimitive, we find that the lateral deviation during maneuveris acceptable without overly constraining the motion of theAV.
When computing the lane changing cruise-to-cruise motion primitives (e.g., from lateral position R1 toR2), we also impose allowable lateral excursions in the opti-mization:R(t) ∈ [R1−Rb,R2+Rb]. Otherwise, larger lateralexcursions are generated.
0 0.2 0.4 0.6 0.8 10
0.05
0.1
0.15
0.2
0.25
0.3
0.35Longitudinal motion
time (sec)
Dis
plac
emen
t(ra
d)
0 0.2 0.4 0.6 0.8 10.26
0.27
0.28
0.29
0.3
0.31Longitudinal velocity
time(sec)
Vel
ocity
(rad
/sec
)
0 0.2 0.4 0.6 0.8 10.425
0.43
0.435
0.44
0.445
0.45Lateral motion
time(sec)
Dis
plac
emen
t(m
)
0 0.2 0.4 0.6 0.8 1−0.05
0
0.05Lateral velocity
time(sec)
Vel
ocity
(m/s
)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−1
−0.5
0
0.5
1Heaving and pitching displacement
time(sec)
Ang
le(r
ad)
Heaving Pitching
−0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4−1
−0.5
0
0.5
1Phase portrait of heaving and pitching angle
Heaving (rad)
Pitc
hing
(ra
d)
Fig. 13. Unconstrained cruise at energy, E = 0.1, andlateral location R = 0.43 m: top, longitudinal and lateraldisplacement and velocity profiles; bottom, heaving andpitching profiles.
5.3. Examples
Some examples of cruise, accelerate, change-lanecruise− to− cruise, anddeceleratemotion prim-itives for the unconstrained system are shown in Figures 13–16.
Notice that, at the same energy levelE = 0.1, thecruiseprimitive for the unconstrained system is significantly fasterthan the counterpart for the constrained system (see Figures13 and 5).
Observe also that the phase relationships between heav-ing and pitching motions are similar to those in the later-ally constrained case. For example, in both cases, pitching
Saimek and Li / Swimming Machine 39
0 1 2 30
0.05
0.1
0.15
0.2Longitudinal motion
time (sec)
Dis
plac
emen
t(ra
d)
0 1 2 3−0.1
0
0.1
0.2
0.3Longitudinal velocity
time(sec)
Vel
ocity
(rad
/sec
)
0 1 2 30.41
0.42
0.43
0.44
0.45
0.46Lateral motion
time(sec)
Dis
plac
emen
t(m
)
0 1 2 3−0.15
−0.1
−0.05
0
0.05
0.1
0.15
0.2Lateral velocity
time(sec)
Vel
ocity
(m/s
)
0 0.5 1 1.5 2 2.5 3−1
−0.5
0
0.5
1Heaving and pitching displacement
time(sec)
Ang
le(r
ad)
Heaving Pitching
−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8−1
−0.5
0
0.5
1Phase portrait of heaving and pitching angle
Heaving (rad)
Pitc
hing
(ra
d)
Fig. 14. Unconstrained accelerate primitive atR = 0.43 m to energy, E = 0.1: top, longitudinal andlateral displacement and velocity profiles; bottom, heavingand pitching profiles.
leads heaving by approximately 90◦ in phase in cruiseand they are nearly in-phase in decelerate. In the caseof accelerate, the strategies seem to be different in thatin the unconstrained case (Figure 14) a multi-cycle increasingamplitude gradual acceleration strategy is employed, whereasin the constrained case (Figure 6) a one shot approach is used.Since the constrained accelerate is obtained by concate-nating two segments, they may not be directly compared.
For accelerate in Figure 14, even though the heavingand pitching frequencies are allowed to vary independently,the frequencies for both are similar and do not vary much. Forthe lane-change primitive in Figure 15, there is a positive biasof the heaving motion. This biases the foil so that it points
0 0.5 1 1.5 20
0.1
0.2
0.3
0.4
0.5
0.6
0.7Longitudinal motion
time (sec)
Dis
plac
emen
t(ra
d)
0 0.5 1 1.5 20.24
0.26
0.28
0.3
0.32
0.34
0.36
0.38Longitudinal velocity
time(sec)
Vel
ocity
(rad
/sec
)
0 0.5 1 1.5 20.43
0.44
0.45
0.46
0.47
0.48
0.49Lateral motion
time(sec)
Dis
plac
emen
t(m
)
0 0.5 1 1.5 2−0.04
−0.02
0
0.02
0.04
0.06
0.08Lateral velocity
time(sec)
Vel
ocity
(m/s
)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2−1
−0.5
0
0.5
1Heaving and pitching displacement
time(sec)
Ang
le(r
ad)
Heaving Pitching
−0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2−1
−0.5
0
0.5
1Phase portrait of heaving and pitching angle
Heaving (rad)
Pitc
hing
(ra
d)
Fig. 15. Unconstrained lane-changing cruise-to-cruise primitive from R = 0.43 m to R = 0.48 m: top,longitudinal and lateral displacement and velocity profiles;bottom, heaving and pitching profiles.
to the direction of the turn.1 Although it is difficult to drawany conclusions, it is interesting to point out that the sameheaving and pitching frequencies also appear in forward gaitsobtained from first-order averaging theory (Morgansen, Vela,and Burdick 2002). The simplified lateral control algorithm inMorgansen, Vela, and Burdick (2002) also has the observedbiasing effect.
The motion primitives for the unconstrained system canalso be concatenated. For example, the concatenated motionplan involving a lane change from R1 = 0.43 m to R2 =0.48 m is shown in Figure 17.
1. Although pitching has a negative bias, it plays a smaller role as the heavingmotion.
40 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
Feedback control is needed to ensure that the vehicle followsthe planned motion for the desired maneuver. To this end,a combination (see Figure 18) of finite-time LQR, (partial)input–output linearization and sliding mode control approachis adopted. In addition, the controller can be time-scaled, sothat each motion primitive has an associated controller. Wedescribe the controller for the unconstrained system eq. (6)which can easily be re-derived for the laterally constrainedsystem eq. (7).
For the unconstrained system eq. (6), let q = (y,ψψψ)T
where y = (α, R)T is the position of the AV, and partitionthe dynamics accordingly
0 5 10 15 200
1
2
3
4
5
6Longitudinal motion planning
time(sec)
Dis
plac
emen
t(ra
d)
0 5 10 15 20−0.1
0
0.1
0.2
0.3
0.4
0.5
time(sec)
Vel
ocity
(rad
/s)
0 5 10 15 200.4
0.42
0.44
0.46
0.48
0.5Lateral motion planning
time(sec)
Dis
plac
emen
t(m
)
0 5 10 15 20−0.2
−0.1
0
0.1
0.2
time(sec)
Vel
ocity
(m/s
)
Fig. 17. Concatenated motion plan for the uncon-strained system: { accelerateR1,2, (cruiseR1,2)
with Cm1(q, q) ∈ 2×4, Cm2(q, q) ∈ 2×4. Similarly, partitioncan easily be done for the constrained system eq. (7) withy = α, Mm11(q) ∈ , Mm12(q) ∈ 1×2, etc.
Saimek and Li / Swimming Machine 41
u(t)=M io−1
(q) 1..
(q,q)q + Cτ io
LQRI/O Linearization
ε
disturbance
disturbance
αα.
ψ. .
= u(t)
=−Mm11 m1
−1..
m11
−1(q)C (q,q)q−M (q)M m11
(q)u(t)X
R
ψ
Rψ.
.
_ _ _ _. . _ _
_ _ _ _
Fig. 18. Cascade control strategy.
Then, the AV dynamics (6) can be written as
Mm11(q)y+ Cm1(q, q)q = −Mm12(q)ψψψ (14)
Mm21(q)y+Mm22(q)ψψψ + Cm2(q, q)q = τττ . (15)
Basically, the finite-time LQR uses the heaving and pitch-ing accelerations ψψψ = (φ, θ )T as inputs to control the state,(q, q), while the feedback linearizing controller generates theactual motor torques to achieve those foil acceleration inputs.Notice that the feedback linearizing control is partial in thesense that it is concerned only with the foil acceleration. Un-modeled effects (such as vortices, etc.) and other disturbancesare compensated by a sliding mode control term.
6.1. Linear Quadratic Optimal Control (LQR)
The LQR controller considers the system
(yψψψ
)=
(−M−1
m11(q)Cm1(q, q)q0
)+
(−M−1
m11(q)Mm12 (q)I
)u,
(16)
with u = ψψψ being the input. Let xo = (qT, qT)T, and letthe desired trajectory be xdes(t) =
(qTdes(t) qT
des(t)
)T. Since
the translation motion y is the main objective of the AV, weaugment eq. (16) with the integral translational displacementerror dynamics
ey(t) = ydes(t)− y(t).
Let udes(t) = ψψψdes(t) be the nominal input, x = [ey, xTo−
xTdes]T, and u = u − udes . Consider the Jacobian linearization
of the augmented system about the trajectory [02, xTdes(t)]T and
the nominal input
˙x ≈ F(t)x +G(t)u (17)
where F(t) ∈ 10×10, G(t) ∈ 10×2. We define the perfor-mance index
J =T∫
0
[uTR(t)u+ xTQ(t)x
]dt + xT(T )Sx(T ) (18)
where Q(t) ∈ 10×10 and R(t) ∈ 2×2 have continuous en-tries, are symmetric, and are non-negative and positive def-inite, respectively. S ∈ 10×10 is non-negative definite. T isthe duration of the maneuver. Optimal control u∗(·) that min-imizes eq. (18) subject to eq. (17) is (Anderson and Moore1990)
u∗(t) = −R−1(t)GT(t)P(t)x(t) (19)
where P(t) satisfies P(T ) = S and the matrix Riccati equation
− P(t) = P(t)F(t)+ FT(t)P(t)
− P(t)G(t)R−1(t)GT(t)P(t)+Q(t). (20)
From eq. (19) we can write
u(t) = udes(t)− R−1(t)GT(t)P(t)x(t) (21)
which is the necessary heaving and pitching accelerations φand θ . Notice that the matrices, P(t), G(t), Q(t) and R(t) canbe defined or computed off-line and stored for each primitivemotions. The LQ controller for a concatenation of motionprimitives will be obtained by concatenating the LQ controller(i.e., the P(t), R(t), etc.) for each motion primitive.
6.2. I/O Linearizing Control
Now consider an input–output linearization procedure toachieve the desired acceleration inputs. Eliminating y fromthe second row of eq. (14) by substituting eq. (16) into thesecond row of eq. (14) and rewriting the resultant equation as
u(t) is given by the LQ controller (21), and Mmij and Cmj
correspond to the entry and row (for the indices) of Mm(q) andCm(q, q), etc. From eq. (22), it is easy to show that, in orderto achieve the desired acceleration of heaving and pitching,u(t), we should have τττ = τττ 1 which is given by
τττ 1 =Miou(t)+ Cio(q, q)q. (23)
42 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
6.3. Sliding Mode Control
Since central and free vortices are not modeled, and our basicLQ control law uses a Jacobian linearization, we treat theseunmodeled effects as disturbances to the system. To com-pensate for the disturbances, we adopt a sliding mode controlstrategy. We rewrite eq. (7) with disturbances to the input forceand torques and the extra sliding mode signal, εεε, as follows
Mm(q)q+ Cm(q, q)q =(
d1(t)
τττ 1 + εεε + d2(t)
)(24)
where d1(t) ∈ 2 and d2(t) ∈ 2 are the disturbances, τττ =τττ 1 + εεε, with τττ 1 given by eq. (23) and εεε is the robust controlterm to be determined.
To determine εεε, consider the Lyapunov function
V = xTP(t)x
for the linearized system under LQ control, where P(t) is thesolution to the Riccati equation (20). When τττ = τττ 1+εεε, whereτττ 1 is given by eq. (22),
V =− xT[Q(t)+ P(t)G(t)R−1(t)GT(t)P(t)
]x (25)
+2[(εεε + d2 − d1K(t))TM−1
ioGT + dT
1 M−1m11
AT]
P(t)x
where
A =02×6
I2×2
02×2
and K(t) = Mm21(q(t))M−1m11(q(t)). The effect of the distur-
bances will be compensated if the second term in eq. (25) isrendered negative by choosing a suitable εεε(t). This term canbe written in the form
εεεTKTM−1io
GTP(t)x +D(d1(t),d2(t), t).
Thus, if we choose
εεε = − D(t)
‖KTM−1io GTP(t)x‖2
2
KTM−1io
GTP(t)x
where D(t) is chosen so that D(t) ≥ D(d1(t),d2(t), t), thenwe would have V ≤ 0 in eq. (25). From eq. (25), this ispossible as long as the following matching-like condition issatisfied:
KTM−1io
GTP(t)x �= 0 when ATP(t)x �= 0.
While we cannot ensure that the above condition is alwayssatisfied, by saturating εεε when KTM−1
io GTP(t)x is small, asignificant amount of disturbances can be compensated.
6.4. Time-Scaling of the LQ Controller
As we mentioned before, the motion primitives can be time-scaled. Since the computation of the LQ controller is time-consuming, due to the need to solve the time-varying dif-ferential Riccati equation (20), the LQ controller should betime-scalable also to be useful. Notice that the I/O linearizingcontrol does not require any special re-computation once theLQ controller and the motion plan have been time-scaled.
Suppose that a motion primitive q(t) is to be time-scaledto be q(t)← q(λt). Also suppose that the linearized systemabout the scaled state and input trajectories, xλdes(t), uλ
des(t), is
˙xscaled = Fλ(t)xscaled +Gλ(t)uscaled . (26)
By direct computation, it is easy to see that Gλ(t) = G(λt)and F(t) and Fλ(t) are related by
F(t) =02×2 [I2×2 02×2] 02×4
04×2 04×4 I4×4
04×2 F32(t) F33(t)
Fλ(t) =02×2 [I2×2 02×2] 02×4
04×2 04×4 I4×4
04×2 λ2F32(λt) λF33(λt)
.
Therefore, if we define
;(λ) := diag{λ2I2×2, λI4×4, I4×4
}, (27)
we have
Fλ(t) = λ;−1(λ)F(λt);(λ); Gλ(t) = G(λt). (28)
THEOREM 1. Suppose that the LQ optimal controller for thesystem, x = F(t)x(t)+G(t)u(t)with performance index (18)are given by (19)–(20). Then, the optimal control law for thetime-scaled system (26)–(28) with respect to the performanceindex
Jλ =Tλ∫
0
uTscaled
Rλ(t)uscaled + xTscaled
Qλ(t)xscaled dt
+ xTscaled
(T
λ
)Sλxscaled
(T
λ
)(29)
with
Qλ(t) := ;(λ)Q(λt);(λ),Rλ(t) := 1
λR(λt),
Sλ := ;(λ)S;(λ),(30)
is given by
uscale(t) = −R−1λ(t)Gλ
T(t)Pλ(t)xscale(t) (31)
Saimek and Li / Swimming Machine 43
where
Pλ(t) = ;(λ)P(λt);(λ)
with P(t) given by the Riccati equation for the non-time-scaled system (20) and ;(λ) given by (27).
Proof. Using the definitions for Pλ(t), Gλ(t), Rλ(t), and Qλ(t),we need only show that Pλ(t) satisfies the Riccati equation forthe time-scaled system
− Pλ(t) = Pλ(t)Fλ(t)+ FTλ(t)Pλ(t)
− Pλ(t)Gλ(t)R−1λ(t)Gλ
T(t)Pλ(t)+Qλ(t) (32)
with the proper boundary condition. The boundary conditionsare satisfied because Pλ(T /λ) = Sλ. By direct substitutionand the fact that P(t) satisfies the Riccati equation (20) forthe non-time-scaled system, eq. (32) can be easily verified. �
From the above theorem, in comparison with eq. (21), theLQ control for the time-scaled system is simply
where x(t) = x(t)−xλdes(t). The scalings for the cost weight-
ing matrices Qλ(t), Rλ(t), Sλ in eq. (30) indicate that, forλ > 1 (time truncation) the state penalties Qλ(t) and Sλ areemphasized. This implies that time truncation and time dila-tion will have better and worse control performances respec-tively.
The time-scaling property of the LQ controller means thatwe can pre-compute the LQ controller for each motion prim-itive off-line. These controllers can then be time-scaled andconcatenated as the motion primitives are.
7. Experimental Results
The proposed motion planning and control were implementedand tested on the experimental setup (Figure 1) for both thelaterally constrained and unconstrained cases. Longitudinaland lateral motions, α and R, heaving φ and pitching θ weremeasured directly by encoders. Their velocities were obtainedby numerical differentiation of the position readings. A sam-pling time of 8 m s−1 was used.
The tuning of the LQ gains is important to obtain reason-able performance. R ∈ 2×2 was chosen to be a constantdiagonal matrix. The matrix Q ∈ 10×10 (∈ 7×7 for the con-strained system) was also constant with a structure chosen tohighlight the block control of each degree of freedom inde-pendently as follows
xTQ(t)x = zTGg(λ)z
0 5 10 15 20 25 30−0.5
0
0.5
1
1.5
2
2.5
3Desired and Actual Displacement
time(sec)
Dis
tanc
e(m
)
0 5 10 15 20 25 30−0.1
−0.05
0
0.05
0.1
0.15
0.2
0.25Desired and Actual Velocity
time(sec)
Vel
ocity
(m/s
)
Fig. 19. Desired (dashed line) and actual (solid line) dis-placement and velocity for nominal maneuver, without usingthe sliding mode controller.
where
z =
˙α + 2λαα + λ2α
∫ t
0α dτ
˙R + 2λRR + λ2
R
∫ t
0R dτ
˙φ + λφφ˙θ + λθ θ
,
α = α− αdes , R = R−Rdes , φ = φ − φdes , θ = θ − θdes , λα,λR, λφ , and λθ are positive time constants, and Gg(λ) ∈ 4×4
is a positive definite diagonal coordination gain matrix. No-tice that, if a component of z converge to 0, the correspond-ing degree of freedom will also converge exponentially withthe given time constant (λα, λR, λφ , or λθ ). Thus, tuning Qamounts to tuning the [λα, λR, λφ, λθ ], and Gg(λ). The matrixS is chosen to be a scaled version of the steady-state solutionP(t →−∞) of the Riccati equation (20) for an average F andG in eq. (16). This is an approximate method to ensure thatthe relative scalings among the various variables specified byS are similar to that specified by Q and R.
7.1. Laterally Constrained System
The motion planning and control system was first testedon the laterally constrained system eq. (7). The de-sired maneuver { accelerate1, cruise1, cruise1,cruise− to− cruise1,2, cruise2, cruise2 anddecelerate2} was tested. The accelerate2 primi-tive lasts for 4 s. Each cruise primitive lasts for 5 s. Thecruise− to− cruise1,2 maneuver lasts for 2 s. Thedecelerate2 primitive lasts for 4 s.
Figure 19 shows that the actual longitudinal displacementand velocity trajectories, α and α, tracked the desired motionfor this maneuver using LQ and I/O linearization only.
44 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
0 5 10 15 20 25−0.5
0
0.5
1
1.5
2
2.5
3Desired and Actual Displacement
time(sec)
Dis
tanc
e(m
)
0 5 10 15 20 25−0.1
0
0.1
0.2
0.3Desired and Actual Velocity
time(sec)
Vel
ocity
(m/s
)
Fig. 20. Desired (dashed line) and actual (solid line) dis-placement and velocity for time-scaled motion with λ = 1.2,without using the sliding mode controller.
0 5 10 15 20 25 30−0.5
0
0.5
1
1.5
2
2.5
3Desired and Actual Displacement
time(sec)
Dis
tanc
e(m
)
0 5 10 15 20 25 30−0.2
−0.1
0
0.1
0.2
0.3Desired and Actual Velocity
time(sec)
Vel
ocity
(m/s
)
Fig. 21. Desired (dashed line) and actual (solid line) dis-placement and velocity for nominal motion using the slidingmode controller.
The result for the same maneuver but with a time-scalingof λ = 1.2 is shown in Figure 20. We can see that the vehi-cle attained the same displacement in approximately 80% ofthe time taken in Figure 19 without time-scaling. Notice alsothat tracking performances with and without time-scaling aresimilar.
Figure 21 shows the tracking performance when the slid-ing mode controller is used in addition to the LQ and I/Olinearizing controllers. When compared to the result in Fig-
0 5 10 15 20 25−0.5
0
0.5
1
1.5
2
2.5
3Desired and Actual Displacement
time(sec)
Dis
tanc
e(m
)
0 5 10 15 20 25−0.2
−0.1
0
0.1
0.2
0.3Desired and Actual Velocity
time(sec)
Vel
ocity
(m/s
)
Fig. 22. Desired (dashed line) and actual (solid line) dis-placement and velocity for time-scaled motion with λ = 1.2using the sliding mode controller.
ure 19, the sliding mode control decreased the two-norm ofthe displacement error by about 14%. The controller schemeis quite robust to initial conditions and also to gravitationaldisturbances due to the platform not being completely planar.
Figure 22 shows the tracking performance when slidingmode controller is used for the time-scaled maneuver withλ = 1.2. Compared to Figure 20, the two-norm of the positionerror decreased by about 12%. The effectiveness of the slidingmode control can be improved if a faster sampling rate is used.
Figures 23 and 24 show additional situations for relativelylarge time-scalings (λ = 1.67 and λ = 0.8, respectively).While the control is still effective in both cases, it can be seenthat tracking performance degraded significantly when a largetime truncation is used (λ = 1.20, Figure 23). This is despitethe theoretical prediction based on scalings of the LQ weightsin Section 6.4. The reason for the performance degradation isthat at this high speed and control efforts, many surface wavesare being generated. This degrades the validity of the dynamicmodel used to design our controller. In contrast, in the time-dilation case (λ = 0.80, Figure 24), the tracking performancewas quite good.
7.2. Unconstrained System
The control system was also applied to the unconstrained sys-tem. Two motion plans were tested: a lane keeping maneuverand a lane-change maneuver. Both the nominal LQ controllerand the controller with sliding mode, as well as time-scalings,were tested. In these experiments, the Cartesian position ofthe AV was
Saimek and Li / Swimming Machine 45
0 2 4 6 8 10 12 14 16 18−0.5
0
0.5
1
1.5
2
2.5
3Desired and Actual Displacement
time(sec)
Dis
tanc
e(m
)
0 2 4 6 8 10 12 14 16 18−0.2
−0.1
0
0.1
0.2
0.3
0.4
0.5Desired and Actual Velocity
time(sec)
Vel
ocity
(m/s
)
Fig. 23. Desired (dashed line) and actual (solid line) dis-placement and velocity for time-scaled motion with λ = 1.67without using the sliding mode controller.
0 5 10 15 20 25 30 35 40−0.5
0
0.5
1
1.5
2
2.5
3Desired and Actual Displacement
time(sec)
Dis
tanc
e(m
)
0 5 10 15 20 25 30 35 40−0.1
−0.05
0
0.05
0.1
0.15
0.2
0.25Desired and Actual Velocity
time(sec)
Vel
ocity
(m/s
)
Fig. 24. Desired (dashed line) and actual (solid line) dis-placement and velocity for time-scaled motion with λ = 0.80without using the sliding mode controller.
(x(t)
y(t)
)= R(t)
(cos(α(t))sin(α(t))
).
To evaluate the performance of the control scheme, the two-norm of the individual longitudinal error α(t), the lateral er-ror R(t), and the two-norm of the Cartesian position error(x(t), y(t))− (xdes(t), ydes(t)) are used.
7.2.1. Lane Keeping
The desired maneuver tested is
{accelerateR1,2, (cruiseR1,2)14,decelerateR1,2},
in which each primitive is designed for the lane at R1 =0.43m, for energy level of E2 = 0.1. Each accelerate,cruise, and decelerate primitive lasts 3 s, 1 s, and 3 srespectively, so that the complete maneuver lasts 20 s.
Figures 25 and 26 show the achieved performance for thenominal lane-keeping maneuver, with and without the slidingmode control, respectively. Both controllers worked reason-ably and, in both cases, the AV stopped within 0.5 cm of thedesired lane. When using the sliding mode controller, how-ever, the two-norm of the Cartesian errors and of the longitu-dinal error decreased by about 12%, and the two-norm of thelateral position error by 5%.
Figures 27 and 28 show the achieved performance for theλ = 1.1 time-truncated lane-keeping maneuver, with andwithout the sliding mode control, respectively. In this case,with the sliding mode controller, the two-norm of the Carte-sian position error and of the lateral error were improved byapproximately 10%, while the longitudinal position error wasnot improved.
7.2.2. Lane-Change Maneuvering Control
The lane-change maneuver was tested using the maneuver:
{accelerateR1,2, (cruiseR1,2)5,
cruise− to− cruise(R1,2),(R2,2),(cruiseR2,2)
10, decelerateR2,2}.Each change-lane cruise− to− cruise primitive lastsfor 2 s, and the other primitives are the same as in the lane-keeping case. The complete maneuver lasts for 23 s.
Figures 29 and 30 show the achieved performance for thenominal and time-truncated (λ = 1.25) lane-keeping maneu-ver, with and without the sliding mode control. The controllerperformed reasonably in both cases. Without using slidingmode controller (Figures 31–32), the errors were slightlyworse to a similar degree as in the lane-keeping case.
Notice from Figures 29–32 that the lateral velocities re-mained 0 at various times, especially at the end of the maneu-ver. These flat regions indicate that there is significant frictionin the railings. This can indeed be verified by pushing on theAV manually in the lateral direction.
In general, the control performance is limited by the sam-pling time (8 ms) used in our setup, as the sliding modegains cannot be too high. We hope to overcome this limitationby migrating to another real-time environment in the future.However, the control system does work repeatably and con-sistently. The system is even able to correct for deliberatelygenerated wave disturbances.
46 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
Fig. 32. Unconstrained lane-change with time-scaling ofλ = 1.25 using nominal control: top, Cartesian path; middle,longitudinal motion; bottom, lateral motion.
50 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
8. Conclusions
In this paper, we have presented control strategy and experi-mental results for maneuvering an AV using an oscillating foilas a propulsor. The strategy consists of an off-line motion plan-ning step and a on-line feedback control step. Motion planningis achieved by using optimal control approach while controlstrategy is accomplished by a cascade of finite-time, time-scalable LQ control, input–output linearization, in combina-tion with a sliding mode controller. To avoid on-line compu-tational expense in the motion planning and control design foreach maneuver, complete maneuvers are defined by concate-nating time-scaled copies of motion plans and the controllersfor each motion primitive according to a state transition dia-gram. This is achieved by exploiting the special properties ofthe AV system. The proposed approach has been verified ex-perimentally. Our present approach does not make explicit useof vortex shedding or vortices already in the water, but onlytreats these as disturbances. Although the controller is robustenough to overcome these effects, the achieved motion maynot be energy efficient. For this reason, our current research isdirected toward purposefully incorporating the contributionsof vortex shedding into both motion planning and control.
Appendix A: Derivation of the AV Equations ofMotion
We derive the system model for an AV in two ways. Thefirst is obtained by writing down the kinetic energies of thewater and the mechanical only portion of the AV, and thenapplying the Euler–Lagrange equation. The second methodcomputes the hydrodynamic forces acting on the dynamics ofthe mechanical only portion of the AV.
A.1. Energy Method
The complex potential ω(z) is given by
ω(z) = U(t)ω1(ζ )+ V (t)ω2(ζ )+�(t)ω3(ζ )
+γc(t)ω4(ζ )+∑k
γkω5(ζ ; ζk(t)) (34)
where ζ = F−1(z) with F : ζ �→ z being the Joukowskitransformation (1)
ω1(ζ ) = − r2c
ζ+ ζc + a2
ζ + ζc (35)
ω2(ζ ) = −ir2c
ζ− iζc − i
a2
ζ + ζc (36)
ω3(ζ ) = −i
(ζcr
2c
ζ+ a2 r
2c/ζ + ζcζ + ζc
+ a4ζc
(ζ + ζc)(r2c+ δ2)
+a4 + r2
c− δ4
2(r2c− δ2)
)(37)
ω4(ζ ) = i logζ
rc(38)
ω5(ζ ; ζk) = i log
(− rcζk
ζ − ζkζ − r2
c/ζk
). (39)
The sum of the kinetic energies of the AV and of the sur-rounding water is given by
T = L
2ρ
∫Sv
dω
dz
(dω
dz
)dA+ 1
2qTM(q)q
where ρ is the density of the water, and L is the out-of-planedimension of the foil. The integral in the first term is takenover the area outside the foil and outside any free vortices.
Let Qs(z) = [ω1(z), ω2(z), ω3(z)] ∈ C1×3, and ignoringcentral and free vortices, we have
dω
dz(z) = dQs(z)
dz
U(t)V (t)
�(t)
.
The kinematic relationship (Figure 33)
U = −R sin θ + (R − Ld)α cos θ + Lh(φ + α) sin(θ − φ)V = −R cos θ − (R − Ld)α sin θ + Lh(φ + α) cos(θ − φ)
+ Lf (θ + α)� = α + θ , (40)
where Ld, Lh and Lf are the AV parameters which are shownin Figure 33, implies that
UV�
= Q2(q)q
for some Q2(q) ∈ 3×4. Thus, we define the combined inertiamatrix of the system to be
Mm(q) :=M(q)
+ LρQT2 (q)RS
∫Sv
dQs
dz
TdQs
dzdA
QT
2 (q) (41)
whereRS[·] denotes the real symmetric part of the argument.The second term of eq. (41) is the hydrodynamic added masswhereas the first term is the inertia of the AV itself. The totalkinetic energy (4) can then be written as
T = 1
2qTMm(q)q.
The added mass component in eq. (41) can be evaluatedusing the Area Theorem (Milne-Thomson 1968), which statesthat, given f (z, z) which is a continuous and differentiablefunction in the area A enclosed by the contour C, an area
Saimek and Li / Swimming Machine 51
���
���
����
����
α
φ
θ
R Ld
C.G.
L h
Lf
X
Y
2
Fig. 33. Top view of the AV.
integral can be written in terms of a contour integral as follows:∫C
f (z, z)dz = 2i∫A
∂f
∂zdA,
∫C
f (z, z)dz = −2i∫A
∂f
∂zdA. (42)
Using the fact that ωi(z) is a holomorphic function so thatdωjdz= dωj
dz= 0, and applying the Area Theorem to the el-
ements of the matrix inside [·] in eq. (41), we have for the(i, j)th entry given by
∫Sv
dωidz
dωjdz
dA =∫Sv
d
dz
[ωi
dωjdz
]dA
=− i
2
∫S
ωi(z)dωj(z)+ i
2
∫C2
ωi(z)dωj(z)
=− i
2
∫S
ωi(z)dωj(z).
(43)
Here, S (the foil) and C2 (a circular contour at infinity) formpart of a connected contour that encircles all the water outsidethe foil (Figure 34). Because the water is at rest at infinity, theC2 contour integral is zero.
The integrals in eq. (43) are then computed explicitly (al-though rather tediously) by first using eq. (35), then per-forming partial-fraction expansion, and finally applying theCauchy Residue theorem (Milne-Thomson 1968).
Once the matrix Mm(q) in eq. (41) is obtained, the dynam-ics of the AV is computed using the standard Euler–Lagrangeequation.
A.2. Hydrodynamic Force Method
An alternative approach to obtain the equations of motionof the AV system is to consider the interaction forces and
o
AB
ξ
eEdesired path
Fig. 34. Contour used in computing the added mass matrix.
the moment between the foil and the water as added massand virtual damping. Consider the equations of motion of themechanical portion of the AV
M(q)q+ C(q, q)q =
00τhτp
+
FintαFintRτinthτintp
(44)
where 0 < M ∈ 4×4 is a mass matrix of the AV, C(q, q)q ∈4 is a Coriolis vector of the AV, τh and τp are heaving andpitching input torques, and [Fintα , FintR , τinth , τintp ]T is hydro-dynamic interaction forces and torque vector.
To calculate the hydrodynamic interaction forces andtorque on the foil, we consider the pressure distribution onthe foil per unit span. We write the real and imaginary part ofthe complex potential in eq. (2) as
ω(z, t) = φw(z, t)+ iψw(z, t)
where φw(z, t), the velocity potential andψw(z, t), the streamfunction, are real functions, and z = x + iy, the pressuredistribution, is given as
p(z) = −ρ ∂φw∂t
(z, t)− 1
2ρ(u2 + v2)
where (u, v) is the velocity of the fluid particle at location zgiven by (Streitlien 1994; Li and Saimek 1999)
dω
dz= u− iv. (45)
Forces and torque are obtained by integrating the pressuredistribution per unit span around the foil S to obtain
Fx + iFy = i∫S
p(z) · dz
τz = Re
∫S
z p(z) · dz
52 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / January 2004
where Fx and Fy are the forces in the direction parallel andperpendicular to the foil’s cord, and τz is the counterclock-wise moment on the foil parallel to its span. The detail ofthe derivation can be found in Streitlien (1994) and Milne-Thomson (1968). Since we assume that there is no contri-bution from central and free vortices, the contour is a closedcontour around the foil and at infinity. After some tedious cal-culations (using the Cauchy Residue theorem) detailed in Stre-itlien (1994), we can obtain the constantsmij , i, j = 1, · · · , 6which depend only on the size and shape of the foil in
FxFyτz
=
−m11 0 0
0 −m22 00 −m62 −m66
U
V
�
+ m22�V +m26�
2
−m11�U
−m26�U + (m11 −m22)UV
. (46)
The relationship between the interaction forces in the absoluteframe and the hydrodynamic generalized forces in eq. (44) isobtained using the virtual work principle:FintαFintRτhintτpint
=
(R − Ld) cos θ −(R − Ld) sin θ 0− sin θ − cos θ 0
Lh sin(θ − φ) Lh cos(θ − φ) 00 Lf 1
FxFyτz
. (47)
Differentiating eq. (40), substituting into eqs. (46) and (47),the interaction hydrodynamic generalized forces can be ex-pressed as added mass and virtual damping
FintαFintRτhintτpint
= −VM(q)q− VC(q, q)q (48)
where VM ∈ 4×4 is the added mass matrix, and VC(q, q)q ∈4 is virtual damping. Substituting eq. (48) into eq. (44), weobtain
Mm(q)q+ Cm(q, q)q =
00τhτp
(49)
where Mm(q) = M(q)+ VM(q) is a modified mass matrix,which is a combination of the AV mass matrix and addedmass matrix, and Cm(q, q)q = C(q, q)q + VC(q, q)q is amodified Coriolis vector, which is a combination of the AVCoriolis vector and virtual damping vector.
The dynamic models obtained using the energy method orthe hydrodynamic force method are exactly the same.
Acknowledgment
This research is partially supported by the University of Min-nesota Graduate School Grant-in-aid program.
References
Anderson, B. D., and Moore, J. B. 1990. Optimal Control: Lin-ear Quadratic Methods, Prentice Hall, Englewood Cliffs,NJ.
Barrett, D., Grosenbaugh, M., and Triantafyllou, M. 1996.The optimal control of a flexible hull robotic undersea ve-hicle propelled by an oscillating foil. In Proceedings ofthe IEEE Symposium on Autonomous Underwater VehicleTechnology, 1–9.
Barrett, D., Triantafyllou, M., Yue, D., Grosenbaugh, M., andWolfgang, M.J. 1999. Drag reduction in fish-like locomo-tion. Journal of Fluid Mechanics392, 183–212.
Bryson Jr., A. E. 1999. Dynamic Optimization, Addison Wes-ley Longman, Reading, MA.
Czarnowski, J., Cleary, R., and Kreamer, B. May 1997. Ex-ploring the possibility of placing traditional marine vesselsunder oscillating foil propulsion. In Proceedings of the 7thInternational Offshore and Polar Engineering Conference,Vol. 2, 76–81.
Godhavn, J., Balluchi, A., Crawford, L., and Sastry, S. 1999.Steering of a class of nonholonomic systems with driftterms. Automatica35:837–847.
Gray, J. 1936. Studies in animal locomotion. VI. The propul-sive powers of the dolphin. Journal of Experimental Biol-ogy13:192–199.
Harper, K. A., Berkemeier, M. D., and Grace, S. 1998. Model-ing the dynamics of spring-driven oscillating-foil propul-sion. IEEE Journal of Oceanic Engineering23(3):285–296.
Hoar, W., and Randall, D., editors. 1978. Fish Physiology,Vol. 7, Academic Press, New York. Locomotion.
Kato, N. 2000. Control performance in the horizontal planeof a fish robot with mechanical pectoral fins. IEEE Journalof Oceanic Engineering25(1):121–129.
Kelly, S. D., and Murray, R. M. 2000. Modeling efficient pis-ciform swimming for control. International Journal of Ro-bust and Nonlinear Control10:217–241.
Kelly, S. K., Mason, R. J., Anhalt, C. T., Murray, R. M., andBurdick, J. W. June 1998. Modeling and experimental in-vestigation of Carangiform locomotion for control. In Pro-ceedings of the American Control Conference, 1271–1276.
Li, P., and Saimek, S. 1999. Modeling and estimation of hy-drodynamic potentials. In Proceedings of the IEEE Con-ference on Decision and Control.
Liu, P., and Bose, N. 1999. Hydrodynamic characteristics ofa lunate shape oscillating propulsor. Ocean Engineering26:519–529.
Mason, R., and Burdick, J. 1999. Construction and modeling
Saimek and Li / Swimming Machine 53
of a Carangiform robotic fish. In International Symposiumon Experimental Robotics, Sydney, Australia.
Mason, R., and Burdick, J. 2000. Experiments in Carangiformrobotic fish locomotion. In Proceedings of the 2000 In-ternational Conference on Robotic and Automation, 428–435.
Mclsaac, K. A., and Ostrowski, J. P. 2002. Experiments inclosed-loop control for an underwater eel-like robot. InProceedings of the 2002 IEEE International Conferenceon Robotics and Automation, 750–755.
Milne-Thomson, L. M. 1968. Theoretical Hydrodynamics,5th edition, Dover, New York.
Morgansen, K., Duindam, V., Mason, R., Burdick, J., andMurray, R. 2001. Nonlinear control methods for planarCarangiform robot fish locomotion. In Proccedings of theIEEE International Conference on Robotic and Automa-tion, 427–434.
Morgansen, K., Vela, P. A., and Burdick, J. 2002. Trajectorystabilization for a planar Carangiform robot fish. In Pro-ceedings of the IEEE International Conference on Roboticand Automation, 756–762.
Murray, R. M., and Sastry, S. S. 1993. Nonholonomic motionplanning. steering using sinusoids. IEEE Transactions onAutomatic Control38(5):700–716.
Reyhanoglu, M., van der Schaft, A., McClamroch, N. H., andKolmanovsky, I. 1999. Dynamics and control of a class ofunderactuated mechanical systems. IEEE Transactions onAutomatic Control44(9):1663–1670.
Streitlien, K. 1994. A simulation procedure for vortex flowover an oscillating wing. Technical Report MITSG 94-7,Massachusetts Institute of Technology Sea Grant.
Techet, A., and Triantafyllou, M. 1999. Boundary layer relam-inarization in swimming fish. In The International Offshoreand Polar Engineering Conference, Vol. 2.
Triantafyllou, M. S., and Triantafyllou, G. S. 1995. An effi-cient swimming machine. Scientific American272(3):64–70.
Vela, P. A., Morgansen, K. A., and Burdick, J. W. May 2002.Second-order averaging methods for oscillatory control ofunderactuated mechanical systems. In Proceedings of the2002 American Control Conference, 4672–4677.
Yamaguchi, H., and Bose, N. April 1994. Oscillating foilsfor marine propulsion. In Proceedings of the 4th Interna-tional Offshore and Polar Engineering ConferenceVol. 3,539–544.
Yen, V., and Nagurka, M. L. 1990. Fourier-based optimal con-trol approach for structural systems. Journal of GuidanceControl and Dynamics13(2):265–276.
