Dynamic object removed solid state laser radar and inertia coupled positioning and three-dimensional reconstruction method

Abstract

The invention relates to a dynamic object removed solid state laser radar and inertia coupled positioning and three-dimensional reconstruction method, and belongs to the field of three-dimensional scene positioning and mapping. According to the method, voxels serve as basic units, a local dynamic map composed of a plurality of recent frame observations is constructed, a hidden Markov model is adopted for modeling a voxel state updating process, probability updating is carried out on the occupancy state of the voxels through a state transition model and an observation model, and therefore modeling of the time sequence change characteristics of the voxels is achieved. Analyzing time sequence change characteristics of voxel observation in a sliding window, discriminating dynamic voxels and removing corresponding dynamic points, and performing tight coupling fusion with motion information provided by an inertial measurement unit only by using point clouds in static voxels as geometric constraints, so as to obtain motion information of the inertial measurement unit; therefore, high-precision positioning and stable mapping of the mobile carrier in a dynamic environment are realized.

Description

Technical Field

[0001] This invention belongs to the field of 3D scene localization and mapping, and relates to a method for localization and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal. Background Technology

[0002] Solid-state lidar-inertial simultaneous localization and mapping (SLAM) technology acquires environmental geometric information through lidar to achieve pose estimation and map construction for mobile vehicles. To improve the stability of the system under complex motion conditions, existing research has tightly coupled lidar and inertial measurement unit (IMU) to form laser-inertial SLAM methods, such as LIO technology based on extended Kalman filtering or nonlinear optimization frameworks, which can achieve high positioning accuracy in static or weakly dynamic environments.

[0003] However, most existing solid-state lidar-inertial SLAM methods are based on the assumption that the environment is static overall. In practical applications, dynamic objects such as pedestrians and vehicles are widely present, and the lidar point cloud contains a large number of dynamic observation points. If these dynamic points are directly used for pose estimation and map updating, inconsistent constraints can easily be introduced, leading to a decrease in positioning accuracy and causing map ambiguity or redundancy.

[0004] Existing technologies typically employ outlier removal methods based on residual thresholds or geometric consistency, or introduce semantic segmentation to identify dynamic targets. However, the former struggles to reliably distinguish between dynamic and static points in complex dynamic scenes, while the latter relies on large amounts of training data and computational resources, and is not tightly coupled with the laser inertial state estimation process.

[0005] Therefore, existing solid-state lidar-inertial SLAM technology still suffers from severe dynamic point interference, insufficient utilization of temporal information, and insufficient robustness in positioning and mapping under dynamic environments. There is an urgent need for a laser inertial SLAM method suitable for dynamic environments to solve these problems. Summary of the Invention

[0006] In view of this, existing solid-state lidar-inertial SLAM methods are susceptible to dynamic point interference, resulting in decreased positioning accuracy and mapping stability in dynamic environments. The purpose of this invention is to provide a positioning and 3D reconstruction method that couples solid-state lidar with inertial technology to remove dynamic objects. By effectively modeling and detecting dynamic observations in the lidar point cloud, stable fusion of lidar and inertial information can be achieved without relying on complex semantic information, thereby improving the positioning accuracy and map construction reliability of the system in complex dynamic scenes.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] A method for localization and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal, the method comprising: S1. Acquire lidar point cloud data and inertial measurement unit (IMU) data, and preprocess the data; S2. Transform the current LiDAR scan point cloud from the LiDAR coordinate system to the global coordinate system, perform voxelization on the global point cloud, discretize the continuous space into a regular voxel grid, and map each point to its corresponding voxel to obtain the current set of occupied voxels. S3. Using voxels as the basic unit, construct a local dynamic map composed of the most recent few frames of observations, and insert or update the observed voxels at the current moment according to the existence state of the voxels in the local dynamic map in order to update the local dynamic map. S4. In the updated local dynamic map, query the occupancy status of voxels located in the current frame point cloud, filter out potential dynamic voxels, calculate the spatiotemporal confidence of all potential dynamic voxels, and remove the corresponding dynamic points based on the spatiotemporal confidence. S5. The system pose is optimized based on the error state iterative extended Kalman filter algorithm to complete the localization and 3D reconstruction.

[0009] Furthermore, in step S1, the raw point cloud data collected by the lidar and the angular velocity and acceleration data output by the inertial measurement unit (IMU) are first acquired, and time synchronization and reference coordinate system transformation are performed on the two types of sensor data; then, the IMU data is preprocessed by integration, and the state of the lidar during the scanning cycle is estimated using the IMU measurement information.

[0010]

[0011] In the formula, These are state variables and belong to the manifold space. 18-dimensional variables; superscript " "Represents the global coordinate system, subscript" "Represents the IMU coordinate system at the current moment, This indicates the transpose operation. , These represent the rotation and translation matrices from the global coordinate system to the IMU coordinate system, respectively. This indicates the velocity of the IMU coordinate system in the global coordinate system; These represent the angular velocity random walk and acceleration random walk of the IMU, respectively. Represents gravitational acceleration; The information measured by the IMU, i.e., the control input, includes angular velocity information. and acceleration measurement information ; Indicates measurement noise. These represent angular velocity measurement noise, acceleration measurement noise, angular velocity random walk noise, and acceleration random walk noise, respectively.

[0012] In the Time, take advantage of The state at the next moment can be predicted. The sampling period is subscript " in the formula "All indicate the first" The value of the variable at any given moment. "" indicates generalized addition of the left and right variables, which mainly affects the composition of rotations.

[0013] Next, motion compensation is performed on the point cloud to obtain point cloud data at a unified reference time.

[0014] Furthermore, in step S2, the process specifically includes: At time step The pose of the lidar in the global coordinate system is estimated through preprocessing and forward propagation, and the distortion removal of the point cloud is completed by combining backpropagation to obtain the lidar scan point cloud at the current moment. Then, the point cloud is transformed from the lidar coordinate system to the global coordinate system to obtain a spatial representation in a unified coordinate system.

[0015] In the formula, For the global coordinate system The coordinates of the radar points For the radar coordinate system, the first The coordinates of the radar points , This represents the rotation and translation matrices from the global coordinate system to the radar coordinate system.

[0016] Then, the global point cloud is voxelized, discretizing the continuous space into a regular voxel grid. Each point is mapped to its corresponding voxel, resulting in the current set of occupied voxels. :

[0017] Occupying voxel set Used to describe spatial areas in the environment that are directly observed by lidar; Finally, a ray projection operation is performed to characterize a more complete observation space, in which rays are emitted from the voxel containing the origin of the lidar along the direction of the laser beam to each occupying voxel, and voxels along the ray path except for the endpoint voxel are marked as free voxels. The obtained occupied voxels and free voxels are used to construct the observation voxel set corresponding to the current lidar point cloud. The observation voxel set is used as the input for subsequent local dynamic map updates.

[0018] Furthermore, in step S3, each voxel in the constructed local dynamic map not only records its spatial location but also maintains statistical information related to the dynamic state, which is represented as follows:

[0019] in, Let be the voxel state vector, and , The probability of being in an idle state. The probability of occupying a state; The most recent observation time, Indicates the observation interval. For dynamic markers; For the current set of observed voxels, if a voxel does not yet exist in the local dynamic map, it is initialized and directly inserted into the map; if the voxel already exists, its most recent observation time is updated, and the voxel state is recursively updated based on historical state and current observation information.

[0020] Furthermore, when recursively updating voxel states based on historical and current observation information, a Hidden Markov Model (HMM) is used to model the voxel state update process. The voxel occupancy state is probabilistically updated through a state transition model and an observation model. Specifically, the range variance is modeled using error propagation theory, comprehensively considering point cloud measurement noise and lidar pose estimation errors to obtain a more robust voxel occupancy observation probability.

[0021] In the formula, Represents the normalization coefficient. The state transition probability; To observe the likelihood, it is based on voxels. Euclidean distance to the nearest voxel The calculation is performed, and its expression is an exponential function based on a Gaussian distribution:

[0022] In the formula, Euclidean distance The uncertainty, when the distance At that time, the observed likelihood takes the value of 1; Uncertainty Calculations are performed using an error propagation model:

[0023] In the formula, For Euclidean distance Jacobian matrix relative to voxel coordinates The covariance of a voxel in the global coordinate system is determined by the rotational uncertainty, translational uncertainty, and spatial distribution of the point in the lidar coordinate system.

[0024] In the same frame of laser scanning, all occupying voxels share the same covariance.

[0025] Furthermore, in step S4, after completing the local map update, the occupancy status of the voxels located in the current frame point cloud is queried. If the voxel was idle in the previous time step and is occupied in the current time step, it is judged as a potential dynamic voxel. Spatiotemporal confidence is calculated for all potential dynamic voxels. The calculation method is to count the number of potential dynamic voxels in the spatial neighborhood voxels and the number of potential voxels in the neighborhood voxels in the past several time periods, which is used as the dynamic confidence of the potential dynamic voxels. The confidence level is segmented according to the Ostu algorithm. Potential dynamic voxels with a confidence level greater than the segmentation threshold are identified as dynamic voxels, and the LiDAR points in the dynamic voxels are identified as dynamic points. In the solid-state LiDAR, each radar point has a unique number. Points with non-dynamic point numbers are retained from a complete radar point cloud frame as static point clouds to participate in pose optimization.

[0026] Furthermore, in step S5, after completing the dynamic point cloud detection and removal, the pose is optimized based on the error state iterative extended Kalman filter algorithm. Specifically, by fusing IMU constraints and generalized iterative nearest-point constraints, a maximum a posteriori cost function is constructed.

[0027] in Indicates about state The objective function to be minimized; the first term accumulates the residuals generated by GICP matching between static points and map points. And by the corresponding information matrix Weighted; the second term is the prior residual. , used to constrain the state using inertial measurements, where represents its information matrix; the weighted square norm is defined as ; The maximum a posteriori cost function, combined with constraints on geometric consistency and prior information between point clouds, enables more robust and stable pose estimation.

[0028] Furthermore, step S5 also includes introducing a line search mechanism in the error state iterative Kalman filtering, whereby the state is updated in each iteration as follows:

[0029] in The weight of the current update amount, Indicates the scaling factor. The Armijo condition is defined as follows:

[0030] In the formula, This indicates the objective function in the current state. The gradient at the given point; the step size. Initially set to 1, and determined using a line search method; if the Armijo condition is not satisfied, then... The step size is decreased by the scaling factor until the condition is met or the maximum number of iterations is reached.

[0031] The beneficial effects of this invention are as follows: This invention employs a Hidden Markov Model (HMM) for modeling. By using a state transition model and an observation model to probabilistically update the occupancy state of voxels, it achieves modeling of the temporal variation characteristics of voxels. Within a sliding window, it analyzes the temporal variation characteristics of voxel observations, identifies dynamic voxels, and removes their corresponding dynamic points. It utilizes only the point cloud in static voxels as geometric constraints and tightly couples and fuses it with the motion information provided by the inertial measurement unit, thereby achieving high-precision positioning and stable mapping of moving carriers in dynamic environments.

[0032] Compared with existing technologies, this invention introduces voxel-level dynamic state modeling and combines multi-frame temporal information to discriminate and suppress dynamic observations, effectively reducing the interference of dynamic points on laser inertial state estimation and improving the positioning accuracy and stability of the system in dynamic environments. At the same time, it only uses static point clouds for map updates, avoiding dynamic objects from entering the map, thereby improving the consistency and usability of the map. Moreover, it does not rely on complex semantic information and has good real-time performance and engineering applicability.

[0033] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0034] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein: Figure 1 This is a schematic diagram of the overall architecture of a solid-state lidar and inertial coupling positioning and 3D reconstruction method for dynamic object removal according to an embodiment of the present invention. Detailed Implementation

[0035] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0036] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0037] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0038] Please see Figure 1 This is a method for localization and 3D reconstruction using solid-state lidar coupled with inertial coupling for dynamic object removal.

[0039] Example This embodiment details the implementation process of a localization and 3D reconstruction method using a solid-state lidar coupled with inertial coupling for dynamic object removal. Figure 1 As shown, it specifically includes the following steps: S1. Acquire lidar point cloud data and inertial measurement unit (IMU) data, and preprocess the data; S2. Transform the current LiDAR scan point cloud from the LiDAR coordinate system to the global coordinate system, perform voxelization on the global point cloud, discretize the continuous space into a regular voxel grid, and map each point to its corresponding voxel to obtain the current set of occupied voxels. S3. Using voxels as the basic unit, construct a local dynamic map composed of the most recent few frames of observations, and insert or update the observed voxels at the current moment according to the existence state of the voxels in the local dynamic map in order to update the local dynamic map. S4. In the updated local dynamic map, query the occupancy status of voxels located in the current frame point cloud, filter out potential dynamic voxels, calculate the spatiotemporal confidence of all potential dynamic voxels, and remove the corresponding dynamic points based on the spatiotemporal confidence. S5. The system pose is optimized based on the error state iterative extended Kalman filter algorithm to complete the localization and 3D reconstruction.

[0040] In step S1 of this embodiment, the raw point cloud data collected by the lidar and the angular velocity and acceleration data output by the inertial measurement unit (IMU) are first acquired. Time synchronization and reference coordinate system transformation are performed on the two types of sensor data to ensure data consistency under the same time reference and coordinate system. Subsequently, the IMU data undergoes integral preprocessing, and the state of the lidar within the scanning cycle is estimated using the IMU measurement information, as shown below:

[0041]

[0042] In the formula, These are state variables and belong to the manifold space. 18-dimensional variables; superscript " "Represents the global coordinate system, subscript" "Represents the IMU coordinate system at the current moment, This indicates the transpose operation. , These represent the rotation and translation matrices from the global coordinate system to the IMU coordinate system, respectively. This indicates the velocity of the IMU coordinate system in the global coordinate system; These represent the angular velocity random walk and acceleration random walk of the IMU, respectively. Represents gravitational acceleration; The information measured by the IMU, i.e., the control input, includes angular velocity information. and acceleration measurement information ; Indicates measurement noise. These represent angular velocity measurement noise, acceleration measurement noise, angular velocity random walk noise, and acceleration random walk noise, respectively.

[0043] In the Time, take advantage of The state at the next moment can be predicted. The sampling period is subscript " in the formula "All indicate the first" The value of the variable at any given moment. "" indicates generalized addition of the left and right variables, which mainly affects the composition of rotations.

[0044] Next, motion compensation is performed on the point cloud to obtain point cloud data at a unified reference time, providing reliable input for subsequent dynamic discrimination and laser-inertial joint state estimation.

[0045] In step S2 of this embodiment, Dynamic Object Removal (DOR) aims to identify and remove dynamic points from the current LiDAR scan, thereby reducing the interference of dynamic objects on pose estimation and mapping processes, and improving the system's robustness and mapping accuracy in dynamic environments. This module mainly consists of three parts: voxelization and ray casting, local dynamic map representation and updating, and dynamic point identification.

[0046] At time step Following the standard processing procedure, the pose of the lidar in the global coordinate system is first estimated through preprocessing and forward propagation, and then point cloud distortion correction is performed using backpropagation to obtain the lidar scan point cloud at the current moment. Subsequently, the point cloud is transformed from the lidar coordinate system to the global coordinate system to obtain a spatial representation in a unified coordinate system.

[0047] In the formula, For the global coordinate system The coordinates of the radar points For the radar coordinate system, the first The coordinates of the radar points , This represents the rotation and translation matrices from the global coordinate system to the radar coordinate system.

[0048] Based on this, the global point cloud is voxelized, discretizing the continuous space into a regular voxel grid, with each point mapped to its corresponding voxel. This yields the current set of occupied voxels. Used to describe spatial regions in the environment that are directly observed by lidar:

[0049] After voxelization, a ray projection operation is performed to characterize a more complete observation space. Specifically, starting from the voxel containing the lidar origin, rays are emitted along the laser beam direction towards each occupied voxel, and voxels along the ray path, except for the endpoint voxel, are marked as free voxels. Through this process, both occupied and free voxels can be obtained simultaneously, thus constructing the observation voxel set corresponding to the current scan. This observation voxel set will serve as input for subsequent local dynamic map updates.

[0050] In step S3 of this embodiment, to assist in the detection of dynamic points in the current lidar scan, a local dynamic map composed of the most recent few frames of observations is constructed. This map uses voxels as basic units; each voxel not only records its spatial location but also maintains statistical information related to the dynamic state, including the voxel's occupancy status. Recent observation time and dynamic markers wait:

[0051] in, Let be the voxel state vector, and .

[0052] For the current set of observed voxels, if a voxel does not yet exist in the local dynamic map, it is initialized and directly inserted into the map; if the voxel already exists, its most recent observation time is updated, and the voxel state is recursively updated based on historical state and current observation information.

[0053] The voxel state update process is modeled using a Hidden Markov Model (HMM). The occupancy state of voxels is probabilistically updated through a state transition model and an observation model, thereby modeling the temporal changes in voxels. This mechanism effectively characterizes the continuous occupancy and non-occupancy processes of voxels, providing a foundation for subsequent dynamic detection. To reasonably describe the impact of distance uncertainty on observation likelihood, the distance variance is modeled using error propagation theory, comprehensively considering point cloud measurement noise and lidar pose estimation errors. This approach allows for a more robust voxel occupancy observation probability even in the presence of pose uncertainty.

[0054]

[0055] In the formula, Represents the normalization coefficient. The state transition probability; To observe the likelihood, it is based on voxels. Euclidean distance to the nearest voxel The calculation is performed, and its expression is an exponential function based on a Gaussian distribution:

[0056] In the formula, Euclidean distance The uncertainty, when the distance At that time, the observed likelihood takes the value of 1; Uncertainty Calculations are performed using an error propagation model:

[0057] In the formula, For Euclidean distance Jacobian matrix relative to voxel coordinates The covariance of a voxel in the global coordinate system is determined by the rotational uncertainty, translational uncertainty, and spatial distribution of the point in the lidar coordinate system.

[0058] In the same frame of laser scanning, all occupying voxels share the same covariance.

[0059] In step S4 of this embodiment, after updating the local map, the occupancy status of the voxels located in the current frame point cloud is queried. If the voxel state was idle at the previous moment ( >0.99) and the current state is occupied ( A value >0.99 is considered a potential dynamic voxel. Then, spatiotemporal confidence is calculated for all potential dynamic voxels. Specifically, the number of potential dynamic voxels in their spatial neighborhood (27) and the number of potential voxels in their neighborhood voxels over the past two time points are counted as the dynamic confidence of the potential dynamic voxel.

[0060] The Ostu algorithm is used to segment the radar based on confidence levels. Potential dynamic voxels with confidence levels greater than a segmentation threshold are identified as dynamic voxels, and radar points located within these voxels are identified as dynamic points. In solid-state lidar, each radar point corresponds to a unique number. Points with non-dynamic point numbers are retained from a complete radar point cloud frame as static point clouds for pose optimization.

[0061] In step S5 of this embodiment, after completing dynamic point cloud detection and removal, the present invention optimizes the system pose based on Error-State Iterated Extended Kalman Filter (ESIKF). By fusing IMU constraints and Generalized Iterative Closest Point (GICP) constraints, a Maximum a Posteriori (MAP) cost function is constructed:

[0062] in Indicates about state The objective function to be minimized. The first term accumulates the residuals generated by GICP matching between static points and map points. And by the corresponding information matrix Weighted. The second term is the prior residual. , used to constrain the state using inertial measurements, where represents its information matrix. The weighted square norm is defined as This modeling approach jointly constrains the geometric consistency and prior information among point clouds, thereby achieving more robust and stable pose estimation.

[0063] To alleviate the degradation problem under conditions of limited static observations, this invention introduces a line search mechanism into the Error-State Iterative Kalman Filter (ESIKF). Specifically, the state is updated in each iteration as follows:

[0064] in The weight of the current update amount, Indicates the scaling factor. The Armijo condition is defined as follows:

[0065] here, This indicates the objective function in the current state. The gradient at that point. Step size. Initially set to 1, and determined using a line search method; if the Armijo condition is not satisfied, then... The step size is decreased by a scaling factor until the condition is met or the maximum number of iterations is reached, thus ensuring that each state update leads to a sufficient decrease in the objective function. By introducing LS-ESIKF, not only is the optimization efficiency improved, but the positioning accuracy is also further enhanced.

[0066] In general, this invention first generates a distortion-free lidar point cloud using lidar and IMU data through preprocessing and forward and backward propagation modules; then, a dynamic object removal module is used to detect and remove dynamic points in the current scan; finally, the state estimation module uses an error state iterative Kalman filter with an introduced line search mechanism to optimize the system state.

[0067] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for localization and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal, characterized in that: The method includes: S1. Acquire lidar point cloud data and inertial measurement unit (IMU) data, and use the IMU measurement information to estimate the state of the lidar during the scanning cycle; S2. Transform the current LiDAR scan point cloud from the LiDAR coordinate system to the global coordinate system, perform voxelization on the global point cloud, discretize the continuous space into a regular voxel grid, and map each point to its corresponding voxel to obtain the current set of occupied voxels. S3. Using voxels as the basic unit, construct a local dynamic map composed of the most recent few frames of observations, and insert or update the observed voxels at the current moment according to the existence state of the voxels in the local dynamic map in order to update the local dynamic map. S4. In the updated local dynamic map, query the occupancy status of voxels located in the current frame point cloud, filter out potential dynamic voxels, calculate the spatiotemporal confidence of all potential dynamic voxels, and remove the corresponding dynamic points based on the spatiotemporal confidence. S5. The system pose is optimized based on the error state iterative extended Kalman filter algorithm to complete the localization and 3D reconstruction.

2. The method for positioning and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal according to claim 1, characterized in that: In step S1, the raw point cloud data collected by the lidar and the angular velocity and acceleration data output by the inertial measurement unit (IMU) are first acquired, and time synchronization and reference coordinate system transformation are performed on the two types of sensor data. Then, the IMU data is preprocessed by integration, and the state of the lidar during the scanning cycle is estimated using the IMU measurement information. In the formula, These are state variables and belong to the manifold space. 18-dimensional variables; superscript " "Represents the global coordinate system, subscript" "Represents the IMU coordinate system at the current moment, Indicates the transpose operation; , These represent the rotation and translation matrices from the global coordinate system to the IMU coordinate system, respectively. This indicates the velocity of the IMU coordinate system in the global coordinate system; These represent the angular velocity random walk and acceleration random walk of the IMU, respectively. Represents gravitational acceleration; The information measured by the IMU, i.e., the control input, includes angular velocity information. and acceleration measurement information ; Indicates measurement noise. These represent angular velocity measurement noise, acceleration measurement noise, angular velocity random walk noise, and acceleration random walk noise, respectively. In the Time, take advantage of The state at the next moment can be predicted. The sampling period is subscript " in the formula "All indicate the first" The value of the corresponding variable at any given time, "Indicates the generalized addition of the left and right variables, affecting the composition of the rotation; Next, motion compensation is performed on the point cloud to obtain point cloud data at a unified reference time.

3. The method for positioning and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal according to claim 2, characterized in that: In step S2, the process specifically includes: At time step The pose of the lidar in the global coordinate system is estimated through preprocessing and forward propagation, and the distortion removal of the point cloud is completed by combining backpropagation to obtain the lidar scan point cloud at the current moment. Then, the point cloud is transformed from the lidar coordinate system to the global coordinate system to obtain a spatial representation in a unified coordinate system. In the formula, For the global coordinate system The coordinates of the radar points For the radar coordinate system, the first The coordinates of the radar points , The rotation and translation matrices from the global coordinate system to the radar coordinate system; Then, the global point cloud is voxelized, discretizing the continuous space into a regular voxel grid. Each point is mapped to its corresponding voxel, resulting in the current set of occupied voxels. : Occupying voxel set Used to describe spatial areas in the environment that are directly observed by lidar; Finally, a ray projection operation is performed to characterize a more complete observation space, in which rays are emitted from the voxel containing the origin of the lidar along the direction of the laser beam to each occupying voxel, and voxels along the ray path except for the endpoint voxel are marked as free voxels. The obtained occupied voxels and free voxels are used to construct the observation voxel set corresponding to the current lidar point cloud. The observation voxel set is used as the input for subsequent local dynamic map updates.

4. The method for positioning and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal according to claim 3, characterized in that: In step S3, each voxel in the constructed local dynamic map not only records its spatial location but also maintains statistical information related to the dynamic state, which is represented as follows: in, Let be the voxel state vector, and , The probability of being in an idle state. The probability of occupying a state; The most recent observation time, Indicates the observation interval. For dynamic markers; For the current set of observed voxels, if a voxel does not yet exist in the local dynamic map, it is initialized and directly inserted into the map; if the voxel already exists, its most recent observation time is updated, and the voxel state is recursively updated based on historical state and current observation information.

5. The method for positioning and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal according to claim 4, characterized in that: When recursively updating voxel states based on historical and current observation information, a Hidden Markov Model (HMM) is used to model the voxel state update process. The voxel occupancy state is probabilistically updated through a state transition model and an observation model. Furthermore, the range variance is modeled using error propagation theory, comprehensively considering point cloud measurement noise and lidar pose estimation errors to obtain a more robust voxel occupancy observation probability. In the formula, Represents the normalization coefficient. The state transition probability; To observe the likelihood, it is based on voxels. Euclidean distance to the nearest voxel The calculation is performed, and its expression is an exponential function based on a Gaussian distribution: In the formula, Euclidean distance The uncertainty, when the distance At that time, the observed likelihood takes the value of 1; Uncertainty Calculations are performed using an error propagation model: In the formula, For Euclidean distance Jacobian matrix relative to voxel coordinates The covariance of a voxel in the global coordinate system is determined by the rotational uncertainty, translational uncertainty, and spatial distribution of the point in the lidar coordinate system. In the same frame of laser scanning, all occupying voxels share the same covariance.

6. The method for positioning and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal according to claim 5, characterized in that: In step S4, after updating the local map, the occupancy status of the voxels in the current frame point cloud is queried. If the voxel was idle in the previous time step and is occupied in the current time step, it is judged as a potential dynamic voxel. Spatiotemporal confidence is calculated for all potential dynamic voxels. The calculation method is to count the number of potential dynamic voxels in the spatial neighborhood voxels and the number of potential voxels in the neighborhood voxels in the past several time periods, which is used as the dynamic confidence of the potential dynamic voxels. The confidence level is segmented according to the Ostu algorithm. Potential dynamic voxels with a confidence level greater than the segmentation threshold are identified as dynamic voxels, and the LiDAR points in the dynamic voxels are identified as dynamic points. In the solid-state LiDAR, each radar point has a unique number. Points with non-dynamic point numbers are retained from a complete radar point cloud frame as static point clouds to participate in pose optimization.

7. The method for positioning and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal according to claim 6, characterized in that: In step S5, after completing dynamic point cloud detection and removal, the pose is optimized based on the error state iterative extended Kalman filter algorithm. Specifically, by fusing IMU constraints and generalized iterative nearest-point constraints, a maximum a posteriori cost function is constructed. in Indicates about state The objective function to be minimized; the first term accumulates the residuals generated by GICP matching between static points and map points. And by the corresponding information matrix Weighted; the second term is the prior residual. , used to constrain the state using inertial measurements, where represents its information matrix; the weighted square norm is defined as ; The maximum a posteriori cost function, combined with constraints on geometric consistency and prior information between point clouds, enables more robust and stable pose estimation.

8. The method for positioning and 3D reconstruction using a solid-state lidar coupled with inertial coupling for dynamic object removal according to claim 7, characterized in that: Step S5 also includes introducing a line search mechanism in the error state iterative Kalman filtering, where the state is updated in each iteration as follows: in The weight of the current update amount, Indicates the scaling factor. The Armijo condition is defined as follows: In the formula, This indicates the objective function in the current state. gradient at point; step size Initially set to 1, and determined using a line search method; if the Armijo condition is not satisfied, then... The step size is decreased by the scaling factor until the condition is met or the maximum number of iterations is reached.

Images