Intelligent agricultural machine obstacle avoidance trajectory planning method and system, equipment and storage medium

Abstract

The invention provides an intelligent agricultural machine obstacle avoidance trajectory planning method and system, equipment and a storage medium. The method comprises the steps of determining current course angle information of an agricultural machine based on acquired pose information of the agricultural machine; determining an effective obstacle detection area in the obtained obstacle detection area based on the agricultural machine parameter data; in the effective obstacle detection area, constructing a layered risk field based on obstacle information, and determining an agricultural machine obstacle avoidance area; based on the course angle information and the agricultural machine obstacle avoidance area, constructing an agricultural machine obstacle avoidance moving trajectory; and selecting an optimal obstacle avoidance trajectory from the obstacle avoidance moving trajectory of the agricultural machine based on an obstacle avoidance constraint. According to the method, the operation safety and efficiency of the agricultural machine in a complex farmland environment are improved, so that the agricultural machine flexibly moves in a narrow space and a severe terrain, the damage to the surrounding environment is reduced, and the overall efficiency of agricultural operation is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of agricultural machinery obstacle avoidance, and specifically relates to an intelligent agricultural machinery obstacle avoidance trajectory planning method and system, equipment and storage medium. Background Art

[0002] With the rapid development of precision agriculture and intelligent equipment technology, agricultural robots are increasingly becoming the core equipment and key support force for the transformation and upgrading of modern agriculture. Their intelligence level directly determines agricultural production efficiency, resource utilization and operation safety. In complex, changeable and highly unstructured farmland operation environments, such as vast open fields, dense orchards or greenhouses, stable and reliable autonomous navigation and operation, obstacle avoidance capability is one of the indispensable key core technologies of agricultural robots, which is directly related to whether they can safely, efficiently and reliably complete delicate operations such as sowing, fertilizing, spraying, weeding and harvesting.

[0003] Most of the current mainstream traditional obstacle avoidance methods are based on global path planning algorithms, such as the classic graph search-based A * algorithm and its many optimized variants, as well as the fast-expanding random tree algorithm based on probability sampling (RRT, RRT * These methods can effectively plan a collision-free path from the starting point to the end point in known, structured, or relatively static environments (such as indoor warehouses and pre-set roads), solving the obstacle avoidance problem when obstacle locations are predictable. However, when faced with the complex, highly dynamic and uncertain scenarios common in real farmland, these traditional global planning methods exhibit a serious lack of dynamic responsiveness. The farmland environment is far from being fully described by static maps. While crops themselves are the targets of operation, their varying growth states and random distribution, such as uneven density, lodging, and missing seedlings, constitute dynamic background obstacles. Obstacles that arise during agricultural machinery operations are even more common, such as agricultural machinery temporarily entering the operation area, personnel, animals, scattered agricultural supplies, or temporary accumulations on ridges. Furthermore, the posture changes and irregular trajectories of agricultural machinery as it moves across complex terrain, such as undulating furrows, muddy terrain after rain, and collapsed ridges, as well as the uncertainty in environmental perception caused by changing weather and lighting, all create a complex and continuously evolving system. Global path planning methods face significant challenges in this scenario.

[0004] First, its core assumption relies on a relatively accurate and static environmental model. However, the real-time dynamic changes in the farmland environment, such as the sudden appearance of moving obstacles and real-time changes in terrain due to tillage or rain, quickly render the pre-built or updated global map invalid. The algorithm needs to frequently recalculate the complete path of the entire operating area to adapt to these changes. For example, the replanning time of RRT algorithms in complex environments will be significantly extended with the increase in environmental complexity and spatial dimensions, making it difficult to meet the demanding requirements of real-time obstacle avoidance of agricultural robots (usually requiring response in milliseconds to seconds).

[0005] Secondly, global planning usually aims to find an optimal path, and its calculation process often involves repeated searches and evaluations of the entire planning space. Even with the use of heuristic strategies, the calculation efficiency is still low in large-scale scenarios such as vast farmlands, causing path updates to lag behind environmental changes. The robot may not have time to react and may collide with new obstacles.

[0006] Furthermore, such algorithms are highly dependent on the accuracy of environmental models. The unstructured nature of farmland environments and the inherent noise of sensors make it extremely difficult to construct accurate and complete global maps. The algorithms are also insufficiently robust to map errors and perception noise. Slight perception deviations may lead to the planning of "virtual paths" that pass through actual obstacles or frequent unnecessary replanning, seriously affecting the smoothness and reliability of operations.

[0007] Furthermore, global path planning often focuses on connectivity between the starting point and the end point, insufficiently considering or difficult to integrate the robot's kinematic constraints, real-time dynamic state, and the constraints of the task itself. This makes it difficult to generate feasible local trajectories that are both safe and collision-free while also meeting the kinematic characteristics and agronomic requirements of the agricultural machinery. Consequently, traditional obstacle avoidance methods based on global path planning face fundamental limitations in computational efficiency and environmental adaptability, making them unable to effectively support agricultural robots in achieving safe and efficient operations with high reliability and autonomy in complex, real-world farmland environments. Summary of the Invention

[0008] To address the challenges of agricultural machinery obstacle avoidance in dynamic scenarios with randomly distributed crops, which can be difficult to adapt to sudden obstacles in the field, irregular crops, or terrain variations, resulting in low computational efficiency and insufficient adaptability, the present invention provides an intelligent agricultural machinery obstacle avoidance trajectory planning method, system, device, and storage medium.

[0009] To achieve the above object, the present invention provides the following technical solutions: The present invention proposes a method for intelligent agricultural machinery obstacle avoidance trajectory planning, comprising the following steps: Determining the current heading angle information of the agricultural machine based on the acquired posture information of the agricultural machine; Determining an effective obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data; In the effective obstacle detection area, a hierarchical risk field is constructed based on obstacle information to determine an obstacle avoidance area for agricultural machinery; Constructing an obstacle avoidance trajectory for the agricultural machinery based on the heading angle information and the obstacle avoidance area of ​​the agricultural machinery; Based on the obstacle avoidance constraints, an optimal obstacle avoidance trajectory is selected from the obstacle avoidance operation trajectories of the agricultural machinery.

[0010] Preferably, determining the current heading angle information of the agricultural machine based on the acquired vehicle posture information includes: Obtain the posture information of the agricultural machinery and convert it into UTM coordinates; Convert the UTM coordinates into a geodetic coordinate system to determine the heading angle information of the agricultural machinery; The process of converting the UTM coordinates into the geodetic coordinate system and determining the heading angle information of the agricultural machinery is as follows:

[0011]

[0012] in, is the heading angle, It is the initial agricultural machinery information obtained by the perception sensor. Initial conversion value of heading angle.

[0013] Preferably, the determining of the effective obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data includes: Obtaining the distances of all obstacles from the agricultural machine within the current location of the agricultural machine, and combining the coordinate data of the current location of the agricultural machine to determine the coordinate data of the obstacles, counting the coordinate data of all obstacles, and fitting them to the geodetic coordinate system to obtain an obstacle detection area; Based on the current speed of the agricultural machine, the response time of the system in the agricultural machine, the friction coefficient of the ground at the current location of the agricultural machine, and the preset safe distance margin that the agricultural machine needs to maintain between it and other objects, the obstacle avoidance radius of the effective obstacle detection area is calculated; Based on the obtained wheelbase of the agricultural machine, the steering angle margin of the agricultural machine when turning, and the minimum turning radius of the agricultural machine when turning, the obstacle avoidance angle range of the effective obstacle detection area is calculated; The obstacle avoidance planning area is determined in the detection area based on the obstacle avoidance angle range, the obstacle avoidance radius and the position of the agricultural machinery as the center point.

[0014] Preferably, the performing false obstacle detection on the obstacle avoidance planning area to obtain a valid obstacle detection area includes: Performing a continuity check on the sensed objects obtained in the obstacle avoidance planning area, screening out false obstacles in the obstacle avoidance planning area, and marking them to obtain an initial screening area for obstacle avoidance planning; The effectiveness of the sensed objects in the initial screening area of ​​the obstacle avoidance plan is checked, abnormal obstacles are marked, and a valid obstacle detection area is obtained.

[0015] Preferably, in the effective obstacle detection area, constructing a layered risk field based on obstacle information and determining an agricultural machinery obstacle avoidance area includes: Obtaining the type of obstacles in the effective obstacle detection area, constructing coordinate data of the obstacles in the geodetic coordinate system, and obtaining a static risk field model; Obtaining the speed of the obstacle in the effective obstacle detection area and the angle between the obstacle's moving direction and the current path of the agricultural machine, and constructing a dynamic risk field model of the obstacle; Obtaining the vertical distance from the coordinate point of the obstacle to the current travel position of the agricultural machine, and constructing a ridge line potential field; Based on the ridge line potential field, the static risk field model and the dynamic risk field model, a total potential field is constructed in the corresponding obstacle area in the effective obstacle detection area, a layered risk field is constructed, and the drivable area of ​​the agricultural machinery is extracted to obtain the agricultural machinery obstacle avoidance area.

[0016] Preferably, the selecting the optimal obstacle avoidance trajectory from the obstacle avoidance operation trajectory of the agricultural machinery based on the obstacle avoidance constraint includes: Based on the heading angle information of the current position of the agricultural machine, the obstacle avoidance path planning area during the forward movement of the agricultural machine is determined; Randomly sampling horizontally in the obstacle avoidance path planning area using a polynomial to obtain multiple path planning areas in each obstacle avoidance path planning area, and screening them using risk constraints; An obstacle avoidance completion area and an obstacle avoidance start area are determined in the obstacle avoidance path planning area, the path planning point in the obstacle avoidance start area is used as the obstacle avoidance start point, the path planning point in the obstacle avoidance completion area is used as the obstacle avoidance end point, and the path planning points in other areas are connected in series to construct multiple agricultural machinery obstacle avoidance operation trajectories.

[0017] Preferably, the selecting of the optimal obstacle avoidance trajectory from the obstacle avoidance operation trajectory of the agricultural machinery based on the obstacle avoidance constraint includes: Establish objective function constraints and collision constraints. First, multiple agricultural machinery obstacle avoidance operation trajectories are screened through objective function constraints. After screening through collision constraints, the remaining agricultural machinery obstacle avoidance operation trajectories are screened again to obtain the optimal obstacle avoidance trajectory.

[0018] The present invention proposes an intelligent agricultural machinery obstacle avoidance trajectory planning system, which applies the above-mentioned intelligent agricultural machinery obstacle avoidance trajectory planning method, and is characterized by comprising: An acquisition module is configured to acquire agricultural machinery posture information, obstacle information, and obstacle detection area; The first processing module is configured to determine the current heading angle information of the agricultural machine based on the acquired posture information of the agricultural machine; a second processing module configured to determine a valid obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data; a risk field model building module configured to build a hierarchical risk field model based on obstacle information in the effective obstacle detection area to determine an obstacle avoidance area for agricultural machinery; An obstacle avoidance trajectory construction module is configured to construct an obstacle avoidance operation trajectory of the agricultural machinery based on the heading angle information and the obstacle avoidance area of ​​the agricultural machinery; an obstacle avoidance trajectory selection module configured to select an optimal obstacle avoidance trajectory from the obstacle avoidance operation trajectory of the agricultural machinery based on the obstacle avoidance constraint; The output module is configured to output the optimal obstacle avoidance trajectory.

[0019] The present invention proposes a computer device comprising a memory, a processor, and a computer program stored in the memory and executable in the processor. When the processor executes the computer program, the steps of the above-mentioned intelligent agricultural machinery obstacle avoidance trajectory planning method are implemented.

[0020] The present invention proposes a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the steps of the above-mentioned intelligent agricultural machinery obstacle avoidance trajectory planning method.

[0021] Compared with the prior art, the present invention has the following beneficial technical effects: The present invention proposes a method for intelligent agricultural machinery obstacle avoidance trajectory planning. This method effectively screens obstacles by fusing the output of multiple sensors such as lidar and cameras, and adopts a combination of polynomial sampling and obstacle avoidance constraints to achieve real-time perception and intelligent processing of complex operating environments. This improves the accuracy and efficiency of the agricultural machinery system in identifying obstacles, enabling agricultural machinery to quickly identify and locate obstacles in various complex environments, and ensuring the flexible obstacle avoidance capabilities of agricultural machinery in confined spaces and environments with multiple obstacles.

[0022] Furthermore, this method constructs a hierarchical risk field based on obstacles, and combines the dynamic adjustment of static risk field, dynamic risk field and ridge line potential field to achieve a refined description and intelligent evaluation of the working environment, improve the accuracy of obstacle avoidance path planning, and by evaluating the risks of different types of obstacles, the agricultural machinery system can quickly adjust the obstacle avoidance strategy when facing emergencies, ensuring safe operation in high-risk environments.

[0023] Furthermore, this method uses Kalman filtering to detect the effectiveness of perceived obstacles. Through motion state prediction and observation residual analysis, the agricultural machinery system can effectively identify and eliminate false targets, improve the reliability of obstacle detection, reduce the impact of misjudgment and interference factors in complex environments, and ensure the accuracy and stability of the detection results.

[0024] Furthermore, this method effectively reduces the computational load and improves the real-time performance and response speed of the agricultural machinery system by setting screening conditions such as effective obstacle detection area, target continuity test and target validity test, so that agricultural machinery can maintain efficient operation in large-scale environments and reduce resource waste and unnecessary computational overhead. Furthermore, this method generates trajectory clusters based on polynomial sampling and screens trajectories through collision constraints, ensuring the continuity and safety of planned trajectories and effectively avoiding collision risks in path planning. By generating and screening multiple trajectory schemes, the agricultural machinery system can flexibly select the optimal path in different environments to ensure the safety and efficiency of operations. Furthermore, this method achieves an optimal balance between obstacle avoidance effect and trajectory smoothness by reasonably setting weight coefficients, thereby improving the operating safety and efficiency of agricultural machinery in complex farmland environments, thereby enabling agricultural machinery to move flexibly in confined spaces and harsh terrains, reducing damage to the surrounding environment and improving the overall efficiency of agricultural operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 The present invention provides a flow chart of a method for intelligent agricultural machinery obstacle avoidance trajectory planning; Figure 2 The present invention provides a schematic diagram of an effective obstacle detection area in a method for obstacle avoidance trajectory planning for intelligent agricultural machinery; Figure 3 The present invention provides a schematic diagram of the total potential field in a method for planning an obstacle avoidance trajectory for an intelligent agricultural machinery; Figure 4 The present invention provides a schematic diagram of obstacle avoidance trajectory planning results in an intelligent agricultural machinery obstacle avoidance trajectory planning method; Figure 5 A schematic diagram of a computer device provided in accordance with an embodiment of the present invention; Figure 6The block diagram of a chip provided according to one embodiment of the present invention is shown. DETAILED DESCRIPTION

[0026] Hereinafter, only certain exemplary embodiments are briefly described. As will be appreciated by those skilled in the art, the described embodiments may be modified in various ways without departing from the spirit or scope of the present invention. Therefore, the drawings and description are to be considered as illustrative in nature and not restrictive.

[0027] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0028] This invention proposes a method for intelligent agricultural machinery obstacle avoidance trajectory planning. Figure 1 As shown, the following steps are included: Determine the current heading angle information of the agricultural machine based on the acquired vehicle posture information; Specifically, the position and attitude information of the agricultural machinery is obtained from the inertial navigation (IMU / GNSS) integrated navigation system, that is, the latitude and longitude information of the current position of the agricultural machinery and the current attitude data of the agricultural machinery are obtained from the inertial navigation (IMU / GNSS) integrated navigation system; The longitude and latitude information of the current position of the agricultural machinery is converted into UTM coordinates, and then the UTM coordinates are converted into a geodetic coordinate system to determine the heading angle information of the agricultural machinery. That is, through the UTM projection, the earth is divided into a UTM zone at intervals of 6° of longitude. Each UTM zone is based on the Transverse Mercator projection. The longitude and latitude are converted into easting and northing coordinates on the plane to obtain UTM coordinates and a geodetic coordinate system.

[0029] The heading angle conversion process is:

[0030]

[0031] in, is the heading angle, It is the initial agricultural machinery information obtained by the perception sensor. Initial conversion value of heading angle.

[0032] Determining an effective obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data; Specifically, various sensors such as lidar and cameras are used to obtain the distance of all obstacles from the agricultural machinery within its current location. The coordinate data of the agricultural machinery's current location is then combined to determine the coordinate data of the obstacles. The coordinate data of all obstacles are then statistically analyzed and fitted into the geodetic coordinate system to obtain the obstacle detection area. The process of determining the coordinate data of the obstacle is as follows:

[0033] in, is the abscissa value of the absolute coordinate, is the ordinate value of the absolute coordinate, is the horizontal coordinate value of the current location of the agricultural machinery, is the vertical coordinate value of the current location of the agricultural machinery, The relative lateral distance between the agricultural machinery and the obstacle obtained by the sensor. is the relative longitudinal distance between the agricultural machinery and the obstacle obtained by the sensor. is the heading angle.

[0034] Obtain agricultural machinery parameter data, namely the current speed of the agricultural machinery, the response time of the system in the agricultural machinery, the ground friction coefficient at the current location of the agricultural machinery, and preset the safety distance margin that the agricultural machinery needs to maintain with other objects. The obstacle avoidance radius of the effective obstacle detection area is calculated based on the response time, agricultural machinery speed, ground friction coefficient and safety distance margin. ; The calculation process of the obstacle avoidance radius of the effective obstacle detection area is:

[0035] in, is the speed of agricultural machinery, is the response time, is the ground friction coefficient, Safety distance margin.

[0036] Obtain the wheelbase of the agricultural machinery, the steering angle margin of the agricultural machinery when turning, and the minimum turning radius of the agricultural machinery when turning. Calculate the obstacle avoidance angle range of the effective obstacle detection area through the wheelbase, steering angle margin and minimum turning radius of the agricultural machinery. ; Obstacle avoidance angle range of effective obstacle detection area The calculation process is:

[0037] in, Wheelbase of agricultural machinery, Steering angle margin, is the minimum turning radius, is the heading angle of the agricultural machinery.

[0038] Obstacle avoidance angle range , obstacle avoidance radius With the location of the agricultural machinery as the center point, the obstacle avoidance planning area where the agricultural machinery needs to avoid obstacles during its forward movement is determined in the detection area, such as Figure 2 As shown; Perform false obstacle detection on the obstacle avoidance planning area to obtain the effective obstacle detection area; Specifically, in actual operation, sensors such as millimeter-wave radars may experience fluctuations in operating conditions or be subject to external interference. These conditions can easily cause the radar to report targets that do not actually exist, i.e., false targets. Such targets are manifested as jumpy and discontinuous data changes. Therefore, the continuity test is performed on the perceived objects obtained in the obstacle avoidance planning area, and false obstacles in the obstacle avoidance planning area are screened out and marked; The maximum speed of the agricultural machinery, the standard deviation of speed measurement noise, and the position of the perceived object at the current and previous moments in the obstacle avoidance planning area are obtained. Within the time difference between the current and previous moments, the relative distance between the perceived object and the agricultural machinery at the current moment and the relative distance between the perceived object and the agricultural machinery at the previous moment are calculated to obtain the relative speed. The relative speed is compared with the maximum speed of the agricultural machinery. If the difference between the relative speed and the maximum speed of the agricultural machinery is less than or equal to the standard deviation of speed measurement noise, it is indicated as a real obstacle. If the difference between the relative speed and the maximum speed of the agricultural machinery is greater than the standard deviation of speed measurement noise, it is indicated that the perceived object is a false perceived object and is marked to obtain the initial screening area for obstacle avoidance planning.

[0039] The specific process of performing continuity check on the perceived objects obtained in the obstacle avoidance planning area and screening out false obstacles in the obstacle avoidance planning area is as follows:

[0040] in, is the relative distance between the perceived object and the agricultural machinery at the current moment, is the relative distance between the perceived object and the agricultural machinery at the previous moment, is the time difference between the current moment and the previous moment, is the maximum speed of agricultural machinery, is the speed measurement noise standard deviation.

[0041] Since the perception results of radar and other perception sensors are uncertain and may cause misidentification and missed identification, it is necessary to perform validity detection on the perception objects in the initial screening area of ​​obstacle avoidance planning based on Kalman filtering. The authenticity of obstacles can be judged through motion state prediction and observation residual analysis. That is, the validity of the perception objects obtained in the initial screening area of ​​obstacle avoidance planning is tested in turn to determine the authenticity of the obstacles.

[0042] The motion state prediction and observation residual analysis of the objects in the initial screening area of ​​the obstacle avoidance planning are carried out, including: Obtain the relative distance between the sensor and the agricultural machinery in the initial screening area of ​​the obstacle avoidance planning , and the relative speed between the agricultural machinery and the sensor , through the relative distance of agricultural machinery , the relative speed between agricultural machinery and the perceived object Construct the Kalman filter state vector ; Kalman filter state vector for:

[0043] in, is the horizontal distance between the agricultural machine and the perceived object with coordinates (x, y), is the longitudinal distance between the agricultural machine and the perceived object with coordinates (x, y), is the lateral relative speed between the agricultural machine and the sensing object with coordinates (x, y), is the longitudinal relative speed between the agricultural machine and the perceived object with coordinates (x, y).

[0044] The Kalman filter state vector of the current position of the agricultural machinery , and the Kalman filter state vector of the agricultural machinery's previous position A Kalman filter state transition model is constructed and used to predict the state of the objects in the obstacle avoidance planning area; Kalman filter state transition model for:

[0045] in, is the process noise, , is the state transition matrix, .

[0046] is the process noise covariance matrix, specifically ,in, 、 are the horizontal and vertical relative distances and the standard deviation of the loudness speed process noise respectively.

[0047] The Kalman filter state vector and Constructing the observation model of Kalman filter ; Observation model The specific construction process is:

[0048] in, is the Kalman filter state vector, is the observation matrix, , is the observation noise, and .

[0049] is the observation noise variance, , is the standard deviation of the horizontal relative distance to the obstacle, is the standard deviation of the longitudinal relative distance to the obstacle.

[0050] Through observation model , Kalman filter state transition model , observation noise variance 、 and Calculate the residual and residual covariance matrix to judge the perceived objects in the obstacle avoidance planning area; The calculation process of the residual and residual covariance matrix is:

[0051]

[0052] The Mahalanobis distance is calculated by the residual and residual covariance matrix , through the Mahalanobis distance Determine the observation results, where if the Mahalanobis distance Greater than , then the observation result is abnormal, the obstacle is an abnormal obstacle, if the Mahalanobis distance Less than or equal to , then the obstacle is a normal obstacle, where The value is 2.

[0053] Mahalanobis distance The calculation process is:

[0054] in, is the residual vector, is the transposed vector of the residual vector, is the inverse of the residual covariance matrix.

[0055] In the effective obstacle detection area, a hierarchical risk field is constructed based on obstacle information to determine the obstacle avoidance area for agricultural machinery; Specifically, the type of obstacles in the effective obstacle detection area is obtained, and the coordinate data of the obstacles in the geodetic coordinate system are used to construct a static risk field model. ; Static risk field model for:

[0056] in, is the type coefficient of the perceived object. When the agricultural machinery can cross, , when agricultural machinery cannot cross, , is the attenuation coefficient, is the coordinate point ( x, y ) and obstacles distance.

[0057] Obtain the speed of the obstacle in the effective obstacle detection area, and the angle between the obstacle movement direction and the current path of the agricultural machinery to construct a dynamic risk field model of the obstacle. ; Dynamic risk field model for:

[0058] in, is the speed of the obstacle, is the angle between the obstacle's moving direction and the current path of the agricultural machinery, is the coordinate point ( x, y ) and obstacles distance.

[0059] Get the vertical distance from the coordinate point of the obstacle to the current position of the agricultural machinery , the Long line potential field is constructed; Long line potential field for

[0060] in is the vertical distance from point q to the nearest ridge line, is the intensity coefficient of the potential field at the current driving position of the agricultural machinery, is the parameter that controls the influence range of the potential field.

[0061] Through the Long Line potential field , static risk field model and dynamic risk field models Based on different weight coefficients, the total potential field is constructed in the corresponding obstacle area in the effective obstacle detection area, that is, the hierarchical risk field is constructed, such as Figure 3 As shown in FIG, the drivable area of ​​the agricultural machinery is extracted, that is, the area outside the total potential field in the effective obstacle detection area, and the obstacle avoidance area of ​​the agricultural machinery is obtained.

[0062] Total potential field The expression is:

[0063] in, is the static risk field weight, is the dynamic risk field weight, is the potential field weight of the Long line.

[0064] Constructing an obstacle avoidance trajectory for the agricultural machinery based on the heading angle information and the obstacle avoidance area of ​​the agricultural machinery; Obtain the heading angle information of the current position of the agricultural machine, determine the current obstacle position to be avoided by the forward extension direction of the heading angle during the forward movement of the agricultural machine, and mark the agricultural machine obstacle avoidance area around the current obstacle position as the obstacle avoidance path planning area; Polynomials are used to perform random horizontal and vertical sampling in the obstacle avoidance path planning area to obtain multiple path planning points in each obstacle avoidance path planning area. The collected path planning points are screened using risk constraints. If they do not meet the requirements, new horizontal sampling is performed in the obstacle avoidance path planning area. Among them, the risk constraint of polynomial horizontal sampling is:

[0065] in, is the basic sampling width, , is the risk value of the sampling point in the agricultural machinery obstacle avoidance area, It is the maximum risk value in the agricultural machinery obstacle avoidance area.

[0066] The process of horizontal sampling is:

[0067] The process of longitudinal sampling is:

[0068] in, are the coefficients of the transverse polynomial to be solved, is the longitudinal distance, are the coefficients of the longitudinal polynomial to be solved, For time.

[0069] The obstacle avoidance area of ​​the agricultural machinery extending forward from the heading angle to the front of the obstacle is the obstacle avoidance starting area, and the path planning points collected in this area are the obstacle avoidance starting point. The obstacle avoidance area of ​​the agricultural machinery extending from the heading angle to the back of the obstacle is the obstacle avoidance completion area, and the path planning points collected in this area are the obstacle avoidance end point. Taking the obstacle avoidance starting point and obstacle avoidance end point as endpoints, the path planning points in other areas are connected in series to obtain multiple agricultural machinery obstacle avoidance paths, that is, multiple agricultural machinery obstacle avoidance operation trajectories are constructed.

[0070] Based on the obstacle avoidance constraints, the optimal obstacle avoidance trajectory is selected from the obstacle avoidance operation trajectories of the agricultural machinery.

[0071] Specifically, obstacle avoidance constraints are established, that is, objective function constraints and collision constraints are established. First, multiple agricultural machinery obstacle avoidance operation trajectories are screened by objective function constraints. After screening by collision constraints, the remaining agricultural machinery obstacle avoidance operation trajectories are screened again to obtain the optimal obstacle avoidance trajectory, such as Figure 4 shown.

[0072] Among them, the establishment of the objective function constraint: randomly select a point from the agricultural machinery obstacle avoidance trajectory as the detection point, and obtain the coordinate value of the detection point , the total potential field of obstacles around the obstacle avoidance area of ​​the agricultural machinery where the detection point is located After the agricultural machine reaches the detection point, the vertical distance from the detection point to the agricultural machine is , set the weights and construct the objective function constraints; Among them, the objective function constraint is:

[0073] in, is the sampling end time, Initial sampling time , , , is the weight, is the Long line potential field, is the total potential field of obstacles around the obstacle avoidance area of ​​the agricultural machinery where the detection point is located, is the field strength value of the end sampling point at the end of sampling time, is the field strength value at the target point, is the sum of the squares of the coordinate values ​​of the detection points.

[0074] By adjusting , , , The size of the machine is used to select the obstacle avoidance trajectory of agricultural machinery close to the ridge line.

[0075] After the objective function constraint is screened, the obstacle avoidance trajectory of the agricultural machinery is further screened by the collision constraint, including: Randomly select a point from the obstacle avoidance trajectory of the agricultural machinery after the objective function constraint screening, record it as the constraint point, and obtain the coordinate value of the constraint point , based on the coordinate values ​​of the constraint points and the coordinate values ​​of the detection points Calculate the curvature ,

[0076] in, is the horizontal coordinate of the constraint point, is the ordinate of the constraint point, is the horizontal coordinate of the detection point, is the vertical coordinate of the detection point; Get the coordinates of the obstacle , and the coordinate value of any point on the obstacle avoidance trajectory of the agricultural machinery , calculate the minimum distance between two ; Minimum distance The calculation process is:

[0077] in, is the horizontal coordinate value of any point on the obstacle avoidance trajectory of the agricultural machinery, is the vertical coordinate value of any point on the obstacle avoidance trajectory of the agricultural machinery, is the horizontal coordinate value of the obstacle, is the vertical coordinate value of the obstacle.

[0078] From the agricultural machinery obstacle avoidance operation trajectory after the objective function constraint screening, the agricultural machinery obstacle avoidance operation trajectory that meets the collision constraint condition is selected; The collision constraint condition is

[0079] in, is the radius of the agricultural machinery safety zone, is the safe obstacle avoidance distance between agricultural machinery and obstacles. ),( ) are the center coordinates of the front and rear circles after the agricultural machinery is simplified into two circles, where ( ) is the coordinate point of the agricultural machinery in the geodetic coordinate system. The relationship between the center coordinates of the front and rear circles is as follows:

[0080] If there is one obstacle avoidance trajectory for agricultural machinery, it is the optimal obstacle avoidance trajectory. If there are multiple obstacle avoidance trajectories for agricultural machinery, the obstacle avoidance distances of the multiple obstacle avoidance trajectories after screening are calculated respectively, and the obstacle avoidance trajectory of the agricultural machinery with the smallest obstacle avoidance distance is selected as the optimal obstacle avoidance trajectory.

[0081] The present invention proposes an intelligent agricultural machinery obstacle avoidance trajectory planning system, which applies the above-mentioned intelligent agricultural machinery obstacle avoidance trajectory planning method, including an acquisition module, a first processing module, a second processing module, a risk field model construction module, an obstacle avoidance trajectory construction module, an obstacle avoidance trajectory optimization module and an output module; The acquisition module is configured to acquire agricultural machinery posture information, obstacle information and obstacle detection area; The first processing module is configured to determine the current heading angle information of the agricultural machine based on the acquired posture information of the agricultural machine; a second processing module configured to determine a valid obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data; a risk field model building module configured to build a hierarchical risk field model based on obstacle information in the effective obstacle detection area to determine an obstacle avoidance area for agricultural machinery; An obstacle avoidance trajectory construction module is configured to construct an obstacle avoidance operation trajectory of the agricultural machinery based on the heading angle information and the obstacle avoidance area of ​​the agricultural machinery; an obstacle avoidance trajectory selection module configured to select an optimal obstacle avoidance trajectory from the obstacle avoidance operation trajectory of the agricultural machinery based on the obstacle avoidance constraint; The output module is configured to output the optimal obstacle avoidance trajectory.

[0082] In another embodiment of the present invention, a computer device is provided, which includes a processor and a memory, wherein the memory is used to store a computer program, the computer program includes program instructions, and the processor is used to execute the program instructions stored in a computer storage medium. The processor may be a central processing unit (CPU), or may be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions to implement corresponding method processes or corresponding functions; the processor of the embodiment of the present invention can be used for the operation of a rock structure quantitative identification method based on new parameters of electrical imaging logging, including: Based on the acquired vehicle posture information, the current heading angle information of the agricultural machinery is determined; based on the agricultural machinery parameter data, an effective obstacle detection area is determined in the acquired obstacle detection area; in the effective obstacle detection area, a layered risk field model is constructed based on the obstacle information to determine the agricultural machinery obstacle avoidance area; based on the heading angle information and the agricultural machinery obstacle avoidance area, an agricultural machinery obstacle avoidance operation trajectory is constructed; based on the obstacle avoidance constraints, the optimal obstacle avoidance trajectory is selected from the agricultural machinery obstacle avoidance operation trajectories.

[0083] In another embodiment of the present invention, a storage medium is provided, specifically a computer-readable storage medium (Memory). The computer-readable storage medium is a memory device in a terminal device, used to store programs and data. It is understood that the computer-readable storage medium herein may include both built-in storage media in the terminal device and, of course, extended storage media supported by the terminal device. The computer-readable storage medium provides storage space, which stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for being loaded and executed by a processor. These instructions may be one or more computer programs (including program code). It should be noted that the computer-readable storage medium herein may be a high-speed RAM memory or a non-volatile memory, such as at least one disk storage device.

[0084] The processor may load and execute one or more instructions stored in a computer-readable storage medium to implement the corresponding steps of the method for quantitatively identifying rock structure based on new parameters of electrical imaging logging in the above embodiment. The processor may load and execute the following steps: Based on the acquired vehicle posture information, the current heading angle information of the agricultural machinery is determined; based on the agricultural machinery parameter data, an effective obstacle detection area is determined in the acquired obstacle detection area; in the effective obstacle detection area, a layered risk field model is constructed based on the obstacle information to determine the agricultural machinery obstacle avoidance area; based on the heading angle information and the agricultural machinery obstacle avoidance area, an agricultural machinery obstacle avoidance operation trajectory is constructed; based on the obstacle avoidance constraints, the optimal obstacle avoidance trajectory is selected from the agricultural machinery obstacle avoidance operation trajectories.

[0085] See also Figure 5 The terminal device is a computer device. Computer device 60 in this embodiment includes: a processor 61, a memory 62, and a computer program 63 stored in memory 62 and executable by processor 61. When executed by processor 61, computer program 63 implements the method for calculating fluid composition in a reservoir-stimulated wellbore according to the embodiment. To avoid repetition, this description is omitted here. Alternatively, when executed by processor 61, computer program 63 implements the functions of various models / units in the system for calculating fluid composition in a reservoir-stimulated wellbore according to the embodiment. To avoid repetition, this description is omitted here.

[0086] The computer device 60 may be a desktop computer, a notebook computer, a PDA, a cloud server, or other computing devices. The computer device 60 may include, but is not limited to, a processor 61 and a memory 62. It will be understood by those skilled in the art that Figure 5This is only an example of the computer device 60 and does not constitute a limitation of the computer device 60. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the computer device may also include input and output devices, network access devices, buses, etc.

[0087] The processor 61 may be a central processing unit (CPU), other general-purpose processors, central processing units (CPUs), graphics processors (GPUs), digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), other programmable logic devices, discrete gate or transistor logic devices, quantum computing-based data processing logic, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor.

[0088] The memory 62 may be an internal storage unit of the computer device 60, such as a hard disk or memory of the computer device 60. The memory 62 may also be an external storage device of the computer device 60, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the computer device 60.

[0089] Furthermore, the memory 62 may include both an internal storage unit of the computer device 60 and an external storage device. The memory 62 is used to store computer programs and other programs and data required by the computer device. The memory 62 may also be used to temporarily store data that has been output or is about to be output.

[0090] Any reference to memory, database, or other media used in the various embodiments provided herein may include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical storage, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory may include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0091] The databases involved in the various embodiments provided herein may include at least one of a relational database and a non-relational database. Non-relational databases may include, but are not limited to, distributed databases based on blockchains. The processors involved in the various embodiments provided herein may include, but are not limited to, general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic units, data processing logic units based on quantum computing, and the like.

[0092] See also Figure 6 The terminal device is a chip. The chip 600 of this embodiment includes a processor 622, which may be one or more, and a memory 632 for storing a computer program executable by the processor 622. The computer program stored in the memory 632 may include one or more modules, each corresponding to a set of instructions. In addition, the processor 622 may be configured to execute the computer program to perform the above-mentioned generalizable monocular absolute depth map estimation method.

[0093] In addition, the chip 600 may further include a power supply component 626 and a communication component 650. The power supply component 626 may be configured to perform power management of the chip 600, and the communication component 650 may be configured to implement communication, such as wired or wireless communication, of the chip 600. In addition, the chip 600 may further include an input / output interface 658. The chip 600 may operate based on an operating system stored in the memory 632.

[0094] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. It is obvious to those skilled in the art that the present invention is not limited to the details of the above exemplary embodiments, and that the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention. Therefore, from all points of view, the embodiments should be regarded as illustrative and non-restrictive. The scope of the present invention is defined by the appended claims rather than the above description, and it is intended that all changes that fall within the meaning and range of equivalents of the claims are included in the present invention. Any reference signs in the claims should not be construed as limiting the claim to which they relate.

[0095] In addition, it should be understood that although this specification describes the embodiments, not every embodiment contains only one independent technical solution. This description is for clarity only. Those skilled in the art should consider the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art. The above content is only for the purpose of illustrating the technical concept of the present invention and cannot be used to limit the scope of protection of the present invention. Any changes made based on the technical solution in accordance with the technical concept proposed by the present invention shall fall within the scope of protection of the claims of the present invention.

Claims

1. A method for intelligent agricultural machinery obstacle avoidance trajectory planning, characterized in that: The following steps are involved: Determining the current heading angle information of the agricultural machine based on the acquired posture information of the agricultural machine; Determining an effective obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data; In the effective obstacle detection area, a hierarchical risk field is constructed based on obstacle information to determine an obstacle avoidance area for agricultural machinery; Constructing an obstacle avoidance trajectory for the agricultural machinery based on the heading angle information and the obstacle avoidance area of ​​the agricultural machinery; Based on the obstacle avoidance constraints, an optimal obstacle avoidance trajectory is selected from the obstacle avoidance operation trajectories of the agricultural machinery.

2. The intelligent agricultural machinery obstacle avoidance trajectory planning method according to claim 1, characterized in that: Determining the current heading angle information of the agricultural machine based on the acquired vehicle posture information includes: Obtain the posture information of the agricultural machinery and convert it into UTM coordinates; Convert the UTM coordinates into a geodetic coordinate system to determine the heading angle information of the agricultural machinery; The process of converting the UTM coordinates into the geodetic coordinate system and determining the heading angle information of the agricultural machinery is as follows: in, is the heading angle, It is the initial agricultural machinery information obtained by the perception sensor. Initial conversion value of heading angle.

3. The intelligent agricultural machinery obstacle avoidance trajectory planning method according to claim 1, characterized in that: The determining of a valid obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data includes: Obtaining the distances of all obstacles from the agricultural machine within the current location of the agricultural machine, and combining the coordinate data of the current location of the agricultural machine to determine the coordinate data of the obstacles, counting the coordinate data of all obstacles, and fitting them to the geodetic coordinate system to obtain an obstacle detection area; Based on the current speed of the agricultural machine, the response time of the system in the agricultural machine, the friction coefficient of the ground at the current location of the agricultural machine, and the preset safe distance margin that the agricultural machine needs to maintain between it and other objects, the obstacle avoidance radius of the effective obstacle detection area is calculated; Based on the obtained wheelbase of the agricultural machine, the steering angle margin of the agricultural machine when turning, and the minimum turning radius of the agricultural machine when turning, the obstacle avoidance angle range of the effective obstacle detection area is calculated; The obstacle avoidance planning area is determined in the detection area based on the obstacle avoidance angle range, the obstacle avoidance radius and the position of the agricultural machinery as the center point.

4. The intelligent agricultural machinery obstacle avoidance trajectory planning method according to claim 1, characterized in that: The false obstacle detection is performed on the obstacle avoidance planning area to obtain a valid obstacle detection area, including: Performing a continuity check on the sensed objects obtained in the obstacle avoidance planning area, screening out false obstacles in the obstacle avoidance planning area, and marking them to obtain an initial screening area for obstacle avoidance planning; The effectiveness of the sensed objects in the initial screening area of ​​the obstacle avoidance plan is checked, abnormal obstacles are marked, and a valid obstacle detection area is obtained.

5. The intelligent agricultural machinery obstacle avoidance trajectory planning method according to claim 2, characterized in that: In the effective obstacle detection area, constructing a layered risk field based on obstacle information and determining an agricultural machinery obstacle avoidance area includes: Obtaining the type of obstacles in the effective obstacle detection area, constructing coordinate data of the obstacles in the geodetic coordinate system, and obtaining a static risk field model; Obtaining the speed of the obstacle in the effective obstacle detection area and the angle between the obstacle's moving direction and the current path of the agricultural machine, and constructing a dynamic risk field model of the obstacle; Obtaining the vertical distance from the coordinate point of the obstacle to the current travel position of the agricultural machine, and constructing a ridge line potential field; Based on the ridge line potential field, the static risk field model and the dynamic risk field model, a total potential field is constructed in the corresponding obstacle area in the effective obstacle detection area, a layered risk field is constructed, and the drivable area of ​​the agricultural machinery is extracted to obtain the agricultural machinery obstacle avoidance area.

6. The intelligent agricultural machinery obstacle avoidance trajectory planning method according to claim 1, characterized in that: The step of selecting an optimal obstacle avoidance trajectory from the obstacle avoidance operation trajectory of the agricultural machinery based on the obstacle avoidance constraint includes: Based on the heading angle information of the current position of the agricultural machine, the obstacle avoidance path planning area during the forward movement of the agricultural machine is determined; Randomly sampling horizontally and vertically in the obstacle avoidance path planning area using polynomials to obtain multiple path planning points in each obstacle avoidance path planning area, and screening them using risk constraints; An obstacle avoidance completion area and an obstacle avoidance start area are determined in the obstacle avoidance path planning area, the path planning point in the obstacle avoidance start area is used as the obstacle avoidance start point, the path planning point in the obstacle avoidance completion area is used as the obstacle avoidance end point, and the path planning points in other areas are connected in series to construct multiple agricultural machinery obstacle avoidance operation trajectories.

7. The intelligent agricultural machinery obstacle avoidance trajectory planning method according to claim 1, characterized in that: The method of selecting the optimal obstacle avoidance trajectory from the obstacle avoidance trajectory of the agricultural machinery based on the obstacle avoidance constraint includes: Establish objective function constraints and collision constraints. First, multiple agricultural machinery obstacle avoidance operation trajectories are screened through objective function constraints. After screening through collision constraints, the remaining agricultural machinery obstacle avoidance operation trajectories are screened again to obtain the optimal obstacle avoidance trajectory.

8. An intelligent agricultural machinery obstacle avoidance trajectory planning system, applying the intelligent agricultural machinery obstacle avoidance trajectory planning method according to any one of claims 1 to 7, characterized in that: include: An acquisition module is configured to acquire agricultural machinery posture information, obstacle information, and obstacle detection area; The first processing module is configured to determine the current heading angle information of the agricultural machine based on the acquired posture information of the agricultural machine; a second processing module configured to determine a valid obstacle detection area in the acquired obstacle detection area based on the agricultural machinery parameter data; a risk field model building module configured to build a hierarchical risk field model based on obstacle information in the effective obstacle detection area to determine an obstacle avoidance area for agricultural machinery; An obstacle avoidance trajectory construction module is configured to construct an obstacle avoidance operation trajectory of the agricultural machinery based on the heading angle information and the obstacle avoidance area of ​​the agricultural machinery; an obstacle avoidance trajectory selection module configured to select an optimal obstacle avoidance trajectory from the obstacle avoidance operation trajectory of the agricultural machinery based on the obstacle avoidance constraint; The output module is configured to output the optimal obstacle avoidance trajectory.

9. A computer device, characterized in that: The method comprises a memory, a processor, and a computer program stored in the memory and executable in the processor. When the processor executes the computer program, the method implements the steps of the intelligent agricultural machinery obstacle avoidance trajectory planning method described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by the processor, the steps of the intelligent agricultural machinery obstacle avoidance trajectory planning method according to any one of claims 1 to 7 are implemented.

Images