CN113682314A - Slope detection method and terminal - Google Patents
Slope detection method and terminal Download PDFInfo
- Publication number
- CN113682314A CN113682314A CN202111105728.7A CN202111105728A CN113682314A CN 113682314 A CN113682314 A CN 113682314A CN 202111105728 A CN202111105728 A CN 202111105728A CN 113682314 A CN113682314 A CN 113682314A
- Authority
- CN
- China
- Prior art keywords
- slope
- height difference
- vehicle
- gradient
- laser
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
- B60W40/06—Road conditions
- B60W40/076—Slope angle of the road
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60T—VEHICLE BRAKE CONTROL SYSTEMS OR PARTS THEREOF; BRAKE CONTROL SYSTEMS OR PARTS THEREOF, IN GENERAL; ARRANGEMENT OF BRAKING ELEMENTS ON VEHICLES IN GENERAL; PORTABLE DEVICES FOR PREVENTING UNWANTED MOVEMENT OF VEHICLES; VEHICLE MODIFICATIONS TO FACILITATE COOLING OF BRAKES
- B60T7/00—Brake-action initiating means
- B60T7/12—Brake-action initiating means for automatic initiation; for initiation not subject to will of driver or passenger
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/72—Electric energy management in electromobility
Landscapes
- Engineering & Computer Science (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Mathematical Physics (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
According to the slope detection method and the terminal, the first laser distance is acquired according to the single-point laser obliquely arranged downwards in front of the vehicle, and the second laser distance is acquired according to the single-point laser after the vehicle runs for the preset time, so that the height difference of two acquisition points is calculated; judging the height change speed of the front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, if the height change speed is greater than a threshold value, the front slope is a cliff and controls the vehicle to brake, otherwise, calculating the gradient of the slope, and judging whether the front slope can pass through the slope smoothly according to the maximum accessible gradient; therefore, the slope gradient can be calculated when the altitude change speed is less than or equal to the threshold value, the calculation time is reduced, and the accuracy of slope detection is improved while the slope judgment is rapidly made before the vehicle enters the slope.
Description
Technical Field
The invention relates to the technical field of unmanned vehicle sensing, in particular to a slope detection method and a terminal.
Background
With the development of robotics in recent years, various robot products, such as cleaning robots, security robots, inspection robots, delivery robots, and the like, have been increasingly popularized. In order to ensure that the robot can work safely and reliably in an application scene. The robot needs to have the ability to sense the surrounding environment and then determine the ground passability.
At present, the robot ramp detection can use various sensors, such as a laser radar sensor, a depth camera, an infrared sensor, an ultrasonic sensor and the like. In general, the conventional ramp detection generally uses three-dimensional data of a field to calculate a ramp angle through a smooth interpolation method, or uses pictures acquired by a binocular camera to mark road conditions, or compares gray data of real-time images to perform slope estimation.
However, due to the limitation of the characteristics of the sensors and the influence of environmental factors, when sensors such as a depth camera, an infrared sensor and an ultrasonic sensor perform ramp detection, a large misjudgment rate exists, and a large amount of data calculation is required. The actual gradient of the ramp can not be correctly determined in time in some specific scenes, and great hidden danger is brought to normal operation of the machine.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the slope detection method and the terminal are provided, and the accuracy of slope detection can be improved.
In order to solve the technical problems, the invention adopts the technical scheme that:
a gradient detection method comprising the steps of:
acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of a vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time;
obtaining the height difference of two acquisition points according to the first laser distance and the second laser distance, judging the height change speed of a front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, and if the height change speed is greater than a threshold value, determining that the front slope is a cliff and controlling the vehicle to brake;
otherwise, calculating the gradient of the slope according to the height difference of the two acquisition points, the preset time of the interval of the two acquisition points and the running speed of the vehicle, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, and otherwise, controlling the vehicle to enter the slope.
In order to solve the technical problem, the invention adopts another technical scheme as follows:
a grade detection terminal comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, the processor implementing the following steps when executing the computer program:
acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of a vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time;
obtaining the height difference of two acquisition points according to the first laser distance and the second laser distance, judging the height change speed of a front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, and if the height change speed is greater than a threshold value, determining that the front slope is a cliff and controlling the vehicle to brake;
otherwise, calculating the gradient of the slope according to the height difference of the two acquisition points, the preset time of the interval of the two acquisition points and the running speed of the vehicle, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, and otherwise, controlling the vehicle to enter the slope.
The invention has the beneficial effects that: acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of the vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time, so as to calculate the height difference of two acquisition points; judging the height change speed of the front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, if the height change speed is greater than a threshold value, the front slope is a cliff and controls the vehicle to brake, otherwise, calculating the gradient of the slope, and judging whether the front slope can pass through the slope smoothly according to the maximum accessible gradient; therefore, the slope gradient can be calculated when the altitude change speed is less than or equal to the threshold value, the calculation time is reduced, and the accuracy of slope detection is improved while the slope judgment is rapidly made before the vehicle enters the slope.
Drawings
FIG. 1 is a flow chart of a method of slope detection according to an embodiment of the invention;
fig. 2 is a schematic diagram of a slope detection terminal according to an embodiment of the present invention;
fig. 3 is a schematic view illustrating calculation of a height difference of a single-point laser collection point of a downward slope in the slope detection method according to the embodiment of the present invention;
fig. 4 is a schematic view of a height difference between two collection points of single-point laser on a downward slope in the slope detection method according to the embodiment of the present invention;
fig. 5 is a schematic diagram illustrating calculation of height difference of single-point laser collection points of an upper slope according to the slope detection method of the embodiment of the present invention;
fig. 6 is a schematic diagram of a height difference between two collection points of single-point laser on an upper slope in the slope detection method according to the embodiment of the present invention;
FIG. 7 is a line graph of a single point laser data change in the event of a cliff being encountered for a method of slope detection according to an embodiment of the invention;
FIG. 8 is a single-point laser data change line graph under an incline condition for a slope detection method according to an embodiment of the present invention;
fig. 9 is a line graph of a single-point laser data change in the case of an up slope according to a slope detection method of an embodiment of the present invention.
Detailed Description
In order to explain technical contents, achieved objects, and effects of the present invention in detail, the following description is made with reference to the accompanying drawings in combination with the embodiments.
Referring to fig. 1, an embodiment of the present invention provides a slope detection method, including:
acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of a vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time;
obtaining the height difference of two acquisition points according to the first laser distance and the second laser distance, judging the height change speed of a front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, and if the height change speed is greater than a threshold value, determining that the front slope is a cliff and controlling the vehicle to brake;
otherwise, calculating the gradient of the slope according to the height difference of the two acquisition points, the preset time of the interval of the two acquisition points and the running speed of the vehicle, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, and otherwise, controlling the vehicle to enter the slope.
From the above description, the beneficial effects of the present invention are: acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of the vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time, so as to calculate the height difference of two acquisition points; judging the height change speed of the front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, if the height change speed is greater than a threshold value, the front slope is a cliff and controls the vehicle to brake, otherwise, calculating the gradient of the slope, and judging whether the front slope can pass through the slope smoothly according to the maximum accessible gradient; therefore, the slope gradient can be calculated when the altitude change speed is less than or equal to the threshold value, the calculation time is reduced, and the accuracy of slope detection is improved while the slope judgment is rapidly made before the vehicle enters the slope.
Further, obtaining the height difference of the two acquisition points according to the first laser distance and the second laser distance comprises:
calculating the height difference between the single-point laser and a first acquisition point according to the first laser distance and the inclination angle of the single-point laser, and calculating the first height difference between the lower edge of the wheel and the first acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
calculating the height difference between the single-point laser and a second acquisition point according to the second laser distance and the inclination angle of the single-point laser, and calculating the second height difference between the lower edge of the wheel and the second acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
and obtaining the height difference of the two acquisition points according to the difference value of the first height difference and the second height difference.
According to the description, the height difference between the single-point laser and the two collecting points can be calculated according to the laser distance obtained by two times of measurement and the inclination angle of the single-point laser, so that the height difference of the two collecting points can be calculated, and the subsequent judgment on the height change degree of the slope and the calculation on the slope gradient can be facilitated.
Further, judging the height change speed of the front slope according to the preset time interval between two acquisition points and the height difference between the two acquisition points, wherein if the height change speed is greater than a threshold value, the front slope is a cliff and the vehicle brake is controlled to comprise the following steps:
dividing the height difference of the two acquisition points by the preset time of the interval of the two acquisition points to obtain the height change speed of the front slope;
if the height change speed is greater than the threshold value, the front slope is a cliff, whether the height difference between the bottom surface of the cliff and the current plane is greater than the maximum difference height or not is judged, if yes, the vehicle is controlled to brake, and if not, the vehicle is controlled to continue to run.
From the above description, when the height change speed of the slope is greater than the threshold value, the front slope is the cliff, and therefore if the height difference between the bottom surface of the cliff and the current plane is less than or equal to the maximum difference height, the vehicle can continue to run, and the flexibility of the vehicle running is ensured.
Further, calculating the slope of the slope according to the height difference of the two acquisition points, the preset time interval between the two acquisition points and the running speed of the vehicle comprises:
judging whether the height difference values of the multiple continuously detected acquisition points and the single-point laser are gradually increased, if so, calculating the gradient of the descending slope:
θ1=arctan(d/((d/tanα)+v*t));
otherwise, calculate the slope of the upper ramp:
θ2=arctan(d/(v*t-(d/tanα)));
in the formula, theta 1 represents a downward slope gradient, theta 2 represents an upward slope gradient, d represents the height difference of two collection points, alpha represents the inclination angle of the single-point laser, v represents the running speed of the vehicle, and t represents the preset time of the interval of the two collection points.
According to the description, whether the slope is an ascending slope or a descending slope is judged according to whether the height difference value of the continuously detected multiple collecting points and the single-point laser is gradually increased or not, and the slope of the slope is correspondingly calculated according to the type of the slope, so that slope judgment can be rapidly made in the rapid movement process of the equipment, and the response sensitivity of the equipment can be improved.
Further, controlling the vehicle to brake if the grade is greater than a maximum passable grade, otherwise controlling the vehicle to enter the grade comprises:
if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, otherwise, if the height difference between the multiple continuously detected acquisition points and the single-point laser is in a trend of ascending before descending or descending before ascending, determining that the gradient is a bank or a groove, and judging whether to execute bank passing or groove passing operation according to the height of the bank or the groove.
As can be seen from the above description, when the slope gradient is less than or equal to the maximum accessible gradient, it is further necessary to determine whether the height difference between the multiple continuously detected collection points and the single-point laser is in a trend of ascending before descending or descending before ascending, if so, the slope is a threshold or a groove, and further determine whether to perform threshold-crossing or groove-crossing operation according to the depth of the threshold or the groove, so that corresponding determination can be performed according to different road conditions ahead, and the response sensitivity of the device is improved.
Referring to fig. 2, another embodiment of the present invention provides a slope detection terminal, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor executes the computer program to implement the following steps:
acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of a vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time;
obtaining the height difference of two acquisition points according to the first laser distance and the second laser distance, judging the height change speed of a front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, and if the height change speed is greater than a threshold value, determining that the front slope is a cliff and controlling the vehicle to brake;
otherwise, calculating the gradient of the slope according to the height difference of the two acquisition points, the preset time of the interval of the two acquisition points and the running speed of the vehicle, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, and otherwise, controlling the vehicle to enter the slope.
As can be seen from the above description, the first laser distance is acquired according to the single-point laser obliquely arranged downwards in front of the vehicle, and the second laser distance is acquired according to the single-point laser after the vehicle runs for the preset time, so that the height difference between the two acquisition points is calculated; judging the height change speed of the front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, if the height change speed is greater than a threshold value, the front slope is a cliff and controls the vehicle to brake, otherwise, calculating the gradient of the slope, and judging whether the front slope can pass through the slope smoothly according to the maximum accessible gradient; therefore, the slope gradient can be calculated when the altitude change speed is less than or equal to the threshold value, the calculation time is reduced, and the accuracy of slope detection is improved while the slope judgment is rapidly made before the vehicle enters the slope.
Further, obtaining the height difference of the two acquisition points according to the first laser distance and the second laser distance comprises:
calculating the height difference between the single-point laser and a first acquisition point according to the first laser distance and the inclination angle of the single-point laser, and calculating the first height difference between the lower edge of the wheel and the first acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
calculating the height difference between the single-point laser and a second acquisition point according to the second laser distance and the inclination angle of the single-point laser, and calculating the second height difference between the lower edge of the wheel and the second acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
and obtaining the height difference of the two acquisition points according to the difference value of the first height difference and the second height difference.
According to the description, the height difference between the single-point laser and the two collecting points can be calculated according to the laser distance obtained by two times of measurement and the inclination angle of the single-point laser, so that the height difference of the two collecting points can be calculated, and the subsequent judgment on the height change degree of the slope and the calculation on the slope gradient can be facilitated.
Further, judging the height change speed of the front slope according to the preset time interval between two acquisition points and the height difference between the two acquisition points, wherein if the height change speed is greater than a threshold value, the front slope is a cliff and the vehicle brake is controlled to comprise the following steps:
dividing the height difference of the two acquisition points by the preset time of the interval of the two acquisition points to obtain the height change speed of the front slope;
if the height change speed is greater than the threshold value, the front slope is a cliff, whether the height difference between the bottom surface of the cliff and the current plane is greater than the maximum difference height or not is judged, if yes, the vehicle is controlled to brake, and if not, the vehicle is controlled to continue to run.
From the above description, when the height change speed of the slope is greater than the threshold value, the front slope is the cliff, and therefore if the height difference between the bottom surface of the cliff and the current plane is less than or equal to the maximum difference height, the vehicle can continue to run, and the flexibility of the vehicle running is ensured.
Further, calculating the slope of the slope according to the height difference of the two acquisition points, the preset time interval between the two acquisition points and the running speed of the vehicle comprises:
judging whether the height difference values of the multiple continuously detected acquisition points and the single-point laser are gradually increased, if so, calculating the gradient of the descending slope:
θ1=arctan(d/((d/tanα)+v*t));
otherwise, calculate the slope of the upper ramp:
θ2=arctan(d/(v*t-(d/tanα)));
in the formula, theta 1 represents a downward slope gradient, theta 2 represents an upward slope gradient, d represents the height difference of two collection points, alpha represents the inclination angle of the single-point laser, v represents the running speed of the vehicle, and t represents the preset time of the interval of the two collection points.
According to the description, whether the slope is an ascending slope or a descending slope is judged according to whether the height difference value of the continuously detected multiple collecting points and the single-point laser is gradually increased or not, and the slope of the slope is correspondingly calculated according to the type of the slope, so that slope judgment can be rapidly made in the rapid movement process of the equipment, and the response sensitivity of the equipment can be improved.
Further, controlling the vehicle to brake if the grade is greater than a maximum passable grade, otherwise controlling the vehicle to enter the grade comprises:
if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, otherwise, if the height difference between the multiple continuously detected acquisition points and the single-point laser is in a trend of ascending before descending or descending before ascending, determining that the gradient is a bank or a groove, and judging whether to execute bank passing or groove passing operation according to the height of the bank or the groove.
As can be seen from the above description, when the slope gradient is less than or equal to the maximum accessible gradient, it is further necessary to determine whether the height difference between the multiple continuously detected collection points and the single-point laser is in a trend of ascending before descending or descending before ascending, if so, the slope is a threshold or a groove, and further determine whether to perform threshold-crossing or groove-crossing operation according to the depth of the threshold or the groove, so that corresponding determination can be performed according to different road conditions ahead, and the response sensitivity of the device is improved.
The slope detection method and the terminal are suitable for slope detection in the vehicle running process. The road gradient road condition on the driving route of the vehicle can be quickly and accurately detected, accurate environmental data are provided for judging the passability of the road for the vehicle, and the following description is given by a specific implementation mode:
example one
Referring to fig. 1, 3 to 9, a slope detection method includes the steps of:
and S1, acquiring a first laser distance according to the single-point laser obliquely arranged downwards in front of the vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time.
Specifically, in order to calculate the slope of the slope, the height drop change condition and the horizontal displacement distance between the two collected single-point laser collection points need to be acquired, so that the laser distance acquired by the two single-point lasers needs to be acquired according to the preset time.
S2, obtaining the height difference of the two acquisition points according to the first laser distance and the second laser distance, judging the height change speed of the front slope according to the preset time interval between the two acquisition points and the height difference of the two acquisition points, and if the height change speed is greater than a threshold value, enabling the front slope to be a cliff and controlling the vehicle to brake.
Obtaining the height difference of the two acquisition points according to the first laser distance and the second laser distance comprises the following steps:
and S21, calculating the height difference between the single-point laser and the first acquisition point according to the first laser distance and the inclination angle of the single-point laser, and calculating the first height difference between the lower edge of the wheel and the first acquisition point according to the height difference between the single-point laser and the lower edge of the wheel.
And S22, calculating the height difference between the single-point laser and the second acquisition point according to the second laser distance and the inclination angle of the single-point laser, and calculating the second height difference between the lower edge of the wheel and the second acquisition point according to the height difference between the single-point laser and the lower edge of the wheel.
And S23, obtaining the height difference of the two acquisition points according to the difference value of the first height difference and the second height difference.
Specifically, referring to fig. 3, l represents a laser distance acquired by a single-point laser; d represents the height difference between the ground acquisition point and the lower edge of the wheel; h1 represents the height difference between the single-point laser and the lower edge of the wheel; h2 represents the height difference between the single-point laser and the ground acquisition point; α represents a single-point laser installation angle;
the calculation process of the height difference between the ground acquisition point and the lower edge of the wheel comprises the following steps:
height difference between single-point laser and ground laser collection points: h2 ═ l × sin α;
height difference between the ground collection point and the lower edge of the wheel: d-h 2-h 1-l sin α -h 1;
referring to fig. 4, d2 represents the second height difference, and d3 represents the first height difference, so that the height difference d1 between two acquisition points is d2-d 3.
Similarly, please refer to fig. 5, wherein l represents the laser distance collected by the single-point laser; d represents the height difference between the ground acquisition point and the lower edge of the wheel; h1 represents the height difference between the single-point laser and the ground acquisition point; h2 represents the height difference between the single-point laser and the lower edge of the wheel; α represents a single-point laser installation angle;
the calculation process of the height difference between the ground acquisition point and the lower edge of the wheel comprises the following steps:
height difference between single-point laser and ground laser collection points: h2 ═ l × sin α;
height difference between the ground collection point and the lower edge of the wheel: d-h 2-h 1-h 2-l sin α;
referring to fig. 6, d2 represents the second height difference, and d3 represents the first height difference, so that the height difference d1 between two collection points is d2-d 3.
And S24, dividing the height difference of the two collection points by the preset time of the interval of the two collection points to obtain the height change speed of the front slope.
And S25, if the height change speed is greater than a threshold value, the front slope is a cliff, whether the height difference between the bottom surface of the cliff and the current plane is greater than the maximum difference height or not is judged, if yes, the vehicle is controlled to brake, and if not, the vehicle is controlled to continue to run.
Specifically, in the present embodiment, the threshold value of the height variation speed is 5000 mm/s;
referring to fig. 7 to 9, the height change speed in fig. 7 is 40000mm/s, so when data increases steeply, it is determined as a cliff, and at this time, it is also necessary to determine whether to brake according to whether the fall value between the bottom surface of the cliff and the current plane is greater than the maximum fall height;
the height change speed in fig. 8 is 750mm/s, and when the data is gradually increased, the data is judged to be a downward slope;
the height change speed in fig. 9 is 500mm/s, and when data is descending slowly, it is determined that the data is ascending.
And S3, if not, calculating the gradient of the slope according to the height difference of the two collection points, the preset time of the interval of the two collection points and the running speed of the vehicle, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, otherwise, controlling the vehicle to enter the slope.
Wherein, step S3 specifically includes the following steps:
s31, if not, judging whether the height difference value between the multiple continuously detected acquisition points and the single-point laser gradually increases, if so, calculating the gradient of the descending slope:
θ1=arctan(d/((d/tanα)+v*t));
otherwise, calculate the slope of the upper ramp:
θ2=arctan(d/(v*t-(d/tanα)));
in the formula, theta 1 represents a downward slope gradient, theta 2 represents an upward slope gradient, d represents the height difference of two collection points, alpha represents the inclination angle of the single-point laser, v represents the running speed of the vehicle, and t represents the preset time of the interval of the two collection points.
Specifically, if the height difference between the continuously detected multiple collection points and the single-point laser gradually increases, the slope of the descending slope is calculated:
referring to fig. 4, the auxiliary line corresponds to a distance s1 ═ d1/tan α;
the vehicle horizontal displacement distance is s2 ═ v × t;
the horizontal distance between two single-point laser acquisition points is s1+ s 2;
the slope inclination angle is θ ═ arctan (d 1/s).
If the height difference between a plurality of continuously detected acquisition points and the single-point laser gradually decreases, calculating the gradient of an upper slope:
referring to fig. 6, the auxiliary line corresponds to a distance s1 ═ d1/tan α;
the vehicle horizontal displacement distance is s2 ═ v × t;
the horizontal distance between two single-point laser collection points is s2-s 1;
the slope inclination angle is θ ═ arctan (d 1/s).
And S32, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, otherwise, if the continuously detected multiple data differences have a trend of ascending first and descending second or ascending first and descending second, determining the gradient to be a threshold or a ditch, and judging whether to execute threshold passing or ditch passing operation according to the height of the threshold or the ditch.
Specifically, whether the slope is a threshold or a ditch is judged according to the slope gradient of the slope and the height of the slope, if yes, whether threshold-crossing or ditch-crossing operation is executed is judged according to the height of the threshold or the ditch, and if not, the vehicle is directly controlled to enter the slope.
Therefore, the present embodiment determines whether there is an upward slope or a downward slope or a cliff in the forward direction of the vehicle based on the change of data measured by the single-point laser obliquely installed right ahead of the vehicle during forward movement. The slope and cliff are mainly distinguished according to whether the data collected by the single-point laser is steeply increased, namely whether the change rate exceeds a threshold value. If the data is increased sharply, the data is judged as a cliff, and if the data is decreased slowly or increased slowly, the data is judged as an upward slope or a downward slope. Under the condition of the slope, the system can calculate the slope of the slope according to the change condition of the single-point laser data, and judge the passability of the road surface according to the slope of the slope, so that the road slope road condition on the vehicle driving route can be detected quickly and accurately.
Example two
Referring to fig. 2, a slope detection terminal includes a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor executes the computer program to implement the steps of a slope detection method according to an embodiment.
In summary, according to the slope detection method and the terminal provided by the invention, the first laser distance is acquired according to the single-point laser obliquely arranged downwards in front of the vehicle, and the second laser distance is acquired according to the single-point laser after the vehicle runs for the preset time, so that the height difference of two acquisition points is calculated; and judging the height change speed of the front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, if the height change speed is greater than a threshold value, judging the front slope as a cliff and controlling the vehicle to brake, otherwise, judging the front slope as a downward slope when the data is slowly increased, entering a downward slope angle calculation process, and judging the road surface trafficability according to the downward slope angle. And when the data slowly falls, determining that the road surface can pass through the road surface, entering an upward slope angle calculation process, and determining the road surface can pass through according to the upward slope angle. Judging that the data are over ditch and over bank respectively if the data are ascending first and then descending and are ascending first and then ascending; during the running process of the vehicle, if an up/down slope exceeding the maximum passing gradient of the equipment exists in the running direction, the vehicle responds and performs processing; therefore, the slope gradient can be calculated when the altitude change speed is less than or equal to the threshold value, the calculation time is reduced, and the accuracy of slope detection is improved while the slope judgment is rapidly made before the vehicle enters the slope.
The above description is only an embodiment of the present invention, and not intended to limit the scope of the present invention, and all equivalent changes made by using the contents of the present specification and the drawings, or applied directly or indirectly to the related technical fields, are included in the scope of the present invention.
Claims (10)
1. A gradient detection method is characterized by comprising the following steps:
acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of a vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time;
obtaining the height difference of two acquisition points according to the first laser distance and the second laser distance, judging the height change speed of a front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, and if the height change speed is greater than a threshold value, determining that the front slope is a cliff and controlling the vehicle to brake;
otherwise, calculating the gradient of the slope according to the height difference of the two acquisition points, the preset time of the interval of the two acquisition points and the running speed of the vehicle, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, and otherwise, controlling the vehicle to enter the slope.
2. The slope detection method of claim 1, wherein obtaining the height difference of the two acquisition points according to the first laser distance and the second laser distance comprises:
calculating the height difference between the single-point laser and a first acquisition point according to the first laser distance and the inclination angle of the single-point laser, and calculating the first height difference between the lower edge of the wheel and the first acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
calculating the height difference between the single-point laser and a second acquisition point according to the second laser distance and the inclination angle of the single-point laser, and calculating the second height difference between the lower edge of the wheel and the second acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
and obtaining the height difference of the two acquisition points according to the difference value of the first height difference and the second height difference.
3. The gradient detection method according to claim 1, wherein the step of judging the height change speed of the front slope according to the preset time interval between two acquisition points and the height difference between the two acquisition points, and if the height change speed is greater than a threshold value, the front slope is a cliff and the vehicle brake is controlled comprises the following steps:
dividing the height difference of the two acquisition points by the preset time of the interval of the two acquisition points to obtain the height change speed of the front slope;
if the height change speed is greater than the threshold value, the front slope is a cliff, whether the height difference between the bottom surface of the cliff and the current plane is greater than the maximum difference height or not is judged, if yes, the vehicle is controlled to brake, and if not, the vehicle is controlled to continue to run.
4. The grade detection method according to claim 1, wherein calculating the grade of the slope according to the height difference between the two collection points, the preset time between the two collection points and the running speed of the vehicle comprises:
judging whether the height difference values of the multiple continuously detected acquisition points and the single-point laser are gradually increased, if so, calculating the gradient of the descending slope:
θ1=arctan(d/((d/tanα)+v*t));
otherwise, calculate the slope of the upper ramp:
θ2=arctan(d/(v*t-(d/tanα)));
in the formula, theta 1 represents a downward slope gradient, theta 2 represents an upward slope gradient, d represents the height difference of two collection points, alpha represents the inclination angle of the single-point laser, v represents the running speed of the vehicle, and t represents the preset time of the interval of the two collection points.
5. A grade detection method according to claim 1 wherein controlling the vehicle to brake if the grade is greater than a maximum passable grade, and otherwise controlling the vehicle to enter the grade comprises:
if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, otherwise, if the height difference between the multiple continuously detected acquisition points and the single-point laser is in a trend of ascending before descending or descending before ascending, determining that the gradient is a bank or a groove, and judging whether to execute bank passing or groove passing operation according to the height of the bank or the groove.
6. A grade detection terminal comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor when executing the computer program implements the steps of:
acquiring a first laser distance according to single-point laser obliquely arranged downwards in front of a vehicle, and acquiring a second laser distance according to the single-point laser after the vehicle runs for a preset time;
obtaining the height difference of two acquisition points according to the first laser distance and the second laser distance, judging the height change speed of a front slope according to the preset time of the interval of the two acquisition points and the height difference of the two acquisition points, and if the height change speed is greater than a threshold value, determining that the front slope is a cliff and controlling the vehicle to brake;
otherwise, calculating the gradient of the slope according to the height difference of the two acquisition points, the preset time of the interval of the two acquisition points and the running speed of the vehicle, if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, and otherwise, controlling the vehicle to enter the slope.
7. The slope detection terminal of claim 6, wherein obtaining the height difference between the two collection points according to the first laser distance and the second laser distance comprises:
calculating the height difference between the single-point laser and a first acquisition point according to the first laser distance and the inclination angle of the single-point laser, and calculating the first height difference between the lower edge of the wheel and the first acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
calculating the height difference between the single-point laser and a second acquisition point according to the second laser distance and the inclination angle of the single-point laser, and calculating the second height difference between the lower edge of the wheel and the second acquisition point according to the height difference between the single-point laser and the lower edge of the wheel;
and obtaining the height difference of the two acquisition points according to the difference value of the first height difference and the second height difference.
8. The grade detection terminal according to claim 6, wherein the step of determining the height change speed of the front slope according to the preset time interval between the two collection points and the height difference between the two collection points, and if the height change speed is greater than a threshold value, the front slope is a cliff and the vehicle brake is controlled comprises:
dividing the height difference of the two acquisition points by the preset time of the interval of the two acquisition points to obtain the height change speed of the front slope;
if the height change speed is greater than the threshold value, the front slope is a cliff, whether the height difference between the bottom surface of the cliff and the current plane is greater than the maximum difference height or not is judged, if yes, the vehicle is controlled to brake, and if not, the vehicle is controlled to continue to run.
9. The grade detection terminal of claim 6, wherein calculating the grade of the slope according to the height difference between the two collection points, the preset time between the two collection points and the running speed of the vehicle comprises:
judging whether the height difference values of the multiple continuously detected acquisition points and the single-point laser are gradually increased, if so, calculating the gradient of the descending slope:
θ1=arctan(d/((d/tanα)+v*t));
otherwise, calculate the slope of the upper ramp:
θ2=arctan(d/(v*t-(d/tanα)));
in the formula, theta 1 represents a downward slope gradient, theta 2 represents an upward slope gradient, d represents the height difference of two collection points, alpha represents the inclination angle of the single-point laser, v represents the running speed of the vehicle, and t represents the preset time of the interval of the two collection points.
10. The grade detection terminal of claim 6, wherein if the grade is greater than a maximum passable grade, controlling the vehicle to brake, otherwise, controlling the vehicle to enter the grade comprises:
if the gradient is larger than the maximum accessible gradient, controlling the vehicle to brake, otherwise, if the height difference between the multiple continuously detected acquisition points and the single-point laser is in a trend of ascending before descending or descending before ascending, determining that the gradient is a bank or a groove, and judging whether to execute bank passing or groove passing operation according to the height of the bank or the groove.
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310455163.8A CN116476846A (en) | 2021-09-22 | 2021-09-22 | Gradient detection method, vehicle using gradient detection method and terminal |
CN202111105728.7A CN113682314B (en) | 2021-09-22 | 2021-09-22 | Gradient detection method and terminal |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111105728.7A CN113682314B (en) | 2021-09-22 | 2021-09-22 | Gradient detection method and terminal |
Related Child Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310455163.8A Division CN116476846A (en) | 2021-09-22 | 2021-09-22 | Gradient detection method, vehicle using gradient detection method and terminal |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113682314A true CN113682314A (en) | 2021-11-23 |
CN113682314B CN113682314B (en) | 2023-06-02 |
Family
ID=78586901
Family Applications (2)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111105728.7A Active CN113682314B (en) | 2021-09-22 | 2021-09-22 | Gradient detection method and terminal |
CN202310455163.8A Pending CN116476846A (en) | 2021-09-22 | 2021-09-22 | Gradient detection method, vehicle using gradient detection method and terminal |
Family Applications After (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310455163.8A Pending CN116476846A (en) | 2021-09-22 | 2021-09-22 | Gradient detection method, vehicle using gradient detection method and terminal |
Country Status (1)
Country | Link |
---|---|
CN (2) | CN113682314B (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115435077A (en) * | 2022-08-31 | 2022-12-06 | 中国重汽集团济南动力有限公司 | Gear control method and device based on heavy-duty vehicle and storage medium |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0735862A (en) * | 1993-07-20 | 1995-02-07 | Komatsu Ltd | Obstacle detector for vehicle |
JP2005043261A (en) * | 2003-07-23 | 2005-02-17 | Denso Corp | Vehicle run support system |
CN101644568A (en) * | 2008-08-07 | 2010-02-10 | 矢崎总业株式会社 | Slope detection method and slope detector |
US20150336546A1 (en) * | 2014-05-19 | 2015-11-26 | Umm Al-Qura University | Method and system for vehicle to sense roadblock |
CN108016440A (en) * | 2016-11-02 | 2018-05-11 | 法乐第(北京)网络科技有限公司 | Control method for vehicle, device and electronic equipment |
JP2018135042A (en) * | 2017-02-23 | 2018-08-30 | 三菱自動車工業株式会社 | Slope-descending speed controller for vehicle |
CN112918444A (en) * | 2021-03-22 | 2021-06-08 | 苏州大学 | Parking control method and device for self-moving equipment |
CN113212399A (en) * | 2021-05-31 | 2021-08-06 | 东风柳州汽车有限公司 | Steep slope slow descending control system based on electronic calipers |
-
2021
- 2021-09-22 CN CN202111105728.7A patent/CN113682314B/en active Active
- 2021-09-22 CN CN202310455163.8A patent/CN116476846A/en active Pending
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0735862A (en) * | 1993-07-20 | 1995-02-07 | Komatsu Ltd | Obstacle detector for vehicle |
JP2005043261A (en) * | 2003-07-23 | 2005-02-17 | Denso Corp | Vehicle run support system |
CN101644568A (en) * | 2008-08-07 | 2010-02-10 | 矢崎总业株式会社 | Slope detection method and slope detector |
US20150336546A1 (en) * | 2014-05-19 | 2015-11-26 | Umm Al-Qura University | Method and system for vehicle to sense roadblock |
CN108016440A (en) * | 2016-11-02 | 2018-05-11 | 法乐第(北京)网络科技有限公司 | Control method for vehicle, device and electronic equipment |
JP2018135042A (en) * | 2017-02-23 | 2018-08-30 | 三菱自動車工業株式会社 | Slope-descending speed controller for vehicle |
CN112918444A (en) * | 2021-03-22 | 2021-06-08 | 苏州大学 | Parking control method and device for self-moving equipment |
CN113212399A (en) * | 2021-05-31 | 2021-08-06 | 东风柳州汽车有限公司 | Steep slope slow descending control system based on electronic calipers |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115435077A (en) * | 2022-08-31 | 2022-12-06 | 中国重汽集团济南动力有限公司 | Gear control method and device based on heavy-duty vehicle and storage medium |
Also Published As
Publication number | Publication date |
---|---|
CN113682314B (en) | 2023-06-02 |
CN116476846A (en) | 2023-07-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP3280976B1 (en) | Object position measurement with automotive camera using vehicle motion data | |
CN106096525B (en) | A kind of compound lane recognition system and method | |
EP3324152B1 (en) | Own-position estimating device and own-position estimating method | |
KR101762504B1 (en) | Method for detecting floor obstacle using laser range finder | |
KR101152720B1 (en) | Apparaus and Method for Detecting Slip of a Mobile Robot | |
EP1978432B1 (en) | Routing apparatus for autonomous mobile unit | |
KR101405583B1 (en) | Apparatus and method for estimating velocity of vehicle | |
CN103781685A (en) | Autonomous driving control system for vehicle | |
CN106476728A (en) | Motion compensation for vehicle-mounted vehicle sensors | |
AU2007201110A1 (en) | Systems and methods for determining threshold warning distances for collision avoidance | |
KR102570338B1 (en) | Method and system for predicting a trajectory of a target vehicle in an environment of a vehicle | |
CN109080628A (en) | Target determines equipment and driving assistance system | |
JP6776202B2 (en) | In-vehicle camera calibration device and method | |
CN113682314B (en) | Gradient detection method and terminal | |
CN111722249A (en) | Object recognition device and vehicle control system | |
EP3901573A1 (en) | Information processing device and mobile robot | |
JP5056861B2 (en) | Ranging device | |
KR102401382B1 (en) | Road surface detection system using Lidar and road surface detection method using the same | |
US6956959B2 (en) | Apparatus for recognizing environment | |
JP6263453B2 (en) | Momentum estimation device and program | |
CN104978568B (en) | Front vehicles detection method | |
JPH05157558A (en) | Vehicular gap detector | |
JP2019067115A (en) | Road surface detecting device | |
EP3859475A1 (en) | Information processing device and mobile robot | |
CN109753982B (en) | Obstacle point detection method, obstacle point detection device, and computer-readable storage medium |
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 |