A method and device for quickly correcting the course of a front wheel

Abstract

The present invention discloses a method and device for rapid heading correction of front wheels, the method comprising the following steps: calculating the yaw angle and adding a proportional coefficient to weight the yaw angle; obtaining a front wheel steering control formula, performing steering control on the front wheels, and performing PID adjustment on the yaw angle; obtaining a yaw coefficient, the yaw coefficient representing the degree of deviation of the current motion trajectory of the vehicle from the target path; dynamically adjusting PID parameters according to the yaw coefficient. The present invention has the advantages of achieving more precise steering control of the front wheels and more accurate tracking control results.

Description

Technical Field

[0001] The present invention relates to the field of agricultural robots and agricultural machinery path following, and more specifically to a method and device for rapid front wheel heading correction. Background Art

[0002] One of the most common algorithms is to control the vehicle to follow the path based on the single vehicle model and pure tracking algorithm. The principle diagram of pure tracking is as follows: Figure 1 and Figure 2 As shown, δ represents the front wheel turning angle, α represents the yaw angle, l d The core idea of ​​the algorithm is to adjust the angle of the front wheel in real time to ensure that the rear wheel fits on the track. However, the algorithm has serious defects in lateral control.

[0003] 1. Understeering problem. When the preview distance is short, the vehicle will have a significant delay in turning. The core is that the yaw angle is used as a calculation parameter, which is insufficient in steering control and has a certain delay.

[0004] 2. Slow convergence of lateral error. Improper setting of the forward sight distance will also cause the vehicle body to produce obvious oscillation (snaking) during movement. Traditional PID parameter adjustment has poor real-time performance, long convergence time, and poor tracking control result accuracy. The fitting of PID parameters is also very complicated. For agricultural machinery, the complex and changeable terrain conditions make it impossible to find a set of standard PID parameters to adapt to all terrains.

[0005] Chinese patent authorization announcement number CN105867377B discloses an automatic navigation control method for agricultural machinery, which includes the following steps: (1) GPS receiver measures the motion state data of agricultural machinery at the current moment in real time; (2) adopts the preview control method to compare and analyze the motion state data with the predefined path data to calculate the lateral deviation and heading deviation; (3) fuzzification is performed through the fuzzy control algorithm, and then the fuzzy quantity is introduced into the fuzzy controller to obtain the front wheel angle; a geometric model is established through the improved pure tracking algorithm to derive the front wheel angle; the front wheel angles obtained by the two algorithms are synthesized into one front wheel angle; (4) finally, the agricultural machinery is controlled in real time according to the front wheel angle to achieve path tracking. The invention combines two navigation methods to achieve automatic control, has good stability, strong anti-interference ability, and can improve the robustness of the system. The combination of the two methods complements each other's advantages, so that it can meet the operation needs of dry land and paddy fields. However, the invention does not dynamically adjust the parameters of the front wheel angle, so that in a complex agricultural machinery environment, the body vibrates and the tracking control result is not accurate enough. Summary of the invention

[0006] The technical problem to be solved by the present invention is that the front wheel heading control of agricultural machinery in the prior art has a large oscillation amplitude due to complex terrain conditions, and the deviation from the heading leads to inaccurate tracking control results.

[0007] The present invention solves the above technical problems through the following technical means: a method for rapid front wheel heading correction, the method comprising the following steps:

[0008] Step 1: Calculate the yaw angle and add a proportional coefficient to weight the yaw angle;

[0009] Step 2: Use the formula Steering control is performed on the front wheels, where α represents the weighted yaw angle, l d represents the distance from the rear wheel of the vehicle to the target path point, L represents the wheelbase between the front wheel and the rear wheel, and pid(α) represents the PID adjustment of the weighted yaw angle;

[0010] Step 3: Obtaining a yaw coefficient, wherein the yaw coefficient represents the degree of deviation of the vehicle's current motion trajectory from the target path;

[0011] Step 4: Dynamically adjust PID parameters according to the yaw coefficient.

[0012] The present invention obtains the yaw coefficient, which represents the degree of deviation of the vehicle's current motion trajectory from the target path. According to the yaw coefficient, the PID parameters are dynamically adjusted, and then the weighted yaw angle is PID adjusted in real time, thereby achieving more precise steering control of the front wheels.

[0013] Furthermore, the step 1 comprises:

[0014] The yaw angle is calculated by the formula α′=α1+α2, where α1 represents the angle between the vehicle body and the target point, and α2 represents the angle between the tangent line at the target point and the front wheel axle;

[0015] The weighted yaw angle is obtained by adding a proportional coefficient to the yaw angle through the formula α=β·α1+δ·α2, where β represents the coefficient of the angle between the vehicle body and the target point, δ represents the coefficient of the angle between the tangent at the target point and the front wheel axle, and β+δ=1.

[0016] Furthermore, the step three comprises:

[0017] By formula Get the yaw coefficient, where S yaw represents the yaw area, which is the area formed between the yaw trajectory of the vehicle body and the target path; S tolerate Represents the tolerance area, which is the area formed between the maximum allowable deviation path and the target path.

[0018] Furthermore, the step 4 includes:

[0019] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0020] pass Dynamically adjust the differential coefficient d of PID regulation,

[0021] Where k is a constant, And k>1.0.

[0022] Furthermore, the step 4 further includes:

[0023] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0024] pass Dynamically adjust the differential coefficient d of PID regulation,

[0025] in,

[0026] The present invention also provides a front wheel rapid course correction device, the device comprising:

[0027] The yaw angle acquisition module is used to calculate the yaw angle and add a proportional coefficient to weight the yaw angle;

[0028] Steering control module, used to utilize the formula Steering control is performed on the front wheels, where α represents the weighted yaw angle, l d represents the distance from the rear wheel of the vehicle to the target path point, L represents the wheelbase between the front wheel and the rear wheel, and pid(α) represents the PID adjustment of the weighted yaw angle;

[0029] A yaw coefficient acquisition module is used to acquire a yaw coefficient, wherein the yaw coefficient represents the degree of deviation of the vehicle's current motion trajectory from the target path;

[0030] The parameter adjustment module is used to dynamically adjust the PID parameters according to the yaw coefficient.

[0031] Furthermore, the yaw angle acquisition module is also used for:

[0032] The yaw angle is calculated by the formula α′=α1+α2, where α1 represents the angle between the vehicle body and the target point, and α2 represents the angle between the tangent line at the target point and the front wheel axle;

[0033] The weighted yaw angle is obtained by adding a proportional coefficient to the yaw angle through the formula α=β·α1+δ·α2, where β represents the coefficient of the angle between the vehicle body and the target point, δ represents the coefficient of the angle between the tangent at the target point and the front wheel axle, and β+δ=1.

[0034] Furthermore, the yaw coefficient acquisition module is also used for:

[0035] By formula Get the yaw coefficient, where S yaw represents the yaw area, which is the area formed between the yaw trajectory of the vehicle body and the target path; S tolerate Represents the tolerance area, which is the area formed between the maximum allowable deviation path and the target path.

[0036] Furthermore, the parameter adjustment module is also used for:

[0037] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0038] pass Dynamically adjust the differential coefficient d of PID regulation,

[0039] Where k is a constant, And k>1.0.

[0040] Furthermore, the parameter adjustment module is also used for:

[0041] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0042] pass Dynamically adjust the differential coefficient d of PID regulation,

[0043] in,

[0044] The advantages of the present invention are as follows: the present invention obtains the yaw coefficient, which represents the degree of deviation of the vehicle's current motion trajectory from the target path, dynamically adjusts the PID parameters according to the yaw coefficient, and then performs PID adjustment on the weighted yaw angle in real time, thereby achieving more precise steering control of the front wheels. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a schematic diagram of a pure tracking algorithm in the prior art in one state;

[0046] Figure 2 It is a schematic diagram of another state of the pure tracking algorithm in the prior art;

[0047] Figure 3 A schematic diagram of yaw supplementation principle in a method for rapid front wheel heading correction disclosed in an embodiment of the present invention;

[0048] Figure 4 A schematic diagram of the calculation principle of the yaw coefficient in a method for rapid front wheel heading correction disclosed in an embodiment of the present invention;

[0049] Figure 5 The present invention discloses an algorithm flow chart of a method for rapid front wheel heading correction according to an embodiment of the present invention. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0051] Example 1

[0052] like Figure 5 As shown, a method for rapid front wheel heading correction comprises the following steps:

[0053] Step 1: Calculate the yaw angle and add a proportional coefficient to weight the yaw angle; The motion following of agricultural machinery requires the vehicle to always stay on the predetermined track. However, due to the influence of vehicle speed and terrain, the vehicle heading always fluctuates. The faster the speed and the bumpier the ground, the faster the vehicle will deviate from the predetermined track, resulting in control shock. Figure 3 As shown in the figure, α1 represents the angle between the vehicle body and the target point, and α2 represents the angle between the tangent at the target point and the front wheel axle. The larger the yaw, the larger α1 and α2. The smaller the yaw, the smaller α1 and α2. α1 and α2 are weighted items to calculate the comprehensive yaw angle α.

[0054] Yaw angle calculation formula: α′=α1+α2

[0055] In order to facilitate control, a1, a2 are preceded by a proportional coefficient to control the weight of each weighted item. Therefore,

[0056] The weighted yaw angle is obtained by adding a proportional coefficient to the yaw angle through the formula α=β·α1+δ·α2, where β represents the coefficient of the angle between the vehicle body and the target point, δ represents the coefficient of the angle between the tangent at the target point and the front wheel axle, and β+δ=1.

[0057] Step 2: Use the formula Steering control is performed on the front wheels, where α represents the weighted yaw angle, l d represents the distance from the rear wheel of the vehicle to the target path point, L represents the wheelbase between the front wheel and the rear wheel, and pid(α) represents the PID adjustment of the weighted yaw angle;

[0058] In order to ensure the smoothness of control, α is adjusted by PID, but the disadvantages of PID are P (proportional), I (integral), and D (differential), and the dynamic adjustment is complex and there is no rule to follow. The present invention adjusts the P and D parameters. P and D can adjust the speed at which the unmanned vehicle converges to the center line. The larger the P and D, the faster the convergence speed. Too large will cause overshoot and safety problems. The yaw coefficient is calculated as the basis for adjustment.

[0059] Step 3: Obtaining a yaw coefficient, wherein the yaw coefficient represents the degree of deviation of the vehicle's current motion trajectory from the target path;

[0060] like Figure 4 As shown, the deviation threshold d1 is the maximum distance that deviates from the target path. The d1 value can be defined arbitrarily and represents the tolerance to errors.

[0061] Buffer boundary: The buffer boundary is formed by radiating outward from the path as the center and the yaw threshold d1 as the distance. The inside of the boundary is the tolerance zone, and the outside is the out-of-bounds area. For the tolerance zone and the out-of-bounds area, different strategies need to be set to adjust the P and D parameters.

[0062] Ideally, the vehicle body and the track are aligned, and the projection area of ​​the axle and the track tends to 0.0. When yaw occurs, such as in state 2, the axle and the path will produce a yaw area S1. The larger the yaw area, the more serious the position deviation or attitude deviation. For example, in state 3, it has completely deviated from the buffer boundary.

[0063] Yaw area in state 3: S = S2 + S3

[0064] State 1 is a critical state, and S4 is just equal to the maximum yaw threshold.

[0065] Through the above principle, we can know that the yaw coefficient Among them, S yaw represents the yaw area, which is the area formed between the yaw trajectory of the vehicle body and the target path; S tolerate Represents the tolerance area, which is the area formed between the maximum allowable deviation path and the target path.

[0066] Step 4: Dynamically adjust the PID parameters according to the yaw coefficient. There are two ways to adjust the PID parameters. The first one is:

[0067] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0068] pass Dynamically adjust the differential coefficient d of PID regulation,

[0069] Where k is a constant, And k>1.0.

[0070] The second one is:

[0071] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0072] pass Dynamically adjust the differential coefficient d of PID regulation,

[0073] in,

[0074] It should be noted that the initial values ​​of the proportional coefficient p and the differential coefficient d are obtained after multiple tests, and the initial proportional coefficient p and the differential coefficient d are preset at the beginning of PID adjustment.

[0075] Through the above technical scheme, the present invention obtains the yaw coefficient, which represents the degree of deviation of the vehicle's current motion trajectory from the target path. According to the yaw coefficient, the PID parameters are dynamically adjusted, and then the weighted yaw angle is PID adjusted in real time, thereby achieving more precise steering control of the front wheels.

[0076] Example 2

[0077] Based on Example 1, Example 2 of the present invention further provides a front wheel rapid heading correction device, the device comprising:

[0078] The yaw angle acquisition module is used to calculate the yaw angle and add a proportional coefficient to weight the yaw angle;

[0079] Steering control module, used to utilize the formula Steering control is performed on the front wheels, where α represents the weighted yaw angle, l d represents the distance from the rear wheel of the vehicle to the target path point, L represents the wheelbase between the front wheel and the rear wheel, and pid(α) represents the PID adjustment of the weighted yaw angle;

[0080] A yaw coefficient acquisition module is used to acquire a yaw coefficient, wherein the yaw coefficient represents the degree of deviation of the vehicle's current motion trajectory from the target path;

[0081] The parameter adjustment module is used to dynamically adjust the PID parameters according to the yaw coefficient.

[0082] Specifically, the yaw angle acquisition module is also used for:

[0083] The yaw angle is calculated by the formula α′=α1+α2, where α1 represents the angle between the vehicle body and the target point, and α2 represents the angle between the tangent line at the target point and the front wheel axle;

[0084] The weighted yaw angle is obtained by adding a proportional coefficient to the yaw angle through the formula α=β·α1+δ·α2, where β represents the coefficient of the angle between the vehicle body and the target point, δ represents the coefficient of the angle between the tangent at the target point and the front wheel axle, and β+δ=1.

[0085] Specifically, the yaw coefficient acquisition module is also used for:

[0086] By formula Get the yaw coefficient, where S yaw represents the yaw area, which is the area formed between the yaw trajectory of the vehicle body and the target path; S tolerate Represents the tolerance area, which is the area formed between the maximum allowable deviation path and the target path.

[0087] More specifically, the parameter adjustment module is also used for:

[0088] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0089] pass Dynamically adjust the differential coefficient d of PID regulation,

[0090] Where k is a constant, And k>1.0.

[0091] More specifically, the parameter adjustment module is also used for:

[0092] pass Dynamically adjust the proportional coefficient p of PID regulation,

[0093] pass Dynamically adjust the differential coefficient d of PID regulation,

[0094] in,

[0095] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for rapid front wheel heading correction, characterized in that: The method comprises the following steps: Step 1: Calculate the yaw angle and add a proportional coefficient to weight the yaw angle; Step 2: Use the formula Steering control is performed on the front wheels, where α represents the weighted yaw angle, l d represents the distance from the rear wheel of the vehicle to the target path point, L represents the wheelbase between the front wheel and the rear wheel, and pid(α) represents the PID adjustment of the weighted yaw angle; Step 3: Obtaining a yaw coefficient, wherein the yaw coefficient represents the degree of deviation of the vehicle's current motion trajectory from the target path; Step 4: Dynamically adjust PID parameters according to the yaw coefficient.

2. A method for rapid front wheel heading correction according to claim 1, characterized in that: The step one comprises: The yaw angle is calculated by the formula α′=α1+α2, where α1 represents the angle between the vehicle body and the target point, and α2 represents the angle between the tangent line at the target point and the front wheel axle; The weighted yaw angle is obtained by adding a proportional coefficient to the yaw angle through the formula α=β·α1+δ·α2, where β represents the coefficient of the angle between the vehicle body and the target point, δ represents the coefficient of the angle between the tangent at the target point and the front wheel axle, and β+δ=1.

3. A method for rapid front wheel heading correction according to claim 1, characterized in that: The step three comprises: By formula Get the yaw coefficient, where S yaw represents the yaw area, which is the area formed between the yaw trajectory of the vehicle body and the target path; S tolerate Represents the tolerance area, which is the area formed between the maximum allowable deviation path and the target path.

4. A method for rapid front wheel heading correction according to claim 3, characterized in that: The fourth step comprises: pass Dynamically adjust the proportional coefficient p of PID regulation, pass Dynamically adjust the differential coefficient d of PID regulation, Where k is a constant, And k>1.

0.

5. A method for rapid front wheel heading correction according to claim 3, characterized in that: The step 4 also includes: pass Dynamically adjust the proportional coefficient p of PID regulation, pass Dynamically adjust the differential coefficient d of PID regulation, in, 6. A front wheel rapid course correction device, characterized in that: The device comprises: The yaw angle acquisition module is used to calculate the yaw angle and add a proportional coefficient to weight the yaw angle; Steering control module, used to utilize the formula Steering control is performed on the front wheels, where α represents the weighted yaw angle, l d represents the distance from the rear wheel of the vehicle to the target path point, L represents the wheelbase between the front wheel and the rear wheel, and pid(α) represents the PID adjustment of the weighted yaw angle; A yaw coefficient acquisition module is used to acquire a yaw coefficient, wherein the yaw coefficient represents the degree of deviation of the vehicle's current motion trajectory from the target path; The parameter adjustment module is used to dynamically adjust the PID parameters according to the yaw coefficient.

7. A front wheel rapid course correction device according to claim 6, characterized in that: The yaw angle acquisition module is also used for: The yaw angle is calculated by the formula α′=α1+α2, where α1 represents the angle between the vehicle body and the target point, and α2 represents the angle between the tangent line at the target point and the front wheel axle; The weighted yaw angle is obtained by adding a proportional coefficient to the yaw angle through the formula α=β·α1+δ·α2, where β represents the coefficient of the angle between the vehicle body and the target point, δ represents the coefficient of the angle between the tangent at the target point and the front wheel axle, and β+δ=1.

8. The front wheel rapid course correction device according to claim 6, characterized in that: The yaw coefficient acquisition module is also used for: By formula Get the yaw coefficient, where S yaw represents the yaw area, which is the area formed between the yaw trajectory of the vehicle body and the target path; S tolerate Represents the tolerance area, which is the area formed between the maximum allowable deviation path and the target path.

9. The front wheel rapid course correction device according to claim 8, characterized in that: The parameter adjustment module is also used for: pass Dynamically adjust the proportional coefficient p of PID regulation, pass Dynamically adjust the differential coefficient d of PID regulation, Where k is a constant, And k>1.

0.

10. The front wheel rapid course correction device according to claim 8, characterized in that: The parameter adjustment module is also used for: pass Dynamically adjust the proportional coefficient p of PID regulation, pass Dynamically adjust the differential coefficient d of PID regulation, in,

Images