AGV trajectory tracking control method based on nonlinear optimization algorithm

Abstract

The invention discloses an AGV trajectory tracking control method based on a nonlinear optimization algorithm, and belongs to the technical field of trajectory tracking, and the method comprises the following steps: 1, building a discrete motion model according to the kinematics relation of an AGV in a two-dimensional plane; 2, based on the discrete motion model, designing a secondary loss function of the AGV according to the reference trajectory; step 3, calculating gradients of control input at all sampling moments; step 4, calculating a Hessian matrix controlled and input at all sampling moments; 5, designing a nonlinear optimization algorithm and solving an optimal solution; 6, converting the first control input of the optimal solution into a control instruction to drive the AGV to operate; and when the pose of the AGV in the plane coordinate system changes, the steps 3-5 are executed repeatedly, and then trajectory tracking control of the AGV is achieved. The method is high in calculation efficiency and stable, and online real-time high-precision AGV trajectory tracking can be realized.

Description

Technical Field

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

[0002] AGV (Automated Guided Vehicle) is an intelligent handling device that relies on automatic navigation technology to complete material transportation. It achieves efficient and safe material handling and reduces human intervention through preset paths and flexible navigation systems. Track tracking is the core technology that ensures that AGV runs accurately along the path, thereby improving transportation stability and operating efficiency. AGV is widely used in logistics warehousing, production line transportation and other scenarios, and is one of the important tools for intelligent manufacturing and modern industry.

[0003] At present, the commonly used algorithms for 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 algorithm, 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, so there are certain challenges in meeting online real-time computing requirements. Summary of the invention

[0004] In order to solve the above problems, the present invention proposes an AGV trajectory tracking control method based on a nonlinear optimization algorithm. The method gives an explicit iterative formula for solving gradients and Hessian matrices in dynamic problems, and designs a new nonlinear optimization algorithm with low computational complexity, high computational efficiency and stability, which helps to achieve online real-time high-precision AGV trajectory tracking.

[0005] The technical solution of the present invention is as follows: An AGV trajectory tracking control method based on a nonlinear optimization algorithm comprises the following steps: 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; Step 4: Calculate the Hessian matrix of the control input at all sampling moments; 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.

[0006] Furthermore, in step 1, the discrete motion model is: (1); in, , , Respectively represent The horizontal coordinate, vertical coordinate and heading angle of AGV in the plane coordinate system at each sampling moment; , , Respectively represent The horizontal coordinate, vertical coordinate and heading angle of AGV in the plane coordinate system at each sampling moment; represents the sampling interval; , Respectively represent The linear velocity and angular velocity of AGV in the plane coordinate system at each sampling moment.

[0007] Furthermore, in step 2, the quadratic loss function for: (2); in, , Respectively represent The position and reference position of AGV in the plane coordinate system at each sampling moment; , , Respectively The reference abscissa, reference ordinate, and reference heading angle of the AGV in the plane coordinate system at each sampling moment; , Respectively represent The control input and reference input of AGV in the plane coordinate system at each sampling moment; , Respectively The reference linear velocity and reference angular velocity of the AGV in the plane coordinate system at each sampling moment; , They represent respectively the given dimension-compatible semi-positive definite weight matrix and positive definite weight matrix; Represents the total number of sampling moments.

[0008] Furthermore, the specific process of step 3 is as follows: Step 3.1: Record all sampling times The control input is ,in, , Respectively The control input of AGV in the plane coordinate system at each sampling moment The transposition of The control input of AGV in the plane coordinate system at each sampling moment The transpose of ; given all sampling moments Control input The initial iteration value of ; 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: (3); in, represents the first Hamiltonian function; Indicates Sampling time The corresponding co-variables; , , Respectively Sampling time , , The corresponding co-variables; Indicates The position and posture of AGV in the plane coordinate system at each sampling moment; for The transpose of Step 3.3.2. Co-state variables Satisfies the following equation: (4); (5); in, Indicates Sampling time The corresponding co-variables; Indicates Sampling time The corresponding co-state variable, at this time, the co-state variable is the given terminal value; Indicates The position and posture of AGV in the plane coordinate system at each sampling moment; 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 quadratic loss function The formula for the gradient of the control input at all sampling moments is as follows: (6); in, Represents the quadratic loss function The gradient of the control input at all sampling moments has 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; is a two-dimensional row vector containing two elements, which are the gradient and The element at index position.

[0009] Furthermore, the specific process of step 4 is as follows: 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: (7); in, is the second Hamiltonian function; Indicates Sampling time The corresponding backward iterative adjoint state; Indicates Sampling time The corresponding forward iteration adjoint state; for The transpose of for The transpose of The element at the 0th index position is recorded as ; , , They are the initial position of AGV in the plane coordinate system at the 0th sampling time, the control input of AGV in the plane coordinate system at the 0th sampling time, and the co-state variable at the 1st sampling time; Step 4.1.2, the forward iterative adjoint state and the backward iterative adjoint state satisfy the following equations respectively: (8); (9); (10); in, Indicates Sampling time The corresponding forward iteration adjoint state; Indicates Sampling time The corresponding backward iterative adjoint state; Indicates the 0th sampling time The corresponding forward iteration adjoint state, at this time the forward iteration adjoint state is the given initial value; Indicates Sampling time The corresponding backward iterative adjoint state, at this time, the backward iterative adjoint state is the given terminal value; Step 4.1.3: Execute formula (8) and formula (9) respectively to obtain all sampling times The forward iterative adjoint state and the backward iterative adjoint state of ; Step 4.2, calculate the first row element of the Hessian matrix; the specific process is: Execute formula (11) to obtain the first row element of the Hessian matrix: (11); in, Represents the quadratic loss function Regarding the first row elements of the Hessian matrix of the control input at all sampling moments, 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 first row of the Hessian matrix. and The element at index position; 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 the second Hamiltonian function in formula (7) to , each time you calculate, , , The value in the brackets is incremented by one, that is, the next element is taken in turn; the calculation method for the even-numbered row elements of the Hessian matrix is ​​the same as the calculation method for the first row elements. When calculating, take The element at the first index position is recorded as , changing the second Hamiltonian for , each time you calculate, , , The values ​​in the brackets are incremented by one, that is, one element is taken at a time; the elements of all rows constitute the Hessian matrix.

[0010] Furthermore, the specific process of step 5 is as follows: Step 5.1: Given the 0th iteration Initial value of and the algorithm stopping threshold ; 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 ; (12); (13); (14); in, , The numbers representing the number of iterations of the outer loop and inner loop respectively; , Respectively represent sequence Outer loop iteration The iteration value of , , Respectively represent the 0th and 1st iterations of the inner loop sequence Updates , Represent the quadratic loss function respectively exist 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 the , with the final As the optimal solution of the nonlinear optimization algorithm.

[0011] Beneficial technical effects brought by the present invention: The AGV trajectory tracking method of the present invention has excellent stability and accuracy, and can be widely used in scenes such as ports, logistics distribution centers, and smart parking lots, effectively improving operating efficiency, reducing accidental risks, and ensuring operational safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] Figure 1 It is a flow chart of the AGV trajectory tracking control method based on the nonlinear optimization algorithm of the present invention.

[0013] Figure 2 It is the horizontal and vertical coordinate tracking diagram of the AGV after the nonlinear optimization algorithm of the present invention is adopted in the experiment of the present invention.

[0014] Figure 3 This is a comparison chart of the calculation time after the nonlinear optimization algorithm of the present invention is used in the experiment of the present invention. DETAILED DESCRIPTION

[0015] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments: like Figure 1 As shown, an AGV trajectory tracking control method based on a nonlinear optimization algorithm includes the following steps: Step 1: Establish a discrete motion model based on the kinematic relationship of AGV in a two-dimensional plane: (1); in, , , Respectively represent The horizontal coordinate, vertical coordinate and heading angle of AGV in the plane coordinate system at each sampling moment; , , Respectively represent The horizontal coordinate, vertical coordinate and heading angle of AGV in the plane coordinate system at each sampling moment; represents the sampling interval; , Respectively represent The linear velocity and angular velocity of AGV in the plane coordinate system at each sampling moment.

[0016] 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: (2); in, , Respectively represent The position and reference position of AGV in the plane coordinate system at each sampling moment; , , Respectively The reference abscissa, reference ordinate, and reference heading angle of the AGV in the plane coordinate system at each sampling moment; , Respectively represent The control input and reference input of AGV in the plane coordinate system at each sampling moment; , Respectively The reference linear velocity and reference angular velocity of the AGV in the plane coordinate system at each sampling moment; , They represent respectively the given dimension-compatible semi-positive definite weight matrix and positive definite weight matrix; Represents the total number of sampling moments.

[0017] Step 3: Calculate the gradient of the control input at all sampling moments; the specific process is: Step 3.1: Record all sampling times The control input is ,in, , Respectively The control input of AGV in the plane coordinate system at each sampling moment The transposition of The control input of AGV in the plane coordinate system at each sampling moment Given all sampling moments Control input The initial iteration value of ; 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: (3); in, represents the first Hamiltonian function; Indicates Sampling time The corresponding co-variables; , , Respectively Sampling time , , The corresponding co-variables; Indicates The position and posture of AGV in the plane coordinate system at each sampling moment; for The transpose of .

[0018] Step 3.3.2: According to the maximum principle and combined with the first Hamiltonian function of formula (3), the co-state variable Satisfies the following equation: (4); (5); in, Indicates Sampling time The corresponding co-variables; Indicates Sampling time The corresponding co-state variable, at this time, the co-state variable is the given terminal value; Indicates The position and posture of AGV in the plane coordinate system at each sampling moment; Represents a 3-dimensional zero vector.

[0019] Step 3.3.3, execute formula (4) to obtain the co-state variables at all sampling times; Step 3.4: Given the quadratic loss function of formula (2), design the gradient formula for the control input at all sampling moments as follows: (6); in, Represents the quadratic loss function The gradient of the control input at all sampling moments has index positions, where is a positive integer, and the total number of sampling moments 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; is a two-dimensional row vector containing two elements, which are the gradient and Execute formula (6) to get the quadratic loss function The gradient of the control input with respect to all sampling times.

[0020] Step 4: Based on the gradient formula (6) obtained in step 3, calculate the Hessian matrix of the control input at all sampling times; 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, combining formula (1) and formula (4), define the following second Hamiltonian function: (7); in, is the second Hamiltonian function; Indicates Sampling time The corresponding backward iterative adjoint state; Indicates Sampling time The corresponding forward iteration adjoint state; for The transpose of for Take the transpose of The element at the 0th index position is recorded as ; , , They are the initial posture at the 0th sampling moment, the control input at the 0th sampling moment, and the co-state variables at the 1st sampling moment.

[0021] Step 4.1.2: According to the maximum principle and the second Hamiltonian function of formula (7), the forward iterative adjoint state and the backward iterative adjoint state satisfy the following equations respectively: (8); (9); (10); in, Indicates Sampling time The corresponding forward iteration adjoint state; Indicates Sampling time The corresponding backward iterative adjoint state; Indicates the 0th sampling time The corresponding forward iteration adjoint state, at this time the forward iteration adjoint state is the given initial value; Indicates Sampling time The corresponding backward iterative adjoint state, at this time the backward iterative adjoint state is the given terminal value.

[0022] Step 4.1.3: Execute formula (8) and formula (9) respectively to obtain all sampling times The forward iterative adjoint state and the backward iterative adjoint state of ; Step 4.2, calculate the first row element of the Hessian matrix; the specific process is: Given formula (2), the calculation formula for the first row element of the Hessian matrix of the control input at all sampling moments is as follows: (11); in, Represents the quadratic loss function Regarding the first row elements of the Hessian matrix of the control input at all sampling moments, 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 first row of the Hessian matrix. and Execute formula (11) to get the first row element 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 the second Hamiltonian function in formula (7) to , each time you calculate, , , The value in the brackets is incremented by one, that is, the next element is taken in turn; the calculation method for the even-numbered row elements of the Hessian matrix is ​​the same as the calculation method for the first row elements. When calculating, take The element at the first index position , changing the second Hamiltonian for , each time you calculate, , , The values ​​in the brackets are incremented by one, that is, one element is taken at a time; the elements of all rows constitute the Hessian matrix.

[0023] Step 5: Design a nonlinear optimization algorithm and find the optimal solution; the specific process is: Step 5.1: Given the 0th iteration Initial value of and the algorithm stopping threshold ; 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 ; (12); (13); (14); in, , The numbers representing the number of iterations of the outer loop and inner loop respectively; , Respectively represent sequence Outer loop iteration The iteration value of , , Respectively represent the 0th and 1st iterations of the inner loop sequence Updates , Represent the quadratic loss function respectively exist The gradient and Hessian matrix of the control input at all sampling moments; Represents the given Dimensionally consistent positive definite weight matrix.

[0024] Step 5.5: Repeat steps 5.2 to 5.4 until the , with the final As the optimal solution of the nonlinear optimization algorithm.

[0025] Step 6: Convert the first control input of the optimal solution obtained in step 5 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.

[0026] In order to prove the superiority and feasibility of the method proposed in this invention, the following comparative experiments are carried out with the mainstream quasi-Newton algorithm and sequential quadratic programming algorithm as the benchmark. For the AGV discrete motion model shown in formula (1), the initial pose is selected as , , , They represent the horizontal coordinate, vertical coordinate, and heading angle of the AGV in the plane coordinate system at the 0th sampling time, and the sampling interval is The total number of sampling moments is ; For the quadratic loss function shown in formula (2), choose the semi-positive definite weighting matrix , the positive definite weight matrix ; For formula (12)-formula (14), select Dimensionally consistent positive definite weight matrix , for dimensional identity matrix, and the stopping conditions of the above three algorithms are that the infinite norm of the gradient is less than 10 -6 .

[0027] Figure 2 The AGV trajectory tracking effect based on the nonlinear optimization algorithm proposed in the present invention is shown, wherein the reference trajectory is a circular trajectory as shown in the figure. It can be seen that after adopting the nonlinear optimization algorithm of the present invention, the AGV can achieve fast and high-precision trajectory tracking on the horizontal and vertical coordinates.

[0028] Figure 3 The calculation time of the nonlinear optimization algorithm of the present invention is compared with that of the quasi-Newton algorithm and the sequential quadratic programming algorithm. For the fairness of the comparison, 8 experiments were conducted independently on the above three algorithms. It can be seen that the nonlinear optimization algorithm of the present invention has significant calculation efficiency in calculation time, and its calculation time is greatly shortened, which is much lower than the existing method.

[0029] 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 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; Step 4: Calculate the Hessian matrix of the control input at all sampling moments; 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: (1)； in, , , Respectively represent The horizontal coordinate, vertical coordinate and heading angle of AGV in the plane coordinate system at each sampling moment; , , Respectively represent The horizontal coordinate, vertical coordinate and heading angle of AGV in the plane coordinate system at each sampling moment; represents the sampling interval; , Respectively represent The linear velocity and angular velocity of AGV in the plane coordinate system at each 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 for: (2)； in, , Respectively represent The position and reference position of AGV in the plane coordinate system at each sampling moment; , , Respectively The reference abscissa, reference ordinate, and reference heading angle of the AGV in the plane coordinate system at each sampling moment; , Respectively represent The control input and reference input of AGV in the plane coordinate system at each sampling moment; , Respectively The reference linear velocity and reference angular velocity of the AGV in the plane coordinate system at each sampling moment; , They represent respectively the given dimension-compatible semi-positive definite weight matrix and positive definite weight matrix; 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 3 is as follows: Step 3.1: Record all sampling times The control input is ,in, , Respectively The control input of AGV in the plane coordinate system at each sampling moment The transposition of The control input of AGV in the plane coordinate system at each sampling moment The transpose of ; given all sampling moments Control input The initial iteration value of 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: (3)； in, represents the first Hamiltonian function; Indicates Sampling time The corresponding co-variables; , , Respectively Sampling time , , The corresponding co-variables; Indicates The position and posture of AGV in the plane coordinate system at each sampling moment; for The transpose of Step 3.3.

2. Co-state variables Satisfies the following equation: (4)； (5)； in, Indicates Sampling time The corresponding co-variables; Indicates Sampling time The corresponding co-state variable, at this time, the co-state variable is the given terminal value; Indicates The position and posture of AGV in the plane coordinate system at each sampling moment; 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 quadratic loss function The formula for the gradient of the control input at all sampling moments is as follows: (6)； in, Represents the quadratic loss function The gradient of the control input at all sampling moments has 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; is a two-dimensional row vector containing two elements, which are the gradient and The element at index position.

5. The AGV trajectory tracking control method based on the nonlinear optimization algorithm according to claim 4 is characterized in that: The specific process of step 4 is as follows: 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: (7)； in, is the second Hamiltonian function; Indicates Sampling time The corresponding backward iterative adjoint state; Indicates Sampling time The corresponding forward iteration adjoint state; for The transpose of for The transpose of The element at the 0th index position is recorded as ; , , They are the initial position of AGV in the plane coordinate system at the 0th sampling time, the control input of AGV in the plane coordinate system at the 0th sampling time, and the co-state variable at the 1st sampling time; Step 4.1.2, the forward iterative adjoint state and the backward iterative adjoint state satisfy the following equations respectively: (8)； (9)； (10)； in, Indicates Sampling time The corresponding forward iteration adjoint state; Indicates Sampling time The corresponding backward iterative adjoint state; Indicates the 0th sampling time The corresponding forward iteration adjoint state, at this time the forward iteration adjoint state is the given initial value; Indicates Sampling time The corresponding backward iterative adjoint state, at this time, the backward iterative adjoint state is the given terminal value; Step 4.1.3: Execute formula (8) and formula (9) respectively to obtain all sampling times The forward iterative adjoint state and the backward iterative adjoint state of ; 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: (11)； in, Represents the quadratic loss function Regarding the first row elements of the Hessian matrix of the control input at all sampling moments, 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 first row of the Hessian matrix. and The element at index position; 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 the second Hamiltonian function in formula (7) to , each time you calculate, , , The value in the brackets is incremented by one, that is, the next element is taken in turn; the calculation method for the even-numbered row elements of the Hessian matrix is ​​the same as the calculation method for the first row elements. When calculating, take The element at the first index position is recorded as , changing the second Hamiltonian for , each time you calculate, , , The values ​​in the brackets are incremented by one, that is, one element is taken at a time; the elements of all rows constitute the Hessian matrix.

6. The AGV trajectory tracking control method based on the nonlinear optimization algorithm according to claim 5 is characterized in that: The specific process of step 5 is as follows: Step 5.1: Given the 0th iteration Initial value of and the algorithm stopping threshold ; 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 ; (12)； (13)； (14)； in, , The numbers representing the number of iterations of the outer loop and inner loop respectively; , Respectively represent sequence Outer loop iteration The iteration value of , , Respectively represent the 0th and 1st iterations of the inner loop sequence Updates , Represent the quadratic loss function respectively exist 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 the , with the final As the optimal solution of the nonlinear optimization algorithm.

Images