Path preview point acquisition method based on dynamic radius search and adaptive extension

Abstract

The invention relates to a path preview point acquisition method based on dynamic radius search and adaptive extension, and belongs to the technical field of agricultural vehicle automatic driving navigation. According to the method, a dynamic radius search mechanism with the current position of the vehicle as the circle center is introduced, the consistency judgment of the path section direction and the vehicle course is combined, a reasonable path section is screened out in the search radius, adaptive extension is performed based on a projection point direction vector to generate a preview point, and meanwhile, a cost function scoring mechanism is introduced. The factors of distance error, direction deviation and curvature are comprehensively considered, and an optimal projection section is selected from multiple dimensions for extension, so that the dynamic property and the direction robustness of path point acquisition are realized. By optimizing a preview point selection mechanism, the stability and the control precision of path tracking are enhanced, so that the operation reliability of agricultural machinery in an unstructured farmland is improved.

Description

Technical Field

[0001] This invention relates to a path prediction point acquisition method based on dynamic radius search and adaptive extension, belonging to the field of agricultural vehicle autonomous driving navigation technology. The method includes: constructing a dynamic search radius region centered on the vehicle's current position, which can be adaptively adjusted according to vehicle speed and path curvature; combining the consistency between the path segment direction and the vehicle's heading angle, selecting the optimal path segment and extending it based on its direction vector to obtain a dynamically predicted target point, thereby improving the navigation accuracy and operational stability of agricultural vehicles on their work paths. Background Technology

[0002] Smart agriculture, as a key direction for future agricultural development, relies on the continuous breakthroughs and widespread application of intelligent agricultural machinery technology. The development of intelligent agricultural machinery, in turn, is highly dependent on the continuous advancement of automatic navigation technology for agricultural vehicles. Agricultural vehicle navigation systems typically include two key modules: path planning and path tracking. Path tracking, as the core component, directly affects the accuracy and overall efficiency of operations through its control precision and stability.

[0003] In existing technologies, commonly used path tracking algorithms include Pure Pursuit, Model Predictive Control (MPC), and Stanley's algorithm. These algorithms generally rely on path preview points as target control inputs, but they still have limitations to varying degrees in terms of path point selection strategies, dynamic adaptability, and control accuracy. Pure tracking algorithms use a fixed-distance extension method to select forward path points as preview points and calculate steering angles through geometric relationships. They are simple in structure and have low computational cost, but they rely only on a single preview point and ignore the consistency between the path direction and the vehicle direction. This makes them prone to control jitter or delayed response issues in scenarios with drastic curvature changes or path loops. The MPC algorithm predicts the future state in each control cycle and solves for optimal control input based on a set of continuous preview points. It has strong robustness and flexibility and can comprehensively consider multiple constraints such as path, speed, and vehicle dynamics. However, it has high computational complexity and requires high system modeling accuracy and real-time optimization capabilities, making it difficult to deploy in some resource-constrained agricultural scenarios. The Stanley algorithm uses the nearest point projected onto the path from the current vehicle position as the control target and combines lateral error and heading error to jointly adjust steering control. It is suitable for straight or gently curved paths under medium- and low-speed driving conditions. However, it is prone to selecting incorrect target segments or oscillations when there are many curves, dense paths, or areas with overlapping directions (such as loops). Existing algorithms generally suffer from unstable point selection, insufficient directional judgment, and poor dynamics in the path prediction point acquisition process, making them unsuitable for navigation requirements of unstructured paths in complex farmland environments. Therefore, researching a more robust and adaptive path prediction point acquisition method is of significant practical importance for improving the path tracking performance of agricultural vehicles.

[0004] Existing agricultural machinery navigation systems suffer from poor robustness, inconsistent orientation, and a lack of dynamic adaptability in path pre-aiming point selection. This is particularly problematic in complex farmland environments, where traditional methods struggle to handle conditions such as dense path loops, variable directions, and unstructured terrain. Most current agricultural machinery path tracking systems use the minimum Euclidean distance between the current position and a path point as the basis for pre-aiming point selection, neglecting the directionality of the path segment and the vehicle's heading information. This can easily lead to unreasonable pre-aiming point selection, causing vehicle vibration, oversteer, or path deviation, thus affecting operational accuracy and stability. Summary of the Invention

[0005] The purpose of this invention is to address the problems of poor robustness, inconsistent direction, and lack of dynamic adaptability in the selection of path pre-aiming points in existing agricultural machinery navigation systems. This is particularly true in complex farmland environments where traditional methods struggle to handle actual working conditions such as dense path loops, variable directions, and unstructured terrain. Most current agricultural machinery path tracking systems use the minimum Euclidean distance between the current position and a path point as the basis for pre-aiming point selection, ignoring the directionality of the path segment and the vehicle's heading information. This easily leads to unreasonable pre-aiming point selection, causing vehicle vibration, oversteer, or path deviation, affecting operational accuracy and stability.

[0006] This invention provides a method for obtaining path preview points based on dynamic radius search and adaptive extension, comprising:

[0007] Estimate the path curvature of global path planning points;

[0008] Using the vehicle's position as the center, the search radius is adaptively set according to the vehicle's speed and path curvature, and all path segments that satisfy the condition that the start and end points are within the search circle are selected to obtain a set of candidate path segments.

[0009] For each candidate path segment, calculate the orthogonal projection point of the vehicle's current position on that path segment;

[0010] Calculate the angle between the vehicle's heading vector and the direction of each path segment in the candidate path segment set, in order to filter out valid candidate path segments with the same direction;

[0011] The optimal path segment is selected from the selected valid candidate path segments using a pre-built joint objective selection mechanism;

[0012] At the orthogonal projection point of the optimal path segment, the extension distance is dynamically calculated based on the vehicle speed and the curvature of the optimal path segment to determine the coordinates of the optimal aiming point.

[0013] Optionally, the three-point method is used to estimate the path curvature of global path planning points, and the mathematical expression is:

[0014]

[0015] Among them, K i Let P be the curvature of the i-th path segment. i Let P be the coordinates of the i-th point. v =(x v ,y v () indicates location.

[0016] Optionally, the mathematical expression for adaptively setting the search radius based on vehicle speed and path curvature is as follows:

[0017] R = R0 + k v ·vkk ·K i

[0018] Where R is the dynamic search radius, R0 is the basic search radius, representing the initial search range of the system in a static or low-speed state, and k v Here, k is the speed adjustment coefficient, v is the vehicle speed, and k is the speed regulation coefficient. k K is the curvature suppression coefficient. i Let be the curvature of the i-th path segment.

[0019] Optionally, the calculation expression for the orthogonal projection point is:

[0020]

[0021]

[0022] in, Let P be the direction vector of the path segment. v This is the vehicle's current location. P is the starting point of the path segment. i Point to the vehicle's current position P v The vector, Let t be the orthogonal projection point of the current vehicle position on the path segment. i For vectors The projection length onto the path direction vector.

[0023] Optionally, when calculating the orthogonal projection point of the vehicle's current position on the path segment, if t i If <0, the projection point becomes P. i ;like Then the projection point is at P i+1 after.

[0024] Optionally, the mathematical expression for calculating the angle between the vehicle's heading vector and the direction of each path segment in the candidate path segment set is as follows:

[0025]

[0026]

[0027] Where, θ v This is the current heading angle of the vehicle. Let Δθ be the unit heading vector of the current vehicle. i This is the difference in angle between the vehicle's current heading and the direction of the path segment; P is the starting point of the path segment. i Point to the vehicle's current position P v ;

[0028] If |△θ i |≤θthresh The vehicle's direction is basically consistent with the path segment, making this segment a valid candidate path segment; if |△θ i |>θ thresh If so, then the path segment will be removed.

[0029] Optionally, the joint objective selection mechanism evaluates the course deviation and lateral deviation as dual objective functions, and constructs a cost function with lateral error term, course error term and path segment curvature as factors, and selects the optimal path segment by combining the dual objective function and the cost function;

[0030] The mathematical expression for the joint objective selection mechanism is:

[0031]

[0032]

[0033]

[0034] Among them, C i The objective function score for the i-th candidate path segment. This is the lateral deviation distance between the vehicle's current position and its orthogonal projection point on the path segment. Let Δθ be the unit heading vector of the current vehicle. i This is the difference in angle between the vehicle's current heading and the direction of the path segment. P is the starting point of the path segment. i Point to the vehicle's current position P v The vector, J i Let i be the cost function score of the i-th candidate path segment. * α is the index number of the finally selected optimal path segment. i w represents the weight coefficients of the objective function. i λ is the weighting coefficient for the scoring item. i To integrate the weighted coefficients, K i Let be the curvature of the i-th path segment.

[0035] Optionally, the mathematical expression for dynamically calculating the extension distance based on vehicle speed and path curvature to determine the optimal aiming point coordinates is as follows:

[0036]

[0037]

[0038] Where L is the dynamic extension distance, L0 is the basic extension distance, and k l Here, k is the speed adjustment coefficient, v is the vehicle speed, and k is the speed regulation coefficient. kl The curvature suppression coefficient, The curvature value of the optimal path segment. These are the orthogonal projection points on the optimal path segment. P is the direction vector on the optimal path segment. f These are the coordinates of the final aiming point.

[0039] Optionally, the method is applied to three stages, each of which has a corresponding pre-aiming point selection range;

[0040] The first stage is the initialization stage before the vehicle enters the path; the second stage is the operation stage of accurately tracking the planned path; and the third stage is the loop recovery stage after the path is completed.

[0041] In the first stage, the search radius is gradually increased from the preset minimum search radius until a path point is detected within a certain radius. The path points within that radius are used as the center, and the path points before and after them at preset proportions are extracted as the range of the aiming point selection.

[0042] In the second stage, a path segment clustering constraint strategy is introduced into the downward sequential retrieval mechanism of the online point to divide the entire path into multiple continuous job row clusters with the same direction as the range of pre-aiming points;

[0043] In the third stage, the starting point of the path is selected as the predetermined proportion of the overall path planning points to form the loop guide segment, which is used as the range for selecting the aiming points.

[0044] Optionally, each of the three stages has corresponding restrictive conditions:

[0045] In the first stage, the lateral deviation is required to be less than a preset multiple of the dynamic search radius, and the projection point must fall within the path segment or be compensated through endpoint projection. If the heading deviation |△θ is satisfied within five consecutive cycles, then the lateral deviation must be within a preset multiple of the dynamic search radius. i If the angle is ≤25° and the lateral deviation is less than 0.08m, the online status is considered successful.

[0046] The second stage requires a heading deviation of |△θ i If the angle is ≤15° and the lateral deviation is less than 0.05m, and the deviation exceeds the above range 8 times consecutively, it is determined to be a fault pre-aiming point, and the vehicle is stopped. The dynamic extension distance L is limited to the range of 1.2m to 4m.

[0047] In the third stage, the loop path segment is limited to the interval between 1% and 5% before the path start. When restoring the path segment, if the heading deviation |Δθ is satisfied within eight consecutive cycles... i If the angle is ≤15° and the lateral deviation is less than 0.05m, the loop closure is considered successful.

[0048] Compared with the prior art, the present invention has the following beneficial effects:

[0049] This invention introduces a dynamic radius search mechanism centered on the vehicle's current position, combined with a judgment of the consistency between the path segment direction and the vehicle's heading. Within the search radius, reasonable path segments are selected, and adaptive extension is performed based on the projection point direction vector to generate a pre-aiming point. Simultaneously, a cost function scoring mechanism is introduced, comprehensively considering distance error, direction deviation, and curvature factors, to select the optimal projection segment for extension from multiple dimensions, thereby achieving dynamic and directional robustness in path point acquisition. By optimizing the pre-aiming point selection mechanism, the stability and control accuracy of path tracking are enhanced, thus improving the reliability of agricultural machinery operations in unstructured farmland. Attached Figure Description

[0050] Figure 1 This is a flowchart of the aiming point acquisition process for the dynamic radius search and adaptive extension method according to an embodiment of the present invention.

[0051] Figure 2 This is a flowchart illustrating the full path condition conversion method and the range of pre-aiming point selection in an embodiment of the present invention.

[0052] Figure 3 This is a global path planning diagram for an application example of the present invention;

[0053] Figure 4 This is a state sequence change diagram of an application example of the present invention;

[0054] Figure 5 This is a diagram showing the lateral and heading deviations for straight-line tracking on a hard cement road surface, which is an application example of the present invention.

[0055] Figure 6 The diagram shows the lateral and heading deviations during straight-line tracking on bumpy dry land, which is an application example of the present invention. Detailed Implementation

[0056] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and should not be used to limit the scope of protection of the present invention.

[0057] This embodiment provides a path preview point acquisition method based on dynamic radius search and adaptive extension. After path planning is completed, the method sends the path information to the agricultural vehicle navigation controller. The generation and transmission process of preview points is divided into three stages: the first stage is the Off-Path initialization stage before the vehicle enters the path; the second stage is the On-Path operation stage for accurately tracking the planned path; and the third stage is the Off-Path loop recovery stage after the path is completed. This method aims to shorten the online time of agricultural vehicles, thereby effectively improving agricultural operation efficiency and intelligence.

[0058] In the first stage, before the agricultural vehicle enters the planned path, the navigation controller obtains the vehicle's real-time position and attitude status through the onboard information monitoring unit. Combined with the global path point set provided by the path planning algorithm, the path segments are traversed and analyzed. Since the vehicle is not currently online, a pre-aiming point guidance method is used to drive the vehicle to quickly approach the path.

[0059] During the path initialization phase, the selection of the target point area adopts the minimum radius method, that is, starting from the preset minimum search radius, the search radius is gradually increased until a path point is detected within a certain radius range.

[0060] Using the path points within this radius as the center, extract 5% of the path points before and after it as the target path segment for subsequent projection calculations and preview point selection, ensuring the rationality of the initial preview point and the continuity of path guidance.

[0061] To achieve efficient path segment selection and obtain reasonable aiming distances and aiming points, a three-point method is used to estimate the curvature of the path segments. Combined with the current vehicle speed, a dynamic search radius is constructed to select a set of path segments that meet the radius constraints.

[0062] For each candidate path segment, projection points are calculated, and filtering is performed based on the consistency between the vehicle's current heading and the path segment's direction. A dual objective function evaluation mechanism based on heading deviation and lateral deviation, along with a comprehensive cost function scoring mechanism incorporating factors such as path curvature and deviation, are then introduced. Adaptive extension is performed on the path segment with the best score to dynamically determine reasonable pre-aiming points. Finally, continuous target deviation is used to determine convergence and whether the system has successfully launched.

[0063] In the second stage, which is the On-Path precision tracking stage where agricultural vehicles have been successfully put into operation and are traveling on the planned path, the method of obtaining the aiming point needs to more strictly constrain the vehicle's trajectory to ensure accurate tracking of the operation path.

[0064] In the downward sequential retrieval mechanism from the online point, a path segment clustering constraint strategy is introduced: If the current vehicle is on a straight or curved path, the system only searches and selects candidate preview points within the same path cluster, without crossing into other non-adjacent work rows or discontinuous path segments. If the directional angles of three adjacent path segments are basically the same, the path segment is considered to belong to the same straight cluster as the preceding and following path segments; if two of the three path segments have basically the same directional angles and one is different, the segment is determined to be at the transition position between the straight cluster and the curved cluster; if all three directional angles are different, the path segment is considered to belong to the curved cluster.

[0065] By using this type of angle-changing clustering method, the system can divide the entire path into multiple "work row clusters" with the same continuous direction, which makes it easier for vehicles to select feasible aiming points within the work row range in complex paths.

[0066] The system uses multi-dimensional indicators such as heading deviation, lateral deviation and path curvature to construct a comprehensive objective function, scores candidate pre-aiming points, and further combines them with a cost function for fusion calculation to select the optimal path segment and its extension direction, and eliminates invalid pre-aiming points with inconsistent headings or excessive deviations.

[0067] For the selected path segment, the system dynamically adjusts the extension distance of the aiming point based on the current vehicle speed and path curvature. This ensures that the generated aiming point has sufficient foresight on straight sections and maintains higher tracking stability in sharp curves. The aiming point and the vehicle's real-time deviation information are input into the closed-loop control module of the navigation controller, dynamically fine-tuning the control commands to ensure that the agricultural vehicle remains within the set error range throughout the entire path tracking process.

[0068] The third stage is the Off-Path loopback recovery stage, mainly used for scenarios involving path closure or cyclical operations. When the waypoint index in the navigation controller reaches the end of the path, the system automatically triggers the loopback strategy and enters the path recovery process in this stage.

[0069] Select points from the starting point of the path to 1-5% of the overall path plan as the loop guide segment.

[0070] Using the vehicle's current position as a reference point, the navigation controller constructs a dynamic search radius and traverses all candidate path segments within the guidance segment, evaluating them accordingly. For each candidate segment, the system calculates its corresponding lateral deviation, heading deviation, and path curvature, thereby constructing a comprehensive objective function. This objective function, combined with a cost function, scores each candidate segment against multiple metrics, selecting the loopback path segment with the best score. Adaptive extension is then performed on the optimal segment to generate new pre-aiming points, guiding the vehicle to gradually approach the loopback starting path.

[0071] During the loopback process, the navigation controller continuously monitors the vehicle's driving status and calculates its lateral and heading deviations from the path in real time. When the vehicle's deviation is below a preset threshold for several consecutive cycles, and the vehicle's heading is consistent with the path segment direction, the system determines that the vehicle has successfully returned to the path start point and completes the "re-entry" process.

[0072] All three stages employ dynamic radius search and adaptive extension methods to find the optimal aiming point. Specific steps include:

[0073] The first step is to estimate the path curvature of the global path planning points using the three-point method. The mathematical expression for this is:

[0074]

[0075] Among them, K i Let P be the curvature of the i-th segment. i Let P be the coordinates of the i-th point. v =(x v ,y v ).

[0076] The second step involves constructing the dynamic search radius and filtering path segments. This is done based on the vehicle speed v and the path curvature K. i Adaptively set the search radius and filter all results that satisfy the starting point P. i Or the endpoint P i+1 Search for the path segment within the circle.

[0077] R = R0 + k v ·vk k ·K i Formula (2)

[0078] (xx v ) 2 +(yy v ) 2 ≤R 2 Formula (3)

[0079] Where R is the dynamic search radius, R0 is the basic search radius, representing the initial search range of the system in a static or low-speed state, and k v k is the speed adjustment coefficient. k K is the curvature suppression coefficient. i To determine the curvature of the path segments, we select path segments that meet the range specified in formula (3) to form a candidate path segment set S. al .

[0080] The third step is to calculate the projection points. For each candidate path segment set S... al Calculate the orthogonal projection point of the vehicle's position on this segment.

[0081]

[0082] in, Let P be the direction vector of the path segment. v The current location of the agricultural vehicle. P is the starting point of the path segment. i Point to the vehicle's current position P v The vector, Let t be the orthogonal projection point of the current vehicle position on the path segment. i For vectors The projection length onto the path direction vector.

[0083] If t iIf <0, the projection point becomes P. i ;like Then the projection point is at P i+1 after.

[0084] The fourth step is to determine directional consistency. Calculate the angle between the vehicle's heading vector and the direction of the path segment, and filter out path segments that do not meet the conditions.

[0085]

[0086]

[0087] Where, θ v The heading angle of the current agricultural vehicle. Let Δθ be the unit heading vector of the current agricultural vehicle. i This is the difference in angle between the vehicle's current heading and the direction of the path segment.

[0088] If |△θ i |≤θ thresh The vehicle's direction is basically consistent with the path segment, making this segment a valid candidate path segment; if |△θ i |>θ thresh If so, then the path segment will be removed.

[0089] The fifth step is the joint objective selection mechanism. An evaluation mechanism is constructed using heading deviation and lateral deviation as dual objective functions. A cost function is also constructed, incorporating lateral error, heading error, and path segment curvature as factors. The optimal path segment is selected by combining the dual objective functions and the cost function.

[0090]

[0091]

[0092]

[0093] Among them, C i The objective function score for the i-th candidate path segment. J is the lateral deviation distance between the vehicle's current position and its orthogonal projection point on the path segment. i Let i be the cost function score of the i-th candidate path segment. * α is the index number of the finally selected optimal path segment. i w represents the weight coefficients of the objective function. i λ is the weighting coefficient for the scoring item. i The weighting coefficients are used for fusion.

[0094] Step 6: Adaptively extend to obtain the aiming point. At the optimal projection point... At that point, based on the vehicle speed v and the path curvature Dynamically calculate the extension distance to obtain the optimal aiming point coordinates.

[0095]

[0096]

[0097] Where L0 is the basic extension distance, k l k is the speed adjustment coefficient. kl The curvature suppression coefficient, Let L be the curvature value of the optimal path segment, and L be the dynamic extension distance. These are the orthogonal projection points on the optimal path segment. P is the direction vector on the optimal path segment. f These are the coordinates of the final aiming point.

[0098] Although all three stages use dynamic radius search and adaptive extension methods to find the optimal aiming point, the search range and limiting conditions of the aiming point are different. The following are some limiting conditions for each stage:

[0099] In the first stage, the lateral deviation range is required to be less than 5R, and the projection point must fall within the path segment or be compensated through endpoint projection. If |△θ| is satisfied within five consecutive cycles... i If the lateral deviation is less than 0.08m and the angle is ≤25°, the online status is considered successful; in the second stage, the heading deviation is required to be |△θ|. i If the lateral deviation is less than 0.05m and the deviation exceeds the above range 8 times consecutively, it is determined to be a fault warning point, and the vehicle is stopped. The dynamic extension distance L is limited to the range of 1.2m to 4m. In the third stage, the loop path segment is limited to the interval of 1% to 5% before the starting point of the path. When restoring the path segment, if |△θ| is satisfied in eight consecutive cycles, the following conditions must be met. i If the angle is ≤15° and the lateral deviation is less than 0.05m, the loop closure is considered successful.

[0100] To illustrate this with a specific application example, a field test platform was constructed to verify the accuracy of the path prediction point acquisition method based on dynamic radius search and adaptive extension, and its effectiveness in navigation controllers. The test platform used a modified Dongfanghong tractor as the carrier, equipped with a high-precision inertial measurement unit, an RTK-GNSS positioning and direction-finding terminal, and control equipment with integrated display and control functions. The test was conducted in the Chentian Road area of ​​Yazhou District, Sanya City, Hainan Province, and global path planning was performed on the entire plot of land. Based on the tractor's low-speed, first-gear operation, in... Figure 3 Tracking tests were conducted along the indicated work path.

[0101] The overall performance of the path prediction point acquisition method proposed in this invention is evaluated by recording and analyzing the following three key indicators. In the first and third stages, the total time required for the vehicle to go from navigation initiation to successful online status is recorded as the online time, and the time required for the vehicle to travel from the waypoint index in the navigation controller to the end of the path and to successful online status is recorded as the loopback time. Path tracking state variables are defined, where: state 0 indicates the vehicle is not online, state 1 indicates the vehicle is initially online, and state 2 indicates the vehicle is fully online. A total of 10 online operation tests and loopback tests were conducted during the experiment. The state changes and corresponding times for each online process were recorded, and the average online time was calculated. The vehicle state changes and corresponding time curves are shown below. Figure 4 As shown.

[0102] The overall performance of the path prediction point acquisition method proposed in this invention in actual agricultural operations was evaluated by recording and analyzing the following three key indicators. In the first and third stages, the time required for the vehicle to successfully go online from navigation initiation, and the time required for the vehicle to return to online status from reaching the end of the path index from the navigation controller, were recorded. Path tracking state variables were set, where state 0 indicates the vehicle is not online, state 1 indicates the vehicle is initially online, and state 2 indicates the vehicle is fully online. The vehicle online distance was set to 6.4m. A total of 10 online tests and 10 loop tests were conducted during the experiment. The state change process and its corresponding time in each operation were recorded in detail, and the average value of the obtained time data was taken. The vehicle state sequence change diagram is shown below. Figure 4 As shown.

[0103] After 10 trials and averaging the results, the average time from navigation startup to successful online status was 6.8 seconds, and the average time required for the navigation controller to reach the end of the path index and re-enter online status was 9.2 seconds. Overall, the path prediction point acquisition method based on dynamic radius search and adaptive extension can quickly go online and return to the loop, which meets the requirements of agricultural vehicles.

[0104] The second phase involved testing and evaluating the vehicle's path-tracking performance under different ground conditions. Hard concrete pavement and bumpy dry land were selected as typical operating environments. The changes in lateral and directional deviations during straight-line path tracking were recorded and analyzed, and their curves are shown below. Figure 5 and Figure 6 As shown.

[0105] from Figure 5 and Figure 6As can be seen, during the straight-line tracking process, the lateral deviation of the vehicle was consistently controlled within an error range of ±0.05m, demonstrating good stability. On cement roads, the average lateral deviation was 0.24cm, with a standard deviation of 0.94cm; in bumpy dry land, the average lateral deviation was 2.91cm, with a standard deviation of 3.42cm. The experimental results show that the path pre-aiming point acquisition method proposed in this invention possesses strong adaptability and stability under different operating conditions, enabling high-precision navigation control of agricultural machinery and meeting the actual accuracy requirements of agricultural environments.

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

Claims

1. A method for obtaining path preview points based on dynamic radius search and adaptive extension, characterized in that, include: Estimate the path curvature of global path planning points; Using the vehicle's position as the center, the search radius is adaptively set according to the vehicle's speed and path curvature, and all path segments that satisfy the condition that the start and end points are within the search circle are selected to obtain a set of candidate path segments. For each candidate path segment, calculate the orthogonal projection point of the vehicle's current position on that path segment; Calculate the angle between the vehicle's heading vector and the direction of each path segment in the candidate path segment set, in order to filter out valid candidate path segments with the same direction; The optimal path segment is selected from the selected valid candidate path segments using a pre-built joint objective selection mechanism; At the orthogonal projection point of the optimal path segment, the extension distance is dynamically calculated based on the vehicle speed and the curvature of the optimal path segment to determine the coordinates of the optimal aiming point.

2. The path prediction point acquisition method based on dynamic radius search and adaptive extension according to claim 1, characterized in that, The three-point method is used to estimate the path curvature of global path planning points. The mathematical expression is as follows: Among them, K i Let P be the curvature of the i-th path segment. i Let P be the coordinates of the i-th point. v =(x v ,y v () indicates location.

3. The path preview point acquisition method based on dynamic radius search and adaptive extension according to claim 1, characterized in that, The mathematical expression for adaptively setting the search radius based on vehicle speed and path curvature is: R=R0+k v ·v-k k ·K i Where R is the dynamic search radius, R0 is the basic search radius, representing the initial search range of the system in a static or low-speed state, and k v Here, k is the speed adjustment coefficient, v is the vehicle speed, and k is the speed regulation coefficient. k K is the curvature suppression coefficient. i Let be the curvature of the i-th path segment.

4. The path prediction point acquisition method based on dynamic radius search and adaptive extension according to claim 1, characterized in that, The calculation expression for the orthogonal projection points is: in, Let P be the direction vector of the path segment. v This is the vehicle's current location. P is the starting point of the path segment. i Point to the vehicle's current position P v The vector, Let t be the orthogonal projection point of the current vehicle position on the path segment. i For vectors The projection length onto the path direction vector.

5. The path preview point acquisition method based on dynamic radius search and adaptive extension according to claim 4, characterized in that, When calculating the orthogonal projection point of the vehicle's current position on the path segment, if t i If <0, the projection point becomes P. i ;like Then the projection point is at P i+1 after.

6. The path preview point acquisition method based on dynamic radius search and adaptive extension according to claim 1, characterized in that, The mathematical expression for calculating the angle between the vehicle's heading vector and the direction of each path segment in the candidate path segment set is as follows: Where, θ v This is the current heading angle of the vehicle. Let Δθ be the unit heading vector of the current vehicle. i This is the angle difference between the vehicle's current heading and the direction of the path segment; P is the starting point of the path segment. i Point to the vehicle's current position P v ; If |△θ i |≤θ thresh The vehicle's direction is basically consistent with the path segment, making this segment a valid candidate path segment; if |△θ i |>θ thresh If so, then the path segment will be removed.

7. The path preview point acquisition method based on dynamic radius search and adaptive extension according to claim 1, characterized in that, The joint objective selection mechanism evaluates the course deviation and lateral deviation as dual objective functions, and constructs a cost function with lateral error term, course error term and path segment curvature as factors. The optimal path segment is selected by combining the dual objective function and the cost function. The mathematical expression for the joint objective selection mechanism is: Among them, C i The objective function score for the i-th candidate path segment. This is the lateral deviation distance between the vehicle's current position and its orthogonal projection point on the path segment. Let Δθ be the unit heading vector of the current vehicle. i This is the difference in angle between the vehicle's current heading and the direction of the path segment. P is the starting point of the path segment. i Point to the vehicle's current position P v The vector, J i Let i be the cost function score of the i-th candidate path segment. * α is the index number of the finally selected optimal path segment. i w represents the weight coefficients of the objective function. i λ is the weighting coefficient for the scoring item. i To integrate the weighted coefficients, K i Let be the curvature of the path segment.

8. The path preview point acquisition method based on dynamic radius search and adaptive extension according to claim 1, characterized in that, The mathematical expression for dynamically calculating the extension distance based on vehicle speed and path curvature to determine the optimal aiming point coordinates is as follows: Where L is the dynamic extension distance, L0 is the basic extension distance, and k l Here, k is the speed adjustment coefficient, v is the vehicle speed, and k is the speed regulation coefficient. kl The curvature suppression coefficient, The curvature value of the optimal path segment. These are the orthogonal projection points on the optimal path segment. P is the direction vector on the optimal path segment. f These are the coordinates of the final aiming point.

9. The path preview point acquisition method based on dynamic radius search and adaptive extension according to claim 1, characterized in that, The method is applied to three stages, each of which has a corresponding pre-aiming point selection range. The first stage is the initialization stage before the vehicle enters the path; the second stage is the operation stage of accurately tracking the planned path; and the third stage is the loop recovery stage after the path is completed. In the first stage, the search radius is gradually increased from the preset minimum search radius until a path point is detected within a certain radius. The path points within that radius are used as the center, and the path points before and after them at preset proportions are extracted as the range of the aiming point selection. In the second stage, a path segment clustering constraint strategy is introduced into the downward sequential retrieval mechanism of the online point to divide the entire path into multiple continuous job row clusters with the same direction as the range of pre-aiming points; In the third stage, the starting point of the path is selected as the predetermined proportion of the overall path planning points to form the loop guide segment, which is used as the range for selecting the aiming points.

10. The path preview point acquisition method based on dynamic radius search and adaptive extension according to claim 9, characterized in that, Each of the three stages has corresponding restrictive conditions: In the first stage, the lateral deviation is required to be less than a preset multiple of the dynamic search radius, and the projection point must fall within the path segment or be compensated through endpoint projection. If the heading deviation |△θ is satisfied within five consecutive cycles, then the lateral deviation must be within a preset multiple of the dynamic search radius. i If the angle is ≤25° and the lateral deviation is less than 0.08m, the online status is considered successful. The second stage requires a heading deviation of |△θ i If the angle is ≤15° and the lateral deviation is less than 0.05m, and the deviation exceeds the above range 8 times consecutively, it is determined to be a fault pre-aiming point, and the vehicle is stopped. The dynamic extension distance L is limited to the range of 1.2m to 4m. In the third stage, the loop path segment is limited to the interval between 1% and 5% before the path start. When restoring the path segment, if the heading deviation |Δθ is satisfied within eight consecutive cycles... i If the angle is ≤15° and the lateral deviation is less than 0.05m, the loop closure is considered successful.

Images