MPCAGV trajectory tracking control method based on improved incremental control

Abstract

The invention provides a trajectory tracking control method based on an incremental dominating MPCAGV. The method comprises the following steps: constructing an AGV kinematic model, and carrying out linearization and discretization analysis; establishing a traditional MPC controller according to the motion model; constructing an increment dominating vector and an increment dominating matrix, and constructing an improved increment dominating MPC controller; on the basis of BiEO, a PBiEO optimization algorithm is provided, and a dominating relation to be optimized in an incremental dominating vector is optimized; and using the optimized increment to control the MPC controller to track the expected trajectory. By defining an increment dominating relation vector and an increment dominating matrix, the input quantity of MPC online solving is reduced, the operation time is shortened, and the real-time requirement is met. And optimizing the dominating relationship to be optimized in the incremental dominating relationship vector through a PBiEO algorithm, thereby achieving an optimal control effect.

Description

Technical Field

[0001] The present invention relates to the technical field of AGV trajectory control, and in particular to a trolley trajectory tracking control method based on improved incremental control MPCAGV. Background Art

[0002] With the development of science and technology, more and more scenarios can be replaced by automated products to perform more efficient operations. Automated guided vehicles are one of them. Automated Guided Vehicle, also known as AGV, is an optical and visual navigation system with a guiding function, and is an artificial intelligence robot. Originated from the logistics industry, it is widely used in warehousing and logistics to transport goods. Due to its unique advantages, it is now used in other scenarios. Such as hospitals, hotels, etc. The key technologies of AGV include perception, decision-making and control. Trajectory tracking, as the core of the control part, is the key to ensure accurate, stable, efficient and safe delivery of goods to the destination when transporting goods. Therefore, it has important research value. When studying trajectory tracking, traditional control methods such as PID control, sliding film control, adaptive control, and fuzzy control are usually used. The MPC control algorithm adopts an online solution method, which can not only handle trajectory tracking problems, but also has significant advantages in dealing with multi-constrained optimal control problems. MPC is an advanced control strategy based on a dynamic model. It generates an optimal control sequence by predicting the dynamic model of the system, thereby realizing closed-loop control of the system. It can handle multi-variable, nonlinear systems and handle system constraints more flexibly. However, there are also some limitations, such as high computational complexity that makes it difficult to meet the real-time requirements of high-frequency sampling systems and high requirements for model accuracy. Therefore, if you want to use an MPC controller to control AGV trajectory tracking, you need to meet real-time requirements. Summary of the invention

[0003] In view of the above-mentioned prior art, the present invention provides an MPCAGV vehicle trajectory tracking control method based on improved incremental control, which mainly solves the technical problems existing in the above-mentioned background technology.

[0004] To achieve the above object, the technical solution of the embodiment of the present invention is implemented as follows: Based on the improved incremental control MPCAGV vehicle trajectory tracking control method, the method includes the following steps:

[0005] Construct the kinematic model of the AGV and perform linearization and discretization analysis;

[0006] Establish a traditional MPC controller based on the motion model;

[0007] Construct the incremental dominance vector and incremental dominance matrix, and build an improved incremental dominance MPC controller;

[0008] Based on BiEO, the PBiEO optimization algorithm is proposed to optimize the dominance relationship to be optimized in the incremental dominance vector;

[0009] The optimized increment-dominated MPC controller is used to track the desired trajectory.

[0010] Construct the kinematic model of the AGV, including:

[0011] In the global coordinate system, the kinematic model of the four-wheel AGV is:

[0012]

[0013] Where L represents the wheelbase between the front and rear wheels, θ represents the steering angle of the front wheel, (x, y) represents the midpoint of the line connecting the two rear wheels as the AGV reference point, δ represents the AGV yaw angle, and v is the rear wheel speed.

[0014] Linearize and discretize the AGV kinematic model, including:

[0015] Assume that the AGV control quantity is u=[v,δ] T , the state variable is X = [x, y, θ] T Therefore, at time t, the AGV state space equation is:

[0016]

[0017] The desired trajectory state equation can be written as:

[0018]

[0019] Where X r =[x r ,y r ,θ r ] T ,u r =[v r ,δ r ] T . Apply formula (2) at the desired trajectory point (X r ,u r ) and ignore the higher-order terms:

[0020]

[0021] Subtract formula (4) from formula (3):

[0022]

[0023] make Formula (5) can be simplified as:

[0024]

[0025] in is the Jacobian matrix of f with respect to X, which can be simplified to Same reason is the Jacobian matrix of f with respect to u, and we can calculate So far, the linearization of the AGV motion model has been completed, and the model will be discretized next.

[0026] Using the forward difference method to discretize formula (6), the discrete state equation at time k is obtained:

[0027]

[0028] in I is the identity matrix and T is the sampling period.

[0029] Build an MPC controller, including:

[0030] Set the controller input to the input increment at each moment It is related to and The relationship is Combining formula (7) we get:

[0031]

[0032] Formula (8) can be written in matrix form as:

[0033]

[0034] Where N u , N x They represent the number of controlled quantities and the number of state variables, which are N in this paper. u =2,N x =3. 0 represents the zero matrix, I represents the identity matrix, and the subscript represents the matrix dimension. The tracking effect of MPC-controlled AGV is ultimately reflected by its posture (X = [x, y, θ] T ), so let in Combined with formula (9), we get:

[0035]

[0036] In MPC control, there is a prediction horizon N p and control time domain N c, the control time domain is less than or equal to the control time domain. Based on the MPC principle, the prediction equation at the kth moment in the prediction time domain can be listed from formula (10):

[0037] η(k)=Φ(k)ξ(k)+Ψ(k)ΔU(k) ​​(11)

[0038] Where η(k)=[χ(k+1|k),χ(k+2|k),…χ(k+N c |k),…,χ(k+N p |k)] T is the prediction output matrix, is the state variable coefficient matrix, is the input matrix.

[0039] is the input coefficient matrix. 0 represents the zero matrix and the dimension is N x ×N u . Establish the loss function for AGV tracking trajectory:

[0040]

[0041] The first term represents the process error, and Q represents the weight coefficient of this term. The second term represents the incremental change, and R represents its weight. The third term is the relaxation factor and its weight, which has no practical significance. It can prevent the phenomenon of no solution in the process of solving formula (12). In addition, for the incremental It also needs to be set. When AGV is transporting goods, too fast speed or speed change will lead to unstable transportation of goods and cause accidents. Therefore, constraints can be given:

[0042]

[0043] From the previous definition, we can see that Therefore, the predicted output at time k+1 is And so on:

[0044]

[0045] Written in matrix form:

[0046]

[0047] The dimensions of the identity matrix and the zero matrix are both N u ×N u , simplifying formula (15) to:

[0048] U=Ut+A_tΔU (16)

[0049] Therefore, formula (13) can be written as:

[0050]

[0051] Substituting formula (10) and formula (11) into formula (12) to simplify it into the standard form of quadratic programming, and adding constraint condition (17) to obtain:

[0052]

[0053] St

[0054] U min ≤Ut+A_tΔU≤U max

[0055] ΔU min ≤ΔU≤ΔU max

[0056] The design of the incremental dominance vector matrix includes:

[0057] The incremental dominance relationship parameter is defined as ε, which means:

[0058]

[0059] Since the quantity to be solved in the cost function established in the previous section is 2 (N u ), so both the I matrix and the 0 matrix are 2-dimensional. For the control domain N c In order to make the increment calculation of the predicted time closer to the current time more accurate, we can set q uncontrolled increments, w controlled increments, and the remaining increments are e increments that need to optimize the control relationship. c It can be expressed by q, w, e as:

[0060] N c =q+e+w (20)

[0061] According to formula (22), the incremental control vector IDV can be set as:

[0062] IDV=[ε1,…,ε q ,…,ε q+e ,…,ε q+e+w ] (twenty one)

[0063] According to the definition of q and w and formula (19), we can get:

[0064]

[0065] The e variables in the middle need to be further optimized. Substituting formula (22) into formula (21), we get:

[0066] IDV=[I,…,I,εq+1 ,…,ε q+e ,0,…,0] (23)

[0067] Construct N based on IDV c First, initialize IDM and set IDM to N. c The matrix is ​​a zero-order matrix. According to formula (23), the elements on the first q diagonals can be assigned the value I. If there is a value of I in the following w rows, that is, ε=I in the interval [q+1,q+e] in IDV, according to formula (19), the item is not dominated by the previous increment, so the element in the same column of the row is I. If there is a value of 0, that is, ε=0 in the interval [q+1,q+e] in IDV, according to the definition, the item is dominated by the previous increment, so the column in that row that is closest to the increment that is not dominated by this item is set to I. Finally, according to the definition of formula (19), the remaining w items are treated in the same way as ε=0 in the q item.

[0068] Construct an incremental dominant MPC controller, including:

[0069]

[0070] Based on BiEO, PBiEO is proposed to optimize the vectors to be optimized in the incremental dominance vector, including:

[0071] BIEO needs to initialize the population. Since ε is a two-dimensional array with a determinant of 1 or 0, and EO is generally a one-dimensional number, it needs to be processed:

[0072]

[0073] in is the i-th individual to be initialized. eye(2) is a two-dimensional identity matrix, rand i is a random number between [0,1], and round is a rounding operation.

[0074] Then we need to define a loss function and propose an indicator for the optimization goal, so as to guide the individual to update and iterate. This paper needs to obtain the IRM matrix through optimization and bring it into formula (25) so that the AGV can meet the requirements of real-time and accuracy when tracking the trajectory. Therefore, we can comprehensively analyze the evaluation error of the state quantity of each sampling point and the online solution time to establish the cost equation:

[0075] COST=MEAN(time)+MEAN(|x(i)-x r (i)|+|y(i)-y r (i)|+|θ(i)-θ r(i)|) (26)

[0076] Where time is the time used to solve the quadratic optimization problem at each sampling point using the "interior point method". Mean is the average value.

[0077] Next, we need to build a balanced optimization pool. The four best particles so far after each update iteration plus the rounded average of these four particles are used as candidate equilibrium states. During each iteration, the five particles are selected with equal probability as the equilibrium state direction for subsequent particle update iterations. This mechanism can not only update particles with high quality, but also prevent particles from falling into local optimality. Its expression is:

[0078] ψ eq,pool =[ψ eq,best1 ,ψ eq,best2 ,ψ eq,best3 ,ψ eq,best4 ,ψ eq,ave ] (27)

[0079] ψ eq,ave =round(mean(ψ eq,best1 ,ψ eq,best2 ,ψ eq,best3 ,ψ eq,best4 ))

[0080] The exponential term can adapt to the global and local search capabilities of the conditions as the iteration cycle progresses, and its formula is:

[0081]

[0082] In formula (29), is the Hadamard product. a1 and a2 are the exploration capability and mining capability of this iteration, respectively, and are generally constants. r and λ are both 2D random vectors; time t decreases as the number of iterations increases. Iter and MaxIter are the current number of iterations and the maximum number of iterations, respectively.

[0083] Setting the quality generation item can prevent falling into the local optimum in a certain dimension during the optimization process. The expression is:

[0084]

[0085] Where r1 and r2 are both random numbers in [0,1]. ones(2) is a matrix of all "1"s, and its dimension is 2. eq,pool is the candidate solution selected in the balancing pool during this iteration, and Ψ is the current solution. GCP is the generation rate parameter, which mainly determines the probability of the generated item participating in the update process. GP is the generation probability, which determines how many particles use the generated item for update, and is generally taken as 0.5.

[0086] Finally, the update rules are formulated. A method for updating BIEO particles is proposed, and this method will be used in this paper.

[0087]

[0088] α=GP×s1 (33)

[0089]

[0090] In formula (35), is the kth dimension of the ith individual. s1 is a random number between [0,1] and greater than 0.5. s2 is an arbitrary random number between [0,1]. The update process uses formula (32) and formula (33) to calculate the change ΔΨ of each particle, and then substitutes it into the V-type function of formula (34) to calculate the update probability of each dimension of each individual. Finally, the individual situation is updated according to the update rule in formula (35), and the update is iterated until the optimal solution is found.

[0091] a1 and a2 respectively reflect the exploration ability and mining ability in the update and iteration process. It can be seen from formulas (28) and (29) that when a1 is large, the exploration ability is relatively strong, and the corresponding mining ability is relatively weak. At this time, this situation is suitable for the initial large-scale search (global search). When a2 is large, the mining ability is relatively strong, and the corresponding exploration ability is relatively weak. At this time, it is suitable for the later small-scale search (local search). Therefore, this paper replaces the fixed values ​​of a1 and a2 in formulas (38) and (39) with values ​​that can be self-adjusted according to the number of update iterations:

[0092]

[0093] For PBiEO, initialization is also required. First, the initialized particles Grouping. The number of populations is generally set to ten, so this article divides the groups into 5 groups. Then sort the fitness of the particles in each group, find the top 4 particles with strong fitness in each group to build a balanced optimization pool and the best particles in each group. At the same time, the global optimal solution and the top 4 particles with the strongest global fitness are also found, and a global balanced optimization pool is established. After that, each group is updated and iterated separately. When a certain number of iterations are reached, two different strategies will be executed for communication between groups. The number of iterations for communication between groups set in this article is a multiple of 10. The communication method of the first strategy is shown in the following formula:

[0094]

[0095] The first communication strategy is to randomly select a particle Ψ from the global equilibrium optimization pool when the current number of iterations is less than 2 / 5 of the total number of iterations and the current number of iterations is a multiple of 10. eq , and combined with the global optimal particle Bestp. According to formula (38), the particles with poor fitness in each group are mutated (the particles with poor fitness are the second half of the particles with poor fitness). Through this communication between groups, particles with poor fitness can be screened out in the exploration stage and these particles can be quickly brought close to the optimal solution. This further improves the convergence speed of the algorithm.

[0096] When the number of iterations exceeds 2 / 5 of the total number of iterations, and the number of iterations is a multiple of 10, the second communication strategy will be used for updating. The second half of the particles with poor fitness are replaced by the round integer of the average value of the best particles in the adjacent group. The second strategy is expressed as:

[0097]

[0098] Replace all particles with poor fitness in each group. This strategy strengthens the connection between populations, improves the mining ability of the algorithm in the middle and late stages, and effectively avoids premature convergence of the algorithm. Except for these special iteration times, these two strategies need to be applied. At other times, the particles in each group are updated and iterated in the way described in the previous section.

[0099] The beneficial effects of the present invention are as follows: the improved incremental dominance MPC AGV vehicle trajectory tracking control method proposed in this application constructs an AGV vehicle kinematic model in order to meet the real-time requirements when MPC controls the AGV trajectory tracking. At the same time, the kinematic model is linearized and discretized using Taylor's formula and forward Euler. The MPC controller is designed based on the linearized and discretized motion model. By defining an incremental dominance relationship vector and an incremental dominance matrix, the input amount of the MPC online solution is reduced, and the calculation time is reduced to meet the real-time requirements. The dominance relationship to be optimized in the incremental dominance relationship vector is optimized by the PBiEO algorithm, so as to achieve the optimal control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] Figure 1 This is a flow chart of the MPCAGV trajectory tracking control method based on improved incremental control in an embodiment of the present application;

[0101] Figure 2 This is the AGV motion model diagram;

[0102] Figure 3 Generate IRM flow chart for IRV;

[0103] Figure 4 Updated flowchart for BiEO;

[0104] Figure 5 This is a schematic diagram of the first communication strategy;

[0105] Figure 6 This is a schematic diagram of the second communication strategy;

[0106] Figure 7 It is the PBIEO flow chart;

[0107] Figure 8 is the PIEO pseudo code;

[0108] Fig. 9 This is the first set of experimental iteration comparison chart (N p =70);

[0109] Fig.10 This is the second set of experimental iteration comparison chart (N p =80);

[0110] Fig.11 This is the third set of experimental iteration comparison chart (N p =90);

[0111] Fig.12 Tracking effects of two different MPC controllers (N p =70);

[0112] Fig.13 Tracking effects of two different MPC controllers (N p =80);

[0113] Fig.14 Tracking effects of two different MPC controllers (N p =90). DETAILED DESCRIPTION

[0114] The technical solution of the present invention is further described in detail below in conjunction with the drawings and specific implementations of the specification. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used in the specification of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0115] Please refer to the attached Figure 1 The present application provides a MPCAGV vehicle trajectory tracking control method based on improved incremental dominance, the method comprising the following steps:

[0116] S1: Construct the kinematic model of the AGV and perform linearization and discretization analysis;

[0117] S2: Establish a traditional MPC controller based on the motion model;

[0118] S3: construct the incremental dominance vector and incremental dominance matrix, and build an improved incremental dominance MPC controller;

[0119] S4: Based on BiEO, the PBiEO optimization algorithm is proposed to optimize the dominance relationship to be optimized in the incremental dominance vector;

[0120] S5: Use the optimized increment-dominated MPC controller to track the desired trajectory.

[0121] In an implementation manner, constructing an AGV kinematic model specifically includes:

[0122] In the global reference system, combined Figure 2 , the AGV motion model is:

[0123]

[0124] Where L represents the wheelbase between the front and rear wheels, θ represents the steering angle of the front wheels, (x, y) represents the midpoint of the line connecting the two rear wheels as the AGV reference point, (x r ,y r ) represents the desired trajectory coordinates. δ represents the AGV yaw angle, and v is the rear wheel speed.

[0125] Then the model is linearized and discretized, including:

[0126] Assume that the AGV control quantity is u=[v,δ] T , the state variable is X = [x, y, θ] T Therefore, at time t, formula (1) can be written as the state equation:

[0127]

[0128] The desired trajectory state equation can be written as:

[0129]

[0130] Where X r =[x r ,y r ,θ r ] T ,u r =[v r ,δ r ] T . Apply formula (2) at the desired trajectory point (X r ,u r ) and ignore the higher-order terms:

[0131]

[0132] Subtract formula (4) from formula (3):

[0133]

[0134] make Formula (5) can be simplified as:

[0135]

[0136] in is the Jacobian matrix of f with respect to X, which can be simplified to Same reason is the Jacobian matrix of f with respect to u, and we can calculate

[0137] Using the forward difference method to discretize formula (6), the discrete state equation at time k is obtained:

[0138]

[0139] in I is the identity matrix and T is the sampling period.

[0140] Then, the MPC controller is established through the linearized and discretized model, including:

[0141] Set the controller input to the input increment at each moment It is related to and The relationship is Combining formula (7) we get:

[0142]

[0143] Formula (8) can be written in matrix form as:

[0144]

[0145] Where N u , N x They represent the number of controlled quantities and the number of state variables, which are N in this paper. u =2,N x =3. 0 represents the zero matrix, I represents the identity matrix, and the subscript represents the matrix dimension. The tracking effect of MPC-controlled AGV is ultimately reflected by its posture (X = [x, y, θ] T ), so let in Combined with formula (9), we get:

[0146]

[0147] In MPC control, there is a prediction horizon N p and control time domain N c , the control time domain is less than or equal to the control time domain. Based on the MPC principle, the prediction equation at the kth moment in the prediction time domain can be listed from formula (10):

[0148] η(k)=Φ(k)ξ(k)+Ψ(k)ΔU(k) ​​(11)

[0149] Where η(k)=[χ(k+1|k),χ(k+2|k),…χ(k+N c |k),…,χ(k+N p |k)] T is the prediction output matrix, is the state variable coefficient matrix, is the input matrix.

[0150] is the input coefficient matrix. 0 represents the zero matrix and the dimension is N x ×N u . Establish the loss function for AGV tracking trajectory:

[0151]

[0152] The first term represents the process error, and Q represents the weight coefficient of this term. The second term represents the incremental change, and R represents its weight. The third term is the relaxation factor and its weight. It also needs to be set. When AGV is transporting goods, too fast speed or speed change will lead to unstable transportation of goods and cause accidents. Therefore, constraints can be given:

[0153]

[0154] From the previous definition, we can see that Therefore, the predicted output at time k+1 is And so on:

[0155]

[0156] Written in matrix form:

[0157]

[0158] The dimensions of the identity matrix and the zero matrix are both N u ×N u , simplifying formula (15) to:

[0159] U=Ut+A_tΔU (16)

[0160] Therefore, formula (13) can be written as:

[0161]

[0162] Substituting formula (10) and formula (11) into formula (12) to simplify it into the standard form of quadratic programming, and adding constraint condition (17) to obtain:

[0163]

[0164] St

[0165] U min ≤Ut+A_tΔU≤U max

[0166] ΔU min ≤ΔU≤ΔU max

[0167] Then construct the incremental dominance vector and incremental dominance matrix, and build an improved incremental dominance MPC controller, including:

[0168] The incremental dominance relationship parameter is defined as ε, which means:

[0169]

[0170] Since the quantity to be solved in the cost function established in the previous section is 2 (N u ), so both the I matrix and the 0 matrix are 2-dimensional. For the control domain N c In order to make the increment calculation of the predicted time closer to the current time more accurate, we can set q uncontrolled increments, w controlled increments, and the remaining increments are e increments that need to optimize the control relationship. c It can be expressed by q, w, e as:

[0171] Nc=q+e+w (20)

[0172] According to formula (20), the incremental control vector IDV can be set as:

[0173] IDV=[ε1,...,ε q ,…,ε q+e ,…,ε q+e+w ] (twenty one)

[0174] According to the definition of q and w and formula (19), we can get:

[0175]

[0176] The e variables in the middle need to be further optimized. Substituting formula (22) into formula (21), we get:

[0177] IDV=[I,…,I,ε q+1 ,…,ε q+e ,0,…,0] (23)

[0178] Construct N based on IDV c First, initialize IDM and set IDM to N. c Order zero matrix. According to formula (23), the elements on the first q diagonals can be assigned the value of I. If there is a value of I in the following w rows, that is, ε=I in the interval [q+1,q+e] in IDV, according to formula (19), the item is not dominated by the previous increment, then the element in the same column of the row is I. If there is a value of 0, that is, ε=0 in the interval [q+1,q+e] in IDV, according to the definition, the item is dominated by the previous increment, so the column in that row that is closest to the increment that is not dominated by this item is set to I. Finally, according to the definition of formula (19), the remaining w items are processed in the same way as ε=0 in the q item. The specific flow chart is as follows Figure 3 shown.

[0179] Convert the new MPC controller into the standard form of quadratic programming:

[0180]

[0181] For quadratic programming problems, the "interior point method" can be used to solve them.

[0182] Based on BiEO, the PBiEO optimization algorithm is proposed to optimize the dominance relationship to be optimized in the incremental dominance vector. The implementation includes:

[0183] First, like other intelligent optimization algorithms, BIEO needs to initialize the population. Since ε is a two-dimensional array with a determinant of 1 or 0, and EO is generally a one-dimensional number, it needs to be processed:

[0184]

[0185] in is the i-th individual to be initialized. eye(2) is a two-dimensional identity matrix, rand i is a random number between [0,1], and round is a rounding operation.

[0186] Then we need to define a loss function and propose an indicator for the optimization goal, so as to guide the individual to update and iterate. This paper needs to obtain the IRM matrix through optimization and bring it into formula (24) so ​​that the AGV can meet the requirements of real-time and accuracy when tracking the trajectory. Therefore, we can comprehensively analyze the evaluation error of the state quantity of each sampling point and the online solution time to establish the cost equation:

[0187] COST=MEAN(time)+MEAN(|x(i)-x r (i)|+|y(i)-y r (i)|+|θ(i)-θ r (i)|) (26)

[0188] Where time is the time used to solve the quadratic optimization problem at each sampling point using the "interior point method". Mean is the average value.

[0189] Next, we need to build a balanced optimization pool. The four best particles so far after each update iteration plus the rounded average of these four particles are used as candidate equilibrium states. During each iteration, the five particles are selected with equal probability as the equilibrium state direction for subsequent particle update iterations. This mechanism can not only update particles with high quality, but also prevent particles from falling into local optimality. Its expression is:

[0190] ψ eq,pool =[ψ eq,best1 ,ψ eq,best2 ,ψ eq,best3 ,ψ eq,best4 ,ψ eq,ave ] (27)

[0191] ψ eq,ave =round(mean(ψ eq,best1 ,ψ eq,best2 ,ψ eq,best3 ,ψ eq,best4 )) (28)

[0192] The exponential term can adapt to the global and local search capabilities of the conditions as the iteration cycle progresses, and its formula is:

[0193]

[0194] In formula (38), is the Hadamard product. a1 and a2 are the exploration capability and mining capability of this iteration, respectively, and are generally constants. r and λ are both 2D random vectors; time t decreases as the number of iterations increases. Iter and MaxIter are the current number of iterations and the maximum number of iterations, respectively.

[0195] Setting the quality generation item can prevent falling into the local optimum in a certain dimension during the optimization process. The expression is:

[0196]

[0197]

[0198] Where r1 and r2 are both random numbers in [0,1]. ones(2) is a matrix of all "1"s, and its dimension is 2. eq,pool is the candidate solution selected in the balancing pool during this iteration, and Ψ is the current solution. GCP is the generation rate parameter, which mainly determines the probability of the generated item participating in the update process. GP is the generation probability, which determines how many particles use the generated item for update, and is generally taken as 0.5.

[0199] Finally, formulate the update rules. This article will use this method to update:

[0200]

[0201] α=GP×s1 (34)

[0202]

[0203] In formula (36), is the kth dimension of the ith individual. s1 is a random number between [0,1] and greater than 0.5. s2 is an arbitrary random number between [0,1]. The update process uses formula (33) and formula (34) to calculate the change ΔΨ of each particle, and then substitutes it into the V-type function of formula (35) to calculate the update probability of each dimension of each individual. Finally, the individual situation is updated according to the update rule in formula (36), and the update is iterated until the optimal solution is found. Figure 4 This is a schematic diagram of the BIEO update process.

[0204] a1 and a2 respectively reflect the exploration ability and mining ability in the update and iteration process. It can be seen from formulas (29) and (30) that when a1 is large, the exploration ability is relatively strong, and the corresponding mining ability is relatively weak. At this time, this situation is suitable for the initial large-scale search (global search). When a2 is large, the mining ability is relatively strong, and the corresponding exploration ability is relatively weak. At this time, it is suitable for the later small-scale search (local search). Therefore, this paper replaces the fixed values ​​of a1 and a2 in formulas (38) and (39) with values ​​that can be self-adjusted according to the number of update iterations:

[0205]

[0206] For PBiEO, initialization is also required. First, the initialized particles Grouping. The number of populations is generally set to ten, so this article divides the groups into 5 groups. Then sort the fitness of the particles in each group, find the top 4 particles with strong fitness in each group to build a balanced optimization pool and the best particles in each group. At the same time, the global optimal solution and the top 4 particles with the strongest global fitness are also found, and a global balanced optimization pool is established. After that, each group is updated and iterated separately. When a certain number of iterations are reached, two different strategies will be executed for communication between groups. The number of iterations for communication between groups set in this article is a multiple of 10. The communication method of the first strategy is shown in the following formula:

[0207]

[0208] The first communication strategy is to randomly select a particle Ψ from the global equilibrium optimization pool when the current number of iterations is less than 2 / 5 of the total number of iterations and the current number of iterations is a multiple of 10. eq , and combined with the global optimal particle Bestp. According to formula (39), the particles with poor fitness in each group are mutated (the particles with poor fitness are the second half of the particles with poor fitness). Through this communication between groups, particles with poor fitness can be screened out in the exploration stage and these particles can be quickly brought close to the optimal solution. This further improves the convergence speed of the algorithm. Figure 5 The first communication strategy is demonstrated.

[0209] When the number of iterations exceeds 2 / 5 of the total number of iterations and the number of iterations is a multiple of 10, the second communication strategy will be used for updating. The second half of the particles with poor fitness are replaced by the round integer of the average value of the best particles in the adjacent group. The second communication strategy is as follows Figure 6 The expression is:

[0210]

[0211] Replace all particles with poor fitness in each group. This strategy strengthens the connection between populations, improves the mining ability of the algorithm in the middle and late stages, and effectively avoids the algorithm from converging prematurely. Except for these special iteration times, these two strategies need to be applied. At other times, the particles in each group are updated and iterated in the way described in the previous section. The specific process and pseudo code are as follows Figure 7 , 8 shown.

[0212] The expected trajectory equation of the tracked trajectory is:

[0213]

[0214] When the prediction time domain is 70, 80, 90 and the control time domain is 40, 50, 50, the PBiEO algorithm is simulated and compared with the BiEO algorithm. The results are as follows Fig. 9 As shown. It can be seen that the proposed PBiEO algorithm converges faster than the BiEO algorithm and can also avoid the algorithm iteration from entering the local optimum. The following table shows the optimal values ​​after the final iteration of the three groups of experiments

[0215]

[0216] The IRV dominant relationship to be optimized after PBiEO iteration (e) is shown in the following table

[0217]

[0218] The optimized incremental control MPC controller is used to track the desired trajectory. The simulation results are as follows: Figure 10-12 Specifically:

[0219] When the incremental dominant MPC controller improved in this paper controls the trajectory tracking of the AGV, multiple dimensions are improved, and the average solution time is shorter than that of the traditional MPC control and the incremental dominant MPC controller optimized by BiEO, which further meets the real-time requirements of the high-sampling AGV tracking system. The following table shows the comparison of online solution time under different prediction domains with a sampling time of 0.01s:

[0220]

[0221] It can be found from the table that for the traditional MPC controller, when the control domain is very small, the real-time requirements can be met. However, in order to obtain accurate trajectory tracking effects, when the control domain begins to become larger, the amount of online calculation will also become larger, and the real-time requirements cannot be met. The present invention solves this problem, and the online solution time is greatly reduced, and is less than the sampling time of 0.001s, meeting the sampling frequency AGV tracking rule requirements.

[0222] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. The protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. Based on the improved incremental control MPCAGV vehicle trajectory tracking control method, the characteristics are: The method comprises the following steps: Construct the kinematic model of the AGV and perform linear analysis; Establish a traditional MPC controller based on the motion model; Construct the incremental dominance vector and incremental dominance matrix, and build an improved incremental dominance MPC controller; The PBiEO optimization algorithm is proposed to optimize the dominance relationship to be optimized in the incremental dominance vector; The optimized increment-dominated MPC controller is used to track the desired trajectory.

2. The MPCAGV vehicle trajectory tracking control method based on improved incremental control according to claim 1 is characterized in that: Construct the kinematic model of the AGV, including: In the global coordinate system, the kinematic model of the four-wheel AGV is: Where L represents the wheelbase between the front and rear wheels, θ represents the steering angle of the front wheel, (x, y) represents the midpoint of the line connecting the two rear wheels as the AGV reference point in the XOY coordinate system, δ represents the AGV yaw angle, and v is the rear wheel speed.

3. The MPCAGV vehicle trajectory tracking control method based on improved incremental control according to claim 1 is characterized in that: The AGV car motion model is linearized and discretized, including: Assume that the AGV control quantity is u=[v,δ] T , the state variable is X = [x, y, θ] T , the expected trajectory control amount u r =[v r ,δ r ] T , the expected trajectory state variable is X r =[x r ,y r ,θ r ] T , and write them in the form of state equations: Apply equation (2) at the desired trajectory point (X r ,u r ) and ignore the higher-order terms: Subtract formula (4) from formula (3): make Formula (5) can be simplified as: in is the Jacobian matrix of f with respect to X, which can be simplified to Same reason is the Jacobian matrix of f with respect to u, and we can calculate The forward difference method is used to discretize formula (6), and the discrete state equation at time k is obtained as follows: in I is the unit matrix, T is the sampling period, 4. The MPCAGV vehicle trajectory tracking control method based on improved incremental control according to claim 1 is characterized in that: The traditional MPC controller is established based on the motion model, including: Set the controller input to the input increment at each moment It is related to and The relationship is Combining formula (7) we get: Formula (8) can be written in matrix form as: Where N u , N x They represent the number of controlled quantities and the number of state variables, which are N in this paper. u =2,N x =3, 0 represents the zero matrix, I represents the identity matrix, and the subscript represents the matrix dimension. The tracking effect of MPC-controlled AGV is ultimately reflected by its posture (X = [x, y, θ] T ), so let in Combined with formula (9), we get: In MPC control, there is a prediction horizon N p and control time domain N c , the control time domain is less than or equal to the control time domain. Based on the MPC principle, the prediction equation at the kth moment in the prediction time domain can be listed from formula (10): η(k)=Φ(k)ξ(k)+Ψ(k)ΔU(k) ​​(11) Where η(k)=[χ(k+1|k),χ(k+2|k),…χ(k+N c |k),…,χ(k+N p |k)] T is the prediction output matrix, is the state variable coefficient matrix, is the input matrix, is the input coefficient matrix, where 0 represents a zero matrix and the dimension is N x ×N u , establish the loss function for AGV tracking trajectory: The first term represents the process error, Q represents the weight coefficient of this term, the second term represents the incremental change, R represents its weight, and the third term is the relaxation factor and its weight, which has no practical meaning. It can prevent the phenomenon of no solution in the process of solving formula (12). Adding the constraint conditions, the final formula is: St U min ≤Ut+A_tΔU≤U max ΔU min ≤ΔU≤ΔU max 。 5. The MPCAGV vehicle trajectory tracking control method based on improved incremental control according to claim 1 is characterized in that: Construct the incremental dominance vector and incremental dominance matrix, including: First, define the incremental dominance relationship parameter as ε, which means: Since the quantity to be solved in the cost function established in the previous section is 2 (N u ), so the I matrix and the 0 matrix are both 2-dimensional. For the control domain N c In order to make the increment calculation of the predicted time closer to the current time more accurate, we can set q uncontrolled increments, w controlled increments, and the remaining increments are e increments that need to optimize the control relationship. Therefore, the control domain N c It can be expressed by q, w, e as: N c =q+e+w (15) According to formula (15), the incremental control vector IDV can be set as: IDV=[ε1,…,ε q ,…,eh q+e ,…,eh q+e+w ] (16) According to the definition of q and w and formula (19), we can get: Construct N based on IDV c First, initialize IDM and set IDM to N. c For a zero-order matrix, according to formula (23), the elements on the first q diagonals can be assigned the value of I. If there is a value of I in the following w rows, that is, ε=I in the interval [q+1,q+e] in IDV, according to formula (14), the item is not dominated by the previous increment, then the element in the same column of the row is I. If there is a value of 0, that is, ε=0 in the interval [q+1,q+e] in IDV, according to the definition, the item is dominated by the previous increment, so the column in that row that is closest to the increment that is not dominated by this item is set to I. Finally, according to the definition of formula (14), the remaining w items are treated in the same way as ε=0 in the q item.

6. The MPCAGV vehicle trajectory tracking control method based on improved incremental control according to claim 1 is characterized in that: Build an improved incremental control MPC controller, including: St 7. The MPCAGV vehicle trajectory tracking control method based on improved incremental control according to claim 1 is characterized in that: The PBiEO optimization algorithm is proposed, which includes: Based on BiEO, the fixed values ​​of a1 and a2 are replaced with values ​​that can be adjusted according to the number of update iterations. The initial randomized particles are grouped and two strategies are exchanged between groups at fixed update iteration points. The first communication method: At 2 / 5 of the total number of iterations, and when the number of iterations is a multiple of 10, a particle Ψ is randomly selected from the global equilibrium optimization pool eq , and combined with the global optimal particle Bestp, expressed as (21). According to expression (21), the particles with poor fitness in each group are mutated (the particles with poor fitness are the second half of the particles with poor fitness). Through this communication method between groups, the particles with poor fitness can be screened out in the exploration stage and these particles can be quickly close to the optimal solution. The second communication strategy: When the number of iterations exceeds 2 / 5 of the total number of iterations and the number of iterations is a multiple of 10, the second communication strategy will be used for updating. The second half of the particles with poor fitness will be replaced by the round integer of the average value of the best particles in the adjacent group. The expression is: Among them, worst refers to the worst half of the particles, best refers to the best particle in each group, and round refers to rounding.

Images