WO2020237996A1 - 一种车辆位姿的修正方法和装置 - Google Patents

一种车辆位姿的修正方法和装置 Download PDF

Info

Publication number
WO2020237996A1
WO2020237996A1 PCT/CN2019/113486 CN2019113486W WO2020237996A1 WO 2020237996 A1 WO2020237996 A1 WO 2020237996A1 CN 2019113486 W CN2019113486 W CN 2019113486W WO 2020237996 A1 WO2020237996 A1 WO 2020237996A1
Authority
WO
WIPO (PCT)
Prior art keywords
particle
vehicle body
target
preset
map
Prior art date
Application number
PCT/CN2019/113486
Other languages
English (en)
French (fr)
Inventor
李江龙
穆北鹏
Original Assignee
魔门塔(苏州)科技有限公司
北京初速度科技有限公司
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 魔门塔(苏州)科技有限公司, 北京初速度科技有限公司 filed Critical 魔门塔(苏州)科技有限公司
Publication of WO2020237996A1 publication Critical patent/WO2020237996A1/zh

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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Definitions

  • the invention relates to the technical field of automatic driving, in particular to a method and device for correcting vehicle pose.
  • the embodiment of the invention discloses a vehicle pose correction method and device, which solves the problem of low positioning accuracy using a consumer-level preset positioning device, and realizes the technical effect of centimeter-level high-precision positioning of the vehicle.
  • an embodiment of the present invention discloses a method for correcting the pose of a vehicle, the method including:
  • particle sampling of the vehicle position information is performed based on the prior position of the vehicle body; wherein the prior position is obtained by the preset positioning device ;
  • the pose of the vehicle body at the prior position is optimized based on the target matching relationship.
  • the particle sampling of vehicle position information based on the prior position of the vehicle body includes:
  • the vehicle position information is sampled by particles in 2D space.
  • the particle sampling of vehicle position information based on the prior position of the vehicle body includes:
  • particle sampling of vehicle position information is performed in the three-dimensional space where the vehicle body is located to obtain particles with a 3D probability distribution.
  • update the pose of the particles obtained by sampling and the weight information corresponding to each particle including:
  • the weight information corresponding to each particle is updated.
  • projecting all map elements in the preset navigation map that meets a preset distance requirement from the location information onto the perception image includes:
  • the coordinate axis corresponding to the map element that meets the fourth set distance is used as the key, and the identifier corresponding to the map element is used as the value. Sort the map elements in the fourth set range;
  • updating the weight information corresponding to each particle includes:
  • determining the state quantity of the vehicle body position according to the updated weight information of each target particle includes:
  • x k represents the state quantity of the vehicle body position at time k; Is the weight of particle i at time k.
  • obtaining the target matching relationship between the perceived image and the preset navigation map based on the state quantity and the vehicle body posture includes:
  • the method further includes:
  • particle resampling is performed according to the updated weight value until the positions of the set number of target particles meet the preset convergence condition.
  • an embodiment of the present invention also provides a vehicle pose correction device, which includes:
  • the particle sampling module is configured to perform particle sampling of vehicle position information based on the prior position of the vehicle body when it is detected that there is a coverage area corresponding to the prior position of the vehicle body in the preset navigation map; wherein, the prior The position is obtained by the preset positioning device;
  • the weight update module is configured to update the pose of the sampled particles and the weight information corresponding to each particle, so that the positions of the set number of target particles meet the preset convergence condition;
  • the target matching relationship establishment module is configured to determine the state quantity of the vehicle body position according to the updated weight information of each target particle, and obtain the relationship between the perception image and the preset navigation map based on the state quantity and the vehicle body posture.
  • Target matching relationship
  • the pose optimization module is configured to optimize the pose of the vehicle body at the prior position based on the target matching relationship.
  • the particle sampling module is specifically configured as:
  • each target lane line that meets the first set distance from the prior position of the vehicle body is extracted ;
  • the vehicle position information is sampled by particles in 2D space.
  • the particle sampling module is specifically configured as:
  • the vehicle position is performed in the three-dimensional space where the vehicle body is located at a position that meets the third set distance from the priori position of the vehicle body Sampling the particles of information to obtain particles with a 3D probability distribution.
  • the weight update module includes:
  • the position information determining unit is configured to determine the position information of any particle obtained by sampling at the current moment
  • a projection unit configured to project all map elements in the preset navigation map that meet the preset distance requirement with the location information on the perception image based on the location information;
  • An initial matching relationship establishment unit configured to establish an initial matching relationship between the map element and each perceptual element in the perceptual image according to the magnitude of the reprojection residual
  • the weight information update unit is configured to update the weight information corresponding to each particle based on the initial matching relationship.
  • the projection unit is specifically configured as:
  • the coordinate axis corresponding to the map element that meets the fourth set distance is used as the key, and the identifier corresponding to the map element is used as the value. Sort the map elements in the fourth set range;
  • the weight information update unit is specifically configured as:
  • the target matching relationship establishment module is specifically configured as:
  • the device further includes:
  • the effective particle detection module is configured to detect whether the number of effective particles in the target particles reaches a second preset number threshold if the positions of the set number of target particles do not meet the preset convergence condition;
  • a re-sampling module configured to, if the number of effective particles does not reach the second preset number threshold, perform particle re-sampling according to the updated weight value until the positions of the set number of target particles meet the preset convergence condition .
  • an embodiment of the present invention also provides a vehicle-mounted terminal, including:
  • a memory storing executable program codes
  • a processor coupled with the memory
  • the processor calls the executable program code stored in the memory to execute part or all of the steps of a vehicle pose correction provided by any embodiment of the present invention.
  • an embodiment of the present invention also provides a computer-readable storage medium that stores a computer program.
  • the computer program includes part or all of the vehicle pose correction method provided by any embodiment of the present invention. Step instructions.
  • the embodiments of the present invention also provide a computer program product, which when the computer program product runs on a computer, causes the computer to execute part of the vehicle pose correction method provided by any embodiment of the present invention Or all steps.
  • the particle sampling of the vehicle position information is performed based on the prior position of the vehicle body to use Multiple particle states represent the current positioning results of the vehicle, which can greatly improve the success rate and stability of subsequent matching of perception elements and map elements.
  • the positions of the set number of target particles can be converged to a relatively small range, so that the weight information of the target particles can be used to determine the state of the vehicle body position, and
  • the target matching relationship between the perception image and the preset navigation map is obtained based on the state quantity and the car body posture.
  • the target matching relationship can be used to optimize the pose of the car body at the prior position.
  • the method provided in this embodiment avoids the fact that the arrow information on the road surface is too sparse to ensure a continuous centimeter-level height.
  • the problem of fine positioning is solved by optimizing the pose of the car body at the prior position based on the target matching relationship, which solves the problem of low positioning accuracy using consumer-grade preset positioning devices, which can make the pose of the vehicle reach centimeter-level positioning Accuracy.
  • the invention points of the present invention include:
  • the current positioning result of the vehicle can be characterized by multiple particle states, which greatly improves the success rate and stability of subsequent matching of perception elements and map elements.
  • the positions of the set number of target particles can be converged to a relatively small range, so that the weight information of the target particles can be used to determine the state of the vehicle body position, and
  • the target matching relationship between the perception image and the navigation map is obtained based on the state quantity and the posture of the vehicle body.
  • the target matching relationship can be used to optimize the pose of the car body at the prior position, which solves the problem that the arrow information on the road surface is too sparse when the existing technology uses the Kalman filter to fuse GPS and arrow information in the map for combined positioning.
  • the problem that continuous centimeter-level high-precision positioning cannot be guaranteed realizes the technical effect of centimeter-level high-precision positioning for vehicles.
  • the technical solution provided by the embodiment of the present invention solves the problem of the large number of particles when sampling in the three-dimensional space by combining the priori position of the car body and the lane line in the preset navigation map to sample the car body position in 2D space.
  • the number of particles is reduced, and the time efficiency of the algorithm is greatly improved.
  • Figure 1 is a schematic diagram of system state switching provided by an embodiment of the present invention.
  • 2a is a schematic flowchart of a method for correcting vehicle pose provided by an embodiment of the present invention
  • Figure 2b is a schematic diagram of a particle sampling provided by an embodiment of the present invention.
  • Figure 2c is another schematic diagram of particle sampling provided by an embodiment of the present invention.
  • 3a is a schematic flowchart of a method for correcting vehicle pose provided by an embodiment of the present invention.
  • 3b is a schematic diagram of a search range of map elements provided by an embodiment of the present invention.
  • FIG. 4 is a schematic flowchart of a method for correcting vehicle pose provided by an embodiment of the present invention.
  • FIG. 5 is a schematic structural diagram of a vehicle pose correction device provided by an embodiment of the present invention.
  • Fig. 6 is a schematic structural diagram of a vehicle-mounted terminal provided by an embodiment of the present invention.
  • the main purpose of the embodiments of the present invention is: an unmanned vehicle uses a positioning module composed of an inexpensive IMU (Inertial Measurement Unit), a camera, and a GPS (Global Positioning System) to input about 10m into the system The a priori position of the absolute position error.
  • IMU Inertial Measurement Unit
  • GPS Global Positioning System
  • the system uses a series of algorithms to combine the map elements such as street light poles, traffic signs, lane lines, and dotted endpoints in the preset navigation map with no one.
  • the sensing elements such as street light poles, traffic signs, lane lines, and dotted endpoints on the image obtained by the driving camera are matched one-to-one, and the correct sensing element and map element matching pair are output to form the perception image and the preset navigation map Correct matching relationship.
  • the vehicle body pose can reach centimeter-level positioning accuracy.
  • the algorithm can also provide continuous high-precision positioning after the above-mentioned initial matching is completed.
  • the system specifically includes the following states when performing the above process:
  • Fig. 1 is a schematic diagram of system state switching provided by an embodiment of the present invention, as shown in Fig. 1:
  • the initial initialization state represents the initial stage of a complete initialization matching process. This stage mainly determines whether the priori position of the vehicle is in the area covered by the preset navigation map. If it is in the map area, the vehicle state particle Take samples and switch to the initializing state; if not, continue to maintain the initializing state.
  • Initialization state which means that according to the information perceived on the image and the street lamp poles, traffic signs, lane lines and dotted line endpoints in a certain range in the map, update the weight of particles, resample, update the matching relationship, and detect the state of particles Whether to converge, etc. If the particles converge, switch the state to the initialization completed state; if the particles do not converge, continue to maintain the initializing state; if the perceived information does not have any matching relationship with the elements in the map, switch the state to the initial initialization status.
  • Matching state which means that you can directly use the pose information of the vehicle body to obtain the correct matching relationship between the perception element and the map element. But if there is a matching error in the continuous frame perception image, the matching state is switched to the initial initialization state.
  • FIG. 2a is a schematic flowchart of a method for correcting vehicle pose provided by an embodiment of the present invention.
  • This method is used in automatic driving. This method is typically applied to the scene where the vehicle enters the area covered by the preset navigation map for the first time and corresponds to the prior position. Its main task is to ensure that the vehicle body position accuracy is not accurate. In the case of high, a correct target matching relationship between the perceived image and the preset navigation map is generated, so that the target matching relationship is used to optimize the car body pose with centimeter-level positioning accuracy.
  • the method provided in this embodiment can be executed by a vehicle pose correction device, which can be implemented by software and/or hardware, and can generally be integrated in vehicle terminals such as vehicle computers, vehicle industrial personal computers (IPC), etc. In the embodiment of the present invention, it is not limited. As shown in Figure 2a, the method provided in this embodiment specifically includes:
  • the priori position of the vehicle body is obtained through a preset positioning device.
  • the preset positioning device is a low-precision consumer-grade positioning device such as single-point GPS (Global Positioning System), IMU, and camera.
  • the navigation map refers to a high-precision navigation map with an error level of centimeters for automatic driving.
  • the high-precision navigation map has 3D location information of elements such as traffic signs, street light poles, lane lines and dotted line endpoints of lane lines.
  • the presence of the coverage area corresponding to the priori position of the vehicle body in the preset navigation map means that the preset navigation map is within the set range of the priori position of the vehicle, for example, within a range of more than ten meters.
  • the map elements can be traffic signs, street light poles, lane lines or dotted end points of lane lines, etc. If no map element is found in the preset navigation map, it means that there is no coverage area corresponding to the priori position of the car body in the preset navigation map, that is, the current vehicle has not driven into the preset navigation map. Range, the initial initialization state of the system needs to be maintained at this time.
  • the detection of the coverage of the preset navigation map is performed after several frames of images are spaced apart. In this way, the method of performing detection relative to one frame of image each time is set to improve the calculation efficiency.
  • particle sampling may be performed based on the vehicle position information.
  • Each position obtained by sampling is a particle, and each particle is a possible position of a vehicle body.
  • the attitude of the car body can be kept unchanged, because the error of the attitude of the car body under the GPS heading information and the gravity observation information of the IMU is relatively small, and the error mainly occurs in the position of the vehicle.
  • the particle sampling of vehicle position information based on the prior position of the vehicle body may specifically be:
  • particle sampling of vehicle position information can be performed in the three-dimensional space where the vehicle body is located according to the Gaussian distribution model to obtain particles with a 3D probability distribution.
  • the three-dimensional sampling in this embodiment is mainly applied to the situation where the lane line is not retrieved in the preset navigation map.
  • the particle sampling of the vehicle position information based on the prior position of the vehicle body may specifically be:
  • each target lane line that meets the first set distance from the priori position of the vehicle body for example, the range of 15 meters from the front, rear, left and right of the vehicle body to the priori position of the vehicle body
  • a second set distance for example, two meters
  • Fig. 2b is a schematic diagram of a particle sampling provided by an embodiment of the present invention.
  • 1 and 2 indicate the target lane line that meets the first set distance from the prior position of the vehicle body, and the target vehicle line
  • the black dots indicate the discrete points of the lane line, and multiple particles can be sampled within the second set distance from each discrete point as possible positions of the vehicle body.
  • Fig. 2c is a schematic diagram of another particle sampling provided by an embodiment of the present invention.
  • the position of the lane line can be used, at the center of every two lane lines (1 and 2, or 2 and 3). Perform particle sampling at points to obtain a series of sampled particles.
  • the particle sampling in the 2D space mentioned above combines the prior position of the car body and the road surface information of the preset navigation map, and uses the lane lines in the preset navigation map to perform on the 2-dimensional plane around the discrete points of the lane lines Random sampling effectively reduces the dimensionality of the particle sampling space, reduces the number of particles, and greatly improves the time efficiency of the algorithm.
  • the single-point GPS elevation error can sometimes reach more than ten meters, and the latitude and longitude error is also about 10 meters, if the prior position of the car body is covered by 10 meters, the elevation is 15 meters above and below, and the particles in 3D space are used.
  • Sampling method according to the calculation of one particle per cubic meter, 20*20*30 requires 12000 particles.
  • the particle sampling in the 2D space mentioned above uses the lane lines in the preset navigation map.
  • the height of the car body can be determined by the height of the lane line. When the discrete points of the lane line are used to sample the car body position particles, there is no need to target The height dimension is used for sampling. Therefore, the sampling dimension can be reduced from three-dimensional space sampling to two-dimensional, which greatly reduces the number of particles and improves the calculation speed of the algorithm.
  • the posture of the particle can be the posture of the vehicle at the prior position
  • the change of the position of the particle is a process of continuous iteration, which can be updated according to the following formula:
  • the position information of any particle at the current moment can be determined.
  • all map elements corresponding to the location information in the preset navigation map can be searched, thereby establishing an initial matching relationship between the map elements and the elements in the perception image, so as to update the weight information of the particles according to the initial matching relationship.
  • the process of updating the weight information corresponding to each particle may specifically be:
  • the map elements that meet the preset distance requirement in the preset navigation map and the current moment particle position information are projected onto the perception image, and reprojected according to the The size of the residual establishes the initial matching relationship between the map elements and the perception elements in the perception image; based on the initial matching relationship, the weight information corresponding to each particle is updated.
  • the perception image is obtained after recognizing the image containing road information collected by the camera using a preset perception model.
  • the preset perception model can use a large number of road sample images marked with image semantic features to train the perception model in advance.
  • the semantic features of the image may include traffic signs, street light poles, lane lines, and dotted endpoints of lane lines.
  • the preset perception model can be obtained in the following ways:
  • the training sample set includes multiple sets of training sample data, each set of training sample data includes road sample images and corresponding road perception sample images marked with image semantic features; based on the training sample set to build the initial neural network
  • a preset perception model is obtained through training, and the preset perception model makes the road sample images in each set of training sample data associated with corresponding road perception sample images marked with image semantic features.
  • the output of the model can be called perceptual image.
  • the various road information in the perceptual image can be called perceptual elements.
  • the size of the reprojection residual between the map element and the corresponding perception element needs to meet certain threshold requirements.
  • the more accurate the initial matching relationship is established the greater the weight information of the particles obtained according to the initial matching relationship.
  • the size of the weight information of the particles updated based on the initial matching relationship can also reflect the accuracy of the establishment of the initial matching relationship.
  • the larger the weight value the more accurate the establishment of the initial matching relationship.
  • particles whose weight values are lower than the set threshold and their corresponding matching relationships can be filtered.
  • the positions of the target particles can be more and more converged until the positions of the set number of target particles meet the preset convergence condition.
  • the preset convergence condition means that the position variance of the target particle is less than the set threshold.
  • the vehicle attitude is a priori attitude of the vehicle.
  • the state quantity of the vehicle body position can be determined according to the updated weight information of each target particle according to the following formula:
  • x k represents the state quantity of the vehicle body position at time k; Is the weight of particle i at time k.
  • obtaining the target matching relationship between the perception image and the preset navigation map based on the state quantity and the vehicle body posture refers to: based on the current vehicle pose, a sufficient number of map elements and perception images in the preset navigation map
  • Corresponding perception elements in can establish a one-to-one matching relationship, and when the reprojection residuals of each matching pair meet the preset threshold requirements, it can indicate that the target matching relationship is established between the perception image and the preset navigation map.
  • a sufficient number of matching pairs can provide six degrees of freedom constraints for the vehicle. At this time, it can indicate that the system is initialized successfully and the system enters the matching state. Conversely, if the system initialization is unsuccessful, it returns to step 110, and the system enters the initial initialization state again.
  • the pose of the car body at the prior position can be optimized based on the target matching relationship with six degrees of freedom.
  • the specific optimization process can be realized by using a nonlinear optimization algorithm, so that centimeter-level positioning can be obtained. Accuracy.
  • the external input movement increment and the centimeter-level positioning position maintained inside the vehicle system can be used to obtain more accurate results according to the size of the reprojection residuals of the sensing elements and map elements.
  • the target matching relationship is continuously used to optimize the pose of the vehicle.
  • the positions of the set number of target particles can be converged to a relatively small range, so that the weight information of the target particles can be used to determine the state of the vehicle body position, and
  • the target matching relationship between the perception image and the preset navigation map is obtained based on the state quantity and the car body posture, so as to optimize the pose of the car body at the prior position based on the target matching relationship.
  • the method provided in this embodiment avoids that the arrow information on the road surface is too sparse and cannot guarantee continuous centimeter-level height.
  • the pose of the vehicle can reach centimeter-level positioning accuracy.
  • FIG. 3a is a flowchart of a method for correcting vehicle pose provided by an embodiment of the present invention. This embodiment optimizes the search process of the map elements projected on the perception image on the basis of the above embodiments. As shown in Figure 3a, the method includes:
  • the camera coordinate system is defined as the x-axis facing right, the y-axis facing down, and the z-axis facing forward.
  • the setting direction of the camera coordinate system refers to the direction in which the Z axis in front of the camera is forward, that is, the positive direction of the Z axis.
  • the coordinate axis corresponding to the map element meeting the fourth setting distance is used as the key
  • the identifier corresponding to the map element is used as the value
  • the key-value pair information is constructed to satisfy the fourth setting
  • the range of map elements are sorted.
  • the key-value pair information MAP may be a red-black tree or other binary tree data structure.
  • the advantage of constructing key-value pair information is that the map element in front of the particle in the Z-axis direction can be directly searched, thereby reducing the number of subsequent invalid reprojections and improving the time efficiency of the algorithm.
  • Fig. 3b is a schematic diagram of a search range of a map element provided by an embodiment of the present invention. As shown in Figure 3b, z1 represents the relative distance between the particle and the prior position of the vehicle body, and z2 represents the relative distance between the map element and the prior position of the vehicle body.
  • the range in which the distance in front of the particle along the Z axis is less than d1 can be selected as the search range.
  • the range in which the distance in front of the particle along the Z axis is less than d2 can be selected as the search range.
  • the arrangement of the street light poles on the road surface is relatively sparse relative to the end point of the dashed line of the lane, it may be preferably set to d1 greater than d2.
  • this embodiment constructs the key-value pair information of the direction set by the map element in the camera coordinate system by using the relative position relationship between the map element and the prior position of the car body, so that each particle can According to the relative relationship with the prior position of the car body, the map elements in front of the particles are projected directly from the key value pair information.
  • this embodiment excludes map elements that cannot be projected into the perception image, thereby reducing the number of invalid reprojections and improving the time efficiency of the algorithm.
  • FIG. 4 is a schematic flowchart of a method for correcting vehicle pose provided by an embodiment of the present invention.
  • this embodiment updates the weight information corresponding to each particle and perceives the image
  • the process of establishing the target matching relationship with the preset navigation map has been optimized.
  • the method provided in this embodiment specifically includes:
  • the positions of the set number of target particles do not meet the preset convergence condition, it is detected whether the number of effective particles in the target particles reaches the second preset number threshold. If the number of effective particles does not reach the second preset number threshold, particle resampling is performed according to the updated weight value until the positions of the set number of target particles meet the preset convergence condition.
  • the target matching relationship is established with respect to the initial matching relationship in the foregoing embodiment, and it is necessary to ensure that the number of matching pairs is sufficiently large, that is, to meet the first preset number threshold, and the reprojection residual of each set of matching pairs should also be less than
  • the initial matching relationship establishment is the corresponding re-projection residuals, that is, all meet the preset threshold requirements, so as to provide the vehicle with six degrees of freedom constraints.
  • FIG. 5 is a schematic structural diagram of a vehicle pose correction device provided by an embodiment of the present invention.
  • the device includes: a particle sampling module 410, a weight update module 420, a target matching relationship establishment module 430, and a pose optimization module 440; among them,
  • the particle sampling module 410 is configured to perform particle sampling of vehicle position information based on the priori position of the vehicle body when it is detected that there is a coverage area corresponding to the priori position of the vehicle body in the preset navigation map; The inspection position is obtained by the preset positioning device;
  • the weight update module 420 is configured to update the pose of the sampled particles and the weight information corresponding to each particle, so that the positions of the set number of target particles meet the preset convergence condition;
  • the target matching relationship establishment module 430 is configured to determine the state quantity of the vehicle body position according to the updated weight information of each target particle, and obtain the relationship between the perception image and the preset navigation map based on the state quantity and the vehicle body posture. Target matching relationship;
  • the pose optimization module 440 is configured to optimize the pose of the vehicle body at the prior position based on the target matching relationship.
  • the positions of the set number of target particles can be converged to a relatively small range, so that the weight information of the target particles can be used to determine the state of the vehicle body position, and
  • the target matching relationship between the perception image and the preset navigation map is obtained based on the state quantity and the vehicle body posture, so as to optimize the pose of the vehicle body at the prior position based on the target matching relationship.
  • the method provided in this embodiment avoids that the arrow information on the road surface is too sparse and cannot guarantee continuous centimeter-level height.
  • the pose of the vehicle can reach centimeter-level positioning accuracy.
  • the particle sampling module is specifically configured as:
  • each target lane line that meets the first set distance from the prior position of the vehicle body is extracted ;
  • the vehicle position information is sampled by particles in 2D space.
  • the particle sampling module is specifically configured as:
  • the vehicle position is performed in the three-dimensional space where the vehicle body is located at a position that meets the third set distance from the priori position of the vehicle body Sampling the particles of information to obtain particles with a 3D probability distribution.
  • the weight update module includes:
  • the position information determining unit is configured to determine the position information of any particle obtained by sampling at the current moment
  • a projection unit configured to project all map elements in the preset navigation map that meet the preset distance requirement with the location information on the perception image based on the location information;
  • An initial matching relationship establishment unit configured to establish an initial matching relationship between the map element and each perceptual element in the perceptual image according to the magnitude of the reprojection residual
  • the weight information update unit is configured to update the weight information corresponding to each particle based on the initial matching relationship.
  • the projection unit is specifically configured as:
  • the coordinate axis corresponding to the map element that meets the fourth set distance is used as the key, and the identifier corresponding to the map element is used as the value. Sort the map elements in the fourth set range;
  • the weight information update unit is specifically configured as:
  • the target matching relationship establishment module is specifically configured as:
  • the device further includes:
  • the effective particle detection module is configured to detect whether the number of effective particles in the target particles reaches a second preset number threshold if the positions of the set number of target particles do not meet the preset convergence condition;
  • a re-sampling module configured to, if the number of effective particles does not reach the second preset number threshold, perform particle re-sampling according to the updated weight value until the positions of the set number of target particles meet the preset convergence condition .
  • the device for correcting vehicle pose provided by the embodiment of the present invention can execute the method for correcting vehicle pose provided by any embodiment of the present invention, and has functional modules and beneficial effects corresponding to the execution method.
  • the method for correcting vehicle pose provided in any embodiment of the present invention can execute the method for correcting vehicle pose provided by any embodiment of the present invention, and has functional modules and beneficial effects corresponding to the execution method.
  • FIG. 6 is a schematic structural diagram of a vehicle-mounted terminal according to an embodiment of the present invention.
  • the vehicle-mounted terminal may include:
  • a memory 701 storing executable program codes
  • a processor 702 coupled with the memory 701;
  • the processor 702 calls the executable program code stored in the memory 701 to execute the method for correcting the vehicle pose provided by any embodiment of the present invention.
  • the embodiment of the present invention discloses a computer-readable storage medium that stores a computer program, where the computer program causes a computer to execute the vehicle pose correction method provided by any embodiment of the present invention.
  • the embodiment of the present invention discloses a computer program product, wherein when the computer program product runs on a computer, the computer is caused to execute part or all of the steps of the vehicle pose correction method provided by any embodiment of the present invention.
  • B corresponding to A means that B is associated with A, and B can be determined according to A.
  • determining B according to A does not mean that B is determined only according to A, and B can also be determined according to A and/or other information.
  • the functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units may be integrated into one unit.
  • the above-mentioned integrated unit can be implemented in the form of hardware or software functional unit.
  • the aforementioned integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-accessible memory.
  • the essence of the technical solution of the present invention or the part that contributes to the existing technology or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a memory.
  • a computer device which may be a personal computer, a server, or a network device, etc., specifically a processor in a computer device
  • the program can be stored in a computer-readable storage medium.
  • the storage medium includes read-only Memory (Read-Only Memory, ROM), Random Access Memory (RAM), Programmable Read-only Memory (PROM), Erasable Programmable Read Only Memory, EPROM), One-time Programmable Read-Only Memory (OTPROM), Electronically-Erasable Programmable Read-Only Memory (EEPROM), CD-ROM (Compact Disc) Read-Only Memory, CD-ROM) or other optical disk storage, magnetic disk storage, tape storage, or any other computer-readable medium that can be used to carry or store data.
  • Read-Only Memory ROM
  • RAM Random Access Memory
  • PROM Programmable Read-only Memory
  • EPROM Erasable Programmable Read Only Memory
  • OTPROM One-time Programmable Read-Only Memory
  • EEPROM Electronically-Erasable Programmable Read-Only Memory
  • CD-ROM Compact Disc

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

一种车辆位姿的修正方法和装置，该方法包括：当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样（110）；其中，先验位置通过预设定位装置得到；更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件（120）；根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于该状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系（130）；基于目标匹配关系对先验位置处车体的位姿进行优化。通过采用该技术方案，解决了使用消费级预设定位装置定位精度不高的问题，实现了对车辆进行厘米级的高精度定位的技术效果。

Description

一种车辆位姿的修正方法和装置 技术领域
本发明涉及自动驾驶技术领域，具体涉及一种车辆位姿的修正方法和装置。
背景技术
在自动驾驶领域，高精度定位至关重要。近年来，深度学习等技术的成果，极大促进了图像语义分割、图像识别领域的发展，这为高精度地图及高精度定位提供了坚实的基础。
在基于高精度地图的定位方案中，当无人驾驶车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，需要获得一个全局而精准的位置信息进行初始化，继而可以使用高精度地图进行精准定位，即绝对位置精度可达到厘米级。但在消费级设备，例如单点GPS(Global Positioning System，全球定位系统)、相机与廉价IMU(Inertial measurement unit，惯性测量单元)的定位方案中，由于单点GPS所提供的定位精度信息的误差较大，利用单点GPS所提供的位置，将高精度地图中的交通标志信息与利用深度学习感知模型感知出图像中的交通标志信息，例如车道线、路灯杆等，进行重投影匹配时，容易造成车道线左右匹配错误，路灯杆前后向匹配错误等问题。如果利用错误的匹配信息对车体位置进行修正，不仅不能利用高精地图使车体的位置精度达到厘米级，反而可能会使车体的位置与真实位置偏离更大。
现有技术在基于视觉、GPS与高精度地图融合的方法对车辆进行定位时，通常需要使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位。但路面上的箭头信息过于稀疏，无法保证连续的厘米级高精定位。
发明内容
本发明实施例公开一种车辆位姿的修正方法和装置，解决了使用消费级预设定位装置定位精度不高的问题，实现了对车辆进行厘米级的高精度定位的技术效果。
第一方面，本发明实施例公开了一种车辆位姿的修正方法，该方法包括：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
可选的，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
可选的，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
可选的，更新采样得到的粒子的位姿和每个粒子对应的权重信息，包括：
对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上，并根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
基于所述初始匹配关系，更新每个粒子对应的权重信息。
可选的，基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均 投影到感知图像上，包括：
将所述位置信息转换到相机坐标系下，并计算所述位置信息在所述相机坐标系的设定方向上对应的目标位置；
将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到所述相机坐标系下；
在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
可选的，基于所述初始匹配关系，更新每个粒子对应的权重信息，包括：
按照如下公式更新每个粒子对应的权重信息：
Figure PCTCN2019113486-appb-000001
其中，
Figure PCTCN2019113486-appb-000002
是k时刻的粒子i的权重；
Figure PCTCN2019113486-appb-000003
是k-1时刻粒子i的权重；N i为第i个粒子上获得的感知元素和地图元素的匹数数量；f(N i)是N i正相关的函数；s j为每种地图元素对应的一个归一化参数；r j第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上除以了N i将感知图像和导航地图匹配的数量进行了概率归一化。
可选的，根据每个目标粒子更新后的权重信息确定车体位置的状态量，包括：
按照如下公式根据每个目标粒子更新后的权重信息确定车体位置的状态量：
Figure PCTCN2019113486-appb-000004
其中，x k表示k时刻车体位置的状态量；
Figure PCTCN2019113486-appb-000005
是k时刻的粒子i的权重。
可选的，基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系，包括：
基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系
可选的，所述方法还包括：
如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
第二方面，本发明实施例还提供了一种车辆位姿的修正装置，该装置包括：
粒子采样模块，被配置为当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
权重更新模块，被配置为更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
目标匹配关系建立模块，被配置为根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基 于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
位姿优化模块，被配置为基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
可选的，所述权重更新模块，包括：
位置信息确定单元，被配置为对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
投影单元，被配置为基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上；
初始匹配关系建立单元，被配置为根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
权重信息更新单元，被配置为基于所述初始匹配关系，更新每个粒子对应的权重信息。
可选的，所述投影单元，具体被配置为：
将所述位置信息转换到相机坐标系下，并计算所述位置信息在所述相机坐标系的设定方向上对应的目标位置；
将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到所述相机坐标系下；
在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
可选的，所述权重信息更新单元，具体被配置为：
按照如下公式更新每个粒子对应的权重信息：
Figure PCTCN2019113486-appb-000006
其中，
Figure PCTCN2019113486-appb-000007
是k时刻的粒子i的权重；
Figure PCTCN2019113486-appb-000008
是k-1时刻粒子i的权重；N i为第i个粒子上获得的感知元素和地图元素的匹数数量；f(N i)是N i正相关的函数；s j为每种地图元素对应的一个归一化参数；r j第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上除以了N i将感知图像和导航地图匹配的数量进行了概率归一化。
可选的，所述目标匹配关系建立模块，具体被配置为：
根据每个目标粒子更新后的权重信息确定车体位置的状态量，基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系。
可选的，所述装置还包括：
有效粒子检测模块，被配置为如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
重采样模块，被配置为如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
第三方面，本发明实施例还提供了一种车载终端，包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明任意实施例所提供的一种车辆位姿的修正的部分或全部步骤。
第四方面，本发明实施例还提供了一种计算机可读存储介质，其存储计算机程序，所述计算机程序包括用于执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤的指令。
第五方面，本发明实施例还提供了一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤。
本发明实施例提供的技术方案，当车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，通过基于车体的先验位置进行车辆位置信息的粒子采样，以利用多个粒子状态表征车辆的当前定位结果，从而可大大提高后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系，该目标匹配关系可用于对先验位置处车体的位姿进行优化。相对于现有技术提供的使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位的方式，本实施例提供的方式避免了由于路面上的箭头信息过于稀疏而无法保证连续的厘米级高精定位的问题，通过基于目标匹配关系对先验位置处车体的位姿进行优化，解决了使用消费级预设定位装置定位精度不高的问题，可使得车辆的位姿达到厘米级的定位精度。
本发明的发明点包括：
1、通过基于车体的先验位置进行车辆位置信息的粒子采样，以通过多个粒子状态表征车辆的当前定位结果，大大提高了后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和导航地图之间的目标匹配关系。利用目标匹配关系可对先验位置处车体的位姿进行优化，解决了现有技术在使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位时，由于路面上的箭头信息过于稀疏而导致的无法保证连续的厘米级高精定位的问题，实现了对车辆进行厘米级的高精度定位的技术效果。
2、本发明实施例提供的技术方案，通过结合车体先验位置和预设导航地图中车道线对车体位置进行2D空间采样，解决了在三维空间采样时粒子数目较多的问题，有效减少了粒子数量，极大地提升了算法的时间效率。
3、通过利用地图元素与车体先验位置之间的相对位置关系，构建地图元素在相机坐标系下设定方向的键值对信息，可使得每个粒子根据与车体先验位置的相对关系直接从键值对信息中搜索出该粒子前方的地图元素进行投影。解决了采用直接从预设导航地图中搜索地图元素的方式所导致的无法排除不可能投影到感知图像中的地图元素的问题，有效减少了做无效重投影的次数，提升了算法时间效率。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种系统状态切换示意图；
图2a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图；
图2b是本发明实施例提供的一种粒子采样示意图；
图2c是本发明实施例提供的又一种粒子采样示意图；
图3a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图；
图3b是本发明实施例提供的一种地图元素的搜索范围示意图；
图4是本发明实施例提供的一种车辆位姿的修正方法的流程示意图；
图5是本发明实施例提供的一种车辆位姿的修正装置的结构示意图；
图6是本发明实施例提供的一种车载终端的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了更加清楚、明白地解释各实施例的内容，下面先对本发明实施例提供的技术方案的工作原理进行简单介绍：
本发明实施例的主要目的为：无人驾驶车辆利用廉价的IMU(Inertial measurement unit，惯性测量单元)、相机和GPS(Global Positioning System，全球定位系统)构成的定位模块，向本系统输入10m左右绝对位置误差的先验位置。当车辆刚刚驶入预设导航地图所覆盖的与先验位置对应的区域时，系统通过一系列算法将预设导航地图中的路灯杆、交通牌、车道线以及虚线端点等地图元素与无人驾驶相机获取的图像上的路灯杆、交通牌、车道线以及虚线端点等感知元素进行一对一的匹配，输出正确的感知元素与地图元素匹配对，以形成感知图像与预设导航地图之间正确的匹配关系。通过利用正确的匹配关系优化车体位姿，可使车体位姿达到厘米级的定位精度。在上述初始化匹配完成后算法也能提供连续的高精度定位。其中，系统在执行上述过程时具体包括如下几个状态：
(1)开始初始化状态；(2)初始化中状态；(3)初始化完成状态；(4)匹配状态。请参阅图1，图1是本发明实施例提供的一种系统状态切换示意图，如图1所示：
(1)开始初始化状态，表示一个初始化匹配完整流程的起始阶段，该阶段主要判断车辆的先验位置是否在预设导航地图所覆盖的区域内，如果在地图区域内，则进行车辆状态粒子采样，并切换到初始化中状态；如果不在，则继续保持开始初始化状态。
(2)初始化中状态，表示根据图像上感知到的信息与地图中的一定范围内的路灯杆、交通牌、车道线以及虚线端点进行粒子的权重更新、重采样、更新匹配关系、检测粒子状态是否收敛等，如果粒子收敛，则将状态切换到初始化完成状态；如果粒子未收敛，则继续保持初始化中状态；如果感知到的信息与地图中元素无任何匹配关系，则将状态切换到开始初始化状态。
(3)初始化完成状态，表示粒子状态已经收敛，并检测是否可以获得匹配，即判断匹配对数量是否足够以为车辆提供六自由度约束(满足条件A)，如果满足，则将状态切换到匹配状态；如果不满足，则将状态切换到开始初始化状态。
(4)匹配状态，表示可以直接利用车体的位姿信息获得正确的感知元素与地图元素的匹配关系。但如果连续帧感知图像存在匹配错误，则将匹配状态切换到开始初始化状态。
下面，分别对本发明实施例所涉及的各个状态的具体执行过程进行详细介绍。
实施例一
请参阅图2a，图2a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图。该方法应用于自动驾驶中，该方法典型的是应用于车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域范围的场 景下，其主要任务是在车体位置精度不高的情况下来产生正确的感知图像与预设导航地图之间的目标匹配关系，以利用该目标匹配关系优化出厘米级定位精度的车体位姿。本实施例提供的方法可由车辆位姿的修正装置来执行，该装置可通过软件和/或硬件的方式实现，一般可集成在车载电脑、车载工业控制计算机(Industrial personal Computer，IPC)等车载终端中，本发明实施例不做限定。如图2a所示，本实施例提供的方法具体包括：
110、当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样。
其中，车体的先验位置通过预设定位装置得到。预设定位装置为单点GPS(Global Positioning System，全球定位系统)、IMU和相机等低精度的消费级定位设备。导航地图是指应用于自动驾驶的误差级别为厘米级的高精度导航地图。高精度导航地图中具有交通牌、路灯杆、车道线和车道线虚线端点等元素的3D位置信息。
本实施例中，预设导航地图中存在与车体的先验位置对应的覆盖区域是指在预设导航地图中，在车辆先验位置设定范围内，例如十几米的范围内，可搜索到地图元素。其中，地图元素可以为交通牌、路灯杆、车道线或车道线虚线端点等。如果在预设导航地图中未搜索到任何地图元素，则说明预设导航地图中不存在与车体的先验位置对应的覆盖区域，即当前车辆并未驶入该预设导航地图所覆盖的范围，此时需保持系统的开始初始化状态。当间隔数帧图像时间戳的间隔后再次轮循检测预设导航地图中是否存在与车体的先验位置对应的覆盖区域，如果存在，则说明车辆驶入了预设导航地图所覆盖的范围，此时可进行车辆位姿信息的粒子采样。本实施例中，采用的是间隔数帧图像后再进行预设导航地图覆盖范围的检测，这样设置相对于每次来一帧图像都做检测的方式，可以提升计算效率。
本实施例中，当检测到车辆驶入预设导航地图与车体先验位置对应的覆盖区域后，可基于车辆位置信息进行粒子采样。采样得到的每个位置是一个粒子，每个粒子作为一个车体可能的位置。在采样过程中，可保持车体的姿态不变，因为车体的姿态在GPS航向信息和IMU的重力观测信息下误差较小，误差主要发生在车辆的位置上。
作为一种可选的实施方式，本实施例中，基于车体的先验位置进行车辆位置信息的粒子采样，具体可以为：
在与车体的先验位置满足第三设定距离的位置处，可按照高斯分布模型在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。本实施例中三维采样主要应用于在预设导航地图中未检索到车道线的情况。
作为另一种可选的的实施方式，本实施例中，基于车体的先验位置进行车辆位置信息的粒子采样，具体可以为：
在预设导航地图中，提取与车体的先验位置满足第一设定距离(例如距离车体的先验位置前后左右各15米的范围)的各目标车道线；对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离(例如两米)的位置处，基于二维平面上均匀分布的概率模型，对车辆位置信息进行2D空间的粒子采样。
具体的，图2b是本发明实施例提供的一种粒子采样示意图，如图2b所示，1和2表示距离车体先验位置满足第一设定距离的目标车道线，该目标车辆线上的黑点表示车道线离散点，在距离每个离散点第二设定距离的范围内可采样得到多个粒子，作为车体可能存在的位置。
具体的，图2c是本发明实施例提供的又一种粒子采样示意图，如图2c所示，可利用车道线的位置，在每两根车道线(1和2，或2和3)的中心点处进行粒子采样，得到一系列采样粒子。
相对于3D空间的粒子采样方式，上述2D空间的粒子采样结合了车体先验位置和预设导航地图的路面信息，利用预设导航地图中的车道线在车道线离散点周围2维平面进行随机采样，有效降低了粒子采样空间的维度，减少了粒子数量，极大的提升了算法的时间效率。
具体的，由于单点GPS高程上误差有时会达到十几米，经纬度误差也在10米左右，因此若覆盖车体先验位置前后左右各10米，高程上下各15米，如果采用3D空间的粒子采样方式，按照每立方米一个粒子计算需要20*20*30需要12000个粒子。而上述2D空间的粒子采样使用了预设导航地图中的车道线，车体的高度可通过车道线的高度得到确定，在使用了车道线离散出的点来采样车体位置粒子时，无需针对高 度这个维度去进行采样，因此，采样维度可从三维空间采样降到二维，大大减小了粒子数量，提升了算法的运算速率。
120、更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件。
本实施例中，由于车辆在运动，采样得到的粒子的位姿也需要得到更新。其中，粒子的姿态可采用先验位置处车辆的姿态，粒子的位置的变化是不断迭代的过程，具体可按照如下公式进行更新：
Figure PCTCN2019113486-appb-000009
其中，
Figure PCTCN2019113486-appb-000010
和
Figure PCTCN2019113486-appb-000011
表示第i号粒子在K时刻和K-1时刻的位姿状态；ΔT k表示第K-1时刻和K时刻的相对运动信息；ΔN k表示运动噪声。
基于上述公式，可确定任意一个粒子在当前时刻的位置信息。根据该位置信息，可搜索到预设导航地图中该位置信息对应的所有地图元素，从而建立地图元素与感知图像中元素的初始匹配关系，以根据该初始匹配关系对粒子的权重信息进行更新。
示例性的，更新每个粒子对应的权重信息的过程具体可以为：
对于采样得到的任意一个粒子，基于该粒子在当前时刻的位置信息，将预设导航地图中与当前时刻粒子的位置信息满足预设距离要求的地图元素均投影到感知图像上，并根据重投影残差的大小建立地图元素与感知图像中各感知元素之间的初始匹配关系；基于初始匹配关系，更新每个粒子对应的权重信息。
其中，感知图像是利用预设感知模型对摄像头采集的包含道路信息的图像进行识别后得到的。预设感知模型可以预先采用大量标注有图像语义特征的道路样本图像对感知模型进行训练。其中，图像语义特征可包括交通牌、路灯杆、车道线、车道线虚线端点等。通过将包含有道路信息的道路图像输入至训练好的预设感知模型，基于预设感知模型的识别结果，即可得到道路图像中的图像语义特征。其中，预设感知模型可以通过以下方式得到：
构建训练样本集，该训练样本集包括多组训练样本数据，每组训练样本数据包括道路样本图像和对应的标注有图像语义特征的道路感知样本图像；基于训练样本集对搭建的初始神经网络进行训练得到预设感知模型，该预设感知模型使得每组训练样本数据中的道路样本图像与对应的标注有图像语义特征的道路感知样本图像相关联。模型输出的即可称之为感知图像。感知图像中的各种道路信息可称为感知元素。
本实施例中，在建立地图元素和感知元素的初始匹配关系时，各地图元素与对应感知元素之间的重投影残差的大小需满足一定的阈值要求。初始匹配关系建立的越准确，根据该初始匹配关系所得到的粒子的权重信息越大。同样的，基于初始匹配关系更新出的粒子的权重信息的大小，也可反映出初始匹配关系建立的准确性，权重值越大，说明初始匹配关系建立的越准确。本实施例中，可将权重值低于设定阈值的粒子及其对应的匹配关系进行过滤。通过采用不断迭代的方式更新采样粒子的位置和每个粒子对应的权重信息，可使得目标粒子的位置越来越聚拢，直到设定数目的目标粒子的位置满足预设收敛条件。其中，预设收敛条件是指目标粒子的位置方差小于设定阈值。此时，即表示完成初始化过程。
130、根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于该状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系。
其中，车辆姿态为车辆先验姿态。示例性的，可按照如下公式根据每个目标粒子更新后的权重信息确定车体位置的状态量：
Figure PCTCN2019113486-appb-000012
其中，x k表示k时刻车体位置的状态量；
Figure PCTCN2019113486-appb-000013
是k时刻的粒子i的权重。
示例性的，基于该状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系是指：基于当前车辆的位姿，预设导航地图中的足够数量的地图元素与感知图像中对应的感知元素均能建立一一对应的 匹配关系，且每组匹配对的重投影残差均符合预设阈值要求时，可说明感知图像和预设导航地图之间建立了目标匹配关系，足够数量的匹配对可为车辆提供六自由度约束，此时，可说明系统初始化成功，系统进入匹配状态。反之，如果系统初始化不成功，则返回执行步骤110，系统重新进入开始初始化状态。
140、基于目标匹配关系对先验位置处车体的位姿进行优化。
示例性的，在匹配状态下，可基于目标匹配关系对先验位置处车体的位姿进行六自由度的优化，具体优化过程可采用非线性优化算法来实现，从而可以获得厘米级的定位精度。
进一步的，随着车辆的运动，在后续每一帧利用外部输入的运动增量和车辆系统内部维护的厘米级定位的位置，可根据感知元素和地图元素重投影残差的大小进一步获取更加精确的目标匹配关系，并不断利用获取的目标匹配关系对车辆的位姿进行优化。
进一步的，随着车辆的运动，如果连续数帧感知图像都无法与预设导航地图建立目标匹配关系，则说明车辆驶出了预设导航地图的覆盖范围，此时，可返回执行步骤110，以重新进入开始初始化状态。
本实施例提供的技术方案，当车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，通过基于车体的先验位置进行车辆位置信息的粒子采样，以通过多个粒子状态表征当前定位结果，由于考虑到了车体可能存在的多个位置，可大大提高了后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系，以基于目标匹配关系对所述先验位置处车体的位姿进行优化。相对于现有技术提供的使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位的方式，本实施例提供的方式避免了由于路面上的箭头信息过于稀疏，无法保证连续的厘米级高精定位的问题，通过基于目标匹配关系对先验位置处车体的位姿进行优化，可使的车辆的位姿达到厘米级的定位精度。
实施例二
请参阅图3a，图3a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图。本实施例在上述实施例的基础上，对投影到感知图像上的地图元素的搜索过程进行了优化。如图3a所示，该方法包括：
210、当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样。
220、对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息。
230、将世界坐标系中，与车体先验位置满足第四设定距离的地图元素转换到相机坐标系下。
其中，相机坐标系定义为x轴朝右，y轴朝下，z轴朝前。本实施例中，相机坐标系的设定方向是指相机前方Z轴朝前的方向，即Z轴的正方向。
240、在相机坐标系的设定方向上，将满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对满足第四设定范围的地图元素进行排序。
示例性的，键值对信息MAP可以为红黑树或其他二叉树数据结构。本实施例中，构建键值对信息的好处在于，可以直接搜索出在Z轴方向上粒子前方的地图元素，从而减少了后续无效重投影的次数，提升了算法的时间效率。
250、将粒子在当前时刻的位置信息信息转换到相机坐标系下，并计算位置信息在相机坐标系设定方向上对应的目标位置。
260、从键值对信息中，依次查找目标位置对应的各地图元素。
示例性的，由于沿着Z轴方向，相机只能拍摄到它前方的物体，因此，从键值对信息中查找目标位置对应的各地图元素时，可直接搜索出在Z轴方向上粒子前方的地图元素。由于地图元素的种类较多，对于不同种类的地图元素，可根据Z轴坐标值的大小设置不同的搜索范围。图3b是本发明实施例提供的一种地图元素的搜索范围示意图。如图3b所示，z1表示粒子与车体先验位置之间的相对距离，z2表示地图元素与车体先验位置之间的相对距离。对于交通牌而言，可在Z轴方向上粒子前方所有的交通牌(图3b中为未示出)。对于路灯杆而言，可选取沿Z轴方向上粒子前方距离小于d1(如图3b中方框1的宽度)的范围作为搜索范围。对于车道线虚线端点，可选取沿Z轴方向上粒子前方距离小于d2(如图3b中方框2的 宽度)的范围作为搜索范围。其中，由于路灯杆的在路面中的设置相对于车道线虚线端点而言较为稀疏，因此可优选设置为d1大于d2。
270、将各地图元素投影到感知图像上，并根据重投影残差的大小建立地图元素与感知图像中各感知元素之间的初始匹配关系。
280、基于初始匹配关系，更新每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件。
290、根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系，以基于目标匹配关系对先验位置处车体的位姿进行优化。
本实施例在上述实施例的基础，通过利用地图元素与车体先验位置之间的相对位置关系，构建了地图元素在相机坐标系下设定方向的键值对信息，这样每个粒子可根据与车体先验位置的相对关系直接从键值对信息搜索出粒子前方的地图元素进行投影。相对于直接从预设导航地图中搜索地图元素的方式，本实施例这样设置排除了不可能投影到感知图像中的地图元素，从而减少了做无效重投影的次数，提升了算法时间效率。
实施例三
请参阅图4，图4是本发明实施例提供的一种车辆位姿的修正方法的流程示意图，本实施例在上述实施例的基础上，对每个粒子对应的权重信息的更新以及感知图像和预设导航地图之间的目标匹配关系的建立过程进行了优化。如图4所示，本实施例提供的方法具体包括：
310、当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样。
320、更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件。
示例性的，如果设定数目的目标粒子的位置不满足预设收敛条件，则检测目标粒子中的有效粒子数目是否达第二预设数目阈值。如果有效粒子数目达不到第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
其中，目标粒子是否为有效粒子可通过如下公式进行判断：
Figure PCTCN2019113486-appb-000014
其中，
Figure PCTCN2019113486-appb-000015
表示有效粒子所占目标粒子的百分比，
Figure PCTCN2019113486-appb-000016
表示第i个粒子的权重，N s表示目标粒子的总个数。
330、根据每个目标粒子更新后的权重信息确定车体位置的状态量。
340、基于车体位置的状态量和车体姿态，将导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与感知图像中对应的感知元素建立一对一的匹配关系。
350、将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和预设导航地图之间的目标匹配关系。
其中，目标匹配关系相对于上述实施例中初始匹配关系的建立，需保证匹配对的数量是足够多的，即满足第一预设数目阈值，并且每组匹配对的重投影残差也应小于初始匹配关系建立是对应的重投影残差，即都满足预设阈值要求，从而为车辆提供六自由度的约束。
360、基于目标匹配关系对先验位置处车体的位姿进行优化。
本实施例中，如果足够数量的匹配对的重投影残差均达到预设阈值要求，则确定感知图像和预设导航地图之间建立了目标匹配关系。通过目标匹配关系对先验位置处车体的位姿六自由度进行优化，可以获得厘米级的定位精度。
实施例四
请参阅图5，图5是本发明实施例提供的一种车辆位姿的修正装置的结构示意图。如图5所示，该装置包括：粒子采样模块410、权重更新模块420、目标匹配关系建立模块430和位姿优化模块440；其中，
粒子采样模块410，被配置为当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
权重更新模块420，被配置为更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
目标匹配关系建立模块430，被配置为根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
位姿优化模块440，被配置为基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
本实施例提供的技术方案，当车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，通过基于车体的先验位置进行车辆位置信息的粒子采样，以通过多个粒子状态表征当前定位结果，由于考虑到了车体可能存在的多个位置，可大大提高了后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系，以基于目标匹配关系对所述先验位置处车体的位姿进行优化。相对于现有技术提供的使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位的方式，本实施例提供的方式避免了由于路面上的箭头信息过于稀疏，无法保证连续的厘米级高精定位的问题，通过基于目标匹配关系对先验位置处车体的位姿进行优化，可使的车辆的位姿达到厘米级的定位精度。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
可选的，所述权重更新模块，包括：
位置信息确定单元，被配置为对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
投影单元，被配置为基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上；
初始匹配关系建立单元，被配置为根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
权重信息更新单元，被配置为基于所述初始匹配关系，更新每个粒子对应的权重信息。
可选的，所述投影单元，具体被配置为：
将所述位置信息转换到所述相机坐标系下，并计算所述位置信息在所述设定方向上对应的目标位置；
将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到相机坐标系下；
在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
可选的，所述权重信息更新单元，具体被配置为：
按照如下公式更新每个粒子对应的权重信息：
Figure PCTCN2019113486-appb-000017
其中，
Figure PCTCN2019113486-appb-000018
是k时刻的粒子i的权重；
Figure PCTCN2019113486-appb-000019
是k-1时刻粒子i的权重；N i为第i个粒子上获得的感知元素和地图元素的匹数数量；f(N i)是N i正相关的函数；s j为每种地图元素对应的一个归一化参数；r j第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上除以了N i将感知图像和导航地图匹配的数量进行了概率归一化。
可选的，所述目标匹配关系建立模块，具体被配置为：
根据每个目标粒子更新后的权重信息确定车体位置的状态量，基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系。
可选的，所述装置还包括：
有效粒子检测模块，被配置为如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
重采样模块，被配置为如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
本发明实施例所提供的车辆位姿的修正装置可执行本发明任意实施例所提供的车辆位姿的修正方法，具备执行方法相应的功能模块和有益效果。未在上述实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的车辆位姿的修正方法。
实施例五
请参阅图6，图6是本发明实施例提供的一种车载终端的结构示意图。如图6所示，该车载终端可以包括：
存储有可执行程序代码的存储器701；
与存储器701耦合的处理器702；
其中，处理器702调用存储器701中存储的可执行程序代码，执行本发明任意实施例所提供的车辆位姿的修正方法。
本发明实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行本发明任意实施例所提供的车辆位姿的修正方法。
本发明实施例公开一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤。
在本发明的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
在本发明所提供的实施例中，应理解，“与A相应的B”表示B与A相关联，根据A可以确定B。但还应理解，根据A确定B并不意味着仅仅根据A确定B，还可以根据A和/或其他信息确定B。
另外，在本发明各实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以 采用软件功能单元的形式实现。
上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分，可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(Compact Disc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
以上对本发明实施例公开的一种车辆位姿的修正方法和装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

  1. 一种车辆位姿的修正方法，应用于自动驾驶，其特征在于，包括：
    当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
    更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
    根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
    基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
  2. 根据权利要求1所述的方法，其特征在于，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
    在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
    对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
  3. 根据权利要求1所述的方法，其特征在于，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
    在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
  4. 根据权利要求1所述的方法，其特征在于，更新采样得到的粒子的位姿和每个粒子对应的权重信息，包括：
    对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
    基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上，并根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
    基于所述初始匹配关系，更新每个粒子对应的权重信息。
  5. 根据权利要求4所述的方法，其特征在于，基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上，包括：
    将所述位置信息转换到相机坐标系下，并计算所述位置信息在所述相机坐标系的设定方向上对应的目标位置；
    将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到所述相机坐标系下；
    在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
    从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
  6. 根据权利要求4所述的方法，其特征在于，基于所述初始匹配关系，更新每个粒子对应的权重信息，包括：
    按照如下公式更新每个粒子对应的权重信息：
    Figure PCTCN2019113486-appb-100001
    其中，
    Figure PCTCN2019113486-appb-100002
    是k时刻的粒子i的权重；
    Figure PCTCN2019113486-appb-100003
    是k-1时刻粒子i的权重；N i为第i个粒子上获得的感知元素和地图元素的匹数数量；f(N i)是N i正相关的函数；s j为每种地图元素对应的一个归一化参数；r j第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上 除以了N i将感知图像和导航地图匹配的数量进行了概率归一化。
  7. 根据权利要求1所述的方法，其特征在于，根据每个目标粒子更新后的权重信息确定车体位置的状态量，包括：
    按照如下公式根据每个目标粒子更新后的权重信息确定车体位置的状态量：
    Figure PCTCN2019113486-appb-100004
    其中，x k表示k时刻车体位置的状态量；
    Figure PCTCN2019113486-appb-100005
    是k时刻的粒子i的权重。
  8. 根据权利要求1-7任一所述的方法，其特征在于，基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系，包括：
    基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
    将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系。
  9. 根据权利要求1-8任一所述的方法，其特征在于，所述方法还包括：
    如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
    如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
  10. 一种车辆位姿的修正装置，应用于自动驾驶，其特征在于，包括：
    粒子采样模块，被配置为当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
    权重更新模块，被配置为更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
    目标匹配关系建立模块，被配置为根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
    位姿优化模块，被配置为基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
PCT/CN2019/113486 2019-05-30 2019-10-26 一种车辆位姿的修正方法和装置 WO2020237996A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201910460548.7A CN110954113B (zh) 2019-05-30 2019-05-30 一种车辆位姿的修正方法和装置
CN201910460548.7 2019-05-30

Publications (1)

Publication Number Publication Date
WO2020237996A1 true WO2020237996A1 (zh) 2020-12-03

Family

ID=69976186

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2019/113486 WO2020237996A1 (zh) 2019-05-30 2019-10-26 一种车辆位姿的修正方法和装置

Country Status (2)

Country Link
CN (1) CN110954113B (zh)
WO (1) WO2020237996A1 (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112558130A (zh) * 2020-12-07 2021-03-26 安徽江淮汽车集团股份有限公司 定位数据的同步方法、装置、设备及存储介质
CN113465620A (zh) * 2021-06-02 2021-10-01 上海追势科技有限公司 一种基于语义信息的停车场粒子滤波定位方法
CN114001742A (zh) * 2021-10-21 2022-02-01 广州小鹏自动驾驶科技有限公司 车辆定位方法、装置、车辆和可读存储介质
CN114119673A (zh) * 2022-01-25 2022-03-01 北京地平线机器人技术研发有限公司 初始位姿的确定方法和装置、电子设备和存储介质
CN114111817A (zh) * 2021-11-22 2022-03-01 武汉中海庭数据技术有限公司 基于slam地图与高精度地图匹配的车辆定位方法及系统
CN114419590A (zh) * 2022-01-17 2022-04-29 北京百度网讯科技有限公司 高精地图的验证方法、装置、设备以及存储介质
CN115235493A (zh) * 2022-07-19 2022-10-25 合众新能源汽车有限公司 一种基于矢量地图进行自动驾驶定位的方法及装置
WO2023131048A1 (zh) * 2022-01-06 2023-07-13 上海安亭地平线智能交通技术有限公司 位姿信息的确定方法、装置、电子设备和存储介质
CN118067113A (zh) * 2024-04-24 2024-05-24 合众新能源汽车股份有限公司 一种车道线匹配时的异常过滤方法、存储介质

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114248778B (zh) * 2020-09-22 2024-04-12 华为技术有限公司 移动设备的定位方法和定位装置
CN112747744B (zh) * 2020-12-22 2022-11-18 浙江大学 一种结合航位推算和多车道路网地图的车辆定位方法
CN112902987B (zh) * 2021-02-02 2022-07-15 北京三快在线科技有限公司 一种位姿修正的方法及装置
CN113447023B (zh) * 2021-06-18 2022-11-01 上海宏景智驾信息科技有限公司 一种基于吊灯的室内视觉定位方法
CN114088114B (zh) * 2021-11-19 2024-02-13 智道网联科技(北京)有限公司 车辆位姿校准方法、装置和电子设备
CN114323033B (zh) * 2021-12-29 2023-08-29 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN115388906A (zh) * 2022-08-24 2022-11-25 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质
CN115143996B (zh) * 2022-09-05 2023-01-17 北京智行者科技股份有限公司 定位信息修正方法及电子设备和存储介质
CN115248040B (zh) * 2022-09-22 2022-12-23 毫末智行科技有限公司 初始化定位方法、装置、终端及存储介质
CN117485356B (zh) * 2023-12-29 2024-04-05 合众新能源汽车股份有限公司 基于风险等级评估的高精定位故障诊断方法和系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101625572A (zh) * 2009-08-10 2010-01-13 浙江大学 基于改进重采样方法和粒子选取的FastSLAM算法
US20150142248A1 (en) * 2013-11-20 2015-05-21 Electronics And Telecommunications Research Institute Apparatus and method for providing location and heading information of autonomous driving vehicle on road within housing complex
CN106940704A (zh) * 2016-11-25 2017-07-11 北京智能管家科技有限公司 一种基于栅格地图的定位方法及装置
CN108053443A (zh) * 2017-11-20 2018-05-18 中国科学院空间应用工程与技术中心 一种基于粒子群优化的物体点云位姿估算方法和系统
CN108759833A (zh) * 2018-04-25 2018-11-06 中国科学院合肥物质科学研究院 一种基于先验地图的智能车辆定位方法
CN108871341A (zh) * 2018-07-05 2018-11-23 内江市下代互联网数据处理技术研究所 一种全局优化的并发定位与建图方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101509781B (zh) * 2009-03-20 2011-09-14 同济大学 基于单目摄像头的步行机器人定位系统
CN108732603B (zh) * 2017-04-17 2020-07-10 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
US10234291B1 (en) * 2017-10-06 2019-03-19 Cisco Technology, Inc. Collaborative localization between phone and infrastructure
CN107991683B (zh) * 2017-11-08 2019-10-08 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109297500B (zh) * 2018-09-03 2020-12-15 武汉中海庭数据技术有限公司 基于车道线特征匹配的高精度定位装置及方法
CN109556611B (zh) * 2018-11-30 2020-11-10 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101625572A (zh) * 2009-08-10 2010-01-13 浙江大学 基于改进重采样方法和粒子选取的FastSLAM算法
US20150142248A1 (en) * 2013-11-20 2015-05-21 Electronics And Telecommunications Research Institute Apparatus and method for providing location and heading information of autonomous driving vehicle on road within housing complex
CN106940704A (zh) * 2016-11-25 2017-07-11 北京智能管家科技有限公司 一种基于栅格地图的定位方法及装置
CN108053443A (zh) * 2017-11-20 2018-05-18 中国科学院空间应用工程与技术中心 一种基于粒子群优化的物体点云位姿估算方法和系统
CN108759833A (zh) * 2018-04-25 2018-11-06 中国科学院合肥物质科学研究院 一种基于先验地图的智能车辆定位方法
CN108871341A (zh) * 2018-07-05 2018-11-23 内江市下代互联网数据处理技术研究所 一种全局优化的并发定位与建图方法

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112558130B (zh) * 2020-12-07 2023-12-19 安徽江淮汽车集团股份有限公司 定位数据的同步方法、装置、设备及存储介质
CN112558130A (zh) * 2020-12-07 2021-03-26 安徽江淮汽车集团股份有限公司 定位数据的同步方法、装置、设备及存储介质
CN113465620A (zh) * 2021-06-02 2021-10-01 上海追势科技有限公司 一种基于语义信息的停车场粒子滤波定位方法
CN114001742A (zh) * 2021-10-21 2022-02-01 广州小鹏自动驾驶科技有限公司 车辆定位方法、装置、车辆和可读存储介质
CN114001742B (zh) * 2021-10-21 2024-06-04 广州小鹏自动驾驶科技有限公司 车辆定位方法、装置、车辆和可读存储介质
CN114111817A (zh) * 2021-11-22 2022-03-01 武汉中海庭数据技术有限公司 基于slam地图与高精度地图匹配的车辆定位方法及系统
CN114111817B (zh) * 2021-11-22 2023-10-13 武汉中海庭数据技术有限公司 基于slam地图与高精度地图匹配的车辆定位方法及系统
WO2023131048A1 (zh) * 2022-01-06 2023-07-13 上海安亭地平线智能交通技术有限公司 位姿信息的确定方法、装置、电子设备和存储介质
CN114419590A (zh) * 2022-01-17 2022-04-29 北京百度网讯科技有限公司 高精地图的验证方法、装置、设备以及存储介质
CN114419590B (zh) * 2022-01-17 2024-03-19 北京百度网讯科技有限公司 高精地图的验证方法、装置、设备以及存储介质
CN114119673B (zh) * 2022-01-25 2022-04-22 北京地平线机器人技术研发有限公司 初始位姿的确定方法和装置、电子设备和存储介质
CN114119673A (zh) * 2022-01-25 2022-03-01 北京地平线机器人技术研发有限公司 初始位姿的确定方法和装置、电子设备和存储介质
CN115235493A (zh) * 2022-07-19 2022-10-25 合众新能源汽车有限公司 一种基于矢量地图进行自动驾驶定位的方法及装置
CN118067113A (zh) * 2024-04-24 2024-05-24 合众新能源汽车股份有限公司 一种车道线匹配时的异常过滤方法、存储介质

Also Published As

Publication number Publication date
CN110954113B (zh) 2021-10-15
CN110954113A (zh) 2020-04-03

Similar Documents

Publication Publication Date Title
WO2020237996A1 (zh) 一种车辆位姿的修正方法和装置
US11176701B2 (en) Position estimation system and position estimation method
US11175145B2 (en) System and method for precision localization and mapping
US20220028163A1 (en) Computer Vision Systems and Methods for Detecting and Modeling Features of Structures in Images
KR102382420B1 (ko) 차량 위치 결정 방법, 전자 기기 및 저장 매체
CN110979346B (zh) 确定车辆所处车道的方法、装置及设备
KR20220053513A (ko) 이미지 데이터 자동 라벨링 방법 및 장치
KR102103941B1 (ko) 포인트 클라우드 맵 기반의 자율주행차량용 도로 및 차선데이터 실시간 업데이트 방법
CN109141444B (zh) 定位方法、装置、存储介质及移动设备
US11232582B2 (en) Visual localization using a three-dimensional model and image segmentation
CN113989450A (zh) 图像处理方法、装置、电子设备和介质
WO2020199565A1 (zh) 一种基于路灯杆的车辆位姿的修正方法和装置
JP7149362B2 (ja) 環境変化を検出するための方法及び装置、計算装置、記憶媒体並びにコンピュータプログラム
CN116255992A (zh) 一种同时定位与建图的方法和装置
CN110827353B (zh) 一种基于单目摄像头辅助的机器人定位方法
CN114111774B (zh) 车辆的定位方法、系统、设备及计算机可读存储介质
CN113008260A (zh) 一种导航信息处理方法、装置、电子设备及存储介质
JP2020153956A (ja) 移動体位置推定システムおよび移動体位置推定方法
CN115205803A (zh) 自动驾驶环境感知方法、介质及车辆
CN111597986A (zh) 用于生成信息的方法、装置、设备和存储介质
CN113838129A (zh) 一种获得位姿信息的方法、装置以及系统
CN115790621A (zh) 高精地图更新方法、装置及电子设备
CN112907659B (zh) 移动设备定位系统、方法及设备
CN109901589B (zh) 移动机器人控制方法和装置
CN115841514A (zh) 一种自动泊车方法、装置及设备

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19930836

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19930836

Country of ref document: EP

Kind code of ref document: A1