CN116878542A - Laser SLAM method for inhibiting height drift of odometer - Google Patents

Laser SLAM method for inhibiting height drift of odometer Download PDF

Info

Publication number
CN116878542A
CN116878542A CN202310928868.7A CN202310928868A CN116878542A CN 116878542 A CN116878542 A CN 116878542A CN 202310928868 A CN202310928868 A CN 202310928868A CN 116878542 A CN116878542 A CN 116878542A
Authority
CN
China
Prior art keywords
odometer
pose
key frame
radar
point cloud
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310928868.7A
Other languages
Chinese (zh)
Inventor
秦毫
邹艳丽
庾国梁
刘会鹏
谭宇飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangxi Normal University
Original Assignee
Guangxi Normal University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangxi Normal University filed Critical Guangxi Normal University
Priority to CN202310928868.7A priority Critical patent/CN116878542A/en
Publication of CN116878542A publication Critical patent/CN116878542A/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Manufacturing & Machinery (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

The invention provides a laser SLAM method for inhibiting height drift of an odometer, which comprises the following steps: acquiring various preprocessed point clouds, optimizing displacement by adopting Kalman filtering, determining the pose of the radar odometer according to the optimized radar odometer displacement, and optimizing the radar odometer with high drift by adopting a gradient limiting algorithm; and converting the various point clouds and the odometer information to a point cloud map to be built, extracting a key frame, extracting features of the point clouds of the key frame, carrying out smooth operation on the height by adopting a history pose after solving the pose of the odometer of the key frame, extracting loop detection factors in a Scan Context loop detection method based on radius search, and obtaining global laser radar odometer 6-DOF pose information and a three-dimensional point cloud map by carrying out graph optimization on factor graphs. The invention takes a gradient limiting algorithm and a height smoothing algorithm as cores to realize the effects of high-precision odometer pose and a point cloud map without distortion.

Description

Laser SLAM method for inhibiting height drift of odometer
Technical Field
The invention belongs to the technical field of automatic driving, and particularly relates to a laser SLAM method for inhibiting height drift of an odometer.
Background
With the vigorous development of automatic driving technology, more and more enterprises begin to apply the automatic driving technology in different scenes, wherein a high-precision map is a key problem which needs to be solved in realizing automatic driving, and the high-precision map not only can help vehicles to complete automatic driving functions, but also can improve the safety of the vehicles. In order to be able to construct high-precision maps, mapping and positioning (Simultaneous Localization and Mapping) technology is a central concern in the industry. The sensors used in the current mainstream high-precision map construction method comprise a monocular camera, a binocular camera, a depth camera, a laser radar, a millimeter wave radar and the like, wherein the laser radar has the characteristics of strong anti-interference capability, long detection distance, high precision, good instantaneity and the like compared with other sensors, and therefore, the sensor is widely applied to high-precision map reconstruction. However, the existing SLAM algorithm only relying on the laser radar also exposes the phenomenon of low vertical resolution and easy high drift on undulating road sections and even flat ground.
In order to solve the problem of the traditional SLAM algorithm, zhang and the like propose a lom algorithm, an environment acquisition device of a two-dimensional laser radar and an Inertial Measurement Unit (IMU) is adopted, and a matching method based on angle, point and plane characteristics is adopted for point cloud matching, so that the graph construction precision and the matching efficiency are improved compared with the traditional point cloud registration algorithm such as iterative closest point (iterative closet point, ICP), normal distribution transformation (normal distributions transform, NDT) and the like. In addition, the lom algorithm uses a uniform model to perform motion compensation on point cloud and fuses two types of sensor data through a loose coupling mode to improve the accuracy of the laser radar odometer, but the initial laser point cloud is not considered to be subjected to data preprocessing, and meanwhile, the back-loop detection is not considered to be added to the rear end part to perform pose correction, so that the robustness of the algorithm is not strong, and drifting and double images are easy to occur in the process of building a map. The light-weight lego-lom is proposed by Shan and the like, firstly, the front end is used for preprocessing point cloud, and the non-ground points are separated through angle detection of the laser radar harness and clustered, so that the point cloud registration efficiency is greatly improved, and the data calculation amount is reduced. And then a loop detection algorithm based on radius search is added to the rear end part, so that the position and the posture of the point cloud with partial errors are corrected, but the problems of low loop detection efficiency, low robustness and high drift of the odometer exist in the actual environment.
Disclosure of Invention
In order to solve the above-mentioned shortcomings of the prior art, the present invention provides a laser SLAM method for suppressing the height drift of an odometer, so as to solve the above-mentioned technical problems.
The invention provides a laser SLAM method for inhibiting height drift of an odometer, which comprises the following steps:
acquiring various preprocessed point clouds, extracting features, calculating laser radar odometer displacement, optimizing the displacement by adopting Kalman filtering, determining radar odometer pose according to the optimized radar odometer displacement, and optimizing the radar odometer with height drift by adopting a gradient limiting algorithm to obtain more accurate radar odometer pose;
converting the various point clouds and the odometer information to a point cloud map to be built, extracting a key frame, extracting characteristics of the point clouds of the key frame, carrying out smooth operation on the height by adopting a history pose after solving the odometer pose of the key frame, optimizing the radar odometer pose and the point cloud map, storing the key frame information and the odometer pose, and adding the key frame information and the odometer pose into a factor graph to obtain a factor of the odometer pose of the key frame;
and extracting loop-back detection factors in the Scan Context loop-back detection method based on radius search, adding the loop-back detection factors and the key frame odometer pose factors into an ISAM2 factor graph, and correcting the odometer pose and the point cloud map by carrying out graph optimization on the factor graph to obtain global laser radar odometer 6-DOF pose information and a three-dimensional point cloud map.
Further, the obtaining the preprocessed point clouds, extracting features and calculating laser radar odometer displacement includes:
extracting features of the preprocessed various point cloud data, and extracting plane points and edge points of the point cloud by adopting a smoothness principle; the smoothness c of the to-be-evaluated local curved surface for distinguishing the plane point and the edge point is as follows:
in the above, S is a point set for calculating a point neighborhood, r j 、r i For the point p in the set S j 、p i Euclidean distance of the corresponding pixel point to the sensor;
solving [ delta ] t of the odometer at the moment k relative to the moment k-1 through the change of the plane point characteristics z ,Δθ roll ,Δθ pich ]Wherein Δt is z Delta theta is the increment of Z-axis displacement unit time roll Delta theta is the increment of the unit moment of the roll angle pich The pitch angle is increased per unit time; solving [ delta ] t through change of edge point characteristics x ,Δt y ,Δθ yaw ]Wherein Δt is x Delta t is the increment of x-axis displacement unit time y Delta theta is the increment of the y-axis displacement unit time yaw And (5) obtaining the incremental 6-DOF pose of the odometer for the increment of the yaw angle unit time.
Further, the optimizing the displacement by adopting the kalman filter, and determining the radar odometer pose according to the optimized radar odometer displacement comprises the following steps:
for highly correlated Δt z 、Δθ roll 、Δθ pich Performing Kalman filtering optimization, wherein the state equation of a Kalman filter system is x k ＝x k-1 +u k-1 +w k-1 The method comprises the steps of carrying out a first treatment on the surface of the The Kalman filter observation equation is: z k ＝x k +v k ；
x k Is a state vector, i.e. the pre-filter pose Deltat z 、Δθ roll 、Δθ pich ，u k-1 Is the state control vector at time k-1, w k-1 Is the noise at system k time, subject to highThe S distribution: w (w) k-1 -N (0, Q), Q being the system noise covariance matrix; z k Is the observation vector at the moment k, namely the filtered pose output value, v k Is the noise observed at time k, obeys gaussian distribution: v k -N (0, R), R being the covariance matrix of the observed noise;
increment 3-DOF pose [ delta t ] of the optimized odometer x ,Δt y ,Δt z ]Converting into a translation matrix, and obtaining the 3-DOF pose [ delta theta ] roll ,Δθ pich ,Δθ yaw ]Converting into a rotation matrix;
the rotation matrix of the 3-DOF pose of the odometer at any moment in the world coordinate system can be obtained through continuous accumulation:
wherein ,for the rotation matrix of the incremental 3-DOF pose of the mileometer at time k relative to time k-1 in the radar coordinate system L +.>The rotation matrix of the incremental 3-DOF pose of the mileometer at the moment k under the world coordinate system W relative to the origin; />The rotation matrix of the incremental 3-DOF pose of the mileometer at the moment k-1 under the world coordinate system W relative to the origin;
by inverse solutionObtaining the 3-DOF pose of the mileometer at the moment k under the world coordinate system>Simultaneously, the integration of the translation matrix is combined to obtain +.>
Further, the method for optimizing the radar odometer with the height drift by adopting the gradient limiting method to obtain a more accurate radar odometer pose comprises the following steps:
calculating the gradient G of the odometer from the moment k-1 to the moment k according to the following r ：
Obtaining road design gradient of map to be builtAnd->
By determining the gradient G of the odometer from time k-1 to time k r And road design gradient of map to be builtTo determine whether a high drift occurs, wherein in +.>Determining that a height drift occurs; when->Determining that no height drift has occurred;
in the case of determining that a height drift occurs, forRestriction is performed to make G r Equal to->Then inverse solution G r Obtaining newAnd use New->Replacement->
Further, the step of smoothing the altitude by using the historical pose after solving the pose of the key frame odometer, and optimizing the radar odometer pose and the point cloud map includes:
optimizing odometer pose using planar features of key framesOptimizing the position of the odometer by using edge features +.>
Continuously adding the optimized point cloud and the odometer information into a map queue, and then performing continuous iterative optimization in a scan bitmap mode, wherein the optimization mode is to perform smooth filtering operation on the height of key frames by adopting historical key frames, and the method specifically comprises the following steps: collecting 6-DOF pose information of historical key frame odometerAdding the height difference of the position and the posture of the milestones at the k-1 moment and the k moment into a map queue, and performing intervention after finding that the height difference of the milestones at the k moment exceeds a preset value, and enabling the height of the milestones at the k moment to be +.>Consists of a current key frame height and a historical key frame height:
where η=a+b+c+d, a, b, c, d is a weight parameter.
Further, the extracting loop detection factors in the Scan Context loop detection method based on radius search includes:
after receiving a frame of complete scanning data from a radar, checking whether the frame is a key frame or not, inquiring whether a history key frame exists in the radius of the current key frame, if so, taking the history key frame as a candidate history key frame, and extracting a radius search loop factor in the process;
and developing the 3D point cloud of each frame into a 2D image, then establishing a Scan Context, carrying out two-stage hierarchical search by introducing a rotation invariance descriptor Ring Key, constructing a KD tree for scanning the index Key, quickly finding candidate historical Key frames to match with the current Key frames by using a nearest neighbor search algorithm, calculating the Ring Key similarity, finding the most similar candidate historical Key frames, and extracting a Scan Context loop factor of the process.
Further, the correcting of the odometer pose and the point cloud map by optimizing the factor graph to obtain global laser radar odometer 6-DOF pose information and a three-dimensional point cloud map comprises the following steps:
and constructing constraint items for optimizing the pose and the track, iterating pose transformation of a current key frame and a closed-loop key frame and incremental map registration through ICP, expressing pose estimation as a maximum posterior problem, then based on Gaussian noise covariance, converting the problem into a nonlinear least square problem, and smoothing and linearizing partial nonlinear factors such as the track, the pose and the like by utilizing ISAM2 on the basis of Bayesian tree to obtain a final accurate global track and pose.
The invention has the beneficial effects that: the laser SLAM method for inhibiting the height drift of the odometer, provided by the invention, aims at the problems that the height drift phenomenon of the odometer and the map distortion phenomenon caused by the conventional laser SLAM algorithm are easy to occur in the process of building the map, can realize high-precision map building under complex terrain by using a laser radar, improves and realizes the effects of high-precision odometer pose and a non-distorted point cloud map through algorithm optimization, and provides a new solution for the SLAM field by taking a gradient limiting algorithm and a height smoothing algorithm as cores instead of adopting a traditional multi-sensor fusion mode.
In addition, the invention can discard the sensors such as the IMU, the camera and the like in the environment such as a factory and the like, can acquire the surrounding environment information only through the laser radar with low cost, and is beneficial to realizing the wide-range application of the robot. In addition, the invention has reliable design principle, simple structure and very wide application prospect.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required to be used in the description of the embodiments or the prior art will be briefly described below, and it will be obvious to those skilled in the art that other drawings can be obtained from these drawings without inventive effort.
FIG. 1 is a schematic flow chart of a method of one embodiment of the invention.
FIG. 2 is a schematic flow chart of a data processing method of one embodiment of the invention.
FIG. 3 is a schematic flow chart of front-end radar odometer pose calculation according to an embodiment of the invention.
Fig. 4 is a schematic diagram of a point cloud segmentation algorithm according to an embodiment of the present invention.
Fig. 5 is a schematic diagram of a slope of a pose of an odometer according to an embodiment of the invention.
Fig. 6 is a schematic flow chart of a back-end radar setup diagram of one embodiment of the invention.
Detailed Description
In order to make the technical solution of the present invention better understood by those skilled in the art, the technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, shall fall within the scope of the present invention.
The vertical resolution of the conventional multi-line laser radar is low, the conventional SLAM algorithm is easy to cause the phenomenon of high drift of the odometer when the conventional SLAM algorithm is used for building the image on an outdoor undulating road section or even on a flat road, so that the problems of distortion, ghost and the like of the image are caused. As shown in fig. 1, the method includes:
s1, data preprocessing: after the environmental data acquired by the laser radar are obtained, marking the ground point cloud and the non-ground point cloud by a segmentation algorithm, then clustering the non-ground point cloud by a clustering algorithm, and finally transmitting the segmented ground point cloud and the clustered non-ground point cloud to S2.
S2, calculating the pose of the front-end radar odometer: acquiring various preprocessed point clouds, extracting features, calculating laser radar odometer displacement, optimizing the displacement by adopting Kalman filtering, determining radar odometer pose according to the optimized radar odometer displacement, and optimizing the radar odometer with height drift by adopting a gradient limiting algorithm to obtain more accurate radar odometer pose;
s3, rear-end radar mapping: converting the various point clouds and the odometer information to a point cloud map to be built, extracting a key frame, extracting characteristics of the point clouds of the key frame, carrying out smooth operation on the height by adopting a history pose after solving the odometer pose of the key frame, optimizing the radar odometer pose and the point cloud map, storing the key frame information and the odometer pose, and adding the key frame information and the odometer pose into a factor graph to obtain a factor of the odometer pose of the key frame;
s4, loop detection and factor graph optimization: and extracting loop-back detection factors in the Scan Context loop-back detection method based on radius search, adding the loop-back detection factors and the key frame odometer pose factors into an ISAM2 factor graph, and correcting the odometer pose and the point cloud map by carrying out graph optimization on the factor graph to obtain global laser radar odometer 6-DOF pose information and a three-dimensional point cloud map.
The method provided by the embodiment of the invention comprises links of data preprocessing, front-end radar odometer, rear-end radar image construction, loop detection and the like, and takes a gradient limiting algorithm and a height smoothing algorithm as cores, and integrates the methods of Kalman filtering, loop detection, image optimization and the like to optimize the laser radar odometer with high drift, so that the system can output a high-precision odometer pose and a point cloud map without distortion.
In order to facilitate understanding of the present invention, the following description will be given with respect to the above-mentioned S1 to S4, to further describe a laser SLAM method for suppressing the height drift of an odometer provided by the present invention.
As shown in fig. 2, the data preprocessing includes:
s11: acquiring environmental data acquired by a laser radar;
s12: marking a ground point cloud and a non-ground point cloud of the environmental data by a segmentation algorithm;
s13: clustering the non-ground point cloud by a clustering algorithm;
s14: and taking the ground point cloud obtained by segmentation and the non-ground point cloud after clustering as the preprocessed various point clouds.
In particular, since a multi-line lidar is used in the market and the radar is fixedly installed on a vehicle, the height of the lidar from the ground is constant, and thus the segmentation algorithm can be implemented by the line number of the lidar. As shown in FIG. 3, the RS-16 laser radar adopted in the embodiment has 16 transmitters in the vertical direction, the vertical angle resolution is 0.2 degrees, the horizontal angle of view is 360 degrees, the horizontal angle resolution is 0.2 degrees, and then the number of return points of each transmitter is 1800. Setting point cloud P acquired at time t t ＝{p 1 ,p 2 ,p 3 ,...,p n }，p i Point cloud P at time t t I is more than or equal to 1 and n is more than or equal to n. The point cloud is projected into a range image with a resolution of 1800 x 16. The vertical scanning range of the RS-16 laser radar is [ -15 degrees, 15 degrees.)]Then the ground surface is present at-15 deg. -1 deg.]In the embodiment, the ground is set below the 7 th line, the pitch angle (i.e. the angle alpha in fig. 3) between the upper line and the lower line is solved for the point cloud of the 0-7 wire harness, and ground plane estimation is carried out, which is realizedThe example specifies that the point cloud with an angle α in the range of 10 ° is a ground point, and the other point clouds are set as non-ground points. The method comprises the steps of acquiring the environmental data acquired by the laser radar, and marking the ground point cloud and the non-ground point cloud of the environmental data by a segmentation algorithm.
Clustering operation adopts BFS (Briadth-First-Search) clustering algorithm to perform clustering operation on non-ground points to obtain p i Searching outwards for the center. The embodiment sets that if the number of the cluster point clouds exceeds 30, the cluster point clouds are marked as an available cluster; if the clustering point cloud is smaller than 30 but larger than or equal to 5, counting the point cloud number in the vertical direction, and if the clustering point cloud is larger than 3, calibrating the clustering point cloud as an available cluster. The interference of dynamic objects (pedestrians, vehicles) and small objects such as leaves in the point cloud can be effectively reduced through clustering operation, and then the clustered non-ground points and the segmented ground points can be processed in the next step. Therefore, the data preprocessing method provided by the embodiment of the invention can reduce the data volume of the point cloud and improve the operation speed of the subsequent algorithm.
As shown in fig. 4, the main purpose of the front-end radar odometer link is to extract odometer 6-DOF pose data. The method specifically comprises the following steps:
s21: and adopting a smoothness principle to extract the characteristics.
Firstly, extracting features of various preprocessed point cloud data, and dividing the feature points into two main categories according to a smoothness principle: the invention adopts the smoothness principle to extract the plane points and the edge points of the point cloud when the characteristics are extracted.
The smoothness c of the to-be-evaluated local curved surface for distinguishing the plane point and the edge point is as follows:
in the above formula, S is a point set of computing a point neighborhood, and in this embodiment, S is set to 10.r is (r) j 、r i For the point p in the set S j 、p i Euclidean distance of the corresponding pixel point to the sensor.
S22: and calculating the incremental 6-DOF pose of the odometer through the plane point features and the edge point features.
Then, the [ delta ] t of the milestone at the moment k relative to the moment k-1 is solved through the change of the plane point characteristics z ,Δθ roll ,Δθ pich ]Wherein Δt is z Delta theta is the increment of the z-axis displacement unit moment roll Delta theta is the increment of the unit moment of the roll angle pich Solving [ delta t ] by the change of edge point characteristics for increment of pitch angle unit moment x ,Δt y ,Δθ yaw ]Wherein Δt is x Delta t is the increment of x-axis displacement unit time y Delta theta is the increment of the y-axis displacement unit time yaw The increment of the yaw angle unit moment is adopted, so that the increment 6-DOF pose [ delta t ] of the odometer can be obtained z ,Δt x ,Δt y ,Δθ yaw ,Δθ roll ,Δθ pich ]。
S23: and carrying out Kalman filtering optimization on the incremental 6-DOF pose of the odometer.
Because of simple rough matching of characteristic points, and the small error of the laser radar on the horizontal plane, mainly has the condition of drifting on the height, the method does not relate to deltat of horizontal position x 、Δt y 、Δθ yaw Optimizing only highly correlated deltat z 、Δθ roll 、Δθ pich And (3) performing Kalman filtering optimization:
x k ＝x k-1 +u k-1 +w k-1 (2)
z k ＝x k +v k (3)
wherein, formula (2) is a Kalman filter system state equation, and formula (3) is a Kalman filter observation equation. X is x k Is the state vector at time k, i.e. the pre-filter pose Deltat z 、Δθ roll 、Δθ pich ，u k-1 Is a state control vector at time k-1, u k-1 Is the state control vector at time k-1, w k-1 Is the noise of the control system at time k-1, obeys Gaussian distribution: w (w) k-1 N (0, Q), Q is the system noise covariance matrix, pass-throughSensor calibration is obtained off-line. z k Is the observation vector at the moment k, namely the filtered pose output value, v k Is observed noise at time k, obeying a gaussian distribution: v k N (0, R), R is the covariance matrix of the observed noise.
The present embodiment is set to be at z Q is 0.5 and R is 1 during treatment; for delta theta roll 、Δθ pich Q is 0.001 and R is 0.005 at the time of treatment.
S24: and calculating the 6-DOF pose of the odometer according to the 3-DOF pose of the increment of the odometer after Kalman filtering optimization.
3-DOF pose [ delta t ] of speedometer increment after Kalman filtering optimization x ,Δt y ,Δt z ]Converting into a translation matrix, and converting the 3-DOF pose [ delta theta ] roll ,Δθ pich ,Δθ yaw ]Turning into a rotation matrix. Because the starting point of the odometer is the origin of the world coordinate system, the rotation matrix of the 3-DOF pose of the odometer at any moment in the world coordinate system can be obtained by continuously accumulating according to the formula (4):
wherein ,for the rotation matrix of the incremental 3-DOF pose of the mileometer at time k relative to time k-1 in the radar coordinate system L +.>The rotation matrix of the incremental 3-DOF pose of the mileometer at the moment k under the world coordinate system W relative to the origin; />The rotation matrix of the incremental 3-DOF pose of the mileometer at the moment k-1 under the world coordinate system W relative to the origin; by counter-solving->Obtaining the world coordinate system3-DOF pose of lower k moment odometer>Simultaneously combining the accumulation of the translation matrixThe 6-DOF pose of the front-end odometer is obtained, the laser SLAM method provided by the embodiment of the invention solves the problem that the odometer has drift in height, and reduces the pose error of the laser radar odometer.
However, the position pose estimated by the front-end odometer is easily influenced by the terrain to generate high drift, in order to reduce the drift of the odometer in height, a series of construction indexes are usually regulated when the road is constructed, wherein the gradient is an important index, the maximum gradient index of the common expressway is 5%, and the track gradient value of the front-end odometer after the high drift is not in the index range.
In order to solve the above problem, an embodiment of the present invention further includes:
s25: and optimizing the odometer in the process of drawing by adopting a gradient limiting method. The method comprises the following steps:
as shown in FIG. 5, the point A is the position and the posture of the mileometer at the moment k-1B point is the pose at k time>BC is the vertical displacement of the odometer and AC is the horizontal displacement.
The road gradient, i.e. the percentage of the vertical height ΔH and the horizontal distance ΔL of the slope, is defined as G r The representation is:
the slope of the odometer from time k-1 to time k can be obtained in combination with fig. 5 and equation (5).
wherein ,for the x-axis pose at time k +.>The position and the posture of the x axis at the moment k-1; the z-axis and y-axis are the same and are not described in detail herein.
Obtaining road design gradient of map to be builtAnd->By determining the gradient G of the odometer from time k-1 to time k r And road design gradient of map to be built +.>To determine whether a high drift occurs, wherein in +.>Determining that a height drift occurs; when->Determining that no height drift has occurred; in the case of determining that a height drift occurs, forRestriction is performed to make G r Equal to->Then inverse solution G r Get new->And use New->Replacement->
As shown in fig. 6, the purpose of back-end radar mapping is to build a map and determine map factors. The method specifically comprises the following steps:
s31: and extracting the key frame and acquiring the odometer pose of the key frame.
Firstly, converting various preprocessed point clouds and front-end odometer information to a map to be built, extracting key frames according to a series of historical radar frames, and optimizing the position and posture of the odometer by adopting the plane characteristics of the key frames [ t ] z ,θ roll ,θ pich ]Information, using edge feature optimization [ t ] x ,t y ,θ yaw ]Information.
And continuously adding the optimized point cloud and the odometer information into a map queue, and then performing continuous iterative optimization in a scan to map mode. Although the position and the posture of the odometer after the optimization of the scan to map are more accurate than those of the front-end odometer, if the front-end odometer has no high drift, the odometer still can find the high drift phenomenon even if the front-end given data is wrong, so that the embodiment of the invention adopts the idea of smooth filtering to smoothly optimize the odometer part with the high drift.
S32: and carrying out smooth optimization on the odometer pose of the key frame by using a highly smooth filtering algorithm. Specifically, the highly smooth filtering algorithm is as follows:
firstly, 6-DOF pose information of a historical key frame odometer is required to be continuously collected and added into a map queue, then intervention is carried out after the difference of the heights of the odometer poses at k-1 and k moments is found to exceed 0.1m, and the equation (7) is utilized to enable the height of the odometer at k momentsConsists of a current frame height and a historical frame height:
where η=a+b+c+d.
Regarding the value of the weight a, b, c, d, multiple experiments show that the mapping effect obtained by selecting different weight parameters under different road environments is best. In a flatter road environment, a, b, c, d is taken as 9, 5, 3 and 1; in a rough road environment, a, b, c, d is taken as 3, 5, 3 and 1. After the highly smooth filtering, the occurrence of the height drift of the odometer can be restrained, and the map distortion phenomenon is further reduced. And finally, storing the optimized key frames and the odometer factors, and enabling the optimized key frames and the odometer factors to be used for a subsequent loop detection and factor graph optimization method.
The loop detection method and the factor graph optimization method provided by the embodiment of the invention are mainly used for inhibiting the odometer height drift phenomenon in the SLAM process, realizing loop detection in a complex environment and realizing efficient and stable graph construction.
Specifically, after receiving a frame of complete environmental data from the laser radar, the loop detection thread searches the radius of the historical key frame by checking whether the key frame is the key frame or not, and then inquires whether the radius of the current key frame (which can be 7m or 30s from the current key frame) is the historical key frame or not; and expanding the 3D point cloud of each frame into a 2D image, then establishing a Scan Context, carrying out two-stage hierarchical search by introducing a rotation invariance descriptor Ring Key, constructing a KD tree for scanning the index Key, using a nearest neighbor search algorithm to find a candidate historical Key frame to match with a current Key frame, and calculating the point cloud similarity (for example, ring Key similarity) to find the most similar Key frame, thereby realizing loop detection.
The loop detection method provided by the embodiment of the invention is different from a historical key frame radius search loop method which can only realize loop in a simple environment, and the Scan Context loop detection method can realize loop in a complex environment and has higher running speed. The loop detection method provided by the embodiment of the invention takes the Scan Context loop as a main part and takes the key frame radius search loop as an auxiliary part, and after the two loop detection methods are matched, two loop factors are added into a factor graph for further optimization. The laser SLAM method provided by the embodiment of the invention has the advantages that the track with partial deviation is corrected by loop detection, so that the global odometer has no large deviation, and the fitting degree with the real track is higher.
The implementation of the factor graph optimization method comprises the steps of firstly initializing prior factors, gradually adding radar milemeter factors, radius search loop factors and Scan Context loop factors into a factor graph, constructing constraint items for optimizing positions and trajectories, iterating position and orientation transformation of a current key frame and a closed-loop key frame and incremental MAP registration through ICP, expressing gesture estimation as a maximum posterior (MAP) problem, then based on Gaussian noise covariance, converting the problem into a nonlinear least square problem, and smoothing and linearizing partial nonlinear factors such as the trajectories and the positions by using ISAM2 on the basis of Bayesian trees to obtain final accurate global trajectories and positions. Because the embodiment of the invention introduces the loop-back factor of the space point cloud descriptor Scan Context in the lego-lom original loop-back detection part, a loop-back detection algorithm based on the historical key frame radius search factor and the Scan Context factor is formed, and global consistency optimization is carried out through the ISAM 2.
Although the present invention has been described in detail by way of preferred embodiments with reference to the accompanying drawings, the present invention is not limited thereto. Various equivalent modifications and substitutions may be made in the embodiments of the present invention by those skilled in the art without departing from the spirit and scope of the present invention, and it is intended that all such modifications and substitutions be within the scope of the present invention/be within the scope of the present invention as defined by the appended claims. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims (7)

1. A laser SLAM method of suppressing odometer height drift, comprising:
acquiring various preprocessed point clouds, extracting features, calculating laser radar odometer displacement, optimizing the displacement by adopting Kalman filtering, determining radar odometer pose according to the optimized radar odometer displacement, and optimizing the radar odometer with height drift by adopting a gradient limiting algorithm to obtain more accurate radar odometer pose;
converting the various point clouds and the odometer information to a point cloud map to be built, extracting a key frame, extracting characteristics of the point clouds of the key frame, carrying out smooth operation on the height by adopting a history pose after solving the odometer pose of the key frame, optimizing the radar odometer pose and the point cloud map, storing the key frame information and the odometer pose, and adding the key frame information and the odometer pose into a factor graph to obtain a factor of the odometer pose of the key frame;
and extracting loop-back detection factors in the Scan Context loop-back detection method based on radius search, adding the loop-back detection factors and the key frame odometer pose factors into an ISAM2 factor graph, and correcting the odometer pose and the point cloud map by carrying out graph optimization on the factor graph to obtain global laser radar odometer 6-DOF pose information and a three-dimensional point cloud map.
2. The method of claim 1, wherein the obtaining preprocessed point clouds of various types, extracting features and calculating lidar odometer displacement, comprises:
extracting features of the preprocessed various point cloud data, and extracting plane points and edge points of the point cloud by adopting a smoothness principle; the smoothness c of the to-be-evaluated local curved surface for distinguishing the plane point and the edge point is as follows:
in the above, S is the point set r for calculating the point neighborhood j 、r i For the point p in the set S j 、p i Euclidean distance of the corresponding pixel point to the sensor;
solving [ delta ] t of the odometer at the moment k relative to the moment k-1 through the change of the plane point characteristics z ,Δθ roll ,Δθ pich ]Wherein Δt is z Delta theta is the increment of the z-axis displacement unit moment roll Delta theta is the increment of the unit moment of the roll angle pich The pitch angle is increased per unit time;
solving [ delta ] t through change of edge point characteristics x ,Δt y ,Δθ yaw ]Wherein Δt is x Delta t is the increment of x-axis displacement unit time y Delta theta is the increment of the y-axis displacement unit time yaw Is the increment of yaw angle unit time.
3. The method of claim 2, wherein optimizing the displacement using kalman filtering and determining the radar odometer pose based on the optimized radar odometer displacement comprises:
for highly correlated Δt z 、Δθ roll 、Δθ pich Performing Kalman filtering optimization, wherein the state equation of a Kalman filter system is x k ＝x k-1 +u k-1 +w k-1 The method comprises the steps of carrying out a first treatment on the surface of the The Kalman filter observation equation is: z k ＝x k +v k ；x k Is the state vector at time k, i.e. the pose deltat before filtering z 、Δθ roll 、Δθ pich ，u k-1 Is the state control vector at time k-1, w k-1 Is noise at the time of the system k, obeys Gaussian distribution: w (w) k-1 -N (0, Q), Q being the system noise covariance matrix; z k Is the observation vector at the moment k, namely the filtered pose output value, v k Is the noise observed at time k, obeys gaussian distribution: v k N (0, R), R is the covariance matrix of the observed noise.
Increment 3-DOF pose [ delta t ] of the optimized odometer x ,Δt y ,Δt z ]Converting into a translation matrix, and obtaining the 3-DOF pose [ delta theta ] roll ,Δθ pich ,Δθ yaw ]Converting into a rotation matrix;
the rotation matrix of the 3-DOF pose of the odometer at any moment in the world coordinate system can be obtained through continuous accumulation:
wherein ,for a rotation matrix of the incremental 3-DOF pose of the odometer at time k relative to time k-1 in the radar coordinate system L,the rotation matrix of the incremental 3-DOF pose of the mileometer at the moment k under the world coordinate system W relative to the origin; />The rotation matrix of the incremental 3-DOF pose of the mileometer at the moment k-1 under the world coordinate system W relative to the origin;
by inverse solutionObtaining the 3-DOF pose of the mileometer at the moment k under the world coordinate system>Simultaneously, the integration of the translation matrix is combined to obtain +.>
4. A method according to claim 3, wherein the optimization of the radar odometer with the height drift using the slope limiting method results in a more accurate radar odometer pose, comprising:
calculating the gradient G of the odometer from the moment k-1 to the moment k according to the following r ：
Obtaining road design gradient of map to be builtAnd->
By determining the gradient G of the odometer from time k-1 to time k r And road design gradient of map to be builtTo determine whether a high drift occurs, wherein in +.>Determining that a height drift occurs; when->Determining that no height drift has occurred;
in the event of a determination that a height drift has occurred, the z-axis displacement at time kRestriction is performed to make G r Equal to->Then inverse solution G r Obtain new z-axis displacement at time k>And use New->Replacement->
5. The method of claim 1, wherein smoothing the altitude with the historical poses after solving for the key frame odometer pose, optimizing the radar odometer pose and the point cloud map, comprises:
optimizing odometer pose using planar features of key framesOptimizing the position of the odometer by using edge features +.>
Continuously adding the optimized point cloud and the odometer information into a map queue, and then performing continuous iterative optimization in a scan bitmap mode, wherein the optimization mode is to perform smooth filtering operation on the height of key frames by adopting historical key frames, and the method specifically comprises the following steps: collecting 6-DOF pose information of historical key frame odometerAdding the height difference of the position and the posture of the milestones at the k-1 moment and the k moment into a map queue, and performing intervention after finding that the height difference of the milestones at the k moment exceeds a preset value, and enabling the height of the milestones at the k moment to be +.>Consists of a current key frame height and a historical key frame height:
where η=a+b+c+d, a, b, c, d is a weight parameter.
6. The method according to claim 1, wherein the extracting loop detection factors in the Scan Context loop detection method based on radius search comprises:
after receiving a frame of complete scanning data from a radar, checking whether the frame is a key frame or not, inquiring whether a history key frame exists in the radius of the current key frame, if so, taking the history key frame as a candidate history key frame, and extracting a radius search loop factor in the process;
and developing the 3D point cloud of each frame into a 2D image, then establishing a Scan Context, carrying out two-stage hierarchical search by introducing a rotation invariance descriptor Ring Key, constructing a KD tree for scanning the index Key, quickly finding candidate historical Key frames to match with the current Key frames by using a nearest neighbor search algorithm, calculating the Ring Key similarity, finding the most similar candidate historical Key frames, and extracting a Scan Context loop factor of the process.
7. The method according to claim 1, wherein the correcting the odometer pose and the point cloud map by performing graph optimization on the factor graph to obtain global lidar odometer 6-DOF pose information and a three-dimensional point cloud map comprises:
constructing constraint items for optimizing the pose and the track, iterating the pose transformation of the current key frame and the closed-loop key frame through ICP and registering an incremental map; and the attitude estimation is expressed as a maximum posterior problem, then the problem is converted into a nonlinear least square problem based on Gaussian noise covariance, and partial nonlinear factors such as the track and the pose are smoothed and linearized by using an ISAM2 on the basis of a Bayesian tree, so that the final accurate global track and the pose are obtained.
CN202310928868.7A 2023-07-26 2023-07-26 Laser SLAM method for inhibiting height drift of odometer Pending CN116878542A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310928868.7A CN116878542A (en) 2023-07-26 2023-07-26 Laser SLAM method for inhibiting height drift of odometer

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310928868.7A CN116878542A (en) 2023-07-26 2023-07-26 Laser SLAM method for inhibiting height drift of odometer

Publications (1)

Publication Number Publication Date
CN116878542A true CN116878542A (en) 2023-10-13

Family

ID=88264296

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310928868.7A Pending CN116878542A (en) 2023-07-26 2023-07-26 Laser SLAM method for inhibiting height drift of odometer

Country Status (1)

Country Link
CN (1) CN116878542A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117808882A (en) * 2024-02-29 2024-04-02 上海几何伙伴智能驾驶有限公司 SLAM drift detection and compensation method based on multi-sensor fusion in degradation scene

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117808882A (en) * 2024-02-29 2024-04-02 上海几何伙伴智能驾驶有限公司 SLAM drift detection and compensation method based on multi-sensor fusion in degradation scene
CN117808882B (en) * 2024-02-29 2024-05-17 上海几何伙伴智能驾驶有限公司 SLAM drift detection and compensation method based on multi-sensor fusion in degradation scene

Similar Documents

Publication Publication Date Title
CN108802785B (en) Vehicle self-positioning method based on high-precision vector map and monocular vision sensor
CN113436260B (en) Mobile robot pose estimation method and system based on multi-sensor tight coupling
CN110796728B (en) Non-cooperative spacecraft three-dimensional reconstruction method based on scanning laser radar
CN106595659A (en) Map merging method of unmanned aerial vehicle visual SLAM under city complex environment
CN111862673B (en) Parking lot vehicle self-positioning and map construction method based on top view
CN111337943B (en) Mobile robot positioning method based on visual guidance laser repositioning
CN113485441A (en) Distribution network inspection method combining unmanned aerial vehicle high-precision positioning and visual tracking technology
CN113506318B (en) Three-dimensional target perception method under vehicle-mounted edge scene
CN115407357B (en) Low-harness laser radar-IMU-RTK positioning mapping algorithm based on large scene
CN115372989A (en) Laser radar-based long-distance real-time positioning system and method for cross-country automatic trolley
CN111998862B (en) BNN-based dense binocular SLAM method
CN116878542A (en) Laser SLAM method for inhibiting height drift of odometer
CN111932614B (en) Laser radar instant positioning and mapping method based on clustering center characteristics
CN111829514A (en) Road surface working condition pre-aiming method suitable for vehicle chassis integrated control
CN114648584B (en) Robustness control method and system for multi-source fusion positioning
CN114777775A (en) Multi-sensor fusion positioning method and system
CN115908539A (en) Target volume automatic measurement method and device and storage medium
CN110288620B (en) Image matching method based on line segment geometric features and aircraft navigation method
CN108921896B (en) Downward vision compass integrating dotted line characteristics
CN117029870A (en) Laser odometer based on road surface point cloud
CN116681733B (en) Near-distance real-time pose tracking method for space non-cooperative target
CN114897967B (en) Material form identification method for autonomous operation of excavating equipment
CN114234967B (en) Six-foot robot positioning method based on multi-sensor fusion
CN116468786A (en) Semantic SLAM method based on point-line combination and oriented to dynamic environment
CN102175227A (en) Quick positioning method for probe car in satellite image

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination