ManageAcousticPing & AcousticPingPlanner Behaviours
In order for a formation control behaviour to function, it must be able to generate a target position for its vehicle such that the vehicle is directed to a desired point in the formation. To do so, acoustic pings containing information about the relative positions of neighbouring vehicles must be received and filtered (since bearing/range measurements have noise). This information is what allows a formation control behaviour to competently plan a target point for its vehicle, and the process of handling acoustic pings is performed by the following two behaviours - ManageAcousticPing and AcousticPingPlanner.
3.3.1 ManageAcousticPing Behaviour
The purpose of the ManageAcousticPing behaviour is simple - upon reception of a range and bearing measurement, this behaviour filters the measurement, converts it to Cartesian coordinates, and stores this point in memory as a neighbour position for use by inheriting behaviours. Table 3.2 lists behaviour-specific parameters which the user can specify, along with an explanatory description of each. Unless otherwise specified, the default values listed are those used during simulation and testing of my behaviours.
Algorithm 3 lists the pseudo-code for the two primary functions of the ManageAcous- ticPing behaviour which allow it to update neighbour positions from received pings. When a formation control behaviour of a vehicle receives a ping, it obtains a bearing measurement, a globally unique vehicle ID, and a time difference. It can then call the updateN eighbourP os − ition(dt, θ) function listed in algorithm 3 in order to localize the vehicle’s neighbours. The basic process of this function is as follows - given the time difference (between transmission and reception of the ping) and the bearing of a ping, it first finds (in a storage dictionary) the corresponding vehicle which generated the ping (in our case this correspondence is per- formed explicitly by transmitting globally unique vehicle IDs, although for formation control behaviours that do not require IDs, this can be performed via a point-correspondence algo- rithm, thus reducing communication requirements); following this, it updates (or if a ping from that vehicle does not yet exist in the dictionary, adds) the position of the corresponding neighbour via a weighted moving average filter.
Filtering of the ping is performed using a weighted moving average filter, which operates as follows - when a new ping is received from a given neighbour, the f ilterP ing(ID, ping) function first retrieves a filter dictionary corresponding to that neighbour; it then adds a new entry to this dictionary with a key set to the current mission time, and the value set to the
Chapter 3. Distributed Formation Control Behaviours for AUVs
received ping (time difference and bearing pair); following this, it iterates over all the key- value pairs in this dictionary, subtracting the key (which is the time at which that value was added) from the current time, and removing that pair from the dictionary if this time differ- ence is greater than the user-specified filter time-out; the remaining values in the dictionary are weighted by a value inversely proportional to its age in the filter, summed, and averaged; finally, a polar to Cartesian conversion is applied to the range (where range is calculated as the time difference multiplied by the specified acoustic sound speed) and bearing, and this point is returned as the updated filtered position of the neighbour.
Range (actual, noisy, filtered) Measurements vs Mission Time
Mission Time (s) Range (m) 0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 20 40 60 80 100 120 140 160 Actual Range Noisy Range Filtered Range
Figure 3.5: Plot comparing the actual, noisy (measured), and filtered range measurements; to highlight the filtering effect, the noise added to the actual range is Gaussian with a standard deviation of 3m, and the filter time-out is set to 360s; pings occur every 30s.
This filtering approach does fairly well in removing the noise associated with bearing/range measurements, but, as expected, it adds a latency when comparing the actual position to the filtered position. This ’lag’ can be seen in figure 3.5, along with the desired ’smoothing’ effect. This associated latency results in inaccurate neighbour positioning primarily when either the neighbour or the vehicle is moving (the filtered position lags the actual position by a few meters). When both vehicles are still or drifting, then this filtering allows the vehicle to effectively remove noise from the measured positions of neighbouring vehicles.
Chapter 3. Distributed Formation Control Behaviours for AUVs
Algorithm 3 ManageAcousticPing update function on ping reception.
1: procedure ManageAcousticPing::updateNeighbourPosition(dt, θ) 2: ping neigbour ← ping(dt, θ)
3: ping neigbour key ← find corresponding source ID of ping neighbour . in simulation this is done via explicit passing of vehicle IDs, but in reality can be done using some form of point-correspondence
4: neighbour pos dictionary[ping neighbour key] ← call
f ilterP ing(ping neighbour key, ping neigbour)
5: end procedure
6:
7: procedure ManageAcousticPing::filterPing(ID, ping) 8: f iltered pos ← point(N one, N one)
9: accum weights ← 0
10: accum range ← 0
11: accum sin bearing ← 0
12: accum cos bearing ← 0
13: curr time ← get current mission time
14: ping f ilter ← ping f ilter dictionary[ID]
15: ping f ilter[curr time] ← ping
16: for key-value pair (ping time, ping value) in ping f ilter do
17: if (curr time − ping time) ≥ ping f ilter timeout then 18: ping f ilter.delete(ping time)
19: else
20: range ← ping value.dt × sound speed
21: bearing ← ping value.θ
22: weight ← 1 − ((curr time − ping time) ÷ ping f ilter timeout)
23: accum weights ← accum weights + weight
24: accum range ← accum range + (weight × range)
25: accum sin bearing ← accum sin bearing + (weight × sin(bearing))
26: accum cos bearing ← accum cos bearing + (weight × cos(bearing))
27: end if
28: end for
29: range f iltered ← accum range ÷ accum weights
30: bearing f iltered ← atan2(accum sin bearing, accum cos bearing)
31: f iltered pos ← point(x, y) from polar to Cartesian conversion of (range f iltered, bearing f iltered)
32: return (f iltered pos, curr time)
33: end procedure
34:
35: procedure ManageAcousticPing::getNeighbourPos() 36: return neighbour pos dictionary
37: end procedure
Chapter 3. Distributed Formation Control Behaviours for AUVs
Parameter Description Default
ping filter timeout Maximum ping age of the ping filter - the user can specify the maximum ping age of a weighted mov- ing average filter; pings greater than this age are re- moved from the filter. (s)
120.0
contact timeout Maximum ping age of a neighbour - pings from neigh- bours remain in memory until this age (this value is not used by ManageAcousticPing, but is available for inheriting behaviours for neighbour management). (s)
120.0
sound speed The acoustic sound speed. (m/s) 1500.0
display filtered contact Display a point in the pM arineV iewer visualizer of the filtered neighbour position.
false display unfiltered contact Display points in the pM arineV iewer visualizer for
all the neighbour positions in the filter.
false
Table 3.2: Parameters of the ManageAcousticPing behaviour.
3.3.2 AcousticPingPlanner Behaviour
Since some of my formation control behaviours require a user-specified formation, some way of providing this information to behaviours had to be implemented. This is the job of the AcousticPingPlanner behaviour - essentially this behaviour provides the user with a method of specifying the desired geometry of the formation, and provides inheriting behaviours with a data structure holding this user-specified geometry. Table 3.3 lists behaviour-specific param- eters which the user can specify, along with an explanatory description of each. As you can see, there is only one parameter, called node of f sets. This parameter should be specified by the user multiple times, with one value for each vehicle in the formation, using the following string format - ”name = vehiclename, x = x position, y = y position”. vehiclename speci- fies the globally unique vehicle ID (unused for some of my formation control behaviours), and x position and y position specify the global coordinates of the desired vehicle position in the formation. It is important that this collection of specified positions is consistent across all vehicles in the swarm (i.e. all vehicles share the same plan).
Parameter Description Default
node offsets A string allowing the user to specify the ID and position of a vehicle in a desired formation; this string must have the following format: ”name = vehiclename, x = x position, y = y position”
None
Table 3.3: Parameters of the AcousticPingPlanner behaviour.
Chapter 3. Distributed Formation Control Behaviours for AUVs
The AcousticPingPlanner behaviour does one thing - it reads in the specified node of f sets values, subtracts all other node of f sets values from the node of f sets value corresponding to the vehicle on which it is running, and stores this relative plan as a dictionary for use by inheriting behaviours. Pseudo-code for this is shown in algorithm 4.
The ManageAcousticPing and AcousticPingPlanner behaviours allow inheriting behaviours to access a dictionary of neighbour positions calculated from measured pings, and a dictio- nary of desired neighbour positions that are user-specified. A function is provided by both the ManageAcousticPing and AcousticPingPlanner behaviours in order to access these dic- tionaries - getN eighbourP os() and getN eighbourP lan(), as shown at the ends of algorithm 3 and algorithm 4 respectively.
Algorithm 4 AcousticPingPlanner function to read user-specified formation plan.
1: procedure AcousticPingPlanner::setFormationPlan()
2: read in all node of f sets and add to dictionary: neighbour pos plan[vehiclename] ← point(x position, y position)
3: for all points in neighbour pos plan, subtract point corresponding to ownship position
4: remove point corresponding to ownship position from neighbour pos plan
5: end procedure
6:
7: procedure AcousticPingPlanner::getNeighbourPlan() 8: return neighbour pos plan
9: end procedure