CN113408157A - World coordinate and road coordinate conversion method and device of unmanned simulation system - Google Patents
World coordinate and road coordinate conversion method and device of unmanned simulation system Download PDFInfo
- Publication number
- CN113408157A CN113408157A CN202110945844.3A CN202110945844A CN113408157A CN 113408157 A CN113408157 A CN 113408157A CN 202110945844 A CN202110945844 A CN 202110945844A CN 113408157 A CN113408157 A CN 113408157A
- Authority
- CN
- China
- Prior art keywords
- road
- coordinate
- new
- search
- vector
- 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.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
Abstract
The invention discloses a world coordinate and road coordinate conversion method and device of an unmanned simulation system. The method comprises the steps of recording the current road coordinates of a detected vehicle; searching a target road identified by the current road identification according to the new world coordinate sent by the detected unmanned driving algorithm software, and if the search is successful, taking the searched road coordinate as a new road coordinate; if the search is not successful, taking the road connected with the target road as a new road, searching the new road according to the new world coordinate, if the search is successful, taking the searched road coordinate as the new road coordinate, if the search is not successful, taking the road connected with the new road and not searched as the new road, and returning to execute the step of searching the new road according to the new world coordinate; and sending the new road coordinates and the new world coordinates to the tested unmanned algorithm software. By applying the scheme provided by the invention, the time consumption can be reduced, and the efficiency can be improved.
Description
Technical Field
The invention relates to the technical field of unmanned driving, in particular to a world coordinate and road coordinate conversion method and device of an unmanned driving simulation system.
Background
At present, when the unmanned simulation test is carried out, the unmanned simulation system needs to carry out bidirectional real-time communication, namely interaction with the tested unmanned algorithm software, and the tested unmanned simulation software is tested by accurately controlling the running of a tested vehicle in the unmanned simulation system through the tested unmanned algorithm software.
Because the position coordinates in the unmanned simulation system are defined by a road coordinate system, and the position coordinates in the tested unmanned algorithm software are defined by a world coordinate system, when the tested unmanned algorithm software is communicated with the unmanned simulation system in real time to send the world coordinates of a certain position of the tested vehicle, the unmanned simulation system needs to convert the world coordinates into the road coordinates in the road coordinate system to perform subsequent steps.
In the prior art, a manner of converting world coordinates into road coordinates in a road coordinate system by an unmanned simulation system is as follows: and for each world coordinate sent by the detected unmanned algorithm software at each moment, sequentially traversing from the first road to the last road in the whole map of the unmanned simulation system to search and find the road coordinate corresponding to the world coordinate. This approach results in a long time and low efficiency.
Disclosure of Invention
The invention provides a world coordinate and road coordinate conversion method and device of an unmanned simulation system, which can reduce time consumption and improve efficiency. The specific technical scheme is as follows.
In a first aspect, the present invention provides a method for converting world coordinates and road coordinates of an unmanned simulation system, which is applied to the unmanned simulation system, and the method includes:
recording the current road coordinates of the detected vehicle, wherein the current road coordinates at least comprise a current road identifier;
receiving new world coordinates of the tested vehicle sent by the tested unmanned algorithm software;
searching on the target road identified by the current road identification according to the new world coordinate, and if the search is successful, taking the searched road coordinate as a new road coordinate;
if the search is not successful, taking the road connected with the target road identified by the current road identification as a new road, searching on the new road according to the new world coordinate, if the search is successful, taking the searched road coordinate as a new road coordinate, if the search is not successful, taking the searched road connected with the new road as a new road, and returning to execute the step of searching on the new road according to the new world coordinate;
and sending the new road coordinate and the new world coordinate to the tested unmanned driving algorithm software.
Optionally, the step of searching on the target road identified by the current road identifier according to the new world coordinate, and if the search is successful, taking the searched road coordinate as a new road coordinate includes:
and converting the new world coordinate into a road coordinate system, judging whether the converted road coordinate on the target road can be obtained or not, if so, determining that the search is successful, taking the converted road coordinate as the searched road coordinate, and taking the searched road coordinate as the new road coordinate.
Optionally, the step of converting the new world coordinate into a road coordinate system, determining whether a converted road coordinate located on the target road can be obtained, and if so, determining that the search is successful includes:
converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate system, wherein the positive direction of an s axis in the road coordinate system is the extending direction of the road length, the positive direction of the t axis is the extending direction of the left road width, and the intersection point of the s axis and the t axis is the origin of the road coordinate system;
judging whether the size of the s coordinate is smaller than the road length of the target road or not;
if yes, judging whether the t coordinate is located in the road width range of the target road;
if so, determining the lane mark of the lane where the point where the converted road coordinate is located according to the t coordinate, obtaining the converted road coordinate which is formed by the s coordinate, the t coordinate and the lane mark and is located on the target road, and determining that the search is successful.
Optionally, the step of converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate includes:
calculating a first vector pointing from a starting point of the center line of the target road to a point where the new world coordinate is located;
calculating a tangent vector of the target road, and converting the tangent vector into a unit tangent vector, wherein the tangent vector of the target road is a vector along the tangent direction at the starting point;
performing dot product operation on the first vector and the unit tangent vector to obtain a first dot product operation result, and taking the first dot product operation result as an s coordinate in the converted road coordinate;
calculating a second vector of which the coordinate on the s axis is that the point of the s coordinate points to the point of the new world coordinate;
carrying out a difference product operation on the unit tangent vector and the second vector to obtain a difference product operation result;
performing dot product operation on the difference product operation result and the vector (0, 0, 1) to obtain a second dot product operation result;
calculating a quotient between the second dot product operation result and an absolute value of the second dot product operation result;
and calculating the product between the quotient and the length of the second vector, and taking the product as the t coordinate in the new road coordinate.
Optionally, the current road coordinate further includes a lane identification of the current lane.
In a second aspect, the present invention provides a world coordinate and road coordinate conversion apparatus for an unmanned simulation system, applied to the unmanned simulation system, the apparatus comprising:
the recording module is used for recording the current road coordinates of the detected vehicle, and the current road coordinates at least comprise a current road identifier;
the receiving module is used for receiving the new world coordinates of the tested vehicle sent by the tested unmanned algorithm software;
the first searching module is used for searching on the target road identified by the current road identification according to the new world coordinate, and if the searching is successful, the searched road coordinate is used as a new road coordinate;
a second searching module, configured to, if the search is not successful, take a road connected to the target road identified by the current road identifier as a new road, search the new road according to the new world coordinate, if the search is successful, take the searched road coordinate as a new road coordinate, if the search is not successful, take the road connected to the new road that is not searched as a new road, and return to the step of performing the search on the new road according to the new world coordinate;
and the sending module is used for sending the new road coordinate and the new world coordinate to the tested unmanned driving algorithm software.
Optionally, the first search module includes:
and the judgment sub-module is used for converting the new world coordinate into a road coordinate system, judging whether the converted road coordinate on the target road can be obtained or not, if so, determining that the search is successful, taking the converted road coordinate as the searched road coordinate, and taking the searched road coordinate as the new road coordinate.
Optionally, the determining sub-module includes:
the conversion unit is used for converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate system, wherein the positive direction of an s axis in the road coordinate system is the extending direction of the road length, the positive direction of the t axis is the extending direction of the left road width, and the intersection point of the s axis and the t axis is the origin of the road coordinate system;
the first judgment unit is used for judging whether the size of the s coordinate is smaller than the road length of the target road or not, and if so, the second judgment unit is triggered;
the second judging unit is used for judging whether the t coordinate is located in the road width range of the target road, and if so, the determining unit is triggered;
and the determining unit is used for determining the lane mark of the lane where the point where the converted road coordinate is located according to the t coordinate, obtaining the converted road coordinate which is formed by the s coordinate, the t coordinate and the lane mark and is located on the target road, and determining that the search is successful.
Optionally, the conversion unit is specifically configured to:
calculating a first vector pointing from a starting point of the center line of the target road to a point where the new world coordinate is located;
calculating a tangent vector of the target road, and converting the tangent vector into a unit tangent vector, wherein the tangent vector of the target road is a vector along the tangent direction at the starting point;
performing dot product operation on the first vector and the unit tangent vector to obtain a first dot product operation result, and taking the first dot product operation result as an s coordinate in the converted road coordinate;
calculating a second vector of which the coordinate on the s axis is that the point of the s coordinate points to the point of the new world coordinate;
carrying out a difference product operation on the unit tangent vector and the second vector to obtain a difference product operation result;
performing dot product operation on the difference product operation result and the vector (0, 0, 1) to obtain a second dot product operation result;
calculating a quotient between the second dot product operation result and an absolute value of the second dot product operation result;
and calculating the product between the quotient and the length of the second vector, and taking the product as the t coordinate in the new road coordinate.
Optionally, the current road coordinate further includes a lane identification of the current lane.
As can be seen from the above, the method and device for converting world coordinates and road coordinates of an unmanned simulation system according to the embodiments of the present invention can record current road coordinates of a vehicle under test, where the current road coordinates at least include a current road identifier; receiving new world coordinates of the tested vehicle sent by the tested unmanned algorithm software; searching on a target road identified by the current road identification according to the new world coordinate, and if the search is successful, taking the searched road coordinate as a new road coordinate; if the search is not successful, taking the road connected with the target road identified by the current road identification as a new road, searching on the new road according to the new world coordinate, if the search is successful, taking the searched road coordinate as the new road coordinate, if the search is not successful, taking the un-searched road connected with the new road as the new road, and returning to execute the step of searching on the new road according to the new world coordinate; and sending the new road coordinates and the new world coordinates to the tested unmanned algorithm software. In this embodiment, the unmanned simulation system converts the world coordinate into the road coordinate in the road coordinate system by searching on the target road identified by the current road identifier, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the new road connected to the target road, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the road connected to the new road, and this way of searching according to the principle of proximity makes it possible to find the road coordinate corresponding to the world coordinate without searching on the whole map, which can reduce time consumption and improve efficiency. Of course, not all of the advantages described above need to be achieved at the same time in the practice of any one product or method of the invention.
The innovation points of the embodiment of the invention comprise:
1. in this embodiment, the unmanned simulation system converts the world coordinate into the road coordinate in the road coordinate system by searching on the target road identified by the current road identifier, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the new road connected to the target road, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the road connected to the new road, and this way of searching according to the principle of proximity makes it possible to find the road coordinate corresponding to the world coordinate without searching on the whole map, which can reduce time consumption and improve efficiency.
2. After the new road coordinates are obtained, the unmanned simulation system not only sends the new road coordinates to the tested unmanned algorithm software, but also sends the new world coordinates to the tested unmanned algorithm software, so that the tested unmanned algorithm does not need to store the new world coordinates, the memory consumption is reduced, and after the new road coordinates and the new world coordinates are received, how to control the tested vehicle to run in the next step can be calculated according to the received new road coordinates and the new world coordinates, the new world coordinates do not need to be searched from the memory of the unmanned simulation system, and the efficiency is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below. It is to be understood that the drawings in the following description are merely exemplary of some embodiments of the invention. For a person skilled in the art, without inventive effort, further figures can be obtained from these figures.
Fig. 1 is a schematic flowchart of a world coordinate and road coordinate conversion method of an unmanned simulation system according to an embodiment of the present invention;
FIG. 2 is a schematic view of a road coordinate system;
fig. 3 is a schematic structural diagram of a world coordinate and road coordinate conversion device of the unmanned simulation system according to the embodiment of the present invention.
Detailed Description
The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. It is to be understood that the described embodiments are merely a few embodiments of the invention, and not all embodiments. All other embodiments, which can be obtained by a person skilled in the art without inventive effort based on the embodiments of the present invention, are within the 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.
The embodiment of the invention discloses a world coordinate and road coordinate conversion method and device of an unmanned simulation system, which can reduce time consumption and improve efficiency. The following provides a detailed description of embodiments of the invention.
Fig. 1 is a schematic flow chart of a world coordinate and road coordinate conversion method of an unmanned simulation system according to an embodiment of the present invention. The method is applied to the unmanned driving simulation system. The method specifically comprises the following steps.
S110: and recording the current road coordinates of the detected vehicle, wherein the current road coordinates at least comprise a current road identifier.
When the unmanned simulation system is just started, a preset initialization file can be loaded, the initial road coordinate and the initial world coordinate of the tested vehicle are stored in the initialization file, the unmanned simulation system records the loaded initial road coordinate of the tested vehicle as the current road coordinate of the tested vehicle, and records the loaded initial world coordinate of the tested vehicle as the current world coordinate of the tested vehicle.
The current road coordinates at least comprise a current road mark, and the current road mark represents which road in the unmanned simulation system map the detected vehicle is located on currently. The current road coordinates may also include a lane identification of the current lane, the current road identification indicating which lane on the road identified by the current road identification the vehicle under test is currently located in. The current road coordinate may further include a current s coordinate and a current t coordinate of the vehicle under test, where the current s coordinate represents a current abscissa of the vehicle under test in the road coordinate system, and the current t coordinate represents a current ordinate of the vehicle under test in the road coordinate system.
S120: and receiving the new world coordinates of the tested vehicle sent by the tested unmanned algorithm software.
When the unmanned simulation test is carried out, the unmanned simulation system needs to carry out bidirectional real-time communication, namely interaction with the tested unmanned algorithm software, and the tested unmanned simulation software is tested by accurately controlling the running of a tested vehicle in the unmanned simulation system through the tested unmanned algorithm software. Therefore, the tested unmanned algorithm software sends the new world coordinates of the tested vehicle to the unmanned simulation system in order to control the tested vehicle to run.
The unmanned simulation system receives new world coordinates of the tested vehicle sent by the tested unmanned algorithm software, and the tested unmanned algorithm is an Automatic driving algorithm.
S130: and searching the target road identified by the current road identification according to the new world coordinate, and if the search is successful, taking the searched road coordinate as a new road coordinate.
Since the detected vehicle runs on the target road identified by the current road identifier before the new world coordinate is not received, the detected unmanned algorithm software is very likely to still control the detected vehicle to run on the target road identified by the current road identifier, and therefore after the new world coordinate is received, the target road identified by the current road identifier is searched according to the new world coordinate, and then the subsequent steps are carried out according to the search result.
And if the search is successful, the detected unmanned algorithm software still controls the detected vehicle to run on the target road identified by the current road identifier, and the searched road coordinate is used as a new road coordinate.
In the embodiment of the present invention, the process of searching for the target road identified by the current road identifier according to the new world coordinate is actually a process of converting the world coordinate into the road coordinate, and specifically, step S130 may include:
and converting the new world coordinate into a road coordinate system, judging whether the converted road coordinate on the target road can be obtained or not, if so, determining that the search is successful, taking the converted road coordinate as the searched road coordinate, and taking the searched road coordinate as the new road coordinate.
Because the position coordinates in the unmanned simulation system are defined by a road coordinate system, and the position coordinates in the measured unmanned algorithm software are defined by a world coordinate system, after the measured unmanned algorithm software communicates with the unmanned simulation system in real time to send new world coordinates of the measured vehicle, the unmanned simulation system needs to convert the new world coordinates into the road coordinate system in order to search on a target road identified by the current road identification according to the new world coordinates.
And then judging whether the converted road coordinate on the target road identified by the current road identification can be obtained or not by the unmanned simulation system, if so, determining that the search is successful, taking the converted road coordinate as the searched road coordinate, and taking the searched road coordinate as a new road coordinate, which indicates that the tested unmanned algorithm software controls the tested vehicle to run from the position of the current road coordinate to the position of the new road coordinate.
The above-mentioned manner of converting the world coordinate into the road coordinate may be any coordinate conversion manner in the prior art, as long as the conversion from the world coordinate to the road coordinate can be realized, and the embodiment of the present invention is not limited thereto.
Specifically, the converting the new world coordinate into the road coordinate system, and determining whether the converted road coordinate on the target road can be obtained, if so, determining that the search is successful may include:
converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate system, wherein the positive direction of an s axis in the road coordinate system is the extending direction of the road length, the positive direction of the t axis is the extending direction of the road width towards the left, and the intersection point of the s axis and the t axis is the origin of the road coordinate system;
judging whether the size of the s coordinate is smaller than the road length of the target road or not;
if yes, judging whether the t coordinate is located in the road width range of the target road;
if so, determining the lane mark of the lane where the point where the converted road coordinate is located according to the t coordinate, obtaining the converted road coordinate which is formed by the s coordinate, the t coordinate and the lane mark and is located on the target road, and determining that the search is successful.
Fig. 2 is a schematic diagram of a road coordinate system, and referring to fig. 2, in the road coordinate system, an s-axis and a t-axis are perpendicular to each other, a positive direction of the s-axis in the road coordinate system is an extending direction of a road length, and since the extending direction of the road in fig. 2 is an upward direction, the positive direction of the s-axis is an upward direction, the positive direction of the t-axis is an extending direction of a road width to the left, and an intersection point of the s-axis and the t-axis is an origin of the road coordinate system.
In general, the center line of the road coincides with the s-axis, the starting point of the center line of the road is the origin of the road coordinate system, in fig. 2, for convenience of viewing, the center line of the road does not coincide with the s-axis, the lanes of the lanes on the left side of the s-axis are marked as positive numbers, for example, the lane 1 next to the s-axis is sequentially marked as the 2 lane, the 3 lane … … on the left side, the lanes of the lanes on the right side of the s-axis is marked as positive numbers, for example, the lane-1 next to the s-axis is sequentially marked as the-2 lane and the-3 lane … … on the left side, and only the lane 1 and the lane-1 are marked in fig. 2.
The converting the new world coordinate into the road coordinate system to obtain the s coordinate and the t coordinate in the converted road coordinate may include:
calculating a first vector pointing from the starting point of the center line of the target road to the point of the new world coordinate;
calculating a tangent vector of the target road, and converting the tangent vector into a unit tangent vector, wherein the tangent vector of the target road is a vector along the tangent direction at the starting point;
performing dot product operation on the first vector and the unit tangent vector to obtain a first dot product operation result, and taking the first dot product operation result as an s coordinate in the converted road coordinate;
calculating a second vector of a point with the coordinate on the s axis as the s coordinate pointing to the point of the new world coordinate;
carrying out difference product operation on the unit tangent vector and the second vector to obtain a difference product operation result;
performing dot product operation on the difference product operation result and the vector (0, 0, 1) to obtain a second dot product operation result;
calculating a quotient between the second dot product operation result and an absolute value of the second dot product operation result;
and calculating the product between the quotient and the length of the second vector, and taking the product as the t coordinate in the new road coordinate.
The above-mentioned way of calculating the tangent vector of the target road is: the vector in the tangential direction at the starting point along the center line of the target road is used as the tangent vector of the target road, and the manner of converting the tangent vector into the unit tangent vector is any manner of converting the vector into the unit vector in the prior art, which is not limited in this embodiment of the present invention.
For convenience of understanding, the process of converting the new world coordinate into the road coordinate system to obtain the s coordinate and the t coordinate in the converted road coordinate system is described below by a specific embodiment:
assuming that a start point of a center line of the target road, i.e., an origin of a road coordinate system, is (0, 0), a road length of the target road is 100 meters, an extending direction of the road length of the target road is parallel to a y-axis, i.e., a heading angle hdg =90 degrees, there are two lanes of-1 lane and-2 lanes, each lane has a width of 1.0 meter, i.e., a range of a world coordinate x of the target road is [0, 2], a range of a t coordinate in the road coordinates is [0, 2], and a range of an s coordinate in the road coordinates is [0, 100 ]. Assuming that the new world coordinate point is P and the new world coordinate is (1.5, 1, 0), the process of converting the new world coordinate (1.5, 1, 0) into the road coordinate system to obtain the s coordinate and the t coordinate in the converted road coordinate system is as follows:
1. calculating a first vector pointing from a starting point of a center line of the target road to a point P
，
=(1.5, 1, 0)；
2. Calculating tangent vector of target road, and converting the tangent vector into unit tangent vector
，
=(0, 1, 0)；
3. For the first vector
And unit tangent vector
Performing dot product operation to obtain a first dot product operation result = 1.5 × 0 + 1 × 1 + 0 × 0 = 1.0, and taking the first dot product operation result 1.0 as an s coordinate in the road coordinates after conversion;
4. calculating the second vector of the point with the coordinate on the s-axis being the s-coordinate, i.e. 1.0, pointing to the point P where the new world coordinate is located
，
=(1.5, 0, 0)；
5. To unit tangent vector
And second vector
Performing a difference product operation to obtain a difference product operation result, and performing a dot product operation on the difference product operation result and the vector (0, 0, 1) to obtain a second dot product operation result, that is, the second dot product operation result dir = the difference product operation result and the vector (0, 0, 1) perform the dot product operation = -1.5;
6. calculating a quotient between the absolute values of the second dot product result-1.5 and the second dot product result-1.5, calculating the quotient and the second vector
Is used as the value of t coordinate in the new road coordinate, i.e. t = dir/dir absolute value ×. second vector
Length of = -1.5.
And after the new world coordinate is converted into a road coordinate system, obtaining an s coordinate and a t coordinate in the converted road coordinate, then judging whether the size of the s coordinate is smaller than the road length of the target road, if so, indicating that the s coordinate is positioned in the road length range of the target road, and if not, indicating that the s coordinate is not positioned in the road length range of the target road, and failing to search.
And when the size of the s coordinate is smaller than the road length of the target road, continuously judging whether the t coordinate is located in the road width range of the target road, and if so, determining the lane mark of the lane where the point where the converted road coordinate is located according to the t coordinate. For example: receiving the above example of t = -1.5, it may be determined that the lane mark of the lane where the point where the road coordinate is located after the conversion is-2 according to the t coordinate-1.5, that is, the point where the road coordinate is located after the conversion is located in the-2 lane.
In conclusion, the converted road coordinate on the target road, which is composed of the s coordinate, the t coordinate and the lane mark, is obtained, the search success is determined, and the searched road coordinate is taken as a new road coordinate.
S140: and if the search is not successful, taking the road connected with the target road identified by the current road identification as a new road, searching the new road according to the new world coordinate, if the search is successful, taking the searched road coordinate as the new road coordinate, if the search is not successful, taking the un-searched road connected with the new road as the new road, and returning to execute the step of searching the new road according to the new world coordinate.
If the search is not successful, it indicates that the detected unmanned algorithm software does not control the detected vehicle to travel on the target road identified by the current road identifier, and according to the principle of proximity, the detected unmanned algorithm software may control the detected vehicle to travel on the road connected to the target road, so that the search may be continued on the road connected to the target road, that is, if the search is not successful, the road connected to the target road identified by the current road identifier is used as a new road, and the search is performed on the new road according to the new world coordinate, where the search method on the new road according to the new world coordinate is described in step S130 in a manner of performing the search on the target road identified by the current road identifier according to the new world coordinate, and is not repeated here.
And if the search is successful, the tested unmanned algorithm software controls the tested vehicle to run on a new road connected with the target road, and the searched road coordinate is used as a new road coordinate.
If the search is not successful, the tested unmanned algorithm software does not control the tested vehicle to run on a new road connected with the target road, the search is continuously carried out on the road connected with the new road, if the search is not successful, the search is continuously carried out, namely if the search is not successful, the road which is connected with the new road and is not searched is used as the new road, and the step of searching on the new road according to the new world coordinates is returned to be executed.
For convenience of understanding, the above-mentioned manner of searching the road circularly according to the principle of proximity is described below by a specific embodiment:
assuming that the target road is road1, the roads connected with the target road1 are road2 and road3, the roads connected with road2 and not searched are road4 and road5, the roads connected with road3 and not searched are road6 and road7, and the roads connected with road4 and not searched are road8 … …, it is assumed that the search on the target road1 identified by the current road identification is not successful according to the new world coordinates.
The above circular search mode on the road according to the principle of proximity is as follows:
1. roads road2 and road3 connected to the target road1 are taken as new roads;
2. searching on new roads road2 and road3 according to the new world coordinates;
3. if the search is successful, the searched road coordinate is used as a new road coordinate;
4. if the search is not successful, the unsearched roads road4, road5, road6 and road7 connected to the new roads road2 and road3 are taken as new roads;
5. searching on new roads road4, road5, road6 and road7 according to the new world coordinates;
6. if the search is successful, the searched road coordinate is used as a new road coordinate;
7. if the non-search is successful, the non-searched road rowd 8 … … connected to the new roads rowd 4, rowd 5, rowd 6 and rowd 7 is taken as a new road;
8. a search is made on a new road8 … … based on the new world coordinates, and so on a near-by basis until the map of the unmanned simulation system is completely traversed.
S150: and sending the new road coordinates and the new world coordinates to the tested unmanned algorithm software.
Because the measured unmanned algorithm software needs to calculate how to control the measured vehicle to drive in the next step according to the new road coordinate and the new world coordinate, the unmanned simulation system sends the new road coordinate and the new world coordinate to the measured unmanned algorithm software so that the measured unmanned algorithm software can calculate how to control the measured vehicle to drive in the next step according to the received new road coordinate and the new world coordinate.
As can be seen from the above, the present embodiment may record the current road coordinate of the detected vehicle, where the current road coordinate at least includes the current road identifier; receiving new world coordinates of the tested vehicle sent by the tested unmanned algorithm software; searching on a target road identified by the current road identification according to the new world coordinate, and if the search is successful, taking the searched road coordinate as a new road coordinate; if the search is not successful, taking the road connected with the target road identified by the current road identification as a new road, searching on the new road according to the new world coordinate, if the search is successful, taking the searched road coordinate as the new road coordinate, if the search is not successful, taking the un-searched road connected with the new road as the new road, and returning to execute the step of searching on the new road according to the new world coordinate; and sending the new road coordinates and the new world coordinates to the tested unmanned algorithm software. In this embodiment, the unmanned simulation system converts the world coordinate into the road coordinate in the road coordinate system by searching on the target road identified by the current road identifier, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the new road connected to the target road, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the road connected to the new road, and this way of searching according to the principle of proximity makes it possible to find the road coordinate corresponding to the world coordinate without searching on the whole map, which can reduce time consumption and improve efficiency.
And after the new road coordinate is obtained, the unmanned simulation system not only sends the new road coordinate to the tested unmanned algorithm software, but also sends the new world coordinate to the tested unmanned algorithm software, so that the tested unmanned algorithm does not need to store the new world coordinate, the memory consumption is reduced, and after the new road coordinate and the new world coordinate are received, how to control the tested vehicle to run in the next step can be calculated according to the received new road coordinate and the new world coordinate, the new world coordinate does not need to be searched from the memory of the unmanned simulation system, and the efficiency is improved.
Corresponding to the above method embodiment, an embodiment of the present invention provides a world coordinate and road coordinate conversion apparatus for an unmanned simulation system, which is applied to the unmanned simulation system, and as shown in fig. 3, the apparatus may include:
the recording module 310 is configured to record a current road coordinate of the vehicle under test, where the current road coordinate at least includes a current road identifier;
the receiving module 320 is used for receiving the new world coordinates of the tested vehicle sent by the tested unmanned algorithm software;
the first searching module 330 is configured to search for a target road identified by the current road identifier according to the new world coordinate, and if the search is successful, take the searched road coordinate as a new road coordinate;
a second searching module 340, configured to, if the search is not successful, take a road connected to the target road identified by the current road identifier as a new road, search the new road according to the new world coordinate, if the search is successful, take the searched road coordinate as a new road coordinate, if the search is not successful, take the un-searched road connected to the new road as a new road, and return to the step of performing the search on the new road according to the new world coordinate;
and a sending module 350, configured to send the new road coordinate and the new world coordinate to the unmanned algorithm software under test.
The device provided by the embodiment can record the current road coordinate of the detected vehicle, wherein the current road coordinate at least comprises a current road mark; receiving new world coordinates of the tested vehicle sent by the tested unmanned algorithm software; searching on a target road identified by the current road identification according to the new world coordinate, and if the search is successful, taking the searched road coordinate as a new road coordinate; if the search is not successful, taking the road connected with the target road identified by the current road identification as a new road, searching on the new road according to the new world coordinate, if the search is successful, taking the searched road coordinate as the new road coordinate, if the search is not successful, taking the un-searched road connected with the new road as the new road, and returning to execute the step of searching on the new road according to the new world coordinate; and sending the new road coordinates and the new world coordinates to the tested unmanned algorithm software. In this embodiment, the unmanned simulation system converts the world coordinate into the road coordinate in the road coordinate system by searching on the target road identified by the current road identifier, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the new road connected to the target road, if the search is successful, finding the road coordinate corresponding to the world coordinate, if the search is unsuccessful, searching on the road connected to the new road, and this way of searching according to the principle of proximity makes it possible to find the road coordinate corresponding to the world coordinate without searching on the whole map, which can reduce time consumption and improve efficiency.
In another embodiment of the present invention, the first searching module 330 may include:
and the judgment sub-module is used for converting the new world coordinate into a road coordinate system, judging whether the converted road coordinate on the target road can be obtained or not, if so, determining that the search is successful, taking the converted road coordinate as the searched road coordinate, and taking the searched road coordinate as the new road coordinate.
In another embodiment of the present invention, the determining sub-module may include:
the conversion unit is used for converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate system, wherein the positive direction of an s axis in the road coordinate system is the extending direction of the road length, the positive direction of the t axis is the extending direction of the left road width, and the intersection point of the s axis and the t axis is the origin of the road coordinate system;
the first judgment unit is used for judging whether the size of the s coordinate is smaller than the road length of the target road or not, and if so, the second judgment unit is triggered;
the second judging unit is used for judging whether the t coordinate is located in the road width range of the target road, and if so, the determining unit is triggered;
and the determining unit is used for determining the lane mark of the lane where the point where the converted road coordinate is located according to the t coordinate, obtaining the converted road coordinate which is formed by the s coordinate, the t coordinate and the lane mark and is located on the target road, and determining that the search is successful.
In another embodiment of the present invention, the conversion unit may specifically be configured to:
calculating a first vector pointing from a starting point of the center line of the target road to a point where the new world coordinate is located;
calculating a tangent vector of the target road, and converting the tangent vector into a unit tangent vector, wherein the tangent vector of the target road is a vector along the tangent direction at the starting point;
performing dot product operation on the first vector and the unit tangent vector to obtain a first dot product operation result, and taking the first dot product operation result as an s coordinate in the converted road coordinate;
calculating a second vector of which the coordinate on the s axis is that the point of the s coordinate points to the point of the new world coordinate;
carrying out a difference product operation on the unit tangent vector and the second vector to obtain a difference product operation result;
performing dot product operation on the difference product operation result and the vector (0, 0, 1) to obtain a second dot product operation result;
calculating a quotient between the second dot product operation result and an absolute value of the second dot product operation result;
and calculating the product between the quotient and the length of the second vector, and taking the product as the t coordinate in the new road coordinate.
In another embodiment of the present invention, the current road coordinates may further include a lane identification of the current lane.
The above device embodiment corresponds to the method embodiment, and has the same technical effect as the method embodiment, and for the specific description, refer to the method embodiment. The device embodiment is obtained based on the method embodiment, and for specific description, reference may be made to the method embodiment section, which is not described herein again.
Those of ordinary skill in the art will understand that: the figures are merely schematic representations of one embodiment, and the blocks or flow diagrams in the figures are not necessarily required to practice the present invention.
Those of ordinary skill in the art will understand that: modules in the devices in the embodiments may be distributed in the devices in the embodiments according to the description of the embodiments, or may be located in one or more devices different from the embodiments with corresponding changes. The modules of the above embodiments may be combined into one module, or further split into multiple sub-modules.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A world coordinate and road coordinate conversion method of an unmanned simulation system is applied to the unmanned simulation system, and the method comprises the following steps:
recording the current road coordinates of the detected vehicle, wherein the current road coordinates at least comprise a current road identifier;
receiving new world coordinates of the tested vehicle sent by the tested unmanned algorithm software;
searching on the target road identified by the current road identification according to the new world coordinate, and if the search is successful, taking the searched road coordinate as a new road coordinate;
if the search is not successful, taking the road connected with the target road identified by the current road identification as a new road, searching on the new road according to the new world coordinate, if the search is successful, taking the searched road coordinate as a new road coordinate, if the search is not successful, taking the searched road connected with the new road as a new road, and returning to execute the step of searching on the new road according to the new world coordinate;
and sending the new road coordinate and the new world coordinate to the tested unmanned driving algorithm software.
2. The method of claim 1, wherein the step of searching for the target link identified by the current link identification based on the new world coordinates and, if the search is successful, regarding the searched link coordinates as new link coordinates comprises:
and converting the new world coordinate into a road coordinate system, judging whether the converted road coordinate on the target road can be obtained or not, if so, determining that the search is successful, taking the converted road coordinate as the searched road coordinate, and taking the searched road coordinate as the new road coordinate.
3. The method of claim 2, wherein the step of converting the new world coordinates into a road coordinate system, determining whether the converted road coordinates located on the target road are available, and if so, determining that the search is successful comprises:
converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate system, wherein the positive direction of an s axis in the road coordinate system is the extending direction of the road length, the positive direction of the t axis is the extending direction of the left road width, and the intersection point of the s axis and the t axis is the origin of the road coordinate system;
judging whether the size of the s coordinate is smaller than the road length of the target road or not;
if yes, judging whether the t coordinate is located in the road width range of the target road;
if so, determining the lane mark of the lane where the point where the converted road coordinate is located according to the t coordinate, obtaining the converted road coordinate which is formed by the s coordinate, the t coordinate and the lane mark and is located on the target road, and determining that the search is successful.
4. The method of claim 3, wherein the step of converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate system comprises:
calculating a first vector pointing from a starting point of the center line of the target road to a point where the new world coordinate is located;
calculating a tangent vector of the target road, and converting the tangent vector into a unit tangent vector, wherein the tangent vector of the target road is a vector along the tangent direction at the starting point;
performing dot product operation on the first vector and the unit tangent vector to obtain a first dot product operation result, and taking the first dot product operation result as an s coordinate in the converted road coordinate;
calculating a second vector of which the coordinate on the s axis is that the point of the s coordinate points to the point of the new world coordinate;
carrying out a difference product operation on the unit tangent vector and the second vector to obtain a difference product operation result;
performing dot product operation on the difference product operation result and the vector (0, 0, 1) to obtain a second dot product operation result;
calculating a quotient between the second dot product operation result and an absolute value of the second dot product operation result;
and calculating the product between the quotient and the length of the second vector, and taking the product as the t coordinate in the new road coordinate.
5. The method of any one of claims 1-4, wherein the current road coordinates further include a lane identification of a current lane.
6. A world coordinate and road coordinate conversion apparatus of an unmanned simulation system, applied to the unmanned simulation system, the apparatus comprising:
the recording module is used for recording the current road coordinates of the detected vehicle, and the current road coordinates at least comprise a current road identifier;
the receiving module is used for receiving the new world coordinates of the tested vehicle sent by the tested unmanned algorithm software;
the first searching module is used for searching on the target road identified by the current road identification according to the new world coordinate, and if the searching is successful, the searched road coordinate is used as a new road coordinate;
a second searching module, configured to, if the search is not successful, take a road connected to the target road identified by the current road identifier as a new road, search the new road according to the new world coordinate, if the search is successful, take the searched road coordinate as a new road coordinate, if the search is not successful, take the road connected to the new road that is not searched as a new road, and return to the step of performing the search on the new road according to the new world coordinate;
and the sending module is used for sending the new road coordinate and the new world coordinate to the tested unmanned driving algorithm software.
7. The apparatus of claim 6, wherein the first search module comprises:
and the judgment sub-module is used for converting the new world coordinate into a road coordinate system, judging whether the converted road coordinate on the target road can be obtained or not, if so, determining that the search is successful, taking the converted road coordinate as the searched road coordinate, and taking the searched road coordinate as the new road coordinate.
8. The apparatus of claim 7, wherein the determining sub-module comprises:
the conversion unit is used for converting the new world coordinate into a road coordinate system to obtain an s coordinate and a t coordinate in the converted road coordinate system, wherein the positive direction of an s axis in the road coordinate system is the extending direction of the road length, the positive direction of the t axis is the extending direction of the left road width, and the intersection point of the s axis and the t axis is the origin of the road coordinate system;
the first judgment unit is used for judging whether the size of the s coordinate is smaller than the road length of the target road or not, and if so, the second judgment unit is triggered;
the second judging unit is used for judging whether the t coordinate is located in the road width range of the target road, and if so, the determining unit is triggered;
and the determining unit is used for determining the lane mark of the lane where the point where the converted road coordinate is located according to the t coordinate, obtaining the converted road coordinate which is formed by the s coordinate, the t coordinate and the lane mark and is located on the target road, and determining that the search is successful.
9. The apparatus as claimed in claim 8, wherein said conversion unit is specifically configured to:
calculating a first vector pointing from a starting point of the center line of the target road to a point where the new world coordinate is located;
calculating a tangent vector of the target road, and converting the tangent vector into a unit tangent vector, wherein the tangent vector of the target road is a vector along the tangent direction at the starting point;
performing dot product operation on the first vector and the unit tangent vector to obtain a first dot product operation result, and taking the first dot product operation result as an s coordinate in the converted road coordinate;
calculating a second vector of which the coordinate on the s axis is that the point of the s coordinate points to the point of the new world coordinate;
carrying out a difference product operation on the unit tangent vector and the second vector to obtain a difference product operation result;
performing dot product operation on the difference product operation result and the vector (0, 0, 1) to obtain a second dot product operation result;
calculating a quotient between the second dot product operation result and an absolute value of the second dot product operation result;
and calculating the product between the quotient and the length of the second vector, and taking the product as the t coordinate in the new road coordinate.
10. The apparatus of any one of claims 6-9, wherein the current road coordinates further include a lane identification of a current lane.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110945844.3A CN113408157B (en) | 2021-08-18 | 2021-08-18 | World coordinate and road coordinate conversion method and device of unmanned simulation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110945844.3A CN113408157B (en) | 2021-08-18 | 2021-08-18 | World coordinate and road coordinate conversion method and device of unmanned simulation system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113408157A true CN113408157A (en) | 2021-09-17 |
| CN113408157B CN113408157B (en) | 2021-11-26 |
Family
ID=77688814
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110945844.3A Active CN113408157B (en) | 2021-08-18 | 2021-08-18 | World coordinate and road coordinate conversion method and device of unmanned simulation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113408157B (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110066303A1 (en) * | 2009-09-16 | 2011-03-17 | Hitachi, Ltd. | Determining system for localization methods combination |
| CN108896994A (en) * | 2018-05-11 | 2018-11-27 | 武汉环宇智行科技有限公司 | A kind of automatic driving vehicle localization method and equipment |
| CN110361020A (en) * | 2018-09-30 | 2019-10-22 | 长城汽车股份有限公司 | For determining the method and system of lane line coordinates |
-
2021
- 2021-08-18 CN CN202110945844.3A patent/CN113408157B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110066303A1 (en) * | 2009-09-16 | 2011-03-17 | Hitachi, Ltd. | Determining system for localization methods combination |
| CN108896994A (en) * | 2018-05-11 | 2018-11-27 | 武汉环宇智行科技有限公司 | A kind of automatic driving vehicle localization method and equipment |
| CN110361020A (en) * | 2018-09-30 | 2019-10-22 | 长城汽车股份有限公司 | For determining the method and system of lane line coordinates |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113408157B (en) | 2021-11-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112304302B (en) | Multi-scene high-precision vehicle positioning method and device and vehicle-mounted terminal | |
| CN106918342B (en) | Method and system for positioning driving path of unmanned vehicle | |
| US9400500B2 (en) | Autonomous vehicle refueling locator | |
| CN110667563B (en) | Transverse control method and device for automatic driving vehicle and vehicle | |
| CN108367722A (en) | Parking assistance method and device | |
| Joshi | A new approach to map matching for in-vehicle navigation systems: the rotational variation metric | |
| CN110361021A (en) | Lane line approximating method and system | |
| CN105571606A (en) | Methods and systems for enabling improved positioning of a vehicle | |
| CN110515055A (en) | The method positioned using radius chess game optimization laser radar | |
| CN108398701B (en) | Vehicle positioning method and device | |
| JP2003042772A (en) | Communication navigation system, communication navigation method, path-guiding information transmission apparatus, and terminal apparatus | |
| KR101248868B1 (en) | Self control driving system based on driving record | |
| CN113096433A (en) | Autonomous parking method and device based on vehicle-road cooperation and storage medium | |
| CN108444492A (en) | A kind of electric vehicle path planning system and planing method | |
| CN110031015B (en) | Method for approximately representing lane plane curve by using continuous derivative function | |
| CN104724121A (en) | Apparatus and method for recognizing driving field of vehicle | |
| JP2021191658A (en) | Driving assisting device, drive assisting method, and drive assisting program | |
| CN113408157B (en) | World coordinate and road coordinate conversion method and device of unmanned simulation system | |
| CN115237121A (en) | Scene reconstruction-based remote calling method and system and readable storage medium | |
| JP2003296899A (en) | Preceding vehicle determining device | |
| CN110626349B (en) | Control method and device for automatic driving vehicle, automobile controller and storage medium | |
| CN110069066B (en) | Unmanned vehicle tracking method and terminal | |
| Wong et al. | Single camera vehicle localization using SURF scale and dynamic time warping | |
| CN117213510A (en) | Automatic driving tracking travel information processing method, device, equipment and storage medium | |
| CN113706682B (en) | Method and device for three-dimensional rendering of road by unmanned simulation system |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP01 | Change in the name or title of a patent holder | ||
| CP01 | Change in the name or title of a patent holder |
Address after: 100070 401, 4th floor, 66 Zizhuyuan Road, Haidian District, Beijing Patentee after: Beijing Saimu Technology Co.,Ltd. Address before: 100070 401, 4th floor, 66 Zizhuyuan Road, Haidian District, Beijing Patentee before: Beijing saimu Technology Co.,Ltd. |