Autonomous Navigation in Unknown Environments using Sparse Kernel-based Occupancy Mapping

02/05/2020 ∙ by Thai Duong, et al. ∙ University of California, San Diego 0

This paper focuses on real-time occupancy mapping and collision checking onboard an autonomous robot navigating in an unknown environment. We propose a new map representation, in which occupied and free space are separated by the decision boundary of a kernel perceptron classifier. We develop an online training algorithm that maintains a very sparse set of support vectors to represent obstacle boundaries in configuration space. We also derive conditions that allow complete (without sampling) collision-checking for piecewise-linear and piecewise-polynomial robot trajectories. We demonstrate the effectiveness of our mapping and collision checking algorithms for autonomous navigation of an Ackermann-drive robot in unknown environments.



There are no comments yet.


page 6

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Autonomous navigation in robotics involves localization, mapping, motion planning, and control in a partially known environment perceived through streaming data from onboard sensors [human_friendly_nav_guzzi_icra13, safe_auto_nav_pavone_rss18]. In this paper, we focus on the mapping problem and, specifically, on enabling large-scale, yet compact, representations and efficient collision checking to support autonomous navigation. Existing work uses a variety of map representations based on voxels [thrun2005probabilistic, octomap, ieee_map_rep_standard, voxblox], surfels [behley2018rss_surfel_slam], geometric primitives [Kaess_infiniteplanes], objects [Bowman_SemanticSLAM_ICRA17], etc.

We propose a novel mapping method that uses a kernel perceptron model to represent the occupied and free space of the environment. The model uses a set of support vectors to represent obstacle boundaries in configuration space. The complexity of this representation scales with the complexity of the obstacle boundaries rather than the environment size. We develop an online training algorithm to update the support vectors incrementally as new depth observations of the local surroundings are provided by the robot’s sensors. To enable motion planning in the new occupancy representation, we develop an efficient collision checking algorithm for piecewise-linear and piecewise-polynomial trajectories in configuration space.

Related Work.

Occupancy grid mapping is a commonly used approach for modeling the free and occupied space of an environment. The space is discretized into a collection of cells, whose occupancy probabilities are estimated online using the robot’s sensory data. While early work 

[thrun2005probabilistic] assumes that the cells are independent, Gaussian process (GP) occupancy mapping [OCallaghan2012gaussian, wang2016fast, jadidi2017warped] uses a kernel function to capture the correlation among grid cells and predict the occupancy of unobserved cells. Online training of a Gaussian process model, however, does not scale well as its computational complexity grows cubically with the number of data points. Ramos et al. [ramos2016hilbertmap]

improve on this by projecting the data points into Hilbert space and training a logistic regression model. Lopez and How 

[lopez2017aggressive] propose an efficient determinstic alternative, which builds a k-d tree from point clouds and queries the nearest obstacles for collision checking. Using spatial partitioning similar to a k-d tree, octree-based maps [octomap, chen2017improving] offer efficient map storage by performing octree compression. Meanwhile, AtomMap [fridovich2017atommap] stores a collection of spheres in a k-d tree as a way to avoid grid cell discretization of the map.

Navigation in an unknown environment, requires the safety of potential robot trajectory to be evaluated through a huge amount of collision checks with respect to the map representation [bialkowski2016efficient, luo2014empirical, hauser2015lazy]. Many works rely on sampling-based collision checking, simplifying the safety verification of continuous-time trajectories by evaluating only a finite set of samples along the trajectory [Tsardoulias2016planninggridmap, luo2014empirical]. This may be undesirable in safety critical applications. Bialkowski et al. [bialkowski2016efficient] propose an efficient collision checking method using safety certificates with respect to the nearest obstacles. Using a different perspective, learning-based collision checking methods [das2017fastron, pan2015efficient, huh2016learningGMM]

sample data from the environment and train machine learning models to approximate the obstacle boundaries. Pan et al. 


propose an incremental support vector machine model for pairs of obstacles but train the models offline. Closely related to our work, Das et al. 

[das2017fastron, das2019learning] develop an online training algorithm, called Fastron, to train a kernel perceptron collision classifier. To handle dynamic environments, Fastron actively resamples the environment and updates the model globally. Geometry-based collision checking methods, such as the Flexible Collision Library (FCL) [pan2012fcl], are also related but rely on mesh representations of the environment which may be inefficient to generate from local observations.

Inspired by GP mapping techniques, we utilize a radial basis function (RBF) kernel to capture occupancy correlations but focus on a compact representation of obstacle boundaries using kernel perceptron. Furthermore, motivated by the safety certificates in 

[bialkowski2016efficient], we derive our own safety guarantees for efficient collision checking algorithms.

Contributions. This paper introduces a sparse kernel-based mapping method that:

  • [leftmargin=2em,nosep]

  • represents continuous-space occupancy using a sparse set of support vectors stored in an -tree data structure, scaling efficiently with the complexity of obstacle boundaries (Sec. IV),

  • allows online map updates from streaming partial observations using our proposed incremental kernel perceptron training algorithm built on the Fastron model (Sec. IV), and

  • provides efficient and complete (without sampling) collision checking for piecewise-linear and piecewise-polynomial trajectories with safety guarantees based on nearest support vectors queried from the -tree (Sec. V and VI).

Ii Problem Formulation

Consider a spherical robot with center and radius navigating in an unknown environment. Let and be the obstacle space and free space in , respectively. In configuration space (C-space) , the robot body becomes a point , while the obstacle space and free space are transformed as , where , and . Assume that the robot position at time is known or provided by a localization algorithm. Let characterize the robot dynamics for an action . Applying at also incurs a motion cost (e.g., distance or energy). The robot is equipped with a depth sensor that provides distance measurements to the obstacle space within its field of view. Our objective is to construct an occupancy map of the C-space based on accumulated observations , where “-1” and “1” mean “free” and “occupied”, respectively. Assuming unobserved regions are free, we rely on to plan a robot trajectory to a goal region . As the robot is navigating, new sensor data is used to update the map and recompute the motion plan. In this online setting, the map update, , is a function of the previous estimate and a newly received depth observation .

Problem 1.

Given a start state and a goal region , find a sequence of actions that leads the robot to safely, while minimizing the motion cost:


Iii Preliminaries

In this section, we provide a summary on kernel perceptron and Fastron which is useful for our derivations in the next sections. The kernel perceptron model is used to classify a set of labeled data points. For , a data point with label is assigned a weight . Training determines a set of positive support vectors and their weights and a set of negative support vectors and their weights . The decision boundary is represented by a score function,


where is a kernel function and . The sign of is used to predict the class of a test point .

Fastron [das2017fastron, das2019learning] is an efficient training algorithm for the kernel perceptron model. It prioritizes updating misclassified points based on their margins instead of random selection as in the original kernel perceptron training. Our previous work [das2017fastron, das2019learning] shows that if for some , then is correctly classified with label . Based on this fact, Fastron utilizes one-step weight correction where if and if .

Iv Sparse Kernel-based Occupancy Mapping

Fig. 1: Example of our mapping method: (a) scan in work space; (b) scan in C-space; (c) samples from scan; (d) augmented free points (e) training data from one lidar scan; (f) the exact decision boundary generated by the score and the inflated boundary generated by the upper bound ; and along two rays: (g) one that enters the occupied space and (h) one that remains obstacle-free.

We propose a new version of the Fastron algorithm, utilizing only local streaming data, to achieve real-time sparse kernel-based occupancy mapping. For online learning, Fastron resamples a global training dataset around the current support vectors and updates the support vectors. In our setting, only local data in the robot’s vicinity is available from the onboard sensors. We propose an incremental version of Fastron in Alg. 1 such that: 1) training is performed with local data generated from a depth measurement at a time and 2) the support vectors are stored in an -tree data structure, enabling efficient score function (2) computations.

1:Sets and of positive and negative support vectors stored in an -tree; Local dataset generated from location ; ;
2:Updated .
3:Get nearest negative and positive support vectors.
4:for  in  do
5:      Calculate
6:for  to  do
7:      if  then return Margin-based priotization       
9:      if  then One-step weight correction
10:      else       
11:      if  then ,
12:      else if  then ,
13:      else if  then ,
14:      else,       
15:      for  do Remove redundant support vectors
16:            if  and  then
18:            if  and  then
Algorithm 1 Incremental Fastron Training with Local Data

Data Generation. Fig. 0(a) illustrates a lidar scan obtained by the robot at time . In configuration space, each laser ray end point corresponds to a ball-shaped obstacle, while the robot body becomes a point as shown in Fig. 0(b). To generate local training data , the occupied and free C-space areas observed by the lidar are sampled (e.g., on a regular grid). As shown in Fig. 0(c), this generates a set of points with label “1” (occupied) in the ball-shaped occupied areas and with label “-1” (free), outside. As unobserved areas are assumed free, neighboring points to the occupied samples in that are not already in or in the support vectors are added to an augmented set with label “-1”. The augmented dataset is illustrated in Fig. 0(d) assuming the set of support vectors is empty. The local data (Fig. 0(e)) is used in our Incremental Fastron Algorithm to update the support vectors (Fig. 0(f)). Storing the sets of support vectors , over time requires significantly less memory than storing the training data . The occupancy of a query point can be estimated from the support vectors by evaluating the score function in Eq. (2). Specifically, if and if . Fig. 0(f) illustrates the boundaries generated by Alg. 1.

Score Approximation. As the robot explores the environment, the number of support vectors required to represent the obstacle boundaries increases. Since the score function (2) depends on all support vectors, the time to train the kernel perceptron model online would increase as well. We propose an approximation to the score function under the assumption that the kernel function is radial (depends only on ) and monotone (its value decreases as increases). To keep the presentation specific, we make the following assumption in the remainder of the paper.


The kernel parameters can be optimized offline via automatic relevance determination [neal2012bayesian] using training data from known occupancy maps. The assumption implies that the value of is not affected significantly by support vectors far from . We introduce an -tree data structure constructed from the sets of positive and negative support vectors , to allow efficient nearest-neighbor lookup. The nearest support vectors, consisting of and positive and negative support vectors, are used to approximate the score (Lines 1-3 in Alg. 1).

V Collision Checking with Kernel-based Maps

A map representation is useful for navigation only if it allows checking a potential robot trajectory over time for collisions. We derive conditions for complete (without sampling) collision-checking of continuous-time trajectories in our sparse kernel-based occupancy map representation. Checking that a curve is collision-free is equivalent to verifying that , . It is not possible to express this condition for explicitly due to the nonlinearity of . Instead, in Prop. 1, we show that an accurate upper bound of the score exists and can be used to evaluate the condition explicitly in terms of .

Proposition 1.

For any , the score is bounded above by where is the closest positive support vector to .


The proposition holds because , and , . ∎

Fig. 0(f), 0(g), 0(h) illustrate the exact decision boundary and the accuracy of the upper bound along two lines in C-space. The upper bound is loose in the occupied space but remains close to the score in the free space since the RBF kernel is negligible away from the obstacle’s boundary. As a result, the boundary remains close to the true decision boundary. The upper bound provides a conservative but fairly accurate “inflated boundary”, allowing efficient collision checking for line segments and polynomial curves as shown next.

V-a Collision Checking for Line Segments

Suppose that the robot’s path is described by a ray , such that is obstacle-free, i.e., , and is constant. To check if collides with the inflated boundary, we find the first time such that . This means that is collision-free for .

Proposition 2.

Consider a ray , with . Let and be arbitrary positive and negative support vectors. Then, any point is free as long as:


where and


From Prop. 1, a point is free if or


Since varies with but belongs to a finite set, if we take the minimum of over all .∎

Prop. 1 and 2 hold for any negative support vector . Since belongs to a finite set, we can take the best bound on over the set of negative support vectors.

Corollary 1.

Consider a ray , with . Let and be arbitrary positive and negative support vectors, respectively. A point is free as long as:


The computational complexities of calculating and are and , respectively, where . Note that since often the robot’s movement is limited to the neighborhood of its current position, can reasonably approximate if is chosen as the negative support vector, closest to the current location . Calculation of in Eq. (3) is efficient in the sense that it has the same complexity as checking a point for collision (), yet it can evaluate the collision status for an entire line segment for without sampling.

Collision checking becomes slower when the number of support vectors increases. We improve this further by using and nearest positive and negative support vectors instead of and , respectively. Assuming and are constant, the computational complexities of calculating and reduce to which is the complexity of an -tree lookup.

In path planning, one often performs collision checking for a line segment . All points on the segment can be expressed as , , . Using the upper bound on provided by Eq. (3) or Eq. (5), we find the free region on starting from . Similarly, we calculate which specifies the free region from . If , the entire line segment is free; otherwise the segment is considered colliding. The proposed approach is summarized in Alg. 2 and illustrated in Fig. 2.

1:Line segment ; support vectors and
3:Calculate and using Eq. (3) or Eq. (5)
4:if  then return True (Free)
5:elsereturn False (Colliding)
Algorithm 2 Line segment collision check
Fig. 2: Collision checking for line segments (left), with bounds and obtained from Eq. (5), and for second-order polynomial curves (right) using Euclidean balls.
Polynomial curve , ; threshold ; support vectors and , ,
while True do
      Calculate using Eq. (6) or Eq. (7).
      if  then return False (Colliding)       
      Solve for
      if  then return True (Free)       
Algorithm 3 Polynomial curve collision check

V-B Collision Checking for Polynomial Curves

In the previous section, was a constant velocity representing the direction of motion of the robot. In general, the velocity might be time varying leading to trajectories described by polynomial curves [liu2017search]. We extend the collision checking algorithm by finding an Euclidean ball around whose interior is free of obstacles.

Corollary 2.

Let be such that and let and be arbitrary positive and negative support vectors. Then, every point inside the Euclidean balls is free for:


where and .


Directly follows from Prop. 2 by using the Cauchy-Schwarz inequality to bound for any unit vector (i.e. ).∎

Consider a polynomial , from to . Collorary 2 shows that all points inside are free for or . If we can find the smallest positive such that , then all points on the curve for are free. This is equivalent to finding the smallest non-negative root of a -order polynomial. Note that, if , there is a closed-form solution for . For higher order polynomials, one can use a root-finding algorithm to obtain numerically. We perform collision checking by iteratively covering the curve by Euclidean balls. If the radius of any ball is smaller than a threshold , the curve is considered colliding. Otherwise, the curve is considered free. The collision checking process for -order polynomial curves is shown in Alg. 3 and Fig. 2.

Vi Autonomous Navigation

Finally, we present a complete online mapping and navigation approach that solves Problem 1. Given the kernel-based map proposed in Sec. IV, a motion planning algorithm such as  [Russell_AI_Modern_Approach] may be used with our collision-checking algorithms to generate a path that solves the autonomous navigation problem. The robot follows the path for some time and updates the map estimate with new observations. Using the updated map, the robot re-plans the path and follows the new path instead. This process is repeated until the goal is reached or a time limit is exceeded (Alg. 4).

We consider robots with two different motion models. In simulation, we use a first-order fully actuated robot, , with piecewise-constant velocity for , leading to piecewise-linear trajectories:


where . In real experiments, we consider a ground wheeled Ackermann-drive robot:


where is the position, is the orientation, is the linear velocity, is the steering angle, and is the distance between the front and back wheels. The nonlinear car dynamics can be transformed into a 2nd-order fully actuated system via feedback linearization [de2000stabilization, franch2009control]. Using piecewise-constant acceleration for leads to piecewise-2nd-order-polynomial trajectories:


where , , . To apply to these models, we restrict the input sets and to be finite.

Vii Experimental Results

This section presents an evaluation of Alg. 4 using a fully actuated robot (8) in simulation and a car-like robot (Fig. 4) with Ackermann-drive dynamics (9) in real experiments. We use an RBF kernel with parameters , and an -tree approximation of the score with nearest support vectors around the robot location for map updating. The online training data (Sec. IV) were generated from a grid with resolution . Timing results are reported from an Intel i7 2.2 GHz CPU with 16GB RAM.

Vii-a Simulations

Fig. 3: Ground truth map and robot trajectory (left) used to generate simulated lidar scans, the occupancy map generated from our sparse kernel-based representation (middle) and the inflated map from the upper bound proposed in Prop. 1 (right).
Fig. 4: Third person views of the autonomous Racecar robot navigating in an unknown environment with moving obstacles.
Fig. 5: Final support vectors (left), kernel-based map (middle), and inflated map (right) obtained from the real experiments.
Fig. 6: Checking line segments in simulation (left): comparison between our method with score approximation (KM-SA) and a sampling-based method with different sampling resolution . Planning time per motion primitive in the real experiment (middle): comparison between our method with (KM) and without score approximation (KM-SA). Map update time for the real experiment (right).
1:Initial position ; goal region ; time limit ; prior support vectors and ,
2:while  and  do
3:       New Depth Sensor Observation
4:       Training Data Generation Sec. IV
5:       Incremental Fastron Alg. 1
6:      Path Planning Replan via (Alg. 2&3)
7:       Move to the first position along the path
Algorithm 4 Autonomous Mapping and Navigation with a Sparse Kernel-based Occupancy Map

The accuracy and memory consumption of our sparse kernel-based map was compared with OctoMap [octomap] in a warehouse environment shown in Fig. 3. As the ground-truth map represents the work space instead of C-space, a point robot () was used for an accurate comparison. Lidar scan measurements were simulated along a robot trajectory shown in Fig. 3 and used to build our map and OctoMap simultaneously. OctoMap’s resolution was also set to to match that of grid used to sample our training data from. Furthermore, since our map does not provide occupancy probability, OctoMap’s binary map was used as the baseline.

Table I compares the accuracy and the memory consumption of OctoMap’s binary map versus our kernel-based map and inflated map (using the upper bound in Prop. 1) with and without score approximation (Sec. IV). The kernel-based and inflated maps (with score approximation) are shown in Fig. 3. The kernel-based maps and OctoMap’s binary map lead to similar accuracy of compared to the ground truth map. OctoMap required a compressed octree with non-leaf nodes with 2 bytes per node, leading to a memory requirement of . As the memory consumption depends on the computer architecture and how the information on the support vectors is compressed, we provide only a rough estimate to show that our map’s memory requirements are at least comparable to those of OctoMap. We stored an integer representing a support vector’s location on the underlying grid and a float representing its weight. This requires 8 bytes on a 32-bit architecture per support vector. Our maps contained and support vectors with and without score approximation, leading to memory requirements of and , respectively. The recall (true positive rate) reported in Table I demonstrates the safety guarantee provided by our inflated map as of the occupied cells are correctly classified.

Accuracy 98.5% 98.5% 83.8% 83.2% 96.1%
Recall 97.4% 97.3% 99.0% 98.9% 96.8%
Vectors/Nodes 1947 2721 1947 2721 12372
Storage 15.6kB 21.7kB 15.6kB 21.7kB 24.7kB
TABLE I: Comparison among our kernel-based map (KM), KM map with score approximation (KM-SA), our inflated map (IM), IM map with score approximation (IM-SA) and OctoMap (OM) [octomap].

We also compared the average collision checking time over random line segments using our complete method (Alg. 2 with Eq. (5) and for score approximation) and sampling-based methods with different sampling resolutions using the ground truth map. Fig. 6 shows that the time for sampling-based collision checking increased as the line length increased or the sampling resolution decreased. Meanwhile, our method’s time was stable at suggesting its suitability for real-time applications.

Vii-B Real Robot Experiments

Real experiments were carried out on an th scale Racecar robot (Fig. 4) equipped with a Hokuyo UST-10LX Lidar and Nvidia TX2 computer. The robot body was modeled by a ball of radius . Second-order polynomial motion primitives were generated with time discretization of s as described in Sec. VI. The motion cost was defined as to encourage both smooth and fast motion [liu2017search]. Alg. 3 with Eq. (7), , and score approximation with was used for collision checking. The trajectory generated by an motion planner was tracked using a closed-loop controller [arslan2016exact]. The robot navigated in an unknown hallway with moving obstacles to destinations randomly chosen by a human operator. Fig. 5 shows the support vectors, the kernel-based map, and the inflated map with score approximation for the experiment.

We observed that kernel-based mapping is susceptible to noise since the support vectors are quickly updated with newly observed data, even though it is noisy and affected by localization errors. This is caused by the kernel perceptron model not maintaining occupancy probabilities. Future work will focus on sparse generative models for occupancy maps.

The time taken by Alg. 1 to update the support vectors from one lidar scan and the replanning time per motion primitive with and without score approximation are shown in Fig. 6. Map updates implemented in Python took s on average. To evaluate collision checking time, the replanning time was normalized by the number of motion primitives being checked to account for differences in planning to nearby and far goals. Without score approximation, the planning time per motion primitive was in the order of milliseconds and increased over time as more support vectors were added. With score approximation, it was stable at illustrating the benefits of our -tree data structure.

Viii Conclusion

This paper proposes a sparse kernel-based mapping method for a robot navigating in an unknown environment. The method offers efficient map storage that scales with obstacle complexity rather than environment size. We developed efficient and complete collision checking for linear and polynomial trajectories in this new map representation. The experimental results show the potential of our approach to provide compressed, yet accurate, occupancy representations of large environments. The developed mapping and collision checking algorithms offer a promising avenue for safe, real-time, long-term autonomous navigation in unpredictable and rapidly changing environments. Future work will explore simultaneous localization and mapping, sparse generative models that account for occupancy probability, and active exploration and map uncertainty reduction.