2.1. GNSS-Aware Planning with Environmental Model

This subsection reviews studies on ensuring reliable robot movement and task execution by avoiding areas where the GNSS position estimation accuracy deteriorates.

Gutierrez et al. proposed a path planning method ²⁶ that passes through locations where several line-of-sight signals received directly from satellites can be obtained. Alyammahi et al. ²⁷ and Zhang and Hsu ²⁸ aimed to achieve robust position estimation by creating maps that directly predicted the accuracy of GNSS position estimation.

However, to estimate the effects of multipath in advance, it is necessary to accurately simulate the reflection and diffraction of GNSS signals. The effectiveness of this approach can be compromised by modeling inaccuracies or changes in the geometry of the structures and terrain. Therefore, adaptive control methods are necessary to enable an escape when entering multipath-affected spaces.

2.2. Control and Path Planning for Robust Cooperative Localization Systems

This subsection reviews studies that address avoiding accuracy degradation in cooperative localization systems through control and path planning.

Go and Liu proposed a method ²⁹ to improve the stability of relative observations by the offline trajectory optimization of the UAVs. Although this demonstrates the importance of formation control, the method is not applicable for adaptively escaping from multipath-affected spaces. Tasooji and Parasuraman proposed a method ³⁰ to stabilize the relative position estimation by increasing the communication frequency with robots whose position estimation accuracy has deteriorated. However, even if this approach is adopted, it only assists in estimating the position of the support-UAV. This does not improve the position estimation accuracy of the work-UAV. Zhang et al. proposed a planning method ³¹ that controls the support robots to optimal positions as moving anchors to improve the position estimation accuracy of the worker robots. However, this method was developed based on the assumption that the position of the mobile anchor is known, which differs from our focus on preventing a decrease in the position estimation accuracy of the support-UAVs themselves.

Jiang et al. conducted preliminary studies²⁵ on a technique for escaping from multipath-affected spaces. This approach has the potential to improve the position estimation of both the work-UAV and support-UAVs. In this paper, we propose an adaptive formation control method for incorporating the concept of escape from a space with multipath effects into the cooperative positioning system proposed by Kimura et al. ^18,19.

3.1. Statements of Localization Models

This section describes the localization model. This system measures the absolute position of the support-UAV by GNSS-localization and the relative position between the work-UAV and support-UAVs based on relative-localization. These models are based on those presented by Kimura et al. ¹⁸, except for the addition of multipath effects. Note that, in this study, the GNSS information from the work-UAV working under the structures is considered unreliable and excluded from the cooperative localization system. These models are described as follows.

3.2. Sensor Fusion

After performing GNSS-localization and relative-localization, sensor fusion methods are applied. We adopted a hierarchical sensor fusion scheme to prioritize the robustness of the position estimation of the work-UAV for reliable task execution. First, the position of the work-UAV is estimated by performing the selective sensor fusion of the position estimation results from each support-UAV, while filtering out the information affected by the multipath. Subsequently, the position of the support-UAV is corrected by feeding back the position estimation results of the work-UAV to the support-UAV.

3.3. Pre-Planning of Support-UAVs’ Paths

The proposed method is controlled by referring to the pre-planned path for each support-UAV obtained using the method proposed by Kimura et al. ¹⁸. This method involves the offline planning of semi-optimal paths based on a cost map. The cost is defined on a dynamic map that evolves according to the movement of the work-UAV to represent the optimal location for the support-UAV. The cost map consists of two factors.

• GNSS-localization cost: The smaller the standard deviation of the GNSS-localization error, the lower is the cost.

• Relative-localization cost: The closer the support-UAV is to the work-UAV, the lower is the cost.

Based on the work-UAV’s waypoints \(W^{\mathrm{(w)}}=\{\mathbf{w}_{i}^\mathrm{(w)}\}_{i=1}^{I}\) for task execution, optimization is performed while predicting the movement of the work-UAV. Consequently, waypoints \(W^\mathrm{(s)}_n=\{\mathbf{w}_{n,i}^\mathrm{(s)}\}_{i=1}^I\) for each support-UAV are generated. When the work-UAV heads toward the \(i\)-th waypoint, the support-UAVs also head toward the \(i\)-th waypoint. In other words, their movements are synchronized. This allows the support-UAVs to maintain a relative position to maintain both the accuracy of GNSS-localization and relative-localization. However, this method cannot address the deterioration of the position estimation accuracy caused by the influence of multipath, which is unpredictable in advance.

4.1. Modeling and Updating of GNSS-Localizability

This subsection describes the GNSS-localizability model and its update method. The GNSS-localizability in the 3D space is modeled as a 3D grid map model, \(G\). The length of a side of each voxel is \(l_\mathrm{v}\), and each voxel \(\symbfup{\gamma} \in G\) stores a representative value for the standard deviation of the GNSS-localization error at time step \(t\), \(\sigma_{\mathrm{GNSS},t}(\symbfup{\gamma})\). \(G\) is initialized before starting the tasks, and is then updated for every control cycle. Each voxel is initialized to \(\sigma_{\mathrm{GNSS}, 0}(\symbfup{\gamma}) = \mathrm{Tr} (\Sigma_{\mathrm{GNSS}}(\mathrm{center}(\symbfup{\gamma})) )^{0.5}\) with Eq. \(\eqref{eq:sigmagnss}\) based on the satellite configuration. The function \(\mathrm{center}(\symbfup{\gamma})\) returns the position vector of the center of the voxel. Voxel updates are performed using a Gaussian kernel with center at \(\mathrm{center}(\symbfup{\gamma}_{\hat{\mathbf{y}}_n})\), where \(\symbfup{\gamma}_{\hat{\mathbf{y}}_n}\) is a voxel containing \(\hat{\mathbf{y}}_n\). Using the neighborhood factor \(\lambda\), the updates based on a Gaussian kernel are performed, as shown in Fig. 2, as follows:

4.2. MPC-Based Movement Command Generation

This subsection proposes an MPC that enables each support-UAV to perform both the relative-localization of the work-UAV and escape from multipath-affected spaces by referring to the GNSS-localizability model \(G\). The prediction of the semi-optimal local path \(V^{\mathrm{(s)}}_n\) for each support-UAV is the main topic of this subsection. The procedure (shown in Fig. 3) is as follows:

1. Predict \(V^{\mathrm{(w)}}=\{\mathbf{v}^{\mathrm{(w)}}_u\}_{u=1}^{U+1}\) (the local path for the work-UAV) and the destination indices \(K_\mathrm{pred}^\mathrm{(w)}=\{k_u\}_{u=1}^{U+1}\), corresponding to pre-planned waypoints \(\mathbf{w}_{k_u}\).

2. Generate \(m^\mathrm{(w)}\) (movement command for the work-UAV).

3. Predict \(V^{\mathrm{(s)}}_n=\{\mathbf{v}^{\mathrm{(s)}}_{n,u}\}_{u=1}^{U}\) (local path for each support-UAV (\(n\))) between the current estimated position \(\hat{\mathbf{y}}_n\) and terminal position \(\mathbf{w}_{n,k_{U+1}}^\mathrm{(s)}\).

4. Generate \(m^\mathrm{(s)}_n\) (movement commands for each support-UAV).

The first and second procedures are described in Section 4.2.1. The third and fourth procedures are described in Section 4.2.2. Note that the prediction time step, \(t_\mathrm{pred}\), is distinct from the control period \(t_\mathrm{cont}\).

4.2.1. MPC for Work-UAV

The MPC for the work-UAV starts prediction from \(\hat{\mathbf{x}}\) and performs the prediction of \(V^{\mathrm{(w)}}=\{\mathbf{v}^{\mathrm{(w)}}_u\}_{u=1}^{U+1}\), which greedily move toward the current target waypoint \(\mathbf{w}_k\). The maximum movement distance per prediction step is \(d^\mathrm{(w)}_\mathrm{max}\), defined as \(d^\mathrm{(w)}_\mathrm{max} = s_\mathrm{max}^\mathrm{(w)} t_\mathrm{pred}\), where \(s_\mathrm{max}^\mathrm{(w)}\) represents the speed limit of the work-UAV. The MPC algorithm is presented as Algorithm 1. The MPC predicts positions that are as close as possible to the waypoint \(\mathbf{w}_k\) within a range of distances \(d^\mathrm{(w)}_\mathrm{max}\) from the preview position \(\mathbf{v}_\mathrm{preview}^{\mathrm{(w)}}\). The target index \(k\) is incremented if the predicted waypoint \(\mathbf{v}_u^{\mathrm{(w)}}\) of the work-UAV reaches the actual waypoint \(\mathbf{w}_k\). With this MPC, work-UAVs can focus on key tasks, such as data collection.

The sets resulting from this prediction, \(V^{\mathrm{(w)}}=\{\mathbf{v}^{\mathrm{(w)}}_u\}_{u=1}^{U+1}\) and \(K_\mathrm{pred}^\mathrm{(w)}=\{k_u\}_{u=1}^{U+1}\), are then utilized by the MPC for the support-UAV.

4.2.2. MPC for Support-UAV

The objective of the support-UAV’s MPC is twofold: to follow its pre-planned path, and to escape from the detected multipath-affected spaces. The reason for the first objective is that the pre-planned path is treated as a global semi-optimal reference. The MPC generates a predictive path, \(V^{\mathrm{(s)}}_n=\{\mathbf{v}^{\mathrm{(s)}}_{n,u}\}_{u=1}^{U}\), that connects the current estimated position, \(\hat{\mathbf{y}}_n\), to a terminal point on a pre-planned path, \(\mathbf{w}_{n,k_{U+1}}^\mathrm{(s)}\). This prediction ensures that the UAV returns to the pre-planned path within the prediction horizon, allowing for local optimization while maintaining the global path fidelity.

The MPC algorithm for the support-UAV is described in Algorithm 2. We define the maximum movement distance per prediction step, \(d^\mathrm{(s)}_\mathrm{max}\), as \(d^\mathrm{(s)}_\mathrm{max} = s_\mathrm{max}^\mathrm{(s)} t_\mathrm{pred}\), where \(s_\mathrm{max}^\mathrm{(s)}\) represents the speed limit of support-UAVs.

The prediction path \(V^{\mathrm{(s)}}_n=\{\mathbf{v}^{\mathrm{(s)}}_{n,u}\}_{u=1}^{U}\) for each support-UAV (\(n\)) is generated by minimizing the cost function, as shown in Line 1. For cost function minimization, we use a smoothed GNSS-localizability field \(G_\mathrm{s}\) for cost calculation, instead of the direct GNSS-localizability model, \(G\). The smooth field \(G_\mathrm{s}\) addresses the issue of sparse gradients inherent in the original model \(G\). These sparse gradients would otherwise prevent optimization from effectively guiding the path \(\mathbf{v}^{\mathrm{(s)}}_{n,u}\) toward regions with high GNSS localizability. To calculate \(G_\mathrm{s}(\mathbf{v}'_{n,u})\), we used Hermite interpolation³⁵. The interpolation nodes were defined at the center of each voxel, \(\mathrm{center}(\symbfup{\gamma} )\), with a corresponding value of \(\sigma_{\mathrm{GNSS},t}(\symbfup{\gamma})\). For interpolation, the gradient at each node was assumed to be zero.

The cost function is as follows:

\begin{align} &C \left( V'_n \mid W_{n}^\mathrm{(s)},V^{\mathrm{(w)}}, d^\mathrm{(s)}_\mathrm{max}, G_\mathrm{s}, U, \hat{\mathbf{y}}_n, K_\mathrm{pred}^\mathrm{(w)}\right) \nonumber \\ &= C_\mathrm{GNSS} \left(V'_n,G_\mathrm{s},U\right) \nonumber \\ &\phantom{=~}+ C_\mathrm{interval} \left(V'_n,\hat{\mathbf{y}}_n, \mathbf{w}_{n,k_{U+1}}^\mathrm{(s)} \in W_{n}^\mathrm{(s)}, d^\mathrm{(s)}_\mathrm{max},U\right) \nonumber \\ &\phantom{=~}+ C_\mathrm{waypoint} \left(V'_n,W_{n}^\mathrm{(s)},U, K_\mathrm{pred}^\mathrm{(w)}\right) \nonumber \\ &\phantom{=~}+ C_\mathrm{relative} \left(V'_n,V^{\mathrm{(w)}},U \right). \label{eq:optimize_target} \end{align}

This cost function consists of four functions. We describe each function.

First, the cost of escaping from a poor GNSS-localizability space is as follows:

\begin{equation} \begin{aligned} C_\mathrm{GNSS}\left(V'_n,G_\mathrm{s},U \right)=\sum_{u=1}^{U}\omega_\mathrm{GNSS} G_\mathrm{s} \left(\mathbf{v}'_{n,u}\right) , \label{eq:Cgnss} \end{aligned} \end{equation}

where \(\omega_\mathrm{GNSS}\) denotes the weight coefficient. This calculates the sum of GNSS-localizability at each prediction step.

Second, the cost of reducing the movement distance is as follows:

\begin{align} &C_\mathrm{interval} \left(V'_n,\hat{\mathbf{y}}_n, \mathbf{w}_{n,k_{U+1}}^\mathrm{(s)} \in W_{n}^\mathrm{(s)}, d^\mathrm{(s)}_\mathrm{max},U \right) \nonumber \\ &=\xi_\mathrm{interval} \left( \left\lVert\hat{\mathbf{y}}_n-\mathbf{v}'_{n,1}\right\rVert,d^\mathrm{(s)}_\mathrm{max} \right) \nonumber \\ &\phantom{=~}+\sum_{u=1}^{U-1}\xi_\mathrm{interval} \left( \left\lVert\mathbf{v}'_{n,u+1}-\mathbf{v}'_{n,u}\right\rVert, d^\mathrm{(s)}_\mathrm{max} \right) \nonumber \\ &\phantom{=~}+ \xi_\mathrm{interval} \left( \left\lVert\mathbf{w}_{n,k_{U+1}}^\mathrm{(s)}-\mathbf{v}'_{n,U}\right\rVert, d^\mathrm{(s)}_\mathrm{max} \right) , \label{eq:Cinterval} \\ \end{align}

\begin{align} &\xi_\mathrm{interval}\left(d_\mathrm{interval}, d^\mathrm{(s)}_\mathrm{max}\right) \nonumber \\ &= \begin{cases} \omega_\mathrm{interval}d_\mathrm{interval}^2 & \text{if $d_\mathrm{interval}\leq d^\mathrm{(s)}_\mathrm{max}$} , \\ \omega_\mathrm{penalty}d_\mathrm{interval}^2 & \text{if $d_\mathrm{interval}> d^\mathrm{(s)}_\mathrm{max}$} , \end{cases} \label{eq:intervalHelp} \end{align}

where \(\omega_\mathrm{interval}\) denotes the weight coefficient. This sequentially calculates the sum of costs based on the distance between prediction steps. In addition, a penalty coefficient \(\omega_\mathrm{penalty}\) is imposed when the movement distance exceeds \(d^\mathrm{(s)}_\mathrm{max}\).

Third, the cost to follow the pre-planned path is as follows:

\begin{align} & C_\mathrm{waypoint} \left(V'_n,W_{n}^\mathrm{(s)},U, K_\mathrm{pred}^\mathrm{(w)}\right) \nonumber \\ &=\sum_{u=1}^{U}\omega_\mathrm{waypoint}\left\lVert\mathbf{v}'_{n,u}-\mathbf{w}_{n,k_u}^\mathrm{(s)}\right\rVert^2 , \label{eq:Cwaypoint} \end{align}

where \(\omega_\mathrm{waypoint}\) denotes the weight coefficient. This calculates the cost based on the distance from each prediction step to the destination waypoint \(\mathbf{w}_{n,k_u}^\mathrm{(s)}\) in a pre-planned path.

Finally, the cost of preventing separation from the work-UAV is calculated as follows:

\begin{align} &C_\mathrm{relative} \left(V'_n,V^{\mathrm{(w)}},U \right)= \sum_{u=1}^{U}\xi_\mathrm{relative} \left(\mathbf{v}_{u}^\mathrm{(w)},\mathbf{v}'_{n,u} \right), \nonumber \\ \label{eq:Crelative} &\xi_\mathrm{relative}\left(\mathbf{v}_{u}^\mathrm{(w)},\mathbf{v}'_{n,u}\right)\\ \end{align}

\begin{align} &= \begin{cases} \omega_\mathrm{relative}\left\lVert\mathbf{v}_{u}^\mathrm{(w)}-\mathbf{v}'_{n,u}\right\rVert^2 & \text{if observable} , \\ \omega_\mathrm{penalty}\left\lVert\mathbf{v}_{u}^\mathrm{(w)}-\mathbf{v}'_{n,u}\right\rVert^2 & \text{if not observable}, \end{cases} \label{eq:relativeHelp} \end{align}

where \(\omega_\mathrm{relative}\) denotes the weight coefficient. These calculations are based on the relative positions of the support-UAV and work-UAV at each prediction step. Additionally, a penalty coefficient \(\omega_\mathrm{penalty}\) is imposed if the line of sight to the work-UAV is obstructed, as this condition prevents relative-localization.

In Line 2, each MPC generates a movement command \(m_n^\mathrm{(s)}\) for the support-UAV based on the prediction path \(V^{\mathrm{(s)}}_n\). By following this command, the support-UAVs can escape from the multipath-affected space.

5.1. Experiment Condition

The path of the work-UAV simulated the work under a bridge. The path of each support-UAV was generated using the method developed by Kimura et al. ¹⁸ to estimate the position of the work-UAV. The path for each UAV contained 15 waypoints (\(I=15\)), and the simulation began with each UAV at the first waypoint. The task completion time is defined as the moment at which the work-UAV reached its final waypoint \(\mathbf{w}_{I}^\mathrm{(w)}\).

The satellite configuration was based on GPS and GLONASS data at 12:00 (UTC+09:00) on October 1, 2021, above Nagoya. The red voxels represent areas with poor GNSS-localizability calculated from the structural shapes and satellite configurations, which are known to UAVs. Framed voxels indicate multipath-affected spaces that are unpredictable to UAVs. For short-duration tasks, we assumed that multipath effects were spatially constant.

The parameter settings of the GNSS-localization model and relative-localization are listed in Table 1. The parameters for GNSS-localization noise were set to a magnitude of approximately 0.15 m. This level of accuracy can be achieved using the RTK-GNSS. The parameters for the relative-localization noise were empirically determined based on the hardware characteristics and prior work by Kimura et al. ¹⁸, assuming a monocular camera. Specifically, the model assumed an error of several tens of centimeters at a distance of 10 m between the work-UAV and the support-UAV. The errors caused by multipath were modeled as an offset in the \(x\)-direction to represent a systematic bias resistant to standard noise reduction techniques such as averaging. The control parameters are listed in Table 2. \(s_\mathrm{max}^\mathrm{(w)}\) and \(s_\mathrm{max}^\mathrm{(s)}\) were set separately owing to their different roles. The work-UAV was considered to have reached its target waypoint \(\mathbf{w}_{k_\mathrm{now}}\) when the condition \(\lVert\mathbf{w}_{k_\mathrm{now}} - \hat{\mathbf{x}}\rVert < 0.1\) m was satisfied. In this study, we used an interior-point algorithm implemented in the MATLAB fmincon function ^a to optimize each \(V^{\mathrm{(s)}}_n\).

Under the conditions described above, we simulated UAV control and position estimation using the proposed method. In this study, we used a version of the proposed method with the \(G\) update function removed for comparison. Hereafter, the proposed method is referred to as “Adaptive-control” (AC), and the method with the update function removed is referred to as “Non-adaptive-control” (NAC). This comparison was performed to evaluate the effect of the proposed GNSS-localizability model update and the corresponding adaptation of the MPC on the robustness of the localization and task execution efficiency. Both control methods were tested 15 times for each scenario.

5.2. Results

Figures 6–9 show the plots of the true and estimated positions and each error between the estimated and true positions. These correspond to trials in which the task completion time was the median for each condition. The “step” on the horizontal axis on localization error plots corresponds to the control cycle time, \(t_\mathrm{cont}\). The red-shaded areas in the localization error plots indicate periods when support-UAV (1) entered the multipath-affected space. In both scenarios, it was confirmed that the work-UAV could complete tasks with both AC and NAC. Therefore, in the following discussion, we focus on whether it is possible to escape from multipath-affected spaces and evaluate efficiency improvements.

First, the trajectory of the UAV was evaluated. In the Along-scenario, the comparison between the NAC (Fig. 6) and the AC (Fig. 7) confirmed that only the support-UAV (1) with AC escaped from the multipath-affected space. The behavior during escape is shown in Fig. 10. When entering a multipath-affected space, UAVs detect a deterioration in the GNSS-localization accuracy. Then, \(G\) is updated, which can be confirmed from the framed voxels; the surrounding voxels are colored in red. Finally, support-UAV (1) escapes. In the Wall-scenario, the comparison between NAC and AC showed little difference in the shape of the trajectory. On the other hand, in the work stage, where support-UAV (1) is expected to wait inside the multipath-affected space when following the pre-planned path, support-UAV (1) with AC escapes from the multipath-affected space, as shown in Fig. 11. This is also considered to be the result of attempts to escape from multipath-affected spaces.

Next, for quantitative analysis, we compared the performance of each method using the statistical results presented in Table 3. To evaluate whether it was possible to escape from multipath-affected spaces, we counted the number of steps taken in the multipath-affected space. The average number of steps spent in the multipath-affected space decreased by 83.7% in the Along-scenario and 79.9% in the Wall-scenario through AC. This difference indicates that the support-UAV can escape from the multipath-affected space. The average completion time was 37.6% shorter in the Along-scenario and 14.3% shorter in the Wall-scenario. This reduction in task completion time is attributed to the improved stability of the work-UAV’s localization. Stable position estimation enables the UAV to efficiently approach and satisfy the waypoint arrival condition, thereby mitigating the impact of erratic movements and delays caused by position fluctuations.

These results demonstrate that the proposed method improves the robustness of cooperative localization systems by escaping multipath-affected spaces, which leads to efficient task execution by the work-UAV.