CN113823123B - Vehicle obstacle avoidance early warning method and device based on discrete point track fitting - Google Patents
Vehicle obstacle avoidance early warning method and device based on discrete point track fitting Download PDFInfo
- Publication number
- CN113823123B CN113823123B CN202111142557.5A CN202111142557A CN113823123B CN 113823123 B CN113823123 B CN 113823123B CN 202111142557 A CN202111142557 A CN 202111142557A CN 113823123 B CN113823123 B CN 113823123B
- Authority
- CN
- China
- Prior art keywords
- early warning
- fitting
- vehicle
- probability
- distance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/16—Anti-collision systems
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/09—Arrangements for giving variable traffic instructions
- G08G1/0962—Arrangements for giving variable traffic instructions having an indicator mounted inside the vehicle, e.g. giving voice messages
- G08G1/0967—Systems involving transmission of highway information, e.g. weather, speed limits
- G08G1/096708—Systems involving transmission of highway information, e.g. weather, speed limits where the received information might be used to generate an automatic action on the vehicle control
- G08G1/096725—Systems involving transmission of highway information, e.g. weather, speed limits where the received information might be used to generate an automatic action on the vehicle control where the received information generates an automatic action on the vehicle control
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Atmospheric Sciences (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a vehicle obstacle avoidance early warning method and device based on discrete point track fitting. The early warning method comprises the following steps: performing NURBS spline fitting on the plurality of continuous track points according to historical track data to obtain a fitting curve, and calculating a tangent of an endpoint of the fitting curve; calculating the safe distance of the vehicle; according to the tangential direction vector and the actual speed, the fitting direction angle error of the previous track point, the safe distance is used as the radius at the fitting curve end point, the tangential vector of the fitting curve end point is in linear symmetry, the sector area with the angle being the multiple fitting direction angle error is defined as the approximate probability collision area, whether the approximate probability collision area has a fixed obstacle or/and a moving obstacle is judged, and the early warning probability I and the early warning probability II are calculated: and early warning the vehicle according to the early warning probability I and the early warning probability II. The invention predicts the collision direction, has the accurate early warning function on the safe running of the vehicles and reduces the collision probability among the vehicles.
Description
Technical Field
The invention relates to a vehicle obstacle avoidance early warning method in the technical field of vehicle obstacle avoidance early warning, in particular to a vehicle obstacle avoidance early warning method based on discrete point track fitting and an early warning device.
Background
The vehicle running track can be obtained by a vehicle-mounted running recorder and the like, but the vehicle-mounted running recorder is mainly used for storing the current vehicle running road condition and does not have the function of predicting the vehicle running track. The traditional curve fitting is mainly used for interpolation calculation, namely, a numerical value of an independent variable corresponding to a dependent variable in an interval is obtained, and the prediction precision outside the interval is generally low. The traditional road monitoring mode is generally static monitoring, and early warning prompts cannot be given in advance aiming at conditions that the driving direction of a vehicle deviates from a lane or collides with an obstacle and the like possibly occur in the driving process of the vehicle. In addition, the existing vehicle-mounted anti-collision system generally adopts distance sensing, but does not have collision direction prediction, is only suitable for slow parking conditions such as backing and the like, and does not have a networking communication function between vehicles applying the system at present.
Therefore, in order to avoid the defects of the existing vehicle running track prediction technology and solve the problems of inaccurate vehicle running track prediction and high accident probability, a vehicle track fitting method and a warning device thereof are needed.
Disclosure of Invention
In order to solve the technical problem of low prediction precision of the existing vehicle running track, the invention provides a vehicle obstacle avoidance early warning method and device based on discrete point track fitting.
The invention is realized by adopting the following technical scheme: a vehicle obstacle avoidance early warning method based on discrete point track fitting comprises the following steps:
a vehicle obstacle avoidance early warning method based on discrete point track fitting is characterized by comprising the following steps:
s1: obtaining historical track data of a vehicle, carrying out NURBS spline fitting on a plurality of continuous track points according to the historical track data to obtain a fitting curve, and finally calculating a tangent of an endpoint of the fitting curve;
s2: firstly, according to the running speed and the position of the vehicle at any moment, obtaining the maximum braking deceleration of the vehicle to calculate a first minimum safe running distance, then calculating a reaction distance of the vehicle before braking, and finally calculating a first safe distance of the vehicle according to the first minimum safe running distance and the reaction distance;
s3: firstly, calculating a tangential direction vector and an actual speed of a previous track point of a fitting curve endpoint, then calculating a fitting direction angle error of the previous track point according to the tangential direction vector and the actual speed, then taking the safe distance I as a radius at the fitting curve endpoint, enabling the tangential direction vector of the fitting curve endpoint to be in linear symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction angle error as a first probable collision area, finally judging whether a first probable collision area has a fixed obstacle or not, and calculating a corresponding first early warning probability;
s4: firstly, calculating a second minimum safe driving distance according to the driving speed and the position of the vehicle relative to a moving obstacle at any moment, then calculating a reaction distance of the vehicle before braking, and finally calculating a second safe distance of the vehicle according to the second minimum safe driving distance and the reaction distance;
taking the safe distance II as a radius at the end point of the fitting curve, enabling the tangent vector of the end point of the fitting curve to be in straight line symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction and an angle error as a large probability collision area II, finally judging whether the large probability collision area II has the moving barrier or not, and calculating a corresponding early warning probability II; wherein a multiple of the fitting direction angle error is equal to the fitting times of the fitting curve;
s5: and early warning the vehicle according to the early warning probability I and the early warning probability II.
According to the invention, through a vehicle historical track fitting method, a vehicle high-probability collision area is calculated, the collision direction is predicted, an accurate early warning function is provided for safe driving of the vehicle, the technical problem of low prediction accuracy of the vehicle driving track is solved, the collision probability in the vehicle driving process can be reduced, the application to automatic driving of the vehicle can be used for reference, and the blank in the prior art is made up.
As a further improvement of the above scheme, in step S5, a low risk early warning probability, a medium risk early warning probability, and a high risk early warning probability are preset according to actual driving data of the vehicle, and then the early warning probability one is compared with the low risk early warning probability and the high risk early warning probability respectively to obtain a corresponding early warning level according to a preset scheme, or/and the early warning probability two is compared with the low risk early warning probability and the high risk early warning probability respectively to obtain a corresponding early warning level according to the preset scheme.
As a further improvement of the above solution, in step S1, the number of NURBS spline fits is three; the vehicle running state defining the current time t is read by a vehicle sensing unit, and the speed is VtAcceleration of atAt the position StThe vehicle driving history track can be read from the storage unit, and the n track points are respectively St-n、St-n+1、St-n+2…St-2、St-1、StCalculating the tangent vector v of the end point of the fitting curvet。
As a further improvement of the above solution, in step S2, the calculation formula of the first minimum safe driving distance is:
Lmin1＝St+1-St＝Vt 2/(2*dmax)
in the formula, Lmin1For the minimum safe driving distance one, StAs the position of the vehicle at time t, St+1Dmax is the maximum braking deceleration of the vehicle for the position of the vehicle at time t + 1;
the calculation formula of the reaction distance is as follows:
L0＝Vt*T0
in the formula, L0Is the reaction distance, T0A brake reaction time for a driver in the vehicle;
the calculation formula of the first safety distance is as follows:
Ls＝L0+Lmin1
wherein Ls is the safe distance one.
As a further improvement of the above solution, in step S3, a tangential direction vector v of a trace point before the end point of the fitting curve is definedt-1And the actual speed Vt-1The calculation formula of the fitting direction angle error is as follows:
wherein α is the fitting direction angle error.
As a further improvement of the above scheme, in step S3, when there is no fixed obstacle in the first approximate collision area, the first early warning probability is zero;
when a fixed obstacle exists in the first approximate probability collision area, calculating the fixed obstacle and the position StVector V ofoAnd fitting vector vtAngle of (a) gammaO：
The calculation formula of the early warning probability I is as follows:
P1＝(3α－γo)/(3α)×100％
in the formula, P1And the early warning probability is one.
As a further improvement of the above solution, in step S4, the calculation formula of the minimum safe driving distance two is:
Lrmin2＝Vr 2/(2*dmax)
in the formula, VrIs the speed of travel, V, of the vehicle relative to the moving obstacler＝Vt-Vo；Lrmin2The minimum safe driving distance is two.
As a further improvement of the above scheme, in step S4, when there is no moving obstacle in the approximate probability collision area two, the early warning probability two is zero;
when a moving obstacle exists in the second high-probability collision area, calculating the moving obstacle and the position StVector V ofPAnd fitting vector vtAngle of (a) gammaP：
The calculation formula of the early warning probability II is as follows:
in the formula, P2And the early warning probability is two.
As a further improvement of the above solution, in step S5, a low risk pre-warning probability P is definedaOne middle risk early warning probability PbAnd a high risk early warning probability Pc；
If P1≤PaIf so, not prompting collision and prompting normal driving;
if Pa＜P1≤PbIf so, carrying out low-level risk collision prompt and prompting prudent driving;
if Pb＜P1≤PcIf so, performing middle-grade risk collision prompt and prompting deceleration driving;
if P1＞PcThen, high-level risk collision prompt is carried out, and braking and parking are prompted;
and/or the first and/or second light sources,
if P2≤PaIf so, not prompting collision and prompting normal driving;
if Pa＜P2≤PbIf so, carrying out low-level risk collision prompt and prompting prudent driving;
if Pb＜P2≤PcIf so, performing middle-grade risk collision prompt and prompting deceleration driving;
if P2＞PcAnd then, carrying out high-grade risk collision prompt and prompting to brake and stop.
The invention also provides an early warning device, which applies any one of the above vehicle obstacle avoidance early warning methods based on discrete point track fitting, and the method comprises the following steps:
a storage unit for acquiring and storing historical trajectory data of the vehicle;
the logic synthesis unit is used for carrying out NURBS spline fitting on a plurality of continuous track points according to the historical track data to obtain a fitting curve, and finally calculating a tangent of an endpoint of the fitting curve; the logic synthesis unit is further used for obtaining the maximum braking deceleration of the vehicle according to the running speed and the position of the vehicle at any moment so as to calculate a first minimum safe running distance, then calculating a reaction distance of the vehicle before braking, and finally calculating a first safe distance of the vehicle according to the first minimum safe running distance and the reaction distance; the logic synthesis unit is further used for calculating a tangential direction vector and an actual speed of a previous track point of the fitting curve end point, calculating a fitting direction angle error of the previous track point according to the tangential direction vector and the actual speed, then taking the safe distance I as a radius at the fitting curve end point, enabling the tangential direction vector of the fitting curve end point to be in linear symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction angle error as a probable collision area I, finally judging whether a fixed obstacle exists in the probable collision area I, and calculating a corresponding early warning probability I; the logic synthesis unit is also used for firstly calculating a second minimum safe driving distance according to the driving speed and the position of the vehicle relative to a moving obstacle at any moment, then calculating a reaction distance of the vehicle before braking, and finally calculating a second safe distance of the vehicle according to the second minimum safe driving distance and the reaction distance; the logic synthesis unit is further used for taking the safe distance II as a radius at the end point of the fitting curve, enabling the tangent vector of the end point of the fitting curve to be in straight line symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction and an angle error as a second approximate collision area, finally judging whether the second large-probability collision area has the moving obstacle or not, and calculating a corresponding second early warning probability; wherein a multiple of the fitting direction angle error is equal to the fitting times of the fitting curve; and
and the early warning prompting unit is used for early warning the vehicle according to the early warning probability I and the early warning probability II.
The vehicle obstacle avoidance early warning method and device based on discrete point track fitting have the following beneficial effects:
1. according to the vehicle obstacle avoidance early warning method based on discrete point track fitting, a vehicle high-probability collision area is calculated through a vehicle historical track fitting method, the collision direction is predicted, and an accurate early warning function is achieved for safe driving of a vehicle.
2. The vehicle obstacle avoidance early warning method based on the discrete point track fitting can prevent the phenomena of lane departure, obstacle collision and the like in the vehicle driving process, reduce the collision probability between vehicles, can be used for reference in the automatic driving of the vehicles, and fills the blank in the prior art.
3. The beneficial effects of the early warning device are the same as those of the vehicle obstacle avoidance early warning method based on the discrete point track fitting, and are not repeated herein.
Drawings
Fig. 1 is a flowchart of a vehicle obstacle avoidance early warning method based on discrete point trajectory fitting in embodiment 1 of the present invention;
fig. 2 is a schematic diagram of trace point fitting in embodiment 1 of the present invention;
fig. 3 is a block diagram of a configuration of the warning device in embodiment 2 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. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Example 1
Referring to fig. 1 and fig. 2, the present embodiment provides a vehicle obstacle avoidance early warning method based on discrete point trajectory fitting. The early warning method comprises the steps of firstly carrying out BURBS spline fitting from a storage unit according to historical track data, and then calculating a tangent line of a fitting spline endpoint. And calculating the vehicle safety distance Ls according to the current vehicle data. And calculating the collision probability P of the vehicle according to the safe distance and the tangent distance, and issuing early warning information. In this embodiment, the vehicle obstacle avoidance early warning method based on discrete point trajectory fitting is mainly implemented by the following steps, specifically steps S1-S5.
Step S1: the method comprises the steps of firstly obtaining historical track data of a vehicle, then carrying out NURBS spline fitting on a plurality of continuous track points according to the historical track data to obtain a fitting curve, and finally calculating a tangent line of an endpoint of the fitting curve. The number of NURBS spline fits is three; the vehicle running state defining the current time t is read by a vehicle sensing unit, and the speed is VtAcceleration of atAt the position StThe vehicle driving history track can be read from the storage unit, and the n track points are respectively St-n、St-n+1、St-n+2…St-2、St-1、StCalculating the tangent vector v of the end point of the fitting curvet. Obtaining a fitting curve by utilizing a cubic NURBS spline fitting method, and calculating a tangent vector v of an endpoint of the fitting curvetAs shown in fig. 2.
Step S2: the method comprises the steps of firstly obtaining the maximum braking deceleration of a vehicle according to the running speed and the position of the vehicle at any moment and according to vehicle performance parameters to calculate the first minimum safe running distance, then calculating the reaction distance of the vehicle before braking, and finally calculating the first safe distance of the vehicle according to the first minimum safe running distance and the reaction distance. In this embodiment, the minimum safe driving distance one is calculated by the following formula:
Lmin1＝St+1-St＝Vt 2/(2*dmax)
in the formula, Lmin1The minimum safe driving distance is one, StAs the position of the vehicle at time t, St+1Dmax is the maximum braking deceleration of the vehicle, which is the position of the vehicle at time t + 1.
The formula for the reaction distance is:
L0＝Vt*T0
in the formula, L0For reaction distance, T0The brake reaction time of the driver in the vehicle.
The calculation formula of the first safety distance is as follows:
Ls＝L0+Lmin1
wherein Ls is a safety distance one.
Step S3: firstly, calculating a tangential direction vector and an actual speed of a previous track point of a fitting curve endpoint, then calculating a fitting direction angle error of the previous track point according to the tangential direction vector and the actual speed, then taking a safety distance I as a radius at the fitting curve endpoint, enabling a straight line where the tangential vector of the fitting curve endpoint is located to be symmetrical, defining a fan-shaped area with an angle being a multiple of the fitting direction angle error as a first approximate collision area, finally judging whether a first approximate collision area has a fixed obstacle or not, and calculating a corresponding first early warning probability. Wherein the multiple in the error of the multiple fitting direction angle is equal to the fitting times of the fitting curve. In this embodiment, a tangential direction vector v of a previous trace point of the end point of the fitting curve is definedt-1And the actual speed gammat-1The calculation formula of the angle error of the fitting direction is as follows:
in the formula, α is the fitting direction angle error.
When there is no fixed obstacle in the first probable collision area, the first early warning probability is zero.
When a fixed obstacle exists in the first approximate collision area, the fixed obstacle and the position S are calculatedtVector V ofoAnd fitting vector vtAngle of (a) gammaO：
The calculation formula of the early warning probability I is as follows:
P1＝(3α－γo)/(3α)×100％
in the formula, P1The early warning probability is one.
Step S4: firstly, according to the running speed and position of the vehicle relative to a moving obstacle at any moment, calculating a minimum safe running distance II, then calculating a reaction distance of the vehicle before braking, and finally calculating a safe distance II of the vehicle according to the minimum safe running distance II and the reaction distance.
And taking the safe distance II as the radius at the end point of the fitting curve, enabling the tangent vector of the end point of the fitting curve to be in linear symmetry, defining a fan-shaped area with the angle being the angle error of multiple fitting directions as a high probability collision area II, finally judging whether a moving obstacle exists in the high probability collision area II or not, and calculating the corresponding early warning probability II. The multiple in the multiple fitting direction angle error is equal to the fitting times of the fitting curve, and in this embodiment, the multiple is three times.
The calculation formula of the minimum safe driving distance II is as follows:
Lrmin2＝Vr 2/(2*dmax)
in the formula, VrSpeed of travel of the vehicle relative to moving obstacles, Vr＝Vt-Vo；Lrmin2The minimum safe driving distance is two.
Also at position StTaking the safety distance as the radius of Ls, and a tangent vector vtThe straight line is symmetrical, and the sector area with the angle of 3 alpha is defined as the approximate probability collision area, as shown by the light area in fig. 2.
And when no moving obstacle exists in the second approximate collision area, the second early warning probability is zero.
When there is a moving obstacle in the approximate collision area two, the moving obstacle and the position S are calculatedtVector V ofPAnd fitting vector vtAngle of (a) gammaP：
The calculation formula of the early warning probability II is as follows:
in the formula, P2The early warning probability is two.
Step S5: and early warning the vehicle according to the early warning probability I and the early warning probability II.
In some embodiments, a low-risk early warning probability, a medium-risk early warning probability and a high-risk early warning probability are preset according to actual driving data of a vehicle, then the early warning probability I is compared with the low-risk early warning probability and the high-risk early warning probability respectively, and corresponding early warning levels are obtained according to a preset scheme, or/and the early warning probability II is compared with the low-risk early warning probability and the high-risk early warning probability respectively, and corresponding early warning levels are obtained according to the preset scheme.
In this embodiment, when the obstacle is a fixed obstacle or a moving obstacle, the warning probability P is P1Or P2Comparing P with P0 and P1, P0 and P1 are low risk pre-warning probability and high risk pre-warning probability respectively, and refer to fig. 1 specifically.
In other embodiments, a low risk pre-warning probability P is definedaOne middle risk early warning probability PbAnd a high risk early warning probability PG. Relative to a fixed obstacle, then:
if P1≤PaIf so, not prompting collision and prompting normal driving;
if Pa＜P1≤PbIf so, carrying out low-level risk collision prompt and prompting prudent driving;
p ifb＜P1≤PcIf so, performing middle-grade risk collision prompt and prompting deceleration driving;
fourthly, if P1＞PcAnd then, carrying out high-grade risk collision prompt and prompting to brake and stop.
Relative to the moving obstacle, then:
(1) if P2≤PaIf so, not prompting collision and prompting normal driving;
(2) if Pa＜P2≤PbIf so, carrying out low-level risk collision prompt and prompting prudent driving;
(4) if Pb＜P2≤PcIf so, performing middle-grade risk collision prompt and prompting deceleration driving;
(5) if P2＞PcAnd then, carrying out high-grade risk collision prompt and prompting to brake and stop.
Of course, the comparison method of the second early warning probability is similar to that of the first early warning probability, and the comparison method may be present in this embodiment or in other embodiments, and the comparison method of the two early warning probabilities may be present simultaneously or may be present separately. This is because, in some embodiments, the fixed obstacle and the mobile obstacle may occur simultaneously or may occur separately, and thus, it is necessary to perform early warning according to different situations. Of course, in some embodiments, the preset probabilities compared by the two early warning probabilities may be different, which may be determined according to data obtained by actual experimental measurement, so as to meet different obstacle avoidance requirements. When the obstacle is a fixed obstacle or a moving obstacle, the early warning probability P is P1Or P2Through P and Pa、Pb、PcAnd (6) comparing.
In summary, compared with the existing vehicle obstacle avoidance early warning technology, the vehicle obstacle avoidance early warning method based on the discrete point trajectory fitting of the embodiment has the following advantages:
1. according to the vehicle obstacle avoidance early warning method based on discrete point track fitting, a vehicle high-probability collision area is calculated through a vehicle historical track fitting method, the collision direction is predicted, and an accurate early warning function is achieved for safe driving of a vehicle.
2. The vehicle obstacle avoidance early warning method based on the discrete point track fitting can prevent the phenomena of lane departure, obstacle collision and the like in the vehicle driving process, reduce the collision probability between vehicles, can be used for reference in the automatic driving of the vehicles, and fills the blank in the prior art.
Example 2
Referring to fig. 3, the present embodiment provides an early warning device, which is applied to the vehicle obstacle avoidance early warning method based on discrete point trajectory fitting in embodiment 1, and specifically includes a control unit, a wireless communication module, a storage unit, a logic synthesis unit, and an early warning prompt module. The control unit realizes a control algorithm and interacts data with each unit, the wireless communication module realizes a communication function between the vehicle-mounted warning devices, the storage unit stores vehicle historical track data, the logic synthesis unit realizes vehicle driving warning probability calculation, and the warning prompting module realizes warning.
The storage unit is used for acquiring and storing historical track data of the vehicle, wherein the vehicle running state at the current time t is read by the vehicle sensing unit. And the logic synthesis unit is used for carrying out NURBS spline fitting on the plurality of continuous track points according to the historical track data to obtain a fitting curve, and finally calculating a tangent of an endpoint of the fitting curve. The logic synthesis unit is also used for firstly obtaining the maximum braking deceleration of the vehicle according to the running speed and the position of the vehicle at any moment so as to calculate the first minimum safe running distance, then calculating the reaction distance of the vehicle before braking, and finally calculating the first safe distance of the vehicle according to the first minimum safe running distance and the reaction distance. The logic synthesis unit is also used for calculating a tangential direction vector and an actual speed of a previous track point of the fitting curve end point, calculating a fitting direction angle error of the previous track point according to the tangential direction vector and the actual speed, taking a safety distance I as a radius at the fitting curve end point, enabling the tangential direction vector of the fitting curve end point to be in linear symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction angle error as a high probability collision area I, judging whether a fixed obstacle exists in the high probability collision area I or not, and calculating a corresponding early warning probability I. The logic synthesis unit is also used for firstly calculating a second minimum safe driving distance according to the driving speed and the position of the vehicle relative to a moving obstacle at any moment, then calculating a reaction distance of the vehicle before braking, and finally calculating the second safe driving distance of the vehicle according to the second minimum safe driving distance and the reaction distance. The logic synthesis unit is also used for taking the safe distance II as the radius at the end point of the fitting curve, enabling the tangent vector of the end point of the fitting curve to be in linear symmetry, defining a fan-shaped area with the angle being the angle error of multiple fitting directions as a large probability collision area II, finally judging whether a moving obstacle exists in the large probability collision area II or not, and calculating the corresponding early warning probability II. Wherein the multiple in the multiple fitting direction angle error is equal to the fitting times of the fitting curve.
And the early warning prompting unit is used for early warning the vehicle according to the early warning probability I and the early warning probability II. The wireless communication module is mainly used for receiving and transmitting wireless signals, can perform a communication function with a traffic command center, and can also perform signal transmission with alarm equipment, a mobile phone and the like, which is not described herein in detail.
Example 3
The embodiment provides a computer terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor. And when the processor executes the program, the steps of the vehicle obstacle avoidance early warning method based on the discrete point track fitting in the embodiment 1 are realized.
When the vehicle obstacle avoidance early warning method based on discrete point track fitting is applied, the vehicle obstacle avoidance early warning method can be applied in a software mode, for example, a program which is designed to run independently is installed on a computer terminal, and the computer terminal can be a computer, a smart phone, a control system and other internet of things equipment. The vehicle obstacle avoidance early warning method based on the discrete point track fitting can also be designed into an embedded running program and installed on a computer terminal, such as a single chip microcomputer.
Example 4
The present embodiment provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by the processor, the steps of the vehicle obstacle avoidance early warning method based on the discrete point trajectory fitting in embodiment 1 are realized. When the vehicle obstacle avoidance early warning method based on the discrete point track fitting is applied, the vehicle obstacle avoidance early warning method can be applied in a software mode, for example, the vehicle obstacle avoidance early warning method is designed into a program which can be independently operated by a computer readable storage medium, the computer readable storage medium can be a U disk and is designed into a U shield, and the U disk is designed into a program which starts the whole method through external triggering.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents and improvements made within the spirit and principle of the present invention are intended to be included within the scope of the present invention.
Claims (10)
1. A vehicle obstacle avoidance early warning method based on discrete point track fitting is characterized by comprising the following steps:
s1: obtaining historical track data of a vehicle, carrying out NURBS spline fitting on a plurality of continuous track points according to the historical track data to obtain a fitting curve, and finally calculating a tangent of an endpoint of the fitting curve;
s2: firstly, according to the running speed and the position of the vehicle at any moment, obtaining the maximum braking deceleration of the vehicle to calculate a first minimum safe running distance, then calculating a reaction distance of the vehicle before braking, and finally calculating a first safe distance of the vehicle according to the first minimum safe running distance and the reaction distance;
s3: firstly, calculating a tangential direction vector and an actual speed of a previous track point of a fitting curve end point, then calculating a fitting direction angle error of the previous track point according to the tangential direction vector and the actual speed, then taking the safe distance I as a radius at the fitting curve end point, enabling the tangential direction vector of the fitting curve end point to be in linear symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction angle error as a probable collision area I, finally judging whether a fixed obstacle exists in the probable collision area I, and calculating a corresponding early warning probability I;
s4: firstly, calculating a second minimum safe driving distance according to the driving speed and the position of the vehicle relative to a moving obstacle at any moment, then calculating a reaction distance of the vehicle before braking, and finally calculating a second safe distance of the vehicle according to the second minimum safe driving distance and the reaction distance;
taking the safe distance II as a radius at the end point of the fitting curve, enabling the tangent vector of the end point of the fitting curve to be in straight line symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction and an angle error as a large probability collision area II, finally judging whether the large probability collision area II has the moving barrier or not, and calculating a corresponding early warning probability II; wherein a multiple of the fitting direction angle error is equal to the fitting times of the fitting curve;
s5: and early warning the vehicle according to the early warning probability I and the early warning probability II.
2. The vehicle obstacle avoidance early warning method based on discrete point trajectory fitting as claimed in claim 1, wherein in step S5, a low risk early warning probability, an intermediate risk early warning probability and a high risk early warning probability are preset according to actual driving data of the vehicle, and then the early warning probability one is compared with the low risk early warning probability and the high risk early warning probability respectively to obtain corresponding early warning levels according to a preset scheme, or/and the early warning probability two is compared with the low risk early warning probability and the high risk early warning probability respectively to obtain corresponding early warning levels according to the preset scheme.
3. The vehicle obstacle avoidance early warning method based on discrete point trajectory fitting as claimed in claim 1, wherein in step S1, the number of NURBS spline fitting is three; the vehicle running state defining the current time t is read by a vehicle sensing unit, and the speed is VtAcceleration of atAt the position StThe vehicle driving history track can be read from the storage unit, and the n track points are respectively St-n、St-n+1、St-n+2…St-2、St-1、StCalculating the tangent vector v of the end point of the fitting curvet。
4. The vehicle obstacle avoidance early warning method based on the discrete point trajectory fitting as claimed in claim 3, wherein in the step S2, the calculation formula of the first minimum safe driving distance is:
Lmin1＝St+1-St＝Vt 2/(2*dmax)
in the formula, Lmin1For the minimum safe driving distance one, StAs the position of the vehicle at time t, St+1Dmax is the maximum braking deceleration of the vehicle for the position of the vehicle at time t + 1;
the calculation formula of the reaction distance is as follows:
L0＝Vt*T0
in the formula, L0Is said reaction distance, T0A brake reaction time for a driver in the vehicle;
the calculation formula of the first safety distance is as follows:
Ls＝L0+Lmin1
wherein Ls is the safe distance one.
5. The vehicle obstacle avoidance early warning method based on discrete point track fitting as claimed in claim 4, wherein in the step S3, a tangential direction vector v of a previous track point of the fitting curve end point is definedt-1And the actual speed Vt-1The calculation formula of the fitting direction angle error is as follows:
wherein α is the fitting direction angle error.
6. The vehicle obstacle avoidance early warning method based on the discrete point trajectory fitting as claimed in claim 5, wherein in step S3, when there is no fixed obstacle in the first approximate probability collision area, the early warning probability one is zero;
when a fixed obstacle exists in the first approximate collision area, calculating the fixed obstacle and the position StVector V ofOAnd fitting vector vtAngle of (a) gammaO：
The calculation formula of the early warning probability I is as follows:
P1＝(3α－γo)/(3α)×100％
in the formula, P1And the early warning probability is the first early warning probability.
7. The vehicle obstacle avoidance early warning method based on the discrete point trajectory fitting as claimed in claim 6, wherein in the step S4, the calculation formula of the minimum safe driving distance two is:
Lrmin2＝Vr 2/(2*dmax)
in the formula, VrIs the speed of travel, V, of the vehicle relative to the moving obstacler＝Vt-Vo；Lrmin2The minimum safe driving distance is two.
8. The vehicle obstacle avoidance early warning method based on the discrete point trajectory fitting as claimed in claim 7, wherein in the step S4, when no moving obstacle exists in the second approximate collision area, the second early warning probability is zero;
when a moving obstacle exists in the second high-probability collision area, calculating the moving obstacle and the position StVector V ofPAnd the fitting vector vtAngle of (a) gammaP：
The calculation formula of the early warning probability II is as follows:
in the formula, P2And the early warning probability is two.
9. The vehicle obstacle avoidance early warning method based on discrete point trajectory fitting as claimed in claim 1, wherein in the step S5, a low risk early warning probability P is definedaOne middle risk early warning probability PbAnd a high risk early warning probability Pc；
If P1≤PaIf so, not prompting collision and prompting normal driving;
if Pa＜P1≤PbIf so, carrying out low-level risk collision prompt and prompting prudent driving;
if Pb＜P1≤PcIf so, performing middle-grade risk collision prompt and prompting deceleration driving;
if P1＞PcThen, performing high-grade risk collision prompt and prompting braking and parking;
and/or the first and/or second light sources,
if P2≤PaIf so, not prompting collision and prompting normal driving;
if Pa＜P2≤PbIf so, carrying out low-level risk collision prompt and prompting prudent driving;
if Pb＜P2≤PcIf so, performing middle-grade risk collision prompt and prompting deceleration driving;
if P2＞PcAnd then, carrying out high-grade risk collision prompt and prompting to brake and stop.
10. An early warning device applying the vehicle obstacle avoidance early warning method based on the discrete point trajectory fitting as claimed in any one of claims 1 to 9, characterized in that the early warning device comprises:
a storage unit for acquiring and storing historical trajectory data of the vehicle;
the logic synthesis unit is used for carrying out NURBS spline fitting on a plurality of continuous track points according to the historical track data to obtain a fitting curve, and finally calculating a tangent of an endpoint of the fitting curve; the logic synthesis unit is further used for obtaining the maximum braking deceleration of the vehicle according to the running speed and the position of the vehicle at any moment so as to calculate a first minimum safe running distance, then calculating a reaction distance of the vehicle before braking, and finally calculating a first safe distance of the vehicle according to the first minimum safe running distance and the reaction distance; the logic synthesis unit is further used for calculating a tangential direction vector and an actual speed of a previous track point of the fitting curve end point, calculating a fitting direction angle error of the previous track point according to the tangential direction vector and the actual speed, then taking the safe distance I as a radius at the fitting curve end point, enabling the tangential direction vector of the fitting curve end point to be in linear symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction angle error as a probable collision area I, finally judging whether a fixed obstacle exists in the probable collision area I, and calculating a corresponding early warning probability I; the logic synthesis unit is also used for firstly calculating a second minimum safe driving distance according to the driving speed and the position of the vehicle relative to a moving obstacle at any moment, then calculating a reaction distance of the vehicle before braking, and finally calculating a second safe distance of the vehicle according to the second minimum safe driving distance and the reaction distance; the logic synthesis unit is further used for taking the safe distance II as a radius at the end point of the fitting curve, enabling the tangent vector of the end point of the fitting curve to be in straight line symmetry, defining a fan-shaped area with an angle being a multiple of the fitting direction and an angle error as a second approximate collision area, finally judging whether the second large-probability collision area has the moving obstacle or not, and calculating a corresponding second early warning probability; wherein a multiple of the fitting direction angle error is equal to the fitting times of the fitting curve; and
and the early warning prompting unit is used for early warning the vehicle according to the early warning probability I and the early warning probability II.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111142557.5A CN113823123B (en) | 2021-09-28 | 2021-09-28 | Vehicle obstacle avoidance early warning method and device based on discrete point track fitting |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111142557.5A CN113823123B (en) | 2021-09-28 | 2021-09-28 | Vehicle obstacle avoidance early warning method and device based on discrete point track fitting |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113823123A CN113823123A (en) | 2021-12-21 |
| CN113823123B true CN113823123B (en) | 2022-07-01 |
Family
ID=78915797
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111142557.5A Active CN113823123B (en) | 2021-09-28 | 2021-09-28 | Vehicle obstacle avoidance early warning method and device based on discrete point track fitting |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113823123B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101405174A (en) * | 2006-03-22 | 2009-04-08 | 日产自动车株式会社 | Avoidance operation calculation device, avoidance control device, vehicle with each of the devices, avoidance operation calculation method, and avoidance control method |
| CN107145147A (en) * | 2017-04-10 | 2017-09-08 | 广州小鹏汽车科技有限公司 | A kind of vehicle low speed automatic Pilot collision prevention method and system |
| CN109523830A (en) * | 2018-11-08 | 2019-03-26 | 中交第公路勘察设计研究院有限公司 | Track of vehicle prediction and anti-collision warning method based on high-frequency high-precision location information |
| WO2019062630A1 (en) * | 2017-09-30 | 2019-04-04 | 蔚来汽车有限公司 | Forward collision avoidance method and system for vehicle |
| CN111591307A (en) * | 2020-04-15 | 2020-08-28 | 毫末智行科技有限公司 | Obstacle avoidance track planning method and system and vehicle |
| CN112389427A (en) * | 2021-01-19 | 2021-02-23 | 腾讯科技（深圳）有限公司 | Vehicle track optimization method and device, electronic equipment and storage medium |
| CN113267199A (en) * | 2021-06-24 | 2021-08-17 | 上海欧菲智能车联科技有限公司 | Driving track planning method and device |
| CN113345228A (en) * | 2021-06-01 | 2021-09-03 | 星觅(上海)科技有限公司 | Driving data generation method, device, equipment and medium based on fitted track |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109817021B (en) * | 2019-01-15 | 2021-11-02 | 阿波罗智能技术(北京)有限公司 | Method and device for avoiding traffic participants in roadside blind areas of laser radar |
| CN109712421B (en) * | 2019-02-22 | 2021-06-04 | 百度在线网络技术（北京）有限公司 | Method, apparatus and storage medium for speed planning of autonomous vehicles |
-
2021
- 2021-09-28 CN CN202111142557.5A patent/CN113823123B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101405174A (en) * | 2006-03-22 | 2009-04-08 | 日产自动车株式会社 | Avoidance operation calculation device, avoidance control device, vehicle with each of the devices, avoidance operation calculation method, and avoidance control method |
| CN107145147A (en) * | 2017-04-10 | 2017-09-08 | 广州小鹏汽车科技有限公司 | A kind of vehicle low speed automatic Pilot collision prevention method and system |
| WO2019062630A1 (en) * | 2017-09-30 | 2019-04-04 | 蔚来汽车有限公司 | Forward collision avoidance method and system for vehicle |
| CN109523830A (en) * | 2018-11-08 | 2019-03-26 | 中交第公路勘察设计研究院有限公司 | Track of vehicle prediction and anti-collision warning method based on high-frequency high-precision location information |
| CN111591307A (en) * | 2020-04-15 | 2020-08-28 | 毫末智行科技有限公司 | Obstacle avoidance track planning method and system and vehicle |
| CN112389427A (en) * | 2021-01-19 | 2021-02-23 | 腾讯科技（深圳）有限公司 | Vehicle track optimization method and device, electronic equipment and storage medium |
| CN113345228A (en) * | 2021-06-01 | 2021-09-03 | 星觅(上海)科技有限公司 | Driving data generation method, device, equipment and medium based on fitted track |
| CN113267199A (en) * | 2021-06-24 | 2021-08-17 | 上海欧菲智能车联科技有限公司 | Driving track planning method and device |
Non-Patent Citations (3)
| Title |
|---|
| An Obstacles Avoidance Algorithm Based on Improved Artificial Potential Field;Si Xi Lu 等;《2020 IEEE International Conference on Mechatronics and Automation (ICMA)》;20201026;全文 * |
| 侧向安全边界理论在圆曲线路段限速中的应用;陈一锴 等;《哈尔滨工业大学学报》;20190331(第3期);全文 * |
| 智能汽车避障风险评估及轨迹规划;王斌;《汽车技术》;20180624(第06期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113823123A (en) | 2021-12-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8660778B2 (en) | Running plan creating apparatus | |
| US20190389487A1 (en) | Tailgating alert system in vehicles | |
| CN111797780B (en) | Vehicle following track planning method, device, server and storage medium | |
| CN103782192A (en) | Safety device for motor vehicles | |
| US20210309217A1 (en) | Apparatus for controlling autonomous driving of a vehicle, system having the same and method thereof | |
| EP3456596A1 (en) | Method and device of predicting a possible collision | |
| JP2003536096A (en) | Tracking map generator | |
| US20230182696A1 (en) | Braking control system and method for autonomous vehicle | |
| EP3885226A1 (en) | Method and system for planning the motion of a vehicle | |
| CN114291116B (en) | Surrounding vehicle track prediction method and device, vehicle and storage medium | |
| CN114212110A (en) | Obstacle trajectory prediction method, obstacle trajectory prediction device, electronic device, and storage medium | |
| US20220001899A1 (en) | Vehicle control system using reliability of input signal for autonomous vehicle | |
| CN115871690A (en) | Method, system and storage medium for predicting self-vehicle track | |
| CN114802251A (en) | Control method and device for automatic driving vehicle, electronic device and storage medium | |
| CN113823123B (en) | Vehicle obstacle avoidance early warning method and device based on discrete point track fitting | |
| CN114132311A (en) | Method and module for screening dangerous targets for automatic emergency braking of vehicle | |
| CN113911111A (en) | Vehicle collision detection method, system, electronic device, and storage medium | |
| CN116279341B (en) | Safety braking method and device, electronic equipment and storage medium | |
| EP2238007B1 (en) | Method and apparatus for preventing over-speed by predicting vehicle speed | |
| CN115320589B (en) | Following speed planning method, device, chip, terminal, electronic equipment and medium | |
| CN110450788A (en) | A kind of driving mode switching method, device, equipment and storage medium | |
| CN113561996B (en) | Trajectory planning method and device | |
| US20220379922A1 (en) | System for maneuvering a vehicle | |
| CN111176285B (en) | Method and device for planning travel path, vehicle and readable storage medium | |
| JPH102954A (en) | Radar and method for identifying preceding 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |