Collision Avoidance for Nonholonomic Mobile Robots ...

5 downloads 0 Views 541KB Size Report
algorithms in the first category (for examples see [1]-[9]) directly provide the collision free path(s) of the robot(s) by assuming the shapes of obstacles are known ...
Please cite this final version: Zeng, L. and Bone, G. M., "Collision Avoidance for Nonholonomic Mobile Robots among Dynamic Obstacles including Humans", Proceedings of the 2010 IEEE International Conference on Automation Science and Engineering , Toronto, Canada, May 2010, pp. 940-947. DOI: 10.1109/COASE.2010.5584243

Collision Avoidance for Nonholonomic Mobile Robots Among Unpredictable Dynamic Obstacles Including Humans Lingqi Zeng and Gary M. Bone

Abstract In many service applications, mobile robots need to share their work areas with obstacles. Avoiding collisions is a fundamental requirement for these robots. In this paper a novel collision avoidance system is developed for avoiding unpredictable dynamic obstacles, including humans. The collision avoidance algorithm is based on the virtual force field (VFF) concept. The velocities of the obstacles are used in addition to their positions to improve the avoidance performance for dynamic obstacles. Unlike prior algorithms, the proposed VFF is designed to be continuous to diminish both path oscillations and the time cost for reaching the goal. To further reduce the time cost, a new virtual force (termed the detour force) is introduced. The detour force also solves the challenging avoidance problem that occurs when the centers of the robot, human/obstacle and goal are collinear; and the human/obstacle and robot are moving towards each other. In simulations and experiments with a maximum approach velocity of 1.7 m/s, the avoidance system with the new VFF algorithm generates collision-free paths with less oscillation and lower time cost. I. INTRODUCTION In many service applications, mobile robots need to share their work areas with obstacles including humans. Examples include: delivering the specimens in a hospital, and cleaning floors in an office. Avoiding collisions with humans and obstacles is a fundamental requirement for those robots. The objective of avoidance is to plan and control the motion of the robot from its initial position to its goal position while avoiding obstacles. The collision avoidance problem for mobile robots has been the focus of tremendous research effort recently. Most of the research involved avoiding static obstacles or dynamic obstacles whose motions are, predictable such as other mobile robots. Humans are common

dynamic obstacles for service applications in industrial, office and domestic environments. However, avoiding humans is different from avoiding predictable obstacles, and poses the following additional challenges: - Human motion is unpredictable, changing its speed or direction arbitrarily. Precise future position information of humans is unobtainable for use in avoidance algorithms. This makes the path planning of the avoidance system more difficult. - For safety, humans must always possess a higher priority in an avoidance system than robots or inanimate objects. Humans should be able to pursue their activities unhindered and without worrying about robots around them. Robots should always be aware of the presence of humans and avoid them. Because of their lower cost, nonholonomic robots are much more popular for service applications than holonomic robots. In this paper, the focus is on developing an avoidance algorithm for nonholonomic robots. See [16] for the definitions of nonholonomic and holonomic mobile robots. Currently, the majority of collision avoidance algorithms for mobile robots can be classified into two categories. The algorithms in the first category (for examples see [1]-[9]) directly provide the collision free path(s) of the robot(s) by assuming the shapes of obstacles are known and their motion is predictable. Furthermore, the robot(s) dynamics are neglected. The algorithms in the second category (for examples see [10]-[18]) indirectly generate the collision free path(s) by using the current kinematic and geometric information of the robot(s) and obstacles, such as the distances between obstacles and robot(s), and the current velocities of obstacles. These algorithms consider the robot(s) dynamics. Algorithms in the first category have the advantage that optimal collision free paths can be found with static obstacles (for examples see [3], [6] and [9]) and dynamic obstacles

Gary M. Bone is with the Department of Mechanical Engineering, McMaster University, Hamilton, ON, Canada. L8S 4L7. (e-mail: [email protected]). Lingqi Zeng is with the Department of Mechanical Engineering, McMaster University, Hamilton, ON, Canada. L8S 4L7.

whose motions are predictable (for examples see [1], [2], [4], [5], [7] and [8]). However, those algorithms do not work for moving obstacles like humans due to a lack of accurate position predictions. Belonging to this category, the concept of the collision map was introduced in [1] to avoid moving obstacles. The collision map shows the predicted collision relationship between the trajectories of the robot and moving obstacles. The trajectories of the robot can be chosen from the collision map with the objective of minimizing the arrival time (or time cost) of the robot to its goal. This approach was extended in [7] by incorporating fuzzy logic. No experimental results were included in [1] or [7]. Another path planning algorithm for nonholonomic robots was introduced in [2]. This algorithm first plans a path towards the goal, and then examines whether the robot will collide with any obstacle in its path. Future positions of moving obstacles are predicted by assuming the obstacles will move with zero acceleration. If the robot is predicted to collide with an obstacle, the path is bent to detour around the obstacle. After that, the new path is examined for other obstacles. By iteratively forecasting collisions and re-planning the path every sampling period of the sensing system, a collision-free path is obtained. Based on their simulation results, this algorithm can avoid dynamic obstacles whose velocities are less than 0.3 m/s. No experimental results were included. In [4] an algorithm for avoiding moving obstacles including humans was presented. They proposed the concept of ‘velocity obstacles’, defined as the set of all feasible velocity vectors of the robot that lead to a collision-free path. This is computed by assuming the velocities of the obstacles do not change during the next sampling period of the sensing system. The velocity vector closest to the velocity vector pointing from the robot towards the goal, and capable of keeping at least a predefined distance away from obstacles, was selected as a command to the robot. This distance defines a safe circular region around an obstacle such that the robot has sufficient space to avoid the obstacle. The authors derived this distance by assuming that the worst condition of the avoidance system is a moving obstacle accelerating with its maximum acceleration while approaching the robot. In fact, the worst case for an avoidance system is when the robot and obstacles are approaching with their maximum speed, because the robot then has the least time for avoidance. They did not present any experimental results. In [5], a complex motion planner was presented for avoiding dynamic obstacles. This planner iteratively builds a tree-shaped roadmap (i.e. an interconnected set of collision-free points) in the coordinates of the 3-D space consisting of the predicted robot X-Y position and the future time sequence. Those points are connected by the feasible trajectories that satisfy the kinematic and dynamic motion constraints, including the nonholonomic constraint. The roadmap is recomputed every sampling interval to avoid moving obstacles. Simulations and experiments with an air-cushioned robot were performed to validate the avoidance. The maximum velocity of the robot was 0.2 m/s. The

maximum velocities of the obstacles were less than or equal to 0.2 m/s, which is much slower than that of moving humans. In [8], a path optimization algorithm was proposed for mobile manipulators to avoid unpredictable obstacles. The performance measurement in their optimization is a combination of minimizing the energy consumption and the time cost to reach the robot’s goal. To avoid unpredictable dynamic obstacles, the planner generates a set of random collision-free trajectory candidates, and optimally selects one from them. Simulations of various environments were included that show the algorithm is flexible and able to avoid unpredictable dynamic obstacles. However, no experiments were included. The avoidance algorithms in the second category are based on artificial potential field (APF) (introduced in [10] in 1985) or virtual force field (VFF) (introduced in [11] in 1989). These algorithms are normally simple to implement, consume less computational load than those in the first category, and are suitable for applications requiring online or real-time avoidance. Since virtual forces can be obtained by using the gradient descent method on APFs, they are closely related and belong to the same category. These algorithms assume the existence of a repulsive artificial potential field or a repulsive virtual force surrounding each obstacle to push robots away from the obstacle. Another attractive potential field or virtual force is also assumed to be acting to pull the robot to reach the goal. These algorithms do not directly provide a collision-free path for a robot. The desired accelerations of the robot are given through the robot’s dynamic interactions with the virtual forces. The collision-free path can be obtained by integrating the accelerations with Euler's method. The VFF or APF based algorithms have been extensively studied for avoiding stationary obstacles (for examples see: [10]-[12] and [15]). In those algorithms, the force functions or the potential fields are only related to the distance from the robot to the goal and the distance to the obstacle(s). The VFF or APF based algorithms have also been applied to avoid dynamic obstacles (for examples see: [13][14][16][17]). These algorithms incorporate the velocities of the obstacles and robots in the force functions and potential fields to avoid dynamic obstacles. In [13], an ‘avoidability measure’, defined as a function of current distances from moving obstacles to the robot and the rate of change of the distance was introduced. The repulsive and attractive potential fields are built using this measure, and virtual forces are computed with the gradient descent method. An ‘active region’ (also used in [10]-[12] and [15]) surrounding each obstacle is used to define where the repulsive potential field from the obstacle is active. If the robot is inside an obstacle’s active region, the repulsive potential field is applied to repel the robot away from the obstacle and the attractive potential field may also be active to move the robot towards goal during avoidance. If the robot is outside the region, only the attractive potential field is applied to the robot to pull it towards the goal. The artificial potential field is divided into two parts by the boundary of the active

Robot moving away with the detour force Goal

Mobile Detour virtual force

Obstacle

Vo

robot

Vr

Attractive Repulsive virtual force Robot virtual force

The model of the obstacle

Active region

The goal

Fig.1. Collinear condition for collision avoidance. region. Their algorithm was demonstrated with several simulations. In [14], a virtual force that acts perpendicular to the repulsive force was added. This force helps the robot to detour around the obstacle and reduces the time cost for reaching the goal. Since this force helps the robot detour around the obstacle, we will term it the detour virtual force. They also utilized active regions to design their VFF. A problem with conventional VFFs, that we will term the collinear condition, was first described in [14]. With this condition, an obstacle is located between the robot and the goal, and the centers of the robot, obstacle and goal are collinear. The velocity directions of the robot and the obstacle are also on this line, as shown in Fig. 1. The attractive force and the repulsive force with conventional VFF algorithms will all be along this line. Hence, the robot either collides with the obstacle, or is pushed away so that it will never reach its goal. Obviously, the solution is to move the robot sideways. A non-zero detour force can satisfy this requirement, but the algorithm in [14] gives a zero detour force at the collinear condition. They also defined a region, that we term the critical region, that is very close to the obstacle. A robot may not have sufficient space to complete avoidance, so an alternative to VFF is needed inside this region. This issue was not addressed in [14]. Furthermore, severe path oscillations occur with the existing VFF algorithms whenever the robot, obstacle and goal locations are near to the collinear condition. VFF-based algorithms cannot be used with nonholonomic robots without special extensions. The virtual forces are typically defined as vectors in two-dimensional Cartesian T coordinates, i.e. Fv =  Fx Fy  . For nonholonomic robots, the force component perpendicular to the robot’s heading direction will be neglected since the robot cannot move sideways without turning first. As a consequence, the robot would maintain its previous moving direction rather than moving sideways to avoid the obstacles. To solve this problem, in [12][14] a steering command was added that equals a positive gain times the angle difference between the angle of the resultant force of the VFF and current steering angle of the robot. This method helps the robot achieve the avoidance. However, the robot's path can oscillate which increases the time cost. Another algorithm was introduced in [16] based on a dipolar inverse potential field. By modeling the moving obstacle as the position and velocity constraints and including the nonholonomic constraint, the robot control commands were optimally selected to minimize the difference between the feasible robot heading direction and the gradient

Fig. 2. Example of the path oscillation problem. The solid line is the path with a discontinuous VFF. The dashed line is the path with a continuous VFF. direction of the potential field. However, the gradient descent method can cause the robot path to oscillate. A time-varying coefficient is used in their potential field to solve this oscillation problem. They did not present any experimental results. Since the gradient descent method is not used, this source of oscillations does not exist with VFF methods. However, with VFF, due to the discontinuous virtual forces at the boundary of the active region of an obstacle, oscillations may also occur. An example is shown in Fig. 2. When the robot first enters into the active region (the disk region in this figure), the repulsive force activates and jumps to a large value with discontinuous VFF algorithms. After this large force pushes the robot out of the region, the attractive force directed towards the goal pulls the robot back into the region, and an oscillating path occurs. In [17], the attractive virtual force and the repulsive virtual force were calculated using the error between the heading angle of the nonholonomic robot and the angle to the goal and the distances from the robot to its goal and obstacles. This helps to produce a smooth collision free path for nonholonomic robots. At a speed of 5 m/s the robot successfully avoided two dynamic obstacles in their experiments. No solution was provided for the collinear condition. The VFF-based collision avoidance algorithm is suitable for avoiding dynamic obstacles since the velocities of the obstacles can be considered. However, the following problems remain unsolved to date: path oscillation, avoidance in the collinear condition, and the control of nonholonomic robots using the Cartesian VFF. The contributions of this paper are: 1) A new VFF is designed to avoid unpredictable obstacles including humans. The force field is continuous at the edges of the active regions of obstacles to diminish the path oscillations. 2) A detour force that reduces the time cost for reaching the goal is proposed. 3) The detour force also solves the problem caused by collinear condition. 4) A novel three-layer control system for operating a nonholonomic robot with the Cartesian VFF. The organization of the paper is as follows. In section II, we state our assumptions. The control system for the

nonholonomic robot is described in section III. In section IV, the sizes of the active and critical regions, and the virtual force functions are designed. Simulation and experimental collision avoidance results are shown in section V. First, a comparison with important prior VFF algorithms is presented. Next, simulations and experimental results for avoiding a moving human, and for avoiding a human and two stationary obstacles, are presented. Finally, conclusions are drawn in section VI. II. ASSUMPTIONS We assume the following are true: 1) The human(s) motion is restricted to common walking with a velocity, Vh , less than 1 m/s [18]. 2) The human(s) do not chase the robot. 3) The robot goal is stationary and outside of the active region(s) of the obstacle(s) for safety. III. NONHOLONOMIC ROBOT CONTROL SYSTEM

conventional controllers have been designed for this task. In this paper, the simple state-tracking controller from [19] is used. Next, υ and ω are transformed to the desired angular accelerations, velocities and positions ( θɶ1 and θɶ2 ) of the driven wheels of the robot through the robot’s kinematics. The wheel controllers generate the control signals ( u1 and u2 ) used to power the DC motors driving the wheels. A proportional plus derivative plus feedforward controller is employed for each motor. IV. THE VIRTUAL FORCE FIELD A. Sizes of active and critical regions The active and critical regions must be sized to handle the worst case. The worst case for avoiding an obstacle is when the obstacle and robot approach with their maximum velocities at the collinear condition. For this case, the robot has the least amount of time to avoid a head-on collision. We denote the active region as ℂ 2 , the critical region as ℂ 3 , and

The control system for the nonholonomic robot has the three levels presented in Fig. 3. The first level is the path planner. In the planner, the VFF, Fv , is generated from the current measured position(s) of the dynamic obstacle(s), the position(s) of the static obstacle(s) and the measured robot position, Pr , using the equations presented in section IV. Further details on the vision system used to measure the positions are provided in section V. Note that Fig. 3 only shows the case with one human as the dynamic obstacle whose measured position is Ph . Fv cannot be applied directly to the nonholonomic robot dynamics since the sideways force component would be ignored. To include this force component, we apply Fv to the dynamic equations of a reference holonomic robot. As in [14], the holonomic dynamics can be defined as: a r = Fv mr (1)

the work area excluding ℂ 2 and ℂ 3 as ℂ1 . Note that there is

where mr is the mass of the robot and a r is the acceleration vector of the robot in Cartesian coordinates. Next, with the ɶ of the acceleration vector known, the reference path P r reference robot can be derived using Euler integration. The robot controller computes the linear and angular velocities, υ ɶ under the nonholonomic constraint. Many and ω to track P r

This section begins by discussing the case of a robot avoiding a human as an example of a dynamic obstacle. The geometric configuration of the goal, the robot and the human for a collision avoidance system is presented in Fig. 4. In this figure, Pg , Pr and Ph are the current position vectors of the

Vision Pr System P h

Fv

VFF

ɶ P r

Holonomic Dynamics

υ ω

Path planner The human

Ph

The robot

u1 u2

Pr

Wheel Controllers

Robot Controller

θɶ1 θɶ2

Nonholonomic Kinematics

Fig. 3. The nonholonomic robot control system

only one region ℂ1 . It excludes the ℂ 2 and ℂ 3 regions for all of the obstacles. For humans, ℂ 2 and ℂ 3 are disks with radii r2 and r3 , respectively. For other obstacles, ℂ 2 and ℂ 3 are obtained by expanding the obstacle contour by radii r2 and r3 . Based on our simulations for the worst case, r2 and r3 are listed in Table I. ℂ 2 and ℂ 3 for a human are shown in Fig. 4. ℂ 3 is larger than the region occupied by head and torso of a walking human. TABLE I. THE SIZES OF ℂ 2 AND ℂ 3 Obstacle

r2

r3

Humans Static obstacles

2.5 m 0. 55 m

0.8 m 0.27 m

B. Design of virtual force functions

goal, the robot and the human, respectively. Vr and Vh are the velocity vectors of the robot and the human. We have: E = Pg − Pr (2a)

D = Pr − Ph

(2b)

T = Pg − Ph

(2c)

where E is the vector from the robot to its goal, D is the vector from the obstacle to the robot, and T is the vector from the obstacle to the robot’s goal. An attractive virtual force is used to guide the robot to the goal. It is activated when the robot is within ℂ1 or ℂ 2 . Its force function is defined as in [14]:  K E + K Eɺ if Pr ∈ ℂ 2 ∪ ℂ1 2 Fa =  1 (3)  undefined if Pr ∈ ℂ 3  where K1 and K 2 are positive attractive virtual force gains. A repulsive potential force is used to keep the robot away from the human and is activated when the robot is within ℂ 2 . The repulsive force is normally a function of the distance between the human and robot d (i.e. the length of D ) [12]-[14], and its rate of change, dɺ [13][14]. Its direction is along D . We propose the new repulsive force function:  ɺ* if Pr ∈ ℂ 2  K 3 Λ + K 4 Λ u D (4) FΛ =   undefined if P ∈ ℂ ∪ ℂ r 1 3  where K3 and K 4 are positive repulsive force gains;

(

2

Λ = (r2 − d )

)

2

(d − r3 ) ; Λɺ * = −dɺ (r2 − d ) 2 (d − r3 ) ; and

u D is a unit vector along D , pointing away from the human. Pr ∈ ℂ 2 here means d < r2 and d > r3 . The numerator term of 2

Λ , (r2 − d ) , causes a gentle increase of the repulsive force when the robot enters the active region. The denominator term, 1 (d − r3 ) , causes the force to increase greatly when the robot is near the edge of ℂ 3 . This tends to push the robot away. FΛ has also been designed to make the VFF continuous as will be proven below. Note that the velocities of the human and robot are included in Λɺ * of (4). When the human and robot approach each other, dɺ < 0 , so Λɺ * > 0 and the repulsive force will increase to push the robot away.

Critical Region ℂ3

ℂ1

help the robot reach its goal. For the collinear condition, since the robot, its goal and the obstacle are collinear and their velocity lines are also along this line, the condition for avoidance is symmetrical to this line. When there are other obstacles, we should choose the force direction to satisfy uψ ⋅ ∑ Fψ > 0 , where ∑ Fψ is the sum of the detour forces from the other obstacles. There is no velocity line for stationary obstacles. Therefore, to reach the goal early, the force direction should be chosen towards the goal, as shown in Fig. 5(b). This force should be zero when the robot is between the goal and the human (on the same line), or α = β ( α is the angle of the vector T to the positive X-axis and β is angle of D ). So, the proposed detour force function is:  K ψ + K ψɺ * u if Pr ∈ ℂ 2  5 6 ψ Fψ =  (5)  undefined ℂ ℂ if P ∈ ∪ r 1 3 

(

)

2

Pg

E

r2 = 2.5 m



obstacle. This is acceptable for static obstacles, but is not desirable with humans for technical and emotional reasons. Technically, although the current human velocity may be slow, the human could accelerate at any instant. Therefore, there may not be enough time for the robot to pass in front. In this case, the robot could block the motion of the human possibly resulting in tripping and/or a severe impact. Emotionally, it may be considered rude. The choices for the force direction are illustrated in Fig. 5. In Fig. 5(a), the goal and robot are on different sides of the human’s velocity line. In this case, the detour force should cause the robot to detour around behind the human to prevent blocking the human. In Fig 5(b), since the goal and robot are on the same side of the human’s velocity line, the robot will not block the human. Then, the direction of Fψ , uψ should be towards the goal to

T

Ph r3 = 0.8 m Vr D

Fψ' as in Fig. 4, the robot would have to pass in front of the

where K5 and K 6 are the detour force gains; ψ = [ r2 − d ] Φ ;

Pg

Active Region ℂ2 T

The detour force is a virtual force perpendicular to u D . Its main purpose is to push the robot to detour around the human while still moving to the goal. Its other purpose is dealing with the collinear condition. The first issue when designing this force is its direction. This issue has not been adequately addressed in the existing literature. If the force direction were

Vh Fa

D

F

β

X

β

' ψ

Pr

Vh

D

Y

α

X

Ph

(b)

FΛ X

Fig. 4. Configuration of collision avoidance for a mobile robot and a human obstacle



(a)

Pg

T

α

Ph



Pr

Vh

Pr

α ∈ 0, 2π)

β ∈ 0, 2π)

Fig. 5. The selection of the detour force direction

2 Φ = α − β ; ψɺ * = ( r2 − d ) Φɺ ; and uψ is the unit vector in

the direction of the detour force. For other obstacles, such as stationary obstacles, (4) and (5) are also applicable by changing r2 and r3 to the corresponding values of those obstacles. The VFF is the combination of the three forces. Thus, the VFF for the robot with a single human/obstacle is:

 Fa     FV = Fa + FΛ + Fψ    undefined  

if Pr ∈ ℂ 2

(6)

Images

(7)

lim Λ = lim ( r2 − d ) ( d − r3 ) = 0

(8a)

d → r2

( d − r3 )

2

=0

(8b)

2 lim ψɺ = lim ( r2 − d ) (β − α ) = 0

(8c)

2 lim ψɺ * = lim ( r2 − d ) Φɺ = 0

(8d)

d → r2

d → r2

Position information

Video First PC: performs Second PC: performs camera image processing to path planning, robot control and wheel estimate human and control. robot positions

Obstacle Human

Obstacle

Robot

Control signals to each motor

Fig. 7. The experimental setup

At the boundary of ℂ 2 :

d → r2

Wheel 2

Fig. 6. The nonholonomic robot used in the experiments

Goal

FV , Pr ∈ℂ1 = K1E + K 2 Eɺ

ɺ * = lim −dɺ (r − d ) 2 lim Λ 2

Gears Wheel 1

if Pr ∈ ℂ 3

boundary of ℂ1 :

d → r2

Two Motors

if Pr ∈ ℂ 1

If the robot intrudes into ℂ 3 , since it may not have enough space for collision avoidance the VFF is not used, and the robot is decelerated to a stop. This stopping action should prevent or at least mitigate the collision. Any other action made by the robot could worsen the situation. A VFF should be continuous to minimize path oscillations. For our VFF to be continuous the virtual forces at the boundary between ℂ1 and ℂ 2 must be equal. From (6) at the

d → r2

Wheel 3 – Omnidirectional wheel

d → r2

d → r2

Therefore : lim FV , Pr ∈ℂ 2 = K1E + K 2 Eɺ = FV , Pr ∈ℂ1 d → r2

(9)

and the piecewise VFF is continuous. According to the above analysis, if the robot is sharing its work area with N obstacles (which could include other robots), the force field will be:  if Pr ∈ ℂ 1  Fa    if P ∈ ℂ 3,i i ∈ [1,..., N ] undefined  r FV =   N    Fa + ∑ ci (FΛ ,i + Fψ ,i ) otherwise   i=1   (10)  1 if Pr ∈ ℂ 2,i where ci =  ; and FΛ ,i and Fψ,i are the    0 otherwise

experiments. With this robot, shown in Fig. 6, wheels 1 and 2 are driven by DC motors. By controlling the angular velocities of those wheels the surging and yawing of the robot can be independently controlled. Wheel 3 is a passive omnidirectional wheel that supports the robot. The kinematic equations of the robot are excluded here for brevity, and can be found in [14]. The total mass of the robot is mr = 3.7 kg. As illustrated in Fig. 7, the experimental setup includes a calibrated color video camera (PGR model DR2 HICOL) for capturing images of the human and the robot. Color patches are attached to the human's shoulder and the robot at known Z heights. With a standard PC, the X-Y positions of the human and robot are reconstructed from the image centroids of the color patches using the camera calibration matrix and the patch heights. The sampling frequency of the image processing is 16.7 Hz. The maximum error of the position

repulsive and detour forces for the ith obstacle, respectively. With (10), the force field is also continuous. The proof is omitted here for brevity. V. SIMULATIONS AND EXPERIMENTS In order to verify the avoidance performance of our new VFF, a nonholonomic robot was built for performing

Fig. 8. The avoidance paths with the different VFFs when avoiding a static obstacle. The paths are only shown up to t=8.3 s to contrast their goal reaching performance.

Fig. 10. The path for avoiding a walking human under the collinear condition with the new VFF. The algorithms from [12] and [14] fail to work under this condition. Fig. 9. Distances e and d when avoiding a static obstacle with the VFFs

measurements is 0.025 m. The position data are transferred via serial communication to a second PC. The position(s) of the stationary obstacle(s) are pre-defined. The second PC acts as the path planner, robot controller and wheel controller, configured as discussed in section III. The motor currents and motor position encoder signals are transmitted at 1000 Hz to/from the robot using an umbilical cable. A. Avoiding a stationary obstacle A basic test of a VFF algorithm is to avoid a stationary obstacle. In Fig. 8 the simulation and experimental results are compared with simulations of the VFF algorithms from [12] and [14]. The robot starts moving from position coordinates (3.99, 0.60) m to reach its goal at (0, 0.60) m; the obstacle is modelled as a disk with 0.1 m radius. Its centre is located at (2.16, 0.595) m. Hence the avoidance condition is near to the collinear condition. The collinear condition is the worst condition for avoidance, as discussed in section I. We chose the nearly collinear condition to allow a comparison to be made with the algorithms from [12] and [14] since they cannot handle the exactly collinear condition. The active and critical regions of the obstacle are labeled as ℂ 2 and ℂ 3 , respectively. We can see that with all three VFFs the robot avoids the obstacle. However it is obvious that the discontinuous VFFs of [12] and [14] incur oscillating paths of the robot. From Fig. 9, we can see that the path with our continuous VFF has no oscillation and takes less time to reach its goal (8.3 s in the simulation and experimental results) than the other VFFs (11.5 s and 12.1 s). In this figure, e is the distance between the robot and its goal (i.e. e is the length of E ) and d is the distance between the obstacle and the robot. Note that d is always greater than r3 therefore the robot never enters the critical region of the obstacle. B. Avoiding a Moving Human in the Collinear Condition A more challenging test of a VFF algorithm is to avoid a moving human under the collinear condition. In this case, shown in Fig. 10, the human starts from position (0.2, 0.60), and moves from left to right and then stops at (4.0, 0.60). At

Fig. 11. Distances e and d when avoiding a walking human under the collinear condition with the new VFF

the same time, the robot starts from (3.99, 0.60) and moves from right to left and towards its goal at (0.0, 0.60). The maximum approach velocity was 1.7 m/s. For this case, the existing VFF algorithms either result in a collision or the robot failing to reach the goal. Note in this collinear condition, when the robot first enters ℂ 2 of the human, α = π and β = 0 in (5). We can see that the robot needs to change its moving direction sharply in comparison with Fig. 8. The reason is the human’s approach velocity acts to increase the magnitude of repulsive and detour forces. Then the robot needs to move backwards and sideways fast to avoid the human. Fig. 11 shows that the measured human velocity (i.e. Vh ) varies by ±0.4 m/s, and has an average of around 1.0 m/s while walking (i.e. before 5.2 s). The minimum distance between the human and the robot ( d in Fig. 11) is over 0.9 m in the simulation (around 1.0 m in the experiment), and larger than r3 (0.8 m for humans). The robot is always outside of the critical region of the human. It reaches the goal at 11.3 s. Note that the human positions from the experimental data are used in the simulation. The discrepancy between the simulation and

experimental results is from the error of the vision system, and wheel slip. The vision system error also influences the calculation of the virtual forces and causes a larger difference between the simulation and experimental results. C. Avoiding a moving human and two stationary obstacles Another challenging test of our VFF algorithm involves avoiding two stationary obstacles and a moving human. Obstacle 1 is modeled as a rectangle with four corners at (-0.3,1.6), (0.5,1.6), (0.5,3.0) and (-0.3,3.0). Obstacle 2 is modeled as a disk with 0.1 m radius and located at (2.16,1.5). The human starts from position (0.2, 0.60), moves from left to right and then stops at (4.0, 0.60). Results of the simulation and experiment are shown in Figs. 12 and 13, respectively. We can see that the robot successfully avoided the three obstacles. The robot reaches the goal at 11.8 s. The times when the robot is actively avoiding each obstacle can be found by comparing its d value with its r2 value. The robot first avoids Obstacle 1 (from 0.6 to 2.3 s), then the human gets close so the robot moves sideways to avoid him (from 1.1 to 7.4 s). After that, Obstacle 2 is encountered and avoided (from 6.9 to 8.1 s). In Fig. 13, d for Obstacle 2 is significantly larger than the d data for the static obstacle in Fig. 9. It is because in Fig. 9, the avoidance is near to the worst case, the collinear condition, so the robot comes closer to (but still does not enter) the obstacle's critical region.

Fig. 12. The robot path when avoiding a walking human and two static obstacles with the new VFF

VI. CONCLUSIONS In this paper, a new VFF based collision avoidance algorithm for nonholonomic mobile robots has been investigated. This algorithm is suitable for avoiding unpredictable dynamic obstacles, such as humans. To reduce the time cost for the robot to reach its goal and to solve the collinear problem, a novel virtual force, the detour force, was proposed. A new virtual repulsive force was also introduced that allows the VFF to be continuous. After proving the continuity of the new VFF, collision avoidance simulations and experiments were performed. The new algorithm was compared with two important VFF algorithms from the literature. The new algorithm was found to diminish oscillations in the robot's path, and to significantly reduce the time cost for the robot reaching its goal (e.g. 8.3 s vs. 11.5 s). In the experiments, the robot successfully avoided stationary obstacles and a walking human.

REFERENCES [1] B. H. Lee and C. S. G. Lee, “Collision-free motion planning of two robots”, IEEE Tran. on Systems, Man, and Cybernetics, , no. 1, pp. 21–31, 1987. [2] T. Tsubouchi, S. Kuramochi and S. Arimoto, “ Iterated forecast and planning algorithm to steer and drive a mobile robot in the presence of multiple moving obstacles”, In Proc. of IEEE Int. Conf. on Intelligent Robots and Systems, vol. 2, pp. 33–38, 1995. [3] A. Divebiss and J. Wen, “ A path space approach to nonholonomic motion planning in the presence of obstacles”, IEEE Trans. on Robotics and Automation, vol. 13, No. 3, pp. 443–451, 1997

Fig. 13. Distances e and d to the human, Obstacle 1 and Obstacle 2 for avoiding a walking human and two static obstacles with the new VFF [4] M. Yamamoto, M. Shimada and A. Mohri, “Online navigation of mobile robot under the existence of dynamically moving multiple obstacles”, In Proc. of 4th IEEE Inter. Symp. on Assembly and Task Planning Soft Research Park, pp:13 – 18. 2001, [5] D. Hsu, R. Kindel, J. Latombe and S. Rock, “ Randomized kinodynamic motion planning with moving obstacles”, The Int. Journal of Robotics Research, vol. 21, no. 3, pp. 233–255, 2002 [6] M. Earl and R. D’Andrea “Iterative MILP methods for vehicle-control problems”, IEEE Trans. on Robotics, vol. 21, no. 6, pp. 1158–1167, 2005. [7] S. Park, B. Lee, “Analysis of robot collision characteristics using concept of the collision map”, Robotica, vol, 24, no. 3, pp. 295–303, 2006. [8] J. Vannoy and J. Xiao, “Real-time adaptive motion planning (RAMP) of mobile manipulators in dynamic environments with unforeseen changes”, IEEE Trans. on Robotics, vol. 24, no. 5, pp. 1199–1212, 2008. [9] M. Pivtoraiko, R. Knepper and A. Kelly, “Differentially constrained mobile robot motion planning in State Lattices”, J. of Field Robotics, vol. 26, no. 3, pp. 308-333, 2009. [10] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots”, In Proc. of IEEE Int. Conf. on Robotics and Automation, vol. 2, pp. 500–505, 1985.

[11] J. Borenstein and Y. Koren, “Real-time obstacle avoidance for fast mobile robots”, IEEE Trans. on Systems, Man, and Cybernetics, vol. 19, no. 5, pp. 1179–1186, 1989. [12] Y. Koren, and J. Borenstein, “Potential field methods and their inherent limitations for mobile robot navigation”, In Proc. of IEEE Int. Conf. on Robotics and Automation, vol. 2, pp. 1398–1404, 1991. [13] N. Ko and B. Lee, “Avoidability measure in moving obstacle avoidance problem and its use for robot motion planning”, In Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, vol. 3, pp. 1296–1303, 1996. [14] S. Ge and Y. Cui, “Dynamic motion planning for mobile robots using potential field method”, Autonomous Robots, vol. 13, pp. 207–222, 2002. [15] H. Tanner, S. Loizou and K. Kyriakopoulos “Nonholonomic navigation and control of cooperating mobile manipulators”, IEEE Trans. on Robotics and Automation, vol. 19, no. 1, pp. 53–64, 2003. [16] J. Ben, K. McIsaac and R. Patel, “Modified Newton’s method applied to potential field-based navigation for nonholonomic robots in dynamic environments”, Robotica, vol. 26, no. 3, pp. 285-294, 2008 [17] B. Hammer, S. Singh, S. Roth and T. Takahashi, “An efficient system for combined route traversal and collision avoidance”, Autonomous Robot, vol. 3, no.4, pp. 365–385, 2008. [18] R. van Emmerik and R. Wagenaar, “Effects of walking velocity on relative phase dynamics in trunk in human walking”, Journal of Biomechanics, vol. 29, no. 9, pp. 1175–1184, 1996. [19] G. Klančar and I. Škrjanc, “Tracking-error model-based predictive control for mobile robots in real time”, Robotics and Autonomous Systems, vol. 55, no. 6, pp. 460–469, 2007.