An Optimal Control Approach to Mapping GPS-Denied Environments Using a Stochastic Robotic Swarm

Abstract

This paper presents an approach to mapping a region of interest using observations from a robotic swarm without localization. The robots have local sensing capabilities and no communication, and they exhibit stochasticity in their motion. We model the swarm population dynamics with a set of advection-diffusion-reaction partial differential equations (PDEs). The map of the environment is incorporated into this model using a spatially-dependent indicator function that marks the presence or absence of the region of interest throughout the domain. To estimate this indicator function, we define it as the solution of an optimization problem in which we minimize an objective functional that is based on temporal robot data. The optimization is performed numerically offline using a standard gradient descent algorithm. Simulations show that our approach can produce fairly accurate estimates of the positions and geometries of different types of regions in an unknown environment.

Appendices

Appendix 1: Mathematical Preliminaries

We study the solution to PDEs in the weak sense, which can be found in the Sobolev space $H^1(\varOmega ) = \left\{ y \in L^2(\varOmega ) : \frac{\partial y}{\partial x_1} \in L^2(\varOmega ),\right. \left. \ \frac{\partial y}{\partial x_2} \in L^2(\varOmega ) \right\}$. Here, the spatial derivative is to be understood as a weak derivative defined in the distributional sense. The space is equipped with the common Sobolev space norm, $\left\| y \right\| _{H^1(\varOmega )} = \sqrt{\left( \left\| y \right\| ^2_{L^2(\varOmega )} + \sum _{i =1}^{2} \left\| \frac{\partial y}{\partial x_i} \right\| ^2_{L^2(\varOmega )} \right) }$. We also define $V = H^1(\varOmega )$, which has the dual space $V^* = H^1(\varOmega )^*$.

We consider the general system for Eqs. (3)–(5):

$$\begin{aligned} \frac{\partial u}{\partial t} = Au + \sum _{i=1}^{2}v_iB_iu -K(\mathbf {x})u + f\ ~~~in\ ~L, \nonumber \\ \mathbf {n} \cdot (D\nabla u - \mathbf {v} u) = g\ ~~~on\ ~\varGamma , \nonumber \\ u(\mathbf {x},0) = u_0,\qquad \end{aligned}$$

(9)

where A is a formal operator and $B_i$ is an operator defined as $B_i : L^2(0,T;V) \rightarrow L^2(0,T;L^2(\varOmega ))$, $K(\mathbf {x}) \in L^2(\varOmega )$, $f \in F = L^2(0,T;L^2(\varOmega ))$ is the forcing function in the system, $g \in G = L^2(0,T;L^2(\partial \varOmega ))$, and $u_0 \in L^2(\varOmega )$. The variational form of the operator A, called $A_g$, is defined as $A_g : L^2(0,T;V) \rightarrow L^2(0,T;V^*)$. The solution of the system in the weak sense is given by $u \in U = L^2(0,T;V)$ with $u_t \in U^* = L^2(0,T;V^*)$ if it satisfies the equation:

$$\begin{aligned} \left\langle \frac{\partial u}{\partial t},\phi \right\rangle _{U^*,U} = \left\langle A_g,\phi \right\rangle _{U^*,U} + \sum _{i=1}^{2} \left\langle v_iB_iu, \phi \right\rangle _F -\left\langle K(\mathbf {x})u, \phi \right\rangle _F + \left\langle f,\phi \right\rangle _ F \end{aligned}$$

(10)

for all $\phi \in L^2(0,T;V)$. The boundary conditions are equipped with $A_g$ in the variational formulation using Green’s theorem. This is essentially the variational form of the Laplacian,

$$\begin{aligned} \left\langle A_g u,\phi \right\rangle _{U^*,U} = -\left\langle D \nabla u, \nabla \phi \right\rangle _{L^2(\varOmega )} + \int _{\partial \varOmega } \left( g + \mathbf {n} \cdot \mathbf {v} u \right) \phi dx. \end{aligned}$$

(11)

In the macroscopic model Eqs. (3)–(5), we define $A = \nabla ^2$, $B_i = \frac{\partial }{\partial x_i}$, $f = 0$, and $g = 0$.

Appendix 2: Adjoint Equations

The adjoint equation $\nabla _\mathbf {u} \mathscr {L} = 0$ implies that $[\nabla _{u_{1}} \mathscr {L},\ldots ,\nabla _{u_{i}} \mathscr {L},\ldots ,\nabla _{u_{N}} \mathscr {L} ] = 0.$ From Eq. (8),

$$\begin{aligned} \nabla _{u_{i}}\mathscr {L}= & {} \nabla _{u_{i}}\mathbf {J}(\mathbf {u},K) + \nabla _{u_{i}} \sum _{j = 1}^{N} \langle p_j, \varPsi _j(u_j,K) \rangle \nonumber \\= & {} \nabla _{u_{i}}\mathbf {J}(u_{i},K) + \nabla _{u_{i}} \langle p_i, \varPsi _i(u_i,K) \rangle , \end{aligned}$$

(12)

since a term in the sum is a function of $u_i$ only when $i=j$. By Eq. (7),

$$\begin{aligned} \nabla _{u_{i}}\mathbf {J}(u_{i},K) = \nabla _{u_{i}} \sum _{j = 1}^{N} W_j J_j(u_j) = W_i \nabla _{u_{i}} J_i(u_i). \end{aligned}$$

(13)

From Eq. (6),

$$\begin{aligned} \nabla _{u_{i}} J_i(u_i) = \nabla _{u_{i}} \left( \frac{1}{2}\left\| (D u_i)(t) - g_i(t)\right\| _{L^2([0,T])} ^2\right) , \end{aligned}$$

(14)

where $D := U \rightarrow L^2([0,T])$ and $(D u_i)(t) = \int _{\varOmega } u_i(\mathbf {x},t) d\mathbf {x}$. Then, by the chain rule of differentiation [4, 12], the directional derivative of $J_i(u_i)$, $\nabla _{u_{i}} J_i(u_i)$, is given by

$$\begin{aligned} \langle \nabla _{u_{i}} J_i(u_i) , s \rangle _{U} = \langle (D u_i)(t) - g_i(t), Ds \rangle _{L^2([0,T])} = \langle D^*((D u_i)(t) - g_i(t)), s \rangle _{U}. \end{aligned}$$

(15)

Here, $D^* := L^2([0,T]) \rightarrow U$ and $(D^*f)(t) = f(t)\cdot \mathbf {1}_\varOmega (\mathbf {x})$, where $f(t) \in L^2([0,T])$ and $\mathbf {1}_\varOmega$ is the indicator function of $\varOmega \subset \mathbb {R}^2$. We can show that $\langle D y,f \rangle = \langle y,D^*f \rangle \ \forall y \in U , ~f \in L^2([0,T])$. Therefore,

$$\begin{aligned} \nabla _{u_{i}} J_i(u_i) = D^*((D u_i)(t) - g_i(t)). \end{aligned}$$

(16)

By definition,

$$\begin{aligned} \langle p_i,\nabla _{u_{i}} \varPsi _i(u_i,K)s \rangle = \langle \nabla _{u_{i}} \varPsi _i(u_i,K)^* p_i,s \rangle ~~ \forall s \in U, \end{aligned}$$

(17)

where $\nabla _{u_{i}} \varPsi _i(u_i,K)^*$ is the adjoint operator of $\nabla _{u_{i}} \varPsi _i(u_i,K)$ corresponding to the inner product of the Hilbert space. Now, by taking the directional derivative of $\varPsi _i(u_i,K)$ at $u_i$ in the direction of s, we obtain

$$\begin{aligned} \nabla _{u_{i}} \varPsi _i(u_i,K)s = \frac{\partial s}{\partial t} -( \nabla \cdot (D\nabla s - \mathbf {v}_i(t)s) -kK(\mathbf {x})s). \end{aligned}$$

(18)

Substituting Eq. (18) into Eq. (17) yields

$$\begin{aligned} \langle p_i,\nabla _{u_{i}} \varPsi _i(u_i,K)s \rangle = \int _{0}^{T}\langle p_i,\frac{\partial s}{\partial t} \rangle _{L^2(\varOmega )} - \langle p_i,D\nabla ^2s \rangle + \langle p_i,\nabla \cdot \mathbf {v}_i(t)s \rangle + \langle p_i,kK(\mathbf {x})s \rangle . \end{aligned}$$

Using integration by parts on the integral term in the equation above, we get

$$\begin{aligned} \int _{0}^{T}\langle p_i,\frac{\partial s}{\partial t} \rangle _{L^2(\varOmega )} = \langle p_i(T),s(T)\rangle - \langle p_i(0),s(0)\rangle -\int _{0}^{T}\langle s,\frac{\partial p_i}{\partial t} \rangle _{L^2(\varOmega )}. \end{aligned}$$

As this is true for all $s \in U$, we could choose the s with $s(0) = 0$ and construct $p_i(T)$ such that $\int _{0}^{T}\langle p_i,\frac{\partial s}{\partial t} \rangle _{L^2(\varOmega )} = \int _{0}^{T}\langle -\frac{\partial p_i}{\partial t},s \rangle _{L^2(\varOmega )}$. Thus, we choose the final condition of the adjoint equation as $p_i(T) =0$. We now make use of the following lemma:

Lemma 1 Let L and $L^*$ be operators defined by $L : L^2(0,T;V) \rightarrow L^2(0,T;V^*)$ and $L^* : L^2(0,T;V) \rightarrow L^2(0,T;V^*)$, respectively. The variational form of L is:

$$\begin{aligned} \langle Lu,\phi \rangle _{V^*,V} = -\left\langle D \nabla u, \nabla \phi \right\rangle _{L^2(\varOmega )} - \langle \mathbf {v} \cdot \nabla u,\phi \rangle _{L^2(\varOmega )} + \int _{\partial \varOmega } \mathbf {n} \cdot (\mathbf {v}u \phi ) dx \end{aligned}$$

$\forall \phi \in V$. Also, by Lagrange’s identity, $\langle Lu,p \rangle _{V^*,V} = \langle u,L^*p \rangle _{V,V^*} ~\forall u,p \in L^2 (0,T;V)$. We use the zero-flux boundary condition in Eq. (4) to compute the variational form of the operator $L^*$ to be $\langle L^*p,\phi \rangle _{V^*,V} = -\left\langle D \nabla p, \nabla \phi \right\rangle _{L^2(\varOmega )} + \langle \mathbf {v}\cdot \nabla p,\phi \rangle _{L^2(\varOmega )}$ $\forall p \in L^2(0,T;V)$ and $\forall \phi \in V$.

Using the variational form of the Laplacian as in Eq. (11) and applying Lemma 1 and integration by parts, we can show that $-\langle p_i,D\nabla ^2s \rangle + \langle p_i,\nabla \cdot \mathbf {v}_i(t)s \rangle$ can be transformed into $~ -\langle D\nabla ^2p_i,s \rangle - \langle \nabla \cdot \mathbf {v}_i(t)p_i,s \rangle$ with the boundary condition $\mathbf {n}\cdot \nabla p_i = 0$. Finally, we observe that $\langle p_i,K(\mathbf {x})s \rangle = \langle p_i K(\mathbf {x}),s \rangle$. By combining these results with Eqs. (12), (15), and (17), we obtain

$$\begin{aligned} \langle \nabla _{u_{i}} J_i(u_i) , s \rangle + \langle -\frac{\partial p_i}{\partial t} - D\nabla ^2p_i -\nabla \cdot \mathbf {v}_i(t)p_i + p_i kK(\mathbf {x}), s \rangle = 0. \end{aligned}$$

Thus, the set of adjoint equations for the system defined by the $i^{th}$ set of constraints, $\varPsi _i(u_i,K)$, with respect to the objective functional, $\mathbf {J}$, is given by

$$\begin{aligned} -\frac{\partial p_i}{\partial t} = \nabla \cdot (D\nabla p_i +\mathbf {v}_i(t)p_i) - p_i k K(\mathbf {x}) - \nabla _{u_{i}} J_i(u_i) ~~\ in ~\ L \end{aligned}$$

(19)

with the Neumann boundary conditions

$$\begin{aligned} \mathbf {n}\cdot \nabla p_i = 0 ~~\ on\ ~\varGamma , ~~~ p_i(T) = 0, ~~~i = 1,\ldots ,N. \end{aligned}$$

(20)

Here, Eq. (19) with Eq. (20) has a solution in the weak sense.

Appendix 3: Gradient Equation

Using a similar analysis to the one in Appendix 2, we find that $\nabla _K \mathscr {L}$ reduces to

$$\begin{aligned} \nabla _K \mathscr {L} = \nabla _{K}\mathbf {J}(\mathbf {u},K) + \sum _{i = 1}^{N} \nabla _{K} \langle p_i, \varPsi _i(u_i,K) \rangle . \end{aligned}$$

(21)

From Eq. (7), we can derive the following expressions:

$$\begin{aligned} \nabla _{K}\mathbf {J}(\mathbf {u},K) = \nabla _{K} \frac{\lambda }{2}\Vert K(\mathbf {x})\Vert ^2_{L^2(\varOmega )}, ~~~ \langle \nabla _{K}\mathbf {J}(\mathbf {u},K),s \rangle = \langle \lambda K(\mathbf {x}),s \rangle . \end{aligned}$$

(22)

As in Appendix 2, we could express $\langle p_i, \nabla _{K} \varPsi _i(u_i,K) s \rangle$ as $\langle \nabla _{K} \varPsi _i(u_i,K)^* p_i,s \rangle \ \forall s \in L^2(\varOmega )$, where $\nabla _{K} \varPsi _i(u_i,K)^*$ is the adjoint operator of $\nabla _{K} \varPsi _i(u_i,K)$ corresponding to the inner product of the Hilbert space. Now, by taking the directional derivative of $\varPsi _i(u_i,K)$ at K in the direction of s, we find that $\nabla _{K} \varPsi _i(u_i,K)s = ku_is$. Therefore, with further simplification, we can show that

$$\begin{aligned} \langle \nabla _{K} \varPsi _i(u_i,K)^* p_i,s \rangle = \langle (\varXi (ku_ip_i))(\mathbf {x}),s\rangle _{L^2(\varOmega )}, \end{aligned}$$

(23)

where $\varXi := L^2(0,T;\varOmega ) \rightarrow L^2(\varOmega )$ and $(\varXi f)(\mathbf {x}) = \int _{0}^{T}fdt$ for all $f \in L^2([0,T];\varOmega )$ and $\mathbf {x} \in \varOmega$. By combining Eqs. (21)–(23), we formulate the objective functional derivative as

$$\begin{aligned} \mathbf {J}' = \sum _{i = 1}^{N} (\varXi (ku_ip_i))(\mathbf {x}) + \lambda K(\mathbf {x}). \end{aligned}$$

(24)

Thus, the computation of $\mathbf {J}'$ requires $u_i$ and $p_i$, which can be obtained by solving $\varPsi _i(u_i,K)$ forward and solving Eqs. (19), (20) backward.