CN111854727B - Vehicle pose correction method and device - Google Patents
Vehicle pose correction method and device Download PDFInfo
- Publication number
- CN111854727B CN111854727B CN201910346790.1A CN201910346790A CN111854727B CN 111854727 B CN111854727 B CN 111854727B CN 201910346790 A CN201910346790 A CN 201910346790A CN 111854727 B CN111854727 B CN 111854727B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- lane line
- vehicle body
- ground plane
- current position
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000012937 correction Methods 0.000 title claims description 44
- 239000013598 vector Substances 0.000 claims abstract description 126
- 238000012216 screening Methods 0.000 claims abstract description 16
- 238000001914 filtration Methods 0.000 claims description 6
- 230000008447 perception Effects 0.000 abstract description 17
- 238000010586 diagram Methods 0.000 description 13
- 238000004590 computer program Methods 0.000 description 6
- 238000013135 deep learning Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
Images
Classifications
-
- 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
-
- 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
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
-
- 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
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
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)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
The embodiment of the invention discloses a method and a device for correcting the pose of a vehicle, wherein the method comprises the following steps: screening out a first lane line meeting a set distance range with the current position in a lane where the vehicle is located at present according to the current position of the vehicle provided by a preset positioning device; discretizing the first lane line to obtain a plurality of lane line discrete points, and fitting a ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points; and taking the normal vector of the ground plane as an actual normal vector of the vehicle body, and correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector. By adopting the technical scheme, the problem that the automatic driving vehicle cannot be positioned when the information of the perception model is lost is solved, and the robustness of the positioning system is improved.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to a method and a device for correcting a vehicle pose.
Background
In the field of automatic driving, high-precision positioning is of great importance. In recent years, the development of the image semantic segmentation and image recognition field is greatly promoted by the achievement of technologies such as deep learning, and the like, so that a solid foundation is provided for high-precision maps and high-precision positioning.
In a common unmanned positioning scheme, a high-precision map is used, generally depending on a perception model provided by deep learning, and the position of a vehicle is determined through matching and checking of perception model information and high-precision map information. The method comprises the steps of projecting high-precision map information to an image plane through a pinhole camera model, comparing the information with information provided by a deep learning perception model, and correcting the position and the posture of a vehicle by taking an obtained error as a correction amount in a positioning process. The accuracy of the positioning of the vehicle is also dependent to some extent on the accuracy of the perceptual model.
However, when the vehicle is in poor ambient lighting conditions, the vehicle-mounted camera device is abnormal, and the like, and the sensing model information cannot be correctly provided, the positioning fails, and the automatic driving vehicle cannot normally run.
Disclosure of Invention
The embodiment of the invention discloses a method and a device for correcting the pose of a vehicle, which solve the problem that an automatic driving vehicle cannot be positioned when perception model information is lost, and improve the robustness of a positioning system.
In a first aspect, an embodiment of the invention discloses a method for correcting a vehicle pose, which comprises the following steps:
screening out a first lane line which meets a set distance range with the current position in a lane where the vehicle is located at present according to the current position of the vehicle provided by a preset positioning device;
discretizing the first lane line to obtain a plurality of lane line discrete points, and fitting a ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points;
and taking the normal vector of the ground plane as an actual normal vector of the vehicle body, and correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector.
Optionally, the fitting out the ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points includes:
randomly selecting a preset number of target discrete points from the plurality of lane line discrete points;
fitting a plurality of fitted ground planes where the vehicle is located at the current position by using the target discrete points;
for any one fitted ground plane, judging the sum of the distances from other discrete points except the target discrete point to the fitted ground plane;
and selecting the fitted ground plane corresponding to the distance with the minimum sum value from the sum of the distances as the ground plane where the vehicle is located at the current position.
Optionally, the step of correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector includes:
and based on a Kalman filtering algorithm, correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector.
Optionally, the method further includes:
screening out a second lane line with the direction consistent with the driving direction of the vehicle from the first lane line;
if a plurality of target lane lines with the same direction vector number and the set number are present in the second lane line, taking the average value of the direction vectors of the plurality of target lane lines as the actual direction vector of the vehicle body;
and correcting the course of the vehicle at the current position according to the difference between the actual direction vector and the current direction vector of the vehicle body.
Optionally, the method further includes:
determining an average value of the elevations of the first lane line based on a preset navigation map;
and taking the average value of the elevations of the first lane lines as the elevation of the vehicle at the current position so as to correct the elevation of the vehicle at the current position.
In a second aspect, an embodiment of the present invention further provides a device for correcting a vehicle pose, where the device includes:
the first lane line screening module is configured to screen out a first lane line, which meets a set distance range with a current position, in a lane where the vehicle is located currently according to the current position of the vehicle provided by a preset positioning device;
the ground plane fitting module is configured to discretize the first lane line to obtain a plurality of lane line discrete points, and fit out a ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points;
and the angle correction module is configured to take the normal vector of the ground plane as an actual normal vector of the vehicle body, and correct the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector.
Optionally, the ground plane fitting module is specifically configured to:
discretizing the first lane line to obtain a plurality of lane line discrete points, and randomly selecting a preset number of target discrete points from the plurality of lane line discrete points;
fitting a plurality of fitted ground planes where the vehicle is located at the current position by using the target discrete points;
for any one fitted ground plane, judging the sum of the distances from other discrete points except the target discrete point to the fitted ground plane;
and selecting the fitted ground plane corresponding to the distance with the minimum sum value from the sum of the distances as the ground plane where the vehicle is located at the current position.
Optionally, the angle correction module is specifically configured to:
and taking the normal vector of the ground plane as an actual normal vector of the vehicle body, and correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector based on a Kalman filtering algorithm.
Optionally, the apparatus further comprises:
the second lane line screening module is configured to screen out a second lane line of which the direction is consistent with the driving direction of the vehicle from the first lane line;
an actual direction vector determination module configured to, in the second lane line, if there are a plurality of target lane lines whose number reaches a set number and whose direction vectors are consistent, take an average of the plurality of target lane line direction vectors as an actual direction vector of the vehicle body;
and the orientation correction module is configured to correct the heading of the vehicle at the current position according to the difference value between the actual direction vector and the current direction vector of the vehicle body.
Optionally, the apparatus further comprises:
a lane line elevation determination module configured to determine an average of elevations of a first lane line based on a preset navigation map;
and the vehicle elevation correction module is configured to take the average value of the elevations of the first lane lines as the elevation of the vehicle at the current position so as to correct the elevation of the vehicle at the current position.
In a third aspect, an embodiment of the present invention further provides a vehicle-mounted terminal, including:
a memory storing executable program code;
a processor coupled with the memory;
the processor calls the executable program codes stored in the memory to execute part or all of the steps of the vehicle pose correction method provided by any embodiment of the invention.
In a fourth aspect, the embodiment of the present invention further provides a computer-readable storage medium storing a computer program including instructions for executing part or all of the steps of the method for correcting a vehicle pose provided by any embodiment of the present invention.
In a fifth aspect, the embodiment of the present invention further provides a computer program product, which when run on a computer, causes the computer to execute part or all of the steps of the method for correcting the vehicle pose provided by any embodiment of the present invention.
According to the technical scheme, when the sensing model information is lost and the vehicle cannot be positioned, the first lane line is discretized, the ground plane where the vehicle at the current position is located is fitted by utilizing a plurality of lane line discrete points, the normal vector of the ground plane can be used as the actual normal vector of the vehicle body, the roll angle and the pitch angle of the vehicle body can be corrected by utilizing the difference between the actual normal vector and the current vehicle body normal vector, the problem that the vehicle cannot be automatically driven to be positioned when the sensing model information is lost is solved, and the robustness of the positioning system is improved.
The invention comprises the following steps:
1. according to the scheme of the embodiment of the invention, the four-degree-of-freedom information of the elevation, the pitch angle, the roll angle and the course angle of the vehicle can be provided when the sensing information is lost, the problem that the vehicle cannot be positioned under the condition that the sensing model information is lost is solved, and the normal running state of the vehicle can be kept under the condition that the sensing model information is lost.
2. The ground plane where the vehicle is located at the current position is fitted by utilizing the first lane line discrete points, the roll angle and the pitch angle of the vehicle body are corrected by taking the normal vector of the ground plane as the actual normal vector of the vehicle body and utilizing the difference between the actual normal vector and the current vehicle body normal vector, so that the problem that the roll angle and the pitch angle of the automatic driving vehicle cannot be positioned under the condition that a perception model is lost is solved, the automatic driving vehicle can normally run under the condition that information of the perception model is lost, and the robustness of a positioning system is improved.
3. A plurality of target lane lines with the same number of direction vectors and the same number of set number are screened out from second lane lines with the same driving direction of the vehicle. The invention relates to a method for correcting the course of a vehicle by using the difference between the average value of the direction vectors of a plurality of target lane lines and the current direction vector of a vehicle body, which solves the problem that the course of an automatic driving vehicle cannot be positioned under the condition of information loss of a perception model, so that the automatic driving vehicle can still normally run under the condition of information loss of the perception model, and the robustness of a positioning system is improved.
4. The determined lane line elevation can be used as the elevation of the current position of the vehicle by determining the lane line elevation within a range of several meters around the current position of the vehicle in the navigation map. Under the condition of the lack of the perception model information, the elevation of the vehicle can be lowered to the position of the lane line, and meanwhile, the accuracy of the subsequent vehicle posture correction can be improved.
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 embodiments will be briefly described 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 that other drawings can be obtained according to these drawings without creative efforts.
Fig. 1a is a schematic flow chart of a method for correcting a vehicle pose according to an embodiment of the present invention;
fig. 1b is a schematic flow chart of a method for correcting roll angle and pitch angle of a vehicle body according to an embodiment of the present invention;
FIG. 1c is a schematic diagram of a ground plane fitting provided by an embodiment of the present invention;
FIG. 1d is a schematic view of a vehicle position provided by an embodiment of the present invention;
FIG. 1e is a schematic diagram of a vehicle roll and pitch angle modification according to an embodiment of the present invention;
FIG. 2a is a schematic flow chart of a method for correcting a vehicle body heading according to an embodiment of the present invention;
fig. 2b is a schematic diagram of the vehicle body before the heading correction according to the embodiment of the present invention.
FIG. 2c is a schematic view of a vehicle body after a course correction according to an embodiment of the present invention is performed;
FIG. 3a is a schematic flow chart illustrating a method for correcting vehicle body elevation according to an embodiment of the present invention;
FIG. 3b is a schematic view of elevation correction of a vehicle body according to an embodiment of the present invention;
fig. 4 is a schematic structural diagram of a vehicle pose correction device according to an embodiment of the present invention;
fig. 5 is a schematic structural diagram of a vehicle-mounted terminal according to an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, 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 invention.
It is to be noted that the terms "comprises" and "comprising" and any variations thereof in the embodiments and drawings of the present invention are intended to cover non-exclusive inclusions. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those steps or elements listed, but may alternatively include other steps or elements not listed, or inherent to such process, method, article, or apparatus.
Example one
Referring to fig. 1a, fig. 1a is a schematic flow chart of a method for correcting a vehicle pose according to an embodiment of the present invention, where the method is applied to automatic driving, typically in a scene where an automatic driving vehicle cannot be located due to lack of perception model information, and the method may be executed by a method device for correcting a vehicle pose, where the method device may be implemented by software and/or hardware, and may generally be integrated in a vehicle-mounted terminal such as a vehicle-mounted Computer, a vehicle-mounted Industrial control Computer (IPC), and an embodiment of the present invention is not limited. As shown in fig. 1a, the pose correction process provided by the embodiment of the present invention occurs after the high-precision map is initialized, and during the process from the vehicle entering the map positioning algorithm to the vehicle leaving the map positioning algorithm, the correction of the specific vehicle pose includes the respective correction of the elevation, the pitch angle, the roll angle and the heading of the vehicle by using the lane line information, and the correction process of each attitude angle does not have the precedence of the execution sequence. In the following, the present embodiment first describes the roll angle and pitch angle correction process of the vehicle in detail:
fig. 1b is a schematic flow chart of a method for correcting a roll angle and a pitch angle of a vehicle body according to an embodiment of the present invention, as shown in fig. 1b, the method specifically includes:
110. and screening out a first lane line meeting a set distance range with the current position in the lane where the vehicle is located currently according to the current position of the vehicle provided by the preset positioning device.
The preset positioning device is a single-point GPS or low-precision consumer positioning equipment. The preset positioning device can provide a rough position for the vehicle, namely the current position of the vehicle. According to the current position, the attributes of a plurality of lane lines at the current position can be determined from a high-precision map with centimeter-level error level, and the attributes comprise the category, the position, the direction and the like of the lane lines. Based on the attribute of the lane line, the lane line in the lane where the current vehicle is located can be screened out. By cutting the screened lane line, a first lane line satisfying a set distance range (for example, within 50 meters) from the current position of the vehicle can be obtained.
120. Discretizing the first lane line to obtain a plurality of lane line discrete points, and fitting out the ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points.
Since the lane line is a continuous curve and it is difficult to express a certain position in the lane line, the lane line may be dispersed into a plurality of discrete points, for example, a continuous first lane line may be dispersed into 10 discrete points in units of 1 meter.
In this embodiment, the ground plane where the current vehicle is located may be fitted using a plurality of lane line discrete points. As an example, the specific fitting method may be:
and randomly selecting a preset number of target discrete points from the plurality of lane line discrete points, wherein the number of the target discrete points is at least 3. And fitting a plurality of fitted ground planes in which the vehicle is located at the current position by using the target discrete points. And for any one fitted ground plane, judging the sum of the distances from other discrete points except the target discrete point to the fitted ground plane, and selecting the fitted ground plane corresponding to the distance with the minimum sum value from the sum of the distances as the ground plane where the vehicle is located at the current position.
Specifically, fig. 1c is a schematic diagram of a ground plane fitting according to an embodiment of the present invention, as shown in fig. 1c, 1 represents a fitted ground plane fitted by using any target discrete point, and by using the above embodiment, a ground plane where the vehicle at the current position is located, as represented by 2 in fig. 1c, can be selected from a plurality of fitted ground planes.
For example, three target discrete points may be randomly selected each time to perform plane fitting, the fitted ground plane obtained by each fitting is used as a candidate ground plane, and the sum of distances from other discrete points except the target discrete points to the candidate ground plane is calculated. And if the distance sum corresponding to the ground plane obtained by the next fitting is larger than the distance sum corresponding to the candidate ground plane, keeping the candidate ground plane unchanged until the distance sum corresponding to the ground plane obtained by the fitting at a certain time is smaller than the distance sum corresponding to the candidate ground plane, and taking the ground plane obtained by the current fitting as a new candidate ground plane to replace the previous candidate ground plane. For example, if the distance sum value corresponding to the candidate ground plane obtained by the first fitting is 10 centimeters and the distance sum value corresponding to the ground plane obtained by the second fitting is 12 centimeters, the candidate ground plane obtained by the first fitting is still kept unchanged. And if the distance and the value corresponding to the ground plane obtained by the third fitting are 8 centimeters, replacing the candidate ground plane obtained by the first fitting with the ground plane obtained by the third fitting to be used as a new candidate ground plane. Through successive iterative solution in the above manner, the fitted ground plane with the smallest corresponding sum can be determined as the ground plane on which the vehicle is located.
130. And taking the normal vector of the ground plane as an actual normal vector of the vehicle body, and correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector.
And the current vehicle body normal vector is the axial direction of the vehicle body course. The heading of the vehicle body may be determined from the current position and the acceleration and angular velocity of the vehicle body provided by an Inertial Measurement Unit (IMU). When the roll angle and the pitch angle of the vehicle body are corrected, a Kalman filtering algorithm can be used for correction, and specifically, the roll angle and the pitch angle of the vehicle body can be continuously adjusted, so that the difference between an actual normal vector and a current vehicle body normal vector is continuously close to zero.
Specifically, fig. 1d is a schematic diagram of a vehicle position according to an embodiment of the present invention, and fig. 1e is a schematic diagram of a vehicle roll angle and a vehicle pitch angle that are corrected according to an embodiment of the present invention. As shown in fig. 1d, 1 represents the actual normal vector of the vehicle body, 2 represents the current normal vector of the vehicle body, and there is a deviation between 1 and 2. As shown in fig. 1e, 3 denotes correction of the roll angle of the vehicle body, and 4 denotes correction of the pitch angle of the vehicle body. By using the kalman filter algorithm, the difference between the actual normal vector and the current vehicle body normal vector can be made to approach zero.
According to the technical scheme, when the vehicle cannot be positioned due to the loss of the perception model information, the first lane line is discretized, the ground plane where the vehicle at the current position is located is fitted by utilizing a plurality of lane line discrete points, the normal vector of the ground plane can be used as the actual normal vector of the vehicle body, the roll angle and the pitch angle of the vehicle body are corrected by utilizing the difference between the actual normal vector and the current vehicle body normal vector, the problem that the vehicle cannot be positioned due to the loss of the perception model information is solved, and the robustness of a positioning system is improved.
Example two
Referring to fig. 2a, fig. 2a is a schematic flow chart of a method for correcting a vehicle body heading according to an embodiment of the invention. The embodiment is optimized on the basis of the embodiment, and the heading of the vehicle is further corrected. As shown in fig. 2a, the method comprises:
210. and screening out a first lane line meeting a set distance range with the current position in the lane where the vehicle is located currently according to the current position of the vehicle provided by the preset positioning device.
220. And screening out a second lane line with the direction consistent with the driving direction of the vehicle from the first lane line.
In the present embodiment, when the vehicle travels on a high-precision map section, the vehicle generally travels along the lane line, and therefore the heading of the vehicle can be corrected using the track direction of the lane line. Generally, when a vehicle performs an operation such as steering or lane changing, the vehicle traveling direction is different from the current lane line trajectory direction, and therefore, it is generally impossible to correct the vehicle heading using the lane line, that is, when the vehicle is corrected for heading, it is necessary to recognize the lane line that matches the vehicle traveling direction.
For example, according to the attribute of the first lane line, the lane lines such as the incoming line, the stop line, and the tide line, which are not consistent with the driving direction of the vehicle, can be filtered, and the second lane line consistent with the driving direction of the vehicle is reserved. The driving direction of the vehicle can be judged through the vehicle-mounted gyroscope.
230. If there are a plurality of target lane lines with a set number of direction vectors matching each other in the second lane line, the average value of the direction vectors of the plurality of target lane lines is used as the actual direction vector of the vehicle body.
The direction vector of the vehicle line is a tangent vector of discrete points of the lane line. For a plurality of second lane lines, the tangent vector of each second lane line may be counted, and if the direction vectors of most (e.g., 90%) of the second lane lines are consistent, the average of the direction vectors of a plurality of target lane lines where all the direction vectors are consistent is taken as the actual direction vector of the vehicle body. In this embodiment, the actual direction vector of the vehicle body is calculated to correct the heading of the vehicle by using the track direction of the lane line.
240. And correcting the course of the vehicle at the current position according to the difference between the actual direction vector and the current direction vector of the vehicle body.
The current direction vector of the vehicle body can be obtained through the vehicle position provided by the GPS and the acceleration and the angular speed of the vehicle provided by the IMU.
For example, the heading of the vehicle at the current position may be corrected by using a kalman filter algorithm according to a difference between the actual direction vector and the current direction vector of the vehicle body, and the specific correction process may refer to a correction manner of a roll angle and a pitch angle of the vehicle body, which is not described in detail in this embodiment.
Specifically, fig. 2b is a schematic diagram before the heading correction is performed on the vehicle body according to the embodiment of the present invention, and fig. 2c is a schematic diagram after the heading correction is performed on the vehicle body according to the embodiment of the present invention. As shown in fig. 2b, 1 represents the actual direction vector of the vehicle body, 2 represents the current direction vector of the vehicle body, 3 represents the actual normal vector of the vehicle body, and 4 represents the current normal vector of the vehicle body. Before the course of the vehicle body is corrected, the actual direction vector of the vehicle body is deviated from the current direction vector. As shown in fig. 2c, by correcting the heading of the vehicle body, the current direction vector 2 of the vehicle body can be continuously close to the actual direction vector 1 of the vehicle body.
The technical scheme provided by the embodiment solves the problem that the vehicle course cannot be positioned when the information of the perception model is lost, and improves the robustness of the positioning system.
EXAMPLE III
Fig. 3a is a schematic flow chart of a method for correcting vehicle body elevation according to an embodiment of the present invention, and as shown in fig. 3a, the method for correcting vehicle body elevation includes:
310. and screening out a first lane line meeting a set distance range with the current position in the lane where the vehicle is located currently according to the current position of the vehicle provided by the preset positioning device.
When a vehicle passes through a main road, a secondary road, an elevated road and other special conditions, multiple road sections may exist at the same position, so that the vehicle needs to be distinguished, the current road section where the vehicle is located is filtered, and a first lane line in a certain range at the current position of the vehicle is cut out.
320. And determining the average value of the elevation of the first lane line based on a preset navigation map.
330. And taking the average value of the elevations of the first lane lines as the elevation of the vehicle at the current position so as to correct the elevation of the vehicle at the current position.
In this embodiment, the elevation of the vehicle refers to the altitude of the vehicle provided by the consumer-grade location device. When a vehicle is positioned by using a consumer-grade positioning device, the positioning precision is low, sometimes errors of several meters or even tens of meters exist, but the height change of the same road section is small in the error range. Therefore, according to the current position of the vehicle provided by the consumer-level positioning equipment, the lane line information which is several meters near the current position can be searched in the navigation map, so that the elevation of the vehicle can be corrected by using the lane line information, and the vehicle can be pulled to the height of the lane line.
Specifically, the lane line within a range of several meters around the current position of the vehicle may be searched in the navigation map, and the average height of the lane line, that is, the elevation of the lane line, may be calculated. And taking the calculated elevation of the lane line as the elevation of the current position of the vehicle so as to finish the correction of the elevation of the vehicle. Fig. 3b is a schematic diagram of performing elevation correction on a vehicle body according to an embodiment of the present invention, and as shown in fig. 3b, after performing elevation correction on the vehicle body by using a first lane line, a vehicle may be pulled down to the lane line height.
In summary, the technical solution provided by the above embodiment can still provide the four-degree-of-freedom information of the elevation, pitch angle, roll angle and course angle of the vehicle when the sensing information is missing, so as to keep the vehicle running normally, solve the problem that the vehicle cannot be positioned when the sensing model information is missing, and improve the robustness of the positioning system.
It should be noted that, in the embodiment of the present invention, there is no difference in the order of correcting the elevation, the pitch angle, the roll angle, and the heading of the vehicle, and the correction may be performed synchronously or sequentially. In the embodiment of the invention, the elevation of the vehicle can be preferably corrected first, so that the accuracy of subsequent posture correction is improved. After elevation correction is completed, roll angle, pitch angle and course of the vehicle can be corrected in sequence, so that the vehicle can be ensured to normally run under the condition that the perception model is absent and cannot be positioned.
It should be further noted that, when the sensing model is normal, the vehicle pose may also be corrected by using the technical solution of the embodiment of the present invention, and the corrected result is fused with the vehicle pose information obtained by using the sensing model, so as to improve the positioning accuracy of the vehicle.
Example four
Referring to fig. 4, fig. 4 is a schematic structural diagram of a device for correcting a vehicle pose according to an embodiment of the present invention. As shown in fig. 4, the apparatus includes: a first lane line screening module 410, a ground plane fitting module 420, and an angle correction module 430;
the first lane line screening module 410 is configured to screen out a first lane line, which meets a set distance range with a current position, in a lane where a vehicle is currently located according to the current position of the vehicle provided by a preset positioning device;
a ground plane fitting module 420, configured to discretize the first lane line to obtain a plurality of lane line discrete points, and fit a ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points;
and an angle correction module 430, configured to use the normal vector of the ground plane as an actual normal vector of the vehicle body, and correct the roll angle and the pitch angle of the vehicle body by using a difference between the actual normal vector and a current vehicle body normal vector.
According to the technical scheme, when the vehicle cannot be positioned due to the loss of the perception model information, the first lane line is discretized, the ground plane where the vehicle at the current position is located is fitted by utilizing a plurality of lane line discrete points, the normal vector of the ground plane can be used as the actual normal vector of the vehicle body, the roll angle and the pitch angle of the vehicle body can be corrected by utilizing the difference between the actual normal vector and the current vehicle body normal vector, the problem that the vehicle cannot be positioned due to the loss of the perception model information is solved, and the robustness of a positioning system is improved.
Optionally, the ground plane fitting module is specifically configured to:
discretizing the first lane line to obtain a plurality of lane line discrete points, and randomly selecting a preset number of target discrete points from the plurality of lane line discrete points;
fitting a plurality of fitted ground planes where the vehicle is located at the current position by using the target discrete points;
for any one fitted ground plane, judging the sum of the distances from other discrete points except the target discrete point to the fitted ground plane;
and selecting the fitted ground plane corresponding to the distance with the minimum sum value from the sum of the distances as the ground plane where the vehicle is located at the current position.
Optionally, the angle correction module is specifically configured to:
and taking the normal vector of the ground plane as an actual normal vector of the vehicle body, and correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector based on a Kalman filtering algorithm.
Optionally, the apparatus further comprises:
the second lane line screening module is configured to screen out a second lane line of which the direction is consistent with the driving direction of the vehicle from the first lane line;
an actual direction vector determination module configured to, in the second lane line, if there are a plurality of target lane lines whose number reaches a set number and whose direction vectors are consistent, take an average of the plurality of target lane line direction vectors as an actual direction vector of the vehicle body;
and the orientation correction module is configured to correct the heading of the vehicle at the current position according to the difference value between the actual direction vector and the current direction vector of the vehicle body.
Optionally, the apparatus further comprises:
a lane line elevation determination module configured to determine an average of elevations of a first lane line based on a preset navigation map;
and the vehicle elevation correction module is configured to take the average value of the elevations of the first lane lines as the elevation of the vehicle at the current position so as to correct the elevation of the vehicle at the current position.
The vehicle pose correction device provided by the embodiment of the invention can execute the vehicle pose correction method provided by any embodiment of the invention, and has corresponding functional modules and beneficial effects of the execution method. Technical details that are not described in detail in the above embodiments may be referred to a method of correcting a vehicle pose provided in any embodiment of the present invention.
EXAMPLE five
Referring to fig. 5, fig. 5 is a schematic structural diagram of a vehicle-mounted terminal according to an embodiment of the present invention. As shown in fig. 5, the in-vehicle terminal may include:
a memory 701 in which executable program code is stored;
a processor 702 coupled to 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 according to any embodiment of the present invention.
The embodiment of the invention discloses a computer-readable storage medium which stores a computer program, wherein the computer program enables a computer to execute the method for correcting the vehicle pose provided by any embodiment of the invention.
The embodiment of the 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 method for correcting the vehicle pose provided by any embodiment of the invention.
In various embodiments of the present invention, it should be understood that the sequence numbers of the above-mentioned processes do not imply an inevitable order of execution, and the execution order of the processes should be determined by their functions and inherent logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.
In the embodiments provided herein, it should be understood that "B corresponding to A" means that B is associated with A from which B can be determined. It should also be understood that determining B from a does not mean determining B from a alone, but may also be determined from a and/or other information.
In addition, 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 are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The integrated units, if implemented as software functional units and sold or used as a stand-alone product, may be stored in a computer accessible memory. Based on such understanding, the technical solution of the present invention, which is a part of or contributes to the prior art in essence, or all or part of the technical solution, can be embodied in the form of a software product, which is stored in a memory and includes several requests for causing a computer device (which may be a personal computer, a server, a network device, or the like, and may specifically be a processor in the computer device) to execute part or all of the steps of the above-described method of each embodiment of the present invention.
It will be understood by those skilled in the art that all or part of the steps in the methods of the embodiments described above may be implemented by hardware instructions of a program, and the program may be stored in a computer-readable storage medium, where the storage medium includes 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), or other Memory, such as a magnetic disk, or a combination thereof, A tape memory, or any other medium readable by a computer that can be used to carry or store data.
The method and the device for correcting the vehicle pose disclosed by the embodiment of the invention are described in detail, the principle and the implementation mode of the invention are explained by applying specific examples, and the description of the embodiment is only used for helping to understand the method and the core idea of the invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.
Claims (8)
1. A vehicle pose correction method is characterized by comprising the following steps:
screening out a first lane line which meets a set distance range with the current position in a lane where the vehicle is located at present according to the current position of the vehicle provided by a preset positioning device;
discretizing the first lane line to obtain a plurality of lane line discrete points, and fitting a ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points;
taking the normal vector of the ground plane as an actual normal vector of the vehicle body, and correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector;
wherein, the fitting out the ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points comprises:
randomly selecting a preset number of target discrete points from the plurality of lane line discrete points;
fitting a plurality of fitted ground planes where the vehicle is located at the current position by using the target discrete points;
for any one fitted ground plane, judging the sum of the distances from other discrete points except the target discrete point to the fitted ground plane;
and selecting the fitted ground plane corresponding to the distance with the minimum sum value from the sum of the distances as the ground plane where the vehicle is located at the current position.
2. The method of claim 1, wherein using the difference between the actual normal vector and the current vehicle body normal vector to correct roll and pitch angles of the vehicle body comprises:
and based on a Kalman filtering algorithm, correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector.
3. The method according to any one of claims 1-2, further comprising:
screening out a second lane line with the direction consistent with the driving direction of the vehicle from the first lane line;
if a plurality of target lane lines with the same direction vector number and the set number are present in the second lane line, taking the average value of the direction vectors of the plurality of target lane lines as the actual direction vector of the vehicle body;
and correcting the course of the vehicle at the current position according to the difference between the actual direction vector and the current direction vector of the vehicle body.
4. The method of claim 1, further comprising:
determining an average value of the elevations of the first lane line based on a preset navigation map;
and taking the average value of the elevations of the first lane lines as the elevation of the vehicle at the current position so as to correct the elevation of the vehicle at the current position.
5. A vehicle pose correction device applied to automatic driving is characterized by comprising:
the first lane line screening module is configured to screen out a first lane line, which meets a set distance range with a current position, in a lane where the vehicle is located currently according to the current position of the vehicle provided by a preset positioning device;
the ground plane fitting module is configured to discretize the first lane line to obtain a plurality of lane line discrete points, and fit out a ground plane where the vehicle is located at the current position based on the plurality of lane line discrete points;
the angle correction module is configured to take the normal vector of the ground plane as an actual normal vector of a vehicle body, and correct the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector;
wherein, the ground plane fitting module is specifically configured to:
discretizing the first lane line to obtain a plurality of lane line discrete points, and randomly selecting a preset number of target discrete points from the plurality of lane line discrete points;
fitting a plurality of fitted ground planes where the vehicle is located at the current position by using the target discrete points;
for any one fitted ground plane, judging the sum of the distances from other discrete points except the target discrete point to the fitted ground plane;
and selecting the fitted ground plane corresponding to the distance with the minimum sum value from the sum of the distances as the ground plane where the vehicle is located at the current position.
6. The apparatus of claim 5, wherein the angle correction module is specifically configured to:
and taking the normal vector of the ground plane as an actual normal vector of the vehicle body, and correcting the roll angle and the pitch angle of the vehicle body by using the difference between the actual normal vector and the current vehicle body normal vector based on a Kalman filtering algorithm.
7. The apparatus of claim 5, further comprising:
the second lane line screening module is configured to screen out a second lane line of which the direction is consistent with the driving direction of the vehicle from the first lane line;
an actual direction vector determination module configured to, in the second lane line, if there are a plurality of target lane lines whose number reaches a set number and whose direction vectors are consistent, take an average of the plurality of target lane line direction vectors as an actual direction vector of the vehicle body;
and the orientation correction module is configured to correct the heading of the vehicle at the current position according to the difference value between the actual direction vector and the current direction vector of the vehicle body.
8. The apparatus of claim 5, further comprising:
a lane line elevation determination module configured to determine an average of elevations of a first lane line based on a preset navigation map;
and the vehicle elevation correction module is configured to take the average value of the elevations of the first lane lines as the elevation of the vehicle at the current position so as to correct the elevation of the vehicle at the current position.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910346790.1A CN111854727B (en) | 2019-04-27 | 2019-04-27 | Vehicle pose correction method and device |
| PCT/CN2019/113484 WO2020220616A1 (en) | 2019-04-27 | 2019-10-26 | Vehicle pose correction method and apparatus |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910346790.1A CN111854727B (en) | 2019-04-27 | 2019-04-27 | Vehicle pose correction method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111854727A CN111854727A (en) | 2020-10-30 |
| CN111854727B true CN111854727B (en) | 2022-05-13 |
Family
ID=72951422
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910346790.1A Active CN111854727B (en) | 2019-04-27 | 2019-04-27 | Vehicle pose correction method and device |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN111854727B (en) |
| WO (1) | WO2020220616A1 (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112284400B (en) * | 2020-12-24 | 2021-03-19 | 腾讯科技（深圳）有限公司 | Vehicle positioning method and device, electronic equipment and computer readable storage medium |
| CN113110447B (en) * | 2021-04-13 | 2022-11-25 | 上海新纪元机器人有限公司 | Relative position relation analysis and system |
| CN114140538B (en) * | 2021-12-03 | 2022-09-27 | 禾多科技（北京）有限公司 | Vehicle-mounted camera pose adjusting method, device, equipment and computer readable medium |
| CN115143996B (en) * | 2022-09-05 | 2023-01-17 | 北京智行者科技股份有限公司 | Positioning information correction method, electronic device, and storage medium |
| CN115993137B (en) * | 2023-02-22 | 2023-06-13 | 禾多科技（北京）有限公司 | Vehicle positioning evaluation method, device, electronic equipment and computer readable medium |
| CN116892898B (en) * | 2023-09-11 | 2024-02-02 | 农业农村部南京农业机械化研究所 | Track error detection method, device and system for agricultural machinery |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107643086A (en) * | 2016-07-22 | 2018-01-30 | 北京四维图新科技股份有限公司 | A kind of vehicle positioning method, apparatus and system |
| CN108732584A (en) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | Method and apparatus for updating map |
| CN108986450A (en) * | 2018-07-25 | 2018-12-11 | 北京万集科技股份有限公司 | Vehicle environmental cognitive method, terminal and system |
| WO2019043833A1 (en) * | 2017-08-30 | 2019-03-07 | 日産自動車株式会社 | Method for correcting positional error and device for correcting positional error in driving assistance vehicle |
| CN109581449A (en) * | 2018-12-14 | 2019-04-05 | 安徽江淮汽车集团股份有限公司 | A kind of localization method and system of autonomous driving vehicle |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2874300B1 (en) * | 2004-08-11 | 2006-11-24 | Renault Sas | AUTOMATIC CALIBRATION METHOD OF A STEREOVISION SYSTEM |
| CN106767853B (en) * | 2016-12-30 | 2020-01-21 | 中国科学院合肥物质科学研究院 | Unmanned vehicle high-precision positioning method based on multi-information fusion |
| CN108303103B (en) * | 2017-02-07 | 2020-02-07 | 腾讯科技（深圳）有限公司 | Method and device for determining target lane |
| CN109676604B (en) * | 2018-12-26 | 2020-09-22 | 清华大学 | Robot curved surface motion positioning method and motion positioning system thereof |
-
2019
- 2019-04-27 CN CN201910346790.1A patent/CN111854727B/en active Active
- 2019-10-26 WO PCT/CN2019/113484 patent/WO2020220616A1/en active Application Filing
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107643086A (en) * | 2016-07-22 | 2018-01-30 | 北京四维图新科技股份有限公司 | A kind of vehicle positioning method, apparatus and system |
| CN108732584A (en) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | Method and apparatus for updating map |
| WO2019043833A1 (en) * | 2017-08-30 | 2019-03-07 | 日産自動車株式会社 | Method for correcting positional error and device for correcting positional error in driving assistance vehicle |
| CN108986450A (en) * | 2018-07-25 | 2018-12-11 | 北京万集科技股份有限公司 | Vehicle environmental cognitive method, terminal and system |
| CN109581449A (en) * | 2018-12-14 | 2019-04-05 | 安徽江淮汽车集团股份有限公司 | A kind of localization method and system of autonomous driving vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111854727A (en) | 2020-10-30 |
| WO2020220616A1 (en) | 2020-11-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111854727B (en) | Vehicle pose correction method and device | |
| CN111750881B (en) | Vehicle pose correction method and device based on light pole | |
| CN110954112A (en) | Method and device for updating matching relation between navigation map and perception image | |
| CN101819042B (en) | Navigation device and navigation program | |
| CN101819044B (en) | Navigation device and navigation program | |
| CN111750878B (en) | Vehicle pose correction method and device | |
| EP3936822B1 (en) | Vehicle positioning method and apparatus, and vehicle, and storage medium | |
| JP2020516880A (en) | Method and apparatus for reducing midpoints in a polygon | |
| CN111380539A (en) | Vehicle positioning and navigation method and device and related system | |
| CN111750882B (en) | Method and device for correcting vehicle pose during initialization of navigation map | |
| KR20200119920A (en) | Appratus and method for estimating the position of an automated valet parking system | |
| JPH08502591A (en) | Map matching with competing sensing locations | |
| KR102441073B1 (en) | Apparatus for compensating sensing value of gyroscope sensor, system having the same and method thereof | |
| CN112163475A (en) | Method and device for determining lane line direction | |
| US20210041248A1 (en) | Method for estimating the position of a vehicle on a map | |
| CN111194397B (en) | Method for operating a navigation system | |
| CN112912894B (en) | Road boundary identification method and device | |
| US10839005B2 (en) | Dynamic graphic information classification device and method | |
| CN110702135A (en) | Navigation method and device for vehicle, automobile and storage medium | |
| CN112797998A (en) | Vehicle lane level positioning method, corresponding program carrier, product, device and vehicle | |
| JP2010151691A (en) | Road shape estimation device, road shape estimation method, and road shape estimation program | |
| CN113341443A (en) | Processing method of positioning track information and vehicle-mounted navigation device | |
| KR102231563B1 (en) | Method for matching map of high-precision with navigation link | |
| US20210139046A1 (en) | Method for operating a more highly automated vehicle (hav), in particular a highly automated vehicle | |
| CN114454888A (en) | Lane line prediction method and device, electronic equipment and vehicle |
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 | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20220303 Address after: 100083 unit 501, block AB, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing Applicant after: BEIJING MOMENTA TECHNOLOGY Co.,Ltd. Address before: Room 28, 4 / F, block a, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing 100089 Applicant before: BEIJING CHUSUDU TECHNOLOGY Co.,Ltd. |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |