Online decentralized information gathering with spatial–temporal constraints

Abstract

We are interested in coordinating a team of autonomous mobile sensor agents in performing a cooperative information gathering task while satisfying mission-critical spatial–temporal constraints. In particular, we present a novel set of constraint formulations that address inter-agent collisions, collisions with static obstacles, network connectivity maintenance, and temporal-coverage in a resource-efficient manner. These constraints are considered in the context of the target search problem, where the team plans trajectories that maximize the probability of target detection. We model constraints continuously along the agents’ trajectories and integrate these constraint models into decentralized team planning using a computationally efficient solution method based on the Lagrangian formulation and decentralized optimization. We validate our approach in simulation with five UAVs performing search, and through hardware experiments with four indoor mobile robots. Our results demonstrate team planning with spatial–temporal constraints that preserves the performance of unconstrained information gathering and is feasible to implement with reasonable computational and communication resources.

Appendices

1: Multistep motion model derivatives

This section details the explicit gradient form for the states of a sensor agent with respect to its action variables along a multi-step piece-wise constant action horizon (Gan and Sukkarieh 2011). The agent’s state transition is:

$$\begin{aligned} s^{k+1} = s^{k} + \left[ {\begin{array}{l} { \frac{V}{u^k} \left( \sin \left( \psi ^{k}+\Delta \psi ^k \right) -\sin {\psi ^{k}} \right) } \\ { \frac{V}{u^k} \left( -\cos \left( \psi ^{k}+\Delta \psi ^k \right) +\cos {\psi ^{k}} \right) } \\ { \Delta \psi ^k } \\ \end{array}} \right] , \nonumber \\ \end{aligned}$$

(57)

where $\Delta \psi ^k = u^k \Delta t^k$. Differentiating Eq. (57) with respect to the turn rate command $u^k$ gives:

$$\begin{aligned} \frac{{\partial {s}^{k + 1} }}{{\partial u^k }} = \left[ {\begin{array}{l} \frac{{V\Delta t^k \cos \psi ^{k + 1} - x^{k + 1} + x^{^k } }}{u^k} \\ \frac{{V\Delta t^k \sin \psi ^{k + 1} - y^{k + 1} + y^{^k } }}{u^k} \\ {\Delta t^k } \\ \end{array}} \right] . \end{aligned}$$

(58)

Note that $\frac{{\partial {s}^{k} }}{{\partial u^k }} = 0$ since $\psi ^k$ is the action starting from state s $^k$ and thus it has no effect on s $^{k}$. Similarly, the next sensing state is:

$$\begin{aligned} s^{k + 2}&= {s}^{k + 1} \nonumber \\&+ \left[ {\begin{array}{*{20}c} {\frac{V}{{u^{k + 1} }}\left( {\sin \left( \psi ^{k+1} + \Delta \psi ^{k+1} \right) - \sin \psi ^{k + 1} } \right) } \\ {\frac{V}{{u^{k + 1} }}\left( { - \cos \left( \psi ^{k+1} + \Delta \psi ^{k+1} \right) + \cos \psi ^{k + 1} } \right) } \\ {\Delta \psi ^{k+1} } \\ \end{array}} \right] .\nonumber \\ \end{aligned}$$

(59)

Its derivative is:

$$\begin{aligned} \frac{{\partial {s}^{k + 2} }}{{\partial u^k }} = \frac{{\partial {s}^{k + 1} }}{{\partial u^k }} + \frac{{\partial \psi ^{k + 1} }}{{\partial u^k }}\left[ {\begin{array}{c} { - \left( {y^{k + 2} - y^{k + 1} } \right) } \\ {x^{k + 2} - x^{k + 1} } \\ 0 \\ \end{array}} \right] . \end{aligned}$$

(60)

The sensitivity of the remaining sensing states to the same action variable can be obtained recursively by differentiating Eq. (58) with respect to the same action for the remaining segments. This can be compactly described in a matrix form:

$$\begin{aligned} \frac{{\partial { s^{k+1:k+N}}}}{{\partial v^k }} = \left[ {\begin{array}{l@{\quad }l@{\quad }l@{\quad }l} {\frac{{\partial {s}^{k + 1} }}{{\partial u^k }}} &{} 0 &{} \ldots &{} 0 \\ {\frac{{\partial {s}^{k + 2} }}{{\partial u^k }}} &{} {\frac{{\partial {s}^{k + 2} }}{{\partial u^{k + 1} }}} &{} 0 &{} \vdots \\ \vdots &{} {\frac{{\partial {s}^{k + 3} }}{{\partial u^{k + 1} }}} &{} \ddots &{} 0\\ {\frac{{\partial {s}^{k + N} }}{{\partial u^k }}} &{} \ldots &{} {\frac{{\partial {s}^{k + N - 1} }}{{\partial u^{k + N - 2} }}} &{} {\frac{{\partial {s}^{k + N} }}{{\partial u^{k + N - 1} }}} \\ \end{array}} \right] , \end{aligned}$$

(61)

where the diagonal and cross components are calculated using Eqs. (58) and (60) respectively.

2: Information-theoretic search derivatives

This section details the explicit gradient form for the probabilistic search objective function with respect to its action variables along a multi-step piece-wise constant action horizon (Gan and Sukkarieh 2011). In decentralized information-theoretic target search, a commonly defined objective function is the joint probability of target no-detection events. From the perspective of agent $i$:

$$\begin{aligned} J_i\left( v_i^k, \alpha _{\mathcal {J}^{-i} i}^k \right) \!=\! \mathop \int \limits _{\xi } P \left( z_{i}^{k+1:k+N}\!=\!\overline{D}| \xi ,s_{i}^{k+1:k+N} \right) {}^{i}\bar{b}^k_\xi d\xi .\nonumber \\ \end{aligned}$$

(62)

Assuming the target belief ${}^{i}\bar{b}_\xi ^k$ is static over the action horizon $H$, which is reasonable for slow target dynamics and fast replanning frequency, and assuming each consecutive sensor observation is independent, the joint conditional probability of not detecting a target over the whole action horizon is:

$$\begin{aligned} P\left( {z}^{k + 1:k + N}=\overline{D}|\xi ,s^{k + 1:k + N} \right)&= \prod \limits _{l = 1}^N {P\left( {z}^{k + l}=\overline{D}|\xi ,s^{k + l} \right) }\nonumber \\&= \prod \limits _{l = 1}^N {O\left( \xi ,s^{k + l} \right) }. \end{aligned}$$

(63)

The first order derivative of $J_i$ with respect to its local action vector $v_i^k$ is:

$$\begin{aligned} \frac{{\partial J_i}}{{\partial v_i^k }} = \left[ {\frac{{\partial J_i}}{{\partial u_i^k }}, \ldots ,\frac{{\partial J_i}}{{\partial u_i^{k + N - 1} }}} \right] ^T . \end{aligned}$$

Substituting Eq. (63) into Eq. (62) and applying partial derivatives with respect to each of the individual action variable results in the following chained partial derivatives:

$$\begin{aligned}&\frac{{\partial J_i }}{{\partial u_i^{k + m} }}\nonumber \\&\quad = \mathop \int \limits _\xi {\sum \limits _{\begin{array}{c} n =\\ m + 1 \end{array}}^N {\frac{{\partial O\left( {s_i^{k + n} },\xi \right) }}{{\partial u_i^{k + m} }}\prod \limits _{\begin{array}{c} l = 1 \\ l \ne n \end{array}}^N {O\left( {s_i^{k + l},\xi } \right) {}^{i}\bar{b}_\xi ^k d\xi } } } \nonumber \\&\quad = \mathop \int \limits _\xi {\sum \limits _{\begin{array}{c} n =\\ m + 1 \end{array}}^N { \left( {\frac{{\partial O\left( {s_i^{k + n} },\xi \right) }}{{\partial {s_i} }}\frac{{\partial s_i^{k + n} }}{{\partial u_i^{k + m} }}} \right) \prod \limits _{\begin{array}{c} l = 1 \\ l \ne n \end{array}}^N {O\left( {s_i^{k + l} ,\xi } \right) {}^{i}\bar{b}_\xi ^k d\xi } } } , \nonumber \\ \end{aligned}$$

(64)

where $\frac{{\partial s_i^{k + n} }}{{\partial u_i^{k + m} }}$ is the sensitivity of the sensing state to the action variables, obtainable from “Multistep motion model derivatives” section, and $\frac{{\partial O\left( {s_i^{k + n} },\xi \right) }}{{\partial {s_i} }}$ is the sensitivity of the sensor model to its corresponding sensing state.

For a distance-based sensor model,

$$\begin{aligned} O\left( \xi , s \right) = 1 - P_{d_{max}} e^{-\sigma \left( \frac{d}{d_{max}} \right) ^2}. \end{aligned}$$

(65)

Since it is invariant to the sensor orientation, its sensitivity to sensor orientation is zero. We are left only with the derivative with respect to sensor position as

$$\begin{aligned} \frac{{\partial O\left( {\xi ,s } \right) }}{{\partial { p}}} = \frac{{2\sigma }}{{d_{\max }^2 }}\left( {{ p} - \xi } \right) \left( 1-O\left( {\xi ,s } \right) \right) , \end{aligned}$$

(66)

and thus:

$$\begin{aligned} \frac{{\partial O\left( {\xi ,s } \right) }}{{\partial {s}}} = \left[ \frac{{\partial O\left( {\xi ,s} \right) }}{{\partial { p}}}, 0 \right] . \end{aligned}$$

(67)

3: Temporal-coverage derivatives

This section details the explicit gradient of the temporal-coverage constraint model in Sect. 7.3. The partial derivative of Eq. (53) with respect to sensing states is:

$$\begin{aligned} \frac{\partial G_{is}^k}{\partial s_{i}^k} = h\left( \bar{\delta }_{is}^k \right) \frac{\partial {\bar{\delta }}_{is}^k}{\partial s_{i}^k} , \end{aligned}$$

(68)

where

$$\begin{aligned} \dfrac{\partial \delta }{\partial s}= -\frac{-R_{mc}}{d_{BC}}\left( \frac{\partial \alpha }{\partial s} + \frac{\partial \beta }{\partial s} \right) - \frac{d_{CO} \delta }{{d_{BC}}^2}\left( \frac{\partial d_{CO}}{\partial s} \right) . \end{aligned}$$

(69)

Here,

$$\begin{aligned} \frac{\partial d_{CO}}{\partial s} = \left[ \begin{array}{c} {\frac{p_O-p_s}{d_{CO}}} \\ { \frac{\left( x_O-x \right) \left( y-y_s \right) +\left( y_O-y \right) \left( x_s-x \right) }{d_{CO}}} \end{array} \right] , \end{aligned}$$

(70)

$$\begin{aligned} \frac{\partial \alpha }{\partial s} = -\frac{R_{mc}}{{d_{CO}}^2\cos {\alpha }} \frac{\partial d_{CO}}{\partial s}, \end{aligned}$$

(71)

and

$$\begin{aligned} \frac{\partial \beta }{\partial s} = \left[ \begin{array}{c} {\frac{{d_{CO}}^2\cos {\psi }-a_1(x_s-x_O)}{{d_{CO}}^3\sin {\beta }}} \\ {\frac{{d_{CO}}^2\sin {\psi }-a_1(y_s-y_O)}{{d_{CO}}^3\sin {\beta }}} \\ {\frac{a_2 {d_{CO}} - wR_{mc} d_{CO} + a_1 \frac{\partial d_{CO}}{\partial \psi } }{{d_{CO}}^2\sin {\beta }}} \end{array} \right] , \end{aligned}$$

(72)

where $a_1$ and $a_2$ are defined respectively as

$$\begin{aligned} a_1 = (x_s-x_O)\cos {\psi } + (y_s-y_O)\sin {\psi }, \end{aligned}$$

and

$$\begin{aligned} a_2 = (x_s-x_O)\sin {\psi } + (y_s-y_O)\cos {\psi }. \end{aligned}$$