Sterowanie jednostką autonomiczną

Roman Śmierzchalski
Katedra Automatyki
Wydział Elektrotechniki i Automatyki
roman.smierzchalski@pg.edu.pl
 UNIKANIE KOLIZJI
WYKŁAD II

Roman Śmierzchalski 2
 Problem of a ship control

• dynamic positioning (DP)

Direction

Roman Śmierzchalski Position 3

Proportional performance of reasons of shipwrecks
(accidence at sea)
 Vessel’s Multi-Level Hierarchy Control
Structure

Strategy Level
Management Human Guidance
Operator System

Tactical Level Motion Controllers

Automatic and/or Remote
Control Control Allocation

Objects Level Actuators

Manual Control

Sensors &
Signal Processing Vessel, Environment
Marine Craft, USV
Roman Śmierzchalski 5
Strategy, Tactics
Data of Mission
(goal of destination, path ...)
Traffic at Sea

Strategy Electronic Chart

and Data Information
Weather Routing Systems (ECDIS),
Optimization of the Route
Statistic Data of Weather
and Actual Weather Report
Generating track commands
Way-Points
Followig track commands
Collision
Tactics Motion Vessel Control System
Course keeping Estimated avoid
Manoeuvring (trajectory tracking) positions and
Way point tracking velocities
Speed regulation
Wave motion damping

Navigation ARPA, AIS

Objects System
DGPS, Gyrocompass
Log, Sonar
Waves, Rudder
currents, Kinematics Dinamics
wind
Roman Śmierzchalski Truster Engine 6
 Guidance, control, navigation
Avoid
Collision
Way-Points Weather routing
Weather data Waves, currents
program Targets, lands
wind
Feed Forward Control

Autopilot Vessel DGPS, AIS,

Trajectory Control
Motion EICDS, ARPA, all
Generator Allocation
Controllers sensor
Sensors &
Signal Processing
Feed Back Control
Observer
Estimated positions and
velocities
Guidance System Control System Navigation System

Roman Śmierzchalski
I.Fossen 2002 7
Avoiding collision at the sea

Roman Śmierzchalski 12
Avoiding collision at the sea

Roman Śmierzchalski 13
Avoiding collision at the sea

Roman Śmierzchalski 14
Avoiding collision at the sea

Roman Śmierzchalski 15
Avoiding collision at the sea

Roman Śmierzchalski 16
Avoiding collision at the sea

Roman Śmierzchalski 17
 Automatyzacja Systemów
© Roman Śmierzchalski Okrętowych 19
 Automatyzacja Systemów
© Roman Śmierzchalski Okrętowych 20
 Automatyzacja Systemów
© Roman Śmierzchalski Okrętowych 21
Roman Śmierzchalski 22
 Automatyzacja Systemów
© Roman Śmierzchalski Okrętowych 23
 Automatyzacja Systemów
© Roman Śmierzchalski Okrętowych 24
Roman Śmierzchalski 25
Roman Śmierzchalski 26
Optimization of trajectory

Roman Śmierzchalski 27
Survey of works:

 determining the safe trajectory as a non-linear programming

Roman Śmierzchalski 28
Survey of works:

 the reduction of the solution space to the finite-dimensional

one by creating a so called digitized matrix of permissible
manoeuvres

Roman Śmierzchalski 29
Survey of works:

 the problem of avoiding collisions was formulated as the

multi-criterion optimisation task

Roman Śmierzchalski 30
Roman Śmierzchalski 31
Roman Śmierzchalski 32
Roman Śmierzchalski 33
Roman Śmierzchalski 34
Roman Śmierzchalski 35
Roman Śmierzchalski 36
Roman Śmierzchalski 37
 Control system

Roman Śmierzchalski 38
 Control system

ARPA Identification

Electrinic Chart

Roman Śmierzchalski 39
 Control system

ARPA Identification

Electrinic Chart

Planning safe trajectory

Mode „off line”

Roman Śmierzchalski 40
 Control system

ARPA Identification
Autopilot
Electrinic Chart

Planning safe trajectory

Mode „off line”

Ship tracking

Governer

Roman Śmierzchalski 41
 Control system

ARPA Identification
Autopilot
Electrinic Chart

Planning safe trajectory

Mode „off line” Mode „on line”

Ship tracking

Governer

Roman Śmierzchalski 42
Roman Śmierzchalski 43
Roman Śmierzchalski 44
Definition of environment and constraints

Roman Śmierzchalski 45
Situation of meeting

Roman Śmierzchalski 46
Ships’ domains

Domains of objects according to

a) Goodwin, b) Davis c) present work.

Creating hexagon around moving target

Roman Śmierzchalski 47
Population of trajectories

Roman Śmierzchalski 48
Population of „m”
individuals
individual 1 individual 2 individual m
x0, y0, V01 x0, y0, V02 .... .... x0, y0, V0m
....... ....... .... .... .......
xk1, yk1, Vk1 xk2, yk2, Vk2 .... .... xkm, ykm, Vkm
........ ........ .... .... ........
........ xn2, yn2, Vn2 .... .... ........
........ xe, ye .... .... xnm, ynm, Vnm
xn1, yn1, Vn1 .... .... xe, ye
xe, ye .... ....
co-ordinates (xi, yi), being the line segment si turning points of path passing

x0 y0 V0 xk yk Vk xn yn Vn xe, ye
Roman Śmierzchalski 49
Genetic operators

mutation soft mutation adding a gene swapping gene

crossing smoothing deleting a gene individual repair

Roman Śmierzchalski 50
Roman Śmierzchalski 51
Individual fitness function

• Total_Cost(S) =
Safe_Cost(S) + Econ_Cost(S)

• Safe_Cost(S) = wc*clear(S)

• Econ_Cost(S) = wd*dist(S)+
ws*smooth(S)+
wt*time(S)

Roman Śmierzchalski 52
 T=0

Evolutionary algorithm Input data

Creation of initial population of chmos.

T=T+1
Building of dynamic obstacles
Operator selection
Population evaluation
Selection of trajectory to modification

Creation offspring
no
Building of dynamic obstacles Terminatig
conditions
Population evaluation
yes
Introduction new individual
Best trajectory from population it is
Selection the best trajectory solution

Roman Śmierzchalski 53
Evolutionary_algorithm() 10 max_path_nodes, i.e., maximum number of path nodes in each path
{ 1000 max_generations, i.e., maximum number of generations
int T; /*T - generation40counter variable*/
population_size
10 display_size, i.e., the number of population dislayed on the screen
T = 0:
0.5 -1.0 : mutation1_rate[], i.e., Mutation 1 ->Probability
input_data_for_parametrising_operation_of_evolutionary_algorithm();
0.5 -1.0 : mutation2_rate[], i.e., Mutation 2 ->probability
input_data_for_defining_environment();
0.5 -1.0 : ins_del_rate[], i.e., Insertion ->probability
P(T) = creation_of_initial_chromosome_population();
0.5 -1.0 : swap_rate[], i.e., Swap ->probability
0.5 -1.0 : crossover_rate[], i.e., Crossover ->probability
/* chromosomes (individuals)
0.5 -1.0 are generated ini.e.,
: smooth_rate[], suchSmootha manner that they belong
->probability
to the unrestricted space0.5 X-1.0
. */ : deletion_rate[], i.e., Deletion ->probability
0.5 -1.0 /*
building_dynamic_obstacles(); : detour_rate[],
moving targets i.e., Detour ->probability
are modelled */
0.5 -1.0 : mutatSpeed[], i.e., MutatSpeed ->probability
population_evaluation ( P(T) );
/* each individual (path) is attributed
0.65 its fitness
node_mutation1_rate, i.e.,function, i.e. aitsnode
Prob. to mutate value for the
by mutation1
examined problem. Such 0.30
annode_mutation2_rate,
evaluation makesi.e., Prob. to mutate
it possible a node by mutation2
to recognise and make
0.60 node_deletion_rate_u, i.e., Prob. to delete a node of unfeasible path
a distinction between better and worse individuals
0.6 node_deletion_rate_f, i.e., Prob.in to the
deleteset.
a node of feasible path
The individuals are evaluated and ordered from the best to the worst, at the same
time being divided into 1.0
the distance_factor,
safe and unsafe 1.0 individuals */
1.0 smooth_factor,
while ( != terminating_conditions() )
1.0 clearance_factor 3.0
{
/* the operation of the 7.0 coefficient_a,
evolutionary i.e., coefficient
algorithm in computing
terminates clearance
when the cost
“stop” tests
are satisfied : certain number of generations,
10.0 clearance, the clearance the fitness
distance function accuracy */
of a path
4.0 min_path_segm_len, see planner.c for explanation
1.0 boundary_tolerance, see planner.c for explanation

0.0001 if the relative decreasing of path cost < 0.0001, terminate.

Evolutionary_algorithm()
{ 700.0 700.0 :environment x,y range
int T; /*T - generation counter variable*/
3 2 6 :obstacles static, obstacles dynamic, max
vertices
number of vertices static, x1, y1, x2.......
T = 0: 6
input_data_for_parametrising_operation_of_evolutionary_algorithm();
212.2 243.2 89.2 156.8
input_data_for_defining_environment();6.8 28.4 51.4 37.8
105.4 145.9 244.6 239.2
P(T) = creation_of_initial_chromosome_population();
3
/* chromosomes (individuals) are generated
650.0 in such
237.8 a manner
700.0 187.8 that they belong
to the unrestricted space X. */ 700.0 317.6
4
building_dynamic_obstacles(); /* moving targets
582.4 237.8 are modelled
437.8 0.0 */
population_evaluation ( P(T) ); 700.0 0.0 700.0 120.3
/* each individual (path) is attributed its fitness function, i.e. its value for the
143.2 550.0 677.0 168.9 :Start (x,y) Goal (x,y)
examined problem. Such an evaluation makes it possible to recognise and make
a distinction between better and worse 8.6 individuals in the set.
5.0 :speed_of_own_ship
number of vertices
The individuals are evaluated and ordered from the best dynamic,
to thex1, worst,
y1, x2........ speedsame
at the ,course
6
time being divided into the safe and unsafe
463.0 individuals
685.0 407.0 */694.0
while ( != terminating_conditions() ) 333.0 630.0 352.0 574.0
{ 389.0 574.0 463.0 639.0

/* the operation of the evolutionary algorithm terminates when the “stop” tests
10.6 225.0
are satisfied : certain number of generations,
6 the fitness function accuracy */
9.0 185.0 55.0 176.0
120.0 231.0 120.0 287.0
74.0 287.0 9.0 231.0

7.8 45.4
Evolutionary_algorithm()
{
int T; /*T - generation counter variable*/

T = 0:
input_data_for_parametrising_operation_of_evolutionary_algorithm();
input_data_for_defining_environment();
P(T) = creation_of_initial_chromosome_population();
/* chromosomes (individuals) are generated in such a manner that they belong
to the unrestricted space X. */
building_dynamic_obstacles(); /* moving targets are modelled */
population_evaluation ( P(T) );
/* each individual (path) is attributed its fitness function, i.e. its value for the
examined problem. Such an evaluation makes it possible to recognise and make
a distinction between better and worse individuals in the set.
The individuals are evaluated and ordered from the best to the worst, at the same
time being divided into the safe and unsafe individuals */
while ( != terminating_conditions() )
{
/* the operation of the evolutionary algorithm terminates when the “stop” tests
are satisfied : certain number of generations, the fitness function accuracy */
T = T + 1;
Op = operator_selection();
/*, a genetic operator Op is selected from the set of available operators for the
individual re-combination */
Par = selection_of_parents_or_parent();
/* This selection depends on the operator defined earlier in the step "operator_selection().
adds an individual, or individuals to the parent set in the next generation */
offspring_creation_and_ introduction( Op, Par );
/* an offspring is created basing on the information which operator Op has to
re-combine on the parent set Par. The newly generated offspring becomes a member
of a new population P(T) and replaces the worst individual in the old population.
This new offspring may be better then its parent or parents */
building_dynamic_obstacles();
/* moving targets are modelled for the new population P(T) */
population_evaluation ( P(T) );
/* evaluation of new individuals in population P(T) */
introduction_of_new_individual();
/* if a new individual is better that those existing in the population,
the worst individual is removed and the new one is introduced to the population */
selection_of_best_individual ( P(T) );
/* the best individual in population P(T) is selected */
}
}
T = T + 1;
Op = operator_selection();
********************** generation = 100 **********************
/*, a genetic operator Op is selected from thePaths
Feasible set of available
Unfeasible operators for the
Paths
individual re-combination */ about N generations:
statistics
# of paths per generation 0.000 40.000
Par = selection_of_parents_or_parent();
cost per path 0.000 245.080
/* This selection depends on the
# of nodes operator defined
per path 0.000 earlier in the step "operator_selection().
2.475
adds an individual, or individuals to the parent set in the next generation */
statistics about the last generation:
offspring_creation_and_ introduction( Op, Par ); 0.000
# of paths the last generation 40.000
/* an offspring is created basing
cost per path on the information0.000 which operator Op has to
74.906
re-combine on the parent set Par.
# of nodes The newly generated
per path 0.000 offspring
2.150 becomes a member
the best path has 3 nodes and its cost is 37.977 (unfeasible).
of a new population P(T) and replaces the worst individual in the old population.
This new offspring may be better then its parent or parents */
building_dynamic_obstacles();
Statistics about genetic operators (every 100 generations):
/* moving targets are total
modelled for the new population P(T) */
times the genetic operators applied:
population_evaluation (0.0 P(T)15.0); 21.0 6.0 10.0 0.0 18.0 10.0 20.0
/* evaluation of new individuals in population
The effectiveness P(T) */
of genetic operators:
0.00 0.40 0.24 0.00 0.20 0.00 0.28 1.00 0.00
introduction_of_new_individual();
/* if a new individual isTime
better that those
per operation (sec.):existing in the population,
the worst individual is 0.000
removed 0.021and the0.020
0.010 new one 0.021is 0.000
introduced to the0.019
0.013 0.010 population */
selection_of_best_individual ( P(T) );
Number of node changes per operation:
/* the best individual in population
0.000 0.000 1.333 P(T)0.000
is selected */ -1.556 2.200 0.000
-0.600 0.000
} # of nodes examined per operation (averaged over all operators): 4.79
Time per operation (averaged over all operators): 0.016 sec.

total times (accumulated) the genetic operators applied:

0.0 15.0 21.0 6.0 10.0 0.0 18.0 10.0 20.0

Testing basic collision
situations:

one and two

moving targets

Roman Śmierzchalski 59
Path evolution

Roman Śmierzchalski 60
 Testing navigational
situations
with fixed constraints and moving targets

Roman Śmierzchalski 61
Testing navigational situations
with fixed constraints and moving targets

Speed of Target 2
=16.9 knots

Roman Śmierzchalski 62
 Change of speed

• ship
speed = 8,6 knot

• ship
speed = 5,6 knot

• ship
speed = 12 knots

Roman Śmierzchalski 63
 Modifications of evolutionary
algorithm

 Permitted own ship speeds ={3,6; 8,6; 13,6 knots}

Stop Slow
Half

Full Ahead

Roman Śmierzchalski 64
Modification - speed change
 V = { 3.6; 8.6; 13.6w}

Roman Śmierzchalski 65
Target 3 speed reduction: (a) from 5 to
3 knots, (b) from 5 to 4 knots.

Roman Śmierzchalski 66
Roman Śmierzchalski 67
Roman Śmierzchalski 68
Roman Śmierzchalski 69
Roman Śmierzchalski 70
Roman Śmierzchalski 71
Roman Śmierzchalski 72
Roman Śmierzchalski 73
Roman Śmierzchalski 74
Roman Śmierzchalski 75
Roman Śmierzchalski 76
Roman Śmierzchalski 77
Roman Śmierzchalski 78
Roman Śmierzchalski 79
Roman Śmierzchalski 80
Roman Śmierzchalski 81
Roman Śmierzchalski 82
Roman Śmierzchalski 83
Roman Śmierzchalski 84