Efficient and transparent localization and mapping using constrained zonotopes

2023 note: This research plan was part of my submission to the National Science Foundation’s Graduate Research Fellowship Program. I didn’t get it this time– oh well! If you’re applying and are curious about the application feedback, you can view it here.

Robot localization typically follows a two-stage process: First a map is generated using a simultaneous localization and mapping (SLAM) technique, and then a dedicated localization algorithm locates the robot by using the offline map as a prior. Localization approaches are highly diverse, but the most accurate and stable options also tend to be the most computationally expensive. First, scan matching algorithms, including Iterative Closest Point and its many derivatives, are vulnerable to local minima and perform poorly in dynamic environments. Second, histogram filters, Bayesian estimators, and other statistical methods are more resistant to noise, but their computational complexity scales impractically in large environments. Finally, graph-based localization shows enormous promise, since an arbitrary number of factors can be added, but the underlying dependence on nonlinear optimization makes them difficult to use in real time. Recent work with factor graphs for localization has aimed to sidestep the computational demands by using a sliding window technique to compare with an existing landmark map [1]. No matter how capable, all three of these localization classes treat the map as a perfect, static reference, a dangerous assumption that fails to hold in autonomous driving applications.

Constrained zonotopes, a novel class of sets presented in 2016, demonstrate a promising compromise between computational efficiency and flexibility in expressing geometric relationships [2]. Constrained zonotopes can optimize topological maps linearly, and they can encode “memory” of an environment that makes dependent algorithms more resilient.

Proposal

I propose to develop a landmark-based SLAM algorithm that leverages the computational efficiency and flexibility of constrained zonotopes (c-zonotopes). Rather than using distributions, c-zonotope sets will be used to quantify various sources of uncertainty that appear in mapping and localization. C-zonotopes bring compelling capabilities, such as expressing SLAM as a linear optimization and encoding memory in the chain of propagating and aggregating uncertainty. Critically, c-zonotopes provide new opportunities to quantify uncertainty in the map itself and enable capturing maps that change over time.

Resource requirements

This work will mostly occur in simulation and will require a suitable workstation. A sensor-equipped vehicle will also be necessary for real-world tests. Finally, public datasets such as KITTI-360 [3] will be leveraged to validate and compare this framework.

Diagram of proposed SLAM method using constrained zonotopes in autonomous driving

Research plan

Task I concerns the development of a landmark-based SLAM algorithm using c-zonotopes. This initial algorithm will assume that the environment is unseen and will therefore not consider any map prior. It will first perform dead reckoning using IMU preintegration, as in [4], to determine an initial region of possible vehicle locations. This region of possibility will be modeled as a c-zonotope (blue in the figure). Given a set of observed landmarks from a frontend classifier (or set of classifiers), this method will assign a measurement uncertainty to each landmark position, using this to constrain the initial c-zonotope provided by the IMU preintegration and therefore reduce the uncertainty of the vehicle’s state (pink in the figure). As the vehicle moves, the locations of each landmark are recorded onto the map. This initial algorithm will only consider a single agent operating in an unseen area. As part of this first task, I will develop an open-source c-zonotope software library, the c-zonotope equivalent of factor graph libraries like [5]. This initial SLAM algorithm will be validated on simulator ground truths and in the KITTI-360 dataset. In Task II, this algorithm will be extended to fuse multiple SLAM runs into a single map by applying a linear optimizer to the c-zonotope sets. Whereas the initial algorithm only used c-zonotopes to model the vehicle’s state, this iteration will also use them to model the position of landmarks, adding constraints as new landmark observations are recorded. In this fashion, the position uncertainty of each map feature is asymptotically reduced as new runs are recorded. Novel contribution: a SLAM algorithm that can both create and refine a map over time, in real time, adjusting the landmark topology as the map changes. In Task III, the localization and mapping algorithm will be scaled to multiple agents, each with sensors of varying types and qualities. As part of this multiagent approach, a coverage map modeling map quality at each point will be generated, allowing agents to identify and adapt to well-mapped, poorly mapped, and unmapped regions. Finally, a multiagent protocol will be developed to synchronize changes to the map. Novel contribution: A SLAM algorithm that provides metric information, semantic information, and now a quality metric at each location, so that agents can adapt intelligently. Challenges and mitigation strategies: (1) Changes in the map (a missing street sign) need to be detected and separated from small disturbances (a street sign that is simply occluded). Mitigation strategy (MS): Observing landmark states over multiple timesteps and verifying through multiple agents and runs. (2) Faulty constraints need to be detected and filtered. MS: Associate each constraint with a sensor quality rating, which would allow conflicts to be removed by filtering out poorly rated constraints first. Contributions can be weighted so that a single faulty actor cannot meaningfully influence map quality. (3) The upstream classifier(s) may provide unreliable landmark information. MS: Corrupted landmark information may lead to infeasibility in the SLAM optimization. When this occurs, we can consider dropping subsets of landmarks to recover feasibility; alternatively, uncertainty sets can be temporarily inflated until feasibility is regained.

Intellectual merit

A successful extension of c-zonotopes to SLAM would mean a significantly faster, more flexible, and more transparent framework for mapping and localization with autonomous vehicles (AVs) and mobile robots more generally. This work would change the fundamental thinking about offline maps, turning them into living, dynamic environments that a fleet of AVs can manage collaboratively. This is made possible by the efficiency gains of c-zonotopes, and by the flexibility of the underlying topological map structure. A faster SLAM framework is more scalable and also more accurate: More landmarks can be considered, which means better resistance to noise and tighter convergence. Zonotope-based SLAM is sensor agnostic, so different landmarks can be detected by different sensors, making the system more resilient to disturbance. For example, if traffic signs are occluded by snow, road boundaries can be detected by radar and used instead. Landmark-based maps are much smaller than point cloud maps, and they encode semantic data/labelled features by design. Graph-based SLAM gives us both a topological and metric representation, the best of both worlds, and c-zonotopes make this practical in real time. The proposed method is independent of the frontend, so that an arbitrary number and variety of landmarks can be provided. Multiple frontends can feed the optimizer simultaneously, protecting against failure. C-zonotopes give us a clear, accurate measurement of our localization confidence, including the current “region of possibility” of the estimate. Since the framework preserves frontend landmark classifications, downstream methods can generate additional information like lane boundaries automatically without additional dependencies. Finally, the underlying network of zonotopes can be used to estimate the accuracy of the map at any point, akin to a coverage map provided by a cellphone network, informing the safe operation of AVs.

Broader impacts

The proposed work is a system that any AV platform, from consumer-grade vehicles to expensive platforms with high-end sensors, can use, extend, and contribute to. This approach supports a network of AVs, where each agent plays a part in making maps safe, complete, and accurate. A decentralized topology prevents large companies from monopolizing this important service. More immediately, the open-source c-zonotope software library would enable the research community to explore and extend this promising new technique.

References

[1] D. Wilbers, C. Merfels and C. Stachniss, “Localization with Sliding Window Factor Graphs on Third-Party Maps for Automated Driving,” 2019 International Conference on Robotics and Automation (ICRA), 2019, pp. 5951-5957, doi: 10.1109/ICRA.2019.8793971.

[2] J. Scott, et al. “Constrained zonotopes: A new tool for set-based estimation and fault detection.” Automatica 69 (2016): 126-136.

[3] Y. Liao, J. Xie and A. Geiger, “KITTI-360: A Novel Dataset and Benchmarks for Urban Scene Understanding in 2D and 3D,” in IEEE Transactions on Pattern Analysis and Machine Intelligence, doi: 10.1109/TPAMI.2022.3179507.

[4] T. Lupton and S. Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions,” in IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61-76, Feb. 2012, doi: 10.1109/TRO.2011.2170332.

[5] F. Dellaert, “Factor Graphs and GTSAM: A Hands-on Introduction.” (2012).

arrow_back Back home