Robotics and Autonomous Systems 54 (2006) 989–1004 www.elsevier.com/locate/robot A 3-level autonomous mobile robot navigation system designed by using reasoning/search approaches Jasmin Velagic ∗ , Bakir Lacevic, Branislava Perunicic University of Sarajevo, Faculty of Electrical Engineering, Zmaja od Bosne bb, 71000 Sarajevo, Bosnia and Herzegovina Received 3 November 2004; received in revised form 24 April 2006; accepted 15 May 2006 Available online 14 July 2006 Abstract This paper describes how soft computing methodologies such as fuzzy logic, genetic algorithms and the Dempster–Shafer theory of evidence can be applied in a mobile robot navigation system. The navigation system that is considered has three navigation subsystems. The lower-level subsystem deals with the control of linear and angular volocities using a multivariable PI controller described with a full matrix. The position control of the mobile robot is at a medium level and is nonlinear. The nonlinear control design is implemented by a backstepping algorithm whose parameters are adjusted by a genetic algorithm. We propose a new extension of the controller mentioned, in order to rapidly decrease the control torques needed to achieve the desired position and orientation of the mobile robot. The high-level subsystem uses fuzzy logic and the Dempster–Shafer evidence theory to design a fusion of sensor data, map building, and path planning tasks. The fuzzy/evidence navigation based on the building of a local map, represented as an occupancy grid, with the time update is proven to be suitable for real-time applications. The path planning algorithm is based on a modified potential field method. In this algorithm, the fuzzy rules for selecting the relevant obstacles for robot motion are introduced. Also, suitable steps are taken to pull the robot out of the local minima. Particular attention is paid to detection of the robot’s trapped state and its avoidance. One of the main issues in this paper is to reduce the complexity of planning algorithms and minimize the cost of the search. The performance of the proposed system is investigated using a dynamic model of a mobile robot. Simulation results show a good quality of position tracking capabilities and obstacle avoidance behavior of the mobile robot. c 2006 Elsevier B.V. All rights reserved. Keywords: Dynamic model of mobile robot; Backstepping algorithm; Hybrid position controller; Map building; Genetic algorithm; Fuzzy logic; Path planning; Pruning of relevant obstacles; Local minima problem 1. Introduction The basic feature of an autonomous mobile robot is its capability to operate independently in unknown or partially known environments. The autonomy implies that the robot is capable of reacting to static obstacles and unpredictable dynamic events that may impede the successful execution of a task [10]. To achieve this level of robustness, methods need to be developed to provide solutions to localization, map building, planning and control. The development of such techniques for autonomous robot navigation is one of the major trends in current robotics research [23]. The robot has to find a collision-free trajectory between the starting configuration and the goal configuration in a static ∗ Corresponding author. Tel.: +387 33 25 07 65; fax: +387 33 25 07 25. E-mail address: jasmin.velagic@etf.unsa.ba (J. Velagic). 0921-8890/$ - see front matter c 2006 Elsevier B.V. All rights reserved. doi:10.1016/j.robot.2006.05.006 or dynamic environment containing some obstacles. To this end, the robot needs the capability to build a map of the environment, which is essentially a repetitive process of moving to a new position, sensing the environment, updating the map, and planning subsequent motion. Most of the difficulties in this process originate in the nature of the real world [17]: unstructured environments and inherent large uncertainties. First, any prior knowledge about the environment is, in general, incomplete, uncertain, and approximate. For example, maps typically omit some details and temporary features; also, spatial relations between objects may have changed since the map was built. Second, perceptually acquired information is usually unreliable. Third, a real-world environment typically has complex and unpredictable dynamics: objects can move, other agents can modify the environment, and apparently stable features may change with time. Finally, the effects of control actions are not 990 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 completely reliable, e.g. the wheels of a mobile robot may slip, resulting in accumulated odometric errors. Robot navigation can be defined as the combination of three basic activities [4,16]: • Map building. This is the process of constructing a map from sensor readings taken at different robot locations. The correct treatment of sensor data and the reliable localization of the robot are fundamental in the map-building process. • Localization. This is the process of getting the actual robot’s location from sensor readings and the most recent map. An accurate map and reliable sensors are crucial to achieving good localization. • Path planning. This is the process of generating a feasible and safe trajectory from the current robot location to a goal based on the current map. In this case, it is also very important to have an accurate map and a reliable localization. Path planning is one of the key issues in mobile robot navigation. Path planning is traditionally divided into two categories: global path planning and local path planning. In global path planning, prior knowledge of the workspace is available [21]. Local path planning methods use ultrasonic sensors, laser range finders, and on-board vision systems to perceive the environment to perform on-line planning [1,8]. In our paper, the workspace for the navigation of the mobile robot is assumed to be unknown and it has stationary obstacles only. In local path planning methods, particular attention is paid to local minima problem. This problem occurs when a robot navigates towards a desired target with no prior knowledge of the environment and gets trapped in a loop [7,14]. This happens usually if the environment consists of concave obstacles, mazes, and such objects. To get out of the loop, the robot must comprehend its repeated traversal through the same environment, which involves memorizing the environment that has already been seen [12]. The main contribution of this paper is the design of a robust autonomous mobile robot control system suitable for on-line applications with real-time requirements by using soft computing methodologies. This system provides the mobile robot that may navigate in an a priori unknown indoor environment using sonar sensor information. To achieve these requirements, the proposed system is hierarchically organized into three distinct separated subsystems with arbitrary responsibility. At each level of this system, one or more soft computing methodologies are adopted to solve its specific problems. A low-level velocity controller is developed using the standard PI multivariable control law. The medium-level position control law has to be nonlinear in order to ensure stability of the error, that is, its convergence to zero [6,18]. Some of the control parameters are continuous time functions, and usually the backstepping method [6,15,22] was used for their adjustment. In order to achieve the optimal parameter values, we used a genetic algorithm. Both controllers are based on a dynamic model of a differential drive mobile robot having angular velocities as main variables. Fig. 1. Mobile robot navigation system for real-time requirements. A high-level subsystem contains map-building and pathplanning algorithms. In this paper, the application of an occupancy-based map [17] using Dempster–Shafer evidence theory based on sonar measurements is demonstrated. The integration of sonar data is obtained by a low-level fusion [9]. The building of occupancy maps is well suited to path planning and obstacle avoidance [10]. In this paper, we propose a new path-planning approach based on the repulsive and attractive forces, which sets the fuzzy rules for determining which obstacles should have an influence on the mobile robot motion. Fuzzy logic offers the possibility to mimic expert human knowledge [2,3,11,26]. This approach provides both obstacleavoidance and target-following behaviors and uses only the local information for decision making for the next action. Also, we propose a new algorithm for the identification and solution of the local minima situation during the robot’s traversal using the set of fuzzy rules. 2. Navigation system The proposed three-level control navigation system is shown in Fig. 1. The low-level velocity control system is composed of a multivariable PI controller and a dynamic model of a mobile robot and actuators. At the medium level, the position control system generates a nonlinear control law whose parameters are obtained using a genetic algorithm. The high-level system performs map-building and path-planning tasks. This system is in charge of sensor interpretation, sensor data fusion, map building, and path planning. All the modules are designed using fuzzy logic and the Dempster–Shafer theory of evidence. In the following sections, the design of the navigation system blocks from Fig. 1 is described. 3. Dynamics of mobile robot In this section, a dynamic model of a nonholonomic mobile robot with viscous friction will be derived first. A typical representation of a nonholonomic mobile robot is shown in Fig. 2. The robot has two driving wheels mounted on the same axis and a free front wheel. The two driving wheels are J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 991 equations below follow: r (θ̇ R + θ̇ L ) cos θ − d θ̇ sin θ, 2 r ẏC = (θ̇ R + θ̇ L ) sin θ + d θ̇ cos θ. 2 ẋC = Fig. 2. The representation of a nonholonomic mobile robot. independently driven by two actuators to achieve both transition and orientation. The position of the mobile robot in the global frame {X, O, Y } can be defined by the position of the mass center of the mobile robot system, denoted by C, or alternatively by the position A, which is the center of the mobile robot gear, and the angle between the robot’s local frame {xm , C, ym } and the global frame. The kinetic energy of the whole structure is given by the following equation: T = Tl + Tr + Tkr , (1) (8) (9) By substituting terms in Eq. (1) with expressions in Eqs. (2)–(9), the total kinetic energy of the vehicle can be calculated in terms of dθ R /dt and dθ L /dt:   (I A + Md 2 )r 2 I0 Mr 2 + θ̇ R2 + T (θ̇ R , θ̇ R ) = 8 2 8R 2   Mr 2 (I A + Md 2 )r 2 I0 + + θ̇ L2 + 8 2 8R 2   Mr 2 (I A + Md 2 )r 2 θ̇ R θ̇ L . (10) + − 4 4R 2 d dt d dt Now, the Lagrange equations:   ∂L ∂L − = τ R − K θ̇ R , ∂θ R ∂ θ̇ R   ∂L ∂L = τ L − K θ̇ L , − ∂θ L ∂ θ̇ L (11) (12) where Tl is the kinetic energy that is the consequence of pure translation of the entire vehicle, Tr is the kinetic energy of rotation of the vehicle in the XOY plane, and Tkr is the kinetic energy of rotation of the wheels and rotors of the DC motors. The values of energy terms introduced can be expressed by Eqs. (2)–(4): are applied. Here, τ R and τ L are right and left actuation torques and K dθ R /dt and K dθ L /dt are the viscous friction torques of right and left wheel-motor systems, respectively. Finally, the dynamic equations of motion can be expressed as: 1 1 Mvc2 = M(ẋc2 + ẏc2 ), 2 2 1 Tr = I A θ̇ 2 , 2 1 1 Tkr = I0 θ̇ R2 + I0 θ̇ L2 , 2 2 Aθ̈ R + B θ̈ L = τ R − K θ̇ R , (13) B θ̈ R + Aθ̈ L = τ L − K θ̇ L , (14) Tl = (2) (3) (4) where M is the mass of the entire vehicle, vc is the linear velocity of the vehicle’s center of mass C, I A is the moment of inertia of the entire vehicle considering point A, θ is the angle that represents the orientation of the vehicle (Fig. 2), I0 is the moment of inertia of the rotor/wheel complex, and dθ R /dt and dθ L /dt are the angular velocities of the right- and left-hand wheels, respectively. Further, the components of the velocity of the point A can be expressed in terms of dθ R /dt and dθ L /dt: r ẋ A = (θ̇ R + θ̇ L ) cos θ, 2 r ẏ A = (θ̇ R + θ̇ L ) sin θ, 2 r (θ̇ R − θ̇ L ) . θ̇ = 2R (5) (6) (7) Since ẋC = ẋ A − d θ̇ sin θ and ẏC = ẏ A + d θ̇ cos θ , where d is the distance between points A and C, it is obvious that the where   (I A + Md 2 )r 2 Mr 2 + A= + I 0 4 4R 2   Mr 2 (I A + Md 2 )r 2 . B= − 4 4R 2 (15) In this paper, we used a mobile robot with the following parameters: M = 10 kg, I A = 1 kg m2 , r = 0.035 m, R = 0.175 m, d = 0.05 m, m 0 = 0.2 kg and I0 = 0.001 kg m2 . In the following section, a design for both velocity and position controls will be established. 4. Position and velocity control designs The function of this controller is to implement a mapping between the known information (e.g. reference position, velocity and sensor information) and the actuator commands designed to achieve the robot task. For a mobile robot, the controller design problem can be described as follows: given the reference position qr (t) and velocity q̇r (t), design a control law for the actuator torques so that the mobile robot velocity may track a given reference control path with a given smooth 992 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 open. However, usage of this law is justified by simulation results. The key problem in such a control design is to obtain control coefficients k1 to k6 . To solve this problem, a genetic algorithm is used to find the optimal values of those coefficients. To apply this method, a low-level velocity controller has to be designed first. 4.2. Velocity control Fig. 3. The concept of tracking of a virtual reference robot. velocity. Let the velocity and position of the reference robot (Fig. 3) be given as: T (16) qr = [xr yr θr ] , where ẋr = vr cos θr , ẏr = vr sin θr , θ̇r = ωr , and vr > 0 is the reference linear velocity and ωr is the reference angular velocity. 4.1. Position control The trajectory tracking problem for a mobile robot is based on a virtual reference robot [5] that has to be tracked (Fig. 3). The tracking position error between the reference robot and the actual robot can be expressed in the robot frame as:      cos θ sin θ 0 xr − x e1 e p = e2  = T p eq = − sin θ cos θ 0  yr − y  , (17) 0 0 1 e3 θr − θ  T where eq = ex e y eθ . The dynamics of the position error derived in (17) is postulated as: ė1 = ωe2 + u 1 , ė2 = −ωe1 + vr sin e3 , ė3 = u 2 , (18) where inputs u 1 and u 2 are new control inputs. In this paper, we propose the following control inputs in the velocity control loop: u 1 = vr cos e3 + q k 1 e1 , k4 + e12 + e22 k3 vr sin e3 + q u 2 = ωr + q , k6 + e32 k5 + e12 + e22 k2 vr e2 (19) where k1 , k2 , k3 , k4 , k5 and k6 are positive parameters. Eq. (19) is a modified backstepping control law given first in [6]. The modification consists of the introduction of denominators. In [6], Lyapunov’s stability theory was used to prove that the control law that is considered provides a uniformly bounded norm of error ke p (t)k. The issue of rigorous proof of stability for the control law introduced (19) remains The dynamics of the velocity controller is given by the following equations in the Laplace domain:      1 g1 (s) g2 (s) ev (s) τ (s) , (20) τ (s) = R = τ L (s) r g1 (s) −g2 (s) eω (s) where ev (s) is the linear velocity error and eω (s) is the angular velocity error. This structure differs from previously used diagonal structures. Transfer functions g j (s) are chosen to represent PI controllers:   1 · R, g1 (s) = K 1 1 + Ti1 s   (21) 1 · R. g2 (s) = K 2 1 + Ti2 s The particular choice of the adopted multivariable PI controller described by Eqs. (20) and (21) is justified with the following theorem. Theorem. Torque control (20) ensures tracking servo inputs u 1 and u 2 with zero steady-state errors. Proof. When we substitute θ̇ R with ω R , θ̇ L with ω L , and consider (20), we can write another form of (13) and (14):    As + K Bs ω R (s) Bs As + K ω L (s)    1 g1 (s) g2 (s) u 1 (s) − v(s) , (22) = r g1 (s) −g2 (s) u 2 (s) − ω(s) where ω R and ω L can be expressed in terms of ω and v as: v − Rω v + Rω , ωL = r r and then (22) can be transformed to:    As + K Bs v(s) + Rω(s) Bs As + K v(s) − Rω(s)    g (s) g2 (s) u 1 (s) − v(s) = 1 g1 (s) −g2 (s) u 2 (s) − ω(s) ωR = and further to:       α1 (s) α2 (s) v(s) g (s) g2 (s) u 1 (s) = 1 , (23) α1 (s) −α2 (s) ω(s) g1 (s) −g2 (s) u 2 (s) where (A + B)s 2 + (K + K 1 Ti1 )s + K 1 s R(A − B)s 2 + (R K + K 2 Ti2 )s + K 2 α2 (s) = . s α1 (s) = (24) J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 993 Fig. 6. Chromosome structure. If ex (t), e y (t) and eθ (t) (0 < t < ts ) are error functions, the objective function is calculated as in (26) [13]. Parameters ax , a y and aθ are some positive real numbers. N is the number of error samples. The mapping between the objective and the fitness function has not been performed, so it is obvious that the better individual has the smaller value of the objective function (fitness). In order to evaluate the fitness of the individual, it is necessary to run the simulation:   N −1  X ts ln 1 + ex i F = ax N i=0   N −1  X ts ln 1 + e y i + ay N i=0 Fig. 4. Linear velocity step response. + aθ N −1 X i=0 The following equations could be easily derived from (23): v(s) = = ω(s) = = (26) Values of the objective function parameters are: ax = aθ = 1, a y = 2, N = 1000 and ts = 10 s. In (26), we have chosen the following criterion: Fig. 5. Angular velocity step response. g1 u 1 (s) = G 1 u 1 (s) α1 K 1 Ti1 s + K 1 u 1 (s) (A + B)s 2 + (K + K 1 Ti1 )s + K 1 g2 u 2 (s) = G 2 u 2 (s) α2 K 2 Ti2 s + K 2 u 2 (s). R(A − B)s 2 + (R K + K 2 Ti2 )s + K 2    ts . ln 1 + eθ i N J1 = N X ln(1 + |ei |), (27) i=1 rather than the criteria that are normally used, given by (28) and (29): (25) It is obvious that transfer functions G 1 and G 2 are static with gains equal to “1”, which completes the proof. The velocity control loop structure is shown in Fig. 1, as an inner loop. From the simulation results obtained (Figs. 4 and 5), it can be seen that the proposed PI controller successfully tracks the given linear and angular velocity profiles. The controller parameters used for this simulation are K 1 = 129.7749, K 2 = 41.0233, Ti1 = 11.4018, and Ti2 = 24.1873, which are tuned using standard GA. 4.3. Evolution of the coefficients In this paper, a simple genetic algorithm is used for parameter evolution of coefficients k1 to k6 , which encodes into a binary chromosome (Fig. 6). J2 = N X |ei | (28) N X ei2 . (29) i=1 J3 = i=1 The criterion J1 penalizes more smaller errors (expected in the stationary state) (Fig. 7) and consequently it ensures better position tracking. Error eθ (t) is calculated as follows:  θr − θ,     if |θr − θ| < min(|θr − θ − 2π |, |θr − θ + 2π |)   θ − θ − 2π, r eθ =  if |θr − θ − 2π| < min(|θr − θ|, |θr − θ + 2π|)      θr − θ + 2π, if |θr − θ + 2π | < min(|θr − θ|, |θr − θ − 2π |). (30) This type of error is appropriate, since it prevents unnecessary full-circle rotation. The coefficient evolution is 994 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 9. Tracking robot does not “see” virtual robot. Fig. 7. Graphical illustration of different criteria. Fig. 10. Producing ωs (t). to the straight line, determined with the robot and its initial orientation (Fig. 9). For that purpose, the following control law, which provides velocity servo inputs, is proposed: ū 1 (t) = α(t)u 1 (t) ū 2 (t) = α(t)u 2 (t) + (1 − α(t))ωs (t). (32) Function ωs (t) is generated as the output of the following system (Fig. 10). The input d(t) is given by: d(t) = sgn(atan2(e y (t), ex (t)) − θ(t)). (33) The function α(t) satisfy the following differential equation: Fig. 8. Change of the best fitness in the population through generations. d2 α(t) dα(t) + α(t) = z(t), (34) + b1 2 dt dt where z(t) is close to a step function and is given by:  1, if ∃t1 ∈ [0, t] : θ(t1 ) = atan2(e y (t1 ), ex (t1 )) z(t) = (35) 0, otherwise. b0 performed using a lemniscate as a suitable complex trajectory: xr (t) = yr (t) = a sin(αt) 1 + sin2 (αt) a sin(αt) cos(αt) 1 + sin2 (αt)   ẏr (αt) θr (t) = arctan , ẋr (αt) (31) where a = α = 1. The evolution process (population size is 40) is depicted in Fig. 8. This evolution yielded the following coefficient values: K 1 = 6.2457, K 2 = 221.2306, K 3 = 2.3433, K 4 = 13.5117, K 5 = 3.1933, K 6 = 8.3361. 4.4. Design of hybrid position controller It has been noticed that, during the preliminary simulations, at the beginning of tracking the control torques increase rapidly if the initial position of the reference robot does not belong This way, the robot does not start tracking the virtual robot instantly; it first rotates around its own axis with an increasing angular velocity ωs (t), until it “sees” the virtual robot (Fig. 11). 4.5. Simulation results The validation of the proposed hybrid controller was tested with a lamniscate trajectory using an ordinary backstepping algorithm [13] and a hybrid backstepping algorithm. The results of the trajectory tracking in both cases are shown in Figs. 12 and 13. From these figures, it can be concluded that satisfactory tracking results are obtained using both control strategies. However, better tracking of the reference trajectory is achieved J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 11. “Seeking” a virtual robot. Fig. 14. X and Y coordinate error. Fig. 12. Tracking a lemniscate trajectory (hybrid). Fig. 15. Orientation error. Fig. 13. Tracking a lemniscate trajectory (ordinary). in the case of an ordinary controller, especially at the beginning of transient process. This is illustrated in Figs. 14 and 15. However, the hybrid controller ensures that much smaller values of the control input torques are needed to obtain the reference position and orientation trajectories (Figs. 16 and 18). Consequently, the required power of the DC motors is also much smaller in the case of a hybrid controller (Figs. 17 and 19). Finally, the comparison of Cartesian error norm profiles is presented for various friction coefficients in Fig. 20. This experiment has also been performed for a lemniscate path. It is obvious that the increase in friction causes degradation of the control system performance. The next section describes a map-building process which uses Dempster–Shafer evidence theory. Fig. 16. Control torques (hybrid controller). Fig. 17. Required power of DC motors (hybrid controller). 995 996 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 18. Control torques (ordinary controller — the first second of tracking). Here, the proposed map-building process utilizes Dempster– Shafer evidence theory. The sonar sensor readings are interpreted by this theory and used to modify the map using Dempster’s inference rule. Whenever the robot moves, it catches new information about the environment and updates the old map. More specifically, to build an occupancy map of the environment, we first construct a grid representing the whole space. Every discrete region of the map (each cell) may be in two states: Empty and Full. A series of range readings {r1 , . . . , rn } collected at known sensor locations is available. In principle, the task of the Map Building System is to process the readings in order to assess, as accurately as possible, which cells are (even partially) occupied by obstacles and which cells are (completely) empty and thus suitable for robot navigation. 5.1. Review of Dempster–Shafer’s theory of evidence Fig. 19. Required power of DC motors (ordinary controller — the first second of tracking). The theory of evidence appears in the definition of a frame of discernment, Θ. This is a set of labels representing mutually exhaustive events. As described in [19,20], the relevant labels for map-building applications are Θ = {E, O}. This means that a grid cell can be empty (E) or occupied (O). A basic probability assignment is a function m : Λ → [0, 1], where Λ is the set of all subsets of Θ; in our case Λ = {φ, E, O, {E, O}}. The state of each cell is described by assigning basic probability values to each label in Λ. However, we know that, for each cell i, j in the grid, m i, j (φ) = 0, X m i, j (φ) + m i, j (E) + m i, j (O) m i, j (A) = (36) A⊂Ψ + m i, j ({E, O}) = 1. (37) Thus, the label A is assigned a basic probability value m(A) describing the degree of belief that is committed to exactly A. Every cell in the map is first initialized, as follows: m i, j (E) = m i, j (O) = 0, Fig. 20. Norms of Cartesian error for various friction coefficients. m i, j ({E, O}) = 1. (38) A basic probability assignment usually defines the current four states of each cell. However, the number of states can be reduced to two (m i, j (E), m i, j (O)), assuming that m i, j (φ) = 0 and applying (37). The state (0, 0) means total ignorance, and so m i, j ({E, O}) = 1. When the robot is sure about the cell occupancy, then m i, j (O) = 1, and that makes the other labels equal to zero. Conversely, m i, j (E) = 1 when the robot is sure that a cell is empty. 5. Map building 5.2. Sensor modeling and measurements interpretation An occupancy grid is essentially a data structure that indicates the certainty that a specific part of space is occupied by an obstacle. It represents an environment as a twodimensional array. Each element of the array corresponds to a specific square on the surface of the actual world, and its value shows the certainty that there is some obstacle there. When new information about the world is received, the array is adjusted on the basis of the nature of the information. The Polaroid Ultrasonic Rangefinder (Polaroid, 1987) is used for map building. This is a very common device that can detect distances in the range of 0.47–10 m with 1% accuracy. The multi-lobed power diagram of the transmitter is obtained from the radiation directivity function of a plane circular piston: D(ϑ) = 2 J1 (ωη) ωη sin ϑ (39) 997 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 In region I, where R − ε < r < R + ε:  2  2 β−α ε−|R−r | + β ε m(O) = , 2 m(E) = 0, m({E, O}) = 1 − m(O). (40) (41) (42) In region II, where Rmin < r < R − ε: Fig. 21. Typical beam pattern of Polaroid ultrasonic sensor. m(O) = 0,  m(E) = β−α β (43) 2 +  R−ε−r R−ε 2 m({E, O}) = 1 − m(E). 2 , (44) (45) R is the range response from the ultrasonic sensor, and (r, α) is the coordinate of a point inside the sonar cone. ε is the range error, and it distributes the evidence in Region I. β is the half open beam angle of the sonar cone. 5.3. The fusion of sensor data Fig. 22. The profile of the ultrasonic sensor model. where J1 (·) is the Bessel function of the first order, ω = 2π/λ depends on the wavelength λ, η is the piston radius, and ϑ is the azimuthal angle measured with respect to the beam’s central axis. For the particular sensor that we used, η = 0.01921 m and λ = v/ϕ, where v is the sound speed in air and ϕ = 49.410 kHz. In practice, it is sufficient to take into account only the principal lobe of the pattern, and consider the waves to be diffused over a radiation cone of 30◦ width (Fig. 21). A single reading provides the information that one or more obstacles are located somewhere along the 30◦ arc of circumference of the radius R (Fig. 21). Hence, there is evidence that cells located in the proximity may be ‘occupied’. On the other hand, cells well inside the circular sector of radius R are likely to be ‘empty’. To model this knowledge, we use the Dempster–Shafer theory of evidence. The evidence is obtained by projecting the raw ultrasonic sensor responses onto the evidence grid through the sonar profile model. This profile sonar model is a function of the angle and the sonar range reading, as shown in Fig. 22. The model converts the range information into probability values. The model in Fig. 22 is given by Eqs. (40)–(45). The sonar data interpreted by Dempster–Shafer’s theory of evidence are collected and updated into a map using the same theory of evidence. In our approach, the basic probability assignment corresponding to a range reading r is obtained directly using Eqs. (40)–(45). Finally, each cell in the map is updated using Dempster’s rule of combination. This rule allows us to combine independent evidence about a certain event A, m 1 (A) and m 2 (A). In our case, they will be the mentioned basic probability assignments for the old map belief m t−1 M (A) and the current sensor reading m tS (A). For the events E and O, the update rules are: t m tM (E) = m t−1 M ⊕ m S (E) t m t−1 (E)m t (E) + m t−1 M (E)m S ({E, O}) = M t−1 S t t 1 − m M (E)m S (O) − m t−1 M (O)m S (E) t−1 m M ({E, O})m tS (E) + t−1 t 1 − m M (E)m tS (O) − m t−1 M (O)m S (E) t m tM (O) = m t−1 M ⊕ m S (O) t m t−1 (O)m t (O) + m t−1 M (O)m S ({E, O}) = M t−1 S t t 1 − m M (E)m S (O) − m t−1 M (O)m S (E) t−1 t m M ({E, O})m S (O) + . t−1 t 1 − m M (E)m tS (O) − m t−1 M (O)m S (E) (46) (47) The next example illustrates the usage of Dempster–Shafer’s theory of evidence for the map-building process. Example 1. Let the robot be located in cell (14, 10) in the beginning of the navigation process. The basic probability assignments of the map cells have zero values before the start. Then, sonars scan the environment and sonars S0 and S1 detect some possible obstacles on distance r0 = 7 and r1 = 5, with angles α0 = 0 and α1 = 3 (Fig. 23). In this example, R = 8 and ε = 1.5. 998 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 (r0 = 6.5, r1 = 7, r15 = 7.5, α0 = 2, α1 = 5 and α15 = 4)): 2  2  15−2 + 1.5−|8−6.5| 15 1.5 S0 (6.5) = 0.376 m 8,8 (O) = 2 S0 (6.5) m 8,8 (E) = 0 S (6.5) S (6.5) 0 m 8,8 0 ({E, O}) = 1 − m 8,8 (Z ) = 0.624, 2  2  15−5 + 1.5−|8−7| 15 1.5 S1 (7) = 0.278 m 6,10 (O) = 2 S1 (7) m 6,10 (E) = 0 S (7) Fig. 23. Initial position. S (7) 1 1 ({E, O}) = 1 − m 6,10 = 0.722, m 6,10  2  2 15−4 + 1.5−|8−7.5| 15 1.5 S15 (7.5) m 5,14 (O) = = 0.491 2 S15 (7.5) m 5,14 (E) = 0 S (7.5) 15 m 5,14 S (7.5) 15 ({E, O}) = 1 − m 5,14 = 0.509. Evidence about occupancies of cells (8, 8) and (6, 10) are updated using Dempster’s rules of combination (46) and (47): t−1 m t8,8 (E) = m 8,8 ⊕ m t8,8 (E) = 0 t m t8,8 (O) = m t−1 8,8 ⊕ m 8.8 (O) = 0.723 m t8,8 ({E, O}) = 0.277, t−1 m t6,10 (E) = m 6,10 ⊕ m t6,10 (E) = 0 t−1 m t6,10 (O) = m 6,10 ⊕ m t6.10 (O) = 0.519 Fig. 24. Occupancy determination based on sonar measurements in t = 2 s. The new basic probability assignments for cells (8, 8) and (6, 10) in the map become (the first lies in region I and the second lies in region II): S0 (7) m 8,8 (O) =  15−0 15 2 +  1.5−|8−7| 1.5 2 2 = 0.556 S (7) 0 m 8,8 (E) = 0 S (7) S (7) m t6,10 ({E, O}) = 0.481, m t5,14 (Z ) = 0.491 m t5,14 (P) = 0 m t5,14 ({P, Z }) = 0.509. From the new results it can be concluded that the evidence about the occupancy of cells (8, 8) and (6, 10) is increased. This process continues until the target is reached. In the following section, the new path-planning algorithm is introduced. 0 0 m 8,8 ({E, O}) = 1 − m 8,8 (Z ) = 0.444, 6. Path planning and The solution of the obstacle avoidance problem in this paper is a modified principle of acting attractive and repulsive forces. S1 (5) m 6,10 (O) S1 (5) m 6,10 (E) S (5) =0  = 15−3 15 2 +  8−1.5−5 10−1.5 2 2 6.1. Calculation of attractive and repulsive forces = 0.336 S (5) 1 1 m 6,10 ({E, O}) = 1 − m 6,10 = 0.664. In the next moment, the robot is moving in a direction depicted by the broken line and sonars scan the environment again. This situation is given in Fig. 24. The possibilities that cells (8, 8), (6, 10) and (5, 14) are occupied is based on sonar measurements (S0 , S1 and S15 In this approach, environment obstacles exert a repulsive force on the robot and the target position exerts an attractive force. The amplitude of the repulsive force is defined as F= 1 , Rr2 (48) where Rr is the distance between the current robot location and the obstacle location. In this situation, the planning algorithm J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 25. The concept of path planning based on repulsive and attractive forces. calculates the following values (Fig. 25): 1xo = xo − xm 1yo = yo − ym q Rr = 1xo2 + 1yo2 , (49) where 1xo and 1yo are coordinate distances between the mobile robot position and the obstacle position. The projections of force F on x and y axis are: Fx = |F · cos(θ)|, Fy = |F · sin(θ)|, (50) (51) where θ denotes the angle between the robot velocity direction and the repulsive force direction: θ = arctan 1yo . 1xo (52) The coordinates of the vector of the repulsive force are:  x + Fx , for 1xo < 0 xr = m (53) xm − Fx , for 1xo > 0,  y + Fy , for 1y p < 0 yr = t (54) yt − Fy , for 1y p > 0. 999 Fig. 26. Stochastic force acting. is moved to another location. This parameter is related to the radius R of the sonar model. In the following consideration, the R is 4 m. In the case of a larger value of R, another value of κ will be selected. The resulting vector, which determines the new target position of the robot, is obtained as a sum of the vectors of repulsive and attractive forces (Fig. 25). The new target coordinates of the robot are: xn = xr + 1xt yn = yr + 1yt . (57) To avoid a possible standstill which may arise if the robot, the obstacle and the target aligned, we introduce a stochastic force orthogonal to the planned trajectory when the sum of forces is zero, and the robot is not yet at the target. This situation is shown in Fig. 26 and explained by fuzzy rules (58). The fuzzy rules for action of the stochastic force are: If |Fa | = |F| and the angle between Fa and F is less then 0.01 rad then Fs is big and acts orthogonally on the robot. In other cases, Fs is small. (58) In the following subsection, the proposed concept of the pruning of relevant obstacles is demonstrated. The attractive force is defined using Ra as 1xt = xt − xm 1yt = yt − ym q Ra = 1xt2 + 1yt2 . (55) In this paper, we proposed the following law for attractive forces:  κ|Ra |, for |Ra | ≥ 1 m Fa = (56) |Ra |, for |Ra | < 1 m, where κ = 5. This parameter is involved to reach the target when obstacles (relevant obstacles which satisfy pruning rules in the next subsection) are located in the circle with radius 1 m and then they produce strong repulsive forces. The discontinuity at a point of 1 m causes the orientation of the robot to be slightly modified, since the new intermediate target point 6.2. Pruning of relevant obstacles The number of obstacles in the environment may be large. If all the obstacles exert a repulsive force, the calculation of the field is extensive, and yet it may produce erratic trajectories. Therefore, we introduce a pruning of obstacles with an aim to preserve only those relevant for the trajectory planning. This pruning is performed in a set of steps. In these steps, we use the concept of the visibility field of the mobile robot [24,25]. The practical visibility field of a mobile robot here is a circle with the radius rv = 4 m. However, in our approach we use only a square inscribed in this circle. The reason is that the local map has m×m points. This local map is now represented as an m×m matrix, and each cell in the map is one matrix element. 1000 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 27. The visibility region of a mobile robot using sonar sensors. Due to this geometry, some obstacles will be in visibility in the field given by Rr < rv . Fig. 28. The forces acting on the robot motion. (59) but they will not be taken into account. For example, in Fig. 27, the obstacles 3, 4 and 5 satisfy (59). However, only obstacles 4 and 5 remain as relevant. Next, all the obstacles in the square are not equally relevant to the future robot motion towards the target. To pinpoint the most relevant, we first define a circular area around the target, named target circle with a radius Ra [24]. The value Ra is slightly larger than the distance between the robot and the target, as represented in Fig. 28 (Ra + 0.1). The extension of 0.1 ensures a safe region around the robot. We consider that only the obstacles in the section where the target circle and the visibility square overlap are really relevant to the motion planning. Then, among these obstacles, we take the one nearest to the robot. The distance between the target and the obstacle is R p : q (60) R p = (xt − xo )2 + (yt − yo )2 . Fig. 29. The strong forces produced by an obstacle that is behind and near the target. This force pulls the robot out from the target. In our approach, the rule for the visibility of an obstacle inside the square is given as: If ((Ra + 0.1) > R p ) and (Rr < rv ) then the obstacle is visible and it should influence the motion. (61) In Fig. 28, only obstacle 3 influences the new target point determination. The further reduction of obstacles is still possible. An obstacle located in the above-defined area (61) but behind the current robot position, or behind the target, and not on the direction of motion should not be taken into account. The force produced by such an obstacle can be strong, but obviously it should be neglected. This situation is illustrated in Fig. 29. To solve this problem, we introduce a new circle around the robot with a radius equal to the distance from the robot to the target (Fig. 30). In this way, the obstacles located behind the target and outside this circle are not considered and the robot will not be pushed aside to go back. So, we introduce a new restriction in the final visibility rule: If ((Ra + 0.1) > R p ) and (Rr < rv ) and (Rr < Ra ) then the obstacle is visible and it should influence the motion. (62) Fig. 30. Introduction of a new circle around the target to avoid the influence of the strong repulsive force. Finally, in our example (Fig. 31) we have only one relevant obstacle 3. In the case that no obstacles are located on the robot motion towards the target, its velocity is governed by the position controller. When the distance between the robot and relevant J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 31. General concept of determination of new target position. Fig. 34. Illustration of a local minima problem. Fig. 32. Dependence of robot velocity on distance between the robot and the relevant obstacle. Fig. 35. Identification of point a, in which the robot is trapped. 1001 6.3. Local minima problem solving For local minima problem avoidance (Fig. 34), we introduce an algorithm that does not require memorizing the already seen environment. This algorithm is capable of identifying the local minima situation during the robot’s motion, by recognizing its trapped state (infinite loop). In this state, the robot oscilates between two points. This algorithm is described as follows: Algorithm for a local minima avoidance problem. Fig. 33. Movement when two relevant obstacles are in a 1 m circle. obstacles, which satisfy above conditions, is less then 1 m, we introduced the parameter k, which multiplies the velocities of the left and the right wheels. Using this parameter, possible collisions between the robot and obstacle and the robot turning over are avoided. The graph of robot velocity versus the distance between the robot and the relevant obstacle is shown in Fig. 32. In the situation represented in Fig. 33, two obstacles are placed in a 1 m circle. The robot cannot pass between them and it continues towards the target during a maneuver around the obstacle so that it gets further away from the robot location (smaller repulsive force). Hence the robot does not enter into an oscillatory movement. In the rest of the section, the steps for a local minima problem solution are introduced. Step 1: Calculate attractive and reflexive forces and their sum (resultant force) Step 2: If resultant force tends to push robot to go back then do not take into consideration the attractive force and keep the reflexive force constant and equal to the closest obstacle distance else apply the principle of attractive and reflexive forces end Step 3: if robot has not reached target point then go to Step 1 end The above steps are executed until the robot reaches the target. The algorithm turns the robot right at switching point a (point at which the robot tends to go back) and the robot continues to see the wall on its left until it arrives at switching point b after it goes towards a target point using the principle of attractive and reflexive forces (Fig. 35). Simulation results of local minima avoidance using the proposed algorithm are given in Fig. 36. In this case, the 1002 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 36. Simulation results of local minima problem solution using the proposed algorithm. Fig. 37. Obstacle avoidance moving to the target. Fig. 38. Wall following. Fig. 39. Passing through a narrow door. proposed algorithm executes a sequence of steps that pulls it out of the trap. In addition, more simulations are performed in the next section to present the effectiveness of the proposed approach in cluttered environment and environments with obstacle loops and mazes. 7. Simulation results To show the usefulness of the proposed approach, a series of simulations has been conducted using an arbitrarily constructed environment including obstacles. The robot has been modeled as a small circle, and imaginary sensors (eight in number) are placed in the form of an arc along the circumference of the robot. The minimum distance obtained within the cone of each sensor is considered as the distance of the obstacle from that sensor, and it is available as an input to the algorithm. All of the simulations are performed in the MATLAB/SIMULINK environment. In these simulations, the positions of all the obstacles in the workspace are unknown to the robot. The robot is aware of its start and finish positions only. Simulation results obtained in seven different working scenarios (with different obstacle constellations) are presented in Figs. 37–44. Fig. 40. Finding a target partially surrounded by obstacles. The proposed algorithm performs well in a cluttered environment, as shown in Fig. 37. The processes for wall following and passing through a narrow door using our algorithm are shown in Figs. 38 and 39, respectively. From the results obtained, it can be concluded that the robot successfully tracks the wall and passes through the narrow door. J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 Fig. 41. Finding a target surrounded by obstacles. 1003 Fig. 44. Finding a target in a more cluttered maze. A new problem arrises if the environment consists of concave obstacles and the robot gets into an infinite loop or into a local minimum. An even more complex problem arises when the obstacles are long and have many bends and kinks; the target attracting and goal repulsing behavior conflict and the robot also gets itself into an infinite loop (Fig. 43). The effectiveness of the proposed algorithm for solving this problem is illustrated in the same figure. In this way, the robot does not get trapped. Finally, the behavior of the mobile robot in a more cluttered environment is illustrated in Fig. 44. These results clearly show that this system is able to navigate in complex environments such as mazes. 8. Conclusions Fig. 42. Finding a target under a long loop obstacle. Fig. 43. Finding a target in a maze. In all of the cases in which the target is surrounded by cluttered obstacles, which form the convex loop, the proposed algorithm, described by Eqs. (48)–(62), helps the robot to find the target (Figs. 40 and 41). The algorithm works very well in the case when a cluttered environment consists of walls in the form of long loops, which is always convex in order to current robot location (Fig. 42). In this paper, we present a new mobile robot navigation strategy based on the soft computing methodologies in an a priori unknown environment. The aim was to obtain robust robot control suitable for on-line applications with real-time requirements. For this purpose, soft computing methodologies, such as fuzzy logic and other reasoning techniques, were successfully used. A dynamic model of a mobile robot with nonholonomic constraints is derived first. The special feature of this model is that the main variables are the angular velocities of the wheels. Due to this approach, the impossibility of lateral motion is embedded into the model. In addition, such a model is easily simulated. The proposed navigation system consists of three control subsystems. The low-level control of robot velocities is designed using PI controllers. The efficiency of this controller is demonstrated by various trajectory trackings. The coefficients of the medium-level nonlinear backstepping position controller are adjusted by a genetic algorithm. This controller reaches good position-tracking performance, but control torque values are too big. To solve this problem, a new control law is introduced which provides velocity servo inputs, and in this way it significantly decreases the control input torques. The high-level control system consists of sonar data interpretation and fusion, map building, and path planning. 1004 J. Velagic et al. / Robotics and Autonomous Systems 54 (2006) 989–1004 The sonar data interpretation and fusion were realized using the Dempster–Shafer theory of evidence. The local map is represented by an occupancy grid with the updates obtained by Dempster rules of combination. An efficient local path planning algorithm is required to achieve real-time operation. The proposed local planning is based on repulsive and attractive forces and the pruning of relevant obstacles by fuzzy rules. It shows robust and stable performance and simulation results. For the local minima avoidance problem, an algorithm that does not need to memorize previous movement is proposed. A possible extension of this work is the navigation of a mobile robot in a dynamic environment. References [1] J. Borenstein, Y. Koren, The vector field histogram — fast obstacle avoidance for mobile robots, IEEE Transactions on Robotics and Automation 7 (1991). [2] W. Chiang, J. Lee, Fuzzy Logic for the Applications to Complex Systems, World Scientific, Singapore, 1996. [3] D. Driankov, H. Hellendoorn, M. Reinfrank, An Introduction to Fuzzy Control, Springer-Verlag, Berlin, 1996. [4] G. Dudek, M. Jenkin, Computational Principles of Mobile Robotics, Cambridge University Press, Cambridge, 2000. [5] M. Egerstedt, X. Hu, A. Stotsky, Control of mobile platforms using a virtual vehicle approach, IEEE Transactions on Automatic Control 46 (11) (2001) 1777–1882. [6] T. Hu, S.X. Yang, Real-time motion control of a nonholonomic mobile robot with unknown dynamics, in: Proceedings of the Computational Kinematics Conference, Seoul, Korea, 2001. [7] H.P. Huang, P.C. Lee, A real-time algorithm for obstacle avoidance of autonomous mobile robots, Robotica 10 (1992) 217–227. [8] S. Ishikawa, A method of indoor mobile robot navigation by fuzzy control, in: Proceedings of the International Conference on Intelligent Robot and Systems, Osaka, Japan, 1991, pp. 1013–1018. [9] M. Kam, X. Zhu, P. Kalata, Sensor fusion for mobile robot navigation, Proceedings of the IEEE 85 (1) (1997) 108–119. [10] Y.C. Kim, S.B. Cho, S.R. Oh, Map-building of a real mobile robot with GA-fuzzy controller, International Journal of Fuzzy Systems 4 (2) (2002) 696–703. [11] B. Kosko, Neural Networks and Fuzzy Systems, Prentice Hall, NJ, 1992. [12] K.M. Krishna, P.K. Karla, Solving the local minima problem for a mobile robot by classification of spatio-temporal sensory sequences, Journal of Robotic Systems 17 (10) (2000) 549–564. [13] B. Lacevic, J. Velagic, B. Perunicic, Evolution of parameters of nonlinear position control for dynamic model of mobile robot with friction, in: Proceedings of the 16th IFAC World Congress, Prague, Czech Republic, 3–8 June 2005, Paper no. 2682. [14] V.J. Lumelsky, A comparative study on the path performance of mazesearching and robot motion planning algorithms, IEEE Transactions on Robotics and Automation 7 (1991) 57–66. [15] P. Morin, C. Samson, Application of backstepping techniques to timevarying exponential stabilisation of chained systems, European Journal of Control 3 (1) (1997) 15–36. [16] U. Nehmzow, Mobile Robotics: A Practical Introduction, in: Series on Applied Computing, Springer-Verlag, London, UK, 2000. [17] G. Oriolo, G. Ulivi, M. Vendittelli, Real-time map building and navigation for autonomous robots in unknown environments, IEEE Transactions on System, Man and Cybernetics part B 28 (3) (1998) 316–333. [18] G. Oriolo, A. De Luca, M. Vendittelli, WMR control via dynamic feedback linearization: design, implementation and experimental validation, IEEE Transactions on Control System Technology 10 (6) (2002) 835–852. [19] D. Pagac, E. Nebot, H. Durrant-Whyte, An evidential approach to probabilistic map-building, IEEE Transactions on Robotics and Automation 14 (4) (1998) 623–629. [20] M. Ribo, A. Pinz, A comparison of three uncertainty calculi for building sonar-based occupancy grids, Robotics and Autonomous Systems 35 (2001) 201–209. [21] O. Takahashi, R.J. Schilling, Motion planning in a plane using generalized Voronoi diagrams, IEEE Transactions on Robotics and Automation 5 (1989) 143–150. [22] H.G. Tanner, K.J. Kyriakopoulos, Backstepping for nonsmooth systems, Automatica 39 (5) (2003) 1259–1265. [23] S.G. Tzafestas, Advances in Intelligent Autonomous Systems, Kluwer, Dordrecht, Boston, 1999. [24] J. Velagic, B. Perunicic, M. Hebibovic, N. Osmic, Autonomous mobile robot navigation system based on soft computing methodologies, in: Proceedings of the IEEE International Conference on Mechatronics and Robotics, MechRob2004, 13–15 September 2004, Aachen, Germany, 2004, pp. 695–701. [25] J. Velagic, B. Lacevic, B. Perunicic, New concept of the fast reactive mobile robot navigation using a pruning of relevant obstacles, in: Proceedings of the IEEE International Symposium on Industrial Electronics, ISIE 2005, 20–23 June 2005, Dubrovnik, Croatia, 2005, pp. 161–166. [26] A. Ziloouchian, M. Jamshidi, Intelligent Control Systems Using Soft Computing Methodologies, CRC Press, Boca Raton, 2001. Jasmin Velagic Ph.D. received a M.Sc.E.E. degree from the Faculty of Electrical Engineering and Computing, University of Zagreb, Croatia in 1999 and a Ph.D. degree from the Faculty of Electrical Engineering, University of Sarajevo, in 2005. He was a teaching assistant with the Faculty of Electrical Engineering, University of Sarajevo, from 1997 until 2001. During 2001–2006, he was a senior assistant at the same faculty. Since 2006, he has been an assistant professor with the Department of Automatic Control and Electronics, University of Sarajevo. His research interests include intelligent control, mobile robotics, marine systems, industrial automation, and adaptive and robust control. Dr. Velagic is a member of IEEE Control System and IEEE Robotics and Automation Societies. Bakir Lacevic received a Diploma in electrical engineering from the Faculty of Electrical Engineering, University of Sarajevo, Bosnia and Herzegovina, in 2003. He is a teaching and research assistant at the departments for Control Systems and Computer Science at the same faculty, and he is currently working towards an M.S. degree. His research interests include evolutionary computation, mobile robotics, and computational intelligence in control. Professor Branislava Perunicic received her Candidate of Sciences degree (an equivalent to Ph.D.) from the Institute of Control Problems of the Academy of Sciences of the USSR in 1968. Her dissertation treated conditions of the existence of sliding modes and invariance conditions in sliding mode motion which occurs in Variable Structure Systems. She is currently professor of Electrical Engineering at the Department of Electrical Engineering of the University of Sarajevo. Her research interests are variable structure control, identification, and soft computing applications in control. She has served as a reviewer of scientific journals. Professor Perunicic is an elected member of the Academy of Sciences of Bosnia and Herzegovina. She is also the IEEE Bosnia and Herzegovina section chair.