Dynamic environment positioning method based on lidar and imu

Abstract

The present invention provides a dynamic environment positioning method based on lidar and an IMU, comprising step 1: obtaining a current environment point cloud by means of a front-end odometer of a robot; step 2: matching the current environment point cloud with an offline map, and determining the degree of difference therebetween; step 3: determining whether a difference value exceeds a preset threshold; if the degree of difference does not exceed the preset threshold, considering a current scene to be basically consistent with the environment and a great change not to have occurred, and transferring the environment point cloud to a robot coordinate system and storing same in a real-time incremental voxelization-based local map; and if the degree of difference exceeds the preset threshold, considering the current scene to have changed greatly from the environment, matching the environment point cloud with the real-time local map, and obtaining positioning information at this time on the basis of an initial pose obtained by IMU pre-integration. The method of the present invention can achieve accurate positioning in a dynamic environment, improve the positioning stability of an autonomous platform, and prevent the occurrence of positioning jumps.

Description

A dynamic environment positioning method based on lidar and IMU TECHNICAL FIELD

[0001] The present application relates to the technical field of dynamic positioning method, in particular to a dynamic environment positioning method based on lidar and IMU. BACKGROUND

[0002] The positioning method based on lidar and IMU plays an important role in many fields, especially in applications such as autonomous navigation, robotics, autonomous driving and geographic information system (GIS); lidar and IMU have different advantages and disadvantages. Lidar provides high-precision environmental geometric information, but is easily affected by environmental changes and occlusions; IMU provides continuous acceleration and angular velocity measurements, but is prone to error accumulation. Combining the data of the two sensors can complement each other's shortcomings and improve the overall positioning accuracy.

[0003] The existing positioning method based on lidar and IMU mainly relies on the prior establishment of an offline map. If the current environment and the offline map have large changes in some areas, the existing positioning method will likely cause the positioning to jump. In order to effectively position the autonomous platform based on lidar and IMU, the present application proposes a hybrid positioning method based on the fusion of the current environment and the prior map, which can effectively reduce the probability of positioning jump.

[0004] SUMMARY

[0005] In view of the deficiencies of the prior art, the present application aims to provide a dynamic environment positioning method based on lidar and IMU, which can solve the existing problems.

[0006] In order to achieve the above-mentioned purpose, the technical scheme of the present application is as follows:

[0007] The present application is realized by the following technical scheme: a dynamic environment positioning method based on lidar and IMU, comprising the following steps:

[0008] Step 1: obtaining the current environment point cloud through the robot front-end odometer;

[0009] Step 2: matching the current environment point cloud with the offline map to determine the difference degree;

[0010] Step 3: determining whether the difference value exceeds the preset threshold;

[0011] If the difference degree does not exceed the preset threshold, it is considered that the current scene and the environment are basically consistent and no large changes have occurred, then the environment point cloud is transferred to the robot coordinate system and saved to the real-time local map based on incremental voxel;

[0012] If the difference exceeds the preset threshold, it is considered that the current scene and the environment have changed greatly; the environment point cloud is matched with the real-time local map, and on the basis of the initial pose obtained by IMU pre-integration, the positioning information at this time is obtained.

[0013] Further, the matching of the current environment point cloud with the offline map comprises:

[0014] The obtained new frame of laser environment point cloud is subjected to a distortion removal operation, and the environment point cloud is divided into corner points and plane points for feature extraction, and then the processed new frame of laser environment point cloud is subjected to point cloud scanning matching with the offline map.

[0015] Further, the division of the environment point cloud into corner points and plane points is based on the curvature of the points, and the points with greater curvature are considered as corner points, and the points with smaller curvature are considered as plane points; the calculation formula of the curvature is as follows:

[0016] In formula (1), Cij is the curvature of the jth line and the ith point, is the corresponding detection distance, and n is the search range, is the detection distance of the adjacent point. is the detection distance of the adjacent point. is the detection distance of the adjacent point.

[0017] Further, the calculation formula of the difference is as follows:

[0018] In formula (2), m s and n s are the number of matching points that do not meet the point-to-line and point-to-plane distance constraints; m is the total number of points that meet the point-to-line matching in the current frame, and n is the total number of points that meet the point-to-plane matching in the current frame.

[0019] Further, the process of IMU pre-integration and bias optimization comprises:

[0020] First, the world coordinate system and the robot body coordinate system are set as W and B respectively, wherein the IMU coordinate system coincides with the robot body coordinate system; the state of the robot in the i-th frame of laser radar in the W system can be described as: X i ={R i , p i , v i , b i}, wherein R i ∈S03 is a rotation matrix, p i is a translation vector, v i is a velocity, and b i is the bias of the IMU.

[0021] The initial position of scan matching is pre-integrated by IMU, which will also be used to optimize the bias of IMU after getting the accurate pose matching result; IMU measures the angular velocity and acceleration in B frame through the present speedometer and gyroscope, and the measurement formula is:

[0022] In formula (3), and are the original angular velocity and acceleration measured by IMU in B frame at time t, which are affected by the slowly changing bias and the white noise with mean value 0 , g is the stable gravity vector in W frame, is the rotation matrix from W frame to B frame, ω t is the real angular velocity, a t is the real acceleration;

[0023] The method of median integration is used to calculate the angular velocity and acceleration between two consecutive IMU measurements, and the calculation formula is:

[0024] In formula (4) is the original angular velocity at time i, is the original acceleration at time i, is the bias at time i, is the original angular velocity at time i+1, is the bias at time i+1, is the rotation matrix from W frame to B frame at time i, is the rotation matrix from W frame to B frame at time i+1.

[0025] From the median integration, if the time stamps of the adjacent two frames of laser are l t and l t+1 , the corresponding translation integral velocity integral and rotation integral can be obtained by pre-integrating IMU, and the calculation formula is:

[0026] In formula (5) is the pre-integrated translation from the IMU time of the two frames of laser radar to the initial laser radar time, is the pre-integrated velocity from the IMU time of the two frames of laser radar to the initial laser radar time, is the pre-integrated rotation quantity from the IMU time of the frame of the laser radar to the initial laser radar time;

[0027] Finally, the IMU pre-integrated state can be expressed as:

[0028] In formula (6), is the pre-integrated translation quantity of the two frames of the laser radar, is the pre-integrated velocity quantity of the two frames of the laser radar, is the pre-integrated rotation quantity of the two frames of the laser radar, are the angular velocity bias and the acceleration bias at the time of the second frame of the laser radar, respectively;

[0029] Let q be the quaternion corresponding to R, [.] xyz is the imaginary part corresponding to the quaternion, and the corresponding residual can be expressed as:

[0030] Further, the positioning information calculation process at this time is:

[0031] First, the laser environment point cloud is converted to the world coordinate system according to the pose obtained by the IMU pre-integration, and the nearest five points in the offline map are found for each point. The corner point performs point-to-line matching, and the straight line is fitted by the five adjacent corner points, with a requirement that the point-to-line distance is within 1 m. The plane point performs point-to-plane matching, and the plane is fitted by the five adjacent plane points, with a requirement that the point-to-plane distance is within 1 m. Let the corner point be The corresponding fitting straight line has two points And the plane point The corresponding fitting plane has three points Their residuals can be calculated, respectively:

[0032] Finally, the Gauss-Newton method is used to complete the optimization of the pose, i.e., the positioning information, which is as follows:

[0033] A kind of vehicle networking millimeter wave wireless communication beam alignment device based on vision auxiliary, including processor and memory;The memory is used to store program;The processor executes the program and realizes the method described in any one of the above.

[0034] A computer readable storage medium, the storage medium stores a program, the program is executed by a processor to realize the method described in any one of the above.

[0035] Compared with the prior art, the beneficial effects of the present application include:

[0036] The application matches the current environment point cloud with the offline map using the front-end odometer to obtain the current positioning result and the matching degree of the point cloud, simultaneously uses the incremental voxel structure to update the real-time local map based on the current pose; determines the map change according to the point cloud matching degree, if the change degree is large, does not trust the odometer result this time, and takes the matching result of the current environment point cloud and the real-time local map as the positioning information source; through the method of the application, accurate positioning in a dynamic environment can be realized, and the positioning stability of the autonomous platform is improved; the environment change degree is determined according to the point cloud matching result of the front-end odometer, and the occurrence of the positioning jump is prevented; the local map based on the current pose is maintained in real time, and the efficient and accurate positioning is ensured. BRIEF DESCRIPTION OF DRAWINGS

[0037] The disclosure of the application will be described with reference to the accompanying drawings. It should be understood that the drawings are only for illustrative purposes, and are not intended to limit the scope of protection of the application. In the drawings, the same reference signs are used to refer to the same parts. Among them:

[0038] Fig. 1 is a flowchart of a dynamic environment positioning method based on a laser radar and an IMU according to the application;

[0039] Fig. 2 is a detailed flowchart of the dynamic environment positioning method according to the application. DETAILED DESCRIPTION

[0040] It is easy to understand that, according to the technical scheme of the application, those skilled in the art can propose a plurality of structure modes and implementation modes which can be replaced with each other without changing the essential spirit of the application. Therefore, the following specific embodiments and drawings are only exemplary descriptions of the technical scheme of the application, and should not be regarded as the whole or as a limitation or restriction on the technical scheme of the application.

[0041] The application is described by taking the application of a laser radar and an IMU to robot positioning as an example, but is not limited thereto. The application provides a dynamic environment positioning method based on a laser radar and an IMU, and the flowchart is shown in Fig. 1, which includes the following steps:

[0042] Step one: obtaining the current environment point cloud through the front-end odometer of the robot;

[0043] Step two: matching the current environment point cloud with the offline map to determine the difference degree;

[0044] The laser radar odometer is described, wherein when the current environment point cloud is matched with the offline map, the obtained new frame of laser environment point cloud is first subjected to a distortion removal operation, and the environment point cloud is divided into corner points and plane points for feature extraction, and then the processed new frame of laser environment point cloud is matched with the offline local map in the offline map through point cloud scanning matching; as shown in Fig. 2;

[0045] For example, when a new frame of laser environment point cloud comes, after the distortion removal operation is completed, the point cloud needs to be divided into corner points and plane points. The basis is the curvature of the points. Points with larger curvature are considered as corner points, and points with smaller curvature are considered as plane points. The formula for calculating the curvature is as follows:

[0046] In formula (1) is the curvature of the jth line and the ith point, is the corresponding detection distance, and n is the search range, is the detection distance of its adjacent points.

[0047] The formula for calculating the difference degree is:

[0048] In formula (2) s and n s are the number of matching points that do not meet the point-to-line and point-to-plane distance constraints; m is the total number of points in the current frame that meet the point-to-line matching, and n is the total number of points in the current frame that meet the point-to-plane matching.

[0049] The difference degree of matching the current frame of environment point cloud with the offline map is calculated, which is used as the difference of the degree of map updating, and it is selected whether to use the real-time local map for supplementary matching.

[0050] Step three: judge whether the difference value exceeds the preset threshold;

[0051] 3.1, if the difference degree does not exceed the preset threshold, it is considered that the current scene is basically consistent with the environment, and no large change has occurred, then the environment point cloud is transferred to the robot coordinate system and saved to the real-time local map based on incremental voxel; and only a certain number of environment point clouds are saved in the map, and the exceeding part will be cleared according to the principle of queue first in first out.

[0052] The incremental voxel (Incremental Voxelization, abbreviated as iVox) is a commonly used technology in real-time point cloud processing and three-dimensional reconstruction. It efficiently processes and stores point cloud data by dynamically updating voxel grids, which is used in application scenarios that require real-time updating and maintenance of environment models; mainly including voxelization and incremental updating, where voxelization is the process of dividing three-dimensional space into regular cubic units (voxels). Each voxel can contain one or more point cloud data points. The main purpose of voxelization is to discretize continuous three-dimensional space to facilitate efficient storage and processing of point cloud data. Incremental updating is the core idea of incremental voxelization, which only updates the changed part when receiving new point cloud data, instead of rebuilding the entire voxel grid. This greatly improves the processing efficiency, especially in real-time applications.

[0053] The workflow of incrementally updating the voxel grid includes:

[0054] Initialize the voxel grid: Define the size (resolution) of the voxels and initialize an empty voxel grid. Choose an appropriate voxel size to balance storage efficiency and detail preservation.

[0055] Receive new point cloud data: Obtain new point cloud data from sensors such as lidar.

[0056] Map point cloud to voxel: Map the new point cloud data to the voxel grid. Each point is assigned to the corresponding voxel according to its coordinates.

[0057] Incremental update: For each new point, check if the voxel it belongs to already exists. If the voxel exists, update the information of the voxel (such as average color, normal direction, etc.). If the voxel does not exist, create a new voxel and add it to the grid.

[0058] Remove redundant points: In some cases, there may be multiple points in the same voxel. These points can be merged by some strategy (such as nearest neighbor, average value, etc.) to reduce redundancy.

[0059] 3.2, if the difference exceeds the preset threshold, it is considered that the current scene and the environment have changed greatly; match the environment point cloud with the real-time local map, and obtain the positioning information at this time based on the initial pose obtained by IMU pre-integration.

[0060] The process of IMU pre-integration and its bias optimization includes:

[0061] First, set the world coordinate system and the robot body coordinate system as W and B, respectively, where the IMU coordinate system coincides with the robot body coordinate system. The state of the robot in the W system at the i-th frame of lidar can be described as: X i = {R i , p i , v i , b i}, where R i ∈S03 is a rotation matrix, p i is a translation vector, v i is a velocity, and b i is the bias of the IMU.

[0062] The initial position guess of scan matching is obtained by IMU pre-integration, and after obtaining the accurate pose matching result, it will also be used to optimize the bias of the IMU; the IMU measures the angular velocity and acceleration in the B system through the present speedometer and gyroscope, and its measurement method can be described as:

[0063] In formula (3), and are the raw angular velocity and acceleration measured by IMU in B frame at time t, which are affected by the slowly changing bias and the white noise with zero mean , g is the stable gravity vector in W frame, is the rotation matrix from W frame to B frame, ω t is the true angular velocity, a t is the true acceleration.

[0064] The method of median integration is used to calculate the angular velocity and acceleration between two consecutive IMU measurements, which can be described as:

[0065] In equation (4), ω is the raw angular velocity at time i, is the raw acceleration at time i, is the bias at time i, is the raw angular velocity at time i+1, is the bias at time i+1, is the rotation matrix from W frame to B frame at time i, is the rotation matrix from W frame to B frame at time i+1.

[0066] From median integration, if the timestamps of two adjacent laser frames are l t and l t+1 , the corresponding translational integral velocity integral and rotational integral can be obtained by IMU pre-integration. Assuming the time interval of two adjacent IMU frames is Δt:

[0067] In equation (5), ω is the pre-integrated translation from the time of a certain frame of IMU in two laser frames to the time of the initial laser frame, is the pre-integrated velocity from the time of a certain frame of IMU in two laser frames to the time of the initial laser frame, is the pre-integrated rotation from the time of a certain frame of IMU in two laser frames to the time of the initial laser frame.

[0068] Finally, the IMU pre-integrated state can be represented as:

[0069] In equation (6), is the pre-integrated translation of two frames of lidar, is the pre-integrated velocity of two frames of lidar, is the pre-integrated rotation of two frames of lidar, are the angular velocity bias and acceleration bias of the second frame of lidar respectively.

[0070] Let q be the quaternion corresponding to R, xyz is the imaginary part of the quaternion, the corresponding residual can be expressed as:

[0071] In formula (7), is, P t+1 is, p t is, v t is, Δt is, g is, is, v t+1 is, is, is, is, is, is, is, is.

[0072] The above is the process of IMU pre-integration and bias optimization, which is in the factor graph optimization part of the overall flow.

[0073] The positioning information calculation process at this time is:

[0074] First, the laser point cloud is converted to the W system according to the pose obtained by the IMU pre-integration, and the nearest five points in the offline local map are found for each point. The corner point performs point-to-line matching, and the straight line is fitted by the five adjacent corner points, and the point-to-line distance is required to be within 1m; and the plane point performs point-to-plane matching, and the plane is fitted by the five adjacent plane points, and the point-to-plane distance is required to be within 1m. Let the corner point The corresponding fitting straight line has two points And the plane point The corresponding fitting plane has three points Their residuals can be calculated respectively:

[0075] Finally, the Gauss-Newton method is used to complete the optimization of the pose, that is, the positioning information, which is as follows:

[0076] The application uses a front-end odometer to match the current environment point cloud with an offline map, obtains the matching degree of the current positioning result and the point cloud, and simultaneously uses an incremental voxel structure to update a real-time local map based on the current pose; the map change condition is judged according to the point cloud matching degree, if the change degree is large, the odometer result is not trusted, and the matching result of the current environment point cloud and the real-time local map is used as the positioning information source; through the method of the application, accurate positioning in a dynamic environment can be realized, and the positioning stability of the autonomous platform is improved; the environment change degree is judged according to the point cloud matching result of the front-end odometer, and the occurrence of the positioning jump condition is prevented; the local map based on the current pose is maintained in real time, and the efficient and accurate positioning is ensured.

[0077] Those skilled in the art can realize that the units and algorithm steps of the examples described in combination with the embodiments disclosed herein can be realized in electronic hardware, computer software or a combination of both. In order to clearly illustrate the interchangeability of hardware and software, the components and steps of the examples have been described in the above description in general terms. Whether the functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. A person skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present disclosure.

[0078] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described system, device and unit can refer to the corresponding processes in the foregoing method embodiments, which will not be described here.

[0079] In several embodiments provided herein, it should be understood that the disclosed system, device and method can be implemented in other ways. For example, the above-described device embodiments are only schematic, for example, the division of the units is only a logical function division, and actual implementation can have another division manner, for example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the units shown or discussed can be indirect coupling or communication connection through some interfaces, devices or units, and can also be electrical, mechanical or other forms of connection.

[0080] The units described as separate components can or can not be physically separated, and the components shown as units can or can not be physical units, i.e. can be located in one place or distributed on a plurality of network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the embodiments of the present disclosure.

[0081] In addition, each functional unit in each embodiment herein can be integrated in one processing unit, or each unit can be physically present separately, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0082] When the integrated unit is realized in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer readable storage medium. Based on such understanding, the technical solutions herein, essentially or in the form of a contribution to the prior art, or all or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium, and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in each embodiment herein. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disk, and various other media that can store program codes.

[0083] The principles and implementation manners of the present application are described herein by applying specific embodiments. The above description of the embodiments is only used to help understand the methods and core ideas thereof; meanwhile, for those skilled in the art, according to the ideas herein, the specific implementation manners and application ranges can be changed; in conclusion, the content of the specification should not be understood as limiting the present application.

Claims

1. A method of dynamic environment localization based on lidar and IMU, characterized by: The method comprises the following steps: Step 1: obtaining a current environment point cloud through a robot front end odometry; Step 2: matching the current environment point cloud with an offline map to determine a difference degree; Step 3: judging whether the difference value exceeds a preset threshold; If the difference degree does not exceed the preset threshold, it is considered that the current scene is basically consistent with the environment and no large change has occurred, and the environment point cloud is transferred to a robot coordinate system and saved to a real-time local map based on an incremental voxel; If the difference degree exceeds the preset threshold, it is considered that the current scene and the environment have undergone a large change; the environment point cloud is matched with the real-time local map, and on the basis of an initial pose obtained through IMU pre-integration, positioning information at this time is obtained.

2. The method of claim 1, wherein: The matching of the current environment point cloud with the offline map comprises: The obtained new frame of laser environment point cloud is subjected to a de-warping operation, and the environment point cloud is divided into corner points and plane points for feature extraction, and then the processed new frame of laser environment point cloud is subjected to point cloud scanning matching with the offline map.

3. The method of claim 2, wherein: The division of the environment point cloud into corner points and plane points is based on the curvature of the points, and points with larger curvature are considered as corner points and points with smaller curvature are considered as plane points. The formula for calculating the curvature is as follows: in formula (1) is the curvature of the jth line at the ith point, is its corresponding detection distance, n is the search range, The detection distance of the point is the distance between the point and its adjacent point.

4. The method of claim 1, wherein: The difference degree is calculated by the following formula: m s and n s are the number of matches that do not satisfy the point-to-line and point-to-plane distance constraints; m is the total number of points in the current frame that match to lines, and n is the total number of points in the current frame that match to planes.

5. The method of claim 1, wherein: The process of the IMU pre-integration and the bias optimization thereof comprises: First, a world coordinate system and a robot body coordinate system are set as W and B respectively, wherein an IMU coordinate system is consistent with the robot body coordinate system; The body coordinate system coincides; the state of the robot in the W system at the i-th frame of laser radar can be described as: X i = {R i , p i , v i , b i}, wherein R i ∈SO3 is a rotation matrix, p i is a translation vector, v i is a velocity, and b i is the bias of the IMU; The initial position of scan matching is pre-integrated by the IMU, and is used to optimize the bias of the IMU after obtaining the accurate pose matching result; the IMU measures the angular velocity and acceleration in the B system through the present speedometer and gyroscope, and the measurement formula is as follows: in formula (3), and are the raw angular velocity and acceleration measured by the IMU in body frame B at time t, subject to slowly varying biases and white noise with mean 0 the influence of gravity, g is the gravity vector, is the rotation matrix from W-frame to B-frame, ω t is the real angular velocity, a t is the real acceleration; The angular velocity and acceleration between two consecutive IMU measurements are calculated using the method of median integration, with the formula: in formula (4) is the original angular velocity at time i, is the original acceleration at time i, is the bias at time i, is the original angular velocity at time i+1, is the bias at time i+1, is a rotation matrix from the W-frame to the B-frame at time i, is a rotation matrix from the W system to the B system at i+1 time; From the median integration, if the timestamps of two adjacent frames of laser are l t and l t+1 , the corresponding translation integral quantity velocity integral and a rotating integral The calculation can be obtained by frame-by-frame iteration. Assuming that the time interval of two adjacent frames of IMU is Δt, the calculation formula is: in formula (5) is the pre-integrated translation from the IMU time of the frame to the initial lidar time, is a pre-integrated velocity quantity from the IMU time of a frame of the two frames of lidar to the initial lidar time, is a pre-integration rotation amount from an IMU time of a certain frame of laser radar to an initial laser radar time; Ultimately, the IMU pre-integrated state can be expressed as: in formula (6), is a pre-integrated translation of two frames of lidar, is a pre-integrated velocity quantity of two frames of lidar, is a pre-integrated rotation quantity of two frames of lidar, are an angular velocity bias and an acceleration bias at the second frame of laser radar time respectively; Let q be the quaternion corresponding to R, [·] xyz is the imaginary part of the quaternion, the corresponding residual can be expressed as:

6. The method of claim 1, wherein: The process of calculating the positioning information at this time comprises: Firstly, the laser environment point cloud is converted to the world coordinate system according to the pose obtained by IMU pre-integration, the nearest five points in the offline map are found for each point, the point-to-line matching is performed for the corner points, and the straight line is fitted by the five adjacent corner points with the requirement that the point-to-line distance is within 1 m; the point-to-plane matching is performed for the plane points, and the plane is fitted by the five adjacent plane points with the requirement that the point-to-plane distance is within 1 m; let the corner point The corresponding fitted straight line has two points And the plane point The corresponding fitting plane has three points Their residuals can be computed respectively as: Finally, the Gauss-Newton method is used to complete the optimization of the pose, i.e. the positioning information, as follows: 7.A visual aid based V2X millimeter wave wireless communication beam alignment device, characterized in that: The device comprises a processor and a memory; the memory is used for storing a program; and the processor executes the program to realize the method according to any one of claims 1-6.

8. A computer-readable storage medium, characterized in that: The storage medium stores a program, and the program is executed by the processor to realize the method according to any one of claims 1-6.

Images