IAES Inter national J our nal of Robotics and A utomation (IJRA) V ol. 15, No. 1, March 2026, pp. 1 ∼ 20 ISSN: 2722-2586, DOI: 10.11591/ijra.v15i1.pp1-20 ❒ 1 Real-time lo w-drift global optimization f or dynamic scene LiD AR SLAM localization P eiyan Y ang 1 , Jiuyang Y u 1,2 , P an Liu 1 , W enfeng Xia 1 , Y aonan Dai 1 1 State K e y Laboratory of Green and Ef cient De v elopment of Phosphorus Resources, School of Mechanical and Electrical Engineering, W uhan Institute of T echnology , W uhan, China 2 Hubei Pro vincial Engineering T echnology Research Center of Green Chemical Equipment, School of Mechanical and Electrical Engineering, W uhan Institute of T echnology , W uhan, China Article Inf o Article history: Recei v ed Apr 18, 2025 Re vised No v 14, 2025 Accepted Feb 9, 2026 K eyw ords: LiD AR SLAM Normal distrib utions transform Pose estimation Real-w orld scenario v alidation Robot hardw are selection ABSTRA CT T o address challenges lik e global drift, unstable matching, and high computa- tional cost in light detection and ranging simultaneous localization and mapping (LiD AR SLAM) under comple x conditions, this paper proposes an impro v ed al- gorithm based on the LeGO-LO AM frame w ork. A Ne wton-optimized normal distrib utions transform (NDT) is inte grated to impro v e point cloud re gistration by constructing a ne g ati v e log-lik elihood objecti v e and optimizi ng pose estima- tion. Using initial pos e information from LeGO-LO AM accelerates con v er gence and enhances system rob ustness. This w ork addresses the problem of insuf - cient adaptability of e xisting algorithms in real scenarios. By deplo ying an inde- pendently designed four -wheel omnidirectional mobile robot platform, a h ybrid LiD AR SLAM frame w ork is used for precise positioning and map construction in comple x campus en vironments, successfully reducing the positioning error to the centimeter le v el. Experiments on the KITTI dataset sho w a 43.51% re- duction in maximum localization error and a 30.83% decrease in a v erage error . Field tests in real-w orld campus en vironments with pedestrians, bic ycles, and v ehicles demonstrate strong reliability , adaptability , and resistance to interfer - ence. Horizontal error w as reduced by about 58.26%, lo wering the a v erage error from 4.60 m to 1.92 m. Although computational load increases, it is of fset by us- ing high-performance LiD AR and processors. The enhanced accurac y and drift reduction signicantly outperform traditional methods. At critical time points such as 50 seconds and 100 seconds, the system achie v ed high-precision pose estimation and accurate en vironmental reconstruction. This is an open access article under the CC BY -SA license . Corresponding A uthor: Y aonan Dai State K e y Laboratory of Green and Ef cient De v elopment of Phosphorus Re sources, School of Mechanical and Electrical Engineering, W uhan Institute of T echnology W uhan 430205, China Email: dyn1121758919@163.com 1. INTR ODUCTION Simultaneous localization and mapping (SLAM) is a fundamental technology that enables autonomous robots to estimate their pose while incrementally b uilding a map of the surrounding en vironment [1]. SLAM systems are generally cate gorized into vision-based SLAM (V -SLAM) and light detection and ranging (LiD AR)- based SLAM (L-SLAM) [2]. V -SLAM uses visual sensors such as monocular or stereo cameras for motion J ournal homepage: http://ijr a.iaescor e .com Evaluation Warning : The document was created with Spire.PDF for Python.
2 ❒ ISSN: 2722-2586 estimation through feature tracking or direct methods. Although lightweight and cost-ef fecti v e, V -SLAM is highly sensiti v e t o lighting v ariations, motion blur , and lo w-t e xt ure surf aces, making it less reliable in unstruc- tured or dynamic outdoor en vironments [3]. In contrast, L-SLAM le v erages LiD AR sensors t o capture 3D point clouds with precise range and angular measurements, of fering strong geometric consistenc y and rob ustness to lighting conditions [2]. This mak es LiD AR-based approaches particularly suitable for mobile robot na vig ation in comple x and dynamic en vironments such as campuses or urban streets. F or embedded robotic platforms operating in real-w orld scenarios, a lightweight yet accurate LiD AR SLAM system is essential for maintaining stable locali zation and high-delity map reconstruction under real-time constraints [4]. LeGO-LO AM is a widely adopted lightweight LiD AR SLAM frame w ork that pro vides real-time per - formance with decent accurac y [5]. Ne v ertheless, it sho ws signicant de gradation in en vironments with sparse structural features or rapid motion, as its feature e xtraction and odometry modules rely hea vily on stabl e edge and planar points. Moreo v er , its localization accurac y is susceptible to drift in e xtended operations, particu- larly when deplo yed on embedded robotic system s na vig ating real-w orld en vironments be yond controlled lab settings [6]. T o o v ercome these limitations, we propose an impro v ed LiD AR SLAM frame w ork by inte grating a Ne wton-optimized normal distrib utions transform (NDT) into the LeGO-LO AM pipeline. The proposed method is tailored for real-time deplo yment on embedded mobile robots and achie v es rob ust l ocalization and mapping in challenging outdoor en vironments [7]. It enhances both local feature association and global pose correction by combining geometric feature e xtraction with probabilistic optimization. The main contrib utions of this w ork are as follo ws: a. A h ybrid LiD AR SLAM frame w ork combining feature-based odometry and NDT -based optimization: W e inte grate the g e ometric feature e xtraction of LeGO-LO AM with a Ne wton-optimized NDT back-end. This enhances pose estimation in lo w-feature or dynamically changing en vironments by combining data-dri v en odometry with probabilistic global alignment. b . Rob ust real-time localization enabled by adapti v e pose fusion and optimized map updating: The proposed approach fuses odometry and NDT pose estimations through an adapti v e weighting scheme to minimize drift and impro v e pose accurac y . In parallel, the local map is ef ciently updated using v ox el-based ltering to maintain spatial consistenc y while managing computat ional resources, making the system suitable for deplo yment on resource-constrained embedded platforms. c. Extensi v e v alidation in real-w orld dynamic outdoor scenarios: The algorithm is deplo yed on a mobile robot equipped with an embedded computing platform and high-performance sensors. Field e xperiments conducted in a comple x campus en vironment with dynamic elements—such as pedestrians, bic ycles, and v ehicles—demonstrate signicant impro v ements in localization accurac y , rob ustness, and en vironmental adaptability compared to baseline methods. 2. LITERA TURE REVIEW Ov er the past decade, LiD AR SLAM has achie v ed widespread adoption in outdoor industrial A GV inspection systems, as illustrated in Figure 1. Adv ances in computational po wer and algorithmic inno v ation ha v e spurred the de v elopment of numerous SLAM methodologies. Zhang and Singh pioneered LiD AR odom- etry and mapping (LO AM), a foundational w ork in LiD AR SLAM, which achie v ed real-time 3D positioning and mapping by decoupling high-frequenc y odometry and lo w-frequenc y mapping modules. Ho we v er , its re- liance on iterati v e closest point (ICP) matching and lack of loop closure detection led to signicant cumulati v e errors [8]. Shan and Englot proposed LeGO-LO AM, a lightweight, ground-optimized LiD AR SLAM frame- w ork tailored for ground v ehicles, introducing ground se gmentation and a tw o-stage optimization strate gy to achie v e lo w-drift localization on embedded platforms. Y et, its reliance on ICP for point cloud re gistration lim- its rob ustness in feature-sparse scenarios [9]. Subsequent studies, such as M-LO AM [10] and F-LO AM [11], enhanced ef cienc y through greedy feature selection and non-iterati v e distortion compensation b ut remained constrained by the inherent limitations of the ICP frame w ork, incl uding sensiti vity to initial v alues and poor noise rob ustness. While e xisting LiD AR SLAM methods demonstrate satisf actory performance in simple en vironments, the y struggle in comple x outdoor scenarios where dynamic obstacles (e.g., pedestrians, v ehicles) and te xtureless structures (e.g., b uildings, v e getation) introduce interference, leading to substant ial localization errors. T o IAES Int J Rob & Autom, V ol. 15, No. 1, March 2026: 1–20 Evaluation Warning : The document was created with Spire.PDF for Python.
IAES Int J Rob & Autom ISSN: 2722-2586 ❒ 3 address these challenges, researchers ha v e e xplored NDT to impro v e tolerance for initial re gistration errors and dynamic noise. Jiang et al . inte grated NDT wit h ICP for orchard spray robot na vig ation, le v eraging NDT’ s coarse re gistration to accelerate ICP con v er gence and reduce lateral positioning errors belo w 16 cm. Ho we v er , their approach ne glected dynamic obstacle-induced v ox el distrib ution distortions [12]. W ang et al . proposed NDT -V GICP , combini ng v ox el-based normal distrib ution models with geometric alignment for cross- platform forestry point cloud re gistration, achie ving a horizontal error of 4.27 cm. Y et, their method assumes static en vironments and struggles with mobil e platforms [13]. Other studies, such as ground se gmentation and v ertical clustering optimizations in LeGO-LO AM [14], impro v ed rotation error b ut f ailed to enhance the rob ustness of the re gistration algorithm itself. H PPLO - N et Ou r   S u rv ey s 1 202 5 20 20 202 2 202 1 202 3 202 4 201 9 201 8 201 4 L OA M L e GO - L OA M B a sl - ad   s la m L O CUS   2.0 M - L OA M F - L OA M L IO - SA M HD L - Gr a p h - SL A M Tr a n s LO P IN - SL A M R OL O - SL A M GS - L I VO Figure 1. Research on SLAM technology in recent ten years Concurrent ef forts to mitig ate dynamic noise and drift ha v e f aced trade-of fs. W ei et al . designed a dynamic f actor graph model with planar constraints, reducing Z-axis errors by 20.47%, b ut high accurac y de- pended on 32-line LiD ARs, lim iting applicability to lo w-cost sensors [15]. R OLO-SLAM decoupled rotational and translational optimization in unstructured terrains using spherical re gistration to suppress v ertical drift, yet its forw ard prediction mechanism f ailed under rapid terrai n undulations [16]. GS-LIV O fused LiD AR-IMU- camera data with Gaussian Splatting for real-time embedded deplo yment b ut suf fered from computational comple xity and slo w rendering due to multi-sensor dependencies [17]. These limitations underscore critical g aps in algorithmic rob ustness and sensor adaptability . T ra- ditional ICP frame w orks e xhibit sensiti vity to ini tial re gistration errors, often di v er ging in feature-sparse or dynamic en vironments [18]. While NDT impro v es noise tolerance through normal distrib ution modeling, e x- isting methods f ail to address v ox el distrib ution distortions caused by dynamic obstacles, leading to de graded localization accurac y [19]. M oreo v er , although emer ging approaches emphasize GPU acceleration and multi- sensor fusion, the y often incur signicant computati o na l o v erhead, making them impractical for deplo yment on mobile platforms such as industrial robots or lightweight drones. As a r esult, these methods are typically limited to e v aluations on public datasets and are rarely adopted in real-w orld applications. LeGO-LO AM, a lightweight LiD AR odometry and mapping frame w ork, e xtracts edge and plane features for localization and mapping [20], [22]. Ho we v er , in comple x en vironments (e.g., dense b uildings, v e getation, mix ed-material surf aces), feature e xtraction becomes unreliable due to te xture ambiguity or occlu- sions. These feature quality issues propag ate into matchi ng and pose estimation stages, e xacerbating errors [23]. Moreo v er , LeGO-LO AM’ s feature-based matching is vulnerable to point cloud noise, sparsity , and mis- matches, particularly in lar ge-scale mapping. Its optimization process further demands signicant computa- tional resources, challenging real-time performance under high-point-cloud loads [24], [26]. In summary , while NDT -enhanced LeGO-LO AM frame w orks sho w promise, classical NDT as signs equal weights to all occupied v ox els during r e gi stration, ne glecting the v arying impacts of cells with distinct spatial ranges and feature dimensions on transformation parameter estimation. T o bridge this g ap, we pro- pose a Ne wton-optimized NDT method to replace traditional ICP , enhancing noise resilience and addressing re gistration f ailures in feature-sparse scenarios. Combined with LeGO-LO AM’ s lightweight architecture, our approach introduces dynamic v ox el ltering and hierarchical ground constraint optimization. This frame w ork suppresses dynamic v ox el distortions while resolving v ertical drift through radar -compatible design and lo w- computational o v erhead, of fering a rob ust, cost-ef fecti v e solution for outdoor mobile robotics. 3. METHODOLOGY T o address the limitations of LeGO-LO AM in localization precision, this s tudy focuses on front-end optimization within the SLAM frame w ork, proposing a real-time LiD AR odometry and map update method Real-time low-drift global optimization for dynamic scene LiD AR SLAM localization (P eiyan Y ang) Evaluation Warning : The document was created with Spire.PDF for Python.
4 ❒ ISSN: 2722-2586 enhanced by NDT renement.The core inno v ation inte grates feature e xtraction, adapti v e NDT -based pose es- timation, multi-source pose fusion, and dynamic map updating into a unied pipeline. By embedding this frame w ork into a mobile robot platform, we demonstrate its feasibility in achie ving high-precision pose esti- mation and en vironmental mapping under dynamic scenarios. As illustrated in Figure 2, LeGO-LO AM-NDT is di vided into tw o modules: direct odometry and local feature renement. The LiD AR point cl o ud processing pipeline inte grates feature e xtraction and NDT -based pose optimization [27]. As sho wn in Figure 3, Initially , the system estimates the initial pose using real-time LiD AR point cloud data. The feature e xtraction module identies edge and plane features from the point cloud, which are subsequently matched via feature-based algori thms to estimate the current frame’ s pose. T o rene pose estimation accurac y , an NDT -based optimization algorithm is applied for pose correction [28]. This method iterati v ely optimizes transformation parameters to minimize re gistration errors, thereby enhancing pose precision while maintaining computational ef cienc y . As depicted in Figure 4, the proposed frame w ork emplo ys a weighted fusion strate gy to inte grate NDT -rened pose estimates with initial odometry outputs. This approach preserv es the real-time adv antages of odometry while fully le v eraging NDT’ s global optimization capabilities, thereby enhancing o v erall posi- tioning accurac y . The fused pose not only impro v es localization precision b ut also informs local map updates, enabling precise en vironmental reconstruction. Through iterati v e pose optimization and map renement, the system achie v es high-delity mapping of dynamic en vironments [29]. The updated local maps subsequently pro vide reliable spatial conte xt for do wnstream na vig ation tasks, ensuring stable operation in comple x, real- w orld scenarios [30]. L i D A R po i n t   cl o u d   d at a NDT Mat ch i n g   b as ed   o n   cu rrent   fram e In i t i al   p o s e U p d at ed   Map s Po s t u re  A d j u s t m en t G l o b al   O p t i m i zat i o n D i rect   o d o m et er L o cal   feat u re  ad j u s t m en t Figure 2. LeGO-LO AM-NDT system frame w ork Pcd m ap In i t i al i zat i o n   i n fo rm at i o n Real - t i m e las er  p o i n t   cl o u d L o cal   m ap   u p d at e i n i t i al i zat i o n Po i n t   cl o u d   t o   p cl NDT Mo b i l e  ro b o t   pose N ew t o n   o p t i m i zati o n   al g o ri t h m Figure 3. Processing process of laser point cloud data L i D A R L i d ar  d at a LEGO - L O A M  feat u re  ex t ract i o n G ro u n d   s eg m en t at i o n N o n - G ro u n d   Po i n t   Cl o u d   Seg m en t at i o n Retu rn   ed g e  featu res Fe at u re  ex t rac t i on 3 D  Poi n t   Cl o u d O ri g i n al   m ap Map  Match i n g Re - o p t i m i zat i o n O d o m et er O d o m et er  es t i m at i o n o p t i m i zat i o n N D T   Pos i t i o n i n g Po s e  Fu s i o n Map  U p d at es T_odom T_ndt W ei g h t ed   A v erage Poi n t   cl o u d   d at a  acqu i s i t i o n N D T   O p t i m i zed   Po s i t i o n i n g Figure 4. Ov erall algorithm o w chart of LeGO-LO AM-NDT IAES Int J Rob & Autom, V ol. 15, No. 1, March 2026: 1–20 Evaluation Warning : The document was created with Spire.PDF for Python.
IAES Int J Rob & Autom ISSN: 2722-2586 ❒ 5 3.1. F eatur e extraction in the LeGO-LO AM fr ontend The RANSA C-based plane se gmentation algorithm serv es as the cornerstone for ground point cloud e xtraction. F ormally , gi v en a point cloud P = p 1 , p 2 , . . . , p n , where p i = ( x i , y i , z i ) , RANSA C iterati v ely estimates a plane model: ax + by + cz + d = 0 by optimizing parameters a, b, c, d . The objecti v e is to minimize the orthogonal distance from each point p i = ( x i , y i , z i ) to the plane, dened as: Distance ( p i , Plane) = | ax i + by i + cz i + d | √ a 2 + b 2 + c 2 (1) As can be seen from the pseudocode belo w , RANSA C minimizes this distance function to i d e ntify the ground points P ground that t the plane model. Subsequently , these ground points are remo v ed from the original point cloud, and the remaining points are used for subsequent feature e xtraction. F or the remaining non-ground point cloud, this paper applies the Euclidean clustering algorithm for point cloud se gmentation. By calculating the Euclidean distance between points, adjacent points are grouped into the same cluster . If the distance between an y tw o points within a cluster is smaller than a gi v en threshold ϵ , the y are considered to belong to the same cluster . Specically , the dist ance between a pair of points p i and p j is dened as: d ( p i , p j ) = q ( x i − x j ) 2 + ( y i − y j ) 2 + ( z i − z j ) 2 (2) After clustering, multiple clusters C 1 , C 2 ,. . . , C k are obtained, with each cluster C i representing a potential feature re gion in the en vironment. F or each cluster C i , edge features and surf ace patch features are further e xtracted. The curv ature κ i is used to describe the de gree of curv ature of the point cloud surf ace, and is calculated as follo ws: κ i = 1 | C i | X p j ∈ C i | p j − p cen troid | 2 (3) Here, p cen troid represents the centroid of the cluster C i , and it is calculated as follo ws: p cen troid = 1 | C i | X p j ∈ C i p j (4) The curv ature κ i reects the sum of the squared Euclidean distances from the points to the centroid. A higher curv ature indicates that the cluster belongs to an edge feature. Based on a predene d threshold κ threshold , the feature points are classied as either edge features or surf ace features. If the curv ature e x- ceeds the threshold κ threshold , the cluster is considered an edge feature; otherwise, it is re g arded as a surf ace feature. Edge F eatures = { C i | κ i > κ threshold } (5) Planar F eatur e s = { C i | κ i ≤ κ threshold } (6) Finally , by mer ging the edge features and surf ace features, the feature point cloud F cur for the current frame is formed, which is then used for subsequent odometry estimation and map updating. The detailed implementation can be found in Algorithm 1: LeGO LO AM feature e xtraction. Real-time low-drift global optimization for dynamic scene LiD AR SLAM localization (P eiyan Y ang) Evaluation Warning : The document was created with Spire.PDF for Python.
6 ❒ ISSN: 2722-2586 Algorithm 1: LeGO LO AM feature e xtraction Input: point cl oud Output: edg e f eatur es, pl anar f eatur es, g r ound pts Function l eg o l oam f eatur e extr action ( point cl oud ) : g r ound pts ← ransac plane se gmentation ( point cl oud ) ; cl us te r s ← euclidean clustering ( point cl oud − g r ound pts ) ; edg e f eatur es ← [] ; pl anar f eatur es ← [] ; f or each cluster in cluster s do cur v atur e ← P ( np.linalg.norm ( pi − p centr oid ) ∗ ∗ 2 for pi in cluster ) ; if cur v atu r e > thr es hol d edg e then edg e f eatur es.append ( cl us t er ) ; end else pl anar f eatur es.append ( cl uster ) ; end end r etur n edg e f eatur es, pl anar f eatur es, g r ound pts 3.2. NDT -based pose r enement The NDT , whose modeling concept is illustrated in Figure 5, is a probabilistic method widely em- plo yed in point cloud re gistration and pose renement tasks. T raditional NDT partitions the point cloud into v ox els and ass umes Gaussian distrib ution within each v ox el to model the en vironment; in our imple- mentation, the NDT v ox el size is set to 1.0m, which of fers a good trade-of f between re gistration accurac y and computational cost in lar ge-scale outdoor en vironments, as sho wn in Figure 5(a). F or a v ox el containing { x 1 , x 2 , . . . , x n } points, the mean q and co v ariance matrix Σ are computed as: q = 1 n n X i =1 x i (7) Σ = 1 n n X i =1 ( x i − q ) ( x i − q ) T (8) Here, q represents the mean of the point set within the v ox el, and Σ denotes the co v ariance matrix, which describes the distrib ution characteristics of the point cloud within the v ox el. During the NDT re gistra- tion process, the input point cloud is aligned with the tar get point cloud through rotational and translational transformations, as sho wn in Figure 5(b). F or each transformed point x ′ , NDT calculates the probability den- sity of the point f alling within the v ox el based on the Gaussian distrib ution model of the v ox el. Assuming that the transformed point x ′ is near the v ox el mean q , the probability density function p ( x ′ ) of the point can be e xpressed as follo ws: p ( x ′ ) = 1 (2 π ) d/ 2 | Σ | 1 / 2 exp − 1 2 ( x ′ − q ) T Σ − 1 ( x ′ − q ) (9) Here, d represents the dimension of the point cloud, Σ denotes the co v ariance matrix, q is the mean of the v ox el, and Σ − 1 is the in v ers e of the co v ariance matrix. After completing the point cloud transformation, NDT optimizes the transformation matrix by calculating the matching de gree between the transformed point cloud and the tar get point cloud. Specically , the NDT algorithm computes the probability densities of all transformed points within the corresponding v ox els of the tar get point cloud, and then sums the log arithms of these probabilities to obtain the re gistration score: Score ( T ) = − N X i =1 log p ( x ′ i ) (10) IAES Int J Rob & Autom, V ol. 15, No. 1, March 2026: 1–20 Evaluation Warning : The document was created with Spire.PDF for Python.
IAES Int J Rob & Autom ISSN: 2722-2586 ❒ 7 Here, x ′ i represents the i -th transformed point, and p ( x ′ i ) denotes the probability density of that point within the corresponding v ox el. In NDT , the core of scan matching is to align the current laser scan point cloud with the NDT probability model of the reference scan by optimizing the rigid body transformation parameters p = ( t x , t y , ϕ ) ⊤ . NDT di vides the reference scan’ s 2D space into grid cells, and the distrib ution of laser points within each cell is modeled by a local normal distrib ution. The optimization objecti v e is dened as minimizing the ne g ati v e log-lik elihood function between the current scan points and the NDT model: f ( p ) = − N X i =1 exp − 1 2 ( x ′ i − q i ) ⊤ Σ − 1 i ( x ′ i − q i ) (11) (a) (b) Figure 5. Normal distrib ution modeling of reference scan points in multi-dimensional v ox els: (a) NDT con v erts reference scan into normal distrib ution on each ND v ox els and (b) one reference scan point f alls into eight o v erlapped ND v ox els Here, x ′ i = T ( x i , p ) represents the coordinates of the current scan point x i after being transformed by T and mapped to the reference coordinate system. q i and Σ i denote the mean and co v ariance matrix, respecti v ely , of the corresponding NDT grid cell. By minimizing f ( p ) , the joint probability density of the current scan points in the reference NDT model is maximized, thereby achie ving precise alignment between the tw o scan frames. Ne wton’ s method is an iterati v e approach used for optimization problems, which relies on the second-order deri v ati v e information of a function to nd its optimal solution. Compared to rst-order gradient descent, Ne wton’ s method can con v er ge more qui ckly to the optimal soluti on by utilizi ng additional information pro vided by the Hessian matrix (i.e., the second-order deri v ati v e matrix of the objecti v e function). Ne wton’ s method updates the parameters p iterati v ely , and its k e y lies in the ef cient computation of both the rst-order gradient (gradient v ector) and the second-order deri v ati v e information (Hessian matrix) of the objecti v e function, as sho wn in Figure 6. The gradient v ector g represents the direction of descent of the objecti v e function at the current parameter . F or a single scan point x i , the gradient contrib ution is deri v ed using the chain rule: g i = exp − 1 2 q ⊤ Σ − 1 q · J ⊤ Σ − 1 q T (12) Here, q = x ′ i − q i , and J T represents the Jacobian matrix of the transformation T , which characterizes the local linear impact of the parameter p on the coordinates x ′ i . The total gradient g is obtained by summing the gradient contrib utions of all scan points. The Hessian matrix H describes the curv ature characteristics of the objecti v e function and directl y inuences the con v er gence speed and stability of Ne wton’ s method. Its elements are composed of second-order deri v ati v es, and the Hessian contrib ution of a single point is gi v en by: H i = exp − 1 2 q ⊤ Σ − 1 q h Σ − 1 q · J T ⊤ Σ − 1 q · J T − J ⊤ Σ − 1 J T T i (13) T o pre v ent con v er gence f ailure caused by a non-positi v e denite Hessian matrix, a damped Ne w- ton’ s method strate gy is emplo yed. When a non-positi v e denite Hessian matrix is detected, a damping term Real-time low-drift global optimization for dynamic scene LiD AR SLAM localization (P eiyan Y ang) Evaluation Warning : The document was created with Spire.PDF for Python.
8 ❒ ISSN: 2722-2586 λI ( λ > 0) is added to enforce positi v e deniteness, ensuring the v alidity of the iterati v e direction. The param- eter update in Ne wton’ s method is achie v ed by solving the linear equation: H ∆ p = − g (14) After obtaining the increment ∆ p , the parameters are updated as follo ws: p ← p + ∆ p (15) Figure 6. Frame w ork diagram of the optimized NDT algorithm proposed in this paper Algorithm 2: NDT objecti v e function Input: T Output: score Function ndt objective( T ) : tr ansf or med cl oud = tr ansf or m point cl oud ( F cur , T ) ; scor e = 0 ; f or p ∈ tr ansf or med cl oud do v oxel = g et v oxel ( p, l ocal map, ndt r esol ution ) ; if v oxel then mu = v oxel .mean ; sig ma = v oxel .cov a r iance ; dx = p − mu ; scor e + = − 0 . 5 ∗ dx T · np.l inal g .inv ( sig ma ) · dx ; end end r etur n − scor e ; initial g uess = T odom ; T ndt = optimiz e ( ndt obj ectiv e, initial g uess, method = ′ N ew ton ′ ) ; The enhanced performance of the proposed algorithm stems from the syner gistic inte gration of mul- tiple k e y components. The LeGO-LO AM feature e xtraction module f acilitates ef cient and rob ust initial pose estimation through the e xtraction of edge and planar features, supporting real-time processing. Nonetheless, its accurac y may de grade in en vironments characterized by lo w feature density or dynamic elem ents. T o mitig ate this limitation, the NDT -based weighted fusion module renes the initial pose by probabilistically modeling point cloud distrib utions and emplo ying Ne wton-optimized alignment, thereby signicantly impro ving re g- istration accurac y and rob ustness to dynamic disturbances. Moreo v er , the adapti v e thresholding mechanism IAES Int J Rob & Autom, V ol. 15, No. 1, March 2026: 1–20 Evaluation Warning : The document was created with Spire.PDF for Python.
IAES Int J Rob & Autom ISSN: 2722-2586 ❒ 9 dynamically modulates matching constraints to accommodate en vironmental v ariability and ef fecti v ely sup- press outliers induced by mo ving obstacles. Collecti v ely , these components enhance l ocalization accurac y and map consistenc y while maintaining real-time performance, as demonstrated through e xtensi v e e v aluations on public datasets and real-w orld scenarios. The proposed method runs at o v er 10 Hz on a standard CPU-GPU platform, with feature e xtraction and NDT optimization taking approximately 30 ms and 50 ms per frame, respecti v ely . This ensures real-time performance for mobile robot deplo yment. This study introduces a Ne wton-Raphson-optimized NDT frame w ork to enhance LeGO-LO AM’ s lo- calization accurac y and stability . By iterati v ely optimizing transformation parameters via gradient descent and Hessian approximation, our method dynamically adjusts v ox el-based distrib ution models to minimize re gistra- tion errors. The rened NDT algorithm addresses tw o critical limitations of traditional NDT : i) dynamic v ox el distortion caused by mo ving obstacles, and ii) error accumulation from feature matching inaccuracies. 4. EXPERIMENTS AND RESUL TS This study introduces LeGO-LO AM-NDT , an enhanced LiD AR odometry frame w ork, to v alidate the ef cac y of the optimized T -NDT re gistration and k e yframe optimization strate gy . Comprehensi v e e v aluations were conducted on the public KITTI dataset and in real-w orld outdoor en vironments to quantify impro v ements in trajectory accurac y , map consistenc y , and computational ef cienc y . 4.1. Public dataset benchmarking Figure 7 illustrates the KITTI dataset, which is jointly released by Karlsruhe Institute of T echnology (KIT) and T o yota T echnological Institute, serving as the primary benchmark. It includes synchronized Li- D AR, stereo camera, GPS/IMU data, and ground truth trajectories across urban, highw ay , and rural scenarios, co v ering di v erse en vironmental comple xities. Figure 7. KITTI e xperimental platform with sensors and trajectory path The positioning accurac y w as assessed using the e v o toolkit, which computes absolute trajectory error (APE) and relati v e pose error (RPE). APE quanties global consistenc y between estimated and ground-truth trajectories, while RPE e v aluates local consistenc y o v er short time interv als. As demonstrated in Figure 8, When emplo ying the baseline LeGO-LO AM algorithm, signicant error accumulation w as observ ed between the estimated and ground-truth trajectories, especially when in v olving sharp turns, comple x en vironmental features, and late stages of operation. Quantitati v e e v aluation with the e v o toolkit sho wed an A TE of 2.93 m and an RPE of 1.38 m,indicating progressi v e pose drift o v er e xtended runtime. Figure 8 presents the comparison of initial pose and odometry trajectories in Figure 8(a) and the t ime- dependent APE along the X, Y , and Z ax es in Figure 8(b) for both the baseline LeGO-LO AM and the optimized LeGO-LO AM-NDT methods. T able 1 sho ws the A TE, denes the Euclidean distance between the estimated trajectory and the true trajectory , and quanties the global positioning accurac y . The rened algorithm demon- strates three k e y impro v ements: i) Y -Axis Stability: LeGO-LO AM-NDT reduces position v ariance on the Y axis by approximately 78%, achie ving sub-centimeter consistenc y . ii) Global Consistenc y: The robot’ s a v erage error dropped from 1.37 m to 0.95 m, conrming superior trajectory consistenc y . Figure 9 compares the APE between the baseline LeGO-LO AM and the optimized LeGO-LO AM- NDT . Figure 9(a) illustrates the time-dependent APE uctuations across the trajectory frames, highlighting Real-time low-drift global optimization for dynamic scene LiD AR SLAM localization (P eiyan Y ang) Evaluation Warning : The document was created with Spire.PDF for Python.
10 ❒ ISSN: 2722-2586 the error v ariations o v er time for both algorithms. Figure 9(b) summarizes k e y statistical metrics of the APE, including maximum, mean, median, minimum, root mean square error (RMSE), and standard de viation, pro vid- ing a comprehensi v e quantitati v e comparis on. The unoptimized baseline e xhibits signicant error uctuations, with peak errors e xceeding 2.0 m in 11% of frames, indicating poor res istance to error propag ation. In contrast, the NDT -rened algorithm demonstrates three k e y impro v ements: (a) (b) Figure 8. Ev aluation of rened pose: (a) the initial pose/odometry trajectory comparison between le go-loam and le go-loam-ndt and (b) component errors along the xyz T able 1. Absolute trajectory error (A TE) En vironment Method Max Mean Median Min RMSE SSE STD KITTI Le go-loam 2.9267 1.37832 1.35702 0.43702 1.47101 2382.42 0.513916 KITTI Ours 1.65335 0.95335 0.965205 0.23487 1.00998 1123.08 0.333436 (a) (b) Figure 9. Comparison of metrics: (a) time-dependent APE and (b) summary of APE statistical metrics IAES Int J Rob & Autom, V ol. 15, No. 1, March 2026: 1–20 Evaluation Warning : The document was created with Spire.PDF for Python.