CN117606508A - Method and device for acquiring closest point in track, electronic equipment and storage medium - Google Patents
Method and device for acquiring closest point in track, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN117606508A CN117606508A CN202410089974.5A CN202410089974A CN117606508A CN 117606508 A CN117606508 A CN 117606508A CN 202410089974 A CN202410089974 A CN 202410089974A CN 117606508 A CN117606508 A CN 117606508A
- Authority
- CN
- China
- Prior art keywords
- point
- suspected
- points
- cache
- locating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 66
- 238000012216 screening Methods 0.000 claims abstract description 31
- 238000004590 computer program Methods 0.000 claims description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 238000004891 communication Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000002457 bidirectional effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/36—Input/output arrangements for on-board computers
- G01C21/3667—Display of a road map
- G01C21/3676—Overview of the route on the road map
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
Abstract
The application provides a method and a device for acquiring the closest point in a track, electronic equipment and a storage medium, relates to the field of automatic driving, and aims to solve the problem of accuracy of a mode for acquiring the closest point in the track according to a distance in the prior art. The method comprises the following steps: screening effective suspected points according to the included angles formed by the two adjacent suspected points and the locating points, and deleting the rest points; screening out effective suspected points again according to the course angle formed by two adjacent suspected points and the course angle of the locating point, and deleting the rest points; if the suspected point set is not empty and the n value does not exceed the preset threshold, judging whether the suspected point set contains a cache point or the next point of the cache point, if not, dividing the suspected point set into two groups according to coordinates, selecting one group close to the cache point, and taking the point closest to the locating point in the group as the point closest to the locating point in the reference track.
Description
Technical Field
The present invention relates to the field of autopilot, and in particular, to a method and apparatus for acquiring a closest point in a track, an electronic device, and a storage medium.
Background
In the automatic driving process, the routes of the vehicle which are passed and not passed are required to be displayed on a screen, the automatic driving system can provide a reference track and vehicle positioning information, and the vehicle is required to find track coordinates corresponding to the current positioning information of the vehicle on the reference track to display the routes of the vehicle which are passed and not passed. In general, a locus point having the smallest distance from the current position of the vehicle (the current position is obtained from the positioning information) in the reference locus is taken as a locus point corresponding to the current positioning of the vehicle. However, in a scene such as a bidirectional lane or a curve, the nearest point found in the above manner may be wrong, resulting in misleading passengers by displaying a route that has not been travelled as already travelled on the screen or displaying a route that has been travelled as not travelled.
Disclosure of Invention
In order to solve the above problems, an embodiment of the present application provides a method and an apparatus for acquiring a closest point in a track, an electronic device, and a storage medium.
An embodiment of a first aspect of the present application provides a method for acquiring a closest point in a track, including the steps of: screening effective suspected points from a suspected point set according to an included angle formed by two adjacent suspected points and a locating point, and deleting the rest of the suspected points from the suspected point set, wherein the suspected points are n points closest to the locating point in a reference track, a set formed by the n suspected points is called a suspected point set, and the effective suspected point is a suspected point possibly closest to the locating point in the n suspected points; screening out effective suspected points again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point, and deleting the rest suspected points from the suspected point set; if the suspected point set is not empty and the value of n does not exceed a preset threshold, judging whether the suspected point set contains a cache point or a next point of the cache point, wherein the cache point is the last acquired nearest point; and if the suspected point set does not contain the cache point and the next point of the cache point, dividing the suspected point set into two groups according to coordinates, selecting one group close to the cache point, and taking the point closest to the locating point in the group as the point closest to the locating point in the reference track.
In one possible implementation manner, the screening the effective suspected points from the suspected point set according to the included angle formed by the adjacent two suspected points and the locating point includes: traversing each suspected point in the suspected point set, taking an angle formed by a last point of the suspected point, the locating point and the suspected point as a first included angle, taking an angle formed by the suspected point, the locating point and a next point of the suspected point as a second included angle, and taking an angle formed by the last point of the suspected point, the suspected point and the next point of the suspected point as a third included angle, wherein if the first included angle and the second included angle are smaller than 90 degrees, the sum of the first included angle and the second included angle is larger than 90 degrees, and the third included angle is larger than 90 degrees, taking the suspected point as a point closest to the locating point in an effective suspected point and the reference track; if the first included angle is larger than 90 degrees, the suspected point is used as an effective suspected point; and if the second included angle is larger than 90 degrees, the suspected point is used as an effective suspected point.
In one possible implementation manner, the re-screening the valid suspected points according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point includes: traversing each suspected point in the suspected point set, and taking the suspected point as an effective suspected point if the absolute value of the difference between the heading angle from the last point of the suspected point to the suspected point and the heading angle of the locating point is smaller than or equal to 90 degrees.
In one possible implementation manner, the method further includes: and if the suspected point set comprises the cache point or the next point of the cache point, taking the cache point or the next point of the cache point as the point closest to the positioning point in the reference track.
In one possible implementation manner, the taking the cache point or a point next to the cache point as a point closest to the positioning point in the reference track includes: if the suspected point set contains the cache point but does not contain the next point of the cache point, taking the cache point as the point closest to the locating point in the reference track; if the suspected point set contains the next point of the cache point but does not contain the cache point, taking the next point of the cache point as a point closest to the positioning point in the reference track; and if the suspected point set comprises the cache point and the next point of the cache point, taking the point which is close to the locating point in the cache point and the next point of the cache point as the point which is closest to the locating point in the reference track.
In one possible implementation manner, the method further includes: and if the suspected point set is empty and the value of n does not exceed the threshold value, increasing the value of n, returning to the step of screening effective suspected points from the suspected point set according to the included angle formed by the adjacent two suspected points and the locating point, and deleting the rest of suspected points from the suspected point set.
An embodiment of a second aspect of the present application provides a method apparatus for acquiring a closest point in a track, including: the first effective point screening unit is suitable for screening effective suspected points from a suspected point set according to an included angle formed by two adjacent suspected points and a locating point, deleting the rest of the suspected points from the suspected point set, wherein the suspected points are n points closest to the locating point in a reference track, a set formed by the n suspected points is called a suspected point set, and the effective suspected points are suspected points possibly closest to the locating point in the n suspected points; the second effective point screening unit is suitable for screening effective suspected points again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point, and deleting the rest suspected points from the suspected point set; the judging unit is adapted to judge whether the suspected point set contains a cache point or a next point of the cache point if the suspected point set is not empty and the value of n does not exceed a preset threshold, wherein the cache point is the last acquired nearest point; and the grouping unit is suitable for dividing the suspected point set into two groups according to coordinates if the suspected point set does not contain the cache point and the next point of the cache point, selecting one group close to the cache point, and taking the point closest to the locating point in the group as the point closest to the locating point in the reference track.
Embodiments of the third aspect of the present application further provide an electronic device, including: at least one processor and a memory storing a computer program; the computer program, when read and executed by the processor, causes the electronic device to perform the method of acquiring the closest point in the trajectory as described above.
The fourth aspect of the present application also provides a readable storage medium storing a computer program, which when read and executed by an electronic device, causes the electronic device to execute the method of acquiring the closest point in the trajectory as above.
According to the method and device for acquiring the nearest point in the track, the electronic equipment and the storage medium, factors such as the distance, the included angle between the suspected point and the locating point, the course angle and the like are comprehensively considered to exclude noise points, the point closest to the locating point in the reference track is acquired by combining the cache point, the accuracy of the acquired nearest point is obviously improved, and the automatic correction time is obviously shortened.
Drawings
FIG. 1 is a schematic diagram of an electronic device 100 according to one embodiment of the invention;
FIG. 2 is a flow chart of a method 200 of acquiring a closest point in a trajectory according to one embodiment of the invention;
FIG. 3 is a schematic diagram of an included angle between a suspected point and a locating point according to an embodiment of the present invention;
FIG. 4 is another schematic view of an included angle between a suspected point and a locating point according to an embodiment of the present invention;
FIG. 5 is a third schematic diagram illustrating an included angle between a suspected point and a locating point according to an embodiment of the present invention;
FIG. 6 is a detailed flow chart of a method of acquiring the closest point in a trajectory according to one embodiment of the invention;
fig. 7 is a schematic diagram of an acquisition device 700 for the closest point in a trajectory according to one embodiment of the invention.
Detailed Description
In order to make the technical solutions and advantages of the embodiments of the present application more apparent, the following detailed description of exemplary embodiments of the present application is given with reference to the accompanying drawings, and it is apparent that the described embodiments are only some of the embodiments of the present application and not exhaustive of all the embodiments. It should be noted that, in the case of no conflict, the embodiments and features in the embodiments may be combined with each other.
In the prior art, the point closest to the positioning point in the reference track is obtained only according to the distance, the accuracy is obviously reduced under certain road conditions, once the incorrect closest point is obtained, the incorrect closest point is continuously obtained for a long time, and the recovery time is long. In order to solve the above-mentioned problems, the embodiment of the present application provides a method for acquiring the closest point in the track.
The method for acquiring the closest point in the track in the embodiment of the application is executed in the electronic equipment. Fig. 1 is a schematic diagram of an electronic device 100 according to one embodiment of the invention. As shown in fig. 1, the electronic device 100 includes at least a memory 110, a processor 120, and a wireless communication device 130. The memory 110 stores program instructions for executing the method of acquiring the closest point in the track, and the processor 120 reads and executes the method of acquiring the closest point in the track from the memory 110. The wireless communication device 130 is used to implement communication between the electronic device 100 and the cloud.
The method of acquiring the closest point in the trajectory of the present embodiment is performed in an electronic device, such as the electronic device 100 described above.
The method for acquiring the closest point in the track in the embodiment comprises the following steps: screening effective suspected points from a suspected point set according to an included angle formed by two adjacent suspected points and a locating point, and deleting the rest of the suspected points from the suspected point set, wherein the suspected points are n points closest to the locating point in a reference track, a set formed by the n suspected points is called a suspected point set, and the effective suspected point is a suspected point possibly closest to the locating point in the n suspected points; screening out effective suspected points again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point, and deleting the rest suspected points from the suspected point set; if the suspected point set is not empty and the value of n does not exceed a preset threshold, judging whether the suspected point set contains a cache point or a next point of the cache point, wherein the cache point is the last acquired nearest point; and if the suspected point set does not contain the cache point and the next point of the cache point, dividing the suspected point set into two groups according to coordinates, selecting one group close to the cache point, and taking the point closest to the locating point in the group as the point closest to the locating point in the reference track.
Fig. 2 is a flowchart of a method 200 for acquiring a closest point in a track according to an embodiment of the present invention, and as shown in fig. 2, the method 200 starts in step S210.
In step S210, according to the included angle formed between the adjacent two suspected points and the positioning point, effective suspected points are selected from the suspected point set, and the rest of the suspected points are deleted from the suspected point set. The suspected points are n points closest to the positioning point in the reference track, a set formed by the n suspected points is called a suspected point set, and the effective suspected point is the suspected point which possibly becomes closest to the positioning point in the n suspected points.
When receiving the trigger signal, the electronic device 100 obtains n points closest to the positioning point currently in the reference track and located away from the positioning point, where the n points form a suspicious point set.
In one implementation, the trigger signal may be sent at a fixed frequency, such as every 1 second.
In another implementation manner, the trigger signal may be sent according to the sparse and dense condition of the track points (hereinafter referred to as points) in the reference track, for example, the track points are dense, the sending frequency of the trigger signal is high, the track points are sparse, and the sending frequency of the trigger signal is low.
Upon receiving the trigger signal, the electronic device 100 begins to execute the method 200. First, the position of the vehicle at the current moment is acquired according to the positioning information output by the positioning source configured by the vehicle, the position is called a positioning point of the vehicle at the current moment, and then the distance between each track point in the reference track and the positioning point is calculated.
In a first implementation manner, a range is preset as a search range, a locating point is used as a center to determine the boundary of the range, a part of a reference track falls into the range, coordinates of all track points of the part of reference track are obtained, distances between each track point and the locating point are calculated in sequence, and n track points closest to the locating point are selected as suspicious points according to a calculation result. According to the method, the suspected points are found in a certain range near the locating points, so that the suspected points are acquired more pertinently, and the efficiency and accuracy of acquiring the suspected points can be ensured.
If the number of points contained in the reference trajectory within the search range is smaller than n+2, the search range needs to be enlarged, specifically: (1) And simultaneously expanding the search range forwards and backwards by one point respectively, and after expanding, if the number of points contained in the reference track in the search range is still smaller than n+2, continuing to expand the search range forwards and backwards by one point respectively until the number of points contained in the reference track in the search range reaches n+2.
If the distances from the points in the search range to the positioning points are sequentially increased or sequentially decreased, it is stated that the point closest to the positioning points on the reference track is likely not to be in the range, and at this time, the search range needs to be enlarged. Specifically, if the distances from the respective points to the positioning points within the search range increase in order along the track traveling direction, the search range should be extended backward; conversely, if the distances from the respective points to the anchor points within the search range decrease in order along the track traveling direction, the search range should be extended forward.
In the present embodiment, the reference trajectory travel direction is used as a reference for both the front and rear, and the front and rear represent the front in the travel direction and the rear represents the direction opposite to the travel direction. The point subsequent to a certain point or the point next to a certain point refers to a point which is located in front of the certain point and closest to the certain point on the reference track with the traveling direction of the reference track as a reference; the point preceding a certain point or the point immediately preceding a certain point refers to a point located behind a certain point and closest to the certain point on the reference track with respect to the reference track traveling direction.
For example, a certain section of the reference track sequentially contains M i-2 、M i-1 、M i 、M i+1 、M i+2 、……、M i+n 、M i+n+1 、M i+n+2 N+5 points in total, where M i+1 、M i+2 、……、M i+n The 5 points are located in the initial search range if M i+1 、M i+2 、……、M i+n Distance d to the anchor point i+1 、d i+2 、……、d i+n Sequentially increasing, the initial search range should be extended backward by at least one point, i.e., M i Adding a search scope, then the new search scope contains M i 、M i+1 、M i+2 、……、M i+n There are 6 points in total. Calculate M i Distance d to the anchor point i If d i ≤d i+1 Continuing to expand at least one point backwards to make M i-1 Adding the search range, then calculating M i-1 Distance d to the anchor point i-1 If d i-1 ≤d i Continuing to expand at least one point backwards to make M i-2 Adding search Range, if d i-1 d i The expansion is stopped.
The search range may be various shapes, such as a circle or a rectangle, or other shapes, and the specific shape of the search range is not limited in the present application.
For example, a search range is set to be a circle with a radius of 5 meters, a circle is drawn by taking a locating point as a circle center and a radius of 5 meters, a part of reference tracks fall into the circle range, coordinates of points of the reference tracks in the circle are obtained, the distance between each point and the locating point is calculated, and then n points with the smallest distance are selected as suspected points.
For another example, the search range is set to be a rectangle with the length of 10 meters and the width of 1 meter, a locating point is taken as the center of the rectangle, the rectangle with the length of 10 meters and the width of 1 meter is drawn on the map, the direction of the long side of the rectangle is parallel to the course of the locating point, the course of the locating point is detected and obtained by a vehicle sensor, then a part of the reference track falls into the rectangular range, the coordinates of the points of the reference track in the rectangle are obtained, the distance between each point and the locating point is calculated, and then n points with the smallest distance are selected as suspected points.
In a second implementation manner, distances from n previous suspected points and a plurality of points in front of the n previous suspected points to the current locating point are calculated, and then n points with the smallest distances are selected as the suspected points. The last n suspected points are the n suspected points selected at the last execution of the method 200. The distance between the n selected suspected points and the current positioning point is the smallest when the method 200 is executed last time, and when the vehicle runs forward to the current position, the distance between the point behind the n suspected points and the current position (i.e. the current positioning point) is more probable than the distance between the n suspected points and the current position, so that backward expansion is not needed. In the two adjacent execution processes of the method 200, the two locating points are not far away, so that the n suspected points are selected by taking the n suspected points selected in the last execution of the method 200 as references, and the efficiency of selecting the suspected points can be improved. Specifically, n suspicious points at the last time and distances from 1 point in front of the n suspicious points at the last time to the positioning point can be calculated, the distances from n+1 points to the positioning point are ordered from small to large, if the distance from the forefront point to the positioning point is positioned in the first n minimum distances, forward expansion is continued, and the distance from the 2 nd point in front of the n suspicious points at the last time to the positioning point is calculated; sequencing the distances from n+2 points to the positioning points from small to large, and selecting the points arranged in the front n as suspicious points if the distances from the forefront point to the positioning points are arranged in the n+1th position or the position behind the n+1th position; if the distance from the forefront point to the locating point is still in the first n minimum distances, continuing to expand forwards, and calculating the distance from the 3 rd point to the locating point, which is positioned in front of the last n suspected points.
In a third implementation manner, the distance from the nearest point obtained last time to the current positioning point and the distances from a plurality of points in front of and behind the nearest point to the current positioning point are calculated, and then n points with the smallest distance are selected as suspected points. In the two adjacent execution processes of the method 200, the distance between the two positioning points is not very large, and correspondingly, the distance between the two nearest points is not very large, so that the n suspected points are selected by taking the nearest point selected in the last execution of the method 200 as a reference, and the efficiency of selecting the suspected points can be improved. Specifically, the last closest point, p points before the last closest point, and q points after the last closest point are selected first, and p+q+1 is not less than n+2. The distance from the initial p+q+1 points to the locating point is calculated, and n points with the smallest distance are selected from the distances.
If the forefront point in the initial p+q+1 points is not one of n points with the smallest distance, the forward expansion is not needed; if the forefront point in the p+q+1 points is one of n points with the smallest distance, one point is extended forward, namely, the p+1 point in front of the last nearest point is extended, then the distance from the p+1 point in front of the last nearest point to a locating point is calculated, the n points with the smallest distance are selected from the p+q+2 points, and whether the forward extension needs to be continued is judged in the same way.
If the rearmost point of the initial p+q+1 points is not one of the n points with the smallest distance, then no backward expansion is required; if the rearmost point of the p+q+1 points is one of n points with the smallest distance, one point is extended backward, namely, the (q+1) th point behind the last nearest point is extended, then the distance from the (q+1) th point in front of the last nearest point to a locating point is calculated, the n points with the smallest distance are selected from the p+q+2 points, and whether the backward extension is needed to be continued is judged in the same way.
Next, in step S220, effective suspicious points are screened again according to the course angle formed by two adjacent suspicious points in the suspicious point set and the course angle of the positioning point, and the rest suspicious points are deleted from the suspicious point set.
In this step, each suspected point in the suspected point set is traversed, an angle formed by a last point, a locating point and a suspected point of the suspected point is taken as a first included angle (the locating point is a vertex), an angle formed by the suspected point, the locating point and a next point of the suspected point is taken as a second included angle (the locating point is a vertex), an angle formed by the last point, the suspected point and the next point of the suspected point is taken as a third included angle (the suspected point is a vertex), effective suspected points are selected according to the first included angle, the second included angle and the third included angle, and other suspected points are deleted from the suspected point set, so that a course angle formed by two adjacent suspected points is calculated.
The valid suspected point is a suspected point that may be the closest point to the anchor point. For any one suspected point, it can be determined whether it is a valid suspected point by:
(1) If the first angle ACB and the second angle BCD are both less than 90 degrees, the sum of the first angle and the second angle ACD is greater than 90 degrees, and the third angle ABD is greater than 90 degrees, as shown in fig. 3, the suspected point may be directly used as the point closest to the positioning point in the reference track, and then the suspected point is naturally also an effective suspected point, so far, the method 200 ends;
(2) If the first included angle ACB is greater than 90 degrees, as shown in fig. 4, the suspected point is taken as an effective suspected point, and the distance from the locating point to the suspected point is calculated;
(3) If the second included angle BCD is greater than 90 degrees, as shown in fig. 5, the suspected point is taken as an effective suspected point, and the distance from the locating point to the suspected point is calculated.
If the included angle formed by two adjacent suspected points and the locating point (taking the locating point as a vertex) is smaller than 90 degrees and the sum of the two adjacent included angles is smaller than 90 degrees, the three suspected points are far away from the locating point; if the included angle formed by two adjacent suspected points and the locating point (taking the locating point as a vertex) is larger than 90 degrees, the locating point is located between the two suspected points, the two suspected points are possibly the points closest to the locating point on the reference track, the points on the reference track, which are obviously far away from the locating point, can be filtered out through the method, the points are deleted from the suspected point set, and the suspected points contained in the new suspected point set are all effective suspected points.
And then, effective suspected points are screened out again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point, and the rest suspected points are deleted from the suspected point set.
This step further filters out invalid suspected points. The specific method is as follows: and for any one of the suspected points in the suspected point set obtained in the last step, calculating the course angle from the last point of the suspected point to the suspected point, and if the absolute value of the difference between the course angle and the course angle of the locating point is smaller than or equal to 90 degrees, taking the suspected point as an effective suspected point. More specifically:
(1) If the course angle calculated in the previous step is an invalid course angle, filtering out the corresponding suspected points in the previous step;
(2) And acquiring the course angle of the vehicle at the current positioning point according to a sensor configured by the vehicle, and if the absolute value of the difference between the course angle calculated in the previous step and the course angle of the positioning point exceeds 90 degrees, indicating that the corresponding suspected point is not the point which the vehicle just passes or the point which the vehicle is about to reach, and filtering the suspected point.
This step is a further update of the set of suspected points.
Next, in step S230, if the suspected point set is not null and the value of n does not exceed the preset threshold, it is determined whether the suspected point set includes a cache point or a next point of the cache point, where the cache point is the last acquired nearest point.
The method includes the steps of firstly judging whether a suspected point set is empty or not and judging whether the value of n exceeds a preset threshold value or not.
Through the first three steps, the number of suspected points in the suspected point set is reduced, if the suspected point set becomes an empty set, the value of n may be increased reasonably in order to obtain the nearest point, and then step S210 is returned, where n points closest to the positioning point in the reference track are acquired again as the suspected points, as shown in fig. 6. In general, in order to maximize the computing efficiency, the initial value of n may be set to 6 and the maximum value of n (i.e., the threshold) may be set to 18, depending on the hardware configuration of the computing device 100. In this step, if the set of suspected points becomes an empty set, the value of n should be increased. Since the method 200 requires calculation of n suspected points once per cycle, this step can increase the value of n to 2n, and if the value of n still needs to be increased next, n should be increased to 3n. When the value of n reaches the threshold 18, if the set of suspected points is still empty, the acquisition of the point closest to the locating point is aborted, ending the method 200.
If the result of the step is that the suspected point set is not empty and the value of n does not exceed the threshold, further judging whether the suspected point set contains a cache point or the next point of the cache point. The cache point refers to the most recent point that was obtained last time the method 200 was run.
And if the suspected point set comprises a cache point or a next point of the cache point, taking the cache point or the next point of the cache point as a point closest to the locating point in the reference track.
If the suspected point set comprises a cache point and a next point of the cache point, calculating the distance from the cache point to the locating point and the distance from the next point of the cache point to the locating point, and selecting one point with smaller distance from the cache point and the next point of the cache point as a point closest to the locating point in the reference track.
And if the suspected point set comprises the cache point and does not comprise the next point of the cache point, taking the cache point as the point closest to the locating point in the reference track.
And if the suspected point set does not contain the cache point but contains the next point of the cache point, taking the next point of the cache point as the point closest to the locating point in the reference track.
If the suspected point set contains neither the cache point nor the next point of the cache point, the process proceeds to step S240.
In step S240, the set of suspected points is divided into two groups according to coordinates, a group of points close to the cache is selected, and a point closest to the locating point in the group is taken as a point closest to the locating point in the reference track, as shown in fig. 6.
In a first implementation manner, under a rectangular coordinate system, if the heading of the locating point is close to the horizontal axis and far from the vertical axis, ordering all the suspected points in the suspected point set according to the size of the horizontal coordinate; taking two suspected points with the maximum and minimum abscissa, and calculating the intermediate value (i.e. average value) of the abscissa of the two suspected points; then, the suspected points with the abscissa larger than the intermediate value are taken as one group, and the suspected points with the abscissa smaller than the intermediate value are taken as another group; and then judging which group the cache point is close to, if the cache point coordinate is close to the group with larger abscissa, selecting the point closest to the positioning point from the group with larger abscissa as the point closest to the positioning point in the reference track, and if the cache point coordinate is close to the group with smaller abscissa, selecting the point closest to the positioning point from the group with smaller abscissa as the point closest to the positioning point in the reference track.
In a second implementation manner, under a rectangular coordinate system, if the heading of the locating point is close to the vertical axis and far from the horizontal axis, ordering all the suspected points in the suspected point set according to the size of the vertical coordinate; taking two suspected points with the maximum and minimum ordinate, and calculating the intermediate value (i.e. average value) of the ordinate of the two suspected points; then, the suspected points with the ordinate larger than the intermediate value are taken as one group, and the suspected points with the ordinate smaller than the intermediate value are taken as another group; and then judging which group the cache point is close to, if the cache point coordinate is close to the group with the larger ordinate, selecting the point closest to the positioning point from the group with the larger ordinate as the point closest to the positioning point in the reference track, and if the cache point coordinate is close to the group with the smaller ordinate, selecting the point closest to the positioning point from the group with the smaller ordinate as the point closest to the positioning point in the reference track.
In a third implementation manner, taking the course of the positioning point as the horizontal axis/vertical axis of the rectangular coordinate system, and sorting all the suspected points in the suspected point set according to the size of the horizontal coordinate/vertical coordinate; taking two suspected points with the maximum and minimum horizontal/vertical coordinates, and calculating the intermediate value (i.e. average value) of the horizontal/vertical coordinates of the two suspected points; then, the suspected points with the transverse/longitudinal coordinates larger than the intermediate value are taken as one group, and the suspected points with the transverse/longitudinal coordinates smaller than the intermediate value are taken as another group; and then judging which group the cache point is close to, if the cache point coordinate is close to the group with the larger transverse/longitudinal coordinates, selecting the point closest to the positioning point from the group with the larger transverse/longitudinal coordinates as the point closest to the positioning point in the reference track, and if the cache point coordinate is close to the group with the smaller transverse/longitudinal coordinates, selecting the point closest to the positioning point from the group with the smaller transverse/longitudinal coordinates as the point closest to the positioning point in the reference track.
Step S240 uses the last calculation result (cache point) to obtain the nearest point, so as to improve accuracy of the nearest point and calculation efficiency of the nearest point.
According to the method 200, the noise points are eliminated by comprehensively considering factors such as the distance, the included angle between the suspected point and the locating point, the course angle and the like, and the point closest to the locating point in the reference track is acquired by combining the last calculation result (the cache point), compared with the mode of acquiring the closest point only according to the distance in the prior art, the accuracy of the closest point acquired by the method 200 is obviously improved, even if the closest point cannot be acquired, the accurate closest point can be acquired after 1-2 seconds, and the automatic correction time is obviously shortened.
The present embodiment also provides a device for acquiring the closest point in the track, which can execute the processing of each step of the method 200 for acquiring the closest point in the track described above.
Fig. 7 is a schematic diagram of an acquisition device 700 for the closest point in a trajectory according to one embodiment of the invention. As shown in fig. 7, the apparatus 700 includes a first effective point screening unit 710, a second effective point screening unit 720, a judging unit 730, and a grouping unit 740.
The first effective point screening unit 710 is adapted to screen effective suspected points from a set of suspected points according to an included angle formed by two adjacent suspected points and a positioning point, and delete the rest of the suspected points from the set of suspected points, where the suspected points are n points closest to the positioning point in a reference track, a set formed by n suspected points is referred to as a set of suspected points, and the effective suspected points are suspected points possibly closest to the positioning point in the n suspected points.
The second effective point screening unit 720 is adapted to screen effective suspected points again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the positioning point, and delete the rest suspected points from the suspected point set.
The determining unit 730 is adapted to determine whether the set of suspected points includes a cache point or a next point of the cache point, where the cache point is the last acquired nearest point, if the set of suspected points is not empty and the value of n does not exceed a preset threshold.
The grouping unit 740 is adapted to divide the set of suspected points into two groups according to coordinates if the set of suspected points does not include the cache point and a next point of the cache point, select a group close to the cache point, and use a point closest to the locating point in the group as a point closest to the locating point in the reference track.
As a preferred embodiment of the present application, the first significant point screening unit 710 includes:
a nearest point determining subunit, adapted to traverse each suspected point in the suspected point set, taking an angle formed by a previous point of the suspected point, the locating point, and the suspected point as a first included angle, an angle formed by the suspected point, the locating point, and a next point of the suspected point as a second included angle, and an angle formed by the previous point of the suspected point, and the next point of the suspected point as a third included angle, and taking the suspected point as a nearest point to the locating point in the effective suspected point and the reference track if the first included angle and the second included angle are both smaller than 90 degrees, the sum of the first included angle and the second included angle is greater than 90 degrees, and the third included angle is greater than 90 degrees;
The first effective point screening subunit is suitable for taking the suspected point as an effective suspected point if the first included angle is larger than 90 degrees; and
and the second effective point screening subunit is suitable for taking the suspected point as an effective suspected point if the second included angle is larger than 90 degrees.
As a preferred embodiment of the present application, the second valid point screening unit 720 screens valid suspected points by:
traversing each suspected point in the suspected point set, and taking the suspected point as an effective suspected point if the absolute value of the difference between the heading angle from the last point of the suspected point to the suspected point and the heading angle of the locating point is smaller than or equal to 90 degrees.
As a preferred embodiment of the present application, the apparatus 700 further comprises a closest point determination unit.
The closest point determining unit is adapted to take the cache point or a next point of the cache point as a point closest to the positioning point in the reference track if the suspected point set contains the cache point or the next point of the cache point.
As a preferred embodiment of the present application, the nearest point determining unit regards the cache point or a next point of the cache point as a point nearest to the anchor point in the reference trajectory by:
If the suspected point set contains the cache point but does not contain the next point of the cache point, taking the cache point as the point closest to the locating point in the reference track;
if the suspected point set contains the next point of the cache point but does not contain the cache point, taking the next point of the cache point as a point closest to the positioning point in the reference track; and
and if the suspected point set comprises the cache point and the next point of the cache point, taking the point which is closest to the locating point in the cache point and the next point of the cache point as the point which is closest to the locating point in the reference track.
As a preferred embodiment of the present application, the grouping unit 740 groups the set of suspected points into two groups by coordinates in the following manner:
and selecting two points with maximum and minimum transverse/longitudinal coordinates in the suspected point set, calculating the average value of the transverse/longitudinal coordinates of the two points, taking the point with the transverse/longitudinal coordinate larger than the average value in the suspected point set as one group, and taking the point with the transverse/longitudinal coordinate smaller than the average value in the suspected point set as the other group.
As a preferred embodiment of the present application, the apparatus 700 further comprises a circulation unit.
And the circulation unit is suitable for increasing the value of n if the suspected point set is empty and the value of n does not exceed the threshold value, returning to the step of screening effective suspected points from the suspected point set according to the included angle formed by the adjacent two suspected points and the locating point, and deleting the rest of suspected points from the suspected point set.
The various techniques described herein may be implemented in connection with hardware or software or, alternatively, with a combination of both. Thus, the methods and apparatus of the present invention, or certain aspects or portions of the methods and apparatus of the present invention, may take the form of program code (i.e., instructions) embodied in tangible media, such as removable hard drives, U-drives, floppy diskettes, CD-ROMs, or any other machine-readable storage medium, wherein, when the program is loaded into and executed by a machine, such as a computer, the machine becomes an apparatus for practicing the invention.
In the case of program code execution on programmable computers, the electronic device will generally include a processor, a storage medium readable by the processor (including volatile and non-volatile memory and/or storage elements), at least one input device, and at least one output device. Wherein the memory is configured to store program code; the processor is configured to execute the method of acquiring the closest point in the trajectory of the present invention in accordance with instructions in said program code stored in the memory.
The present invention is not directed to any particular programming language. It should be appreciated that the teachings of the present invention as described herein may be implemented in a variety of programming languages and that the foregoing descriptions of specific languages are provided for disclosure of preferred embodiments of the present invention.
In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the invention may be practiced without these specific details. In some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.
Furthermore, those skilled in the art will appreciate that while some embodiments described herein include some features but not others included in other embodiments, combinations of features of different embodiments are meant to be within the scope of the invention and form different embodiments. Furthermore, some of the embodiments are described herein as methods or combinations of method elements that may be implemented by a processor of a computer system or by other means of performing the functions. Thus, a processor with the necessary instructions for implementing the described method or method element forms a means for implementing the method or method element. Furthermore, the elements of the apparatus embodiments described herein are examples of the following apparatus: the apparatus is for carrying out the functions performed by the elements for carrying out the objects of the invention.
It will be apparent to those skilled in the art that various modifications and variations can be made in the present application without departing from the spirit or scope of the application. Thus, if such modifications and variations of the present application fall within the scope of the claims and the equivalents thereof, the present application is intended to cover such modifications and variations.
Claims (10)
1. The method for acquiring the closest point in the track is characterized by comprising the following steps:
screening effective suspected points from a suspected point set according to an included angle formed by two adjacent suspected points and a locating point, and deleting the rest of the suspected points from the suspected point set, wherein the suspected points are n points closest to the locating point in a reference track, a set formed by the n suspected points is called a suspected point set, and the effective suspected point is a suspected point possibly closest to the locating point in the n suspected points;
screening out effective suspected points again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point, and deleting the rest suspected points from the suspected point set;
if the suspected point set is not empty and the value of n does not exceed a preset threshold, judging whether the suspected point set contains a cache point or a next point of the cache point, wherein the cache point is the last acquired nearest point; and
If the suspected point set does not contain the cache point and the next point of the cache point, the suspected point set is divided into two groups according to coordinates, one group close to the cache point is selected, and the point closest to the locating point in the group is taken as the point closest to the locating point in the reference track.
2. The method of claim 1, wherein,
the step of screening out effective suspected points from the suspected point set according to the included angle formed by the adjacent two suspected points and the locating point comprises the following steps:
traversing each suspected point in the suspected point set, taking an angle formed by a last point of the suspected point, the locating point and the suspected point as a first included angle, taking an angle formed by the suspected point, the locating point and a next point of the suspected point as a second included angle, and taking an angle formed by the last point of the suspected point, the suspected point and the next point of the suspected point as a third included angle, wherein if the first included angle and the second included angle are smaller than 90 degrees, the sum of the first included angle and the second included angle is larger than 90 degrees, and the third included angle is larger than 90 degrees, taking the suspected point as a point closest to the locating point in the reference track;
If the first included angle is larger than 90 degrees, the suspected point is used as an effective suspected point; and
and if the second included angle is larger than 90 degrees, the suspected point is used as an effective suspected point.
3. The method of claim 1 or 2, wherein,
the step of screening out effective suspected points again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point comprises the following steps:
traversing each suspected point in the suspected point set, and taking the suspected point as an effective suspected point if the absolute value of the difference between the heading angle from the last point of the suspected point to the suspected point and the heading angle of the locating point is smaller than or equal to 90 degrees.
4. The method as recited in claim 1, further comprising:
and if the suspected point set comprises the cache point or the next point of the cache point, taking the cache point or the next point of the cache point as the point closest to the positioning point in the reference track.
5. The method of claim 4, wherein,
the taking the cache point or the next point of the cache point as the point closest to the positioning point in the reference track includes:
If the suspected point set contains the cache point but does not contain the next point of the cache point, taking the cache point as the point closest to the locating point in the reference track;
if the suspected point set contains the next point of the cache point but does not contain the cache point, taking the next point of the cache point as a point closest to the positioning point in the reference track; and
and if the suspected point set comprises the cache point and the next point of the cache point, taking the point which is closest to the locating point in the cache point and the next point of the cache point as the point which is closest to the locating point in the reference track.
6. The method of claim 1, wherein,
the classifying the suspected point set into two groups according to coordinates comprises:
and selecting two points with maximum and minimum transverse/longitudinal coordinates in the suspected point set, calculating the average value of the transverse/longitudinal coordinates of the two points, taking the point with the transverse/longitudinal coordinate larger than the average value in the suspected point set as one group, and taking the point with the transverse/longitudinal coordinate smaller than the average value in the suspected point set as the other group.
7. The method as recited in claim 1, further comprising:
And if the suspected point set is empty and the value of n does not exceed the threshold value, increasing the value of n, returning to the step of screening effective suspected points from the suspected point set according to the included angle formed by the adjacent two suspected points and the locating point, and deleting the rest of suspected points from the suspected point set.
8. An acquisition device of a closest point in a trajectory, comprising:
the first effective point screening unit is suitable for screening effective suspected points from a suspected point set according to an included angle formed by two adjacent suspected points and a locating point, deleting the rest of the suspected points from the suspected point set, wherein the suspected points are n points closest to the locating point in a reference track, a set formed by the n suspected points is called a suspected point set, and the effective suspected points are suspected points possibly closest to the locating point in the n suspected points;
the second effective point screening unit is suitable for screening effective suspected points again according to the course angle formed by two adjacent suspected points in the suspected point set and the course angle of the locating point, and deleting the rest suspected points from the suspected point set;
the judging unit is adapted to judge whether the suspected point set contains a cache point or a next point of the cache point if the suspected point set is not empty and the value of n does not exceed a preset threshold, wherein the cache point is the last acquired nearest point; and
And the grouping unit is suitable for dividing the suspected point set into two groups according to coordinates if the suspected point set does not contain the cache point and the next point of the cache point, selecting one group close to the cache point, and taking the point closest to the locating point in the group as the point closest to the locating point in the reference track.
9. An electronic device, comprising:
a memory;
a processor; and
a computer program;
wherein the computer program is stored in the memory and configured to be executed by the processor to implement the method of any one of claims 1 to 7.
10. A computer-readable storage medium, characterized by a computer program stored thereon; the computer program being executed by a processor to implement the method of any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410089974.5A CN117606508A (en) | 2024-01-23 | 2024-01-23 | Method and device for acquiring closest point in track, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410089974.5A CN117606508A (en) | 2024-01-23 | 2024-01-23 | Method and device for acquiring closest point in track, electronic equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117606508A true CN117606508A (en) | 2024-02-27 |
Family
ID=89948334
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410089974.5A Pending CN117606508A (en) | 2024-01-23 | 2024-01-23 | Method and device for acquiring closest point in track, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117606508A (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109823393A (en) * | 2018-12-14 | 2019-05-31 | 清华大学苏州汽车研究院（相城） | A kind of intelligent driving Vehicle tracing control method |
| CN113815646A (en) * | 2021-09-14 | 2021-12-21 | 上汽通用五菱汽车股份有限公司 | Intelligent driving method of vehicle, vehicle and readable storage medium |
| CN113865597A (en) * | 2021-10-13 | 2021-12-31 | 智道网联科技(北京)有限公司 | Map matching positioning method, device and storage medium |
| CN113989784A (en) * | 2021-11-30 | 2022-01-28 | 福州大学 | Road scene type identification method and system based on vehicle-mounted laser point cloud |
| JP2023151345A (en) * | 2022-03-31 | 2023-10-16 | 日産自動車株式会社 | Travel path shape estimation device and method |
| CN117213510A (en) * | 2023-08-15 | 2023-12-12 | 蘑菇车联信息科技有限公司 | Automatic driving tracking travel information processing method, device, equipment and storage medium |
| EP4290181A1 (en) * | 2022-06-01 | 2023-12-13 | Suzuki Motor Corporation | Operation system for small electric vehicle |
-
2024
- 2024-01-23 CN CN202410089974.5A patent/CN117606508A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109823393A (en) * | 2018-12-14 | 2019-05-31 | 清华大学苏州汽车研究院（相城） | A kind of intelligent driving Vehicle tracing control method |
| CN113815646A (en) * | 2021-09-14 | 2021-12-21 | 上汽通用五菱汽车股份有限公司 | Intelligent driving method of vehicle, vehicle and readable storage medium |
| CN113865597A (en) * | 2021-10-13 | 2021-12-31 | 智道网联科技(北京)有限公司 | Map matching positioning method, device and storage medium |
| CN113989784A (en) * | 2021-11-30 | 2022-01-28 | 福州大学 | Road scene type identification method and system based on vehicle-mounted laser point cloud |
| JP2023151345A (en) * | 2022-03-31 | 2023-10-16 | 日産自動車株式会社 | Travel path shape estimation device and method |
| EP4290181A1 (en) * | 2022-06-01 | 2023-12-13 | Suzuki Motor Corporation | Operation system for small electric vehicle |
| CN117213510A (en) * | 2023-08-15 | 2023-12-12 | 蘑菇车联信息科技有限公司 | Automatic driving tracking travel information processing method, device, equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4548460B2 (en) | Navigation device | |
| CN107924569B (en) | Image processing apparatus, image processing method, and storage medium | |
| EP1655578A1 (en) | Voice guidance method of travel route in navigation system | |
| JPH0792388B2 (en) | Position detector | |
| CN107917716B (en) | Fixed line navigation method, device, terminal and computer readable storage medium | |
| JP2009180631A (en) | Navigator, navigation method and program | |
| CN111815742A (en) | Lane line generation method and system | |
| CN110782656B (en) | Road bottleneck point identification method and device, electronic equipment and storage medium | |
| CN113076386A (en) | Road data processing method and device, electronic equipment and storage medium | |
| KR20180033552A (en) | Image processing apparatus and image processing method | |
| CN117606508A (en) | Method and device for acquiring closest point in track, electronic equipment and storage medium | |
| CN108399179B (en) | Method and device for determining road curvature | |
| JP2019179324A (en) | System and program for setting target parking spot | |
| JP4116681B2 (en) | Optimal route search method | |
| JP5177579B2 (en) | Image processing system and positioning system | |
| CN111486857B (en) | Road network prediction tree construction method and device, electronic equipment and storage medium | |
| US10883845B2 (en) | Method of detecting a virtual tolling point | |
| JP5935661B2 (en) | Intersection guidance system, method and program | |
| JP2012164051A (en) | License plate character recognition device and license plate character recognition method | |
| JP2007171211A (en) | Optimum path searching method | |
| JP4966788B2 (en) | Shooting direction determination device and shooting direction determination program | |
| JP2009115485A (en) | Navigation device, method, and program | |
| KR101667484B1 (en) | Method and Device for Estimating position of Vehicle Using Digital Map | |
| CN112255658B (en) | Method, device and medium for counting number of vehicle running turns based on GPS positioning points | |
| CN114485689B (en) | Road segment segmentation method and device for constructing high-precision map by utilizing crowdsourcing data |
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 |