CN110647151B - Coordinate conversion method and device, computer readable storage medium and electronic equipment - Google Patents
Coordinate conversion method and device, computer readable storage medium and electronic equipment Download PDFInfo
- Publication number
- CN110647151B CN110647151B CN201910984513.3A CN201910984513A CN110647151B CN 110647151 B CN110647151 B CN 110647151B CN 201910984513 A CN201910984513 A CN 201910984513A CN 110647151 B CN110647151 B CN 110647151B
- Authority
- CN
- China
- Prior art keywords
- target
- target vehicle
- current
- coordinate
- road section
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Abstract
The embodiment of the invention relates to a coordinate conversion method and device, a computer readable storage medium and electronic equipment, and relates to the technical field of unmanned driving, wherein the method comprises the following steps: acquiring a plurality of ordered waypoints of a target vehicle on a route to be driven, and calculating the length of a target road section consisting of the ordered waypoints; calculating the rated driving time of the target vehicle on the target road section according to the rated driving speed of the target vehicle on the target road section and the length of the target road section; constructing a plurality of constraint conditions according to the rated driving speed, the rated driving time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition; and converting the current coordinates of the target vehicle on the target road section into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle. The embodiment of the invention improves the accuracy of the target coordinate.
Description
Technical Field
The embodiment of the invention relates to the technical field of automatic driving of vehicles, in particular to a coordinate conversion method, a coordinate conversion device, a computer readable storage medium and electronic equipment.
Background
The guiding line is an important basis for decision planning of vehicles in the structured road, and the high-quality guiding line is beneficial to accurate implementation of decision planning. The shape of a piece of leader line can be arbitrarily curved within the range allowed by the curvature, and even if the vehicle runs on a slightly curved road with equal width on both sides of the leader line, the corresponding collision avoidance constraint is difficult to describe easily in a cartesian coordinate system. Therefore, to address this issue, the concept of the Frenet coordinate system is introduced into automated driving decision planning on roads.
In a specific process of converting coordinate points in the cartesian coordinate system to the Frenet coordinate system, a point (x, y) on the cartesian coordinate system may be projected in the Frenet coordinate system based on a given guiding line and denoted as (s, l). How to map points on a cartesian coordinate system to a Frenet coordinate system quickly and determine their coordinates is an important basis for implementing automated driving on structured roads.
However, when the point (x, y) is searched for the corresponding coordinate in the corresponding Frenet coordinate system, the prior art scheme often cannot accurately find the matching point, so that after the coordinate system is continuously and bidirectionally converted for many times, the coordinate value is continuously changed, i.e. the coordinate system conversion process is unreliable and unstable, and the accuracy of the conversion result is low.
Therefore, it is desirable to provide a new coordinate transformation method and apparatus.
It is to be noted that the information invented in the above background section is only for enhancing the understanding of the background of the present invention, and therefore, may include information that does not constitute prior art known to those of ordinary skill in the art.
Disclosure of Invention
An object of the present invention is to provide a coordinate transformation method, a coordinate transformation apparatus, a computer-readable storage medium, and an electronic device, which overcome, at least to some extent, the problem of low accuracy of transformation results due to limitations and disadvantages of the related art.
According to an aspect of the present disclosure, there is provided a coordinate conversion method including:
acquiring a plurality of ordered waypoints of a target vehicle on a route to be driven, and calculating the length of a target road section consisting of the ordered waypoints;
calculating the rated running time of the target vehicle on the target road section according to the rated running speed of the target vehicle on the target road section and the length of the target road section;
constructing a plurality of constraint conditions according to the rated running speed, the rated running time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition;
and converting the current coordinates of the target vehicle on the target road section into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle.
In an exemplary embodiment of the present disclosure, the plurality of ordered waypoints comprises an initial waypoint, a plurality of intermediate ordered waypoints and a termination waypoint;
wherein the coordinate conversion method further comprises:
and respectively calculating an initial rated attitude angle and a final rated attitude angle of the target vehicle on the target road section according to the initial waypoint and the final waypoint.
In an exemplary embodiment of the present disclosure, the plurality of constraints include a first constraint, a second constraint, a third constraint, and a fourth constraint;
wherein constructing a plurality of constraints based on the rated travel speed, the rated travel time, and each of the ordered waypoints comprises:
constructing the first constraint condition according to the current running speed, the current attitude angle, the current deflection angle, the current angular speed and the current acceleration of the target vehicle on the target road section;
constructing a second constraint condition according to the initial rated attitude angle, the initial waypoint and the rated running speed;
constructing a third constraint condition according to the termination rated attitude angle and the rated running speed;
and constructing a fourth constraint condition according to the rated running speed, the rated running time and each ordered waypoint.
In an exemplary embodiment of the disclosure, obtaining the parameterized guidance line of the target vehicle on the target road segment according to each constraint condition includes:
constructing an objective function according to the current angular velocity, the current acceleration, each ordered waypoint and the current waypoint corresponding to each ordered waypoint;
performing a minimization operation on the objective function based on the first constraint, the second constraint, the third constraint and the fourth constraint;
and obtaining a parameterized guiding line of the target vehicle on the target road section according to the operation result of the minimization operation.
In an exemplary embodiment of the present disclosure, converting the current coordinates of the target vehicle on the target road segment to target coordinates based on the parameterized guideline comprises:
converting current coordinates of the target vehicle on the target road segment in a Cartesian coordinate system to target coordinates in a Frenet coordinate system based on the parameterized guideline; or
Converting current coordinates of the target vehicle on the target road segment in a Frenet coordinate system to target coordinates in a Cartesian coordinate system based on the parameterized guidelines.
In an exemplary embodiment of the disclosure, converting the current coordinates of the target vehicle on the target road segment in a cartesian coordinate system to target coordinates in a Frenet coordinate system based on the parameterized guideline comprises:
calculating a first projection point of a current coordinate of the target vehicle on the target road segment in a Cartesian coordinate system on the parametric guideline;
constructing a Euclidean distance according to the first projection point and the current coordinate; wherein the Euclidean distance is perpendicular to a tangent of the parametric guideline at the first projection point;
sampling the projection points in the parameterized guiding line to obtain a plurality of sampling points, and performing second-order fitting on the Euclidean distance to obtain a second-order fitting function;
calculating a first extreme value of the second-order fitting function, and obtaining initial longitudinal displacement of the current coordinate in the Frenet coordinate system according to each sampling point and the first extreme value;
obtaining the initial transverse displacement of the current coordinate in the Frenet coordinate system according to the initial longitudinal displacement and the Euclidean distance;
and obtaining an initial target coordinate according to the initial longitudinal displacement and the initial transverse displacement.
In an exemplary embodiment of the present disclosure, the coordinate conversion method further includes:
when the deviation between the longitudinal displacement and any one sampling point is determined to be smaller than a preset threshold value, calculating a first derivative and a second derivative of the Euclidean distance;
and obtaining standard longitudinal displacement according to the first derivative and the second derivative, obtaining standard transverse displacement according to the standard longitudinal displacement and the Euclidean distance, and obtaining a standard target coordinate according to the standard transverse displacement and the standard longitudinal displacement.
In an exemplary embodiment of the disclosure, converting the current coordinates of the target vehicle in a Frenet coordinate system on the target road segment to target coordinates in a cartesian coordinate system based on the parameterized guideline comprises:
calculating a second projection point of the current coordinate of the target vehicle on the target road section in a Frenet coordinate system on the parameterized guiding line;
calculating an included angle between the parameterized guiding line and a tangent line of the parameterized guiding line at the second projection point;
and obtaining the target coordinate of the current coordinate in the Cartesian coordinate system according to the included angle, the second projection point and the current coordinate.
According to an aspect of the present disclosure, there is provided a coordinate conversion apparatus including:
the first calculation module is used for acquiring a plurality of ordered waypoints of a target vehicle on a route to be driven and calculating the length of a target road section consisting of the ordered waypoints;
the second calculation module is used for calculating the rated running time of the target vehicle on the target road section according to the rated running speed of the target vehicle on the target road section and the length of the target road section;
the constraint condition construction module is used for constructing a plurality of constraint conditions according to the rated running speed, the rated running time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition;
and the coordinate conversion module is used for converting the current coordinate of the target vehicle on the target road section into a target coordinate based on the parameterized guiding line so that the target vehicle avoids the obstacle according to the target coordinate and the real-time coordinate of the obstacle.
According to an aspect of the present disclosure, there is provided a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the coordinate conversion method of any one of the above.
According to an aspect of the present disclosure, there is provided an electronic device including:
a processor; and
a memory for storing executable instructions of the processor;
wherein the processor is configured to perform any one of the coordinate conversion methods described above via execution of the executable instructions.
On one hand, the method and the device for converting the coordinates calculate the length of a target road section consisting of all ordered waypoints, and calculate the rated running time of a target vehicle on the target road section according to the rated running speed of the target vehicle on the target road section and the length of the target road section; then, constructing a plurality of constraint conditions according to the rated running speed, the rated running time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition; finally, converting the current coordinates of the target vehicle on the target road section into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle; the problems that in the prior art, after coordinate system conversion is continuously carried out for multiple times in a bidirectional way due to the fact that matching points cannot be accurately found, coordinate values are continuously changed, the coordinate system conversion process is unreliable and unstable, and the accuracy of conversion results is low are solved, the reliability and the stability of the coordinate system conversion process are improved, and the accuracy of target coordinates is improved; on the other hand, a plurality of constraint conditions are constructed according to the rated driving speed, the rated driving time and each ordered route point, and a parameterized guiding line of the target vehicle on the target road section is obtained according to each constraint condition; finally, the current coordinates of the target vehicle on the target road section are converted into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle, and the loss of the target vehicle caused by the fact that the target vehicle cannot avoid the obstacle due to low accuracy of the target coordinates is avoided; on the other hand, a plurality of constraint conditions are constructed according to the rated running speed, the rated running time and each ordered waypoint, and a parameterized guiding line of the target vehicle on the target road section is obtained according to each constraint condition; and finally, converting the current coordinate of the target vehicle on the target road section into the target coordinate based on the parameterized guiding line, so that the conversion speed from the current coordinate to the target coordinate is increased.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the invention, as claimed.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and together with the description, serve to explain the principles of the invention. It is obvious that the drawings in the following description are only some embodiments of the invention, and that for a person skilled in the art, other drawings can be derived from them without inventive effort.
Fig. 1 schematically shows a flowchart of a coordinate conversion method according to an exemplary embodiment of the present invention.
Fig. 2 schematically shows a flow chart of a method for constructing a plurality of constraints from the nominal travel speed, the nominal travel time and the ordered waypoints, according to an exemplary embodiment of the invention.
Fig. 3 schematically shows an example diagram of a degree-of-freedom vehicle motion model (target vehicle motion model) according to an example embodiment of the invention.
Fig. 4 schematically illustrates an example graph of limiting deviation between a target vehicle and a corresponding waypoint at a scaled time according to an example embodiment of the invention.
Fig. 5 is a flowchart schematically illustrating a method for obtaining a parameterized guidance line of the target vehicle on the target road segment according to each constraint condition according to an exemplary embodiment of the present invention.
Fig. 6 schematically shows a flow chart of a method for converting the current coordinates of the target vehicle on the target path in a cartesian coordinate system into target coordinates in a Frenet coordinate system based on the parameterized guiding line according to an exemplary embodiment of the invention.
Fig. 7 schematically illustrates a selection principle diagram of a matching point of a current coordinate point on a guiding line according to an exemplary embodiment of the present invention.
Fig. 8 schematically shows a flowchart of another coordinate conversion method according to an exemplary embodiment of the present invention.
Fig. 9 schematically shows a flow chart of a method for translating the current coordinates of the target vehicle on the target path in the Frenet coordinate system to target coordinates in the cartesian coordinate system based on the parameterized guidelines according to an exemplary embodiment of the invention.
Fig. 10 schematically shows a block diagram of a coordinate conversion apparatus according to an exemplary embodiment of the present invention.
Fig. 11 schematically illustrates an electronic device for implementing the coordinate conversion method according to an exemplary embodiment of the present invention.
Detailed Description
Example embodiments will now be described more fully with reference to the accompanying drawings. Example embodiments may, however, be embodied in many different forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to provide a thorough understanding of embodiments of the invention. One skilled in the relevant art will recognize, however, that the invention can be practiced without one or more of the specific details, or with other methods, components, devices, steps, and so forth. In other instances, well-known technical solutions have not been shown or described in detail to avoid obscuring aspects of the invention.
Furthermore, the drawings are merely schematic illustrations of the invention and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus their repetitive description will be omitted. Some of the block diagrams shown in the figures are functional entities and do not necessarily correspond to physically or logically separate entities. These functional entities may be implemented in the form of software, or in one or more hardware modules or integrated circuits, or in different networks and/or processor devices and/or microcontroller devices.
In the present exemplary embodiment, a coordinate transformation method is first provided, where the method may be performed in a server, a server cluster, a cloud server, or the like, or may be performed in an equipment terminal; of course, those skilled in the art may also run the method of the present invention on other platforms as needed, which is not limited in this exemplary embodiment. Referring to fig. 1, the coordinate conversion method may include the steps of:
and S110, acquiring a plurality of ordered route points of the target vehicle on the route to be driven, and calculating the length of a target road section consisting of the ordered route points.
And S120, calculating the rated running time of the target vehicle on the target road section according to the rated running speed of the target vehicle on the target road section and the length of the target road section.
And S130, constructing a plurality of constraint conditions according to the rated running speed, the rated running time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition.
And S140, converting the current coordinate of the target vehicle on the target road section into a target coordinate based on the parameterized guiding line, so that the target vehicle avoids the obstacle according to the target coordinate and the real-time coordinate of the obstacle.
In the coordinate transformation method, on one hand, the length of a target road section consisting of the ordered waypoints is calculated, and the rated driving time of the target vehicle on the target road section is calculated according to the rated driving speed of the target vehicle on the target road section and the length of the target road section; then constructing a plurality of constraint conditions according to the rated driving speed, the rated driving time and each ordered route point, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition; finally, converting the current coordinates of the target vehicle on the target road section into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle; the problems that in the prior art, due to the fact that matching points cannot be found accurately, coordinate values are changed continuously after coordinate system conversion is carried out continuously and bidirectionally for multiple times, the coordinate system conversion process is unreliable and unstable, and the accuracy of conversion results is low are solved, the reliability and stability of the coordinate system conversion process are improved, and the accuracy of target coordinates is improved; on the other hand, a plurality of constraint conditions are constructed according to the rated driving speed, the rated driving time and each ordered route point, and a parameterized guiding line of the target vehicle on the target road section is obtained according to each constraint condition; finally, the current coordinates of the target vehicle on the target road section are converted into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle, and the loss of the target vehicle caused by the fact that the target vehicle cannot avoid the obstacle due to low accuracy of the target coordinates is avoided; on the other hand, a plurality of constraint conditions are constructed according to the rated driving speed, the rated driving time and each ordered route point, and a parameterized guiding line of the target vehicle on the target road section is obtained according to each constraint condition; and finally, converting the current coordinate of the target vehicle on the target road section into the target coordinate based on the parameterized guiding line, so that the conversion speed from the current coordinate to the target coordinate is increased.
Hereinafter, each step in the coordinate conversion method according to the exemplary embodiment of the present invention will be explained and explained in detail with reference to the drawings.
First, the guiding lines are explained and explained. The guiding line is an important basis for decision planning of vehicles in the structured road, and the high-quality guiding line is beneficial to accurate implementation of decision planning. It is generally considered that the guiding line is a running path which can be perfectly tracked by the underlying controller when the vehicle is in a state that the current lane is empty (namely, no other moving or static vehicle or obstacle such as pedestrian exists in the current lane).
Furthermore, the generation of the guiding line depends on the coordinate position information of a series of waypoints (waypoints), and the task of generating the guiding line is to generate a path which is as close to the waypoints as possible and is sufficiently smooth. In order to generate the guiding lines in a customized manner, a target vehicle which is the same as the current vehicle kinematic model can be constructed, and a path which enables the target vehicle to travel close to a series of waypoints as much as possible and has better smoothness is planned. The problem is constructed as a track planning optimal control problem which enables the vehicle to run at the nominal speed, and the path part in the optimal track is output as the guiding line.
In step S110, a plurality of ordered waypoints of the target vehicle on the route to be traveled are acquired, and the length of the target section composed of each of the ordered waypoints is calculated.
In the present exemplary embodiment, first, (N) of the target vehicle i on the route to be traveled (route) may be acquired from the navigation module waypoints +1) ordered waypoints; wherein the plurality of ordered waypoints may include an initial waypoint, a plurality of intermediate ordered waypoints, and a termination waypoint; then, recording each ordered waypoint in the point set { (x) wk ,y wk ),k＝0,...,N waypoints In (1) }; then, calculating the length s of the target road section formed by sequentially connecting the ordered waypoints waypoints . Wherein:
wherein, N waypoints Is that the above-mentionedThe number of ordered waypoints; (x) wk ,y wk ) The coordinates of the kth ordered waypoints.
In step S120, a rated travel time of the target vehicle on the target road segment is calculated according to the rated travel speed of the target vehicle on the target road segment and the length of the target road segment.
In the present exemplary embodiment, first, the rated travel speed v of the target vehicle i on the above-described target section is determined nominal (ii) a Then, according to the rated running speed of the target vehicle on the target road section and the length of the target road section, the rated running time of the target vehicle on the target road section is calculated. Specifically, the rated travel time t of the target vehicle on the target road section estimated This can be shown as follows:
in step S130, a plurality of constraint conditions are constructed according to the rated driving speed, the rated driving time and each of the ordered waypoints, and a parameterized guiding line of the target vehicle on the target road segment is obtained according to each of the constraint conditions.
In the present exemplary embodiment, first, in order that the plurality of constraints may be easily constructed, it is also necessary to calculate an initial rated attitude angle and a termination rated attitude angle of the target vehicle on the target road segment. The method specifically comprises the following steps: and respectively calculating an initial rated attitude angle and a final rated attitude angle of the target vehicle on the target road section according to the initial waypoint and the final waypoint.
First, the initial waypoint (x) may be passed w0 ,y w0 ) And a next ordered waypoint (x) corresponding to the initial waypoint w1 ,y w1 ) An initial nominal attitude angle θ (0) of the target vehicle i at the initial time t ═ 0 can be determined. Wherein:
tan(θ(0))·(x w1 -x w0 )＝y w1 -y w0 ； (3a)
similarly, the waypoints may be based on the termination waypoints
And the last ordered waypoint corresponding to the terminating waypoint
It may be determined that the target vehicle i is at the termination time t ═ t f End nominal attitude angle theta of i (t f )：
Further, after the initial rated attitude angle and the final rated attitude angle are obtained, a plurality of constraint conditions can be constructed according to the rated running speed, the rated running time and each ordered waypoint; wherein the plurality of constraints include a first constraint, a second constraint, a third constraint, and a fourth constraint. Specifically, referring to fig. 2, constructing a plurality of constraints according to the rated driving speed, the rated driving time, and each of the ordered waypoints may include steps S210 to S240, which will be described in detail below.
In step S210, the first constraint condition is constructed according to the current driving speed, the current attitude angle, the current yaw angle, the current angular speed, and the current acceleration of the target vehicle on the target road segment.
In this exemplary embodiment, the first constraint condition may be, for example, a system dynamic equation constraint, which may specifically be as follows:
wherein, referring to FIG. 3, in the above formula (4), t ∈ [0, t ∈ f ]Representing the motion time domain, (x) i (t),y i (t)) is the rear axle midpoint coordinate, v, of the target vehicle i i (t) and a i (t) respectively representing the current running speed and the current acceleration along the longitudinal axis direction of the vehicle body, so that the advancing direction of the vehicle is a positive direction; phi is a unit of i (t) is a vehicle front wheel deflection angle (current deflection angle) with the left-turn direction as the positive direction; omega i (t) is the front wheel yaw angular velocity (current angular velocity); theta i And (t) represents the attitude angle of the vehicle in the current coordinate system (current attitude angle), namely the included angle formed by the positive direction of the X axis of the coordinate system and the longitudinal axis direction of the vehicle body. Wherein, t 0 、t f Respectively the starting movement moment and the ending movement moment of the target vehicle i on the target road section, t 0 Is known, but t f Not fixed, t f And t estimated The difference therebetween is not greater than a preset value.
In addition, four geometry-related parameters of the vehicle i are also defined in fig. 3. Wherein L is w Representing the front and rear wheel base, L f Represents the front overhang distance of the vehicle, L r Represents the rear overhang distance, L b Representing the vehicle width. And, due to x i (t)、y i (t)、φ i (t)、θ i (t) and v i (t) belongs to the state variable x (t), and a i (t)、ω i (t) belongs to the control variable u (t). In this respect, it can be intuitively understood that: if a given vehicle i is at an initial time t 0 Motion state x (t) 0 ) And the motion time domain [ t 0 ,t f ]U (t) above, the motion state x (t) in the time domain can be uniquely determined (by means of an integration operation), corresponding to the unique driving trajectory of the vehicle i.
In step S220, a second constraint condition is constructed according to the initial nominal attitude angle, the initial waypoint and the nominal driving speed.
In step S230, a third constraint condition is constructed according to the terminal rated attitude angle and the rated running speed.
Step S220 and step S230 will be explained and explained below. First, the second constraint and the third constraint may be, for example, edge value constraints. Specifically, the second constraint may be, for example, an initial value constraint, and the third constraint may be, for example, a termination value constraint. Specifically, the initial value constraint may be, for example:
tan(θ(0))·(x w1 -x w0 )＝y w1 -y w0 ； (3a)
and
[x i (0),y i (0),v i (0),a i (0),ω i (0)]＝[x w0 ,y w0 ,v nominal ,0,0]； (5a)
wherein x is i (0) And y i (0) Is the coordinate of the middle point of the rear axle of the target vehicle i at the time t equal to 0, v i (0)、a i (0) And ω i (0) The current running speed, the current acceleration, and the current angular velocity of the target vehicle i at the time t-0.
Further, the termination value constraint may be, for example:
[v i (t f ),a i (t f ),ω i (t f )]＝[v nominal ,0,0]； (5b)
wherein v is i (t f )、a i (t f ) And ω i (t f ) Is respectively t ═ t f The current running speed, the current acceleration, and the current angular velocity of the target vehicle i at the time. Further, there is no pair of φ in the initial value constraint and the end value constraint i (0)、φ i (t f )、x i (t f ) And y i (t f ) By setting the equality constraints, it is possible to prevent the vehicle motion behavior from being excessively limited, resulting in a problem-free or path-distorted behavior.
In step S240, a fourth constraint condition is constructed according to the rated travel speed, the rated travel time, and each of the ordered waypoints.
In the present exemplary embodiment, the fourth constraint may be, for example, a manifold constraint. Wherein the manifold constraint comprises limiting the current vehicle speed at each moment in v nominal Conditions of fluctuation in a minute range:
|v i (t)-v n o minal |≤ε velocity ,t∈[0,t f ]； (6a)
and limiting an actual travel time t for traveling the target link f At t estimated Inequality constraints floating over a range:
|t f -t estimated |≤ε time ； (6b)
and limiting the time when the target vehicle is divided into the proportion
Upper and corresponding waypoint (x) wk ,y wk ) The deviation between does not exceed a certain threshold:
the limitation of the deviation between the scaled time and the corresponding waypoint of the target vehicle may be specifically described with reference to fig. 4.
In addition, the allowable action section in which the mechanical characteristics inherent in the target vehicle i correspond to the state/control variable is included, and the following should be set for the vehicle i:
|φ i (t)|≤Φ max ； (6d)
|a i (t)|≤a max ； (6e)
|v i (t)|≤v max ； (6f)
|ω i (t)|≤Ω max ； (6g)
wherein t is ∈ [0, t ∈ [ ] f ]；Φ max 、a max 、v max And omega max Respectively, amplitude parameters of each interval. Phi max Representing the angle of rotation phi of the front wheels of the vehicle i (t) a maximum allowable deflection angle value; v. of max Not the design speed limit of the vehicle, but a safety upper speed limit set specifically in low speed scenarios; to ensure passenger comfort, a max And omega max Respectively linear acceleration, front wheel rotationThe angular velocity sets the amplitude. In addition, if the acceleration variable changes smoothly, the differential variable jerk of the acceleration should be supplemented i (t) and setting the amplitude value thereto.
Further, after obtaining the constraint conditions, a parameterized guidance line of the target vehicle on the target road segment may be obtained according to the constraint conditions. Specifically, referring to fig. 5, obtaining the parameterized guidance line of the target vehicle on the target road segment according to the constraint conditions may include steps S510 to S530, which will be described in detail below.
In step S510, an objective function is constructed according to the current angular velocity, the current acceleration, each ordered waypoint, and a current waypoint corresponding to each ordered waypoint.
In step S520, a minimization operation is performed on the objective function based on the first constraint, the second constraint, the third constraint, and the fourth constraint.
In step S530, a parameterized guiding line of the target vehicle on the target road segment is obtained according to the operation result of the minimization operation.
Hereinafter, steps S510 to S530 will be explained and explained. Firstly, an objective function is constructed according to the current angular velocity, the current acceleration, each ordered waypoint and the current waypoint corresponding to each ordered waypoint. Specifically, the objective function J may be set such that the trajectory of the vehicle is as close to each given route point as possible, and the driving process is as stable as possible. For example, the objective function may be as follows:
wherein w1, w2 and w3 are weights greater than 0, and the sum of w1, w2 and w3 is 1. Further, after obtaining the objective function, the following optimal control proposition can be obtained:
the minimum value (7) is obtained,
s.t. system dynamic equation constraints (4)
Initial time constraints (3a), (5 a); (8)
constraint of termination time (3b), (5b)
Manifold constraint (6)
Furthermore, the proposition (8) can be solved by a common numerical optimization method (such as an interior point algorithm or an SQP algorithm), so that a smooth guiding line is obtained. In the numerical solving process, the path formed by the waypoints and the nominal speed value v matched with the path nominal An initial solution can be formed, and the initial trajectory is used for initialization, so that the numerical solution process is completed quickly. Generally, the generation of the guiding line does not depend on a real and complex driving environment, so that the generation can be finished off line or triggered intermittently on line, and the calculation timeliness of the guiding line does not always form technical difficulty.
In step S140, the current coordinates of the target vehicle on the target road segment are converted into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle.
In this example embodiment, converting the current coordinates of the target vehicle on the target road segment to target coordinates based on the parameterized guideline may include: converting current coordinates of the target vehicle on the target road segment in a Cartesian coordinate system to target coordinates in a Frenet coordinate system based on the parameterized guidelines; or converting the current coordinates of the target vehicle on the target road segment in the Frenet coordinate system to target coordinates in a Cartesian coordinate system based on the parameterized guidelines.
Specifically, first, referring to fig. 6, converting the current coordinates of the target vehicle on the target road segment in the cartesian coordinate system into the target coordinates in the Frenet coordinate system based on the parameterized guiding line may include steps S610 to S650, which will be described in detail below.
In step S610, a first projection point of the current coordinates of the target vehicle on the target road segment in the cartesian coordinate system on the parameterized guideline is calculated.
In the present exemplary embodiment, first, a parametric guiding line with a length L in a cartesian coordinate system is assumed, which is further abbreviated as:
Γ(x(s),y(s)),s∈[0,L]； (9)
where s represents mileage. Known point P 0 The coordinate in the Cartesian coordinate system is (x) 0 ,y 0 ) To determine P 0 The coordinate values of the points in the Frenet coordinate system need to be calculated on the curve segment Γ and P 0 The matched first projection point P is (x(s), y (s)), and the line segment P is made 0 P is shortest in length, i.e.:
in step S620, constructing a euclidean distance according to the first projection point and the current coordinate; wherein the Euclidean distance is perpendicular to a tangent of the parametric guideline at the first projection point.
In this exemplary embodiment, after obtaining the first projection point, an euclidean distance d(s) may be calculated. Wherein the content of the first and second substances,
D(s)＝(x(s)-x 0 ) 2 +(y(s)-y 0 ) 2 ； (11)
it should be added here that the point P meeting the condition enables the tangent direction of the curve at that point to be aligned with P 0 P is perpendicular, as shown in FIG. 7, P 0 The coordinates in the Frenet coordinate system can be determined as (s, l).
In step S630, the projection points are sampled in the parameterized guiding line to obtain a plurality of sampling points, and second-order fitting is performed on the euclidean distance to obtain a second-order fitting function.
In step S640, a first extreme value of the second-order fitting function is calculated, and an initial longitudinal displacement of the current coordinate in the Frenet coordinate system is obtained according to each sampling point and the first extreme value.
In step S650, an initial lateral displacement of the current coordinate in the Frenet coordinate system is obtained according to the initial longitudinal displacement and the euclidean distance, and an initial target coordinate is obtained according to the initial longitudinal displacement and the initial lateral displacement.
Hereinafter, steps S630 to S650 will be explained and explained. First, it can be calculated by the method of the minimum second order
The core idea is to perform second-order fitting on D(s) through multi-point sampling, and to obtain a quadratic function
Considered as an estimate of D(s), and then looking for
Taking the closed solution of the minimum value. Specifically, first, three different preliminary estimates are formed for s on the curve, and each of the three preliminary estimates is denoted as s 1 、s 2 And s 3 . Second, a second order fit should be applied to D(s), since D(s) at s can be found 1 、s 2 And s 3 From the function values, a second order fit function for d(s) can be determined as:
further, due to
Is a quadratic function, and can obtain extreme value thereof through elementary mathematical knowledge
Finally, s is required to be 1 、s 2 、s 3 And
respectively substituting four mileage values
Selecting three mileage values with larger corresponding function values, and recording the three mileage values as s 1 、s 2 、s 3 And repeating the second step, iterating to
The convergence value is recorded as s at convergence, so that P can be determined 0 The coordinates in the Frenet coordinate system are (s, D (s)). In engineering practice, the existence range of s is often shortened in advance through historical data, and the method is significant when the curve mileage L is large. Suppose we define s e [ s ∈. [ s ] k ,s k+1 ]Then three initial estimates of s may be set to s k 、s k+1 And
at this point, if the final s converges to the interval s k ,s k+1 ]Otherwise, the interval is considered to have deviation, and the iterative solution of s is carried out again in the adjacent interval correspondingly.
Furthermore, the calculation can also be carried out by a Newton iteration method
The core idea is to iteratively find an extreme value s ═ s where D'(s) ═ 0 is established by a newton method. In particular, s may be provided at some initial value of s init Thereafter, the following iterations are performed:
then, use
To s init Assign value and continue repeating (14) until
And when the value is converged, the convergence value is recorded as s.
What needs to be added here is: although the minimum second-order method has high convergence speed, the adoption of second-order fitting can cause solution precision loss; the newton iteration method has higher solving accuracy due to the use of d(s) second derivative information, but the convergence speed is obviously not fast. Thus, in engineering practice, a coarse solution may be first obtained using a least-second order method, followed by a fine calculation using newton's iterative method in its small neighborhood. Therefore, referring to fig. 8, the coordinate conversion method may further include steps S810 to S820, which will be described in detail below.
In step S810, when it is determined that the deviation between the longitudinal displacement and any one of the sampling points is smaller than a preset threshold, a first derivative and a second derivative of the euclidean distance are calculated.
In step S820, a standard longitudinal displacement is obtained according to the first derivative and the second derivative, and a standard transverse displacement is obtained according to the standard longitudinal displacement and the euclidean distance.
In step S830, standard target coordinates are obtained according to the standard transverse displacement and the standard longitudinal displacement.
In the exemplary embodiment shown in fig. 8, on the one hand, the calculation speed of the target coordinates can be increased; on the other hand, the accuracy of the target coordinate can be improved.
Further, referring to fig. 9, converting the current coordinates of the target vehicle in the Frenet coordinate system on the target road segment into the target coordinates in the cartesian coordinate system based on the parameterized guiding line may include steps S910 to S930, which will be described in detail below.
In step S910, a second projection point of the current coordinates of the target vehicle on the target road segment in the Frenet coordinate system on the parameterized guiding line is calculated.
In step S920, an included angle between the parametric guiding line and a tangent line of the parametric guiding line at the second projection point is calculated.
In step S930, a target coordinate of the current coordinate in the cartesian coordinate system is obtained according to the included angle, the second projection point, and the current coordinate.
Hereinafter, steps S910 to S930 will be explained and explained. First, a piece of parameterized guiding line Γ (x(s), y (s)) in a Cartesian coordinate system and a point P are known 0 Coordinates (s, l) in the Frenet coordinate system for determining P 0 The coordinate value of point in Cartesian coordinate system is determined by determining P 0 Index line tangent direction θ at the projected point (s, 0) of the point on the index line:
p can then be determined 0 The coordinates of a point in a cartesian coordinate system are:
and finally, after the target coordinates are obtained, planning a path for the target vehicle according to the target coordinates of the target vehicle i and the real-time coordinates of the obstacles, so that the target vehicle avoids the obstacles according to the target coordinates and the real-time coordinates of the obstacles.
By the coordinate conversion method provided by the embodiment of the invention, the target coordinate of the target vehicle at any moment can be rapidly calculated, so that the obstacle can be avoided as much as possible according to the target coordinate and the real-time coordinate of the obstacle under any condition, smooth running of the target vehicle is ensured, and great contribution is made to the unmanned technology.
The exemplary embodiment of the present invention also provides a coordinate conversion apparatus. Referring to fig. 10, the coordinate conversion apparatus may include a first calculation module 1010, a second calculation module 1020, a constraint building module 1030, and a coordinate conversion module 1040. Wherein:
the first calculating module 1010 may be configured to obtain a plurality of ordered waypoints of the target vehicle on the route to be traveled, and calculate a length of the target road segment composed of the ordered waypoints.
The second calculating module 1020 may be configured to calculate a rated driving time of the target vehicle on the target road segment according to the rated driving speed of the target vehicle on the target road segment and the length of the target road segment.
The constraint condition constructing module 1030 may be configured to construct a plurality of constraint conditions according to the rated driving speed, the rated driving time, and each ordered waypoint, and obtain a parameterized guiding line of the target vehicle on the target road segment according to each constraint condition.
The coordinate conversion module 1040 may be configured to convert the current coordinates of the target vehicle on the target road segment into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle.
In an exemplary embodiment of the present disclosure, the plurality of ordered waypoints includes an initial waypoint, a plurality of intermediate ordered waypoints, and a termination waypoint;
wherein the coordinate conversion apparatus further comprises:
and the third calculation module can be used for calculating an initial rated attitude angle and a termination rated attitude angle of the target vehicle on the target road section according to the initial waypoint and the termination waypoint respectively.
In an exemplary embodiment of the present disclosure, the plurality of constraints include a first constraint, a second constraint, a third constraint, and a fourth constraint.
Wherein constructing a plurality of constraints based on the rated travel speed, the rated travel time, and each of the ordered waypoints comprises:
constructing the first constraint condition according to the current running speed, the current attitude angle, the current deflection angle, the current angular speed and the current acceleration of the target vehicle on the target road section; constructing a second constraint condition according to the initial rated attitude angle, the initial waypoint and the rated running speed; constructing a third constraint condition according to the termination rated attitude angle and the rated running speed; and constructing a fourth constraint condition according to the rated running speed, the rated running time and each ordered waypoint.
In an exemplary embodiment of the disclosure, obtaining the parameterized guidance line of the target vehicle on the target road segment according to each constraint condition includes:
constructing an objective function according to the current angular velocity, the current acceleration, each ordered waypoint and the current waypoint corresponding to each ordered waypoint; performing a minimization operation on the objective function based on the first constraint, the second constraint, the third constraint and the fourth constraint; and obtaining a parameterized guiding line of the target vehicle on the target road section according to the operation result of the minimization operation.
In an exemplary embodiment of the present disclosure, converting the current coordinates of the target vehicle on the target road segment to target coordinates based on the parameterized guideline includes:
converting current coordinates of the target vehicle on the target road segment in a Cartesian coordinate system to target coordinates in a Frenet coordinate system based on the parameterized guideline; or converting the current coordinates of the target vehicle on the target road segment in the Frenet coordinate system to target coordinates in a Cartesian coordinate system based on the parameterized guidelines.
In an exemplary embodiment of the disclosure, converting the current coordinates of the target vehicle on the target road segment in a cartesian coordinate system to target coordinates in a Frenet coordinate system based on the parameterized guidelines comprises:
calculating a first projection point of a current coordinate of the target vehicle on the target road segment in a Cartesian coordinate system on the parametric guideline; constructing a Euclidean distance according to the first projection point and the current coordinate; wherein the Euclidean distance is perpendicular to a tangent of the parametric guideline at the first projection point; sampling the projection points in the parameterized guiding line to obtain a plurality of sampling points, and performing second-order fitting on the Euclidean distance to obtain a second-order fitting function; calculating a first extreme value of the second-order fitting function, and obtaining initial longitudinal displacement of the current coordinate in the Frenet coordinate system according to each sampling point and the first extreme value; and obtaining an initial transverse displacement of the current coordinate in the Frenet coordinate system according to the initial longitudinal displacement and the Euclidean distance, and obtaining an initial target coordinate according to the initial longitudinal displacement and the initial transverse displacement.
In an exemplary embodiment of the present disclosure, the coordinate conversion apparatus further includes:
the fourth calculating module can be used for calculating a first derivative and a second derivative of the Euclidean distance when the deviation between the longitudinal displacement and any one sampling point is determined to be smaller than a preset threshold value;
the fifth calculation module may be configured to obtain a standard longitudinal displacement according to the first derivative and the second derivative, and obtain a standard transverse displacement according to the standard longitudinal displacement and the euclidean distance.
And the coordinate determination module can be used for obtaining standard target coordinates according to the standard transverse displacement and the standard longitudinal displacement.
In an exemplary embodiment of the disclosure, converting the current coordinates of the target vehicle in a Frenet coordinate system on the target road segment to target coordinates in a cartesian coordinate system based on the parameterized guideline comprises:
calculating a second projection point of the current coordinate of the target vehicle in the Frenet coordinate system on the target road section on the parameterized guiding line; calculating an included angle between the parameterized guiding line and a tangent line of the parameterized guiding line at the second projection point; and obtaining a target coordinate of the current coordinate in the Cartesian coordinate system according to the included angle, the second projection point and the current coordinate.
The specific details of each module in the coordinate conversion apparatus have been described in detail in the corresponding coordinate conversion method, and therefore are not described herein again.
It should be noted that although in the above detailed description several modules or units of the device for action execution are mentioned, such a division is not mandatory. Indeed, the features and functionality of two or more modules or units described above may be embodied in one module or unit, according to embodiments of the invention. Conversely, the features and functions of one module or unit described above may be further divided into embodiments by a plurality of modules or units.
Moreover, although the steps of the methods of the present invention are depicted in the drawings in a particular order, this does not require or imply that the steps must be performed in this particular order, or that all of the depicted steps must be performed, to achieve desirable results. Additionally or alternatively, certain steps may be omitted, multiple steps combined into one step execution, and/or one step broken down into multiple step executions, etc.
In an exemplary embodiment of the present invention, there is also provided an electronic device capable of implementing the above method.
As will be appreciated by one skilled in the art, aspects of the present invention may be embodied as a system, method or program product. Thus, various aspects of the invention may be embodied in the form of: an entirely hardware embodiment, an entirely software embodiment (including firmware, microcode, etc.) or an embodiment combining hardware and software aspects that may all generally be referred to herein as a "circuit," module "or" system.
An electronic device 1100 according to this embodiment of the invention is described below with reference to fig. 11. The electronic device 1100 shown in fig. 11 is only an example and should not bring any limitations to the function and the scope of use of the embodiments of the present invention.
As shown in fig. 11, electronic device 1100 is embodied in the form of a general purpose computing device. The components of the electronic device 1100 may include, but are not limited to: the at least one processing unit 1110, the at least one memory unit 1120, and a bus 1130 that couples various system components including the memory unit 1120 and the processing unit 1110.
Wherein the storage unit stores program code that is executable by the processing unit 1110 to cause the processing unit 1110 to perform steps according to various exemplary embodiments of the present invention as described in the above section "exemplary methods" of the present specification. For example, the processing unit 1110 may execute step S110 as shown in fig. 1: acquiring a plurality of ordered waypoints of a target vehicle on a route to be driven, and calculating the length of a target road section consisting of the ordered waypoints; step S120: calculating the rated running time of the target vehicle on the target road section according to the rated running speed of the target vehicle on the target road section and the length of the target road section; step S130: constructing a plurality of constraint conditions according to the rated running speed, the rated running time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition; step S140: and converting the current coordinates of the target vehicle on the target road section into target coordinates based on the parameterized guiding lines, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle.
The storage unit 1120 may include a readable medium in the form of a volatile memory unit, such as a random access memory unit (RAM)11201 and/or a cache memory unit 11202, and may further include a read only memory unit (ROM) 11203.
The electronic device 1100 can also communicate with one or more external devices 1200 (e.g., keyboard, pointing device, bluetooth device, etc.), one or more devices that enable a user to interact with the electronic device 1100, and/or any device (e.g., router, modem, etc.) that enables the electronic device 1100 to communicate with one or more other computing devices. Such communication may occur via an input/output (I/O) interface 1150. Also, the electronic device 1100 may communicate with one or more networks (e.g., a Local Area Network (LAN), a Wide Area Network (WAN), and/or a public network such as the internet) via the network adapter 1160. As shown, the network adapter 1160 communicates with the other modules of the electronic device 1100 over the bus 1130. It should be understood that although not shown in the figures, other hardware and/or software modules may be used in conjunction with the electronic device 1100, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems, to name a few.
Through the above description of the embodiments, those skilled in the art will readily understand that the exemplary embodiments described herein may be implemented by software, and may also be implemented by software in combination with necessary hardware. Therefore, the technical solution according to the embodiment of the present invention can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a usb disk, a removable hard disk, etc.) or on a network, and includes several instructions to make a computing device (which can be a personal computer, a server, a terminal device, or a network device, etc.) execute the method according to the embodiment of the present invention.
In an exemplary embodiment of the present invention, there is also provided a computer-readable storage medium having stored thereon a program product capable of implementing the above-described method of the present specification. In some possible embodiments, aspects of the invention may also be implemented in the form of a program product comprising program code means for causing a terminal device to carry out the steps according to various exemplary embodiments of the invention described in the above section "exemplary methods" of the present description, when said program product is run on the terminal device.
According to the program product for realizing the method, the portable compact disc read only memory (CD-ROM) can be adopted, the program code is included, and the program product can be operated on terminal equipment, such as a personal computer. However, the program product of the present invention is not limited in this regard and, in the present document, a readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The program product may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium include: an electrical connection having one or more wires, a portable disk, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
A computer readable signal medium may include a propagated data signal with readable program code embodied therein, for example, in baseband or as part of a carrier wave. Such a propagated data signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination thereof. A readable signal medium may also be any readable medium that is not a readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.
Program code embodied on a readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.
Program code for carrying out operations for aspects of the present invention may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C + + or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device, or entirely on the remote computing device or server. In the case of a remote computing device, the remote computing device may be connected to the user computing device through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computing device (e.g., through the internet using an internet service provider).
Furthermore, the above-described drawings are only schematic illustrations of processes involved in methods according to exemplary embodiments of the invention, and are not intended to be limiting. It will be readily understood that the processes shown in the above figures are not intended to indicate or limit the chronological order of the processes. In addition, it is also readily understood that these processes may be performed, for example, synchronously or asynchronously in multiple modules.
Other embodiments of the invention will be apparent to those skilled in the art from consideration of the specification and practice of the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the invention following, in general, the principles of the invention and including such departures from the present disclosure as come within known or customary practice within the art to which the invention pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the invention being indicated by the following claims.
Claims (10)
1. A coordinate conversion method, comprising:
acquiring a plurality of ordered waypoints of a target vehicle on a route to be driven, and calculating the length of a target road section consisting of the ordered waypoints;
calculating the rated driving time of the target vehicle on the target road section according to the rated driving speed of the target vehicle on the target road section and the length of the target road section;
constructing a plurality of constraint conditions according to the rated running speed, the rated running time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition;
converting current coordinates of the target vehicle on the target road segment in a Cartesian coordinate system to target coordinates in a Frenet coordinate system based on the parameterized guideline; or converting the current coordinates of the target vehicle in a Frenet coordinate system on the target road section into target coordinates in a Cartesian coordinate system based on the parameterized guiding line, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle.
2. The coordinate conversion method of claim 1, wherein the plurality of ordered waypoints comprises an initial waypoint, a plurality of intermediate ordered waypoints, and a termination waypoint;
wherein the coordinate conversion method further comprises:
and respectively calculating an initial rated attitude angle and a termination rated attitude angle of the target vehicle on the target road section according to the initial waypoint and the termination waypoint.
3. The coordinate conversion method according to claim 2, wherein the plurality of constraints include a first constraint, a second constraint, a third constraint, and a fourth constraint;
wherein constructing a plurality of constraints based on the rated travel speed, the rated travel time, and each of the ordered waypoints comprises:
constructing the first constraint condition according to the current running speed, the current attitude angle, the current deflection angle, the current angular speed and the current acceleration of the target vehicle on the target road section;
constructing a second constraint condition according to the initial rated attitude angle, the initial waypoint and the rated running speed;
constructing a third constraint condition according to the termination rated attitude angle and the rated running speed;
and constructing a fourth constraint condition according to the rated running speed, the rated running time and each ordered waypoint.
4. The coordinate conversion method according to claim 3, wherein obtaining the parameterized guidance line of the target vehicle on the target road segment according to each constraint condition comprises:
constructing an objective function according to the current angular velocity, the current acceleration, each ordered waypoint and the current waypoint corresponding to each ordered waypoint;
performing a minimization operation on the objective function based on the first constraint, the second constraint, the third constraint and the fourth constraint;
and obtaining a parameterized guiding line of the target vehicle on the target road section according to the operation result of the minimization operation.
5. The coordinate conversion method of claim 1, wherein converting the current coordinates of the target vehicle in a cartesian coordinate system on the target road segment to target coordinates in a Frenet coordinate system based on the parameterized guideline comprises:
calculating a first projection point of a current coordinate of the target vehicle on the target road segment in a Cartesian coordinate system on the parameterized guideline;
constructing a Euclidean distance according to the first projection point and the current coordinate; wherein the Euclidean distance is perpendicular to a tangent of the parametric guideline at the first projection point;
sampling the projection points in the parameterized guiding line to obtain a plurality of sampling points, and performing second-order fitting on the Euclidean distance to obtain a second-order fitting function;
calculating a first extreme value of the second-order fitting function, and obtaining initial longitudinal displacement of the current coordinate in the Frenet coordinate system according to each sampling point and the first extreme value;
and obtaining an initial transverse displacement of the current coordinate in the Frenet coordinate system according to the initial longitudinal displacement and the Euclidean distance, and obtaining an initial target coordinate according to the initial longitudinal displacement and the initial transverse displacement.
6. The coordinate conversion method according to claim 5, characterized by further comprising:
when the deviation between the longitudinal displacement and any one sampling point is determined to be smaller than a preset threshold value, calculating a first derivative and a second derivative of the Euclidean distance;
obtaining standard longitudinal displacement according to the first derivative and the second derivative, and obtaining standard transverse displacement according to the standard longitudinal displacement and the Euclidean distance;
and obtaining a standard target coordinate according to the standard transverse displacement and the standard longitudinal displacement.
7. The coordinate conversion method of claim 1, wherein converting the current coordinates of the target vehicle on the target road segment in a Frenet coordinate system to target coordinates in a Cartesian coordinate system based on the parameterized guideline comprises:
calculating a second projection point of the current coordinate of the target vehicle in the Frenet coordinate system on the target road section on the parameterized guiding line;
calculating an included angle between the parameterized guiding line and a tangent line of the parameterized guiding line at the second projection point;
and obtaining the target coordinate of the current coordinate in the Cartesian coordinate system according to the included angle, the second projection point and the current coordinate.
8. A coordinate conversion apparatus, characterized by comprising:
the first calculation module is used for acquiring a plurality of ordered waypoints of a target vehicle on a route to be driven and calculating the length of a target road section consisting of the ordered waypoints;
the second calculation module is used for calculating the rated running time of the target vehicle on the target road section according to the rated running speed of the target vehicle on the target road section and the length of the target road section;
the constraint condition construction module is used for constructing a plurality of constraint conditions according to the rated running speed, the rated running time and each ordered waypoint, and obtaining a parameterized guiding line of the target vehicle on the target road section according to each constraint condition;
the coordinate conversion module is used for converting the current coordinate of the target vehicle on the target road section in a Cartesian coordinate system into a target coordinate in a Frenet coordinate system based on the parameterized guiding line; or converting the current coordinates of the target vehicle in a Frenet coordinate system on the target road section into target coordinates in a Cartesian coordinate system based on the parameterized guiding line, so that the target vehicle avoids the obstacle according to the target coordinates and the real-time coordinates of the obstacle.
9. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the coordinate conversion method according to any one of claims 1 to 7.
10. An electronic device, comprising:
a processor; and
a memory for storing executable instructions of the processor;
wherein the processor is configured to perform the coordinate conversion method of any of claims 1-7 via execution of the executable instructions.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910984513.3A CN110647151B (en) | 2019-10-16 | 2019-10-16 | Coordinate conversion method and device, computer readable storage medium and electronic equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910984513.3A CN110647151B (en) | 2019-10-16 | 2019-10-16 | Coordinate conversion method and device, computer readable storage medium and electronic equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110647151A CN110647151A (en) | 2020-01-03 |
| CN110647151B true CN110647151B (en) | 2022-09-06 |
Family
ID=68994302
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910984513.3A Active CN110647151B (en) | 2019-10-16 | 2019-10-16 | Coordinate conversion method and device, computer readable storage medium and electronic equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110647151B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111338339B (en) * | 2020-02-20 | 2024-02-06 | 北京京东乾石科技有限公司 | Track planning method, track planning device, electronic equipment and computer readable medium |
| GB2594079A (en) * | 2020-04-16 | 2021-10-20 | Daimler Ag | A method for tracking at least one object in the surroundings of an autonomous motor vehicle with a Frenet frame, as well as an assistance system |
| CN113682298B (en) * | 2020-05-19 | 2024-04-05 | 北京京东乾石科技有限公司 | Vehicle speed limiting method and device |
| CN113759888A (en) * | 2020-06-08 | 2021-12-07 | 北京京东乾石科技有限公司 | Method, device and equipment for smoothing leader line and storage medium |
| CN111862604B (en) * | 2020-07-20 | 2022-03-04 | 北京京东乾石科技有限公司 | Unmanned vehicle control method and device, computer storage medium and electronic equipment |
| CN113804207A (en) * | 2020-09-14 | 2021-12-17 | 北京京东乾石科技有限公司 | Vehicle path planning method, system, equipment and storage medium |
| CN112710301B (en) * | 2020-12-09 | 2023-01-06 | 上汽大众汽车有限公司 | High-precision positioning method and system for automatic driving vehicle |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8880246B1 (en) * | 2012-08-22 | 2014-11-04 | United States Of America As Represented By The Secretary Of The Navy | Method and apparatus for determining spacecraft maneuvers |
| CN104375512B (en) * | 2014-10-30 | 2017-01-11 | 北京控制工程研究所 | Spacecraft maneuvering path optimization method based on spectral analysis |
| US10665115B2 (en) * | 2016-01-05 | 2020-05-26 | California Institute Of Technology | Controlling unmanned aerial vehicles to avoid obstacle collision |
| US10571921B2 (en) * | 2017-09-18 | 2020-02-25 | Baidu Usa Llc | Path optimization based on constrained smoothing spline for autonomous driving vehicles |
| CN110189547B (en) * | 2019-05-30 | 2020-10-20 | 广州小鹏汽车科技有限公司 | Obstacle detection method and device and vehicle |
-
2019
- 2019-10-16 CN CN201910984513.3A patent/CN110647151B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN110647151A (en) | 2020-01-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110647151B (en) | Coordinate conversion method and device, computer readable storage medium and electronic equipment | |
| CN110260867B (en) | Method, equipment and device for determining and correcting neutral position in robot navigation | |
| CN110160552B (en) | Navigation information determination method, device, equipment and storage medium | |
| US10823575B2 (en) | Reference line smoothing method using piecewise spiral curves with weighted geometry costs | |
| WO2020237890A1 (en) | Speed planning method and apparatus, electronic device and storage medium | |
| CN112020014B (en) | Lane change track planning method, device, server and storage medium | |
| CN109955853A (en) | For operating the method, system and storage medium of automatic driving vehicle | |
| CN109434831B (en) | Robot operation method and device, robot, electronic device and readable medium | |
| WO2019047639A1 (en) | Method and device for calculating curvature of vehicle trajectory | |
| WO2022056770A1 (en) | Path planning method and path planning apparatus | |
| CN110782092A (en) | Trajectory planning method and device of unmanned distribution vehicle in unstructured scene | |
| CN110836671A (en) | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus | |
| CN112904858A (en) | Path planning method, system and equipment with continuous curvature | |
| Bertolazzi et al. | Efficient re-planning for robotic cars | |
| CN113361152A (en) | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus | |
| WO2023082726A1 (en) | Lane changing strategy generation method and apparatus, computer storage medium, and electronic device | |
| CN117203108A (en) | Lane change based solely on local information | |
| CN115534925A (en) | Vehicle control method, device, equipment and computer readable medium | |
| CN114771551A (en) | Method and device for planning track of automatic driving vehicle and automatic driving vehicle | |
| Jo et al. | Construction process of a three-dimensional roadway geometry map for autonomous driving | |
| CN114763133A (en) | Vehicle parking planning method, device, equipment and computer storage medium | |
| CN111338339B (en) | Track planning method, track planning device, electronic equipment and computer readable medium | |
| CN113790725B (en) | Path planning method, path planning device, storage medium and mobile device | |
| CN114741790A (en) | Vehicle path tracking control method and device, storage medium and electronic equipment | |
| Daniel et al. | Energy constrained trajectory generation for ADAS |
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 |