New optimization algorithm and AGV trajectory tracking method based on optimal controller design

Abstract

The present invention discloses a new optimization algorithm based on optimal controller design and an AGV trajectory tracking method, which belongs to the field of algorithm and control technology. The design process of the new optimization algorithm is: converting the optimized problem into an optimal controller design problem; proposing a new iteration format according to the optimal control input and the optimal state equation of the optimal controller design problem; and designing a specific optimization iteration algorithm. The new optimization algorithm proposed in the present invention is superlinearly convergent, converges faster than the gradient descent method, and has a wider range of applicability than the Newton method. AGV trajectory tracking is one of the applications of the new optimization algorithm. The AGV trajectory tracking problem is described as a nonlinear optimization problem. The optimal control input sequence for AGV trajectory tracking can be obtained by solving the new optimization algorithm. The AGV trajectory tracking implemented by the new optimization algorithm has the characteristics of stability and high tracking accuracy.

Description

Technical Field

[0001] The present invention belongs to the field of algorithm and control technology, and specifically relates to a new optimization algorithm based on optimal controller design and an AGV trajectory tracking method. Background Art

[0002] In control theory, control input is also called controller. With the development and application of optimization algorithms, more and more researchers have begun to use optimization methods to solve practical problems in different fields. Common optimization methods mainly include gradient descent method, Newton method and some of their variants. Although these methods are powerful and have been widely used, they also have their own limitations, such as the slow convergence of gradient descent method and the possibility of falling into local extreme points. Newton method requires that the Hessian matrix (Hessian matrix) at each iteration point is non-singular. If the Hessian matrix is ​​singular or ill-conditioned, Newton method may diverge. In addition, Newton method is very sensitive to the selection of initial values. When the selected initial value is far away from the minimum value, Newton method may converge to the local minimum value or even diverge. In addition, some variants of gradient descent method or Newton method improve the algorithm by introducing new parameters, but how to adjust the introduced parameters is still a difficult problem. Unlike the parameters in these methods, each parameter or variable in the optimal control problem has a clear physical meaning. The new optimization algorithm proposed in the present invention is derived from the optimal control problem, so the parameters therein can be more conveniently adjusted to achieve the desired effect.

[0003] The new optimization algorithm proposed in the present invention has a wide range of applications, and AGV trajectory tracking is one of them. AGV is the abbreviation of Automated Guided Vehicle, also known as automatic guided vehicle, automatic guided vehicle or automatic guided transport vehicle. AGV can achieve highly automated and intelligent material handling and management, reduce the risk of work-related injuries, reduce the time and errors of manual operations, and improve overall logistics and production efficiency. Therefore, it is an important part of intelligent manufacturing and Industry 4.0. Trajectory tracking is one of the core technologies of AGV. The trajectory tracking methods currently used by AGV include PID control, model predictive control (MPC), simultaneous localization and mapping (SLAM), rapid random tree (RRT), dynamic window method (DWA), etc. These algorithms have the following disadvantages: either the execution is complex and unstable, or the convergence speed is slow, so the real-time performance is poor or the accuracy is low. Summary of the invention

[0004] In order to solve the above problems, the present invention proposes a new optimization algorithm based on optimal controller design and an AGV trajectory tracking method. The proposed new optimization algorithm not only converges faster than the gradient descent method, but also has a wider range of applicability than the Newton method. Even if the Hessian matrix at the iteration point is singular or ill-conditioned, the proposed algorithm can still be used due to the introduction of the weighted matrix. The new optimization algorithm can achieve real-time and high-precision AGV trajectory tracking.

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

[0006] A new optimization algorithm based on optimal controller design includes the following steps:

[0007] Step 1: Convert the problem to be optimized into the optimal controller design problem;

[0008] Step 2: Propose a new iteration format based on the optimal control input and optimal state equation of the optimal controller design problem;

[0009] Step 3: Design a specific optimization iterative algorithm.

[0010] Furthermore, in step 1, first, the optimization problem Transformed into an optimal controller design problem, Status The objective function of this optimal controller design problem is to design a control sequence To adjust the dynamic system The state of finally minimizes a performance index; the formula of the performance index is as follows:

[0011] (1);

[0012] in, Indicates sub-sampled control input, ; , Respectively represent Second and The state of subsampling; is the number of sampling times; , Respectively represent the objective function in , The value of ; Indicates The state of subsampling; is the transpose symbol; is a positive definite weight matrix consistent with the given dimension.

[0013] Furthermore, in step 2, the formula for the optimal control input is:

[0014] (2);

[0015] in, is the optimal control input; ; For dynamic systems No. The state of subsampling; for exist The gradient of

[0016] The new iteration format is as follows:

[0017] (3);

[0018] (4);

[0019] (5);

[0020] Among them, for the times sampling, the inner loop is performed correspondingly Second-rate, To carry out Final iteration value after the inner loop; is the sequence number of the inner loop iterations; , , They represent the initial value of the inner loop iteration, Iteration value, Iteration value; express exist The gradient of express exist The Heather matrix.

[0021] Furthermore, in step 3, the specific process of optimizing the iterative algorithm is as follows:

[0022] Step 3.1. Select , for any initial state and precision ;

[0023] Step 3.2: Execute formula (3)-formula (5) to obtain ;

[0024] Step 3.3: If , then go to step 3.2; otherwise stop and As an optimization problem The solution.

[0025] An AGV trajectory tracking method, using the new optimization algorithm as described above to track the AGV trajectory, specifically includes the following steps:

[0026] Step a, establishing a discrete time kinematic model of AGV;

[0027] Step b, establishing secondary tracking indicators of AGV;

[0028] Step c: describe the AGV trajectory tracking problem as an optimization problem;

[0029] Step d: using the new optimization algorithm to solve the optimization problem.

[0030] Furthermore, in step a, the discrete time kinematic model of the AGV is:

[0031] (6);

[0032] in, , , They are The horizontal coordinate, vertical coordinate and body yaw angle of the AGV in the world coordinate system are sampled; , , They are The horizontal coordinate, vertical coordinate and body yaw angle of the AGV in the world coordinate system are sampled; It is The forward speed of the sub-sampled vehicle body; It is Sub-sampling body steering angular velocity; is the sampling period;

[0033] Furthermore, in step b, the secondary tracking index of the AGV is:

[0034] (7);

[0035] in, Indicates AGV Sub-sampling control input; , , Respectively represent The tracking error, pose, and reference pose of the sub-sampled AGV in the world coordinate system; , , Respectively represent The reference horizontal coordinate, reference vertical coordinate, and reference body yaw angle of the sub-sampled AGV in the world coordinate system; is a semi-positive definite weight matrix consistent with the given dimension; is a positive definite weight matrix compatible with the given dimension, The dimensions are different; is the number of AGV sampling times.

[0036] Furthermore, in step c, the optimization problem is:

[0037] (8);

[0038] in, It is AGV The subsampled control input is stacked into a column vector; for The transpose of yes The objective function of .

[0039] Furthermore, in step d, the formula for solving the optimization problem using the new optimization algorithm is as follows:

[0040] (9);

[0041] (10);

[0042] (11);

[0043] in, is the sequence number of the outer loop iterations; is the sequence number of the inner loop iterations; The first iteration of the outer loop Iteration value; and They represent the outer loop iterations. Second and The iterations produced The iteration value of , , They represent the initial value of the inner loop iteration, Iteration value, Iteration value; express exist The gradient of express exist The Heather Matrix; is a given vector Dimensionally consistent positive definite weighting matrix;

[0044] The specific process of solving the optimization problem using the new optimization algorithm is as follows:

[0045] Step d.1: Given the initial iteration value of the column vector of the control input stack and precision ;

[0046] Step d.2: Execute the update relationship of formula (9)-formula (11) to obtain ;

[0047] Step d.3: If , go to step d.2 to continue updating; otherwise stop and Solve the optimization problem (8).

[0048] The beneficial technical effects brought by the present invention are as follows: the new optimization algorithm proposed by the present invention is superlinearly convergent, which is faster than the gradient descent method; the proposed optimization algorithm is also applicable to the case where the Hessian matrix at the iteration point is singular or pathological, and has a wider range of applicability than the Newton method. At the same time, the AGV trajectory tracking realized by the new optimization algorithm has the characteristics of stability and high tracking accuracy, which can effectively improve the operating efficiency of warehouses, production lines, airports, etc., avoid accidents, and improve safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 This is a flow chart of the new optimization algorithm designed based on the optimal controller of the present invention.

[0050] Figure 2 It is a flow chart of the AGV trajectory tracking method of the present invention.

[0051] Figure 3 This is a horizontal coordinate tracking diagram of the AGV after the new optimization algorithm of the present invention is adopted in the embodiment of the present invention.

[0052] Figure 4 This is a vertical coordinate tracking diagram of the AGV after the new optimization algorithm of the present invention is adopted in the embodiment of the present invention.

[0053] Figure 5 This is a yaw angle tracking diagram of the AGV after the new optimization algorithm of the present invention is adopted in an embodiment of the present invention. DETAILED DESCRIPTION

[0054] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0055] like Figure 1 As shown in the figure, a new optimization algorithm based on optimal controller design is proposed. The core of the new optimization algorithm is that the iterative format of the optimization algorithm is derived from the controller design of a special optimal control problem closely related to the optimization problem. It has the advantages of fast convergence, good stability, high accuracy, intuitive physical meaning of parameters, and easy adjustment. The construction process of the new optimization algorithm is as follows:

[0056] Step 1: Take any optimization problem Transformed into an optimal controller design problem; the specific process is as follows:

[0057] First, the optimization problem Transformed into an optimal controller design problem, Status The objective function of this optimal controller design problem is to design a control sequence To adjust the dynamic system The state of finally minimizes a performance index, and the formula of the performance index is as follows:

[0058] (1);

[0059] in, Indicates sub-sampled control input, ; , Respectively represent Second and The state of subsampling; is the number of sampling times; , They are , The objective function of To indicate the The state of subsampling; is the transpose symbol; is a positive definite weight matrix consistent with the given dimension.

[0060] Step 2: According to the optimal control input and optimal state equation of the optimal controller design problem, a new iteration format is proposed; the formula of the optimal control input is:

[0061] (2);

[0062] in, is the optimal control input; ; For dynamic systems No. The state of subsampling; for exist The gradient of

[0063] The new iteration format is proposed as follows:

[0064] (3);

[0065] (4);

[0066] (5);

[0067] Among them, for the times sampling, the inner loop is performed correspondingly Second-rate, To carry out The final value of the iteration after the inner loop. For example, when sampling for the first time, the inner loop is performed once; when sampling for the second time, the inner loop is performed twice. is the sequence number of the inner loop iterations; , , Respectively represent the inner loop iteration about The initial value of Iteration value, Iteration value; express exist The gradient of express exist The Heather matrix.

[0068] Step 3: The specific iterative process of the optimization algorithm is as follows:

[0069] Step 3.1. Select , for any initial state and precision ;

[0070] Step 3.2: Execute formula (3)-formula (5) to obtain ;

[0071] Step 3.3: If , then go to step 3.2; otherwise stop and As an optimization problem The solution.

[0072] Inspired by the analytical solution of the optimal control problem, the new optimization algorithm iteratively updates along the optimal state track as much as possible. It has a superlinear convergence rate, and the adjustable parameters In terms of norm, the smaller the iterative process converges faster.

[0073] like Figure 2 As shown, the present invention also proposes an AGV trajectory tracking method, which uses the new optimization algorithm proposed in the present invention to track the AGV trajectory; specifically, it includes the following steps:

[0074] Step a: Select an appropriate sampling period and establish a discrete-time kinematic model of the AGV:

[0075] (6);

[0076] in, , , They are The horizontal coordinate, vertical coordinate and body yaw angle of the AGV in the world coordinate system are sampled; , , They are The horizontal coordinate, vertical coordinate and body yaw angle of the AGV in the world coordinate system are sampled; It is The forward speed of the sub-sampled vehicle body; It is Sub-sampling body steering angular velocity; is the sampling period.

[0077] Step b: Based on the kinematic model established in step a, select an appropriate semi-positive definite weighted matrix, a positive definite weighted matrix and a non-negative integer, and give the secondary tracking index of the AGV:

[0078] (7);

[0079] in, Indicates AGV Sub-sampling control input; , , Respectively represent The tracking error, pose, and reference pose of the sub-sampled AGV in the world coordinate system; , , Respectively represent The reference horizontal coordinate, reference vertical coordinate, and reference body yaw angle of the sub-sampled AGV in the world coordinate system; is a semi-positive definite weight matrix consistent with the given dimension; is a positive definite weight matrix compatible with the given dimension, The dimensions are different; is the number of AGV sampling times.

[0080] Step c: Describe the AGV trajectory tracking problem described by steps a and b as an optimization problem:

[0081] (8);

[0082] in, It is AGV The subsampled control input is stacked into a column vector; for The transpose of yes The objective function is yes Equivalent deformation of .

[0083] Step d, solving the optimization problem in step c using a new optimization algorithm;

[0084] The formula for solving the optimization problem using the new optimization algorithm is as follows:

[0085] (9);

[0086] (10);

[0087] (11);

[0088] in, is the sequence number of the outer loop iterations; is the sequence number of the inner loop iterations; The first iteration of the outer loop Iteration value; and They represent the outer loop iterations. Second and The iterations produced The iteration value of , , They represent the initial value of the inner loop iteration, Iteration value, The iteration value can be iteratively generated according to formula (5)-formula (6); express exist The gradient of express exist The Heather Matrix; is a given vector Dimensionally consistent positive definite weighting matrix;

[0089] The specific process of solving the optimization problem using the new optimization algorithm is as follows:

[0090] Step d.1: Given the initial iteration value of the column vector of the control input stack and precision ;

[0091] Step d.2: Execute the update relationship of formula (9)-formula (11) to obtain ;

[0092] Step d.3: If , go to step d.2 to continue updating; otherwise stop and As the solution of the optimization problem (8).

[0093] In the present invention, the new optimization algorithm has a superlinear convergence speed, wherein the adjustable parameters In terms of norm, the smaller the iterative process converges faster.

[0094] The AGV trajectory tracking method proposed by the present invention is stable and has high tracking accuracy, and can thus effectively improve the operational efficiency of warehouses, production lines, airports, etc., avoid accidents, and improve safety.

[0095] In order to demonstrate the feasibility and superiority of the present invention, the following examples and comparative experiments are given.

[0096] In the embodiment of the present invention, the reference posture of the AGV is selected as , the starting position is , sampling period , AGV sampling times Set to 10, 15, 20 respectively, the initial iteration value Pick dimensional zero vector, weight matrix , , , for dimensional identity matrix. Figure 3-Figure 5 Shows the AGV sampling times The trajectory tracking effect diagram of AGV after using the new optimization algorithm of the present invention. Figure 3 is the horizontal axis tracking diagram, Figure 4 is the vertical axis tracking graph; Figure 5 is the yaw angle Tracking map. Figure 3-Figure 5 It can be seen that after adopting the new optimization algorithm of the present invention, AGV can achieve high-precision trajectory tracking.

[0097] In the experiment of the present invention, the secondary tracking index of the AGV in formula (7) of the present invention is an optimal control problem. For the optimization problem formula (8) transformed from the optimal control problem formula (7), the new optimization algorithm of the present invention and the existing quasi-Newton algorithm are used for comparative analysis. Table 1 compares the calculation time and number of iterations of the new optimization algorithm proposed by the present invention and the existing quasi-Newton algorithm. The specific experimental results are as follows:

[0098] Table 1 Comparison of parameter results of the new optimization algorithm and the quasi-Newton algorithm at different sampling times

[0099]

[0100] It can be clearly seen from Table 1 that for different AGV sampling times Under the premise of achieving the same accuracy, the new optimization algorithm proposed in the present invention has a faster convergence speed and the calculation time is much less than that of the quasi-Newton algorithm.

[0101] Of course, 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 technicians in this technical field within the essential scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. An AGV trajectory tracking method, characterized in that: A new optimization algorithm based on optimal controller design is used for AGV trajectory tracking. The new optimization algorithm includes the following steps: Step 1: Convert the problem to be optimized into the optimal controller design problem; Step 2: Propose a new iteration format based on the optimal control input and optimal state equation of the optimal controller design problem; Step 3: Design a specific optimization iterative algorithm; In step 1, first, the optimization problem Transformed into an optimal controller design problem, is the objective function of state s; The optimal controller design problem aims to design a control sequence u0,…,u M-1 To adjust the dynamic system k+1 =s k +u k The state of finally minimizes a performance index; the formula of the performance index is as follows: Among them, u k represents the control input of the kth sampling, k = 0, 1, ..., M-1; s k 、s k+1 They represent the states of the kth and k+1th samplings respectively; M is the number of samplings; Respectively represent the objective function in s k 、s M The value of s M Indicates the state of the Mth sampling; T is the transposition symbol; is a positive definite weight matrix consistent with the given dimension; In step 2, the formula for the optimal control input is: Among them, u′ k is the optimal control input; l=k+1,…,M; s l For dynamic systems k+1 =s k +u k The state of the lth sampling; for In s l The gradient of The new iteration format is as follows: Among them, for the kth sampling, the inner loop is performed k times in total. is the final value of the iteration after k inner loops; i is the number of inner loop iterations; They represent the initial value, the i-1th iteration value, and the i-th iteration value during the inner loop iteration respectively; express In s k The gradient of express In s k The Heather Matrix; In step 3, the specific process of optimizing the iterative algorithm is as follows: Step 3.

1. Select For any initial state s0 and precision ε>0; Step 3.2: Execute formula (3)-formula (5) to obtain s k+1 ; Step 3.3, if ||s k+1 -s k ||>ε, go ​​to step 3.2; otherwise stop and k+1 As an optimization problem The solution; The AGV trajectory tracking method specifically includes the following steps: Step a, establishing a discrete time kinematic model of AGV; Step b, establishing secondary tracking indicators of AGV; Step c: Describe the AGV trajectory tracking problem as an optimization problem; the optimization problem is: in, is a column vector of AGVN times sampled control input; For U k The transpose of yes The objective function of Step d: using the new optimization algorithm to solve the optimization problem.

2. The AGV trajectory tracking method according to claim 1, characterized in that: In step a, the discrete time kinematic model of the AGV is: Among them, x k ,y k ,θ k are the horizontal coordinate, vertical coordinate, and body yaw angle of the k-th sampling AGV in the world coordinate system; x k+1 ,y k+1 ,θ k+1 are the horizontal coordinate, vertical coordinate, and body yaw angle of the k+1th sampling AGV in the world coordinate system; v k is the forward speed of the k-th sampling vehicle body; ω k is the kth sampled body steering angular velocity; T s is the sampling period.

3. The AGV trajectory tracking method according to claim 2, characterized in that: In step b, the secondary tracking index of AGV is: Among them, U k =[v k ,ω k ] T represents the control input of the kth sampling of AGV; X k =[x k ,y k ,θ k ] T , They represent the tracking error, pose, and reference pose of the k-th sampling AGV in the world coordinate system respectively; They represent the reference abscissa, reference ordinate, and reference body yaw angle of the k-th sampling AGV in the world coordinate system respectively; Q is a given dimensionally compatible semi-positive definite weighting matrix; R is a given dimensionally compatible positive definite weighting matrix, and The dimensions are different; N is the number of AGV sampling times.

4. The AGV trajectory tracking method according to claim 3, characterized in that: In step d, the formula for solving the optimization problem using the new optimization algorithm is as follows: Where j is the number of iterations of the outer loop; i is the number of iterations of the inner loop; It is the jth iteration value of the outer loop iteration; and They represent the j+1th and jth iterations of the outer loop. The iteration value of They represent the initial value, the i-1th iteration value, and the i-th iteration value during the inner loop iteration respectively; express exist The gradient of express exist The Hessian matrix of dimensionally consistent positive definite weighting matrix; The specific process of solving the optimization problem using the new optimization algorithm is as follows: Step d.1: Given the initial iteration value of the column vector of the control input stack and precision ε>0; Step d.2: Execute the update relationship of formula (9)-formula (11) to obtain Step d.3: If Go to step d.2 to continue updating; otherwise stop and Solve the optimization problem (8).

Images