Rationally Inattentive Path-Planning via RRT*

The control cost is simply the commanded travel distance in the Euclidean metric:

Jointly accounting for both the control efficiency and sensing simplicity in planning necessitates the formulation of a metric that captures the information acquisition cost required for path following. We utilize the information gain (entropy reduction) for this purpose.

Assume that the control input $\bm{u}_{k}=x_{k+1}-x_{k}$ is applied to (4). The propagation of the prior covariance during the movement of the robot, over the time interval $[t_{k},t_{k+1})$, is denoted as $\hat{P}_{k}=P_{k}+\|x_{k+1}-x_{k}\|W$. At time $t_{k+1}$, the covariance is “reduced” to $P_{k+1}(\preceq\hat{P}_{k})$ by utilizing a sensor input. The minimum information gain (minimum number of bits that must be contained in the sensor data) for this transition is:

The notion of an “optimal” sensing signal which reduces $\hat{P}_{k}$ to $P_{k+1}$ has been previously discussed in [22] in the context of optimal sensing in filtering theory. The information cost function $\mathcal{D}_{\text{info}}(k)$ in (6) is well-defined for the pairs $(P_{k},P_{k+1})$ satisfying $P_{k+1}\preceq\hat{P}_{k}$. For those pairs which do not satisfy $P_{k+1}\preceq\hat{P}_{k}$, we generalize (6) as:

Notice that (7) takes a non-negative value for any given transition from an origin $(x_{k},P_{k})$ to destination $(x_{k+1},P_{k+1})$. However, (7) is an implicit function involving a convex optimization problem in its expression (more precisely, the max-det problem [23]). To see why (7) is an appropriate generalization of (6), consider a two-step procedure $\hat{P}_{k}\rightarrow Q_{k+1}\rightarrow P_{k+1}$ to update the prior covariance $\hat{P}_{k}$ to the posterior covariance $P_{k+1}$. In the first step, the uncertainty is “reduced” from $\hat{P}_{k}$ to satisfy both $Q_{k+1}\preceq\hat{P}_{k}$ and $Q_{k+1}\preceq P_{k+1}$. The associated information gain (the amount of telemetry data) is $\frac{1}{2}\log_{2}\det\hat{P}_{k}-\frac{1}{2}\log_{2}\det Q_{k+1}$. In the second step, the covariance $Q_{k+1}$ is “increased” to $P_{k+1}(\succeq Q_{k+1})$. This step incurs no information cost, since the location uncertainty can be increased simply by “deteriorating” the prior knowledge. The max-det problem (7) can then be interpreted as finding the optimal intermediate step $Q_{k+1}$ which minimizes the information gain in the first step.

The cost to steer a random state variable $\bm{x}_{k}\sim\mathcal{N}(x_{k},P_{k})$ to $\bm{x}_{k+1}\sim\mathcal{N}(x_{k+1},P_{k+1})$ is a weighted sum of $\mathcal{D}_{\text{cont}}(k)$ and $\mathcal{D}_{\text{info}}(k)$. Introducing $\alpha>0$, the total RI cost is:

By increasing $\alpha$, more weight is placed on the amount of information which must be gained compared to the distance traversed. Note that the information cost $\mathcal{D}_{\text{info}}$ is an asymmetric function, so that transitioning $(x_{1},P_{1})\rightarrow(x_{2},P_{2})$ does not return the same cost as $(x_{2},P_{2})\rightarrow(x_{1},P_{1})$.

In this subsection, we introduce a topology for the space of paths $\gamma:[0,T]\rightarrow\mathbb{R}^{d}\times\mathbb{S}_{++}^{d}$, which is necessary to discuss continuity of $c(\gamma)$. The space of all paths $\gamma:[0,T]\rightarrow\mathbb{R}^{d}\times\mathbb{S}_{++}^{d}$ can be thought of as a subset (convex cone) of the space of generalized paths $\gamma:[0,T]\rightarrow\mathbb{R}^{d}\times\mathbb{S}^{d}$. The space of generalized paths is a vector space on which addition and scalar multiplication exist and are defined as $(\gamma_{1}+\gamma_{2})(t)=(x_{1}(t)+x_{2}(t),P_{1}(t)+P_{2}(t))$ and $\alpha\gamma(t)=(\alpha x(t),\alpha P(t))$ for $\alpha\in\mathbb{R}$, respectively. Assuming that a path can be partitioned such that $\mathcal{P}=(0=t_{0}<t_{1}<\cdots<t_{N}=T)$, the variation $V(\gamma;\mathcal{P})$ of a generalized path $\gamma$ with respect to the choice of $\mathcal{P}$ is given by:

where $\Delta x_{k}=x(t_{k+1})-x(t_{k})$, and $\Delta P_{k}=P(t_{k+1})-P(t_{k})$. Utilizing the above definition for the variation of a path, the total variation of a generalized path $\gamma$ corresponds to the partition $\mathcal{P}$ which results in the supremum of the variation:

Notice that $|\cdot|_{\text{TV}}$ defines a norm on the space of generalized paths. The following relationship holds between $|\gamma|_{\text{TV}}$ and

In what follows, we assume on the space of generalized paths $\gamma:[0,T]\rightarrow\mathbb{R}^{d}\times\mathbb{S}^{d}$ the topology of total variation metric $|\gamma_{1}-\gamma_{2}|_{\text{TV}}$, which is then inherited to the space of paths $\gamma:[0,T]\rightarrow\mathbb{R}^{d}\times\mathbb{S}_{++}^{d}$. We denote by $\mathcal{BV}[0,T]$ the space of paths $\gamma:[0,T]\rightarrow\mathbb{R}^{d}\times\mathbb{S}_{++}^{d}$ such that $|\gamma|_{\text{TV}}<\infty$. In the next subsection, we discuss the continuity of the RI path cost $c(\cdot)$ in the space $\mathcal{BV}[0,T]$.

The continuity of RI path cost function plays a critical role in determining the theoretical guarantees we can provide when we use sampling-based algorithms to find the shortest RI path. Specifically, the asymptotic optimality (the convergence to the path with the minimum cost as the number of nodes is increased) of RRT* algorithms [12], the main numerical method we use in this paper, expects the continuity of the path cost function. Showing that the RI cost function (8) is continuous requires additional derivation which is shown via Theorem 1.

Before discussing the required modifications for implementing RRT* algorithm with RI cost in Section IV, we first characterize the shortest RI path in the obstacle-free space, and then formally define the shortest RI path problem in obstacle-filled spaces in the following subsections.

In obstacle-free space, it can be shown that the optimal path cost between $z_{1}=(x_{1},P_{1})$ and $z_{2}=(x_{2},P_{2})$ is equal to $\mathcal{D}(z_{1},z_{2})$. In other words, the triangular inequality $\mathcal{D}(z_{1},z_{2})\leq\mathcal{D}(z_{1},z_{int})+\mathcal{D}(z_{int},z_{2})$ holds. This means it is optimal for the robot to follow the direct path from $x_{1}$ to $x_{2}$ without sensing, and then make a measurement at $x_{2}$ to shrink the uncertainty from $\hat{P}_{1}$ to $P_{2}$. In what follows, we call such a motion plan the “move-and-sense” strategy. The optimality of the move-and-sense path for one-dimensional geometric space is shown in Appendix C. We confirm this optimality by simulation in Section V-A, where the move-and-sense path is the wedge-shaped path depicted in Fig. 3 (a).

The utility of path-planning algorithms is made non-trivial by the introduction of obstacles in the path space. Let $X_{obs}\subset\mathbb{R}^{d}$ be a closed subset of spatial points representing obstacles. The initial configuration of the robot is defined as $z_{\text{init}}=(x_{0},P_{0})\in\mathbb{R}^{d}\times\mathbb{S}_{++}^{d}$, while $\mathcal{Z}_{\text{target}}\subset\mathbb{R}^{d}\times\mathbb{S}_{++}^{d}$ is a given closed subset representing the target region which the robot desires to attain. Given a confidence level parameter $\chi^{2}>0$, the shortest RI path problem can be formulated as:

The $\chi^{2}$ term in the constraints of (9) is implemented to provide a confidence bound on probability that a robot with position $x(t)$ will not be in contact with an obstacle $x_{\text{obs}}$.

The RRT algorithm [25] constructs a tree of nodes (state realizations) through random sampling of the feasible state-space and then connects these nodes with edges (tree branches). A user-defined cost is utilized to quantify the length of the edges, which are in turn summed to form path lengths. Each new node is connected via a permanent edge to the existing node which provides the shortest path between the new node and the initial node of the tree. Although the RRT algorithm is known to be probabilistically complete (the algorithm finds a feasible path if one exists), it does not achieve asymptotic optimality (path cost does not converge to the optimal one as the number of nodes is increased) [12]. The RRT* algorithm [12] attains asymptotic optimality by including an additional “re-wiring” step that re-evaluates if the path length for each node can be reduced via a connection to the newly created node. This paper utilizes RRT* as a numerical approach to the shortest path problem (9).

Provided below is a Rationally Inattentive RRT* (RI-RRT*) algorithm for finding a solution to (9). Like the original RRT* algorithm, the RI-RRT* algorithm constructs a graph of state nodes and edges ($G\leftarrow(Z,E)$) in spaces with or without obstacles.

In Algorithm 1, the $\textsc{Generate}(i)$ function creates a new point by randomly sampling a spatial location ($x\in\mathbb{R}^{d}$) and covariance ($P\in\mathbb{S}_{++}^{d}$). Notice that for a $d$-dimensional configuration space, the corresponding uncertain configuration space $\mathbb{R}^{d}\times\mathbb{S}^{d}_{++}$ has $d+\frac{1}{2}d(d+1)$ dimensions from which the samples are generated. The $\textsc{Nearest}(Z,z_{i})$ function finds the nearest point ($z_{\text{near}}$), in metric $\hat{\mathcal{D}}(z,z^{\prime}):=\|x-x^{\prime}\|+\|P-P^{\prime}\|_{F}$, between the newly generated state $z_{i}=(x_{i},P_{i})$ and an existing state in the set $Z$. Using the metric $\hat{\mathcal{D}}(z,z^{\prime})$, the $\textsc{Scale}(z_{\text{near}},z_{i},ED_{\text{min}})$ function linearly shifts the generated point ($z_{i}$) to a new location as:

where $ED_{\text{min}}$ is a user-defined constant. In addition to generating $z_{\text{new}}$, the Scale function also ensures that its $\chi^{2}$ covariance region does not interfere with any obstacles.

The $\textsc{ObsCheck}(z_{\text{near}},z_{\text{new}})$ function ensures that transition from state $z_{\text{near}}$ to $z_{\text{new}}$ does not intersect with the obstacles. More precisely, we assume the transition $z_{\text{near}}\rightarrow z_{\text{new}}$ follows the move-and-sense path, introduced in Section III-C, and the ObsCheck function returns $False$ if all state pairs along the move-and-sense path, shown by blue ellipses in Fig. 2, has $\chi^{2}$ covariance regions that are non-interfering with obstacles.

The $\textsc{Neighbor}(Z,z_{\text{new}},ED_{\text{nbors}})$ function returns the sub-set of nodes described as $Z_{\text{nbors}}=\{z_{i}=(x_{i},P_{i})\in Z:\hat{\mathcal{D}}(z_{i},z_{\text{new}})\leq ED_{\text{nbors}}\}$. This set is then evaluated for the presence of obstacles via the ObsCheck function of the previous paragraph. Note that in this instance, the function is evaluating obstacle interference along the continuous path of state-covariance pairs from $z_{j}\rightarrow z_{\text{new}}$. Lines 14-17 of Algorithm 1 connect the new node to the existing graph in an identical manner to RRT*, where the $Path_{z}$ denote the cost of the path from the $z_{init}$ to node $z$ through the edges of $G$. Line 18 creates a new edge between the new node and the existing nodes from the neighbor group $Z_{\text{nbors}}$ which results in the minimum $Path_{z_{\text{new}}}$. The calculation of RI path cost in Line 14 utilizes (8).

Lines 19-24 are the tree re-wiring steps of Algorithm 1. In line 20, the ObsCheck function is called again. This is because the move-and-sense path is direction-dependent, and thus $\textsc{ObsCheck}(z_{j},z_{\text{new}})=False$ does not necessarily imply $\textsc{ObsCheck}(z_{\text{new}},z_{j})=False$. Finally, for each rewired node $z_{j}$, its cost (i.e., $Path_{z_{j}}$) and the cost of its descendants are updated via $\textsc{UpdateDes}(G,z_{j})$ function in line 25.

To increase the computational efficiency of RI-RRT* algorithm we deploy a branch-and-bound technique as detailed in [26]. For a given tree $G$, let $z_{\text{min}}$ be the node that has the lowest cost along the nodes of $G$ within $\mathcal{Z}_{\text{target}}$. As discussed in Section III-C, $\mathcal{D}(z,z_{\text{goal}})$ is a lower-bound for the cost of transitioning from $z$ to $z_{\text{goal}}$. The branch-and-bound algorithm periodically deletes the nodes $Z^{\prime\prime}=\{z\in Z:Path_{z}+\mathcal{D}(z,z_{\text{goal}})\geq Path_{z_{\text{min}}}\}$. This elimination of the non-optimal nodes speeds up the RI-RRT* algorithm.

The question regarding the asymptotic optimality of RI-RRT* naturally arises. Recall that the proof of the asymptotic optimality of the RRT* algorithm [27] is founded on four main assumptions:

1. 1.

   additivity of the cost function,

2. 2.

   the cost function is monotonic,

3. 3.

   there exists a finite distance between all points on the optimal path and the obstacle space,

4. 4.

   the cost function is Lipschitz continuous, either in the topology of total variation metric [27] or the supremum norm metric [12].

The proofs of the first three assumptions are trivial for the RI cost (8). However, Theorem 1 does not suffice to guarantee that the RI cost meets the fourth condition for $d\geq 2$. For this reason, currently the asymptotic optimality of the RI-RRT* algorithm cannot be guaranteed, while the numerical simulations of Section V do show that the proposed algorithm does have merit in rationally inattentive path-planning.

The first study is the case of a robot which is allowed to travel at a constant velocity in a one-dimensional geometric space from a predetermined initial position and covariance $z_{0}=(x_{0},P_{0})$, specified by the red dot in Fig. 3 (a). The robot has a goal of reaching some final state within the blue box representing a goal region which is a sub-set of the reachable space. Note that the goal region contains acceptable bounds on both location and uncertainty. Although, in one-dimensional setting, the strategy which minimizes the control cost is obviously the one that moves directly toward the target region, we utilize the RI-RRT* algorithm to solve the non-trivial measurement scheduling problem.

In Fig. 3 (a), the blue curve represents the path generated by the RI-RRT* algorithm with $10,000$ nodes, which is sufficiently close to the shortest path obtainable via the RI-distance depicted as the red curve. These wedge-shaped optimal paths are created by the “move-and-sense” strategy integrated in the RI-cost, where it has a section of covariance propagation followed by an instantaneous reduction of covariance as discussed in Section III-C. For example, if the robot were an autonomous ground vehicle with GPS capabilities, then this path signifies the robot driving the total distance without any GPS updates, followed by a reduction its spatial uncertainty with a single update once the goal region is reached. The minimum path cost at the end of each iteration of the for-loop in Algorithm 1 is depicted in Fig. 3 (b), where the red curve represents the average of $100$ independent simulations. The path cost of each simulation approaches to the optimal cost.

The asymmetric characteristic of the RI-cost is demonstrated via a simulation in the two-dimensional configuration space with a diagonal wall, as seen in Fig. 4. The initial position and covariance of the robot is depicted as a red dot, while the target region is illustrated as the black rectangle at the upper-right corner.

The path is generated by the RI-RRT* algorithm by sampling $10,000$ nodes. The corresponding sampled covariance ellipses are shown in black where the blue ellipses represent covariance propagation. As shown in Fig. 4, there are two options; path A requires the robot to move into a funnel-shaped corridor, while the path B moves out of a funnel. In this setting, the RI-cost prefers the path B even though both A and B have the same Euclidean distance. This asymmetric behavior results from the fact that as the robot approaches the goal region path B requires a less severe uncertainty reduction compared to path A. Similarly, by exchanging the start and goal positions, the RI-cost prefers path A over path B, thus displaying the directional dependency of our efficient sensing strategy.

In a final demonstration, the RI-RRT* algorithms with $\alpha=0,0.1,$ and $0.3$ are implemented in a two-dimensional configuration space containing multiple obstacles in order to illustrate the effects of varying the information cost. All three paths in panels of Fig. 5 are generated from 4,000 nodes.

As seen in Fig. 5 (a), when $\alpha=0$ the algorithm simply finds a path that has the shortest Euclidean distance, even if that path requires frequent sensing actions. In contrast, the RI-RRT* algorithm with $\alpha=0.3$ does not take a constrained pathway, and thus requires fewer sensor actuations in order to avoid obstacle collisions. As a result, the algorithm deviates from the shortest Euclidean distance path and allows the covariance to propagate safely, as seen in Fig. 5 (c). A moderate path, illustrated in Fig. 5 (b), can be also obtained by choosing $\alpha=0.1$.