Model predictive control-based aircraft and ship formation path tracking control method
Through a model-based prediction control method, combined with virtual structure formation method and nonlinear perturbation observer, the problem of path tracking of unmanned ship-drone formations is solved under environmental interference, and stable and efficient formation control is achieved, improving task feasibility.
Patent Information
- Application Number
- CN202411122259.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-15
- Publication Date
- 2024-11-26
AI Technical Summary
The existing unmanned ship-drone formations are difficult to control stably under environmental interference conditions and cannot effectively track paths, limiting their feasibility in search and rescue and maritime patrol tasks.
Using the model prediction control method, the aircraft and ship formation path tracking control method is adopted, and by constructing a mathematical model of motion of unmanned ships and drones that consider external interference, designing virtual structure formation method and nonlinear perturbation observer, combining the model prediction control algorithm, the construction of The law of aircraft and ship formation path tracking control, realizes formation path tracking control of unmanned ships and drones.
In the presence of external disturbances, stable tracking control of the formation paths of unmanned ships and drones is realized, which improves the feasibility and superiority of the system and overcomes the unstable impact of environmental interference on formation control.
Smart Images

Figure CN119024881A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of unmanned ship and unmanned aerial vehicle motion control and modeling, and in particular to a path tracking control method of a motor-ship formation based on model predictive control. Background Art
[0002] With the development of intelligent and unmanned transportation, various unmanned vehicles have made great progress in combination with emerging technologies such as unmanned driving and artificial intelligence. In the shipping industry, unmanned ships occupy a very important position, which can not only save construction costs, but also greatly reduce maritime accidents caused by human factors.
[0003] However, unmanned ships also have some shortcomings that cannot be ignored. Compared with drones, unmanned ships have smaller visual distance and monitoring range, so they have difficulties in performing tasks such as search and rescue or maritime patrols. In addition, the existing unmanned ship-drone formations cannot fully consider the unstable effects on the formation control system under the existence of environmental interference conditions. Therefore, there is an urgent need for a control method that can complete the path tracking of the unmanned ship-drone formation under the existence of environmental interference conditions, so as to combine the advantages of drones and unmanned ships to form a three-dimensional system to complete the task of tracking a predetermined route. Summary of the invention
[0004] The present invention provides a path tracking control method for a motor-vessel formation based on model predictive control to overcome the above technical problems.
[0005] In order to achieve the above object, the technical solution of the present invention is:
[0006] A path tracking control method for a motor-vessel formation based on model predictive control comprises the following steps:
[0007] S1: Construct a mathematical model of the motion of an unmanned ship with three degrees of freedom and a mathematical model of the motion of a drone with six degrees of freedom considering external interference;
[0008] S2: Based on the virtual structure formation method, the reference trajectory of the unmanned ship and the unmanned aerial vehicle is designed with time as the parameter;
[0009] S3: Construct a nonlinear disturbance observer based on the mathematical model of the unmanned ship motion to observe the unmanned ship interference and compensate it at the control input end;
[0010] S4: Based on the model predictive control algorithm, the path tracking control law of the motor-ship formation is constructed according to the reference trajectories of the unmanned ship and the drone, so as to realize the control of the path tracking of the unmanned ship and the drone formation according to the path tracking control law of the motor-ship formation.
[0011] Furthermore, the method for constructing the reference trajectory of the unmanned ship and the unmanned aerial vehicle in S2 includes the following steps:
[0012] S21: Define the virtual reference path point of the unmanned ship, and its expression is
[0013]
[0014] Where: x r (k) represents the x-coordinate value of the virtual reference point at the k-th sampling moment; y r (k) represents the y coordinate value of the virtual reference point at the kth sampling moment; T s Indicates the sampling time;
[0015] S22: Assuming that the reference motion trajectory of the unmanned ship coincides with the virtual reference path point, the reference trajectory of the unmanned ship is
[0016]
[0017] Where: x ref (k) represents the x-coordinate reference value of the unmanned ship at the kth sampling moment; y ref (k) represents the y coordinate reference value of the unmanned ship at the kth sampling moment; ψ ref (k) represents the reference value of the heading angle of the unmanned ship at the kth sampling moment; ψ r (k) represents the heading angle reference value of the virtual reference point at the kth sampling moment;
[0018] S23: Based on the virtual structure formation method, the reference trajectory of the UAV is designed as
[0019]
[0020] Where: x bref (k) represents the x-coordinate reference value of the UAV at the kth sampling moment; y bref (k) represents the y coordinate reference value of the UAV at the kth sampling moment; z bref (k) represents the z-coordinate reference value of the UAV at the kth sampling moment; d L Indicates the relative distance between the drone and the virtual reference point.
[0021] Further, the S3 comprises the following steps:
[0022] S31: Simplify and rewrite the mathematical model of unmanned ship motion as
[0023]
[0024] Where: υ represents the velocity vector of the unmanned ship and υ=[uvr] T; M represents the inertia matrix; d represents the total disturbance caused by external interference and model uncertainty; τ represents the control input of the unmanned ship; f(υ) represents the nonlinear term in the mathematical model of the unmanned ship's motion; u represents the forward speed of the unmanned ship; v represents the drift speed of the unmanned ship; r represents the bow angular velocity of the unmanned ship;
[0025] S32: Define the auxiliary variable ξ of the nonlinear disturbance observer, which is expressed as
[0026]
[0027] Where: represents the observed value of d; p(υ) represents the nonlinear function to be designed;
[0028] S33: Based on the rewritten mathematical model of unmanned ship motion and the auxiliary variable ξ, a nonlinear disturbance observer is constructed, and its expression is:
[0029]
[0030] Where: represents the first-order derivative of ξ; L(υ) represents the gain matrix of the nonlinear disturbance observer, and
[0031] represents the first derivative of υ.
[0032] Further, the S4 comprises the following steps:
[0033] S41: Based on the model predictive control algorithm, the state space equations of the unmanned ship and the unmanned aerial vehicle are discretized according to the unmanned ship motion mathematical model and the six-degree-of-freedom unmanned aerial vehicle motion mathematical model. The expression is:
[0034]
[0035] Where: T s represents the sampling time; X(k+1|k) represents the state of the unmanned ship at time k+1 predicted at time k; X(k) represents the state of the unmanned ship at time k; represents the first-order differential of X(k); X b (k+1|k) represents the state vector of the drone at time k+1 predicted at time k; Represents X b The first-order differential of (k+1|k); X b (k) represents the state vector of the drone at time k; Represents X b (k) first-order derivative; Represents X b The second-order derivative of (k);
[0036] S42: According to the discretized state space equations of the unmanned ship and the unmanned aerial vehicle, the predicted state vector and the predicted control input vector at each sampling moment in the set prediction interval and control interval are obtained, and the expression is:
[0037]
[0038] Where: Represents the state vector of the unmanned ship predicted at each sampling moment within the prediction interval; Represents the control input vector of the unmanned ship predicted at each sampling moment within the control interval; Indicates prediction k+N at time k p The state vector value of the unmanned ship at the moment; represents the control input vector of the unmanned ship at each sampling moment within the predicted control interval; Represents the state vector of the predicted drone at each sampling moment within the prediction interval; Represents the control input vector of the UAV predicted at each sampling moment within the control interval; Indicates prediction k+N at time k p The drone state vector value at the moment; represents the control input vector of the UAV at each sampling moment within the predicted control interval;
[0039] S43: Based on the reference trajectory of the UAV and the reference trajectory of the unmanned ship, the state error vector and the control input change of the unmanned ship and the UAV are obtained according to the predicted state vector and the predicted control input vector. The expression is:
[0040]
[0041]
[0042] Where: represents the state error vector of the unmanned ship at time k+i; represents the state error vector of the unmanned ship at time k+i; X ref (k+i) represents the reference state vector of the unmanned ship at time k+i; ΔU(k+j) represents the control input change vector of the unmanned ship at time k+j; represents the predictive control input vector of the unmanned ship at time k+j; represents the state error vector of the drone at time k+i; represents the state error vector of the drone at time k+i; X bref (k+i) represents the reference state vector of the UAV at time k+i; represents the predicted control input vector of the UAV at time k+j; ΔU b (k+j) represents the control input change vector of the UAV at time k+j;
[0043] S44: Construct a cost function for the state error vector of the unmanned ship and the unmanned aerial vehicle and the control input change, and its expression is
[0044]
[0045] Where: J cost represents the cost function designed according to the error vector of the unmanned ship's motion and its control input variation; Q represents the weight matrix of the unmanned ship's state error vector related terms in the cost function; R represents the weight matrix of the unmanned ship's predicted control input variation vector related terms in the cost function; J bcost represents the cost function designed according to the error vector of the UAV motion and the change of its control input; Q b Represents the weight matrix of the state error vector related items of the UAV in the cost function; R b The weight matrix of the related items of the predictive control input change vector of the UAV in the cost function is shown;
[0046] S45: constructing a target model for the unmanned ship and unmanned aircraft formation path tracking optimization problem according to the cost function, and solving the target model to obtain a path tracking control law for the unmanned ship and unmanned aircraft formation;
[0047] The target model of the unmanned ship-drone formation path tracking optimization problem is constructed, and its expression is
[0048] argmin{J cost (ΔU(k))}
[0049] st
[0050] X(k+1)=f(X(k),U(k))
[0051] U min ≤U(k)≤U max
[0052] Y min ≤Y(k)≤Y max
[0053] argmin{J bcost (ΔU(k))}
[0054] st
[0055] X b (k+1)=f(X b (k),U b (k))
[0056] U bmin ≤U b (k)≤U bmax
[0057] Y bmin ≤Y b (k)≤Y bmax
[0058] Where: X(k) represents the state vector of the unmanned ship at time k, i.e. the current time; U(k) represents the control input vector of the unmanned ship at time k, i.e. the current time; Y(k) represents the output vector of the unmanned ship at time k, i.e. the current time; X(k+1) represents the state vector of the unmanned ship at time k+1; U min represents the minimum value of the control input constraint of the unmanned ship; U max represents the maximum value of the control input constraint of the unmanned ship; Y min represents the minimum value of the state output constraint of the unmanned ship; Y max represents the maximum value of the state output constraint of the unmanned ship; X b (k) represents the state vector of the UAV at time k, i.e. the current time; U b (k) represents the control input vector of the UAV at time k, i.e. the current time; Y(k) represents the output vector of the UAV at time k, i.e. the current time; X b (k+1) represents the state vector of the UAV at time k+1; U bmin represents the minimum value of the UAV control input constraint; U bmax represents the maximum value of the UAV control input constraint; Y bmin Represents the minimum value of the drone state output constraint; Y bmax Indicates the maximum value of the drone state output constraint;
[0059] S46: Implement the path tracking control of the unmanned ship and unmanned aerial vehicle formation according to the path tracking control law of the motor-ship formation.
[0060] Furthermore, the S1 comprises the following steps:
[0061] S101: Establish a mathematical model of the motion of an unmanned ship with three degrees of freedom;
[0062] S102: Establish a mathematical model of UAV motion with six degrees of freedom.
[0063] Furthermore, the expression of the mathematical model of the three-degree-of-freedom unmanned ship motion established in S101 is:
[0064]
[0065] Where: x represents the horizontal coordinate of the position of the unmanned ship; y represents the vertical coordinate of the position of the unmanned ship; ψ represents the heading angle of the unmanned ship; u represents the forward speed of the unmanned ship; v represents the horizontal drift speed of the unmanned ship; r represents the bow angular velocity of the unmanned ship; f u , f v , f r represents the nonlinear term in the unmanned ship model; τ u represents the thrust input of the unmanned ship; τ r represents the bow turning moment input of the unmanned ship; τ wu , τ wv , τ wr Indicates the external environmental disturbance acting on the ship; Δf u , Δf v , Δf r represents the parameter uncertainty caused by unmodeled dynamics; m 11 , m 22 , m 33 Represents the parameters of the ship's inertia matrix; d 11 , d 22 , d 33 , d 23 , d 23 Parameters representing the ship damping matrix;
[0066] The expression of the mathematical model of the six-degree-of-freedom UAV motion established in S102 is:
[0067]
[0068] Where: x b Indicates the horizontal coordinate of the drone’s position; y b Indicates the vertical coordinate of the drone’s position; z b Indicates the vertical coordinate of the drone; Represents the roll angle of the quadrotor drone; θ b represents the pitch angle of the quadrotor drone; ψ b represents the heading angle of the quadrotor drone; U1 represents the vertical lift of the quadrotor drone; U2 represents the rolling moment of the quadrotor drone; U3 represents the pitch moment of the quadrotor drone; U4 represents the yaw moment of the quadrotor drone; I x Represents the moment of inertia of the quadrotor drone on the x-axis; I y Represents the moment of inertia of the quadrotor drone on the y-axis; I z represents the moment of inertia of the quadrotor drone on the y-axis; J rrepresents the rotor inertia of the quadrotor drone; Ω represents the motor speed; K1 represents the influence coefficient of air resistance on the horizontal coordinate of the quadrotor drone; K2 represents the influence coefficient of air resistance on the vertical coordinate of the quadrotor drone; K3 represents the influence coefficient of air resistance on the height coordinate of the quadrotor drone; K4 represents the influence coefficient of air resistance on the roll angle of the quadrotor drone; K5 represents the influence coefficient of air resistance on the pitch angle of the quadrotor drone; K6 represents the influence coefficient of air resistance on the yaw angle of the quadrotor drone; g represents the acceleration of gravity; l represents the wheelbase of the quadrotor drone.
[0069] Beneficial effect: The present invention provides a path tracking control method for a motor-ship formation based on model predictive control. Firstly, based on the virtual structure formation method, according to the mathematical model of the unmanned ship motion and the mathematical model of the unmanned aircraft carrier, and with time as the design parameter, the reference paths of the unmanned ship and the unmanned aircraft carrier used to construct the motor-ship formation configuration are designed to ensure the team configuration of the structural formation. Then, in view of the uncertainty of the unmanned ship model and the external environmental interference, a nonlinear disturbance observer is designed to compensate for the control input. Finally, based on the model predictive control algorithm, the motor-ship formation path tracking control law is constructed according to the reference path trajectory of the unmanned ship and the unmanned aircraft carrier to realize the path tracking control of the unmanned ship and the unmanned aircraft carrier, so that it has good feasibility and superiority in the presence of external disturbances. BRIEF DESCRIPTION OF THE DRAWINGS
[0070] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0071] Figure 1 It is a flow chart of the path tracking control method of a motor-vessel formation based on model predictive control of the present invention;
[0072] Figure 2 Schematic diagram of the unmanned ship drone in different coordinate systems in this embodiment;
[0073] Figure 3 Schematic diagram of a path tracking control system for a motor-vessel formation based on model predictive control in this embodiment;
[0074] Figure 4 This is a schematic diagram of the model prediction principle in this embodiment;
[0075] Figure 5 This is a plane diagram of the motor-ship formation in this embodiment;
[0076] Figure 6 This is a schematic diagram of the space of the aircraft-ship formation in this embodiment;
[0077] Figure 7 Graph showing simulation results of prediction intervals of different lengths in this embodiment;
[0078] Figure 8 This is a three-dimensional simulation diagram of the unmanned ship and unmanned aerial vehicle formation sailing in this embodiment;
[0079] Fig. 9 This is a two-dimensional simulation diagram of the unmanned ship and unmanned aerial vehicle formation navigation in this embodiment;
[0080] Fig.10 This is a simulation diagram of the longitudinal relative positions of the unmanned ship and unmanned aerial vehicle formation in this embodiment;
[0081] Fig.11 This is a simulation diagram of the lateral relative positions of the unmanned ship and unmanned aerial vehicle formation in this embodiment;
[0082] Fig.12 This is a simulation diagram of the navigation error of the unmanned ship and unmanned aerial vehicle formation in this embodiment:
[0083] Fig.13 This is a simulation diagram of the change of the heading angle of the unmanned ship in this embodiment;
[0084] Fig.14 This is a diagram of the simulation result of the UAV flight altitude in this embodiment;
[0085] Fig.15 This is a simulation diagram of the change of the attitude angle of the UAV in this embodiment;
[0086] Fig.16 is the unmanned ship control input curve in this embodiment;
[0087] Fig.17 This is the UAV control input curve in this embodiment;
[0088] Fig.18 Graph showing the observation results of the interference observer in this embodiment;
[0089] Fig.19 Graph showing the observed error of the disturbance observer in this embodiment. DETAILED DESCRIPTION
[0090] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, 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.
[0091] This embodiment provides a path tracking control method for a motor-vessel formation based on model predictive control. Figure 1 As shown, the following steps are included:
[0092] S1: Construct a mathematical model of the motion of an unmanned ship with three degrees of freedom and a mathematical model of the motion of a drone with six degrees of freedom considering external interference;
[0093] Specifically, the unmanned ship and the unmanned aerial vehicle can communicate with each other through a wireless self-organizing network, and realize heterogeneous collaborative path tracking with the unmanned aerial vehicle according to the control motion trajectory of the unmanned ship; specifically, an attached coordinate system is established with the center of gravity of the unmanned ship and the unmanned aerial vehicle as the origin, and a kinematic model of the unmanned ship and the unmanned aerial vehicle is established based on the attached coordinate system, and the position coordinates of the kinematic models of the unmanned ship and the unmanned aerial vehicle are converted from space coordinates to an inertial coordinate system (OX O Y O Z O )like Figure 2 As shown; and the establishment of the hull appendage coordinate system and the inertial coordinate system and the transformation process of the space coordinates are all existing well-known technical means, which are not the invention of this application and will not be repeated here;
[0094] In a specific embodiment, the S1 comprises the following steps:
[0095] S101: Establish a mathematical model of the motion of an unmanned ship with three degrees of freedom, which is expressed as follows:
[0096]
[0097] Where: x represents the horizontal coordinate of the position of the unmanned ship; y represents the vertical coordinate of the position of the unmanned ship; ψ represents the heading angle of the unmanned ship; u represents the forward speed of the unmanned ship; v represents the horizontal drift speed of the unmanned ship; r represents the bow angular velocity of the unmanned ship; f u , f v , f r represents the nonlinear term in the unmanned ship model; τ u represents the thrust input of the unmanned ship; τ r represents the bow turning moment input of the unmanned ship; τ wu , τ wv , τ wr Indicates the external environmental disturbance acting on the ship; Δf u , Δf v , Δf r represents the parameter uncertainty caused by unmodeled dynamics; m 11 , m 22 , m 33 Represents the parameters of the ship's inertia matrix; d 11 , d 22 , d 33 , d23 , d 23 Parameters representing the ship damping matrix;
[0098] S102: Establish a mathematical model of the six-degree-of-freedom UAV motion, which is expressed as follows:
[0099]
[0100] Where: x b Indicates the horizontal coordinate of the drone’s position; y b Indicates the vertical coordinate of the drone’s position; z b Indicates the vertical coordinate of the drone; Represents the roll angle of the quadrotor drone; θ b represents the pitch angle of the quadrotor drone; ψ b represents the heading angle of the quadrotor drone; U1 represents the vertical lift of the quadrotor drone; U2 represents the rolling moment of the quadrotor drone; U3 represents the pitch moment of the quadrotor drone; U4 represents the yaw moment of the quadrotor drone; I x Represents the moment of inertia of the quadrotor drone on the x-axis; I y Represents the moment of inertia of the quadrotor drone on the y-axis; I z represents the moment of inertia of the quadrotor drone on the y-axis; J r represents the rotor inertia of the quadrotor drone; Ω represents the motor speed; K1 represents the influence coefficient of air resistance on the horizontal coordinate of the quadrotor drone; K2 represents the influence coefficient of air resistance on the vertical coordinate of the quadrotor drone; K3 represents the influence coefficient of air resistance on the height coordinate of the quadrotor drone; K4 represents the influence coefficient of air resistance on the roll angle of the quadrotor drone; K5 represents the influence coefficient of air resistance on the pitch angle of the quadrotor drone; K6 represents the influence coefficient of air resistance on the yaw angle of the quadrotor drone; g represents the acceleration of gravity; l represents the wheelbase of the quadrotor drone;
[0101] S2: Based on the virtual structure formation method, the reference trajectory of the unmanned ship and the unmanned aerial vehicle for constructing the ship-machine formation configuration is designed with time as a parameter; the virtual structure formation method is one of the existing formation technologies and is a known technical means, which will not be described in detail here;
[0102] Specifically, Figure 3 As shown, the method for constructing the reference trajectory of the unmanned ship and the unmanned aerial vehicle includes the following steps:
[0103] S21: Define the virtual reference path point of the unmanned ship, and its expression is
[0104]
[0105] Where: x r(k) represents the x-coordinate value of the virtual reference point at the k-th sampling moment; y r (k) represents the y coordinate value of the virtual reference point at the kth sampling moment; T s Indicates the sampling time;
[0106] S22: Assuming that the motion trajectory of the unmanned ship coincides with the virtual reference path point, the reference trajectory of the unmanned ship is obtained based on the mathematical model of the unmanned ship motion:
[0107]
[0108] Where: x ref (k) represents the x-coordinate reference value of the unmanned ship at the kth sampling moment; y ref (k) represents the y coordinate reference value of the unmanned ship at the kth sampling moment; ψ ref (k) represents the reference value of the heading angle of the unmanned ship at the kth sampling moment; ψ r (k) represents the heading angle reference value of the virtual reference point at the kth sampling moment;
[0109] S23: Based on the virtual structure formation method, the reference trajectory of the UAV is designed according to the reference trajectory of the unmanned ship:
[0110]
[0111] Where: x bref (k) represents the x-coordinate reference value of the UAV at the kth sampling moment; y bref (k) represents the y coordinate reference value of the UAV at the kth sampling moment; z bref (k) represents the z-coordinate reference value of the UAV at the kth sampling moment; d L Indicates the relative distance between the drone and the virtual reference point;
[0112] S3: In order to eliminate the uncertainty of the unmanned ship model and the influence of external disturbance on the system performance, a disturbance observer is introduced to observe and compensate for the disturbance. That is, a nonlinear disturbance observer is constructed according to the mathematical model of the unmanned ship motion to realize the observation of the unmanned ship interference and compensate it at the control input end. The specific steps include:
[0113] S31: Simplify and rewrite the mathematical model of unmanned ship motion as
[0114]
[0115] Where: υ represents the velocity vector of the unmanned ship and υ=[uvr] T; M represents the inertia matrix; d represents the total disturbance caused by external interference and model uncertainty; τ represents the control input of the unmanned ship; f(υ) represents the nonlinear term in the mathematical model of the unmanned ship's motion; u represents the forward speed of the unmanned ship; v represents the drift speed of the unmanned ship; r represents the bow angular velocity of the unmanned ship;
[0116] S32: Define the auxiliary variable ξ of the nonlinear disturbance observer, which is expressed as
[0117]
[0118] Where: represents the observed value of d; p(υ) represents the nonlinear function to be designed;
[0119] S33: Based on the rewritten mathematical model of unmanned ship motion and the auxiliary variable ξ, a nonlinear disturbance observer is constructed, and its expression is:
[0120]
[0121] Where: represents the first-order derivative of ξ; L(υ) represents the gain matrix of the nonlinear disturbance observer, and
[0122] represents the first derivative of υ.
[0123] S4: Based on the model predictive control algorithm, a path tracking control law for the ship-motor formation is constructed according to the reference trajectories of the unmanned ship and the unmanned aerial vehicle, so as to realize the path tracking control of the unmanned ship and the unmanned aerial vehicle formation according to the path tracking control law of the ship-motor formation, which specifically includes the following steps:
[0124] S41: Based on the model predictive control algorithm, the state space equations of the unmanned ship and the unmanned aerial vehicle are discretized according to the unmanned ship motion mathematical model and the six-degree-of-freedom unmanned aerial vehicle motion mathematical model. The expression is:
[0125]
[0126] Where: T s represents the sampling time; X(k+1|k) represents the state of the unmanned ship at time k+1 predicted at time k; X(k) represents the state of the unmanned ship at time k; represents the first-order differential of X(k); X b (k+1|k) represents the state vector of the drone at time k+1 predicted at time k; Represents X b The first-order differential of (k+1|k); X b (k) represents the state vector of the drone at time k; Represents Xb (k) first-order derivative; Represents X b The second-order derivative of (k);
[0127] In this embodiment, the model predictive control algorithm solves an open-loop optimization problem in a finite time domain at each sampling moment according to the system state information obtained at the current moment. The solution obtained is the predicted optimal control sequence. The first element of the optimal control sequence is taken as the control input at the next moment, and the above optimization process is repeated at the next sampling moment. The principle is as follows: Figure 4 As shown, it is necessary to establish the discretized state space equations of the unmanned ship and the unmanned aerial vehicle respectively;
[0128] S42: According to the discretized state space equations of the unmanned ship and the unmanned aerial vehicle, the predicted state vector and the predicted control input vector at each sampling moment in the set prediction interval and control interval are obtained, and the expression is:
[0129]
[0130]
[0131] Where: Represents the state vector of the unmanned ship predicted at each sampling moment within the prediction interval; Represents the control input vector of the unmanned ship predicted at each sampling moment within the control interval; Indicates prediction k+N at time k p The state vector value of the unmanned ship at the moment; represents the control input vector of the unmanned ship at each sampling moment within the predicted control interval; Represents the state vector of the predicted drone at each sampling moment within the prediction interval; Represents the control input vector of the UAV predicted at each sampling moment within the control interval; Indicates prediction k+N at time k p The drone state vector value at the moment; represents the control input vector of the UAV at each sampling moment within the predicted control interval;
[0132] S43: Based on the reference trajectory of the UAV and the reference trajectory of the unmanned ship, the state error vector and the control input change of the unmanned ship and the UAV are obtained according to the predicted state vector and the predicted control input vector. The expression is:
[0133]
[0134] Where: represents the state error vector of the unmanned ship at time k+i; represents the state error vector of the unmanned ship at time k+i; X ref (k+i) represents the reference state vector of the unmanned ship at time k+i; ΔU(k+j) represents the control input change vector of the unmanned ship at time k+j; represents the predictive control input vector of the unmanned ship at time k+j; represents the state error vector of the drone at time k+i; represents the state error vector of the drone at time k+i; X bref (k+i) represents the reference state vector of the UAV at time k+i; represents the predicted control input vector of the UAV at time k+j; ΔU b (k+j) represents the control input change vector of the UAV at time k+j;
[0135] S44: Construct a cost function for the state error vector of the unmanned ship and the unmanned aerial vehicle and the control input change, and its expression is
[0136]
[0137] Where: J cost represents the cost function designed according to the error vector of the unmanned ship's motion and its control input variation; Q represents the weight matrix of the unmanned ship's state error vector related terms in the cost function; R represents the weight matrix of the unmanned ship's predicted control input variation vector related terms in the cost function; J bcost represents the cost function designed according to the error vector of the UAV motion and the change of its control input; Q b Represents the weight matrix of the state error vector related items of the UAV in the cost function; R b The weight matrix of the related items of the predictive control input change vector of the UAV in the cost function is shown;
[0138] S45: constructing a target model for the unmanned ship and unmanned aircraft formation path tracking optimization problem according to the cost function, and solving the target model to obtain a path tracking control law for the unmanned ship and unmanned aircraft formation;
[0139] The target model of the unmanned ship-drone formation path tracking optimization problem is constructed, and its expression is
[0140] argmin{J cost (ΔU(k))}
[0141] st
[0142] X(k+1)=f(X(k),U(k))
[0143] U min ≤U(k)≤Umax
[0144] Y min ≤Y(k)≤Y max
[0145] argmin{J bcost (ΔU(k))}
[0146] st
[0147] X b (k+1)=f(X b (k),U b (k))
[0148] U bmin ≤U b (k)≤U bmax
[0149] Y bmin ≤Y b (k)≤Y bmax
[0150] Where: X(k) represents the state vector of the unmanned ship at time k, i.e. the current time; U(k) represents the control input vector of the unmanned ship at time k, i.e. the current time; Y(k) represents the output vector of the unmanned ship at time k, i.e. the current time; X(k+1) represents the state vector of the unmanned ship at time k+1; U min represents the minimum value of the control input constraint of the unmanned ship; U max represents the maximum value of the control input constraint of the unmanned ship; Y min represents the minimum value of the state output constraint of the unmanned ship; Y max represents the maximum value of the state output constraint of the unmanned ship; X b (k) represents the state vector of the UAV at time k, i.e. the current time; U b (k) represents the control input vector of the UAV at time k, i.e. the current time; Y(k) represents the output vector of the UAV at time k, i.e. the current time; X b (k+1) represents the state vector of the UAV at time k+1; U bmin represents the minimum value of the UAV control input constraint; U bmax represents the maximum value of the UAV control input constraint; Y bmin Represents the minimum value of the drone state output constraint; Y bmax Indicates the maximum value of the drone state output constraint;
[0151] In this embodiment, since the model predictive control obtains the optimal control input by solving the optimization problem with constraints, there is no specific algebraic expression to express the control law, that is, solving the target model of the above optimization problem can obtain the optimal control input, that is, the path tracking control law of the motor-ship formation;
[0152] S46: Implement the path tracking control of the unmanned ship and unmanned aerial vehicle formation according to the path tracking control law of the motor-ship formation.
[0153] The simulation results in this embodiment are as follows Figures 5 to 18 As shown, Figure 5 The simulation results for prediction intervals of different lengths are shown in Figure 2. Figure 6 is a schematic diagram of the formation of the above system in a two-dimensional plane. Figure 7 It is a schematic diagram of the formation of the above system in a three-dimensional plane; Figure 8 and Fig. 9 They are the simulation results of the formation system sailing in two-dimensional plane and three-dimensional space respectively; Fig.10 and Fig.11 are the longitudinal and lateral relative positions of the formation system during navigation, Fig.12 Schematic diagram of the position error of the unmanned ship and the drone relative to the reference position of the fixed formation configuration. Fig.13 This is a simulation diagram of the change of the heading angle of the unmanned ship; Fig.14 This is the simulation result diagram of the UAV flight altitude; Fig.15 This is a simulation diagram of the change of the attitude angle of the UAV; Fig.16 is the control input curve of the unmanned ship; Fig.17 is the drone control input curve; Fig.18 is the disturbance observer observation result diagram; Fig.19 is the disturbance observer observation error map;
[0154] And from the simulation experiments, it can be seen that the beneficial effects of the present invention are that the unmanned ship and UAV formation path tracking control strategy is based on model predictive control, the formation configuration of the unmanned ship and the UAV is designed based on the virtual structure method, and the nonlinear disturbance observer is combined to estimate the unmanned ship disturbance term to compensate for the disturbance in the controller. This method has good feasibility and superiority in the presence of external disturbances.
[0155] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, 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 path tracking control method for a motor-vessel formation based on model predictive control, characterized in that: The following steps are involved: S1: Construct a mathematical model of the motion of an unmanned ship with three degrees of freedom and a mathematical model of the motion of a drone with six degrees of freedom considering external interference; S2: Based on the virtual structure formation method, the reference trajectory of the unmanned ship and the unmanned aerial vehicle is designed with time as the parameter; S3: Construct a nonlinear disturbance observer based on the mathematical model of the unmanned ship motion to observe the unmanned ship interference and compensate it at the control input end; S4: Based on the model predictive control algorithm, the path tracking control law of the motor-ship formation is constructed according to the reference trajectories of the unmanned ship and the drone, so as to realize the path tracking control of the unmanned ship and the drone formation according to the path tracking control law of the motor-ship formation.
2. The path tracking control method of a motor-vessel formation based on model predictive control according to claim 1, characterized in that: The method for constructing the reference trajectory of the unmanned ship and the unmanned aerial vehicle in S2 includes the following steps: S21: Define the virtual reference path point of the unmanned ship, and its expression is Where: x r (k) represents the x-coordinate value of the virtual reference point at the k-th sampling moment; y r (k) represents the y coordinate value of the virtual reference point at the kth sampling moment; T s Indicates the sampling time; S22: Assuming that the reference motion trajectory of the unmanned ship coincides with the virtual reference path point, the reference trajectory of the unmanned ship is Where: x ref (k) represents the x-coordinate reference value of the unmanned ship at the kth sampling moment; y ref (k) represents the y coordinate reference value of the unmanned ship at the kth sampling moment; ψ ref (k) represents the reference value of the heading angle of the unmanned ship at the kth sampling moment; ψ r (k) represents the heading angle reference value of the virtual reference point at the kth sampling moment; S23: Based on the virtual structure formation method, the reference trajectory of the UAV is designed as Where: x bref (k) represents the x-coordinate reference value of the UAV at the kth sampling moment; y bref (k) represents the y coordinate reference value of the UAV at the kth sampling moment; z bref (k) represents the z-coordinate reference value of the UAV at the kth sampling moment; d L Indicates the relative distance between the drone and the virtual reference point.
3. The path tracking control method of a motor-vessel formation based on model predictive control according to claim 2 is characterized in that: The S3 comprises the following steps: S31: Simplify and rewrite the mathematical model of unmanned ship motion as Where: υ represents the velocity vector of the unmanned ship and υ=[uvr] T ; M represents the inertia matrix; d represents the total disturbance caused by external interference and model uncertainty; τ represents the control input of the unmanned ship; f(υ) represents the nonlinear term in the mathematical model of the unmanned ship's motion; u represents the forward speed of the unmanned ship; v represents the drift speed of the unmanned ship; r represents the bow angular velocity of the unmanned ship; S32: Define the auxiliary variable ξ of the nonlinear disturbance observer, which is expressed as Where: represents the observed value of d; p(υ) represents the nonlinear function to be designed; S33: Based on the rewritten mathematical model of unmanned ship motion and the auxiliary variable ξ, a nonlinear disturbance observer is constructed, and its expression is: Where: represents the first-order derivative of ξ; L(υ) represents the gain matrix of the nonlinear disturbance observer, and represents the first derivative of υ.
4. The path tracking control method of a motor-vessel formation based on model predictive control according to claim 2, characterized in that: The S4 comprises the following steps: S41: Based on the model predictive control algorithm, the state space equations of the unmanned ship and the unmanned aerial vehicle are discretized according to the unmanned ship motion mathematical model and the six-degree-of-freedom unmanned aerial vehicle motion mathematical model. The expression is: Where: T s represents the sampling time; X(k+1|k) represents the state of the unmanned ship at time k+1 predicted at time k; X(k) represents the state of the unmanned ship at time k; represents the first-order differential of X(k); X b (k+1|k) represents the state vector of the drone at time k+1 predicted at time k; Represents X b The first-order differential of (k+1|k); X b (k) represents the state vector of the drone at time k; Represents X b (k) first-order derivative; Represents X b The second-order derivative of (k); S42: According to the discretized state space equations of the unmanned ship and the unmanned aerial vehicle, the predicted state vector and the predicted control input vector at each sampling moment in the set prediction interval and control interval are obtained, and the expression is: Where: Represents the state vector of the unmanned ship predicted at each sampling moment within the prediction interval; Represents the control input vector of the unmanned ship predicted at each sampling moment within the control interval; Indicates prediction k+N at time k p The state vector value of the unmanned ship at the moment; represents the control input vector of the unmanned ship at each sampling moment within the predicted control interval; Represents the state vector of the predicted drone at each sampling moment within the prediction interval; Represents the control input vector of the UAV predicted at each sampling moment within the control interval; Indicates prediction k+N at time k p The drone state vector value at the moment; represents the control input vector of the UAV at each sampling moment within the predicted control interval; S43: Based on the reference trajectory of the UAV and the reference trajectory of the unmanned ship, the state error vector and the control input change of the unmanned ship and the UAV are obtained according to the predicted state vector and the predicted control input vector. The expression is: Where: represents the state error vector of the unmanned ship at time k+i; represents the state error vector of the unmanned ship at time k+i; X ref (k+i) represents the reference state vector of the unmanned ship at time k+i; ΔU(k+j) represents the control input change vector of the unmanned ship at time k+j; represents the predictive control input vector of the unmanned ship at time k+j; represents the state error vector of the drone at time k+i; represents the state error vector of the drone at time k+i; X bref (k+i) represents the reference state vector of the UAV at time k+i; represents the predicted control input vector of the UAV at time k+j; ΔU b (k+j) represents the control input change vector of the UAV at time k+j; S44: Construct a cost function for the state error vector of the unmanned ship and the unmanned aerial vehicle and the control input change, and its expression is Where: J cost represents the cost function designed according to the error vector of the unmanned ship's motion and its control input variation; Q represents the weight matrix of the unmanned ship's state error vector related terms in the cost function; R represents the weight matrix of the unmanned ship's predicted control input variation vector related terms in the cost function; J bcost represents the cost function designed according to the error vector of the UAV motion and the change of its control input; Q b Represents the weight matrix of the state error vector related items of the UAV in the cost function; R b The weight matrix of the related items of the predictive control input change vector of the UAV in the cost function is shown; S45: constructing a target model for the unmanned ship and unmanned aircraft formation path tracking optimization problem according to the cost function, and solving the target model to obtain a path tracking control law for the unmanned ship and unmanned aircraft formation; The target model of the unmanned ship-drone formation path tracking optimization problem is constructed, and its expression is angry me{J cost (ΔU(k))} st X(k+1)=f(X(k),U(k)) IN min ≤U(k)≤U max AND min ≤Y(k)≤Y max argmin{J bcost (ΔU(k))} st X b (k+1)=f(X b (k),U b (k)) IN bmin ≤U b (k)≤U bmax Where: X(k) represents the state vector of the unmanned ship at time k, i.e. the current time; U(k) represents the control input vector of the unmanned ship at time k, i.e. the current time; Y(k) represents the output vector of the unmanned ship at time k, i.e. the current time; X(k+1) represents the state vector of the unmanned ship at time k+1; U min represents the minimum value of the control input constraint of the unmanned ship; U max represents the maximum value of the control input constraint of the unmanned ship; Y min represents the minimum value of the state output constraint of the unmanned ship; Y max represents the maximum value of the state output constraint of the unmanned ship; X b (k) represents the state vector of the UAV at time k, i.e. the current time; U b (k) represents the control input vector of the UAV at time k, i.e. the current time; Y(k) represents the output vector of the UAV at time k, i.e. the current time; X b (k+1) represents the state vector of the UAV at time k+1; U bmin represents the minimum value of the UAV control input constraint; U bmax represents the maximum value of the UAV control input constraint; Y bmin Represents the minimum value of the drone state output constraint; Y bmax Indicates the maximum value of the drone state output constraint; S46: Implement the path tracking control of the unmanned ship and unmanned aerial vehicle formation according to the path tracking control law of the motor-ship formation.
5. The path tracking control method of a motor-vessel formation based on model predictive control according to claim 1, characterized in that: The S1 comprises the following steps: S101: Establish a mathematical model of the motion of an unmanned ship with three degrees of freedom; S102: Establish a mathematical model of UAV motion with six degrees of freedom.
6. The path tracking control method of a motor-vessel formation based on model predictive control according to claim 5, characterized in that: The expression of the mathematical model of the three-degree-of-freedom unmanned ship motion established in S101 is: Where: x represents the horizontal coordinate of the position of the unmanned ship; y represents the vertical coordinate of the position of the unmanned ship; ψ represents the heading angle of the unmanned ship; u represents the forward speed of the unmanned ship; v represents the horizontal drift speed of the unmanned ship; r represents the bow angular velocity of the unmanned ship; f u , f v , f r represents the nonlinear term in the unmanned ship model; τ u represents the thrust input of the unmanned ship; τ r represents the bow turning moment input of the unmanned ship; τ wu , τ wv , τ wr Indicates the external environmental disturbance acting on the ship; Δf u , Δf v , Δf r represents the parameter uncertainty caused by unmodeled dynamics; m 11 , m 22 , m 33 Represents the parameters of the ship's inertia matrix; d 11 , d 22 , d 33 , d 23 , d 23 Parameters representing the ship damping matrix; The expression of the mathematical model of the six-degree-of-freedom UAV motion established in S102 is: Where: x b Indicates the horizontal coordinate of the drone’s position; y b Indicates the vertical coordinate of the drone’s position; z b Indicates the vertical coordinate of the drone; Represents the roll angle of the quadrotor drone; θ b represents the pitch angle of the quadrotor drone; ψ b represents the heading angle of the quadrotor drone; U1 represents the vertical lift of the quadrotor drone; U2 represents the rolling moment of the quadrotor drone; U3 represents the pitch moment of the quadrotor drone; U4 represents the yaw moment of the quadrotor drone; I x Represents the moment of inertia of the quadrotor drone on the x-axis; I y Represents the moment of inertia of the quadrotor drone on the y-axis; I z Represents the moment of inertia of the quadrotor drone on the y-axis; J r represents the rotor inertia of the quadrotor drone; Ω represents the motor speed; K1 represents the influence coefficient of air resistance on the horizontal coordinate of the quadrotor drone; K2 represents the influence coefficient of air resistance on the vertical coordinate of the quadrotor drone; K3 represents the influence coefficient of air resistance on the height coordinate of the quadrotor drone; K4 represents the influence coefficient of air resistance on the roll angle of the quadrotor drone; K5 represents the influence coefficient of air resistance on the pitch angle of the quadrotor drone; K6 represents the influence coefficient of air resistance on the yaw angle of the quadrotor drone; g represents the acceleration of gravity; l represents the wheelbase of the quadrotor drone.
Citation Information
Cited By
Unmanned ship trajectory tracking control method based on preset performance convex online model prediction
CN119846974A
