Abstract
This paper presents a cooperative navigation architecture in a global navigation satellite system (GNSS)-denied subterranean environment using an unmanned ground vehicle (UGV) and unmanned aerial vehicle (UAV) team. The main focus of this design is to prolong the UAV mission time by reducing the UAV payload, sensing, and computational elements. To accomplish this, the UGV handles the mapping of the environment, its own state estimation, and the state estimation of the UAV using the UAV’s proprioceptive sensors, a three-dimensional lidar, and an ultra-wideband ranging radio that communicates with a similar radio on the UAV. The UAV is assumed to be instrumented with an inertial measurement unit, stereo camera, and laser altimeter, and the data from these instruments are shared with the UGV over a local network for use in UAV state estimation. This paper presents the architecture for localization of a UAV/UGV team and realizes the implementation using two different nonlinear state estimators. Details and a comparison between an extended Kalman filter and an incremental factor graph optimization implementation are provided. The performance of the presented algorithms is analyzed via experiments conducted in a motion-capture facility.
1 INTRODUCTION
Robotic systems have many potential applications in subterranean environments, such as exploration and mapping (Dang et al., 2019), search and rescue (Zhao et al., 2017), infrastructure inspection and maintenance (Martinez Rocamora Jr et al., 2023; Szrek et al., 2020), mining and excavation (Marshall et al., 2016), and hazardous material handling (Dekkata et al., 2020). These environments present significant challenges for autonomous navigation, as global navigation satellite system (GNSS) signals are often degraded or absent, lighting is poor, the terrain is typically rough, uneven, and unstructured, and pollutants such as dust and smoke, which can degrade sensor performance, are often encountered (Khattak et al., 2020; Papachristos et al., 2019).
Autonomous navigation in these environments is greatly enhanced when a robot can map and localize itself within the map. Several works have demonstrated that visual–inertial (VI) localization methods can be effectively used in subterranean environments. For example, Li et al. (2020) used a novel VI localization algorithm for unmanned aerial vehicle (UAV) localization in dynamic underground environments. Their results showed that their proposed method can improve localization accuracy by more than 67% in these environments as compared with monocular VI methods. Further, Weiss et al. (2012) proposed an onboard navigation algorithm for micro aerial vehicles integrating a single camera and an inertial measurement unit (IMU), enabling real-time speed estimation and full self-calibration within an extended Kalman filter (EKF) framework and thus facilitating robust six-degree-of-freedom pose estimation and rapid speed control. In addition, Shen et al. (2015) introduced a monocular VI navigation system for autonomous quad-rotors, utilizing an off-the-shelf camera and an IMU; this approach enables robust state estimation for executing trajectories at high speeds and angles alongside innovative methods for motion estimation without initialization and real-time scale determination without encountering degeneracy. Kimera VI odometry (VIO) is a popular VIO package that uses pre-integrated IMU data and random sample consensus for outlier rejection on the front-end and a fixed-lag smoother on the back-end optimization (Rosinol et al., 2020). While VI methods can yield high localization accuracies, the perceptually degraded nature of subterranean environments makes mapping difficult. Consequently, although the long, featureless corridors in such environments can cause drift in lidar-based mapping, three-dimensional (3D) lidar mapping is often the better choice for simultaneous localization and mapping (SLAM) (Ren et al., 2019).
Several authors have realized the benefit of heterogeneous robots for various applications, such as construction monitoring (Asadi et al., 2020), stone mine exploration and mapping (Martinez Rocamora Jr et al., 2023), and target detection (Minaeian et al., 2015), amongst others. These applications typically leverage the higher-fidelity sensing of an unmanned ground vehicle (UGV) and the greater mobility of a UAV in harsh terrains to improve overall situational awareness and operations. These systems can also enhance the overall localization. For example, Sivaneri and Gross (2017) and, more recently, Wang et al. (2021) demonstrated the benefits of UAVs for assisting UGV localization in GNSS-degraded environments, such as urban canyons. These cooperative UAV/UGV localization solutions with ultra-wideband (UWB) ranging solutions have been demonstrated in field tests in areas such as GNSS-degraded urban environments and provide an alternative solution when GNSS alone is not sufficient (Xianjia et al., 2021). Another line of research has considered cooperative mapping with heterogeneous teams. For example, Hood et al. (2017) considered indoor exploration in which a UAV followed a UGV to provide a bird‘s-eye view while each platform performed SLAM, and cooperative localization was employed to refine state estimations. Similarly, Kim et al. (2019) showed the benefit of having a UAV develop an initial model to facilitate route planning for the UGV to collect data for a more detailed map. Li et al. (2021) considered the application of smart cities and used UAV and UGV mapping to develop a 3D occupancy grid for traversability mapping.
Our work considers approaches for cases in which GNSS is completely denied. Here, the UAV and UGV work together so that the heterogeneous team has different sensing and computational capabilities. In particular, to reduce its payload and complexity, the UAV does not have a mapping capability; thus, cooperative localization via teaming is required, where the UAV and UGV benefit from their partner’s strength.
This paper builds on our previous works (De Petrillo et al., 2021, 2023; Gross et al., 2019) with multiple significant differences. First, in all of our prior papers, the presented UAV localization algorithm was constrained to the assumption that either the UAV or the UGV is in motion, but not both at the same time. In particular, while the UAV was in motion and the UAV localization was estimated, the UGV was assumed to be static. This assumption was made to simplify the problem by reducing the observation models needed for ranging and visual-sensor-based tracking. In this paper, we remove this constraint and estimate the UAV location when both vehicles are in motion. Second, in this paper, in addition to presenting an error-state EKF localization formulation, we also re-formulate the problem using factor graph optimization (FGO) and present a comparison between using an EKF or incremental FGO for the nonlinear estimator in UAV localization. The contributions of this paper include a novel UAV–UGV teaming and relative localization formulation and an experimental comparison between two popular nonlinear estimation approaches, namely, an error-state EKF and incremental FGO. In particular, insights are drawn from experiments conducted in a large motion-capture facility, which provides localization ground truths for both the UAV and the UGV.
2 SYSTEM DESIGN
Several sensing modalities were used to estimate the UAV state, as shown in Table 1. First, in addition to the IMU in the UAV’s flight controller, the UAV had a laser altimeter mounted as part of the sensor payload. Second, a UWB radio, DWM-1001, was mounted on the UAV, and a companion UWB was mounted on the UGV to offer ranging measurements between the UAV and UGV. A stereo camera connected to the UAV’s onboard computer was used for velocity estimation.
Summary of Sensors
Figure 1 presents a photograph of the UAV with its sensors. The UAV platform was a Tarot 650 drone equipped with a single board computer and some minimal sensors. A SECO Intel Celeron N3350 Core Single Board Computer coupled with a PixHawk 4 autopilot with an integrated IMU served as the UAV’s onboard autopilot and computer. A LIDAR-Lite single-beam laser altimeter was integrated into the bottom of the UAV, which also carries a UWB radio to provide a direct ranging measurement to the UGV. An Intel RealSense T265 camera was incorporated into the UAV for use as a body-frame velocity sensor on the UAV. To facilitate communication over a local network, the UAV and UGV were Wi-Fi-enabled. As a result, a single instance of the robot operating system (ROS) core running on the computer of the UGV allowed both the UAV and UGV to access sensor messages. With sharing of the IMU and laser-altimeter data from the autopilot, it was possible to control the UAV from the UGV and support relative localization.
Quad-rotor UAV for experimental evaluation (West Virginia University [WVU] photo)
The UGV used for the experiments in this work was equipped with an array of sensors to support 3D SLAM and to estimate the location of a multi-rotor UAV. In particular, as shown in Figure 2, a Clearpath Robotics Husky UGV platform (Clearpath, 2023) served as the main drive chassis for the UGV. The UGV was equipped with a custom computer box and sensor mounting stack constructed from a T-bar as well as a 200-m-range Ouster OS1 64-channel 3D lidar with a 45° vertical field of view ( + 22.5° to −22.5° ) and 360° horizontal field of view. The UGV was also equipped with a tactical-grade micro-electromechanical system (MEMS) IMU (Analog Devices ADIS 16488) to support 3D lidar–inertial SLAM. The main computer was equipped with an i7-7700 processor, and the ROS framework (Quigley et al., 2009) served as the foundation for all software running on the UGV.
Sensor setup for a subterranean UGV called Badger (WVU photo)
3 FORMULATION OF THE NAVIGATION ALGORITHM
For our UAV/UGV collaboration, deviating from our past related work (De Petrillo et al., 2021, 2023; Gross et al., 2019), both the UAV and UGV are assumed to potentially be in motion. While the UGV navigates and maps the environment, the UAV provides an aerial viewpoint ideal for locating potential targets. The UAV can use the map generated by the UGV for path planning. The UGV generates a 3D SLAM solution as it traverses the environment using the lidar–inertial odometry package FAST-LIO (Xu & Zhang, 2021). Other state-of-the-art SLAM packages such as LOAM (Zhang & Singh, 2014) and LIO-SAM (Shan et al., 2020) could also potentially have been used without issue. However, the primary focus of this paper is UAV localization within a local frame of reference, with no contributions to UGV SLAM technology.
In this work, to expand upon our prior work and offer additional improvements, we implement and compare two different nonlinear state estimation frameworks, namely, an error-state EKF and an incremental FGO. Although the EKF is relatively easier to implement and is usually more computationally efficient, it often requires more extensive tuning and can easily be derailed by degraded measurements (Hu et al., 2024; Wen et al., 2021). Compared with the EKF, FGO has a few advantages. First, unlike a standard Kalman filter, which iterates only once for each state, the FGO uses multiple iterations to minimize cost. Second, in contrast to the single linearization step of the standard Kalman filter, the FGO also linearizes the nonlinear measurement model at each iteration step for each state. Additionally, it has been demonstrated that factor graphs more effectively utilize the temporal correlation between earlier and more recent epochs, which has been attributed to the batch nature of the estimation method (Das et al., 2021). Given that some measurement updates that depend on the UGV localization will be error-prone because of errors in the UGV pose estimate and that the lidar position update can be an outlier as well as highly nonlinear because of the short-distance ranging measurements, we expect that a comparison will help determine which approach is better suited for our application. More details regarding the benefits of FGO have been detailed in a recent tutorial article (Taylor & Gross, 2024). Figures 3 and 5 show summaries of these two approaches. To predict the navigation state, the UAV’s onboard inertial navigation is integrated, and four different asynchronous measurement sources are available for sensor fusion within the EKF/incremental FGO to determine the UAV’s full state.
Block diagram of the GNSS-denied UAV–UGV navigation system
Flow chart for object tracking and detection with 3D lidar data
Incremental FGO for the GNSS-denied UAV–UGV navigation system
3.1 Error-State EKF Algorithm
Using a formulation similar to that in our previous work (Gross et al., 2019) and building upon it to allow UGV motion, the attitude, position, and velocity of the UAV in a local north–east–down (NED) navigation frame, with its origin centered at the UGV’s starting location, comprise the state vector of interest, as shown in Equation (1), where we have the following:
(vN, vE, vD) represents the UAV velocity, and
(rN, rE, rD) represents the UAV position.
As shown in Equation (2), the error-state EKF estimates the errors of the state vector of interest as small deviations (δ), as well as the IMU accelerometer and rate gyroscope biases (ba and bg), where
The states and error states in Equations (1) and (2) were predicted within the error-state EKF at a high rate (50 Hz) using inertial navigation and were then updated as new measurements became available at different rates. The available measurements included data from a laser altimeter on the UAV publishing at 10 Hz, lidar-based tracking of the UAV from the UGV at 10 Hz, stereo-camera-based measurements of the UAV’s body-axis velocity at 40 Hz, and UWB ranging measurements between the UAV and UGV publishing at 10 Hz.
By describing the nonlinear state updates and linearized error-state transition model for the inertial navigation system (INS), as well as the observation models and observation sensitivity matrices for each measurement update source, the subsections below detail the specific formulations adopted for each of the primary navigation information sources used for fusion in the EKF.
3.1.1 UAV Inertial Navigation
Because a MEMS IMU is used, the INS models were simplified by omitting the effects of the Earth’s rotation and craft-rate terms (e.g., assuming that the partial derivatives are zero), and we applied the NED navigation frame described by Groves (2015). The localization accuracy is not significantly impacted by these contributions because our UAV had relatively low velocities in our local frame and these signals were imperceptible relative to the stochastic errors of our IMU’s sensor-grade. Further, these models require a knowledge of the platform location on the surface of the Earth, which is a limited assumption for a GNSS-denied application. The UAV attitude is predicted using Equation (3):
where ϵATT is the attitude integration process noise,
The velocity update then converts the specific force measurements from the IMU’s body frame to a local navigation frame, accounts for the acceleration due to gravity, and integrates over time, disregarding Coriolis terms as a further simplification, as shown in Equation (5):
where ϵvel is the velocity integration process noise. The position estimates are updated by simply integrating the velocity estimates using trapezoidal integration to reduce integration errors over the integration interval:
where ϵPOS denotes the noise in the assumed position integration process. The IMU sensor biases are all updated, assuming random walk bias dynamics as in Equation (7):
Note that for various classes of IMUs, a more rigorous approach for bias and noise stochastic modeling could have been applied by characterizing the bias noise and bias instability characteristics of each sensor via Allan variance analysis (El-Sheimy et al., 2007). In particular, the parameters of a first-order Gauss–Markov stochastic model that represents the non-infinite time correlation of the bias dynamics would be more accurate than the infinite time correlation utilized for the random walk in our formulation. However, given the short time span of our experimental UAV flights, we elected to use a simple random walk formulation. Equation (8) provides the linearized error-state dynamics:
where Λ denotes a vector’s skew-symmetric matrix. Equation (9) provides the error-state transition matrix expanded to the first order:
3.1.2 Laser-Altimeter Measurement Update
A local Wi-Fi network was used to transmit data from a laser altimeter mounted on the bottom of the UAV, close to its center, to the UGV. Wi-Fi communication latency is assumed to be negligible for this application, as our test environment is relatively small. We acknowledge that this is a limitation of the current work but leave this issue to future work. When the UAV has zero pitch and roll attitude, altimeter measurements are taken to equal the height of the UAV above the ground, assuming that the environment has a flat, level surface below the UAV and UGV. This assumption is valid for our test environment; however, a changing topography would lead to systematic errors. This measurement scales with the orientation of the UAV, as shown in Equation (10), whose observation model is provided in Equation (11) under the assumption that the pitch and roll angles are relatively small (i.e., no singularities):
It was assumed that the measurement error covariance of the altimeter measurement was
3.1.3 Lidar UAV Position Measurement Update
The lidar sensor generates a point cloud at a rate of approximately 10 Hz, P = {pi ∈ R3 for i = 1, 2, …}, where each point pi = {xi, yi, zi} represents the relative location from the sensor reference frame to the surface at which the point was sampled. The 3D lidar data are used to constrain the relative position estimate between the UAV and UGV. A sparse representation of the area around the sensor is given by the collection of these points, P. As a result, identifying the points in the point cloud from which the UAV has reflected provides the UAV’s precise position. The rolling shutter effect of the 10-Hz sampled spinning lidar sensor was not considered in this work.
Figure 4 shows the algorithm for detecting the UAV within the point cloud. A torus is formed from the point cloud using the UWB and altimeter measurements. In this way, many potential false UAV detections are removed, and the computation required to process the point cloud is reduced. Using the open-source Point Cloud Library (Rusu & Cousins, 2011), a Euclidean clustering algorithm (Cao et al., 2022; Wei et al., 2019) is applied to the segmented point cloud. This clustering algorithm can potentially identify several clusters in each point cloud. Therefore, to further identify which candidate clusters can represent the UAV, the candidate clusters are assessed against a set of heuristics that serve to reduce the potential for false positives (i.e., a simple decision tree).
The following heuristics were utilized to identify a cluster as a UAV:
A minimum volume constraint regarding the extremities-defined bounding box of the cluster,
Constraints on the shape of the bounding box that encapsulates the cluster, for example, specifying the permitted variation in the length, width, and height of the bounding box,
A requirement that the point cloud height be within the manually adjustable threshold of the UAV onboard laser altimeter, and
A constraint that the detected cluster be close to the predicted UAV position, assuming a reasonable UAV velocity between detections.
Empirically tuning the thresholds for these heuristics allowed us to reduce the number of clusters incorrectly reported as UAV locations based on visual inspection. Based on the size and shape of the UAV, these thresholds will need to be updated. Once a particular cluster satisfies the set of heuristics, its centroid is reported as a UAV position estimate in the sensor frame of the UGV lidar
It was assumed that the lidar-position-updated measurement error covariance was
3.1.4 UWB Ranging Radio Measurement Update
A 10-Hz ranging measurement is provided by the pair of ranging radios on the UAV and UGV. This range is used to aid in segmenting the lidar point cloud, as discussed in Section 3.1.3. In the error-state EKF, this measurement is also fused as an observation. Equation (15) models the range measurement, Equation (16) models the predicted measurement, and Equation (17) provides the observation sensitivity matrix. Accounting for the different frames of the UGV, the UGV pose is used, with the range between the UAV and UGV, to update the UAV pose:
Here, it is assumed that the measurement error covariance of the UWB radio is
3.1.5 UAV Velocity Update
An Intel RealSense T265 camera provided UAV velocity measurements directly in the body frame, which were then fused in the error-state EKF. The models for the velocity measurement and the observation matrix are shown in Equations (18) and (19), respectively:
It is assumed that the measurement error covariance of the velocity measurement is
3.2 Incremental Factor Graph Algorithm
FGO (Dellaert et al., 2017) is an estimation framework that utilizes the local structure of measurements and states. For example, range measurements depend only on UGV and UAV state nodes corresponding to a specific time, and visual odometry measurements depend only on UAV nodes i and i + 1. This locality helps factorize the joint distribution of all states of the trajectory into a product of local factors Ψ. Because the marginal distribution of measurements is independent of the states, the posterior of the states is proportional to the product of the factors. Maximizing this product leads to the maximum a posteriori (MAP) estimate of the states:
This maximization problem can be converted into a minimization problem by using a negative logarithm, resulting in a batch nonlinear least-squares problem that can be efficiently solved by existing open-source solvers such as GTSAM (Dellaert, 2012).
The UAV state estimation problem was formulated as an FGO that incrementally smooths the state trajectory over time. The optimization estimates a navigation state Xi comprising the drone’s 3D pose, velocity, and IMU biases at each time step i. The factor graph structure incorporates inertial and visual–range sensor measurements. Initially, the prior factor for the first state node is based on the provided initialization parameters. For subsequent states, prior factors come from IMU propagation or lidar and UWB ranging measurements, when available. At each time step, a new IMU factor is added between consecutive state nodes to constrain the motion predicted by pre-integrating inertial measurements (Forster et al., 2015). Additional measurement factors are incorporated to correct the current state estimate when new visual or range observations are received.
These measurement factors include a prior factor (Ψprior) initialized with velocity prior information obtained from in-built visual odometry from the T265 camera and a position prior from lidar segmentation achieved by the UGV, as shown in Section 3.1.3. Measurements from the altimeter are used as the prior for the elevation factor (Ψelevation), which constrains the z-position at all states. An additional constraint is utilized by adding a range factor (Ψrange) between the UAV and the UGV, whose state is added as a node in the graph with its prior set from the FAST-LIO result. The prior of this factor is obtained from the UWB.
After adding new factors, the incremental FGO computes the MAP estimate of the total state trajectory. Incrementally smoothing the trajectory with new constraints improves the state estimates over time. The optimization integrates IMU propagation with intermittent visual and range updates for accurate and robust state estimation. The formulation is shown in Equation (21):
4 EXPERIMENTAL SETUP
Multiple experimental field tests consisting of simultaneous, autonomous UAV flights and UGV traversals were conducted to assess the performance of the proposed GNSS-denied UAV–UGV navigation system. These tests were conducted inside a large motion-capture facility, shown in Figure 6, whose dimensions were approximately 10 m by 5 m. The room was outfitted with 30 VICON Vantage V5 high-resolution cameras, each with a resolution of 5 megapixels, to capture motion data. Feedback from the VICON motion-capture cameras was used for real-time control. This feedback had a high frame rate of approximately 400 frames per second, allowing precise motion tracking.
Motion-capture facility with the UAV and UGV navigating autonomously during the experimental test (WVU photo)
The trajectory waypoints, as shown in Figure 11, traversed by both the UGV and UAV were pre-planned before the experiment commenced. Further, the real-time estimate of the UAV pose that was used for waypoint following was obtained from the VICON motion-tracking system. These pose estimates were obtained by attaching six reflective markers to the body of the UAV and calculating the geometric center from the constellation of markers. The flight controller onboard the UAV used the VICON pose data to fly the UAV across the planned waypoints autonomously. Therefore, the UAV position estimates from our EKF and FGO solutions were not used in a closed loop, and the experiments only serve to evaluate the accuracy of the localization approach, not its use for UAV control. Unlike the UAV, the UGV used the VICON motion data only in post-processing to assess the efficacy of the motion controller, which uses pose estimates from the implemented FAST-LIO algorithm (Xu & Zhang, 2021).
To reduce the potential of accidental collisions between the vehicles, the UAV was flown above the UGV at a fixed altitude of 2 m, ensuring that both vehicles were operating in the same test environment. During each test, the UGV followed the predetermined driving trajectory in an 8-shaped pattern across the floor. This specific trajectory was chosen so that the UAV would come into the field of view of the lidar sensor mounted on the UGV platform multiple times during the experiment. With a total test time of approximately 150 s, all of the data collected during the two experimental tests were then post-processed for analysis by a replay of the data stored in ROS bag files.
5 RESULTS
5.1 SLAM Solution
The accuracy of the UGV pose estimation achieved by the FAST-LIO SLAM algorithm (Xu & Zhang, 2021) used by the UGV is examined first. A 3D position root-mean square (RMS) error of 0.4 m was obtained from the UGV pose estimation. Figure 7 compares the estimated UGV position to the ground truth determined by our motion-capture system. The error estimates for the SLAM solution are highlighted in Table 2. The accuracy of the UAV localization estimation is directly affected by the accuracy of the UGV pose estimation. Inaccuracies in the UGV pose lead to erroneous measurement updates because the lidar and UWB measurement updates rely on accurate UGV localization. The RMS position and attitude errors in Table 2 are reasonably small and were deemed sufficient for this work. While improving UGV SLAM is not the focus of this work, we acknowledge that other implementations, such as LIO-SAM (Shan et al., 2020; You et al., 2023) or R-LIO (Chen et al., 2022), have been compared with the adopted FAST-LIO formulation and have demonstrated potential for performance increases. There was a notable peak in the 3D position error, approaching almost 1 m at one point in the experiment. This event would result in an extremely erroneous lidar and UWB measurement update for the UAV in the period around that time in the current implementation. Future work will consider how the UAV information can be fed back to improve the UGV SLAM.
UGV SLAM position estimation compared with the ground truth position
SLAM Error Estimates of the UGV State
Runtimes for FGO in Experiments 1 and 2
5.2 UAV Localization Results
The results of our EKF and incremental FGO are presented in this section. Experimental testing was conducted to determine how well the estimation algorithms performed; the results for two flight trajectories are outlined below.
To improve UAV pose estimation, we systematically assessed the effects of adding various updates to the filter by post-processing the recorded data and excluding or including the use of various sensor information in each of the estimation algorithms. At first, only inertial navigation and lidar position updates were used. Figure 8 compares the ground truth from the motion-capture solution and lidar position updates for one experiment, and Figure 9 shows a snapshot of a lidar position detection from a point cloud (blue box). However, when only lidar updates with inertial navigation were used, the resulting filter estimate was poor, owing to the periods of drift during the periods in which there were not many lidar updates. Additionally, frame transformation errors and SLAM localization errors contributed to errors in the lidar position measurements. Because the east and down positions of the UGV, as well as the roll and pitch attitudes, have the largest errors, the lidar position measurement errors for the east and down axes are the largest.
Lidar position measurement updates compared with the ground truth position
Example of lidar position detection (blue box) and EKF estimate (red box) in a 3D point cloud (WVU data)
As shown in Figure 8, the UAV is not consistently tracked during the entire experiment. This result arises because the UAV was outside the lidar’s field of view for portions of the experiments and because the heuristics used to identify the UAV, as detailed above, are conservative to avoid falsely identifying another object as the UAV. To improve UAV detection via lidar, research is being conducted in parallel (Olawoye & Gross, 2023) to complement the scope of this paper.
In addition to lidar position updates, the analysis was expanded to include UWB updates. The results show that the UWB updates reduced the 3D localization RMS error to approximately 12 m. The error was further reduced by including the altimeter updates, which, together with the UAV velocity updates, brought the error to less than 1 m. Figure 10 compares the estimated NED position from our filter/incremental FGO with the ground truth for the first experiment, Figure 11 compares the estimate with a 3D plot for the first experiment, and Figure 12 compares the estimated attitude from our filter/incremental FGO with the ground truth for the first experiment. Figures 13 and 14 show comparison plots for the second experiment. For the second experiment, despite being shorter, we observe consistent trends, as reflected in the comparable patterns of RMS errors observed for both trajectories. These results indicate that the incremental FGO solution outperforms the EKF, particularly when there are fewer measurement updates. The error estimates for the FGO solution are significantly smaller than those of the EKF up until the addition of the velocity updates. When lidar, UWB, and altimeter updates are included, the EKF has position errors that are up to five-fold larger than the FGO errors, and for most of the experiment, the FGO solution is smoother. The FGO solution also appears to recover more quickly from poor measurement updates. With all of the measurement updates, both approaches had similar performances, with the EKF performing better in some metrics. Considering the simplicity and lower computational overhead of the error-state EKF compared with the FGO, with sufficient measurement updates and more careful tuning, the EFK is probably the more practical approach. Yet, the incremental FGO approach is a better choice when measurement updates are limited.
EKF/FGO position compared with the ground truth position (Experiment 1)
3D plot of EKF/FGO position compared with the ground truth position (Experiment 1)
EKF/FGO attitude compared with the ground truth attitude (Experiment 1)
EKF/FGO position compared with the ground truth position (Experiment 2)
EKF/FGO attitude compared with the ground truth attitude (Experiment 2)
Tables 4 and 6 show the RMS errors for the two flight trajectories using different combinations of measurement updates. In the second experiment, the UAV was outside the UGV lidar’s field of view for almost the entire trajectory; thus, there was an insignificant number of lidar updates. Estimation for this experiment was conducted with the other measurement updates, with the RMS error shown in Table 6. For the first trajectory, lidar was also only able to offer updates during the short period in which the UAV was in the UGV lidar’s field of view; we feel that comparing these results with those for the second flight (with no lidar) provides insight into the importance of these updates. As shown in both tables, the error decreased with each additional update. The UWB updates provided the most significant improvement on the NED axes for both the EKF and the FGO. The FGO appears to better use the UWB updates to improve its attitude estimation, as shown by the reduction in the roll and pitch RMS errors of the FGO. The addition of the altimeter updates provided only a slight improvement for most states, but a significant improvement in the down-axis estimate. This result is due to the fact that the altimeter information primarily constrains only one state (position in the down-axis). Adding velocity updates significantly improved performance for all states of interest because velocity information constrains the position in all axes. Tables 5 and 7 show the maximum error for each axis across both flight trajectories. The maximum error follows trends similar to those of the RMS errors. The EKF attitude estimates were much better in the second experiment than in the first, likely because the flight experiment was shorter, such that the gyroscope bias did not lead to as much attitude error as in the first experiment. Smaller attitude errors translate to smaller position errors, as attitude and position are strongly correlated in inertial navigation. This trend can be seen in the “IMU Only” column of Table 6, where the position error is much smaller than the corresponding field in Table 4. The benefit of lidar updates can be seen by comparing the FGO position RMS errors for the two trajectories. The FGO position errors are notably larger in the second experiment than in the first for all measurement update scenarios. Tables 4 and 6 show that lidar updates benefit both estimation methods, but the FGO, again, seems better able to utilize the information from lidar updates simply because it serves as a batch estimator over the entire time history rather than acting as a sequential estimator.
EKF/FGO RMS Error (Experiment 1)
EKF/FGO Maximum Error (Experiment 1)
EKF/FGO RMS Error (Experiment 2)
EKF/FGO Maximum Error (Experiment 2)
Our analysis further investigates the runtime of the FGO algorithm for both flight trajectories, unveiling insights into its computational characteristics across varying graph sizes. Notably, we observe a consistent linear growth in computational time as the size of the FGO increases, as shown in Figure 15. This linear relationship underscores the scalability of FGO for this particular problem formulation, indicating that its computational complexity increases proportionally with the size of the graph. This result is expected, as the computational overhead of propagating messages over dense or large graphs amplifies the runtime. At the same time, the order in which messages are passed can also impact convergence speed and overall runtime performance. This observed relationship is consistent with other runs in our scenario, as shown in Table 3.
Graph size versus runtime for the FGO algorithm
6 CONCLUSION
In this work, an approach for cooperative localization of a UAV using information from a UGV in GNSS-denied subterranean environments was designed using multiple nonlinear estimators, implemented using real sensors and communication, and assessed experimentally. Experiments were conducted in a motion-capture room to assess the system’s performance while both the UAV and UGV were in autonomous motion to estimate the UAV pose. A teaming arrangement such as this would enable the UAV to be deployed by a UGV to facilitate an overall mission goal such as search and rescue.
The UGV, equipped with a 64-channel lidar and tactical-grade IMU, kept track of its pose using the FAST-LIO SLAM algorithm (Xu & Zhang, 2021), with a 3D RMS error of less than 0.5 m. For the UAV pose estimation, we employed an error-state EKF and an incremental FGO, integrating data from IMU, lidar, UWB, altimeter, and velocity measurements. Measurements from the flight controller IMU were noisy and led to considerable INS errors when left unaided. To mitigate this issue, using a higher-grade IMU on the flight controller, similar to the UGV IMU, would be preferable for future applications. However, other measurement updates compensated for the drift of the UAV IMU. The lidar position measurement updates were sometimes biased because of errors in the UGV pose. While our setup demonstrated efficient real-time data transmission between the UAV-mounted laser altimeter and the UGV through Wi-Fi, it is essential to recognize this limitation for applications in which latency is a critical concern, particularly in larger environments or settings with potential for significant delays in communication. Future work could consider incorporating more realistic communication signals, such as 5G, which would have the added benefit of being useful for ranging and communications. By carefully tuning the filter’s system and measurement noise models, the effect of these offsets was reduced in the localization solution. In future works, the pose estimate of the UGV could be improved by using updates from the UAV, such as range measurements, and by taking advantage of pseudo-measurement constraints of the UGV. This approach would make the algorithm truly collaborative. Additional sensor updates progressively improved the UAV pose accuracy, with the FGO approach outperforming the error-state EKF by providing smoother and more accurate pose estimates.
HOW TO CITE THIS ARTICLE
Akhihiero, D., Olawoye, U., Das, S., & Gross, J. (2024). Cooperative localization for GNSS-denied subterranean navigation: A UAV–UGV team approach. NAVIGATION, 71(4). https://doi.org/10.33012/navi.677
CONFLICT OF INTEREST
The authors declare no conflicts of interest.
ACKNOWLEDGMENTS
This research was supported in part by an academic grant from the National Geospatial-Intelligence Agency (NGA) (Award No. HM0476-18-1-2000, Project Title: Autonomous Navigation of Small UAV/UGV Teams in Underground Tunnels). Any opinions, findings, conclusions, or recommendations expressed in this material are those of the authors and do not necessarily reflect the views of the NGA, Department of Defense, or US government. This work has been approved for public release (NGA-U-2024-01756).
This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.
Jump to section
Related Articles
Cited By...
- No citing articles found.