A laser inertial odometer drift suppression and mapping method and device in a degraded environment, an electronic device and a storage medium

Abstract

The application relates to a laser inertial odometer drift suppression and mapping method, device, electronic equipment and storage medium in a degenerative environment, the drift suppression method comprising: acquiring multi-modal sensor data in a degenerative environment; performing geometric feature extraction on at least one frame of laser radar point cloud data, establishing a global geometric prior model of the degenerative environment, and extracting structure parameters of the global geometric prior model; in the processing process of a subsequent frame, constructing a distance residual term of the laser radar point cloud data of the current frame to the global geometric prior model based on the structure parameters; constructing a joint factor graph, and adding the distance residual term as a geometric prior constraint factor into the joint factor graph to constrain the degree of freedom of the pose in the normal direction of the global geometric prior model; performing graph optimization processing based on the joint factor graph, and outputting a target pose estimation result, so that the application has the advantages of better eliminating mapping distortion and pose drift caused by long-distance degeneration.

Description

Technical Field

[0001] This application relates to the technical field of synchronous positioning and mapping, and in particular to a method, apparatus, electronic device and storage medium for laser inertial odometry drift suppression and mapping under degraded environments. Background Technology

[0002] In routine inspections and maintenance of underground pipe networks, long straight tunnels, and other similar environments, inspection robots typically need to be equipped with multiple sensors for autonomous localization and mapping (SLAM). However, these environments are characterized by highly simple structures and extremely limited geometric features, making them typical "degraded scenarios."

[0003] When traditional laser inertial odometry (LIO) operates in such degenerate scenarios, the point clouds scanned by the lidar often contain similar geometric contours such as the inner wall of a smooth pipe or an infinitely long straight corridor. This makes it difficult to extract sufficiently effective corner or facet constraints, leading to rank deficiency in the state estimation matrix. This results in severe cumulative drift along the axial direction of the pipe or corridor, or deviation in the heading angle, ultimately manifesting as a curved, misaligned, or distorted 3D map.

[0004] To mitigate this issue, some existing technologies attempt to incorporate wheel odometers for multi-sensor fusion assistance. However, current fusion schemes often treat wheel odometer data as an absolutely ideal motion reference, failing to adequately consider the effects of non-translational swaying, such as pitch or roll, that occur when the robot travels on uneven ground. Because the inertial measurement unit (IMU) and wheel odometer have spatial linkages in their physical installation, in non-translational swaying conditions, directly using wheel speed observations as velocity constraints for IMU measurement points without lever conversion will introduce a systematic error proportional to angular velocity, thus introducing new errors into the positioning system. Furthermore, most existing technologies remain at the level of local feature matching, lacking effective utilization of the global geometry of degenerate scenarios, such as the topological characteristics of cylinders, and thus failing to fundamentally eliminate mapping distortion and pose drift caused by long-distance degradation. Summary of the Invention

[0005] To address the aforementioned technical problems, this application provides a method, system, electronic device, and storage medium for suppressing and mapping laser inertial odometry drift under degraded environments.

[0006] On the one hand, this application provides a method for suppressing drift of a laser inertial odometer under degraded conditions, which adopts the following technical solution: A method for suppressing drift in a laser inertial odometer under degraded conditions includes: Acquire multimodal sensor data under degraded environments, wherein the multimodal sensor data includes at least lidar point cloud data and inertial measurement data; Geometric features are extracted from at least one frame of the lidar point cloud data to establish a global geometric prior model of the degraded environment, and the structural parameters of the global geometric prior model are extracted. In the processing of subsequent frames, the distance residual term of the current frame's lidar point cloud data to the global geometric prior model is constructed based on the structural parameters. Construct a joint factor graph and add the distance residual term as a geometric prior constraint factor to the joint factor graph to constrain the degree of freedom of the pose in the normal direction of the global geometric prior model; Graph optimization processing is performed based on the joint factor graph to output the target pose estimation result.

[0007] In one embodiment: the step of establishing a global geometric prior model of the degraded environment and extracting the structural parameters of the global geometric prior model specifically includes: A cylinder model is established by fitting the at least one frame of lidar point cloud data using a preset random sampling consensus algorithm, and the cylinder model is used as the global geometric prior model. The central axis vector and radius of the cylindrical model are extracted as the structural parameters.

[0008] In one embodiment, the step of constructing the distance residual term from the current frame LiDAR point cloud data to the global geometric prior model based on the structural parameters specifically includes: Extract target feature points from the lidar point cloud data of the current frame; Calculate the vertical distance from the target feature point to the central axis vector; The difference between the vertical distance and the radius is used as the distance residual term.

[0009] In one embodiment: before the step of performing graph optimization processing based on the joint factor graph, the method further includes: The current driving trajectory direction and current heading angle are calculated based on the multimodal sensor data; Detect the angular deviation between the current driving trajectory direction and the central axis vector; When the included angle deviation is greater than a first preset threshold, a direction constraint factor is introduced into the joint factor diagram; The included angle deviation is used as a penalty term by the direction constraint factor to correct the current heading angle.

[0010] In one embodiment: the multimodal sensor data further includes wheel speed odometer data; prior to the step of graph optimization based on the joint factor graph, the method further includes: Acquire the physical lever parameters between the first sensor that generates the inertial measurement data and the second sensor that generates the wheel speed odometer data; Based on the physical lever parameters and the inertial measurement data, the wheel speed odometer data is compensated for speed observation deviation to obtain compensated wheel speed data; The compensated wheel speed data is added to the joint factor graph as a wheel speed constraint factor.

[0011] In one embodiment: the step of compensating for speed observation deviations in the wheel speed odometer data based on the physical lever parameters and the inertial measurement data to obtain compensated wheel speed data specifically includes: Extract the angular velocity information from the inertial measurement data; Based on the angular velocity information and the physical lever parameters, a cross product calculation is performed to obtain the compensated velocity component; The original wheel speed in the wheel speed odometer data is transformed to the inertial measurement coordinate system through a pre-calibrated rotational extrinsic parameter, and then added to the compensated speed component to obtain the compensated wheel speed data.

[0012] In one embodiment: before the step of performing graph optimization processing based on the joint factor graph, the method further includes: Extract point cloud features from the lidar point cloud data; Based on the point cloud features, the fixed structure joint features in the degraded environment are identified; The joint factor graph is updated based on the fixed structural joint features.

[0013] In one embodiment: the fixed structure seam feature is obtained by clustering and identifying abrupt changes in reflection intensity or geometric curvature in the point cloud feature.

[0014] In one embodiment: the step of updating the joint factor map based on the fixed structural joint features specifically includes: The number of joint features in the fixed structure is obtained by counting the joint features. The current absolute position is calculated by combining the preset physical spacing between the fixed structural joint features and the number of the joint features; The current absolute position is used as an absolute position constraint factor to update the joint factor graph.

[0015] On the other hand, the laser inertial odometry drift mapping method provided in this application adopts the following technical solution: A method for mapping laser inertial odometry in degraded environments includes: Acquire multimodal sensor data under degraded environments; The multimodal sensor data is processed using the laser inertial odometry drift suppression method under degraded environment as described in any one of claims 1 to 9 to obtain target pose estimation results; Based on the target pose estimation results, the lidar point cloud data in the multimodal sensor data are globally consistent and stitched together to construct a global map of the degraded environment.

[0016] Thirdly, the laser inertial odometry drift mapping device for degraded environments provided in this application adopts the following technical solution: A laser inertial odometry drift mapping device for use in degraded environments includes: The data acquisition module is used to acquire multimodal sensor data under degraded environments; The pose calculation module is used to process the multimodal sensor data using the laser inertial odometry drift suppression method under the degradation environment to obtain the target pose estimation result. The mapping and stitching module is used to perform globally consistent stitching of the lidar point cloud data in the multimodal sensor data based on the target pose estimation results, and to construct a global map of the degraded environment.

[0017] Fourthly, the electronic device provided in this application adopts the following technical solution: An electronic device includes a processor and a memory, the memory storing a computer program, which, when executed by the processor, implements the laser inertial odometry drift suppression method under degradation environment or the laser inertial odometry mapping method under degradation environment.

[0018] Fifthly, this application provides a computer-readable storage medium, which adopts the following technical solution: A computer-readable storage medium storing a computer program, which, when executed by a processor, implements the laser inertial odometry drift suppression method under degradation conditions or the laser inertial odometry mapping method under degradation conditions.

[0019] In summary, this application has the following beneficial effects: 1. By extracting global geometric prior model parameters and constructing the distance residual term from the point cloud to the model in subsequent frames as a factor graph constraint, the pose estimation is subject to global topological rigidity constraint in the normal direction of the model, which effectively suppresses radial drift, pipe wall bending and pipe diameter scaling distortion caused by the lack of point cloud features. 2. A cross product compensation mechanism based on physical lever parameters and inertial angular velocity is introduced to eliminate the systematic velocity deviation caused by the lever not being converted in non-translation states such as equipment swaying in place, ensuring that the axial velocity observation provided by the wheel speed constraint factor is consistent with the velocity state of the measurement point of the inertial measurement unit. 3. By detecting the angular deviation between the driving trajectory and the central axis, a directional constraint factor is introduced to forcibly correct the heading angle when the limit is exceeded, ensuring that the mapping extension direction is consistent with the physical axis when the equipment deviates from the axis; 4. Clustering identifies abrupt changes in point cloud features as fixed seams, and combines this with preset spacing to calculate absolute positions for factor map updates, giving the odometer absolute positioning correction capability and eliminating long-distance cumulative drift from wheel speed integration. The above four mechanisms correspond to the constraint requirements of different degrees of freedom in a degraded environment: geometric prior constraint factors lock radial displacement and pitch / yaw attitude, wheel speed constraint factors provide axial velocity observations, lever compensation ensures the reliability of these axial observations, seam absolute position constraint factors eliminate long-distance cumulative errors from wheel speed integration, and orientation constraint factors correct heading deviations; the roll degree of freedom around the tube axis is constrained by gravity vector observations from accelerometer components in inertial measurement data. Attached Figure Description

[0020] Figure 1 This is a flowchart of a laser inertial odometry drift suppression method under degraded conditions according to this embodiment; Figure 2 This is a schematic diagram of a laser inertial odometry drift suppression method under degraded conditions according to this embodiment; Figure 3 This is a structural block diagram of a laser inertial odometry mapping device under a degraded environment according to this embodiment. Detailed Implementation

[0021] To better understand the purpose, technical solutions, and advantages of this application, it has been described and illustrated below with reference to the accompanying drawings and embodiments. However, those skilled in the art should understand that this application can be implemented without these details. In some cases, to avoid obscuring various aspects of this application due to unnecessary description, well-known methods, processes, systems, components, and / or circuits already described at a higher level will not be elaborated upon. It will be apparent to those skilled in the art that various modifications can be made to the embodiments disclosed in this application, and the general principles defined in this application can be applied to other embodiments and application scenarios without departing from the principles and scope of this application. Therefore, this application is not limited to the illustrated embodiments, but conforms to the broadest scope consistent with the scope of protection claimed in this application.

[0022] It should be noted that the descriptions of these embodiments are for the purpose of aiding understanding the present invention, but do not constitute a limitation thereof. Furthermore, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0023] In the description of this application, "several" means one or more, "more than" means two or more, "greater than," "less than," and "exceeding" are understood to exclude the stated number, while "above," "below," and "within" are understood to include the stated number. The use of "first" and "second" in the description is merely for distinguishing technical features and should not be construed as indicating or implying relative importance, or implicitly indicating the number of indicated technical features, or implicitly indicating the order of the indicated technical features.

[0024] In the description of this application, the terms "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any one or more embodiments or examples.

[0025] See Figure 1 and Figure 2 As shown in the embodiments of this application, a method for suppressing laser inertial odometry drift under degraded environments is provided. This method specifically includes: S100, acquire multimodal sensor data under degraded environment, the multimodal sensor data including at least lidar point cloud data and inertial measurement data.

[0026] It should be noted that a degraded environment refers to a physical scene with a simple spatial structure and a lack of rich corner points and planar features. Typical examples of degraded environments include long, straight underground pipes, mine shafts, and narrow corridors.

[0027] It is understandable that acquiring multimodal data can provide a fundamental data source for subsequent multi-sensor tight coupling fusion and global geometric feature extraction.

[0028] In practice, electronic devices use onboard sensors such as lidar and IMU to collect multimodal sensor data in real time under degraded environments.

[0029] S200, extract geometric features from at least one frame of the lidar point cloud data, establish a global geometric prior model of the degraded environment, and extract the structural parameters of the global geometric prior model.

[0030] It should be noted that abstracting the entire environment into a global prior model and using this model as a fixed benchmark is equivalent to providing the algorithm with an invisible "virtual track" in the subsequent localization process, thereby suppressing cumulative drift.

[0031] Understandably, after this step is performed during the initialization phase, the fixed reference geometric parameters will not be updated again and will remain unchanged, thus avoiding the drift caused by the accumulation of errors in the parameters over time.

[0032] In its implementation, the system extracts regular geometric structure parameters from the point cloud of the first frame or the accumulated point cloud of the initial few frames.

[0033] Furthermore, before adding the distance residual term to the joint factor graph in step S400, degradation direction determination and constraint direction selection can be performed. Specifically, the system calculates the Jacobian matrix J of the distance residual term with respect to the pose state, constructs an information matrix H = JᵀJ, and performs eigenvalue decomposition on H. If the k-th eigenvalue of the information matrix H is lower than a preset threshold, the corresponding eigenvector is the degradation direction. The formula for determining the set of degradation direction vectors is as follows: λ k <λ th ,v k ∈D Where: λ k λ represents the k-th eigenvalue of the information matrix H. th This represents the preset feature value threshold; v k represents the eigenvector corresponding to the k-th eigenvalue; D represents the set of degeneracy direction vectors.

[0034] If the above formula is satisfied, the corresponding eigenvector is added to the set of degenerate direction vectors. Subsequently, the geometric prior constraint factor applies constraints only to the subspace spanned by the set of degenerate direction vectors, and no additional constraints are applied to non-degenerate directions to avoid over-constraint. Taking a cylindrical pipe as an example, the normal vector of the point cloud on the inner wall of the pipe is distributed radially across 360°, but lacks the axial direction. Therefore, the eigenvalue of the information matrix is ​​smallest in the axial direction. The degenerate direction is the pipe axial direction and the roll direction around the axis, totaling two degenerate degrees of freedom. This scheme differs from the existing technology's approach of abandoning updates in that direction after detecting degradation. In the degenerate direction, independent observations are supplemented through wheel speed observation and joint anchor points. In the non-degenerate direction, observability is reconstructed through geometric prior constraints, thereby achieving effective constraints on all degrees of freedom in the degenerate environment.

[0035] S300, during the processing of subsequent frames, the distance residual term of the current frame's lidar point cloud data to the global geometric prior model is constructed based on the structural parameters.

[0036] It should be noted that the distance residual term is a physical quantity used to quantify the degree to which the point cloud deviates from the global prior model at the current moment.

[0037] Understandably, this residual term provides an effective mathematical constraint on the direction of degradation, compensating for the lack of point-to-surface features in environments such as long corridors.

[0038] In the specific implementation, the system transforms the current frame of lidar point cloud data to the world coordinate system, and then calculates the distance from the interior point on the surface of the reference geometric structure to the reference geometric model as the distance residual term.

[0039] The formula for obtaining the pose estimation of the current frame is: T t =(R t ,p t )∈SE(3) Wherein: T t R represents the pose estimation for the current frame; t p represents the rotation matrix for the current frame pose estimation; t represents the translation vector for the current frame pose estimation; SE(3) represents the three-dimensional special Euclidean group.

[0040] Then, a point cloud coordinate transformation is performed, and the corresponding coordinate transformation formula is: W q i =R t · L p i +p t ,and L p i ∈P t in, W q i R represents the coordinates of the i-th point cloud transformed to the world coordinate system; t This represents the rotation matrix used for the current frame pose estimation. L p i p represents the coordinates of the i-th point in the current frame of LiDAR point cloud data in the sensor's local coordinate system; t P represents the translation vector for the current frame pose estimation; t This represents the set of lidar point cloud data for the current frame.

[0041] Preferably, step S300, which involves constructing the distance residual term from the current frame's lidar point cloud data to the global geometric prior model based on the structural parameters, specifically includes: extracting target feature points from the lidar point cloud data of the current frame; calculating the vertical distance from the target feature points to the central axis vector; and using the difference between the vertical distance and the radius as the distance residual term.

[0042] In the embodiments of this specification, the target feature points (i.e., the set of interior points located on the surface of the cylindrical model) are extracted, and the determination formula is as follows: I t ={ W q i ||d( W q i ,l0)-r0|<ε} Among them, I t This represents the set of extracted interior points (i.e., the target feature points). W q i d(·,l0) represents the i-th point transformed to the world coordinate system; d(·,l0) represents the perpendicular distance function from the point to the reference axis l0; l0 represents the reference axis determined by the central axis vector â0 and the reference point c0; r0 represents the radius of the global geometric prior model; ε represents the interior point determination threshold.

[0043] For each extracted target feature point, its distance residual term is calculated using the following formula: e cyl,i =d(R t · L p i +p t ,l0)-r0 Where: e cyl,i Let represent the geometric residual (i.e., the distance residual term) of the i-th target feature point; d(·,l0) represents the perpendicular distance function from the coordinate point to the reference axis; R t This represents the rotation matrix used for the current frame pose estimation. L p i p represents the coordinates of the i-th point cloud in the sensor's local coordinate system in the current frame; t The vector represents the translation vector for the current frame pose estimation; l0 represents the reference axis; and r0 represents the radius of the global geometric prior model.

[0044] S400, construct a joint factor graph and add the distance residual term as a geometric prior constraint factor to the joint factor graph to constrain the degree of freedom of the pose in the normal direction of the global geometric prior model.

[0045] It should be noted that the joint factor graph is a graph model used for multi-sensor fusion optimization. It injects the distance residual term as a constraint factor, which is equivalent to forcibly adding a global constraint.

[0046] Understandably, through joint factor graph optimization, the geometric residual provides an effective observation in the normal direction of the global geometric prior model, compensating for the underdeterminacy of the point-to-surface constraint in that direction, thereby effectively suppressing radial drift and pipe wall bending distortion.

[0047] It should be noted that, taking the cylindrical model as an example, the Jacobian of its radial distance residual with respect to the two degrees of freedom of translation along the axis and roll about the axis is always zero. That is, the geometric prior constraint factor locks the two radial translational degrees of freedom and the two attitude degrees of freedom of pitch and yaw. The drift rate along the axis is constrained by the axial velocity observation provided by the wheel speed constraint factor described later (the lever compensation ensures the reliability of this observation in non-translational motion states), and the long-distance cumulative error of the wheel speed integral is further eliminated by the absolute position constraint factor described later; the deviation in the heading direction is corrected by the direction constraint factor described later.

[0048] Furthermore, the roll degree of freedom about the central axis is constrained by the accelerometer component in the inertial measurement data through gravity vector observation. It should be noted that when the pipe axis coincides with the direction of gravity (vertical pipe), the roll degree of freedom is not considerable, requiring the introduction of additional sensors (such as magnetometers); in this embodiment, it is assumed that the pipe axis has a non-zero angle with the direction of gravity.

[0049] In practical implementation, geometric prior constraint factors, along with original point-to-surface constraint factors and IMU pre-integration constraint factors, are all integrated into the factor graph. The geometric residuals of all interior points are combined into constraint terms and added to the factor graph, as shown in the formula: F cyl =∑ i∈It ρ(e cyl,i 2 / σ cyl 2 ) Wherein: F cyl Represents the geometric residual constraint term (i.e., the geometric prior constraint factor); i∈I t Describe the set of interior points I t Traverse each point i in the array; ρ(·) represents a robust kernel function (such as the Huber kernel); e cyl,i σ represents the geometric residual of the i-th interior point; cyl σ represents the standard deviation of geometric measurement noise. cyl 2 It represents the square of the standard deviation of geometric measurement noise (i.e., variance).

[0050] S500, perform graph optimization processing based on the joint factor graph, and output the target pose estimation result.

[0051] It should be noted that graph optimization processing solves for the optimal pose state by minimizing the error residuals of each item in the factor graph.

[0052] Understandably, using nonlinear optimization algorithms to solve the problem can leverage the advantages of multimodal data and output high-precision target pose estimation results.

[0053] In the specific implementation, optimization libraries such as GTSAM or Ceres are used to solve the joint factor graph. The set of state variables to be optimized is X={T}. i ,v i ,b i}, where T i v represents the pose of the i-th state node. i b represents the velocity of the i-th state node. i Let represent the zero bias of the inertial measurement unit at the i-th state node, and let i traverse each state node in the sliding window or global factor graph. The objective function formula for its joint graph optimization is: X̂=argmin X [F p2f (X)+F IMU (X)+F cyl (X)+F enc (X)+F dir (X)+F abs (X)] Where: X̂ represents the optimal state estimation result, and the pose component therein is the target pose estimation result; argmin X F represents the mathematical operation of finding the set of state variables X that minimizes the subsequent cost function; p2f (X) represents the standard point-to-surface constraint term function; F IMU (X) represents the pre-integration constraint function for inertial measurement data; F cyl (X) represents the geometric residual constraint term function (i.e., the geometric prior constraint factor); F enc (X) represents the wheel speed constraint term function (i.e., the wheel speed constraint factor); F dir (X) represents the directional constraint term function (i.e., the directional constraint factor); F abs (X) represents the absolute position constraint term function (i.e., the absolute position constraint factor); X represents the set of state variables to be optimized; where F enc F dir F abs This is an optional constraint, and the joint factor graph is added when introducing the corresponding implementation.

[0054] In one possible implementation, for environments such as cylindrical pipes, the step S200 of establishing a global geometric prior model of the degraded environment and extracting the structural parameters of the global geometric prior model specifically includes: using a preset random sampling consensus algorithm to perform cylindrical fitting on at least one frame of lidar point cloud data to establish a cylindrical model as the global geometric prior model; and extracting the central axis vector and radius of the cylindrical model as the structural parameters.

[0055] In the embodiments described in this specification, the system uses a preset Random Sample Consensus (RANSAC) algorithm to perform cylindrical fitting on the initial frame data to establish a cylindrical model. The calculation formula for the set of structural parameters extracted by the system is as follows: Θ0={â0,c0,r0} Wherein: Θ0 represents the extracted set of structural parameters; â0 represents the central axis vector of the global geometric prior model (i.e., the unit vector of the pipe axis direction, represented in the world coordinate system); c0 represents the coordinates of the reference point on the central axis; and r0 represents the radius of the global geometric prior model (i.e., the inner diameter of the pipe).

[0056] Optionally, before the step of graph optimization based on the joint factor graph, the method further includes: calculating the current driving trajectory direction and the current heading angle based on the multimodal sensor data; detecting the angular deviation between the current driving trajectory direction and the central axis vector; when the angular deviation is greater than a first preset threshold, introducing a direction constraint factor in the joint factor graph; and using the angular deviation as a penalty term to correct the current heading angle through the direction constraint factor.

[0057] In the embodiments of this specification, when the robot briefly deviates from the pipeline axis due to obstacle avoidance, undulating terrain, or other reasons, the system calculates the included angle deviation using the following formula: θ t =arccos(|d t ·a0|) Where: θ t This represents the angular deviation between the current trajectory direction and the central axis vector; arccos(|·|) represents the inverse cosine function after taking the absolute value of the inner product, and taking the absolute value ensures that the robot will not be falsely triggered when turning back (opposite to the axis); d̂ t 'a' represents the current instantaneous displacement direction vector derived from wheel speed odometer data (i.e., the current driving trajectory direction); 'a0' represents the central axis vector of the global geometric prior model. It should be noted that this scheme assumes the device satisfies nonholonomic motion constraints, i.e., the driving direction is approximately consistent with the vehicle's heading; the direction constraint factor only applies to the yaw component.

[0058] When the included angle deviation exceeds a first preset threshold, a directional constraint factor is introduced to penalize and correct the heading angle. Quantitative experimental data shows that when the robot deviates 8° from the axis (5m length of the off-axis travel segment) due to obstacles in the pipeline, without correction, the map of the detour segment will have a lateral offset of approximately 700mm. After adding directional correction, the directional constraint of the off-axis segment guides the mapping back to the axial direction, reducing the lateral offset to less than 50mm, significantly improving the continuity of the mapping.

[0059] Since the geometric prior constraint factor has a Jacobian of zero in the degradation direction, pose drift along the axial direction needs to be constrained by an independent observation source. In one possible implementation, the multimodal sensor data further includes wheel speed odometer data; before the step of graph optimization processing based on the joint factor graph, the method further includes: obtaining the physical lever parameters between the first sensor that generates the inertial measurement data and the second sensor that generates the wheel speed odometer data; performing velocity observation deviation compensation on the wheel speed odometer data based on the physical lever parameters and the inertial measurement data to obtain compensated wheel speed data; and adding the compensated wheel speed data as a wheel speed constraint factor to the joint factor graph.

[0060] Further, the step of compensating for the speed observation deviation of the wheel speed odometer data based on the physical lever parameters and the inertial measurement data to obtain the compensated wheel speed data specifically includes: extracting angular velocity information from the inertial measurement data; performing a cross product calculation based on the angular velocity information and the physical lever parameters to obtain the compensated velocity component; transforming the original wheel speed in the wheel speed odometer data to the inertial measurement coordinate system through a pre-calibrated rotational extrinsic parameter, and then adding it to the compensated velocity component to obtain the compensated wheel speed data.

[0061] In the embodiments of this specification, since the inertial measurement unit and the wheel speed odometer have spatial levers in their physical installation, if the wheel speed observation is directly used as the velocity constraint of the inertial measurement unit measurement point without lever conversion during non-translational movements such as robot swaying in place, a systematic error proportional to the angular velocity will be introduced. Therefore, an equivalent compensation velocity formula is constructed as follows: b v O_b corr = b R bw · w v O_w enc + b ω̂× b r bw in: b v O_b corr This represents the compensated wheel speed data (i.e., the equivalent wheel speed observations in the IMU coordinate system). b R bw This represents the rotation matrix from the wheel velocity coordinate system to the IMU coordinate system; w v O_w enc This represents the raw wheel speed observation value output by the wheel speed odometer; b ω̂ represents the angular velocity information in the inertial measurement data (i.e., the angular velocity output by the gyroscope); b rbw The table represents the displacement vector (i.e., physical lever parameters) from the wheel speed measurement point to the inertial measurement unit measurement point. b ω̂× b r bw The overall representation is the compensated velocity component calculated based on the cross product of angular velocity information and physical lever parameters.

[0062] The compensated wheel speed data is added to the factor graph as a speed factor, and the formula is: F enc =| b v- b v O_b corr | 2 Σenc -1 Wherein: F enc Represents the wheel speed constraint factor; |·| 2 Σ Let represent the squared error function of the Mahalanobis distance based on the covariance matrix Σ; b v represents the IMU speed state variable to be optimized; b v O_b corr This represents the compensated wheel speed data; Σ enc -1 This represents the inverse of the covariance matrix of the observed wheel speeds.

[0063] Optionally, before the step of performing graph optimization processing based on the joint factor graph, the method further includes: extracting point cloud features from the lidar point cloud data; identifying fixed structure joint features in the degraded environment based on the point cloud features; and updating the joint factor graph based on the fixed structure joint features.

[0064] Furthermore, the fixed structure seam features are obtained by clustering and identifying abrupt changes in reflection intensity or geometric curvature in the point cloud features.

[0065] Furthermore, the step of updating the joint factor graph based on the fixed structural joint features specifically includes: counting the fixed structural joint features to obtain the number of joint features; calculating the current absolute position by combining the preset physical spacing between the fixed structural joint features and the number of joint features; and updating the joint factor graph with the current absolute position as an absolute position constraint factor.

[0066] In the embodiments described in this specification, the system can also provide discrete absolute position anchor points using the number of welds or joints inside the pipe. The formula for calculating the absolute position is: P abs =N×L+P0 Among them, Pabs This represents the calculated current absolute position (a one-dimensional position component along the pipe axis); N represents the number of fixed structural joint features; L represents the preset physical spacing between fixed structural joint features; P0 represents the axial distance from the equipment's starting position to the first joint (an initial value can be given by the integral of the initial segment wheel speed). It should be noted that, to avoid absolute position jumps caused by N value deviations due to missed or false weld inspections, consistency gating is required before adding the current absolute position as an absolute position constraint factor to the joint factor graph: when |N×L+P0−p̂ ax ᵢ al When |>γ (where p̂) ax ᵢ al The anchor point is marked as an anomaly and discarded, and is not added to the factor graph, where γ is the gating threshold (the current axial pose estimate of the joint factor graph). Furthermore, when the equipment is traveling back and forth in a degraded environment, the update direction of N depends on the sign relationship between the current travel direction and the pipeline axis direction; that is, N increases when traveling forward and decreases when traveling backward.

[0067] In addition, the system can also use UWB or 5GRTT ranging results at the entrance of the degraded environment to perform a one-time correction of the global pose as a global zero-point correction of the map; or introduce a non-contact odometer interpolation module to use odometer integration to interpolate the position between adjacent absolute position anchor points.

[0068] To more clearly demonstrate the actual effect of the proposed solution, detailed embodiments of three quantitative experiments are provided below.

[0069] Example 1: Comparison of Graph Construction Before and After Cylindrical Constraints The test scenario involved a steel pipe with a diameter of 600mm and a total length of 50m, with the equipment moving forward at a constant speed of 0.3m / s.

[0070] If a baseline scheme without cylindrical constraints is adopted (only point-to-surface constraints and IMU pre-integration), after the mapping is completed, the pipe wall will bend by a maximum of 47 mm in the axial direction, the pipe profile will show obvious delamination at 30 m, and the axial displacement will jump by about 12 mm.

[0071] If the scheme of this application is adopted, prior geometric constraints for the cylinder are introduced (setting r0=300mm, ε=15mm, σ...). cyl =5mm). After mapping, the pipe wall curvature was significantly reduced to 3.2mm, and the cross-sectional delamination was completely eliminated. In terms of computational overhead, initializing the cylinder fitting took only about 3.1ms, and the additional time for finding points and calculating residuals in each frame was about 0.8ms, which accounted for a very small percentage of the computation.

[0072] Example 2: Comparison of speed deviation before and after lever arm compensation The test scenario involves the device adjusting its attitude at the entrance to a degraded environment, resulting in pitch and sway (pitch angular velocity ω). pitch =0.3rad / s), the wheel speed odometer output is zero, and the vertical component r of the physical lever parameter from the IMU to the drive wheel is zero. bw,z =0.08m.

[0073] If lever arm compensation is not performed, the formula for calculating the velocity deviation introduced by the lever arm effect is: Δv=|ω pitch ×r bw,z |=0.3×0.08=0.024m / s Where: Δv represents the velocity deviation introduced by the lever effect; ω pitch The r represents the robot's pitch angular velocity, measured in rad / s. bw,z This represents the component of the physical lever arm parameter in the vertical direction (Z-axis direction), with the unit being meters (m).

[0074] Directly using the raw wheel speed observation, which outputs zero, as the velocity constraint at the inertial measurement unit (IMU) measurement point will incorrectly pull the velocity state towards zero, thus creating a systematic deviation from the actual motion state reflected by the IMU. After 10 seconds of integration, this results in a position error of approximately 0.24 m. However, by adopting the scheme described in this application, through the aforementioned cross-product compensation mechanism, the raw wheel speed is converted into an equivalent velocity observation value of 0.024 m / s at the IMU measurement point. The system velocity deviation is reduced to a noise level of less than 1 mm / s, and the position error caused by stationary swaying is effectively eliminated.

[0075] Example 3: Mapping Correction for Deviation of Driving Direction from Axis The test scenario involved the equipment deviating from its axis in a pipeline due to obstacles, with an included angle deviation of up to 8° and a length of 5m for the off-axis travel section.

[0076] Without directional correction, the detour map will have a lateral offset of about 700mm, causing the equipment to be misaligned with the main axis map after turning back.

[0077] When the proposed solution is adopted and the trigger threshold is set to 5°, the directional constraint factor forces the mapping projection direction to the central axis direction. After the device turns back, the axis is continuous and the lateral offset is reduced to less than 50mm, which is within the normal positioning accuracy range of the sensor.

[0078] In one possible implementation, this application also provides a laser inertial odometry mapping method for a degraded environment, comprising: acquiring multimodal sensor data in a degraded environment; processing the multimodal sensor data using the aforementioned laser inertial odometry drift suppression method for a degraded environment to obtain a target pose estimation result; and, based on the target pose estimation result, performing global consistency stitching on the lidar point cloud data in the multimodal sensor data to construct a global map of the degraded environment.

[0079] In the embodiments described in this specification, by combining the above-described drift suppression method, a high-quality global map can be constructed.

[0080] Combined with appendix Figure 3 This application also provides a laser inertial odometry drift mapping device for use in degraded environments. The device includes: The data acquisition module is used to acquire multimodal sensor data under degraded environments; The pose calculation module is used to process the multimodal sensor data using the aforementioned laser inertial odometry drift suppression method under degraded environment to obtain the target pose estimation result; The mapping and stitching module is used to perform globally consistent stitching of the lidar point cloud data in the multimodal sensor data based on the target pose estimation results, and to construct a global map of the degraded environment.

[0081] The specific functions of each of the above modules have been described in detail in the corresponding method embodiments, and will not be repeated here.

[0082] Furthermore, embodiments of this application also provide an electronic device, including a processor and a memory, wherein the memory stores a computer program, and when the computer program is executed by the processor, it implements the aforementioned laser inertial odometry drift suppression method under degradation environment, or implements the aforementioned laser inertial odometry mapping method under degradation environment.

[0083] In addition, this application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the aforementioned laser inertial odometry drift suppression method under degradation environment, or the aforementioned laser inertial odometry mapping method under degradation environment.

[0084] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A method for suppressing drift in a laser inertial odometer under degraded conditions, characterized in that, include: Acquire multimodal sensor data under degraded environments, wherein the multimodal sensor data includes at least lidar point cloud data and inertial measurement data; Geometric features are extracted from at least one frame of the lidar point cloud data to establish a global geometric prior model of the degraded environment, and the structural parameters of the global geometric prior model are extracted. In the processing of subsequent frames, the distance residual term of the current frame's lidar point cloud data to the global geometric prior model is constructed based on the structural parameters. Construct a joint factor graph and add the distance residual term as a geometric prior constraint factor to the joint factor graph to constrain the degree of freedom of the pose in the normal direction of the global geometric prior model; Graph optimization processing is performed based on the joint factor graph to output the target pose estimation result.

2. The drift suppression method according to claim 1, characterized in that, The steps of establishing a global geometric prior model of the degraded environment and extracting the structural parameters of the global geometric prior model specifically include: A cylinder model is established by fitting the at least one frame of lidar point cloud data using a preset random sampling consensus algorithm, and the cylinder model is used as the global geometric prior model. The central axis vector and radius of the cylindrical model are extracted as the structural parameters.

3. The drift suppression method according to claim 2, characterized in that, The steps for constructing the distance residual term from the current frame LiDAR point cloud data to the global geometric prior model based on the structural parameters specifically include: Extract target feature points from the lidar point cloud data of the current frame; Calculate the vertical distance from the target feature point to the central axis vector; The difference between the vertical distance and the radius is used as the distance residual term.

4. The drift suppression method according to claim 2, characterized in that, Before the step of performing graph optimization processing based on the joint factor graph, the method further includes: The current driving trajectory direction and current heading angle are calculated based on the multimodal sensor data; Detect the angular deviation between the current driving trajectory direction and the central axis vector; When the included angle deviation is greater than a first preset threshold, a direction constraint factor is introduced into the joint factor diagram; The included angle deviation is used as a penalty term by the direction constraint factor to correct the current heading angle.

5. The drift suppression method according to claim 1, characterized in that, The multimodal sensor data also includes wheel speed odometer data; prior to the step of graph optimization based on the joint factor graph, the following steps are also included: Acquire the physical lever parameters between the first sensor that generates the inertial measurement data and the second sensor that generates the wheel speed odometer data; Based on the physical lever parameters and the inertial measurement data, the wheel speed odometer data is compensated for speed observation deviation to obtain compensated wheel speed data; The compensated wheel speed data is added to the joint factor graph as a wheel speed constraint factor.

6. The drift suppression method according to claim 5, characterized in that, The step of compensating for speed observation deviations in the wheel speed odometer data based on the physical lever parameters and the inertial measurement data to obtain compensated wheel speed data specifically includes: Extract the angular velocity information from the inertial measurement data; Based on the angular velocity information and the physical lever parameters, a cross product calculation is performed to obtain the compensated velocity component; The original wheel speed in the wheel speed odometer data is transformed to the inertial measurement coordinate system through a pre-calibrated rotational extrinsic parameter, and then added to the compensated speed component to obtain the compensated wheel speed data.

7. The drift suppression method according to claim 1, characterized in that, Before the step of performing graph optimization processing based on the joint factor graph, the method further includes: Extract point cloud features from the lidar point cloud data; Based on the point cloud features, the fixed structure joint features in the degraded environment are identified; The joint factor graph is updated based on the fixed structural joint features.

8. The drift suppression method according to claim 7, characterized in that, The fixed structure seam features are obtained by clustering and identifying abrupt changes in reflection intensity or geometric curvature in the point cloud features.

9. The drift suppression method according to claim 7 or 8, characterized in that, The step of updating the joint factor map based on the fixed structural joint features specifically includes: The number of joint features in the fixed structure is obtained by counting the joint features. The current absolute position is calculated by combining the preset physical spacing between the fixed structural joint features and the number of the joint features; The current absolute position is used as an absolute position constraint factor to update the joint factor graph.

10. A method for mapping using a laser inertial odometry under degraded environments, characterized in that, include: Acquire multimodal sensor data under degraded environments; The multimodal sensor data is processed using the laser inertial odometry drift suppression method under degraded environment as described in any one of claims 1 to 9 to obtain the target pose estimation result; Based on the target pose estimation results, the lidar point cloud data in the multimodal sensor data are globally consistent and stitched together to construct a global map of the degraded environment.

11. A laser inertial odometry drift mapping device for degraded environments, characterized in that, include: The data acquisition module is used to acquire multimodal sensor data under degraded environments; The pose calculation module is used to process the multimodal sensor data using the laser inertial odometry drift suppression method under the degradation environment as described in any one of claims 1 to 9, and obtain the target pose estimation result; The mapping and stitching module is used to perform globally consistent stitching of the lidar point cloud data in the multimodal sensor data based on the target pose estimation results, and to construct a global map of the degraded environment.

12. An electronic device, characterized in that, The system includes a processor and a memory, the memory storing a computer program that, when executed by the processor, implements the laser inertial odometry drift suppression method under degraded conditions as described in any one of claims 1-9, or implements the laser inertial odometry mapping method under degraded conditions as described in claim 10.

13. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the laser inertial odometry drift suppression method under degraded conditions as described in any one of claims 1-9, or the laser inertial odometry mapping method under degraded conditions as described in claim 10.

Images