2.4 Rtree Data Structure
2.5.1 Occupancy Grid Formulation
The Rtree occupancy grid is probabilistic in nature and models the occupancy of its grid cells based on sensor observations. The proposed approach utilizes a specific sensor model to incrementally generate the grid structure and update the tree hierarchy composed of axis aligned rectangular cuboids. This chapter focuses on two different sensor models specifically the beam-based as well as thebeam-end point model, whose characteristics are discussed in the following section. Depending on the sensor model, grid cells are either initialized and updated at the beam end points or also along the path followed by the
2.5 Rtree Occupancy Grid
beam. Figure2.4 shows the grid cell initialization process for thebeam-based sensor model in context of the Rtree based occupancy grid in comparison to the standard occupancy grid whose grid structure is predefined. Initially all grid cells i.e. entries in a leaf node of the Rtree occupancy grid are of cubic volume based on the chosen resolution of the grid, axis aligned and do not overlap. However, the inner nodes MBRC can overlap as discussed in the previous section.
Standard Occupancy Grid Update Rtree Occupancy Grid Update y
x
y
x
Fig. 2.4: The cell initialization process for the Rtree based Occupancy grid in context of the beam based sensor model. The standard occupancy grid has a predefined structure in which cubic grid cells are initialized with 0.5 occupancy probability in a fixed region. In contrast, the Rtree based occupancy grid incrementally generates the grid structure as sensor observations are obtained. The robot is shown as a solid grey block.
Let ˆzt represent the sensor observation, where the subscript denotes the time index.
Consider a grid Gt={g1, g2, . . . , gn} at time t consisting of n cubic or variable resolution grid cells gi, i= 1, . . . , n. The notation used in this section is valid for cubic and variable resolution representations. Initially, the occupancy grid is composed of cubic grid cells, however as sensor observations are obtained the adaptation of grid cell resolution takes place (see Section 2.5.2) based on occupancy probabilities leading to variable resolution cells. The occupancy probability of any entry in the leaf node gi representing the ith grid cell can be derived from the posterior distribution over the cells given all the sensor observations ˆz1:t and robot poses ζ1:t
P(g1, g2, . . . , gn|ˆz1:t,ζ1:t).
A common assumption in the standard occupancy grid to reduce the dimensionality and computational complexity of the problem is to calculate the occupancy probability of a cell independently of other grid cells
P(g1, g2, . . . , gn|ˆz1:t,ζ1:t) =
n
Y
i=1
P(gi|ˆz1:t,ζ1:t).
Furthermore, by transforming the observations based on the pose estimates into the global
frame of reference we can omit the pose information
n
Y
i=1
P(gi|ˆz1:t,ζ1:t) =
n
Y
i=1
P(gi|z1:t),
wherez1:trepresents the transformed observations in the global frame of reference. By using the Bayes rule and incorporating the Markov assumption in the first term of the numerator, i.e. the current observationzt is conditionally independent of previous observations z1:t−1 given the robot pose, each grid cell probability can be written as
P(gi|z1:t) = P(zt|gi)P(gi|z1:t−1)
P(zt|z1:t−1) . (2.1)
Similarly, the first term of the numerator in (2.1) can be written as P(zt|gi) = P(gi|zt)P(zt)
P(gi) . (2.2)
By substituting (2.2) into (2.1),
P(gi|z1:t) = P(gi|zt)P(zt)P(gi|z1:t−1)
P(gi)P(zt|z1:t−1) , (2.3) the equation defining the occupancy probability of cell gi is derived. The probability that the cellgi is free can then be similarly calculated as
1−P(gi|z1:t) = (1−P(gi|zt))P(zt)(1−P(gi|z1:t−1))
(1−P(gi))P(zt|z1:t−1) . (2.4) Dividing (2.3) by (2.4) gives the odds,
P(gi|z1:t)
1−P(gi|z1:t) = P(gi|zt)P(gi|z1:t−1)(1−P(gi))
(1−P(gi|zt))(1−P(gi|z1:t−1))P(gi), (2.5) which can be simplified by simple algebraic manipulation as
P(gi|z1:t) = (1−P(gi|z1:t))
P(gi|zt)P(gi|z1:t−1)(1−P(gi)) (1−P(gi|zt))(1−P(gi|z1:t−1))P(gi)
,
P(gi|z1:t)
1 + P(gi|zt)P(gi|z1:t−1)(1−P(gi)) (1−P(gi|zt))(1−P(gi|z1:t−1))P(gi)
= P(gi|zt)P(gi|z1:t−1)(1−P(gi)) (1−P(gi|zt))(1−P(gi|z1:t−1))P(gi),
2.5 Rtree Occupancy Grid Shifting the left hand side terms to the right gives
P(gi|z1:t) = P(gi|zt)P(gi|z1:t−1)(1−P(gi))
(1−P(gi|zt))(1−P(gi|z1:t−1))P(gi) +P(gi|zt)P(gi|z1:t−1)(1−P(gi)), and inverting the right hand side gives
P(gi|z1:t) =
P(gi|zt)P(gi|z1:t−1)(1−P(gi)) + (1−P(gi|zt))(1−P(gi|z1:t−1))P(gi) P(gi|zt)P(gi|z1:t−1)(1−P(gi))
−1
, that can be easily transformed into [73, 125, 200]
P(gi|z1:t) =
which is a commonly used inverse sensor model in robotic mapping. P(gi|z1:t) represents the occupancy probability of the ith grid cell given all observations. P(gi) represents the occupancy probability of a grid cell prior to any observations. P(gi|zt) and P(gi|z1:t−1) represent the probability given the most current observationzt and observations since the beginning of time until time t−1 respectively. Eq (2.5) can be also be converted to log odds form to simplify the computation as it reduces the occupancy update to a simple addition operation
l(gi|z1:t) =l(gi|zt) +l(gi|z1:t−1)−lo, (2.7) wherel(gi) = logh P(gi)
1−P(gi)
i is the log-odds form whereaslo represents the prior which is the same for every cell. In literature [73,200], occupancy grids use a probability clamping threshold to prevent each cell of being over confident about its state. Following the same pattern, the Rtree based occupancy grid defines a minimum and maximum probability (or log-odds) threshold µmin, µmax respectively after which a grid cell is no longer updated.
The following subsection provides details about the inverse sensor models considered in this chapter as well as their properties.
Inverse Sensor Models
The proposed Rtree based occupancy grid is a generic approach to model a grid, hence it can be used with a wide range of inverse sensor models which define different update rules.
The termP(gi|zt) in (2.6) orl(gi|zt) in (2.7) determine how these updates are carried out.
In the domain of the robotics, the following inverse sensor models are commonly used
• Beam-based model
• Beam-end point model
Beam-based Model:
The beam-based approach models the physical properties of a beam, hence it considers the complete path traversed by the beam along with the beam end point which corresponds to an object/surface detected by the sensor. To be more specific a beam based model traces a ray through the grid [2, 18] and updates the end point as being occupied and the path of the beam as free space. Mathematically the above mentioned update is written as
P(gi|zt) =
Pocc if beam is reflected within volume Pfree if beam traversed volume
,
where the terms Pocc and Pfree are dependent on the sensor properties. Figure 2.4 shows an example of the grid cell update process for a beam-based sensor model.
Beam-end point Model:
As beam-based models tend to consider the complete path of the beam, they can be com-putationally expensive. To reduce computational cost, an alternative approach can be to ignore the path of the beam. Hence, beam-end point models tend to update the end points of the beam while ignoring the complete path traversed by the beam. Mathematically this update is written as
P(gi|zt) = Pocc if beam is reflected within volume.
Figure 2.5 shows the difference in update between the beam based and the beam-end point sensor model.
Beam-based sensor model Beam-end point sensor model Rtree Occupancy Grid Update
y
x
y
x
Fig. 2.5: Comparison of the beam-based and the beam-end point sensor model. The robot position is depicted as a solid square.