6.3
Laser-based Collision Avoidance on a Mobile Robot
One central task for a mobile robot is to safely maneuver in its environment. Therefore, the robot must find a collision-free path to a given target location. The objective is to find such a collision-free path while maximizing the robot’s speed. In dynamic environments the method applied must also account for obstacles which may suddenly appear in the robot’s trajectory. The control algorithm must either be able to drive around the dynamic obstacle or, if this is not possible, it has to stop right in front of the obstacle without colliding.
In this section we describe our method for attacking the problem of collision avoidance. We make use of the 360◦laser range finder which provides us with distance measurements in a fre- quency of10 Hz. Roughly, the algorithm works as follows: first, a collision-free path to the target point is found by applyingA∗search over an occupancy grid with a fixed size. The last visible
point on the path lying in the grid is selected as a local target point. Ray-tracing with the current orientation of the robot yields a collision point in front of the robot, i.e. the point where the transla- tional trajectory hits an obstacle. Using the target point, the collision point and the robot’s location we construct a triangle. By the way we construct this triangle we ensure that every grid cell in it is collision-free. To achieve optimized velocities for approaching the local target point,A∗searches
forhatrans, aroti-values which keeps the robot inside the triangle and which minimizes the time to
reach the target point. In the following we describe the method in detail.
Navigation Algorithm
Our algorithm is based on anA∗search in thehx, yi-space. The resulting path is modified in such
a way that the robot can follow this path for as long as possible without the need to calculate a new one in each cycle of the algorithm.
Input: laser scans, odometry position pr= (xr, yr), θr, target positionpt= (xt, yt), θt,
current velocityvr= (υr, ωr) Output: motor commandsc = (υm, ωm)
pathP = {}, Pprev= {} while¬atGoal(pr, pt) do
M = buildOccupancyMap(s, pr, pt, υr) if valid(P ) then P = Pprev
elseP = searchPath(M ) end if|P | = 0 then c = escapeAction else Popt= smoothPath(P ) c = calculateMotorCommand(Popt) end Pprev=Popt end
l2 l2 l1 l1 l1 l1 l2 l2 Laserbeam Laserbeam
(a) Two examples of obstacle integration (b) Speed0 m/s
(c) Speed1 m/s (d) Speed2 m/s
Figure 6.2: Examples of integrating obstacles in the occupancy grid map.
Each cycle, i.e. each time the navigation algorithm is called, it retrieves an up-to-date sweep from the laser range finder, the robot’s own pose, which is derived from the odometry, the target pose and the robot’s current velocity as its input. First, we integrate the distance measures into an occupancy grid and search for a path to the target point. If a path from a previous cycle of the algorithm exists, it is checked whether this path is still valid, i.e. if it is still collision-free. A new path is calculated otherwise. In the next step the path is smoothed. This means that in this extra computation step it is checked if the path could not be displaced further away from obstacles. This is especially useful in narrow doorways. Moreover, this makes the path more persistent, i.e. a once generated path will be used during subsequent cycles of the algorithm if possible which saves computation time. In a final step appropriate motor commands in terms of translational and rotational velocities are calculated. If no path could be found in the search which means that the robot is surrounded by obstacles, a special escape procedure tries to navigate the robot slowly out of the stuck situation. If the robot is not in an escape situation, the generated path is smoothed.
6.3. LASER-BASED COLLISION AVOIDANCE ON A MOBILE ROBOT 149
Building the Occupancy Grid Map. In the first step of our algorithm we integrate the laser sweep into an occupancy grid. In case of our soccer robots, we integrate360 single values, for the B21 only180.1 The occupancy grid is the basis for our path calculation. At the beginning,
the robot is located in the origin of the 2D occupancy grid representation. To ensure safe passage of the robot during the execution of the next driving command, we take account for the robot’s velocity. The faster the robot moves the greater the security distance to the obstacles should be, or the farer away from obstacles the calculated path should be. Therefore, in the step of integrating new sensor information, we represent a single distance measurement from the laser range finder not as a single occupied cell, we calculate an ellipsis which depends on the size and the velocity of the robot. Figure 6.2(a) shows two examples. The length of the robot isl1, the width of the robot is
represented asl2. Note that the width and the length of the robot (and therefore of the ellipse) also
depend on the angle of the laser measurement. Further note that the ellipses in Figure 6.2(a) are calculated when the robot stands still, i.e.vr= 0. In case the robot has a velocity v we multiply
l1andl2with the velocities inx and y direction:
l1= l1+ lsec+ l1· | sin θ · υ|, l2= l2+ lsec+ l2· | cos θ · ω|
withθ denoting the angle of the respective measurement, hυ, ωi the translational and rotational velocity of the robot, andlsecan additional security distance.
The idea of integrating the velocity of the robot is the following: to account for the safety aspect that the robot has to be able to stop in front of an obstacle at any time the size of the obstacles is extended. Thus, the robot itself can be represented as a mass point which simplifies the detection of possible collisions between the robot and an obstacle in the algorithm.
We use a fixed occupancy grid size. Thus, we cut off sensor readings at a certain distance to keep the search for an optimal path in a certain time-bound. When the robot is moving at high speed, we shift the position of the robot in the occupancy grid so that it uses more look-ahead.
Figure 6.2 shows the integration of laser readings into the grid map. The laser scans were taken from our department hall. Note that we only applied translational speeds and the robot was oriented down the hallway. Therefore, the ellipses were only expanded in parallel to the robot. On the right wall the robot can “see” into an open door. The repositioning of the robot inside the grid with increasing speed can be observed by looking at the doorway on the right-hand side which moves “down” the hall in Figure 6.2(b)-6.2(c). Our soccer robots have blind spots where the mounting for the layer with the camera is installed (see the markings in Figure 6.2(b)). One can observe these with the loops in the hallway walls. With increasing speed these blind spots disappear since the obstacles are further expanded.
Searching a Path withA∗. After integrating the new laser distance measurements into the
occupancy grid, we calculate an optimal path from the current position to the target position. If
the target position lies outside the grid range, then we project the target point onto the border of the grid to get a local target.
The heuristic used for theA∗search is the Manhattan distance between the robot and the target
point. The possible actions in each state of the search areA = {N, NE, E, SE, S, ...}, i.e. all unoccupied neighboring cells that can be reached in one step of the search. The cost function between two neighboring grid cellsi and j is defined as
cost(a, b) = (
d ifi, j adjacent, √
2d otherwise
and∀k∃j.cost(i, k) = cost(i, j) + cost(j, k). If the distance weight d is set to 1, the cost function describes the Euclidean distance of two grid cells (normalized by the cell size). There is no need to integrate a more complex cost function such as a function which decreases with the distance to an obstacle or is proportional to the occupancy of a grid cell as in (Stachniss and Burgard 2002) (which takes an extra value iteration step over the grid for each new target point)2because we can
guarantee that the robot takes a security margin around the obstacle as described above. Moreover, the calculation of such a cost function is more expensive than using our simple cost function plus taking the extra computation time to integrate the obstacles as ellipses into the grid.
This step of the algorithm yields a pathP = hpr, p0, p1, . . . , pti, where pr= (xr, yr) is the
robot position andptis the target position, which can either be the global target position or which
is a local target position that resulted from the projection onto the border of the grid.
Smoothing the Path. For stable navigation it is important that the path does not change too much from cycle to cycle. The worst case would be if the path planning method returns a new path each cycle which could lead to oscillating behavior. The reason for this alternating solutions is mainly the sensor noise (even if a sensor model is integrated when calculating the grid) and the fact that in the step of integrating the laser distance measurements rounding errors might occur (due to the sine and cosine operations). The result is that the integrated obstacle positions might swing. To prevent recalculations of the whole path we stabilize the path by shifting it further away from obstacles nearby. This is important for narrow doorways. With smoothing the path as described below the robot is able to traverse narrow passages faster than using solely the planned path.
The pathP is represented by a sequence of grid cells from the start to the target point. To enlarge the clearance of a way-pointpi= (xi, yi)T we need to find the obstacles perpendicular
to the path segment throughpi. These obstacles can be found by ray-tracing orthogonally to the
current gradient of the path segment.
Therefore we need to calculate the gradient for each way-point. We define the derivative of a
2We discuss the paper Stachniss and Burgard (2002) which is the most related work to our algorithm at the end of
6.3. LASER-BASED COLLISION AVOIDANCE ON A MOBILE ROBOT 151 Obstacles Solution pi pi−1 pi+1 λ1 i ρ1 i ▽pi=pi−1−pi+1 2 λ1 i= xyii + 1 · ∂pi/∂y −∂pi/∂x ρ1 i= xyii + 1 · −∂pi/∂y ∂pi/∂x
Figure 6.3: Derivation of solution
way-point by
▽pi= ▽(xi, yi)T =pi−1− pi+1 2
The left and right environment around the way-pointpican be found by
λεl i = xi yi + εl· ∂pi/∂y −∂pi/∂x ρεr i = xi yi + εr·−∂pi/∂y ∂pi/∂x
whereε denotes the step size, i.e. the number of grid cells searched around pi. The derivatives
∂pi/∂x and ∂pi/∂y are the x and y coordinates to the left and the right of the way-point pi. We
applyλεlorρεrand check if the grid cellλεl
i orρ εr
i are occupied. The new way-point is found
by applying eitherλ or ρ ((εl+ εr)/2)-times depending on whether a collision was found on the
left or on the right side of the way-point. Theε are bounded to a small number (we use a bound of 15 cells in our implementation) to search only in the neighborhood ofpi. Figure 6.3 shows an
example.
In each cycle of the algorithm we check if there exists a path from a previous cycle and if this path is still valid for the current situation. This validity check comprises the coordinate transforma- tion of the previous solution into the current occupancy map and it checks if the path collides with any obstacle in the world. If the path is still valid, we continue to use it, otherwise a re-planning is initiated.
Generating Motor Commands. Now we have calculated a safe path to the next target point. The robot will follow this path as long as no obstacles appear on it along the way. The collision- free path has to be realized, i.e. appropriate motor commands need to be generated. For the realization of the path we have to take the current orientationθrof the robot into account. If the
pc pp
pr
pw
Figure 6.4: Collision-free triangle
Given the current orientationθrof the robot one can find the next obstacle in the direction
θrby ray-tracing from the current position of the robot. The robot would collide with this point
when, from the current point of time on, only translation is applied to the motors. Now we search for a pointpcon this line whose connection to the last “visible” point on the calculated path,
denoted bypw, is collision-free. Together with the robot positionprwe can define the triangle
△(pr, pc, pw). Each grid cell in this triangle can be guaranteed to be unoccupied by construction
of the triangle. Figure 6.4 shows an example. The green line represents the planned path, the red dots represents the robot and its orientation, the blue lines yield the collision-free triangle. We construct this triangle to constrain the search for the motor commands to reach the target point in minimal time. For the calculations of the acceleration values below, we further need the following measures from the triangle:dw = |pwpc|, dc= |pcpr|, and α = ∠(prpc, prpw). The point ppis
the orthogonal projection ofpwonto the line throughprandpc.
Inside this triangle we have to find appropriate translation and rotation velocity pairs which take us fromprtopwin minimal time.
To realize the path on the motor, i.e. to drive to the target point, we have to find a sequence of translational and rotational accelerations such thatS(t) = S(0) + V (t) · t +12· A(t) · t2= p
tin
minimal time. Here,S(t) denotes the displacement, V (t) the velocity, and A(t) the acceleration of the robot at timet with the usual connections S(t) =R V (t)dt and V (t) = R A(t)dt between displacement, velocity, and acceleration for accelerated motion. We have to constrain the solu- tion such that all points reachable with feasiblehatrans, aroti lie inside this triangle and thus are
collision-free.S(0) denotes the current location of the robot.
We approximate the problem iteratively by (1) discretizing the acceleration space (as described below) and (2) by approximating the displacement bysi= si−1+ vi−1· δ(t) +12· ai−1· δ(t)2for
6.3. LASER-BASED COLLISION AVOIDANCE ON A MOBILE ROBOT 153
To find a sequence of motor commands taking the robot to the target position we applyA∗on
the acceleration space of the robot. The current pose of the robot and the collision-free triangle yield the information we need to approximate the velocity values: we know that the robot has to turn by angleα to reach the target point and has to cover the distance dw.
We search for rotational and translational accelerations in two separate steps. We do so to avoid the search in the five-dimensional pose-velocity space with a prohibitive branching factor for the search. The rotational velocities may take valuesvrot = crot· amaxrot · δ(t) + vrot0 with
crot ∈ {−1, 0, 1} during the search with amaxrot the maximal rotational acceleration. We find a
sequence of accelerations which cover the angleα in minimal time. It is obvious that when the angleα between the robot and the target position is less than 90◦the time to reach the target
is smaller when translation and rotation is performed at the same time than when the robot first turn towards the target point, stops, and then start to translate towards it. Note that if rotation minimizing the angle to the target point and translation towards to collision point is performed at the same time, all reachable positions lie inside the collision-free triangle. Driving along the edges of the triangle yields the worst solution to our approximation for which we can guarantee collision- freeness. Each path inside the triangle has a shorter path length and one needs shorter time to reach the target. If theα ≥ 90◦it takes less time first to turn towards the target, and then start to
translate towards the target point. This consideration gives us a heuristic when to start translating towards the target point. Ifα ≥ 90◦the distance between the robot’s position and the orthogonal
projection of the target point onto the orientation line of the robot (dp= prpp) is less than the
distance between the robot’s position and the collision point (dc= prpc). This relates the search
for translational velocities to the already calculated rotational velocities. This means that until we are oriented towards the target, our intermediate target distance isd = min(dc, dp). The objective
of theA∗search for translational accelerations is to minimize the time to cover this distanced. The
search branches overvtrans= ctrans· amaxtrans· δ(t) + v0transwithctrans∈ {−1, −23, . . . , 1}. amaxtrans
is the maximal translational acceleration. For each node in the search tree we check if the required translational acceleration is admissible w.r.t. the given rotational acceleration given the kinematic constraints of the robot. Note that in each iteration the collision pointpcand the projection point
ppas well as the triangle changes. After one time step the robot’s heading is towards a point
betweenpcandpw. After several iterations the robot is oriented towards the target point and only
translation is performed. Hence, we generate a sequence of velocities which takes the robot on a