CN119596961A - Method and device for judging driver intervention in automatic driving of automobile - Google Patents
Method and device for judging driver intervention in automatic driving of automobile Download PDFInfo
- Publication number
- CN119596961A CN119596961A CN202510143877.4A CN202510143877A CN119596961A CN 119596961 A CN119596961 A CN 119596961A CN 202510143877 A CN202510143877 A CN 202510143877A CN 119596961 A CN119596961 A CN 119596961A
- Authority
- CN
- China
- Prior art keywords
- point set
- order gradient
- determination point
- point
- automatic driving
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
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/40—Control within particular dimensions
- G05D1/43—Control of position or course in two dimensions
-
- 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/60—Intended control result
- G05D1/617—Safety or protection, e.g. defining protection zones around obstacles or avoiding hazards
- G05D1/622—Obstacle avoidance
- G05D1/633—Dynamic obstacles
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)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种汽车自动驾驶中驾驶员干预判断方法及装置，方法包括：按照预设时间间隔获取汽车自动驾驶测试中的多个点位；根据多个所述点位与预先规划的基准参考线进行计算，确定第一判定点位集合；第一判定点位集合不为空时，基于所述第一判定点位集合进行拟合计算，获得第二判定点位集合；第二判定点位集合不为空时，基于所述第二判定点位集合进行梯度计算，获得一阶梯度集合、二阶梯度集合以及第三判定点位集合；第三判定点位集合不为空时，基于所述第三判定点位集合进行曲率计算，获得第四判定点位集合；第四判定点位集合不为空时，确定汽车自动驾驶中受到驾驶员干预。该方法能够有效判断汽车自动驾驶中驾驶员是否干预。
The present invention discloses a method and device for judging driver intervention in an automatic driving vehicle. The method comprises: obtaining multiple points in an automatic driving vehicle test at a preset time interval; calculating based on the multiple points and a pre-planned reference line to determine a first judgment point set; when the first judgment point set is not empty, performing fitting calculation based on the first judgment point set to obtain a second judgment point set; when the second judgment point set is not empty, performing gradient calculation based on the second judgment point set to obtain a first-order gradient set, a second-order gradient set and a third judgment point set; when the third judgment point set is not empty, performing curvature calculation based on the third judgment point set to obtain a fourth judgment point set; when the fourth judgment point set is not empty, determining that the automatic driving of the vehicle is subject to driver intervention. The method can effectively judge whether the driver intervenes in the automatic driving of the vehicle.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to a method and a device for judging driver intervention in automatic driving of an automobile.
Background
In the development process of the automatic driving technology, the automobile automatic driving test is a key link for promoting the development of an intelligent traffic system in the future, and the core significance of the test is to verify the safety and stability of the automatic driving system in complex and changeable actual scenes, optimize sensing, decision and control algorithms and ensure that the automobile can cope with challenges in different environments. Meanwhile, through data accumulation and technology iteration, a basis is provided for the establishment of relevant laws and regulations and industry standards, the technology is promoted to be commercialized to fall to the ground, and finally the aims of reducing traffic accident rate and improving travel efficiency and convenience are achieved. These tests not only provide a powerful support for technological development, but also will change the way of travel for humans deeply.
The patent CN109491370A proposes a safety control method and a system for automatic driving test of an automobile, wherein the method comprises the steps of acquiring vehicle safety state information through a CAN bus, judging whether a tested vehicle meets driving conditions according to the vehicle safety state information, and if not, controlling the vehicle to stop running. When the vehicle meets the running condition, acquiring a vehicle control signal and verifying the correctness of the vehicle control signal, and if the verification is not passed, judging that the vehicle control signal is an invalid signal. When the vehicle is running, judging whether the test vehicle, the rotating hub or the automatic driving robot is in an emergency state, and if so, controlling the vehicle to stop running. The method can stop running when the vehicle does not meet the running condition, however, the method may cause inefficiency in testing.
In the automatic driving test of an automobile, in order to avoid accidents, repeated experiments are required to be carried out on the automobile on the same road section. In order to ensure the safety of experiments, at least one safety driver must monitor the vehicles in a vehicle-mounted manner during the test of the vehicles so as to take over the operation of the vehicles in time in case of emergency, and the steering wheel can be rotated when the path deviates excessively, thereby avoiding accidents caused by the fact that the vehicles exit the road. However, in order to analyze the performance of the automatic driving system and ensure the reliability of the data, it is necessary to determine whether the trajectory of the vehicle during the test is interfered by the driver for each test experiment. If it is found that the driver has performed an artificial operation on the steering wheel during the test, for example turning the steering wheel, resulting in a deviation of the course, the relevant test data will be regarded as unreliable and rejected. Such screening processes help ensure that subsequent analysis is based only on real data that is fully controlled by the autopilot system, however, subsequent screening of the test data is difficult, especially if the test process requires acquisition of multiple sets of trajectories.
The automatic driving test system and method which are proposed at present do not consider the factor of driver interference, so that the test precision is reduced.
Disclosure of Invention
The invention provides a method and a device for judging the intervention of a driver in automatic driving of an automobile, which can accurately judge whether the driver intervenes in automatic driving of the automobile, and improve the scientificity and the accuracy of automatic driving performance evaluation.
A method for judging the intervention of a driver in the automatic driving of an automobile comprises the following steps:
acquiring a plurality of point positions in automatic driving of the automobile according to a preset time interval;
calculating according to the plurality of point positions and a pre-planned base reference line, and determining a first judgment point position set;
when the first judgment point location set is not empty, fitting calculation is carried out based on the first judgment point location set, and a second judgment point location set is obtained;
When the second judgment point position set is not empty, performing gradient calculation based on the second judgment point position set to obtain a gradient set, a second-order gradient set and a third judgment point position set;
when the third judgment point location set is not empty, curvature calculation is carried out based on the third judgment point location set, and a fourth judgment point location set is obtained;
And when the fourth judgment point position set is not empty, determining that the automobile is interfered by a driver in automatic driving.
Further, after obtaining a plurality of points in the automatic driving of the automobile according to the preset time interval, the method further comprises the following steps:
fitting the plurality of point positions to obtain an automobile running track curve.
Further, when the first set of decision points is empty, determining that the vehicle is not being interfered by the driver in the automatic driving process of the vehicle, or
When the second judgment point set is empty, determining that the automobile is not interfered by a driver in the automatic driving process or
And when the third judgment point position set is empty, determining that the automobile is not interfered by a driver in automatic driving.
Further, according to the calculation of the plurality of the points and the pre-planned base reference line, a first judgment point location set is determined, which comprises:
establishing an initial point location set according to the acquired multiple point locations;
setting a tolerance threshold and initializing a first judgment point location set;
And calculating the distance between each point in the initial point location set and the datum reference line, and if the absolute distance between the point in the initial point location set and the datum reference line is greater than or equal to the tolerance threshold, putting the corresponding point into the first judgment point location set.
Further, performing fitting calculation based on the first judgment point location set to obtain a second judgment point location set, including:
Setting a distance threshold value and initializing a second judging point location set;
performing straight line fitting on the points in the first judgment point set to obtain an automobile track straight line;
And calculating the distance between each point in the first judgment point position set and the automobile track straight line, and if the absolute distance between the point in the first judgment point position set and the automobile track straight line is greater than or equal to the distance threshold value, putting the corresponding point in the second judgment point position set.
Further, performing gradient calculation based on the second decision point location set to obtain a gradient set, a second order gradient set and a third decision point location set, including:
Initializing a step set and a third decision point set;
Calculating first-order gradient values of all the points in the second judgment point location set, and putting the first-order gradient values obtained by calculation into the first-order gradient set;
traversing the one-step gradient set, and placing one point corresponding to two adjacent first-order gradient values with different positive and negative values into the third judgment point set;
initializing a second-order gradient set;
calculating second-order gradient values of all the points in the second judgment point position set, and putting the calculated second-order gradient values into the second-order gradient set;
Traversing the second-order gradient set, and placing one point which corresponds to two adjacent second-order gradient values with different positive and negative values and is not placed in the third judgment point position set into the third judgment point position set.
Further, the first-order gradient value of the current point is the distance between the corresponding current point in the second judgment point set and the datum reference line, and the difference value obtained by subtracting the distance between the last point and the datum reference line is obtained;
The second-order gradient value of the current point is the first-order gradient value of the corresponding current point in the second judgment point set, and the difference value obtained by subtracting the first-order gradient value of the last point is obtained.
Further, performing curvature calculation based on the third decision point location set to obtain a fourth decision point location set, including:
Setting a curvature threshold value and initializing a fourth judgment point location set;
and calculating the curvature of each point in the third judgment point position set corresponding to the automobile running track curve, and if the curvature is larger than or equal to the curvature threshold value, putting the point corresponding to the curvature into the fourth judgment point position set.
Further, the method further comprises:
if the fourth judgment point location set is empty, determining the point location in the third judgment point location set as a local interference point location;
Setting a trend judgment point bit number N and initializing a fifth judgment point position set;
Respectively calculating the median of the first N first-order gradient values and the last N first-order gradient values of the first-order gradient values corresponding to the local interference point positions in the first-order gradient set to obtain the median of the previous step and the median of the next step of the local interference point positions;
if the positive and negative signs of the previous step median and the subsequent step median are different, the corresponding local interference point is put into the fifth judgment point position set;
Respectively calculating the median of the first N second-order gradient values and the last N second-order gradient values of the second-order gradient values corresponding to the local interference point positions in the second-order gradient set to obtain the median of the first second-order gradient and the median of the last second-order gradient of the local interference point positions;
if the positive and negative signs of the median of the front second-order gradient and the median of the rear second-order gradient are different, the corresponding local interference point is placed in the fifth judgment point position set;
and when the fifth judgment point position set is not empty, determining that the automobile is under the intervention of a driver in the automatic driving process, and when the fifth judgment point position set is empty, determining that the automobile is not under the intervention of the driver in the automatic driving process.
A driver intervention judgment device in automatic driving of an automobile, comprising:
The point position acquisition module is used for acquiring a plurality of point positions in automatic driving of the automobile according to a preset time interval;
the first calculation module is used for calculating according to the plurality of point positions and the pre-planned base reference line and determining a first judgment point position set;
The second calculation module is used for carrying out fitting calculation based on the first judgment point location set to obtain a second judgment point location set when the first judgment point location set is not empty;
The third calculation module is used for carrying out gradient calculation based on the second judgment point location set when the second judgment point location set is not empty, so as to obtain a step degree set, a second order gradient set and a third judgment point location set;
The fourth calculation module is used for calculating curvature based on the third judgment point location set to obtain a fourth judgment point location set when the third judgment point location set is not empty;
and the judging module is used for determining that the automobile is interfered by a driver in automatic driving when the fourth judging point position set is not empty.
Further, the device also comprises a fitting module, which is used for fitting the plurality of point positions after acquiring the plurality of point positions in the automatic driving test of the automobile according to the preset time interval to obtain the running track curve of the automobile.
Further, the judging module is also used for determining that the automobile is not interfered by a driver in the automatic driving process of the automobile when the first judging point position set is empty, or
When the second judgment point set is empty, determining that the automobile is not interfered by a driver in the automatic driving process or
And when the third judgment point position set is empty, determining that the automobile is not interfered by a driver in automatic driving.
Further, the first calculation module calculates according to the plurality of points and a pre-planned base reference line, and determines a first determination point set, including:
establishing an initial point location set according to the acquired multiple point locations;
setting a tolerance threshold and initializing a first judgment point location set;
And calculating the distance between each point in the initial point location set and the datum reference line, and if the absolute distance between the point in the initial point location set and the datum reference line is greater than or equal to the tolerance threshold, putting the corresponding point into the first judgment point location set.
Further, the second calculation module performs fitting calculation based on the first determination point location set to obtain a second determination point location set, including:
Setting a distance threshold value and initializing a second judging point location set;
performing straight line fitting on the points in the first judgment point set to obtain an automobile track straight line;
And calculating the distance between each point in the first judgment point position set and the automobile track straight line, and if the absolute distance between the point in the first judgment point position set and the automobile track straight line is greater than or equal to the distance threshold value, putting the corresponding point in the second judgment point position set.
Further, the third calculation module performs gradient calculation based on the second determination point location set to obtain a gradient set, a second order gradient set, and a third determination point location set, including:
Initializing a step set and a third decision point set;
Calculating first-order gradient values of all the points in the second judgment point location set, and putting the first-order gradient values obtained by calculation into the first-order gradient set;
traversing the one-step gradient set, and placing one point corresponding to two adjacent first-order gradient values with different positive and negative values into the third judgment point set;
initializing a second-order gradient set;
calculating second-order gradient values of all the points in the second judgment point position set, and putting the calculated second-order gradient values into the second-order gradient set;
Traversing the second-order gradient set, and placing one point which corresponds to two adjacent second-order gradient values with different positive and negative values and is not placed in the third judgment point position set into the third judgment point position set.
Further, the first-order gradient value of the current point is the distance between the corresponding current point in the second judgment point set and the datum reference line, and the difference value obtained by subtracting the distance between the last point and the datum reference line is obtained;
The second-order gradient value of the current point is the first-order gradient value of the corresponding current point in the second judgment point set, and the difference value obtained by subtracting the first-order gradient value of the last point is obtained.
Further, the fourth calculation module performs curvature calculation based on the third set of determination points, to obtain a fourth set of determination points, including:
Setting a curvature threshold value and initializing a fourth judgment point location set;
and calculating the curvature of each point in the third judgment point position set corresponding to the automobile running track curve, and if the curvature is larger than or equal to the curvature threshold value, putting the point corresponding to the curvature into the fourth judgment point position set.
Further, the apparatus further comprises a fifth calculation module for:
if the fourth judgment point location set is empty, determining the point location in the third judgment point location set as a local interference point location;
Setting a trend judgment point bit number N and initializing a fifth judgment point position set;
Respectively calculating the median of the first N first-order gradient values and the last N first-order gradient values of the first-order gradient values corresponding to the local interference point positions in the first-order gradient set to obtain the median of the previous step and the median of the next step of the local interference point positions;
if the positive and negative signs of the previous step median and the subsequent step median are different, the corresponding local interference point is put into the fifth judgment point position set;
Respectively calculating the median of the first N second-order gradient values and the last N second-order gradient values of the second-order gradient values corresponding to the local interference point positions in the second-order gradient set to obtain the median of the first second-order gradient and the median of the last second-order gradient of the local interference point positions;
if the positive and negative signs of the median of the front second-order gradient and the median of the rear second-order gradient are different, the corresponding local interference point is placed in the fifth judgment point position set;
and when the fifth judgment point position set is not empty, determining that the automobile is under the intervention of a driver in the automatic driving process, and when the fifth judgment point position set is empty, determining that the automobile is not under the intervention of the driver in the automatic driving process.
The invention provides a method and a device for judging the intervention of a driver in the automatic driving of an automobile, which at least comprise the following beneficial effects:
(1) The method can accurately judge whether the driver intervenes in the automatic driving test of the automobile, is helpful for ensuring that the subsequent analysis is only based on the real data which is completely controlled by the automatic driving system, and improves the scientificity and accuracy of the automatic driving performance evaluation;
(2) By fitting the point positions, calculating the distance, calculating the first order and the second order and calculating the curvature in the automatic driving test of the automobile, whether the driver intervenes or not can be judged, the operation speed is high, meanwhile, the judgment result is accurate, and whether the driver interferes or not in the automatic driving test of the automobile can be judged efficiently;
(3) The method does not need additional test equipment, does not need pretreatment on the automobile or the test road section, and has low cost, simple operation and strong reproducibility;
(4) The method can also be applied to actual automatic driving, and the method can be used for determining the road sections which are interfered or not interfered by the driver, screening the road sections with more interference through big data, analyzing the road sections, and optimizing the corresponding automatic driving algorithm.
Drawings
Fig. 1 is a flowchart of an embodiment of a method for determining driver intervention in automatic driving of an automobile according to the present invention.
Fig. 2 (a) and fig. 2 (b) are schematic diagrams of vehicle track curves without intervention and offset in the method for judging driver intervention in automatic driving of a vehicle.
Fig. 3 (a) and fig. 3 (b) are schematic diagrams of a vehicle track curve without a dry pre-deviation in the method for determining driver intervention in automatic driving of a vehicle according to the present invention.
Fig. 4 (a) and fig. 4 (b) are schematic diagrams of an automobile track under a condition that there is a driver intervention in the method for determining driver intervention in automatic driving of an automobile according to the present invention.
Fig. 5 (a) and fig. 5 (b) are schematic diagrams of an automobile track diagram of another case of driver intervention in the method for determining driver intervention in automatic driving of an automobile according to the present invention.
Fig. 6 (a) to fig. 6 (c) are schematic diagrams of an automobile track with local interference points in the method for determining driver intervention in automatic driving of an automobile according to the present invention.
Fig. 7 is a flowchart of another embodiment of a method for determining driver intervention in automatic driving of an automobile according to the present invention.
Fig. 8 (a) to fig. 8 (j) are schematic diagrams of determining an automobile track curve in an application scenario of the method for determining driver intervention in automatic driving of an automobile according to the present invention.
Fig. 9 is a flowchart of an embodiment of a device for determining driver intervention in automatic driving of an automobile according to the present invention.
Detailed Description
In order to better understand the above technical solutions, the following detailed description will be given with reference to the accompanying drawings and specific embodiments.
Referring to fig. 1, in some embodiments, a method for determining driver intervention in automatic driving of an automobile is provided, including:
s1, acquiring a plurality of point positions in automatic driving of an automobile according to a preset time interval;
s2, calculating according to a plurality of point positions and a pre-planned base reference line, and determining a first judgment point position set;
s3, when the first judgment point position set is not empty, fitting calculation is carried out based on the first judgment point position set, and a second judgment point position set is obtained;
S4, when the second judgment point position set is not empty, carrying out gradient calculation based on the second judgment point position set to obtain a gradient set, a second-order gradient set and a third judgment point position set;
S5, when the third judgment point position set is not empty, curvature calculation is carried out based on the third judgment point position set, and a fourth judgment point position set is obtained;
And S6, when the fourth judgment point position set is not empty, determining that the automobile is interfered by a driver in automatic driving.
Specifically, if the automatic driving process of the automobile is not disturbed, the track curve of the automobile has two conditions, namely, the track curve is completely not deviated and keeps moving along the direction of the datum reference line, slight disturbance can occur in the process, or micro-bending of the curve occurs due to the positioning error of the automobile, as shown in fig. 2 (a) and fig. 2 (b), once the track deviates more than the upper part or the lower part of the datum reference line, the track always deviates towards the direction away from the datum line, the backward trend cannot occur, and the absolute value of the slope of the mathematical characteristic is kept unchanged or always increases, as shown in fig. 3 (a) and fig. 3 (b). Fig. 3 (a) and 3 (b) show the trajectory of the vehicle shifted above the baseline reference line, and downward without driver intervention.
If the automobile is interfered by the driver during the automatic driving process, the situation that the driver adjusts the direction once or adjusts the direction twice in the process, but the last adjustment just makes the automobile return to the datum reference line and travel along the datum reference line is that the track of the automobile is C-shaped as shown in fig. 4 (a) and 4 (b), and the situation that the driver adjusts the direction multiple times in the process, so that the track of the automobile has two or more inflection points and is S-shaped as shown in fig. 5 (a) and 5 (b) may occur.
In addition, the automobile positioning device has errors, and the fluctuation of an automobile track curve can be caused due to errors in the track point position data acquisition process, but the fluctuation is sharp and rapid, namely the curvature of the fluctuation inflection point is smaller, and the fluctuation point is called a local interference point. As shown in fig. 6 (a), 6 (b) and 6 (c), fig. 6 (a) and 6 (b) show two cases where local interference points occur, and fig. 6 (c) shows a case where local interference points occur. Local disturbance points are easily misinterpreted as being caused by driver disturbance, so care is taken to distinguish these points from inflection points that are actually caused by driver disturbance.
Further, in step S1, a plurality of points in the automatic driving test of the automobile are obtained according to a preset time interval, where the points are positions of the automobile at different moments in the automatic driving process. Assuming that the duration of auto-driving of the car is T seconds, one point is obtained every T milliseconds, then a total of (1000 x T/T) Rounding down points are included, designated p_ori_1, p_ori_2,..p_ori_ (1000 x T/T) Rounding down . And establishing an initial point position set according to all the acquired point positions, and fitting the initial point position set into a track curve by using a polynomial fitting or other fitting methods to obtain an automobile track curve.
And setting a reference line on the automobile track graph, wherein the reference line is the running track of the automobile, namely the running path planned by the automobile, under the condition that no deviation occurs in the process of hopeing automatic driving.
Further, in steps S2 to S4, when the first set of determination points is empty, it is determined that the vehicle is not being interfered by the driver during the automatic driving of the vehicle, or
When the second judgment point set is empty, determining that the automobile is not interfered by a driver in the automatic driving process or
And when the third judgment point position set is empty, determining that the automobile is not interfered by a driver in automatic driving.
Further, in step S2, a first set of decision points is determined by calculating according to the plurality of points and the pre-planned reference line, including:
S21, establishing an initial point location set according to the acquired multiple point locations;
s22, setting a tolerance threshold and initializing a first judgment point location set;
S23, calculating the distance between each point in the initial point location set and the datum reference line, and if the absolute distance between the point in the initial point location set and the datum reference line is greater than or equal to the tolerance threshold value, putting the corresponding point location into the first judgment point location set.
Specifically, a tolerance threshold x is set, an initialized first set of decision points is empty, a distance dis_baseline_i from each point p_ori_i in the initial set of points (i=1, 2, (1000 x t/t) Rounding down ) to a base reference line is calculated, (i=1, 2, (1000 x t/t) Rounding down ), the base reference line is defined as lateral, dis_baseline_i >0 if a point is above the base reference line, and dis_baseline_i <0 if a point is below the base reference line. If the absolute value of the bit-to-baseline_i, |dis_baseline_i| < x, then the bit-p_ori_i is considered not to be caused by driver disturbances, even if there is a slight fluctuation. If the absolute value |dis_baseline_i| of the distance dis_baseline_i between the point location and the base reference line is not less than x, p_ori_i is put into the first judgment point location set. If the first decision point set is empty, the curve is considered not to be interfered by the driver, the algorithm is ended, and otherwise, the step S3 is entered.
Further, in step S3, fitting calculation is performed based on the first set of determination points, to obtain a second set of determination points, including:
S31, setting a distance threshold value and initializing a second judgment point location set;
s32, performing straight line fitting on the points in the first judgment point set to obtain an automobile track straight line;
S33, calculating absolute distances between each point in the first judgment point location set and the automobile track straight line, and if the absolute distances between the points in the first judgment point location set and the automobile track straight line are greater than or equal to the distance threshold value, placing corresponding points in the second judgment point location set.
Specifically, assume that num1 points are in the first decision point set, and that all points are named p_fig_1, p_fig_2, p_fig_num 1 in order. And fitting an automobile track straight line strline according to the first judgment point position set. Wherein the starting point of the automobile track line is p_fir_1, and the ending point of the automobile track line is p_fir_num1.
And simultaneously setting a distance threshold maxD and initializing a second judgment point location set. The distance threshold is a real number and represents the distance between the point location and the straight line of the automobile track, and the second judgment point location set is initially set to be empty.
The absolute distance |dis_ strline _i| of each point p_fir_i (i=1, 2..num 1) in the first set of decision points to the vehicle track line is calculated. If |dis_ strline _i| < maxD, it is stated that the point p_fir_i is very close to the straight line of the track of the car, where there is a high probability of no driver disturbance, even if there is driver disturbance, where it is not possible to be the only one driver disturbance point of the whole track. If |dis_ strline _i| is not less than maxD, the point location is stored in the second decision point location set. If the second judgment point position set is empty, the curve is considered not to be interfered by the driver, the algorithm is ended, and otherwise, the step S4 is started.
Further, in step S4, gradient calculation is performed based on the second set of determination points to obtain a gradient set, a second-order gradient set, and a third set of determination points, including:
S41, initializing a step degree set and a third judgment point position set;
S42, calculating first-order gradient values of all the points in the second judgment point location set, and putting the calculated first-order gradient values into the first-order gradient set;
S43, traversing the one-step gradient set, and placing one point corresponding to two adjacent first-order gradient values with different positive and negative values into the third judgment point set;
s44, initializing a second-order gradient set;
S45, calculating second-order gradient values of all the points in the second judgment point position set, and putting the calculated second-order gradient values into the second-order gradient set;
S46, traversing the second-order gradient set, and placing one point which corresponds to two adjacent positive and negative second-order gradient values and is not placed in the third judgment point location set into the third judgment point location set.
The first-order gradient value of the current point is the distance between the corresponding current point in the second judgment point position set and the datum reference line, and the difference value obtained by subtracting the distance between the last point and the datum reference line is obtained;
The second-order gradient value of the current point is the first-order gradient value of the corresponding current point in the second judgment point set, and the difference value obtained by subtracting the first-order gradient value of the last point is obtained.
Specifically, assuming that num2 points exist in the second decision point set, all points are named p_sec_1, p_sec_2, and..p_sec_num 2 in order. Initializing a step degree set and a third decision point position set, calculating a first-order gradient value g_i=dis_baseline_i-dis_baseline_i_before of a point position p_sec_i (i=1, 2,..num 2) in the second decision point position set, wherein dis_baseline_i is the distance from a current point position to a reference line, dis_baseline_i_before is the distance from a previous point position of the current point position to the reference line on an automobile track curve, and then storing the first-order gradient value g_i into the step degree set. Traversing a gradient set, and if the first-order gradient value sign is positive or negative, storing the point positions with inconsistent first-order gradient value signs of the front point position and the rear point position into a third judgment point position set. If the first-order gradient values in a certain step gradient set are 3, 7, 4, 2, -1, -2, -3, -2, respectively, the point with the first-order gradient value of 2 (the 4 th point) or the point with the first-order gradient value of-1 (the 5 th point) is stored in the third decision point set.
Initializing a second-order gradient set, calculating a second-order gradient value gg_i=g_i-g_ (i-1) of a point p_sec_i (i=2,..num 1) in a second judging point position set, wherein g_i is a first-order gradient value of the point, g_ (i-1) is a first-order gradient value of a point before the point in the second judging point position set, and then storing the second-order gradient value gg_i in the second-order gradient set. Traversing the second-order gradient set, and if the sign of the second-order gradient value is positive and negative, storing the points which are inconsistent in the signs of the second-order gradient values of the front and back points and are not put into the third judgment point position set. If the second order gradient values in a certain second order gradient set are respectively 7, 4, 2, -1, -2, -3, -2, -3, then the point with the second order gradient value of 2 (3 rd point) or the point with the second order gradient value of-1 (4 th point) is stored in the third decision point set.
If the third judgment point position set is empty, the curve is considered not to be interfered by the driver, the algorithm is ended, and otherwise, the step S5 is started.
Further, in step S5, curvature calculation is performed based on the third set of determination points, to obtain a fourth set of determination points, including:
s51, setting a curvature threshold value and initializing a fourth judgment point location set;
S52, calculating the curvature of each point in the third judgment point position set corresponding to the automobile running track curve, and if the curvature is greater than or equal to the curvature threshold value, placing the point corresponding to the curvature into the fourth judgment point position set.
Specifically, assume that there are num3 points in the third decision point set, and that all points are named p_thi_1, p_thi_2, p_thi_num3 in order. Setting a curvature threshold c and initializing a fourth judgment point location set, firstly calculating the curvature cur_i of an automobile track curve of all the point locations p_thi_i (i=1, 2,..num 3) in the third judgment point location set at the point location, for the point location of cur_i < c, judging that the point location is a local interference point location caused by positioning error or data acquisition error, and if cur_i is more than or equal to c, storing the point location into the fourth judgment point location set.
In step S6, if the fourth set of determination points is not empty, the curve is considered to be interfered by the driver, and the algorithm is ended, otherwise, the points in the third determination points are all local interference points, and at this time, the situation that the local interference points are exactly located at the first-order or second-order gradient value sign change points of the whole curve may occur, so that further judgment is needed according to the automobile track curves at the two ends of the local interference points, and the steps S71-S77 are performed.
Further, the method further comprises:
S71, if the fourth judgment point location set is empty, determining the point location in the third judgment point location set as a local interference point location;
s72, setting a trend judgment point bit number N and initializing a fifth judgment point position set;
S73, respectively calculating the median of the first N first-order gradient values and the last N first-order gradient values of the first-order gradient values corresponding to the local interference point in the first-order gradient set to obtain the median of the first-order gradient and the median of the last-order gradient of the local interference point;
S74, if the positive and negative signs of the previous step median and the next step median are different, the corresponding local interference point is placed in the fifth judgment point set;
S75, respectively calculating the median of the first N second-order gradient values and the last N second-order gradient values of the second-order gradient values corresponding to the local interference point in the second-order gradient set to obtain the median of the first second-order gradient and the median of the last second-order gradient of the local interference point;
s76, if the positive and negative signs of the median of the front second-order gradient and the median of the rear second-order gradient are different, the corresponding local interference point is placed in the fifth judgment point set;
and S77, when the fifth judgment point position set is not empty, determining that the automobile is under the intervention of a driver in the automatic driving process, and when the fifth judgment point position set is empty, determining that the automobile is not under the intervention of the driver in the automatic driving process.
If the local interference point passes the first-order judgment and then is put into the fifth judgment point position set, the second-order judgment is not performed, and if the local interference point does not meet the first-order judgment, the second-order judgment is performed.
Specifically, a trend judgment point bit number N and a fifth judgment point bit set are set, for all the points in the third judgment point bit set, the first-order gradient values of the rear N points are taken in a step degree set and the median is calculated and is marked as a rear step degree median g_backsaward, and the first-order gradient values of the front N points are taken in a step degree set and the median is calculated and is marked as a front step degree median g_forward. If the sign of the middle bit g_backward of the last step of the point bit is inconsistent with the sign of the middle bit g_forward of the previous step, the point bit is stored in a fifth judgment point bit set.
If the fifth set of decision points is not empty, the curve is considered to be disturbed by the driver and the algorithm ends. Assuming that n=9 is taken, there is a local interference point, and the steps of the rear 9 points are respectively 4, 5, 6, 5, 3, 1, -2, -5, and the steps of the front 9 points are respectively-4, -2, 1, 3, 4, and 5, then the steps of the rear 9 points have a median g_background=4, and the steps of the front 10 points have a median g_background=3, and the g_background and g_forward symbols are identical, so the following determination is continued.
Otherwise, for each local interference point, taking the second-order gradient values of the N points at the rear of the second-order gradient set and calculating the median, and marking the second-order gradient values as the median gg_backsaward of the rear second-order gradient, and taking the second-order gradient values of the N points at the front of the second-order gradient set and calculating the median, and marking the second-order gradient values as the median gg_forward of the front second-order gradient. If a local interference point exists, and the secondary gradient median gg_backsaward is inconsistent with the primary secondary gradient median gg_forward sign, the point is stored in a fifth judgment point set. If the fifth set of decision points is not empty, the curve is considered to be disturbed by the driver and the algorithm ends.
Otherwise, the curve is considered not to be interfered by the driver, and the algorithm is ended.
The method provided by the above embodiment is further described below through a specific application scenario.
It is assumed that the points of the curve trace of the automobile obtained by a certain experiment are shown in fig. 8 (a).
Firstly, we preset a tolerance threshold value x, and use the distance from two broken lines in fig. 8 (b) to a reference line to represent, calculate the absolute distance from each point in the track point location set of the automobile to the reference line, if the absolute distance is greater than or equal to x, then store the point location into the first decision point location set, for example, as solid points in fig. 8 (c), and see that the first decision point location set is not empty.
A straight line is then fitted from the first set of decision points, as indicated by the dashed line in fig. 8 (d) obliquely upward. Setting a distance threshold maxD, representing the distance from two broken lines in fig. 8 (e) to a fitting straight line, calculating the absolute distance from each point in the track point position set of the automobile to the fitting straight line, and if the absolute distance is greater than or equal to maxD, storing the point position in a second judgment point position set, wherein the second judgment point position set is not empty as shown in solid points in fig. 8 (f), and the point position of the second judgment point position set is shown in fig. 8 (g).
And then calculating the first-order gradient and the second-order gradient of each point in the second judgment point position set, finding out the point positions of the sign changes of the first-order gradient and the second-order gradient value, and storing the point positions in a third judgment point position set, wherein the third judgment point position set is not empty as shown in solid points in fig. 8 (h), and the middle point position of the third judgment point position set is shown in fig. 8 (i).
Setting a curvature threshold value c and a fourth judgment point position set, calculating the curvature of each point in the third judgment point position set, and storing the point with the curvature larger than the curvature threshold value in the fourth judgment point position set, wherein the fourth judgment point position set is not empty as shown by solid points in fig. 8 (j), the explanation curve is interfered by a driver at this time, and the algorithm is ended after the judgment is completed.
Referring to fig. 9, in some embodiments, there is provided a driver intervention judgment device in automatic driving of an automobile, including:
The point position obtaining module 201 is configured to obtain a plurality of point positions in automatic driving of the automobile according to a preset time interval;
A first calculation module 202, configured to calculate according to a plurality of the points and a pre-planned base reference line, and determine a first set of determination points;
the second calculation module 203 is configured to perform fitting calculation based on the first determination point location set when the first determination point location set is not empty, to obtain a second determination point location set;
A third calculation module 204, configured to perform gradient calculation based on the second decision point location set when the second decision point location set is not empty, to obtain a step degree set, a second order gradient set, and a third decision point location set;
a fourth calculation module 205, configured to perform curvature calculation based on the third decision point location set when the third decision point location set is not empty, to obtain a fourth decision point location set;
and the judging module 206 is configured to determine that the vehicle is under the intervention of the driver during automatic driving when the fourth set of judging points is not empty.
Further, the device also comprises a fitting module, which is used for fitting the plurality of point positions after acquiring the plurality of point positions in the automatic driving of the automobile according to the preset time interval to obtain an automobile driving track curve.
Further, the judgment module 206 is further configured to determine that the vehicle is not being interfered by the driver during the automatic driving of the vehicle when the first set of judgment points is empty, or
When the second judgment point set is empty, determining that the automobile is not interfered by a driver in the automatic driving process or
And when the third judgment point position set is empty, determining that the automobile is not interfered by a driver in automatic driving.
Further, the first calculation module 202 calculates according to the plurality of points and the pre-planned base reference line, and determines a first set of determination points, including:
establishing an initial point location set according to the acquired multiple point locations;
setting a tolerance threshold and initializing a first judgment point location set;
And calculating the distance between each point in the initial point location set and the datum reference line, and if the absolute distance between the point in the initial point location set and the datum reference line is greater than or equal to the tolerance threshold, putting the corresponding point into the first judgment point location set.
Further, the second calculation module 203 performs fitting calculation based on the first set of determination points to obtain a second set of determination points, including:
Setting a distance threshold value and initializing a second judging point location set;
performing straight line fitting on the points in the first judgment point set to obtain an automobile track straight line;
And calculating the distance between each point in the first judgment point position set and the automobile track straight line, and if the absolute distance between the point in the first judgment point position set and the automobile track straight line is greater than or equal to the distance threshold value, putting the corresponding point in the second judgment point position set.
Further, the third calculation module 204 performs gradient calculation based on the second set of determination points to obtain a gradient set, a second-order gradient set, and a third set of determination points, including:
Initializing a step set and a third decision point set;
Calculating first-order gradient values of all the points in the second judgment point location set, and putting the first-order gradient values obtained by calculation into the first-order gradient set;
traversing the one-step gradient set, and placing one point corresponding to two adjacent first-order gradient values with different positive and negative values into the third judgment point set;
initializing a second-order gradient set;
calculating second-order gradient values of all the points in the second judgment point position set, and putting the calculated second-order gradient values into the second-order gradient set;
Traversing the second-order gradient set, and placing one point which corresponds to two adjacent second-order gradient values with different positive and negative values and is not placed in the third judgment point position set into the third judgment point position set.
Further, the first-order gradient value of the current point is the distance between the corresponding current point in the second judgment point set and the datum reference line, and the difference value obtained by subtracting the distance between the last point and the datum reference line is obtained;
The second-order gradient value of the current point is the first-order gradient value of the corresponding current point in the second judgment point set, and the difference value obtained by subtracting the first-order gradient value of the last point is obtained.
Further, the fourth calculation module 205 performs curvature calculation based on the third set of decision points to obtain a fourth set of decision points, including:
Setting a curvature threshold value and initializing a fourth judgment point location set;
and calculating the curvature of each point in the third judgment point position set corresponding to the automobile running track curve, and if the curvature is larger than or equal to the curvature threshold value, putting the point corresponding to the curvature into the fourth judgment point position set.
Further, the apparatus further comprises a fifth calculation module 207 for:
if the fourth judgment point location set is empty, determining the point location in the third judgment point location set as a local interference point location;
Setting a trend judgment point bit number N and initializing a fifth judgment point position set;
Respectively calculating the median of the first N first-order gradient values and the last N first-order gradient values of the first-order gradient values corresponding to the local interference point positions in the first-order gradient set to obtain the median of the previous step and the median of the next step of the local interference point positions;
if the positive and negative signs of the previous step median and the subsequent step median are different, the corresponding local interference point is put into the fifth judgment point position set;
Respectively calculating the median of the first N second-order gradient values and the last N second-order gradient values of the second-order gradient values corresponding to the local interference point positions in the second-order gradient set to obtain the median of the first second-order gradient and the median of the last second-order gradient of the local interference point positions;
if the positive and negative signs of the median of the front second-order gradient and the median of the rear second-order gradient are different, the corresponding local interference point is placed in the fifth judgment point position set;
and when the fifth judgment point position set is not empty, determining that the automobile is under the intervention of a driver in the automatic driving process, and when the fifth judgment point position set is empty, determining that the automobile is not under the intervention of the driver in the automatic driving process.
The method and the device for judging the intervention of the driver in the automatic driving of the automobile at least have the following beneficial effects:
(1) The method can accurately judge whether the driver intervenes in the automatic driving test of the automobile, is helpful for ensuring that the subsequent analysis is only based on the real data which is completely controlled by the automatic driving system, and improves the scientificity and accuracy of the automatic driving performance evaluation;
(2) By fitting the point positions, calculating the distance, calculating the first order and the second order and calculating the curvature in the automatic driving test of the automobile, whether the driver intervenes or not can be judged, the operation speed is high, meanwhile, the judgment result is accurate, and whether the driver interferes or not in the automatic driving test of the automobile can be judged efficiently;
(3) The method does not need additional test equipment, does not need pretreatment on the automobile or the test road section, and has low cost, simple operation and strong reproducibility;
(4) The method can also be applied to actual automatic driving, and the method can be used for determining the road sections which are interfered or not interfered by the driver, screening the road sections with more interference through big data, analyzing the road sections, and optimizing the corresponding automatic driving algorithm.
While preferred embodiments of the present invention have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. It is therefore intended that the following claims be interpreted as including the preferred embodiments and all such alterations and modifications as fall within the scope of the invention. It will be apparent to those skilled in the art that various modifications and variations can be made to the present invention without departing from the spirit or scope of the invention. Thus, it is intended that the present invention also include such modifications and alterations insofar as they come within the scope of the appended claims or the equivalents thereof.
Claims (10)
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202510143877.4A CN119596961B (en) | 2025-02-10 | 2025-02-10 | A method and device for judging driver intervention in automatic driving of a vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202510143877.4A CN119596961B (en) | 2025-02-10 | 2025-02-10 | A method and device for judging driver intervention in automatic driving of a vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN119596961A true CN119596961A (en) | 2025-03-11 |
| CN119596961B CN119596961B (en) | 2025-04-18 |
Family
ID=94831123
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202510143877.4A Active CN119596961B (en) | 2025-02-10 | 2025-02-10 | A method and device for judging driver intervention in automatic driving of a vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN119596961B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR102012230B1 (en) * | 2018-02-26 | 2019-08-21 | (주)에스더블유엠 | Method and apparatus for verifying operation of autonomous vehicle by test section |
| CN113219955A (en) * | 2021-05-13 | 2021-08-06 | 吉林大学 | Multi-driver in-the-loop driving test platform |
| CN116842688A (en) * | 2023-05-11 | 2023-10-03 | 长沙汽车创新研究院 | Online compliance verification system oriented to automatic driving decision algorithm |
| CN118887394A (en) * | 2024-09-29 | 2024-11-01 | 浙江双元科技股份有限公司 | A vehicle deviation detection method and system based on vision |
-
2025
- 2025-02-10 CN CN202510143877.4A patent/CN119596961B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR102012230B1 (en) * | 2018-02-26 | 2019-08-21 | (주)에스더블유엠 | Method and apparatus for verifying operation of autonomous vehicle by test section |
| CN113219955A (en) * | 2021-05-13 | 2021-08-06 | 吉林大学 | Multi-driver in-the-loop driving test platform |
| CN116842688A (en) * | 2023-05-11 | 2023-10-03 | 长沙汽车创新研究院 | Online compliance verification system oriented to automatic driving decision algorithm |
| CN118887394A (en) * | 2024-09-29 | 2024-11-01 | 浙江双元科技股份有限公司 | A vehicle deviation detection method and system based on vision |
Also Published As
| Publication number | Publication date |
|---|---|
| CN119596961B (en) | 2025-04-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111009153B (en) | Training method, device and equipment of trajectory prediction model | |
| CN108773373B (en) | Method and device for operating an autonomous vehicle | |
| CN111380538B (en) | Vehicle positioning method, navigation method and related device | |
| CN107491073A (en) | The data training method and device of automatic driving vehicle | |
| CN110779539B (en) | Driving path correction method, device, device and storage medium | |
| CN111267860B (en) | Sensor fusion target prediction device and method for vehicle and vehicle | |
| WO2022112397A1 (en) | Vehicle autonomous driving validation system and method, vehicle autonomous driving system, vehicle and computer readable storage medium | |
| CN112644487B (en) | Automatic driving method and device | |
| CN105718939A (en) | Vehicle parking locus drift removal method and apparatus based on increment clustering | |
| JP2020042786A (en) | Processing method of car image, processing device of car image and computer-readable storage medium | |
| CN113806465A (en) | Longitude and latitude correction method of bayonet position based on new energy vehicle trajectory data | |
| CN119596961B (en) | A method and device for judging driver intervention in automatic driving of a vehicle | |
| JP6514631B2 (en) | Steering angle detection device and steering angle detection method | |
| CN115195785B (en) | Optimization method, device, equipment and storage medium for autonomous driving model | |
| CN109991024B (en) | Three-level automatic driving vehicle over-bending capability test method | |
| CN113543014A (en) | Vehicle satellite positioning data aggregation optimization system and method thereof | |
| CN112346451B (en) | Automatic driving planning path safety verification method and device and automatic driving control system | |
| CN117789444A (en) | Parking lot data matching method, device, equipment, medium and vehicle | |
| JP5682302B2 (en) | Traveling road estimation device, method and program | |
| CN118606311A (en) | Vehicle trajectory completion method, device, electronic device and storage medium | |
| CN117689694A (en) | Track prediction method, track prediction device, computer equipment and storage medium | |
| US20210316780A1 (en) | Zero point compensation method and device for electric power steering | |
| CN117058506A (en) | Lane line fusion method and device | |
| CN115303288B (en) | Vehicle control method, control device and camera device | |
| US11403441B2 (en) | Method for checking a vehicle dynamics model |
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 |