A multi-sensor fusion positioning method and system for automatic hitching of agricultural machines

Abstract

The present application relates to a kind of multi-sensor fusion positioning method and system for automatic hitching of agricultural machinery, belong to the technical field for agricultural hitching.It includes: obtaining original ranging signal and image data;By ultra-wideband technology, the tractor and the hitch implement are ranged, and pose data is obtained;According to pose data, tractor reverses;Judge whether the tractor after reversing reaches pre-alignment point coordinate and visual recognition succeeds;Three-dimensional reconstruction is carried out to image data, and three-dimensional coordinate is obtained;Based on three-dimensional coordinate, the pose of agricultural machinery is solved, and six-degree-of-freedom pose deviation is obtained;According to six-degree-of-freedom pose deviation, control suspension mechanism to adjust pose;Judge whether the suspension mechanism after adjustment is pose aligned and contact force is detected;According to the six-dimensional moment generated by the contact of suspension mechanism and hitch point, the compliance compensation amount is obtained;Control suspension mechanism to carry out compliance insertion and lock;Judge whether the suspension mechanism after compliance insertion is inserted in place and mechanically locked.

Description

Technical Field

[0001] This invention belongs to the field of agricultural attachment technology, and in particular relates to a multi-sensor fusion positioning method and system for automatic attachment of agricultural machinery. Background Technology

[0002] The three-point suspension system is the international standard for connecting tractors and trailers, and its core function is for power transmission and work execution. However, existing trailers rely on manual operation, which has significant limitations in complex field environments and low visibility scenarios, restricting work efficiency and posing safety hazards. Automated positioning and guidance methods are needed to solve this problem.

[0003] In existing technologies, the attachment of a tractor to a trailer relies entirely on manual operation by the driver. The core process unfolds based on the structural characteristics of a three-point suspension system: First, the driver drives the tractor towards the trailer and roughly aligns it; then, they must get out of the vehicle to closely observe the relative positional deviation between the hook and the attachment point; next, they return to the cab and make fine adjustments by controlling the tractor's forward, reverse, and steering movements; until the pins at the three attachment points can be manually inserted and locked, completing the entire attachment process. This technical solution relies heavily on the driver's visual judgment and operational experience, without introducing any automated sensing, positioning, or guidance components, making it a purely manual attachment method.

[0004] In addition, the existing manual coupling has many drawbacks: it is inefficient and requires a high level of driver experience; at the same time, it is labor-intensive and requires frequent getting on and off the vehicle; furthermore, it lacks precision and is prone to loose coupling or equipment wear. Summary of the Invention

[0005] In view of the shortcomings of the existing technology, the purpose of the invention is to provide a multi-sensor fusion positioning method and system for automatic attachment of agricultural machinery, taking into account the technical problems of low efficiency, high requirements for the driver's operating experience, high labor intensity, frequent getting on and off the vehicle, and insufficient accuracy, which can easily lead to loose attachment or equipment wear.

[0006] In a first aspect, the present invention provides a multi-sensor fusion positioning method for automatic attachment of agricultural machinery, the method comprising: S1, acquire the original distance measurement signal between the tractor and the attach implement, as well as the image data of the attach implement; S2, using ultra-wideband technology, measures the distance between the tractor and the attach implement based on the original ranging signal to obtain position and pose data; S3, based on the pose data, generate the reversing path of the tractor and drive the tractor to reverse along the reversing path; S4. Determine whether the tractor has reached the pre-aligned point coordinates after the reversing operation and whether the visual recognition is successful; if yes, proceed to step S5; otherwise, return to step S2. S5, perform stereo matching and 3D reconstruction on the image data to obtain 3D coordinates; S6, based on three-dimensional coordinates, performs pose calculations on the tractor and the attaching device to obtain the six-degree-of-freedom pose deviation; S7 controls the tractor's suspension mechanism to adjust its posture based on the six-degree-of-freedom posture deviation. S8, determine whether the suspension mechanism after posture adjustment is in posture alignment and whether contact force is detected; if yes, proceed to step S9; otherwise, return to step S5. S9, based on the real-time six-dimensional torque generated by the contact between the suspension mechanism and the contact point, the compliance compensation amount is obtained through the expected impedance model; S10, based on the compliance compensation amount, control the suspension mechanism to perform compliance engagement, and lock the suspension mechanism after compliance engagement; S11, determine whether the suspension mechanism after compliant insertion is properly inserted and mechanically locked; if yes, the agricultural machinery is automatically attached; otherwise, return to step S10.

[0007] Furthermore, S1 specifically includes: S101 uses ultra-wideband technology to acquire the raw ranging signals between multiple tags between the tractor and the attach implement; S102 acquires image data of the attachment equipment through a binocular camera.

[0008] Furthermore, S2 specifically includes: S201 uses ultra-wideband technology to deploy an ultra-wideband base station at the tractor end and set a tag at the attachment end; S202 uses the two-way time-of-flight method to collect raw ranging signals between multiple base stations and multiple tags, and obtains the distance data between each base station and each tag; S203, Based on the distance data, calculate the two-dimensional coordinates of each label using triangulation; S204, Calculate the coordinates and yaw angle of the attachment tool based on the two-dimensional coordinates and preset geometric dimensions; S205, combined with coordinates and yaw angle, yields pose data.

[0009] Furthermore, S3 specifically includes: S301, based on the pose data and the preset reversing distance, calculates the coordinates of the pre-alignment point and the direction of the tractor's front end; S302 uses the Hybrid A* algorithm to generate a reversing path by combining the tractor's pose, pre-alignment point coordinates, and vehicle orientation. The S303, through the navigation controller, drives the tractor to reverse along the reversing path.

[0010] Furthermore, S5 specifically includes: S501 uses the AprilTag detection algorithm to extract the sub-pixel coordinates of multiple corner points in the image data; S502 calculates the disparity of each corner point based on the sub-pixel coordinates of the corner points in the left and right cameras; S503, combining parallax, camera parameters, and the baseline distance of the binocular cameras, calculates the three-dimensional coordinates of each corner point in the left camera coordinate system.

[0011] Furthermore, S6 specifically includes: S601 uses the PnP algorithm to solve for the rotation matrix and translation vector from the preset coordinates to the three-dimensional coordinates, thus obtaining the pose transformation relationship from the preset coordinates to the three-dimensional coordinates. S602, the pose transformation relationship is transformed using fixed coordinates to obtain the six-degree-of-freedom pose deviation.

[0012] Furthermore, S7 specifically includes: S701, which generates fine-tuning commands based on the six-degree-of-freedom pose deviation and the reversing kinematics model; S702, according to the fine-tuning command, drives the suspension mechanism to make adjustments and adjusts the positional deviation in real time.

[0013] Furthermore, S9 specifically includes: S901 uses a six-dimensional force sensor to obtain the real-time six-dimensional torque generated by the contact between the suspension mechanism and the mounting point; S902 performs filtering and coordinate system transformation on real-time six-dimensional torque through a coordinate system transformation matrix; S903, based on preset impedance control parameters, inputs the converted six-dimensional torque into the desired impedance model to obtain the compliance compensation amount.

[0014] Furthermore, S10 specifically includes: S1001 converts compliance compensation amount into compliance motion correction command; S1002, according to the compliant motion correction command, manipulate the suspension mechanism to perform compliant engagement, and lock the suspension mechanism after compliant engagement.

[0015] In a second aspect, the present invention provides a multi-sensor fusion positioning system for automatic attachment of agricultural machinery, comprising: a memory and a processor; The memory stores an application program adapted to be executed by the processor to implement the multi-sensor fusion positioning method for automatic attachment of agricultural machinery as described in the first aspect.

[0016] The beneficial effects of this invention are as follows: In this embodiment of the invention, the attachment of the agricultural machinery is automatically adjusted based on the pose data, the six-degree-of-freedom pose deviation, and the compliance compensation amount, which improves efficiency, requires less operating experience from the driver, and reduces labor intensity. In addition, the agricultural machinery is adjusted multiple times based on multiple calculation results, and the attachment is judged multiple times to improve the attachment accuracy and prevent the attachment from becoming loose or the equipment from wearing out. Attached Figure Description

[0017] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts. It is obvious that the drawings described below are merely some embodiments of the present invention, and those skilled in the art can obtain other drawings based on these drawings.

[0018] Figure 1 This is a flowchart illustrating a multi-sensor fusion positioning method for automatic attachment of agricultural machinery provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of a multi-sensor fusion positioning system for automatic attachment of agricultural machinery provided in an embodiment of the present invention. Detailed Implementation

[0019] To enable those skilled in the art to better understand the technical solutions in the embodiments of the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. It should be understood that these descriptions are merely exemplary and are not intended to limit the scope of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0020] Furthermore, descriptions of well-known structures and techniques are omitted in the following description to avoid unnecessarily obscuring the concepts disclosed in this invention.

[0021] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicating orientation or positional relationships based on the orientation or positional relationships shown in the accompanying drawings, are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance. The terms "installed," "connected," and "linked" should be interpreted broadly; for example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal communication of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0022] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numerals in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Rather, they are merely examples of methods and systems consistent with some aspects of the invention as detailed in the appended claims.

[0023] This invention proposes a multi-sensor fusion positioning method and system for automatic attachment of agricultural machinery, which solves the technical problems of low efficiency, high requirements for driver experience, high labor intensity, frequent getting on and off the vehicle, and insufficient accuracy, which can easily lead to loose attachment or equipment wear.

[0024] Method Implementation Examples Reference Figure 1 The diagram shows a flowchart of a multi-sensor fusion positioning method for automatic attachment of agricultural machinery provided by an embodiment of the present invention.

[0025] This invention provides a multi-sensor fusion positioning method for automatic attachment of agricultural machinery, characterized in that the agricultural machinery includes a tractor and an attachment device, and the method includes: Specifically, the method includes steps S1 to S11.

[0026] S1, acquire the original distance measurement signal between the tractor and the attach implement, as well as the image data of the attach implement.

[0027] In one possible implementation, S1 specifically includes sub-steps S101 and S102: S101 uses ultra-wideband technology to acquire raw ranging signals between multiple tags between the tractor and the attach implement.

[0028] Ultra-wideband (UWB) technology refers to a wireless communication technology that uses nanosecond-level non-sinusoidal narrow pulses to transmit data. It features high time resolution, strong resistance to multipath interference, and good penetration, making it particularly suitable for high-precision ranging and positioning.

[0029] It should be noted that, relying on the UWB positioning system's anti-occlusion and anti-multipath interference characteristics, the original signal is obtained through multi-tag redundant ranging. This not only ensures the stability of signal acquisition in complex field environments, but also provides multi-dimensional data support for subsequent pose calculation, thereby improving the accuracy and reliability of long-distance coarse positioning.

[0030] S102 acquires image data of the attachment equipment through a binocular camera.

[0031] It should be noted that, with the help of the stereo imaging characteristics of binocular cameras, high-definition visual data of the attachment equipment can be collected simultaneously, adapting to field lighting fluctuations and partially obscured scenes. This provides high-fidelity raw materials for subsequent AprilTag corner point extraction, 3D reconstruction, and millimeter-level pose calculation. Moreover, the non-contact acquisition does not interfere with the movement of the attachment mechanism, ensuring the continuity and security of data acquisition.

[0032] In this embodiment of the invention, multi-source data fusion acquisition through UWB positioning system and binocular camera not only utilizes the anti-occlusion and anti-interference characteristics of UWB to obtain stable ranging signals, but also acquires high-fidelity image data through binocular camera, achieving complementarity between long-distance positioning and close-range visual perception. This provides comprehensive and reliable raw data support for subsequent coarse positioning, fine guidance and compliant splicing, and is suitable for complex field operation environments.

[0033] S2 uses ultra-wideband technology to measure the distance between the tractor and the attach implement based on the original ranging signal, and obtains the position and posture data.

[0034] In one possible implementation, S2 specifically includes sub-steps S201 to S205: S201 uses ultra-wideband technology to deploy an ultra-wideband base station at the tractor end and set a tag at the attachment end.

[0035] Specifically, the UWB tag arrangement involves placing three UWB tags (Tag A, Tag B, and Tag C) on the rigid body of the machine, forming an isosceles triangle ABC of known dimensions. Tag A and Tag B are defined as vertices A and B of the base of isosceles triangle ABC, respectively, and Tag C is a vertex. The length of the base is L. AB The height H to vertex C CThe exact location is known. The plane containing this triangle is approximately perpendicular to the ground, the base AB is parallel to the transverse axis of the machine, and point C points directly in front of the machine (i.e., the direction of the attachment point).

[0036] It should be noted that by using ultra-wideband technology, which meets the needs of tractor-assisted active positioning of attachments, the anti-obstruction and anti-multipath interference characteristics of ultra-wideband technology are adapted to the complex field environment. The separate deployment of base stations and tags not only ensures the rationality of the positioning link, but also provides data redundancy through multi-node configuration, laying a solid hardware foundation for the subsequent stable acquisition of ranging signals and the realization of long-distance accurate coarse positioning.

[0037] S202 uses the two-way time-of-flight method to collect raw ranging signals between multiple base stations and multiple tags, and obtains the distance data between each base station and each tag.

[0038] Among them, the Two-Way Time of Flight (TW-TOF) method is the core method for achieving high-precision ranging and identification between modules in ultra-wideband (UWB) technology. It is a method for calculating distance by measuring the round-trip time of a signal between two points, without requiring strict synchronization of the clocks at the transmitting and receiving ends.

[0039] Specifically, each UWB base station on the tractor performs TW-TOF ranging with each tag. For any base station-tag pair, the distance D is calculated using the following formula: Where D represents distance (unit: meters, m), and c represents the speed of light (3 × 10⁻⁶). 8 m / s), T flight This indicates the one-way flight time (unit: seconds, s).

[0040] Specifically, the formula for calculating one-way flight time is: Among them, T round T represents the total round-trip time of the signal. reply This indicates a delay in the label fixing process.

[0041] It should be noted that the two-way time-of-flight method does not require strict clock synchronization between the transmitting and receiving ends, has centimeter-level ranging accuracy, and the many-to-many acquisition by multiple base stations and multiple tags forms data redundancy, effectively resisting field occlusion and multipath interference, providing a stable and accurate distance data foundation for subsequent pose calculation.

[0042] S203. Based on the distance data, the two-dimensional coordinates of each label are calculated using the triangulation method.

[0043] Specifically, the tag coordinate calculation utilizes a UWB base station installed at the rear of the tractor to calculate the two-dimensional coordinates (x, y, y) of Tag A, Tag B, and Tag C in the tractor coordinate system using triangulation. A , y A ),(x B , y B ),(x C , y C Height information can be temporarily ignored since the subsequent focus will be on planar path planning.

[0044] It should be noted that the triangulation method relies on redundant distance data from multiple base stations and multiple tags to achieve two-dimensional coordinate positioning of the tags through spatial geometric calculation. This can effectively offset single-point ranging errors, improve the anti-interference capability and accuracy of coordinate calculation, and provide a reliable two-dimensional position reference for the global pose estimation of subsequent attachment equipment, thus adapting to the positioning needs in complex field environments.

[0045] S204, calculate the coordinates and yaw angle of the attachment tool based on the two-dimensional coordinates and preset geometric dimensions.

[0046] Specifically, the reference center point O for calculating the coordinates of the attachment equipment. im (For example, the coordinates of the midpoint M of line segment AB in the tractor coordinate system) , Taking point M as an example, the specific formula is: in, Indicates the reference center point O im In a Cartesian coordinate system, the x-axis coordinate is x A The x-axis coordinates and y-axis coordinates of feature point A are represented by... A The x-axis coordinate of feature point A is represented by x. B The x-coordinate and y-coordinate of feature point B represent the coordinates of the feature point B. B This represents the y-coordinate of feature point B. Indicates the reference center point O im The y-axis coordinate in a Cartesian coordinate system.

[0047] Specifically, calculating the yaw angle of the attaching equipment is crucial for achieving coarse attitude alignment, primarily involving calculating the equipment's orientation vector. The formula for calculating the base vector of the attaching equipment is: Among them, V AB This represents the bottom vector of the attachment equipment.

[0048] The theoretical formula for calculating the forward vector is: in, H represents the theoretical forward vector from M to C. C This represents the vertical direction parameter.

[0049] Specifically, the rotation angle is calculated by measuring V. AB and its theoretical value (L) AB The correspondence between , 0) or directly obtained through measurement (x C , y C The relationship between point M and point M allows us to calculate the planar yaw angle θ of the implement relative to the tractor coordinate system. The specific formula is as follows: Where θ represents the plane yaw angle, This represents the arctangent function in the four quadrants, returning the azimuth angle of the line connecting the coordinate point and the origin, x. C The x-coordinate and y-coordinate of feature point C represent the coordinates of the feature point C. C This represents the y-axis coordinate of feature point C.

[0050] It should be noted that those skilled in the art can set the preset geometric dimensions according to actual needs, and this invention does not limit this.

[0051] It should be noted that by reusing the two-dimensional coordinates of the preceding label and the preset geometric dimensions of the attached implement, the coordinates and yaw angle of the implement can be calculated simultaneously without additional sensors. This reduces system complexity and cost, and the structural constraints of the implement offset single-point ranging errors, providing a complete and stable pose reference for subsequent path planning and accurately adapting to the needs of coarse positioning scenarios in the field.

[0052] S205, combined with coordinates and yaw angle, yields pose data.

[0053] It should be noted that by efficiently integrating the previously calculated machine coordinates and yaw angles, complete pose data can be output without additional sensing equipment. This ensures data consistency and continuity, and provides a precise global pose reference for subsequent path planning, thus meeting the core needs of long-distance coarse positioning in the field.

[0054] In this embodiment of the invention, ultra-wideband technology is used as the core. Through a layered design of base station and tag deployment, two-way time-of-flight ranging, and triangulation calculation, combined with the preset geometric dimensions of the equipment, pose integration is achieved. It not only relies on multi-node redundant acquisition to resist field occlusion interference, but also ensures centimeter-level positioning accuracy through multi-algorithm collaboration. It can output complete and stable pose data without additional complex equipment, providing highly reliable support for long-distance coarse positioning.

[0055] S3, based on the pose data, generates the reversing path of the tractor and drives the tractor to reverse along the reversing path.

[0056] In one possible implementation, S3 specifically includes sub-steps S301 to S303: S301 calculates the coordinates of the pre-alignment point and the direction the tractor is facing based on the pose data and the preset reversing distance.

[0057] Specifically, the target of path planning is not the machine center point O. im Instead, it is a pre-alignment point located directly behind the implement, where the tractor can easily begin visual guidance. At this point, the tractor's front end should be facing exactly opposite to the implement's direction, i.e., reverse alignment. The coordinates of this point (x...) goal, y goal ) and the direction the car is facing ψ goal According to the machine's position (x) o , y o Calculate θ: Where, x goal The x-axis coordinates and y-axis coordinates represent the target positions that the agricultural machinery needs to reach. goal This represents the y-axis coordinate of the target position that the agricultural machinery needs to reach. This indicates the target heading angle that the agricultural machinery needs to be adjusted to, d. back θ represents the preset backward offset distance that the agricultural machinery needs to perform, θ represents the current real-time heading angle of the agricultural machinery, π represents pi, cos represents the cosine function, and sin represents the sine function.

[0058] It should be noted that those skilled in the art can set the preset reversing distance according to actual needs, and this invention does not limit this.

[0059] It should be noted that by reusing the preceding ultra-wideband precise pose data and combining it with the preset reversing distance to scientifically plan the pre-alignment point and the direction of the vehicle head, it is possible to ensure that the tractor approaches the attaching device accurately and lay the foundation for the optimal initial position for subsequent visual guidance and connection, thereby reducing the amount of subsequent adjustments and improving the efficiency and stability of the attachment process.

[0060] S302 uses the Hybrid A* algorithm to generate a reversing path by combining the tractor's pose, pre-alignment point coordinates, and the vehicle's orientation.

[0061] Hybrid A* algorithm refers to a hybrid heuristic path planning technique oriented towards motion constraints. It is a path planning algorithm specifically designed for mobile vehicles in continuous space with nonholonomic constraints.

[0062] Specifically, path generation: The path planning module (using algorithms such as Hybrid A* that consider vehicle kinematics) starts from the tractor's current pose and... , , The process generates an executable, smooth reversing path, with the destination being the endpoint.

[0063] Furthermore, the navigation controller takes over, translating the path into specific instructions for the tractor's steering, throttle, and braking systems.

[0064] It should be noted that the Hybrid A* algorithm can accurately adapt to the nonholonomic motion constraints of tractors. By combining the preceding high-precision pose data, pre-alignment point coordinates, and tractor head orientation, it generates a smooth and executable reversing path. This not only avoids interference from complex terrain in the field but also guides the tractor to efficiently reach the optimal pre-alignment position, reducing adjustment redundancy for subsequent visual fine guidance and improving the continuity and reliability of the coupling process.

[0065] The S303, through the navigation controller, drives the tractor to reverse along the reversing path.

[0066] Specifically, the tractor chassis executes the above instructions to perform autonomous reversing until it reaches the vicinity of the pre-alignment point and meets the stage switching conditions.

[0067] It should be noted that the navigation controller can accurately execute the optimal path generated by the Hybrid A* algorithm, stably drive the tractor to reverse precisely in complex field terrain, ensure efficient arrival at the pre-alignment point, provide stable initial conditions for visual precision guidance and connection, and improve the controllability and continuity of the coupling process.

[0068] In this embodiment of the invention, relying on the preceding ultra-wideband precise pose data, the optimal path adapted to the tractor's motion constraints is generated by scientifically planning the pre-alignment point and the Hybrid A* algorithm, and then precisely driven by the navigation controller. This not only ensures the accuracy and safety of reversing in complex field terrain, but also efficiently connects with visual precision guidance, greatly improving the automation level and overall efficiency of the coupling process.

[0069] S4: Determine whether the tractor has reached the pre-aligned point coordinates after the reversing operation and whether visual recognition is successful. If yes, proceed to step S5. Otherwise, return to step S2.

[0070] Specifically, successful visual recognition means that the binocular vision system (camera) at the back end of the agricultural machinery recognizes the image of a high-precision visual marker (such as an AprilTag, which is a black and white grid similar to a QR code) on the machinery and obtains the pose of the marker.

[0071] The criteria for successful visual recognition include: continuous and stable detection of key features (such as marker points and boundary lines); high consistency in position estimation across multiple frames; and alignment error between the predicted trajectory and the target position of less than 5 cm / 2 degrees.

[0072] Once the recognition is successful, the six-degree-of-freedom pose of the implement reference point can be calculated and returned, and the relative position of the agricultural machinery rear suspension mechanism to the implement reference point can be determined in real time.

[0073] In this embodiment of the invention, a dual verification mechanism of location compliance and visual recognition is adopted, combined with closed-loop feedback logic. If the standard is not met, the system automatically returns to the previous positioning steps for recalibration. This ensures that the initial conditions for entering the fine guidance stage are accurate and reliable, improves the process's fault tolerance and anti-interference ability, effectively avoids invalid operations, and guarantees the stability and success rate of the connection process.

[0074] S5 performs stereo matching and 3D reconstruction on the image data to obtain 3D coordinates.

[0075] In one possible implementation, S5 specifically includes sub-steps S501 to S503: S501 uses the AprilTag detection algorithm to extract the sub-pixel coordinates of multiple corner points in image data.

[0076] The AprilTag detection algorithm is a real-time, robust target detection and pose estimation algorithm based on visual reference markers (structured patterns similar to QR codes).

[0077] Specifically, images are simultaneously captured by the left and right cameras, and the image data undergoes preprocessing including grayscale conversion, noise reduction, and illumination normalization. The AprilTag detection algorithm is then run on the preprocessed images to obtain the sub-pixel coordinates of the four corner points of the marker in the left and right images. Let the corner point coordinates in the left image be... The coordinates of the corresponding corner point in the right figure are: i=1,2,3,4 This represents the column coordinates of the i-th feature point at the top corner of the left camera image. This represents the column coordinates of the i-th feature point on the right camera image corresponding to the corner point coordinates. This represents the row coordinate of the i-th feature point in the left camera image. This represents the row coordinate of the i-th feature point on the right camera image corresponding to the corner point coordinates.

[0078] It should be noted that the AprilTag detection algorithm has strong robustness and high accuracy, effectively resisting field light fluctuations and partial occlusion interference, and accurately extracting corner point sub-pixel coordinates. This provides ultra-high precision feature data support for subsequent 3D reconstruction and PnP pose calculation, ensuring the foundation for millimeter-level precision guidance.

[0079] S502 calculates the disparity of each corner point based on the sub-pixel coordinates of the corner points in the left and right cameras.

[0080] Specifically, for each pair of corner points, the formula for calculating disparity is: in, Let represent the disparity of the i-th feature point.

[0081] It should be noted that by relying on the sub-pixel-level high-precision coordinates of the left and right camera corners to calculate parallax, the advantages of binocular stereo imaging are fully utilized. This not only improves the accuracy of parallax calculation but also resists interference from field lighting fluctuations, providing reliable depth information for subsequent three-dimensional coordinate solving of corners and laying a solid foundation for millimeter-level alignment guided by vision.

[0082] S503, combining parallax, camera parameters, and the baseline distance of the binocular cameras, calculates the three-dimensional coordinates of each corner point in the left camera coordinate system.

[0083] Specifically, using the camera intrinsic parameter matrix K and the baseline B, the three-dimensional coordinates of the corner points in the left camera coordinate system are calculated. The formula for calculating the three-dimensional coordinates is as follows: in, This represents the three-dimensional coordinate vector of the i-th feature point in the left camera coordinate system. This represents the x-axis coordinate of the i-th feature point in the left camera coordinate system. This represents the y-coordinate of the i-th feature point in the left camera coordinate system. Let f represent the z-axis coordinate of the i-th feature point in the left camera coordinate system, B represent the baseline distance of the stereo cameras, and f x f represents the x-axis focal length of a stereo camera. y d represents the y-axis focal length of a stereo camera. i Represents the disparity of the i-th feature point. This represents the row coordinates of the i-th feature point in the left camera image.

[0084] Specifically, the binocular camera has a baseline distance of 0.3m to 0.5m from B, is mounted at the rear of the tractor, and faces rearward. A global shutter camera is used, with a frame rate greater than or equal to 30fps.

[0085] It should be noted that by integrating the preceding subpixel-level parallax data, the calibrated camera parameters, and the fixed baseline distance, the three-dimensional coordinates of the corner points are accurately solved using the principle of stereo vision. This ensures millimeter-level positioning accuracy and resists interference from field lighting, providing highly reliable data support for subsequent relative pose calculations and laying a solid foundation for the core of visual precision guidance.

[0086] In this embodiment of the invention, relying on the AprilTag algorithm and binocular stereo vision technology, three-dimensional reconstruction is achieved through sub-pixel level corner point extraction, accurate parallax calculation and calibration parameter fusion. This not only resists field lighting fluctuations and occlusion interference, but also outputs millimeter-level high-precision three-dimensional coordinates, providing highly reliable data support for subsequent relative pose calculation, efficiently replacing manual visual judgment, and adapting to complex field precision guidance scenarios.

[0087] S6, based on three-dimensional coordinates, performs pose calculations on the tractor and the attaching device to obtain the six-degree-of-freedom pose deviation.

[0088] In one possible implementation, S6 specifically includes sub-steps S601 and S602: S601 uses the PnP algorithm to solve for the rotation matrix and translation vector from the preset coordinates to the three-dimensional coordinates, thus obtaining the pose transformation relationship from the preset coordinates to the three-dimensional coordinates.

[0089] Among them, PnP (Perspective-n-Point) refers to the problem in computer vision of solving the pose (rotation and translation) of the camera relative to a set of 3D points and their 2D projections on an image.

[0090] Specifically, the world coordinates of the four corner points of the marker in the marker coordinate system. Find a rotation matrix R and a translation vector T such that The optimality holds for all i. Where, Let R represent the three-dimensional coordinate vector of the i-th feature point in the target coordinate system, and let R represent the rotation matrix. Let represent the 3D coordinate vector of the i-th feature point in the source coordinate system, and T represent the translation vector. This yields the transformation from the marker coordinate system to the left camera coordinate system. After a fixed coordinate transformation, the 6-DOF pose of the implement attachment point relative to the end of the tractor suspension mechanism can be obtained, including translation (Δx, Δy, Δz) and rotation (Euler angles (Δroll, Δpitch, Δyaw)), thus obtaining the transformation from the marker coordinate system to the left camera coordinate system.

[0091] It should be noted that those skilled in the art can set preset coordinates for judgment according to actual needs, and this invention does not limit such settings.

[0092] It should be noted that the PnP algorithm is robust and efficient. Based on the preceding millimeter-level high-precision three-dimensional coordinates and preset coordinates, it can accurately calculate the rotation matrix and translation vector, directly outputting a clear pose transformation relationship without additional complex calculations. It is also resistant to field lighting interference, providing core pose information for the subsequent precise alignment of the tractor and the attaching device, ensuring millimeter-level control accuracy in the fine guidance stage.

[0093] S602, the pose transformation relationship is transformed using fixed coordinates to obtain the six-degree-of-freedom pose deviation.

[0094] It should be noted that by using a fixed coordinate transformation to unify the reference system, the preceding precise pose transformation relationship is transformed into a six-degree-of-freedom pose deviation, which fully covers position and attitude errors. This not only ensures the accuracy and intuitiveness of deviation calculation, but also provides a clear control basis for the precise adjustment of tractors, resists interference from the field environment, and lays a solid control foundation for millimeter-level alignment.

[0095] In this embodiment of the invention, relying on millimeter-level high-precision three-dimensional coordinates, the robust PnP algorithm is used to accurately calculate the six-degree-of-freedom pose deviation, comprehensively reflecting the spatial relative position difference between the tractor and the attach implement. It is resistant to interference from field lighting and shading, and can provide accurate adjustment basis without manual intervention, laying a solid core foundation for subsequent millimeter-level alignment control and greatly improving the automation level and alignment accuracy of the fine guidance stage.

[0096] S7 controls the tractor's suspension mechanism to adjust its posture based on the six-degree-of-freedom posture deviation.

[0097] In one possible implementation, S7 specifically includes sub-steps S701 and S702: S701 generates fine-tuning commands based on the six-degree-of-freedom pose deviation and the reversing kinematics model.

[0098] Specifically, the servo controller takes over and converts the pose deviation into extension / retraction control commands for each hydraulic cylinder of the three-point suspension mechanism. The control module uses the calculated pose deviation, especially the lateral deviation Δy and the heading angle deviation Δyaw, as feedback. Combined with the reversing kinematic model, it generates fine-tuning commands for the tractor's rear wheel steering angle (or articulated steering angle) and the suspension mechanism's hydraulic cylinders, forming a closed-loop control that brings the deviation close to zero.

[0099] It should be noted that, based on the precise six-degree-of-freedom pose deviation of the preceding sequence, and combined with the kinematic model of the tractor reversing, targeted fine-tuning instructions are generated. This not only strictly adapts to the motion characteristics of the equipment, but also accurately compensates for spatial position and attitude deviations, resists interference from the field environment, provides a reliable control basis for millimeter-level alignment, and significantly improves the adjustment accuracy and response efficiency in the fine guidance stage.

[0100] S702, according to the fine-tuning command, drives the suspension mechanism to make adjustments and adjusts the positional deviation in real time.

[0101] Specifically, the suspension actuator (hydraulic cylinder) drives the suspension mechanism to make fine adjustments in up and down, left and right, forward and backward, as well as pitch and roll, so that the hook is precisely aligned with the engagement point until the stage switching conditions are met.

[0102] It should be noted that, based on the preceding precise fine-tuning commands, the drive suspension mechanism makes targeted dynamic adjustments, and at the same time, corrects the positional deviation through real-time closed-loop correction, which not only strictly adapts to the tractor's motion characteristics, but also dynamically offsets interference from the field environment.

[0103] In this embodiment of the invention, based on the six-degree-of-freedom pose deviation with millimeter-level precision, the suspension mechanism is directly driven to make targeted pose adjustments. This not only strictly matches the core requirements of the attachment but also avoids errors caused by human intervention. It has strong resistance to interference from the field environment and can achieve precise alignment between the tractor and the attachment implement, greatly improving the accuracy, stability and automation success rate of the attachment operation.

[0104] S8. Determine whether the suspension mechanism is aligned after pose adjustment and whether contact force is detected. If yes, proceed to step S9. Otherwise, return to step S5.

[0105] In this embodiment of the invention, a dual verification logic of pose alignment and contact force detection is adopted, combined with a closed-loop feedback mechanism. When the standard is not met, the system automatically returns to the visual fine guidance stage for recalibration. This not only avoids splicing failure caused by false alignment, but also enhances the process's resistance to field interference, ensuring the accuracy, safety, and final success rate of splicing operations.

[0106] S9, based on the real-time six-dimensional torque generated by the contact between the suspension mechanism and the engagement point, obtains the compliance compensation amount through the desired impedance model.

[0107] In one possible implementation, S9 specifically includes sub-steps S901 to S903: The S901 uses a six-dimensional force sensor to acquire the real-time six-dimensional torque generated by the contact between the suspension mechanism and the mounting point.

[0108] Specifically, the six-dimensional force / torque sensor measures real-time six-dimensional torque, and the specific formula is as follows: Where F represents the real-time six-dimensional torque, F x F represents the force component along the x-axis in space. y F represents the force component along the y-axis in space. z M represents the force component along the z-axis in space. x M represents the torque component about the x-axis in space. y M represents the torque component about the y-axis in space.z Represents the torque component about the z-axis in space, [ ] T This represents the vector transpose operator.

[0109] It should be noted that the use of a six-dimensional force sensor to accurately capture the real-time six-dimensional torque of the contact between the suspension mechanism and the attachment point can provide comprehensive feedback on the contact force and attitude distribution, which not only avoids equipment damage caused by overload, but also provides high-precision force feedback for subsequent locking operations, resists interference from complex field conditions, and ensures the safety and stability of the attachment process.

[0110] S902 performs filtering and coordinate system transformation on real-time six-dimensional torque through a coordinate system transformation matrix.

[0111] Specifically, the measured real-time six-dimensional torque F is transformed into the control coordinate system {E} of the suspension end effector to obtain the real-time torque F experienced by the coupling mechanism when it contacts the coupling point. e .

[0112] It should be noted that by synchronously achieving torque filtering and coordinate unification through the coordinate system transformation matrix, noise interference caused by field vibration and contact impact is effectively eliminated, and torque data is mapped to the target reference system, providing accurate and stable force feedback for subsequent locking control, avoiding splicing damage caused by force data distortion, and ensuring the safety and reliability of the splicing and locking process.

[0113] S903, based on preset impedance control parameters, inputs the converted six-dimensional torque into the desired impedance model to obtain the compliance compensation amount.

[0114] Impedance control refers to a robot force control strategy that establishes a dynamic relationship between the position / velocity of the end effector and the contact force (such as a mass-damping-spring model) to make it exhibit the desired compliance in a contact environment.

[0115] Specifically, the impedance controller runs an impedance control algorithm to calculate in real time a motion compensation amount that enables the end effector to exhibit "compliant" behavior based on the contact force. The formula for calculating the desired dynamics is as follows: Among them, M d B represents the inertia of the expectation of a diagonal positive definite matrix. d K represents the damping of the expectation of a diagonal positive definite matrix. d This represents the stiffness of the expected value of a diagonal positive definite matrix. This represents the desired acceleration vector of the coupling mechanism. This represents the real-time acceleration vector of the coupling mechanism. This represents the desired velocity vector of the coupling mechanism. This represents the real-time velocity vector of the coupling mechanism. This represents the desired pose vector of the coupling mechanism. F represents the real-time pose vector of the coupling mechanism. e This indicates the real-time torque experienced by the coupling mechanism when it comes into contact with the coupling point.

[0116] For example, desired inertia, damping, and stiffness are key adjustable parameters, such as... Set the stiffness in each direction, where diag represents the diagonal matrix generation operator.

[0117] It should be noted that those skilled in the art can set preset impedance control parameters according to actual needs, and this invention does not limit such settings.

[0118] It should be noted that, based on the preset impedance control parameters and the desired impedance model, the six-dimensional torque after filtering and calibration is accurately mapped, and the output is a compliance compensation amount adapted to the hanging scenario. This not only achieves flexible buffering of the contact force to avoid equipment damage caused by rigid impact, but also dynamically adapts to complex contact conditions in the field.

[0119] In this embodiment of the invention, a six-dimensional force sensor is used to accurately capture the six-dimensional contact torque. After filtering, noise reduction and coordinate unification calibration, a compliance compensation amount is output through the desired impedance model. This achieves flexible buffering of the contact force to avoid rigid impact damage, has strong anti-vibration interference capability, and greatly improves operational safety and connection reliability.

[0120] S10, based on the compliance compensation amount, control the suspension mechanism to perform compliance engagement, and lock the suspension mechanism after compliance engagement.

[0121] In one possible implementation, S10 specifically includes sub-steps S1001 and S1002: S1001 converts compliance compensation amount into compliance motion correction command.

[0122] Specifically, the servo controller generates compliant micro-motion commands for the suspension hydraulic cylinder based on the output of the impedance controller. Simultaneously, the lock controller monitors the status and prepares to trigger the lock. The formula for calculating the acceleration correction vector used for compliance compensation is as follows: in, This represents the acceleration correction vector. This represents the inverse of the desired inertia matrix.

[0123] Furthermore, the final position or speed correction command ΔX or ΔẊ is obtained by integration (or by directly superimposing it on the speed loop and position loop) and sent to the servo drive.

[0124] It should be noted that the previously precisely calibrated compliance compensation amount is efficiently converted into a compliant motion correction command that adapts to the control logic of the suspension mechanism. This not only achieves a precise connection between force control requirements and motion execution, but also avoids rigid impacts through flexible correction, dynamically adapts to complex contact conditions in the field, provides stable motion support for coupling and locking, and ensures operational safety and coupling consistency.

[0125] S1002, according to the compliant motion correction command, manipulate the suspension mechanism to perform compliant engagement, and lock the suspension mechanism after compliant engagement.

[0126] Specifically, the suspension actuator performs a compliant micro-motion, guiding the hook to slide into the engagement point. Immediately afterwards, the locking actuator (such as an electromagnetic connector) actuates to complete the rigid mechanical locking.

[0127] Furthermore, the servo system executes the command superimposed with compliance compensation. For example, when the plug contacts the edge of the orifice, a lateral force F is generated. y At that time, according to the impedance model, the controller will instruct the actuator to reduce F. y The direction of movement guides the insertion along the "path of least force" into the hole. When F is continuously detected... z Once the expected load capacity is reached and forces / torques in other directions approach zero, and the position sensor detects that the insertion has reached the mechanical locking position, the system determines that the docking is successful, triggers the locking pin, and ends the process.

[0128] It should be noted that, based on the preceding precise and compliant motion correction commands, the suspension mechanism achieves flexible insertion operation, effectively avoiding equipment wear and impact damage caused by rigid contact. At the same time, the locking mechanism ensures the firmness of the connection, dynamically adapts to complex contact conditions and vibration interference in the field, and ensures the safety, stability and final connection reliability of the insertion operation.

[0129] In this embodiment of the invention, the suspension mechanism is flexibly connected based on the precise compliance compensation amount, which avoids the impact and wear of rigid contact on the equipment, and ensures the connection firmness through the locking mechanism. It is suitable for field vibration and complex contact conditions, and takes into account the operational safety, connection stability and long-term connection reliability. Stable connection can be completed without manual intervention.

[0130] S11, determine whether the suspension mechanism after compliant insertion is properly inserted and mechanically locked. If yes, the automatic attachment of the agricultural machinery is completed. Otherwise, return to step S10.

[0131] In this embodiment of the invention, a dual verification and closed-loop feedback logic of plug-in positioning and mechanical locking is adopted. If the standard is not met, the automatic retraction and compliant plug-in stage is recalibrated. This ensures the thoroughness of the connection and the stability of the structure, avoids the risk of loosening caused by vibration during field operations, ensures the safety of subsequent agricultural machinery operations, and further improves the integrity and ultimate reliability of the closed-loop control of automatic connection.

[0132] The beneficial effects of this invention are as follows: In this embodiment of the invention, the attachment of the agricultural machinery is automatically adjusted based on the pose data, the six-degree-of-freedom pose deviation, and the compliance compensation amount, which improves efficiency, requires less operating experience from the driver, and reduces labor intensity. In addition, the agricultural machinery is adjusted multiple times based on multiple calculation results, and the attachment is judged multiple times to improve the attachment accuracy and prevent the attachment from becoming loose or the equipment from wearing out.

[0133] System Implementation Examples Reference manual attached Figure 2 The diagram shows a schematic of a multi-sensor fusion positioning system for automatic attachment of agricultural machinery provided by an embodiment of the present invention.

[0134] The present invention proposes a multi-sensor fusion positioning system 30 for automatic attachment of agricultural machinery, comprising: a memory 303 and a processor 301.

[0135] The memory 303 stores an application program adapted to be executed by the processor 301 to implement the multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to the method embodiment.

[0136] The multi-sensor fusion positioning system 30 for automatic attachment of agricultural machinery includes a processor 301 and a memory 303. The processor 301 and the memory 303 are connected, for example, via a bus 302.

[0137] The structure of the multi-sensor fusion positioning system 30 for automatic attachment of agricultural machinery does not constitute a limitation on the embodiments of the present invention.

[0138] Processor 301 may be a CPU, a general-purpose processor, a DSP, an ASIC, an FPGA, or other programmable logic device, transistor logic device, hardware component, or any combination thereof. It may implement or execute the various exemplary logic blocks, modules, and circuits described in connection with this disclosure. Processor 301 may also be a combination that implements computational functions, such as including one or more microprocessor combinations, a combination of a DSP and a microprocessor, etc.

[0139] Bus 302 may include a pathway for transmitting information between the aforementioned components. Bus 302 may be a PCI bus or an EISA bus, etc. Bus 302 may be divided into address bus, data bus, control bus, etc. For ease of illustration, only one thick line is used to represent it in the figure, but this does not mean that there is only one bus or one type of bus.

[0140] The memory 303 may be a ROM or other type of static storage device capable of storing static information and instructions, RAM or other type of dynamic storage device capable of storing information and instructions, or it may be an EEPROM, CD-ROM or other optical disc storage, optical disc storage (including compressed optical discs, laser discs, optical discs, digital universal optical discs, Blu-ray discs, etc.), magnetic disk storage media or other magnetic storage devices, or any other medium capable of carrying or storing desired program code in the form of instructions or data structures and accessible by a computer, but is not limited thereto.

[0141] Computer-readable storage medium embodiments The present invention proposes a computer-readable storage medium having a computer program stored thereon, the computer program being loadable and executed by a processor for a multi-sensor fusion positioning method for automatic attachment of agricultural machinery, as described in the first aspect.

[0142] The applicant of this invention has provided a detailed description of the embodiments of the invention in conjunction with the accompanying drawings. However, those skilled in the art should understand that the above embodiments are merely preferred embodiments of the invention. The detailed description is only intended to help readers better understand the spirit of the invention and is not intended to limit the scope of protection of the invention. On the contrary, any improvements or modifications made based on the inventive spirit of the invention should fall within the scope of protection of the invention.

[0143] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the embodiments of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the protection scope of the present invention.

Claims

1. A multi-sensor fusion positioning method for automatic attachment of agricultural machinery, characterized in that, The agricultural machinery includes a tractor and a trailer, and the method includes: S1, acquire the original distance measurement signal between the tractor and the attach implement, as well as the image data of the attach implement; S2, using ultra-wideband technology, distance is measured between the tractor and the attach implement based on the original ranging signal to obtain position and pose data; S3, Based on the pose data, generate the reversing path of the tractor and drive the tractor to reverse along the reversing path; S4. Determine whether the tractor has reached the pre-aligned point coordinates after the reversing operation and whether the visual recognition is successful; if yes, proceed to step S5; otherwise, return to step S2. S5, Perform stereo matching and 3D reconstruction on the image data to obtain 3D coordinates; S6. Based on the three-dimensional coordinates, the pose of the tractor and the attach device is calculated to obtain the six-degree-of-freedom pose deviation. S7, based on the six-degree-of-freedom pose deviation, control the suspension mechanism of the tractor to perform pose adjustment; S8, determine whether the suspension mechanism after posture adjustment is in posture alignment and whether contact force is detected; if yes, proceed to step S9; otherwise, return to step S5. S9. Based on the real-time six-dimensional torque generated by the contact between the suspension mechanism and the contact point, the compliance compensation amount is obtained through the desired impedance model. S10, according to the compliance compensation amount, control the suspension mechanism to perform compliant insertion, and lock the suspension mechanism after compliant insertion; S11, determine whether the suspension mechanism after compliant insertion is properly inserted and mechanically locked; if yes, the agricultural machinery is automatically attached; otherwise, return to step S10.

2. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, S1 specifically includes: S101, using ultra-wideband technology, acquire the raw ranging signals between multiple tags of the tractor and the attachment; S102, image data of the mounting device is acquired through a binocular camera.

3. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, S2 specifically includes: S201, using ultra-wideband technology, an ultra-wideband base station is deployed at the tractor end, and a tag is set at the attachment end; S202, by using the two-way time-of-flight method, raw ranging signals between multiple base stations and multiple tags are collected to obtain distance data between each base station and each tag; S203, Based on the distance data, calculate the two-dimensional coordinates of each label using triangulation; S204, Calculate the coordinates of the attachment and the yaw angle of the attachment based on the two-dimensional coordinates and the preset geometric dimensions; S205, combining the coordinates and the yaw angle, the pose data is obtained.

4. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, S3 specifically includes: S301, Based on the posture data and combined with the preset reversing distance, calculate the coordinates of the pre-alignment point and the orientation of the tractor's front end; S302, using the Hybrid A* algorithm, combined with the tractor's pose, the coordinates of the pre-alignment point, and the vehicle's orientation, a reversing path is generated; S303, via the navigation controller, drives the tractor to reverse along the reversing path according to the reversing path.

5. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, S5 specifically includes: S501, using the AprilTag detection algorithm, extract the sub-pixel coordinates of multiple corner points in the image data; S502, calculate the disparity of each corner point based on the sub-pixel coordinates of the corner points in the left and right cameras; S503, combining the parallax, camera parameters, and binocular camera baseline distance, calculate the three-dimensional coordinates of each corner point in the left camera coordinate system.

6. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, S6 specifically includes: S601, using the PnP algorithm, solve for the rotation matrix and translation vector from the preset coordinates to the three-dimensional coordinates, and obtain the pose transformation relationship from the preset coordinates to the three-dimensional coordinates; S602, perform fixed coordinate transformation on the pose transformation relationship to obtain the six-degree-of-freedom pose deviation.

7. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, Specifically, S7 includes: S701, based on the six-degree-of-freedom pose deviation and combined with the reversing kinematics model, generate fine-tuning instructions; S702, according to the fine-tuning command, drive the suspension mechanism to make adjustments and adjust the positional deviation in real time.

8. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, S9 specifically includes: S901, the real-time six-dimensional torque generated by the contact between the suspension mechanism and the attachment point is obtained through a six-dimensional force sensor; S902, the real-time six-dimensional torque is filtered and the coordinate system is transformed by the coordinate system transformation matrix; S903, based on preset impedance control parameters, inputs the converted six-dimensional torque into the desired impedance model to obtain the compliance compensation amount.

9. The multi-sensor fusion positioning method for automatic attachment of agricultural machinery according to claim 1, characterized in that, S10 specifically includes: S1001, convert the compliance compensation amount into a compliance motion correction command; S1002, according to the compliant motion correction command, the suspension mechanism is manipulated to perform compliant insertion, and the compliantly inserted suspension mechanism is locked.

10. A multi-sensor fusion positioning system for automatic attachment of agricultural machinery, characterized in that, include: Memory and processor; The memory stores an application program adapted to be executed by the processor to implement the multi-sensor fusion positioning method for automatic attachment of agricultural machinery as described in any one of claims 1 to 9.

Images