board laser range finders (see Appendix C.1 “Object tracking based on the occupancy grid and planar laser
scanners”)). The result is a list of objects which are probably moving, their current position and velocity including the current direction, and their track (past sets of data with corresponding time stamp (see Figure 5.2)). Additionally, a circle segment is matched to the track to determine if the motion is straight or a curve, and in case of a curve to determine the radius. The data is provided via the local world model.
5.1. Reactive avoidance of moving objects
This behavior group is part of the reactive behaviors. It consists of two individual behavior modules that both generate a repelling vector to be merged with the remaining vectors (such as goal attraction or avoid- ance of static obstacles) which finally add up to the resulting velocity set-point vector (see Fig. 5.3 for the MCA implementation). The first behavior of this group is a safety reflex that generates a vector that points directly away from a nearby moving object to obtain a safety distance. This behavior can be applied even if hardly any information on the object is available. The second behavior needs a movement model from the local world model gained by mid-term observations of the object. Based on the movement model a probabilistic temporal repelling vector is calculated – in the following figures a complete field is shown for illustrative purposes, but in the control system actually only the vector relevant for the robot is calculated. Both behavior modules work based on the object database containing the detected moving obstacles in robot’s close vicinity along with their characteristics such as position and movement model. The concepts were implemented in F. Steinhardt’s diploma thesis: [Ste09b].
Fig. 5.3.: MCA implementation of the group for avoiding collisions with moving objects. The fusion behavior is the same one which accepts the input from the static collision handling behaviors. Hence, one single output velocity set-point vector is created for all reactive behaviors.
5.1.1. RB: The Escape Behavior
This behavior module lets the robot retreat directly away when in the the close vicinity of a moving object. To calculate the repelling velocity vector (~uEscape), a repelling vector (~RO) for each visible moving object (O) is calculated. The direction of the repelling vector is from the moving object towards the robot ( ˆOR) and
its length depends on the distance between object and robot (| ~dOR|) as well as on the velocity of the object (|~vO|). The area of influence is restricted by the maximum radius rmax.
~uEscape = ~FRO· fp· m · (1 − i) (5.1) ~FRO = ( ~SR if |~SR| 1 ˆSR otherwise (5.2) ~SR = O
Â
(~RO) (5.3) ~RO = OR ·ˆ max(rmax− | ~dOR|;0) rmax · |~vO| vmax (5.4) a = |~uEscape| (5.5) r = |~FRO| (5.6)The moving direction of the object is not relevant. The objective of this behavior module is to keep the robot away from moving obstacles as they tend to be unpredictable. Furthermore, this module is able to operate even when no prediction of the object’s motion is available. But obviously the parameter rmaxhas to be set to an reasonable value to curb the area of attention. 0.5m have been proven to be a suitable value.
Fig. 5.4.: A moving object (red circle) passes by the robot (green). Two examples for distance vector ( ~dM
O = (dX,dY))
are illustrated based on the object’s estimated motion. Left: the estimated movement is linear. Right: the estimated movement of the object is a curve. In both cases two components are calculated: the first one along the movement direction, the second one orthogonal.
5.1.2. RB: The Evade Behavior
This behavior module clears the way of approaching moving objects. A probabilistic estimation of the object’s path is calculated based on the estimated movement model of the object. Based on the resulting temporal estimation of the object’s location, the repelling velocity set-point vector is calculated, which lets the robot move away from the potentially occupied space and therefore makes a collision improbable.
To calculate the resulting repelling velocity vector (~uEvade) for the behavior module a repelling vector (~RO) for each visible moving object is calculated. The direction ( ˆvO?) of this object-dependent repelling vector is orthogonal to the movement direction of the object ( ˆvO). The length of the vector depends on the actual velocity of the object (|~vO|) and a force factor ( f ( ~dOM,~VO)). The force factor is a function of the distance vector between robot and the model-based movement estimation of the object ( ~dM
O, see Fig. 5.4) as well as the variance (~VO) of the movement model. It consists of two components, one based on the distance along the path of the object (the X-component) the other one based on the distance between the robot and the path of the object (the Y-component). Both components are limited by corresponding distance restriction
5.1. Reactive avoidance of moving objects
Fig. 5.5.: The complete field of influence of one moving object for three different Y-variances of the movement model. The width of the area of influence in Y-direction depends on the Y-variance
(VY) of the movement model. The length depends on the velocity
of the object (the object moves along the X-axis).
Fig. 5.6.: Two moving objects (circle) approach the robot (box). The various red lines illustrate possible repelling vectors. The object moving in a curve is harder to predict resulting in a raised variance and thus in a larger area of influence.
functions (Dmax
X (),DmaxY (VY,dX)). These mainly depend on the parameter for the maximum distance of influence (dmax
X ,dYmax). DYmax(VY,dX)also takes the variance of the estimation and the distance along the path (dX) into account. Therefore, the area of effect enlarges with the distance to the object and with the variance (see the plot in Fig.5.5).
~uEvade = ~FRO· fp· m · (1 − i) (5.7) ~FRO = ( ~SR if |~SR| 1 ˆSR otherwise (5.8) ~SR = O
Â
(~RO) (5.9) ~RO = ˆvO?· f ( ~dOM, ~VO)·v|~vO| max (5.10) f ( ~dM O,~VO) = ✓ 1 −min(dX;DmaxX ()) Dmax X () ◆ (5.11) · ✓ 1 −min(dY;DmaxY (VY,dX)) Dmax Y (VY,dX) ◆ a = |~uEvade| (5.12) r = |~FRO| (5.13) ~ dM O = dX dY ! ,~VO= VX VY ! ,DmaxX () =dmaxX (5.14) Dmax Y (VY,dX) =dYmax· (VY+1) · ((VY· dX) +1) (5.15) 5.1.3. Experimental resultsThis section presents some experimental results, showing the functions and the limitations of the reactive
behaviors. More extensive results on will be given at the end of this chapter when the complete system is
evaluated. Fig. 5.6 shows the area of influence – thus the amount of possible repelling vectors – generated by the two behavior modules Escape (circular part) and Evade (oval part). Here one out of the two objects
does not move straight ahead but in a curve. Additionally, the prediction of one object is bad, so the variance of the movement model rises. Figure 5.7 shows a scene with an shopping cart approaching from the front while Figure 5.8 presents the resulting evasive motion of the robot. The robot is repelled from the predicted path of the objects and moves around the critical zones smoothly. This concept works well as long as only few moving objects are in the close vicinity of the robot and the robot has sufficient space to avoid them. But scenes can occur where the robot cannot escape the objects. For example if two objects move in parallel and trap the robot in between them or too many objects are heading for the robot from different sides. To avoid these situations, the predictive handler for moving obstacles will be introduced in the next section. The combined concept is based on the division of duty between a predictive part for the mid-term control and a part for the reactive micro management.
Fig. 5.7.: In a corridor the robot InBOT is confronted with a shopping cart approaching from the front. On the right hand side the corresponding occupancy map with the velocity set point vectors from the individual behavior modules is sketched. The next figure (Fig. 5.8) shows InBOT’s reaction.
Tests have proven the expected limitations of the local and reactive approaches. Two examples can be seen in Figure 5.9. There are three major kinds of situations: (1) either the repelling vectors force the robot into a trap, (2) the situation can only be solved by withdrawing, or (3) the situation is very confused due to fuzzy information and/or a large number of objects. The first situation takes place for example when a objects moves in parallel to a wall in some distance, and the robot is closer to the wall, or when two objects move in parallel and the robot is in between. The second situation happens when the robot meets an approaching object in a corridor which is to narrow to pass each other. Here one of the two has to withdraw. This is be handled by the Escape behavior, but not in a satisfying manner. And in the third case with the large number of objects and the fuzzy information it would be best to avoid the crowd of moving objects altogether.
Obviously the reactive behaviors have to be supported by more anticipatory capabilities. This could be done by more intelligent fusion strategies for the repelling vectors, by modifying the shape of the repelling field depending on the environment and so forth. But this would still be local and reactive methods. Thus, in this work it was decided to provide a plan-based approach to be able to solve situations of all complexities.
5.1. Reactive avoidance of moving objects
Fig. 5.8.: Resulting evasive motions (the robot is indicated by the green rectangle, the moving objects’s track by the green line and the robot’s path by the red line).
Left: The standing robot frees the path of an approaching object. In the bottom scene the object was very hard to track resulting in a jittery prediction and thus a not optimal movement of the robot.
Right: Here the robot is moving itself while confronted with an approaching object (top: head to head, bottom: object is overtaking the robot).
Fig. 5.9.: Sketches showing the limitations of the reactive behaviors.
Top: Two objects (red circles) approach the robot along the indicated arrows. The robot is located in the middle between the two objects. By adding the two virtual force vectors, the robot will be forced to stay between the two objects.
Middle: As expected in the description of the top figure, a collision occurs. The red line illustrating the path of the robot shows that the Escape behavior tried to solve the situation, but it was too late as it had to decelerate the robot before accelerating in the opposite direction.
Bottom: Another example which cannot be handled by the reactive behaviors: a large object which fills the complete corridor forces the robot to withdraw. The Escape behavior will try to keep a safety distance but the retreat will be in an inefficient manner.