An Obstacle Avoidance System for an Autonomous Underwater Vehicle Mike Eichhorn Institute of Automation and Systems Engineering Technical University of Ilmenau, PO Box 100565, 98684 Ilmenau, Germany (phone +49 3677 691421; fax +49 3677 691434 e-mail:
[email protected]) Abstract— This paper describes an Obstacle Avoidance System for an AUV. The specific requirements of the underwater world, the computational capacity and the sensors of the vehicle as well as its manoeuvrability was considered by the choice and the development of strategies used. Such requirements include the sea current information, the consideration of moving objects, the evasion in 3D and the constrained view of sonar. The system consists of a hierarchical two-level architecture. In the upper level is the course planning, which generates a route for the vehicle to follow. In the lower level a reactive control system is installed, which guide the vehicle by sudden emergence of obstacles or deactivated course-planning module. Index Terms— Autonomous Underwater Vehicle, Geometric Graph, Obstacle Avoidance, Path Planning
I. INTRODUCTION This article presents a part (guidance of the vehicle in special situations) of the current research project "DeepC". On this project an interdisciplinary consortium, consisting of German companies and research establishments, works under guidance of the Federal Ministry for research and development, on the development of an autonomous underwater vehicle [1], [2]. This underwater vehicle should be able to accomplish a given mission and act independently in unexpected situations (emerging obstacles, loss of components). The vehicle is specified by the following performance parameters:
These is realized by the following technical details: • • • • •
Separate payload- and vehicle hulls Redundancy construction of all important systems and separate placing in the individual vehicle hulls (1) Energy generation by using PEM fuel cells Exchangeable payload container (4) Specific software modules for the fulfilment of the required autonomy
A schematic figure of the vehicle, including with its most important drive and manoeuvring equipment is represented in Figure 1. DeepC obtains here good positioning and manoeuvring qualities in standing position and at less speed by the thrusters (2). At higher speeds, the manoeuvring of the vehicle in done by a combination of the thrusters (2), the fin units (3) and rotation speed differences of the main drives (5).
1
5
2 3
Parameter Limits of the vehicle
4
Value Length: 5800 mm
5
Bright: 2300 mm Height: 1700 mm Weight
2,4 t
Weight Payload
300 kg
Diving Depth
4000 m
Cruise/Max speed
4 kts / 6 kts
Endurance
40 h
Equipment
TV-Camera Sonar
Energy
Polymer Electrolyte Membrane (PEM) fuel cell
0-7803-8541-1/04/$20.00 @2004 IEEE
Figure 1: Structure of the AUV DeepC For the obstacle avoidance system of the vehicle a two-level structure was preferred. The first level uses in addition to the current information of the sonar, the collected obstacle data of the past mission as well as the data of a digital sea chart. Here a global path planning on the basis of geometrical graphs is done. These methods make it possible to determine an optimal way, which the vehicle has to follow according to pre-defined criteria in a feasible time. If unexpected, suddenly emerging obstacles arise while driving on the generated path, the lower level, the reactive control, takes over. This level reacts to the obstacles detected in close range by the sonar by providing appropriate reactive control commands, whereby path-optimal obstacle avoidance is not possible. This option is here
however not at highest priority. Here a safe and fast avoiding of a collision situation is concerned. Figure 2 shows travel routes generated by the path planning and the reactive control modules on an example of a possible obstacle scenario, respectively.
flux lead from the positive to the negative charge. The value for the negative charge in the goal is selected to -1. The positive charge of the obstacle can be expressed by the equation:
q= Path Planning
Reactive Control
R R+D
(1)
R is the radius of the obstacle circle. D describes the distance between the two charges. By equation (1) it is guaranteed that all gradient lines, which begin within the circle, are led out to end in the goal. Gradient lines, which begin outside of the circle, run exclusive outside of the circle. For the proof, it can be referred to [3] and [4]. For the determination of the resulting gradient G at the current vehicle position, the two gradients for the positive and negative charges are to be computed separately and summarized by superposition. Figure 3 shows the potential field with its gradient lines, developed in the way described previously.
Figure 2: Travel routes by application of the different levels
II. REACTIVE EVASIVE CONTROL BY MEANS OF GRADIENT LINES The idea of the method presented in this chapter is based on the generation of lines, which point to the goal from any position of the area of operation. The target course of the vehicle is determined by calculation of the course gradient (G) at the current vehicle position. In this way the vehicle can be led past the obstacles to the goal from its current position. Since in the reactive level only the x-y plane of the obstacles is considered, only the 2D-world is to be treated in this section. However, by appropriate modifications it is also possible to use this procedure for the 3D-World. The method presented in this chapter is based on the work of [3], [4]. Here gradient lines are produced by artificial harmonious dipole potentials. The negative characteristics of the artificial potential fields can be compensated by the use of such dipole potentials. Such characteristics include the trapping of the vehicle in local minima, oscillating movements in narrow passages as well as closing of other possible routes by the resulting repulsive field at the entrance of a passage. A method is presented, which makes the production of the gradients from a geometrical construction of the gradient lines possible. Furthermore, possibilities of avoiding obstacles using the speed information of the sea-current and the obstacles are pointed out.
A. Production of gradient lines by harmonious dipole potentials For the following variants it is assumed that a circle describes an obstacle. If one takes the circle as a positive point charge and the goal as a negative point charge, then this structure forms a dipole and its lines of
Gradient lines
negative Charge -1 (Goal point)
positive Charge q (Obstacle)
D
R
The object recognition software module in the form of elliptical cylinders will supply the obstacle information for the alternative methods. This form can be generated easily from the raw data of the sonar and is characterized by few geometrical dimensions (major axis, minor axis and rotation angle of the ellipse, height of the cylinder). Obstacle avoidance with the reactive control takes place only in the X-Y plane. This is justified by the limited pitch angle of the vehicle and good manoeuvrability in the horizontal. During active course planning obstacle avoidance can take place in the 3D. In the following chapters the two levels are described in detail regarding their functions and algorithmic conversions.
Gradient lines
Figure 3: Gradient lines of the dipole Since the gradient lines can tangents the obstacle (however, not cut it), the radius R of the circle must be increased by an amount, which prevents a collision between vehicle and object. Here an increase by n-multiples of the vehicle length is appropriate. The circle developed in this way is also called the safety zone [3]. If the vehicle is on the side of the goal and drives in the proximity of the symmetry axis of the dipole it is forced to go around the obstacle before reaching the obstacle (see Figure 3). Strong guidance commands are necessary, in order to keep the vehicle on the given gradient line. Furthermore, the form of the gradient lines within this range and in the laterals does not correspond to an optimal travel route. These effects increase with the increase in the parameters R and D. These disadvantages are to be eliminated in the next section by the geometrical construction of gradient lines for optimal guidance.
B. Production of gradient lines by geometrical construction To start with, it is mentioned here that the gradient lines produced in this section do not have physical "models". They must only lead to an optimal travel route. To accomplish this, the area of operation is divided into individual sectors (see Figure 4). The division into sectors depends on the vehicle position xveh, the obstacle center xobst and the goal position.
α=
2
x veh − xobst − R Rslid − R
(5)
G = α G sector1or2 + (1 − α ) G sector3
1
3 2
R
outside of circle
Goal point
inside of circle
Figure 5 shows a gradient field produced in this way.
Obstacle
Gradient lines
2
Figure 4: Division of the area of operation into sectors
Obstacle
Goal point
Rslid
If the condition (|xveh - xobst| dline _ max G = G obst_n for dline < −dline _ max α G obst_m + (1-α ) G obst_n for dline < d line _ max
α = 0.5 1 +
The centre of the ellipse can be expressed as follows:
E. Application to elliptical safety zones In the preceding descriptions it was assumed that a circle describes an obstacle. Since a circle describes many obstacles unfavourably, an elliptical form for such obstacles is more favourable. The adjustment of the presented method to an elliptical form would need much computational effort and would not be feasible at all. Here a pre-transformation must be accomplished, which transfers a turned ellipse with the position
xcenter ellipse
into a circle lying in the origin. The
computation of the gradient lines is then done for circular obstacles, as described in section B and then an inverse transform follows. The transformation for any point x in this space consists of several steps: 1) Translation of the point by
xcenter ellipse
(Shift of the ellipse into the origin of the display space.)
x trans = x − xcenter ellipse
(13)
2) Rotation of the point around the rotation angle of the ellipse θellipse. (Rotation of the ellipse to orient axis-parallel to the ellipse in the display space.)
cos θ ellipse x trans_rot = − sin θ ellipse
sin θ ellipse x cos θ ellipse trans
(14)
d m _n
xveh
2d m _ n
The resulting gradient can be expressed as follows:
The coefficient vrel describes the relationship between the amount of the resulting speed |vres| and the given cruising speed vcruise of the vehicle.
xobst_vel
2 2 d veh _ n − d veh _ m
(7)
−1
vrel =
A simplified determination of this distance is done via the following equation based on Figure 7.
d line
2d line_max
3) Scaling of the point with the inverse values of the semi major and minor axis (aellipse and bellipse) of the ellipse. (Accomplishment of a circle for the ellipse with R = 1.)
d
ve h_ n
xobst_n
Figure 7: Simplified determination of dline
Obstacle n
1 a ellipse x = 0
0 x trans_rot 1 bellipse
(15)
-40 Safety area
-30
Obstacle
-20 Start point 1
y(m)
-10 0 10 Goal point
20 30 Start point 2 20
40
60 x(m)
80
100
After execution of this transformation the actual computations can be accomplished in the display space. The gradient G computed here must be back transformed again from the display space into the original space (equation (16), (17)). Rescaling:
G rescal
aellipse = 0
Re-rotation:
cos θ ellipse G= sin θ ellipse
0 G bellipse
120
Figure 8: Paths through the obstacle scenario
Start point 1: x=5 y=-15 50
r(deg/s)
0
0
-50
(16)
Geometric Construction Harmonic Dipole Potentials 0
10
20
30 40 Time(s) Start point 2: x=5 y=30
50
60
50
− sin θ ellipse G cos θ ellipse rescal
(17)
F. Testing the method Finally the procedure of the geometrical construction is to be compared with the procedure of the harmonious dipole potentials. Therefore an obstacle scenario illustrated in Figure 8 was defined. The vehicle is to be driven with a cruising speed of 2m/s through the obstacles. It could be shown that with the use of the geometrical construction method the target course change rate r possesses substantially smaller maximum values than by using the procedure of the harmonious dipole potentials (see Figure 9). This is an important criterion for real applications, since the applied vehicle cannot follow reference trajectory changes very fast, which causes deviations in the gradient lines and thus to possible collisions. Furthermore each fast change of target course causes an increase in navigational error. Already for this reason such manoeuvres should be avoided. The necessary path length for driving through the obstacle scenario with the geometrical procedure amounts up to 9% less than that with the method of the harmonious dipole potentials. The trajectories produced by the method with the geometrical gradient construction method are similar to those in [5] generated by an evolution strategy.
r(deg/s)
40
Harmonic Dipole Potentials Geometric Construction
0
-50
Geometric Construction Harmonic Dipole Potentials 0
10
20
30 Time(s)
40
50
60
Figure 9: Evaluation of the reference turning rate r
III. PATH PLANNING During course planning the entire information about the current area of operation is used for the generation of a route suggestion. This information includes apart from the current information of the sonar, the "collected" obstacle data of the past mission as well as the data of a digital sea chart. The generation of such a route suggestion takes place based on graphs. Points (vertices) in the area of operation, which are passable by the vehicle, are defined. The passable connections between these points are registered as edges in the graph. Each edge possesses an evaluation (weight), which can be the length of the connection, resulting costs when travelling on the connection or the time needed for it (see Figure 10).
Edges
5
Vertices 3
4
3
2.5
2.5 2.5
1.5
2.5
Start point
1
2
2 1
4
4
1
1 1.5
Goal point
Weights
2
for disc > 0 :
2.5
2
2.5
T T 2 disc = ( xunit path _ dir ⋅ x current ) + vveh _ bf − x current ⋅ x current
Figure 10: Geometrical Graph After generation of such a graph, a path is determined by the initial vertex (starting point) to the end vertex (goal) with the smallest total costs by search algorithms (Dijkstra [6], A * - [7], D * - algorithm [8]). They scan the graph, in order to determine a combination of edges, which connect the starting vertex and the end vertex with the smallest sum of edge weights. In the next sections the procedures are described for the generation of a graph using the current environmental information.
T v path _ ef = xunit disc path _ dir ⋅ x current +
If the discriminant becomes negative, it means that the vehicle can no longer be held in that path. Is the speed vpath_ef is negative the vehicle is still on the path, but moving backwards. Both cases must be considered by setting a large numerical value of the edge weight. Thus such paths are excluded in the search and it does not come to a situation that the vehicle comes into a strong backwards current and leaves the course. By applying weighted functions described above it is also possible to determine a time-optimal path by complex see current profiles. This is described in the following example. The task is to determine a time-optimal path when crossing a river, which possesses the following current profile: x vcurrent = 0.0
To include the see current information into the evaluation of the edge the necessary time to travel on the edge is used as criterion. This time is calculated for the ith edge by formation of the quotient of the distance spath and the speed vpath_ef, with which the vehicle travels on the path in relation to a fixed world coordinate system.
t
=
4 0 x ( b − x ) vcurrent 2 b
y vcurrent =
A. Calculation of Cost Value
i path
(20)
s ipath
(18)
v ipath _ ef
This speed vpath_ef is dependent on the vehicle speed vcurrent through the water vveh_bf (cruising speed), the amount and the direction of the see current vector as well as the direction of the path xpath_dir. It can be determined by the intersection point between a line and a circle (2D) and/or sphere (3D) [9] based on Figure 11 according to the following relation:
0 with b = 300m vcurrent = 1.8
(21)
m s
Figure 12 shows the path determined by the graph method as well as the time-optimal solution by optimal control [10]. Both paths show similar profiles, which can be confirmed by every swimmer, who crossed a river with strong current. First one swims against the current, then in the centre of the river let himself carried by the river towards the other bank. The graph for this task is made up of grid network of nodes. A finer discretisation of this net would lead to a further approximation to the accurate solution. Path planning with current 30 Optimal Control Graph Method 20 Current profile
path dir
y(m)
nt
eh _b
re cu r
v
y
vv
x path_
ψ
10
f
0
ef
v path_
-10
Intersection point
x
-20
-30
Figure 11: Definition of the velocities
line: x ( v path _ ef ) = v path _ ef x 2 veh _ bf
circle / spheres : v
0
50
100
150 x(m)
200
Figure 12: Path planning with see current unit path _ dir
= x − x current
2
(19)
250
300
B. Possible illustration of the environment in a graph In this section 2 methods are presented for the production of a geometrical graph. These methods were selected based on the given obstacle form (elliptical cylinder) and the demand on computation time of the algorithms under the given computational restrictions of the vehicle. The application of the methods in the 2D and 3D-world should be possible.
1) Visibility graph In the visibility graph the corners of the obstacles form the nodes of the graph [11]. Obstacles without corners (circle, ellipse) are modelled by a polygon, which includes the obstacle completely. All possible lines between the nodes, which do not cut obstacles, become edges of the graph. Figure 13 shows a graph generated in this way with the elliptical obstacle form used in the project. xgoal
Thus a systematic dismantling of the area of operation, similar to the branching of a tree, takes place. After production of all sectors the graph is developed. In addition the centres of all obstacle-free sectors form the vertices of the graph. The edges are the connections between neighbouring obstacle-free sectors (see Figure 14).
C. Practical implementation of the tests The accomplished tests took place on a Pentium III 450 MHz Desktop. This computer is not to reflect the technical equipment condition of our institute, but the arithmetic and logic unit used in the vehicle. There single board computers of the type VP7 with a Pentium III processor 450 MHz of the company SBS Technologies are used [12]. For the tests an obstacle environment with the dimensions 170 m length, 84 m width, and 50 m height was defined. Within this area of operation obstacles were placed according to the following pattern (see Figure 15): Test test5 test10 test15 test20 0
xstart
y(m)
20
xstart
Number of obstacles 5 10 15 20
2
Obstacle numbers 1-5 1-10 1-15 1-20
4 8
12
15
16
40
6
19
3
13
xgoal
9
20 18
60
Figure 13: Visibility graph
2) Quadtree/Octree graph
xgoal
2 3
xstart
Figure 14: Quadtree graph
7
5
14
80
Here the area of operation is partitioned into square (2D-plane) and/or cubic (3D-space) sectors. As a function of obstacles found in them they can be "free" "occupied" or "partly occupied". Partly occupied sectors are recursively divided, until a maximum division depth and/or a minimum sector size is reached. As "free" or "occupied" classified sectors are not divided any more.
1
10
17
1
11
4
0
20
40
60
80 x(m)
100
120
140
160
Figure 15: Obstacle scenario The contours in Figure 15 correspond to the obstacles increased by a safety area. A practice relevance of Test15 and test 20 is not possible by the limited capacity of the sonar. These two tests are to control the efficiency of the algorithms. The tests were accomplished for the 2D and 3D space. In the 2D-Tests the obstacles were projected on the X-Y plane. The graphs resulting from the tests include the cost function presented in section A of this chapter. This leads to the generation of a directed graph, since each edge include its own cost value depending on the driving direction. The Boost graph LIBRARY [13], [14] was used for the path search. It contains a library with complete data structures for graphs and search algorithms and supports many C++ compilers. As path search algorithm the Dijkstra method came to employment. Furthermore, the Borland C++ Builder Version 6.0 Compiler [15], Microsoft Visual C++ (Version 6.0) und Microsoft Visual C++ .NET (Version 7.0) [16] were used for the tests. Since Borland C++ Builder was used in the project, the computing times specified in Figure 16 and Figure 17 are a result of the programs, which were compiled by this compiler.
25 0
2D-tests computation time
ms 20 0
15 0
10 0
50 S e arc h P at h G en e rat e Grap h
0 VG te s t5
VG Visibility Graph QG Quadtree Graph
QG t es t 5
VG t e s t1 0
QG t es t 10
VG t e st 1 5
QG te s t 15
VG t e st 2 0
QG te s t2 0
Number of Vertices
146
356
176
606
212
806
258
941
Number of Edges
918
1910
1114
3224
1502
4208
2112
4844
Figure 16: 2D tests 2000
3D-tests
ms
The presented method for the geometrical construction of gradient lines connects the advantages of the method of the artificial harmonious dipole potentials with the demands on optimal path and control driving fashion. The sea-current as well as possible movements of the obstacles were considered in a simple manner by the construction of the gradient lines by formation of a substitute form for the obstacle. By using the graph methods for the determination of a route suggestion in the course planning level the demand on an energy-optimal driving fashion could be easily considered. The inclusion of the sea-current into the cost function makes possible to exclude the areas with strong sea-current from route planning. The selection of the examined methods for the generation of a geometrical graph was done based on the given obstacle form (elliptical cylinder) and the computational restrictions. The accomplished tests showed the real time ability of the graph methods during route planning. The implementation of the presented algorithms on the goal platform and the tests with the AUV simulator will be finished in summer 2004. Subsequently, the first free water attempts with the AUV take place.
computation time
1800
ACKNOWLEDGMENT
1600 1400
This work is part of the current research project “DeepC”, which is being promoted by the BMBF. (Promotion no: 03SX104E). I would like to thank Professor J. Wernstedt, who made it possible for me to work in this interesting project, Marco Jacobi for the software-technical support and exchange in experience in the development of the graph methods and my colleague Divas Karimanzira for good co-operation and assistance during the project.
1200 1000 800 600 400
S earc h P ath
200
G enerate G raph
0
VG Visibility Graph OG Octree Graph
VG tes t5
OG tes t5
VG tes t10
OG tes t10
VG tes t15
OG tes t15
VG tes t20
OG tes t20
Number of Vertices
266
1834
416
3102
447
4314
539
4989
Number of Edges
3382
29482
7528
49819
7972
68182
11786
77804
Figure 17: 3D tests The comparison of Figure 16 and Figure 17 shows a clear increase in computation time in the 3D tests. The largest expenditure of time is in the generation of the graph. The time for the path search takes thereby a negligible small portion. The cost of computation for the generation of a Quad tree -/Octree graphs is 3 times in the 2D space and around 7 times in the 3D space more than with the visibility graph. This is justified by the necessary minimum sector size (1m x 1m x 6m) and the associated time-consuming generation of the data structures for the 2-d and/or 3-d-trees [17] and the graph. Only by such a fine discretisation can "free" sectors between the obstacles be generated. The employment of Microsoft compilers reduced the computing time by around 35%. For the project the visibility graph was preferred. This is justified by its smaller computation cost and the form of the determined paths (compare Figure 13 and Figure 14). Fewer course changes during navigation through obstacles take occur with the visibility graph than with the octree graph.
IV. CONCLUSIONS In this article a concept for the guidance of an underwater vehicle in obstacle avoidance situations was presented. This concept was developed for the AUV "DeepC". The special characteristics of the vehicle (existing arithmetic performance and sensor technology, driving characteristic and manoeuvrability) formed the main criteria for the selection and development of the algorithms.
REFERENCES [1] http://www.deepc-auv.de/deepc/DeepC.htm [2] Förderprojekt des BMBF: DeepC. Aktiv autonomes Unterwasserfahrzeug für große Tauchtiefen. Förderkennzeichen: 03SX104E. [3] J. Guldner, “Lokale Kollisionsvermeidung für mobile Roboter mittels künstlicher harmonischer Dipol-Potentiale,” at - Automatisierungstechnik, Volume 45, Issue 01, pp. 24-35 1997. [4] J. Guldner, Intelligentes hierarchisches Regelungskonzept für autonome mobile Robotersysteme. Düsseldorf, Germany: VDI-Verlag, 1995 [5] K. Watanabe, M.M.A. Hashem and K.Izumi, “Global Path Planning of Mobile Robots as an Evolutionary Control Problem,” European Control Conference, Karlsruhe, Germany, 1999 [6] E.W. Dijkstra, “A Note on Two Problems in Connexion with Graphs“,Numerische Mathematik, 1, pp.269-271, 1959 [7] N.J. Nilsson, Problem-Solving Methods in Artificial Intelligence, McGraw-Hill, 1971 [8] A. Stentz, “The focussed D* Algorithm for Real-Time Replanning” in Proceedings of the International Joint Conference on Artificial Intelligence, Montreal, Canada, August 1995. [9] P.J. Schneider and D.H. Eberly, D.H., Geometric Tools for Computer Graphics. San Fancisco, USA: Morgan Kaufmann, 2003 [10] A. E. Bryson and Y.-C. Ho, Applied Optimal Control. USA: Ginn and Company, 1969 [11] G. Dudek and M. Jenkin, Computational principles of mobile robotics. New York, USA : Cambridge University Press,2000 [12] http://www.sbs.com [13] J.G. Siek, L.-Q. Lee, and A. Lumsdaine, The Boost graph library: user guide and reference manual. New York, USA: Addison Wesley, 2002 [14] http://www.boost.org [15] http://www.borland.com [16] http://msdn.microsoft.com/visualc [17] R. Klein, Algorithmische Geometrie. Bonn, Germany: AddisonWesley-Longman, 1997
