Compensation method, device and related equipment for rigidly connected mount tracking trajectory

Abstract

The application provides a rigidly connected load tracking trajectory compensation method, device and related equipment, which can first acquire a collection point set in an original running trajectory of a towing vehicle. Then, based on the longitude and latitude coordinates and the heading angle of each collection point in the collection point set, a mirror running trajectory of a load rigidly connected with the towing vehicle and the original running trajectory of the load is determined, and the mirror distance is an integer multiple of the length of the load. Then, based on the working width, a first equidistant trajectory is generated on one side of the original running trajectory of the load, and a second equidistant trajectory is generated on the same side of the mirror running trajectory. Then, the first equidistant trajectory and the second equidistant trajectory are weighted and averaged to obtain a first reference trajectory of the vehicle. By optimizing the mirror neighbor number and the multiple of the length of the load, the first reference trajectory obtained and the trajectory of the towing vehicle have higher compatibility, the deviation of the tracking curve can be effectively reduced, and the consistency of the work can be ensured.

Description

Technical Field

[0001] This invention relates to the field of vehicle control technology, and specifically to a method, device, and related equipment for compensating the tracking trajectory of a rigidly connected load. Background Technology

[0002] In practical engineering operations, it is common to use a tractor to tow a trailer or implements, with the implements typically rigidly connected to the tractor. Taking agricultural applications as an example, in large fields, different agricultural implements are rigidly mounted on tractors for operations. When designing the work path, it is usually based on the tractor's position. This doesn't cause problems when the work path is only a straight line. However, to meet practical needs, tracking and control of many curved paths are required. When tracking such paths, because the designed path is based on the tractor's position, the implement's center will deviate from the tracking curve. When multiple parallel work paths need to be executed, simply calculating equidistant parallel curves for the vehicle's position cannot guarantee that the implement's center position remains equidistant. In actual operation, this manifests as either repeated work between adjacent rows or missed work. How to more accurately compensate for the trajectory of rigidly connected implements to achieve better tracking accuracy has become an urgent technical problem to be solved. Summary of the Invention

[0003] To address the problems of large deviations and low accuracy in existing technologies, this invention provides a compensation method, device, and related equipment for tracking the trajectory of rigidly connected loads, which features more accurate tracking and smaller trajectory deviations.

[0004] A method for compensating the tracking trajectory of a rigidly connected load according to a specific embodiment of the present invention includes:

[0005] Obtain the set of data collection points from the original operating trajectory of the tractor vehicle;

[0006] Based on the mirror distance and the latitude, longitude coordinates and heading angle of each collection point in the collection point set, the mirror trajectory of the original running trajectory of the trailer rigidly connected to the traction vehicle is determined. The mirror distance is an integer multiple of the length of the trailer, and the mirror trajectory is parallel to the original running trajectory of the trailer.

[0007] Based on the working width, a first equidistant trajectory is generated on one side of the original running trajectory of the attached object, and a second equidistant trajectory is generated on the same side of the mirrored running trajectory. The interval between the first equidistant trajectory and the original running trajectory of the attached object is the working width of the attached object, and the interval between the second equidistant trajectory and the mirrored running trajectory is the working width of the attached object.

[0008] The first reference trajectory of the traction vehicle is obtained by weighted averaging the first equidistant trajectory and the second equidistant trajectory.

[0009] Furthermore, the compensation method for the tracking trajectory of the mounted object also includes:

[0010] A third equidistant trajectory is generated on the side of the first equidistant trajectory of the load that is close to the original running trajectory of the load, and a fourth equidistant trajectory is generated on the side of the second equidistant trajectory that is close to the original running trajectory of the load. The interval between the third equidistant trajectory and the first equidistant trajectory is the working width of the load, and the interval between the fourth equidistant trajectory and the second equidistant trajectory is the working width of the load.

[0011] The second reference trajectory of the traction vehicle is obtained by weighted averaging the third equidistant trajectory and the fourth equidistant trajectory.

[0012] The validity of the first reference trajectory is verified based on the degree of overlap between the second reference trajectory and the original running trajectory of the traction vehicle.

[0013] Furthermore, the compensation method for the tracking trajectory of the rigidly connected mounted object also includes:

[0014] If the first reference trajectory is invalid, the first and second reference trajectories are updated based on the adjusted mirror distance and the multiple of the payload length, and the validity of the updated first reference trajectory is verified.

[0015] Furthermore, the acquisition of the set of collection points in the original operating trajectory of the tractor vehicle includes:

[0016] The collection point set is obtained by removing points from the discrete curve point set of the original running trajectory of the traction vehicle, respectively, points with adjacent intervals less than a preset distance value and points with speeds less than a preset speed value.

[0017] Furthermore, based on the mirror distance and the latitude and longitude coordinates and heading angle of each collection point in the collection point set, the mirror trajectory of the original trajectory of the trailer rigidly connected to the traction vehicle is determined, including:

[0018] based on

[0019]

[0020] The mirrored trajectory is obtained, x(MirP) i ) represents the x-coordinate of the k-th mirror image acquisition point of the i-th n-th neighbor, MirP iLet x(P) be the x-coordinate of the i-th n-th neighboring k-th mirror image acquisition point, P be the center point of the rear axle of the tractor vehicle, k be a multiple of the length of the load, len be the length of the load, and θ be the distance between the two points. i Let y(MirP) be the heading angle of the i-th n-neighbor k-image point. i y(P) is the ordinate of the i-th n-th neighboring k-th mirror point, y(P) is the ordinate of the collection point P, and n is the number of mirror neighboring objects.

[0021] Furthermore, the step of generating a first equidistant trajectory on one side of the original running trajectory of the payload based on operational requirements, and generating a second equidistant trajectory on one side of the mirrored running trajectory, includes:

[0022] The normal vector is determined based on the coordinates of the collected points in the collection point set.

[0023] The first equidistant trajectory and the second equidistant trajectory are determined based on the normal vector and the working width, respectively.

[0024] Furthermore, the compensation method for the tracking trajectory of the rigidly connected mounted object also includes:

[0025] If the maximum deviation between the second reference trajectory and the original running trajectory of the traction vehicle is not greater than a preset threshold, then the first reference trajectory is taken as the actual running trajectory of the traction vehicle.

[0026] A compensation device for tracking the trajectory of a rigidly connected load, according to a specific embodiment of the present invention, includes:

[0027] The point set acquisition module is used to acquire the collection point set in the original running trajectory of the traction vehicle;

[0028] The trajectory generation module is used to determine the mirror trajectory of the original running trajectory of the trailer rigidly connected to the traction vehicle based on the mirror distance and the latitude and longitude coordinates and heading angle of each collection point in the collection point set, wherein the mirror distance is an integer multiple of the length of the trailer;

[0029] A trajectory mirroring module is used to generate a first equidistant trajectory on one side of the original running trajectory of the payload based on the working width, and a second equidistant trajectory on the same side of the mirrored running trajectory. The interval between the first equidistant trajectory and the original running trajectory of the payload is the working width of the payload, and the interval between the second equidistant trajectory and the mirrored running trajectory is the working width of the payload; and

[0030] The trajectory synthesis module is used to obtain the first reference trajectory of the traction vehicle by weighted averaging the first equidistant trajectory and the second equidistant trajectory.

[0031] An apparatus according to a specific embodiment of the present invention includes: a memory and a processor;

[0032] The memory is used to store programs;

[0033] The processor is used to execute the program to implement the various steps of the compensation method for the tracking trajectory of the rigidly connected mount as described above.

[0034] According to a specific embodiment of the present invention, a storage medium is provided thereon storing a computer program. When the computer program is executed by a processor, it implements each step of the above-described method for compensating the tracking trajectory of a rigidly connected load.

[0035] The compensation method for tracking the trajectory of a rigidly connected mount provided by this invention first obtains a set of collection points from the original running trajectory of the tractor vehicle. Then, based on the mirror distance and the latitude, longitude, and heading angle of each collection point in the collection point set, a mirror running trajectory of the original running trajectory of the mount rigidly connected to the tractor vehicle is determined, where the mirror distance is an integer multiple of the mount length. Next, based on the working width, a first equidistant trajectory is generated on one side of the original running trajectory of the mount, and a second equidistant trajectory is generated on one side of the mirror running trajectory. The interval between the first equidistant trajectory and the original running trajectory of the mount is the working width of the mount, and the interval between the second equidistant trajectory and the mirror running trajectory is also the working width of the mount. The orientation of the first equidistant trajectory relative to the original running trajectory of the mount is the same as the orientation of the second equidistant trajectory relative to the mirror running trajectory. Finally, the first and second equidistant trajectories are weighted and averaged to obtain the first reference trajectory of the tractor vehicle. Optimizing and selecting an appropriate number of mirror adjacent items and their mirror length multiples for the mount ensures higher compatibility between the obtained first reference trajectory and the trajectory of the tractor vehicle, effectively reducing the deviation of the tracking curve and ensuring operational consistency. Attached Figure Description

[0036] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0037] Figure 1 This is a flowchart of a method for compensating the tracking trajectory of a rigidly connected mount according to an exemplary embodiment;

[0038] Figure 2 This is a connection diagram of a tractor and a trailer provided according to an exemplary embodiment;

[0039] Figure 3 This is a diagram showing the original trajectory of the tractor and the original trajectory of the payload, provided according to an exemplary embodiment.

[0040] Figure 4 It is a neighboring item mirror image provided according to an exemplary embodiment;

[0041] Figure 5 This is a mirror trajectory diagram of a mount provided according to an exemplary embodiment;

[0042] Figure 6 This is a trajectory deviation diagram provided according to an exemplary embodiment;

[0043] Figure 7 This is a structural diagram of a compensation device for tracking the trajectory of a rigidly connected load, provided according to an exemplary embodiment.

[0044] Figure 8 This is a structural diagram of a device provided according to an exemplary embodiment. Detailed Implementation

[0045] The technical solutions of the embodiments 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, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0046] Reference Figure 1 As shown, an embodiment of the present invention provides a method for compensating the tracking trajectory of a rigidly connected mounted object, which may include the following steps:

[0047] 101. Obtain the set of collection points in the original running trajectory of the traction vehicle.

[0048] Reference Figure 2 The diagram shows a rigid connection between a mount 1 (e.g., an agricultural implement) and a tractor 2 (e.g., a tractor). The mount 1 and the tractor 2 can be rigidly connected through components such as a drive shaft. Figure 3 The diagram shows the difference between the original trajectory 3 of the tractor vehicle centered on the tractor head and the original trajectory 4 of the trailer centered on the trailer. It can be seen that when the tractor vehicle travels in a curve, the two trajectories do not overlap and there is a large deviation.

[0049] At the start of the operation, a discrete curve point set is typically collected. This set of points includes the latitude and longitude coordinates and heading angle of the tractor vehicle during its movement. Because the data collection is affected by various factors, the points in the set need to be filtered to meet compensation requirements. For example, the collected point set might look like this:

[0050] P = {P1, P2, ..., P} m}

[0051] The data collection point set needs to be filtered:

[0052] Dis i =Dis(P i P i+1 Dis i <0.2 delete P i-1 ;

[0053]

[0054] The collection point set is obtained by removing points whose adjacent interval distance is less than a preset distance value and whose speed is less than a preset speed value. For example, points whose distance between two consecutive points is less than 0.2m and whose speed is less than 0.2m / s are removed respectively.

[0055] 102. Based on the mirror distance and the latitude and longitude coordinates and heading angle of each collection point in the collection point set, determine the mirror trajectory of the original running trajectory of the trailer rigidly connected to the traction vehicle. The mirror distance is an integer multiple of the length of the trailer.

[0056] Reference Figure 4 As shown, with the center of the rear axle 5 of the tractor 2 as the center point, and the current heading angle as the starting heading angle, the entire working plane is divided into n subspaces, each with a length k times the length of the implement. (Refer to...) Figure 4 The diagram shows a 4-neighbor 1-times mirror image division. With the current heading angle at 0°, the rear axis center point is used as the starting point. The entire planar space is divided into 4 mirror neighbor intervals along the dotted line, with each interval being 1 time the length of the mounted object. Therefore, the calculation method for the k-times mirror image of the n neighbors of the acquisition point P is as follows:

[0057]

[0058] x(MirP i Let ) be the x-coordinate of the i-th n-neighbor k-image point, and MirP i Let x(P) be the x-coordinate of the sampling point P, where P is the center point of the rear axle of the tractor vehicle, k is a multiple of the length of the load, len is the length of the load, and θ is the distance between the two points. i Let y(MirP) be the heading angle of the i-th n-neighbor k-image point. i Let y(P) be the ordinate of the i-th n-th neighboring k-th mirror point, and y(P) be the ordinate of the sampling point P. n is the number of mirror neighboring objects.

[0059] The above formula yields the n neighboring k times mirror images of the mounted object for each acquisition point, thus constructing the mounted object's mirror trajectory. (Refer to...) Figure 5 The figures show the positional relationships of the original running trajectory 3 of the tractor, the original running trajectory 4 of the trailer, and the mirrored running trajectory 6 of the trailer.

[0060] 103. Based on the working width, a first equidistant trajectory is generated on one side of the original running trajectory of the attached object, and a second equidistant trajectory is generated on the same side of the mirrored running trajectory. The interval between the first equidistant trajectory and the original running trajectory of the attached object is the working width of the attached object, and the interval between the second equidistant trajectory and the mirrored running trajectory is the working width of the attached object.

[0061] After obtaining the original and mirrored running trajectories of the payload, equidistant trajectories of the original and mirrored running trajectories can be generated, either to the left or right of the tractor, depending on the actual operational requirements. These trajectories can be calculated using the following formula:

[0062]

[0063] Where P′ is a point on the equidistant trajectory, x' is the corresponding x-coordinate, and y' is the corresponding y-coordinate. r represents the right side, l is the working width, and η is the normal vector.

[0064] 104. After weighted averaging the first equidistant trajectory and the second equidistant trajectory, the first reference trajectory of the traction vehicle is obtained.

[0065] After obtaining n equidistant trajectories, these trajectories need to be comprehensively processed. Since n is set to an even value, each pair of equidistant trajectories forms a mirror image. The weighted average of these mirror images yields the trajectory path we want to track, ensuring good consistency between the generated path and the base path being followed. In practical engineering, this improves operational efficiency while maintaining consistency. Furthermore, by optimizing the selection of appropriate mirror neighbor numbers and their mirror length multiples, the obtained first reference trajectory and the trajectory of the traction vehicle can achieve higher compatibility, effectively reducing deviations in the tracking curve and ensuring operational consistency.

[0066] It is understood that the value of n can be determined using other spatial partitioning methods, or by projecting to a higher-dimensional space for partitioning. The choice of k can also be optimized by using different k-fold values ​​for different neighboring terms based on actual application requirements; this invention does not impose any restrictions on this.

[0067] To verify the effectiveness of the parameters n and k used, in another specific embodiment of the present invention, a third equidistant trajectory can be generated on the opposite side of the first equidistant trajectory of the load, i.e., on the side closer to the original running trajectory of the load, and a fourth equidistant trajectory can be generated on the side of the second equidistant trajectory, i.e., on the side closer to the original running trajectory of the load. The interval between the third equidistant trajectory and the first equidistant trajectory is the working width of the load, and the interval between the fourth equidistant trajectory and the second equidistant trajectory is the working width of the load.

[0068] Then, the third and fourth equidistant trajectories are weighted and averaged to obtain the second reference trajectory of the mounted object.

[0069] The effectiveness of the first reference trajectory is verified based on the degree of overlap between the second reference trajectory and the original running trajectory of the traction vehicle.

[0070] Specifically, in the above embodiments, the first equidistant trajectory is to the left of the original running trajectory of the attached vehicle, the second equidistant trajectory is also to the left of the mirrored running trajectory, the third equidistant trajectory is to the right of the first equidistant trajectory, and the fourth equidistant trajectory is to the right of the second equidistant trajectory. The effectiveness of the obtained first reference trajectory is verified by comparing the degree of overlap between the second reference trajectory and the original running trajectory of the traction vehicle. When the second reference trajectory and the original running trajectory of the traction vehicle have a good degree of overlap, it proves that the first reference trajectory is effective and compatible with the original running trajectory. However, when there is a large distance difference between the second reference trajectory and the original running trajectory of the traction vehicle, and this deviation still exists after running multiple times under different n adjacent k times mirror parameters, it may be due to the constraint of the minimum turning radius of the traction vehicle, which has corrected the running trajectory. In this case, a preset threshold deviation can be set, that is, if the maximum value of the deviation between the second reference trajectory and the original running trajectory of the traction vehicle is not greater than the preset threshold, then the first reference trajectory is used as the tracking trajectory of the traction vehicle. (See reference...) Figure 6 As shown, the maximum deviation value can be set to 8mm. When the deviation value is not greater than 8mm, the parameters n and k can be considered as optimization parameters for tracking trajectory planning.

[0071] Of course, if the first reference trajectory is invalid, the first and second reference trajectories can be updated based on the adjusted mirror distance, and the validity of the updated first reference trajectory can be verified again. That is, after adjusting the parameters n and k, the first and second reference trajectories are recalculated, and then the trajectory validity is verified. This results in a path compensation that is more consistent with the current mounted conditions.

[0072] Based on the same design concept, and referring to Figure 7The embodiment of the present invention also provides a compensation device for the tracking trajectory of a rigidly connected load. This device, when in operation, can implement the various steps of the above-described compensation method for the tracking trajectory of a rigidly connected load. The device may include:

[0073] The point set acquisition module 701 is used to acquire the collection point set in the original running trajectory of the traction vehicle.

[0074] The trajectory generation module 702 is used to determine the mirror trajectory of the original running trajectory of the trailer rigidly connected to the traction vehicle based on the mirror distance and the latitude and longitude coordinates and heading angle of each collection point in the collection point set. The mirror distance is an integer multiple of the length of the trailer, and the mirror trajectory is parallel to the original running trajectory of the trailer.

[0075] The trajectory mirroring module 703 is used to generate a first equidistant trajectory on one side of the original running trajectory of the payload based on the working width, and a second equidistant trajectory on the same side of the mirrored running trajectory. The interval between the first equidistant trajectory and the original running trajectory of the payload is the working width of the payload, and the interval between the second equidistant trajectory and the mirrored running trajectory is also the working width of the payload. The orientation of the first equidistant trajectory relative to the original running trajectory of the payload is the same as the orientation of the second equidistant trajectory relative to the mirrored running trajectory.

[0076] The trajectory synthesis module 704 is used to obtain the first reference trajectory of the traction vehicle by weighted averaging of the first equidistant trajectory and the second equidistant trajectory.

[0077] The rigid connection mount trajectory tracking compensation device has the same beneficial effects as the rigid connection mount trajectory tracking compensation method described above. Its specific implementation can be referred to the embodiments of the rigid connection mount trajectory tracking compensation method described above, and will not be repeated here.

[0078] Reference Figure 8 As shown, embodiments of the present invention also provide a device, which may include a memory 801 and a processor 802.

[0079] The memory 801 is used to store the program.

[0080] The processor 802 is used to execute the program to implement the various steps of the compensation method for the tracking trajectory of the rigid connection mount as described in the above embodiment.

[0081] Embodiments of the present invention also provide a storage medium storing a computer program thereon, which, when executed by a processor, implements the various steps of the compensation method for the tracking trajectory of a rigidly connected mount as described in the above embodiments.

[0082] For the foregoing method embodiments, in order to simplify the description, they are all described as a series of actions. However, those skilled in the art should understand that the present invention is not limited to the described order of actions, because according to the present invention, some steps can be performed in other orders or simultaneously. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are preferred embodiments, and the actions and modules involved are not necessarily essential to the present invention.

[0083] It should be noted that the various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For apparatus embodiments, since they are basically similar to method embodiments, the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments.

[0084] The steps in the methods of the various embodiments of the present invention can be adjusted, merged, or deleted in order according to actual needs, and the technical features described in the various embodiments can be replaced or combined.

[0085] The modules and sub-modules in the various embodiments of the present invention can be merged, divided, and deleted according to actual needs.

[0086] In the embodiments provided by this invention, it should be understood that the disclosed terminals, devices, and methods can be implemented in other ways. For example, the terminal embodiments described above are merely illustrative. For instance, the division of modules or sub-modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple sub-modules or modules may be combined or integrated into another module, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection through some interfaces, devices, or modules, and may be electrical, mechanical, or other forms.

[0087] The modules or submodules described as separate components may or may not be physically separate. The components that constitute a module or submodule may or may not be physical modules or submodules; that is, they may be located in one place or distributed across multiple network modules or submodules. Some or all of the modules or submodules can be selected to achieve the purpose of this embodiment's solution, depending on actual needs.

[0088] Furthermore, the functional modules or sub-modules in the various embodiments of the present invention can be integrated into one processing module, or each module or sub-module can exist physically separately, or two or more modules or sub-modules can be integrated into one module. The integrated modules or sub-modules described above can be implemented in hardware or in the form of software functional modules or sub-modules.

[0089] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0090] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein can be implemented directly by hardware, a software unit executed by a processor, or a combination of both. The software unit can be located in random access memory (RAM), main memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art.

[0091] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

Claims

1. A method of compensating for a rigidly connected payload tracking trajectory, characterized by, The method comprises the following steps: acquiring a set of collection points in an original running track of a towing vehicle; determining a mirror running track of an original running track of a mounted object rigidly connected to the towing vehicle based on a mirror distance and latitude and longitude coordinates and a heading angle of each collection point in the set of collection points, the mirror distance being an integer multiple of a length of the mounted object, the determination process comprising: based on obtaining the mirror running track, is the horizontal coordinate of the i-th n-neighbor k-mirror collection point, is the i-th n-neighbor k-mirror collection point, is the horizontal coordinate of the collection point P, P is the center point of the rear axle of the towing vehicle, k is a multiple of the length of the load, and len is the length of the load, is the heading angle of the i-th n-neighbor k-mirror point, is the longitudinal coordinate of the i-th n-neighbor k-mirror point, is the longitudinal coordinate of the collection point P, and n is the number of mirror neighbors of the load; generating a first equidistant track on one side of the original running track of the mounted object based on a working width and a second equidistant track on the same side of the mirror running track, the interval distance between the first equidistant track and the original running track of the mounted object being the working width of the mounted object, the interval distance between the second equidistant track and the mirror running track being the working width of the mounted object; weighting and averaging the first equidistant track and the second equidistant track to obtain a first reference track of the towing vehicle.

2. The method of claim 1, wherein, The method further comprises the following steps: generating a third equidistant track on the side of the first equidistant track close to the original running track of the mounted object and a fourth equidistant track on the side of the second equidistant track close to the original running track of the mounted object, the interval distance between the third equidistant track and the first equidistant track being the working width of the mounted object, the interval distance between the fourth equidistant track and the second equidistant track being the working width of the mounted object; weighting and averaging the third equidistant track and the fourth equidistant track to obtain a second reference track of the towing vehicle; verifying the validity of the first reference track based on the coincidence degree between the second reference track and the original running track of the towing vehicle.

3. The method of claim 2, wherein, The method further comprises the following steps: if the first reference track is invalid, verifying the validity of the updated first reference track after updating the first reference track and the second reference track based on the adjusted mirror distance and the multiple of the length of the mounted object.

4. The method of claim 1, wherein, The acquiring of the set of collection points in the original running track of the towing vehicle comprises the following steps: respectively removing points with an interval distance less than a preset distance value and a speed less than a preset speed value from a discrete curve point set of the original running track of the towing vehicle to obtain the set of collection points.

5. The method of claim 1, wherein, The generation of the first equidistant track on one side of the original running track of the mounted object based on the working width and the generation of the second equidistant track on the same side of the mirror running track comprise the following steps: determining a normal vector based on the coordinates of the collection points in the set of collection points; respectively determining the first equidistant track and the second equidistant track based on the normal vector and the working width.

6. The method according to any one of claims 1 to 5, characterized in that, The method further comprises the following steps: if the maximum deviation of the second reference track from the original running track of the towing vehicle is not greater than a preset threshold value, taking the first reference track as an actual planning running track of the towing vehicle.

7. A compensation device for a rigidly connected payload tracking trajectory, characterized by The method comprises the following steps: a point set acquisition module is configured to acquire a set of collection points in an original running track of a towing vehicle; The trajectory generation module is configured to determine a mirror running trajectory of an original running trajectory of a rigidly connected load of the towing vehicle based on a mirror distance, latitude and longitude coordinates and a heading angle of each of the collection points, the mirror distance being an integer multiple of a length of the load, and the determination process of the mirror running trajectory comprising: based on get the mirror running track, is the horizontal coordinate of the i-th n-neighbor k-mirror collection point, is the i-th n-neighbor k-mirror collection point, is the horizontal coordinate of the collection point P, P is the center point of the rear axle of the towing vehicle, k is the multiple of the length of the load, len is the length of the load, is the heading angle of the i-th n-neighbor k-mirror point, is the longitudinal coordinate of the i-th n-neighbor k-mirror point, is the longitudinal coordinate of the collection point P, n is the number of mirror neighbors of the load; The trajectory mirror module is configured to generate a first equidistant trajectory on one side of the original running trajectory of the load based on a working width, and generate a second equidistant trajectory on the same side of the mirror running trajectory, the interval distance between the first equidistant trajectory and the original running trajectory of the load being the working width of the load, and the interval distance between the second equidistant trajectory and the mirror running trajectory being the working width of the load; and The trajectory synthesis module is configured to obtain a first reference trajectory of the towing vehicle by weighted averaging of the first equidistant trajectory and the second equidistant trajectory.

8. An apparatus, comprising: comprise: a memory and a processor; the memory is configured to store a program; the processor is configured to execute the program to implement the steps of the rigidly connected load tracking trajectory compensation method according to any one of claims 1 to 6.

9. A storage medium having stored thereon a computer program, characterized in that The computer program, when executed by the processor, implements the steps of the rigidly connected load tracking trajectory compensation method according to any one of claims 1 to 6.

Images