Agricultural machinery sideslip estimation and compensation method, device and electronic equipment

Abstract

The present invention relates to the field of agricultural machinery technology, and provides a method, device and electronic device for estimating and compensating sideslip of agricultural machinery. The method for estimating and compensating sideslip of agricultural machinery comprises: obtaining the track point of the agricultural machinery on the three-dimensional surface; obtaining the soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the track point; calculating the posture data of the agricultural machinery at the track point; calculating the sideslip estimation value of each wheel of the agricultural machinery at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machinery; and performing sideslip compensation on the agricultural machinery at the track point based on the movement direction of the agricultural machinery, the track point, the sideslip estimation value and the posture data. The embodiment of the present invention is used to solve the defect that the curved surface agricultural machinery navigation method in the prior art is not effective in path planning for agricultural machinery on a three-dimensional curved surface such as a sloping farmland.

Description

Technical Field

[0001] The present invention relates to the technical field of agricultural machinery, and in particular to a method, device and electronic equipment for estimating and compensating sideslip of agricultural machinery. Background Art

[0002] The surface of sloping farmland can be abstracted as a three-dimensional surface. At present, the navigation method of curved surface agricultural machinery still mainly adopts the operation path of plane planning, uses GNSS / INS to measure the posture of agricultural machinery, and the navigation system tracks the path after correction. Due to the undulating terrain, the projection of the plane-planned operation path on the curved surface is a curve. Under the influence of factors such as gravity and centrifugal force, the tires of the agricultural machinery slide and rub against the ground. The side slip causes the posture of the agricultural machinery to change sharply, exceeding the navigation control range, and causing the agricultural machinery to deviate seriously from the expected path. The existing agricultural machinery navigation control method based on side slip estimation can reduce the side slip, such as from 120cm to 70cm, but it still does not fundamentally solve the problem. The reason is that the side slip distance of the agricultural machinery kinematic model used is the significant side slip generated by the unit moving on the curved surface, which exceeds the control range of the path tracking algorithm. Therefore, the existing curved surface agricultural machinery navigation method is not effective in path planning for agricultural machinery on three-dimensional curved surfaces such as sloping farmland. Summary of the invention

[0003] The present invention provides a method, device and electronic device for estimating and compensating sideslip of agricultural machinery, which are used to solve the defect that the curved surface agricultural machinery navigation method in the prior art is not effective in path planning for agricultural machinery on three-dimensional curved surfaces such as sloping farmland.

[0004] The present invention provides a method for estimating and compensating sideslip of agricultural machinery, comprising:

[0005] Obtain the trajectory points of the agricultural machinery on the three-dimensional surface;

[0006] Acquire soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machine at the trajectory point;

[0007] Calculate the position and posture data of the agricultural machine at the trajectory point;

[0008] Calculating a side slip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machine;

[0009] The sideslip compensation is performed on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data.

[0010] According to a method for estimating and compensating sideslip of agricultural machinery provided by the present invention, the method of calculating the sideslip estimation value of each wheel of the agricultural machinery at the track point based on the soil characteristic parameters and the wheel parameters of each wheel of the agricultural machinery comprises:

[0011] Calculating a wheel slip rate of the agricultural machine based on the wheel parameters;

[0012] Calculating the amount of subsidence of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the force value of each wheel of the agricultural machine at the track point;

[0013] The side slip estimation value of each wheel of the agricultural machine at the track point is calculated based on the sinking amount, the wheel parameter and the wheel slip ratio.

[0014] According to a method for estimating and compensating sideslip of an agricultural machine provided by the present invention, the calculation of the posture data of the agricultural machine at the trajectory point includes:

[0015] The cross product of the first side wheel vector and the second side wheel vector of the agricultural machinery is calculated to obtain the position and posture data of the agricultural machinery at the trajectory point.

[0016] According to a method for estimating and compensating sideslip of agricultural machinery provided by the present invention, the method for performing sideslip compensation on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data comprises:

[0017] Determine the mass ratio of each wheel of the agricultural machine based on the posture data; the mass ratio is used to describe the ratio of the mass of each wheel of the agricultural machine to the total wheel mass of the agricultural machine;

[0018] Obtaining a sideslip compensation value of the agricultural machine based on the sideslip estimation value and a mass ratio of each wheel of the agricultural machine;

[0019] Determining a side slip compensation direction of the agricultural machine based on the movement direction of the agricultural machine and the trajectory point;

[0020] Side slip compensation is performed on the trajectory point based on the sideslip compensation direction and the sideslip compensation value.

[0021] According to a method for estimating and compensating sideslip of an agricultural machine provided by the present invention, the sideslip compensation value of the agricultural machine is obtained based on the sideslip estimation value and the mass ratio of each wheel of the agricultural machine through the following formula:

[0022] J all =λ 1 J y1 +λ 2 J y2 +λ 3 J y3 +λ 4 J y4 ;

[0023] Among them, J all represents the side slip compensation value of the agricultural machine; J y1 , Jy2 , J y3 , J y4 Respectively represent the side slip estimation value of each wheel of the agricultural machine; λ 1 , 2 , 3 , 4 Respectively represent the mass ratio of each wheel of the agricultural machine.

[0024] According to a method for estimating and compensating sideslip of an agricultural machine provided by the present invention, the method for determining the sideslip compensation direction of the agricultural machine based on the movement direction of the agricultural machine and the trajectory point comprises:

[0025] A plane is formed based on the movement direction of the agricultural machine and the trajectory points;

[0026] The normal vector direction of the plane is used as the sideslip compensation direction of the agricultural machinery.

[0027] The present invention also provides an agricultural machinery sideslip estimation and compensation device, comprising:

[0028] The first acquisition module is used to acquire the track points of the agricultural machinery on the three-dimensional surface;

[0029] A second acquisition module is used to acquire soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machine at the trajectory point;

[0030] A first calculation module, used for calculating the position and posture data of the agricultural machine at the trajectory point;

[0031] A sideslip estimation module, used for calculating the sideslip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machine;

[0032] A sideslip compensation module is used to perform sideslip compensation on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data.

[0033] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, any of the above-mentioned agricultural machinery sideslip estimation and compensation methods is implemented.

[0034] The present invention also provides a non-transitory computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the agricultural machinery sideslip estimation and compensation method as described in any one of the above is implemented.

[0035] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the computer program implements any of the above-mentioned agricultural machinery sideslip estimation and compensation methods.

[0036] The agricultural machinery sideslip estimation and compensation method, device and electronic device provided by the present invention obtain the soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the trajectory point; and calculate the sideslip estimation value of each wheel of the agricultural machinery at the trajectory point based on the soil characteristic parameters and the wheel parameters of the agricultural machinery; so that the present invention considers the influence of soil physical characteristics in the path planning of the three-dimensional curve of the agricultural machinery; calculates the posture data of the agricultural machinery at the trajectory point; and performs sideslip compensation on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data. Among them, the sideslip compensation direction of the agricultural machinery at the trajectory point can be determined based on the movement direction of the agricultural machinery and the trajectory point, and the sideslip estimation value and the posture data can be determined based on the sideslip estimation value and the posture data, so as to perform sideslip compensation on the agricultural machinery at the trajectory point based on the sideslip compensation direction and the sideslip compensation value. Compared with the traditional curved surface agricultural machinery navigation method, which can only correct the travel direction through the motor and the steering wheel when the agricultural machinery is operating, so as to compensate for the sideslip value that has been exited at the next moment, the present invention determines the sideslip compensation direction and the sideslip compensation value based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data before the sideslip occurs, and then compensates for the agricultural machinery at the trajectory point. The present invention avoids the impending sideslip of the agricultural machinery at the path planning level, and improves the effect of path planning for agricultural machinery on three-dimensional curved surfaces such as sloping farmland. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0038] Figure 1 It is one of the flow charts of the agricultural machinery sideslip estimation and compensation method provided by the present invention;

[0039] Figure 2 is a schematic diagram of a first side wheel vector and a second side wheel vector of an agricultural machine provided by the present invention;

[0040] Figure 3 This is the second flow chart of the agricultural machinery sideslip estimation and compensation method provided by the present invention;

[0041] Figure 4 It is a schematic diagram of the force analysis of the agricultural machinery wheel provided by the present invention;

[0042] Figure 5is a schematic diagram of a side slip angle β between the wheel direction of the agricultural machine provided by the present invention and the moving direction of the agricultural machine;

[0043] Figure 6 This is the third flow chart of the agricultural machinery sideslip estimation and compensation method provided by the present invention;

[0044] Figure 7 It is a structural schematic diagram of the agricultural machinery sideslip estimation and compensation device provided by the present invention;

[0045] Figure 8 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0046] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings 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.

[0047] Combine the following Figure 1-Figure 6 The agricultural machinery sideslip estimation and compensation method of the present invention is described.

[0048] Please refer to Figure 1 The agricultural machinery sideslip estimation and compensation method according to an embodiment of the present invention comprises:

[0049] Step 100, obtaining the track points of the agricultural machinery on the three-dimensional surface;

[0050] The agricultural machinery of the present invention refers to various mechanical equipment used in agricultural production. For example, agricultural machinery may include large / small tractors, land leveling machinery, plows, tillers, micro-tillage machines, rice transplanters, seeders, threshers, pumps, combine harvesters, roller blinds, thermal insulation felts and other equipment. The three-dimensional ground surface refers to a three-dimensional ground surface that is uneven, with a certain inclination and undulations, such as the surface of a sloping farmland.

[0051] When the present invention performs path planning for agricultural machinery on a three-dimensional surface, the working plot (such as sloping farmland) where the agricultural machinery is located is first mapped to obtain the boundary of the working plot where the agricultural machinery is located and the model of the three-dimensional surface. Based on the boundary of the working plot where the agricultural machinery is located and the model of the three-dimensional surface, three-dimensional path planning is performed to obtain the trajectory points of the agricultural machinery on the three-dimensional surface.

[0052] Step 200, obtaining soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machine at the trajectory point;

[0053] Specifically, the electronic device obtains the soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the trajectory point. It should be noted that in addition to the kinematic influencing factors such as the wheel speed and angular velocity of the wheels in the agricultural machinery path planning, the influence of the soil characteristic parameters of the working plot where the agricultural machinery is located on the agricultural machinery path is also needed. For example, the soil characteristic parameters obtained by the embodiment of the present invention may include soil cohesive deformation modulus, friction deformation modulus, settlement index, shear deformation modulus, soil cohesion, soil internal friction angle, etc. The above-mentioned soil characteristic parameters that describe the soil quality, friction and other characteristics of the soil will affect the sideslip of the agricultural machinery on the three-dimensional surface.

[0054] Step 300: Calculate the position and posture data of the agricultural machine at the trajectory point.

[0055] The electronic device calculates the posture data of the agricultural machine at the track point. By calculating the posture data of the agricultural machine at the track point, it is helpful to determine the overall sideslip value of the agricultural machine.

[0056] Specifically, in the curved state of the three-dimensional surface, the posture of the agricultural machinery fluctuates. At this time, take the grid of each tire surrounded by the digital surface model, obtain the spatial information (X, Y, Z) corresponding to the grid, and obtain the position data of the agricultural machinery at the trajectory point through the cross product of the first side wheel vector and the second side wheel vector of the agricultural machinery.

[0057] For details, please refer to Figure 2 , the first side wheel vector can be the front left wheel-rear left wheel vector V1 of the agricultural machinery, and the second side wheel vector can be the front wheel (i.e., the front left wheel and the front and rear wheels) vector V2 of the agricultural machinery. By taking the cross product of the two vectors, the Euler angle (i.e., posture data) of the vehicle can be obtained. For details, see calculation formula (1)-formula (3).

[0058]

[0059]

[0060] The normal vector representing the agricultural machinery posture data is is the cross product of the two:

[0061]

[0062] It should be noted that the second side wheel vector may also be the rear wheel vector of the agricultural machinery, that is, the vector between the rear left wheel and the rear right wheel of the agricultural machinery.

[0063] Step 400: Calculate the side slip estimation value of each wheel of the agricultural machine at the trajectory point based on the soil characteristic parameters and the wheel parameters of the agricultural machine.

[0064] The electronic device calculates the side slip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machine obtained in step 300.

[0065] In one embodiment, please refer to Figure 3 Step 400, calculating the side slip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the wheel parameters of each wheel of the agricultural machine, comprises:

[0066] Step 410: Calculate the wheel slip rate of the agricultural machine based on the wheel parameters.

[0067] The electronic device calculates the wheel slip rate of the agricultural machine based on the wheel parameters. In the embodiment of the present application, a slip function is generally defined to evaluate the wheel slip rate of the driving wheel of the agricultural machine. In the embodiment of the present invention, it is assumed that the agricultural machine is 4-wheel driven during operation. The wheel slip rate i is defined as follows with reference to formula (4):

[0068]

[0069] Where v is the forward speed of the center of the agricultural machinery wheel, ω is the angular velocity of the agricultural machinery wheel, and r is the radius of the agricultural machinery wheel. In other words, the mileage of the agricultural machinery tire edge rotation is not equal to the actual mileage of the agricultural machinery, and its driving efficiency can usually only reach 80%-90%. In the present invention, the wheel slip rate is 10%-20%.

[0070] Step 420: Calculate the amount of subsidence of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the force values ​​of each wheel of the agricultural machine at the track point.

[0071] The electronic device calculates the amount of sinking of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the force value of each wheel of the agricultural machine at the track point.

[0072] Specifically, the embodiment of the present invention may use the stress-strain relationship formula of the soil pressure-bearing characteristics as a basic pressure-bearing model to calculate the amount of sinking of each wheel of the agricultural machinery at the track point.

[0073] The original formula of the stress-strain relationship formula of soil bearing characteristics is shown in formula (5):

[0074]

[0075] Where p is the pressure or normal stress, b is the wheel width of the agricultural machine, k c represents the soil cohesive deformation modulus, represents the friction deformation modulus, and n represents the sinking index. c (soil cohesive deformation modulus), (friction deformation modulus), n (sinking index) are experimentally determined parameters, and z represents the sinking amount of the agricultural machinery wheel. c (soil cohesive deformation modulus), (friction deformation modulus) and n (sinking index) are both related to the inherent properties of soil and can be obtained through soil sampling, experiments or relevant literature search.

[0076] In summary, when the mass and posture of the agricultural machinery are determined, the amount of sinking z of the agricultural machinery wheel at the point can be determined by the force value p of each wheel of the agricultural machinery at the point of the trajectory, as shown in formula (6).

[0077] That is: z = f(p); Formula (6)

[0078] It should be noted that the force value p of each wheel at the track point is equal to mg, that is, the force value of each wheel at the track point is positively correlated with the mass of the wheel; the posture is to determine the different slips of each wheel, because the force of each tire is inconsistent on the three-dimensional surface, and the force value p of each wheel of the agricultural machinery at the track point can be determined only by knowing the posture information. The embodiment of the present invention assumes that the posture of the agricultural machinery body at this point is determined when the wheel size and soil characteristic parameters are known, so it is assumed that the sinking amount of each wheel of the agricultural machinery at the track point is obtained by the force value p of each wheel of the agricultural machinery at the track point in the known formula (6).

[0079] Step 430: Calculate the estimated side slip value of each wheel of the agricultural machine at the trajectory point based on the sinking amount, the wheel parameters and the wheel slip rate.

[0080] The electronic device calculates the estimated side slip value of each wheel of the agricultural machinery at the trajectory point based on the settlement amount, the wheel parameters and the wheel slip rate. Specifically, the present invention estimates the shear displacement of the agricultural machinery wheel at this point through a soil shear model, and the shear displacement is related to the estimated side slip value of each wheel of the agricultural machinery. Since the maximum traction that can be generated by agricultural machinery when traveling on soft ground is limited by the tangential shear strength of the soil, the shear properties of the soil will affect the passability of the agricultural machinery in the soil. The direction of the settlement amount passes through the center of the wheel, and the direction of the shear amount is tangent to the outer circle of the wheel. It can be said that the estimated side slip value is a part of the shear displacement.

[0081] Please refer to Figure 4 , Figure 4 This is the force analysis diagram of the agricultural machinery wheel. Figure 4 The entry angle is θ 1 , the departure angle is θ 2 , p is stress, z 1 is the amount of settlement, and s is the shear stress (including direction).

[0082] In this embodiment, the entry angle θ is determined by geometric relationship 1 :

[0083] θ 1 =arccos[(rz 1 ) / r]; Formula (7)

[0084] The embodiment of the present invention assumes that the rebound settlement is small and insufficient to affect the force analysis of the agricultural machinery, so θ2=0 is set. The shear formula of the soil shear model is:

[0085]

[0086] Among them, S max is the maximum shear stress, S is the shear stress, j is the shear displacement we are concerned about, and K is the shear deformation modulus (related to soil texture and can be measured experimentally). The maximum shear stress can be given by the Mohr–Coulomb formula:

[0087] s max =c+ptanφ; Formula (9)

[0088] Where c is the soil cohesion and φ is the soil internal friction angle. Both parameters can be obtained through experiments. Since p can be obtained by the weight of the agricultural machinery on the tire, S max It is also known. We can know the relationship between the shear stress S and the shear displacement j of the agricultural machinery wheel at this moment through the shear formula, which can be abbreviated as:

[0089] S = f(j); Formula (10)

[0090] The shear stress S of the agricultural machinery wheel at the point can be determined through the shear displacement j of each wheel of the agricultural machinery at the point of the trajectory.

[0091] When the agricultural machine of the present invention is used for cultivation on a sloping field, the wheel direction of the agricultural machine is not parallel to the movement direction of the agricultural machine, but a side slip angle is generated, which we set as β. Please refer to Figure 5 As shown:

[0092] Figure 5 v x represents the wheel direction of the agricultural machinery, v represents the movement direction of the agricultural machinery, then v y represents the side slip displacement direction of the agricultural machinery wheel, then the estimated side slip value of the agricultural machinery wheel is:

[0093] J y =tanβ×j;

[0094] Expressed in terms of integration, we have:

[0095]

[0096] Through formula (4), formula (5), formula (6), formula (7) and formula (11), that is, based on the settlement amount, the wheel parameters and the wheel slip rate, the side slip estimation value of each wheel of the agricultural machinery at the trajectory point can be calculated.

[0097] Step 500: Perform side slip compensation on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the side slip estimation value and the posture data.

[0098] The electronic device performs sideslip compensation on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data.

[0099] Specifically, the electronic device can determine the sideslip direction of the agricultural machine at the trajectory point based on the movement direction of the agricultural machine and the trajectory point, and the electronic device can determine the sideslip compensation value of the agricultural machine at the trajectory point based on the sideslip estimation value and the posture data.

[0100] In one embodiment, please refer to Figure 6 Step 500, performing sideslip compensation on the agricultural machine at the track point based on the movement direction of the agricultural machine, the track point, the sideslip estimation value and the posture data, comprises:

[0101] Step 510: Determine the mass ratio of each wheel of the agricultural machinery based on the posture data. The mass ratio is used to describe the ratio of the mass of each wheel of the agricultural machinery to the total wheel mass of the agricultural machinery; the mass ratio is used to describe the ratio of the mass of each wheel of the agricultural machinery to the total wheel mass of the agricultural machinery.

[0102] Considering that the pressure on the four wheels of the agricultural machinery is uneven, we can set the mass ratio of the four wheels to be λ 1 , 2 , 3 and λ 4 , then: 1 +λ 2 +λ 3 +λ 4 =100%; and 1 , 2 , 3 , 4 It is uniquely determined by the Euler angle (posture data) of the agricultural machinery at this moment.

[0103] The following is a detailed description of how to solve the mass distribution process using Euler angles: Consider the Euler angles of the tractor in the x, y, and z directions:

[0104] The rotation matrices in the three directions can be given by the following formula:

[0105]

[0106]

[0107]

[0108] Assume that the force in the x, y, and z directions at any point P of the agricultural machinery tire in the initial horizontal state is F x ,F y ,F z (can be obtained by calibrating the four wheels in a horizontal state), without considering the change in kinetic energy, the corresponding Euler angle is The force F' under the posture of θ,ψ x ,F` y ,F` z As shown in formula (15):

[0109]

[0110] Then the mass ratio λ of each wheel of the agricultural machinery i It can be uniquely determined by the Euler angle and the initial horizontal force, as shown in formula (16):

[0111]

[0112] Among them, each of the formula (16) It can be given by formula (15). Thus, by calculating formula (12)-formula (16), the mass proportion of the four wheels of the agricultural machinery in the embodiment of the present invention can be calculated as λ 1 , 2 , 3 and λ 4 .

[0113] Step 520: Obtain a sideslip compensation value of the agricultural machine based on the sideslip estimation value and the mass ratio of each wheel of the agricultural machine.

[0114] The electronic device obtains the sideslip compensation value of the agricultural machine based on the sideslip estimation value and the mass ratio of each wheel of the agricultural machine. In one embodiment, specifically, obtaining the sideslip compensation value of the agricultural machine based on the sideslip estimation value and the mass ratio of each wheel of the agricultural machine is implemented by the following formula (17):

[0115] J all =λ 1 J y1 +λ 2 J y2 +λ 3 Jy3 +λ 4 J y4 ;Formula (17)

[0116] Among them, J all represents the side slip compensation value of the agricultural machine; J y1 , J y2 , J y3 , J y4 Respectively represent the side slip estimation value of each wheel of the agricultural machine; λ 1 , 2 , 3 , 4 It should be noted that the side slip estimation value of each wheel of the agricultural machine can be calculated by the calculation method of step 400.

[0117] Step 530: Determine the sideslip compensation direction of the agricultural machinery based on the movement direction of the agricultural machinery and the trajectory point.

[0118] The electronic device determines the side slip compensation direction of the agricultural machine based on the movement direction of the agricultural machine and the trajectory point. Specifically, step 530, determining the side slip compensation direction of the agricultural machine based on the movement direction of the agricultural machine and the trajectory point, includes:

[0119] Step 531, forming a plane based on the movement direction of the agricultural machine and the trajectory points;

[0120] The electronic device forms a plane based on the movement direction of the agricultural machine and the track points. The movement direction of the agricultural machine can be determined based on the axle direction between the front and rear wheels of the agricultural machine, and the track points of the agricultural machine at this time serve as the center of mass of the agricultural machine and form a plane with the movement direction of the agricultural machine.

[0121] Step 532: Use the normal vector direction of the plane as the sideslip compensation direction of the agricultural machine.

[0122] The electronic device then uses the normal vector direction of the plane obtained in step 531 as the sideslip compensation direction of the agricultural machine.

[0123] Step 540: Perform sideslip compensation on the trajectory point based on the sideslip compensation direction and the sideslip compensation value.

[0124] The electronic device performs sideslip compensation on the trajectory point based on the sideslip compensation value of the agricultural machinery obtained in step 520 and the sideslip compensation direction obtained in step 530. Thus, the embodiment of the present invention can avoid the impending sideslip of the agricultural machinery at the path planning level before the sideslip occurs, thereby improving the accuracy and safety of three-dimensional path planning of the agricultural machinery on a three-dimensional surface or a three-dimensional curved surface.

[0125] The following describes the agricultural machinery sideslip estimation and compensation method according to an embodiment of the present invention through a typical embodiment.

[0126] Step 1: The electronic equipment maps the working plot (such as sloping farmland) where the agricultural machinery is located, and obtains the boundary of the working plot where the agricultural machinery is located and a three-dimensional surface model.

[0127] Step 2: The electronic device performs three-dimensional path planning based on the boundary of the working plot where the agricultural machinery is located and the three-dimensional surface model to obtain the trajectory points of the agricultural machinery on the three-dimensional surface.

[0128] Step 3: The electronic device uses the trajectory point as the centroid of the agricultural machinery and calculates the position and posture data of the agricultural machinery at the trajectory point.

[0129] Step 4: The electronic device calculates the estimated side slip value of each wheel of the agricultural machine at the trajectory point based on the soil characteristic parameters and the wheel parameters of the agricultural machine.

[0130] Step 5: The electronic device performs sideslip compensation on the agricultural machinery at the trajectory point based on the movement direction, trajectory point, sideslip estimation value and posture data of the agricultural machinery.

[0131] Step 6: The electronic device traverses the compensated trajectory points in sequence and regenerates the trajectory points of the global three-dimensional path planning.

[0132] By obtaining the soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the trajectory point; and calculating the estimated side slip values ​​of each wheel of the agricultural machinery at the trajectory point based on the soil characteristic parameters and the wheel parameters of the agricultural machinery; thus, the present invention considers the influence of soil physical characteristics in the path planning of the three-dimensional curve of the agricultural machinery; by calculating the posture data of the agricultural machinery at the trajectory point; and performing side slip compensation on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the estimated side slip value and the posture data. Wherein, the side slip compensation direction of the agricultural machinery at the trajectory point can be determined based on the movement direction of the agricultural machinery and the trajectory point, and the sideslip estimation value and the posture data can be determined based on the sideslip estimation value and the posture data, so as to perform side slip compensation on the agricultural machinery at the trajectory point based on the sideslip compensation direction and the sideslip compensation value. Compared with the traditional curved surface agricultural machinery navigation method, which can only correct the travel direction through the motor and the steering wheel when the agricultural machinery is operating, so as to compensate for the sideslip value that has been exited at the next moment, the present invention determines the sideslip compensation direction and the sideslip compensation value based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data before the sideslip occurs, and then compensates for the agricultural machinery at the trajectory point. The present invention avoids the impending sideslip of the agricultural machinery at the path planning level, and improves the effect of path planning for agricultural machinery on three-dimensional curved surfaces such as sloping farmland.

[0133] The agricultural machinery sideslip estimation and compensation device provided by the present invention is described below. The agricultural machinery sideslip estimation and compensation device described below and the agricultural machinery sideslip estimation and compensation method described above can be referenced to each other.

[0134] Please refer to Figure 7 The present invention also provides an agricultural machinery sideslip estimation and compensation device, comprising:

[0135] The first acquisition module 201 is used to acquire the track points of the agricultural machine on the three-dimensional surface;

[0136] The second acquisition module 202 is used to acquire soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machine at the trajectory point;

[0137] A first calculation module 203, used to calculate the position and posture data of the agricultural machine at the trajectory point;

[0138] A sideslip estimation module 204, configured to calculate a sideslip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machine;

[0139] The sideslip compensation module 205 is used to perform sideslip compensation on the agricultural machine at the trajectory point based on the movement direction of the agricultural machine, the trajectory point, the sideslip estimation value and the posture data.

[0140] The agricultural machinery sideslip estimation and compensation device of the present invention obtains the soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the trajectory point; and calculates the sideslip estimation value of each wheel of the agricultural machinery at the trajectory point based on the soil characteristic parameters and the wheel parameters of the agricultural machinery; thus, the present invention considers the influence of soil physical characteristics in the path planning of the three-dimensional curve of the agricultural machinery; calculates the posture data of the agricultural machinery at the trajectory point; and performs sideslip compensation on the agricultural machinery at the trajectory point based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data. Wherein, the sideslip compensation direction of the agricultural machinery at the trajectory point can be determined based on the movement direction of the agricultural machinery and the trajectory point, and the sideslip estimation value and the posture data can be determined based on the sideslip estimation value and the posture data, so as to perform sideslip compensation on the agricultural machinery at the trajectory point based on the sideslip compensation direction and the sideslip compensation value. Compared with the traditional curved surface agricultural machinery navigation method, which can only correct the travel direction through the motor and the steering wheel when the agricultural machinery is operating, so as to compensate for the sideslip value that has been exited at the next moment, the present invention determines the sideslip compensation direction and the sideslip compensation value based on the movement direction of the agricultural machinery, the trajectory point, the sideslip estimation value and the posture data before the sideslip occurs, and then compensates for the agricultural machinery at the trajectory point. The present invention avoids the impending sideslip of the agricultural machinery at the path planning level, and improves the effect of path planning for agricultural machinery on three-dimensional curved surfaces such as sloping farmland.

[0141] Based on the above embodiments, as an optional embodiment, the sideslip estimation module includes:

[0142] A wheel slip rate calculation module, used for calculating the wheel slip rate of the agricultural machine based on the wheel parameters;

[0143] A settlement amount calculation module, used to calculate the settlement amount of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the force value of each wheel of the agricultural machine at the track point;

[0144] The side slip estimation value determination module is used to calculate the side slip estimation value of each wheel of the agricultural machine at the track point based on the sinking amount, the wheel parameter and the wheel slip rate.

[0145] On the basis of the above embodiments, as an optional embodiment, the calculation of the posture data of the agricultural machinery at the trajectory point is specifically used to calculate the cross product of the first side wheel vector and the second side wheel vector of the agricultural machinery to obtain the posture data of the agricultural machinery at the trajectory point.

[0146] Based on the above embodiments, as an optional embodiment, the sideslip compensation module includes:

[0147] A mass ratio determination module, used to determine the mass ratio of each wheel of the agricultural machine based on the posture data; the mass ratio is used to describe the ratio of the mass of each wheel of the agricultural machine to the total wheel mass of the agricultural machine;

[0148] a sideslip compensation value determination module, configured to obtain a sideslip compensation value of the agricultural machine based on the sideslip estimation value and a mass ratio of each wheel of the agricultural machine;

[0149] A side slip compensation direction determination module, used to determine the side slip compensation direction of the agricultural machine based on the movement direction of the agricultural machine and the trajectory point;

[0150] A final sideslip compensation module is used to perform sideslip compensation on the trajectory point based on the sideslip compensation direction and the sideslip compensation value.

[0151] Based on the above embodiments, as an optional embodiment, the sideslip compensation value of the agricultural machine is obtained based on the sideslip estimation value and the mass ratio of each wheel of the agricultural machine by the following formula:

[0152] J all =λ 1 J y1 +λ 2 J y2 +λ 3 J y3 +λ 4 Jy4 ;

[0153] Among them, J all represents the side slip compensation value of the agricultural machine; J y1 , J y2 , J y3 , J y4 Respectively represent the side slip estimation value of each wheel of the agricultural machine; λ 1 , 2 , 3 , 4 Respectively represent the mass ratio of each wheel of the agricultural machine.

[0154] Based on the above embodiments, as an optional embodiment, the sideslip compensation direction determination module includes:

[0155] A plane acquisition module, used for forming a plane based on the movement direction of the agricultural machine and the trajectory points;

[0156] The side slip compensation direction final determination module is used to use the normal vector direction of the plane as the side slip compensation direction of the agricultural machinery.

[0157] Figure 8 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 8 As shown, the electronic device may include: a processor 810, a communication interface 820, a memory 830 and a communication bus 840, wherein the processor 810, the communication interface 820 and the memory 830 communicate with each other through the communication bus 840. The processor 810 may call the logic instructions in the memory 830 to execute the agricultural machinery side slip estimation and compensation method, the method comprising: obtaining the track point of the agricultural machinery on the three-dimensional surface; obtaining the soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the track point; calculating the posture data of the agricultural machinery at the track point; calculating the side slip estimation value of each wheel of the agricultural machinery at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machinery; and performing side slip compensation on the agricultural machinery at the track point based on the movement direction of the agricultural machinery, the track point, the side slip estimation value and the posture data.

[0158] In addition, the logic instructions in the above-mentioned memory 830 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0159] On the other hand, the present invention also provides a computer program product, which includes a computer program. The computer program can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the agricultural machinery sideslip estimation and compensation method provided by the above-mentioned methods, and the method includes: obtaining trajectory points of the agricultural machinery on a three-dimensional surface; obtaining soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the trajectory points; calculating the posture data of the agricultural machinery at the trajectory points; calculating the sideslip estimation value of each wheel of the agricultural machinery at the trajectory points based on the soil characteristic parameters and the wheel parameters of the agricultural machinery; and performing sideslip compensation on the agricultural machinery at the trajectory points based on the movement direction of the agricultural machinery, the trajectory points, the sideslip estimation values ​​and the posture data.

[0160] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the agricultural machinery sideslip estimation and compensation method provided by the above-mentioned methods, the method comprising: obtaining trajectory points of the agricultural machinery on a three-dimensional surface; obtaining soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machinery at the trajectory points; calculating the posture data of the agricultural machinery at the trajectory points; calculating the sideslip estimation value of each wheel of the agricultural machinery at the trajectory points based on the soil characteristic parameters and the wheel parameters of the agricultural machinery; and performing sideslip compensation on the agricultural machinery at the trajectory points based on the movement direction of the agricultural machinery, the trajectory points, the sideslip estimation values ​​and the posture data.

[0161] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0162] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0163] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. 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 estimating and compensating sideslip of agricultural machinery, characterized in that: include: Obtain the trajectory points of the agricultural machinery on the three-dimensional surface; Acquire soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machine at the trajectory point; Calculate the position and posture data of the agricultural machine at the trajectory point; Calculating a side slip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machine; Performing side slip compensation on the agricultural machine at the track point based on the movement direction of the agricultural machine, the track point, the side slip estimation value and the posture data; The calculating the side slip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameter and the wheel parameter of each wheel of the agricultural machine comprises: Calculating a wheel slip rate of the agricultural machine based on the wheel parameters; Calculating the amount of subsidence of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the force value of each wheel of the agricultural machine at the track point; The sideslip estimation value of each wheel of the agricultural machinery at the trajectory point is calculated based on the settlement amount, the wheel parameters and the wheel slip rate, and the shear displacement of the agricultural machinery wheel at the point is estimated by the soil shear model, and the shear displacement is related to the sideslip estimation value of each wheel of the agricultural machinery.

2. The agricultural machinery sideslip estimation and compensation method according to claim 1, characterized in that: The calculating the posture data of the agricultural machine at the trajectory point includes: The cross product of the first side wheel vector and the second side wheel vector of the agricultural machinery is calculated to obtain the position and posture data of the agricultural machinery at the trajectory point.

3. The agricultural machinery sideslip estimation and compensation method according to claim 1 or 2, characterized in that: The method of performing sideslip compensation on the agricultural machine at the track point based on the movement direction of the agricultural machine, the track point, the sideslip estimation value and the posture data comprises: Determine the mass ratio of each wheel of the agricultural machine based on the posture data; the mass ratio is used to describe the ratio of the mass of each wheel of the agricultural machine to the total wheel mass of the agricultural machine; Obtaining a sideslip compensation value of the agricultural machine based on the sideslip estimation value and a mass ratio of each wheel of the agricultural machine; Determining a side slip compensation direction of the agricultural machine based on the movement direction of the agricultural machine and the trajectory point; Side slip compensation is performed on the trajectory point based on the sideslip compensation direction and the sideslip compensation value.

4. The agricultural machinery sideslip estimation and compensation method according to claim 3, characterized in that: The sideslip compensation value of the agricultural machine is obtained based on the sideslip estimation value and the mass ratio of each wheel of the agricultural machine by the following formula: J all =λ1J y1 +λ2J y2 +λ3J y3 +λ4J y4 ; Among them, J all represents the side slip compensation value of the agricultural machine; J y1 , J y2 , J y3 , J y4 They respectively represent the sideslip estimation values ​​of each wheel of the agricultural machinery; λ1, λ2, λ3, and λ4 respectively represent the mass ratios of each wheel of the agricultural machinery.

5. The agricultural machinery sideslip estimation and compensation method according to claim 3, characterized in that: The determining of the sideslip compensation direction of the agricultural machine based on the movement direction of the agricultural machine and the trajectory point comprises: A plane is formed based on the movement direction of the agricultural machine and the trajectory points; The normal vector direction of the plane is used as the sideslip compensation direction of the agricultural machinery.

6. An agricultural machinery sideslip estimation and compensation device, characterized in that: include: The first acquisition module is used to acquire the track points of the agricultural machinery on the three-dimensional surface; A second acquisition module is used to acquire soil characteristic parameters corresponding to the three-dimensional surface position of the agricultural machine at the trajectory point; A first calculation module, used for calculating the position and posture data of the agricultural machine at the trajectory point; A sideslip estimation module, used for calculating the sideslip estimation value of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the wheel parameters of the agricultural machine; A sideslip compensation module, used for performing sideslip compensation on the agricultural machine at the track point based on the movement direction of the agricultural machine, the track point, the sideslip estimation value and the posture data; The sideslip estimation module comprises: A wheel slip rate calculation module, used for calculating the wheel slip rate of the agricultural machine based on the wheel parameters; A settlement amount calculation module, used to calculate the settlement amount of each wheel of the agricultural machine at the track point based on the soil characteristic parameters and the force value of each wheel of the agricultural machine at the track point; The side slip estimation value determination module is used to calculate the side slip estimation value of each wheel of the agricultural machinery at the trajectory point based on the settlement amount, the wheel parameters and the wheel slip rate, and estimate the shear displacement of the agricultural machinery wheel at the point through the soil shear model, and the shear displacement is related to the side slip estimation value of each wheel of the agricultural machinery.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the agricultural machinery sideslip estimation and compensation method as described in any one of claims 1 to 5 is implemented.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the agricultural machinery sideslip estimation and compensation method as described in any one of claims 1 to 5 is implemented.

9. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the agricultural machinery sideslip estimation and compensation method as described in any one of claims 1 to 5 is implemented.

Images