By adaptingthe gain, k1 , instability of the controller is avoided: Control Law — Servo to x = 0 On reaching the y-axis and θ = 0, the following control law on the vehicle’s velocity is
Trang 1S Yuta et al (Eds.): Field and Service Robotics, STAR 24, pp 61–70, 2006.
© Springer-Verlag Berlin Heidelberg 2006
Landmark-Based Nonholonomic Visual Homing
Kane Usher1,2, Peter Corke1, and Peter Ridley2
P.O Box 883, Kenmore, 4069, Queensland, Australia
{firstname}.{surname}@csiro.au
http://www.cat.csiro.au/cmst/automation
Queensland University of Technology
Brisbane, 4001, Queensland, Australia
Abstract In this paper, we present a method which allows pose stabilization of a car-like
vehicle to a learnt location based on feature bearing angle and range discrepancies betweenthe vehicle’s current view of the environment, and that at the learnt location We then extendthe technique to include obstacle avoidance Simulations and experimental results using ouroutdoor mobile platform are presented
1 Introduction
In order to perform useful tasks, a mobile robot requires the ability to servo toparticular poses in the environment For the nonholonomic, car-like vehicle used inthese experiments, Brockett [2], showed that there is no smooth, continuous feedbackcontrol law which can locally stabilise such systems
Insects in general display amazing navigation abilities, traversing distances farsurpassing the best of our mobile robots on a relative scale Evolution has providedinsects with many ‘shortcuts’ enabling the achievement of relatively complex taskswith a minimum of resources in terms of processing power and sensors [12] The high
ground temperatures encountered by the desert ant, cataglyphis bicolor, eliminates
pheromones as a potential navigation aid, as is used by ants in cooler climates [6].The desert ant navigates using a combination of path integration and visual homing.Visual homing is the process of matching an agent’s current view of a location
in a distinctive locale to a (pre-stored) view at some target position Discrepanciesbetween the two views are used to generate a command that drives the agent closer
to the target position The process enables the agent to ‘find’ positions in distinctivelocales When applied to nonholonomic mobile robots, the constraints of Brockett’stheorem prevent the insect-based strategies from completely resolving the posestabilization problem; they enable servoing to a position but cannot guarantee aparticular orientation (see e.g [6,12])
In the control community, the problem of stabilising a mobile robot to a specificpose has generally been approached from two directions; the open-loop and closed-loop strategies Open-loop strategies seek to find a bounded sequence of controlinputs, driving the vehicle from an initial pose to some arbitrary goal pose, usuallyworking in conjunction with a motion planner (e.g [7,9]) Finding this sequence of
Trang 262 K Usher, P Corke, and P Ridley
control inputs is difficult for the nonholonomic case and, in the event of disturbances,
a new plan has to be formulated The closed-loop strategies consist of designing afeedback loop using proprioceptive and exteroceptive sensors to provide estimates ofthe vehicle’s state Feedback control systems are generally more robust to uncertaintyand disturbances when compared to their open-loop counterparts All real mobilerobots and sensors are subject to noise and uncertainty — feedback control wouldthus seem essential Feedback pose stabilization for under-actuated nonholonomicsystems is addressed with either discontinuous control techniques (see e.g [1]),time-varying control (see e.g [10]), or combinations thereof
Much of the literature does not address what has been found in this study to be
a significant limitation of many control algorithms — input saturation Further tothis, many of the algorithms in the literature cannot cope with systems which havesignificant velocity and steering loop dynamics In fact, there are few instances ofactual implementations of pose control to a car-like vehicle (an exception is Fraichardand Garnier [4] who use fuzzy control techniques applied to a small electric car)
In this paper we develop a discontinuous feedback control strategy, showing that
it can be used in combination with a visual sensor in a system which has significantvelocity and steering loop dynamics The remainder of this paper is arranged asfollows: Section 2 details the switching control technique and our version of visualhoming; Section 3 describes our experimental system and briefly outlines somepreliminary results; Section 4 outlines some preliminary results in pose stabilizationwith obstacle avoidance; and Section 5 concludes the paper
2 Control Strategy
In this section, the switching control strategy presented by Lee et al [8] is developedand an instability is corrected We then show how a derivative of the insect-inspiredAverage Landmark Vector model of navigation presented by Lambrinos et al [6]can be used to provide the required quantities to the switching controller
2.1 Kinematics
Car-like vehicle kinematics are usually represented using the bicycle model
Refer-ring to Fig 1, the kinematics of our experimental vehicle are:
˙x = v cos θ
˙θ = vtan φ
L
where v is the vehicle’s forward velocity (measured at the centre of the rear axle),
L is the vehicle’s length, φ is the steering angle, and the point (x, y) refers to thecentre of the rear axle
Trang 3Landmark-Based Nonholonomic Visual Homing 63
y
φ
θ v L
control − servo to x = 0
control − servo to x−axis
x
y WORKSPACE
(b) Illustration of the controlstrategy
Fig 1 The car-like vehicle system and the stages of control.
2.2 Control Law Development
Lee et al [8] use discontinuous control to sidestep Brockett’s theorem, breakingthe stabilization task into two stages Without loss of generality, consider the goalpose to be (x, y, θ) = (0, 0, 0) The initial stage involves minimising y and θ; i.e.the vehicle converges to the x-axis with an orientation of 0 The second stage thenmoves the vehicle along the x-axis to the desired point In practice, we have foundthe need for an additional stage which helps to minimise the space required to servo
to a pose A discrete event supervisor decides which stage of control is in effect
Control Law — Converge to x-axis Using a suitably chosen Lyapunov function,
Trang 464 K Usher, P Corke, and P Ridley
which in turn leads to instability when the vehicle is far from the x-axis By adaptingthe gain, k1 , instability of the controller is avoided:
Control Law — Servo to x = 0 On reaching the y-axis and θ = 0, the following
control law on the vehicle’s velocity is invoked:
bringing the vehicle to the desired pose of (x, y, θ) = (0, 0, 0) In the original work
of Lee et al., the steering angle was set to zero in this stage of control We havefound that in practice, due to the imperfections of ‘real’ systems, it is necessary tocontrol the steering angle — we again use equation 2 An analysis of the Lyapunovfunction for this stage of control shows that convergence is still guaranteed
Control Law — Turn Stage Although the velocity in the ‘converge to x-axis’ stage
of control is chosen according to the vehicle’s initial orientation with respect to
‘home’, this does not always guarantee that in converging to the x-axis, the vehiclewill make efficient use of the workspace If using landmarks to position the vehicle,
a wide area of operation may result in landmarks disappearing from the vehicle’sperception horizon Thus, we have added a further stage to the controller, whichturns the vehicle towards the goal Again, the controller is based upon a Lyapunovformulation, this time with the aim of minimising the goal’s orientation relative tothe vehicle Once a threshold is reached, the ‘converge to x-axis’ stage is invoked
Discussion In combination, these control laws stabilise the vehicle to the desired
pose from any initial condition Again in practice, the imperfections of ‘real’ systemsrequires that switching to the ‘servo to x = 0’ stage of control occurs when the y and
θ errors drop below pre-specified thresholds
Many of the pose stabilization algorithms with which we experimented failed inthe face of input saturation on the steering angle and could not cope with actuatordynamics in the velocity and steering loops We believe that this method is successfulbecause it does not attempt to resolve pose in one step — in servoing to the x-axisfirst, the dynamic response of the vehicle becomes less important, as does steeringinput saturation On reaching the x-axis with the correct orientation, the dynamicsare less important and in general, smaller steering inputs are required and saturation
is avoided
The control technique presented in this section assumes that the pose of thevehicle is known In the next section, we detail a landmark-based vision techniquewhich yields pose estimates in an extremely simple, yet surprisingly robust, mannerusing an omnidirectional camera and a compass
Trang 5Landmark-Based Nonholonomic Visual Homing 65
2.3 Ant Navigation
An elegant, correspondence free, homing method developed from hypotheses on antvisual homing is the Average Landmark Vector model [6] An ALV for any particularposition is found by summing unit vectors towards all currently visible landmarksand dividing by the number of landmarks By matching the current ALV with apre-stored ALV of the target location, a homing vector can be formed which drivesthe robot towards the target location [6] In order to consistently add the vectors inthe ALV model, an absolute reference direction is required, and, unless apparent sizeinformation is incorporated, a minimum of three landmarks is needed
We have found that by using range vectors to the individual landmarks, rather than unit vectors, the distance and angle to the goal are yielded directly In addition,
the minimum required landmarks is reduced to one An example of the IALV method
is shown in Fig 2 In essence, the IALV method is equivalent to finding a positionrelative to the centroid of the landmarks in the workspace
δ current position
Fig 2 Illustration of the the IALV method for n = 2 landmarks in a workspace.
As with the ALV method, the IALV method is sensor-based Landmark bearingsare readily ascertained with an omnidirectional camera If a flat-earth assumption ismade, range information can be derived from such an image through the geometry
of the camera/mirror optics One of the advantages of the ALV, and hence the IALVmethod, is that knowledge of a target location is contained within a single quan-tity This reduces the need for complex map-like representations of the environmentand is well suited for a topological navigation method, (see e.g [5]) Additionally,landmarks need not be unique, and the need for landmark correspondence is alsobypassed Of course, like all sensor-based techniques, this method has a finite catch-
Trang 666 K Usher, P Corke, and P Ridley
ment area, limited by the omnidirectional sensor’s range and, in addition, has thepotential to suffer from perceptual aliasing, or in a similar sense, the local minimaproblem
The homing vector provided by the IALV method can be used to drive the agenttowards home but does not provide a means of guaranteeing a final orientation.However, the quantities derived from the IALV can easily be converted to the statesrequired by the switching controller presented in Section 2
2.4 Simulations
The model of the vehicle consists of the kinematic equations (Equation 1) anddynamic models of the steering and velocity loops (identified as first and second-order respectively) Also included in the model are input saturation and rate limiting
A simulation of the omnidirectional sensor (with random noise) provides the IALV,from which the required states are derived and fed to the switching controller Fig 3shows the path generated and control inputs for a starting pose of (x, y, θ) = (0, 3, 0)and a goal pose of (0, 0, 0) Gains were set to (k1 , k2 , k3 ) = (0.35, 0.1, 0.1) for thesesimulations, based on a root locus analysis of the linearised system The methodworks for all starting and goal poses
Simulated controller demands
(a) Demanded inputs
−3
−2
−1 0 1 2 3 4 5
home position start position
x−axis (m)
Simulated robot pose throughout journey
(b) Vehicle’s path
Fig 3 Results of the simulation for a starting pose of (x, y, θ) = (0, 3, 0).
The method has been extensively tested in simulation and found to be very robust
to input saturation and noise The next section presents some experimental resultswhich validate our simulations
3 Experiments
Experimental validation of this visual servoing technique was conducted on ouroutdoor research testbed In these experiments, artificial landmarks in the form
Trang 7Landmark-based Nonholonomic Visual Homing 67
of red traffic cones (witches hats) were used The following sections give a briefoverview of the vehicle and an outline of the image processing used to extract thelandmarks
3.1 Robotic Testbed
The experimental platform is a Toro ride-on mower which has been retro-fitted withactuators, sensors and a computer system, enabling control over the vehicle’s opera-tions All computing occurs on-board The vehicle is fitted with an array of sensorsincluding odometry, GPS, a magnetometer, a laser range-finder (for collision avoid-ance only) and an omnidirectional camera (see Fig 4) For these experiments, theprimary sensor used is the omnidirectional camera with the magnetometer providing
an absolute reference direction
Fig 4 The experimental platform.
3.2 Image Processing
In testing our controller, we use artificial landmarks in the form of bright red roadcones (also known as witches hats) We use colour segmentation to find the land-marks, estimate landmark range based upon a geometric model of the camera / mirrorsystem and a flat-Earth assumption, track the landmarks using a vehicle/landmarkrelative motion model, and improve the quality of the measurements using comple-mentary filters which combine vehicle odometry with the vision and compass data.For a complete description of the sensing system see [11]
3.3 Results and Discussion
The testing arena used for these experiments is a large shed The vehicle was placed
in the middle of the shed defining the goal pose (x, y, θ) = (0, 0, 0) A ‘landmark’was then placed at (x, y) = (5.85, −1), and the target IALV was found and stored
Trang 868 K Usher, P Corke, and P Ridley
The vehicle was then manually driven to (x, y, θ) = (0, 3, 0), and the control systemactivated Fig 5 depicts the results of the experiment, showing the vehicle servoing
to the goal pose based on vision and compass data alone The similarity to thesimulation results plotted in Fig 3 is quite clear, although the experimental systemdid take longer to stabilise to the desired pose This could be due to the quite coarseon-board velocity measurements and the noisy pose estimates
Experimental controller demands
(a) Demanded inputs
−3
−2
−1 0 1 2 3 4 5
home position
start position Experimental robot pose throughout journey (from odometry data)
means of stabilising to a pose and avoiding obstacles in most obstacle configurations,
assuming that in the pathological cases, a higher level of control could be invoked.Our approach is similar to that used by Fox et al [3] At each time step, thecurrent demands to the vehicle are used to predict the trajectory of the vehicle, givenknowledge of the kinematics and dynamics of the vehicle In addition, an obstaclemotion model is used to predict the relative position of each of the currently detectedobstacles along the trajectory If the trajectory passes too close to an obstacle,then it is flagged as unsuitable At this stage, a loop is entered in which trajectoriesresulting from every available steering angle (quantized at 5◦intervals) at the currentvelocity are evaluated From the suitable evaluated trajectories, the one with a steeringcommand closest to the original demand is selected If none is available, then thevehicle’s velocity command is iteratively reduced and the trajectories reevaluated If
Trang 9Landmark-Based Nonholonomic Visual Homing 69
suitable commands are still not found, the control level is notified and a temporary,intermediate target is selected in a direction in reverse to the current travel direction.The vehicle then ‘homes’ to this temporary target point, re-targeting if necessary
On reaching the temporary target point, the original goal pose is re-acquired.Figure 6 illustrates a simulation of pose stabilization with obstacle avoidance.Note that the vehicle initially detects that it cannot move forward and reassigns thetarget to the rear of the vehicle Once this target is acquired the original target isre-acquired
(a) Demanded inputs
home position start position
We have presented simulations and real experimental results showing the validity
of our approach even in the face of input saturation and noise In addition, we havepresented preliminary results in pose stabilization with obstacle avoidance Futurework includes further experimental validation of the method, along with a deeperunderstanding of the impact of dynamic elements in the control loops Finally,
Trang 1070 K Usher, P Corke, and P Ridley
this technique is ideally suited to topological navigation, and this too represents adirection for future research
Acknowledgements
The authors thank the automation team for their invaluable assistance In particular,the support of the CMIT Tractor Team — Peter Hynes, Stuart Wolfe, Stephen Bros-nan, Graeme Winstanley, Pavan Sikka, Elliot Duff, Jonathan Roberts, Les Overs,Craig Worthington and Steven Hogan — is gratefully acknowledged JonathonO’Brien at UNSW is gratefully thanked for the loan of the Toro ride-on mower
References
1 Michele Aicardi, Giuseppe Casalino, Antonio Bicchi, and Aldo Balestrino Closed
loop steering of unicycle-like vehicles via Lyapunov techniques IEEE Robotics and
Automation Magazine, pages 27–35, March 1995.
2 R.W Brockett Asymptotic stability and feedback stabilization In R W Brockett,
R S Millman, and H J Sussman, editors, Differential Geometric Control Theory, pages
181–191 Birkhauser, Boston, USA, 1983
3 D Fox, W Burgard, and S Thrun The dynamic window approach to collision avoidance
IEEE Robotics and Automation Magazine, v(1), 1997.
4 Th Fraichard and Ph Garnier Fuzzy control to drive car-like vehicles Robotics and
Autonomous Systems, 34:1–22, 2001.
5 Benjamin Kuipers and Yung-Tai Byun A robot exploration and mapping strategy based
on a semantic hierarchy of spatial representations Robotics and Autonomous Systems,
8:47–63, 1991
6 Dimitrios Lambrinos, Ralf M¨oller, Thomas Labhart, Rolf Pfiefer, and Rudiger Wehner
A mobile robot employing insect strategies for navigation Robotics and Autonomous
Systems, 30:39–64, 2000.
7 Jean-Claude Latombe Robot Motion Planning Kluwer Academic, 1991.
8 Sungon Lee, Manchul Kim, Youngil Youm, and Wankyun Chung Control of a
car-like mobile robot for parking problem In International Conference on Robotics and
Automation, pages 1–6, Detroit, Michigan, 1999 IEEE.
9 Richard M Murray and S Shankar Sastry Nonholonomic motion planning: Steering
using sinusoids IEEE Transactions on Automatic Control, 38(5):700–716, May 1993.
10 C Samson and K Ait-Abderrahim Feedback control of a non-holonomic wheeled cart
in cartesian space In International Conference on Robotics and Automation, pages
1136–1141, Sacramento, California, USA, April 1991 IEEE
11 Kane Usher, Matthew Dunbabin, Peter Corke, and Peter Ridley Sensing for visual
hom-ing In Proceedings of the 2003 Australasian Conference on Robotics and Automation,
Brisbane, Australia, December 2003 published via CDROM
12 Keven Weber, Svetha Venkatesh, and Mandyam Srinivasan Insect-inspired robotic
hom-ing Adaptive Behavior, 7(1):65–97, 1999.
Trang 11Recursive Probabilistic Velocity Obstacles for
Reflective Navigation
Boris Kluge and Erwin Prassler
Research Institute for Applied Knowledge Processing
Helmholtzstr 16, 89081 Ulm, Germany
{kluge, prassler}@faw.uni-ulm.de
http://www.faw.uni-ulm.de
Abstract An approach to motion planning among moving obstacles is presented, whereby
obstacles are modeled as intelligent decision-making agents The decision-making processes
of the obstacles are assumed to be similar to that of the mobile robot A probabilistic extension
to the velocity obstacle approach is used as a means for navigation as well as modelinguncertainty about the moving obstacles’ decisions
1 Introduction
For the task of navigating a mobile robot among moving obstacles, numerous proaches have been proposed previously However, moving obstacles are most com-monly assumed to be traveling without having any perception or motion goals (i.e.collision avoidance or goal positions) of their own In the expanding domain ofmobile service robots deployed in natural, everyday environments, this assumptiondoes not hold, since humans (which are the moving obstacles in this context) doperceive the robot and its motion and adapt their own motion accordingly There-fore, reflective navigation approaches which include reasoning about other agents’navigational decision processes become increasingly interesting
ap-In this paper an approach to reflective navigation is presented which extends thevelocity obstacle navigation scheme to incorporate reasoning about other objects’perception and motion goals
1.1 Related Work
Some recent approaches (see for example [3,5]) use a prediction of the future motion
of the obstacles in order to yield more successful motion (with respect to travel time orcollision avoidance) However, reflective navigation approaches are an extension ofthis concept, since they include further reasoning about perception and navigationalprocesses of moving obstacles
The velocity obstacle paradigm [2] is able to cope with obstacles moving onstraight lines and has been extended [6] for the case of obstacles moving on arbitrary(but known) trajectories
Modeling other agents’ decision making similar to the own agent’s decisionmaking is used by the recursive agent modeling approach [4], where the own agent
S Yuta et al (Eds.): Field and Service Robotics, STAR 24, pp 71–79, 2006.
© Springer-Verlag Berlin Heidelberg 2006
Trang 1272 B Kluge and E Prassler
bases its decisions not only on its models of other agents’ decision making processes,but also on its models of the other agents’ models of its own decision making, and
so on (hence the label recursive).
1.2 Overview
This paper is organized as follows: The basic velocity obstacle approach is introduced
in Section 2, and its probabilistic extension is presented in Section 3 Now being able
to cope with uncertain obstacle velocities, Section 4 describes how to recursivelyapply the velocity obstacle scheme in order to create a reflective navigation behavior
An implementation of the approach and an experiment are given in Section 5 Afterdiscussing the presented work in Section 6, Section 7 concludes the paper
2 Velocity Obstacle Approach
Let Biand Bjbe circular objects with centers ciand cjand radii riand rj, movingwith constant velocities vi= ˙ciand vj= ˙cj To decide if these two objects are on acollision course, it is sufficient to consider their current positions together with theirrelative velocity vij = vi− vj, see Fig 1 Let
Fig 1 Collision cone and velocity obstacle
Therefore we can define a set of colliding relative velocities, which is called the
collision cone CCij, as
Trang 13Recursive Probabilistic Velocity Obstacles for Reflective Navigation 73
In order to be able to decide if an absolute velocity viof Bileads to a collisionwith Bj, we define the velocity obstacle of Bjfor Bito be the set
For any velocity vi /∈ VOi, object Biwill not collide with any other object.Finally, a simple navigation scheme based on velocity obstacles (VO) can beconstructed as following The moving and non-moving obstacles in the environmentare continuously tracked, and the corresponding velocity obstacles are repeatedlycomputed In each cycle, a velocity is chosen which avoids collisions and approaches
a motion goal, for example a maximum velocity towards a goal position
3 Probabilistic Velocity Obstacles
The velocity obstacle approach as presented in the preceeding section can be tended to deal with uncertainty in shape and velocity of the objects This allows toreflect the limitations of real sensors and object tracking techniques
ex-3.1 Representing Uncertainty
Uncertainty in the exact shape of an object is reflected by uncertainty in the
cor-responding collision cone Therefore, we define the probabilistic collision cone of
object Bjrelative to object Bito be a function
Trang 1474 B Kluge and E Prassler
With these two definition, we get the probabilistic velocity obstacle of object Bjrelative to object Bias a function
3.2 Probabilistic Velocity Obstacle
The probability of Bicolliding with any other obstacle when traveling with ity viis the probability of not avoiding collisions with each other moving obstacle
veloc-Therefore we may define the probabilistic velocity obstacle for Bias the functionPVOi= 1 −
j= i
3.3 Navigating with Probabilistic VO
In the deterministic case, navigating is rather easy since we consider only collisionfree velocities and can choose a velocity which is optimal for reaching the goal Butnow, we are confronted with two objectives: reaching a goal and minimizing theprobability of a collision
Let Ui : R2 → [0, 1] a function representing the utility of velocities vifor themotion goal of Bi However, the full utility of a velocity viis only attained if (a) viisdynamically reachable, and (b) viis collision free Therefore we define the relativeutility function
where Di: R2 → [0, 1] describes the reachability of a new velocity
Now a simple navigation scheme for object Bi based on probabilistic velocityobstacles (PVO) is obtained by repeatedly choosing a velocity viwhich maximizesthe relative utility RUi
4 Recursive Probabilistic VO
In contrast to traditional approaches, recursive modeling for mobile robot navigation
as presented in this paper presumes moving obstacles to deploy navigational decision
Trang 15Recursive Probabilistic Velocity Obstacles for Reflective Navigation 75
making processes similar to the approach used by the robot This means any object Bj
is assumed to take actions maximizing its relative utility function RUj Therefore,
in order to predict the action of obstacle Bj, we need to know its current utilityfunction Uj, dynamic capabilities Dj, and velocity obstacle PVOj
The utility of velocities can be inferred by recognition of the current motiongoal of the moving obstacle For example, Bennewitz et al [1] learn and recognizetypical motion patterns of humans If no global motion goal is available throughrecognition, one can still assume that there exists such a goal which the obstaclestrives to approach, expecting it to be willing to keep its current speed and heading
By continuous observation of a moving obstacle it is also possible to deduce a model
of its dynamics, which describes feasible accelerations depending on its currentspeed and heading Further details about aquiring models of velocity utilities anddynamic capabilities of other objects are beyond the scope of this paper
Finally, the velocity obstacle PVOj for object Bj is computed in a recursivemanner, where predicted velocities are used in all but the terminal recursion step
(14)
expresses the assumption that objects will move according to their relative utilityfunction Probabilistic velocity obstacles PVOdjof depth d ≥ 0 are computed in theobvious way from depth-d models of other objects’ velocities
Computational demands will increase with the depth of the recursion, but, itively, one does not expect recursion depths of more than two or three to be of broadpractical value, since such deeper modeling is not observed when we are walking ashuman beings among other humans
intu-4.2 Navigation with Recursive PVO
To navigate a mobile robot Bi using depth-d recursive probabilistic velocity stacles, we repeatedly choose a velocity vi maximizing RUdi For d = 0, we get
ob-a behob-avior thob-at only obeys the robot’s utility function Ui and its dynamic bilities Di, but completely ignores other obstacles For d = 1, we get the plainprobabilistic velocity obstacle behavior as described in Section 3 Finally, for d > 1,the robot starts modeling the obstacles as perceptive and decision making
Trang 16capa-76 B Kluge and E Prassler
5 Implementation
The implementation is given by Algorithm 1 Recursive function calls are not used,the models are computed starting from depth zero up to a predefined maximumdepth
Algorithm 1 RPVO(depth r, n objects)
5.1 Complexity
We begin the complexity assessment by measuring the sizes of the supporting sets
of the discretized functions used in Algorithm 1 Line 4 implies
Trang 17Recursive Probabilistic Velocity Obstacles for Reflective Navigation 77
for d > 0 Line 3 implies
for d > 0, using the three preceding Equations
Now we count the numbers of operations used in the algorithm, which we writedown using Ni = |σ(Di) ∪ σ(Vi)| Line 8 can be implemented to use O(Ni·j= iNj) operations Lines 9, 11, and 13 each require O(Ni) operations, and arethus dominated by line 8 Therefore the loop starting in line 7 requires
in this situation
Another example is the situation where a fast object approaches a slower onefrom behind, i.e where overtaking is imminent, which is depicted by Fig 3 If theslow object B (depth 2) expects the fast object A (depth 1) from behind to avoidcollisions, it mostly stays in its lane (Fig 3(a)) On the other hand, if object B (nowdepth 3) assumes other objects to expect it to avoid collisions, a rather defensivedriving style is realized (Fig 3(b))