A trajectory tracking control method for unmanned ship predicted by a preset performance convex line model

Abstract

The present invention discloses a method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex online model, which belongs to the field of trajectory tracking technology and is used for unmanned ship trajectory tracking control, including modeling an unmanned ship, discretizing a continuous time-varying nonlinear model using the preceding Euler method, using a linear time-varying model predictive control method to process nonlinear state space equations; optimizing the objective function, converting the predictive control into quadratic programming, establishing the constraints required for the predictive control, establishing a preset performance function discrete at a specified time through predefined time control, thereby rewriting the constraints; solving the objective function, and obtaining the latest position under the trajectory of the unmanned ship. The present invention discretizes the continuous nonlinear model through the preceding Euler method to reduce the complexity of the calculation, and uses the predefined time control method to construct a preset performance function discrete at a specified time, so as to meet the accuracy requirements of trajectory tracking under the preset performance of the specified time of the tracking error.

Description

Technical Field

[0001] The invention discloses a method for predicting an unmanned ship trajectory tracking control using a preset performance convex line model, and belongs to the technical field of trajectory tracking. Background Art

[0002] The trajectory tracking control of unmanned ships requires them to accurately reach the predetermined trajectory point within a specific time. At the same time, according to different application requirements, the control algorithm may introduce multiple constraints such as speed and acceleration. Compared with path tracking control, trajectory tracking not only needs to follow the spatial trajectory, but also needs to meet time constraints. These constraints significantly increase the complexity and computational complexity of the controller, and therefore put forward higher requirements for the design of the control algorithm. Given that the control of unmanned ships mainly relies on embedded systems or real-time control platforms, ensuring the real-time performance of the algorithm has become a key requirement in system design, especially when sailing in complex environments. The response speed and accuracy of the control system directly affect the performance of the unmanned ship.

[0003] Model predictive control has the advantages of constraint processing, state prediction, multi-input multi-output control and rolling optimization, and is one of the most popular control methods in the current control field. However, the traditional linear predictive control MPC algorithm currently has problems such as high computational complexity, strong dependence on model accuracy, difficulty in achieving efficient real-time control, and decreased prediction accuracy in dealing with nonlinear models. Therefore, solving these problems is particularly important for unmanned ships to resolve uncertainties in complex environments and achieve efficient control and mission efficiency. Summary of the invention

[0004] The purpose of the present invention is to provide a method for predicting the trajectory tracking control of an unmanned ship based on a preset performance convex line model, so as to solve the problems in the prior art that the traditional linear predictive control MPC algorithm has high computational complexity, strong dependence on model accuracy, difficulty in achieving efficient real-time control, and decreased prediction accuracy in processing nonlinear models.

[0005] A method for predicting trajectory tracking control of an unmanned ship using a preset performance convex line model, comprising:

[0006] S1. Model the unmanned ship and use the Euler method to discretize the continuous time-varying nonlinear model;

[0007] S2, using linear time-varying model predictive control method to deal with nonlinear state space equations;

[0008] S3, optimize the objective function and transform the predictive control into quadratic programming;

[0009] S4. Establish the constraints required for predictive control, establish a discrete preset performance function for a specified time through predefined time control, and rewrite the constraints;

[0010] S5. Solve the objective function to obtain the latest position of the unmanned ship trajectory.

[0011] Modeling of unmanned ships includes:

[0012] ;

[0013] ;

[0014] In the formula, is the horizontal coordinate of the unmanned ship, is the vertical coordinate of the unmanned ship, is the heading angle of the unmanned ship, is the longitudinal speed of the unmanned ship, is the lateral speed of the unmanned ship, is the angular velocity of the unmanned ship, is the nonlinear state space equation of the unmanned ship, is the interference matrix, is the state transition matrix, is the control matrix, is the unmanned ship control input, is the target state vector of the unmanned ship.

[0015] Modeling of unmanned ships includes:

[0016] ;

[0017] ;

[0018] ;

[0019] ;

[0020] ;

[0021] ;

[0022] ;

[0023] In the formula, is the coordinate transformation matrix, is the added mass matrix, is the centripetal and Coriolis force coefficient matrix, is the inertial damping matrix, is the quality of the ship, is the moment of inertia, , , , , is the ship damping coefficient, , , For the additional mass, , , is the ship's additional mass coefficient, It is the distance between the center of gravity of the ship and the following coordinate system.

[0024] Discretization processing includes:

[0025] ;

[0026] ;

[0027] In the formula, yes In the process , is the discrete time step, yes In the process .

[0028] S2 includes, rewrite :

[0029] ;

[0030] In the formula, , , , They are In the process , , , , is the identity matrix.

[0031] The objective functions include:

[0032] ;

[0033] ;

[0034] ;

[0035] ;

[0036] ;

[0037] ;

[0038] ;

[0039] In the formula, is the objective function, Indicates the control input of the unmanned ship Take down The minimum value of express Iterations, and are two weight matrices, express Second processing, is the current state vector of the unmanned ship And the target state vector of the unmanned ship The difference, for Second , represents the constraints of the objective function, , , are three known matrices, is the coefficient matrix of the inequality constraints, is the upper limit of the inequality constraint, For the control input minimum value, Enter the maximum value for the control.

[0040] After converting the predictive control into quadratic programming, the objective function includes:

[0041] ;

[0042] ;

[0043] ;

[0044] ;

[0045] In the formula, , , , , , , , , , , , , is a variable that can be sought, yes in vector form.

[0046] In the objective function:

[0047] ;

[0048] ;

[0049] ;

[0050] ;

[0051] ;

[0052] ;

[0053] ;

[0054] ;

[0055] ;

[0056] ;

[0057] ;

[0058] ;

[0059] ;

[0060] ;

[0061] In the formula, is the matrix form of the current state vector.

[0062] The objective function is converted into quadratic programming, and the quadratic programming is solved by combining the quadprog function in the MATLAB toolbox with the convex optimization toolkit CVX.

[0063] Update the objective function in conjunction with a predefined time control:

[0064] ;

[0065] ;

[0066] ;

[0067] ;

[0068] ;

[0069] ;

[0070] ;

[0071] ;

[0072] ;

[0073] ;

[0074] In the formula, , , , , is a variable that can be sought, yes In the process , is the three-dimensional identity matrix, is a very small positive number, is a discrete preset performance function.

[0075] Compared with the prior art, the present invention has the following beneficial effects: the present invention discretizes the continuous nonlinear model through the previous Euler method to reduce the complexity of calculation, adopts the predefined time control method to construct a discrete preset performance function of a specified time, and meets the accuracy requirement of trajectory tracking under the preset performance of the specified time of the tracking error. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 is a schematic diagram of error performance constraints;

[0077] Figure 2 It is the overall idea of ​​the simulation program algorithm;

[0078] Figure 3 This is the tracking effect diagram after adding the preset performance function to the different starting point prototype track tracking simulation;

[0079] Figure 4 This is the tracking effect diagram after one thousand times of traditional MPC tracking in the different starting point prototype trajectory tracking simulation;

[0080] Figure 5 It is the error constraint diagram in the trajectory tracking simulation of the prototype with different starting points;

[0081] Figure 6 It is the horizontal axis speed diagram in the simulation of different starting point prototype track tracking;

[0082] Figure 7 It is the vertical axis velocity diagram in the simulation of the prototype track tracking with different starting points;

[0083] Figure 8 This is the deviation angle diagram in the different starting point prototype track tracking simulation;

[0084] Fig. 9 It is the thrust diagram of the left and right propellers in the trajectory tracking simulation of the prototype with different starting points;

[0085] Fig.10This is the tracking effect diagram of the circular trajectory simulation with different starting points after adding the preset performance function;

[0086] Fig.11 This is the tracking effect diagram after one thousand times of traditional MPC tracking in the simulation of circular trajectory tracking with different starting points;

[0087] Fig.12 It is the error constraint diagram in the simulation of circular trajectory tracking with different starting points;

[0088] Fig.13 It is the thrust and yaw angle diagram in the simulation of circular trajectory tracking with different starting points;

[0089] Fig.14 It is the horizontal axis velocity diagram in the simulation of circular trajectory tracking with different starting points;

[0090] Fig.15 It is the vertical axis velocity diagram in the simulation of circular trajectory tracking with different starting points;

[0091] Fig.16 This is the yaw angle diagram in the simulation of tracking circular trajectories with different starting points. DETAILED DESCRIPTION

[0092] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0093] A method for predicting trajectory tracking control of an unmanned ship using a preset performance convex line model, comprising:

[0094] S1. Model the unmanned ship and use the Euler method to discretize the continuous time-varying nonlinear model;

[0095] S2, using linear time-varying model predictive control method to deal with nonlinear state space equations;

[0096] S3, optimize the objective function and transform the predictive control into quadratic programming;

[0097] S4. Establish the constraints required for predictive control, establish a discrete preset performance function for a specified time through predefined time control, and rewrite the constraints;

[0098] S5. Solve the objective function to obtain the latest position of the unmanned ship trajectory.

[0099] Modeling of unmanned ships includes:

[0100] ;

[0101] ;

[0102] In the formula, is the horizontal coordinate of the unmanned ship, is the vertical coordinate of the unmanned ship, is the heading angle of the unmanned ship, is the longitudinal speed of the unmanned ship, is the lateral speed of the unmanned ship, is the angular velocity of the unmanned ship, is the nonlinear state space equation of the unmanned ship, is the interference matrix, is the state transition matrix, is the control matrix, is the unmanned ship control input, is the target state vector of the unmanned ship.

[0103] Modeling of unmanned ships includes:

[0104] ;

[0105] ;

[0106] ;

[0107] ;

[0108] ;

[0109] ;

[0110] ;

[0111] In the formula, is the coordinate transformation matrix, is the added mass matrix, is the centripetal and Coriolis force coefficient matrix, is the inertial damping matrix, is the quality of the ship, is the moment of inertia, , , , , is the ship damping coefficient, , , For the additional mass, , , is the ship's additional mass coefficient, It is the distance between the center of gravity of the ship and the following coordinate system.

[0112] Discretization processing includes:

[0113] ;

[0114] ;

[0115] In the formula, yes In the process , is the discrete time step, yes In the process .

[0116] S2 includes, rewrite :

[0117] ;

[0118] In the formula, , , , They are In the process , , , , is the identity matrix.

[0119] The objective functions include:

[0120] ;

[0121] ;

[0122] ;

[0123] ;

[0124] ;

[0125] ;

[0126] ;

[0127] In the formula, is the objective function, Indicates the control input of the unmanned ship Take down The minimum value of express Iterations, and are two weight matrices, express Second processing, is the current state vector of the unmanned ship And the target state vector of the unmanned ship The difference, for Second , represents the constraints of the objective function, , , are three known matrices, is the coefficient matrix of the inequality constraints, is the upper limit of the inequality constraint, For the control input minimum value, Enter the maximum value for the control.

[0128] After converting the predictive control into quadratic programming, the objective function includes:

[0129] ;

[0130] ;

[0131] ;

[0132] ;

[0133] In the formula, , , , , , , , , , , , , is a variable that can be sought, yes in vector form.

[0134] In the objective function:

[0135] ;

[0136] ;

[0137] ;

[0138] ;

[0139] ;

[0140] ;

[0141] ;

[0142] ;

[0143] ;

[0144] ;

[0145] ;

[0146] ;

[0147] ;

[0148] ;

[0149] In the formula, is the matrix form of the current state vector.

[0150] The objective function is converted into quadratic programming, and the quadratic programming is solved by combining the quadprog function in the MATLAB toolbox with the convex optimization toolkit CVX.

[0151] Update the objective function in conjunction with a predefined time control:

[0152] ;

[0153] ;

[0154] ;

[0155] ;

[0156] ;

[0157] ;

[0158] ;

[0159] ;

[0160] ;

[0161] ;

[0162] In the formula, , , , , is a variable that can be sought, yes In the process , is the three-dimensional identity matrix, is a very small positive number, is a discrete preset performance function.

[0163] The MPC algorithm needs to solve an optimization problem at each sampling moment, which usually involves complex matrix operations and possible nonlinear programming solutions. As the dimension of the system increases or the prediction time domain grows, the computational burden will increase significantly. At the same time, because the linear time-varying model predictive control is used to process the nonlinear state space equation, in order to be able to perform operations and to reduce the amount of calculation, the Euler method is used to discretize the continuous time-varying nonlinear model. The original complex operation is converted into an incremental form for calculation. The calculation process only requires simple algebraic operations, and does not require complex iterations or matrix inversion operations, so the calculation efficiency is high. The discretization formula of the Euler method is simple and intuitive, which is easy to implement on embedded systems or real-time control platforms. It is suitable for application scenarios such as unmanned ships that have high real-time requirements. The Euler method converts the continuous time model into a discrete state space form, which can be directly used in the MPC prediction model, simplifying the MPC modeling process. The Euler method may have insufficient accuracy at a large step size. In fact, with this treatment, the error will not be too large in a shorter time domain, and a motion trend that is consistent with the expectation can be obtained in a not too long time domain, and it has been used in unmanned vehicles.

[0164] When obtaining the objective function, the present invention The matrix form of Bring in And the state expression after rolling iteration N steps is:

[0165] ;

[0166] ;

[0167] ;

[0168] ;

[0169] ;

[0170] ;

[0171] In the formula, , , is a searchable variable.

[0172] Convert to matrix form:

[0173] ;

[0174] In short:

[0175] ;

[0176] In the tracking issue, there are , the matrix is:

[0177] ;

[0178] In the formula, yes In the process , yes The matrix form of .

[0179] When the predictive control is converted into quadratic programming, the derivation process is:

[0180] ;

[0181] In the formula, and are all diagonal matrices.

[0182] The state constraint is , which is equivalent to:

[0183] .

[0184] In the formula, It's time.

[0185] In order to control the time, it is necessary to Predefined time control is performed to achieve a preset performance of the tracking error at a specified time. Predefined time control (PTC) is a control strategy designed to ensure that the system stabilizes and reaches the target state or trajectory within a predetermined time (rather than an infinite time or asymptotic time). Predefined time control requires the system to stabilize and reach the target state or trajectory within a known and fixed time. Inner convergence. Therefore, the preset performance function of the specified time discrete is established:

[0186] ;

[0187] ;

[0188] ;

[0189] ;

[0190] In the formula, It is Second , is the initial error performance boundary, is the steady-state error performance boundary, is the judgment threshold. From the above formula, we can know that when hour, When , the above formula can be obtained = It is the initial error performance boundary at the current zero time, and when the preset convergence time is reached hour, The above formula will give , so that we can ensure The curve must converge to Then, By converting the two positive and negative curves as boundaries, we can get the error performance constraints as follows:

[0191] .

[0192] In the formula, is the error value, such as Figure 1 , the function can only be used in and The middle area changes. Similarly, model predictive control (MPC) can constrain the state quantity, which is an important advantage of MPC. Then this range can be used as an error that the entire system can allow to deviate from, and a target state vector is added to the left and right sides respectively. , then we can get a range curve:

[0193] ;

[0194] In the formula, is the current real-time status of the system, then the entire system can be restricted to operate within this range, so that the system can be gradually Limitations and constraints are imposed to ensure that the system does not change too drastically during operation, resulting in the unmanned ship's performance failing to meet the required control requirements for the change.

[0195] Convert it into the unmanned ship state constraint matrix, then we have:

[0196] ;

[0197] According to the iteration relationship, we get:

[0198] ;

[0199] In the formula, yes In matrix form, since , so we have:

[0200] ;

[0201] Since the quadratic programming problem can only solve equality constraints with boundaries, the above formula can be further tightened to the following inequality constraints with boundaries:

[0202] .

[0203] Use MATLAB software as the test platform and write a simulation program. The overall idea of ​​the simulation program algorithm is as follows Figure 2 As shown, it includes a given ideal trajectory of an unmanned boat, a discrete linear control unmanned boat operation and dynamics model, constructing an MPC cost function, constructing a convex MPC solution algorithm, calculating the optimal control sequence in each cycle, taking the first control quantity of the control sequence as the actual control input quantity of the unmanned boat, updating the state of the unmanned boat, judging whether it has reached the vicinity of the end point, and returning to the discrete linear control unmanned boat operation and dynamics model if it has reached the end point, and terminating the process if it has reached the end point, and also initializing a discrete preset time preset performance function, establishing a linear inequality constraint for achieving the preset performance and adding it to the calculation of the optimal control sequence in each cycle. The present invention performs different starting point prototype track tracking simulation and different starting point tracking circular track simulation.

[0204] In the simulation process, in order to fully verify the rapidity and tracking progress of the predefined time tracking of the proposed algorithm, the different starting point prototype track tracking simulation is adopted. The initial state is (That is, the initial state is The axis coordinate is -8, The axis coordinates, direction angle, initial speed, angular velocity and direction angular velocity are all zero) to track the starting point of the low-speed curve. The radius is A circle with its center at Set the control amount that the ship can provide to .

[0205] use The ship parameters are simulated and tested. The tracking times are 1000 times and the results are as follows: Figure 3 This is the tracking effect diagram after adding the preset performance function. Figure 4 This is the tracking effect diagram of the traditional MPC after tracking for one thousand times. It can be seen that the algorithm proposed in this paper has a faster tracking speed and better tracking effect than the traditional MPC with preset performance constraints. Figure 5 As shown, the horizontal axis speed is Figure 6 As shown, the vertical axis speed is Figure 7 As shown, the yaw angle is Figure 8 As shown, the thrust of the left and right propellers is Fig. 9 shown.

[0206] In the simulation process, in order to fully verify the rapidity and tracking progress of the predefined time tracking of the proposed algorithm, the strategy of tracking the circular trajectory with different starting points is adopted. The initial state is (That is, the initial state is The axis coordinate is 5, The axis coordinates, direction angle, initial velocity, angular velocity and direction angular velocity are all zero) to track the same curve as the previous experiment to verify the ability of the algorithm to cope with complex convergence processes. The number of tracking times is 1000 and the results are as follows: Fig.10 This is the tracking effect diagram after adding the preset performance function. Fig.11 This is the tracking effect diagram after the traditional MPC tracking is performed one thousand times. Because the predefined time control requires the target track to be followed in a short time, after quickly following the track, the ship's running direction is toward the negative direction of the x-axis, and the target track is toward the negative direction of the y-axis. The enlarged area in the figure has completed the U-turn operation, and the tracking effect is stable after the operation. Fig.12 The black line in the middle is the set error constraint, the red line is the allowed error limit, and the blue line is the difference between the actual runtime state and the target state. After the actual model is put into place, the trajectory tracking is completed within the set constraint time and the trajectory tracking error is controlled within the set error limit.

[0207] Thrust and yaw angle Fig.13 Since the starting point is far away from the target trajectory, the unmanned ship will use the maximum thrust to catch up with the target trajectory. When it is about to catch up, it will slow down and gradually match the target curve speed. Thus, a stable state is achieved. Fig.14 As shown, the vertical axis speed is Fig.15 As shown, the yaw angle is Fig.16 shown.

[0208] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, a person skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features may be replaced by equivalents, and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model, characterized in that: include: S1. Model the unmanned ship and use the Euler method to discretize the continuous time-varying nonlinear model; S2, using linear time-varying model predictive control method to deal with nonlinear state space equations; S3, optimize the objective function and transform the predictive control into quadratic programming; S4. Establish the constraints required for predictive control, establish a discrete preset performance function for a specified time through predefined time control, and rewrite the constraints; S5, solving the objective function to obtain the latest position of the unmanned ship under the trajectory; Update the objective function in conjunction with a predefined time control: ； ； ； ； ； ； ； ； ； ； In the formula, is the objective function, U is the control input, , , , , , , , , , , , , , , , , is a variable that can be sought, yes In vector form, is the nonlinear state space equation of the unmanned ship, yes The target state vector of the unmanned ship in the second processing is: yes In the process , is the interference matrix, yes In the process , is the three-dimensional identity matrix, is a very small positive number, is a discrete preset performance function.

2. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 1 is characterized in that: Modeling of unmanned ships includes: ； ； In the formula, is the horizontal coordinate of the unmanned ship, is the vertical coordinate of the unmanned ship, is the heading angle of the unmanned ship, is the longitudinal speed of the unmanned ship, is the lateral speed of the unmanned ship, is the angular velocity of the unmanned ship, is the state transition matrix, is the control matrix, is the unmanned ship control input, is the target state vector of the unmanned ship.

3. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 2 is characterized in that: Modeling of unmanned ships includes: ； ； ； ； ； ； ； In the formula, is the coordinate transformation matrix, is the added mass matrix, is the centripetal and Coriolis force coefficient matrix, is the inertial damping matrix, is the quality of the ship, is the moment of inertia, , , , , is the ship damping coefficient, , , For the additional mass, , , is the ship's additional mass coefficient, It is the distance between the center of gravity of the ship and the following coordinate system.

4. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 3 is characterized in that: Discretization processing includes: ； ； In the formula, yes In the process , is the discrete time step.

5. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 4 is characterized in that: S2 includes, rewrite : ； In the formula, , , They are In the process , , , is the identity matrix.

6. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 5 is characterized in that: The objective function includes: ； ； ； ； ； ； ； In the formula, Indicates the control input of the unmanned ship Take down The minimum value of express Iterations, and are two weight matrices, express Second processing, is the current state vector of the unmanned ship And the target state vector of the unmanned ship The difference, for Second , represents the constraints of the objective function, , , are three known matrices, is the coefficient matrix of the inequality constraints, is the upper limit of the inequality constraint, For the control input minimum value, Enter the maximum value for the control.

7. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 6 is characterized in that: After converting the predictive control into quadratic programming, the objective function includes: ； ； ； 。 8. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 7 is characterized in that: In the objective function: ； ； ； ； ； ； ； ； ； ； ； ； ； ； In the formula, is the matrix form of the current state vector, is a searchable variable.

9. The method for predicting the trajectory tracking control of an unmanned ship using a preset performance convex line model according to claim 8, characterized in that: The objective function is converted into quadratic programming, and the quadratic programming is solved by combining the quadprog function in the MATLAB toolbox with the convex optimization toolkit CVX.

Images