Agricultural machine control method, device and equipment

Abstract

The embodiment of the invention discloses an agricultural machine control method, device and equipment. The method comprises the steps that the running speed of pilot agricultural machine equipment corresponding to follower agricultural machine equipment in a target working area at the current moment and the longitudinal error of the follower agricultural machine equipment and a virtual pilot at the current moment are acquired; according to a preset objective function and a preset constraint condition, the speed increment of the follower agricultural equipment at the current moment is determined, the objective function is used for minimizing the predicted longitudinal error of the follower agricultural equipment and the virtual navigator at the next moment, and the constraint condition comprises a preset state equation; and according to the speed increment of the follower agricultural equipment at the current moment, determining a target speed of the follower agricultural equipment at the current moment, and controlling the follower agricultural equipment to continue running in the target working area at the current moment according to the target speed.

Description

Technical Field

[0001] This document relates to the field of computer technology, and more particularly to agricultural machinery control methods, devices and equipment. Background Art

[0002] With the rapid development of navigation technology and vehicle control technology, agricultural machinery and equipment are gradually moving towards the path of automated precision development. In the agricultural machinery cluster operation scenario, follower agricultural machinery and navigator agricultural machinery need to perform agricultural operations in the form of formation.

[0003] In the agricultural machinery cluster operation scenario, in order to keep each agricultural machinery equipment in accordance with the preset formation to perform agricultural operations, the follower agricultural machinery equipment can follow the path tracking according to the pre-planned static operation path. However, in actual agricultural operations, there may be a risk of collision between the agricultural machinery equipment in the agricultural machinery cluster due to problems such as kinematic errors, and the working efficiency of the agricultural machinery cluster operation is low. To this end, the embodiments of this specification provide a technical solution to improve the working efficiency of agricultural machinery cluster operations. Summary of the invention

[0004] The purpose of the embodiments of this specification is to provide a technical solution to improve the working efficiency of agricultural machinery cluster operations.

[0005] In order to implement the above technical solution, the embodiments of this specification are implemented as follows:

[0006] An agricultural machinery control method provided in an embodiment of the present specification is applied to a follower agricultural machinery equipment, and the method includes: obtaining the driving speed of a navigator agricultural machinery equipment corresponding to the follower agricultural machinery equipment in a target working area at the current moment, and the longitudinal error between the follower agricultural machinery equipment and the virtual navigator at the current moment; determining the speed increment of the follower agricultural machinery equipment at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery equipment and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery equipment at the current moment, the driving speed of the follower agricultural machinery equipment at the previous moment, and the speed increment of the follower agricultural machinery equipment at the current moment; determining the target speed of the follower agricultural machinery equipment at the current moment according to the speed increment of the follower agricultural machinery equipment at the current moment, and controlling the follower agricultural machinery equipment to continue driving in the target working area according to the target speed at the current moment.

[0007] An agricultural machinery control device provided in an embodiment of the present specification comprises: a data acquisition module for acquiring the driving speed of a navigator agricultural machinery corresponding to the agricultural machinery control device in a target working area at a current moment, and a longitudinal error between the agricultural machinery control device and the virtual navigator at a current moment; an increment determination module for determining the speed increment of the agricultural machinery control device at a current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal error of the agricultural machinery control device and the virtual navigator at a next moment, and the constraints comprise a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at a current moment, the driving speed of the agricultural machinery control device at a previous moment, and the speed increment of the agricultural machinery control device at a current moment; a speed determination module for determining the target speed of the agricultural machinery control device at a current moment according to the speed increment of the agricultural machinery control device at a current moment, and controlling the agricultural machinery control device to continue driving in the target working area according to the target speed at the current moment.

[0008] An agricultural machinery control device provided in an embodiment of the present specification comprises: a processor; and a memory arranged to store computer executable instructions, wherein when the executable instructions are executed, the processor is caused to: obtain the driving speed of the navigator agricultural machinery corresponding to the agricultural machinery control device in the target working area at the current moment, and the longitudinal error between the agricultural machinery control device and the virtual navigator at the current moment; determine the speed increment of the agricultural machinery control device at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted speed increment of the agricultural machinery control device and the virtual navigator at the next moment; The constraint condition includes a preset state equation, which is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery equipment at the current moment, the driving speed of the agricultural machinery control equipment at the previous moment, and the speed increment of the agricultural machinery control equipment at the current moment; according to the speed increment of the agricultural machinery control equipment at the current moment, the target speed of the agricultural machinery control equipment at the current moment is determined, and the agricultural machinery control equipment is controlled to continue driving in the target working area according to the target speed at the current moment.

[0009] The embodiments of the present specification also provide a storage medium, which is used to store computer executable instructions. When the executable instructions are executed by the processor, the following process is implemented: obtaining the driving speed of the navigator agricultural machinery corresponding to the follower agricultural machinery in the target working area at the current moment, and the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment; determining the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the speed increment of the follower agricultural machinery at the current moment; determining the target speed of the follower agricultural machinery at the current moment according to the speed increment of the follower agricultural machinery at the current moment, and controlling the follower agricultural machinery to continue driving in the target working area according to the target speed at the current moment.

[0010] The embodiments of the present specification also provide a computer program product, including a computer program, which implements the following process when executed by a processor: obtaining the driving speed of the navigator agricultural machinery corresponding to the follower agricultural machinery in the target working area at the current moment, and the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment; determining the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the speed increment of the follower agricultural machinery at the current moment; determining the target speed of the follower agricultural machinery at the current moment according to the speed increment of the follower agricultural machinery at the current moment, and controlling the follower agricultural machinery to continue driving in the target working area according to the target speed at the current moment. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] In order to more clearly illustrate the technical solutions in the embodiments of this specification or the prior art, the drawings required for use in the embodiments or the prior art description will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this specification. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative labor.

[0012] Figure 1 This is an agricultural machinery control method embodiment of the present specification;

[0013] Figure 2 This is a schematic diagram of a multi-aircraft formation operation path planning in this manual;

[0014] Figure 3 This is a schematic diagram of a multi-machine communication topology structure in this specification;

[0015] Figure 4 A schematic diagram of a kinematic model of an agricultural machinery with a following structure in this specification;

[0016] Figure 5 This is another agricultural machinery control method embodiment of the present specification;

[0017] Figure 6 This is a schematic diagram of a multi-machine formation path index in this specification;

[0018] Figure 7 This is a schematic diagram of an agricultural machinery control system in this specification;

[0019] Figure 8 This is a schematic diagram of another agricultural machinery control system of this specification;

[0020] Fig. 9 A schematic diagram of a steering kinematics model of agricultural machinery in this specification;

[0021] Fig.10 A schematic diagram of a distance-speed kinematic model of this specification;

[0022] Fig.11 This is a schematic diagram of another agricultural machinery control system of this specification;

[0023] Fig.12 This is another agricultural machinery control method embodiment of the present specification;

[0024] Fig.13 A schematic diagram of the location of an agricultural machine in this manual;

[0025] Fig.14 This is an embodiment of an agricultural machinery control device of this specification;

[0026] Fig.15This is an agricultural machinery control device embodiment of this specification. DETAILED DESCRIPTION

[0027] The embodiments of this specification provide a method, device and equipment for controlling agricultural machinery.

[0028] In order to enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions in the embodiments of this specification will be described clearly and completely below in conjunction with the drawings in the embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this specification, not all of the embodiments. Based on the embodiments in this specification, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of this specification.

[0029] The embodiments of this specification provide a technical solution to improve the work efficiency of agricultural machinery cluster operations. With the rapid development of navigation technology and vehicle control technology, agricultural machinery and equipment are gradually moving towards the path of automated precision development. In the agricultural machinery cluster operation scenario, follower agricultural machinery equipment and navigator agricultural machinery equipment need to perform agricultural operations in the form of formations. In the agricultural machinery cluster operation scenario, in order to keep each agricultural machinery equipment performing agricultural operations in accordance with a preset formation, the follower agricultural machinery equipment can track the path according to a pre-planned static operation path. However, in actual agricultural operations, there may be a risk of collision between the agricultural machinery equipment in the agricultural machinery cluster due to problems such as kinematic errors, and the work efficiency of agricultural machinery cluster operations is low. To this end, the embodiments of this specification provide a technical solution to improve the work efficiency of agricultural machinery cluster operations. In this scheme, the driving speed of the navigator agricultural machinery equipment corresponding to the follower agricultural machinery equipment in the target working area at the current moment, as well as the longitudinal error between the follower agricultural machinery equipment and the virtual navigator at the current moment can be obtained, and the speed increment of the follower agricultural machinery equipment at the current moment is determined according to the preset objective function and the preset constraints. The objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery equipment and the virtual navigator at the next moment. The constraints may include a preset state equation, which can be used to characterize the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery equipment at the current moment, the driving speed of the follower agricultural machinery equipment at the previous moment, and the relationship between the speed increment of the follower agricultural machinery equipment at the current moment. Finally, the target speed of the follower agricultural machinery equipment at the current moment can be determined according to the speed increment of the follower agricultural machinery equipment at the current moment, and the follower agricultural machinery equipment can be controlled to continue driving in the target working area according to the target speed at the current moment. In this way, the real-time communication between the follower agricultural machinery and the navigator agricultural machinery can be used to obtain the current driving speed of the navigator equipment, and then the speed increment of the follower agricultural machinery at the current moment can be determined according to the kinematic characteristics, objective functions and preset conditions of the follower agricultural machinery and the navigator agricultural machinery. The target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment at the current moment, so as to ensure that the follower agricultural machinery can accurately track the predetermined planned path in the agricultural machinery cluster while maintaining the preset formation to operate, thereby reducing the risk of collision and improving the work efficiency of the agricultural machinery cluster operation.

[0030] like Figure 1 As shown, the embodiment of this specification provides an agricultural machinery control method, the execution subject of the method can be a server, and the method can be applied to follower agricultural machinery equipment, and the follower agricultural machinery equipment can be any agricultural machinery equipment in the agricultural machinery cluster that performs agricultural operations under the leadership of the navigator agricultural machinery equipment. The method can specifically include the following steps:

[0031] In step S102, the current driving speed of the leader agricultural machinery corresponding to the follower agricultural machinery in the target working area and the longitudinal error between the follower agricultural machinery and the virtual leader at the current moment are obtained.

[0032] Among them, the target work area can be any area where agricultural operations need to be carried out, for example, the target work area can be indoor areas such as greenhouses, etc. The navigator agricultural machinery equipment can be agricultural machinery equipment that can independently carry out agricultural operations, and the follower agricultural machinery equipment can be any agricultural machinery equipment in the agricultural machinery cluster that carries out agricultural operations under the leadership of the navigator agricultural machinery equipment.

[0033] In practice, in an agricultural machinery cluster operation scenario, there may be multiple follower agricultural machinery devices. For example, taking an agricultural machinery cluster constructed by three agricultural machinery devices as an example, in this agricultural machinery cluster, there may be one leader agricultural machinery device and two follower agricultural machinery devices. Assume that these three agricultural machinery devices need to be in Figure 2 Before the collaborative operation, the server can perform full coverage path planning based on the kinematic characteristics of the agricultural machinery and the operating width of the target working area. For example, the server can determine the preset planning path of the agricultural machinery in the target working area based on the equipment parameters of the agricultural machinery and the area parameters of the target working area according to the preset model predictive control (MPC) algorithm. The server can use the AB line offset method to perform full coverage path planning for the target working area. The multi-machine collaborative operation planning path obtained by the path planning can be shown as follows Figure 2 shown.

[0034] exist Figure 2 In the multiple planned paths shown, each planned path can be composed of a straight segment and a turning segment. For example, the straight segment in area 2 can be used as an operation segment, and the agricultural machinery equipment needs to perform agricultural operations on the plot of land as required in the straight segment, while the turning segment in area 1 can be used as a U-turn segment for the agricultural machinery equipment, that is, the agricultural machinery equipment does not need to perform agricultural operations in this area, but only needs to complete the turning action. In order to enable the agricultural machinery group to continue to operate without stopping and waiting at the turning point to avoid collisions between agricultural machinery equipment at the U-turn point, the first agricultural machinery equipment (i.e., the agricultural machinery equipment corresponding to path 3) can be determined as the navigator agricultural machinery equipment, and the latter two agricultural machinery equipment (i.e., the agricultural machinery equipment corresponding to path 1 and path 2) can be determined as the follower agricultural machinery equipment. The follower agricultural machinery equipment and the navigator agricultural machinery equipment only need to operate on their respective planned paths to maintain the lateral distance. Therefore, the server can obtain the driving speed of the navigator agricultural machinery equipment corresponding to the follower agricultural machinery equipment in the target working area at the current moment, as well as the longitudinal error between the follower agricultural machinery equipment and the virtual navigator at the current moment.

[0035] The navigator agricultural machinery can send the current driving speed of the navigator agricultural machinery in the target working area to each follower agricultural machinery through the wireless communication module between the navigator agricultural machinery and each follower agricultural machinery. For example, Figure 3 As shown, the navigator agricultural machinery equipment can send the current driving speed in the target working area to the follower agricultural machinery equipment 1 and the follower agricultural machinery equipment 2 respectively through the wireless communication module. In this way, each follower agricultural machinery equipment can receive the current driving speed of the navigator agricultural machinery equipment in the target working area through the wireless communication module.

[0036] In addition, the navigator agricultural machinery equipment can also send the current position information of the navigator agricultural machinery equipment in the target working area to each follower agricultural machinery equipment through the wireless communication module between the navigator agricultural machinery equipment and each follower agricultural machinery equipment. In this way, each follower agricultural machinery equipment can determine the position information of the virtual navigator based on the position information of the navigator agricultural machinery equipment, and then determine the longitudinal error between the follower agricultural machinery equipment and the virtual navigator at the current moment based on the position information of the virtual navigator and the position information of each follower agricultural machinery equipment.

[0037] Among them, the kinematic models of the leading agricultural machinery and the follower agricultural machinery can be as follows: Figure 4 As shown, the virtual navigator can replace the leader agricultural machinery as a reference for the follower agricultural machinery. When the follower agricultural machinery reaches the position of the virtual navigator, it means that the formation is in the desired motion state. At this time, the follower agricultural machinery maintains a predetermined distance and orientation with the leader agricultural machinery, realizing the agricultural machinery cluster formation control.

[0038] In addition, there may be various methods for determining the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment. Different determination methods may be selected according to different actual application scenarios, and the embodiments of this specification do not specifically limit this.

[0039] In step S104, the speed increment of the follower agricultural machinery at the current moment is determined according to the preset objective function and the preset constraint conditions.

[0040] Among them, the objective function can be used to minimize the predicted longitudinal errors of the follower agricultural machinery and the virtual navigator at the next moment, and the constraints can include a preset state equation, which can be used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the speed increment of the follower agricultural machinery at the current moment.

[0041] In implementation, the server can obtain the speed increment of the follower agricultural machinery at the current moment by solving the quadratic programming (QP) problem constructed by the objective function and constraints, that is, the server can determine the speed increment of the follower agricultural machinery at the current moment based on the preset objective function and preset constraints.

[0042] In step S106, the target speed of the follower agricultural machinery at the current moment is determined according to the speed increment of the follower agricultural machinery at the current moment, and the follower agricultural machinery is controlled to continue traveling in the target working area according to the target speed at the current moment.

[0043] In implementation, the server can use the MPC algorithm to determine the target speed of the follower agricultural machinery at the current moment based on the speed increment of the follower agricultural machinery at the current moment, or the server can also determine the target speed of the follower agricultural machinery at the current moment based on the driving speed of the follower agricultural machinery at the previous moment and the speed increment at the current moment.

[0044] In addition, there are many methods for determining the target speed, and different determination methods can be selected according to different actual application scenarios. The embodiments of this specification do not make specific limitations on this.

[0045] The embodiments of the present specification provide an agricultural machinery control method, which can obtain the driving speed of a navigator agricultural machinery corresponding to a follower agricultural machinery in a target working area at the current moment, as well as the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment, and determine the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints. The objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery and the virtual navigator at the next moment. The constraints may include a preset state equation, which can be used to characterize the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the relationship between the speed increment of the follower agricultural machinery at the current moment. Finally, the target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment of the follower agricultural machinery at the current moment, and the follower agricultural machinery can be controlled to continue driving in the target working area according to the target speed at the current moment. In this way, the real-time communication between the follower agricultural machinery and the navigator agricultural machinery can be used to obtain the current driving speed of the navigator equipment, and then the speed increment of the follower agricultural machinery at the current moment can be determined according to the kinematic characteristics, objective functions and preset conditions of the follower agricultural machinery and the navigator agricultural machinery. The target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment at the current moment, so as to ensure that the follower agricultural machinery can accurately track the predetermined planned path in the agricultural machinery cluster while maintaining the preset formation to operate, thereby reducing the risk of collision and improving the work efficiency of the agricultural machinery cluster operation.

[0046] In practical applications, there are various specific processing methods for obtaining the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment in the above step S102. An optional processing method is provided below, such as Figure 5 As shown, the process may specifically include the following steps S1022 to S1026.

[0047] In step S1022, the first position information of the navigator agricultural machinery in the target working area at the current moment is obtained.

[0048] In step S1024, the path index point of the virtual navigator at the current moment is determined based on the path index point in the first position information, the expected longitudinal distance of the virtual navigator at the current moment, and the preset distance between two adjacent path index points in the preset planned path of the follower agricultural machinery equipment.

[0049] In implementation, since the MPC algorithm tracks the expected trajectory according to the set one, the expected trajectory can also be regarded as a smooth connection of multiple path points. Therefore, a path index point can be assigned to each trajectory point of the expected trajectory, and each trajectory point corresponding to the path index point stores its own position coordinates, expected speed, expected heading angle and other information. The server can obtain the positioning signal of each agricultural machinery through positioning equipment such as satellite navigation, and perform signal filtering and other processing on the positioning signal to obtain the current posture and state of the agricultural machinery, and determine the path index point where the agricultural machinery is located at the current moment based on the obtained information. In this way, the server can determine the location information of the agricultural machinery in the preset planned path based on the path index point at the current moment.

[0050] Among them, Figure 6 As shown, the planned path corresponding to each agricultural machinery equipment in the agricultural machinery cluster contains its own path index point, and the number of path index points contained in the planned path corresponding to each agricultural machinery equipment is the same, the same path index point of each agricultural machinery equipment corresponds to the same position on its own planned path, and the length between the trajectory points corresponding to two adjacent path index points on the same planned path is d (that is, the preset distance between two adjacent path index points in the preset planned path of the follower agricultural machinery equipment is d), where d is the length between two adjacent path index points on the same planned path, not the Euclidean distance.

[0051] For an agricultural machinery cluster consisting of three agricultural machinery equipment, the expected lateral distance and the expected longitudinal distance between the leader agricultural machinery equipment and the two follower agricultural machinery equipment can be L x1 , L x2 , L y1 , L y2 . The navigator agricultural machinery equipment and the two follower agricultural machinery equipment only need to operate on the preset planned paths respectively to achieve lateral distance maintenance. As for maintaining the longitudinal distance, the navigator agricultural machinery equipment only needs to track its own predetermined planned path to operate, obtain its own real-time longitude and latitude coordinates (i.e., the first position information) through the satellite navigation module, and calculate the real-time position index point of the machine on its predetermined planned path (i.e., the path index point at the current moment) through the obtained longitude and latitude coordinates. Then, the navigator agricultural machinery equipment can use the wireless communication module to calculate the path index point as follows Figure 3 The communication topology structure shown is sent to each follower agricultural machinery equipment.

[0052] In this way, the follower agricultural machinery equipment can receive the path index point of the navigator agricultural machinery equipment at the current moment through the wireless communication module, and then the follower agricultural machinery equipment can determine the path index point of the virtual navigator at the current moment according to the expected longitudinal distance. The virtual navigator path index point is the expected position index point of the follower agricultural machinery equipment. The calculation method of the path index point of the virtual navigator at the current moment can be as follows:

[0053]

[0054] Where index_VL is the path index point of the virtual navigator at the current moment, index_L is the path index point in the first position information (i.e., the path index point of the navigator's agricultural machinery at the current moment), and L y is the expected longitudinal distance of the virtual navigator at the current moment, d is the preset distance between two adjacent path index points in the preset planned path of the follower agricultural machinery, Indicates a round-up operation.

[0055] In step S1026, the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment is determined based on the path index point of the follower agricultural machinery and the path index point of the virtual navigator at the current moment.

[0056] In implementation, Figure 7 As shown in the figure, due to the constraints of the planned path, the formation operation task can be decomposed into two subtasks, namely the path tracking subtask and the formation formation keeping subtask. In the formation system, a model predictive control (MPC) controller can be designed for each member separately. For the MPC controller of the leader agricultural machinery, it only needs to complete the path tracking subtask, while for the MPC controller of the follower agricultural machinery, it needs to complete the path tracking subtask and the formation formation keeping task subtask. This method can reduce the computational load of each MPC controller and improve the response speed and real-time performance of the entire formation system.

[0057] In practical applications, the specific processing methods of determining the target speed of the follower agricultural machinery at the next moment according to the speed increment of the follower agricultural machinery at the current moment in the above step S106, and controlling the follower agricultural machinery to continue traveling in the target working area according to the target speed at the current moment can be various. The following is another optional processing method, such as Figure 5 As shown, the process may specifically include the following steps S1062 to S1068.

[0058] In step S1062, according to the speed increment of the follower agricultural machinery at the current moment, a preset model predictive control algorithm is used to determine the first speed of the follower agricultural machinery at the current moment.

[0059] In step S1064, the lateral coordinates and heading angle of the virtual navigator at the current moment are determined according to the first position information, the expected lateral distance and the expected heading angle of the virtual navigator at the current moment.

[0060] In step S1066, the lateral error and heading angle error of the follower agricultural machinery and the virtual navigator at the current moment are determined based on the lateral coordinates and heading angle of the follower agricultural machinery at the current moment, and the lateral coordinates and heading angle of the virtual navigator at the current moment.

[0061] In implementation, Figure 4 In the kinematic model of the navigator agricultural machinery and the follower agricultural machinery shown in FIG, the motion parameters of the navigator agricultural machinery, the virtual navigator, and the follower agricultural machinery under the multi-machine collaborative navigation control may include: the global position coordinates and the heading angle (X L , Y L ,θ L ), the global position coordinates of the virtual navigator and the heading angle (X VL ,Y VL ,θ VL ), as well as the global position coordinates and heading angle (X F , Y F ,θ F ).

[0062] The global position coordinates and heading angles of the navigator and the follower agricultural machinery can be obtained by filtering the satellite navigation device signal, and the position coordinates of the virtual navigator can be obtained by the expected longitudinal distance L from the navigator agricultural machinery. y The desired lateral distance L x It is determined that the expected heading angle of the follower agricultural machinery may be θ.

[0063] The global position coordinates and heading angle (X VL , Y VL ,θ VL ) can be calculated by the following formula:

[0064]

[0065] The lateral deviation between the follower agricultural machinery and the virtual navigator x , heading angle deviation e θ It can be expressed as:

[0066]

[0067] In step S1068, based on the longitudinal error, longitudinal error and heading angle error, and the first speed at the current moment, the preset model predictive control algorithm is used to determine the target speed and target turning angle of the follower agricultural machinery at the current moment, and control the follower agricultural machinery to continue driving in the target working area according to the target speed and target turning angle at the current moment.

[0068] In implementation, the virtual navigator's position and posture calculated from the position and posture information of the navigator agricultural machinery can be used as the expected reference position and posture of the follower agricultural machinery. The follower agricultural machinery needs to meet the lateral deviation e with the virtual navigator. x →0, longitudinal deviation e y →0, heading angle deviation e θ →0, it can be achieved that each agricultural machinery can maintain the formation while driving. Based on this idea, Figure 8 As shown, the corresponding lower-level MPC path tracking controller and upper-level MPC speed planner can be designed for each follower agricultural machinery according to the kinematic characteristics and constraints of each agricultural machinery. Among them, the upper-level MPC speed planner of the follower agricultural machinery can be used to determine the speed increment of the follower agricultural machinery at the current moment according to the preset objective function and the preset constraints, and determine the target speed of the follower agricultural machinery at the current moment according to the speed increment of the follower agricultural machinery at the current moment. The lower-level MPC path tracking controller of the follower agricultural machinery can be used to determine the target speed and target turning angle of the follower agricultural machinery at the current moment according to the longitudinal error, longitudinal error and heading angle error at the current moment, and the first speed, using a preset model predictive control algorithm.

[0069] Specifically, the upper-level MPC speed planner of the follower agricultural machinery can obtain the optimal control increment input sequence ΔV by solving the quadratic programming (QP) problem F，k：k+N-1 (i.e., the input sequence consisting of the speed increment corresponding to each moment), and then further according to the optimal control increment input sequence ΔV F，k：k+N-1 The optimal control input sequence V can be obtained F，k：k+N-1 (i.e., the input sequence consisting of the first speed corresponding to each moment).

[0070] Then, if Figure 8As shown, the optimal control input sequence can be used as the expected speed sequence of the lower-level MPC path tracking controller of the follower agricultural machinery. The lower-level MPC path tracking controller can perform real-time calculations based on the optimal control input sequence, the longitudinal error, the longitudinal error, and the heading angle error at the current moment to obtain the final optimal control input sequence for angle (i.e., the sequence constructed by the target angle corresponding to each moment) and the optimal control input sequence for speed (i.e., the sequence constructed by the target speed corresponding to each moment), and the first input of the sequence (i.e., the target speed and target angle corresponding to the current moment) is applied to the follower agricultural machinery. By repeating the above process in a loop, the current state can be updated in real time and the follower agricultural machinery can be replanned and controlled.

[0071] In this way, through the upper-level MPC speed planner of the follower agricultural machinery equipment, the follower agricultural machinery equipment can adjust the driving speed in real time to maintain the stability of the formation, reduce the distance error and ensure that the follower agricultural machinery equipment drives within a safe range. It can also consider the actual constraints of the system, smooth the speed changes, and reduce the burden on the mechanical system.

[0072] In addition, if Figure 8 As shown in the figure, a corresponding MPC path tracking controller can also be designed for the navigator agricultural machinery. In this way, not only can the differences between different agricultural machinery be fully considered, but also the centralized calculation of data can be avoided, the calculation burden of the controller can be reduced, and it has strong flexibility.

[0073] As the leader in the formation, the navigator agricultural machinery needs to achieve the task of tracking its own preset planned path. Its MPC path tracking controller needs to be able to accurately control the agricultural machinery to track the preset trajectory. The MPC path tracking controller should consider the speed, heading angle and deviation from the trajectory of the navigator agricultural machinery, and adjust the steering angle and speed in real time to keep it on the predetermined trajectory.

[0074] First, we can establish Fig. 9 The agricultural machinery steering kinematic model shown is used to describe the motion characteristics of the navigator agricultural machinery equipment. This model can accurately describe the motion characteristics of the agricultural machinery on a predetermined trajectory. Fig. 9 Where φ is the yaw angle of the vehicle, i.e. the heading angle, δ is the front wheel deflection angle (steering angle), v is the vehicle heading angle speed, and l is the vehicle wheelbase.

[0075] The kinematic model of the vehicle can be obtained as:

[0076]

[0077] From the above formula, we can see that the vehicle kinematic model can be regarded as a control system with input u(v, δ) and state variables X(x, y, φ), which can be written in general form as:

[0078]

[0079] For a given reference trajectory of a vehicle, it can be described by the motion trajectory of the reference vehicle. Each point on the motion trajectory satisfies the vehicle kinematic equation, which is in the general form:

[0080]

[0081] Among them, X r =[x r y r φ r ] T ,u r =[v r δ r ], r is reference, representing the reference quantity. For the X in the navigator's reference path r with u r is the X corresponding to the operation path trajectory point closest to the current coordinates of the navigator r with u r

[0082] In order to obtain the error between the actual vehicle driving trajectory and the reference driving trajectory, the vehicle running trajectory in the above formula is expanded at the reference trajectory point and the high-order residual terms are ignored. For the navigator, the reference trajectory point is the discrete trajectory point closest to the current position of the agricultural machinery on the planned path:

[0083]

[0084] By subtracting this form of driving trajectory from the reference trajectory, we can obtain the linearized state space expression of the unmanned agricultural vehicle error model:

[0085]

[0086] in,

[0087] After successfully building the linear error model of agricultural machinery vehicles, since the prediction part of the model predictive control (MPC) algorithm is based on the discrete linear error model, the forward Euler method can be used to discretize the continuous linear state space equations.

[0088] Using the forward Euler method on the above formula, we get:

[0089]

[0090] After sorting, we can get:

[0091]

[0092] Therefore, the general form of the discretized linear error model is obtained:

[0093]

[0094] in, Where T is the sampling time.

[0095] When designing a trajectory tracking controller, the following objective function can be used. The objective function can be composed of three parts: state deviation term, control increment input term, and introduction of relaxation factor term. Q and R are weight matrices, N p is the prediction time domain; N c is the control time domain; ρ is the weight coefficient; ε is the relaxation factor.

[0096]

[0097] The first term quantifies the deviation between the system state and the predetermined trajectory, thus reflecting the system's ability to track the predetermined running trajectory. The second term reflects the constraint on the change of the input control quantity. The third term introduces the relaxation factor, which is of great significance for improving the feasibility of the problem. In the face of the unsolvable situation that may be caused by strict constraints, the relaxation factor provides flexibility for finding feasible solutions by allowing moderate violations of constraints. The advantage of this objective function is that it is easy to transform into a standard quadratic programming problem, which facilitates the solution of the problem.

[0098] In the objective function, it is necessary to calculate the output of the system in the future, and the prediction model is established as follows;

[0099] From the above, we know that the discretized linear error model state space expression is:

[0100]

[0101] In order to make the final prediction model meet the requirements of the objective function, the above formula is transformed as follows to define a new state quantity ξ:

[0102]

[0103] Then we have:

[0104]

[0105] in,

[0106] Thus, we can use ξ(k | t) as the state variable and the control quantity increment as As the new state-space expression is input, the complete state-space equation can be written as:

[0107]

[0108] In the formula, n is the dimension of the state quantity, here n=3: m is the dimension of the control quantity, here m=2.

[0109] If the prediction time domain of the system is N p , the control time domain is N c , where N p ≥N c , by recursively deducing the state quantity and system output quantity in the prediction time domain and summarizing the rules, the general form of the output expression of the linear time-varying prediction model of the system can be derived from the discretized linear error model:

[0110] Y=W k，t ξ(k|t)+Z k，t ΔU

[0111] In this formula:

[0112]

[0113] In the design and analysis of control systems, constraints form the basis of the system's feasible domain. They are a set of predefined restrictions designed to ensure that the system does not exceed predetermined safety and performance boundaries during operation.

[0114] Constraints can be divided into control quantities Constraint and control increments constraint.

[0115] The general expression of the constraint condition can be obtained:

[0116]

[0117] Where i = 0, 1, ..., N c -1, N c To control the time domain.

[0118] After clearly selecting the objective function and establishing the constraints, the entire optimization problem needs to be expressed in matrix form. By converting the system model, objective function and constraints into the form of matrix multiplication, the above problem can be transformed into a quadratic programming problem. The matrix form of expression not only simplifies the problem-solving process, but also improves the computational efficiency.

[0119] (1) Objective function transformation:

[0120] Define the system output Y and the output reference value as Y Ref for:

[0121]

[0122] Let E = Wk，t ξ(k|t) then:

[0123] Substitute into the objective function, ignore the quantities unrelated to ΔU, further simplify the objective function and remove the one-dimensional constant term and match it to get:

[0124]

[0125] in: That is X T =[ΔU T ε]; remember:

[0126] (2)Constraint transformation:

[0127] In the optimization problem, the constraints also need to be converted into the form of control increments or control increments multiplied by the transformation matrix. Therefore, the constraints also need to be transformed to obtain the corresponding transformation matrix. c Recursion inside can be obtained

[0128] U k =U k-1 +A E ΔU

[0129]

[0130] Where E is the identity matrix.

[0131] The following is the transformation matrix form of the constraints:

[0132] U min ≤U=U k-1 +A E ΔU≤U max

[0133] Right now:

[0134]

[0135] The upper and lower limit constraints are:

[0136]

[0137] At this point, the design of the model predictive controller can be specifically transformed into a quadratic optimization problem:

[0138]

[0139]

[0140] In practical applications, the preset state equation can be e y,k+1 =ey,k +T*(V L,k -(V F,k-1 +ΔV F,k )), where e y,k+1 is the predicted longitudinal error at the next moment, e y,k is the longitudinal error at the current moment, T is the preset sampling time, V L,k is the driving speed of the navigator agricultural machinery at the current moment, V F,k-1 is the driving speed of the follower agricultural machinery at the previous moment, ΔV F,k It is the speed increment of the follower agricultural machinery at the current moment.

[0141] The distance-speed kinematic model between the leader agricultural machinery, the virtual leader and the follower agricultural machinery can be expressed as Fig.10 As shown, according to the schematic diagram, the preset state equation can be established and discretized to obtain:

[0142] e y,k+1 =e y,k +T*(V L,k -(V F,k-1 +ΔV F,k )).

[0143] In practical applications, the preset constraints may also include constraints on the predicted longitudinal error at the next moment, constraints on the speed increment of the follower agricultural machinery at the current moment, and constraints on the driving speed of the follower agricultural machinery at the current moment.

[0144] The constraint condition for constraining the predicted longitudinal error at the next moment can be a state variable constraint condition, which can be used to ensure that the distance error of the follower agricultural machinery is within a safe range, that is, the state variable constraint condition can be:

[0145] e min ≤e y,k ≤e max , where e min and e max It may be a preset value determined according to equipment parameters of the follower agricultural machinery.

[0146] The constraint condition for constraining the speed increment of the follower agricultural machinery at the current moment can be an input variable constraint condition, which can be used to control the input variable (i.e., ΔV F,k ) satisfies the actual speed change limit to consider the acceleration and deceleration capability of the vehicle system, that is, the input variable constraint condition can be:

[0147] ΔV min ≤ΔV F,k ≤ΔV max , where emin and e max It may be a preset value determined according to a speed limit parameter of the follower agricultural machinery.

[0148] The constraint condition for constraining the driving speed of the follower agricultural machinery at the current moment may be a speed constraint condition, and the speed constraint condition may be used to control the speed of the follower agricultural machinery within a feasible range, that is, the speed constraint condition may be:

[0149] V min ≤V F,k ≤V max , where V min and V max It may be a preset value determined according to equipment parameters of the follower agricultural machinery.

[0150] In practical applications, the objective function can be Where N is the length of the prediction time domain, Q is the first preset weight matrix, R is the second preset weight matrix, e y,k is the longitudinal error at the current moment, ΔV F,k is the speed increment of the follower agricultural machinery at the current moment. Among them, the length of N is not greater than the predicted time domain length N of the lower-level MPC path tracking controller of the follower agricultural machinery p , that is (N≤N p ).

[0151] The objective function can be used to minimize the distance error between the follower agricultural machinery and the leader agricultural machinery through the upper-level MPC speed planner. y , while trying to smoothly follow the speed changes of agricultural machinery.

[0152] Based on the distance-speed discrete state equation (i.e., the preset state equation), while considering the speed control input constraints, input variable constraints, and state variable constraints of the agricultural machinery, an upper-level MPC speed planner can be designed for the follower agricultural machinery, and the speed control input optimal solution sequence obtained by the upper-level MPC speed planner can be used as the desired speed of the lower-level MPC path tracking controller of the follower agricultural machinery. The upper-level MPC speed planner can plan the desired speed of the follower agricultural machinery in real time, so that e y →0, thereby achieving the formation maintenance function of the fleet.

[0153] In this way, Fig.11 As shown, the global path planning of the agricultural machinery cluster can be realized through the planner of each agricultural machinery equipment.

[0154] In practical applications, the specific processing methods for obtaining the current driving speed of the leader agricultural machinery corresponding to the follower agricultural machinery in the target working area in the above step S102, and the longitudinal error between the follower agricultural machinery and the virtual leader at the current moment can be various. The following is an optional processing method, such as Fig.12 As shown, the processing may specifically include the following step S1022.

[0155] In step S1022, when the path index point of the follower agricultural machinery equipment at the current moment is different from the path index point of the virtual navigator at the current moment, the driving speed of the navigator agricultural machinery equipment corresponding to the follower agricultural machinery equipment in the target working area at the current moment, and the longitudinal error between the follower agricultural machinery equipment and the virtual navigator at the current moment are obtained.

[0156] In implementation, the follower agricultural machinery equipment needs to implement two subtasks, the first subtask is to track and control the predetermined planned path, and the second subtask is to maintain the formation. When designing the model prediction controller of the follower agricultural machinery equipment, the follower agricultural machinery equipment can receive the path index point in the current position information (i.e., the first position information) of the navigator agricultural machinery equipment sent by the navigator agricultural machinery equipment according to the communication topology structure through the wireless communication module. The follower agricultural machinery equipment can calculate the path index point index_VL of the virtual navigator according to the calculation method of the path index point of the virtual navigator at the current moment.

[0157] If the difference between index_VL and index_F (i.e., the path index point of the follower agricultural machinery equipment) is small, the discrete trajectory point on the follower preset path corresponding to the path index point can be used as the expected reference point of the follower agricultural machinery equipment, that is, The kinematic model of the follower agricultural machinery can be linearized at this point and the predicted output equation of the agricultural machinery equipment can be derived. The optimal control sequence can be solved based on the predicted output equation combined with the constraints of the follower agricultural machinery equipment and the objective function. Except for selecting the expected reference point for linearization, the other steps are consistent with the design steps of the navigator model predictive controller. That is, when the state error of the follower agricultural machinery equipment at index_F and the expected index index_VL is small, the state quantity of the model can still converge to the expected trajectory.

[0158] However, when the follower agricultural machinery equipment is on the planned path, due to some interference, the distance between the current position of the follower agricultural machinery equipment and the expected position at the next moment is far, for example, Fig.13As shown, index_VL is located at the turning point, index_F is located at the straight line, and the difference between the two is large. Since the expected turning angles at the straight line and the turning point are different, the follower agricultural machinery equipment located at index_F is linearized at index_VL to obtain a linear error model. Solving the control law based on the linear error model will cause a large model mismatch, and the prediction model cannot predict the future change trend of the original nonlinear model.

[0159] Therefore, the steering kinematic model of the follower located at index_F can be linearized at index_F instead of at index_VL, and at the same time, the expected speed of the follower agricultural machinery after the planned path index_F is planned in real time.

[0160] The embodiments of the present specification provide an agricultural machinery control method, which can obtain the driving speed of a navigator agricultural machinery corresponding to a follower agricultural machinery in a target working area at the current moment, as well as the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment, and determine the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints. The objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery and the virtual navigator at the next moment. The constraints may include a preset state equation, which can be used to characterize the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the relationship between the speed increment of the follower agricultural machinery at the current moment. Finally, the target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment of the follower agricultural machinery at the current moment, and the follower agricultural machinery can be controlled to continue driving in the target working area according to the target speed at the current moment. In this way, the real-time communication between the follower agricultural machinery and the navigator agricultural machinery can be used to obtain the current driving speed of the navigator equipment, and then the speed increment of the follower agricultural machinery at the current moment can be determined according to the kinematic characteristics, objective functions and preset conditions of the follower agricultural machinery and the navigator agricultural machinery. The target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment at the current moment, so as to ensure that the follower agricultural machinery can accurately track the predetermined planned path in the agricultural machinery cluster while maintaining the preset formation to operate, thereby reducing the risk of collision and improving the work efficiency of the agricultural machinery cluster operation.

[0161] The above is the agricultural machinery control method provided by the embodiment of this specification. Based on the same idea, the embodiment of this specification also provides an agricultural machinery control device, such as Fig.14 shown.

[0162] The agricultural machinery control device comprises: a data acquisition module 1401, an increment determination module 1402 and a speed determination module 1403, wherein:

[0163] The data acquisition module 1401 is used to acquire the current driving speed of the navigator agricultural machinery corresponding to the agricultural machinery control device in the target working area, and the longitudinal error between the agricultural machinery control device and the virtual navigator at the current moment;

[0164] The increment determination module 1402 is used to determine the speed increment of the agricultural machinery control device at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal errors of the agricultural machinery control device and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the agricultural machinery control device at the previous moment, and the speed increment of the agricultural machinery control device at the current moment;

[0165] The speed determination module 1403 is used to determine the target speed of the agricultural machinery control device at the current moment according to the speed increment of the agricultural machinery control device at the current moment, and control the agricultural machinery control device to continue traveling in the target working area according to the target speed at the current moment.

[0166] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the agricultural machinery control device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0167] The embodiments of the present specification provide an agricultural machinery control device, which can obtain the driving speed of a navigator agricultural machinery corresponding to a follower agricultural machinery in a target working area at the current moment, as well as the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment, and determine the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints. The objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery and the virtual navigator at the next moment. The constraints may include a preset state equation, which can be used to characterize the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the relationship between the speed increment of the follower agricultural machinery at the current moment. Finally, the target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment of the follower agricultural machinery at the current moment, and the follower agricultural machinery can be controlled to continue driving in the target working area according to the target speed at the current moment. In this way, the real-time communication between the follower agricultural machinery and the navigator agricultural machinery can be used to obtain the current driving speed of the navigator equipment, and then the speed increment of the follower agricultural machinery at the current moment can be determined according to the kinematic characteristics, objective functions and preset conditions of the follower agricultural machinery and the navigator agricultural machinery. The target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment at the current moment, so as to ensure that the follower agricultural machinery can accurately track the predetermined planned path in the agricultural machinery cluster while maintaining the preset formation to operate, thereby reducing the risk of collision and improving the work efficiency of the agricultural machinery cluster operation.

[0168] The above is an agricultural machinery control device provided in the embodiments of this specification. Based on the same idea, the embodiments of this specification also provide an agricultural machinery control device, such as Fig.15 shown.

[0169] The agricultural machinery control device may provide a terminal device or a server, etc. for the above-mentioned embodiments.

[0170] The agricultural machinery control device may have relatively large differences due to different configurations or performances, and may include one or more processors 1501 and memory 1502, and the memory 1502 may store one or more storage applications or data. Among them, the memory 1502 may be a short-term storage or a persistent storage. The application stored in the memory 1502 may include one or more modules (not shown in the figure), and each module may include a series of computer executable instructions in the agricultural machinery control device. Furthermore, the processor 1501 can be configured to communicate with the memory 1502 to execute a series of computer executable instructions in the memory 1502 on the agricultural machinery control device. The agricultural machinery control device may also include one or more power supplies 1503, one or more wired or wireless network interfaces 1504, one or more input and output interfaces 1505, and one or more keyboards 1506.

[0171] Specifically in this embodiment, the agricultural machinery control device includes a memory and one or more programs, wherein the one or more programs are stored in the memory, and the one or more programs may include one or more modules, and each module may include a series of computer executable instructions in the agricultural machinery control device, and the one or more programs are configured to be executed by one or more processors, including the following computer executable instructions:

[0172] Obtaining the current driving speed of the navigator agricultural machinery corresponding to the agricultural machinery control device in the target working area, and the longitudinal error between the agricultural machinery control device and the virtual navigator at the current moment;

[0173] Determine the speed increment of the agricultural machinery control device at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal errors of the agricultural machinery control device and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the agricultural machinery control device at the previous moment, and the speed increment of the agricultural machinery control device at the current moment;

[0174] According to the speed increment of the agricultural machinery control device at the current moment, the target speed of the agricultural machinery control device at the current moment is determined, and the agricultural machinery control device is controlled to continue traveling in the target working area according to the target speed at the current moment.

[0175] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the agricultural machinery control equipment embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0176] The embodiments of the present specification provide an agricultural machinery control device, which can obtain the driving speed of a navigator agricultural machinery corresponding to a follower agricultural machinery in a target working area at the current moment, as well as the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment, and determine the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints. The objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery and the virtual navigator at the next moment. The constraints may include a preset state equation, which can be used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the speed increment of the follower agricultural machinery at the current moment. Finally, the target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment of the follower agricultural machinery at the current moment, and the follower agricultural machinery can be controlled to continue driving in the target working area according to the target speed at the current moment. In this way, the real-time communication between the follower agricultural machinery and the navigator agricultural machinery can be used to obtain the current driving speed of the navigator equipment, and then the speed increment of the follower agricultural machinery at the current moment can be determined according to the kinematic characteristics, objective functions and preset conditions of the follower agricultural machinery and the navigator agricultural machinery. The target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment at the current moment, so as to ensure that the follower agricultural machinery can accurately track the predetermined planned path in the agricultural machinery cluster while maintaining the preset formation to operate, thereby reducing the risk of collision and improving the work efficiency of the agricultural machinery cluster operation.

[0177] Furthermore, based on the above Figures 1 to 13 In one embodiment, the present specification further provides a storage medium for storing computer executable instruction information. In a specific embodiment, the storage medium may be a USB flash drive, an optical disk, a hard disk, etc. When the computer executable instruction information stored in the storage medium is executed by the processor, the following process can be implemented:

[0178] Obtaining the current driving speed of the leader agricultural machinery corresponding to the follower agricultural machinery in the target working area, and the longitudinal error between the follower agricultural machinery and the virtual leader at the current moment;

[0179] Determine the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal errors of the follower agricultural machinery and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the speed increment of the follower agricultural machinery at the current moment;

[0180] According to the speed increment of the follower agricultural machinery at the current moment, the target speed of the follower agricultural machinery at the current moment is determined, and the follower agricultural machinery is controlled to continue traveling in the target working area according to the target speed at the current moment.

[0181] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the above-mentioned storage medium embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0182] The embodiments of the present specification provide a storage medium, which can obtain the driving speed of the navigator agricultural machinery equipment corresponding to the follower agricultural machinery equipment in the target working area at the current moment, and the longitudinal error between the follower agricultural machinery equipment and the virtual navigator at the current moment, and determine the speed increment of the follower agricultural machinery equipment at the current moment according to a preset objective function and preset constraints. The objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery equipment and the virtual navigator at the next moment. The constraints may include a preset state equation, which can be used to characterize the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery equipment at the current moment, the driving speed of the follower agricultural machinery equipment at the previous moment, and the relationship between the speed increment of the follower agricultural machinery equipment at the current moment. Finally, the target speed of the follower agricultural machinery equipment at the current moment can be determined according to the speed increment of the follower agricultural machinery equipment at the current moment, and the follower agricultural machinery equipment can be controlled to continue driving in the target working area according to the target speed at the current moment. In this way, the real-time communication between the follower agricultural machinery and the navigator agricultural machinery can be used to obtain the current driving speed of the navigator equipment, and then the speed increment of the follower agricultural machinery at the current moment can be determined according to the kinematic characteristics, objective functions and preset conditions of the follower agricultural machinery and the navigator agricultural machinery. The target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment at the current moment, so as to ensure that the follower agricultural machinery can accurately track the predetermined planned path in the agricultural machinery cluster while maintaining the preset formation to operate, thereby reducing the risk of collision and improving the work efficiency of the agricultural machinery cluster operation.

[0183] Furthermore, based on the above Figures 1 to 13 In one or more embodiments of the present specification, a computer program product is provided, including a computer program. When the computer program in the computer program product is executed by a processor, the following process can be implemented:

[0184] Obtaining the current driving speed of the leader agricultural machinery corresponding to the follower agricultural machinery in the target working area, and the longitudinal error between the follower agricultural machinery and the virtual leader at the current moment;

[0185] Determine the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal errors of the follower agricultural machinery and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the speed increment of the follower agricultural machinery at the current moment;

[0186] According to the speed increment of the follower agricultural machinery at the current moment, the target speed of the follower agricultural machinery at the current moment is determined, and the follower agricultural machinery is controlled to continue traveling in the target working area according to the target speed at the current moment.

[0187] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the above-mentioned computer program product embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0188] The embodiments of the present specification provide a computer program product, which can obtain the driving speed of the navigator agricultural machinery equipment corresponding to the follower agricultural machinery equipment in the target working area at the current moment, and the longitudinal error between the follower agricultural machinery equipment and the virtual navigator at the current moment, and determine the speed increment of the follower agricultural machinery equipment at the current moment according to a preset objective function and preset constraints. The objective function is used to minimize the predicted longitudinal error of the follower agricultural machinery equipment and the virtual navigator at the next moment. The constraints may include a preset state equation, which can be used to characterize the predicted longitudinal error at the next moment, the longitudinal error at the current moment, the preset sampling time, the driving speed of the navigator agricultural machinery equipment at the current moment, the driving speed of the follower agricultural machinery equipment at the previous moment, and the relationship between the speed increment of the follower agricultural machinery equipment at the current moment. Finally, the target speed of the follower agricultural machinery equipment at the current moment can be determined according to the speed increment of the follower agricultural machinery equipment at the current moment, and the follower agricultural machinery equipment can be controlled to continue driving in the target working area according to the target speed at the current moment. In this way, the real-time communication between the follower agricultural machinery and the navigator agricultural machinery can be used to obtain the current driving speed of the navigator equipment, and then the speed increment of the follower agricultural machinery at the current moment can be determined according to the kinematic characteristics, objective functions and preset conditions of the follower agricultural machinery and the navigator agricultural machinery. The target speed of the follower agricultural machinery at the current moment can be determined according to the speed increment at the current moment, so as to ensure that the follower agricultural machinery can accurately track the predetermined planned path in the agricultural machinery cluster while maintaining the preset formation to operate, thereby reducing the risk of collision and improving the work efficiency of the agricultural machinery cluster operation.

[0189] The above is a description of a specific embodiment of the specification. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recorded in the claims can be performed in an order different from that in the embodiments and still achieve the desired results. In addition, the processes depicted in the drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0190] In the 1990s, improvements to a technology could be clearly distinguished as hardware improvements (for example, improvements to the circuit structure of diodes, transistors, switches, etc.) or software improvements (improvements to the method flow). However, with the development of technology, many improvements to the method flow today can be regarded as direct improvements to the hardware circuit structure. Designers almost always obtain the corresponding hardware circuit structure by programming the improved method flow into the hardware circuit. Therefore, it cannot be said that an improvement in a method flow cannot be implemented using a hardware entity module. For example, a programmable logic device (PLD) (such as a field programmable gate array (FPGA)) is such an integrated circuit whose logical function is determined by the user's programming of the device. Designers can "integrate" a digital system on a PLD by programming it themselves, without having to ask a chip manufacturer to design and produce a dedicated integrated circuit chip. Moreover, nowadays, instead of manually making integrated circuit chips, this kind of programming is mostly implemented by "logic compiler" software, which is similar to the software compiler used when developing and writing programs, and the original code before compilation must also be written in a specific programming language, which is called hardware description language (HDL). There is not only one HDL, but many kinds, such as ABEL (Advanced Boolean Expression Language), AHDL (Altera Hardware Description Language), Confluence, CUPL (Cornell University Programming Language), HDCal, JHDL (Java Hardware Description Language), Lava, Lola, MyHDL, PALASM, RHDL (Ruby Hardware Description Language), etc. The most commonly used ones are VHDL (Very-High-Speed ​​Integrated Circuit Hardware Description Language) and Verilog. Those skilled in the art should also know that it is only necessary to program the method flow slightly in the above-mentioned hardware description languages ​​and program it into the integrated circuit, and then it is easy to obtain the hardware circuit that implements the logic method flow.

[0191] The systems, devices, modules or units described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0192] For the convenience of description, the above devices are described in terms of functions and are divided into various units. Of course, when implementing one or more embodiments of this specification, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0193] Those skilled in the art will appreciate that the embodiments of this specification may be provided as methods, systems, or computer program products. Therefore, one or more embodiments of this specification may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, one or more embodiments of this specification may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0194] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable fraud case serial and parallel device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0195] These computer program instructions may also be loaded onto a computer or other programmable device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0196] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0197] The memory may include non-permanent storage in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0198] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0199] It should be understood by those skilled in the art that the embodiments of this specification may be provided as methods, systems or computer program products. Therefore, one or more embodiments of this specification may take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware. Moreover, one or more embodiments of this specification may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0200] One or more embodiments of the present specification may be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. One or more embodiments of the present specification may also be practiced in distributed computing environments where tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules may be located in local and remote computer storage media, including storage devices.

[0201] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0202] The above description is only an embodiment of this specification and is not intended to limit this document. For those skilled in the art, this specification may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of this specification should be included in the scope of the claims of this specification.

Claims

1. A method for controlling agricultural machinery, the method being applied to a follower agricultural machinery, the method comprising: Acquire the current driving speed of the leader agricultural machinery corresponding to the follower agricultural machinery in the target working area, and the longitudinal error between the follower agricultural machinery and the virtual leader at the current moment; Determine the speed increment of the follower agricultural machinery at the current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal errors of the follower agricultural machinery and the virtual navigator at the next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the follower agricultural machinery at the previous moment, and the speed increment of the follower agricultural machinery at the current moment; According to the speed increment of the follower agricultural machinery at the current moment, the target speed of the follower agricultural machinery at the current moment is determined, and the follower agricultural machinery is controlled to continue traveling in the target working area according to the target speed at the current moment.

2. The method according to claim 1, wherein obtaining the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment comprises: Acquire the first position information of the navigator agricultural machinery in the target working area at the current moment; Determine the path index point of the virtual navigator at the current moment according to the path index point in the first position information, the expected longitudinal distance of the virtual navigator at the current moment, and the preset distance between two adjacent path index points in the preset planned path of the follower agricultural machinery equipment; According to the path index point of the follower agricultural machinery at the current moment and the path index point of the virtual navigator at the current moment, the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment is determined.

3. The method according to claim 2, wherein the target speed of the follower agricultural machinery at the next moment is determined according to the speed increment of the follower agricultural machinery at the current moment, and the follower agricultural machinery is controlled to continue to travel in the target working area according to the target speed at the current moment, comprising: According to the speed increment of the follower agricultural machinery at the current moment, a preset model predictive control algorithm is used to determine a first speed of the follower agricultural machinery at the current moment; Determine the lateral coordinates and heading angle of the virtual navigator at the current moment according to the first position information, the expected lateral distance and the expected heading angle of the virtual navigator at the current moment; Determine the lateral error and heading angle error of the follower agricultural machinery and the virtual navigator at the current moment according to the lateral coordinate and heading angle of the follower agricultural machinery at the current moment, and the lateral coordinate and heading angle of the virtual navigator at the current moment; According to the longitudinal error at the current moment, the longitudinal error and the heading angle error, and the first speed, the preset model predictive control algorithm is used to determine the target speed and target turning angle of the follower agricultural machinery at the current moment, and the follower agricultural machinery is controlled to continue traveling in the target working area according to the target speed and the target turning angle at the current moment.

4. The method according to claim 1, wherein the preset state equation is e y,k+1 =e y,k +T*(V L,k -(V F,k-1 +ΔV F,k )),in, e y,k+1 is the predicted longitudinal error at the next moment, e y,k is the longitudinal error at the current moment, T is the preset sampling time, V L,k is the driving speed of the pilot agricultural machinery at the current moment, V F,k-1 is the driving speed of the follower agricultural machinery at the previous moment, ΔV F,k is the speed increment of the follower agricultural machinery at the current moment.

5. According to the method described in claim 4, the preset constraints also include constraints on the predicted longitudinal error at the next moment, constraints on the speed increment of the follower agricultural machinery at the current moment, and constraints on the driving speed of the follower agricultural machinery at the current moment.

6. The method according to claim 5, wherein the objective function is in, N is the length of the prediction time domain, Q is the first preset weight matrix, R is the second preset weight matrix, e y,k is the longitudinal error at the current moment, ΔV F,k is the speed increment of the follower agricultural machinery at the current moment.

7. The method according to claim 2, wherein the obtaining the current driving speed of the leader agricultural machinery corresponding to the follower agricultural machinery in the target working area and the longitudinal error between the follower agricultural machinery and the virtual leader at the current moment comprises: When the path index point of the follower agricultural machinery at the current moment is different from the path index point of the virtual navigator at the current moment, the driving speed of the navigator agricultural machinery corresponding to the follower agricultural machinery in the target working area at the current moment and the longitudinal error between the follower agricultural machinery and the virtual navigator at the current moment are obtained.

8. An agricultural machinery control device, comprising: A data acquisition module, used to acquire the current driving speed of the navigator agricultural machinery corresponding to the agricultural machinery control device in the target working area, and the longitudinal error between the agricultural machinery control device and the virtual navigator at the current moment; an increment determination module, for determining a speed increment of the agricultural machinery control device at a current moment according to a preset objective function and preset constraints, wherein the objective function is used to minimize the predicted longitudinal errors of the agricultural machinery control device and the virtual navigator at a next moment, and the constraints include a preset state equation, wherein the preset state equation is used to characterize the relationship between the predicted longitudinal error at the next moment, the longitudinal error at the current moment, a preset sampling time, the driving speed of the navigator agricultural machinery at the current moment, the driving speed of the agricultural machinery control device at a previous moment, and the speed increment of the agricultural machinery control device at the current moment; The speed determination module is used to determine the target speed of the agricultural machinery control device at the current moment according to the speed increment of the agricultural machinery control device at the current moment, and control the agricultural machinery control device to continue traveling in the target working area according to the target speed at the current moment.

9. An agricultural machinery control device, characterized in that: The method comprises a processor, a memory and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the steps of the agricultural machinery control method as claimed in any one of claims 1 to 7.

10. A computer program product, characterized in that It comprises a computer program, which, when executed by a processor, implements the steps of the agricultural machinery control method according to any one of claims 1 to 7.

Images