The two presented reactive behaviors manage the short term collision avoidance with moving objects with fast reaction times. But they do not perform a thorough analysis of the scene. This done by the planner being introduced here (originating again from [Ste09b]). It generates a spatio-temporal sub-plan in the local environment to reach a given target while taking the predicted movements of all detected moving objects into account. The goal is to basically avoid critical situations which could not be solved by reactive behaviors only (e.g. trapping situations between objects) or to avoid critical areas altogether. The plan consists – according to the requirements of the tactical layer – of sparsely distributed sub-targets with time constrains, it is not a trajectory for the robot.
TB: Plan execution The execution of the plan is performed by a associated plan execution behavior
module which hands new targets down to the reactive behaviors one by one – which again guarantee the safe behavior of the robot even if the planning process should fail or provide illegitimate goals. Figure 5.10 shows the implementation of these two modules in MCA2.
Fig. 5.10.: MCA implementation of the proactive planner: one module generates the plan, a second module inserts the sub-goals of of the plan into to goal-refinement chain of the tactical layer.
5.2.1. Baseline: Data and pre-processing
The environmental robot-centered 2D occupancy map of the LWM (in the case of the robot InBOT with a size of 10x10m) is extended with a time-dimension generating a 3D occupancy map (Z-axis for time). Static objects are assumed as time invariant and therefore span the complete Z-axis. The position of moving objects is altered according to the movement estimation of the object (see Fig. 5.11). A new grid has to be built for each plan: The time-distance t between two X-Y layers is set so that it matches the time interval needed by the robot to move one cell along the x axis at default speed (t =cellsizevde f – with vde f usually 0.5
5.2. Proactive avoidance of moving objects using spatio-temporal plans
Fig. 5.11.: All three figures show the same scene: a moving object (red circle with arrow) moves in the close vicinity of the robot (green rectangle) in a curved path (indicated by the black arrow).
Top left: The scene viewed in the MCAGUI. The tiny blue and red dots represent the measurements of the laser range finders.
Bottom left: The corresponding occupancy map is shown (the robot is located at the blue rectangle (but larger), the object is represented by the occupied cells (black) in the bottom right corner).
Right: predictive 3D occupancy map with enlarged obstacles (50 ⇥ 50 ⇥ 50 cells, cell size: 20cm ⇥ 20cm ⇥ 0.4sec). The grey cubes represent the blocked cells – by the static (and thus time-invariant) wall in the background as well as by the moving object indicated by the arc of cubes in the foreground. As the obstacles are enlarged, the illustration of the robot is too large here – the robot actually has the size of one cell.
to 1 m/s). The height (time) of the 3D occupancy grid limits the length of the path that can be found. The resolution of the basic occupancy grid is reduced and moving obstacles as well as special objects (the user, other low-priority robots (see chapter on multi-robot behaviors: Chap. 7)) are deleted from this occupancy grid. The static obstacles are enlarged (the size of the robot can now be assumed as point like) and added to the 3D grid. Additionally, for each time-step an enlarged obstacle is entered at the predicted positions of the moving obstacles. The resulting predictive 3D obstacle map is shown on the right-hand side of Fig. 5.11. It was decided to generate a new plan each time instead of altering the old one, because of the drastic changes in the local map due to the changing field of view and perspective when the robot moves, the dependency of the robot’s actual velocity and finally the fuzzy prediction of the moving objects’ behavior which suddenly can be subject to major changes.
5.2.2. Spatio-temporal calculation of safe path using an A⇤algorithm
A starting cell and the goal coordinates have to be given to calculate a path around the moving obstacles in the predictive 3D occupancy grid. The robot is positioned at the middle of the lowest time-layer of the grid – the start cell. As it is not important at exactly which point in time the robot reaches the goal, all cells that share the X-Y-coordinates of the goal point are possible goal cells for the A⇤search. If the goal point is
located outside of the occupancy map a substitute goal point located on the grid’s border is chosen. Figure 5.12 illustrates this setup.
The A⇤algorithm is forced to take a time step for each X/Y movement, therefore a cell has 9 neighbors which are located in the succeeding time layer t0=t +1. Occupied cells in the 3D obstacle grid are obviously off-limits.
The heuristic function used for the A⇤ search is the straight-line distance to the goal in an X-Y layer (pDx2+Dy2) multiplied with the factorp2 to estimate the needed time to get to the goal (this is equivalent
to the straight-line distance in the 3D occupancy grid). This factor is a correct estimation if the A⇤search can find a free way to the goal without having to wait or having to take a detour around an obstacle.
The search will be aborted if the current cell in the A⇤search is located in the top X-Y layer t = tmax. If the A⇤search does not find a path to the goal there is no free path to the goal in the given time constraint and the search has to be restarted on the next new occupancy grid data. In the meantime the robot relies on the reactive behaviors to avoid collisions.
Fig. 5.12.: The robot plans a path to a location which will be crossed by a moving object. So the robot has to reach the goal after the moving object has moved past it. The robot is located at the center of the grid (in the center of the X-Y-Plane, at the bottom regarding the time dimension). The goal is marked with the read boxes on the left hand side of the grid – obviously, it is suitable to reach the goal at any point of time, so the goal location is marked as goal throughout the complete time span.
Left: The blue boxes show the path planned by the A⇤.
Right: The green line is the optimized path. It only consists of the few kinks of the green line (accompanied by a time constraint).
5.2.3. Optimization of path
The path calculated by the A⇤consists of a list of sub-goal points. If the sub-goals are located too close to each other the flexibility of the Behavior-Based Control is impaired. If there are no obstacles, the path still consists of many sub-goals in a straight line even if only the goal point itself would be sufficient to provide a collision-free path.
Therefore, the path is simplified to contain only the needed points to drive around the obstacles by repeat- edly removing sub-goals and then testing for collisions with occupied cells (see Fig. 5.12). If the resulting path is collision-free the removed sub-goals are removed permanently.
5.2. Proactive avoidance of moving objects using spatio-temporal plans
5.2.4. Utilization of theBehavior-Based Control
Once a path is found and simplified, its sub-goals are given to the Behavior-Based Control as goal points. The Behavior-Based Control moves the robot to these points successively while utilizing the full range of
reactive behaviors.
5.2.5. Experimental results using the planner
The described algorithm was tested thoroughly. An easy example is displayed in Fig.5.13. A moving object approaches from the direction the robot intends to move in. After detecting the object, a plan is generated to move around the object. A more challenging scene is depicted in Fig. 5.14. This time, there is not sufficient space to move around the object. Therefore, the plan leads the robot back around a corner to let the object pass before continuing with moving towards the target.
Fig. 5.13.: Top: The robot’s target is the green “X” at the right side of the scene. A moving object approaches from this direction (red circle with arrow). Therefore a plan of sub-targets is generated that leads the robot around it. Middle and bottom: The scene from Figure 5.9 is shown again. Here the reactive behaviors failed. As indicated by the plan (green line with arrows) and the path taken by the robot (red line), the planner is able to solve this situation.
Fig. 5.14.: Here again a scene is taken up from Figure 5.9 where the reactive behaviors failed. The robot’s target is at the left hand side end of the corridor. A large moving object approaches from this direction (red circle with arrow). Therefore, a plan of sub-targets is generated that leads the robot back around the corner, lets it wait there until the object passed and finally leads to the target again. Top: plan in a cut open 3D view. Bottom/right: plan in 2D view in MCAGUI. Bottom/left: actual path taken by the robot.