A mine unmanned track tracking control method

Abstract

The present application relates to a kind of mine unmanned track tracking control method, it is related to unmanned driving technical field, solve the existing technology cannot accurately control mine car travel under the scene of mine, the method comprises: obtaining the travel path data when mine car is driven artificially;According to travel path data, determine the first vehicle travel data;Collect the second vehicle travel data when mine car is unmanned, and the second vehicle travel data is the real-time data generated in the transport process of mine car;According to the first vehicle travel data and the second vehicle travel data, determine deviation data;According to deviation data, control the travel speed and steering of mine car so that mine car travels along the travel path corresponding to travel path data;Wherein, deviation data is the difference between the first vehicle travel data and the second vehicle travel data.The technical scheme provided by the present application can improve work efficiency and reduce safety accidents.

Description

Technical Field

[0001] This invention relates to the field of unmanned driving technology, and in particular to a method for trajectory tracking and control of unmanned vehicles in mining areas. Background Technology

[0002] The application of autonomous driving technology in mining areas can eliminate the risk of personnel injury, improve labor productivity, and enhance the level of intelligence and automation in mining areas. Meanwhile, open-pit mines are also an ideal scenario for the implementation of autonomous driving technology, as wide-body mining vehicles operate on fixed routes, and the roads in mining areas are closed, with no external vehicles or personnel.

[0003] The vehicle's position information after the delay is calculated using a vehicle kinematics model and a delay prediction model, thereby obtaining the desired steering wheel angle.

[0004] However, the above method requires building models of the mining trucks and roads, and determining the steering wheel angle and vehicle position information based on the model parameters. In mining areas, roads are unstructured, and mining trucks are not standardized, making it difficult to build an accurate model for the mining environment. This results in the inability to precisely control the mining trucks in such scenarios. Summary of the Invention

[0005] Based on the above analysis, the present invention aims to provide a trajectory tracking and control method for unmanned vehicles in mining areas, applicable to mining scenarios, thereby improving operational efficiency and reducing safety accidents.

[0006] The objective of this invention is mainly achieved through the following technical solutions:

[0007] This invention provides a method for trajectory tracking and control of unmanned vehicles in mining areas, comprising:

[0008] Obtain the driving path data of the mining truck during manual driving;

[0009] Based on the driving path data, determine the driving data of the first vehicle;

[0010] Collect second vehicle driving data when the mining truck is unmanned, the second vehicle driving data is real-time data generated by the mining truck during transportation;

[0011] Based on the driving data of the first vehicle and the driving data of the second vehicle, the deviation data is determined;

[0012] Based on the deviation data, the driving speed and steering of the mining truck are controlled so that the mining truck travels along the driving path corresponding to the driving path data; wherein, the deviation data is the difference between the first vehicle driving data and the second vehicle driving data, and the deviation data includes: gradient difference, load difference, driving speed difference, driving acceleration difference, heading angle difference, tracking error, and front wheel angular velocity difference.

[0013] Further, the throttle opening or brake opening of the mine car is determined based on at least one of the gradient difference, the load difference, and the driving acceleration difference, as well as the driving speed difference.

[0014] The speed of the mine car is controlled according to the throttle opening or the brake opening.

[0015] Furthermore, fuzzy control rules are determined based on any one of the gradient difference, the load difference, and the driving acceleration difference, as well as the driving speed difference;

[0016] The opening fuzzy value is determined based on any one of the slope difference, the load difference, and the driving acceleration difference, as well as the driving speed difference and the fuzzy control rule.

[0017] The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0018] Furthermore, a first fuzzy control rule is determined based on the gradient difference and the driving speed difference;

[0019] The second fuzzy control rule is determined based on the load difference and the driving speed difference;

[0020] Based on the first fuzzy control rule, determine the first opening fuzzy value;

[0021] The second opening fuzzy value is determined according to the second fuzzy control rule;

[0022] The aperture fuzzy value is determined using the following formula 1:

[0023] U = k1U1 + k2U2 (Formula 1)

[0024] Wherein, U is used to represent the opening fuzzy value, U1 is used to represent the first opening fuzzy value, k1 is used to represent the weight of U1, U2 is used to represent the second opening fuzzy value, and k2 is used to represent the weight of U2.

[0025] The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0026] Furthermore, a first fuzzy control rule is determined based on the gradient difference and the driving speed difference;

[0027] The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed.

[0028] Based on the first fuzzy control rule, determine the first opening fuzzy value;

[0029] The third fuzzy value of the opening degree is determined according to the third fuzzy control rule.

[0030] The aperture fuzzy value is determined using the following formula 2:

[0031] U=k1U1+k3U3 Formula 2;

[0032] Wherein, U is used to represent the opening degree fuzzy value, U1 is used to represent the first opening degree fuzzy value, k1 is used to represent the weight of U1, U3 is used to represent the third opening degree fuzzy value, and k3 is used to represent the weight of U3.

[0033] The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0034] Furthermore, a second fuzzy control rule is determined based on the load difference and the driving speed difference;

[0035] The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed.

[0036] The second opening fuzzy value is determined according to the second fuzzy control rule;

[0037] The third fuzzy value of the opening degree is determined according to the third fuzzy control rule.

[0038] The aperture fuzzy value is determined using the following formula 3:

[0039] U=k2U2+k3U3 Formula 3;

[0040] Wherein, U is used to represent the opening degree fuzzy value, U2 is used to represent the second opening degree fuzzy value, k2 is used to represent the weight of U2, U3 is used to represent the third opening degree fuzzy value, and k3 is used to represent the weight of U3.

[0041] The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0042] Furthermore, a first fuzzy control rule is determined based on the gradient difference and the driving speed difference;

[0043] The second fuzzy control rule is determined based on the load difference and the driving speed difference;

[0044] The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed.

[0045] Based on the first fuzzy control rule, determine the first opening fuzzy value;

[0046] The second opening fuzzy value is determined according to the second fuzzy control rule;

[0047] The third fuzzy value of the opening degree is determined according to the third fuzzy control rule.

[0048] The aperture fuzzy value is determined using the following formula 4:

[0049] U=k1U1+k2U2+k3U3 Formula 4;

[0050] Wherein, U is used to represent the opening fuzzy value, U1 is used to represent the first opening fuzzy value, k1 is used to represent the weight of U1, U2 is used to represent the second opening fuzzy value, k2 is used to represent the weight of U2, U3 is used to represent the third opening fuzzy value, and k3 is used to represent the weight of U3.

[0051] The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0052] Furthermore, the front wheel steering angle is determined based on the heading angle difference, the tracking error, and the front wheel angular velocity difference;

[0053] The steering of the mining car is determined based on the front wheel steering angle.

[0054] Furthermore, the front wheel steering angle is calculated and determined using the following formula 5:

[0055]

[0056] Where δ(t) characterizes the front wheel steering angle, ψ(t) characterizes the heading angle difference, e(t) characterizes the tracking error, v(t) characterizes the vehicle speed, m characterizes the mine car mass, C_y characterizes the roll stiffness, and ω meas ω is used to characterize the real-time observed value of the front wheel angular velocity. traj Used to characterize the preset value of the front wheel angular velocity, (ω meas -ω traj ) represents the difference in angular velocity between the front wheels, ψ ss δ is used to characterize the tire slip angle caused by lateral acceleration in wide-body mining vehicles. meas (i) represents the observed value of the front wheel steering angle in the i-th real-time observation, k yaw k steer k soft Both k and are coefficients, and both are constants.

[0057] Furthermore, the k yaw The value range of k is 20-50; steerThe value range of k is 0.01-0.05; soft =1; the value of k ranges from 2 to 3.

[0058] The technical solutions provided by the embodiments of the present invention have at least one of the following technical effects:

[0059] 1. Unstructured roads typically lack lane markings and clear road boundaries. Furthermore, the presence of shadows and watermarks makes it difficult to distinguish between road and non-road areas. Therefore, path data is first determined to differentiate between road and non-road areas. Then, based on the mine car's path data, the first vehicle's driving data is pre-determined. During the mine car's journey, using this first vehicle's driving data as a reference, deviation data is used to ensure that the mine car's real-time speed and steering (the second vehicle's driving data) are as close as possible to the corresponding speed and steering of the first vehicle, thus reducing the risk of accidents. Simultaneously, when controlling real-time speed and steering, the non-standardized characteristics of the mining area's roads and mine cars are considered. Factors such as gradient, load, heading angle, driving trajectory, and front wheel angular velocity are digitized to improve operational efficiency and reduce accidents.

[0060] 2. Considering the complex road and vehicle conditions in the mining area, and taking into account data such as gradient differences, load differences, driving acceleration differences, and driving speed differences, multiple longitudinal control methods were set up using fuzzy control to improve the applicability of the method.

[0061] 3. Considering the complex road and vehicle conditions in the mining area, the coefficients (constant k) of various parameters are adjusted. yaw k steer k soft (and k), which makes lateral control more applicable.

[0062] Other features and advantages of the invention will be set forth in the following description, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures particularly pointed out in the written description and the accompanying drawings. Attached Figure Description

[0063] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0064] Figure 1 A flowchart of a trajectory tracking and control method for unmanned vehicles in a mining area provided by an embodiment of the present invention;

[0065] Figure 2 This is a schematic diagram of the transportation route provided in an embodiment of the present invention;

[0066] Figure 3 A diagram showing the positional relationship between the wheels of a wide-body mining vehicle when turning, provided for an embodiment of the present invention.

[0067] Figure label:

[0068] A, B - Curves; a - Distance from the front axle to the center of gravity of the mine car; b - Distance from the middle axle to the center of gravity of the mine car; c - Distance from the rear axle to the center of gravity of the mine car. Detailed Implementation

[0069] Embodiments of the present disclosure will now be described with reference to the accompanying drawings. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of the disclosure. Furthermore, descriptions of well-known structures and technologies are omitted in the following description to avoid unnecessarily obscuring the concepts of the present disclosure.

[0070] Unmanned driving in mining areas can eliminate the risk of personnel injury, improve labor productivity, and enhance the level of intelligence and automation in mining operations. Open-pit mines are also ideal application scenarios for unmanned driving technology, as the routes for unmanned transport operations using wide-body mining vehicles are fixed, and the mine roads are closed, with no external vehicles or personnel. However, in actual production operations in open-pit mines, the complex conditions limit the application of unmanned driving technology, mainly in terms of mine paths and mining vehicles.

[0071] On the one hand, mining trucks are heavy and have a large load capacity, resulting in greater inertia than ordinary vehicles. Combined with their length, this leads to significant and non-linear control delays. On the other hand, the demand for mining trucks is far less than that for conventional vehicles, making large-scale production of customized mining trucks difficult and resulting in enormous costs. Furthermore, the complex usage scenarios of mining trucks make it challenging to design a universal model. Given these circumstances, mining areas typically use different types and performance levels of mining trucks, making it impossible to standardize their parameters. This non-standardization hinders effective control of the mining trucks' movement.

[0072] On the other hand, structured roads generally refer to well-structured highways such as expressways and urban arterial roads. The performance of these roads is set according to national standards. That is, factors such as gradient and load capacity are considered to a certain extent during highway design. Therefore, for structured roads, factors such as gradient and load capacity have little, if any, impact on lateral and longitudinal control. However, the unstructured road environment in mining areas is complex and difficult to meet the national standards for structured roads. In this case, the impact of factors such as gradient and load capacity on lateral and longitudinal control cannot be ignored. For example, there are uphill and downhill slopes, road depressions, and protruding rocks and soil; dust, landslides, and falling rocks are all potential safety hazards. These factors will directly or indirectly affect the unmanned driving control strategy of mining trucks.

[0073] In summary, conventional vehicle or road models for lateral or longitudinal control are not suitable for mining scenarios. To obtain a control method for unmanned mining vehicles that matches the mining environment, this invention provides a trajectory tracking control method for unmanned mining vehicles, such as... Figure 1 As shown, it includes the following steps:

[0074] Step 1: Obtain the driving path data of the mine truck when it is manually driven.

[0075] In this embodiment of the invention, the specific method for obtaining the driving path data of the mining truck is as follows:

[0076] Step S1: Record the transportation route in advance when driving manually.

[0077] In this embodiment of the invention, the transportation path in step S1 is a discrete set of points, and each point is also recorded with longitude, latitude, elevation, driving speed and load.

[0078] Step S2: Remove obvious errors in the transportation route.

[0079] In this embodiment of the invention, due to measurement errors, there are problems such as sharp corners, depressions and flying points in the transportation path obtained during manual driving. Therefore, it is necessary to remove the erroneous points corresponding to sharp corners, depressions and flying points in order to obtain a discrete set of points with uneven distribution.

[0080] Step S3: Set the area within the road radius r of the transportation route point as a drivable area.

[0081] In this embodiment of the invention, for cases where the distance between two points is greater than 2r, interpolation is performed between the two path points, and then the area within the radius r of the inserted point is set as a drivable area. Ultimately, the transportation path is transformed from a set of points into a connected drivable area.

[0082] Step S4: Solve using a general path planning algorithm to obtain a dense and continuous transportation path.

[0083] Step S5: Smooth the transportation path obtained in step S4.

[0084] In this embodiment of the invention, since the path is continuous, existing path smoothing algorithms, such as energy function-based path smoothing algorithms, can be used for processing.

[0085] Step S6: For the smoothed transportation path, use interpolation to solve the path equation and obtain the set of equally spaced path points, such as... Figure 2 As shown.

[0086] In this embodiment of the invention, the driving path data of the mine truck is the path point set obtained in step S6 and the longitude, latitude, elevation, driving speed and load collected in step S1.

[0087] Step 2: Determine the driving data of the first vehicle based on the driving route data.

[0088] In this embodiment of the invention, the preset slope, preset load, preset position, preset driving speed, preset acceleration, preset heading angle, preset front wheel angular velocity, and preset trajectory curvature of each trajectory point in step S6 are determined to obtain the first vehicle driving data.

[0089] Based on the longitude, latitude, and elevation recorded in step S1, calculate Figure 2 The longitude, latitude, and elevation of each trajectory point are used to determine the preset position and preset slope of each trajectory point.

[0090] Then according to Figure 2 Calculate the preset trajectory curvature for each trajectory point, and determine the speed based on the preset trajectory curvature, safe driving speed, and driving speed recorded in step S1. Figure 2 The preset driving speed at each path point. Specifically, such as... Figure 2 As shown, the speed at point A is the driving speed recorded in step S1, specifically 38 km / h. Point B is the trajectory point obtained by interpolation. If the radius of curvature of point B is greater than that of point A, and the safe driving speed at the curve is set to be no more than 40 km / h, then the preset driving speed at point B can be 39 km / h.

[0091] Calculate the preset acceleration at each track point based on the preset travel speed.

[0092] Based on the preset travel speed and preset trajectory curvature, the preset heading angle of each track point is determined.

[0093] Based on the preset driving speed and preset heading angle, determine the preset front wheel angular velocity at each track point.

[0094] Set the load recorded in step S1 as the preset load for each track point.

[0095] Step 3: Collect the second vehicle driving data when the mining truck is unmanned. The second vehicle driving data is the real-time data generated by the mining truck during transportation.

[0096] In this embodiment of the invention, the second vehicle driving data is real-time data generated by the mining truck during transportation. The second vehicle driving data includes one or more of the following: gradient, load, driving speed, driving acceleration, heading angle, tracking, front wheel angular velocity, longitude, latitude, elevation, heading angle, speed and trajectory curvature.

[0097] Step 4: Determine the deviation data based on the driving data of the first vehicle and the driving data of the second vehicle.

[0098] In this embodiment of the invention, the deviation data is the difference between the first vehicle's driving data and the second vehicle's driving data. For example, the slope difference is the difference between the real-time slope in the second vehicle's driving data and the preset slope of the nearest track point. The tracking error is the distance between the real-time position of the second vehicle and the nearest track point in the second vehicle's driving data. The deviation data includes: slope difference, load difference, driving speed difference, driving acceleration difference, heading angle difference, tracking error, and front wheel angular velocity difference.

[0099] Step 5: Based on the deviation data, control the speed and steering of the mine car so that it travels along the path corresponding to the travel path data.

[0100] In this embodiment of the invention, the throttle opening or brake opening of the mine car is determined based on at least one of the gradient difference, load difference, and driving acceleration difference, as well as the driving speed difference; and the driving speed of the mine car is controlled based on the throttle opening or brake opening.

[0101] Specifically, in order to cope with the complexity of roads in mining areas, embodiments of the present invention provide a variety of methods to control throttle opening or brake opening.

[0102] Method 1:

[0103] Based on the difference in slope and the difference in driving speed, fuzzy control rules are determined.

[0104] In this embodiment of the invention, the slope difference is used as one input signal E of the fuzzy controller, and the speed difference is used as the second input signal EC of the fuzzy controller. Let the fundamental universe of discourse for signal E be [-6,6], the fundamental universe of discourse for signal EC be [-6,6], and the fundamental universe of discourse for the opening fuzzy value U be [0,1]. The fuzzy set takes seven linguistic values ​​{NB,NM,NS,ZO,PS,PM,PB}, where NB,NM,NS,ZO,PS,PM,PB represent negative large, negative medium, negative small, 0, positive small, positive medium, and positive large, respectively. The fuzzy control rules are shown in Table 1.

[0105] Table 1 Fuzzy Control Rules

[0106]

[0107] The fuzzy value of the opening is determined based on the gradient difference, the driving speed difference, and the fuzzy control rules.

[0108] Specifically, based on the gradient difference, the corresponding language value is determined; based on the speed difference, the corresponding language value is determined. Then, based on the language values ​​corresponding to the gradient difference and the speed difference, the fuzzy value of the opening is determined.

[0109] The throttle or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0110] In this embodiment of the invention, the fuzzy control has a truth table corresponding to the fuzzy control rules. The truth table pre-stores the throttle opening or brake opening of the mine car, as well as the correspondence between the throttle opening or brake opening of the mine car and the fuzzy value.

[0111] The only difference between Method 2 and Method 1 is that the difference in load is used instead of the difference in slope.

[0112] The only difference between Method 3 and Method 1 is that the difference in acceleration is used instead of the difference in slope.

[0113] Therefore, Method 1 is suitable for mining areas with many slopes, Method 2 is suitable for mining cars with large load capacities, and Method 3 is suitable for mining cars with large inertia. Because fuzzy control is used, these three methods are applicable to various types and performance levels of mining cars. Furthermore, fuzzy control can be applied to scenarios with many curves in mining areas using slip ratio differences and travel speed differences.

[0114] However, as mentioned above, the road and vehicle conditions in mining areas are complex, often involving a combination of various scenarios. Preferably, embodiments of the present invention provide methods 4-7 for longitudinal control.

[0115] Method 4:

[0116] The first fuzzy control rule is determined based on the difference in slope and the difference in driving speed.

[0117] The second fuzzy control rule is determined based on the load difference and the driving speed difference;

[0118] Determine the first fuzzy value of the opening degree according to the first fuzzy control rule;

[0119] The second fuzzy value of the opening is determined according to the second fuzzy control rule;

[0120] The aperture fuzzy value is determined using the following formula 1:

[0121] U = k1U1 + k2U2 (Formula 1)

[0122] Wherein, U represents the fuzzy value of the opening degree, U1 represents the first fuzzy value of the opening degree, k1 represents the weight of U1, U2 represents the second fuzzy value of the opening degree, and k2 represents the weight of U2; usually, k1 is equal to k2. Specifically, the weight can be determined according to the magnitude of the slope and the load, for example, when the vehicle is going downhill empty, k1 is greater than k2.

[0123] The throttle or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0124] Method 5:

[0125] The first fuzzy control rule is determined based on the difference in slope and the difference in driving speed.

[0126] The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed;

[0127] Determine the first fuzzy value of the opening degree according to the first fuzzy control rule;

[0128] The third fuzzy control rule is used to determine the third opening fuzzy value;

[0129] The aperture fuzzy value is determined using the following formula 2:

[0130] U=k1U1+k3U3 Formula 2;

[0131] Where U represents the opening degree fuzzy value, U1 represents the first opening degree fuzzy value, k1 represents the weight of U1, U3 represents the third opening degree fuzzy value, and k3 represents the weight of U3; usually, k1 is equal to k3. Specifically, the weight can be determined according to the slope, for example, when the downhill slope is too steep, k1 is greater than k3.

[0132] The throttle or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0133] Method 6:

[0134] The second fuzzy control rule is determined based on the load difference and the driving speed difference;

[0135] The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed;

[0136] The second fuzzy value of the opening is determined according to the second fuzzy control rule;

[0137] The third fuzzy control rule is used to determine the third opening fuzzy value;

[0138] The aperture fuzzy value is determined using the following formula 3:

[0139] U=k2U2+k3U3 Formula 3;

[0140] Where U represents the first degree of ambiguity, U2 represents the second degree of ambiguity, k2 represents the weight of U2, U3 represents the third degree of ambiguity, and k3 represents the weight of U3; typically, k2 equals k3. Specifically, the weights can be determined based on the load; for example, when the vehicle is empty, k2 is less than k3.

[0141] The throttle or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0142] Method 7:

[0143] The first fuzzy control rule is determined based on the difference in slope and the difference in driving speed.

[0144] The second fuzzy control rule is determined based on the load difference and the driving speed difference;

[0145] The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed;

[0146] Determine the first fuzzy value of the opening degree according to the first fuzzy control rule;

[0147] The second fuzzy value of the opening is determined according to the second fuzzy control rule;

[0148] The third fuzzy control rule is used to determine the third opening fuzzy value;

[0149] The aperture fuzzy value is determined using the following formula 4:

[0150] U=k1U1+k2U2+k3U3 Formula 4;

[0151] Where U represents the fuzzy value of the opening degree, U1 represents the first fuzzy value of the opening degree, k1 represents the weight of U1, U2 represents the second fuzzy value of the opening degree, k2 represents the weight of U2, and U3 represents the third fuzzy value of the opening degree, k3 represents the weight of U3; usually, k1 = k2 = k3. Specifically, the weights can be determined according to the magnitude of the slope and the load. For example, when the vehicle is going downhill empty, k1 is greater than k2, while k2 is equal to k3.

[0152] The throttle or brake opening of the mine car is determined based on the fuzzy value of the opening.

[0153] It should be noted that in all fuzzy control rules involved in methods 4-7, the fundamental universe of discourse for signal E is [-6,6], the fundamental universe of discourse for signal EC is [-6,6], the fundamental universe of discourse for the opening fuzzy value U is [0,1], and the fuzzy set takes seven linguistic values: {NB,NM,NS,ZO,PS,PM,PB}. When considering multiple factors, it is necessary to set weights for the opening fuzzy value corresponding to each factor, for example, k1, k2, k3. As can be seen from methods 1-7, the technical solution provided by the embodiments of the present invention can select appropriate control methods according to the specific road conditions and mine car conditions in the mining area. In addition, fuzzy control corresponding to slip ratio difference and driving speed difference can be added to methods 4-7 to deal with situations with many curves in the mining area, such as mountain roads.

[0154] In this embodiment of the invention, the front wheel steering angle is determined based on the heading angle difference, tracking error, and front wheel angular velocity difference; the steering angle of the mine car is determined based on the front wheel steering angle.

[0155] Specifically, to address the complexity of mining roads and the diversity of mining vehicles, this embodiment of the invention provides a lateral control method:

[0156] The front wheel steering angle is determined based on the heading angle difference, the tracking error, and the front wheel angular velocity difference.

[0157] The steering of the mining car is determined based on the front wheel steering angle.

[0158] The front wheel steering angle is calculated using the following formula 5:

[0159]

[0160] Where δ(t) characterizes the front wheel steering angle, ψ(t) characterizes the yaw angle difference, e(t) characterizes the tracking error, v(t) characterizes the vehicle speed, C_y characterizes the roll stiffness (usually taken as 100 kN / rad per tire), and ω meas ω is used to characterize the real-time observed value of the front wheel angular velocity. traj The preset value used to characterize the front wheel angular velocity is obtained based on the driving data of the first vehicle; (ω meas -ω traj ) represents the difference in angular velocity between the front wheels, ψ ss δ is used to characterize the tire slip angle caused by lateral acceleration in wide-body mining vehicles. meas (i) represents the observed value of the front wheel steering angle in the i-th real-time observation, k yaw k steer k soft Both k and are coefficients, and both are constants.

[0161] When applying the Stanley algorithm to wide-body mining cars, the lack of forward aiming means control commands are only issued based on the nearest trajectory point to ensure the car's path conforms to a preset route. During this process, the car experiences slight swaying, resulting in an S-shaped trajectory. This S-shaped trajectory is prone to accidents. To improve safety, the angular velocity deviation is multiplied by a coefficient k. yaw For mining cards on k yaw The value range is 20-50.

[0162] Formula 5, term 5, is used to increase the smoothness of the steering mechanism to prevent sudden steering wheel movements during driving. steer The value range is 0.01-0.05, which ensures that the 5th item can provide a smooth steering angle output without reducing the control response speed.

[0163] To prevent δ(t) from changing too rapidly at low speeds due to v(t) being close to 0, a control coefficient k is set. soft =1, to improve control performance at low speeds.

[0164] The value of the coefficient k ranges from 2 to 3.

[0165] Preferably, the second term ψ in Formula 5 ss Expressed as follows:

[0166]

[0167] 'm' represents the mass of the mine car, 'a' is the distance from the front axle to the center of gravity of the mine car, 'b' is the distance from the middle axle to the center of gravity of the mine car, and 'c' is the distance from the rear axle to the center of gravity of the mine car. The positional relationship between 'a', 'b', and 'c' is as follows: Figure 3 As shown. ψ ss The side slip angle of a wide-body mining vehicle due to lateral acceleration is given. When the wide-body mining vehicle turns, the Stanley algorithm has a theoretical steady-state error, which can be addressed by appropriately increasing k. ag To compensate for steady-state error, k ag The calculated value is 70. To adapt it for use with mining cards, this invention will use k... ag The value range is set to 100-120.

[0168] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for trajectory tracking and control of unmanned vehicles in mining areas, characterized in that, include: Obtain the driving path data of the mining truck during manual driving; Based on the driving path data, determine the driving data of the first vehicle; The first vehicle driving data includes: preset slope, preset load, preset position, preset driving speed, preset acceleration, preset heading angle, preset front wheel angular velocity, and preset trajectory curvature; Collect second vehicle driving data when the mining truck is unmanned, the second vehicle driving data is real-time data generated by the mining truck during transportation; Based on the first vehicle driving data and the second vehicle driving data, deviation data is determined; the second vehicle driving data includes real-time collected gradient, load, driving speed, driving acceleration, heading angle, tracking, front wheel angular velocity, longitude, latitude, elevation, speed and trajectory curvature; Based on the deviation data, the speed and steering of the mining truck are controlled so that the mining truck travels along the travel path corresponding to the travel path data; wherein, the deviation data is the difference between the first vehicle travel data and the second vehicle travel data, and the deviation data includes: gradient difference, load difference, travel speed difference, travel acceleration difference, heading angle difference, tracking error, and front wheel angular velocity difference; The step of controlling the speed of the mining car based on the deviation data includes: The throttle opening or brake opening of the mine car is determined based on at least one of the gradient difference, the load difference, and the driving acceleration difference, as well as the driving speed difference. The speed of the mine car is controlled according to the throttle opening or the brake opening.

2. The method according to claim 1, characterized in that, Determining the throttle or brake opening of the mine car based on any one of the gradient difference, the load difference, and the acceleration difference, as well as the speed difference, includes: The fuzzy control rules are determined based on any one of the slope difference, the load difference, and the driving acceleration difference, as well as the driving speed difference. The opening fuzzy value is determined based on any one of the slope difference, the load difference, and the driving acceleration difference, as well as the driving speed difference and the fuzzy control rule. The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

3. The method according to claim 1, characterized in that, Determining the throttle or brake opening of the mine car based on the gradient difference, the load difference, and the speed difference includes: The first fuzzy control rule is determined based on the slope difference and the driving speed difference; The second fuzzy control rule is determined based on the load difference and the driving speed difference; Based on the first fuzzy control rule, determine the first opening fuzzy value; The second opening fuzzy value is determined according to the second fuzzy control rule; The aperture fuzzy value is determined using the following formula 1: U = k1U1 + k2U2 (Formula 1) Wherein, U is used to represent the opening fuzzy value, U1 is used to represent the first opening fuzzy value, k1 is used to represent the weight of U1, U2 is used to represent the second opening fuzzy value, and k2 is used to represent the weight of U2. The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

4. The method according to claim 1, characterized in that, Determining the throttle or brake opening of the mine car based on the gradient difference, the acceleration difference, and the speed difference includes: The first fuzzy control rule is determined based on the slope difference and the driving speed difference; The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed. Based on the first fuzzy control rule, determine the first opening fuzzy value; The third fuzzy value of the opening degree is determined according to the third fuzzy control rule. The aperture fuzzy value is determined using the following formula 2: U=k1U1+k3U3 Formula 2; Wherein, U is used to represent the opening degree fuzzy value, U1 is used to represent the first opening degree fuzzy value, k1 is used to represent the weight of U1, U3 is used to represent the third opening degree fuzzy value, and k3 is used to represent the weight of U3. The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

5. The method according to claim 1, characterized in that, Based on the load difference, the driving acceleration difference, and the driving speed difference, the opening fuzzy value is determined, including: The second fuzzy control rule is determined based on the load difference and the driving speed difference; The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed. The second opening fuzzy value is determined according to the second fuzzy control rule; The third fuzzy value of the opening degree is determined according to the third fuzzy control rule. The aperture fuzzy value is determined using the following formula 3: U=k2U2+k3U3 Formula 3; Wherein, U is used to represent the opening degree fuzzy value, U2 is used to represent the second opening degree fuzzy value, k2 is used to represent the weight of U2, U3 is used to represent the third opening degree fuzzy value, and k3 is used to represent the weight of U3. The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

6. The method according to claim 1, characterized in that, The throttle or brake opening of the mine car is determined based on the gradient difference, the load difference, the acceleration difference, and the speed difference, including: The first fuzzy control rule is determined based on the slope difference and the driving speed difference; The second fuzzy control rule is determined based on the load difference and the driving speed difference; The third fuzzy control rule is determined based on the difference in driving acceleration and the difference in driving speed. Based on the first fuzzy control rule, determine the first opening fuzzy value; The second opening fuzzy value is determined according to the second fuzzy control rule; The third fuzzy value of the opening degree is determined according to the third fuzzy control rule. The aperture fuzzy value is determined using the following formula 4: U=k1U1+k2U2+k3U3 Formula 4; Wherein, U is used to represent the opening fuzzy value, U1 is used to represent the first opening fuzzy value, k1 is used to represent the weight of U1, U2 is used to represent the second opening fuzzy value, k2 is used to represent the weight of U2, U3 is used to represent the third opening fuzzy value, and k3 is used to represent the weight of U3. The throttle opening or brake opening of the mine car is determined based on the fuzzy value of the opening.

7. The method according to claim 6, characterized in that, The step of controlling the steering of the mine car based on the deviation data includes: The front wheel steering angle is determined based on the heading angle difference, the tracking error, and the front wheel angular velocity difference. The steering of the mining car is determined based on the front wheel steering angle.

8. The method according to claim 7, characterized in that, The step of determining the front wheel steering angle based on the heading angle difference, the tracking error, and the front wheel angular velocity difference includes: The front wheel steering angle is calculated using the following formula 5: Where δ(t) characterizes the front wheel steering angle, ψ(t) characterizes the heading angle difference, e(t) characterizes the tracking error, v(t) characterizes the vehicle speed, m characterizes the mine car mass, C_y characterizes the roll stiffness, and ω meas ω is used to characterize the real-time observed value of the front wheel angular velocity. traj Used to characterize the preset value of the front wheel angular velocity, (ω meas -ω traj ) represents the difference in angular velocity between the front wheels, ψ ss δ is used to characterize the tire slip angle caused by lateral acceleration in wide-body mining vehicles. meas (i) represents the observed value of the front wheel steering angle in the i-th real-time observation, k yaw k steer k soft Both k and are coefficients, and both are constants.

9. The method according to claim 8, characterized in that, The k yaw The value range of k is 20-50; steer The value range of k is 0.01-0.05; soft =1; the value of k ranges from 2 to 3.

Images