Reactive Local Navigation

Transcription

1 Reactive Local Navigation Daniel Castro*, Urano Nunes, António Ruano* Institute of Systems and Rootics ISR *University of Algarve, University of Coimra PORTUGAL Astract In this paper a reactive local navigation system is presented, for an autonomous non-holonomic moile root navigating in dynamic environments. The reactive navigation system integrates an ostacle detection method [4] and a new reactive collision avoidance method. The sensory perception is ased in a laser range finder (LRF) system. Simulation results are presented to verify the effectiveness of the proposed local navigation system in unknown environments with multiple moving ojects. The comparison of results are shown, etween two different approaches, a Velocity Ostacle (VO) approach and our own, ased in the Dynamic Window (DW) concept. I. INTRODUCTION A typical application for a moile root is to assist human eings in indoor environments, like offices. These roots should e ale to react appropriately to unforeseen changes in the environment or to unpredictale ojects locking their trajectories. Local navigation techniques are the ones responsile to achieve these reactive issues. These techniques are sensory-ased approaches, using only local sensory information, or an additional small fraction of the world model. Although a considerale amount of work on ostacle avoidance and local navigation for moile roots exist, many approaches do not consider the dynamic activity of moving ostacles and determine their motion commands without a consistent representation of the surrounding free space. This paper is organised as follows. In Section II, a rief description of related work is introduced. In Section III, the dynamic window approach is riefly presented. Section IV, summarises the VO approach. Section V presents the laser range finder (LRF) ased ostacle detection system. Section VI introduces the new reactive local navigation method. In Section VII, simulation results are shown with discussion. Finally, concluding remarks are given in Section VIII. II. RELATED WORK Some of the most popular reactive collision avoidance methods are ased on artificial potential fields [13], where the roots steering direction is determined assuming that ostacles assert repulsive forces on the root and the goal asserts attractive forces. These methods are extremely fast and they typically consider only a small suset of ostacles near the root. An extended version of this approach was introduced in [12]. In the vector field histogram approach [2] an occupancy grid representation is used to model the roots environment. The motion direction and velocity of the root are computed from the transformation of the occupancy information into a histogram description. All these methods calculate the desired motion direction and steering commands in two different steps, which is not acceptale in a dynamic point of view. The curvature-velocity method [15] and the dynamic window approach [9] are ased on the steer angle field approach [7]. It is assumed that the root moves in circular paths, and the search of motion commands is performed directly in the space of translational and rotational velocities. In oth approaches the roots kinematics and dynamic constraints are considered y constraining the search space to a set of admissile velocities. In spite of their very good results for ostacle avoidance at high velocities (60 cm/s ~ 95 cm/s) the local minima prolem persists. The dynamic window approach was extended to use a map in conjunction with sensory information [10] to generate collision free motions. A ayesian approach to ostacle avoidance was linked with gloal path planning [11]. These approaches require a previous knowledge of the environment for the execution of the motion command. New approaches comine a model stage with a planning stage [3, 1]. These approaches allow roust execution of high-velocity, go-to-goal, local minima free, comining a wave-propagation technique [14] starting at the goal position with the DW approach. Introducing ostacle motion estimation and prediction, a different framework called the Velocity Ostacle (VO) approach was proposed in [8], which determines potential collisions and computes collision-free paths for roots moving in dynamic environments. III. DYNAMIC WINDOW APPROACH The dynamic window (DW) approach [9] is a sensor-ased ostacle avoidance technique that takes into account the kinematics and dynamic constraints of differential and synchro--drive roots. Kinematic constraints are taken into account y directly searching the velocity space Vs of the root. This space is defined y the set of tuples (v,ω) of longitudinal velocities v and rotational velocities that are achievale y the root. Among all velocity tuples the ones that are selected are those that allow the root to stop efore it reaches a ostacle, given the current position, current velocity and the acceleration capailities of the root. These velocities are called admissile velocities. The set Va of admissile velocities is defined as Va = {( v, ω ), v 2 dist ( v, ω ) v ω 2 dist ( v, ω ) ω } (1) The function dist ( v, ω ) represents the distance to the closest

2 ostacle on the curvature defined y the velocity (v, ), measured y r. γ, where r = v ω is the radius of the circular trajectory and γ the angle etween the intersection with the ostacle and root position. The accelerations for reakage are v& and ω&. Introducing a rectangular dynamic window, we could reduce the search space to all velocities that can e reached within the next time interval, according to the dynamic limitations of the root, given its current velocity and its acceleration capailities. The dynamic window Vd is defined as, {( v, ω ), v [ v&. h, + vh &. ] ω [ ω & ω. h, ω + & ω h] } Vd = v c v c c c. (2) where h is the time interval during which accelerations v& and ω& will e applied and (, ) v ω is the current velocity. c c To determine the next motion command all admissile velocities within the dynamic window are considered, forming the resulting search space Vr defined as Vr= Vs Va Vd. Among those a velocity is chosen that maximise a certain ojective function (linear comination of three functions) where the alignment of the root to a goal position, the distance to the closest ostacles and its velocity could e considered, as in the following expression: (, ω ) ( α (, ω ) β (, ω ) δ (, ω )) G v = Max head v + dist v + vel v (3) The function ( v, ω) = 1 θ π head, where is the angle etween the direction of motion and the goal heading, is maximal to trajectories that are directed towards the goal position. For a realistic measure of the target heading we have to consider the dynamics of the rotation, therefore is computed at the predicted position that the root will reach when exerting maximal deceleration after the next interval. vel v, ω is used to evaluate the progress of the The function ( ) root on its trajectory. The three components of the ojective function are normalized to the interval [0,1]. Parameters α, β and δ are used to weight the different components. Their values are crucial to the performance of the method and root ehaviour. Using this approach, roust ostacle avoidance ehaviors have een demonstrated at high velocities [9]. However, since the dynamic window approach only considers goal heading and no connectivity information aout free space, it is still susceptile to local minima. Figures 1a) and 1) shows a root scan with a moving oject and the respective velocity space, with a dynamic window for v& =50cm/s 2 and ω& =60º/s 2. IV. VELOCITY OSTACLE APPROACH The VO approach [8] creates a velocity space, where sets of oject avoiding velocities and oject colliding velocities are computed. Avoidance manoeuvres could e done y selecting velocities to avoid a future collision within a given time-horizon, considering the dynamic constraints of the root. To compute the VO, we consider two circular ojects, A and, at time t 0, with velocities v and A v. Let the circle A represents the moile root and the circle represents the ostacle. First, we reduce the moile root to a single point  and enlarge the oject circle with the radius of creating ˆ (see Fig.2). Position and velocity vectors are attached to its centres. Then the Collision Cone CC, set of colliding relative velocities etween  and ˆ, is defined y { λ ˆ 0} CC = v (4) where v is the relative velocity of  with respect to ˆ, v = va v, and λ is the line of v. This cone is the gray sector with apex in Â, ounded y two tangents λ and f λ r from  to ˆ. Any relative velocity that lies inside the CC will cause a collision etween A and. The VO is otained y VO = CC v (5) λ ˆ λ f  λ r CC VO R v X Y v A W Z U T v S RAV Fig. 1. (a) Root workspace simulation. () Velocity space-vs. The gray area represents the non-admissile velocity tuples. The rectangle represents the dynamic window-vd. Four different points are represented in oth Figures with the same numer. v Fig. 2. Schematic representing two colliding ojects, the moile root circle (--), and the ostacle, circle (-.-). The circle ( ) represents the oject enlarged with the radius of A. The velocity ostacle (VO) and the reachale avoidance velocity (RAV) areas, which represent sets of relative velocities, are shown in gray.

3 where is the Minskowski vector sum operation, which adds the velocity v to each velocity in CC. To avoid multiple ostacles, the union of individual velocity ostacles m is done y VO = i= 1VO where m is the numer of ostacles. i The next step is the computation of the set of velocities that are dynamically reachale y the root in the next time interval. The reachale velocities (RV) set is schematically represented in Fig. 2 y the polygon RSTU. The set of reachale avoidance velocities (RAV), is defined as the difference etween the RV and the VO, polygons YSTZ and RXWU. A manoeuvre of avoiding ostacle can e computed y selecting any velocity in RAV, for example with an ojective function like (3). It is also possile to choose different types of avoidance manoeuvres, y selecting on which side of the ostacle the moile root will pass. Velocities in RAV su-set YSTZ will give to the root the possiility to avoid the ostacle from the rear, and the su-set RXWU will allow a front side avoidance manoeuvre. V. LRF ASED OSTACLE DETECTION Our oject detection system [4] is laser range finder ased and composed y four procedures: the segmentation, the classification, the tracking and the collision detection (see Fig. 3). Oject detection is achieved y the segmentation of one or two consecutive scan sets, and vectorization of the segments found. Oject tracking is also possile from the oject classification procedure presented in this paper. Finally a collision detection procedure estimate future collisions etween the root and the ojects detected. Localization Data (x,y,θ,goal) One Data Set Local Map Grid ased Laser One/Two Data Sets Static Ojects Segmentation Dynamic Ojects Our oject detection method is quite similar to the Dietmayer et al. [6] method. Some modifications were introduced in the data preparation efore the segmentation, and in the segments filtration process [4]. In this work the LRF has a scan angle of 180, with an angular resolution α. The laser range data measured in the instant k is represented y the set LRF K, of N data points P i, with angle α i and distance value r i. A. Segmentation αi LRFk = Pi =, i [ 0, N ] ri The segmentation process is ased in the computation of the distance etween two consecutive scan points (see Fig.4), calculated y ( ) 2 2 i i+ 1 i+ 1 i i+ 1 i i+ 1 i (6) d P, P = P P = r + r 2r r cos α (7) If the distance is less than the threshold chosen with (, ) min {, } d P P C + C r r (8) i i i i+ 1 ( α ) C1 = 2 1 cos (9) the point P i+1 elongs to the same segment as point P i. The constant C 0 allows an adjustment of the algorithm to noise and strong overlapping of pulses in close range [6]. The constant C 1 is the distance etween consecutive points in each scan. This distance is also used to search the scan data, on the left and on the right, for points not selected in the data analysis. This operation gives the possiility to the algorithm to catch points with different dynamic activity from the rest of the points just selected, like edge segment corners. Special tests could e done to comine segments that proaly elong to the same oject. Segments have a minimum size estalished in the eginning (3 points) though pairs and isolated scan points are rejected. This is a very simple way of noise filtering. Velocity Space Approach Heading Velocity Distance Classification Tracking Collision Detection SEG2 α β δ Max(Σ) ε = 1, If collision is detected; ε = 0, otherwise. SEG4 SEG3 SEG1 P i+1 r i+ P i α r i (v,ω) Fig. 3. Laser range finder ased ostacle detection system. Fig. 4. Schematic representing the segmentation process applied to a hypothetical scan data.

4 . Oject Classification After the asic segmentation process, the segments founded need to e descried y a set of points {,C}, where {A} is the closest point to the sensor and the other two {,C}, the two segment extremes. Oject classification could e done with a certain proaility, y matching the segment representation data, points {,C}, with models of possile ojects that could exist in the root scene. Generally, most ojects of interest in the root s or vehicle environment are either almost convex or can e decomposed into almost convex ojects. A dataase of such possile ojects is necessary, and a priori knowledge of the working environment is crucial to create it. E. Example Using a SICK LMS 200 indoor LRF with a scanning frequency of 75Hz and an angular resolution of 0.5 for a 180 scan, we show in Fig. 5, real comparing results etween our two-scan oject detection algorithm [4] and the one-scan oject detection approach [6], for a Scout root moving with 1m/s in front of another Scout equipped with a LRF. From the two-scan algorithm results only data segments for moving ojects, otherwise, the one-scan algorithm give us all the ojects found in the scan data. C. Oject Tracking In order to perceive the oject motion it is necessary to select a reference point of the oject which could e the segment centre or point A. Usually, segment point A is the one chosen [8, 6]. In this work the tracking system is simulated and considered as optimal. ecause our goals are the validation of the scanning algorithms and the reactive navigation approaches. A discrete Kalman Filter ased tracking system is under development. D. Collision Detection The collision detection process predicts inside a predetermined temporal window (n time intervals) if any of the ojects detected collide with the root. This process is done assuming that the following statement is true: two line segments do not intersect if their ounding oxes do not intersect [5]. The ounding ox of a geometric figure is the smallest rectangle that contains the figure and whose segments are parallel to the x-axis and y-axis. The ounding ox of a line segment p1 p 2 is represented y the rectangle ˆ, ˆ pˆ = xˆ, yˆ and upper right ( p1 p 2 ) with lower left point 1 ( 1 1 ) point pˆ 2 = ( xˆ ˆ 2, y2 ), where xˆ 1 min ( x1, x2 ) yˆ = min ( y, y ), xˆ = max ( x, x ) and yˆ max ( y, y ) =, = Two rectangles, represented y lower left and upper right ˆ, ˆ pˆ, p ˆ, intersect if and only if the points ( p p ) and ( ) following conjunction is true, ( xˆ xˆ ) ( xˆ xˆ ) ( yˆ yˆ ) ( yˆ yˆ ) (10) In this work, the ounding oxes represent ojects and their predicted trajectory. It is assumed that all known dynamic activity are constant in the next n time samples (for example, with n=20 we can predict the collision of moving ojects with 1m/s in a range of 4 m, with h=0.2s). In Fig. 5) it is presented the ounding oxes associated to all detected ojects. The ounding ox associated to the moving oject in front of the root represents the oject and its trajectory for the next n time intervals. Fig. 5. (a) Representation of the two-scan oject detection algorithm [4]; () Representation of the one-scan oject detection approach [6], with the ounding oxes used in the collision detection. VI. REACTIVE LOCAL NAVIGATION METHOD Our method, comines a LRF ased oject detection methodology with a velocity ased approach, like the dynamic window. Similar to Fiorini s VO approach, we have a collision avoidance technique, which takes into account the dynamic constraints of the root and the motion constraints of the neary ojects. As shown in figures 6a) and 6), for the same situation, oth approaches consider the dynamic constraints of the root, throughout the RAV area in Fig. 6a) and the Vr search space of DW area in Fig. 6). This method aims to calculate avoidance trajectories simply y selecting velocities outside a predicted collision cone, penalizing the cone and neary velocity areas and adding a onus to escape velocities. Fig.6.a) VO collision cones repesentation. The static ostacles collision cones are in white and the dynamic ostacle collision cone is in dark. ) DW representation of the onus cone for the same situation in (a).

5 The new ojective function to e maximised is: (, ) = ( α (, ω ) + β (, ω ) + δ (, ω ) + ε (, ω )) (11) G v w Max head v dist v vel v onus v where the alignment of the root to a goal position, the distance to the closest ostacles and its velocity can e comined with a onus/penalty function. Function (, ) onus v ω is descried as follows, v v w, 1, ω + ω rˆ rˆ onus( v, ω) = 0, otherwise 1, v v w ω, + ω rˆ + R rˆ + R (12) where rˆ is the estimated collision radius, ω an angular increment necessary to penalise the collision area and neary areas. The value of ω depends on the roots dynamic constraints and on collision detection information, such as the proximity to ostacles and relative velocities etween the root and the ostacles. This prevents the root to get undesirale collisions, from unknown changes in the oject path. The parameter R is necessary to compensate the collision radius rˆ when the radius value is too ig. From the estimated root position we calculate the trajectory collision radius through some trigonometric equalities. Knowing the root s and oject s actual position, P and R P O, and their actual velocities, it is easy to compute their estimated positions, P and ˆR P (see Fig. 7). ˆO Considering the inner triangles of the collision arc etween the root and ojects estimated positions, we have AC ~ DC, and the following equalities are true, Considering D C c = = =, a A D 2rˆ = a + c we otain 2 1 rˆ = + a 2 a 2 = ac (13) (14) All the components of the ojective function are normalized to the interval [0,1]. Parameters α, β and δ are used to weight the different components. ( x y ) P O o, k, o, k Pˆ O ( x y ) o, k + 1, o, k + 1 R ˆ( v,ω ) P ˆ R ( xr, k + 1, yr, k + 1) c A a C ( x y ) P R r, k, r, k Fig. 7. Collision avoidance ased on oject motion prediction: P, R P actual root / oject position, O P, ˆR P - estimated ˆO root/oject position, ˆR - estimated collision radius. narrow indoor real spaces, where the LRF is always detecting static ojects, the uilding walls. Ojects were modelled as rectangular shapes, for people or other roots moving in the corridor. The user in the eginning of a new simulation could specify their moving direction, velocity and diameter. In all examples the root started without any knowledge of the environment. Several tests were done to compare the asic DW approach, our reactive navigation system and the VO approach. A great effort was done to maintain similar as possile all simulation parameters. The optimization function used to select the est velocity in Vr and in RAV is equal to (3), with parameter values {α=0.8, β=0.5, δ=0.2}. Some differences exist in the dist function settings, ecause one spans a (v,ω) space and the other a Cartesian space. In Fig.8, we have an avoidance manoeuvre etween two moving ojects. The root starts with v 0 =50cm/s aligned with the goal position. The root must pass etween to moving ojects ecause the oject in front is moving in the goal direction. All methods have a similar ehaviour. D (v=50cm/s) VII. SIMULATION RESULTS Extensive simulation tests were done to validate and test the methods exposed aove. A sensor model was created for the LRF with the following specifications: each scan has an angle of view of aout 180, the maximum distance measured etween the target and the sensor was 300 cm, with ±1 cm of accuracy. The sensor is centred with the roots coordinate axis, aligned with the motion axis. The workspace is a typical indoor dynamic environment, a corridor with two escape ways for doors and a turn on the top, see Fig.8. The workspace dimensions are similar to (1m/s) (v0=50cm/s) Fig. 8. Avoidance manoeuvre etween two moving ojects.

6 In Fig. 9a) one of the goals of this work is shown. We have a root moving aligned with the goal position, ut the oject in front is moving in the roots direction. The success of the avoidance manoeuvre is totally dependent on the relative velocity value etween the root and the moving oject. The velocity space of the asic DW approach represents only static information, so this approach can not deal with this situation. The concept of select velocities that allow the root to stop efore it reaches a ostacle is only true if there are not any moving oject in the search space. With the asic DW the root tries to keep the heading orientation as possile and some time later tries to avoid a moving oject that it perceives as eing static. The VO approach easily deal with this situations ecause the collision cones impels the root automatically in a secure avoidance trajectory. Our reactive algorithm introduces in the velocity space a collision cone similar to the VO approach, giving to the DW approach the aility to deal with situations like this. In Fig. 9) an aortive manoeuvre is shown. oth strategies tried to pass etween the ostacles, and non of them have stopped without hitting the ostacles, or tried to follow the oject moving to the goal direction with slow velocity. VIII. CONCLUSIONS Results from oth reactive navigation strategies shows that oth approaches deal with success to unforeseen changes in the environment or to unpredictale ojects locking their trajectories. Some of the advantages of the VO approach are concerned to its space (v x,v y ) representation, which has rootostacles collision information represented, giving to the root the possiility to avoid ostacles as soon as they are detected. The (v,ω) space only represents the root s velocity limit to hit oject in their sensory range limit. Our local navigation scheme looks promising, in the way that it gives to the DW approach the possiility to avoid ostacles as soon as they are detected, too. (50cm/s) (v0=50cm/s) (30cm/s) (30cm/s) (v0=50cm/s) Fig. 9 a) Avoidance manoeuvre for a root moving aligned with the goal, with an oject moving in the roots direction. ) Aortive manoeuvre. Future work will e done in local space representation (topological and geometric). A perception scheme that gives to the root all local knowledg necessary to navigate etween multiple moving oject, negotiating secure trajectories, is necessary. IX. ACKNOWLEDGEMENTS This work was partially supported y a PhD student grant (D/1104/2000) given to the author Daniel Castro y FCT (Fundação para a Ciência e Tecnologia). X. REFERENCES [1] K.Arras, J.Persson, N.Tomatis and R.Siegwart, Real-Time Ostacle Avoidance for Polygonal Roots with a Reduced Dynamic Window, in IEEE Int. Conf. on Rootics and Automation ICRA 02, Washington DC, May 2002, pp [2] J. orenstein and Y. Koren, The Vector Field Histogram Fast Ostacle Avoidance for Moile Roots, IEEE Trans. on Rootics and Automation, 7(3), June 1991, pp [3] O. rock and O. Khati, High-Speed Navigation Using the Gloal Window Approach, in IEEE Int. Conf. on Rootics and Automation ICRA 99, Detroid, May 1999, pp [4] D. Castro, U. Nunes and A. Ruano, Ostacle Avoidance in Local Navigation, in IEEE Mediterranean Conference on Control and Automation MED 2002, Lison, July [5] T. Cormen, C. Leiserson and R. Rivest, Introduction to Algorithms, The MIT Press, [6] K. Dietmayer, J. Sparert and D. Streller, Model ased Oject Classification and Tracking in Traffic Scenes from Range Images, in IEEE Intelligent Vehicle Symposium - IV2001, Tokyo, May [7] W. Feiten, R. auer and G. Lawitzky, Roust Ostacle Avoidance in Unknown and Cramped Environments, in IEEE Int. Conf. on Rootics and Automation ICRA 94, San-Diego, May 1994, pp [8] P. Fiorini and Z. Shiller, Motion Planning in Dynamic Environments using Velocity Ostacles, Int. Journal on Rootics Research, 17 (7), 1998, pp [9] D. Fox, W. urgard. and S. Thrun, The Dynamic Window Approach to Collision Avoidance, IEEE Rootics and Automation Magazine, 4(1), March 1997, pp [10] D. Fox, W. urgard., S. Thrun and A. Cremers, A Hyrid Collision Avoidance Method for Moile Roots, in IEEE Int. Conf. on Rootics and Automation ICRA 98, 1998, pp [11] H. Hu and M. rady, A ayesian Approach to Real-time Ostacle Avoidance for Moile Roots, Autonomous Roots, 1, 1994, pp [12] M. Khati and R. Chatila, An Extended Potential Field Approach for Moile Root Sensor-ased Motions, in IEEE Int. Conf. on Intelligent Autonomous Systems - IAS 4, pp , [13] O. Khati, Real-time Ostacle Avoidance for Manipulators and Moile Roots, The Int. Journal of Rootics Research, 5(1), pp.90-98, [14] J-C. Latome, Root Motion Planning, Kluwer Academic Pulishers, oston, [15] R. Simmons, The Curvature-Velocity Method for Local Ostacle Avoidance, in IEEE Int. Conf. on Rootics and Automation ICRA 96, Minneapolis, April 1996, pp