Topological belief space planning

Abstract

Methods and systems are provided for belief space planning for multiple robots at an initial belief state, for reducing belief space uncertainty, including receiving multiple, candidate, multi-robot paths, each including one or more pose constraints; generating a candidate factor graph for each candidate multi-robot path, and generating from each candidate factor graph a topology graph; calculating a signature metric from each topology graph, wherein the signature metric is one of a Von Neumann (VN) metric and a spanning tree (ST) metric; selecting a multi-robot path having the greatest signature metric from among the candidate, multi-robot paths; and instructing the multiple robots to proceed according to the selected multi-robot path.

Claims

1. A method of belief space planning for navigating multiple robots from initial poses to respective target goals for each robot, while reducing trajectory uncertainty, the method comprising: for each robot of the multiple robots, generating a set of candidate paths for navigating from its initial pose to its respective target pose, each candidate path including a sequence of discrete poses that are reached by sequential transitions caused by respective control actions; generating candidate multi-robot paths, each candidate multi-robot path including one candidate path from the set of candidate paths for each of the multiple robots, wherein each candidate multi-robot path has associated transition and observation constraints, wherein the transition constraints indicate the sequential transitions of each robot along its respective candidate path, and observation constraints indicate observations between poses, determined by a relative distance between poses of two robots or between poses of the same robot at different times, and wherein the sequential transitions and observations have zero-mean Gaussian process and measurement noise; generating a topology graph of each candidate multi-robot path, wherein nodes of each topology graph represent poses of robots along candidate paths, with all the poses of the robots along all the candidate paths of the candidate multi-robot path being represented in the given topology graph, and wherein edges connecting nodes represent the transition and observation constraints between the poses; calculating a signature metric for each topology graph, wherein the signature metric is calculated, based on parameters of a Laplacian matrix of each topology graph, as one of a Von Neumann graph entropy approximation or a metric of the number of spanning trees of each topology graph; adding to each signature metric a correction to generate a normalized signature metric, wherein the correction is a function of the number of nodes of each topology graph and a determinant of an information matrix of the measurement noise; selecting a multi-robot path having the greatest normalized signature metric from among the candidate multi-robot paths; and navigating the multiple robots to proceed according to the selected multi-robot path.

2. The method of claim 1, wherein the Von Neumann graph entropy approximation is calculated as ${\hat{s}}_{\mathrm{VN}}\left(G\right)=n/2\ln 2-1/2\underset{\left(i,j\right)\in E}{\overset{n}{.Math.}}1/\left[d\left(i\right)d\left(j\right)\right]$ ${\hat{s}}_{\mathrm{VN}}^{\mathrm{correction}}\left(G\right)={\hat{s}}_{\mathrm{VN}}\left(G\right)+\left(n-1\right)/2\left[\ln .Math.{\Omega }_{\mathrm{vij}}.Math.-{\ln \left(2\pi e\right)}^{\kappa }\right]$ and the normalized signature metric is calculated as ${\hat{S}}_{\mathrm{VN}}^{\mathrm{corrected}}\left(G\right)={\hat{S}}_{\mathrm{VN}}\left(G\right)+\left(n-1\right)/2\left[\ln \left|{\Omega }_{vij}\right|-{\ln \left(2\pi e\right)}^{\kappa }\right]$ where G is the topology graph, E represents all edges of the topology graph G, i and j are indices of a pair of nodes connected by an edge, d(i) and d(j) are node degrees of respective nodes i and j, n is the number of nodes of G, κ is the number of degrees of freedom of each of the poses of the robots along all candidate paths of the candidate multi-robot path, Ω.sub.vij is the information matrix of measurement noise, and |.Math.| is a determinant function.

3. The method of claim 1, wherein the normalized signature metric is calculated according to the number of spanning trees of the topology graph as:
s.sub.ST(G)=κ/2 ln t(G)+(n−1)/2[ln|Ω.sub.vij|−ln(2πe).sup.κ] where G is the topology graph, n is the number of nodes of G, t(G) is the number of spanning trees of G, Ω.sub.vij is the information matrix of the measurement noise, |.Math.| is a determinant function, and κ is the number of degrees of freedom of each of the poses of the robots along all candidate paths of the candidate multi-robot path.

4. The method of claim 1, further comprising calculating error bounds and determining that an uncertainty measure of the selected multi-robot path meets design specifications for accuracy and relative error with respect to a solution of multi-robot path selection by belief propagation and evaluation of an objective function.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) For a more complete understanding of the invention, reference is made to the following description and accompanying drawings, in which:

(2) FIG. 1 is a schematic diagram of a set of two candidate navigation paths for two robots, A and B, for a given control action, according to an embodiment of the present invention;

(3) FIG. 2 shows an example of a factor graph representing the joint probability distribution for the multi-robot belief for the candidate navigation paths of the two robots A and B, according to an embodiment of the present invention;

(4) FIG. 3 shows a topological graph corresponding to the factor graph of FIG. 2, according to an embodiment of the present invention;

(5) FIGS. 4A and 4B show candidate paths for two robots in a single planning session, and the paths selected by signature ranking vs. optimal planning, according to embodiments of the present invention;

(6) FIGS. 5A and 5B are graphs of simulation results of calculating Von Neumann (VN) spanning tree (ST) metrics for topology graphs of different node degrees and number of nodes, according to embodiments of the present invention;

(7) FIG. 6 is a flow chart of a process 600 for selecting an approximate solution .Math. for optimal BSP control actions to reduce uncertainty, according to an embodiment of the present invention.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

(8) An optimal non-myopic control action *.sub.k:k+L−1 at planning time k is found with respect to the objective function J related to the design task as

(9) $\begin{array}{cc}{𝒰}_{k\text{:}k+L-1}^{\mathrm{.star-solid.}}=\arg {\min }_{𝒰}{J}_k\left(𝒰\right),\mathrm{where}\mathrm{for}𝒰={𝒰}_{k\text{:}k+L-1}{J}_k\left(𝒰\right)=\underset{ℨ}{𝔼}\left\{\underset{l=0}{\overset{L-1}{.Math.}}{c}_l\left[b\left(X_{k+l}\right),{𝒰}_{k+l}\right]+c_L\left[b\left(X_{k+L}\right)\right]\right\}.& \left(1\right)\end{array}$

(10) In the BSP problem, the state of the system X is not directly observable, but is estimated through a set of stochastic measurements from which the future posterior beliefs b(X.sub.k+l) must be inferred upon optimization. Expectation is determined with respect to future observations .sub.k−1:k+L. The optimal solution of BSP provides a control strategy *.sub.k:k+L−1 for L look-ahead steps, and L can generally vary among control actions.

(11) The objective function in its general form reflects the design task through intermediate cost functions c.sub.l, which depend on belief evolution b[X.sub.k+l] and a control action U.sub.k+l applied at time t.sub.k+l, and through a final cost c.sub.L. For example, the cost functions can be chosen to minimize trajectory uncertainty, time or energy required to reach a goal, state uncertainty of variables of interest at some specific time instant etc. In one embodiment of the present invention, optimal solutions are provided for reducing state uncertainty for active SLAM, which is often the most computationally intensive BSP problem in robotics. Active SLAM includes simultaneously estimating the robot's pose and the map of the environment, while planning for the path that improves both estimates.

(12) Hereinbelow, path uncertainty is quantified by joint entropy. Measurements are assumed to be expressed in the form of pairwise relations between states. Robots' landmark measurements are given relative to the robot's pose, e.g. as a range or as bearing measurements. Mobile robots are assumed to operate in an unknown or uncertain environment that, given a task (e.g. exploring or reaching certain goal), aims to autonomously decide their future actions based on information accumulated thus far. For clarity we only consider the final state cost term [c.sub.L(b[X.sub.k+L])], i.e. the joint entropy at the end of the planning horizon. The intermediate cost functions can be treated in a similar way.

(13) Assuming the belief is modeled by a multivariate Gaussian distribution and taking the common maximum likelihood (ML) observations assumption, the objective function becomes
J.sub.k(U)=N/2 ln(2πe)−½ ln|I(X.sub.k+L)|,  (2)
where I(X.sub.k+L) denotes the estimated Fisher information matrix of the robot's belief b[X.sub.k+L], and N represents the dimension of the state X.sub.k−L. Notice that minimizing the global entropy (2) corresponds to maximizing the total information gain obtained by executing this trajectory, i.e. maximizing the estimation accuracy.

(14) Solving the optimization problem (1) explicitly (i.e., the “optimal solution”) would require belief propagation for all given controls in evaluating the objective function. In embodiments of the present invention, referred to herein as topological BSP, the ranking of candidate control actions is based on the topology of their associated belief states. That is, the original problem (1) is reformulated in a topological space, where it can be solved more easily, and then, its solution is related to the solution of the original problem. Topological BSP aims to determine a topological signature s: G.fwdarw., which is highly correlated with the cost function and maintains action consistent decision making.

(15) Single Robot Case

(16) The belief state evolves over time, and includes the controls and observations obtained up until the planning time t.sub.k and the subsequent controls and observations for planning L look-ahead steps.

(17) Hereinbelow, (X.sub.k|.sub.k) represents the posterior probability density function (pdf) at planning time t.sub.k over states of interest X.sub.k of the robot. In the pose SLAM framework, states of interest include a robot's current and past poses, i.e. X.sub.k={x.sub.0, x.sub.1, . . . , x.sub.k}. History .sub.k≐={.sub.1:k, U.sub.0:k−1} contains all observations .sub.1:k and controls U.sub.0:k−1 by time t.sub.k. Consider conventional state transition and observation models
x.sub.i+1=ƒ(x.sub.i, u.sub.i)+w.sub.i, z.sub.i,j=g(x.sub.i, x .sub.j)+v.sub.i,j,  (3)
with zero-mean Gaussian process and measurement noise w.sub.i˜N(0, Ω.sub.w.sup.−1) and v.sub.i,j˜N(0, Ω.sub.vij.sup.−1), and with known information matrices Ω.sub.w and Ω.sub.vij. Belief (X.sub.k.sub.k) is then

(18) $\begin{array}{cc}\mathbb{P}\left(X_k.Math.{\mathscr{H}}_k\right)\propto \mathbb{P}\left(x_0\right)\underset{i=1}{\overset{k}{.Math.}}\mathbb{P}\left(x_i.Math.x_{i-1},u_{i-1}\right)\mathbb{P}\left(Z_i.Math.X_i\right).& \left(4\right)\end{array}$

(19) The future sampled states of the robot along one of its candidate paths generated at planning time t.sub.k are {x.sub.k+1, . . . , x.sub.k+L}. The future posterior belief b[X.sub.k+L]=(X.sub.k+L|.sub.1:k+L, U.sub.0:k+L−1) that would be obtained by following the path , can be written in terms of the belief at planning time (X.sub.k|H.sub.k) and the corresponding state transition and observation models as

(20) $\begin{array}{cc}b\left[X_{k+L}\right]=\mathbb{P}\left(X_k.Math.{\mathscr{H}}_k\right)\underset{l=k+1}{\overset{k+L\left(𝒫\right)}{.Math.}}\mathbb{P}\left(x_l.Math.x_{l-1},u_{l-1}\right)\mathbb{P}\left(Z_l.Math.X_l\right),& \left(5\right)\end{array}$
where .sub.k:k+L−1 and .sub.k+1:k+L represent controls and (unknown) observations, respectively, to be acquired by following the path .

(21) Each candidate action corresponds to a posterior belief b[X.sub.k+L], i.e. a trajectory uncertainty that can be represented by a factor graph, which, in turn, can be represented by a topology. In the case of active pose SLAM, a topology can be defined by a simple undirected graph G=(V, E) such that graph nodes V represent robot's poses and edges E pose constraints between them. Pose constraints are typically poses at which there are common observations that are made either by different robots or by the same robot at different discrete times. Notice that the initial poses of the robots x.sub.0 are known, that is, it is a deterministic node in the factor graph, making the initial nodes a shared constraint of all robots.

(22) Multi-Robot Case

(23) In a multi-robot framework, each robot maintains a joint belief (X.sub.k|H.sub.k) on its own while communicating to each other relevant pieces of information. For R robots in a group, (X.sub.k|.sub.k) represents the pdf over the joint state X.sub.k at time t.sub.k, i.e. X.sub.k≐{X.sub.k.sup.r}.sub.r=1.sup.R and .sub.k≐{.sub.1:k; U.sub.0:k−1}, with .sub.1:k≐{.sub.1:k.sup.r}.sub.r=1.sup.R and .sub.0:k−1≐{.sub.0:k−1.sup.r}.sub.r=1.sup.R.

(24) Note that given transition and observation models (3), it is sufficient for each robot to only transmit (in addition to what is required by multi-robot inference) its own control actions. Any other robot that receives this information can then formulate the multi-robot belief (X.sub.k|.sub.k). If we consider different paths .sup.r for each robot r∈{1, . . . , R}, the future posterior multi-robot belief associated to a control action ≐{.sup.r}.sub.r=1.sup.R, is given by

(25) $\begin{array}{cc}b\left[X_{k+L}\right]=\mathbb{P}\left(X_k.Math.{\mathscr{H}}_k\right)\underset{r=1}{\overset{R}{.Math.}}\left[\begin{array}{c}\underset{l=1}{\overset{L\left({𝒫}^r\right)}{.Math.}}\mathbb{P}\left(x_{k+l}^r.Math.x_{k+l-1}^r,u_{k+l-1}^r\right).Math.\\ \mathbb{P}\left(Z_{k+l}^r.Math.X_{k+l}^r\right)\underset{\left\{i,j\right\}}{\Pi }\mathbb{P}\left(z_{i,j}^{r,r^{\prime }}.Math.x_i^r,x_j^{r^{\prime }}\right)\end{array}\right],& \left(6\right)\end{array}$
where the last product corresponds to multi-robot (MR) constraints that can involve different time instances, representing mutual observations of a scene.

(26) The ordered index set {i, j} in eq. (6) represents the time indices that facilitate multi-robot pose constraints among the robots {r, r′}. We assume a given criteria function cr.sub.MR(x.sub.i.sup.r, x.sub.j.sup.r′) that determines if there should be a multi-robot constraint between the two nodes x.sub.i.sup.r and x.sub.j.sup.r′ such as relative distance between robots.

(27) A joint belief (6) can be represented by a factor graph model (an illustration of which is provided in FIG. 2, described below). A factor graph (FG) is a bipartite graph whose nodes consist of factors F and variables V. The variables represent the random variables in the estimation problem, whereas the factors represent probabilistic information on those variables, derived from measurements or prior knowledge. Different candidate paths typically yield different factor graphs.

(28) A topology may be induced by an FG to represent a multi-robot belief (6). In the case F contains only binary factors, topology of the FG can be further simplified in the following way.

(29) Defining as a vertex labeling function of FG that assigns to its variables V a set of unique labels Γ={1,2, . . . , |V|}.

(30) We define a topological graph G≐(Γ, E) as a graph whose nodes are in the set Γ and whose edges are E={(i,j), i,j∈Γ⇔ a factor exists in F involving variables .sup.−1(i) and .sup.−1(j)}.

(31) A Laplacian matrix of a graph G is by definition L(G)=D(G)−A(G), where A(G) is its |Γ|×|Γ| adjacency matrix with elements

(32) $\begin{array}{cc}A\left(i,j\right)’\{\begin{array}{c}1,\mathrm{if}\left(i,j\right)\in E\\ 0,\mathrm{otherwise}\end{array}& \left(7\right)\end{array}$
and D(G) its node degree matrix defined as a diagonal matrix with graph node degrees on its main diagonal, i.e. D(i, i)=d(i)=Σ.sub.j∈rA(i, j). Finally, a normalized Laplacian matrix is defined as {circumflex over (L)}(G)=D.sup.−1/2LD.sup.−1/2 which can be also written in the form

(33) $\begin{array}{cc}\hat{L}\left(i,j\right)=\{\begin{array}{c}1,\mathrm{if}i=j,d\left(i\right)\ne 0\\ \frac{-1}{\sqrt{d\left(i\right)d\left(j\right)}},\left(i,j\right)\in E\\ 0,\mathrm{otherwise}\end{array}& \left(8\right)\end{array}$

(34) We define a spectral decomposition of the normalized Laplacian matrix to be {circumflex over (L)}=QΛQ.sup.−1, where Λ=diag({circumflex over (λ)}.sub.1, {circumflex over (λ)}.sub.2, . . . , {circumflex over (λ)}.sub.|Γ|) is a diagonal matrix of the eigenvalues of {circumflex over (L)}, and Q is an orthogonal matrix whose columns are their corresponding eigenvectors. The normalized Laplacian matrix of G is symmetric and positive semi-definite so all its eigenvalues are real and non-negative. Hence, they can be ordered
{circumflex over (λ)}.sub.1≤{circumflex over (λ)}.sub.2≤ . . . ≤{circumflex over (λ)}.sub.|Γ|,
with {circumflex over (λ)}.sub.1=0, {circumflex over (λ)}.sub.|Γ|≤2, and Σ.sub.i=1.sup.|Γ|{circumflex over (λ)}.sub.i=|Γ| (because G has no isolated nodes, as described in Chung, “Spectral graph theory,” 92, American Mathematical Soc., 1997, which is incorporated herein by reference).

(35) FIG. 1 shows a set of two candidate navigation paths for two robots, A and B. For a given control action (i.e., set of controls) Robot A plans to make observations at six poses (1-6) and robot B at five poses (7-11). Start poses for the two robots are marked with gray circles. Final poses are indicated with stars. Intermediate poses for robot A are indicated as circles 2-5; intermediate poses for robot B are indicated as circles 8-10. Observations of the same scene or exchanges of information between robots results in loop closures between positions. As indicated in the figure, there are loop closures (common observations or measurements) between poses 2 and 6 for robot A, and between poses 8 and 10 for robot B. In addition, there is a multi-robot pose constraint (e.g., a common observation or measurement) between pose 3 of robot A and pose 10 of robot B. These multi-robot (MR) pose constraints are also indicated in the factor graph and in the topological graph.

(36) FIG. 2 shows an example of a factor graph representing the joint probability distribution (6) for the multi-robot belief for the candidate navigation paths of the two robots A and B in FIG. 1. Probabilistic factors (t. represent pose constraints in pose graph optimization.

(37) FIG. 3 shows a topological graph corresponding to the factor graph of FIG. 2. The number of future sampled states of the robots A and B are n.sub.A=5 and n.sub.B=4, respectively. (Node labeling is arbitrary.)

(38) When the map feature states are maintained with the robot's trajectory, the observation model (3) can be written in the form
z.sub.i,j.sup.ƒ=g(x.sub.i, l.sub.j)+v.sub.i,j.sup.ƒ,  (9)
where x, represents the robot's pose at time t.sub.i, l.sub.j feature state vector belonging to the j-th landmark and v.sub.i,j.sup.ƒ, a noise in sensor measurement of that feature.
Graph Signature Metrics

(39) Embodiments of the present invention provide a metric of topological graph complexity for characterizing a graph G associated with a factor graph FG as a signature candidate for the optimization objective defined by (2). Two graph signatures described hereinbelow are Von Neumann (VN)entropy and spanning-tree (ST).

(40) Von Neumann Signature

(41) The Von Neumann entropy H.sub.VN(G) of graph G is the Shannon entropy associated with its normalized Laplacian's eigenvalues {{circumflex over (λ)}.sub.i}.sub.i=1.sup.|Γ|. The concept of the Von Neumann entropy of a graph was introduced in Passerini and Severini, “Quantifying Complexity in Networks: The Von Neumann Entropy,” Int. J. Agent Technol. Syst., Vol. 1, No. 4, 2009, pp. 58-67, doi:10.4018/jats.2009071005, which is incorporated herein by reference. The Von Neumann entropy may be written with convention 0 ln 0=0 as:

(42) $\begin{array}{cc}{s}_{\mathrm{VN}}\left(G\right)=-\underset{i=1}{\overset{.Math.\Gamma .Math.}{.Math.}}\frac{{\hat{\lambda }}_i}{.Math.\Gamma .Math.}\ln \frac{{\hat{\lambda }}_i}{.Math.\Gamma .Math.}.& \left(10\right)\end{array}$

(43) Using Han's quadratic approximation, this may be simplified to:

(44) $\begin{array}{cc}{s}_{\mathrm{VN}}\left(G\right)\approx \underset{i=1}{\overset{.Math.\Gamma .Math.}{.Math.}}\frac{{\hat{\lambda }}_i}{.Math.\Gamma .Math.}\left(1-\frac{{\hat{\lambda }}_i}{.Math.\Gamma .Math.}\right)& \left(11\right)\\ =\frac{\underset{i=1}{\overset{.Math.\Gamma .Math.}{.Math.}}{\hat{\lambda }}_i}{.Math.\Gamma .Math.}-\frac{\underset{i=1}{\overset{.Math.\Gamma .Math.}{.Math.}}{\hat{\lambda }}_i^2}{{.Math.\Gamma .Math.}^2}=\frac{\mathrm{Tr}\left[\hat{L}\right]}{.Math.\Gamma .Math.}-\frac{\mathrm{Tr}\left[{\hat{L}}^2\right]}{{.Math.\Gamma .Math.}^2}.& \left(12\right)\end{array}$

(45) The normalized Laplacian matrix {circumflex over (L)} is symmetric with unit diagonal. Therefore, Tr[{circumflex over (L)}]=|Γ|, and, after some basic matrix manipulations,

(46) $\begin{array}{cc}\mathrm{Tr}\left[{\hat{L}}^2\right]=.Math.\Gamma .Math.+\underset{\left(i,j\right)\in E}{.Math.}\frac{1}{d\left(i\right)d\left(j\right)}.& \left(13\right)\end{array}$

(47) Given the above, an expression for a Von Neumann entropy approximation that may be used as a graph signature is:

(48) 0$\begin{array}{cc}{s}_{\mathrm{VN}}\left(G\right)\approx {\hat{s}}_{\mathrm{VN}}\left(G\right)=1-\frac{1}{.Math.\Gamma .Math.}-\frac{1}{{.Math.\Gamma .Math.}^2}\underset{\left(i,j\right)\in E}{.Math.}\frac{1}{d\left(i\right)d\left(j\right)}.& \left(14\right)\end{array}$

(49) The computation depends only on the degree matrix D(G) and, in general case, has quadratic complexity in the number of nodes, O(|Γ|.sup.2) but in the case of BSP, where {circumflex over (L)} is sparse it depends only on the small number of non-zero elements of A(G), i.e. the number of edges |E| in the graph G. Also, the expression (14) can be computed incrementally, as new edges (measurements) are added to the multi-robot factor graph as the robots explore the environment or re-plan their paths.

(50) Alternative definition. To rank control actions, we can also use the following definition of the Von Neumann entropy with different scaling of the eigenvalues since we know that {circumflex over (λ)}.sub.i∈[0,2]:

(51) $\begin{array}{cc}{s}_{\mathrm{VN}}\left(G\right)=-\underset{i=1}{\overset{.Math.\Gamma .Math.}{.Math.}}{\hat{\lambda }}_i\text{/}2\ln \left({\hat{\lambda }}_i\text{/}2\right).& \left(15\right)\end{array}$
which can be simplified to

(52) $\begin{array}{cc}{s}_{\mathrm{VN}}\left(G\right)\simeq \frac{.Math.\Gamma .Math.\ln 2}{2}-\frac{1}{2}\underset{i=1}{\overset{.Math.\Gamma .Math.}{.Math.}}{\hat{\lambda }}_i\left({\hat{\lambda }}_i-1\right)=\frac{.Math.\Gamma .Math.\ln 2}{2}-\frac{1}{2}\left(\mathrm{Tr}\left[{\hat{L}}^2\right]-.Math.\Gamma .Math.\right).& \left(16\right)\end{array}$
and finally to

(53) $\begin{array}{cc}{s}_{\mathrm{VN}}\left(G\right)\approx {\hat{s}}_{\mathrm{VN}}\left(G\right)=.Math.\Gamma .Math.\text{/}2\ln 2-\frac{1}{2}\underset{\left(i,j\right)\in E}{.Math.}\frac{1}{d\left(i\right)d\left(j\right)}.& \left(17\right)\end{array}$
Spanning-Tree Signature

(54) An additional metric for the graph signature may be a spanning tree (ST) metric, based on the number of spanning trees t(G) in a graph G. The calculation of t(G) was described in the context of measurement selection and graph pruning by Khosoussi, et al., “Reliable graph topologies for SLAM,” IJRR, 2018, which is incorporated herein by reference. By this metric,
ln|Σ(X.sub.k+L)|≈−3 ln(t(G))−ln(η),  (18)
where η depends on the noise variances and geometry and, in the case of BSP, on the graph dimension n (number of graph nodes). We can write η=Cξ, where C captures the geometry and the noise parameters and is assumed constant, i.e. independent on the FG topology, while ξ is a function of graph dimension n. Minimizing (2) can therefore be formulated as

(55) $\begin{array}{cc}{𝒰}^{\mathrm{.star-solid.}}=\underset{𝒰}{\arg \max }\left[-J\left(𝒰\right)\right]\approx \underset{𝒰}{\arg \max }\left[\frac{3}{2}\ln t\left(G\right)+\frac{\ln \left(\xi \right)}{2}-\frac{n}{2}\ln \left(2\pi e\right)+\mathrm{const}.\right].& \left(19\right)\end{array}$

(56) The value ξ can be determined from the odometry factor graph associated with b[X.sub.k+L]. Given its process noise information matrix is Ω.sub.w and its topological graph G.sup.o contains n nodes, it follows from (18), as t(G.sup.o)=1, that ln(η)=n ln(|Ω.sub.w|). From this, we can derive a normalized value of an ST signature, s.sub.ST(G) that maximizes (19), a full derivation of which is described in the ST signature derivation section hereinbelow. As shown in the full derivation:

(57) $\begin{array}{cc}{s}_{\mathrm{ST}}\left(G\right)=\frac{3}{2}\tau \left(G\right)+\frac{n}{2}\left[\ln .Math.{\Omega }_w.Math.-\ln \left(2\pi e\right)+\mathrm{const}\right]& \left(20\right)\end{array}$

(58) Both graph signatures, VN and ST, are metrics that can be used to compare BSP control actions. An advantage of using the VN signature is that it is faster, O(|E|), and possible incremental calculations depending only on the node degrees, which can be updated easily. The number of spanning trees in a graph is equal to any cofactor of its Laplacian matrix, whose determination is an operation with cubic complexity in the number of nodes for general dense graphs. Because SLAM graphs typically have a small number of edges due to limited sensor range and field of view, their graph Laplacian is sparse and state-of-the-art algorithms use this fact to efficiently calculate τ(G) by performing sparse matrix factorization. However, this approach, to be efficient, still requires finding a good fill-reducing permutation of SLAM reduced graph Laplacian associated to this cofactor. The problem of finding the best ordering is an NP-complete problem and is thus intractable, so heuristic methods are used instead. Even with a good variable ordering, finding a matrix factorization is still more expensive than updating node degrees.

(59) ST Signature Normalization, Full Derivation

(60) In order to derive the graph signature based on the number of spanning trees t(G) to be used in BSP for optimizing objective (2), we will use the results of Khosoussi regarding the relation of t(G) to a determinant of Fisher information matrix of a Maximum Likelihood (ML) pose SLAM estimator. In (6) we assumed that relative pose measurements belonging to the factors of b[] of the joint multi-robot state X.sub.n at planning time t.sub.n are independent identically distributed Gaussian random variables, so they can be generated according to
z=h(X.sub.n)+ϵ,ϵ˜(0, Σ),  (21)
where z is a stacked vector of relative pose measurements (all odometry, loop closing and multi-robot constraints) and Σ is a diagonal measurement noise covariance matrix. We denote by X.sub.n=[{X.sub.n.sub.r.sup.r}.sub.r=1.sup.R] the joint state composed of poses sampled on the paths of all R robots with n.sub.r denoting a dimension of a state belonging to a path .sup.r of a single robot r, and n the dimension of the joint state.

(61) The ML SLAM optimization problem can be then formulated as

(62) $\begin{array}{cc}{X}_n^{*}=\underset{X_n}{\arg \min }{.Math.z-h\left(X_n\right).Math.}_{{\Sigma }^{-1}}^2& \left(22\right)\\ =\underset{X_n}{\arg \min }\underset{i}{.Math.}{.Math.z_i-h\left(x_{j_1},x_{j_2}\right).Math.}_{{\Omega }_w^i}^2,& \left(23\right)\end{array}$
where z.sub.i is a relative pose measurement between poses x.sub.j.sub.1 and x.sub.j.sub.2. For simplicity we proceed with a 2D SLAM setting and assume that Ω.sub.w.sup.i=Ω.sub.w=diag(σ.sub.p.sup.−2, σ.sub.p.sup.−2, σ.sub.θ.sup.−2), i.e. process noise information matrix with σhd p.sup.2 and σ.sub.θ.sup.2 being the position (p) and angular (θ) variances respectively. The Fisher information matrix I(X.sub.n) given the measurement model (21) is given by Sorenson, “Parameter estimation: principles and problems,” Vol. 9, M. Dekker, 1980, which is incorporated herein by reference.
I(X.sub.n)=J.sup.TΣ.sup.−1J,  (24)
where J=∂h/∂X.sub.n is a measurement Jacobian. I.sup.−1(X.sub.n) evaluated at true value of X.sub.n is known as Cramér-Rao lower bound. Commonly, covariance matrix of ML estimate is approximated with inverse of a Fisher information matrix evaluated at ML estimate X*.sub.n.

(63) Typically, multi-robot factor graphs are composed of odometry factors and initial pose constraints, which may be used in derivation of the normalization of the spanning tree graph signature. In general, different robots have paths of different lengths L(.sup.r) or planning horizons L.sub.r, r∈1, 2, . . . , R.

(64) Now, let a factor graph FG.sup.o representing a multi-robot belief b[.sup.o] be composed of only the odometry factors, i.e. Π.sub.r=1.sup.R(x.sub.l.sup.r|x.sub.l−1.sup.r, u.sub.l−1.sup.r), and relative transformations of initial robots' poses z.sub.0.sup.r,r′ that enable establishing a common reference frame. The later are represented by pdfs (z.sub.0.sup.r,r′|x.sub.0.sup.r, x.sub.0.sup.r′) for involved pairs of robots r, r′ in a group. FG.sup.o is a subgraph of a factor graph FG associated to b[] given by (6) and its topological graph G.sup.o is a tree.

(65) In Khosoussi, bounds of ln detI(X.sub.n) are shown to be
3.sub.T(G)+ln detI.sub.o(X.sub.n)≤ln detI(X.sub.n)
≤2.sub.T(G)+ln det({circumflex over (L)}+ΨI)+ln detI.sub.o(X.sub.n),  (25)
where {circumflex over (L)} is a reduced Laplacian of a graph G obtained by removing arbitrary row and column of L, I.sub.o(X.sub.n) information matrix associated to any tree sub-graph and

(66) $\begin{array}{cc}\Psi \stackrel{.}{=}\frac{{\sigma }_{\theta }^2}{{\sigma }_p^2}{\max }_i{\left({\Delta }^T\Delta \right)}_{\mathrm{ii}}{\mathrm{where}\left({\Delta }^T\Delta \right)}_{\mathrm{ii}}=\underset{j\in \left\{j.Math.\left(i,j\right)\in E\right\}}{.Math.}{.Math.p_i-p_j.Math.}^2.& \left(26\right)\end{array}$
Notice that Ψ depends only on the noise variances and geometry of the SLAM graph. Therefore, for small values of Ψ a good approximation of ln detI(X.sub.n) is its bound, i.e.
ln detI(X.sub.n)≈2.sub.T(G)+ln det({circumflex over (L)}+ΨI)+ln detI.sub.o(X.sub.n)  (27)

(67) The second term in (27) can be expressed using Matrix determinant lemma and Matrix tree theorem as

(68) $\begin{array}{cc}\ln \det \left(\tilde{L}+\Psi I\right)=\ln \det \tilde{L}+\ln \det \left(I_n+{\tilde{L}}^{-1}\Psi \right)& \left(28\right)\\ =\tau \left(G\right)+\ln \det \left(I_n+{\tilde{L}}^{-1}\Psi \right).& \left(29\right)\end{array}$

(69) Consequently, using the notation η=Cξ, we can write

(70) $\begin{array}{cc}\ln \mathrm{detI}\left(X_n\right)\approx 3\tau \left(G\right)+\ln \stackrel{\stackrel{C}{︷}}{\det \left(I_n+{\tilde{L}}^{-1}\Psi \right)}+\ln \stackrel{\stackrel{\xi }{︷}}{{\mathrm{detI}}_o\left(X_n\right)}.& \left(30\right)\end{array}$
Also, C is almost constant for small enough Ψ. Using Lemma 2 and Lemma 3 from Khosoussi, for b[.sup.o] whose topological graph G.sup.o is a tree, it follows

(71) 0$\begin{array}{cc}\ln {\mathrm{detI}}_o\left(X_n\right)=\underset{\underset{0}{︸}}{\tau \left(G_o\right)}+{\ln \left({\sigma }_p^{-4}{\sigma }_{\theta }^{-2}\right)}^n=n\ln \det {\Omega }_w.& \left(31\right)\end{array}$
Therefore,
ln detI(X.sub.n)≈3.sub.T(G)+n ln detΩ.sub.w+const  (32)

(72) Putting (32) into the expression for objective function (2) and using the fact that In detI(X.sub.n)=−ln detΣ(X.sub.n), the expression for properly normalized graph signature for use in BSP (20) follows.

(73) Sub-Sampling and Optimization

(74) After obtaining graph signatures for all candidate path combinations, a sub-sampling strategy may be employed, depending on the optimization objective's relation to the signature and on the available time resources we have. If we can assume that the BSP solution has high correlation with the signature, e.g. minimizing uncertainty in (2) corresponds to maximizing the graph's complexity (14), we can then choose the top N.sub.s best ranked signatures, evaluate the objective function for the corresponding actions, and then select the best among them. Alternatively, signatures may be clustered and a search performed for an optimal solution inside the best topology cluster. In that case, N.sub.s is proportional to the number of clusters and we assume that similar topology classes will produce similar costs. Notice that the proposed topological BSP algorithm can be considered as an anytime algorithm, as its solution quality depends on the available computational budget and is monotonically improved as more samples are considered.

(75) Sample Signature Calculations

(76) The VN signature metrics defined by the respective equations (15) and (17) may be written as:

(77) $\begin{array}{cc}{s}_{\mathrm{VN}}\left(G\right)=-\underset{i=1}{\overset{n}{.Math.}}{\hat{\lambda }}_i/2\ln \left({\hat{\lambda }}_i/2\right)\approx & \left(33\right)\end{array}$$\begin{array}{cc}{\hat{s}}_{\mathrm{VN}}\left(G\right)=n/2\ln 2-1/2\underset{\left(i,j\right)\in E}{.Math.}1/\left[d\left(i\right)d\left(j\right)\right],& \left(34\right)\end{array}$

(78) where ŝ.sub.VN(G) may be further normalized for improved action consistency as follows (as described below with respect to FIG. 5B):

(79) $\begin{array}{cc}{\hat{s}}_{\mathrm{VN}}^{\mathrm{correction}}\left(G\right)={\hat{s}}_{\mathrm{VN}}\left(G\right)+\left(n-1\right)/2\left[\ln .Math.{\Omega }_{\mathrm{vij}}.Math.-{\ln \left(2\pi e\right)}^{\kappa }\right]& \left(35\right)\end{array}$

(80) The ST signature metric (20) may be written as:
s.sub.ST(G)=κ/2 ln t(G)+(n−1)/2[ln|Ω.sub.vij|−ln(2πe).sup.κ]  (36)

(81) Above, G=(V, E) is a topological graph, n=|V|, {circumflex over (λ)}.sub.i are the eigenvalues of the normalized Laplacian of G, and κ is the dimension of the robot pose (i.e., degrees of freedom, e.g. kappa=3 for planning in 2D, where robot's pose comprises 2 position coordinates x, y and a dimension representing orientation).

(82) For the pair of candidate paths shown in FIG. 1, which correspond to the topological graph G in FIG. 3:
n=11, κ=3(for planning in 2D space).

(83) Assuming all relative pose measurements have the same noise with standard deviation in position σ.sub.p=0.1 m and σ.sub.θ=0.01 rad in orientation

(84) ${\Omega }_w={\Omega }_{\mathrm{vij}}=\left(\begin{array}{ccc}{\sigma }_p^{-2}& 0& 0\\ 0& {\sigma }_p^{-2}& 0\\ 0& 0& {\sigma }_{\theta }^{-2}\end{array}\right)=\left(\begin{array}{ccc}100& 0& 0\\ 0& 100& 0\\ 0& 0& 10000\end{array}\right)$

(85) The graph Laplacian L and the node degrees vector d of the topological graph of FIG. 3 are indicated below. Note that the node degrees vector is the diagonal of the graph Lapacian, and other values of the graph Lapacian are either 0, for no connection between nodes, or −1 to indicate a connection:

(86) $L=\left(\begin{array}{ccccccccccc}2& -1& 0& 0& 0& 0& -1& 0& 0& 0& 0\\ -1& 3& -1& 0& 0& -1& 0& 0& 0& 0& 0\\ 0& -1& 3& -1& 0& 0& 0& 0& 0& -1& 0\\ 0& 0& -1& 2& -1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -1& 2& -1& 0& 0& 0& 0& 0\\ 0& -1& 0& 0& -1& 2& 0& 0& 0& 0& 0\\ -1& 0& 0& 0& 0& 0& 2& -1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& -1& 3& -1& -1& 0\\ 0& 0& 0& 0& 0& 0& 0& -1& 2& -1& 0\\ 0& 0& -1& 0& 0& 0& 0& -1& -1& 4& -1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& -1& 1\end{array}\right),d=\left(\begin{array}{c}2\\ 3\\ 3\\ 2\\ 2\\ 2\\ 2\\ 3\\ 2\\ 4\\ 1\end{array}\right)$

(87) The number of spanning trees of G is equal to any cofactor of its Laplacian matrix, i.e. t(G)=82. By definition, normalized graph Laplacian is
{circumflex over (L)}=diag(d).sup.−1/2L diag(d).sup.−1/2.

(88) For the given graph, eigenvalues of {circumflex over (L)} are {{circumflex over (λ)}.sub.i}={0, 0.17119, 0.36398, . . . 0.65906, 0.75561, 0.90491 1.2837, 1.5346, 1.6526, 1.807, 1.8674}.

(89) Applying (33)-(36) gives the following signature metric values:
s.sub.VN(G)=2.4141
ŝ.sub.VN(G)=1.5762
s.sub.ST(G)=56.1453

(90) These numbers are ranks of the given candidate paths based on the different topological signature metrics. The same calculations would be performed for all other candidate robot paths, to determine the best path, or more precisely, the best control actions to achieve the best path. For example, if we consider a control that produces the same topological graph but without an edge (3, 10), meaning that corresponding combination of candidate paths will not lead to this particular multi-robot constraint, the ranks given by different signatures would be:
s.sub.VN(G)=2.2437
ŝ.sub.VN(G)=1.3679
s.sub.ST(G)=53.597

(91) By all three topological signatures, we see that the rank of the control action that includes the edge (3, 10) is better. In other words, the cost of the first control action is smaller. In general, the ranks of actions for different topological signatures can vary depending on the accuracy of approximations. In practice, we need to calculate only one signature for ranking actions depending on the computational power available in a given application.

(92) Experimental Selection Results

(93) Matlab using GTSAM was used to investigate key aspects of the topological BSP signature ranking. A probabilistic roadmap (PRM) was used to discretize the environment and generate candidate paths over the roadmap. Given two robots, each having 10 candidate paths, means that in every planning session 100 different factor graphs are formed representing the multi-robot belief of the corresponding controls and predicted observations. We used a simple heuristic for the function to determine if two poses admit a multi-robot pose constraint: these constraints, possibly involving different future time instances, are formulated between any two poses with relative distance closer than d=320 meters. In every planning session, topological BSP algorithms chose one sample (path combination) to evaluate the objective function according to their corresponding signature and sub-sampling strategy. For the objective of minimizing the global entropy, we selected the top best ranked signature. FIGS. 4A and 4B show candidate paths for 2 robots in a single planning session, and the paths selected by signature ranking vs. optimal planning. Robots' paths are marked with dark gray and light gray double lines, respectively, multi-robot constraints with light gray lines, and loop closure constraints with dashed black lines.

(94) In a second simulation, we showed that good action consistency is still kept even when comparing significantly varying pose graphs. We generated pose graphs with varying number of nodes n and edges tn, of which n−1 correspond to odometry and the rest to LC=m−n+1 loop closing factors. We choose LC=Δdn/2, causing average node degree of a graph increasing in steps of Δd∈{0.1, 0.5, 0.75, 1, 2} from a given odometry graph of size n (whose average node degree is approx. 2).

(95) For each pair (n, tn) we created a group of 10 random topologies, calculated their topological metrics s.sub.ST and s.sub.VN, and measured the average time for calculating t.sub.ST and t.sub.VN respectively. The results of this simulation are given in FIGS. 5A and 5B. FIG. 5A demon-strates that ŝ.sub.VN is calculated much faster than s.sub.ST for high dimensional BSP problems. FIG. 5B shows that correlation with the posterior entropy of the Fischer Information Matrix (FIM) of the Maximum Likelihood Estimator (MLE) is high for s.sub.ST over the whole range of topologies while ŝ.sub.VN follows the overall trend and is high for graphs with the same n, but breaks the action consistency with jumps of n. This suggests that normalization ƒ of ŝ.sub.VN should be applied according to its relation to the objective, as indicated in the correction (35), by which the second term in equation (36) for s.sub.ST was added to ŝ.sub.VN to improve action consistency.

(96) Determining Error Bounds

(97) The above results can be extended to a BSP problem to provide online performance guarantees of topological BSP.

(98) Reliability of SLAM topology

(99) In the context of Maximum Likelihood Estimation (MLE) in pose SLAM, the optimal set of poses X.sub.k for which the belief (4) is maximized can be obtained by fixing one of the poses, e.g. x.sub.0, and treating the rest as unknown (or with an uninformative prior) while minimizing the sum of weighted squared errors between predicted and measured relative poses, i.e.

(100) $\begin{array}{cc}{\hat{X}}_k=\underset{X_k}{\arg \max }\mathbb{P}\left({\Delta }_k.Math.X_k\right)\equiv \underset{X_k}{\arg \min }{.Math.{\Delta }_k-h\left(X_k\right).Math.}_{{\Sigma }^{-1}}^2.& \left(37\right)\end{array}$
(Because the state x.sub.0 is considered deterministic, here we estimate the rest of the variables X.sub.k={x.sub.1, x.sub.2, . . . , x.sub.k} and I(X.sub.k) denotes their joint information matrix from now on.)

(101) In this formulation, a measurement Δ.sub.k represents a vector of m stacked relative pose measurements z.sub.i,j∈SE(2), r=1,2, . . . from motion and observation model (3) at time t.sub.k with m=|E|, the number of edges in the topological graph of the belief (4).

(102) Relative pose measurements in pose SLAM resulting from state transitions can be obtained by the motion composition z.sub.i+1,i(x.sub.i,x.sub.i+1)=⊖x.sub.i⊖x.sub.i+1=⊖x.sub.i⊖ƒ(x.sub.i, u.sub.i, w.sub.i). In this work, we assume independent relative pose measurements with additive noises
Δ.sub.k=h(X.sub.k)+v.sub.k, v.sub.k˜N(0, Σ.sup.−1).  (38)

(103) For simplicity, we also assume a 2D pose SLAM setting in which all relative positions and orientations between poses x.sub.i and x.sub.j have equal variance, σ.sub.p.sup.2 and σ.sub.θ.sup.2 respectively, i.e. Ω.sub.v.sub.i,j=diag(σ.sub.p.sup.−2, σ.sub.p.sup.−2, σ.sub.θ.sup.−2).

(104) The information matrix I(X.sub.k) of the MLE is I(X.sub.k)=H.sup.TΣ.sup.−1H where H=∂h/∂X.sub.k is a measurement Jacobian. I(X.sub.k) evaluated at the true value of X.sub.k is known as Fisher Information Matrix (FIM) and its inverse the Cramer-Rao lower bound (CRLB). Commonly, FIM is approximated with I({circumflex over (X)}.sub.k).

(105) In Khosoussi, cited above, bounds of the determinant of I(X.sub.k) are expressed in terms of pose SLAM graph topology, geometry and noise as
3.sub.T(G)+ln detI.sub.o(X.sub.k)≤ln detI(X.sub.k)
≤2.sub.T(G)+ln det({tilde over (L)}+ΨI)+ln detI.sub.o(X.sub.k),  (39)
where {tilde over (L)} is a reduced Laplacian of a graph G obtained by removing an arbitrary r-th row and r-th column of the graph Laplacian L, I.sub.0(X.sub.k) estimated information matrix based on the odometry measurements Δ.sub.k={z.sub.i+1,i, i<k}, and Ψ≐ξ.sup.2dist.sub.max.sup.2 .sub.x where ξ=σ.sub.θ/σ.sub.p, and dist.sub.max.sup.2=max.sub.i∈VΣ.sub.(i,j∈E)∥x.sub.i−x.sub.j∥.sup.2.

(106) Notice that generally Ψ depends on the noise variances, geometry and topology of the SLAM graph. For small values of Ψ, a good approximation of ln detI(X.sub.k) is its bound that depends on τ(G), i.e. lower and upper bounds become tight.

(107) Even when it is not negligible, if there is only one path realization to consider as in passive SLAM, information gain of adding relative pose measurements with constant noise distribution to a SLAM odometry graph is solely characterized by graph G topology, i.e. Ψ=Ψ(G). Similar logic applies to the graph pruning and measurement selection problem described by Khosoussi, where all graphs are only subgraphs of the original graph with the same embedding in metric space. The same metric τ(G) cannot be used in BSP problems nor guarantee optimality using the bounds on I(X.sub.k), for the following reasons. In measurement selection problems, the robot considers, at planning time k, a single path and the factor graph of that path as well as which subset of measurements to take. Pose samples X.sub.k+L are fixed and minimizing entropy (2) by a measurements subset (action) is the same as maximizing ln|I(X.sub.k+L)|−ln|I.sub.o(X.sub.k+L)|. Its bounds as can be seen from (39) depend only on the topology and ξ, i.e., all actions share the same pose variables X.sub.k+L which can be considered constant in optimization. (dist.sub.max can vary between actions but only because of the change in topology.) However, in BSP where a robot needs to compare different paths corresponding to different factor graphs, this is not true anymore. To determine the best action according to (2), one has to account additionally for different path geometries and path lengths and larger variety of topologies.

(108) In BSP we need to consider different path realizations X.sub.k−L and therefore Ψ=Ψ()=Ψ(X.sub.k+L(), G()), in contrast to graph pruning and measurement selection.

(109) It is not necessary to propagate belief in planning for determining Ψ under ML observations assumption, i.e. [X.sub.k+L]=[{circumflex over (X)}.sub.kx.sub.k+1 . . . x.sub.k+L] where future sampled poses from the path corresponding to action U are added to the prior state estimate {circumflex over (X)}.sub.k which is the same for all actions.

(110) Decision Making by Typological BSP

(111) The best control action obtained by solving the decision making problem using either of the topological metrics s∈{s.sub.ST, s.sub.VN, ŝ.sub.VN} is given by

(112) $\begin{array}{cc}\widehat{𝒰}=\underset{𝒰}{\arg \max }s\left(𝒰\right).& \left(40\right)\end{array}$

(113) While the above topological metrics exhibit strong correlation with the information theoretic cost in the general case, the obtained best action may be somewhat different than the optimal action * from (1), leading to some error in the quality of solution.

(114) A definition of action consistent state approximations is described in Elimelech and Indelman, “Fast action elimination for efficient decision making and belief space planning using bounded approximations,” Proc. of the Intl. Symp. of Robotics Research (ISRR), December 2017, which is incorporated herein by reference. Hereinbelow, we modify the definition to support topological BSP and action consistent objective approximation in the following way, giving the error of topological BSP as
ϵ(J, s)≐|J(*)−ƒ[s()]|,  (41)
where .Math.=arg max.sub.u s(U) and f is a monotonic function such that ƒ[s()]=J() and ƒ=arg min.sub.ƒγƒ, where γ.sub.ƒ=max.sub.u|J(U)−ƒ [s()]|.

(115) In particular, ƒ for which (41) is zero, corresponds to topological BSP being action consistent, i.e. =*, and when also γ.sub.ƒ=0, simplified representation preserves action order too.

(116) In the case s=s.sub.ST, we can select ƒ(s)=−s+const. to quantify the error in (41) because the topological metric s.sub.ST is designed as the negative value of the entropy J up to a constant, when approximation error is small. However, ƒ for s.sub.VN is more complicated, but we have shown empirically in realistic simulations that such function ƒ exists for which the error e is small.

(117) Calculating ϵ(J, s) is essentially equivalent to solving the original problem. Therefore, a key aspect is to provide online performance guarantees by developing sufficiently tight bounds on ϵ(J, s) that can be evaluated online. One can then resort to topological BSP to reduce computational cost while carefully monitoring a conservative estimate on the sacrifice in performance, which would be provided by the bound on ϵ(J, s). Another perspective of using this bound is to guarantee global optimality of the topological BSP algorithm. In this approach, actions may be ranked by a topological metric and the objective function evaluated sequentially from best to worst. Having bounds on topological BSP error provides a stopping condition for action consistent topological BSP.

(118) Entropy Bounds in BSP

(119) In BSP, we have to consider multiple path realizations from different controls, with greater variety in both topology of factor graphs and other non-topological factors that influence estimation accuracy, e.g non-fixed geometry and different path lengths. Given a topological signature, such as given above s.sub.ST (20) for topological BSP, we may also provide global optimality guarantees of topological BSP based on this metric.

(120) For a control action U corresponding to robot's poses X.sub.k+L={x.sub.1, . . . , x.sub.k+L} each of dimension κ, the entropy of the future posterior belief b[X.sub.k+L] can be written using Eq. (2) and the fact ln|Σ(X.sub.k+L)|=−ln|I(X.sub.k+L)|
J()=(k+L)κ/2 ln(2πe)−½ ln|I(X.sub.k+L)|.  (42)

(121) Notice that the number of graph nodes |V|=n=k+L+1. Using Lemma 2 and Lemma 3 from Khosoussi, for posterior belief b[.sup.o]=(X.sub.k+L.sub.0:k+L−1) corresponding to an odometry factor graph whose topological graph G.sup.o is a tree, it follows

(122) $\begin{array}{cc}\begin{array}{c}\ln {\mathrm{detI}}_o\left(X_{k+L}\right)=\underset{\underset{0}{︸}}{\tau \left(G^o\right)}+{\ln \left({\sigma }_p^{-4}{\sigma }_{\theta }^{-2}\right)}^{k+L}\\ =\left(k+L\right)\ln \det \left({\Omega }_{v_{i,j}}\right).\end{array}& \left(43\right)\end{array}$

(123) Inequalities (39) and Eqs. (42) and (43) give the entropy bounds ß[J(U)]J(U)UB[J(U)], where

(124) $\begin{array}{cc}\begin{array}{c}\mathrm{𝒰ℬ}\left[J\left(𝒰\right)\right]=\frac{\left(n-1\right)\kappa }{2}\ln \left(2\pi e\right)-\frac{1}{2}\left[3\tau \left(G\right)+\left(n-1\right)\ln .Math.{\Omega }_{\mathrm{vij}}.Math.\right]\\ =-\frac{3}{2}\tau \left(G\right)-\frac{n-1}{2}\left[\ln .Math.{\Omega }_{v_{i,j}}.Math.-{\ln \left(2\pi e\right)}^{\kappa }\right]=-s_{\mathrm{ST}}\left(𝒰\right)\end{array}& \left(44\right)\end{array}$

(125) Similarly, for the lower bound we get
B[J(U)]=−s.sub.ST()−½ ln|{tilde over (L)}+Ψ()I.sub.n−1|+½.sub.τ(G)=  (45)
−s.sub.ST()−½[ln|{tilde over (L)}+Ψ()I.sub.n−1−−ln|{tilde over (L)}|]=  (46)
−s.sub.ST()−½ ln|I.sub.n−1+{circumflex over (L)}.sup.−1Ψ()|.  (47)

(126) As applied in eq. (46) the matrix tree theorem states t(G)=|{tilde over (L)}|, and in eq. (47) Shur's determinant lemma. From eq. (47) we see that when Ψ.fwdarw.0, the entropy goes to the BSP topological metric and solely depends on the beliefs topology and path length. Otherwise, we have to account for the graph embedding in the metric space as well, as it appears in the scalar Ψ of the second term in the above equations. Now, given all candidate control actions, the bounds on the topological BSP error are defined as:
ϵ(J, s.sub.ST)J.sub.max, where

(127) $\begin{array}{cc}\Delta J_{\max }=\mathrm{𝒰ℬ}\left[J\left(\widehat{𝒰}\right)\right]-\underset{𝒰}{\min }\mathrm{ℒℬ}\left[J\left(𝒰\right)\right]& \left(48\right)\end{array}$

(128) To prove the above equation for ΔJ.sub.max, we may rewrite ΔJ.sub.max as

(129) 0$\begin{array}{cc}\Delta J_{\max }=\left\{J\left(\widehat{𝒰}\right)+\underset{\underset{\delta \left(\widehat{𝒰}\right)\ge 0}{︸}}{\mathrm{𝒰ℬ}\left[J\left(\widehat{𝒰}\right)\right]-J\left(\widehat{𝒰}\right)}\right\}+& \left(49\right)\end{array}$$\begin{array}{cc}\left\{-J\left({𝒰}^{*}\right)+\underset{\underset{\delta \left({𝒰}^{*}\right)\ge 0}{︸}}{J\left({𝒰}^{*}\right)-\underset{𝒰}{\min }\mathrm{ℒℬ}\left[J\left(𝒰\right)\right]}\right\}.& \left(50\right)\end{array}$
Therefore, δ=δ()+δ(U*)≥0 and ΔJ.sub.max=J()−J(U*)+δ≥0 because U* is the minimum of the entropy. Also, Uß[J()]=−s.sub.ST() is the minimum upper bound of all actions because we select action in topological BSP according to eq. (40). We know that the optimal action must have entropy below this value, otherwise the selected action would be better which is contradiction by itself. Then, ΔJ.sub.max=J(.Math.)−J()+δ=|J()−J(U*)|+δ.Math.|J()−J(U*)|=ϵ(J, s.sub.ST)=ΔJ.sub.max−δΔJ.sub.max.

(130) The upper bound of the entropy is already determined by the topological metric as can be seen from eq. (44). However, calculating the lower bound requires an additional cost due to the second term in equations (45)-(47). Calculating it requires evaluating the determinant of a sparse matrix M={tilde over (L)}+ΨI.sub.n−1∈.sup.n−1×n−1.

(131) A further goal is to find a fast method for limiting the determinant of M from above to replace the second term in eq. (45) so as not to introduce a large difference in the tightness of the lower bound. A direct approach for calculating a lower bound of the entropy includes sparse matrix factorization of M, e.g. Cholesky factorization M=RR.sup.T where R is the lower triangular matrix, from which then it is easy to calculate its determinant. However, this approach, to be efficient, still requires finding a good fill-reducing permutation P.sub.M of M. The problem of finding the best ordering is an NP-complete problem and is thus intractable, so heuristic methods are used instead. However, some calculations from the topological metric can be re-used. In particular, because M differs from {tilde over (L)} only in diagonal elements, they both have the same sparsity pattern. Therefore, if P.sub.{tilde over (L)} is the best fill-reducing permutation of {tilde over (L)} (already found for determining |{tilde over (L)}|), it can be re-used for calculation of the lower bound, i.e. P.sub.M=P.sub.{tilde over (L)}.

(132) Because {tilde over (L)} is a reduced graph Laplacian, it is symmetric positive definite (SPD), and because also Ψ>0, the matrix M is SPD. For large values of Ψ, the matrix M becomes strongly diagonally dominant and Hadamard inequality gives a good approximation of its determinant, i.e.

(133) $\begin{array}{cc}.Math.\tilde{L}+\Psi I_{n-1}.Math.⩽\underset{i=2}{\overset{n}{.Math.}}\left[d\left(i\right)+\Psi \right].& \left(51\right)\end{array}$

(134) Calculation of the right side of inequality (51) requires only multiplication of node degrees with added value of Ψ. Applying (51) to eq. (45), we can get somewhat more conservative but faster to compute lower bound

(135) $\begin{array}{cc}\mathrm{ℒℬ}\left[J\left(𝒰\right)\right]=-s_{\mathrm{ST}}\left(𝒰\right)+1/2\left(\tau \left(𝒰\right)-\underset{i=2}{\overset{n}{.Math.}}\ln \left[d_{𝒰}\left(i\right)+\Psi \left(𝒰\right)\right]\right)& \left(52\right)\end{array}$

(136) Performance guarantees can be either in the form of a selected solution's entropy upper bound, i.e. guarantees on the accuracy, or in bounding the error of topological BSP with respect to the optimal solution.

(137) The first form can be used when the maximum admissible path uncertainty is known at the planning time, e.g. for obstacle avoidance. In that case, one can get an answer if a topological BSP solution satisfies the specification by ranking actions using very efficient O(m|A|) topological metric ŝ.sub.VN and calculating only its entropy's upper bound. If global optimality guarantees are required, the topological metric s.sub.ST needs to be calculated for all actions, it is currently a good choice as a graph signature. If an uncertainty specification is not met by topological BSP, one can still use its ranked actions. Also, if an optimal solution needs to be found, we can use topological BSP to eliminate suboptimal actions.

(138) Path Selection with Error Bounds

(139) FIG. 6 is a flow chart of a process 600 for selecting an approximate solution .Math. for optimal BSP control actions to reduce uncertainty, according to an embodiment of the present invention. Steps of process 600 are implemented by a system including a computer processor configured to execute the following instructions.

(140) At a step 602, the system receives multiple, candidate, multi-robot paths, each including one or more pose constraints. The multiple paths may be provided as respective factor graphs, or the respective factor graphs may be generated by the system at a subsequent step 604, at which the system also generates from each candidate factor graph a topology graph. Next, at a step 606, the system calculates a signature metric from each topology graph, wherein the signature metric is one of a Von Neumann (VN) metric and a spanning tree (ST) metric. At a step 608, the system selects a multi-robot path having the greatest signature metric from among the candidate, multi-robot paths. At a step 610, the system may then instruct the multiple robots to proceed according to the selected multi-robot path. The system may also output, at a step 612, a sorted list of the multiple, mult-robot paths, according to the calculated signature metrics. Alternatively or additionally, the system may determine whether the selected path meets a preset uncertainty margin and may forgo outputting an instruction if the uncertainty margin is not met. The system may also calculate and output (at step 612) uncertainty bounds, the bounds being calculated as described above.

(141) The following outline describes a variation of the process using mathematical notation and indicating optional output. Input: Set of factor graphs FG corresponding to control actions U Output: Approximate solution to the BSP .Math. and its performance guarantees or actions subset A Parameters: Graph signature s∈{s.sub.ST, s.sub.VN, s.sub.VN}, uncertainty margin γ (optional)
Steps:

(142) 1. Receive input, set S=0;

(143) 2. Represent each FG with a topological graph G(U);

(144) 3. Iterate, for each U: Evaluate the topological metric s(U)≐s[G(U)]; S=S∪{U, s(U)}

(145) 4.

(146) $\mathrm{Select}\widehat{𝒰}=\underset{𝒰}{\mathrm{argmax}}s\left(𝒰\right);$

(147) 5. UB[J(.Math.)]=s.sub.ST(U);

(148) 6. If uncertainty margin γ is given then, if UB[J(.Math.)]<γ, Return .Math.

(149) 7. Else A=sort(S) with respect to topological metric, Return A

(150) 8. If uncertainty margin γ is not given, then

(151) $\begin{array}{cc}{\mathrm{ℒℬ}}_{\min }=\underset{𝒰}{\min }\mathrm{ℒℬ}\left[J\left(𝒰\right)\right]\mathrm{using}\mathrm{eq}.\left(52\right);\Delta J_{\max }=\mathrm{𝒰ℬ}\left[J\left(\widehat{𝒰}\right)\right]-{\mathrm{ℒℬ}}_{\min };A=\left\{𝒰\in S:\mathrm{ℒℬ}\left[J\left(𝒰\right)\right]<\mathrm{𝒰ℬ}\left[J\left(\widehat{𝒰}\right)\right]\right\};\mathrm{Return}\widehat{𝒰},t-\mathrm{bsp}\mathrm{error}\mathrm{bound}\Delta J_{\max },A& \end{array}$

(152) Performance guarantees can be either in the form of selected solution's entropy upper bound, i.e. guarantees of accuracy, or in the form of bounding the error of the solution with respect to the optimal solution. The first form can be used when the maximum admissible path uncertainty is known at the planning time, e.g. for obstacle avoidance. In that case, one can get an answer if a solution satisfies the specification by ranking actions using the topological metric ŝ.sub.VN, which is O(m|A|) efficient, and calculating only its entropy's upper bound. If global optimality guarantees are required, the topological metric s.sub.ST needs to be calculated for all actions. In the first usage, if an uncertainty specification is not met, the ranked actions set A can be used in an anytime algorithm.