Agricultural robot trajectory planning method and system with optimal energy consumption under collision constraint

Abstract

The invention discloses an agricultural robot trajectory planning method and system with optimal energy consumption under collision constraint. The method comprises the following steps: acquiring historical state sequence data and environmental map information of a target machine and peripheral machines in real time by a self-machine; identifying the operation intention of the target machine, and outputting a multi-modal prediction trajectory; the method comprises the following steps of: generating a global coarse reference track covering an operation area by taking completion of an own operation task as a main target; generating a space-time safety corridor by adopting a space-time inheritance strategy, and reserving a safety space for the dynamic obstacle; constructing an optimization problem cost function, and comprehensively considering reference trajectory cost, collision risk cost and energy consumption cost; and under the boundary constraint of the safety corridor and the kinematics constraint of the robot, solving the optimization problem by using a numerical optimization solver to obtain an optimized local trajectory. According to the method, safe, efficient and energy-saving collaborative operation can be realized by fusing multi-modal trajectory prediction of the dynamic obstacle, refining energy consumption modeling and risk perception planning.

Description

Technical Field

[0001] This invention relates to the field of intelligent agricultural robot technology, and in particular to an agricultural robot trajectory planning method and system with optimal energy consumption under collision constraints. Background Technology

[0002] With the development of smart agriculture, automated operation of agricultural robots has become a key means to improve agricultural production efficiency. However, current path planning systems for agricultural robots have significant shortcomings: First, most systems only consider obstacle avoidance for static obstacles and fail to effectively predict the driving intentions of other dynamic obstacles (such as other agricultural robots), posing a collision risk; second, the planning process often ignores the impact of special farmland terrain (such as slope and soil resistance) on robot energy consumption, resulting in short endurance and low operating efficiency; finally, existing methods fail to deeply couple the quality of task completion (such as farmland coverage) with motion planning, making it difficult to guarantee the overall effectiveness of automated operations.

[0003] Therefore, there is an urgent need for an agricultural robot trajectory planning solution specifically designed for agricultural scenarios that can collaboratively achieve safety, efficiency, and energy saving. Summary of the Invention

[0004] The purpose of this invention is to provide an agricultural robot trajectory planning method and system with optimal energy consumption under collision constraints. It can achieve safe, efficient and energy-saving collaborative operation by integrating multimodal trajectory prediction of dynamic obstacles, refined energy consumption modeling and risk perception planning.

[0005] To achieve the above objectives, this invention provides an energy-optimal trajectory planning method for agricultural robots under collision constraints, comprising: S1, System initialization, loading farmland map, static obstacle information and task area, and allocating initial working area to the machine; S2, the self-machine acquires historical state sequence data and environmental map information of the target machine and surrounding machines in real time; S3, based on historical state sequence data and environmental map information, identifies the target machine's operational intent through a multimodal trajectory prediction model and outputs a multimodal predicted trajectory. Among them, the multimodal trajectory prediction model adopts a deep learning model, and step S3 specifically includes: Step S31, using an LSTM network The historical state sequence data of the target machine and peripheral machines are encoded, and the characteristics of the target machine are obtained through equations (1) and (2) respectively. Features of peripheral machines ; (1) (2) in, , These represent the historical state sequence data of the target machine and the peripheral machines, respectively. , These represent the hidden state and cell state of the target machine output from the coding layer of the LSTM network, respectively. , These represent the hidden state and cell state of the peripheral machine output from the LSTM network's coding layer, respectively. Indicates a fully connected function; Step S32, using the attention head described by equation (3) Capture potential spatial interactions between the target machine and / or surrounding machines, and focus all attention on The output connection and linear transformation are used to obtain the hidden features between the target machine and / or surrounding machines after interaction in the latent space. In equation (3), This represents the softmax function. This represents a multilayer perceptron. This represents the dot product operation. Indicates the first The dimension of each attention head vector; (3) Step S33: Use the softmax function to extract the hidden features. Mapped to the intended maneuver probability and combined with the LSTM decoder output to predict the trajectory. ; S4, run the dynamic programming algorithm to generate a global coarse reference trajectory covering the work area with the main goal of completing its own task; S5. Along the coarse reference trajectory, a spatiotemporal safety corridor is generated using a spatiotemporal inheritance strategy. This corridor dynamically avoids static obstacles and reserves a safety space for dynamic obstacles. S6. Construct the cost function for the optimization problem, taking into account the cost of the reference trajectory, the cost of collision risk, and the cost of energy consumption. S7. Under the boundary constraints of the safety corridor and the kinematic constraints of the robot, the optimization problem is solved using a numerical optimization solver to obtain the optimized local trajectory.

[0006] Furthermore, The uncertainty is represented by a bivariate Gaussian distribution described by equation (5); (5) in, Indicates the target machine's location. Represents the Gaussian distribution function. , and Let represent the mean, standard deviation, and correlation coefficient of the Gaussian distribution, respectively. Indicates the location of the target machine The average value Indicates the location of the target machine The average value Indicates the location of the target machine The mean square deviation of the value Indicates the location of the target machine The mean squared deviation of the value.

[0007] Furthermore, when the dynamic programming algorithm in step S4 generates the global coarse reference trajectory, the cost function is defined as shown in equation (7). : (7) in, and These represent the first and second digits of the planned trajectory. The planned state value and reference state value of each discrete point This represents the number of discrete points in the expression. This represents the estimated collision damage value for each collision probability assignment. Representing discrete time The energy consumption in the process is determined by factors including the terrain slope angle. , , and These are the weighted coefficients for reference trajectory cost, collision risk cost, and energy consumption cost, respectively.

[0008] Furthermore, the energy consumption cost in step S6 is calculated based on a discrete-time energy consumption estimation model, which is described by equation (11): (11) in, , The first The velocity and acceleration of a discrete point , , These are the energy consumption coefficients corresponding to air resistance, acceleration resistance, and gradient resistance, respectively.

[0009] Furthermore, each collision probability is assigned an estimated collision damage value. Collision probability Collision damage value The product is obtained, where, , and These are all transitional parameters used to simplify the formula, where, , and These represent the x-coordinate, y-coordinate, and auto-orientation angle of the planned trajectory in the global coordinate system, respectively: .

[0010] The energy-optimal agricultural robot trajectory planning system under collision constraints provided in this invention includes: The communication module and the sensing module are used to acquire the status information of themselves and surrounding machines in real time through the sensing unit and the communication module. The status information includes position, speed, acceleration and orientation. A multimodal trajectory prediction model is used to identify the operational intent of a target machine based on historical state sequence data and environmental map information, and output a multimodal predicted trajectory. The multimodal trajectory prediction model employs a deep learning model, which specifically includes: The trajectory encoding module is used to encode the historical state sequence data of the target machine and the peripheral machines using an LSTM network, and obtains the features of the target machine through equations (1) and (2) respectively. Features of peripheral machines ; (1) (2) in, , These represent the historical state sequence data of the target machine and the peripheral machines, respectively. , These represent the hidden state and cell state of the target machine output from the coding layer of the LSTM network, respectively. , These represent the hidden state and cell state of the peripheral machine output from the LSTM network's coding layer, respectively. Indicates a fully connected function; Spatial interaction module, used for attention head described by equation (3) Capture potential spatial interactions between the target machine and / or surrounding machines, and focus all attention on The output connection and linear transformation are used to obtain the hidden features between the target machine and / or surrounding machines after interaction in the latent space. In equation (3), This represents the softmax function. This represents a multilayer perceptron. This represents the dot product operation. Indicates the first The dimension of each attention head vector; (3) The multimodal trajectory generation module is used to extract hidden features using the softmax function. Mapped to the intended maneuver probability and combined with the LSTM decoder output to predict the trajectory. ; The dynamic programming unit is used to generate a global coarse reference trajectory covering the work area with the main goal of completing its own task. Along the coarse reference trajectory, a spatiotemporal safety corridor is generated using a spatiotemporal inheritance strategy. This corridor dynamically avoids static obstacles and reserves safety space for dynamic obstacles. The local trajectory optimization unit is used to construct the cost function of the optimization problem. It comprehensively considers the cost of the reference trajectory, the cost of collision risk, and the cost of energy consumption. Under the boundary constraints of the safety corridor and the kinematic constraints of the robot, the numerical optimization solver is used to solve the optimization problem and obtain the optimized local trajectory.

[0011] Furthermore, the uncertainty of is represented by a bivariate Gaussian distribution described by equation (5); (5) in, Indicates the target machine's location. Represents the Gaussian distribution function. , and Let represent the mean, standard deviation, and correlation coefficient of the Gaussian distribution, respectively. Indicates the location of the target machine The average value Indicates the location of the target machine The average value Indicates the location of the target machine The mean square deviation of the value Indicates the location of the target machine The mean squared deviation of the value.

[0012] Furthermore, when the dynamic programming unit generates the global coarse reference trajectory, the cost function is defined as shown in equation (7). : (7) in, and These represent the first and second digits of the planned trajectory. The planned state value and reference state value of each discrete point This represents the number of discrete points in the expression. This represents the estimated collision damage value for each collision probability assignment. Representing discrete time The energy consumption in the process is determined by factors including the terrain slope angle. , , and These are the weighted coefficients for reference trajectory cost, collision risk cost, and energy consumption cost, respectively.

[0013] Furthermore, the energy consumption cost is calculated based on an energy consumption estimation model over discrete time, which is described by equation (11): (11) in, , The first The velocity and acceleration of a discrete point , , These are the energy consumption coefficients corresponding to air resistance, acceleration resistance, and gradient resistance, respectively.

[0014] Furthermore, each collision probability is assigned an estimated collision damage value. Collision probability Collision damage value The product is obtained, where, , and These are all transitional parameters used to simplify the formula, where, , and These represent the x-coordinate, y-coordinate, and auto-orientation angle of the planned trajectory in the global coordinate system, respectively:

[0015] The present invention has the following advantages due to the adoption of the above technical solutions: 1. High safety: By predicting the movement trajectory of surrounding dynamic obstacles, potential collision risks can be anticipated in advance. Combined with spatiotemporal safety corridor and risk field models, collision accidents are fundamentally avoided. 2. High operational efficiency: The planning process is task-oriented (such as full coverage of farmland) and reduces waiting and conflict between robots through collaborative prediction, which significantly improves the efficiency of automated operations; 3. Energy optimization: It is the first to incorporate farmland terrain slope into the energy consumption estimation model of trajectory planning, enabling the robot to autonomously select more energy-efficient paths and extend its endurance. 4. High adaptability: The system is highly modular and can be applied to various agricultural robot operation scenarios such as sowing, fertilizing, and harvesting, and has good versatility and promotion value. Attached Figure Description

[0016] Figure 1This is a schematic diagram of robot classification in this invention.

[0017] Figure 2 This is a schematic diagram of the overall system architecture provided in an embodiment of the present invention.

[0018] Figure 3 This is a schematic diagram of the algorithm of the multimodal trajectory prediction model M2-TPM (MultimodalTrajectory Prediction Module) according to an embodiment of the present invention.

[0019] Figure 4 This is a schematic diagram of the algorithm of the trajectory planning module CEA-TPM (Collision and Energy Consumption Aware Trajectory Planning Module) in an embodiment of the present invention.

[0020] Figure 5 This is a schematic diagram of the kinematic model of an agricultural robot according to an embodiment of the present invention.

[0021] Figure 6 This is a schematic diagram of the safe corridor generation process according to an embodiment of the present invention. Detailed Implementation

[0022] In the accompanying drawings, the same or similar reference numerals are used to denote the same or similar elements or elements having the same or similar functions. The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0023] In the description of this invention, the terms "center," "longitudinal," "lateral," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting the scope of protection of this invention.

[0024] The energy-optimal trajectory planning method for agricultural robots under collision constraints provided in this invention includes: S1, System initialization, loading farmland map, static obstacle information and task area, and allocating initial working area for the machine.

[0025] like Figure 1 As shown, the self-machine is an agricultural robot used for trajectory planning, and is represented by red in the figure. The target machine is a dynamic obstacle with potential spatial interaction with the self-machine, and is represented by blue in the figure. The surrounding machines are dynamic obstacles with potential spatial interaction with the target machine, and are represented by yellow in the figure.

[0026] S2: The autonomous machine acquires historical state sequence data and environmental map information of the target machine and surrounding machines in real time. The historical state sequence data includes the position, velocity, acceleration, and orientation of the target machine and surrounding machines. The environmental map information includes map boundaries, and the location and shape information of static obstacles.

[0027] S3 runs a multimodal trajectory prediction model, which identifies the target machine's operational intent based on historical state sequence data and environmental map information, and predicts multimodal uncertain trajectories.

[0028] Specifically, such as Figure 2 and Figure 3 As shown, the multimodal trajectory prediction model (M2-TPM) in step S3. The deep learning model is employed, specifically including: Step S31, the multimodal trajectory prediction model uses a deep learning LSTM network to encode the historical state sequence data of the target machine, as shown in equation (1): (1) in, This represents the historical state sequence data of the target machine. Represents a fully connected function, and represents an LSTM network. This represents the characteristics of the target machine output from the coding layer of the LSTM network. This represents the hidden state of the target machine as output by the coding layer of the LSTM network. This represents the cell state of the target machine output by the coding layer of the LSTM network.

[0029] The multimodal trajectory prediction model uses a deep learning LSTM network to encode the historical state sequence data of the peripheral machine, as shown in equation (2): (2) in, This represents the historical state sequence data of the surrounding machines. This represents a fully connected function. This represents the characteristics of the peripheral machine output from the coding layer of an LSTM network. This represents the hidden state of the peripheral machines as output by the coding layer of the LSTM network. This indicates the cell state of the peripheral unit output by the coding layer of the LSTM network.

[0030] Step S32, based on the characteristics of the target machine Features of peripheral machines The spatial interaction module is used to capture potential spatial interactions between the target machine and / or surrounding machines, generating hidden features. .

[0031] The spatial interaction module in this embodiment adopts the attention head described by equation (3): (3) in, The softmax function provides a method for normalizing weights. This represents a multilayer perceptron. This represents the dot product operation. Indicates the first The dimension of each attention head vector.

[0032] When the spatial interaction module performs multi-head attention calculation, The output of each attention head uses a matrix Connect them together, then perform a linear transformation to obtain the hidden features resulting from the potential spatial interactions between obstacles. As shown in equation (4): (4) Of course, simpler neural networks can also be used, such as convolutional neural networks (CNNs), to capture the spatial interaction between the target machine and the peripheral machines.

[0033] Step S33, the multimodal trajectory prediction module predicts the hidden features based on the potential spatial interactions between obstacles. ,pass The function maps to the probability of each intended maneuver and combines it with the output of the LSTM decoder to predict the trajectory. In this embodiment, to describe the uncertainty of motion, a bivariate Gaussian distribution represented by equation (5) is used to output the predicted trajectory. : (5) in, , and Let represent the mean, standard deviation, and correlation coefficient of the Gaussian distribution, respectively. The values ​​are derived from the trajectory prediction module M2-TPM mentioned earlier. Represents the Gaussian distribution function. Indicates the target machine's position (its coordinates in the global coordinate system). coordinate, Indicates the location of the target machine coordinate, Indicates the location of the target machine The average value of the coordinates, Indicates the location of the target machine The average value of the coordinates, Indicates the location of the target machine The mean square error of the coordinates. Indicates the location of the target machine The mean square error of the coordinates.

[0034] This embodiment quantifies the uncertainty of collision risk through a multimodal trajectory prediction model, thereby improving the safety of the final planned trajectory. Of course, those skilled in the art can also ensure the safety of the planned trajectory based on a collision detection method with a fixed safety threshold, but this method cannot take into account motion uncertainties.

[0035] S4, combined Figure 4 The dynamic programming algorithm is run to generate a global coarse reference trajectory covering the work area, with the main goal of completing its own task.

[0036] The global reference trajectory is discretized from the continuous time domain to the discrete time domain by running the dynamic programming (DP) algorithm described in equation (6), and the cost function is defined as in equation (7). : (6) (7) in, This is the current state of the host; It is the front-end state of the host; → From state arrive The trajectory; Represents the cost function; It is possible to reach The set of all possible predecessor states, This represents the number of discrete points in the expression. This represents the estimated collision damage value for each collision probability assignment. Representing discrete time Energy consumption in , and These are weighted coefficients for reference trajectory cost, collision risk cost, and energy consumption cost. The values ​​of these weighted coefficients are selected based on the focus of the trajectory planning task. Specifically, if safety is the primary focus, then... The weighting value increases; if energy conservation is the primary focus, then... The weight value increases if the task is the primary focus. The weight value increases. and These represent the first and second digits of the planned trajectory. The planned state value and reference state value of each discrete point.

[0037] The purpose of global reference trajectory cost is to make the optimized trajectory as close as possible to the reference trajectory.

[0038] (8) in, , and These represent the x-coordinate, y-coordinate, and auto-axis orientation angle of the reference trajectory in the global coordinate system, respectively. , and These represent the horizontal coordinate, vertical coordinate, and orientation angle of the planned trajectory in the global coordinate system, respectively.

[0039] The goal of collision cost estimation is to ensure that the optimized planned trajectory does not collide with other target robots and / or surrounding robots in the scene. First, the collision probability density is calculated based on the multimodal predicted trajectory. Second, the original robot contour is approximated by a rectangle aligned to three axes, and the double integral of the probability density over its occupied area is calculated to obtain the corresponding collision probability. Finally, an estimated collision damage value is assigned to each collision probability. , It can be described as Equation (9): (9) in, Let the collision probability density be... This represents the collision damage value; the specific value was obtained through experiments and simulations. Let the collision probability be expressed as Equation (10): (10) In the formula, and These are all transitional parameters used to simplify the formula (without substantial physical meaning). , and These represent the x-coordinate, y-coordinate, and auto-orientation angle of the planned trajectory in the global coordinate system, respectively: .

[0040] Those skilled in the art can also obtain the form in A and B of variation (10). Other descriptive forms.

[0041] In one embodiment, discrete time Energy consumption It can be described as equation (11): (11) in, , The first The velocity and acceleration of a discrete point , , The energy consumption coefficients corresponding to air resistance, acceleration drag, and slope drag are obtained empirically. This embodiment takes into account the terrain slope. The influence of environmental factors, such as [missing information], is used to reduce the energy consumption of the planned trajectory. Similarly, those skilled in the art can also obtain [missing information] through a variation of formula (11). Other descriptive forms.

[0042] The cost function of the above embodiments is considered comprehensively: 1. Reference trajectory cost: The guiding trajectory closely follows the overall task. 2. Collision risk cost: Based on the prediction results of S3, the collision probability between the local trajectory and the predicted trajectory is calculated, and then the collision damage estimate is obtained; 3. Energy consumption cost: Establish a refined energy consumption estimation model related to soil resistance, terrain slope, velocity and acceleration.

[0043] Besides the cost function, solving the objective function also requires considering vehicle kinematic constraints, vehicle dynamics constraints, and safety corridor constraints. Accurate system modeling is a prerequisite for achieving safe and accurate trajectory planning. By reasonably simplifying and decoupling the high-degree-of-freedom vehicle model, the planned trajectory can satisfy vehicle kinematic or dynamic constraints. Therefore, as... Figure 5 As shown, this embodiment of the invention introduces a commonly used bicycle model to approximate the vehicle's response behavior to control actions.

[0044] The vehicle kinematic constraints are described by equation (12): (12) in, , , , , , , They respectively represent the planned trajectory in Moment coordinate, Coordinates, vehicle heading angle, vehicle speed, vehicle front wheel steering angle, and vehicle acceleration; This indicates the front and rear wheelbase of the vehicle. and This indicates the vehicle's yaw rate and jerk.

[0045] The vehicle dynamics constraints are described by equation (13): (13) in, , and These represent the minimum and maximum values ​​of the machine's acceleration, respectively. Indicates the maximum speed; Indicates the maximum value of acceleration; This represents the maximum value of the angular velocity; This indicates the maximum value of the front wheel steering angle.

[0046] S5, along the coarse reference trajectory, uses a spatiotemporal inheritance strategy to generate a spatiotemporal safety corridor, which dynamically avoids static obstacles and reserves safety space for dynamic obstacles.

[0047] like Figure 6 As shown, the safety corridor constraint can reduce the range of solutions to a certain extent. First, a global reference trajectory is searched to obtain dense discrete points. Then, the rectangle is expanded along directions 1, 2, 3 and 4 at each discrete point until it has the maximum possible free area, as shown in Equation (14).

[0048] (14) in, , , , , These represent the upper and lower boundaries of the safety corridor, respectively. Indicates the first The coordinates of the center position of the contour circle are obtained as shown in equation (15).

[0049] (15) in, , , and These represent the distance from the rear axle center to the rear profile, the wheelbase, and the distance from the front axle center to the front profile, respectively.

[0050] S7, based on the energy consumption cost of farmland terrain and vehicle kinematic constraints, under the boundary constraints of the safety corridor and the kinematic constraints of the robot, uses a numerical optimization solver to solve the optimization problem and obtain the optimized local trajectory.

[0051] S8 sends the planned trajectory to the underlying controller for execution and enters the next planning cycle, realizing real-time scrolling optimization of the trajectory.

[0052] The energy-optimal trajectory planning system for agricultural robots under collision constraints provided in this embodiment of the invention includes a communication module and a perception module, a multimodal trajectory prediction model, a dynamic programming unit, and a local trajectory optimization unit, wherein: The communication module and the sensing module are used to acquire the status information of themselves and surrounding machines in real time through the sensing unit and the communication module. The status information includes position, speed, acceleration and orientation.

[0053] The multimodal trajectory prediction model is used to identify the operational intent of a target machine based on historical state sequence data and environmental map information, and output a multimodal predicted trajectory. , The uncertainty is represented by a binary Gaussian distribution described by equation (5).

[0054] The dynamic programming unit is used to generate a global coarse reference trajectory covering the work area with the main goal of completing its own task. Along the coarse reference trajectory, a spatiotemporal safety corridor is generated using a spatiotemporal inheritance strategy. This corridor dynamically avoids static obstacles and reserves safety space for dynamic obstacles.

[0055] The local trajectory optimization unit is used to construct the cost function of the optimization problem. It comprehensively considers the cost of the reference trajectory, the cost of collision risk, and the cost of energy consumption. Under the boundary constraints of the safety corridor and the kinematic constraints of the robot, the numerical optimization solver is used to solve the optimization problem and obtain the optimized local trajectory.

[0056] In one embodiment, the multimodal trajectory prediction model employs a deep learning model, which specifically includes an LSTM network, a spatial interaction module, and a multimodal trajectory generation module, wherein: LSTM networks are used to encode historical state sequence data of the target machine and peripheral machines to obtain the characteristics of the target machine. Features of peripheral machines As shown in equations (1) and (2) above.

[0057] The spatial interaction module is used to capture potential spatial interactions between the target machine and / or surrounding machines, generating hidden features.

[0058] The multimodal trajectory generation module is used to map to the intended maneuver probability through the softmax function and combine it with the LSTM decoder to output the predicted trajectory.

[0059] Below is a specific implementation example: Taking collaborative farming with multiple unmanned tractors as an example, the cloud management platform first divides the farmland to be cultivated into multiple areas and assigns them to each agricultural robot via a communication module. Each agricultural robot acquires its own pose through GPS / IMU, senses nearby obstacles through onboard radar, and shares its status through a communication network (such as 5G). The multimodal trajectory prediction model module determines the robot's working intention and predicts its future trajectory based on the received status of other robots. The dynamic programming unit then runs, generating a "bow"-shaped reference path with the goal of providing 100% cultivation coverage to its assigned area. The planner constructs a dynamic safety corridor based on the reference path and the predicted trajectories of surrounding robots. Finally, the optimization solver, considering soil resistance and operational energy consumption estimation models, solves for an optimal trajectory that is strictly within the safety corridor, has the lowest energy consumption, and provides smooth travel. The underlying controller tracks this trajectory to complete the cultivation task.

[0060] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Those skilled in the art should understand that modifications can be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A collision-constrained, energy-optimal trajectory planning method for agricultural robots, characterized in that, Comprise: S1, system initialization, load farmland map, static obstacle information and task area, assign initial work area for self-machine; S2, the self-machine acquires the historical state sequence data and environment map information of the target machine and the surrounding machines in real time; S3, based on the historical state sequence data and the environment map information, identifying the work intention of the target machine through a multi-modal trajectory prediction model, and outputting a multi-modal predicted trajectory wherein: the multi-modal trajectory prediction model adopts a deep learning model, and step S3 specifically comprises: Step S31, using an LSTM network Encode the historical state sequence data of the target machine and the surrounding machines, and obtain the features of the target machine and the surrounding machines through formulas (1) and (2) respectively , the features of the surrounding machines ; （1） （2） wherein, , respectively represent the historical state sequence data of the target machine, the peripheral machine, , , respectively represent the hidden state, the cell state of the target machine output from the encoding layer of the LSTM network, , , respectively represent the hidden state, the cell state of the peripheral machine output from the encoding layer of the LSTM network, represents a fully connected function; Step S32, using the attention head described in formula (3) Capture the potential space interaction between the target machine and / or the surrounding machine, connect and linearly transform the output of all attention heads , obtain the hidden features of the target machine and / or the surrounding machine after the potential space interaction ; in formula (3), , represents the softmax function, , represents the multi-layer perception, , represents the dot product operation, , represents the dimension of the first Attention head vector （3） Step S33, the hidden features are mapped to the intention maneuvering probability through a softmax function, and a predicted trajectory is output by combining the LSTM decoder; S4, running a dynamic programming algorithm to generate a global rough reference trajectory covering the work area as the main target to complete the self-work task; S5, along the rough reference trajectory, a time-space safety corridor is generated using a time-space inheritance strategy, which dynamically avoids static obstacles and reserves safety space for dynamic obstacles; S6, an optimization problem cost function is constructed, considering the reference trajectory cost, collision risk cost and energy consumption cost; S7, under the boundary constraints of the safety corridor and the kinematics constraints of the robot, a numerical optimization solver is used to solve the optimization problem to obtain the optimized local trajectory.

2. The collision-constrained, energy-optimal agricultural robot trajectory planning method of claim 1, wherein, The uncertainty of the is represented by a bivariate Gaussian distribution described by equation (5). （5） wherein, represents a target machine position, represents a Gaussian distribution function, , and respectively represent a mean, a standard deviation, and a correlation coefficient of the Gaussian distribution, represents a mean value of the value of the target machine position, represents a mean value of the value of the target machine position, represents a mean square error of the value of the target machine position, represents a mean square error of the value of the target machine position.

3. The method of claim 1 or 2, wherein, The cost function defined in equation (7) is used by the dynamic programming algorithm in step S4 to generate the global coarse reference trajectory : （7） wherein, and respectively represent the planning state value and the reference state value of the i-th discrete point of the planning trajectory, represents the number of discrete points in the middle, represents the collision damage value estimated by each collision probability distribution, represents the energy consumption in the discrete time , the determining factors of which include the terrain slope angle , , and are the weighting coefficients of the reference trajectory cost, the collision risk cost, and the energy consumption cost, respectively.​ 4. The collision-constrained, energy-optimal agricultural robot trajectory planning method of claim 3, wherein, The energy consumption cost in step S6 is calculated based on an energy consumption estimation model in discrete time, which is described as formula (11): （11） wherein, , are the velocities and accelerations of the first and second discrete points, respectively, , , , are the energy consumption coefficients corresponding to the air resistance, acceleration resistance, and slope resistance, respectively.

5. The collision-constrained, energy-optimal agricultural robot trajectory planning method of claim 4, wherein, Each collision probability is assigned an estimated collision damage value By the collision probability The product of the collision damage value Wherein, , And Are transition parameters for simplifying the formula, wherein, , And Respectively represent the horizontal coordinate value, the vertical coordinate value and the self-machine orientation angle value of the planned trajectory in the global coordinate system: 。 6. An optimal energy consumption under collision constraint agricultural robot trajectory planning system, characterized in that, Comprise: A communication module and a perception module are used to acquire the state information of the self-machine and the surrounding machines in real time through the perception unit and the communication module, and the state information includes position, speed, acceleration and orientation; The multi-modal trajectory prediction model is used for identifying the operation intention of the target machine based on historical state sequence data and environment map information, and outputting a multi-modal predicted trajectory The multi-modal trajectory prediction model adopts a deep learning model, and the model specifically comprises: A trajectory encoding module is configured to use an LSTM network Encode the historical state sequence data of the target machine and the surrounding machines, and obtain the features of the target machine and the surrounding machines through formulas (1) and (2) respectively ;​ （1） （2） wherein, , respectively represent the historical state sequence data of the target machine and the peripheral machine, , respectively represent the hidden state and the cell state of the target machine output by the encoding layer of the LSTM network, , respectively represent the hidden state and the cell state of the peripheral machine output by the encoding layer of the LSTM network, represents a full connection function; a spatial interaction module configured to employ an attention head described by equation (3) capture potential spatial interactions between the target machine and / or the surrounding machines, connect and linearly transform the outputs of all attention heads to obtain hidden features of the target machine and / or the surrounding machines after the potential spatial interactions ; in equation (3), denotes a softmax function, denotes a multi-layer perceptron, denotes a dot product operation, denotes the dimension of the th attention head vector; （3） a multi-modal trajectory generation module for mapping the hidden features to intent maneuver probabilities through a softmax function and outputting a predicted trajectory in conjunction with an LSTM decoder ;​ A dynamic programming unit is used to generate a global rough reference trajectory covering the work area as the main target to complete the self-work task; along the rough reference trajectory, a time-space safety corridor is generated using a time-space inheritance strategy, which dynamically avoids static obstacles and reserves safety space for dynamic obstacles; A local trajectory optimization unit is used to construct an optimization problem cost function, considering the reference trajectory cost, collision risk cost and energy consumption cost; under the boundary constraints of the safety corridor and the kinematics constraints of the robot, a numerical optimization solver is used to solve the optimization problem to obtain the optimized local trajectory.

7. The collision-constrained, energy-optimal agricultural robot trajectory planning system of claim 6, wherein, The uncertainty of the target machine is represented by a binary Gaussian distribution described by formula (5); （5） wherein, represents a target machine position, represents a Gaussian distribution function, , and respectively represent a mean, a standard deviation, and a correlation coefficient of a Gaussian distribution, represents a mean value of the value of the target machine position, represents a mean value of the value of the target machine position, represents a mean square error of the value of the target machine position, represents a mean square error of the value of the target machine position.

8. The collision-constrained, energy-optimal agricultural robot trajectory planning system of claim 6 or 7, wherein, When the dynamic programming unit generates the global coarse reference trajectory, the cost function is defined as indicated in equation (7) : （7） wherein, and respectively represent the planning state value and the reference state value of the i-th discrete point of the planning trajectory, represents the number of discrete points in the middle, represents the collision damage value estimated by each collision probability distribution, represents the energy consumption in the discrete time , the determining factors of which include the terrain slope angle , , and are the weighting coefficients of the reference trajectory cost, the collision risk cost and the energy consumption cost respectively.​ 9. The collision-constrained, energy-optimal agricultural robot trajectory planning system of claim 8, wherein, The energy consumption cost is calculated based on an energy consumption estimation model in discrete time, which is described as formula (11): （11） wherein, , are the velocities and accelerations of the first , , are the energy consumption coefficients corresponding to the air resistance, acceleration resistance and slope resistance, respectively.​ 10. The collision-constrained, energy-optimal agricultural robot trajectory planning system of claim 9, wherein, Each collision probability is assigned an estimated collision damage value By the collision probability The product of the collision damage value Where, , And Are transition parameters used to simplify the formula, where, , And Respectively represent the horizontal coordinate value, vertical coordinate value and self-machine orientation angle value of the planning trajectory in the global coordinate system: 。

Images