An AGV trajectory tracking control method based on a non - linear optimization algorithm

Abstract

The present invention discloses an AGV trajectory tracking control method based on a non - linear optimization algorithm, belonging to the technical field of trajectory tracking, and comprising the following steps: Step 1, establish a discrete motion model according to the kinematic relationship of the AGV in a two - dimensional plane; Step 2, based on the discrete motion model, design a quadratic loss function for the AGV according to the reference trajectory; Step 3, calculate the gradient of the control input at all sampling moments; Step 4, calculate the Hessian matrix of the control input at all sampling moments; Step 5, design a non - linear optimization algorithm and find the optimal solution; Step 6, convert the first control input of the optimal solution into a control command to drive the AGV to run; when the pose of the AGV changes in the plane coordinate system, repeat Steps 3 - 5 to realize the trajectory tracking control of the AGV. The present invention has high calculation efficiency and stability, and helps to achieve high - precision online real - time AGV trajectory tracking.

Description

Technical Field

[0001] The present invention belongs to the technical field of trajectory tracking, and particularly relates to an AGV trajectory tracking method based on a non - linear optimization algorithm. Background Art

[0002] An AGV (Automated Guided Vehicle) is an intelligent handling device that relies on automatic navigation technology to complete material transportation. It realizes efficient and safe material handling with reduced manual intervention through preset paths and a flexible navigation system. As a core technology, trajectory tracking ensures that the AGV runs precisely along the path, thus improving the stability of transportation and operation efficiency. AGVs are widely used in scenarios such as logistics warehousing and production line transportation, and are one of the important tools for intelligent manufacturing and modern industry.

[0003] Common algorithms for current AGV trajectory tracking include linearized model predictive control, optimization - based methods (such as gradient descent method, quasi - Newton algorithm, sequential quadratic programming, etc.), PID control algorithms, etc. These algorithms usually have one or more of the following defects in practical applications: high computational complexity, low computational efficiency, and poor tracking accuracy. Therefore, there are certain challenges in meeting the requirements of online real - time calculation. Summary of the Invention

[0004] To solve the above problems, the present invention proposes an AGV trajectory tracking control method based on a non - linear optimization algorithm. This method gives explicit iterative formulas for solving the gradient and Hessian matrix in dynamic problems, and designs a new non - linear optimization algorithm. This algorithm has low computational complexity, high computational efficiency and stability, which helps to achieve high - precision AGV trajectory tracking in real - time online.

[0005] The technical solution of the present invention is as follows:

[0006] An AGV trajectory tracking control method based on a non - linear optimization algorithm, comprising the following steps:

[0007] Step 1: Establish a discrete motion model according to the kinematic relationship of the AGV in the two - dimensional plane;

[0008] Step 2: Based on the discrete motion model, design a quadratic loss function for the AGV according to the reference trajectory;

[0009] Step 3: Calculate the gradient of the control input at all sampling times;

[0010] Step 4: Calculate the Hessian matrix of the control input at all sampling times;

[0011] Step 5: Design a non - linear optimization algorithm and find the optimal solution;

[0012] Step 6: Convert the first control input of the optimal solution into a control command to drive the AGV to run; when the pose of the AGV changes in the plane coordinate system, repeat Steps 3 - 5 to achieve the trajectory tracking control of the AGV.

[0013] Further, in the said Step 1, the discrete motion model is:

[0014] (1);

[0015] Wherein, , , respectively represent the abscissa, ordinate, and heading angle of the AGV in the plane coordinate system at the th sampling moment; , , respectively represent the abscissa, ordinate, and heading angle of the AGV in the plane coordinate system at the th sampling moment; represents the sampling interval; , respectively represent the linear velocity and angular velocity of the AGV in the plane coordinate system at the th sampling moment.

[0016] Further, in the said Step 2, the quadratic loss function is:

[0017] (2);

[0018] Wherein, , respectively represent the pose and reference pose of the AGV in the plane coordinate system at the th sampling moment; , , respectively are the reference abscissa, reference ordinate, and reference heading angle of the AGV in the plane coordinate system at the th sampling moment; , respectively represent the control input and reference input of the AGV in the plane coordinate system at the th sampling moment; , respectively are the reference linear velocity and reference angular velocity of the AGV in the plane coordinate system at the th sampling moment; , respectively represent the given dimension - compatible positive semi - definite weighting matrix and positive definite weighting matrix; represents the total number of sampling moments.

[0019] Further, the specific process of step 3 is as follows:

[0020] Step 3.1: Denote the control inputs at all sampling times as , where and are the transposes of the control inputs of the AGV at the -th sampling time in the plane coordinate system and the transposes of the control inputs of the AGV at the -th sampling time in the plane coordinate system, respectively; Given the initial iteration values of the control inputs at all sampling times ;

[0021] Step 3.2: Execute formula (1) to obtain the poses at all sampling times;

[0022] Step 3.3: Calculate the co-state variables at all sampling times; The specific process is as follows:

[0023] Step 3.3.1: Based on the discrete motion model and the quadratic loss function, define the following first Hamiltonian function:

[0024] (3);

[0025] where represents the first Hamiltonian function; represents the co-state variable corresponding to the -th sampling time ; and and are the co-state variables corresponding to the -th sampling time and and respectively; represents the pose of the AGV in the plane coordinate system at the -th sampling time; is the transpose of ;

[0026] Step 3.3.2: The co-state variable satisfies the following equation:

[0027] (4);

[0028] (5);

[0029] where represents the -th sampling time ​​The corresponding co-state variable; Indicates the th sampling time The corresponding co-state variable, and the co-state variable at this time is the given terminal value; Indicates the th sampling time, the pose of the AGV in the plane coordinate system; Indicates a 3D zero vector;

[0030] Step 3.3.3. Execute formula (4) to obtain the co-state variables at all sampling times;

[0031] Step 3.4. Calculate the quadratic loss function The gradient with respect to the control inputs at all sampling times is as follows:

[0032] (6);

[0033] where Indicates the quadratic loss function The gradient with respect to the control inputs at all sampling times. This gradient has a total of index positions, and The relationship is ; is a two-dimensional row vector containing two elements, which are the elements at the 0th and 1st index positions of the gradient respectively; is a two-dimensional row vector containing two elements, which are the elements at the th and th index positions of the gradient respectively.

[0034] Furthermore, the specific process of step 4 is as follows:

[0035] Step 4.1. Calculate the forward iterative adjoint state and the backward iterative adjoint state at all sampling times; the specific process is as follows:

[0036] Step 4.1.1. Define the following second Hamiltonian function:

[0037] (7);

[0038] where is the second Hamiltonian function; Indicates the th sampling time The corresponding backward iterative adjoint state; Indicates the th sampling time The corresponding forward iterative adjoint state; is the transpose of; is the transpose of; take the element at the 0th index position and denote it as ; ; , , are respectively the initial pose of the AGV at the 0th sampling moment in the plane coordinate system, the control input of the AGV at the 0th sampling moment in the plane coordinate system, and the co-state variable at the 1st sampling moment;

[0039] Step 4.1.2, the forward-iterated adjoint state and the backward-iterated adjoint state respectively satisfy the following equations:

[0040] (8);

[0041] (9);

[0042] (10);

[0043] where represents the forward-iterated adjoint state corresponding to the th sampling moment ; represents the backward-iterated adjoint state corresponding to the th sampling moment ; represents the forward-iterated adjoint state corresponding to the 0th sampling moment , and the forward-iterated adjoint state at this time is the given initial value; represents the backward-iterated adjoint state corresponding to the th sampling moment , and the backward-iterated adjoint state at this time is the given terminal value;

[0044] Step 4.1.3, respectively execute formula (8) and formula (9) to obtain the forward-iterated adjoint state and the backward-iterated adjoint state at all sampling moments ;

[0045] Step 4.2, calculate the elements of the first row of the Hessian matrix; the specific process is as follows:

[0046] Execute formula (11) to obtain the elements of the first row of the Hessian matrix:

[0047] (11);

[0048] where represents the elements of the first row of the Hessian matrix of the quadratic loss function with respect to the control inputs at all sampling moments, and there are index positions; is a two - dimensional row vector containing two elements, which are the elements at the 0th and 1st index positions of the first row of the Hessian matrix, is a two - dimensional row vector containing two elements, which are the th and the th index position elements of the first row of the Hessian matrix;

[0049] Step 4.3: The calculation method of the elements in the other odd rows of the Hessian matrix is the same as that of the elements in the first row. When calculating, change the in the second Hamiltonian function of formula (7). Each time when calculating, , , the values in the brackets are incremented by one in sequence, that is, the next element is taken in sequence; the calculation method of the elements in the even rows of the Hessian matrix is the same as that of the elements in the first row. When calculating, take the element at the 1st index position and denote it as , change the in the second Hamiltonian function to . Each time when calculating, , , the values in the brackets are incremented by one in sequence, that is, the next element is taken in sequence; the elements of all rows form the Hessian matrix.

[0050] Furthermore, the specific process of step 5 is as follows:

[0051] Step 5.1: Given the initial value of at the 0th iteration and the algorithm stop threshold ;

[0052] Step 5.2: Calculate the gradients of the control inputs at all sampling times according to step 3;

[0053] Step 5.3: Calculate the Hessian matrices of the control inputs at all sampling times according to step 4;

[0054] Step 5.4: Execute formulas (12) - (14) to obtain ;

[0055] (12);

[0056] (13);

[0057] (14);

[0058] where, , Sequence numbers representing the number of iterations of the outer loop and the inner loop respectively; , respectively represent the th, th outer loop iteration iteration values; , , respectively represent the 0th, th, th update amounts of the inner loop iteration; , respectively represent the gradients and Hessian matrices of the quadratic loss function at with respect to the control inputs at all sampling times; represents a given positive definite weighting matrix compatible with the dimension;

[0059] Step 5.5, repeat Steps 5.2 - 5.4 until is satisfied, and use the final as the optimal solution of the nonlinear optimization algorithm.

[0060] Advantageous technical effects brought by the present invention: The AGV trajectory tracking method of the present invention has excellent stability and accuracy, can be widely applied to scenarios such as ports, logistics distribution centers, intelligent parking lots, etc., effectively improving operation efficiency, reducing accidental risks, and ensuring operation safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 is a flowchart of the AGV trajectory tracking control method based on the nonlinear optimization algorithm of the present invention.

[0062] Figure 2 is the abscissa and ordinate tracking diagram of the AGV after adopting the nonlinear optimization algorithm of the present invention in the experiment of the present invention.

[0063] Figure 3 is the calculation time comparison diagram after adopting the nonlinear optimization algorithm of the present invention in the experiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0064] The present invention will be further described in detail below with reference to the drawings and specific embodiments:

[0065] As Figure 1 shown, an AGV trajectory tracking control method based on a nonlinear optimization algorithm includes the following steps:

[0066] Step 1, establish a discrete motion model according to the kinematic relationship of the AGV in the two-dimensional plane:

[0067] (1);

[0068] Among them, , , respectively represent the abscissa, ordinate, and heading angle of the AGV at the -th sampling moment in the plane coordinate system; , , respectively represent the abscissa, ordinate, and heading angle of the AGV at the -th sampling moment in the plane coordinate system; represents the sampling interval; , respectively represent the linear velocity and angular velocity of the AGV at the -th sampling moment in the plane coordinate system.

[0069] Step 2: Based on the discrete motion model established in Step 1, design the quadratic loss function of the AGV according to the reference trajectory , as follows:

[0070] (2);

[0071] Among them, , respectively represent the pose and reference pose of the AGV at the -th sampling moment in the plane coordinate system; , , are respectively the reference abscissa, reference ordinate, and reference heading angle of the AGV at the -th sampling moment in the plane coordinate system; , respectively represent the control input and reference input of the AGV at the -th sampling moment in the plane coordinate system; , are respectively the reference linear velocity and reference angular velocity of the AGV at the -th sampling moment in the plane coordinate system; , respectively represent the given dimension-compatible positive semi-definite weighting matrix and positive definite weighting matrix; represents the total number of sampling moments.

[0072] Step 3: Calculate the gradients of the control inputs at all sampling moments; the specific process is as follows:

[0073] Step 3.1: Denote the control inputs at all sampling moments as , among which, , are respectively the The control input of the AGV at a sampling moment in the plane coordinate system The transpose of, the The control input of the AGV at a sampling moment in the plane coordinate system The transpose of. Given the control inputs at all sampling moments The control input The initial iteration value;

[0074] Step 3.2. Execute formula (1) to obtain the poses at all sampling moments;

[0075] Step 3.3. Calculate the co-state variables at all sampling moments; The specific process is as follows:

[0076] Step 3.3.1. Based on the discrete motion model and the quadratic loss function, define the following first Hamiltonian function:

[0077] (3);

[0078] Where, Represents the first Hamiltonian function; Represents the Co-state variable corresponding to the Sampling moment; , , Are respectively the Co-state variables corresponding to the , , Sampling moment; Represents the Pose of the AGV in the plane coordinate system at the Is the Transpose of.

[0079] Step 3.3.2. According to the maximum principle and combined with the first Hamiltonian function of formula (3), it can be known that the co-state variable Satisfies the following equation:

[0080] (4);

[0081] (5);

[0082] Where, Represents the Co-state variable corresponding to the Sampling moment; Represents the Co-state variable corresponding to the Sampling moment. At this time, the co-state variable is the given terminal value; Represents the The pose of the AGV at a sampling moment in the plane coordinate system; Represents a 3D zero vector.

[0083] Step 3.3.3: Execute formula (4) to obtain the co-state variables at all sampling moments;

[0084] Step 3.4: Give the quadratic loss function of formula (2) and design the gradient formula for the control input at all sampling moments as follows:

[0085] (6);

[0086] Where, Represents the quadratic loss function The gradient with respect to the control input at all sampling moments. This gradient has a total of Index positions, where Is a positive integer, and its relationship with the total number of sampling moments Is ; Is a two-dimensional row vector containing two elements, which are the elements at the 0th and 1st index positions of the gradient respectively; Is a two-dimensional row vector containing two elements, which are the elements at the th and th index positions of the gradient respectively. Execute formula (6) to obtain the gradient of the quadratic loss function With respect to the control input at all sampling moments.

[0087] Step 4: Based on the gradient formula (6) obtained in Step 3, calculate the Hessian matrix of the control input at all sampling moments; the specific process is as follows:

[0088] Step 4.1: Calculate the forward iterative adjoint state and backward iterative adjoint state at all sampling moments; the specific process is as follows:

[0089] Step 4.1.1: Combine formula (1) and formula (4) to define the following second Hamiltonian function:

[0090] (7);

[0091] Where, Is the second Hamiltonian function; Represents the th sampling moment Corresponding backward iterative adjoint state; Represents the th sampling moment Corresponding forward iterative adjoint state; Is The transpose of; is the transpose of. Take the element at the 0th index position and denote it as ; ; , , are respectively the initial pose at the 0th sampling moment, the control input at the 0th sampling moment, and the co-state variable at the 1st sampling moment.

[0092] Step 4.1.2: According to the maximum principle and combining with the second Hamiltonian function in formula (7), it can be known that the forward iterative adjoint state and the backward iterative adjoint state respectively satisfy the following equations:

[0093] (8);

[0094] (9);

[0095] (10);

[0096] where represents the forward iterative adjoint state corresponding to the th sampling moment ; represents the backward iterative adjoint state corresponding to the th sampling moment ; represents the forward iterative adjoint state corresponding to the 0th sampling moment, and the forward iterative adjoint state at this time is the given initial value; ; represents the backward iterative adjoint state corresponding to the th sampling moment ; the backward iterative adjoint state at this time is the given terminal value.

[0097] Step 4.1.3: Execute formula (8) and formula (9) respectively to obtain the forward iterative adjoint state and the backward iterative adjoint state at all sampling moments ;

[0098] Step 4.2: Calculate the elements of the first row of the Hessian matrix; the specific process is as follows:

[0099] The calculation formula for the elements of the first row of the Hessian matrix of the quadratic loss function in formula (2) with respect to the control inputs at all sampling moments is given as follows:

[0100] (11);

[0101] where represents the quadratic loss function For the first row elements of the Hessian matrix of the control input at all sampling instants, there are index positions; is a two-dimensional row vector containing two elements, which are the elements at the 0th and 1st index positions of the first row of the Hessian matrix, is a two-dimensional row vector containing two elements, which are the th and th index position elements of the first row of the Hessian matrix. Executing formula (11) gives the first row elements of the Hessian matrix;

[0102] Step 4.3: The calculation method of the elements in the other odd rows of the Hessian matrix is the same as that of the first row elements. When calculating, change in the second Hamiltonian function of formula (7). Each time when calculating, , , the values in the parentheses are incremented by one in sequence, that is, the next element is taken in sequence; the calculation method of the elements in the even rows of the Hessian matrix is the same as that of the first row elements. When calculating, take the element at the 1st index position , and change in the second Hamiltonian function to . Each time when calculating, , , the values in the parentheses are incremented by one in sequence, that is, the next element is taken in sequence; the elements of all rows form the Hessian matrix.

[0103] Step 5: Design a nonlinear optimization algorithm and find the optimal solution; the specific process is as follows:

[0104] Step 5.1: Given the initial value of at the 0th iteration and the algorithm stop threshold ;

[0105] Step 5.2: Calculate the gradient of the control input at all sampling instants according to Step 3;

[0106] Step 5.3: Calculate the Hessian matrix of the control input at all sampling instants according to Step 4;

[0107] Step 5.4: Execute formulas (12) - (14) to obtain ;

[0108] (12);

[0109] (13);

[0110] (14);

[0111] Among them, and respectively represent the sequence numbers of the outer loop and inner loop iteration times; and respectively represent the th and th iteration values during the outer loop iteration; ; and and respectively represent the update amounts at the 0th, th, and th inner loop iterations; and respectively represent the gradient and Hessian matrix of the quadratic loss function with respect to the control inputs at all sampling times at ; represents a given positive definite weighting matrix compatible with the dimension.

[0112] Step 5.5. Repeat Steps 5.2 - 5.4 until is satisfied, and use the final as the optimal solution of the nonlinear optimization algorithm.

[0113] Step 6. Convert the first control input of the optimal solution obtained in Step 5 into a control command to drive the AGV to run; when the pose of the AGV changes in the plane coordinate system, repeat Steps 3 - 5 to achieve the trajectory tracking control of the AGV.

[0114] To prove the superiority and feasibility of the method proposed in the present invention, the following comparative experiments are carried out with the mainstream quasi - Newton algorithm and sequential quadratic programming algorithm as benchmarks. For the AGV discrete motion model shown in formula (1), select the initial pose as , , , respectively represent the abscissa, ordinate, and heading angle of the AGV in the plane coordinate system at the 0th sampling time, the sampling interval is , and the total number of sampling times is ; for the quadratic loss function shown in formula (2), select the semi - positive definite weighting matrix , and the positive definite weighting matrix ; for formulas (12) - (14), select the positive definite weighting matrix compatible with the dimension, and is The unit matrix of dimension, and the stopping condition for the above three algorithms is that the infinity norm of the gradient is less than 10 -6 .

[0115] Figure 2 The AGV trajectory tracking effect based on the non-linear optimization algorithm proposed by the present invention is shown. The reference trajectory is selected as the circular trajectory shown in the figure. It can be seen that after adopting the non-linear optimization algorithm of the present invention, the AGV can achieve fast and high-precision trajectory tracking in the abscissa and ordinate directions.

[0116] Figure 3 The calculation times of the non-linear optimization algorithm of the present invention, the quasi-Newton algorithm and the sequential quadratic programming algorithm are compared. For the fairness of comparison, 8 independent experiments are carried out for each of the above three algorithms respectively. It can be seen that the non-linear optimization algorithm of the present invention has significant computational efficiency in terms of calculation time, and its calculation time is greatly shortened, far lower than the existing methods.

[0117] Certainly, the above description is not a limitation of the present invention, and the present invention is not limited to the above examples. Changes, modifications, additions or substitutions made by those skilled in the art within the essence of the present invention should also fall within the protection scope of the present invention.

Claims

1. An AGV trajectory tracking control method based on nonlinear optimization algorithm, characterized in that: The steps include: Step 1: Establish a discrete motion model based on the kinematic relationship of AGV in a two-dimensional plane; Step 2: Based on the discrete motion model, design the quadratic loss function of the AGV according to the reference trajectory; Step 3: Calculate the gradient of the control input at all sampling moments; the specific process is: Step 3.1, let the control input at all sampling moments k=0,...,N be X=[u′(0),...,u′(N)]′, where u′(0) and u′(N) are the transposition of the control input u(0) of the AGV in the plane coordinate system at the 0th sampling moment and the transposition of the control input u(N) of the AGV in the plane coordinate system at the Nth sampling moment, respectively; give the initial iteration value of the control input X at all sampling moments k=0,...,N; Step 3.2: Execute formula (1) to obtain the pose at all sampling moments; Step 3.3, calculate the co-state variables at all sampling moments; the specific process is: Step 3.3.1, based on the discrete motion model and the quadratic loss function, define the following first Hamiltonian function: H(x(k),u(k),λ(k+1))=[[x(k)-x r (k)]′Q[x(k)-x r (k)]+[u(k)-u r (k)]′R[u(k)-ur(k)]]+λ′(k+1)x(k+1)(3); Wherein, H(·) represents the first Hamiltonian function; λ(k+1)=[λ1(k+1),λ2(k+1),λ3(k+1)]′ represents the co-state variable corresponding to the k+1th sampling time x(k+1); λ1(k+1), λ2(k+1), λ3(k+1) are the co-state variables corresponding to the k+1th sampling time x1(k+1), x2(k+1), x3(k+1) respectively; x(k+1)=[x1(k+1),x2(k+1),x3(k+1)]′ represents the position and posture of AGV in the plane coordinate system at the k+1th sampling time; λ′(k+1) is the transpose of λ(k+1); Step 3.3.2, the covariate variable λ(k+1) satisfies the following equation: λ(N+1)=0 3×1 (5); Among them, λ(k) represents the co-state variable corresponding to the k-th sampling time x(k); λ(N+1) represents the co-state variable corresponding to the N+1-th sampling time x(N+1), and the co-state variable at this time is a given terminal value; x(N+1) represents the position and posture of the AGV in the plane coordinate system at the N+1-th sampling time; 0 3×1 Represents a 3-dimensional zero vector; Step 3.3.3, execute formula (4) to obtain the co-state variables at all sampling times; Step 3.4: Calculate the gradient of the quadratic loss function J with respect to the control input at all sampling moments. The formula is as follows: in, Represents the gradient of the quadratic loss function J with respect to the control input at all sampling moments. The gradient has M index positions, and the relationship between M and N is M = 2(N+1) ； is a two-dimensional row vector containing two elements, which are the elements at the 0th and 1st index positions of the gradient; is a two-dimensional row vector containing two elements, which are the elements of the M-2nd and M-1th index positions of the gradient; Step 4, calculate the Hessian matrix of the control input at all sampling moments; the specific process is: Step 4.1, calculate the forward iterative adjoint state and the backward iterative adjoint state of all sampling moments; the specific process is: Step 4.1.1, define the following second Hamiltonian function: Where H0(·) is the second Hamiltonian function; σ(k+1) represents the backward iterative adjoint state corresponding to the k+1th sampling time x(k+1); η(k) represents the forward iterative adjoint state corresponding to the kth sampling time λ(k); σ′(k+1) is the transpose of σ(k+1); η′(k) is the transpose of η(k); The element at the 0th index position is recorded as Φ1(x(0),u(0),λ(1)); x(0), u(0), λ(1) are the initial position of the AGV in the plane coordinate system at the 0th sampling time, the control input of the AGV in the plane coordinate system at the 0th sampling time, and the co-state variable at the 1st sampling time, respectively; Step 4.1.2, the forward iterative adjoint state and the backward iterative adjoint state satisfy the following equations respectively: n(0)=σ(N+1)=0 3×1 (10); Wherein, η(k+1) represents the forward iterative companion state corresponding to the k+1th sampling time λ(k+1); σ(k) represents the backward iterative companion state corresponding to the kth sampling time x(k); η(0) represents the forward iterative companion state corresponding to the 0th sampling time λ(1), and the forward iterative companion state at this time is the given initial value; σ(N+1) represents the backward iterative companion state corresponding to the N+1th sampling time x(N+1), and the backward iterative companion state at this time is the given terminal value; Step 4.1.3, respectively execute formula (8) and formula (9) to obtain the forward iterative adjoint state and the backward iterative adjoint state of all sampling moments k=0,...,N; Step 4.2, calculate the first row elements of the Hessian matrix; the specific process is: Execute formula (11) to obtain the first row element of the Hessian matrix: in, The first row of the Hessian matrix representing the quadratic loss function J controlling the input at all sampling moments has a total of M index positions; is a two-dimensional row vector containing two elements, which are the elements at the 0th and 1st index positions of the first row of the Hessian matrix. is a two-dimensional row vector containing two elements, which are the elements at the M-2th and M-1th index positions of the first row of the Hessian matrix; Step 4.3: The calculation method for the elements of the other odd rows of the Hessian matrix is ​​the same as that for the elements of the first row. When calculating, change Φ1(x(0),u(0),λ(1)) in the second Hamiltonian function of formula (7). Each time the calculation is performed, the values ​​in the brackets of x, u, and λ are increased by one, that is, the next element is taken in turn. The calculation method for the elements of the even rows of the Hessian matrix is ​​the same as that for the elements of the first row. When calculating, take The element at the first index position is recorded as φ2(x(0),u(0),λ(1)), and the φ1(x(0),u(0),λ(1)) in the second Hamiltonian function is changed to Φ2(x(0),u(0),λ(1)). In each calculation, the values ​​in the brackets of x, u, and λ are increased by one, that is, the next element is taken in turn; the elements of all rows constitute the Hessian matrix; Step 5: Design a nonlinear optimization algorithm and find the optimal solution; Step 6: Convert the first control input of the optimal solution into a control instruction to drive the AGV to run; when the posture of the AGV in the plane coordinate system changes, repeat steps 3 to 5 to achieve trajectory tracking control of the AGV.

2. The AGV trajectory tracking control method based on the nonlinear optimization algorithm according to claim 1 is characterized in that: In step 1, the discrete motion model is: Among them, x1(k+1), x2(k+1), and x3(k+1) respectively represent the horizontal coordinate, vertical coordinate, and heading angle of the AGV in the plane coordinate system at the k+1th sampling moment; x1(k), x2(k), and x3(k) respectively represent the horizontal coordinate, vertical coordinate, and heading angle of the AGV in the plane coordinate system at the kth sampling moment; Δ represents the sampling interval; u1(k) and u2(k) respectively represent the linear velocity and angular velocity of the AGV in the plane coordinate system at the kth sampling moment.

3. The AGV trajectory tracking control method based on the nonlinear optimization algorithm according to claim 2 is characterized in that: In step 2, the quadratic loss function J is: Where x(k) = [x1(k), x2(k), x3(k)]′, They represent the position and reference position of AGV in the plane coordinate system at the kth sampling moment respectively; are the reference abscissa, reference ordinate, and reference heading angle of the AGV in the plane coordinate system at the kth sampling moment; u(k) = [u1(k), u2(k)]′, They represent the control input and reference input of AGV in the plane coordinate system at the kth sampling moment respectively; are the reference linear velocity and reference angular velocity of AGV in the plane coordinate system at the kth sampling moment, respectively; Q and R represent the given dimensionally compatible semi-positive definite weighted matrix and positive definite weighted matrix, respectively; N represents the total number of sampling moments.

4. The AGV trajectory tracking control method based on the nonlinear optimization algorithm according to claim 3 is characterized in that: The specific process of step 5 is as follows: Step 5.1, given the initial value X0 of X at the 0th iteration and the algorithm stopping threshold τ>0; Step 5.2, calculate the gradient of the control input at all sampling moments according to step 3; Step 5.3, calculate the Hessian matrix of the control input at all sampling moments according to step 4; Step 5.4: Execute formula (12)-formula (14) to obtain X m+1 ; X m+1 =X m -h n (X m ) (12); Among them, m and n represent the number of iterations of the outer loop and inner loop respectively; X m+1 , X m They represent the iteration values ​​of X at the m+1th and mth outer loop iterations respectively; h0(X m ),h n-1 (X m ),h n (X m ) represent the 0th, n-1th, and nth update amounts of the inner loop iteration respectively; They represent the quadratic loss function J in X m The gradient and Hessian matrix of the control input at all sampling moments; Γ represents the given Dimensionally consistent positive definite weighting matrix; Step 5.5: Repeat steps 5.2 to 5.4 until ||X is satisfied. m+1 -X m ||<τ, with the final X m+1 As the optimal solution of the nonlinear optimization algorithm.

Images