Robotic systems have tremendous value in the advancement of scientific exploration, with robots collecting data on planetary surfaces [Estlin2012], outer space [Gao2017science], and in the deep oceans [Zhang2021science]. In these settings usage of multi-robot systems conveys several advantages: observations can be made more rapidly, over larger areas, and the robots can collaborate to more efficiently accomplish the mission. However, a key challenge in real-world deployment of mobile robotic systems in general is accurate localization.
A common approach to multi-robot localization relies on teams in which the robots use inter-robot ranging and a subset of the robots are considered anchors [Bahr12iros, Bahr07ecmr]. These anchors have high quality self-localization, possibly due to global navigation satellite systems (GNSS) or high-precision inertial systems, and enable for absolute localization of the remainder of the network through the relative range measurements. However, the accuracy of localizing the entire network via range measurements depends on the relative positioning of the agents [patwari2003relative] and the localization problem can become ill-posed under certain network configurations, causing localization techniques to fail drastically. As a result, as the team of robots moves throughout space the localization of the non-anchor robots can vary and substantially deteriorate. In safety-critical or high-cost missions poor localization can lead to disastrous outcomes such as loss of expensive equipment or even loss of life. To avoid the potential costs of poor localization we propose a technique for planning trajectories for such robot networks that effectively maintains a minimum localization quality at all timesteps.
We measure the ability for a given configuration to be localized through the Fisher information matrix (FIM) and FIM-based optimality measures from the field of design of experiments. These measures are scalar values that relate the FIM eigenvalues to various statistical properties of the localization problem[pukelsheim2006optimal]. By constraining these optimality measures with lower bounds, the localization quality of the network can be controlled. However, planning trajectories which constrain these measures is difficult, as the optimality measures are complicated functions of the network configuration which tightly couple the states of the separate robots in the network. The primary challenge is in the inherent dimensionality of the planning problem, as the individual robots cannot be easily decoupled and computational issues arise with the resulting complexity of the problem. We present a framework for decoupling the planning process and performing prioritized planning, i.e. planning each robot individually in a predetermined sequence, while ensuring that the network localization quality is maintained.
This paper extends our previous work on graph-based, prioritized, localizability-constrained planning [papalia2020network] with the following contributions:
The relationship between localization quality and the theory of design of experiments is made clear.
We present original proofs relating network connectivity, the FIM, and FIM optimality measures.
This paper considers the case where there are sufficient anchor robots to fully constrain the localization problem, whereas [papalia2020network] neglected the effects of anchor robots.
More extensive experimental results with a more computationally efficient localizability-constrained planner and greater accuracy localization approach.
The source code will be made freely available 111
Ii Related Work
As our work seeks to control robot networks to improve their localization we first review similar works in the area of active localization. Then, as our described problem is in multi-robot planning, we review relevant works in the planning community. We note that our consideration of localization quality is closely related to the well-known geometric dilution of precision [langley1999dilution], but for brevity we do not discuss this in further depth.
Ii-a Active Localization
Prior works recognize the importance of geometry on the quality of range-only localization and consider how to control members of a team to improve the localization of others. Many approaches consider properties of either the FIM or the closely related covariance matrix as measures of localization quality [Bahr12iros, Walls2015iros, LeNy2018]. Existing approaches learn a policy [Chitre2010planning, tan2014cooperative] or perform belief space planning [Walls2015iros] for a single anchor vehicle to support a number of other vehicles. A different approach developed a distributed algorithm for any number of anchor vehicles to reduce the total uncertainty of any number of non-anchor vehicles [Bahr12iros]. However, these works assume that the non-anchor vehicles have fixed trajectories and do not consider how the non-anchor trajectories could be planned to improve their own localization.
Other works [LeNy2018, Zelazo2015] present potential-field methodologies for distributed trajectory planning for networks of anchor and non-anchor robots. In [LeNy2018] potential functions are presented for several FIM optimality metrics. The work in [LeNy2018] relates to earlier work in [Zelazo2015] which developed rigidity matrices222Under our assumptions of Gaussian or log-normal measurement distributions, the FIM we consider is a rigidity matrix and E-optimality is equivalent to the infinitesimal rigidity metric [LeNy2018]. to control the infinitesimal rigidity of such a network of robots, a necessary property for both control and localization in the range-only case. However, the approaches in [LeNy2018, Zelazo2015] are both potential-field based techniques. In these approaches, local minima in the potential fields trap the planner and prevent the network from reaching the goal locations. While our approach considers the same optimality measures as [LeNy2018], our approach does not rely on the gradient of the optimality metrics and thus does not suffer from the very common local minima presented in these potential-field based approaches.
Ii-B Multi-Robot Planning
Each additional robot in the network increases the dimensionality of the planning problem. This leads to a challenge in the field of multi-robot planning known as the curse of dimensionality, which refers to how the size of the planning space scales exponentially with increasing dimensions. Prioritized planning, in which the robots in a network plan their trajectories independently in a predetermined sequence, has been proposed to reduce the effects of dimensionality[VanDenBerg2005iros]. In a similar vein, [Wagner2011iros] presents the idea of subdimensional expansion. With this approach, trajectories are planned independently on a graph until interactions occur between robots, at which point the planner considers the joint configuration space of the interacting robots. Other works in multi-agent trajectory planning consider combinations of discrete and continuous representations of space [Park2020icra, Tordesillas2020mader]. They also reduce the dimensionality by planning for subgroups of the robots at a time as opposed to the whole team at once, using different strategies to handle potential conflicts between robots. While these notions of dimensionality reduction are a key part of the approach we present, none of these approaches can be immediately extended to our problem as the FIM construction enforces that each robot needs to be considered as always interacting with all other robots.
Iii Problem Formulation
We begin by establishing the difference between anchor and non-anchor robots in our network. We present the FIM that will be referred to throughout this paper, as originally derived in [patwari2003relative], as well as the different optimality criteria of the FIM which our approach applies to. Finally, in this section, we formally define the problem this paper considers.
As in [patwari2003relative, papalia2020network]
, we assume that range measurements are either Gaussian or log-normally distributed. Additionally, we assume a sensing horizon which prevents robots from obtaining range measurements to each other if they are beyond a certain distance from each other.
Iii-a Terminology and Notation
Throughout this paper we use the abbreviation LC to refer to a localizability constraint (LC) and LCSAT to refer to a set of positions that satisfy a group of localizability constraints, i.e the localizability constraint satisfying (LCSAT) set. All indexing is zero-indexed, i.e. the first row in a matrix is the th row. We use two separate neighbor functions: acts on a set of nodes on a planning graph and returns the union of all neighboring locations of all locations in the set, acts on a robot in a network and returns all of the robots it has range measurements to.
Iii-B Anchor and Non-Anchor Indexing
Anchor robots are robots which are considered to have known absolute position. In practice, this position information could be from sources such as GNSS or high-precision inertial navigation systems. We order the robots such that the anchors are grouped first and followed by the non-anchors, with no particular ordering within these subgroups. For example, if we have anchors and non-anchors the indices are .
Iii-C Fisher Information Matrix
We present the FIM in Equation 5 as derived in [patwari2003relative]. We begin by defining an undirected graph over the network of robots where the edge exists if there is a range measurement between robots and . The position of robot is denoted . The FIM is a ()-block-structured matrix which is similar to a weighted graph Laplacian over where the Laplacian’s rows and columns corresponding to the indices of anchor nodes have been removed. The contributions of the sensor model in the FIM appear in the following parameters:
, which is equal to 1 if the sensor model is Gaussian and 2 if the model is log-normally distributed, and
, the standard deviation of the sensor distribution.
We first define the difference in robot positions and the distance between robots as follows:
From this, we define and
, which relate the relative positions of robots in the network to sub-blocks of the FIM. The vectorsand are ()-block-structured vectors which relate nodes and whose th-blocks are given in Equations 4 and 3.
We next define as the set of edges between a non-anchor node and an anchor node and as the set of edges between two non-anchor nodes. In the case of we assume without loss of generality that for each edge-pair the first value, , refers to the non-anchor node. With these edge sets the FIM, , can be expressed as follows:
Note that there is no contribution from edges between two anchor nodes, as the positions are known and thus no information is gained from such measurements.
As shown in [LeNy2018, Zelazo2015], the eigenvalues of the FIM are invariant to rigid-motion transformations of the network members and the FIM is similar to a weighted Laplacian of the connectivity graph of the network. Unsurprisingly, the weights in the Laplacian representation come exactly from the sensor measurement model and the relative positioning of the robots with respect to each other. By construction the FIM is a real, symmetric, positive semidefinite matrix, meaning all eigenvalues are nonnegative.
Iii-D Localizability Criteria
In design of experiments there are several common optimality measures which relate the eigenvalues of the FIM to quality of the underlying estimation[pukelsheim2006optimal]. For our problem this can be thought of as different measures of how the network configuration at a given timestep affects the underlying information geometry and, as a result, the localization quality.
As our approach uses prioritized planning, these measures are observed with increasing numbers of robots in the network, which corresponds to inference problems of increasing dimensionality. Because of this changing dimensionality, it can be difficult to relate the values of these optimality measures as robots are added to the network. For this reason, we focus on A-optimality and E-optimality in this paper, as they are relatively consistent as robots are added, and do not consider other popular measures such as D-optimality, which measures the volume of the covariance ellipsoid and thus grows geometrically with increasing robots.333In addition, LCs could be designed which do not involve the FIM or information theory, but within this paper we strictly consider FIM optimality measures as LCs.
We present A- and E-optimality in Equations 7 and 6 and refer the interested reader to [pukelsheim2006optimal] for theoretical properties of these measures as well as a list of other possible measures. We define as the vector of robot positions for robots in the network and as the FIM that is formed from that configuration of robots. The optimality measures are then as follows:
We consider LCs to be predetermined, user-specified minimum values on these measures. Under the LCs defined by Equations 7 and 6 the set of allowed locations is for some user-defined values of and . For a general set of such user-defined criteria, we refer to network configurations which satisfy those criteria as localizability-constrained.
Iii-E Planning Problem Definition
Given the ability to compute the FIM for a given network, we describe our localizability constrained planning problem. This problem takes as input a set of beginning locations (), a set of goal locations (), and a set of LCs . Given these inputs, the problem is to find trajectories for each robot () such that each robot begins at its starting position, ends at its goal position, and the LCs of the network are satisfied at every timestep.
Iv Localizability Constrained Planning
To solve the planning problem described in Section III-E we present a prioritized, graph-based planner which plans for all robots on a single, common graph. To allow for scalability to larger numbers of robots we use a prioritized approach [VanDenBerg2005iros] to reduce the computational cost of planning from the -dimensional joint-configuration space of the robot network to the cost of planning in the -dimensional configuration space of just a single robot. The graph-based planning is a probabilistic roadmap [Kavraki1996] approach in which we consider the network to move along the graph in discrete timesteps and enforce LCs at each timestep. We make use of constraint sets from our previous work [papalia2020network] to efficiently enforce the LCs. In particular, we require that the trajectories planned for each robot stay within what we term the valid set, in Equation 15, for all timesteps .
Iv-a Constraint Sets
The trajectory of the robot network can be thought of as a time-indexed sequence of networks, with each network corresponding to a unique timestep. Similarly, prioritized planning for each robot can be thought of as adding a single node to each timestep-indexed network. This framework for viewing the problem is key in understanding the benefit of the constraint sets described in this section, as the constraint sets represent where nodes can be added to maintain certain constraints on the network.
We plan each robot’s trajectory separately and the localization conditions for each timestep are independent of each other. For each robot and timestep , there is a separate set of constraint sets . Constraint sets were first described in our previous work [papalia2020network] as a means to reduce the necessary computation to perform planning. The constraint sets presented in this paper are the same concept as those from our previous work, with the notable difference in that we replace the rigid set, in [papalia2020network], with the localizability constraint satisfying LCSAT set, . In [papalia2020network] was constraining the E-optimality of the network, albeit without directly stating this relationship, whereas allows for a combination of such optimality constraints and more directly ties into the theory of design of experiments.
Effectively, we use relationships between the constraint sets to reduce the actual search space for each robot to a small subset of the actual nodes in the planning graph. In this approach, the search space is reduced to what we term the valid set, , which, for non-anchor robots is the intersection of all other constraint sets corresponding to robot at timestep .
We begin by defining the boolean indicator function for the LCSAT set in Equation 11. We slightly abuse notation in using to indicate the position of an arbitrary node in the planning graph, whereas refers to the position of robot at timestep according to a planned trajectory. The indicator function, , evaluates to true if and only if at timestep the set of network positions formed by the position in addition to the positions of all previously planned robots satisfies the user-determined localizability criteria.
We formally define in Equation 8, where is a partial assignment of robot positions from the robots before in the priority queue. This arises from the prioritized planning, as the trajectories for robots to will have been planned, so the positions of all of these robots at a given timestep will be fixed. As such, suggests that the set of positions considered is just indexed by an arbitrary position as all other positions in the set are fixed given the timestep .
The constraint sets are defined as follows, where and index robots, indexes timesteps, and is the radius of the sensing horizon:
The reachable set represents all points that robot can get to by timestep while remaining within the valid set at all previous timesteps. The connected set represents all points that robot can occupy at timestep while being within the sensing horizon of one of the positions of the already planned robots () at that timestep. The localizability-constrained set is the set of all positions which satisfy the described localizability indicator function, defined in Equation 11. Finally, the valid set is the true set of interest in that it describes locations which robot can reach by time while satisfying the LCs at all times.
Within this work we assume the existence of anchor robots, which have
known position and therefore for these robots if a location is reachable we
consider it valid,444 For certain applications one may consider imposing
heuristic constraints on these vehicles to encourage them towards certain
behaviors, e.g. requiring they stay within a certain distance of each
other, however in this work we do not explore such situations.
For certain applications one may consider imposing heuristic constraints on these vehicles to encourage them towards certain behaviors, e.g. requiring they stay within a certain distance of each other, however in this work we do not explore such situations.whereas for the non-anchor robots a location must satisfy LCs as well as be reachable to be valid. In fact, as we assume the existence of three anchors, we prove in Lemma IV-A and Section IV-A that if at least one non-anchor robot has fewer than range measurements to it the FIM must be singular.555This can be intuitively understood, as for -dimensional localization range measurements are necessary to fully constrain the position estimate.
[-Connectivity and FIM Singularity] Given the FIM defined in Equation 5, for any number of non-anchor nodes, if there exists a non-anchor node with less than neighbors the FIM will be singular.
[Localizability Requires -Connectivity] Consider the localizability constraint indicator as in Equation 11. We define a nontrivial constraint to be an assignment of .666As the FIM is positive semidefinite, these lower bounds (, 0) are the minimum values of the respective images of Equations 7 and 6 and thus a ’trivial’ assignment of these values satisfies for all possible network configurations. Given nontrivial constraints, if there exists such that then the localizability constraints must be violated, i.e. must evaluate to false.
This relationship between the FIM and the connectivity of the non-anchor robots leads to the result that for any reasonable LCs using any of the criteria in Equations 6 to 7 the relationship in Equation 16 holds.
We take advantage of the relationship in Equation 16 when constructing the constraint sets, as in general checking whether a given position is in the LCSAT set () requires computing the eigenvalues of the FIM. As the FIM, and thus its eigenvalues, differ for every possible , constructing the FIM and finding eigenvalues becomes computationally demanding. To reduce the necessary computation, we only check states which are already connected, i.e. .
Furthermore, notice that the only set needed to perform planning is the valid set, and as valid states must be reachable (from Equation 15) we can improve efficiency by only checking the LCSAT conditions for states which are also reachable, i.e. . This is seen in creftypeplural 13 and 12 of Algorithm 2, where the intersection of the reachable and connected sets form the candidate states for the valid set.
Iv-B Prioritized Planning
Given a planning graph, the prioritized planning proceeds by iterating over all the robots in the network in the predetermined order, first generating the constraint sets for that robot and then performing planning along the graph such that the robot is within the valid set for all timesteps . This process is repeated for each robot in sequence until either all robot trajectories have been planned or the planner has failed. The planner is considered to fail if in the constraint set construction phase the valid sets never contain the goal location for that robot. As after a given amount of timesteps the valid sets will either become empty or reach a steady-state and no longer include new positions, it is simple in practice to check if the planner fails in this manner. If at some point the valid set possesses the goal location for robot then it is guaranteed by the definition of the valid set that there exists a trajectory to that robot’s goal position which satisfies the LCSAT conditions at all timesteps. In this event, set construction is considered successful.
In our previous work [papalia2020network] we presented a conflict-based approach for attempting to handle failures in the constraint set construction phase. However, in practice we observed no occasions where the conflict resolution approach led to successful replanning within any reasonable timespan. For this reason we remove this aspect from the planning framework described here in favor of an approach which fails outright when a trajectory cannot be found. As the success of prioritized planning can depend on the planning order, planning failures can be followed by a reordering and subsequent replanning attempts.
As the anchor robots are necessary in evaluating the LCs, the anchor robots are assumed to be planned before the non-anchor robots. Beyond this, no requirements are made on the ordering of the planning, the method to generate the planning graph, or the graph-based planner used.
|Test Case||# Robots||Env. Complexity||Algorithm||Planning Time (s)||Orderings||ALE (m)||MLE (m)||AD (m)|
*The average and max error for the RRT approach for test case 2 are not included since one robot moved out of the sensing range of all other robots, which prevented the solver from producing a solution for the network during those timesteps.
We tested our localization-constrained graph planning (LCGP) framework over a number of two-dimensional simulated environments with varying numbers of robots and obstacles. One of the tested environments and corresponding trajectories in mid-execution is shown in Figure 2 for reference. We set the first three robots as anchors (having known position) and the remaining robots as non-anchors (having unknown positions).
We compare statistics on timing, planning, and localization for our planning technique to three alternatives. We test against a prioritized RRT planner [kuffner2000rrt], against a prioritized A* planner, and against the potential field motion planner of [LeNy2018] with E-optimality terms. All planners were implemented in Python and run on an Intel i7-10875H processor.
To generate our planning graph, we use a Halton sequence to sample the environment. Edges were made between every sampled node and neighbors within 2 units, excluding edges that intersect obstacles. We assume each robot to be a point robot and that there are no kinematic restrictions on a robot’s movement. To then perform planning on this graph, we use heuristic-directed A* search with the Euclidean distance from a node to the goal location as the heuristic.
Ordering is known to have substantial effects on the results of prioritized planning. In the event of a failed planning, the planning order of the non-anchor robots was reshuffled and planning was reattempted. In cases 2 and 5, the initial ordering did not produce a viable trajectory, so an alternative ordering was attempted.
During planning we constrained E-optimality to be greater than a value of 0.1 and placed no constraints on the A-optimality. This was justified by tests we ran, which compared localization quality to various combinations of A-optimality and E-optimality constraints and indicated that A-optimality measures had relatively little effect on localization error in comparison to E-optimality.
To compare the ability to localize over the course of a given path, we implemented a nonlinear least-squares solver to find the maximum a posteriori estimate and report both the mean error across all non-anchor robots at all timesteps in the network and the max error of the mean across all non-anchor robots at a single timestep. This error is calculated as the Euclidean distance between the localization estimates and the ground truth positions.
The results for trajectories generated by our algorithm show the least max localization errors in all instances except for the potential field in case 1 and A* in test case 5. The greatest improvement was in test case 3, where LCGP reduced the maximum localization error 42% as compared to RRT and 26% relative to A*. The average localization errors for our algorithm are also comparable or lower than the alternatives for all cases. These results indicate that localizability-constrained planning constructs trajectories which improve range-only localization.
To evaluate the feasibility of each planner, we report the time required to plan and the average distance travelled for each robot. In the case of LCGP, the planning time includes the time required to build the planning graph and replan using alternate orderings if necessary.
In environments with marked complexity or more challenging obstacles, notably cases 3, 4, and 5, we found that our approach was as fast as or faster than the RRT approach if the initial ordering succeeded. In test cases with any obstacles (2-5), the potential field approach failed to find a trajectory to the goal configuration due to local minima. The relative speed of our LCGP planner as compared to the RRT planner increased as either the number of robots or the complexity of the environment increased. However, in more simple environments with a large amount of free space, the RRT planner was able to plan trajectories up to an order of magnitude faster than the LCGP approach, as seen in cases 1 and 2.
Beyond planning time, we note that our LCGP approach found trajectories with notably shorter execution distances compared to our RRT implementation. This difference between our RRT implementation and our LCGP algorithm is likely due to the use of A* to perform planning on the graph, which encourages the LCGP planner to minimize travel distance. In comparison, the RRT approach would often generate inefficient trajectories with no weight given to the travel time. We do not compare the resulting distances to the theoretically optimal distances, but these results along with qualitative inspection of the trajectories indicate that the LCGP planner does generate distance-efficient trajectories.
In this paper we present a framework for efficiently planning localizability-constrained trajectories for a network of robots localizing via inter-robot ranging. We relate localization constraints to design of experiments and prove that for -dimensional localization every robot of unknown position must have range measurements to at least other robots in the network to satisfy our localization constraints. We validate the effectiveness of our planning approach and its effect on the localization errors through extensive simulations.
Continuation of this work could consider the effects of different localizability measures on the localization error. Additionally, future work could explore how existing approaches in optimization-based trajectory planning can be modified to accommodate localization constraints.
Vii-a Proof of Lemma Iv-A
To show that in a network where any non-anchor node has fewer than neighbors the FIM is singular, we first demonstrate that the FIM has at least zero-eigenvalues when a non-anchor node has no neighbors and then demonstrate that neighbors are the minimum number required to eliminate all of the zero-eigenvalues. Our proof focuses on the column structure of the FIM, but the FIM is symmetric, so the same arguments apply to the row-structure of the FIM.
Recall that the FIM is a ()-block-structured matrix, with the th () block-column relating to position of non-anchor robot . From Equation 5 it is apparent that the FIM is the sum of rank-1 updates from the vectors and corresponding to measurements in the network. It directly follows from Equations 4 and 3 that if non-anchor node has no neighbors then the th block-column will be entirely zeros. By the size of the block-column, the FIM must then have at least columns of zero-vectors and therefore have at least zero eigenvalues.
By continuation of this idea, in the previous case the eigenvectors corresponding to the zero eigenvalues are known to correspond to the dimensions in robot ’s position. The FIM is a series of rank-1 updates where the dimensions affected by the updates are in the direction of the vectors and . Similarly, from Equations 4 and 3 it follows that the direction of the update only affects the dimensions corresponding to robot ’s position if the update corresponds to a measurement edge including robot .
As each rank-1 update can only affect a single direction in the -dimensional null-space, there must be linearly independent rank-1 updates to make the FIM non-singular.777This requirement of linearly independent rank-1 updates to make the FIM non-singular relates to the property: To have linearly independent rank-1 updates requires at least measurement edges involving robot . Therefore if robot has less than neighbors the FIM is singular.
Vii-B Proof of Theorem Iv-A
For an arbitrary nontrivial constraint defined by as in Section IV-A and its corresponding constraint indicator function . We show that in the event that any non-anchor node has less than neighbors in the network any nontrivial constraint will be violated. By Lemma IV-A, if a non-anchor node has less than neighbors the FIM will be singular. If the FIM is singular it immediately follows that and thus any nontrivial E-optimality constraint is violated. Similarly, as the FIM approaches a singular representation the A-optimality metric approaches negative infinity, i.e. , and thus in the limit violates any nontrivial A-optimality constraint. As is undefined when the FIM is singular as the FIM cannot be inverted, we consider the limiting behavior to show violation of nontrivial A-optimality constraints. Thus if any non-anchor node has less than neighbors any nontrivial constraint set will be violated.