CN112683291A - Vehicle turning path planning method and device, vehicle and storage medium - Google Patents
Vehicle turning path planning method and device, vehicle and storage medium Download PDFInfo
- Publication number
- CN112683291A CN112683291A CN202011629184.XA CN202011629184A CN112683291A CN 112683291 A CN112683291 A CN 112683291A CN 202011629184 A CN202011629184 A CN 202011629184A CN 112683291 A CN112683291 A CN 112683291A
- Authority
- CN
- China
- Prior art keywords
- position information
- turning
- cost
- calculating
- path
- 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
- 238000000034 method Methods 0.000 title claims abstract description 47
- 238000005070 sampling Methods 0.000 claims abstract description 88
- 230000008859 change Effects 0.000 claims description 37
- 238000004364 calculation method Methods 0.000 claims description 27
- 238000004590 computer program Methods 0.000 claims description 19
- 238000010200 validation analysis Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 11
- 230000008569 process Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000012790 confirmation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
Abstract
The embodiment of the invention provides a method and a device for planning a turning path of a vehicle, the vehicle and a storage medium, wherein the method comprises the following steps: the method comprises the steps of obtaining current vehicle position information of a vehicle and a pre-planned navigation path, determining a sampling range and a turning arc length according to the navigation path, sampling the sampling range to obtain target vehicle position information, calculating a spiral line parameter according to the current vehicle position information, the target vehicle position information and the turning arc length, calculating the turning path according to the spiral line parameter, calculating the cost of the turning path, selecting the turning path with the minimum cost as the target turning path, and being capable of adapting to various complex and large-angle turning environments, for example, rapidly determining the optimal target turning path in parking lots, pedestrian and vehicle mixed traffic districts and the like, turning comfortably and bringing excellent automatic driving experience to users.
Description
Technical Field
The invention relates to the technical field of artificial intelligence, in particular to a vehicle turning path planning method and device, a vehicle and a storage medium.
Background
The automatic driving vehicle path planning generally comprises three methods of sampling, optimizing and searching, wherein the method based on sampling is the method with the highest implementation efficiency, and therefore, the method is the most widely applied to the market.
However, the sampling-based method is not flexible enough in complex environments, such as parking lots, pedestrian and vehicle mixed-driving districts and the like, cannot give consideration to the kinematics and complex driving requirements of vehicles, and cannot be applied to conditions of large-angle turning and the like.
Disclosure of Invention
In view of the above problems, embodiments of the present invention are proposed to provide a vehicle turning path planning method and a corresponding vehicle turning path planning apparatus, vehicle, storage medium that overcome or at least partially solve the above problems.
In order to solve the above problems, an embodiment of the present invention discloses a vehicle turning path planning method, including:
acquiring current vehicle position information of the vehicle and a pre-planned navigation path;
determining a sampling range and a turning arc length according to the navigation path;
sampling the sampling range to obtain the position information of the target vehicle;
calculating spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length;
calculating a turning path according to the spiral line parameters;
and calculating the cost of the turning path, and selecting the turning path with the minimum cost as a target turning path.
Preferably, the determining a sampling range and a turning arc length according to the navigation path includes:
determining the position information of the curve end point in the navigation path;
and determining the sampling range and the turning arc length according to the position information of the curve end point.
Preferably, the determining the sampling range and the turning arc length according to the information of the position of the curve end point includes:
calculating a sampling range according to the position information of the curve end point and a preset parameter;
and calculating the turning arc length according to the current vehicle position information and the position information of the curve end point.
Preferably, the current vehicle position information includes a first coordinate position and a first heading angle, the target vehicle position information includes a second heading angle, the curve ending point position information includes a third coordinate position, the calculating the cost of the turning path, and selecting the turning path with the minimum cost as the target turning path includes:
calculating a second coordinate position of the target vehicle position information according to the spiral line parameter and the turning arc length;
calculating a distance cost according to the first coordinate position, the second coordinate position and the third coordinate position, wherein the distance cost is less than a first preset threshold value;
calculating course angle difference cost according to the first course angle and the second course angle, wherein the course angle difference cost is smaller than a second preset threshold value;
calculating the curvature change cost according to the turning arc length and the spiral line parameters;
and weighting and summing the distance cost, the course angle difference value cost and the curvature change cost to obtain the total cost of the turning path, and selecting the turning path with the minimum total cost as the target turning path.
Preferably, the calculating a spiral parameter according to the current vehicle position information, the target vehicle position information and the turning arc length includes:
and calculating the five spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length.
Preferably, the target vehicle position information includes a second curvature and a second curvature change rate, wherein the second curvature is smaller than a third preset threshold, and the second curvature change rate is smaller than a fourth preset threshold.
The embodiment of the invention discloses a vehicle turning path planning device, which comprises:
the position information acquisition module is used for acquiring the current vehicle position information of the vehicle and a pre-planned navigation path;
the sampling range determining module is used for determining a sampling range and a turning arc length according to the navigation path;
the position information sampling module is used for sampling the sampling range to obtain the position information of the target vehicle;
the spiral line parameter calculation module is used for calculating spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length;
the turning path calculation module is used for calculating a turning path according to the spiral line parameters;
and the target turning path selecting module is used for calculating the cost of the turning path and selecting the turning path with the minimum cost as the target turning path.
Preferably, the sampling range determination module includes:
the position information confirming submodule is used for confirming the position information of the curve end point in the navigation path;
and the sampling range confirming submodule is used for confirming the sampling range and the turning arc length according to the position information of the turning end point.
Preferably, the sampling range confirmation submodule includes:
the sampling range calculating unit is used for calculating a sampling range according to the position information of the curve end point and a preset parameter;
and the curve arc length calculating unit is used for calculating the curve arc length according to the current vehicle position information and the curve end point position information.
Preferably, the current vehicle position information includes a first coordinate position and a first heading angle, the target vehicle position information includes a second heading angle, the curve ending point position information includes a third coordinate position, and the target turning path selecting module includes:
the coordinate position calculation submodule is used for calculating a second coordinate position of the target vehicle position information according to the spiral line parameter and the turning arc length;
the distance cost calculation submodule is used for calculating distance cost according to the first coordinate position, the second coordinate position and the third coordinate position, wherein the distance cost is smaller than a first preset threshold value;
the course angle difference value cost calculation submodule is used for calculating course angle difference value cost according to the first course angle and the second course angle, wherein the course angle difference value cost is smaller than a second preset threshold value;
the curvature change cost calculation submodule is used for calculating the curvature change cost according to the turning arc length and the spiral line parameters;
and the target turning path selecting submodule is used for weighting and summing the distance cost, the course angle difference value cost and the curvature change cost to obtain the total cost of the turning path, and selecting the turning path with the minimum total cost as the target turning path.
Preferably, the spiral parameter calculating module includes:
and the spiral parameter calculating submodule is used for calculating the spiral parameter five times according to the current vehicle position information, the target vehicle position information and the turning arc length.
Preferably, the target vehicle position information includes a second curvature and a second curvature change rate, wherein the second curvature is smaller than a third preset threshold, and the second curvature change rate is smaller than a fourth preset threshold.
The embodiment of the invention discloses a vehicle, which comprises a processor, a memory and a computer program which is stored on the memory and can run on the processor, wherein the computer program realizes the steps of the vehicle turning path planning method when being executed by the processor.
The embodiment of the invention discloses a computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the steps of the vehicle turning path planning method are realized.
The embodiment of the invention has the following advantages:
in the embodiment of the invention, the current vehicle position information of the vehicle and the pre-planned navigation path are obtained, the sampling range and the turning arc length are determined according to the navigation path, the sampling range is sampled to obtain the target vehicle position information, the spiral line parameter is calculated according to the current vehicle position information, the target vehicle position information and the turning arc length, the turning path is calculated according to the spiral line parameter, the cost of the turning path is calculated, and the turning path with the minimum cost is selected as the target turning path. According to the embodiment of the invention, the current vehicle position information is acquired, the position information of a plurality of target vehicles is acquired by sampling from the sampling range, the corresponding spiral line parameters are calculated according to the current vehicle position information and the position information of the plurality of target vehicles, the turning paths corresponding to the position information of the plurality of target vehicles are determined, and the target turning path with the minimum cost is selected from the plurality of turning paths by calculating the cost of the turning path.
Drawings
FIG. 1 is a flow chart of the steps of an embodiment of a method of planning a turn path for a vehicle of the present invention;
FIG. 2 is a flow chart of steps in another embodiment of a method of vehicle turn path planning in accordance with the present invention;
FIG. 3 is a schematic view of a vehicle turn path planning of the present invention;
fig. 4 is a block diagram of a vehicle turning path planning apparatus according to an embodiment of the present invention.
Detailed Description
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
Referring to fig. 1, a flowchart illustrating steps of an embodiment of a method for planning a turning path of a vehicle according to the present invention is shown, and the embodiment of the present invention may specifically include the following steps:
The navigation path is a path from a starting place to a destination planned by a third-party map platform or other global planning methods.
Specifically, in the automatic driving process of the vehicle, when the situation that a curve exists in front of the vehicle is known through a navigation map or a vehicle-mounted sensor, the current vehicle position information of the vehicle at the current moment and a navigation path from a departure place to a destination planned by a third-party map platform or other global planning methods are obtained.
And 102, determining a sampling range and a turning arc length according to the navigation path.
Wherein, the arc length of turning is the arc length of the curve ahead of the vehicle. Specifically, after the vehicle senses that a curve exists in front of the vehicle and acquires the current vehicle position information and the navigation path, a sampling range and a direct turning arc length of the current vehicle position and the curve end point are determined according to the curve end point in the navigation path.
And 103, sampling the sampling range to obtain the position information of the target vehicle.
Specifically, after a sampling range is determined according to a curve end point in the navigation path, sampling is started in the sampling range, and a plurality of pieces of target vehicle position information are obtained.
And 104, calculating spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length.
After the current position information, the turning arc length and the plurality of target vehicle position information obtained through sampling are obtained, five times of spiral line parameters corresponding to the target vehicle position information are calculated through five times of spiral lines according to the current position information, the turning arc length and the target vehicle position information of the vehicle.
And 105, calculating a turning path according to the spiral line parameters.
And after five times of spiral parameters corresponding to the target vehicle position information are obtained through calculation, calculating a turning path corresponding to the target vehicle position information according to the five times of spiral parameters.
And 106, calculating the cost of the turning path, and selecting the turning path with the minimum cost as a target turning path.
After the turning path corresponding to the target vehicle position information is calculated according to the quintic spiral, the cost of the plurality of turning paths is obtained through calculation and compared, the turning path with the minimum cost is selected as the target turning path, and the vehicle is controlled to comfortably pass through the front curve according to the target turning path.
In summary, in the embodiment of the present invention, current vehicle position information of a vehicle and a pre-planned navigation path are obtained, a sampling range and a turning arc length are determined according to the navigation path, the sampling range is sampled to obtain target vehicle position information, a helix parameter is calculated according to the current vehicle position information, the target vehicle position information and the turning arc length, a turning path is calculated according to the helix parameter, a cost of the turning path is calculated, and the turning path with the minimum cost is selected as the target turning path. According to the embodiment of the invention, the current vehicle position information is acquired, the position information of a plurality of target vehicles is acquired by sampling from the sampling range, the corresponding spiral line parameters are calculated according to the current vehicle position information and the position information of the plurality of target vehicles, the turning paths corresponding to the position information of the plurality of target vehicles are determined, and the target turning path with the minimum cost is selected from the plurality of turning paths by calculating the cost of the turning path.
Referring to fig. 2, a flow chart of steps of another embodiment of a method for planning a turning path of a vehicle according to the present invention is shown, and the embodiment of the present invention may specifically include the following steps:
And 102, determining the position information of the curve end point in the navigation path.
Specifically, after the navigation path is acquired, the middle of the curve end position is acquired according to the navigation path and is used as the curve end point, and the information of the curve end point is acquired.
And 103, determining the sampling range and the turning arc length according to the position information of the curve end point.
In an embodiment of the present invention, the determining the sampling range and the turning arc length according to the information of the position of the curve end point includes: calculating a sampling range according to the position information of the curve end point and a preset parameter; and calculating the turning arc length according to the current vehicle position information and the position information of the curve end point.
Wherein the acquired position information of the curve end point comprises a third coordinate position (x)t，yt) A third course angle thetatA third curvature ktAt a third rate of curvature dktThe preset parameters are Δ θ, Δ k, Δ dk.
Specifically, after the information of the position of the end point of the curve is obtained, the third course angle theta in the information of the position of the end point of the curve is obtainedtA third curvature ktAt a third rate of curvature dktCalculating the sampling range theta by the sum of preset parameters delta theta, delta k and delta dkt-Δθ～θt+Δθ，kt-Δk～kt+Δk，dkt-Δdk～dkt+ Δ dk; arc length of turn sfAnd calculating the curve path between the current vehicle position and the curve end point position in the intercepted original navigation path, wherein the curve arc length is the curve length.
And 104, sampling the sampling range to obtain the position information of the target vehicle.
Specifically, the sampling range θ is acquiredt-Δθ～θt+Δθ，kt-Δk～kt+Δk，dkt-Δdk～dktAfter + delta dk, sampling in the sampling range to obtain a second heading angle theta of the target vehicle position information1A second curvature k1And a second rate of change of curvature dk1Wherein, the sampling step length of course angle, curvature and curvature change rate is thetastep，kstep，dkstepThus the total number of samples is
It is noted that the samples are obtainedThe vehicle position information does not include a coordinate position.
And 105, calculating spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length.
Wherein the current vehicle position information includes a first coordinate position (x)0，y0) A first course angle theta0First curvature k0And a first rate of curvature change dk0。
Specifically, according to a first heading angle theta in the current vehicle position information0First curvature k0And a first rate of curvature change dk0Second heading angle theta in target vehicle position information1A second curvature k1And a second rate of change of curvature dk1And the arc length of turning sfWherein s isfThe length s of the turning arc length corresponding to the current vehicle position is the turning arc length corresponding to the target vehicle position0When the two sets of parameters (θ) are equal to 0t，kt，dkt，sf)，(θ0，k0，dk0，s00) into five helices:
θ(s)＝a0+a1s+a2s2+a3s3+a4s4+a5s5
k(s)＝a1+2a2s+3a3s2+4a4s3+5a5s4
dk(s)＝2a2+6a3s+12a4s2+20a5s3
obtaining:
a0＝θ0
a1＝k0
2a2＝dk0
to obtain a0，a1，a2，a3，a4，a5Six spiral parameters.
And 106, calculating a turning path according to the spiral line parameters.
Specifically, a is obtained by calculation0，a1，a2，a3，a4，a5After six spiral parameters, the six spiral parameters and the arbitrary turning arc length s in the curve can be substituted into the following formula:
θ(s)＝a0+a1s+2a2s2+3a3s3+4a4s4+5a5s5
can obtain the arbitrary turning arc length s in the curvefAnd obtaining all coordinate positions in the turning path according to the corresponding coordinate positions.
Referring to fig. 3, a schematic diagram of a vehicle turning path planning according to the present invention is shown, as can be seen, after obtaining current vehicle position information of a vehicle and a plurality of target vehicle position information, a plurality of sampling spirals (turning paths) can be calculated according to the current vehicle position information and the plurality of target vehicle position information, and positions of calculated end points of the plurality of turning paths are near positions of end points of a curve.
And 107, calculating the cost of the turning path, and selecting the turning path with the minimum cost as a target turning path.
In an embodiment of the present invention, the current vehicle position information includes a first coordinate position and a first course angle, the target vehicle position information includes a second course angle, the curve end point position information includes a third coordinate position, the calculating the cost of the turning path, and selecting the turning path with the minimum cost as the target turning path includes: calculating a second coordinate position of the target vehicle position information according to the spiral line parameter and the turning arc length; calculating a distance cost according to the first coordinate position, the second coordinate position and the third coordinate position, wherein the distance cost is less than a first preset threshold value; calculating course angle difference cost according to the first course angle and the second course angle, wherein the course angle difference cost is smaller than a second preset threshold value; calculating the curvature change cost according to the turning arc length and the spiral line parameters; and weighting and summing the distance cost, the course angle difference value cost and the curvature change cost to obtain the total cost of the turning path, and selecting the turning path with the minimum total cost as the target turning path.
Specifically, the second coordinate position of the target vehicle position information may be calculated by substituting the quintic helix parameter and the turning arc length into the following formula:
θ(s)＝a0+a1s+2a2s2+3a3s3+4a4s4+5a5s5
after the second coordinate position of the target vehicle position information is obtained through calculation, calculating the distance cost between the target vehicle position and the curve end point position according to the first coordinate position, the second coordinate position and the third coordinate position, wherein the calculation formula is as follows:
calculating the course angle difference cost according to the first course angle and the second course angle, wherein the calculation formula is as follows:
|θ0-θt|
calculating the curvature change cost according to the turning arc length and the spiral line parameters, wherein the calculation formula is as follows:
after the distance cost, the course angle difference value cost and the curvature change cost are obtained through calculation, the distance cost, the course angle difference value cost and the curvature change cost are subjected to weighted summation to obtain the total cost of a turning path, the turning path with the minimum total cost is selected as a target turning path, and in addition, for the rigid requirement of the turning path, the calculated distance cost needs to be smaller than a first preset threshold value, and the course angle difference value cost needs to be smaller than a second preset threshold value, as follows:
|θ0-θt|＜θmax
in the embodiment of the invention, the cost calculation is carried out on a plurality of turning paths obtained by sampling calculation, and the turning path with the minimum cost is selected as the target turning path, so that the optimal turning path can be quickly selected in a complex road environment, and the vehicle can be controlled to comfortably pass through the curve in front of the vehicle according to the turning path.
In an embodiment of the present invention, the target vehicle position information includes a second curvature and a second curvature change rate, where the second curvature is smaller than a third preset threshold, and the second curvature change rate is smaller than a fourth preset threshold.
Specifically, for the rigid requirement of the turning path, after calculating the second curvature of the target vehicle position information and the second curvature change rate of the person, the second curvature is required to be smaller than a third preset threshold, and the second curvature change rate is smaller than a fourth preset threshold, as follows:
in the embodiment of the invention, the distance cost is required to be less than a first preset threshold value, the course angle difference value cost is less than a second preset threshold value, the second curvature is less than a third preset threshold value, and the second curvature change rate is less than a fourth preset threshold value according to the rigid requirement of the turning path, so that the planned turning path of the vehicle can not deviate from the real curve path, and the vehicle can safely run.
It should be noted that the quintic spiral adopted in the embodiment of the present invention can cover more complicated road conditions than other polynomial curves, and the embodiment of the present invention is not limited to the quintic spiral, and may actually be used for six times, four times, three times, or even other times.
In summary, in the embodiment of the present invention, current vehicle position information of a vehicle and a pre-planned navigation path are obtained, position information of a curve end point is determined in the navigation path, a sampling range and a turning arc length are determined according to the position information of the curve end point, the sampling range is sampled to obtain target vehicle position information, a helix parameter is calculated according to the current vehicle position information, the target vehicle position information and the turning arc length, a turning path is calculated according to the helix parameter, a cost of the turning path is calculated, and the turning path with the minimum cost is selected as the target turning path. According to the embodiment of the invention, the current vehicle position information is acquired, the position information of a plurality of target vehicles is acquired by sampling from the sampling range, the corresponding spiral line parameters are calculated according to the current vehicle position information and the position information of the plurality of target vehicles, the turning paths corresponding to the position information of the plurality of target vehicles are determined, and the target turning path with the minimum cost is selected from the plurality of turning paths by calculating the cost of the turning path.
In the embodiment of the invention, the distance cost is less than the first preset threshold value, the course angle difference value cost is less than the second preset threshold value, the second curvature is less than the third preset threshold value, and the second curvature change rate is less than the fourth preset threshold value according to the rigid requirement of the turning path, so that the planned turning path of the vehicle can not deviate from the real curve path, and the vehicle can safely run.
It should be noted that, for simplicity of description, the method embodiments are described as a series of acts or combination of acts, but those skilled in the art will recognize that the present invention is not limited by the illustrated order of acts, as some steps may occur in other orders or concurrently in accordance with the embodiments of the present invention. Further, those skilled in the art will appreciate that the embodiments described in the specification are presently preferred and that no particular act is required to implement the invention.
Referring to fig. 4, a block diagram of an embodiment of a vehicle turning path planning apparatus according to the present invention is shown, and the embodiment of the present invention may specifically include the following modules:
a position information obtaining module 401, configured to obtain current vehicle position information of the vehicle and a pre-planned navigation path;
a sampling range determining module 402, configured to determine a sampling range and a turning arc length according to the navigation path;
a position information sampling module 403, configured to sample the sampling range to obtain target vehicle position information;
a helix parameter calculating module 404, configured to calculate a helix parameter according to the current vehicle position information, the target vehicle position information, and the turning arc length;
a turning path calculation module 405, configured to calculate a turning path according to the spiral parameter;
and a target turning path selecting module 406, configured to calculate the cost of the turning path, and select the turning path with the minimum cost as a target turning path.
In an embodiment of the present invention, the sampling range determining module 402 includes:
the position information confirming submodule is used for confirming the position information of the curve end point in the navigation path;
and the sampling range confirming submodule is used for confirming the sampling range and the turning arc length according to the position information of the turning end point.
In an embodiment of the present invention, the sampling range determining sub-module includes:
the sampling range calculating unit is used for calculating a sampling range according to the position information of the curve end point and a preset parameter;
and the curve arc length calculating unit is used for calculating the curve arc length according to the current vehicle position information and the curve end point position information.
In an embodiment of the present invention, the current vehicle position information includes a first coordinate position and a first heading angle, the target vehicle position information includes a second heading angle, the curve ending point position information includes a third coordinate position, and the target turning path selecting module 406 includes:
the coordinate position calculation submodule is used for calculating a second coordinate position of the target vehicle position information according to the spiral line parameter and the turning arc length;
the distance cost calculation submodule is used for calculating distance cost according to the first coordinate position, the second coordinate position and the third coordinate position, wherein the distance cost is smaller than a first preset threshold value;
the course angle difference value cost calculation submodule is used for calculating course angle difference value cost according to the first course angle and the second course angle, wherein the course angle difference value cost is smaller than a second preset threshold value;
the curvature change cost calculation submodule is used for calculating the curvature change cost according to the turning arc length and the spiral line parameters;
and the target turning path selecting submodule is used for weighting and summing the distance cost, the course angle difference value cost and the curvature change cost to obtain the total cost of the turning path, and selecting the turning path with the minimum total cost as the target turning path.
In an embodiment of the present invention, the spiral parameter calculating module 404 includes:
and the spiral parameter calculating submodule is used for calculating the spiral parameter five times according to the current vehicle position information, the target vehicle position information and the turning arc length.
In an embodiment of the present invention, the target vehicle position information includes a second curvature and a second curvature change rate, where the second curvature is smaller than a third preset threshold, and the second curvature change rate is smaller than a fourth preset threshold.
In summary, in the embodiment of the present invention, current vehicle position information of a vehicle and a pre-planned navigation path are obtained, a sampling range and a turning arc length are determined according to the navigation path, the sampling range is sampled to obtain target vehicle position information, a helix parameter is calculated according to the current vehicle position information, the target vehicle position information and the turning arc length, a turning path is calculated according to the helix parameter, a cost of the turning path is calculated, and the turning path with the minimum cost is selected as the target turning path. According to the embodiment of the invention, the current vehicle position information is acquired, the position information of a plurality of target vehicles is acquired by sampling from the sampling range, the corresponding spiral line parameters are calculated according to the current vehicle position information and the position information of the plurality of target vehicles, the turning paths corresponding to the position information of the plurality of target vehicles are determined, and the target turning path with the minimum cost is selected from the plurality of turning paths by calculating the cost of the turning path.
For the device embodiment, since it is basically similar to the method embodiment, the description is simple, and for the relevant points, refer to the partial description of the method embodiment.
The embodiment of the invention discloses a vehicle, which comprises a processor, a memory and a computer program which is stored on the memory and can run on the processor, wherein the computer program realizes the steps of the vehicle turning path planning method embodiment when being executed by the processor.
The embodiment of the invention discloses a computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the steps of the embodiment of the vehicle turning path planning method are realized.
The embodiments in the present specification are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other.
As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, apparatus, or computer program product. Accordingly, embodiments of the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, embodiments of the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
Embodiments of the present invention are described with reference to flowchart illustrations and/or block diagrams of methods, terminal devices (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing terminal to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing terminal, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing terminal to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing terminal to cause a series of operational steps to be performed on the computer or other programmable terminal to produce a computer implemented process such that the instructions which execute on the computer or other programmable terminal provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
While preferred embodiments of the present invention have been described, additional variations and modifications of these embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. Therefore, it is intended that the appended claims be interpreted as including preferred embodiments and all such alterations and modifications as fall within the scope of the embodiments of the invention.
Finally, it should also be noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or terminal that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or terminal. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other like elements in a process, method, article, or terminal that comprises the element.
The present invention provides a method for planning a vehicle turning path, a device for planning a vehicle turning path, a vehicle and a storage medium, which are described in detail above, wherein specific examples are applied to illustrate the principle and the implementation of the present invention, and the description of the above examples is only used to help understanding the method and the core idea of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.
Claims (12)
1. A method of vehicle turn path planning, the method comprising:
acquiring current vehicle position information of the vehicle and a pre-planned navigation path;
determining a sampling range and a turning arc length according to the navigation path;
sampling the sampling range to obtain the position information of the target vehicle;
calculating spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length;
calculating a turning path according to the spiral line parameters;
and calculating the cost of the turning path, and selecting the turning path with the minimum cost as a target turning path.
2. The method of claim 1, wherein determining a sampling range and a turn arc length from the navigation path comprises:
determining the position information of the curve end point in the navigation path;
and determining the sampling range and the turning arc length according to the position information of the curve end point.
3. The method of claim 2, wherein the determining the sampling range and the curve arc length from the curve end point location information comprises:
calculating a sampling range according to the position information of the curve end point and a preset parameter;
and calculating the turning arc length according to the current vehicle position information and the position information of the curve end point.
4. The method of claim 2, wherein the current vehicle position information includes a first coordinate position, a first heading angle, the target vehicle position information includes a second heading angle, the curve end point position information includes a third coordinate position, the calculating the cost of the turn path, selecting the turn path with the smallest cost as the target turn path, comprises:
calculating a second coordinate position of the target vehicle position information according to the spiral line parameter and the turning arc length;
calculating a distance cost according to the first coordinate position, the second coordinate position and the third coordinate position, wherein the distance cost is less than a first preset threshold value;
calculating course angle difference cost according to the first course angle and the second course angle, wherein the course angle difference cost is smaller than a second preset threshold value;
calculating the curvature change cost according to the turning arc length and the spiral line parameters;
and weighting and summing the distance cost, the course angle difference value cost and the curvature change cost to obtain the total cost of the turning path, and selecting the turning path with the minimum total cost as the target turning path.
5. The method of claim 1, wherein said calculating a helix parameter from said current vehicle position information, said target vehicle position information, and said turning arc length comprises:
and calculating the five spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length.
6. The method according to claims 1 to 5, wherein the target vehicle position information comprises a second curvature, a second rate of change of curvature, wherein the second curvature is less than a third preset threshold and the second rate of change of curvature is less than a fourth preset threshold.
7. A vehicle turning path planning apparatus, characterized by comprising:
the position information acquisition module is used for acquiring the current vehicle position information of the vehicle and a pre-planned navigation path;
the sampling range determining module is used for determining a sampling range and a turning arc length according to the navigation path;
the position information sampling module is used for sampling the sampling range to obtain the position information of the target vehicle;
the spiral line parameter calculation module is used for calculating spiral line parameters according to the current vehicle position information, the target vehicle position information and the turning arc length;
the turning path calculation module is used for calculating a turning path according to the spiral line parameters;
and the target turning path selecting module is used for calculating the cost of the turning path and selecting the turning path with the minimum cost as the target turning path.
8. The apparatus of claim 7, wherein the sampling range determination module comprises:
the position information confirming submodule is used for confirming the position information of the curve end point in the navigation path;
and the sampling range confirming submodule is used for confirming the sampling range and the turning arc length according to the position information of the turning end point.
9. The apparatus of claim 8, wherein the sampling range validation submodule comprises:
the sampling range calculating unit is used for calculating a sampling range according to the position information of the curve end point and a preset parameter;
and the curve arc length calculating unit is used for calculating the curve arc length according to the current vehicle position information and the curve end point position information.
10. The apparatus of claim 8, wherein the current vehicle position information comprises a first coordinate position, a first heading angle, the target vehicle position information comprises a second heading angle, the curve end point position information comprises a third coordinate position, the target turn path selection module comprises:
the coordinate position calculation submodule is used for calculating a second coordinate position of the target vehicle position information according to the spiral line parameter and the turning arc length;
the distance cost calculation submodule is used for calculating distance cost according to the first coordinate position, the second coordinate position and the third coordinate position, wherein the distance cost is smaller than a first preset threshold value;
the course angle difference value cost calculation submodule is used for calculating course angle difference value cost according to the first course angle and the second course angle, wherein the course angle difference value cost is smaller than a second preset threshold value;
the curvature change cost calculation submodule is used for calculating the curvature change cost according to the turning arc length and the spiral line parameters;
and the target turning path selecting submodule is used for weighting and summing the distance cost, the course angle difference value cost and the curvature change cost to obtain the total cost of the turning path, and selecting the turning path with the minimum total cost as the target turning path.
11. A vehicle comprising a processor, a memory and a computer program stored on and executable on the memory, the computer program when executed by the processor implementing the steps of the vehicle turning path planning method of any one of claims 1 to 6.
12. A computer-readable storage medium, characterized in that a computer program is stored on the computer-readable storage medium, which computer program, when being executed by a processor, carries out the steps of the vehicle turning path planning method according to any one of claims 1 to 6.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011629184.XA CN112683291B (en) | 2020-12-30 | 2020-12-30 | Vehicle turning path planning method and device, vehicle and storage medium |
| PCT/CN2021/141754 WO2022143560A1 (en) | 2020-12-30 | 2021-12-27 | Vehicle turning path planning method and apparatus, vehicle, and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011629184.XA CN112683291B (en) | 2020-12-30 | 2020-12-30 | Vehicle turning path planning method and device, vehicle and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112683291A true CN112683291A (en) | 2021-04-20 |
| CN112683291B CN112683291B (en) | 2023-03-31 |
Family
ID=75455881
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011629184.XA Active CN112683291B (en) | 2020-12-30 | 2020-12-30 | Vehicle turning path planning method and device, vehicle and storage medium |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN112683291B (en) |
| WO (1) | WO2022143560A1 (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114001739A (en) * | 2021-10-11 | 2022-02-01 | 广州小鹏自动驾驶科技有限公司 | Path planning method and device, vehicle and storage medium |
| CN114111788A (en) * | 2021-11-09 | 2022-03-01 | 武汉乐庭软件技术有限公司 | Trajectory planning method and device based on multi-segment clothoid and storage device |
| WO2022143560A1 (en) * | 2020-12-30 | 2022-07-07 | 广州小鹏自动驾驶科技有限公司 | Vehicle turning path planning method and apparatus, vehicle, and storage medium |
| CN115200604A (en) * | 2022-09-16 | 2022-10-18 | 广州小鹏自动驾驶科技有限公司 | Turning path planning method, device, vehicle and storage medium |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115123217B (en) * | 2022-09-02 | 2022-11-25 | 青岛慧拓智能机器有限公司 | Mine obstacle vehicle driving track generation method and device and computer equipment |
| CN116797004B (en) * | 2023-08-25 | 2023-12-08 | 深圳市金泰谊电子有限公司 | Industrial traffic management method, device, equipment and medium based on 5G communication |
| CN117555340B (en) * | 2024-01-12 | 2024-04-09 | 北京集度科技有限公司 | Path planning method and related device |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104303123A (en) * | 2012-05-11 | 2015-01-21 | 天宝导航有限公司 | Path planning autopilot |
| US9816822B1 (en) * | 2016-09-30 | 2017-11-14 | X Development Llc | Assisted roadmap generation |
| CN107662609A (en) * | 2016-07-29 | 2018-02-06 | 长城汽车股份有限公司 | Control method, system and the vehicle of turn inside diameter |
| CN108082185A (en) * | 2017-03-30 | 2018-05-29 | 长城汽车股份有限公司 | A kind of travel control method of vehicle, device and vehicle |
| CN110262508A (en) * | 2019-07-06 | 2019-09-20 | 深圳数翔科技有限公司 | Applied to the automated induction systems and method on the closing unmanned goods stock in place |
| CN110825078A (en) * | 2019-10-10 | 2020-02-21 | 江苏大学 | Ground turning path control system of autonomous navigation tracked vehicle |
| CN111552296A (en) * | 2020-05-14 | 2020-08-18 | 宁波智能装备研究院有限公司 | Local smooth track planning method based on curved cylindrical coordinate system |
| CN112078594A (en) * | 2020-08-31 | 2020-12-15 | 纵目科技（上海）股份有限公司 | Curvature continuous parking path planning device and method for intelligent parking system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112683291B (en) * | 2020-12-30 | 2023-03-31 | 广州小鹏自动驾驶科技有限公司 | Vehicle turning path planning method and device, vehicle and storage medium |
-
2020
- 2020-12-30 CN CN202011629184.XA patent/CN112683291B/en active Active
-
2021
- 2021-12-27 WO PCT/CN2021/141754 patent/WO2022143560A1/en active Application Filing
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104303123A (en) * | 2012-05-11 | 2015-01-21 | 天宝导航有限公司 | Path planning autopilot |
| CN107662609A (en) * | 2016-07-29 | 2018-02-06 | 长城汽车股份有限公司 | Control method, system and the vehicle of turn inside diameter |
| US9816822B1 (en) * | 2016-09-30 | 2017-11-14 | X Development Llc | Assisted roadmap generation |
| CN108082185A (en) * | 2017-03-30 | 2018-05-29 | 长城汽车股份有限公司 | A kind of travel control method of vehicle, device and vehicle |
| CN110262508A (en) * | 2019-07-06 | 2019-09-20 | 深圳数翔科技有限公司 | Applied to the automated induction systems and method on the closing unmanned goods stock in place |
| CN110825078A (en) * | 2019-10-10 | 2020-02-21 | 江苏大学 | Ground turning path control system of autonomous navigation tracked vehicle |
| CN111552296A (en) * | 2020-05-14 | 2020-08-18 | 宁波智能装备研究院有限公司 | Local smooth track planning method based on curved cylindrical coordinate system |
| CN112078594A (en) * | 2020-08-31 | 2020-12-15 | 纵目科技（上海）股份有限公司 | Curvature continuous parking path planning device and method for intelligent parking system |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022143560A1 (en) * | 2020-12-30 | 2022-07-07 | 广州小鹏自动驾驶科技有限公司 | Vehicle turning path planning method and apparatus, vehicle, and storage medium |
| CN114001739A (en) * | 2021-10-11 | 2022-02-01 | 广州小鹏自动驾驶科技有限公司 | Path planning method and device, vehicle and storage medium |
| CN114001739B (en) * | 2021-10-11 | 2024-04-16 | 广州小鹏自动驾驶科技有限公司 | Path planning method, device, vehicle and storage medium |
| CN114111788A (en) * | 2021-11-09 | 2022-03-01 | 武汉乐庭软件技术有限公司 | Trajectory planning method and device based on multi-segment clothoid and storage device |
| CN115200604A (en) * | 2022-09-16 | 2022-10-18 | 广州小鹏自动驾驶科技有限公司 | Turning path planning method, device, vehicle and storage medium |
| WO2024056064A1 (en) * | 2022-09-16 | 2024-03-21 | 广州小鹏自动驾驶科技有限公司 | Turning path planning method, device, vehicle, and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2022143560A1 (en) | 2022-07-07 |
| CN112683291B (en) | 2023-03-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112683291B (en) | Vehicle turning path planning method and device, vehicle and storage medium | |
| CN108995657B (en) | Method for operating an autonomous vehicle and data processing system | |
| CN110728014B (en) | Reference line smoothing method using segmented spiral curve with weighted geometric cost | |
| WO2021063228A1 (en) | Dashed lane line detection method and device, and electronic apparatus | |
| JP6055821B2 (en) | Combined radar and GPS location system | |
| CN106891893B (en) | Vehicle mode determination | |
| JP4293751B2 (en) | Boeing coefficient display for curvature of geographic form | |
| CN108279016B (en) | Smoothing processing method and device for HAD map, navigation system and automatic driving system | |
| CN111824130A (en) | Parking control method and device, vehicle and storage medium | |
| CN113286983A (en) | Reference line smoothing method based on spline curve and spiral curve | |
| JP6643370B2 (en) | Driving reference line determination device and automatic driving device | |
| WO2004031691A1 (en) | Estimation of vehicle forward path | |
| CN110789531B (en) | Vehicle bending speed determining method and device, electronic equipment and vehicle | |
| CN112020686B (en) | QP spline path and spiral path based reference line smoothing method for autopilot | |
| CN109947101B (en) | Path smoothing processing method and device | |
| CN111427260A (en) | Control method, device, controller and system for vehicle path tracking and vehicle | |
| CN108340915A (en) | Controller of vehicle | |
| CN112229411B (en) | Data processing method and device | |
| US20200133303A1 (en) | Map information system | |
| CN112161628B (en) | Path data processing method and device, vehicle and readable medium | |
| CN114202912B (en) | Traffic service providing method, device, server and storage medium | |
| CN113177665B (en) | Method and terminal for improving tracking route precision | |
| CN115098821A (en) | Trajectory reference curvature determination method, apparatus, device, medium, and program | |
| CN112644487B (en) | Automatic driving method and device | |
| CN112650975B (en) | Enumeration based failure prevention QP smoother for autonomous vehicles |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20240226 Address after: 510000 No.8 Songgang street, Cencun, Tianhe District, Guangzhou City, Guangdong Province Patentee after: GUANGZHOU XIAOPENG MOTORS TECHNOLOGY Co.,Ltd. Country or region after: China Address before: Room 46, room 406, No. 1, Yichuang street, Zhongxin knowledge city, Huangpu District, Guangzhou, Guangdong 510725 Patentee before: Guangzhou Xiaopeng Automatic Driving Technology Co.,Ltd. Country or region before: China |