CN112180382B - Constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system - Google Patents
Constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system Download PDFInfo
- Publication number
- CN112180382B CN112180382B CN202011040245.9A CN202011040245A CN112180382B CN 112180382 B CN112180382 B CN 112180382B CN 202011040245 A CN202011040245 A CN 202011040245A CN 112180382 B CN112180382 B CN 112180382B
- Authority
- CN
- China
- Prior art keywords
- map
- point cloud
- pose
- sub
- current
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 58
- 238000005457 optimization Methods 0.000 claims abstract description 47
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 230000003044 adaptive effect Effects 0.000 claims description 17
- 230000006870 function Effects 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 6
- 238000002360 preparation method Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
Classifications
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a constant-speed model-based self-adaptive 3D-LSLAM positioning method, which comprises the following steps: receiving laser point cloud information, converting point cloud coordinates from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining an optimized point cloud pose through an LM method; updating the current point cloud information into the current posterior sub-map through Bejess filtering based on the optimized point cloud pose; combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle, and performing nonlinear optimization on the pose of the posterior sub-map; and converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map. The invention can output the pose of the vehicle according to any frequency, and has strong robustness to the environment; on the premise of ensuring the precision, the calculated amount is reduced, and the real-time positioning output of the vehicle is realized; no accumulated error and high positioning precision.
Description
Technical Field
The invention relates to the field of real-time positioning of vehicles, and belongs to a constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system.
Background
The current vehicle pose acquisition is mainly achieved by the following two methods: according to the first method, based on a 2D-lslam prior map, particle filtering is carried out by adopting 2D laser point cloud information to obtain the posterior pose of the vehicle. And in the second method, registering is carried out by adopting the 3D laser point cloud information and a global 3D-lslam prior map to obtain the vehicle pose.
However, both methods for acquiring the pose of the vehicle have some problems, for example, the first method is very dependent on an odometer, 2D laser point cloud features are few, robustness is poor, the method is not suitable for outdoor open scenes, and the improvement of accuracy is at the cost of consuming calculation amount; the second method has large calculation amount, is difficult to ensure real-time performance, is easy to sink into local optimum and has accumulated errors.
Disclosure of Invention
The invention aims to provide a constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system, which can output vehicle pose according to any frequency and have strong robustness to the environment; on the premise of ensuring the precision, the calculated amount is reduced, and the real-time positioning output of the vehicle is realized; no accumulated error and high positioning precision.
In order to achieve the above purpose, the present invention provides the following technical solutions:
a constant-speed model-based adaptive 3D-LSLAM positioning method, the positioning method comprising:
s1, initializing a receiving interface for receiving IMU information and laser point cloud information; initializing the pose of the vehicle; initializing a posterior sub-map; the laser point cloud information comprises geometric positions and intensity information of obstacles in front of a vehicle perceived by a laser radar;
s2, updating the vehicle posture in real time according to the received IMU information;
s3, receiving laser point cloud information, converting point cloud coordinates from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining an optimized point cloud pose through an LM method; updating the current point cloud information into the current posterior sub-map through Bejess filtering based on the optimized point cloud pose;
s4, calculating and updating the speed and the angular speed of the vehicle according to the pose of the current point cloud and the pose of the point cloud after the optimization of the previous frame;
s5, combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle, and performing nonlinear optimization on the pose of the posterior sub-map; the nonlinear constraint is constructed by matching the real-time point cloud with the priori sub-map;
s6, converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map;
and S7, outputting the pose of the vehicle under the global coordinate system at the current moment by adopting a constant-speed model.
Further, in step S1, the process of initializing the vehicle pose includes:
and initializing the pose of the vehicle according to the received IMU information of the first frame.
Further, in step S1, the process of initializing the posterior sub-map includes:
and converting the point cloud coordinates from a laser radar coordinate system to a vehicle body coordinate system by combining the received first frame of laser point cloud information, and initializing a posterior sub-map.
Further, in step S3, the process of updating the current point cloud information into the current posterior sub-map through the bayesian filtering based on the optimized point cloud pose includes the following steps:
updating the current point cloud information to the current posterior sub-map through Bejess filtering based on the optimized point cloud pose, and initializing the next preparation posterior sub-map by using the current point cloud if the number of frames of the current posterior sub-map reaches half of the set value of the number of frames of the constructed sub-map.
Further, the positioning method further comprises:
steps S5 to S7 are processed based on separate thread queues.
Further, in step S5, the process of performing nonlinear optimization on the pose of the posterior sub-map by combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle includes the following steps:
s51, converting the pose of the point cloud in the current posterior sub-map into the pose of the global prior map, traversing the prior sub-map, and searching for the prior sub-map set with the geometric distance of the point cloud within a preset distance threshold;
s52, selecting one of the prior sub-maps in the prior sub-map set, and obtaining the real-time matching frequency of the point cloud and the prior sub-map by adopting a self-adaptive law according to the updated speed and angular speed of the vehicle;
s53, if the real-time matching frequency reaches the preset matching frequency, matching the current point cloud with the prior sub-map by adopting a multi-resolution matching method, turning to a step S54, and if the real-time matching frequency does not reach the preset matching frequency, directly turning to a step S56;
s54, if the matching result reaches a preset matching degree threshold, adding the position and the pose of the point cloud and the constraint of the position and the pose of the prior sub-map into a nonlinear optimization function for optimizing the position and the pose of the posterior sub-map, turning to step S55, and if the matching result does not reach the preset matching degree threshold, directly turning to step S56;
s55, if the number of the point cloud frames reaches the optimization condition, carrying out nonlinear optimization on the pose of the posterior sub-map, and turning to the step S56, and if the number of the point cloud frames does not reach the optimization condition, directly turning to the step S55;
s56, judging whether the prior sub map set is traversed, if so, turning to a step S57, otherwise, turning to a step S52;
s57, based on the optimized current posterior sub-map, converting the pose of the current point cloud into a global prior map coordinate system.
Based on the foregoing positioning method, the present invention also provides an adaptive 3D-LSLAM positioning device based on a constant speed model, where the positioning device includes:
a module for initializing a receiving interface for receiving IMU information and laser point cloud information; a module for initializing a vehicle pose; a module for initializing a posterior sub-map; the laser point cloud information comprises geometric positions and intensity information of obstacles in front of a vehicle perceived by a laser radar;
a module for updating the vehicle attitude in real time according to the received IMU information;
the module is used for receiving laser point cloud information, converting the point cloud coordinate from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining the optimized point cloud pose through an LM method; the module is used for updating the current point cloud information into the current posterior sub-map through Bejes filtering based on the optimized point cloud pose;
the module is used for calculating and updating the speed and the angular speed of the vehicle according to the pose of the current point cloud and the pose of the point cloud after the optimization of the previous frame;
the module is used for carrying out nonlinear optimization on the pose of the posterior sub-map by combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle; the method comprises the steps of adopting real-time point cloud and priori sub-map matching to construct constraint of a nonlinear optimization function for nonlinear optimization of the pose of the posterior sub-map;
the module is used for converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map;
and the module is used for outputting the pose of the vehicle under the global coordinate system at the current moment by adopting the constant-speed model.
The invention also relates to a self-adaptive 3D-LSLAM positioning system based on a constant speed model, which comprises a processor and a memory;
the memory is used for storing a computer program, and the processor is used for executing the computer program in the memory to realize the adaptive 3D-LSLAM positioning method based on the constant speed model.
The invention has the beneficial effects that:
(1) Through the constant speed model, the system can output the vehicle pose according to any frequency.
(2) And carrying out nonlinear optimization through real-time point cloud and priori sub-map matching construction constraint, and eliminating accumulated errors.
(3) The nonlinear optimization process adjusts the pose of the posterior sub-map instead of the pose of each frame of point cloud, so that the calculated amount is greatly reduced.
(4) The frequency of matching the point cloud with the prior sub-map is adaptively adjusted according to the real-time speed and angular speed information of the vehicle, so that the calculated amount is reduced while the accuracy is ensured under the condition of different speeds and angular speeds of the vehicle.
The foregoing description is only an overview of the present invention, and is intended to provide a better understanding of the present invention, as it is embodied in the following description, with reference to the preferred embodiments of the present invention and the accompanying drawings.
Drawings
Fig. 1 is a flow chart of the adaptive 3D-LSLAM positioning method based on the constant speed model of the present invention.
Fig. 2 is a flowchart of a positioning method according to a first embodiment of the present invention.
Fig. 3 is a schematic illustration of a posterior sub-map optimization flow according to a first embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made apparent and fully in view of the accompanying drawings, in which some, but not all embodiments of the invention are shown. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
In the description of the present invention, it should be noted that the directions or positional relationships indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. are based on the directions or positional relationships shown in the drawings, are merely for convenience of describing the present invention and simplifying the description, and do not indicate or imply that the devices or elements referred to must have a specific orientation, be configured and operated in a specific orientation, and thus should not be construed as limiting the present invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
In the description of the present invention, it should be noted that, unless explicitly specified and limited otherwise, the terms "mounted," "connected," and "connected" are to be construed broadly, and may be either fixedly connected, detachably connected, or integrally connected, for example; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present invention will be understood in specific cases by those of ordinary skill in the art.
In addition, the technical features of the different embodiments of the present invention described below may be combined with each other as long as they do not collide with each other.
First, the technical terms related to the present invention will be described:
SLAM: simultaneous Localization And Mapping, real-time mapping and positioning.
3D-LSLAM: SLAM technology based on multi-line lidar sensors.
2D-LSLAM: SLAM technology based on single-line lidar sensors.
IMU: inertial measurement unit, inertial measurement unit.
And (3) point cloud: the laser radar's measurement outputs data (laser perceived obstacle geometry and intensity information).
Pose: the position and attitude of the vehicle within a certain coordinate system.
Sub map: a local map composed of a certain number of frames of laser point cloud information.
A priori map: an off-line map is made by LSLAM technology before starting positioning, and the global prior map described by the technology is composed of sub-maps.
Posterior map: real-time maps made by LSLAM technology during the positioning process.
Constant speed model: before the optimization of the point cloud matching of each frame is completed, the speed of the vehicle at the current moment is defaulted to be the same as that at the last moment.
LM method: an algorithm for extremum of nonlinear function.
Detailed description of the preferred embodiments
Referring to fig. 1, the present invention refers to a constant-speed model-based adaptive 3D-LSLAM positioning method, which includes:
s1, initializing a receiving interface for receiving IMU information and laser point cloud information; initializing the pose of the vehicle; initializing a posterior sub-map; the laser point cloud information includes geometric positions of obstacles in front of and around the vehicle perceived by the laser radar.
And S2, updating the vehicle posture in real time according to the received IMU information.
S3, receiving laser point cloud information, converting point cloud coordinates from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining an optimized point cloud pose through an LM method; updating the current point cloud information into the current posterior sub-map through Bejess filtering based on the optimized point cloud pose;
and S4, calculating and updating the speed and the angular speed of the vehicle according to the pose of the current point cloud and the pose of the point cloud after the optimization of the previous frame of point cloud.
And S5, combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle, and performing nonlinear optimization on the pose of the posterior sub-map. And the constraint of a nonlinear optimization function for nonlinear optimization of the pose of the posterior sub-map is constructed by adopting real-time point cloud and priori sub-map matching.
And S6, converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map.
And S7, outputting the pose of the vehicle under the global coordinate system at the current moment by adopting a constant-speed model.
The following describes the technical scheme of the adaptive 3D-LSLAM positioning method based on the constant speed model with reference to fig. 2 and 3.
The technical scheme of the invention comprises the following steps:
and step 1, initializing a receiving interface for receiving IMU information and laser point cloud information.
And 2, initializing the pose of the vehicle according to the IMU output information when the first frame of IMU information is received.
And 3, continuously receiving the IMU information, and updating the vehicle posture according to the IMU output information.
And 4, converting the point cloud coordinate from a laser radar coordinate system to a vehicle body coordinate system when the first frame of laser point cloud information is received, and initializing a posterior sub-map.
And 5, continuously receiving laser point cloud information, converting the point cloud coordinates from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining the optimized point cloud pose through an LM method.
And 6, updating the current point cloud information to the current posterior sub-map through Bejes filtering based on the optimized point cloud pose, initializing the next preparation posterior sub-map by using the current point cloud if the number of frames of the current posterior sub-map reaches half of the set value of the number of frames of the constructed sub-map, and entering the step 7, otherwise, directly entering the step 7.
And 7, converting the pose of the point cloud in the current sub-map into the pose of the global prior map, traversing the prior sub-map, and searching the prior sub-map set with the geometric distance from the current point cloud within a certain threshold.
And 8, calculating and updating the speed and the angular speed of the vehicle in the period of time according to the current pose and the pose after the last optimization, and obtaining the matching frequency of the point cloud and the prior sub-map through a self-adaptive law. The frequency of matching the point cloud with the prior sub-map is adaptively adjusted according to the real-time speed and angular speed information of the vehicle, so that the calculated amount is reduced while the accuracy is ensured under the condition of different speeds and angular speeds of the vehicle.
And 9, if the matching frequency is met, matching the current point cloud with the prior sub-map by adopting a multi-resolution matching method, and if the matching result reaches a certain threshold value, adding the position and the pose of the point cloud and the constraint of the position and the prior sub-map into a nonlinear optimization function for optimizing the posterior sub-map. And the constraint between the real-time point cloud and the prior map is constructed through matching of the point cloud and the prior sub-map, and then the accumulated error is eliminated through nonlinear optimization.
And step 10, if the number of the point cloud frames reaches the optimization condition, nonlinear optimization of the posterior sub-map is carried out, and in the optimization process, only the pose of the posterior sub-map is adjusted. The pose of the posterior sub-map is adjusted in the nonlinear optimization process instead of the pose of each frame of point cloud, so that the calculated amount is greatly reduced.
And step 11, converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map.
And step 12, outputting the pose of the vehicle under the global coordinate system at the current moment through a constant-speed model. Through the constant speed model, the system can output the vehicle pose according to any frequency.
Preferably, steps 7-12 are processed in separate thread queues in order to optimize the calculation process.
Second embodiment
Based on the foregoing positioning method, the present invention also provides an adaptive 3D-LSLAM positioning device based on a constant speed model, where the positioning device includes:
(1) A module for initializing a receiving interface for receiving IMU information and laser point cloud information; a module for initializing a vehicle pose; a module for initializing a posterior sub-map; the laser point cloud information comprises geometric position and intensity information of obstacles in front of the vehicle perceived by a laser radar.
(2) And the module is used for updating the vehicle posture in real time according to the received IMU information.
(3) The module is used for receiving laser point cloud information, converting the point cloud coordinate from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining the optimized point cloud pose through an LM method; and updating the current point cloud information into the current posterior sub-map through Bejes filtering based on the optimized point cloud pose.
(4) And the module is used for calculating and updating the speed and the angular speed of the vehicle according to the pose of the current point cloud and the pose of the point cloud after the optimization of the previous frame of point cloud.
(5) And the module is used for carrying out nonlinear optimization on the pose of the posterior sub-map by combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle. And the nonlinear constraint is constructed by adopting real-time point cloud and priori sub-map matching.
(6) And the module is used for converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map.
(7) And the module is used for outputting the pose of the vehicle under the global coordinate system at the current moment by adopting the constant-speed model.
The invention also refers to an adaptive 3D-LSLAM positioning system based on a constant speed model, said positioning system comprising a processor and a memory.
The memory is used for storing a computer program, and the processor is used for executing the computer program in the memory to realize the adaptive 3D-LSLAM positioning method based on the constant speed model.
The technical features of the above-described embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above-described embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The above examples illustrate only a few embodiments of the invention, which are described in detail and are not to be construed as limiting the scope of the invention. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the invention, which are all within the scope of the invention. Accordingly, the scope of protection of the present invention is to be determined by the appended claims.
Claims (8)
1. The self-adaptive 3D-LSLAM positioning method based on the constant-speed model is characterized by comprising the following steps of:
s1, initializing a receiving interface for receiving IMU information and laser point cloud information; initializing the pose of the vehicle; initializing a posterior sub-map; the laser point cloud information comprises geometric positions and intensity information of obstacles in front of a vehicle perceived by a laser radar;
s2, updating the vehicle posture in real time according to the received IMU information;
s3, receiving laser point cloud information, converting point cloud coordinates from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining an optimized point cloud pose through an LM method; updating the current point cloud information into the current posterior sub-map through Bejess filtering based on the optimized point cloud pose;
s4, calculating and updating the speed and the angular speed of the vehicle according to the pose of the current point cloud and the pose of the point cloud after the optimization of the previous frame;
s5, combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle, and performing nonlinear optimization on the pose of the posterior sub-map; the nonlinear constraint is constructed by matching the real-time point cloud with the priori sub-map;
s6, converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map;
and S7, outputting the pose of the vehicle under the global coordinate system at the current moment by adopting a constant-speed model.
2. The method for adaptive 3D-LSLAM positioning based on a constant speed model according to claim 1, wherein in step S1, the process of initializing the vehicle pose comprises:
and initializing the pose of the vehicle according to the received IMU information of the first frame.
3. The method for adaptive 3D-LSLAM positioning based on a constant velocity model according to claim 1, wherein in step S1, the process of initializing a posterior sub-map comprises:
and converting the point cloud coordinates from a laser radar coordinate system to a vehicle body coordinate system by combining the received first frame of laser point cloud information, and initializing a posterior sub-map.
4. The method for positioning adaptive 3D-LSLAM based on a constant velocity model according to claim 1, wherein in step S3, the process of updating the current point cloud information into the current posterior sub-map by bayesian filtering based on the optimized point cloud pose comprises the following steps:
updating the current point cloud information to the current posterior sub-map through Bejess filtering based on the optimized point cloud pose, and initializing the next preparation posterior sub-map by using the current point cloud if the number of frames of the current posterior sub-map reaches half of the set value of the number of frames of the constructed sub-map.
5. The adaptive 3D-LSLAM positioning method based on a constant speed model according to claim 1, wherein the positioning method further comprises:
steps S5 to S7 are processed based on separate thread queues.
6. The method for positioning a constant-speed model based adaptive 3D-LSLAM according to claim 1, wherein in step S5, the process of non-linearly optimizing the pose of the posterior sub-map by combining the updated current posterior sub-map, the global prior map and the calculated velocity and angular velocity of the vehicle comprises the following steps:
s51, converting the pose of the point cloud in the current posterior sub-map into the pose of the global prior map, traversing the prior sub-map, and searching for the prior sub-map set with the geometric distance of the point cloud within a preset distance threshold;
s52, selecting one of the prior sub-maps in the prior sub-map set, and obtaining the real-time matching frequency of the point cloud and the prior sub-map by adopting a self-adaptive law according to the updated speed and angular speed of the vehicle;
s53, if the real-time matching frequency reaches the preset matching frequency, matching the current point cloud with the prior sub-map by adopting a multi-resolution matching method, turning to a step S54, and if the real-time matching frequency does not reach the preset matching frequency, directly turning to a step S56;
s54, if the matching result reaches a preset matching degree threshold, adding the position and the pose of the point cloud and the constraint of the position and the pose of the prior sub-map into a nonlinear optimization function for optimizing the position and the pose of the posterior sub-map, turning to step S55, and if the matching result does not reach the preset matching degree threshold, directly turning to step S56;
s55, if the number of the point cloud frames reaches the optimization condition, carrying out nonlinear optimization on the pose of the posterior sub-map, and turning to the step S56, and if the number of the point cloud frames does not reach the optimization condition, directly turning to the step S55;
s56, judging whether the prior sub map set is traversed, if so, turning to a step S57, otherwise, turning to a step S52;
s57, based on the optimized current posterior sub-map, converting the pose of the current point cloud into a global prior map coordinate system.
7. An adaptive 3D-LSLAM positioning device based on a constant speed model, the positioning device comprising:
a module for initializing a receiving interface for receiving IMU information and laser point cloud information; a module for initializing a vehicle pose; a module for initializing a posterior sub-map; the laser point cloud information comprises geometric positions and intensity information of obstacles in front of a vehicle perceived by a laser radar;
a module for updating the vehicle attitude in real time according to the received IMU information;
the module is used for receiving laser point cloud information, converting the point cloud coordinate from a laser radar coordinate system to a current posterior sub-map coordinate system, constructing a nonlinear optimization function of the point cloud and the current posterior sub-map, and obtaining the optimized point cloud pose through an LM method; the module is used for updating the current point cloud information into the current posterior sub-map through Bejes filtering based on the optimized point cloud pose;
the module is used for calculating and updating the speed and the angular speed of the vehicle according to the pose of the current point cloud and the pose of the point cloud after the optimization of the previous frame;
the module is used for carrying out nonlinear optimization on the pose of the posterior sub-map by combining the updated current posterior sub-map, the global prior map and the calculated speed and angular speed of the vehicle; the method comprises the steps of adopting real-time point cloud and priori sub-map matching to construct constraint of a nonlinear optimization function for nonlinear optimization of the pose of the posterior sub-map;
the module is used for converting the pose of the current point cloud into a global priori map coordinate system based on the optimized current posterior sub-map;
and the module is used for outputting the pose of the vehicle under the global coordinate system at the current moment by adopting the constant-speed model.
8. An adaptive 3D-LSLAM positioning system based on a constant velocity model, the positioning system comprising a processor and a memory;
the memory is for storing a computer program and the processor is for executing the computer program in the memory for implementing the constant velocity model based adaptive 3D-LSLAM positioning method as claimed in any one of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011040245.9A CN112180382B (en) | 2020-09-28 | 2020-09-28 | Constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011040245.9A CN112180382B (en) | 2020-09-28 | 2020-09-28 | Constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112180382A CN112180382A (en) | 2021-01-05 |
| CN112180382B true CN112180382B (en) | 2024-03-08 |
Family
ID=73945211
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011040245.9A Active CN112180382B (en) | 2020-09-28 | 2020-09-28 | Constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112180382B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112414403B (en) * | 2021-01-25 | 2021-04-16 | 湖南北斗微芯数据科技有限公司 | Robot positioning and attitude determining method, equipment and storage medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107015657A (en) * | 2017-04-14 | 2017-08-04 | 深圳市唯特视科技有限公司 | A kind of method that moveable robot movement space is limited based on interactive mode |
| CN108732584A (en) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | Method and apparatus for updating map |
| CN109341705A (en) * | 2018-10-16 | 2019-02-15 | 北京工业大学 | Intelligent detecting robot simultaneous localization and mapping system |
| AU2018282435B1 (en) * | 2018-11-09 | 2020-02-06 | Beijing Didi Infinity Technology And Development Co., Ltd. | Vehicle positioning system using LiDAR |
| WO2020048623A1 (en) * | 2018-09-07 | 2020-03-12 | Huawei Technologies Co., Ltd. | Estimation of a pose of a robot |
-
2020
- 2020-09-28 CN CN202011040245.9A patent/CN112180382B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107015657A (en) * | 2017-04-14 | 2017-08-04 | 深圳市唯特视科技有限公司 | A kind of method that moveable robot movement space is limited based on interactive mode |
| CN108732584A (en) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | Method and apparatus for updating map |
| WO2020048623A1 (en) * | 2018-09-07 | 2020-03-12 | Huawei Technologies Co., Ltd. | Estimation of a pose of a robot |
| CN109341705A (en) * | 2018-10-16 | 2019-02-15 | 北京工业大学 | Intelligent detecting robot simultaneous localization and mapping system |
| AU2018282435B1 (en) * | 2018-11-09 | 2020-02-06 | Beijing Didi Infinity Technology And Development Co., Ltd. | Vehicle positioning system using LiDAR |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112180382A (en) | 2021-01-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP7112993B2 (en) | Laser Radar Internal Parameter Accuracy Verification Method and Its Apparatus, Equipment and Medium | |
| CN113269837B (en) | Positioning navigation method suitable for complex three-dimensional environment | |
| CN111060923B (en) | Multi-laser-radar automobile driving obstacle detection method and system | |
| CN111521195B (en) | Intelligent robot | |
| CN114842438A (en) | Terrain detection method, system and readable storage medium for autonomous driving vehicle | |
| CN109849930B (en) | Method and device for calculating speed of adjacent vehicle of automatic driving automobile | |
| CN113903011B (en) | Semantic map construction and positioning method suitable for indoor parking lot | |
| CN110967038B (en) | Vehicle positioning method, vehicle positioning device and vehicle | |
| US11836861B2 (en) | Correcting or expanding an existing high-definition map | |
| CN109949364B (en) | Vehicle attitude detection precision optimization method based on road side monocular camera | |
| CN115077541A (en) | Positioning method and device for automatic driving vehicle, electronic equipment and storage medium | |
| CN112180382B (en) | Constant-speed model-based self-adaptive 3D-LSLAM positioning method, device and system | |
| WO2022078342A1 (en) | Dynamic occupancy grid estimation method and apparatus | |
| CN115015956A (en) | Laser and vision SLAM system of indoor unmanned vehicle | |
| CN113734176A (en) | Environment sensing system and method for intelligent driving vehicle, vehicle and storage medium | |
| CN117036447A (en) | Indoor scene dense three-dimensional reconstruction method and device based on multi-sensor fusion | |
| CN117115271A (en) | Binocular camera external parameter self-calibration method and system in unmanned aerial vehicle flight process | |
| CN112798020B (en) | System and method for evaluating positioning accuracy of intelligent automobile | |
| CN115307646A (en) | Multi-sensor fusion robot positioning method, system and device | |
| CN113034538B (en) | Pose tracking method and device of visual inertial navigation equipment and visual inertial navigation equipment | |
| CN113687336A (en) | Radar calibration method and device, electronic equipment and medium | |
| CN113390425B (en) | Map data processing method, device, equipment and storage medium | |
| CN112985417A (en) | Pose correction method for particle filter positioning of mobile robot and mobile robot | |
| CN117611762B (en) | Multi-level map construction method, system and electronic equipment | |
| CN116645400B (en) | Vision and inertia mixed pose tracking method, system, helmet and storage medium |
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 | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 215123 g2-1901 / 1902 / 2002, No. 88, Jinjihu Avenue, Suzhou Industrial Park, Suzhou City, Jiangsu Province Applicant after: Zhixing Automotive Technology (Suzhou) Co.,Ltd. Address before: 215123 g2-1901 / 1902 / 2002, No. 88, Jinjihu Avenue, Suzhou Industrial Park, Suzhou City, Jiangsu Province Applicant before: IMOTION AUTOMOTIVE TECHNOLOGY (SUZHOU) Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |