A shipborne laser inertial navigation synchronous positioning and mapping method for wave disturbance scenarios

Abstract

The application discloses a shipborne laser inertial navigation synchronous positioning and mapping method for a wave disturbance scene. The method first acquires laser radar point cloud data and inertial measurement unit (IMU) data; then, a sliding window of vertical pseudo-observation is constructed around the current time or the current key frame, a vertical fast-changing component wave-induced oscillator dynamics model, an interference design matrix and an equivalent noise covariance are established, and a quotient space observation is obtained through weighted projection; when a preset observability check condition is met, a weighted least square method with priori is used to solve the vertical fast-changing component wave-induced; finally, a relative vertical motion compensation quantity is generated according to the vertical fast-changing component wave-induced, and the in-scan point cloud de-distortion result and the key frame local optimization process are corrected. The application can reduce the influence of the vertical fast-changing motion caused by wave disturbance on point cloud matching and main link state estimation, thereby relieving the vertical error accumulation and z-axis drift problems in the shipborne synchronous positioning and mapping process.

Description

Technical Field

[0001] This invention relates to the fields of laser radar and inertial measurement unit fusion positioning, wave disturbance compensation, and shipborne synchronous positioning and mapping technology, and particularly to a shipborne laser inertial navigation synchronous positioning and mapping method for wave disturbance scenarios. Background Technology

[0002] Simultaneous localization and mapping (SLAM) technology, which integrates lidar and inertial measurement units (INS), can continuously estimate the pose of a vehicle by utilizing the geometric constraints provided by lidar and the high-frequency motion information provided by INS, and further construct an environmental map. This type of technology has been widely applied in scenarios such as autonomous vehicles, robots, drones, and mobile mapping.

[0003] For shipborne platforms, wave excitation causes them to continuously generate significant vertical rapid motion. Compared to rigid land-based platforms, shipborne platforms, during navigation or floating, not only experience conventional translation and rotation, but also wave-induced vertical oscillations, pitch and roll coupling, and low-frequency drift in the vertical channel. Existing laser inertial navigation simultaneous localization and mapping systems typically prioritize general motion compensation, keyframe local matching, and global map optimization, lacking specific modeling for the vertical rapid components caused by wave disturbances. This leads to two main problems: Firstly, in the front-end processing stage, point cloud distortion correction based on inertial propagation typically assumes that the motion during the scan can be approximated by general rigid body motion. For shipborne platforms with significant wave-induced vertical rapid changes, this assumption leads to the inability to accurately compensate for the vertical relative motion during the scan, resulting in residual vertical errors in the distorted point cloud, further affecting the stability of point cloud matching and pose estimation.

[0004] Secondly, during the state estimation stage, vertical inertial observations typically incorporate real wave-induced vertical motion, inertial device zero-bias residuals, slow drift within the window, and leakage terms from attitude errors into the vertical channel. When these factors are directly superimposed into the same observation channel, indiscriminately applying them to local or graphical optimization can easily lead to problems such as accumulated vertical errors, local registration jitter, and difficulty in suppressing global z-axis drift.

[0005] Existing schemes for vertical displacement estimation often follow the approach of high-pass filtering, band-pass filtering, state dimension expansion, or general observers. While they can extract wave-induced vertical motion to some extent, they mostly exist as independent modules and do not directly serve distortion correction and keyframe optimization in shipborne laser inertial navigation synchronous positioning and mapping systems. Furthermore, existing methods typically emphasize correcting unobservable directions in the state space rather than eliminating interference subspaces caused by low-frequency offsets, slow drift, and attitude leakage at the observation end.

[0006] Meanwhile, shipborne lidar may face insufficient effective geometric features in open waters, and sparse or unstable surface echoes can lead to degradation of vertical geometric constraints. Therefore, relying solely on the general-purpose lidar-inertial odometry (LIO) main link is insufficient to stably suppress vertical drift in the long term. For nearshore scenarios near docks, shore-end buildings, bridge piers, or shoreline structures, if wave-induced vertical rapid variation components can be extracted from the IMU vertical channel and fed back to the point cloud distortion correction and local matching process in a controlled manner, the vertical distortion of the point cloud and local registration jitter caused by wave disturbances can be alleviated. Summary of the Invention

[0007] To address at least one of the problems existing in the prior art, this invention belongs to the field of laser radar and inertial measurement unit (IMU) fusion positioning technology. It provides a shipborne laser inertial navigation synchronous positioning and mapping method for wave disturbance scenarios. By introducing a windowed observation structure for the vertical channel of the IMU, interference subspace elimination, and wave-induced vertical rapid change component solution mechanism in addition to the main link of laser inertial navigation synchronous positioning and mapping, a relative vertical motion compensation amount is obtained. The compensation amount is then used to correct the initial point cloud distortion removal results and the key frame local optimization process, thereby reducing the impact of vertical rapid change motion on scan-to-map registration and global graph optimization, and suppressing the z-axis drift of the shipborne platform in wave disturbance scenarios.

[0008] This invention can model and estimate the vertical rapidly changing components in wave disturbance scenarios without relying on external height sensors, and feed them back to the point cloud distortion correction and keyframe optimization process while ensuring the stability of the main link state estimation, thereby mitigating the vertical error and z-axis drift caused by wave disturbance.

[0009] This invention reduces the impact of rapid vertical motion caused by wave disturbance on point cloud matching and main link state estimation by using frequency band isolation, state isolation and anomaly isolation mechanisms, thereby alleviating the problems of vertical error accumulation and z-axis drift in the shipborne synchronous positioning and mapping process.

[0010] To achieve the objective of this invention, this invention provides a shipborne laser inertial navigation synchronous positioning and mapping method for wave disturbance scenarios, comprising the following steps.

[0011] Step S1: Obtain the lidar point cloud data and timestamp, and obtain the specific force data, angular velocity data and timestamp of the inertial measurement unit (IMU).

[0012] Step S2 involves performing pre-integration based on the IMU data to complete the initial distortion correction of the point cloud, obtaining the initial pose estimation of the current point cloud frame, and performing keyframe selection and management based on the point cloud matching results. Step S2 may further include: Step S21, performing IMU pre-integration; Step S22, performing initial point cloud distortion correction; Step S23, performing local scan-to-map point cloud matching and pose estimation; and Step S24, performing keyframe selection and management.

[0013] Step S3: Set a vertical pseudo-observation sliding window around the current time or the current keyframe. Within the sliding window, construct the vertical pseudo-observation sequence, the oscillator dynamics model of the wave-induced vertical rapidly changing component, the interference design matrix, and the equivalent noise covariance to obtain the window observation model. Step S3 may further include: Step S31: Set the sliding window; Step S32: Construct the vertical pseudo-observation; Step S33: Construct the oscillator dynamics model; Step S34: Construct the interference design matrix; Step S35: Calculate the equivalent noise covariance.

[0014] Step S4 involves performing weighted projection or interference cancellation on the interference subspace corresponding to the interference design matrix based on the equivalent noise covariance, constructing quotient space observations, and, when the observability check condition is met, performing weighted least squares with priors on the wave-induced vertical rapidly varying components to obtain the wave-induced vertical state estimation result corresponding to the current window. Step S4 may further include: Step S41, constructing a weighted projection matrix; Step S42, forming quotient space observations; Step S43, performing observability and numerical stability checks; Step S44, performing weighted least squares with priors.

[0015] Step S5: Generate a relative vertical motion compensation amount based on the wave-induced vertical state estimation result. Use the relative vertical motion compensation amount to correct the initial point cloud distortion correction result. Then, use the corrected point cloud and pose constraints for keyframe local optimization and global graph optimization to output the shipborne platform pose result and point cloud map result. Step S5 may further include: Step S51: Generate relative vertical motion compensation amount; Step S52: Correct the point cloud distortion correction result; Step S53: Perform keyframe local optimization; Step S54: Perform global graph optimization.

[0016] Furthermore, the vertical motion of the shipborne platform is decomposed into a slowly varying vertical pose component and a wave-induced rapidly varying vertical component. The slowly varying vertical pose component is characterized by the master state of laser inertial navigation synchronous positioning and mapping, while the wave-induced rapidly varying vertical component is characterized by an independent oscillator state. The subsequent relative vertical motion compensation is constructed based on the relative change of the wave-induced rapidly varying vertical component with respect to the scanning reference time.

[0017] Furthermore, the equivalent noise covariance includes a process noise propagation term and a measurement noise term.

[0018] Because the projection matrix is ​​for The column space performs interference cancellation, so the interference components corresponding to offset, slow drift and attitude leakage are suppressed in the column space observation, and only the observation information useful for wave-induced vertical rapid change components is retained.

[0019] Based on the window starting state estimate obtained from the solution, the wave-induced vertical rapid change component estimate at the current moment can be obtained by extrapolation, and the relative vertical motion compensation amount of each moment within the scan relative to the reference moment can be calculated.

[0020] Furthermore, the present invention employs an isolation mechanism between vertical compensation feedback and main link state estimation. The isolation mechanism includes: limiting the wave-induced vertical rapid change component to a preset wave frequency band for estimation; preventing the wave-induced vertical rapid change component state from directly updating the IMU zero bias and the laser inertial navigation main state; and freezing the wave-induced vertical compensation link and continuing to execute the laser inertial navigation main link when the observability check is not satisfied, the information matrix condition number exceeds a threshold, or the compensation residual is abnormal.

[0021] This invention also provides a method for shipborne laser inertial navigation synchronous positioning and mapping in wave disturbance scenarios.

[0022] The present invention also provides an electronic device.

[0023] The present invention also provides a computer-readable storage medium.

[0024] Compared with the prior art, the present invention has at least the following beneficial effects: First, this invention does not simply treat wave disturbance as a general noise term, but decomposes vertical motion into slowly varying vertical pose components and wave-induced rapidly varying vertical components, and explicitly models the wave-induced rapidly varying vertical components through an oscillator dynamics model, thereby making the vertical motion characteristics in shipboard scenarios more structurally interpretable.

[0025] Second, instead of correcting the unobservable direction after the full state update, this invention performs weighted projection or interference cancellation on the interference subspace corresponding to the interference design matrix based on the equivalent noise covariance after the window observation is constructed. This suppresses the influence of low-frequency bias, slow drift and attitude leakage on the estimation of wave-induced vertical fast-changing components from the observation end, thereby improving the observation decoupling capability.

[0026] Third, this invention uses relative vertical motion compensation to correct the distortion of the initial point cloud and feeds the corrected result back to the local optimization process of the keyframe, making the local matching under wave disturbance conditions more stable and effectively reducing the pollution of local scan-to-map registration by rapid vertical motion.

[0027] Fourth, this invention reduces the risk of reverse contamination of the LIO main link IMU bias estimation and main state estimation by the wave-induced vertical compensation link through frequency band isolation, state isolation, anomaly isolation, and observability numerical stability checks.

[0028] Fifth, this invention retains the main link of laser inertial navigation synchronous positioning and mapping with keyframe local optimization and global graph optimization. By introducing vertical compensation results at the front end and local optimization interface, it achieves mitigation of z-axis drift and can work without additional configuration of external height sensors. Attached Figure Description

[0029] Figure 1 is a flowchart of the overall algorithm of an embodiment of the present invention.

[0030] Figure 2 is a schematic diagram of the vertical pseudo-observation structure of the sliding window according to an embodiment of the present invention.

[0031] Figure 3 is a schematic diagram of the point cloud distortion removal and wave-induced vertical compensation effects according to an embodiment of the present invention. Detailed Implementation

[0032] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be understood that the following embodiments are for illustrative purposes only and are not intended to limit the scope of protection of the present invention. All equivalent substitutions, simple modifications, or substitutions using conventional technical means made within the scope of the concept and principles of the present invention should fall within the scope of protection of the present invention.

[0033] Figure 1 illustrates the overall processing flow of an embodiment of the present invention. Overall, the embodiment of the present invention includes two mutually coupled processing links.

[0034] The first link is the main link for simultaneous laser inertial navigation localization and mapping. This main link starts from the lidar point cloud and IMU data, and successively performs IMU pre-integration, initial point cloud distortion correction, point cloud matching and pose estimation, and keyframe selection and management. Then it enters the local optimization of keyframes and global map optimization to complete the platform pose estimation and map construction.

[0035] The second link is a vertical rapid-change modeling and compensation link for wave disturbances. This link constructs a sliding window around the IMU's vertical channel, establishing a vertical pseudo-observation model, an oscillator dynamics model, a disturbance design matrix, and an equivalent noise covariance within this window. Furthermore, it constructs quotient spatial observations through weighted projection or disturbance elimination to solve for the wave-induced vertical rapid-change components. The output of this link does not replace the main link but is fed back to it as a relative vertical motion compensation, used to correct point cloud distortion removal results and keyframe local optimization results.

[0036] By coordinating the two links mentioned above, this embodiment of the invention models and compensates for the vertical rapid motion caused by wave disturbances on a shipborne platform without changing the general laser inertial navigation synchronous positioning and mapping process, thereby enabling the system to simultaneously achieve local real-time performance and global consistency.

[0037] Please see Figure 1 The present invention provides a shipborne laser inertial navigation synchronous positioning and mapping method for wave disturbance scenarios, comprising the following steps: Step S1: Obtain the lidar point cloud data and corresponding timestamps, and obtain the specific force data, angular velocity data and corresponding timestamps of the inertial measurement unit (IMU).

[0038] Step S2: Perform pre-integration based on IMU data to complete the initial distortion correction of the point cloud, obtain the initial pose estimation of the current point cloud frame, and perform keyframe selection and management based on the point cloud matching results.

[0039] In one embodiment, this step includes the following sub-steps: Step S21: Perform pre-integration on the IMU data to obtain the initial values ​​of inertial motion within the scan cycle; Step S22: Perform initial point cloud distortion correction on the current point cloud frame based on the initial value of inertial motion to eliminate the main distortion caused by rigid body motion during the scanning cycle.

[0040] Since the wave-induced vertical rapid motion of the shipborne platform has not been estimated separately in the initial stage, the initial point cloud distortion is preferentially compensated for the general rigid body motion term to provide an initial point cloud for subsequent point cloud matching and pose estimation.

[0041] Step S23: Perform point cloud matching and pose estimation using the distorted point cloud and the local map to obtain the initial pose of the current frame; Step S24: Select keyframes from continuous point cloud frames and perform keyframe selection and management based on pose change, time interval, point cloud coverage, or point cloud overlap.

[0042] In one embodiment, using the previous keyframe as a reference, the translation increment, rotation increment, and time interval between the current point cloud frame and the previous keyframe are calculated, and a comprehensive determination is made based on the point cloud overlap rate and the newly added coverage area between the current frame and the previous keyframe or local map. The current point cloud frame is determined as a keyframe when at least one of the following conditions is met: translation increment greater than a preset translation threshold, rotation increment greater than a preset rotation threshold, time interval greater than a preset time threshold, point cloud overlap rate lower than a preset overlap rate threshold, or newly added coverage area greater than a preset coverage area threshold. For selected keyframes, their timestamp, pose estimation, distortion-free point cloud, corresponding IMU pre-integration information, and vertical compensation results are saved and added to the keyframe queue and keyframe pose map. Simultaneously, a fixed number of local keyframe sets are maintained according to temporal order or spatial proximity for constructing local maps and performing subsequent local optimizations. Keyframes that exceed the local window but still have global constraints are retained in the global pose map.

[0043] Keyframes are the basic units for both local optimization of the main link and global graph optimization, and also serve as the time reference for the subsequent windowed vertical observation construction.

[0044] Step S3: Set a sliding window for vertical pseudo-observations around the current time or the current keyframe. Within the sliding window, construct the vertical pseudo-observation sequence, the oscillator dynamics model of the wave-induced vertical rapidly changing component, the interference design matrix, and the equivalent noise covariance to obtain the window observation model.

[0045] Figure 2 illustrates the process of constructing a vertical pseudo-observation within a sliding window. In one embodiment, the system inputs the IMU data within the current window and the current attitude estimate, and initializes the sliding window length and reference time. The starting time of the window is determined, and samples from each time point within the sliding window are read sequentially and attitude compensation is performed to construct the vertical pseudo-observation for the current time. If not all time points within the sliding window have been traversed, then the sample from the next time point is read to continue constructing observations; if the entire sliding window has been traversed, then the vertical pseudo-observations at each time point are stacked in chronological order to form a window observation vector. Subsequently, an oscillator dynamics model was used to describe the wave-induced vertical rapid-change components of the hull under wave excitation. In this model, the vertical displacement and velocity corresponding to each wave's dominant frequency were taken as states, and the natural angular frequency and damping ratio were used to characterize the near-period vertical motion caused by the waves. A window observation matrix was constructed based on the discrete propagation relationship of this oscillator dynamics model. and process noise propagation matrix And construct the interference design matrix. and equivalent noise covariance Thus, a window observation model is obtained that describes the relationship between the window observation vector and the wave-induced vertical rapidly changing component state, process noise, interference term and measurement noise.

[0046] The method of this invention decomposes the vertical motion of a shipborne platform into a slowly varying vertical pose component and a wave-induced rapidly varying vertical component. The slowly varying vertical pose component is characterized by the master state of laser inertial navigation synchronous positioning and mapping, while the wave-induced rapidly varying vertical component is characterized by an independent oscillator state. The relative vertical motion compensation amount is constructed based on the relative change of the wave-induced rapidly varying vertical component relative to the scanning reference time.

[0047] This step includes the following sub-steps: Step S31: Set the length to the current time or the current keyframe as the endpoint. The sliding window for vertical pseudo-observations.

[0048] Step S32: For each moment within the sliding window, construct a vertical pseudo-observation based on the attitude-compensated navigation chord acceleration. Subsequently, stack the vertical pseudo-observations from each moment in chronological order to form the window observation vector. .

[0049] The vertical pseudo-observation is constructed from the attitude-compensated vertical acceleration and is expressed as follows:

[0050] in, It is a unit vector in the vertical direction. The index represents the linear acceleration estimate of the navigation system obtained based on the current attitude compensation. This represents the time index within the sliding window.

[0051] Considering the zero-bias residual of the accelerometer and the leakage of attitude error, after performing first-order linearization on the attitude error, the vertical pseudo-observation... It can be represented as:

[0052] in, Output coefficient vector for vertical acceleration. For wave-induced vertical components The rapidly changing component states, For vertical channel offset residuals, and For the lateral and longitudinal attitude leakage basis functions, and These represent the angular increments of the attitude error of the carrier system in the lateral and longitudinal directions, respectively. For measuring noise.

[0053] The attitude leakage basis function is constructed by linearizing the first order of the attitude error to avoid representing the effect of attitude leakage using only symbolic representation. Let the estimated linear acceleration of the navigation system after attitude compensation be:

[0054] When there is a small angle error in the posture At that time, the vertical channel leakage term can be approximated as:

[0055] Therefore, the lateral attitude leakage basis functions and the longitudinal attitude leakage basis functions can be constructed as follows:

[0056]

[0057] in, This represents the attitude matrix from the carrier system to the navigation system. Indicates IMU specific force measurement, This indicates the zero bias estimate of the accelerometer. This represents the gravitational acceleration vector of the navigation system. Indicates transpose. This represents the cross product operator. and These represent the horizontal and vertical unit vectors of the load system, respectively.

[0058] Through the above construction, the leakage effect of attitude error into the vertical channel can be incorporated into the interference subspace corresponding to the interference design matrix and suppressed in the subsequent weighted projection process.

[0059] Wave-induced vertical rapid-change components are characterized by superposition of multiple damped oscillator modes. For the first... There are several modes, and their dynamic equations are as follows:

[0060] in, For the first Vertical displacement of each mode, and These correspond to the speed and acceleration, respectively. For the first The natural angular frequencies of each mode, For the first Modal damping ratio, For process noise, The modal number.

[0061] Stacking the state variables of all modes in order, we get Wave-induced vertical rapidly varying component state vector at time 1 :

[0062] in, and They represent the first Each mode in Displacement and velocity at any given moment.

[0063] Based on the dynamic parameters of each mode, the local state transition matrix and local process noise covariance of each mode are constructed respectively, and then... The stacking order is combined to form a global discrete state transition matrix. process noise covariance matrix Simultaneously, based on the output relationship between modal displacement and vertical acceleration, a vertical acceleration output vector is constructed. .

[0064] In one embodiment, the modal number of the oscillator dynamics model Take values ​​from 2 to 5, natural angular frequency Pick to Within the wave frequency band, the damping ratio Set within the range of 0.01 to 0.3.

[0065] To facilitate the characterization of wave-induced vertical rapid motion, this embodiment of the invention employs multiple damped oscillator modes to model the wave-induced vertical rapid component. Unlike simply treating vertical motion as a random perturbation, this modeling method uses frequency and damping to express the dominant dynamics of wave-induced motion, allowing the vertical rapid component to propagate continuously in the state space and distinguish it from the slow-varying bias term, drift term, and attitude leakage term.

[0066] Step S34: Construct the interference design matrix.

[0067] In one embodiment, the disturbance design matrix within the sliding window is composed of constant terms, drift terms, and attitude leakage basis function terms. The constant terms absorb fixed biases, the drift terms absorb slowly varying drifts within the window, and the attitude leakage basis function terms absorb the leakage effect of attitude errors into the vertical channel. Since these disturbance terms constitute a low-dimensional disturbance subspace within the sliding window, observation-end disturbance cancellation can be achieved in subsequent steps through weighted projection.

[0068] Interference Design Matrix Preferred options are:

[0069] in, This represents a constant term, used to characterize a fixed bias. This represents the drift term within the window, used to characterize slow drift. and These represent the attitude leakage basis function terms in the lateral and longitudinal directions, respectively.

[0070] Step S35: Calculate the equivalent noise covariance.

[0071] The equivalent noise covariance consists of two parts: one is the cumulative term of the wave-induced vertical rapid-change component noise after propagation through the window, and the other is the measurement noise term of the vertical pseudo-observations within the window. These two parts are then incorporated into the equivalent noise covariance. This can uniformly characterize the sources of uncertainty in observations within a window, providing a statistical weighting basis for subsequent weighted projection and weighted least squares solutions with priors.

[0072] Let the length of the sliding window be... The first sliding window The wave-induced vertical rapid component state at each sampling time is as follows ( The starting state of the window is recorded as follows: In one embodiment, the wave-induced vertically rapidly varying component state propagates according to a discrete oscillator dynamics model:

[0073] in, The discrete state transition matrix, The process noise input matrix, For the first The process noise vector of the step. When the process noise is directly entered as the state increment. It can be taken as an identity matrix.

[0074] By stacking the vertical pseudo-observations at each moment within the sliding window in chronological order, we obtain the window observation model:

[0075] in, For the window observation vector, The stacked process noise vector ( ), Design matrix for interference, The interference coefficient vector, To measure the noise vector.

[0076] In one embodiment, the window observation matrix Represented as:

[0077] Process noise propagation matrix Represented as a lower triangular block matrix:

[0078] Based on this, the equivalent noise covariance is given by the following formula:

[0079] in, The noise covariance matrix for a single-step process. To measure the noise covariance at a single time point, Indicates the Kronecker product. and These are the identity matrices of the corresponding dimensions.

[0080] Step S4: Perform weighted projection or interference cancellation on the interference subspace corresponding to the interference design matrix based on the equivalent noise covariance, construct quotient space observation, and when the observability check condition is met, perform weighted least squares solution with priors on the wave-induced vertical fast variable component to obtain the wave-induced vertical state estimation result corresponding to the current window.

[0081] This step includes the following sub-steps: Step S41: Construct the weighted projection matrix.

[0082] The weighted projection or interference cancellation is achieved using a weighted projection matrix based on the equivalent noise covariance.

[0083] First, construct a weighting matrix based on the equivalent noise covariance. :

[0084] Then through the interference design matrix Construct a weighted projection matrix for eliminating interference subspaces :

[0085] The function of this projection matrix is ​​to eliminate [the interference between the projection matrix and the projection matrix]. The corresponding interference components are used to isolate the effects of fixed bias, slow drift, and attitude leakage in the window observation from the master observation.

[0086] Step S42, forming a commercial space observation.

[0087] Using weighted projection matrix Window observation Weighted projection is performed to obtain commercial space observations. :

[0088] Corresponding quotient space observation matrix The weighted projection matrix acts on the original window observation matrix. get:

[0089] Equivalent noise covariance of commutative space observations Given by the error propagation law:

[0090] Since the quotient space observations retain only the effective observation information related to the wave-induced vertical rapidly changing component state, the disturbance coefficient vector can be estimated without explicit estimation. In the case of the window's starting state Solve the problem.

[0091] S43, perform observability and numerical stability checks.

[0092] Based on the quotient space observation matrix Perform an observability check. If the rank condition and at least one of the numerical stability conditions (including the minimum eigenvalue condition of the information matrix and the condition number condition) are satisfied, the current window is considered to satisfy the update conditions for the wave-induced vertical rapidly changing component state.

[0093] The rank condition is:

[0094] in, For the damped oscillator mode number, Corresponding wave-induced vertical rapidly changing component state vector The dimension of (each mode contains two state components: displacement and velocity).

[0095] The numerical stability condition is:

[0096]

[0097] in, Represents the smallest eigenvalue of the matrix. The preset lower bound threshold value is used. The condition number represents the matrix. This is the preset upper bound threshold for the condition number.

[0098] If the rank condition is not met, skip the current update and use the valid estimation result from the previous time step. If the rank condition is met but the condition number exceeds [a certain threshold], [the update will proceed as follows]. At this time, a weight reduction process is performed on the update, that is, the information matrix of the current window is multiplied by a decay factor inversely proportional to the condition number. This reduces the contribution of ill-conditioned observations to state updates, thereby preventing noise from being exacerbated by ill-conditioned least squares.

[0099] Step S44: Perform weighted least squares with priors to obtain the wave-induced vertical state estimation result corresponding to the current window.

[0100] The state determination of the wave-induced vertically rapidly varying component employs a weighted least squares objective function with prior knowledge:

[0101] in, The prior mean of the starting state of the window. The prior covariance of the starting state of the window.

[0102] The reason for using weighted least squares with priors instead of regular least squares is that while stacked observations can improve discriminability, under conditions of high-correlation column space and finite window length, it is still necessary to utilize prior means and prior covariance to constrain the scaling and stability of the state solution, thereby suppressing erroneous coupling between wave-induced vertically rapidly varying components and residual noise. This approach improves the numerical stability of the actual solution while maintaining the advantages of observational-side noise reduction.

[0103] Step S5: Generate relative vertical motion compensation amount based on the wave-induced vertical state estimation result, use the relative vertical motion compensation amount to correct the initial point cloud distortion correction result, and use the corrected point cloud and pose constraints for keyframe local optimization and global graph optimization to output the shipborne platform pose result and point cloud map result.

[0104] In one embodiment, keyframe local optimization uses a local scan-to-map nonlinear optimization based on a local map constructed from the current keyframe and its surrounding keyframes; global map optimization performs incremental optimization based on the locally optimized keyframe pose to maintain the consistency of the global map. Since this embodiment focuses on vertical compensation in wave-disturbance scenarios, it does not require replacing the entire solution framework of existing laser inertial navigation simultaneous localization and mapping systems. Instead, it introduces vertical compensation results at the front-end distortion correction and keyframe local optimization interfaces.

[0105] This step includes the following sub-steps: Step S51: Output the relative vertical motion compensation amount.

[0106] After obtaining the estimated state value of the window starting point, it is pushed forward to the current time according to the discrete state transition matrix to obtain the estimated values ​​of the wave-induced vertical rapid component displacement, velocity, or acceleration at the current time. Furthermore, the difference between the wave-induced vertical displacement corresponding to each time stamp in the current scan and the displacement at the reference time is output as the relative vertical motion compensation amount.

[0107] The solution yields the estimated state value of the window's starting point. Then, using the discrete state transition matrix Push it forward to the current moment The wave-induced vertical rapid displacement component is obtained at the current moment. Velocity or acceleration estimates. For timestamps within the scan. Point cloud points, for reference time The displacement (usually taken as the keyframe moment or the start of the scan) is used as the reference, then the relative vertical motion compensation amount is... The quantity is expressed as:

[0108] in, and These represent the times obtained through wave-induced vertical state estimation. and The estimated value of the vertical displacement.

[0109] Step S52: Correct the distortion removal results of the initial point cloud based on the relative vertical motion compensation.

[0110] Point cloud points after initial inertial distortion correction for:

[0111] in, These are the original point coordinates in the lidar coordinate system. and They are time points The rotation matrix and translation vector from the carrier system to the navigation system. and Reference times The rotation matrix and translation vector, This indicates that the relative position in the navigation system will be converted back to the reference system.

[0112] Based on this, a wave-induced vertical relative motion compensation amount is introduced to obtain the wave-induced vertically compensated point cloud points. :

[0113] in, Let be the unit vector in the vertical direction of the navigation system. This is the relative vertical motion compensation amount. The physical meaning of this compensation is: on the basis of the initial distortion correction, to further eliminate the vertical relative displacement caused by waves, so that each point in the point cloud more accurately reflects the static scene structure.

[0114] Step S53: Perform local optimization of keyframes.

[0115] Point cloud points after wave-induced vertical compensation This is used for local scan-to-map nonlinear optimization between the current keyframe and surrounding keyframes to obtain more stable local pose estimation. In the local optimization, the point-to-plane residual... It can be represented as:

[0116] in, For the current keyframe The coordinates of the point cloud points after compensation. and These represent the rotation matrix and translation vector (i.e., the pose to be optimized) of the current keyframe. ), For local maps and The corresponding matching point coordinates, for The unit normal vector of the local plane. This represents the transpose of the normal vector.

[0117] The corresponding local optimization objective function is:

[0118] in, This is a robust kernel function used to suppress the effects of mismatches or gross errors; For the first The information matrix of each residual (i.e., the inverse of the covariance matrix) can have its weights set according to point cloud quality, observation noise, or the confidence level of vertical compensation. Since wave-induced vertical compensation improves the consistency of vertical components and effectively increases the confidence level of vertical residuals, an information matrix related to the accuracy of the compensation amount can be used, or corresponding vertical constraints can be introduced into the residual terms.

[0119] Step S54: Perform global graph optimization.

[0120] During the global graph optimization phase, the locally optimized keyframe poses are written into the global keyframe pose graph, and the following optimizations are performed:

[0121] in, and They represent the first The and the first The pose matrix of each keyframe (belonging to the set of keyframe poses to be optimized) ), The set of edges in the global keyframe pose graph. Indicates the first Frame and the There are relative pose constraints between frames; The first point obtained through point cloud matching Frame relative to the first The relative pose of the frame (i.e., measurement constraints). Its opposite; This is a logarithmic mapping from Lie groups to Lie algebras, used to vectorize pose errors; For the edge The information matrix reflects the confidence level of the relative pose constraint; This represents the square of the Mahalanobis distance, i.e. ,in .

[0122] In this way, the relative vertical motion compensation amount is not only used for point cloud coordinate correction, but can also participate in keyframe local optimization in the form of point cloud residuals or residual weight correction terms after compensation.

[0123] The distortion removal in step S2 is the initial point cloud distortion removal, which mainly relies on IMU pre-integration to compensate for general rigid body motion; the correction in step S5 is the compensation correction performed on the vertical rapid change component caused by wave disturbance. Therefore, the two are not contradictory, but rather two interconnected processing steps: the former provides initial values, and the latter improves the compensation accuracy in the vertical direction.

[0124] Figure 3 illustrates the process of point cloud distortion correction and wave-induced vertical compensation. When shipborne radar scans targets on shore or docks under wave disturbance, the rapid vertical motion of the ship during the scanning period causes vertical undulations or interlayer misalignment in the original point cloud. Initial inertial distortion correction can compensate for general rigid body motion, but vertical errors may still remain when wave disturbance is significant. In this embodiment of the invention, a relative vertical compensation amount is generated based on the estimation results of the wave-induced rapid vertical component, and the point cloud points collected at different times during the scan are corrected, so that the compensated point cloud restores better geometric consistency on the surface of the shore target.

[0125] Specifically, for timestamps during scanning The point cloud points, if the reference time is According to The relative vertical compensation at this point is calculated and combined with the initial inertial compensation result to obtain the corrected point cloud distortion correction result. The corrected point cloud is then used for local optimization in subsequent keyframes to reduce the impact of wave-induced rapid vertical motion on the local scan-to-map registration residuals.

[0126] In the local optimization stage, the current keyframe and surrounding keyframes form a local map, and the pose of the current keyframe is solved through local nonlinear optimization. In the global graph optimization stage, the locally optimized keyframe pose is written as a node into the global pose graph, and further global graph optimization is performed to obtain a globally consistent pose and map result. Since the wave-induced vertical rapid change component has been fed back to the point cloud distortion correction and local optimization stages through relative vertical motion compensation, this embodiment of the invention can mitigate z-axis drift without relying on an additional external height sensor.

[0127] To ensure that wave-induced vertical rapid component estimation errors do not contaminate the estimation process of IMU bias and master state in the LIO main link through the corrected distortion-free point cloud, this embodiment of the invention introduces an isolation mechanism between vertical compensation feedback and master link state estimation.

[0128] First, a frequency band isolation mechanism is employed. The natural frequencies of the oscillator dynamics model are confined within the wave frequency band, for example... to IMU accelerometer bias, slow drift, and long-period attitude leakage are mainly concentrated in the low-frequency or quasi-static frequency band. By using the interference design matrix and weighted projection, low-frequency bias, slow drift, and attitude leakage are suppressed at the observation end, thereby reducing the frequency domain coupling between wave-induced vertical fast-changing components and IMU bias estimation.

[0129] Second, a state isolation mechanism is adopted. This addresses the wave-induced vertical rapidly changing component state. Instead of directly merging into the LIO main state or directly updating the IMU zero-bias state, it only compensates for the relative vertical motion. It can participate in point cloud point-level coordinate correction in the form of residual correction terms or weight correction terms in the construction of the objective function for local optimization of keyframes.

[0130] Third, an anomaly isolation mechanism is adopted. When the observability check is not met, the number of information matrix conditions exceeds the threshold, the compensation amount exceeds the preset physical range, or the point cloud residuals before and after compensation are not improved, the wave-induced vertical compensation link is frozen and the effective estimation results of the previous moment are used. At the same time, the laser inertial navigation main link continues to run according to the initial inertial distortion correction, point cloud matching, and key frame local optimization process, thereby avoiding the degradation state of the compensation link from polluting the main link.

[0131] Through the aforementioned frequency band isolation, state isolation, and anomaly isolation mechanisms, the embodiments of the present invention can reduce the adverse effects on the main link IMU bias convergence characteristics while feeding back the vertical compensation results to point cloud distortion correction and local optimization.

[0132] In one implementation, modal parameters are selected and updated online, and the number of modes in the oscillator dynamics model is... Natural angular frequency Damping ratio The settings can be configured based on ship dimensions, sea state frequency band, sampling frequency, or engineering experience. To improve adaptability under changing sea states, embodiments of the present invention can also employ an online frequency identification mechanism.

[0133] Specifically, the system continuously performs short-time Fourier transform or wavelet analysis on the vertical pseudo-observation sequence to extract the energy-rich samples. The primary frequency is used as a candidate intrinsic frequency. When multiple consecutive sliding windows detect a primary frequency offset exceeding a set threshold, the oscillator frequency parameters are updated. To avoid compensation discontinuities caused by abrupt changes in frequency parameters, an exponential smoothing strategy can be adopted.

[0134] in, The updated natural angular frequency, The smoothing coefficient controls the fusion weights of the old and new frequencies. The closer it is to 1, the faster the response to the current observation; the closer it is to 0, the smoother the update. These are the candidate values ​​of the dominant frequency obtained from the current window's spectral analysis. This refers to the inherent angular frequency used at the previous moment. This method improves the model's adaptability to changes in wave frequency without altering the core structure of the invention.

[0135] In one embodiment, taking the process shown in Figure 1 as an example, a complete execution process of the method of the present invention is illustrated.

[0136] First, during navigation near the shore, docks, or shore-side structures, the shipborne platform continuously outputs a continuous point cloud from the lidar, while the IMU continuously outputs specific force and angular velocity. Based on the IMU pre-integration results, the system performs initial point cloud distortion correction on the point cloud within the current scanning cycle, and obtains the initial pose of the current frame based on the point cloud matching results. Subsequently, key frames are selected based on the pose change and time interval.

[0137] Secondly, a sliding window of vertical pseudo-observations is constructed around the time period corresponding to the current keyframe. The vertical pseudo-observation at each moment within the window is obtained from the vertical acceleration after attitude compensation. Simultaneously, an oscillator dynamics model is constructed based on a preset frequency range or historical window estimation results, and an interference design matrix is ​​constructed based on the attitude leakage characteristics within the window. Subsequently, the window equivalent noise covariance is obtained by combining process noise propagation and measurement noise.

[0138] Next, weighted projection or interference cancellation is performed on the window observations to construct quotient space observations, and the observability and numerical stability conditions of the current window are checked. If they are met, weighted least squares with priors are performed to obtain the wave-induced vertical rapidly varying component state; if they are not met, the current update is skipped or a weighted update is performed, and the valid estimation results from the previous time step are retained.

[0139] Subsequently, based on the obtained wave-induced vertical displacement estimate, the relative vertical motion compensation amount of each point within the current scan relative to the reference time is generated. This compensation amount is used to correct the initial point cloud distortion result to obtain the vertically compensated point cloud.

[0140] Finally, keyframe local optimization is performed using the corrected point cloud, and the optimization results are written into the global pose graph to complete the global graph optimization. The final output is the current platform pose and the global point cloud map. Thus, this embodiment of the invention, while retaining the general laser inertial navigation synchronous positioning and mapping main link, achieves explicit modeling and compensation for rapidly changing vertical motion in wave-disturbed scenarios.

[0141] In one embodiment, the shipborne platform is a 1.5m long unmanned surface vessel equipped with a 3D LiDAR and a high-performance MEMSIMU. The LiDAR scanning frequency can be from 10Hz to 20Hz, and the IMU sampling frequency can be from 100Hz to 400Hz. The length of the sliding window... It can be set to 40 to 120 IMU sampling points.

[0142] In this embodiment, the number of oscillator modes is set to The natural angular frequencies of the three modes can be preset as follows: , and The corresponding wave periods are approximately 7.9s, 3.5s, and 2.2s; the damping ratios are set to 0.05, 0.10, and 0.08, respectively. Process noise covariance... The diagonal elements can be inversely proportional to the modal frequency to measure the noise covariance. It can be taken as 5 to 20 times the noise variance of the vertical accelerometer.

[0143] The above values ​​are only used to illustrate one possible implementation of the present invention. In practical applications, the number of modes, natural angular frequency, damping ratio, window length, process noise covariance, and measurement noise covariance can all be adjusted according to the ship's dimensions, sensor accuracy, sea state, speed, and sampling frequency.

[0144] The embodiments of this invention are mainly applicable to shipborne laser inertial navigation synchronous positioning and mapping scenarios near the shore, docks, shore-end buildings, bridge piers, or hydraulic structures. In these scenarios, lidar can obtain relatively stable shore-end geometric features, while wave disturbances introduce significant vertical rapid motion within the scanning cycle. Therefore, this invention can improve the distortion removal quality and local matching stability of point clouds through wave-induced vertical compensation.

[0145] It should be noted that the embodiments of the present invention do not replace external height sensors or long-term vertical geometric constraints. In scenarios with open water, extremely scarce effective laser geometric features, or insufficient vertical excitation, the present invention can serve as an enhancement compensation mechanism for the LIO main link, but its effectiveness is still constrained by the stability of the IMU vertical channel, the geometric observability of the point cloud, and the degree of dynamic excitation of the window.

[0146] Without altering the core concept of this invention, the invention may also have the following alternatives.

[0147] First, the modal number of the oscillator dynamics model. The settings can be made based on platform size, sea state frequency band, sampling frequency, or engineering experience, or adjusted based on the main frequency distribution of historical windows.

[0148] Secondly, in addition to constant terms, drift terms, and attitude leakage basis function terms, the interference design matrix can also add second-order or higher-order slow drift terms, provided that the window degrees of freedom allow, to adapt to more complex low-frequency interference environments.

[0149] Third, keyframe local optimization can be implemented in the form of the current keyframe and surrounding keyframes, the current keyframe and local sub-keyframe set, or other equivalent local map structures, depending on the specific system configuration; global graph optimization can also be implemented in any incremental or batch graph optimization method that can maintain the global consistency of the keyframe pose graph.

[0150] Fourth, the relative vertical motion compensation amount can be directly used to correct the point cloud distortion correction result, or it can be converted into an equivalent residual, constraint information or weight correction term that acts on the local optimization of the key frame; as long as it is essentially generated by the wave-induced vertical rapid change component estimation result of the present invention, it falls within the protection scope of the present invention.

[0151] Fifth, in addition to using the rank condition, observability checks can also use the minimum eigenvalue of the information matrix, the condition number, the residual consistency test, or the improvement in the point cloud matching residuals before and after compensation as criteria to form a hierarchical update strategy.

[0152] In summary, this invention introduces a windowed vertical observation structure, interference subspace elimination, and wave-induced vertical rapid component solution mechanism into the main link of shipborne laser inertial navigation synchronous positioning and mapping. This enables the correction of the initial point cloud distortion removal results and keyframe local optimization results by the relative vertical motion compensation amount. This can alleviate z-axis drift in wave disturbance scenarios and improve the stability and applicability of the shipborne synchronous positioning and mapping system.

[0153] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Those skilled in the art can make various improvements, substitutions, and modifications to the present invention without departing from its concept and principles, and these improvements, substitutions, and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A shipborne laser inertial navigation synchronous positioning and mapping method for wave-disturbance scenarios, characterized in that, Includes the following steps: Acquire lidar point cloud data and corresponding timestamps, and acquire specific force data, angular velocity data and corresponding timestamps from the inertial measurement unit; Pre-integration is performed based on the force and angular velocity data of the inertial measurement unit to complete the initial distortion correction of the point cloud, obtain the initial pose estimation of the current point cloud frame, and select key frames based on the point cloud matching results. A sliding window for vertical pseudo-observations is set around the current time or the current keyframe. Within the sliding window, a vertical pseudo-observation sequence, an oscillator dynamics model for wave-induced vertical rapid components, an interference design matrix, and an equivalent noise covariance are constructed to obtain the window observation model. Based on the equivalent noise covariance, a weighted projection or interference cancellation is performed on the interference subspace corresponding to the interference design matrix to construct the quotient space observation. When the preset observability check condition is met, a weighted least squares solution with prior is performed on the wave-induced vertical fast-changing component to obtain the wave-induced vertical state estimation result corresponding to the current window. Based on the wave-induced vertical state estimation result, a relative vertical motion compensation amount is generated. The relative vertical motion compensation amount is used to correct the initial point cloud distortion result. The corrected point cloud or the residual correction term constructed by the relative vertical motion compensation amount is used for keyframe local optimization and global graph optimization to output the shipborne platform pose result and point cloud map result.

2. The method according to claim 1, characterized in that, By stacking the vertical pseudo-observations at each moment within the sliding window in chronological order, we obtain the window observation model: in, For the window observation vector, This is the window observation matrix obtained from the propagation of oscillator dynamics. The wave-induced vertical rapidly varying component state at the starting point of the window. The process noise propagation matrix is... Design matrix for interference, The interference coefficient vector, For measuring noise; The interference design matrix is ​​as follows: in, This represents a constant term, used to characterize a fixed bias. For items that drift within the window, and These represent the attitude leakage basis function terms in the lateral and longitudinal directions, respectively; The equivalent noise covariance is composed of the process noise propagation term and the measurement noise term. The vertical pseudo-observation is constructed using the vertical acceleration after attitude compensation. Considering the accelerometer zero-bias residual and attitude error leakage, the attitude error is linearized to the first order. The resulting vertical pseudo-observation is expressed as follows: in, Output coefficient vector for vertical acceleration. For wave-induced vertical rapid variable components The rapidly changing component states, For vertical channel offset residuals, and For the lateral and longitudinal attitude leakage basis functions, and These represent the angular increments of the attitude error of the carrier system in the lateral and longitudinal directions, respectively. For measuring noise; The oscillator dynamics model uses a superposition of multiple damped oscillators to describe the wave-induced vertical rapid change component, satisfying: in, For the first Vertical displacement of each mode, and These correspond to the speed and acceleration, respectively. For the first The natural angular frequencies of each mode, For the first Modal damping ratio, For process noise, The modal number; The displacements and velocities of each mode are stacked to obtain the wave-induced vertical rapid change component. Rapidly changing component states: and They represent the first Each mode in Displacement and velocity at any given moment.

3. The method according to claim 2, characterized in that, The natural angular frequency of the oscillator dynamic model can be updated online based on the short-time spectrum analysis results of the vertical pseudo-observation. The update formula is as follows: in, The candidate values ​​of the dominant frequency extracted from the vertical pseudo-observation sequence are used to trigger a smooth update of the intrinsic angular frequency when multiple consecutive windows detect a dominant frequency offset exceeding a threshold.

4. The method according to claim 1, characterized in that, Commercial space observation is represented as: This represents the quotient space observation after the interference subspace is eliminated. This represents the weighted projection matrix used to eliminate interference subspaces. This represents the window observation vector.

5. The method according to claim 1, characterized in that, The observability check condition is that if any one of the following conditions is met, such as the rank condition of the quotient space observation matrix, the minimum eigenvalue condition of the information matrix, or the condition number condition, the current sliding window is considered to meet the update condition for the wave-induced vertical rapid change component state; if the update condition is not met, the update is skipped; if the rank condition is met but the condition number exceeds the preset upper bound threshold of the condition number, the update is weighted. The rank condition is: The condition for the minimum eigenvalue of the information matrix is: The condition number condition is: In the formula, Represents the quotient space observation matrix. For the damped oscillator mode number, Represents the smallest eigenvalue of the matrix. The preset lower bound threshold value is used. The condition number represents the matrix. The upper bound threshold of the preset condition number, This indicates transpose.

6. The method according to claim 1, characterized in that, The objective function used in weighted least squares with prior knowledge is: in, The prior mean of the starting state of the window. The prior covariance of the starting state of the window; Based on the solution results, the estimated value of the wave-induced vertical rapid change component at the current moment is obtained by extrapolation. Subsequently, the relative vertical motion compensation amount of the current scan relative to the reference moment is calculated.

7. The method according to any one of claims 1-6, characterized in that, The initial point cloud distortion correction result is corrected using the relative vertical motion compensation amount, and the corrected point cloud and pose constraints are used for keyframe local optimization and global graph optimization to output the shipborne platform pose result and point cloud map result, including: For the scanned timestamp is The point cloud points, if the reference time is The relative vertical motion compensation is expressed as: in, and These represent the times obtained through wave-induced vertical state estimation. and The estimated value of the vertical displacement; Then, the point cloud points after correction for vertical motion compensation , These are the point cloud points after initial inertial distortion correction. For reference time, Let be the unit vector in the vertical direction of the navigation system. Indicates transpose; The keyframe local optimization constructs a local map using the current keyframe and surrounding keyframes, and performs local nonlinear optimization using the corrected point cloud. The local optimization objective function is: in, The pose to be optimized. For robust kernel functions, For the first The information matrix of each residual. Represents the point-to-plane residual in local nonlinear optimization; The global graph optimization is performed based on the keyframe pose graph after local optimization, and the optimization objective is: in, and They represent the first The and the first The pose matrix of each keyframe. For relative pose constraints between keyframes. Reflects the confidence level of relative pose constraints. It is the set of edges in the global keyframe pose graph.

8. A shipborne laser inertial navigation synchronous positioning and mapping device for wave disturbance scenarios, characterized in that, For implementing the method according to any one of claims 1-6, the apparatus comprises the following modules: The data acquisition module is used to acquire lidar point cloud data and inertial measurement unit data; The front-end processing and keyframe management module is used to perform pre-integration based on the force data and angular velocity data of the inertial measurement unit, complete the initial distortion correction of the point cloud, obtain the initial pose estimation of the current point cloud frame, and select keyframes based on the point cloud matching results. The window observation construction module is used to set a sliding window for vertical pseudo-observations around the current time or the current keyframe. Within the sliding window, the vertical pseudo-observation sequence, the oscillator dynamics model of wave-induced vertical rapid components, the interference design matrix, and the equivalent noise covariance are constructed to obtain the window observation model. The interference subspace elimination and state solution module is used to perform weighted projection or interference elimination on the interference subspace corresponding to the interference design matrix based on the equivalent noise covariance, construct quotient space observation, and perform weighted least squares solution with prior on the wave-induced vertical fast variable component when the preset observability check condition is met, so as to obtain the wave-induced vertical state estimation result corresponding to the current window. The result feedback and back-end processing module is used to generate a relative vertical motion compensation amount based on the wave-induced vertical state estimation result, use the relative vertical motion compensation amount to correct the initial point cloud distortion correction result, and use the corrected point cloud or the residual correction term constructed by the relative vertical motion compensation amount for keyframe local optimization and global graph optimization to output the shipborne platform pose result and point cloud map result.

9. An electronic device, characterized in that, The device includes a processor and a memory, wherein the memory stores a computer program that, when executed by the processor, causes the electronic device to perform the method according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it causes the processor to perform the method according to any one of claims 1 to 7.

Images