An orchard environment agricultural robot autonomous operation control method and system

Abstract

The application provides an orchard environment agricultural robot autonomous operation control method and system. Based on the environment input information, the terrain surface fitting and local terrain attribute analysis are carried out, the local area passability evaluation result is obtained, the path planning, trajectory optimization and local trajectory tracking control are executed combined with the current state information and target track information of the robot, the robot is controlled to march to the pre-docking area; the target track image is collected, the image preprocessing and track feature extraction are carried out, the target track feature is obtained, the pose deviation parameter of the robot relative to the target track is calculated, and the visual track alignment control is executed; the robot up-track action is controlled, when the robot enters the target track and meets the stable driving condition, the zero return locking or operation mode switching is executed; when the abnormal state is detected, the back-off correction mechanism is triggered. The application improves the autonomous arrival capability, track identification accuracy, track alignment accuracy and up-track success rate of the agricultural robot in the complex environment of the orchard.

Description

Technical Field

[0001] This invention relates to the field of autonomous operation control technology for agricultural robots, specifically to a method and system for autonomous operation control of agricultural robots in an orchard environment. Background Technology

[0002] As modern orchard operations move towards intelligence and automation, agricultural robots, as crucial equipment for orchard transportation, inspection, harvesting assistance, and plant protection, are gradually becoming key mobile platforms in smart agriculture systems. Unlike greenhouses, factories, or regular road environments, orchard environments typically feature undulating terrain, limited space between rows, severe shading from tree branches and leaves, complex obstacle types, and fixed or specific operating tracks. These characteristics place higher demands on the autonomous mobility, environmental adaptability, and precision operation capabilities of agricultural robots.

[0003] Existing navigation methods for agricultural mobile robots or mobile operation platforms mostly employ path planning and trajectory tracking technologies based on satellite positioning, lidar, visual perception, or pre-set maps to achieve autonomous movement from the starting point to the target area. In relatively regular, flat, and open environments, these methods can meet the needs of mobile operations to a certain extent. However, in orchard environments, due to the complex terrain features such as potholes, varying slopes, localized mud, and raised tree roots, and the presence of dynamic or static obstacles such as branches and leaves obstructing the rows of fruit trees, pillars, farm tools, fallen fruit, and workers, traditional navigation methods often struggle to accurately model the traversability of complex terrain. They also fail to balance path safety, trajectory smoothness, and vehicle stability, resulting in insufficient navigation robustness, difficulties in local obstacle avoidance, and poor trajectory execution performance.

[0004] To improve navigation performance in complex environments, existing technologies have proposed several techniques based on 3D point cloud analysis, grid map construction, global path planning, and local optimization control. For example, environmental models are built using point cloud filtering, ground segmentation, and grid mapping, followed by path generation and local tracking control using sampling search algorithms, graph search algorithms, or model predictive control methods. While these technologies have made some progress in mobile robotics, they still have the following shortcomings in orchard scenarios: Firstly, existing path planning methods typically focus on two-dimensional obstacle avoidance, failing to fully reflect the slope, flatness, and sparseness of the orchard terrain, resulting in planned paths that, while reaching the target area, may not be suitable for stable passage by agricultural robots. Secondly, there are often discontinuities, lack of smoothness, or excessive curvature between discrete paths and the actual executable trajectory of the vehicle, making it difficult to meet the continuous and stable operation requirements of agricultural robots in narrow orchard passages.

[0005] Furthermore, in some orchard operation scenarios, agricultural robots not only need to perform general autonomous navigation, but also need to drive onto pre-set tracks or precisely dock with track structures to complete tasks such as transportation, lifting, harvesting assistance, or track-based operations. For such operational needs, simply relying on global navigation to drive the robot to the vicinity of the track is usually insufficient. Due to factors such as changes in lighting, foliage obstruction, uneven ground, and accumulated positioning errors in the orchard environment, the robot often still has a certain angular and lateral deviation from the track after reaching the vicinity of the target track. Without a precise track recognition and alignment control mechanism, problems such as inaccurate track alignment, multiple repeated adjustments, track mounting failure, or even mechanical interference can easily occur.

[0006] Among existing technologies related to track docking, one type of method mainly relies on manual remote control or manual-assisted positioning to complete track alignment and placement operations. This results in low automation and operational efficiency that is heavily influenced by human experience. Another type of method, while incorporating cameras, lidar, or proximity sensors for auxiliary detection, mostly only achieves simple target positioning or localized correction, lacking a complete closed-loop control mechanism from autonomous navigation, track recognition, deviation estimation, fine alignment to track placement execution. Especially in orchard environments, where tracks may be affected by soil cover, weed interference, shadow occlusion, or changes in local viewing angle, traditional vision methods have limited stability in extracting track features, making it difficult to accurately obtain the agricultural robot's attitude relationship relative to the target track. This impacts subsequent track alignment accuracy and track placement success rate.

[0007] Furthermore, existing technologies typically treat "navigation" and "alignment / mounting" as two independent processes, lacking a unified, integrated design. The navigation phase often focuses only on reaching the target area, without fully considering the subsequent visual inspection field of view, the initial alignment posture, and the conditions for mounting on the track. The alignment phase, on the other hand, often involves adjustments within a localized area, lacking effective integration with preceding path planning and trajectory control. This fragmented design can easily lead to agricultural robots reaching the vicinity of the track, but their position, orientation, or surrounding environment may not meet the requirements for high-quality visual inspection and mounting execution, thereby increasing system control complexity and reducing overall operational efficiency and reliability.

[0008] In summary, existing technologies for controlling agricultural robots in complex orchard environments have at least the following problems: 1) Insufficient analysis of the accessibility of complex orchard terrain and obstacle environment makes it difficult to achieve autonomous navigation that balances safety and feasibility; 2) The lack of effective continuous trajectory optimization and stable tracking control between the discrete path and the actual vehicle motion affects the smoothness and robustness of navigation; 3) The lack of high-precision visual inspection and attitude deviation calculation for the target track makes it difficult to achieve reliable track alignment; 4) The lack of an integrated closed-loop control mechanism that organically combines autonomous navigation, visual alignment, and track-on execution results in low alignment efficiency, low track-on success rate, and insufficient ability to back up in case of anomalies.

[0009] Currently, no descriptions or reports of technologies similar to this invention have been found, and no similar information has been collected domestically or internationally. Summary of the Invention

[0010] To address the aforementioned shortcomings in the existing technology, this invention provides a method and system for autonomous operation control of agricultural robots in an orchard environment.

[0011] According to one aspect of the present invention, an autonomous operation control method for agricultural robots in an orchard environment is provided, comprising: Acquire orchard environmental input information, current status information of agricultural robots, and target trajectory information, and determine the pre-docking area near the target trajectory; Based on the orchard environment input information, terrain surface fitting and local terrain attribute analysis are performed to obtain the accessibility evaluation results of local areas of the orchard. Based on the accessibility evaluation results, the current status information of the agricultural robot, and the target trajectory information, path planning, trajectory optimization, and local trajectory tracking control are performed, and the agricultural robot is controlled to autonomously move to the pre-docking area, realizing the autonomous navigation process of the agricultural robot from its current position to the pre-docking area; After the agricultural robot reaches the pre-docking area, it acquires a target trajectory image and performs image preprocessing and trajectory feature extraction on the target trajectory image to obtain the target trajectory features. Based on the characteristics of the target track, the pose deviation parameters of the agricultural robot relative to the target track are calculated, including angular deviation and lateral deviation. Visual orbit alignment control is performed based on the angular and lateral deviations. When the angular deviation and lateral deviation meet the preset upper rail conditions, the agricultural robot is controlled to perform the upper rail action, and the posture state of the agricultural robot, the wheel-rail relative position and the target track characteristics are detected during the upper rail process. Based on the attitude state, wheel-rail relative position, and target track characteristics, when it is determined that the agricultural robot has entered the target track and meets the stable driving conditions, it performs zero-locking or operation mode switching; when an abnormal state is detected, it triggers the backtracking correction mechanism and re-executes path planning, visual track alignment control, and / or upper track control.

[0012] According to a second aspect of the present invention, an autonomous operation control system for agricultural robots in an orchard environment is provided, comprising: The environmental perception module is used to acquire orchard environmental input information and target track information; The status acquisition module is used to acquire the current status information of the agricultural robot; The terrain analysis module is used to perform terrain surface fitting and local terrain attribute analysis on the input information of the orchard environment, and output the accessibility evaluation results of local areas of the orchard. The path planning and trajectory tracking module is used to perform path planning, trajectory optimization and local trajectory tracking control based on the drivability evaluation results, the current state information of the agricultural robot and the target trajectory information, and to control the agricultural robot to autonomously move to the pre-docking area near the target trajectory, so as to realize the autonomous navigation process of the agricultural robot from the current position to the pre-docking area; The visual track recognition module is used to acquire target track images and perform image preprocessing and target track feature extraction on the target track images; The pose deviation calculation module is used to calculate the angular and lateral deviations of the agricultural robot relative to the target track based on the extracted target track features. A visual track alignment module is used to perform visual track alignment control based on the angular deviation and lateral deviation. The upper rail control module is used to control the agricultural robot to perform the upper rail action when the angular deviation and lateral deviation of the agricultural robot meet the preset upper rail conditions, and to detect the posture state, wheel-rail relative position and target track characteristics of the agricultural robot during the upper rail process. The closed-loop detection and backtracking correction module is used to detect the agricultural robot's navigation status, track alignment status, and track status based on the agricultural robot's attitude status, wheel-rail relative position, and target track characteristics. When it is determined that the agricultural robot has entered the target track and meets the stable driving conditions, it is used to perform zero-locking or operation mode switching, and trigger the backtracking correction mechanism when an abnormal status is detected.

[0013] By adopting the above technical solution, the present invention has at least one of the following beneficial effects compared with the prior art: This invention improves the autonomous navigation capabilities, environmental adaptability, and operational stability of agricultural robots in complex orchard environments. By comprehensively analyzing point cloud, map, and obstacle information within the orchard environment, and combining local map modeling and drivability assessment, it provides a unified characterization of factors such as surface flatness, slope, sparsity, and obstacle distribution. Based on this, it completes global path planning, safe corridor construction, continuous trajectory optimization, and local planning control. Compared to existing technologies that only focus on two-dimensional obstacle avoidance or simple path tracking, this invention enables agricultural robots to more accurately identify drivable and high-risk areas, generating safer, smoother navigation trajectories that better conform to vehicle motion constraints. This enhances their autonomous navigation capabilities, environmental adaptability, and driving stability in orchard scenarios with undulating terrain, narrow passages, and complex obstacle distributions.

[0014] This invention improves the accuracy of target track recognition, track alignment, and track placement success rate for agricultural robots. After navigating to the vicinity of the target track, the agricultural robot acquires an image of the target track through onboard visual perception. Combining track feature extraction, image preprocessing, geometric transformation, and coordinate mapping, it calculates the angular and lateral deviations of the agricultural robot relative to the target track and performs coarse and fine alignment control based on these deviations. Compared to existing methods that rely on human experience, simple sensor detection, or localized single-step corrections, this invention can more accurately obtain the relative pose relationship between the vehicle and the track, quickly complete attitude adjustment under large deviation conditions, and further compensate for fixed and systematic errors under small deviation conditions. This effectively improves the accuracy of track recognition, the precision of track alignment, and the track placement success rate of the agricultural robot, reducing the risks of wheel-track misalignment, repeated corrections, and track placement failures.

[0015] This invention achieves integrated closed-loop control encompassing autonomous navigation, visual alignment, and track placement. It unifies orchard environment perception, accessibility analysis, path planning, trajectory optimization, local control, track detection, deviation calculation, alignment adjustment, and track placement, forming a complete closed-loop control process from autonomous movement from the starting position to the target track, achieving precise alignment, and stabilizing on the track. Furthermore, it incorporates a closed-loop detection and anomaly correction mechanism. When anomalies occur, such as path obstruction, trajectory tracking mismatch, loss of track features, excessive deviation, or track placement failure, the system can re-execute local planning, global planning, visual alignment, or track placement correction. This significantly enhances the system's fault tolerance, operational continuity, and reliability in complex orchard environments, demonstrating a high level of automation and significant engineering application value.

[0016] This invention integrates autonomous navigation, visual alignment, and on-track execution into a single design, which not only improves the autonomous arrival capability of agricultural robots in complex orchard environments, but also enhances track recognition accuracy, alignment accuracy, and on-track success rate, thereby increasing the robustness and operational continuity of the system.

[0017] This invention enables effective analysis of orchard environment accessibility, rational planning of navigation paths, smooth optimization and stable tracking of continuous trajectories. Based on this, it achieves high-precision deviation calculation of agricultural robots relative to the target track, track alignment control, and reliable track execution, thereby improving the autonomous operation capability, track docking accuracy, track success rate, and overall system robustness of agricultural robots in orchard environments. Attached Figure Description

[0018] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings: Figure 1 This is a flowchart illustrating the overall workflow of an autonomous operation control method for agricultural robots in an orchard environment, according to a preferred embodiment of the present invention.

[0019] Figure 2 This is a flowchart illustrating the autonomous operation control method for agricultural robots in an orchard environment, according to a specific embodiment of the present invention. Detailed Implementation

[0020] The embodiments of the present invention are described in detail below: These embodiments are implemented based on the technical solution of the present invention, and provide detailed implementation methods and specific operation processes. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention.

[0021] In existing technologies, agricultural robots designed for orchard environments often suffer from problems such as poor adaptability to complex terrain, insufficient feasibility of path planning, low stability of trajectory tracking, limited accuracy of track alignment, and low success rate of track mounting during autonomous navigation, track alignment, and track control.

[0022] To address the aforementioned problems, one embodiment of the present invention provides an autonomous operation control method for agricultural robots in orchard environments. This method, from three aspects—autonomous navigation, visual track alignment, and track control—controls the agricultural robot to autonomously navigate to a pre-docking area near the target track in the complex environment of an orchard, based on environmental perception information, terrain analysis results, and target track information. Within the pre-docking area, closed-loop control is performed based on onboard visual perception to complete target track image acquisition, track feature extraction, pose deviation calculation, coarse alignment control, fine alignment control, track execution, and abnormal backtracking correction. This method comprehensively realizes environmental perception and drivability analysis, global path planning, continuous trajectory optimization, local planning control, visual track alignment, and track execution control, thereby improving the autonomous navigation capability, track docking accuracy, and track operation reliability of agricultural robots in orchard environments.

[0023] Specifically, such as Figure 1 As shown in the embodiment, the autonomous operation control method for agricultural robots in an orchard environment may include: S1: Obtain orchard environment input information, current status information of agricultural robot, and target track information, and determine the pre-docking area near the target track; S2, based on the orchard environment input information, perform terrain surface fitting and local terrain attribute analysis to obtain the accessibility evaluation results of local areas of the orchard; S3, based on the drivability evaluation results, the current state information of the agricultural robot, and the target trajectory information, execute path planning, trajectory optimization, and local trajectory tracking control to control the agricultural robot to autonomously move to the pre-docking area near the target trajectory, thus realizing the autonomous navigation process of the agricultural robot from its current position to the pre-docking area; wherein, the process of acquiring orchard environment input information, performing terrain surface fitting and local terrain attribute analysis, executing path planning, trajectory optimization, and local trajectory tracking control, and controlling the agricultural robot to move to the pre-docking area together constitute the autonomous navigation process of the agricultural robot from its current position to the pre-docking area; S4. After the agricultural robot arrives at the pre-docking area, it acquires images of the target track and performs image preprocessing and track feature extraction on the target track images to obtain the target track features. S5, calculate the pose deviation parameters of the agricultural robot relative to the target track based on the characteristics of the target track. The pose deviation parameters include angular deviation and lateral deviation. S6, performs visual track alignment control based on angular and lateral deviations; S7, when the angular deviation and lateral deviation meet the preset upper rail conditions, control the agricultural robot to perform the upper rail action, and detect the posture state, wheel-rail relative position and target track characteristics of the agricultural robot during the upper rail process; S8, based on the agricultural robot's attitude state, wheel-rail relative position, and target track characteristics, when the agricultural robot is detected to have entered the target track and met stable driving conditions, a zero-return lock or operation mode switch is executed; when an abnormal state is detected, a backtracking correction mechanism is triggered, and path planning, visual track alignment control, and / or upper rail control are re-executed. The zero-return lock includes resetting and locking the upper rail mechanism, guiding mechanism, lateral movement mechanism, and / or chassis steering actuator to a preset zero position. The operation mode switch includes switching the agricultural robot's control state from ground autonomous navigation state or visual track alignment state to track driving state or track operation state.

[0024] In this embodiment: The orchard environment input information includes at least one of point cloud data, local obstacle information, and terrain information. The current status information of the agricultural robot includes at least one of current position, attitude, speed, and motion state. The target track information includes at least one of target track entrance position, target track extension direction, and pre-docking target point. The pre-docking target point is a target point located in the pre-docking area near the target track entrance, used as the termination position for the agricultural robot's autonomous navigation. The pre-docking target point is determined based on the target track entrance position, target track extension direction, agricultural robot current pose, effective field of view of the vision sensor, upper track attitude adjustment space, and local area drivability evaluation results.

[0025] Local terrain attribute analysis includes at least one of surface projection, local plane fitting, normal estimation, slope analysis, roughness analysis, and obstacle distribution analysis.

[0026] Visual track alignment control includes coarse alignment control and / or fine alignment control.

[0027] Abnormal conditions include at least one of the following: path blockage, trajectory tracking mismatch, loss of track features, deviation exceeding limits, track alignment failure, wheel-rail misalignment, and track failure.

[0028] In some preferred embodiments, S1, the pre-docking area, refers to a candidate area located near the target track entrance, used for the agricultural robot to switch from the autonomous navigation stage to the visual track alignment stage and the upper track control stage; wherein, the pre-docking area is determined based on the visual track detection field of view requirements, the upper track attitude adjustment requirements, and the passability evaluation requirements; wherein: The visual track detection field of view requirements include: the target track entrance position must be within the effective field of view of the agricultural robot's onboard visual sensor; The requirements for upper rail attitude adjustment include: the agricultural robot having motion space for angle and lateral correction within the pre-docking area; and / or, the angle between the initial orientation of the agricultural robot within the pre-docking area and the direction of the target track entrance is less than a preset navigation positioning threshold; wherein, the preset navigation positioning threshold is set based on the effective field of view of the vehicle-mounted vision sensor, the width of the target track entrance, the minimum turning radius of the agricultural robot, the available motion space within the pre-docking area, and the allowable attitude error of the upper rail, preferably 0° to 8°; The accessibility evaluation requirements include: the pre-docking area is located in an accessible position in front of or to the side of the target track entrance.

[0029] In some preferred embodiments, the above-mentioned S2, terrain surface fitting and local terrain attribute analysis, may further include: S21, performs filtering on the point cloud data and extracts ground points; S22, perform surface projection, local plane fitting and surface fitting on the point cloud data in the local area to obtain the fitting result; S23, based on the fitting results, calculate one or more terrain attributes among flatness, slope, roughness, elevation change, point cloud sparsity and obstacle distribution in the local area; S24. Based on slope, normal, roughness, elevation change, point cloud sparseness and / or obstacle distribution, determine the accessibility evaluation results and terrain risk cost of a local area; wherein, the terrain risk cost is reflected by the accessibility cost and is used to characterize the accessibility risk of a local area caused by excessive slope, abnormal normal change, surface roughness, abrupt elevation change, sparse point cloud or dense obstacles.

[0030] In some preferred embodiments, the above-mentioned S3, path planning, trajectory optimization, and local trajectory tracking control, may further include: S31, based on the drivability evaluation results and target trajectory information, a global discrete path is generated from the current position of the agricultural robot to the pre-docking area near the target trajectory, and path planning is performed; further, based on the drivability evaluation results and target trajectory information, a pre-docking target point is determined within the pre-docking area, and a global discrete path is generated from the current position of the agricultural robot to the pre-docking target point, and path planning is performed; wherein, the pre-docking target point is a target position within the pre-docking area that meets the visual trajectory detection field of view requirements, upper rail attitude adjustment requirements, navigation positioning angle requirements, and drivability evaluation requirements; S32, based on the global discrete path and the local obstacle distribution, construct a local safe and feasible region or safe corridor around the path; further, based on the global discrete path, the local obstacle distribution, and the terrain risk cost, construct a safe corridor around the path; wherein, the safe corridor consists of multiple local safe and feasible regions generated sequentially along the global discrete path, and the local safe and feasible region is a local feasible space that satisfies the obstacle safety distance constraint and the terrain risk cost constraint; S33, perform continuous trajectory construction and smoothing optimization on the global discrete path to obtain a continuous reference trajectory; S34, based on the current state of the agricultural robot, continuous reference trajectory, local obstacle changes, and terrain risk cost, local planning and trajectory tracking control planning are performed in the rolling time domain; further, based on the current state of the agricultural robot, continuous reference trajectory, local obstacle changes, and terrain risk cost, local planning and trajectory tracking control are performed in the rolling time domain; wherein, the terrain risk cost is the terrain risk cost determined in S2; the local obstacle changes are obtained from the real-time local map update results before the rolling time domain local planning, specifically by comparing the local obstacle distribution obtained in the current perception cycle with the local obstacle distribution obtained in the previous perception cycle to obtain changes in obstacle position, changes in obstacle quantity, changes in obstacle-occupied area, and / or changes in the minimum distance between the agricultural robot and obstacles; the local planning is used to generate a local predicted trajectory or predicted state sequence that avoids local obstacles and high-risk terrain areas in the prediction time domain based on the continuous reference trajectory, local obstacle changes, and terrain risk cost; the trajectory tracking control is used to generate a control input sequence for the agricultural robot based on the local predicted trajectory or predicted state sequence. S35, based on the results of local planning and trajectory tracking control, output speed control quantity, steering control quantity, angular velocity control quantity, or motion control command to drive the agricultural robot to the pre-docking area; further, based on the control input sequence obtained from the local planning and trajectory tracking control, select the first control quantity in the optimal control input sequence in the rolling time domain, and combine it with the chassis kinematic model of the agricultural robot to convert the first control quantity into speed control quantity, steering control quantity, angular velocity control quantity, or motion control command to drive the agricultural robot to autonomously move to the pre-docking target point.

[0031] In some preferred embodiments, the above-mentioned S4, image preprocessing and orbit feature extraction, may further include: S41, Perform camera distortion correction, filtering, enhancement, grayscale conversion and / or edge enhancement processing on the target track image to obtain a preprocessed image; S42, Extract the region of interest from the preprocessed image; S43, Perform inverse perspective transformation on the extracted region of interest to obtain the orbital feature distribution from a top-down perspective; S44, for the track feature distribution under the top-down view, extract the target track features; wherein, the target track features include: track line, track edge, track centerline, track region, track entrance direction, track endpoints and / or track feature points; wherein, the track line and / or track edge are used to fit or determine the target track centerline, the track centerline is used to calculate the lateral deviation of the agricultural robot relative to the target track, the track entrance direction is used to calculate the angular deviation between the direction of the agricultural robot body and the target track, the track region is used to determine the effective detection range of the target track in the image, and the track endpoints and / or track feature points are used to determine the target track entrance position, verify the track centerline, or assist in coordinate mapping.

[0032] Furthermore, in S4 above, the track edges are extracted based on edge detection; the track lines are extracted based on straight line fitting and / or curve fitting; the track centerline is calculated based on the left and right track lines or track edges; the track region is extracted based on image segmentation; the track entry direction is determined based on the track line extension direction, the track centerline direction, and / or the track endpoints; and the track endpoints and track feature points are extracted based on the track line ends, track edge intersections, track region boundary points, and / or key points output by the track detection network.

[0033] In some preferred embodiments, S5, calculating the pose deviation parameters of the agricultural robot relative to the target trajectory, may further include: S51, determine the centerline of the target track or the direction of the target track entrance based on the characteristics of the target track; S52, based on the mapping relationship between the image coordinate system and the vehicle coordinate system, transform the target track centerline, target track entrance direction, track endpoints and / or track feature points in the target track features to the vehicle coordinate system or ground coordinate system; S53, calculate the angular deviation between the direction of the agricultural robot body and the target track based on the angle between the direction of the target track centerline or the direction of the target track entrance and the direction of the agricultural robot body centerline in the vehicle coordinate system. S54, calculate the lateral deviation of the agricultural robot relative to the target track based on the distances of the centerline of the agricultural robot body, the centerline of the wheel set, and the upper rail reference point relative to the centerline of the target track; wherein, the upper rail reference point is a preset reference point used to characterize the lateral relative position of the vehicle body and the target track when the agricultural robot performs the upper rail action, and the upper rail reference point includes the center point of the front wheel set, the center point of the guide mechanism, the center point of the upper rail mechanism, the geometric center point of the chassis, and / or the vehicle body mounting reference point obtained by calibration; S55, the angular deviation and lateral deviation are compensated and corrected based on the pre-calibrated camera installation offset, vehicle structure offset, image geometric transformation error parameters, and / or vehicle attitude change. Specifically, the camera installation offset is obtained based on the position and direction offsets of the track centerline, track entry direction, track endpoints, and / or track feature points in the target track features within the image coordinate system, and is used to correct the angular deviation calculated from the target track features. The vehicle structure offset is obtained based on the structural installation offset between the vehicle centerline, wheel assembly centerline, and / or the upper rail reference point, and is used to correct the lateral deviation calculated based on the vehicle centerline, wheel assembly centerline, and upper rail reference point. The image geometric transformation error is obtained based on the transformation error of the track centerline, track entry direction, track endpoints, and / or track feature points during the mapping process from the image coordinate system to the vehicle coordinate system, and is used to correct the angular deviation and lateral deviation caused by the coordinate system mapping error. The vehicle attitude change is obtained based on the direction of the agricultural robot's vehicle centerline, the vehicle coordinate system attitude, and the position change of the target track features in the vehicle coordinate system, and is used to correct the angular deviation and lateral deviation caused by the vehicle attitude change.

[0034] In some preferred embodiments, the above-mentioned S6, coarse alignment control, may further include: S611, based on the corrected angle deviation and the corrected lateral deviation, determine whether the coarse alignment trigger condition is met; wherein the coarse alignment trigger condition is that the corrected angle deviation is greater than an angle threshold, and / or that the corrected lateral deviation is greater than a lateral threshold; at this time, generate chassis wheel speed, steering angle, linear velocity, angular velocity and / or lateral movement control commands based on the corrected angle deviation and / or the corrected lateral deviation, so as to perform attitude correction and position correction on the agricultural robot; S612 continuously acquires target track images during the coarse alignment process, re-extracts target track features, and calculates the updated angle deviation and updated lateral deviation in real time. S613, when the updated angle deviation is less than or equal to the angle threshold and the updated lateral deviation is less than or equal to the lateral threshold, switch to fine alignment control; S614 If the corrected angle deviation is less than or equal to the angle threshold, and the corrected lateral deviation is less than or equal to the lateral threshold, then skip the coarse alignment control and directly execute the fine alignment control.

[0035] In some preferred embodiments, the above-mentioned S6, fine alignment control, may further include: S621, after entering the fine alignment control, the target track image is re-acquired and the target track features are re-extracted. Based on the updated angle deviation and updated lateral deviation after the coarse alignment stage, combined with camera installation error, vehicle structure error, image geometric transformation error and / or vehicle attitude error, a second fine compensation is performed to obtain the fine-tuned angle deviation and fine-tuned lateral deviation after the fine alignment stage. S622, Based on the refined angle deviation and refined lateral deviation, generate corresponding control commands to perform fine angle correction, fine lateral correction and / or reverse fine adjustment on the agricultural robot until the angle deviation and lateral deviation between the agricultural robot and the target track meet the preset upper track conditions.

[0036] In some preferred embodiments, the above-mentioned S7, the upper rail action, may further include: S71, when the angular deviation of the agricultural robot relative to the target track is less than the upper rail threshold and the lateral deviation is less than the upper rail threshold, it is determined that the angular deviation and lateral deviation meet the preset upper rail conditions, and the upper rail control command is triggered; wherein, the upper rail threshold is set according to the target track entry direction, wheel-rail fit clearance, allowable attitude error of the upper rail mechanism and visual detection error, and is less than or equal to the first angular threshold, preferably 1° to 3°; the upper rail threshold is set according to the target track width, wheel set width, guide mechanism width, allowable wheel-rail deviation and pixel distance calibration coefficient, and is less than or equal to the first lateral threshold, preferably 5mm to 20mm.

[0037] S72, according to the upper rail control command, controls the agricultural robot to move along the target track entrance direction at a preset speed and preset posture; S73 monitors the relative positional relationship between the agricultural robot and the target track, the vehicle's attitude relationship, and the wheel-rail relative positional relationship in real time during the track-mounting process. S74, when it is detected that the agricultural robot has entered the target track, and the relative position error between the wheel assembly, guiding mechanism and / or upper rail mechanism and the target track is less than a preset position error threshold, and this position error is maintained for a preset time, the upper rail is deemed to be complete. The preset position error threshold is preferably 5mm to 20mm; the preset time is preferably 1s to 3s.

[0038] In some preferred embodiments, the above-mentioned S8, the rollback correction mechanism, may further include: S81, when the agricultural robot encounters path blockage, obstacle change exceeding limits, and / or trajectory tracking mismatch during autonomous navigation, the robot is controlled to stop its current navigation action and re-execute local planning, global path planning, and / or pre-docking area selection. Path blockage is determined based on the obstacle occupancy status, minimum passage width, or safe distance on the global discrete path, continuous reference trajectory, or local predicted trajectory. Path blockage is determined when the obstacle-occupied area overlaps with the safe passage area, or when the minimum distance between the agricultural robot and the obstacle is less than a preset safe distance. Obstacle change exceeding limits is determined based on the change in obstacle position, quantity, occupied area, and / or minimum distance between the current sensing cycle and the previous sensing cycle. Obstacle change exceeding limits is determined when the change exceeds a preset obstacle change threshold. Trajectory tracking mismatch is determined based on the position error, heading error, and / or velocity error between the agricultural robot's current pose and the continuous reference trajectory or local predicted trajectory. Trajectory tracking mismatch is determined when the position error, heading error, and / or velocity error exceeds a preset tracking error threshold and persists for a preset time. S82, when the agricultural robot experiences track feature loss, pose deviation exceeding limits, recognition confidence level below a threshold, and / or trajectory tracking control mismatch during visual track alignment, the agricultural robot is controlled to return to the previous correction state and / or pre-docking area, and the target track image acquisition, track feature extraction, and visual track alignment control are re-executed; wherein, the track feature loss is determined based on the detection results of track lines, track edges, track center lines, track entry directions, and / or track feature points in multiple consecutive frames of target track images. When the target track features are not detected within a preset number of frames or the detection results do not meet the integrity requirements, it is determined that the track features are lost; the pose deviation exceeding limits is based on the corrected angle deviation and / or corrected lateral deviation. The system determines that when the corrected angular deviation and / or the corrected lateral deviation are greater than a preset alignment deviation threshold, the pose deviation is deemed to be excessive. The recognition confidence level is determined based on the confidence level, fitting residual, and / or effective pixel ratio output by the track detection network, image segmentation results, or track line fitting results. When the confidence level is lower than a preset confidence threshold, the fitting residual is greater than a preset residual threshold, or the effective pixel ratio is lower than a preset ratio threshold, the recognition confidence level is deemed to be lower than the threshold. The trajectory tracking control mismatch is determined based on the deviation between the actual pose change and the expected pose change after the execution of the visual track alignment control command. When the deviation is greater than a preset control mismatch threshold and persists for a preset time, the trajectory tracking control mismatch is deemed to be incomplete. S83: When the agricultural robot experiences wheel-rail misalignment, abnormal posture, track deviation (track alignment failure) and / or track-mounting failure during the track-mounting process, the agricultural robot is controlled to exit the track-mounting action and re-execute fine alignment control, coarse alignment control or autonomous navigation control.

[0039] Based on the same inventive concept, another embodiment of the present invention provides an autonomous operation control system for agricultural robots in an orchard environment for implementing the above-described method of the present invention.

[0040] Specifically, the autonomous operation control system for agricultural robots in an orchard environment provided in this embodiment may further include: The environmental perception module is used to acquire orchard environmental input information and target track information; The status acquisition module is used to acquire the current status information of the agricultural robot; The terrain analysis module is used to perform terrain surface fitting and local terrain attribute analysis on the input information of the orchard environment, and output the accessibility evaluation results of local areas of the orchard. The path planning and trajectory tracking module is used to perform path planning, trajectory optimization and local trajectory tracking control based on the drivability evaluation results, the current state information of the agricultural robot and the target trajectory information, and to control the agricultural robot to autonomously move to the pre-docking area near the target trajectory, so as to realize the autonomous navigation process of the agricultural robot from the current position to the pre-docking area; The visual track recognition module is used to acquire target track images and perform image preprocessing and target track feature extraction on the target track images; The pose deviation calculation module is used to calculate the angular and lateral deviations of the agricultural robot relative to the target track based on the extracted target track features. A visual track alignment module is used to perform visual track alignment control based on the angular deviation and lateral deviation. The upper rail control module is used to control the agricultural robot to perform the upper rail action when the angular deviation and lateral deviation of the agricultural robot meet the preset upper rail conditions, and to detect the posture state, wheel-rail relative position and target track characteristics of the agricultural robot during the upper rail process. The closed-loop detection and backtracking correction module is used to detect the agricultural robot's navigation status, track alignment status, and track status based on the agricultural robot's attitude status, wheel-rail relative position, and target track characteristics. When it is determined that the agricultural robot has entered the target track and meets the stable driving conditions, it is used to perform zero-locking or operation mode switching, and trigger the backtracking correction mechanism when an abnormal status is detected.

[0041] Based on the same inventive concept, other embodiments of the present invention provide an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the method of any of the above embodiments of the present invention.

[0042] Optionally, the memory is used to store programs; the memory may include volatile memory, such as random-access memory (RAM), such as static random-access memory (SRAM), double data rate synchronous dynamic random-access memory (DDR SDRAM), etc.; the memory may also include non-volatile memory, such as flash memory. The memory is used to store computer programs (such as application programs and functional modules that implement the above methods), computer instructions, etc., and the aforementioned computer programs and computer instructions can be partitioned and stored in one or more memories. Furthermore, the aforementioned computer programs, computer instructions, data, etc., can be accessed by the processor.

[0043] A processor is used to execute computer programs stored in memory to implement the various steps in the methods or modules in the systems described in the above embodiments. For details, please refer to the relevant descriptions in the preceding method and system embodiments.

[0044] The processor and memory can be separate structures or integrated structures. When the processor and memory are separate structures, they can be coupled together via a bus.

[0045] Based on the same inventive concept, other embodiments of the present invention provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method of any of the above embodiments of the present invention.

[0046] Computer-readable media include computer storage media and communication media, wherein communication media include any medium that facilitates the transfer of computer programs from one place to another. Storage media can be any available medium accessible to a general-purpose or special-purpose computer. An exemplary storage medium is coupled to a processor, enabling the processor to read information from and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and storage medium can reside in an ASIC. Alternatively, the ASIC can reside in a user device. Of course, the processor and storage medium can also exist as discrete components in a communication device.

[0047] The autonomous operation control method and system for agricultural robots in an orchard environment provided in the above embodiments of the present invention are designed in the following ways for the three links of autonomous navigation, visual track alignment and track control: The method acquires orchard environment input information, current state information of the agricultural robot, and target track information; performs terrain surface fitting and local terrain attribute analysis based on environmental perception data to obtain the drivability evaluation results of local areas of the orchard; performs global path planning and local trajectory tracking control according to the drivability evaluation results to guide the agricultural robot to autonomously move to a pre-docking area near the target track; acquires target track images in the pre-docking area and performs image preprocessing, geometric transformation, and track feature extraction; calculates the angular deviation and lateral deviation of the agricultural robot relative to the target track based on the processed track features; performs visual track alignment control based on the angular deviation and lateral deviation, including coarse alignment control and fine alignment control; when the angular deviation and lateral deviation meet the preset track-up threshold, the method controls the agricultural robot to perform the track-up action; during the track-up process, the attitude state and wheel-rail relative position of the agricultural robot are detected in real time, and a backtracking correction and closed-loop retry mechanism is triggered when deviation exceeds the limit, track alignment fails, wheel-rail misalignment or track-up failure is detected. Compared with the prior art, the method and system provided by the above embodiments of the present invention integrate autonomous navigation, visual alignment and on-track execution, which not only improves the autonomous arrival capability of agricultural robots in complex orchard environments, but also improves track recognition accuracy, alignment accuracy and on-track success rate, and enhances the robustness and operational continuity of the system.

[0048] The working principle and specific working process of the technical solution provided in the above embodiments of the present invention will be further described in detail below with reference to a specific embodiment.

[0049] In orchard scenarios, due to uneven terrain, significant slope variations, narrow tree-lined spaces, and complex obstacle distribution, existing navigation methods often struggle to effectively model the orchard's surface flatness, slope, sparseness, and obstacle distribution. This results in paths that, while geometrically reaching the target area, fail to meet the safety and stable driving requirements of agricultural robots in actual operation. Furthermore, existing methods lack effective connection between discrete paths and the vehicle's actual trajectory, leading to discontinuous paths, uneven trajectories, excessive curvature variations, and local control instability. These issues negatively impact the navigation robustness and execution accuracy of agricultural robots in complex orchard environments.

[0050] Furthermore, after navigating to the vicinity of the target track, agricultural robots typically need to complete precise alignment with the track and the installation process. Due to factors such as changes in light intensity, shading from branches and leaves, uneven ground, installation errors, and visual perception errors in the orchard environment, existing technologies struggle to accurately extract track features and reliably calculate the angular and lateral deviations of the agricultural robot relative to the target track. This results in low efficiency in the alignment process, numerous repeated corrections, and even problems such as alignment failure, wheel-rail misalignment, or installation failure.

[0051] Furthermore, existing technologies typically treat autonomous navigation, visual alignment, and track execution as independent control processes, lacking a unified closed-loop control mechanism that integrates environmental perception, path planning, trajectory optimization, local control, track recognition, deviation estimation, precise alignment, and track execution. This results in agricultural robots failing to complete subsequent track operations even when they reach the vicinity of the target track due to unreasonable positioning, insufficient visual detection conditions, or poor initial alignment, thus affecting the overall system's operational efficiency and reliability.

[0052] Therefore, this invention provides an autonomous navigation, visual track alignment, and track-on-rail control technology for agricultural robots in complex orchard environments. This technology enables effective analysis of orchard accessibility, rational planning of navigation paths, smooth optimization and stable tracking of continuous trajectories. Based on this, it achieves high-precision deviation calculation, track alignment control, and reliable track-on-rail execution for agricultural robots relative to the target track, thereby improving the autonomous operation capability, track docking accuracy, track-on-rail success rate, and overall system robustness of agricultural robots in orchard environments.

[0053] The autonomous navigation, visual track alignment, and upper track control technology for agricultural robots in orchard environments provided by this invention are designed for complex orchard terrain, narrow passages, and track operation scenarios. It integrates environmental perception, terrain analysis, accessibility evaluation, autonomous path planning, local trajectory tracking, visual track recognition, pose deviation calculation, coarse alignment control, fine alignment control, upper track execution, and abnormal backtracking correction into a unified design. This forms a closed-loop control process from autonomous navigation on the ground to the vicinity of the target track, and then completes visual track alignment and stable upper track.

[0054] like Figure 2 As shown, this specific embodiment is based on the technical solution provided in the above embodiments of the present invention. The autonomous operation control method for agricultural robots in an orchard environment is designed for complex terrain and narrow operating passages in orchards. It combines local map analysis, drivability assessment, global path planning, continuous trajectory optimization, local predictive control, visual trajectory detection, deviation calculation, track alignment control, and track execution to form a closed-loop control process. This method mainly includes steps 1 to 8: Step 1: Obtain orchard environmental input information, current status information of the agricultural robot, and target trajectory information. Acquire orchard environmental input information, current status information of agricultural robots, and target trajectory information.

[0055] The orchard environment input information includes at least one of point cloud data, local obstacle information, terrain information, and map information; the current status information of the agricultural robot includes at least one of the current position, attitude, heading angle, speed, angular velocity, and operating status of the agricultural robot; the target track information includes at least one of the target track entry position, target track extension direction, estimated position of the target track centerline, target track width, and pre-docking target point.

[0056] The above information establishes the initial operating state of the agricultural robot in the orchard environment, providing an input basis for subsequent terrain analysis, autonomous navigation, visual alignment, and upper track control.

[0057] In step 1, the environmental input information of the agricultural robot in the orchard environment is first acquired. This input information includes point cloud data, local obstacle information, the robot's current state information, and the target trajectory area information. Let's assume the set of local point clouds collected by LiDAR or depth sensors... Represented as:

[0058] in, Indicates the total number of sampling points. Indicates the first A three-dimensional point, Represents the coordinates of a three-dimensional point.

[0059] The robot's current state It can be represented as:

[0060] in, Indicates the robot's position. Indicates the heading angle. Indicates linear velocity. It represents angular velocity.

[0061] Target point status Represented as:

[0062] Local obstacle collection It can be represented as:

[0063] in, The number of obstacles, each obstacle It can be characterized by its boundary points, bounding box, or occupied region.

[0064] To improve the stability of subsequent calculations, voxel downsampling and statistical filtering are preferably performed on the original point cloud. If the voxel mesh side length is... The downsampled point cloud Recorded as:

[0065] Furthermore, outliers are removed to obtain an effective point cloud. for:

[0066] Step 2: Perform terrain surface fitting and local accessibility evaluation Based on the input information of the orchard environment, terrain surface fitting and local terrain attribute analysis are performed on the local area around the agricultural robot to obtain the accessibility evaluation results of the local area of ​​the orchard.

[0067] Specifically, the point cloud data is filtered, outliers are removed, and ground points are extracted; the point cloud data in the local area is subjected to surface projection, local plane fitting, or surface fitting; based on the fitting results, at least one terrain attribute parameter among the slope, normal, roughness, elevation change, flatness, and obstacle distribution of the local area is calculated; and then the accessibility evaluation result or terrain risk cost of the local area is determined based on the terrain attribute parameters.

[0068] This step can determine whether a local area of ​​the orchard is suitable for agricultural robots to pass through, and provide terrain constraints for subsequent path planning and trajectory optimization.

[0069] In step 2, the effective point cloud is formed. Perform local map modeling and extract surface geometric features to complete the accessibility assessment.

[0070] Step 21. Local Region Division The working area in front of the agricultural robot is divided into several grid cells, denoted as the first... Each grid cell is The set of surface points it contains for:

[0071] Step 22. Surface plane fitting Perform plane fitting on the surface points in each grid cell, and let the local plane equation be:

[0072] in, Let be the normal vector component of the local fitting plane corresponding to the k-th grid cell. This is a constant term for the locally fitted plane. These represent the horizontal, vertical, and elevation coordinates of a point on the ground in a three-dimensional coordinate system, respectively, and k represents the raster cell number.

[0073] Solving parameters using the least squares method The objective function is:

[0074] To avoid zero solutions, constraints are usually added:

[0075] Step 23. Flatness Calculation Based on the distances from each fitted point to the plane, the first... Flatness index of each region for:

[0076] in, Point to the fitting plane The distance, that is:

[0077] Flatness index The smaller the value, the flatter the area.

[0078] Step 24. Slope Calculation Let the normal vector of the fitting plane be... for:

[0079] Unit vector in the vertical direction For reference, the regional slope angle It can be represented as:

[0080] because The above equation can be simplified to:

[0081] The steeper the slope, the greater the risk to vehicles.

[0082] Step 25. Roughness Calculation The roughness is determined based on the distance residual, residual variance, or root mean square distance from ground points within a local area to the fitted plane or fitted surface; wherein: Based on the local plane fitting results obtained in step 22, the distance residuals from each surface point in the local area to the fitted plane are calculated, and the roughness of the local area is determined according to the variance, root mean square value, or mean absolute value of the distance residuals. The greater the roughness, the more obvious the surface undulations, and the higher the stability risk when the agricultural robot passes through.

[0083] Step 26. Calculation of Elevation Change Elevation variations are determined based on the difference between the maximum and minimum elevations of ground points within a local area, the elevation difference between adjacent grid cells, or the height gradient of the fitted plane / surface; where: Based on the elevation information of ground points within a local area, the difference between the maximum and minimum elevations of ground points within that local area is calculated, and / or the elevation difference between adjacent grid cells is calculated to obtain an elevation change index. A greater elevation change indicates a higher likelihood of the presence of steps, potholes, or abrupt changes in terrain within the local area.

[0084] Step 27. Sparsity Calculation Let the first The number of points in each grid cell is The grid area is Then the sparsity of point clouds is defined as:

[0085] in, To prevent extremely small positive numbers with a denominator of zero. The greater the sparsity, the sparser the point cloud in the area, and the lower the reliability of the terrain estimation.

[0086] Step 28. Obstacle density calculation Let the first The number of obstacle points within each grid is Then the barrier density Defined as:

[0087] Step 29. Constructing the Feasibility Cost By weighting and integrating flatness, slope, sparsity, obstacle density roughness, and elevation variation, we obtain the first... The cost of passage in each area :

[0088] in, These represent the normalized flatness, slope, sparsity, roughness, and elevation change barrier density, respectively. For the corresponding weights, the following conditions must be met:

[0089] According to the passage cost The area can be divided into passable areas, cautious passable areas, and impassable areas to obtain passability evaluation results.

[0090] Step 3: Perform path planning, trajectory optimization, and local trajectory tracking control to enable the agricultural robot to autonomously move to the pre-docking area near the target track. Based on the accessibility evaluation results, the current status information of the agricultural robot, and the target trajectory information, path planning, trajectory optimization, and local trajectory tracking control are performed to control the agricultural robot to autonomously move to the pre-docking area near the target trajectory.

[0091] Specifically, a globally discrete path is searched within the passable area from the agricultural robot's current position to a pre-docking area near the target track. During the path search, path length, obstacle distribution, slope cost, terrain risk cost, and area passability cost are comprehensively considered. Multiple locally safe and feasible regions are constructed along the globally discrete path, and these regions are combined to form a safe corridor. A continuous trajectory is constructed and smoothed on the globally discrete path to obtain a continuous reference trajectory that satisfies the agricultural robot's kinematic or dynamic constraints. Then, combining the agricultural robot's current state, the continuous reference trajectory, local obstacle changes, and terrain risks, local planning and trajectory tracking control are performed in the rolling time domain, outputting speed control, steering control, angular velocity control, or motion control commands to enable the agricultural robot to reach the pre-docking area near the target track.

[0092] The pre-docking area is a candidate area located near the entrance of the target track and meeting the requirements of visual track detection field of view and upper track attitude adjustment. Preferably, the pre-docking area meets at least one of the following conditions: the entrance of the target track is within the effective field of view of the on-board visual sensor; the agricultural robot has motion space for angle correction and lateral correction; the angle between the initial orientation of the agricultural robot and the direction of the target track entrance is less than a preset navigation positioning threshold; the pre-docking area is located in a passable position in front of or to the side of the target track entrance.

[0093] In step 3, based on the mobility evaluation results (a mobility cost map) obtained in step 2, from the initial state... to the target pre-docking point Perform global path planning. Define the planned path. It consists of a series of discrete nodes:

[0094] in, Starting from, The target point.

[0095] In this step, the total path cost Defined as:

[0096] in, This represents the Euclidean distance between adjacent path nodes; Represents a node The cost of passage in the area; This represents the cost of obstacle risk near the node; These are the weighting coefficients.

[0097] The cost of obstacle risk can be further defined as:

[0098] in, Represents a node To the obstacle The minimum distance. By minimizing the total cost. To obtain the global discrete path :

[0099] Global planning algorithms can be implemented using RRT*, PF-RRT*, or other cost-constrained sampling planning algorithms.

[0100] Due to global path Typically, these are discrete node sequences and cannot be directly used as control inputs, therefore trajectory continuity and smoothing processing are required.

[0101] Discrete path nodes Construct a local convex region centered on a collision-free space. All convex areas combine to form a safety corridor. :

[0102] Optimized trajectory Must meet:

[0103] in, For control points; for B-order spline basis functions; To determine the number of control points.

[0104] To simultaneously consider trajectory length, smoothness, and obstacle safety, the following optimization objective is constructed. :

[0105] in, Indicates path length or energy term; Indicates the smoothing term; Indicates the penalty for obstacles; Weights. Obstacle penalty function. It can be defined as:

[0106] in, Let the safe distance threshold be the optimization problem. for:

[0107] It simultaneously satisfies initial and final state constraints as well as velocity, curvature, and safety corridor constraints.

[0108] The initial and final state constraints are expressed as follows:

[0109] in, This indicates the pose or position state of the continuous reference trajectory at the initial moment; This indicates the initial pose or initial position of the agricultural robot. It represents the pose or position state of the continuous reference trajectory at the termination time; This indicates the target pose or target position corresponding to the pre-docking target point.

[0110] To enable agricultural robots to stably track optimized trajectories Local planning is performed using model predictive control.

[0111] Assuming the agricultural robot uses a differential chassis, its discrete-time kinematic model is as follows:

[0112]

[0113]

[0114] in, Let x be the position coordinates of the agricultural robot along the x-direction of the global coordinate system or the ground coordinate system at the first sampling time. Let be the position coordinates of the agricultural robot along the y-direction of the global coordinate system or the ground coordinate system at the k-th sampling time. Let be the linear velocity of the agricultural robot at the k-th sampling time. Let be the heading angle of the agricultural robot at the k-th sampling time. Let be the angular velocity of the agricultural robot at the k-th sampling time. The sampling period is [period]. State vector. and control vector They are defined as follows:

[0115]

[0116] Let the reference trajectory be at the th The reference state corresponding to the time is Then the tracking error for:

[0117] In the prediction time domain Internally, define the optimization objective of model predictive control. for:

[0118] in, These are the error weight matrix, the control quantity weight matrix, and the control increment weight matrix, respectively. , representing the control input increment between adjacent prediction steps; This indicates the cost of risk in the market. Weights for risk items; This indicates the off-step index in the prediction time domain.

[0119] If obstacle risk and terrain risk are represented in a unified manner, then:

[0120] in, This represents the cost of the obstacle risk potential field constructed from the local obstacle distribution; This represents the terrain risk potential field cost constructed from the terrain risk cost or drivability cost;

[0121]

[0122] This indicates the passable cost corresponding to the current location. The terrain risk weights are used. The final solution is the optimal control sequence:

[0123] And take the first set of control quantities Applications to agricultural robots.

[0124] Step 4: Track image acquisition and visual track detection, extracting track features. Once the agricultural robot reaches the pre-docking area, it acquires images of the target track using onboard vision sensors, and performs image preprocessing and track feature extraction on the target track images to obtain the target track features.

[0125] Image preprocessing includes at least one of camera distortion correction, filtering, enhancement, grayscale conversion, edge enhancement, region of interest extraction, and inverse perspective transformation. Inverse perspective transformation converts the track image from the camera's viewpoint into a top-down viewpoint of track feature distribution, thereby reducing the impact of perspective distortion on track recognition.

[0126] The target track features include at least one of the following: track line, track edge, track centerline, track region, track entrance direction, track endpoint, and track feature points. Target track features can be obtained through at least one of the following methods: edge detection, line fitting, curve fitting, image segmentation, object detection, or a pre-trained track detection network.

[0127] Once the agricultural robot reaches the pre-docking area near the target track, it switches to track vision detection mode. Track images are acquired using the onboard camera; let the original image be: , in, These are the pixel coordinates of the image. First, distortion correction is performed on the original image to obtain the corrected image. :

[0128] in, For the camera intrinsic parameter matrix, These are the distortion parameters. Then, inverse perspective transformation (IPM) is performed on the corrected image to obtain the top view image. :

[0129] in, This is the homography transformation matrix. Then, using a track detection network or track line extraction algorithm, the pixel set of the left track line is obtained. and the right track pixel set :

[0130] The orbital line was fitted using a quadratic polynomial, resulting in the following:

[0131]

[0132] in, These are the coefficients of the quadratic term, the coefficients of the linear term, and the constant term of the quadratic polynomial on the left side of the orbital line, respectively. These are the coefficients of the quadratic term, the coefficients of the linear term, and the constant term of the quadratic polynomial on the right-hand track line, respectively.

[0133] Then the center line of the track It can be represented as:

[0134] Step 5: Calculate the pose deviation parameters of the agricultural robot relative to the target trajectory based on the characteristics of the target trajectory. Based on the characteristics of the target trajectory, calculate the pose deviation parameters of the agricultural robot relative to the target trajectory.

[0135] The pose deviation parameters include angular deviation and lateral deviation. Specifically, the target track centerline or target track entrance direction is determined based on the target track characteristics; a mapping relationship between the image coordinate system and the vehicle coordinate system is established based on camera intrinsic parameters, camera extrinsic parameters, and the agricultural robot's attitude information; the target track characteristics are transformed to the vehicle coordinate system or ground coordinate system; the angular deviation is calculated based on the angle between the agricultural robot's vehicle centerline direction and the target track entrance direction; and the lateral deviation is calculated based on the distance of the agricultural robot's vehicle centerline, wheel assembly centerline, or upper rail reference point relative to the target track centerline.

[0136] Preferably, when calculating the pose deviation parameters, compensation is also made for camera installation error, vehicle structure error, image geometric transformation error, and attitude changes such as vehicle pitch and roll, so as to improve the calculation accuracy of angle deviation and lateral deviation.

[0137] In step 5, the lateral and angular deviations of the agricultural robot relative to the target track are calculated based on the track centerline.

[0138] Step 51. Calculation of Lateral Deviation Let the pixel coordinates of the reference center point of the agricultural robot in the top view image be... The centerline of the track is The x-coordinate of the pixel at that location is:

[0139] in, Let be the fitting coefficients for the orbital centerline. Then the lateral deviation in the pixel domain is:

[0140] The conversion coefficient from pixel to actual distance can be obtained through calibration. The actual lateral deviation for:

[0141] If fixed installation error compensation is considered The corrected lateral deviation is:

[0142] Step 52. Calculation of Angular Deviation The centerline of the track is at point The slope at is:

[0143] Therefore, the direction angle of the orbit centerline relative to the vertical axis of the image It can be represented as:

[0144] Let the reference angle of the longitudinal direction of the agricultural robot body in the top view image be . Then the pixel domain angle deviation for:

[0145] If we further consider the fixed angle calibration error The corrected angle deviation for:

[0146] Step 53. Joint Deviation Vector Therefore, the pose deviation vector of the agricultural robot relative to the track It can be represented as:

[0147] This deviation vector serves as the input for subsequent track control.

[0148] Step 6: Perform visual track alignment control based on angular and lateral deviations. Visual orbit alignment control is performed based on angular and lateral deviations. Visual orbit alignment control includes coarse alignment control and fine alignment control.

[0149] When the angular deviation and / or lateral deviation exceed a first preset threshold, coarse alignment control is executed. Coarse alignment control generates chassis steering control, wheel speed control, linear velocity control, angular velocity control, and / or lateral movement control based on the angular and lateral deviations, controlling the agricultural robot to perform attitude and position corrections. During coarse alignment, target track images are continuously acquired, and the angular and lateral deviations are updated in real time until they are reduced to within the first preset threshold range.

[0150] Once the angular and lateral deviations are reduced to within the first preset threshold range, fine alignment control is executed. Fine alignment control includes compensating for camera installation errors, vehicle structure errors, image geometric transformation errors, and / or vehicle posture errors; after compensation, the target track image is reacquired, and track feature extraction and pose deviation calculation are re-performed; based on the recalculated angular and lateral deviations, fine angle correction, fine lateral correction, and / or reversing fine-tuning are performed until the angular and lateral deviations between the agricultural robot and the target track meet the preset upper track conditions.

[0151] In step 6, based on the deviation vector Perform track alignment control.

[0152] Step 61. Coarse Alignment Control When the lateral or angular deviation exceeds the preset coarse alignment threshold, the following condition is met:

[0153] Then the coarse alignment control stage begins. At this point, the control law can be designed as follows:

[0154]

[0155] in, This is the linear velocity control value output during the coarse alignment control stage. This refers to the angular velocity control value output during the coarse alignment control stage. Based on forward speed, This is for gain control. It means that when the angular deviation is large, the forward speed is automatically reduced; simultaneously, angular velocity control prioritizes eliminating directional deviation while also taking into account lateral offset correction.

[0156] Step 62. Fine Alignment Control When the following conditions are met:

[0157] in, The lateral deviation threshold for switching from the coarse alignment control stage to the fine alignment control stage. The angle deviation threshold for switching from the coarse alignment control stage to the fine alignment control stage; Based on the target track width, wheel assembly width, allowable wheel-rail clearance, and visual measurement error preset, the following... The target track entrance direction, allowable angle error of the upper track, and visual measurement error are preset.

[0158] Then switch to the fine alignment stage. Fine alignment control can employ higher-precision, small-range linear feedback control:

[0159]

[0160] in, This refers to the linear velocity control quantity output during the fine alignment control stage. The angular velocity control quantity output during the fine alignment control stage. For low-speed fine-tuning, For fine-tuning the gain, and typically satisfying:

[0161] To improve the correction sensitivity under small deviations.

[0162] If the upper rail judgment threshold is reached:

[0163] Then it is determined that the agricultural robot has met the conditions for going onto the track.

[0164] Step 7: Once the upper rail conditions are met, execute the upper rail action and check the upper rail status. When the angular deviation of the agricultural robot relative to the target track is less than the upper rail threshold and the lateral deviation is less than the upper rail threshold, the agricultural robot is determined to meet the preset upper rail conditions, and the agricultural robot is controlled to perform the upper rail action.

[0165] Specifically, the agricultural robot is controlled to move along the entrance direction of the target track at a preset speed and preset posture; during the track mounting process, at least one of the following is detected in real time: the posture state of the agricultural robot, the relative position of the wheels and rails, the relative positional relationship between the vehicle body and the target track, and the characteristic state of the target track; when it is detected that the agricultural robot has entered the target track, and the relative positional error between the wheel assembly, the guiding mechanism, or the track mounting mechanism and the target track is less than a preset positional error threshold, and this is maintained for a preset time, it is determined that the agricultural robot has completed the track mounting.

[0166] This step avoids problems such as wheel-rail misalignment, attitude deviation, or failure to mount the rail, which can be caused by simply aligning the rail once and then mounting it directly.

[0167] In step 7, the agricultural robot moves at a low speed along the track entrance direction to perform the upper track action.

[0168] Set the upper rail stage control input for:

[0169] in, This indicates the linear velocity control value output during the upper rail phase. This represents the angular velocity control value output during the upper orbit phase; typically, let:

[0170] A preset low-speed constant value is used. During the rail mounting process, the wheel-rail relative position error is continuously monitored. If the following conditions are met:

[0171] And continuous If the agricultural robot maintains a stable posture within a given timeframe, it is considered to have successfully completed the orbital ascent. The criterion for successful orbital ascent can be expressed as:

[0172] in, The threshold for the relative position error between the wheel and the rail is preferably 5mm to 20mm. The heading angle error threshold for determining the completion of the upper orbit determination is preferably 1° to 3°. This indicates the robot's current heading angle, preferably between 1 and 3 seconds; Indicates the target orbital orientation angle; This indicates that the upper track is complete.

[0173] Step 8: Perform zero-return locking, job mode switching, and anomaly rollback correction. When the agricultural robot is detected to have entered the target track and met the conditions for stable driving, a zero-lock or operation mode switch is executed, which switches the agricultural robot from ground navigation mode to track operation mode.

[0174] Stable driving conditions include at least one of the following: the relative position error between the agricultural robot's wheel assembly, guiding mechanism, or upper rail mechanism and the target track is less than a preset position error threshold; the angle between the agricultural robot's heading angle and the target track direction is less than a preset stable angle threshold; the agricultural robot's driving state on the target track is continuous and stable and remains so for a preset time; the target track features remain identifiable in multiple consecutive frames of images; and the agricultural robot does not detect wheel-rail misalignment, abnormal vibration, or track deviation.

[0175] When path blockage, trajectory tracking mismatch, obstacle change exceeding limits, track feature loss, deviation exceeding limits, track alignment failure, wheel-rail misalignment, abnormal attitude, track deviation, or track mounting failure is detected, a backoff correction mechanism is triggered.

[0176] Specifically, when the agricultural robot encounters path blockage, excessive obstacle changes, or trajectory tracking mismatch during autonomous navigation, the robot is controlled to stop its current navigation action and re-execute local planning, global path planning, or pre-docking area selection. When the agricultural robot experiences track feature loss, recognition confidence below the threshold, excessive deviation, or control mismatch during visual track alignment, it is controlled to return to the previous correction state or pre-docking area and re-execute target track image acquisition, track feature extraction, and visual track alignment control. When the agricultural robot experiences wheel-track misalignment, abnormal posture, track deviation, or track alignment failure during track mounting, it is controlled to exit the track mounting action and re-execute fine alignment control, coarse alignment control, or autonomous navigation control. When the number of backtracking corrections exceeds the preset number, the agricultural robot is controlled to enter a safe stop state and output a fault prompt message.

[0177] To improve system robustness, a closed-loop detection mechanism is set up in step 8. System status flags are defined. for:

[0178] in, Indicates normal navigation status; Indicates the track adjustment status; Indicates the execution status of the upper rail; This indicates a failover status. Failover is triggered when any of the following conditions are met:

[0179] or

[0180] or

[0181] or

[0182] or

[0183] in, Indicates the robot's current position With local obstacle set The distance between the nearest obstacles; Indicates the first The trajectory tracking error norm between the actual state of the agricultural robot and the reference trajectory state at each sampling time; For minimum safe distance, For the error threshold, This represents the maximum allowed number of retries. If a failure is triggered, a fallback strategy will be implemented:

[0184] in, express, express; express; This is to roll back the distance. After rolling back, re-enter the local planning or re-execute the visual alignment process.

[0185] Through the detailed description of the technical solution above, this invention integrates autonomous navigation, visual track recognition, pose deviation calculation, coarse and fine alignment control, track execution, and abnormal backtracking correction in complex orchard environments. This forms a complete closed-loop control process from target point navigation, track recognition, precise alignment to stable track execution, thereby improving the autonomous operation capability, track alignment accuracy, track execution success rate, and operational reliability of agricultural robots in orchard environments.

[0186] In summary, the autonomous operation control method and system for agricultural robots in orchard environments provided by the above embodiments of the present invention first utilizes lidar or depth sensors to collect point cloud data of the orchard environment and establish a local map. By fitting the ground plane, accessibility indicators such as flatness, slope, sparsity, and obstacle density are calculated to form a cost map. Then, global path planning is performed based on the cost map, and B-spline continuous trajectory optimization is performed on discrete paths under safe corridor constraints. Next, model predictive control enables the agricultural robot to track and avoid obstacles locally in complex orchard environments. When the agricultural robot reaches the vicinity of the target track, a track image is acquired using an onboard camera. After distortion correction and inverse perspective transformation, the track line is extracted, and the lateral and angular deviations of the agricultural robot relative to the track are calculated. Finally, coarse alignment, fine alignment, and track alignment control are sequentially performed based on the deviations, and backtracking correction is performed in abnormal situations, thereby completing the autonomous navigation, precise track alignment, and stable track alignment of the agricultural robot in the orchard environment.

[0187] Any matters not covered in the above embodiments of the present invention are well-known in the art.

[0188] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various modifications or variations within the scope of the claims, which do not affect the essence of the present invention.

Claims

1. A method for autonomous operation control of agricultural robots in an orchard environment, characterized in that, include: Acquire orchard environmental input information, current status information of agricultural robots, and target trajectory information, and determine the pre-docking area near the target trajectory; Based on the orchard environment input information, terrain surface fitting and local terrain attribute analysis are performed to obtain the accessibility evaluation results of local areas of the orchard. Based on the accessibility evaluation results, the current status information of the agricultural robot, and the target trajectory information, path planning, trajectory optimization, and local trajectory tracking control are performed, and the agricultural robot is controlled to autonomously move to the pre-docking area, realizing the autonomous navigation process of the agricultural robot from its current position to the pre-docking area; After the agricultural robot reaches the pre-docking area, it acquires a target trajectory image and performs image preprocessing and trajectory feature extraction on the target trajectory image to obtain the target trajectory features. Based on the characteristics of the target track, the pose deviation parameters of the agricultural robot relative to the target track are calculated, including angular deviation and lateral deviation. Visual orbit alignment control is performed based on the angular and lateral deviations. When the angular deviation and lateral deviation meet the preset upper rail conditions, the agricultural robot is controlled to perform the upper rail action, and the posture state of the agricultural robot, the wheel-rail relative position and the target track characteristics are detected during the upper rail process. Based on the agricultural robot's posture state, wheel-rail relative position, and target track characteristics, when it is determined that the agricultural robot has entered the target track and meets the stable driving conditions, zero-locking or operation mode switching is executed. When an abnormal state is detected, a rollback correction mechanism is triggered, and path planning, visual track alignment control, and / or top track control are re-executed.

2. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The pre-docking area refers to a candidate area located near the target track entrance, used for the agricultural robot to switch from the autonomous navigation stage to the visual track alignment stage and the upper track control stage; wherein, the pre-docking area is determined based on the visual track detection field of view requirements, upper track attitude adjustment requirements, and passability evaluation requirements; wherein: The visual track detection field of view requirements include: the target track entrance position must be within the effective field of view of the agricultural robot's onboard visual sensor; The requirements for upper rail attitude adjustment include: the agricultural robot has a motion space for angle correction and lateral correction within the pre-docking area; and / or, the angle between the initial orientation of the agricultural robot within the pre-docking area and the direction of the target track entrance is less than a preset navigation positioning threshold. The accessibility evaluation requirements include: the pre-docking area is located in an accessible position in front of or to the side of the target track entrance.

3. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The terrain surface fitting and local terrain attribute analysis include: The effective shape point cloud data in the orchard environment input information is filtered and ground points are extracted to obtain ground point cloud data; Surface projection, local plane fitting, and surface fitting are performed on ground point cloud data within a local area to obtain the surface plane fitting results; Based on the surface plane fitting results, the terrain attributes of the local area are calculated. The terrain attributes include any one or more of the following: flatness, slope, roughness, elevation change, point cloud sparseness, and obstacle distribution. Based on the terrain attribute results, the corresponding terrain attribute parameters are normalized and then weighted and fused according to preset weights to obtain the accessibility cost of a local area; based on the accessibility cost, the accessibility evaluation result and terrain risk cost of the local area are determined.

4. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The path planning, trajectory optimization, and local trajectory tracking control include: Based on the drivability evaluation results and target trajectory information, a pre-docking target point is determined within the pre-docking area, and a global discrete path from the current position of the agricultural robot to the pre-docking target point is generated for path planning; wherein, the pre-docking target point is a target location within the pre-docking area that meets the requirements of visual trajectory detection field of view, upper rail attitude adjustment, navigation positioning angle, and drivability evaluation. Based on the global discrete path, local obstacle distribution, and terrain risk cost, a safe corridor is constructed around the path; wherein, the safe corridor consists of multiple locally safe and feasible regions generated sequentially along the global discrete path, and the locally safe and feasible regions are locally feasible spaces that satisfy obstacle safety distance constraints and terrain risk cost constraints; Continuous trajectory construction and smoothing optimization are performed on the global discrete path to obtain a continuous reference trajectory; Based on the current state of the agricultural robot, continuous reference trajectory, local obstacle changes, and terrain risk costs, local planning and trajectory tracking control are performed in the rolling time domain. The local planning is used to generate a local predicted trajectory or predicted state sequence that avoids local obstacles and high-risk terrain areas within the prediction time domain, based on the continuous reference trajectory, local obstacle changes, and terrain risk costs. The trajectory tracking control is used to generate a control input sequence for the agricultural robot based on the local predicted trajectory or predicted state sequence. Based on the control input sequence obtained from the local planning and trajectory tracking control, the first control quantity in the optimal control input sequence within the rolling time domain is selected. Combined with the chassis kinematic model of the agricultural robot, the first control quantity is converted into a speed control quantity, steering control quantity, angular velocity control quantity, or motion control command to drive the agricultural robot to autonomously move to the pre-dock target point.

5. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The image preprocessing and orbit feature extraction include: The target orbit image is processed by camera distortion correction, filtering, enhancement, grayscale conversion and / or edge enhancement to obtain a preprocessed target orbit image; Region of interest extraction is performed on the preprocessed target orbit image; The extracted region of interest is subjected to inverse perspective transformation to obtain the orbital feature distribution from a top-down view. Based on the distribution of track features from the top-down view, target track features are extracted; wherein, the target track features include: track lines, track edges, track centerlines, track regions, track entry directions, track endpoints, and / or track feature points.

6. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The calculation of the pose deviation parameters of the agricultural robot relative to the target trajectory includes: Determine the centerline and entry direction of the target orbit based on its characteristics; Based on the mapping relationship between the image coordinate system and the vehicle coordinate system, the target track centerline, target track entrance direction, track endpoints and / or track feature points in the target track features are transformed to the vehicle coordinate system. Calculate the angular deviation between the direction of the agricultural robot body and the target track based on the angle between the direction of the target track centerline or the direction of the target track entrance and the direction of the agricultural robot body centerline, which is transformed into the vehicle body coordinate system. The lateral deviation of the agricultural robot relative to the target track is calculated based on the distances of the centerline of the agricultural robot body, the centerline of the wheel assembly, and the upper rail reference point relative to the centerline of the target track. The upper rail reference point is a preset benchmark point used to characterize the lateral relative position of the agricultural robot body and the target track when the agricultural robot performs the upper rail action. The upper rail reference point includes the center point of the front wheel assembly, the center point of the guide mechanism, the center point of the upper rail mechanism, the geometric center point of the chassis, and / or the calibrated body mounting benchmark point. Based on the pre-calibrated camera mounting offset, vehicle structure offset, image geometric transformation error parameters, and / or vehicle attitude change, the angular deviation and lateral deviation are compensated and corrected to obtain the corrected angular deviation and lateral deviation.

7. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The visual track alignment control includes: coarse alignment control and / or fine alignment control; wherein: The coarse alignment control includes: triggering coarse alignment control when the corrected angle deviation is greater than an angle threshold, and / or the corrected lateral deviation is greater than a lateral threshold. At this time, chassis wheel speed, steering angle, linear velocity, angular velocity and / or lateral movement control commands are generated based on the corrected angle deviation and / or the corrected lateral deviation to perform attitude correction and position correction on the agricultural robot. During the coarse alignment process, target orbit images are continuously acquired, target orbit features are re-extracted, and updated angular and lateral deviations are calculated in real time. When the updated angle deviation is less than or equal to the angle threshold, and the updated lateral deviation is less than or equal to the lateral threshold, switch to fine alignment control; If the corrected angle deviation is less than or equal to the angle threshold, and the corrected lateral deviation is less than or equal to the lateral threshold, then the coarse alignment control is skipped and the fine alignment control is executed directly. The fine alignment control includes: After entering the fine alignment control, the target track image is re-acquired and the target track features are re-extracted. Based on the updated angle deviation and updated lateral deviation after the coarse alignment stage, combined with camera installation error, vehicle structure error, image geometric transformation error and / or vehicle attitude error, a second fine compensation is performed to obtain the fine-tuned angle deviation and fine-tuned lateral deviation after the fine alignment stage. Based on the refined angular deviation and refined lateral deviation, corresponding control commands are generated to perform fine angle correction, fine lateral correction and / or reverse fine adjustment on the agricultural robot until the angular deviation and lateral deviation between the agricultural robot and the target track meet the preset upper track conditions.

8. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The upper rail action includes: When the angular deviation of the agricultural robot relative to the target track is less than the upper angular rail threshold and the lateral deviation is less than the upper lateral rail threshold, the upper rail control command is triggered. According to the upper rail control command, the agricultural robot is controlled to move along the target track entrance direction at a preset speed and preset posture; During the track mounting process, the relative positional relationship between the agricultural robot and the target track, the vehicle's attitude relationship, and the wheel-rail relative positional relationship are monitored in real time. When it is detected that the agricultural robot has entered the target track, and the relative position error between the wheel assembly, guiding mechanism and / or upper track mechanism and the target track is less than the preset position error and remains so for a preset time, it is determined that the upper track is completed.

9. The method for autonomous operation control of agricultural robots in an orchard environment according to claim 1, characterized in that, The rollback correction mechanism includes: When an agricultural robot encounters path blockage, obstacle change exceeding limits, and / or trajectory tracking mismatch during autonomous navigation, the robot is controlled to stop its current navigation action and re-execute local planning, global path planning, and / or pre-docking area selection. When an agricultural robot experiences loss of track features, excessive pose deviation, recognition confidence below the threshold, and / or trajectory tracking control mismatch during visual track alignment control, the robot is controlled to return to the previous correction state and / or pre-docking area, and the target track image acquisition, track feature extraction, and visual track alignment control are re-executed. When the agricultural robot experiences wheel-rail misalignment, abnormal posture, track deviation, and / or failure to mount the rail during the mounting process, the robot is controlled to stop the mounting action and re-execute visual track alignment control and / or autonomous navigation control.

10. An autonomous operation control system for agricultural robots in an orchard environment, characterized in that, include: The environmental perception module is used to acquire orchard environmental input information and target track information; The status acquisition module is used to acquire the current status information of the agricultural robot; The terrain analysis module is used to perform terrain surface fitting and local terrain attribute analysis on the input information of the orchard environment, and output the accessibility evaluation results of local areas of the orchard. The path planning and trajectory tracking module is used to perform path planning, trajectory optimization and local trajectory tracking control based on the drivability evaluation results, the current state information of the agricultural robot and the target trajectory information, and to control the agricultural robot to autonomously move to the pre-docking area near the target trajectory, so as to realize the autonomous navigation process of the agricultural robot from the current position to the pre-docking area; A visual track recognition module is used to acquire target track images and perform image preprocessing and track feature extraction on the target track images; The pose deviation calculation module is used to calculate the angular and lateral deviations of the agricultural robot relative to the target track based on the extracted target track features. A visual track alignment module is used to perform visual track alignment control based on the angular deviation and lateral deviation. The upper rail control module is used to control the agricultural robot to perform the upper rail action when the angular deviation and lateral deviation of the agricultural robot meet the preset upper rail conditions, and to detect the posture state, wheel-rail relative position and target track characteristics of the agricultural robot during the upper rail process. When it is determined that the agricultural robot has entered the target track and meets the stable driving conditions, it is used to perform zero-lock or operation mode switching, and triggers the back-off correction mechanism when an abnormal state is detected.

Images