Differential AGV path following method and system based on improved pure tracking control

Abstract

The application discloses a differential AGV path following method and system based on improved pure tracking control, and the method comprises the following steps: obtaining a path trajectory by using cubic B-spline curve fitting; taking the midpoint of an axle as an AGV reference point; selecting a current trajectory reference point according to the AGV reference point; calculating a turning radius and planning a set speed according to the trajectory reference point; calculating a preview mark length according to the current speed difference between the left and right wheels of the differential AGV; calculating the coordinates of a preview point according to the path length from the starting point of the current path segment to the trajectory reference point, the current path segment length, the relationship between the current path segment and the path trajectory and the preview mark length; calculating the preview distance and the included angle between the AGV reference point and the trajectory reference point according to the coordinates of the preview point and the AGV reference point; and calculating the speeds of the left and right wheels of the differential AGV by using a kinematic model of the differential AGV. The method solves the problems that pure tracking control causes oscillation due to too short preview distance during straight-line driving and poor following due to too long preview distance during curve driving.

Description

TECHNICAL FIELD

[0001] The present application relates to the AGV path tracking technical field, particularly relates to a differential AGV path following method and system based on improved pure pursuit control. BACKGROUND

[0002] Pure pursuit control algorithm (Pure Pursuit) is widely used in the control field of unmanned driving as a mainstream control algorithm due to its high control accuracy and strong robustness.

[0003] Since the differential AGV travels at a slow speed and in a relatively smooth environment, a relatively simple control method is usually used. However, with the development of automation, various complex application scenarios have led to strict requirements for the accuracy of AGV trajectory following, especially when the AGV is under high load and high-speed turning. The previous control method has the disadvantages of large error, delay and the like, and the AGV is prone to derailment. Therefore, how to apply the pure pursuit control algorithm to the control of the differential AGV has become a problem that technicians need to consider. SUMMARY

[0004] The purpose of the present application is to provide a differential AGV path following method and system based on improved pure pursuit control, so as to solve one or more technical problems existing in the prior art and at least provide a beneficial choice or create conditions.

[0005] The solution to the technical problem of the present application is to provide a differential AGV path following method and system based on improved pure pursuit control.

[0006] According to the embodiment of the first aspect of the present application, a differential AGV path following method based on improved pure pursuit control is provided, comprising the following steps:

[0007] The set control point coordinates are fitted by using a cubic B-spline curve to obtain a path trajectory followed by the AGV, wherein the path trajectory is composed of a plurality of path segments;

[0008] The midpoint of the AGV wheel shaft is taken as an AGV reference point, and the point closest to the current path segment is taken as the current trajectory reference point;

[0009] According to the trajectory reference point, a turning radius is calculated, and a set speed of the AGV is planned according to the turning radius;

[0010] According to the current speed difference of the left and right wheels of the AGV, a preview mark length is calculated;

[0011] According to the path length from the segment starting point of the current path segment to the trajectory reference point, the length of the current path segment, the relationship between the current path segment and the path trajectory, and the preview mark length, the preview point coordinates are calculated;

[0012] According to the preview point coordinates and the AGV reference point, the preview distance and the angle between the AGV reference point and the trajectory reference point are calculated;

[0013] According to the set speed, the preview distance, and the angle between the AGV reference point and the trajectory reference point, the speeds of the differential left and right wheels are calculated respectively by using the constructed differential AGV kinematic model.

[0014] Further, the trajectory reference point specifically includes:

[0015] The two-point distance formula is calculated by the AGV reference point (X r ,Y r ) and the set trajectory reference point;

[0016] The first movement coefficient closest to the AGV reference point (X r ,Y r ) on the current path segment is obtained by deriving the two-point distance formula and using Newton iteration method to calculate the minimum value of the derived two-point distance formula;

[0017] According to the first movement coefficient and all control point coordinates constituting the current path segment, the current trajectory reference point (X c ,Y c ) is calculated.

[0018] Further, the set speed of the AGV is planned according to the turning radius calculated based on the trajectory reference point, specifically including:

[0019] The first movement coefficient is added by a step length and subtracted by a step length to obtain the rear control point coordinates (X c+1 ,Y c+1 ) and the front control point coordinates (X c-1 ,Y c-1 );

[0020] According to the trajectory reference point (X c ,Y c ), the rear control point coordinates (X c+1 ,Y c+1 ), and the front control point coordinates (X c-1 ,Y c-1 ), the turning radius R c at the trajectory reference point is calculated;

[0021] The maximum threshold of the turning radius is R max , and the minimum threshold is Rmin , the maximum threshold value of the set speed is V max , the minimum threshold value is V min , the set speed is planned to obtain a planning relationship:

[0022]

[0023] Further, the pre-look point coordinate calculation process specifically includes:

[0024] According to the trajectory reference point (X c , Y c ), the path length S(t) from the start point of the current path segment to the trajectory reference point is calculated by using the numerical integration method;

[0025] When S(t)+L pp ≤S k (1), the pre-look point is located on the current path segment, wherein L pp is the pre-look calibration length, and S k (1) is the length of the current path segment;

[0026] The second movement coefficient of the pre-look point on the current path segment is calculated by using the Newton iteration method;

[0027] According to the second movement coefficient and all control point coordinates constituting the current path segment, the pre-look point coordinate is calculated.

[0028] Further, the pre-look point coordinate calculation process specifically includes:

[0029] When S(t)+L pp >S k (1), and the current path segment is not the last segment in the path trajectory, the pre-look point is located on the next path segment;

[0030] The third movement coefficient of the pre-look point on the next path segment is calculated by using the Newton iteration method;

[0031] According to the third movement coefficient and all control point coordinates constituting the next path segment, the pre-look point coordinate is calculated.

[0032] Further, the pre-look point coordinate calculation process specifically includes:

[0033] When S(t)+L pp >S k (1), and the current path segment is the last segment in the path trajectory, the pre-look point is located outside the end point of the path trajectory;

[0034] According to the end point coordinate information of the path trajectory and the set calibration distance, the pre-look point coordinate is calculated.

[0035] Further, the pre-look mark length calculation process specifically includes:

[0036] The current speed of the differential left wheel and the current speed of the differential right wheel are obtained, and a current speed difference AV is calculated;

[0037] According to the current speed difference AV and a set distance parameter, a pre-look mark length L is calculated pp .

[0038] Further, the calculation of the pre-look distance and the angle between the AGV reference point and the track reference point according to the pre-look point coordinates specifically includes:

[0039] According to the pre-look point coordinates (X r ,Y r ) and the AGV reference point (X r ,Y p , θ p ), a pre-look distance is calculated;

[0040] According to the pre-look point coordinates (X r ,Y r ) and the AGV reference point (X r ,Y 0,3 , θ 1,3 ), the angle between the AGV reference point and the track reference point is calculated using the angle calculation formula:

[0041]

[0042] .

[0043] Further, the speed calculation process of the differential left and right wheels specifically includes:

[0044] According to the distance between the differential left and right wheels and the AGV reference point, a differential AGV kinematic model is constructed;

[0045] Based on the differential AGV kinematic model, the speed of the differential left and right wheels is calculated through the pre-look distance, the angle between the set speed AGV reference point and the track reference point.

[0046] According to the second aspect of the embodiment of the application, a differential AGV path following system based on improved pure tracking control is provided, which includes:

[0047] At least one processor; at least one memory for storing at least one program; when the at least one program is executed by the at least one processor, the at least one processor implements the method of any one of the first aspect based on improved pure tracking control of differential AGV path following.

[0048] The beneficial effect of the present application is that, compared with the traditional pure tracking control, the pre-look mark length is obtained by the current speed difference of the differential left and right wheels of the AGV, and the current pre-look distance is obtained, according to the constructed kinematic model of the differential AGV, the speed of the differential left and right wheels required to reach the pre-look point is obtained, and the problem of oscillation when the pre-look distance is too short when pure tracking control is used for high-speed straight-line driving and poor following when the pre-look distance is too long when the AGV is in a curve is flexibly solved. The pre-look length is calibrated according to the characteristics of the differential AGV, the trajectory reference point is obtained by using the reference point of the AGV, and the optimal pre-look distance is obtained according to the pre-look mark length and the turning radius obtained by the trajectory reference point, so that the best following effect is achieved. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 is a schematic flow chart of a differential AGV path following method based on improved pure tracking control provided by the present application;

[0050] Figure 2 is a schematic diagram of a kinematic model of a differential AGV path following method based on improved pure tracking control provided by the present application;

[0051] Figure 3 is a schematic diagram of an AGV tracking control relationship based on a kinematic model of a differential AGV path following method based on improved pure tracking control provided by the present application. DETAILED DESCRIPTION

[0052] In order to make the purposes, technical solutions and advantages of the present application clearer, the present application is further described in detail below with reference to the drawings and examples. It should be understood that the specific examples described herein are only used to explain the present application, and cannot be understood as limiting the present application.

[0053] It should be noted that although the functional modules are divided in the system schematic diagram, in some cases, the steps shown or described can be different from the module division in the system or the order of execution in the flow chart. The terms "first", "second", etc. in the specification and claims and the above drawings are used to distinguish similar objects, and do not necessarily describe a specific order or sequence.

[0054] In the description of the present application, it should be noted that, unless otherwise explicitly limited, the words such as setting, installing, connecting, etc. should be broadly understood, and those skilled in the art can reasonably determine the specific meaning of the above words in the present application in combination with the specific content of the technical solution.

[0055] According to the embodiment of the first aspect of the present application, with reference to Figure 1In some embodiments of the present application, a differential type AGV path following method based on improved pure tracking control comprises the following steps:

[0056] S100, the set several control point coordinates are fitted by cubic B-spline curve to obtain the path trajectory of AGV following, and the path trajectory is composed of several path segments.

[0057] In this embodiment, the number of several control point coordinates is 4N, wherein N is a positive integer, and the path trajectory is obtained based on cubic B-spline curve fitting. The number of several path segments is n, that is, the path trajectory can be composed of n path segments, wherein n is a positive integer.

[0058] The cubic B-spline curve equation is written as:

[0059] P(t) = P0 * B 0,3 (t) + P1 * B 1,3 (t) + P2 * B 2,3 (t) + P3 * B 3,3 (t) wherein:

[0060]

[0061] P0, P1, P2, P3 are control point coordinates constituting a cubic B-spline curve.

[0062] It can be written in matrix form:

[0063]

[0064] S200, the midpoint of the AGV axle is taken as the AGV reference point, and the point closest to the current path segment is taken as the current trajectory reference point.

[0065] In this embodiment, according to the differential wheel mechanical structure of AGV, the midpoint of the axle is taken as the reference point of the differential type AGV model, wherein the model reference point is effective for all AGVs. The point closest to the AGV reference point on the current path segment is taken as the trajectory reference point, and the trajectory reference point is constantly followed and updated during AGV walking, so as to achieve the effect of following the set trajectory path.

[0066] S300, according to the trajectory reference point, the turning radius is calculated, and the set speed of AGV is planned according to the turning radius.

[0067] In this embodiment, the turning radius is calculated by using the trajectory reference point obtained in S200. The set speed is linearly planned by using the obtained turning radius. The set speed is affected by the turning radius, so that when the turning radius is smaller, the planned speed is smaller, and the trajectory can be tracked more accurately, and when the turning radius is larger, the planned set speed is larger.

[0068] In S400, the preview mark length is calculated according to the obtained current speed difference between the differential right wheel and the differential left wheel of the AGV.

[0069] In this embodiment, the current speed difference between the differential right wheel and the differential left wheel in the differential mechanism of the AGV is obtained, and the preview mark length is calculated by using the obtained speed difference. The preview mark length is not the preview distance.

[0070] In S500, the preview point coordinates are calculated according to the path length from the segment starting point of the current path segment to the trajectory reference point, the length of the current path segment, the relationship between the current path segment and the path trajectory, and the preview mark length.

[0071] In this embodiment, the path length from the segment starting point of the current path segment to the trajectory reference point is obtained by using the cubic B-spline curve equation in S100, introducing the three-point Legendre integral formula, and using the numerical integral method. The preview point coordinates are calculated by using the position relationship of the current path segment on the path trajectory, the length of the current path segment, and the preview mark length obtained in S400.

[0072] In S600, the preview distance and the included angle between the AGV reference point and the trajectory reference point are calculated according to the preview point coordinates and the AGV reference point.

[0073] In this embodiment, the preview distance between the AGV reference point and the preview point, and the included angle between the AGV reference point and the trajectory reference point are calculated by using the preview point coordinates obtained in S500 and the AGV reference point obtained in S200.

[0074] In S700, the speeds of the differential left wheel and the differential right wheel are calculated according to the set speed, the preview distance, and the included angle between the AGV reference point and the trajectory reference point by using the constructed kinematic model of the differential AGV.

[0075] In this embodiment, the kinematic model of the differential AGV is constructed, and the speeds of the differential left wheel and the differential right wheel are calculated according to the set speed obtained in S300, the preview distance obtained in S600, and the included angle obtained in S600.

[0076] Compared with the traditional pure tracking control, the current speed difference of the differential left and right wheels of the AGV is used to obtain the pre-look mark length, and the current pre-look distance is obtained, according to the constructed kinematic model of the differential AGV, the speed of the differential left and right wheels required to reach the pre-look point is obtained, and the problem that the pre-look distance is too short to cause oscillation when pure tracking control is used for high-speed straight-line driving and the pre-look distance is too long to cause poor following when pure tracking control is used for curved driving is flexibly solved. The pre-look length is calibrated according to the characteristics of the differential AGV, the trajectory reference point is obtained by using the reference point of the AGV, and the optimal pre-look distance is obtained by using the pre-look mark length and the turning radius obtained by the trajectory reference point, so that the best following effect is achieved.

[0077] In some embodiments of the application, the selection process of the trajectory reference point in S200 specifically includes:

[0078] S210, setting the trajectory reference point, and obtaining the distance formula between two points by the AGV reference point obtained in S100.

[0079] In this embodiment, the trajectory reference point is set as (x c ,y c ), and the distance formula between two points is obtained by the AGV reference point (X r ,Y r ):

[0080] S220, the derivative of the distance formula between two points in S210 is calculated, the minimum value of the derivative of the distance formula between two points is calculated by Newton iteration method, and the first moving coefficient is obtained.

[0081] In this embodiment, the derivative of is calculated, and f(t) is set as (X r -x c ) 2 +(Y r -x c ) 2 Since the cubic B-spline has geometric invariance and convexity, there is only one minimum distance between the AGV reference point and the trajectory, and when f ′ (t) = 0, there is a minimum value of f(t), that is, the minimum distance d min can be calculated. The minimum value of f(t) is calculated by using Newton iteration method, and is iterated for multiple times, and the error factor is set. When the number of iterations reaches the set maximum number of iterations or is less than the error factor, the value of t1 is output, and at this time, the value of t1 is the first moving coefficient.

[0082] S230, the current trajectory reference point is calculated by the first moving coefficient in S220 and all control point coordinates on the current path segment.

[0083] In this embodiment, all control point coordinates of the current path segment are acquired, a cubic B-spline curve is constructed using all the control point coordinates, the first movement coefficient t1 is substituted into the cubic B-spline curve of the current path segment through the first movement coefficient t1 in S220, and the current trajectory reference point (X c ,Y c ) is obtained.

[0084] In some embodiments of the present application, the calculation of the turning radius and the planning of the setting speed in S300 specifically include:

[0085] S310, a step is added to the first movement coefficient t1 of the trajectory reference point obtained in S220 to obtain the rear control point coordinate (X c+1 ,Y c+1 ), and a step is subtracted from the first movement coefficient t1 to obtain the front control point coordinate (X c-1 ,Y c-1 ).

[0086] In this embodiment, a step is added to the first movement coefficient t1 of the trajectory reference point obtained in S220, the t1 after the step is added is substituted into the cubic B-spline curve equation of the current path segment, and the rear control point coordinate of the trajectory reference point, i.e., the front control point coordinate (X c+1 ,Y c+1 ) is obtained.

[0087] A step is subtracted from the first movement coefficient t1 of the trajectory reference point obtained in S220, the t1 after the step is subtracted is substituted into the cubic B-spline curve equation of the current path segment, and the front control point coordinate of the trajectory reference point (X c+1 ,Y c+1 ) is obtained.

[0088] S320, the front control point coordinate (X c-1 ,Y c-1 ), the trajectory reference point (X c ,Y c ), and the rear control point coordinate (X c+1 ,Y c+1 ) are used to calculate the turning radius R c at the trajectory reference point.

[0089] In this embodiment, the three control point coordinates obtained are used to calculate the turning radius R c at the trajectory reference point (X c ,Y c ) through the following formula:

[0090]

[0091] .

[0092] S330, set the maximum threshold for turning radius to R max The minimum threshold for the turning radius is set to R. min Set the maximum speed threshold to V. max The minimum threshold for speed is set to V. min By planning for a given speed, the planning formula is obtained:

[0093]

[0094] In this embodiment, the maximum threshold of R is set to R. max The minimum threshold for the turning radius is set to R. min Set the maximum speed threshold to V. max The minimum threshold for speed is set to V. min For the set speed V c By performing linear programming, we obtain the programming equation:

[0095]

[0096] Set speed V c Subject to turning radius R c The smaller the turning radius, the lower the planned speed, allowing for more accurate trajectory tracking. Conversely, when the turning radius is larger, the planned speed can be set to be higher.

[0097] In some embodiments of the present invention, the process of S400 calculating the length of the pre-aiming mark specifically includes:

[0098] S410: Obtain the current speed of the left differential wheel and the current speed of the right differential wheel. Calculate the current speed difference ΔV based on the two current speeds.

[0099] In this embodiment, the current speed of the differential left wheel is read by the encoder of the differential left wheel, and the current speed of the differential right wheel is read by the encoder of the differential right wheel. The current speed difference ΔV is calculated based on the current speeds of the differential left wheel and the differential right wheel.

[0100] S420 calculates the pre-aiming target length L based on the current speed difference and the set distance parameters. pp .

[0101] In this embodiment, the length of the pre-aiming marker is selected as a function of the current vehicle speed and the difference between the differential wheel speeds, L pp =K1*ΔV+L c L c For the set distance parameter (L when in a straight line) pp =L c), K1 is a differential ratio coefficient, and AV is a speed difference of the two differential gears.

[0102] The calibration preview length is set according to the characteristics of the differential AGV, and the problems of vibration caused by too short preview distance when the pure tracking control is in straight-line high-speed running and poor following caused by too long preview distance when the pure tracking control is in a curve are flexibly solved.

[0103] In some embodiments of the present application, in S500, the trajectory reference point selection process specifically includes:

[0104] S510, according to the trajectory reference point (X c ,Y c ), the path length S(t) from the start point of the current path segment to the trajectory reference point is calculated by using a numerical integration method;

[0105] In this embodiment, the first derivative of the cubic B-spline curve formula of the current path segment is calculated to obtain:

[0106] ,

[0107] Then the speed can be expressed as:

[0108] ,

[0109] The three-point-Legendre integral formula is introduced:

[0110] ,

[0111] The path length from the start point of the current path segment to the trajectory reference point is calculated by using a numerical integration method: wherein,

[0112] S511, if S(t)+L pp ≤S k (1), then the AGV can reach the preview point after passing through the preview calibration length L pp , and the preview point falls on the current path segment.

[0113] In this embodiment, when S(t)+L pp ≤S k (1), the AGV can reach the preview point after passing through the preview calibration length L pp , and the preview point falls on the current path segment. S k (1) is the length of the current path segment.

[0114] S512, the second movement coefficient of the preview point on the current path segment is calculated by using the Newton iteration method.

[0115] In this embodiment, the second moving coefficient of the preview point on the current path is solved by Newton iteration method:

[0116]

[0117] The iteration is repeated until the accuracy reaches the tolerance standard, the iteration is stopped and the result is output, that is, the second moving coefficient t2 is output, wherein S p =S(t)+L pp .

[0118] S513, the second moving coefficient t2 obtained by S512, and all control point coordinates constituting the current path segment are used to calculate the preview point coordinates.

[0119] In this embodiment, the cubic B-spline curve equation is obtained by all control point coordinates of the current path segment, the second moving coefficient t2 is brought into the cubic B-spline curve equation, and the preview point coordinates (X p ,Y p ) on the current path segment are obtained.

[0120] In some embodiments of the present application, in S500, the trajectory reference point selection process further comprises:

[0121] S520, if S(t)+L pp >S k (1), and the current path segment is not the last path segment, then the preview point is located on the next path segment.

[0122] In this embodiment, when S(t)+L pp ≤S k (1), and the current path segment is not the last path segment (k k (1) is the length of the current path segment.

[0123] S521, the third moving coefficient of the preview point on the next path segment is calculated by Newton iteration method.

[0124] In this embodiment, the third moving coefficient of the preview point on the current path is solved by Newton iteration method:

[0125]

[0126] The iteration is repeated until the accuracy reaches the tolerance standard, the iteration is stopped and the result is output, that is, the third moving coefficient t3 is output, wherein S pk =L p +L pp -S k (1).

[0127] S522, the third moving coefficient t3 obtained by S521 and all control point coordinates of the next path segment are used to calculate the preview point coordinates.

[0128] In this embodiment, the cubic B-spline curve equation is obtained by all control point coordinates of the next path segment, and the third moving coefficient t3 is brought into the cubic B-spline curve equation to obtain the preview point coordinates (X p ,Y p ) on the current path segment.

[0129] In some embodiments of the present application, in S500, the trajectory reference point selection process further comprises:

[0130] S530, if S(t)+L pp >S k (1), and the current path segment is the last path segment, the preview point is located outside the end point of the path trajectory.

[0131] In this embodiment, if S(t)+L pp >S k (1), and the current path segment is the last path segment (k=n), n is a positive integer, and the path trajectory is composed of n path segments. It is indicated that the preview point is located outside the end point (X e ,Y e ,θ e ) of the path trajectory.

[0132] S531, the preview point coordinates are calculated by the end point coordinate information of the path trajectory and the set calibration distance.

[0133] In this embodiment, in order to achieve the purpose that the AGV can smoothly reach the end point and reduce the oscillation of the steering wheel, a point set at a calibration distance behind the end point (X e ,Y e ,θ e ) of the path trajectory is defined as the preview end point, and the coordinates of the calibration preview point are (X e +D*cosθ e ,Y e +D*sinθ e ), and D is the set calibration distance.

[0134] Referring to Figure 2 , in some embodiments of the present application, in S600, the preview distance and the angle calculation process between the AGV reference point and the trajectory reference point further comprises:

[0135] S610, the preview point coordinates (X p ,Y p), and the AGV reference point (X) obtained through S200. r ,Y r ,θ r The aiming distance L was calculated. d .

[0136] S620, the coordinates of the pre-aiming point (X) are dynamically selected by S500. p ,Y p ), and the AGV reference point (X) obtained through S200. r ,Y r ,θ r The angle α between the AGV reference point and the trajectory reference point is calculated.

[0137] In the above embodiments, reference is made to Figure 2 Based on the selected aiming point coordinates (X... p ,Y p ) and AGV reference point (X) r ,Y r ,θ r The distance between the two points is calculated using the equation for the distance between them.

[0138]

[0139] The aiming distance L is obtained. d .

[0140] Using the angle calculation formula:

[0141]

[0142] The angle α between the AGV reference point and the trajectory reference point is calculated.

[0143] By selecting the aforementioned dynamic preview points, when the driving path is a straight line, the curvature of the path is 0, L pp =L d The aiming distance is equal to the given aiming mark length, and the aiming distance is directly positively correlated with the current driving speed, allowing the AGV to drive smoothly; however, when entering a curved path, the path curvature is not 0, and L... pp >L d When the turning radius of the curve is smaller (the curve is sharper), the speed difference is larger, L pp Much larger than L d A smaller pre-aiming distance results in higher tracking accuracy and thus the best tracking effect. This is achieved by calibrating a suitable pre-aiming distance based on the turning radius of the trajectory, while L is determined based on the speed difference. ppThe two superimposed operations affect the subsequent operation from the beginning, and the resulting preview distance is the optimal solution. The characteristics of the differential AGV are calibrated to obtain the optimal preview length, the trajectory reference point is obtained using the reference point of the AGV, and the optimal preview distance is obtained according to the preview mark length and the turning radius obtained by the trajectory reference point, so as to achieve the best following effect.

[0144] Reference Figure 2 and Figure 3 In some embodiments of the present application, in S700, the speed calculation process of the differential left and right wheels specifically includes:

[0145] S710, the differential AGV kinematics model is constructed by the distance between the differential left and right wheels and the AGV reference point obtained in S200.

[0146] In this embodiment, referring to Figure 2 , Figure 2 is the differential AGV model, and the differential AGV kinematics model is constructed according to the AGV reference point (X r , Y r , θ r ) obtained in S200:

[0147]

[0148] Reference point velocity

[0149] Then the left wheel speed

[0150] Right wheel speed Wherein, L is the distance between the two differential wheels.

[0151] S720, based on the differential AGV kinematics model constructed in S710, the obtained preview distance, the set speed and the angle between the AGV reference point and the trajectory reference point are combined to calculate the speed of the differential left and right wheels.

[0152] In this embodiment, referring to Figure 3 , Figure 3 is the AGV tracking control relationship diagram based on the kinematics model.

[0153] By the sine theorem:

[0154] ,

[0155] That is:

[0156]

[0157] According to the preview distance L d , the set speed Vc The angle a between the AGV reference point and the trajectory reference point can be used to derive the differential mechanism left and right wheel speed:

[0158]

[0159] According to the embodiment of the second aspect of the present application, a differential AGV path following system based on improved pure tracking control, the system comprises: at least one processor, at least one memory.

[0160] When at least one program is executed by the at least one processor, the at least one processor implements a differential AGV path following method based on improved pure tracking control as any one of the first aspect.

[0161] The processor and the memory can be connected by a bus or other means.

[0162] The memory as a non-transitory computer readable storage medium can be used to store non-transitory software programs and non-transitory computer executable programs, such as the differential AGV path following method based on improved pure tracking control described in the embodiment of the present application. The processor realizes the differential AGV path following method based on improved pure tracking control of the first aspect of the present application by running the non-transitory software programs and instructions stored in the memory.

[0163] The memory can include a program storage area and a data storage area, wherein the program storage area can store the operating system and the application program required by at least one function; the data storage area can store the above-mentioned differential AGV path following method based on improved pure tracking control. In addition, the memory can include a high-speed random access memory, and can also include a non-transitory memory, such as at least one magnetic disk storage device, a flash memory device, or other non-transitory solid-state storage device. In some embodiments, the memory can optionally include a memory remotely arranged relative to the processor, and these remote memories can be connected to the processor through a network. Examples of the above-mentioned network include but are not limited to the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0164] As will be appreciated by one of ordinary skill in the art, all or some of the steps, systems, etc. in the above-disclosed methods can be embodied in software, firmware, hardware, and / or suitable combinations thereof. Some or all of the physical components can be implemented with software executed by a processor, such as a central processing unit, a digital signal processor, or microprocessor, or can be implemented as hardware, or as an integrated circuit, such as an application- specific integrated circuit. Such software can be distributed on computer readable media, which can comprise computer storage media (or non-transitory media), and communication media (or transitory media). As is well known to those of ordinary skill in the art, computer storage media includes both volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. Computer storage media includes, but is not limited to, RAM, ROM, EEPROM, flash memory or other memory technology, CD-ROM, digital versatile disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium which can be used to store the desired information and which can be accessed by a computer. Further, as will be appreciated by one skilled in the art, communication media typically embodies computer readable instructions, data structures, program modules, or other data in a modulated data signal, such as carrier waves or other transport mechanisms, and includes any information delivery media.

[0165] The preferred embodiments of the present application have been disclosed with the understanding that variations of the details are possible without departing from the spirit of the application, the scope of which is defined by the appended claims.

Claims

1. A differential type AGV path following method based on improved pure pursuit control, characterized in that, The application relates to an improved pure tracking control-based differential AGV path following method. The application comprises the following steps: A third-order B-spline curve is used to fit a plurality of control point coordinates to obtain a path trajectory of AGV following, wherein the path trajectory is composed of a plurality of path segments; A midpoint of an AGV wheel shaft is taken as an AGV reference point, and a point closest to a current path segment is taken as a current trajectory reference point; A turning radius is calculated according to the trajectory reference point, and a set speed of the AGV is planned according to the turning radius; A preview mark length is calculated according to a current speed difference between left and right wheels of the AGV; A preview point coordinate is calculated according to a path length from a segment starting point of the current path segment to the trajectory reference point, a length of the current path segment, a relationship between the current path segment and the path trajectory and the preview mark length; A preview distance and an included angle between the AGV reference point and the trajectory reference point are calculated according to the preview point coordinate and the AGV reference point; Speeds of the left and right wheels of the differential AGV are calculated according to the set speed, the preview distance and the included angle between the AGV reference point and the trajectory reference point by using a constructed kinematic model of the differential AGV. The AGV reference point (X r ,Y r ) and the set trajectory reference point are calculated to obtain a two-point distance formula; Deriving the two-point distance formula, using Newton iteration method to calculate the minimum value of the derived two-point distance formula, obtaining the first moving coefficient closest to the AGV reference point (X r ,Y r ) on the current path segment; According to the first movement coefficient and all control point coordinates constituting the current path segment, a current trajectory reference point (X c ,Y c ) is calculated. The trajectory reference point specifically comprises the following steps: The first moving coefficient is added by a step length and subtracted by a step length to obtain a rear control point coordinate (X c+1 ,Y c+1 ) and a front control point coordinate (X c-1 ,Y c-1 ); According to the trajectory reference point (X c ,Y c ), the back control point coordinate (X c+1 ,Y c+1 ) and the front control point coordinate (X c-1 ,Y c-1 ), the turning radius R c at the trajectory reference point is calculated. Let the maximum threshold of the turning radius be R max , and the minimum threshold be R min Let the maximum threshold of the set speed be V max , and the minimum threshold be V min , and the set speed is planned to obtain a planning relationship 2. The differential AGV path following method based on improved pure pursuit control according to claim 1, characterized in that, The set speed of the AGV is planned according to the turning radius calculated according to the trajectory reference point. According to the trajectory reference point (X c ,Y c ), the path length S(t) from the segment starting point of the current path segment to the trajectory reference point is calculated by using a numerical integration method; When S(t) + L pp ≤ S k (1), the preview point is located on the current path segment, where L pp is the preview calibration length, and S k (1) is the length of the current path segment. The preview point coordinate calculation process specifically comprises the following steps: A second moving coefficient of the preview point on the current path segment is calculated by using a Newton iteration method; 3. The differential AGV path following method based on improved pure pursuit control according to claim 2, characterized in that, The preview point coordinate is calculated according to the second moving coefficient and all control point coordinates constituting the current path segment. When S(t) + L pp >S k (1), and the current path segment is not the last segment in the path trajectory, the preview point is located on the next path segment. The preview point coordinate calculation process further specifically comprises the following steps: A third moving coefficient of the preview point on a next path segment is calculated by using a Newton iteration method; 4. The differential AGV path following method based on improved pure pursuit control according to claim 3, characterized in that, The preview point coordinate is calculated according to the third moving coefficient and all control point coordinates constituting the next path segment. when S(t) + L pp > S k (1), and the current path segment is the last segment in the path trajectory, the preview point is located outside the end point of the path trajectory. The preview point coordinate calculation process further specifically comprises the following steps:

5. The differential AGV path following method based on improved pure pursuit control according to claim 1, characterized in that, The preview point coordinate is calculated according to terminal point coordinate information of the path trajectory and a set calibration distance. The preview mark length calculation process specifically comprises the following steps: According to the current speed difference AV and the set distance parameter, the pre-look mark length L is calculated pp .

6. The differential AGV path following method based on improved pure pursuit control according to claim 1, characterized in that, A current speed difference AV is calculated according to a current speed of the left wheel and a current speed of the right wheel of the differential AGV. According to the pre-look point coordinates (X p ,Y p ) and AGV reference point (X r ,Y r ,θ r ), the pre-look distance is calculated; According to the pre-look point coordinates (X p ,Y p ) and AGV reference point (X r ,Y r ,θ r ), the angle calculation formula is used: ， The preview distance and the included angle between the AGV reference point and the trajectory reference point are calculated according to the preview point coordinate.

7. The differential AGV path following method based on improved pure pursuit control according to claim 1, characterized in that, The included angle alpha between the AGV reference point and the trajectory reference point is calculated. The speed calculation process of the left and right wheels of the differential AGV specifically comprises the following steps: A kinematic model of the differential AGV is constructed according to a distance between the left and right wheels of the differential AGV and the AGV reference point; 8. A differential type AGV path following system based on improved pure pursuit control, characterized in that, Speeds of the left and right wheels of the differential AGV are calculated according to the preview distance, the set speed, the included angle between the AGV reference point and the trajectory reference point and the kinematic model of the differential AGV. The system comprises: At least one processor; at least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor realizes the improved pure tracking control-based differential AGV path following method.

Images