CN114200462A - Positioning method, positioning apparatus, vehicle, and computer-readable storage medium - Google Patents

Positioning method, positioning apparatus, vehicle, and computer-readable storage medium Download PDF

Info

Publication number
CN114200462A
CN114200462A CN202111223000.4A CN202111223000A CN114200462A CN 114200462 A CN114200462 A CN 114200462A CN 202111223000 A CN202111223000 A CN 202111223000A CN 114200462 A CN114200462 A CN 114200462A
Authority
CN
China
Prior art keywords
point
point cloud
map
distance
positioning
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
CN202111223000.4A
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.)
DeepRoute AI Ltd
Original Assignee
DeepRoute AI Ltd
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 DeepRoute AI Ltd filed Critical DeepRoute AI Ltd
Priority to CN202111223000.4A priority Critical patent/CN114200462A/en
Publication of CN114200462A publication Critical patent/CN114200462A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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
    • 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
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The application provides a positioning method, a positioning device, a vehicle and a computer readable storage medium. The positioning method comprises the following steps: collecting point clouds of an area where the positioning equipment is located at the current moment; matching the point cloud with a map so as to determine a target position point matched with the point cloud in the map; judging whether the distance between each data point in the point cloud and the corresponding target position point is smaller than a preset distance; if not, deleting the data points, of which the distance between the data points and the corresponding target position points is greater than or equal to the preset distance, in the point cloud; and matching the point cloud after the data points are deleted with the map until the distance between each data point in the point cloud after the data points are deleted and the corresponding target position point is smaller than the preset distance, and confirming the initial position of the positioning equipment according to the target position point corresponding to the point cloud after the data points are deleted. By the method, the interference points in the point cloud can be effectively removed, and therefore stable and accurate initial position positioning at the current moment is achieved.

Description

Positioning method, positioning apparatus, vehicle, and computer-readable storage medium
Technical Field
The present application relates to the field of automatic driving positioning technology, and in particular, to a positioning method, a positioning device, a vehicle, and a computer-readable storage medium.
Background
For an automatic driving positioning system, an accurate initial position is very important, and the stability of a subsequent positioning system and whether the positioning is possible to drift are directly influenced. For the current positioning system for automatic driving, an initialization mode generally adopted is to directly perform positioning initialization through a position received by RTK (Real Time Kinematic, carrier-phase differential technology), which is relatively simple, but the problem is also relatively obvious, and for some places where RTK signals are not good, such as closed scenes like tunnels, the initialization position provided by RTK is very inaccurate, which directly affects the start of positioning.
In addition, in an actual automatic driving scene, it is a common phenomenon that dynamic objects are contained in the scanned point cloud, such as vehicles, pedestrians, and the like, and the existence of the dynamic objects affects the matching effect of the point cloud and the map. In contrast, in the currently adopted method, the point cloud of the dynamic object is deleted when the point cloud is acquired, and if the point cloud identification precision of the dynamic object is not enough, the point cloud of part of the non-dynamic object is also deleted in the process, so that the positioning stability and accuracy are influenced.
Disclosure of Invention
The application provides a positioning method, a positioning device, a vehicle and a computer readable storage medium.
The application provides a positioning method, which comprises the following steps:
collecting point clouds of an area where the positioning equipment is located at the current moment;
matching the point cloud with a map to determine a target location point in the map that matches the point cloud;
judging whether the distance between each data point in the point cloud and the corresponding target position point is smaller than a preset distance;
if not, deleting the data points, of which the distance between the point cloud and the corresponding target position point is greater than or equal to the preset distance, from the point cloud;
and matching the point cloud after the data points are deleted with the map until the distance between each data point in the point cloud after the data points are deleted and the corresponding target position point is smaller than the preset distance, and confirming the initial position of the positioning equipment according to the target position point corresponding to the data point.
The positioning method further comprises the following steps:
and when the distance between each data point in the point cloud and the corresponding target position point is smaller than the preset distance, confirming the initialization position of the positioning equipment according to the target position point corresponding to the point cloud.
Wherein, deleting all or part of the data points of which the distance between the target position points corresponding to the point cloud is greater than or equal to the preset distance comprises:
and deleting data points with preset proportion in all the data points of which the distance between the point cloud and the corresponding target position point is more than or equal to the preset distance.
The positioning method further comprises the following steps:
sorting all data points, of which the distance between the point cloud and the corresponding target position point is greater than or equal to the preset distance, according to the distance;
and deleting the data points with preset proportion from all the data points according to the distance from large to small.
Wherein the matching the point cloud to a map to determine a target location point in the map that matches the point cloud comprises:
determining a search step length;
searching an area meeting preset matching requirements with the point cloud in the map as a target area according to the searching step length;
determining a set of map points for the target area in the map;
searching a map point closest to each data point in the point cloud in the map point set, and establishing a corresponding relation;
and determining a target position point matched with the point cloud in the map point set based on the corresponding relation.
Wherein, according to the search step length, searching an area which meets the preset matching requirement with the point cloud in the map as a target area, and the method comprises the following steps:
reducing the search step size to update the search step size;
and searching an area meeting preset matching requirements with the point cloud in the target area and the adjacent area of the target area based on the updated search step length so as to update the target area.
Wherein, based on the updated search step length, searching for an area meeting preset matching requirements with the point cloud in the target area and the adjacent area of the target area so as to update the target area, including:
judging whether the updated search step length is a preset minimum step length;
and if so, determining the target area corresponding to the current search step length as the final target area.
Wherein the determining the search step comprises:
determining a pyramid level, and determining a minimum step size;
and determining an initial search step size according to the pyramid level and the minimum step size.
Wherein, according to the search step length, searching an area which meets the preset matching requirement with the point cloud in the map as a target area, and the method comprises the following steps:
dividing the map into a plurality of search areas based on the search step length;
determining the number of matching points matched with the point cloud in each search area;
and determining the matching degree of the search area and the point cloud according to the ratio of the number of the matching points to the total number of the points in the search area.
Wherein the determining, based on the correspondence, a target location point matching the point cloud in the set of map points comprises:
calculating each corresponding relation by adopting a least square method so as to correct the target position point;
and determining the corrected target position point in the map point set.
Wherein, for each data point in the point cloud, searching the map point set with the closest distance, and establishing the corresponding relation, comprising:
establishing a binary tree by using the map point set in a preset range with the target position point in the map as the center;
searching the map point with the nearest distance in the binary tree for each data point in the point cloud, and establishing a corresponding relation
The application also provides a positioning device, which comprises a processor and a memory, wherein the memory is stored with program data, and the processor is used for executing the program data to realize the positioning method.
The application also provides a vehicle comprising the positioning device.
The present application also provides a computer-readable storage medium for storing program data which, when executed by a processor, is adapted to implement the positioning method described above.
The beneficial effect of this application is: the method comprises the steps that a positioning device collects point clouds of an area where the positioning device is located at the current moment; matching the point cloud with a map so as to determine a target position point matched with the point cloud in the map; judging whether the distance between each data point in the point cloud and the corresponding target position point is smaller than a preset distance; if not, deleting the data points, of which the distance between the data points and the corresponding target position points is greater than or equal to the preset distance, in the point cloud; and matching the point cloud after the data points are deleted with the map until the distance between each data point in the point cloud after the data points are deleted and the corresponding target position point is smaller than the preset distance, and confirming the initial position of the positioning equipment according to the target position point corresponding to the point cloud after the data points are deleted. By the method, the interference points in the point cloud can be effectively removed, and therefore stable and accurate initial position positioning at the current moment is achieved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts. Wherein:
fig. 1 is a schematic flowchart of an embodiment of a positioning method provided in the present application;
fig. 2 is a schematic flow chart of the sub-step of step S12 in the positioning method of fig. 1;
fig. 3 is a schematic flow chart illustrating the sub-step of step S122 in the positioning method of fig. 2;
fig. 4 is a schematic flowchart of another embodiment of a positioning method provided in the present application;
FIG. 5 is a schematic structural diagram of an embodiment of a pointing device provided herein;
FIG. 6 is a schematic structural diagram of another embodiment of a pointing device provided herein;
FIG. 7 is a schematic block diagram of an embodiment of a vehicle provided herein;
FIG. 8 is a schematic structural diagram of an embodiment of a computer-readable storage medium provided in the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
Aiming at the problems that the received position is very poor due to the limitation of the problems of scenes and the like of the RTK, the accurate initialization position cannot be provided due to the Point cloud ICP (Iterative Closest Point) matching or the influence of illumination on the image, and the influence on the automatic driving positioning equipment is very large, the application provides a stable and accurate positioning initialization method based on the Point cloud of the laser radar and the map in a rough to precise mode.
Referring to fig. 1, fig. 1 is a schematic flowchart illustrating an embodiment of a positioning method according to the present application.
The positioning method is applied to positioning equipment, wherein the positioning equipment can be a server, and can also be a system formed by the server and terminal equipment which are matched with each other. Accordingly, each part, such as each unit, sub-unit, module, and sub-module, included in the positioning device may be disposed in the server, or may be disposed in the server and the terminal device, respectively.
Further, the server may be hardware or software. When the server is hardware, it may be implemented as a distributed server cluster formed by multiple servers, or may be implemented as a single server. When the server is software, it may be implemented as a plurality of software or software modules, for example, software or software modules for providing distributed servers, or as a single software or software module, and is not limited herein. In some possible implementations, the positioning method of the embodiments of the present application may be implemented by a processor calling computer readable instructions stored in a memory.
Specifically, as shown in fig. 1, the positioning method in the embodiment of the present application specifically includes the following steps:
step S11: and collecting the point cloud of the area where the positioning equipment is located at the current moment.
In the embodiment of the application, the positioning device acquires the point cloud at the current moment through the laser radar and acquires the map through information acquisition technologies such as a GPS (global positioning system). It should be noted that the map described in the embodiment of the present application is generally a three-dimensional map, and represents three-dimensional information of a target position and an environment.
Step S12: and matching the point cloud with a map so as to determine a target position point matched with the point cloud in the map.
In the embodiment of the application, the positioning device matches the point cloud with the map within the set tolerance search range so as to determine the target position point matched with the point cloud in the map. The target position points are the position points with the highest point cloud matching degree according to the matching rules including the following matching rules embodied in the rough matching and fine matching processes, and each data point in the point cloud is determined to have the matched target position point.
Specifically, the setting process of the tolerance search range refers to the following process: the positioning apparatus acquires the position received by the RTK mounted on the autonomous vehicle and its three-dimensional coordinates, and then sets a tolerance value of the RTK accuracy and a tolerance range, for example, the tolerance value may be set to 20 m. Through the arrangement mode, the positioning device can set tolerance search ranges of +/-20 m in the x direction, the y direction and the z direction of the position received by the RTK, and the tolerance search ranges are far larger than the precision tolerance range of the RTK.
The preset distance algorithm for calculating the distance between the point in the point cloud and the map comprises but is not limited to the following distance algorithm: chamfer distance algorithm, geometrist distance algorithm, and the like.
The 3D point cloud is a superposition of laser scans obtained at different locations and times. Since laser scans capture snapshots of the surrounding environment, they often contain moving objects, which may be indirectly observed. Dynamic objects in the point cloud map can reduce the quality of the map and affect the positioning accuracy, so it is very important to delete the dynamic objects from the 3D point cloud map. The positioning device can sense the dynamic object in the 3D point cloud map, and then removes the dynamic point of the dynamic object in the point cloud according to a sensing result.
Specifically, the positioning apparatus of the embodiment of the present application may construct an occupancy grid map in which voxels represent the occupancy state of the amount of space within the extended period of time. After generating the occupancy grid map, we use it as a filter to remove dynamic target points in the lidar scan before adding them to the map. Furthermore, we use object detection and a new voxel traversal method to speed up the process of building an occupancy grid map. And after the occupied grid map is constructed, the dynamic object removal can be operated in real time.
Step S13: and judging whether the distance between each data point in the point cloud and the corresponding target position point is smaller than a preset distance.
In the embodiment of the present application, since the point cloud obtained in step S11 may contain interference data points of a dynamic object, some pairs of wrong matching points may be generated in the matching process of step S12. Therefore, the positioning apparatus needs to determine whether there is an interference data point of the dynamic object in the matching point pair by calculating the distance between the matching point pair. If the distance between any data point in the point cloud and the corresponding target position point is judged to be greater than or equal to the preset distance, it is indicated that an interference data point of the dynamic object exists in the matching point pair at the moment, and the interference data point needs to be eliminated, and the step S14 is entered.
If the distances between all the data points in the point cloud and the corresponding target position points are smaller than the preset distance, it is indicated that no interference data point of the dynamic object exists in the matching point pair at the moment, and then an accurate initialization position can be output.
It should be noted that the preset distance in the embodiment of the present application is a distance value set by a worker according to experience, and specific numerical values are not limited herein.
Step S14: and deleting the data points, of which the distance between the data points and the corresponding target position point is greater than or equal to a preset distance, in the point cloud.
In this embodiment of the application, after the positioning device is matched to the target position in the map in step S12, and when the distance between any data point in the point cloud and the matching point of the target position is greater than or equal to the preset distance, it is indicated that there are still interference data points of a part of dynamic objects in the point cloud, which results in a part of wrong matching point pairs in the matching process between the point cloud and the map, and therefore, the positioning device needs to delete the part of wrong matching point pairs.
On one hand, the positioning equipment can delete all data points which are more than or equal to the preset distance away from the target position point; on the other hand, the positioning equipment can delete part of data points with the distance between the data points and the target position point being greater than or equal to the preset distance according to the preset proportion, so that the fault tolerance of the positioning method is improved.
Specifically, the positioning device deletes a point with a distance greater than a certain proportion k% of a first preset distance from the point cloud when judging that the distance between a plurality of points and a matching point of the target position is greater than or equal to the first preset threshold value. On one hand, the positioning device may randomly delete points having a distance greater than a certain percentage k% of the first preset distance; on the other hand, the positioning device may also sort the points whose distance is greater than the first preset distance according to the distance from large to small, and delete the points sorted in the first certain proportion of k%.
Step S15: and matching the point cloud after the data points are deleted with the map until the distance between each data point in the point cloud after the data points are deleted and the corresponding target position point is smaller than the preset distance, and confirming the initial position of the positioning equipment according to the target position point corresponding to the point cloud after the data points are deleted.
After the positioning device deletes the data point, the matching process between the point cloud and the map in step S12 is executed again. Through continuous loop iteration, the initial position can be positioned based on the target position until the distance between each point in the point cloud and the matching point of the target position is smaller than a first preset threshold value, and an accurate initial position can be provided for the automatic driving vehicle in motion.
Further, the positioning apparatus in the embodiment of the application may further match the point cloud with the target location in the map in a pyramid matching manner, please refer to fig. 2 for the matching process of the target location specifically, and fig. 2 is a schematic flow chart of the substep of step S12 in the positioning method in fig. 1.
Specifically, as shown in fig. 2, step S12 of the positioning method according to the embodiment of the present application specifically includes the following sub-steps:
step S121: a search step size is determined.
In the embodiment of the application, the positioning device determines the pyramid levels and the step size corresponding to each pyramid level, wherein in order to ensure that the search result is within a reasonable range, it is required to ensure that the maximum step size in the pyramid levels is within a tolerance search range. Specifically, the step size and the tolerance search range of the embodiment of the present application should satisfy the following conditions:
min_step*2^n＜range
wherein min _ step is the minimum step size of the search, n is the pyramid level, and range is the tolerance search range. Therefore, the maximum step size of the search can then be embodied as: max _ step min _ step 2^ n.
Step S122: and searching an area meeting the preset matching requirement with the point cloud in the map as a target area based on the searching step length.
In the embodiment of the application, after the positioning device determines the search step size, the target region corresponding to the optimal matching score of the maximum pyramid level is calculated from the maximum pyramid level and the maximum step size max _ step. The target area is a map area with the highest matching degree with the point cloud in a plurality of search areas divided by the map.
Taking the maximum pyramid level and the maximum step size as examples, referring to fig. 3 for the specific search of the target region in the embodiment of the present application, fig. 3 is a schematic flow chart of the substep of step S122 in the positioning method of fig. 2.
Specifically, as shown in fig. 3, step S122 of the positioning method according to the embodiment of the present application specifically includes the following sub-steps:
step S1221: the map is divided into a plurality of search areas based on the search step size.
In an embodiment of the application, a positioning device divides a map into a plurality of search areas based on a maximum search step size.
Step S1222: and determining the number of matching points matched with the point cloud in each search area.
In the embodiment of the application, the positioning equipment traverses a plurality of search areas of a map according to point clouds, and searches matching points of the point clouds and each search area. If the positioning equipment successfully searches a matching point in the search area according to the point cloud, recording a matching score and adding 1; if the positioning equipment cannot search out the matching points in the search area according to the point cloud, the matching score is not increased.
And after the positioning equipment traverses all the search areas in the map, acquiring a final matching score corresponding to the number of matching points of the map and the point cloud.
Step S1223: and determining the matching degree of the search area and the point cloud according to the ratio of the number of the matching points to the total number of the points in the search area.
In the embodiment of the present application, the positioning device obtains a final score of the search area, that is, the matching degree, by using a ratio of the number of matching points to the total number of points in the search area, that is, dividing the total matching score by the total number of points in the search area. The formula for calculating the matching degree is specifically as follows:
score＝total score/points number
wherein score is the matching degree, total score is the number of matching points, and points number is the total number of points in the search area.
It should be noted that the search modes of other step lengths are substantially the same as the search mode of the maximum step length, and are not described herein again.
Step S123: and reducing the search step size to update the search step size.
In the embodiment of the application, after the positioning device calculates the matching degrees of all the search areas corresponding to the maximum pyramid level according to the maximum step length, the search area with the maximum matching degree is selected as a target area of the next pyramid level search, namely, the optimal position.
And the positioning equipment reduces the maximum step length to half of the original step length to obtain the next layer step length corresponding to the next pyramid level.
Step S124: and searching an area meeting preset matching requirements with the point cloud in the target area and the adjacent area of the target area in the map based on the search step length so as to update the target area.
In the embodiment of the application, the positioning device searches the target area and the adjacent area thereof by adopting the next layer of step length to obtain the target area of the pyramid level of the layer so as to update the target area of the maximum pyramid level.
The range of the adjacent region can be set to be twice of the step size of the next layer, namely 2 × current _ step, and adding the adjacent region as the search region can optimize the search efficiency and improve the search accuracy.
The positioning device repeatedly executes the steps S121 to S124 until the target area search of all pyramid levels is completed. When the positioning equipment iteratively searches the final level of the pyramid level, the search area with the maximum matching degree in the search area of the final level is the optimal initialization position.
In the embodiment of the application, the positioning equipment acquires the point cloud of the area where the positioning equipment is located at the current moment; matching the point cloud with a map so as to determine a target position point matched with the point cloud in the map; judging whether the distance between each data point in the point cloud and the corresponding target position point is smaller than a preset distance; if not, deleting all or part of data points, of which the distance between the point cloud and the corresponding target position point is greater than or equal to a preset distance, in the point cloud; and matching the point cloud after the data points are deleted with the map until the distance between each data point in the point cloud after the data points are deleted and the corresponding target position point is smaller than the preset distance, and confirming the initial position of the positioning equipment according to the corresponding target position point. By the method, the interference points in the point cloud can be effectively removed, and therefore stable and accurate initial position positioning at the current moment is achieved.
The positioning method of the embodiment realizes coarse matching of the accurate initialization position, and the embodiment of the application can further realize fine matching of the initialization position on the basis of the coarse matching result of the positioning method of the embodiment so as to further improve the accuracy of the initialization position.
Referring to fig. 4, fig. 4 is a schematic flowchart illustrating a positioning method according to another embodiment of the present application.
Specifically, as shown in fig. 4, the positioning method in the embodiment of the present application specifically includes the following steps:
step S21: a set of map points based on the target location is determined in the map.
Step S22: and searching each point in the point cloud in a map point set for a map point closest to the point in the point cloud, and establishing a corresponding relation.
Due to the influence of the minimum step precision, the initialized position obtained by the coarse matching result of the positioning method in the embodiment is not accurate enough, and the positioning device in the embodiment of the application further refines the initialized position by adopting a fine registration mode.
Specifically, the positioning device establishes a kd tree at the optimal initialization position obtained by the positioning method in the above embodiment, searches for a map point closest to each point in the point cloud in the kd tree, and establishes a corresponding relationship. The mapping relationship may be a mapping relationship, and the mapping relationship reflects a one-to-one correspondence relationship between one data point in the point cloud and the nearest map point in the map.
Step S23: and calculating each corresponding relation by adopting a least square method so as to correct the target position.
In the embodiment of the application, the positioning device adopts LM (Levenberg-Marquardt) least square optimization to solve the matching relation, and calculates the cost function between the point cloud and the map. Wherein, the lower the cost of the cost function, the better the corresponding matching result. Thus, the positioning device may determine the optimal target position in the map points according to the cost values of the cost function.
Specifically, the cost function (cost function) between the point cloud and the map matching is embodied as:
Figure BDA0003313324590000111
wherein R is a rotation matrix, T is a translation amount, PiAs points in the point cloud, QiThe matching points on the map and the point cloud are obtained.
Step S24: and positioning the current moment according to the corrected target position.
In the embodiment of the present application, the positioning apparatus uses the optimum target position determined in step S23 as the positioning initialization position of the autonomous vehicle.
Furthermore, the positioning equipment can also calculate the optimal target position for the continuously acquired point clouds of a plurality of frames of laser radars, and construct an initialization position interpolator; then, interpolation processing is carried out on the system time when the system time is started at the next moment by using the initialization position interpolator, so that a more accurate initialization position is provided for the automatic driving vehicle in motion, and the problem of poor RTK signal of a partially closed area is solved.
It will be understood by those skilled in the art that in the method of the present invention, the order of writing the steps does not imply a strict order of execution and any limitations on the implementation, and the specific order of execution of the steps should be determined by their function and possible inherent logic.
To implement the positioning method of the above embodiment, the present application further provides a positioning apparatus, and specifically refer to fig. 5, where fig. 5 is a schematic structural diagram of an embodiment of the positioning apparatus provided in the present application.
As shown in fig. 5, the positioning apparatus 500 provided by the present application includes a point cloud obtaining module 51, a point cloud matching module 52, a distance obtaining module 53, a point cloud deleting module 54, and a position positioning module 55.
The point cloud obtaining module 51 is configured to collect a point cloud of an area where the positioning apparatus is located at a current time.
A point cloud matching module 52, configured to match the point cloud with a map, so as to determine a target location point in the map that matches the point cloud.
And the distance obtaining module 53 is configured to determine whether a distance between each data point in the point cloud and the corresponding target location point is smaller than a preset distance.
And the point cloud deleting module 54 is configured to delete all or part of the data points in the point cloud, which are located at a distance greater than or equal to the preset distance from the corresponding target location point, when the distance between the corresponding target location point and the point cloud is greater than or equal to the preset distance.
And the position positioning module 55 is configured to match the point cloud after the data point deletion with the map until the distance between each data point in the point cloud after the data point deletion and the corresponding target position point is smaller than the preset distance, and determine the initial position of the positioning device according to the target position point corresponding to the point cloud after the data point deletion.
To implement the positioning method of the above embodiment, the present application further provides another positioning apparatus, and specifically refer to fig. 6, where fig. 6 is a schematic structural diagram of another embodiment of the positioning apparatus provided in the present application.
The positioning apparatus 600 of the embodiment of the present application includes a memory 61 and a processor 62, wherein the memory 61 and the processor 62 are coupled.
The memory 61 is used for storing program data, and the processor 62 is used for executing the program data to implement the positioning method according to the above embodiment.
In the present embodiment, the processor 62 may also be referred to as a CPU (Central Processing Unit). The processor 62 may be an integrated circuit chip having signal processing capabilities. The processor 62 may also be a general purpose processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic, discrete hardware components. A general purpose processor may be a microprocessor or the processor 62 may be any conventional processor or the like.
Further, the positioning apparatus 600 of the embodiment of the present application further includes a laser radar 63, where the laser radar 63 is connected to the processor 62, and is configured to collect point cloud data and transmit the point cloud data to the processor 62.
To implement the positioning method of the foregoing embodiment, the present application further provides a vehicle 700, and specifically refer to fig. 7, and fig. 7 is a schematic structural diagram of an embodiment of the vehicle provided in the present application.
The vehicle 700 of the embodiment of the present application includes the positioning device 71, and the positioning method of the above embodiment is implemented by the positioning device 71. The specific structure of the positioning device 71 is the same as the positioning device 600 described in the above embodiment, and is not described herein again.
To implement the positioning method of the above embodiment, the present application further provides a computer-readable storage medium, as shown in fig. 8, the computer-readable storage medium 800 is used for storing program data 81, and the program data 81, when executed by the processor, is used to implement the positioning method of the above embodiment.
The present application further provides a computer program product, wherein the computer program product comprises a computer program operable to cause a computer to execute the positioning method according to the embodiments of the present application. The computer program product may be a software installation package.
The positioning method according to the above embodiments of the present application, when implemented in the form of a software functional unit and sold or used as a standalone product, may be stored in a device, for example, a computer readable storage medium. Based on such understanding, the technical solution of the present application may be substantially implemented or contributed by the prior art, or all or part of the technical solution may be embodied in a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) or a processor (processor) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above description is only for the purpose of illustrating embodiments of the present application and is not intended to limit the scope of the present application, and all modifications of equivalent structures and equivalent processes, which are made by the contents of the specification and the drawings of the present application or are directly or indirectly applied to other related technical fields, are also included in the scope of the present application.

Claims (14)

1. A positioning method, characterized in that the positioning method comprises:
collecting point clouds of an area where the positioning equipment is located at the current moment;
matching the point cloud with a map to determine a target location point in the map that matches the point cloud;
judging whether the distance between each data point in the point cloud and the corresponding target position point is smaller than a preset distance;
if not, deleting the data points, of which the distance between the point cloud and the corresponding target position point is greater than or equal to the preset distance, from the point cloud;
and matching the point cloud after the data points are deleted with the map until the distance between each data point in the point cloud after the data points are deleted and the corresponding target position point is smaller than the preset distance, and confirming the initial position of the positioning equipment according to the target position point corresponding to the point cloud after the data points are deleted.
2. The positioning method according to claim 1,
the positioning method further comprises the following steps:
and when the distance between each data point in the point cloud and the corresponding target position point is smaller than the preset distance, confirming the initialization position of the positioning equipment according to the target position point corresponding to the point cloud.
3. The positioning method according to claim 1,
the deleting the data points of which the distance between the point cloud and the corresponding target position point is greater than or equal to the preset distance comprises the following steps:
and deleting data points with a preset proportion in the data points with the distance between the point cloud and the corresponding target position point being more than or equal to the preset distance.
4. The positioning method according to claim 3,
the positioning method further comprises the following steps:
sorting the data points, which are positioned in the point cloud and have a distance greater than or equal to the preset distance from the corresponding target position point, according to the distance;
and deleting data points with preset proportion from the data points with the distance larger than the preset distance according to the distance from large to small.
5. The positioning method according to claim 1,
the matching the point cloud with a map to determine a target location point in the map that matches the point cloud, comprising:
determining a search step length;
searching an area meeting preset matching requirements with the point cloud in the map as a target area according to the searching step length;
determining a set of map points for the target area in the map;
searching a map point closest to each data point in the point cloud in the map point set, and establishing a corresponding relation;
and determining a target position point matched with the point cloud in the map point set based on the corresponding relation.
6. The positioning method according to claim 5,
according to the search step length, searching an area which meets the preset matching requirement with the point cloud in the map as a target area, wherein the step of searching comprises the following steps:
reducing the search step size to update the search step size;
and searching an area meeting preset matching requirements with the point cloud in the target area and the adjacent area of the target area based on the updated search step length so as to update the target area.
7. The positioning method according to claim 6,
searching an area meeting preset matching requirements with the point cloud in the target area and the adjacent area of the target area based on the updated search step length so as to update the target area, wherein the step of updating comprises the following steps:
judging whether the updated search step length is a preset minimum step length;
and if so, determining the target area corresponding to the current search step length as the final target area.
8. The positioning method according to claim 5,
the determining the search step comprises:
determining a pyramid level, and determining a minimum step size;
and determining an initial search step size according to the pyramid level and the minimum step size.
9. The positioning method according to claim 5,
according to the search step length, searching an area which meets the preset matching requirement with the point cloud in the map as a target area, wherein the step of searching comprises the following steps:
dividing the map into a plurality of search areas based on the search step length;
determining the number of matching points matched with the point cloud in each search area;
and determining the matching degree of the search area and the point cloud according to the ratio of the number of the matching points to the total number of the points in the search area.
10. The positioning method according to claim 5,
the determining, based on the correspondence, a target location point matching the point cloud in the set of map points comprises:
calculating each corresponding relation by adopting a least square method so as to correct the target position point;
and determining the corrected target position point in the map point set.
11. The positioning method according to claim 5 or 10,
for each data point in the point cloud, searching a map point closest to the point in the map point set, and establishing a corresponding relation, wherein the method comprises the following steps:
establishing a binary tree by using the map point set in a preset range with the target position point in the map as the center;
and searching the map point closest to the point in the binary tree for each data point in the point cloud, and establishing a corresponding relation.
12. A positioning device, characterized in that the positioning device comprises a processor and a memory, in which program data are stored, the processor being adapted to execute the program data for implementing the positioning method according to any of claims 1-11.
13. A vehicle characterized in that it comprises a positioning device according to claim 12.
14. A computer-readable storage medium for storing program data, which when executed by a processor, is adapted to implement the positioning method of any one of claims 1-11.
CN202111223000.4A 2021-10-20 2021-10-20 Positioning method, positioning apparatus, vehicle, and computer-readable storage medium Pending CN114200462A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111223000.4A CN114200462A (en) 2021-10-20 2021-10-20 Positioning method, positioning apparatus, vehicle, and computer-readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111223000.4A CN114200462A (en) 2021-10-20 2021-10-20 Positioning method, positioning apparatus, vehicle, and computer-readable storage medium

Publications (1)

Publication Number Publication Date
CN114200462A true CN114200462A (en) 2022-03-18

Family

ID=80646233

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111223000.4A Pending CN114200462A (en) 2021-10-20 2021-10-20 Positioning method, positioning apparatus, vehicle, and computer-readable storage medium

Country Status (1)

Country Link
CN (1) CN114200462A (en)

Similar Documents

Publication Publication Date Title
US11624827B2 (en) Method for generating a high precision map, apparatus and storage medium
CN108319655B (en) Method and device for generating grid map
CN111210429B (en) Point cloud data partitioning method and device and obstacle detection method and device
CN112526513B (en) Millimeter wave radar environment map construction method and device based on clustering algorithm
US8199977B2 (en) System and method for extraction of features from a 3-D point cloud
CN111582054B (en) Point cloud data processing method and device and obstacle detection method and device
CN112179330A (en) Pose determination method and device of mobile equipment
CN111968229A (en) High-precision map making method and device
CN111553946B (en) Method and device for removing ground point cloud and method and device for detecting obstacle
WO2021035618A1 (en) Point cloud segmentation method and system, and movable platform
CN112700479B (en) Registration method based on CNN point cloud target detection
CN111308500B (en) Obstacle sensing method and device based on single-line laser radar and computer terminal
CN112505671B (en) Millimeter wave radar target positioning method and device under GNSS signal missing environment
CN113096181B (en) Method and device for determining equipment pose, storage medium and electronic device
WO2024012211A1 (en) Autonomous-driving environmental perception method, medium and vehicle
CN114648640B (en) Target object monomer method, device, equipment and storage medium
WO2024012212A1 (en) Environmental perception method, domain controller, storage medium, and vehicle
CN112130165A (en) Positioning method, positioning device, positioning medium and unmanned equipment
CN115265519A (en) Online point cloud map construction method and device
JP2022087822A (en) Radar tracking method, noise removal method, device and instrument
WO2022099620A1 (en) Three-dimensional point cloud segmentation method and apparatus, and mobile platform
CN114509774A (en) Positioning method, positioning system, vehicle, and computer-readable storage medium
CN117269952A (en) Method and device for semi-automatically labeling moving target point cloud of 4D imaging millimeter wave radar
CN114200462A (en) Positioning method, positioning apparatus, vehicle, and computer-readable storage medium
CN117053779A (en) Tightly coupled laser SLAM method and device based on redundant key frame removal

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