2006.12.27 Collision Avoidance and Escape JAR...

Submitted Journal of Autonomous Robots. 22 July 2006

Design and Evaluation of a Reactive and Deliberative Collision Avoidance and

Escape Architecture For Autonomous Robots

Jonathan Evans, Pedro Patrón, Ben Smith and David M. Lane

Ocean Systems Laboratory Heriot-Watt University, Edinburgh, UK

Email: [email protected], P. [email protected], [email protected], [email protected]

Web: http://www.ece.eps.hw.ac.uk/oceans

Abstract We present the design and evaluation of an architecture for collision avoidance and escape of mobile autonomous robots operating in unstructured environments. The approach mixes both reactive and deliberative components. This provides the vehicle’s behaviour designers with an explicit means to design-in avoidance strategies that match system requirements in concepts of operations and for robot certification. The now traditional three layer architecture is extended to include a fourth Scenario layer, where scripts describing specific responses are selected and parameterised on the fly. A local map is maintained using available sensor data, and adjacent objects are combined as they are observed. This has been observed to create safer trajectories. Objects have persistence and fade if not re-observed over time. In common with behaviour based approaches, a reactive layer is maintained containing pre-defined knee jerk responses for extreme situations. The reactive layer can inhibit outputs from above. Path planning of updated goal point outputs from the Scenario layer is performed using a fast marching method made more efficient through lifelong planning techniques. The architecture is applied to applications with Autonomous Underwater Vehicles. Both simulated and open water tests are carried out to establish the performance and usefulness of the approach.

Keywords: Obstacle avoidance, robot architecture, deliberative, reactive, planning, autonomous underwater vehicle, unstructured

environments

I INTRODUCTION A. Motivation and Research Challenge In a-priori unknown unstructured environments, robot understanding of its environment is in permanent change. Noisy sensors, partial/local coverage, incomplete a-priori information and moving objects with unknown intentions or presence are the principle causes. In some scenarios, such as underwater or hostile environments, recourse to a loop with the operator via communication links is not always possible. An autonomous response to the unexpected is therefore required, capable of adapting trajectory, waypoints, goals or even mission plan while on mission. Collision avoidance and escape is a key capability for this autonomous adaptation in robot navigation. Traditional approaches to collision avoidance and escape are purely reactive and usually involve an algorithmic formulation of response dictated by the objects’ geometries and locations and the relative location of the vehicle. In general they are not closely linked to the mission objectives or user requirements and do not take account of the architecture within which they operate. Additionally, certification issues are a practical concern for industrial or consumer use of autonomous systems. To be certified safe they must exhibit well defined event responses in all conditions. For collision avoidance and escape, this necessitates that the designer of the robot’s behaviour has the ability to specify manoeuvres defined from a set of user requirements or concept of operations in given circumstances. This is not possible with the current reactive systems. In this work we pursue development of a collision avoidance and escape approach where responses can be defined by the designer in the context of the mission domain taking into account the scenarios in which the vehicle will operate. We do this by incorporating a deliberative component into the overall system architecture that can adapt in real time and be sufficiently responsive, without causing a complete re-plan of the vehicle mission. Programming these deliberative behaviours provides the defined and bounded event responses we require, linked to geometric methods of path planning.

Evans, Patrón, Smith, Lane: Reactive and Deliberative Collision Avoidance and Escape Architecture

July 2006 2

For illustration we focus on the domain of autonomous underwater vehicles (AUVs) now used routinely in applications such as offshore survey/inspection, marine science oceanographic measurements and military mine countermeasures. However, the approach is more generally applicable. System tests performing in some of these underwater vehicle operations in simulation and open water have been carried out. Results are described showing the usefulness of the approach. In this section we briefly review relevant prior art. Section II presents our approach to system architecture, and sections III, IV and V consider the Sensor, Scenario and Reactive layers which provide the embodiment of the approach. Section VI present simulation and in water real-world test results working on the domain of Autonomous Underwater Vehicles, and section VII concludes. B Prior Art A large literature of robot motion planning exists, focused primarily on algorithmic methods for trajectory specification based on obstacle data and goal points [1-2]. Such algorithms have previously been classified as global or local. The former use knowledge of the entire known robot world, can be computationally complex, and assumes nothing changes during the robot motion. It is not well suited to dynamic and unstructured environments. Local planning, on the other hand is flexible and reactive, looking only a relatively short distance in front of the sensor. It is better suited to dynamically changing environments. Numerous methods have been espoused including artificial potential fields [3], fast marching [4-7], constraint based approaches [8-10] and many more. Real applications had appear in the past for slow-motion space rover domains [29], in which some kind of mission factor weights have been added to the traverse cost function minimisation equation [30]. Whilst technically sound, on their own they provide limited opportunity for the vehicle’s behaviour designer (i.e. its personality coach) to explicitly ensure the vehicle’s observed behaviour stays in step with operational requirements. Few if any of these algorithmic methods consider architecture and its implications. Within the classic functional architecture [11], the obstacle avoidance function is devolved to very low level modules and works in isolation. Behavioural approaches [33][12][13] are the exception. Here, layered simple behaviours are employed, and connected such that lower levels, can inhibit higher levels. New and unexpected properties are expected to emerge. However, these do not scale well to more complex situations, and are difficult to specify and control [31] [32]. Obstacle avoidance for underwater vehicles remains in its infancy. Exemplars of earlier work are in [4-6, 8-10, 14-17.] Although COTS obstacle avoidance sonars are now available [18] they primarily provide detection, and interface to the host vehicle directly where others make use of the data. .


July 2006 3

Fig 1: Four Level Architecture Incorporating Deliberative Scenario Layer for Real-Rime Escape Planning

II ARCHITECTURE OVERVIEW Autonomous robot design requires a clear definition of architecture within which sensor data, a priori information, mission plans, world knowledge, navigation/control, communications and user interaction functions exist and interact. From early work [11] there has been general convergence on a three level approach. However, to achieve the additional reactive deliberation that certification requires, we here revert to a four level model (fig 1). In addition to the normal sensor, reactive and planning (mode) layers we introduce a further layer called Scenario which contains fast reacting deliberative scripts. Following the subsumption approach [12], we empower the reactive layer to take over from the scenario layer in the event of extreme emergency, thus safeguarding the vehicle. We focus on the design of the sensor, scenario and reactive layers, as these contain the principle elements of our collision avoidance and escape approach.

III. THE SENSOR LAYER The primary objective of the Sensor layer is the generation of a local map. The output from this map provides an input for the algorithms in the scenario layer that tries to match the arrangement of targets within the map against known scenarios. Figure 2 illustrates the layers internal workings

• The data flow in this layer starts with the raw data coming from the different sensors. One module for each sensor processes received data to generate an abstract list of objects detected with its properties. Several techniques of image and signal processing are utilised inside these modules, depending on the sensor type [19-26].

Mode Layer Mission Requirements World Modelling

Local Modelling Sensor Prediction Sensor Processing

Scenario Deliberator Inhibition

Safety Deliberator

Emergency (Reactive Control)

Sensory Input

Actuators

DB Info

Fuzzy MF, rules

Sensor Layer

Scenario Layer

Reactive Layer

Autopilot

The World

Inhibition

Vehicle Systems

Waypoints

requests

Control Requests

Problem Position


July 2006 4

SENSORLAYER

SCENARIOLAYER

MISSION REQUIREMENTS

NAVIGATIONSYSTEM

WORLDMODELLING

SCENARIODELIBERATOR

Data FusionLocal MapGenerator

Data processingSensor 1

Data processingSensor N

SENSOR

Sensor 1

Sensor N

raw data

raw data

detectedobjects

Sensor 1

detectedobjects

Sensor NREACTIVELAYER

time

vehicleposition

Reactive LayerReport Message

ScrollingLocal Map

Time t

ObjectDB

previousdetectedobjects

Sensor LayerReport Message

clusterobjects 1

clusterobjects

M

LocalModelling

SensorSensor

Prediction

Scenario LayerReport Message

Figure 2: Internal mechanism of the sensor layer, this handles things like Local Modelling, Sensor Prediction and Sensor Processing through Map Generation, Data Fusion and Data Processing systems.

• With these lists of detected objects, and the information of the previous detected objects stored

in a database in the world modelling module, data fusion takes place to detect clusters of objects. Merging of objects based on their common properties (depth, distance, alignment, trajectory, etc.) generate these clusters.

• Mission Requirements, flowing down from the higher-level planner systems, are used to adjust biases and select different algorithms.

• Finally, a module reads these clusters and the current properties of the vehicle from the navigation system to generate a local detailed map that scrolls with time to keep the vehicle close to the centre.

• To inform the other layers about the actions that have taken place, a report message is sent by this module through the common communication protocol. Similarly, the sensor layer modules can receive and modify their behaviour based on the output and status of the other layers as necessary.

IV. THE SCENARIO LAYER

To provide deliberation in the generation of obstacle avoidance and escape behaviours, we employ user programmable scripts called Scenarios. They are selected and applied to obstacle information to generate inputs to a path planning system, which then generates a new trajectory. A Scenarios, Scenes, Objects and Deliberation A Scenario Deliberation module (fig 3) processes objects identified within the sensor data, clustering objects where necessary, and searches its database of known scenarios for similar situations. If a match is found, it parameterises and executes the associated behaviour, outputing a new sequence of goals to the Path Planner Objects are represented as cubes defined by two vectors for size and position. Two additional vectors define the bounding box of its maximum and minimum dimension (for combining objects and


July 2006 5

improving collision search with non-cuboid object groups). A collection of such map-objects forms a Scene, and is hierarchically organised into groups thus speeding complex queries and group reduction techniques. Initial pre-processing of objects uses three conditions:

• If the scene is currently empty, just add the new object. • If the object fits completely inside another object that is active, it is discarded. • If neither of the above conditions hold true, perform a distance comparison between both

the vector holding the size and position of each object. To perform a comparison between two objects, the Euclidian distance between the respective vectors δc and δn is computed. This distance is then mapped into a Gaussian function and the sigma width value is used to control the likeness of two objects. Objects that have a very close fit will give a high δ and are classified as the same, As the distances from the respective vectors increases the δ value will fall off steeply.

nc

oo

oo

enc

≡⇒≥=

−−

κδδ σ

vv

(1)

After computing δ , it is possible to say that the two objects Oc and On are equivalent provided the likeness is above some threshold κ. Subsequent post processing of objects uses programmable scripts to examine and manipulate the Scene Scripts are pre-compiled and stored on an execution stack (Script Stack) Cycling though this execution stack performs calls to each of the scripts which in turn modify the Scene. Objects are classified as left, right, above or below the vehicle. Queries extracts from the Scene objects greater than, less than or equal to some factor of the vehicle along an axis. Objects are then grouped using:

• Close group is used to compute the box that encloses all objects in a group, and to replace them.

• Union of two groups is used to find all the items that are in both the first and second group, the result is then placed in additional group.

• Intersection of two groups is used to find all the items that are not in both the first and second group, the result is then placed in an additional group.

• Expand group is used to expand all items in group by a given factor of the vehicle size along a specified axis.


July 2006 6

VEHICLECONTROLSYSTEM

SENSORLAYER

SCENARIOLAYER


NAVIGATIONSYSTEM

LOCAL MAPGENERATOR

PathGenerator

Waypoint Generator

ScenarioDeliberator

TargetSelection

MISSIONPLANNER

Scenario+

MapOAS Target

Local Map

vehicleposition

Scenario LayerReport Message

ScenarioDB

path(Vehicle position, OAS Target)

Scenarioset ofpossible

scenarios

WPRequest

Sensor LayerReport Message

WP

WORLDMODELLING

ObjectDB

Figure 3: Internal working of the scenario system. The key feature of this system is the translation of sensor layer information

into waypoint requests through intelligent reasoning. Scenarios contain appropriate avoidance, escape and collision strategies according to object content and geometries and requirements/concepts of operations for the vehicle. Ultimately, they pass parameterised behaviours to the Path Generator to generate waypoints that can be applied to the vehicle autopilot. Scenarios are selected based on priority policies which are themselves based on the three inputs to the Scenario Deliberator (World Modelling, Mission Requirements, and Local Map from the Sensor Layer).

For illustration, Table 1 shows a classification of different objects identified for specific classes of mission for typical AUV missions. For each object, the most efficient / practical strategy was identified under each of the headings of “obstacle avoidance”, “escape” and collision avoidance” categories. Object Avoidance

(Normal Deliberative) Escape

(Safety Deliberative) Collision

(Emergency Reactive) Nets (open, suspended roughly vertically)

(1) If extent detected, then Horizontal Diversion

(2) If extent unknown, then Horizontal Diversion to parallel course, and map extent

(1) Impending Entrapment: Max deceleration to reverse followed by recovery trajectory

(2) Trapped: Period of no thrust (opportunity for net’s tension to relax and free vehicle), following by very slow reverse

(1) Full Reverse to clear danger

Dense marine vegetation (e.g. kelp forest)



(1) Impending Entrapment: Max yaw (slowing if appropriate), followed by recovery trajectory

(2) Trapped: Slow speed reverse, following previous incoming course

(1) Full Reverse to clear danger

Debris (e.g. fully observed, compact targets on seabed) Mines (on seabed)

(1) Upward diversion to maintain appropriate safety altitude from

(1) Impending Entrapment: Max ascent & max yaw (slowing if appropriate), followed

(1) Steep Ascent and full Yaw to clear danger


July 2006 7

target by recovery trajectory

(2) Trapped: N/A

Mines (mid-water, tethered to anchor) Cables & Risers (vertical pipelines) Rope & Fibre (vertical, full depth)

(1) Horizontal Diversion with appropriate safety factor around identified target

(1) Impending Entrapment: Max deceleration to reverse followed by recovery trajectory


(1) Full Yaw to clear danger

Buoys (anchored) Rigs & Platforms

(1) Horizontal Diversion with appropriate safety factor around identified target

(1) Impending Entrapment: Max yaw (slowing if appropriate), followed by recovery trajectory

(2) Trapped: N/A


General Seabed Pipelines (on seabed)

[1] Upward Diversion to maintain appropriate safety altitude from target

(1) Impending Entrapment: Max ascent (slowing if appropriate), followed by recovery trajectory

(2) Trapped: N/A

(1) Steep Ascent to clear danger

Harbour Structures (e.g. Dock walls, piles, gates) Beach Structures (Civil & Military)



(1) Impending Entrapment: Max ascent (if stealth mode allows) & max yaw followed by recovery trajectory


(1) Steep Ascent (if stealth mode allows) and full Yaw to clear danger

Known obstructions (pre-mission defined)

(1) Planned Horizontal and/or Vertical Diversion to avoid obstruction

1. N/A N/A

Ships & Surface Contacts (floating)

(1) Downward and Horizontal Diversion around target

(1) Impending Entrapment: Max descent & max yaw followed by recovery trajectory

(2) Trapped: N/A

(1) Steep Descent and full Yaw to clear danger

Submerged contacts (e.g. other AUVs, submarines)

(1) Horizontal Diversion (following general maritime rules if appropriate Error! Reference source not found.) with safety factor around identified target

(1) Impending Entrapment: Max descent & max yaw followed by recovery trajectory

(2) Trapped: N/A


Table 1: Classification of the Identified Objects under the Collision, Obstacle Avoidance and Escape Headings.

B. Path Generation The path generator takes parameterised scripts defining the strategy to be executed along with map object information and vehicle position and tries to find a path between the vehicle position and the target. The path is checked and waypoints generated then sent to the vehicle control system. Whilst many methods could be used, we have recently been developing methods building on the technique of Fast Marching (FM) and Lifelong Planning (LP) graph search. Fast Marching [4-7] is a global motion planning approach that searches forward from a given robot start position towards a goal point, expanding locations in space as points in a grid at a defined resolution. The first derivative of the cost function is used to generate a contour plot for the locale moving forward like a wave. This is then traversed backwards from the goal point to the start position


July 2006 8

using steepest descent to select the path. In this way classical problems with local minima are avoided. Figure 4 illustrates two examples for Euclidean (4 way) and Manhattan (continuous) motion models and the cost function

1

1

( ) [ ]n

p pip i

i

L x x x x/

=

′, = | − |′∑ (2)

for any ρ ≥ 1

`

Fig 4: Fast Marching gradients and steepest descent trajectories for Manhattan and Euclidean motion models In our approach, the gradient is calculated in continuous space, by approximating the first derivative of the cost function. The approach is therefore independent of grid resolution and hence more precise. The minimum cumulative cost at x can be defined as:

0( ) min ( ( ))

Ax

L

CU x C s dsτ= ,∫ (3)

where CAx is the set of all paths linking A and x, and τ is the cumulative travel cost from a starting point to a destination. The path length is L and the starting and ending points are C(O)=A and C(L)=x respectively. The path that gives the minimum integral is the minimum-cost path. The solution of Equation 3 satisfies the Eikonal equation

τ=∇U (4)

1 2

2 21 2 1 2( ) min( )AB A B C

t tu C t u t u t t τ

,= + + + ⋅ ,

1 2 1 2s t 1and 0t t t t. . + = , > (5)

We have extended this basic FM to include heuristics to direct the search (c.f. A* search). We have further applied incremental search methods from lifelong planning [27] and reuse information from previous searches. Such incremental methods are potentially faster than solving each search problem from scratch. This is important in the underwater path planning application since the system may have to adapt its plans continuously to the changing/evolving knowledge of the world. This new approach we have called Lifelong Planning Fast Marching (LPFM*). Based on the Lifelong planning scheme, it calculates the min action function by using fast marching with heuristic and 8-connectivity. When new objects are mapped into the local map, the method does not have to recalculate the whole map from scratch at each iteration. It re-uses previous iterations to recalculate only in the areas where the map has been modified. Further details are in [4-6]. To initially demonstrate these improvements two synthetic environments are used to represent the sampled grid. Cobs (black) and Cfree (white). The local environment at time t is represented in image A and at time t+1 in image B (figure 5). Image size are 200x200 pixels. The start point is in the top left corner and the goal is located at the bottom right corner. Several objects with different shapes are represented in the environment. The problem of possible local minima occurs several times. Two objects are modified in the second grid with-respect-to the first: the object in the left lower border is increased in size while a narrow passage is open in the middle of the grid at the end of the snail-shaped entrapment. A new alternative path is found at time t+1. Table 2 compares results of fast marching for


July 2006 9

4-connected motion with fast marching with lifelong planning with 8 connected motion in both environments. The larger number of nodes makes the latter initially slower to compute. However, in subsequent computation the reduced number of nodes make it significantly faster.

.

Figure 5 Path planning results at time t and t+1 using Lifelong Planning Fast Marching (LPFM*) in an 8-connected sampled grid with

∞L metric as heuristic

Method Time (ms) Explored Nodes Nodes in Path Minimum Cost

Image A 198.912 21,109 1,121 577.32 Fast Marching 4- Connected Grid Image B 176.082 18,582 836 451.23

Image A 456.097 14,954 915 518.03 Lifeling Planning Fast Marching 8 Connected Grid Image B 59.326 855 1,819 593.26

Table 2: Path Planning Methods. Metrics and Comparison

V. THE REACTIVE LAYER

A. Analysis Methods for performing reactive control exist in two main flavours, there are numerical systems that encode information directly as numbers or numerical relationships and word based systems, like expert and fuzzy systems, which use linguistic values for the numerical representation [35]. Expert systems have poor mechanisms for dealing with differences between the same measurements, as precise matching is required. This makes them too rigid for our requirements in system maintenance and adaptation. For the same reason, knowledge in neural networks is embedded inside the set of synaptic weights, in a complexity that is not possible to discretely extract or update [34]. The design overhead associated with producing a system that has this capability is very large. In potential fields [36], vector summation is used to combine behaviours to produce a global emergent behaviour. This sometimes produces unforeseen conditions in the real world that leads to poor tolerance to uncertainty and imprecision. In a same way, subsumption systems tolerance to uncertainty is quite bad due to the design overhead associated with producing effective behaviours and binary transition between states [37]. However, unable to cope with continually changing measurements, the execution speed is very good due to a tight coupling between sensing and acting. Fuzzy systems are similar to expert systems in that they can represent knowledge as a set of IF-THEN type rules. This gives a good explanation ability and knowledge representation. Where fuzzy systems differ is in their attempt to mimic human thinking to reason in an approximate way rather than a precise way through use of words and sentences. This is particularly useful at dealing with uncertainty in real world environments where measurements of the world can be noisy and include errors. Production of fuzzy systems relies on information from domain experts and their relative simplicity makes them quite easy to maintain. Adaptability is fairly bad in terms of moving the system out of its intended environment, however adaptability within the intended environment is much better. For example in this system we may require stealth in certain areas, fuzzy systems can be adapted to use these stealth rules by applying a modifier to sets of pre-programmed rules. The design overhead can be kept low as long as the behaviours are simple. B. Fuzzy System Design


July 2006 10

The architecture of the reactive layer (Figure 5), follows the common sense-act paradigm found in simple obstacle avoidance systems:

• Sensor inputs enter on the left hand side; these undergo some minor processing in the Fast Processing and Data Fusion module. This is responsible for performing simple operations to try and extract some features from the data e.g. range to nearest object. This data can then be used to trigger the behaviours.

• This processed information is fed into each of the fuzzy behaviours. Each individual behaviour has the following features:

o A set of inputs, these can be any number of the processed pieces of sensor information. In addition current parameters can be used such as time or other variables within the system.

o A set of rules, these rules can be selected based on the Mission Requirements from the Fuzzy Rules DB.

• Each output from the fuzzy behaviours contributes some control request values to the coordination system. This coordination system effectively weights the behaviours in a fuzzy way according to their strength and priority to produce the final control request value that drives the system.

VEHICLECONTROLSYSTEM

REACTIVELAYER


NAVIGATIONSYSTEM

CoordinationSystem

Behaviour 1Fuzzy Controller

Behaviour NFuzzy Controller

currentparameters

ControlRequest

Reactive LayerReport Message

Fuzzy rulesDB

rules set foreach controller

SENSOR

Sensor 1

Sensor N

raw data

raw data

Fast Processingand

Data Fusion

Figure 6: Reactive System Architecture

Data input is initially fuzzified using a singleton fuzzifier for simplicity and ease of use. The inference engine uses Mamdani reasoning to generate outputs from combinations of if-then rules. The main advantage of Mamdani is its suitability for human input, which is key for a system with a manually designed set of rules that requires tuning.

1ℜ :if x is A1 and y is B1 then z is C1

also

2ℜ :if x is A2 and y is B2 then z is C2

fact: x is 0x and y is 0y

consequence: z is C Here x and y denote linguistic variables, for example “x is low” and “y is high”. The problem is then to

find the membership function of the consequence C from the rule-base { 1ℜ , 2ℜ } with the given facts

A and B. The number of required rules is dependent on the inputs to the system; which should be kept to a minimum. Adding more inputs results in an exponential increase in the number of rules needed to satisfy the system. The fuzzy implication is modelled by Mamdani’s minimum operator, the sentence


July 2006 11

connective is interpreted as OR-ing the propositions. The firing levels of the rules αi =1,2 are computed by

:)()(

),()(

02022

01011

yBxA

yBxA

∧=∧=

αα

The individual rule outputs are obtained by

))(()(

)),(()(

222

111

wCwC

wCwC

∧=′∧=′

αα

Then the overall system output is computed by OR-ing the individual rule outputs.

))(())(()()()( 221121 wCwCwCwCwC ∧∨∧=′∨′= αα

To transform back to crisp values a modified height defuzzifier is used. The computation for a crisp control action Zo is:

∑∑

=

== n

i i

n

i iicZ

1

10

α

α

Where αi denotes the firing level of the i-th rule, and ci is the output from this rule. For illustration, we consider two AUV reactive behaviours – surfacing and avoidance. C, Surfacing Behaviour The surfacing behaviour makes use of a simple controller with a fuzzy mapping that increases the output based on approximate distance. With a high distance, slow precautionary movements are used, since information may be unreliable. As distance decreases the certainty increases to a point where full thrust needs to be applied. Figure gives the input and output membership functions along with rules.

Sensor Translational (Vertical) Velocity

low Fast medium medium high Slow


July 2006 12

Figure 7: The input and output membership functions with rules for the surfacing behaviour

Figure 8 shows the output surface for this behaviour. The velocity steeply increases to a maximum thrust value of 10 (100%) as the distance reduces, thus ensuring appropriate behaviour for a moving obstacle.

Figure 8: The output surface from translational velocity against sensor reading

D. Avoidance Behaviour

The avoidance behaviour makes use of two input sensor values that share the same membership function. In addition, outputs for rotational velocity (yaw) and translational velocity are used to specify how to perform the avoidance action (fig 9). This behaviour is designed to steer the vehicle away from an obstacle unless it is too close at which point a reverse action is required.

Sensor Sensor Translational Velocity

Rotational Velocity

low low reverse stop


July 2006 13

low medium stop right fast low high stop right slow medium low stop left fast medium medium medium stop medium high medium stop high low stop left fast high medium medium Stop high high fast stop

Fig 9: The input and output membership functions with rules for the avoidance behaviour

Fig 10 shows a surface plot for the translational velocity output. When the vehicle is a long distance from the obstacle, it moves at some nominal speed, as it gets closer, this speed decreases to allow the vehicle to turn away from the obstacle. If the vehicle gets too close, a fast reverse manoeuvre will be performed.

Figure 10: The surface plot for the two sensor inputs against the translational velocity output.

Fig 11 shows a surface plot of the rotational velocity output. When the vehicle is either very close or far away from the obstacle there is no rotation as the vehicle may have to move backwards, or if it is a sufficient distance no rotational adjustment is required. Depending on the location of the obstacle (if it is to the left or right of the vehicle) a counter direction is applied to attain a safe position.

Figure 11: The surface plot for the two sensor inputs against the rotational velocity output

VI SIMULATION AND FIELD TESTING To evaluate the performance of the architecture and algorithms therein, a comprehensive programme of simulation and open water evaluations have been carried out for autonomous underwater vehicle applications. Table 3 summarises the test cases and the results achieved. We here focus on four for illustration. Test #

Method / case Functionality tested Pass criteria Result

1 Net avoidance Three different starting points with different angles Object extension detection

Horizontal deviation for obstacle avoidance from different starting points and angles

Goal achieved Vehicle avoids net shaped obstacle and achieves goal point


July 2006 14

2 Harbour Horizontal deviation Escape from entrapment Reactive if necessary

Full tour of harbour structure

Vehicle followed all waypoints, avoid entrapments and get back to the original position

3 Beach structure Exclusion zones avoidance Sensed objects avoidance Re-planning when new objects detected

Zigzag movement between columns shaped objects without hitting them

Vehicle creates a zigzag manoeuvre while avoiding exclusion zones and sensed objects

4 Single rope Low sensor range Reactive horizontal diversion

Rope avoided and Goal achieved

Vehicle reacts and avoids rope when sensed.

5 Kelp. Three different executions with different safety factor and scenario manager parameters

Scenario manager clustering of objects Safety factor parameter

Kelp clusters avoided and Goal achieved

Vehicle avoid kelp clusters following three different paths

6 Single surface contact Reactive behaviour Surface contact avoided

Vehicle reacts submerging again when trying to surface due to a surface contact

7 Re-planning Two different executions with different safety factor

Re-planning Safety factor parameter Path planning real time recalculation

Goal achieved in both senses even if unexpected objects are obstructing the trajectory

Vehicle re-plans trajectory when new unexpected objects are sensed and achieves the goal in both directions

8 Full mission Horizontal deviation Exclusion zones avoidance Sensed objects avoidance Re-planning Scenario manager processing of objects Escape from entrapment Sensed object “memory” for re-planning

Target inside the harbour structure and goal for the end of the mission achieved.

Vehicle navigates inside the harbour avoiding the obstacles and structures and manages to escape and to complete the mission successfully.

Table 3: Autonomous Underwater Vehicle Experiment Summary

The symbology of Fig 12 is used in presenting results.

Fig 12: Result Symbology

A Single Surface Contact (Test #6)

Figure 13: Surface contact test

The first test exercises purely reactive obstacle avoidance for illustration, and in particular the Reactive Layer surfacing behaviour of section V. The vehicle is placed below the surface with a goal waypoint at the surface. A moving surface vessel appears during this manoeuvre and causes an obstruction.


July 2006 15

Surface

-1

0

1

2

3

4

5

6

7

8

9

-15-10-50510152025

North (m)

Dep

th (m

)

Output Trajectory Waypoints Surface Vessel Positions

Fig 14 Vehicle Output Trajectory

Here the vehicle starts at 1 and after moving some distance is required to surface. At the surface position 2 it makes an initial observation of some obstacle at 3. Multiple observations are subsequently made and as these observations become closer the vehicle dives until a final observation has been made at 4 and the obstacle is no longer visible. The vehicle continues to dive to a safe position then continues with another surface manoeuvre.

B. Kelp (Test #5.)

Figure 15: Kelp test. Left: Normal behaviour. Centre: Low safety behaviour. Right: Normal behaviour with scenario manager

This experiment tests the behaviour when several clusters of kelp are detected, and illustrates deliberative obstacle avoidance producing safer trajectories than simpler reactive responses. A low detection range is supposed. The goal is set so the vehicle is obstructed by two groups of large kelp fronds. The vehicle is sent in a diagonal line trajectory to a waypoint that is placed behind the kelp. The expected behaviours are that the reactive layer provides full yaw to clear danger if necessary, and the

1. Start Position

2. Surface Position 3. Initial observation

4. Final Observation

5. Safe Position

6. Resurface Position


July 2006 16

deliberative layer provides the horizontal diversion with an appropriate safety factor around the target. An important benefit of the scenario manager here is the association of data and the possible notion of unsensed objects in the scene. With the scenario manager (SM), kelp obstacles are joined up into a single obstacle, but without the SM they remain separate. As a result of this joining, the deliberative layer plots a safer trajectory…

Kelp High Saftey

-20

0

20

40

60

80

100

120

-20 0 20 40 60

E ast ( m)

Out put Trajectory

Waypo ints

Dynamic Obst acles

Out put TrajectoryScenario

Wapo int s Scenario

Figure 16:Alternative waypoints chosen by the scenarios manager in the deliberative layer override the waypoints generated by the reactive layer to realise a safer trajectory around the obstacle.

C Full Mission (Test #8)

Figure 17: Full Simulated Mission Test

In this test a full simulated mission is tested where the vehicle must penetrate inside a harbour and return. The shape of the harbour is known in advance and is pre-configured as exclusion zones for the mission planner. However, the harbour contains a group of kelp fronds lying on the side of the harbour dock, a set of columns from the support of the dock extension and two overlapping nets placed at the entrance.


July 2006 17

Figure 17 and 18 shows the execution of the mission. After reaching waypoint 1, en route to waypoint 2, sensors detect the two groups of kelp lying on the side of the harbour dock. Using the scenario manager and object grouping the system plans and executes a trajectory that reaches waypoint 2. Note there is further re-planning and course correction when the second group of kelp fronds are detected. Thereafter the pilings are successfully avoided, and the net targets grouped into two and avoided by deliberation. On the return trip, the deliberative layer still maintains the obstacle information and the exit route is planned without sensor data adding new information, and with more direct and efficient trajectories. Such is the success of the deliberative planning for this configuration, there is no necessity or opportunity for purely reactive planning to contribute. This has been a recurring theme in practice across a range of missions.

Mission

-300

-200

-100

0

100

200

300

400

500

-250 -200 -150 -100 -50 0 50 100

East (m)

No

rth

(m

)

Output Trajectory Waypoints Exclusion Zone Dynamic Obstacles Fig 18;Output trajectories for the harbour mission. Deliberative re-planning is so successful and safe that there is no need or

opportunity for reactive planning to intervene. D. In Water Trials A comprehensive set of in-water trials have been carried out to validate some of the science reported above. Trials were carried out in Portmore Loch Scotland and Vobster Quarry Somerset, UK using HWU RAUVER hover capable AUV (fig 19). The vehicle was equipped with an inexpensive Tritech Sea King mechanically scanning forward looking sonar for obstacle detection. Navigation used an integrated GPS and Doppler Velocity Log solution mixing absolute and dead reckoning modes. For reasons of space we here report net avoidance results (test #1) only.

Fig 19 RAUVER Autonomous Underwater Vehicle Figure 20: Net avoidance from different starting positions. In this test, a net structure is set in the middle of the scene. The vehicle is sent to a waypoint located in the other side of the net. The starting point of the mission is situated in different places and the scenario manager is switched on and off to observe the different reactions. The expected behaviour is for the


July 2006 18

deliberative layer to plan a parallel course until the extent is directed, then a horizontal diversion. If this fails, the reactive layer should reverse to clear danger. Simulation studies were initially carried out to check for correct behaviour. Three tests were executed at starting points A, B and C in figure 20. During the test A the deliberative system found an alternative path to reach the target avoiding the obstacle with a left horizontal diversion. In test B, the system, that was keeping track of the net extension, found a path on the left of the net and re-planned the mission to go back and do the left horizontal diversion. Test C demonstrated that by moving the starting point a little forward to the right the net extent is detected and the system is able to find a horizontal diversion on the right side of the net. In all cases, deliberative behaviours were always successful, without recourse to the reactive layer.

Ne t A Scenario M anager

-20

0

20

40

60

80

100

120

-20 0 20 40 60 80 100

East (m)

Nor

th (

m) Output Trajectory

Waypoint

Dynam ic Obstacles

Net B Scenario Manager

-20

0

20

40

60

80

100

120

-20 0 20 40 60 80 100

East (m)

No

rth

(m

) Output Trajectory

Waypoints

Dynamic Obstacles

Net C Scenario Manager

-20

0

20

40

60

80

100

120

-20 0 20 40 60 80 100

East (m)

No

rth

(m

) Output Trajectory

Waypoints

Dynamic Obstacles

Fig 21: Net avoidance behaviours for different starting positions

For practical evaluation, net avoidance was inserted as part of longer ISR (intelligence, surveillance and reconnaissance) mission in Vobster Quarry.. Fig 22 shows a layout of the Vobster trials location and the net detection in water mission profile. Red dots and lines show pre-planned mission goals and trajectories. Green lines show the as-swam trajectories, including avoidance behaviours.


July 2006 19

Fig 22: Vobster Quarry Somerset UK. The nets are located in grid boxes (2,4) and (3,4), and avoidance behaviours are clearly seen as the green trail deviates from the initial plan shown in red. For illustration, fig 23 shows screenshots from the forward looking sonar display, and snapshots of the corresponding local map constructed for the scenario manager in the deliberative layer. The quarry is relatively shallow in places, and the vertical beam-width of the scanning sonar picks up boundary reverberation returns from the quarry floor at longer ranges. In practice, these ghost clutter objects are then built into the local map and also used in making avoidance and escape decisions.

Fig 23: Tritech Forward Look Sonar Screenshots Showing Net and Bottom Returns, Scenario Layer Local Map is Top Right. Grey Level Represents Currency Of Map Data, White is Recently observed, grey some time ago., Eventually, grey objects

disappear when not re-observed for some time. Fig 24 shows typical avoidance behaviours in practice, overlayed onto the available obstacle information retained in the local map. As long as the nets are detected, avoidance proceeds without difficulty. However, the exact trajectory is dependent on clutter objects, which in turn depends on the orientation of the vehicle and the topography of the quarry floor. As clutter obstacles are approached, they are not re-observed (quarry floor falls out of the vertical beam-width of the sonar) and are eventually discarded. Depending on the scenarios designed, the system may already have chosen an alternative route, containing unnecessary transits and less efficient. However, careful tuning of the persistence of obstacles in the local map can prevent this. The trajectories achieved in practice were as per simulation when obstacle geometries were the same.


July 2006 20

Fig 24 Four sequential observations of the local map. Vehicle is moving right to left. Trail shows planned forward trajectory to achieve the goal point. Note obstacles on the right are not re-observed by the sonar as the vehicle moves away. Over time they therefore go from white (recently observed) through grey (not seen for a few scans) to black (not re-observed for a significant

time)

VII CONCLUSION We have designed, developed and tested a collision avoidance and escape approach that includes deliberative behaviour to guide an autonomous robot’s response to given situations. This allows the designer of a robot’s behaviour to meet needs laid down in concept of operations and certification requirements. Avoidance (normal deliberative), Escape (safety deliberative) and Collision (emergency reactive) behaviours have been implemented for AUV applications. Path planning to link the waypoints thus re-defined has used topical Fast Marching and Lifelong Planning methods. By inserting a Scenario layer into the conventional three level architecture, local maps generated in the Sensor layer are used to choose and parameterise appropriate avoidance behaviours on the fly. The Scenario layer takes these local maps generated in the Sensor layer and fuses individually detected adjacent objects using defined criteria. This has been demonstrated to produce safer trajectories, with an earlier onset of correct and more efficient avoidance behaviour. A Reactive layer has also been implemented and contributes by inhibiting goal points from the Scenario layer in extremis. It uses fuzzy behaviours to rapidly extricate the vehicle in dire emergency conditions. Surfacing and Avoidance behaviours have been described. In practice, the Scenario layer is rarely if ever inhibited by this Reactive layer, and results for the Reactive Layer experiments could only be realised by disabling the Scenario layer. Tests have been carried out in simulation and on a real AUV in open water. Whilst the experiments faithfully reproduced the simulation results, additional practical factors were introduced. Principle among these include segmentation of bottom reverberation at longer ranges into image data being detected and segmented into the local map. This appears as obstacle data and affects computed trajectories particularly at longer ranges.


July 2006 21

Although the system has been implemented and tested for an underwater platform, the architecture has been carefully designed to remain portable and for being environment and platform independent. The level of abstraction in the sensor layer also allows compatibility with a different variety of sensors. The system is also extendable. Some examples of this extendibility have been recently shown in parallel works, where trajectory constraints based on vehicle kinematics [6] and localisation uncertainties [28] are added to the system to improve its capability of adaptation within a changing environment. More extensions assessing changes in higher levels of plan abstraction, such as mission goals or resources, are planned for the future for letting the system continue to grow. Additionally tests in other platforms and environments are currently under analysis.

ACKNOWLEDGEMENTS This work was sponsored by the UK Ministry of Defence within the BAUUV programme; contract number WP220304 CCMM2 2004/5. Our thanks to Clement Pêtrès and Yan Pailhas in the Ocean Systems Laboratory for inputs on fast marching methods, and to all at SEA Ltd, Subsea7 Ltd, National Oceanographic Centre Southampton and DSTL who have provided helpful critique. Thanks also to all at SeeByte Ltd, Edinburgh for providing the necessary AUV, mobilisation, practical trials infrastructure and knowledge to run this architecture in the real world.

REFERENCES [1] J.C Latombe; “Robot Motion Planning”, Kluwer 1991 [2] IEEE International Conference on Robotics and Automation. Annual Conference. Motion Planning Sessions. [3] O. Khatib, "Real time obstacle avoidance for manipulators and mobile robots," Inr. J. Robotics Research, Vol. 5, No. 1,

Spring 1986, pp. 90-99. [4] C. Pêtrès, Y. Pailhas, J. Evans, Y. Petillot and D.M. Lane. “Underwater vehicle obstacle avoidance using fast marching

algorithms”. In Proceedings of Ocean 2005 Conference, Brest, France, June 21 2005 [5] C. Pêtrès, P. Patrón. “Path planning for unmanned underwater vehicles”, In 19th International Joint Conference on

Artificial Intelligence, Edinburgh, Scotland, July 2005 [6] C Pêtrès, Y, Pailhas, Y, Petillot, D.M. Lane “Path Planning for Autonomous Underwater Vehicles” IEEE Transactions

Robotics (Accepted for publication 2007) [7] J.A. Sethian “A fast marching level set method for monotonically advancing fronts”; Proc. Nat. Acad. Sci, 93(4), 1996. [8] Y.Wang, D.M. Lane "Subsea Vehicle Path Planning Using Nonlinear Programming and Constructive Solid Geometry"

IEE Proceedings Control Theory & Applications. Vol 144 No 2 March 1997 pp143-152Albus nist architecture [9] Yongji Wang, David M. Lane, Gavin J. Falconer “Two novel approaches for unmanned underwater vehicle path planning:

constrained optimisation and semi-infinite constrained optimisation” Robotica (2000) Vol 18 pp123-142 [10] Y. Wang, D.M. Lane, "Robot Motion Planning Using Constrained Optimisation with AND/OR Relationships and

Constructive Solid Geometry" IEEE Transactions on Systems Man & Cybernetics, Volume 30 No 4 November 2000 pp525-536

[11] J.S. Albus, H.G. McCain, and R. Lumia, “NASA/NBS Standard Reference Model for Telerobot Control System Architecture (Nasrem),” Tech. Report 1,235, Nat’l Inst. Standards and Technology, Gaithersburg, Md., 1989.

[12] R.A. Brooks; “A Robust Layered Control System for a Mobile Robots”, IEEE Transactions on Robotics and Automation Volume 1 (1), 1986, pp1-10

[13] D.M. Lyons, R.C. Arkin “Towards performance guarantees for emergent behavior” Proc. IEEE International Conference on Robotics and Automation, Vol. 4, Apr 26-May 1, 2004 Page(s):4153 - 4158

[14] A.W. Quinn, D.M. Lane "Local Motion Planning For Underwater Manipulators Using Distributed Search" IFAC Intern. Symposium on Robot Control SYROCO 94. Sept 19-21 1994, Capri, Italy

[15] Y. Petillot, I. Tena Ruiz, D.M.Lane “Underwater Vehicle Obstacle Avoidance and Path Planning Using a Multi-Beam Forward Looking Sonar” IEEE Journal Oceanic Engineering, Vol 26. No 2, April 2001. pp 240-251

[16] Zhang Rubo, Gu Guochang, Zhang Guoyin “AUV obstacle-avoidance based on information fusion of multi-sensors” IEEE International Conference on Intelligent Processing SystemsVolume 2, 28-31 Oct. 1997 Page(s):1381 - 1384 vol.2

[17] Lynn R. Fodrea “Obstacle Avoidance Control for the REMUS Autonomous Underwater Vehicle.” MSc Dissertation, Naval Postgraduate School. Monterey, Ca 2002. Report No. A163114

[18] E.O Belcher, W.L.J Fo.; W.H Hanot, “Dual-frequency acoustic camera: a candidate for an obstacle avoidance, gap-filler, and identification sensor for untethered underwater vehicles” Proc Oceans 2002 MTS/IEEE Volume 4, 29-31 Oct. 2002 Page(s):2124 - 2128 vol.4

[19] D.M. Lane, J.P. Stoner "Automated Sonar Interpretation Using Qualitative Feature Matching" IEEE Journal Oceanic Engineering, Vol 19 No 3 1994.

[20] D.M. Lane, D.Y. Dai, M.J. Chantler "Robust Tracking of Multiple Objects In Sector Scan Sonar Image Sequences Using Optical Flow Motion Estimation" IEEE Journal of Oceanic Engineering. Vol 23 No 1, January 1998

[21] I. Tena Ruiz, D.M.Lane, M.J.Chantler, “Robustness Investigation of Object Classification in Sector Scan Sonar Image Sequences Using Inter-Frame Features” IEEE Journal Oceanic Engineering, v24 no 4 pp 458-469 October 1999

[22] E. Trucco, Y. Petillot, I. Tena Ruiz, C. Plakas, D. M. Lane "Feature Tracking in Video and Sonar Subsea Sequences, With Applications" Computer Vision and Image Understanding 79, pp92-122, 2000

[23] K Lebart, C Smith, E Trucco, D M Lane “Automatic indexing of underwater survey video: Algorithm and benchmarking method” IEEE Journal Of Oceanic Engineering, 28 (4): 673-686, Oct 2003

[24] I. Tena Ruiz, S. de Raucourt, Y. Petillot and D. M. Lane “Concurrent Mapping and Localisation Using Side-Scan Sonar” IEEE Journal Oceanic Engineering 29 (2): 442-456, April 2004

[25] S. Reed, Y. Petillot, J. Bell “An unsupervised approach to the detection and extraction of mine features in sidescan sonar” IEEE Journal Oceanic Engineering Vol 28 No 1 pp90-105 Jan 2003

[26] S. Reed, I. Ten Ruiz, C. Capus, Y. Petillot, “The Fusion of Large Scale Classified Side-Scan Sonar Image Mosaics” IEEE Transactions on Image Processing Volume 15, Issue 7, July 2006 Page(s):2049 - 2060

[27] S. Koenig, M. Likhachev and D. Furcy, “Lifelong planning A*”, Artificial Intelligence, 155(1-2):93-146, May 2004


July 2006 22

[28] P. Patrón and I. Tena Ruiz; “A smooth simultaneous localisation and mapping solution to improve situational awareness, mission planning and re-planning for AUVs”; World Maritime Technology Conference - Advances in Technology for Underwater Vehicles; March 2006

[29] J.P. Gonzalez, B. Nagy, and A. Stentz; “The Geometric Path Planner for Navigating Unmanned Vehicles in Dynamic Environments”; Proceedings ANS 1st Joint Emergency Preparedness and Response and Robotic and Remote Systems, February, 2006

[30] P. Tompkins, A. Stentz, and D. Wettergreen; “Mission-level path planning and re-planning for rover exploration”; Robotics and Autonomous Systems, Intelligent Autonomous Systems, Vol. 54, No. 2, February, 2006, pp. 174 – 183

[31] G. Schöner, M. Dose and C. Engels; “Dynamics of behaviour: theory and applications for autonomous robot architectures”; Robotics and Autonomous Systems 16 (1995) 213-245.

[32] P. Pirjanian, M. Matarie: “Multi-Robot Target Acquisition using Multiple Objective Behavior Coordination”. ICRA 2000: 2696-2702

[33] R. C. Arkin; “Behavior-based Robotics”; 1998; MIT Press [34] Y. Hayashi, J.J. Buckley and E. Czogala, "Fuzzy expert systems versus neural networks"; IJCNN., International Joint

Conference on Neural Networks, Vol.2, pp.720-725, June 1992 [35] L.A. Zadeh; "Fuzzy logic = computing with words"; IEEE Transactions on Fuzzy Systems, Vol.4, No.2 , pp. 103-111,

May 1996 [36] Y. Koren, and J. Borenstein,; “Potential Field Methods and Their Inherent Limitations for Mobile Robot Navigation”;

Proceedings of the IEEE Conference on Robotics and Automation, Vol. 2, pp. 1398-1404, April 1991 [37] S.G. Goodridge, M.G. Kay and R.C. Luo; “Multilayered fuzzy behavior fusion for real-time reactive control of systems

with multiple sensors”; IEEE Transactions on Industrial Electronics, Vol. 43, No. 3, June 1996, pp. 387-394

2006.12.27 Collision Avoidance and Escape JAR...

Documents

Transcript of 2006.12.27 Collision Avoidance and Escape JAR...