Environmental map construction method, device and equipment of degraded environment and medium

Abstract

The invention relates to a degradation environment mapping technology, and discloses a degradation environment environment map construction method comprising the following steps: obtaining current frame IMU data, and carrying out IMU integral calculation on the current frame IMU data and historical frame IMU data to obtain a current frame IMU factor; obtaining GPS data of a current frame, and calculating a GPS factor of the current frame by combining the IMU data of the current frame and utilizing a preset filtering algorithm; acquiring current frame laser radar data, and performing feature extraction on the current frame laser radar data to obtain laser radar features; performing degradation detection according to the laser radar features and then performing point cloud registration to obtain laser radar factors; and performing factor graph optimization on the laser radar factor, the current frame IMU factor and the current frame GPS factor by using a factor graph optimization algorithm to obtain a degraded environment map. The invention further provides an environment map construction device of the degraded environment, electronic equipment and a storage medium. According to the invention, the accuracy of mapping in the degraded environment can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of mapping in degraded environments, and particularly to a method, device, equipment, and medium for constructing an environmental map of a degraded environment. Background Art

[0002] The laser SLAM (Simultaneous Localization and Mapping) technology refers to a robot's perception of the environment through its own sensors (such as lidar, camera, etc.), and based on this perception information, constructing an environmental map while determining its own position in the map.

[0003] In the real road environment, in various degraded scenarios such as tunnels, bridges, and open roads, traditional LiDAR (Light Detection and Ranging)-based SLAM algorithms often have difficulty constructing accurate maps due to the lack of geometric information in these degraded environments. Existing SLAM optimization algorithms are usually based on graph optimization and Kalman filtering. However, the graph optimization-based SLAM optimization algorithm will fail to build a map due to the lack of geometric information in the degraded environment, and the Kalman filtering-based SLAM optimization algorithm will have a large error in point cloud registration due to the lack of geometric information in the degraded environment, causing the filtering system to quickly accumulate errors and resulting in map construction failure in a long degraded scenario. It can be seen that both of the above two SLAM optimization algorithms will have problems of map construction failure due to inaccurate point cloud registration in the degraded environment. Summary of the Invention

[0004] The present invention provides a method, device, equipment, and medium for constructing an environmental map of a degraded environment, which can improve the accuracy of map construction in a degraded environment.

[0005] To achieve the above object, a method for constructing an environmental map of a degraded environment provided by the present invention includes:

[0006] Obtain the current frame of IMU data, and perform IMU integration calculation on the current frame of IMU data and historical frame of IMU data to obtain the current frame of IMU factor;

[0007] Obtain the current frame of GPS data, and calculate the current frame of GPS factor by using a preset filtering algorithm in combination with the current frame of IMU data;

[0008] Obtain the current frame of lidar data, and perform feature extraction on the current frame of lidar data to obtain lidar features;

[0009] Perform degradation detection according to the lidar features and then perform point cloud registration to obtain the lidar factor;

[0010] Use the factor graph optimization algorithm to optimize the lidar factor, the current frame IMU factor, and the current frame GPS factor to obtain a degraded environment map.

[0011] Optionally, the performing degradation detection according to the lidar characteristics further includes:

[0012] Perform degradation detection according to the lidar characteristics to obtain a degradation detection result;

[0013] Judge the environmental degradation state according to the degradation detection result, and select the initial pose of the degraded environment according to the environmental degradation state.

[0014] Optionally, the judging the environmental degradation state according to the degradation detection result includes:

[0015] Extract the edge points along the lidar scan line in the current frame lidar data, and count the number of edge points;

[0016] Judge whether the current frame environment is a degraded environment according to the number of edge points;

[0017] When the number of edge points is greater than or equal to the preset edge point number threshold, it is judged that the current frame environment is not a degraded environment;

[0018] When the number of edge points is less than the preset edge point number threshold, it is judged that the current frame environment is a degraded environment.

[0019] Optionally, the selecting the initial pose of the degraded environment according to the environmental degradation state includes:

[0020] When the environmental degradation state is non-degraded, use the current frame IMU factor for IMU initialization to obtain the IMU initial pose, use the IMU initial pose to perform point cloud matching with the historical map to obtain the first lidar factor; use the factor graph optimization algorithm to optimize the first lidar factor, the current frame IMU factor, and the GPS factor, and obtain a degraded environment map after the optimization is completed;

[0021] When the environmental degradation state is degraded, use the current frame GPS factor for GPS initialization to obtain the GPS initial pose, use the GPS initial pose to perform point cloud matching with the historical map to obtain the second lidar factor; use the factor graph optimization algorithm to optimize the second lidar factor, the current frame IMU factor, and the GPS factor, and obtain a degraded environment map after the optimization is completed.

[0022] Optionally, the performing point cloud registration to obtain the lidar factor includes:

[0023] Calculate the curvature of all points along the lidar scan line according to the current frame lidar data;

[0024] Extract all feature points of the current frame lidar data according to the curvature, convert all feature points to the global coordinate system, and construct a local map;

[0025] Use the point cloud registration method to perform point cloud registration of the current frame lidar data to the local map to obtain the lidar factor.

[0026] Optionally, the converting all feature points to the global coordinate system and constructing the local map includes:

[0027] Sort the feature points corresponding to the curvature from large to small, and extract a preset number of feature points from the front of the sorting as edge points;

[0028] Extract a preset number of feature points from the back of the sorting as plane points;

[0029] Construct an edge point local map according to the edge points, construct a plane point local map according to the plane points, and integrate the edge point local map and the plane point local map to obtain the local map.

[0030] Optionally, the performing IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor includes:

[0031] Obtain the acceleration and angular velocity in the current frame IMU data;

[0032] Generate the current frame IMU factor according to the acceleration and angular velocity by using the IMU pre-integration technology.

[0033] To solve the above problems, the present invention also provides an environmental map construction device for a degraded environment, and the device includes:

[0034] A factor calculation module, configured to obtain the current frame IMU data, and perform IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor; obtain the current frame GPS data, and calculate the current frame GPS factor by using a preset filtering algorithm in combination with the current frame IMU data; obtain the current frame lidar data, and perform feature extraction on the current frame lidar data to obtain lidar features; perform degradation detection according to the lidar features and then perform point cloud registration to obtain the lidar factor;

[0035] A degraded environment map construction module, configured to perform factor graph optimization on the lidar factor, the current frame IMU factor, and the current frame GPS factor by using the factor graph optimization algorithm to obtain a degraded environment map.

[0036] To solve the above problems, the present invention also provides an electronic device, and the electronic device includes:

[0037] At least one processor; and,

[0038] A memory communicatively connected to the at least one processor; wherein,

[0039] The memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor to enable the at least one processor to execute the environmental map construction method for the degraded environment described above.

[0040] To solve the above problems, the present invention also provides a computer-readable storage medium, in which at least one computer program is stored, and the at least one computer program is executed by a processor in an electronic device to implement the environmental map construction method for the degraded environment described above.

[0041] The present invention can achieve high-precision motion estimation of a robot in a short time by obtaining current-frame IMU data and performing IMU integration calculation on the current-frame IMU data and historical-frame IMU data to obtain current-frame IMU factors. Even in a degraded environment lacking geometric information, high-precision basic information can be obtained, improving the accuracy in subsequent environmental mapping. Additionally, by obtaining current-frame GPS data and using a preset filtering algorithm to calculate current-frame GPS factors in combination with the current-frame IMU data, the position information obtained from the GPS data can be combined with the high-frequency motion estimation of the IMU to improve the positioning accuracy. Furthermore, by performing feature extraction on the current-frame lidar data to obtain lidar features, key features can be extracted from the lidar point cloud data, and after performing degradation detection based on the lidar features and then performing point cloud registration to obtain lidar factors, accurate identification of the degraded environment and dynamic adjustment of point cloud registration can be achieved. Finally, by combining multiple factors and performing factor graph optimization to obtain a degraded environment map, the accuracy of degraded environment mapping can be improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is a schematic flowchart of the environmental map construction method for the degraded environment provided by an embodiment of the present invention;

[0043] Figure 2 It is a schematic diagram of the algorithm framework of the environmental map construction method for the degraded environment provided by an embodiment of the present invention;

[0044] Figure 3 It is a schematic diagram of the degradation detection principle of the environmental map construction method for the degraded environment provided by an embodiment of the present invention;

[0045] Figure 4 It is a comparison diagram of the technical effects of multi-technology implementation of environmental mapping for the environmental map construction method for the degraded environment provided by an embodiment of the present invention;

[0046] Figure 5An example diagram for environmental mapping of the environmental map construction method for a degraded environment provided by an embodiment of the present invention;

[0047] Figure 6 A functional module diagram of an environmental map construction device for a degraded environment provided by an embodiment of the present invention;

[0048] Figure 7 A schematic structural diagram of an electronic device for implementing the environmental map construction method for a degraded environment provided by an embodiment of the present invention.

[0049] The realization, functional features, and advantages of the present invention will be further described with reference to the embodiments and the accompanying drawings. Detailed implementation manners

[0050] It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0051] The embodiments of the present application provide an environmental map construction method for a degraded environment. The execution subject of the environmental map construction method for a degraded environment includes, but is not limited to, at least one of electronic devices such as a server, a terminal, etc. that can be configured to execute the method provided by the embodiments of the present application. In other words, the environmental map construction method for a degraded environment can be executed by software or hardware installed on a terminal device or a server device, and the software can be a blockchain platform. The server includes, but is not limited to: a single server, a server cluster, a cloud server, or a cloud server cluster, etc. The server can be an independent server or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, domain name services, security services, Content Delivery Network (CDN), and big data and artificial intelligence platforms.

[0052] Refer to Figure 1 As shown, it is a flowchart of the environmental map construction method for a degraded environment provided by an embodiment of the present invention. In this embodiment, the environmental map construction method for a degraded environment includes:

[0053] S1. Obtain the current frame of IMU data, and perform IMU integration calculation on the current frame of IMU data and the historical frame of IMU data to obtain the current frame of IMU factor.

[0054] In the embodiments of the present invention, the current frame IMU data refers to a set of IMU measurement values that are temporally aligned with the lidar frame data being currently processed. Here, IMU (Inertial Measurement Unit) refers to an inertial measurement unit, which is a sensor device that measures the motion state of an object through the inertial principle and is applied in fields such as navigation, attitude control, and motion tracking. Its core is to obtain the acceleration, angular velocity, and magnetic field information of an object in real time through multi-sensor fusion, and then calculate parameters such as position, velocity, and attitude.

[0055] In the embodiments of the present invention, let W represent the world coordinate system (i.e., the GPS coordinate system), B represent the IMU coordinate system, and L represent the lidar coordinate system. The state of the robot is defined as:

[0056] x = [R T , p T , V T , b T where R ∈ SO(3) is the rotation matrix, is the position vector, V is the velocity vector, b is the IMU bias vector, and the transformation T ∈ SE(3) from coordinate system L to coordinate system B is expressed as T = [R|p].

[0057] As an embodiment of the present invention, performing IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor includes:

[0058] Obtaining the acceleration and angular velocity in the current frame IMU data;

[0059] Generating the current frame IMU factor using the IMU pre-integration technique based on the acceleration and angular velocity.

[0060] Exemplarily, performing IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor can adopt the following implementation steps:

[0061] Step 1. Use the following formula to define the measurement values of the acceleration and angular velocity of the IMU:

[0062]

[0063] where, is the measurement value of the angular velocity of the IMU at time t in coordinate system B, is the acceleration of the IMU at time t in coordinate system B, is the deviation of the angular velocity, is the Gaussian white noise, is the rotation matrix from the W coordinate system to the B coordinate system at time t, g is the gravitational acceleration vector in the world coordinate system, is the deviation of the acceleration, is Gaussian white noise.

[0064] Step 2: The pose and velocity of the robot at time t+Δt can be calculated using the following formula:

[0065]

[0066] where v t is the motion velocity of the robot in the global coordinate system at time t, R t is the rotation matrix of the robot from its own coordinate system to the global coordinate system at time t, is the original acceleration measurement value, is the acceleration bias, is the random noise of the acceleration, Δt is the time increment, is the original angular velocity measurement value, is the original angular velocity bias, is the random noise of the angular velocity, exp(·) is the exponential map that converts the angular velocity increment into a rotation matrix.

[0067] Step 3: The acceleration and angular velocity are continuous during the integration process. The current state is updated using the above formula with the IMU and historical state, and is added as a factor node to the factor graph.

[0068] S2: Obtain the current frame GPS data, and calculate the current frame GPS factor by combining the current frame IMU data using a preset filtering algorithm.

[0069] In the embodiments of the present invention, the current frame GPS data refers to the GPS data that is time-aligned with the lidar frame data currently being processed, and is used to provide global position constraints.

[0070] In the embodiments of the present invention, the preset filtering algorithm can adopt ESKF (Error-State Kalman Filter), where ESKF refers to a state estimation method for nonlinear systems to achieve accurate state estimation, usually including a nominal state and an error state. For example, the position, velocity, and attitude directly calculated by IMU integration are used as the nominal state, and the small deviations between the actual state and the nominal state (such as displacement error, velocity error, attitude error, sensor bias error) are used as the error state.

[0071] S3: Obtain the current frame lidar data, and perform feature extraction on the current frame lidar data to obtain lidar features.

[0072] In an embodiment of the present invention, Light Detection and Ranging (LiDAR) refers to an active remote sensing technology that detects the distance, shape, and surface characteristics of target objects by emitting lasers and measuring their reflected signals.

[0073] In an embodiment of the present invention, the current frame LiDAR data is a set of data collected by the LiDAR and temporally aligned with the current frame IMU data.

[0074] As an embodiment of the present invention, feature extraction is performed on the current frame LiDAR data to obtain LiDAR features, including:

[0075] Parse the current frame LiDAR data into point cloud data and remove the noise points in the point cloud data;

[0076] Classify the point cloud data after removing the noise points according to the LiDAR scan lines to obtain multiple sub-point cloud blocks;

[0077] Use the dynamic threshold segmentation technology to divide the multiple sub-point cloud blocks into different regions, and perform feature extraction according to the curvatures of different point clouds in different regions to obtain LiDAR features.

[0078] In an embodiment of the present invention, the dynamic threshold segmentation technology (Dynamic Thresholding Segmentation) refers to an image processing method that automatically adjusts the threshold according to local image features or environmental changes. Its core is to dynamically calculate the threshold of each pixel or region instead of using a fixed global threshold, so as to achieve a more accurate separation of the target and the background, and is applicable to scenarios with uneven illumination, noise interference, or complex backgrounds.

[0079] S4. Perform degradation detection based on the LiDAR features and then perform point cloud registration to obtain the LiDAR factor.

[0080] In an embodiment of the present invention, point cloud registration refers to the process of aligning multiple point clouds collected from different perspectives or times to the same coordinate system by calculating a rigid body transformation matrix. Its core goal is to solve the geometric alignment problem of point cloud data in three-dimensional space and further meet the requirements of generating a complete scene representation or achieving high-precision positioning.

[0081] As an embodiment of the present invention, performing degradation detection based on the LiDAR features further includes:

[0082] Perform degradation detection based on the LiDAR features to obtain a degradation detection result;

[0083] Judge the environmental degradation state according to the degradation detection result, and select the initial pose of the degraded environment according to the environmental degradation state.

[0084] Further, judging the environmental degradation state according to the degradation detection result includes:

[0085] Extract the edge points along the lidar scan line from the current frame lidar data and count the number of edge points;

[0086] Judge whether the current frame environment is a degraded environment according to the number of edge points;

[0087] When the number of edge points is greater than or equal to the preset edge point number threshold, it is judged that the current frame environment is not a degraded environment;

[0088] When the number of edge points is less than the preset edge point number threshold, it is judged that the current frame environment is a degraded environment.

[0089] Furthermore, select the initial pose of the degraded environment according to the environmental degradation state, including:

[0090] When the environmental degradation state is non-degraded, use the current frame IMU factor to perform IMU initialization to obtain the IMU initial pose, and use the IMU initial pose to perform point cloud matching with the historical map to obtain the first lidar factor; use the factor graph optimization algorithm to optimize the first lidar factor, the current frame IMU factor, and the GPS factor, and obtain the degraded environment map after the optimization is completed;

[0091] When the environmental degradation state is degraded, use the current frame GPS factor to perform GPS initialization to obtain the GPS initial pose, and use the GPS initial pose to perform point cloud matching with the historical map to obtain the second lidar factor; use the factor graph optimization algorithm to optimize the second lidar factor, the current frame IMU factor, and the GPS factor, and obtain the degraded environment map after the optimization is completed.

[0092] Furthermore, perform point cloud registration to obtain the lidar factor, including:

[0093] Calculate the curvature of all points along the lidar scan line according to the current frame lidar data;

[0094] Extract all feature points of the current frame lidar data according to the curvature, and transform all feature points to the global coordinate system and construct a local map;

[0095] Use the point cloud registration method to perform point cloud registration of the current frame lidar data to the local map to obtain the lidar factor.

[0096] In the embodiments of the present invention, the point cloud registration method refers to the technology of aligning multiple different point cloud data sets to a unified coordinate system. The ICP (Iterative Closest Point) point cloud registration is often used. The ICP point cloud registration refers to a three-dimensional point cloud alignment algorithm based on iterative optimization. Its core goal is to solve the optimal rigid body transformation (rotation matrix and translation vector) by minimizing the spatial distance between two groups of point clouds, so that the point clouds collected from different perspectives or times can achieve high-precision alignment of the point clouds in the unified coordinate system.

[0097] Further, converting all feature points to the global coordinate system and constructing a local map includes:

[0098] Sort the feature points corresponding to the curvature from large to small, and extract a preset number of feature points from the front of the sorting as edge points;

[0099] Extract a preset number of feature points from the back of the sorting as plane points;

[0100] Construct a local map of edge points according to the edge points, construct a local map of plane points according to the plane points, and integrate the local map of edge points and the local map of plane points to obtain the local map.

[0101] In the embodiments of the present invention, the curvature c of all points along the LiDAR scan line is calculated according to the current frame of LiDAR data, and the following formula can be used:

[0102]

[0103] where S is the set of points of point i on the same LiDAR scan line, and |S| represents the number of points included in set S, is the coordinate of point i in the LiDAR coordinate system, is the coordinate norm of point i in the LiDAR coordinate system, that is, the Euclidean distance from point i to the LiDAR center, k represents the kth frame, and point j is the neighboring point of point i.

[0104] Exemplarily, to extract all feature points of the current frame of LiDAR data according to the curvature, the following steps can be adopted:

[0105] The point with the largest median value of the curvature c is represented as an edge point, denoted by , and the point with the smallest median value of the curvature c is represented as a plane point, denoted by . In the current frame, all feature points are represented by F i where

[0106] In the embodiments of the present invention, constructing a local map of edge points according to the edge points, constructing a local map of plane points according to the plane points, and integrating the local map of edge points and the local map of plane points to obtain the local map is represented by the following formula:

[0107]

[0108] Among them, M i is a local map, is a local map of edge points, is a local map of planar points, and the symbol ′ is for coordinate transformation of feature points.

[0109] S5. Use the factor graph optimization algorithm to optimize the LiDAR factor, the current frame IMU factor, and the current frame GPS factor to obtain a degraded environment map.

[0110] In the embodiments of the present invention, the factor graph optimization algorithm refers to a non - linear optimization method based on a graph model. By modeling multi - sensor data as factors and constructing a global optimization problem to solve the robot pose and the environment map. In the scenario of multi - sensor fusion of LiDAR, IMU, and GPS, its core idea is to model the observation constraints of each sensor as factor nodes, express the dependency relationship between variables through a graph structure, and achieve high - precision state estimation.

[0111] The present invention obtains the current frame IMU data, and performs IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor, which can achieve high - precision motion estimation of the robot in a short time. Even in a degraded environment lacking geometric information, high - precision basic information can be obtained, improving the accuracy during subsequent environment mapping. In addition, by obtaining the current frame GPS data and using a preset filtering algorithm to calculate the current frame GPS factor in combination with the current frame IMU data, the position information obtained from the GPS data can be combined with the high - frequency motion estimation of the IMU to improve the positioning accuracy. Furthermore, by performing feature extraction on the current frame LiDAR data to obtain LiDAR features, key features can be extracted from the LiDAR point cloud data, and after performing degradation detection based on the LiDAR features and then performing point cloud registration to obtain the LiDAR factor, accurate identification of the degraded environment and dynamic adjustment of point cloud registration can be achieved. Finally, by combining multiple factors and performing factor graph optimization to obtain a degraded environment map, the accuracy of degraded environment mapping can be improved.

[0112] As Figure 2 shown, it is a schematic diagram of the algorithm framework of the method for constructing an environment map of a degraded environment provided by an embodiment of the present invention.

[0113] As Figure 3 shown, it is a schematic diagram of the degradation detection principle of the method for constructing an environment map of a degraded environment provided by an embodiment of the present invention.

[0114] In the embodiments of the present invention, blue dots and green dots represent two different frames of point clouds. When using the ICP registration method, it relies on obvious jump points (edge feature points) in the point cloud. Usually, the following steps are used for degradation detection: Extract edge points along the lidar scan line in the current frame lidar data, and count the number of edge points; Determine whether the current frame environment is a degraded environment based on the number of edge points; When the number of edge points is greater than or equal to the preset edge point number threshold, it is determined that the current frame environment is not a degraded environment; When the number of edge points is less than the preset edge point number threshold, it is determined that the current frame environment is a degraded environment.

[0115] As Figure 4 shown, it is a technical effect comparison diagram of multi-technology implementation for environment mapping of the environment map construction method for a degraded environment provided by an embodiment of the present invention.

[0116] As Figure 5 shown, it is an example diagram of environment mapping of the environment map construction method for a degraded environment provided by an embodiment of the present invention.

[0117] As Figure 6 shown, it is a functional module diagram of an environment map construction device for a degraded environment provided by an embodiment of the present invention.

[0118] The environment map construction device 100 for a degraded environment described in the present invention can be installed in an electronic device. According to the functions achieved, the environment map construction device 100 for a degraded environment can include a factor calculation module 101 and a degraded environment map construction module 102.

[0119] The module described in the present invention can also be referred to as a unit, which refers to a series of computer program segments that can be executed by an electronic device processor and can complete fixed functions, and are stored in the memory of the electronic device.

[0120] In this embodiment, the functions of each module / unit are as follows:

[0121] The factor calculation module 101 is used to obtain the current frame IMU data, perform IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor; Obtain the current frame GPS data, and calculate the current frame GPS factor by using a preset filtering algorithm in combination with the current frame IMU data; Obtain the current frame lidar data, and perform feature extraction on the current frame lidar data to obtain lidar features; After performing degradation detection according to the lidar features, perform point cloud registration to obtain the lidar factor.

[0122] In the embodiments of the present invention, the current frame IMU data refers to a set of IMU measurement values that are temporally aligned with the lidar frame data being currently processed. IMU (Inertial Measurement Unit) refers to an inertial measurement unit, which is a sensor device that measures the motion state of an object through the inertial principle and is applied in fields such as navigation, attitude control, and motion tracking. Its core is to obtain the acceleration, angular velocity, and magnetic field information of an object in real time through multi-sensor fusion, and then calculate parameters such as position, velocity, and attitude.

[0123] In the embodiments of the present invention, W is represented as the world coordinate system (i.e., the GPS coordinate system), B is represented as the IMU coordinate system, and L is represented as the lidar coordinate system. The state of the robot is defined as:

[0124] x = [R T , p T , V T , b T

[0125] where R ∈ SO(3) is the rotation matrix, is the position vector, V is the velocity vector, b is the IMU bias vector, and the transformation T ∈ SE(3) from the coordinate system L to the coordinate system B is represented as T = [R|p].

[0126] As an embodiment of the present invention, performing IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor includes:

[0127] Obtaining the acceleration and angular velocity in the current frame IMU data;

[0128] Generating the current frame IMU factor using the IMU pre-integration technique based on the acceleration and angular velocity.

[0129] Exemplarily, performing IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor can adopt the following implementation steps:

[0130] Step 1. Use the following formula to define the measurement values of the acceleration and angular velocity of the IMU:

[0131]

[0132] where, is the measurement value of the angular velocity of the IMU at time t in the B coordinate system, is the acceleration of the IMU at time t in the B coordinate system, is the deviation of the angular velocity, is the Gaussian white noise, ​is the rotation matrix from the W coordinate system to the B coordinate system at time t, and g is the gravitational acceleration vector in the world coordinate system. is the deviation of the acceleration. is Gaussian white noise.

[0133] Step 2: The pose and velocity of the robot at time t + Δt can be calculated using the following formula:

[0134]

[0135] where v t is the motion velocity of the robot in the global coordinate system at time t, and R t is the rotation matrix of the robot from its own coordinate system to the global coordinate system at time t. is the raw acceleration measurement value. is the acceleration bias. is the random noise of the acceleration. Δt is the time increment. is the raw angular velocity measurement value. is the raw angular velocity bias. is the random noise of the angular velocity. exp(·) is the exponential map that converts the angular velocity increment into a rotation matrix.

[0136] Step 3: The acceleration and angular velocity are continuous during the integration process. The current state is updated using the above formula with the IMU and historical state, and is added as a factor node to the factor graph.

[0137] In the embodiments of the present invention, the current frame of GPS data refers to the GPS data that is time-aligned with the currently processed lidar frame data and is used to provide global position constraints.

[0138] In the embodiments of the present invention, the preset filtering algorithm can adopt ESKF (Error-State Kalman Filter). ESKF is a state estimation method for nonlinear systems, which can achieve accurate state estimation. It usually includes a nominal state and an error state. For example, the position, velocity, and attitude directly calculated by IMU integration are used as the nominal state, and the small deviations between the actual state and the nominal state (such as displacement error, velocity error, attitude error, sensor bias error) are used as the error state.

[0139] In the embodiments of the present invention, lidar (Light Detection and Ranging) refers to an active remote sensing technology that detects the distance, shape, and surface characteristics of target objects by emitting lasers and measuring their reflected signals.

[0140] In the embodiments of the present invention, the current-frame lidar data is a set of data collected by the lidar and temporally aligned with the current-frame IMU data.

[0141] As an embodiment of the present invention, feature extraction is performed on the current-frame lidar data to obtain lidar features, including:

[0142] Parse the current-frame lidar data into point cloud data and remove the noise points in the point cloud data;

[0143] Classify the point cloud data after removing the noise points according to the lidar scan lines to obtain multiple sub-point cloud blocks;

[0144] Use the dynamic threshold segmentation technique to divide the multiple sub-point cloud blocks into different regions, and perform feature extraction according to the curvatures of different point clouds in different regions to obtain lidar features.

[0145] In the embodiments of the present invention, the dynamic threshold segmentation technique (Dynamic Thresholding Segmentation) refers to an image processing method that automatically adjusts the threshold according to local image features or environmental changes. Its core is to dynamically calculate the threshold of each pixel or region instead of using a fixed global threshold, so as to achieve more accurate separation of the target and the background, and is applicable to scenarios with uneven illumination, noise interference or complex backgrounds.

[0146] In the embodiments of the present invention, point cloud registration refers to the process of aligning multiple point clouds collected from different perspectives or times to the same coordinate system by calculating a rigid body transformation matrix. Its core goal is to solve the geometric alignment problem of point cloud data in three-dimensional space and further meet the requirements of generating a complete scene representation or achieving high-precision positioning.

[0147] As an embodiment of the present invention, performing degradation detection according to the lidar features further includes:

[0148] Perform degradation detection according to the lidar features to obtain a degradation detection result;

[0149] Judge the environmental degradation state according to the degradation detection result, and select the initial pose of the degraded environment according to the environmental degradation state.

[0150] Further, judging the environmental degradation state according to the degradation detection result includes:

[0151] Extract the edge points along the lidar scan line in the current-frame lidar data and count the number of edge points;

[0152] Judge whether the current-frame environment is a degraded environment according to the number of edge points;

[0153] When the number of edge points is greater than or equal to the preset edge point number threshold, it is determined that the current frame environment is not a degraded environment;

[0154] When the number of edge points is less than the preset edge point number threshold, it is determined that the current frame environment is a degraded environment.

[0155] Further, an initial pose of the degraded environment is selected according to the environmental degradation state, including:

[0156] When the environmental degradation state is non-degraded, the current frame IMU factor is used for IMU initialization to obtain the IMU initial pose, and the first lidar factor is obtained by performing point cloud matching between the IMU initial pose and the historical map; the factor graph optimization algorithm is used to optimize the first lidar factor, the current frame IMU factor, and the GPS factor, and after the optimization is completed, a degraded environment map is obtained;

[0157] When the environmental degradation state is degraded, the current frame GPS factor is used for GPS initialization to obtain the GPS initial pose, and the second lidar factor is obtained by performing point cloud matching between the GPS initial pose and the historical map; the factor graph optimization algorithm is used to optimize the second lidar factor, the current frame IMU factor, and the GPS factor, and after the optimization is completed, a degraded environment map is obtained.

[0158] Further, point cloud registration is performed to obtain the lidar factor, including:

[0159] Calculate the curvature of all points along the lidar scan line according to the current frame lidar data;

[0160] Extract all feature points of the current frame lidar data according to the curvature, and transform all feature points to the global coordinate system and construct a local map;

[0161] Use the point cloud registration method to perform point cloud registration of the current frame lidar data to the local map to obtain the lidar factor.

[0162] In the embodiment of the present invention, the point cloud registration method refers to the technology of aligning different multiple point cloud data sets to a unified coordinate system, and usually adopts ICP (Iterative Closest Point) point cloud registration. ICP point cloud registration refers to a three-dimensional point cloud alignment algorithm based on iterative optimization. Its core goal is to solve the optimal rigid body transformation (rotation matrix and translation vector) by minimizing the spatial distance between two groups of point clouds, so that the point clouds collected from different perspectives or times can be accurately aligned in the unified coordinate system.

[0163] Further, transforming all feature points to the global coordinate system and constructing a local map includes:

[0164] Sort the feature points corresponding to the curvature from large to small, and extract the preset number of feature points counted from the front as edge points;

[0165] Extract the preset number of feature points counted from the back as plane points;

[0166] Construct a local map of edge points based on the edge points, construct a local map of plane points based on the plane points, and integrate the local map of edge points and the local map of plane points to obtain a local map.

[0167] In the embodiment of the present invention, the curvature c of all points along the LiDAR scan line is calculated according to the current frame of LiDAR data, and the following formula can be used:

[0168]

[0169] where S is the set of points of point i on the same LiDAR scan line, and |S| represents the number of points included in set S, is the coordinate of point i in the LiDAR coordinate system, is the coordinate norm of point i in the LiDAR coordinate system, that is, the Euclidean distance of point i from the LiDAR center, k represents the kth frame, and point j is the neighboring point of point i.

[0170] Exemplarily, to extract all feature points of the current frame of LiDAR data according to the curvature, the following steps can be adopted:

[0171] The point with the largest median value of the curvature c is represented as an edge point, denoted by The point with the smallest median value of the curvature c is represented as a plane point, denoted by In the current frame, all feature points are represented by F i where

[0172] In the embodiment of the present invention, to construct a local map of edge points based on the edge points, construct a local map of plane points based on the plane points, and integrate the local map of edge points and the local map of plane points to obtain a local map, it is represented by the following formula:

[0173]

[0174] where M i is the local map, is the local map of edge points, is the local map of plane points, and the symbol ′ is a coordinate transformation of the feature points.

[0175] The degraded environment map construction module 102 is used to optimize the LiDAR factor, the current frame IMU factor, and the current frame GPS factor by using the factor graph optimization algorithm to obtain a degraded environment map.

[0176] In the embodiments of the present invention, the factor graph optimization algorithm refers to a non-linear optimization method based on a graph model. By modeling multi-sensor data as factors and constructing a global optimization problem, the robot pose and environmental map are solved. In the scenario of multi-sensor fusion of LiDAR, IMU, and GPS, its core idea is to model the observation constraints of each sensor as factor nodes, and express the dependence relationship between variables through a graph structure to achieve high-precision state estimation.

[0177] Referring to Figure 7 As shown, it is a schematic structural diagram of an electronic device for implementing the method for constructing an environmental map of a degraded environment provided by an embodiment of the present invention.

[0178] The electronic device may include a processor 10, a memory 11, a communication bus 12, and a communication interface 13. It may also include a computer program stored in the memory 11 and executable on the processor 10, such as a program for the method for constructing an environmental map of a degraded environment.

[0179] Among them, in some embodiments, the processor 10 may be composed of integrated circuits. For example, it may be composed of a single packaged integrated circuit, or may be composed of multiple integrated circuits with the same or different functions, including a combination of one or more central processing units (CPUs), microprocessors, digital processing chips, graphics processors, and various control chips. The processor 10 is the control core (Control Unit) of the electronic device, connecting various components of the entire electronic device through various interfaces and lines. By running or executing programs or modules stored in the memory 11 (such as executing a program for the method for constructing an environmental map of a degraded environment, etc.), and calling data stored in the memory 11, it executes various functions of the electronic device and processes data.

[0180] The memory 11 at least includes one type of readable storage medium, and the readable storage medium includes flash memory, mobile hard disk, multimedia card, card-type memory (such as SD or DX memory, etc.), magnetic memory, magnetic disk, optical disc, etc. The memory 11 can be an internal storage unit of the electronic device in some embodiments, such as the mobile hard disk of the electronic device. The memory 11 can also be an external storage device of the electronic device in some other embodiments, such as a plug-in mobile hard disk, Smart Media Card (SMC), Secure Digital (SD) card, Flash Card, etc. equipped on the electronic device. Further, the memory 11 can also include both the internal storage unit and the external storage device of the electronic device. The memory 11 can be used not only to store the application software installed on the electronic device and various types of data, such as the code of a method program for constructing an environmental map of a degraded environment, etc., but also to temporarily store the data that has been output or will be output.

[0181] The communication bus 12 can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. This bus can be divided into an address bus, a data bus, a control bus, etc. The bus is configured to realize the connection and communication between the memory 11 and at least one processor 10, etc.

[0182] The communication interface 13 is used for the communication between the above-mentioned electronic device and other devices, including a network interface and a user interface. Optionally, the network interface can include a wired interface and / or a wireless interface (such as a WI-FI interface, a Bluetooth interface, etc.), and is generally used to establish a communication connection between this electronic device and other electronic devices. The user interface can be a display and an input unit (such as a keyboard). Optionally, the user interface can also be a standard wired interface and a wireless interface. Optionally, in some embodiments, the display can be an LED display, a liquid crystal display, a touch liquid crystal display, and an OLED (Organic Light-Emitting Diode) toucher, etc. Among them, the display can also be appropriately referred to as a display screen or a display unit, and is used to display the information processed in the electronic device and to display the visual user interface.

[0183] Figure 7 Only the electronic device with components is shown, and those skilled in the art can understand that Figure 7The structure shown does not constitute a limitation on the electronic device, which may include fewer or more components than shown, or combine certain components, or have a different component arrangement.

[0184] For example, although not shown, the electronic device may further include a power source (such as a battery) for powering each component. Preferably, the power source can be logically connected to the at least one processor 10 through a power management device, so as to implement functions such as charging management, discharging management, and power consumption management through the power management device. The power source may also include any components such as one or more DC or AC power sources, a recharge device, a power failure detection circuit, a power converter or inverter, and a power status indicator. The electronic device may also include various sensors, a Bluetooth module, a Wi-Fi module, etc., which will not be elaborated here.

[0185] It should be understood that the embodiments are for illustrative purposes only and are not limited by this structure in the scope of the patent application.

[0186] The method program for constructing an environmental map of a degradation environment stored in the memory 11 of the electronic device is a combination of multiple instructions. When running in the processor 10, it can implement:

[0187] Obtain the current frame IMU data, and perform IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor;

[0188] Obtain the current frame GPS data, and calculate the current frame GPS factor by using a preset filtering algorithm in combination with the current frame IMU data;

[0189] Obtain the current frame lidar data, and perform feature extraction on the current frame lidar data to obtain lidar features;

[0190] After performing degradation detection according to the lidar features, perform point cloud registration to obtain the lidar factor;

[0191] Use the factor graph optimization algorithm to optimize the lidar factor, the current frame IMU factor, and the current frame GPS factor to obtain a degradation environment map.

[0192] Specifically, the specific implementation method of the above instructions by the processor 10 can refer to the description of the relevant steps in the corresponding embodiments of the attached drawings, which will not be elaborated here.

[0193] Furthermore, if the modules / units integrated in the electronic device 1 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. The computer-readable storage medium can be volatile or non-volatile. For example, the computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM, Read-Only Memory).

[0194] The present invention also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor of an electronic device, it can implement:

[0195] Obtain the current frame IMU data, and perform IMU integration calculation on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor;

[0196] Obtain the current frame GPS data, and calculate the current frame GPS factor by using a preset filtering algorithm in combination with the current frame IMU data;

[0197] Obtain the current frame lidar data, and perform feature extraction on the current frame lidar data to obtain lidar features;

[0198] Perform degradation detection according to the lidar features and then perform point cloud registration to obtain the lidar factor;

[0199] Use the factor graph optimization algorithm to optimize the lidar factor, the current frame IMU factor, and the current frame GPS factor to obtain a degraded environment map.

[0200] In several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses, and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the modules is only a logical function division, and there can be other division methods in actual implementation.

[0201] The modules described as separate components may or may not be physically separated. The components shown as modules may or may not be physical units, that is, they can be located in one place, or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0202] In addition, in each embodiment of the present invention, each functional module can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware, or in the form of a combination of hardware and software functional modules.

[0203] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above-mentioned exemplary embodiments, and without departing from the spirit or basic characteristics of the present invention, the present invention can be implemented in other specific forms.

[0204] Therefore, from any point of view, the embodiments should be regarded as exemplary and non-restrictive. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, all changes falling within the meaning and scope of the equivalent elements of the claims are intended to be included in the present invention. Any reference signs in the claims should not be regarded as limiting the claimed invention.

[0205] The blockchain referred to in the present invention is a new application mode of computer technologies such as distributed data storage, peer-to-peer transmission, consensus mechanism, and encryption algorithms. Blockchain, in essence, is a decentralized database, a series of data blocks generated by using cryptographic methods. Each data block contains information about a batch of network transactions, which is used to verify the validity of the information (anti-counterfeiting) and generate the next block. The blockchain can include a blockchain underlying platform, a platform product service layer, an application service layer, etc.

[0206] The embodiments of the present application can acquire and process relevant data based on artificial intelligence technology. Among them, artificial intelligence (AI) is to use a digital computer or a machine controlled by a digital computer to simulate, extend, and expand human intelligence, perceive the environment, acquire knowledge, and use knowledge to obtain the best results of theory, method, technology, and application system.

[0207] In addition, obviously, the term "including" does not exclude other units or steps, and the singular does not exclude the plural. The multiple units or devices stated in the system claims can also be implemented by one unit or device through software or hardware. The terms such as "first" and "second" are used to represent names and do not represent any specific order.

[0208] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A method for constructing an environmental map of a degraded environment, characterized in that, The method includes: Obtaining the current frame IMU data, and performing IMU integration calculation on the current frame IMU data and historical frame IMU data to obtain the current frame IMU factor; Obtaining the current frame GPS data, and calculating the current frame GPS factor by using a preset filtering algorithm in combination with the current frame IMU data; Obtaining the current frame lidar data, and performing feature extraction on the current frame lidar data to obtain lidar features; Performing degradation detection based on the lidar features and then performing point cloud registration to obtain the lidar factor; Using a factor graph optimization algorithm to optimize the lidar factor, the current frame IMU factor, and the current frame GPS factor to obtain a degraded environment map.

2. The method for constructing an environmental map of a degraded environment according to claim 1, characterized in that, The performing degradation detection based on the lidar features further includes: Performing degradation detection based on the lidar features to obtain a degradation detection result; Judging the environmental degradation state according to the degradation detection result, and selecting an initial pose of the degraded environment according to the environmental degradation state.

3. The method for constructing an environmental map of a degraded environment according to claim 2, wherein, The judging the environmental degradation state according to the degradation detection result includes: Extracting edge points along the lidar scan line in the current frame lidar data, and counting the number of edge points; Judging whether the current frame environment is a degraded environment according to the number of edge points; When the number of edge points is greater than or equal to a preset edge point number threshold, it is judged that the current frame environment is not a degraded environment; When the number of edge points is less than the preset edge point number threshold, it is judged that the current frame environment is a degraded environment.

4. The method for constructing an environmental map of a degraded environment according to claim 2, characterized in that, The selecting an initial pose of the degraded environment according to the environmental degradation state includes: When the environmental degradation state is non-degraded, performing IMU initialization by using the current frame IMU factor to obtain an IMU initial pose, performing point cloud matching between the IMU initial pose and the historical map to obtain a first lidar factor; using a factor graph optimization algorithm to optimize the first lidar factor, the current frame IMU factor, and the GPS factor, and obtaining a degraded environment map after the optimization is completed; When the environmental degradation state is degraded, performing GPS initialization by using the current frame GPS factor to obtain a GPS initial pose, performing point cloud matching between the GPS initial pose and the historical map to obtain a second lidar factor; using a factor graph optimization algorithm to optimize the second lidar factor, the current frame IMU factor, and the GPS factor, and obtaining a degraded environment map after the optimization is completed.

5. The method for constructing an environmental map of a degraded environment according to any one of claims 1-4, characterized in that, The performing point cloud registration to obtain the lidar factor includes: Calculating the curvature of all points along the lidar scan line according to the current frame lidar data; Extracting all feature points of the current frame lidar data according to the curvature, and converting all feature points to the global coordinate system and constructing a local map; Performing point cloud registration of the current frame lidar data to the local map by using a point cloud registration method to obtain the lidar factor.

6. The method for constructing an environmental map of a degraded environment according to claim 5, wherein The converting all feature points to the global coordinate system and constructing a local map includes: Sorting the feature points corresponding to the curvature from large to small, and extracting a preset number of feature points counted from the front as edge points; Extracting a preset number of feature points counted from the back as plane points. Construct a local map of edge points based on edge points, construct a local map of planar points based on planar points, and integrate the local map of edge points and the local map of planar points to obtain a local map.

7. The method for constructing an environmental map of a degraded environment according to claim 1, characterized in that, The calculating the current frame IMU factor by performing IMU integration on the current frame IMU data and the historical frame IMU data includes: Obtain the acceleration and angular velocity in the current frame IMU data; Generate the current frame IMU factor by using the IMU pre-integration technology according to the acceleration and angular velocity.

8. An environmental map construction device for a degraded environment, characterized in that, The device implements the method for constructing an environmental map of a degraded environment as described in any one of claims 1-7, and the device includes: A factor calculation module, configured to obtain the current frame IMU data, and perform IMU integration on the current frame IMU data and the historical frame IMU data to obtain the current frame IMU factor; obtain the current frame GPS data, and calculate the current frame GPS factor by using a preset filtering algorithm in combination with the current frame IMU data; obtain the current frame lidar data, and perform feature extraction on the current frame lidar data to obtain lidar features; perform degradation detection according to the lidar features and then perform point cloud registration to obtain lidar factors; A degraded environment map construction module, configured to perform factor graph optimization on the lidar factor, the current frame IMU factor, and the current frame GPS factor by using a factor graph optimization algorithm to obtain a degraded environment map.

9. An electronic device, characterized in that, The electronic device includes: At least one processor; And, a memory communicatively connected to the at least one processor; wherein, The memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute the method for constructing an environmental map of a degraded environment as described in any one of claims 1-7.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the method for constructing an environmental map of a degraded environment as described in any one of claims 1-7.

Images