CN116678427A - Dynamic positioning method and device based on urban canyon sparse feature map constraint - Google Patents
Dynamic positioning method and device based on urban canyon sparse feature map constraint Download PDFInfo
- Publication number
- CN116678427A CN116678427A CN202310751141.6A CN202310751141A CN116678427A CN 116678427 A CN116678427 A CN 116678427A CN 202310751141 A CN202310751141 A CN 202310751141A CN 116678427 A CN116678427 A CN 116678427A
- Authority
- CN
- China
- Prior art keywords
- map
- mileage
- point cloud
- feature
- node
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000005457 optimization Methods 0.000 claims abstract description 5
- 238000006073 displacement reaction Methods 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 238000000605 extraction Methods 0.000 claims 1
- 238000004891 communication Methods 0.000 abstract description 5
- 238000005259 measurement Methods 0.000 abstract description 4
- 230000005540 biological transmission Effects 0.000 abstract description 3
- 230000011218 segmentation Effects 0.000 abstract description 2
- 238000005516 engineering process Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 2
- 238000012423 maintenance Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Classifications
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- 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
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
The invention discloses a dynamic positioning method based on urban canyon sparse feature map constraint, which is characterized by firstly designing an urban canyon sparse feature point cloud map structure based on a road mileage node as a main line by referring to the thought of conducting wire layout measurement in the field of geodetic measurement; then, based on the established sparse feature map, estimating pose parameters of the unmanned vehicle in real time by utilizing a 2D laser radar matching algorithm; and finally, carrying out smooth optimization on the pose parameter estimation value by combining a factor graph algorithm. Furthermore, the invention also discloses a dynamic positioning device based on urban canyon sparse feature map constraint, which corresponds to the method. The method improves the convenience and flexibility of the high-precision point cloud map in the aspects of segmentation, splicing, retrieval and communication transmission, and can basically realize the unmanned vehicle dynamic positioning precision within 0.5m especially in complex urban canyon scenes.
Description
Technical Field
The invention belongs to the technical field of autonomous positioning and navigation of unmanned vehicles, and particularly relates to a dynamic positioning method and device based on urban canyon sparse feature map constraint.
Background
Currently, the emerging high-technology industry using ubiquitous positioning service as traction is vigorous, and various mobile unmanned intelligent devices such as unmanned vehicles, unmanned planes and unmanned ships are used for providing continuous, stable and reliable positioning requirements for a navigation system. Particularly for campus unmanned logistics vehicles and intelligent unmanned vehicles, ensuring accurate and reliable positioning of the campus unmanned logistics vehicles and the intelligent unmanned vehicles under the environment of global navigation satellite rejection such as urban canyons is still one of the technical problems challenging in the field.
Satellite radio communication technology that can achieve high-precision dynamic positioning in an open environment is severely limited in urban canyons due to the shielding of high-rise buildings. Although positioning methods such as pseudolites, UWB and the like exist to alleviate the building shielding problem, the multipath signal interference problem caused by the reflection on the building surface is still difficult to solve. With the vigorous development of synchronous positioning and mapping technologies, a positioning method based on a high-definition map is considered as one of the most potential technologies for solving the problem of the last kilometer of satellite positioning.
At present, the related scientific research institutions develop intelligent driving automobile high-precision map standards on a tight drum. The most remarkable characteristics of the method are high-precision three-dimensional coordinates in centimeter level and very rich map elements, and particularly, the method is used for carrying out detailed semantic annotation on urban road traffic marks, traffic signs, barriers and the like. However, this is a large, complex and expensive comprehensive project for the production, maintenance and updating of very large urban high precision maps. In the aspect of matching and positioning by using a high-precision map, common geometric matching, topological matching, intelligent learning matching, filtering estimation and the like exist, wherein the matching precision is closely related to the initial outline pose precision, and particularly, the geometric and topological matching method is sensitive to abnormal gross errors. In addition, because of the rich and dense features of the high-definition map elements, the map retrieval and matching process occupies a large amount of computing resources, so that the complex matching and positioning algorithm has low practicability and is usually designed to run in an off-line mode at the rear end of the algorithm. It should be noted that in the process of real-time matching and positioning of the local map scanned by the unmanned vehicle and the whole high-precision map, the dynamic objects of pedestrians and vehicles not only can shade the scanning view of the laser radar sensor, but also can form wrong matching characteristics, so that the correct matching information in the matching estimation model is too little, the wrong matching information is too much, and the accuracy and the robustness of the estimation parameters are greatly reduced.
In general, in the high-precision positioning technology based on a high-precision map in a complex urban environment, three technical problems mainly exist: firstly, because of the richness and complexity of a high-precision map, a dense point cloud map can cause the map data to increase the application difficulty in the aspects of communication transmission, matching retrieval, maintenance and updating; secondly, a matching positioning algorithm with excessively complex calculation usually needs to run off-line at the back end, and real-time and efficient calculation requirements are difficult to meet; thirdly, the initial outline position precision and the disturbance of the dynamic object characteristics can greatly reduce the stability and reliability of the matching positioning. Therefore, the current high-precision map matching and positioning technology also needs a higher technical application threshold, and is difficult to meet the urban positioning service requirements of low cost and high precision for serving the public, and a more efficient, robust and reliable practical map matching and positioning method is urgently needed.
Disclosure of Invention
In order to solve the problems, the invention discloses a dynamic positioning method and a device based on urban canyon sparse feature map constraint; designing an urban canyon sparse characteristic point cloud map structure based on a road mileage node as a main line by referring to the thought of conducting wire layout measurement in the field of geodetic survey; then, based on the established sparse feature map, estimating pose parameters of the unmanned vehicle in real time by utilizing an improved laser radar mileage calculation method; and finally, the pose parameter estimation is smoothly optimized by combining a factor graph algorithm, so that the convenience and the flexibility of the high-precision point cloud map in the aspects of segmentation, splicing, retrieval and communication transmission are improved, and the dynamic high-precision positioning of the unmanned vehicle in the complex urban canyon scene is realized.
In order to achieve the above purpose, the technical scheme of the invention is as follows:
a dynamic positioning method based on urban canyon sparse feature map constraint comprises the following steps:
s1, designing a sparse feature map structure according to the shape of an urban road, selecting mileage nodes at fixed mileage intervals to form a map structure main line, and dividing the prior point cloud map with the extracted features by taking the nodes as units;
s2, removing the prior point cloud map near-ground point cloud on each node by using the elevation information of the mileage nodes to obtain a prior high-precision sparse feature point cloud map which is composed of each node and only comprises corner features and face features (non-ground);
s3, estimating real-time pose parameter estimation information of the unmanned vehicle by using a 2D laser radar matching algorithm based on the sparse feature point cloud map;
s4, constructing constraint factor nodes by combining a factor graph algorithm to smoothly optimize the real-time pose estimation value, so as to obtain dynamic navigation positioning information of the unmanned vehicle in the urban canyon road driving process;
as a preferred solution, the sparse feature map architecture design is obtained according to the following procedure:
firstly, interpolation is carried out according to the urban road center line so as to obtain the three-dimensional coordinates of each mileage node. The three-dimensional coordinates are obtained as follows:
in the formula e i+1 、n i+1 And u i+1 Three-dimensional coordinates of (i+1) th mileage node, p e 、p n And p u D is a fixed mileage interval, s is an interpolation scale factor. Therefore, all mileage nodes of the urban road can be extracted to be connected into a main line of the map structure.
Then, the prior point cloud map after the features are extracted is divided by taking mileage nodes as units, and the prior point cloud map is obtained according to the following process:
in the formula e i 、n i And u i Three-dimensional coordinates, p, of the ith mileage node respectively x 、p y And p z The three-dimensional coordinates of feature point clouds such as faces, corner points and the like in the prior map are obtained, psi is the azimuth angle of the mileage node, and d is a fixed mileage interval. Thus, the point cloud satisfying all the conditions thereof can be divided by using the formula (2), thereby composing a high-precision point cloud map belonging to the mileage point.
Preferably, the fixed mileage interval preset value is 10m.
As a preferable scheme, the prior point cloud map near-ground point cloud on each node is removed by using the elevation information of the mileage nodes, and the method is obtained according to the following steps:
wherein u is i Elevation of ith mileage node, p z ' is the elevation of the characteristic point cloud above the mileage node, p z "is the elevation of the corner feature point cloud on the mileage node. Therefore, the prior point cloud map on the node can be removed from the point cloud on the near ground by utilizing the formula (3), so that the prior high-precision sparse feature point cloud map only comprising the elevation features and the corner features is obtained.
As a preferable scheme, the real-time pose parameter estimation information of the unmanned vehicle is matched and estimated by using a 2D laser radar mileage calculation method, and is obtained according to the following process:
firstly, screening and extracting point clouds scanned by a current frame of a laser radar sensor installed on an unmanned vehicle by means of a conventional laser radar odometer front-end LOAM algorithm to obtain face and corner features, and estimating preliminary rough pose information to obtain point clouds of the current frame face and corner features after preliminary correction of the rough pose.
Then, calculating and searching the nearest node in the road mileage nodes by utilizing the three-dimensional coordinate information in the preliminary outline pose, wherein a calculation formula is as follows:
wherein x is 0 、y 0 And z 0 E is three-dimensional coordinates in the outline pose k 、n k And u k The three-dimensional coordinates of the kth and nearest mileage node, d is the mileage interval fixed on this node.
And finally, respectively matching the current frame surface and the corner feature point cloud with the vertical surface and the corner feature in the nearest mileage node map, and estimating real-time pose parameter estimation information of the unmanned vehicle by using a 2D laser radar matching algorithm.
As a preferred scheme, the 2D lidar matching algorithm is obtained according to the following procedure:
firstly, establishing a distance equation from a point of the angular point feature of the current frame to a straight line of the angular point feature in the map:
wherein p is u 、p v Two points are positioned on a straight line of the corner feature in the map, and h is the point of the corner feature of the current frameDistance to the straight line where the corner feature in the map is located. Similarly, a distance equation from the point of the current frame surface feature to the plane of the elevation feature in the map is established:
wherein p is u 、p v And p w Three points d are on the plane of the elevation feature in the map P Points that are characteristic of the current frameDistance to the plane in which the facade features in the map lie. Further, partial differential equations are obtained for the equations (5) and (6):
wherein J is L 、J P First-order coefficient matrix of partial differential equation of corner feature and plane feature respectively, δx= [ δeδn ]] T Is the plane displacement parameter increment in the pose parameters.
Estimating displacement parameters by utilizing Newton iterative algorithm:
in the method, in the process of the invention,for the Hessian matrix,>for gradient vectors, h, d are respectively the distance vector from the corner feature to the straight line, the distance vector from the face feature to the plane,>is a 2D displacement parameter in pose parameters.
As a preferred solution, the construction constraint factor node performs smooth optimization on the real-time pose estimation, and may be obtained according to the following procedure:
firstly, estimating relative pose parameters among key frames by using a conventional LOAM algorithm, constructing a 3D odometer factor, and establishing a factor graph model;
then, using the displacement parameters estimated by the 2D laser radar matching algorithm, and combining the elevation of the mileage nodes to construct a 3D position constraint factor to be added into the constructed factor graph model;
and finally, smoothly updating the whole factor graph model, and outputting the optimized pose parameters as dynamic navigation positioning information of the unmanned vehicle in the urban canyon road driving process.
The urban canyon sparse feature map constraint positioning device comprises a processor and a memory, wherein the memory stores a sparse feature map constraint positioning program, and the program is used for realizing the urban canyon sparse feature map constraint positioning method in scheme one when being run by the processor.
The beneficial effects of the invention are as follows:
(1) By designing the sparse feature map architecture based on the road mileage nodes, the local high-precision map can be directly searched by searching the nearest mileage nodes in the map matching and positioning application process, so that all point cloud data of the whole map are prevented from carrying out polling type search and judgment, and the map search time is greatly shortened. The map is divided locally by the mileage nodes, so that the situation that the wrong features (such as certain normal vectors are consistent or similar but the surface features with far position differences) are matched can be effectively avoided, meanwhile, the whole map can be prevented from being distributed and transmitted to a terminal user, only the local map on the mileage nodes with a relatively short distance from the terminal is required to be packaged and transmitted, and the requirement of real-time communication bandwidth is greatly reduced. The whole map can be locally updated and spliced by taking the mileage nodes as units, when a scene of a certain section is changed (such as a newly built building, a street lamp and the like), the map on the mileage node to which the map belongs can be locally updated, and then the map can be spliced and assembled with the maps on surrounding nodes to form the updated complete high-precision map.
(2) Since lidars are typically mounted on unmanned roofs, the point cloud on the near ground typically occupies more than half of the entire point cloud. However, the ground point cloud generally has strong constraining forces only at high Cheng Fangmian for positioning and weaker constraining forces for other pose parameters. Therefore, the data volume of the point cloud map can be effectively reduced by utilizing the elevation information on the mileage nodes to replace the constraint effectiveness of the near-ground point cloud on positioning, so that the feature map has good sparse characteristics. From observation analysis, dynamic objects (moving cars, pedestrians, etc.) in urban roads are typically distributed within a few meters from the ground. Therefore, the cloud of near-ground points is removed through the mileage node Gao Chengti, so that the interference of dynamic objects on map matching and positioning can be effectively eliminated.
(3) By utilizing a matching and positioning algorithm in the 2D laser radar, excessive parameterization of an estimation model can be effectively avoided, stability of parameter estimation is improved, and further smooth optimization of pose parameters can be realized by combining a factor graph algorithm.
Drawings
FIG. 1 is a flow chart of an implementation of a dynamic positioning method based on urban canyon sparse feature map constraints according to embodiment 1;
fig. 2 is a schematic diagram of a sparse feature map architecture design of a dynamic positioning method based on urban canyon sparse feature map constraints according to embodiment 1;
FIG. 3 is a vertical-face feature point cloud map of the dynamic positioning method based on urban canyon sparse feature map constraints described in example 1;
fig. 4 is a corner feature point cloud map of the dynamic positioning method based on the urban canyon sparse feature map constraint of embodiment 1;
FIG. 5 is a map information statistics table of the dynamic positioning method based on urban canyon sparse feature map constraints described in example 1;
fig. 6 is an error accumulation distribution diagram of the dynamic positioning and attitude measurement of the dynamic positioning method based on the urban canyon sparse feature map constraint of embodiment 1.
Detailed Description
The present invention is further illustrated in the following drawings and detailed description, which are to be understood as being merely illustrative of the invention and not limiting the scope of the invention.
With reference to fig. 1 and fig. 2, embodiment 1 discloses a dynamic positioning method based on urban canyon sparse feature map constraint, which comprises the following specific steps:
s1, designing a sparse feature map structure according to the shape of an urban road, selecting mileage nodes at fixed mileage intervals to form a map structure main line, and dividing the prior point cloud map with extracted features by taking the nodes as units, wherein the prior point cloud map is obtained according to the following process:
firstly, interpolation is carried out according to the urban road center line so as to obtain the three-dimensional coordinates of each mileage node. The three-dimensional coordinates are obtained as follows:
in the formula e i+1 、n i+1 And u i+1 Three-dimensional coordinates of (i+1) th mileage node, p e 、p n And p u D is a fixed mileage interval, s is an interpolation scale factor. Therefore, all mileage nodes of the urban road can be extracted to be connected into a main line of the map structure.
Then, the prior point cloud map after the features are extracted is divided by taking mileage nodes as units, and the prior point cloud map is obtained according to the following process:
in the formula e i 、n i And u i Three-dimensional coordinates, p, of the ith mileage node respectively x 、p y And p z The three-dimensional coordinates of feature point clouds such as faces, corner points and the like in the prior map are obtained, psi is the azimuth angle of the mileage node, and d is a fixed mileage interval. Thus, the point cloud satisfying all the conditions thereof can be divided by using the formula (2), thereby composing a high-precision point cloud map belonging to the mileage point. The fixed mileage interval preset value is 10m, the specialThe road section can be properly contracted on a steep slope.
S2, removing the prior point cloud map near-ground point cloud on each node by using the elevation information of the mileage nodes to obtain a prior high-precision sparse feature point cloud map which is composed of each node and only comprises corner features and face features (non-ground), wherein the prior high-precision sparse feature point cloud map is obtained according to the following process:
wherein u is i Elevation of ith mileage node, p z ' is the elevation of the characteristic point cloud above the mileage node, p z "is the elevation of the corner feature point cloud on the mileage node. Therefore, the prior point cloud map on the node can be removed from the point cloud on the near ground by utilizing the formula (3), so that the prior high-precision sparse feature point cloud map only comprising the elevation features and the corner features is obtained.
S3, based on the sparse feature point cloud map, estimating real-time pose parameter estimation information of the unmanned vehicle by using a 2D laser radar matching algorithm, and obtaining the real-time pose parameter estimation information according to the following process:
firstly, screening and extracting point clouds scanned by a current frame of a laser radar sensor installed on an unmanned vehicle by means of a conventional laser radar odometer front-end LOAM algorithm to obtain face and corner features, and estimating preliminary rough pose information to obtain point clouds of the current frame face and corner features after preliminary correction of the rough pose.
Then, calculating and searching the nearest node in the road mileage nodes by utilizing the three-dimensional coordinate information in the preliminary outline pose, wherein a calculation formula is as follows:
wherein x is 0 、y 0 And z 0 E is three-dimensional coordinates in the outline pose k 、n k And u k Three of the kth and nearest mileage nodes respectivelyThe dimensional coordinates, d, are the fixed mileage intervals on the node.
And finally, respectively matching the current frame surface and the corner feature point cloud with the vertical surface and the corner feature in the nearest mileage node map, and estimating real-time pose parameter estimation information of the unmanned vehicle by using a 2D laser radar matching algorithm.
The 2D laser radar matching algorithm is obtained according to the following process:
firstly, establishing a distance equation from a point of the angular point feature of the current frame to a straight line of the angular point feature in the map:
wherein p is u 、p v Two points are positioned on a straight line of the corner feature in the map, and h is the point of the corner feature of the current frameDistance to the straight line where the corner feature in the map is located. Similarly, a distance equation from the point of the current frame surface feature to the plane of the elevation feature in the map is established:
wherein p is u 、p v And p w Three points d are on the plane of the elevation feature in the map P Points that are characteristic of the current frameDistance to the plane in which the facade features in the map lie. Further, partial differential equations are obtained for the equations (5) and (6):
wherein J is L 、J P First-order coefficient matrix of partial differential equation of corner feature and plane feature respectively, δx= [ δeδn ]] T Is the plane displacement parameter increment in the pose parameters.
Estimating displacement parameters by utilizing Newton iterative algorithm:
in the method, in the process of the invention,for the Hessian matrix,>for gradient vectors, h, d are respectively the distance vector from the corner feature to the straight line, the distance vector from the face feature to the plane,>is a 2D displacement parameter in pose parameters.
S4, constructing constraint factor nodes by combining a factor graph algorithm to smoothly optimize the real-time pose estimation value, so as to obtain dynamic navigation positioning information of the unmanned vehicle in the urban canyon road driving process, and the dynamic navigation positioning information is obtained according to the following process:
firstly, estimating relative pose parameters among key frames by using a conventional LOAM algorithm, constructing a 3D odometer factor, and establishing a factor graph model;
then, using the displacement parameters estimated by the 2D laser radar matching algorithm, and combining the elevation of the mileage nodes to construct a 3D position constraint factor to be added into the constructed factor graph model;
finally, smoothly updating the whole factor graph model, and outputting the optimized pose parameters as dynamic navigation positioning information of the unmanned vehicle in the urban canyon road driving process
Correspondingly, embodiment 2 discloses an urban canyon sparse feature map constraint positioning device, which comprises a processor and a memory, wherein the memory stores a sparse feature map constraint positioning program, and the program executes instructions corresponding to steps S1 to S4 in embodiment 1 when being run by the processor.
It should be noted that the foregoing merely illustrates the technical idea of the present invention and is not intended to limit the scope of the present invention, and that a person skilled in the art may make several improvements and modifications without departing from the principles of the present invention, which fall within the scope of the claims of the present invention.
Claims (8)
1. A dynamic positioning method based on urban canyon sparse feature map constraint is characterized by comprising the following steps:
s1, designing a frame structure of a sparse feature map according to the shape of an urban road, selecting mileage nodes at fixed mileage intervals to form a map structure main line, and dividing the prior point cloud map after feature extraction by taking the nodes as units;
s2, removing the prior point cloud map near-ground point cloud on each node by using the elevation information of the mileage nodes to obtain a prior high-precision sparse feature point cloud map which is composed of each node and only comprises corner features and surface features;
s3, estimating real-time pose parameter estimation information of the unmanned vehicle by using a 2D laser radar matching algorithm based on the sparse feature point cloud map;
s4, constructing constraint factor nodes by combining a factor graph algorithm to carry out smooth optimization on the real-time pose estimation value, so that dynamic navigation positioning information of the unmanned vehicle in the urban canyon road driving process is obtained.
2. The dynamic positioning method based on urban canyon sparse feature map constraint of claim 1, wherein the sparse feature map architecture design of S1 is obtained according to the following procedure:
firstly, interpolating according to the urban road center line to obtain the three-dimensional coordinates of each mileage node; the three-dimensional coordinates are obtained as follows:
in the formula e i+1 、n i+1 And u i+1 Three-dimensional coordinates of (i+1) th mileage node, p e 、p n And p u D is a fixed mileage interval, s is an interpolation scale factor; therefore, all mileage nodes of the urban road can be extracted to be connected into a main line of the map structure;
then, the prior point cloud map after the features are extracted is divided by taking mileage nodes as units, and the prior point cloud map is obtained according to the following process:
in the formula e i 、n i And u i Three-dimensional coordinates, p, of the ith mileage node respectively x 、p y And p z The three-dimensional coordinates of feature point clouds such as faces, corner points and the like in the prior map are three-dimensional coordinates, psi is the azimuth angle of the mileage node, and d is a fixed mileage interval; thus, the point cloud satisfying all the conditions thereof is divided by using the formula (2), thereby composing a high-precision point cloud map belonging to the mileage point.
3. The dynamic positioning method based on urban canyon sparse feature map constraint of claim 2, wherein the fixed mileage interval preset value is 10m, and is properly narrowed on a steep slope road section.
4. The dynamic positioning method based on urban canyon sparse feature map constraint of claim 1, wherein the removing of the prior point cloud map near-ground point cloud on each node by using the mileage node elevation information is obtained according to the following procedures:
wherein u is i Elevation of ith mileage node, p' z For the elevation of the characteristic point cloud on the mileage node, p z The elevation of the characteristic point cloud of the angular point on the mileage node is determined; therefore, the prior high-precision sparse feature point cloud map only comprising the elevation features and the corner features is obtained by removing the prior point cloud map on the node near the ground by using the formula (3).
5. The dynamic positioning method based on urban canyon sparse feature map constraint of claim 1, wherein the matching estimation of real-time pose parameter estimation information of the unmanned vehicle by using a 2D laser radar mileage calculation method is obtained according to the following procedures:
firstly, screening and extracting point clouds scanned by a current frame of a laser radar sensor installed on an unmanned vehicle by means of a conventional laser radar odometer front-end LOAM algorithm to obtain face and corner features, and estimating preliminary rough pose information to obtain point clouds of the current frame face and corner features after preliminary correction of the rough pose;
then, calculating and searching the nearest node in the road mileage nodes by utilizing the three-dimensional coordinate information in the preliminary outline pose, wherein a calculation formula is as follows:
wherein x is 0 、y 0 And z 0 E is three-dimensional coordinates in the outline pose k 、n k And u k The three-dimensional coordinates of the kth and nearest mileage node are respectively, and d is a mileage interval fixed on the node;
and finally, respectively matching the current frame surface and the corner feature point cloud with the vertical surface and the corner feature in the nearest mileage node map, and estimating real-time pose parameter estimation information of the unmanned vehicle by using a 2D laser radar matching algorithm.
6. The urban canyon sparse feature map constraint-based dynamic positioning method of claim 5, wherein the 2D lidar matching algorithm is obtained according to the following process:
firstly, establishing a distance equation from a point of the angular point feature of the current frame to a straight line of the angular point feature in the map:
wherein p is u 、p v Two points are positioned on a straight line of the corner feature in the map, and h is the point of the corner feature of the current frameDistance to the straight line where the corner feature in the map is located; similarly, a distance equation from the point of the current frame surface feature to the plane of the elevation feature in the map is established:
wherein p is u 、p v And p w Three points d are on the plane of the elevation feature in the map P Points that are characteristic of the current frameDistance to the plane of the facade features in the map; further, partial differential equations are obtained for the equations (5) and (6):
wherein J is L 、J P Respectively corner pointsFirst-order coefficient matrix of partial differential equation of sign and face characteristics, δx= [ δeδn ]] T The plane displacement parameter increment in the pose parameters is adopted;
estimating displacement parameters by utilizing Newton iterative algorithm:
in the method, in the process of the invention,for the Hessian matrix,>for gradient vectors, h, d are respectively the distance vector from the corner feature to the straight line, the distance vector from the face feature to the plane,>is a 2D displacement parameter in pose parameters.
7. The urban canyon sparse feature map based constrained positioning method of claim 1, wherein the construction constraint factor node performs a smooth optimization on the real-time pose estimate, obtained according to the following procedure:
firstly, estimating relative pose parameters among key frames by using a conventional LOAM algorithm, constructing a 3D odometer factor, and establishing a factor graph model;
then, using the displacement parameters estimated by the 2D laser radar matching algorithm, and combining the elevation of the mileage nodes to construct a 3D position constraint factor to be added into the constructed factor graph model;
and finally, smoothly updating the whole factor graph model, and outputting the optimized pose parameters as dynamic navigation positioning information of the unmanned vehicle in the urban canyon road driving process.
8. An urban canyon sparse feature map based constrained positioning device comprising a processor and a memory, the memory storing a sparse feature map constrained positioning program which, when executed by the processor, is operable to implement an urban canyon sparse feature map based constrained positioning method as claimed in any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310751141.6A CN116678427A (en) | 2023-06-25 | 2023-06-25 | Dynamic positioning method and device based on urban canyon sparse feature map constraint |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310751141.6A CN116678427A (en) | 2023-06-25 | 2023-06-25 | Dynamic positioning method and device based on urban canyon sparse feature map constraint |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116678427A true CN116678427A (en) | 2023-09-01 |
Family
ID=87779102
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310751141.6A Pending CN116678427A (en) | 2023-06-25 | 2023-06-25 | Dynamic positioning method and device based on urban canyon sparse feature map constraint |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116678427A (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130197795A1 (en) * | 2012-02-01 | 2013-08-01 | General Motors Llc | Estimation of Vehicle Location |
| CN104764457A (en) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | Urban environment composition method for unmanned vehicles |
| US20180161986A1 (en) * | 2016-12-12 | 2018-06-14 | The Charles Stark Draper Laboratory, Inc. | System and method for semantic simultaneous localization and mapping of static and dynamic objects |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113376669A (en) * | 2021-06-22 | 2021-09-10 | 东南大学 | Monocular VIO-GNSS fusion positioning algorithm based on dotted line characteristics |
| CN113819914A (en) * | 2020-06-19 | 2021-12-21 | 北京图森未来科技有限公司 | Map construction method and device |
| CN114359476A (en) * | 2021-12-10 | 2022-04-15 | 浙江建德通用航空研究院 | Dynamic 3D urban model construction method for urban canyon environment navigation |
| CN114581492A (en) * | 2022-05-07 | 2022-06-03 | 成都理工大学 | Vehicle-mounted laser radar point cloud non-rigid registration method fusing road multi-feature |
| CA3156087A1 (en) * | 2021-07-30 | 2023-01-30 | The Hong Kong Polytechnic University | 3d lidar aided global navigation satellite system and the method for non-line-of-sight detection and correction |
-
2023
- 2023-06-25 CN CN202310751141.6A patent/CN116678427A/en active Pending
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130197795A1 (en) * | 2012-02-01 | 2013-08-01 | General Motors Llc | Estimation of Vehicle Location |
| CN104764457A (en) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | Urban environment composition method for unmanned vehicles |
| US20180161986A1 (en) * | 2016-12-12 | 2018-06-14 | The Charles Stark Draper Laboratory, Inc. | System and method for semantic simultaneous localization and mapping of static and dynamic objects |
| CN113819914A (en) * | 2020-06-19 | 2021-12-21 | 北京图森未来科技有限公司 | Map construction method and device |
| CN113066105A (en) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit |
| CN113376669A (en) * | 2021-06-22 | 2021-09-10 | 东南大学 | Monocular VIO-GNSS fusion positioning algorithm based on dotted line characteristics |
| CA3156087A1 (en) * | 2021-07-30 | 2023-01-30 | The Hong Kong Polytechnic University | 3d lidar aided global navigation satellite system and the method for non-line-of-sight detection and correction |
| CN114359476A (en) * | 2021-12-10 | 2022-04-15 | 浙江建德通用航空研究院 | Dynamic 3D urban model construction method for urban canyon environment navigation |
| CN114581492A (en) * | 2022-05-07 | 2022-06-03 | 成都理工大学 | Vehicle-mounted laser radar point cloud non-rigid registration method fusing road multi-feature |
Non-Patent Citations (2)
| Title |
|---|
| WEN, HE 等: ""Real-time single-frequency GPS/BDS code multipath mitigation method based on C/N0 normalization."", 《MEASUREMENT》, vol. 164, no. 0, 31 December 2020 (2020-12-31), pages 1 - 5 * |
| 杨显赐 等: ""基于因子图优化PPP的GNSS/INS松组合导航"", 《全球定位系统》, vol. 48, no. 3, 15 June 2023 (2023-06-15), pages 85 - 92 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111551958B (en) | Mining area unmanned high-precision map manufacturing method | |
| CN110146910B (en) | Positioning method and device based on data fusion of GPS and laser radar | |
| CN108986207B (en) | Road and along-line building simulation modeling method based on real pavement data | |
| CN111652179A (en) | Semantic high-precision map construction and positioning method based on dotted line feature fusion laser | |
| CN107850445A (en) | Method and system for generating and using locating reference datum | |
| JP5339953B2 (en) | 3D map correction apparatus and 3D map correction program | |
| CN110542908A (en) | laser radar dynamic object perception method applied to intelligent driving vehicle | |
| CN113593017A (en) | Method, device and equipment for constructing surface three-dimensional model of strip mine and storage medium | |
| CN113989451B (en) | High-precision map construction method and device and electronic equipment | |
| CN110187375A (en) | A kind of method and device improving positioning accuracy based on SLAM positioning result | |
| CN112987065A (en) | Handheld SLAM device integrating multiple sensors and control method thereof | |
| CN114459471B (en) | Positioning information determining method and device, electronic equipment and storage medium | |
| CN111710040B (en) | High-precision map construction method, system, terminal and storage medium | |
| CN114859374B (en) | Newly-built railway cross measurement method based on unmanned aerial vehicle laser point cloud and image fusion | |
| CN111829514A (en) | Road surface working condition pre-aiming method suitable for vehicle chassis integrated control | |
| CN114283070B (en) | Method for manufacturing terrain section by fusing unmanned aerial vehicle image and laser point cloud | |
| CN113838129B (en) | Method, device and system for obtaining pose information | |
| WO2018145334A1 (en) | Method, device and system for localizing a moving object | |
| CN115031744A (en) | Cognitive map positioning method and system based on sparse point cloud-texture information | |
| CN114063127A (en) | Method for fusing multi-focal-length visual SLAM and GPS and storage medium | |
| CN111612829B (en) | High-precision map construction method, system, terminal and storage medium | |
| CN113959437A (en) | Measuring method and system for mobile measuring equipment | |
| CN116929363A (en) | Mining vehicle autonomous navigation method based on passable map | |
| CN114543788B (en) | Multi-layer global perception map construction method and system universal to structural unstructured environment | |
| CN116678427A (en) | Dynamic positioning method and device based on urban canyon sparse feature map constraint |
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 |