Dynamic path planning and obstacle avoidance method for greenhouse robot

Abstract

The invention provides a dynamic path planning and obstacle avoidance method for a greenhouse robot, and belongs to the technical field of agricultural robots. The method comprises the following steps: collecting multi-source heterogeneous data and carrying out data synchronization and preprocessing; generating a reference trajectory through a path planning algorithm based on the preprocessed multi-source heterogeneous data; and on the basis of the reference trajectory, a controller is predicted through a self-adaptive model, constraint conditions are set with the minimum cost function as the target and with physical limitation and safety requirements of the robot, iteration is conducted through an optimization algorithm, and an optimal control sequence is output. According to the method, through online model parameter identification, the AMPC can adapt to the dynamic change and environmental disturbance of the robot, trajectory tracking control more accurate than that of a fixed model controller is provided, and the effect is remarkable especially when the load changes or the ground slips.

Description

Technical Field

[0001] This invention relates to the field of agricultural robot technology, and in particular to a dynamic path planning and obstacle avoidance method for greenhouse robots. Background Technology

[0002] In modern agricultural greenhouses, robots are core equipment for performing tasks such as sowing, inspection, and spraying. Their path planning and obstacle avoidance capabilities directly affect operational efficiency and crop safety. Currently, most greenhouse robots use preset fixed path patterns, relying on collecting two-dimensional coordinates of crop furrows and static facility location information to construct an environmental map. This method cannot effectively cope with dynamic changes occurring within the greenhouse, such as temporary workers entering the site, moving irrigation equipment, or changes in row spacing due to crop growth. Manual intervention to adjust the path is often required, resulting in low efficiency and a high risk of collisions or crop damage.

[0003] Furthermore, existing obstacle avoidance systems often rely on single sensors, resulting in incomplete perception and poor flexibility in densely packed crop rows. This necessitates large safety gaps, reducing space utilization. Simultaneously, the systems' control over obstacle avoidance and operational coordination is simplistic, typically triggering obstacle avoidance only based on distance thresholds. This fails to consider differences in crop growth status, ambient light, and ground flatness, leading to operational interruptions, crop damage, or equipment malfunctions in complex scenarios. Moreover, existing control methods have limited capacity to handle system constraints (such as robot dynamics constraints and environmental physical constraints), making it difficult to achieve real-time optimization in dynamic environments while ensuring safety. While Model Predictive Control (MPC) can handle constraint and optimization problems, traditional MPC models are fixed and struggle to adapt to changes in robot dynamic parameters (such as wheel slippage and load variations) and drastic changes in environmental characteristics within greenhouse environments.

[0004] Therefore, there is an urgent need for a dynamic path planning and obstacle avoidance method for greenhouse robots that can cope with dynamic environments, achieve precise obstacle avoidance, ensure collaborative operations, and have environmental adaptability. Summary of the Invention

[0005] In view of this, the present invention provides a dynamic path planning and obstacle avoidance method for greenhouse robots. The method collects greenhouse environment data by multi-dimensional sensors and combines it with the robot's own state to construct a nonlinear prediction model to obtain the predicted state. The objective function is determined by the predicted state and the path planning is carried out through optimization iteration.

[0006] Therefore, the present invention provides the following technical solution:

[0007] A dynamic path planning and obstacle avoidance method for a greenhouse robot includes: Collect multi-source heterogeneous data and perform data synchronization and preprocessing; generate a reference trajectory based on the preprocessed multi-source heterogeneous data using a path planning algorithm; Based on the reference trajectory, an adaptive model predicts the controller, aiming to minimize the cost function. Constraints are set according to the robot's physical limitations and safety requirements. An optimization algorithm is used for iteration to output the optimal control sequence.

[0008] Furthermore, the generation of a reference trajectory based on preprocessed multi-source heterogeneous data using a path planning algorithm includes: Using A The algorithm uses a global path planner to plan a collision-free initial path connecting the starting point and the target point, represented as a path sequence:

[0009] in, include , for The desired orientation of the point; By performing B-spline fitting on the discrete path point sequence, a continuous reference trajectory with arc length s as the parameter is obtained:

[0010] Where s is the path arc length parameter; This represents the X-coordinate of a point with arc length s on the trajectory in the global coordinate system, which is the horizontal position of the robot at that point. This represents the Y-coordinate of a point with arc length s on the trajectory in the global coordinate system, which is the vertical position of the robot at that point. This represents the tangent angle of the trajectory at a point with arc length s, which is the ideal orientation the robot should have at that point.

[0011] Furthermore, the cost function is:

[0012] in, Let k+i be the predicted state of the robot at time k+i. This is the reference state at the corresponding moment; The sequence of control inputs to be optimized is given by ; Q and R are given positive definite weight matrices used to balance tracking accuracy and control smoothness. Let be the predicted distance between the robot and the j-th obstacle; The preset safe distance threshold, Weighting for obstacle penalties; To predict the length of the time domain, To control the length of the time domain, M represents the number of obstacles.

[0013] Furthermore, the constraints include:

[0014] in, This represents the robot's future X-axis coordinate. This represents the X-axis coordinate of the future state of a dynamic obstacle. This represents the robot's future Y-axis coordinate. This represents the Y-axis coordinate of the future state of a dynamic obstacle.

[0015] Furthermore, the multi-source heterogeneous data includes: Greenhouse environmental data and robot status data; The greenhouse environment data includes: crop row spacing, soil firmness, temperature and humidity, and light intensity; The robot's state data includes: pose, velocity, wheel speed, and load.

[0016] Furthermore, the preprocessing includes: After filtering and denoising, the lidar point cloud is fused with camera data and segmented to extract elements such as the ground, crop rows, and obstacles. Multispectral images, through correction and semantic segmentation, identify passable areas and obstacles; The data from the inertial measurement unit and the encoder are fused using Kalman filtering to generate a smooth real-time pose and velocity estimate for the robot.

[0017] Advantages and positive effects of the present invention: This invention utilizes online model parameter identification, enabling AMPC to adapt to changes in robot dynamics and environmental disturbances, providing more precise trajectory tracking control than fixed model controllers, especially under load changes or slippery conditions. Based on predictive models, AMPC can anticipate the state several steps ahead and optimize current decisions accordingly, achieving proactive and smooth obstacle avoidance rather than passive reaction, thus enhancing safety. The proposed method unifies various constraints, such as the robot's physical limits and environmental obstacle boundaries, within an optimization framework, fundamentally ensuring the feasibility and safety of control commands. Structurally, an integrated design is employed, resulting in a more compact system. Multiple functional modules, including path tracking, motion control, and dynamic obstacle avoidance, are unified within a single core controller in AMPC, reducing the discontinuity and latency caused by module switching in traditional architectures, leading to faster system response and more coordinated behavior. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 This is a flowchart of the dynamic path planning and obstacle avoidance method for the greenhouse robot in Example 1; Figure 2 This is a schematic diagram of the obstacle avoidance trajectory generated by AMPC rolling optimization in Example 2; Figure 3 This is the AMPC control simulation block diagram in Example 2; Figure 4 This is a simulation diagram of the AMPC control effect in the greenhouse robot operation scenario in Example 2.

[0020] Figure 5 This is a logic diagram of the dynamic path planning and obstacle avoidance method for the greenhouse robot in Example 1.

[0021] Figure 6 This is the AMPC rolling optimization logic diagram in Example 1. Detailed Implementation

[0022] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0023] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0024] This invention provides a dynamic path planning and obstacle avoidance method for a greenhouse robot, comprising: Collect multi-source heterogeneous data and perform data synchronization and preprocessing; generate a reference trajectory based on the preprocessed multi-source heterogeneous data using a path planning algorithm; Based on the reference trajectory, an adaptive model predicts the controller, aiming to minimize the cost function. Constraints are set according to the robot's physical limitations and safety requirements. An optimization algorithm is used for iterative processing to output the optimal control sequence.

[0025] Example 1 like Figure 1 As shown, a dynamic path planning and obstacle avoidance method for a greenhouse robot includes the following steps: Step 1: Multi-sensor fusion in preprocessing.

[0026] 1. Real-time acquisition of multi-source heterogeneous data, including greenhouse environment data and robot status data, through multispectral cameras, lidar, soil sensors, IMU and wheel hub motor encoders.

[0027] Greenhouse environmental data includes: crop row spacing, soil firmness, temperature and humidity, and light intensity.

[0028] Robot status data includes: pose, velocity, wheel speed, and load.

[0029] 2. Perform data synchronization and preprocessing on the collected multi-source heterogeneous data to ensure data spatiotemporal consistency.

[0030] 1) Ensure time consistency of all sensor data through precise clock synchronization.

[0031] 2) The LiDAR point cloud is filtered and denoised, fused with camera data, and segmented to extract elements such as the ground, crop rows, and obstacles. Multispectral images are then corrected and semantically segmented to identify passable areas and obstacles. Inertial Measurement Unit (IMU) and encoder data are fused using Kalman filtering to generate smooth real-time pose and velocity estimates for the robot. This process transforms the raw data into reliable information that is spatiotemporally unified and has clear features, providing support for subsequent environmental modeling.

[0032] Step 2: Generate a reference trajectory based on the preprocessed multi-source heterogeneous data using a path planning algorithm.

[0033] 1. Using A The algorithm uses a global path planner to plan a collision-free initial path connecting the starting point and the target point, represented as a path sequence:

[0034] in, include , for The desired orientation of the point.

[0035] 2. Perform B-spline fitting on the discrete path point sequence to obtain a continuous reference trajectory with arc length s as the parameter:

[0036] Where s is the path arc length parameter; This represents the X-coordinate of a point with arc length s on the trajectory in the global coordinate system, which is the horizontal position of the robot at that point. This represents the Y-coordinate of a point with arc length s on the trajectory in the global coordinate system, which is the vertical position of the robot at that point. This represents the tangent angle of the trajectory at a point with arc length s, which is the ideal orientation the robot should have at that point.

[0037] Step 3, Combining Figure 5 and 6 As shown, an adaptive model predictive controller is constructed based on the reference trajectory; 1. An adaptive model predictive controller for a robot system is constructed based on preprocessed multi-source heterogeneous data to describe the robot's pose. With control input The dynamic relationship between linear velocity, angular velocity and disturbance d (such as wheel slippage, uneven ground):

[0038] in, This is the robot's pose at the next moment.

[0039] By using the recursive least squares method with a forgetting factor, based on real-time collected ontological state data, key parameters (such as the effective value of wheel radius and vehicle body rotational inertia) in the prediction model f are identified and updated online, enabling the prediction model to adapt to changes in the robot's dynamic characteristics.

[0040] 2. Input the reference trajectory into the Adaptive Model Predictive Controller (AMPC) to solve for the optimal control law, so that the robot's predicted future state sequence fits the reference trajectory as closely as possible, while satisfying all safety and dynamic constraints.

[0041] 3. With the goal of minimizing the cost function, constraints are set based on the robot's physical limitations and safety requirements. An optimization algorithm is used iteratively to output the optimal control sequence. The first control variable in the optimal control sequence is then applied to the robot.

[0042] Time domain generated using a linear Kalman filter Internal state sequence:

[0043] In each control cycle k, the following cost function J(k) is constructed for rolling optimization:

[0044] The first term enables the robot to track the global reference path, the second term suppresses drastic changes in the control variables, and the third term penalizes the robot when it gets too close to obstacles, thereby guiding the optimizer to generate a safe, smooth, and accurate trajectory. Let k+i be the predicted state of the robot at time k+i. This is the reference state at the corresponding moment; The sequence of control inputs to be optimized is given by ; Q and R are given positive definite weight matrices used to balance tracking accuracy and control smoothness. Let be the predicted distance between the robot and the j-th obstacle; The preset safe distance threshold, Weighting for obstacle penalties; To predict the length of the time domain, To control the length of the time domain ( ≤ M represents the number of obstacles.

[0045] 4. Obstacle avoidance constraints: Determine the safe zone around the obstacles.

[0046] Based on point cloud data, Euclidean clustering algorithm is used to separate point cloud clusters that do not belong to the static map and identify them as static obstacles. A circular area with the static obstacle as the center and a preset safe distance as the radius is designated as the safe area.

[0047] Each dynamic obstacle is tracked using a Kalman filter, and the state vector of dynamic obstacle j is:

[0048] in, Let x be the X coordinate of the dynamic obstacle j in the global coordinate system; Let j be the Y-coordinate of the dynamic obstacle j in the global coordinate system; Let be the velocity component of the dynamic obstacle j in the X direction; The velocity component of the dynamic obstacle j in the Y direction.

[0049] Centered on the predicted location, A circular area with a radius of 1 is designated as a safety zone; This is a preset safety margin used to provide additional buffer distance to cope with sensor errors, control delays, and uncertainties; Let the radius of the circumcircle of the robot body be denoted as . Let be the radius of the circumcircle of obstacle j; the radius of a dynamic obstacle can be preset with a conservative value based on the obstacle type.

[0050] The hard constraint for obstacle avoidance in AMPC is: at each step k+i in the prediction time domain, the minimum Euclidean distance between the robot profile and the obstacle profile j must be no less than the sum of their safety radii and safety margins, expressed as:

[0051] in, This represents the robot's future X-axis coordinate. This represents the X-axis coordinate of the future state of a dynamic obstacle. This represents the robot's future Y-axis coordinate. This represents the Y-axis coordinate of the future state of a dynamic obstacle.

[0052] Example 2 gather Figure 3 As shown, this is a simulation experiment of dynamic path planning and obstacle avoidance for a greenhouse robot in a dynamic environment. The experimental results are as follows. Figure 2 and Figure 4 As shown, the robot's path diagram under AMPC control demonstrates the path planning performance of the greenhouse robot based on adaptive model predictive control in a dynamic environment. The horizontal axis of the diagram represents the distance or position in the horizontal direction, ranging from 10 to 70, which may correspond to the actual length of the greenhouse environment or the coordinate index of a grid map. Figure 2 The image clearly shows the robot's trajectory, which is smooth and continuous, demonstrating the advantages of the AMPC algorithm in path optimization. The "X" points marked on the path map represent obstacle locations. The trajectory shape shows that while maintaining overall path tracking accuracy, the robot can achieve smooth local adjustments at specific locations, reflecting the real-time response capability of the AMPC system to dynamic obstacles. The continuity of the trajectory indicates that the system performs rolling optimization in each control cycle, generating a path that both conforms to the robot's dynamic constraints and effectively avoids obstacles in the environment.

[0053] Experiments show that the dynamic path planning and obstacle avoidance method for greenhouse robots proposed in this invention can effectively achieve obstacle avoidance, demonstrating the natural integration of global path planning and local obstacle avoidance. Through online model parameter identification and constrained optimization, this method can adapt to the dynamic changes in the greenhouse environment, generating safe, smooth motion trajectories that meet the requirements of agricultural operations, providing reliable technical support for autonomous operations in precision agriculture.

[0054] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A dynamic path planning and obstacle avoidance method for a greenhouse robot, characterized in that, include: Collect heterogeneous data from multiple sources and perform data synchronization and preprocessing; A reference trajectory is generated based on preprocessed multi-source heterogeneous data using a path planning algorithm; Based on the reference trajectory, an adaptive model predicts the controller, aiming to minimize the cost function. Constraints are set according to the robot's physical limitations and safety requirements. An optimization algorithm is used for iteration to output the optimal control sequence.

2. The method according to claim 1, characterized in that, The generation of reference trajectories based on preprocessed multi-source heterogeneous data using a path planning algorithm includes: Using A The algorithm uses a global path planner to plan a collision-free initial path connecting the starting point and the target point, represented as a path sequence: in, include , for The desired orientation of the point; By performing B-spline fitting on the discrete path point sequence, a continuous reference trajectory with arc length s as the parameter is obtained: Where s is the path arc length parameter; This represents the X-coordinate of a point with arc length s on the trajectory in the global coordinate system, which is the horizontal position of the robot at that point. This represents the Y-coordinate of a point with arc length s on the trajectory in the global coordinate system, which is the vertical position of the robot at that point. This represents the tangent angle of the trajectory at a point with arc length s, which is the ideal orientation the robot should have at that point.

3. The method according to claim 1, characterized in that, The cost function: in, Let k+i be the predicted state of the robot at time k+i. This is the reference state at the corresponding moment; The sequence of control inputs to be optimized is given by ; Q and R are given positive definite weight matrices used to balance tracking accuracy and control smoothness. Let be the predicted distance between the robot and the j-th obstacle; The preset safe distance threshold, Weighting for obstacle penalties; To predict the length of the time domain, To control the length of the time domain, M represents the number of obstacles.

4. The method according to claim 1, characterized in that, The constraints include: in, This represents the robot's future X-axis coordinate. This represents the X-axis coordinate of the future state of a dynamic obstacle. This represents the robot's future Y-axis coordinate. This represents the Y-axis coordinate of the future state of a dynamic obstacle.

5. The method according to claim 1, characterized in that, The multi-source heterogeneous data includes: Greenhouse environmental data and robot status data; The greenhouse environment data includes: crop row spacing, soil firmness, temperature and humidity, and light intensity; The robot's state data includes: pose, velocity, wheel speed, and load.

6. The method according to claim 1, characterized in that, The preprocessing includes: After filtering and denoising, the lidar point cloud is fused with camera data and segmented to extract elements such as the ground, crop rows, and obstacles. Multispectral images, through correction and semantic segmentation, identify passable areas and obstacles; The data from the inertial measurement unit and the encoder are fused using Kalman filtering to generate a smooth real-time pose and velocity estimate for the robot.

Images