Underwater Vehicles Part 11 pps

Underwater docking experiments The goal of the experiments was to verify the final approach algorithm and system validity.. Underwater docking experiment without the attitude keeping co

Development of Test-Bed AUV ‘ISiMI’ and Underwater Experiments on Free Running

Fig 18 Raw Image (left) and Noisy Luminaries (right): Several lamps were outside of the basin, and the dock lights are reflected down from the water surface

Fig 19 Image process sequence (test screenshots was used): Raw image (left), binary some noisy luminaries remain (center), and processed image - elimination of noisy

image-luminaries and discrimination of the dock lights

7 Final approach algorithm

It was first suggested by Deltheil et al (2000) that a vision system is suitable for docking because it offers simplicity, stealthiness and robustness In this chapter, a final approach algorithm based on vision-guidance is suggested It was supposed that the AUV could be

guided to the dock by controlling only yaw and pitch This final approach algorithm

generates reference yaw and reference pitch and makes the AUV track them

The docking stage begins when the AUV arrives within 10-15 m in front of the dock The

docking stage of the return process is subdivided here into two stages because there exists

an area where the dock lights are out of the camera viewing range when the AUV is close to

the dock Figure 20 shows the first and second stages During the second stage the AUV is

about 1.4m from the dock, and the lights of the dock are out of the range of the camera The

essential difference of the second stage is the manner of generating reference yaw for

steering motion and reference pitch for diving motion During both parts of the docking

stage, a conventional Proportional-Derivative (PD) control is applied to track the references

Values of these gains were tuned by trial-and-error using the results of the simulations and

underwater experiments

A The first stage

In this stage, reference yaw and pitch were generated based on vision-guidance All dock

lights were located in the viewing range of the CCD camera This vision-guidance controller

generated reference yaw and reference pitch using the estimated center of the dock A

discrepancy between the estimated dock center and the origin of the image coordinates

became an error input of the vision-guidance controller Fig 21 is a block diagram of the

vision-guidance control A Proportional-Integral (PI) controller was used to generate

reference yaw and pitch from the position error To eliminate steady-state error, I-control

was used By conducting repeated underwater experiments, values of the PI gains were

tuned

Fig 20 The 1st stage and the 2nd stage of docking approach

B The second stage

When the distance estimated by the image processing became smaller than a pre-specified

threshold value, the second stage began In this area, the last reference yaw and pitch

Development of Test-Bed AUV ‘ISiMI’ and Underwater Experiments on Free Running

become fixed Because the AUV is very close to the dock, it was supposed that changing yaw or pitch could be dangerous and keeping the final references would be plausible This method is referred to as ‘attitude keeping control.’ (Park et al., 2007) During this phase, ISiMI becomes blind and simply tracks these final fixed references until contacting the dock Fig 22 shows a flow chart of the final approach algorithm

Fig 21 The vision-guidance control algorithm Po is the origin of the image coordinate frame Pc is the estimated center of the dock θ is pitch, ψ is yaw θ ref and ψ ref are generated reference pitch and yaw, respectively

Fig 22 Flow chart of the final approach algorithm

8 Underwater docking experiments

The goal of the experiments was to verify the final approach algorithm and system validity Figure 23 describes the initial start point for the final docking approach It shows a top view (left) and a side view (right) of the initial start conditions The dock was placed within viewing range of the camera The center of the dock was placed at a depth of 1.5m The dock was introduced by (Lee et al, 2003), (Park et al, 2007) The dock was funnel-shaped This shape makes it possible for the AUV to dock successfully through sliding even if she approaches obliquely The dock used an external power source

Because robustness against disturbance has not yet been developed and this attempt was

during the early stages of development, some restrictions were applied There was no

current and there were no waves The dock was fixed on the basin floor The water was

clean ISiMI was operated using a wired LAN communication RF wireless communication

was not suitable to receive the large amount of image data necessary The wireless LAN was

disconnected when the AUV submerged The R.P.M of the thrust propeller was invariant

and the forward speed was about 1.0m/s The relation between R.P.M and speed was

determined by (Jun et al 2008) There was no speed control Experiments without the

attitude keeping control and experiments with the attitude keeping control were conducted

separately

A Underwater docking experiment without the attitude keeping control

Only the vision-guidance control was applied No distance estimation was applied ISiMI

depended on the camera until contact with the dock In Fig 24, pixel errors are plotted

against time A pixel error is defined as deviation between the origin and the estimated

center of the dock center in the image coordinate The pixel errors decreased and were

regulated during the first 9 seconds of the test However, between seconds 9-15, there were

discontinuous oscillations These oscillations were caused by the defect of the image

processing system to process, not by actual motions of the AUV, i.e one more light moved

out of the camera viewing range The AUV became confused and it could not find the center

of the dock This occurred when the AUV was in the second stage area To estimate the

center precisely, all five lights were required, but in this area, the AUV could not see all of

them It was found that the AUV had some head-on collisions with Light #5 or the inner

plane of the dock She performed imprecise final approaches and suffered collisions with the

dock Fig 25 is a sequence of continuously grabbed images taken by an underwater camera

(a) ISiMI starts, (b) she cruises to (c) the dock, (d) an imprecise approach near the dock, (e)

after a collision, she rebounded and (f) she could not enter the dock Thus, it was proven

that the vision-guidance control was not unnecessary during this part of the docking

procedure

Fig 23 Initial start point: (left) top view and (right) side view

Development of Test-Bed AUV ‘ISiMI’ and Underwater Experiments on Free Running

Fig 24 Position error (unit: pixel) in the image coordinate The vision-guidance control was applied through all intervals (1) t = 0-9seconds : Errors are decreasing In this interval, all 5 lights were in the viewing range of the camera The AUV was able to estimate the center precisely (2)t = 9-15seconds : one or more lights were out of the viewing range of the camera Precise estimation of the center became impossible The oscillation was caused by the defect of information from image processing rather than actual motion of the AUV

Fig 25 Docking : Grabbed images by an underwater camera (Arrows indicate moving directions of the AUV): (a): ISiMI starts, (b): She cruises to (c) the dock, (d): An imprecise approach near the dock, (e): After a collision, she rebounded, and (f): She could not enter the dock

B Underwater docking experiment with the attitude keeping control

The attitude keeping controller was applied when ISiMI was near the dock Image

processing was used to estimate both the location of the center and the distance to the dock

The patterns were similar to that of Fig 24 during the first 9 seconds of the test Oscillations

of the sort encountered during the first test were anticipated after 9 seconds However, after

the vision-guidance control was stopped, the reference yaw and pitch were fixed by the

attitude keeping controller In Fig 26, the solid lines are the yaw(the upper graph) and

pitch(the lower graph) measured by AHRS The short-dash lines are the generated reference

yaw and pitch After 9 seconds, the references were fixed The long-dash lines show the

fixed references and the AUV tracked them Fig 27 shows the moment of docking The

Fig 26 Final approach: (upper) Yaw, Ref yaw and fixed ref yaw (lower) Pitch, Ref pitch

and fixed ref pitch are shown respectively After 9 seconds, ref yaw and ref pitch were

fixed (1)t = 0-9seconds : The vision-guidance control was applied In this interval, all 5

lights were in the viewing range of the camera (2)t = 9-15seconds : At t = 9.4seconds, the

attitude keeping control began and the references were fixed

Fig 27 Docking: (left) The original photograph The original photograph was sharpened

anad the edge of ISiMI was emphasized to make her more easy to recognize The white

arrow indicates ISiMI (right)

Development of Test-Bed AUV ‘ISiMI’ and Underwater Experiments on Free Running

original photo was sharpened and the edge of IsiMI was emphasized in order to make her more easy to recognize The photo shows that ISiMI was going into the dock with a more precise approach

9 Conclusion

In this chpater, the design, implementation and test results of a small AUV named ISiMI are presented The AUV, ISiMI, developed in KORDI is a test-bed for the validation of the algorithms and instruments of the AUV For fast experimental feedback on new algorithms, ISiMI was designed to be able to cruise in the Ocean Engineering Basin environment at KORDI The zigzag test and the turning test were carried out to check ISiMI’s maneuvering properties The depth control and waypoint tracking tests were carried out to validate the feedback controller of ISiMI The experiment results were compared with those of the simulation The research works were fed back to the design and implementation of a 100m-class AUV named ISiMI100 ISiMI100 is equipped with additional sensors such as a doppler velocity log, an acoustic telemetry modem, an obstacle avoidance sonar, a range sonar, and a GPS module A photo of ISiMI100 is shown in Fig 28 The mission test of ISiMI and the sea trial of ISiMI100 remain to be performed in future works

Figure 28 Sea-trial version of ISiMI AUV named ISiMI100

A final approach algorithm based on vision guidance for the underwater docking of an AUV was developed and introduced The algorithm allowed the tested AUV to identify dock lights, eliminate interfering luminary noises and successfully estimate both the center

of the dock and the distance to it during the first stage of the docking sequence despite the fact that the AUV was unable to detect the dock lights when close to the dock The final approach algorithm based on vision guidance did guide the AUV to the dock successfully The area where the lights were out of the camera viewing range occasioned confusion, as expected, but the attitude keeping control was able to keep the AUV on the way to the dock Underwater docking experiments showed the necessity of the attitude keeping control The use of the attitude keeping control as well as the vision-guidance control improved the precision of docking performance The fixed references guided the AUV more precisely and safely Although the docking experiments were conducted under controlled conditions, the results of the experiments showed the utility and potential of the vision-based guidance algorithm for docking

Future problems include successfully docking when (1) the dock is moving, (2) the dock is placed out of the camera viewing range at the beginning of a return process, and (3) currents

and waves are present Generation of the optimized path from any initial start point to the

dock is also a subject for future study

10 Acknowledgments

This work was supported in part by MLTMA of Korea for the “development of a deep-sea

unmanned underwater vehicle,” and KORDI, for the “development of ubiquitous-based key

technologies for the smart operation of maritime exploration fleets."

11 References

Allen, B., Stokey, R., Austin, T., Forrester, N., Goldsborough, R., Purcell, M., von Alt, C.,

1997 REMUS: A small, low cost AUV; system description, field trials and

performance results Proceedings of the Oceans Conference pp 994-1000

Allen, B., Austin, T., Forrester, N Goldsborough, R Kukulya, A., Packard, G., Purcell M.,

Stokey, R., 2006 Autonomous docking demonstrations with enhanced REMUS

technology, Proceedings of OCEANS 2006 MTS/IEEE, pp 1-6

Bellingham, J.G., Bales, J.W., Goudey, C.A., Consi, T.R., 1993 Performance characteristics of

the Odyssey AUV Proceedings of the Eighth International Symposium on

Unmanned Untethered Submersible Technology (AUV '93), pp 37-59

Cowen, S., Briest, S., Dombrowski, J., 1997 Underwater docking of autonomous undersea

vehicles using optical terminal guidance, Proceedings of OCEANS ’97 MTS/IEEE,

Vol 2, pp 1143-1147

Deltheil, C., Didier, L., Hospital, E., Brutzman, D P., 2000 Simulating an optical guidance

system for the recovery of an unmanned underwater vehicle, IEEE Journal of

Oceanic Engineering, Vol 25, No 4, pp 568-574

Edwards, D.B., Bean, T.A., Odell, D.L., Anderson, M.J., 2004 A leader-follower algorithm for

multiple AUV formations Proceedings of IEEE/OES pp 40-46

Feezor, M D., Blankinship, P R., Bellingham, J G., Sorrell, F Y., 1997 Autonomous

underwater vehicle homing/docking via electromagnetic guidance, Proceedings of

OCEANS ’97 MTS/IEEE, Vol 2, pp 1137-1142

Feldman, J., 1979 DTNSRDC Revised Standard Submarine Equations of Motion

DTNSRDC/SPD-0393-09

Fiorelli, E., Leonard, N.E., Bhatta, P., Paley, D., Bachmayer, R., Fratantoni, D.M., 2004

Multi-AUV control and adaptive sampling in Moterey Bay Workshop on Multiple Multi-AUV

Operations (AUV04) pp 134-147

Fossen, T.I., 1994 Guidance and Control of Ocean Vehicles John Wiley & Sons Ltd

Gertler, M., Hagen, G R., 1967 Standard equations of motion for submarine simulations,

NSRDC Report No 2510

Healey, A.J., Lienard, D., 1993 Multivariable sliding-mode control for autonomous diving

and steering of unmanned underwater vehicles IEEE Journal of Oceanic

Engineering, 18 (3), 327-339

Hobson, B W., McEwen, R S., Erickson, J., Hoover T., McBride, L., Shane, F., Bellingham, J

G., 2007 The development and ocean testing of an AUV docking station for a 21”

AUV, Proceedings of OCEANS 2007

Development of Test-Bed AUV ‘ISiMI’ and Underwater Experiments on Free Running

Hong, Y H., Kim, J Y., Lee, P M., Jeon, B H., Oh, K H., Oh, J H., 2003 Development of the

homing and docking algorithm for AUV, Proceedings of the Thirteenth International Offshore and Polar Engineering Conference, pp 205-212

Jeon, B.H., Lee, P.M., Li, J.H., Hong, S.W., Kim, Y.G., Lee, J., 2003 Multivariable optimal

control of an autonomous underwater vehicle for steering and diving control in variable speed, Proceedings of Oceans Conference, San Diego, pp 2659-2664 Jun, B H., Park, J Y., Lee, P M., Ma, S J., Kim, Y S., Oh, J H., Lim, Y K., 2007 Design,

implementation and free running test of ISiMI; an AUV for cruising in ocean engineering basin environment, Proceedings of OCEANS 2007 IEEE Aberdeen Jun, B H., Park, J Y., Lee, F Y., Lee, P M., Lee, C M., Kim, K H., Lim, Y K., Oh, J H., 2008

Development of the AUV ‘ISiMI’ and a free running test in an Ocean Engineering Basin, Ocean Engineering, http://dx.doi.org/10.1016/j.oceaneng.2008.07.009 Kim, J Y., Park, I W., Oh J H., 2006 Experimental realization of dynamic walking of the

biped humanoid robot KHR-2 using zero moment point feedback and inertial measurement, Advanced Robotics, Vol 20, No 6, pp 707-736

Lee, P M., Hong, S.W., Lim, Y.K., Lee, C.M., Jeon, B.H., Park, J.W, 1999 Discrete-time

quasi-sliding mode control of an autonomous underwater vehicle IEEE Journal of Oceanic Engineering, 24 (3), 388-395

Lee, P M., Jeon, B H., Lee, C M., 2002 A docking and control system for an autonomous

underwater vehicle, Proceedings of OCEANS 2002 MTS/IEEE, pp 1609-1614 Lee, P M., Jeon, B H., Kim, S M., 2003 Visual servoing for underwater docking of an

autonomous underwater vehicle with one camera, Proceedings of OCEANS 2003, Vol 2, pp 677-682

Lee, P M., Jun, B H., Kim, K H., Lee, J H., Aoki, T., Hyakudome, T., 2007 Simulation of an

inertial acoustic navigation system with range aiding for an autonomous underwater vehicle IEEE Journal of Oceanic Engineering, 32 (2), 329-345

Lee, P M., Jun, B H., 2007 Pseudo long base line navigation algorithm for underwater

vehicles with inertial sensors and two acoustic range measurements, Ocean Engineering, Vol 34, Issues 3-4, pp 416-425

Myring, D F., 1976 A theoretical study of body drag in subcritical axisymmetric flow,

Aeronautical Quarterly, vol 27, pp 186-194

Nerstein W., Smith, K.C., 1968 Hydrodynamic coefficient equations and computer

programs The bendix Corporation report

Park, J Y., Jun, B H., Lee, P M., Lee, F Y., Oh, J H., 2007 Experiment on underwater

docking of an autonomous underwater vehicle ‘ISiMI’ using optical terminal guidance, Proceedings of OCEANS 2007 IEEE Aberdeen

Prestero, T., 2001 Verification of a Six-Degree of Freedom Simulation Model for the REMUS

Autonomous Underwater Vehicle M.S Dissertation, MIT and WHOI

Shapiro, L G., Stockman, G C, 2001 Computer Vision, Prentice Hall, NJ

Singh, H., Bellingham, J.G., Hover, F., Lerner, S., Moran, B.A., Heydt, K., Yoerger, D., 2001

Docking for an autonomous ocean sampling network IEEE Journal of Oceanic Engineering, 26 (4), 498-514

Stokey, R., Allen, B., Austin, T., Goldsborough, R., Forrester, N., Purcell, M., Alt, C.V., 2001

Enabling technologies for REMUS docking : an integral component of an autonomous ocean-sampling network IEEE Journal of Oceanic Engineering, 26 (4), 487-497

Utkin, V.I., 1977 Variable structure system with sliding modes IEEE Transactions on

Automatic Control, 22 (2), 212-222

http://auvlab.mit.edu/vehicles/vehiclespecEARLY.html#OD1

http://www.gavia.is/downloads/brochures/GaviaBrochure0402.pdf

21

Trajectory Planning for Autonomous

Underwater Vehicles

This chapter is a contribution to the field of Artificial Intelligence Artificial Intelligence can

be defined as the study of methods by which a computer can simulate aspects of human intelligence (Moravec, 2003) Among many mental capabilities, a human being is able to find his own path in a given environment and to optimize it according to the situation requirements For an autonomous mobile robot, the computation of a safe trajectory is crucial for the success of a mission Here is the ultimate goal of the trajectory planning issue for autonomous robots:

given a set of internal and external constraints from the robot capabilities and from the environment

what is the best trajectory solution to reach a given target?

This is the problem we want to solve in this chapter For this purpose, a novel approach is developed which is inspired from a level set method that originally emerged within the image processing community This method, called Fast Marching (FM) algorithm, is analyzed and extended to improve the trajectory planning process for mobile robots Theory and algorithms hold for any kind of autonomous mobile robot Nonetheless, since this research work has been supported by the Oceans Systems Laboratory, the trajectory planning methods are applied to the underwater environment Simulations and results are given assuming the use of an autonomous underwater vehicle (AUV)

1.2 Underwater environment and autonomous underwater vehicles

In mobile robotics, trajectory planning research has focussed on wheeled robots moving on surfaces equipped with high rate communication modules The underwater environment is much more demanding: it is difficult to communicate because of low bandwidth channels undersea; it is prone to currents; and the three dimensional workspace may be worldwide Moreover, torpedo-like vehicles are strongly nonholonomic

The current state of technology allows many laboratories such as the Oceans Systems Laboratory to move forward in the development of AUVs The need for a reliable cognition process for finding a feasible trajectory derived from underwater imagery is important

1.3 Contributions

The main contribution of the authors is to present a Fast Marching based method as an

advanced tool for underwater trajectory planning (Petres et al., 2007) With a similar

complexity to classical graph-search techniques in Artificial Intelligence, the Fast Marching

algorithm converges to a smooth solution in the continuous domain even when it is

implemented on a sampled environment This specificity is crucial to the understanding of

the other contributions of our method:

• FM* algorithm: we develop a new algorithm called FM* that is a heuristically guided

version of the Fast Marching algorithm The FM* algorithm combines the efficiency of

the A* algorithm (Hart, 1968) with the accuracy of the Fast Marching algorithm

(Sethian, 1999)

• Curvature constrained trajectory planning: the FM* algorithm allows the curvature of

the trajectory solution to be constrained, which enables us to take the turning radius of

any mobile robot into account

• Dynamic and partially-known domains: a dynamic version of the Fast Marching

algorithm, called DFM, is proposed to deal with dynamic environments DFM

algorithm is then proved to be very efficient to recompute trajectories after minor

changes in the robot perception of the world

• Simulations and open water trials: a complete architecture has been designed,

developed and tested for simulated and real AUV missions In-water experiments are

compared to simulation results to demonstrate the performance and usefulness of the

DFM-based trajectory planning approach in the real world

2 Trajectory planning framework

2.1 Environment representation

The usual framework to study the trajectory planning problem among static or dynamic

obstacles is the configuration space (C-space) The main idea of the C-space is to represent the

robot as a point, called a configuration

A robot configuration is a vector of parameters specifying position, orientation and all the

characteristics of the robot in the environment The C-space is the set of all possible

configurations Its dimension is the number of parameters that defines a configuration

C-free is the set of configurations that are C-free of obstacles Obstacles in the workspace become

C-obstacles in the C-space

Usually a simple rigid body transformation (Latombe, 1991) is used to map the real

environment into the C-space We focus on 2D and 3D C-spaces in this chapter, nonetheless

this framework holds for C-spaces of any dimensions

where [0,1] is the parameterization interval and s is the arc-length parameter of C If xstart

and xgoal are the start and the goal configurations respectively, then C(0) = xstart and C(1) =

xgoal

Trajectory Planning for Autonomous Underwater Vehicles 401

An optimal trajectory is a curve C that minimizes a set of internal and external constraints

(time, fuel consumption or danger for instance) It is assumed in this chapter that the

complete set of constraints is described in a cost function τ:

τ(x)xΩ:

1,x ) τ Cx ,x (s) ds

where Cx1,x2 is a trajectory between two configurations x1 and x2, and τ is the cost function

This metric can be seen as the “cost-to-go” for a specific robot to reach x2 from x1 At a

configuration x, τ(x) can be interpreted as the cost of one step from x to its neighbours If a

C-obstacle in some region S is impenetrable, then τ(S) will be infinite The function τ is

supposed to be strictly positive for an obvious physical reason: τ(x) = 0 would mean that

free transportation from some configuration x is possible

2.4 Distance function concept

A grid-search algorithm aims at building a distance functionu:Ω2→ℜ, which is solution

of the functional minimization problem defined as follows:

{ }ρ(x ,x)inf

x),u(xstart C ,x start

start x

where {Cx start , x}is the set of all the possible curves between the source xstart and the current

configuration x within Ω For the sake of notational simplicity, and assuming that the source

of exploration xstart is fixed, we note u(xstart, x) = u(x)

The distance function u may be related to the value function concept in reinforcement

learning The difference lies only in the fact that value functions are refined in an iterative

process (called learning), whereas the distance function is built from scratch In the path

planning literature one can find other names for the distance function, such as navigation

function (LaValle, 2006), convex-map (Melchior et al., 2003) or multi-valued distance map

(Kimmel et al., 1998)

Once the distance function has been found through the goal configuration, the optimal path

is the one which follows the gradient descent over the distance function from the goal to the

start configuration This backtracking technique is reliable as no local minima have been

exhibited during the exploration process

3 Fast marching based trajectory planning

3.1 Related previous work

A method for computing consistent distance functions in the continuous domain was first

proposed in (Tsitsiklis, 1995) but the method of the author is less efficient than the Fast

Marching method (Sethian, 1999) A FM based trajectory planning method among moving

obstacles has been proposed in (Kimmel et al., 1998) The Fast Marching algorithm has also

been applied in trajectory planning in (Melchior et al., 2003), where the authors compare A*

and FM efficiencies among static obstacles In (Philippsen & Siegwart, 2005), the authors

develop a FM based trajectory planning method that allows dynamic replanning and

improves Fast Marching efficiency in the case of a-priori unknown or dynamic domains All

these works are close in spirit to what we describe in this chapter except for the fact that we

introduce a heuristic in a novel FM* algorithm to speed up the exploration process

3.2 Eikonal equation

Before introducing the Fast Marching algorithm itself, we start from the observation that the

functional minimization problem (4) is equivalent to solving the Eikonal equation:

τ

u =

We give here a geometrical intuition in two dimensions of how to convert equation (4) into

equation (5) It is inspired by a level set formulation of the Eikonal equation in (Cohen &

Kimmel, 1997) and a formal proof can be found in (Bruckstein, 1988)

Fig 1 On a small surface dΩ around a configuration x with a radius dx, one can

approximate the distance function u as a plane wave, for which the level sets are parallel

between them and perpendicular to the gradient u∇ of u

We start from the fact that the gradient u∇ of u is normal to its level sets Let nG=∇u/∇u ,

where is the Euclidean norm, be the outwards unit normal vector to level sets of u located

in x (see figure 1) Express a variation du of u according to a variation dx of the position x:

dxu,u(x)dxx

u⎜⎛ + ⎟= + ∇

dxu,

where , is the standard dot product in ℜ2

Within the small region dΩ of Ω centered on x with a radius dx, we can assimilate τ as a

constant: ∀p∈dΩτ(p)=τ(x)=τ

Within dΩ level sets of u are seen as straight lines:

Trajectory Planning for Autonomous Underwater Vehicles 403

dx,nτ

From equations (6) and (7) we get ∇u,dx =τ ∇u,dx /∇u , which leads to the Eikonal

equation (5)

3.3 Upwind schemes and numerical approximations

The Fast Marching algorithm uses a first order numerical approximation of the Eikonal

equation (5) based on the following operators Suppose a function u is given with values

)

u(x

u k= i, k on a 3D Cartesian grid with grid spacing h

• Forward operator (direction i): D i (u) (ui 1,k u k)/h

The following upwind scheme, originally due to Godunov (Godunov, 1969) and well

explained in (Rouy & Tourin, 1992) and in (Sethian, 1999), is used to estimate the gradient

2 j j,k i, j j,k i,

2 i j,k i, i j,k i,

τ(u),0D(u),Dmax

(u),0D(u),Dmax

(u),0D(u),Dmax

=+

3.4 Fast Marching algorithm

3.4.1 Pseudo-code

The pseudo code of the Fast Marching algorithm is given in table 1 The FM algorithm relies

on a partitioning of the C-space in three sets: Accepted configurations for which the distance

function u has been computed and frozen, Current configurations for which an estimate v of

u has been estimated (and not frozen), and the remaining Unvisited configurations for which

u is unknown

Definitions

Start is the set of start configurations;

Goal is the set of goal configurations;

xtop is the configuration in priority queue Current with the highest priority

Procedure Initialization()

{01} Accepted = Start, u(Accepted) = 0;

{02} Unvisited = Ω \ Accepted, u(Unvisited) = v(Unvisited) = ∞;

{03} Current = Neigh(Start), v(Current) = τ(Current);

Procedure Main()

{04} Loop : while Goal ⊄ Accepted

{05} Remove xtop from Current and insert it in Accepted with u(xtop) = v(xtop);

{06} FMComputeV(Neigh(xtop));

Table 1 Pseudo code of the Fast Marching algorithm

The set of Current configurations is stored in a priority queue On top of this queue the

configuration with the highest priority is called xtop At each iteration of the exploration

process, xtop is moved from Current to Accepted and its Unvisited neighbours are updated

and moved from Unvisited to Current The exploration process expands from the start

configuration and ends when the goal configuration is eventually set to Accepted

3.4.2 Computation procedure

The computation procedure for the 3D Fast Marching algorithm described in table 2 can be

found in (Deschamps & Cohen, 2001) We give here additional calculation details to update

the distance function estimate vk of an xtop's neighbour xk with a cost τk

Procedure FMComputeV(Neigh(x top ))

01} Loop : for all configurations xk in Neigh(xtop)

{02} If xk is Unvisited, then remove it from Unvisited and insert it in Current with vk = ∞

{03} If xk is Current then apply case 1 or case 2 for the computation of vk

{04} Sort Current list according to the priority assignment

Table 2 Pseudo code of the FM procedure for updating Neigh(xtop)

One, two or three Accepted configurations are used to solve equation (8) We note {A1, A2},

{B1, B2} and {C1, C2} the three couples of opposite neighbours of xk (in 6-connexity) with the

ordering u(A1) ≤ u(A2), u(B1) ≤ u(B2), u(C1) ≤ u(C2) and u(A1) ≤ u(B1) ≤ u(C1) Two different

cases are to be examined sequentially:

Case 1: considering that vk ≥ u(C1) ≥ u(B1) ≥ u(A1), the upwind scheme (8) is equivalent to:

( ) ( ) ( ( ) ) ( ( ) ) 2

k 2 1 k 2 1 k 2 1

1 1 1 k

CuBuCuAuBuAuCuBuAu23τ

3

1

CuBuA

+

−

+++

1 k 2 1

1 1 2 1

1

2

1BuAu21

Trajectory Planning for Autonomous Underwater Vehicles 405

In the Fast Marching algorithm the highest priority is assigned to the Current configuration

xtop with the lowest estimate etop = v(xtop), see table 3

xtop (etop) x1 (e1) x2 (e2) … xN (eN)

Table 3 List of Current configurations stored in a priority queue The highest priority is

given the to lowest estimate e: etop < e1 < e2 < … < eN

Since u(x) does not depend on the goal configuration, the distance function u is built

symmetrically around the start configuration, see figure 2.a In this figure, distance maps

and trajectories have been computed over a constant cost map We use cool colours for small

distances and hot colours for high distances (in arbitrary units)

Fig 2 Examples of distance maps and trajectories computed over a constant 100x100 cost

map (τ = 1) using a 4-connexity: a) FM algorithm and b) FM* algorithm (using the Euclidean

distance He as a heuristic)

In the FM* algorithm the highest priority is assigned to the Current configuration xtop with

the lowest estimate etop =

2

1v(xtop) +

2

1

He(xtop, xgoal) Here He(xtop, xgoal) is the heuristic that

estimates the residual distance between the Current configuration xtop and the goal

configuration xgoal Similarly to the A* algorithm, instead of exploring around the start

configuration, the FM* algorithm focuses the search towards the goal configuration, see

figure 2.b

Bi-directional versions of these grid-search algorithms can also be implemented We just

have to launch the grid-search algorithm simultaneously from the start and the goal

configurations We stop it when the two sets of Accepted configurations are merging

4 Curvature constrained trajectory planning

In this section, differential constraints are reduced to curvature constraints A Fast Marching

based fully coupled approach (Petres et al., 2007) is proposed that ensures the trajectory

solution to be smooth enough for an AUV with a given turning radius

4.1 Problem statement

In this section the influence of the cost function τ on the smoothness of a trajectory C is

analyzed Here C is the solution of the functional minimization problem:

)x,ρ(xargminC

)x,

~Ω

2 1 } {C 2

1 2

2 , 1

=

→

where Ω~ is the set of all the possible curves in Ω, {Cx1, x2} is the set of all the possible curves

in Ω between x1 and x2 and ρ is the continuous metric:

=[0,1] 1 22

1,x ) τ Cx ,x (s) ds

The Fast Marching method computes a derivable solution C associated with the continuous

metric ρ Therefore, tools from differential geometry can be used to examine the curvature

properties of C

Let us define the curvature parameters considered here

• Curvature magnitude of a curve C:

s

Ck(C) 22

• Lower bound on the curvature radius along a curve C: Rmin(C)=infs∈[0,1]R(C(s))

• Turning radius of a vehicle v: r(v)

4.2 Lower bound on the curvature radius

Given a cost function τ, our goal is to insure the feasibility of any trajectory C for an AUV v

before computing the distance function u Mathematically speaking, we want

r(v)(C)R

will express a formal link between the cost function τ and the lower bound Rmin(C) for any

curve C minimizing the metric ρ between two configurations

Using the differential geometry framework, it is shown in (Caselles et al., 1997) that the

Euler-Lagrange equation associated with the functional minimization (14) is:

0NNτ,N

where NG is the normal unit vector to a curve C

From equation (16), it is deduced in (Cohen & Kimmel, 1997) that the curvature magnitude k

is bounded along any curve C minimizing ρ The lower bound Rmin is then:

Trajectory Planning for Autonomous Underwater Vehicles 407

}τ{supτinfR

Ω

Ω

The conclusion is that to increase the lower bound on the curvature radius Rmin(C) of an

optimal trajectory C, two choices are possible:

• smoothing the cost function τ to decrease supΩ{∇τ}

• adding an offset to the cost function to increase the numerator infΩτ without affecting

the denominator

The following illustrations depict some trajectories computed using the FM* algorithm after

smoothing the cost map (figure 3) and after smoothing the cost map and adding an offset

(figure 4)

Fig 3 Influence of smoothing the cost function a) A binary 100x100 cost function τ,

τ(C-free) = 1 and τ(C-obstacles) = 11 and the related optimal trajectory Ca, Rmin(Ca) = 332 (in

arbitrary units) b) τ after smoothing using a 11x11 average filter, Rmin(Cb) = 1216 c) τ after

smoothing using a 21x21 average filter, Rmin(Cc) = 1377

Fig 4 Influence of both smoothing and adding an offset The original cost function τ is

similar to the one in figure 3.a a) Offset = 5, average filter 7x7, Rmin(Ca) = 1977 (in arbitrary

units) b) Offset = 5, average filter 15x15, Rmin(Cb) = 2787

5 Trajectory planning in dynamic and partially-known domains

The two problems of planning trajectories in unpredictable dynamic environment and in

partially-known environments are equivalent In both cases the robot has to adapt its plans

continuously to changes in (its knowledge of) the world In this section we present a

dynamic version of the Fast Marching algorithm called DFM and we compare it to A*, FM,

FM* and D* Lite algorithms in simulated 2D environments The DFM based trajectory

planning method is eventually tested in a real open water environment using the AUV

prototype of the Ocean Systems Laboratory

5.1 DFM algorithm

The DFM algorithm is inspired from the LPA* and D* Lite algorithms described in (Koenig

et al., 2004) It is similar to the E* algorithm developed by Philippsen in (Philippsen &

Siegwart, 2005) but we prefer to name this algorithm DFM instead of E* because the asterisk

usually refers to heuristically guided search algorithms (such as A* and D* algorithms)

Since no heuristic has been integrated yet in any dynamic version of the Fast Marching

algorithm, we propose to use the abbreviation DFM for Dynamic Fast Marching

According to the principle of optimality it is not necessary to recompute an entire trajectory

from A to B when a change appears in C somewhere between A and B An efficient

algorithm may only update the trajectory from C to B and leave the sub-trajectory from A to

C unchanged

5.1.1 Local consistency concept

Since changes appear dynamically in the cost function, any configuration x ∈ Ω may be

updated more than once The computation process of the distance function needs to be

dynamic and the previous division between Accepted, Current and Unvisited sets of

configurations is not compliant any more with a refresh of an Accepted configuration Recall

that an Accepted configuration in the static FM algorithm is frozen Several updates of the

estimate v(x) of the distance function u for a configuration x in Current is possible but the

exploration process can only proceed forward from Unvisited to Accepted such as a flame in

a landscape The “engine” of this mechanism is that, at each iteration of the FM algorithm,

the configuration xtop is moved from Current to Accepted Then, its neighbours are updated,

and the process continues until the goal configuration (initially tagged as Unvisited) is set as

Accepted The “Unvisited-Current-Accepted” scheme is well designed for static problems

since a configuration can only proceed one way:

uv

AcceptedCurrent

In the DFM algorithm, the “tripartite” structure “Unvisited-Current-Accepted” is removed

and replaced by a more subtle mechanism between the estimate v and the distance function

u The latter structure is made dynamic by the fact that the relationship between u and v is

bilateral The estimate v, which is affected by changes in the cost function τ, is computed

from u, but u itself is computed from v:

uv

This mechanism, described in detail in the pseudo-code of the next section, stops when v

and u match The “engine” that leads to the “bipartite” agreement between v and u is the

processing of a priority queue Q that contains exactly the inconsistent configurations

defined as follows (Koenig et al., 2004) A configuration x is called locally consistent if v(x) =

u(x) and is called locally inconsistent if v(x) ≠ u(x) In (Philippsen & Siegwart, 2005), the

authors reproduce this inequality in their pseudo-code However, since Fast Marching

methods use real numbers for approximating the distance function, a tolerance ε (set

empirically at 0.1 in our implementations) must be introduced in the DFM algorithm so that

the previous inequality becomes: