CN111928860A - Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability - Google Patents

Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability Download PDF

Info

Publication number
CN111928860A
CN111928860A CN202010727458.2A CN202010727458A CN111928860A CN 111928860 A CN111928860 A CN 111928860A CN 202010727458 A CN202010727458 A CN 202010727458A CN 111928860 A CN111928860 A CN 111928860A
Authority
CN
China
Prior art keywords
positioning
curved surface
laser
autonomous vehicle
map
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010727458.2A
Other languages
Chinese (zh)
Inventor
王景川
于翔宇
茅天阳
高丽丽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Yantai Information Technology Research Institute Shanghai Jiaotong University
Shanghai Jiaotong University
Original Assignee
Yantai Information Technology Research Institute Shanghai Jiaotong University
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Yantai Information Technology Research Institute Shanghai Jiaotong University, Shanghai Jiaotong University filed Critical Yantai Information Technology Research Institute Shanghai Jiaotong University
Priority to CN202010727458.2A priority Critical patent/CN111928860A/en
Publication of CN111928860A publication Critical patent/CN111928860A/en
Pending legal-status Critical Current

Links

Images

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
    • G01C21/32—Structuring or formatting of map data
    • 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/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention provides an autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability, which is based on an improved multi-layer grid map and extracts different types of characteristic information in the map; calculating a three-dimensional curved surface positioning capacity index based on an octree map model and a three-dimensional laser sensor observation model; generating an active positioning path suitable for positioning the autonomous vehicle by taking the three-dimensional curved surface positioning capability as a main constraint condition; based on the topographic characteristic factor of the environment where the autonomous vehicle is located, the initialization state of the positioning particle swarm is restrained, AMCL global positioning is carried out, the matching degree of a global positioning strategy to environment information is improved, and the calculated amount caused by redundant information is reduced; and (3) checking and repositioning the autonomous vehicle pose obtained by global positioning based on a point cloud matching strategy, and avoiding the algorithm from converging to a wrong single area in a high-similarity scene.

Description

Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability
Technical Field
The invention relates to the technical field of autonomous vehicles, in particular to an autonomous vehicle active positioning method based on three-dimensional curved surface positioning capacity.
Background
In recent years, research and application related to autonomous vehicles mainly focus on the fields of intelligent driving, logistics distribution, space exploration and the like. Unmanned vehicles such as google unmanned vehicles and tesla electric vehicles have accumulated a large amount of drive test data, and although the unmanned related technologies have not yet completely achieved the safety and stability requirements of practical applications, the functions of intelligent parking, high-speed driving assistance and the like derived from the unmanned vehicles have been widely applied in daily life of people.
In the various application scenarios mentioned above, the basis of autonomous movement and operation of the vehicle is environmental perception through the sensors carried by the vehicle, wherein the most central technology is immediate positioning and mapping (SLAM). Due to the difference in the sensors used, SLAM techniques are generally divided into visual SLAM and laser SLAM. The visual SLAM generally uses a monocular camera, a binocular camera or a depth camera as a basic sensor, and has the advantages of capability of acquiring richer environment information, small sensor volume and low cost, but due to the limitation of the visual sensor, the effect of the visual SLAM is greatly influenced by environment factors such as illumination and the like, and the monocular camera is relatively difficult to estimate the environment scale. In contrast, the laser ranging sensor (also called as "laser radar") has the advantages of long observation distance, high data accuracy, stronger stability in different environments and the like, and is therefore widely applied to the task of autonomous vehicles in outdoor environments. The two types of common laser ranging sensors are two-dimensional laser sensors which only perform plane ranging, and the other type of common laser ranging sensors is a three-dimensional laser sensor which has a plurality of laser scanning lines with different heights and can acquire three-dimensional observation information. The primary sensor used herein is a three-dimensional laser sensor with 16 scan lines, 360 degree observation angle.
Thanks to decades of research and development, SLAM technology has had a more sophisticated solution in a small range of scenarios with distinct structural features. However, as the application environment of the autonomous vehicle becomes diversified and complicated, the technical difficulties and challenges faced by the autonomous vehicle positioning technology are increasing. Under a complex outdoor scene, because the information acquired by the sensor of the autonomous vehicle is greatly interfered by environmental factors, the method gives great importance to how to reasonably analyze and infer the information such as the attitude, observation and the like of the autonomous vehicle in actual work through the pre-acquired environmental map and the model information of the autonomous vehicle, so that the autonomous vehicle can be ensured to better perform autonomous global positioning aiming at environmental characteristics.
Disclosure of Invention
The invention provides an autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability, and aims to solve the technical problems that in a complex outdoor scene, the acquisition of information by a sensor of an autonomous vehicle is greatly interfered by environmental factors, and the positioning difficulty is high, so that the autonomous vehicle can perform autonomous global positioning better aiming at environmental characteristics.
The invention provides an autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability, which comprises the following steps:
step 1: a characteristic extraction step: the method comprises the steps of collecting map data by using a laser sensor carried by an autonomous vehicle, and extracting map height and normal characteristic information based on a multilayer grid map model.
Step 2: calculating the index of the three-dimensional curved surface positioning capability: and calculating the three-dimensional curved surface positioning capacity index based on the octree map model and the three-dimensional laser sensor observation model.
And step 3: an active positioning path generation step: and generating an active positioning path suitable for the autonomous vehicle positioning by taking the three-dimensional curved surface positioning capability as a main constraint condition.
And 4, step 4: global positioning step: and based on the topographic characteristic factors of the environment where the autonomous vehicle is located, restraining the initial state of the positioning particle swarm, and carrying out AMCL global positioning.
And 5: pose checking and repositioning: and (3) checking and repositioning the autonomous vehicle pose obtained by global positioning based on a point cloud matching strategy, and avoiding the algorithm from converging to a wrong single area in a high-similarity scene.
Preferably, the feature extraction step includes:
a ground height extraction step: the method comprises the steps of firstly carrying out outlier filtering on a three-dimensional octree map, removing miscellaneous points in the map, then traversing the grid of an xOy plane, and reserving the lowest height point in the grid as the corresponding ground point height. In order to describe the position information of the laser, the distance from the laser center to the ground is measured in advance, and compensation with the same height is added on the basis of the height of the ground point to serve as the actual position information of the laser sensor when the autonomous vehicle moves in the environment.
And (3) ground normal extraction: and (3) selecting a certain number of adjacent points to construct a corresponding tangent plane by an improved PCA method, namely selecting nearest neighbor searching mode of the KD tree for each point in the point cloud library, and calculating the normal direction of the tangent plane by introducing viewpoint and neighborhood point constraints after the tangent plane is determined.
Preferably, the step of calculating the three-dimensional curved surface positioning capability index includes:
index calculation: let (x, y, z) be the position coordinates of the laser in space,
Figure BDA0002600713550000035
respectively representing the roll angle, pitch angle and yaw angle of the laser, can be used
Figure BDA0002600713550000034
To define the observation state of the laser in three-dimensional space.
One frame of observation data Z of the laser comprises a group of discrete laser point information, and the expected length value and the angle value of the ith laser point in the observation Z are assumed to be riEAnd alphaiWherein i is 1, 2.And N, the observation model of the laser sensor is as follows:
riE＝d(p，αi)+i，i＝1，2，...，N
wherein d (p, α)i) Representing the autonomous vehicle at the p position with an angle value of alphaiIs measured by the distance between the nearest obstacles,ian expected value of 0 and a variance of σ2Gaussian noise of (2);
the three-dimensional curved surface positioning capability matrix is:
Figure BDA0002600713550000031
the calculation formula of the three-dimensional curved surface positioning capability index is as follows:
Figure BDA0002600713550000032
preferably, the active positioning path generating step includes:
a step of analyzing feasibility: factors can be distinguished into the following two categories based on whether a certain factor can directly decide the trafficability of each voxel in the octree map.
Factors which can be binarized, some factors can directly determine the trafficability of the vehicle in a certain grid or voxel, and the relationship between the factors and the trafficability index is described by the following formula:
Figure BDA0002600713550000033
factors which cannot be binarized and other factors cannot directly determine whether the grids are passable or impassable, and the common influence of the factors on the passability indexes is analyzed through different indexes and weights. Let fjAnd (3) an index representing the factor j after normalization, wherein the influence of the factor j on the trafficability of each grid is as follows:
Figure BDA0002600713550000041
assuming that there are m factors that can be binarized and l influencing factors that cannot be binarized, their overall influence on the map feasibility is calculated by the following formula:
Figure BDA0002600713550000042
Figure BDA0002600713550000043
Figure BDA0002600713550000044
wherein wjIs represented by FjThe weight of (c).
Planning an active positioning path: let P be an arbitrary path in a two-dimensional grid map, with the grid traversed by it being g1，g2，g3，...，gtThe curved surface positioning capability cost function of the path P is:
Figure BDA0002600713550000045
where C (P) is the total cost of path P,
Figure BDA0002600713550000046
representing grid giAnd the corresponding curved surface positioning capability index value. To obtain the route with the maximum positioning capability, the following objective function is solved:
Figure BDA0002600713550000047
meanwhile, the path length is fixed as the most main constraint condition, and other trafficability influencing factors such as obstacles are used as conventional constraints, so that the planned path meets the following conditions:
Figure BDA0002600713550000048
Figure BDA0002600713550000049
s.t.
Figure BDA00026007135500000410
wherein d (g)i，gi+1) Is a grid giAnd gi+1Euclidean distance, S, between corresponding curved surface pointspassD (P) is a calculated total path length for the map passable area acquired on the basis of the passability analysis step.
Preferably, the global positioning step includes:
initial particle distribution step: based on the extracted map feature information, the height and normal information are used as constraint conditions, the possible pose of the particles is constrained to the range of the motion surface from the three-dimensional space when the global particles are initially distributed, and the random distribution of the particle groups in the three-dimensional space is constrained to the single-layer distribution in the range of the motion surface.
Particle positioning step: based on the AMCL positioning algorithm, positioning is carried out through KLD sampling.
Preferably, the pose verifying and repositioning step comprises:
a pose checking step: the method comprises the steps of fusing a laser observation model based on a laser beam and a laser observation model based on a likelihood field, firstly respectively calculating the likelihoods of the two laser models, wherein the likelihoods of the likelihood field observation model are obtained by using a larger number of random sampling points, the likelihoods of the laser beam observation model are obtained by using a minimum number of random sampling points, and then the likelihoods of all particles are obtained by multiplying the likelihoods of the two models, so that the point cloud registration effect is realized by using a smaller calculation amount on the premise of reducing error matching.
A relocation step: and setting a proper threshold value according to the ratio of the successfully matched laser points to all the points in each frame of observation, wherein the threshold value can be used as a standard for judging the global positioning pose. If the proportion of the matched point cloud is lower than the threshold value, the global positioning is not converged to the correct pose, at the moment, the particle initialization distribution is carried out again, the convergence updating is carried out, and the process is repeated until the autonomous vehicle converges to the correct pose state.
Compared with the prior art, the application has the following beneficial effects:
the invention provides an autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability, which improves a multilayer grid map through a technical scheme and extracts different types of characteristic information in the map; calculating a three-dimensional curved surface positioning capacity index based on an octree map model and a three-dimensional laser sensor observation model; generating an active positioning path suitable for positioning the autonomous vehicle by taking the three-dimensional curved surface positioning capability as a main constraint condition; based on the topographic characteristic factor of the environment where the autonomous vehicle is located, the initialization state of the positioning particle swarm is restrained, AMCL global positioning is carried out, the matching degree of a global positioning strategy to environment information is improved, and the calculated amount caused by redundant information is reduced; the autonomous vehicle pose acquired by global positioning is checked and repositioned based on a point cloud matching strategy, the algorithm is prevented from converging to a wrong single region in a high-similarity scene, and the technical effect of improving the active positioning efficiency and stability of the autonomous vehicle in an outdoor complex scene with uneven ground can be achieved.
Drawings
FIG. 1 is a flow chart of the method of the present invention.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
Aiming at the defects of the prior art, the autonomous vehicle active positioning method based on the three-dimensional curved surface positioning capability is innovatively provided.
The invention provides an autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability, which mainly comprises the following steps:
step 1: and (4) feature extraction, namely acquiring map data by using a laser sensor carried by an autonomous vehicle, and extracting the height and normal feature information of the map based on a multilayer grid map model.
Step 2: and calculating the three-dimensional curved surface positioning capability index based on the octree map model and the three-dimensional laser sensor observation model.
And step 3: and generating an active positioning path, wherein the active positioning path is suitable for the autonomous vehicle positioning by taking the three-dimensional curved surface positioning capability as a main constraint condition.
And 4, step 4: and global positioning, namely constraining the initialization state of the positioning particle swarm based on the topographic characteristic factors of the environment where the autonomous vehicle is located, and carrying out AMCL global positioning.
And 5: and (4) position and orientation detection and relocation, wherein the position and orientation of the autonomous vehicle obtained by global positioning are detected and relocated based on a point cloud matching strategy.
Specifically, the feature extraction process of step 1 mainly includes the following steps:
step 1.1: and (3) extracting the ground height, namely performing outlier filtering on the three-dimensional octree map to remove miscellaneous points in the map, traversing the grid of the xOy plane, and keeping the lowest height point in the grid as the corresponding ground point height. In order to describe the position information of the laser, the distance from the laser center to the ground is measured in advance, and compensation with the same height is added on the basis of the height of the ground point to serve as the actual position information of the laser sensor when the autonomous vehicle moves in the environment.
Step 1.2: and (3) performing ground normal extraction, selecting a certain number of adjacent points to construct a corresponding tangent plane by an improved PCA method, namely selecting nearest neighbor searching modes of a KD tree for each point in the point cloud library, and calculating the normal direction of the tangent plane by introducing viewpoint and neighborhood point constraints after the tangent plane is determined.
Specifically, the calculation of the three-dimensional curved surface positioning capability index in step 2 mainly includes the following steps:
step 2.1: index calculation, let (x, y, z) be the position coordinates of the laser in space,
Figure BDA0002600713550000061
respectively representing the roll angle, pitch angle and yaw angle of the laser, can be used
Figure BDA0002600713550000062
To define the observed state of the laser in three-dimensional space.
One frame of observation data Z of the laser comprises a group of discrete laser point information, and the expected length value and the angle value of the ith laser point in the observation Z are assumed to be riEAnd alphaiWhere i 1, 2., N, then the observation model of the laser sensor is:
riE＝d(p，αi)+i，i＝1，2，...，N
wherein d (p, α)i) Representing the autonomous vehicle at the p position with an angle value of alphaiIs measured by the distance between the nearest obstacles,ian expected value of 0 and a variance of σ2Gaussian noise.
The three-dimensional curved surface positioning capability matrix is:
Figure BDA0002600713550000071
the calculation formula of the three-dimensional curved surface positioning capability index is as follows:
Figure BDA0002600713550000072
specifically, the active positioning path generating process in step 3 mainly includes the following steps:
step 3.1: the feasibility analysis is used for directly determining the feasibility of each voxel in the octree map based on whether a certain factor can be directly determined, and the factor can be divided into the following two types.
Factors which can be binarized, some factors can directly determine the trafficability of the vehicle in a certain grid or voxel, and the relationship between the factors and the trafficability index is described by the following formula:
Figure BDA0002600713550000073
factors which cannot be binarized and other factors cannot directly determine whether the grids are passable or impassable, and the common influence of the factors on the passability indexes is analyzed through different indexes and weights. Let fjAnd (3) an index representing the factor j after normalization, wherein the influence of the factor j on the trafficability of each grid is as follows:
Figure BDA0002600713550000074
assuming that there are m factors that can be binarized and l influencing factors that cannot be binarized, their overall influence on the map feasibility is calculated by the following formula:
Figure BDA0002600713550000075
Figure BDA0002600713550000076
Figure BDA0002600713550000081
wherein wjIs represented by FjThe weight of (c).
Step 3.2: and (3) planning an active positioning path, wherein P is an arbitrary path in the two-dimensional grid map, and the grid passing through the path is g1，g2，g3，...，gtThe curved surface positioning capability cost function of the path P is:
Figure BDA0002600713550000082
where C (P) is the total cost of path P,
Figure BDA0002600713550000083
representing grid giAnd the corresponding curved surface positioning capability index value. To obtain the route with the maximum positioning capability, the following objective function is solved:
Figure BDA0002600713550000084
meanwhile, the path length is fixed as the most main constraint condition, and other trafficability influencing factors such as obstacles are used as conventional constraints, so that the planned path meets the following conditions:
Figure BDA0002600713550000085
Figure BDA0002600713550000086
s.t.
Figure BDA0002600713550000087
wherein d (g)i，gi+1) Is a grid giAnd gi+1Euclidean distance, S, between corresponding curved surface pointspassD (P) is a calculated total path length for the map passable area acquired on the basis of the passability analysis step.
Specifically, the global positioning in step 4 mainly includes the following steps:
step 4.1: and (3) initial particle distribution, wherein the possible pose of the particles is constrained to a motion curved surface range from a three-dimensional space when global particles are initially distributed based on the extracted map characteristic information, the height and the normal information as constraint conditions, and the random distribution of the particle swarm in the three-dimensional space is constrained to single-layer distribution in the curved surface range.
Step 4.2: and (4) positioning the particles by using KLD sampling based on an AMCL positioning algorithm.
Specifically, the pose verification and repositioning of step 5 mainly includes the following steps:
step 5.1: and (2) pose detection, namely fusing a laser observation model based on a laser beam and a laser observation model based on a likelihood field, firstly respectively calculating the likelihoods of the two laser models, wherein the likelihoods of the laser observation models are obtained by using a larger number of random sampling points, the likelihoods of the laser observation models are obtained by using a minimum number of random sampling points, and then the likelihoods of all particles are obtained by multiplying the likelihoods of the two models, so that the point cloud registration effect is realized by a smaller calculated amount on the premise of reducing error matching.
Step 5.2: and (4) repositioning, namely setting a proper threshold value according to the ratio of the successfully matched laser points to all the points in each frame of observation, and taking the threshold value as a standard for judging the global positioning pose. If the proportion of the matched point cloud is lower than the threshold value, the global positioning is considered not to be converged to the correct pose, at the moment, the particle initialization distribution is carried out again, the convergence updating is carried out, and the process is repeated until the autonomous vehicle converges to the correct pose state.
In a specific implementation process, as shown in fig. 1, the method is divided into five parts, namely (1) feature extraction, (2) three-dimensional curved surface positioning capability index calculation, (3) active positioning path generation, (4) global positioning, (5) pose detection and repositioning, and mainly comprises the following steps:
step 1: the method comprises the steps of collecting map data by using a laser sensor carried by an autonomous vehicle, and extracting map height and normal characteristic information based on a multilayer grid map model. The concrete implementation is as follows:
step 1.1: and (3) extracting the ground height, namely performing outlier filtering on the three-dimensional octree map to remove miscellaneous points in the map, traversing the grid of the xOy plane, and keeping the lowest height point in the grid as the corresponding ground point height. In order to describe the position information of the laser, the distance from the laser center to the ground is measured in advance, and compensation with the same height is added on the basis of the height of the ground point to serve as the actual position information of the laser sensor when the autonomous vehicle moves in the environment.
Step 1.2: and (3) performing ground normal extraction, selecting a certain number of adjacent points to construct a corresponding tangent plane by an improved PCA method, namely selecting nearest neighbor searching modes of a KD tree for each point in the point cloud library, and calculating the normal direction of the tangent plane by introducing viewpoint and neighborhood point constraints after the tangent plane is determined. The specific algorithm is as follows:
Figure BDA0002600713550000091
Figure BDA0002600713550000101
step 2: and calculating the three-dimensional curved surface positioning capacity index based on the octree map model and the three-dimensional laser sensor observation model. The concrete implementation is as follows:
step 2.1: index calculation, let (x, y, z) be the position coordinates of the laser in space,
Figure BDA0002600713550000104
respectively representing the roll angle, pitch angle and yaw angle of the laser, can be used
Figure BDA0002600713550000105
To define the observed state of the laser in three-dimensional space.
One frame of observation data Z of the laser comprises a group of discrete laser point information, and the observation Z is assumed to beThe desired length and angle values of the ith laser spot are riEAnd alphaiWhere i 1, 2., N, then the observation model of the laser sensor is:
riE＝d(p，αi)+i，i＝1，2，...，N
wherein d (p, α)i) Representing the autonomous vehicle at the p position with an angle value of alphaiIs measured by the distance between the nearest obstacles,ian expected value of 0 and a variance of σ2Gaussian noise.
The three-dimensional curved surface positioning capability matrix is:
Figure BDA0002600713550000102
the calculation formula of the three-dimensional curved surface positioning capability index is as follows:
Figure BDA0002600713550000103
and step 3: and generating an active positioning path suitable for positioning the autonomous vehicle by taking the three-dimensional curved surface positioning capability as a main constraint condition. The concrete implementation is as follows:
step 3.1: the feasibility analysis is used for directly determining the feasibility of each voxel in the octree map based on whether a certain factor can be directly determined, and the factor can be divided into the following two types.
Factors which can be binarized, some factors can directly determine the trafficability of the vehicle in a certain grid or voxel, and the relationship between the factors and the trafficability index is described by the following formula:
Figure BDA0002600713550000111
factors which cannot be binarized and other factors cannot directly determine whether the grids are passable or impassable, and the common use of the factors on the passability indexes is analyzed through different indexes and weightsThe same effect can be obtained. Let fjAnd (3) an index representing the factor j after normalization, wherein the influence of the factor j on the trafficability of each grid is as follows:
Figure BDA0002600713550000112
assuming that there are m factors that can be binarized and l influencing factors that cannot be binarized, their overall influence on the map feasibility is calculated by the following formula:
Figure BDA0002600713550000113
Figure BDA0002600713550000114
Figure BDA0002600713550000115
wherein wjIs represented by FjThe weight of (c).
Step 3.2: and (3) planning an active positioning path, wherein P is an arbitrary path in the two-dimensional grid map, and the grid passing through the path is g1，g2，g3，...，gtThe curved surface positioning capability cost function of the path P is:
Figure BDA0002600713550000116
where C (P) is the total cost of path P,
Figure BDA0002600713550000117
representing grid giAnd the corresponding curved surface positioning capability index value. To obtain the route with the maximum positioning capability, the following objective function is solved:
Figure BDA0002600713550000118
meanwhile, the path length is fixed as the most main constraint condition, and other trafficability influencing factors such as obstacles are used as conventional constraints, so that the planned path meets the following conditions:
Figure BDA0002600713550000119
Figure BDA00026007135500001110
Figure BDA0002600713550000121
wherein d (g)i，gi+1) Is a grid giAnd gi+1Euclidean distance, S, between corresponding curved surface pointspassD (P) is a calculated total path length for the map passable area acquired on the basis of the passability analysis step.
And 4, step 4: and constraining the initialization state of the positioning particle swarm based on the topographic characteristic factors of the environment where the autonomous vehicle is located, and carrying out AMCL global positioning. The concrete implementation is as follows:
step 4.1: and (3) initial particle distribution, wherein the possible pose of the particles is constrained to a motion curved surface range from a three-dimensional space when global particles are initially distributed based on the extracted map characteristic information, the height and the normal information as constraint conditions, and the random distribution of the particle swarm in the three-dimensional space is constrained to single-layer distribution in the curved surface range.
Step 4.2: and (4) positioning the particles by using KLD sampling based on an AMCL positioning algorithm.
And 5: and (3) checking and repositioning the autonomous vehicle pose obtained by global positioning based on a point cloud matching strategy, and avoiding the algorithm from converging to a wrong single area in a high-similarity scene. The concrete implementation is as follows:
step 5.1: and (2) pose detection, namely fusing a laser observation model based on a laser beam and a laser observation model based on a likelihood field, firstly respectively calculating the likelihoods of the two laser models, wherein the likelihoods of the laser observation models are obtained by using a larger number of random sampling points, the likelihoods of the laser observation models are obtained by using a minimum number of random sampling points, and then the likelihoods of all particles are obtained by multiplying the likelihoods of the two models, so that the point cloud registration effect is realized by a smaller calculated amount on the premise of reducing error matching.
Step 5.2: and (4) repositioning, namely setting a proper threshold value according to the ratio of the successfully matched laser points to all the points in each frame of observation, and taking the threshold value as a standard for judging the global positioning pose. If the proportion of the matched point cloud is lower than the threshold value, the global positioning is considered not to be converged to the correct pose, at the moment, the particle initialization distribution is carried out again, the convergence updating is carried out, and the process is repeated until the autonomous vehicle converges to the correct pose state.
Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and individual modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps into logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.

Claims (6)

1. An autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability is characterized by comprising the following steps:
a characteristic extraction step: acquiring map data by using a laser sensor carried by an autonomous vehicle, and extracting map height and normal characteristic information based on a multilayer grid map model;
calculating the index of the three-dimensional curved surface positioning capability: calculating a three-dimensional curved surface positioning capacity index based on an octree map model and a three-dimensional laser sensor observation model;
an active positioning path generation step: generating an active positioning path suitable for positioning the autonomous vehicle by taking the three-dimensional curved surface positioning capability as a main constraint condition; global positioning step: based on the topographic characteristic factor of the environment where the autonomous vehicle is located, the initialization state of the positioning particle swarm is restrained, and AMCL global positioning is carried out;
pose checking and repositioning: and (3) checking and repositioning the autonomous vehicle pose obtained by global positioning based on a point cloud matching strategy, and avoiding the algorithm from converging to a wrong single area in a high-similarity scene.
2. The active positioning method for autonomous vehicles based on three-dimensional curved surface positioning capability of claim 1, wherein said feature extraction step comprises:
a ground height extraction step: firstly, performing outlier filtering on the three-dimensional octree map, removing miscellaneous points in the map, traversing the grid of the xOy plane, and reserving the lowest height point in the grid as the corresponding ground point height; in order to describe the position information of the laser, the distance from the laser center to the ground is measured in advance, and the compensation with the same height is added on the basis of the height of a ground point to serve as the actual position information of the laser sensor when the autonomous vehicle moves in the environment;
and (3) ground normal extraction: the method comprises the steps of selecting a certain number of adjacent points to construct a corresponding tangent plane by an improved PCA method, namely selecting nearest neighbor searching of a KD tree for each point in a point cloud base, and calculating the normal direction of the tangent plane by introducing viewpoint and neighborhood point constraints after the tangent plane is determined.
3. The active positioning method for autonomous vehicles based on three-dimensional curved surface positioning capability of claim 1, wherein said three-dimensional curved surface positioning capability index calculating step comprises:
index calculation: let (x, y, z) be the position coordinates of the laser in space,
Figure FDA0002600713540000011
respectively representing the roll angle, pitch angle and yaw angle of the laser, can be used
Figure FDA0002600713540000012
To define the observation state of the laser in the three-dimensional space;
one frame of observation data Z of the laser comprises a group of discrete laser point information, and the expected length value and the angle value of the ith laser point in the observation Z are assumed to be riEAnd alphaiWhere i 1, 2., N, then the observation model of the laser sensor is:
riE＝d(p，αi)+i，i＝1，2，...，N
wherein d (p, α)i) Representing the autonomous vehicle at the p position with an angle value of alphaiIs measured by the distance between the nearest obstacles,ian expected value of 0 and a variance of σ2Gaussian noise of (2);
the three-dimensional curved surface positioning capability matrix is:
Figure FDA0002600713540000021
the calculation formula of the three-dimensional curved surface positioning capability index is as follows:
Figure FDA0002600713540000022
4. the active three-dimensional curved surface positioning capability-based autonomous vehicle positioning method of claim 1, wherein the active positioning path generating step comprises:
a step of analyzing feasibility: based on whether a certain factor can directly determine the feasibility of each voxel in the octree map, the factor can be divided into the following two categories;
factors which can be binarized, some factors can directly determine the trafficability of the vehicle in a certain grid or voxel, and the relationship between the factors and the trafficability index is described by the following formula:
Figure FDA0002600713540000023
factors which cannot be binarized and other factors cannot directly determine whether the grid is passable or impassable, the common influence of the factors on the passability indexes is analyzed through different indexes and weights, and f is setjAnd an index representing the factor j after normalization, wherein the influence of the factor j on the trafficability of each grid is as follows:
Figure FDA0002600713540000024
assuming that there are m factors that can be binarized and l influencing factors that cannot be binarized, their overall influence on the map trafficability is calculated by the following formula:
Figure FDA0002600713540000025
Figure FDA0002600713540000031
Figure FDA0002600713540000032
wherein wjIs represented by FjThe weight of (c);
planning an active positioning path: let P be an arbitrary path in a two-dimensional grid map, with the grid traversed by it being g1，g2，g3，...，gtThe curved surface positioning capability cost function of the path P is:
Figure FDA0002600713540000033
where C (P) is the total cost of path P,
Figure FDA0002600713540000034
representing grid giThe corresponding curved surface positioning capacity index value; to obtain the route with the maximum positioning capability, the following objective function is solved:
Figure FDA0002600713540000035
meanwhile, the path length is fixed as the most main constraint condition, and other trafficability influencing factors such as obstacles are used as conventional constraints, so that the planned path meets the following conditions:
Figure FDA0002600713540000036
Figure FDA0002600713540000037
Figure FDA0002600713540000038
wherein d (g)i，gi+1) Is a grid giAnd gi+1Euclidean distance, S, between corresponding curved surface pointspassD (P) is a calculated total path length for the map passable area acquired on the basis of the passability analysis step.
5. The autonomous vehicle active localization method based on three-dimensional curved surface localization capability of claim 1, wherein the global localization step comprises:
initial particle distribution step: based on the extracted map feature information and the height and normal information as constraint conditions, constraining the possible pose of the particles from a three-dimensional space to a motion surface range when the global particles are initially distributed, and constraining the random distribution of the particle swarm in the three-dimensional space to single-layer distribution in the surface range;
particle positioning step: based on the AMCL positioning algorithm, positioning is carried out through KLD sampling.
6. The autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability of claim 1, wherein the pose checking and repositioning step comprises:
a pose checking step: fusing a laser observation model based on a laser beam and a laser observation model based on a likelihood field, firstly respectively calculating the likelihoods of the two laser models, wherein the likelihoods of the likelihood field observation model are obtained by using a larger number of random sampling points, the likelihoods of the laser beam observation model are obtained by using a minimum number of random sampling points, and then the likelihoods of all particles are obtained by multiplying the likelihoods of the two models, so that the point cloud registration effect is realized by a smaller calculated amount on the premise of reducing error matching;
a relocation step: setting a proper threshold value according to the ratio of the successfully matched laser points to all the points in each frame of observation, wherein the threshold value can be used as a standard for judging the global positioning pose; if the proportion of the matched point cloud is lower than the threshold value, the global positioning is not converged to the correct pose, at the moment, the particle initialization distribution is carried out again, the convergence updating is carried out, and the process is repeated until the autonomous vehicle converges to the correct pose state.
CN202010727458.2A 2020-07-24 2020-07-24 Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability Pending CN111928860A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010727458.2A CN111928860A (en) 2020-07-24 2020-07-24 Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010727458.2A CN111928860A (en) 2020-07-24 2020-07-24 Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability

Publications (1)

Publication Number Publication Date
CN111928860A true CN111928860A (en) 2020-11-13

Family

ID=73316071

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010727458.2A Pending CN111928860A (en) 2020-07-24 2020-07-24 Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability

Country Status (1)

Country Link
CN (1) CN111928860A (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112747736A (en) * 2020-12-22 2021-05-04 西北工业大学 Indoor unmanned aerial vehicle path planning method based on vision
CN113405560A (en) * 2021-05-28 2021-09-17 武汉理工大学 Unified modeling method for vehicle positioning and path planning
CN113607171A (en) * 2021-08-04 2021-11-05 清华大学建筑设计研究院有限公司 Evacuation path planning method, evacuation path planning device, evacuation path planning equipment and storage medium
CN113865588A (en) * 2021-08-24 2021-12-31 上海交通大学 Robot positioning method and device

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103970135A (en) * 2014-04-22 2014-08-06 重庆邮电大学 Multi-mobile-robot cooperation positioning method based on filtering of MAPSO particle optimization filtering
WO2016045615A1 (en) * 2014-09-25 2016-03-31 科沃斯机器人有限公司 Robot static path planning method
CN106017497A (en) * 2016-07-06 2016-10-12 上海交通大学 Route planning method based on map orientation capacity
CN108896050A (en) * 2018-06-26 2018-11-27 上海交通大学 A kind of mobile robot long-term tillage system and method based on laser sensor
CN109084732A (en) * 2018-06-29 2018-12-25 北京旷视科技有限公司 Positioning and air navigation aid, device and processing equipment
CN110068330A (en) * 2019-01-16 2019-07-30 上海交通大学 Autonomic positioning method based on the robot that arma modeling is realized
CN111091101A (en) * 2019-12-23 2020-05-01 中国科学院自动化研究所 High-precision pedestrian detection method, system and device based on one-step method

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103970135A (en) * 2014-04-22 2014-08-06 重庆邮电大学 Multi-mobile-robot cooperation positioning method based on filtering of MAPSO particle optimization filtering
WO2016045615A1 (en) * 2014-09-25 2016-03-31 科沃斯机器人有限公司 Robot static path planning method
CN106017497A (en) * 2016-07-06 2016-10-12 上海交通大学 Route planning method based on map orientation capacity
CN108896050A (en) * 2018-06-26 2018-11-27 上海交通大学 A kind of mobile robot long-term tillage system and method based on laser sensor
CN109084732A (en) * 2018-06-29 2018-12-25 北京旷视科技有限公司 Positioning and air navigation aid, device and processing equipment
CN110068330A (en) * 2019-01-16 2019-07-30 上海交通大学 Autonomic positioning method based on the robot that arma modeling is realized
CN111091101A (en) * 2019-12-23 2020-05-01 中国科学院自动化研究所 High-precision pedestrian detection method, system and device based on one-step method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
BEHNAM IRANI: "基于定位能力的移动机器人路径规划研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 1, pages 140 - 848 *
XIANGYU YU等: "A Map Accessibility Analysis Algorithm for Mobile Robot Navigation in Outdoor Environment", 《2019 IEEE INTERNATIONAL CONFERENCE ON REAL-TIME COMPUTING AND ROBOTICS (RCAR)》, pages 475 - 480 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112747736A (en) * 2020-12-22 2021-05-04 西北工业大学 Indoor unmanned aerial vehicle path planning method based on vision
CN113405560A (en) * 2021-05-28 2021-09-17 武汉理工大学 Unified modeling method for vehicle positioning and path planning
CN113607171A (en) * 2021-08-04 2021-11-05 清华大学建筑设计研究院有限公司 Evacuation path planning method, evacuation path planning device, evacuation path planning equipment and storage medium
CN113607171B (en) * 2021-08-04 2023-05-26 清华大学建筑设计研究院有限公司 Evacuation path planning method, device, equipment and storage medium
CN113865588A (en) * 2021-08-24 2021-12-31 上海交通大学 Robot positioning method and device
CN113865588B (en) * 2021-08-24 2024-03-26 上海交通大学 Robot positioning method and device

Similar Documents

Publication Publication Date Title
CN111928860A (en) Autonomous vehicle active positioning method based on three-dimensional curved surface positioning capability
CN113587933B (en) Indoor mobile robot positioning method based on branch-and-bound algorithm
CN112184736B (en) Multi-plane extraction method based on European clustering
CN106199558A (en) Barrier method for quick
CN114119920A (en) Three-dimensional point cloud map construction method and system
CN110986945B (en) Local navigation method and system based on semantic altitude map
CN111950440B (en) Method, device and storage medium for identifying and positioning door
CN115639823A (en) Terrain sensing and movement control method and system for robot under rugged and undulating terrain
CN113947636B (en) Laser SLAM positioning system and method based on deep learning
CN115342821A (en) Unmanned vehicle navigation cost map construction method under complex unknown environment
Xu et al. Separation of wood and foliage for trees from ground point clouds using a novel least-cost path model
CN116127405A (en) Position identification method integrating point cloud map, motion model and local features
CN112733971B (en) Pose determination method, device and equipment of scanning equipment and storage medium
CN118411507A (en) Semantic map construction method and system for scene with dynamic target
CN113759928B (en) Mobile robot high-precision positioning method for complex large-scale indoor scene
CN114488026A (en) Underground parking garage passable space detection method based on 4D millimeter wave radar
CN118037790A (en) Point cloud processing method and device, computer equipment and storage medium
CN117629215A (en) Chassis charging pile-returning method based on single-line laser radar point cloud registration
CN113448340B (en) Unmanned aerial vehicle path planning method and device, unmanned aerial vehicle and storage medium
CN117053779A (en) Tightly coupled laser SLAM method and device based on redundant key frame removal
Zou et al. PatchAugNet: Patch feature augmentation-based heterogeneous point cloud place recognition in large-scale street scenes
Li et al. A systematic strategy of pallet identification and picking based on deep learning techniques
US20210405197A1 (en) GLOBAL LOCALIZATION APPARATUS AND METHOD IN DYNAMIC ENVIRONMENTS USING 3D LiDAR SCANNER
Gao et al. NEGL: Lightweight and Efficient Neighborhood Encoding-Based Global Localization for Unmanned Ground Vehicles
CN113671511A (en) Laser radar high-precision positioning method for regional scene

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
AD01 Patent right deemed abandoned

Effective date of abandoning: 20241008

AD01 Patent right deemed abandoned