In this paper, we develop an approach that enables autonomous robots to build and compress semantic environment representations from point-cloud data. Our approach builds a three-dimensional, semantic tree representation of the environment from raw sensor data which is then compressed by a novel information-theoretic tree-pruning approach. The proposed approach is probabilistic and incorporates the uncertainty in semantic classification inherent in real-world environments. Moreover, our approach allows robots to prioritize individual semantic classes when generating the compressed trees, so as to design multi-resolution representations that retain the relevant semantic information while simultaneously discarding unwanted semantic categories. We demonstrate the approach by compressing semantic octree models of a large outdoor, semantically rich, real-world environment. In addition, we show how the octree abstractions can be used to create semantically-informed graphs for motion planning, and provide a comparison of our approach with uninformed graph construction methods such as Halton sequences.

We present an incremental search algorithm, called Lifelong-GLS, which combines the vertex efficiency of Lifelong Planning A* (LPA*) and the edge efficiency of Generalized Lazy Search (GLS) for efficient replanning on dynamic graphs where edge evaluation is expensive. We use a lazily evaluated LPA* to repair the cost-to-come inconsistencies of the relevant region of the current search tree based on the previous search results, and then we restrict the expensive edge evaluations only to the current shortest subpath as in the GLS framework. The proposed algorithm is complete and correct in finding the optimal solution in the current graph, if one exists. We also show the efficiency of the proposed algorithm compared to the standard LPA* and the GLS algorithms over consecutive search episodes in a dynamic environment.

Both geometric and semantic information of the search space are imperative for a good plan. We encode those properties in a weighted colored graph (geometric information in terms of edge weight and semantic information in terms of edge and vertex color) and propose a generalized A∗to find the shortest path among the set of paths with minimal inclusion of low-ranked color edges. We prove the completeness and optimality of this Class-Ordered A∗(COA∗) algorithm with respect to the hereto defined notion of optimality. The utility of COA∗is numerically validated in a ternary graph with feasible, infeasible, and unknown vertices and edges for the cases of a 2D mobile robot, a 3D robotic arm, and a 5D robotic arm with limited sensing capabilities. We compare the results of COA∗to that of the regular A∗algorithm, the latter of which finds a shortest path regardless of the semantic information, and we show that the COA∗dominates the A∗solution in terms of finding less uncertain paths.

Replanning is an essential problem for robots operating in a dynamic and complex environment for responsive and robust autonomy. Previous incremental-search algorithms efficiently reuse existing search results to facilitate a new plan when the environment changes. Yet, they rely solely on geometric information of the environment encoded in an edge-weighted graph. However, semantic information often provides valuable insights that cannot easily be captured quantitatively. We encode both semantic and geometric information of the environment in a weighted colored graph, in which the edges are partitioned into a finite set of ordered semantic classes (e.g., colors), and then we incrementally search for the shortest path among the set of paths with minimal inclusion of inferior classes, using information from the previous search using ideas similar to LPA*. The proposed Class-Ordered LPA* (COLPA*) algorithm inherits the strong theoretical properties of LPA*, namely, optimality and efficiency, but optimality now is with respect to the total path order. Numerical examples show that semantic information helps reduce the relevant search space in a dynamic environment.

We present a multi-scale forward search algorithm for distributed agents to solve single-query shortest path planning problems. Each agent first builds a representation of its own search space of the common environment as a multi-resolution graph, it communicates with the other agents the result of its local search, and it uses received information from other agents to refine its own graph and update the local inconsistency conditions. As a result, all agents attain a common subgraph that includes a provably optimal path in the most informative graph available among all agents, if one exists, without necessarily communicating the entire graph. We prove the completeness and optimality of the proposed algorithm, and present numerical results supporting the advantages of the proposed approach.