Compensation method, device and related equipment for flexible connection mount tracking trajectory

Abstract

The present application provides a flexible connection load tracking trajectory compensation method, device and related equipment, which can obtain a set of collection points in the original running trajectory of a towing vehicle. Then, based on the latitude and longitude coordinates and the heading angle of each collection point in the obtained set of collection points and the length of the load, the original running trajectory of the load is determined. Then, based on the working width, an equidistant trajectory is generated, wherein the interval distance between the equidistant trajectory and the original running trajectory of the load is the working width. Based on the equidistant trajectory and the length of the load, the control trajectory of the towing vehicle can be deduced. The compensation method can determine the control trajectory of the towing vehicle under the premise of ensuring the working width of the load, effectively avoiding the occurrence of repeated work, missed work and the like, and improving the efficiency of the work.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicle control, in particular to a flexible connection hanging object tracking trajectory compensation method and device and related equipment. BACKGROUND

[0002] In engineering or agricultural operations, a tractor is often used to pull a larger hanging object for operation. The control of the hanging object is achieved by steering the vehicle head, and the tractor and the hanging object are connected flexibly. This connection method needs to design a path according to the status of the tractor to make the hanging object reach the expected path. For example, in agricultural applications, a large tractor is used to pull a farm tool flexibly. When steering control is performed, the farm tool does not need to be lifted for direct traction for operation. In specific use, many parallel and equidistant operation paths are usually designed, and ideally, each operation path is the path formed by the center of the pulled farm tool. However, in actual operation, whether automatic driving or assisted driving, the position of the tractor is controlled, which may cause repeated operation and missed operation in adjacent operation processes. SUMMARY

[0003] In order to solve the problems of repeated operation and easy omission in the prior art, the present application provides a flexible connection hanging object tracking trajectory compensation method, device and related equipment, which has the characteristics of more accurate operation, less omission and repeated operation.

[0004] According to the flexible connection hanging object tracking trajectory compensation method provided by the embodiment of the present application, the method comprises the following steps:

[0005] Obtaining a set of collection points in an original running trajectory of a tractor;

[0006] Determining an original running trajectory of a hanging object based on the longitude and latitude coordinates and the heading angle of each collection point in the set of collection points and the length of the hanging object;

[0007] Generating an equidistant trajectory of the hanging object based on the operation width, and the interval distance between the equidistant trajectory and the original running trajectory is the operation width;

[0008] Determining a control trajectory of the tractor based on the equidistant trajectory and the length of the hanging object.

[0009] Further, the flexible connection hanging object tracking trajectory compensation method further comprises: before determining the control trajectory of the tractor based on the equidistant trajectory and the length of the hanging object, optimizing the equidistant trajectory to reduce the angle jump of adjacent points in the equidistant trajectory.

[0010] Further, the method of obtaining a set of collection points in an original running trajectory of a tractor comprises:

[0011] From the discrete curve points set of the original running track of the towing vehicle, respectively, eliminate the points with adjacent interval distance less than a preset distance value and speed less than a preset speed value to obtain the collection point set.

[0012] Further, the original running track of the towing vehicle is determined based on the latitude and longitude coordinates and the heading angle of each collection point in the collection point set and the length of the towing object, comprising:

[0013] Based on

[0014]

[0015] obtaining the original running track of the towing object, the abscissa of the corresponding point on the original running track of the towing object, the abscissa of the i-th collection point, ToolLen is the length of the towing object, the heading angle at the i-th collection point, the ordinate of the corresponding point on the original running track of the towing object, the ordinate of the i-th collection point.

[0016] Further, the equidistant track of the towing object is generated based on the working width, comprising:

[0017] Based on the normal vector of one side of the original running track of the towing object and the working width of the towing object, the equidistant track is obtained.

[0018] Further, the equidistant track is optimized to reduce the angle jump of adjacent points in the equidistant track, comprising:

[0019] Based on the quadratic programming, the points corresponding to the collection points in the equidistant track are optimized to obtain the optimal fitting processing point set to reduce the angle jump in the equidistant track.

[0020] Further, the control track of the towing vehicle is determined based on the equidistant track and the length of the towing object, comprising:

[0021] Based on

[0022]

[0023]

[0024]

[0025] obtaining the control track of the towing vehicle, a heading angle of the towing vehicle, ToolLen is a length of the towed object, a heading angle of the towed object in the offset trajectory, ;

[0026] a heading angle of the towing vehicle, carLen is a length of the towing vehicle,

[0027] ;

[0028] a preset constant,

[0029] ;

[0030] xTool is an abscissa of a coordinate point in the offset trajectory, yTool is an ordinate of the coordinate point in the offset trajectory, x is an abscissa of a coordinate point in a control trajectory of the towing vehicle, and y is an ordinate of the coordinate point in the control trajectory of the towing vehicle.

[0031] A compensation device for a flexible connection towed object tracking trajectory is provided according to an embodiment of the present application, comprising:

[0032] a point set acquisition module configured to acquire a set of collection points in an original running trajectory of a towing vehicle;

[0033] a trajectory determination module configured to determine an original running trajectory of a towed object based on a latitude and longitude coordinate and a heading angle of each collection point in the set of collection points and a length of the towed object;

[0034] a trajectory translation module configured to generate an offset trajectory of the towed object based on a working width, the offset trajectory being spaced apart from the original running trajectory by the working width; and

[0035] a trajectory synthesis module configured to determine a control trajectory of the towing vehicle based on the offset trajectory and the length of the towed object.

[0036] An apparatus is provided according to an embodiment of the present application, comprising a memory and a processor;

[0037] the memory is configured to store a program;

[0038] the processor is configured to execute the program to implement each step of the compensation method for a flexible connection towed object tracking trajectory.

[0039] A storage medium is provided according to an embodiment of the present application, having a computer program stored thereon, the computer program being executed by a processor to implement each step of the compensation method for a flexible connection towed object tracking trajectory.

[0040] The compensation method for tracking trajectory of the flexible connection load provided by the present application can obtain a set of collection points in the original running trajectory of the towing vehicle. Then, based on the latitude and longitude coordinates and the heading angle of each collection point in the obtained set of collection points and the length of the load, the original running trajectory of the load is determined. Then, based on the work requirement, an equidistant trajectory parallel to the original running trajectory of the load is generated, wherein the interval distance between the equidistant trajectory and the original running trajectory of the load is the working width of the load. Based on the equidistant trajectory and the length of the load, the control trajectory of the towing vehicle can be deduced. The compensation method can determine the control trajectory of the towing vehicle under the premise of ensuring the working width of the load, and can effectively avoid the occurrence of repeated work, missed work and the like, and improve the efficiency of the work. BRIEF DESCRIPTION OF DRAWINGS

[0041] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced below. Obviously, the drawings in the following description are only embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor on the basis of the provided drawings.

[0042] Figure 1 is a flow chart of the compensation method for tracking trajectory of the flexible connection load provided according to an exemplary embodiment;

[0043] Figure 2 is a connection diagram of the towing vehicle and the load provided according to an exemplary embodiment;

[0044] Figure 3 is a trajectory diagram of the equidistant trajectory provided according to an exemplary embodiment;

[0045] Figure 4 is a control trajectory diagram of the towing vehicle provided according to an exemplary embodiment;

[0046] Figure 5 is an effect diagram of trajectory optimization provided according to an exemplary embodiment;

[0047] Figure 6 is a distance difference diagram provided according to an exemplary embodiment;

[0048] Figure 7 is a structure diagram of the compensation device for tracking trajectory of the flexible connection load provided according to an exemplary embodiment;

[0049] Figure 8 is a structure diagram of the device provided according to an exemplary embodiment. DETAILED DESCRIPTION

[0050] With reference to the accompanying drawings: clearly and fully describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative labor fall within the scope of the present application.

[0051] Referring to Figure 1 As shown in the drawings, the embodiments of the present application provide a compensation method for a flexible connection of a load tracking trajectory, which can include the following steps:

[0052] 101, obtaining a set of collected points in the original running trajectory of the towing vehicle.

[0053] Referring to Figure 2 As shown in the drawings, the towing vehicle 2 and the load 1, such as agricultural implements, are connected in a flexible manner. From the discrete curve point set of the original running trajectory of the towing vehicle, the points with a distance less than a preset distance value and a speed less than a preset speed value are removed to obtain a set of collected points. Because there is a certain projection deviation in the general map, and the trajectory line designed by the function cannot reflect the actual situation of the work. Therefore, in engineering, the discrete point set path actually traveled by the towing vehicle is usually taken as the original running trajectory of the towing vehicle work. Usually, it covers latitude and longitude coordinates and heading angle values. When recording the discrete data, there are many influencing factors, so it is necessary to optimize the original running trajectory first: taking the trajectory set P of the original running trajectory collected as an example

[0054]

[0055] According to the change of the speed and position of the points in the set of collected points:

[0056] ,

[0057]

[0058] Wherein diff(yp) is the difference between the y coordinates of the adjacent two points, diff(xp) is the difference between the x coordinates of the adjacent two points, hypot is the square root distance function of the standard square sum, dis is the distance between the two points, and when the distance is less than 0.1m, the corresponding point will be removed, The speed between two points is less than 0.2m / s, and the corresponding point is deleted, and then the set of collected points is obtained.

[0059] It can be understood that a person skilled in the art can modify and limit the removal method of the trajectory set according to the actual application needs, and the present application does not limit this.

[0060] 102. Determine the original running track of the mounted object based on the longitude and latitude coordinates and the heading angle of each of the collection points in the collection point set and the length of the mounted object.

[0061] Based on the obtained collection point set, the original running track of the tracked mounted object can be further calculated:

[0062] The original running track of the mounted object can be obtained by

[0063]

[0064] The original running track of the mounted object is obtained, wherein is the horizontal coordinate of the corresponding point on the original running track of the mounted object, is the horizontal coordinate of the i-th collection point, and ToolLen is the length of the mounted object, is the heading angle at the i-th collection point, is the vertical coordinate of the corresponding point on the original running track of the mounted object, is the vertical coordinate of the i-th collection point.

[0065] 103. Generate an equidistant track of the mounted object based on the working width, and the interval distance between the equidistant track and the original running track of the mounted object is the working width.

[0066] After obtaining the original running track of the mounted object, the track is taken as the base track for tracking, and subsequent related work is based on the track. In actual work, it is necessary to specify whether the tracking track is located on the left side or the right side of the base track according to the actual work requirements. Based on the normal vector of one side of the original running track of the mounted object and the working width of the mounted object, an equidistant track with an interval distance of the working width of the mounted object from the base track can be obtained:

[0067] ;

[0068] wherein width is the working width of the mounted object, and η is the unit normal vector, is the horizontal coordinate of the point on the equidistant track, is the vertical coordinate of the point on the equidistant track, is the horizontal coordinate of the point P on the original running track, is the vertical coordinate of the point P on the original running track, is the horizontal coordinate of the point P on the original running track, is the vertical coordinate of the point P on the original running track, and are two components of the tangent vector of the point P on the original running track. The obtained equidistant track is shown in Figure 3 According to the original running track 5 of the towing vehicle, the original running track 4 of the mounted object is obtained, and then the equidistant track 3 located on the left side of the original running track of the mounted object is obtained.

[0069] 104. Determine the control trajectory of the traction vehicle based on the equidistant trajectory and the length of the load.

[0070] After obtaining the equidistant trajectory, it is necessary to calculate the control trajectory of the traction vehicle with the tractor head as the control center based on the equidistant trajectory. The calculation process is as follows:

[0071] based on

[0072]

[0073]

[0074]

[0075] Obtain the control trajectory of the traction vehicle. ToolLen represents the heading angle of the towing vehicle, and ToolLen represents the length of the load. The heading angle of the load in an equidistant trajectory. ;

[0076] Here, is the heading angle of the towing vehicle, and carLen is the length of the towing vehicle.

[0077] ;

[0078] These are preset constants; users can set them as needed.

[0079] ;

[0080] xTool represents the x-coordinate of the coordinate point in the equidistant trajectory, yTool represents the y-coordinate of the coordinate point in the equidistant trajectory, x represents the x-coordinate of the coordinate point in the control trajectory of the traction vehicle, and y represents the y-coordinate of the coordinate point in the control trajectory of the traction vehicle.

[0081] The above parameters can be obtained by calculating the arc length of the trajectory. The arc length of the trajectory is calculated as follows:

[0082]

[0083] in Let the arc length of the trajectory be . Let x be the x-coordinate of the first point among adjacent points on the equidistant trajectory. Let x be the x-coordinate of the second point among adjacent points on the equidistant trajectory. Let be the ordinate of the first point among adjacent points on the equidistant trajectory. Let be the ordinate of the second point among adjacent points on the equidistant trajectory. The control trajectory with the vehicle's front as the control center can be obtained using the above formula, as shown below. Figure 4The obtained control trajectory of the towing vehicle and the running trajectory of the load are obtained in actual application. Consistency of trajectory tracking in actual operation can be improved, thereby improving the degree of automation of the machine, improving the inconvenience and uncertainty of manual operation in the process of assisted driving, improving the operation efficiency, and avoiding the occurrence of repeated operation and other misoperation conditions.

[0084] To further optimize the technical solution, in another specific embodiment of the present application, before the control trajectory of the towing vehicle is determined based on the equidistant trajectory and the length of the load, the equidistant trajectory is optimized to reduce the angle jump of adjacent points in the equidistant trajectory. Specifically, it can include: first, based on quadratic programming, the points corresponding to the collection points in the equidistant trajectory are optimized to obtain an optimal fitting point set, so as to reduce the angle jump of adjacent points in the equidistant trajectory.

[0085] Specifically, since the obtained equidistant trajectory does not have the same accurate heading data as the original running trajectory, it needs to be derived through position data. Since the collected point set is a discrete point set, the angle value obtained directly using the position data (heading angle, latitude and longitude coordinates) in the point set has a large jump change, so it needs to be optimized. The entire trajectory can be converted into a quadratic programming problem to solve the optimal objective function, and the optimization processing of the entire path trajectory is completed under the constraint condition of minimum fitting variance. Wherein the quadratic programming (QP) is a process for solving a special mathematical optimization problem. A quadratic programming problem with n variables and m constraints can be described in the following form. First, give:

[0086] An n-dimensional vector N, an n×n-dimensional symmetric matrix M, an m×n-dimensional matrix A, and an m-dimensional vector b. The objective of the quadratic programming problem is to find an n-dimensional vector X under the constraint condition that:

[0087]

[0088] is minimum. Here, the vector X can be the fitting coefficient of the horizontal coordinate of each point:

[0089]

[0090]

[0091] , respectively, are four fitting coefficients. After obtaining the fitting coefficients, the vertical coordinates of the coordinate points can be obtained:

[0092]

[0093] ​It should be noted that the specific construction of the above vector N, matrix M and matrix A can be set by those skilled in the art according to actual needs, and the present application will not be repeated here.

[0094] Referring to Figure 5 The optimized angle change is relatively smooth, Figure 6 The distance difference between the optimized point sets is small, and the error is in the order of millimeters.

[0095] Based on the same design idea, referring to Figure 7 The embodiment of the present application also provides a flexible connection mounting object tracking trajectory compensation device, which can realize the steps of the flexible connection mounting object tracking trajectory compensation method described above when in operation, and the device can include:

[0096] The point set acquisition module 701 is configured to acquire a collection point set in the original running trajectory of the towing vehicle.

[0097] The trajectory determination module 702 is configured to determine the original running trajectory of the mounting object based on the latitude and longitude coordinates and the heading angle of each collection point in the collection point set and the length of the mounting object.

[0098] The trajectory translation module 703 is configured to generate an equidistant trajectory of the mounting object based on the working width, and the interval distance between the equidistant trajectory and the original running trajectory is the working width.

[0099] The trajectory synthesis module 704 is configured to determine the control trajectory of the towing vehicle based on the optimized equidistant trajectory and the length of the mounting object.

[0100] The flexible connection mounting object tracking trajectory compensation device has the same beneficial effects as the flexible connection mounting object tracking trajectory compensation method described above, and the specific implementation mode can refer to the embodiments of the mounting object tracking trajectory compensation method described above, and the present application will not be repeated here.

[0101] Referring to Figure 8 The embodiment of the present application also provides a device, which can include a memory and a processor.

[0102] The memory 801 is configured to store a program.

[0103] The processor 802 is configured to execute the program to realize the steps of the flexible connection mounting object tracking trajectory compensation method described in the above embodiments.

[0104] The embodiment of the present application also provides a storage medium having a computer program stored thereon, and the computer program is executed by a processor to realize the steps of the flexible connection mounting object tracking trajectory compensation method described in the above embodiments.

[0105] For each method embodiment described above, for the sake of simple description, it is expressed as a combination of a series of actions, but those skilled in the art should know that the present application is not limited by the order of the described actions, because according to the present application, certain steps can be performed in other order or simultaneously. Secondly, those skilled in the art should know that the embodiments described in the specification are all preferred embodiments, and the actions and modules involved are not necessarily necessary for the present application.

[0106] It should be noted that each embodiment in the specification is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same and similar parts between each embodiment can be referred to each other. For device embodiments, since they are basically similar to method embodiments, they are described more simply, and the relevant parts are referred to the part of the method embodiment.

[0107] The steps in the method of each embodiment of the present application can be adjusted in order, combined and reduced according to actual needs. The technical features recorded in each embodiment can be replaced or combined.

[0108] The modules and sub-modules in the device and terminal of each embodiment of the present application can be combined, divided and reduced according to actual needs.

[0109] In several embodiments provided by the present application, it should be understood that the disclosed terminal, device and method can be implemented by other ways. For example, the terminal embodiments described above are only schematic, for example, the division of modules or sub-modules is only a logical function division, and actual implementation can have another division manner, for example, a plurality of sub-modules or modules can be combined or integrated into another module, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the displayed or discussed each other can be indirect coupling or communication connection between some interfaces, devices or modules, which can be electrical, mechanical or other forms.

[0110] The modules or sub-modules described as separate components can or can not be physically separated, and the components of the modules or sub-modules can or can not be physical modules or sub-modules, that is, they can be located in one place, or can be distributed to multiple network modules or sub-modules. According to actual needs, some or all of the modules or sub-modules can be selected to achieve the purpose of the embodiment.

[0111] In addition, each functional module or sub-module in various embodiments of the present application can be integrated in one processing module, or each module or sub-module can exist physically alone, or two or more modules or sub-modules can be integrated in one module. The integrated module or sub-module can be realized in the form of hardware or in the form of a software functional module or sub-module.

[0112] Those skilled in the art will further appreciate that the functions or steps of the examples described herein can be implemented using electronic hardware, computer software, or any combination of the two. To clearly illustrate this interchangeability of hardware and software, various examples have been described herein in terms of their functional generalities. Whether such functions are implemented as hardware or software depends on the particular application and design constraints imposed on the overall system. Skilled artisans can implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.

[0113] The steps of a method or algorithm described in connection with the examples disclosed herein can be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. A software module can reside in random access memory (RAM), memory, read-only memory (ROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.

[0114] Finally, it should be noted that the terms "first" and "second" and the like are used merely to distinguish one element from another, and do not necessarily indicate a physical or chronological order. Furthermore, the terms "comprise," "include," and the like are used synonymously with the term "comprising" or "including," and are intended to allow for the possibility that the processes, methods, articles, or apparatuses that they describe can include additional elements or steps not expressly mentioned. The use of the term "about" in relation to a numerical value shall be understood to mean that the value is likely to vary by ±10% around the stated value.

[0115] The foregoing description of the disclosed embodiments enables a person skilled in the art to make or use the application. Modifications of these embodiments will occur to persons of skill in the art, and, while certain modifications are discussed, it is desired to be protected in accordance with the spirit and scope of the application. Therefore, the application is not limited to the specific embodiments shown and described, but only by the scope of the appended claims, unless otherwise specified.

Claims

1. A method of compensating for a flexible connection payload tracking trajectory, characterized by, The method comprises: acquiring a set of collection points in an original running track of a towing vehicle; determining an original running track of a load based on the longitude and latitude coordinates and the heading angle of each collection point in the set of collection points and the length of the load; generating an equidistant track of the load based on a working width, the equidistant track having an interval distance from the original running track being the working width; determining a control track of the towing vehicle based on the equidistant track and the length of the load; wherein the process of determining the control track of the towing vehicle based on the equidistant track and the length of the load comprises: based on a control trajectory for the tow vehicle, the heading angle of the tow vehicle, ToolLen is the length of the towed object, the heading angle of the towed object in the loxodrome trajectory, ； is the heading angle of the tow vehicle, carLen is the length of the tow vehicle, ； is a preset constant, ； xTool is the horizontal coordinate of a coordinate point in the equidistant track, yTool is the vertical coordinate of the coordinate point in the equidistant track, x is the horizontal coordinate of a coordinate point in the control track of the towing vehicle, and y is the vertical coordinate of the coordinate point in the control track of the towing vehicle.

2. The method of claim 1, wherein, The method further comprises: before determining the control track of the towing vehicle based on the equidistant track and the length of the load, optimizing the equidistant track to reduce the angle jump of adjacent points in the equidistant track.

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

4. The method of claim 1, wherein, The determining of the original running track of the load based on the longitude and latitude coordinates and the heading angle of each collection point in the set of collection points and the length of the load comprises: based on a longitudinal coordinate of a corresponding point on the original running track of the mount, a longitudinal coordinate of a corresponding point on the original running track of the mount, a longitudinal coordinate of a corresponding point on the original running track of the mount, a longitudinal coordinate of a corresponding point on the original running track of the mount, a longitudinal coordinate of a corresponding point on the original running track of the mount, a longitudinal coordinate of a corresponding point on the original running track of the mount, 5. The method of claim 1, wherein, The generating of the equidistant track of the load based on the working width comprises: based on the normal vector of one side of the original running track of the load and the working width of the load, obtaining the equidistant track.

6. The method of claim 2, wherein, The optimization of the equidistant track to reduce the angle jump of adjacent points in the equidistant track comprises: based on quadratic programming, optimizing points in the equidistant track corresponding to the collection points to obtain a set of processed points after optimal fitting, so as to reduce the angle jump in the equidistant track.

7. A flexible connection payload tracking trajectory compensation apparatus, characterized by, The method comprises: a point set acquisition module configured to acquire a set of collection points in an original running track of a towing vehicle; a track determination module configured to determine an original running track of a load based on the longitude and latitude coordinates and the heading angle of each collection point in the set of collection points and the length of the load; a track translation module configured to generate an equidistant track of the load based on a working width, the equidistant track having an interval distance from the original running track being the working width; and a track synthesis module configured to determine a control track of the towing vehicle based on the equidistant track and the length of the load, wherein the process of determining the control track of the towing vehicle based on the equidistant track and the length of the load comprises: based on a control trajectory for the tow vehicle, a heading angle of the tow vehicle, a heading angle of the payload in the loxodrome trajectory, ； is the heading angle of the tow vehicle, carLen is the length of the tow vehicle, ； is a preset constant, ； xTool is the horizontal coordinate of a coordinate point in the equidistant track, yTool is the vertical coordinate of the coordinate point in the equidistant track, x is the horizontal coordinate of a coordinate point in the control track of the towing vehicle, and y is the vertical coordinate of the coordinate point in the control track of the towing vehicle.

8. An apparatus, comprising: The method comprises: a memory and a processor; the memory is configured to store a program; The processor is configured to execute the program to implement each step of the compensation method for a flexible connection payload tracking trajectory 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 a processor, implements each step of the compensation method for a flexible connection payload tracking trajectory according to any one of claims 1 to 6.

Images