AGV trajectory tracking mode automatic switching method and device, equipment and medium

Abstract

The application discloses an AGV track tracking mode automatic switching method, device, equipment and medium, and the method comprises the following steps: S1, multiple track tracking modes with priority levels are set; S2, whether the running period of the current track tracking mode is less than the period threshold value is judged, if yes, S3 is executed, if not, S2 is executed after mode degradation is completed; S3, whether the pose deviation is within the deviation threshold value range is judged, if yes, S4 is executed, if not, S2 is executed after mode promotion is completed; S4, whether the current track tracking mode is suitable for the track section where the AGV is currently located is judged, if yes, S5 is executed, if not, S2 is executed after mode promotion is completed; S5, whether the running speed is within the speed threshold value range is judged, if yes, S6 is executed, if not, S2 is executed after mode promotion is completed; S6, whether the AGV has completed the given path track is judged, if not, S2 is executed, if yes, the running control is ended. The application can realize the autonomous switching of the AGV track tracking mode.

Description

Technical Field

[0001] This invention relates to the field of AGV application technology, specifically to a method, apparatus, equipment, and medium for automatically switching AGV trajectory tracking modes. Background Technology

[0002] AGV (Automated Guided Vehicle) is a transport vehicle capable of traveling along a predetermined path and possessing safety protection and various transfer functions. The ability of an AGV to travel along a predetermined path primarily relies on a suitable trajectory tracking mode. Currently, technicians have proposed trajectory tracking modes suitable for AGVs, including LQR control mode, pure tracking control mode, and PID control mode. However, different trajectory tracking modes have different requirements regarding hardware computing power, applicable paths, and tracking accuracy. When users want the AGV to travel on a predetermined path with constantly changing curvature, and considering hardware computing power limitations, it is usually necessary to manually switch the trajectory tracking mode to improve the AGV's tracking performance, but the entire switching process is quite cumbersome. Summary of the Invention

[0003] This invention provides a method, apparatus, device, and medium for automatic switching of AGV trajectory tracking modes, in order to solve one or more technical problems existing in the prior art, and at least provide a beneficial option or create conditions.

[0004] Firstly, a method for automatically switching AGV trajectory tracking modes is provided, the method comprising:

[0005] Step S100: Set multiple trajectory tracking modes and determine the corresponding priority levels;

[0006] Step S110: Obtain the current trajectory tracking mode used by the AGV when running on the predetermined path trajectory, and determine whether the mode running cycle is less than the cycle threshold; if yes, execute step S120; if no, downgrade the current trajectory tracking mode and return to execute step S110.

[0007] Step S120: Determine whether the AGV pose deviation falls within the deviation threshold range; if yes, proceed to step S130; if no, upgrade the current trajectory tracking mode and return to step S110.

[0008] Step S130: Determine whether the current trajectory tracking mode is applicable to the trajectory segment currently occupied by the AGV; if yes, proceed to step S140; if no, change the current trajectory tracking mode and return to step S110.

[0009] Step S140: Determine whether the AGV's running speed falls within the speed threshold range; if yes, proceed to step S150; if no, upgrade the current trajectory tracking mode and return to step S110.

[0010] Step S150: Determine whether the AGV has completed the predetermined path trajectory; if not, proceed to step S110; if yes, end the AGV operation control.

[0011] Furthermore, the multiple trajectory tracking modes include a high-priority LQR control mode, a medium-priority pure tracking control mode, and a low-priority PID control mode.

[0012] Furthermore, when it is determined that the current trajectory tracking mode is not suitable for the trajectory segment currently occupied by the AGV, the current trajectory tracking mode is upgraded, including:

[0013] When it is identified that the current trajectory tracking mode is LQR control mode or pure tracking control mode, and the current trajectory segment of the AGV is a straight line, the current trajectory tracking mode will be directly downgraded to PID control mode.

[0014] When it is identified that the current trajectory tracking mode is PID control mode and the AGV is currently on a curved trajectory, the current trajectory tracking mode is upgraded to pure tracking control mode.

[0015] Furthermore, the process of obtaining the AGV pose deviation includes:

[0016] Based on the predetermined path trajectory, establish the AGV path trajectory equation with t as the independent variable;

[0017] The first equation is established and the optimal t value is obtained by solving it using Newton's iteration method. The first equation represents the shortest distance between the AGV body reference point and the trajectory reference point on the predetermined path trajectory.

[0018] The pose information of the trajectory reference point is obtained based on the optimal t value and the AGV path trajectory equation, and then the AGV pose deviation is obtained by combining the pose information of the AGV body reference point.

[0019] Furthermore, the AGV position deviation includes AGV distance deviation and AGV posture deviation, and the deviation threshold range is: AGV distance deviation is less than 50mm, and AGV posture deviation is less than 5°.

[0020] Furthermore, the speed threshold range is: the AGV operating speed is greater than 0.5m / s and less than 1m / s.

[0021] Furthermore, the period threshold is 10ms.

[0022] Secondly, an automatic switching device for AGV trajectory tracking modes is provided, the device comprising:

[0023] The settings module is used to set multiple trajectory tracking modes and determine their corresponding priority levels;

[0024] The first judgment module is used to obtain the current trajectory tracking mode adopted by the AGV when running on the predetermined path trajectory, and to determine whether the mode running cycle is less than the cycle threshold; if yes, the second judgment module is run; if no, the current trajectory tracking mode is downgraded and the first judgment module is run.

[0025] The second judgment module is used to determine whether the AGV's pose deviation falls within the deviation threshold range; if yes, the third judgment module is run; if no, the current trajectory tracking mode is upgraded and the first judgment module is run.

[0026] The third judgment module is used to determine whether the current trajectory tracking mode is applicable to the trajectory segment currently occupied by the AGV; if yes, the fourth judgment module is run; if no, the current trajectory tracking mode is upgraded and the first judgment module is run.

[0027] The fourth judgment module is used to determine whether the AGV's running speed falls within the speed threshold range; if yes, the control module is terminated; if no, the current trajectory tracking mode is upgraded and the first judgment module is run.

[0028] The control module is used to determine whether the AGV has completed the predetermined path trajectory; if not, the first judgment module is run; if yes, the AGV operation control is terminated.

[0029] Thirdly, a computer device is provided, including a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the AGV trajectory tracking mode automatic switching method as described in the first aspect.

[0030] Fourthly, a computer-readable storage medium is provided having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the automatic switching method for AGV trajectory tracking mode as described in the first aspect.

[0031] The present invention has at least the following beneficial effects: by proposing four parameters—mode operation cycle, AGV posture deviation, AGV current trajectory segment, and AGV running speed—the present invention sequentially detects and judges these four parameters in real time as the AGV travels along the predetermined path trajectory, and then selects the optimal AGV trajectory tracking mode from LQR control mode, pure tracking control mode, and PID control mode to autonomously switch the current trajectory tracking mode, so that the AGV can better follow the predetermined path trajectory, and has good practicality and flexibility. Attached Figure Description

[0032] The accompanying drawings are provided to further understand the technical solutions of the present invention and constitute a part of the specification. They are used together with the embodiments of the present invention to explain the technical solutions of the present invention, and do not constitute a limitation on the technical solutions of the present invention.

[0033] Figure 1 This is a flowchart illustrating the automatic switching method for AGV trajectory tracking mode in an embodiment of the present invention.

[0034] Figure 2 This is a schematic diagram of the kinematic model of the differential wheel in an embodiment of the present invention;

[0035] Figure 3 This is a schematic diagram of the kinematic model of a single steering wheel in an embodiment of the present invention;

[0036] Figure 4 This is a schematic diagram of the composition of the AGV trajectory tracking mode automatic switching device in an embodiment of the present invention;

[0037] Figure 5 This is a schematic diagram of the hardware structure of the computer device in an embodiment of this disclosure. Detailed Implementation

[0038] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0039] It should be noted that although functional modules are divided in the system diagram and the logical order is shown in the flowchart, in some cases, the steps shown or described may be performed in a different order than the module division in the system or the order in the flowchart. The terms "first," "second," etc., in the specification, claims, and the aforementioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence.

[0040] Please refer to Figure 1 , Figure 1 This is a flowchart illustrating the automatic switching method for AGV trajectory tracking mode provided in an embodiment of the present invention. The method includes the following:

[0041] Step S100: Set multiple trajectory tracking modes and determine the corresponding priority levels;

[0042] Step S110: Obtain the current trajectory tracking mode used by the AGV when running on the predetermined path trajectory, and determine whether the mode running cycle is less than the cycle threshold; if not, continue to execute the following step S120; if yes, continue to execute the following step S130.

[0043] Step S120: Degrade the current trajectory tracking mode, and then return to re-execute step S110;

[0044] Step S130: Determine whether the AGV pose deviation falls within the deviation threshold range; if not, continue to the next step S140; if yes, continue to the next step S150.

[0045] Step S140: Upgrade the current trajectory tracking mode, then return to re-execute step S110;

[0046] Step S150: Determine whether the current trajectory tracking mode is applicable to the trajectory segment currently occupied by the AGV; if not, continue to execute the following step S160; if yes, continue to execute the following step S170.

[0047] Step S160: Change the current trajectory tracking mode, and then return to execute step S110 again;

[0048] Step S170: Determine whether the AGV's running speed falls within the speed threshold range; if not, proceed to step S140; if yes, continue to proceed to step S180.

[0049] Step S180: Determine whether the AGV has completed the predetermined path trajectory; if not, return to re-execute step S110; if yes, continue to execute the following step S190.

[0050] Step S190: End AGV operation control.

[0051] In this embodiment of the invention, the multiple trajectory tracking modes mentioned in step S100 specifically include LQR (Linear Quadratic Regulator) control mode, pure tracking control mode, and PID (Proportion-Integral-Derivative) control mode, and are arranged in descending order of priority as follows: LQR control mode > pure tracking control mode > PID control mode.

[0052] It should be noted that small, lurking AGVs or AGVs with similar structures typically use differential wheel control, while forklift AGVs or AGVs with similar structures typically use single steering wheel control. When any AGV moves in a relatively smooth horizontal direction, without considering the suspension system and with a relatively low speed, a kinematics model is chosen for analysis and application.

[0053] Combination Figure 2The diagram shows the kinematic model of the differential wheel. The midpoint of the AGV differential wheel is set as the reference point M of the AGV body, and its pose information in the Cartesian coordinate system is (x...). c ,yc,θ c And the linear velocity of the AGV body reference point is v. c and angular velocity ω c The spacing between the differential wheels of the AGV is L1. Therefore, the kinematic model of the differential wheels can be constructed as follows:

[0054]

[0055] Based on the aforementioned differential wheel kinematic model, the corresponding discrete state-space equations are constructed as follows:

[0056]

[0057] In the formula:

[0058]

[0059] in, Collectively referred to as AGV kinematic parameters, v l v is the linear velocity of one of the differential wheels on the AGV. r Let be the linear velocity of the other differential wheel on the AGV, as the AGV runs along a predetermined path. It is characterized as the state error between the AGV body reference point and any point on the predetermined path trajectory after a sampling period T. Characterized as the state error between the AGV body reference point and any point on the predetermined path trajectory before the sampling period T, v r Let ω be the linear velocity of any point on a given path trajectory. r Let θ be the angular velocity of any point on a given path trajectory. r It is the tangent angle at any point on the given path trajectory.

[0060] Combination Figure 3 The schematic diagram of the single steering wheel kinematic model shown is given. The midpoint of the AGV's rear wheel axle is set as the AGV's reference point M, and its pose information in the Cartesian coordinate system is (x...). c y c θ c Let the AGV steering wheel deflection angle be δ, the longitudinal distance between the AGV steering wheel and the midpoint of the rear wheel axle be L2, the AGV steering wheel speed be v, the instantaneous rotation center of the AGV steering wheel on the predetermined path be P, and the instantaneous rotation radius of the AGV steering wheel on the predetermined path be R. Then, the kinematic model of a single steering wheel can be constructed as follows:

[0061]

[0062] Based on the single steering wheel kinematic model, the corresponding discrete state-space equations are constructed as follows:

[0063]

[0064] In the formula: δ=arctan(L2 / R), R=v / ω;

[0065] in, Collectively referred to as AGV kinematic parameters, It is characterized as the state error between the AGV body reference point and any point on the predetermined path trajectory after a sampling period T. Characterized as the state error between the AGV body reference point and any point on the predetermined path trajectory before the sampling period T, v r Let θ be the linear velocity of any point on a given path trajectory. r Let ω be the tangent angle at any point on the given path trajectory, ω be the angular velocity of the AGV steering wheel, and δ be the angular velocity of the AGV steering wheel. r The angle of deviation is the angle at any point on the given path trajectory.

[0066] It should be noted that since the AGV is a rigid body model, when the AGV uses a multi-steering wheel control method, the deflection angle and speed of each steering wheel can be derived from the installation position of each steering wheel relative to the reference point of the AGV body, the deflection angle and speed of each steering wheel, the equal angular velocities of each steering wheel, and the instantaneous rotation radius of each steering wheel.

[0067] Based on the above differential wheel kinematic model and the above single steering wheel kinematic model, the advantages, disadvantages, and implementation principles of the multiple trajectory tracking modes are explained as follows:

[0068] Firstly, the LQR control mode has poor robustness under external interference and requires high computing power. However, when the AGV's running speed increases, it can keep the steady-state error small. It is suitable for paths with large curvature and has extremely high requirements for path following accuracy. The AGV will follow the path at medium to high speed.

[0069] The implementation process of LQR control mode is as follows: For the discrete state-space equations associated with the differential wheel kinematic model or the discrete state-space equations associated with the single steering wheel kinematic model, the objective function for the tracking control law performance index is expressed using a functional model as follows:

[0070]

[0071] Based on the principle of extreme values, the objective function above is optimized and solved to obtain the optimal control law:

[0072] u f=[(R+B T PB) -1 B T PA]X;

[0073] Solving the Riccati equation using an iterative method yields the following positive definite constant matrix:

[0074] P = A T PA-A T PB(R+B T PB) -1 B T PA+Q;

[0075] Using the positive definite constant matrix P, the optimal feedback matrix is ​​obtained as: K = R -1 B T p;

[0076] Using the optimal control law u f Given the optimal feedback matrix K, the actual control law is:

[0077] Where X is a state variable. Let X be the estimated value of the state variable X, Q be the positive semi-definite state weighting matrix, where increasing the size of its internal elements indicates a desire for the tracking error to quickly approach zero, δ be the feedback control variable, and R be the positive semi-definite state weighting matrix, where increasing the size of its internal elements indicates a desire to minimize the control input. This optimization objective represents the cumulative magnitude of the deviation along the path, δ. T The optimization objective Rδ represents the energy loss during tracking, where A and B are coefficient matrices and T is the transpose.

[0078] Secondly, the pure tracking control mode has good robustness under external interference and low computing power requirements, but the steady-state error will increase when the AGV's running speed increases. It is suitable for paths with large curvature and has high requirements for path following accuracy. The AGV will follow the path at a medium speed.

[0079] The pure tracking control mode calculates the current differential wheel speed or the current steering wheel deflection angle using geometric methods based on various trajectory reference points on the predetermined path the AGV will travel. Specifically, it sets the pre-aiming distance to L. d Obtain the pre-aiming distance L extending backward from the current trajectory reference point. d The point is determined, and the attitude deviation between this point and the AGV body reference point is α; when the AGV uses differential wheel control, the current differential wheel speed is calculated as: v l =v c (1+L1sinα / L d ), v r =vc (1-L1sinα / L d When the AGV uses a single steering wheel control method, the current steering wheel deflection angle is calculated as: δ=arctan(2L2sinα / L) d ).

[0080] Third, the PID control mode has poor robustness under external disturbances. When the AGV's running speed increases, the steady-state error will also increase. However, it has low computing power requirements, is suitable for paths with small curvature, and does not have high requirements for path following accuracy. The AGV can follow the path at low speed.

[0081] The specific implementation process of PID control mode is as follows: When the AGV uses differential wheel control, the theoretical differential wheel speed is used as the input of the existing PID controller, and the actual differential wheel speed is used as the output of the existing PID controller; when the AGV uses single steering wheel control, the theoretical steering wheel deflection angle is used as the input of the existing PID controller, and the actual steering wheel deflection angle is used as the output of the existing PID controller.

[0082] It should be noted that, since the error is relatively large when the AGV starts to follow the predetermined path trajectory, when the AGV starts to enter the predetermined path trajectory, this embodiment of the invention selects the LQR control mode with the best following effect and associated with the AGV's corresponding kinematic model to achieve the fastest following response.

[0083] In this embodiment of the invention, the period threshold mentioned in step S110 is preferably 10ms. When a chip with poor computing power executes multiple threads that consume a lot of memory at the same time, such as threads used to implement AGV obstacle avoidance function, threads used to implement AGV path planning, etc., if the running cycle occupied by the current trajectory tracking mode is greater than 10ms, a large software delay (5ms-15ms) will be generated, resulting in a large steady-state error in the entire closed-loop control.

[0084] In this embodiment of the invention, the process of obtaining the mode operation cycle mentioned in step S110 is as follows: First, when the current trajectory tracking mode starts to switch to application, the system records the current timestamp, which is recorded as the first time; second, when the above step S110 is executed, the system records the current timestamp, which is recorded as the second time; then the difference between the second time and the first time is used as the operation cycle of the current trajectory tracking mode.

[0085] In this embodiment of the invention, the degradation processing mentioned in step S120 above is described by way of enumeration as follows: When the current trajectory tracking mode is identified as LQR control mode, the current trajectory tracking mode can only be downgraded to pure tracking control mode; or, when the current trajectory tracking mode is identified as pure tracking control mode, the current trajectory tracking mode is downgraded to PID control mode; or, when the current trajectory tracking mode is identified as PID control mode, the system defaults to automatically converting the degradation processing method of the current trajectory tracking mode to the hold processing method, that is, holding the current trajectory tracking mode as PID control mode.

[0086] In this embodiment of the invention, the specific implementation process of step S130 includes the following:

[0087] Step S131: The predetermined path trajectory is constructed based on the existing cubic B-spline curve fitting principle. Therefore, the AGV path trajectory equation expressed in the world coordinate system can be obtained. The AGV path trajectory equation has t as the independent variable, and the corresponding expression is:

[0088] P(t) = P0 * B 0，3 (t)+P1*B 1，3 (t)+P2*B 2，3 (t)+P3*B 3，3 (t);

[0089]

[0090]

[0091] Where P(t) represents the AGV path trajectory equation, P0, P1, P2, and P3 are the coordinate information of any four consecutive control points used to fit the predetermined path trajectory, and B 0，3 (t), B 1，3 (t), B 2，3 (t), B 3，3 (t) all represent basis functions;

[0092] Step S132: Construct the first equation, which represents the shortest distance between the trajectory reference point and the AGV body reference point on the predetermined path; the AGV body reference point refers to the midpoint of the AGV rear wheel axle, and its specific coordinate information in the world coordinate system is (x... c y c and the tangent angle is θ c The trajectory reference point is the point on the predetermined path trajectory that is closest to the AGV body reference point, and its coordinate information in the world coordinate system is to be set as (x... r y rThe two reference points mentioned above will be continuously updated during the AGV's movement to achieve the effect of the AGV following the predetermined path trajectory. Since the trajectory reference points fall on the predetermined path trajectory, the coordinate information of the trajectory reference points can be determined according to the AGV path trajectory equation. The actual influence of the independent variable t on the coordinate information of the trajectory reference points can be determined. At this time, the expression of the first equation is determined as follows:

[0093] f(t) = (x c -x r ) 2 +(x c -y r ) 2 ;

[0094] Step S133: To ensure high efficiency of the algorithm, the first equation is solved using Newton's iteration method to obtain the optimal value of t. The corresponding implementation method is as follows: The pre-constructed equation t... n+1 =f′(t n ) / f″(t n Perform multiple iterations, and when the number of iterations reaches the set maximum number of iterations or t... n+1 When the value is less than the set error factor, the optimal t value is output; where f′(t) n ) represents the first equation f(t) at t = t n The first derivative in the case of f″(t) n ) represents the first equation f(t) at t = t n The second derivative in the case of t n+1 This is the current value of t obtained in the (n+1)th iteration;

[0095] Step S134: Substituting the optimal t value into the AGV path trajectory equation, the specific coordinate information of the trajectory reference point in the world coordinate system can be solved as (x r y r After taking the first derivative of the AGV path trajectory equation and substituting the optimal t value, the tangent angle θ at the trajectory reference point can be solved. r ;

[0096] Step S135: Based on the pose information of the AGV body reference point and the trajectory reference point, determine the AGV pose deviation relative to the predetermined path trajectory, as follows:

[0097] (1) The AGV distance deviation is:

[0098] (2) The AGV attitude deviation is: Δθ=θ c -θ r ;

[0099] Step S136: Determine whether the AGV distance deviation is less than 50mm and whether the AGV posture deviation is less than 5°. If both conditions are met, it means that the AGV posture deviation falls within the deviation threshold range. If only one condition is met or neither condition is met, it means that the AGV posture deviation does not fall within the deviation threshold range.

[0100] In step S136 above, if at least one of the following conditions is met: AGV distance deviation greater than 100mm and AGV posture deviation greater than 10°, it can be determined that the AGV has significantly deviated from the predetermined path trajectory and may be at risk of colliding with surrounding objects; if at least one of the following conditions is met: AGV distance deviation less than 100mm but greater than 50mm and AGV posture deviation less than 10° but greater than 5°, it can be determined that the AGV's following effect on the predetermined path trajectory is poor.

[0101] In this embodiment of the invention, the upgrade process mentioned in step S140 above is described by way of enumeration as follows: When the current trajectory tracking mode is identified as PID control mode, the current trajectory tracking mode can only be upgraded to pure tracking control mode; or, when the current trajectory tracking mode is identified as pure tracking control mode, the current trajectory tracking mode is upgraded to LQR control mode; or, when the current trajectory tracking mode is identified as LQR control mode, the system defaults to automatically converting the upgrade process of the current trajectory tracking mode to the hold process, that is, keeping the current trajectory tracking mode as LQR control mode.

[0102] In this embodiment of the invention, the specific implementation process of steps S150 and S160 is described by way of enumeration as follows:

[0103] (1) When it is identified that the current trajectory tracking mode is LQR control mode or pure tracking control mode, and the current trajectory segment of the AGV is a curved trajectory, it is determined that the current trajectory tracking mode is applicable to the current trajectory segment of the AGV.

[0104] (2) When it is identified that the current trajectory tracking mode is LQR control mode or pure tracking control mode, and the trajectory segment where the AGV is currently located is a straight trajectory, it is determined that the current trajectory tracking mode is not applicable to the trajectory segment where the AGV is currently located. At this time, the current trajectory tracking mode is directly downgraded to PID control mode.

[0105] (3) When it is identified that the current trajectory tracking mode is PID control mode and the current trajectory segment of the AGV is a straight trajectory, it is determined that the current trajectory tracking mode is applicable to the current trajectory segment of the AGV.

[0106] (4) When it is identified that the current trajectory tracking mode is PID control mode and the current trajectory segment of the AGV is a curve, it is determined that the current trajectory tracking mode is not applicable to the current trajectory segment of the AGV, and the current trajectory tracking mode is directly upgraded to pure tracking control mode.

[0107] In step S150 above, the process of determining the trajectory segment currently occupied by the AGV is as follows: the predetermined path trajectory is obtained by sequentially connecting N-3 cubic B-spline curves fitted by N control points, where N is a positive integer and N≥4. A cubic B-spline curve is selected from the N-3 cubic B-spline curves, such that the starting point of the selected cubic B-spline curve is closest to the reference point of the AGV body, and the trajectory reference point closest to the reference point of the AGV body on the predetermined path trajectory falls on the selected cubic B-spline curve. At this time, the selected cubic B-spline curve is the trajectory segment currently occupied by the AGV.

[0108] It should be noted that when the four consecutive control points on which the AGV is currently located during the fitting process are on the same straight line, the AGV's current trajectory segment can be considered a straight trajectory; conversely, when the four consecutive control points on which the AGV's current trajectory segment is currently located during the fitting process are not on the same straight line, the AGV's current trajectory segment can be considered a curved trajectory.

[0109] In this embodiment of the invention, the implementation process of step S170 is as follows: the maximum rated speed of the AGV is predetermined to be 1.5m / s. The system obtains the AGV running speed in real time and determines whether it is less than 1m / s but greater than 0.5m / s. If it is, it means that the AGV running speed falls within the speed threshold range. If it is not, it means that the AGV running speed does not fall within the speed threshold range.

[0110] In this embodiment of the invention, four parameters are proposed: mode operation cycle, AGV posture deviation, AGV current trajectory segment, and AGV running speed. These four parameters are detected and judged in real time as the AGV travels along the predetermined path trajectory. Then, the AGV autonomously switches between LQR control mode, pure tracking control mode and PID control mode to optimize the current trajectory tracking mode, so that the AGV can better follow the predetermined path trajectory, which has good practicality and flexibility.

[0111] Please refer to Figure 4 , Figure 4 This is a schematic diagram of the composition of the AGV trajectory tracking mode automatic switching device provided in an embodiment of the present invention. The device includes the following:

[0112] The setting module 210 is used to set multiple trajectory tracking modes and determine the priority level corresponding to each trajectory tracking mode.

[0113] The first judgment module 220 is used to obtain the current trajectory tracking mode adopted by the AGV when it runs on a predetermined path trajectory, and to determine whether the mode running cycle is less than the cycle threshold. If it is true, the second judgment module 230 is run. If it is not true, the current trajectory tracking mode is downgraded and the first judgment module 220 is run again.

[0114] The second judgment module 230 is used to determine whether the AGV pose deviation falls within the deviation threshold range. If it does, the third judgment module 240 is run. If it does not, the current trajectory tracking mode is upgraded and the first judgment module 220 is run again.

[0115] The third judgment module 240 is used to determine whether the current trajectory tracking mode is applicable to the trajectory segment currently occupied by the AGV; if it is, the fourth judgment module 250 is run; if it is not, the current trajectory tracking mode is changed, and the first judgment module 220 is run again.

[0116] The fourth judgment module 250 is used to determine whether the AGV running speed falls within the speed threshold range; if it does, the running end control module 260 is activated; if it does not, the current trajectory tracking mode is upgraded and the first judgment module 220 is run again.

[0117] The control module 260 is used to determine whether the AGV has completed the predetermined path trajectory; if not, the first judgment module 220 is run again; if yes, the AGV operation control is terminated.

[0118] The content of the above method embodiments is applicable to the device embodiments. The functions implemented by the device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are the same as those of the above method embodiments, so they will not be repeated here.

[0119] Furthermore, embodiments of the present invention also provide a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it implements the automatic switching method for AGV trajectory tracking modes described in the above embodiments. The computer-readable storage medium includes, but is not limited to, any type of disk (including floppy disks, hard disks, optical disks, CD-ROMs, and magneto-optical disks), ROM (Read-Only Memory), RAM (Random Access Memory), EPROM (Erasable Programmable Read-Only Memory), EEPROM (Electrically Erasable Programmable Read-Only Memory), flash memory, magnetic cards, or optical cards. In other words, the storage device includes any medium on which a device (e.g., a computer, mobile phone, etc.) stores or transmits information in a readable form, and can be a read-only memory, a disk, or an optical disk, etc.

[0120] also, Figure 5 This is a schematic diagram of the hardware structure of a computer device provided in an embodiment of the present invention. The computer device includes components such as a processor 320, a memory 330, an input unit 340, and a display unit 350. Those skilled in the art will understand that... Figure 5 The illustrated device structure is not intended to limit all devices and may include more or fewer components than shown, or combine certain components. The memory 330 can be used to store the computer program 310 and various functional modules. The processor 320 runs the computer program 310 stored in the memory 330, thereby performing various functional applications and data processing of the device. The memory can be internal memory or external memory, or include both internal and external memory. Internal memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, or random access memory. External memory may include hard disks, floppy disks, ZIP disks, USB flash drives, magnetic tapes, etc. The memory 330 disclosed in the embodiments of this invention includes, but is not limited to, these types of memory. The memory 330 disclosed in the embodiments of this invention is only an example and not a limitation.

[0121] Input unit 340 is used to receive signal input and user-input keywords. Input unit 340 may include a touch panel and other input devices. The touch panel can collect user touch operations on or near it (such as operations performed by the user using a finger, stylus, or any suitable object or accessory on or near the touch panel) and drive the corresponding connection device according to a pre-set program; other input devices may include, but are not limited to, one or more of physical keyboards, function keys (such as play control buttons, power buttons, etc.), trackballs, mice, joysticks, etc. Display unit 350 can be used to display user-input information or information provided to the user, as well as various menus of the terminal device. Display unit 350 may be in the form of a liquid crystal display, organic light-emitting diode, etc. Processor 320 is the control center of the terminal device, connecting various parts of the entire device through various interfaces and lines, performing various functions and processing data by running or executing software programs and / or modules stored in memory 320, and calling data stored in memory.

[0122] As one embodiment, the computer device includes a processor 320, a memory 330, and a computer program 310, wherein the computer program 310 is stored in the memory 330 and configured to be executed by the processor 320, and the computer program 310 is configured to perform the AGV trajectory tracking mode automatic switching method in the above embodiment.

[0123] Although the description of this application has been quite detailed and particularly focused on several of the described embodiments, it is not intended to limit itself to any of these details or embodiments or any particular embodiment. Rather, it should be considered as effectively covering the intended scope of this application by referring to the appended claims and taking into account the prior art, which provides for a broad possible interpretation of these claims. Furthermore, the foregoing description of this application with respect to embodiments foreseeable by the inventors is intended to provide a useful description, and non-substantial modifications to this application that have not yet been foreseen may still represent equivalent modifications.

Claims

1. A method for automatically switching AGV trajectory tracking modes, characterized in that, The method includes: Step S100: Set multiple trajectory tracking modes and determine their corresponding priority levels. The multiple trajectory tracking modes include the LQR control mode with high priority, the pure tracking control mode with medium priority, and the PID control mode with low priority. Step S110: Obtain the current trajectory tracking mode used by the AGV when running on the predetermined path trajectory, and determine whether the mode running cycle is less than the cycle threshold; if yes, execute step S120; if no, downgrade the current trajectory tracking mode and return to execute step S110. Step S120: Determine whether the AGV pose deviation falls within the deviation threshold range; if yes, proceed to step S130; if no, upgrade the current trajectory tracking mode and return to step S110. Step S130: Determine whether the current trajectory tracking mode is applicable to the trajectory segment currently occupied by the AGV; if yes, proceed to step S140; if no, change the current trajectory tracking mode and return to step S110. Step S140: Determine whether the AGV's running speed falls within the speed threshold range; if yes, proceed to step S150; if no, upgrade the current trajectory tracking mode and return to step S110. Step S150: Determine whether the AGV has completed the predetermined path trajectory; if not, proceed to step S110; if yes, end the AGV operation control. The degradation of the current trajectory tracking mode includes: When the current trajectory tracking mode is identified as LQR control mode, the current trajectory tracking mode is downgraded to pure tracking control mode; when the current trajectory tracking mode is identified as pure tracking control mode, the current trajectory tracking mode is downgraded to PID control mode; when the current trajectory tracking mode is identified as PID control mode, the current trajectory tracking mode is maintained as PID control mode. Upgrading the current trajectory tracking mode includes: When the current trajectory tracking mode is identified as PID control mode, the current trajectory tracking mode is upgraded to pure tracking control mode; when the current trajectory tracking mode is identified as pure tracking control mode, the current trajectory tracking mode is upgraded to LQR control mode; when the current trajectory tracking mode is identified as LQR control mode, the current trajectory tracking mode is kept as LQR control mode.

2. The automatic switching method for AGV trajectory tracking mode according to claim 1, characterized in that, When it is determined that the current trajectory tracking mode is not suitable for the trajectory segment currently occupied by the AGV, the current trajectory tracking mode is upgraded, including: When it is identified that the current trajectory tracking mode is LQR control mode or pure tracking control mode, and the current trajectory segment of the AGV is a straight line, the current trajectory tracking mode will be directly downgraded to PID control mode. When it is identified that the current trajectory tracking mode is PID control mode and the AGV is currently on a curved trajectory, the current trajectory tracking mode is upgraded to pure tracking control mode.

3. The automatic switching method for AGV trajectory tracking mode according to claim 1, characterized in that, The process of obtaining AGV pose deviation includes: Based on the predetermined path trajectory, establish the AGV path trajectory equation with t as the independent variable; The first equation is established and the optimal t value is obtained by solving it using Newton's iteration method. The first equation represents the shortest distance between the AGV body reference point and the trajectory reference point on the predetermined path trajectory. The pose information of the trajectory reference point is obtained based on the optimal t value and the AGV path trajectory equation, and then the AGV pose deviation is obtained by combining the pose information of the AGV body reference point.

4. The automatic switching method for AGV trajectory tracking mode according to claim 1, characterized in that, AGV position deviation includes AGV distance deviation and AGV posture deviation. The deviation threshold range is: AGV distance deviation is less than 50mm and AGV posture deviation is less than 5°.

5. The automatic switching method for AGV trajectory tracking mode according to claim 1, characterized in that, The speed threshold range is: the AGV operating speed is greater than 0.5m / s and less than 1m / s.

6. The automatic switching method for AGV trajectory tracking mode according to claim 1, characterized in that, The period threshold is 10ms.

7. An automatic switching device for AGV trajectory tracking modes, characterized in that, The device includes: The setting module is used to set multiple trajectory tracking modes and determine their corresponding priority levels. The multiple trajectory tracking modes include the high-priority LQR control mode, the medium-priority pure tracking control mode, and the low-priority PID control mode. The first judgment module is used to obtain the current trajectory tracking mode adopted by the AGV when running on the predetermined path trajectory, and to determine whether the mode running cycle is less than the cycle threshold; if yes, the second judgment module is run; if no, the current trajectory tracking mode is downgraded and the first judgment module is run. The second judgment module is used to determine whether the AGV's pose deviation falls within the deviation threshold range; if yes, the third judgment module is run; if no, the current trajectory tracking mode is upgraded and the first judgment module is run. The third judgment module is used to determine whether the current trajectory tracking mode is applicable to the trajectory segment currently occupied by the AGV; if yes, the fourth judgment module is run; if no, the current trajectory tracking mode is upgraded and the first judgment module is run. The fourth judgment module is used to determine whether the AGV's running speed falls within the speed threshold range; if yes, the control module is terminated; if no, the current trajectory tracking mode is upgraded and the first judgment module is run. The control module is terminated to determine whether the AGV has completed the predetermined path trajectory; if not, the first judgment module is run; if yes, the AGV operation control is terminated. The degradation of the current trajectory tracking mode includes: When the current trajectory tracking mode is identified as LQR control mode, the current trajectory tracking mode is downgraded to pure tracking control mode; when the current trajectory tracking mode is identified as pure tracking control mode, the current trajectory tracking mode is downgraded to PID control mode; when the current trajectory tracking mode is identified as PID control mode, the current trajectory tracking mode is maintained as PID control mode. Upgrading the current trajectory tracking mode includes: When the current trajectory tracking mode is identified as PID control mode, the current trajectory tracking mode is upgraded to pure tracking control mode; when the current trajectory tracking mode is identified as pure tracking control mode, the current trajectory tracking mode is upgraded to LQR control mode; when the current trajectory tracking mode is identified as LQR control mode, the current trajectory tracking mode is kept as LQR control mode.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, The processor executes the computer program to implement the AGV trajectory tracking mode automatic switching method as described in any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the automatic switching method for AGV trajectory tracking mode as described in any one of claims 1 to 6.

Images