Path tracking control method for four-wheel drive slip steering agricultural vehicle

Abstract

The invention relates to the technical field of agricultural vehicle unmanned driving, in particular to a path tracking control method for a four-wheel drive slip steering agricultural vehicle, and the method comprises the steps: building a kinetic model of a four-wheel independent drive slip steering agricultural vehicle; optimizing the dynamic model of the vehicle based on the tire model of the magic formula; simplifying the vehicle dynamics model, performing linearization processing, and establishing a linear error model of a model prediction control algorithm; establishing an objective function of a model predictive control algorithm; establishing constraint conditions of a model predictive control algorithm; according to the method, a vehicle path tracking controller is designed and optimized, the dynamic change and external disturbance of a control system can be well coped with through a real-time rolling optimization control strategy, and then it is guaranteed that the vehicle path tracking controller still has high control precision in hilly and mountainous areas.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned driving of agricultural vehicles, in particular to a path tracking control method of a four-wheel independent drive skid-steering agricultural vehicle based on model predictive control. Background Art

[0002] In the field of vehicle technology, wheeled skid-steer vehicles, with their unique advantages, have demonstrated widespread application value. Unlike traditional vehicles that steer by turning the front wheels, skid-steer vehicles steer by using the speed difference between the tires on either side to cause the vehicle to slide. This eliminates the need for front-wheel steering, saving space and increasing vehicle utilization. Skid-steer vehicles also enable a variety of motion modes, including zero-radius steering, and are widely used in transportation, industrial production, and military warfare.

[0003] Applying four-wheel independent drive skid steering technology to agricultural unmanned vehicles is expected to improve their operating performance in complex working conditions in hilly and mountainous areas.

[0004] Path tracking control is a key technology for unmanned agricultural vehicles, encompassing both longitudinal motion (controlling driving speed) and lateral motion (controlling steering angle). However, skid-steer vehicles face challenges such as complex steering dynamics and difficulty in coordinated control. Although relevant research has been conducted by scholars both domestically and internationally, such as Economou JT et al., who established and validated a full-vehicle skid-steer model, and Yang Yanjing, who analyzed parameter influences based on the model, research on path tracking control for wheeled skid-steer vehicles has been short-lived and yielded limited results. This research has primarily focused on the military and transportation sectors, with agricultural vehicle application research being even more scarce. Furthermore, agricultural machinery operates in an unstructured environment, subject to constraints such as physical constraints and actuator saturation. Model predictive control (MPC) can repeatedly obtain optimal control variables online and is capable of handling multi-constraint optimization problems, meeting constraints such as vehicle steering and wheel slip. As the computing power of on-board controllers increases, MPC-based agricultural machinery path tracking controllers have promising prospects. In light of this, the present invention proposes a path tracking control method for four-wheel independent drive skid-steer agricultural vehicles based on model predictive control to address the shortcomings of existing technologies. Summary of the Invention

[0005] In view of the problems in the prior art, the present invention provides a path tracking control method for a four-wheel drive skid steer agricultural vehicle.

[0006] The technical solution adopted by the present invention to solve its technical problem is:

[0007] A path tracking control method for a four-wheel drive skid-steer agricultural vehicle, comprising:

[0008] S1: Establish a dynamic model of a four-wheel independent drive skid-steer agricultural vehicle;

[0009] S2: Optimize the vehicle's dynamics model based on the tire model of the magic formula;

[0010] S3: Simplify and linearize the vehicle dynamics model to establish a linear error model for the model predictive control algorithm;

[0011] S4: Establish the objective function of the model predictive control algorithm;

[0012] S5: Establish constraints for the model predictive control algorithm;

[0013] S6: Design and optimize the vehicle's path-following controller.

[0014] Furthermore, the establishment of a dynamic model of a four-wheel independent drive skid steer agricultural vehicle includes:

[0015] A dynamic model of three degrees of freedom (lateral, longitudinal and yaw) for a wheeled skid-steer vehicle is established and simplified.

[0016] Convert the vehicle coordinate system to the world coordinate system, use the earth coordinate system to describe the vehicle trajectory; use the vehicle coordinate system to describe the vehicle's driving state;

[0017] Construct the three-degree-of-freedom vehicle dynamics equations.

[0018] Furthermore, the tire model based on the magic formula optimizes the vehicle's dynamic model including:

[0019] The tire lateral force F based on the magic formula tire model y The solution of

[0020] The tire longitudinal force F based on the magic formula tire model x The solution of

[0021] The solved lateral force F y and longitudinal force F x Substituting the original model, a more accurate dynamic model of the wheeled skid-steer agricultural vehicle based on the magic formula tire model is obtained.

[0022] Furthermore, the linear error model of the model predictive control algorithm is established including:

[0023] Establish a simplified three-degree-of-freedom dynamic model of a single-track wheeled skid-steer vehicle;

[0024] The predictive control algorithm of the linear model is designed and implemented according to the deviation between the state path and the actual state quantity of the system, and the linear time-varying equation is obtained.

[0025] Furthermore, the objective function of the model predictive control algorithm includes:

[0026]

[0027] Where N p is the prediction time domain; N c is the control time domain; ρ is the weight coefficient; ε is the relaxation factor.

[0028] Furthermore, in the constraint conditions for establishing the model predictive control algorithm, the control quantity limit constraint and the control increment constraint in the control process are considered, and the constraint expression of the control quantity is:

[0029] u min (t+k)≤u(t+k)≤u max (t+k), k=0, 1, ..., N c -1

[0030] The constraint expression for controlling the increment is:

[0031] Δu min (t+k)≤Δu(t+k)≤Δu max (t+k), k=0,1,…,N c -1

[0032] The corresponding matrix form after the constraint expression of the control increment is converted into:

[0033] U min ≤AΔU t +U t ≤U max

[0034] Where U min , U max are the minimum and maximum sets of the control quantity in the control time domain respectively.

[0035] Furthermore, the vehicle path tracking method includes:

[0036] First, generate a reference path;

[0037] Reference path discretization;

[0038] Select the first point on the path;

[0039] MPC controller to track points on the path;

[0040] Confirm whether it is the last point on the path;

[0041] If yes, the work ends. If not, the MPC controller will track the points on the path again and determine again whether it is the last point until the end.

[0042] Beneficial effects of the present invention:

[0043] This method, through a real-time rolling optimization control strategy, effectively handles dynamic changes in the control system and external disturbances, thereby ensuring that the vehicle's path-tracking controller maintains high control accuracy even in hilly and mountainous areas. The method also controls multiple inputs and outputs simultaneously and naturally incorporates input and output constraints to ensure the stability of the path-tracking controller and the safety of skid-steer agricultural vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Other features, objects and advantages of the present invention will become more apparent upon reading the detailed description of non-limiting embodiments with reference to the following drawings:

[0045] Figure 1 A schematic diagram of a path tracking control method for a four-wheel drive skid-steer agricultural vehicle according to the present invention;

[0046] Figure 2 A four-wheel drive skid steer agricultural vehicle dynamics model diagram of a path tracking control method for a four-wheel drive skid steer agricultural vehicle according to the present invention;

[0047] Figure 3 A specific path tracking flow chart of a path tracking control method for a four-wheel drive skid-steer agricultural vehicle according to the present invention;

[0048] Figure 4 This is a model predictive control principle diagram of a path tracking control method for a four-wheel drive skid steer agricultural vehicle according to the present invention. DETAILED DESCRIPTION

[0049] In order to make the technical means, creative features, objectives and effects achieved by the present invention easier to understand, the present invention is further described below in conjunction with specific implementation methods.

[0050] See also Figure 1-Figure 4 The present invention proposes a path tracking control method for a four-wheel independent drive skid steer agricultural vehicle based on model predictive control, comprising the following steps:

[0051] Step S1: establishing a dynamic model of a four-wheel independent drive skid-steer agricultural vehicle;

[0052] Step S2: Optimizing the vehicle's dynamics model based on the tire model of the magic formula;

[0053] Step S3: simplifying the vehicle dynamics model and performing linearization processing to establish a linear error model of the model predictive control algorithm;

[0054] Step S4: establishing the objective function of the model predictive control algorithm;

[0055] Step S5: Establishing constraints for the model predictive control algorithm;

[0056] Step S6: Design and optimize the path tracking controller of the vehicle. The path tracking control method of the agricultural vehicle is as follows: Figure 1 As shown:

[0057] This method, through a real-time rolling optimization control strategy, effectively handles dynamic changes in the control system and external disturbances, thereby ensuring that the vehicle's path-tracking controller maintains high control accuracy even in hilly and mountainous terrain. The method also controls multiple inputs and outputs simultaneously and naturally incorporates input and output constraints to ensure the stability of the path-tracking controller and the safety of skid-steer agricultural vehicles.

[0058] In step S1:

[0059] Step S11: Make basic assumptions and simplify the vehicle model:

[0060] A dynamic model with three degrees of freedom (lateral, longitudinal, and yaw) is established for a wheeled skid-steer vehicle. In order to simplify the vehicle model and reduce the amount of calculation, the following simplifications are made to the skid-steer vehicle model:

[0061] (1) Since skid-steer vehicles do not have a separate steering device, only the pure side-slip tire characteristics are considered, and the lateral and longitudinal coupling relationship of the tire forces is ignored.

[0062] (2) In the present invention, the main operating condition of the vehicle is low-speed driving, and the air resistance of the vehicle is small, so the air resistance of the vehicle is ignored.

[0063] (3) Since the entire chassis of a skid-steer vehicle remains in a horizontal state when turning, the compression and rebound of the suspension and shock absorbers are not involved. Therefore, the suspension and shock absorbers have little effect on the steering performance of the wheeled skid-steer vehicle, so the effect of the suspension and shock absorbers on the vehicle steering is ignored.

[0064] (4) Since the center of mass of a skid steer vehicle is generally located at the geometric center of the vehicle, it is assumed that the front and rear wheelbases of the vehicle are equal.

[0065] (5) Since the vehicle in the present invention hardly experiences special situations such as sudden acceleration, sudden deceleration, and sharp turns, the load transfer between the front and rear axles and the left and right sides of the vehicle body is ignored.

[0066] Based on the above assumptions, the four-wheel skid steering vehicle dynamics model is established as follows: Figure 2 As shown: (oxy is the vehicle coordinate system, OXY is the earth coordinate system)

[0067] The physical meanings of the symbols used in the vehicle model are shown in the following table:

[0068]

[0069] Step S12: Convert the vehicle coordinate system to the world coordinate system:

[0070] Depend on Figure 2 It can be seen that the origin o of the vehicle coordinate system oxy is located at the center of mass of the vehicle, and the angle between the x-axis of the vehicle coordinate system and the x-axis of the earth coordinate system OXY is called the yaw angle As a wheeled skid-steer vehicle moves, the vehicle coordinate system moves within the earth coordinate system OXY. In studies of wheeled skid-steer vehicle path tracking, the earth coordinate system is used to describe the vehicle trajectory, while the vehicle coordinate system is used to describe the vehicle's driving state. To facilitate observation of the relationship between the vehicle's actual motion path and the desired path, as well as the vehicle's motion status, the conversion relationship between the vehicle's motion state in the earth coordinate system and the vehicle coordinate system is shown in Equation (1-1):

[0071]

[0072] in,

[0073] Step S13, constructing a three-degree-of-freedom vehicle dynamics equation:

[0074] When a wheeled skid-steer vehicle turns, Newton's second law can be used to derive the force balance equations along the x-axis, y-axis, and around the z-axis. Combining the balance equations with the coordinate transformation equations yields equation (1-3):

[0075]

[0076] In step S2:

[0077] Since a wheeled skid-steer vehicle generates a large sliding friction force between the ground and the tires during actual driving, and this friction force will directly affect the vehicle's driving stability and safety, it is necessary to improve the wheeled skid-steer vehicle dynamics model by establishing a tire model that can accurately reflect the tire's sideways characteristics. The vehicle model optimization method adopted by the present invention is to use the magic formula tire model proposed by Professor Pacejka. The magic formula tire model is one of many tire empirical models. It is obtained by performing a large number of tests on the tires and fitting the obtained test data using a combination of trigonometric function formulas to finally obtain an empirical formula for tire force.

[0078] Step S21: Calculate the tire lateral force F based on the magic formula tire model y The solution:

[0079] According to the tire parameters selected by the present invention, by substituting them into the magic formula tire model, the tire lateral force F can be calculated when the tire side slip angle is small. y It can be expressed as a linear function of the sideslip angle α, as shown in formula (2-1):

[0080] F y =C c α (2-1)

[0081]

[0082] In formula (2-2), v yi and v xi is the lateral and longitudinal speed of the tire. The longitudinal and lateral speeds and yaw angular velocity at the center of mass of a wheeled skid steer vehicle can be used to calculate the longitudinal and lateral speeds of each tire. Their specific solution formulas are shown in Equation (2-3):

[0083]

[0084] Then, by combining equations (2-2) and (2-3), the sideslip angle α can be obtained, as shown in equation (2-4):

[0085]

[0086] Combining equations (2-1) and (2-4), we can obtain the cornering force F: y , as shown in formula (2-5):

[0087]

[0088] Step S22: The tire longitudinal force F is calculated based on the magic formula tire model. x The solution:

[0089] Similarly, according to the tire parameters selected by the present invention, by substituting them into the magic formula tire model, the vertical load F at different wheels can be calculated. z In the case of tire longitudinal force F x And the relationship between the slip rate s. And when the tire slip rate is small, the longitudinal force F on the tire x The relationship between and tire slip rate s can be approximately described by a linear function, and the calculation formula is shown in formula (2-6):

[0090] F x =C l s (2-6)

[0091] The slip rate s of the tire on the ground can be calculated by formula (2-7):

[0092]

[0093] When a skid-steer vehicle is turning, the inside wheels are in a sliding state and the outside wheels are in a spinning state.

[0094]

[0095] Then combine equations (2-6) and (2-8) to obtain the longitudinal force F x , as shown in formula (2-9):

[0096]

[0097] Step S23: The solved lateral force F y and longitudinal force F x Substitute into the original model:

[0098] Substituting equations (2-5) and (2-9) into equation (1-3), we can obtain a more accurate dynamic model of a wheeled skid-steer agricultural vehicle based on the magic formula tire model, as shown in equation (2-10):

[0099]

[0100] In step S3:

[0101] The vehicle dynamics model shown in Equation (2-10) is a complex nonlinear system that cannot be directly used for linear time-varying model predictive control. Therefore, this section will simplify, linearize, and discretize it, ultimately establishing the linear error model required for the model predictive control algorithm.

[0102] Step S31: Simplify the vehicle dynamics model:

[0103] First, a simplified three-degree-of-freedom dynamic model of a single-track wheeled skid-steer vehicle is established, considering only the forces acting on the left half of the vehicle. Next, only the lateral control of the skid-steer vehicle is considered, with the longitudinal velocity set to a constant value, v0. The dynamic model of the wheeled skid-steer vehicle is then as shown in Equation (3-1):

[0104]

[0105] Among them, M d is the yaw moment applied to the vehicle body.

[0106] Step S32: Establishment of linear error equation:

[0107] The simplified nonlinear dynamic model established by formula (3-1) is transformed into:

[0108]

[0109] Among them, the state quantity is selected as The control quantity is selected as u=M d , that is, only the yaw moment of the vehicle during path tracking is controlled, while the longitudinal speed of the vehicle remains unchanged.

[0110] Since the expected path cannot provide information about all state points, the predictive control algorithm of the linear model is designed and implemented based on the deviation between the state path and the actual state of the system. The resulting linear time-varying equation is:

[0111]

[0112] Where,

[0113] Then, the first-order difference quotient method is used to discretize Equation (3-3), and the discrete state space expression is obtained as follows:

[0114] ξ(k+1)=A(k)ξ(k)+B(k)u(k) (3-4)

[0115] In the formula, A(k)=I+TA(t); B(k)=TB(t).

[0116] In step S4:

[0117] To ensure that the unmanned agricultural vehicle can quickly and smoothly track the desired path, the objective function needs to include the optimization of the deviation of the system state and the control quantity. When designing a path tracking controller, the following objective function is generally used:

[0118]

[0119] Where Q and R are weight matrices.

[0120] The advantage of this objective function is that it is easy to convert into a standard quadratic programming form, but it has an obvious disadvantage: it cannot avoid the sudden change of the controlled quantity of the controlled system, thus affecting the continuity of the controlled quantity. Therefore, the present invention uses a soft constraint method to optimize it and ultimately adopts the following objective function:

[0121]

[0122] Where N p is the prediction time domain; N c is the control time domain; ρ is the weight coefficient; ε is the relaxation factor.

[0123] Compared to equation (4-1), equation (4-2) replaces the control quantity with a control increment and adds a relaxation factor. This not only directly limits the control increment but also prevents the situation where there is no feasible solution during execution.

[0124] In step S5:

[0125] The present invention mainly considers the control quantity limit constraint and the control increment constraint in the control process. The constraint expression of the control quantity is:

[0126] u min (t+k)≤u(t+k)≤u max (t+k), k=0, 1, ..., N c -1 (5-1)

[0127] The constraint expression for controlling the increment is:

[0128] Δu min (t+k)≤Δu(t+k)≤Δu max (t+k), k=0, 1, ..., N c -1 (5-2)

[0129] In the objective function, the variable to be solved is the control increment in the control time domain, and the constraints can only appear in the form of control increment or the multiplication of control increment and transformation matrix. Therefore, it is necessary to transform equation (5-2) to obtain the corresponding matrix form:

[0130] U min ≤AΔU t +U t ≤U max (5-3)

[0131] Where U min , U max are the minimum and maximum sets of the control quantity in the control time domain respectively.

[0132] In step S6:

[0133] Considering the objective function and constraints above, the skid-steering path tracking controller based on the dynamic model must solve the following optimization problem in each control cycle:

[0134]

[0135] Where y hc is a hard-constrained output, that is, the output quantity whose constraint range cannot be relaxed; y sc is a soft output, that is, the output of the constraint range can be dynamically adjusted through the relaxation factor; hc，min with y hc，max is the limit value of the hard constraint; ysc，min with y sc，max is the limit value of the soft constraint.

[0136] After solving Equation (6-1) in each control cycle, a series of control input increments and relaxation factors in the control time domain are obtained:

[0137]

[0138] The first element in the control sequence is used as the actual control input increment to act on the system, that is:

[0139]

[0140] After entering the next control cycle, the above process is repeated, and the tracking control of the desired path is realized in this cycle. The specific path tracking flow chart is as follows Figure 3 As shown:

[0141] The present invention provides a path tracking control method for a four-wheel drive skid-steer agricultural vehicle. Through a real-time rolling optimization control strategy, it can effectively cope with dynamic changes and external disturbances in the control system, thereby ensuring that the vehicle's path tracking controller still has high control accuracy in hilly and mountainous areas. The specific steps of rolling optimization can be seen from Figure 4 express:

[0142] During the control process of skid steering, there is always a desired reference trajectory, such as Figure 4 As shown in curve 1. Taking time k as the current moment, the MPC controller combines the current measurement value and the prediction model to predict the system in the future [k, k+N P ]The output of the system, such as Figure 4 As shown in curve 2. By solving the optimization problem that satisfies the objective function and various constraints, we can obtain [k, k+N c ]A series of control sequences, such as Figure 4 The first element of the control sequence is used as the actual control variable of the controlled object. At the next time point k+1, the above steps are repeated, and each constrained optimization problem is solved in a rolling manner, thus achieving continuous control of the controlled object.

[0143] The essence of rolling optimization is to determine the control action by finding the optimal solution to the objective function. This is done repeatedly and online, rather than just once and offline. Naturally, this significantly increases the computational complexity of the MPC controller, making it necessary to simplify the control vehicle model.

[0144] The method can also control multiple inputs and outputs simultaneously, and naturally includes input and output constraints to ensure the stability of the path tracking controller and the safety of the skid-steer agricultural vehicle. In order to reduce the computational complexity of the MPC controller, the present invention only considers adding constraints on the control quantity and the control increment to ensure the continuity of the control quantity and avoid sudden changes in the control increment. Of course, according to the actual control situation of the vehicle, vehicle dynamic constraints, such as vehicle center of mass deviation constraints and vehicle adhesion condition constraints, can also be added to further constrain the output within a safe and controllable range.

[0145] Setting multiple input and output constraints can more realistically simulate the actual working environment of agricultural vehicles, improve the control accuracy of path tracking, and ensure that the path tracking controller does not encounter solution anomalies.

[0146] Although this specification is described according to implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A path tracking control method for a four-wheel drive skid-steer agricultural vehicle, characterized in that: include: S1: Establish a dynamic model of a four-wheel independent drive skid-steer agricultural vehicle; S2: Optimize the vehicle's dynamics model based on the tire model of the magic formula; S3: Simplify and linearize the vehicle dynamics model to establish a linear error model for the model predictive control algorithm; S4: Establish the objective function of the model predictive control algorithm; S5: Establish constraints for the model predictive control algorithm; S6: Design and optimize the vehicle's path-following controller.

2. The path tracking control method for a four-wheel drive skid steer agricultural vehicle according to claim 1, characterized in that: The dynamic model of the four-wheel independent drive skid steer agricultural vehicle is established as follows: A dynamic model of three degrees of freedom (lateral, longitudinal and yaw) for a wheeled skid-steer vehicle is established and simplified. Convert the vehicle coordinate system to the world coordinate system, use the earth coordinate system to describe the vehicle trajectory; use the vehicle coordinate system to describe the vehicle's driving state; Construct the three-degree-of-freedom vehicle dynamics equations.

3. The path tracking control method for a four-wheel drive skid steer agricultural vehicle according to claim 1, characterized in that: The tire model based on the magic formula optimizes the vehicle dynamics model including: The tire lateral force F based on the magic formula tire model y The solution of The tire longitudinal force F based on the magic formula tire model x The solution of The solved lateral force F y and longitudinal force F x Substituting the original model, a more accurate dynamic model of the wheeled skid-steer agricultural vehicle based on the magic formula tire model is obtained.

4. The path tracking control method for a four-wheel drive skid steer agricultural vehicle according to claim 1, wherein: The linear error model of the model predictive control algorithm is established as follows: Establish a simplified three-degree-of-freedom dynamic model of a single-track wheeled skid-steer vehicle; The predictive control algorithm of the linear model is designed and implemented according to the deviation between the state path and the actual state quantity of the system, and the linear time-varying equation is obtained.

5. The path tracking control method for a four-wheel drive skid steer agricultural vehicle according to claim 1, wherein: The objective function of the model predictive control algorithm includes: Where N p is the prediction time domain; N c is the control time domain; ρ is the weight coefficient; ε is the relaxation factor.

6. The path tracking control method for a four-wheel drive skid steer agricultural vehicle according to claim 1, characterized in that: In the constraint conditions for establishing the model predictive control algorithm, the control quantity limit constraint and the control increment constraint in the control process are considered, and the constraint expression of the control quantity is: u min (t+k)≤u(t+k)≤u max (t+k),k=0,1,…,N c -1 The constraint expression for controlling the increment is: Δu min (t+k)≤Δu(t+k)≤Δu max (t+k),k=0,1,…,N c -1 The corresponding matrix form after the constraint expression of the control increment is converted into: IN min ≤AΔU t +U t ≤U max Where U min , U max are the minimum and maximum sets of the control quantity in the control time domain respectively.

7. The path tracking control method for a four-wheel drive skid steer agricultural vehicle according to claim 1, wherein: The vehicle path tracking method comprises: First, generate a reference path; Reference path discretization; Select the first point on the path; MPC controller to track points on the path; Confirm whether it is the last point on the path; If yes, the work ends. If not, the MPC controller will track the points on the path again and determine again whether it is the last point until the end.

Images