A method for automatic navigation control of agricultural machinery with differential drive steering

Abstract

The present invention discloses an automatic navigation control method for agricultural machinery with differential drive steering, which is characterized by comprising the following steps: S1: collecting the coordinates of point A (x A ,y A ) and the coordinates of point B (x B ,y B ), input the coordinates of points A and B into the path planning module based on coordinate changes to generate the target path; S2: Beidou positioning module obtains the real-time coordinates of the agricultural machinery (x s (k), y s (k)); The controller obtains the target path and the real-time coordinates of the agricultural machinery, and solves and generates the control quantity u r and u l , control quantity u r and u l The power is generated by the motor driver and applied to the power motors on the left and right sides; according to the control quantity u r and u l The power motors on the left and right sides output different torques T Mr and T MI , driving the electric agricultural machinery to move differentially, and realizing the tracking control of the target trajectory. The present invention establishes a set of path tracking control algorithm design for electric agricultural machinery based on wheel differential drive steering, realizing high-precision real-time tracking of the target path, and meeting the requirements of precision agricultural operations.

Description

Technical Field

[0001] The invention relates to the technical field of navigation control, in particular to an automatic navigation control method for a differential-drive steering agricultural machine. Background Art

[0002] The automatic navigation technology of agricultural machinery realizes the autonomous navigation and operation control of agricultural machinery through precise positioning and intelligent control. This technology can be applied to various types of agricultural machinery, such as tractors, harvesters, and seeders. Through automatic navigation and operation, it significantly improves the growth and harvesting efficiency of crops. For example, during the planting process, the automatic navigation of agricultural machinery can autonomously complete field operations within the plot, reducing the coverage area and staggered area of ​​non-operating areas, thereby improving planting efficiency.

[0003] Traditional agricultural production methods often rely on manual operation, which is not only inefficient but also labor-intensive. The introduction of automatic navigation and control technology for agricultural machinery has freed drivers from the monotonous and harsh working environment, transforming them from workers to managers, thereby reducing labor costs. At the same time, the application of automation technology has also reduced the tediousness and time cost of manual operation, further improving the economic benefits of agricultural production.

[0004] The automatic navigation technology of agricultural machinery can achieve centimeter-level navigation control, allowing agricultural machinery to accurately drive along the set path, effectively avoiding the phenomenon of missing or overlapping operating areas. This not only reduces the waste of arable land resources and fuel resources, but also improves the quality of operations. For example, in the prevention and control of pests and diseases, the automatic navigation of agricultural machinery can accurately locate the pesticide spraying location according to the distribution of pests and diseases, avoiding the waste of pesticides and pest resistance.

[0005] In summary, the significance of agricultural machinery automatic navigation and control technology is to improve agricultural production efficiency, reduce labor costs, improve operation accuracy and quality, promote sustainable agricultural development, and promote agricultural modernization. The application of this technology will bring more convenience and benefits to agricultural production and inject new vitality into the sustainable development of agriculture.

[0006] At present, most of the automatic navigation and control methods on the market are aimed at agricultural machinery with steering wheel front wheel deflection steering, while the navigation control of electric agricultural machinery with wheel differential drive steering is less complicated, and there is a lack of relevant automatic navigation and control methods. The patent of this invention aims at the navigation problem of electric agricultural machinery with wheel differential drive steering, and studies an automatic navigation and control method of electric agricultural machinery with wheel differential drive steering, so as to achieve efficient target path generation and high-precision path tracking, and meet the requirements of precision agricultural operations. Summary of the invention

[0007] In view of this, the present invention proposes an automatic navigation control method for agricultural machinery with differential drive steering, comprising the following steps:

[0008] S1: In the absolute coordinate system xOy, collect the coordinates of point A (x A ,y A ) and the coordinates of point B (x B ,y B ), input the coordinates of points A and B into the path planning module based on coordinate changes to generate the target path;

[0009] S2: Beidou positioning module obtains the real-time coordinates of agricultural machinery (x s (k), y s (k)); The controller obtains the target path and the real-time coordinates of the agricultural machinery (x s (k), y s (k)), and solve it to generate the control quantity u r and u l , control quantity u r and u l The power is generated by the motor driver and applied to the power motors on the left and right sides; according to the control quantity u r and u l The power motors on the left and right sides output different torques T Mr and T Ml , drive the electric agricultural machinery to move differentially and realize the tracking control of the target trajectory.

[0010] In a further technical solution, in step S1, the path planning based on coordinate changes specifically includes:

[0011] Take the set mark point as the coordinate origin O, the direction of the longitude line through this point as the x-axis, and the direction of the latitude line through this point as the y-axis, and establish an absolute coordinate system xOy; mark points A and B in the field, and the coordinates of point A are (x A ,y A ), the coordinates of point B are (x B ,y B ) ; establish a relative coordinate system x′Oy′ with A as the origin, AB as the x′ axis, and perpendicular to AB as the y′ axis, and perform path planning in the relative coordinate system x′Oy′. The horizontal coordinate x′ of the target path 0 (k) The calculation formula is:

[0012]

[0013] The horizontal coordinate y′ of the target path 0 (k) The calculation formula is:

[0014]

[0015] After path planning in the coordinate system x′Oy′, the coordinates need to be changed and converted to the absolute coordinate system xOy.0 (k) The expression is:

[0016] x 0 (k) = (x′ 0 (k)+x A )cosθ-(y′ 0 (k)+y A )sinθ

[0017] After transformation, the horizontal axis y 0 (k) The expression is:

[0018] y 0 (k) = (y′ 0 (k)+y A )cosθ+(x′ 0 (k)+x A )sinθ

[0019] The expression for sinθ is:

[0020]

[0021] The expression of cosθ is:

[0022]

[0023] The expression of |AB| is:

[0024]

[0025] Among them, k is the mark point number, j is the number of rows, n is the maximum number of rows in the job plan, |AB| is the absolute value of the distance between AB, and c n is the vertical distance between adjacent work plan rows, % is the remainder operation, if is the logic judgment operation, Floor is the rounding down operation, and θ is the angle between the relative coordinate system x′Oy′ and the absolute coordinate system xOy.

[0026] In a further technical solution, in step S2, the control amount algorithm design specifically includes:

[0027] Left power control amount u l The expression is:

[0028]

[0029]

[0030] Right power control amount u r The expression is:

[0031]

[0032] The expression of the deviation ds(k) between the actual position of the agricultural machinery and the target position is:

[0033]

[0034] The deviation dθ between the actual direction angle of the agricultural machinery and the target direction angle s (k) The expression is:

[0035]

[0036] Driving torque T of left and right wheels Mr and T MI The size is determined by the control quantity u r and u l The determining relationship is:

[0037]

[0038] Among them, k p1 is the position loop proportional control parameter, k d1 is the position loop differential control parameter, k I1 is the position loop integral control parameter, k p2 is the position loop proportional control parameter, k d2 is the position loop differential control parameter, k I2 is the position loop integral control parameter; x s (k) is the real-time horizontal coordinate of the agricultural machinery in the absolute coordinate system xOy, y s (k) is the real-time ordinate of the agricultural machinery in the absolute coordinate system xOy; T Mr is the driving torque of the right wheel, T Ml is the driving torque of the left wheel, K T It is the torque conversion coefficient of the motor driver driving the motor.

[0039] Compared with the prior art, the automatic navigation control method of the agricultural machinery with differential drive steering of the present invention has the following beneficial effects:

[0040] The present invention efficiently generates the target path of agricultural machinery operation based on the path planning of coordinate changes. The automatic navigation and control of electric agricultural machinery with wheel differential drive steering are accurately modeled, and a set of path tracking control algorithm design based on electric agricultural machinery with wheel differential drive steering is established, which can realize efficient target path generation and high-precision real-time tracking of target path, meeting the requirements of precision agricultural operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the accompanying drawings:

[0042] Figure 1 It is a schematic diagram of an automatic navigation control method of an agricultural machinery with differential drive steering according to the present invention;

[0043] Figure 2 A schematic diagram of the movement of a differential-driven steering agricultural machine according to the present invention;

[0044] Figure 3 Generate a target path diagram for the present invention;

[0045] Figure 4 A schematic diagram showing the comparison between the actual path and the target path of the present invention;

[0046] Figure 5 Schematic diagram of the tracking error curve of the present invention. DETAILED DESCRIPTION

[0047] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0048] like Figure 1 and Figure 2 As shown, the present invention preferably provides an automatic navigation control method for agricultural machinery with differential drive steering, comprising the following steps:

[0049] S1: In the absolute coordinate system xOy, collect the coordinates of point A (x A ,y A ) and the coordinates of point B (x B ,y B ), input the coordinates of points A and B into the path planning module based on coordinate changes to generate the target path;

[0050] S2: Beidou positioning module obtains the real-time coordinates of agricultural machinery (x s (k), y s (k)); The controller obtains the target path and the real-time coordinates of the agricultural machinery (x s (k), y s (k)), and solve it to generate the control quantity u r and u l , control quantity u r and u l The power is generated by the motor driver and applied to the power motors on the left and right sides; according to the control quantity u r and u l The power motors on the left and right sides output different torques T Mr and T Ml , drive the electric agricultural machinery to move differentially and realize the tracking control of the target trajectory.

[0051] like Figure 2 and Figure 3 As shown, in step S1, the path planning based on coordinate changes specifically includes:

[0052] Take the set mark point as the coordinate origin O, the direction of the longitude line through this point as the x-axis, and the direction of the latitude line through this point as the y-axis, and establish an absolute coordinate system xOy; mark points A and B in the field, and the coordinates of point A are (x A ,y A ), the coordinates of point B are (x B ,y B ) ; establish a relative coordinate system x′Oy′ with A as the origin, AB as the x′ axis, and perpendicular to AB as the y′ axis, and perform path planning in the relative coordinate system x′Oy′. The horizontal coordinate x′ of the target path 0 (k) The calculation formula is:

[0053]

[0054] The horizontal coordinate y′ of the target path 0 (k) The calculation formula is:

[0055]

[0056] After path planning in the coordinate system x′Oy′, the coordinates need to be changed and converted to the absolute coordinate system xOy. 0 (k) The expression is:

[0057] x 0 (k) = (x′ 0 (k)+x A )cosθ-(y′ 0 (k)+y A )sinθ

[0058] After transformation, the horizontal axis y 0 (k) The expression is:

[0059] y 0 (k) = (y′ 0 (k)+y A )cosθ+(x′ 0 (k)+x A )sinθ

[0060] The expression for sinθ is:

[0061]

[0062] The expression of cosθ is:

[0063]

[0064] The expression of |AB| is:

[0065]

[0066] Among them, k is the mark point number, j is the number of rows, n is the maximum number of rows in the job plan, |AB| is the absolute value of the distance between AB, and c n is the vertical distance value between adjacent operation planning rows, % is the remainder operation, if is the logic judgment operation, Floor is the rounding down operation, and θ is the angle between the relative coordinate system x′Oy′ and the absolute coordinate system xOy.

[0067] like Figure 4 and Figure 5 As shown, in step S2, the control amount algorithm design specifically includes:

[0068] Left power control amount u l The expression is:

[0069]

[0070]

[0071] Right power control amount u r The expression is:

[0072]

[0073]

[0074] The expression of the deviation ds(k) between the actual position of the agricultural machinery and the target position is:

[0075]

[0076] The deviation dθ between the actual direction angle of the agricultural machinery and the target direction angle s (k) The expression is:

[0077]

[0078] Driving torque T of left and right wheels Mr and T Ml The size is determined by the control quantity u r and u l The determining relationship is:

[0079]

[0080] Among them, k p1 is the position loop proportional control parameter, k d1 is the position loop differential control parameter, k I1 is the position loop integral control parameter, k p2is the position loop proportional control parameter, k d2 is the position loop differential control parameter, k I2 is the position loop integral control parameter; x s (k) is the real-time horizontal coordinate of the agricultural machinery in the absolute coordinate system xOy, y s (k) is the real-time ordinate of the agricultural machinery in the absolute coordinate system xOy; T Mr is the driving torque of the right wheel, T Ml is the driving torque of the left wheel, K T It is the torque conversion coefficient of the motor driver driving the motor.

[0081] like Figure 2 As shown, the dynamic equation of the electric agricultural machinery wheel is:

[0082]

[0083] Among them, T Mr is the driving torque of the right wheel, T Mr is the driving torque of the left wheel, T zr is the resistance torque of the right wheel, T zl is the resistance torque of the left wheel,

[0084] The angular acceleration of the right wheel, The angular acceleration of the left wheel, J is the moment of inertia of the wheel.

[0085]

[0086] Among them, ω r Angular velocity of the right wheel, ω l Angular velocity of the left wheel, V r is the right wheel edge speed, V l is the left wheel edge velocity,

[0087] R is the tire radius.

[0088] like Figure 2 As shown, the real-time value of the lateral movement speed of the agricultural machinery The expression is:

[0089]

[0090] in, The real-time value of the lateral movement speed of the agricultural machinery, θ s is the real-time value of the agricultural machinery direction angle, V r is the right wheel edge speed,

[0091] V l is the left wheel edge speed.

[0092] Real-time value of the longitudinal movement speed of agricultural machinery The expression is:

[0093]

[0094] in, The real-time value of the longitudinal movement speed of the agricultural machinery, θ s is the real-time value of the agricultural machinery direction angle, V r is the right wheel edge speed,

[0095] V l is the left wheel edge speed.

[0096] Real-time value of the angular velocity of agricultural machinery The expression is:

[0097]

[0098] Wherein, d is the wheelbase of the left and right wheels of the agricultural machinery.

[0099] Path planning effect Figure 3 As shown. Select points A (5.8537, -2.1085) and B (15.7257, 1.7286) for path planning. n Taking the value of 2m and n as 5, we can see that target paths of equal distance and width are generated.

[0100] Path tracking control performance Figure 4 As shown, the control algorithm of the present invention is used to drive the electric agricultural machine to track the target path, and the left and right wheelbase d of the driven electric agricultural machine is 1.2m. Figure 4 It is a comparison diagram between the actual path and the target path. The solid line is the target path, and the dotted line is the actual movement path of the electric agricultural machinery.

[0101] The tracking error curve is as follows Figure 5 As shown in the figure, the solid line is the tracking error curve of the horizontal axis, the dotted line is the tracking error curve of the vertical axis, the peaks and troughs are the tracking errors of the electric agricultural machinery when turning and turning, and the relatively straight short section is the tracking error of the agricultural machinery when operating between rows. The maximum tracking error of the agricultural machinery when operating between rows does not exceed 0.091m, which meets the requirements of agricultural machinery operating between rows.

[0102] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for automatic navigation control of agricultural machinery with differential drive steering, characterized in that: The following steps are involved: S1: In the absolute coordinate system xOy, collect the coordinates of point A (x A ,y A ) and the coordinates of point B (x B ,y B ), input the coordinates of points A and B into the path planning module based on coordinate changes to generate the target path; S2: Beidou positioning module obtains the real-time coordinates of agricultural machinery (x s (k), y s (k)); The controller obtains the target path and the real-time coordinates of the agricultural machinery (x s (k), y s (k)), and solve it to generate the control quantity u r and u l , control quantity u r and u l The power is generated by the motor driver and applied to the power motors on the left and right sides; according to the control quantity u r and u l The power motors on the left and right sides output different torques T Mr and T Ml , drive the electric agricultural machinery to move differentially and realize the tracking control of the target trajectory.

2. A method for automatic navigation control of agricultural machinery with differential drive steering according to claim 1, characterized in that: In step S1, the path planning based on coordinate changes specifically includes: Take the set mark point as the coordinate origin O, the direction of the longitude line through this point as the x-axis, and the direction of the latitude line through this point as the y-axis, and establish an absolute coordinate system xOy; mark points A and B in the field, and the coordinates of point A are (x A ,y A ), the coordinates of point B are (x B ,y B ) ; establish a relative coordinate system x′Oy′ with A as the origin, AB as the x′ axis, and perpendicular to AB as the y′ axis, and perform path planning in the relative coordinate system x′Oy′. The calculation formula for the horizontal coordinate x′0(k) of the target path is: The calculation formula of the horizontal coordinate y′0(k) of the target path is: After path planning in the coordinate system x′Oy′, the coordinates need to be changed and converted to the absolute coordinate system xOy. The expression of the horizontal coordinate x0(k) after conversion is: x0(k)=(x′0(k)+x A )cosθ-(y′0(k)+y A )sinθ The expression of the horizontal coordinate y0(k) after transformation is: y0(k)=(y′0(k)+y A )cosθ+(x′0(k)+x A )sinθ The expression for sinθ is: The expression of cosθ is: The expression of |AB| is: Among them, k is the mark point number, j is the number of rows, n is the maximum number of rows in the job plan, |AB| is the absolute value of the distance between AB, and c n is the vertical distance between adjacent work plan rows, % is the remainder operation, if is the logic judgment operation, Floor is the rounding down operation, and θ is the angle between the relative coordinate system x′Oy′ and the absolute coordinate system xOy.

3. A method for automatic navigation control of agricultural machinery with differential drive steering according to claim 2, characterized in that: In step S2, the control amount algorithm design specifically includes: Left power control amount u l The expression is: Right power control amount u r The expression is: The expression of the deviation ds(k) between the actual position of the agricultural machinery and the target position is: The deviation dθ between the actual direction angle of the agricultural machinery and the target direction angle s (k) The expression is: Driving torque T of left and right wheels Mr and T Ml The size is determined by the control quantity u r and u l The determining relationship is: Among them, k p1 is the position loop proportional control parameter, k d1 is the position loop differential control parameter, k I1 is the position loop integral control parameter, k p2 is the position loop proportional control parameter, k d2 is the position loop differential control parameter, k I2 is the position loop integral control parameter; x s (k) is the real-time horizontal coordinate of the agricultural machinery in the absolute coordinate system xOy, y s (k) is the real-time ordinate of the agricultural machinery in the absolute coordinate system xOy; T Mr is the driving torque of the right wheel, T Ml is the driving torque of the left wheel, K T It is the torque conversion coefficient of the motor driver driving the motor.

Images