CN116499486A - Complex off-road environment path planning method and system and electronic equipment - Google Patents
Complex off-road environment path planning method and system and electronic equipment Download PDFInfo
- Publication number
- CN116499486A CN116499486A CN202310752885.XA CN202310752885A CN116499486A CN 116499486 A CN116499486 A CN 116499486A CN 202310752885 A CN202310752885 A CN 202310752885A CN 116499486 A CN116499486 A CN 116499486A
- Authority
- CN
- China
- Prior art keywords
- path
- information
- planning
- road
- safety distance
- 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 77
- 238000005070 sampling Methods 0.000 claims abstract description 49
- 238000005457 optimization Methods 0.000 claims abstract description 45
- 238000000354 decomposition reaction Methods 0.000 claims abstract description 14
- 238000006073 displacement reaction Methods 0.000 claims description 19
- 230000008569 process Effects 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 7
- 230000004044 response Effects 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 5
- 238000009499 grossing Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 10
- 238000004422 calculation algorithm Methods 0.000 abstract description 8
- 230000001133 acceleration Effects 0.000 description 17
- 238000006243 chemical reaction Methods 0.000 description 9
- 238000005096 rolling process Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000001788 irregular Effects 0.000 description 5
- 230000003068 static effect Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 239000010426 asphalt Substances 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000000638 solvent extraction 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
-
- 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)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a method, a system and electronic equipment for planning a path in a complex off-road environment, and relates to the technical field of path planning. The method divides the information units based on the vertical unit decomposition method to obtain the feasible sampling points, reduces the number of the sampling points compared with the traditional planning method, and reduces the uniform sampling calculation cost of path planning. In addition, by establishing a longitudinal and lateral safety distance model, the invention provides a reliable basis for establishing and solving a first optimization problem considering safety, smoothness and operation efficiency to obtain a preliminary path curve (namely a first path curve); the constraint conditions and the task indexes are comprehensively considered to optimize the preliminary path curve, and a path curve which is safe and smooth and accords with the vehicle dynamics constraint (namely, a second path curve) is finally obtained, so that the calculation efficiency of a path planning algorithm is improved, the capability of the vehicle for adapting to complex road conditions in an off-road environment is improved, and the running safety of the vehicle in the off-road environment is improved.
Description
Technical Field
The present invention relates to the field of path planning technologies, and in particular, to a method, a system, and an electronic device for planning a path in a complex off-road environment.
Background
The path planning is one of the key technologies of automatic driving, and is a basis for realizing safe and stable operation of the automatic driving vehicle. The purpose of path planning is to generate a path connecting a start point and an end point, which path needs to meet the following requirements: no collision with static obstacle occurs; meets the limitation condition of vehicle kinematics; the path is smooth to ensure comfort.
Currently, path planning is performed by considering traffic restrictions and motion characteristics restrictions of vehicles per se, aiming at a structured environment. In the off-road environment, the running state of the vehicle is influenced by various factors due to complex terrain and irregular passing area, the influence of the complex terrain on the running state of the vehicle and the safety distance between the complex terrain and the obstacle is not considered in the conventional path planning, the method is not suitable for the complex off-road environment, and the conventional path planning is high in uniform sampling calculation cost.
Disclosure of Invention
In order to solve the problems in the prior art, the invention provides a complex off-road environment path planning method, a system and electronic equipment.
In order to achieve the above object, the present invention provides the following solutions:
a complex off-road environment path planning method, comprising:
acquiring planning information; the planning information includes: global reference path information, vehicle state information, off-road parameter information, and obstacle state information;
dividing the planning information into information units by adopting a vertical unit decomposition method;
acquiring a feasible region of the information unit, and determining the midpoint of the boundary in the feasible region as a feasible sampling point;
establishing a longitudinal and lateral safety distance model, and determining an expected longitudinal and lateral safety distance based on the feasible sampling points and the planning information;
constructing a first optimization problem based on the viable sampling points, the planning information and the desired longitudinal-lateral safety distance;
solving the first optimization problem based on a dynamic programming method to obtain a first path curve;
based on the first path curve, taking constraint conditions into consideration, and constructing a second optimization problem with safety and smoothness as targets; the constraint conditions comprise continuity constraint and boundary constraint;
solving the second optimization problem to obtain a second path curve; the second path curve is a path planning result of the complex off-road environment.
Optionally, establishing a longitudinal and lateral safety distance model based on the feasible sampling points specifically comprises the following steps:
analyzing the vehicle braking stress condition and the vehicle braking process based on the vehicle state information and the off-road parameter information in the planning information;
on the basis of analyzing and obtaining the braking stress condition and the braking process of the vehicle, establishing the longitudinal and lateral safety distance model; the longitudinal and lateral safety distance model comprises: a longitudinal safety distance model and a lateral safety distance model.
Optionally, the longitudinal safety distance model is:
；
in the formula ,for longitudinal safety distance>Following error for longitudinal displacement +.>Is a longitudinal risk weight factor of off-road, +.>For parking safety distance>Is the speed of the bicycle, is%>For braking response time, < >>The braking distance is brought about by the running state of the bicycle and the road condition.
Optionally, the lateral safety distance model is:
；
in the formula ,following error for lateral displacement +.>For off-road lateral risk weighting factor, +.>For the lateral safety distance threshold, +.>Is a lateral safety distance.
Optionally, the first optimization problem is:
；
in the formula ,for the safety weight coefficient, +.>For smoothing weight coefficients, +.>For the bias weight coefficient>For the security cost item, ++>For the bias cost term from the global reference path, < +.>For smoothness cost term, +.>And taking the sum of the costs, and taking the min as the minimum value.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
according to the complex off-road environment path planning method provided by the invention, aiming at the characteristics of complex off-road environment terrain and irregular passing area, the information units are divided based on the vertical unit decomposition method, the feasible sampling points are obtained based on the divided information units, the number of the sampling points is reduced compared with the traditional planning method, and the uniform sampling calculation cost of path planning is reduced. In addition, the influence of off-road conditions and track following errors on the safety distance between the vehicle and the obstacle is considered, a longitudinal and lateral safety distance model is provided, and a reliable basis is provided for establishing and solving a first optimization problem considering safety, smoothness and running efficiency to obtain a preliminary path curve (namely a first path curve); the constraint conditions and the task indexes are comprehensively considered to optimize the preliminary path curve, and finally, a path curve which is safe and smooth and accords with the dynamic constraint of the vehicle is obtained, so that the calculation efficiency of a path planning algorithm is improved, the capability of the vehicle for adapting to complex road conditions in an off-road environment is improved, and the running safety of the vehicle in the off-road environment is improved.
In addition, the invention also provides the following implementation structure:
the complex off-road environment path planning system is applied to the complex off-road environment path planning method; the system comprises:
the information acquisition module is used for acquiring planning information; the planning information includes: global reference path information, vehicle state information, off-road parameter information, and obstacle state information;
the information dividing module is used for dividing the planning information into information units by adopting a vertical unit decomposition method;
the sampling point determining module is used for acquiring a feasible region of the information unit and determining the midpoint of the boundary in the feasible region as a feasible sampling point;
the safety distance model building module is used for building a longitudinal and lateral safety distance model and determining an expected longitudinal and lateral safety distance based on the feasible sampling points and planning information;
constructing a first optimization problem based on the viable sampling points, the planning information and the desired longitudinal-lateral safety distance;
the first path curve determining module is used for solving the first optimization problem based on a dynamic programming method to obtain a first path curve;
the second optimization problem construction module is used for constructing a second optimization problem with safety and smoothness as targets by considering constraint conditions on the basis of the first path curve; the constraint conditions comprise continuity constraint and boundary constraint;
the second path curve determining module is used for solving the second optimization problem to obtain a second path curve; the second path curve is a path planning result of the complex off-road environment.
An electronic device, comprising:
a memory for storing a computer program;
and the processor is connected with the memory and is used for calling and executing the computer program so as to implement the complex off-road environment path planning method.
Optionally, the memory is a computer readable storage medium.
The technical effects achieved by the two implementation structures provided by the invention are the same as those achieved by the complex off-road environment path planning method provided by the invention, so that the description is omitted here.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are needed in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a complex off-road environment path planning method provided by an embodiment of the invention;
FIG. 2 is a schematic diagram of a possible sampling point according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of a solution of a first optimization problem based on a dynamic programming method according to an embodiment of the present invention;
FIG. 4 is a schematic view of a lateral-longitudinal distance between a vehicle and an obstacle according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of lateral boundary constraints provided by an embodiment of the present invention;
fig. 6 is a schematic diagram of a vehicle profile according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention aims to provide a method, a system and electronic equipment for planning a path in a complex off-road environment, which can be suitable for the complex off-road environment and reduce the calculation cost of uniform sampling in path planning.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
Example 1
As shown in fig. 1, the complex off-road environment path planning method provided in this embodiment includes:
step 100: and acquiring planning information. The planning information includes: global reference path information, vehicle state information, off-road parameter information, and obstacle state information. The purpose of this step is to provide parameter information for the partitioning unit to obtain a feasible solution, and to provide an information basis for path planning. The off-road parameter information and the obstacle state information are acquired through a sensing system.
Further, the global reference path in step 100 is a globally planned reference path, which includes a series of heading points. The heading point specifically comprises a heading point abscissa and a heading point ordinate and a corresponding curvature. The vehicle state information includes vehicle pose information and vehicle actuator information. The vehicle pose information includes a vehicle position abscissa, a vehicle speed, a vehicle heading angle, and a yaw rate. The vehicle actuator information includes accelerator opening information, brake pressure information, and front wheel steering angle information. The off-road parameters include linear conditions, road surface types, and adhesion coefficients and rolling resistance coefficients corresponding to the road surface types. The linear conditions include ramp angle, curvature, and degree of undulation. The obstacle state information (static) includes static obstacle position information. The static obstacle position information specifically includes an abscissa and an ordinate of the static obstacle position and an altitude.
Further, the road surface type and the corresponding adhesion coefficient and rolling resistance coefficient may be obtained by: and fusing laser radar point cloud information and image information by utilizing a multisource information fusion algorithm, identifying the road surface type through a convolutional neural network, and acquiring a corresponding attachment coefficient and a rolling resistance coefficient according to the identified road surface type. The road adhesion coefficient and the rolling resistance coefficient are shown in table 1.
Step 101: the planning information is divided into information units using a vertical unit decomposition method. When this step is performed, it is necessary to determine whether an obstacle exists in the front, and if no obstacle exists, the vehicle continues to travel along the reference line. If an obstacle exists, the obstacle is projected to a frenet coordinate system, a SL diagram is built, and the information unit is divided through a vertical unit decomposition method.
Step 102: a viable area of the information unit is acquired and a midpoint of a boundary within the viable area is determined as a viable sampling point. Specifically, after the information units are divided by the vertical unit decomposition method, the boundary in the feasible region of each information unit is obtained, and the midpoint of the boundary and the intersection point of the boundary and the reference line are obtained as feasible sampling points, as shown in fig. 2.
Step 103: and establishing a longitudinal and lateral safety distance model, and determining the expected longitudinal and lateral safety distance based on the feasible sampling points and the planning information. In the present embodiment, the established longitudinal and lateral safety distance models include a longitudinal safety distance model that considers road conditions, road risks, and track following errors, and a lateral safety distance model that considers road conditions and track following errors. Based on the method, the establishment process of the longitudinal and lateral safety distance model is as follows:
step 1031, obtaining vehicle state information and off-road parameter information.
Step 1032, analyzing the braking stress condition and the braking process of the vehicle based on the acquired vehicle state information and the off-road parameter information.
Step 1033, on the basis of analyzing the braking stress condition and the braking process of the vehicle, establishing a longitudinal safety distance model considering road risks and track following errors under road conditions:
1) The traditional safety distance model based on the braking process can be expressed as:
。
wherein ,for braking response time, < >>For safety distance->For stopping safety distance, related to specific road conditions, in generalTaking 2-5m @ of @ and>is the speed of the vehicle. />The maximum braking deceleration is generally a given value, and does not change with road conditions.
2) The traditional safety distance model based on the braking process does not consider road conditions, but considers the road conditions, and the maximum braking deceleration in the downhill process can be obtained according to the braking stress analysis chart. The maximum braking deceleration can be expressed as:
。
wherein ,for the adhesion coefficient, different road types correspond to different adhesion coefficients, and the present example considers that the good road condition is a flat dry asphalt road surface, and the adhesion coefficient thereof is 0.8./>Is the ramp angle. />Gravitational acceleration.
The simplified total braking distance (i.e., the safe distance) of the vehicle can be obtained from the braking process analysis, expressed as:
。
wherein ,is the braking system response time. />For brake on time, since the present embodiment is directed to an autopilot system,driver response time is ignored and brake system response time is of interest.
The safety distance model comprehensively considering the influence of the road condition on the safety distance between vehicles is as follows:
。
wherein ,。
3) Quantifying off-road environment road condition risk:
the braking distance under the good road condition is calculated according to the current state of the vehicle, and is as follows:
。
wherein ,is the braking distance under good road conditions.
Road parameter information such as the gradient, curvature and pavement type of a road in front is obtained according to the parameter identification, and the braking distance of the vehicle under the current road condition is calculated:
。
wherein ,for the braking distance of the vehicle under the current road conditions, < > on>The road surface adhesion coefficient corresponds to the road surface type of the front road section. />Is the angle of the ramp at the road section ahead.
And finally, determining the longitudinal risk weight factor of the off-road according to the braking distances of the vehicles under two road conditions so as to evaluate the risk of the road section ahead. The off-road longitudinal risk weight factor is expressed as:
。
wherein ,is a longitudinal risk weight factor of the off-road.
Then, the longitudinal risk weight factor is introduced into a longitudinal safety distance model, and the longitudinal safety distance model taking the road condition and the road risk into consideration is as follows:
。
wherein ,is the difference between the expected longitudinal safety distance and the actual longitudinal safety distance，/>To take into account road conditions and longitudinal safety distance of road risk.
4) In the actual track following process, longitudinal displacement following errors and speed following errors can occur due to environmental interference and other factors, and the longitudinal displacement following errors and the speed following errors are considered to influence the speed following errors, and the speed following errors can finally appear as longitudinal displacement errors, so that the bus displacement errors are considered in a longitudinal safety distance model, and the longitudinal safety distance model comprehensively considering the track following errors, road conditions and road risks can be expressed as:
。
wherein ,the following error for longitudinal displacement can be expressed as:
。
wherein ,for the longitudinal position of the vehicle at time t under the Frenet coordinate system, < >>Is the longitudinal reference position of the vehicle at time t under the Frenet coordinate system.
Step 1034, establishing a lateral safety distance model considering road conditions and track following errors, wherein the process is as follows:
1) Quantifying road risk:
considering the effect of curve curvature variation on vehicle operation, curve curvature variation changes the magnitude of lateral acceleration, which can be expressed as:
。
wherein ,for lateral acceleration +.>For the relative speed of car and air, +.>Is the curvature of the road.
The lateral reaction force can be expressed as:
. wherein ,/>For vehicle mass>Is a lateral reaction force.
On the basis of straight channels, defining risk weight factors brought by curvature as follows:
。
wherein ,risk weighting factors for curvature +.>For the minimum turning radius of the vehicle +.>Is the curvature of the road ahead +.>To take a minimum function.
The change in road conditions may result in a decrease in the lateral reaction force available to the ground, the maximum lateral reaction force available to the ground may be expressed as：
。
wherein ,the ground longitudinal reaction force can be expressed as:
。
wherein ,for longitudinal acceleration, the vehicle is considered to keep the current speed and acceleration during planning>For the rolling resistance coefficient->Is the air resistance coefficient>Is windward area, is->Is the air density.
The maximum lateral reaction force variation that can be provided by the ground formed by the variation of other road conditions (ramp, road surface type, etc.) is：
。
wherein , and />Represents the maximum lateral reaction force which the ground can provide for the road conditions ahead and the current road conditions, respectively,/->，/>，/>Is the longitudinal reaction force of the ground under the condition of the front road, < +.>，/>For the longitudinal reaction force of the ground under the current road condition, < +.>。
Defining risk weight factors brought by other road conditions as：
。
wherein , and />The road ramp angle and the reference ramp angle at the forward planning point are respectively 0, < +.> and />The road surface rolling resistance coefficient at the front planning point and the reference road surface rolling resistance coefficient are respectively, and />The road surface attachment coefficient at the forward planning point and the reference road surface attachment coefficient.
The off-road lateral risk weight factor can be expressed as：
。
The lateral safety distance model considering road conditions is:
。
wherein ,for the lateral safety distance threshold, +.>For lateral safety distance>Is an off-road lateral risk weight factor.
2) In the actual track following process, lateral displacement following errors are caused by environmental interference and other factors, and the safety distance model comprehensively considering road conditions and track following errors can be expressed as:
。
wherein ,following error for lateral displacement +.>，/>For the lateral position of the vehicle at time t in the Frenet coordinate system,/>Is the lateral reference position of the vehicle at time t in the Frenet coordinate system.
Step 104: a first optimization problem is constructed based on the viable sampling points, the planning information, and the desired longitudinal-lateral safety distance. In this embodiment, an optimization problem that comprehensively considers the running safety, running efficiency, and smoothness of the vehicle, that is, a first optimization problem, is mainly established. Based on this, the first optimization problem that comprehensively considers the vehicle running safety, the task index and the smoothness of the speed curve is:
。
in the formula ,for the safety weight coefficient, +.>For smoothing weight coefficients, +.>For the deviation weight coefficient, for the safety cost term, < ->For the bias cost term from the global reference path, < +.>For smoothness cost term, +.>And taking the sum of the costs, and taking the min as the minimum value.
Wherein, the security cost itemThe expression is as follows:
。
in the formula ,representing the longitudinal dimension +.>Represents lateral dimension +.>Indicating obstacle label, < >>Represents the number of columns of viable sampling points in the longitudinal direction, +.>Represents the lateral line number of the feasible sampling point, and takes the maximum line number of all columns as +.>，/>The number of obstacles is indicated and the number of obstacles, and />Vehicle and obstacle at feasible sampling points (i, j), respectively>A desired longitudinal safety distance therebetween and a desired lateral safety distance therebetween. /> and />Vehicle and obstacle at feasible sampling points (i, j), respectively>The longitudinal and lateral distances between them, as shown in fig. 4, can be expressed in particular as:
。
in the formula ,for viable sampling points->Longitudinal position of->Is a barrier->Is arranged at the longitudinal position of the (c),for viable sampling points->Lateral position of->Is a barrier->Lateral position of (c).
The cost term for the deviation from the global reference path is expressed as follows:
。
for the smoothness cost term, expressed as:
。
wherein ,weight coefficient for lateral velocity, +.>Weight coefficient for lateral acceleration, +.>Weight coefficient for side impact, +.>For viable sampling points->Lateral velocity at the site,/>As viable sampling pointsLateral acceleration at>For viable sampling points->Degree of lateral impact at (1) where、/> and />Can be expressed as:
。
wherein ,for viable sampling points->Lateral position of->Is->Column node and->Longitudinal spacing of column nodes, +.>For viable sampling points->Lateral velocity at the site,/>For viable sampling points->Lateral acceleration at.
Step 105: and solving a first optimization problem based on a dynamic programming method to obtain a first path curve. The first path curve solution is shown in fig. 3.
Step 106: on the basis of the first path curve, constraint conditions are considered, and a second optimization problem is built with the aim of safety and smoothness. Constraints include continuity constraints and boundary constraints. The second optimization problem is established by the following steps:
1) The objective of the optimization problem is to obtain the coordinates of the N waypointsThe final curve is composed, since the symbols are more, in this description the optimization process +.>Any of the N path points is represented, as distinguished from the longitudinal dimension in step 104. Starting from s=0, every fixed interval +.>Take the value to obtain NS coordinates: />。/>For coordinates->The rate of change with respect to the coordinate S, which represents the velocity of lateral movement with respect to the global path. />For coordinates->The second derivative with respect to the coordinates (i.e. arc length) S represents the acceleration of the lateral motion with respect to the global path. Thus, the optimization variable can be expressed as +.>：。
The cost function mainly comprises two indexes of the offset degree of the global path and the smoothness of the curve. The degree of offset of the global path may be accomplished by minimizing the lateral distance. For the smoothness of the curve, the lateral variation of the curve with respect to the global path can be made as slight as possible, i.e. minimizing the firstLateral distance of the path point->Is>Second derivative->And side impact +.>. The cost function can thus be expressed as:
。
wherein ,and respectively representing the offset degree cost weight, the lateral speed cost weight, the lateral acceleration cost weight and the lateral impact cost weight.
2) Establishing constraint conditions, and ensuring continuity of a path curve and safety and smoothness of vehicle operation, wherein the method specifically comprises the following steps:
establishing a continuity constraint: since the curve in the optimization problem is represented by discrete points, continuity constraints need to be imposed from point to determine、/> and />Meaning of representation. In->Route points and->Between +1 waypoints, the following relationship needs to be satisfied:
。
wherein ,is->From a path point toFirst->Side impact of +1 waypoints, +.>，Is->Lateral acceleration of +1 waypoints, +.>Is->Lateral distance of +1 waypoints +.>Is a first derivative of (a).
Establishing boundary constraint: each path point on the path curve should meet boundary constraint, so that the vehicle motion is ensured to be in a reasonable area and not collide with the obstacle, and all N path points should be locatedThe direction satisfies the upper and lower boundary constraints of the location, and the lateral boundary constraints of the vehicle are shown in fig. 5. Let the upper and lower boundaries of lateral displacement, lateral velocity, lateral acceleration and lateral impact degree be +.>、/>、/>、/>、/>、/>、/> and />，/>The boundary constraint may be expressed as:
。
wherein ,for all waypoint lateral displacement, lateral velocity, lateral acceleration and lateral jerk information, +.>Andthe upper and lower bounds of the lateral displacement, the lateral speed, the lateral acceleration and the lateral impact of all the path points can be expressed as follows: />
in the formula ,is a waypoint->Lateral displacement lower bound at->Is a waypoint->Lateral displacement upper bound of the region->Is a waypoint->Lateral velocity lower bound at->Is a waypoint->Lateral speed upper bound at->Is a waypoint->Lateral acceleration lower bound at->Is a waypoint->At the upper boundary of lateral acceleration.
The lateral displacement boundary is given by a passable range where the path curve initial solution is located, and the lateral speed, the lateral acceleration and the lateral impact boundary can be calculated through a vehicle dynamics model.
Since the vehicle is not a point quality model, the shape of the vehicle is considered, the head and the tail are considered to be kept within the boundary constraint range, the occurrence of collision accidents is prevented, and the head and the tail are added as follows:
。
wherein ,、/>、/> and />Four vertexes of the profile left front, right front, left rear and right rear are at +.>Maximum lateral displacement at the individual path points, < >>、/>、/> and />Four vertexes of the profile left front, right front, left rear and right rear are at +.>Maximum lateral displacement at the individual path points, < >>Representing the four vertices of the vehicle contour, left front, right front, left rear and right rear, respectively, as shown in fig. 6, may be specifically represented as:
。
in the formula ,respectively +.>Left front, right front, left rear and right rear four vertexes of the vehicle contour of the single feasibility sampling point, < >>Left and right vertices of the vehicle profile, respectively, ">For the heading angle of the vehicle>Representing the vehicle width.
Step 107: and solving the second optimization problem to obtain a second path curve. The second path curve is a path planning result of the complex off-road environment. The process mainly obtains a safe and smooth path curve which accords with the vehicle dynamics constraint by solving the second optimization problem.
In summary, the embodiment starts from complex road conditions in an off-road environment, considers the influence of complex road conditions and track following errors on the safety distance between a vehicle and an obstacle according to the complex road conditions and the complex road conditions in the off-road environment, designs a path planning method which integrates a vertical unit decomposition method and dynamic planning and comprehensively considers the complex road conditions and the track following errors, and can improve the calculation efficiency of a path planning algorithm and the capability of the vehicle to adapt to the complex road conditions in the off-road environment, thereby improving the running safety of the vehicle in the off-road environment. Aiming at the characteristics of complex terrain of an off-road environment and irregular passing area, a method for solving a path curve primary solution (namely a first path curve) by combining a vertical unit decomposition method and a multi-stage dynamic decision is provided, an information unit is divided based on the vertical unit decomposition method, feasible sampling points are obtained based on the divided information unit, and the number of the sampling points is reduced compared with that of a traditional planning method. And an optimization problem comprehensively considering safety, smoothness and operation efficiency is established, and a preliminary path curve is obtained by solving the optimization problem based on a dynamic decision method. Considering the influence of off-road conditions and track following errors on the safety distance between the vehicle and the obstacle, respectively providing longitudinal and lateral safety distance models, and providing reliable basis for selecting optimal path points. And optimizing the preliminary path curve by comprehensively considering constraint conditions and task indexes, and finally obtaining a path curve which is safe and smooth and accords with the vehicle dynamics constraint. The method can improve the calculation efficiency of the path planning algorithm and the capability of the vehicle to adapt to complex road conditions of the off-road environment, thereby improving the running safety of the vehicle in the off-road environment.
Example two
The embodiment provides a complex off-road environment path planning system, which is applied to the complex off-road environment path planning method provided in the first embodiment. The system comprises:
and the information acquisition module is used for acquiring planning information. The planning information includes: global reference path information, vehicle state information, off-road parameter information, and obstacle state information.
And the information dividing module is used for dividing the planning information into information units by adopting a vertical unit decomposition method.
And the sampling point determining module is used for acquiring the feasible region of the information unit and determining the midpoint of the boundary in the feasible region as a feasible sampling point.
And the safety distance model building module is used for building a longitudinal and lateral safety distance model and determining an expected longitudinal and lateral safety distance based on the feasible sampling points and the planning information.
The first optimization problem construction module is used for constructing a first optimization problem based on the feasible sampling points, the planning information and the expected longitudinal and lateral safety distance;
the first path curve determining module is used for solving a first optimization problem based on a dynamic programming method to obtain a first path curve.
And the second optimization problem construction module is used for constructing a second optimization problem with the aim of safety and smoothness by considering constraint conditions on the basis of the first path curve. Constraints include continuity constraints and boundary constraints.
And the second path curve determining module is used for solving a second optimization problem to obtain a second path curve. The second path curve is a path planning result of the complex off-road environment.
Example III
This embodiment provides an electronic device. The electronic device includes:
and a memory for storing a computer program.
And the processor is connected with the memory and is used for retrieving and executing the computer program to implement the complex off-road environment path planning method provided by the first embodiment.
Furthermore, the computer program in the above-described memory may be stored in a computer-readable storage medium when it is implemented in the form of a software functional unit and sold or used as a separate product. Based on this understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art or in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the method of the embodiments of the present invention. And the aforementioned storage medium includes: various media capable of storing program codes, such as a U disk, a mobile hard disk, a read-only memory, a random access memory, a magnetic disk or an optical disk.
Based on the description, the invention fills the gap of the complex off-road environment path planning technology considering complex road conditions and track following errors. The conventional path planning is mostly aimed at a structural environment, and the path planning is performed by considering traffic limitation and motion characteristic limitation of a vehicle. The off-road environment is complex in terrain and irregular in passing area, the running state of the vehicle is influenced by various factors, the influence of the complex terrain on the running state of the vehicle and the safety distance between the complex terrain and obstacles is not considered in traditional path planning, and the method is not suitable for the complex off-road environment. And the traditional path planning uniform sampling calculation cost is higher.
Aiming at the off-road environment, in order to solve the problem that a path planning algorithm cannot adapt to the planned optimal path curve of the characteristics of irregular road, complex road conditions and the like of the off-road environment, the invention designs a complex off-road environment path planning method, adopts the idea of a vertical unit decomposition method to obtain feasible sampling points, obtains a path curve initial solution based on a dynamic planning algorithm, respectively establishes longitudinal and lateral safety distance models which comprehensively consider the off-road conditions and track following errors, and can improve the calculation efficiency of the path planning algorithm and the capability of the vehicle to adapt to the complex road conditions of the off-road environment, thereby improving the running safety of the vehicle in the off-road environment.
The expected benefits and commercial values after the technical scheme of the invention is converted are as follows: for the off-road environment automatic driving vehicle, such as a mine vehicle and a capital transportation vehicle, the method for planning the path provided by the technical scheme for carrying out the transportation task can improve the capability of the vehicle to adapt to the complex road conditions of the off-road environment, so that the running safety of the vehicle in the off-road environment is improved.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to assist in understanding the methods of the present invention and the core ideas thereof; also, it is within the scope of the present invention to be modified by those of ordinary skill in the art in light of the present teachings. In view of the foregoing, this description should not be construed as limiting the invention.
Claims (8)
1. A complex off-road environment path planning method, comprising:
acquiring planning information; the planning information includes: global reference path information, vehicle state information, off-road parameter information, and obstacle state information;
dividing the planning information into information units by adopting a vertical unit decomposition method;
acquiring a feasible region of the information unit, and determining the midpoint of the boundary in the feasible region as a feasible sampling point;
establishing a longitudinal and lateral safety distance model, and determining an expected longitudinal and lateral safety distance based on the feasible sampling points and the planning information;
constructing a first optimization problem based on the viable sampling points, the planning information and the desired longitudinal-lateral safety distance;
solving the first optimization problem based on a dynamic programming method to obtain a first path curve;
based on the first path curve, taking constraint conditions into consideration, and constructing a second optimization problem with safety and smoothness as targets; the constraint conditions comprise continuity constraint and boundary constraint;
solving the second optimization problem to obtain a second path curve; the second path curve is a path planning result of the complex off-road environment.
2. The complex off-road environment path planning method according to claim 1, wherein building a longitudinal and lateral safety distance model specifically comprises:
analyzing the vehicle braking stress condition and the vehicle braking process based on the vehicle state information and the off-road parameter information in the planning information;
on the basis of analyzing and obtaining the braking stress condition and the braking process of the vehicle, establishing the longitudinal and lateral safety distance model; the longitudinal and lateral safety distance model comprises: a longitudinal safety distance model and a lateral safety distance model.
3. The complex off-road environment path planning method of claim 2, wherein the longitudinal safe distance model is:
；
in the formula ,for longitudinal safety distance>Following error for longitudinal displacement +.>Is a longitudinal risk weight factor of off-road, +.>For parking safety distance>Is the speed of the bicycle, is%>For braking response time, < >>The braking distance is brought about by the running state of the bicycle and the road condition.
4. The complex off-road environment path planning method of claim 2, wherein the lateral safety distance model is:
；
in the formula ,following error for lateral displacement +.>Is off-roadRisk weighting factor in the lateral direction of the road, +.>For the lateral safety distance threshold, +.>Is a lateral safety distance.
5. The complex off-road environment path planning method of claim 1, wherein the first optimization problem is:
；
in the formula ,for the safety weight coefficient, +.>For smoothing weight coefficients, +.>For the bias weight coefficient>For the security cost item, ++>For the bias cost term from the global reference path, < +.>For smoothness cost term, +.>And taking the sum of the costs, and taking the min as the minimum value.
6. A complex off-road environment path planning system, characterized by being applied to the complex off-road environment path planning method according to any one of claims 1-5; the system comprises:
the information acquisition module is used for acquiring planning information; the planning information includes: global reference path information, vehicle state information, off-road parameter information, and obstacle state information;
the information dividing module is used for dividing the planning information into information units by adopting a vertical unit decomposition method;
the sampling point determining module is used for acquiring a feasible region of the information unit and determining the midpoint of the boundary in the feasible region as a feasible sampling point;
the safety distance model building module is used for building a longitudinal and lateral safety distance model and determining an expected longitudinal and lateral safety distance based on the feasible sampling points and the planning information;
a first optimization problem construction module for constructing a first optimization problem based on the viable sampling points, the planning information, and the desired longitudinal-lateral safety distance;
the first path curve determining module is used for solving the first optimization problem based on a dynamic programming method to obtain a first path curve;
the second optimization problem construction module is used for constructing a second optimization problem with safety and smoothness as targets by considering constraint conditions on the basis of the first path curve; the constraint conditions comprise continuity constraint and boundary constraint;
the second path curve determining module is used for solving the second optimization problem to obtain a second path curve; the second path curve is a path planning result of the complex off-road environment.
7. An electronic device, comprising:
a memory for storing a computer program;
a processor, coupled to the memory, for retrieving and executing the computer program to implement the complex off-road environment path planning method of any one of claims 1-5.
8. The electronic device of claim 7, wherein the memory is a computer-readable storage medium.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310752885.XA CN116499486B (en) | 2023-06-26 | 2023-06-26 | Complex off-road environment path planning method and system and electronic equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310752885.XA CN116499486B (en) | 2023-06-26 | 2023-06-26 | Complex off-road environment path planning method and system and electronic equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116499486A true CN116499486A (en) | 2023-07-28 |
| CN116499486B CN116499486B (en) | 2023-09-22 |
Family
ID=87325072
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310752885.XA Active CN116499486B (en) | 2023-06-26 | 2023-06-26 | Complex off-road environment path planning method and system and electronic equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116499486B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117077869A (en) * | 2023-10-11 | 2023-11-17 | 鱼快创领智能科技(南京)有限公司 | Method for allocating and planning routes of vehicle with customized access sequence in scene |
| CN117091618A (en) * | 2023-10-18 | 2023-11-21 | 理工雷科智途(北京)科技有限公司 | Unmanned vehicle path planning method and device and electronic equipment |
| CN117571012A (en) * | 2024-01-15 | 2024-02-20 | 北京理工大学 | Global path planning method, system and equipment for unmanned vehicle in off-road environment |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109669461A (en) * | 2019-01-08 | 2019-04-23 | 南京航空航天大学 | Automatic driving vehicle decision system and its method for planning track under a kind of complex working condition |
| CN110377039A (en) * | 2019-07-26 | 2019-10-25 | 吉林大学 | A kind of vehicle obstacle-avoidance trajectory planning and tracking and controlling method |
| CN112068545A (en) * | 2020-07-23 | 2020-12-11 | 哈尔滨工业大学（深圳） | Method and system for planning driving track of unmanned vehicle at crossroad and storage medium |
| CN113276848A (en) * | 2021-04-09 | 2021-08-20 | 江苏大学 | Intelligent driving lane changing and obstacle avoiding track planning and tracking control method and system |
| CN114372375A (en) * | 2022-01-13 | 2022-04-19 | 华中科技大学 | Method and system for solving safe driving area of off-road vehicle |
| CN114895676A (en) * | 2022-05-12 | 2022-08-12 | 扬州大学 | Method for realizing high-speed running of ground automatic driving vehicle based on space intelligent system |
| US20220355792A1 (en) * | 2021-05-10 | 2022-11-10 | Dr. Ing. H.C. F. Porsche Aktiengesellschaft | Method and device for trajectory planning for a vehicle |
| CN115933641A (en) * | 2022-11-17 | 2023-04-07 | 杭州电子科技大学 | AGV path planning method based on model prediction control guidance deep reinforcement learning |
-
2023
- 2023-06-26 CN CN202310752885.XA patent/CN116499486B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109669461A (en) * | 2019-01-08 | 2019-04-23 | 南京航空航天大学 | Automatic driving vehicle decision system and its method for planning track under a kind of complex working condition |
| CN110377039A (en) * | 2019-07-26 | 2019-10-25 | 吉林大学 | A kind of vehicle obstacle-avoidance trajectory planning and tracking and controlling method |
| CN112068545A (en) * | 2020-07-23 | 2020-12-11 | 哈尔滨工业大学（深圳） | Method and system for planning driving track of unmanned vehicle at crossroad and storage medium |
| CN113276848A (en) * | 2021-04-09 | 2021-08-20 | 江苏大学 | Intelligent driving lane changing and obstacle avoiding track planning and tracking control method and system |
| US20220355792A1 (en) * | 2021-05-10 | 2022-11-10 | Dr. Ing. H.C. F. Porsche Aktiengesellschaft | Method and device for trajectory planning for a vehicle |
| CN114372375A (en) * | 2022-01-13 | 2022-04-19 | 华中科技大学 | Method and system for solving safe driving area of off-road vehicle |
| CN114895676A (en) * | 2022-05-12 | 2022-08-12 | 扬州大学 | Method for realizing high-speed running of ground automatic driving vehicle based on space intelligent system |
| CN115933641A (en) * | 2022-11-17 | 2023-04-07 | 杭州电子科技大学 | AGV path planning method based on model prediction control guidance deep reinforcement learning |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117077869A (en) * | 2023-10-11 | 2023-11-17 | 鱼快创领智能科技(南京)有限公司 | Method for allocating and planning routes of vehicle with customized access sequence in scene |
| CN117077869B (en) * | 2023-10-11 | 2023-12-19 | 鱼快创领智能科技(南京)有限公司 | Method for allocating and planning routes of vehicle with customized access sequence in scene |
| CN117091618A (en) * | 2023-10-18 | 2023-11-21 | 理工雷科智途(北京)科技有限公司 | Unmanned vehicle path planning method and device and electronic equipment |
| CN117091618B (en) * | 2023-10-18 | 2024-01-26 | 理工雷科智途(北京)科技有限公司 | Unmanned vehicle path planning method and device and electronic equipment |
| CN117571012A (en) * | 2024-01-15 | 2024-02-20 | 北京理工大学 | Global path planning method, system and equipment for unmanned vehicle in off-road environment |
| CN117571012B (en) * | 2024-01-15 | 2024-03-15 | 北京理工大学 | Global path planning method, system and equipment for unmanned vehicle in off-road environment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116499486B (en) | 2023-09-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116499486B (en) | Complex off-road environment path planning method and system and electronic equipment | |
| CN110362096B (en) | Unmanned vehicle dynamic trajectory planning method based on local optimality | |
| CN109669461B (en) | Decision-making system for automatically driving vehicle under complex working condition and track planning method thereof | |
| CN108256233B (en) | Intelligent vehicle trajectory planning and tracking method and system based on driver style | |
| CN110286681B (en) | Dynamic automatic driving track-changing planning method for curvature-variable curve | |
| CN111089594B (en) | Autonomous parking trajectory planning method suitable for multiple scenes | |
| CN109727469B (en) | Comprehensive risk degree evaluation method for automatically driven vehicles under multiple lanes | |
| CN112389427A (en) | Vehicle track optimization method and device, electronic equipment and storage medium | |
| CN113932823A (en) | Unmanned multi-target-point track parallel planning method based on semantic road map | |
| CN111717192A (en) | Control method and system for automatically driving vehicle | |
| CN112896188B (en) | Automatic driving decision control system considering front vehicle encounter | |
| CN116513246B (en) | Off-road environment speed planning method, system and equipment | |
| CN112578672B (en) | Unmanned vehicle trajectory control system based on chassis nonlinearity and trajectory control method thereof | |
| CN112977411A (en) | Intelligent chassis control method and device | |
| CN114987461A (en) | Intelligent passenger car dynamic lane change trajectory planning method under multi-car complex traffic environment | |
| CN114637292A (en) | Vehicle track tracking robust control method and system considering obstacle avoidance | |
| CN117141489B (en) | Intelligent vehicle track layered planning method based on minimum action quantity principle | |
| CN112238856B (en) | Intelligent vehicle overtaking track optimization method based on hybrid particle swarm optimization | |
| US20220176987A1 (en) | Trajectory limiting for autonomous vehicles | |
| CN116465427B (en) | Intelligent vehicle lane changing obstacle avoidance path planning method based on space-time risk quantification | |
| CN115712950A (en) | Automatic driving decision-making method for semi-trailer | |
| CN116039672A (en) | Data-model back-end fusion method for automatic driving decision control function | |
| CN112793566B (en) | Collision avoidance method and device | |
| CN114889589A (en) | Intelligent automobile steering and braking cooperative collision avoidance control system and method | |
| CN111857112B (en) | Automobile local path planning method and electronic equipment |
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 |