Agricultural machine autonomous navigation obstacle avoidance method and device, unmanned agricultural machine

Abstract

The application provides an autonomous navigation obstacle avoidance method and device for agricultural machinery and unmanned agricultural machinery. The method comprises the following steps: acquiring obstacle information of an agricultural machinery operation area and self-state information of the agricultural machinery; based on the obstacle information and the self-state information, a global path planning algorithm based on random sampling is used to perform path searching in the agricultural machinery operation area, so as to generate an initial path; and the initial path is subjected to smoothing treatment by introducing kinematic constraints and minimum turning radius constraints of the agricultural machinery, so as to generate a smooth reference path. The method combines global path planning and local obstacle avoidance control, and effectively improves the operation efficiency and safety of the unmanned agricultural machinery in a complex farmland environment.

Description

Technical Field

[0001] This invention relates to the field of smart agriculture and automated control technology, and in particular to a method and device for autonomous navigation and obstacle avoidance of agricultural machinery, and unmanned agricultural machinery. Background Technology

[0002] Unmanned agricultural machinery is now widely used in all aspects of unmanned farm operations, ensuring the safe and reliable conduct of agricultural tasks. Path planning is an important component of agricultural machinery automatic navigation technology research. Agricultural machinery mainly uses technologies such as satellite positioning, automated control, and operational navigation to perform autonomous field operations, turning at the edge of the field, U-turns, and obstacle avoidance. After acquiring a high-precision semantic map of the farmland, the shortest path from the starting point to the destination without colliding with obstacles is generated.

[0003] Existing path planning algorithms include: Search-based algorithms: Dijkstra, A*, and D* algorithms, which intuitively reduce the probability of getting trapped in local optima through average partitioning, suitable for static path planning in small local areas; however, their computation time increases exponentially with the expansion of the search area, failing to meet the real-time path planning requirements of unstructured large-scale field environments. Mathematical model-based algorithms: linear algorithms and optimal control, which obtain optimal solutions by modeling a given environment and dynamic system and utilizing constraints; however, they require a significant amount of time to model environmental conditions and calculate constraints, resulting in excessive modeling costs and computational overhead in large-scale, dynamic, and highly uncertain farmland environments, leading to poor engineering practicality. Sampling-based algorithms: Probabilistic Roadmap (PRM), Rapid Exploration Randomized Tree (RRT), and their variants, but their global path planning can only be completed in known static environments. However, in most navigation processes, the environment and obstacles are dynamically changing. When agricultural machinery executes tasks based on global path planning, it must utilize various onboard devices, such as lidar, depth cameras, and IMUs, to perceive the surrounding environment and its own status, and quickly plan a collision-free local path. However, the aforementioned existing technologies cannot meet the needs of agricultural machinery for real-time perception and rapid obstacle avoidance in dynamic and unknown environments. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention proposes an autonomous navigation and obstacle avoidance method and device for agricultural machinery, as well as an unmanned agricultural machine. This method combines global and local path planning to achieve obstacle avoidance, effectively improving the operational efficiency and safety of unmanned agricultural machines in complex field environments.

[0005] This invention provides a method for autonomous navigation and obstacle avoidance of agricultural machinery, comprising:

[0006] Obtain information on obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself;

[0007] Based on the obstacle information and its own state information, a global path planning algorithm based on random sampling is used to search for paths in the agricultural machinery operation area and generate an initial path.

[0008] By introducing kinematic constraints and minimum turning radius constraints for agricultural machinery, the initial path is smoothed to generate a smooth reference path.

[0009] In one embodiment of the present invention, the method further includes: using an MPC strategy, combined with an agricultural machinery dynamics model, to control the agricultural machinery to track the smooth reference path.

[0010] In one embodiment of the present invention, the step of acquiring information about the surrounding environment of the agricultural machinery and the agricultural machinery's own status includes:

[0011] Obtain distance information of obstacles using obstacle avoidance radar;

[0012] The camera acquires visual image information of obstacles to identify their type, size, and spatial pose.

[0013] The obstacle information includes the obstacle's distance, type, size, and spatial pose;

[0014] The position, heading angle, acceleration, and angular velocity information of agricultural machinery are obtained through a GNSS / INS integrated navigation system.

[0015] The angle sensor is used to obtain the front wheel rotation angle information of the agricultural machinery.

[0016] The self-state information includes the position, heading angle, acceleration and angular velocity information of the agricultural machinery, and front wheel rotation angle information.

[0017] In one embodiment of the present invention, the global path planning algorithm based on random sampling adopts an improved fast exploratory random tree (RRT*) algorithm;

[0018] The step of generating an initial path by searching for a path in the agricultural machinery operation area based on the obstacle information and the machine's own state information using a global path planning algorithm based on random sampling includes:

[0019] Based on the obstacle information, the agricultural machinery operation area is divided into feasible space and obstacle space, and the obstacle space is marked as an impassable area;

[0020] Using the starting position of the agricultural machinery as the starting point, sampling points are randomly selected in the feasible space;

[0021] Traverse the nodes in the random tree and determine the node closest to the sampling point as the nearest node;

[0022] A new node is generated by extending the nearest node toward the sampling point by a specified step size. Collision detection is performed on the path between the nearest node and the new node based on the obstacle information. If the path intersects with the obstacle space, the new node is discarded and resampling is performed.

[0023] If the path does not intersect with the obstacle space, search for candidate parent nodes within the specified distance range of the new node, calculate the cost from each candidate parent node to the new node, and select the candidate parent node that minimizes the total cost from the starting point to the new node as the parent node of the new node.

[0024] Add the new node to the random tree and search for neighboring nodes within a specified range. Calculate whether using the new node as the parent node of each neighboring node can reduce the cost from the starting point to each neighboring node. If so, update the parent node of the corresponding neighboring node to the new node.

[0025] Repeat the above process until the target point is contained in the random tree, then backtrack the parent node to generate the initial path.

[0026] In one embodiment of the present invention, the agricultural machinery is simplified as a two-wheeled vehicle model, and the agricultural machinery state vector is defined. ,in The yaw angle is defined by taking the coordinates of the midpoint of the rear axle to define the coordinates (x, y) of the agricultural machine in two-dimensional space.

[0027] Define control vector ,in For speed control, and , Here, v is the steering angle control value, and v is the speed. For steering angle, ;

[0028] Define the motion primitives as the arc segment L that turns left with the minimum turning radius, the arc segment R that turns right, and the straight line segment S that goes straight.

[0029] Based on the aforementioned agricultural machinery state vector, control vector, and motion primitives, the kinematic model of the agricultural machinery in two-dimensional space is constructed as follows:

[0030] , ,

[0031] Where ω is the yaw rate and the minimum turning radius. L1 is the length between the front and rear axles of the agricultural machinery.

[0032] In one embodiment of the present invention, a smooth reference path is generated by smoothing the initial path using a smooth curve composed of arc segments and straight line segments, comprising:

[0033] The connection paths between adjacent nodes in the initial path are replaced with smooth curves composed of various motion primitives. The smooth curves include a first type of curve composed of arc segments-straight line segments-arc segments, and a second type of curve composed of arc segments-arc segments-arc segments. The motion primitive combination methods include RSR, RSL, LSR, LSL, RLR, and LRL, where RSR, RSL, LSR, and LSL belong to the first type of curve, and RLR and LRL belong to the second type of curve.

[0034] Calculate the path length for each combination of motion primitives, and take the combination with the smallest path length as the smooth motion trajectory between adjacent nodes;

[0035] The smooth reference path is obtained by tracing back the smooth motion trajectories between all adjacent nodes in the initial path.

[0036] In one embodiment of the present invention, the step of controlling the agricultural machinery to track the smooth reference path using an MPC strategy combined with an agricultural machinery dynamics model includes:

[0037] A simplified 3-DOF single-track model was used to model the lateral and longitudinal dynamics of the agricultural machinery, and a dynamic model of the agricultural machinery was constructed. The state variables of the model include lateral trajectory error and relative yaw angle.

[0038] The lateral trajectory error and relative yaw angle are used as input variables of the model predictive controller, and the open-loop optimal control problem is solved in each control cycle. The objective function of the open-loop optimal control problem is to minimize the sum of the weighted norms of the input variables in the prediction time domain and the weighted error between the predicted output and the reference output.

[0039] The first control variable in the optimal control sequence obtained by solving is applied to the agricultural machinery to drive the agricultural machinery to travel along the smooth reference path.

[0040] In one embodiment of the present invention, a simplified 3-DOF single-track model is used to perform lateral and longitudinal dynamic modeling of agricultural machinery. The steps for constructing the agricultural machinery dynamic model include:

[0041] Define lateral trajectory error derivative and relative yaw angle for:

[0042]

[0043]

[0044] in, Let be the lateral velocity of the agricultural machinery in a fixed reference frame. This represents the heading angle of the agricultural machinery relative to the nearest trajectory segment. The angle of the front wheels relative to the vehicle, and , ;

[0045] Define the derivative of the heading angle as:

[0046]

[0047] in, Let be the yaw rate, and a and b be the distances from the center of gravity to the front and rear wheels, respectively. The longitudinal velocity of agricultural machinery in a fixed reference frame;

[0048] The equations of motion for the longitudinal and lateral dynamics of agricultural machinery are established as follows:

[0049]

[0050]

[0051]

[0052] Where m represents the mass of the agricultural machinery. Let be the moment of inertia of the agricultural machinery about its vertical axis. and The components of the force provided by the front and rear wheels in their rolling direction. and These are the lateral forces of the front and rear wheels, respectively.

[0053] Lateral trajectory error and relative yaw angle As state variables, the longitudinal and lateral dynamic motion equations of the agricultural machinery are converted into state-space model equations as follows:

[0054]

[0055] in, , These are longitudinal acceleration and longitudinal jerk, respectively. For lateral acceleration, It is a time constant. , These are the lateral stiffness coefficients of the front and rear wheels, respectively. This is the gain coefficient. .

[0056] In one embodiment of the present invention, within each control cycle, the lateral trajectory error is... and relative yaw angle As input to the model predictive controller, the motion state of the agricultural machinery in the prediction time domain is predicted based on the state-space model equations. The open-loop optimal control problem is then solved to obtain the optimal control sequence in the control time domain, where:

[0057] The objective function to be optimized is:

[0058]

[0059] State prediction equation:

[0060] Initial state:

[0061] Output equation:

[0062] Predicted output:

[0063] Reference output:

[0064] Control constraints:

[0065] Longitudinal acceleration constraint:

[0066] Steering angle constraint:

[0067] in, Let cost function be To control the quantity, Let be the predicted value of the j-th output object at time k for the i-th prediction level step. Let be the reference value for the j-th output object at time k for the i-th prediction level step. The output error is weighted diagonal matrix. To control the change of the weighted diagonal matrix, To output the number of variables, To control the number of variables, To predict the time domain, To control the time domain; , Let be the state vectors for the (j+1)th and jth predicted level steps at time k, respectively. Let be the control input for the j-th prediction level step at time k. Let A be the perturbation at time k for the j-th prediction level step, and let A be the state matrix. For the control matrix, Let C be the perturbation matrix and C be the output matrix. To control the quantity limit, For the maximum longitudinal acceleration, Maximum steering angle, This is a reference value for longitudinal velocity.

[0068] In another aspect, the present invention provides an autonomous navigation and obstacle avoidance device for agricultural machinery, comprising:

[0069] The sensing module is used to acquire information about obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself.

[0070] The global path planning module is used to perform path search in the agricultural machinery operation area based on the obstacle information and its own state information, and generate an initial path through a global path planning algorithm based on random sampling.

[0071] The path smoothing module is used to introduce agricultural machinery kinematic constraints and minimum turning radius constraints to smooth the initial path and generate a smooth reference path;

[0072] The MPC control module is used to control the agricultural machinery to track the smooth reference path by adopting the MPC strategy and combining it with the agricultural machinery dynamics model.

[0073] Another aspect of the present invention provides an unmanned agricultural machine, characterized in that it comprises:

[0074] Body

[0075] The environmental perception system includes an obstacle avoidance radar and a depth camera. The obstacle avoidance radar is installed at the front of the aircraft to collect the distance and position information of obstacles in real time. The depth camera is installed at the front of the aircraft to collect visual image information of obstacles.

[0076] The positioning system, including a GNSS / INS integrated navigation system, is installed on the aircraft to acquire the position, heading angle, and speed information of the agricultural machinery;

[0077] The vehicle-mounted computing platform, installed in the driver's cab, incorporates an autonomous navigation and obstacle avoidance device for agricultural machinery. This device includes:

[0078] The sensing module is used to acquire information about obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself.

[0079] The global path planning module is used to perform path search in the agricultural machinery operation area based on the obstacle information and its own state information, and generate an initial path through a global path planning algorithm based on random sampling.

[0080] The path smoothing module is used to introduce agricultural machinery kinematic constraints and minimum turning radius constraints to smooth the initial path and generate a smooth reference path;

[0081] The MPC control module is used to generate decision control commands by adopting the MPC strategy and combining it with the agricultural machinery dynamics model to control the agricultural machinery to track the smooth reference path;

[0082] The control actuator includes an electronically controlled steering wheel and an angle sensor. The electronically controlled steering wheel receives decision control commands from the on-board computing platform and adjusts the steering angle of the front wheels through a built-in motor to regulate the travel state of the agricultural machinery and track the smooth reference path. The angle sensor is installed at the steering mechanism of the machine body to monitor and provide feedback on the actual deflection angle of the wheels in real time.

[0083] As can be seen from the above solutions, the advantages of the present invention are:

[0084] This invention discloses an autonomous navigation and obstacle avoidance method for agricultural machinery, which perceives the surrounding environment and acquires the machinery's own status in real time. During the path planning phase, a global path search is performed using an improved RRT* algorithm. The Extend* process is used to reselect the parent node with the lowest cost, and the tree structure is continuously optimized through the Rewire process to ensure asymptotic optimality of the path. Addressing the zigzag path problem caused by agricultural machinery dynamic constraints, the machinery is modeled as a Dubins vehicle model, introducing a minimum turning radius constraint. The growth path of RRT* is replaced with a Dubins curve composed of left turns, right turns, and straight-line movement, thereby generating a smooth motion trajectory that conforms to physical constraints. An MPC model predictive control strategy is employed, based on a simplified 3-DOF single-track model. By solving the open-loop optimal control problem, the speed and front wheel steering angle commands of the machinery are calculated in real time, achieving high-precision tracking of the reference path. This invention combines global path planning with local obstacle avoidance control, effectively improving the operational efficiency and safety of unmanned agricultural machinery in complex farmland environments. Attached Figure Description

[0085] Figure 1 This is a structural block diagram of an unmanned agricultural machine provided in an embodiment of the present invention;

[0086] Figure 2 This is a schematic diagram of the overall process of an agricultural machinery autonomous navigation and obstacle avoidance method provided in an embodiment of the present invention;

[0087] Figure 3 A schematic diagram of the RRT algorithm;

[0088] Figure 4 This is a diagram illustrating the Extend* process;

[0089] Figure 5A diagram illustrating the Rewire process;

[0090] Figure 6 for Figure 2 A detailed flowchart of step S2 is shown below;

[0091] Figure 7 A schematic diagram of a Dubins vehicle model;

[0092] Figure 8 This is a schematic diagram of the configuration;

[0093] Figure 9 This is a schematic diagram of an arc and a direction angle;

[0094] Figure 10 A schematic diagram of 6 Dubins paths;

[0095] Figure 11 for Figure 2 A detailed flowchart of step S4 is shown below;

[0096] Figure 12 A simplified model diagram of a 3-DOF monorail;

[0097] Figure 13 This is a schematic diagram illustrating the definition of the control variables relative to the reference trajectory;

[0098] Figure 14 A schematic diagram of the simulation results of the Dubins-RRT* algorithm of this invention;

[0099] Figure 15 A block diagram of the overall structure of an agricultural machinery autonomous navigation and obstacle avoidance device provided in an embodiment of the present invention is shown.

[0100] The attached figures are labeled as follows:

[0101] 10: Vehicle body;

[0102] 20: Environmental sensing system;

[0103] 30: Positioning system;

[0104] 40: Control the actuator;

[0105] 50: In-vehicle computing platform;

[0106] 600: Agricultural machinery autonomous navigation and obstacle avoidance device;

[0107] 610: Sensing module;

[0108] 620: Global Path Planning Module;

[0109] 630: Path smoothing module;

[0110] 640: MPC control module. Detailed Implementation

[0111] It should be noted that, in this invention, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus.

[0112] In the absence of further restrictions, an element defined by the phrase "comprising a..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0113] Example 1

[0114] One embodiment of the present invention provides an unmanned agricultural machine, see reference. Figure 1 As shown, Figure 1 A structural block diagram of an unmanned agricultural machine provided in an embodiment of the present invention is shown.

[0115] An unmanned agricultural machine, comprising:

[0116] Vehicle body 10,

[0117] The environmental perception system 20 includes an obstacle avoidance radar and a depth camera, used to acquire multi-source information about the agricultural machinery's operating area in real time. The obstacle avoidance radar, installed at the front of the machine, is responsible for scanning obstacles along the forward path in real time, providing distance and orientation data for local path planning. The depth camera, also installed at the front, is used to acquire high-precision semantic images of the field. Through visual recognition and classification of obstacles, it identifies the type, size, and spatial pose of obstacles, assisting the system in solving complex spatial environmental features.

[0118] The positioning system 30 includes a GNSS / INS integrated navigation system. This GNSS / INS integrated navigation system can be composed of a GNSS antenna and receiver, and an INS inertial navigation system, or it can be a GNSS / INS integrated navigator. It is responsible for real-time calculation of the absolute attitude and motion state of the agricultural machinery, providing a reference for autonomous operation. If the GNSS antenna and receiver / INS inertial navigation system is used, the GNSS antenna and receiver are located on the top of the cab and inside the machine body, respectively, and are responsible for receiving satellite signals to obtain the absolute coordinates of the agricultural machinery in the field. The INS inertial navigation system is installed on the top of the cab and is used to sense changes in the acceleration and angular velocity of the agricultural machinery, maintaining attitude awareness when satellite signals are weak. If an integrated measuring instrument, such as a GNSS / INS integrated navigator, is used, the GNSS / INS integrated navigator is installed inside the machine body and fuses satellite positioning and inertial measurement data through a deep coupling algorithm to output continuous, stable, and high-precision latitude, longitude, heading angle, and velocity information.

[0119] The control actuator 40, including an electronically controlled steering wheel and an angle sensor, adjusts the agricultural machinery's movement according to decision commands to achieve precise trajectory tracking. The electronically controlled steering wheel replaces the traditional mechanical steering system, receiving control commands from the onboard computing platform and precisely adjusting the front wheel steering angle via a built-in motor. The angle sensor, installed at the steering mechanism, monitors and provides real-time feedback on the actual wheel deflection angle, forming a closed loop with the control commands to ensure precise steering control.

[0120] The vehicle-mounted computing platform 50 is installed in the driver's cab and integrates the Dubins-RRT* path planning algorithm and MPC control strategy. It receives multi-source data from the perception system and positioning system in real time, calculates the lateral trajectory error and relative yaw angle, and issues operating speed and steering commands to the control actuator. Detailed technical details will be explained in Embodiment 2.

[0121] In this embodiment, the deep collaboration between the hardware platform's sensing, positioning, and execution mechanisms effectively solves technical challenges such as "lack of perception, jagged paths, and mismatched dynamic constraints" in complex farmland environments for unmanned agricultural machinery. By combining the physical constraints of the Dubins curve with the dynamic correction of the MPC controller, smooth transitions and precise alignment of the agricultural machinery during obstacle avoidance operations are achieved, significantly improving operational efficiency while greatly enhancing the safety of unmanned agricultural tasks.

[0122] Example 2

[0123] refer to Figure 2 As shown, Figure 2 The diagram shows the overall flow of an agricultural machinery autonomous navigation and obstacle avoidance method according to an embodiment of the present invention.

[0124] A method for autonomous navigation and obstacle avoidance of agricultural machinery includes the following steps:

[0125] Step S1: Obtain information on obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself.

[0126] Step S2: Based on the obstacle information and its own state information, a path search is performed in the agricultural machinery operation area using a global path planning algorithm based on random sampling to generate an initial path.

[0127] Step S3: Introduce agricultural machinery kinematic constraints and minimum turning radius constraints to smooth the initial path and generate a smooth reference path.

[0128] Step S4: Using the MPC strategy and combining it with the agricultural machinery dynamics model, control the agricultural machinery to track the smooth reference path.

[0129] In one embodiment, obstacle avoidance radar is used to acquire distance information of obstacles; a camera is used to acquire visual image information of obstacles, and the type, size, and spatial pose of obstacles are identified. The obstacle information includes the distance, type, size, and spatial pose of the obstacles.

[0130] The position, heading angle, acceleration, and angular velocity information of the agricultural machinery are obtained through a GNSS / INS integrated navigation system; the front wheel rotation angle information of the agricultural machinery is obtained through an angle sensor; wherein, the self-state information includes the position, heading angle, acceleration and angular velocity information, and front wheel rotation angle information of the agricultural machinery.

[0131] In one embodiment, in step S2, the global path planning algorithm based on random sampling employs an improved Fast Exploratory Random Tree (RRT) algorithm. The improved RRT algorithm, based on the traditional RRT algorithm, improves upon this by using Extend* and Rewire to re-select the parent node. Through this dual optimization, the algorithm maintains probabilistic integrity while ensuring the asymptotic optimality of the obstacle avoidance path, significantly reducing redundant nodes and ineffective turns in the path.

[0132] Rapid Exploration Randomized Tree (RRT) has proven to be a highly efficient global path planning algorithm. The RRT algorithm constructs a node tree in a built space, starting from the starting point and progressively planning towards the target point, using randomly generated nodes. This allows for fast and simple path planning. The algorithm finds the nearest node to the random point in the random tree and then extends one step towards it. If the extended step does not lead to successful navigation through obstacles, new nodes are added to the random tree sequentially until the target point is also in the random tree and a path is found, thus completing the path planning process. (Reference) Figure 3 As shown, Figure 3A schematic diagram of the RRT algorithm is shown. The RRT algorithm mainly consists of the following four steps: 1. Start: Initialization settings (starting point p) 起点 , endpoint p 目标点 1. Step size l); 2. Sampling function: Randomly select a point p in space. 随机点 3. Find the distance from p 随机点 The most recent node, starting from the first root node p. 最近点 ;4. p 随机点 and p 最近点 The connecting lines serve as the growth direction of the tree, and then a new node p is generated. 新节点 If the tree encounters an obstacle during its growth, the growth attempt is abandoned. Condition: Compare p 新节点 to p 目标点 The distance and the step size l, if p 新节点 to p 目标点 If the distance is less than the step size l, stop the tree growth and generate a path; otherwise, repeat steps 2 and 3 for all nodes (root node and other nodes) until a path is generated.

[0133] However, the traditional RRT algorithm randomly selects sampling points when searching for a path, which makes its search efficiency low, and the planned path contains a lot of redundant nodes and redundant road segments. Therefore, the RRT algorithm needs to be improved.

[0134] refer to Figure 4 , Figure 5 As shown in Figure 4, this is a schematic diagram illustrating the Extend* process. Figure 5 This is a diagram illustrating the Rewire process. The RRT* algorithm incorporates two important improvements over the RRT algorithm, both involving the neighborhood set of the node being added to the tree.

[0135] First improvement Instead of Extend, use Extend* to select the parent node within a new specified distance, thus minimizing the cost of the new node. For example... Figure 4 As shown in Figure (a), the tree structure after Extend is such that the new node E is directly connected to the nearest node D (labeled as...) through the Extend procedure. The weight of each edge is 2. At this time, the total cost from the starting point A to the new node E is 6; as shown in Figure (b), when the new node E (labeled as After adding, with E as the center and radius... Define the neighborhood search range. Within this range, retrieve all existing nodes as candidate parent nodes. The candidate node of the retrieved neighborhood set is C (labeled as C). From graph (c), by connecting nodes C and D, starting from A, the total cost of the path A→C→E is 4.2, and the total cost of the path A→B→D→E is 6. Therefore, the total cost generated by connecting through node C is lower, so connect nodes E and C. The final tree structure after Extend* is shown in graph (d).

[0136] The second improvement, in After a new process called Rewire is added to the generated tree, each node in the generated neighbors is tested by checking whether the new node, instead of the current parent node, increases its cost. If so, the parent node is modified to... Become the new parent node. The tree structure is continuously optimized through the Rewire process to ensure asymptotic optimality of the path. The RRT* algorithm improves upon the RRT algorithm by reselecting the parent node. The path is gradually optimized by continuously adjusting the parent node. The RRT* algorithm is asymptotically optimal, ensuring asymptotic optimality beyond probabilistic integrity. (Reference) Figure 5 As shown, the tree node G (labeled as) after Extend* Add it to the tree, as shown in Figure (a); with the new node G as the center and radius... The neighborhood search range is defined, and three candidate nodes are found: B, E, and F, as shown in Figure (b). Then, as shown in Figure (c), the initial cost of maintaining the original connection and the potential cost of changing the connection to G are calculated for each node: Node B: Initial cost (A→B): 3.0; Potential cost (A→G→B): 2.5 + 3.0 = 5.5, which is greater than 3.0, so the connection is not changed; Node E: Initial cost (A→C→E): Potential costs (A→G→E): If the cost is less than 5.9, reconnect. Node F: Initial cost (A→B→F): Potential costs (A→G→F): If the cost is less than 5.9, a rewire operation is performed. The final rewired tree structure is shown in Figure (d). Node B did not perform a rewire operation because it would not reduce costs, but nodes E and F did perform rewire operations.

[0137] Specifically, participate Figure 6 As shown, Figure 6 A schematic diagram of the specific process for step S2 is shown.

[0138] Based on the obstacle information and its own state information, the improved Fast Exploration Random Tree (RRT*) global path planning algorithm is used to perform path search in the agricultural machinery operation area and generate an initial path. The steps specifically include:

[0139] Step S21: Based on the obstacle information, the agricultural machinery operation area is divided into feasible space and obstacle space, and the obstacle space is marked as an impassable area.

[0140] Step S22: Using the starting position of the agricultural machinery as the starting point, randomly select sampling points in the feasible space.

[0141] Step S23: Traverse the nodes on the random tree and determine the node closest to the sampling point as the nearest node.

[0142] Step S24: Extend the sampling point direction by a specified step size from the nearest node to generate a new node.

[0143] Step S25: Perform collision detection on the path from the nearest node to the new node based on the obstacle information.

[0144] Step S261: If the path is detected to intersect with the obstacle space, discard the new node and resample.

[0145] Step S262: If the path does not intersect with the obstacle space, search for candidate parent nodes within the specified distance range of the new node, calculate the cost from each candidate parent node to the new node, and select the candidate parent node that minimizes the total cost from the starting point to the new node as the parent node of the new node.

[0146] Step S27: Add the new node to the random tree and search for neighboring nodes within a specified range. Calculate whether using the new node as the parent node of each neighboring node can reduce the cost from the starting point to each neighboring node. If so, update the parent node of the corresponding neighboring node to the new node.

[0147] Step S28: Repeat the above process until the random tree contains the target point, and backtrack the parent node to generate the initial path.

[0148] Because the trajectory obtained through RRT or RRT* is jagged (with sharp inflection points), but agricultural machinery in real-world scenarios has dynamic constraints and its steering control is continuous, it is difficult to follow such a trajectory. In order to introduce constraints into the motion of agricultural machinery while reducing the number and complexity of controls, the agricultural machinery is modeled as a Dubins vehicle model.

[0149] Specifically, the agricultural machinery is simplified into the Dubins two-wheeled vehicle model. Figure 7This is a schematic diagram of the Dubins vehicle model. The state vector of the agricultural machinery is defined. ,in Let the yaw angle be the coordinates of the midpoint of the rear axle, defining the coordinates (x, y) of the agricultural machinery in two-dimensional space. Define the control vector. ,in For speed control, and , Here, v is the steering angle control value, and v is the speed. For steering angle, Define the motion primitives as the circular arc segment L turning left with the minimum turning radius, the circular arc segment R turning right, and the straight line segment S.

[0150] Based on the aforementioned agricultural machinery state vector, control vector, and motion primitives, the kinematic model of the agricultural machinery in two-dimensional space is constructed as follows:

[0151] , ,

[0152] Where ω is the yaw rate and the minimum turning radius. L1 is the length between the front and rear axles of the agricultural machinery.

[0153] The Dubins vehicle model discretizes the motion of agricultural machinery into motion primitives consisting of left-turning arc segments (L), right-turning arc segments (R), and straight line segments (S), ensuring that each planned trajectory is physically feasible and effectively solving the problem of difficult tracking of agricultural machinery caused by the jagged trajectories generated by traditional sampling algorithms.

[0154] A smooth reference path is generated by smoothing the initial path using a smooth curve composed of arc segments and straight line segments. Specifically, the connection paths between adjacent nodes in the initial path are replaced with smooth curves composed of various motion primitives. The smooth curves include a first type of curve composed of arc segments-straight line segments-arc segments, and a second type of curve composed of arc segments-arc segments-arc segments. The motion primitive combinations include RSR, RSL, LSR, LSL, RLR, and LRL. RSR, RSL, LSR, and LSL belong to the first type of curves, while RLR and LRL belong to the second type of curves, i.e., first turning in one direction, then turning in the opposite direction, and then turning back to the original direction. The path length for each motion primitive combination is calculated, and the combination with the shortest path length is taken as the smooth motion trajectory between adjacent nodes. The smooth reference path is obtained by tracing back the smooth motion trajectories between all adjacent nodes in the initial path.

[0155] In a specific implementation, see Figure 8, 9 10, Figure 8 (a) is a schematic diagram of the initial configuration, and (b) is a schematic diagram of the final configuration. Figure 9 This is a schematic diagram of an arc and a direction angle; Figure 10 There are 6 types of Dubins paths (RSR, RSL, LSR, LSL, RLR, LRL).

[0156] Let there be three corresponding operators. Turn left. Turn right. Go straight, using three operators to connect any point Convert to its in The corresponding image points in the image.

[0157] Equation (1)

[0158] Initial configuration of the model and final configuration In a rectangular coordinate system.

[0159] d is the initial point and the final point The Euclidean distance, with the origin at... , The initial and final orientation angles are and , measured counterclockwise relative to the positive x-axis direction.

[0160] arc Obtaining an arc by symmetry along the x-axis The arc is obtained by center symmetry through point O. Arcs are obtained through combination transformations. For connecting two points , For any path, there exist three other paths that are topologically equivalent, with corresponding orientation angles of . , and .

[0161] Let t be the length of the first path segment, p be the length of the second path segment, and q be the length of the third path segment. Then the total path length is: The initial configuration of Dubins was... The final configuration is .

[0162] (1) RSL transformation expression Using Formula 1, its scalar equation is obtained as follows:

[0163] ,

[0164] The solution to the equation is:

[0165] ,

[0166] The path length is:

[0167]

[0168] (2) Transformation expression of LRL Using Formula 1, the corresponding scalar equation is obtained as follows:

[0169] ,

[0170] The solution to the equation is:

[0171] ,

[0172] The path length is:

[0173]

[0174] (3) LSL transformation expression Using Formula 1, the corresponding scalar equation is obtained as follows:

[0175] ,

[0176] The solution to the equation is:

[0177] ,

[0178] The path length is:

[0179]

[0180] (4) RSR transformation expression Using Formula 1, the corresponding scalar equation is obtained as follows:

[0181] ,

[0182] The solution to the equation is:

[0183] ,

[0184] The path length is:

[0185]

[0186] (5) RSL transformation expression Using Formula 1, the corresponding scalar equation is obtained as follows:

[0187] ,

[0188] The solution to the equation is:

[0189] ,

[0190] The path length is:

[0191]

[0192] (6) Transformation expression of RLR Using Formula 1, the corresponding scalar equation is obtained as follows:

[0193] ,

[0194] The solution to the equation is:

[0195]

[0196] The path length is:

[0197]

[0198] Finally, the path combination with the shortest path length is selected to obtain the Dubins path, which serves as the smooth motion trajectory between adjacent nodes:

[0199]

[0200] By tracing back through all adjacent nodes in the initial path, the initial path is replaced with a smooth motion trajectory to obtain the smooth reference path.

[0201] In step S4, the reference path is tracked by calculating the front wheel steering angle and throttle / brake commands. Combined with the dynamic constraints of the agricultural machinery, the tracking performance of the agricultural machinery's motion is improved. Model Predictive Control (MPC) strategy is applied to calculate the speed and front wheel steering angle commands of the agricultural machinery, realizing lateral and longitudinal dynamics respectively.

[0202] Specifically, refer to Figure 11 As shown, Figure 11 A schematic diagram of the specific process for step S4 is shown.

[0203] Step S41: Use a simplified 3-DOF single-track model to perform lateral and longitudinal dynamic modeling of the agricultural machinery, and construct the agricultural machinery dynamic model. The state variables of the model include lateral trajectory error and relative yaw angle.

[0204] in, Figure 12 This is a simplified schematic diagram of a 3-DOF monorail model. Figure 13This is a schematic diagram illustrating the definition of control variables relative to a reference trajectory. A simplified 3-DOF single-track model is used to model the lateral and longitudinal dynamics of the agricultural machinery. Its equations of motion are based on the reference pose error generated by local path planning. The agricultural machinery's speed... The kinematic motion can be represented by the lateral trajectory error of the front axle guide wheel. The relative yaw angle of the wheel with respect to the nearest segment of the trajectory to be tracked. To describe. The steps for constructing an agricultural machinery dynamics model include:

[0205] Define lateral trajectory error derivative and relative yaw angle for:

[0206]

[0207]

[0208] in, Let be the lateral velocity of the agricultural machinery in a fixed reference frame. This represents the heading angle of the agricultural machinery relative to the nearest trajectory segment. The angle of the front wheels relative to the vehicle, and , .

[0209] Define the derivative of the heading angle as:

[0210]

[0211] in, Let be the yaw rate, and a and b be the distances from the center of gravity (CoG) to the front and rear wheels, respectively. The longitudinal velocity of agricultural machinery in a fixed reference frame;

[0212] The equations of motion for the longitudinal and lateral dynamics of agricultural machinery are established as follows:

[0213]

[0214]

[0215]

[0216] Where m represents the mass of the agricultural machinery. Let be the moment of inertia of the agricultural machinery about its vertical axis. and The components of the force provided by the front and rear wheels in their rolling direction. and These are the lateral forces of the front and rear wheels, respectively.

[0217] Lateral trajectory error and relative yaw angle As state variables, the longitudinal and lateral dynamic motion equations of the agricultural machinery are converted into state-space model equations as follows:

[0218]

[0219] in, , These are longitudinal acceleration and longitudinal jerk, respectively. For lateral acceleration, It is a time constant; , These are the lateral stiffness coefficients of the front and rear wheels, respectively, and the front wheel slip angle. Rear wheel slip angle , , ; This is the gain coefficient. .

[0220] Step S42: Using the lateral trajectory error and relative yaw angle as input variables of the model predictive controller, solve the open-loop optimal control problem in each control cycle; wherein, the objective function of the open-loop optimal control problem is: minimizing the sum of the weighted norms of the input variables in the prediction time domain, and the sum of the weighted error between the predicted output and the reference output.

[0221] Specifically, within each control cycle, the lateral trajectory error is... and relative yaw angle As input to the model predictive controller, the motion state of the agricultural machinery in the prediction time domain is predicted based on the state-space model equations. The open-loop optimal control problem is then solved to obtain the optimal control sequence in the control time domain, where:

[0222] The objective function to be optimized is:

[0223]

[0224] State prediction equation:

[0225] Initial state:

[0226] Output equation:

[0227] Control constraints:

[0228] Longitudinal acceleration constraint:

[0229] Steering angle constraint:

[0230] Predicted output:

[0231] Reference output:

[0232] in, Let cost function be To control the quantity, Let be the predicted value of the j-th output object at time k for the i-th prediction level step. Let be the reference value for the j-th output object at time k for the i-th prediction level step. The output error is weighted diagonal matrix. To control the change of the weighted diagonal matrix, To output the number of variables, To control the number of variables, To predict the time domain, To control the time domain; , Let be the state vectors for the (j+1)th and jth predicted level steps at time k, respectively. Let be the control input for the j-th prediction level step at time k. Let A be the perturbation at time k for the j-th prediction level step, and let A be the state matrix. For the control matrix, Let C be the perturbation matrix and C be the output matrix. To control the quantity limit, For the maximum longitudinal acceleration, Maximum steering angle, This is a reference value for longitudinal velocity.

[0233] Step S43: Apply the first control quantity from the obtained optimal control sequence to the control actuator to drive the agricultural machinery along the smooth reference path. Apply the first control quantity from the optimal control sequence to the control actuator and move the prediction level forward one step. Repeat the prediction and optimization process for the new available agricultural machinery state.

[0234] In this embodiment, a simplified 3-DOF single-track model is used to establish a dynamic model that considers the lateral, longitudinal, and directional dynamic characteristics. The focus is on analyzing the evolution of the distance from the center of gravity to the front and rear axles, as well as the lateral trajectory error and relative yaw angle. The MPC controller solves the open-loop optimal control problem in the predictive time domain and dynamically outputs the front wheel steering angle and throttle / brake commands. Under the premise of satisfying various physical constraints, the deviation between the agricultural machinery's motion state and the reference path is minimized.

[0235] like Figure 14 , Figure 14The diagram shows the simulation results of the Dubins-RRT* algorithm of this invention. Simulation verification shows that in a field scenario containing multiple complex static obstacles, the system can guide agricultural machinery to automatically plan a collision-free and smooth local path, and smoothly complete the obstacle avoidance task with minimal error, fully verifying the effectiveness and robustness of the entire algorithm process.

[0236] In summary, the autonomous navigation and obstacle avoidance method for agricultural machinery provided in this embodiment combines global path planning with local obstacle avoidance control, effectively improving the operational efficiency and safety of unmanned agricultural machinery in complex farmland environments. Specifically, a global path search is performed using an improved RRT* algorithm, the parent node with the minimum cost is reselected using the Extend* process, and the tree structure is continuously optimized through the Rewire process to ensure asymptotic optimality of the path. To address the zigzag path problem caused by agricultural machinery dynamic constraints, the agricultural machinery is modeled as a Dubins vehicle model, a minimum turning radius constraint is introduced, and the growth path of RRT* is replaced with a Dubins curve composed of left turns, right turns, and straight-line movement, thereby generating a smooth motion trajectory that conforms to physical constraints. Furthermore, an MPC model predictive control strategy is adopted, based on a simplified 3-DOF single-track model. By solving the open-loop optimal control problem, the speed and front wheel steering angle commands of the agricultural machinery are calculated in real time, achieving high-precision tracking of the reference path.

[0237] Example 3

[0238] The method embodiments corresponding to the above-described method embodiments are shown in Figure 15. Figure 15 A block diagram of the overall structure of an agricultural machinery autonomous navigation and obstacle avoidance device provided in an embodiment of the present invention is shown.

[0239] An autonomous navigation and obstacle avoidance device 600 for agricultural machinery includes:

[0240] The sensing module 610 is used to obtain information about obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself.

[0241] The global path planning module 620 is used to perform path search in the agricultural machinery operation area based on the obstacle information and its own state information, and generate an initial path through a global path planning algorithm based on random sampling.

[0242] The path smoothing module 630 is used to introduce agricultural machinery kinematic constraints and minimum turning radius constraints to smooth the initial path and generate a smooth reference path.

[0243] The MPC control module 640 is used to control the agricultural machinery to track the smooth reference path by employing the MPC strategy and combining it with the agricultural machinery dynamics model.

[0244] Furthermore, this device embodiment can be implemented in conjunction with the implementation methods described above. The relevant technical details mentioned in the implementation methods of the above embodiments remain valid in the implementation methods of this device embodiment, and will not be repeated here to avoid repetition.

[0245] In addition, one embodiment of the present invention provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the above-described agricultural machinery autonomous navigation and obstacle avoidance method.

[0246] Furthermore, one embodiment of the present invention provides a non-transitory computer-readable storage medium storing a computer program thereon, characterized in that the computer program, when executed by a processor, implements the above-described agricultural machinery autonomous navigation and obstacle avoidance method.

[0247] It should be noted that the scope of the methods in the embodiments of the present invention is not limited to performing functions in the order shown or discussed. For example, the described methods may be performed in a different order than described, and various steps may be applied, omitted, or combined. In addition, features described with reference to certain examples may be combined in other examples.

[0248] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of the present invention without departing from the spirit and scope of the claims, and all of these forms are within the protection scope of the present invention.

Claims

1. A method for autonomous navigation and obstacle avoidance of agricultural machinery, characterized in that, include: Obtain information on obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself; Based on the obstacle information and its own state information, a global path planning algorithm based on random sampling is used to search for paths in the agricultural machinery operation area and generate an initial path. By introducing kinematic constraints and minimum turning radius constraints for agricultural machinery, the initial path is smoothed to generate a smooth reference path.

2. The method according to claim 1, characterized in that, It also includes: using an MPC strategy, combined with an agricultural machinery dynamics model, to control the agricultural machinery to track the smooth reference path.

3. The method according to claim 1, characterized in that, The acquisition of information about the surrounding environment and the agricultural machinery's own status includes: Obtain distance information of obstacles using obstacle avoidance radar; The camera acquires visual image information of obstacles to identify their type, size, and spatial pose. The obstacle information includes the obstacle's distance, type, size, and spatial pose; The position, heading angle, acceleration, and angular velocity information of agricultural machinery are obtained through a GNSS / INS integrated navigation system. The angle sensor is used to obtain the front wheel rotation angle information of the agricultural machinery. The self-state information includes the position, heading angle, acceleration and angular velocity information of the agricultural machinery, and front wheel rotation angle information.

4. The method according to claim 3, characterized in that, The global path planning algorithm based on random sampling adopts an improved fast exploratory random tree (RRT*) algorithm; The step of generating an initial path by searching for a path in the agricultural machinery operation area based on the obstacle information and the machine's own state information using a global path planning algorithm based on random sampling includes: Based on the obstacle information, the agricultural machinery operation area is divided into feasible space and obstacle space, and the obstacle space is marked as an impassable area; Using the starting position of the agricultural machinery as the starting point, sampling points are randomly selected in the feasible space; Traverse the nodes in the random tree and determine the node closest to the sampling point as the nearest node; A new node is generated by extending the nearest node toward the sampling point by a specified step size. Collision detection is performed on the path between the nearest node and the new node based on the obstacle information. If the path intersects with the obstacle space, the new node is discarded and resampling is performed. If the path does not intersect with the obstacle space, search for candidate parent nodes within the specified distance range of the new node, calculate the cost from each candidate parent node to the new node, and select the candidate parent node that minimizes the total cost from the starting point to the new node as the parent node of the new node. Add the new node to the random tree and search for neighboring nodes within a specified range. Calculate whether using the new node as the parent node of each neighboring node can reduce the cost from the starting point to each neighboring node. If so, update the parent node of the corresponding neighboring node to the new node. Repeat the above process until the target point is contained in the random tree, then backtrack the parent node to generate the initial path.

5. The method according to claim 1, characterized in that, The agricultural machinery is simplified into a two-wheeled vehicle model, and the state vector of the agricultural machinery is defined. ,in The yaw angle is defined by taking the coordinates of the midpoint of the rear axle to define the coordinates (x, y) of the agricultural machine in two-dimensional space. Define control vector ,in For speed control, and , Here, v is the steering angle control value, and v is the speed. For steering angle, ; Define the motion primitives as the arc segment L that turns left with the minimum turning radius, the arc segment R that turns right, and the straight line segment S that goes straight. Based on the aforementioned agricultural machinery state vector, control vector, and motion primitives, the kinematic model of the agricultural machinery in two-dimensional space is constructed as follows: ， ， Where ω is the yaw rate and the minimum turning radius. L1 is the length between the front and rear axles of the agricultural machinery.

6. The method according to claim 5, characterized in that, The initial path is smoothed using a smooth curve composed of arc segments and straight line segments to generate a smooth reference path, which includes: The connection paths between adjacent nodes in the initial path are replaced with smooth curves composed of various motion primitives. The smooth curves include a first type of curve composed of arc segments-straight line segments-arc segments, and a second type of curve composed of arc segments-arc segments-arc segments. The motion primitive combination methods include RSR, RSL, LSR, LSL, RLR, and LRL, where RSR, RSL, LSR, and LSL belong to the first type of curve, and RLR and LRL belong to the second type of curve. Calculate the path length for each combination of motion primitives, and take the combination with the smallest path length as the smooth motion trajectory between adjacent nodes; The smooth reference path is obtained by tracing back the smooth motion trajectories between all adjacent nodes in the initial path.

7. The method according to claim 2, characterized in that, The steps of controlling the agricultural machinery to track the smooth reference path using the MPC strategy combined with the agricultural machinery dynamics model include: A simplified 3-DOF single-track model was used to model the lateral and longitudinal dynamics of the agricultural machinery, and a dynamic model of the agricultural machinery was constructed. The state variables of the model include lateral trajectory error and relative yaw angle. The lateral trajectory error and relative yaw angle are used as input variables of the model predictive controller, and the open-loop optimal control problem is solved in each control cycle. The objective function of the open-loop optimal control problem is to minimize the sum of the weighted norms of the input variables in the prediction time domain and the weighted error between the predicted output and the reference output. The first control variable in the optimal control sequence obtained by solving is applied to the agricultural machinery to drive the agricultural machinery to travel along the smooth reference path.

8. The method according to claim 7, characterized in that, A simplified 3-DOF single-track model is used to model the lateral and longitudinal dynamics of agricultural machinery. The steps for constructing the agricultural machinery dynamics model include: Define lateral trajectory error derivative and relative yaw angle for: in, Let be the lateral velocity of the agricultural machinery in a fixed reference frame. This represents the heading angle of the agricultural machinery relative to the nearest trajectory segment. The angle of the front wheels relative to the vehicle, and , ; Define the derivative of the heading angle as: in, Let be the yaw rate, and a and b be the distances from the center of gravity to the front and rear wheels, respectively. The longitudinal velocity of agricultural machinery in a fixed reference frame; The equations of motion for the longitudinal and lateral dynamics of agricultural machinery are established as follows: Where m represents the mass of the agricultural machinery. Let be the moment of inertia of the agricultural machinery about its vertical axis. and The components of the force provided by the front and rear wheels in their rolling direction. and These are the lateral forces of the front and rear wheels, respectively. Lateral trajectory error and relative yaw angle As state variables, the longitudinal and lateral dynamic motion equations of the agricultural machinery are converted into state-space model equations as follows: in, , These are longitudinal acceleration and longitudinal jerk, respectively. For lateral acceleration, It is a time constant. , These are the lateral stiffness coefficients of the front and rear wheels, respectively. This is the gain coefficient.

9. The method according to claim 8, characterized in that, Within each control cycle, the lateral trajectory error is... and relative yaw angle As input to the model predictive controller, the motion state of the agricultural machinery in the prediction time domain is predicted based on the state-space model equations. The open-loop optimal control problem is then solved to obtain the optimal control sequence in the control time domain, where: The objective function to be optimized is: State prediction equation: Initial state: Output equation: Predicted output: Reference output: Control constraints: Longitudinal acceleration constraint: Steering angle constraint: in, Let cost function be To control the quantity, Let be the predicted value of the j-th output object at time k for the i-th prediction level step. Let be the reference value for the j-th output object at time k for the i-th prediction level step. The output error is weighted diagonal matrix. To control the change of the weighted diagonal matrix, To output the number of variables, To control the number of variables, To predict the time domain, To control the time domain; , Let be the state vectors for the (j+1)th and jth predicted level steps at time k, respectively. Let be the control input for the j-th prediction level step at time k. Let A be the perturbation at time k for the j-th prediction level step, and let A be the state matrix. For the control matrix, Let C be the perturbation matrix and C be the output matrix. To control the quantity limit, For the maximum longitudinal acceleration, Maximum steering angle, This is a reference value for longitudinal velocity.

10. An autonomous navigation and obstacle avoidance device for agricultural machinery, characterized in that, include: The sensing module is used to acquire information about obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself. The global path planning module is used to perform path search in the agricultural machinery operation area based on the obstacle information and its own state information, and generate an initial path through a global path planning algorithm based on random sampling. The path smoothing module is used to introduce agricultural machinery kinematic constraints and minimum turning radius constraints to smooth the initial path and generate a smooth reference path; The MPC control module is used to generate decision control commands by employing the MPC strategy and combining it with the agricultural machinery dynamics model to control the agricultural machinery to track the smooth reference path.

11. An unmanned agricultural machine, characterized in that, Include: Body The environmental perception system includes an obstacle avoidance radar and a depth camera. The obstacle avoidance radar is installed at the front of the aircraft to collect the distance and position information of obstacles in real time. The depth camera is installed at the front of the aircraft to collect visual image information of obstacles. The positioning system, including a GNSS / INS integrated navigation system, is installed on the aircraft to acquire the position, heading angle, and speed information of the agricultural machinery; The vehicle-mounted computing platform, installed in the driver's cab, incorporates an autonomous navigation and obstacle avoidance device for agricultural machinery. This device includes: The sensing module is used to acquire information about obstacles in the agricultural machinery operation area and the status information of the agricultural machinery itself. The global path planning module is used to perform path search in the agricultural machinery operation area based on the obstacle information and its own state information, and generate an initial path through a global path planning algorithm based on random sampling. The path smoothing module is used to introduce agricultural machinery kinematic constraints and minimum turning radius constraints to smooth the initial path and generate a smooth reference path; The MPC control module is used to generate decision control commands by adopting the MPC strategy and combining it with the agricultural machinery dynamics model to control the agricultural machinery to track the smooth reference path; The control actuator includes an electronically controlled steering wheel and an angle sensor. The electronically controlled steering wheel receives decision control commands from the on-board computing platform and adjusts the steering angle of the front wheels through a built-in motor to regulate the travel state of the agricultural machinery and track the smooth reference path. The angle sensor is installed at the steering mechanism of the machine body to monitor and provide feedback on the actual deflection angle of the wheels in real time.

Images