Improved Boundary Following for a Mobile Robot with Limited SensingEmploying Particle Filters and Path Planning

Michael C. Hoy and Waqqas AhmadSchool of Electrical Engineering and Telecommunications

University of New South Wales, Australia

Abstract— In this paper, we propose a control law fornavigating a robot along the boundary of a stationary obstacle,where the only sensor information available is the minimumdistance to the obstacle. Using a particle filter, the curvatureprofile of the obstacle around the observed point is estimatedon-line. Safety is ensured by performing local path planninginside an area guaranteed to be safe. The method allows thevehicle’s speed to be adjusted in response to the changingboundary of the obstacle. Simulations confirm the validity ofthe method.

I. INTRODUCTION

In this paper, we consider the problem of navigating amobile robot along the boundary of an unknown obstacle.This type of navigation is fundamental in robotics, andmany approaches have been proposed to achieve it (see e.g.[2], [4], [8], [11], [14], [24]). In most cases, mobile robotsare equipped with a multi-directional LIDAR range-findingdevice, which informs the robot of the distance to obstaclein a large number of directions around the sensor. Manydifferent types of approaches, such as path planning, SLAM,potential field methods may be applied (see e.g. [6] andreferences therein).

While technological advances have greatly reduced thesize and weight of these sensors, there will always beexamples of vehicles for which these sensors are too largeand bulky∗. When employing small, lightweight vehiclessuch as micro UAV’s, it may be desirable to use highlysimplified sensing.

A. Previous Work

Several navigation methods have been proposed whichhave relaxed sensor requirements:• Minimum distance to the obstacle. This may be real-

ized with acoustic, optic flow, radar, or other smalllightweight sensors, see e.g. [16]. This problem hasbeen been solved with sliding mode control laws by[14]. Similar is the rate of change of the minimumdistance (see [12]).

• Detection rays in a single distance. This is consideredin several works, see [8], [13], [22]–[24]. The examples

This work was supported by the Australian Research Council. Corre-sponding author: Michael Hoy (email: m.hoy@unsw.edu.au)∗An example of a lightweight sensor (SICK TiM310) weighs 150g, while

an example of a quad-copter (SQ-4 RECON) weighs 198g.

[13], [24] require the tangential angle of the detectedpoint. The paper [8] requires the curvature of theboundary at the detected point as well as its tangentialangle.

• Visual-type information. One example is the VisBugclass of algorithms, which navigates towards a visibleedge of an obstacle inside the detection range (seee.g. [9], [11], [19]). This type of information may beobtained by looking for sharp edges in a video feed.

One disadvantage of all the aforementioned approachesis they use a fixed vehicle speed and set distance from theobstacle, which may not be the most optimal way to navigatealong the boundary.

Several approaches have been able to account for thevehicle dynamics, but still have inadequate models of thevehicle sensor. For example, [21] considers the joggers prob-lem, which exhibits safe navigation by ensuring the vehiclecan stop in the currently sensed obstacle free set. However,an abstracted sensor model was used, which presumes thevehicle has continuous knowledge about the obstacle set. In[2], a formulation which achieves boundary following bypicking instant goals based on observable obstacles. A raybased sensor model is used, though a velocity controlledholonomic model is assumed. In [3], instant goals are alsoused, and allowance is made for the vehicle kinematics.However a ray based obstacle sensor model was not used.

An MPC based approach has been proposed for bound-ary following, which generates avoidance constraints andsuitable target points for navigation [4]. This was found togive better performance than simpler approaches in certainsituations, and may be a first step for the application of MPCto the boundary following problems.

B. Contributions

The contributions of this work are listed as follows:

• A method of estimating the properties of the nearestpoint on an obstacle using only the distance measure-ment.

• A method of performing path planning when only theminimum distance to obstacles is available.

• An approach to boundary following which uses this es-timation, combined with local path planning, to achievesafe, convergent navigation.

2014 11th IEEE InternationalConference on Control & Automation (ICCA)June 18-20, 2014. Taichung, Taiwan

978-1-4799-2837-8/14/$31.00 ©2014 IEEE 336

Fig. 1. Vehicle and sensing model.

In this manuscript, we extend [4] to the cases wherevery limited information about the obstacle is present. Tothe best of the author’s knowledge, this is the first time apath planning method has been proposed with such sensordeficient vehicles.

II. PROBLEM STATEMENT

A single vehicle with unicycle kinematics traveling in aplane is considered, which contains a single static obstacleD. It is assumed that D is simply connected, and thus doesnot enclose the vehicle. The boundary of D is denoted by∂D, which is assumed to have continuous curvature. The de-sired transversal direction is given, which is encoded by thevariable Γtrans ∈ {−1,+1}, where clockwise correspondsto −1 and counter-clockwise corresponds to +1.

The minimum distance to be maintained from any pos-sible obstacle point during planning is d0, which may beinterpreted as the physical size of the robot plus some safetymargin.

The vehicle has access to the distance dmeas(t) from thenearest point on D. We assume the absolute error in thismeasurement is upper bounded by ∆dmeas, however fortheoretical verification of the control law we will assume∆dmeas ≡ 0. The vehicle also is assumed to know whetherthe nearest obstacle point lies on the left or right side ofits center-line. This information is encoded by the variableΓmeas ∈ {−1,+1}, corresponding to the left and rightdirections. This sensing modality is described in Figure 1.

A. Obstacle Assumptions

To describe the obstacle boundary, we use the obstaclecurvature κ parametrized by distance along the boundary r,so that the boundary is described by the function κ(r)†. Theabsolute curvature |κ(r)| is assumed to be upper boundedby κmax. Furthermore, we assume the rate of change ofκ is described by a given distribution. Specifically, dκ

dr isassumed to be described by Gaussian white noise, having aknown standard deviation given by κsd. However, this priordoes not affect the overall stability of the control system. Forconvenience, the radius dob is defined to be 1

κmax.

†The point on the boundary with r ≡ 0 is arbitrary.

Remark 2.1: Future work could look at higher orderMarkov type models of the boundary curvature.

Furthermore, one more assumption ensures that there issufficient space around the obstacle for boundary followingto successfully occur. For this, we employ a tunable pa-rameter dmax, which is essentially the maximum allowabledistance from the obstacle.

Assumption 2.1: For each point on the obstacle boundary,generate a second point by moving dmax in the normaldirection. Then, the nearest point on the obstacle boundaryto this second point is the original boundary point.

Somewhat similar assumptions are described by [12].Future work will consider moving obstacles, however thisgenerally complicates analysis (see e.g. [18], [20]).

B. Vehicle Model

In this paper, we consider a discrete-time, point-mass andunicycle-like model of the wheeled robot described by [5].Unicycle kinematic models are commonly used to modelmany types of vehicles such as robots, aircraft, and missiles( [10], [15], [17]). The kinematics are governed by thefollowing equations, where the control signals are updatedat whole integer multiples of ∆t:

For k∆t < t ≤ (k + 1)∆t:

s(t) = s(k∆t) +

∫ t

k∆t

[cos(θ(t))sin(θ(t))

]· v(t)dt

v(t) = v(k∆t) + uv(k) · (t− k∆t), |uv(k)| ≤ uv,maxθ(t) = θ(k∆t) + uθ(k) · (t− k∆t), |uθ(k)| ≤ uθ,max

0 ≤ v(t) ≤ vmax(1)

Where k is the time index, t is the actual time, s(t) =[sx(t) sy(t)]T is the vector of the robot’s Cartesian co-ordinates in the world frame, v(t) is the speed, θ(t) isthe orientation angle, and vmax is the maximal achievablespeed. There are two control inputs to the model; uv(k) isthe longitudinal acceleration upper bounded by uv,max, anduθ(k) is the angular velocity, upper bounded by uθ,max.

It is assumed the robot has a radius of d0, and this distancemust be maintained from the obstacle at all times. Forbrevity, we will assume the system is free from disturbance;extending our results to systems with disturbance remains atopic of future research.

III. NAVIGATION SYSTEM ARCHITECTURE

The idea behind this approach is to estimate some param-eters of the obstacle, thus making the vehicle move into theregion known to be safe, but also in a direction which maybe known to advance it along the obstacle.

A. Estimating the Obstacle Position

A particle filter is used to estimate the position of thenearest obstacle point. This essentially uses a large numberof hypotheses about a system state, which are generated

337

Fig. 2. Inferring the state of the boundary using a sequential Monte-Carlomethod.

randomly using sequential Monte-Carlo (SMC) methods (formore details of these methods see e.g. [1], [7]). At each timestep, the particles which better match the measurements ofthe system are given additional weight, and are more likelyto be used for the generation of particles corresponding tothe current time step. For the problem at hand, the states(xs) and inputs (us) of this filter are given by:

xs := [αest dest κest]T , (2)

us := [dmeas Γmeas ∆sx ∆sx ∆θ]T (3)

Here αest is the relative heading of the nearest obstaclepoint relative to the vehicle’s orientation (which is notmeasured), dest is the distance to the nearest obstacle point(which is measured via dmeas with error ∆dmeas), andus includes both the sensor measurement and the vehicle’sposition relative to the previous time step.

Unfortunately, the state transition and output relations can-not be expressed in a closed form. For each particle, we maycalculate a possible boundary segment by randomly selectingcurvatures from the appropriate distribution, continuouslyand over an infinite horizon.

Remark 3.1: Theoretically this process should be donecontinuously, and over an infinite horizon, however neither ofthese are possible practically. The finite horizon and non-zerostep for sampling the curvature were chosen heuristically andwere found to give good performance for the simulationspresented in this paper. These were labeled dhor and dsteprespectively.

The particle weights may then be updated by finding theminimum distance point from the updated (current) vehicleposition to each generated boundary segment, and retain-ing all particles for which dest agrees with dmeas withinthe measurement error ∆dmeas, and also agrees with themeasured side Γmeas. Rejected particles may be re-sampledevenly from all other valid particles.

B. Path Planning Constraints

From the results of the filtering operation, the path plan-ning can be performed. There are three types of constraintson the path planning: collision avoidance, convergence, and

maximum distance. These are summarized in Fig. 3. Further-more, a cost function is heuristically formulated to providegood practical performance.

1) Collision Avoidance Constraint: At any point of time,there is a disk of points which can be guaranteed to beobstacle free surrounding the vehicle. The radius of thisdisk may be taken to be dmeas − ∆dmeas − d0. This isnot dependent on the filtering operations, and as long as alltrajectory points lie in this circle, safety is ensured.

2) Convergence Constraint: This constraint ensures thatthe vehicle progresses around the obstacle. This is illustratedin Fig. 3. Firstly, the particles are combined to find anaverage αest. The set of safe points may then be split intotwo semicircles by a line with angle αest passing through thevehicle’s center. The semicircle in the transverse direction isof interest, and furthermore is slightly tightened by removingpoints a distance of ddec from this dividing line (see Fig. 3).

All planned trajectories must either:

• Have all planned future vehicle positions inside thetightened semicircle.

• Have a latter portion of the trajectory inside the tight-ened semicircle. In this case the initial portion of thetrajectory is unavoidably executed (to ensure conver-gence).

3) Maximum Distance Constraint: To ensure the condi-tions of Assumption 2.1 are met, it is necessary that thevehicle is no more than dmax from the obstacle. To ensurethis, the path must lie within the circle of radius dmax fromaverage (center) of the set of estimated nearest points. Thisis illustrated in Fig. 3.

4) Heuristic Estimation of Trajectory Quality: The pro-cess described for estimating the obstacle position alsopredicts a distribution of obstacle shapes in the future. Usingthese estimated obstacle segments, possible trajectories canbe quantified in their likelihood of attaining the navigationobjective.

• Distance from obstacle: The farther the vehicle is fromthe obstacle, the greater the radius of its safe area;however this also increases the total distance which hasto be traveled to navigate around the obstacle. Balancingthese two factors, we set a desired distance from theobstacle, ddes. For a particle i and a planned vehicleposition p, let the minimum distance to the projectedobstacle boundary be given by ci(p).

• Progression along the obstacle: For each particle i,we may compute the distance that the planned vehicleposition p advances the nearest point along the pro-jected obstacle boundary. For a particle i this distanceis labeled ei(p).

• Speed: Practically, preferring higher speeds may resultin better overall closed loop performance.

This can be described by the following cost function whichmay be applied to a possible trajectory s∗(j|k):

338

Fig. 3. Constraints applicable to the path planning process.

J [s∗(j|k)] =1

npart

jmax∑j=1

npart∑i=1[

− ei(s∗(j|k))− γ1 ·min{ci(s

∗(j|k)), ddes}]

− γ2 · u∗v(0|j) (4)

Here jmax is the number of steps in the path planninghorizon, npart is the number of particles, and γ1 and γ2

are tunable parameters. It should be emphasized that thisselection is purely arbitrary - it is difficult to see whetherthere exists an optimal method of selecting trajectories.

C. Vehicle Navigation Approach

A type of receding horizon control is employed, whereprobational trajectories are constrained to achieve the navi-gation objective.

A probational trajectory commencing at the current systemstate is found over a variable finite horizon at every timeinstant k. This probational trajectory is specified by a finitesequence of way-points, which satisfy the constraints ofthe vehicle model. Probational trajectories necessarily havezero velocity at the terminal planning time, which ensuresavailability of at least one safe probational trajectory at allsubsequent time steps. The number of time steps in theplanning horizon is denoted by jmax.

Assumption 3.1: It is required that:

jmax >π

uθ,max∆t+ 2 (5)

ddec < uv,max∆t2 (6)This ensures that a trajectory can always be found when

the vehicle is stationary, i.e. by rotating in place thenperturbing its position forward.

D. Trajectory Formulation

To generate trajectories, a parametrized trajectory gener-ator is used, which is similar to [4]. This has two tunableparameters, usp and Λ.

When the vehicle is moving, the trajectory initially con-sists of an acceleration chosen from the set:

u∗v(0|j) ∈ {min {usp, (vmax − v(k))} ,−min {usp, v(k)}}(7)

This is followed by a fixed deceleration of usp at allsubsequent time-steps until the vehicle halts.

The trajectories are constrained to consist of a sharp turn,followed by a straight section. The turning rate profile maybe described by the following equation:

u∗θ(j|k) = uθ,max · sgn(Λ(k)) · sat10(|Λ(k)| − j). (8)

The set of possible values of Λ are arbitrary, and aretunable parameters set by the designer.

IV. ANALYSIS

In this section, we provide some analysis of the proposedcontrol law.

Proposition 4.1: The actual distance from the obstaclealways exceeds d0.

Proof: This is trivial – any planned path is inside adisk of points is guaranteed to be safe. As we assumed nodisturbance in the vehicle model, there is no deviation fromthis path. •

Lemma 4.1: Suppose the vehicle is traveling at constantspeed b parallel to a straight obstacle boundary, which is adistance d1 away. If ∆dmeas is zero and the state estimatoris complete (i.e. there is an arbitrarily large number of par-ticles), then the range of possible values of αest approacheszero as the time-step ∆t approaches zero.

Proof: This argument is based on Figs. IV(a) and IV(b).In Fig. IV(b) it is easy to see that:

∆α = cos−1

(1− b2∆t2

2d21

)(9)

Thus ∆α tends to zero as ∆t tends to zero. •Remark 4.1: This argument is in fact conservative. In

fact there only exists tangential circles with curvature lessthan κmax, which are tangential to the subsequent detectioncircles. However, this leads towards more complex analysis,and the presented argument is sufficient to show the requiredproperty.

Proposition 4.2: If ∆dmeas is zero and the state estimatoris complete (i.e. there is an arbitrarily large number of par-ticles), then the range of possible values of αest approacheszero as the time-step ∆t approaches zero.

Remark 4.2: This conjecture is based on Fig. IV(a), andmakes intuitive sense. As the centers of the semicircularsegments approach each other, one would expect the rangeof possible values of α to approach a limit. Future work willfocus extending Lemma 4.1 to this more general case, andalso considering non-zero values of ∆dmeas.

Proposition 4.3: If the conditions from Lemma 4.2 aremet, there is zero error in the state estimate, and the pathplanner is complete‡; then the distance r of the nearest

‡If a path exists, then a path will be found.

339

(a) (b)

(c)

Fig. 4. (a) The range of possible values of αest is bounded; (b) A simplifiedversion where the obstacle and vehicle path are both straight and parralel;(c) By moving forward ddec, the progression along the obstacle increases.

point along the obstacle boundary monotonically increases.Furthermore this is at least rinc in the time jmax (wherejmax is the planning horizon).

Proof: Due to Assumption 3.1, if the vehicle is sta-tionary, it can perform a rotation until it is aligned perpen-dicularly to the nearest point, then it can reach equal andopposite longitudinal accelerations on subsequent time-stepsto cover the distance ddec. If the path planner is complete,then at least this path can be found.

Due to Assumption 2.1, we know the nearest point to theobstacle at the next time step is progressed compared to thecurrent nearest point, see Fig. (c). In the worst case, wherethe vehicle is dmax from the nearest point, and the obstaclehas curvature κmax away from the vehicle, ∆r will be givenby:

∆r = dob tan−1

(ddec

dmax + dob

)(10)

Any value of rinc less than the value of ∆r would suffice.•

V. SIMULATIONS

The control law was simulated using the perfect discretetime model, updated at a sample time of 0.1s (howevertrajectories were sampled using a slower period of 1.0s). Thecontrol parameters may be found in Figure 5. Simulationswere carried out on a 3.0 GHz Pentium processor usingMATLAB interfaced with C++ mex files.

It can be seen in Figs. 7 and 8 that the obstacle wassuccessfully transversed. In particular, it may be seen thatthe vehicle slowed down appropriately to transverse concavecorners.

Fig. 6 shows a typical set of projected boundary segments,and it can be seen these span the vicinity quite well. Theaverage time taken to compute each update of the control

∆t 1.0s

uθ,max 0.8rads−1

uv,nom 0.4ms−2

vmax 0.8ms−1

d0 0.3m

∆dmeas 0.4m

npart 5000

dstep 0.2m

dhor 2m

κmax 5radm−1

κsd 5radm−2

Λ ±{0.5k}, k = 0 : 5

ddes 1.4m

dmax 10m

γ1 100

γ2 200

Fig. 5. Control parameters used for simulations

Fig. 6. A set of fairly typical projected obstacle boundary which wasgenerated online by the algorithm. The two circles represents the boundscorresponding to the sensor error.

law was approximately 0.1s, but it is expected that this couldbe improved with more efficient implementations. Duringsimulations, there was always a non-zero number of validparticles; the risk of having no particles could be mitigatedby increasing the number of particles.

VI. CONCLUSIONS

In this paper, a method for navigating a robot along anobstacle boundary was proposed when only limited infor-mation about the nearest obstacle point is available. Usingparticle filter concepts, we generate a number of hypothesesabout the obstacle boundary, and update these as more sensorinformation becomes available. Using local path planning,we are able to plan a trajectory towards the edge of the pointsknown to be safe. This allows the obstacle offset and vehiclespeed in accordance with the changing boundary of theobstacle. Computer simulations and real world experimentsconfirm the method’s validity. Future work will validate thecontrol law with experiments in more complex environments.

REFERENCES

[1] A. Bain and D. Crisan. Fundamentals of Stochastic Filtering, vol-ume 60 of Stochastic Modelling and Applied Probability. Springer,2009.

[2] S. S. Ge, X. Lai, and A. A. Mamun. Boundary following and globallyconvergent path planning using instant goals. IEEE Transactions onSystems, Man, and Cybernetics, Part B: Cybernetics, 35(2):240–254,2005.

[3] S. S. Ge, X. Lai, and A. A. Mamun. Sensor–based path planning fornonholonomic mobile robots subject to dynamic constraints. Roboticsand Autonomous Systems, 55(7):513–526, 2007.

[4] M. Hoy. A method of boundary following by a wheeled mobile robotbased on sampled range information. In Journal of Intelligent andRobotic Systems, volume 72, pages 463–482, 2013.

340

(a)

0 10 20 30 40 500

0.5

1

Vel

(m

s−1 )

0 10 20 30 40 50−1

0

1

Rot

(s−

1 )

0 10 20 30 40 501

2

3

Dis

(m

)

Time (s)

(b)

Fig. 7. Simulations with a relatively simple obstacle.

[5] M. Hoy, A. S. Matveev, and A. V. Savkin. Collision free cooperativenavigation of multiple wheeled robots in unknown cluttered environ-ments. Robotics and Autonomous Systems, 60(10):1253–1266, 2012.

[6] M. Hoy, A. S. Matveev, and A. V. Savkin. Algorithms for collisionfree navigation of mobile robots in complex cluttered environments:A survey. Robotica [accepted], 2013.

[7] X.-L. Hu, T. B. Schon, and L. Ljung. A basic convergence re-sult for particle filtering. IEEE Transactions on Signal Processing,56(4):1337–1348, 2008.

[8] J. Kim, F. Zhang, and M. Egerstedt. Curve tracking control forautonomous vehicles with rigidly mounted range sensors. Journalof Intelligent and Robotic Systems, 56(2):177–197, 2009.

[9] R. A. Langer, L. S. Coelho, and G. H. Oliveira. K–bug–a new bugapproach for mobile robot’s path planning. In Proceedings of theIEEE International Conference on Control Applications, pages 403–408, Singapore, 2007.

[10] E. M. Low, I. R. Manchester, and A. V. Savkin. A biologically inspiredmethod for vision–based docking of wheeled mobile robots. Roboticsand Autonomous Systems, 55(10):769–784, 2007.

[11] V. J. Lumelsky and T. Skewis. Incorporating range sensing in therobot navigation function. IEEE Transactions on Systems, Man andCybernetics, 20(5):1058–1069, 1990.

[12] A. S. Matveev, M. Hoy, and A. V. Savkin. A method for reactivenavigation of nonholonomic robots in maze–like environments. Auto-matica, 49(5):1268–1274, 2013.

[13] A. S. Matveev, M. Hoy, and A. V. Savkin. The problem of boundaryfollowing by a unicycle–like robot with rigidly mounted sensors.Robotics and Autonomous Systems, 61(3):312–327, 2013.

[14] A. S. Matveev, H. Teimoori, and A. V. Savkin. A method for guidanceand control of an autonomous vehicle in problems of border patrollingand obstacle avoidance. Automatica, 47(3):515–514, 2011.

[15] A. S. Matveev, H. Teimoori, and A. V. Savkin. Navigation ofa unicycle–like mobile robot for environmental extremum seeking.Automatica, 47(1):85–91, 2011.

(a)

0 50 100 150 200 250 300 3500

0.5

1

Vel

(m

s−1 )

0 50 100 150 200 250 300 350−1

0

1

Rot

(s−

1 )

0 50 100 150 200 250 300 3501

2

3D

is (

m)

Time (s)

(b)

Fig. 8. Simulations with a more complex obstacle.

[16] A. S. Matveev, H. Teimoori, and A. V. Savkin. Range–only measure-ments based target following for wheeled mobile robots. Automatica,47(1):177–184, 2011.

[17] A. S. Matveev, H. Teimoori, and A. V. Savkin. Method for trackingof environmental level sets by a unicycle–like vehicle. Automatica,48(9):2252–2261, 2012.

[18] A. S. Matveev, C. Wang, and A. V. Savkin. Real–time navigation ofmobile robots in problems of border patrolling and avoiding collisionswith moving and deforming obstacles. Robotics and AutonomousSystems, 60(6):769–788, 2012.

[19] J. Ng and T. Braunl. Performance comparison of bug navigationalgorithms. Journal of Intelligent and Robotic Systems, 50(1):73–84,2007.

[20] A. V. Savkin and C. Wang. A simple biologically inspired algorithmfor collision-free navigation of a unicycle-like robot in dynamicenvironments with moving obstacles. Robotica, 31(6):993–1001, 2013.

[21] A. M. Shkel and V. J. Lumelsky. Incorporating body dynamics intosensor–based motion planning: the maximum turn strategy. IEEETransactions on Robotics and Automation, 13(6):873–880, 1997.

[22] H. Teimoori and A. V. Savkin. A biologically inspired method forrobot navigation in a cluttered environment. Robotica, 28(5):637–648,2010.

[23] H. Teimoori and A. V. Savkin. Equiangular navigation and guidance ofa wheeled mobile robot based on range–only measurements. Roboticsand Autonomous Systems, 58(2):203–215, 2010.

[24] J. M. Toibero, F. Roberti, and R. Carelli. Stable contour–followingcontrol of wheeled mobile robots. Robotica, 27(1):1–12, 2009.

341