CN116136417B - Unmanned vehicle local path planning method facing off-road environment - Google Patents
Unmanned vehicle local path planning method facing off-road environment Download PDFInfo
- Publication number
- CN116136417B CN116136417B CN202310395181.1A CN202310395181A CN116136417B CN 116136417 B CN116136417 B CN 116136417B CN 202310395181 A CN202310395181 A CN 202310395181A CN 116136417 B CN116136417 B CN 116136417B
- Authority
- CN
- China
- Prior art keywords
- path
- sampling
- longitudinal
- stage
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000005070 sampling Methods 0.000 claims abstract description 192
- 230000003068 static effect Effects 0.000 claims abstract description 31
- 230000009194 climbing Effects 0.000 claims abstract description 15
- 230000004888 barrier function Effects 0.000 claims description 11
- 239000002689 soil Substances 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 6
- 230000006872 improvement Effects 0.000 description 9
- 239000013598 vector Substances 0.000 description 8
- 230000011218 segmentation Effects 0.000 description 5
- 238000005452 bending Methods 0.000 description 4
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 244000025254 Cannabis sativa Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application relates to an unmanned vehicle local path planning method facing an off-road environment, belongs to the technical field of path planning, and solves the problem that the path feasibility is low due to the fact that the terrain characteristic is ignored in the existing local path planning. Based on a layered map, according to curvature and unit climbing height, longitudinal sampling points of a global reference path are obtained, and a plurality of longitudinal sampling stages are obtained; generating a plurality of transverse sampling points for the longitudinal sampling points of the stage end point according to the length of the longitudinal sampling stage and the preset transverse offset; generating a candidate path of each transverse sampling point from a stage starting point to a stage ending point in each longitudinal sampling stage in turn; taking a candidate path with the minimum total cost value and meeting the constraint conditions of no collision and static stability of the plane as an optimal path, and taking a corresponding transverse sampling point as a stage starting point of the next longitudinal sampling stage; and connecting the optimal paths of each longitudinal sampling stage to obtain a complete local path. The safety and stability of the path are realized.
Description
Technical Field
The application relates to the technical field of path planning, in particular to an unmanned vehicle local path planning method facing an off-road environment.
Background
The path planning is used as a core module of the ground unmanned platform, and forms a navigation module of the ground unmanned platform together with the perception, the positioning and the image construction. In order to ensure the working performance of the unmanned vehicle in an off-road environment, it is very important to comprehensively consider environmental constraints and vehicle characteristics and generate a collision-free, trackable, safe and stable path from the current vehicle position to a specified target position by a vehicle control algorithm.
Most of the existing path planning methods based on searching, sampling and optimizing are concentrated on the urban environment with good structured roads or pavement conditions, and are not suitable for off-road environments. Due to the special terrain characteristics in off-road environments, it is more important to ensure the feasibility of the path and stable driving of the vehicle.
The conventional path planning method for the unmanned vehicle is based on a planar binary grid map, only the constraint of obstacles is considered, the minimum path length or time is pursued while obstacle avoidance is performed, the influence of vehicle characteristics, the terrain height in the environment and the pavement semantic information on path planning is ignored, and the feasibility of the planned path is difficult to guarantee.
Disclosure of Invention
In view of the above analysis, the embodiment of the application aims to provide a local path planning method of an unmanned vehicle facing an off-road environment, which is used for solving the problem that the path feasibility is not high due to the fact that the terrain characteristic is ignored in the existing local path planning.
The embodiment of the application provides an unmanned vehicle local path planning method facing an off-road environment, which comprises the following steps:
based on the layered map, according to the curvature and the unit climbing height, longitudinal sampling points of the global reference path are obtained, and a plurality of continuous longitudinal sampling stages are obtained; generating a plurality of transverse sampling points for the longitudinal sampling points of each stage end point according to the length of each longitudinal sampling stage and the preset transverse offset;
generating a candidate path of each transverse sampling point corresponding to the longitudinal sampling point from the start point of the phase to the end point of the phase in each longitudinal sampling phase in turn; calculating the total cost value of the candidate paths, taking the candidate path with the minimum total cost value and meeting the constraint conditions of no collision and static stability of the plane as an optimal path, and taking the corresponding transverse sampling point as a stage starting point of the next longitudinal sampling stage; and connecting the optimal paths of each longitudinal sampling stage to obtain a complete local path.
Based on further improvement of the method, the layered map is obtained by modeling by adopting a layered planning map model according to image data and radar point cloud data of an off-road environment, and comprises an obstacle layer, a height layer and a pavement semantic layer.
Based on the further improvement of the method, based on the hierarchical map, according to the curvature and the unit climbing height, the longitudinal sampling points of the global reference path are obtained, and the method comprises the following steps:
calculating the curvature of each path node on the global reference path on the barrier layer, and acquiring a plurality of first sampling points according to sampling step sizes corresponding to the curvature change range;
projecting a plurality of first sampling points to a height layer, calculating the unit climbing height between every two adjacent three-dimensional first sampling points, and inserting a second sampling point between the two three-dimensional first sampling points with the unit climbing height being greater than a height threshold value;
and combining the first sampling point and the second sampling point to obtain a longitudinal sampling point of the global reference path.
Based on a further improvement of the above method, the number of transverse sampling points is dynamically determined according to the length of each longitudinal sampling stage and the preset sampling length range by the following formula:
，
wherein ,represent the firstjThe number of transverse sampling points corresponding to the longitudinal sampling points,representing a preset sampling length range, a and B representing preset fixed values, + a>Indicating that the end of the phase is the firstjThe length of the longitudinal sampling phase of the longitudinal sampling points.
Based on the further improvement of the method, generating a candidate path from a stage starting point to each transverse sampling point of the longitudinal sampling stage in each longitudinal sampling stage, and solving the curvature by using a nonlinear parameter optimization method based on a cubic spiral line; and substituting curvature based on the tracked vehicle kinematic model to generate candidate paths under a Cartesian coordinate system.
Based on the further improvement of the method, the total cost value of the candidate path is obtained by multiplying the static stability cost of the height layer and the pavement semantic cost of the pavement semantic layer with corresponding weights respectively according to the transverse deviation cost of the candidate path in the barrier layer and summing the multiplied static stability cost and the pavement semantic cost.
Based on further improvement of the method, the transverse deviation cost of the barrier layer is the distance between the transverse sampling point corresponding to the candidate path and the longitudinal sampling point in the same longitudinal sampling stage.
Based on the further improvement of the method, the static stability cost of the height layer is obtained by discretizing the candidate path into a plurality of path nodes, calculating the static stability cost of each path node through the following formula, and then accumulating and summing the calculated static stability costs:
，
wherein ,representing discretized firstiIndividual path nodes->Represent the firstiThe path nodei-1 absolute pitch angle difference between path nodes, < >>Represent the firstiThe path nodei-absolute value of the roll angle difference between 1 path nodes +.>Represent the firstiThe path nodei-1 unit climb height between path nodes.
Based on the further improvement of the method, the pavement semantic cost of the pavement semantic layer is obtained by discretizing the candidate paths into a plurality of path nodes, calculating the pavement semantic cost of each path node through the following formula, and then accumulating and summing the pavement semantic cost:
wherein ,、/> and />Respectively expressed in discretizationiThe proportion of the soil road, grassland and shrub road types at each path node is->、/> and />Respectively representing weights corresponding to the soil road, the grassland and the shrub road types.
Based on a further improvement of the method, the constraint of planar collision-free is that the closest distance of the path node of the candidate path from the obstacle is greater than the radius of the unmanned vehicle; the constraint of static stability is that the pitch angle and roll angle of the unmanned vehicle at the path node of the candidate path are both less than the corresponding angle thresholds.
Compared with the prior art, the application has at least one of the following beneficial effects:
1. introducing a self-adaptive longitudinal sampling strategy to the global reference path, wherein the longitudinal sampling strategy based on curvature and height is more reasonable in segmentation and is more suitable for road characteristics in an off-road environment; the self-adaptive transverse sampling strategy is introduced, the number of transverse sampling points is determined based on the path length, the generation of candidate paths with overlarge bending energy is avoided, and the calculated amount is reduced;
2. the planned local path may satisfy both collision-free constraints, static stability constraints and vehicle kinematics constraints.
In the application, the technical schemes can be mutually combined to realize more preferable combination schemes. Additional features and advantages of the application will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the application. The objectives and other advantages of the application may be realized and attained by the structure particularly pointed out in the written description and drawings.
Drawings
The drawings are only for purposes of illustrating particular embodiments and are not to be construed as limiting the application, like reference numerals being used to designate like parts throughout the drawings;
FIG. 1 is a flow chart of a method for planning a local path of an unmanned vehicle facing an off-road environment in an embodiment of the application;
FIG. 2 is a schematic diagram of longitudinal sampling in an embodiment of the present application;
FIG. 3 is a schematic diagram of candidate paths generated during a vertical sampling phase in an embodiment of the present application.
Detailed Description
The following detailed description of preferred embodiments of the application is made in connection with the accompanying drawings, which form a part hereof, and together with the description of the embodiments of the application, are used to explain the principles of the application and are not intended to limit the scope of the application.
The application discloses a local path planning method of an unmanned vehicle facing an off-road environment, which is shown in fig. 1 and comprises the following steps:
s11, based on a layered map, according to curvature and unit climbing height, longitudinal sampling points of a global reference path are obtained, and a plurality of continuous longitudinal sampling stages are obtained; generating a plurality of transverse sampling points for the longitudinal sampling points of each stage end point according to the length of each longitudinal sampling stage and the preset transverse offset;
s12, sequentially generating candidate paths of each transverse sampling point corresponding to the longitudinal sampling points from the start point to the end point of each longitudinal sampling stage; calculating the total cost value of the candidate paths, taking the candidate path with the minimum total cost value and meeting the constraint conditions of no collision and static stability of the plane as an optimal path, and taking the corresponding transverse sampling point as a stage starting point of the next longitudinal sampling stage; and connecting the optimal paths of each longitudinal sampling stage to obtain a complete local path.
In implementation, the global reference path is divided into several successive stages by adopting an adaptive longitudinal sampling strategy. And in each longitudinal sampling stage, generating a plurality of feasible end point states by using a self-adaptive transverse sampling strategy, solving a plurality of two-point boundary value problems reaching the end point of each sampling stage by using a nonlinear parameter optimization method based on a three-time spiral line, and generating a plurality of candidate paths which are transversely deviated from a reference path to a certain extent. And in each longitudinal sampling stage, calculating the cost value of each candidate path, adopting a greedy search strategy, selecting the candidate path with the minimum cost value, and finally planning a local optimal path which is consistent with the reference path as much as possible, smooth and collision-free and meets the vehicle kinematic constraint and the vehicle static stability.
It should be noted that, in the off-road environment in which the present embodiment faces, the road surface has an uneven road, and the attribute difference of the road surface is obvious, the environment complexity is high, and an off-road field of the Hebei mountain village is selected as an example. The unmanned vehicle is an unmanned crawler platform, industrial cameras are arranged around the vehicle body, and a laser radar, a global positioning system (GPS, global Positioning System) and an inertial navigation system (IMU, inertial Measurement Unit) are arranged on the vehicle roof. And 3 industrial personal computers are adopted as computing equipment, one of the industrial personal computers is provided with RTX 2080Ti as an image semantic segmentation processing industrial personal computer, the acquired off-road annular image data is processed, one of the industrial personal computers performs rasterization processing on the acquired laser radar point cloud, the image semantic segmentation result is matched, a layered map is generated, and the other industrial personal computer processes a motion planning control program to control the running of the vehicle.
The layered map is obtained by modeling through a layered planning map model according to image data and radar point cloud data of an off-road environment, and comprises an obstacle layer, a height layer and a pavement semantic layer.
Specifically, according to the acquired image data of the off-road environment, extracting image features by adopting a double-channel convolutional neural network, classifying image pixel levels, and identifying soil paths, grasslands, shrubs and unknown types in the off-road environment to obtain image semantics; assigning image semantics to the collected radar point cloud data based on the sensor coordinate system conversion relation to obtain semantic point cloud; constructing a pavement semantic layer map according to the semantic point cloud;
acquiring the point cloud height based on Lei Dadian cloud data, and constructing a height layer map; and carrying out binary modeling according to the relative height difference threshold, and constructing an obstacle layer map when the height difference between the point clouds is higher than the set relative height difference threshold, wherein the obstacle is an obstacle, and otherwise the obstacle is a non-obstacle.
Each layer of map adopts a storage form of occupying grid map, and each grid can be rapidly indexed through two-dimensional coordinates.
In the obtained hierarchical map, if the pixel value in the barrier layer is 0, the barrier is indicated, otherwise, the barrier is indicated; each pixel value in the height layer represents the terrain height in the actual environment, and the larger the pixel value is, the larger the height is, the smaller the pixel value is, and the smaller the height is; each pixel value in the pavement semantic layer represents the pavement type corresponding to the pavement in the actual environment, and the pavement type comprises soil roads, grasslands, shrubs and unknown types.
Further, the embodiment obtains the global reference path according to the starting point, the ending point and the topology road network of the path planning. In step S11, based on the hierarchical map, a longitudinal sampling point of the global reference path is acquired according to the curvature and the unit ascent height, including:
(1) and calculating the curvature of each path node on the global reference path at the barrier layer, and acquiring a plurality of first sampling points according to the sampling step length corresponding to the curvature change range.
It should be noted that the first sampling point includes a start point and an end point of the global reference path. Where the curvature is smaller, a larger sampling step is used, and where the curvature is larger, a smaller sampling step is used to accommodate paths with larger curvature fluctuations. Illustratively, if the rate of change of the first derivative of curvature is greater than 0.005, then the curvature is considered to be changing significantly, sampling is performed according to a preset small sampling step, the sampling step having a lower limit of 4 meters; if the rate of change of the first derivative of curvature is 0.005 or less, the curvature is considered to be less changed, and sampling is performed according to a preset large sampling step.
(2) Projecting a plurality of first sampling points to a height layer, calculating the unit climbing height between every two adjacent three-dimensional first sampling points, and inserting a second sampling point between the two three-dimensional first sampling points with the unit climbing height being greater than a height threshold value.
It should be noted that the unit climbing height is calculated by the following formula:
，
wherein ,represent the firstkAnd (b)k-1 unit ascent height between the first three-dimensional sampling points, the corresponding three-dimensional coordinates being respectively: /> and />。
And inserting a second sampling point between two three-dimensional first sampling points with the unit climbing height being greater than the height threshold value through a binary interpolation method.
(3) And combining the first sampling point and the second sampling point to obtain a longitudinal sampling point of the global reference path.
It should be noted that, each longitudinal sampling point includes a node position coordinate, a heading angle, a pitch angle, and a roll angle.
When the vehicle is stationary at a certain sampling point in a certain posture, the projection of the vehicle on a height layer is approximately a rectangle, the mean value and covariance matrix of data points in the projection area are calculated, the covariance matrix is subjected to feature decomposition, the feature vector corresponding to the minimum feature value is the curved surface normal vector at the position of the vehicle, and the Z-axis unit normal vector can be obtained after normalization. Calculating a unit direction vector along the X axis of the vehicle according to the heading angle of the vehicle and the unit normal vector of the Z axis, and calculating a unit direction vector of the Y axis according to the right hand rule; and calculating the pitch angle and the roll angle of the vehicle at the sampling point according to the X-axis and Y-axis unit direction vectors and the Z-axis unit normal vector.
As shown in fig. 2, every two adjacent longitudinal sampling points (including a start point and an end point) divide the global reference path into one longitudinal sampling phase, so as to obtain a plurality of continuous longitudinal sampling phases.
Further, since the length of the longitudinal sampling stage is changed, the number of the transverse sampling points is dynamically adjusted along with the change of the length of the longitudinal sampling stage, when the length of the longitudinal sampling stage is larger, the number of the transverse sampling points is increased, otherwise, the number of the transverse sampling points is reduced, and therefore generation of candidate paths with larger bending energy is avoided.
It should be noted that, for the longitudinal sampling point of each stage end point, a plurality of transverse sampling points are generated, that is, other longitudinal sampling points (including the end point) except for the start point correspond to a plurality of transverse sampling points. According to the length of each longitudinal sampling stage and the preset sampling length range, the number of the transverse sampling points corresponding to each longitudinal sampling point is dynamically determined through the following formula:
，
wherein ,represent the firstjThe number of transverse sampling points corresponding to the longitudinal sampling points,representing a preset sampling length range, a and B representing preset fixed values, + a>Indicating that the end of the phase is the firstjThe length of the longitudinal sampling phase of the longitudinal sampling points. Illustratively, the preset sample length range is [4,10]A is 8 and B is 13.
And according to the number of the transverse sampling points corresponding to each longitudinal sampling point, taking the longitudinal sampling points of each longitudinal sampling stage as initial transverse sampling points, and expanding the initial transverse sampling points to two sides along the normal direction of the heading of the initial transverse sampling points at intervals by taking preset transverse deflection quantity as intervals to generate transverse sampling points. I.e. the longitudinal sampling point of the phase end of each longitudinal sampling phase is also taken as the transverse sampling point of the longitudinal sampling phase.
Preferably, in the process of transverse sampling, based on constraint conditions of plane collision-free and static stability, plane collision detection and static stability detection are carried out on transverse sampling points, and sampling points which do not meet the constraint conditions can be deleted in advance, so that redundant calculation is avoided.
It should be noted that, the constraint condition of no collision on the plane is that the nearest distance between the path node and the obstacle is larger than the radius of the unmanned vehicle; the constraint of static stability is that the pitch angle and roll angle of the unmanned vehicle at the path node are both less than the corresponding angle thresholds. Illustratively, the pitch angle corresponds to an angle threshold of 20 degrees and the roll angle corresponds to an angle threshold of 15 degrees.
Compared with the prior art, the self-adaptive longitudinal sampling strategy is introduced in the step S11, and the segmentation is more reasonable based on the curvature and the longitudinal sampling strategy based on the height, so that the method is more suitable for the road characteristics in the off-road environment; and an adaptive transverse sampling strategy is introduced, the number of transverse sampling points is determined based on the path length, the generation of candidate paths with overlarge bending energy is avoided, and the calculated amount is reduced.
As shown in fig. 3, in step S12, the start point of the global reference path is used as the start point of the first longitudinal sampling stage, the next longitudinal sampling point of the start point is the longitudinal sampling point of the stage end point of the first longitudinal sampling stage, a plurality of corresponding transverse sampling points are used as the plurality of stage end points of the longitudinal sampling stage, candidate paths from the start point of the stage to each stage end point are respectively generated, a candidate path with the minimum total cost value and meeting the constraint conditions of no collision and static stability of the plane is selected as the optimal path of the longitudinal sampling stage, the transverse sampling point corresponding to the optimal path is used as the stage start point of the next longitudinal sampling stage, and the steps are sequentially analogized until the last longitudinal sampling stage; and connecting the optimal paths of each longitudinal sampling stage to obtain a complete local path.
That is, the stage start point of the first vertical sampling stage is the start point of the global reference path, and the stage start points of the other vertical sampling stages are one horizontal sampling point corresponding to the optimal path selected from the plurality of horizontal sampling points of the last stage end point based on the greedy search strategy.
The three-time spiral line can simultaneously meet the position, heading and curvature constraint in the boundary constraint, and simultaneously keeps smaller parameter space dimension, so that the curvature is solved by using a nonlinear parameter optimization method based on the three-time spiral line; and substituting curvature based on the tracked vehicle kinematic model to generate candidate paths under a Cartesian coordinate system.
Specifically, the objective function of the nonlinear parameter optimization method based on the cubic spiral is expressed by the following formula:
，
wherein ,representing curvature->Parameter-> and />Respectively expressed in->Andcurvature at; />Then +.>Correspondingly (I)>Boundary conditions representing the start of a phase, +.>For the boundary conditions of the phase end, the x-coordinate, y-coordinate, heading and curvature, respectively, +.> and />Respectively representing the lower and upper bounds of the preset curve arc length,/for> and />Representing the lower and upper bounds of the curve curvature, respectively.
Solving for curvature using a non-linear optimization solver, e.g. Ceres solver. Substituting the arc length parameterized expression form of the tracked vehicle kinematic model shown below to obtain a candidate path under a Cartesian coordinate system:
，
it should be noted that, in this embodiment, a cost function is established according to a hierarchical map, and the total cost value of the candidate paths is obtained by multiplying the static stability cost of the height layer and the pavement semantic cost of the pavement semantic layer with corresponding weights respectively according to the lateral deviation cost of the candidate paths in the barrier layer, and the formula is as follows:
，
wherein ,representing the total cost value of the r-th candidate path, < >>Represents the lateral deviation cost of the r-th candidate path,/->Static stability cost representing the r-th candidate path,/->Representing the pavement semantic cost of the r candidate path; />、/> and />The weights corresponding to the three costs are respectively. The smaller the weight, the more emphasis is placed on the corresponding item. For example, set +.>At a minimum, it is indicated that the desired candidate path is closer to the reference path.
Specifically, the lateral deviation cost of the barrier layer, that is, the lateral deviation cost from the global reference path, is taken to be the distance from the lateral sampling point corresponding to the candidate path to the longitudinal sampling point in the same longitudinal sampling stage.
The static stability cost of the height layer and the pavement semantic cost of the pavement semantic layer are obtained by discretizing the candidate paths into a plurality of path nodes, calculating the static stability cost and the pavement semantic cost of each path node through a formula (6) and a formula (7), and accumulating and summing the static stability cost and the pavement semantic cost of the candidate paths:
，
wherein ,representing the discretized i-th path node, a ∈>Represents the absolute value of the pitch angle difference between the i-th path node and the i-1 th path node,/->Represents the absolute value of the roll angle difference between the i-th path node and the i-1 th path node,/and->Representing the unit climbing height between the ith path node and the (i-1) th path node, and calculating according to the formula (1);
，
wherein ,、/> and />Respectively representing the proportion of three road surface types of soil, grass and shrub calculated at the discretized ith path node according to the occupied area of the unmanned vehicle, < ->、/>Andrespectively representing weights corresponding to three road surface types of soil road, grassland and shrub; />Indicating that the vehicle is expected to walk more soil, less grasslands and avoid walking the shrub pavement.
It should be noted that, there are many ways to discretize the candidate path, for example, according to the curvature size and according to a fixed step, and the present embodiment discretizes the candidate path into a plurality of path nodes according to a fixed step.
Further, taking the candidate path with the minimum total cost value and meeting the constraint conditions of no collision and static stability of the plane as the optimal path comprises the following steps: after the total cost value of the candidate paths is calculated, sorting from small to large according to the total cost value, starting from the candidate path with the minimum total cost value, judging whether each discretized path node meets the constraint conditions of plane collision-free and static stability, if any path node which does not meet the constraint conditions exists, taking the next candidate path for judgment until the discrete path node meets the constraint conditions of plane collision-free and static stability, and taking the candidate path as the optimal path.
And finally, connecting the optimal paths of each longitudinal sampling stage, and planning a three-dimensional local path which is consistent with the global reference path as much as possible, has no collision and meets the kinematic constraint and static stability of the vehicle.
Compared with the prior art, the unmanned vehicle local path planning method for the off-road environment, provided by the embodiment, introduces a self-adaptive longitudinal sampling strategy to an initial global reference path, and is more reasonable in segmentation based on the curvature and the longitudinal sampling strategy based on the height, and more suitable for road characteristics in the off-road environment; the self-adaptive transverse sampling strategy is introduced, the number of transverse sampling points is determined based on the path length, the generation of candidate paths with overlarge bending energy is avoided, and the calculated amount is reduced; the planned local path may satisfy both collision-free constraints, static stability constraints and vehicle kinematics constraints.
Those skilled in the art will appreciate that all or part of the flow of the methods of the embodiments described above may be accomplished by way of a computer program to instruct associated hardware, where the program may be stored on a computer readable storage medium. Wherein the computer readable storage medium is a magnetic disk, an optical disk, a read-only memory or a random access memory, etc.
The present application is not limited to the above-mentioned embodiments, and any changes or substitutions that can be easily understood by those skilled in the art within the technical scope of the present application are intended to be included in the scope of the present application.
Claims (9)
1. The unmanned vehicle local path planning method facing the off-road environment is characterized by comprising the following steps of:
based on the layered map, according to the curvature and the unit climbing height, longitudinal sampling points of the global reference path are obtained, and a plurality of continuous longitudinal sampling stages are obtained; generating a plurality of transverse sampling points for the longitudinal sampling points of each stage end point according to the length of each longitudinal sampling stage and the preset transverse offset;
generating a candidate path of each transverse sampling point corresponding to the longitudinal sampling point from the start point of the phase to the end point of the phase in each longitudinal sampling phase in turn; calculating the total cost value of the candidate paths, taking the candidate path with the minimum total cost value and meeting the constraint conditions of no collision and static stability of the plane as an optimal path, and taking the corresponding transverse sampling point as a stage starting point of the next longitudinal sampling stage; connecting the optimal paths of each longitudinal sampling stage to obtain a complete local path;
based on the hierarchical map, according to curvature and unit climbing height, the longitudinal sampling point of the global reference path is obtained, and the method comprises the following steps:
calculating the curvature of each path node on a global reference path on an obstacle layer of the layered map, and acquiring a plurality of first sampling points according to sampling step sizes corresponding to curvature change ranges;
projecting a plurality of first sampling points to a height layer of the layered map, calculating the unit climbing height between every two adjacent three-dimensional first sampling points, and inserting a second sampling point between the two three-dimensional first sampling points with the unit climbing height being greater than a height threshold;
combining the first sampling point and the second sampling point to obtain a longitudinal sampling point of the global reference path;
the number of the transverse sampling points is dynamically determined according to the length of each longitudinal sampling stage and a preset sampling length range.
2. The method for planning the local path of the unmanned vehicle facing the off-road environment according to claim 1, wherein the layered map is obtained by modeling by adopting a layered planning map model according to image data and radar point cloud data of the off-road environment, and comprises an obstacle layer, a height layer and a pavement semantic layer.
3. The method for planning a local path of an unmanned vehicle facing an off-road environment according to claim 1, wherein the number of the lateral sampling points is calculated by the following formula:
，
wherein ,represent the firstjThe number of transverse sampling points corresponding to the longitudinal sampling points,/->Representing a preset sampling length range, a and B representing preset fixed values, + a>Indicating that the end of the phase is the firstjThe length of the longitudinal sampling phase of the longitudinal sampling points.
4. The method for planning a local path of an unmanned vehicle facing an off-road environment according to claim 1, wherein the step of generating a candidate path from a stage start point to each transverse sampling point of the longitudinal sampling stage in each longitudinal sampling stage is to solve for curvature by using a nonlinear parameter optimization method based on a cubic spiral; and substituting curvature based on the tracked vehicle kinematic model to generate candidate paths under a Cartesian coordinate system.
5. The method for planning the local path of the unmanned vehicle facing the off-road environment according to claim 2, wherein the total cost value of the candidate path is obtained by multiplying the static stability cost of the height layer and the pavement semantic cost of the pavement semantic layer with corresponding weights respectively according to the transverse deviation cost of the candidate path on the barrier layer and summing the multiplied values.
6. The method for planning a local path of an unmanned vehicle facing an off-road environment according to claim 5, wherein the cost of the lateral deviation of the barrier layer is the distance between the lateral sampling point corresponding to the candidate path and the longitudinal sampling point in the same longitudinal sampling stage.
7. The method for local path planning for an unmanned vehicle in an off-road environment according to claim 5, wherein the static stability cost of the height layer is obtained by discretizing a candidate path into a plurality of path nodes, and calculating the static stability cost of each path node by the following formula and then summing the calculated static stability costs together:
，
wherein ,representing discretized firstiIndividual path nodes->Represent the firstiThe path nodei-1 absolute pitch angle difference between path nodes, < >>Represent the firstiThe path nodei-absolute value of the roll angle difference between 1 path nodes +.>Represent the firstiThe path nodei-1 unit climb height between path nodes.
8. The method for planning local paths of unmanned vehicles facing off-road environment according to claim 5, wherein the pavement semantic cost of the pavement semantic layer is obtained by discretizing a candidate path into a plurality of path nodes, and calculating the pavement semantic cost of each path node by the following formula, and then accumulating and summing the obtained pavement semantic cost:
，
wherein ,、/> and />Respectively expressed in discretizationiThe proportion of the soil road, grassland and shrub road types at each path node is->、/> and />Respectively representing weights corresponding to the soil road, the grassland and the shrub road types.
9. The method for planning a local path of an unmanned vehicle facing an off-road environment according to claim 1, wherein the planar collision-free constraint is that the closest distance of the path node of the candidate path from the obstacle is greater than the radius of the unmanned vehicle; the constraint condition of the static stability is that the pitch angle and the roll angle of the unmanned vehicle at the path node of the candidate path are smaller than corresponding angle thresholds.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310395181.1A CN116136417B (en) | 2023-04-14 | 2023-04-14 | Unmanned vehicle local path planning method facing off-road environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310395181.1A CN116136417B (en) | 2023-04-14 | 2023-04-14 | Unmanned vehicle local path planning method facing off-road environment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116136417A CN116136417A (en) | 2023-05-19 |
| CN116136417B true CN116136417B (en) | 2023-08-22 |
Family
ID=86334598
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310395181.1A Active CN116136417B (en) | 2023-04-14 | 2023-04-14 | Unmanned vehicle local path planning method facing off-road environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116136417B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116513246B (en) * | 2023-07-04 | 2023-09-12 | 北京理工大学 | Off-road environment speed planning method, system and equipment |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109813328A (en) * | 2019-02-22 | 2019-05-28 | 百度在线网络技术（北京）有限公司 | A kind of driving path planing method, device and vehicle |
| CN109976355A (en) * | 2019-04-26 | 2019-07-05 | 腾讯科技（深圳）有限公司 | Method for planning track, system, equipment and storage medium |
| CN110262488A (en) * | 2019-06-18 | 2019-09-20 | 重庆长安汽车股份有限公司 | Local paths planning method, system and the computer readable storage medium of automatic Pilot |
| CN110341711A (en) * | 2019-07-06 | 2019-10-18 | 深圳数翔科技有限公司 | A kind of driving trace generation system and method based on port environment |
| CN111582543A (en) * | 2020-04-01 | 2020-08-25 | 北京百度网讯科技有限公司 | Generation method of prediction model, and determination method and device of estimated arrival time |
| CN111679678A (en) * | 2020-06-30 | 2020-09-18 | 安徽海博智能科技有限责任公司 | Track planning method and system for transverse and longitudinal separation and computer equipment |
| CN111898804A (en) * | 2020-07-10 | 2020-11-06 | 东风商用车有限公司 | Road self-adaptive sampling method for local planning |
| CN113739805A (en) * | 2021-08-31 | 2021-12-03 | 重庆兰德适普信息科技有限公司 | Path planning information acquisition method, path planning method and electronic equipment |
| CN114924568A (en) * | 2022-05-31 | 2022-08-19 | 芜湖奇瑞科技有限公司 | Method, device, vehicle and medium for planning local path of automatic driving vehicle |
| CN115077534A (en) * | 2022-08-11 | 2022-09-20 | 合肥井松智能科技股份有限公司 | AGV path planning method based on B spline curve |
| WO2022193584A1 (en) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | Multi-scenario-oriented autonomous driving planning method and system |
| CN115123293A (en) * | 2022-06-09 | 2022-09-30 | 燕山大学 | Track planning method based on potential field guidance |
| CN115140096A (en) * | 2022-07-28 | 2022-10-04 | 西安电子科技大学芜湖研究院 | Spline curve and polynomial curve-based automatic driving track planning method |
-
2023
- 2023-04-14 CN CN202310395181.1A patent/CN116136417B/en active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109813328A (en) * | 2019-02-22 | 2019-05-28 | 百度在线网络技术（北京）有限公司 | A kind of driving path planing method, device and vehicle |
| CN109976355A (en) * | 2019-04-26 | 2019-07-05 | 腾讯科技（深圳）有限公司 | Method for planning track, system, equipment and storage medium |
| CN110262488A (en) * | 2019-06-18 | 2019-09-20 | 重庆长安汽车股份有限公司 | Local paths planning method, system and the computer readable storage medium of automatic Pilot |
| CN110341711A (en) * | 2019-07-06 | 2019-10-18 | 深圳数翔科技有限公司 | A kind of driving trace generation system and method based on port environment |
| CN111582543A (en) * | 2020-04-01 | 2020-08-25 | 北京百度网讯科技有限公司 | Generation method of prediction model, and determination method and device of estimated arrival time |
| CN111679678A (en) * | 2020-06-30 | 2020-09-18 | 安徽海博智能科技有限责任公司 | Track planning method and system for transverse and longitudinal separation and computer equipment |
| CN111898804A (en) * | 2020-07-10 | 2020-11-06 | 东风商用车有限公司 | Road self-adaptive sampling method for local planning |
| WO2022193584A1 (en) * | 2021-03-15 | 2022-09-22 | 西安交通大学 | Multi-scenario-oriented autonomous driving planning method and system |
| CN113739805A (en) * | 2021-08-31 | 2021-12-03 | 重庆兰德适普信息科技有限公司 | Path planning information acquisition method, path planning method and electronic equipment |
| CN114924568A (en) * | 2022-05-31 | 2022-08-19 | 芜湖奇瑞科技有限公司 | Method, device, vehicle and medium for planning local path of automatic driving vehicle |
| CN115123293A (en) * | 2022-06-09 | 2022-09-30 | 燕山大学 | Track planning method based on potential field guidance |
| CN115140096A (en) * | 2022-07-28 | 2022-10-04 | 西安电子科技大学芜湖研究院 | Spline curve and polynomial curve-based automatic driving track planning method |
| CN115077534A (en) * | 2022-08-11 | 2022-09-20 | 合肥井松智能科技股份有限公司 | AGV path planning method based on B spline curve |
Non-Patent Citations (1)
| Title |
|---|
| 基于分段优化的车辆换道避障轨迹规划;唐斌等;《汽车工程》;第44卷(第6期);第831-841页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116136417A (en) | 2023-05-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Badue et al. | Self-driving cars: A survey | |
| CN108519094B (en) | Local path planning method and cloud processing terminal | |
| CN113593017B (en) | Method, device, equipment and storage medium for constructing surface three-dimensional model of strip mine | |
| CN112762957B (en) | Multi-sensor fusion-based environment modeling and path planning method | |
| CN114442621A (en) | Autonomous exploration and mapping system based on quadruped robot | |
| CN115639823B (en) | Method and system for controlling sensing and movement of robot under rugged undulating terrain | |
| Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
| CN112432648A (en) | Real-time planning method for safe motion trail of mobile robot | |
| CN114119920A (en) | Three-dimensional point cloud map construction method and system | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| CN111310919B (en) | Driving control strategy training method based on scene segmentation and local path planning | |
| CN116182859A (en) | Path planning method and device, equipment and storage medium | |
| CN116147653B (en) | Three-dimensional reference path planning method for unmanned vehicle | |
| US20240019250A1 (en) | Motion estimation apparatus, motion estimation method, path generation apparatus, path generation method, and computer-readable recording medium | |
| Zhang et al. | A robot navigation system in complex terrain based on statistical features of point clouds | |
| Chen et al. | Improving Autonomous Vehicle Mapping and Navigation in Work Zones Using Crowdsourcing Vehicle Trajectories | |
| CN117289301A (en) | Air-ground unmanned platform collaborative path planning method under unknown off-road scene | |
| CN115167466B (en) | Mobile robot standard control set local path planning method and system | |
| CN116295485A (en) | Unmanned vehicle track planning method based on energy optimization | |
| CN114815899A (en) | Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor | |
| CN118189975B (en) | Autonomous navigation method and device of ground unmanned platform for complex scene | |
| CN118603104B (en) | Channel optimization method for low-altitude high-efficiency flight of valley topography | |
| CN116909268B (en) | 5G-based path planning method, device, equipment and medium for walking robot | |
| Moradi | A non-linear mpc local planner for tractor-trailer vehicles in forward and backward maneuvering | |
| Tran et al. | A Novel of Improving the Accuracy of High-Definition Map for Autonomous Vehicle: A Realistic Case Study in Hanoi Area |
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 |