An unmanned agricultural machine obstacle avoidance system and dynamic path planning method based on a virtual target point and a virtual gravitational field

Abstract

The application discloses an unmanned agricultural machine obstacle avoidance system and dynamic path planning method based on a virtual target point and a virtual gravity field, and belongs to the field of intelligent agricultural equipment. The system is composed of an agricultural machine 43, an information acquisition and control unit 44 and a path planning and decision unit 45. The method comprises the following steps: 1. establishing an agricultural machine operation area coordinate system to realize conversion of a vehicle coordinate system and a radar coordinate system; 2. performing voxelization grid downsampling on point cloud data collected by a laser radar, selecting an obstacle detection area, removing point cloud outliers, removing ground point clouds, and clustering and sorting obstacle point clouds; 3. calculating the distance between an obstacle and the agricultural machine and the posture of the obstacle model; and 4. generating an obstacle avoidance path according to an improved artificial potential field method based on obstacle position, size and posture information. Through obstacle discrimination, different obstacle models are established, and the optimal obstacle avoidance path of the agricultural machine is selected through an adaptive virtual target point and a path evaluation function, so that the agricultural machine can avoid obstacles.

Description

Technical Field

[0001] This invention relates to intelligent agricultural equipment technology, specifically to an unmanned agricultural machinery obstacle avoidance system and dynamic path planning method based on virtual target points and virtual gravitational fields. Background Technology

[0002] With the migration of rural labor to cities and the reluctance of able-bodied adults to engage in agricultural production, highly intelligent unmanned agricultural machinery involved in all aspects of "plowing, planting, management, and harvesting" has emerged in my country and undergone multiple rounds of field demonstrations, achieving some success. However, it has not yet achieved full-field coverage and long-term continuous and reliable operation, with the complex and dynamic farmland environment being the main limiting factor. Farmland often contains obstacles such as power towers, wells, pump houses, trees, other machinery, people, and animals. When unmanned agricultural machinery encounters obstacles while operating along a globally planned path, it needs to adopt different obstacle avoidance strategies and dynamically plan obstacle avoidance paths based on the obstacle's characteristics to bypass it and return to the global path for further operation. Unfortunately, currently, unmanned agricultural machinery in my country is not equipped with intelligent obstacle avoidance systems and corresponding obstacle avoidance strategies, making it impossible to plan safe, collision-free, and low-cost operating paths after encountering obstacles. This significantly restricts the safe and reliable operation of unmanned agricultural machinery. Therefore, obstacle avoidance systems and dynamic obstacle avoidance path planning technology are key technologies for achieving stable, reliable, and continuous full-coverage operation of unmanned agricultural machinery.

[0003] In the prior art, Patent 1 (Application No. 201710156169.X) discloses a path planning and control method for unmanned agricultural machinery, which uses an improved shortest tangent method to plan obstacle avoidance paths offline, but cannot achieve online planning of obstacle avoidance paths. Patent 2 (Application No. 201711036801.3) discloses a multi-objective fusion intelligent agricultural machinery path planning method. This method requires the prior generation of a grid navigation map and the use of the A* algorithm and Dijkstra's algorithm to determine the optimal straight path. It requires two stages: route map construction and path search on the map, which is slow and has poor real-time performance. Patent 3 (Application No. 201811508272.7) discloses an agricultural machinery trajectory tracking and obstacle avoidance system and method based on multi-source information fusion. It uses MPC theory to plan obstacle avoidance paths. The path planning heavily relies on a high-precision vehicle model. In practice, it is difficult to obtain an accurate vehicle model, so the obstacle avoidance method has poor robustness. The paper “Adaptive coordinated collision avoidance control of autonomous ground vehicle” (Proceedings of the Institution of Mechanical Engineers Part I - Journal of Systems and Control Engineering, 2018, 232(9): 1120-1133) used an improved artificial potential field method to study the collision-free trajectory of autonomous ground vehicles. However, the improved artificial potential field method still has the inherent defect of getting trapped in local optima, which makes the target unattainable.

[0004] The paper "Polynomial Design of Tractor Driving Path" (Agricultural Mechanization Research, 2006) uses a fifth-order polynomial function to design the tractor's driving path. This method has the advantages of short computation time and smooth path, but the tractor cannot return to the original working route after obstacle avoidance, and it does not consider how to determine the start and end points of the path. The paper "Shortest Tangent Path Algorithm for Mobile Robot Path Planning" (Guangdong Automation and Information Engineering, 2003) proposes a method for planning obstacle avoidance paths for mobile robots using the shortest tangent method. This method has the advantages of simple and fast path generation and short path length, but the path is not smooth and is not applicable to agricultural machinery using the Ackermann steering model. The paper proposes to plan the obstacle avoidance path of the tractor based on the improved shortest tangent method, but the path planned by this method has discontinuous curvature and is not easy to track and control. The paper "Obstacle Avoidance Control Method for Automatic Driving of Agricultural Machinery Based on Bezier Curve Optimization" (Transactions of the Chinese Society of Agricultural Engineering, 2019, 35(19):82-88) proposes a method for optimizing obstacle avoidance paths using Bezier curves, but this method has the disadvantages of excessively rapid curvature changes and complex optimization adjustments. The paper "Research on Agricultural Machinery Obstacle Avoidance Method Based on Improved Artificial Potential Field Method" (Chinese Journal of Agricultural Mechanization, 2020) proposes an agricultural machinery obstacle avoidance method based on an improved artificial potential field method. Although this method can enable agricultural machinery to bypass obstacles, the path is too long, resulting in a reduction in the coverage of the operating area. Summary of the Invention

[0005] The purpose of this invention is to plan a dynamic path that is safe and collision-free, with continuous curvature, the shortest path, and the ability to quickly return to the global work path after obstacle avoidance, based on the characteristics of the working environment, such as the rice transplanter's inability to trample on already planted areas during obstacle avoidance, the combine harvester's inability to enter unharvested areas, and constraints related to agricultural machinery kinematics. This enables unmanned agricultural machinery to accurately and quickly avoid obstacles during operation, laying the foundation for improving the safety of unmanned agricultural machinery operations and reducing human intervention. To this end, an obstacle avoidance system and dynamic path planning method for unmanned agricultural machinery based on virtual target points and virtual gravitational fields are proposed.

[0006] To solve the above technical problems, the technical solution adopted by the present invention is as follows:

[0007] A dynamic path planning method for an unmanned agricultural machinery obstacle avoidance system based on virtual target points and virtual gravitational fields includes the following steps:

[0008] Step 1: Using the navigation receiving and processing module, read the coordinates of the four vertices of the work area to establish the coordinate system of the work area and realize the conversion between the radar coordinate system and the vehicle coordinate system;

[0009] Step 2: Perform voxelization and mesh downsampling on the point cloud data collected by lidar scanning, select the agricultural machinery obstacle detection area, remove outliers in the point cloud of the obstacle detection area, remove ground point cloud, cluster the obstacle point cloud, and sort the obstacles.

[0010] Step 3: Calculation of the distance between the obstacle and the agricultural machinery, and the attitude of the obstacle model;

[0011] Step 4: Based on the obstacle location, size, and attitude information obtained from Steps 2 and 3, perform path planning to generate an obstacle avoidance path using the improved artificial potential field method.

[0012] Furthermore, the specific process of step 1 is as follows:

[0013] First, within its working area, the agricultural machinery uses its navigation module to read the coordinates of the four vertices of the field, establishing a global coordinate system for the field. Then, it performs coordinate system transformation. This transformation involves converting from the radar coordinate system to the vehicle coordinate system and from the vehicle coordinate system to the global coordinate system. The steps are as follows:

[0014] The transformation from the lidar coordinate system to the vehicle coordinate system is shown in equation (1):

[0015]

[0016] Where (d) lx ,d ly ,d lz (x) is the radar's installation position relative to the vehicle's coordinate system; l ,y l ,z l ) and (x c ,y c ,z c These are the coordinates of the obstacle in the lidar coordinate system and the vehicle coordinate system, respectively.

[0017] The transformation from obstacle to field coordinate system is shown in equation (2).

[0018]

[0019] Among them, R s Let R be the rotation matrix. p =(x p ,y p (x0, y0, z0) represents the positioning coordinates of the agricultural machinery in the global coordinate system, and (x0, y0, z0) represents the coordinates of the obstacle in the global coordinate system.

[0020]

[0021] The roll, pitch, and yaw angles are obtained through attitude sensors installed on the agricultural machinery.

[0022] From equations (1) and (2), the transformation from the radar coordinate system to the field coordinate system is shown in equation (4):

[0023]

[0024] Furthermore, in step 2, it is necessary to extract point cloud data, use a plane fitting model to segment the ground point cloud and non-ground point cloud, perform Euclidean clustering on the non-ground point cloud, construct an OBB bounding box model for the obstacle point cloud, and extract the size and position information of the obstacles.

[0025] Furthermore, in step 3, it is necessary to calculate the relative position of the obstacle and the agricultural machinery in the global coordinate system:

[0026] Through steps 1 and 2, the transformation from the radar coordinate system to the vehicle coordinate system, and then the transformation from the vehicle coordinate system to the field coordinate system, yields the position coordinates of the obstacle in the field coordinate system: Obs = (x0, y0) and the position coordinates of the agricultural machinery in the field coordinate system: Car = (x0, y0). p ,y p The coordinates of the intersection of the major axis of the obstacle model and the principal axis of the obstacle in the global coordinate system are Obs. t =(x t ,y t Distance between obstacles and agricultural machinery i The angle α between the obstacle and the x-axis in the global coordinate system is shown in (8) and (9):

[0027]

[0028]

[0029] Furthermore, step 4 includes:

[0030] Step 4.1: Based on the size information of the obstacles, establish different obstacle repulsion field models, such as circular repulsion field and elliptical repulsion field;

[0031] Step 4.2: To prevent the gravitational force on the agricultural machinery from being much greater than the repulsive force, a piecewise function is used to replace the gravitational field, and the gravitational field function is optimized.

[0032] Step 4.3: When the agricultural machinery gets stuck in a local optimum, set a virtual target point to guide the agricultural machinery to avoid the obstacle;

[0033] Step 4.4: To reduce the obstacle avoidance path length of agricultural machinery, an adaptive virtual target point method is adopted to generate multiple obstacle avoidance path clusters;

[0034] Step 4.5: To select the optimal obstacle avoidance path, an evaluation function is designed to evaluate the obstacle avoidance path length and obstacle avoidance curvature, and then the function is normalized to obtain the optimal obstacle avoidance path that conforms to the kinematic constraints of agricultural machinery.

[0035] The present invention discloses an obstacle avoidance system for unmanned agricultural machinery based on virtual target points and virtual gravitational fields, comprising agricultural machinery, an information acquisition and control unit and a path planning and decision-making unit 45; the information acquisition and control unit and the path planning and decision-making unit 45 are installed on the agricultural machinery;

[0036] The agricultural machinery is a four-wheeled vehicle with Ackermann steering geometry, equipped with an electric steering wheel 18 and a speed adjustment device 42;

[0037] The information acquisition and control unit consists of a vehicle controller 26, a front wheel angle sensor 15, a vehicle posture sensor 17, an electric steering wheel 18, a throttle position sensor 16, a sensor signal conditioning module 40, a vehicle speed adjustment drive module 41, a vehicle speed adjustment device 42, an SD card 34, a DC voltage regulator module 38, a 12V DC power supply 36, a DC boost module 39, and a touch screen 37. The vehicle controller 26 is connected to the front wheel angle sensor 15, the vehicle posture sensor 17, the electric steering wheel 18, the touch screen 37, the sensor signal conditioning module 40, the vehicle speed adjustment drive module 41, the SD card 34, and the DC voltage regulator module 38. In addition, the sensor signal conditioning module 40 is connected to the throttle position sensor 16, and the vehicle speed adjustment drive module 41 is connected to the vehicle speed adjustment device 42.

[0038] The path planning and decision-making unit 45 comprises a lidar, an industrial control computer, and a navigation receiver; the lidar is connected to the industrial control computer via an integrated WLAN network interface, and the lidar and the industrial control computer transmit data via the UDP protocol; the navigation receiver is connected to the industrial control computer via an integrated RS485 interface.

[0039] The information acquisition and control unit is connected to the RS232 interface in the path planning and decision-making unit 45 via the RS232 interface D of the vehicle controller 26.

[0040] Furthermore, the vehicle body attitude sensor 17 is an LPMS-IG1 RS232 metal housing attitude sensor, which is fixed to a horizontal plane at the center of the vehicle body by a vibration damping device; the vehicle body attitude sensor 17 is connected to the controller's RS232 interface A 29 via its built-in RS232.

[0041] The front wheel angle sensor 15 is an LPMS-IG1 RS232 metal housing attitude sensor, which is fixed to the front wheel steering linkage shaft by a vibration damping device. The front wheel angle sensor 15 is connected to the controller's RS232 interface C 31 via its built-in RS232.

[0042] The vehicle controller 26 is an EMB8616I industrial control board, which uses ST's STM32F107VCT6 as the MCU. The industrial control board integrates 4 RS232 serial ports, 1 RS485 serial port, 8 12-bit A / D conversion interfaces, 2 12-bit D / A interfaces and 1 SD card interface.

[0043] The electric steering wheel 18 is an EMS2 steering drive motor developed by LianShi Navigation Technology Co., Ltd., which is connected to the vehicle controller 26 via RS232 interface B 30;

[0044] The touchscreen 37 is a DC80480F070-6111-0T resistive touchscreen from Guangzhou Dacai Optoelectronics Technology Co., Ltd. The touchscreen integrates an RS485 communication port and a TTL interface. The touchscreen 37 is connected to the RS485 interface 32 of the vehicle controller 26 via an RS485 bus to display lateral deviation, heading deviation, obstacle information, vehicle attitude values, and the settings of forward sight distance and control parameters.

[0045] Furthermore, the speed adjustment device 42 consists of a throttle cable and a throttle electric push rod; one end of the throttle cable is connected to the throttle pedal of the agricultural machinery, and the other end is connected to the piston rod of the throttle electric push rod; the throttle electric push rod is fixed to the vehicle body by a support frame; the speed adjustment drive module 41 adopts an H-bridge drive circuit to drive the throttle electric push rod, realize the extension and retraction of the piston rod of the throttle electric push rod, and thus control the vehicle speed.

[0046] Furthermore, the throttle displacement sensor 16 is a displacement sensor integrated within the electric throttle push rod. This displacement sensor converts the piston rod displacement into a corresponding resistance value to reflect the throttle position. The throttle displacement sensor 16 is connected to the A / D conversion interface 27 of the vehicle controller 26 via the sensor signal conditioning module 40. The sensor signal conditioning module 40 is used to convert the resistance value output by the throttle displacement sensor into a voltage signal of 0-5V.

[0047] Furthermore, the aforementioned lidar is the Velodyne VLP-16 16-line lidar.

[0048] The beneficial effects of this invention are:

[0049] (1) This invention addresses the lack of effective obstacle avoidance measures for existing unmanned agricultural machinery. Considering the requirements of combining agricultural machinery with agronomy, it proposes an obstacle avoidance system and dynamic path planning method for unmanned agricultural machinery based on virtual target points and virtual gravitational fields. Based on the relative positions of the vehicle and obstacles, a local path planning strategy using virtual target points is proposed to overcome the local optima problem inherent in traditional artificial potential field methods. This achieves dynamic obstacle avoidance for agricultural machinery that meets agronomic requirements, ensuring that the machinery does not pass through transplanted or unharvested areas during obstacle avoidance. By setting virtual target points, the constraints of combining agricultural machinery and agronomy are met, providing a basis for the rational selection of obstacle avoidance paths.

[0050] (2) The obstacle avoidance system and dynamic path planning method of unmanned agricultural machinery based on virtual target points and virtual gravitational fields are common key technologies in the field of unmanned agricultural machinery. They are applicable to the dynamic planning of obstacle avoidance paths of agricultural machinery in all stages of cultivation, planting, management and harvesting, and have a wide range of applications. Attached Figure Description

[0051] Figure 1 Hardware connection diagram;

[0052] Figure 2 Lower-level machine structure diagram;

[0053] Figure 3 System processing flowchart;

[0054] Figure 4 Agricultural machinery coordinate system transformation diagram;

[0055] Figure 5 LiDAR coordinate system diagram;

[0056] Figure 6 Point cloud processing flowchart;

[0057] Figure 7 Comparison of point clouds before and after 3D voxel mesh downsampling.

[0058] a - Original point cloud (22,087 points), b - Grid size 0.2m (8,872 points);

[0059] Figure 8 Map of agricultural machinery operating areas;

[0060] Figure 9 Point cloud comparison before and after statistical filtering

[0061] a - Original point cloud: 16,502 points; b - Obstacle point cloud: 14,746 points; c - Ground point cloud: 1,756 points;

[0062] Figure 10 Progressive morphological processing of point cloud images (processing time 2.4s).

[0063] a - Original point cloud: 16,502 points; b - Obstacle point cloud: 9,450 points; c - Ground point cloud: 7,052 points;

[0064] Figure 11 Minimum Point Plane Fitting and Removal of Ground Point Cloud (GPF) Algorithm (Processing time: 0.62s)

[0065] a - Original point cloud: 16,502 points; b - Obstacle point cloud: 4,401 points; c - Ground point cloud: 4,998 points;

[0066] Figure 12 GPF planar fitting model diagram;

[0067] Figure 13 3D spatial KD tree segmentation diagram (Wikipedia);

[0068] Figure 14 Euclidean clustering flowchart;

[0069] Figure 15 Force diagram of the force field;

[0070] Figure 16 Overall obstacle avoidance diagram;

[0071] Figure 17 Obstacle type identification diagram;

[0072] Figure 18 Circular repulsive force field diagram;

[0073] Figure 19 Elliptical repulsive force field diagram;

[0074] Figure 20 Diagram showing the relative positions of agricultural machinery and obstacles.

[0075] a - Elliptical obstacle is on the left side of the work path; b - Elliptical obstacle is in the middle of the work path; c - Elliptical obstacle is on the right side of the work path; d - Circular obstacle is on the left side of the work path; e - Circular obstacle is in the middle of the work path; f - Circular obstacle is on the right side of the work path.

[0076] Figure 21 A diagram illustrating the selection of virtual target points.

[0077] a - Elliptical repulsive force field, b - Circular repulsive force field;

[0078] Figure 22 Elliptical repulsive field obstacle avoidance path planning diagram

[0079] a - Simulation diagram of the obstacle in the obstacle avoidance path; b - Curvature diagram of the obstacle avoidance path in the obstacle avoidance path; c - Simulation diagram of the obstacle on the right side of the obstacle avoidance path; d - Curvature diagram of the obstacle avoidance path on the right side of the obstacle avoidance path. Detailed Implementation

[0080] To make the objectives, technical solutions, and advantages of this invention clearer, the following description is provided in conjunction with the appendix. Figure 1-22 The present invention will be further described in detail below with reference to the embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of the invention.

[0081] like Figure 1 As shown, the hardware of this system mainly consists of an industrial control computer, a navigation receiver, a lower-level computer (STM32 microcontroller), a display screen, an electric steering wheel, a Velodyne 16 LiDAR, and attitude sensors.

[0082] First, the industrial control computer connects to the display screen via an HDMI interface, to the LiDAR via an Ethernet port, to the STM32 slave device via a serial port, and to the attitude sensor and navigation receiver via an RS232 interface. The slave device (STM32) is responsible for driving the electric steering wheel, the vehicle speed adjustment module, and the raising and lowering of the seedling box, specifically as follows: Figure 2 As shown.

[0083] The present invention discloses an obstacle avoidance system for unmanned agricultural machinery based on virtual target points and virtual gravitational fields, comprising agricultural machinery, an information acquisition and control unit and a path planning and decision-making unit 45; the information acquisition and control unit and the path planning and decision-making unit 45 are installed on the agricultural machinery;

[0084] The agricultural machinery is a four-wheeled vehicle with Ackermann steering geometry, equipped with an electric steering wheel 18 and a speed adjustment device 42;

[0085] The information acquisition and control unit consists of a vehicle controller 26, a front wheel angle sensor 15, a vehicle posture sensor 17, an electric steering wheel 18, a throttle position sensor 16, a sensor signal conditioning module 40, a vehicle speed adjustment drive module 41, a vehicle speed adjustment device 42, an SD card 34, a DC voltage regulator module 38, a 12V DC power supply 36, a DC boost module 39, and a touch screen 37. The vehicle controller 26 is connected to the front wheel angle sensor 15, the vehicle posture sensor 17, the electric steering wheel 18, the touch screen 37, the sensor signal conditioning module 40, the vehicle speed adjustment drive module 41, the SD card 34, and the DC voltage regulator module 38. In addition, the sensor signal conditioning module 40 is connected to the throttle position sensor 16, and the vehicle speed adjustment drive module 41 is connected to the vehicle speed adjustment device 42.

[0086] The path planning and decision-making unit 45 comprises a lidar, an industrial control computer, and a navigation receiver; the lidar is connected to the industrial control computer via an integrated WLAN network interface, and the lidar and the industrial control computer transmit data via the UDP protocol; the navigation receiver is connected to the industrial control computer via an integrated RS485 interface.

[0087] The information acquisition and control unit is connected to the RS232 interface in the path planning and decision-making unit 45 via the RS232 interface D of the vehicle controller 26.

[0088] The vehicle attitude sensor 17 is an LPMS-IG1 RS232 metal housing attitude sensor, which is fixed to a horizontal plane at the center of the vehicle body by a vibration damping device; the vehicle attitude sensor 17 is connected to the controller's RS232 interface A 29 via its built-in RS232.

[0089] The front wheel angle sensor 15 is an LPMS-IG1 RS232 metal housing attitude sensor, which is fixed to the front wheel steering linkage shaft by a vibration damping device. The front wheel angle sensor 15 is connected to the controller's RS232 interface C 31 via its built-in RS232.

[0090] The vehicle controller 26 is an EMB8616I industrial control board, which uses ST's STM32F107VCT6 as the MCU. The industrial control board integrates 4 RS232 serial ports, 1 RS485 serial port, 8 12-bit A / D conversion interfaces, 2 12-bit D / A interfaces and 1 SD card interface.

[0091] The electric steering wheel 18 is an EMS2 steering drive motor developed by LianShi Navigation Technology Co., Ltd., which is connected to the vehicle controller 26 via RS232 interface B 30;

[0092] The touchscreen 37 is a DC80480F070-6111-0T resistive touchscreen from Guangzhou Dacai Optoelectronics Technology Co., Ltd. The touchscreen integrates an RS485 communication port and a TTL interface. The touchscreen 37 is connected to the RS485 interface 32 of the vehicle controller 26 via an RS485 bus to display lateral deviation, heading deviation, obstacle information, vehicle attitude values, and the settings of forward sight distance and control parameters.

[0093] The speed adjustment device 42 consists of a throttle cable and a throttle electric push rod; one end of the throttle cable is connected to the throttle pedal of the agricultural machinery, and the other end is connected to the piston rod of the throttle electric push rod; the throttle electric push rod is fixed to the vehicle body by a support frame; the speed adjustment drive module 41 adopts an H-bridge drive circuit to drive the throttle electric push rod, realize the extension and retraction of the piston rod of the throttle electric push rod, and thus control the vehicle speed.

[0094] The throttle displacement sensor 16 is a displacement sensor integrated within the electric throttle push rod. This displacement sensor converts the piston rod displacement into a corresponding resistance value to reflect the throttle position. The throttle displacement sensor 16 is connected to the A / D conversion interface 27 of the vehicle controller 26 through the sensor signal conditioning module 40. The sensor signal conditioning module 40 is used to convert the resistance value output by the throttle displacement sensor into a voltage signal of 0-5V.

[0095] The lidar mentioned is the Velodyne VLP-16 16-line lidar.

[0096] like Figure 3 As shown, the system software processing flow mainly consists of three parts: coordinate system establishment and transformation module, obstacle detection module, and path planning module, which includes the following steps:

[0097] Establishment and transformation of the S1 coordinate system

[0098] First, within its working area, the agricultural machinery uses its navigation module to read the coordinates of the four vertices of the field, establishing a global coordinate system for the field. Then, it performs coordinate system transformation. This transformation involves converting the radar coordinate system to the vehicle coordinate system and vice versa, such as... Figure 4 Figure 5 As shown, the coordinate transformation steps are as follows:

[0099] The transformation from the lidar coordinate system to the vehicle coordinate system is shown in equation (1):

[0100]

[0101] Where (d) lx ,d ly ,d lz (x) is the radar's installation position relative to the vehicle's coordinate system; l ,y l ,z l ) and (x c ,y c ,z c These are the coordinates of the obstacle in the lidar coordinate system and the vehicle coordinate system, respectively.

[0102] The transformation from obstacle to field coordinate system is shown in equation (2).

[0103]

[0104] Among them, R s Let R be the rotation matrix. p =(x p ,y p(x0, y0, z0) represents the positioning coordinates of the agricultural machinery in the global coordinate system, and (x0, y0, z0) represents the coordinates of the obstacle in the global coordinate system.

[0105]

[0106] The roll, pitch, and yaw angles are obtained through attitude sensors installed on the agricultural machinery.

[0107] From equations (1) and (2), the transformation from the radar coordinate system to the field coordinate system is shown in equation (4):

[0108]

[0109] S2 Field Obstacle Detection

[0110] like Figure 6 As shown, obstacle detection mainly consists of voxel mesh downsampling, selection of agricultural machinery obstacle detection areas, removal of outliers in the agricultural machinery driving area, removal of ground point clouds, Euclidean clustering of point clouds, and extraction of obstacle models. The processing steps are as follows:

[0111] 1) Voxelized Mesh Downsampling

[0112] The point cloud data acquired by the lidar is extensive and chaotic. To maintain the shape of the point cloud while reducing the number of points, voxelized mesh downsampling is performed on the original point cloud. The voxelization parameters in the x, y, and z axes are all set to 0.2m. The results are as follows: Figure 7 As shown.

[0113] 2) Selection of Agricultural Machinery Obstacle Detection Area

[0114] During agricultural machinery operation in the field, it is necessary to detect obstacles along the working path. To reduce the computational burden on the processor, the working area of ​​the machinery needs to be divided. Considering the speed, braking distance, and safety distance around the vehicle, the point cloud processing area is divided into an emergency obstacle avoidance area and a safe driving area. Since the experimental platform is a Kubota 68CMD model rice transplanter with dimensions of 3140mm length, 2210mm width, and 2595mm height, and an operating speed of 0-5.83km / h, the emergency obstacle avoidance area is set 5m in front of the transplanter, and the safe obstacle avoidance area is set 8-10m in front of the transplanter. The specific driving areas are as follows: Figure 8 As shown.

[0115] 3) Removal of outliers in agricultural machinery operating areas

[0116] When LiDAR scans the environment around agricultural machinery, it generates datasets with varying point densities. During machinery operation, vehicle vibration and uneven road surfaces cause the LiDAR to vibrate along with the vehicle, leading to more sparse outliers and degrading the dataset quality. Utilizing this data complicates the estimation of local point cloud features (normal vectors or curvature rates of change), necessitating outlier removal from the point cloud data in the machinery's operating area. By setting the number of query points and the outlier threshold to 50 and 1.0 respectively, the StatisticalOutlierRemoval algorithm is used to statistically analyze the neighborhood of each point, deleting points that do not meet specific criteria and eliminating interference from outliers. The specific processing results are shown below. Figure 9 As shown.

[0117] 4) Removal of ground point clouds

[0118] Ground point clouds constitute the majority of the data in a single frame of point cloud data. To remove interference during subsequent point cloud clustering, ground point clouds need to be removed. Agricultural machinery is often used in environments with uneven terrain and significant slope. Considering these characteristics, there are two main ground point cloud removal algorithms: Progressive Morphology-based and Lowest Point Plane Fitting (GPF). Both methods are suitable for scenarios with a certain degree of slope.

[0119] The method based on progressive morphological processing of ground point clouds performs better than the method of removing ground point clouds by fitting the lowest point plane, but it is more time-consuming and cannot meet the real-time processing requirements of unmanned agricultural machinery. The processing effects of the two methods are as follows: Figure 10 Figure 11 As shown in the figure. It is now decided to use the lowest point plane fitting method to remove the ground point cloud.

[0120] In the method of removing ground point clouds by plane fitting at the lowest point, it is assumed that the ground point cloud consists of points on a plane and has the feature of lowest height. The specific processing steps are as follows:

[0121] Step 1: Input a set of point cloud data p in Then initialize the model parameters. The model parameters include: the number of iterations N for the planar model. iter The number N points used to estimate the lowest representative point LPR LPR Th is used to obtain the height threshold of the initial seed point. seeds ;

[0122] Step 2: Extract ground seed points P from the point cloud data after voxelization mesh downsampling. seedsAssume there exists a lowest point among all road surface points, and on a relatively flat road surface, this point should also be the lowest point in the point cloud. However, in the operating environment of agricultural machinery, the point cloud data collected by lidar contains noise, making it unsuitable to use the lowest point as a point on the ground model parameters. To determine the representative point on the planar model, the input point cloud data is first sorted according to its height along the Z-axis from smallest to largest, resulting in an ordered point cloud set P. sorted Then select P sorted Middle front N LPR For each point p, the average value of these points is used to obtain the Least Plane Representative Point (LPR). After obtaining the LPR, the input point cloud p is then processed... in Perform a traversal; if p in Point P in k The height is less than LPR plus Th seeds If a point is considered a ground seed point, then after traversing all points, the ground seed point P is obtained. seeds ;

[0123] Step 3: Based on seed point P seeds Fit the ground model and adjust p based on the ground model. in Divided into ground point P g Non-ground point P ng .

[0124] The planar model is shown in (5):

[0125] ax + by + cz + d = 0 (5)

[0126] a, b, c, and d are the coefficients of the plane equation, and x, y, and z are the coordinates of a point in the plane model on the x-axis, y-axis, and z-axis, respectively.

[0127] To find the normal vector of the planar model, we first need to find the seed point P. seeds The covariance matrix C xyz As shown in equation (6):

[0128]

[0129] Step 4: Based on the planar model, re-segment the input point cloud p in Calculate point P k The distance to the planar model, if this distance is less than the threshold Th g Add P to this point g Otherwise, add P ng ;

[0130] Step 5: Repeat the iteration N iterNext, optimize the planar parameters. To ensure the fitted planar model better adapts to the ground environment, the ground points obtained in step four are used as seed points for a new round of planar fitting. Steps three and four are repeated iteratively until the number of iterations reaches a set value. At this point, the planar model parameters are considered the optimal planar model for the ground, the iteration ends, and ground and non-ground point cloud data are output. The model is as follows. Figure 12 As shown.

[0131] 5) Euclidean clustering based on point clouds

[0132] Point cloud data after ground removal is characterized by its unordered nature and large volume. To improve the retrieval efficiency of point cloud data, the KDTree data structure is used to process it. The data obtained from the Velodyne 16-line LiDAR is three-dimensional (x, y, z), so the KDTree data dimension used here is k = 3. The tree structure of KDTree is as follows: Figure 13 As shown, it has the following characteristics: root node, left subtree, and right subtree, and on the same feature dimension, the value of the left subtree is smaller than the value of the right subtree. After establishing the KDTree retrieval structure for the point cloud data, it is necessary to perform Euclidean clustering on the non-ground point cloud data to extract the number of obstacles in the non-ground point cloud. The principle of Euclidean clustering is: based on the distance between points in space, points that are closer together are grouped into one class. The Euclidean clustering process is as follows: Figure 14 As shown below:

[0133] 1) Based on the non-ground point cloud data to be segmented, establish the data structure of KDTree;

[0134] 2) Set the distance threshold D for clustering. thre Minimum number of cluster points Num min Maximum number of cluster points Num max ;

[0135] 3) Randomly select an initial point p from the non-ground point cloud data to be segmented. 11 (x1, y1, z1), using the KDTree data structure, search for the distance from p. 11 Given the nearest n points, calculate the relationship between the n points and p. 11 Distance D i1 (i = 1, 2, 3, ..., n). D i1 <D thre point p 10 p 12 p 13 …p 1n Add to set Q;

[0136]

[0137] 4) Find a point p in set Q.10 Repeat step 3;

[0138] 5) In set Q, continue to select a point and repeat step 3 until no other points are added to set Q, then the search ends;

[0139] 6) In the set of obstacles, the number of points in the set needs to be less than Num. min Or the number of points in the set is greater than Num max The set of obstacles is eliminated.

[0140] 7) Output the point cloud set of obstacles in the non-ground point cloud.

[0141] 6) Sorting of obstacles

[0142] After Euclidean clustering of the point cloud data, a set of point clouds of obstacles is obtained, which needs to be used to build a bounding box model of the obstacles. Common obstacles encountered in farmland include people, agricultural machinery (tillers, harvesters, and rice transplanters), utility poles, and trees. These obstacles have obvious differences in geometry and size, and these features can be used to classify the encountered obstacles.

[0143] The obstacle model uses the OBB bounding box model. The specific steps for sorting obstacles are as follows:

[0144] Step 1: Extract the point cloud set of obstacles in the emergency obstacle avoidance area Obsset1 = {obs 11 ,obs 12 ,…obs 1n Obsset2 = {obs}, a cluster of obstacle points in the safe driving area. 21 ,obs 22 ,…obs 2n}

[0145] Step 2: For obstacles in Obsset2, add obstacles with a point cloud count greater than that of the agricultural machinery body to the Obsset1 obstacle list. Then, use the OBB bounding box model to extract the base area s of the obstacles in Obsset1. i , length L i Width (W), Height of the obstacle i and the horizontal projection coordinates of the obstacle center

[0146] Step 3: For the obstacle point cloud set Obsset1, sort out the obstacles that affect the movement of the agricultural machinery. If there are multiple obstacles, treat them as a single obstacle and provide the coordinates (x, y) of the obstacle. l ,y l ,z l), base area S0, length L0 and width W0.

[0147] Step 4: Based on the bounding boxes obtained in Step 3, roughly classify the obstacles into humanoid obstacles, tree-like obstacles, and agricultural machinery-like obstacles.

[0148] S3 Obstacle Localization and Attitude Calculation

[0149] Through S1 and S2, the transformation from the radar coordinate system to the vehicle coordinate system, and then the transformation from the vehicle coordinate system to the field coordinate system, yields the position coordinates of the obstacle in the field coordinate system: Obs = (x0, y0) and the position coordinates of the agricultural machinery in the field coordinate system: Car = (x0, y0). p ,y p The coordinates of the intersection of the major axis of the obstacle model and the principal axis of the obstacle in the global coordinate system are Obs. t =(x t ,y t Distance between obstacles and agricultural machinery (Dis) i The angle α between the obstacle and the x-axis in the global coordinate system is shown in (8) and (9).

[0150]

[0151]

[0152] S4 Obstacle Avoidance Path Planning

[0153] To meet the practical needs of rice transplanters in field operations, and addressing the problem that traditional artificial potential field methods are prone to getting trapped in local optima, an improved artificial potential field method is proposed, such as... Figure 15 As shown. This method mainly includes solving problems related to gravitational fields, obstacle repulsion fields, using adaptive virtual target points to solve local minima, obstacle avoidance path oscillations, and selecting obstacle avoidance paths that meet agronomic requirements.

[0154] When rice transplanters operate in the field, they follow a pre-planned path for planting. When encountering dynamic obstacles, they stop and yield; when encountering static obstacles, they use a localized path to avoid them. Figure 16 As shown. Considering that the rice transplanter cannot walk on already planted areas during obstacle avoidance to avoid damaging the transplanted seedlings, virtual target points are used to guide the agricultural machinery to choose a reasonable obstacle avoidance direction.

[0155] 1) Determining the type of obstacle

[0156] The length L0, width W0, and angle α of the obstacle are obtained from S2 and S3. Let the threshold value of the difference between the length and width of the obstacle be L. th If the obstacle's impact distance is D0, the machine's width is W1, and its length is L1, then the obstacle type determination mechanism is as follows: Figure 17 As shown.

[0157] 2) Improvement of the circular repulsive field

[0158] Depend on Figure 17 The obstacle was identified as a roughly circular obstacle. To ensure the safe obstacle avoidance of agricultural machinery, [further details needed]. As the radius of the obstacle model C0, with As the radius of the inflated obstacle model C1, with As the radius of the area C2 affected by the obstacle, such as Figure 18 As shown.

[0159] Establish a circular repulsive field for a near-circular obstacle, as shown in (10):

[0160]

[0161] Among them, U Crep (X) is the repulsive potential field function of the circular obstacle, k Crep D is the gain coefficient of the repulsive field of a quasi-circular obstacle, D0 is the distance of influence of the obstacle, and D i (X) is the distance from the outer contour of the obstacle to the outer contour of the agricultural machinery, as shown in equation (11):

[0162]

[0163] The negative gradient of the repulsive force field of the obstacle is shown in equation (12):

[0164]

[0165] F Crep (X) is the repulsive force function of the circular obstacle;

[0166] 3) Improvement of the elliptical repulsive field

[0167] Depend on Figure 17 The obstacle is obtained as an elliptical obstacle. The mathematical model of the ellipse can be used to obtain the elliptical expression equation with the obstacle coordinates as the center, as shown in equation (13).

[0168]

[0169] Figure 19 In the equation, the four ellipses share the same axis of symmetry. E0 represents the position of the obstacle ellipse, E2 represents the position of the agricultural machinery ellipse, E3 represents the area affected by the obstacle, and E4 represents the area where the obstacle is located after expansion. W3 is the length of the minor axis of ellipse E4, and L4 is the length of the major axis of ellipse E4, as shown in equations (14)-(16).

[0170]

[0171]

[0172]

[0173] The areas of ellipses E2, E3, and E4 are shown in equations (17)-(19):

[0174]

[0175] S3=πL3W3 (18)

[0176] S4=πW3L4 (19)

[0177] A repulsive field is established for the elliptical obstacle as shown in equation (20):

[0178]

[0179] Among them, F Erep (S i ) represents the repulsive force function of an elliptical obstacle, k Erep S is the repulsive force coefficient of the repulsive field of an elliptical obstacle. max S represents the maximum area of ​​influence of the obstacle. i S is the difference in area between ellipse E2 and ellipse E4. th Ellipse area threshold. S max and S i As shown in equations (21) and (22).

[0180] S max =S3-S4 (21)

[0181] S i =S2-S4 (22)

[0182] Considering the safety of agricultural machinery in obstacle avoidance, the agricultural machinery's... The width is transferred to the obstacle size, and the agricultural machinery is assumed to be a point mass. The expression for the expansion obstacle is as shown in (23).

[0183]

[0184] 4) Improvement of the gravitational field

[0185] If the target point is too far from the location of the agricultural machinery, the gravitational force on the agricultural machinery will be too great, while the repulsive force of the obstacle on the agricultural machinery may be much smaller than the gravitational force. If the gravitational force is too great, the agricultural machinery will collide with the obstacle and fail to achieve the obstacle avoidance effect. To solve this problem, a distance threshold is set in the gravitational field, as shown in Equation (24).

[0186]

[0187] Among them, Uatt (X) is the gravitational field function, k att ρ is the gravitational field gain coefficient. g (X) represents the distance from the vehicle to the target point, ρ * g The distance threshold is given. The negative gradient function of the gravitational field is shown in (25).

[0188]

[0189] 5) Calculation of the resultant force field

[0190] By determining the type of obstacle, the gravitational force F at the target point is obtained. att (X), repulsive force F of the circular obstacle Crep (X) and the repulsive force F of the elliptical obstacle Erep (S i ), thus obtaining the resultant force F Sum As shown in equation (26).

[0191]

[0192] 6) Virtual target point guides agricultural machinery to avoid obstacles

[0193] When agricultural machinery and obstacles are collinear, the gravitational force exerted on the machinery by the incoming target and the repulsive force from the obstacle lie on the same straight line. In this situation, the machinery may be in a locally optimal state, such as... Figure 17 As shown in the diagram. In this scenario, a virtual target point is introduced between the obstacle and the actual target point to help the agricultural machinery escape the local minimum and reach the actual target point, thus completing obstacle avoidance.

[0194] a) Obstacle Avoidance Scenarios Analysis

[0195] Based on the external shape of the obstacles, models of circular and elliptical obstacles, as described above, are established. The distribution of obstacles encountered by agricultural machinery in the field is as follows: Figure 20 As shown.

[0196] Figure 20 P(x) p ,y p T(x) represents the starting coordinates of the obstacle avoidance point of the agricultural machinery. t ,y t ) represents the coordinates of the target point, d x From X0 to line T i S i The distance is shown in equation (30), d th This is the distance threshold.

[0197] Ax + By + C = 0 (27)

[0198] A = y p -yt (28)

[0199] B = x t -x p (29)

[0200] C = x p y t -y p x t (31)

[0201]

[0202] If the horizontal distance d x If equation (33) is satisfied, then there is no need to set a virtual target point to guide the agricultural machinery to avoid obstacles, such as Figure 20 As shown in a and d. Figure 20 In b and e, the lateral distance d between the obstacle and the agricultural machinery x If equation (34) is satisfied, and the distance between the agricultural machinery and the obstacle is too close, the obstacle avoidance trajectory generated during the obstacle avoidance process will oscillate. Therefore, it is necessary to set a virtual target point to generate a smooth obstacle avoidance trajectory. Figure 20 In cases c and f, the obstacle is biased to the right of the agricultural machinery. The agricultural machinery needs to avoid the obstacle in the unplanted area, so it is necessary to set a virtual target point to guide the agricultural machinery to avoid the obstacle.

[0203] d x >d th (33)

[0204] d x ≤d th (34)

[0205] against Figure 20 Problems arise in b, e, c, and f regarding obstacle avoidance paths that do not meet the agronomic requirements of agricultural machinery. To address this, a method is proposed that virtual target points be set up for local path planning. This method aims to overcome the problems of local optima and path oscillations inherent in the artificial potential field method, thereby achieving obstacle avoidance paths that meet agronomic requirements and guiding agricultural machinery to avoid obstacles.

[0206] b) Strategies for setting and adjusting virtual target points

[0207] Figure 21 In equations a and b, the initial length L1 is one vehicle length of the rice transplanter, and L2 is the obstacle avoidance reaction distance of the rice transplanter. Since the agricultural machinery normally travels at low speed, L2 is set to one vehicle length to meet the emergency braking distance of the rice transplanter. Assume the maximum front wheel turning angle of the agricultural machinery is δ. max Angular step size θ th The step size of the artificial potential field method is step, A1(x) a1 ,y a1 P is the initial virtual target point, and the obstacle avoidance starting point is P. i The obstacle avoidance endpoint is T.i (i = 0, 1, 2, 3…n-1), P i With T i Regarding obstacle symmetry, P n-1 The initial position is the intersection of the obstacle's influence area and the vehicle's direction of travel; the obstacle avoidance starting point is P. i The angle between the line parallel to A1 and the y-axis is δ. i (δ i ≤δ max ).

[0208] When the agricultural machinery is two vehicle lengths away from the obstacle, it begins to select the obstacle avoidance starting point, the actual target point, and the virtual target point. The selection steps are as follows:

[0209] (1) Selection of the starting point P0

[0210] On line segment L2, calculate the angle δ0 between line P0A1 and the y-axis. If δ0 ≤ δ n -6θ th If so, then P0 is set as the farthest starting point of line segment L2; otherwise, the slope is k = tan(δ). n -6θ th The intersection of the straight line formed by point A1 and point A1 with the working path is taken as the farthest starting point of L2.

[0211] (2)P i Selection of starting point and virtual target point (i = 1, 2, ..., n-1)

[0212] After selecting the endpoint P0 of line segment L2, divide L2 into n-2 equal initial starting points and calculate δ. i Set P i The virtual target point is A i =(x a1 ,y ai ) T As shown in expression (35):

[0213] y ai =(δ i -δ0)*step+y a1 (35)

[0214] (3) Switching between virtual target points and actual target points

[0215] The actual target point and the virtual target point have the same function; at any given time, only one can exist. If the target point is reached and then switched to the actual target point, the planned obstacle avoidance path will become tortuous. Therefore, a target point switching strategy is set, as shown in formulas (36) and (37):

[0216]

[0217]

[0218] Equation (36) restricts the agricultural machinery from approaching obstacles in the longitudinal direction, and Equation (37) restricts the agricultural machinery from approaching obstacles in the lateral direction. Both equations must be satisfied simultaneously so that the agricultural machinery can maintain a safe distance from the obstacle when approaching it, and also allow the agricultural machinery to switch back to the actual target point before reaching the virtual target point.

[0219] 7) Path smoothing

[0220] B-spline interpolation is a technique that constructs a smooth curve by interpolating a set of discrete, unevenly distributed points between them. Using B-spline interpolation helps to smooth obstacle avoidance paths, eliminate points with abrupt curvature changes in the path, prevent sudden changes in the front wheel angle of the agricultural machinery during path tracking, avoid collisions with obstacles, and reduce the energy consumption of the agricultural machinery.

[0221] The definition of the B-spline function is shown in equation (38).

[0222]

[0223] In equation (33): m is the node x j The number of x j The range of values ​​for is [x0, x...]. m-1 ], x0 <x1<…<x m-1 ;P i As the control node, there are a total of mn-1 nodes; B i,n Let be an nth-order B-spline function, recursively defined as shown in equation (40).

[0224]

[0225]

[0226] In the B-spline curve formula, the value of n represents the smoothness of the curve. The larger the value of n, the smoother the curve, but the greater the computational complexity; the smaller the value of n, the worse the smoothness of the curve. To balance the smoothness of the agricultural machinery obstacle avoidance trajectory with computational complexity, this paper chooses a third-order B-spline curve, i.e., n=3.

[0227] 8) Evaluation Criteria

[0228] To improve the operational efficiency of agricultural machinery and reduce the cost of obstacle avoidance, it is necessary to set up a path point set for obstacle avoidance with virtual target points. i The optimal obstacle avoidance path is selected from (i = 1, 2, ..., n-1). i As shown in Equation (41), the obstacle avoidance path will be evaluated from three aspects: path length, path radius of curvature, and path smoothness.

[0229] Path i ={(x0,y0),(x1,y1),(x2,y2)…,(x n ,y n (41)

[0230] a) Path curvature estimation

[0231] During obstacle avoidance, the curvature K of the obstacle avoidance path must be continuous and the radius of curvature R must not be less than the minimum turning radius constraint R of the agricultural machinery. th To evaluate the obstacle avoidance path, its curvature needs to be calculated. In the Euclidean plane, the rate of change of the gradient with respect to the arc length is defined as curvature. Assuming the equation of the obstacle avoidance path curve is y = f(x), the curvature k(j) is shown in equation (42).

[0232]

[0233]

[0234]

[0235] The obstacle avoidance path is composed of discrete points; therefore, a method based on the forward and backward directions of a reference point is used to estimate the curvature of all path points. Obstacle avoidance paths with a minimum radius of curvature smaller than the minimum turning radius of the agricultural machinery are removed from the candidate path set. Since the obstacle avoidance paths vary in length, the average curvature J is used to represent the smoothness J of the obstacle avoidance path. i As shown in equation (45).

[0236]

[0237] b) Path length evaluation

[0238] Obstacle Avoidance Path i Calculate the length between two adjacent points using LD. i The length of the obstacle avoidance path is shown in equation (46).

[0239]

[0240] c) Evaluation function

[0241] Path smoothness index J i and path length index L i Since different units of measurement cannot comprehensively evaluate obstacle avoidance paths, they need to be normalized to obtain an index Q that can comprehensively evaluate path length and path smoothness. i .

[0242] Figure 21In cases a and b, the closer the obstacle avoidance starting point is to the obstacle, the shorter the path length and the greater the path curvature; conversely, the farther away the obstacle is from the obstacle, the longer the path length and the smaller the path curvature. Therefore, we choose starting point P. n The obstacle avoidance path of P0 is used as a reference path. The path evaluation metric Q... i As shown in equation (47), Q i A higher Q value indicates a shorter path length and a smoother obstacle avoidance path; conversely, a lower Q value indicates a longer path length and a greater curvature of the obstacle avoidance path. Therefore, choosing Q... i The largest one is selected as the optimal obstacle avoidance path, and the result is as follows: Figure 22 As shown.

[0243]

[0244] In equation (47), ω1 is the distance coefficient and ω2 is the curvature coefficient.

[0245] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0246] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. A dynamic path planning method for an unmanned agricultural machinery obstacle avoidance system based on virtual target points and virtual gravitational fields, characterized in that: Includes the following steps: Step 1: Using the navigation receiving and processing module, read the coordinates of the four vertices of the work area to establish the coordinate system of the work area and realize the conversion between the radar coordinate system and the vehicle coordinate system; Step 2: Perform voxelization and mesh downsampling on the point cloud data collected by lidar scanning, select the agricultural machinery obstacle detection area, remove outliers in the point cloud of the obstacle detection area, remove ground point cloud, cluster the obstacle point cloud, and sort the obstacles. Step 3: Calculation of the distance between the obstacle and the agricultural machinery, and the attitude of the obstacle model; Step 4: Based on the obstacle location, size, and attitude information obtained from Step 2 and Step 3, perform path planning to generate an obstacle avoidance path using the improved artificial potential field method. Step 4 includes: Step 4.1: Based on the size information of the obstacles, establish different obstacle repulsion field models, such as circular repulsion field and elliptical repulsion field; Step 4.2: To prevent the gravitational force on the agricultural machinery from being much greater than the repulsive force, a piecewise function is used to replace the gravitational field, and the gravitational field function is optimized. Step 4.3: When the agricultural machinery gets stuck in a local optimum, set a virtual target point to guide the agricultural machinery to avoid the obstacle; Step 4.4: To reduce the obstacle avoidance path length of agricultural machinery, an adaptive virtual target point method is adopted to generate multiple obstacle avoidance path clusters; Step 4.5: To select the optimal obstacle avoidance path, an evaluation function is designed to evaluate the obstacle avoidance path length and obstacle avoidance curvature, and then the function is normalized to obtain the optimal obstacle avoidance path that conforms to the kinematic constraints of agricultural machinery.

2. The dynamic path planning method for an unmanned agricultural machinery obstacle avoidance system based on a virtual target point and a virtual gravitational field according to claim 1, characterized in that: The specific process of step 1 is as follows: First, within its working area, the agricultural machinery uses its navigation module to read the coordinates of the four vertices of the field, establishing a global coordinate system for the field. Then, it performs coordinate system transformation; this transformation involves converting the radar coordinate system to the vehicle coordinate system and vice versa. The steps are as follows: The transformation from the lidar coordinate system to the vehicle coordinate system is shown in equation (1): (1)； in It refers to the installation position of the radar relative to the vehicle coordinate system; and These are the coordinates of the obstacle in the lidar coordinate system and the vehicle coordinate system, respectively. The transformation from the obstacle to the field coordinate system is shown in equation (2): (2)； in, For rotation matrix, These are the positioning coordinates of the agricultural machinery in the global coordinate system. The coordinates of the obstacle in the global coordinate system; (3)； The roll, pitch, and yaw angles are obtained through attitude sensors installed on the agricultural machinery. From equations (1) and (2), the transformation from the radar coordinate system to the field coordinate system is shown in equation (4): (4) 。 3. The dynamic path planning method for an unmanned agricultural machinery obstacle avoidance system based on a virtual target point and a virtual gravitational field according to claim 1, characterized in that: In step 2, it is necessary to extract point cloud data, use a plane fitting model to segment the ground point cloud and non-ground point cloud, perform Euclidean clustering on the non-ground point cloud, build an OBB bounding box model for the obstacle point cloud, and extract the size and position information of the obstacles.

4. The dynamic path planning method for an unmanned agricultural machinery obstacle avoidance system based on a virtual target point and a virtual gravitational field according to claim 1, characterized in that: In step 3, it is necessary to calculate the relative position of the obstacle and the agricultural machinery in the global coordinate system: Through steps 1 and 2, the transformation from the radar coordinate system to the vehicle coordinate system, and then the transformation from the vehicle coordinate system to the field coordinate system, yields the position coordinates of the obstacle in the field coordinate system. The position coordinates of agricultural machinery in the field coordinate system The coordinates of the intersection point of the obstacle model's major axis and the obstacle's principal axis in the global coordinate system. The distance between the obstacle and the agricultural machinery and obstacles in the global coordinate system Angle between axes As shown in (8)(9): (8)； (9) 。 5. An obstacle avoidance system for unmanned agricultural machinery based on virtual target points and virtual gravitational fields, employing the dynamic path planning method as described in any one of claims 1 to 4, characterized in that, Includes agricultural machinery, information acquisition and control unit and path planning and decision-making unit (45); the information acquisition and control unit and path planning and decision-making unit (45) are installed on the agricultural machinery; The agricultural machinery is a four-wheeled vehicle with Ackermann steering geometry, equipped with an electric steering wheel (18) and a speed adjustment device (42). The information acquisition and control unit consists of a vehicle controller (26), a front wheel angle sensor (15), a vehicle body posture sensor (17), an electric steering wheel (18), a throttle displacement sensor (16), a sensor signal conditioning module (40), a vehicle speed adjustment drive module (41), a vehicle speed adjustment device (42), an SD card (34), a DC voltage regulator module (38), a 12V DC power supply (36), a DC boost module (39), and a touch screen (37). The vehicle controller (26) is connected to the front wheel angle sensor (15), the vehicle body posture sensor (17), the electric steering wheel (18), the touch screen (37), the sensor signal conditioning module (40), the vehicle speed adjustment drive module (41), the SD card (34), and the DC voltage regulator module (38). In addition, the sensor signal conditioning module (40) is connected to the throttle displacement sensor (16), and the vehicle speed adjustment drive module (41) is connected to the vehicle speed adjustment device (42). The path planning and decision-making unit (45) consists of a lidar, an industrial control computer, and a navigation receiver; the lidar is connected to the industrial control computer through the WLAN network interface integrated inside the industrial control computer, and the lidar and the industrial control computer transmit data through the UDP protocol; the navigation receiver is connected to the industrial control computer through the RS485 interface integrated inside the industrial control computer. The information acquisition and control unit is connected to the RS232 interface in the path planning and decision-making unit (45) via the RS232 interface D of the vehicle controller (26).

6. The obstacle avoidance system for unmanned agricultural machinery based on virtual target points and virtual gravitational fields according to claim 5, characterized in that, The vehicle body attitude sensor (17) is an LPMS-IG1 RS232 metal housing attitude sensor, which is fixed to the horizontal plane at the center of the vehicle body by a vibration damping device; the vehicle body attitude sensor (17) is connected to the controller's RS232 interface A (29) via its built-in RS232; The front wheel angle sensor (15) is an LPMS-IG1 RS232 metal housing attitude sensor, which is fixed to the front wheel steering linkage shaft by a vibration damping device. The front wheel angle sensor (15) is connected to the controller's RS232 interface C (31) through its own RS232. The vehicle controller (26) is an EMB8616I industrial control board, which uses ST's STM32F107VCT6 as the MCU. The industrial control board integrates 4 RS232 serial ports, 1 RS485 serial port, 8 12-bit A / D conversion interfaces, 2 12-bit D / A interfaces and 1 SD card interface. The electric steering wheel (18) is an EMS2 steering drive motor developed by LianShi Navigation Technology Co., Ltd., which is connected to the vehicle controller (26) via RS232 interface B (30); The touch screen (37) is a DC80480F070-6111-0T resistive touch screen from Guangzhou Dacai Optoelectronics Technology Co., Ltd. The touch screen integrates an RS485 communication port and a TTL interface. The touch screen (37) is connected to the RS485 interface (32) of the vehicle controller (26) via an RS485 bus to display lateral deviation, heading deviation, obstacle information, vehicle attitude value, and the settings of forward sight distance and control parameters.

7. The obstacle avoidance system for unmanned agricultural machinery based on virtual target points and virtual gravitational fields according to claim 5, characterized in that, The speed adjustment device (42) consists of a throttle cable and a throttle electric push rod; one end of the throttle cable is connected to the throttle pedal of the agricultural machinery, and the other end is connected to the piston rod of the throttle electric push rod; the throttle electric push rod is fixed to the vehicle body by a support frame; the speed adjustment drive module (41) adopts an H-bridge drive circuit to drive the throttle electric push rod, realize the extension and retraction of the piston rod of the throttle electric push rod, and thus control the vehicle speed.

8. The obstacle avoidance system for unmanned agricultural machinery based on virtual target points and virtual gravitational fields according to claim 5, characterized in that, The throttle displacement sensor (16) is a displacement sensor integrated inside the electric throttle push rod. This displacement sensor converts the piston rod displacement into a corresponding resistance value to reflect the throttle position. The throttle displacement sensor (16) is connected to the A / D conversion interface (27) of the vehicle controller (26) through the sensor signal conditioning module (40). The sensor signal conditioning module (40) is used to convert the resistance value output by the throttle displacement sensor into a voltage signal of 0-5V.

9. The obstacle avoidance system for unmanned agricultural machinery based on virtual target points and virtual gravitational fields according to claim 5, characterized in that, The lidar mentioned is the Velodyne VLP-16 16-line lidar.

Images