Repositorio Institucional de Documentos

Trabajo Fin de M´aster M´aster en Ingenier´ıa de Sistemas e Inform´atica Curso 2012/2013 Evaluaci´on y comparaci´on de sistemas de planificaci´on de navegaci´on de robots en entornos din´amicos Mar´ıa Teresa Lorente Cebri´an Director: Luis Montano Gella Departamento de Inform´atica e Ingenier´ıa de Sistemas Escuela de Ingenier´ıa y Arquitectura Universidad de Zaragoza Diciembre 2013 Evaluaci´on y comparaci´on de sistemas de planificaci´on de navegaci´on de robots en entornos din´amicos RESUMEN Este trabajo aborda un an´alisis comparativo de diferentes t´ecnicas de planificaci´on de movimientos en entornos din´amicos. Se basa en trabajos anteriores, en los que se desarrollaron dos t´ecnicas de planificaci´on de movimientos para un robot que se mueve en un entorno din´amico. Se trata de t´ecnicas de navegaci´on roboc´entricas en las que el modelo del entorno din´amico se basa en el espacio de velocidad-tiempo del robot, donde se representan tanto los objetos est´aticos como din´amicos. La primera t´ecnica trabaja sobre un espacio de velocidades bidimensional (velocidad lineal-velocidad angular). Explota la idea de identificar la mejor estrategia en funci´on de la situaci´on en la que se encuentra el robot, y que depende de la localizaci´on y velocidad relativas entre el robot y los obst´aculos. La segunda t´ecnica optimiza una funci´on objetivo en el espacio de velocidad-tiempo para obtener comandos ´optimos y trayectorias seguras, ponderando criterios de maximizaci´on de velocidad, seguridad (distancia a obst´aculos) y suavidad de movimientos. Adem´as, incorpora la t´ecnica desarrollada en el primer trabajo como heur´ıstica para mejorar la toma de decisiones, dando lugar a Strategies-Optimization. Para evaluar el rendimiento de la navegaci´on con dichas t´ecnicas se define una serie de m´etricas, que permiten seleccionar los mejores par´ametros de optimizaci´on para cada tipo de escenario. Estas m´etricas eval´uan y comparan los comportamientos en diferentes escenarios, lo que permite tener una evaluaci´on completa de todas las t´ecnicas. Adem´as, en aplicaciones reales los robots tienen que moverse en escenarios tanto de interior como de exterior. Sin embargo, para que los robot construyan un mapa del entorno, se localicen y navegen utilizan diferentes sensores, debido al tipo de informaci´on disponible (laser en interior y GPS en exterior) y a la incertidumbre de cada sensor en cada momento (p´erdida o reducci´on de precisi´on del GPS, pocas caracter´ısticas para construir el mapa). Esto provoca discontinuidades en localizaci´on o incluso p´erdida de ello, lo que debe evitarse. En este trabajo se presenta una t´ecnica de localizaci´on unificada para entornos de interiorexterior que permite una transici´on continua entre una zona de la que se dispone un mapa construido con los sensores l´aser a bordo del robot y una zona que utiliza el GPS para la localizaci´on del robot. Evaluation and comparison of planning systems for robot navigation in dynamic environments ABSTRACT This work addresses a comparative analysis of different techniques for robot motion planning in dynamic environments. It is based on previous works, which developed two motion planning techniques based on the velocity-time space of the environment to map static and moving objects. The first technique works on a bi-dimensional velocity space (linear velocity-angular velocity). It exploits the idea of situation-based strategies identification, which depends on several relative locations and velocities between the robot and the obstacles. The second technique searches for the optimal commands obtained by optimizing an objective function in the velocity-time space, weighing criterion for maximizing velocity, safety (distance to obstacles) and smooth of movements. In addition, it integrates the first technique as heuristic to improve the decision making procedure and thus yielding the Strategies-Optimization technique. Metrics for assessing the performance of the navigation with those techniques in different scenarios are defined, which allows obtain the best optimization parameters and have a complete comparison of the techniques. Moreover, in real robotic applications the robots have to move within indoor and outdoor scenarios. However, to achieve mapping, localization and navigation tasks, the robots utilize different sensors, due to the type of available information (rangefinder infoor, GPS outdoor) and the sensor uncertainty at each moment (lost or reduced precision of GPS, few features to build a map and to localize). These issues lead to discontinuities in localization or even lost of it, which has to be avoided. This work presents a unified localization technique for indoor-outdoor environments that allows a seamless transition between a mapped zone using laser rangefinder on-board sensors and a GPS based localization zone. Contents 1 Introduction 2 1.1 Motivation ..................................... 2 1.2 Structureofthework ............................... 3 2 Motion planning in dynamic environments 6 2.1 Relatedwork .................................... 6 2.2 Modeling the environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 2.2.1 The DOVTS space............................. 7 2.2.2 Dealing with multiple objects in the DOVS ............... 10 2.2.3 Static objects in the DOVS ........................ 10 2.3 Decision making strategies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.4 Situation-Strategies planningtechnique...................... 12 2.4.1 Decisionvariables ............................. 12 2.4.2 Navigation planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.5 Cost-Optimization planningtechnique ...................... 17 2.5.1 Strategies-Optimization: integration of Situation-Strategies and CostOptimization ................................ 19 3Situation-Strategies evaluation and comparison with Cost-Optimization and Strategies-Optimization 21 3.1 Metricsforassessment............................... 21 3.1.1 Simulation scenarios . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 4 Seamless localization 25 4.1 Relatedwork .................................... 25 4.2 Localization framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 4.3 GPS-basedorientation............................... 26 4.3.1 Improvement of the GPS-based orientation method . . . . . . . . . . . 28 4.3.2 Results ................................... 30 5 Conclusions 33 A Simulations 35 A.1 Cluttered scenario ................................. 35 i B Article: Seamless indoor-outdoor robust localization for robots 37 List of figures 51 List of tables 51 Bibliography 53 1 Chapter 1 Introduction 1.1 Motivation In order to use robots in real environments, they need to adapt the motion to the obstacles appearing during the mission. A lot of work has been devoted over the years to developing motion planning and reactive navigation techniques. Many of these have focused on static scenarios, and the solutions can be considered to be sufficiently robust. However, in many of the missions in which robots have to operate together with people (i.e. museums, rescue, factories, etc.), robust planning in dynamic environments is compulsory. [10] and [11] develop new planning techniques which confer the high degree of manoeuvrability needed in highly dynamic environments and that combine decision strategies and optimization techniques for safe navigation, working directly on the command space, in our case the velocity space. This work introduces the metrics defined in [15] to quantitatively asses the performance of the resulting motion, which in turn makes available well-founded criteria for selecting the control parameters of the planner. [15] include the results for [11] and its integration with [10]. One of the objectives of this work is to evaluate the technique developed in [10] with those metrics to obtain a comparison of the different planning techniques. Specifically, the tasks involved were: •Use the same software to construct the static and moving object maps of the scenario as in [11]. •Evaluate the technique using the metrics in scenarios with more obstacles and moving in non-straight trajectories. •Analize the results obtained and compare them with respect to the other planning techniques. The final result of the analysis and the conclusions extend the article [15], sent to the 2 journal Autonomous Robots. It is common in indoor robotics applications to assume a limited environment (a room, a building floor...) for localization purposes. This limitation is forced by the need of a finite map of the features in the zone to localize the robot. Due to the sparseness of the features needed to get a reliable localization system, the use of maps in outdoor scenarios is uncommon. Instead, outdoor applications usually utilize GPS based localization which avoids any limitation in the environment as it is accessible almost everywhere. However, there are few systems that provide a continuous localization for both indoor and outdoor scenarios in such a way that the robot is not confined in a limited space. Main difficulties come from the fact that very different sensors (odometry, IMU, rangefinders, GPS...) are needed to get a good estimation. These problems become more obvious during the transitions as measurements are more imprecise. Another objetive of this work is to contribute a unified framework for a seamless localization during navigation within different type of environments. The tasks related to this issue were: •Identify the zones of an environment where the localization system has to operate, which have different characteristics. •Implement a method to localize the robot integrating different sources of information, which weights them depending on the situation of the robot. •Propose a technique to determine the orientation of the robot based on GPS measurements. •Evaluate the technique with real experiments The results are presented in [21] (attached in the appendix) in the Fist Iberian Conference on Roboics. In section 4.3.1 an extension of the method to estimate the orientation from GPS measurements is proposed, and the results are written as part of [20], for the Int. Conference on Robotics and Automation. The work carried out is part of the projects Intelligent Technologies for Autonomous Transportation of Goods Indoors and Outdoors (TITAMie)and Teams of robots for Logistics, Maintenance and Environment monitoring (TELOMAN). 1.2 Structure of the work Following this introduction, chapter 2 deals with planning techniques for robot navigation in dynamic environments. It includes the modeling of the environment of the robot and the general procedure followed to take decisions for navigating. Then, it analyzes two planning techniques and their integration, as well as metrics for their evaluation and comparison. Next, 3 chapter 4 introduces a method to provide a seamless localization system between indoor and outdoor scenarios. The evaluation of the technique with real experiments is also shown. The conclusions about the work carried out are included in chapter 5. The appendix section contains the article Seamless indoor-outdoor robust localization for robots ([21]). Finally, a list of figures and tables, and the bibliography consulted are included. 4 Figure 2.5: Trajectory in green corresponds to when the robot passes before moving obstacle and the trajectory in blue to when the robot passes after moving obstacle. strategy is decomposed into different sub-strategies depending on whether the robot is before, inside or after the collision band. The execution of each motion strategy depends in turn on several decision variables, such as the relative location and the relative velocity between the robot and the objects, and the number of surrounding objects, formally defined in the next section. 2.4 Situation-Strategies planning technique The decision making strategies defined in the previous section, extend to different situations that the robot might detect by means of its onboard sensors. The situation identification is made from several decision variables computed in DOV S and WS spaces. This section is a formalization of the technique developed in [10]. 2.4.1 Decision variables A set of decision variables is computed to detect the situations. Some of the variables are obtained from the workspace (WS), others from the velocity space (DOV S). Table 2.1 summarizes all of them, including a brief explanation about their meaning. The table shows the acronyms, the meaning of the variables and their use in the different situations. The variables are utilized in the navigation strategies to identify the current situation, from which a specific motion action will be processed. Figure 2.6 depicts some of the decision variables mapped on the DOV S space. 12 Variable Meaning Situation WS RelPos relative position before, in or after the collision band all AngDis angular distance robot-goal all DOVS FV −DOV free and non-free (DOV )velocities all Vhigh-Vlow min.velocities to pass before-max.velocities to pass behind PassingBefore, SlowingDown VV alley min(Vhigh) PassingBefore UpperF ree free angular velocities at maximum linear velocity PassingBefore, PassingAligned LowerFree free velocities in lower zone (v<Vlow) SlowingDown BehindV el −F rontV el velocities to manoeuver with respect to the object AvoidingObject, PassingBehind MaxAngV el maximum angular velocity all SafeV el velocities to brake before crash SlowingDown BoundRight −BoundLeft boundaries of free velocities at both sides AvoidingObject SteeringDir mapped angular deviation all GoalDir mapped goal direction all Table 2.1: Decision variables in WS and DOVS spaces. Vhigh-Vlow are computed for all the radii. (a) (b) Figure 2.6: Decision variables on DOVS space. (a) Situation in which there are UpperFree (magenta, blue and pink). (b) Situation in which UpperFree is empty, and would lead to collisision (grey). 2.4.2 Navigation planning The robocentric planner developed in [10] assumes that there is a path planner at a higher level computing the consecutive subgoals to be reached in order to move to the final goal. The planner executes cyclically. It establishes a medium-term local plan in the horizon defined by the field of view of the sensors, but only the first step of this plan is executed for a sampling period. Thus the plan can be modified in the next steps according to de environment dynamics. In each sampling period the relative situation and motion between robot and objects are analyzed by means of the decision variables defined in the previous section and the situations identified. Figure 2.7 shows the situation tree, which is evaluated for every cycle, establishing the navigation strategy for the situation identified. It instances the decision strategies described in section 2.3. The leaves are all the situations that can be identified. The intuitive idea 13 for the applied actions is to choose velocity commands in successive steps in the velocity free space of DOV S (FV ). Roughly speaking, the criteria to select these commands are to apply a sequence of maximum linear and angular accelerations to reach the maximum linear velocity if it is possible. Figure 2.7: Situations Tree. Leaves represent all the situations that can be identified. Each of them has an associated navigation strategy. If there are no objects in the field of view, the FreeMotion situation is reached. The action applied aligns the robot towards the goal, reaching GoalDir mapped in DOV S, by applying a sequence of clothoid (to reduce the angular deviation to the goal) and anticlothoid (to reach the maximum linear velocity) trajectories. If there are objects in the field of view, the variable RelPos (Table 2.1) expressing the relative situation of the robot with respect to the collision band of the object is analyzed. If the robot has passed the object (AfterBand in the tree), the FreeMotion situation is reached. In the InBand state two situations can be reached. The first is FreeMotion that applies actions as previously explained. Otherwise, a CertainCollision situation appears when the robot is in a state of inevitable collision. This situation can only happen when objects suddenly appear close to the robot. From the BeforeBand state, one of the two global strategies is selected: RobotFront or RobotBehind. In turn, depending on the relative position and velocity of the robot and objects, and on the safety criteria, four situations can be reached: PassingBefore, and PassingAligned for the first strategy, and SlowingDown and AvoidingObject for the second. The current situation is detected by means of the decision variables. More details of each situation are now given. 14 a) RobotFront Strategy The Robotfront strategy shown in Fig. 2.5 is implemented by means of two possible substrategies, depending on the situation detected: PassingBefore or P assingAligned. These situations are distinguished by whether there are free velocities in the UpperFree zone or not. The actions to be applied in both situations are explained. PassingBefore Situation In this situation the robot passes before the object (see Fig. 2.8). To achieve this, an UpperFree velocity over Vhigh is selected (V3). This velocity is reached through a sequence of a clothoid (V1−V2) to align the robot towards the goal until reaching V2 velocity at maximum angular acceleration, and an anti-clothoid (V2−V3) to speed up and reach the V3 velocity in the minimum number of steps. The sequence is maintained until the next sampling period. PassingAligned Situation In this situation (see Fig. 2.9) a zone free of collision exists on one side of the DOV , which matches the velocities leading the robot in the same direction as the object movement. Safety is the priority in this case, so an extremal control to escape this situation has to be applied. The robot combines clothoid and anti-clothoid trajectories to align and escape. b) RobotBehind Strategy This strategy leads the robot to pass after the moving obstacle (see Fig. 2.5). When the UpperFree ={∅} velocities are not reachable (see Fig. 2.6b), the only option for the robot is slowing down and choosing LowerFree velocities so that the object passes first, using the BoundRight (BR) or BoundLeft (BL) velocities. The manoeuver is made using the SafeVel defined in Table 2.1, which is the highest linear velocity value at which the robot can brake to avoid a collision. The trajectories computed cause the robot to go around the object, passing after it and thus avoiding entering the collision band. Two situations can be found, depending on the free velocities that can be reached: SlowingDown or AvoidingObject (see Fig. 2.7). SlowingDown Situation This corresponds to the case in which UpperV el velocities in the DOV S cannot be chosen because they would lead to collision. Since the upper velocities are prohibited (see Fig. 2.10a), a prioritized safety criterion is taken. A sequence of anti-clothoid and clothoid trajectories is applied to escape from the dangerous velocities. When the UpperF ree 6={∅}, an anti-clothoid at maximum acceleration makes the robot move towards the goal at maxi15 (a) (b) (c) (d) (e) (f) (g) (h) Figure 2.8: Evolution of PassingBefore situation. The highest velocities (V3) can be chosen for the robot to pass before the moving object. (a) (b) (c) (d) Figure 2.9: Evolution in PassingAligned situation. The robot has to align to the direction of motion of the object to avoid collision by passing in front of the object, as can be seen between Loc3and Loc4, where a F reeMotion situation appears. mum velocities. Figure 2.10 represents this sequence. AvoidingObject Situation This represents a situation in which the moving object velocity lies in the zone of DOV , and so there is a danger of collision (see Fig. 2.11). There are two zones in UpperFree 16 (a) (b) (c) (d) (e) (f) (g) (h) Figure 2.10: Evolution in SlowingDown situation. The robot has to slow down to avoid the collision, by combining anti-clothoid trajectories until reaching high and free velocities, permitting the object passes before the robot. velocities free of collision, representing angular velocities higher than the represented by BoundRight(BR) and BoundLeft(BL). First the angular velocity closest to the GoalDir is chosen to escape from collision, reaching maximum linear velocity V1. Although these strategies are designed to reach the maximum velocities whilst maintaining the robot and environment safety, they do not ensure an optimal control in the sense of minimum time to goal, or under another criterium. The technique developed in [11], introduced in the next section, minimizes a cost function that balances several criteria related to time to goal and safety. 2.5 Cost-Optimization planning technique A main objective of this Master’s thesis is to compare Situation-Strategies with Cost-Optimization and Strategies-Optimization. Thus, this section summarizes the basis of the two last techniques but does not give details. To extend the information refer to [11] and [15]. The Cost-optimization planning technique ([11]) is based on the optimization of a cost function to compute the optimal motion commands, stating the optimization problem in the DOV TS space. Working on the bidimensional projection (DOV S) of the DOV TS as described in section 2.4 restricts the action and movement capacity of the robot, due to the 17 (a) (b) (c) (d) Figure 2.11: Evolution of AvoidingObject situation. The robot manoeuvres to pass in front of or behind the object, depending on the current relative situation between the goal and the robot. Blue line is GoalDir, representing every moment the direction for aligning the goal. tendency to choose movements in the velocity free space (FV ), when some commands under the forbidden obstacle surface (DOV T) are still available. First, the Cost-Optimization technique discretizes the DOV TS creating a mesh of rectangular prismatic cells with regular sizes, δv,δω,δt, balancing both precision and computational cost. The space defined by the forbidden surface is represented as occupied cells. Second, an A∗-like search algorithm is used in this space to find the optimal trajectory in terms of velocity sequences. It explores iteratively the free cells or nodes of the mesh, opening several paths from the initial node and generating a spanning tree from the lowest level corresponding to the current time (t= 0). In each iteration, the neighbor cells in FV T, corresponding to free velocities, are visited and the one with the lowest cost is expanded. Finally, a cost objective function is defined to guide the search, which balances time to goal, safety in terms of proximity to obstacles, and smooth motions with limited velocity changes: f(c) = g(c) + h(c) where g(c) is the cost of reaching the current cell from the initial cell and h(c) is a heuristic term that estimates the cost of reaching the goal from the current cell. This heuristic cost is defined from three weighted components: h(c) = αv·hv+αd·hd+αs·hs The first term hvis computed as the minimum number of iterations in vand wto reach the next velocity goal (or the number of cells to traverse in FV T ). In Fig. 2.12 the DOV TS for two moving objects is represented. As can be seen, a velocity goal is computed for each moving object. These are used to lead the search sequentially for the optimal trajectories. The second one hdestimates the time that the robot needs to reach the goal from the current location, without considering obstacles. The third component hscontributes to safe motions, avoiding obstacles. It is measured as the distance from the corresponding cell to the DOV T surface in the Taxis. 18 Figure 2.12: Two moving objects in DOVTS space. Goal(1) and Goal(2) represent two velocity goals (GoalVel) computed in the Free Velocity Sp`ace of DOVTS. 2.5.1 Strategies-Optimization: integration of Situation-Strategies and CostOptimization This section outlines the Strategies-Optimization technique, defined in [11] and extended in [15]. In the Cost-Optimization approach, the velocity subgoals are chosen only using a proximity to the aligning direction (GoalDir variable) criterion. This imposes some constraints on finding the optimal command in the presence of obstacles. The Situation-Strategies method allows velocity subgoals to be chosen using more complex criteria than that proposed in CostOptimization. For this reason, [11] propose an approach to integrate both techniques. The key point is the velocity cost term hvin the cost optimization function, which is redefined as: hs(c) = αv·hs v+αd·hd+αs·hs where hs vis now computed from the GoalV el of the Situation-Strategies technique. [11] evaluates the performance between Cost-Optimization and the integration of both Situation-Strategies and Cost-Optimization. Figure 2.13 shows two simulations in a simple scenario. The first one is carried out without using Situation-Strategies, that is, only the Cost-Optimization is active. The second one represents the same scenario in which both Situation-Strategies and Cost-Optimization are active. It can be seen that not using the Situation-Strategies yields a longer and more oscillatory trajectory. In the integrated Strategies-Optimization the maximum linear velocity is also maintained for longer. 19 (a) (b) Figure 2.13: Image taken from [11], where simulations in two cases are displayed: (a) Only the CostOptimization technique is active, αv= 0, αd= 1, αs= 1. (b) Both techniques are active, αv= 1, αd= 0.5, αs= 1. Notice that in the second case the maximum linear velocity is maintained whilst in the first one the time to goal is higher and the trajectory more oscillatory. 20 Chapter 3 Situation-Strategies evaluation and comparison with Cost-Optimization and Strategies-Optimization 3.1 Metrics for assessment [15] defines a methodology to evaluate the quality of the movement with the Cost-Optimization and Strategies-Optimization techniques, as a function of the αparameters of the cost function. Table 3.1 shows the metrics used for evaluation, defined to evaluate the performance of the techniques and to determine the most suitable selection of the αparameters for the Cost-Optimization and Strategies-Optimization methods: the percentage in time to goal improvement, in smoothness of the motion measured as change of the velocity (acceleration), and in the safe distance. Instead of using absolute values for quantifying the criteria, [15] computes relative values with respect to the worst case in each scenario. The work carried out for the Master’s thesis focuses on using the same metrics to evaluate the Situation-Strategies approach and compare it with the others techniques. 3.1.1 Simulation scenarios Two kinds of navigation scenarios with moving objects have been chosen. The first includes few moving objects around the robot (uncluttered scenario, between 10 and 12)), and four different goals to achieve. The second contains a high density of moving objects reducing the ability of the robot to navigate (cluttered scenario, between 20 and 25), and also four different goals to reach. Both scenarios are non structured and the obstacles moving around reduce the robot capability of navigating. We have also introduced non-straight line trajectories for the objects, to test simultaneously the robustness of the navigation techniques. Figure 3.1 depicts a snapshot of the simulation in one of the scenarios. Appendix A contains several steps during one of the simulation in the cluttered scenario. 21 Rgps=  Rxgps 0−wn LRxgps sin ˆα 0Rygps wn LRygps cos ˆα −wn LRxgps sin ˆαwn LRygps cos ˆα σ2  Otherwise, the orientation is taken from the prediction step (given by odometry/IMU), and then, ygps =  xgps ygps 0 ,R−1 gps =   1 Rxgps 0 0 01 Rygps 0 0 0 0    4.3.1 Improvement of the GPS-based orientation method The method explained above imposes some constraints to estimate the orientation of the robot based on GPS measurements. The steering angle φof the wheels were used, in order to estimate that the robot was moving in straight line, when φ≃0. But it can not always be a good measure of this behavior. The constraints to use this method to estimate the robot orientation are analyzed from several experiments. Next, we present the experiments performed to propose different constraints to the method which are less conservative but still achieving good orientation estimations. By weakening the restrictions we are able to obtain these estimations more often, reducing the orientation error accumulated from the odometry/IMU prediction. In the first experiment the robot moves at constant linear velocity and the steering angle φtakes values from equation (4.6). Parameter Amay take values from {0.1,0.2,0.3,0.4}and ωfrom the set {0.0,0.5,1.0,1.5,2.0}. φ(t) = Asin(ωt) (4.6) Figure 4.1 illustrates the results obtained with A= 0.4 and ω= 2. In figure 4.1a, the GPS positions of the robot are depicted, which show the robot is moving nearly in straight line. The steering angle, the orientation from odometry and the orientation computed from GPS measurements are shown in Fig. 4.1b, which reflects that the amplitude of the steering angle is bigger than the one provided by the odometry. So in general the robot orientation is a better source than the steering angle to determine whether the robot is moving in straight line. Figure 4.1b also depicts that when the robot is not moving the value of the orientation jumps. To analyze the conditions to get a good estimation of the orientation, we have measured the error in the estimations while navigating the robot as straight as possible along 12 mat constant speed. Figure 4.2 shows the square mean error of the estimations. For distances greater than 0.4 there is no improvement in the estimations. Therefore, whenever the procedure for estimating the orientation is initialized, the minimum distance dbetween the initial xgps0data and the next one to estimate the orientation should be of 0.4. In this experiment we found that the threshold th for the maximum variation of orientation accepted 28 -2 -1.5 -1 -0.5 0 0.5 -12 -10 -8 -6 -4 -2 0 Y (m) X (m) -4 -3 -2 -1 0 1 2 3 0 5 10 15 20 25 30 35 40 45 S te e rin g a n gle G P S -b as e d o rie n ta tio n Odometry orientation Radians Time (s) (a) GPS measurements. (b) Steering angle, orientation from odometry and GPS-based orientation. Figure 4.1: Results obtained from the experiment following (4.6) with A= 0.4 and ω= 2. 0 1 2 3 4 5 6 0.0 0.5 1.0 1.5 2.0 Minimum distance (m) MSE (rd²) Figure 4.2: Mean square error of the estimation of the orientation using GPS in straight line for different minimum distance constraint. to determine if the robot is moving in straight line were 1 ◦every 2 m, approximately. Otherwise, the orientation only can be computed from the odometry/IMU estimation. The constraint on the total distance Lbetween the initial xgps0measurement and the last one used to estimate the orientation is examined by performing several experiments in which the robot moves following non-straight paths. The frequency of the GPS measurements is 4Hz. Figures 4.3a and 4.3b show the number of total estimations and the mean of the covariances of the orientation obtained during the experiment for different values of L. Values higher than 1 mwould decrease the frequency of orientation updates as the number of total estimations is considerably reduced, so they are discarded. A value of L= 1 reduces almost in 25% the number of estimations computed with respect to L= 0.5, whereas there is no much difference in covariance. Thus, we consider that a total distance of 0.5 wolud increase the number of updates of orientation and provide good estimations of the orientation. The method to estimate the orientation with these new constraints is outlined in Algorithm 1, which follows equations from (4.3) to (4.5). In lines 7 and 8, ∆θodom is the total variation of orientation experimented in odometry during the interval between xgps0and 29 0 0.0001 0.0002 0.0003 0.0004 0.0005 0.0006 0.0007 0.0008 0 0.5 1 1.5 2 Mean Covariance (rd2) L (m) 0 20 40 60 80 100 120 0 0.5 1 1.5 2 Num. Estimations L (m) (a) Number of estimations of the orientation. (b) Mean of covariance of the total estimations. Figure 4.3: Results of the experiment to determine L. Algorithm 1 Estimation of the orientation from GPS measurements Require: 1: xgps0is the initial GPS measurement taken as reference 2: xgpsjis the current GPS measurement 3: th is the maximum variation of orientation in odometry accepted to determine if the robot moves in straight line 4: dis the minimum distance between xgps0and xgpsj 5: Lis the total distance to estimate the orientation 6: procedure EstimateOrientation(xgps0,xgpsj,th,d,L) 7: if ∆θodom > th then 8: xgps0=xgpsj 9: else 10: if Distance(xgps0,xgpsj)>=dthen 11: ˆ θj(xgps0,xgpsj), σ2 j(xgps0,xgpsj) 12: if Distance(xgps0,xgpsj)>=Lthen 13: return ˆ θ, σ2 14: xgps0=xgpsj 15: end if 16: end if 17: end if 18: end procedure xgpsj. If ∆θodom is not under th (i.e, the robot is not moving in straight line) we discard the GPS measurements considered for the estimation so far and initialize the GPS measurement of reference with the current one. Line 14 reflects that once the estimation of the orientation has been calculated, the process is initiated and the GPS measurement of reference is initialized with the current one. 4.3.2 Results This section presents the results obtained from the experiment to compare the GPS-based orientation improvement method with respect to the previous approach. The platform used is a Robucar-TT1equipped with IMU, odometry, three range-finder sensors and GPS receiver (figure 4.4). 1www.robosoft.com 30 Figure 4.4: Robucar-TT platform In [21] a real experiment in a large scenario within indoor and outdoor scenarios was achieved. Figure 4.5 reproduces the experiment, showing the continuity in localization and the limited uncertainty in the whole trajectory using the unified localization technique. This work extends the technique presented to improve the orientation estimation from GPS measurements when it can provide better estimations as explained in section 4.2. We focus our analysis only in a interval of the full experiment where the robot is not moving in a straight line (see green rectangle in figure 4.5). Figure 4.6a shows the pose estimations with both methods and the GPS data received. The previous method does not update the orientation from GPS because the robot is not moving in straight line. Now, we obtain better results in the pose estimations, which are closer to GPS measurements. In figure 4.6b the detail of the GPS-based orientation estimated from both methods is illustrated. It is clear the improvement in orientation estimation using the GPS-based technique. 31 Figure 4.5: Test scenario for evaluating our localization method. It covers an area of 12000 m2and the trajectory length is about 1000 meters. 26 26.5 27 27.5 28 28.5 29 -50.5 -50 -49.5 -49 -48.5 -48 -47.5 -47 -46.5 X (m) Y (m) Previous estimation GPS Our estimation (a) Pose estimations and GPS measurements. -0.5 0 0.5 1 1.5 46 48 50 52 54 56 58 Orientation (rd) Time (s) (b) Orientation estimations. Blue and magenta marks are the GPS-based orientation estimated using, respectively, the previous method and our method. Continuous lines represent the resulting orientation integrated with odometry/IMU measurements from previous (in red) method and our method (in green). Figure 4.6: Robot’s pose and orientation estimations in a non-straight stretch. 32 Chapter 5 Conclusions Regarding the planning of motion for robots in dynamic environments, a comparison of different planning techniques has been developed. The metrics defined in [15] and its use to quantitatively evaluate the performance of the navigation for Cost-Optimization and StrategiesOptimization have served to guide the evaluation of Situation-Strategies and to elaborate founded conclusions on the performance of the three techniques. As a result of the evaluation, sets of optimization parameters can be selected to be applied in the Strategies-Optimization planning technique. This methodology allows this selection to be made under a well-founded criterion, yielding a reduced number of best control parameters to be applied in different situations and scenarios. Future work will focus on the extension of the technique to sharing the decision making process among several robots and obtaining optimized plans. Regarding localization, the previously developed robust seamless continuous localization technique ([21]) for indoor-outdoor environments has been improved. A unified framework for continuous localization in large environments has been presented. The most suitable and accurate sensors in each moment are integrated to provide a continuous localization without discontinuities in the estimations and having a limited uncertainty, even when transitions indoor-outdoor or viceversa are produced. The parameters and conditions allowing the best sensor integration have been obtained from several experiments in real environments. 33 Appendix A Simulations A.1 Cluttered scenario This section shows several steps of the simulation performed to evaluate the SituationStrategies technique. This scenario contains a high density of moving objects, which reduces considerably the capability of the robot to navigate. Figure A.2 shows several snapshots during the simulation. Some of the objects describe non-straight trajectories to test the robustness of the navigation techniques. In such a scenario, with such amount of moving objects, the velocity space is usually complety occupied and thus the robot has to wait until the moving objects has passed. Figure A.1 describes the linear and angular velocity profiles during the simulation. The robot tries to maintain the maximum linear velocity when it is possible. However, it has to slow down several times during the simulation and even stop to assure the safety. At the end of the simulation, when almost all the objects have passed the robot can increase the velocity, reaching the goal at maximum linear velocity. Figure A.1: Velocity profiles in the cluttered scenario. 35 (a) (b) (c) (d) (e) (f) Figure A.2: Snapshots of the simulation in the cluttered scenario 36 Appendix B Article: Seamless indoor-outdoor robust localization for robots 37 Table 2: Update weights in different situations Situation γmap γgps γod Reset Indoors 0.5 0.0 0.5 No In–Out (inside map) 0.5 0.0 0.5 No Out–In (inside map) 0.5 0.0 0.5 Yes Outdoors (no GPS) 0.0 0.0 1 No Outdoors (good GPS) 0.0 0.5 0.5 No During a distance Lin which the robot moves straightforward, i.e. the steering angle is 0 and the orientation θremains constant, different GPS measurements are stored. For each measurement, an estimation of the orientation ˆ θjis calculated using (4). The covariances of the measurements Rgps0and Rgpsjare propagated by means of the jacobian to get the covariance of the orientation σj. Then, once the robot has reached the distance L, a final estimation of the robot’s orientation is calculated (5) as a weighted sum of the individual computed estimations (Figure 2), based on a mixture probability density function defined in [10]. The weights are valued depending on the covariance of each estimation (6). ˆ θj= arctan(ygpsj−ygps0 xgpsj−xgps0 ) (4) ˆ θ= n X j=1 wjˆ θj, σ2= n X j=1 wjσ2 j+ˆ θj−ˆ θ2(5) wj=1 n−1 1−σ2 j Pn k=1 σ2 k!(6) Parameter Lmay be tuned for each scenario. As Lincreases, more GPS measurements are used to compute orientation, leading to a more accurate estimation. However, it requires that the robot moves long straight trajectories. After several experimental tests, we have set L= 2.5mas a trade-off between accuracy and the need of long straight trajectories. The measurement terms hygps,Rgpsiused in the update phase in (3) are finally obtained as follows: 1. If the robot moves in a straight line (an orientation estimation can be computed), ygps =  xgps ygps ˆ θ ,Rgps =  Rxgps 0−wn LRxgps sin ˆ θ 0Rygps wn LRygps cos ˆ θ −wn LRxgps sin ˆ θwn LRygps cos ˆ θ σ2  (xgps0, ygps0) (xgpsn, ygpsn) Fig. 2: Orientation of the robot obtained from GPS measurements 2. If the robot does not move in straight line the orientation is taken from the prediction step (given by odometry/IMU), and then, ygps =  xgps ygps 0 ,R−1 gps =   1 Rxgps 0 0 01 Rygps 0 0 0 0    Note that the error in the position components (xgps, ygps) in GPS measurements are uncorrelated. However, as the orientation is computed from those components, there are non-zero correlation terms when the robot moves in straight line. 6 Results In this section we present the experimental results obtained to show the robustness of the methods proposed above. The platform used is a Robucar-TT1with all the sensors needed on-board and car-like motion capabilities (see Figure 3). First, to show the capabilities of the outdoor localization method in good GPS measurement conditions, including orientation estimation, we designed an experiment in which the robot autonomously navigates describing a rectangle defined by four goals on its corners. The navigation technique is an adaptation of the ORM technique [11] for car-like vehicles. It takes into account the kinodynamic constraints of the robot 1www.robosoft.com Fig. 3: Robucar-TT platform equipped with IMU, odometry, three range-finder sensors and GPS receiver and permits maneuvers to avoid obstacles and to guide the robot towards the goal. The details of the navigation technique are out of the scope of this paper. The goals are defined by GPS coordinates so that the localization should permit the robot to repeat the rectangular path with no drift although the odometry accumulates significant errors. Figure 4 shows the trajectory followed by the robot completing 11 times the rectangular path for a total distance travelled of 2696 mwith an average speed of 0.95 ms−1. 30 20 10 0 10 20 30 40 50 x (m) 80 70 60 50 40 30 20 10 0 10 y (m) Fig. 4: Estimated localization (solid line) and odometry measurements (dashed line) for the rectangle experiment. The goals are represented by stars. Figure 5 presents the evolution of the estimation drift during the experiment. We compare the filter localization estimation from the odometry/IMU and the GPS-based one. As known, odometry drift keeps growing with time, being the orientation error the main cause of the position error as well. At the end of the experiment, the odometry reaches a maximum of 18 mof position error and more than 0.3rad of orientation error. Meanwhile, the estimation of the localization method is able to keep bounded the error to a small value. 0 500 1000 1500 2000 2500 3000 Time (s) 0 2 4 6 8 10 12 14 16 18 Distance (m) 0 500 1000 1500 2000 2500 3000 Time (s) 0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 Angular distance (rad) Fig. 5: Position (left) and orientation (right) error of the filter estimation (solid line) and the odometry (dashed line) during the experiment Second, we designed a full experiment involving all kinds of situations and transitions. Figure 6 shows the trace of the robot during the experiment. The scenario covers an area of 12000 m2and the robot navigates for more than 1000 meters. In the zones with good localization, the variance of the estimation was below 0.1mfor xand yand below 0.05 rads for the orientation. The maxima in the variances of the estimations occur during transitions, the estimations are reset as well as during the outdoor with no GPS measurements due to the odometry error accumulation. However, transitions are temporal situations and, soon as a measurement is received, variances return to low values. In case of indoor-outdoor transitions, the covariance of the orientation may remain in high values for a longer period because the robot can only measure its orientation when it is moving in straight lines. Figure 7 show in detail some transition examples of the experiment. On the top left figure, the robot starts inside the building with map-based localization. After leaving the building, the GPS data arrive but their quality is too low to be useful, mainly because the robot navigates close to a wall. The robot leaves the map but still the GPS is imprecise so the odometry/IMU prediction is used until a good enough GPS signal is received. On the top right, all the trace remains inside the map limits. The robot navigates towards the building and when it is in front of the door, the GPS quality dramatically drops down and thus an outdoor-indoor transition is detected. In that moment, the map-based localization is reset, corresponding to a covariance enlargement. After a while inside the indoor zone, where the robot is continuously localized in the map by means of the laser and odometry/IMU measurements, the robot leaves the building and, when the GPS signal is good enough, an indoor-outdoor transition is detected and the localization system adapts to the outdoor situation. Fig. 6: A complete experiment. All the transitions are shown in which the filter works in the continuous localization. As can be seen in the second row of Figure 7, localization continuity is kept along the experiment. Only around transitions, some discontinuities may be found as transitions take place in zones where some of the sensors start to provide bad measurements. However, the biggest gap detected is about 0.5min xcoordinate. A bigger discontinuity was found in t= 250 s(Figure 7 bottom right) caused by a spurious bad GPS measurement. 7 Conclusions We present a mobile robot localization method that allows a robot to seamlessly switch between indoor and outdoor environments. We propose an outdoor GPS-based localization method which is able to estimate the position and orientation of the robot bounding the drifting error obtained from the odometry/IMU measurements. By using a GPS quality estimation and the robot localization (in map or not), we can determine which is the situation of the robot and select the measurements to achieve the best continuous localization. All the contributions have been tested in different experiments with a car-like platform. The data obtained in those experiments validates their functionality and robustness. 0 5 10 15 20 25 30 35 x ( m ) 40 35 30 25 20 15 10 5 0 5 y ( m ) 0 10 20 30 40 50 60 70 Time (s) 2.0 1.5 1.0 0.5 0.0 0.5 θ ( rad ) 25 20 15 10 5 0 5 x ( m ) 5 0 5 10 15 20 25 y ( m ) 240 260 280 300 320 Time (s) 1.5 2.0 2.5 3.0 3.5 4.0 4.5 5.0 θ ( rad ) Fig. 7: In the first row, two examples of transitions where it is shown the GPSbased localization (blue), the laser-based localization (red) and the estimation using our method (black). The ellipses represent the uncertainty. In the second row the state variables (in black) during these transitions are depicted. Red crosses represent the GPS data received. The background color indicates whether the robot is using the GPS (gray) or not (white) for localization. In future works, a smoother approach to the transition method can be studied, by using the weights in the covariance intersection framework to minimize the gaps during transitions. Also, an study of the limits of the GPS-based orientation method will be performed to determine the best parameters such as maximum curvature or minimum distance needed to get a good estimation. References 1. Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. The MIT Press (2005) 2. Kang, J., Kim, D., Kim, E., Kim, Y., Yoo, S., Wi, D.: Seamless mobile robot localization service framework for integrated localization systems. In: 3rd International Symposium on Wireless Pervasive Computing (ISWPC). (May 2008) 175–179 3. Goel, P., Roumeliotis, S., Sukhatme, G.: Robust localization using relative and absolute position estimates. In: Intelligent Robots and Systems, 1999. IROS ’99. Proceedings. 1999 IEEE/RSJ International Conference on. Volume 2. (1999) 1134– 1140 4. Collier, J., Ramirez-Serrano, A.: Environment classification for indoor/outdoor robotic matching. In: Canadian Conference on Computer and Robot Vision. (2009) 276–283 5. Pacis, E.B., Sights, B., Ahuja, G., Kogut, G., Everett, H.R.: An adapting localization system for outdoor/indoor navigation. In: SPIE Proc. 6230: Unmanned Systems Technology VIII, Defense Security Symposium, Orlando, EEUU (April 2006) 6. Julier, S.J., Uhlmann, J.K.: Using covariance intersection for SLAM. Robotics and Autonomous Systems (55) (2007) 3–20 7. Hentschel, M., Wulf, O., Wagner, B.: A gps and laser-based localization for urban and non-urban outdoor environments. In: Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on. (2008) 149–154 8. Gu, D., El-Sheimy, N.: Heading accuracy improvement of mems imu/dgps integrated navigation system for land vehicle. In: Position, Location and Navigation Symposium, 2008 IEEE/ION. (2008) 1292–1296 9. Fox, D.: Adapting the sample size in particle filters through KLD-sampling. The International Journal of Robotics Research 22(12) (December 2003) 985–1003 10. Bar-Shalom, Y., Li, X.R., Kirubarajan, T.: Estimation with applications to tracking and navigation. John Wiley & Sons, Inc. (2001) 11. Minguez, J.: The obstacle-restriction method for robot obstacle avoidance in difficult environments. In: Intelligent Robots and Systems, 2005. (IROS 2005). 2005 IEEE/RSJ International Conference on. (2005) 2284–2290 List of Figures 2.1 Workspace...................................... 8 2.2 DOVTS space.................................... 8 2.3 Merged objects in DOVS ............................. 10 2.4 Static objects in DOVS .............................. 11 2.5 Decisionstrategies ................................. 12 2.6 Decision variables on DOVS ............................ 13 2.7 Situationtree.................................... 14 2.8 PassingBefore situation .............................. 16 2.9 PassingAligned situation.............................. 16 2.10 SlowingDown situation............................... 17 2.11 AvoidingObject situation.............................. 18 2.12 Velocity goals in DOVTS ............................. 19 2.13 Cost-Optimization and Strategies-Optimization simulations . . . . . . . . . . 20 3.1 Evaluationscenario................................. 23 3.2 Metricsassessment................................. 24 4.1 Improving GPS-based orientation technique . . . . . . . . . . . . . . . . . . . 29 4.2 Minimum distance d................................ 29 4.3 Distance L...................................... 30 4.4 Robucar-TTplatform ............................... 31 4.5 Experiment. Localization evaluation . . . . . . . . . . . . . . . . . . . . . . . 32 4.6 Experiment. New method for GPS-based orientation . . . . . . . . . . . . . . 32 A.1 Simulation - cluttered scenario. Velocity profiles . . . . . . . . . . . . . . . . 35 A.2 Simulation - cluttered scenario. Snapshots . . . . . . . . . . . . . . . . . . . . 36 51 List of Tables 2.1 Decisionvariables.................................. 13 3.1 Evaluationmetrics ................................. 22 3.2 Combinations of αparameters .......................... 22 4.1 Situationdetection................................. 26 4.2 Update weights in different situations . . . . . . . . . . . . . . . . . . . . . . 27 52 Bibliography [1] P. Fiorini and Z. Shiller. Robot motion planning in dynamic environments. In International Symposium of Robotic Research, pages 237–248, 1995. [2] Dieter Fox. Adapting the sample size in particle filters through KLD-sampling. The International Journal of Robotics Research, 22(12):985–1003, December 2003. [3] T. Fraichard. Dynamic trajectory planning with dynamic constraints: A ‘state-time space’ approach. In Intelligent Robots and Systems ’93, IROS ’93. Proceedings of the 1993 IEEE/RSJ International Conference on, volume 2, pages 1393–1400 vol.2, 1993. [4] T. Fraichard and H. Asama. Inevitable collision states. a step towards safer robots? In Intelligent Robots and Systems, 2003. (IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on, volume 1, pages 388–393 vol.1, 2003. [5] E. Frazzoli, M.A. Dahleh, and E. Feron. Real-time motion planning for agile autonomous vehicles. In American Control Conference, 2001. Proceedings of the 2001, volume 1, pages 43–49 vol.1, 2001. [6] Dongqing Gu and N. El-Sheimy. Heading accuracy improvement of mems imu/dgps integrated navigation system for land vehicle. In Position, Location and Navigation Symposium, 2008 IEEE/ION, pages 1292–1296, 2008. [7] M. Hentschel, O. Wulf, and B. Wagner. A gps and laser-based localization for urban and non-urban outdoor environments. In Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on, pages 149–154, 2008. [8] S. J. Julier and J. K. Uhlmann. Using covariance intersection for SLAM. Robotics and Autonomous Systems, (55):3–20, 2007. [9] J. Kuffner and S. LaValle. Randomize kinodynamic planning. International Journal of Robotics Research, (6):378–400, 2001. [10] M. T. Lorente. Planificaci´on de la navegaci´on en entornos din´amicos. Master’s thesis, Escuela de Ingenier´ıa y Arquitectura, Universidad de Zaragoza, 2011. [11] C. Men´endez. Navegaci´on de robots aut´onomos en entornos din´amicos. Master’s thesis, Escuela de Ingenier´ıa y Arquitectura, Universidad de Zaragoza, 2012. 53