Mapping method and device based on laser inertial odometer
By converting coordinates, fitting planes, filtering point sets and obtaining dynamic noise in laser inertia odometers, the problem of fixed noise reducing distance measurement accuracy is solved, and higher graph construction accuracy is achieved.
Patent Information
- Application Number
- CN202411031176.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-30
- Publication Date
- 2024-11-08
AI Technical Summary
When building a three-dimensional point cloud map, the distance measurement accuracy is reduced due to the fixed noise variance setting, which affects the accuracy of the map construction.
By converting the laser point coordinates in the laser point cloud to the global coordinate system, performing plane fitting, filtering out laser points that meet the point-to-plane distance constraints, obtaining dynamic noise, and denoising the point cloud using Kalman filtering method. Replace fixed noise to improve graph building accuracy.
It effectively improves the accuracy of the laser inertia odometer in the construction of the diagram, reduces the impact of noise, and improves the distance measurement accuracy.
Smart Images

Figure CN118913250A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of navigation, positioning and surveying and mapping, and specifically to a mapping method and device based on a laser inertial odometer. Background Art
[0002] Existing LiDAR mapping methods are mainly based on the high precision and high resolution characteristics of LiDAR sensors. The distance between the object and the radar is calculated by measuring the time difference of the returned laser pulses, thereby constructing a three-dimensional point cloud map of the object. Laser inertial odometry is an odometer technology that combines LiDAR and an inertial measurement unit. It can estimate the position, attitude and speed of the carrier through continuous measurement and calculation without external positioning information.
[0003] Existing laser inertial odometry uses point cloud registration to estimate the state of the odometry. The mainstream registration method is to calculate the distance from a point to a distribution, such as the distance from a point to a plane. When constructing a point-to-plane constraint, the observation noise variance of the constraint is set to a constant.
[0004] However, considering that the specific point cloud distribution is not strictly planar, and the influence of the angle, distance and product model during observation on the ranging accuracy of the laser inertial odometry, setting the observation noise variance to a constant value will reduce the accuracy of mapping using the laser inertial odometry. Summary of the invention
[0005] The present application provides a mapping method and device based on a laser inertial odometer, which can improve the accuracy of mapping using a laser inertial odometer.
[0006] In a first aspect, an embodiment of the present application provides a mapping method based on a laser inertial odometer, the method comprising:
[0007] The coordinates of each laser point in the laser point cloud are converted from the laser radar coordinate system to the global coordinate system to obtain the nearest point set of each laser point;
[0008] Perform plane fitting on each of the nearest point sets to obtain the first plane that can be fitted;
[0009] Selecting laser points that satisfy the point-to-surface distance constraint condition from the laser point cloud to generate a first laser point set;
[0010] Acquire the observation noise corresponding to each laser point in the first laser point set based on preset parameters of the laser radar;
[0011] Based on the observation noise, and the normal vector and the flatness of the first plane, dynamic noise is obtained;
[0012] The laser point cloud is denoised based on the dynamic noise, and a map is constructed according to the denoised laser point cloud.
[0013] In combination with the first aspect, in one implementation, before the coordinate system conversion, the method further includes:
[0014] Calculating intermediate quantities through the measurement values of the inertial measurement unit between the previous moment and the current moment, wherein the intermediate quantities include the position and velocity of the measured object between the previous moment and the current moment;
[0015] Based on the intermediate amount, the motion distortion of each laser point in the laser point cloud is removed, and the laser point cloud is aligned to the laser radar coordinate system at the current moment.
[0016] In combination with the first aspect, in one implementation, obtaining the closest point set of each laser point includes:
[0017] The nearest point set of each laser point is obtained by K nearest neighbor algorithm.
[0018] In combination with the first aspect, in one implementation, the point-to-plane distance constraint condition includes:
[0019] The distance between the laser point and the first plane is smaller than a preset distance.
[0020] In combination with the first aspect, in one implementation, the method for obtaining the normal vector and flatness of the first plane includes:
[0021] Obtaining a covariance matrix of the first laser point set;
[0022] Performing eigenvalue decomposition or singular value decomposition on the covariance matrix to obtain a corresponding principal component vector set;
[0023] The normal vector and the flatness of the first plane are obtained according to the principal component vector set.
[0024] In combination with the first aspect, in one implementation, acquiring the normal vector and the flatness of the first plane according to the principal component vector set includes:
[0025] The principal component vectors include first, second, and third principal component vectors, which correspond to the maximum, second largest, and minimum variances of the first laser point set respectively;
[0026] If the eigenvalues corresponding to the first principal component vector and the second principal component vector are much larger than the eigenvalue corresponding to the third principal component vector, the third principal component vector is used as the normal vector of the first plane;
[0027] The flatness of the first plane is calculated according to the eigenvalues corresponding to the first, second, and third principal component vectors.
[0028] In combination with the first aspect, in one implementation, obtaining the observation noise corresponding to each laser point in the first laser point set based on preset parameters of the laser radar includes:
[0029] Based on the preset parameters of the laser radar, a random model of the original observation value and a laser point measurement model are obtained;
[0030] Based on the random model of the original observation value and the laser point measurement model, a laser point random model is established;
[0031] Converting the coordinates of the laser points in the laser point random model from the laser radar coordinate system to the global coordinate system to obtain a first measurement model;
[0032] Based on the first measurement model, an observation noise corresponding to each laser point in the first laser point set is obtained.
[0033] In combination with the first aspect, in one implementation, obtaining the dynamic noise based on the observation noise, and the normal vector and flatness of the first plane includes:
[0034] Obtaining a projection of the observation noise on the normal vector of the first plane;
[0035] The projection is divided by the square of the flatness of the first plane to obtain dynamic noise.
[0036] In combination with the first aspect, in one implementation, the laser point cloud is denoised based on the dynamic noise:
[0037] Obtaining a point-to-surface distance residual and a first Jacobian matrix of each laser point in the first laser point set;
[0038] Dividing the point-to-plane distance residual and the first Jacobian matrix by the dynamic noise to obtain a normalized observation noise matrix;
[0039] The laser point cloud is denoised based on the normalized observation noise matrix and the Kalman filter method.
[0040] In a second aspect, an embodiment of the present application provides a mapping device based on a laser inertial odometer based on the above method, the device comprising:
[0041] A preprocessing module is used to convert the coordinates of each laser point in the laser point cloud from the laser radar coordinate system to the global coordinate system to obtain the nearest point set of each laser point;
[0042] An analysis module is used to perform plane fitting on each of the nearest point sets to obtain a first plane that can be fitted; and to select laser points that meet the point-to-plane distance constraint condition from the laser point cloud to generate a first laser point set;
[0043] An observation module, configured to obtain observation noise corresponding to each laser point in the first laser point set based on preset parameters of the laser radar; and obtain dynamic noise based on the observation noise, and a normal vector and flatness of the first plane;
[0044] The construction module is used to denoise the laser point cloud based on the dynamic noise and construct a map according to the denoised laser point cloud.
[0045] The beneficial effects brought by the technical solution provided in the embodiments of the present application include:
[0046] The coordinates of each laser point in the laser point cloud are converted from the laser radar coordinate system to the global coordinate system to obtain the nearest point set of each laser point. Each nearest point set is plane-fitted to obtain the first plane that can be fitted. The laser points that meet the point-to-surface distance constraint conditions are selected from the laser point cloud to generate the first laser point set. The observation noise corresponding to each laser point in the first laser point set is obtained based on the preset parameters of the laser radar. Based on the observation noise, the normal vector and flatness of the first plane, the dynamic noise is obtained. The laser point cloud is denoised based on the dynamic noise, and a map is constructed based on the denoised laser point cloud. By replacing the fixed noise of the point-to-surface constraint with dynamic noise, the accuracy of mapping by laser inertial odometer is effectively improved. BRIEF DESCRIPTION OF THE DRAWINGS
[0047] Figure 1 This is a flow chart of the first embodiment of the mapping method based on laser inertial odometer of the present application.
[0048] Figure 2 This is a schematic diagram of a process for obtaining the observation noise corresponding to each laser point in the first laser point set based on preset parameters of the laser radar in this application. DETAILED DESCRIPTION
[0049] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.
[0050] In order to make the objectives, technical solutions and advantages of the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.
[0051] First, please refer to Figure 1 , Figure 1This is a flow chart of the first embodiment of the mapping method based on laser inertial odometer of the present application. The present application embodiment provides a mapping method based on laser inertial odometer, the method comprising the following steps:
[0052] Step S1: Convert the coordinates of each laser point in the laser point cloud from the laser radar coordinate system to the global coordinate system, and then obtain the nearest point set of each laser point.
[0053] Step S2: performing plane fitting on each of the nearest point sets to obtain a first plane that can be fitted.
[0054] Step S3: Filter out laser points that meet the point-to-surface distance constraint condition from the laser point cloud to generate a first laser point set.
[0055] Step S4: obtaining the observation noise corresponding to each laser point in the first laser point set based on the preset parameters of the laser radar.
[0056] Step S5: Obtain dynamic noise based on the observation noise, and the normal vector and flatness of the first plane.
[0057] Step S6: De-noise the laser point cloud based on the dynamic noise, and construct a map based on the de-noised laser point cloud.
[0058] The method obtains observation noise based on point-surface constraints through steps S1-S4, then obtains dynamic noise based on the observation noise, denoises the laser point cloud through the dynamic noise, and constructs a map based on the denoised laser point cloud, effectively improving the accuracy of mapping through the laser inertial odometer.
[0059] In some embodiments, before the above step S1, the method further includes:
[0060] First, the intermediate quantity is calculated through the measurement value of the inertial measurement unit from the previous moment to the current moment. The intermediate quantity includes the position and velocity of the measured object from the previous moment to the current moment.
[0061] Afterwards, the motion distortion of each laser point in the laser point cloud is removed based on the intermediate quantity, and the laser point cloud is aligned to the laser radar coordinate system at the current moment.
[0062] In this embodiment, the above-mentioned measurement values include the acceleration and angular velocity of the measured object in the three axes XYZ of the inertial measurement unit. k The predicted state x of the object being measured at any moment k|k-1 and covariance P k|k-1 With t k To fuse the laser point cloud at time t, the laser point cloud needs to be aligned to k The laser radar coordinate system at time.
[0063] The calculation formula for the above intermediate quantity is:
[0064]
[0065] In formula (1), t j and t m Represent different sampling times respectively, j represents the sampling time t j The laser radar coordinate system where the laser point is located, m represents t m The angular velocity ω measured by the inertial measurement unit at the moment m and acceleration a m The laser radar coordinate system. b ω represents the zero bias of the inertial measurement unit, b a represents the accelerometer zero bias, k g represents the acceleration due to gravity, t m The speed of the object being measured at a moment is k v m , location is k p m and posture t j The speed of the object being measured at a moment is k v j , location is k p j and posture By b ω 、b a and k g, can be obtained from t according to formula (1) m The speed of the object being measured at any moment k v m ,Location k p m and posture Calculate t j The speed of the object being measured at any moment k v j ,Location k p j and posture Where Δt = t j -t m .
[0066] It should be noted that removing the motion distortion of each laser point in the laser point cloud by the intermediate quantity calculated by the inertial measurement unit can effectively improve the ghosting blur of the constructed map and improve the accuracy of the constructed map.
[0067] In some embodiments, in the above step S1, K nearest points of each laser point are obtained as the nearest point set through a K nearest neighbor algorithm.
[0068] In some embodiments, the above step S2 includes the following steps:
[0069] S21: According to G n TG p- G d=0 to get the plane fitting equation, G indicates that all variables are in the global coordinate system, G n represents the plane normal vector, G d represents the distance from the laser point to the origin of the global coordinate system along the normal vector direction. G p is an arbitrary 3D point on the plane.
[0070] The formula of the plane fitting equation is as follows:
[0071]
[0072] In formula (2), x = G n / G d, G p1 to G p N is a 3D point on the plane and T represents the matrix transpose.
[0073] S22: performing plane fitting on each of the closest point sets based on the above plane fitting equation to obtain a first plane that can be fitted.
[0074] Specifically, plane fitting is performed on each of the nearest point sets based on the plane fitting equation. If the nearest point set cannot form a plane, the subsequent steps are not performed. If the nearest point set can form a first plane, the first plane corresponding to the nearest point set is obtained.
[0075] It should be noted that due to the noise in the observation of the laser point cloud, the first plane obtained by fitting is not an absolute plane under ideal conditions. Therefore, it is necessary to perform uncertainty analysis on the first plane through subsequent steps, so as to calculate its dynamic noise according to each point surface constraint and improve the accuracy of the calculation.
[0076] In some embodiments, in the above step S3, the point-to-plane distance constraint condition is: the distance between the laser point in the laser point cloud and the first plane is less than a preset distance.
[0077] In this embodiment, the preset distance is preferably set to 0.5 m. Since the laser points that do not meet the point-surface constraint cannot be subjected to uncertainty analysis, it is necessary to set the point-surface distance constraint condition to screen out the laser points in the laser point cloud that are suitable for uncertainty analysis.
[0078] In some embodiments, the method for obtaining the normal vector and flatness of the first plane includes:
[0079] First, the covariance matrix of the first laser point set is obtained, and then the covariance matrix is subjected to eigenvalue decomposition or singular value decomposition to obtain a corresponding principal component vector set, and finally the normal vector and flatness of the first plane are obtained according to the principal component vector set.
[0080] In this embodiment, the covariance matrix of the first laser point set is obtained, and then the covariance matrix is subjected to eigenvalue decomposition or singular value decomposition to obtain a corresponding principal component vector set, and the method used is principal component analysis.
[0081] The core idea of principal component analysis is to extract the main components of the data to replace the original data. Specifically, it maps n-dimensional features to a feature space composed of k-dimensional orthogonal features. This new k-dimensional feature is called the principal component, and the method to achieve this mapping is through projection. When analyzing the distribution of laser point clouds, these principal components are identified by calculating the covariance matrix of the laser point cloud. The covariance matrix reveals the correlation between different variables. Subsequently, the covariance matrix is subjected to eigenvalue decomposition or singular value decomposition to obtain a set of principal component vectors.
[0082] It should be noted that obtaining the normal vector and flatness of the first plane by principal component analysis can reduce the loss caused by the calculation process.
[0083] In some embodiments, the principal component vectors include a first principal component vector, a second principal component vector, and a third principal component vector, which correspond to the maximum variance, the second largest variance, and the minimum variance of the first laser point set, respectively.
[0084] If the eigenvalues corresponding to the first principal component vector and the second principal component vector are much larger than the eigenvalues corresponding to the third principal component vector, the third principal component vector is used as the normal vector of the first plane, and then the flatness of the first plane is calculated based on the eigenvalues corresponding to the first principal component vector, the second principal component vector, and the third principal component vector.
[0085] In this embodiment, the flatness calculation formula of the first plane is:
[0086]
[0087] In formula (3), λ1, λ2, and λ3 are the eigenvalues corresponding to the first principal component vector, the second principal component vector, and the third principal component vector, respectively. λ1>λ2>λ3, σ plane is the flatness of the first plane, indicating how close the first plane is to the absolute plane. The range is (0,1]. When the flatness is 1, the first plane is a square. When the flatness is less than 1, the first plane is a rectangle or a curved surface close to a plane.
[0088] In some embodiments, please refer to Figure 2, the present application is a flow chart of obtaining the observed noise corresponding to each laser point in the first laser point set based on the preset parameters of the laser radar. In the above step S4, obtaining the observed noise corresponding to each laser point in the first laser point set based on the preset parameters of the laser radar includes the following steps:
[0089] Step S41: Based on the preset parameters of the laser radar, a random model of the original observation value and a laser point measurement model are obtained.
[0090] Step S42: Building a laser point random model based on the random model of the original observation value and the laser point measurement model.
[0091] Step S43: converting the coordinates of the laser points in the laser point random model from the laser radar coordinate system to the global coordinate system to obtain a first measurement model.
[0092] Step S44: Based on the first measurement model, obtaining the observation noise corresponding to each laser point in the first laser point set.
[0093] In this embodiment, in the above step S41, the original observation value x of the laser radar is obs =[d α β] T Located in the polar coordinate system centered on the lidar, the original observation noise includes two aspects: the noise of the distance d measured along the direction of the laser beam and the noise from the LiDAR azimuth angle α and elevation angle β So the random model of the original observations is n obs for:
[0094]
[0095] Specifically, taking the Livox Horizon radar as an example, its original observation value is:
[0096] R obs =diag(0.02 2 , 0.05 2 , 0.05 2 ), the units are m 2 、deg 2 and deg 2 .
[0097] According to the calculation method from polar coordinate system to Cartesian coordinate system, the laser point measurement model is:
[0098]
[0099] In formula (5), L p is the three-dimensional laser point coordinate in the laser radar coordinate system, x obsis the original observation value of the lidar, including the vertical angle α, horizontal angle β and distance d from the laser point to the center of the lidar.
[0100] In this embodiment, in the above step S42, the laser point random model is obtained according to the above formula (4) and formula (5):
[0101]
[0102] In formula (6), J obs is the Jacobian of the laser point relative to the original observation value of the laser point obtained by the above laser point measurement model, R obs is the original observation value of the laser point, which can be obtained from the manual of the laser radar sensor. T is the matrix transpose. The observation noise R of the laser point in the laser radar coordinate system can be obtained through the error propagation principle. p .
[0103] It should be noted that many existing measurement methods set the observation noise of the laser point to a fixed value. However, considering the different measurement distances and angles, the observation noise of each laser point actually changes dynamically. Therefore, the fixed observation noise needs to be replaced with dynamic observation noise to improve the measurement accuracy.
[0104] In this embodiment, in the above step S43, the coordinates of the laser points in the laser point random model are converted from the laser radar coordinate system to the global coordinate system to obtain the first measurement model, including the following steps:
[0105] First, since the constraint association from the laser point to the plane is performed in the global coordinate system, the coordinates of the laser point in the laser point random model need to be converted from the lidar coordinate system to the global coordinate system. The conversion formula is as follows:
[0106]
[0107] In formula (7), L p is the coordinate of the laser point in the laser radar coordinate system, using the rotation matrix from the laser radar coordinate system to the global coordinate system and pan G t L , the coordinates of the laser point in the global coordinate system can be calculated G p.
[0108] Then, according to formula (6) and formula (7), the first measurement model is obtained:
[0109]
[0110] In formula (8), due to the uncertainty of the conversion parameters from the lidar coordinate system to the global coordinate system, and R t Represents the two observation noise matrices of the laser point in the lidar coordinate system, R θ To convert the parameters, use R θ Jacobian J of the point relative to the laser point in the global coordinate system θ ,and Jacobian of the point in the global coordinate system relative to the laser point According to the error propagation principle, the observation noise of the laser point in the global coordinate system can be calculated
[0111] In some embodiments, in the above step S5, the dynamic noise is obtained based on the observed noise, and the normal vector and the flatness of the first plane, including the following steps:
[0112] First, the projection of the observation noise on the normal vector of the first plane is obtained, and then the projection is divided by the square of the flatness of the first plane to obtain the dynamic noise.
[0113] In this embodiment, a mathematical model of the distance constraint from the laser point to the first plane is first constructed:
[0114] z d = G p TG n- G d+n d (9)
[0115] In formula (9), z d is the distance from the laser point to the first plane, the theoretical value is 0, n d is the noise of the distance from the laser point to the first plane, n d The distribution of Ideally, the covariance is 0. G p is the coordinate of the laser point in the global coordinate system, G n is the normal vector of the first plane, G d is the distance from the laser point to the origin of the global coordinate system along the direction of the normal vector of the first plane.
[0116] The ideal state means that the flatness of the plane is 1, the thickness is 0, and the laser point is a point with a radius of 0. However, in actual applications, the distance from the point to the plane and the noise of the distance from the point to the plane do not meet the ideal state, so n is usually set. d Variance
[0117] Assuming that a laser point falls on an absolute plane, the point-surface constraint noise is the projection of the laser point observation noise on the plane normal vector. If the plane is not an absolute plane, the point-surface constraint noise will be amplified accordingly. In this case, flatness can be used to approximate the noise amplification process.
[0118] Combining the above formula (9) and the above inference, we can get the dynamic noise model based on point-to-surface distance constraint:
[0119]
[0120] In formula (10), n d is the dynamic noise, n T ∑ p n is the projection of the observation noise on the normal vector of the first plane, σ plane is the flatness of the first plane.
[0121] In some embodiments, in the above step S6, denoising the laser point cloud based on dynamic noise includes the following steps:
[0122] Firstly, the point-to-surface distance residuals and the first Jacobian matrix of each laser point in the first laser point set are obtained. Then, the point-to-surface distance residuals and the first Jacobian matrix are divided by the dynamic noise to obtain the normalized observation noise matrix. Finally, the laser point cloud is denoised based on the normalized observation noise matrix and the Kalman filter method.
[0123] Specifically, the first Jacobian matrix is the result of derivation of formula (9) with respect to the laser radar pose variables.
[0124] It should be noted that when using the existing formula to calculate the Kalman filter gain, it is necessary to calculate the matrix inversion of two observation dimensions and the matrix inversion of two sub-state dimensions. When the point-to-surface distance noise is a fixed value, the process of inverting the observation noise matrix can be simplified, thereby converting the main time consumption in calculating the Kalman filter gain into the matrix inversion of two sub-state dimensions.
[0125] However, when dynamic noise is used, the observation noise matrix no longer has the property of simplifying matrix inversion. At this time, in order to obtain an observation noise matrix that is easy to invert, it is necessary to use the "noise whitening" method, that is, divide both the point-to-surface distance residual and the first Jacobian matrix by the dynamic noise to obtain a normalized noise matrix.
[0126] In this embodiment, the normalized observation noise matrix is:
[0127]
[0128] In formula (11), is the residual distance between points and surfaces, is the first Jacobian matrix, is the dynamic noise, x= G n / G d, G n represents the first plane normal vector,G d represents the distance from the laser point to the origin of the global coordinate system along the direction of the normal vector of the first plane. At this time, the observation noise matrix becomes a unit matrix, thereby reducing the calculation time of the Kalman filter.
[0129] In a second aspect, the present application provides a mapping device based on a laser inertial odometer, which includes a preprocessing module, an analysis module, an observation module and a construction module.
[0130] The above-mentioned preprocessing module is used to convert the coordinates of each laser point in the laser point cloud from the laser radar coordinate system to the global coordinate system, and obtain the nearest point set of each laser point.
[0131] The above analysis module is used to perform plane fitting on each nearest point set to obtain a first plane that can be fitted, and then select laser points that meet the point-to-plane distance constraint from the laser point cloud to generate a first laser point set.
[0132] The observation module uses preset parameters of the laser radar to obtain the observation noise corresponding to each laser point in the first laser point set, and then obtains the dynamic noise based on the observation noise, and the normal vector and flatness of the first plane.
[0133] The above-mentioned construction module uses dynamic noise to denoise the laser point cloud and constructs a map based on the denoised laser point cloud.
[0134] It should be noted that before the device is started, each module needs to be time synchronized. Specifically, the signal is received through Beidou communication, and then the processed data is imported into each module through the serial port or direct wiring, so that the time information of the recorded data is unified to Beidou time, thereby realizing the time synchronization of data in all modules.
[0135] After data time synchronization, the spatial coordinates of each sensor in each module are not unified. It is necessary to unify the coordinate systems of the lidar radar and the inertial measurement unit so that the posture changes received by the inertial measurement unit can be provided to other modules during the mapping process to increase the mapping accuracy.
[0136] It should be noted that the serial numbers of the above-mentioned embodiments of the present application are only for description and do not represent the advantages or disadvantages of the embodiments.
[0137] The terms "including" and "having" and any variations thereof in the specification and claims of this application and the above-mentioned drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally includes steps or units that are not listed, or optionally includes other steps or units inherent to these processes, methods, products or devices. The terms "first", "second" and "third" are used to distinguish different objects, etc., and do not represent a sequence, nor do they limit "first", "second" and "third" to different types.
[0138] In the description of the embodiments of the present application, "exemplary", "for example" or "for example" are used to indicate examples, illustrations or descriptions. Any embodiment or design described as "exemplary", "for example" or "for example" in the embodiments of the present application should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary", "for example" or "for example" is intended to present related concepts in a specific way.
[0139] In the description of the embodiments of the present application, unless otherwise specified, “ / ” means or, for example, A / B can mean A or B; the “and / or” in the text is merely a description of the association relationship of associated objects, indicating that three relationships may exist, for example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. In addition, in the description of the embodiments of the present application, “multiple” refers to two or more than two.
[0140] In some processes described in the embodiments of the present application, multiple operations or steps that appear in a specific order are included, but it should be understood that these operations or steps may not be executed in the order in which they appear in the embodiments of the present application or in parallel, and the sequence number of the operation is only used to distinguish the different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed in sequence or in parallel, and these operations or steps may be combined.
[0141] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, disk, CD) as described above, and includes a number of instructions for a terminal device to execute the methods described in each embodiment of the present application.
[0142] The above are only preferred embodiments of the present application, and are not intended to limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.
Claims
1. A mapping method based on laser inertial odometer, characterized in that: The method comprises: The coordinates of each laser point in the laser point cloud are converted from the laser radar coordinate system to the global coordinate system to obtain the nearest point set of each laser point; Perform plane fitting on each of the nearest point sets to obtain the first plane that can be fitted; Selecting laser points that satisfy the point-to-surface distance constraint condition from the laser point cloud to generate a first laser point set; Acquire the observation noise corresponding to each laser point in the first laser point set based on preset parameters of the laser radar; Based on the observation noise, and the normal vector and the flatness of the first plane, dynamic noise is obtained; The laser point cloud is denoised based on the dynamic noise, and a map is constructed according to the denoised laser point cloud.
2. The mapping method based on laser inertial odometer according to claim 1, characterized in that: Before the coordinate system conversion, it also includes: Calculating intermediate quantities through the measurement values of the inertial measurement unit between the previous moment and the current moment, wherein the intermediate quantities include the position and velocity of the measured object between the previous moment and the current moment; Based on the intermediate amount, the motion distortion of each laser point in the laser point cloud is removed, and the laser point cloud is aligned to the laser radar coordinate system at the current moment.
3. The mapping method based on laser inertial odometer according to claim 1, characterized in that: The step of obtaining the closest point set of each laser point includes: The nearest point set of each laser point is obtained by K nearest neighbor algorithm.
4. The mapping method based on laser inertial odometer according to claim 1, characterized in that: The point-to-surface distance constraint conditions include: The distance between the laser point and the first plane is smaller than a preset distance.
5. The mapping method based on laser inertial odometer according to claim 1, characterized in that: The method for obtaining the normal vector and flatness of the first plane includes: Acquire a covariance matrix of the first laser point set; Performing eigenvalue decomposition or singular value decomposition on the covariance matrix to obtain a corresponding principal component vector set; The normal vector and the flatness of the first plane are obtained according to the principal component vector set.
6. The mapping method based on laser inertial odometer according to claim 5, characterized in that: The acquiring the normal vector and the flatness of the first plane according to the principal component vector set includes: The principal component vectors include first, second, and third principal component vectors, which correspond to the maximum, second largest, and minimum variances of the first laser point set respectively; If the eigenvalues corresponding to the first principal component vector and the second principal component vector are much larger than the eigenvalue corresponding to the third principal component vector, the third principal component vector is used as the normal vector of the first plane; The flatness of the first plane is calculated according to the eigenvalues corresponding to the first, second, and third principal component vectors.
7. The mapping method based on laser inertial odometer according to claim 1, characterized in that: The obtaining, based on preset parameters of the laser radar, observation noise corresponding to each laser point in the first laser point set includes: Based on the preset parameters of the laser radar, a random model of the original observation value and a laser point measurement model are obtained; Based on the random model of the original observation value and the laser point measurement model, a laser point random model is established; Converting the coordinates of the laser points in the laser point random model from the laser radar coordinate system to the global coordinate system to obtain a first measurement model; Based on the first measurement model, an observation noise corresponding to each laser point in the first laser point set is obtained.
8. The mapping method based on laser inertial odometer according to claim 1, characterized in that: The obtaining of dynamic noise based on the observed noise, and the normal vector and the flatness of the first plane comprises: Obtaining a projection of the observation noise on the normal vector of the first plane; The projection is divided by the square of the flatness of the first plane to obtain dynamic noise.
9. The mapping method based on laser inertial odometer according to claim 1, characterized in that: Denoising the laser point cloud based on the dynamic noise: Obtaining a point-to-surface distance residual and a first Jacobian matrix of each laser point in the first laser point set; Dividing the point-to-plane distance residual and the first Jacobian matrix by the dynamic noise to obtain a normalized observation noise matrix; The laser point cloud is denoised based on the normalized observation noise matrix and the Kalman filter method.
10. A mapping device based on a laser inertial odometer according to any one of claims 1 to 9, the device comprising: A preprocessing module is used to convert the coordinates of each laser point in the laser point cloud from the laser radar coordinate system to the global coordinate system to obtain the nearest point set of each laser point; An analysis module is used to perform plane fitting on each of the nearest point sets to obtain a first plane that can be fitted; and to select laser points that meet the point-to-plane distance constraint condition from the laser point cloud to generate a first laser point set; An observation module, configured to obtain observation noise corresponding to each laser point in the first laser point set based on preset parameters of the laser radar; and obtain dynamic noise based on the observation noise, and a normal vector and flatness of the first plane; The construction module is used to denoise the laser point cloud based on the dynamic noise and construct a map according to the denoised laser point cloud.
