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 largescale, 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 piecewiselinear and piecewisepolynomial 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 kd tree from point clouds and queries the nearest obstacles for collision checking. Using spatial partitioning similar to a kd tree, octreebased maps [octomap, chen2017improving] offer efficient map storage by performing octree compression. Meanwhile, AtomMap [fridovich2017atommap] stores a collection of spheres in a kd 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 samplingbased collision checking, simplifying the safety verification of continuoustime 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, learningbased collision checking methods [das2017fastron, pan2015efficient, huh2016learningGMM]
sample data from the environment and train machine learning models to approximate the obstacle boundaries. Pan et al.
[pan2015efficient]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. Geometrybased 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 kernelbased mapping method that:

[leftmargin=2em,nosep]

represents continuousspace 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
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 (Cspace) , 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 Cspace 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:
(1)  
s.t.  
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,
(2) 
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 onestep weight correction where if and if .
Iv Sparse Kernelbased Occupancy Mapping
We propose a new version of the Fastron algorithm, utilizing only local streaming data, to achieve realtime sparse kernelbased 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.
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 ballshaped obstacle, while the robot body becomes a point as shown in Fig. 0(b). To generate local training data , the occupied and free Cspace 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 ballshaped 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.
Assumption.
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 nearestneighbor lookup. The nearest support vectors, consisting of and positive and negative support vectors, are used to approximate the score (Lines 13 in Alg. 1).
V Collision Checking with Kernelbased 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) collisionchecking of continuoustime trajectories in our sparse kernelbased occupancy map representation. Checking that a curve is collisionfree 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 .
Proof.
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 Cspace. 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.
Va Collision Checking for Line Segments
Suppose that the robot’s path is described by a ray , such that is obstaclefree, i.e., , and is constant. To check if collides with the inflated boundary, we find the first time such that . This means that is collisionfree for .
Proposition 2.
Consider a ray , with . Let and be arbitrary positive and negative support vectors. Then, any point is free as long as:
(3) 
where and
.
Proof.
From Prop. 1, a point is free if or
(4) 
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:
(5) 
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.
VB 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:
(6)  
(7) 
where and .
Proof.
Directly follows from Prop. 2 by using the CauchySchwarz 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 nonnegative root of a order polynomial. Note that, if , there is a closedform solution for . For higher order polynomials, one can use a rootfinding 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 kernelbased map proposed in Sec. IV, a motion planning algorithm such as [Russell_AI_Modern_Approach] may be used with our collisionchecking 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 replans 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 firstorder fully actuated robot, , with piecewiseconstant velocity for , leading to piecewiselinear trajectories:
(8) 
where . In real experiments, we consider a ground wheeled Ackermanndrive robot:
(9) 
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 2ndorder fully actuated system via feedback linearization [de2000stabilization, franch2009control]. Using piecewiseconstant acceleration for leads to piecewise2ndorderpolynomial trajectories:
(10) 
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 carlike robot (Fig. 4) with Ackermanndrive 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.
Viia Simulations
The accuracy and memory consumption of our sparse kernelbased map was compared with OctoMap [octomap] in a warehouse environment shown in Fig. 3. As the groundtruth map represents the work space instead of Cspace, 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 kernelbased map and inflated map (using the upper bound in Prop. 1) with and without score approximation (Sec. IV). The kernelbased and inflated maps (with score approximation) are shown in Fig. 3. The kernelbased maps and OctoMap’s binary map lead to similar accuracy of compared to the ground truth map. OctoMap required a compressed octree with nonleaf 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 32bit 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.
KM  KMSA  IM  IMSA  OM  
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 
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 samplingbased methods with different sampling resolutions using the ground truth map. Fig. 6 shows that the time for samplingbased 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 realtime applications.
ViiB Real Robot Experiments
Real experiments were carried out on an th scale Racecar robot (Fig. 4) equipped with a Hokuyo UST10LX Lidar and Nvidia TX2 computer. The robot body was modeled by a ball of radius . Secondorder 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 closedloop 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 kernelbased map, and the inflated map with score approximation for the experiment.
We observed that kernelbased 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 kernelbased 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, realtime, longterm 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.
Comments
There are no comments yet.