Lidar-inertial odometry method and system based on sp model and mscikf filtering

Abstract

The application relates to a laser radar-inertial odometer method and system based on an SP model and an MSCIKF filter, and belongs to the field of integrated navigation. The method comprises the following steps: after system initialization, correcting laser point cloud distortion, extracting plane / line features from the point cloud, performing frame-map matching to solve a relative pose, determining a key frame and inserting the key frame into a sliding window, parameterizing plane / line features extracted from the key frame in the sliding window based on a spherical polar projection model, constructing distance-angle weighted residuals in a spherical coordinate domain, fixing the sliding window pose to alternately estimate feature parameters and projection point parameters, calculating linearized residuals for the key frame in the sliding window and stacking the same feature observations, adopting two-stage projection to eliminate feature parameters and reserve pose constraints, iteratively calculating gain and state updating in an MSCIKF filtering framework, outputting optimized poses and maintaining the sliding window. The application can effectively reduce long-term drift of an odometer while keeping a low calculation amount.

Description

Technical Field

[0001] This invention relates to the field of integrated navigation technology, and in particular to a lidar-inertial odometry method and system based on the SP model and MSCIKF filtering. Background Technology

[0002] 3D LiDAR SLAM (Simultaneous Localization and Mapping) achieves vehicle pose estimation and map construction by perceiving the geometric structure of the environment, and has been widely applied in scenarios such as unmanned vehicles, mobile robots, and mobile mapping. Many existing LiDAR odometry / mapping methods employ a registration strategy from scan frames to local maps, typically extracting geometric features such as planes or edges and constructing geometric residuals from points to planes and points to lines to iteratively solve for the pose of the current frame. While these methods are simple to implement and robust in engineering, they often treat the local map or feature model as deterministic quantities in subsequent incremental updates, lacking a unified expression and propagation of uncertainties between historical pose, map, and new observations. This leads to cumulative drift during long-distance operation, especially noticeable in structured environments or regions with weak geometric constraints.

[0003] To reduce drift, clustering adjustment methods can jointly optimize pose and environmental feature parameters across multiple frames, introducing multi-view constraints to improve global consistency. However, explicitly maintaining feature parameters leads to a simultaneous increase in the dimensionality of optimization variables and the number of features, resulting in high computational burden, insufficient real-time performance, and unfavorable deployment on mobile platforms. Multi-state constrained Kalman filtering methods improve efficiency by implicitly eliminating feature parameters from optimization through elimination. However, in LiDAR scenarios, if point clouds are directly treated as Cartesian coordinate measurements and simply assumed to be independent and identically distributed Gaussian noise, it is difficult to accurately reflect the ranging-angle measurement mechanism of LiDAR: point cloud noise often varies with distance and scanning direction, and its spatial distribution is not uniform. Simple assumptions will cause uncertainty modeling bias, thus affecting estimation accuracy and consistency.

[0004] Furthermore, lidar features (planar / line) typically contain a large number of points. If high-dimensional joint estimation is performed directly on point-level observations within a multi-view constraint framework, the computational cost is high. If feature uncertainties need to be recalculated after each pose update, it will also incur additional time consumption, making it difficult for the system to meet real-time requirements while maintaining high accuracy. Summary of the Invention

[0005] Based on the above analysis, this invention aims to disclose a lidar-inertial odometry method and system based on the SP model and MSCIKF filtering, which solves the technical problems of existing lidar odometry / mapping methods, such as long-distance operation cumulative drift, uncertainty modeling bias, large computational burden, and insufficient real-time performance, making it difficult to meet real-time requirements while maintaining high accuracy.

[0006] This invention discloses a lidar-inertial odometry method based on the SP model and MSCIKF filtering, comprising:

[0007] S1. Perform system initialization, calibrate the external parameters of the lidar and IMU, set the sensor noise parameters, initialize the IMU state and covariance, and configure the sliding window size;

[0008] S2. Predict pose and correct laser point cloud distortion using IMU integration; extract planar / line features from point cloud; perform frame-map matching to solve relative pose, determine keyframes based on pose change thresholds and insert sliding windows;

[0009] S3. Based on the parametric sliding window parameterized by the spherical projection model, the planar / line features extracted from the keyframes are constructed in the spherical coordinate domain to construct distance-angle weighted residuals; the sliding window pose is fixed to alternately estimate the feature parameters and projection point parameters.

[0010] S4. Calculate linearized residuals for keyframes within the sliding window and stack observations with the same features. Perform point-level fast projection in non-final rounds and feature-level left null space projection in the final round to eliminate feature parameters and preserve pose constraints.

[0011] S5. Based on the post-projection observation model, the MSCIKF filtering framework is used to iteratively calculate the gain and state update, output the optimized pose and maintain the sliding window.

[0012] This invention discloses a lidar-inertial odometry system for implementing the lidar-inertial odometry method based on the SP model and MSCIKF filtering as described above. The system includes a lidar, an IMU sensor, and a processor, wherein the processor is configured as follows:

[0013] The system initialization module is used to calibrate the external parameters of the lidar and IMU, set the sensor noise parameters, initialize the IMU state and covariance, and configure the sliding window size.

[0014] The front-end motion estimation module is used to predict pose and correct laser point cloud distortion using IMU integration; extract planar / line features from the point cloud; perform frame-map matching to solve relative pose, determine keyframes based on pose change thresholds, and insert sliding windows.

[0015] The feature parameter estimation module is used to parameterize the planar / line features extracted from keyframes in the sliding window based on the spherical projection model, and to construct distance-angle weighted residuals in the spherical coordinate domain; the sliding window pose is fixed to alternately estimate feature parameters and projection point parameters;

[0016] The feature elimination module is used to calculate the linearized residuals of each keyframe within the sliding window and stack the observations with the same feature. In non-final rounds, it performs point-level fast projection, and in the final round, it performs feature-level left null space projection to eliminate feature parameters and preserve pose constraints.

[0017] The iterative update module is used to iteratively calculate the gain and state update based on the post-projection observation model using the MSCIKF filtering framework, output the optimized pose and maintain the sliding window.

[0018] The beneficial effects that this invention can achieve are:

[0019] The lidar-inertial odometry method and system disclosed in this invention, based on the SP model and MSCIKF filtering, effectively reduces long-term odometry drift while maintaining low computational load through precise and efficient feature parameterization modeling and state estimation methods. It is expected to provide broad technical support for applications such as autonomous driving and robot positioning and navigation. Attached Figure Description

[0020] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals refer to the same parts.

[0021] Figure 1 This is a flowchart of the lidar-inertial odometry method in an embodiment of the present invention;

[0022] Figure 2 This is a schematic diagram of spherical projection in an embodiment of the present invention;

[0023] Figure 3 This is a schematic diagram of the planar / line feature spherical projection parameterization of the lidar point cloud in an embodiment of the present invention;

[0024] Figure 4 This is a schematic diagram showing the composition and connection of the lidar-inertial odometry system in an embodiment of the present invention. Detailed Implementation

[0025] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and, together with the embodiments of the present invention, serve to illustrate the principles of the present invention.

[0026] Example 1

[0027] This embodiment discloses a lidar-inertial odometry method based on the SP (Stereographic Projection) model and MSCIKF (Multi-State Constrained Iterated Kalman Filter) filtering, such as... Figure 1As shown, it includes:

[0028] S1. Perform system initialization, calibrate the external parameters of the lidar and IMU, set the sensor noise parameters, initialize the IMU state and covariance, and configure the sliding window size;

[0029] S2. Predict pose and correct laser point cloud distortion using IMU integration; extract planar / line features from point cloud; perform frame-map matching to solve relative pose, determine keyframes based on pose change thresholds and insert sliding windows;

[0030] S3. Based on the parametric sliding window parameterized by the spherical projection model, the planar / line features extracted from the keyframes are constructed in the spherical coordinate domain to construct distance-angle weighted residuals; the sliding window pose is fixed to alternately estimate the feature parameters and projection point parameters.

[0031] S4. Calculate the linearized residuals for each keyframe within the sliding window and stack observations with the same features. Perform point-level fast projection in non-final rounds and feature-level left null space projection in the final round to eliminate feature parameters and preserve pose constraints.

[0032] S5. Based on the post-projection observation model, the MSCIKF filtering framework is used to iteratively calculate the gain and state update, output the optimized pose and maintain the sliding window.

[0033] Specifically, system initialization is performed in S1, including setting and initializing coordinate system extrinsic parameters; among which,

[0034] The coordinate system includes the global coordinate system. IMU coordinate system LiDAR coordinate system Since the installation locations of the lidar and IMU sensors cannot be perfectly aligned, the external parameters of the lidar and IMU must be calibrated and set before the system is put into operation. This parameter allows for the conversion between the two coordinate systems. Odometry is a form of self-motion estimation and does not involve absolute coordinates. Therefore, the origin of the coordinate system is usually the location where the system acquires the first frame of lidar data, i.e., the global coordinate system.

[0035] Initialization includes setting IMU noise parameters and lidar ranging / angle measurement noise parameters, and initializing the IMU state. and its covariance At the same time, set the size of the sliding window used by MSCIKF. .

[0036] The IMU state is represented as follows: This means that the system at time The IMU state definition, where For location, The rotation matrix in attitude quaternion form , For speed, These are the accelerometer and gyroscope biases, respectively; generally, the IMU state is set to a state to be optimized. Specifically, with... This represents the set of all IMU states within the sliding window.

[0037] Specifically, front-end motion estimation is performed in S2, including IMU integral prediction and local tracking by lidar;

[0038] The system continuously receives LiDAR scan point cloud data and simultaneously acquires the corresponding IMU high-frequency measurement sequences. Integral prediction is performed using the IMU measurement data to correct motion distortion in the LiDAR point cloud data, and the predicted state is used as the initial value for estimating the local tracking state of the LiDAR.

[0039] The local tracking method of the lidar first extracts planar / line candidate feature points from the current distorted point cloud, then performs scan frame to local map matching to calculate the pose of the current frame relative to the local map. If the frame has a certain change relative to the local map, it is inserted as a keyframe into the sliding window for the next step; otherwise, it continues to wait for the next frame for local tracking.

[0040] Specifically, S3 performs point cloud feature spherical projection representation and parameter estimation; including:

[0041] S3-1. Construct a spherical polar projection parameterization tool; map three-dimensional unit vectors to two-dimensional parameters without redundancy, and simultaneously construct orthogonal tangential bases to achieve stable representation;

[0042] Mapping the three-dimensional unit vector of the lidar measurement point to a non-redundant two-dimensional parameter Simultaneous construction of orthogonal tangential bases ;

[0043] like Figure 2 The diagram shown is a schematic of a spherical projection; in the diagram,

[0044] The spherical projection will remove the North Pole from the sphere. The three-dimensional unit vector on the map is mapped to a two-dimensional parameter without redundancy. This allows radar-measured points to be directly represented as points on a plane or straight line, more closely resembling the physical measurement model of lidar ranging and angle measurement, facilitating the separate weighting of distance and angle in the residuals. Furthermore, compared to directly using latitude and longitude as angle parameters, the spherical polar projection (SP) significantly reduces numerical instability caused by pole singularities and angular discontinuities, and simultaneously constructs orthogonal tangential bases. This achieves coplanar / collinear parameterization of planar / linear features, making the Jacobian more regular and the linearization smoother, thereby improving the stability and real-time performance of estimator iterative updates and null space elimination. (See figure.) Let be the tangent space, and let represent the plane containing the tangent vector.

[0045] S3-2. Establish a feature geometry parameterization model based on parameterization tools; planar features are defined by spherical projection parameters and intercepts, and line features are defined by projection parameters and origin moments. Local points are transformed to the global coordinate system by mapping normal vectors / direction vectors and tangential basis vectors.

[0046] Specifically, such as Figure 3 As shown, this is a schematic diagram of the parametric projection of planar / line features of a lidar point cloud; where,

[0047] Plane Feature Represented as: ;

[0048] in, planar features The two-dimensional spherical projection parameter vector, , planar features The transverse and longitudinal components of the normal unit vector after spherical polar projection;

[0049] For plane The intercept; via spherical projection function After transformation, planar features are obtained. normal vector ;

[0050] For planar features On point In the global coordinate system, it is expressed as:

[0051] ;

[0052] Line Feature Represented as ;

[0053] in, Line features The two-dimensional spherical projection parameter vector, Line features The transverse and longitudinal components of the spherical projection of the direction vector; Line features The transverse and longitudinal components of the origin moment parameter;

[0054] Line features The direction is from Sure;

[0055] Line features The origin moment is given by Sure;

[0056] For line features Points on , For online ordering The local position; expressed in the global coordinate system as:

[0057] .

[0058] S3-3. Weighted squared cost function of observation residuals based on geometric parameterization model; Inversely project global feature points to radar coordinate system, calculate the weighted squared difference between theoretical range / angle values ​​and actual observed values, including range, azimuth, and elevation angles;

[0059] For the first in the sliding window The first frame The first planar / line feature Coordinates of observation points The formula for transforming from local coordinates to global coordinates is:

[0060] ;

[0061] in, , For the rotation matrix and translation vector in the extrinsic parameters of the laser radar to the IMU; , For the first Rotation matrix and translation vector from frame IMU coordinate system to global coordinate system;

[0062] Then the coordinates of the observation point The weighted residual cost function is:

[0063] ;

[0064] in, These represent the standard deviations of ranging noise, azimuth noise, and elevation noise, respectively. , , These represent the distance extraction function, azimuth extraction function, and elevation extraction function, respectively. For the global coordinate system to the first Inverse transformation of the frame lidar coordinate system For observation point The coordinates of the predicted point in the global coordinate system after modeling by spherical projection.

[0065] S3-4. Perform alternating iterative optimization to solve the unknown parameters in the cost function; in each iteration, fix the IMU state and sliding window pose, and alternately estimate the feature parameters and point projection coordinates.

[0066] Given unknown planar / line feature parameters and the corresponding projected coordinates of each point, the feature parameters are estimated first, and then the projected point parameters are estimated, under the condition of fixed IMU state and sliding window pose.

[0067] Preferably, the feature parameters are first estimated by least squares, and then the projection parameters of each point are calculated, which converts the point-to-projection association into an explicit residual that can be used for filtering updates.

[0068] Specifically, S4 includes:

[0069] S4-1. Construct the three-dimensional observation residual vector;

[0070] For three-dimensional space points Define the distance-angle normalized projection function:

[0071] ;

[0072] For each observation point on the feature The predicted global points are transformed into radar frame projections, and the result is obtained by subtracting the original measurement projections. ;

[0073] ;

[0074] S4-2. Linearize the three-dimensional residual vector and stack all points / observations of the same feature;

[0075] Linearize the residual vector at the pose and the current estimated values ​​of the feature parameters:

[0076] ;

[0077] ;

[0078] in, For residual vectors relative pose Jacobi, For the first in the sliding window Error status of frame pose; For residual vectors For feature parameters Jacobi, for planar features , for Three-dimensional features; for line features , for Four-dimensional features; For feature parameters Error state, For measurement error noise;

[0079] Then, the residuals of all observation points for the same feature are longitudinally concatenated with the Jacobian;

[0080] Stacked residual vector of all observation points corresponding to planar / line feature k :

[0081] ;

[0082] in, For the positional easel, stacking residuals The sensitivity matrix to the sliding window state error describes the coupling effect of pose changes on multi-frame residuals; For the sliding window state error, the stacked vector of IMU pose errors for N frames within the window; For the characteristic parameter Jacobian, the sensitivity matrix of the stacked residuals to the characteristic geometric parameters is used for subsequent null projection elimination; Noise is measured for stacking.

[0083] S4-3. Perform null space projection on the stacked residual vector in stages to obtain constraints without feature parameters; in non-final rounds, only perform lighter projection / margining on point-level local variables, and only perform strict feature-level null space projection in the last iteration.

[0084] Final iteration: for stacked residual vectors The left null space basis is calculated by orthogonalization. Strict feature elimination is implemented:

[0085] ;

[0086] In the formula, Features Stacked Jacobian matrices This refers to the sliding window state error. The length of the sliding window; Measure the noise of the projected stack;

[0087] Use Schmidt orthogonalization to calculate .

[0088] Non-final iteration: Projecting point-level residuals to the null space The form is:

[0089] ;

[0090] In the formula, Let be the pose Jacobian matrix of the observation point. For the first The pose error status of the frame; This refers to the measurement noise after projection.

[0091] Specifically, in S5, given the projected observation model ,in, and The residuals and Jacobian matrices in point or feature form are stacked in form;

[0092] In non-final rounds, point-by-point independent stacking is used, with each point-frame pair forming an independent residual term:

[0093] Point-form residuals ;in, , These are the point-level residuals of the first observation point of the first planar / line feature in the first frame of the sliding window, and the point-level residuals of the second observation point of the second planar / line feature in the second frame;

[0094] Stacked Jacobian matrices in point form ;in, , These are the Jacobian matrix stacks of the first observation point of the first planar / line feature in the first frame of the sliding window, and the Jacobian matrix stacks of the second observation point of the second planar / line feature in the second frame;

[0095] In the final round, feature-based aggregation and stacking are used to compress multiple observations of the same feature into a single constraint:

[0096] residuals in characteristic form ;

[0097] Stacked Jacobian matrices in characteristic form ;

[0098] , The Jacobian matrices of the residuals of the first feature within the sliding window are stacked. This represents the total number of features observed within the sliding window.

[0099] No. The next iteration estimate is... The relationship between the predicted state and the predicted state is through Jacobian. Related; Jacobi for:

[0100] ;

[0101] It is a generalized addition operation on a manifold used to incorporate sliding window state errors. Superimposed on vector superior; It is the inverse operation, which calculates the tangent space error between two states;

[0102] State estimation at the start of the sliding window;

[0103] Covariance matrix of all IMU states within the sliding window :

[0104] ;

[0105] in, To predict the covariance matrix;

[0106] Gain of MSCIKF filter for:

[0107] ;

[0108] Status updated to:

[0109] ;

[0110] in, , The first , Sub-iteration estimation; State estimation at the start of the sliding window;

[0111] After the iteration, the final update is:

[0112] ;

[0113] It is the posterior state estimate after MSCIKF iteration update; To and The corresponding posterior covariance matrix.

[0114] In summary, the lidar-inertial odometry method proposed in this invention, based on the SP model and MSCIKF filtering, effectively reduces long-term odometry drift while maintaining low computational load through precise and efficient feature parameterization modeling and state estimation methods. It is expected to provide broad technical support for applications such as autonomous driving and robot positioning and navigation.

[0115] Example 2

[0116] This embodiment discloses a lidar-inertial odometry system for implementing the lidar-inertial odometry method based on the SP model and MSCIKF filtering as described in Embodiment 1. Figure 4 As shown, it includes a lidar, an IMU sensor, and a processor, wherein the processor is configured as follows:

[0117] The system initialization module is used to calibrate the external parameters of the lidar and IMU, set the sensor noise parameters, initialize the IMU state and covariance, and configure the sliding window size.

[0118] The front-end motion estimation module is used to predict pose and correct laser point cloud distortion using IMU integration; extract planar / line features from the point cloud; perform frame-map matching to solve relative pose, determine keyframes based on pose change thresholds, and insert sliding windows.

[0119] The feature parameter estimation module is used to parameterize the planar / line features extracted from keyframes in the sliding window based on the spherical projection model, and to construct distance-angle weighted residuals in the spherical coordinate domain; the sliding window pose is fixed to alternately estimate feature parameters and projection point parameters;

[0120] The feature elimination module is used to calculate the linearized residuals of each keyframe within the sliding window and stack the observations with the same feature. In non-final rounds, it performs point-level fast projection, and in the final round, it performs feature-level left null space projection to eliminate feature parameters and preserve pose constraints.

[0121] The iterative update module is used to iteratively calculate the gain and state update based on the post-projection observation model using the MSCIKF filtering framework, output the optimized pose and maintain the sliding window.

[0122] The specific technical details and beneficial effects in this embodiment are the same as those in Embodiment 1. Please refer to them for details, and they will not be repeated here.

[0123] Example 3

[0124] This embodiment utilizes a LARK LiDAR and a 6-axis CH110 IMU as sensors, uses RTK to provide true position reference, and employs an Nvidia Jetson AGX as the computing platform to build a LiDAR-inertial odometry method verification platform. The LARK LiDAR has a field of view of 25°. 120°, angular resolution of 0.1° The scan frequency is 10Hz; the consumer-grade IMU output frequency is 200Hz, and the MSCIKF sliding window size is set to 5.

[0125] In this embodiment, the lidar-inertial odometry method based on the SP model and MSCIKF filtering in Embodiment 1 is implemented.

[0126] The position estimation errors of the proposed method were compared with those of algorithms such as FAST-LIO2, LiLi-OM-ROT, and VoxelMap using two experimental data sequences. The experimental results are shown in the table below.

[0127] Table 1. Comparison of Trajectory Errors

[0128]

[0129] It can be seen that most of the algorithms can complete trajectory estimation, while LiLi-OM fails on sequence b, and our proposed method achieves the lowest trajectory error, i.e., the highest positioning accuracy. Therefore, the method of this invention has lower trajectory drift and better positioning accuracy. Figure 1 It exhibits better consistency and superior robustness compared to some comparative methods.

[0130] As described above, these are merely preferred embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A lidar-inertial odometry method based on a spherical projection SP model and a multi-state constrained iterative Kalman filter (MSCIKF), characterized in that, Includes the following steps: S1. Perform system initialization, calibrate the external parameters of the lidar and IMU, set the sensor noise parameters, initialize the IMU state and covariance, and configure the sliding window size; S2. Predict pose and correct laser point cloud distortion using IMU integration; extract planar / line features from point cloud; perform frame-map matching to solve relative pose, determine keyframes based on pose change thresholds and insert sliding windows; S3. Based on the parametric sliding window parameterized by the spherical projection model, the planar / line features extracted from the keyframes are constructed in the spherical coordinate domain to construct distance-angle weighted residuals; the sliding window pose is fixed to alternately estimate the feature parameters and projection point parameters. S4. Calculate linearized residuals for keyframes within the sliding window and stack observations with the same features. Perform point-level fast projection in non-final rounds and feature-level left null space projection in the final round to eliminate feature parameters and preserve pose constraints. S5. Based on the post-projection observation model, the MSCIKF filtering framework is used to iteratively calculate the gain and state update, output the optimized pose and maintain the sliding window.

2. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 1, is characterized in that, The S3 includes: S3-1. Construct a spherical polar projection parameterization tool; map three-dimensional unit vectors to two-dimensional parameters without redundancy, and simultaneously construct orthogonal tangential bases to achieve stable representation; S3-2. Establish a feature geometry parameterization model based on parameterization tools; planar features are defined by spherical projection parameters and intercepts, and line features are defined by projection parameters and origin moments. Local points are transformed to the global coordinate system by mapping normal vectors / direction vectors and tangential basis vectors. S3-3. Construct a weighted squared cost function for the observation residuals based on a geometric parameterization model; inversely project global feature points to the radar coordinate system and calculate the weighted squared difference between the theoretical range / angle values ​​and the actual observed values; S3-4. Perform alternating iterative optimization to solve the unknown parameters in the cost function; in each iteration, fix the IMU state and sliding window pose, and alternately estimate the feature parameters and point projection coordinates.

3. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 2, is characterized in that, In S3-2, planar features Represented as: ; in, planar features The two-dimensional spherical projection parameter vector, , planar features The transverse and longitudinal components of the normal unit vector after spherical polar projection; For plane The intercept; via spherical projection function After transformation, planar features are obtained. normal vector ; For planar features On point In the global coordinate system, it is expressed as: ； Line features Represented as ; in, Line features The two-dimensional spherical projection parameter vector, Line features The transverse and longitudinal components of the spherical projection of the direction vector; Line features The transverse and longitudinal components of the origin moment parameter; Line features The direction is determined by the spherical projection function Determined; Line Features The origin moment is given by Sure; For line features Points on , For online ordering The local position; expressed in the global coordinate system as: 。 4. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 3, is characterized in that, In S3-3, for the first... The first frame The first planar / line feature Coordinates of observation points The formula for transforming from local coordinates to global coordinates is: ； in, , For the rotation matrix and translation vector in the extrinsic parameters of the laser radar to the IMU; , For the first Rotation matrix and translation vector from the frame IMU coordinate system to the global coordinate system; Then the coordinates of the observation point The weighted residual cost function is: ； in, These represent the standard deviations of ranging noise, azimuth noise, and elevation noise, respectively. , , These represent the distance extraction function, azimuth extraction function, and elevation extraction function, respectively. For the global coordinate system to the first Inverse transformation of the frame lidar coordinate system For observation point The coordinates of the predicted point in the global coordinate system after modeling by spherical projection.

5. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 1, characterized in that, S4 includes: S4-1. Construct the three-dimensional observation residual vector; Define a range-angle normalized projection function for points in three-dimensional space. For each observation point on the feature, transform the predicted global point to the radar frame projection and subtract it from the original measurement projection to obtain the three-dimensional residual vector. S4-2. Linearize the three-dimensional residual vector and stack all points / observations of the same feature to obtain a stacked residual vector; S4-3. Perform null space projection on the stacked residual vector in stages to obtain constraints without feature parameters; in non-final rounds, only perform lighter projection / marginalization on point-level local variables, and only perform strict feature-level null space projection in the last iteration.

6. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 5, is characterized in that, In S4-3, the final iteration, features The characteristic residuals are: ； In the formula, Features Stacked Jacobian matrices This refers to the sliding window state error. The length of the sliding window; Measure the noise of the projected stack; Non-final iteration, the first iteration in the sliding window The first frame The first planar / line feature The point-level residuals for each observation point are: ； In the formula, Let be the pose Jacobian matrix of the observation point. For the first The pose error status of the frame; This refers to the measurement noise after projection.

7. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 6, is characterized in that, In S5, given the projected observation model , in, and The residuals and Jacobian matrices are stacked in point or feature form; in non-final rounds, point-by-point independent stacking is used, with each point-frame pair forming an independent residual term: Point-form residuals ;in, , These are the point-level residuals of the first observation point of the first planar / line feature in the first frame of the sliding window, and the point-level residuals of the second observation point of the second planar / line feature in the second frame; Stacked Jacobian matrices in point form ;in, , These are the Jacobian matrix stacks of the first observation point of the first planar / line feature in the first frame of the sliding window, and the Jacobian matrix stacks of the second observation point of the second planar / line feature in the second frame; In the final round, feature-based aggregation and stacking are used to compress multiple observations of the same feature into a single constraint: residuals in characteristic form ; Stacked Jacobian matrices in characteristic form ; , These are the stacked residuals and Jacobian matrices of the first feature within the sliding window, respectively. This represents the total number of features observed within the sliding window.

8. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 7, is characterized in that, Covariance matrix of all IMU states within the sliding window : ； in, To predict the covariance matrix; No. Jacobian estimated in the next iteration for: ； It is a generalized addition operation on a manifold used to incorporate sliding window state errors. Superimposed on vector superior; yes The inverse operation.

9. The lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in claim 8, is characterized in that, Gain of MSCIKF filter for: ； Status updated to: ； in, , The first , Sub-iteration estimation; State estimation at the start of the sliding window; After the iteration, the final update is: ； in, It is the posterior state estimate after MSCIKF iteration update; To and The corresponding posterior covariance matrix.

10. A lidar-inertial odometry system for implementing the lidar-inertial odometry method based on the spherical projection SP model and multi-state constrained iterative Kalman filter MSCIKF as described in any one of claims 1-9, comprising a lidar, an IMU sensor, and a processor, characterized in that, The processor is configured as follows: The system initialization module is used to calibrate the external parameters of the lidar and IMU, set the sensor noise parameters, initialize the IMU state and covariance, and configure the sliding window size. The front-end motion estimation module is used to predict pose and correct laser point cloud distortion using IMU integration; extract planar / line features from the point cloud; perform frame-map matching to solve relative pose, determine keyframes based on pose change thresholds, and insert sliding windows. The feature parameter estimation module is used to parameterize the planar / line features extracted from keyframes in the sliding window based on the spherical projection model, and to construct distance-angle weighted residuals in the spherical coordinate domain; the sliding window pose is fixed to alternately estimate feature parameters and projection point parameters; The feature elimination module is used to calculate the linearized residuals of each keyframe within the sliding window and stack the observations with the same feature. In non-final rounds, it performs point-level fast projection, and in the final round, it performs feature-level left null space projection to eliminate feature parameters and preserve pose constraints. The iterative update module is used to iteratively calculate the gain and state update based on the post-projection observation model using the MSCIKF filtering framework, output the optimized pose and maintain the sliding window.

Images