CN111121804A - Intelligent vehicle path planning method and system with safety constraint - Google Patents
Intelligent vehicle path planning method and system with safety constraint Download PDFInfo
- Publication number
- CN111121804A CN111121804A CN201911221205.1A CN201911221205A CN111121804A CN 111121804 A CN111121804 A CN 111121804A CN 201911221205 A CN201911221205 A CN 201911221205A CN 111121804 A CN111121804 A CN 111121804A
- Authority
- CN
- China
- Prior art keywords
- target
- vehicle
- threat
- field
- safety
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- 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
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
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 an intelligent vehicle path planning method and system with safety constraint. In the modeling stage, firstly, environmental factors related to the safety of the intelligent vehicle are extracted, a static threat field is constructed according to the environmental factors and target characteristics aiming at each target in the environment, then the kinematic characteristics of each target are considered, a target dynamic threat field is constructed and fused, and finally, an optimized objective function in the shortest time is constructed according to safety constraints to establish a path planning model. In the operation stage, firstly, a vehicle-mounted sensor is used for collecting environmental factors and target characteristic data in real time, a fused global target dynamic threat field is calculated, then the global target dynamic threat field is divided according to a set safety threshold, and a shortest time path is planned in a safe region. The invention can make the intelligent vehicle shortest in running time under the condition of meeting the safety requirement, and can simultaneously give consideration to the passing efficiency and the safety of the planned path.
Description
Technical Field
The invention belongs to the technical field of automation and computers, particularly relates to the technical field of path planning of intelligent vehicles, and particularly relates to an intelligent vehicle path planning method and system with safety constraint.
Background
Path planning is one of the important links of intelligent vehicle technology and is the premise of vehicle behavior control. When a task is executed, the intelligent vehicle is required to plan an optimal path from a starting point to a terminal point according to certain standards. At present, a plurality of mature path planning methods are available, which mainly aim at planning the shortest path, and although the shortest path length can be ensured, the path is likely to cling to the obstacle, thereby generating great threat to the vehicle and reducing the driving safety. In addition, when the vehicle travels to the vicinity of an obstacle, the vehicle can only pass through the obstacle at a small speed, and the traveling time of the vehicle is increased, so that the traveling efficiency is reduced.
In the existing path planning method, chinese patent application: the intelligent vehicle path planning method (application number: CN201610050880) based on threat estimation adopts a threat estimation technology to realize threat index reasoning on an external target, establishes a comprehensive potential field model of the intelligent vehicle, and obtains the path planning of the intelligent vehicle after solving. Although the method adds the influence of the target, the driving safety can be improved, the method is easy to fall into local optimum when the path is solved, and the driving efficiency of the path is not high. The Chinese patent application: a method and a system (application number: CN201811321625) for planning obstacle detouring paths on a specified path utilize a robot kinematics model to simulate a plurality of growth tracks, combine the speed and the angular speed of a robot, simulate preset time according to each combination to obtain a corresponding growth track, and use the track with short detouring time as a final obstacle detouring path. Although the method can plan the path with the shortest travel time, the threat of an external target to the traveling is not considered, sometimes the planned path is clung to the target, and the safety of the vehicle traveling cannot be ensured. The Chinese patent application: the local dynamic path planning method (application number: CN201711119755.3) of the mobile robot based on the self-adaptive dynamic window calculates the dynamic weight of the distance between obstacles and the linear speed of the vehicle when the obstacle is dense, searches for an alternative speed space to calculate the allowable speed when no collision occurs, substitutes the allowable speed into an objective function to calculate the optimal speed of the vehicle at the next moment, and executes the optimal speed until the terminal point. The method only calculates the optimal running speed of the vehicle according to the distance of the obstacle, does not consider the influence of factors such as the speed of the obstacle, the environment and the like, and reduces the running safety. In addition, the path planned by the method can only ensure the optimal speed at each moment, but cannot ensure the optimal path and the optimal total running time.
The invention provides an intelligent vehicle path planning method with safety constraint aiming at the problem that the driving time and the target threat are difficult to be comprehensively considered when a path is planned, and a target global threat field and a path planning model are established according to the surrounding environment information, so that a proper path and speed are planned, the driving safety is improved, and the driving efficiency is improved.
Disclosure of Invention
The present invention is directed to solving the above problems of the prior art. An intelligent vehicle path planning method and system with safety constraint are provided. The technical scheme of the invention is as follows:
an intelligent vehicle path planning method with safety constraints comprises a modeling phase and an operation phase, wherein,
the modeling phase comprises the steps of: firstly, extracting environment factors related to the safety of the intelligent vehicle, constructing a static threat field according to the environment factors and target characteristics aiming at each target in the environment, then constructing a target dynamic threat field by considering the kinematic characteristics of each target, then fusing the threat fields of multiple targets to obtain a fused global target dynamic threat field, and finally constructing a shortest time optimization objective function according to safety constraint to establish a path planning model;
the operation phase comprises the following steps: firstly, acquiring environmental factors and target characteristic data in real time by using a vehicle-mounted sensor, and calculating a global target dynamic threat field formed by multiple targets on a road to an intelligent vehicle by combining a threat field model established in a modeling stage; setting a safety threshold according to the safety requirement of vehicle running, dividing the global target dynamic threat field into a safe part and an unsafe part by taking the safety threshold as a constraint condition, and planning a shortest time path in the safe part.
Further, the extracting of the threat factors of the intelligent vehicle in the modeling stage specifically includes: target characteristic factor, environment factor and target kinematic factor, wherein the target characteristic refers to the type size of the target, and p is usedcartpRepresents; environmental factors refer to the current climate conditions, in penvirRepresents; the kinematic factors of the object include the speed and position of the object, the speed of the object is a vector, and
indicating that the position of the target is (x)obs,yobs) And (4) showing.
Further, the constructing of the target static threat field in the modeling stage specifically includes: in the static scene, a static threat field E of a single target is constructed according to the target characteristics, the climate environment and the target position, as shown in formula (1). The field is a scalar field and is not affected by the target speed; the larger the target type, the larger the threat field generated; the worse the weather conditions, the larger the threat field generated;
in the formula, kcartp、kenvir、kdisScale factors respectively representing target characteristics, climate environment and position distance; p is a radical ofcartp、penvirRespectively representing a target characteristic value and a climate environment value; (x, y) represents the location of the smart vehicle; (x)obs,yobs) Indicating the position of the target; e denotes the target static threat field.
Further, the constructing a target dynamic threat field: on the basis of a static threat field, combining the motion state of the intelligent vehicle, adding a target kinematics factor, and constructing a target dynamic threat field, wherein the field is a vector field, and a dynamic threat field generated by a single target i to the intelligent vehicle
As shown in formula (2).
In the formula, kcar、kobsScale factors respectively representing the speed of the intelligent vehicle and the target speed;
representing the intelligent vehicle speed and the target speed; theta represents
With an angle to the coordinate axis x in the counter-clockwise direction,
a distance value representing the target and the intelligent vehicle;
representing a target dynamic threat field.
Under the multi-target scene, the threat fields are fused in a superposition mode to obtain a global threat field
As shown in formula (3).
In the formula, m represents the target number.
Further, the setting of the safety threshold specifically includes: in order to ensure the driving safety, a safety threshold value is set on the basis of the global target threat field, the global target dynamic threat field is divided into a safe part and an unsafe part by taking the safety threshold value as a constraint condition, and if the global target dynamic threat field is intelligent, the global target dynamic threat field is divided into the safe part and the unsafe partThe vehicle travels at a speed v at a position (x, y) where it is locatedobs,yobs) Has a speed of
Target of (1), L0If the safety threshold is set, the criterion for judging whether the traffic can be safely passed is as follows:
further, the constructing an optimization objective function specifically includes: in the safe passing area, in order to make the vehicle travel in the shortest time and optimize the performance of the planned path, an optimization objective function in the shortest time is established:
in the formula (x)0,y0) Denotes the starting point, (x)n,yn) Indicating an endpoint; (x, y) represents the smart vehicle position, v represents the speed at which the smart vehicle is traveling at (x, y); s represents the running length of the intelligent vehicle; t represents the total travel time from the start point to the end point; Δ x and Δ y represent the step size of the smart vehicle search in the x and y directions.
Further, the operation phase comprises the following steps:
(1) data acquisition: the system is used for acquiring environmental factors, target characteristics and target kinematic data in real time by using a vehicle-mounted sensor in the vehicle driving process;
(2) and (3) calculating a threat field: calculating a global threat field formed by each target on the road by using the collected data of the environment, the target characteristics and the target kinematics and combining a threat field model established in a modeling stage;
(3) and (3) calculating an optimal path: in order to obtain the optimal path meeting the shortest driving time, the global threat field of the position where the vehicle is located is calculated in real time, the vehicle speed is continuously adjusted according to the safety constraint condition, and the optimal path meeting the shortest driving time is calculated.
Further, the specific process of calculating the optimal path meeting the shortest time includes:
the continuous model is discretized according to the requirement of optimizing the objective function in equation (5), and an optimal path with speed is calculated as shown in equation (6).
In the formula,. DELTA.xi∈[-1,0)∪(0,1]，Δyi∈[-1,0)∪(0,1]；LvA threshold value representing a change in speed of the smart vehicle; path point (x)i+1,yi+1) Is in the previous position (x)i,yi) The speed variation of adjacent moments is within a certain range, so that sudden change of speed is avoided;
according to the above method, if the Path Path { (x) can be solvedi,yi,vi) If i is 0,1,2, … …, n, and n represents the number of the path points, the solution is the optimal solution, so that the highest efficiency of vehicle driving can be ensured;
iterative optimization: if the path can not be solved, the driving speed of the vehicle needs to be readjusted, and the vehicle is in (x)i+1,yi+1) Absolute value of the difference between the global threat field and the security threshold at
Less than a certain convergence value LconvgIt means that the intelligent vehicle is in a safe area, and the vehicle can increase the speed properly in order to improve the running efficiency when the absolute value is
Greater than LconvgThe vehicle is in a threat area, and in order to reduce the driving risk, the vehicle needs to reduce the speed, and the acceleration and deceleration mode is shown as a formula (7);
wherein, Deltav belongs to [1,5], the velocity v calculated at each position takes the maximum value in the range;
calculating a global threat field by sequentially substituting each velocity value into equation (3), and then solving an optimal Path { (x)i,yi,vi)|i＝0,1,2,……,n}。
An intelligent vehicle path planning system with safety constraints, comprising:
a data acquisition module: the system is used for acquiring environmental factors, target characteristics and target kinematic data in real time by using a vehicle-mounted sensor in the vehicle driving process;
a threat field calculation module: constructing a static threat field of the target according to the environment and target characteristic data obtained by the data acquisition module, then adding the kinematic characteristics of each target, and calculating a dynamic threat field formed by multiple targets on the road to the intelligent vehicle; fusing the plurality of dynamic threat fields to obtain a global target dynamic threat field;
an optimal path calculation module: setting a safety threshold according to the safety requirement of vehicle running, and dividing the global target dynamic threat field into a safe part and an unsafe part by taking the safety threshold as a constraint condition; and then constructing a shortest time optimization objective function, and planning a shortest time path in a safe region.
The invention has the following advantages and beneficial effects:
aiming at the path planning of the intelligent vehicle, the invention fully considers the threat influence of an external target on the intelligent vehicle, establishes a target global threat field model and constructs a path planning model with safety constraint including the target type and environment. The invention establishes the mutual relation between the threat field and the intelligent vehicle speed, creatively provides a path expression method containing speed information and a shortest time path planning method taking safety as a constraint condition, and solves the problem that the traditional path planning method cannot comprehensively process the safety and the driving efficiency. The traditional threat field model only considers the distance influence between the intelligent vehicle and the target, but the invention also adds the target characteristics and the kinematics factors besides considering the distance, and fuses the speed of the intelligent vehicle in the model, so that the threat field model constructed by the invention is more sufficient. Meanwhile, the traditional path planning method can only plan a path, but the invention solves the shortest time path under the constraint condition of a safety threshold, can plan the path and speed, and simultaneously considers both traffic efficiency and safety.
Drawings
Fig. 1 is a schematic view of a scenario of an intelligent vehicle path planning method with safety constraints according to a preferred embodiment of the present invention.
FIG. 2 is a general framework of an intelligent vehicle path planning method with safety constraints.
FIG. 3 illustrates an example of a threat field formed by the present invention.
FIG. 4 illustrates an example of the impact of the target threat field of the present invention on the path of a smart vehicle.
Detailed Description
The technical solutions in the embodiments of the present invention will be described in detail and clearly with reference to the accompanying drawings. The described embodiments are only some of the embodiments of the present invention.
The technical scheme for solving the technical problems is as follows:
FIG. 1 is a schematic view of a Path planning method scenario according to the present invention, in which a solid line Path is shown0Representing a Path derived from a conventional Path planning method, the dashed Path1Representing the path with velocity planned by the present invention.
As can be seen from the figure, the path drawn by the conventional method can ensure the shortest total length, but the method cannot ensure the efficient running of the vehicle and cannot ensure the safety of the running of the vehicle. The conventional method usually performs planning based on a start point, an end point, and a positional relationship of each target, and as long as the host vehicle does not collide with the target, the path is considered to be a travelable path. Therefore, the situation that the planned path is close to the target often occurs by adopting the traditional method. In this case, to pass the vehicle safely, only the traveling speed can be reduced, which increases the traveling time to some extent.
In FIG. 1, target 1 is a distance Path0Closest point of (P)1A distance of l1And Path1P of2Distance of point is l2Wherein l is1<l2(ii) a Target 2 distance Path0Closest point of (P)3A distance of l3And Path1P of4Distance of point is l4Wherein l is3<l4. If according to Path0The speed of the normal running of the route is v when the vehicle runs to P1When nearby, the speed of the main vehicle needs to be greatly reduced to v1At a velocity v1Traveling5Then the speed v is recovered; go to P3Near, the velocity drops to v2Go on6Then, the vehicle returns to v until the vehicle runs to the terminal; suppose Path0Has a length of lpath0Then the total time spent is
The value is greater than
The Path planned according to the invention is Path1，Path1Has a length of lpath1Then l ispath1Greater than lpath0. The planned path of the invention has no condition of being close to the target, so the vehicle can run at higher speed in the whole process. If according to Path1The speed of the normal running of the route is v when the vehicle runs to P2Near, the subject accelerates to v because of the distance from the target and the rapid passing3Go on7Then continuously driving at the speed v; go to P4Nearby, the host vehicle may accelerate to v4Go on8Then the vehicle returns to v until the vehicle reaches the end point, and the total driving time is
Because v is3>>v1，v4>>v2，l7>l5，l8>l6Therefore, it is
The method has the advantages of short running time and high efficiency of the planned routeHigh.
Fig. 2 shows an overall framework of the intelligent vehicle path planning method with safety constraints according to the present invention, which includes a modeling phase and an operating phase.
(1) A modeling stage: the method is used for analyzing and extracting environmental factors and target characteristic factors related to the safety of the intelligent vehicle, analyzing the influence degree of each factor on the intelligent vehicle according to the mutual relation among the factors, and constructing a target static threat field. On the basis of the field, the kinematic characteristics of the targets are added, the influence of the kinematic characteristics and the speed of the intelligent vehicle is analyzed, and a dynamic threat field generated by each target on the intelligent vehicle is constructed. And then fusing the threat fields of the multiple targets to obtain a global threat field related to the speed. And finally, constructing a shortest time optimization objective function to obtain a path planning model with safety constraint. The method comprises the following steps:
1) extracting threat factors of the intelligent vehicle: mainly comprises target characteristic factors, environmental factors and target kinematic factors. Wherein the target feature refers to the type and size of the target, and p is usedcartpRepresents; environmental factors refer to the current climate conditions, in penvirRepresents; the kinematic factors of the object include the speed of the object and the position of the object, the speed of the object being a vector
Indicating that the position of the target is (x)obs,yobs) And (4) showing.
2) Constructing a target static threat field: in a static scenario, a static threat field E of a single target is constructed, which is not affected by the target speed, depending on the target characteristics, the climate environment and the target location. If k iscartp、kenvir、kdisScale factors respectively representing target characteristics, climate environment and position distance; p is a radical ofcartp、penvirRespectively representing a target characteristic value and a climate environment value; (x, y) represents the location of the smart vehicle; (x)obs,yobs) Indicating the position of the target; and establishing a static threat field function as shown in the formula (1) according to the relationship of all factors.
3) Constructing a target dynamic threat field: on the basis of the static threat field, target kinematics factors are added in combination with the motion state of the vehicle, and a dynamic threat field is constructed. The field is a vector field, and the dynamic threat field generated by the target i to the intelligent vehicle is
When the target is in a static state, the threat fields generated around the target are evenly distributed. When the object is in motion, the threat field generated is non-uniformly distributed all around, with the threat field being shifted towards the direction of motion of the object. In addition, the running speed of the intelligent vehicle has an expansion effect on a threat field. On the basis of the static threat field, the kinematics characteristics of the target and the motion state of the intelligent vehicle are integrated, and the function of the dynamic threat field of the target i is constructed as shown in the formula (2).
In the formula, kcar、kobsScale factors respectively representing the speed of the intelligent vehicle and the target speed;
representing the intelligent vehicle speed and the target speed; theta represents
With an angle to the coordinate axis x in the counter-clockwise direction,
a distance value representing the target and the intelligent vehicle;
representing movement of an objectA state threat field.
According to the formula (2), when the speed of the intelligent vehicle is 0, the dynamic threat field is determined by the target kinematics and the original static threat field, the threat field in front of the target motion is large, and the threat field in the rear is small. When the target speed is 0, the dynamic threat field is determined by the motion state of the intelligent vehicle and the original static threat field, and the larger the speed of the intelligent vehicle is, the larger the threat field is. When both the smart vehicle and the target are stationary, the dynamic threat field becomes a static threat field.
Multi-target threat field fusion: under the multi-target scene, the threat fields are fused in a superposition mode to obtain a global threat field
As shown in formula (3).
In the formula, m represents the target number.
4) Setting a safety threshold value: in order to ensure the driving safety, a safety threshold is set on the basis of the global target threat field, and the global target dynamic threat field is divided into a safe part and an unsafe part by taking the safety threshold as a constraint condition. If the smart vehicle is traveling at a speed v at location (x, y), it is at location (x)obs,yobs) Has a speed of
Target of (1), L0If the safety threshold is set, the criterion for judging whether the traffic can be safely passed is as follows:
5) constructing an optimization objective function: in the safe traffic area, in order to minimize the vehicle running time and optimize the performance of the planned path, an optimized objective function with the shortest time is established by combining the speed of the intelligent vehicle. The objective function is as follows:
in the formula (x)0,y0) Denotes the starting point, (x)n,yn) Indicating an endpoint; (x, y) represents the smart vehicle position, v represents the speed at which the smart vehicle is traveling at (x, y); s represents the running length of the intelligent vehicle; t represents the total travel time from the start point to the end point; Δ x and Δ y represent the step size of the smart vehicle search in the x and y directions.
(2) And (3) an operation stage: the vehicle-mounted sensor is used for collecting environmental factors and target characteristic data in real time, and a global threat field formed by multiple targets on a road to the intelligent vehicle is calculated by combining a model established in a modeling stage. And setting a safety threshold according to the driving safety requirement of the vehicle, and establishing a discretization solving model by adopting a numerical analysis method. And planning an optimal path and speed within the threshold range. The method comprises the following steps:
(1) data acquisition: the system is used for acquiring environmental factors, target characteristics and target kinematic data in real time by using a vehicle-mounted sensor in the vehicle driving process;
(2) and (3) calculating a threat field: calculating a global threat field formed by each target on the road by using the collected data of the environment, the target characteristics and the target kinematics and combining a threat field model established in a modeling stage;
(3) and (3) calculating an optimal path: in order to obtain the optimal path meeting the shortest driving time, the global threat field of the position where the vehicle is located is calculated in real time, the vehicle speed is continuously adjusted according to the safety constraint condition, and the optimal path meeting the shortest driving time is calculated. The specific process is as follows:
① sets a threat field threshold value, a safety threshold value L is set on the basis of a target threat field according to the formula (4)0。
②, carrying out numerical analysis and solving, namely discretizing the continuous model by adopting a numerical calculation method according to the requirement of the optimal function of the formula (5), and solving an optimal path with speed as shown in the formula (6).
In the formula,. DELTA.xi∈[-1,0)∪(0,1]，Δyi∈[-1,0)∪(0,1]；LvA threshold value representing a change in speed of the smart vehicle; path point (x)i+1,yi+1) Is in the previous position (x)i,yi) The speed variation of adjacent moments is within a certain range, so that sudden change of speed is avoided;
according to the above method, if the Path Path { (x) can be solvedi,yi,vi) If | i is 0,1,2, … …, n }, and n represents the number of route points, the solution is the optimal solution, so that the vehicle can be guaranteed to run most efficiently.
Iterative optimization: if the path cannot be solved, the driving speed of the vehicle needs to be readjusted. The vehicle is in (x)i+1,yi+1) Absolute value of the difference between the global threat field and the security threshold at
Less than a certain convergence value LconvgThe intelligent vehicle is in a safe area, and the speed of the vehicle can be properly increased in order to improve the driving efficiency. When absolute value
Greater than LconvgIndicating that the vehicle is in a threat zone and that the vehicle needs to be slowed down in order to reduce the risk of driving. The acceleration and deceleration mode is shown as formula (7):
in the formula, Deltav ∈ [1,5]]The velocity v calculated at each position takes the maximum value within its range. Sequentially substituting each speed value into an equation (3) to calculate a global threat field, and then solving an optimal Path { (x)i,yi,vi)|i＝0,1,2,……,n}。
Fig. 3 shows an example of a threat field formed by the object of the present invention. The speed of the target in the graph (a) is 0, a static threat field is formed, the field is uniformly distributed to the periphery by taking the target as the center, the threats on the positions with the same distance from the target are the same in size, and the threat field can be regarded as an equipotential threat circle; the farther away from the target, the less threat it is subjected to. When the object moves, a dynamic threat field is formed and the field is unevenly shifted. The greater the target velocity, the greater the degree of field offset. In the graph (b), the target runs at a low speed, the field deviates to the front of the movement, the deviation degree is small, and the threat field range after the movement is reduced. In the graph (c), the target runs at a high speed, the field deviates to the front of the movement, the deviation degree is large, and the threat field range after the movement is reduced.
Fig. 4 shows an example of the effect of the target threat field of the present invention on the path of a smart vehicle.
In (a), the vehicle is intelligent to
The driving, target speed is 0, the threat field generated is a uniform field centered on the target, the field range is continuously expanded. The dashed lines represent safe areas of the threat field and the solid lines represent unsafe areas of the threat field. If the path represents the intelligent vehicle driving path, the point p on the path1、p2、p3The distance from the target is increased in sequence, and the size of the threat field is p1＞p2＞p3The risk degree of the intelligent vehicle on the path is also p1＞p2＞p3. When the intelligent vehicle crosses P3After that point, the safe area is entered.
In (b), the intelligent vehicle and the target are respectively
And (4) driving, wherein the threat field generated by the target is deviated towards the moving direction of the target. If path represents the intelligent vehicle driving path, path point p1And p3Although the distance to the target is the same, the targets are threatened the same on the same equipotential circle; the threat field of each point on the path satisfies p2＞p1＝p3The danger degree of the intelligent vehicle on the path is p2＞p1＝p3。
The above examples are to be construed as merely illustrative and not limitative of the remainder of the disclosure. After reading the description of the invention, the skilled person can make various changes or modifications to the invention, and these equivalent changes and modifications also fall into the scope of the invention defined by the claims.
Claims (7)
1. An intelligent vehicle path planning method with safety constraint is characterized by comprising a modeling phase and an operation phase, wherein,
the modeling phase comprises the steps of: firstly, extracting environmental factors related to the safety of the intelligent vehicle; then, aiming at each target in the environment, constructing a static threat field according to environmental factors and target characteristics; then, the kinematic characteristics of each target are considered, a target dynamic threat field is constructed, and the threat fields of multiple targets are fused to obtain a fused global target dynamic threat field; finally, constructing a shortest time optimization objective function according to the safety constraint to establish a path planning model;
the operation phase comprises the following steps: firstly, acquiring environmental factors and target characteristic data in real time by using a vehicle-mounted sensor; calculating a global target dynamic threat field formed by multiple targets on a road to the intelligent vehicle by combining a threat field model established in a modeling stage; setting a safety threshold according to the safety requirement of vehicle running, dividing the global target dynamic threat field into a safe part and an unsafe part by taking the safety threshold as a constraint condition, and planning a shortest time path in the safe part.
2. The method for intelligent vehicle path planning with security constraints according to claim 1, wherein the extracting threat factors of the intelligent vehicle in the modeling stage specifically comprises: target characteristic factors, environmental factors, and target kinematic factors. Wherein the target feature refers to the type and size of the target, and p is usedcartpRepresents; environmental factors refer to the current climate conditions, in penvirRepresents; the kinematic factors of the object include the speed and position of the object, the speed of the object is a vector, and
indicating that the position of the target is (x)obs,yobs) And (4) showing.
3. The intelligent vehicle path planning method with safety constraints according to claim 2, wherein the modeling phase constructing a target static threat field specifically comprises: in a static scenario, a static threat field E of a single target is constructed, based on target characteristics, climate environment and target location, as shown in equation (1), which is a scalar field, independent of target velocity:
in the formula, kcartp、kenvir、kdisScale factors respectively representing target characteristics, climate environment and position distance; p is a radical ofcartp、penvirRespectively representing a target characteristic value and a climate environment value; (x, y) represents the location of the smart vehicle; (x)obs,yobs) Indicating the position of the target; e denotes the target static threat field. The larger the target type, the larger the threat field generated; the worse the weather conditions, the larger the threat field that is created.
4. The intelligent vehicle path planning method with safety constraints of claim 3, wherein the target dynamic threat field is constructed by adding target kinematic factors in combination with the motion state of the intelligent vehicle on the basis of a static threat field. The field is a vector field, and a dynamic threat field generated by a single target i to the intelligent vehicle
As shown in formula (2):
in the formula, kcar、kobsScale factors respectively representing the speed of the intelligent vehicle and the target speed;
representing the intelligent vehicle speed and the target speed; theta represents
With an angle to the coordinate axis x in the counter-clockwise direction,
a distance value representing the target and the intelligent vehicle;
representing a target dynamic threat field;
under the multi-target scene, the threat fields are fused in a superposition mode to obtain a global threat field
As shown in formula (3).
In the formula, m represents the target number.
5. The intelligent vehicle path planning method with safety constraints according to claim 4, wherein the setting method of the safety threshold specifically comprises: in order to ensure the driving safety, a safety threshold value is set on the basis of the global target threat field, the global target dynamic threat field is divided into a safe part and an unsafe part by taking the safety threshold value as a constraint condition, and if the intelligent vehicle drives at a position (x, y) with a speed v, the intelligent vehicle drives at the position (x, y) at the position (x) with a speed vobs,yobs) Has a speed of
Target of (1), L0If the safety threshold is set, the criterion for judging whether the traffic can be safely passed is as follows:
6. the method according to claim 5, wherein constructing the optimized objective function specifically comprises: in the safe passing area, in order to make the vehicle travel in the shortest time and optimize the performance of the planned path, an optimization objective function in the shortest time is established:
in the formula (x)0,y0) Denotes the starting point, (x)n,yn) Indicating an endpoint; (x, y) represents the smart vehicle position, v represents the speed at which the smart vehicle is traveling at (x, y); s represents the running length of the intelligent vehicle; t represents the total travel time from the start point to the end point; Δ x and Δ y represent the step size of the smart vehicle search in the x and y directions.
7. An intelligent vehicle path planning system with safety constraints, comprising:
a data acquisition module: the system is used for acquiring environmental factors, target characteristics and target kinematic data in real time by using a vehicle-mounted sensor in the vehicle driving process;
a threat field calculation module: constructing a static threat field of the target according to the environment and target characteristic data obtained by the data acquisition module, then adding the kinematic characteristics of each target, and calculating a dynamic threat field formed by multiple targets on the road to the intelligent vehicle; fusing the plurality of dynamic threat fields to obtain a global target dynamic threat field;
an optimal path calculation module: setting a safety threshold according to the safety requirement of vehicle running, and dividing the global target dynamic threat field into a safe part and an unsafe part by taking the safety threshold as a constraint condition; and then constructing a shortest time optimization objective function, and planning a shortest time path in a safe region.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911221205.1A CN111121804B (en) | 2019-12-03 | 2019-12-03 | Intelligent vehicle path planning method and system with safety constraint |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911221205.1A CN111121804B (en) | 2019-12-03 | 2019-12-03 | Intelligent vehicle path planning method and system with safety constraint |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111121804A true CN111121804A (en) | 2020-05-08 |
| CN111121804B CN111121804B (en) | 2023-09-26 |
Family
ID=70497299
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911221205.1A Active CN111121804B (en) | 2019-12-03 | 2019-12-03 | Intelligent vehicle path planning method and system with safety constraint |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111121804B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113178092A (en) * | 2021-03-16 | 2021-07-27 | 淮阴工学院 | Height limiting line selection method based on utility function |
| CN115797852A (en) * | 2023-02-09 | 2023-03-14 | 南京邮电大学 | Physical exercise event evaluation method based on discrete influence points |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070005609A1 (en) * | 1997-10-22 | 2007-01-04 | Intelligent Technologies International, Inc. | Vehicular Communication Arrangement and Method |
| US20090292468A1 (en) * | 2008-03-25 | 2009-11-26 | Shunguang Wu | Collision avoidance method and system using stereo vision and radar sensor fusion |
| US20140207364A1 (en) * | 2013-01-24 | 2014-07-24 | Volvo Car Corporation | Method and arrangement for determining a trajectory |
| WO2015175379A1 (en) * | 2014-05-10 | 2015-11-19 | Aurora Flight Sciences Corporation | Autonomous aerial vehicle collision avoidance system and method |
| US20150336575A1 (en) * | 2014-05-21 | 2015-11-26 | GM Global Technology Operations LLC | Collision avoidance with static targets in narrow spaces |
| CN105526942A (en) * | 2016-01-25 | 2016-04-27 | 重庆邮电大学 | Intelligent vehicle route planning method based on threat assessment |
| US20180308371A1 (en) * | 2017-04-19 | 2018-10-25 | Beihang University | Joint search method for uav multiobjective path planning in urban low altitude environment |
| CN108759839A (en) * | 2018-04-11 | 2018-11-06 | 哈尔滨工程大学 | A kind of unmanned vehicle paths planning method based on situation space |
| CN108762264A (en) * | 2018-05-22 | 2018-11-06 | 重庆邮电大学 | The dynamic obstacle avoidance method of robot based on Artificial Potential Field and rolling window |
| CN109063940A (en) * | 2018-02-05 | 2018-12-21 | 重庆邮电大学 | Intelligent vehicle threat estimating system and method based on structure changes Bayesian network |
| CN110031004A (en) * | 2019-03-06 | 2019-07-19 | 沈阳理工大学 | Unmanned plane static state and dynamic path planning method based on numerical map |
| CN110040134A (en) * | 2019-03-13 | 2019-07-23 | 重庆邮电大学 | Consider the vehicle collision time calculation method of environmental factor |
| US20190232954A1 (en) * | 2018-02-01 | 2019-08-01 | GM Global Technology Operations LLC | Systems and methods for collision detection in autonomous vehicles |
| US10421437B1 (en) * | 2018-12-19 | 2019-09-24 | Motorola Solutions, Inc. | System and method for dynamic perimeter threat detection for a movable vehicle |
| CN110470305A (en) * | 2019-08-23 | 2019-11-19 | 沈阳航空航天大学 | Unmanned plane avoidance path planning method under a kind of dynamic environment |
-
2019
- 2019-12-03 CN CN201911221205.1A patent/CN111121804B/en active Active
Patent Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070005609A1 (en) * | 1997-10-22 | 2007-01-04 | Intelligent Technologies International, Inc. | Vehicular Communication Arrangement and Method |
| US20090292468A1 (en) * | 2008-03-25 | 2009-11-26 | Shunguang Wu | Collision avoidance method and system using stereo vision and radar sensor fusion |
| US20140207364A1 (en) * | 2013-01-24 | 2014-07-24 | Volvo Car Corporation | Method and arrangement for determining a trajectory |
| WO2015175379A1 (en) * | 2014-05-10 | 2015-11-19 | Aurora Flight Sciences Corporation | Autonomous aerial vehicle collision avoidance system and method |
| US20150336575A1 (en) * | 2014-05-21 | 2015-11-26 | GM Global Technology Operations LLC | Collision avoidance with static targets in narrow spaces |
| CN105526942A (en) * | 2016-01-25 | 2016-04-27 | 重庆邮电大学 | Intelligent vehicle route planning method based on threat assessment |
| US20180308371A1 (en) * | 2017-04-19 | 2018-10-25 | Beihang University | Joint search method for uav multiobjective path planning in urban low altitude environment |
| US20190232954A1 (en) * | 2018-02-01 | 2019-08-01 | GM Global Technology Operations LLC | Systems and methods for collision detection in autonomous vehicles |
| CN109063940A (en) * | 2018-02-05 | 2018-12-21 | 重庆邮电大学 | Intelligent vehicle threat estimating system and method based on structure changes Bayesian network |
| CN108759839A (en) * | 2018-04-11 | 2018-11-06 | 哈尔滨工程大学 | A kind of unmanned vehicle paths planning method based on situation space |
| CN108762264A (en) * | 2018-05-22 | 2018-11-06 | 重庆邮电大学 | The dynamic obstacle avoidance method of robot based on Artificial Potential Field and rolling window |
| US10421437B1 (en) * | 2018-12-19 | 2019-09-24 | Motorola Solutions, Inc. | System and method for dynamic perimeter threat detection for a movable vehicle |
| CN110031004A (en) * | 2019-03-06 | 2019-07-19 | 沈阳理工大学 | Unmanned plane static state and dynamic path planning method based on numerical map |
| CN110040134A (en) * | 2019-03-13 | 2019-07-23 | 重庆邮电大学 | Consider the vehicle collision time calculation method of environmental factor |
| CN110470305A (en) * | 2019-08-23 | 2019-11-19 | 沈阳航空航天大学 | Unmanned plane avoidance path planning method under a kind of dynamic environment |
Non-Patent Citations (5)
| Title |
|---|
| STERLING J ANDERSON等: "Constraint-based planning and control for safe, semi-autonomous operation of vehicles", pages 383 - 388 * |
| ZHEN-HUA FAN等: "A novel dynamic Bayesian networkbased threat assessment algorithm", pages 611 - 615 * |
| 李春涛;易小芹;胡木;: "基于速度矢量场的无人机实时动态航路规划", 南京航空航天大学学报, no. 03 * |
| 李珣等: "物联网架构下的车辆动态避险路径规划方法研究", vol. 30, no. 5, pages 729 - 733 * |
| 邓顺平等: "一种基于威胁势场的A星路径规划算法", pages 6 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113178092A (en) * | 2021-03-16 | 2021-07-27 | 淮阴工学院 | Height limiting line selection method based on utility function |
| CN113178092B (en) * | 2021-03-16 | 2022-05-27 | 淮阴工学院 | Height limiting line selection method based on utility function |
| CN115797852A (en) * | 2023-02-09 | 2023-03-14 | 南京邮电大学 | Physical exercise event evaluation method based on discrete influence points |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111121804B (en) | 2023-09-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Stahl et al. | Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios | |
| CN106647769B (en) | Based on A*Extract AGV path trace and the avoidance coordination approach of pilot point | |
| CN107702716B (en) | Unmanned driving path planning method, system and device | |
| Wei et al. | A behavioral planning framework for autonomous driving | |
| WO2018176593A1 (en) | Local obstacle avoidance path planning method for unmanned bicycle | |
| CN112378408A (en) | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot | |
| CN111679678B (en) | Track planning method and system for transverse and longitudinal separation and computer equipment | |
| CN110398967B (en) | Multi-robot cooperative track information processing method adopting discretization method | |
| CN106371439B (en) | Unified automatic driving transverse planning method and system | |
| CN112577506B (en) | Automatic driving local path planning method and system | |
| CN115079705A (en) | Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm | |
| CN111121804A (en) | Intelligent vehicle path planning method and system with safety constraint | |
| CN115257724A (en) | Safety and energy-saving decision control method and system for plug-in hybrid electric vehicle | |
| CN111896004A (en) | Narrow passage vehicle track planning method and system | |
| Chen et al. | Path planning for autonomous vehicle based on a two‐layered planning model in complex environment | |
| CN114237256B (en) | Three-dimensional path planning and navigation method suitable for under-actuated robot | |
| Shang et al. | Research on path planning of autonomous vehicle based on RRT algorithm of Q-learning and obstacle distribution | |
| Liang et al. | Economic MPC-based planning for marine vehicles: Tuning safety and energy efficiency | |
| CN114460933B (en) | Dynamic environment-oriented mobile robot local path planning algorithm | |
| CN208061025U (en) | A kind of automatic driving vehicle avoidance obstacle device based on the paths Dubins | |
| Meng et al. | A navigation framework for mobile robots with 3D LiDAR and monocular camera | |
| Rebollo et al. | Collision avoidance among multiple aerial robots and other non-cooperative aircraft based on velocity planning | |
| Dong et al. | Path Planning Research for Outdoor Mobile Robot | |
| Ma et al. | Overtaking Path Planning for CAV based on Improved Artificial Potential Field | |
| Shi et al. | Local path planning of unmanned vehicles based on improved RRT algorithm |
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 |