Vehicle relative pose sensing and point cloud processing method and related equipment

Abstract

The invention discloses a vehicle relative pose sensing and point cloud processing method and related equipment, and relates to the technical field of agricultural machine data processing.The method comprises the steps that the included angle, measured by an encoder, between a spraying barrel and a vehicle body of a silage maize harvester is obtained in real time; the relative advancing direction of the grass receiving vehicle is judged based on the included angle obtained in real time; according to the relative advancing direction of the grass receiving vehicle, performing area screening on the three-dimensional point cloud data under the coordinate system used by the silage maize harvester to obtain local three-dimensional point cloud data corresponding to the relative advancing direction of the grass receiving vehicle; according to the included angle obtained in real time, calculating a rotation matrix from the coordinate system used by the silage maize harvester to the coordinate system used by the grass receiving vehicle, and converting the local three-dimensional point cloud data into the coordinate system used by the grass receiving vehicle based on the rotation matrix; and based on the transformed local point cloud data, the material throwing operation of the spraying barrel is controlled. According to the method, the accuracy and reliability of automatic material throwing operation can be improved with higher stability and lower calculation expenditure.

Description

Technical Field

[0001] This invention relates to the field of agricultural machinery data processing technology, and in particular to a method and related equipment for vehicle relative pose perception and point cloud processing. Background Technology

[0002] As a modern agricultural piece of equipment, the silage harvester is specifically designed for the efficient harvesting of silage from crops such as corn. Its core value lies in maximizing the retention of nutrients in the feed through mechanical cutting and sealed fermentation processes. During automated operation, the silage harvester must deliver the crushed forage into the receiving truck's hopper in real time, ensuring no spillage. Achieving this automation requires solving two key problems: first, accurately determining the real-time direction of the receiving truck relative to the silage harvester; and second, accurately converting the 3D point cloud data collected by the silage harvester's vision sensors into a coordinate system aligned with the direction of the receiving truck. Only by solving these two problems can forage be prevented from spraying onto the outside of the hopper or onto the truck head, achieving precise distribution.

[0003] To address the aforementioned issues, existing technologies offer corresponding solutions. For determining the vehicle's direction of travel, a common method relies on a global navigation satellite system to obtain the vehicle's absolute position coordinates, combined with real-time measurements of triaxial acceleration and angular velocity by an inertial measurement unit (IMU), to calculate the vehicle's instantaneous heading angle. For coordinate system transformation, a common method is to determine the orientation of the hay truck body based on 3D point cloud data using plane fitting techniques. Specifically, this involves first identifying the plane representing the side of the hay truck body from the point cloud, then calculating the angle between the plane's normal vector and the coordinate axes, using this as a reference for coordinate system rotation and alignment.

[0004] However, the aforementioned existing technical solutions have significant drawbacks. Regarding coordinate system transformation, mainstream methods relying on point clouds for plane fitting, such as least squares, principal component analysis, and random sampling consensus algorithms, are highly sensitive to noise and outliers in the point cloud. In complex farmland environments, point cloud quality is easily affected by interference, leading to errors in the fitted plane orientation and consequently causing significant deviations in subsequent coordinate system transformations. In determining vehicle direction of travel, the combination of a global navigation satellite system (GNSS) and an inertial measurement unit (IMU) is highly susceptible to failure in scenarios where satellite signals are blocked, such as in mountainous or forested areas, resulting in positioning interruptions or drift. Furthermore, the inherent temperature sensitivity and zero-bias instability of the microelectromechanical inertial measurement unit (MEMS) cause heading angle errors to accumulate over time; for example, after one minute of satellite signal loss, the heading angle error may exceed ten degrees.

[0005] In summary, existing technical solutions struggle to reliably and stably address the issues of vehicle relative orientation determination and high-precision coordinate system transformation in the complex environments of actual agricultural machinery operations, thus hindering the accuracy and reliability of automated unloading technology. Therefore, a new technical solution is urgently needed to overcome these shortcomings. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to address the shortcomings of the prior art, and specifically provides a method and related equipment for vehicle relative pose perception and point cloud processing, as detailed below: 1) In a first aspect, the present invention provides a method for vehicle relative pose perception and point cloud processing, the specific technical solution of which is as follows: The angle between the spray nozzle and the forage harvester body, measured by the encoder, is obtained in real time. The relative direction of travel of the hay truck is determined based on the angle obtained in real time; Based on the relative travel direction of the hay receiving vehicle, the three-dimensional point cloud data in the coordinate system used by the silage harvester is filtered by region to obtain the local three-dimensional point cloud data corresponding to the relative travel direction of the hay receiving vehicle. Based on the angle acquired in real time, the rotation matrix from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle is calculated, and the local three-dimensional point cloud data is transformed into the coordinate system used by the hay receiving vehicle based on the rotation matrix. The material throwing operation of the nozzle is controlled based on the transformed local point cloud data.

[0007] The beneficial effects of the vehicle relative pose perception and point cloud processing method provided by this invention are as follows: First, the relative direction of travel of the hay receiving vehicle is determined by acquiring the angle between the spray nozzle and the silage harvester body directly measured by the encoder in real time, completely eliminating the reliance on the Global Navigation Satellite System (GNSS) and inertial measurement units (INS). This method avoids the problem of direction determination failure in signal-blocked environments and eliminates the inherent defect of accumulated heading angle errors in INS, making direction perception more stable and reliable. Second, based on the real-time determined direction of travel, the 3D point cloud data in the coordinate system used by the silage harvester is intelligently filtered to generate local 3D point cloud data containing only key areas. This step significantly reduces the amount of data that subsequent algorithms need to process, effectively improving the overall computational efficiency and real-time performance of the system. Third, the rotation matrix is ​​calculated directly based on the high-precision angle measured by the encoder, completing the transformation from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle. This method abandons the traditional transformation method that relies on point cloud fitting to a plane, thus completely overcoming the transformation error caused by point cloud noise interference, and laying a precise coordinate foundation for subsequent accurate hopper feature recognition and material throwing control based on the transformed local point cloud data. Ultimately, the entire technical solution achieved improved accuracy and reliability in automated material handling operations with higher stability and lower computational overhead.

[0008] Based on the above scheme, the vehicle relative pose perception and point cloud processing method of the present invention can be further improved as follows.

[0009] Furthermore, the relative travel direction of the hay receiving vehicle is determined based on the angle obtained in real time, including: when the real-time angle is less than a first preset angle threshold, it is determined that the hay receiving vehicle is traveling to the left relative to the silage machine; when the real-time angle is greater than a second preset angle threshold, it is determined that the hay receiving vehicle is traveling to the right relative to the silage machine; when the real-time angle is between the first preset angle threshold and the second preset angle threshold, it is determined that the hay receiving vehicle is located directly behind the silage machine.

[0010] The advantages of adopting the above-mentioned further scheme are: simplifying direction determination to a comparison operation with a fixed threshold, resulting in rapid and highly deterministic processing. The established intermediate interval effectively defines the stable state of the hay receiving vehicle located directly behind the silage harvester, avoiding frequent changes in direction decisions caused by minor fluctuations in the real-time angle near the theoretical critical value. The clear output of three states—left-side driving, right-side driving, and directly behind—provides a direct and reliable logical basis for the intelligent partitioning and filtering of subsequent 3D point cloud data, ensuring the stable triggering and efficient execution of subsequent processing flows.

[0011] Furthermore, based on the transformed local point cloud data, the material throwing operation of the nozzle is controlled, including: Based on the transformed local point cloud data, the target features of the hay collection vehicle are identified. Based on the target characteristics of the hay receiving vehicle, control the material throwing operation of the spray nozzle.

[0012] The beneficial effects of adopting the above-mentioned further scheme are as follows: by identifying the target features of the hay receiving vehicle in the coordinate system used by the vehicle, direct and accurate input is provided for control decisions. Controlling the material throwing operation of the spray nozzles based on these identified target features ensures that the direction of the spray nozzles strictly corresponds to the actual filling state and spatial position within the vehicle hopper, thereby achieving precision in the material throwing action. This step ensures the integrity and consistency of the entire system from environmental perception to operation execution, enabling the automated material throwing operation to operate reliably and efficiently in a closed loop.

[0013] Furthermore, the encoder is fixedly installed on the side of the turntable at the bottom of the spray nozzle, and the top gear of the encoder meshes with the turntable gear of the spray nozzle and is engaged by a spring clamping mechanism.

[0014] The advantages of adopting the above-mentioned further solution are as follows: the encoder is fixedly installed on the side of the turntable at the bottom of the spray nozzle, enabling the acquisition of the spray nozzle's rotation angle without intermediaries. The top gear of the encoder meshes with the turntable gear of the spray nozzle, achieving precise transmission of rotational motion. The application of the spring clamping mechanism effectively eliminates any transmission backlash that may exist in the gear meshing, preventing signal jumps or loss caused by backlash. This installation structure together ensures that the angle between the spray nozzle and the forage harvester body is continuously, stably, and accurately measured, providing a solid and reliable hardware foundation for subsequent direction determination and coordinate transformation.

[0015] 2) In a second aspect, the present invention also provides a vehicle relative pose perception and point cloud processing system, the specific technical solution of which is as follows: It includes an angle acquisition module, a relative travel direction determination module, a point cloud filtering module, a transformation module, and a control module; The angle acquisition module is used to: acquire in real time the angle between the spray nozzle and the forage harvester body measured by the encoder; The relative travel direction determination module is used to: determine the relative travel direction of the hay truck based on the included angle obtained in real time; The point cloud filtering module is used to: filter the three-dimensional point cloud data in the coordinate system used by the silage harvester according to the relative travel direction of the hay truck, and obtain the local three-dimensional point cloud data corresponding to the relative travel direction of the hay truck. The transformation module is used to: calculate the rotation matrix from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle based on the real-time acquired angle, and transform the local three-dimensional point cloud data to the coordinate system used by the hay receiving vehicle based on the rotation matrix; The control module is used to control the material throwing operation of the nozzle based on the transformed local point cloud data.

[0016] Based on the above solution, the vehicle relative pose perception and point cloud processing system of the present invention can be further improved as follows.

[0017] Furthermore, the relative travel direction determination module is specifically used to: determine that the hay receiving vehicle is traveling to the left relative to the silage machine when the real-time angle is less than the first preset angle threshold; determine that the hay receiving vehicle is traveling to the right relative to the silage machine when the real-time angle is greater than the second preset angle threshold; and determine that the hay receiving vehicle is located directly behind the silage machine when the real-time angle is between the first preset angle threshold and the second preset angle threshold.

[0018] Furthermore, the control module is specifically used for: Based on the transformed local point cloud data, the target features of the hay collection vehicle are identified. Based on the target characteristics of the hay receiving vehicle, control the material throwing operation of the spray nozzle.

[0019] Furthermore, the encoder is fixedly installed on the side of the turntable at the bottom of the spray nozzle, and the top gear of the encoder meshes with the turntable gear of the spray nozzle and is engaged by a spring clamping mechanism.

[0020] 3) In a third aspect, the present invention also provides an electronic device, the electronic device including a processor coupled to a memory, the memory storing at least one computer program, the at least one computer program being loaded and executed by the processor, so that the electronic device implements any of the above-mentioned vehicle relative pose perception and point cloud processing methods.

[0021] 4) In a fourth aspect, the present invention also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements any of the above-described vehicle relative pose perception and point cloud processing methods.

[0022] It should be noted that the beneficial effects of the technical solutions of the second to fourth aspects of the present invention and their corresponding possible implementations can be found in the above description of the technical effects of the first aspect and its corresponding possible implementations, and will not be repeated here. Attached Figure Description

[0023] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments of the present invention will be briefly introduced below: Figure 1 This is a flowchart illustrating a vehicle relative pose perception and point cloud processing method according to an embodiment of the present invention. Figure 2 This is a schematic diagram showing the encoder's installation location; Figure 3 This is a diagram showing the result of a straight line fitting. Figure 4 This is a schematic diagram of a vehicle relative pose perception and point cloud processing system according to an embodiment of the present invention. Detailed Implementation

[0024] The principles and features of the present invention are described below. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.

[0025] The technical solution of the present invention and how the technical solution of the present invention solves the above-mentioned technical problems are described in detail below with specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments. The embodiments of the present invention will now be described with reference to the accompanying drawings.

[0026] like Figure 1 As shown, an embodiment of the present invention provides a method for vehicle relative pose perception and point cloud processing, comprising the following steps: S1. The angle between the spray nozzle and the forage harvester body, measured by the encoder, is obtained in real time. The specific implementation process is as follows: S10. The foundation for angle measurement is the secure and precise mounting of a rotary encoder at a location that directly reflects the rotational motion of the spray nozzle. According to the design, the selected mounting location is the side of the turntable at the bottom of the spray nozzle. The turntable at the bottom of the spray nozzle is rigidly connected to the nozzle and rotates synchronously; therefore, the angular displacement of this turntable can express the angular displacement of the spray nozzle relative to the vehicle body. To transmit the rotational motion of the turntable to the encoder without loss, a top gear is mounted on the encoder's rotary output shaft. The design parameters of this top gear are completely consistent with those of the turntable gear at the bottom of the spray nozzle. Through precise mechanical assembly, the encoder's top gear meshes with the spray nozzle's turntable gear. To ensure tight meshing and eliminate any potential backlash or slippage during transmission, a spring-loaded clamping mechanism is used. This mechanism applies a continuous pressure to the encoder or its mounting base, forcing the encoder's top gear to remain firmly pressed against the spray nozzle's turntable gear, thus ensuring continuous and high-fidelity angle transmission.

[0027] S11. The raw output value of the installed encoder is merely a digital code representing the position of its internal code disk, such as a value of 1024. This does not directly correspond to the physically significant angle between the spray nozzle and the forage harvester body. Therefore, calibration is necessary to establish an accurate mapping between the encoder output value and the actual physical angle. A five-point calibration method is used to accomplish this. The calibration process requires the cooperation of the operator. The operator is positioned in the forage harvester cab, facing rearward. First, manually rotate the spray nozzle to the leftmost position, considered zero degrees, and record the encoder output value at this point. Second, rotate the spray nozzle sequentially to 45°, 90°, 135°, and 180° positions, and record the corresponding encoder output value at each point. This yields five sets of corresponding angle and code value data pairs. The basic principle of calibration is the assumption of a linear relationship between the spray nozzle rotation angle and the encoder output value, expressed by the formula: In this formula, Indicates the slope. This represents the intercept. Using the five sets of recorded data, the optimal slope can be calculated by fitting a straight line using the least squares method. and intercept The values ​​are obtained to complete the calibration and establish a conversion formula from the encoder's original values ​​to angle values.

[0028] S12. After completing the calibration calculations, the accuracy and reliability of the calibration results must be verified. Verification mainly involves two aspects. The first aspect is evaluating the quality of the fitted straight line, which is accomplished by calculating the correlation coefficient. The formula for the correlation coefficient is: in, This represents the calculated correlation coefficient; This indicates the number of data points involved in the calculation, in the five-point calibration method. It equals five; Indicates the first Encoder values ​​for each data point; Indicates the first The actual angle value corresponding to each data point; This represents the average of all encoder values; This represents the average of all actual angle values. The closer the calculated correlation coefficient is to one, the better the linear relationship between the angle and the coded value, and the more reliable the calibration result. The second aspect is to perform field accuracy verification. The spray nozzle is manually rotated to several non-calibration positions, such as 0°, 90°, and 180°. An angle measuring tool is used to measure the angle between the spray nozzle and the forage harvester body in the field, while simultaneously reading the angle value calculated by the encoder according to the calibration formula. The difference between the two is compared to ensure that the error is within the system's allowable range at all test points, for example, within ±2°.

[0029] S13. After the encoder is installed, calibrated, and verified, it can enter the real-time operation stage. During the operation of the forage harvester, the rotation of the spray nozzle drives the encoder to rotate through gear meshing. The sensors inside the encoder detect the code disk position in real time and output the current raw digital encoded value at a fixed frequency. The central processing unit continuously receives these raw encoded values. For each received real-time encoder raw value, the processing unit uses the conversion formula determined in the second calibration step to perform calculations. Specifically, the real-time acquired encoder raw value is substituted into the formula: This allows us to immediately obtain the angle between the spray nozzle and the forage harvester at the current moment. In this formula, It is the slope obtained from calibration. The intercept is obtained through calibration, and the "encoder value" is the raw digital code read in real time. This process is continuous and automatic, thereby realizing the real-time and accurate measurement and output of the angle between the spray nozzle and the forage harvester, providing a stable and reliable input for subsequent direction determination and coordinate transformation.

[0030] The encoder is fixedly installed on the side of the turntable at the bottom of the spray nozzle. The top gear of the encoder meshes with the turntable gear of the spray nozzle and is engaged by a spring clamping mechanism. Specifically: The turntable at the bottom of the spray nozzle is a ring-shaped metal component rigidly connected to the spray nozzle's rotation axis. Its outer edge is machined with toothed structures, forming the spray nozzle's turntable gear. The selected installation location is the side of the turntable at the bottom of the spray nozzle, a position that allows easy access and observation of the gear meshing from a side direction perpendicular to the turntable's axis. Before installation, the mounting surface of the turntable side and the tooth grooves of the spray nozzle's turntable gears must be cleaned, removing oil and debris. The rotary encoder, as the measuring unit, requires a transmission gear on its rotating shaft. This gear is called the encoder's top gear. During assembly, it is essential to ensure that the key parameters of the selected encoder's top gear, such as module, number of teeth, and pressure angle, are completely consistent with the parameters of the spray nozzle's turntable gear; this is a prerequisite for correct meshing. Using fasteners such as screws or key pins, the encoder's top gear is securely mounted on the encoder's rotating output shaft, ensuring there is no relative slippage or backlash between them. After assembly, a measuring module consisting of the encoder and its top gear is formed. Move the assembled encoder measurement module to the vicinity of the predetermined installation position on the side of the turntable at the bottom of the spray nozzle. Adjust the spatial orientation of the encoder body to initially align the teeth of the encoder's top gear with the teeth of the spray nozzle's turntable gear. Use a mounting bracket or base to initially fix the encoder housing to the forage harvester body or a rigid structure near the turntable at the bottom of the spray nozzle. This fixing point is located on the side of the turntable at the bottom of the spray nozzle. This initial fixing is preliminary and allows for minor positional adjustments. Based on this initial fixing, manually and slowly rotate the spray nozzle to observe the meshing of the encoder's top gear with the spray nozzle's turntable gear. Fine-tune the encoder's mounting angle and axial distance until the teeth of the two gears can smoothly and easily enter each other's tooth grooves, forming a preliminary meshing state, avoiding interference or jamming between tooth tips. To achieve stable, backlash-free meshing, a spring clamping mechanism is required. The spring clamping mechanism typically consists of a spring assembly that provides pressure and an adjustment mechanism. This mechanism is installed between the encoder housing or encoder mounting bracket and a fixed support point. During installation, the force applied by the spring clamping mechanism should be along the line connecting the centers of the encoder's top gear and the spray nozzle's rotary gear, or perpendicular to the common normal of the meshing tooth surfaces. Apply pre-compression force to the spring by tightening the adjusting bolts or changing the installation position. This pre-compression force is transmitted through the mounting bracket and ultimately acts on the encoder as a whole, pushing the encoder's top gear to continuously press against the spray nozzle's rotary gear. During commissioning, pressure should be applied while manually rotating the spray nozzle to feel the changes in gear transmission resistance. The clamping force of the spring clamping mechanism needs to be adjusted to a suitable level: ensuring that the encoder's top gear and the spray nozzle's rotary gear are in close contact in any working position, eliminating gaps when the transmission reverses; yet, the pressure should not be too high, causing accelerated gear wear or excessive rotational resistance that could affect the normal rotation of the spray nozzle.After adjusting the engagement state to the optimal level using the spring clamping mechanism, the entire encoder mounting system needs to be finally locked. Tighten all bolts on the encoder mounting bracket to ensure the encoder is completely fixed in position on the turntable side at the bottom of the spray nozzle, preventing further displacement due to vibration. Next, perform a comprehensive functional check. Manually rotate the spray nozzle slowly from left to right throughout its working range, visually inspecting whether the meshing line between the encoder's top gear and the spray nozzle's turntable gear remains smooth, without any disengagement or jumping. Simultaneously, listen for uniform gear transmission sounds without abnormal knocking noises. After completing the mechanical checks, connect the encoder's electrical wiring, power on the system, and rotate the spray nozzle to observe whether the encoder signal output is continuous and without jumps, verifying successful mechanical installation. At this point, the encoder is securely mounted on the turntable side at the bottom of the spray nozzle, and the specific process of the encoder's top gear meshing with the spray nozzle's turntable gear via the spring clamping mechanism is complete, establishing a reliable hardware foundation for angle measurement.

[0031] The angle between the spray nozzle and the forage harvester's body is a specific physical angular quantity. In a forage harvester system, the spray nozzle is a material-throwing mechanism that can rotate around a vertical axis, while the forage harvester's body refers to its chassis or main structure. The angle between the spray nozzle and the forage harvester's body specifically refers to the angle formed on a horizontal plane between the spray nozzle's central axis and the inherent longitudinal central axis of the forage harvester's body. This angle directly reflects the deflection of the spray nozzle relative to the forage harvester's body, and its value typically ranges from 0 degrees to 180 degrees. Obtaining this angle is a fundamental physical parameter for subsequently determining the relative direction of travel of the forage-collecting vehicle and for coordinate system transformation.

[0032] S2. Determine the relative travel direction of the hay receiving vehicle based on the real-time acquired angle. Specifically, when the real-time angle is less than a first preset angle threshold, it is determined that the hay receiving vehicle is traveling to the left relative to the silage harvester; when the real-time angle is greater than a second preset angle threshold, it is determined that the hay receiving vehicle is traveling to the right relative to the silage harvester; when the real-time angle is between the first and second preset angle thresholds, it is determined that the hay receiving vehicle is located directly behind the silage harvester. The specific implementation process is as follows: S20. During the initialization or parameter configuration phase, two key angle thresholds need to be defined: a first preset angle threshold and a second preset angle threshold. These two thresholds are set based on the numerical range of the angle between the spray nozzle and the forage harvester's body, as well as practical operational experience. The first preset angle threshold is set to a value less than 90°, such as 85°. The second preset angle threshold is set to a value greater than 90°, such as 95°. These two thresholds are introduced to establish a non-sensitive zone or stable interval near the theoretical center point of 90°. When the real-time angle is between the first preset angle threshold and the second preset angle threshold, the system determines that the hay truck is directly behind the forage harvester; this interval is called the "forward driving interval." The first preset angle threshold and the second preset angle threshold are fixed system parameters and are usually not dynamically modified during operation.

[0033] S21. During the operation of the forage harvester, the angle measurement system operates continuously. The encoder measures the real-time angle between the spray nozzle and the forage harvester body at a fixed sampling frequency, for example, once every ten milliseconds, and transmits the angle data to the central processing unit via a signal line. The central processing unit's program maintains a cyclic reading process, continuously obtaining the latest real-time angle value from the data interface and storing it in a designated variable for use by the direction determination logic. This real-time angle value changes dynamically, reflecting the real-time adjustments made by the spray nozzle as the position of the forage receiving vehicle changes.

[0034] S22. The central processing unit runs a direction determination algorithm. This algorithm compares the acquired real-time angle with two preset angle thresholds and outputs the corresponding travel direction status based on the comparison result. The determination logic strictly follows these rules: The current real-time angle value is acquired and compared with the first and second preset angle thresholds. If the real-time angle value is less than the first preset angle threshold, the hay truck is determined to be traveling to the left relative to the forage machine. If the real-time angle value is greater than the second preset angle threshold, the hay truck is determined to be traveling to the right relative to the forage machine. If the real-time angle value is greater than or equal to the first preset angle threshold and simultaneously less than or equal to the second preset angle threshold, the hay truck is determined to be directly behind the forage machine. This logical process can be clearly represented by the following mathematical expression: in, This indicates the angle between the spray nozzle and the forage harvester body, as obtained in real time. This indicates the first preset angle threshold; This represents the second preset angle threshold. The output of the direction determination algorithm is a clear state identifier, such as an enumeration value or string, indicating one of "driving to the left", "driving to the right", or "direction behind".

[0035] S23. To avoid frequent switching between "driving to the left" and "directly behind" or "driving to the right" and "directly behind" near the 90° critical point due to minor nozzle vibration or angle measurement noise, the system introduces state hysteresis processing logic. This does not modify the threshold, but rather adds a hysteresis condition to the state switching. The system internally needs to record the previously determined direction state. When the direction state calculated by the current real-time angle differs from the previous state, and involves entering or leaving the "directly behind" state, the hysteresis logic takes effect. For example, if the previous state was "driving to the left," and the current real-time angle increases, the state only switches to "directly behind" if the real-time angle is greater than the second preset angle threshold; conversely, if the previous state was "directly behind," and the current real-time angle decreases, the state only switches to "driving to the left" if the real-time angle is less than the first preset angle threshold. A similar hysteresis rule is used for switching between "driving to the right" and "directly behind." This processing effectively filters signal jitter and improves the stability of the direction judgment output.

[0036] S24. Once the direction determination logic is complete and outputs a stable direction state, the result will be immediately passed to subsequent subsystems. This determination result—"the hay truck is moving to the left relative to the forage harvester," "the hay truck is moving to the right relative to the forage harvester," or "the hay truck is located directly behind the forage harvester"—will serve as key input, directly guiding the subsequent step of "regional filtering of 3D point cloud data." This dynamically determines which spatial region to concentrate computing resources on for point cloud processing, achieving accurate and efficient target recognition. The entire determination process, from data acquisition to result output, forms an automated decision-making chain.

[0037] "Left-side driving" describes the position of the hay truck relative to the forage harvester. During forage harvester operation, the operator is positioned in the cab, facing rearwards. "Left-side driving" means the hay truck is positioned to the left of the observer's field of vision, and its direction of travel forms an angle with the forage harvester's direction of travel, causing the truck's longitudinal axis to point to the left of the harvester's centerline. This relative position usually means that the spray nozzles need to be deflected to the left at a certain angle to accurately disseminate the hay into the hay truck's hopper.

[0038] "Right-side driving" describes another positional state of the hay receiving cart relative to the forage harvester. During forage harvester operation, the operator is positioned in the cab, facing rearwards. Right-side driving means the hay receiving cart is positioned to the right of the observer's field of vision, and its direction of travel forms an angle with the forage harvester's direction of travel, causing the cart's longitudinal axis to point to the right of the harvester's centerline. This relative position usually means that the spray nozzles need to be deflected to the right at a certain angle to accurately disseminate the hay into the cart's hopper.

[0039] The term "directly rearward" describes a specific position of the hay receiving cart relative to the forage harvester. During forage harvester operation, the operator is positioned in the cab, facing rearward. "Directly rearward" means the hay receiving cart is roughly on the extension of the forage harvester's longitudinal centerline, its direction of travel is essentially parallel and consistent with the harvester's, and it is located directly behind the observer's center of vision. This relative position usually means that the spray nozzles need to be aimed directly rearward, i.e., at a zero-degree deflection angle, to project the hay in a straight line into the hay receiving cart's hopper.

[0040] S3. Based on the relative travel direction of the hay receiving vehicle, the 3D point cloud data in the coordinate system used by the forage harvester is filtered by region to obtain the local 3D point cloud data corresponding to the relative travel direction of the hay receiving vehicle. The specific implementation process is as follows: S30. All spatial calculations and filtering are performed within a unified reference system, namely the coordinate system used by the forage harvester. This coordinate system typically has its origin at a fixed point on the forage harvester (e.g., the optical center of the binocular vision system or the center of the rear axle of the vehicle). The coordinate axis directions are predefined; for example, the X-axis points directly behind the forage harvester, the Y-axis points to the left side of the forage harvester, and the Z-axis points upwards. The binocular vision system installed on the forage harvester acquires images of the hay truck and its surrounding environment at a fixed frequency (e.g., 10 frames per second) and generates raw 3D point cloud data in the coordinate system used by the forage harvester through a stereo matching algorithm. Each frame of raw 3D point cloud data contains tens of thousands of 3D spatial points, each represented by coordinates... It means that, among them, This represents the X-axis coordinate of the point in the coordinate system used by the forage harvester. Represents the Y-axis coordinate value. This represents the Z-axis coordinate value. This dense point cloud data comprehensively describes the three-dimensional scene within the field of view behind the forage harvester.

[0041] S31. During initialization, based on the three possible relative travel directions of the hay collection cart, three corresponding spatial screening areas are predefined in the coordinate system used by the forage harvester. These areas are spatial ranges set based on the typical dimensions of the hay collection cart, safe operating distance, and the coverage range of the spray nozzles. For the direction where "the hay collection cart travels to the left relative to the forage harvester," a "left-side focus area" is defined. This area is represented as a spatial cuboid in the coordinate system used by the forage harvester, and its boundaries are defined by a set of coordinate ranges, such as the X-axis coordinate range within... The Y-axis coordinate range is within The Z-axis coordinate range is within . and These represent the minimum and maximum coordinate values ​​of the left-side region of interest along the X-axis, respectively. and These represent the minimum and maximum coordinate values ​​in the Y-axis direction, respectively. and These represent the minimum and maximum coordinate values ​​in the Z-axis direction, respectively. Similarly, a "right-side focus area" is defined for the direction of "the hay truck moving to the right relative to the silage harvester," with a coordinate range of [missing information]. , , Define a "directly behind" area of ​​interest for the condition that "the hay truck is located directly behind the silage harvester," with coordinates ranging from [value missing]. , , These coordinate range parameters are pre-stored as constants in the system.

[0042] S32. After the direction determination module outputs the current relative travel direction status of the hay receiving vehicle, the area filtering module immediately receives this status information. Internally, the module maintains a query mapping relationship, mapping the direction status one-to-one with predefined spatial filtering areas. If the received direction status is "the hay receiving vehicle is traveling to the left relative to the forage harvester," the area filtering module selects the "left-side focus area" and its corresponding coordinate range parameters. , , This serves as the filtering criterion for the current frame. If the received direction status is "the hay truck is moving to the right relative to the silage harvester," then the "right-side focus area" and its corresponding coordinate range parameters are selected. , , If the received direction status is "the hay truck is directly behind the silage harvester", then select the "directly behind area of ​​interest" and its corresponding coordinate range parameters. , , .

[0043] S33. After selecting the filtering area and its coordinate range, the program begins processing the raw 3D point cloud data of the current frame. The program sequentially traverses each 3D point in the raw 3D point cloud data. For each point traversed, its coordinate values ​​in the coordinate system used by the forage harvester are read. Then, at this point The coordinate values ​​are compared with the X-axis range of the selected area to check if they meet the requirements. ;Will The coordinate values ​​are compared with the Y-axis range of the selected area to check if they meet the requirements. ;Will The coordinate values ​​are compared with the Z-axis range of the selected area to check if they meet the requirements. Here , , , , , These represent the coordinate range parameters of the selected specific area. For example, if the left-hand area of ​​interest is selected, these parameters represent... , , , , , A point is considered to be within the selected region of interest only if its three-dimensional coordinates simultaneously satisfy all three of the above inequalities.

[0044] S34. During the traversal and comparison process, all points that meet the filtering criteria are extracted and aggregated to form a new point cloud set with a significantly reduced data volume. This new point cloud set is called local 3D point cloud data. Local 3D point cloud data is a subset of the original 3D point cloud data; it only contains points located within the spatial filtering area corresponding to the current direction of travel of the hay harvester. For example, when traveling to the left, the output local 3D point cloud data mainly contains points within the left spatial range of the coordinate system used by the hay harvester, and these points are more likely to belong to the front and left side of the hay harvester. This set of local 3D point cloud data is then output and passed to the subsequent coordinate transformation and feature recognition modules for processing. Through this series of steps, the system achieves intelligent focusing of point cloud data based on direction awareness, concentrating computing resources on the most relevant scene parts and laying an efficient data foundation for real-time control.

[0045] S4. Based on the real-time acquired angle, calculate the rotation matrix from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle, and transform the local 3D point cloud data to the coordinate system used by the hay receiving vehicle based on the rotation matrix. The specific implementation process is as follows: S40. The prerequisite for coordinate transformation is a clear definition of the coordinate systems used by the silage harvester and the hay collection cart. The coordinate system used by the silage harvester is a three-dimensional Cartesian coordinate system fixed on the harvester body. As mentioned before, its origin is usually set at the center of the vision sensor at the rear of the silage harvester, the X-axis points directly behind the silage harvester, the Y-axis points to the left side of the silage harvester, and the Z-axis is vertically upward. The coordinate system used by the hay collection cart is a three-dimensional Cartesian coordinate system defined on the hay collection cart body. Ideally, the X' axis of the hay collection cart's coordinate system should point directly forward of the hay collection cart (i.e., in the direction of travel), the Y' axis should point to the left side of the hay collection cart, and the Z' axis should be vertically upward. In the ideal state where the silage harvester and the hay collection cart travel parallel to each other in the same direction, the X-axis of the coordinate system used by the silage harvester is in the same direction as the X' axis of the coordinate system used by the hay collection cart. However, in actual operation, the hay receiving vehicle may be located to the left, right, or directly behind the forage harvester, resulting in a rotational angle around the vertical axis (Z-axis) between the two coordinate systems in the horizontal direction (i.e., the XOY plane and the X'O'Y' plane). This angle is precisely the angle between the spray nozzle and the forage harvester body, denoted as . .

[0046] S41. Continuously read the latest calculated angle between the nozzle and the forage harvester body from the angle measurement module and assign it to the variable. In this step, it is necessary to confirm this included angle. The physical meaning of this is consistent with the definition of the rotation angle required for coordinate transformation. In this invention, when the spray nozzle points directly behind the silage harvester (i.e., the included angle is 90°), the hay receiving cart is located directly behind the silage harvester, the two coordinate systems are aligned, and the rotation angle is... The corresponding angle of change should be zero. When the hay truck moves to the left, the spray nozzle deflects to the left, with an angle of... If the angle is less than 90°, the coordinate system used by the forage harvester needs to be rotated by an angle around its positive Z-axis. Only then can it be aligned with the direction of the hay truck, variable It is directly used as the rotation angle input around the Z-axis.

[0047] S42. The core of coordinate transformation is constructing a rotation matrix. Transforming from the coordinate system used by the silage harvester to the coordinate system used by the hay delivery vehicle involves rotating the coordinates around the Z-axis of the silage harvester's coordinate system by an angle. This is achieved through rotation. The rotation matrix is: In this rotation matrix middle, This indicates the angle between the spray nozzle and the forage harvester body, as obtained in real time. This represents the operations of cosine and trigonometric functions. This represents sine and trigonometric function operations. Elements in a matrix. Indicates the rotation angle cosine value, element Indicates the rotation angle The sine value, element Indicates the rotation angle The opposite of the sine value. The numbers 0 and 1 are constants. This is a 3×3 matrix. This describes how to rotate a point represented in the coordinate system used by the forage harvester about the Z-axis. Angles are transformed into a new coordinate system (i.e., the coordinate system used by the hay-collecting cart).

[0048] S43. Obtain the rotation matrix Then, the system performs coordinate transformation on each point in the local 3D point cloud data. Assume any point in the local 3D point cloud data... Its coordinates in the coordinate system used by the forage harvester are in column vector form. . , , These represent the X, Y, and Z coordinates of the point, respectively. This represents the transpose of a vector. It's used to obtain the coordinates of the point in the coordinate system used by the hay-collecting cart. Perform matrix multiplication: Expanding the calculation yields: Through the above calculations, point The coordinates were transformed from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle. The system iterates through each point in the local 3D point cloud data, repeatedly performing this matrix multiplication operation.

[0049] S44. After all points in the local 3D point cloud data have undergone the aforementioned coordinate transformation calculations, these points constitute 3D point cloud data represented in the new coordinate system used by the hay receiving vehicle. This set of data is the output result of "transformation to the coordinate system used by the hay receiving vehicle". Subsequently, this set of point cloud data with unified coordinates and orientation aligned with the direction of the hay receiving vehicle is output and immediately passed to the next processing module, namely the module for "identifying the target features of the hay receiving vehicle based on the transformed local point cloud data", providing a correct spatial reference basis for accurately identifying the boundary of the truck bed and the material accumulation height. The entire transformation process, from reading the included angle to outputting the result, achieves rapid and accurate alignment of the coordinate system.

[0050] S5. Based on the transformed local point cloud data, control the material throwing operation of the nozzle, specifically including: S50. Based on the transformed local point cloud data, identify the target features of the hay collection vehicle. The specific implementation process is as follows: Although the transformed local 3D point cloud data input by the S500 has undergone region filtering and coordinate alignment, it may still contain a small number of noise points, isolated points, or outliers caused by sensor errors. The first step is to preprocess this part of the data to improve the robustness of subsequent feature recognition. A statistical filtering method is used to calculate the average distance from each point in the point cloud to all its neighbors. Assuming that the average distance of all points in the point cloud follows a Gaussian distribution, points whose mean distance exceeds a certain multiple (e.g., 2 times) of the standard deviation are considered outliers and removed. This step smooths the data, retaining the point cloud that truly belongs to the hay truck body and materials, creating a clean data environment for feature extraction.

[0051] S501. In the coordinate system used by the hay truck, its Z-axis points vertically upwards. This characteristic can be used to perform preliminary segmentation of the pre-processed point cloud. Calculate the statistical histogram of the local point cloud along the Z-axis (i.e., the height direction), and distinguish planes at different heights by finding significant peaks in the histogram. Typically, dense point cloud regions at the lowest height correspond to the ground or the truck bed floor; point clouds at intermediate heights with relatively continuous surfaces may correspond to the top of the loaded material; and isolated point clouds at higher positions with linear edge features may correspond to the side panels and top edge of the truck bed. By setting appropriate height threshold ranges, the point cloud can be initially segmented into suspected ground areas, material areas, and truck bed structure areas, providing guidance for subsequent fine-grained identification.

[0052] S502. The identification of the truck bed boundary focuses on the structural area of ​​the truck bed, particularly the edge contour of the top of the truck bed. From the segmented point cloud of the truck bed structure, points with relatively high Z-coordinate values ​​(near the top of the truck bed) and exhibiting clear boundary distribution are extracted. A plane detection algorithm based on 3D Hough transform or a random sampling consensus algorithm is used to identify the vertical planes where the truck bed side panels are located. The intersection direction of these vertical planes indicates the approximate orientation of the truck bed. Next, at a height near the top, the point cloud is projected in two dimensions (projected onto an approximately horizontal X'O'Y' plane), and an edge detection algorithm (such as boundary point extraction based on normal vector mutation) is used to find the outer contour of the projected point cloud. Finally, the least squares method is used to fit this outer contour into a rectangular polygon. The positions of the four sides of the fitted rectangle in 3D space are the identified truck bed boundary. This rectangular bounding box provides the absolute spatial constraint range for the material throwing operation.

[0053] S503. The calculation of material accumulation height relies on precise analysis of the material area point cloud. The segmented material area point cloud is projected onto the X'O'Y' plane of the coordinate system used by the hay truck, and this plane is divided into a regular two-dimensional grid. For each grid cell, the maximum value of the Z coordinate among all points falling into that cell is found, and this value is regarded as the material surface height at that grid cell. By processing all grid cells, the two-dimensional height field of the material surface can be reconstructed. The material accumulation height can be expressed in two ways: one is the global height, which is the highest point height or average height of the material surface in the entire truck bed; the other is the local height, which is the average height of the grid cells for each of the multiple small areas (such as the front, middle, rear, left, and right sides) of the truck bed. By comparing the heights of these local areas, the low material level area at the current moment can be clearly identified, that is, the area with a relatively small material accumulation height.

[0054] S504. The results identified in the above steps are structured and encapsulated to form a complete target feature description of the hay collection vehicle. This description includes at least: the three-dimensional spatial parameters of the vehicle's hopper boundary (e.g., the coordinates of the four corner points of the rectangle), and the material accumulation height information (e.g., the global height value and the local height values ​​or height distribution map of each zone). This feature data is output to the spray nozzle throwing control module in real time, serving as the direct basis for generating precise throwing commands. The entire identification process realizes the transformation from the original local point cloud to quantified operational features, providing key decision parameters for the closed-loop automated operation.

[0055] The target features of the hay receiving vehicle refer to the key geometric and state information extracted from 3D point cloud data to guide the silage machine's throwing operation. These features are the foundational input for subsequent intelligent throwing decisions, mainly including a precise description of the hopper's spatial structure and loading status. Identifying these targets is the core step in achieving automated and precise throwing. Specifically, the target features of the hay receiving vehicle primarily include the hopper boundary. The hopper boundary defines the spatial range of the physical container used by the hay receiving vehicle to carry materials, typically referring to the edge outline of the open area at the top of the hopper. In point cloud data, it is represented as an approximately rectangular 3D spatial boundary, and its identification directly determines the safe and effective area that the throwing cylinder should not exceed, preventing hay from spilling outside the hopper. The target features of the hay receiving vehicle also include the material accumulation height. The material accumulation height describes the vertical distance from the surface of the loaded hay inside the hopper to the hopper floor, or the remaining space height from the top edge of the hopper. It is a dynamically changing parameter reflecting the hopper's filling status. By identifying the material accumulation height in real time, the system can determine the low material level area in the truck bed, and then plan the throwing cylinder to throw the straw to the location that needs to be filled.

[0056] S51. Based on the target characteristics of the hay receiving vehicle, control the material throwing operation of the spray nozzle. The specific implementation process is as follows: S510: Receives the structured, encapsulated target feature data of the hay truck. This data mainly includes two parts: the first part is the three-dimensional spatial coordinates of the truck bed boundary, usually given in the form of the three-dimensional coordinates of the four corner points of the rectangular outline at the top of the truck bed, for example, points... , , , These coordinates are defined in the coordinate system used by the hay delivery vehicle. , , The first part represents the X, Y, and Z coordinates of a point in the coordinate system used by the hay receiving vehicle, with subscript numbers used to distinguish different corner points. The second part is the material accumulation height information, usually given as the material surface height value of each grid cell after the two-dimensional grid inside the truck bed is divided, or directly outputting the three-dimensional spatial center coordinates of one or more low material level areas.

[0057] S511. The primary task of the control algorithm is to determine the target point inside the hay truck's hopper that most urgently needs to be filled, based on the target characteristics of the hay truck. The algorithm iterates through the material accumulation height information, comparing the height values ​​of different grid cells to find the region with the lowest material surface height. The geometric center point coordinates of this region are calculated as the target point for filling, denoted as... . , , These represent the three-dimensional coordinates of the target point in the coordinate system used by the hay collection vehicle. When determining the target point, a boundary safety check must be performed to ensure the target point... of and The coordinates are located at the boundary of the truck bed. to Within the bounded rectangular horizontal projection area, and The coordinates are set below the height of the truck bed side panel to prevent the calculated target point from being located outside the truck bed or at an illogical height.

[0058] S512, Target point for material throwing The coordinates are defined in the coordinate system used by the hay truck, while the angle control of the spray nozzle needs to be based on the coordinate system used by the forage harvester. Therefore, it is necessary to inversely transform the target point coordinates back to the coordinate system used by the forage harvester. This is done using the previously calculated and stored rotation matrix. Perform the inverse transformation. Rotation matrix. The inverse matrix is ​​equal to its transpose matrix, i.e. The coordinates of the target point in the coordinate system used by the forage harvester are calculated using the following formula. : In the coordinate system used by the forage harvester, the target direction vector that the spray nozzle needs to point to can be simplified to a point from the nozzle outlet (or the origin of the coordinate system) to the target point. The projection of the vector onto the horizontal plane (XOY plane). The angle between the required spray nozzle and the forage harvester body. It can be calculated using the arctangent function: Represents the arctangent function in the four quadrants. and These are the X and Y coordinates of the target point in the coordinate system used by the forage harvester. The calculated angle... It needs to be standardized to map it to the actual mechanical movement range of the nozzle, for example, between 0° and 180°.

[0059] S513, Obtain the desired angle between the spray nozzle and the forage harvester body. Next, the control module needs to generate servo control commands to drive the rotary table at the bottom of the spray nozzle to rotate. The system reads the real-time angle between the spray nozzle and the forage harvester body measured by the current encoder. Calculate the angle deviation. A proportional-integral-derivative (PID) control algorithm is adopted, based on the angle deviation. The algorithm calculates the rate of change of the angle deviation and outputs the control signal to the servo motor, such as the duty cycle of a pulse width modulation signal or the target speed. Approaching zero as quickly and smoothly as possible, i.e., making the real-time angle... Tracking the desired angle Meanwhile, the control logic needs to integrate the boundary safety verification results from the second step. If the target point is invalid or there is a safety risk, the control command should maintain the nozzle at a safe default angle or the previous valid angle and trigger an alarm.

[0060] S514: The generated control commands are sent to the servo motor driver that drives the spray nozzle turntable via fieldbus or hardwired connection. The servo motor drives the turntable gear at the bottom of the spray nozzle to rotate according to the commands, thereby adjusting the spray nozzle's direction. The encoder continuously measures the real-time angle between the spray nozzle and the forage harvester body. The measured values ​​are fed back to the control module, forming a closed-loop control. The system also monitors the continuous status of the material throwing operation, including whether the relative direction of the receiving vehicle changes and whether the material accumulation height is updated. Whenever the feature recognition module outputs a new frame of the receiving vehicle's target features, the entire process from feature analysis to control command generation is re-executed, thereby achieving dynamic and adaptive tracking of the nozzle towards the low-level material area in the hopper, ensuring continuous and accurate material throwing.

[0061] The technical solution of the present invention will be further described through another embodiment.

[0062] This invention provides a method for determining vehicle travel direction and point cloud processing. The aim is to address the technical problems in existing technologies that rely on global navigation satellite systems and inertial measurement units (INS) to determine vehicle travel direction. These problems include susceptibility to failure in signal-blocked environments such as mountainous areas, accumulated errors in heading angles, excessive deviations in the angle between the two vehicles calculated using point cloud plane fitting, and high computational complexity and poor real-time performance in subsequent 3D point cloud processing. This invention reliably determines the travel direction of the hay-collecting vehicle by directly acquiring the mechanical relative angle between the spray nozzle and the silage harvester's body. Based on this, it intelligently partitions the 3D point cloud data, thereby achieving stable, efficient, and low-cost perception of the relative pose of the operating vehicles, improving the accuracy and reliability of automated agricultural machinery operations.

[0063] The method of this invention mainly includes four parts: selecting the encoder installation position, calibrating the encoder, verifying the encoder accuracy, determining the vehicle head direction, and coordinate system rotation. The first step is to select the encoder installation position. In order to achieve accurate measurement of the angle between the spray nozzle and the forage harvester body, it is necessary to select an encoder installation position that can truly and directly reflect the change in the relative angle between the two. The selected encoder installation position is as follows: Figure 2 As shown, the installation position is determined to be on the side of the turntable at the bottom of the spray nozzle. The turntable at the bottom of the spray nozzle rotates synchronously with the spray nozzle, thus it can most directly represent the actual rotation angle of the spray nozzle. To ensure that the encoder can accurately capture every angular change of the turntable, a gear meshing transmission mechanism was designed. A top gear that perfectly matches the parameters of the spray nozzle's turntable gear is added to the selected encoder rotating shaft. Through precise mechanical alignment, the encoder's top gear and the spray nozzle's turntable gear form a tight meshing relationship, thereby transmitting the rotational motion of the spray nozzle to the encoder with almost no loss. To eliminate any potential backlash in the gear meshing and ensure smooth, slip-free transmission, an important fixing measure is also adopted: a spring is used to provide a continuous, adjustable clamping force for the entire encoder mechanism. This spring clamping mechanism can always stably press the encoder's top gear against the spray nozzle's turntable gear, effectively compensating for any small gaps that may be caused by wear or vibration, thereby ensuring the continuity and high fidelity of the angle measurement signal.

[0064] The second step is encoder calibration. The purpose of encoder calibration is to ensure a high degree of consistency between the encoder's output angle signal and the actual physical quantity through a systematic calibration process, thereby improving measurement accuracy and system reliability. The encoder is mounted on the forage harvester turntable, and its gears are connected to the turntable gears. Internally, the encoder divides the 360° mechanical angle into 4096 parts, each with a resolution of approximately 0.088°. The encoder's raw output value only represents the mechanical position encoding of the code disk, not a directly usable physical angle value. For example, an output value of 1024 indicates that the code disk is at the 1024th encoding position, but does not specify the corresponding actual angle; without calibration, this value cannot be directly used to control the nozzle rotation to a 90° position. To ensure the measurement accuracy of the angle between the nozzle and the forage harvester body, an accurate mapping relationship between the nozzle rotation angle and the encoder reading needs to be established. The five-point calibration method can effectively achieve this goal. This method selects five key angles for measurement and uses the least squares method to fit a linear relationship between the angle and the encoded value, thus completing the system calibration. The principle of the five-point calibration method lies in fitting an overall linear relationship by measuring data from multiple discrete points. The selection of 0°, 45°, 90°, 135°, and 180° points is primarily based on considerations of covering the entire measurement range, ensuring computational stability, and operational feasibility. The basic principle of calibration is that it assumes a good linear relationship between the nozzle's rotation angle and the encoder output value, i.e.: By measuring data from five points, the slope can be calculated. and intercept The optimal value is obtained. The entire calibration process must be strictly followed: Define the angle reference. The operator is in the forage harvester cab, facing backwards, and rotates the spray nozzle to the leftmost position, defining this position as the 0° starting point for manual operation. The position directly behind the spray nozzle is 90°, and the rightmost position is 180°. Prepare and inspect the equipment, ensuring the forage harvester is in a safe stopped state. Check if the encoder is securely installed, if the gears mesh well, and if the wiring connections are normal. Perform data recording. Manually control the spray nozzle to rotate it precisely to the 0° position. After the spray nozzle stabilizes, record the current rotation angle of the spray nozzle and the encoder reading. Continue to control the spray nozzle, rotating it 45° each time, stopping at 45°, 90°, and 135° positions and recording the corresponding encoder values. Finally, rotate the spray nozzle to the 180° position to complete the recording of the fifth set of data. The fitting effect of the straight line is as follows: Figure 3 As shown.

[0065] The third step is to verify the encoder's accuracy. After the rotary encoder calibration is completed, verification is required to determine whether the calibration accuracy meets the requirements. Verification consists of three sub-steps. The first sub-step is to evaluate the quality of the fit, calculating the correlation coefficient to assess the degree of agreement on the linear relationship. Generally, the closer the correlation coefficient is to 1, the better the linear relationship, and the more reliable the calibration results. The formula for the correlation coefficient is: in, This represents the calculated correlation coefficient; Indicates the number of data points involved in the calculation; Indicates the first Encoder values ​​for each data point; Indicates the first The actual angle value corresponding to each data point; This represents the average of all encoder values; This represents the average of all actual angle values. The second sub-step involves rotating the spray nozzle to any non-calibrated position and using a tool to measure the actual angle between the spray nozzle and the forage harvester body at that position. The third sub-step involves reading the angle value calculated by the encoder according to the calibration formula and checking whether the error between it and the actual measured angle is within the allowable range, for example, within ±2°.

[0066] The fourth step is determining the vehicle's heading direction and reversing the coordinate system. During operation where the forage harvester and the hay collection cart travel in the same direction, the angle of the spray nozzle relative to the forage harvester directly reflects the horizontal position of the hay collection cart relative to the forage harvester. Based on the real-time angle between the spray nozzle and the forage harvester, the relative direction of travel of the hay collection cart is determined: when the real-time angle is less than 90°, it indicates that the spray nozzle is biased to the left of the forage harvester, and the hay collection cart is determined to be traveling to the left in the field of view; when the real-time angle is greater than 90°, it indicates that the spray nozzle is biased to the right of the forage harvester, and the hay collection cart is determined to be traveling to the right in the field of view; when the real-time angle is approximately equal to 90°, it indicates that the spray nozzle is pointing directly backward, meaning that the hay collection cart is roughly located on the center line directly behind the forage harvester. To avoid frequent switching of the processing area due to slight vibrations of the spray nozzle near the 90° critical point, a judgment hysteresis loop is set to enhance the system's anti-interference capability. For example, when the angle increases from a direction less than 90°, it is only identified as "driving right" when it exceeds 95°; conversely, when the angle decreases from a direction greater than 90°, it is only identified as "driving left" when it falls below 85°. This direction-based region-focusing strategy can significantly reduce the number of point clouds that need to be processed in real time. Computational resources can be concentrated on the area where the front of the vehicle is most likely to appear, thereby reducing interference for subsequent, more refined vehicle front recognition algorithms and improving computational speed.

[0067] Furthermore, the acquired encoder angle value also provides the rotation angle for the coordinate system rotation. Let the angle between the spray nozzle and the forage machine be... This angle is obtained by reading the encoder value. Therefore, the formula for the rotation matrix around the Z-axis is: By rotating around the Z-axis The coordinate system used by the silage harvester is rotated to be basically parallel to the coordinate system used by the hay receiving vehicle. This avoids the problem of large deviations in the calculation of the angle between the hay receiving vehicle and the silage harvester due to the deviation of the point cloud fitting plane, and facilitates a series of subsequent algorithm processing.

[0068] This invention leverages the stable relative positional relationship between the forage harvester and the hay receiving vehicle during collaborative operation. By directly measuring the angle between the spray nozzle and the forage harvester's body, the real-time travel direction of the hay receiving vehicle is indirectly and reliably inferred. This method replaces the traditional combination of a global navigation satellite system and an inertial measurement unit. Based on the real-time determination of the relative travel direction of the hay receiving vehicle, the system dynamically filters the collected 3D point cloud data in the coordinate system used by the forage harvester. The system focuses only on the key areas where the hay receiving vehicle's front is most likely to appear for subsequent identification processing. This strategy reduces the amount of 3D point cloud data requiring real-time processing by more than 50%, overcoming the real-time bottleneck of existing 3D vision algorithms running on onboard computing units. This allows complex hay truck feature recognition algorithms to operate stably. The physical benchmark of the angle between the spray nozzle and the forage harvester's body, measured by a high-precision rotary encoder, serves as the core basis for transforming the coordinate system used by the forage harvester to that used by the hay receiving vehicle. The angle signal comes directly from the mechanical transmission and has the advantages of stability and anti-interference. Its accuracy is not affected by ambient light, point cloud quality or sensor noise, thus ensuring the accuracy of coordinate system transformation.

[0069] In this invention, the encoder is installed at a specific position on the side of the turntable at the bottom of the spray nozzle, a top gear is added to mesh with the turntable gear of the spray nozzle, and a spring clamping mechanism is used to eliminate gear transmission backlash. This specific mechanical installation structure is the basis for obtaining high-fidelity angle signals. A mapping relationship between the encoder value and the actual angle is established through a five-point calibration method. Based on the comparison between the real-time included angle and a preset angle threshold, it is determined whether the hay truck is traveling to the left, right, or directly behind. According to the result of the direction of travel determination, the corresponding spatial filtering area is dynamically selected to filter the original three-dimensional point cloud data and obtain local three-dimensional point cloud data. To avoid frequent switching of the processing area due to slight vibration of the spray nozzle near the 90° critical point, a hysteresis judgment method is set. Specifically, a first preset angle threshold is set. Second preset angle threshold ,For example , The direction determination logic is: when the real-time angle... Less than When, it is determined to be driving to the left; when Greater than When, it is determined to be driving on the right; when Between and When the distance is between, it is determined to be in the direction of direct rearward. Furthermore, hysteresis logic is introduced during state transitions to enhance stability. This invention deeply integrates physical angle measurement of the mechanical system with 3D point cloud processing, proposing a novel solution for relative pose perception of working vehicles. This solution directly and accurately obtains the angle between the spray nozzle and the silage harvester's body through an encoder, thereby reliably determining the direction of travel of the hay-collecting vehicle. This design fundamentally overcomes the technical pain points of traditional global satellite navigation systems combined with inertial measurement units (IGMUs) being prone to failure in signal-blocked scenarios such as mountainous areas, and the accumulation of heading angle errors over time in microelectromechanical inertial measurement units (MEMS). The foundation of this solution lies in the innovative installation method of the encoder: the installation position is chosen on the side of the turntable at the bottom of the spray nozzle. In terms of the installation structure, a top gear precisely matched to the turntable gear of the spray nozzle is added to the top of the encoder, and a spring clamping mechanism is used for clamping and fixing. This design ensures continuous and stable meshing between the gears, effectively eliminating transmission backlash, thus providing a high-fidelity angle signal for subsequent processing. This invention achieves more stable and reliable relative pose perception of operating vehicles with lower hardware costs and computational overhead, effectively promoting the practical application of automated agricultural machinery operations.

[0070] In the above embodiments, although the steps are numbered S1, S2, etc., they are only specific embodiments given by the present invention. Those skilled in the art can adjust the execution order of S1, S2, etc. according to the actual situation. The scheme after adjusting the order is also within the protection scope of the present invention. It can be understood that in some embodiments, some or all of the above embodiments may be included.

[0071] like Figure 4 As shown, a vehicle relative pose perception and point cloud processing system 200 according to an embodiment of the present invention includes an angle acquisition module 201, a relative travel direction determination module 202, a point cloud filtering module 203, a transformation module 204 and a control module 205. The angle acquisition module 201 is used to: acquire in real time the angle between the spray nozzle and the body of the forage harvester as measured by the encoder; The relative travel direction determination module 202 is used to: determine the relative travel direction of the hay delivery vehicle based on the included angle obtained in real time; The point cloud filtering module 203 is used to: filter the three-dimensional point cloud data in the coordinate system used by the silage harvester according to the relative travel direction of the hay truck, and obtain the local three-dimensional point cloud data corresponding to the relative travel direction of the hay truck. The transformation module 204 is used to: calculate the rotation matrix from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle based on the real-time acquired angle, and transform the local three-dimensional point cloud data to the coordinate system used by the hay receiving vehicle based on the rotation matrix; The control module 205 is used to control the material throwing operation of the nozzle based on the transformed local point cloud data.

[0072] Optionally, in the above technical solution, the relative travel direction determination module 202 is specifically used to: determine that the hay receiving vehicle is traveling to the left relative to the silage machine when the real-time angle is less than the first preset angle threshold; determine that the hay receiving vehicle is traveling to the right relative to the silage machine when the real-time angle is greater than the second preset angle threshold; and determine that the hay receiving vehicle is located directly behind the silage machine when the real-time angle is between the first preset angle threshold and the second preset angle threshold.

[0073] Optionally, in the above technical solution, the control module 205 is specifically used for: Based on the transformed local point cloud data, the target features of the hay collection vehicle are identified. Based on the target characteristics of the hay receiving vehicle, control the material throwing operation of the spray nozzle.

[0074] Optionally, in the above technical solution, the encoder is fixedly installed on the side of the turntable at the bottom of the spray nozzle, and the top gear of the encoder meshes with the turntable gear of the spray nozzle and is engaged by a spring clamping mechanism.

[0075] It should be noted that the beneficial effects of the vehicle relative pose perception and point cloud processing system 200 provided in the above embodiments are the same as those of the vehicle relative pose perception and point cloud processing method described above, and will not be repeated here. Furthermore, the system provided in the above embodiments is only illustrated by the division of the above functional modules. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the system can be divided into different functional modules according to the actual situation to complete all or part of the functions described above. In addition, the system and method embodiments provided in the above embodiments belong to the same concept, and their specific implementation process is detailed in the method embodiments, and will not be repeated here.

[0076] An electronic device according to an embodiment of the present invention includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements any of the above-described vehicle relative pose perception and point cloud processing methods.

[0077] An embodiment of the present invention provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements any of the above-described vehicle relative pose perception and point cloud processing methods.

[0078] The above description is merely a preferred embodiment of the present invention and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of disclosure in this invention is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-disclosed concept. For example, technical solutions formed by substituting the above features with (but not limited to) technical features with similar functions disclosed in this invention.

[0079] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A method for vehicle relative pose perception and point cloud processing, characterized in that, include: The angle between the spray nozzle and the forage harvester body, measured by the encoder, is obtained in real time. The relative direction of travel of the hay truck is determined based on the angle obtained in real time; Based on the relative travel direction of the hay receiving vehicle, the three-dimensional point cloud data in the coordinate system used by the silage harvester is filtered by region to obtain the local three-dimensional point cloud data corresponding to the relative travel direction of the hay receiving vehicle. Based on the angle acquired in real time, calculate the rotation matrix from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle, and transform the local three-dimensional point cloud data to the coordinate system used by the hay receiving vehicle based on the rotation matrix; The material throwing operation of the nozzle is controlled based on the transformed local point cloud data.

2. The method for vehicle relative pose perception and point cloud processing according to claim 1, characterized in that, The relative travel direction of the hay receiving vehicle is determined based on the real-time angle, including: when the real-time angle is less than a first preset angle threshold, the hay receiving vehicle is determined to be traveling to the left relative to the silage harvester; when the real-time angle is greater than a second preset angle threshold, the hay receiving vehicle is determined to be traveling to the right relative to the silage harvester; when the real-time angle is between the first preset angle threshold and the second preset angle threshold, the hay receiving vehicle is determined to be located directly behind the silage harvester.

3. A method for vehicle relative pose perception and point cloud processing according to claim 1 or 2, characterized in that, Based on the transformed local point cloud data, the material throwing operation of the nozzle is controlled, including: Based on the transformed local point cloud data, the target features of the hay-collecting vehicle are identified; Based on the target characteristics of the grass receiving vehicle, the material throwing operation of the spray nozzle is controlled.

4. A method for vehicle relative pose perception and point cloud processing according to claim 1 or 2, characterized in that, The encoder is fixedly installed on the side of the turntable at the bottom of the spray nozzle. The top gear of the encoder meshes with the turntable gear of the spray nozzle and is engaged by a spring clamping mechanism.

5. A vehicle relative pose perception and point cloud processing system, characterized in that, It includes a included angle acquisition module, a relative travel direction determination module, a point cloud filtering module, a transformation module, and a control module; The included angle acquisition module is used to: acquire in real time the included angle between the spray nozzle and the body of the forage harvester as measured by the encoder; The relative travel direction determination module is used to: determine the relative travel direction of the hay delivery vehicle based on the included angle obtained in real time; The point cloud filtering module is used to: filter the three-dimensional point cloud data in the coordinate system used by the silage harvester according to the relative travel direction of the hay receiving vehicle, and obtain the local three-dimensional point cloud data corresponding to the relative travel direction of the hay receiving vehicle. The transformation module is used to: calculate the rotation matrix from the coordinate system used by the silage harvester to the coordinate system used by the hay receiving vehicle based on the real-time acquired angle, and transform the local three-dimensional point cloud data to the coordinate system used by the hay receiving vehicle based on the rotation matrix; The control module is used to control the material throwing operation of the spray nozzle based on the transformed local point cloud data.

6. The vehicle relative pose perception and point cloud processing system according to claim 5, characterized in that, The relative travel direction determination module is specifically used to: determine that the hay receiving vehicle is traveling to the left relative to the silage machine when the real-time angle is less than the first preset angle threshold; determine that the hay receiving vehicle is traveling to the right relative to the silage machine when the real-time angle is greater than the second preset angle threshold; and determine that the hay receiving vehicle is located directly behind the silage machine when the real-time angle is between the first preset angle threshold and the second preset angle threshold.

7. A vehicle relative pose perception and point cloud processing system according to claim 5 or 6, characterized in that, The control module is specifically used for: Based on the transformed local point cloud data, the target features of the hay-collecting vehicle are identified; Based on the target characteristics of the grass receiving vehicle, the material throwing operation of the spray nozzle is controlled.

8. A vehicle relative pose perception and point cloud processing system according to claim 5 or 6, characterized in that, The encoder is fixedly installed on the side of the turntable at the bottom of the spray nozzle. The top gear of the encoder meshes with the turntable gear of the spray nozzle and is engaged by a spring clamping mechanism.

9. An electronic device, characterized in that, The method includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the vehicle relative pose perception and point cloud processing method according to any one of claims 1 to 4.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the vehicle relative pose perception and point cloud processing method according to any one of claims 1 to 4.

Images