A Field Operation Route Switching Navigation Method and System Based on Vision and RTK Fusion
By combining vision and RTK methods, along with real-time dynamic differential positioning and cameras, the machine can rotate in place and move laterally, solving the problem of accuracy in rotation and lateral translation during row changes in field operations, thus improving row change precision and efficiency.
Patent Information
- Application Number
- CN202611489038.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-09-23
- Publication Date
- 2026-11-17
AI Technical Summary
When agricultural machinery changes lanes in the field, it is difficult to accurately judge the rotation position and lateral translation, resulting in row alignment errors and an inability to effectively connect the stationary rotation with the target operation channel.
By employing a vision-RTK fusion approach, real-time dynamic differential positioning and inertial measurement units, combined with cameras and controllers, are used to enable agricultural machinery to rotate in place and lateral crab-walk. The rotation is determined by using reverse reference rays and continuous heading changes, and lateral fine-tuning is performed by identifying the navigation centerline of the target operation channel through the camera.
It improves the accuracy and efficiency of row changing in field operations, reduces the dependence on nominal row spacing and absolute positioning accuracy, ensures accurate connection of rotation and translation processes, and reduces the dependence on continuous arc-shaped turning paths.
Smart Images

Figure CN123111291A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of automatic navigation technology for agricultural machinery, and relates to a navigation method and system that uses visual information and real-time dynamic differential positioning information to control agricultural machinery to turn around in place at the end of a row, move laterally, and enter the target operation channel. Background Technology
[0002] Sowing, fertilizing, spraying, and weeding of row-sown crops require agricultural machinery to travel along a designated work lane and turn into another work lane at the end of the row. A work lane refers to the travel path set up for the vehicle or implements, and its navigation centerline can be located between two adjacent crop rows or an offset trajectory determined according to the configuration of the implements.
[0003] Visual navigation provides the relative positional relationship between the agricultural machinery and the actual crop rows, while satellite differential positioning provides the spatial relationship between the agricultural machinery and the field boundaries. Existing four-wheeled independent steering agricultural robots are already capable of lateral translation and row changing, as documented in publications CN114987609A and US20220350344A1, which describe the switching between in-row navigation and field-edge satellite positioning steering. These technologies provide a foundation for autonomous field operations.
[0004] When using a reversing method that involves first reversing the vehicle's orientation and then moving it laterally, it is necessary to clearly define the criteria for determining whether the rotation is complete, the displacement measurement boundaries for rotation and translation, and the final alignment method for the target channel. Simply moving laterally according to a fixed working width may retain alignment errors caused by actual row spacing, seeding joints, and positioning deviations; judging the completion of rotation solely by the distance from the antenna to a straight line cannot distinguish between the initial collinear position and the collinear position after rotation.
[0005] The information disclosed in this background section is intended only to enhance the understanding of the overall background of the invention and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention
[0006] The purpose of this invention is to provide a field operation row-changing navigation method and system based on vision and RTK fusion, which clearly defines the positioning state of half a rotation in place during the row-changing process of agricultural machinery, so that the lateral translation after rotation is accurately aligned with the visual alignment of the target operation channel.
[0007] To achieve the above objectives, this invention provides a field operation lane-changing navigation method based on vision and RTK fusion, applicable to agricultural machinery with in-situ rotation and lateral crab-walking capabilities. The agricultural machinery is equipped with a camera, a real-time dynamic differential positioning module, an inertial measurement unit, and a controller. The projection of the antenna of the real-time dynamic differential positioning module onto the ground is located on the longitudinal centerline of the agricultural machinery and in front of the rotation center, with a non-zero installation distance from the rotation center. The method includes: Obtain the field boundary, vehicle size, operation configuration, and crop row spacing; determine the U-turn trigger distance based on the vehicle size; and determine the initial lateral movement distance based on the operation configuration and crop row spacing. Visual navigation along the rows is performed within the current working channel, and the visual navigation line is converted to the positioning coordinate system used by the real-time dynamic differential positioning module. When the turning trigger distance is reached, the forward movement is stopped. After the rotation space conditions are met and the vehicle orientation is aligned with the direction of the visual navigation line, the direction, initial heading, initial antenna position and rotation center position are saved. A reverse reference ray is established with the rotation center position as the starting point and the opposite direction of the saved direction. The agricultural machinery is controlled to rotate in place around the rotation center. During the rotation, the antenna position and the continuous change in heading relative to the initial heading are acquired. When the vertical distance from the antenna position to the straight line where the reverse reference ray is located is not greater than the position threshold, the projection of the antenna position along the saved direction relative to the rotation center is on the reverse side, and the rotation angle corresponding to the continuous change in heading is within the 180° angle tolerance range, the rotation is determined to be in place and the rotation is stopped. After rotating into position, switch to lateral crab mode. Use the position of the agricultural machinery after rotation into position as the starting point for lateral displacement measurement. Control the agricultural machinery to move towards the target working channel according to the lateral displacement detected by the positioning module. Stop moving after reaching the tolerance range of the initial lateral movement distance. Maintaining the lateral crab-like mode, the camera identifies the navigation centerline of the target work channel. Based on the camera calibration parameters, the lateral distance deviation of the navigation centerline relative to the agricultural machinery work reference is calculated. The lateral crab-like mode is finely adjusted using closed-loop control based on the lateral distance deviation. When the lateral distance deviation meets the row alignment threshold in a series of valid images with a preset number of consecutive frames, the straight-line direction of the wheels is restored and the machine enters the target work channel to continue visual row-following navigation.
[0008] Furthermore, the positioning coordinate system is a local horizontal rectangular coordinate system, and the visual navigation line is determined by transforming the camera's intrinsic and extrinsic parameters and the ground geometry to determine the unit direction vector u0, and is oriented according to the current operating direction; let the initial antenna position be P. A,0 The installation distance is r, and the rotation center position is P. C,0 Then P C,0 =PA,0 -r u0, the reverse reference ray is L rev ={P C,0 -λu0|λ≥0}; Let n0 denote the unit vector perpendicular to u0, and let P... A (t) represents the antenna position during rotation, wherein the rotation to the desired position simultaneously satisfies: |n0 T [P A (t)-P C,0 ]|≤ε R u0 T [P A (t)-P C,0 ]≤-r min , and ||Δψ u (t)|-π|≤ε ψ ; where ε R The position threshold is 0 < r min <r,Δψ u (t) represents the continuous heading change after deployment, ε ψ The angular tolerance is expressed in radians; the position of the rotation center during rotation is calculated using the antenna installation distance and heading, when it is relative to P. C,0 When the displacement exceeds the center drift threshold, the rotation stops and the rotation reference is re-established.
[0009] Furthermore, using the field boundary ahead corresponding to the current direction of travel as the turning trigger boundary, the position of the agricultural machinery is converted to the rotation center, and the distance from the rotation center to the turning trigger boundary is no greater than D. trigger Triggering U-turn preparation; D trigger =kL vehicle +D margin L vehicle Let D be the vehicle length including the work implements, k be the vehicle length coefficient, and D be the length of the vehicle. margin For safety margin; Before performing a stationary turn, the verification includes checking that the rotational sweep envelope of the vehicle body and the working implements, as well as the braking distance, are within the allowable U-turn area. The allowable U-turn area is located within the field boundary and excludes crop protection areas and obstacle areas. If the verification fails, the vehicle remains stationary.
[0010] Furthermore, the operation configuration includes the number T of operation unit center positions arranged at equal intervals according to crop row spacing. n and the number N of the center positions of the work units corresponding to two consecutive repetitions of the work. overlap The initial lateral movement distance is D. lateral =(T n -N overlap )R spacing , where R spacingWhere N is the row spacing for crops, 0 ≤ N overlap <T n The width of the cutter body in each work unit is used to verify the work coverage and seedling avoidance gap, and the initial lateral movement distance is calculated according to the interval of the center position of the work unit.
[0011] Furthermore, when rotating in place, the rolling direction of each wheel is tangent to its circular trajectory around the center of rotation, and the rolling direction is determined according to the position of each wheel relative to the center of rotation; when crabbing laterally, the rolling direction of each wheel is adjusted to the lateral direction relative to the longitudinal axis of the vehicle body, and the vehicle body heading is maintained after rotating to the correct position. Determine the signed lateral unit vector n based on the position of the target operation channel in the positioning coordinate system. tar The rotation center position P is obtained based on the antenna position, antenna installation offset, and heading. C (t), with the rotation arrival time t r Calculate D, starting from the measurement point. current =n tar T [P C (t)-P C (t r )], and control the lateral movement accordingly.
[0012] Furthermore, the navigation centerline of the target operation channel is identified in the following way: the acquired image is distorted and the upper far-end region and the lower near-end region are cropped, and the remaining effective region is divided into N parts along the vertical direction of the image. b The horizontal strips are divided into two parts. The horizontal detection range is determined based on the number and position of the visible crop rows in each strip. The strip area containing the target operation channel is input into the trained YOLO target detection network. The structure formed by the target operation channel and the crop rows on both sides is used as the detection target. The detection box is mapped back to the original image coordinate system. The target operation channel is used for correlation and filtering. The channel center point of the detection box in different strips is extracted and the navigation center line is fitted. The strips are numbered from the distal end to the proximal end, and their heights are in order of h. i =H eff q i-1 / S q Determined, where H eff For the effective area height, S q j ranges from 1 to N b Each item q j-1 The sum of q > 1, i = 1, ..., N b After rounding the height, adjust the height of the last strip so that each strip covers the effective area without overlapping.
[0013] Furthermore, the center point of the channel is converted into a ground point in the vehicle coordinate system through the ground projection relationship obtained by camera calibration. With the vehicle's forward direction as the X-axis and the left side of the vehicle as the Y-axis, the ground point is fitted as Y = aX + b; at the longitudinal position X of the working reference... ref At this point, calculate the lateral distance deviation e. y =aX ref +b-Y ref , where Y ref The lateral position of the work reference is given by e. ψ =arctan(a) represents the heading deviation; the lateral distance deviation is approximated by the product of the lateral pixel deviation and the corresponding distance scale coefficient only when the local scaling at the fixed detection position is valid.
[0014] Furthermore, using the lateral distance deviation as the input to a proportional-integral-derivative (PID) controller, the output is a lateral crabging speed after speed limiting, with the left side of the vehicle body as the positive direction; in each valid image, the lateral distance deviation and the heading deviation are simultaneously verified, and only when M consecutive frames simultaneously satisfy |e y |≤δ and|e ψ |≤δ ψ The process ends when M is equal to the number of stripes N. b Set separately; when the heading deviation exceeds δ ψ When the lateral fine-tuning is paused, the heading is adjusted and visual alignment is restarted when the rotation space conditions allow; when a failure is detected, the consecutive frame count is reset to zero and movement is stopped; when the positioning quality of the positioning module does not meet the preset effective conditions, the stationary rotation and positioning-based lateral movement are stopped.
[0015] This invention also provides a field operation lane-changing navigation system based on vision and RTK fusion. The system includes an agricultural machinery chassis with in-situ rotation and lateral crab-walking capabilities, a camera, a real-time dynamic differential positioning module, an inertial measurement unit, a memory, and a controller. The projection of the antenna of the real-time dynamic differential positioning module on the ground is located on the longitudinal centerline of the agricultural machinery and in front of the rotation center, with a non-zero installation distance from the rotation center. The controller is connected to the camera, the real-time dynamic differential positioning module, the inertial measurement unit, the memory, and the steering drive actuator of the agricultural machinery chassis. The memory stores a computer program, and the controller implements the above-mentioned field operation lane-changing navigation method when executing the computer program.
[0016] Compared with the prior art, the present invention has the following beneficial effects: This invention uses a combination of rotation into position, positioning and horizontal shift, and visual alignment to sequentially connect the nominal line spacing to bring the target channel into the identifiable range. The visual position of the actual channel then constrains the final lateral position, reducing the reliance on nominal line spacing and absolute positioning accuracy.
[0017] The directional constraint of the reverse reference ray and the continuous heading change together eliminate the initial collinearity; the antenna maintains a non-zero installation distance relative to the rotation center, so that the rotational motion can produce observable positional changes in the positioning coordinates.
[0018] Lateral displacement is remeasured from the end of the rotation and the vehicle body reference position calculated using the installation offset is used to avoid miscounting the arc motion of the antenna during rotation in place as line-changing lateral displacement. Visual alignment uses the already installed cameras to maintain consistency in sensor configuration.
[0019] Given sufficient space to allow U-turns, in-situ rotation and lateral translation can reduce reliance on continuous arc-shaped U-turn paths. Attached Figure Description
[0020] Figure 1 This is a flowchart illustrating the field operation lane-changing navigation method based on vision and RTK fusion of the present invention. Figure 2 This is a schematic diagram of the field operation lane-changing navigation system based on vision and RTK fusion of the present invention; Figure 3 This is a geometric schematic diagram of the antenna installation and in-situ rotation combined positioning determination of the present invention. Figure 4 This is a schematic diagram illustrating the motion of the present invention: rotating in place, translating laterally, and entering the target work channel. Figure 5 This is a schematic diagram of the navigation centerline before and after the strip detection and lateral correction of the present invention.
[0021] Explanation of reference numerals in the attached diagram: 1. Agricultural machinery chassis; 2. RTK antenna; 3. Camera; 4. Inertial measurement unit; 5. Controller; 6. Steering drive actuator; 7. Working implement; 8. Memory; 9. RTK positioning receiver. Detailed Implementation
[0022] The specific embodiments of the present invention will be described in detail below, but it should be understood that the scope of protection of the present invention is not limited to the specific embodiments.
[0023] Unless otherwise expressly stated, throughout the specification and claims, the term "comprising" or its variations such as "including" or "comprises" shall be understood to include the stated elements or components without excluding other elements or other components.
[0024] like Figure 2As shown, the system includes an agricultural machinery chassis 1, an RTK antenna 2, a camera 3, an inertial measurement unit 4, a controller 5, a steering drive actuator 6, a work implement 7, a memory 8, and an RTK positioning receiver 9. The RTK antenna 2 and the RTK positioning receiver 9 constitute a real-time dynamic differential positioning module. The controller 5 receives time-synchronized image, positioning, and attitude data, sends steering angle and drive speed commands to the steering drive actuator 6 based on the vehicle configuration and boundary data in the memory 8, and controls the start and stop of the work implement 7.
[0025] This embodiment employs a four-wheel independent steering and independent drive chassis, enabling the wheels to reach a rolling direction perpendicular to the vehicle's longitudinal axis. The zero-steering position is defined as the straight-line direction relative to the vehicle body; the steering angle required for stationary rotation is determined by the actual wheelbase, track width, and the wheel's position relative to the center of rotation. Chassis with only ordinary front-wheel steering capability are not suitable for the pure lateral crabbing steps of this embodiment.
[0026] A local rectangular coordinate system is established within the field, with east and north as the two horizontal axes. All distance calculations use metric coordinates. The vehicle coordinate system has its origin at the center of rotation, with the X-axis pointing forward and the Y-axis pointing to the left of the vehicle. The image coordinate system has the x-axis pointing to the right and the y-axis pointing downward. The heading angle ψ is the direction angle of the vehicle's X-axis in the positioning coordinate system, with counterclockwise being positive. The superscript T indicates transpose, and numbers or letters with subscripts indicate time, object, or parameter category.
[0027] RTK antenna 2 is installed along the longitudinal centerline of the vehicle body, with its ground projection distance from the rotation center being r, where r > 0. The antenna mounting point is in front of the rotation center, and the external parameters of camera 3 are calibrated together with the operating reference of the work implement 7. When there is a measurable slope on the ground or when the vehicle body experiences pitch or roll, the antenna phase center is first converted to the ground model using attitude and three-dimensional mounting arm; the following horizontal plane formula applies to the converted horizontal position.
[0028] P C (t)=P A (t)-ru(ψ(t)); u(ψ)=(cosψ, sinψ) T , Where P A (t) represents the antenna position, P C (t) represents the rotation center position. The initial heading of inertial measurement unit 4 can be calibrated using the positioning motion direction and visual channel direction during stable ridge-following travel, using continuous angle increments during rotation. Installation height, lateral residuals, and sensor time differences are included in the calibration error budget.
[0029] like Figure 1 Step S0: Read the vehicle length L including the working equipment. vehicleExternal dimensions, antenna mounting distance r, camera calibration parameters, crop row spacing R spacing The system also includes the lateral center position and entity width of the work unit. It reads the boundary polygon, the index of already completed channels, the index of the channel to be entered, and the allowed U-turn area to determine the global lateral direction of the target channel.
[0030] For T arranged at equal intervals n The center position of each work unit, where N corresponds to the repetition of two adjacent work units. overlap When the center position is specified, the nominal line break distance is calculated using the following formula: D lateral =(T n -N overlap )R spacing .
[0031] In this formula, "operating unit" refers to the center of action of the implement positioned according to row spacing, such as the center of the weeding blade located in the seedling channel; N overlap The meaning refers to the number of repetitions at the center position of the work unit, which is not the same as the number of crop rows that are processed repeatedly. Only when the center spacing is equal to R... spacing Furthermore, this repetitive method applies only when the current equipment layout is used, and only then is the above calculation formula employed. For unequal spacing configurations, the translation distance can be directly determined based on the set of already marked center positions of adjacent operations.
[0032] For example, the center lateral coordinates of the five weeding work units are -1.20 m, -0.60 m, 0, 0.60 m, and 1.20 m respectively; the width of the blades on both sides is 0.20 m, and the width of the three blades in the middle is 0.40 m. The sum of the blade body widths is 1.60 m, and the envelope width between the edges of the outermost blades is 2.60 m, neither of which is directly equal to the line break step. If the next work area repeats the center position of a side work unit, then T n =5, N overlap =1,R spacing =0.60 m, thus obtaining D lateral = 2.40 m. This interpretation preserves the four-line break spacing to avoid directly describing the five knives as five types of crops.
[0033] The target channel is determined based on the index of the unused channel. During round trip operations, the global propulsion direction can remain unchanged. After the vehicle turns around, the left and right signs of the same global direction in the vehicle coordinate system change accordingly. The controller updates the signs through coordinate transformation and does not mechanically reverse according to the previous global direction.
[0034] In step S1, camera 3 continuously acquires images of the current channel, identifies the navigation centerline between crop rows, and obtains ground navigation points through distortion correction, ground projection, and rigid body transformation from vehicle body to positioning coordinates. The local navigation direction is obtained by fitting the most recent K effective surface navigation points; K is the number of historical cache frames, defined separately from the number of stripes and consecutive confirmation frames described later.
[0035] On an approximately planar ground surface, the homography matrix H can be obtained using the camera's intrinsic and extrinsic parameters or at least four non-collinear ground calibration points. g , such that s(X, Y, 1) T =H g (x, y, 1) T , where s is the homogeneous scaling factor. The positioning coordinates are obtained by rotating the ground points along the heading and adding the vehicle reference position. This model is used only in non-planar regions when the local planar approximation meets the error requirements.
[0036] The fitted direction is normalized and oriented according to the current working direction to obtain u0. For curved crop rows, the tangential direction of the locally fitted segment at the end of the row is taken. When approaching the end of the row, the speed is reduced and the longitudinal axis of the vehicle body is aligned with u0; the rotation reference is locked only after the alignment residual falls within the preset direction error budget and the vehicle stops, to prevent the misaligned visual line from being directly used as the antenna rotation diameter.
[0037] Select the boundary segment pointing in the current direction of travel, and calculate the distance d from the center of rotation to that boundary segment. B When d B ≤D trigger At this time, preparations for turning around are underway, including: D trigger =kL vehicle +D margin .
[0038] For example, L vehicle =2.0 m, k=1.2, D margin When = 0.6 m, D trigger =3.0 m. k can be taken as 1.0 to 1.5, D margin A value of 0.5 m to 1.0 m can be used, and then adjusted according to the site and braking capacity. This threshold is used for trigger preparation and cannot replace rotational space verification.
[0039] The maximum radius of the vehicle body, wheels, and implement outlines relative to the center of rotation is taken as the sweep radius ρ, and safety clearance and braking distance are also considered. For a centrally symmetric rectangular outline, ρ = √(L env 2 +W env 2 ) / 2, where L env W envThese are the outer contour length and width, respectively; for offset machines, ρ is calculated based on the maximum distance from each outer contour vertex to the rotation center. Verify that the entire sweep circle and the expected lateral movement corridor are within the allowable U-turn area.
[0040] U-turn zones should avoid crops and obstacles that are not permitted to be rolled over. If the crop at the end of the row has not yet made room for rotation, move it to a suitable U-turn preparation position within the verified passage range, and then recheck; stop if no position is available, and do not force rotation in the crop simply because the distance threshold is met. Stop the operation of implement 7; if necessary, raise the implement and include its raised state in the sweeping verification.
[0041] like Figure 3 As shown, the initial antenna position P is locked. A,0 Rotation center P C,0 And the initial heading ψ0. The reverse reference ray is related to the direction of the visual navigation line, but its starting point is chosen at the actual center of rotation: P C,0 =P A,0 -r u0;L rev ={P C,0 -λu0|λ≥0} .
[0042] When there is a lateral residual between the original visual navigation line and the actual vehicle center, only the direction of that line is reused and the reference is translated to the actual rotation center. The initialization error should ensure that r multiplied by the sine of the direction error, along with the positioning and installation errors, still falls within the available budget of the position threshold.
[0043] In step S2, the steering drive actuator 6 causes the rolling direction of each wheel to be along the tangential direction of the motion around the rotation center. Let the position of the j-th wheel in the vehicle coordinate system be (X... j Y j If the angular velocity of rotation in place is ω, then the velocity component of the target ground is: v x,j =-ωY j ;v y,j =ωX j .
[0044] by atan2(v y,j v x,j The rolling direction is determined by √(v) x,j 2 +v y,j 2 Determine the wheel speed and use equivalent reverse drive based on the allowable angle of the steering mechanism. Only specific symmetrical wheel geometry corresponds to a 45° turning angle; a 45° rotation for all wheels is not a general requirement. The clockwise and counterclockwise directions of rotation are determined based on the sweep range, cable, and actuator constraints.
[0045] Let n0=(-u0,y u 0,x ) T Calculate the distance from the antenna to the reference line and its projection along the original direction: d line (t)=|n0 T [P A (t)-P C,0 ]| , s dir (t)=u0 T [P A (t)-P C,0 ] .
[0046] The Δψ is obtained by accumulating the heading increments of adjacent sampling times of inertial measurement unit 4 after cross-boundary processing. u (t), to avoid angle jumps when the heading changes from 179° to -179°. The joint positioning conditions are: d line ≤ε R ;s dir ≤-r min ;||Δψ u |-π|≤ε ψ .
[0047] Where, ε R It can be set to 0.02m, ε ψ It can be set as π / 180rad, that is, 1°, r min A value of r / 2 can be used. The specific threshold is determined based on positioning quality, installation accuracy, ground conditions, and operational requirements. The rotation speed is reduced when the value approaches π; the rotation is stopped and locked only when all the above conditions are met simultaneously, or it can be required to be true for several consecutive sampling periods.
[0048] Under ideal conditions, the initial position has d line =0, but s dir =r>0; during half a rotation, P A,π =P C,0 -r u0, therefore d line =0 and s dir = -r. If the course continues to rotate a full circle, the continuous change in heading will no longer fall near π, thus preventing 360° from being misjudged as 180°. Detecting only collinearity does not meet these disambiguation conditions.
[0049] P is estimated based on antenna mast arm and heading during rotation. C (t). When ||P C (t)-P C,0 || greater than ε CIf the positioning fails, the positioning conditions conflict, or the execution time exceeds the limit, the process will stop, and the search for collinear points will not be forced by extending the number of rotations. After restoring positioning and reconfirming the spatial and vehicle orientation, a new rotation reference will be established. When the antenna is located at the center of rotation, r=0, and the arc cannot be observed using this method; therefore, this installation method is not adopted.
[0050] In step S3, such as Figure 4 As shown, at the time t when the rotation is in place r Save P C (t r The rolling direction of the wheels is adjusted to be lateral relative to the longitudinal axis of the vehicle body. The so-called 90° is the final direction relative to the zero position of the vehicle body, not an additional 90° added by rotating the wheel position from the original position.
[0051] The unit horizontal vector pointing from the current channel to the target channel is defined as n. tar The vector is determined in global coordinates and then transformed into a rotated vehicle body command. The target heading ψ0±π is maintained by coordinating the turning angles and wheel speeds of each wheel; when minor heading compensation is needed, wheel speeds and turning angles are fine-tuned according to chassis kinematics.
[0052] D current (t)=n tar T [P C (t)-P C (t r )] .
[0053] v can be used coarse =clip(K c [D lateral -D current ], -v c,max v c,max Generate along n tar The speed command, where clip indicates upper and lower limit truncation, K c This is for distance control gain. The speed decreases as it approaches the target, within |D... current -D lateral |≤ε D When parking, enter the visual line of sight.
[0054] Displacement measurement excludes antenna displacement during in-situ rotation; the residual boom effect caused by heading changes is addressed through P. C Conversion processing. Reaching the nominal position by RTK lateral movement does not mean that the target channel is precisely aligned; at this point, only coarse positioning is completed.
[0055] In step S4, camera 3 captures an image facing the target channel, for example, using a 640×640 pixel image. Training and inference use the same distortion correction and cropping methods, with pixel values normalized. The upper cropping amount d is determined based on crop canopy shading, ground visibility, and the calibrated effective range. top and the amount of cutting below d bottom Effective height H eff =H-d top -d bottom And H eff >0.
[0056] The cropping amount can be pre-calibrated using actual samples and stored as parameters corresponding to the crop growth stage, or it can be updated based on the effective proportion of clear ground and crop channels counted row by row. In adaptive implementation, the usable area proportion is calculated for consecutive image rows, retaining the longest consecutive interval that reaches the threshold and constraining it to be within the near and far distance range already calibrated by the camera; if there is no interval that meets the conditions, the current frame is set to invalid.
[0057] like Figure 5 As shown, the effective region is divided into N parts from the far end to the near end. b The stripes are designed to adapt to changes in perspective scale, with smaller distal stripes and larger proximal stripes.
[0058] h i =H eff q i-1 / S q , q>1.
[0059] Where S q j ranges from 1 to N b Each item q j-1 The sum. For example, H = 640, d top =80, d bottom =80, N b =4, q=1.5, H eff =480, where the integer heights of the four stripes can be 59, 89, 133, and 199 pixels, and the sum of the four is 480. The number of stripes is set based on the effective image resolution and visible crop structure, and does not share a variable with the number of consecutive row confirmation frames.
[0060] The number of visible seedling rows R can be estimated for each band using the column projection peaks of the crop pixel mask and the cross-band continuity relationship. i Crop masking can be achieved using a normalized supergreen index of 2g-r. c -b c obtained by thresholding, where r c g, b cTo normalize the RGB components; a trained crop segmentation model is used when the crop and soil colors are similar. Prioritize preserving the target channel and its left and right boundary crop rows, expanding the lateral range when there are many rows; if the number of rows is insufficient to confirm the side boundaries, the position of the previous valid frame can be used to limit the search, but the row is not directly declared complete based on this.
[0061] The YOLO network uses "the complete structure of the target channel and the crop rows on both sides" as a detection category. Training samples are labeled with rectangular boxes within the bands according to this definition, with the label center aligned with the channel center; positive samples include different growth stages, missing seedlings, weeds, and light levels, while negative samples are from non-target channels. Images and corresponding boxes are formed into training pairs according to the same cropping, scaling, and padding rules, and then transferred to a general YOLO detector for training. Weights that meet the validation requirements are saved and inference is performed on controller 5. The network outputs candidate box coordinates, categories, and confidence scores. This method uses an existing detection network as a recognition tool and does not rely on training details from an unpublished application outside of this application.
[0062] Candidate boxes are first subjected to overlap suppression within the same strip, and then associated based on the position of the nominal target channel, the left-right alignment, and the channel identity of the previous valid frame. Different strips correspond to different vertical positions, and their normal vertical arrangement cannot be treated as duplicate boxes and deleted. Each valid strip retains at most one box belonging to the same target channel, and at least two reliable center points at different vertical positions are used for fitting; if there are multiple indistinguishable channels or the fitting residual exceeds the limit, the current frame is invalidated.
[0063] If the network input is proportional to s i Scale and fill (p) x,i p y,i The clipping start point is (x i y i If ), then the point (x′, y′) on the network box will be restored as: x=(x′-p x,i ) / s i +x i ;y=(y′-p y,i ) / s i +y i .
[0064] Project the restored channel center point onto the vehicle's ground coordinates, and fit a confidence-weighted Y = aX + b. Based on the operating reference (X) of the work implement 7... ref Y ref Definition at ) e y =aX ref +b-Y ref ;e ψ =arctan(a).
[0065] e y >0 indicates that the target navigation centerline is to the left of the work reference and should be slightly adjusted to the left; e y If the value is less than 0, make a slight adjustment to the right. When the camera is not installed directly above the working reference, the calibration offset calculation is used, and the geometric center of the image is not directly used to replace the machine's working reference.
[0066] If only a small range of alignment is performed near a fixed image row and a fixed ground distance, a locally calibrated scale c can be used. s Simplify the calculation. Let x nav x is the x-coordinate of the channel center. ref Let e be the pixel x-coordinate of the work reference corresponding to the detection position. y ≈-c s (x nav -x ref The negative sign comes from the coordinate convention of the image to the right and the vehicle body to the left. Different strip depths correspond to different scales, and an unverified fixed scale cannot be used directly for the entire image.
[0067] According to real-time updates e y Generate lateral velocity command. The discrete control period is Δt, the control period number is k, and the following is adopted: I k =clip(I k-1 +e y,k Δt, -I max I max ), v y,k =clip(K P e y,k +K I I k +K D (e y,k -e y,k-1 ) / Δt,-v max v max ) .
[0068] Where K P K I K D These are the proportional, integral, and differential gains, respectively, v y,k Taking the left side of the vehicle body as positive, the error derivative is filtered, and the integral accumulation is limited when the output saturates. Image timestamps are used to prevent old images from being repeatedly used as new valid feedback. We can initially take K. P =0.5 s -1 K I =0, K D =0, v max =0.05 m / s for low-speed tuning, and then set the integral and derivative terms according to the chassis response.
[0069] Each time the visual alignment is entered, the confirmation count is reset to zero. The target channel is unique, the detection confidence and fitting quality are satisfactory, and |e y |≤δ and |e ψ |≤δ ψ The count is incremented by one if the time is not incremented, otherwise it is reset to zero. Let δ = 0.02 m, δ... ψ =1°, M=5.
[0070] Pure lateral movement will not eliminate the heading difference between the vehicle and the target path. If |e ψ If the limit is exceeded, stop the lateral movement; within the verified ground space, level the heading by rotating slightly in place, then update the ground projection relationship and re-align. If space is insufficient, remain stopped to avoid substituting single-point centering for overall alignment.
[0071] Set the maximum cumulative fine-tuning distance D fine,max And the longest row spacing. For example, in a regular field with a row spacing of 0.60 m, D can be... fine,max Set to 0.20 m, and still limited to the verified lateral movement corridor; stop if the limit is exceeded, the target channel identity is uncertain, or the camera fails. The RTK phase can require a fixed solution and the positioning data timeliness and covariance to meet a preset threshold; stop when the valid conditions are lost; visual failure frames do not continue PID integration and position confirmation counts.
[0072] In step S5, after the continuous confirmation condition is met, the lateral movement is stopped first, then the wheels are returned to the straight-line zero position and the steering angle feedback is confirmed. The lateral fine-tuning control state is reset, and the target channel's ridge-following navigation is switched. After entering the new channel at low speed and confirming that the machine is in the correct working area, the operation of the machine 7 is resumed.
[0073] Parameterized Implementation With a corn row spacing of 0.60 m and the implements positioned at the center of the aforementioned five work units, T is set up. n =5, N overlap =1, L vehicle =2.0 m, k=1.2, D margin =0.6 m, resulting in an initial lateral movement distance of 2.40 m and a triggering distance of 3.0 m. The forward mounting distance of the antenna is set to r = 0.60 m, r... min =0.30 m; the actual rotation sweep range is calculated separately based on the 2.60 m lateral envelope of the machine and the longitudinal position of the machine.
[0074] Taking the stopped and aligned center of rotation as the local origin, let u0 = (1, 0). TThe initial antenna position is (0.60, 0) m. The ideal antenna position after rotating 180° is (-0.60, 0) m. The straight-line distance between the initial and final positions is 0, while the projections along the original direction are 0.60 m and -0.60 m, respectively. The combined direction and continuous angle conditions can distinguish between the two.
[0075] Assuming the target channel is located in the global positive Y direction, after the rotation, the forward direction of the vehicle body is the global negative X direction, and the leftward direction of the vehicle body is the global negative Y direction. Therefore, the coarse lateral movement towards the target channel should be initiated by issuing a rightward command in the vehicle body coordinate system. Save the center coordinates after the rotation ends and move 2.40 m to stop once the coarse positioning tolerance is reached.
[0076] If visual observation indicates that the target channel is located 0.08 m to the left of the vehicle body relative to the working reference, then e y = +0.08m. The controller outputs a positive lateral fine-tuning speed, continuously updating the image, and gradually decelerating as the error decreases; it is not a one-time open-loop command to move 0.08 m. Alignment ends after five valid frames simultaneously satisfying the distance and heading thresholds.
[0077] System operation and program storage Controller 5 can be a combination of an onboard industrial computer and a chassis control unit. The onboard industrial computer performs image processing, coordinate transformation, and status management, while the chassis control unit controls each wheel according to steering angle and speed commands. The two exchange commands and feedback via a bus. Memory 8 stores sensor calibration parameters, field boundaries, operation configuration, detection network weights, and status control programs.
[0078] The program manages the execution sequence using states such as row alignment, turn preparation, in-situ rotation, coarse lateral movement, visual row alignment, row entry recovery, and stop waiting. It only enters the next state when the current state is in place and the valid conditions are met. The computer-readable storage medium can be flash memory, solid-state memory, or other non-transient media capable of storing programs. When the program is executed by the controller 5, it implements the above method.
[0079] These descriptions are not intended to limit the invention to the precise forms disclosed, and it will be apparent that many changes and variations can be made in accordance with the foregoing teachings. The exemplary embodiments were chosen and described in order to explain the specific principles of the invention and its practical application, thereby enabling those skilled in the art to implement and utilize various different exemplary embodiments of the invention, as well as various different choices and modifications. The scope of the invention is intended to be defined by the claims and their equivalents.
Claims
1. A field operation lane-changing navigation method based on vision and RTK fusion, applied to agricultural machinery with in-situ rotation and lateral crab-walking capabilities, wherein the agricultural machinery is equipped with a camera, a real-time dynamic differential positioning module, an inertial measurement unit, and a controller, wherein the projection of the antenna of the real-time dynamic differential positioning module on the ground is located on the longitudinal centerline of the agricultural machinery and in front of the rotation center, and has a non-zero installation distance from the rotation center, characterized in that, include: Obtain the field boundary, vehicle size, operation configuration, and crop row spacing; determine the U-turn trigger distance based on the vehicle size; and determine the initial lateral movement distance based on the operation configuration and crop row spacing. Visual navigation along the rows is performed within the current working channel, and the visual navigation line is converted to the positioning coordinate system used by the real-time dynamic differential positioning module. When the turning trigger distance is reached, the forward movement is stopped. After the rotation space conditions are met and the vehicle orientation is aligned with the direction of the visual navigation line, the direction, initial heading, initial antenna position and rotation center position are saved. A reverse reference ray is established with the rotation center position as the starting point and the opposite direction of the saved direction. The agricultural machinery is controlled to rotate in place around the rotation center. During the rotation, the antenna position and the continuous change in heading relative to the initial heading are acquired. When the vertical distance from the antenna position to the straight line where the reverse reference ray is located is not greater than the position threshold, the projection of the antenna position along the saved direction relative to the rotation center is on the reverse side, and the rotation angle corresponding to the continuous change in heading is within the 180° angle tolerance range, the rotation is determined to be in place and the rotation is stopped. After rotating into position, switch to lateral crab mode. Use the position of the agricultural machinery after rotation into position as the starting point for lateral displacement measurement. Control the agricultural machinery to move towards the target working channel according to the lateral displacement detected by the positioning module. Stop moving after reaching the tolerance range of the initial lateral movement distance. Maintaining the lateral crab-like movement mode, the camera is used to identify the navigation centerline of the target operation channel. The lateral distance deviation of the navigation centerline relative to the agricultural machinery operation reference is calculated based on the camera calibration parameters. The lateral crab-like movement is then finely adjusted using closed-loop control based on the lateral distance deviation. When the lateral distance deviation in a series of valid images within a preset number of frames meets the row threshold, the straight-line direction of the wheels is restored and the vehicle enters the target work channel to continue visual row navigation.
2. The field operation lane-changing navigation method based on vision and RTK fusion according to claim 1, characterized in that: The positioning coordinate system is a local horizontal rectangular coordinate system. The visual navigation line is determined by transforming the camera's intrinsic and extrinsic parameters and the ground geometry to determine the unit direction vector u0, and is oriented according to the current operating direction. Let the initial antenna position be P. A,0 The installation distance is r, and the rotation center position is P. C,0 Then P C,0 =P A,0 -r u0, the reverse reference ray is L rev ={P C,0 -λu0|λ≥0}; Let n0 denote the unit vector perpendicular to u0, and let P... A (t) represents the antenna position during rotation, wherein the rotation to the desired position simultaneously satisfies: |n0 T [P A (t)-P C,0 ]|≤ε R u0 T [P A (t)-P C,0 ]≤-r min , and ||Δψ u (t)|-π|≤ε ψ ; where ε R The position threshold is 0 < r min <r,Δψ u (t) represents the continuous heading change after deployment, ε ψ The angular tolerance is expressed in radians; the position of the rotation center during rotation is calculated using the antenna installation distance and heading, when it is relative to P. C,0 When the displacement exceeds the center drift threshold, the rotation stops and the rotation reference is re-established.
3. The field operation lane-changing navigation method based on vision and RTK fusion according to claim 1, characterized in that: Using the field boundary ahead along the current direction of travel as the turning trigger boundary, the position of the agricultural machinery is converted to the rotation center, and the distance from the rotation center to the turning trigger boundary is no greater than D. trigger Triggering U-turn preparation; D trigger =kL vehicle +D margin L vehicle Let D be the vehicle length including the work implements, k be the vehicle length coefficient, and D be the length of the vehicle. margin For safety margin; Before performing a stationary turn, the verification includes checking that the rotational sweep envelope of the vehicle body and the working implements, as well as the braking distance, are within the allowable U-turn area, which is located within the field boundary and excludes crop protection areas and obstacle areas. The system remains in a stopped state when the verification fails.
4. The field operation lane-changing navigation method based on vision and RTK fusion according to claim 1, characterized in that: The job configuration includes the number T of job unit center positions arranged at equal intervals according to crop row spacing. n and the number N of the center positions of the work units corresponding to two consecutive repetitions of the work. overlap The initial lateral movement distance is D. lateral =(T n -N overlap )R spacing , where R spacing Where N is the row spacing for crops, 0 ≤ N overlap <T n The width of the cutter body in each work unit is used to verify the work coverage and seedling avoidance gap, and the initial lateral movement distance is calculated according to the interval of the center position of the work unit.
5. The field operation lane-changing navigation method based on vision and RTK fusion according to claim 1, characterized in that: When rotating in place, the rolling direction of each wheel is tangent to its circular trajectory around the center of rotation. The rolling direction is determined according to the position of each wheel relative to the center of rotation. When crabbing laterally, the rolling direction of each wheel is adjusted to the lateral direction relative to the longitudinal axis of the vehicle body, and the vehicle body's heading is maintained after rotating to the correct position. Determine the signed lateral unit vector n based on the position of the target operation channel in the positioning coordinate system. tar The rotation center position P is obtained based on the antenna position, antenna installation offset, and heading. C (t), with the rotation arrival time t r Calculate D, starting from the measurement point. current =n tar T [P C (t)-P C (t r )], and control the lateral movement accordingly.
6. The field operation lane-changing navigation method based on vision and RTK fusion according to claim 1, characterized in that: The navigation centerline of the target operation channel is identified as follows: the acquired image is distorted and the upper far-end region and the lower near-end region are cropped, and the remaining effective region is divided into N parts along the vertical direction of the image. b The horizontal strips are divided into two parts. The horizontal detection range is determined based on the number and position of the visible crop rows in each strip. The strip area containing the target operation channel is input into the trained YOLO target detection network. The structure formed by the target operation channel and the crop rows on both sides is used as the detection target. The detection box is mapped back to the original image coordinate system. The target operation channel is used for correlation and filtering. The channel center point of the detection box in different strips is extracted and the navigation center line is fitted. The strips are numbered from the distal end to the proximal end, and their heights are in order of h. i =H eff q i-1 / S q Determined, where H eff For the effective area height, S q j ranges from 1 to N b Each item q j-1 The sum of q > 1, i = 1, ..., N b After rounding the height, adjust the height of the last strip so that each strip covers the effective area without overlapping.
7. The field operation lane-changing navigation method based on vision and RTK fusion according to claim 6, characterized in that: The center point of the passage is converted into a ground point in the vehicle coordinate system through the ground projection relationship obtained by camera calibration. With the vehicle's forward direction as the X-axis and the left side of the vehicle as the Y-axis, the ground point is fitted as Y = aX + b; at the longitudinal position X of the working reference... ref At this point, calculate the lateral distance deviation e. y =aX ref +b-Y ref , where Y ref The lateral position of the work reference is given by e. ψ =arctan(a) represents the heading deviation; the lateral distance deviation is approximated by the product of the lateral pixel deviation and the corresponding distance scale coefficient only when the local scaling at the fixed detection position is valid.
8. The field operation lane-changing navigation method based on vision and RTK fusion according to claim 7, characterized in that: Using the lateral distance deviation as the input to a proportional-integral-derivative (PID) controller, the output is a lateral crabging speed after speed limiting, with the left side of the vehicle body as the positive direction; in each valid image, the lateral distance deviation and the heading deviation are simultaneously verified, and only when M consecutive frames simultaneously satisfy |e y |≤δ and|e ψ |≤δ ψ The process ends when M is equal to the number of stripes N. b Set separately; when the heading deviation exceeds δ ψ Pause laterally for fine-tuning, adjust the course and re-align visually when rotational space allows; When a failure is detected, the consecutive frame count is reset to zero and movement is stopped. When the positioning quality of the positioning module does not meet the preset effective conditions, the stationary rotation and positioning-based lateral movement are stopped.
9. A field operation lane-changing navigation system based on vision and RTK fusion, characterized in that, The system includes an agricultural machinery chassis with the ability to rotate in place and lateral crab-like maneuvers, a camera, a real-time dynamic differential positioning module, an inertial measurement unit, a memory, and a controller. The projection of the antenna of the real-time dynamic differential positioning module on the ground is located on the longitudinal centerline of the agricultural machinery and in front of the rotation center, with a non-zero installation distance from the rotation center. The controller is connected to the camera, the real-time dynamic differential positioning module, the inertial measurement unit, the memory, and the steering drive actuator of the agricultural machinery chassis. The memory stores a computer program, and when the controller executes the computer program, it implements the field operation lane-changing navigation method according to any one of claims 1 to 8.
Citation Information
Patent Citations
Four-wheel independent steering paddy field agricultural robot and navigation method thereof
CN114987609A
Steering Method for an Agricultural Machine
US20220350344A1
