Improved rrt algorithm rice field operation optimal path generation system

Abstract

The invention discloses an improved rrt algorithm rice field operation optimal path generation system, and relates to the technical field of robot path planning. The system comprises a multi-layer cost map building module, an SLAM mapping module, a Gazebo simulation environment integration module, an improved RRT planner, a Dijkstra-Rviz2 joint optimization module and an ROS2 real-time calculation framework. The multi-layer cost map building module is used for building a multi-layer cost map according to the terrain and soil humidity of the rice field. According to the method, multi-dimensional path optimization in a rice field complex environment is realized through multi-layer dynamic cost map construction and a self-adaptive weight mechanism, a dynamic cost model based on obstacle density, soil humidity and terrain gradient is combined with a dynamic weight adjustment strategy, a high-resistance area is automatically avoided, and energy consumption is reduced; through a heuristic cost function of the improved RRT algorithm and adaptive step length control, the path convergence efficiency is significantly improved, and the problems that a traditional method is insufficient in perception of environment dynamic characteristics and limited in energy consumption optimization are effectively solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot path planning, and particularly relates to an improved RRT algorithm optimal path generation system for paddy field operations. Background Art

[0002] Farmland robots, namely agricultural robots, are programmable automated or semi-automated devices that take crops as the operation object and have the capabilities of environmental perception and autonomous movement. They can obtain environmental information in real time through multi-modal sensors (such as vision and touch), and adjust operation strategies based on dynamic algorithms to meet the diverse needs of complex paddy field scenarios. The paddy field operation environment has significant spatial heterogeneity, and its passage cost is affected by multiple dynamic factors such as soil humidity, terrain slope, crop density, and the distribution of temporary obstacles, forming a non-uniform cost distribution field. For example, in high-humidity areas, the traction of the robot decreases due to the increased viscosity of the soil, and the mobile energy consumption increases significantly. Although the flat and dry areas have high passage efficiency, the dense rice stubble or randomly scattered agricultural machinery parts may constitute local obstacles, and the path needs to be dynamically adjusted to balance obstacle avoidance and passage efficiency. In this scenario, path planning needs to simultaneously meet the geometric obstacle avoidance constraints and the global optimization objectives of multi-dimensional costs (such as energy consumption, time, and equipment wear).

[0003] The traditional Rapidly-Exploring Random Trees (RRT) algorithm quickly explores feasible paths through random sampling. However, its core defect is that it takes geometric connectivity as the single planning goal and ignores the differential impact of multi-modal environmental costs. Specifically, when the algorithm expands the search tree, it uses the Euclidean distance or random probability as the basis for node selection, and does not incorporate physical parameters such as soil humidity and slope into the cost evaluation system. This results in the generated path meeting the obstacle avoidance requirements, but it may frequently cross high-resistance areas (such as muddy or steep slopes), causing extended operation time, energy waste, and increased fatigue of the equipment's mechanical structure. Although existing improvement methods attempt to introduce static cost weights (such as a fixed obstacle density coefficient), they lack the ability to adaptively model the dynamic characteristics of the paddy field environment (such as a sudden increase in humidity after rainfall and changes in the ground surface flatness after harvesting). In addition, the strong randomness of the RRT algorithm easily causes the path to fall into a local sub-optimal solution. For example, it oscillates repeatedly near a narrow low-cost channel and is difficult to converge to the globally optimal cost path. Some studies use post-optimization strategies (such as Dijkstra smoothing) to make up for the defects of RRT, but the weight coefficients are fixed in multi-stage optimization and cannot dynamically adjust the path smoothness and the priority of costs according to the environmental evolution, resulting in insufficient path robustness in complex scenarios.

[0004] Therefore, we provide an improved RRT algorithm optimal path generation system for paddy field operations to solve the above problems. Summary of the Invention

[0005] The purpose of the present invention is to provide an improved RRT algorithm optimal path generation system for paddy field operations. By fusing multi-dimensional environmental parameters through a multi-layer dynamic cost map and combining the dynamic collaborative optimization of the improved RRT algorithm and the Dijkstra-Rviz2 joint optimization module, the problems in the prior art such as path planning ignoring the dynamic environmental changes in paddy fields, insufficient multi-dimensional cost optimization, and difficulty in balancing path smoothness and efficiency are solved.

[0006] To solve the above technical problems, the present invention is realized through the following technical solutions.

[0007] The present invention is an improved RRT algorithm optimal path generation system for paddy field operations, including a multi-layer cost map construction module, a SLAM mapping module, a Gazebo simulation environment integration module, an improved RRT planner, a Dijkstra-Rviz2 joint optimization module, and a ROS2 real-time computing framework;

[0008] The multi-layer cost map construction module is used to generate a dynamic cost map based on paddy field terrain, soil humidity, energy consumption, and dynamic obstacle data, including:

[0009] The obstacle cost calculation formula is:

[0010] The humidity cost calculation formula is:

[0011] The slope cost calculation formula is:

[0012] The total cost formula is: C(x,y) = 0.4·C obs (x,y) + 0.3·C hum (x,y) + 0.3·C slope (x,y);

[0013] The SLAM mapping module generates a global cost map by fusing lidar and IMU data based on the gmapping algorithm, and the map update frequency is 10Hz;

[0014] The Gazebo simulation environment integration module is used for three-dimensional physical simulation of paddy field terrain and robot kinematic models, and supports the simulation of RGB camera and lidar sensor plugins;

[0015] The improved RRT planner generates an initial path, and its expansion strategy includes:

[0016] The heuristic cost calculation formula for the randomly sampled point q rand is:

[0017] The adaptive step size control formula is as follows: where free_space is the proportion of sampling points not covered by obstacles;

[0018] The Dijkstra-Rviz2 joint optimization module performs multi-objective optimization on the initial path, including: optimizing the objective function:

[0019]

[0020] The Rviz2 visualization feedback mechanism displays the path curvature and the avoidance trajectory of dynamic obstacles in real time;

[0021] The ROS2 real-time computing framework integrates sensor data fusion, cost map update, and path execution control, including:

[0022] The update frequency of the lidar data is 20Hz, and the local cost map is refreshed every 100ms;

[0023] The CUDA 12.1 acceleration library implements GPU parallel computing, and the path planning time is ≤0.5 seconds.

[0024] The present invention is further configured that the multi-layer cost map construction module further includes: the ground segmentation algorithm extracts non-ground point clouds, calculates the slope θ and humidity H of the grid cells, the dynamic obstacle detection is implemented based on the costmap_2d plugin of ROS2, and the obstacle density is calculated by lidar point cloud clustering.

[0025] The present invention is further configured that the extended node selection strategy of the improved RRT planner is: calculating the total cost C(q new ) + h(q near , q rand ), and selecting the node with the minimum total cost as the extended node.

[0026] The present invention is further configured that the Dijkstra-Rviz2 joint optimization module further includes: the path smoothing constraint condition: the path needs to pass through all key points and avoid dynamic obstacles, the optimization target of the RMS value of the curvature is reduced from 0.12m to 0.07m, and it is verified in real time through the Rviz2 visualization interface.

[0027] The present invention is further configured that the ROS2 real-time computing framework further includes: the rolling window strategy realizes the dynamic expansion of the global cost map, the window size is 5m×5m, and the multi-robot cooperation is realized through the DDS communication mechanism, with a delay ≤50ms.

[0028] The present invention is further configured such that the improved RRT planner further includes: the dynamic weight adjustment mechanism, which adjusts the cost weight coefficients of obstacles, humidity, and slope in real time according to the environmental complexity (in the initial stage, α = 0.4, β = 0.3, γ = 0.3, and β is increased to 0.5 in high-humidity areas).

[0029] The present invention is further configured such that the Dijkstra-Rviz2 joint optimization module further includes: the multi-stage weight adjustment strategy: focusing on cost optimization in the initial stage (λ = 0.3), and focusing on path smoothness in the later stage (λ = 0.7), and the Rviz2 path visualization overlays a dynamic cost heat map.

[0030] The present invention is further configured such that the multi-layer cost map construction module includes: updating the local cost map based on lidar data every 100 ms, and the global cost map is dynamically fused with the rolling window strategy through the SLAM mapping module.

[0031] The present invention is further configured such that the improved RRT planner further includes: the adaptive space sampling strategy, which adjusts the sampling density according to the free space ratio, and the sampling density in high-obstacle areas is increased to 1.5 times the reference value.

[0032] The present invention is further configured such that the path execution module further includes: the real-time obstacle avoidance decision mechanism, which combines the dynamic cost map and the differential kinematics model, and the maximum steering angular velocity is limited to 0.8 rad / s.

[0033] The present invention has the following beneficial effects.

[0034] 1. Through the construction of a multi-layer dynamic cost map and an adaptive weight mechanism, the present invention realizes multi-dimensional path optimization in the complex paddy field environment. Based on the dynamic cost model of obstacle density, soil humidity, and terrain slope, combined with the dynamic weight adjustment strategy, it automatically avoids high-resistance areas and reduces energy consumption; through the heuristic cost function and adaptive step size control of the improved RRT algorithm, it significantly improves the path convergence efficiency and effectively solves the problems of insufficient perception of the dynamic characteristics of the environment and limited energy consumption optimization in traditional methods.

[0035] 2. Through the cooperation of the improved RRT and the Dijkstra-Rviz2 joint optimization module, the present invention takes into account the global optimality and smoothness of the path, adopts a phased optimization strategy, focuses on environmental cost optimization in the initial stage of path generation, and strengthens the path smoothness constraint in the later stage; combined with the dynamic obstacle avoidance mechanism and the rolling window strategy of the real-time computing framework, it realizes rapid response to dynamic obstacles and complex terrain changes, breaking through the bottlenecks of traditional methods in path oscillation and real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for the description of the embodiments will be briefly introduced below.

[0037] Figure 1 It is a flowchart of an optimal path generation system for paddy field operation using an improved RRT algorithm;

[0038] Figure 2 It is a flowchart for constructing a multi-layer cost map in an optimal path generation system for paddy field operation using an improved RRT algorithm;

[0039] Figure 3 It is a flowchart of an improved RRT path planning in an optimal path generation system for paddy field operation using an improved RRT algorithm. Specific Embodiments

[0040] Next, the technical solutions in the embodiments of the present invention will be described in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments.

[0041] Embodiment 1

[0042] Please refer to Figures 1-3 , the present invention is an optimal path generation system for paddy field operation using an improved RRT algorithm, including a multi-layer cost map construction module, a SLAM mapping module, a Gazebo simulation environment integration module, an improved RRT planner, a Dijkstra-Rviz2 joint optimization module, and a ROS2 real-time computing framework;

[0043] The multi-layer cost map construction module is used to generate a dynamic cost map based on paddy field terrain, soil humidity, energy consumption, and dynamic obstacle data, including:

[0044] The obstacle cost calculation formula is:

[0045] The humidity cost calculation formula is:

[0046] The slope cost calculation formula is:

[0047] The total cost formula is: C(x,y) = 0.4·C obs (x,y) + 0.3·C hum (x,y) + 0.3·C slope (x,y);

[0048] The SLAM mapping module generates a global cost map by fusing lidar and IMU data based on the gmapping algorithm, and the map update frequency is 10Hz;

[0049] The Gazebo simulation environment integration module is used for three-dimensional physical simulation of paddy field terrain and robot kinematic models, and supports the simulation of RGB camera and lidar sensor plugins;

[0050] The improved RRT planner generates an initial path, and its expansion strategy includes:

[0051] Random sampling point q rand The heuristic cost calculation formula for is:

[0052] The adaptive step size control formula is: where free_space is the proportion of sampling points not covered by obstacles;

[0053] The Dijkstra-Rviz2 joint optimization module performs multi-objective optimization on the initial path, including: optimization objective function:

[0054]

[0055] The Rviz2 visual feedback mechanism real-time displays the path curvature and the dynamic obstacle avoidance trajectory;

[0056] The ROS2 real-time computing framework integrates sensor data fusion, cost map update, and path execution control, including:

[0057] The lidar data update frequency is 20Hz, and the local cost map is refreshed every 100ms;

[0058] The CUDA 12.1 acceleration library realizes GPU parallel computing, and the path planning time consumption ≤ 0.5 seconds;

[0059] The multi-layer cost map construction module further includes: the ground segmentation algorithm extracts non-ground point clouds, calculates the slope θ and humidity H of the cells, the dynamic obstacle detection is implemented based on the costmap_2d plugin of ROS2, the obstacle density is calculated by lidar point cloud clustering, and the expansion node selection strategy of the improved RRT planner is: calculate the total cost C(q new )+h(q near ,q rand) Select the node with the minimum total cost as the expanding node. The Dijkstra-Rviz2 joint optimization module further includes: Path smoothing constraint: The path needs to pass through all key points and avoid dynamic obstacles. The optimization goal of the curvature RMS value is to decrease from 0.12 m to 0.07 m, and it is verified in real time through the Rviz2 visualization interface. The ROS2 real-time computing framework further includes: The rolling window strategy to achieve dynamic expansion of the global cost map, with a window size of 5 m × 5 m. Multi-robot cooperation is achieved through the DDS communication mechanism, with a latency ≤ 50 ms. The improved RRT planner further includes: A dynamic weight adjustment mechanism that adjusts the cost weight coefficients of obstacles, humidity, and slope in real time according to the environmental complexity (initially α = 0.4, β = 0.3, γ = 0.3, and β is increased to 0.5 in high-humidity areas). The Dijkstra-Rviz2 joint optimization module further includes: A multi-stage weight adjustment strategy: Focus on cost optimization in the initial stage (λ = 0.3), and focus on path smoothness in the later stage (λ = 0.7). The Rviz2 path visualization overlays a dynamic cost heat map. The multi-layer cost map construction module includes: Update the local cost map based on lidar data every 100 ms. The global cost map is dynamically fused with the rolling window strategy through the SLAM mapping module. The improved RRT planner further includes: An adaptive space sampling strategy that adjusts the sampling density according to the free space ratio. The sampling density in high-obstacle areas is increased to 1.5 times the reference value. The path execution module further includes: A real-time obstacle avoidance decision-making mechanism that combines the dynamic cost map and the differential kinematics model, with a maximum steering angular velocity limit of 0.8 rad / s

[0060] Specifically: Through the construction of a multi-layer dynamic cost map and an adaptive weight mechanism, multi-dimensional path optimization in a complex paddy field environment is achieved. Based on a dynamic cost model of obstacle density, soil humidity, and terrain slope, combined with a dynamic weight adjustment strategy, high-resistance areas are automatically avoided and energy consumption is reduced. Through the heuristic cost function and adaptive step size control of the improved RRT algorithm, the path convergence efficiency is significantly improved, effectively solving the problems of insufficient perception of environmental dynamic characteristics and limitations in energy consumption optimization of traditional methods. Through the cooperation of the improved RRT and the Dijkstra-Rviz2 joint optimization module, the global optimality and smoothness of the path are taken into account. A phased optimization strategy is adopted, focusing on environmental cost optimization in the initial stage of path generation and strengthening path smoothness constraints in the later stage. Combining the dynamic obstacle avoidance mechanism of the real-time computing framework with the rolling window strategy, rapid response to dynamic obstacles and complex terrain changes is achieved, breaking through the bottlenecks of traditional methods in path oscillation and real-time performance.

[0061] Embodiment 2

[0062] Please refer to Figures 1-3 , on the basis of Embodiment 1: Multi-objective path planning under complex terrain:

[0063] Technical solution

[0064] 1. Dynamic weight adjustment mechanism: A multi-layer cost map construction module is adopted to adjust the weight coefficients of obstacles (α), humidity (β), and slope (γ) according to the real-time terrain complexity;

[0065] When the detected slope θ≥15°, the slope weight γ is automatically increased to 0.5, and the formula is adjusted to:

[0066] C(x,y) = 0.3C obs +0.2C hum +0.5C slope

[0067] The improved RRT planner adopts an adaptive space sampling strategy, and in the steep slope area (θ>20°), the sampling density is increased to 2 times the reference value;

[0068] 2. Multi-objective optimization execution: Set a dual objective function through the Dijkstra-Rviz2 joint optimization module:

[0069]

[0070] Adopt a rolling window strategy to dynamically expand the 5m×5m local cost map and fuse the global terrain data of SLAM mapping;

[0071] Implementation effect: In the paddy field scenario with a 30° steep slope, the RMS value of the path curvature is optimized from 0.15m to 0.06m, the system automatically avoids 5 high-slope areas, the energy consumption during passage is reduced by 42%, and the dynamic weight adjustment stabilizes the planning time at 0.45 seconds (CUDA acceleration).

[0072] Example 3

[0073] Please refer to Figures 1-3 , on the basis of Example 1, the dynamic light interference processing for night operations:

[0074] Technical solution

[0075] 1. Multi-sensor fusion compensation: The Gazebo simulation environment loads a low-light texture (<10lux) and activates the lidar point cloud enhancement plugin;

[0076] The SLAM mapping module starts the IMU data compensation mode. When the lidar signal-to-noise ratio <15dB, the IMU weight is increased to 70%;

[0077] Add a light cost item to the cost map:

[0078] The total cost formula is adjusted to: C = 0.4C obs +0.2Chum +0.2C slope +0.2C light

[0079] 2. Dynamic obstacle avoidance enhancement: The self-adjusting step size of the improved RRT planner is adjusted to:

[0080]

[0081] The update frequency of the local cost map is increased to 15 Hz (original 10 Hz);

[0082] Implementation effect: In a 10 lux environment simulating moonlight, the accuracy of dynamic obstacle detection is increased from 72% to 89%. The path planner successfully avoids 3 newly added obstacles at night (harvesting leftovers), and the global map construction error is controlled within ±0.2 m (original ±0.5 m).

[0083] Example 4

[0084] Please refer to Figures 1-3 , based on Example 1, path self-recovery under extreme weather:

[0085] Technical solution

[0086] 1. Environmental mutation response mechanism: When the soil humidity sensor detects that H ≥ H threshold, it is automatically triggered:

[0087] The humidity weight β is increased to 0.6 (original 0.3), and the global cost formula is reconstructed as:

[0088] C = 0.2C obs +0.6C hum +0.2C slope

[0089] Start the path backtracking function and reconnect the broken path nodes through the Dijkstra algorithm;

[0090] 2. Anti-interference path optimization: The improved RRT planner loads the flood terrain model, and the self-adjusting step size is adjusted to:

[0091] Δq = 0.1(1 + log2(H / H threshold ))

[0092] The ROS2 real-time framework starts the emergency mode: The local map refresh interval is shortened to 50 ms, and the CUDA acceleration library enables FP16 precision calculation;

[0093] Implementation effect: In the simulated heavy rain (H = 1.8H threshold) scenario, the path self-recovery response time ≤ 0.3 seconds, the system successfully avoided 7 newly added water accumulation areas (C_hum > 0.8), and the curvature fluctuation of the re-planned path was reduced by 63% (RMS 0.05m).

[0094] The preferred embodiments of the present invention disclosed above are only used to help illustrate the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific embodiments described. These embodiments are selected and specifically described in this specification to better explain the principle and practical application of the present invention, so that those skilled in the art can well understand and utilize the present invention.

Claims

1. Improved RRT algorithm for generating the optimal path in paddy field operation, characterized in that: It includes a multi - layer cost map construction module, a SLAM mapping module, a Gazebo simulation environment integration module, an improved RRT planner, a Dijkstra - Rviz2 joint optimization module, and a ROS2 real - time computing framework; The multi - layer cost map construction module is used to generate a dynamic cost map based on paddy field terrain, soil humidity, energy consumption, and dynamic obstacle data, including: The formula for calculating the obstacle cost is as follows: The humidity cost calculation formula is as follows: (when H(x, y) < H 阈值 ) The slope cost calculation formula is as follows: The total cost formula is: C(x, y) = 0.4·C obs (x, y) + 0.3·C hum (x, y) + 0.3·C slope (x, y); The SLAM mapping module generates a global cost map by fusing lidar and IMU data based on the gmapping algorithm, and the map update frequency is 10Hz; The Gazebo simulation environment integration module is used for three - dimensional physical simulation of paddy field terrain and robot kinematic models, supporting RGB camera and lidar sensor plugin simulation; The improved RRT planner generates an initial path, and its expansion strategy includes: The random sampling point q rand has a heuristic cost calculation formula as follows: The adaptive step size control formula is as follows: where free_space is the proportion of sampling points not covered by obstacles; The Dijkstra - Rviz2 joint optimization module performs multi - objective optimization on the initial path, including: optimizing the objective function: The Rviz2 visual feedback mechanism real - time displays the path curvature and dynamic obstacle avoidance trajectory; The ROS2 real - time computing framework integrates sensor data fusion, cost map update, and path execution control, including: The lidar data update frequency is 20Hz, and the local cost map is refreshed every 100ms; The CUDA 12.1 acceleration library realizes GPU parallel computing, and the path planning time consumption ≤ 0.5 seconds.

2. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, wherein: The multi - layer cost map construction module further includes: the ground segmentation algorithm extracts non - ground point clouds, calculates the cell slope θ and humidity H, the dynamic obstacle detection is implemented based on the costmap_2d plugin of ROS2, and the obstacle density is calculated by lidar point cloud clustering.

3. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, wherein: The extended node selection strategy of the improved RRT planner is as follows: calculate the total cost C(q new ) + h(q near , q rand ), and select the node with the minimum total cost as the extended node.

4. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, characterized in that: The Dijkstra - Rviz2 joint optimization module further includes: the path smoothing constraint condition: the path needs to pass through all key points and avoid dynamic obstacles, the optimization target of the curvature RMS value is reduced from 0.12m to 0.07m, and it is verified in real - time through the Rviz2 visualization interface.

5. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, characterized in that: The ROS2 real - time computing framework further includes: the rolling window strategy realizes the dynamic expansion of the global cost map, the window size is 5m×5m, and the multi - robot cooperation is realized through the DDS communication mechanism, with a delay ≤ 50ms.

6. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, wherein: The improved RRT planner also includes: the dynamic weight adjustment mechanism, which adjusts the obstacle, humidity, and slope cost weight coefficients in real - time according to the environmental complexity (at the initial stage, α = 0.4, β = 0.3, γ = 0.3, and β is increased to 0.5 in high - humidity areas).

7. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, characterized in that: The Dijkstra - Rviz2 joint optimization module further includes: the multi - stage weight adjustment strategy: focusing on cost optimization in the initial stage (λ = 0.3), and focusing on path smoothness in the later stage (λ = 0.7), and the Rviz2 path visualization overlays a dynamic cost heat map.

8. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, characterized in that: The multi - layer cost map construction module includes: updating the local cost map based on lidar data every 100ms, and the global cost map is dynamically fused by the SLAM mapping module and the rolling window strategy.

9. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, characterized in that: The improved RRT planner further includes: the adaptive space sampling strategy, which adjusts the sampling density according to the free space ratio, and the sampling density in the high-obstacle area is increased to 1.5 times the reference value.

10. The improved RRT algorithm paddy field operation optimal path generation system according to claim 1, characterized in that: The path execution module further includes: the real-time obstacle avoidance decision-making mechanism, which combines the dynamic cost map and the differential kinematics model, and the maximum steering angular velocity is limited to 0.8 rad / s.

Images