CN112527000B - Local path planning method and system for mine underground intelligent driving - Google Patents
Local path planning method and system for mine underground intelligent driving Download PDFInfo
- Publication number
- CN112527000B CN112527000B CN202011557897.XA CN202011557897A CN112527000B CN 112527000 B CN112527000 B CN 112527000B CN 202011557897 A CN202011557897 A CN 202011557897A CN 112527000 B CN112527000 B CN 112527000B
- Authority
- CN
- China
- Prior art keywords
- path
- mine
- curve
- mine car
- beacon
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- 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
-
- 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种矿井下智能驾驶的局部路径规划方法及系统，步骤1：车载激光雷达检测矿井内特征信标，若存在特征信标，说明矿车即将进入弯道，进入步骤2，否则，说明矿车仍在直行阶段，进入步骤3；步骤2：提取特征信标与矿车之间角度及距离信息，根据信标在全局坐标系下的安装位置计算矿车位置，实现导航定位误差校正，进入步骤4；步骤3：在直道与弯道切接口，根据矿车反馈的车身与反应式规划路径的控制误差，调整反应式规划路径的横向偏移，实现入弯前矿车的横向控制校正；步骤4：矿车进入弯道后根据信标提示的转弯方向，生成矿车弯道期望路径。本发明能实现无人矿车在地下矿井的反应式局部路径规划与轨迹生成。
The invention discloses a local path planning method and system for intelligent driving in mines. Step 1: vehicle-mounted laser radar detects characteristic beacons in the mine. If there is a characteristic beacon, it means that the mine car is about to enter a curve, and the process proceeds to step 2. , indicating that the minecart is still in the straight-forward stage, and enter step 3; Step 2: extract the angle and distance information between the characteristic beacon and the minecart, and calculate the minecart position according to the installation position of the beacon in the global coordinate system to realize the navigation and positioning error Correction, go to Step 4; Step 3: At the interface between the straight road and the curve, adjust the lateral offset of the reactive planning path according to the control error between the body and the reactive planning path fed back by the minecart, so as to realize the lateral direction of the minecart before entering the bend. Control correction; Step 4: After the minecart enters the curve, the expected path of the minecart curve is generated according to the turning direction indicated by the beacon. The invention can realize the reactive local path planning and trajectory generation of the unmanned mine car in the underground mine.
Description
Technical Field
The invention relates to the technical field of intelligent driving and control thereof, in particular to a local path planning method and system for intelligent driving under a mine.
Background
Along with the increase of the mining amount of mine field resources in China and the diversified development of mining environments, the intelligent development of underground unmanned mine cars is wide. The intelligent degree of the existing underground mine car travelling crane is lower by adopting a remote control mode, and how to realize the positioning and intelligent driving of the mine car in the underground mining area with dim light and weak GPS signals is an important research direction for improving the mining productivity, reducing the production cost and adapting to the mining environment of the mining area.
At present, the underground unmanned mine car path planning method is mainly divided into two methods, namely a reactive path planning method and a navigation path planning method, but because the GPS signal of an underground mining area is weak, the navigation path planning method causes larger control error due to GPS error, so the reactive path planning method is mostly adopted. The path planning method based on the reaction formula is characterized in that a mine road is scanned in real time according to a vehicle-mounted sensor, then a planned path which accords with safety indexes is generated by curve fitting, but the planned path directly generated in a curve fitting mode generates a large curvature due to the existence of singular points, and the vehicle cannot track the curve due to the fact that the planned path does not accord with the constraint of vehicle dynamics. Meanwhile, in a narrow underground mining area, the situation that the curve cannot be smoothly entered due to large control error is easily caused before the curve is entered by the reactive path planning method.
Disclosure of Invention
The invention aims to solve the technical problem that the prior art is insufficient, and provides a local path planning method and system for intelligent driving under a mine, which are used for adaptively adjusting tracks, realizing transverse correction of a mine car before an intersection, ensuring that the mine car can safely pass through the intersection and improving the safety of an underground unmanned mine car at a branch intersection.
In order to solve the technical problems, the technical scheme adopted by the invention is as follows: a local path planning method for mine underground intelligent driving comprises the following steps:
1) detecting a characteristic beacon in the mine by the vehicle-mounted laser radar, if the characteristic beacon exists, indicating that the mine car is about to enter a bend, and entering the step 2); otherwise, indicating that the mine car is still in the straight-going stage, and entering the step 3);
2) extracting angle and distance information between the characteristic beacon and the mine car, calculating the relative distance between the characteristic beacon and the mine car in real time according to the angle and distance information, calculating the position of the mine car according to the absolute position of the beacon in the mine and the relative distance under a global coordinate system, realizing navigation positioning error correction, and entering step 4);
3) before the mine car enters the bend, adjusting the transverse deviation of the reference path according to the control error of the car body and the reactive reference path fed back by the mine car at the interface between the straight road and the bend to realize the transverse control correction of the mine car before entering the bend;
4) and after the mine car enters the curve, generating an expected path of the curve of the mine car according to the turning direction prompted by the beacon. The method applies the reactive path planning method to the local path planning of the unmanned mine car under the mine, uses the laser radar as the 'sight line' of the mine car, and solves the problem of inaccurate perception caused by dark visual field of the underground mining area; acquiring route scatter points in a mine car driving boundary acquired in real time by using a random point scattering-based mode, connecting the route scatter points through spline curves to generate an alternative route set, and ensuring that a planned route meets vehicle tracking conditions under the constraint of multiple targets; the characteristic beacon is set at the pre-bending position in consideration of the narrow road of the underground mining area, so that the correction of the positioning error is realized, the planned path is adjusted in real time by combining the tracking error, the mine car is ensured to enter the bend at a proper posture, the collision with the wall of the roadway is avoided, and the safety of the mine car in underground driving is improved. In the step 1), in the straight-going stage, the path planning is carried out by using the following steps:
A) carrying out discrete processing on the clustered and layered data points acquired by the vehicle-mounted laser radar to generate a travelable boundary;
B) taking the advancing direction of the vehicle as the positive direction of a y axis, being vertical to the advancing direction of the vehicle, taking the direction of anticlockwise rotation of 90 degrees of the advancing direction of the vehicle as the positive direction of an x axis, taking the transmitting center of the laser radar as the origin of coordinates to establish a coordinate system of the vehicle, and when a left boundary l is present in a travelable boundary, y isl＝alx2+blx+clY and the right boundary r: yr＝arx2+brx+crGenerating a reference path, wherein al、bl、clRepresenting the left boundary curve coefficient, cl>0，ar、br、crRepresenting the coefficient of the right boundary curve, cr<0; when only one boundary or no boundary exists in the drivable boundaries, detecting whether the laser radar has a fault; the reference path generation method comprises the following steps: randomly sampling in the area between the left boundary and the right boundary, i.e. the travelable area, and randomly generating route scatter points
Taking a laser radar mounting point as a starting point, taking a path scattered point which is farthest from the starting point in the longitudinal direction as an end point, and connecting all curves between any two longitudinally adjacent path scattered points between the starting point and the end point by utilizing cubic spline interpolation to obtain an alternative path; co-generate mkTaking a strip path as an alternative path set, wherein m represents the scatter number of a transverse path, and k is a tableShowing the number of scattered points of the longitudinal path; calculating the fitness value of each alternative path by using the multi-objective advantage function, and taking the alternative path with the maximum fitness value as the multi-objective optimal reactive reference path, namely the reference path
g denotes the g-th segment, a, of the Path Pathg、bg、cgRepresents the curve coefficient of the g-th section of Path.
The invention generates the planned path by randomly scattering points and connecting curves, replaces the mode of directly connecting the points with the maximum safe distance as the planned path, not only ensures the selectivity of the mine car path, but also solves the problem that the vehicle cannot track because of singular points in the planned path.
The path scatter
The specific generation process comprises: the laser radar mounting point is taken as a starting point, the vehicle advancing direction is taken as a longitudinal direction, and the direction vertical to the longitudinal direction is taken as a transverse direction; taking k interval points with delta p as a longitudinal interval and m transverse interval points with delta s as a transverse interval in a transverse direction passing through each longitudinal interval point in a travelable area, and generating k multiplied by m interval points as path scatter points
Wherein
0.5≤Δp≤3；0.3≤Δp≤1。
According to the method for generating the route scatter points, the longitudinal planning distance and the control period are kept in a reasonable ratio through setting of the longitudinal parameters, the planning foresight and practicability are guaranteed, a plurality of selectable routes are generated through setting of the transverse parameters, and the diversity of the routes is guaranteed.
In step B), the expression of the multi-objective advantage function is as follows:
f＝ωdfdis+ωsfsmo+ωufsec；
wherein f isdisRepresenting path length, ωdRepresenting length weight, fsmoIndicates smoothness, ωsRepresenting a smoothness weight, fsecRepresenting the safe distance, ω, between the path and the travelable boundaryuRepresenting a safe distance weight, fmin_disRepresents the shortest distance, f, from the path scatter to the left and right boundaries(j+1)Represents a curve segment, f ″, between longitudinally adjacent path scatter points j and j +1(j+1)Representing a curve segment f(j+1)Of (c) is performed.
The method selects the path in the alternative path set in a multi-target optimization mode, ensures that the planned path achieves comprehensive optimization in path length, path smoothness and safety, and provides important guarantee for vehicle tracking. In step 2), the specific implementation process for implementing the navigation positioning error correction includes:
I) taking the advancing direction of the driving task as the positive direction of a y axis, taking the direction which is vertical to the advancing direction of the driving task and rotates 90 degrees anticlockwise to the advancing direction of the driving task as the positive direction of an x axis, taking the starting point of the driving task as the origin of coordinates to establish a global coordinate system, combining N beacons in pairs, respectively calculating the abscissa and the ordinate of the mine car by utilizing the position of each combination under the global coordinate system and the corresponding distance between the combination and the mine car, and screening out the coordinates of which the difference between the abscissas and the difference between the ordinates are both smaller than an error threshold value; wherein, the relative distance between the mine car and each combination is the component of the distance from the center of the reflector to the laser radar on the installation horizontal plane of the laser radar;
II) respectively averaging the abscissa and the ordinate in the screened coordinates to obtain the actual position of the mine car.
According to the invention, the laser radar reflector is arranged as the characteristic beacon, and the coordinate transformation and geometric theorem are applied, so that the reverse thrust of the mine car on the real-time position of the underground roadway is realized, the problem of inaccurate positioning caused by weak underground GPS signals is solved, and the navigation positioning error correction is realized.
The specific implementation process of the step 3) comprises the following steps: the lateral correction of the tramcar is achieved using the following curve, Trajectory:
wherein Tracjectory represents the reactive reference Path after adjusting the lateral offset, g represents the g-th segment of the Path Path, ag、bg、cgCoefficient of the curve representing the g-th segment of Path, c1Represents the intercept of the curve segment in the path closest to the mine car, epsilon represents the mine car heading correction factor, v represents the longitudinal speed of the mine car, deltad represents the relative position of the mine car in the longitudinal direction with respect to the beacon,
indicating the travel time of the mine car from the beacon.
The invention utilizes the control error fed back by the mine car to adaptively adjust the track, realizes the transverse correction of the mine car in front of the intersection and ensures that the mine car can safely pass through the intersection.
In the step 4), the process of generating the expected path of the curve of the mine car comprises the following steps:
a) identifying the prompting turning direction of the characteristic beacon, and selecting the geometric description l, y of the drivable boundary of the mine tunnel with the same prompting directionl＝alx2+blx+clOr r: yr＝arx2+brx+cr(ii) a Wherein l represents a left turn; r represents a right turn;
b) taking k 'characteristic points on a travelable boundary l or r every delta p' in the advancing direction of the mine car, and generating a curve reference path by using a least square fitting method;
c) deletion curvature greater than Clim' generating a desired trajectory for the curve;
d) and the expected curve track is transversely deviated by delta q to be taken as the expected curve path.
The mine car is limited by a detection visual angle at a turning position, data of roadway walls on two sides of a roadway at the turning position cannot be accurately acquired, and data of only one side can be acquired. The invention also provides a method for planning the straight path under the mine, which comprises the following steps:
A) performing discrete processing on the clustered and layered data points acquired by the vehicle-mounted laser radar, and eliminating convex points in the travelable boundary points to generate travelable boundaries;
B) taking the advancing direction of the vehicle as the positive direction of a y axis, being vertical to the advancing direction of the vehicle, taking the direction of anticlockwise rotation of 90 degrees of the advancing direction of the vehicle as the positive direction of an x axis, taking the transmitting center of the laser radar as the origin of coordinates to establish a coordinate system of the vehicle, and when a left boundary l is present in a travelable boundary, y isl＝alx2+blx+clAnd the right boundary r: yr＝arx2+brx+crGenerating a reference path, wherein al、bl、clRepresenting the left boundary curve coefficient, cl>0，ar、br、crRepresenting the coefficient of the right boundary curve, cr<0; when only one boundary or no boundary exists in the drivable boundaries, detecting whether the laser radar has a fault; the reference path generation method comprises the following steps: randomly sampling in the area between the left boundary and the right boundary, i.e. the travelable area, and randomly generating route scatter points
Taking a laser radar mounting point as a starting point, taking a path scattered point which is farthest from the starting point in the longitudinal direction as an end point, and connecting all curves between any two longitudinally adjacent path scattered points between the starting point and the end point by utilizing cubic spline interpolation to obtain an alternative path; co-generate mkTaking the path as an alternative path set, wherein m represents the scattering number of the transverse path, and k represents the scattering number of the longitudinal path; calculating a fitness value of each alternative path by using a multi-objective advantage function, and taking the alternative path with the maximum fitness value as a multi-objective optimal reactive reference path, namely a reference path
g denotes the g-th segment, a, of the Path Pathg、bg、cgRepresents the curve coefficient of the g-th section of Path.
The invention ensures the selectivity of the mine car path and solves the problem that the vehicle cannot track due to singular points in the planned path by generating the planned path through random point scattering and curve connection.
The invention also provides a system for planning the local path under the mine, which comprises computer equipment; the computer device is configured or programmed for performing the steps of the mine downhole local path planning method of the invention.
In order to facilitate acquisition of corresponding data, the computer device is in communication with a vehicle-mounted lidar; the vehicle-mounted laser radar is used for acquiring signals of characteristic beacons arranged at cut interfaces of straight roads and curved roads in a mine.
Compared with the prior art, the invention has the beneficial effects that:
1. the mine roadway wall detection and reactive planning path generation method based on the laser radar is used, the mine area environment sensing is realized by the laser radar, and a reactive path planning mode is adopted, so that the problem of difficulty in sensing and planning caused by dim light is solved, and the intelligent control of the mine car is preliminarily realized;
2. the invention adopts a positioning compensation method facing to a laser radar beacon, detects a characteristic beacon through the laser radar, calculates the relative position and angle between the beacon and a mine car in real time, converts the relative position and angle to a global coordinate system through the coordinate system, and reversely pushes the position of the mine car according to the absolute position of the beacon, thereby realizing the correction of navigation positioning errors;
3. the invention adopts a track generation method based on feedback type tracking error correction, self-adaptively adjusts the track by utilizing the control error fed back by the mine car, realizes the transverse correction of the mine car before the intersection, ensures that the mine car can safely pass through the intersection, improves the safety of the underground unmanned mine car at the branch intersection, and solves the problem that the intersection fails to turn if the tracking error of the mine car is not corrected in time at the intersection of a straight road and a curve of the underground mine.
Drawings
FIG. 1 is a flow chart of a reactive local path planning and trajectory generation method under a mine;
FIG. 2(a) is a diagram showing the generation of a reaction scheme, and FIG. 2(b) is a diagram showing the experimental comparison of reaction schemes;
FIG. 3(a) is a diagram of a mine travel route, and FIG. 3(b) is a comparative diagram of a positioning error correction experiment
Fig. 4(a) is a diagram for generating a trajectory in a pre-bending stage, and fig. 4(b) is a diagram for comparing a lateral error experiment of pre-bending.
Detailed Description
The intelligent vehicle modified by the underground development machine is adopted in the embodiment, the laser radar is arranged at the tail of the vehicle in front of the vehicle, the vehicle can run in the straight direction and the reverse direction, and the intelligent driving experiment is carried out in an underground mining area.
Referring to fig. 1, a reactive local path planning and trajectory generation method under a mine includes the following steps:
step 1: detecting a characteristic beacon in the mine by the vehicle-mounted laser radar, if the characteristic beacon exists, indicating that the mine car is about to enter a bend, and entering the step 2); otherwise, indicating that the mine car is still in the straight-going stage, and entering the step 3);
step 2: extracting angle and distance information between the characteristic beacon and the mine car, calculating the relative distance between the characteristic beacon and the mine car in real time according to the angle and distance information, calculating the position of the mine car according to the absolute position of the beacon in the mine and the relative distance under a global coordinate system, realizing navigation positioning error correction, and entering step 4);
and step 3: before the mine car enters the bend, adjusting the transverse deviation of the reaction type planning path according to the control error of the car body and the reaction type planning path fed back by the mine car at a straight road and bend cutting interface, and realizing the transverse control correction of the mine car before entering the bend;
and 4, step 4: and after the mine car enters the curve, generating an expected path of the curve of the mine car according to the turning direction prompted by the beacon.
The generation of the straight-going phase reactive planning path in step 1 is further described with reference to fig. 2(a) and fig. 2 (b):
carrying out discrete processing on the clustered and layered data points acquired by the vehicle-mounted laser radar to generate a travelable boundary;
taking the advancing direction of the vehicle as the positive direction of a y axis, being vertical to the advancing direction of the vehicle, taking the direction of anticlockwise rotation of 90 degrees of the advancing direction of the vehicle as the positive direction of an x axis, taking the transmitting center of the laser radar as the origin of coordinates to establish a coordinate system of the vehicle, and when a left boundary l is present in a travelable boundary, y isl＝alx2+blx+clY and the right boundary r: yr＝arx2+brx+crGenerating a reference path, wherein al、bl、clRepresenting the left boundary curve coefficient, cl>0，ar、br、crRepresenting the coefficient of the right boundary curve, cr<0; when only one boundary or no boundary exists in the drivable boundaries, detecting whether the laser radar has a fault; the reference path generation method comprises the following steps: randomly sampling in the area between the left boundary and the right boundary, i.e. the travelable area, and randomly generating route scatter points
Taking a laser radar mounting point as a starting point, taking a path scattered point which is farthest from the starting point in the longitudinal direction as an end point, and connecting all curves between any two longitudinally adjacent path scattered points between the starting point and the end point by utilizing cubic spline interpolation to obtain an alternative path; co-generate mkThe strip path is used as an alternative path set, m represents a horizontal directionThe number of path scatter points, k represents the number of longitudinal path scatter points; calculating the fitness value of each alternative path by using the multi-objective advantage function, and taking the alternative path with the maximum fitness value as the multi-objective optimal reactive reference path, namely the reference path
g denotes the g-th segment, a, of the Path Pathg、bg、cgRepresents the curve coefficient of the g-th section of Path.
The longitudinal direction in the invention refers to the y-axis direction, and the transverse direction is the x-axis direction; a rotation of 90 ° counter-clockwise in the direction of forward movement of the vehicle means a rotation on the plane of the y-axis.
Scatter of path
The specific generation process comprises: the laser radar mounting point is taken as a starting point, the vehicle advancing direction is taken as a longitudinal direction, and the direction vertical to the longitudinal direction is taken as a transverse direction; taking k interval points with delta p as a longitudinal interval and m transverse interval points with delta s as a transverse interval in a transverse direction passing through each longitudinal interval point in a travelable area, and generating k multiplied by m interval points as path scatter points
Wherein
0.5≤Δp≤3；0.3≤Δp≤1。
The method comprises the following steps of setting a multi-objective advantage function, considering path smoothness, path length and safety distance between a local path and a travelable boundary by multiple objectives, wherein the advantage function is as follows:
f＝ωdfdis+ωsfsmo+ωufsec
in the above formula, fdisRepresenting path length, ω d2 denotes the length weight, fsmoIndicates smoothness, ω s4 denotes smoothness weight, fsecRepresenting the safe distance, ω, between the path and the travelable boundaryu1.5 denotes the safe distance weight, fmin_disRepresents the shortest distance, f, from the path scatter to the left and right boundaries(j+1)Represents a curve segment, f ″, between longitudinally adjacent path scatter points j and j +1(j+1)Representing a curve segment f(j+1)Of (c) is performed. As shown in fig. 2(b), the boundary of the mine is not a smooth straight line or curve, the path with the largest safety distance is obtained by averaging the left and right boundaries of the mine, and the path has many singular points and cannot be tracked by vehicles; the method for smoothing the path with the maximum safety distance comprises the following steps: the method comprises the steps that 5 characteristic points are taken at equal intervals on a path with the maximum safety distance, cubic spline interpolation is carried out on the 5 characteristic points to generate a smooth path, the path obtained by the method has high requirement on selection of the characteristic points, and meanwhile if the number of laneway wall defect points is large, the larger the difference of the transverse distances among the characteristic points is, the worse the smoothness of the path is possible; the obtained path has lower requirement on the accuracy of the roadway wall data, and has higher stability and generalization performance on the aspects of path length, path smoothness and safety synthesis, thereby being easy to realize vehicle tracking.
The correction of the navigation positioning error in step two is further described with reference to fig. 3(a) and fig. 3 (b):
fig. 3(a) is a driving scene of an underground mine, which is a F-shaped driving route with a total length of one way 147m, and which includes 15 steps, wherein the straight line is 1, 4,5, 8, 11, 12, 15; the pre-turning is 2, 6, 9 and 13; the turns are 3, 7, 10, 14. The beacons are set at the edge of the mine tunnel in the top view as shown in fig. 3, and in order to ensure that no laser signal blocking occurs between the beacons, the beacons may be set at vertically different positions and should be spaced from each other by a certain distance.
Under a vehicle coordinate system, extracting angle and distance information [ R ] between the center of the characteristic beacon reflector and the vehicle-mounted laser radar through clustering and layeringi,ωi,αi](reference: Xujing Bond, Liuyang, Wang Xiaoze, Von Song, Liu boat.) research on laser-guided AGV Global positioning method based on reflective markers [ J]Manufacturing automation 2020,42(02): 113-; 1,2, …, N, alphaiIs the horizontal rotation angle omega from the center of the reflector to the laser radariThe vertical angle from the center of the reflector to the laser radar is defined; riThe distance from the center of the reflector to the laser radar; where N is the number of characteristic beacons at each intersection of a straight and curved cut.
Because the mine car is only influenced by the transverse position and the longitudinal position when being controlled at the bottom layer, but not influenced by the vertical position, the calculation of the vertical position of the mine car is not considered; taking the left boundary of the mine as the forward direction of a y-axis driving task, taking the direction which is vertical to the forward direction of the driving task and rotates 90 degrees anticlockwise as the forward direction of an x-axis, taking the starting point of the driving task as the origin of coordinates to establish a global coordinate system, and utilizing the absolute position (x) of a beacon under the global coordinate systemi',yi'), (i ═ 1,2, 3) reverse thrust car position [ x0,y0]Firstly, two beacons are combined into three groups, and the transverse and longitudinal positions of the mine car are respectively calculated, and the beacons (x) are used1',y1')、(x2',y2') take two beacon absolute positions (x) as an examplei',yi') as the center of circle, by the relative distance R between the mine car and the beaconicosωi(i-1, 2) calculating intersection coordinates for the radius:
(x-x′1)2+(y-y′1)2＝R2 1 cos2ω1
(x-x'2)2+(y-y'2)2＝R2 2 cos2ω2
each group generating 2Possible coordinates for each tramcar, three sets yielding a total of 6 possible coordinates for each tramcar
(m ═ 1,2,3,4,5, 6); then, the 6 groups of coordinates are screened, and coordinates with the difference between the horizontal coordinates and the difference between the vertical coordinates smaller than an error threshold value are screened out:
and finally, averaging the screened coordinates to generate coordinates which are the actual positions of the mine cars. The experimental result is shown in fig. 3(b), and it can be known from fig. 3(b) that the positioning accuracy on the ground can reach centimeter level and is relatively stable, and the positioning error of the underground mining area can be continuously increased along with the time.
The trajectory generation method based on feedback-type tracking error correction described in step three is further described with reference to fig. 4(a) and 4 (b):
the lateral correction of the tramcar is achieved using the following curve, Trajectory:
wherein Tracjectory represents the reactive Path after adjusting the lateral offset, g represents the g-th segment of the Path Path, ag、bg、cgCoefficient of the curve representing the g-th segment of Path, c1Denotes the intercept of the curve segment closest to the mine car in the path, ε 0.1 denotes the mine car heading correction factor, v 10km/h denotes the longitudinal car velocity, Δ d denotes the relative position of the mine car in the longitudinal direction with respect to the beacon,
indicating the travel time of the mine car from the beacon. The experimental result is shown in fig. 4(b), before pre-entering into a curve, when the error between the vehicle and the expected path is large (more than 1.5m), the vehicle tracks the original multi-target path, and the mine road is limited to be too short and the original multi-target path is trackedThe error can not be corrected in time, and the bend-in failure can be caused, but the planned path can be adjusted in time according to the error between the vehicle and the expected path by adopting the track generation method based on the feedback type tracking error correction, so that the error between the mine car and the expected path before the bend-in can be kept in a small range, and the mine car can smoothly pass through the intersection.
And (3) generating a curve expected path: firstly, identifying the left turn (l) and the right turn (r) of the prompting turn direction of the beacon, and selecting the geometric description l: y of the driving boundary of the mine roadway with the same prompting directionl＝alx2+blx+clOr r: yr＝arx2+brx+crNext, 20 feature points are taken on the curve at intervals of Δ p' of 0.3m in the advancing direction of the mine car, and left turn is taken as an example
As a reference path point, generating a curve reference path by using least square fitting, and then limiting the curvature of the curve reference path, namely deleting the curvature larger than Clim' 0.15 points generate the desired trajectory y of the curvecur＝acurx2+bcurx+ccurFinally, the curve planned trajectory y is obtained by shifting the curve desired trajectory lateral deviation Δ q to 1.7 in consideration of the vehicle body widthcur'＝acurx2+bcurx+ccur+Δq。
Furthermore, the safety of the underground mine is considered, when the mine car collides with an obstacle in the running process, the mine car is immediately decelerated and stopped, and the safety is ensured.
Claims (7)
1. A local path planning method for intelligent driving under a mine is characterized by comprising the following steps:
1) detecting a characteristic beacon in the mine by the vehicle-mounted laser radar, if the characteristic beacon exists, indicating that the mine car is about to enter a bend, and entering the step 2); otherwise, indicating that the mine car is still in the straight-going stage, and entering the step 3); in the straight-going stage, the path planning is carried out by the following steps:
A) carrying out discrete processing on the clustered and layered data points acquired by the vehicle-mounted laser radar to generate a travelable boundary;
B) establishing a vehicle coordinate system by taking the advancing direction of a vehicle as the positive direction of a y axis, the direction which is vertical to the advancing direction of the vehicle and rotates 90 degrees anticlockwise with the advancing direction of the vehicle as the positive direction of an x axis and taking the transmitting center of the laser radar as a coordinate origin, and when a left boundary l is present in a travelable boundary, y isl＝alx2+blx+clAnd a right boundary r: yr＝arx2+brx+crGenerating a reference path, wherein al、bl、clRepresenting the left boundary curve coefficient, cl>0，ar、br、crRepresenting the coefficient of the right boundary curve, cr<0; when only one boundary or no boundary exists in the drivable boundaries, detecting whether the laser radar has a fault; the reference path generation method comprises the following steps: randomly sampling in the area between the left boundary and the right boundary, i.e. the travelable area, and randomly generating route scatter points
Taking a laser radar mounting point as a starting point, taking a path scattered point which is farthest from the starting point in the longitudinal direction as an end point, and connecting all curves between any two longitudinally adjacent path scattered points between the starting point and the end point by utilizing cubic spline interpolation to obtain an alternative path; co-generate mkTaking the path as an alternative path set, wherein m represents the scattering number of the transverse path, and k represents the scattering number of the longitudinal path; calculating the fitness value of each alternative path by using the multi-target advantage function, and taking the alternative path with the maximum fitness value as the multi-target optimal reactive reference path, namely the reference path
g denotes the g-th segment, a, of the Path Pathg、bg、cgThe curve coefficient representing the g section of Path;
2) extracting angle and distance information between the characteristic beacon and the mine car, calculating the relative distance between the characteristic beacon and the mine car in real time according to the angle and distance information, calculating the position of the mine car according to the absolute position of the beacon in the mine and the relative distance under a global coordinate system, realizing navigation positioning error correction, and entering step 4); the specific implementation process for implementing the navigation positioning error correction comprises the following steps:
I) taking the advancing direction of the driving task as the positive direction of a y axis, taking the direction which is vertical to the advancing direction of the driving task and rotates 90 degrees anticlockwise to the advancing direction of the driving task as the positive direction of an x axis, taking the starting point of the driving task as the origin of coordinates to establish a global coordinate system, combining N beacons in pairs, calculating the position of the mine car by utilizing the position of each combination under the global coordinate system and the corresponding relative distance between the combination and the mine car, and generating N x (N-1) possible positions of the mine car altogether
Wherein
Showing the possible abscissa of the tram,
representing possible ordinates of the mine cars, comparing two by two in possible positions of N x (N-1) mine cars, and screening out the coordinates for which the difference between the abscissas and the difference between the ordinates are both less than an error threshold xi, i.e. if
And is
Then screening out
And
wherein the relative distance of the mine car and each combinationThe component of the distance from the center of the reflector to the laser radar on the installation horizontal plane of the laser radar is shown; xi is less than 0.2; u, e ≠ e, 1., N × (N-1), and u ≠ e;
II) respectively averaging the abscissa and the ordinate in the screened coordinates to obtain the actual position of the mine car;
3) before the mine car enters the curve, adjusting the transverse deviation of the reference path according to the control error between the car body and the reactive reference path fed back by the mine car at a straight road and curve switching interface, so as to realize the transverse control correction of the mine car before entering the curve;
4) and after the mine car enters the curve, generating an expected path of the curve of the mine car according to the turning direction prompted by the beacon.
2. The method for local path planning for intelligent driving under mine according to claim 1, wherein the path scatter
The specific generation process comprises: the laser radar mounting point is taken as a starting point, the vehicle advancing direction is taken as a longitudinal direction, and the direction vertical to the longitudinal direction is taken as a transverse direction; taking k interval points with delta p as a longitudinal interval and m transverse interval points with delta s as a transverse interval in a transverse direction passing through each longitudinal interval point in a travelable area, and generating k multiplied by m interval points as path scatter points
Wherein
0.5≤Δp≤3；0.3≤Δp≤1。
3. The local path planning method for intelligent driving under the mine according to claim 1, wherein in the step B), the expression of the multi-objective advantage function is as follows:
f＝ωdfdis+ωsfsmo+ωufsec；
wherein f isdisRepresenting path length, ωdRepresenting length weight, fsmoIndicates smoothness, ωsRepresenting a smoothness weight, fsecRepresenting the safe distance, ω, between the path and the travelable boundaryuRepresenting a safe distance weight, fmin_disRepresents the shortest distance, f, from the path scatter to the left and right boundaries(j+1)Represents a curve segment, f ″, between two longitudinally adjacent path scatter points j and j +1(j+1)Representing a curve segment f(j+1)Of (c) is performed.
4. The local path planning method for intelligent driving under the mine according to claim 1, wherein the specific implementation process of the step 3) comprises the following steps: the lateral correction of the tramcar is achieved using the following curve, Trajectory:
wherein Tracjectory represents the reactive Path after adjusting the lateral offset, g represents the g-th segment of the Path Path, ag、bg、cgCoefficient of the curve representing the g-th segment of Path, c1Represents the intercept of the curve segment in the path closest to the mine car, epsilon represents the mine car heading correction factor, v represents the longitudinal speed of the mine car, deltad represents the relative position of the mine car in the longitudinal direction with respect to the beacon,
indicating the travel time of the mine car from the beacon.
5. The local path planning method for intelligent driving under the mine according to claim 1, wherein in the step 4), the process of generating the expected path of the bend of the mine car comprises:
a) identifying the prompting turning direction of the characteristic beacon, and selecting the geometric description l, y of the drivable boundary of the mine tunnel with the same prompting directionl＝alx2+blx+clOr r: yr＝arx2+brx+cr(ii) a Wherein l represents a left turn; r represents a right turn;
b) taking k 'feature points on the travelable boundary l or r every delta p' in the advancing direction of the mine car, and generating a curve reference path by using a least square fitting method;
c) deleting curvatures greater than C on the curved reference pathlim' generating a desired trajectory for the curve;
d) and the expected curve track is transversely deviated by delta q to be taken as the expected curve path.
6. A local path planning system for mine underground intelligent driving is characterized by comprising computer equipment; the computer device is configured or programmed for carrying out the steps of the method according to one of claims 1 to 5.
7. The system for local path planning for intelligent driving under a mine according to claim 6, wherein the computer device is in communication with a vehicle-mounted laser radar; the vehicle-mounted laser radar is used for acquiring signals of characteristic beacons arranged at cut interfaces of straight roads and curved roads in a mine.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011557897.XA CN112527000B (en) | 2020-12-23 | 2020-12-23 | Local path planning method and system for mine underground intelligent driving |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011557897.XA CN112527000B (en) | 2020-12-23 | 2020-12-23 | Local path planning method and system for mine underground intelligent driving |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112527000A CN112527000A (en) | 2021-03-19 |
| CN112527000B true CN112527000B (en) | 2021-10-26 |
Family
ID=74976376
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011557897.XA Active CN112527000B (en) | 2020-12-23 | 2020-12-23 | Local path planning method and system for mine underground intelligent driving |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112527000B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113204236B (en) * | 2021-04-14 | 2022-05-20 | 华中科技大学 | An agent path tracking control method |
| CN113619605B (en) * | 2021-09-02 | 2022-10-11 | 盟识(上海)科技有限公司 | Automatic driving method and system for underground mining articulated vehicle |
| CN114061612B (en) * | 2021-11-23 | 2024-01-19 | 上海伯镭智能科技有限公司 | Method and device for generating curve angle of unmanned mine car |
| CN114812566B (en) * | 2022-06-27 | 2022-10-04 | 青岛慧拓智能机器有限公司 | Method and device for planning driving path of mine vehicle and computer equipment |
| CN116380086A (en) * | 2023-03-28 | 2023-07-04 | 安徽海博智能科技有限责任公司 | A trajectory planning method for unmanned mining trucks based on drivable area |
| CN117558147B (en) * | 2024-01-11 | 2024-03-26 | 上海伯镭智能科技有限公司 | Mining area unmanned vehicle road right distribution remote control method |
| CN117842171B (en) * | 2024-03-04 | 2024-06-07 | 中铁一局集团电务工程有限公司 | Trolley steering method with multiple control modes |
Family Cites Families (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB1602455A (en) * | 1977-04-16 | 1981-11-11 | Hill Rotherham Ltd Thomas | Vehicles |
| CN101713999B (en) * | 2009-11-18 | 2011-09-07 | 北京矿冶研究总院 | Navigation control method of underground autonomous scraper |
| US8256355B2 (en) * | 2011-02-07 | 2012-09-04 | Coalcap, Llc | Cover system for open top rail cars |
| CN103197675B (en) * | 2013-03-13 | 2016-05-25 | 北京矿冶研究总院 | Autonomous driving and obstacle avoidance motion control and target path planning method of underground scraper |
| TWI585561B (en) * | 2013-06-03 | 2017-06-01 | 新智控私人有限公司 | Method and apparatus for offboard navigation of a robotic device |
| US9174672B2 (en) * | 2013-10-28 | 2015-11-03 | GM Global Technology Operations LLC | Path planning for evasive steering maneuver in presence of target vehicle and surrounding objects |
| CN107065848A (en) * | 2016-11-29 | 2017-08-18 | 攀枝花市九鼎智远知识产权运营有限公司 | A kind of underground mine locomotive Unmanned Systems and method based on high accuracy positioning navigation terminal |
| CN111757822B (en) * | 2018-02-26 | 2024-04-02 | 联邦快递服务公司 | Systems and methods for enhanced collision avoidance on logistical ground support devices using multisensor detection fusion |
| CN109017793B (en) * | 2018-07-26 | 2020-04-07 | 中南大学 | Autonomous parking navigation and control method based on front-rear axis fusion reference |
| CN109388140B (en) * | 2018-09-13 | 2021-08-03 | 江苏大学 | An Improved Pure Tracking Control Method for Ground Vehicle Path Tracking |
| CN109752701B (en) * | 2019-01-18 | 2023-08-04 | 中南大学 | Road edge detection method based on laser point cloud |
| CN109885049B (en) * | 2019-02-12 | 2021-06-04 | 北京航空航天大学 | Automatic mapping and path matching method for laser-guided AGV (automatic guided vehicle) based on dead reckoning |
| CN110109451B (en) * | 2019-04-10 | 2022-07-12 | 东南大学 | A Novel Geometric Path Tracing Algorithm Considering Path Curvature |
| JP7333195B2 (en) * | 2019-05-15 | 2023-08-24 | 株式会社Subaru | Automated driving support system |
| CN111003027B (en) * | 2019-11-19 | 2022-02-08 | 中国科学院合肥物质科学研究院 | Unmanned mine car safety monitoring method and system |
| CN111600933B (en) * | 2020-04-15 | 2022-04-05 | 北京踏歌智行科技有限公司 | Mining area unmanned transportation system based on 5G and mine forklift cooperative control method thereof |
| CN112034479A (en) * | 2020-06-15 | 2020-12-04 | 煤炭科学技术研究院有限公司 | Positioning method and system applied to intelligent inspection unmanned aerial vehicle under coal mine |
-
2020
- 2020-12-23 CN CN202011557897.XA patent/CN112527000B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN112527000A (en) | 2021-03-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112527000B (en) | Local path planning method and system for mine underground intelligent driving | |
| CN110262508B (en) | Automatic guiding system and method applied to unmanned freight vehicle in closed field | |
| CN109952547B (en) | Automatic control of a motor vehicle on the basis of lane data and motor vehicle | |
| CN110341711B (en) | Traveling track generation system and method based on wharf environment | |
| US6169940B1 (en) | Automatic driving system | |
| CN104960520B (en) | Pre- based on Pure Pursuit algorithms takes aim at a determination method | |
| US5926126A (en) | Method and system for detecting an in-path target obstacle in front of a vehicle | |
| CN102591332B (en) | Device and method for local path planning of pilotless automobile | |
| US20080270018A1 (en) | Method for Course Prediction in Driver Assistance Systems for Motor Vehicles | |
| CN104881025B (en) | A kind of reactive navigation control method of underground mine vehicle | |
| CN107037818A (en) | A kind of path following method of pilotless automobile | |
| CN113359752A (en) | Automatic driving method for underground coal mine skip car | |
| CN111006667B (en) | Automatic driving track generation system under high-speed scene | |
| CN109987092A (en) | A kind of determination method on vehicle obstacle-avoidance lane-change opportunity and the control method of avoidance lane-change | |
| CN111176298B (en) | Unmanned vehicle track recording and tracking method | |
| CN106696961A (en) | Control system and method for automatically driving onto and off ramp of freeway | |
| CN110979339B (en) | Front road form reconstruction method based on V2V | |
| CN104019825A (en) | Route planning determination method | |
| CN108594821A (en) | Unmanned mine car positioning and autonomous tracing in intelligent vehicle | |
| CN111137298B (en) | Vehicle automatic driving method, device, system and storage medium | |
| CN112937606A (en) | Anti-collision path planning and control method and system for tracking automatic driving vehicle | |
| CN111857121B (en) | Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar | |
| US20200384997A1 (en) | Convoy of Variable Gap | |
| CN114841260A (en) | Mine underground multi-sensor information fusion autonomous obstacle avoidance navigation method | |
| JP2020098566A (en) | Automatic driving system |
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 |