Nonlinear observability of unicycle multi-robot teams subject to nonuniform environmental disturbances

Abstract

In this work, we consider the problem of localizing a team of robots, without access to direct pose measurements, under the influence of nonuniform environmental disturbances and measurement bias. Specifically, we are interested in the conditions under which teams remain range-only localizable when the environmental disturbances vary from robot to robot. We approach this problem through nonlinear observability and graph theory. After analyzing the system’s observability properties, we present theorems that identify the structural conditions under which the system maintains local weak observability. We demonstrate that rigid structures are important not only in defining multi-robot interactions, but also in characterizing the influence of nonuniform disturbances. We also give several example systems to cement intuition on the derived conditions. An observability-based planner is then presented that guides a subset of robots toward trajectories that are highly observable through finite-horizon optimization on robot headings. Simulations are then presented, along with an extended Kalman filter for state estimation, and a comparison to previous methods, to corroborate and demonstrate the results derived.

Derivation of the observability matrix

As shown in Sect. 3.1 we will need to calculate several successive Lie derivatives in order to get the nonlinear observability matrix. For the following $i \in [1,N]$, $j \in [1,d]$, $\alpha \in [1,A]$, $(u,v) = e_k \in {\mathcal {E}}$, and $(q,r) = e_p \in \mathcal {E^*}$. Let us consider the beacon contribution to the measurement function (7), which gives the following Lie derivatives:

$$\begin{aligned} \nabla _{{\mathbf {x}}} {\mathcal {L}}_f^0(x_\alpha (j))= & {} \nabla _{{\mathbf {x}}} x_\alpha (j)\nonumber \\= & {} [{\mathbf {0}}_d^T, \dots , [{\mathbf {0}}_{j-1}^T, 1, {\mathbf {0}}_{d-j}^T]^T, \dots , \nonumber \\&{\mathbf {0}}_{dA}^T,{\mathbf {0}}_{dN}^T,{\mathbf {0}}_d^T]^T \end{aligned}$$

(18)

Which we can easily expand on to get the next derivative:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^1(x_\alpha (j)) = \nabla _{{\mathbf {x}}}(\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^0(x_i(j))^T f({\mathbf {x}},{\mathbf {u}}))\\&\quad =\nabla _{{\mathbf {x}}}( [{\mathbf {0}}_d^T, \dots , [{\mathbf {0}}_{j-1}^T, 1, {\mathbf {0}}_{d-j}^T]^T, \dots , \\&\qquad {\mathbf {0}}_{dA}^T,{\mathbf {0}}_{dN}^T,{\mathbf {0}}_d^T] \times \left[ \dot{{\mathbf {p}}}_{i}(t)^T, \dot{d}_{x}(t), \dot{d}_{y}(t) \right] ^T)\\&\quad =[{\mathbf {0}}_{dN}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,{\mathbf {v}}_i(j),{\mathbf {0}}_{A-\alpha }^T], {\mathbf {0}}_{2dN-1}^T,\dots ,\\&\qquad [{\mathbf {0}}_{j-1}^T,1,{\mathbf {0}}_{d-j}^T]^T] \end{aligned}$$

Repeat this process to get the next derivative:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^2(x_\alpha (j)) = \nabla _{{\mathbf {x}}}(\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^1(x_i(j))^T f({\mathbf {x}},{\mathbf {u}}))\\&\quad =\nabla _{\mathbf {x}}([{\mathbf {0}}_{dN}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,{\mathbf {v}}_i(j),{\mathbf {0}}_{A-\alpha }^T], {\mathbf {0}}_{2dN-1}^T,\\&\qquad \dots ,{\mathbf {0}}_{d}^T] \times \left[ \dot{{\mathbf {p}}}_{i}(t)^T, \dot{d}_{x}(t), \dot{d}_{y}(t) \right] ^T)\\&\quad =[{\mathbf {0}}_{dN}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,\mathbf {u'}_i(j),{\mathbf {0}}_{A-\alpha }^T], {\mathbf {0}}_{2dN-1}^T,\\&\qquad \dots ,{\mathbf {0}}_{d}^T]\\ \end{aligned}$$

Moving on to the heading measurements, we can evaluate the Lie derivatives associated with these measurements as:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^0\left( \theta _i + d_{\theta _i} \right) = \nabla _{{\mathbf {x}}} \left( \theta _i + d_{\theta _i} \right) \nonumber \\&\quad =[{\mathbf {0}}_{Ad}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,1,{\mathbf {0}}_{N-\alpha }^T]^T,\nonumber \\&\qquad [{\mathbf {0}}_{\alpha -1}^T,1,{\mathbf {0}}_{N-\alpha }^T]^T,\dots ,{\mathbf {0}}_d^T] \end{aligned}$$

(19)

Now replace in next Lie derivative:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^1\left( \theta _i + d_{\theta _i} \right) = \nabla _{{\mathbf {x}}} \left( \nabla _{{\mathbf {x}}} {\mathcal {L}}_f^0\left( \theta _i + d_{\theta _i} \right) ^T f({\mathbf {x}}, {\mathbf {u}}) \right) \\&\quad =\nabla _{\mathbf {x}}([{\mathbf {0}}_{Ad}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,1,{\mathbf {0}}_{N-\alpha }^T]^T,\\&\qquad [{\mathbf {0}}_{\alpha -1}^T,1,{\mathbf {0}}_{N-\alpha }^T]^T,\dots ,{\mathbf {0}}_d^T] \times \\&\qquad \left[ \dot{{\mathbf {p}}}_{i}(t)^T, \dot{d}_{x}(t), \dot{d}_{y}(t) \right] ^T)\\&\quad =[{\mathbf {0}}_{Ad}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,\omega '_i,{\mathbf {0}}_{N-\alpha }^T]^T,\\&\qquad [{\mathbf {0}}_{\alpha -1}^T,g_i'(d_{\theta _i}),{\mathbf {0}}_{N-\alpha }^T]^T,\dots ,{\mathbf {0}}_d^T] \\ \end{aligned}$$

Replace once again to get the next:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^2\left( \theta _i + d_{\theta _i} \right) = \nabla _{{\mathbf {x}}} \left( \nabla _{{\mathbf {x}}} {\mathcal {L}}_f^1\left( \theta _i + d_{\theta _i} \right) ^T f({\mathbf {x}}, {\mathbf {u}}) \right) \\&\quad =\nabla _{\mathbf {x}}([{\mathbf {0}}_{Ad}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,\omega '_i,{\mathbf {0}}_{N-\alpha }^T]^T,\\&\qquad [{\mathbf {0}}_{\alpha -1}^T,g_i'(d_{\theta _i}),{\mathbf {0}}_{N-\alpha }^T]^T,\dots ,{\mathbf {0}}_d^T] \times \\&\qquad \left[ \dot{{\mathbf {p}}}_{i}(t)^T, \dot{d}_{x}(t), \dot{d}_{y}(t) \right] ^T)\\&\quad =[{\mathbf {0}}_{Ad}^T,\dots ,[{\mathbf {0}}_{\alpha -1}^T,(\omega '_i)^2,{\mathbf {0}}_{N-\alpha }^T]^T,\\&\qquad [{\mathbf {0}}_{\alpha -1}^T,(g_i'(d_{\theta _i}))^2,{\mathbf {0}}_{N-\alpha }^T]^T,\dots ,{\mathbf {0}}_d^T] \\ \end{aligned}$$

Finally, we can evaluate the Lie derivatives associated with the range measurement function in a similar way:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^0 \left( \frac{1}{2} \Vert x_{e_k}\Vert ^2 \right) = \nabla _{{\mathbf {x}}} \left( \frac{1}{2} \Vert x_{e_k}\Vert ^2 \right) \nonumber \\&\quad =[{\mathbf {0}}_d^T, \dots , ({\mathbf {p}}_u - {\mathbf {p}}_v)^T, \dots ,\nonumber \\&\qquad ({\mathbf {p}}_v - {\mathbf {p}}_u)^T, \dots , {\mathbf {0}}_d^T, {\mathbf {0}}_{Nd + dk}^T]^T \end{aligned}$$

(20)

Iterate again to get the next order:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^1 \left( \frac{1}{2} \Vert x_{e_k}\Vert ^2 \right) \\&\quad =\nabla _{{\mathbf {x}}} \left( \nabla _{{\mathbf {x}}} {\mathcal {L}}_f^0\left( \frac{1}{2} \Vert x_{e_k}\Vert ^2 \right) ^T f({\mathbf {x}}, {\mathbf {u}}) \right) \\&\quad = \nabla _{\mathbf {x}}([{\mathbf {0}}_d^T, \dots , ({\mathbf {p}}_u - {\mathbf {p}}_v)^T, \dots ,\\&\qquad ({\mathbf {p}}_v - {\mathbf {p}}_u)^T, \dots , {\mathbf {0}}_d^T, {\mathbf {0}}_{Nd + dk}^T] \times \\&\qquad \left[ \dot{{\mathbf {p}}}_{\alpha }(t)^T, \dot{d}_{x}(t), \dot{d}_{y}(t) \right] ^T )\\&\quad = [{\mathbf {0}}_d^T, \dots , ({\mathbf {v}}_u - {\mathbf {v}}_v)^T,\dots , ({\mathbf {v}}_v - {\mathbf {v}}_u)^T,\\&\qquad \dots , {\mathbf {0}}_d^T, \dots , ({\mathbf {p}}_u-{\mathbf {p}}_v)^T \frac{d}{d\theta _u} {\mathbf {v}}_u,\\&\qquad \dots , ({\mathbf {p}}_v-{\mathbf {p}}_u)^T \frac{d}{d\theta _v} {\mathbf {v}}_v, \dots ,\\&\qquad {\mathbf {0}}_d^T, \dots , ({\mathbf {p}}_q - {\mathbf {p}}_r)^T, \dots , ({\mathbf {p}}_r - {\mathbf {p}}_q)^T, \dots , {\mathbf {0}}_d^T]^T \end{aligned}$$

Iterate once more to get the following:

$$\begin{aligned}&\nabla _{{\mathbf {x}}} {\mathcal {L}}_f^2 \left( \frac{1}{2} \Vert x_{e_k}\Vert ^2 \right) \\&\quad = \nabla _{{\mathbf {x}}} \left( \nabla _{{\mathbf {x}}} {\mathcal {L}}_f^1\left( \frac{1}{2} \Vert x_{e_k}\Vert ^2 \right) ^T f({\mathbf {x}}, {\mathbf {u}}) \right) \\&\quad = \nabla _{{\mathbf {x}}} ( [{\mathbf {0}}_d^T, \dots , ({\mathbf {v}}_u - {\mathbf {v}}_v)^T, \dots ,\\&\qquad ({\mathbf {v}}_v - {\mathbf {v}}_u)^T, \dots , {\mathbf {0}}_d^T,\\&\qquad \dots , ({\mathbf {p}}_q - {\mathbf {p}}_r)^T, \dots , ({\mathbf {p}}_r - {\mathbf {p}}_q)^T, \dots , {\mathbf {0}}_d^T] \times \\&\qquad \left[ \dot{{\mathbf {p}}_{\alpha }(t)}^T, \dot{d}_{x}(t), \dot{d}_{y}(t) \right] ^T)\\&\quad = [{\mathbf {0}}_d^T, \dots , (\dot{{\mathbf {v}}_u} - \dot{{\mathbf {v}}_v})^T, \dots ,\\&\qquad (\dot{{\mathbf {v}}_v} - \dot{{\mathbf {v}}_u})^T, \dots , {\mathbf {0}}_d^T, \dots , \\&\qquad 2v_u v_v \sin (\theta _u - \theta _v) ({\mathbf {p}}_u-{\mathbf {p}}_v)^T \frac{d}{d\theta _u} \mathbf {u'}_u,\dots \\&\qquad {\mathbf {0}}_{v-1}^T, \dots ,2v_u v_v \sin (\theta _v - \theta _u) ({\mathbf {p}}_v-{\mathbf {p}}_u)^T \frac{d}{d\theta _v} \mathbf {u'}_v,\\&\qquad \dots ,{\mathbf {0}}_d^T, g_i'(d_{\theta _u})({\mathbf {p}}_u-{\mathbf {p}}_v)^T \frac{d}{d\theta _u} {\mathbf {v}}_u,\dots ,\\&\qquad g_i'(d_{\theta _v})({\mathbf {p}}_v-{\mathbf {p}}_u)^T \frac{d}{d\theta _v} {\mathbf {v}}_v, \dots \\&\qquad {\mathbf {0}}_d^T, \dots , ({\mathbf {v}}_q - {\mathbf {v}}_r)^T, \dots , ({\mathbf {v}}_r - {\mathbf {v}}_q)^T, \dots , {\mathbf {0}}_d^T]^T\\ \end{aligned}$$