An Autonomous Navigation Method for Highly Adaptive Fruit and Tea Garden Equipment Based on Low-Precision Maps

Abstract

This invention discloses a highly adaptable autonomous navigation method for fruit and tea garden equipment based on low-precision maps, belonging to the field of intelligent navigation technology for agricultural machinery. It deploys an autonomous navigation system, acquiring global images of the fruit and tea garden through drone aerial photography, and manually setting key navigation points. Semantic segmentation algorithms are used to detect roads, fitting straight lines to obtain corner points, and automatically generating secondary navigation points connecting the key points. Latitude and longitude coordinates are obtained through automatic conversion between pixel coordinates, projected coordinates, and global geographic coordinates. This invention acquires global low-precision images through drone aerial photography and requires only a few manually set key navigation points. By combining semantic segmentation, automatically generated secondary navigation points, and global geographic coordinates, a usable navigation network can be constructed. This allows for rapid application to different fruit and tea garden environments, avoiding the need for extensive repetitive mapping work for a single garden, and significantly improving the versatility and deployment efficiency of the navigation system across different fruit and tea gardens.

Description

Technical Field

[0001] This invention relates to the field of intelligent navigation technology for agricultural machinery, specifically to an autonomous navigation method for highly adaptable fruit and tea garden equipment based on low-precision maps. Background Technology

[0002] Since fruit and tea gardens are mainly planted in hilly and mountainous areas, the production and management environment is complex and the degree of mechanization is poor. Therefore, it is necessary to solve the problem of walking obstacles in the autonomous operation of fruit and tea gardens.

[0003] Currently, real-time sensing and autonomous navigation technologies have yielded relevant and referable results in industrial and field production environments. These technologies primarily utilize combined navigation with BeiDou RTK precise pose determination technology for regular route planning and autonomous navigation in fields. Their advantages include: strong satellite navigation signals in open fields; relatively flat terrain; and the ability to create templates for route planning, resulting in high versatility. However, the production environment of orchards and tea gardens in hilly and mountainous areas is unstructured. This presents challenges: firstly, compared to open field environments, more precise guidance is required, leading to a heavy workload in preliminary mapping; secondly, there are issues such as poor versatility and difficulty in migrating operational routes or patterns.

[0004] To address the aforementioned issues, this invention aims to solve the following problems: using the unstructured production management environment of fruit and tea gardens as the application scenario, and a cross-track multi-functional management machine as the main application object, this invention analyzes the dependence of agricultural robot navigation on pre-set high-precision maps through the fusion of multi-sensor information. Based on the complexity and dynamic requirements of agricultural scenarios, a closed-loop "perception-decision-execution" system is constructed, deeply integrating agricultural machinery with artificial intelligence technology to build a highly versatile autonomous navigation system that does not rely on high-precision prior maps. This provides a general solution for complex agricultural environments such as hilly areas. Therefore, this invention proposes a highly adaptable autonomous navigation method for fruit and tea garden equipment based on low-precision maps. Summary of the Invention

[0005] To solve the above-mentioned technical problems, the present invention is implemented through the following technical solution: a highly adaptable autonomous navigation method for fruit and tea garden equipment based on low-precision maps, comprising the following steps:

[0006] S1. Deploy an autonomous navigation system and acquire global images of the fruit and tea garden through drone aerial photography. Manually set key navigation points, including hangars, starting / ending points of the fruit and tea garden work area, and transfer hubs.

[0007] S2. Detect roads using semantic segmentation algorithms, fit straight lines to obtain corner points, and automatically generate secondary navigation points that connect key points;

[0008] S3. Use the autonomous navigation system to obtain the latitude and longitude and heading angle of the agricultural machinery, construct a scale suitable for the current aerial image, automatically obtain the mapping of pixel coordinates-projected coordinates-global geographic coordinates, obtain a multi-machine collaborative guidance map, realize the mapping of pixel coordinates to actual latitude and longitude coordinates, and obtain the latitude and longitude information of navigation points.

[0009] S4. Construct a heuristic function based on the distance between key navigation points and the orientation angle to dynamically guide agricultural machinery to perform global autonomous navigation from the starting point to the target point;

[0010] S5. Extract road edges through instance segmentation, fit the centerline and calculate the image drift, and adjust the steering in real time to keep the agricultural machinery driving within the lane;

[0011] S6: Combining obstacle size and road space, it adaptively selects detour or parking strategies to complete local obstacle avoidance and continue navigation to the destination.

[0012] Preferably, S1 specifically includes:

[0013] An autonomous navigation system is deployed, comprising an airborne multi-source sensor module, a UAV aerial photography and image transmission module, and an airborne decision-making module. The UAV aerial photography and image transmission module covers two parts: aerial image acquisition and ground data reception. The airborne multi-source sensor module consists of a Beidou RTK module, a depth camera module, and a visible light camera module. The UAV is operated to conduct aerial photography of the target fruit and tea garden area, acquiring aerial information of the entire area, including global overhead image data of agricultural machinery, hangars, and fruit and tea garden work areas, ensuring coverage of the work area and key facilities.

[0014] On the acquired aerial global image, the operator manually identifies and sets key navigation points according to the actual operation requirements, including hangar entrances and exits, the start and end points of the operation area, and transfer and concentration points, while extracting the pixel coordinates of each key navigation point.

[0015] The pixel coordinates of manually calibrated key navigation points are bound and stored with the image data collected by the UAV. At the same time, the BeiDou RTK module is initialized to complete the startup and data preparation process of the autonomous navigation system.

[0016] Preferably, S2 specifically includes:

[0017] By processing drone aerial images using semantic segmentation algorithms, the distribution of roads, work areas, and obstacles in the fruit and tea gardens is identified, road masks are generated, and the range of paths that agricultural machinery can travel is selected.

[0018] The road mask is fitted with straight lines and sloped. The corner points and intersections of multiple lines in the path are automatically detected. Based on the road connectivity characteristics, secondary navigation points are generated at structural changes so that the two points are approximately straight lines (including key navigation points and secondary navigation points).

[0019] The generated secondary navigation points are topologically connected with the manually marked key navigation points to construct a complete navigation path network, and the pixel distance between connected adjacent points (including key navigation points and secondary navigation points) is obtained through image processing methods.

[0020] By using a rotating frame small target detection algorithm, the agricultural machinery and equipment in operation are identified, and their pixel location, orientation angle, and top-down pixel size information on the aerial image are obtained.

[0021] Preferably, S3 specifically includes:

[0022] The precise latitude and longitude coordinates and heading angle based on true north of the agricultural machinery are obtained in real time by the Beidou RTK module of the autonomous navigation system. The size benchmark for constructing the scale is determined by combining the pixel size of the agricultural machinery obtained by the small target detection of the rotating frame and its known actual size.

[0023] Using the real-time pose of agricultural machinery as a spatial reference point, the Gauss-Kruger projection algorithm is used to convert geographic coordinates into planar projected coordinates. Based on the correspondence between the actual size of the agricultural machinery and the pixel size, an adaptive scale between image pixels and actual distance is constructed, which is suitable for current aerial images.

[0024] By applying the established scale and projection relationship, the pixel coordinates of all key and secondary navigation points in the path network are uniformly converted into actual projected coordinates to form a multi-machine collaborative guidance map. Then, through projection back-calculation, the precise latitude and longitude geographic coordinates corresponding to all navigation points are output.

[0025] Preferably, the process of outputting the precise latitude and longitude geographic coordinates corresponding to all navigation points is as follows:

[0026] Based on the established scale and projection relationship parameters, the pixel coordinates of key navigation points and secondary navigation points in the path network are batch-converted and unified. The positions in the image pixel space are mapped to a unified planar projection coordinate system, thus obtaining the actual projected coordinates corresponding to each navigation point, providing a coordinate basis for the formation of multi-machine collaborative guidance maps.

[0027] Based on the transformed projected coordinates, spatial layout planning is carried out for key navigation points and secondary navigation points. In combination with the needs of multi-machine collaborative operation, including avoiding path conflicts and optimizing operation flow, the relative positional relationship of each navigation point in the map is determined. Then, the discrete navigation points and their connection relationships are integrated and drawn into a concrete multi-machine collaborative guidance map, which presents the network structure of all feasible paths in the operation area, the distribution of navigation points, and the connectivity between navigation points.

[0028] By using the inverse calculation formula of Gauss-Kruger projection and combining it with known projection relationship parameters, the projected coordinates of navigation points in the multi-machine collaborative guidance map are calculated in reverse, and the projected coordinates of each navigation point are restored to accurate latitude and longitude geographic coordinates, thus completing the full mapping from image pixel positioning to the global geographic coordinate system.

[0029] Preferably, S4 specifically includes:

[0030] A heuristic function is constructed to comprehensively evaluate the Euclidean distance from the current navigation point to the target navigation point and the angle between the direction of travel. The priority of each navigation point is evaluated through the heuristic function to provide agricultural machinery with the optimal direction of travel toward the target navigation point.

[0031] Based on existing key and secondary navigation points, a path tree network is constructed according to spatial connectivity. The actual cost is initialized for each path segment. Based on the Euclidean distance between adjacent nodes, the discrete navigation points are organized into a topology that can be searched by heuristic functions.

[0032] Based on the evaluation results of the heuristic function, starting from the starting point, the subsequent navigation point with the best evaluation value is iteratively selected in the path tree network, thereby generating an ordered sequence of navigation points from the starting point to the target navigation point, forming the final global macro navigation route and completing the global path planning.

[0033] Preferably, the process of evaluating the priority of each navigation point using a heuristic function is as follows:

[0034] Based on the coordinates of the current navigation point and the target navigation point, the Euclidean distance is calculated as a spatial proximity index to reflect the straight-line distance cost from the agricultural machinery to the target. At the same time, the cosine value of the angle between the current heading angle and the target heading angle is used to determine the directional angle cost to quantify the turning cost and characterize the energy consumption required for the agricultural machinery to adjust its direction. The two indicators of straight-line distance cost and directional angle cost are normalized respectively.

[0035] After obtaining the straight-line distance cost and the directional angle cost, weight coefficients are assigned to the two costs according to the needs of the actual operation scenario of agricultural machinery, and then candidate navigation points are determined to form a set of candidate navigation points.

[0036] The candidate navigation point set is sorted in ascending order according to the comprehensive heuristic value to generate a priority queue. The navigation point at the head of the queue is the current optimal choice. The agricultural machinery selects navigation points as the next forward target in sequence according to the order of the priority queue. At the same time, the current position and heading data are continuously updated, and the heuristic function parameters are dynamically corrected to ensure that environmental changes are reflected in real time during the movement, forming the optimal path sequence from the starting point to the target navigation point.

[0037] Preferably, the computational expression of the heuristic function is as follows:

[0038] ;

[0039] ;

[0040] ;

[0041] In the formula: For comprehensive heuristic values; This is the current location; for Candidate navigation points between the target point; These are weight values ​​used to balance the importance of different cost components in the cost function; The actual cost already paid is fixed at 0; This is a heuristic cost used to estimate the cost from candidate next navigation points to the final target navigation point; The Euclidean distance represents the distance from the current point ( ) to candidate next navigation point ( The straight-line distance is used to encourage agricultural machinery to choose the nearest next stop; This refers to the directional deviation angle; As a consequence of deviation from the intended direction, Convert it to a coefficient between 0 and 1, when hour, , indicating candidate points , Ideally, the target points should be on the same straight line.

[0042] Preferably, S5 specifically includes:

[0043] The road images are collected in real time by a camera mounted on the agricultural machinery. The road images are processed by a pre-trained instance segmentation model to identify and extract the pixel-level edge contours of the road area. Based on the extracted pixel-level edge contours, the road boundary lines on both sides are reconstructed. Then, based on the reconstructed road boundary lines on both sides, a reference navigation centerline located in the center of the road is generated by geometric calculation and fitting.

[0044] In each frame processing, the fitted reference navigation centerline is projected onto the image coordinate system. The lateral pixel distance between the reference navigation centerline and the preset reference line (vertical center line of the image) is calculated to obtain the lateral drift of the agricultural machinery. At the same time, the angle between the centerline and the reference line is calculated to obtain the heading angle, which together constitute the pose deviation.

[0045] The calculated lateral drift and heading angle are used as inputs to the control system. Based on the lateral drift and heading angle, a predetermined control algorithm (PID controller) is used to generate corresponding steering angle adjustment commands in real time. Through continuous closed-loop feedback and adjustment, the steering actuator is driven to move, so that the agricultural machinery can dynamically maintain its movement on a trajectory aligned with the center line of the road.

[0046] Preferably, S6 specifically includes:

[0047] The system can perceive the physical size of obstacles ahead and the space they occupy on the road in real time. It can calculate the passable width of the current road by combining the road boundary line and calculate the degree of obstruction to the path by analyzing the degree of obstruction. If the degree of obstruction is lower than the preset obstruction threshold, it is determined to be a detour scenario; otherwise, it is determined to be a parking scenario.

[0048] Based on the obstruction assessment results, the obstacle avoidance strategy is dynamically selected: for detour scenarios, the detour trajectory is automatically generated according to the left or right position of the obstacle to ensure a safe distance; for scenarios that require stopping, the braking command is immediately triggered and the system enters a waiting state until the obstacle is cleared.

[0049] After performing a detour or stopping, based on the updated environmental information, the path is locally replanned to the original secondary navigation point, the heuristic navigation process is restored, and the vehicle continues to travel along the global path towards the target point, ensuring that obstacle avoidance actions are seamlessly connected with the global navigation target.

[0050] This invention provides a highly adaptable autonomous navigation method for equipment in fruit and tea orchards based on low-precision maps. It has the following beneficial effects:

[0051] (I) This highly adaptable autonomous navigation method for fruit and tea gardens based on low-precision maps acquires global low-precision images through drone aerial photography. Only a few key navigation points need to be manually set. By combining semantic segmentation, automatically generated secondary navigation points and corresponding latitude and longitude coordinates, a usable navigation network can be constructed. This greatly reduces the heavy preliminary surveying and detailed map production work in traditional methods. It can be quickly applied to different fruit and tea garden environments, avoiding the need for a large amount of repetitive mapping work for a single garden. This significantly improves the universality and deployment efficiency of the navigation system in different fruit and tea gardens.

[0052] (II) This highly adaptable autonomous navigation method for fruit and tea garden equipment based on low-precision maps performs global macro-route planning in a path network composed of key points and secondary points through heuristic functions. During the movement between points, local fine-tuning is performed through visual lane keeping technology. When encountering obstacles, dynamic obstacle avoidance is triggered. By using the progressive structure of global heuristic navigation - local lane keeping - real-time dynamic obstacle avoidance, the agricultural machinery can have both clear macro-goal guidance and flexible response to local unstructured environmental changes and sudden obstacles, thereby achieving stable and highly adaptable autonomous operation in complex hilly and mountainous fruit and tea gardens. Attached Figure Description

[0053] Figure 1 This is a schematic diagram illustrating the workflow of an autonomous navigation method for highly adaptable fruit and tea garden equipment based on low-precision maps according to the present invention.

[0054] Figure 2 This is a schematic diagram showing the deployment location of the airborne multi-source sensing module of the present invention;

[0055] Figure 3 This is a schematic diagram of the method flow for an autonomous navigation method for highly adaptable fruit and tea garden equipment based on a low-precision map according to the present invention.

[0056] Figure 4 This is a schematic diagram of the structure of the UAV aerial photography and image transmission module of the present invention;

[0057] Figure 5 This is a schematic diagram illustrating the structure of the airborne decision-making module of the present invention;

[0058] Figure 6 This is a schematic diagram illustrating the recognition effect of the semantic segmentation algorithm of the present invention in detecting roads;

[0059] Figure 7 This is the overall flowchart of the present invention. Detailed Implementation

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

[0061] Example 1, please refer to Figures 1 to 7 This invention provides a technical solution: an autonomous navigation method for highly adaptable fruit and tea garden equipment based on low-precision maps, comprising the following steps:

[0062] S1. Deploy an autonomous navigation system and acquire global images of the fruit and tea orchard through drone aerial photography. Manually set key navigation points, including hangars, start / end points of the fruit and tea orchard work area, and transfer hubs. Deploy an autonomous navigation system comprising an airborne multi-source sensor module, a drone aerial photography and image transmission module, and an airborne decision-making module. The drone aerial photography and image transmission module covers both aerial image acquisition and ground data reception. The airborne multi-source sensor module consists of a BeiDou RTK module, a depth camera module, and a visible light camera module. Operate the drone to conduct aerial photography of the target fruit and tea orchard area to acquire images of the entire orchard area. The aerial photography information includes global overhead image data of agricultural machinery, hangars, orchards and tea gardens, ensuring coverage of the work area and key facilities. On the acquired global aerial images, operators manually identify and set key navigation points according to actual work needs, including hangar entrances and exits, the start and end points of the work area, and transfer and relocation points. At the same time, the pixel coordinates of each key navigation point are extracted, and the pixel coordinates of the manually marked key navigation points are bound and stored with the image data collected by the UAV. Meanwhile, the Beidou RTK module is initialized to complete the startup and data preparation process of the autonomous navigation system.

[0063] The specific work involves: deploying a complete autonomous navigation system, integrating an airborne multi-source sensor module, a UAV aerial photography and image transmission module, and an airborne decision-making module. The airborne multi-source sensor module specifically comprises a BeiDou RTK module, a depth camera module for acquiring depth information, and visible light camera modules deployed on both sides of the agricultural machinery. After preparation, the UAV is operated to conduct aerial photography of the target orchard area to obtain global overhead image data covering the entire area. The aerial photography must ensure complete capture of key elements within the work area, including the location of the agricultural machinery, hangar, and the work area of ​​the orchard to be operated. After successfully acquiring the global aerial image of the orchard, operators manually identify and calibrate key navigation points on the digital image according to the actual work process and management needs. Key navigation points include the hangar entrances and exits for agricultural machinery, the starting and ending points of specific operations, and the central points for material or harvest transfer. During the calibration process, operators precisely specify the location of key navigation points on the image and simultaneously record the pixel coordinates of the key navigation points in the image coordinate system, transforming human operational experience and spatial judgment into machine-readable position data. After manual calibration, the generated pixel coordinates of the key navigation points are bound to the raw global image data collected by the UAV and stored in the system's storage unit to form an initial navigation reference frame. At the same time, the onboard Beidou RTK module is initialized to put it into working state, ready to provide the agricultural machinery with real-time high-precision pose information of latitude, longitude, and heading angle, thus completing all data preparation processes before startup.

[0064] S2. Detect roads using semantic segmentation algorithms, fit straight lines to obtain corner points, and automatically generate secondary navigation points connecting key points. Process drone aerial images using semantic segmentation algorithms to identify the distribution of roads, work areas, and obstacles within the orchard, generate road masks, filter out the walkable pathways for agricultural machinery, perform straight line fitting and slope analysis on the road masks, automatically detect corner points and multi-line intersections in the path, and generate secondary navigation points at structural changes based on road connectivity features, ensuring that the distance between two points is approximately a straight line (including key navigation points and secondary navigation points). Connect the generated secondary navigation points with manually calibrated key navigation points topologically to construct a complete navigation path network. Obtain the pixel distance between connected adjacent points (including key navigation points and secondary navigation points) using image processing methods. Identify agricultural machinery using a rotating bounding box small target detection algorithm, obtaining its pixel location, orientation angle, and overhead pixel size information on the aerial image.

[0065] The specific work involves: performing pixel-level analysis on acquired UAV aerial images using semantic segmentation algorithms to identify road areas, work areas, and obstacle distribution within the orchard environment; generating precise road mask images to define the potential passable range of agricultural machinery within the orchard; and then performing image analysis on the extracted road masks. Through line fitting and slope calculation, structural feature points in the path are adaptively detected, including corners and multi-line intersections, transforming continuous unstructured roads into approximately straight line segments connected by feature points, thus achieving preliminary structured modeling of the complex orchard road network. Based on the detected structural feature points, secondary navigation points are automatically generated at identified corners and intersections. Secondary navigation points and manually calibrated key navigation points together constitute a complete set of navigation nodes. A topology connection algorithm links all key navigation points and secondary navigation points according to their spatial connectivity, constructing a path network covering the entire walkable area. Simultaneously, image processing methods are used to calculate the pixel distance between every two adjacent connected nodes in the path network. A rotating bounding box small target detection algorithm is employed to identify targets in aerial images, accurately locating agricultural machinery in the images and outputting the pixel coordinates of its detection box. Furthermore, the orientation angle of the agricultural machinery and its pixel size from the overhead view are analyzed to obtain the visual pose and size information of the agricultural machinery, establishing a correlation between the global aerial view and the specific agricultural machinery in operation. Additionally, such as... Figure 6 The diagram shows the recognition effect of the semantic segmentation algorithm in detecting roads. In the diagram, the orange dots are manually set key navigation points, the black dots are automatically generated secondary navigation points, the red part is the semantic segmentation effect of the road, and the red box is the small target detection result.

[0066] S3. Utilize the autonomous navigation system to acquire the latitude, longitude, and heading angle of the agricultural machinery, construct a scale suitable for the current aerial image, automatically obtain the mapping between pixel coordinates, projected coordinates, and global geographic coordinates, and obtain a multi-machine collaborative guidance map. This achieves the mapping from pixel coordinates to actual latitude and longitude coordinates to obtain the latitude and longitude information of navigation points. The BeiDou RTK module of the autonomous navigation system acquires the precise latitude and longitude coordinates of the agricultural machinery and its heading angle based on true north in real time. Simultaneously, it combines the pixel size of the agricultural machinery obtained from the small target detection by the rotating frame with its known actual size to determine the size reference for constructing the scale. Based on the real-time pose of the agricultural machinery as a spatial reference point, the Gauss-Kruger projection algorithm is used to convert geographic coordinates into planar projected coordinates. Based on the correspondence between the actual size of the agricultural machinery and pixel size, an adaptive scale between image pixels and actual distances is constructed, suitable for the current aerial image. Applying the established scale and projection relationship, the pixel coordinates of all key navigation points and secondary navigation points in the path network are uniformly converted into actual projected coordinates, forming a multi-machine collaborative guidance map. Then, through projection back-calculation, the precise latitude and longitude geographic coordinates corresponding to all navigation points are output.

[0067] The specific work involves: Using an autonomous navigation system deployed on agricultural machinery, the BeiDou RTK module continuously outputs the machinery's high-precision latitude and longitude coordinates and heading angle (with true north as a reference) to establish a global geographic coordinate benchmark. Simultaneously, it processes the aerial image's overhead pixel size information, parsed from the aerial photograph by a rotating frame small target detection algorithm. This pixel size is then compared with the machinery's known physical dimensions to establish an accurate size reference benchmark for coordinate transformation. After acquiring the machinery's real-time geographic location and visual size benchmark, the Gauss-Kruger projection algorithm is used to convert the latitude and longitude geographic coordinates provided by the BeiDou RTK module into planar projected coordinates, facilitating planar distance and direction calculations. Based on the established correspondence between the machinery's known physical dimensions and their pixel sizes in the image, adaptive calculations are performed. The conversion ratio between image pixels and actual metric units is determined to construct a scale suitable for the current aerial images, ensuring consistency and reliability in the measurement conversion from image pixels to the real world. After successfully constructing the scale and establishing the projection coordinate system, the pixel coordinates of all navigation points defined in the path network, including manually calibrated key navigation points and generated secondary navigation points, are batch-converted according to the established projection relationship and scale. Specifically, the pixel coordinates are converted into unified planar projection coordinates. Using the inverse calculation formula of Gauss-Kruger projection, the projection coordinates are accurately converted into standard latitude and longitude geographic coordinates to form a multi-aircraft collaborative guidance map. Finally, the latitude and longitude information corresponding to all navigation points with actual geographical significance is output, completing the complete mapping from visual image positioning to the global geographic coordinate system.

[0068] Furthermore, the process of outputting the precise latitude and longitude geographic coordinates corresponding to all navigation points is as follows: Based on the determined scale and projection relationship parameters, the pixel coordinates of key and secondary navigation points in the path network are batch-converted and unified, mapping the positions in the image pixel space to a unified planar projected coordinate system, thus obtaining the actual projected coordinates corresponding to each navigation point. This provides the coordinate basis for the formation of a multi-machine collaborative guidance map. Based on the converted projected coordinates, spatial layout planning is performed for key and secondary navigation points. Combining the needs of multi-machine collaborative operations, including avoiding path conflicts and optimizing workflow, the specific navigation points are determined. The relative positions of points on the map are used to integrate discrete navigation points and their connections into a concrete multi-machine collaborative guidance map. This map presents the network structure of all feasible paths within the work area, the distribution of navigation points, and the connectivity between navigation points. Using the inverse calculation formula of Gauss-Kruger projection, combined with known projection relationship parameters, the projected coordinates of navigation points in the multi-machine collaborative guidance map are calculated in reverse. The projected coordinates of each navigation point are restored to accurate latitude and longitude geographic coordinates, completing the complete mapping from image pixel positioning to the global geographic coordinate system. This provides high-precision geographic location information that can be directly accessed for agricultural machinery autonomous navigation systems.

[0069] S4. Construct a heuristic function based on the distance between key navigation points and the orientation angle to dynamically guide agricultural machinery to perform global autonomous navigation from the starting point to the target point;

[0070] S5. Extract road edges through instance segmentation, fit the centerline and calculate the image drift, and adjust the steering in real time to keep the agricultural machinery driving within the lane;

[0071] S6: Combining obstacle size and road space, it adaptively selects detour or parking strategies to complete local obstacle avoidance and continue navigation to the destination.

[0072] Example 2, as Figures 1 to 7 As shown, based on Embodiment 1, the present invention provides a technical solution: S4 specifically includes: constructing a heuristic function to comprehensively evaluate the Euclidean distance from the current navigation point to the target navigation point and the angle between the forward direction; evaluating the priority of each navigation point through the heuristic function to provide the agricultural machinery with the optimal forward direction guidance toward the target navigation point; constructing a path tree network based on existing key navigation points and secondary navigation points according to spatial connectivity; initializing the actual cost for each path segment; organizing discrete navigation points into a topological structure that can be searched by the heuristic function based on the Euclidean distance between adjacent nodes; iteratively selecting the subsequent navigation point with the best evaluation value in the path tree network starting from the starting point, thereby generating an ordered sequence of navigation points from the starting point to the target navigation point, forming the final global macro navigation route, and completing global path planning;

[0073] Furthermore, the process of evaluating the priority of each navigation point using a heuristic function is as follows: Based on the coordinates of the current navigation point and the target navigation point, the Euclidean distance is calculated as a spatial proximity index, reflecting the straight-line distance cost from the agricultural machinery to the target. Simultaneously, the cosine of the angle between the current heading angle and the target heading angle is used to determine the directional angle cost, quantifying the turning cost and characterizing the energy consumption required for the agricultural machinery to adjust its direction. The straight-line distance cost and directional angle cost are merged into a single cost index to eliminate dimensional differences and ensure that distance and direction costs are comparable on the same scale. After obtaining the straight-line distance cost and directional angle cost, weighting coefficients are assigned to the costs based on the actual operating scenarios of the agricultural machinery. Specifically, in high-speed or open areas, the straight-line distance cost is preferentially reduced. Cost weighting and steering smoothness weighting are added in low-speed or complex environments. The weighted summation is used to calculate a comprehensive heuristic value based on a heuristic function. The smaller the value, the more the navigation point has the dual advantages of being close to the target and having low turning costs. This determines candidate navigation points and forms a set of candidate navigation points. The set of candidate navigation points is sorted in ascending order according to the comprehensive heuristic value to generate a priority queue. The navigation point at the head of the queue is the current optimal choice. The agricultural machinery selects navigation points as the next forward target according to the order of the priority queue. At the same time, the current position and heading data are continuously updated, and the heuristic function parameters are dynamically corrected to ensure that the environmental changes are reflected in real time during the movement, forming an optimal path sequence from the starting point to the target navigation point, providing continuous directional guidance for the agricultural machinery.

[0074] The expression for evaluating the heuristic function is as follows:

[0075] ;

[0076] ;

[0077] ;

[0078] In the formula: This is a comprehensive heuristic value used to evaluate the value from the current point ( Move to the candidate next navigation point ( The overall cost of choosing a path point is calculated as follows: the smaller the value, the lower the cost and the higher the priority. This is the current location; for Candidate navigation points between the target point; These are weight values ​​used to balance the importance of different cost components in the cost function; The actual cost already incurred is fixed at 0, indicating that the cost already incurred in moving from the starting point to the candidate navigation point is not considered in the evaluation, and the contribution of the choice to the future arrival at the final target navigation point is fully evaluated. This is a heuristic cost used to estimate the cost from candidate next navigation points to the final target navigation point; The Euclidean distance represents the distance from the current point ( ) to candidate next navigation point ( The straight-line distance is used to encourage agricultural machinery to choose the nearest next stop; The directional deviation angle is the angle between the global target direction vector (AB) and the current movement direction vector (AB). The angle between the two sides is limited to between 0° and 90°, which measures the choice. When the next stop is taken, the degree to which the agricultural machinery's course deviates from the final target navigation point; As a consequence of deviation from the intended direction, Convert it to a coefficient between 0 and 1, when hour, ,when hour, , indicating candidate points , Ideally, the target points should be on the same straight line. In other cases, the product of the cost of directional deviation and the cost of distance should be considered to encourage agricultural machinery to choose path points that make its course more aligned with the final target.

[0079] The specific work involves: constructing a heuristic function to comprehensively evaluate the distance from the current navigation point to the target point, using Euclidean distance as the core indicator to quantify spatial distance costs. Simultaneously, a forward direction angle parameter is introduced; by calculating the cosine of the angle between the current heading and the target direction, the turning cost is reflected. These two indicators are weighted and fused to form a comprehensive evaluation value, dynamically assessing the priority of each navigation point. A smaller value indicates that the navigation point is closer to the target and has lower turning costs, providing directional guidance for the agricultural machinery. Through heuristic function filtering, the agricultural machinery can prioritize the navigation point with the lowest cost, reducing invalid path exploration and improving the efficiency of global path planning. Based on the spatial connectivity between key and secondary navigation points, a hierarchical tree network topology is constructed, with the starting point as the root node, expanding child nodes level by level according to path branching relationships to form a multi-level path tree. Each path segment (connected to adjacent nodes) is defined. The actual cost is initialized based on Euclidean distance and dynamically corrected by environmental factors such as terrain slope and obstacle density. Discrete navigation points are connected by path segments to form a continuous topology, supporting heuristic search algorithm traversal. The tree network structure ensures path uniqueness, avoids loops, and retains the possibility of multiple path selection, providing a searchable underlying framework for global path planning. Based on the heuristic function evaluation results, the A* algorithm is used to iteratively search for the optimal path in the path tree network. Starting from the starting point, the next navigation point with the best evaluation value (minimum combined distance and direction cost) is selected as the next hop until the target navigation point is reached. During the search process, the current position and heading of the agricultural machinery are dynamically updated, and the weight of the heuristic function is adjusted in real time to adapt to dynamic environmental changes. Finally, an ordered sequence of navigation points is generated, forming a global macro route, ensuring that the agricultural machinery travels along the path with the lowest cost.

[0080] S5 specifically includes: acquiring road images in real time through a camera mounted on the agricultural machinery; processing the road images using a pre-trained instance segmentation model; identifying and extracting pixel-level edge contours of the road area; reconstructing the road boundary lines on both sides based on the extracted pixel-level edge contours; then, based on the reconstructed road boundary lines on both sides, generating a reference navigation centerline located in the center of the road through geometric calculation; in each frame processing, projecting the fitted reference navigation centerline onto the image coordinate system; calculating the lateral pixel distance between the reference navigation centerline and the preset baseline (vertical centerline of the image) to obtain the lateral drift of the agricultural machinery; simultaneously, calculating the angle between the centerline and the baseline to obtain the heading angle, which together constitute the pose deviation; using the calculated lateral drift and heading angle as inputs to the control system; and generating corresponding steering angle adjustment commands in real time using a predetermined control algorithm (PID controller) based on the lateral drift and heading angle; and driving the steering actuator through continuous closed-loop feedback and adjustment to keep the agricultural machinery dynamically aligned with the road centerline.

[0081] The specific work involves: acquiring real-time images of the road ahead using a camera mounted on the front of the agricultural machinery; performing pixel-level analysis on each frame using a pre-trained instance segmentation model to accurately identify each pixel belonging to the road area and outputting the pixel-level edge contour of the road area; based on this segmentation result, reconstructing the boundary lines representing both sides of the road from the cluttered scene; and applying geometric algorithms to fit and generate a reference navigation centerline located in the center of the road based on the spatial position of the road boundary lines. This reference navigation centerline constitutes the ideal reference trajectory for the autonomous driving of the agricultural machinery in the image coordinate system. In each frame image processing cycle, the fitted reference navigation centerline is projected onto a fixed image coordinate system. By calculating the lateral pixel distance between this reference navigation centerline and the preset baseline (vertical center line of the image), the relative position of the agricultural machinery body to the road centerline is quantified. The system calculates the lateral drift and the angle between the reference navigation centerline and the preset reference line in the image to obtain the heading angle of the agricultural machinery. The lateral drift and heading angle together constitute the complete pose deviation, which represents the comprehensive deviation of the agricultural machinery's current actual pose from the ideal trajectory in terms of lateral position and direction of travel. The real-time calculated lateral drift and heading angle are used as input variables of the control system. The lateral drift and heading angle are solved using a predetermined control algorithm (PID controller) to generate corresponding steering angle adjustment commands in real time. The steering angle adjustment commands are then sent to the steering actuator (servo motor or hydraulic steering system) of the agricultural machinery to drive it to perform steering actions. This dynamically and smoothly corrects the agricultural machinery's travel direction and lateral position, enabling it to stably maintain its travel on the desired trajectory aligned with the road reference navigation centerline, thus achieving high-precision lane keeping function.

[0082] S6 specifically includes: real-time perception of the physical size of obstacles ahead and the space they occupy in the road; calculation of the current passable width of the road based on the road boundary line; calculation of the obstruction degree to analyze the degree of obstruction of the path by the obstacle; if the obstruction degree is lower than the preset obstruction threshold, it is determined to be a detourable scenario; otherwise, it is determined to be a parking scenario. Based on the obstruction degree assessment result, an obstacle avoidance strategy is dynamically selected: for detourable scenarios, a detour trajectory is automatically generated according to the left or right position of the obstacle to ensure a safe distance; for parking scenarios, a braking command is immediately triggered, and a waiting state is entered until the obstacle is cleared. After detour or parking, based on the updated environmental information, the path is locally replanned to the original secondary navigation point, the heuristic navigation process is restored, and the vehicle continues to move towards the target point along the global path to ensure seamless connection between obstacle avoidance actions and global navigation targets.

[0083] The specific work involves: acquiring the physical dimensions and specific locations of obstacles ahead in real time using an airborne multi-source sensing module; accurately calculating the actual passable width of the current road using road boundary lines obtained from an instance segmentation model; and calculating the obstruction degree by comprehensively analyzing the obstacle dimensions and locations to quantitatively assess the degree of obstruction to the effective path. This obstruction degree is a key indicator for determining subsequent behavioral strategies. The real-time calculated obstruction degree is compared with a preset obstruction threshold as the basis for decision-making and traffic diversion: if the obstruction degree is lower than the preset threshold, it indicates that the road still has safe detour conditions and is determined as a detourable scenario; if the obstruction degree reaches or exceeds the preset threshold, it indicates that the road is completely or nearly completely blocked and there are no safe detour conditions, and it is determined as a stopping scenario. Based on the scenario determination results, the corresponding obstacle avoidance strategy is dynamically invoked. For scenarios determined to be detourable, a smooth detour trajectory is automatically generated based on the obstacle's lateral position (left or right) in the road. The planning of the detour trajectory strictly adheres to the preset... A safe distance is set to ensure sufficient physical separation between the agricultural machinery and obstacles. In scenarios where stopping is deemed necessary, an emergency braking command is immediately triggered to the braking system, putting the agricultural machinery into a safe stopping and waiting state. This state continues until continuous environmental perception confirms that the obstacle has been removed or the road is clear, ensuring the absolute safety of the agricultural machinery's behavior in dangerous situations where passage is impossible. After successfully executing the detour or removing the obstacle, the agricultural machinery has deviated from the original local path. Simultaneously, based on the latest environmental perception information, starting from the current position of the agricultural machinery and taking the next planned secondary navigation point as the temporary target, a local path replanning algorithm is initiated to generate a new path segment that can safely and efficiently guide the agricultural machinery back to the original route. If the agricultural machinery reaches the secondary navigation point through local replanning, it automatically switches back to the global heuristic navigation mode and continues to travel towards the final target navigation point according to the pre-calculated path sequence. This ensures that a single obstacle avoidance behavior is an independent, closed-loop local event, and can seamlessly connect with the global navigation task after completion, ensuring the continuity and robustness of the overall operation process.

[0084] The expression for calculating the degree of obstruction is as follows:

[0085] ;

[0086] In the formula: The degree of obstruction is used to quantify the extent to which an obstacle hinders the passage of a road. Obstacle width refers to the maximum physical width of an obstacle in the direction perpendicular to the road, reflecting the size of the obstacle itself; The passable width of the road is the net width between the two road boundary lines identified by the instance segmentation model, representing the theoretically maximum safe passage space of the current road segment. This refers to the lateral offset of the obstacle, which is the absolute value of the lateral distance from the center point of the obstacle to the center line of the road. The smaller the value, the closer the obstacle is to the center of the road, and the greater the obstruction to passage. The larger the value, the closer the obstacle is to the roadside, potentially leaving more space for passage. As the blockage increases, the degree of obstruction rises significantly, with an obstacle spanning the entire road surface ( This will cause the blockage to approach its maximum value; when As the width increases, the congestion decreases accordingly. Faced with the same obstacle, a wider road means more space to detour, resulting in better traffic conditions. It is a location-related factor, which occurs when the obstacle is located in the exact center of the road. When this value is 1, the obstruction level reaches its maximum value, indicating the greatest obstruction to traffic. This occurs when the obstacle is close to one side of the road boundary. When this value approaches 0, the congestion calculation reaches its minimum value, at which point there is maximum space available for passage on the other side of the road.

[0087] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

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

Claims

1. A low-precision map-based high-adaptability fruit orchard equipment autonomous navigation method, characterized in that, Comprise the following steps: S1, deploy an autonomous navigation system, and obtain global images of the fruit tea garden by unmanned aerial vehicle aerial photography, and manually set key navigation points, wherein the key navigation points include hangar entrances and exits, starting points and ending points of work areas, and transfer concentration points; S2, detect the road by a semantic segmentation algorithm, fit a straight line to obtain a corner point, and automatically generate a secondary navigation point connecting the key points; S3, use the autonomous navigation system to obtain the latitude and longitude and heading angle of the agricultural machine, construct a scale suitable for the current aerial image, automatically obtain the mapping of pixel coordinates-projection coordinates-global geographic coordinates, and obtain a multi-machine cooperative guidance map; S4, based on the distance and orientation angle between the key navigation points, a heuristic function is constructed to dynamically guide the agricultural machine to globally and autonomously navigate from the starting point to the target point, specifically including: Construct a heuristic function to comprehensively evaluate the Euclidean distance from the current navigation point to the target navigation point and the included angle between the current heading angle and the target direction angle, and evaluate the priority of each navigation point by the heuristic function to provide the agricultural machine with the optimal forward direction guidance to the target navigation point; Based on the existing key navigation points and secondary navigation points, a path tree network is constructed according to spatial connectivity, and the actual cost of each path segment is initialized, and the Euclidean distance between adjacent nodes is used as the basis to organize discrete navigation points into a topological structure that can be searched by the heuristic function; According to the evaluation results of the heuristic function, the subsequent navigation point with the optimal evaluation value is iteratively selected from the starting point in the path tree network, and then an ordered navigation point sequence from the starting point to the target navigation point is generated, forming the final global macroscopic navigation route; The process of evaluating the priority of each navigation point by the heuristic function is: Based on the current navigation point coordinates and target navigation point coordinates, the Euclidean distance is calculated as a spatial proximity index, and the direction angle cost is determined by the cosine value of the included angle between the current heading angle and the target direction angle to quantify the turning cost, and the straight line distance cost and the direction angle cost are normalized respectively; After obtaining the straight line distance cost and the direction angle cost, according to the requirements of the actual work scene of the agricultural machine, weight coefficients are assigned to the two costs respectively, and then the candidate navigation points are determined to form a candidate navigation point set; The candidate navigation point set is sorted in ascending order according to the comprehensive heuristic value to generate a priority queue, and the head navigation point of the queue is the current optimal choice, and the agricultural machine selects the navigation points as the next forward target according to the order of the priority queue, while continuously updating the current position and heading data, dynamically correcting the heuristic function parameters, and forming an optimal path sequence from the starting point to the target navigation point; S5, extract the road edge by instance segmentation, fit the centerline and calculate the picture drift, and adjust the turning to keep the agricultural machine driving in the lane in real time; S6, adaptively select the detour or parking strategy according to the size of the obstacle and the road space, complete local obstacle avoidance and continue to navigate to the end point.

2. The method according to claim 1, wherein: The S1 specifically includes: The autonomous navigation system comprises an airborne multi-source sensing module, a UAV aerial photograph transmission module and an airborne decision module, wherein the airborne multi-source sensing module comprises a Beidou RTK module, a depth camera module and a visible light camera module, and the UAV is operated to take aerial photographs of a target tea plantation area to obtain aerial photograph information of the whole tea plantation area; On the obtained aerial photograph, a key navigation point is manually identified and set by an operator according to actual operation requirements, and pixel coordinates of the key navigation point are extracted; The pixel coordinates of the manually set key navigation point are bound and stored with image data collected by the UAV, and the Beidou RTK module is initialized to complete the starting and data preparation process of the autonomous navigation system.

3. The method of claim 1, wherein: The S2 specifically comprises: The UAV aerial photograph is processed by a semantic segmentation algorithm to identify the distribution of roads, operation areas and obstacles in the tea plantation, generate a road mask, and screen a passable range of agricultural machinery; Straight line fitting and slope analysis are performed on the road mask to automatically detect corner points and multi-line intersection points in the path, and based on the road connectivity feature, a secondary navigation point is generated at the structure change to make the path between the two points approximately straight; The generated secondary navigation point is topologically connected with the manually set key navigation point to construct a complete navigation path network, and the pixel distance between the connected adjacent points is obtained by image processing; A rotating frame small target detection algorithm is used to identify the operation agricultural equipment to obtain pixel positioning, orientation angle and pixel size information of the aerial photograph.

4. The method of claim 1, wherein: The S3 specifically comprises: The Beidou RTK module of the autonomous navigation system is used to obtain the accurate latitude and longitude coordinates and the heading angle based on the north direction of the agricultural equipment in real time, and the pixel size of the aerial photograph obtained by the rotating frame small target detection algorithm is combined with the known actual size to determine the size reference for constructing the scale; Based on the real-time pose of the agricultural equipment as a spatial reference point, the Gauss-Kruger projection algorithm is used to convert the geographic coordinates into plane projection coordinates, and based on the corresponding relationship between the actual size and the pixel size of the agricultural equipment, the scale between the image pixel and the actual distance is adaptively constructed, which is applicable to the current aerial photograph; The established scale and projection relationship are applied to uniformly convert the pixel coordinates of all key navigation points and secondary navigation points in the path network into actual projection coordinates to form a multi-machine cooperative guidance map, and then the accurate latitude and longitude geographic coordinates corresponding to all navigation points are output by projection calculation.

5. The method according to claim 4, wherein: The process of outputting the accurate latitude and longitude geographic coordinates corresponding to all navigation points comprises: According to the determined scale and projection relationship parameters, the pixel coordinates of the key navigation points and the secondary navigation points in the path network are batch-converted and uniformly converted into actual projection coordinates, and the positions in the image pixel space are mapped into a unified plane projection coordinate system to obtain the actual projection coordinates corresponding to each navigation point. Based on the converted projection coordinates, the key navigation points and the secondary navigation points are spatially laid out, the relative position relationship of each navigation point in the map is determined in combination with the demand of multi-machine cooperative operation, and then the discrete navigation points and their connection relationship are integrated and drawn into a visual multi-machine cooperative guidance map, presenting the network structure of all feasible paths in the operation area, the distribution of navigation points and the connectivity between navigation points; By using the inverse calculation formula of Gauss-Kruger projection and combining the known projection relationship parameters, the projection coordinates of the navigation points in the multi-machine cooperative guidance map are inversely calculated, the projection coordinates of each navigation point are restored to accurate latitude and longitude geographical coordinates, and the complete mapping from image pixel positioning to global geographical coordinate system is completed.

6. The method of claim 1, wherein: The calculation expression of the heuristic function is as follows: ； ； ； In the formula: is a comprehensive heuristic value; is the current point; is a candidate navigation point between the current point and the target point; is a weight value; is an actual paid cost, fixed as 0; is a heuristic cost; is a Euclidean distance; is a direction deviation angle; is a direction deviation cost, when , , indicates that the candidate point , , the target point are on the same line, which is the best case.

7. The method of claim 1, wherein: The S5 specifically includes: The camera mounted on the agricultural machine collects road images in real time, processes the road images by using a pre-trained instance segmentation model, identifies and extracts the pixel-level edge contour of the road region, reconstructs the two-side road boundary lines based on the extracted pixel-level edge contour, and then generates a reference navigation center line in the road center according to the reconstructed two-side road boundary lines through geometric calculation fitting; In each frame processing, the fitted reference navigation center line is projected into the image coordinate system, the lateral drift amount of the agricultural machine is obtained by calculating the lateral pixel distance between the reference navigation center line and the image preset reference line, and at the same time, the heading angle is obtained by calculating the included angle between the center line and the reference line, which together constitute the pose deviation amount; The calculated lateral drift amount and heading angle are taken as the input of the control system, and a predetermined control algorithm is used to generate a corresponding steering angle adjustment instruction in real time according to the lateral drift amount and the heading angle, and the steering execution mechanism is driven to act through continuous closed-loop feedback and adjustment.

8. The method according to claim 7, wherein: The S6 specifically includes: The physical size of the front obstacle and the occupied space of the obstacle in the road are perceived in real time, the passable width of the current road is calculated in combination with the road boundary line, the blocking degree is calculated to analyze the blocking degree of the obstacle to the path, and if the blocking degree is lower than the preset blocking threshold, it is determined as a bypassable scene; otherwise, it is determined as a parking scene; Based on the blocking degree evaluation result, an obstacle avoidance strategy is dynamically selected: for the bypassable scene, a bypass trajectory is automatically generated according to the left or right position of the obstacle; for the parking scene, a brake instruction is triggered immediately to enter a waiting state until the obstacle is removed; After the bypassing or parking is performed, the path is locally re-planned to the original secondary navigation point based on the updated environmental information, the heuristic navigation process is restored, and the agricultural machine continues to travel along the global path to the target point.

Images