Automatic operation posture control method and system for agricultural machinery

Abstract

The present invention discloses an automatic working posture control method and system for agricultural machinery, and belongs to the technical field of vehicle state control. The present invention determines whether the agricultural machinery enters a driving state or a working state according to whether the current environmental data contains a working place. When the agricultural machinery enters the driving state, a segmented target trajectory is obtained according to the current environmental data and the navigation system, and the historical control instructions are found according to the segmented target trajectory. A new control instruction is generated according to the correction parameter and the historical control instruction, and the damping and stiffness of the support mechanism are adjusted according to the new control instruction, so that the agricultural machinery can travel smoothly without tipping over. When the agricultural machinery enters the working state, a steering instruction is generated according to the second steering angle, and an operating instruction is generated according to the current posture and the working posture of the agricultural machinery. The main controller aligns the working device of the agricultural machinery with the orientation of the working place according to the steering instruction, and then controls the working device to switch from the current posture to the working posture through the working instruction, so as to realize automatic operation and save manpower.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle state control, and in particular to an automatic operation posture control method and system for an agricultural machine. Background Art

[0002] With the development of automation technology, agricultural machinery is becoming more and more intelligent, and automatic operation is achieved by controlling posture. At the same time, the adaptability of agricultural machinery to complex outdoor environments is improved by combining navigation and environmental sensors. At present, agricultural machinery mainly includes a carrier device, a suspension device, a walking device and an operating device. The posture control system mainly controls the suspension device to make the agricultural machinery run smoothly, and controls the operating device to make the agricultural machinery perform automatic operations. The Chinese patent application with publication number CN118303181A discloses a speed acquisition posture adjustment mechanism for an electric-driven wheat planter. The mechanism collects suspension posture information, calculates the rotation angle required for each suspension component to maintain a stable state, and drives each suspension motor to rotate to compensate for the suspension posture, collects the operating speed of the electric-driven wheat planter, and transmits the collected operating speed to the core sowing controller. The core sowing controller calculates and outputs the required working speed of the seeding shaft and the fertilizer discharge shaft to the motor controller, and the motor controller controls the rotation of the drive shaft motor to work. The above invention obtains the suspension posture compensation by calculation, and the actual compensation effect cannot be determined, and it does not involve the control of the walking posture and the working posture by autonomous recognition operation. It is necessary to make further improvements to the existing technology in terms of controlling the walking posture of agricultural machinery in different environments and automatically identifying the working location and controlling the working posture. Summary of the invention

[0003] In order to solve the defects of the above-mentioned prior art, the present invention proposes an automatic working posture control method and system for agricultural machinery. The present invention determines whether the agricultural machinery has entered the driving state according to whether the working location appears in the current environmental data. If it has entered the driving state, the segmented target trajectory is planned through the navigation system and the environmental sensor, and the historical control instructions are extracted based on the segmented target trajectory and the historical trajectory. The automatic driving of the agricultural machinery is realized according to the segmented target trajectory, the historical control instructions, and the correction parameters; if it has not entered the driving state, the working posture is generated according to the current environmental data, and the working instructions are generated according to the current posture and the working posture and act on the working device to realize the automatic operation.

[0004] The technical solution of the present invention is achieved in this way:

[0005] A method for controlling an automatic working posture of an agricultural machine, wherein the agricultural machine comprises a carrier device, a suspension device, a walking device and an operating device, wherein the operating device comprises a boom, a stick and an operating device, and the suspension device comprises two sets of supporting mechanisms, a first guiding mechanism and a second guiding mechanism.

[0006] The automatic operation posture control method of the agricultural machine comprises the following steps:

[0007] Step 1: Install the first inclination sensor and the second inclination sensor in parallel on the sides of the boom and the stick, install the environmental sensor on the carrier device near the working equipment, and the main controller completes the initialization operation of the correction parameters after the agricultural machine is powered on;

[0008] Step 2: The main controller reads the current environmental data of the environmental sensor and identifies the working location according to the current environmental data. If the identification fails, it goes to step 3; otherwise, it goes to step 7.

[0009] Step 3: The agricultural machine enters the driving state, links to the navigation system, obtains the target trajectory according to the current environmental data and the navigation system, and obtains the segmented target trajectory according to the recognition range of the environmental sensor and the target trajectory;

[0010] Step 4: Obtain the first turning angle and slope angle of the segmented target trajectory according to the current environment data, calculate the similarity between the segmented target trajectory and the historical trajectory according to the first turning angle and slope angle, and compare the segmented target trajectory with the historical trajectory library to obtain at least one historical trajectory with the highest similarity;

[0011] Step 5: If the similarity between the historical trajectory and the segmented target trajectory is less than the first threshold, the segmented target trajectory is added to the historical trajectory library, and the control instructions of the agricultural machinery are monitored in real time and stored in the historical instruction library; otherwise, proceed to step 6;

[0012] Step 6: Update the correction parameters according to the similarity, find the corresponding historical control instructions in the historical instruction library according to the historical trajectory, the main controller generates new control instructions according to the correction parameters and the historical control instructions and acts on the support mechanism, controls the first guide mechanism according to the segmented target trajectory, and returns to step 2;

[0013] Step 7: If the distance between any work site and the agricultural machine is greater than the second threshold, the navigation system generates a segmented target trajectory according to the work site and returns to step 4; otherwise, the target work site is selected according to the current environmental data, the agricultural machine enters the working state, and the process proceeds to step 8;

[0014] Step 8: Obtain a second steering angle according to current environmental data, generate a steering command according to the second steering angle, and the main controller acts on the second guide mechanism according to the steering command to obtain the horizontal distance between the target operation location and the agricultural machine and generate an operation posture;

[0015] Step 9: obtaining the current posture of the agricultural machine through the first inclination sensor and the second inclination sensor, generating an operation instruction according to the operation posture and the current posture, and the main controller acts on the operation device according to the operation instruction;

[0016] Step 10: Identify the next work location based on the current environmental data. If the identification fails, the agricultural machine exits the working state and ends the work. Otherwise, select the next target work location based on the current environmental data, the agricultural machine enters the working state, and returns to step 8.

[0017] In the present invention, the working device includes 7 hinge points, the first hinge point is the connection point between the boom cylinder and the carrier device, the second hinge point is the connection point between the boom carrier device, the third hinge point is the connection point between the boom cylinder and the boom, the fourth hinge point is the connection point between the boom and the arm, the fifth hinge point is the connection point between the boom and the arm, the sixth hinge point is the connection point between the arm and the working equipment, and the seventh hinge point is the connection point between the arm cylinder and the arm.

[0018] In the present invention, in step 3, the navigation system plans multiple paths according to the starting location and the initially set end location, identifies obstacles through current environmental data, eliminates paths with obstacles to obtain at least one feasible path, and selects a feasible path with the shortest distance to obtain the target trajectory, the starting location is the current location of the agricultural machinery, the end location is the location of any work location, the recognition range of the environmental sensor is greater than the diameter of the work circle, and the diameter of the work circle is the maximum distance between any work location and other work locations.

[0019] In the present invention, in step 4, the first turning angle of the segmented target trajectory is obtained according to the current environment data. and slope angle λ, the first steering angle is the angle between the current position of the first guide mechanism and the position of the segmented target trajectory, the similarity between the segmented target trajectory and the historical trajectory , μ1+μ2=1, μ1 is the first coefficient, μ2 is the second coefficient.

[0020] In the present invention, in step 5, the historical instruction library stores historical control instructions, the historical control instructions correspond to the historical tracks one by one, and the agricultural machinery is operated according to the historical control instructions to realize the historical tracks and maintain balance.

[0021] In the present invention, in step 6, the correction parameter , based on the historical control instruction I1 and the correction parameter δ, a new control instruction I is generated 11 , I 11 =I1×δ.

[0022] In the present invention, in step 8, the second steering angle is the angle between the current position of the second guide mechanism and the position of the target working place, the working posture includes the first target inclination angle θ1 and the second target inclination angle θ2, and the distance L between the second hinge point and the sixth hinge point is obtained according to the horizontal distance L0 between the target working place and the agricultural machine. 26 The distance L between the first hinge point and the sixth hinge point 16 , , , where L2 is the horizontal distance between the second hinge point and the agricultural machine, h2 is the distance between the second hinge point and the ground, L1 is the horizontal distance between the first hinge point and the agricultural machine, h1 is the distance between the first hinge point and the ground, and the distance L between the first hinge point and the second hinge point is obtained. 12 , according to L 26 , L 16 and L 12 The first minute angle θ is obtained 11 , , get the distance L between the second hinge point and the fifth hinge point 25 , the distance L between the fifth hinge point and the sixth hinge point 56 , according to L 25 , L 56 and L 26 The second angle θ is obtained 12 , , get the distance L between the second hinge point and the third hinge point 23 , the distance between the second hinge point and the fifth hinge point L 25 , the distance between the third hinge point and the fifth hinge point L 35 , according to L 23 , L 25 and L 35 The third minute angle θ is obtained 13 , , the first target inclination angle θ1=θ 11 +θ 12 +θ 13 .

[0023] In the present invention, in step 9, the first inclination angle is obtained according to the first inclination sensor, and the second inclination angle is obtained according to the second inclination sensor. The current posture includes the first inclination angle θ3 and the second inclination angle θ4. The target boom cylinder length L3 is calculated according to the working posture. , calculate the target boom cylinder length L4, calculate the current boom cylinder length L5 and the current boom cylinder length L6 according to the current posture, obtain the first deviation ΔL1 based on the target boom cylinder length and the current boom cylinder length, and obtain the second deviation ΔL2 based on the target boom cylinder length and the current boom cylinder length.

[0024] In the present invention, in step 9, the operation instructions are the first drive current and the second drive current, the first controller obtains the first control quantity and the second control quantity according to the first deviation and the second deviation and transmits them to the main controller, the main controller amplifies the first control quantity and the second control quantity through the amplifier to generate the first drive current and the second drive current, and inputs the first drive current and the second drive current into the electro-hydraulic proportional valve of the boom cylinder and the electro-hydraulic proportional valve of the arm cylinder respectively.

[0025] A control system for realizing the automatic operation posture control method of the agricultural machine, comprising: a main controller, an environmental sensor, a navigation system, a first inclination sensor, a second inclination sensor, a first controller, and an amplifier.

[0026] Environmental sensors are used to obtain current environmental data of agricultural machinery;

[0027] The navigation system is used to plan the path according to the current position of the agricultural machinery and the initially set end location, and to generate segmented target trajectories according to the operation location;

[0028] The first inclination sensor is used to measure a first inclination;

[0029] The second inclination sensor is used to measure a second inclination;

[0030] The first controller is used to obtain a first control amount and a second control amount according to the first deviation and the second deviation;

[0031] The amplifier is used to amplify the first control amount and the second control amount;

[0032] The main controller is used to read the current environmental data of the environmental sensor and identify the working location according to the current environmental data.

[0033] The automatic operation posture control method and system of the agricultural machinery of the present invention have the following beneficial effects: the present invention plans the target trajectory based on the navigation system and the environmental sensor, realizes the passage of the agricultural machinery on the barrier-free path, can realize the safety and stability of the agricultural machinery, and improves the environmental adaptability of the agricultural machinery. The historical trajectory with the highest similarity to the segmented target trajectory is extracted based on the historical trajectory library, the historical control instruction is extracted from the historical instruction library according to the historical trajectory, and the historical control instruction is corrected by correcting the parameter, so that the historical control instruction is applicable to the segmented target trajectory, and automatic driving is realized. By establishing the historical trajectory library and the historical instruction library, it is not necessary to calculate the compensation of the driving posture according to the balance model, so that the agricultural machinery is more stable during the actual driving process, and the agricultural machinery is more intelligent by continuously updating and improving the historical trajectory library and the historical instruction library. The present invention identifies the operation location according to the current environmental data, generates the steering instruction and the operation instruction, and the main controller acts on the operation device according to the steering instruction and the operation instruction to realize automatic operation, so that the agricultural machinery has stronger adaptability in the unmanned environment and saves manpower. The operation instruction is finely controlled by the first controller to realize the stability and fluency of the automatic operation posture control of the agricultural machinery. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a schematic diagram of the agricultural machinery structure of the present invention;

[0035] Figure 2 It is a structural schematic diagram of the suspension device of the present invention;

[0036] Figure 3 It is a structural schematic diagram of the working device of the present invention;

[0037] Figure 4 It is a flow chart of the automatic working posture control method of the agricultural machinery of the present invention;

[0038] Figure 5 A schematic diagram of obtaining a first driving current according to a first deviation according to the present invention;

[0039] Figure 6 It is a schematic diagram of the working posture of the present invention;

[0040] Figure 7 The present invention is a block diagram of a control system for realizing the automatic working posture control method of the agricultural machinery.

[0041] Reference numerals in the accompanying drawings: 101 carrier device, 102 suspension device, 103 walking device, 104 working device, 105 supporting mechanism, 106 first guide mechanism, 107 second guide mechanism, 108 boom, 109 arm, 110 working equipment, 111 boom hydraulic cylinder, 112 arm hydraulic cylinder, 113 target working location. DETAILED DESCRIPTION

[0042] In order to more clearly understand the purpose, technical solutions and advantages of the present application, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0043] According to whether the current environmental data contains the work site, it is determined whether the agricultural machinery enters the driving state or the working state. When the agricultural machinery enters the driving state, the main controller obtains the segmented target trajectory according to the current environmental data and the navigation system, calculates the similarity between the segmented target trajectory and the historical trajectory according to the first steering angle and slope angle of the segmented target trajectory, finds the corresponding historical control instructions in the historical instruction library according to the historical trajectory and the similarity, updates the correction parameters according to the similarity, and the main controller generates new control instructions according to the correction parameters and the historical control instructions and acts on the suspension device. The new control instructions mainly control the damping and stiffness of the support mechanism, so that the agricultural machinery can travel smoothly without tipping over. When the agricultural machinery enters the working state, a steering instruction is generated according to the second steering angle, and an operating instruction is generated according to the current posture and working posture of the agricultural machinery. The main controller controls the agricultural machinery to align the working device with the direction of the working site according to the steering instruction, and then controls the working device to switch from the current posture to the working posture through the working instruction to complete the work. Embodiment 1

[0044] like Figures 1 to 3As shown, the automatic working posture control method of the agricultural machine of the present invention comprises a carrier device, a suspension device, a walking device and a working device, the working device comprises a boom, a dipper and working equipment, and the suspension device comprises two sets of supporting mechanisms, a first guiding mechanism and a second guiding mechanism. Figure 1 As shown, the agricultural machine includes a carrier device 101, a suspension device 102, a traveling device 103 and a working device 104. Figure 2 As shown, the first guide mechanism 106 is connected to the walking device of the agricultural machine to control the direction of the walking device, and the second guide mechanism 107 is connected to the carrier device to control the steering of the carrier device and thus the steering of the working device. The support mechanism 105 can be an air spring, a shock absorber, or a height control valve. When the ground on which the agricultural machine is traveling is uneven, the driving process is stabilized by changing the stiffness and damping of the support mechanism. Figure 3 As shown, the working device includes a boom 108, a stick 109 and a working device 110, and also includes a boom hydraulic cylinder 111 and a stick hydraulic cylinder 112. The working device also includes 7 hinge points, the first hinge point is represented by the letter A, the second hinge point is represented by the letter B, the third hinge point is represented by the letter C, the fourth hinge point is represented by the letter D, the fifth hinge point is represented by the letter E, the sixth hinge point is represented by the letter F, and the seventh hinge point is represented by the letter G. The first hinge point is the connection point between the boom cylinder and the carrier device, the second hinge point is the connection point between the boom carrier device, the third hinge point is the connection point between the boom cylinder and the boom, the fourth hinge point is the connection point between the stick cylinder and the boom, the fifth hinge point is the connection point between the boom and the stick, the sixth hinge point is the connection point between the stick and the working equipment, and the seventh hinge point is the connection point between the stick cylinder and the stick. The lengths of the boom hydraulic cylinder and the stick hydraulic cylinder are controlled by the electro-hydraulic proportional valve of the boom cylinder and the electro-hydraulic proportional valve of the stick cylinder.

[0045] like Figures 4 to 5 As shown, the automatic operation posture control method of the agricultural machine includes the following steps:

[0046] Step 1: Install the first inclination sensor and the second inclination sensor in parallel on the sides of the boom and the stick, install the environmental sensor on the carrier device near the working equipment, and the main controller completes the initialization operation of the correction parameters after the agricultural machinery is powered on. The first inclination sensor and the second inclination sensor are respectively installed with a base and a protective shell. The first inclination sensor and the second inclination sensor communicate with the main controller of the agricultural machinery through the CAN bus. The environmental sensor can be a radar or a laser. The environmental sensor can detect obstacles, identify the working location, and calculate the slope angle and distance. The initial value of the correction parameter is set to 1.

[0047] Step 2: The main controller reads the current environmental data of the environmental sensor and identifies the work location based on the current environmental data. If the identification fails, it goes to step 3, otherwise, it goes to step 7. It is determined whether the agricultural machine has entered the driving state based on whether there is a work location in the current environmental data.

[0048] Step 3: The agricultural machinery enters the driving state, links to the navigation system, obtains the target trajectory according to the current environmental data and the navigation system, and obtains the segmented target trajectory according to the recognition range of the environmental sensor and the target trajectory. The navigation system plans multiple paths according to the starting location and the initially set end location, identifies obstacles through the current environmental data, excludes the paths with obstacles to obtain at least one feasible path, and selects a feasible path with the shortest distance to obtain the target trajectory. The starting location is the current location of the agricultural machinery, and the end location is the location of any work location. The recognition range of the environmental sensor is greater than the diameter of the work circle, and the diameter of the work circle is the maximum distance between any work location and other work locations. From the starting location, the target trajectory with a length of the recognition range of the environmental sensor is intercepted to obtain the segmented target trajectory. Since the recognition range of the environmental sensor is limited, if the current location of the agricultural machinery is far away from the end location, the current environmental data collected by the environmental sensor does not match the target trajectory. Therefore, it is necessary to segment the target trajectory according to the recognition range of the environmental sensor to obtain the segmented target trajectory, and the segmented target trajectory corresponds to the current environmental data.

[0049] Step 4: Obtain the first steering angle and slope angle of the segmented target trajectory based on the current environmental data, calculate the similarity between the segmented target trajectory and the historical trajectory based on the first steering angle and the slope angle, and compare the segmented target trajectory with the historical trajectory library to obtain at least one historical trajectory with the highest similarity. The first steering angle is the angle between the current position of the first guide mechanism and the position of the segmented target trajectory. If the current position of the first guide mechanism is on the left side of the position of the segmented target trajectory, the first steering angle is a negative value, otherwise, the first steering angle is a positive value. The first steering angle represents the steering angle of the agricultural machinery's walking device, and the slope angle represents the inclination angle of the ground where the segmented target trajectory is located. The value range of the first steering angle is [-180°, 180°], and the value range of the slope angle is [0°, 90°]. Obtain the first steering angle of the segmented target trajectory based on the current environmental data and slope angle λ, the similarity between the segmented target trajectory and the historical trajectory , μ1+μ2=1, μ1 is the first coefficient, representing the weight of the first steering angle in the similarity, and μ2 is the second coefficient, representing the weight of the slope angle in the similarity.

[0050] Step 5: If the similarity between the historical trajectory and the segmented target trajectory is less than the first threshold, the segmented target trajectory is added to the historical trajectory library, and the control instructions of the agricultural machinery are monitored in real time and stored in the historical instruction library. Otherwise, go to step 6. The historical instruction library stores historical control instructions, which correspond to historical trajectories one by one. The agricultural machinery is operated according to the historical control instructions to achieve the historical trajectory and maintain balance. The first threshold should not be set too small to avoid affecting the update of the historical trajectory library and the historical instruction library. The more complete the historical trajectory library and the historical instruction library are, the smarter the agricultural machinery is.

[0051] Step 6: Update the correction parameters according to the similarity, find the corresponding historical control instructions in the historical instruction library according to the historical trajectory, and the main controller generates new control instructions according to the correction parameters and historical control instructions and acts on the support mechanism, controls the first guide mechanism according to the segmented target trajectory, and returns to step 2. Correction parameters , based on the historical control instruction I1 and the correction parameter δ, a new control instruction I is generated 11 , I 11 =I1×δ. The historical control instructions are adjusted by correcting the parameters so that the historical control instructions can be applied to the segmented target trajectory. The first guide mechanism is controlled according to the segmented target trajectory so that the agricultural machine travels along the segmented target trajectory. The damping and stiffness of the support mechanism are controlled by the new control instructions so that the agricultural machine does not tip over and remains stable during the process of traveling along the segmented target trajectory, thereby improving the adaptability of the agricultural machine to the uneven ground environment.

[0052] Step 7: If the distance between any work site and the agricultural machine is greater than the second threshold, the navigation system generates a segmented target trajectory based on the work site and returns to step 4. Otherwise, the target work site is selected according to the current environmental data, and the agricultural machine enters the working state and enters step 8. If the distance between any work site and the agricultural machine is greater than the second threshold, the length of the boom and the dipper arm of the working device is insufficient, resulting in the inability of the working equipment to align with the work site, and the working equipment cannot complete the work. The working equipment may be a funnel, and the work may be sowing seeds. The navigation system generates a segmented target trajectory based on the current position of the agricultural machine and the position of any work site in the current environmental data, and the agricultural machine continues to travel according to the segmented target trajectory. If the distance between any work site and the agricultural machine is less than the second threshold, the work site closest to the agricultural machine is selected as the target work site according to the current environmental data, and the agricultural machine enters the working state.

[0053] Step 8: Obtain the second steering angle according to the current environmental data, generate a steering command according to the second steering angle, and the main controller acts on the second guide mechanism according to the steering command to obtain the horizontal distance between the target work site and the agricultural machinery, and generate the working posture. The second steering angle is the angle between the current position of the second guide mechanism and the position of the target work site. If the current position of the second guide mechanism is on the left side of the position of the target work site, the second steering angle is a negative value, otherwise, the second steering angle is a positive value. Generate a steering command according to the second steering angle, and the main controller controls the agricultural machinery to align the working device with the position of the work site according to the steering command. The working posture includes a first target inclination angle θ1 and a second target inclination angle θ2. The step of generating the working posture is specifically described in Example 2.

[0054] Step 9: The current posture of the agricultural machinery is obtained through the first inclination sensor and the second inclination sensor, and an operation instruction is generated according to the operation posture and the current posture. The main controller acts on the operation device according to the operation instruction. The target boom cylinder length L3 and the target stick cylinder length L4 are calculated according to the operation posture. The specific steps are described in Example 3. The first inclination angle is obtained according to the first inclination sensor, and the second inclination angle is obtained according to the second inclination sensor. The current posture includes the first inclination angle θ3 and the second inclination angle θ4. The current boom cylinder length L5 and the current stick cylinder length L6 are calculated according to the current posture. The specific steps are described in Example 3. The first deviation ΔL1 is obtained based on the target boom cylinder length and the current boom cylinder length, ΔL1=L3-L5, and the second deviation ΔL2 is obtained based on the target stick cylinder length and the current stick cylinder length, ΔL2=L4-L6. The first controller obtains the first control quantity and the second control quantity according to the first deviation and the second deviation and transmits them to the main controller. The main controller amplifies the first control quantity and the second control quantity through an amplifier to generate the first drive current and the second drive current, and inputs the first drive current and the second drive current into the electro-hydraulic proportional valve of the boom cylinder and the electro-hydraulic proportional valve of the stick cylinder respectively.

[0055] like Figure 5 As shown, the first controller may be a PID controller, and the first control quantity , μ3 is the proportional coefficient of the first controller, μ4 is the integral coefficient of the first controller, μ5 is the differential coefficient of the first controller, and t is the duration of the operation instruction acting on the operation device. The second control quantity The first controller mainly adjusts the current boom cylinder length in steps according to the first deviation, and adjusts the current arm cylinder length in steps according to the second deviation, so as to achieve refined adjustment. The operation instruction is the first drive current and the second drive current, the first drive current i1=ζI2, the second drive current i2=ζI3, ζ is the amplification factor of the amplifier, and the operation device is controlled to switch from the current posture to the operation posture through the operation instruction.

[0056] Step 10: Identify the next work site according to the current environmental data. If the identification fails, the agricultural machine exits the working state and ends the work. Otherwise, the next target work site is selected according to the current environmental data, the agricultural machine enters the working state, and returns to step 8. Because the recognition range of the environmental sensor is larger than the diameter of the working circle, the current environmental data obtained when the agricultural machine is working includes all the work sites. If the current environmental data does not include the work site that has not been worked on, the work ends. Embodiment 2

[0057] like Figure 6 As shown, the present invention further discloses a method for generating a working posture according to the horizontal distance between the target working location and the agricultural machinery.

[0058] The working posture includes a first target inclination angle θ1 and a second target inclination angle θ2, wherein the first target inclination angle is ∠ABC, and the second target inclination angle is ∠DEG. The distance L between the second hinge point and the sixth hinge point is obtained according to the horizontal distance L0 between the target working location 113 and the agricultural machine. 26 , , the distance between the first hinge point and the sixth hinge point , where L2 is the horizontal distance between the second hinge point and the agricultural machine, h2 is the distance between the second hinge point and the ground, L1 is the horizontal distance between the first hinge point and the agricultural machine, h1 is the distance between the first hinge point and the ground, and λ is the slope angle. Get the distance L between the first hinge point and the second hinge point 12 , according to L 26 , L 16 and L 12 The first minute angle θ is obtained 11 , , get the distance L between the second hinge point and the fifth hinge point 25 , the distance L between the fifth hinge point and the sixth hinge point 56 , according to L 25 , L 56 and L 26 The second angle θ is obtained 12 , , get the distance L between the second hinge point and the third hinge point 23 , the distance between the second hinge point and the fifth hinge point L 25 , the distance between the third hinge point and the fifth hinge point L 35 , according to L 23 , L 25 and L 35 The third minute angle θ is obtained 13 , , the first target inclination angle θ1=θ 11 +θ 12 +θ 13 The calculation method of the second target inclination angle θ2 is the same as the calculation method of the first target inclination angle. Embodiment 3

[0059] like Figure 6 As shown, the present invention further discloses a method for calculating a target boom cylinder length and a target stick cylinder length according to an operation posture, and calculating a current boom cylinder length and a current stick cylinder length according to a current posture.

[0060] Target boom cylinder length , target arm cylinder length , current boom cylinder length , current arm cylinder length , where L 45 is the distance between the fourth hinge point and the fifth hinge point, L 57 is the distance between the fifth hinge point and the seventh hinge point. Embodiment 4

[0061] like Figure 7 As shown, a control system for realizing the automatic working posture control method of the agricultural machinery in this embodiment includes: an agricultural machinery, a main controller, an environmental sensor, a navigation system, a first inclination sensor, a second inclination sensor, a first controller, and an amplifier. The agricultural machinery includes a carrier device, a suspension device, a walking device, and an operating device. The environmental sensor is used to obtain the current environmental data of the agricultural machinery, identify obstacles and operating locations through the current environmental data, obtain the first steering angle, the slope angle, and the second steering angle, and obtain the horizontal distance between the target operating location and the agricultural machinery. The navigation system is used to plan a path according to the starting location and the ending location, and obtain a segmented target trajectory in combination with the current environmental data. The first inclination sensor is used to measure the first inclination, and the second inclination sensor is used to measure the second inclination. The first controller is used to obtain the first control amount and the second control amount according to the first deviation and the second deviation. The amplifier is used to amplify the first control amount and the second control amount and generate an operating instruction. The main controller is used to complete the initialization operation of the correction parameter, read the current environmental data of the environmental sensor, generate a new control instruction according to the correction parameter and the historical control instruction, and act on the suspension device, act on the suspension device according to the steering instruction, and act on the operating device according to the operating instruction.

[0062] The control system also includes a data processor, a historical trajectory library, and a historical instruction library. The data processor is used to generate the current posture of the agricultural machinery according to the first inclination angle and the second inclination angle, obtain the working posture according to the horizontal distance between the target working location and the agricultural machinery, obtain the first deviation and the second deviation according to the current posture and the working posture, calculate the similarity between the segmented target trajectory and the historical trajectory according to the first steering angle and the slope angle, and generate a steering instruction according to the second steering angle. The historical trajectory library is used to store historical trajectories, and find the historical trajectory with the highest similarity to the segmented target trajectory through comparison. The historical instruction library is used to store historical control instructions of the agricultural machinery, and obtain the corresponding historical control instructions according to the historical trajectory with the highest similarity to the segmented target trajectory.

[0063] 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 and improvements 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 controlling an automatic working posture of an agricultural machine, wherein the agricultural machine comprises a carrier device, a suspension device, a walking device and a working device, wherein the working device comprises a boom, a stick and working equipment, and the suspension device comprises two sets of supporting mechanisms, a first guiding mechanism and a second guiding mechanism. It is characterized in that The automatic operation posture control method of the agricultural machine comprises the following steps: Step 1: Install the first inclination sensor and the second inclination sensor in parallel on the sides of the boom and the stick, install the environmental sensor on the carrier device near the working equipment, and the main controller completes the initialization operation of the correction parameters after the agricultural machine is powered on; Step 2: The main controller reads the current environmental data of the environmental sensor and identifies the working location according to the current environmental data. If the identification fails, it goes to step 3; otherwise, it goes to step 7. Step 3: The agricultural machine enters the driving state, links to the navigation system, obtains the target trajectory according to the current environmental data and the navigation system, and obtains the segmented target trajectory according to the recognition range of the environmental sensor and the target trajectory; Step 4: Obtain the first turning angle and slope angle of the segmented target trajectory according to the current environment data, calculate the similarity between the segmented target trajectory and the historical trajectory according to the first turning angle and slope angle, and compare the segmented target trajectory with the historical trajectory library to obtain at least one historical trajectory with the highest similarity; Step 5: If the similarity between the historical trajectory and the segmented target trajectory is less than the first threshold, the segmented target trajectory is added to the historical trajectory library, and the control instructions of the agricultural machinery are monitored in real time and stored in the historical instruction library; otherwise, proceed to step 6; Step 6: Update the correction parameters according to the similarity, find the corresponding historical control instructions in the historical instruction library according to the historical trajectory, the main controller generates new control instructions according to the correction parameters and the historical control instructions and acts on the support mechanism, controls the first guide mechanism according to the segmented target trajectory, and returns to step 2; Step 7: If the distance between any work site and the agricultural machine is greater than the second threshold, the navigation system generates a segmented target trajectory according to the work site and returns to step 4; otherwise, the target work site is selected according to the current environmental data, the agricultural machine enters the working state, and the process proceeds to step 8; Step 8: Obtain a second steering angle according to current environmental data, generate a steering command according to the second steering angle, and the main controller acts on the second guide mechanism according to the steering command to obtain the horizontal distance between the target operation location and the agricultural machine and generate an operation posture; Step 9: obtaining the current posture of the agricultural machine through the first inclination sensor and the second inclination sensor, generating an operation instruction according to the operation posture and the current posture, and the main controller acts on the operation device according to the operation instruction; Step 10: Identify the next work location based on the current environmental data. If the identification fails, the agricultural machine exits the working state and ends the work. Otherwise, select the next target work location based on the current environmental data, the agricultural machine enters the working state, and returns to step 8.

2. The method for controlling the automatic working posture of an agricultural machine according to claim 1, characterized in that: The working device includes 7 hinge points, the first hinge point is the connection point between the boom cylinder and the carrier device, the second hinge point is the connection point between the boom carrier device, the third hinge point is the connection point between the boom cylinder and the boom, the fourth hinge point is the connection point between the arm cylinder and the boom, the fifth hinge point is the connection point between the boom and the arm, the sixth hinge point is the connection point between the arm and the working equipment, and the seventh hinge point is the connection point between the arm cylinder and the arm.

3. The automatic working posture control method of agricultural machinery according to claim 1, characterized in that: In step 3, the navigation system plans multiple paths according to the starting location and the initially set end location, identifies obstacles through current environmental data, eliminates paths with obstacles to obtain at least one feasible path, and selects a feasible path with the shortest distance to obtain the target trajectory. The starting location is the current location of the agricultural machinery, the end location is the location of any work location, the recognition range of the environmental sensor is greater than the diameter of the work circle, and the diameter of the work circle is the maximum distance between any work location and other work locations.

4. The method for controlling the automatic working posture of an agricultural machine according to claim 2, characterized in that: In step 4, the first turning angle of the segmented target trajectory is obtained according to the current environment data. and slope angle λ, the first steering angle is the angle between the current position of the first guide mechanism and the position of the segmented target trajectory, the similarity between the segmented target trajectory and the historical trajectory , μ1+μ2=1, μ1 is the first coefficient, μ2 is the second coefficient.

5. The method for controlling the automatic working posture of an agricultural machine according to claim 1, characterized in that: In step 5, the historical instruction library stores historical control instructions, the historical control instructions correspond to the historical tracks one by one, and the agricultural machinery is operated according to the historical control instructions to realize the historical tracks and maintain balance.

6. The method for controlling the automatic working posture of an agricultural machine according to claim 4, characterized in that: In step 6, modify the parameters , based on the historical control instruction I1 and the correction parameter δ, a new control instruction I is generated 11 , I 11 =I1×δ.

7. The method for controlling the automatic working posture of an agricultural machine according to claim 6, characterized in that: In step 8, the second steering angle is the angle between the current position of the second guide mechanism and the position of the target working location. The working posture includes the first target inclination angle θ1 and the second target inclination angle θ2. The distance L between the second hinge point and the sixth hinge point is obtained according to the horizontal distance L0 between the target working location and the agricultural machine. 26 The distance L between the first hinge point and the sixth hinge point 16 , , , where L2 is the horizontal distance between the second hinge point and the agricultural machine, h2 is the distance between the second hinge point and the ground, L1 is the horizontal distance between the first hinge point and the agricultural machine, h1 is the distance between the first hinge point and the ground, and the distance L between the first hinge point and the second hinge point is obtained. 12 , according to L 26 , L 16 and L 12 The first minute angle θ is obtained 11 , , get the distance L between the second hinge point and the fifth hinge point 25 , the distance L between the fifth hinge point and the sixth hinge point 56 , according to L 25 , L 56 and L 26 The second angle θ is obtained 12 , , get the distance L between the second hinge point and the third hinge point 23 , the distance between the second hinge point and the fifth hinge point L 25 , the distance between the third hinge point and the fifth hinge point L 35 , according to L 23 , L 25 and L 35 The third minute angle θ is obtained 13 , , the first target inclination angle θ1=θ 11 +θ 12 +θ 13 .

8. The method for controlling the automatic working posture of an agricultural machine according to claim 7, characterized in that: In step 9, the first inclination angle is obtained according to the first inclination sensor, and the second inclination angle is obtained according to the second inclination sensor. The current posture includes the first inclination angle θ3 and the second inclination angle θ4. The target boom cylinder length L3 is calculated according to the working posture. , calculate the target boom cylinder length L4, calculate the current boom cylinder length L5 and the current boom cylinder length L6 according to the current posture, obtain the first deviation ΔL1 based on the target boom cylinder length and the current boom cylinder length, and obtain the second deviation ΔL2 based on the target boom cylinder length and the current boom cylinder length.

9. The method for controlling the automatic working posture of an agricultural machine according to claim 8, characterized in that: In step 9, the operation instructions are the first drive current and the second drive current. The first controller obtains the first control quantity and the second control quantity according to the first deviation and the second deviation and transmits them to the main controller. The main controller amplifies the first control quantity and the second control quantity through an amplifier to generate the first drive current and the second drive current, and inputs the first drive current and the second drive current into the electro-hydraulic proportional valve of the boom cylinder and the electro-hydraulic proportional valve of the arm cylinder, respectively.

10. A control system for implementing the automatic working posture control method of agricultural machinery according to claim 1, characterized in that: include: Main controller, environmental sensor, navigation system, first inclination sensor, second inclination sensor, first controller, amplifier, Environmental sensors are used to obtain current environmental data of agricultural machinery; The navigation system is used to plan the path according to the current position of the agricultural machinery and the initially set end location, and to generate segmented target trajectories according to the operation location; The first inclination sensor is used to measure a first inclination; The second inclination sensor is used to measure a second inclination; The first controller is used to obtain a first control amount and a second control amount according to the first deviation and the second deviation; The amplifier is used to amplify the first control amount and the second control amount; The main controller is used to read the current environmental data of the environmental sensor and identify the working location according to the current environmental data.

Images