Multi-Sensor-Assisted Navigation for UAVs in Power Inspection: A Fusion Approach Using LiDAR, IMU and GPS

Abstract

High-precision localization is essential for autonomous navigation and environment perception of unmanned aerial vehicles (UAVs) in complex power inspection scenarios. To overcome the limited accuracy and accumulated drift of conventional GPS-based single-sensor localization, this paper proposes a LiDAR–IMU–GPS-aided navigation method that combines a tightly coupled front-end and a loosely coupled back-end. The front-end employs an improved Lie-group-based UKF-SLAM framework to explicitly handle the nonlinearities of rotational motion, thereby improving the stability of local pose estimation. The back-end integrates GPS absolute constraints, loop closure detection, and point cloud registration via pose graph optimization, which effectively suppresses long-term accumulated drift. The framework achieves accurate and robust localization for UAV power inspection. Experiments on public benchmark datasets and real-world power inspection scenarios demonstrate the effectiveness of the proposed method. On the MH_02_easy sequence, the absolute trajectory error is reduced from 0.521 m to 0.170 m compared with ROVIO, while in a real inspection sequence the cumulative error is reduced by more than 99% after back-end optimization. Moreover, the system maintains stable navigation under GPS-degraded conditions, indicating strong robustness and practical applicability.

1. Introduction

High-precision localization is one of the key enabling technologies for autonomous navigation and environment perception of unmanned aerial vehicles (UAVs) in complex environments [1,2,3,4]. In industrial applications such as power line inspection, more stringent requirements are imposed on the robustness, accuracy, and real-time performance of localization systems [5]. Conventional UAV localization solutions mainly rely on the Global Positioning System (GPS) [6]. However, in environments characterized by complex terrain, urban canyons, or severe multipath effects, civilian-grade GPS typically provides positioning accuracy in the range of 2–5 m, and its signals are prone to blockage and interference, making it difficult to meet the centimeter- to decimeter-level accuracy demands of power inspection tasks.

Moreover, localization systems based on a single sensor modality inherently suffer from performance limitations. For example, inertial measurement units (IMUs) are susceptible to error accumulation during long-term operation, while LiDAR-based localization in large-scale or long-range scenarios may experience feature degradation or matching failures, leading to significant localization drift [7,8,9].

In recent years, multi-sensor fusion has emerged as an effective approach to improving the localization accuracy of UAVs [6,10]. By integrating the complementary strengths of multiple sensors such as LiDAR, IMU, and GPS, it becomes possible to achieve continuous and stable high-precision localization even in environments where GPS signals are degraded or unavailable. Among these approaches, simultaneous localization and mapping (SLAM) has been widely recognized as a core methodology for autonomous navigation in unmanned systems and has been extensively applied in fields such as autonomous driving and robotic navigation.

According to different back-end processing paradigms, SLAM methods can generally be categorized into filtering-based approaches and optimization-based approaches. The Extended Kalman Filter (EKF) has been widely adopted in early systems due to its high computational efficiency; however, its reliance on linearization assumptions may introduce significant errors in nonlinear motion models and can even lead to system divergence. In contrast, the Unscented Kalman Filter (UKF) approximates nonlinear distributions through sigma-point sampling, thereby avoiding linearization errors and making it more suitable for handling nonlinearities in attitude estimation [11,12].

In power inspection missions, UAVs are often required to operate in environments with complex structures and strong electromagnetic interference, such as high-voltage transmission lines and substations, which poses significant challenges to the robustness and environmental adaptability of localization systems [13]. Existing studies have shown that loosely coupled LiDAR–IMU approaches can achieve relatively stable localization performance; however, many of these systems do not sufficiently incorporate absolute positioning information from GPS, making it difficult to effectively suppress accumulated drift during long-duration missions [14,15].

Furthermore, conventional EKF-based fusion methods are prone to model inconsistency in rotational estimation due to the strong nonlinearity of quaternion representations, which may lead to degradation in estimation accuracy [16,17]. In contrast, UKF methods formulated on Lie groups provide a more principled treatment of three-dimensional rotational kinematics, thereby improving the consistency and reliability of attitude estimation. Additionally, the proposed UKF-SLAM framework improves computational efficiency and robustness by directly handling rotational motion on the SE(3) manifold, avoiding the issues of quaternion-based methods. This formulation enables more accurate state estimation in highly nonlinear environments, such as power inspection scenarios, where accurate pose and motion tracking are critical for UAV navigation.

To address the localization requirements of UAV-based power inspection, this paper proposes a high-precision aided navigation system based on loosely coupled multi-sensor fusion. The system adopts a three-sensor integration architecture comprising LiDAR, IMU, and GPS. In the front-end, loosely coupled LiDAR–IMU fusion is employed to achieve locally accurate pose estimation, while in the back-end, a pose graph optimization framework is introduced to construct a globally consistent trajectory by jointly incorporating GPS absolute position constraints and loop closure information.

At the algorithmic level, this paper develops an improved UKF-SLAM approach formulated on Lie group operations. By modeling rotational kinematics on Lie groups, the proposed method avoids linearization errors associated with quaternion representations and enhances the stability of state estimation in dynamic environments.

It is worthwhile to highlight several aspects of the contributions here:

• We propose a navigation architecture for power inspection that combines a tightly coupled LiDAR-IMU front-end (UKF-SLAM on Lie groups) with a loosely coupled back-end (GPS and loop closure pose graph optimization), achieving both high local accuracy and global consistency;
• We design a UKF-SLAM algorithm formulated on Lie group operations to enhance the mathematical rigor and numerical stability of rotational estimation in the front-end;
• We construct a pose graph optimization model incorporating GPS constraints, loop closure detection, and point cloud registration, which effectively suppresses long-term accumulated drift in a loosely coupled manner.

This paper is organized as follows. Section 2 presents an aided localization method based on UKF filtering for LiDAR–IMU fusion. Section 3 describes an aided localization approach based on pose graph optimization for LiDAR–GPS fusion. Section 4 details the experimental setup and provides a comprehensive analysis of the results. Finally, Section 5 concludes the paper.

2. An Aided Localization Method Based on UKF Filtering for LiDAR–IMU Fusion

In this section, we present the tightly coupled LiDAR-IMU fusion front-end based on an improved UKF-SLAM formulated on Lie groups. This front-end processes raw IMU and LiDAR data jointly to provide high-frequency, locally consistent pose estimates. The Extended Kalman Filter (EKF) performs well in many motion estimation tasks; however, its reliance on linearization can introduce significant approximation errors, which may lead to system divergence in severe cases [18]. In contrast, the Unscented Kalman Filter (UKF) approximates the state distribution through a set of sampled sigma points, thereby avoiding direct linearization of nonlinear models.

Moreover, rotational motion is commonly represented using quaternions, which further increases the nonlinearity of the system. To address this issue, Lie group operations can be employed to map quaternion representations to three-dimensional vector spaces, thereby improving the accuracy and stability of state estimation.

2.1. Fusion Algorithm Design Workflow

2.1.1. Introduction to Data Fusion Strategy

Compared with single-sensor systems, the fusion of heterogeneous sensors can provide richer information representation, higher accuracy, improved measurement precision, and more comprehensive system performance. By evaluating and comparing the performance of single-sensor systems and multi-sensor fusion systems across different application scenarios, it can be clearly observed that multi-sensor fusion approaches exhibit overall significant advantages. The corresponding comparative results are illustrated in the Figure 1 below.

Front-end information fusion methods can be categorized into four types: information fusion strategies based on distinguishable units, fusion strategies based on feature complementarity, fusion strategies based on target attributes from different sensors, and fusion strategies based on decision-level outputs from different sensors. A detailed analysis of these strategies is presented below.

(1)

Information Fusion Strategy Based on Distinguishable Units

FSBDU refers to a fusion process in which data from distinguishable units of different sensors are directly integrated, followed by further processing of the fused data.

(2)

Information Fusion Strategy Based on Feature Complementarity

FSBCF integrates multiple target features extracted from the corresponding sensor data. Heterogeneous sensors capture complementary features of the same target at different and often uncorrelated scales, which are then fused and utilized for subsequent classification and recognition tasks.

(3)

Fusion Strategy Based on Target Attributes from Different Sensors

FSBTA is a distributed data processing approach in which each sensor extracts target parameters and identifies targets independently to generate a target list. Multiple target lists are then fused to obtain reliable and accurate target information, thereby reducing false alarms and missed detections.

(4)

Fusion Strategy Based on Decisions from Different Sensors

FSBMD performs preliminary decisions on target location, attributes, and classification using data from individual sensors, and then comprehensively combines the decisions obtained from multiple sensors through dedicated fusion strategies to produce the final fused result. FSBMD can generally be categorized into decision fusion, decision making, confidence-based fusion, and probabilistic fusion.

2.1.2. Study on Alignment-Based Fusion Strategies

(1)

Temporal Alignment Algorithm

To address the inconsistency in output frequencies among sensors such as LiDAR (10 Hz), IMU (100 Hz), and GPS (1 Hz), an appropriate time synchronization scheme must be designed to enable effective multi-source data fusion [19,20,21]. Because errors in time alignment can lead to inaccurate position estimates. When the timestamps of different sensors are not synchronized, the sensor measurements may no longer correspond to the state at the same moment. For example, LiDAR and IMU may observe the same object at the same time, but due to differences in sampling times, the data points used in the fusion algorithm may not match spatially, which in turn affects the final state estimation [22]. In this study, a timestamp-based sorting method is adopted, as shown in Figure 2 below. This approach primarily utilizes software-level interpolation to perform temporal alignment and synchronization of sensor measurements with different sampling rates. This strategy ensures temporal consistency across data streams and provides reliable inputs for subsequent fusion algorithms.

(2)

Spatial Alignment Algorithm

The proposed method first estimates an initial pose state of the system motion and defines a GPS-based error bound, which is then used as a robust kernel to suppress noisy observations, thereby obtaining an initial fused pose (Initial Pose). Subsequently, trajectory interpolation information generated from IMU pre-integration is incorporated, and the system pose state is further refined through a loosely coupled optimization framework. Finally, high-precision pose estimates are produced as the measurement output. The relevant process is shown in Figure 3 below.

2.2. Optimization Process Based on UKF-SLAM

2.2.1. State Vector and System Model

Assuming that the IMU biases follow a random walk model and considering that the LiDAR has tracked $n$ landmark points, the system state vector can be defined as follows:

$$\mathsf{\chi }={\left[R^T,{\mathsf{\upsilon }}^T,x^T,b_g^T,b_a^T,p_1^T,\dots ,{p_n}^T\right]}^T$$

(1)

Here, $R\in \mathrm{S}\mathrm{O}\left(3\right)$, $x\in {\mathbb{R}}^3$, and $\upsilon \in {\mathbb{R}}^3$ denote the orientation, position, and velocity of the platform, respectively. $b_g\in {\mathbb{R}}^3$ and $b_a\in {\mathbb{R}}^3$ represent the biases of the gyroscope (angular velocity) and the accelerometer, respectively. $p_1\dots p_n\in {\mathbb{R}}^3$ denotes the three-dimensional positions of the $n$ LiDAR-tracked landmark points expressed in the global coordinate frame.

Based on the IMU measurements, the continuous-time kinematic model of the system can be expressed as follows:

$$\dot{\mathrm{R}}=\mathrm{R}\left(\mathrm{w}-{\mathrm{b}}_{\mathrm{g}}+{\mathsf{\eta }}_{\mathrm{g}}\right)$$

(2)

$$\dot{\mathrm{\upsilon }}=R\left(a-b_a+{\mathsf{\eta }}_a\right)+g$$

(3)

$$\dot{x}=\upsilon$$

(4)

$$\dot{b_g}={\mathsf{\eta }}_{b_g},\dot{b_a}={\mathsf{\eta }}_{b_a}$$

(5)

$$\dot{p_i}=0,i=1,\dots ,n$$

(6)

The angular velocity $w$ and acceleration $a$ measured by the IMU both contain bias terms and additive Gaussian white noise:

$${\mathsf{\eta }}_B={\left[{{\mathsf{\eta }}_g}^T,{{\mathsf{\eta }}_a}^T,{{\mathsf{\eta }}_{b_g}}^T,{{\mathsf{\eta }}_{b_a}}^T\right]}^T$$

(7)

The observations obtained by the LiDAR consist of a set of tracked landmark points $p_i$. Based on the standard perspective projection model, the observation of the $i$-th landmark, denoted as $y_i$, can be expressed as follows:

$$y_i={\displaystyle \frac{1}{{\mathsf{\omega }}_i}}\left[\begin{array}{c}{u}_i\\ {\mathsf{\upsilon }}_i\end{array}\right]+n_{y_i}$$

(8)

where ${(u_i,{\upsilon }_i,{\omega }_i)}^T$ is computed as follows:

$$\left[\begin{array}{c}{u}_i\\ {\mathsf{\upsilon }}_i\\ {\mathsf{\omega }}_i\end{array}\right]=R_C^{B^T}\left(R_W^{B^T}\left(p_i-x_W^B\right)-x_C^B\right)$$

(9)

In the above formulation, $R_C^B$ and $x_C^B$ denote the rotation and translation from the camera coordinate frame $\left\{C\right\}$ to the body frame $\left\{B\right\}$, respectively, while $R_W^B$ and $x_W^B$ represent the corresponding transformation from the world coordinate frame $\left\{W\right\}$ to the body frame $\left\{B\right\}$. The observation noise $n_{y_i}$ is assumed to follow a zero-mean Gaussian distribution.

2.2.2. UKF-SLAM Based on Lie Group Operations

In this section, a UKF-SLAM system is constructed based on the aforementioned formulations.

(1)

Unscented Kalman Filter (UKF)

The Unscented Kalman Filter (UKF) is a state estimation method designed for nonlinear systems. It approximates the probability distribution of state variables using a set of deterministically sampled sigma points, thereby avoiding explicit linearization of nonlinear models [11,23].

These sigma points are propagated directly through the nonlinear motion equations, and the mean and covariance of the state estimate are reconstructed via weighted summation. This approach maintains estimation accuracy while reducing errors introduced by linearization.

To accommodate the state estimation requirements of the Unscented Kalman Filter (UKF) in nonlinear systems, an augmented state vector is first defined. Let the original state vector be denoted as $\mathsf{\chi }\in {\mathbb{R}}^S$; the augmented state vector and its corresponding covariance matrix can then be constructed as follows:

$${\mathsf{\chi }}_a={\left[{\mathsf{\chi }}^T,{\mathsf{\eta }}_B^T\right]}^T,P_a={\left[\begin{array}{cc}{P}_{S\times S}& 0_{M\times 12}\\ 0_{12\times M}& Q_{12\times 12}\end{array}\right]}_{N\times N}$$

(10)

where ${\eta }_B\in {\mathbb{R}}^{12}$ denotes the system noise vector, and $Q$ is the noise covariance matrix, whose structure is given by:

$$Q={\left[\begin{array}{cccc}{Q}_g& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& Q_a& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& Q_{b_g}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& Q_{b_a}\end{array}\right]}_{12\times 12}$$

(11)

The dimension of the augmented state is $N=S+12$. Based on this, a symmetric sampling strategy is employed to generate $2N+1$ sigma points:

$${\mathsf{\sigma }}_0={\mathsf{\chi }}_a$$

(12)

$${\mathsf{\sigma }}_i={\mathsf{\chi }}_a+{\left[\sqrt{\left(\lambda +N\right)P_a}\right]}_i,i=1,2,\dots ,N$$

(13)

$${\mathsf{\sigma }}_i={\mathsf{\chi }}_a-{\left[\sqrt{\left(\lambda +N\right)P_a}\right]}_i,i=N+1,\dots ,2N$$

(14)

Here, $\lambda$ denotes the scaling parameter, and $\sqrt{P_a}$ represents the matrix square root obtained via Cholesky decomposition. The notation ${\left[\begin{array}{c}·\end{array}\right]}_i$ indicates the extraction of the $i$-th column vector of the matrix.

Within the UKF framework, each sigma point is propagated forward through the system model function $f$. Based on the propagated sigma points, the a priori state estimate and the corresponding covariance matrix can be computed as follows:

$${\widehat{\chi }}_{k|k-1}=\sum _{i=0}^{2N}{w}_{i}f\left({\sigma }_i\right),{\widehat{P}}_{k|k-1}=\sum _{i=0}^{2N}{w}_i\left(f\left({\sigma }_i\right)-{\widehat{\chi }}_{k|k-1}\right){\left(f\left({\sigma }_i\right)-{\widehat{\chi }}_{k|k-1}\right)}^T$$

(15)

where the weighting coefficients are defined as follows:

$$w_0={\displaystyle \frac{\mathsf{\lambda }}{\mathsf{\lambda }+N}},w_i={\displaystyle \frac{\mathsf{\lambda }}{2\left(\mathsf{\lambda }+N\right)}}\left(i=1,\dots ,2N\right)$$

(16)

The scaling parameter is given by $\lambda ={\alpha }^2(N+\beta )-N$. Typically, $\alpha$ is set to ${10}^{-3}$, and $\beta =2$ is chosen when the state is assumed to follow a Gaussian distribution.

Similarly, the predicted measurement and the corresponding covariance can be updated through the measurement model function $h$ as follows:

$${\widehat{z}}_k=\sum _{i=0}^{2N}{w}_{i}h\left({\mathsf{\sigma }}_i\right)$$

(17)

$$P_{xz}=\sum _{i=0}^{2N}{w}_i(f({\sigma }_i)-{\widehat{\chi }}_{k|k-1}){\left(h\left({\sigma }_i\right)-{\widehat{z}}_k\right)}^T$$

(18)

$$P_{zz}=\sum _{i=0}^{2N}{w}_i(h({\sigma }_i)-{\widehat{z}}_k){\left(h\left({\sigma }_i\right)-{\widehat{z}}_k\right)}^T$$

(19)

Based on the previously obtained cross-covariance matrix $P_{xz}$ and the predicted measurement covariance matrix $P_z$, the Kalman gain matrix can be computed as follows:

$$K_k=P_{xz}{P}_z^{-1}$$

(20)

Subsequently, by combining the actual measurement $z_k$ with the predicted measurement ${\widehat{z}}_k$, the posterior state estimate can be updated as follows:

$${\mathsf{\chi }}_k={\widehat{\mathrm{\chi }}}_{k|k-1}+K_k\left(z_k-{\widehat{z}}_k\right)$$

(21)

The corresponding posterior covariance matrix is updated as follows:

$$P_k={\widehat{P}}_{k|k-1}-K_k{P}_z{K}_k^T$$

(22)

In this formulation, ${\widehat{\chi }}_{k\mid k-1}$ and ${\widehat{P}}_{k\mid k-1}$ denote the a priori state estimate and its corresponding covariance matrix, respectively. This update process effectively incorporates measurement information, thereby yielding an optimal estimate of the system state.

(2)

Improved UKF with Lie Group Operations

A Lie group is a class of matrix groups with continuous structure, whose mathematical properties are well suited for describing geometric transformations such as rotations and rigid-body motions [24,25]. An $n$-dimensional Lie group $G$ satisfies the following fundamental properties:

• Closure: $\forall G_1,G_2\in G,G_1\cdot G_2\in G$;
• Associativity: $\forall G_1,G_2,G_3\in G,(G_1\cdot G_2)\cdot G_3=G_1\cdot (G_2\cdot G_3)$;
• Existence of Inverse Elements: $\forall G_1\in G,G_1^{-1}\in G$.

In state estimation, commonly used Lie groups include:

• The three-dimensional rotation group $\mathrm{S}\mathrm{O}\left(3\right)$, which represents rotation matrices $R$;
• The three-dimensional special Euclidean group $\mathrm{S}\mathrm{E}\left(3\right)$ which represents rigid-body transformation matrices:

  $$T=\left[\begin{array}{cc}R& p\\ 0^T& 1\end{array}\right]\in SE\left(3\right)$$

  (23)

  where $p\in {\mathbb{R}}^3$ denotes the translation vector.

Each Lie group $G$ is associated with a corresponding Lie algebra $\mathfrak{g}$, which is a vector space tangent to the identity element. The Lie group and its Lie algebra are related through the exponential map $\mathrm{e}\mathrm{x}\mathrm{p}(\cdot ):\mathfrak{g}\to G$ and the logarithmic map $\mathrm{l}\mathrm{o}\mathrm{g}(\cdot ):G\to \mathfrak{g}$. For the rotation group $\mathrm{S}\mathrm{O}\left(3\right)$, its Lie algebra $\mathfrak{s}\mathfrak{o}\left(3\right)$ consists of all three-dimensional skew-symmetric matrices. Specifically, if $\mathit{\varphi }\in {\mathbb{R}}^3$ denotes a rotation vector, then:

$${\varphi }^{\wedge }=\mathrm{l}\mathrm{o}\mathrm{g}\left(R\right),\varphi ={\mathrm{l}\mathrm{o}\mathrm{g}\left(R\right)}^{\vee },R=\mathrm{e}\mathrm{x}\mathrm{p}\left({\varphi }^{\wedge }\right)$$

(24)

Here, the operator ${\left(\begin{array}{c}·\end{array}\right)}^{\wedge }$ maps a three-dimensional vector to a skew-symmetric matrix, while ${\left(\begin{array}{c}·\end{array}\right)}^{\vee }$ denotes the corresponding inverse mapping.

To extend the Unscented Kalman Filter (UKF) to Lie group spaces, it is necessary to redefine a Gaussian distribution that is compatible with manifolds. A Gaussian distribution defined on a Lie group $G$ can be expressed as follows:

$$\mathsf{\Theta }\left(X\right)=\mathsf{\alpha }exp\left({\displaystyle \frac{1}{2}}{\left[lo{g}_G\left(X^{\mathsf{\upsilon }}\right)\right]}^T{P}^{-1}lo{g}_G\left(x^{\vee }\right)\right)$$

(25)

where ${\mathrm{l}\mathrm{o}\mathrm{g}}_G\left(X^v\right)\in \mathfrak{g}$ denotes the vector in the Lie algebra associated with the Lie group element $X^v$, $P$ is the covariance matrix, and $\alpha$ is a normalization constant.

This distribution is associated with the Lie group through the exponential map as follows:

$$X=\mu \cdot {exp}_G\left(\widehat{\epsilon }\right)$$

(26)

Here, $\mu \in G$ denotes the mean on the Lie group, and $\widehat{\epsilon }\in \mathfrak{g}$ represents a zero-mean Gaussian perturbation in the Lie algebra. Based on this formulation, the prediction and update processes of the Kalman filter can be extended to the $\mathrm{S}\mathrm{E}\left(3\right)$ manifold.

Specifically, the state propagation equation on the $\mathrm{S}\mathrm{E}\left(3\right)$ manifold is given by:

$${\chi }_{SE\left(3\right)|k}=T_C^W\left[{exp}_{SE\left(3\right)}\left({\left[R_W^C{\chi }_{SE\left(3\right)|k-1}\right]}^{\wedge }\right)\right]$$

(27)

Here, $T_C^W\in \mathrm{S}\mathrm{E}\left(3\right)$ denotes the transformation between coordinate frames, and $R_C^W\in \mathrm{S}\mathrm{O}\left(3\right)$ represents the rotational component. Correspondingly, sigma point sampling on the $\mathrm{S}\mathrm{E}\left(3\right)$ manifold is performed as follows:

$${\mathsf{\sigma }}_{SE{\left(3\right)}_0}={\mathsf{\chi }}_{SE\left(3\right)}$$

(28)

$${\sigma }_{{SE\left(3\right)}_i}={\chi }_{SE\left(3\right)}{exp}_{SE\left(3\right)}\left({\left[\sqrt{\left(\lambda +N\right)P_a}\right]}_i\right),i=1,2,\dots ,N$$

(29)

$${\sigma }_{{SE\left(3\right)}_i}={\chi }_{SE\left(3\right)}{exp}_{SE\left(3\right)}\left(-{\left[\sqrt{\left(\lambda +N\right)P_a}\right]}_i\right),i=N+1,\dots ,2N$$

(30)

In the above formulation, $P_a$ denotes the augmented covariance matrix, and $\lambda$ is the scaling parameter. The sampled sigma points are propagated through the system model; however, it should be noted that on the $\mathrm{S}\mathrm{E}\left(3\right)$ manifold, the computation of the mean is no longer a linear weighted sum. Instead, iterative procedures or Riemannian centroid methods must be employed to compute the mean and adjust the covariance on the manifold, in order to preserve the consistency of the estimation.

On the $\mathrm{S}\mathrm{E}\left(3\right)$ manifold, the a priori mean and covariance must be obtained through statistical operations defined on the manifold. The mean ${\widehat{\mu }}_{SE\left(3\right)}$ is defined as the solution to a minimization problem of the weighted sum of squared geodesic distances:

$${\widehat{\mathrm{\mu }}}_{SE\left(3\right)}=\underset{\mathsf{\rho }\in SE\left(3\right)}{\mathrm{argmin}}\sum _{i=0}^{2N}{w}_i{d}^2\left(f\left({\mathsf{\sigma }}_{SE{\left(3\right)}_i}\right),\mathsf{\rho }\right)$$

(31)

where $d(\cdot )$ denotes the geodesic distance on $\mathrm{S}\mathrm{E}\left(3\right)$. The covariance matrix is then computed in the Lie algebra space via the logarithmic map as follows:

$${\widehat{P}}_{SE\left(3\right)}=\sum _{i=0}^{2N}{w}_{i}lo{g}_{{\mathsf{\mu }}_{SE\left(3\right)}}\left(f\left({\mathsf{\sigma }}_{SE{\left(3\right)}_i}\right)\right)lo{g}_{{\mathsf{\mu }}_{SE\left(3\right)}}{\left(f\left({\mathsf{\sigma }}_{SE{\left(3\right)}_i}\right)\right)}^T$$

(32)

Here, ${\mathrm{l}\mathrm{o}\mathrm{g}}_{\mathit{\mu }}\left(X\right)$ denotes the tangent-space vector that maps the Lie group element $X$ to the tangent space at $\mu$.

During the measurement update stage, the predicted measurement mean ${\widehat{z}}_{SE\left(3\right)}$ is obtained in an analogous manner:

$${\widehat{z}}_{SE\left(3\right)}=\underset{\mathsf{\rho }\in SE\left(3\right)}{\mathrm{argmin}}\sum _{i=0}^{2N}{w}_i{d}^2\left(h\left({\mathsf{\sigma }}_{SE{\left(3\right)}_i}\right),\mathsf{\rho }\right)$$

(33)

Subsequently, the cross-covariance matrix between the state and the observation, as well as the observation covariance matrix, are given by:

$$P_{xz|SE\left(3\right)}=\sum _{i=0}^{2N}{w}_{i}lo{g}_{{\widehat{\mathrm{\mu }}}_{SE\left(3\right)}}\left(f\left({\mathsf{\sigma }}_{SB{\left(3\right)}_i}\right)\right)lo{g}_{{\widehat{z}}_{SB\left(3\right)}}{\left(h\left({\mathsf{\sigma }}_{SE{\left(3\right)}_i}\right)\right)}^T$$

(34)

$$P_{zz|SE\left(3\right)}=\sum _{i=0}^{2N}{w}_{i}lo{g}_{{\widehat{z}}_{SB\left(3\right)}}\left(h\left({\mathsf{\sigma }}_{SE{\left(3\right)}_i}\right)\right)lo{g}_{{\widehat{z}}_{SE\left(3\right)}}{\left(h\left({\mathsf{\sigma }}_{SE{\left(3\right)}_i}\right)\right)}^T$$

(35)

Based on the above matrices, the Kalman gain on the manifold can be computed, and the state and covariance are subsequently updated as follows:

$$K_{k\left|SE\right(3)}={exp}_{SE\left(3\right)}\left(P_{xz|SE\left(3\right)}{P}_{zz|SE\left(3\right)}^{-1}\right)$$

(36)

$${\chi }_{k\left|SE\right(3)}={\widehat{\mu }}_{SE\left(3\right)}+{exp}_{SE\left(3\right)}\left(K_{k\left|SE\right(3)}\right){log}_{{\widehat{z}}_{SE\left(3\right)}}\left({\mathrm{z}}_{k|SE\left(3\right)}\right)$$

(37)

$$P_{k\left|SE\right(3)}={\widehat{P}}_{SE\left(3\right)}+{exp}_{SE\left(3\right)}\left(K_{k\left|SE\right(3)}{P}_{zz|SE\left(3\right)}{K_{k\left|SE\right(3)}}^T\right)$$

(38)

where $z$ denotes the actual measurement.

In summary, the workflow of the improved UKF algorithm based on Lie group operations can be summarized as follows:

• Construct the augmented state on the $\mathrm{S}\mathrm{E}\left(3\right)$ manifold and generate sigma points;
• Propagate the sigma points through the nonlinear system equations, and compute the a priori estimate using geodesic mean and covariance calculations;
• Predict the measurements using the observation model, and compute the cross-covariance and the observation covariance;
• Fuse the measurement residuals using the Kalman gain to update the posterior state and covariance;
• Iteratively execute the above steps to achieve continuous state estimation of the dynamic system on the manifold.

3. An Aided Localization Study Based on Pose Graph Optimization for LiDAR–GPS Fusion

This section describes the loosely coupled back-end optimization module, which refines the front-end poses by incorporating GPS measurements and loop closure constraints through pose graph optimization.

3.1. Construction of the Pose Graph Optimization Model

The pose state variable at an arbitrary time $i$ is defined as ${\xi }_i=[t_i^{\mathrm{T}},{\varphi }_i^{\mathrm{T}}{]}^{\mathrm{T}}={\left[\begin{array}{c}{x}_i,y_i,z_i,{\varphi }_{1i},{\varphi }_{2i},{\varphi }_{3i}\end{array}\right]}^{\mathrm{T}},$ where $t_i=[x_i,y_i,z_i{]}^{\mathrm{T}}\in {\mathbb{R}}^3$ denotes the position in the Cartesian coordinate system, and ${\varphi }_i=[{\varphi }_{1i},{\varphi }_{2i},{\varphi }_{3i}{]}^{\mathrm{T}}\in {\mathbb{R}}^3$ represents the Lie algebra parameters corresponding to the rotation matrix.

Based on this state representation, GPS measurements, loop closure detection results, and point cloud registration constraints can be jointly incorporated to construct various geometric constraints within the pose graph optimization model.

3.1.1. Construction of GPS Coordinate Constraints

The raw GPS data are first preprocessed by sequentially transforming them from the geodetic coordinate system to the Earth-Centered Earth-Fixed (ECEF) coordinate system, and then further into the East–North–Up (ENU) coordinate system [26,27,28]. After coordinate transformation, linear interpolation is applied to temporally align the GPS data with the LiDAR-synchronized timestamps. Let the GPS coordinate observation at time instant $i$ after temporal synchronization be denoted as:

$$t_{gi}={\left[t_{gxi},t_{gyi},t_{gzi}\right]}^T$$

(39)

The GPS residual term at time instant $i$ is defined as the difference between the estimated position $t_i$ and the GPS-observed position $t_{gi}$:

$$r_{gps}^i=t_i-t_{gi}$$

(40)

This residual depends only on the translational component of the state. Taking the derivative with respect to the state variable ${\xi }_i=[t_i^{\mathrm{T}},{\varphi }_i^{\mathrm{T}}{]}^{\mathrm{T}}$ yields the Jacobian matrix:

$${\displaystyle \frac{\partial r_{gps}^i\left(T\right)}{\partial {\mathsf{\xi }}_i}}={\displaystyle \frac{\partial \left(t_i-t_{gi}\right)}{\partial \left(\begin{array}{c}{t}_i\\ {\mathsf{\varphi }}_i\end{array}\right)}}={\left(\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 3}\end{array}\right)}_{3\times 6}$$

(41)

where $I_{3\times 3}$ denotes the identity matrix. Based on this residual, the objective function corresponding to the GPS constraint can be formulated as follows:

$$F_{gps}\left({\xi }_i\right)={‖\begin{array}{c}{r}_{gps}^i\end{array}‖}_2^2$$

(42)

3.1.2. Construction of Loop Closure Constraints

Using a GPS-assisted loop closure detection method, candidate loop closure node pairs can be identified between a historical time instant $k$ and the current time instant $i$ (with $k<i$). Let $T_k,T_i\in \mathrm{S}\mathrm{E}\left(3\right)$ denote the pose transformation matrices at time instants $k$ and $i$, respectively, and let the corresponding state variables be ${\xi }_k=[t_k^{\mathrm{T}},{\varphi }_k^{\mathrm{T}}{]}^{\mathrm{T}}$ and ${\xi }_i=[t_i^{\mathrm{T}},{\varphi }_i^{\mathrm{T}}{]}^{\mathrm{T}}$.

The relationship between the pose transformation matrices and the state variables can be expressed as follows:

$$\left\{\begin{array}{c}{T}_k=\left(\begin{array}{cc}{R}_k& t_k\\ 0& 1\end{array}\right)\\ {\xi }_k={\left[t_k,{\varphi }_k\right]}^T={\left[x_k,y_k,z_k,{\varphi }_{1k},{\varphi }_{2k},{\varphi }_{3k}\right]}^T\\ T_i=\left(\begin{array}{cc}{R}_i& t_i\\ 0& 1\end{array}\right)\\ {\xi }_i={\left[t_i,{\varphi }_i\right]}^T={\left[x_i,y_i,z_i,{\varphi }_{1i},{\varphi }_{2i},{\varphi }_{3i}\right]}^T\end{array}\right.$$

(43)

where $R_k,R_i\in \mathrm{S}\mathrm{O}\left(3\right)$ are the rotation matrices, $t_k,t_i\in {\mathbb{R}}^3$ denote the translation vectors, and ${\varphi }_k,{\varphi }_i\in {\mathbb{R}}^3$ represent the corresponding Lie algebra coordinates.

When the loop closure detection condition is satisfied, the poses $T_k$ and $T_i$ can be regarded as corresponding to the same spatial location. Accordingly, the loop closure residual term is constructed as follows:

$$r_{loop}^{\left(k,i\right)}={\left(\begin{array}{c}ln{\left(R_k^{-1}{R}_i\right)}^{\vee }\\ t_k-t_i\end{array}\right)}_{6\times 1}$$

(44)

In this formulation, $\mathrm{l}\mathrm{n}(\cdot ):\mathrm{S}\mathrm{O}\left(3\right)\to \mathfrak{s}\mathfrak{o}\left(3\right)$ denotes the logarithmic map of the Lie group, and $(\cdot {)}^{\vee }:\mathfrak{s}\mathfrak{o}(3)\to {\mathbb{R}}^3$ maps a skew-symmetric matrix to a three-dimensional vector. This residual jointly characterizes the consistency constraints of both rotational and translational components in the loop closure.

According to the correspondence between Lie algebra elements and rotation parameters, the Lie algebra can be parameterized as $\varphi =\theta a$, where $a\in {\mathbb{R}}^3$ is a unit rotation axis vector and $\theta \in \mathbb{R}$ denotes the rotation angle (in radians) about this axis. Under this parameterization, the left Jacobian matrix associated with the Baker–Campbell–Hausdorff (BCH) formula on the Lie group $\mathrm{S}\mathrm{O}\left(3\right)$ can be expressed as:

$$J_l\left(\mathsf{\varphi }\right)={\displaystyle \frac{sin\mathsf{\theta }}{\mathsf{\theta }}}I+\left(1-{\displaystyle \frac{sin\mathsf{\theta }}{\mathsf{\theta }}}\right)a{a}^T+{\displaystyle \frac{1-cos\mathsf{\theta }}{\mathsf{\theta }}}{a}^{\wedge }$$

(45)

Here, $a^{\wedge }$ denotes the skew-symmetric matrix corresponding to the vector $a$. This Jacobian matrix is subsequently employed in pose graph optimization to perform linearization of the rotational residuals.

b.

By differentiating the residual equation, the Jacobian matrix $J_{loop}^i$ can be derived as follows:

$$\begin{gathered} {\displaystyle \frac{\partial r_{loop}^{\left(k,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_i}}={\displaystyle \frac{\partial \left(\begin{array}{c}{ln\left(R_k^{-1}{R}_i\right)}^{\vee }\\ t_k-t_i\end{array}\right)}{\partial \left(\begin{array}{c}{t}_i\\ {\mathsf{\varphi }}_i\end{array}\right)}}=\left(\begin{array}{cc}{\displaystyle \frac{\partial \left({ln\left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial t_i}}& {\displaystyle \frac{\partial \left({ln\left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_i}}\\ {\displaystyle \frac{\partial \left(t_k-t_i\right)}{\partial t_i}}& {\displaystyle \frac{\partial \left(t_k-t_i\right)}{\partial {\mathsf{\varphi }}_i}}\end{array}\right) \\ =\left(\begin{array}{cc}{0}_{3\times 3}& {\displaystyle \frac{\partial \left({ln\left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_i}}\\ -I_{3\times 3}& 0_{3\times 3}\end{array}\right) \end{gathered}$$

(46)

The computation of ${\displaystyle \frac{\partial \left({\mathit{ln}\left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\varphi }_i}}$ is given as follows:

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial {\ln \left(R_k^{-1}{R}_i\right)}^{\vee }}{\partial {\varphi }_i}}=& \underset{\mathsf{\varphi }\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_k^{-1}{R}_i\exp \left({\varphi }_i^{\wedge }\right)\right)}^{\vee }-{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }}{{\varphi }_i}}=\hfill \\ & =\underset{\mathsf{\varphi }\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }+J_r^{-1}\left({\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right){\varphi }_i-{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }}{{\varphi }_i}}=\hfill \\ & =J_r^{-1}\left({\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right)=J_l^{-1}\left(-{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right)\hfill \end{array}$$

(47)

Combining the above two expressions, we obtain:

$$J_{loop}^i={\displaystyle \frac{\partial r_{loop}^{\left(k,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_i}}={\left(\begin{array}{cc}{0}_{3\times 3}& J_l^{-1}\left(-{\ln \left(R_k{R}_i\right)}^{\vee }\right)\\ -I_{3\times 3}& 0_{3\times 3}\end{array}\right)}_{6\times 6}$$

(48)

b.

By differentiating the residual equation, the Jacobian matrix $J_{loop}^k$ can be derived as follows:

$$\begin{gathered} {\displaystyle \frac{\partial r_{loop}^{\left(k,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_k}}={\displaystyle \frac{\partial \left(\begin{array}{c}{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\\ t_k-t_i\end{array}\right)}{\partial \left(\begin{array}{c}{t}_k\\ {\mathsf{\varphi }}_k\end{array}\right)}}=\left(\begin{array}{cc}{\displaystyle \frac{\partial \left({\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial t_k}}& {\displaystyle \frac{\partial \left({\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_k}}\\ {\displaystyle \frac{\partial \left(t_k-t_i\right)}{\partial t_k}}& {\displaystyle \frac{\partial \left(t_k-t_i\right)}{\partial {\mathsf{\varphi }}_k}}\end{array}\right) \\ =\left(\begin{array}{cc}{0}_{3\times 3}& {\displaystyle \frac{\partial \left({ln\left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_k}}\\ I_{3\times 3}& 0_{3\times 3}\end{array}\right) \end{gathered}$$

(49)

The computation of ${\displaystyle \frac{\partial \left({\mathit{ln}\left(R_k^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\varphi }_k}}$ is given as follows:

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial {\ln \left(R_k^{-1}{R}_i\right)}^{\vee }}{\partial {\mathsf{\varphi }}_k}}=& \underset{\mathsf{\varphi }\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_k^{-1}\exp \left(-{\mathsf{\varphi }}_k^{\wedge }\right)R_i\right)}^{\vee }-{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }}{{\mathsf{\varphi }}_k}}\hfill \\ & =\underset{\mathsf{\varphi }\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_k^{-1}{R}_i\exp \left({\left(-R_i^T{\mathsf{\varphi }}_k\right)}^{\wedge }\right)\right)}^{\vee }-{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }}{{\mathsf{\varphi }}_k}}\hfill \\ & =-J_l^{-1}\left({\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right)R_i^T=-J_l^{-1}\left(-{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right)R_i^T\hfill \end{array}$$

(50)

Combining the above two expressions, we obtain:

$$J_{loop}^k={\displaystyle \frac{\partial r_{loop}^{\left(k,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_k}}={\left(\begin{array}{cc}{0}_{3\times 3}& -J_l^{-1}\left(-{\ln \left(R_k^{-1}{R}_i\right)}^{\vee }\right)R_i^T\\ I_{3\times 3}& 0_{3\times 3}\end{array}\right)}_{6\times 6}$$

(51)

3.1.3. Construction of Point Cloud Registration Constraints

Based on a vision-enhanced point cloud registration algorithm, the relative pose transformation between two consecutive time instants $i-1$ and $i$(with $i\ge 1$) can be obtained. Let the pose transformation matrices at these two time instants be denoted as $T_{i-1},T_i\in \mathrm{S}\mathrm{E}\left(3\right)$, and let their corresponding Lie algebra coordinates be ${\xi }_{i-1},{\xi }_i\in {\mathbb{R}}^6$. According to the definition of point cloud registration, the relative pose transformation can be expressed as $T_{i-1.i}=T_{i-1}^{-1}{T}_i.$

Expanding the pose transformation matrices and the corresponding state variables yields:

$$\left\{\begin{array}{c}{T}_{i-1}=\left(\begin{array}{cc}{R}_{i-1}& t_{i-1}\\ 0& 1\end{array}\right), T_i=\left(\begin{array}{cc}{R}_i& t_i\\ 0& 1\end{array}\right), T_{i-1,i}=\left(\begin{array}{cc}{R}_{i-1,i}& t_{i-1,i}\\ 0& 1\end{array}\right)\\ {\xi }_{i-1}={\left[t_{i-1},{\varphi }_{i-1}\right]}^T,{\xi }_i={\left[t_i,{\varphi }_i\right]}^T\end{array}\right.$$

(52)

Therefore, expanding $T_{i-1.i}=T_{i-1}^{-1}{T}_i$ yields:

$$\left(\begin{array}{cc}{R}_{i-1,i}& t_{i-1,i}\\ 0& 1\end{array}\right)=\left(\begin{array}{cc}{R}_{i-1}^{-1}& -R_{i-1}^{-1}{t}_{i-1}\\ 0& 1\end{array}\right)\left(\begin{array}{cc}{R}_i& t_i\\ 0& 1\end{array}\right)=\left(\begin{array}{cc}{R}_{i-1}^{-1}{R}_i& R_{i-1}\left(t_i-t_{i-1}\right)\\ 0& 1\end{array}\right)$$

(53)

However, due to the presence of errors, the above relation does not hold exactly. Accordingly, the registration residual $r_{registration}^{\left(i-1,i\right)}$ is constructed as follows:

$$r_{registration}^{\left(i-1,i\right)}={\left(\begin{array}{c}{\left(\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)\right)}^{\vee }\\ t_i-t_{i-1}-R_{i-1}{t}_{i-1,i}\end{array}\right)}_{6\times 1}$$

(54)

• By differentiating the residual equation, the Jacobian matrix $J_{registration}^{i-1}$ can be derived as follows:

  $$\begin{array}{cc}\hfill {\displaystyle \frac{\partial r_{\mathrm{registration}}^{\left(i-1,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_{i-1}}}=& {\displaystyle \frac{\partial {\left(\begin{array}{c}{\left(\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)\right)}^{\vee }\\ t_i-t_{i-1}-R_{i-1}{t}_{i-1,i}\end{array}\right)}_{6\times 1}}{\partial \left(\begin{array}{c}{t}_{i-1}\\ \begin{array}{c}{\mathsf{\varphi }}_{i-1}\end{array}\end{array}\right)}}\hfill \\ & =\left(\begin{array}{cc}{\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial t_{i-1}}}& {\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_{i-1}}}\\ {\displaystyle \frac{\partial \left(t_i-t_{i-1}-R_{i-1}{t}_{i-1,i}\right)}{\partial t_{i-1}}}& {\displaystyle \frac{\partial \left(t_i-t_{i-1}-R_{i-1}{t}_{i-1,i}\right)}{\partial {\mathsf{\varphi }}_{i-1}}}\end{array}\right)\hfill \\ & =\left(\begin{array}{cc}{0}_{3\times 3}& {\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_{i-1}}}\\ -I_{3\times 3}& {\left(R_{i-1}{t}_{i-1,i}\right)}^{\wedge }\end{array}\right)\hfill \end{array}$$

  (55)

The computation of ${\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_{i-1}}}$ is given as follows:

$$\begin{array}{cc}{\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\varphi }_{i-1}}}& =\underset{\mathsf{\varphi }\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}\exp \left(-{\varphi }_{i-1}^{\wedge }\right)R_i\right)}^{\vee }-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }}{{\varphi }_{i-1}}}\hfill \\ & =\underset{\mathsf{\varphi }\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\exp \left({\left(-R_i^T{\varphi }_{i-1}\right)}^{\wedge }\right)\right)}^{\vee }-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }}{{\varphi }_{i-1}}}\hfill \\ & =\underset{\mathsf{\varphi }\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\exp \left({\left(-R_i^T{\varphi }_{i-1}\right)}^{\wedge }\right)\right)}^{\vee }-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }}{{\varphi }_{i-1}}}\hfill \\ & =-J_r^{-1}\left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)R_i^T\hfill \\ & =-J_l^{-1}\left(-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)R_i^T\hfill \end{array}$$

(56)

Accordingly, the Jacobian matrix $J_{registration}^{i-1}$ is given by:

$$J_{registration}^{i-1}={\displaystyle \frac{\partial r_{\mathrm{registration}}^{\left(i-1,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_{i-1}}}={\left(\begin{array}{cc}{0}_{3\times 3}& -J_l^{-1}\left(-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)R_i^T\\ -I_{3\times 3}& {\left(R_{i-1}{t}_{i-1,i}\right)}^{\wedge }\end{array}\right)}_{6\times 6}$$

(57)

• By differentiating the residual equation, the Jacobian matrix $J_{registration}^i$ can be derived as follows:

  $$\begin{array}{cc}\hfill {\displaystyle \frac{\partial r_{\mathrm{registration}}^{\left(i-1,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_i}}=& {\displaystyle \frac{\partial {\left(\begin{array}{c}{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\\ t_i-t_{i-1}-R_{i-1}{t}_{i-1,i}\end{array}\right)}_{6\times 1}}{\partial \left(\begin{array}{c}{t}_i\\ {\mathsf{\varphi }}_i\end{array}\right)}}\hfill \\ & =\left(\begin{array}{cc}{\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial t_i}}& {\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_i}}\\ {\displaystyle \frac{\partial \left(t_i-t_{i-1}-R_{i-1}{t}_{i-1,i}\right)}{\partial t_i}}& {\displaystyle \frac{\partial \left(t_i-t_{i-1}-R_{i-1}{t}_{i-1,i}\right)}{\partial {\mathsf{\varphi }}_i}}\end{array}\right)\hfill \\ & =\left(\begin{array}{cc}{0}_{3\times 3}& {\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_i}}\\ I_{3\times 3}& 0_{3\times 3}\end{array}\right)\hfill \end{array}$$

  (58)

The computation of ${\displaystyle \frac{\partial \left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)}{\partial {\mathsf{\varphi }}_i}}$ is given as follows:

$$\begin{array}{c}{\displaystyle \frac{\partial {\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }}{\partial {\mathsf{\varphi }}_i}}\hfill \\ =\underset{{\mathsf{\varphi }}_i\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\exp \left({\mathsf{\varphi }}_i^{\wedge }\right)\right)}^{\vee }-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }}{{\mathsf{\varphi }}_i}}\hfill \\ =\underset{{\mathsf{\varphi }}_i\to 0}{\lim }{\displaystyle \frac{{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }+J_r^{-1}\left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right){\mathsf{\varphi }}_i-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }}{{\mathsf{\varphi }}_i}}\hfill \\ =J_r^{-1}\left({\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)=J_l^{-1}\left(-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)\hfill \end{array}$$

(59)

Accordingly, the Jacobian matrix $J_{registration}^i$ is given by:

$$J_{registration}^i={\displaystyle \frac{\partial r_{\mathrm{registration}}^{\left(i-1,i\right)}\left(T\right)}{\partial {\mathsf{\xi }}_i}}={\left(\begin{array}{cc}{0}_{3\times 3}& J_l^{-1}\left(-{\ln \left(R_{i-1,i}^{-1}{R}_{i-1}^{-1}{R}_i\right)}^{\vee }\right)\\ I_{3\times 3}& 0_{3\times 3}\end{array}\right)}_{6\times 6}$$

(60)

3.1.4. Design of the Pose Graph Model

During the optimization process, if constraints are constructed directly from LiDAR point cloud feature correspondences, the number of matched points will increase rapidly as the number of poses grows. This leads to a substantial computational burden and is unfavorable for the design of lightweight systems.

Therefore, in the back-end, a pose graph optimization framework is adopted, in which the poses obtained from point cloud registration are treated as optimization variables, thereby avoiding large-scale point cloud matching and significantly reducing computational complexity [29,30].

The optimization model incorporates three types of constraints: GPS position constraints, loop closure constraints, and point cloud registration constraints. The resulting pose graph is illustrated as follows:

The objective function is formulated as follows:

$$\begin{array}{cc}\hfill F({\mathsf{\xi }}_1,{\mathsf{\xi }}_2,{\mathsf{\xi }}_3,{\mathsf{\xi }}_4,{\mathsf{\xi }}_5,{\mathsf{\xi }}_6& ,\dots ,{\mathsf{\xi }}_n)\hfill \\ & =\underset{\mathsf{\xi }}{\min }{\displaystyle \frac{1}{2}}({\displaystyle {\displaystyle \sum }_{k,i\in {\mathsf{\epsilon }}_{loop-closure}}}{r}_{k,i}^T{\mathsf{\Sigma }}_{k,i}^{-1}{r}_{k,i}+{\displaystyle {\displaystyle \sum }_{i-1,i\in {\mathsf{\epsilon }}_{registration}}}{r}_{i-1,i}^T{\mathsf{\Sigma }}_{i-1,i}^{-1}{r}_{i-1,i}\hfill \\ & +{\displaystyle {\displaystyle \sum }_{i\in {\mathsf{\epsilon }}_{gps}}}{r}_i^T{\mathsf{\Sigma }}_i^{-1}{r}_i)\hfill \end{array}$$

(61)

Following the standard design of pose graph optimization algorithms, the three types of constraints are incorporated into the optimization framework, which yields the complete objective function of the pose graph optimization algorithm. Additionally, in order to handle measurement uncertainties and improve the robustness of the optimization, each constraint is weighted by an information matrix that reflects the confidence level of the corresponding measurement. These weights ensure that more reliable measurements, such as GPS, have a greater impact on the optimization process, while less reliable constraints are given less importance. To mitigate the impact of potential outliers, the Huber loss function is employed as a robust kernel, with its parameter set to δ = 1.345, which provides a balance between quadratic and linear loss and effectively reduces the influence of large residuals. This enhances the stability and precision of the optimization process, ensuring that it remains both computationally efficient and resilient to data errors.

3.2. Technical Implementation Scheme

(1)

GPS Module

• The satellite information acquisition module is responsible for receiving raw GPS satellite signal data;
• The latitude and longitude solution module computes the UAV’s geodetic coordinates based on the received signals and provides initial information for the coordinate transformation module;
• The coordinate transformation module converts the geodetic coordinates into the Earth-Centered Earth-Fixed (ECEF) coordinate system and subsequently transforms them into relative coordinates in the LiDAR coordinate frame, which are defined as the node coordinates in this study.

(2)

LiDAR Module

• The point cloud acquisition module collects and generates point cloud data of the surrounding environment using an onboard mechanical LiDAR. The point cloud data are represented in the form of three-dimensional coordinates $\left(X,Y,Z\right)$. This module performs data acquisition and transmission, providing raw point cloud data for subsequent processing stages, and serves as the basis for lightweight front-end perception in power inspection scenarios.
• The point cloud filtering module operates on the output of the acquisition layer. In this work, K-means clustering is adopted as a lightweight preprocessing strategy to organize local point cloud structure and suppress redundant or irrelevant small-scale noise points with relatively low computational cost. Compared with more complex density-based or model-based filtering methods, this design is more suitable for the current real-time-oriented framework. In addition, voxel grid filtering is employed to reduce the number of points, thereby decreasing algorithmic complexity and enhancing real-time performance. The combination of K-means clustering and voxel grid downsampling provides a practical balance between filtering effectiveness and computational efficiency.
• The point cloud calibration module utilizes the raw point cloud data together with the matching results from the two preceding frames. A prediction-based approach is adopted to transform the point clouds from each scanning cycle into a unified temporal reference, producing temporally aligned point clouds that are subsequently fed into the feature extraction module, which helps improve temporal consistency before registration.
• The feature extraction module extracts edge features and planar features from the point cloud based on curvature analysis. This curvature-based design is chosen because it can effectively preserve the dominant structural characteristics of power-line environments while maintaining relatively low implementation complexity. Compared with learning-based feature extraction methods, it avoids additional training requirements and higher computational overhead, making it more suitable for the current stage of the proposed framework. Feature sequences corresponding to different time instants are stored for matching purposes. The point cloud matching layer matches features from identical spatial locations across two time instants and forwards the results to the pose estimation layer.
• The point cloud matching module performs feature-based matching between two time instants and employs the Levenberg–Marquardt (LM) nonlinear optimization method to decouple and estimate the UAV rotation matrix $\mathbf{R}$ and translation vector $\mathbf{T}$. The resulting pose estimates are then passed to the localization fusion layer. This feature-based matching strategy further reduces the computational burden compared with direct dense matching, and is therefore more compatible with the efficiency requirements of UAV onboard localization.

(3)

Remaining Modules

• The graph optimization module adopts a sliding-window-based optimization strategy. It fuses the pose transformation information between two consecutive point clouds output by the point cloud matching module with the relative GPS poses provided by the coordinate transformation module. By jointly optimizing multiple historical state variables through a least-squares framework, the pose states of multiple time instants are estimated simultaneously.
• The inverse coordinate transformation module converts the fused pose estimates back into the Earth-Centered Earth-Fixed (ECEF) coordinate system and subsequently transforms them into geodetic coordinates (latitude and longitude), which are then output as the final positioning results. The coordinate transformation process remains as follows: First, the pose estimates (e.g., position and orientation) in the local frame are converted to ECEF coordinates using a series of geometric transformations. This involves first translating the local coordinate system’s origin to the Earth’s center and then rotating the coordinates to align with the Earth’s rotational axes. The rotation is performed using a direction cosine matrix (DCM) or quaternion-based transformations. Second, once the position is in the ECEF frame, it is transformed into geodetic coordinates (latitude, longitude, and altitude). The relationship between ECEF coordinates and geodetic coordinates is non-trivial and requires solving the following set of nonlinear equations:

Latitude equation, Longitude equation and Height equation:

$$tan\left(\varphi \right)={\displaystyle \frac{z+e^2·b·{sin}^3\left(\varphi \right)}{p-e^2·a·{cos}^3\left(\varphi \right)}}$$

(62)

$$\lambda =arctan\left({\displaystyle \frac{y}{x}}\right)$$

(63)

$$h={\displaystyle \frac{p}{cos\left(\varphi \right)}}-N\left(\varphi \right)$$

(64)

where $N\left(\varphi \right)$ is the radius of curvature in the prime vertical, and $p=\sqrt{x^2+y^2}$. These equations are solved iteratively using the Newton–Raphson method to refine the latitude (φ), longitude (λ), and height (h) by iterating based on initial guesses.

The above figure illustrates the procedure for obtaining edge coordinates from the LiDAR module. Specifically, during the $k$-th execution cycle, the LiDAR acquires a point cloud set ${\mathrm{P}\mathrm{C}}_0\left(k\right)$ over one scanning period. Using the integrated navigation pose sequence ${\mathbf{T}}_0\left(k\right)$ within the same period, the point clouds are temporally calibrated to generate a synchronized point cloud set ${\mathrm{P}\mathrm{C}}_1\left(k\right)$, in which all points are referenced to a common time instant. Subsequently, K-means clustering is applied for denoising, resulting in the filtered point cloud set ${\mathrm{P}\mathrm{C}}_2\left(k\right)$.

Line and planar features $\mathbf{Q}\left(k\right)$ are then extracted from the processed point cloud and matched with the corresponding features $\mathbf{Q}(k-1)$ from the previous cycle. Based on the matched features, an error function $\mathbf{X}\left(k\right)$ is constructed with the rotation matrix $\mathbf{R}$ and translation vector $\mathbf{T}$ as optimization variables. If the condition $\mid \mathbf{X}\left(k\right)\mid <T$(where $T$ denotes a predefined accuracy threshold) is not satisfied, the rotation matrix $\mathbf{R}\left(k\right)$ and translation vector $\mathbf{T}\left(k\right)$ are refined using the Levenberg–Marquardt (LM) optimization method. This iterative optimization process continues until the convergence criterion is met, after which the optimized $\mathbf{R}\left(k\right)$ and $\mathbf{T}\left(k\right)$ are output as the relative pose between the two nodes.

Figure 4 above illustrates the pose graph optimization process. In this framework, the GPS-derived estimates are incorporated as node constraints, while the LiDAR-derived pose estimates are treated as edge constraints between nodes. A pose graph consisting of six node constraints and five inter-node edge constraints is constructed. Using a sliding-window strategy, the optimization continuously incorporates the most recent measurements while discarding older state variables. The Levenberg–Marquardt (LM) method is employed to iteratively optimize the pose graph, ultimately yielding optimal estimates of multiple state variables, from which the final poses are obtained.

The iterative algorithm for nonlinear least-squares optimization proceeds as follows. First, the nonlinear problem is linearized to construct an equivalent linear system. Then, the increment of the optimization variables is computed from this linear system and used to update the current estimate. This updated estimate is taken as the starting point of the next iteration. When the maximum number of iterations is reached, or when the objective function value falls below a predefined threshold, the optimization is considered to have converged. At that point, the corresponding optimization variables are taken as the final solution. The following Figure 5 shows the flowchart of the LiDAR-based pose estimation algorithm.

4. Experimental Setup and Result Analysis

4.1. Experimental Evaluation of UKF-Based LiDAR–IMU-Aided Localization

The experimental evaluation in this section is based on the Lie-group-based UKF-SLAM framework introduced in Section 2.2.2, with the objective of validating its effectiveness in improving the accuracy and stability of system state estimation.

The improved UKF algorithm based on Lie group operations follows the formulation presented in Section 2.2.2. For reference and to facilitate reproducibility, the overall workflow of the proposed LiDAR–IMU–GPS fusion framework, including both the front-end UKF-SLAM and the back-end pose graph optimization, is summarized in Algorithm 1.

Algorithm 1: LiDAR–IMU–GPS Fusion for UAV Localization

Input:

$\hspace{1em}\hspace{1em}\text{-} \mathrm{LiDAR} \mathrm{point} \mathrm{clouds} {\mathrm{P}}_{\mathrm{k}}$ at time k

$\hspace{1em}\hspace{1em}\text{-} \mathrm{IMU} \mathrm{measurements} {\mathrm{I}}_{\mathrm{k}}$$= \{w_m$$, a_m$} between k − 1 and k

$\hspace{1em}\hspace{1em}\text{-} \mathrm{GPS} \mathrm{readings} g_k$ (latitude, longitude, altitude)

Output:

$\hspace{1em}\hspace{1em}\text{-} \mathrm{Optimized} \mathrm{pose} \mathrm{trajectory} T_k$∈SE(3) for each time step k

1:  $\mathrm{Initialize} \mathrm{state} \mathrm{vector} {\chi }_0$$(\mathrm{pose}, \mathrm{velocity}, \mathrm{biases}, \mathrm{landmarks}), \mathrm{covariance} P_0$, empty pose graph

2:  for each time step k = 1, 2, … do

3:    // ---------- Front-end: LiDAR–IMU fusion via Lie-group UKF ----------

$4: \hspace{1em}\hspace{1em}\mathrm{Perform} \mathrm{IMU} \mathrm{pre}\text{-}\mathrm{integration} \mathrm{over} [t_{k-1}$$, t_k$] using Equations (2)–(6)

$5: \hspace{1em}\hspace{1em}\mathrm{Generate} \mathrm{sigma} \mathrm{points} {\sigma }_i$ on SE(3) manifold (Equations (28)–(30))

6:    Propagate sigma points through motion model (Equations (2)–(6))

$7: \hspace{1em}\hspace{1em}\hspace{1em}\mathrm{Compute} \mathrm{predicted} \mathrm{mean} {\widehat{\mathrm{\mu }}}_{\mathrm{S}\mathrm{E}\left(3\right)}$$\mathrm{and} \mathrm{covariance} {\widehat{P}}_{\mathrm{S}\mathrm{E}\left(3\right)}$ using geodesic distance (Equations (31) and (32))

$8: \hspace{1em}\hspace{1em}\mathrm{if} \mathrm{new} \mathrm{LiDAR} \mathrm{scan} {\mathrm{P}}_{\mathrm{k}}$ is available then

9:      Extract edge and planar features based on curvature (Section 3.2)

$10: \hspace{1em} \hspace{1em}\hspace{1em}\mathrm{Match} \mathrm{features} \mathrm{with} \mathrm{previous} \mathrm{frame} P_{k-1}$

11:    Compute observation model (Equations (8) and (9))

12:    Update UKF: compute Kalman gain, posterior state and covariance on SE(3) (Equations (33)–(38))

13:    end if

14:

15:    // ---------- Back-end: Pose graph optimization (triggered every N frames) ----------

16:    if k mod N = 0 then

$17: \hspace{1em} \hspace{1em}\hspace{1em}\mathrm{Add} \mathrm{current} \mathrm{pose} T_k$ as a new node in the graph

18:    if GPS data available then

$19: \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{Add} \mathrm{GPS} \mathrm{constraint} \mathrm{edge}: \mathrm{residual} r_{gps}^i$ (Equation (40)), Jacobian (Equation (41))

20:    end if

$21: \hspace{1em} \hspace{1em}\hspace{1em}\mathrm{Add} \mathrm{odometry} \mathrm{edge} \mathrm{between} T_{k-1}$$\mathrm{and} T_k$ using LiDAR registration residual (Equation (54)), Jacobians (Equations (57) and (60))

22:    if loop closure detected between current frame and historical frame l then

23:      Add loop closure edge: residual $r_{loop}^{\left(k,i\right)}$ (Equation (44)), Jacobians (Equations (48) and (51))

24:    end if

25:     Solve nonlinear least-squares problem using Levenberg–Marquardt (Equation (61))

26:    end if

27: end for

$28:\mathrm{return} \mathrm{optimized} \mathrm{poses} T_k$

The following Figure 6 presents the validation results of the algorithm on the test dataset.

Experimental results demonstrate that the proposed method achieves effective and robust state estimation, with localization accuracy that is comparable to or superior to that of current state-of-the-art approaches. The experimental results are shown in Figure 7 below.

Table 1. Experimental Results on the Dataset.

Sequence	MH_01_Easy	MH_02_Easy	MH_03_Medium	MH_04_Difficult	MH_05_Difficult
VIN-MONO	0.114	0.186	0.202	0.257	0.254
ROVIO	0.172	0.521	0.386	0.522	0.418
NormalUKF	0.284	0.199	0.246	0.490	0.351
Proposed	0.139	0.170	0.243	0.323	0.261

presents a comparison of the absolute trajectory error (ATE) between the proposed method and three representative state-of-the-art approaches (VIN-MONO, ROVIO, and NormalUKF) across five data sequences. The results indicate that the proposed method achieves superior performance on multiple sequences, particularly attaining the lowest error on the MH_02_easy sequence (0.170). Overall, its localization accuracy is comparable to or better than that of the best-performing baseline methods.

4.2. Experimental Evaluation of Pose Graph Optimization-Based LiDAR-GPS-Aided Localization

4.2.1. Data Analysis

Experiments are conducted on data sequences containing loop closures to verify the feasibility of the loop closure detection mechanism. The experimental results are shown in

Table 2. Loop Closure Detection Results on Dataset Sequence 2.

Loop Closure Index	New Frame	Old Frame	Fitting Error
1	983	153	0.156819
2	988	158	0.0504666
3	1353	944	0.0826466
4	1358	949	0.0233855
5	1363	954	0.0472858
6	1368	959	0.0849739
7	1373	964	0.0943137
8	1378	969	0.103733
9	1383	973	0.107383
10	1388	977	0.0551427

below.

During operation, ten loop closure points are detected based on GPS coordinate distances and point cloud distance metrics. The fitting errors of all detected loop closures are below the predefined upper threshold (≤0.20). According to the loop closure detection results, ten edges corresponding to loop closure constraints are incorporated into the back-end optimization for further computation.

On Dataset Sequence 2, the front-end odometry is evaluated without back-end optimization. The original localization accuracy error is 4.535629 m, and the cumulative drift reaches 54.519383 m [here, cumulative error refers to the RMSE of the absolute pose error (APE) accumulated over the entire trajectory, which quantifies the total positional drift relative to the ground truth], indicating that the odometry trajectory has significantly deviated from the ground truth. After enabling back-end optimization with loop closure detection, the pose graph incorporates 10 loop-closure constraint edges, 1390 registration-based pose constraint edges, and jointly optimizes 1391 pose nodes. As a result, the localization accuracy error is reduced to 4.455809 m, while the cumulative drift is substantially decreased to 6.830040 m. These results demonstrate that back-end optimization with loop closure detection can effectively correct the localization trajectory in a timely manner and significantly reduce accumulated localization drift.

After further incorporating GPS-based back-end optimization, the pose graph contains a total of 2791 edges, including 1391 edges derived from GPS position constraints, 10 edges from loop closure constraints, and 1390 edges from point cloud registration constraints, with 1391 pose vertices in total. The resulting localization accuracy error is reduced to 1.173358 m. Since the incorporated GPS measurements exhibit only instantaneous (non-accumulating) errors rather than cumulative drift, the overall accumulated error of the system is reduced to 0.086931 m, indicating a substantial suppression of long-term drift.

A comparison with the GPS-assisted back-end optimization experiment without loop closure detection shows no significant degradation in localization accuracy. This is likely due to the relatively small number of loop closure constraints, resulting in comparable performance with or without loop closure detection when GPS constraints are included in the back-end optimization. The following

Table 3. Effect of Back-End Optimization with Loop Closure Detection on Eliminating Accumulated Drift for Dataset Sequence 2.

Method	Attribute	RPE	ATE	APE	Attribute	Value
Front-end odometry	Max	18.938824	110.030900	111.690937	--	--
	Mean	2.807531	42.333587	42.502190	--	--
	Median	1.833126	39.178521	39.217980	--	--
	Min	0.529429	0.000000	0.000001	--	--
	RMSE	4.535629	54.280761	54.519383	--	--
	SSE	555.442219	4098443	4134557	--	--
	STD	3.562261	33.974527	34.145673	--	--
Front-end odometry + back-end optimization with loop closure detection	Max	20.326717	16.018695	16.129760	--	--
	Mean	2.503285	5.470432	5.468591	Number of loop closure edges	10
	Median	1.456017	3.902593	3.899106	Number of optimization edges	1400
	Min	0.623540	0.000000	0.000001	Number of vertices	1391
	RMSE	4.455809	6.829047	6.830040	Error before optimization	--
	SSE	536.064369	64870.5	64889.4	Error after optimization	--
	STD	3.686164	4.087818	4.091939	Optimization time	0.921837
Front-end odometry + GPS-based back-end optimization	Max	2.274238	0.748267	0.800248	--	--
	Mean	1.077270	0.043310	0.055482	Number of loop closure edges	0
	Median	0.998823	0.026902	0.038200	Number of optimization edges	2781
	Min	0.287864	0.001636	0.007302	Number of vertices	1391
	RMSE	1.184077	0.076119	0.086910	Error before optimization	1103180
	SSE	37.855005	8.059595	10.506733	Error after optimization	23.5963
	STD	0.491454	0.062597	0.066896	Optimization time	0.110304
Front-end odometry + back-end optimization with loop closure detection + GPS-based back-end optimization	Max	2.274238	0.748267	0.800247	--	--
	Mean	1.056682	0.043851	0.055488	Number of loop closure edges	10
	Median	0.983374	0.027404	0.038182	Number of optimization edges	2791
	Min	0.245364	0.001638	0.007325	Number of vertices	1391
	RMSE	1.173358	0.076425	0.086931	Error before optimization	1157560
	SSE	37.172754	8.124548	10.511893	Error after optimization	23.7683
	STD	0.510090	0.062593	0.017162	Optimization time	0.17204

demonstrates the effect of back-end optimization and loop closure detection on eliminating cumulative errors in dataset sequence 2, and the following Figure 8 shows the error curves and XYZ errors of various methods.

4.2.2. Experimental Summary

• The system architecture design for UAV-based LiDAR–GPS fusion localization comprises multiple functional modules, including a satellite signal acquisition module, a latitude–longitude computation module, a coordinate transformation module, a point cloud acquisition module, a point cloud filtering module, a point cloud calibration module, a feature extraction module, a point cloud matching module, a graph optimization module, and an inverse coordinate transformation module.
• The algorithmic framework for UAV-based LiDAR–GPS fusion localization is established by integrating several key components, including the conversion of GPS geodetic coordinates into node coordinates for pose graph optimization, the algorithmic design for estimating the relative pose between two LiDAR point cloud frames, and the subsequent coupling of these estimates into a pose graph optimization framework. Through joint optimization, more accurate geodetic position estimates (latitude and longitude) are ultimately obtained.

5. Conclusions

This paper proposes a multi-sensor navigation method for UAV-based power inspection that integrates a tightly coupled LiDAR-IMU front-end (Lie-group UKF-SLAM) with a loosely coupled back-end (pose graph optimization with GPS and loop closure). The front-end enhances local estimation stability and accuracy, while the back-end effectively suppresses long-term drift using GPS constraints and loop closure.

Experimental results demonstrate that the proposed method achieves competitive performance on both public benchmark datasets and self-collected power inspection scenarios. In particular, in Section 4.1, on the MH_02_easy sequence, the proposed method reduces the absolute trajectory error (ATE) from 0.521 m (ROVIO) to 0.170 m, corresponding to an error reduction of approximately 67.4%. In Section 4.2, for Dataset Sequence 2, when only the front-end odometry is used, the original accuracy error is 4.535629 m and the cumulative error is 54.519383 m; after enabling loop-closure-based back-end optimization together with GPS-assisted back-end optimization, the cumulative error is reduced to 0.086931 m, demonstrating a reduction of more than 99% in cumulative drift relative to the front-end-only result. In addition, under GPS-degraded conditions, the system is able to maintain stable navigation through loosely coupled LiDAR–IMU fusion, which further verifies its robustness and practical applicability in complex power inspection environments.

Future work will focus on the following directions. First, further efforts will be devoted to improving the system’s adaptability in dynamic environments and under strong electromagnetic interference, with an emphasis on exploring robustness enhancement strategies for multi-modal sensor fusion. Second, deep learning-based feature extraction and matching methods will be investigated to improve the efficiency and accuracy of point cloud registration. Third, the deployment and optimization of the proposed system on edge computing platforms will be explored to better meet the requirements of real-time onboard processing and low power consumption in UAV applications. Fourth, this work can be extended to small unmanned surface vehicles equipped with autonomous navigation systems, thereby achieving a cross-domain breakthrough. Fifth, we will continue to complete field experiments and promote the application of this algorithm on real UAVs to meet corresponding engineering requirements. Sixth, to further strengthen the statistical validation of the proposed method, future work will include repeated experimental runs under consistent conditions to compute key uncertainty descriptors such as variance and confidence intervals. Seventh, we plan to deploy the proposed system on embedded platforms to evaluate onboard processing capability, and optimize the code for parallel execution and low-latency inference.