Unmanned path tracking method for mine underground articulated vehicle

Abstract

The invention relates to a mine underground articulated vehicle unmanned driving path tracking method, and belongs to the technical field of mining automation. The method comprises the steps of obtaining vehicle path information; acquiring vehicle state information; calculating front frame vehicle position information; an optimal preview point is found; calculating a vehicle control quantity; inputting the vehicle control quantity into a vehicle controller to drive the vehicle to run; and judging whether an end point is reached or not, and if not, repeating the steps. According to the method, the special environment of the mine underground and the structural characteristics of the articulated vehicle are comprehensively considered, high-precision trajectory tracking and autonomous navigation of the vehicle are achieved by designing a reasonable optimized path tracking algorithm and a control system, and the problem of unmanned path tracking of the articulated vehicle in the narrow roadway environment of the mine underground is solved.

Description

Technical Field

[0001] This invention belongs to the field of mining automation technology, specifically relating to an unmanned path tracking method for articulated vehicles in underground mines. Background Technology

[0002] In the mining industry, underground operations have always been high-risk, high-intensity, and labor-intensive work. Traditional articulated vehicles, as the main force in underground material transportation, rely on manual driving. This not only places extremely high demands on the physical strength and skills of the drivers, but also makes them prone to safety accidents in the complex and ever-changing underground environment, affecting work efficiency and personnel safety.

[0003] In recent years, with the rapid development of autonomous driving technology, it has demonstrated significant advantages in multiple fields, especially in improving operational efficiency, reducing safety risks, and lowering labor costs. Applying autonomous driving technology to articulated vehicles in underground mines has become an important way to promote the automation and intelligentization of mining.

[0004] However, applying autonomous driving technology to articulated vehicles in underground mines faces numerous technical challenges. First, the complex and ever-changing underground environment, including insufficient lighting, confined spaces, and undulating terrain, all pose difficulties for the perception, decision-making, and control of autonomous driving systems. Second, the unique structural characteristics of articulated vehicles, such as the flexibility and stability of the articulated sections, also place higher demands on the accuracy and stability of autonomous driving trajectory tracking.

[0005] Currently, although some unmanned driving technologies for underground mining vehicles have been proposed, most existing path tracking technologies often select the aiming point (a virtual target reference point set on the path ahead of the vehicle, which the control system adjusts to track by adjusting the vehicle's state) sequentially or choose the nearest trajectory point as the current aiming point. However, trajectory tracking is a crucial component of unmanned driving technology, and its accuracy and stability directly affect vehicle safety and operational efficiency.

[0006] Therefore, overcoming the shortcomings of existing technologies is an urgent problem to be solved in the field of mining automation technology. Summary of the Invention

[0007] The purpose of this invention is to overcome the shortcomings of the prior art and provide a method for unmanned path tracking of articulated vehicles in underground mines.

[0008] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0009] A method for unmanned path tracking of articulated vehicles in underground mines includes the following steps:

[0010] Step S1: Obtain vehicle route information;

[0011] Step S2: Obtain vehicle status information;

[0012] Step S3: Calculate the vehicle position information of the front frame;

[0013] Step S4: Find the optimal aiming point;

[0014] Step S5: Calculate the vehicle control quantity;

[0015] Step S6: Input the vehicle control quantity to the vehicle controller to drive the vehicle.

[0016] Step S7: Determine whether the destination has been reached. If the destination has not been reached, repeat steps S2-S7.

[0017] Furthermore, preferably, in step S1, the vehicle path information is a set of coordinate points including x-coordinates and y-coordinates.

[0018] Furthermore, preferably, in step S2, the vehicle status information includes the vehicle's driving direction (dir) and lateral coordinate (x). r Vertical coordinate y r Heading angle θ r The front and rear frame hinge angle α, and the distance l from the center of the rear frame to point O at the hinge point between the front and rear frames. r The distance l from the center of the front frame to point O, the hinge point between the front and rear frames. f .

[0019] Furthermore, preferably, in step S3, the method for calculating the front frame position is as follows:

[0020] θ f =θ r -α;

[0021] x f =x r +l f cosθ f +l r cosθ r ;

[0022] y f =y r +l f sinθ f +l r sinθ r ;

[0023] Where, x f : Lateral coordinates of the front frame;

[0024] y f Longitudinal coordinates of the front frame;

[0025] x r : Rear frame lateral coordinates;

[0026] y r : Rear frame longitudinal coordinate;

[0027] θ r : Rear frame heading angle;

[0028] θ f : Front frame heading angle;

[0029] α: Front and rear frame hinge angle;

[0030] l r : The distance from the center of the rear frame to point O, the hinge point between the front and rear frames;

[0031] l f : The distance from the center of the front frame to point O, the hinge point between the front and rear frames;

[0032] Thus, the front frame position information (x) is obtained. f y f θ f ).

[0033] Furthermore, preferably, the specific method of step S4 is as follows:

[0034] Step 401: Based on the vehicle's current driving direction dir, determine whether to use the front frame position information or the rear frame position information for the solution.

[0035] When the direction is forward, dir=1, and the rear frame position information (x) is selected. r y r θ r );

[0036] When the direction is reverse, dir=0, and the front frame position information (x) is selected. f y f θ f );

[0037] Step 402: Traverse the path messages and select points P0 to P1 in the path sequentially. i Let P be the vehicle reference point obtained in t transactions. t (x t ,y t );

[0038] Step 403, calculate the distance error between the current vehicle position and the reference point:

[0039] When dir=1,

[0040] When dir=0,

[0041] Step 404, calculate the heading angle error between the current vehicle position and the reference point, specifically:

[0042] First calculate the vehicle position O r With reference point P t The heading angle θ:

[0043] When dir=1,

[0044] When dir=0,

[0045] Thus, the heading angle error can be obtained:

[0046] When dir=1, e θ =θ r -θ

[0047] When dir=0, e θ =θ f -θ

[0048] Step 405: Determine if the distance error is within the limit values. Select a maximum limit value of 3 times the vehicle's minimum turning radius R, and a minimum limit value of 1.5 times the vehicle's minimum turning radius R. The distance error should then satisfy the following:

[0049] 1.5R <d<3R

[0050] Step 406: Determine if the heading angle error is within the limit value. If it is greater than or less than the limit value, discard it. Let the maximum articulation angle of the vehicle be δ. The heading angle error should satisfy:

[0051] -δ <e θ <δ

[0052] Step 407: The distance error and heading angle error are weighted and summed to obtain the optimal aiming point evaluation function; specifically:

[0053] The scaling factor k is the ratio of the current maximum distance error to the maximum hinge angle:

[0054] k = 3R / δ

[0055] The optimal aiming point evaluation function is:

[0056] f(t) = d + k * e θ

[0057] Where, f(t): evaluation value;

[0058] d: Distance error between the current vehicle position and the reference point;

[0059] k: proportionality coefficient;

[0060] e θ : course angle error;

[0061] Step 408, if the weighted evaluation value is less than the evaluation value of the reference point selected at the (t - 1)th time, that is, f(t) < f(t - 1), then the information of the currently selected reference point P t (x t , y t ) is used as the current optimal preview point information P * (x t , y t ), and at the same time, the course angle error e calculated in step 405 under this reference point θ is used as the vehicle control quantity described in step S5;

[0062] Step 409, determine whether all the reference points of the path information have been traversed: if not all traversed, continue to traverse and evaluate the next reference point; if all the reference points have been traversed, then step S4 is completed, and step S5 is executed.

[0063] Further, preferably, the vehicle control quantity is the course angle error e θ .

[0064] Further, preferably, in step S6, the vehicle controller refers to a vehicle bus controller, which converts the vehicle control quantity into a steering drive signal to control the vehicle hydraulic cylinder to contract for steering control.

[0065] The present invention proposes an unmanned trajectory tracking method applicable to articulated vehicles in underground mines, aiming to solve the problems existing in the prior art, improve the accuracy and stability of trajectory tracking, and further enhance the safety and efficiency of underground mine operations. This method comprehensively considers the special environment of underground mines and the structural characteristics of articulated vehicles, and realizes high-precision trajectory tracking and autonomous navigation of the vehicle by designing a reasonable optimized path tracking algorithm and control system, solving the problem of unmanned path tracking of articulated vehicles in narrow roadway environments in underground mines.

[0066] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0067] (1) Aiming at the special structure of articulated vehicles in underground mines, by optimizing the preview point selection strategy and trajectory tracking algorithm, the path tracking accuracy is significantly improved. Experiments show that under complex working conditions such as roadway bends, the lateral tracking error is reduced by more than 30% compared with traditional methods, effectively avoiding problems such as control instability caused by the saturation of the articulated angular velocity.

[0068] (2) By introducing a pre-aiming point evaluation function, this invention simultaneously considers the heading angle error and distance error between the current position and the reference path information, optimizes the actual driving trajectory, prevents large angle adjustments, and enables more stable vehicle driving control. Attached Figure Description

[0069] Figure 1 This is a flowchart of the unmanned driving path tracking method for articulated vehicles in underground mines according to the present invention.

[0070] Figure 2 Flowchart for selecting aiming points;

[0071] Figure 3 This is a schematic diagram of an articulated vehicle model. Detailed Implementation

[0072] The present invention will now be described in further detail with reference to the embodiments.

[0073] Those skilled in the art will understand that the following embodiments are for illustrative purposes only and should not be construed as limiting the scope of the invention. Where specific techniques or conditions are not specified in the embodiments, they are performed in accordance with the techniques or conditions described in the literature in the field or according to the product instructions. Materials or equipment whose manufacturers are not specified are all conventional products that can be obtained by purchase.

[0074] A method for unmanned path tracking of articulated vehicles in underground mines includes the following steps:

[0075] Step S1: Obtain vehicle route information;

[0076] Step S2: Obtain vehicle status information;

[0077] Step S3: Calculate the vehicle position information of the front frame;

[0078] Step S4: Find the optimal aiming point;

[0079] Step S5: Calculate the vehicle control quantity;

[0080] Step S6: Input the vehicle control quantity to the vehicle controller to drive the vehicle.

[0081] Step S7: Determine if the destination has been reached. If the destination has not been reached, repeat steps S2-S7.

[0082] The path information mentioned in step S1 is a set of coordinate points including x-coordinates and y-coordinates.

[0083] The vehicle status information mentioned in step S2 includes the vehicle's driving direction (dir) and the lateral coordinate (x) of the rear frame. r The longitudinal coordinate of the rear frame is y. r , rear frame heading angle θr The front and rear frame hinge angle α, and the distance l from the center of the rear frame to point O at the hinge point between the front and rear frames. r The distance l from the center of the front frame to point O, the hinge point between the front and rear frames. f .

[0084] The articulated vehicle model mentioned in step S3 is a static motion model of the articulated vehicle, such as... Figure 3 As shown, the coordinates of the front frame position are:

[0085] θ f =θ r -α

[0086] x f =x r +l f cosθ f +l r cosθ r

[0087] y f =y r +l f sinθ f +l r sinθ r

[0088] x f : Lateral coordinates of the front frame;

[0089] y f Longitudinal coordinates of the front frame;

[0090] x r : Rear frame lateral coordinates;

[0091] y r : Rear frame longitudinal coordinate;

[0092] θ r : Rear frame heading angle;

[0093] θ f : Front frame heading angle;

[0094] α: Front and rear frame hinge angle;

[0095] l r : The distance from the center of the rear frame to point O, the hinge point between the front and rear frames;

[0096] l f : The distance from the center of the front frame to point O, the hinge point between the front and rear frames;

[0097] The pre-aiming point evaluation function in step S4 is solved as follows:

[0098] like Figure 2 The diagram shows the pre-aiming point selection flowchart of this invention. The selection principle is to choose the reference point with the shortest distance and the smallest required hinge angle within the effective path information reference point range as the pre-aiming point. The specific steps include:

[0099] Step 401: Determine whether to use the front frame position information or the rear frame position information for solving based on the vehicle's current driving direction (dir).

[0100] When the direction is forward (dir=1), the rear frame position information (x) is selected. r y r θ r );

[0101] When the direction is reverse (dir=0), the front frame position information (x) is selected. f y f θ f );

[0102] In this example, the forward direction is selected for solving; the backward direction can be obtained similarly.

[0103] Step 402: Traverse the path messages and select points P0 to P1 in the path sequentially. i Let P be the vehicle reference point obtained in t transactions. t (x t ,y t );

[0104] Step 403, calculate the distance error between the current vehicle position and the reference point, based on... Figure 3 The distance error can be obtained from the model shown:

[0105] When dir=1,

[0106] When dir=0,

[0107] Step 404: Calculate the heading angle error between the current vehicle position and the reference point, which is the articulation angle that needs to be controlled, such as... Figure 3 As shown, first calculate the vehicle position O. r With reference point P t The heading angle θ:

[0108] When dir=1,

[0109] When dir=0,

[0110] The heading angle error was obtained as follows:

[0111] When dir=1, e θ =θr -θ

[0112] When dir = 0, e θ = θ f -θ

[0113] Step 405: Determine whether the distance error is within the limit value. If it is greater than or less than the limit value, it is discarded. Considering the CPU computing power and the minimum turning radius R of the vehicle, the maximum limit value is selected as 3 times the minimum turning radius R of the vehicle, and the minimum limit value is selected as 1.5 times the minimum turning radius R of the vehicle. Then the distance error should satisfy:

[0114] 1.5R < d < 3R

[0115] Step 406: Determine whether the heading angle error is within the limit value. If it is greater than or less than the limit value, it is discarded. Let the maximum articulation angle of the vehicle be δ, and the heading angle error should satisfy:

[0116] -δ < e θ < δ

[0117] Step 407: Perform weighted summation on the distance error and the heading angle error. After unifying the dimensions of the distance error and the heading angle error through the proportionality coefficient k, perform weighted summation. The proportionality coefficient k is the ratio of the current maximum distance error to the maximum articulation angle:

[0118] k = 3R / δ

[0119] The optimal preview point evaluation function after weighted summation can be obtained as:

[0120] f(t) = d + k * e θ

[0121] f(t): Evaluation value; / /

[0122] d: Distance error between the current vehicle position and the reference point;

[0123] k: Proportionality coefficient;

[0124] e θ : Heading angle error;

[0125] Step 408: If the weighted evaluation value is less than the evaluation value of the reference point selected at t - 1 times (f(t) < f(t - 1)), then the information P t (x t , y t ) of the currently selected reference point is used as the information P*(x t , y t ) of the current optimal preview point. At the same time, the heading angle error e θ calculated under this reference point in Step 405 is used as the vehicle control quantity described in Step S5.

[0126] Step 409: Determine whether all reference points of the path information have been traversed: If not, continue traversing and evaluate the next reference point; if all reference points have been traversed, then step S4 is completed and step S5 is executed.

[0127] The vehicle control quantity calculated in step S5 is the result of calculating the current vehicle position and the optimal aiming point P. * (x t ,y t The heading angle error e θ That is, the hinge angle that needs to be controlled, such as Figure 3 As shown, first calculate the vehicle position O. r With reference point P t The heading angle θ:

[0128] When dir=1,

[0129] When dir=0,

[0130] The heading angle error was obtained as follows:

[0131] When dir=1, e θ =θ r -θ

[0132] When dir=0, e θ =θ f -θ

[0133] Step S6, the vehicle controller refers to the vehicle bus controller, which transmits the vehicle control quantity e θ This is converted into a steering drive signal, which controls the contraction of the vehicle's hydraulic cylinders to perform steering control.

[0134] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A method for unmanned path tracking of articulated vehicles in underground mines, characterized in that, Includes the following steps: Step S1: Obtain vehicle route information; Step S2: Obtain vehicle status information; Step S3: Calculate the vehicle position information of the front frame; Step S4: Find the optimal aiming point; Step S5: Calculate the vehicle control quantity; Step S6: Input the vehicle control quantity to the vehicle controller to drive the vehicle. Step S7: Determine whether the destination has been reached. If the destination has not been reached, repeat steps S2-S7.

2. The unmanned driving path tracking method for articulated vehicles in underground mines according to claim 1, characterized in that, In step S1, the vehicle path information is a set of coordinate points including x-coordinates and y-coordinates.

3. The unmanned driving path tracking method for articulated vehicles in underground mines according to claim 1, characterized in that, In step S2, the vehicle status information includes the vehicle's driving direction (dir) and lateral coordinate (x). r Vertical coordinate y r Heading angle θ r The front and rear frame hinge angle α, and the distance l from the center of the rear frame to point O at the hinge point between the front and rear frames. r The distance l from the center of the front frame to point O, the hinge point between the front and rear frames. f .

4. The unmanned driving path tracking method for articulated vehicles in underground mines according to claim 3, characterized in that, In step S3, the method for calculating the position of the front frame is as follows: i f =θ r -a; x f =x r +l f cosθ f +l r cosθ r ; and f / and r +l f sinθ f +l r sinθ r ; Where, x f : Lateral coordinates of the front frame; y f Longitudinal coordinates of the front frame; x r : Rear frame lateral coordinates; y r : Rear frame longitudinal coordinate; θ r : Rear frame heading angle; θ f : Front frame heading angle; α: Front and rear frame hinge angle; l r : The distance from the center of the rear frame to point O, the hinge point between the front and rear frames; l f : The distance from the center of the front frame to point O, the hinge point between the front and rear frames; Thus, the front frame position information (x) is obtained. f y f θ f ).

5. The unmanned driving path tracking method for articulated vehicles in underground mines according to claim 4, characterized in that, The specific method for step S4 is as follows: Step 401: Based on the vehicle's current driving direction dir, determine whether to use the front frame position information or the rear frame position information for the solution. When the direction is forward, dir=1, and the rear frame position information (x) is selected. r y r θ r ); When the direction is reverse, dir=0, and the front frame position information (x) is selected. f y f θ f ); Step 402: Traverse the path messages and select points P0 to P1 in the path sequentially. i Let P be the vehicle reference point obtained in t transactions. t (x t ,y t ); Step 403, calculate the distance error between the current vehicle position and the reference point: When dir=1, When dir=0, Step 404, calculate the heading angle error between the current vehicle position and the reference point, specifically: First calculate the vehicle position O r With reference point P t The heading angle θ: When dir=1, When dir=0, Thus, the heading angle error can be obtained: When dir = 1, e θ = θ r -θ When dir = 0, e θ = θ f -θ Step 405: Determine if the distance error is within the limit values. Select a maximum limit value of 3 times the vehicle's minimum turning radius R, and a minimum limit value of 1.5 times the vehicle's minimum turning radius R. The distance error should then satisfy the following: 1.5R <d<3R Step 406: Determine if the heading angle error is within the limit value. If it is greater than or less than the limit value, discard it. Let the maximum articulation angle of the vehicle be δ. The heading angle error should satisfy: -d <e θ <d Step 407: The distance error and heading angle error are weighted and summed to obtain the optimal aiming point evaluation function; specifically: The scaling factor k is the ratio of the current maximum distance error to the maximum hinge angle: k = 3R / δ The optimal aiming point evaluation function is: f(t)=d+k*e θ Where, f(t): evaluation value; d: Distance error between the current vehicle position and the reference point; k: proportionality coefficient; e θ : Heading angle error; Step 408, if the weighted evaluation value is less than the evaluation value of the reference point selected at the (t - 1)th time, that is, f(t) < f(t - 1), then the information of the currently selected reference point P t (x t , y t ) is used as the current optimal preview point information P * (x t , y t ). At the same time, the heading angle error e Θ calculated at this reference point in Step 405 is used as the vehicle control quantity described in Step S5; Step 409: Determine whether all reference points of the path information have been traversed: If not, continue traversing and evaluate the next reference point; if all reference points have been traversed, then step S4 is completed and step S5 is executed.

6. The unmanned driving path tracking method for articulated vehicles in underground mines according to claim 5, characterized in that, The vehicle control quantity is the heading angle error e θ .

7. The unmanned driving path tracking method for articulated vehicles in underground mines according to claim 1, characterized in that, In step S6, the vehicle controller refers to the vehicle bus controller, which converts the vehicle control quantity into a steering drive signal and controls the contraction of the vehicle hydraulic cylinder to perform steering control.

Images