Search

CN-122015856-A - Equidistant curve path generation and smoothing method thereof

CN122015856ACN 122015856 ACN122015856 ACN 122015856ACN-122015856-A

Abstract

The invention relates to the field of automatic control, and discloses an equidistant curve path generation and smoothing method thereof, which are used for solving the problems that in the prior art, selfing detection and loop cutting are complex in calculation and poor in instantaneity, an agricultural machine is unstable due to sharp points and curvature mutation of a generated path, and the accuracy, the calculation efficiency and the hardware cost of the path are difficult to consider. The method comprises the steps of carrying out normal deviation according to normal vectors of base curves and set deviation distances, preliminarily generating equidistant curve point sets, utilizing a two-dimensional grid space index technology to rapidly carry out local selfing detection, generating a non-selfing path, eliminating sharp corners and curvature mutation in the path by adopting a bidirectional iterative smoothing algorithm based on actual minimum turning radius constraint of an agricultural machine, and carrying out fitting and interpolation densification on the path. The invention can effectively generate smooth continuous, no selfing and everywhere second-order guided high-quality operation paths in real time, and remarkably improves the stability and operation precision of agricultural machine path tracking.

Inventors

  • Shen Jianzhu
  • REN XUDONG
  • WANG QINGQUAN

Assignees

  • 常州楠瓜星智能科技有限公司

Dates

Publication Date
20260512
Application Date
20260206

Claims (8)

  1. 1. A method for equidistant curve path generation and smoothing thereof, comprising: Performing normal offset according to the normal vector of each point on the base curve C 0 and the offset distance d set by a user to generate an equidistant curve point set C 1 ; Establishing a two-dimensional grid index according to the coordinate range of the equidistant curve point set C 1 , dividing the plane of the curve into uniform grids, and processing to obtain a cut selfless curve point set C 2 ; Based on the practical minimum turning radius constraint of the agricultural machinery, carrying out bidirectional smoothing treatment on the selfless curve point set C 2 , eliminating curvature abrupt points and sharp corners existing in a path, and obtaining a smooth continuous curve point set C 3 ; And constructing a 3-order 2-time quasi-uniform B spline curve by taking the smooth continuous curve point set C 3 as a control point, and carrying out interpolation densification on the curve point set to generate a curve point set C 4 for path tracking.
  2. 2. The equidistant curve path generation and smoothing method as claimed in claim 1, comprising: The calculation strategy calculates the normal vector of each point on the base curve C 0 by using the vector P 0 P 1 to calculate the vector (P 0.y -P 1.y ,P 1.x -P 0.x ) for the first point P 0 of the curve; For the middle point P i , the vector calculation method vector formed by adopting the front and rear points P i-1 and P i+1 is (P i-1.y -P i+1.y ,P i+1.x -P i-1.x ), wherein i is from 1 to n-2; For the last point P n , vector P n-1 P n is used to calculate the vector (P n-1.y -P n.y ,P n.x -P n-1.x ); And translating the coordinates of each point along the calculated unit normal vector direction according to the offset distance d specified by the user to generate an equidistant curve point set C 1 , wherein the offset point coordinates=the original point coordinates+the unit normal vector x d.
  3. 3. The equidistant curve path generation and smoothing method as claimed in claim 2, comprising: Calculating the coordinate range of the equidistant curve point set C 1 , finding x min 、x max 、y min 、y max , dividing the whole boundary frame into uniform grids of 64 x 64 according to the coordinate range, establishing a spatial index for each line segment, and distributing the line segment into the grids where the starting point is located; For each line segment L i formed by P i to P i+1 , determining the grid coordinates of the line segment L i , and detecting only whether the line segment L i intersects other line segments in the grid and other line segments in adjacent grids of the grid; Judging line segment intersection by using a cross-over implementation algorithm, calculating four cross product values d 1 、d 2 、d 3 、d 4 , judging that two line segments intersect if d 1 ×d 2 is less than 0 and d 3 ×d 4 is less than 0, and calculating accurate intersection point coordinates by using a parameter equation method; And dividing the path into three sections of P 0 to P i , an intersection point and P j+1 to P n at the precise intersection point coordinates, reconstructing the path to be P 0 ...P i , the intersection point and P j+1 ...P n , deleting the intermediate selfing loops P i+1 to P j , and obtaining a cut selfless curve point set C 2 .
  4. 4. The equidistant curve path generating and smoothing method as set forth in claim 3, wherein the step of performing line segment intersection judgment by using a cross-over implementation algorithm, calculating four cross product values d 1 、d 2 、d 3 、d 4 comprises: The end point of the current line segment Li is marked as P 1 、P 2 , and the end point of the line segment to be judged is marked as P 3 、P 4 ; d 1 =(P 4 .x-P 3 .x)×(P 1 .y-P 3 .y)-(P 4 .y-P 3 .y)×(P 1 .x-P 3 .x); d 2 =(P 4 .x-P 3 .x)×(P 2 .y-P 3 .y)-(P 4 .y-P 3 .y)×(P 2 .x-P 3 .x); d 3 =(P 2 .x-P 1 .x)×(P 3 .y-P 1 .y)-(P 2 .y-P 1 .y)×(P 3 .x-P 1 .x); d 4 =(P 2 .x-P 1 .x)×(P 4 .y-P 1 .y)-(P 2 .y-P 1 .y)×(P 4 .x-P 1 .x).
  5. 5. The equidistant curve path generation and smoothing method as set forth in claim 3, wherein the calculating the exact intersection point coordinates using a parametric equation method comprises: Calculating denominator term denom=(P 4 .y-P 3 .y)×(P 2 .x-P 1 .x)-(P 4 .x-P 3 .x)×(P 2 .y-P 1 .y); Calculating parameters u=[(P 4 .x-P 3 .x)×(P 1 .y-P 3 .y)-(P 4 .y-P 3 .y)×(P 1 .x-P 3 .x)]÷denom; Calculate intersection coordinates = P 1 +u×(P 2 -P 1 ).
  6. 6. The equidistant curve path generating and smoothing method as set forth in claim 3, comprising: For the combination of each continuous 3 points P i-1 、P i 、P i+1 in the no-selfing curve point set C 2 , vectors V 1 =P i -P i-1 and V 2 =P i +1-P i are calculated, the included angle α= arccos [ (V 1 ·V 2 )/(|V 1 |×|V 2 |) ] of vector V 1 、V 2 is calculated, and the modulo length l= |v 2 | of vector V 2 is calculated; Calculating a turning radius R required by the vehicle to advance a distance L and simultaneously turn an angle alpha according to a formula R=L/sin (alpha) 0.5, and if the turning radius R is smaller than a minimum turning radius R limit of the vehicle, judging a point P i to be a sharp point; For the determined sharp point P i , calculating the midpoint M of P i-1 and P i+1 by adopting an interpolation adjustment strategy, and moving P i to the direction M by a certain proportion; And performing forward smoothing and reverse smoothing on the non-selfing curve point set C 2 , wherein the forward smoothing is to perform smoothing treatment on the detected sharp points by traversing from the starting point to the end point, the reverse smoothing is to perform secondary smoothing from the end point to the starting point, and a plurality of forward and reverse alternate iterations are performed until the loop is terminated when no sharp point is found in a single iteration, so as to obtain a smooth continuous curve point set C 3 .
  7. 7. The equidistant curve path generating and smoothing method as claimed in claim 6, comprising: Generating a node vector by adopting a quasi-uniform node vector generation strategy: The first 4 nodes are set to 0, i.e., U 0 =U 1 =U 2 =U 3 =0; The last 4 nodes are set to 1, i.e., U n =U n+1 =U n+2 =U n+3 =1; The intermediate nodes are uniformly distributed, and the calculation formula is U i = i/(n+3), wherein i is 4 to n-1, and n is the number of control points of the smooth continuous curve point set C 3 ; positioning a node interval where the sampling parameter t is positioned by adopting a binary search algorithm: Performing binary search in the interval [3, n ] to find a node interval index span meeting U m ≤t<U m+1 ; Calculating a third-order B spline basis function by adopting a recursive algorithm, and calculating 4 non-zero basis function values N0 to N3 according to the node interval index span and the sampling parameter t; The dense curve point coordinates are calculated according to formula P out (t i )=ΣN j ×P (span-3+j) , where j=0 to 3, P is the control point coordinates, generating a set of curve points C 4 for path tracking.
  8. 8. The equidistant curve path generating and smoothing method as set forth in claim 7, wherein the calculating a third-order B-spline basis function by a recursive algorithm, calculating 4 non-zero basis function values N [0] to N [3] according to the node interval index span and the sampling parameter t, comprises: Initializing 0-order basis function N0 to 1.0; recursively calculating 1-order to 3-order basis functions from 1 to 3 by a loop variable j; in each cycle, calculating a left term coefficient left [ j-1] = t-U [ span+1-j ] and a right term coefficient right [ j-1] = U [ span+j ] -t, wherein U is the node vector; The base function value of the current order is calculated from 0 to j-1 by the inner layer circulation variable r, the denominator denom =right [ r ] +left [ j-1-r ] is calculated, if | denom | is larger than 1e-10, the intermediate variable temp=Nr/denom is calculated, the base function value Nr is updated and the intermediate value safe is saved, and the last base function value Nj is set after the circulation is finished.

Description

Equidistant curve path generation and smoothing method thereof Technical Field The invention relates to the field of automatic control, in particular to an equidistant curve path generation and smoothing method thereof. Background With the deep development of precise agriculture, an automatic/auxiliary driving system of an agricultural machine has become core equipment for improving the working efficiency and quality. The system acquires the state and position information of the vehicle in real time through integrating sensors such as GNSS positioning and Inertial Measurement Unit (IMU), and further controls the steering and traveling mechanism, so that the pesticide can automatically travel along a preset target track. The cultivated land in China is complex and various, and a large number of irregular fields exist. In such fields, if the agricultural machinery auxiliary driving system continues to adopt the traditional linear operation mode, the agricultural machinery needs to turn around frequently at the ground, so that the operation efficiency is remarkably reduced, compaction is caused by repeated compaction of local soil, and the normal growth of crops is affected. Therefore, the development of a curve operation mode capable of being attached to the boundary of an irregular field has important significance for improving the land utilization rate, the operation efficiency and protecting the soil structure. At present, the generation of a curve operation path for agricultural machinery auxiliary driving is mainly divided into a translation curve and an equidistant curve. The translation curve is simple to realize, but the translation curve is rigid in nature, so that the operation width is difficult to be kept consistent on a curvature change path, and the operation is easy to miss or re-work, so that the overall operation quality is influenced. In contrast, by equidistant offset along the normal direction of the curve, the equidistant curve (also called offset curve) can fundamentally ensure that the distance between the offset path and the reference path at each point is consistent, thereby effectively maintaining uniform operation breadth and being regarded as a better technical path. However, the existing equidistant curve generation method still has some prominent defects in the practical application of automatic driving of the agricultural machinery, and restricts the popularization effect: (1) The selfing process is complex, and the real-time performance is insufficient, namely, when the curvature radius of the reference curve is smaller than or equal to the offset distance, the generated equidistant curve can generate the selfing phenomenon, so that a loop is formed. The existing algorithm needs to execute complex selfing detection and loop cutting operation, has heavy calculation load, and is difficult to meet the requirement of path planning on real-time performance in complex field environment. (2) The path smoothness is poor, so that the control stability is affected, namely, the cut equidistant curves often remain sharp points or cause curvature mutation, so that the path is not smooth. When the agricultural machinery runs along the path, the steering mechanism needs to be frequently adjusted, so that running stability and operation precision are affected, and tool wear is possibly increased. (3) The precision, efficiency and cost are difficult to balance, in order to improve the calculation efficiency, the existing method usually sacrifices the path precision and adopts approximate processing, thereby introducing accumulated errors and curve deformation and reducing the operation quality. If high precision and high efficiency are required to be ensured at the same time, a high-performance processor is required to be relied on, so that the hardware cost is obviously increased, and the large-scale application and popularization are not facilitated. The existing agricultural machinery equidistant curve operation path generation technology mainly has the following problems: CN202310369103.4 generates a turn-around path through a straight-line working path, but does not solve the problem of equidistant offset in a curved working scenario. Although the universality of irregular fields is improved by the CN115617029B, curve smoothing is not involved, and severe rotation angles are easy to occur during steering control of the agricultural machinery, so that the operation stability is affected. We therefore propose an equidistant curve path generation and its smoothing method to solve the above problems. Disclosure of Invention The invention provides an equidistant curve path generation and smoothing method thereof, which are used for solving the problems that in the prior art, selfing detection and loop cutting are complex in calculation and poor in real-time performance, the generated path has sharp points and curvature mutation, so that the operation of an agricultural machine is unstable, and