An orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion

Abstract

The present application relates to the technical field of orchard unmanned vehicle, and particularly relates to an intelligent navigation and control method of orchard unmanned vehicle based on multi-sensor fusion, comprising the following steps: S1, global path planning: generating discrete plane path points, and then fitting a prior navigation global path in advance; S2, three-dimensional environment perception: real-time extracting static obstacle and dynamic obstacle information in the operation path, simultaneously detecting and identifying the target obstacle, and performing sensor information fusion; S3, path tracking control: controlling the orchard unmanned vehicle to travel according to the prior navigation global path; S4, dynamic path optimization: realizing dynamic programming of obstacle avoidance trajectory, synchronously establishing a trajectory coincidence degree evaluation mechanism, and adjusting the travel mode to avoid obstacles. The present application improves the reliability of environment perception and path dynamic adjustment, and constructs a three-dimensional motion control model, so as to ensure that the unmanned vehicle can safely and accurately complete autonomous navigation operation.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle technology in orchards, and in particular to an intelligent navigation and control method for unmanned vehicles in orchards based on multi-sensor fusion. Background Technology

[0002] Autonomous navigation in orchards is a key technology in smart agriculture, playing a vital role in improving orchard production efficiency, reducing labor costs, and achieving standardized and refined management of orchard production. However, the application of autonomous vehicles in dwarf rootstock densely planted orchards still faces many challenges. The orchard operating environment is complex and varied, with uneven terrain, ditches, utility poles, pedestrians, and other obstacles. Furthermore, the growth status and distribution of fruit trees also affect the path planning and operational accuracy of the autonomous vehicles.

[0003] Existing unmanned vehicle navigation systems in dwarf rootstock high-density orchards typically rely on pre-set fixed paths or simple geometric models for path planning, lacking the ability to perceive and dynamically adjust to the actual farmland environment in real time. In practice, when encountering sudden obstacles or terrain changes, orchard drones often fail to make timely and accurate obstacle avoidance decisions and path adjustments, thus affecting operational efficiency and quality. Furthermore, current single-sensor perception technologies are susceptible to environmental influences, resulting in poor performance and an inability to acquire semantic information about obstacles at long distances, leading to a lack of dynamic obstacle identification and early warning capabilities. Therefore, multi-sensor fusion-based autonomous navigation solutions for orchards are a key technology for the future highly intelligent operation of orchards.

[0004] Furthermore, most unmanned vehicles in orchards rely primarily on two-dimensional planar control technology, with the lateral controller often employing traditional PID control for path tracking. However, this control method is suitable for flat terrain but struggles to effectively handle vehicle attitude disturbances (such as rollover and pitch) caused by complex orchard terrain, leading to decreased path tracking accuracy and impacting operational efficiency and stability.

[0005] This invention addresses the pain points of low navigation accuracy, slow dynamic obstacle avoidance response, and poor terrain adaptability of unmanned vehicles in complex dwarf rootstock dense planting orchard scenarios by using multi-sensor fusion and a three-dimensional motion control model, providing a highly reliable and high-precision autonomous operation solution for smart agriculture. Summary of the Invention

[0006] This invention provides an intelligent navigation and control method for unmanned vehicles in orchards based on multi-sensor fusion.

[0007] A method for intelligent navigation and control of unmanned vehicles in orchards based on multi-sensor fusion includes the following steps:

[0008] S1, Global Path Planning: The orchard unmanned vehicle is equipped with an RTK-GNSS global navigation satellite system module. It travels in advance on the planned operation road and obtains the real-time latitude and longitude of the orchard unmanned vehicle through the RTK-GNSS global navigation satellite system module. Based on the Gauss-Kruger projection, it is converted into a set of planar path points to generate a priori navigation global trajectory.

[0009] S2, 3D environmental perception: The orchard unmanned vehicle is equipped with a lidar sensor and an industrial camera. The lidar sensor perceives and acquires the surrounding point cloud information in real time. Through voxel filtering dimensionality reduction, cloth model ground segmentation and improved Euclidean clustering algorithm, it detects static and dynamic obstacles in the operation path in real time. At the same time, the industrial camera uses the YOLOv11 deep learning network to detect and identify target obstacles in real time and perform sensor information fusion.

[0010] S3, Path Tracking Control: Combining prior navigation global trajectory, real-time latitude and longitude, wheel angle sensor data and IMU attitude angle information, it integrates steering feedforward, feedback and longitudinal feedback composite controller to construct a three-dimensional optimal lateral controller, which alleviates attitude disturbances caused by three-dimensional terrain and controls the orchard unmanned vehicle to drive according to the prior navigation global path.

[0011] S4, Dynamic Path Optimization: Based on real-time detected obstacle information, dynamic planning of obstacle avoidance trajectory is realized using the Bézier curve planning method. Simultaneously, a trajectory overlap evaluation mechanism is established to adjust the driving mode of the orchard unmanned vehicle to avoid obstacles.

[0012] Optionally, the global path planning in S1 includes:

[0013] S11, Calibration Alignment: Based on the internal and external parameters of the RTK-GNSS global navigation satellite system module receiver and the coordinate system transformation relationship, the orchard unmanned vehicle and the RTK-GNSS global navigation satellite system module are calibrated and aligned;

[0014] S12, Coordinate system transformation: Based on the Gauss-Kruger projection method, the latitude and longitude of the WGS84 coordinate system are transformed into the coordinate coefficient values ​​of the plane where the vehicle is located, and the prior navigation path is obtained by corresponding discretization fitting.

[0015] Optionally, the three-dimensional environment perception in S2 includes:

[0016] S21, 3D filter construction: Based on the prior navigation global path, standard crop spacing and row spacing, estimate the size of the LiDAR ROI region of interest, and construct a 3D filter for the size of the LiDAR ROI region of interest.

[0017] S22, Point Cloud Downsampling and Filtering: Based on the lidar sensor parameters and the density of the point cloud, a voxel filter block is set to output the downsampled point cloud;

[0018] S23, Point cloud data denoising and ground layer extraction: Remove invalid and outlier points from the point cloud data, fit the inverted point cloud information with a cloth model, analyze the corresponding node position constraints, and extract the ground layer point cloud information and the point cloud information on the ground.

[0019] S24, Dynamic Scene Target Layered Detection: Based on the segmented point cloud data of the ground, an improved Euclidean clustering algorithm is used to perform layered identification of static and dynamic obstacles, specifically including:

[0020] S241, Static obstacle detection: Set a first Euclidean distance threshold for the ground layer point cloud. With minimum point cloud number threshold , for satisfying < and Clustering generates static obstacle bounding boxes;

[0021] S242, Dynamic Target Tracking: Based on historical frame point cloud matching results, an adaptive distance threshold is set for the moving target point cloud. With dynamic point cloud quantity threshold Dynamic target bounding boxes are extracted through spatiotemporal correlation filtering;

[0022] S25, Obstacle Detection and Recognition: Based on the parameters of the industrial camera sensor, the YOLOv11 deep learning network is selected to detect and recognize the target obstacles in real time, and the category, bounding box and confidence score of the obstacles are obtained.

[0023] S26, Sensor Information Fusion: Jointly calibrate the lidar and industrial camera in time and space to obtain the transformation parameter matrix of their coordinate systems, and project the clustered obstacle point cloud onto the two-dimensional image to fuse the detection results of the two.

[0024] S27 combines the detection results and feeds the feature information back to the navigation control system for path adjustment and obstacle avoidance decisions.

[0025] Optionally, the path tracking control in S3 includes:

[0026] S31, Constructing the World Coordinate System: Based on the initial point information of the prior navigation global path, the calibration information of the RTK-GNSS global navigation satellite system module, vehicle position information, IMU attitude angle information, and the calibration information of the lidar sensor and industrial camera, construct the world coordinate system and dynamic / static coordinate system transformation architecture;

[0027] S32, Angle Mathematical Mapping Model Construction and Calibration Strategy: Based on the parameters returned by the vehicle's front wheel angle sensor and the vehicle's actual yaw angle, an angle mathematical mapping model is constructed to complete the calibration strategy based on the empirical model.

[0028] S33, Single-vehicle tracking geometric model construction: A single-vehicle tracking geometric model is constructed based on vehicle parameters, where the steering angular velocity of the vehicle's front wheels is... The distance from the rear wheel center to the vehicle's center of gravity is The distance from the center of the front wheel to the center of gravity of the vehicle is The vehicle's center of gravity speed is The angle between the velocity vector at the center of the vehicle's front axle and the vehicle's longitudinal axis is... The angle between the velocity vector at the center of the rear axle and the longitudinal axis of the vehicle is... The angle between the vehicle velocity vector and the vehicle's longitudinal axis is The steering angular velocity of the front wheels is expressed as:

[0029] ;

[0030] S34, Construction of the optimal lateral controller: The optimal lateral controller is constructed by integrating the calibration information of the RTK-GNSS global navigation satellite system module, the attitude angle information based on the IMU, the vehicle yaw information and the prior navigation trajectory, and combining the vehicle's forward look-ahead distance.

[0031] Optionally, the construction of the optimal lateral controller in S34 includes:

[0032] S341, Steering Feedforward Controller Construction: Constructing the optimal lateral controller and designing steering feedforward control to handle future road curvature, specifically including:

[0033] Nearest Forecast Point Search: Based on the positioning information of the RTK-GNSS global navigation satellite system module, a KD-tree is constructed to search for the nearest forecast point;

[0034] Recent Forecast Point Determination: Determine if the nearest forecast point is at the beginning or end of the global path. If so, move the nearest forecast point forward or backward to the nearest non-end discrete point, and calculate the road curvature based on the three-point circle method. If not, then based on the nearest preview point, search up to the set discrete interval for both the preceding and following points. The road curvature is calculated from discrete points using the three-point circle method. ;

[0035] Constructing a steering feedforward controller: Based on the center of gravity velocity of an unmanned vehicle in an orchard Vehicle wheelbase Road curvature Understeer gradient Construct a steering feedforward controller , is represented as:

[0036] ;

[0037] S342, Steering Feedback Controller Design: Constructing an optimal lateral controller and designing feedback control that minimizes tracking error, specifically including:

[0038] Forward look-ahead error calculation: based on the nearest look-ahead point, the current positioning position of the RTK-GNSS global navigation satellite system module, and the set forward look-ahead distance of the unmanned agricultural vehicle. Calculate the distance to the nearest aiming point lateral path deviation of vehicles Heading deviation and look-ahead error , is represented as:

[0039] ;

[0040] Steering feedback controller construction: based on calculation of look-ahead error Design proportional gain coefficient Construct a steering feedback controller , is represented as:

[0041] = ;

[0042] S343, Longitudinal Feedback Controller Design: Constructing the optimal lateral controller and designing longitudinal height feedback control, specifically including:

[0043] Vehicle positioning within / outside the trajectory marker: Based on the current RTK-GNSS global navigation satellite system module positioning position and navigation global path of the unmanned vehicle in the orchard, the vehicle positioning within / outside the trajectory marker is calculated. If the vehicle is inside the navigation global path trajectory, then... =1, if the vehicle is outside the global navigation path trajectory, then =-1;

[0044] Construction of longitudinal feedback controller: based on heading deviation and lateral path deviation Forward sight distance of unmanned agricultural vehicles Vehicles are located inside or outside the track (markers) Vehicle posture markings Calculate the projection of the lateral deviation onto the rolling plane. Design the longitudinal proportional gain coefficient And construct a longitudinal feedback controller To mitigate attitude disturbances caused by 3D terrain, the following is represented:

[0045] = .

[0046] Optionally, the optimal lateral controller is represented as:

[0047] ;

[0048] ;

[0049] in, This is the optimal lateral controller.

[0050] Optionally, the dynamic path optimization in S4 includes:

[0051] S41, Obstacle Detection: Determine whether an obstacle overlaps with the prior navigation trajectory, specifically including:

[0052] S411, overlap exists: regenerate the real-time local path and make dynamic obstacle avoidance decisions;

[0053] S412, no overlap: no need to regenerate real-time local paths, no dynamic obstacle avoidance decision-making.

[0054] Optionally, the existence of overlap in S411 includes:

[0055] S4111, Obstacle Obstruction Judgment: Determine whether the work road space blocked by obstacles is passable. If it is, control the orchard unmanned vehicle to generate a local path planner, generate a set of safe candidate trajectories that meet the kinematic constraints of orchard operation machinery according to the third-order Bézier curve method, select the optimal trajectory according to the cost function, drive along the optimal obstacle avoidance trajectory to complete dynamic obstacle avoidance, and return to the prior navigation trajectory. If it is not passable, control the orchard unmanned vehicle to suspend operation.

[0056] The equation of the third-order Bézier curve is defined as follows:

[0057] ;

[0058] in, The current position of the orchard robot serves as the starting point. It is a positive control point located at a fixed distance in the direction of robot velocity, and and These are control points obtained by forward prediction sampling along the reference line;

[0059] The cost function evaluates all safe candidate paths and selects the optimal obstacle avoidance path. The obstacle avoidance path is planned with the objectives of minimizing the curvature of the obstacle avoidance path and minimizing the deviation from the prior trajectory. That is, the trajectory with the minimum cost is selected as the optimal path.

[0060] ;

[0061] in, Represents the curvature cost weight, where It is the horizontal offset reward weight. This indicates the longitudinal distance of the unmanned vehicle in the orchard. Lateral displacement at that point, and for The first derivative;

[0062] S4112, Pause Operation Control: During the pause operation of the orchard unmanned vehicle, continuously monitor obstacles in front dynamically. After the obstacle leaves the prior navigation trajectory path, control the orchard unmanned vehicle to resume operation and continue to operate along the prior navigation trajectory.

[0063] Optionally, controlling the unmanned vehicle in the orchard to travel according to a priori navigation global path includes:

[0064] Speed ​​determination: Determine the longitudinal speed of the unmanned vehicle in the orchard and the deceleration position at the end of the prior navigation global path;

[0065] Deceleration position determination: Determine whether the orchard unmanned vehicle has reached the deceleration position at the end of the prior navigation global path. If so, control the orchard unmanned vehicle to decelerate smoothly until it reaches the end of the prior navigation global path and stops working. If not, control the orchard unmanned vehicle to continue traveling along the prior navigation global path at the set longitudinal speed.

[0066] The beneficial effects of this invention are:

[0067] This invention combines RTK-GNSS global navigation satellite system and lidar technology to achieve intelligent navigation and path control for unmanned vehicles in complex agricultural scenarios in orchards. It utilizes the Gauss-Kruger projection method to convert the latitude and longitude acquired by RTK-GNSS into path points in the vehicle's planar coordinate system, generating a priori global navigation path to ensure the accuracy of the unmanned vehicle's path planning. Combined with the point cloud data processing module of the lidar sensor, it can efficiently identify obstacles and field contours in agricultural operations. Through techniques such as 3D filtering, point cloud denoising, and clustering, it accurately constructs obstacle bounding boxes, improving the reliability of environmental perception and dynamic path adjustment, ensuring that the unmanned vehicle can safely and accurately complete autonomous navigation operations.

[0068] This invention achieves dynamic path tracking by constructing an optimal lateral controller and designing feedback and feedforward control to correct tracking errors. Through the design of a longitudinal feedback controller, based on the calculation of inner and outer marks on the trajectory and the analysis of roll plane projection, it effectively reduces the lateral control error and vehicle rollover tendency caused by three-dimensional terrain, significantly improving the navigation stability and operational accuracy of unmanned vehicles in complex agricultural scenarios, and providing important technical support for the promotion and application of unmanned vehicles in the field of smart agriculture. Attached Figure Description

[0069] To more clearly illustrate the technical solutions in this 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 only for this invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0070] Figure 1 This is a schematic flowchart of the navigation and control method according to an embodiment of the present invention;

[0071] Figure 2 This is a schematic diagram of multi-sensor fusion for three-dimensional environmental perception according to an embodiment of the present invention;

[0072] Figure 3 This is a schematic diagram of the orchard unmanned vehicle three-dimensional lateral controller system according to an embodiment of the present invention; Detailed Implementation

[0073] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. It should also be noted that, to make the embodiments more comprehensive, the following embodiments are the best and preferred embodiments, and those skilled in the art can use other alternative methods to implement some well-known technologies; moreover, the accompanying drawings are only for more specific description of the embodiments and are not intended to specifically limit the present invention.

[0074] It should be noted that the use of terms such as "an embodiment," "an embodiment," "an exemplary embodiment," and "some embodiments" in the specification indicates that the described embodiment may include a specific feature, structure, or characteristic, but not every embodiment necessarily includes that specific feature, structure, or characteristic. Furthermore, when a specific feature, structure, or characteristic is described in connection with an embodiment, implementing such a feature, structure, or characteristic in conjunction with other embodiments (whether explicitly described or not) should be within the knowledge of those skilled in the art.

[0075] Generally, terms can be understood at least partly from their use in context. For example, depending at least partly on the context, the term "one or more" as used herein can be used to describe any feature, structure, or characteristic in a singular sense, or a combination of features, structures, or characteristics in a plural sense. Additionally, the term "based on" can be understood not necessarily to convey an exclusive set of factors, but rather, alternatively, depending at least partly on the context, to allow for the presence of other factors that are not necessarily explicitly described.

[0076] like Figures 1-3 As shown, an intelligent navigation and control method for unmanned vehicles in orchards based on multi-sensor fusion includes the following steps:

[0077] S1, Global Path Planning: The orchard unmanned vehicle is equipped with an RTK-GNSS global navigation satellite system module. It travels in advance on the planned operation road and obtains the real-time latitude and longitude of the orchard unmanned vehicle through the RTK-GNSS global navigation satellite system module. Based on the Gauss-Kruger projection, it is converted into a set of planar path points to generate a priori navigation global trajectory.

[0078] S2, 3D environmental perception: The orchard unmanned vehicle is equipped with a lidar sensor and an industrial camera. The lidar sensor perceives and acquires the surrounding point cloud information in real time. Through voxel filtering dimensionality reduction, cloth model ground segmentation and improved Euclidean clustering algorithm, it detects static and dynamic obstacles in the operation path in real time. At the same time, the industrial camera uses the YOLOv11 deep learning network to detect and identify target obstacles in real time and perform sensor information fusion.

[0079] S3, Path Tracking Control: Combining prior navigation global trajectory, real-time latitude and longitude, wheel angle sensor data and IMU attitude angle information, it integrates steering feedforward, feedback and longitudinal feedback composite controller to construct a three-dimensional optimal lateral controller, which alleviates attitude disturbances caused by three-dimensional terrain and controls the orchard unmanned vehicle to drive according to the prior navigation global path.

[0080] S4, Dynamic Path Optimization: Based on real-time detected obstacle information, dynamic planning of obstacle avoidance trajectory is realized using the Bézier curve planning method. Simultaneously, a trajectory overlap evaluation mechanism is established to adjust the driving mode of the orchard unmanned vehicle to avoid obstacles.

[0081] Global path planning in S1 includes:

[0082] S11, Calibration Alignment: Based on the internal and external parameters of the RTK-GNSS global navigation satellite system module receiver and the coordinate system transformation relationship, the orchard unmanned vehicle and the RTK-GNSS global navigation satellite system module are calibrated and aligned;

[0083] S12, Coordinate system transformation: Based on the Gauss-Kruger projection method, the latitude and longitude of the WGS84 coordinate system are transformed into the coordinate coefficient values ​​of the plane where the vehicle is located, and the prior navigation path is obtained by corresponding discretization fitting.

[0084] The three-dimensional environment perception in S2 includes:

[0085] S21, 3D filter construction: Based on the prior navigation global path, standard crop spacing and row spacing, estimate the size of the LiDAR ROI region of interest, and construct a 3D filter for the size of the LiDAR ROI region of interest.

[0086] S22, Point Cloud Downsampling and Filtering: Based on the lidar sensor parameters and the density of the point cloud, a voxel filter block is set to output the downsampled point cloud;

[0087] S23, Point cloud data denoising and ground layer extraction: Remove invalid and outlier points from the point cloud data, fit the inverted point cloud information with a cloth model, analyze the corresponding node position constraints, and extract the ground layer point cloud information and the point cloud information on the ground.

[0088] S24, Dynamic Scene Target Layered Detection: Based on the segmented point cloud data of the ground, an improved Euclidean clustering algorithm is used to perform layered identification of static and dynamic obstacles, specifically including:

[0089] S241, Static obstacle detection: Set a first Euclidean distance threshold for the ground layer point cloud. With minimum point cloud number threshold , for satisfying < and Clustering generates static obstacle bounding boxes;

[0090] S242, Dynamic Target Tracking: Based on historical frame point cloud matching results, an adaptive distance threshold is set for the moving target point cloud. With dynamic point cloud quantity threshold Dynamic target bounding boxes are extracted through spatiotemporal correlation filtering;

[0091] S25, Obstacle Detection and Recognition: Based on the parameters of the industrial camera sensor, the YOLOv11 deep learning network is selected to detect and recognize the target obstacles in real time, and the category, bounding box and confidence score of the obstacles are obtained.

[0092] S26, Sensor Information Fusion: Jointly calibrate the lidar and industrial camera in time and space to obtain the transformation parameter matrix of their coordinate systems, and project the clustered obstacle point cloud onto the two-dimensional image to fuse the detection results of the two.

[0093] S27 combines the detection results and feeds the feature information back to the navigation control system for path adjustment and obstacle avoidance decisions.

[0094] Path tracking control in S3 includes:

[0095] S31, Constructing the World Coordinate System: Based on the initial point information of the prior navigation global path, the calibration information of the RTK-GNSS global navigation satellite system module, vehicle position information, IMU attitude angle information, and the calibration information of the lidar sensor and industrial camera, construct the world coordinate system and dynamic / static coordinate system transformation architecture;

[0096] S32, Angle Mathematical Mapping Model Construction and Calibration Strategy: Based on the parameters returned by the vehicle's front wheel angle sensor and the vehicle's actual yaw angle, an angle mathematical mapping model is constructed to complete the calibration strategy based on the empirical model.

[0097] S33, Single-vehicle tracking geometric model construction: A single-vehicle tracking geometric model is constructed based on vehicle parameters, where the steering angular velocity of the vehicle's front wheels is... The distance from the rear wheel center to the vehicle's center of gravity is The distance from the center of the front wheel to the center of gravity of the vehicle is The vehicle's center of gravity speed is The angle between the velocity vector at the center of the vehicle's front axle and the vehicle's longitudinal axis is... The angle between the velocity vector at the center of the rear axle and the longitudinal axis of the vehicle is... The angle between the vehicle velocity vector and the vehicle's longitudinal axis is The steering angular velocity of the front wheels is expressed as:

[0098] ;

[0099] S34, Construction of the optimal lateral controller: The optimal lateral controller is constructed by integrating the calibration information of the RTK-GNSS global navigation satellite system module, the attitude angle information based on the IMU, the vehicle yaw information and the prior navigation trajectory, and combining the vehicle's forward look-ahead distance.

[0100] The construction of the optimal lateral controller in S34 includes:

[0101] S341, Steering Feedforward Controller Construction: Constructing the optimal lateral controller and designing steering feedforward control to handle future road curvature, specifically including:

[0102] Nearest Forecast Point Search: Based on the positioning information of the RTK-GNSS global navigation satellite system module, a KD-tree is constructed to search for the nearest forecast point;

[0103] Recent Forecast Point Determination: Determine if the nearest forecast point is at the beginning or end of the global path. If so, move the nearest forecast point forward or backward to the nearest non-end discrete point, and calculate the road curvature based on the three-point circle method. If not, then based on the nearest preview point, search up to the set discrete interval for both the preceding and following points. The road curvature is calculated from discrete points using the three-point circle method. ;

[0104] Constructing a steering feedforward controller: Based on the center of gravity velocity of an unmanned vehicle in an orchard Vehicle wheelbase Road curvature Understeer gradient Construct a steering feedforward controller , is represented as:

[0105] ;

[0106] S342, Steering Feedback Controller Design: Constructing an optimal lateral controller and designing feedback control that minimizes tracking error, specifically including:

[0107] Forward look-ahead error calculation: based on the nearest look-ahead point, the current positioning position of the RTK-GNSS global navigation satellite system module, and the set forward look-ahead distance of the unmanned agricultural vehicle. Calculate the distance to the nearest aiming point lateral path deviation of vehicles Heading deviation and look-ahead error , is represented as:

[0108] ;

[0109] Steering feedback controller construction: based on calculation of look-ahead error Design proportional gain coefficient Construct a steering feedback controller , is represented as:

[0110] = ;

[0111] S343, Longitudinal Feedback Controller Design: Constructing the optimal lateral controller and designing longitudinal height feedback control, specifically including:

[0112] Vehicle positioning within / outside the trajectory marker: Based on the current RTK-GNSS global navigation satellite system module positioning position and navigation global path of the unmanned vehicle in the orchard, the vehicle positioning within / outside the trajectory marker is calculated. If the vehicle is inside the navigation global path trajectory, then... =1, if the vehicle is outside the global navigation path trajectory, then =-1;

[0113] Construction of longitudinal feedback controller: based on heading deviation and lateral path deviation Forward sight distance of unmanned agricultural vehicles Vehicles are located inside or outside the track (markers) Vehicle posture markings Calculate the projection of the lateral deviation onto the rolling plane. Design the longitudinal proportional gain coefficient And construct a longitudinal feedback controller To mitigate attitude disturbances caused by 3D terrain, the following is represented:

[0114] = .

[0115] The optimal lateral controller is represented as:

[0116] ;

[0117] ;

[0118] in, This is the optimal lateral controller.

[0119] Dynamic path optimization in S4 includes:

[0120] S41, Obstacle Detection: Determine whether an obstacle overlaps with the prior navigation trajectory, specifically including:

[0121] S411, overlap exists: regenerate the real-time local path and make dynamic obstacle avoidance decisions;

[0122] S412, no overlap: no need to regenerate real-time local paths, no dynamic obstacle avoidance decision-making.

[0123] The existence of overlap in S411 includes:

[0124] S4111, Obstacle Obstruction Judgment: Determine whether the work road space blocked by obstacles is passable. If it is, control the orchard unmanned vehicle to generate a local path planner, generate a set of safe candidate trajectories that meet the kinematic constraints of orchard operation machinery according to the third-order Bézier curve method, select the optimal trajectory according to the cost function, drive along the optimal obstacle avoidance trajectory to complete dynamic obstacle avoidance, and return to the prior navigation trajectory. If it is not passable, control the orchard unmanned vehicle to suspend operation.

[0125] The equation of a third-order Bézier curve is defined as follows:

[0126] ;

[0127] in, The current position of the orchard robot serves as the starting point. It is a positive control point located at a fixed distance in the direction of robot velocity, and and These are control points obtained by forward prediction sampling along the reference line;

[0128] The cost function evaluates all safe candidate paths and selects the optimal obstacle avoidance path. The obstacle avoidance path is planned with the objectives of minimizing the curvature of the obstacle avoidance path and minimizing the deviation from the prior trajectory. That is, the trajectory with the minimum cost is selected as the optimal path.

[0129] ;

[0130] in, Represents the curvature cost weight, where It is the horizontal offset reward weight. This indicates the longitudinal distance of the unmanned vehicle in the orchard. Lateral displacement at that point, and for The first derivative;

[0131] S4112, Pause Operation Control: During the pause operation of the orchard unmanned vehicle, continuously monitor obstacles in front dynamically. After the obstacle leaves the prior navigation trajectory path, control the orchard unmanned vehicle to resume operation and continue to operate along the prior navigation trajectory.

[0132] Controlling the unmanned vehicles in the orchard to travel according to the prior navigation global path includes:

[0133] Speed ​​determination: Determine the longitudinal speed of the unmanned vehicle in the orchard and the deceleration position at the end of the prior navigation global path;

[0134] Deceleration position determination: Determine whether the orchard unmanned vehicle has reached the deceleration position at the end of the prior navigation global path. If so, control the orchard unmanned vehicle to decelerate smoothly until it reaches the end of the prior navigation global path and stops working. If not, control the orchard unmanned vehicle to continue traveling along the prior navigation global path at the set longitudinal speed.

[0135] This invention encompasses any substitutions, modifications, equivalent methods, and solutions made within the spirit and scope of this invention. To provide the public with a thorough understanding of this invention, specific details are described in detail in the following preferred embodiments; however, those skilled in the art will fully understand the invention even without these details. Furthermore, to avoid unnecessary misunderstanding of the essence of this invention, well-known methods, processes, procedures, components, and circuits are not described in detail.

[0136] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. An orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion, characterized in that, The method comprises the following steps: S1, global path planning: the orchard unmanned vehicle is provided with an RTK-GNSS global navigation satellite system module, and is driven on a planned operation road in advance, the real-time longitude and latitude of the orchard unmanned vehicle is obtained through the RTK-GNSS global navigation satellite system module, the real-time longitude and latitude is converted into a plane path point set based on a Gauss-Kruger projection, and a prior navigation global trajectory is generated; S2, three-dimensional environment perception: the orchard unmanned vehicle is provided with a laser radar sensor and an industrial camera, point cloud information around the orchard unmanned vehicle is perceived and obtained in real time through the laser radar sensor, static obstacles and dynamic obstacles in the operation path are detected in real time through voxel filtering dimension reduction, cloth model ground segmentation and an improved Euclidean clustering algorithm, sensor information fusion is performed on the detected target obstacles through the industrial camera using a YOLOv11 deep learning network; S3, path tracking control: a three-dimensional optimal lateral controller is constructed by combining the prior navigation global trajectory, real-time longitude and latitude, wheel angle sensor data and IMU attitude angle information, a steering feedforward, feedback and longitudinal feedback compound controller is integrated, attitude disturbance caused by a three-dimensional terrain is relieved, and the orchard unmanned vehicle is controlled to travel according to the prior navigation global trajectory; S4, dynamic path optimization: according to the real-time detected obstacle information, dynamic planning of an obstacle avoidance trajectory is realized based on a Bezier curve planning method, a trajectory coincidence degree evaluation mechanism is established synchronously, and the driving mode of the orchard unmanned vehicle is adjusted to avoid the obstacles. 2.The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 1, characterized in that, The global path planning in S1 comprises: S11, calibration alignment: based on the internal and external parameters of the RTK-GNSS global navigation satellite system module receiver and the coordinate system conversion relationship, calibration alignment of the orchard unmanned vehicle and the RTK-GNSS global navigation satellite system module is realized; S12, coordinate system conversion: based on a Gauss-Kruger projection method, the WGS84 coordinate system longitude and latitude is converted into a value in the vehicle plane coordinate system, and a corresponding discretization fitting is performed to obtain the prior navigation path. 3.The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 2, characterized in that, The three-dimensional environment perception in S2 comprises: S21, three-dimensional filter construction: referring to the prior navigation global path, standard planting crop plant spacing and row spacing, the ROI region of interest size of the laser radar is estimated, and a three-dimensional filter of the ROI region of interest size of the laser radar is constructed; S22, point cloud downsampling and filtering: based on the laser radar sensor parameters and the point cloud density, a voxel filter block is set, and a down-sampled point cloud is output; S23, point cloud data denoising and ground layer extraction: invalid points and outliers in the point cloud data are removed, the inverted point cloud information is fitted by a cloth model, the corresponding node position constraint condition is analyzed, and the ground layer point cloud information and the ground point cloud information are extracted; S24, dynamic scene target hierarchical detection: based on the point cloud data after ground segmentation, the static obstacles and the dynamic obstacles are hierarchically identified through an improved Euclidean clustering algorithm, and specifically comprising: S241, static obstacle detection: set a first euclidean distance threshold for the ground layer point cloud with a minimum point cloud number threshold , generate a static obstacle bounding box for the cluster that satisfies and ​​ S242, dynamic target tracking: based on the historical frame point cloud matching result, set an adaptive distance threshold for the moving target point cloud and the dynamic point cloud number threshold , extract the dynamic target bounding box through spatio-temporal correlation filtering; S25, obstacle detection and identification: based on the industrial camera sensor parameters, a YOLOv11 deep learning network is selected to detect and identify the target obstacles in real time, and the category, bounding box and confidence of the obstacles are obtained. S26, sensor information fusion: joint calibration of lidar and industrial camera in time and space, obtain the conversion parameter matrix of the coordinate system of both, and project the clustered obstacle point cloud to the two-dimensional image, fuse the detection results of both; S27, combine the detection results, and feed the feature information back to the navigation control system for path adjustment and obstacle avoidance decision.

4. The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 3, characterized in that, The path tracking control in S3 includes: S31, constructing a world coordinate system: based on the initial point information of the prior navigation global path, the calibration information of the RTK-GNSS global navigation satellite system module, the vehicle position information, the IMU attitude angle information, and the calibration information of the lidar sensor and the industrial camera, a world coordinate system and a dynamic and static coordinate system conversion architecture are constructed; S32, angle mathematical mapping model construction and calibration strategy: based on the vehicle front wheel angle sensor return parameters and the actual vehicle yaw angle, an angle mathematical mapping model is constructed, and an empirical model-based calibration strategy is completed; S33, single vehicle tracking geometry model construction: a single vehicle tracking geometry model is constructed based on vehicle parameters, a front wheel steering angular velocity of the vehicle is , a distance from a rear wheel center to a vehicle gravity center is , a distance from a front wheel center to the vehicle gravity center is , a vehicle gravity center velocity is , an included angle between a velocity vector of a vehicle front axle center and a vehicle longitudinal axis is , an included angle between a velocity vector of a vehicle rear axle center and the vehicle longitudinal axis is , an included angle between a vehicle velocity vector and the vehicle longitudinal axis is , and the front wheel steering angular velocity is represented as ； S34, optimal lateral controller construction: fusion of RTK-GNSS global navigation satellite system module calibration information, IMU-based attitude angle information, vehicle yaw information, and prior navigation trajectory, combined with the vehicle forward distance, to construct an optimal lateral controller.

5. The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 4, characterized in that, The optimal lateral controller construction in S34 includes: S341, steering feedforward controller construction: constructing an optimal lateral controller, designing a steering feedforward control to process future road curvature, specifically including: Nearest preview point search: based on the positioning information of the RTK-GNSS global navigation satellite system module, a KD-tree search is constructed to find the nearest preview point; Recent preview point judgment: judging whether the recent preview point is at the head or tail of the global path, if yes, moving the recent preview point to the nearest non-end discrete point, and calculating the road curvature based on the three-point circle drawing method , if not, based on the recent preview point, searching the first discrete points in front and behind according to the set discrete interval, and then calculating the road curvature based on the three-point circle drawing method ; constructing a steering feedforward controller: based on orchard unmanned vehicle center of gravity velocity , vehicle wheelbase , road curvature , vehicle understeer gradient , constructing a steering feedforward controller , is expressed as: ； S342, steering feedback controller design: constructing an optimal lateral controller, designing a feedback control to minimize tracking error, specifically including: Prospective error calculation: based on the most recent preview point, the current RTK-GNSS global navigation satellite system module positioning location and the set unmanned agricultural vehicle forward vision distance , the most recent preview point distance , vehicle lateral path deviation , heading deviation and prospective error , expressed as: ； Steering feedback controller construction: based on computed look-ahead error , designing proportional gain coefficient , constructing steering feedback controller , is expressed as: = ； S343, longitudinal feedback controller design: constructing an optimal lateral controller, designing a feedback control for longitudinal height, specifically including: Vehicle inside-outside track sign calculation: according to the current RTK-GNSS global navigation satellite system module positioning position of the orchard unmanned vehicle and the navigation global path, the vehicle inside-outside track sign is calculated, if the vehicle is inside the track of the navigation global path, then =1, if the vehicle is outside the track of the navigation global path, then =-1; Longitudinal feedback controller construction: based on heading error and lateral path error , front-looking distance of unmanned agricultural vehicle , vehicle inside / outside trajectory marker , vehicle pose marker , calculate the projection of lateral error on the roll plane , design longitudinal proportional gain coefficient , and construct longitudinal feedback controller , to alleviate the attitude disturbance caused by three-dimensional terrain, expressed as: = 。 6. The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 5, characterized in that, The optimal lateral controller is represented as: ； ； wherein is the optimal lateral controller.

7. The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 6, characterized in that, The dynamic path optimization in S4 includes: S41, obstacle judgment: judging whether the obstacle overlaps with the prior navigation trajectory, specifically including: S411, overlap exists: regenerating a real-time local path for decision-making dynamic obstacle avoidance; S412, no overlap exists: no need to regenerate a real-time local path, and no decision-making dynamic obstacle avoidance is performed. 8.The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 7, characterized in that, The overlap exists in S411 includes: S4111, obstacle blocking judgment: judging whether the blocked work road space by the obstacle can be passed, if so, controlling the orchard unmanned vehicle to generate a local path planner, generating a safe candidate trajectory set that meets the kinematic constraints of orchard working machinery according to the three-order Bezier curve method; and selecting the optimal trajectory according to the cost function, driving along the optimal obstacle avoidance trajectory to complete dynamic obstacle avoidance, and returning to the prior navigation trajectory, if not, controlling the orchard unmanned vehicle to pause work; The equation of the three-order Bezier curve is defined as follows: ； wherein, is the current position of the orchard robot, as a starting point, is a forward control point located at a fixed distance in the direction of the robot's velocity, and and are control points obtained by forward prediction sampling along the reference line; The cost function evaluates all safe candidate paths, selects the optimal obstacle avoidance path, and plans the obstacle avoidance path with the minimum curvature and minimum deviation from the prior trajectory as the target, i.e. the trajectory with the minimum cost as the optimal path ； wherein, represents a curvature cost weight, wherein is a lateral offset reward weight, represents the lateral displacement of the orchard unmanned vehicle at a longitudinal distance from the target, and is the first derivative of S4112, suspend job control: during the suspension of the operation of the orchard unmanned vehicle, the front obstacles are dynamically monitored, and after the obstacles leave the prior navigation trajectory path, the operation of the orchard unmanned vehicle is resumed to continue the operation along the prior navigation trajectory.

9. The orchard unmanned vehicle intelligent navigation and control method based on multi-sensor fusion of claim 8, characterized in that, The control of the orchard unmanned vehicle to travel along the prior navigation global path comprises: speed determination: determining the longitudinal speed of the orchard unmanned vehicle and the deceleration position at the end of the prior navigation global path; deceleration position determination: determining whether the orchard unmanned vehicle reaches the deceleration position at the end of the prior navigation global path, if yes, controlling the orchard unmanned vehicle to smoothly decelerate until reaching the end of the prior navigation global path and stopping the operation, if not, controlling the orchard unmanned vehicle to continue traveling along the prior navigation global path at the set longitudinal speed.

Images