CN114690780A - Method for allowing unmanned rail electric locomotive to pass through slope and curve in deep limited space - Google Patents
Method for allowing unmanned rail electric locomotive to pass through slope and curve in deep limited space Download PDFInfo
- Publication number
- CN114690780A CN114690780A CN202210383227.3A CN202210383227A CN114690780A CN 114690780 A CN114690780 A CN 114690780A CN 202210383227 A CN202210383227 A CN 202210383227A CN 114690780 A CN114690780 A CN 114690780A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- electric locomotive
- data
- unmanned
- laser radar
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000003137 locomotive effect Effects 0.000 title claims abstract description 81
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000008569 process Effects 0.000 claims abstract description 20
- 239000007787 solid Substances 0.000 claims abstract description 16
- 230000001603 reducing effect Effects 0.000 claims abstract description 7
- 230000001174 ascending effect Effects 0.000 claims abstract description 6
- 230000001133 acceleration Effects 0.000 claims description 21
- 238000004364 calculation method Methods 0.000 claims description 15
- 230000005484 gravity Effects 0.000 claims description 10
- 230000011218 segmentation Effects 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 230000009471 action Effects 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 230000000295 complement effect Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000009466 transformation Effects 0.000 description 5
- 238000005065 mining Methods 0.000 description 4
- 230000003068 static effect Effects 0.000 description 4
- 239000003245 coal Substances 0.000 description 3
- 206010034719 Personality change Diseases 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 239000011435 rock Substances 0.000 description 2
- 239000002131 composite material Substances 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a method for the gradient and curve passage of an unmanned rail electric locomotive in a deep limited space, wherein the rail electric locomotive travels in an underground roadway at a constant speed, and road data in the traveling process is collected in real time by a solid laser radar and fed back to an industrial control computer to obtain an original three-dimensional point cloud data packet and imu data; and then segmenting the ground point cloud data and the non-ground point cloud data by using a RANSAC algorithm. Calculating the pose of the current solid-state laser radar according to the collected imu data, feeding back whether the electric locomotive runs on the slope or not, further obtaining the length and the position of the uphill slope and the downhill slope, increasing the accelerator of the electric locomotive when the uphill slope is reached, and reducing the accelerator when the downhill slope is reached, so that the electric locomotive can run normally at a constant speed; and judging whether the curve is a curved road according to the shape of the roadway wall, and calculating the curvature of the curved road and the position of the curved road. The unmanned rail electric locomotive can accurately identify the ascending, descending and curve, thereby ensuring the smooth passing of the unmanned rail electric locomotive under the mine.
Description
Technical Field
The invention relates to an environment sensing method of an unmanned vehicle, in particular to a method for the slope and curve passage of an unmanned rail electric locomotive in a deep limited space.
Background
The coal mine auxiliary transportation is an important link of a mine production system and takes the transportation tasks of personnel, equipment, materials and the like. The auxiliary transportation is divided into rail transportation and trackless transportation. The geological structure of some mining areas is abnormally complex, the section of a roadway is small, and the advancing length of a coal face is short and is generally 400-600 m; the number of times of changing coal mining working faces of one mining area per year is large, and the auxiliary transportation line extends or migrates frequently along with the working place; most of the surrounding rocks of the roadway are composite soft rocks, and the phenomena of crushing and falling of a top plate and heaving of a bottom plate of the roadway are serious. The roadway is large in gradient, variable in slope and large in turning, and how to realize unmanned driving of the roadway is achieved, so that the method has great application prospect and strategic value for reducing property loss and casualty rate, reducing transportation cost and improving mining work efficiency.
The environmental perception is the basis for realizing unmanned driving of the underground rail electric locomotive. At present, the main research direction of the environmental perception obstacle avoidance traffic at home and abroad is under the environment of an expressway and an urban road, and few people exist at present aiming at the traffic of slopes and curves under the underground special environment. And for ground road, the underworkings structure is complicated, the slope is great and narrow many bends from top to bottom for it is very difficult, wastes time and energy to pass smoothly under underworkings environment. The existing mode is generally to fuse and sense a laser radar and a camera on a rail electric locomotive, but the mode has complex data processing, increases the calculation amount, has low illuminance of a tunnel underground, and is difficult for the camera to restore a real environment, so that the unmanned rail electric locomotive is misjudged as an obstacle to stop when encountering uphill and downhill paths and curves, or the situation that the speed of the unmanned rail electric locomotive is too fast to derail when passing the uphill and downhill paths and the curves because the uphill and downhill paths and the curves are not detected, and how to provide a method, the unmanned rail electric locomotive can accurately identify the uphill and downhill paths and the curves, thereby ensuring the smooth passing of the unmanned rail electric locomotive under a mine is one of the research directions of the industry.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a method for passing the slope and the curve of the rail electric locomotive in the deep limited space, so that the unmanned rail electric locomotive can accurately identify the ascending, the descending and the curve, and the smooth passing of the unmanned rail electric locomotive under a mine is ensured.
In order to achieve the purpose, the invention adopts the technical scheme that: a method for the gradient and curve passage of an unmanned rail electric locomotive in a deep limited space comprises the following specific steps:
(1) the method comprises the steps that a solid-state laser radar is installed at the front vehicle head of the unmanned electric rail vehicle, an accelerometer and a gyroscope are arranged in the solid-state laser radar, the orientation of the solid-state laser radar can automatically rotate, then the solid-state laser radar is started and is kept to face to the front of the unmanned electric rail vehicle, an underground roadway crossing is taken as a starting point, the unmanned electric rail vehicle can move forwards along a roadway at a constant speed, meanwhile, a bag of solid-state laser radar measurement data and imu data acquired by the gyroscope and the accelerometer every time are stored by an industrial control computer, the unmanned electric rail vehicle traverses all traveling areas in the underground roadway, and the industrial control computer removes miscellaneous points and noise in the data according to the acquired data to obtain a data packet of original three-dimensional point cloud and imu data of the whole traversal process;
(2) performing ground segmentation processing on the obtained original three-dimensional point cloud, and segmenting ground point cloud data and non-ground point cloud data by using a RANSAC (random Sample consensus) algorithm;
(3) the industrial control computer selects the imu data and the corresponding time of the imu data acquired in the step (1), calculates the pose of the current solid-state laser radar through the imu data, determines whether the unmanned rail electric locomotive runs on the up-down slope at the time corresponding to the imu data according to the pose of the solid-state laser radar, repeats the step for many times, analyzes the imu data acquired each time, obtains whether the unmanned rail electric locomotive is on the up-down slope at each corresponding time, and multiplies the running speed of the unmanned rail electric locomotive in the step (1) by each corresponding time to obtain the length and the position of the up-down slope;
(4) judging whether the road is a curve or not according to the non-ground point cloud data segmented in the step (2) and the shape of the wall of the roadway, calculating the curvature of the curve and the position of the curve, and correspondingly rotating the solid laser radar according to the curvature of the curve to enable the x axis of the solid laser radar to be parallel to the track, so that the solid laser radar is prevented from mistakenly identifying the side slope of the roadway as an obstacle;
(5) when the unmanned rail electric locomotive starts to be transported normally, the running speed can be automatically adjusted when the unmanned rail electric locomotive passes through the up-down slope or the curve according to the length and the position of the up-down slope determined in the step (3) and the curvature and the position of the curve determined in the step (4), so that the unmanned rail electric locomotive can be ensured to pass smoothly.
Further, the unmanned rail electric locomotive in the step (1) runs at a constant speed of 1 m/s.
Further, the specific process of the ground segmentation processing in the step (2) is as follows:
A. firstly, dividing an original three-dimensional point cloud into a plurality of sub-point cloud sections along the advancing direction of the unmanned rail electric locomotive, setting the ground in each sub-point cloud section as a plane, reducing the influence of gradient change on a division result, and then dividing the ground point cloud from the point cloud data in each sub-point cloud section by using an RANSAC algorithm; the RANSAC algorithm divides the concrete flow of the ground:
B. selecting point cloud data in a sub-point cloud segment as a data set, randomly selecting a plurality of points in the data set as local points, fitting the local points into a plane model, determining the number of the points according to the characteristics of the plane, namely fitting the plane, and selecting at least three points which are not collinear; sequentially calculating whether other points in the data set meet the plane model or not according to a set threshold (the threshold refers to the distance from other point clouds on two sides of the plane to the plane by taking the fitted plane as a reference, if the threshold is within the threshold, the point can be considered to be on the plane, and if the threshold is within the threshold, the point can be set to be 1cm), and if the point meets the plane model, marking the point as a local point of the plane; after the calculation of all the points is completed, recording the number of the local points of the plane model at the current time; if the number of the obtained local points is more than or equal to the number of the original local points of the plane model, fitting each local point obtained by the calculation into a new plane model, replacing the original plane model, repeating whether other points in the calculation data set meet the current plane model or not, if the number of the obtained local points is less than the number of the original local points of the plane model, keeping the current plane model, randomly selecting the same number of points in the data set to be local points, fitting the current local points into the plane model, repeating the calculation steps, repeating the iteration for multiple times until an iteration exit condition is met, exiting the cycle, and obtaining the optimal plane model in the whole iteration process; then, dividing the point cloud data in the current sub-point cloud segment into ground according to the plane model to obtain the ground point cloud data and non-ground point cloud data of the sub-point cloud segment;
C. and C, repeating the step B to segment the ground for other sub-point cloud sections so as to obtain the ground point cloud data of the original three-dimensional point cloud and the non-ground point cloud data.
Further, the process of determining the iteration exit condition is as follows:
adopting W to represent the probability of randomly drawing the local point, and P to represent the confidence coefficient which represents the expected probability that the RANSAC algorithm provides a useful result after running; n represents the number of data required to be selected for calculating the parameters of the plane model, k represents the number of iterations, and W representsnRepresenting randomly drawn nProbability that points are all local points; 1-WnIndicating a probability of the presence of at least one outlier; (1-W)n)kRepresenting the probability that at least one outlier exists in each of the k iterations; when k is large enough, this probability becomes small, meaning that the planar model confidence is higher;
then the probability of obtaining at least one optimal set of planar model parameters for k iterations is 1- (1-W)n)k
This probability is equal to the confidence parameter P, where P is 1- (1-W)n)k
Taking logarithm, and reversely deducing the value of k as k ═ log (1-P)/log (1-W)n)k
The value of k is the iteration exit condition, and when the actual iteration number is more than k, the loop exits.
Further, the specific process of the step (3) is as follows:
the industrial control computer selects the imu data acquired in the step (1) and the corresponding time, wherein the imu data comprises the angular velocities and the accelerations of three axes;
the accelerometer calculates an attitude angle:
the accelerometer measures the sensed acceleration, when the accelerometer is at rest, the accelerometer does not accelerate, but because of the action of the gravity acceleration, according to the theory of relative motion, the sensed acceleration is opposite to the gravity acceleration, namely the read data is vertically upward; let a be accelerometer data;
if the gesture rotation adopts 3 rotation modes in the ZYX sequence, the above description can be expressed as:
namely, it is
Solving the above equation yields roll and pitch angles (since the acceleration of gravity experienced is constant when rotating about Z, the accelerometer cannot calculate the angle yaw)
The gyroscope resolves an attitude angle:
the gyroscope measures angular velocity about 3 axes, and therefore, integrating the angular velocity, an angle can be obtained. Setting g as gyroscope data;
the attitude angle of the IMU at the nth moment is roll, pitch and yaw, the meaning of the attitude angle is that the IMU coordinate system rotates yaw around Z, pitch around Y and roll around X from the initial position to obtain the final attitude, and the attitude at the next moment (n +1) needs to be calculated; setting the attitude angle at the moment n +1 as roll + delta r, pitch + delta p and yaw + delta y, wherein the attitude also undergoes 3 rotations, calculating the attitude at the moment n +1, and obtaining the variation of the attitude angle only by adding the corresponding variation of the attitude angle on the basis of the attitude at the moment n, wherein the variation of the attitude angle can be obtained by integrating the angular velocity and the time period;
according to the process, the accelerometer can calculate roll and pitch angles at the static moment according to the sensed gravity acceleration, the angle calculation is only related to the current posture, the gyroscope integrates angular velocity in a time interval to obtain angle transformation quantity of each time, the angle transformation quantity is accumulated to the previous posture angle to obtain a new posture angle, and the gyroscope can calculate three angles of roll, pitch and yaw;
in fact, the acceleration can obtain a more accurate attitude only at a static moment, the gyroscope is only sensitive to attitude change during rotation, and if the gyroscope has an error, the error is continuously increased through continuous time integration. Therefore, the attitude of the solid-state laser radar is calculated by combining the accelerometer data and the gyroscope data, and complementary fusion is carried out;
k is a proportionality coefficient and needs to be adjusted and determined according to the reality; such as 0.4.
And finally, reflecting the pose of the current unmanned rail electric locomotive according to the obtained pitch angle of the solid-state laser radar, further determining whether the current unmanned rail electric locomotive is in an up-down slope or not, repeating the step for multiple times, further analyzing the collected imu data each time, obtaining whether the corresponding time of the unmanned rail electric locomotive is in the up-down slope or not, and then multiplying the corresponding time by the running speed of the unmanned rail electric locomotive in the step (1) to further obtain the length and the position of the up-down slope.
Further, the specific process of the step (4) is as follows:
A. when the unmanned electric locomotive encounters a curve, point cloud scanned by the solid-state laser radar is a wall on one side of a roadway, the non-ground point cloud data obtained in the step (2) is directly filtered, three beams of radar point clouds are taken in the y-axis direction, the maximum value in the x-axis direction is calculated in each beam point cloud, and if left _ x _ max < middle _ x _ max < right _ x _ max, the roadway is turned right; otherwise, turning left, further determining the moment when the unmanned electric locomotive encounters the curve, and multiplying the running speed of the unmanned electric locomotive in the step (1) by the moment to obtain the position of the curve;
B. according to three maximum values obtained by three laser radar point clouds, the curvature of the turning roadway can be solved, and the method specifically comprises the following steps:
C. let three points be (x)l,yl)(xm,ym)(xr,yr) To form a triangle, three sides are respectively:
from the cosine theorem:
a circle can be constructed by having all three vertices of the triangle on the circle, so the curvature is:
and (3) tf conversion is carried out on the original three-dimensional point cloud according to the obtained tunnel curvature, so that the rotation angle of the solid-state laser radar is obtained, and then the solid-state laser radar is correspondingly rotated, so that the x axis of the solid-state laser radar is parallel to the track, and the situation that the side wall of the tunnel is mistakenly identified as the obstacle by the solid-state laser radar is prevented.
Compared with the prior art, the method adopts the solid laser radar to be installed on the unmanned rail electric locomotive, then the rail electric locomotive advances in the underground tunnel at a constant speed, road data in the advancing process is collected in real time through the solid laser radar and fed back to the industrial control computer, and finally the traveling area of the whole underground tunnel is traversed to obtain an original point cloud data packet and imu data collected by the gyroscope and the accelerometer each time; and then obtaining three-dimensional point cloud of the underground tunnel according to the original point cloud data packet, and segmenting ground point cloud data and non-ground point cloud data by using an RANSAC algorithm. Calculating the pose of the current solid-state laser radar according to the collected imu data, feeding back whether the electric locomotive runs on the slope or not, further obtaining the length and the position of the uphill slope and the downhill slope, increasing the accelerator of the electric locomotive when the uphill slope is reached, and reducing the accelerator when the downhill slope is reached, so that the electric locomotive can run normally at a constant speed; judging whether the curve is the curved road according to the shape of the roadway wall, and calculating the curvature of the curved road and the position of the curved road. The solid-state laser radar is correspondingly rotated through the curvature of the curve, so that the x axis of the solid-state laser radar is parallel to the track, and the solid-state laser radar is prevented from mistakenly recognizing the roadway side as the obstacle. Therefore, the method of the invention enables the unmanned rail electric locomotive to accurately identify the ascending, descending and curve, thereby ensuring the unmanned rail electric locomotive to smoothly pass under the mine.
Drawings
FIG. 1 is a schematic illustration of the installation of a solid state lidar according to the present invention;
FIG. 2 is a schematic diagram of through filtering after solid state lidar scans a wall in the present invention;
FIG. 3 is a schematic diagram of the point cloud of the present invention transformed from the original three-dimensional point cloud tf for a downhole curve;
fig. 4 is an overall flow chart of the present invention.
Detailed Description
The present invention will be further explained below.
As shown in fig. 4, the method comprises the following specific steps:
(1) as shown in fig. 1, the solid-state laser radar is installed at the front locomotive of the unmanned electric rail car, an accelerometer and a gyroscope are arranged in the solid laser radar, the orientation of the solid laser radar can automatically rotate, then the solid-state laser radar is started and kept to face to the right front of the unmanned rail electric locomotive, the underground roadway opening is taken as a starting point, so that the unmanned rail electric locomotive moves forwards along the track at a constant speed of 1m/s, meanwhile, an industrial control computer is used for storing bag of solid-state laser radar measurement data and imu data acquired by a gyroscope and an accelerometer each time, the unmanned rail electric locomotive traverses all driving areas in an underground roadway, and the industrial control computer removes miscellaneous points and noise in the data according to the acquired data to obtain a data packet of original three-dimensional point cloud and imu data throughout the traversing process; the operating system of the industrial control computer is Ubuntu18.04, and an ROS and PCL library are installed;
(2) performing ground segmentation processing on the obtained original three-dimensional point cloud, and segmenting ground point cloud data and non-ground point cloud data by using a RANSAC (random Sample consensus) algorithm; the specific process of ground segmentation treatment comprises the following steps:
A. firstly, dividing an original three-dimensional point cloud into a plurality of sub-point cloud sections along the advancing direction of the unmanned rail electric locomotive, setting the ground in each sub-point cloud section as a plane, reducing the influence of gradient change on a division result, and then dividing the ground point cloud from the point cloud data in each sub-point cloud section by using an RANSAC algorithm; the RANSAC algorithm divides the concrete flow of the ground:
B. selecting point cloud data in a sub-point cloud segment as a data set, randomly selecting a plurality of points in the data set as local points, fitting the local points into a plane model, determining the number of the points according to the characteristics of the plane, namely fitting the plane, and selecting at least three points which are not collinear; sequentially calculating whether other points in the data set meet the plane model or not according to a set threshold (the threshold refers to the distance from other point clouds on two sides of the plane to the plane by taking the fitted plane as a reference, if the point is within the threshold, the point is considered to be on the plane, and the threshold is set to be 1cm), and if the point meets the threshold, marking the point as a local point of the plane; after the calculation of all the points is completed, recording the number of the local points of the plane model at the current time; if the number of the obtained local points is more than or equal to the number of the original local points of the plane model, fitting each local point obtained by the calculation into a new plane model, replacing the original plane model, repeating whether other points in the calculation data set meet the current plane model or not, if the number of the obtained local points is less than the number of the original local points of the plane model, keeping the current plane model, randomly selecting the same number of points in the data set to be local points, fitting the current local points into the plane model, repeating the calculation steps, repeating the iteration for multiple times until an iteration exit condition is met, exiting the cycle, and obtaining the optimal plane model in the whole iteration process; then, dividing the point cloud data in the current sub-point cloud segment into ground according to the plane model to obtain the ground point cloud data and non-ground point cloud data of the sub-point cloud segment; the iteration exit condition determination process comprises the following steps:
adopting W to represent the probability of randomly drawing the local point, and P to represent the confidence coefficient which represents the expected probability that the RANSAC algorithm provides a useful result after running; n represents the number of data required to be selected for calculating the parameters of the plane model, k represents the number of iterations, and W representsnRepresenting the probability that the randomly extracted n points are all local points; 1-WnIndicates that at least one existsThe probability of an individual outlier; (1-W)n)kRepresenting the probability that at least one outlier exists in each of the k iterations; when k is large enough, this probability becomes small, meaning that the planar model confidence is higher;
then the probability of obtaining at least one optimal set of planar model parameters for k iterations is 1- (1-W)n)k
This probability is equal to the confidence parameter P, where P is 1- (1-W)n)k
Taking logarithm, and reversely deducing the value of k as k ═ log (1-P)/log (1-W)n)k
The k value is an iteration exit condition, and when the actual iteration times is more than k, the loop exits;
C. and D, repeating the step B to segment the ground for other sub-point cloud segments, thereby obtaining the ground point cloud data of the original three-dimensional point cloud and the non-ground point cloud data.
(3) The industrial control computer selects the imu data and the corresponding time of the imu data acquired in the step (1), calculates the pose of the current solid-state laser radar through the imu data, determines whether the unmanned rail electric locomotive runs on the up-down slope at the time corresponding to the imu data according to the pose of the solid-state laser radar, repeats the step for many times, analyzes the imu data acquired each time, obtains whether the unmanned rail electric locomotive is on the up-down slope at each corresponding time, and multiplies the running speed of the unmanned rail electric locomotive in the step (1) by each corresponding time to obtain the length and the position of the up-down slope; the specific process is as follows:
the industrial control computer selects the imu data acquired in the step (1) and the corresponding time, wherein the imu data comprises the angular velocities and the accelerations of three axes;
the accelerometer calculates an attitude angle:
the accelerometer measures the sensed acceleration, when the accelerometer is at rest, the accelerometer does not accelerate, but because of the action of the gravity acceleration, according to the theory of relative motion, the sensed acceleration is opposite to the gravity acceleration, namely the read data is vertically upward; let a be accelerometer data;
if the gesture rotation adopts 3 rotation modes in the ZYX sequence, the above description can be expressed as:
namely, it is
Solving the above equation yields roll and pitch angles (since the acceleration of gravity experienced is constant when rotating about Z, the accelerometer cannot calculate the angle yaw)
Resolving an attitude angle by a gyroscope:
the gyroscope measures angular velocity about 3 axes, and therefore, integrating the angular velocity, an angle can be obtained. Setting g as gyroscope data;
the attitude angle of the IMU at the nth moment is roll, pitch and yaw, the meaning of the attitude angle is that the IMU coordinate system rotates yaw around Z, pitch around Y and roll around X from the initial position to obtain the final attitude, and the attitude at the next moment (n +1) needs to be calculated; setting the attitude angle at the moment n +1 as roll + delta r, pitch + delta p, yaw + delta y, wherein the attitude also undergoes 3 rotations, calculating the attitude at the moment n +1, and obtaining the variation of the attitude angle only by adding the corresponding variation of the attitude angle on the basis of the attitude at the moment n, wherein the variation of the attitude angle can be obtained by integrating the angular speed and the time period;
according to the process, the accelerometer can calculate roll and pitch angles at the static moment according to the sensed gravity acceleration, the angle calculation is only related to the current posture, the gyroscope integrates angular velocity in a time interval to obtain angle transformation quantity of each time, the angle transformation quantity is accumulated to the previous posture angle to obtain a new posture angle, and the gyroscope can calculate three angles of roll, pitch and yaw;
in fact, the acceleration can obtain a more accurate attitude only at a static moment, the gyroscope is only sensitive to attitude change during rotation, and if the gyroscope has an error, the error is continuously increased through continuous time integration. Therefore, the attitude of the solid-state laser radar is calculated by combining the accelerometer data and the gyroscope data, and complementary fusion is carried out;
k is a proportionality coefficient and needs to be adjusted and determined according to the reality; such as 0.4.
And finally, reflecting the pose of the current unmanned rail electric locomotive according to the obtained pitch angle of the solid-state laser radar, further determining whether the current unmanned rail electric locomotive is in an up-down slope or not, repeating the step for multiple times, further analyzing the collected imu data each time, obtaining whether the corresponding time of the unmanned rail electric locomotive is in the up-down slope or not, and then multiplying the corresponding time by the running speed of the unmanned rail electric locomotive in the step (1) to further obtain the length and the position of the up-down slope.
(4) Judging whether the road is a curve or not according to the non-ground point cloud data segmented in the step (2) and the shape of the wall of the roadway, calculating the curvature of the curve and the position of the curve, and correspondingly rotating the solid laser radar according to the curvature of the curve to enable the x axis of the solid laser radar to be parallel to the track, so that the solid laser radar is prevented from mistakenly identifying the side slope of the roadway as an obstacle; the specific process is as follows:
A. when the unmanned electric locomotive encounters a curve, the point cloud scanned by the solid-state laser radar is a wall on one side of a roadway, as shown in fig. 2, the non-ground point cloud data obtained in the step (2) is directly filtered, three beams of radar point clouds are taken in the y-axis direction, the maximum value in the x-axis direction is calculated in each beam point cloud, and if left _ x _ max < middle _ x _ max < right _ x _ max, the roadway is turned right; otherwise, turning left, further determining the moment when the unmanned electric locomotive encounters the curve, and multiplying the running speed of the unmanned electric locomotive in the step (1) by the moment to obtain the position of the curve;
B. according to three maximum values obtained by three laser radar point clouds, the curvature of the turning of the roadway can be solved, and the method specifically comprises the following steps:
C. three points are set as (x)l,yl)(xm,ym)(xr,yr) To form a triangle, three sides are respectively:
from the cosine theorem:
a circle can be constructed by having all three vertices of the triangle on the circle, so the curvature is:
as shown in fig. 3, tf transformation is performed on the original three-dimensional point cloud according to the obtained tunnel curvature, so as to obtain the rotation angle of the solid-state laser radar, and then the solid-state laser radar is correspondingly rotated, so that the x axis of the solid-state laser radar is parallel to the track, and the solid-state laser radar is prevented from mistakenly recognizing the tunnel side wall as the obstacle.
(5) When the unmanned rail electric locomotive starts to be transported normally, the running speed can be automatically adjusted when the unmanned rail electric locomotive passes through the up-down slope or the curve according to the length and the position of the up-down slope determined in the step (3) and the curvature and the position of the curve determined in the step (4), so that the unmanned rail electric locomotive can be ensured to pass smoothly.
The above description is only of the preferred embodiments of the present invention, and it should be noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the invention, and such modifications and adaptations are intended to be within the scope of the invention.
Claims (6)
1. A method for unmanned rail electric locomotive gradient and curve passage under deep limited space is characterized by comprising the following specific steps:
(1) the method comprises the steps that a solid-state laser radar is installed at the front vehicle head of the unmanned electric rail vehicle, an accelerometer and a gyroscope are arranged in the solid-state laser radar, the orientation of the solid-state laser radar can automatically rotate, then the solid-state laser radar is started and is kept to face to the front of the unmanned electric rail vehicle, an underground roadway crossing is taken as a starting point, the unmanned electric rail vehicle can move forwards along a roadway at a constant speed, meanwhile, a bag of solid-state laser radar measurement data and imu data acquired by the gyroscope and the accelerometer every time are stored by an industrial control computer, the unmanned electric rail vehicle traverses all traveling areas in the underground roadway, and the industrial control computer removes miscellaneous points and noise in the data according to the acquired data to obtain a data packet of original three-dimensional point cloud and imu data of the whole traversal process;
(2) performing ground segmentation processing on the obtained original three-dimensional point cloud, and segmenting ground point cloud data and non-ground point cloud data by using an RANSAC algorithm;
(3) the industrial control computer selects the imu data and the corresponding time of the imu data acquired in the step (1), calculates the pose of the current solid-state laser radar through the imu data, determines whether the unmanned rail electric locomotive runs on the up-down slope at the time corresponding to the imu data according to the pose of the solid-state laser radar, repeats the step for many times, analyzes the imu data acquired each time, obtains whether the unmanned rail electric locomotive is on the up-down slope at each corresponding time, and multiplies the running speed of the unmanned rail electric locomotive in the step (1) by each corresponding time to obtain the length and the position of the up-down slope;
(4) judging whether the road is a curve or not according to the non-ground point cloud data segmented in the step (2) and the shape of the wall of the roadway, calculating the curvature of the curve and the position of the curve, and correspondingly rotating the solid laser radar according to the curvature of the curve to enable the x axis of the solid laser radar to be parallel to the track, so that the solid laser radar is prevented from mistakenly identifying the side slope of the roadway as an obstacle;
(5) when the unmanned rail electric locomotive starts to be transported normally, the running speed can be automatically adjusted when the unmanned rail electric locomotive passes through the up-down slope or the curve according to the length and the position of the up-down slope determined in the step (3) and the curvature and the position of the curve determined in the step (4), so that the unmanned rail electric locomotive can be ensured to pass smoothly.
2. The method for the grade and curve passing of the unmanned railed electric locomotive in the deep restricted space according to claim 1, wherein the unmanned railed electric locomotive in the step (1) runs at a constant speed of 1 m/s.
3. The method for the gradient and curve passage of the unmanned railed electric locomotive in the deep limited space according to claim 1, wherein the concrete process of the step (2) ground segmentation treatment is as follows:
A. firstly, dividing an original three-dimensional point cloud into a plurality of sub-point cloud sections along the advancing direction of the unmanned rail electric locomotive, setting the ground in each sub-point cloud section as a plane, reducing the influence of gradient change on a division result, and then dividing the ground point cloud from the point cloud data in each sub-point cloud section by using an RANSAC algorithm; the RANSAC algorithm divides the concrete flow of the ground:
B. selecting point cloud data in a sub-point cloud segment as a data set, randomly selecting a plurality of points in the data set as local points, fitting the local points into a plane model, determining the number of the points according to the characteristics of the plane, namely fitting the plane, and selecting at least three points which are not collinear; sequentially calculating whether other points in the data set meet the plane model according to a set threshold value, and if so, marking as the local interior point of the plane; after the calculation of all the points is completed, recording the number of the local points of the plane model at the current time; if the number of the obtained local points is more than or equal to the number of the original local points of the plane model, fitting each local point obtained by the calculation into a new plane model, replacing the original plane model, repeating whether other points in the calculation data set meet the current plane model or not, if the number of the obtained local points is less than the number of the original local points of the plane model, keeping the current plane model, randomly selecting the same number of points in the data set to be local points, fitting the current local points into the plane model, repeating the calculation steps, repeating the iteration for multiple times until an iteration exit condition is met, exiting the cycle, and obtaining the optimal plane model in the whole iteration process; then, dividing the point cloud data in the current sub-point cloud segment into ground according to the plane model to obtain the ground point cloud data and non-ground point cloud data of the sub-point cloud segment;
C. and D, repeating the step B to segment the ground for other sub-point cloud segments, thereby obtaining the ground point cloud data of the original three-dimensional point cloud and the non-ground point cloud data.
4. The method for the grade and curve passage of the unmanned railed electric locomotive in the deep limited space according to claim 3, wherein the iterative exit condition is determined by the following steps:
adopting W to represent the probability of randomly drawing the local point, and P to represent the confidence coefficient which represents the expected probability that the RANSAC algorithm provides a useful result after running; n represents the number of data required to be selected for calculating the parameters of the plane model, k represents the number of iterations, and W representsnRepresenting the probability that the randomly extracted n points are all local points; 1-WnIndicating a probability of the presence of at least one outlier; (1-W)n)kRepresenting the probability that at least one outlier exists in each of the k iterations; when k is large enough, this probability becomes small, meaning that the planar model confidence is higher;
then the probability that a set of optimal planar model parameters can be obtained at least k iterations is 1- (1-W)n)k
This probability is equal to the confidence parameter P, where P is 1- (1-W)n)k
Taking logarithm, and reversely deducing the value of k as k ═ log (1-P)/log (1-W)n)k
The value of k is the iteration exit condition, and when the actual iteration number is greater than k, the loop exits.
5. The method for the gradient and curve passage of the unmanned railed electric locomotive in the deep limited space according to claim 1, wherein the step (3) comprises the following specific steps:
the industrial control computer selects the imu data acquired in the step (1) and the corresponding time, wherein the imu data comprises the angular velocities and the accelerations of three axes;
the accelerometer calculates an attitude angle:
the accelerometer measures the sensed acceleration, when the accelerometer is at rest, the accelerometer does not accelerate, but because of the action of the gravity acceleration, according to the theory of relative motion, the sensed acceleration is opposite to the gravity acceleration, namely the read data is vertically upward; let a be accelerometer data;
if the gesture rotation adopts 3 rotation modes in the ZYX sequence, the above description can be expressed as:
namely, it is
Solving the equation to obtain a roll angle and a pitch angle;
the gyroscope resolves an attitude angle:
the gyroscope measures angular velocity about 3 axes, and therefore, integrating the angular velocity, an angle can be obtained. Setting g as gyroscope data;
the attitude angles of the IMU at the nth moment are roll, pitch and yaw, the meaning of the attitude angles is that the IMU coordinate system rotates around Z and Y and rotates around X by the roll angle from the initial position to obtain the final attitude, and at the moment, the attitude at the next moment (n +1) needs to be calculated; setting the attitude angle at the moment n +1 as roll + delta r, pitch + delta p, yaw + delta y, wherein the attitude also undergoes 3 rotations, calculating the attitude at the moment n +1, and obtaining the variation of the attitude angle only by adding the corresponding variation of the attitude angle on the basis of the attitude at the moment n, wherein the variation of the attitude angle can be obtained by integrating the angular speed and the time period;
therefore, the attitude of the solid-state laser radar is calculated by combining the accelerometer data and the gyroscope data, and complementary fusion is carried out;
k is a proportionality coefficient and needs to be adjusted and determined according to the reality;
and finally, reflecting the pose of the current unmanned rail electric locomotive according to the obtained pitch angle of the solid-state laser radar, further determining whether the unmanned rail electric locomotive is in an ascending slope or a descending slope at the current moment, repeating the step for multiple times, further analyzing the collected imu data each time, obtaining whether the unmanned rail electric locomotive is in the ascending slope or the descending slope at each corresponding moment, and then multiplying the corresponding moments by the driving speed of the unmanned rail electric locomotive in the step (1) to further obtain the length and the position of the ascending slope and the position of the descending slope.
6. The method for the gradient and curve passage of the unmanned railed electric locomotive in the deep restricted space according to claim 1, wherein the specific process of the step (4) is as follows:
A. when the unmanned electric locomotive encounters a curve, point cloud scanned by the solid-state laser radar is a wall on one side of a roadway, the non-ground point cloud data obtained in the step (2) is directly filtered, three beams of radar point clouds are taken in the y-axis direction, the maximum value in the x-axis direction is calculated in each beam point cloud, and if left _ x _ max < middle _ x _ max < right _ x _ max, the roadway is turned right; otherwise, turning left, further determining the moment when the unmanned electric locomotive encounters the curve, and multiplying the running speed of the unmanned electric locomotive in the step (1) by the moment to obtain the position of the curve;
B. according to three maximum values obtained by three laser radar point clouds, the curvature of the turning of the roadway can be solved, and the method specifically comprises the following steps:
C. three points are set as (x)l,yl)(xm,ym)(xr,yr) To form a triangle, three sides are respectively:
from the cosine theorem:
a circle can be constructed by having all three vertices of the triangle on the circle, so its curvature is:
according to the obtained tunnel curvature, tf conversion is carried out on the original three-dimensional point cloud so as to obtain the rotation angle of the solid-state laser radar, and then the solid-state laser radar is correspondingly rotated, so that the x axis of the solid-state laser radar is parallel to the track, and the solid-state laser radar is prevented from mistakenly recognizing the tunnel side wall as the obstacle.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210383227.3A CN114690780A (en) | 2022-04-13 | 2022-04-13 | Method for allowing unmanned rail electric locomotive to pass through slope and curve in deep limited space |
| PCT/CN2022/121671 WO2023197535A1 (en) | 2022-04-13 | 2022-09-27 | Slope and curve passage method for unmanned rail electric locomotive in deep limited space |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210383227.3A CN114690780A (en) | 2022-04-13 | 2022-04-13 | Method for allowing unmanned rail electric locomotive to pass through slope and curve in deep limited space |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114690780A true CN114690780A (en) | 2022-07-01 |
Family
ID=82143737
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210383227.3A Pending CN114690780A (en) | 2022-04-13 | 2022-04-13 | Method for allowing unmanned rail electric locomotive to pass through slope and curve in deep limited space |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN114690780A (en) |
| WO (1) | WO2023197535A1 (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116047464A (en) * | 2023-01-28 | 2023-05-02 | 武汉理工大学 | Underground mining area gradient detection method and system based on laser radar |
| CN116803814A (en) * | 2023-08-22 | 2023-09-26 | 湖南斯福迈智能科技有限责任公司 | Unmanned control method and system for ore-carrying truck |
| WO2023197535A1 (en) * | 2022-04-13 | 2023-10-19 | 中国矿业大学 | Slope and curve passage method for unmanned rail electric locomotive in deep limited space |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118013233A (en) * | 2024-04-07 | 2024-05-10 | 中汽研汽车检验中心（天津）有限公司 | Construction method for predictive cruising test working condition of heavy commercial vehicle |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105373689B (en) * | 2014-08-21 | 2019-02-26 | 北京协进科技发展有限公司 | Method and device for determining road curvature in electronic map |
| CN104932505B (en) * | 2015-06-08 | 2018-04-13 | 华南理工大学 | Automated navigation system, control method and vertical balanced car based on camera detection |
| CN105843235A (en) * | 2015-10-23 | 2016-08-10 | 范云生 | Experiment system and experiment method for verifying control theory by taking attitude angle as control object |
| CN108052624B (en) * | 2017-12-15 | 2021-05-18 | 深圳市易成自动驾驶技术有限公司 | Point cloud data processing method and device and computer readable storage medium |
| RU2764007C1 (en) * | 2018-04-20 | 2022-01-12 | Сандвик Майнинг Энд Констракшн Г.М.Б.Х. | Method and system for controlling motion of self-propelled mining machine |
| CN109976262B (en) * | 2019-03-12 | 2020-12-01 | 天津大学 | Global curvature continuous fairing method for micro-line segment processing |
| CN110667536A (en) * | 2019-09-04 | 2020-01-10 | 陕西九域通创轨道系统技术有限责任公司 | Bend control decision method for train AEB system |
| CN113935428A (en) * | 2021-10-25 | 2022-01-14 | 山东大学 | Three-dimensional point cloud clustering identification method and system based on image identification |
| CN114690780A (en) * | 2022-04-13 | 2022-07-01 | 中国矿业大学 | Method for allowing unmanned rail electric locomotive to pass through slope and curve in deep limited space |
-
2022
- 2022-04-13 CN CN202210383227.3A patent/CN114690780A/en active Pending
- 2022-09-27 WO PCT/CN2022/121671 patent/WO2023197535A1/en unknown
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2023197535A1 (en) * | 2022-04-13 | 2023-10-19 | 中国矿业大学 | Slope and curve passage method for unmanned rail electric locomotive in deep limited space |
| CN116047464A (en) * | 2023-01-28 | 2023-05-02 | 武汉理工大学 | Underground mining area gradient detection method and system based on laser radar |
| CN116047464B (en) * | 2023-01-28 | 2023-08-11 | 武汉理工大学 | Underground mining area gradient detection method and system based on laser radar |
| CN116803814A (en) * | 2023-08-22 | 2023-09-26 | 湖南斯福迈智能科技有限责任公司 | Unmanned control method and system for ore-carrying truck |
| CN116803814B (en) * | 2023-08-22 | 2023-11-21 | 湖南斯福迈智能科技有限责任公司 | Unmanned control method and system for ore-carrying truck |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2023197535A1 (en) | 2023-10-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114690780A (en) | Method for allowing unmanned rail electric locomotive to pass through slope and curve in deep limited space | |
| Urmson et al. | A robust approach to high‐speed navigation for unrehearsed desert terrain | |
| CN104881025B (en) | A kind of reactive navigation control method of underground mine vehicle | |
| JP2023536407A (en) | Drivable surface identification technology | |
| CN104268862B (en) | Three-dimensional terrain trafficability analytical method for autonomous land vehicle | |
| EP2450763A1 (en) | Global position and orientation estimation system for a vehicle in a passageway environment | |
| CN112389440B (en) | Vehicle driving risk prediction method in off-road environment based on vehicle-road action mechanism | |
| KR102178992B1 (en) | Method and Apparatus for Planning Car Motion | |
| CN112577506B (en) | Automatic driving local path planning method and system | |
| CN113252027B (en) | Underground unmanned vehicle local path planning method, device, equipment and storage medium | |
| Baril et al. | Kilometer-scale autonomous navigation in subarctic forests: challenges and lessons learned | |
| CN116576857A (en) | Multi-obstacle prediction navigation obstacle avoidance method based on single-line laser radar | |
| CN114594756A (en) | Multi-vehicle cooperative formation control method, terminal and medium in dynamic obstacle environment | |
| CN115639823A (en) | Terrain sensing and movement control method and system for robot under rugged and undulating terrain | |
| CN115454083A (en) | Double-layer path planning method based on fast expansion random tree and dynamic window method | |
| US12025465B2 (en) | Drivable surface map for autonomous vehicle navigation | |
| CN114954525A (en) | Unmanned transport vehicle system suitable for phosphorite mining roadway and operation method | |
| CN112519783B (en) | Method and system for generating bottom-up smooth track of intelligent driving | |
| Morales et al. | Safe and reliable navigation in crowded unstructured pedestrian areas | |
| CN116935683A (en) | Safety behavior detection method for autonomous running of unmanned off-road vehicle | |
| Sharma et al. | Ramp: A risk-aware mapping and planning pipeline for fast off-road ground robot navigation | |
| CN113902864B (en) | Vector map generation method and system for mine field and computer system | |
| WO2023069398A1 (en) | Drivable surface map for autonomous vehicle navigation | |
| AU2021232767A1 (en) | Vehicle navigation | |
| CN115792958A (en) | Unmanned mine car obstacle detection method based on 3D laser radar |
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 |