Sudden change trajectory tracking control method based on data driving and learning model predictive control

Abstract

The invention relates to a sudden change trajectory tracking control method based on data driving and learning model predictive control, and belongs to the field of trajectory tracking control. In order to solve the problem that an existing trajectory tracking method cannot track a sudden change trajectory exceeding the tracking capability of an unmanned system, a learning model prediction control algorithm is firstly established, and an optimal target trajectory is re-planned by weighing the tracking capability of the unmanned system and the deviation degree of the sudden change trajectory. Afterwards, for the problems that a traditional planning method is poor in optimization capability and learning model predictive control calculation complexity is high, a long short-term memory network (LSTM) is designed to learn a deep mapping relation between an optimal target trajectory and an auxiliary trajectory calculated by a traditional planning algorithm, and a robust model predictive control algorithm is combined, so that the optimal target trajectory is obtained. And the unmanned system is controlled in real time to track the target track with the abrupt change point.

Description

Technical Field

[0001] The present invention belongs to the field of trajectory tracking control, and in particular relates to a mutation trajectory tracking control method based on data-driven and learning model predictive control. Background Art

[0002] Unmanned systems refer to unmanned equipment that can autonomously complete designated tasks, including drones, unmanned vehicles and unmanned ships. With the continuous development of artificial intelligence and advanced control methods, the intelligence level of unmanned systems has been significantly improved, and they are often used to perform various complex tasks, such as environmental monitoring, scientific exploration, medical rescue and agricultural spraying. Trajectory tracking refers to the process of an unmanned system accurately tracking a pre-designed target trajectory. It is one of the basic problems in the field of control and has a wide range of applications in logistics sorting, navigation and sea patrol.

[0003] The constraint processing capability and rolling optimization mechanism of model predictive control (MPC) give it a significant advantage in solving the trajectory tracking problem of unmanned systems. However, the traditional trajectory tracking method requires that the change speed of the target trajectory is within the trackable range of the unmanned system. When the external error of the tracking target surges or the target trajectory changes suddenly, the unmanned system will lose the target. Therefore, how to minimize the deviation between the planned trajectory and the sudden change trajectory while ensuring the traceability of the planned trajectory is a major problem in realizing sudden change trajectory tracking control.

[0004] Based on data-driven strategies, iterative learning model predictive control (LMPC) can use the tracking error from previous task iterations to improve the tracking performance of the controller at the current moment, but the computational complexity of the controller also increases, and it cannot be applied to unmanned systems that require high-frequency control. The computational complexity of traditional planning algorithms is low and can output target trajectories in real time, but the optimization ability of the algorithm is poor, which reduces the tracking ability of unmanned systems. To this end, the mapping relationship between the optimal trajectory and the auxiliary trajectory output by the traditional planning algorithm is learned using the Long Short-Term Memory (LSTM) network, which improves the distributed control effect while avoiding the rolling optimization problem of solving LMPC, and controls the unmanned system to track the mutation trajectory in real time. Summary of the invention

[0005] The technical problems to be solved by the present invention are:

[0006] In order to solve the problem that the existing unmanned system trajectory tracking control algorithm cannot track the sudden change trajectory, the present invention provides a sudden change trajectory tracking control method based on data-driven and learning model predictive control.

[0007] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0008] A sudden change trajectory tracking control method based on data-driven and learning model predictive control, characterized in that the steps are as follows:

[0009] Step 1: Establish the kinematic and dynamic model of the unmanned ship system;

[0010] Step 2: Randomly sample target trajectories with mutation points, design a learning model predictive control algorithm based on the speed upper bound and mutation trajectory of the unmanned ship system, and calculate the optimal target trajectory that can be tracked by the unmanned system;

[0011] Step 3: Using the auxiliary trajectory and auxiliary control law calculated by B-spline as input and the deviation between the optimal control law calculated by the learning model predictive control algorithm and the auxiliary control law as output, a training set for the sequence-to-sequence problem is constructed;

[0012] Step 4: Establish an encoder-decoder model based on the LSTM network, input the training set into the encoder-decoder model based on the LSTM network to learn the deep mapping relationship between the optimal target trajectory and the auxiliary trajectory;

[0013] Step 5: After detecting the mutation point of the target trajectory, extract the sub-target trajectory centered on the mutation point, and output the approximately optimal target trajectory based on the encoder-decoder model of the LSTM network;

[0014] Step 6: Design a robust model predictive control algorithm to control the unmanned system to track the approximately optimal target trajectory.

[0015] A further technical solution of the present invention: Step 2 comprises:

[0016] Step 2.1: Design a trajectory planning problem in the long prediction time domain and calculate a feasible trajectory connecting the starting point and the end point of the mutation trajectory;

[0017] Step 2.2: Establish an iterative optimization model for the mutation trajectory, select the optimal point in the feasible trajectory as the sub-terminal condition of the learning MPC, and design the terminal relaxation condition;

[0018] Step 2.3: Iteratively solve the optimization problem of step 2.2 until the sub-terminal condition is the end point of the feasible trajectory, and use the output trajectory as the new feasible trajectory to start the next iteration. After the number of iterations reaches the set threshold, the output trajectory is used as the optimal target trajectory.

[0019] A further technical solution of the present invention: In step 2.1, a trajectory planning problem in a long prediction time domain is designed, and the optimization problem is as follows:

[0020]

[0021] st:η g (k+n|k)=η r (k+n|k)-η T

[0022] η r (k|k)=η r (k),η r (k+N T |k)=η T

[0023] η r (k+n+1|k)=f r (η r (k+n|k),u r (k+n|k))

[0024]

[0025]

[0026] Among them, η S and η T are the starting and ending points of the mutation trajectory, f r (·) is the update equation of the virtual tracking target, u r (k+n|k) is the control input of the virtual tracking target, including the linear velocity and angular velocity of the virtual target, η r (k+n|k) is the tracking target state predicted based on the control input, Q T and R T is the weight matrix; and is the state and control input constraints of the virtual tracking target, N T It is a sufficiently long prediction time domain to ensure that the virtual tracking target is trackable for the unmanned system and that it can reach the end point of the mutation trajectory within the prediction time domain.

[0027] A further technical solution of the present invention: In step 2.2, an iterative optimization model for mutation trajectories is established:

[0028]

[0029] Among them, Q L , R L and P L is the weight matrix, N L is the prediction horizon for learning MPC; is the control input at time k+n calculated at time k during the i-th iteration, is the virtual tracking target state predicted based on the control input; and are the states and control input constraints of the virtual tracking target; is the terminal set updated with the iterative process; η d (k+n|k) is the state of the mutation trajectory, is the state error of the virtual tracking target distance mutation trajectory, and its calculation equation is:

[0030]

[0031]

[0032] Among them, θ r (k+n|k) is the heading angle of the unmanned system, Φ L (θ r (k+n|k)) is the rotation matrix, which transforms the state error from the world coordinate system to the virtual tracking target coordinate system; is the control input error, representing the deviation between the planned virtual target speed and the mutation trajectory speed, A v and B v It is the update coefficient matrix of virtual tracking target tracking error and control input error.

[0033] A further technical solution of the present invention: In step 2.2, the optimal point is selected in the feasible trajectory as the sub-terminal condition, and the terminal relaxation condition is designed, specifically:

[0034] Terminal Collection Defined as: Indicates that its center is the iteration set S in the i-1th iteration process i-1 The terminal point with the smallest internal cost; the feasible trajectory of the connection mutation start and end points recorded in the previous iteration process is stored in the iteration set. T i-1 is the length of the optimal target trajectory during the i-1 iteration; S 0 is the initial feasible trajectory obtained by solving the long-term trajectory planning problem in step 2.1; when selecting the optimal terminal point, first select the iteration set S i-1 The points inside are taken as the terminal points of the iterative optimization problem, and the iterative optimization problem is solved. The terminal point with the smallest cost value and ensuring that the iterative optimization problem has a solution is selected as the optimal terminal point at the current moment, recorded as is the virtual tracking target at the terminal time relative to the terminal point When the terminal tracking error is within the terminal set, it indicates that the virtual tracking target is close to the optimal terminal point in the iteration set. That is, a point on the feasible trajectory in the iteration set. At this time, according to the control input in the iteration set, the virtual tracking target can be controlled to extend the feasible trajectory to the end point η of the mutation trajectory T ;when When , the iteration process i ends, and the optimal tracking trajectory at this time is recorded in the set S i Inside.

[0035] A further technical solution of the present invention: Step 3 is specifically:

[0036] The random sampling length is T a The mutation trajectory is constructed to connect the starting point η of the mutation trajectory S and the end point η T The B-spline curve is used as the auxiliary trajectory. The matrix expression of the B-spline curve of order m and n+1 control points is as follows:

[0037] q c (g(t))=G(t)M m (i)C(i)

[0038] Where G(t)=[1,g(t),g 2 (t),,g m-1 (t)], Δ is the set time interval, t i is the node closest to t, and t∈[t i ,t i+1 ); c i is the i-th control point, where the first control point is defined as the starting point η S The position coordinates of the mutation trajectory are set as η d (k B ), the second control point is The position coordinates of the mutation point in the mutation trajectory and the midpoint of the target trajectory coordinates at the previous moment, and the last control point is set to η T , and then use the uniform interpolation method to add control points to n+1; M m (i) is the basis matrix of the B-spline matrix, M1(i) = 1, when m>1,

[0039]

[0040] in, Then, based on the B-spline curve, the auxiliary trajectory is extracted And the corresponding control strategy

[0041] Afterwards, the learning control law for learning MPC calculation is defined as To assist the trajectory And the corresponding control strategy As input, to learn the control law Deviation from the auxiliary control law Construct a training set for the output.

[0042] A further technical solution of the present invention: In step 4, an encoder-decoder model based on an LSTM network is established to learn the deep mapping relationship between the optimal planning trajectory and the auxiliary trajectory, specifically:

[0043]

[0044] set up is an LSTM model whose output is the approximate deviation between the optimal target trajectory control strategy and the auxiliary trajectory control strategy By minimizing the mean square error (MSE) loss function, the relationship between the auxiliary control strategy and the optimal control strategy is found; finally, the approximate deviation is used to obtain the approximate optimal target trajectory.

[0045] A further technical solution of the present invention: In step 6, a robust model predictive control algorithm is designed to control the unmanned system to track the approximately optimal target trajectory. The specific optimization problem is as follows:

[0046]

[0047] st:η(k|k)=η(k)

[0048]

[0049]

[0050] η(k+n+1|k)=f(η(k+n|k),u(k+n|k))

[0051]

[0052] Where η(k) is the state of the unmanned system at time k, u(k+n|k) is the control input at time k+n calculated at time k, and η(k+n|k) is the system state predicted based on the control input; It is based on the LSTM model to calculate the approximate optimal target trajectory; and is the state and control input constraint of the unmanned system, Q, R and P are weight matrices, N R is the prediction horizon of robust MPC, η e (k+n|k) and u e (k+n|k) is the tracking error and control input error of the unmanned system, and its calculation equation is:

[0053]

[0054] η e (k+n+1|k)=A e η e (k+n|k)+B e u e (k+n|k)

[0055] Among them, θ(k+n|k) is the heading angle of the unmanned system, Φ R (θ(k+n|k)) is the rotation matrix; A v and B v It is the update coefficient matrix of the unmanned system tracking error and control input error.

[0056] A computer system, characterized in that it comprises: one or more processors, and a computer-readable storage medium for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned method.

[0057] A computer-readable storage medium, characterized in that it stores computer-executable instructions, which are used to implement the above method when executed.

[0058] The beneficial effects of the present invention are:

[0059] The present invention provides a sudden change trajectory tracking control method based on data-driven and learning model predictive control. First, a learning model predictive control algorithm and a relaxed terminal condition are designed, and a deep mapping relationship between the optimal target trajectory and the auxiliary trajectory of the LSTM model is established. Finally, a robust model predictive control algorithm is designed to control the motion of the unmanned system. The present invention outputs a near-optimal target trajectory in real time through the LSTM model and the learning MPC algorithm, while minimizing the deviation between the planned trajectory and the sudden change trajectory as much as possible, ensuring that the planned trajectory is traceable for the unmanned system. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0061] Figure 1 is a flow chart of an embodiment of the present invention.

[0062] Figure 2 Schematic diagram of the unmanned ship trajectory tracking model.

[0063] Figure 3 Schematic diagram of the unmanned ship's sudden trajectory tracking.

[0064] Figure 4 Schematic diagram of the unmanned vehicle trajectory tracking model.

[0065] Figure 5 Schematic diagram of the UAV trajectory tracking model. DETAILED DESCRIPTION

[0066] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0067] Embodiment 1:

[0068] The present invention provides an unmanned ship sudden trajectory tracking control method based on data drive and learning model predictive control, comprising the following steps:

[0069] Step 1: Establish the kinematic and dynamic model of the unmanned ship:

[0070]

[0071] Among them, η s =[x s ,y s ,θ s ] T is the state of the unmanned ship, including the coordinates of the unmanned ship [x s ,y s ] T and heading angle θ s , v x,s ,v y,s ,w s is the linear velocity and angular velocity of the unmanned ship, and its update equation is as follows:

[0072]

[0073] Among them, m1, m2, m3, d1, d2, d3 are the internal parameters of the unmanned ship related to the additional mass and damping, τ x and τ w They represent the forward thrust and yaw moment of the unmanned ship respectively. Figure 2 As shown, the kinematic model of the bow of the unmanned ship is established:

[0074]

[0075] Among them, η h,s is the bow coordinate of the unmanned ship, r sis the distance between the midpoint of the motors on the left and right sides of the unmanned ship and the bow. When the unmanned ship tracks a target track, η d =[x d ,y d ,θ d ] T , its tracking error As shown below:

[0076]

[0077]

[0078] Among them, v x,d ,v y,d is the linear velocity of the tracking target, θ ds =θ d -θ s , the discretized tracking error is

[0079]

[0080] in,

[0081] Step 2: Randomly sample the target trajectory with mutation points, design a learning model predictive control algorithm based on the upper bound of the unmanned ship's speed, and calculate the optimal target trajectory that can be tracked by the unmanned ship;

[0082] Step 2.1: Design a trajectory planning problem in the long prediction time domain and calculate a feasible trajectory connecting the start and end points of the mutation trajectory.

[0083] First, the kinematic equations of the virtual tracking target are defined.

[0084]

[0085] Among them, u r (k)=[v x,r (k),v y,r (k),w r (k)] T is the speed and angular velocity of the virtual tracking target. After that, the long-term optimization problem is as follows:

[0086]

[0087] Among them, η S and η T are the starting and ending points of the mutation trajectory, f r (·) is the update equation of the virtual tracking target, u r (k+n|k) is the control input of the virtual tracking target, including the linear velocity and angular velocity of the virtual target, η r(k+n|k) is the tracking target state predicted based on the control input, Q T and R T is the weight matrix. and is the state and control input constraints of the virtual tracking target, N T It is a sufficiently long prediction time domain to ensure that the virtual tracking target is trackable for the unmanned system and that it can reach the end point of the mutation trajectory within the prediction time domain.

[0088] Step 2.2: Establish an optimization model for the mutation trajectory, select the optimal point in the feasible trajectory as the sub-terminal condition, design the terminal relaxation condition to reduce the computational complexity of the optimization problem, and design the time-varying control input constraint in combination with the tracking capability of the unmanned system.

[0089]

[0090] Among them, Q L and R L is the weight matrix, N L is the prediction horizon for learning MPC. is the control input at time k+n calculated at time k during the i-th iteration, is the virtual tracking target state predicted based on the control input. and are the states and control input constraints of the virtual tracking target. is the terminal set updated during the iteration process. d (k+n|k) is the state of the mutation trajectory, is the state error of the virtual tracking target distance mutation trajectory, and its calculation equation is:

[0091]

[0092] Among them, θ r (k+n|k) is the heading angle of the unmanned system, and the rotation matrix Φ L (θ r (k+n|k)) is

[0093]

[0094] is the terminal set updated with the iteration process, indicating that its center is the iteration set S in the i-1th iteration process i-1 The terminal point with the smallest internal cost. The feasible trajectory of the connection mutation start and end points recorded in the previous iteration process is stored in the iteration set. T i-1 is the length of the optimal target trajectory during the i-1 iteration. 0is the initial feasible trajectory obtained by solving the long-term trajectory planning problem in step 2.1. When selecting the optimal terminal point, first select the iteration set S i-1 The points inside are taken as the terminal points of the iterative optimization problem, and the iterative optimization problem is solved. The terminal point with the smallest cost value and ensuring that the iterative optimization problem has a solution is selected as the optimal terminal point at the current moment, recorded as is the virtual tracking target at the terminal time relative to the terminal point When the terminal tracking error is within the terminal set, it indicates that the virtual tracking target is close to the optimal terminal point in the iteration set. That is, a point on the feasible trajectory in the iteration set. At this time, according to the control input in the iteration set, the virtual tracking target can be controlled to extend the feasible trajectory to the end point η of the mutation trajectory T .when When , the iteration process i ends, and the optimal tracking trajectory at this time is recorded in the set S i Inside.

[0095] Step 2.3: Iteratively solve the optimization model of step 2.2 until the sub-terminal condition is the end point of the mutation trajectory, that is, The iteration process i ends, and the output trajectory is used as the new feasible trajectory. After that, the next iteration is started, and when the number of iterations reaches the set threshold, the output trajectory is used as the optimal target trajectory.

[0096] Step 3: With the auxiliary trajectory and auxiliary control law calculated by B-spline as input and the deviation between the optimal control law calculated by the learning model predictive control and the auxiliary control law as output, a training set for the sequence-to-sequence problem (Seq2Seq) is constructed.

[0097] The random sampling length is T a The mutation trajectory is constructed to connect the starting point η of the mutation trajectory S and the end point η T The B-spline curve is used as the auxiliary trajectory. The matrix expression of the B-spline curve of order m and n+1 control points is as follows:

[0098] q c (g(t))=G(t)M m (i)Q c (i) (12)

[0099] Where G(t)=[1,g(t),g 2 (t),,g m-1 (t)], Δ is the set time interval, t i is the node closest to t, and t∈[t i ,t i+1 ). Qc (i) = [q i-m+1 ,q i-m+2 ,,q i ] T ,q i is the i-th control point, where the first control point is defined as the starting point η S The position coordinates of the target trajectory are set as η d (k B ), the second control point is The position coordinates of the mutation point in the mutation trajectory and the midpoint of the target trajectory coordinates at the previous moment, and the last control point is set to η T Then, the uniform interpolation method is used to expand the control points to n+1. m (i) is the basis matrix of the B-spline matrix, M1(i) = 1, when m>1,

[0100]

[0101] in, Then, based on the B-spline curve, the auxiliary trajectory is extracted And the corresponding control strategy Then, based on the B-spline curve, the auxiliary trajectory is extracted And the corresponding control strategy

[0102] Afterwards, the learning control law for learning MPC calculation is defined as To assist the trajectory And the corresponding control strategy As input, to learn the control law Deviation from the auxiliary control law Construct a training set for the output.

[0103] Step 4: Establish an encoder-decoder model based on the LSTM network to learn the deep mapping relationship between the optimal planning trajectory and the auxiliary trajectory to solve the Seq2Seq problem in step 3;

[0104]

[0105] set up is an LSTM model whose output is the approximate deviation between the optimal target trajectory control strategy and the auxiliary trajectory control strategy The relationship between the auxiliary control strategy and the optimal control strategy is found by minimizing the mean square error (MSE) loss function.

[0106] Step 5: After detecting the mutation point of the target trajectory, extract the sub-target trajectory centered on the mutation point, and output the approximate optimal target trajectory based on the LSTM model in step 4

[0107] Step 6: Design a robust model predictive control algorithm to control the unmanned ship to track the approximately optimal target trajectory.

[0108]

[0109] Among them, η s (k) is the state of the unmanned ship at time k, u s (k+n|k) is the control input at time k calculated at time k+n, η s (k+n|k) is the system state predicted based on the control input. η e,s (k+n|k) and u e,s (k+n|k) is the tracking error and control input error of the unmanned ship. and is the tracking error constraint and control input constraint of the unmanned ship, Q s ,R s and P s is the weight matrix, N s is the prediction horizon of robust MPC. Finally, the optimal control input is calculated And use Control the movement of the unmanned ship.

[0110] Embodiment 2:

[0111] A method for tracking sudden changes in trajectory of an unmanned vehicle based on data-driven and learning model predictive control, comprising the following steps:

[0112] Step 1: Establish the kinematic model of the unmanned vehicle:

[0113]

[0114] Among them, η a =[x a ,y a ,θ a ] T Including the coordinates and heading angle of the unmanned vehicle, v a and w a is the linear velocity and angular velocity of the unmanned vehicle. Next, define the coordinates of the head of the unmanned vehicle as η h,a , and its update equation is as follows:

[0115]

[0116] Among them, r a is the distance between the center of the unmanned vehicle and the head of the unmanned vehicle. When the unmanned vehicle tracks the target trajectory, the tracking error and its update equation are defined as:

[0117]

[0118] Among them, θ ra =θ r -θ a , the discretized tracking error is:

[0119]

[0120] in, Afterwards, consistent with the learning model predictive control algorithm designed according to the speed upper bound of the unmanned vehicle in real-time example 1, a trajectory planning scheme based on learning model predictive control and LSTM model is also constructed. After the auxiliary trajectory is constructed using the B-spline curve, the approximate optimal target trajectory is calculated according to the LSTM model.

[0121] Step 6: Design a robust model predictive control algorithm to control the unmanned vehicle to track the approximately optimal target trajectory.

[0122]

[0123] Among them, η a (k) is the state of the autonomous vehicle at time k, u a (k+n|k) is the control input at time k calculated at time k+n, η a (k+n|k) is the system state predicted based on the control input. η e,a (k+n|k) and u e,a (k+n|k) is the tracking error and control input error of the unmanned vehicle. and is the tracking error constraint and control input constraint of the unmanned vehicle, Q a ,R a and P S is the weight matrix, N a is the prediction horizon of robust MPC. Finally, the optimal control input is calculated And use Control the movement of the unmanned vehicle.

[0124] Embodiment 3:

[0125] A method for tracking and controlling a sudden change trajectory of a UAV based on data-driven and learning model predictive control comprises the following steps:

[0126] Step 1: Build the kinematic and dynamic model of the drone

[0127]

[0128] in, and q =[φ q ,θq ,ψ q ] T are the coordinates and angles in the world coordinate system, and is the linear velocity and angular velocity of the drone, F is the thrust of the propeller, τ is the torque generated by the propeller, I is the moment of inertia of the drone, and g is the gravity constant E3 = [0,0,1] T , m is the mass of the drone, T Q It is the rotation matrix from the drone coordinate system to the world coordinate system.

[0129]

[0130] Among them, c θ = cosθ, s θ = sinθ, mapping equation [w c ] × for

[0131]

[0132] Define a virtual drone coordinate system, whose coordinate system center coincides with the drone center, the coordinate system's yaw angle is equal to the drone's yaw angle, but the coordinate system's pitch angle and roll angle are 0. In this coordinate system, the relative distance and relative angle between the drone and the tracking target are

[0133]

[0134] In the virtual UAV coordinate system, the velocity error between the UAV and the time-varying target trajectory is Then the update equations for position error and velocity error are:

[0135]

[0136] Among them, h q =[h x,q ,h y,q ,h z,q ] T is the acceleration in the virtual coordinate system, and the tracking error is defined as The control input is u q =[h x,q ,h y,q ,h z,q ,ψ q ] T , The discretized UAV tracking error update equation is:

[0137]

[0138] Where δ is the control interval, according to the control input uq , the expected angle and speed of the drone at the next control moment are:

[0139]

[0140]

[0141] Subsequently, the drone’s internal controller is used to calculate the corresponding thrust F and torque τ to control the drone to reach the desired angle and speed.

[0142] Step 2: Randomly sample target trajectories with mutation points, design a learning model predictive control algorithm based on the upper bound of the UAV's speed, and calculate the optimal target trajectory that can be tracked by the UAV;

[0143] Afterwards, consistent with the trajectory planning scheme based on learning model predictive control and LSTM model in real-time example 1, after constructing the auxiliary trajectory using the B-spline curve, the approximately optimal target trajectory is calculated according to the LSTM model.

[0144] Step 6: Design a robust model predictive control algorithm to control the UAV to track the approximately optimal target trajectory.

[0145]

[0146] in, is the state of the drone at time k, u q (k+n|k) is the control input at time k calculated at time k+n. is the predicted system state based on the control inputs. and u q (k+n|k) is the tracking error and control input error of the UAV. and is the tracking error constraint and control input constraint of the UAV, Ω q is the terminal constraint, Q q ,R q and P q is the weight matrix, N q is the prediction horizon of robust MPC. Finally, the optimal control input is calculated And according to And the internal controller controls the drone movement.

[0147] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should be included in the protection scope of the present invention.

Claims

1. A sudden change trajectory tracking control method based on data-driven and learning model predictive control, characterized in that: Here are the steps: Step 1: Establish the kinematic and dynamic model of the unmanned ship system; Step 2: Randomly sample target trajectories with mutation points, design a learning model predictive control algorithm based on the speed upper bound and mutation trajectory of the unmanned ship system, and calculate the optimal target trajectory that can be tracked by the unmanned system; Step 3: Using the auxiliary trajectory and auxiliary control law calculated by B-spline as input and the deviation between the optimal control law calculated by the learning model predictive control algorithm and the auxiliary control law as output, a training set for the sequence-to-sequence problem is constructed; Step 4: Establish an encoder-decoder model based on the LSTM network, input the training set into the encoder-decoder model based on the LSTM network to learn the deep mapping relationship between the optimal target trajectory and the auxiliary trajectory; Step 5: After detecting the mutation point of the target trajectory, extract the sub-target trajectory centered on the mutation point, and output the approximately optimal target trajectory based on the encoder-decoder model of the LSTM network; Step 6: Design a robust model predictive control algorithm to control the unmanned system to track the approximately optimal target trajectory.

2. According to claim 1, a sudden change trajectory tracking control method based on data-driven and learning model predictive control is characterized in that: The step 2 comprises: Step 2.1: Design a trajectory planning problem in the long prediction time domain and calculate a feasible trajectory connecting the starting point and the end point of the mutation trajectory; Step 2.2: Establish an iterative optimization model for the mutation trajectory, select the optimal point in the feasible trajectory as the sub-terminal condition of the learning MPC, and design the terminal relaxation condition; Step 2.3: Iteratively solve the optimization problem of step 2.2 until the sub-terminal condition is the end point of the feasible trajectory, and use the output trajectory as the new feasible trajectory to start the next iteration. After the number of iterations reaches the set threshold, the output trajectory is used as the optimal target trajectory.

3. According to claim 2, a sudden change trajectory tracking control method based on data-driven and learning model predictive control is characterized in that: In step 2.1, the trajectory planning problem in the long prediction time domain is designed, and the optimization problem is as follows: st:η g (k+n|k)=η r (k+n|k)-η T or r (k|k)=η r (k),n r (k+N T |k)=η T or r (k+n+1|k)=f r (or r (k+n|k),u r (k+n|k)) Among them, η S and η T are the starting and ending points of the mutation trajectory, f r (·) is the update equation of the virtual tracking target, u r (k+n|k) is the control input of the virtual tracking target, including the linear velocity and angular velocity of the virtual target, η r (k+n|k) is the tracking target state predicted based on the control input, Q T and R T is the weight matrix; and is the state and control input constraints of the virtual tracking target, N T It is a sufficiently long prediction time domain to ensure that the virtual tracking target is trackable for the unmanned system and that it can reach the end point of the mutation trajectory within the prediction time domain.

4. According to claim 3, a sudden change trajectory tracking control method based on data-driven and learning model predictive control is characterized in that: In step 2.2, an iterative optimization model for mutation trajectories is established: Among them, Q L , R L and P L is the weight matrix, N L is the prediction horizon for learning MPC; is the control input at time k+n calculated at time k during the i-th iteration, is the virtual tracking target state predicted based on the control input; and are the states and control input constraints of the virtual tracking target; is the terminal set updated with the iterative process; η d (k+n|k) is the state of the mutation trajectory, is the state error of the virtual tracking target distance mutation trajectory, and its calculation equation is: Among them, θ r (k+n|k) is the heading angle of the unmanned system, Φ L (θ r (k+n|k)) is the rotation matrix, which transforms the state error from the world coordinate system to the virtual tracking target coordinate system; is the control input error, representing the deviation between the planned virtual target speed and the mutation trajectory speed, A v and B v It is the update coefficient matrix of virtual tracking target tracking error and control input error.

5. According to claim 4, a sudden change trajectory tracking control method based on data-driven and learning model predictive control is characterized in that: In step 2.2, the optimal point is selected in the feasible trajectory as the sub-terminal condition, and the terminal relaxation condition is designed, specifically: Terminal Collection Defined as: Indicates that its center is the iteration set S in the i-1th iteration process i-1 The terminal point with the smallest internal cost; the feasible trajectory of the connection mutation start and end points recorded in the previous iteration process is stored in the iteration set. T i-1 is the length of the optimal target trajectory during the i-1 iteration; S 0 is the initial feasible trajectory obtained by solving the long-term trajectory planning problem in step 2.1; When selecting the optimal terminal point, first select the iteration set S i-1 The points inside are taken as the terminal points of the iterative optimization problem, and the iterative optimization problem is solved. The terminal point with the smallest cost value and ensuring that the iterative optimization problem has a solution is selected as the optimal terminal point at the current moment, recorded as is the virtual tracking target at the terminal time relative to the terminal point When the terminal tracking error is within the terminal set, it indicates that the virtual tracking target is close to the optimal terminal point in the iteration set. That is, a point on the feasible trajectory in the iteration set. At this time, according to the control input in the iteration set, the virtual tracking target can be controlled to extend the feasible trajectory to the end point η of the mutation trajectory T ;when When , the iteration process i ends, and the optimal tracking trajectory at this time is recorded in the set S i Inside.

6. According to claim 5, a sudden change trajectory tracking control method based on data-driven and learning model predictive control is characterized in that: The step 3 is specifically as follows: The random sampling length is T a The mutation trajectory is constructed to connect the starting point η of the mutation trajectory S and the end point η T The B-spline curve is used as the auxiliary trajectory. The matrix expression of the B-spline curve of order m and n+1 control points is as follows: q c (g(t))=G(t)M m (i)C(i) Where G(t)=[1,g(t),g 2 (t),…,g m-1 (t)], Δ is the set time interval, t i is the node closest to t, and t∈[t i ,t i+1 ); C(i) = [c i-m+1 ,c i-m+2 ,…,c i ] T , c i is the i-th control point, where the first control point is defined as the starting point η S The position coordinates of the mutation trajectory are set as η d (k B ), the second control point is The position coordinates of the mutation point in the mutation trajectory and the midpoint of the target trajectory coordinates at the previous moment, and the last control point is set to η T , and then use the uniform interpolation method to add control points to n+1; M m (i) is the basis matrix of the B-spline matrix, M1(i) = 1, when m>1, in, Then, based on the B-spline curve, the auxiliary trajectory is extracted And the corresponding control strategy Afterwards, the learning control law for learning MPC calculation is defined as To assist the trajectory And the corresponding control strategy As input, to learn the control law Deviation from the auxiliary control law Construct a training set for the output.

7. According to claim 6, a sudden change trajectory tracking control method based on data-driven and learning model predictive control is characterized in that: In step 4, an encoder-decoder model based on the LSTM network is established to learn the deep mapping relationship between the optimal planning trajectory and the auxiliary trajectory. Specifically, set up is an LSTM model whose output is the approximate deviation between the optimal target trajectory control strategy and the auxiliary trajectory control strategy By minimizing the mean square error (MSE) loss function, the relationship between the auxiliary control strategy and the optimal control strategy is found; finally, the approximate deviation is used to obtain the approximate optimal target trajectory.

8. According to claim 1, a sudden change trajectory tracking control method based on data-driven and learning model predictive control is characterized in that: In step 6, a robust model predictive control algorithm is designed to control the unmanned system to track the approximately optimal target trajectory. The specific optimization problem is as follows: st:η(k|k)=η(k) η(k+n+1|k)=f(η(k+n|k),u(k+n|k)) Where η(k) is the state of the unmanned system at time k, u(k+n|k) is the control input at time k+n calculated at time k, and η(k+n|k) is the system state predicted based on the control input; It is based on the LSTM model to calculate the approximate optimal target trajectory; and is the state and control input constraint of the unmanned system, Q, R and P are weight matrices, N R is the prediction horizon of robust MPC, η e (k+n|k) and u e (k+n|k) is the tracking error and control input error of the unmanned system, and its calculation equation is: or e (k+n+1|k)=A e or e (k+n|k)+B e you e (k+n|k) Among them, θ(k+n|k) is the heading angle of the unmanned system, Φ R (θ(k+n|k)) is the rotation matrix; A v and B v It is the update coefficient matrix of the unmanned system tracking error and control input error.

9. A computer system, characterized in that include: One or more processors, and a computer-readable storage medium for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors are enabled to implement the method of claim 1.

10. A computer-readable storage medium, characterized in that Computer executable instructions are stored, and when the instructions are executed, they are used to implement the method of claim 1.

Images