Unmanned mine car path tracking control method based on preview curvature

Abstract

The invention belongs to the technical field of automatic driving, and provides an unmanned mine car path tracking control method based on preview curvature. According to the tracking control method, the concept of the preview curvature is introduced, complex optimization calculation is reduced, the real-time performance of the control method is improved, the current state of the vehicle and the geometrical characteristics of the future path are considered based on the preview curvature, smoother and more accurate path tracking can be achieved, and the tracking control method is especially superior under the complex conditions of S-shaped bending, sharp turning and the like. Through PID control, different control states can be transited smoothly, frequent control input changes are reduced, and the stability and robustness of path tracking control are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and in particular to a path tracking control method for an unmanned mining vehicle based on preview curvature. Background Art

[0002] With the increase in mining depth and the expansion of complex terrain, traditional manually operated mining vehicles have gradually shown problems such as low efficiency, high risk and high cost. Therefore, intelligent and unmanned mining equipment has become an inevitable trend in the development of the industry.

[0003] As an important part of modern mining operations, the path tracking control technology of unmanned mining vehicles is mostly based on the tracking of predetermined trajectories. These technologies usually rely on high-precision positioning systems, such as GPS, inertial navigation systems (INS), etc., combined with environmental perception equipment, such as lidar, cameras, etc., to achieve autonomous navigation of mining vehicles in mining environments. However, due to the complexity and uncertainty of the mining environment, these traditional path tracking control algorithms often show insufficient stability and reliability in the face of dynamic obstacles, complex terrain changes and signal interference.

[0004] In order to improve the autonomous navigation capability and path tracking accuracy of unmanned mining vehicles in complex environments, it is urgent to develop a more intelligent and efficient path tracking control method to meet the following requirements: (1) Improve the accuracy and stability of path tracking: cope with complex terrain and dynamic obstacles to ensure that the mining vehicle can smoothly and accurately travel along the predetermined path. Enhance real-time response capabilities: adjust path planning and tracking strategies in real time to adapt to instantaneous changes in the environment. (2) Reduce dependence on external high-precision positioning systems: maintain stable autonomous navigation capabilities in the event of signal interference or loss. (3) Improve the robustness and fault tolerance of the algorithm: cope with various emergencies in the mining environment, such as equipment failure, sensor failure, etc.

[0005] At present, the following three control methods are mainly used in the prior art: (1) The path tracking method based on model predictive control predicts the motion trajectory of the unmanned mine car in the future by establishing a kinematic or dynamic model of the mine car. (2) The path tracking method based on pure tracking selects a preview point on the predetermined path and directs the vehicle to the preview point. By continuously adjusting the steering of the vehicle to move toward the preview point, path tracking is achieved. (3) The path tracking method based on sliding mode control designs a sliding surface and uses a sliding mode controller to make the vehicle's motion state approach and remain on the sliding surface, thereby achieving path tracking. The sliding surface is usually designed as a function of the path tracking error.

[0006] However, the control methods based on traditional theories have shortcomings when designing the lateral controller of unmanned mining vehicles, which are mainly reflected in the following aspects: (1) MPC relies on the kinematic or dynamic model of the vehicle to predict the future trajectory by solving the optimization problem. However, its computational complexity is high, the real-time requirements are strict, and it requires strong computing power. In addition, the MPC algorithm relies heavily on the accuracy of the model. When the mining vehicle is traveling in complex terrain or signal interference environment, the model error will lead to a decrease in path tracking accuracy. The computational complexity of MPC stems from the need to solve the optimization problem at every moment. At the same time, the uncertainty of the vehicle model and the complex terrain require high model accuracy, resulting in excessive real-time computing burden. (2) The pure tracking method adjusts the vehicle direction by selecting a preview point. It is simple and easy to implement, but the path tracking accuracy is low under high-speed driving and sharp turns, and it is easy to overshoot or path deviation. However, it only considers the position of the current preview point, and fails to fully consider the dynamic characteristics of the vehicle and future path changes, resulting in poor performance in complex road conditions and high dynamic environments. (3) Sliding mode control designs a sliding surface to make the vehicle state approach and remain on this surface, which has good robustness and anti-interference ability. However, sliding mode control is prone to chattering, which causes frequent changes in control input and has an adverse effect on the vehicle's mechanical system. The chattering phenomenon mainly stems from the discreteness and frequent switching of sliding mode control, which causes frequent fluctuations in control system input and affects stability and accuracy. Summary of the invention

[0007] In view of the deficiencies in the above-mentioned prior art, the present invention proposes a path tracking control method for an unmanned mining vehicle based on preview curvature. The tracking control method reduces complex optimization calculations and improves the real-time performance of the control method by introducing the concept of preview curvature. The geometric characteristics of the vehicle's current state and future path are considered based on the preview curvature, and a smoother and more accurate path tracking can be achieved, especially in high-speed and sharp turning situations. Through PID control, the present invention can smoothly transition between different control states, reduce frequent control input changes, and improve the stability and robustness of path tracking control.

[0008] The technical solution of the present invention is as follows:

[0009] A path tracking control method for an unmanned mining vehicle based on preview curvature comprises the following steps:

[0010] Step S1: estimating the curvature of the current position trajectory of the unmanned mining vehicle;

[0011] Step S2: According to the curvature of the current position trajectory, the predicted trajectory of the unmanned mining vehicle is obtained, a preview point is set on the target tracking line, and the preview deviation is calculated;

[0012] Step S3: Calculate the target curvature increment that makes the actual driving trajectory coincide with the target tracking line when the unmanned mining vehicle reaches the preview point according to the preview deviation and the preview point position;

[0013] Step S4: obtaining the current target curvature based on the target curvature increment, the preview time and the control period;

[0014] Step S5: Using the current target curvature, a PID control algorithm is used to determine the target turning angle of the front wheels of the unmanned mine car, so as to correct the driving trajectory of the unmanned mine car and realize the path tracking control of the unmanned mine car.

[0015] Preferably, the step S1 specifically includes:

[0016] Collect the current speed and heading angle of the unmanned mine car, calculate the rate of change of the heading angle of the unmanned mine car, and estimate the curvature c of the current position trajectory of the unmanned mine car. The expression is as follows:

[0017]

[0018] in, represents the rate of change of heading angle, and v represents the current speed of the vehicle.

[0019] Preferably, the step S2 specifically includes:

[0020] Step S2-1: Obtain the predicted trajectory of the unmanned mining vehicle according to the curvature of the current position trajectory;

[0021] Step S2-2: Setting preview time t p , based on the vehicle's current speed v and preview time t p and minimum preview distance d min , determine the preview distance d:

[0022] d=max(v*t p ,d min );

[0023] Step S2-3: According to the preview distance, determine the preview point on the target tracking line, and calculate the radial distance from the preview point to the predicted trajectory, which is the preview deviation e D .

[0024] Preferably, the calculation method of the preview deviation is as follows:

[0025]

[0026] Among them, (x A ,y A ) is the latitude and longitude of the current position of the unmanned mining vehicle, (x P ,y P ) is the longitude and latitude of the preview point, represents the heading angle, R represents the turning radius, OP represents the equivalent turning radius of the unmanned mining vehicle, and OA represents the distance from the preview point to the center of the equivalent turning circle of the unmanned mining vehicle.

[0027] Preferably, the expression of the target curvature increment Δc in step S3 is as follows:

[0028]

[0029] Among them, a 0 、a 1 、a 2 、a 3 、a 4 All represent fitting coefficients.

[0030] Preferably, the step S4 specifically includes:

[0031] Distribute the target curvature increment to the current control period t in proportion to the time c The current target curvature c is obtained t :

[0032]

[0033] Among them, t c represents the control period, c t-1 Indicates the target curvature of the previous control cycle.

[0034] Preferably, the method for determining the target turning angle of the front wheels of the unmanned mining vehicle is as follows:

[0035] (1) Calculate the feedforward front wheel steering angle δ FF , as follows:

[0036] δ FF =arctan(L×c t )

[0037] Among them, L represents the wheelbase of the unmanned mining vehicle;

[0038] (2) Calculate the front wheel angle angle of item P p and D front wheel angle angle D , as follows:

[0039] angle p =k p ×Δc

[0040] angle D =k d ×a

[0041] Among them, k p represents the proportionality coefficient, k d represents the differential coefficient, a represents the rate of change of curvature error;

[0042] (3) Calculate the front wheel angle angle I :

[0043] I=ρ×I last +Δc

[0044] angle I =k i ×I

[0045] Among them, I last represents the integral of the previous control cycle, ρ represents the forgetting factor, I represents the integral term of the curvature error, and k i Represents the integral coefficient.

[0046] Compared with the prior art, the present invention has the following beneficial effects:

[0047] 1. The unmanned mine vehicle path tracking control method based on preview curvature proposed in the present invention estimates the curvature of the unmanned mine vehicle's current position trajectory in real time according to the current speed and heading angle, and calculates the radial distance from the preview point on the target tracking line to the driving trajectory predicted by the curvature of the current position trajectory as the preview deviation. This method simplifies the complex optimization calculation process, reduces the calculation complexity, improves the real-time performance of the control method, and enables the unmanned mine vehicle to adapt to dynamic changes in real time in a complex mining environment.

[0048] 2. The unmanned mine vehicle path tracking control method based on preview curvature proposed in the present invention designs a mechanism to determine the target curvature increment according to the preview distance and the curvature change of the current position trajectory, so that the unmanned mine vehicle can reach the preview point according to the uniform change of the curvature of the current position trajectory during the preview time. This mechanism ensures the smoothness and accuracy of the unmanned mine vehicle path, especially in the case of sharp turns, to achieve higher path tracking accuracy and reduce path deviation.

[0049] 3. The unmanned mining vehicle path tracking control method based on preview curvature proposed in the present invention adopts a PID control algorithm of target curvature feedforward plus curvature error feedback to output the target turning angle of the front wheel in real time. It realizes smooth transition between different control states, reduces the fluctuation of control input, effectively reduces the dragon drawing phenomenon, and improves the stability and robustness of path tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. The features and advantages of the present invention can be more clearly understood by referring to the drawings. The drawings are schematic and should not be understood as limiting the present invention in any way. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0051] Figure 1 It is a flow chart of the unmanned mining vehicle path tracking control method based on preview curvature of the present invention.

[0052] Figure 2 It is a schematic diagram of the preview deviation prediction principle in the preview curvature-based unmanned mining vehicle path tracking control method of the present invention.

[0053] Figure 3 The present invention is a flow chart of calculation of preview deviation in the unmanned mining vehicle path tracking control method based on preview curvature.

[0054] Figure 4 It is a specific implementation flow chart of the unmanned mining vehicle path tracking control method based on preview curvature of the present invention. DETAILED DESCRIPTION

[0055] In order to more clearly understand the above-mentioned purpose, features and advantages of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0056] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited to the specific embodiments disclosed below.

[0057] like Figure 1 As shown, the present invention proposes a path tracking control method for an unmanned mining vehicle based on preview curvature.

[0058] The following steps are involved:

[0059] The first step is to estimate the curvature of the current position trajectory of the unmanned minecart: through the current speed and heading angle (the turning angle of the unmanned minecart relative to the reference direction) data of the unmanned minecart, calculate the rate of change of the heading angle of the unmanned minecart and estimate the curvature of the current position trajectory of the unmanned minecart. This curvature reflects the curvature of the driving trajectory, and the formula is as follows:

[0060]

[0061] Where:

[0062] in, represents the rate of change of heading angle, and v represents the current speed of the vehicle.

[0063] The second step is to calculate the preview deviation: Figure 2As shown in the figure, based on the curvature of the current position trajectory, the predicted trajectory of the unmanned mining vehicle is predicted when it moves forward according to the curvature of the current position trajectory. Select a preview point on the target tracking line (usually a point in front of the vehicle), and calculate the radial distance from the preview point to the predicted trajectory. This distance is the preview deviation.

[0064] Set preview time t p , based on the vehicle's current speed v and preview time t p and minimum preview distance d min (determined according to actual engineering needs, generally positioned at 6m in the mine car), determine the preview distance d:

[0065] d=max(v*t p ,d min );

[0066] First, determine the relationship between the first preview distance and the minimum preview distance according to the above formula. After determining the second preview distance, confirm the preview point on the target tracking line and calculate the longitude and latitude of the preview point. Figure 3 As shown, if the curvature of the current position trajectory is less than or equal to 0.002, it can be regarded as straight-line driving, and the preview deviation is directly calculated by the following formula:

[0067]

[0068] Among them, (x A ,y A ) is the latitude and longitude of the current position of the unmanned mining vehicle, (x P ,y P ) is the longitude and latitude of the preview point, represents the heading angle, R represents the turning radius, OP represents the equivalent turning radius of the unmanned mining vehicle, and OA represents the distance from the preview point to the center of the equivalent turning circle of the unmanned mining vehicle.

[0069] If the curvature of the current position trajectory is greater than 0.002, first calculate the equivalent turning radius OP of the unmanned mining vehicle, calculate the longitude and latitude of the equivalent turning circle center according to the heading angle and the equivalent turning radius OP, calculate the distance OA from the preview point to the equivalent turning circle center, and calculate the preview deviation e D =OP-OA.

[0070] The third step is to determine the target curvature increment: determine how the curvature of the unmanned mining vehicle should change from the current moment to the preview time (i.e. the time required for the vehicle to travel to the preview point) so that the predicted trajectory can just pass through the preview point. According to the preview deviation and the position of the preview point, calculate the target curvature increment so that when the unmanned mining vehicle reaches the preview point, the actual driving trajectory of the unmanned mining vehicle coincides with the target tracking line.

[0071] Substitute the curvature of the current position trajectory and the preview deviation into the fitting formula to calculate the required target curvature increment. The formula is as follows:

[0072]

[0073] Among them, a 0 、a 1 、a 2 、a 3 、a 4 All represent fitting coefficients.

[0074] Step 4: Update the current target curvature: distribute the target curvature increment calculated in step 4 to the current control cycle according to the time ratio.

[0075] Then, according to the following formula, we can get the current target curvature c: t :

[0076]

[0077] Among them, t c represents the control period, c t-1 Indicates the target curvature of the previous control cycle.

[0078] In this way, the curvature change of the unmanned mining vehicle is ensured to be gradual rather than sudden, so as to ensure smooth driving; the target curvature increment assigned to the current control cycle is added to the target curvature of the previous control cycle to obtain the current target curvature c t This process ensures that the steering behavior of the unmanned mining vehicle is continuous and can gradually approach the target tracking line.

[0079] Step 5: Feedforward control of target turning angle: Use the calculated current target curvature to directly derive the target turning angle of the front wheel. This part of the feedforward control mainly considers the ideal situation that the unmanned mining vehicle drives according to the current target curvature.

[0080] Target angle PID feedback control: At the same time, the error between the actual curvature of the unmanned mining vehicle and the current target curvature is calculated, and the error is feedback-adjusted through the PID controller. The correction output by the PID controller will adjust the target angle of the front wheel to correct the vehicle's driving trajectory so that it can better follow the target tracking line, such as Figure 4 shown.

[0081] (1) Calculate the feedforward front wheel steering angle δ FF , as follows:

[0082] δ FF =arctan(L×c t )

[0083] Among them, L represents the wheelbase of the unmanned mining vehicle;

[0084] (2) Calculate the front wheel angle angle of item Pp and D front wheel angle angle D , as follows:

[0085] angle p =k p ×Δc

[0086] angle D =k d ×a

[0087] Among them, k p represents the proportionality coefficient, k d represents the differential coefficient, a represents the rate of change of curvature error;

[0088] (3) Calculate the front wheel turning angle angle I :

[0089] I=ρ×I last +Δc

[0090] angle I =k i ×I

[0091] Among them, I last represents the integral of the previous control cycle, ρ represents the forgetting factor (adjusted according to actual engineering needs, generally 0.85), I represents the integral term of the curvature error, k i Represents the integral coefficient.

[0092] Example 1

[0093] (1) Based on the GPS longitude and latitude information of the unmanned mining vehicle chassis sensor, the heading angle is first calculated, and the curvature of the current position is calculated using the current speed of the unmanned mining vehicle. The curvature of the current position is then subjected to a second-order Kalman filter to output a stable and accurately calculated curvature information.

[0094] (2) First, determine the second preview distance based on the current vehicle speed and preview time, and determine the first preview distance based on the relationship between the minimum preview distance. Then, confirm the preview point on the target tracking line and calculate the longitude and latitude of the preview point. If the curvature of the current position is less than or equal to 0.002, it can be regarded as straight-line driving, and the preview deviation can be directly calculated based on the distance from the point to the straight line; if the curvature of the current position is greater than 0.002, first calculate the equivalent turning radius OP, calculate the longitude and latitude of the equivalent turning circle center based on the heading angle and the equivalent turning radius OP, calculate the distance OA from the preview point to the equivalent turning circle center, and finally calculate the preview deviation e D =OP-OA.

[0095] (3) The current position curvature and preview deviation are substituted into the fitting formula to calculate the required target curvature increment. Then, the steering angle is superimposed according to the real-time curvature feedback PID control to finally obtain the target front wheel steering angle.

[0096] In the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0097] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include that the first and second features are in direct contact, or may include that the first and second features are not in direct contact but are in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes that the first feature is directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0098] In the present invention, the terms "first", "second", "third", "fourth" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance. The term "plurality" refers to two or more, unless otherwise clearly defined.

[0099] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A path tracking control method for an unmanned mining vehicle based on preview curvature, characterized in that: The following steps are involved: Step S1: estimating the curvature of the current position trajectory of the unmanned mining vehicle; Step S2: According to the curvature of the current position trajectory, the predicted trajectory of the unmanned mining vehicle is obtained, a preview point is set on the target tracking line, and the preview deviation is calculated; Step S3: Calculate the target curvature increment that makes the actual driving trajectory coincide with the target tracking line when the unmanned mining vehicle reaches the preview point according to the preview deviation and the preview point position; Step S4: obtaining the current target curvature based on the target curvature increment, the preview time and the control period; Step S5: Using the current target curvature, a PID control algorithm is used to determine the target turning angle of the front wheels of the unmanned mine car, so as to correct the driving trajectory of the unmanned mine car and realize the path tracking control of the unmanned mine car.

2. The unmanned mining vehicle path tracking control method according to claim 1, characterized in that: The step S1 specifically includes: Collect the current speed and heading angle of the unmanned mine car, calculate the rate of change of the heading angle of the unmanned mine car, and estimate the curvature c of the current position trajectory of the unmanned mine car. The expression is as follows: in, represents the rate of change of heading angle, and v represents the current speed of the vehicle.

3. The unmanned mining vehicle path tracking control method according to claim 2, characterized in that: The step S2 specifically includes: Step S2-1: Obtain the predicted trajectory of the unmanned mining vehicle according to the curvature of the current position trajectory; Step S2-2: Setting preview time t p , based on the vehicle's current speed v and preview time t p and minimum preview distance d min , determine the preview distance d: d=max(v*t p ,d min ); Step S2-3: According to the preview distance, determine the preview point on the target tracking line, and calculate the radial distance from the preview point to the predicted trajectory, which is the preview deviation e D .

4. The unmanned mining vehicle path tracking control method according to claim 3, characterized in that: The calculation method of the preview deviation is as follows: Among them, (x A ,y A ) is the latitude and longitude of the current position of the unmanned mining vehicle, (x P ,y P ) is the longitude and latitude of the preview point, represents the heading angle, R represents the turning radius, OP represents the equivalent turning radius of the unmanned mining vehicle, and OA represents the distance from the preview point to the center of the equivalent turning circle of the unmanned mining vehicle.

5. The unmanned mining vehicle path tracking control method according to claim 4, characterized in that: The expression of the target curvature increment Δc in step S3 is as follows: Among them, a0, a1, a2, a3, and a4 all represent fitting coefficients.

6. The unmanned mining vehicle path tracking control method according to claim 5, characterized in that: The step S4 specifically includes: Distribute the target curvature increment to the current control period t in proportion to the time c The current target curvature c is obtained t : Among them, t c represents the control cycle, c t-1 Indicates the target curvature of the previous control cycle.

7. The unmanned mining vehicle path tracking control method according to claim 6, characterized in that: The method for determining the target turning angle of the front wheels of the unmanned mining vehicle is as follows: (1) Calculate the feedforward front wheel steering angle δ FF , as follows: δ FF =arctan(L×c t ) Among them, L represents the wheelbase of the unmanned mining vehicle; (2) Calculate the front wheel angle angle of item P p and D front wheel angle angle D , as follows: angle p =k p ×Δc angle D =k d ×a Among them, k p represents the proportionality coefficient, k d represents the differential coefficient, a represents the rate of change of curvature error; (3) Calculate the front wheel angle angle I : I=ρ×I last +Δc angle I =k i ×I Among them, I last represents the integral of the previous control cycle, ρ represents the forgetting factor, I represents the integral term of the curvature error, and k i Represents the integral coefficient.

Images