CN114647245A - Unmanned vehicle curve obstacle avoidance path planning method based on artificial potential field - Google Patents
Unmanned vehicle curve obstacle avoidance path planning method based on artificial potential field Download PDFInfo
- Publication number
- CN114647245A CN114647245A CN202210295423.5A CN202210295423A CN114647245A CN 114647245 A CN114647245 A CN 114647245A CN 202210295423 A CN202210295423 A CN 202210295423A CN 114647245 A CN114647245 A CN 114647245A
- Authority
- CN
- China
- Prior art keywords
- obstacle
- vehicle
- main vehicle
- potential field
- obstacle avoidance
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000004888 barrier function Effects 0.000 claims description 12
- 239000002131 composite material Substances 0.000 claims description 10
- 238000005457 optimization Methods 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000002457 bidirectional effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000003708 edge detection Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000003709 image segmentation Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
The invention relates to an unmanned vehicle curve obstacle avoidance path planning method based on an artificial potential field, and belongs to the technical field of unmanned vehicle driving. The method comprises the steps of judging whether a main vehicle is trapped into local optimization according to resultant force on the basis of constructing an artificial resultant force field, solving the local optimization problem in a circle-around obstacle avoidance 1 mode, solving the problem that a next track point planned is located in an obstacle expansion range in a circle-around obstacle avoidance 2 mode, and adjusting the speed of the main vehicle according to resultant force in the circle-around obstacle avoidance process so as to guarantee vehicle safety.
Description
Technical Field
The invention relates to an unmanned vehicle curve obstacle avoidance path planning method based on an artificial potential field, and belongs to the technical field of unmanned vehicle driving.
Background
The path planning, as a core technology in the field of unmanned vehicle research, refers to finding a collision-free path from a starting point to a target point in a dynamic environment, and may be specifically expressed as: given the geometric shape and dynamic model of the vehicle, the environmental information of the vehicle, an initial state and a target state set, a feasible trajectory which is free of collision and meets the dynamic and geometric constraints of the vehicle is calculated, and a planning result is output to a motion control layer.
The artificial potential field method is initially applied to path planning of a robot, and the problems of target unreachability and local optimization exist. The scheme for solving the problem in the prior art mainly comprises two categories, namely, the distance of a target point is introduced into a repulsive force field to escape from the local optimum, and when the unmanned vehicle falls into the local optimum, an additional virtual force is added to break a balance state so as to force the unmanned vehicle to jump out of the local optimum. However, when the artificial potential field is applied to obstacle avoidance of the unmanned vehicle, not only the limitation of the algorithm but also the size of the obstacle and the problem of the kinematics and dynamics constraint of the unmanned vehicle need to be considered. For example, chinese patent publication No. CN112344943A discloses an intelligent vehicle route planning method for improving an artificial potential field algorithm, which determines a moving point escaping from a local optimum problem through probability estimation, and compares and judges whether a local minimum point can be escaped according to the potential field size of a minimum point.
Disclosure of Invention
The invention aims to provide an unmanned vehicle curve obstacle avoidance path planning method based on an artificial potential field, which is used for solving the local optimal problem existing when an artificial potential field rule marks an unmanned vehicle curve obstacle avoidance path.
In order to achieve the purpose, the invention provides an unmanned vehicle curve obstacle avoidance path planning method based on an artificial potential field, which comprises the following steps:
s1, acquiring the main vehicle information and the obstacle information, and determining the position of a target point; the host vehicle information includes a host vehicle position and a host vehicle velocity, and the obstacle information includes an obstacle position;
s2, establishing a follow-up rectangular coordinate system which follows the movement of the host vehicle, establishing a target point gravitational potential field which is used for representing the influence of the target point position on the movement of the host vehicle according to the position of the host vehicle and the position of the target point in the follow-up rectangular coordinate system, and establishing an obstacle repulsive potential field which is used for representing the influence of the obstacle on the movement of the host vehicle according to the position of the host vehicle, the position of the obstacle and the position of the target point;
s3, calculating the resultant force of the main vehicle in a composite field formed by combining the attraction potential field of the target point and the repulsion potential field of the obstacle;
s4 and S4, if the resultant force is not 0, the position of the main vehicle is taken as the center of a circle, the set step length is taken as the radius to be taken as a first reference circle, the main vehicle is taken as a starting point, and the point of the main vehicle, which points to the first reference circle along the direction of the resultant force, is taken as the next track point p of the main vehicle motion1According to the locus p1Determining an obstacle avoidance path of the main vehicle, and controlling the main vehicle to run according to the obstacle avoidance path to avoid obstacles;
if the resultant force is 0, taking the position of the main vehicle as the center of a circle, setting the step length as the radius to form a second reference circle, taking the obstacle as the center of a circle, taking the distance between the main vehicle and the obstacle as the radius to form a third reference circle, and taking the intersection point of the second reference circle and the third reference circle as the next track point p of the main vehicle motion2According to the locus p2And determining an obstacle avoidance path of the main vehicle, and controlling the main vehicle to run according to the obstacle avoidance path to avoid the obstacle.
The method comprises the steps of judging whether a resultant force of a potential field is 0 or not, and exciting a round-trip obstacle avoidance 1 to enable a main vehicle to escape from local optimization. Obstacle 1 is kept away around the circle: and taking the obstacle as the center of a circle, taking the distance between the main vehicle and the obstacle as a radius to form a circle, determining a point on the circle, which is away from the main vehicle by a set step length, as a next track point, and driving the main vehicle along an arc between the current position and the track point. The problem of local optimum can be solved by avoiding the obstacle 1 around the circle, and the reliability of the unmanned vehicle obstacle avoidance path planning is improved.
Further, in the above method, in step S4, when the resultant force is not 0, if the locus point p is a locus point p1Within the expansion range of the barrier, the barrier is directed to the track point p1And a point located on the third reference circle as the next locus point p of the host vehicle motion3According to the locus p3Determining an obstacle avoidance path of the main vehicle, and controlling the main vehicle to run according to the obstacle avoidance pathObstacle avoidance is carried out; and amplifying the size of the obstacle according to a set proportion to obtain the expansion range.
Considering the uncertainty of the movement of the obstacle, the expansion range is obtained by enlarging the size of the obstacle, if the trajectory point p is planned by the artificial potential field method1In the expansion range, the obstacle may influence the obstacle avoidance process of the main vehicle, and the track points of the obstacle avoidance of the main vehicle are re-planned by exciting the round obstacle avoidance 2. Obstacle 2 is kept away around the circle to indicate: the position of the obstacle is used as the center of a circle, the distance between the main vehicle and the obstacle is used as the radius to form a circle, and the circle passes through the center of the circle and the track point p1The intersection point of the straight line of (c) and the circle is used as a new track point p3The current position and the track point p of the main vehicle edge3The arc between them. The safety of the unmanned vehicle in obstacle avoidance can be improved by avoiding the obstacle 2 around the circle.
Further, in the above method, the obstacle information further includes an obstacle velocity, in step S2, an obstacle velocity repulsive potential field for characterizing the influence of the obstacle velocity on the movement of the host vehicle is established according to the obstacle velocity information in a follow-up rectangular coordinate system, and in step S3, a resultant force of the host vehicle in a composite field formed by combining the target point attractive potential field, the obstacle repulsive potential field, and the obstacle velocity repulsive potential field is calculated.
Further, in the above method, the obstacle velocity repulsive potential field Urev(X) is represented by the following formula:
in the formula, kvIs a proportional gain factor, v-v0Is the relative speed of the host vehicle and the obstacle, v is the current speed of the host vehicle, v0The current speed of the obstacle, the repulsive force of the speed of the obstacle and the direction of the relative speed are opposite, alpha is the included angle between the direction of the relative speed and the connecting line between the main vehicle position and the obstacle position, and rhoobIs the distance between the main vehicle and the obstacle, and A is the longitudinal influence distance of the obstacle.
Further, in the above method, road boundary information is also obtained, in step S2, a road boundary repulsive potential field for characterizing the influence of the road boundary on the movement of the host vehicle is also established according to the road boundary information in a follow-up rectangular coordinate system, and in step S3, the resultant force of the host vehicle in a composite field formed by the combination of the target point attractive potential field, the obstacle repulsive potential field, the obstacle speed repulsive potential field and the road boundary repulsive potential field is calculated.
The influence of road boundary on the main vehicle in the obstacle avoidance process is considered, so that the road boundary potential field is set according to the road boundary information, the composite field is further constructed, and the reliability of the main vehicle obstacle avoidance path planning can be improved.
Further, in the above method, the road boundary repulsive force potential field Uroad(X) is represented by the following formula:
in the formula, kroadIs the road boundary repulsion coefficient, | | ri-rlI is the vertical distance from the host vehicle to the road boundary, riIs the abscissa, r, of the position of the master vehicle in the following rectangular coordinate systemlIs the longitudinal coordinate of the position of the main vehicle in the follow-up rectangular coordinate system, W is the width of the main vehicle body, k0The distance is influenced for the road boundary.
Further, in the above method, sensor blind zone boundary information is also obtained, in step S2, a sensor blind zone repulsive force field for representing the influence of a sensor acquisition blind zone on the movement of the host vehicle is also established according to the sensor blind zone boundary in the follow-up rectangular coordinate system, and in step S3, the resultant force of the host vehicle in a composite field formed by the target point attractive force field, the obstacle repulsive force field, the obstacle velocity repulsive force field, the road boundary repulsive force field and the sensor blind zone repulsive force field is calculated.
In the obstacle avoidance process of the main vehicle, a blind area exists when the sensor collects lane environment information, and other obstacles possibly exist in the blind area of the sensor, so that a sensor blind area repulsive force field is set, a composite field is further constructed, and the reliability of the obstacle avoidance path planning of the main vehicle can be improved.
Further, in the above method, the sensor blind area repels the potential field Ur-blind(X) is represented by the following formula:
in the formula, Kr-blindIs the front sensor blind area repulsion coefficient, | | ri-rbI is the vertical distance between the boundary of the sensor blind area and the host vehicle, v is the current speed of the host vehicle, the direction is the same as the direction of the repulsion force of the obstacle, and thrIs the threat level of a dangerous obstacle in the dead zone of the sensor.
Further, in the method, the target speed of the main vehicle for avoiding the obstacle is determined according to the resultant force, and the target speed is used as a control target of the main vehicle for avoiding the obstacle, so that the main vehicle is controlled to run according to the obstacle avoiding path for avoiding the obstacle.
In the obstacle avoidance process of the main vehicle, the speed of the main vehicle is controlled according to the resultant force borne by the main vehicle, so that the speed of the main vehicle is more consistent with a planned obstacle avoidance path, and the safety in obstacle avoidance is improved.
Further, in the method, before the host vehicle runs for avoiding the obstacle, if the distance between the host vehicle and the obstacle is smaller than the set safe distance, the host vehicle is controlled to decelerate.
Before the obstacle avoidance driving, the speed of the main vehicle is controlled according to the distance between the main vehicle and the obstacle, so that the collision between the main vehicle and the obstacle is avoided.
Drawings
FIG. 1 is a flow chart of an unmanned vehicle curve obstacle avoidance path planning method based on an artificial potential field in the method embodiment of the invention;
FIG. 2 is a schematic diagram of a servo rectangular coordinate system according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of a sensor detecting a blind area in an embodiment of the method of the present invention;
FIG. 4 is a schematic illustration of the main vehicle under stress in an embodiment of the method of the present invention;
FIG. 5 is a schematic diagram of a method 1 for avoiding obstacles around a circle according to an embodiment of the present invention;
fig. 6 is a schematic diagram of a method 2 for avoiding an obstacle around a circle in an embodiment of the method of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments.
The method comprises the following steps:
the unmanned vehicle curve obstacle avoidance path planning method based on the artificial potential field, as shown in fig. 1, comprises the following steps:
and S1, acquiring the information of the running environment of the main vehicle (namely the unmanned vehicle), the obstacle information and the main vehicle information through a vision sensor, vehicle-mounted equipment, vehicle and infrastructure communication technology (V2I) and a differential GPS/INS navigation system.
The information of the running environment of the main vehicle is road environment information acquired in real time through roadside sensing equipment and a V2I technology, and comprises the width of a lane line on a road where the main vehicle is located, the number of the lane lines and the distance between the main vehicle and the lane lines on two sides. The method comprises the steps of collecting barrier information around the running environment of the main vehicle through cameras arranged on the periphery of the main vehicle body, laser radars arranged on the top of the main vehicle body and millimeter wave radars arranged on a bumper, and classifying and fusing the collected barrier information. The obstacles are usually other vehicles on the road, and the obstacle information includes the number of obstacles, the speed of the obstacles, and the positions of the obstacles. The information of the main car comprises the position of the main car and the current speed of the main car, and the information of the main car also comprises the distance between the main car and the boundary of the sensor blind area because the camera, the laser radar, the millimeter wave radar and other devices have blind acquisition areas, namely the sensor blind area.
And S2, judging whether the main vehicle needs to avoid the obstacle or not through the safe distance, if the distance between the main vehicle and the obstacle is smaller than the safe distance, the main vehicle is considered to be insufficient to avoid the obstacle, and the speed of the main vehicle is controlled to be reduced. And when the speed of the main vehicle after the deceleration meets the formula, the distance between the main vehicle and the obstacle is considered to reach the safe distance, the obstacle avoidance of the main vehicle is judged, and the step S3 is executed.
In the formula, X0For a safe distance, vP(0) Is the initial velocity of the main vehicle before deceleration, vQ(0) Initial velocity of obstacle, aP(τ) is the acceleration during deceleration of the host vehicle, aQ(τ) is the obstacle acceleration, and t is the time required from the detection of the obstacle until the host vehicle will collide with the obstacle.
S3, establishing a follow-up rectangular coordinate system, and establishing a target point attraction potential field, an obstacle repulsion potential field, an obstacle speed repulsion potential field, a road boundary repulsion potential field and a sensor blind area repulsion potential field in the follow-up rectangular coordinate system.
S3.1, as shown in FIG. 2, a new rectangular coordinate system x ' o ' y ' is established as a follow-up rectangular coordinate system according to a projection point of the center of the rear axle of the main vehicle on the ground and the boundary line of the outer edge lane, and the follow-up rectangular coordinate system moves along with the movement of the main vehicle. By the following formula, an arbitrary position (x, y) under the ground coordinate system xoy can be transformed into the following rectangular coordinate system, and a position (x ', y') under the following rectangular coordinate system is obtained.
In the formula, theta is the included angle between the x axis and the x' axis, (x)start，ystart) Is the starting point position of the lane line at the outer edge of the left lane.
S3.2, establishing an obstacle repulsive force field. Obstacle repulsive force potential field Urep(X) is represented by the following formula:
where η is the gain of the repulsive field function, ρobIs the distance between the host vehicle and the obstacle, pgN is an arbitrary real number greater than zero, and A is the longitudinal influence distance of the obstacle.
In order to improve the safety of obstacle avoidance of the unmanned vehicle, the value of eta is further designed by considering the influence of the size, the quality and the shape of the obstacle, and the eta is designed as follows:
in the formula, M is the mass of the obstacle, and the obstacle is other vehicles, so M is the mass of the obstacle vehicle, e is a natural logarithm, p is a front vehicle type, and G is an adjusting base number, and is used for preventing the gain of the repulsive force field function from being too large.
Obstacle repulsive force Frep(X) is represented by the following formula:
in the formula, a0Unit vector, p, for the principal direction of the obstacleobIs the distance between the host vehicle and the obstacle, A is the longitudinal influence distance of the obstacle, B is the transverse influence distance of the obstacle, (x)1，y1) And the position coordinates of the obstacle in the follow-up rectangular coordinate system. Frep1And Frep2Expressed by the following formula:
where η is the gain of the repulsive field function, pobIs the distance between the host vehicle and the obstacle, ρgIs the distance between the host vehicle and the target point, n is oneAny real number greater than zero, a being the longitudinal influence distance of the obstacle.
The longitudinal influence distance B of an obstacle is represented by the following formula:
B＝β(H-c)/2
in the formula, beta is a safety factor, H is the distance between the geometric center line of the driveway where the unmanned vehicle is located and the geometric center line of the driveway where the obstacle is located, and c is the width of the obstacle vehicle.
When the unmanned vehicle runs in the bidirectional lane, vehicles in adjacent lanes do not have running danger to the unmanned vehicle, and at the moment, the repulsive force of the vehicles in the adjacent lanes is ignored, and the repulsive force of the same-direction lanes is only considered.
And S3.3, establishing a target point gravitational potential field. Target point gravitational potential field Uatt(X) is represented by the following formula:
in the formula, kattFor the gain of the gravitational field function, pgIs the distance between the host vehicle and the target point, ρ0Is a target point gravitational force threshold value, U'attThe maximum gravitational potential field for the target point.
In order to improve the safety of obstacle avoidance of the unmanned vehicle, a method of designing sectional attraction is adopted, and when the distance between the unmanned vehicle and a target point is larger than a threshold value p of attraction action of the target point0In time, the attraction force of the target point to the host vehicle is the target point maximum attraction force F'attThe direction of the gravitation changes along with the movement direction of the main vehicle; when the distance between the host vehicle and the target point is less than the threshold value p for the gravitational action of the target point0While the attraction of the target point to the host vehicle is along with rhogIs changed, the direction is directed by the host vehicle to the target point. Target point gravitational force Fatt(X) is represented by the following formula:
in the formula, kattFor the gain of the gravitational field function, pgIs a main vehicle and eyesDistance between punctuation points, p0Is a target point gravitational force action threshold value, F'attThe target point maximum gravity.
And S3.4, establishing a road boundary repulsive force field. The method comprises the steps of acquiring and processing lane information through a visual sensor, wherein the lane information comprises image preprocessing, image segmentation, edge detection and lane line recognition to obtain road boundary information, and establishing a road boundary potential field according to the position of a main vehicle and the road boundary information. Road boundary potential field Uroad(X) is represented by the following formula:
in the formula, kroadIs the road boundary repulsion coefficient, | | ri-rlI is the vertical distance from the host vehicle to the road boundary, riIs the abscissa, r, of the position of the center of mass of the principal vehiclelIs the ordinate of the position of the center of mass of the main vehicle, W is the width of the main vehicle body, k0The distance is influenced for the road boundary.
Road boundary repulsive force Froad(X) is represented by the following formula:
and S3.5, establishing a sensor blind area repulsive force field. And establishing a sensor blind area repulsive force field according to the condition that whether the detected blind area has the obstacle or not. When a dangerous obstacle is present in a blind area, the threat level t of the potential dangerous obstacle is designed as shown in fig. 3hr1, adding a sensor blind area repulsive force field model; designing the threat level t of the potential dangerous barrier when the dangerous barrier does not exist in the blind areahrThe repulsive potential field model is ignored as 0.
Sensor blind area repulsion potential field model Ur-blind(X) is represented by the following formula:
sensor blind area repulsion Fr-blind(X) is represented by the following formula:
in the formula, Kr-blindIs the front sensor blind area repulsion coefficient, | | ri-rbAnd | l is the vertical distance between the sensor blind area boundary and the host vehicle, v is the current vehicle speed of the host vehicle, and the direction is the same as the direction of the repulsion force of the obstacles. If the host vehicle is not decelerated in step S2, the current vehicle speed v of the host vehicle is vP(0) If the host vehicle decelerates in step S2, the host vehicle is decelerated based on the initial velocity v before the host vehicle deceleratesP(0) And acceleration a during deceleration of the host vehiclePAnd (tau), calculating to obtain the current speed v of the host vehicle.
And S3.6, establishing an obstacle velocity repulsive potential field. Barrier velocity repulsive force potential field Urev(X) is represented by the following formula:
obstacle velocity repulsive force Frev(X) is represented by the following formula:
in the formula, kvIs a proportional gain factor, v-v0Is the relative speed of the host vehicle and the obstacle, v is the current speed of the host vehicle, v0The direction of the repulsive force of the barrier speed is opposite to the direction of the relative speed, and alpha is an included angle between the direction of the relative speed and a vector formed by the position points of the main vehicle and the barrier.
S4, generating a resultant force field of the host vehicle according to the virtual force fields obtained in the step S3, wherein the resultant force field U is shown in FIG. 4total(X) by the following formulaThe following steps:
wherein n is the number of obstacles.
And calculating the magnitude of resultant force according to the generated resultant force field, determining the direction of the resultant force, enabling the main vehicle to avoid obstacles along the direction of the resultant force, and rolling and refreshing the information of the main vehicle and the information of obstacles in real time according to the refreshing frequency of each sensor. The resultant force F generatedtotal(X) is represented by the following formula:
s5, setting the current position of the main vehicle as p0And judging according to the resultant force obtained in the step S4, and avoiding the obstacle by adopting a corresponding obstacle avoiding method.
If the generated resultant force is not 0, the main vehicle does not fall into the local optimum, and the next track point p of the main vehicle motion is planned according to the resultant force at the moment1. By master position p0As the circle center, the unit step length h is the radius to make a circle, and the point of the main vehicle pointing to the circle along the resultant force direction and positioned on the circle is determined as the next track point p1。
If the generated resultant force is 0, the main vehicle falls into the local optimum, and the resultant forces at all positions around the local optimum point all point to the local optimum point, so that the unmanned vehicle oscillates around the local optimum point and cannot walk out of the area by itself. At this time, the round obstacle avoidance 1 is excited to escape from the local optimum problem. As shown in fig. 5, the obstacle avoidance around the circle 1 means: taking the current position of the obstacle as the center of a circle, taking the distance from the main vehicle to the obstacle as the radius to make a circle, and determining the point on the circle with the distance h from the current position of the main vehicle as the next track point p according to the unit step length h2The main vehicle reaches the next track point p along the current position2The arc between them is driven to realize avoiding the barrier 1 around the circle. And an interval delta t is also set, and the position coordinates of the main vehicle and the obstacle are updated in real time according to newly acquired information, so that the real-time performance of the planned path is ensured.
When the next track point p of the main vehicle motion is planned by adopting an artificial potential field method1And then, the obstacle is expanded, and the expansion is carried out according to 1.25 times of the length and the width of the obstacle by considering the size of the main vehicle, so that the obstacle range is determined. If p is1And when the distance is within the range of the obstacle, the obstacle 2 is avoided around the circle.
As shown in fig. 6, the obstacle avoidance around the circle 2 refers to: determining a center-passing point and a point p by taking the position of the obstacle as the center of a circle and the distance between the main vehicle and the obstacle as a radius1The position of the straight line on the circle is used as the next track point p3. The main vehicle reaches the next track point p along the current position3And the arc between the two obstacle avoidance devices drives around the circle to avoid the obstacle 2. When p is reached3And when the points are counted, the obstacle avoidance 2 around the circle is quitted, and the next track point is planned according to the artificial potential field.
In the obstacle avoidance process, each track point is fitted through a smooth curve, so that a planned path for the main vehicle to travel in the obstacle avoidance process is obtained.
And S6, when the main vehicle avoids the obstacle, the vehicle speed is restrained, when the main vehicle drives into the obstacle repulsion force range, the speed of the main vehicle is controlled according to the magnitude of the resultant force borne by the main vehicle, so that the vehicle speed is reduced, the safety during obstacle avoidance is improved, and when the main vehicle leaves the obstacle repulsion force range, the vehicle speed before obstacle avoidance is recovered.
The velocity plan is expressed by the following formula:
Ftotal(X)＝KF
F＝ma
wherein v' is the speed of the main vehicle when the main vehicle avoids the obstacle, K is the coefficient for converting the virtual resultant force borne by the main vehicle in the artificial potential field into the real stress in the speed direction of the unmanned vehicle, T is the time for the main vehicle to avoid the obstacle, and rhoobThe distance between the main vehicle and the obstacle is v, the current speed of the main vehicle before obstacle avoidance is v, the mass of the main vehicle is m, and the deceleration of the main vehicle when the obstacle avoidance is a.
By adopting the method, the artificial potential field is constructed, the resultant potential field is generated, the track point of the next movement is planned according to the resultant potential field, whether the unmanned vehicle falls into the local optimum is judged according to the resultant force, the problem that the artificial potential field falls into the local optimum is solved by using the circle-around obstacle avoidance 1, the problem that the planned track point is located in the expansion range of the obstacle is solved by using the circle-around obstacle avoidance 2, and in the process of the circle-around obstacle avoidance, the speed of the main vehicle is planned according with the planned obstacle avoidance path through the resultant force, so that the safety in obstacle avoidance is improved, and the safe obstacle avoidance of the unmanned vehicle is realized.
Claims (10)
1. An unmanned vehicle curve obstacle avoidance path planning method based on an artificial potential field is characterized by comprising the following steps:
s1, acquiring the main vehicle information and the obstacle information, and determining the position of a target point; the host vehicle information includes a host vehicle position and a host vehicle velocity, and the obstacle information includes an obstacle position;
s2, establishing a follow-up rectangular coordinate system which follows the movement of the host vehicle, establishing a target point gravitational potential field which is used for representing the influence of the target point position on the movement of the host vehicle according to the position of the host vehicle and the position of the target point in the follow-up rectangular coordinate system, and establishing an obstacle repulsive potential field which is used for representing the influence of the obstacle on the movement of the host vehicle according to the position of the host vehicle, the position of the obstacle and the position of the target point;
s3, calculating the resultant force of the main vehicle in a composite field formed by combining the attraction potential field of the target point and the repulsion potential field of the obstacle;
s4, if the resultant force is not 0, setting the step length as the radius as the first reference circle by taking the position of the main vehicle as the center of the circle, and taking the main vehicle as the starting point, taking the point of the main vehicle pointing to the first reference circle along the direction of the resultant force as the next track point p of the main vehicle motion1According to the locus p1Determining an obstacle avoidance path of the main vehicle, and controlling the main vehicle to run according to the obstacle avoidance path to avoid obstacles;
if the resultant force is 0, taking the position of the main vehicle as the center of a circle, setting the step length as the radius to be used as a second reference circle, taking the obstacle as the center of a circle, taking the distance between the main vehicle and the obstacle as the radius to be used as a third reference circle, and taking the intersection point of the second reference circle and the third reference circle as the main circleNext point of track p of the vehicle movement2According to the locus p2And determining an obstacle avoidance path of the main vehicle, and controlling the main vehicle to run according to the obstacle avoidance path to avoid the obstacle.
2. The method for planning an unmanned aerial vehicle curve obstacle avoidance path based on artificial potential field as claimed in claim 1, wherein in step S4, if the resultant force is not 0, if the locus point p is a track point p1Within the expansion range of the barrier, the barrier is directed to the track point p1And a point located on the third reference circle as the next locus point p of the host vehicle motion3According to the locus p3Determining an obstacle avoidance path of the main vehicle, and controlling the main vehicle to run according to the obstacle avoidance path to avoid obstacles; and amplifying the size of the obstacle according to a set proportion to obtain the expansion range.
3. An unmanned vehicle curve obstacle avoidance path planning method according to claim 1, wherein the obstacle information further includes an obstacle speed, in step S2, an obstacle speed repulsive potential field for representing the influence of the obstacle speed on the movement of the host vehicle is established according to the obstacle speed information in a follow-up rectangular coordinate system, and in step S3, a resultant force of the host vehicle in a composite field formed by combining the target point gravitational potential field, the obstacle repulsive potential field and the obstacle speed repulsive potential field is calculated.
4. The unmanned vehicle curve obstacle avoidance path planning method based on the artificial potential field as claimed in claim 3, wherein an obstacle velocity repulsive potential field U is adoptedrev(X) is represented by the following formula:
in the formula, kvIs a proportional gain factor, v-v0Is the relative speed of the host vehicle and the obstacle, v is the current speed of the host vehicle, v0Of obstacle present speed, obstacle speed repulsive force and relative speedThe directions are opposite, alpha is the included angle between the direction of the relative speed and the connecting line between the main vehicle position and the obstacle position, and rhoobIs the distance between the main vehicle and the obstacle, and A is the longitudinal influence distance of the obstacle.
5. An unmanned vehicle curve obstacle avoidance path planning method according to claim 3, characterized in that road boundary information is further acquired, in step S2, a road boundary repulsive force field for representing the influence of the road boundary on the motion of the host vehicle is established according to the road boundary information in a follow-up rectangular coordinate system, and in step S3, the resultant force of the host vehicle in a composite field formed by the combination of a target point attractive force field, an obstacle repulsive force field, an obstacle velocity repulsive force field and the road boundary repulsive force field is calculated.
6. The unmanned vehicle curve obstacle avoidance path planning method based on artificial potential field as claimed in claim 5, wherein road boundary repulsive potential field Uroad(X) is represented by the following formula:
in the formula, kroadIs the road boundary repulsion coefficient, | | ri-rlI is the vertical distance from the host vehicle to the road boundary, riIs the abscissa, r, of the position of the master vehicle in the following rectangular coordinate systemlIs the longitudinal coordinate of the position of the main vehicle in the follow-up rectangular coordinate system, W is the width of the main vehicle body, k0The distance is influenced for the road boundary.
7. A curve obstacle avoidance path planning method for an unmanned aerial vehicle based on an artificial potential field as claimed in claim 5, wherein sensor blind area boundary information is also obtained, in step S2, a sensor blind area repulsive force field for representing the influence of a sensor acquisition blind area on the motion of the main vehicle is also established according to the sensor blind area boundary in the follow-up rectangular coordinate system, and in step S3, the resultant force of the main vehicle in a composite field formed by the combination of a target point attractive force potential field, an obstacle repulsive force potential field, an obstacle velocity repulsive force field, a road boundary repulsive force field and the sensor blind area repulsive force field is calculated.
8. The unmanned vehicle curve obstacle avoidance path planning method based on the artificial potential field as claimed in claim 7, wherein a sensor blind area repulsive potential field Ur-blind(X) is represented by the following formula:
in the formula, Kr-blindIs the front sensor blind area repulsion coefficient, | | ri-rbI is the vertical distance between the boundary of the sensor blind area and the host vehicle, v is the current speed of the host vehicle, the direction is the same as the direction of the repulsion force of the obstacle, and thrIs the threat level of a dangerous obstacle in the dead zone of the sensor.
9. The unmanned vehicle curve obstacle avoidance path planning method based on the artificial potential field as claimed in claim 1 or 2, wherein a target speed of the main vehicle for obstacle avoidance is determined according to the resultant force, the target speed is used as a control target of the main vehicle for obstacle avoidance, and the main vehicle is controlled to run according to the obstacle avoidance path for obstacle avoidance.
10. The unmanned vehicle curve obstacle avoidance path planning method based on the artificial potential field as claimed in claim 1 or 2, wherein before the main vehicle is in obstacle avoidance, if the distance between the main vehicle and the obstacle is less than the set safe distance, the main vehicle is further controlled to decelerate.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210295423.5A CN114647245A (en) | 2022-03-23 | 2022-03-23 | Unmanned vehicle curve obstacle avoidance path planning method based on artificial potential field |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210295423.5A CN114647245A (en) | 2022-03-23 | 2022-03-23 | Unmanned vehicle curve obstacle avoidance path planning method based on artificial potential field |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114647245A true CN114647245A (en) | 2022-06-21 |
Family
ID=81995848
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210295423.5A Pending CN114647245A (en) | 2022-03-23 | 2022-03-23 | Unmanned vehicle curve obstacle avoidance path planning method based on artificial potential field |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114647245A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117093005A (en) * | 2023-10-16 | 2023-11-21 | 华东交通大学 | Autonomous obstacle avoidance method for intelligent automobile |
-
2022
- 2022-03-23 CN CN202210295423.5A patent/CN114647245A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117093005A (en) * | 2023-10-16 | 2023-11-21 | 华东交通大学 | Autonomous obstacle avoidance method for intelligent automobile |
| CN117093005B (en) * | 2023-10-16 | 2024-01-30 | 华东交通大学 | Autonomous obstacle avoidance method for intelligent automobile |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11809194B2 (en) | Target abnormality determination device | |
| CN109885066B (en) | Motion trail prediction method and device | |
| EP3803828B1 (en) | Probabilistic object tracking and prediction framework | |
| CN112193244B (en) | Automatic driving vehicle motion planning method based on linear constraint | |
| Cosgun et al. | Towards full automated drive in urban environments: A demonstration in gomentum station, california | |
| US9868443B2 (en) | Reactive path planning for autonomous driving | |
| CN105809130B (en) | Vehicle travelable area calculation method based on binocular depth perception | |
| JP6959056B2 (en) | Mobile robot control device and control method | |
| WO2022053026A1 (en) | Automatic driving meeting scene processing method and apparatus, vehicle, and storage medium | |
| CN112644486B (en) | Intelligent vehicle obstacle avoidance trajectory planning method based on driving safety field | |
| CN113848914B (en) | Method for planning local path by collision coefficient artificial potential field method in dynamic environment | |
| EP3794573B1 (en) | Navigating congested environments with risk level sets | |
| Fassbender et al. | Trajectory planning for car-like robots in unknown, unstructured environments | |
| KR20220110069A (en) | Sampling-based maneuver realizer | |
| CN111554081B (en) | Multi-level leader pigeon group theory-based fleet intersection obstacle avoidance control method | |
| Guo et al. | Humanlike behavior generation in urban environment based on learning-based potentials with a low-cost lane graph | |
| CN114647245A (en) | Unmanned vehicle curve obstacle avoidance path planning method based on artificial potential field | |
| CN114228707B (en) | Anti-collision method and system for unmanned vehicle | |
| Narksri et al. | Crossing blind intersections from a full stop using estimated visibility of approaching vehicles | |
| CN113296521B (en) | Dynamic window-based local obstacle avoidance method for obstacle | |
| JP7304875B2 (en) | AUTOMATIC OPERATION CONTROL METHOD AND AUTOMATIC OPERATION CONTROL SYSTEM | |
| KR102355426B1 (en) | Method and apparatus for detecting and avoiding obstacles on driving path | |
| JP6982405B2 (en) | Vehicle control device | |
| US20220379923A1 (en) | Road User Categorization Through Monitoring | |
| CN115033027A (en) | Dynamic obstacle avoidance prediction management method for fixed-wing unmanned aerial vehicle |
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 |