Path Planning For A Mobile Robot in A Dynamic Environment
Path Planning For A Mobile Robot in A Dynamic Environment
P
Path Plann
ning for a Mobile R
Robot in A Dynamic Environm
ment
Dhanannjay Bodhalee, Nitin Afzuulpurkar Member IEEE and Nguyenn
Truoong Thanh
School of Engineering
E an
nd Technology, Asian Institutte of Technologgy, Klong Luanng,
Pathumthanni 12120, Thailaand
[email protected], [email protected], nitin@@ait.ac.th
Abstract. Inn this paper, thee path planning algorithms are developed for th he mobile robotts which work under
u the dynam
mic
environmentts like manufactturing industriess with machineryy parts as movin ng objects. Pioneeer 2 DX mobile robot is used forf
the experimments in the indooor environmennt. The work foccuses on the deevelopment of path p planning allgorithms with the
t
integration of
o potential field
d technique and Monte
M Carlo Loccalization (MCL L) method for robbot navigation. The path planniing
algorithms are
a divided into two sub modulees, one is global path planning, which w use visibiility graph with A* search methhod
and another is local path plaanning, which use
u potential fieldd method to avo oid the obstacless. The image proocessing is used to
get the workking environmen nt information frrom the overheadd global cameraa. MCL algorithm ms with gradientt path planning are
a
used in roboot control prograam for continuoous localization. User friendly path
p planning sooftware PMADE E V 2.0 with robbot
simulator is developed for the
t user-robot in nteraction. The ssoftware includes image processsing, path planniing, simulator, reeal
robot manuaal control and reaal robot automattic control moduules for navigatioon in dynamic ennvironment.
Keywords. Pioneer mobile robot;
r Path plannning; Potential ffield; Monte Carrlo Localization; Dynamic enviroonment
duction
1. Introd
Mobility is i an importtant feature for f robots too
navigate inn the dynamic environment whichw differs itt
from man nipulator robbotics. Theree are manyy
techniques for the robot to make it moobile, but onlyy
this featuree is not sufficiient. Nowadayys, we can seee
the use off mobile roboots in many fields f such ass
manufacturring industries, offices in wh hich the mobilee
robot w
works underr continuouus dynamicc
environmen nts. Hence, robot shouldd have thosee
required chharacteristics too avoid an obstacle that liess
in his pathh. Mobility is not only the requirement
r off
today’s mo obile robot butt also controlliing the motionn
and other parameters is important according too Fig.1. Genneral control schheme for mobile robots
(Turennouut P. Van et al.)
dynamic environment.
e N
Nonholonomy y is a centrall
issue on controlling a mobile robott in a singlee
There aree many techniqques used to coontrol the motion
operation oro multiple opperation mobille robots. It iss
and path planning in last decades.
not an easy task to achiieve smooth and a continuouss
Mostly used
u techniquess are,
path plannning in an unkknown dynamicc environmentt
• Motor schemaa technique (Huuq Rajibul et aal.,
for nonholoonomic mobilee robots, whichh are subject too
2005)
various robbot constraints (Khatib, O. annd Quinlan, S..
1993). In addition,
a to conntrol such mobbile robot or too • Velocity obstaacle, collision cone
c method
avoid staticc and dynamicc obstacles, mootion planningg (Fiorini P.
P and Shiller ZZ., 1998)
and its conntrolling actioons are also very
v importantt • Polar polynoomial curves (sensor baseed)
like for paath planning. Generally,
G mobbile robots usee approachh (Ge Shuzhi S Sam et al. 2007)
some techn niques for conttrolling its action in dynamicc • Intelligent coomputing baseed method.(GA,
environmen nt. The approaach for controolling action iss AI, Neuural network eetc) (Fernandeeza Juan A. Et
shown in Fig.
F 1. al.1997)
• Artificial potential field
f method
(Agirrebeitia Josu et al.)
In this paper, we are focusing on the obstacle toward the goal while simultaneously avoiding
avoidance technique in the dynamic environment known obstacles.
such as manufacturing industry environment. For this The basic idea behind all potential field
obstacle avoidance technique in dynamic approaches is that the robot is attracted toward the
environment, new algorithms are designed for mobile goal, while being repulsed by the obstacles that are
robot navigation. The algorithms are designed by known in advance. If new obstacles appear during
using two techniques. First one is path planning by robot motion, one could update the potential field in
potential field approach, which is one of the most order to integrate this new information. If we assume
popular for path planning due to its less calculation differentiable potential fields function U (q) around
time, and the second technique is Monte Carlo the robot, we can find the related artificial force F (q)
Localization (MCL) (Fox Dieter et al., 1999), which acting near the obstacles position q(x,y).
is one of the most useful localization techniques for ܨሺݍሻ ൌ െܷሺݍሻ (1)
mobile robotics. In addition, we designed user Where ܷሺݍሻ denotes the gradient vector U at
friendly Path planning for Mobile robot in A position q.
Dynamic Environment (PMADE) V 2.0 software for ఋ
the user-robot interaction. ఋ௫
Path planning is the art of deciding which route to ܷሺݍሻ ൌ ఋ (2)
take for navigation under dynamic environment. The ఋ௬
path planning is based on the current internal Path planning using potential field algorithms are
representation of the terrain. It involves many designed by the combination of Astar search method,
computations for continuous movement and global static path planning module, local dynamic
sequences while moving between the start and end path planning module and image processing module.
goal. The path planning involves the computation of
collision free path between the two known goals. The 3. Pioneer robot control module
overhead global color camera has been used for
image processing. The MCL algorithms use sonar The control program includes Monte Carlo
sensors for proper localization of the robot in an localization algorithms and gradient path planning
unknown dynamic environment. The gradient path algorithms. The sonar sensors are used for robot
planning algorithm is introduced in path planning localization and obstacle detection in the dynamic
module to avoid obstacles and to increase the safety environment. The gradient path-planning module
factor of the robot. The MCL algorithms use all the runs parallel with MCL algorithms. In case the sonar
position data continuously from the path planning sensors detecting any obstacle, at that instant the
algorithms for mobile robot localization. robot plans its own path and moves to the next goal
This paper is organized as follows. First, the path by avoiding the obstacle.
planning module using potential field method is
described. The sub modules of path planning
3.1. Monte Carlo Localization
algorithms global path planning and local path MCL algorithms (Fox Dieter et al. 1999) particularly
planning, is presented. The robot control module is keeps robot updating its steps continuously in the
presented in which the Monte Carlo localization workspace. The sonar and motor encoder sensors of
approach, gradient path planning method is the robot send continuous data to the robot client
presented. In the last portion of the paper, design of program, which is on the PC. The complete system is
the obstacle avoidance algorithms described with closed loop system. MCL algorithms address the
GUI. Finally, the paper is concluded with real robot problem of state estimation from sensor data. MCL is
and simulator experiments with its results and a probabilistic algorithm and maintains a probability
analysis. distribution over the space.
The general idea for MCL algorithms is given as:
2. Path planning module for mobile robot • Initialize the set of current samples so that
using potential field technique their locations are evenly distributed and
their importance weights are equal.
In this method, the real valued potential field is • Repeat until done with the current set of
constructed around the robot (U(q)) and the potential samples.
field is kept zero value at a goal position. So that, the • Move the robot a fixed distance and then
robot tries to move toward the lower potential field take a sensor reading.
i.e. toward the goal. The goal acts as an attractive • Update the location of each of the samples
force on the robot and the obstacles act as peaks, or (using the movement model).
repulsive forces. The superposition of all forces is • Assign the importance weights of each
applied to the robot, which, in most cases, is assumed sample to the likelihood of that sensor
to be a point in the configuration space. Such an reading given that new location (using the
artificial potential field smoothly guides the robot sensor model).
2
The actual distance moved by the robot using MCL navigation. The path planning algorithms use image
may not be correct sometime. Hence, MCL uses processing from overhead camera and plan a shortest
sampling data. The steps are given below. path within the dynamic environment from start to
goal position, but if there is any change in position of
• Create a new collection of samples by the static obstacles after image processing done, the
sampling (with replacement) from the created path by path planning module will not
current set of samples based on their change. Hence, the MCL algorithm with gradient
importance weights. path planning method becomes very important in
• Let this be new collection become the such conditions. However, if we changed the position
current set of samples. of the static obstacle after image processing the path
created by the path-planning module may fail which
In step four, samples with a high importance weight was already planned. In such case, we need to
are more likely to be chosen than those with low maintain safety. To do this, sonar sensor plays an
probability, and some sample may be repeatedly important role for avoiding collision and maintaining
chosen. Therefore, we (usually) end up with a set of safety. After that when robots sonar sensors sense
samples with a higher cumulative importance weight any obstacle, robot tries to avoid the obstacle but
that those we had before. MCL uses fast sampling sometimes it stops for safety reason. As given in the
techniques to represent the robots belief. When robot block diagram (Fig. 2.), camera tracks the workspace
moves or senses, importance re-sampling is applied and all obstacles. The camera sends the data to the
to estimate the posterior distribution. path-planning module where it calculates the position
data in terms of X, Y coordinates. As robot only can
3.2. Gradient path planning algorithms move the distance more than its radius i.e. 250 mm.
The gradient-based (Konolige Kurt., 2001) path Hence to increase the distance between goal points
planning is very useful when sonar detects any we set the appropriate step time so that MCL
obstacle. With the help of these algorithms robot algorithms will read the goals having distance more
plans its own path by avoiding the obstacles. than 250 mm. Now as soon as simulator starts
The path generated by the gradient path planning moving, the real robot also starts moving on the same
considers local obstacle sensed by sonar devices and path on which simulator is moving. While moving
global map information like walls and other the robot in real environment the robot uses sonar
obstacles. In 100 ms, the module updates the position sensors to avoid any unpredictable obstacle. Most
and set the goal point for robot movement. probably the collision situation may occur while
avoiding moving obstacle. Timing is the measure
4. Design of obstacle avoidance factor for avoiding moving obstacle. If sometime
simulator planning a wrong path i.e. showing the
algorithms with GUI collision with the obstacle in that cases the sonar
sensor works well and it avoids the collision with the
The algorithms are designed for the mobile robot
obstacle. If sensor senses moving obstacle which is
working in manufacturing industry environment with
very near to the robot (<15 cm), the robot motor stalls
machinery parts as the moving objects. The potential
for safety reason. This is the inbuilt function in the
field technique is used for path planning in dynamic
P2OS (inbuilt operating system program) of the
environment. The data we get from path planning in
robot. Otherwise if the sensor senses the moving
terms of x, y and theta position i.e. in terms of goals
obstacle at >15 cm the robot gets time to take
(position data). In this section we are focusing on
necessary action and the gradient path planning
how the position data from path planning module use
executes.
to localize the mobile robot in the dynamic
If sometime, simulator planning a wrong path i.e.
environment and the operation of the simulator.
showing the collision with the obstacle in that cases
Total numbers of obstacles are limited up to four
the sonar sensor works well and it avoids the
due to limitation of camera field of view and
collision with the obstacle. If sensor senses moving
workspace area. We are using three static obstacles
obstacle which is very near to the robot (<15 cm), the
and one moving obstacle with constant velocity in
robot motor stalls for safety reason. This is the inbuilt
one direction and the overhead global camera for
function in the robots to plan its own path by
capturing the images. The path-planning module
avoiding the obstacle. After that, the robot try to find
processes the image data from camera and calculates
the next goals from the position data file and try to
position data in terms of X, Y, and theta. The
reach the next goal. While avoiding the obstacle the
Localization algorithms use these continuous position
robot skips some goals from the position data array.
data to localize and to move the real mobile robot in
This cycle continues up to the final goal.
dynamic environment. Total position data from the
path-planning module gives shortest path by avoiding
the obstacles.
As the robot is moving in the dynamic
environment, the safety becomes main factor during
3
Fig. 3 PMA
ADE V 2.0 GUI w
window
data file to run the roobot. The posiition file for the
t
robot inccludes x posittion, y positioon and angle of
robot, whhich is useful tto run the roboot. Proper desiggn
of GUI is very helppful for the whole w processses
required for robot naviggation such as main simulation
window, which showss the prior siimulation befoore
real roboot action, manuual drive contrrol window, annd
video wiindow that shhows the cameera preview annd
image prrocessing. Theere is a need to t develop robbot
Fig.2. Blockk diagram of inteegration of path planning
p and control tabs
t for propeer processing for
f example; we w
robot contro
ol (localization) modules
m cannot ruun the real robbot without prroper connection
between the robot andd the client PC, P also withoout
P2OS (inbuilt
( operatiing system prrogram) of thee running camera and image processsing, it is nnot
robot. Othherwise if the sensor sensees the movingg possible to create thee path and fin nally for safeety
obstacle ata >15 cm thhe robot gets time to takee reason, thhe robot must be in control while
w moving i.e.
necessary action and the t gradient path
p planningg it must be stopped at any time and should be
executes. The robots th hen plan its own path byy disconneected after its navigation orr any time aft fter
avoiding th he obstacle. Affter that, the roobot try to findd initial co
onnection. In addition, the GUIG must shoow
the next gooals from the position data file and try too the statuus of the robot and other elements. Moost
reach the next
n goal. Whiile avoiding thhe obstacle thee importan nt thing whilee constructing the GUI is to
robot skipss some goals frrom the positionn data array. follow thhe processes soo that the GUI will act like usser
This cycle continues up to o the final goall. friendly and will guuide properly throughout the t
mportant to havve user-friendlyy GUI for reall
It is im different processes. Thhis work uses the approach of
robot interaaction (Fig. 3)). As the algoriithms includess MFC appplication in V VC++ programm ming to develop
many subtaasks like, estab blishing wireleess connectionn the proper GUI for coontrolling the processes
p of the
t
with real robot, robots calibration in i workspace,, mobile roobot with simuulator.
manual coontrols of robo ot, simulation of the robott
before expperiment, imagge processing, data handlingg 5. Experimental sttudies and results
r
etc. Hence,, we designed one GUI, whicch includes alll 5.1. Experimental systtem
these subtaasks for real mobile robott control. Thee
purpose of design of this softwaree is to makee Fig. 4 shhows the entiree system, inclu uding the Globbal
simulation of the robot before experiments on reall color cam mera, firewire,, client PC witth win 2000 OS,
O
robot for safety purpose and then run the real robot.. wireless RS 232 m modules for communication
Additionallly, we includeed manual driv ve controls off between mobile roboot and client PC, static annd
the real robbot for new useers to get interaact with robotss dynamic obstacle, workspace
w andd Pioneer 2D DX
drives, simmulation window w and video window,
w whichh mobile Robot.
R The oveerhead global camera acquirres
shows imaage processing g of the imagees captured byy the imag ges of the robotts workspace anda tracks all the
t
the cameraa. The softwaare calculates robot r positionn obstacless within its fielld of view.
data from the
t image proccessing results,, which can bee The camera
c sends all
a the image datad to the Clieent
used for siimulation as well
w as real robbot navigation.. PC. The path-planningg algorithms thhen calculate the t
The mobilee robot softwarre p2os supportts the position position data with thhe help of im mage data. Aft fter
establishhing wireless cconnection bettween robot annd
the clieent PC, the Client program sends the t
position data to the robot. The MCL algorithms
a on
Fig.5. Exp
perimental envirronment A) showws the robot
workspace with robot andd obstacles B) woorkspace C)
Shows thee Client PC D) m
moving obstacle and robot.
Fig.4. Experrimental setup
5.3. Resu ult analysis
the Client PC use that continuous
c possition data andd The ressults are anaalyzed after the successfful
send the goals (positioons) for the Mobile robott experimeents on PMAD DE simulator, SRIsim,
S and reeal
through wiireless RS232 module. The mobile robott robot as shown in Figg. 6 and 7. Th he position daata
then startss following th he goals. Thee robot sendss from the SRIsim and real robot is coompared with the t
Server Infformation Pack kets (SIP) froom its sensorss position data from Paath planning algorithms.
a Aft
fter
such as sonnar sensors, enncoders and oth her accessoriess comparinng this positionns data, we found that there is
informationn. This cycle of sending SIIP from robott not muchh difference inn the path traveelled by PMAD DE
runs for evvery 100 or 500 millisecondss i.e. whateverr simulatorr, real roboot, and SRIIsim simulatoor.
the data roobot sends thaat will be updaated data afterr Howeverr, there are som me errors in po osition data froom
each 100 orr 50 milliseconnds. real roboot and path plaanning data in case of compllex
The expperiments are performed on the real robott paths. Du ue to the sonaar sensors, mobbile robot avoiids
in the labooratory. Total 3200x2400
3 mmm of area wass collisions and wheneever robot gets g the propper
used for thhe robot worksspace. The expperiments weree position data, the robott moves on hiss way toward the t
performed in the differennt environmennts and in twoo goal.
main parts. First part doone by using useru input andd The accuracy
a of thee MCL localizzation is depennds
second parrt done by usin ng image proceessing. All thee on workiing conditions such as type of mobile roboot,
experimentts were perfoormed on real robot andd active ennvironment, ppath complexitty, proper inpput
simulator. data, prooper timing, robot velocityy, data packeets,
During some experim ments, the real robot collidedd proper sensor infoormation annd localization
with the obbstacles due to insufficient timme and packett parameteers. The locaalization erroor increases as
errors. In addition,
a there are some otheer reasons likee distance between the goalsg decreasees. As we know,
high veloccity and insu ufficient sonarr sensor data.. the robott can only movve the minimum m distance up to
During som me experimentts robot was un nable to movee its radiuss i.e. 250 mm. IIf the goal disttance is less thaan
toward hiss goal due sud dden obstacle detection. Ass 250mm. then the roboot will skip thhat goal or will w
discussed before, the robot
r program
m have safetyy rotate aroound him for searching that goal.
g
watchdog inbuilt
i in the robot
r server, which
w stall thee Thereefore, to mainttain this distannce between the t
robot motoors in case of obbstacle detectioon. goals, steep time of thee simulator is sets at 800 ms. If
we comppare the paths ttravelled by PM MADE simulattor
5.2. Experrimental Resullts as shownn in Fig. 9. Thhe result is neaar about same as
The experiiments are perrformed on thhe real mobilee there aree no losses in simulation likee friction lossees,
robot in thhe dynamic envvironment. Fig g. 5 shows thee inertia effects, slippagge, packet erroors, sonar senssor
different positions
p of thhe real mobile robot whenn errors annd so on like reeal robot. Though there are no n
moving toowards the goal g through the dynamicc losses like real robot in SRIsim, we w can find the t
environmen nt. The robotss start positionn in lower leftt differencce in paths traavelled by PM MADE simulattor
corner andd the goal in upper
u right coorner. The redd and SRIIsim. Why thiis is so? As MCL M module is
color show ws the static obstacles
o and the
t blue colorr using alll the position data from thhe path planninng
shows mooving obstacle in straight direction
d withh module to t move the SRIsim/real
S roobot. For simpple
constant veelocity. and straiight paths, thee simulator ruuns correctly by b
followingg the goals ggenerated by path p planningg
Fig.6. Robots travelled path by avoiding the obstacles
Fig.8. Robots positions in presence of three static and one
moving obstacle
6
Obstacles. International Journal of Robotics using potential field and roadmap based
Research Volume 17, Number 7, pp. 760 - planner. Texas A&M University
772, Nam Tang Quoc. 2002. Development of a
Fkbdbric Large et al. 2004. Avoiding Cars and localization algorithm for mobile robot in
Pedestrians Using Velocity Obstacles and material transportation system. AIT Thesis,
Motion Prediction. IEEE Intelligent Vehicles ISE
Symposium, University of Parma Nehmzow Ulrich. Mobile robotics: A practical
Fox Dieter et al. 1999. Monte Carlo Localization: Introduction. ISBN 1-85233-173 9 Springer-
Efficient Position Estimation of Mobile Verlag London Limited, pp. 87-131
Robots. Proceeding of the National Planas R.M. et al. Qualitative Approach for Mobile
Conference on Artificial Intelligence (AAAI) Robot Path Planning based on Potential
Ge Shuzhi Sam et al. 2007. Sensor-based path Field Methods. Automatic Control Dept.
planning for nonholonomic mobile robots Technical University of Catalonia Edif.
subject to dynamic constraints. Department TR11. Rambla Sant Nebridi, 10 Terrassa
of Electrical and Computer Engineering, 08222. Barcelona. Spain
National University of Singapore, 4 Siegwart Roland and Nourbakhsh Illah R. 2004.
Engineering Drive 3, Singapore 117576. Introduction to Autonomous Mobile Robots,
Huq Rajibul et al. 2005. Mobile robot navigation The MIT Press.
using motor schema and fuzzy context Thanh Nguen Truong. 2006. Path planning for a
dependent behavior modulation. CCORE/ mobile robot in a dynamic environment. AIT
Faculty of Engineering, Memorial Thesis, ISE
University of Newfoundland, St. John’s, Turennout P. Van et al. Motion control of a mobile
Nfld, Canada robot. Control Laboratories, Faculty of
Keith J. O’Hara et al. 2004. Distributed Path Electrical Engineering University of
Planning for Robots in Dynamic Technology Delft Netherlands
Environments Using a Pervasive Embedded
Network. The BORG Lab College of
Computing Georgia Institute of Technology,
Khatib, O., Quinlan, S., May 1993. “Elastic Bands:
Connecting, Path Planning and Control,” in
Proceedings of IEEE International
Conference on Robotics and Automation,
Atlanta, GA.
Konolige Kurt. 2001. Robot Notes - Robot Motion:
Probabilistic Model; Sampling and Gaussian
Implementations; Markov Localization. SRI
International.
Kurihara, K. et al. 2005. Mobile robots path planning
method with the existence of moving
obstacles; 10th IEEE Conference on
Emerging Technologies and Factory
Automation, Volume 1, pp 195-202.
LEE.B.H. 1987. Constraints Identification in Time-
Varying Obstacle Avoidance for Mechanical
Manipulators. The Department of Control
and Instrumentation Engineering, Seoul
National University, San 56-1 Shinrim-
Dong, Kwanakku Seoul 151, Korea. IEEE
Log Number 8825076.
LMICSE Project. Monte Carlo Localization.
Magyar Balazset al. Developing an Efficient Mobile
Robot Control Algorithm in the Webots
Simulation Environment. Budapest
University of Technology and Economics
Department of Automation and Applied
Informatics 8 Budafoki street, XI., Budapest,
H-1111, HUNGARY
Malik Waqar Ahmad. Thesis. 2003. Motion planning
of mobile robot in dynamic environment