125
1 INTRODUCTION
With the growing advances in navigation
technologies,thereisagreaterneedtoexploreoceans
for resources as well as for the future needs.
Autonomous unmanned vehicles have shown the
potential towards various missions of scientific and
military significance depending upon the
requirement,environmentandcostinvolved(Serreze
et al., 2008 and Legrand et al., 2003). Unmanned
vehiclescanbeclassifiedint
ofourcategoriesnamely,
unmanned aerial vehicles (UAVs), unmanned
underwater vehicles (UUVs), unmanned ground
vehicles (UGVs) and unmanned surface vehicles
(USVs). USVs are watercraft of small (<1 tonnes) or
medium (100 tonnes) size in terms of water
displacement.
Thegeneralarchit
ectureforanUSVoperationina
maritime environment has three basic systems
namely, control and path planning, communication
andmonitoringandobstacledetectionandavoidance
(ODA), which are responsible for mission planning
andexecutionasshowninfigure1.Pathplanningis
oneofthebasicsubsystemsinthema
ritimeoperation
ofUSVstogenerateway‐pointsforasafenavigation
withinadesiredenvironmentfromstarttoendpoint.
Research and development in areas of artificial
intelligence has provided larger scope for
development in this territory of marine navigation
(Campbell et al., 2012). The abstraction of path
planningforanUSVissumma
rizedinfigure2.
Untilnowinpathplanning ofanUSV,globaland
localapproacheshavebeenadopted,whichhasbeen
summarised in figure 3. In global approaches, the
complete informationof environment is well known
while in the local approach only partial information
ab
out the environment is known. Under global
approaches, grid map‐based path planning
techniquesarethebestknownsincetheygeneratesub
optimaltrajectorieswiththefastestcomputationtime
Towards use of Dijkstra Algorithm for Optimal
Navigation of an Unmanned Surface Vehicle in a Real-
Time Marine Environment with results from Artificial
Potential Field
Y.Singh,S.Sharma,R.Sutton&D.Hatton
A
utonomousMarineSystemsGroup,PlymouthUniversity,UnitedKingdom
ABSTRACT:Thegrowingneedofoceansurveyingandexplorationforscientificandindustrialapplicationhas
ledtotherequirementofroutingstrategiesforoceanvehicles whichareoptimalinnature.Mostoftheop‐timal
pathplanningformarinevehicleshadbeenconductedofflineinaself‐ma
deenvironment.Thispapertakesinto
account a practical marine environment, i.e. Portsmouth Harbour, for finding an optimal path in terms of
computationaltimebetweensourceandendpointsonarealtimemapforanUSV.Thecurrentstudymakes
useofagridmapgeneratedfromoriginalandusesaDijkstraalgorit
hmtofindtheshortestpathforasingle
USV.Inordertobenchmarkthestudy,apathplanningstudyusingawell‐knownlocalpathplanningmethod
artificial path planning (APF) has been conducted in a real time marine environment and effectiveness is
mea
suredintermsofpathlengthandcomputationaltime.
http://www.transnav.eu
the International Journal
on Marine Navigation
and Safety of Sea Transportation
Volume 12
Number 1
March 2018
DOI:10.12716/1001.12.01.14
126
Figure1. General architecture of USV operation in a
maritimeenvironment(Campbelletal.,2012)
Figure2.PathplanningabstractionforUSVs
Figure3.PathplanningapproachesforUSV
1.1 Literaturereview
Dijkstra(1959)initiatedthework in theareaofgrid
map‐basedpathplanningalgorithmbydescribingthe
shortestpathbetweentwonodesspecifiedonamap.
This was later improved by Hart et al. (1968) who
introduced A
*
, which is an extended version of
Dijkstra algorithm. In the last two decade, many
variants of A
*
have been introduced by various
researchers to improve the performance of robots
working in various environments. Stentz (1995)
introducedthefirstmajorimprovementofA
*
,focused
D
*
algorithmforrealtimepathreplanningwhichwas
laterimprovedforpartiallyunknownenvironmentby
induction of D
*
Lite (Koenig and Likhachev, 2002).
Anotherimprovementby fixinginfelicitiesof A
*
ina
dynamic environment was introduced by Likhachev
etal.(2005)throughAnytimeDynamicA
*
.Sincethese
algorithmsdonotconsidertheheadinganddynamics
of a robot in account, another major improvement
was introduced in the form of Theta
*
(Nash et al.,
2007). This algorithm accounts heading angle and
yawrateof arobotinthe pathplanning, whichisa
necessityforUSVpathplanningsinceitcannotfollow
an unrealistic path with sharp turns (Kruger et al.,
2007;PrasanthKumaretal.,2005;Yangetal.,2011).
Advanced approaches like the ant colony algorithm
(ACO)(Song,2014)andparticleswarmoptimization
(PSO)(Songetal.,2015)havebeenadoptedforUSV
navigation but cannot generate trajectories in real
timeduetohighcomputationalload.Alongwiththis,
thesealgorithmsdonotgiveconsiderationtovehicle
dynamicsandturning
radius.
In robotics, various local path planning
approaches such as Collision Cone Concept
(Chakravarthy and Ghose ,1998), Velocity Obstacle
Approach (Fiorini and Shiller ,1998), Vector Field
Histogram (Borenstein and Koren ,1991), and APF
(Khatib,1986),havebeenproposed.Asmostofthe
roboticsproblemisrealtime,theneedto
haveavery
fast and simple motion planner is evident. The
simplicityenablesfastdevelopmentanddeployment
ofarobot,whereas the computationally inexpensive
nature allows the algorithm to be implemented in
robotswithminimumsensingcapabilities.APFisone
ofthesimplestmethods,andthemethodiscapableof
autonomously moving a robot in realistic obstacle
framework.
AfterAPFwasintroducedbyKhatib(1986),many
researchers have attempted to improve the APF,
which suffers from trap situation in local minima,
oscillations in narrow passage and goals non‐
reachable with obstacles nearby (GNRON) (Koren
and Borenstein, 1991). Ge and Cui (2002)
included
velocitytermsfortargetandobstacleswithinAPFto
computepotentialtocorrecttheproblemofGNRON.
Baxteretal.(2007,2009)usedAPFformultiplerobots
in order to correct the sensor errors. Tu and Baltes
(2006)usedafuzzyapproachwithinAPFtosolvethe
problem of
oscillations within narrow passage.
Fahimi et al. (2009) used the concept of fluid
dynamicswithinAPFtocorrecttheissueofatrapped
situationinlocalminima.
Until now in the literature, very few studies
associatedwiththepathplanning ofUSVhavemade
use of the APF in a practical
marine environment.
Most of these studies have been conducted in self‐
simulated environment.The present paper makes
an effort to understand the effectiveness of APF in
path planning of USV in a practical marine
environment.
1.2 Majorcontribution
Many studies in marine navigation have been
conducted but most of them
have been related to
collision avoidance rather than the path planning
problem (Tam et al., 2009). Even the studies
conducted on optimal USV navigation have been
strugglingwiththehighcomputationalloadandare
inapplicable in generating trajectory in real time.
Untilnowintheliterature,pathplanningapproaches
havebeen
appliedonaself‐simulatedEuclideanSE(2)
grid map with no consideration to real time
environment. This study presents the use of the
Dijkstra algorithm in a real time environment with
minimumcomputationalloadtogenerateatrajectory
within a real time operation. This approach is well
suited for optimal
USV navigation in a static
environment with minimum computational
requirement. In order to benchmark the present
study, a well‐known local path planning approach
127
APF has been chosen for USV path planning in a
static environment and its effectiveness is measured
in terms in terms of path length and computational
time.
Thepaperhasbeenorganizedinfoursections.The
sectionafterthe introductory material comprising of
literaturereviewandmajorcontributionexplainsthe
methodologyandtheDijkstraalgorithmusedforthe
study.Thethirdsectiondealswithimplementationof
Dijkstraalgorithminarealtimemarineenvironment
and explains its effectiveness. The fourth section
explainstheconceptandimplementationofAPFina
realtimemarineenvironmentwithresults.Thefinal
sectiondiscusses
theresultsandprovidesconclusions
withrecommendationstowardsfuturework.
2 METHODOLOGY
2.1 DijkstraAlgorithm
There arevarious variants of the Dijkstra algorithm.
The variant used in this study fixes a source node
which is the start point of the USV and finds the
shortestpathsfromsourcenodetoall
othernodesin
the graph leading to shortest‐ path tree. In order to
reducethecomputationalloadintheoriginalvariant,
asparsegraphi.e.graphwithfeweredgesapproach
hasbeenadoptedleadingtomoreefficientstorageof
graphnodes.ThealgorithmisdefinedinAlgorithm1
(Ahuja,1990).
Algorithm1.Dijkstra(Graph,source)
1:functionDijkstra(Graph,source):
2:createvertexsetQ
3:foreachvertexvinGraph://Initialization
4:dist[v] ← INFINITY // Unknown distance from
sourcetov
5:prev[v] ← UNDEFINED // Previous node in
optimalpathfromsource
6:add v to Q//
All nodes initially in Q (unvisited
nodes)
7:dist[source]←0//Distancefromsourcetosource
8:whileQisnotempty:
9:u ← vertex in Q with min dist[u]// Node
withtheleastdistancewillbeselectedfirst
10:removeufromQ
11:for
eachneighbourvofu://visstillinQ.
12:alt←dist[u]+length(u,v)
13:if alt < dist[v]:// A shorter path to v has been
found
14:dist[v]←alt
15:prev[v]←u
16:returndist[],prev[]
2.2
Environmentalmapping
Environmental mapping is the first step in the
abstractionofpathplanningasshowninfigure2.In
order to use a practical environment, Portsmouth
harborhasbeenconsideredasshowninfigure4.The
mapisorganisedasaweightedoccupancymapusing
acelldecompositionmethod
(Latombe,1991).
This map represents obstacles as bla ck and free
space as white in a matrix of black and white as
showninfigure5.A800×800pixelmapsizehasbeen
used for the simulation with a resolution of 3.6
m/pixel.
Figure4.Aerialviewofthesimulationarea(Source:Google
Maps
Figure5.Gridmapofthesimulationarea
Figure6.TheSpringerUSV
128
Table1.SpecificationsofSpringer
_______________________________________________
ConfigurationValues
_______________________________________________
Length(m)4.2
Width(m)2.3
Displacement(tonnes)0.6
Maximumspeed(m/s)4
_______________________________________________
3 SIMULATION
The proposed simulation was executed using
MATLAB2015aonInteli52.80GHzquadcorewitha
16GBRAM.Inthesesimulations,computationaltime
ofthesimulationforthreedifferentstartnodesanda
fixed goal node have been compared in order to
determinetheeffectivenessofthe
algorithminterms
ofcomputationaltimetofindanoptimaltrajectoryin
a practical marineenvironment.The simulations are
assumedtobeusedbySpringer,aUSVavailablewith
PlymouthUniversitywhosespecificationshavebeen
given in Table 1. Figure 6 shows the Springer USV.
Figure7showsthethree
casesofthreedifferentstart
nodeswithinthegridmaphavingafixedgoalnode.
These starting nodes are chosen arbitrarily within
gridmapondifferentpositionswithinthesimulation
area to show the effectiveness of the algorithm in
findingdifferenttrajectorieswithleastcomputational
load.
Table2. Performance analysis for three cases in terms of
computationaltime
_______________________________________________
CasesComputationalTime(s)
_______________________________________________
Case1(Figure6(a))6.801
Case2(Figure6(b))5.579
Case3(Figure6(c))6.141
_______________________________________________
Table 2 shows the comparison of computational
timeforthreecasesasshowninfigure7.Theresults
show that the trajectories generated by the Dijkstra
algorithm within a huge grid map from any source
nodes satisfy the computational efficiency. All cases
are able to generate a complete path in less
than 7
secondswhichinturnsleadtothegenerationofpath
inlessthan1secondpermetrelengthofthedistance
covered by USV. Henceforth, such an algorithm is
applicable in a real time operation where faster
optimaltrajectoriesareneededtobegenerated.
Since the maximum speed
of the USV for which
the algorithm is designed is 4 m/s, henceforth, the
proposed approach satisfies the dynamic constraints
of the platform. Although various factors such as
vehicle dynamics and heading angle have not been
consideredintheapproach,thebasicobjectiveofthe
studytowardsgenerationoftrajectorywith
minimum
computationalloadhasbeenaccomplished.
Figure7.Simulationresultsforthreecases
4 APF:CONCEPTANDMETHODOLOGY
APFsolvestheproblemassumingallobstaclesarea
source of repulsive potential, with the potential
129
inverselyproportionaltothedistanceofarobotfrom
theobstaclewhilethegoalattractsitbyapplyingan
attractivepotential,Kala(2016).Thederivativeofthe
potentialgives thevalueofthe virtual forceapplied
ontherobot,basedonitsmovement,Kala(2016).The
motioniscompletelyreactive
innature.Aschematic
oftheAPFisshowninfigure8.
Figure8.Schematicofthe APF
4.1 AttractivePotential
Theattractivepotentialisappliedbyasinglegoalto
directtherobottowardsitself.Theattractivepotential
is directly proportional to the distance between the
currentpositionoftherobotandthegoal.Thiscauses
thepotentialtotendtozeroastherobotapproaches
thegoal
andhenceitslowsdownasitapproachesthe
goal(Kala,2016).Thepotentialinthisstudyistaken
as,quadraticpotential,representedinEquation(1)
2
1
()
2
att att
Ux kxG
(1)
wherexisthecurrentpositionoftherobotandGis
the goal. ║.║ is the Euclidean distance function and
k
attistheproportionalityconstant,whereasthedegree
istakenas2.
Thedrivingforce isavectorwhosemagnitudeis
measured through the derivative of the potential
function and direction as the line which maximizes
the change in potential, which is given by Equation
(2)
() () .( )
()
()
att att att
att
att
Fx U x k xGuxG
xG
kxG
xG
kxG



(2)
u()istheunitvector.
4.2 RepulsivePotential
Therepulsivepotentialisappliedbyobstacleswhich
repeltherobotcomingcloseandrepellingittoavoid
collision.Thepotentialisinverselyproportionaltothe
distance so that potential tends to infinity if robot
comesnearobstacleleadingtorepulsion.
Obstaclesat
a certain distance d
*
are considered in modeling the
potential(Kala,2016).
TherepulsivepotentialisgivenbyEquation(3).
2
*
*
*
111
2
()
0
rep
i
i
rep
i
k
if x o d
xo d
Ux
if x o d




(3)
where,xisthecurrentdistanceoftherobotando
iis
thepositionoftheobstacle.║.║ is the Euclidian
distance function and k
rep is the proportionality
constant,whereasthedegreeistakenas2.
TherepulsiveforceisgivenbyEquation(4),which
isaderivativeoftherepulsivepotential
2
*
2
*
3
*
() ()
111
()
()
111
()
11
rep rep
rep i
i
i
i
rep
ii
i
i
rep
i
i
Fx Ux
kuxo
xo d
xo
x
o
k
od xo
xo
xo
k
xo d
xo
















(4)
4.3 ResultantPotential
The resultant potential is given by sumof attractive
andrepulsivepotential.Thisfinalforceishenceforth,
thederivativeoftheresultantpotential.Thisisgiven
inEquation(5).
att rep
att rep att rep
UU U
FUU U FF
 
(5)
4.4 Methodology
Inthepresentstudy,APFisusedforUSVnavigation
within a practical marine environment i.e.
PortsmouthHarbourhavingastartandgoalpointas
showninfigure9.
A binary map of 800 x 800 pixel grid resolution,
figure10,istakenintoaccountwitha
USVavailable
fromPlymouthUniversitynamed,Springer,shownin
figure 5 being considered in terms of kinematic
constraints for the purpose of path planning.
ParametersusedinAPFforpathplanningofSpringer
areshowninTable3.
130
Figure9. Simulation area‐ Portsmouth Harbour (Source:
GoogleMaps)
Figure 10. Binary map of the simulation area (1 Pixel =
3.6m)
Table3. Parameters used in APF for path planning of
Springer
_______________________________________________
ParametersValues
_______________________________________________
AttractivePotentialScalingFactor(katt) 300000
RepulsivePotentialScalingFactor(k
rep) 300000
SafetyDistancefromObstacles(d
*
) 30pixels
MaximumTurnRate10pi/180˚
InitialHeadingofUSV‐pi/2
_______________________________________________
EvaluationoftheAPFperformanceforUSVpath
planning in terms of path length and computational
time is described in Table 4. Simulation records
movementsequencesoftheUSVwithinmap.Figure
11 shows the sequence of USV motion from start to
goalpointatdifferenttimeofthemotion.The
overall
trajectory shows that such algorithm is efficient in
generating safe path for USV in a practical marine
environment.
Table 4 shows that USV is able to find a safe
trajectory of length 3075 m within 32.608 s which
means,lessthan1sisrequiredbyUSVtofind
apath
of 1m. Thus real time implementation of such
algorithm is possible within a practical marine
environment.SincetheAPFisaparameterdependent
algorithm, there is a need to find right set of
parametersfordifferentcasescenarios.Inadditionto
this, such algorithm is not complete (i.e. guarantees
findingapathinallscenarios)andismoreintensive
computationallythanglobalpathplannerstobeused
in marine robots which have limited on board
capability.
Table4.PerformanceofAPFinSpringernavigation
_______________________________________________
ParametersValue
_______________________________________________
PathLength3075m
CPUTime32.608s
_______________________________________________
Figure11.SequenceofUSVmotionfromstarttoendpoint
5 CONCLUSION
In this paper, a computationally efficient Dijkstra
algorithm to find a path between a source and goal
node on a grid map is proposed. The performance
was measured in terms of computational time for
three different cases, where source points where
chosenarbitrarily.Theresultsshowthattheproposed
approach satisfies the computational requirement of
thepathplanninginarealtimeenvironment.Inorder
to benchmark the study, a well‐known local path
planningalgorithmAPFwasalsostudiedandresults
were presented. Dijkstra algorithm was found more
effective in terms of finding path optimally and
computationally.In
conclusion,thisnewapproachis
suitableforglobalpathplanningofanUSVinastatic
environment.Towardsfuturework,vehicledynamics
and environmental disturbances can be included in
thegridmaptobetterunderstandtheapplicabilityof
thisapproachinadynamicenvironment.
131
6 ACKNOWLEDGEMENTS
Thisresearchissupportedbythedoctoralgrantofthe
Commonwealth Scholarship Commission, United
KingdomtenablefordoctoralstudiesintheSchoolof
EngineeringatPlymouthUniversity.
REFERENCES
Ahuja,R.K..,Mehlhorn,K.,Orlin,J.B.,andTarjan,R.E.1990.
FasterAlgorithmsfortheShortestPathProblem,Journal
ofAssociationforComputingMachinery(ACM),37(2):213–
223.
Baxter, J.L., Burke, E.K., Garibaldi, J.M., and Norman, M.
2007. Multi‐robot search and rescue: A potential field
basedapproach,AutonomousRobotsandAgents,
Springer,
9‐16.
Baxter, J.L., Burke, E.K., Garibaldi, J.M., and Norman, M.
2009.Sharedpotentialfieldsandtheirplaceinamulti‐
robotco‐ordinationtaxonomy, Robotics andAutonomous
Systems,57(10):1048‐1055.
Borenstein, J., and Koren, Y. 1991. The vector field
histogram‐fast obstacle avoidance for mobile robots,
IEEE
Trans.RoboticsandAutomation,7:278‐288.
Campbell,S.,Naeem,W.andIrwin,G.W.2012.Areviewon
improvingtheautonomyofunmannedsurfacevehicles
through intelligent collision avoidance manoeuvres,
AnnualReviewsinControl,36:267–83.
Chakravarthy,A.,andGhose, D. 1998. Obstacle avoidance
inaDynamicEnvironment: ACollisionConeApproach,
IEEETrans.Systems,ManandCybernetics,Part A:Systems
andHumans,28:562‐74.
Dijkstra,E.1959.Anoteontwoproblemsinconnexionwith
graphs,Numer.Math.1,269‐271.
El‐Hawary, F. 2000. The Ocean Engineering Handbook,
CRCPress,1‐416.
Fahimi,F.,Nataraj,C.,andAshrafiuon,H.
2009.Real‐time
obstacleavoidanceformultiplemobilerobots,Robotica,
27(2):189‐198.
Fiorini, P., and Shiller, Z., 1998. Motion planning in
dynamic environments using velocity obstacles, Int. J.
RoboticsResearch,17:760‐772.
Ge,S.S.,andCui,Y.J.,2002.Dynamicmotionplanningfor
mobilerobotsusingpotentialfieldmethod,Autonomous
Robots,13(3):207‐222.
Hart, P.E., Nilsson, N.J., and Rapheal, B. 1968. A formal
basis for the heuristic determination of minimum cost
paths, IEEE Transactions on System and Scientific
Cybernetics4,100‐107.
Kala,R.2016.Potential‐BasedPlanning,On‐RoadIntelligent
Vehicles,Ch.11:318‐356.
Khatib, O. 1986. Real time obstacle
avoidance for
manipulatorsandmobilerobots,Int.J.RoboticResearch,
5:90‐98.
Koenig, S., and Likhachev, M. 2002. D*
Lite,AAAI/IAAI,476‐483.
Kruger, D, Stolkin, R., Blum, A., and Briganti, J. 2007.
Optimal AUV path planning for extended missions in
complex, fast‐flowing estuarine environments, In
Proceedingsof
theIEEEInternationalConferenceonRobotics
andAutomation(ICRA),4265‐4271.
Latombe,J. 1991. RobotMotionPlanning,KlumerAcademic
Publishers,Norwell,USA.
Legrand,J.,Alfonso,M.,Bozzano,R.,Goasguen,G.,Lindh,
H., Ribotti, A., Rodrigues, I., Tziavos, C. 2003.
Monitoring the Marine environment: Operational
Practices in Europe, In Third International
Conference on
EuroGOOS.
Likhachev,M.,Ferguson,D.I.,Gordon,G.J.,Stentz,A.and
Thrun, S. 2005. Anytime Dynamic A*: An Anytime,
ReplanningAlgorithm,ICAPS,262‐271.
Nash,A.,Daniel,K..,Koenig,S.AndFelner,A.2007.Theta*:
any‐angle path planning on grids, In Proceedings of the
NationalConference
onArtificialIntelligence,1177‐1183.
Prasanth‐Kumar, R., Dasgupta, A., Kumar, C. 2005. Real‐
time optimal motion planning for autonomous
underwatervehicles,OceanEngineering32,4265‐4271.
Serreze, M.C. 2008. Arctic seaice in 2008: standing on the
threshold,InMTS/IEEEOCEANS’08Conference.
Song, C.H. 2014. Global Path Planning Method
for USV
System Based on Improved Ant Colony Algorithm,
AppliedMechanicsandMaterials,568‐570.
Song, L., Mao, Y., Xian, Z., Zhou, Y., and Du, K. 2015. A
StudyonPathPlanningAlgorithmsBaseduponParticle
Swarm Optimization, Journal of Information and
ComputationalScience12,673‐680.
Stentz,A.1995.
TheFocussedD* Algorithm for Real‐Time
Replanning,IJCAI95,1652‐1659.
Tam, C., Bucknall, R., and Greig, A. 2009. Review of
collisionavoidanceandpathplanningmethodsforships
incloserangeencounters,JournalofNavigation59,27‐42.
Tu,K.,andBaltes,J.,2006.Fuzzypotentialenergyfora
map
approachtorobotnavigation,J.RoboticsandAutonomous
Systems,54(7):574‐589.
Yang, Y., Wang, S., Wu, Z., and Wang, Y. 2011. Motion
planning for multi‐HUG formation in an environment
withobstacles,OceanEngineering38,2262‐2269.