Path planning and control method for mine unmanned trackless rubber-tyred vehicle

Abstract

The invention relates to the technical field of unmanned driving of mine transport vehicles, in particular to a path planning and control method for a mine unmanned trackless rubber-tyred vehicle, and the method comprises the steps: building a model according to the dynamics of an underground trackless rubber-tyred vehicle; the basic Bezier smooth curve is improved into a third-order Bezier smooth curve algorithm; an improved third-order Bezier smooth curve algorithm is adopted to carry out obstacle avoidance planning of the trackless rubber vehicle; and establishing a transverse and longitudinal cooperative controller based on the MPC algorithm. A third-order Bezier smooth curve is improved on a second-order Bezier smooth curve, and the method has the advantages that control points of the curve inevitably pass through a path starting point, a convex hull contains all the control points, related lines are inevitably located in the convex hull, the curve is still the Bezier curve after derivation, the control points can be represented by the original control points, and the time is 0 lt; tlt; 1, the method is faster and more labor-saving than a second-order Bezier curve; however, due to narrow roadways, bumpy road surfaces and the like, the driverless vehicle control precision is based on a transverse and longitudinal cooperative controller of an MPC algorithm, and deviation is greatly reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned driving of mine transport vehicles, and in particular to a path planning and control method for an unmanned mine trackless rubber-tyred vehicle. Background Art

[0002] With the advancement of the smart mining concept, mining construction is transitioning towards intelligent systems. However, most mine auxiliary transportation systems still rely on manual operation, resulting in low efficiency and poor safety. Underground tunnels are complex, characterized by poor lighting, narrow tunnels, and bumpy roads, leading to frequent transportation accidents. While autonomous driving technology has gained increasing attention in the mining sector in recent years, the development of autonomous rubber-tyred trackless vehicles for mines still faces numerous challenges, such as the need for path planning algorithms to adapt to the unique underground environment and vehicle control strategies to cope with complex operating conditions.

[0003] While autonomous driving technology has begun to be gradually applied in mining, such as in mine locomotives, open-pit dump trucks, and mine inspection robots, research on autonomous trackless rubber-tyred vehicles underground still faces numerous challenges. Underground roads are poorly lit, making it difficult for a single sensor to construct a high-definition global map. The narrow roads also require advanced path planning technology, and the vehicle's lateral and longitudinal control accuracy needs to be further improved.

[0004] Vehicle control strategies also need to adapt to the unique working conditions underground, such as narrow lanes and steep curves. Otherwise, research solutions may face compatibility issues. Therefore, optimizing the obstacle avoidance planning and control technology for unmanned trackless rubber-tyred vehicles has become a key issue that needs to be addressed in the current intelligent development of mines. Summary of the Invention

[0005] The present invention aims to provide a path planning and control method for an unmanned trackless rubber-tyred vehicle in a mine, so as to solve the problems of low path planning efficiency and poor tracking control accuracy, and promote the development of unmanned underground transportation systems.

[0006] In order to achieve the above objectives, the technical solutions provided are as follows:

[0007] A path planning and control method for an unmanned trackless rubber-tyred vehicle in a mine, characterized by comprising the following steps:

[0008] Step 1: Construct a model based on the dynamics of underground trackless rubber-tyred vehicles;

[0009] Step 2: Improve the basic Bezier smooth curve to a third-order Bezier smooth curve algorithm;

[0010] Step 3: Use the improved third-order Bezier smooth curve algorithm to perform obstacle avoidance planning for the trackless rubber vehicle;

[0011] Step 4: Establish a horizontal and vertical collaborative controller based on the MPC algorithm.

[0012] Preferably, in Step 1, by analyzing the driverless trackless rubber-tyred vehicle system, the underground roadway working conditions, and the rules for the trackless rubber-tyred vehicle driving underground, a linear two-degree-of-freedom model is adopted as the dynamic model of the trackless rubber-tyred vehicle, a two-degree-of-freedom dynamic model of the trackless rubber-tyred vehicle considering the tyre side slip characteristics is constructed, the lateral and longitudinal decoupling control of the vehicle is realized in the Frenet coordinate system, and the dynamic model of the vehicle is constructed; the tyre side slip characteristics are considered in the dynamic model, the forces on the vehicle are analyzed, and according to the force balance and torque balance, we can obtain:

[0013] ma y =F yf cosδ f +F yr (1)

[0014]

[0015] In the formula, m is the total vehicle mass; a y is the acceleration component in the y direction of the vehicle body coordinate system; is the derivative of the heading angle; I is the moment of inertia; F yf is the lateral force of the front wheel, F yr is the lateral force of the rear wheel, δ f is the deflection angle of the front wheel.

[0016] Preferably, in Step 2, the derivation formula for the middle control point of the basic Bessel curve is as follows:

[0017]

[0018] The solution of Equation (3) is described as Equation (4);

[0019]

[0020] M + =M T (MM T ) -1 ∈R (n-1N×2N . (6)

[0021]

[0022] In the formula, P is the position vector, V0 is the initial velocity, T is the time interval, V n is the final velocity, V0 is the initial velocity, M performs a linear transformation on the coefficient vector, each I N is an N-dimensional identity matrix, corresponding to the identity transformation in the N-dimensional space, the diagonal block A n,i,i controls the independence of each order coefficient, and the off-diagonal block A n,i,j (j < i) describes the influence of the low-order terms on the high-order coefficients.

[0023] Assume that point A on P0P1, P 1, Point B on P2 and point C on AB (that is, C is a point on the curve), then according to formulas (3)-(7), we get:

[0024]

[0025] Substituting A and B into C in the above formula, we can get the formula of the quadratic Bezier curve:

[0026] B(t)=(1-t) 2 P0+2t(1-t)P1+t 2 P2,t∈[0,1] (9).

[0027] Preferably, in step 3, a third-order Bezier smooth curve algorithm is used to perform obstacle avoidance planning for the trackless rubber vehicle. As a general discussion on the derivation of the benchmark Bezier curve, the time variable t and the time to reach the destination T are introduced, as shown in the following formula:

[0028]

[0029] Then we can derive

[0030]

[0031] where b i The coefficient vector representing the unfolded n-th order Bezier curve.

[0032] The formula for the third-order Bezier curve is as follows:

[0033] B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3,t∈[0,1] (12)

[0034] The curve display module is used to display the generated Bezier curve trajectory on the graphical interface. Users can zoom, translate and rotate the displayed curve for easy observation. 1, P2 and P3 represent four points in the rectangular coordinate system:

[0035]

[0036] Among them, b0(t) and b5(t) are endpoint-dominated basis functions, which are based on the fourth power of sin and cos, respectively, to control the endpoints (such as t = 0, 2, 4 or t = 1, 3), and the parameters λ and μ adjust the shape;

[0037] b1(t) and b4(t) are for the auxiliary intermediate section, which are the product of the cubic and the first power, and cooperate with parameters to adjust the local slope (in the sections of t = 2 - 4 and t = 0 - 2);

[0038] b2(t) and b3(t) handle the intermediate transition (t = 1 - 3), combine sin and cos to ensure smooth symmetry, have no additional parameters, and are responsible for the continuity of the transition section.

[0039] Preferably, in step four, a lateral and longitudinal cooperative controller combined with the MPC (Model Predictive Control) algorithm is established according to the model in step one. Based on the dynamic model of the system, the behavior of the system in the future for a period of time is predicted, and the control input at the current moment is determined by online solving an optimization problem to minimize a certain performance index; in vehicle control, based on the basic objectives of simultaneously controlling the lateral and longitudinal directions by MPC, for example, minimizing the trajectory error of the vehicle within the prediction horizon and meeting the dynamic constraint requirements of the vehicle, where the prediction horizon of the prediction model is N p The future control input sequence is The future state prediction is:

[0040]

[0041] In the formula, x i , u i is the state vector, x i = x(t + i|t) represents the system state predicted i steps forward at time t; u i = u(t + i|t), represents the control input at the i-th step forward at time t; x N = x(t + N|t) represents the system state at the terminal time N, A and B are system matrices. Generally, a quadratic programming (QP) solver is used to solve the above cost function. The standard form of the QP problem is:

[0042]

[0043] s.t.GU k ≤h, EU k = e (17).

[0044] The beneficial effects of the present invention are as follows: The present invention improves the third-order Bezier smoothing curve on the second-order Bezier smoothing curve. Its advantages are that the control points of the curve must pass through the path starting point, the convex hull contains all control points, and the relevant lines must be located within the convex hull. After derivation, it is still a Bezier curve, and the control points can be represented by the original control points, where the time is 0 < t < 1, which is faster and more labor-saving than the second-order Bezier curve. However, due to narrow roadways, bumpy road surfaces, etc., the control accuracy of the unmanned vehicle is poor. We establish a lateral and longitudinal cooperative controller based on the MPC algorithm to greatly reduce the deviation. Description of the Drawings

[0045] Figure 1 The present invention is a flow chart of the path planning and control method of the unmanned trackless rubber-tyred vehicle in a mine.

[0046] Figure 2 This is the dynamic model diagram of the underground trackless rubber-tyred vehicle.

[0047] Figure 3 It is a second-order Bezier smooth curve.

[0048] Figure 4 It is a third-order Bezier smooth curve. DETAILED DESCRIPTION

[0049] The following is a clear and complete description of the technical solutions in the embodiments of the present invention, in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts are within the scope of protection of the present invention.

[0050] Example 1

[0051] A path planning and control method for an unmanned trackless rubber-wheeled vehicle in a mine, such as Figure 1 As shown, the following steps are included:

[0052] Step 1: Construct a model based on the dynamics of underground trackless rubber-tyred vehicles;

[0053] Step 2: Improve the basic Bezier smooth curve to a third-order Bezier smooth curve algorithm;

[0054] Step 3: Use the improved third-order Bezier smooth curve algorithm to perform obstacle avoidance planning for the trackless rubber vehicle;

[0055] Step 4: Establish a horizontal and vertical collaborative controller based on the MPC algorithm.

[0056] In step one, if Figure 2 As shown in the figure, by analyzing the unmanned trackless rubber-tyred vehicle system, underground tunnel conditions, and underground driving rules of the trackless rubber-tyred vehicle, a linear two-degree-of-freedom model is used as the trackless rubber-tyred vehicle dynamic model. A two-degree-of-freedom trackless rubber-tyred vehicle dynamic model considering tire sideways characteristics is constructed. The vehicle's lateral and longitudinal decoupling control is achieved in Frenet coordinates, and the vehicle's dynamic model is constructed. The dynamic model considers tire sideways characteristics, analyzes the vehicle's forces, and based on its force balance and moment balance, the following can be obtained:

[0057] ma y =F yf cosδf +F yr (1)

[0058]

[0059] In the formula, m is the vehicle mass; a y is the acceleration component in the y direction of the vehicle body coordinate system; is the derivative of the heading angle; I is the moment of inertia. F yf is the lateral force of the front wheel, F yr is the lateral force of the rear wheel, δ f is the deflection angle of the front wheel.

[0060] Figure 2 In, XOY is the earth coordinate system, and xoy is the vehicle body coordinate system.

[0061] In step two, as Figure 3 shown, based on the derivation scheme of the intermediate control points of the Bezier curve, the Bezier curve is fitted by 2 end points and 1 set of intermediate control points. The conditions of (3)-(7) are summarized into the following formula:

[0062]

[0063] In the formula, P is the position vector, V0 is the initial velocity, T is the time interval, V n is the final velocity, V0 is the initial velocity, M performs a linear transformation on the coefficient vector for each I N is the N-dimensional identity matrix, corresponding to the identity transformation in the N-dimensional space.

[0064] The solution of equation (3) is described as equation (4).

[0065]

[0066] M + = M T (MM T ) -1 ∈R (n-1N×2N . (6)

[0067]

[0068] In the formula, P is the position vector, V0 is the initial velocity, T is the time interval, V n is the final velocity, V0 is the initial velocity, M performs a linear transformation on the coefficient vector for each I N is the N-dimensional identity matrix, corresponding to the identity transformation in the N-dimensional space, and the diagonal block A n,i,i controls the independence of each order coefficient, and the off-diagonal block A n,i,j (j < i) describes the influence of the lower-order terms on the higher-order coefficients.

[0069] Assume that point A on P0P1, P 1, Point B on P2 and point C on AB (that is, C is a point on the curve. Then according to formulas (3)-(7), we have:

[0070]

[0071] Substituting A and B into C in the above formula, we can get the formula of the quadratic Bezier curve:

[0072] B(t)=(1-t) 2 P0+2t(1-t)P1+t 2 P2,t∈[0,1] (9)

[0073] Second-order Bezier smooth curves such as Figure 3 shown.

[0074] In step 3, a third-order Bezier smoothing curve algorithm is used for obstacle avoidance planning of the trackless rubber vehicle. As a general discussion of the derivation of the benchmark Bezier curve, the time variable t and the time to reach the destination T are introduced as follows.

[0075]

[0076] Then we can derive

[0077]

[0078] where b i The coefficient vector representing the unfolded n-th order Bezier curve.

[0079] The third-order Bezier curve is obtained by linearly combining two second-order Bezier curves. The formula of the third-order Bezier curve is as follows:

[0080] B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3,t∈[0,1] (12)

[0081] The curve display module is used to display the generated Bezier curve trajectory on the graphical interface. Users can zoom, translate and rotate the displayed curve for easy observation. 1, P2 and P3 represent four points in the rectangular coordinate system.

[0082]

[0083] Among them, b0(t) and b5(t) are endpoint-dominated basis functions, which are based on the fourth power of sin and cos, respectively, to control the endpoints (such as t = 0, 2, 4 or t = 1, 3), and the parameters λ and μ adjust the shape;

[0084] b1(t) and b4(t) are auxiliary intermediate segments, the product of the cubic and the linear power, and the parameters are used to adjust the local slope (t = 2-4 and t = 0-2 segments);

[0085] b2(t) and b3(t) handle the intermediate transition (t=1-3), combining sin and cos to ensure smooth symmetry, without additional parameters, and are responsible for the continuity of the transition segment.

[0086] The third-order Bezier curve can fit 4 points and can depict a complete and smooth robot movement path as time changes. Figure 4 shown.

[0087] In step 4, a lateral and longitudinal coordinated controller is established based on the model of step 1, which combines the MPC (model predictive control) algorithm. The system's behavior in the future is predicted based on the system's dynamic model, and the control input at the current moment is determined by solving an optimization problem online to minimize a certain performance indicator. In vehicle control, the basic goals of controlling the lateral and longitudinal directions simultaneously based on MPC are, for example, minimizing the vehicle's trajectory error in the prediction time domain to meet the vehicle's dynamic constraints. The prediction model prediction time domain is N p The future control input sequence is The future state is predicted to be

[0088]

[0089] Where x i ,u i is the state vector, x i =x(t+i|t) represents the system state predicted i steps ahead at time t; u i =u(t+i|t), which represents the control input of the i-th step forward at time t; x N =x(t+N|t) represents the system state at the terminal time N, A and B are system matrices. Generally, a quadratic programming (QP) solver is used to solve the above cost function. The standard form of the QP problem is:

[0090]

[0091] sqFt k ≤h,EU k =e (17).

[0092] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims, not the foregoing description, and all variations within the meaning and range of equivalents of the claims are intended to be included therein. Any reference sign in a claim should not be construed as limiting the claim to which it relates.

Claims

1. A path planning and control method for an unmanned trackless rubber-tyred vehicle in a mine, characterized in that: The steps include: Step 1: Construct a model based on the dynamics of underground trackless rubber-tyred vehicles; Step 2: Improve the basic Bezier smooth curve to a third-order Bezier smooth curve algorithm; Step 3: Use the improved third-order Bezier smooth curve algorithm to perform obstacle avoidance planning for the trackless rubber vehicle; Step 4: Establish a horizontal and vertical collaborative controller based on the MPC algorithm.

2. The path planning and control method for an unmanned trackless rubber-tyred vehicle in a mine according to claim 1, characterized in that: In step 1, by analyzing the unmanned trackless rubber-tyred vehicle system, underground tunnel conditions, and underground driving rules of the trackless rubber-tyred vehicle, a linear two-degree-of-freedom model was used as the trackless rubber-tyred vehicle dynamics model. A two-degree-of-freedom trackless rubber-tyred vehicle dynamics model considering the tire sideways characteristics was constructed. The lateral and longitudinal decoupling control of the vehicle was realized in Frenet coordinates, and the vehicle dynamics model was constructed. The tire sideways characteristics were considered in the dynamics model, and the vehicle force was analyzed. According to its force balance and moment balance, the following can be obtained: ma y =F yf cosδ f +F yr (1) Where m is the vehicle mass; a y is the acceleration component in the y direction of the vehicle body coordinate system; is the heading angle derivative; I is the moment of inertia; F yf is the front wheel lateral force, F yr is the rear wheel lateral force, δ f is the front wheel deflection angle.

3. The path planning and control method for an unmanned trackless rubber-tyred vehicle in a mine according to claim 1, characterized in that: In step 2, the derivation formulas (3)-(7) of the intermediate control points of the basic Bezier curve are shown as follows: The solution of equation (3) is described as equation (4); M + =M T (MM T ) -1 ∈R (n-1)N×2N . (6) Where, P is the position vector, V0 is the initial velocity, T is the time interval, and V n is the final velocity, V0 is the initial velocity, and M linearly transforms the coefficient vector for each I N is the N-dimensional identity matrix, corresponding to the identity transformation in the N-dimensional space, and the diagonal block A n,i,i controls the independence of each order coefficient, and the off-diagonal block A n,i,j (j < i) describes the influence of the lower-order terms on the higher-order coefficients; Assume that point A on P0P1, P 1, Point B on P2 and point C on AB (i.e., C is a point on the curve), then according to formulas (3)-(7), we get: Substituting A and B into C in the above formula, we can get the formula of the quadratic Bezier curve: B(t)=(1-t) 2 P0+2t(1-t)P1+t 2 P2,t∈[0,1] (9)。 4. The path planning and control method for an unmanned trackless rubber-tyred vehicle in a mine according to claim 1, characterized in that: In step 3, a third-order Bezier smoothing curve algorithm is used for obstacle avoidance planning of the trackless rubber vehicle. As a general discussion of the derivation of the benchmark Bezier curve, the time variable t and the time to reach the destination T are introduced, as shown in the following formula: Then we can derive where b i The coefficient vector representing the unfolded n-th order Bezier curve. The formula for the third-order Bezier curve is as follows: B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3,t∈[0,1] (12) The curve display module is used to display the generated Bezier curve trajectory on the graphical interface. Users can zoom, translate and rotate the displayed curve for easy observation. 1, P2 and P3 represent four points in the rectangular coordinate system: Among them, b0(t) and b5(t) are endpoint-dominated basis functions, which are based on the fourth power of sin and cos, respectively, to control the endpoints (such as t = 0, 2, 4 or t = 1, 3), and the parameters λ and μ adjust the shape; b1(t) and b4(t) are auxiliary intermediate segments, the product of the cubic and the linear power, and the parameters are used to adjust the local slope (t = 2-4 and t = 0-2 segments); b2(t) and b3(t) handle the intermediate transition (t=1-3), combining sin and cos to ensure smooth symmetry, without additional parameters, and are responsible for the continuity of the transition segment.

5. The path planning and control method for an unmanned trackless rubber-tyred vehicle in a mine according to claim 1, characterized in that: In step 4, a lateral and longitudinal coordinated controller combined with the MPC algorithm is established based on the model of step 1. The system behavior in the future is predicted based on the dynamic model of the system, and the control input at the current moment is determined by solving an optimization problem online to minimize a certain performance indicator. In vehicle control, the basic goals of lateral and longitudinal control are controlled simultaneously based on MPC, such as minimizing the vehicle's trajectory error in the prediction time domain to meet the vehicle's dynamic constraints. The prediction time domain of the prediction model is N p The future control input sequence is The future state forecast is: Where x i ,u i is the state vector, x i =x(t+i|t) represents the system state predicted i steps ahead at time t; u i =u(t+i|t), which represents the control input of the i-th step forward at time t; x N =x(t+N|t) represents the system state at the terminal time N, A and B are system matrices. Generally, a quadratic programming (QP) solver is used to solve the above cost function. The standard form of the QP problem is:

Images