Underwater unmanned vehicle dynamic positioning control method based on discrete optimal control

Abstract

The present application relates to the technical field of non-electric variable control, and particularly relates to an underwater unmanned vehicle dynamic positioning control method based on discrete optimal control, which comprises the following steps: obtaining simplified underwater unmanned vehicle longitudinal motion, horizontal plane motion, vertical plane motion equations and auxiliary motion equations; establishing underwater unmanned vehicle longitudinal motion, horizontal plane motion and vertical plane motion state space models; solving underwater unmanned vehicle command longitudinal displacement, command lateral displacement and command heading; designing underwater unmanned vehicle dynamic positioning control law, solving underwater unmanned vehicle command longitudinal thrust, command lateral thrust, command rudder moment, command vertical thrust and command trim moment; solving bow and stern side thrust command speed, bow and stern vertical thrust command speed and main thrust command speed, and completing underwater unmanned vehicle dynamic positioning control. The method provided by the present application can effectively avoid the static error problem of traditional PID control and LQR optimal control.

Description

Technical Field

[0001] This invention relates to the field of control technology for non-electrical variables, and in particular to a dynamic positioning control method for underwater unmanned vehicles based on discrete optimal control. Background Technology

[0002] Underwater unmanned vehicles (UAVs) have been widely used in marine science, marine engineering, and other fields. With the increasing complexity of underwater missions, higher demands are being placed on the motion control precision of underwater UAVs. Dynamic positioning control is a technology that uses vertical and lateral thrust systems to control heading and position, offsetting marine environmental disturbances and achieving high-precision position or trajectory maintenance for underwater UAVs. To achieve zero steady-state error in the dynamic positioning control of underwater vehicles, on the one hand, traditional PID control methods are prone to control system oscillations when the integral term coefficient is too large, and the system's resistance to constant disturbances is weak when the coefficient is too small; on the other hand, linear optimal quadratic (LQR) control in continuous systems inherently lacks an integral element and cannot achieve zero steady-state error control. Summary of the Invention

[0003] The technical problem to be solved by this invention is to provide a dynamic positioning control method for underwater unmanned vehicles based on discrete optimal control. The method performs incremental discrete optimal control on the motion model of the underwater vehicle and designs a steady-state error-free control law for the dynamic positioning of the underwater vehicle, which can effectively avoid the steady-state error problem of traditional PID control and LQR optimal control.

[0004] This invention is achieved through the following technical solution:

[0005] The dynamic positioning control method for underwater unmanned vehicles based on discrete optimal control includes the following steps:

[0006] S1: By simplifying the six-degree-of-freedom motion equations of the underwater unmanned vehicle and the auxiliary motion equations of the underwater unmanned vehicle, we obtain the simplified longitudinal motion equation, horizontal plane motion equation, vertical plane motion equation and auxiliary motion equation of the underwater unmanned vehicle.

[0007] S2: Based on the simplified longitudinal motion equation, horizontal plane motion equation, vertical plane motion equation and auxiliary motion equation of the underwater unmanned vehicle, establish the longitudinal motion state space model, horizontal plane motion state space model and vertical plane motion state space model of the underwater unmanned vehicle.

[0008] S3: Based on the current position information and commanded position information of the underwater unmanned vehicle, calculate the commanded longitudinal displacement, commanded lateral displacement and commanded heading of the underwater unmanned vehicle;

[0009] S4: According to the established longitudinal motion state space model, horizontal plane motion state space model and vertical plane motion state space model of the underwater unmanned vehicle, a dynamic positioning control law of the underwater unmanned vehicle is designed by an incremental discrete optimal control method, and the command longitudinal thrust, command lateral thrust, command yaw moment, command vertical thrust and command trim moment of the underwater unmanned vehicle are calculated in combination with the calculated command longitudinal displacement, command lateral displacement and command heading of the underwater unmanned vehicle;

[0010] S5: According to the calculated command longitudinal thrust, command lateral thrust, command yaw moment, command vertical thrust and command trim moment, the command rotation speed of the bow and stern side thrust and the command rotation speed of the bow and stern vertical thrust are calculated, the command rotation speed of the main thrust is calculated according to the calculated command longitudinal thrust, and the dynamic positioning control of the underwater unmanned vehicle is completed by driving the bow and stern side thrust, the bow and stern vertical thrust and the main thrust respectively through the command rotation speed of the bow and stern side thrust, the command rotation speed of the bow and stern vertical thrust and the command rotation speed of the main thrust.

[0011] The underwater motion six-degree-of-freedom motion equation of the underwater unmanned vehicle in step S1 includes an axial motion equation, a lateral motion equation, a vertical motion equation, a roll motion equation, a pitch motion equation and a yaw motion equation.

[0012] Further, the auxiliary motion equation of the underwater unmanned vehicle in step S1 is formula (7):

[0013] (7);

[0014] Wherein: is the northward velocity of the underwater unmanned vehicle in the fixed coordinate system, is the longitudinal velocity of the underwater unmanned vehicle, is the heading angle of the underwater unmanned vehicle, is the trim angle of the underwater unmanned vehicle, is the lateral velocity of the underwater unmanned vehicle, is the roll angle of the underwater unmanned vehicle, is the vertical velocity of the underwater unmanned vehicle, is the eastward velocity of the underwater unmanned vehicle in the fixed coordinate system, is the depth rate, is the roll angular rate of the underwater unmanned vehicle, is the roll angular velocity of the underwater unmanned vehicle, is the trim angular velocity of the underwater unmanned vehicle, is the heading angular velocity of the underwater unmanned vehicle, is the trim angular rate of the underwater unmanned vehicle, is the heading angular rate of the underwater unmanned vehicle.

[0015] Furthermore, the simplified longitudinal motion equation of the underwater unmanned vehicle obtained in step S1 is equation (8), the simplified horizontal motion equation of the underwater unmanned vehicle is equation (9), and the simplified vertical motion equation of the underwater unmanned vehicle is equation (10).

[0016] (8);

[0017] (9);

[0018] (10);

[0019] in: For the mass of underwater unmanned vehicles, The longitudinal velocity of the underwater unmanned vehicle. The lateral velocity of the underwater unmanned vehicle. The vertical velocity of the underwater unmanned vehicle. For the longitudinal acceleration of the underwater unmanned vehicle, For the lateral acceleration of the underwater unmanned vehicle, The longitudinal force coefficient caused by the longitudinal acceleration of the underwater unmanned vehicle. The longitudinal force coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle. The longitudinal thrust generated by the main thruster For the heading angular velocity of the underwater unmanned vehicle, Let be the derivative of the heading angular velocity of the underwater unmanned vehicle with respect to time. The lateral force coefficient caused by the lateral acceleration of the underwater unmanned vehicle. The lateral force coefficient is caused by the derivative of the heading angular velocity of the underwater unmanned vehicle with respect to time. The lateral force coefficient is caused by the combined effect of the heading angular velocity and longitudinal velocity of the underwater unmanned vehicle. The lateral force coefficient is caused by the combined effect of the lateral and longitudinal velocities of the underwater unmanned vehicle. To assist the lateral thrust generated by the thruster, For underwater unmanned vehicles to circle Moment of inertia of the shaft The turning moment coefficient is caused by the derivative of the heading angular velocity of the underwater unmanned vehicle with respect to time. The turning moment coefficient caused by the lateral acceleration of the underwater unmanned vehicle. The turning moment coefficient is caused by the combined effect of the heading angular velocity and longitudinal velocity of the underwater unmanned vehicle. The turning moment coefficient is caused by the combined effect of the lateral and longitudinal velocities of the underwater unmanned vehicle. The turning torque generated by the auxiliary thruster, a vertical acceleration of the underwater unmanned vehicle, a derivative of a pitch angle velocity of the underwater unmanned vehicle with respect to time, a vertical force coefficient caused by the derivative of the pitch angle velocity of the underwater unmanned vehicle with respect to time, a vertical force coefficient caused by the vertical acceleration of the underwater unmanned vehicle, a vertical force coefficient caused by the pitch angle velocity and the longitudinal velocity of the underwater unmanned vehicle, a pitch angle velocity of the underwater unmanned vehicle, a vertical force coefficient caused by the vertical velocity and the longitudinal velocity of the underwater unmanned vehicle, a vertical thrust generated by the auxiliary thruster, a moment of inertia of the underwater unmanned vehicle about an axis, a pitch moment coefficient caused by the derivative of the pitch angle velocity of the underwater unmanned vehicle with respect to time, a pitch moment coefficient caused by the vertical acceleration of the underwater unmanned vehicle, a pitch moment coefficient caused by the pitch angle velocity and the longitudinal velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the vertical velocity and the longitudinal velocity of the underwater unmanned vehicle, a metacentric height, a gravitational acceleration, a pitch angle of the underwater unmanned vehicle, a pitch moment generated by the auxiliary thruster.

[0020] Further, the simplified underwater unmanned vehicle auxiliary motion equation obtained in step S1 is formula (11):

[0021] (11).

[0022] Further, the longitudinal motion state space model of the underwater unmanned vehicle established in step S1 is formula (12), the horizontal plane motion state space model of the underwater unmanned vehicle is formula (13), and the vertical plane motion state space model of the underwater unmanned vehicle is formula (14):

[0023] (12);

[0024] (13);

[0025] (14);

[0026] wherein: a coefficient of influence of the longitudinal velocity of the underwater unmanned vehicle on the longitudinal acceleration of the underwater unmanned vehicle, a coefficient of influence of the longitudinal thrust of the underwater unmanned vehicle on the longitudinal acceleration of the underwater unmanned vehicle, a longitudinal displacement of the underwater unmanned vehicle, a longitudinal displacement rate of the underwater unmanned vehicle, a heading angle of the underwater unmanned vehicle, a heading angle rate of the underwater unmanned vehicle, a lateral displacement of the underwater unmanned vehicle, a lateral displacement rate of the underwater unmanned vehicle, a coefficient of influence of the lateral velocity of the underwater unmanned vehicle on the lateral acceleration, a coefficient of influence of the lateral velocity of the underwater unmanned vehicle on the lateral acceleration, a coefficient of influence of the lateral velocity of the underwater unmanned vehicle on the derivative of the heading angle rate with respect to time, a coefficient of influence of the heading angle rate of the underwater unmanned vehicle on the derivative of the heading angle rate with respect to time, a coefficient of influence of the lateral thrust generated by the auxiliary thruster on the lateral acceleration, a coefficient of influence of the yawing moment generated by the auxiliary thruster on the derivative of the heading angle rate with respect to time, a pitch angle rate of the underwater unmanned vehicle, a depth of the underwater unmanned vehicle, a depth rate, a coefficient of influence of the vertical velocity of the underwater unmanned vehicle on the derivative of the vertical velocity, a coefficient of influence of the pitch angle rate of the underwater unmanned vehicle on the derivative of the vertical velocity, a coefficient of influence of the vertical velocity of the underwater unmanned vehicle on the derivative of the pitch angle rate, a coefficient of influence of the pitch angle rate of the underwater unmanned vehicle on the derivative of the pitch angle rate, a coefficient of influence of the pitch angle of the underwater unmanned vehicle on the derivative of the pitch angle rate, a coefficient of influence of the vertical thrust of the underwater unmanned vehicle on the derivative of the vertical velocity, a coefficient of influence of the pitch moment of the underwater unmanned vehicle on the derivative of the pitch angle rate.

[0027] Further, the commanded longitudinal displacement of the underwater unmanned vehicle is calculated according to equation (15), the commanded lateral displacement of the underwater unmanned vehicle is calculated according to equation (16), and the commanded heading of the underwater unmanned vehicle is calculated according to equation (17) in step S3:

[0028] (15);

[0029] (16);

[0030] (17);

[0031] in: This refers to the longitudinal displacement of the underwater unmanned vehicle in the ship's coordinate system. This refers to the northward displacement of the underwater unmanned vehicle in the command fixed coordinate system. for The underwater unmanned vehicle is constantly shifting northward. To provide the heading angle for the underwater unmanned vehicle. This represents the eastward displacement of the underwater unmanned vehicle in the command coordinate system. for The underwater unmanned vehicle is constantly shifting eastward. This refers to the lateral displacement of the underwater unmanned vehicle in the command ship coordinate system. This represents the initial eastward displacement of the underwater unmanned vehicle in a fixed coordinate system. This represents the initial northward displacement of the underwater unmanned vehicle in a fixed coordinate system.

[0032] Furthermore, the dynamic positioning control law of the underwater unmanned vehicle designed in step S4 using the incremental discrete optimal control method includes: the longitudinal motion state space control law of the underwater unmanned vehicle is Equation (18), the horizontal motion state space control law of the underwater unmanned vehicle is Equation (19), and the vertical motion state space control law of the underwater unmanned vehicle is Equation (20).

[0033] (18);

[0034] (19);

[0035] (20);

[0036] in: for The command specifies the longitudinal thrust increment. This represents the gain coefficient corresponding to the longitudinal velocity increment of the underwater unmanned vehicle. for The longitudinal velocity increment of the underwater unmanned vehicle at all times This represents the gain coefficient corresponding to the longitudinal displacement increment of the underwater unmanned vehicle. for The longitudinal displacement increment of the underwater unmanned vehicle at any time. This represents the gain coefficient corresponding to the longitudinal displacement of the underwater unmanned vehicle. for longitudinal displacement of the underwater unmanned vehicle at time k, commanded heading angle of the underwater unmanned vehicle, commanded lateral thrust increment at time k, gain coefficient corresponding to the lateral velocity increment in the lateral motion control parameters of the underwater unmanned vehicle, lateral velocity increment of the underwater unmanned vehicle at time k, gain coefficient corresponding to the yaw rate increment in the yaw motion control parameters of the underwater unmanned vehicle, yaw rate increment of the underwater unmanned vehicle at time k, gain coefficient corresponding to the lateral displacement increment in the lateral motion control parameters of the underwater unmanned vehicle, lateral displacement increment of the underwater unmanned vehicle at time k, gain coefficient corresponding to the lateral displacement in the lateral motion control parameters of the underwater unmanned vehicle, lateral displacement of the underwater unmanned vehicle at time k, commanded lateral displacement of the underwater unmanned vehicle, commanded yaw moment increment at time k, gain coefficient corresponding to the lateral velocity increment in the yaw motion control parameters of the underwater unmanned vehicle, gain coefficient corresponding to the yaw rate increment in the yaw motion control parameters of the underwater unmanned vehicle, gain coefficient corresponding to the heading angle increment in the yaw motion control parameters of the underwater unmanned vehicle, gain coefficient corresponding to the heading angle in the yaw motion control parameters of the underwater unmanned vehicle, heading angle of the underwater unmanned vehicle at time k, commanded heading angle of the underwater unmanned vehicle, commanded trim moment increment at time k, gain coefficient corresponding to the vertical velocity increment in the trim motion control parameters of the underwater unmanned vehicle, vertical velocity increment of the underwater unmanned vehicle at time k, gain coefficient corresponding to the trim rate increment in the trim motion control parameters of the underwater unmanned vehicle, trim rate increment of the underwater unmanned vehicle at time k, gain coefficient corresponding to the trim increment in the trim motion control parameters of the underwater unmanned vehicle,​​​​​​​​​​ is the longitudinal trim increment of the underwater unmanned vehicle at time t, is the gain coefficient corresponding to the depth increment in the longitudinal motion control parameters of the underwater unmanned vehicle, is the depth increment of the underwater unmanned vehicle at time t, is the gain coefficient corresponding to the trim in the longitudinal motion control parameters of the underwater unmanned vehicle, is the trim angle of the underwater unmanned vehicle at time t, is the commanded trim, is the commanded vertical thrust increment at time t, is the gain coefficient corresponding to the vertical velocity increment in the depth motion control parameters of the underwater unmanned vehicle, is the gain coefficient corresponding to the trim angular velocity increment in the depth motion control parameters of the underwater unmanned vehicle, is the gain coefficient corresponding to the trim increment in the depth motion control parameters of the underwater unmanned vehicle, is the gain coefficient corresponding to the depth increment in the depth motion control parameters of the underwater unmanned vehicle, is the gain coefficient corresponding to the trim in the depth motion control parameters of the underwater unmanned vehicle, is the gain coefficient corresponding to the depth in the depth motion control parameters of the underwater unmanned vehicle, is the depth of the underwater unmanned vehicle at time t, is the commanded depth.

[0037] Further, the commanded longitudinal thrust, the commanded lateral thrust, the commanded yaw moment, the commanded vertical thrust and the commanded trim moment of the underwater unmanned vehicle are calculated according to equation (21) in step S4:

[0038] (21);

[0039] wherein: is the longitudinal thrust of the underwater unmanned vehicle at time t, is the longitudinal thrust of the underwater unmanned vehicle at time t, is the lateral thrust of the underwater unmanned vehicle at time t, is the lateral thrust of the underwater unmanned vehicle at time t, is the commanded yaw moment of the underwater unmanned vehicle at time t, is the commanded yaw moment of the underwater unmanned vehicle at time t, For the moment command vertical thrust of the underwater unmanned vehicle, For the moment command vertical thrust of the underwater unmanned vehicle, For the moment command heeling moment of the underwater unmanned vehicle, For the moment command heeling moment of the underwater unmanned vehicle.

[0040] The control period of the optimized, calculated command longitudinal thrust, command lateral thrust, command yawing moment, command vertical thrust and command heeling moment of the underwater unmanned vehicle is 0.1 seconds.

[0041] Advantages of the application:

[0042] The underwater unmanned vehicle dynamic positioning control method based on discrete optimal control provided by the application designs a static error-free control law of underwater vehicle dynamic positioning through incremental discrete optimal control of the underwater vehicle motion model, can effectively avoid the static error problem of traditional PID control and LQR optimal control, and the design process is closely combined with the underwater vehicle motion model, which can effectively guide the software implementation of the control law. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 is a flowchart of the application. DETAILED DESCRIPTION

[0044] The underwater unmanned vehicle dynamic positioning control method based on discrete optimal control has a flowchart as shown in Figure 1 , and specifically includes the following steps:

[0045] S1: Simplify the underwater motion six-degree-of-freedom motion equation of the underwater unmanned vehicle and the auxiliary motion equation of the underwater unmanned vehicle to obtain a simplified longitudinal motion equation, a horizontal plane motion equation, a vertical plane motion equation and an auxiliary motion equation of the underwater unmanned vehicle;

[0046] Specifically, the underwater motion six-degree-of-freedom motion equation of the underwater unmanned vehicle is as follows:

[0047] The axial motion equation is formula (1):

[0048] (1);

[0049] The lateral motion equation is formula (2):

[0050] (2);

[0051] The vertical motion equation is formula (3):

[0052] (3);

[0053] The roll motion equation is formula (4):

[0054] (4)

[0055] The pitch motion equation is formula (5):

[0056] (5);

[0057] The yaw motion equation is formula (6):

[0058] (6);

[0059] Wherein: is the mass of the underwater unmanned vehicle, is the longitudinal velocity of the underwater unmanned vehicle, is the lateral velocity of the underwater unmanned vehicle, is the vertical velocity of the underwater unmanned vehicle, is the roll angular velocity of the underwater unmanned vehicle, is the pitch angular velocity of the underwater unmanned vehicle, is the yaw angular velocity of the underwater unmanned vehicle, is the longitudinal thrust generated by the main propeller, is the metacentric height, is the acceleration of gravity, is the lateral thrust generated by the auxiliary propeller, is the vertical thrust generated by the auxiliary propeller, is the roll moment generated by the auxiliary propeller, is the pitch moment generated by the auxiliary propeller, is the yaw moment generated by the auxiliary propeller, is the moment of inertia of the underwater unmanned vehicle about the axis, is the moment of inertia of the underwater unmanned vehicle about the axis, is the moment of inertia of the underwater unmanned vehicle about the axis, is the pitch angle of the underwater unmanned vehicle, is the northward displacement of the underwater unmanned vehicle, is the longitudinal force coefficient caused by the longitudinal acceleration of the underwater unmanned vehicle, is the longitudinal force coefficient caused by the joint action of the lateral velocity and the yaw angular velocity of the underwater unmanned vehicle, is the longitudinal force coefficient caused by the joint action of the vertical velocity and the pitch angular velocity of the underwater unmanned vehicle, A longitudinal force coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle, A longitudinal force coefficient caused by the square of the heading angle velocity of the underwater unmanned vehicle, A longitudinal force coefficient caused by the combined action of the heading angle velocity and the roll angle velocity of the underwater unmanned vehicle, A longitudinal force coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle, A longitudinal force coefficient caused by the square of the lateral velocity of the underwater unmanned vehicle, A longitudinal force coefficient caused by the square of the vertical velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the derivative of the heading angle velocity of the underwater unmanned vehicle with respect to time, A lateral force coefficient caused by the derivative of the roll angle velocity of the underwater unmanned vehicle with respect to time, A longitudinal acceleration of the underwater unmanned vehicle, A derivative of the roll angle velocity of the underwater unmanned vehicle with respect to time, A derivative of the heading angle velocity of the underwater unmanned vehicle with respect to time, A lateral force coefficient caused by the lateral acceleration of the underwater unmanned vehicle, A lateral acceleration of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the roll angle velocity and the absolute value of the roll angle velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the roll angle velocity and the pitch angle velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the heading angle velocity and the pitch angle velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the heading angle velocity and the absolute value of the heading angle velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the heading angle velocity and the longitudinal velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the roll angle velocity and the longitudinal velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the pitch angle velocity and the lateral velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the roll angle velocity and the vertical velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the heading angle velocity and the vertical velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the combined action of the lateral velocity and the absolute value of the heading angle velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the lateral velocity of the underwater unmanned vehicle and the absolute value of the lateral velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the lateral velocity of the underwater unmanned vehicle and the absolute value of the lateral velocity of the underwater unmanned vehicle, A lateral force coefficient caused by the lateral velocity of the underwater unmanned vehicle and the vertical velocity of the underwater unmanned vehicle, A vertical acceleration of the underwater unmanned vehicle, A vertical force coefficient caused by the derivative of the pitch angle of the underwater unmanned vehicle with respect to time, A vertical force coefficient caused by the derivative of the pitch angle of the underwater unmanned vehicle with respect to time, A vertical force coefficient caused by the vertical acceleration of the underwater unmanned vehicle, A vertical force coefficient caused by the square of the roll angle of the underwater unmanned vehicle, A vertical force coefficient caused by the square of the heading angle of the underwater unmanned vehicle, A vertical force coefficient caused by the roll angle of the underwater unmanned vehicle and the heading angle of the underwater unmanned vehicle, A vertical force coefficient caused by the pitch angle of the underwater unmanned vehicle and the absolute value of the pitch angle of the underwater unmanned vehicle, A vertical force coefficient caused by the pitch angle of the underwater unmanned vehicle and the absolute value of the pitch angle of the underwater unmanned vehicle, A vertical force coefficient caused by the pitch angle of the underwater unmanned vehicle and the lateral velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the heading angle of the underwater unmanned vehicle and the lateral velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the roll angle of the underwater unmanned vehicle and the lateral velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the lateral velocity of the underwater unmanned vehicle and the longitudinal velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the longitudinal velocity of the underwater unmanned vehicle and the absolute value of the vertical velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the vertical velocity of the underwater unmanned vehicle and the absolute value of the vertical velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the square of the vertical velocity of the underwater unmanned vehicle, A vertical force coefficient caused by the square of the lateral velocity of the underwater unmanned vehicle, A roll moment coefficient caused by the derivative of the roll angle of the underwater unmanned vehicle with respect to time, A roll moment coefficient caused by the derivative of the heading angle of the underwater unmanned vehicle with respect to time, A roll moment coefficient caused by the lateral acceleration of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the roll angle velocity of the underwater unmanned vehicle and the absolute value of the roll angle velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the heading angle velocity of the underwater unmanned vehicle and the pitch angle velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the roll angle velocity of the underwater unmanned vehicle and the pitch angle velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the heading angle velocity of the underwater unmanned vehicle and the absolute value of the heading angle velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the roll angle velocity of the underwater unmanned vehicle and the longitudinal velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the heading angle velocity of the underwater unmanned vehicle and the longitudinal velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the pitch angle velocity of the underwater unmanned vehicle and the lateral velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the roll angle velocity of the underwater unmanned vehicle and the vertical velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the heading angle velocity of the underwater unmanned vehicle and the vertical velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the lateral velocity of the underwater unmanned vehicle and the longitudinal velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the lateral velocity of the underwater unmanned vehicle and the absolute value of the lateral velocity of the underwater unmanned vehicle, a roll moment coefficient caused by the product of the lateral velocity of the underwater unmanned vehicle and the vertical velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the derivative of the pitch angle velocity of the underwater unmanned vehicle with respect to time, a pitch moment coefficient caused by the vertical acceleration of the underwater unmanned vehicle, a pitch moment coefficient caused by the square of the roll angle velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the product of the pitch angle velocity of the underwater unmanned vehicle and the absolute value of the pitch angle velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the square of the heading angle velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the product of the heading angle velocity of the underwater unmanned vehicle and the roll angle velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the product of the pitch angle velocity of the underwater unmanned vehicle and the longitudinal velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the product of the heading angle velocity of the underwater unmanned vehicle and the lateral velocity of the underwater unmanned vehicle, a pitch moment coefficient caused by the product of the pitch angle velocity of the underwater unmanned vehicle and the absolute value of the vertical velocity of the underwater unmanned vehicle, a trim moment coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle, a trim moment coefficient caused by the vertical velocity and the longitudinal velocity of the underwater unmanned vehicle, a trim moment coefficient caused by the absolute value of the longitudinal velocity and the vertical velocity of the underwater unmanned vehicle, a trim moment coefficient caused by the vertical velocity and the absolute value of the vertical velocity of the underwater unmanned vehicle, a trim moment coefficient caused by the square of the vertical velocity of the underwater unmanned vehicle, a trim moment coefficient caused by the square of the lateral velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the derivative of the heading angle velocity with respect to time of the underwater unmanned vehicle, a yawing moment coefficient caused by the lateral acceleration of the underwater unmanned vehicle, a yawing moment coefficient caused by the derivative of the roll angle velocity with respect to time of the underwater unmanned vehicle, a yawing moment coefficient caused by the roll angle velocity and the absolute value of the roll angle velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the roll angle velocity and the pitch angle velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the heading angle velocity and the pitch angle velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the heading angle velocity and the absolute value of the heading angle velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the heading angle velocity and the longitudinal velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the roll angle velocity and the longitudinal velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the pitch angle velocity and the lateral velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the roll angle velocity and the vertical velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the heading angle velocity and the vertical velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the lateral velocity and the absolute value of the heading angle velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the lateral velocity and the longitudinal velocity of the underwater unmanned vehicle, a yawing moment coefficient caused by the lateral velocity and the absolute value of the lateral velocity of the underwater unmanned vehicle, The yawing moment coefficient caused by the interaction of the lateral velocity and the vertical velocity of the underwater unmanned vehicle.

[0060] The auxiliary motion equation of the underwater unmanned vehicle is formula (7):

[0061] (7);

[0062] Wherein: is the northward velocity of the underwater unmanned vehicle in the fixed coordinate system, is the longitudinal velocity of the underwater unmanned vehicle, is the heading angle of the underwater unmanned vehicle, is the pitch angle of the underwater unmanned vehicle, is the lateral velocity of the underwater unmanned vehicle, is the roll angle of the underwater unmanned vehicle, is the vertical velocity of the underwater unmanned vehicle, is the eastward velocity of the underwater unmanned vehicle in the fixed coordinate system, is the depth rate, is the roll angle rate of the underwater unmanned vehicle, is the roll angle velocity of the underwater unmanned vehicle, is the pitch angle velocity of the underwater unmanned vehicle, is the heading angle velocity of the underwater unmanned vehicle, is the pitch angle rate of the underwater unmanned vehicle, is the heading angle rate of the underwater unmanned vehicle.

[0063] Since the roll angle of the underwater unmanned vehicle is generally not taken as a controlled variable, the roll motion of the underwater unmanned vehicle can be first ignored, and all hydrodynamic coefficient terms with or terms in the above equations are ignored, and the roll angle is considered to be zero. At the same time, since the nonlinear hydrodynamic terms are smaller than the linear hydrodynamic terms, all nonlinear hydrodynamic terms in the equations are ignored, and the simplified motion equation of the underwater unmanned vehicle is obtained.

[0064] The simplified longitudinal motion equation of the underwater unmanned vehicle is formula (8), the simplified horizontal plane motion equation of the underwater unmanned vehicle is formula (9), and the simplified vertical plane motion equation of the underwater unmanned vehicle is formula (10):

[0065] (8);

[0066] (9);

[0067] (10).

[0068] The preliminary simplified auxiliary motion equation of the underwater unmanned vehicle is formula (22):

[0069] (22);

[0070] Since the trim angle of the underwater unmanned vehicle needs to be controlled within a small angle during dynamic positioning control, preferably around 0-10 degrees, it can be approximately considered that , The unit is radian, so the preliminary simplified auxiliary motion equation of the underwater unmanned vehicle can be further simplified, and the simplified auxiliary motion equation of the underwater unmanned vehicle is formula (11):

[0071] (11).

[0072] The simplified longitudinal motion equation, horizontal plane motion equation, and vertical plane motion equation of the underwater unmanned vehicle are obtained, which can extract appropriate simplified models from the complex underwater motion six-degree-of-freedom motion equation of the underwater unmanned vehicle and can be directly used for direct design of dynamic positioning control algorithm.

[0073] S2: According to the simplified longitudinal motion equation, horizontal plane motion equation, and vertical plane motion equation of the underwater unmanned vehicle, the longitudinal motion state space model, horizontal plane motion state space model, and vertical plane motion state space model of the underwater unmanned vehicle are established;

[0074] The specific process is as follows: according to the transformation of the simplified longitudinal motion equation (8) of the underwater unmanned vehicle, formula (23) can be obtained:

[0075] (23);

[0076] At the same time, it is approximately considered that the longitudinal velocity of the underwater unmanned vehicle is the derivative of the longitudinal displacement, that is, .

[0077] Therefore, the longitudinal motion state space model of the underwater unmanned vehicle is formula (12):

[0078] (12);

[0079] Wherein: is the influence coefficient of the longitudinal velocity of the underwater unmanned vehicle on the longitudinal acceleration of the underwater unmanned vehicle, , is the influence coefficient of the longitudinal thrust of the underwater unmanned vehicle on the longitudinal acceleration of the underwater unmanned vehicle, , is the longitudinal displacement of the underwater unmanned vehicle, is the longitudinal displacement rate of the underwater unmanned vehicle.

[0080] Meanwhile, it is approximately considered that the lateral velocity of the underwater unmanned vehicle is the derivative of the lateral displacement, i.e. . By combining , the horizontal plane motion state space model of the underwater unmanned vehicle is formula (13).

[0081] And by combining the simplified vertical motion equation and the trim motion equation of the underwater unmanned vehicle, formula (24) is obtained:

[0082] (24);

[0083] Further combining the simplified auxiliary motion equation of the underwater unmanned vehicle, the vertical plane motion state space model of the underwater unmanned vehicle is formula (14):

[0084] (13);

[0085] (14);

[0086] wherein: is the heading angle rate of the underwater unmanned vehicle, is the lateral displacement of the underwater unmanned vehicle, is the lateral displacement rate of the underwater unmanned vehicle, is the influence coefficient of the lateral velocity of the underwater unmanned vehicle on the lateral acceleration, is the influence coefficient of the lateral velocity of the underwater unmanned vehicle on the lateral acceleration, is the influence coefficient of the lateral velocity of the underwater unmanned vehicle on the derivative of the heading angle with respect to time, is the influence coefficient of the heading angle of the underwater unmanned vehicle on the derivative of the heading angle with respect to time, is the influence coefficient of the lateral thrust generated by the auxiliary thruster on the lateral acceleration, is the influence coefficient of the yawing moment generated by the auxiliary thruster on the derivative of the heading angle with respect to time, is the trim angle rate of the underwater unmanned vehicle, is the depth of the underwater unmanned vehicle, is the depth rate, is the influence coefficient of the vertical velocity of the underwater unmanned vehicle on the derivative of the vertical velocity, is the influence coefficient of the trim angle velocity of the underwater unmanned vehicle on the derivative of the vertical velocity, is the influence coefficient of the vertical velocity of the underwater unmanned vehicle on the derivative of the trim angle velocity, is the influence coefficient of the trim angle velocity of the underwater unmanned vehicle on the derivative of the trim angle velocity. is the influence coefficient of the derivative of the longitudinal angle of the underwater unmanned vehicle to the longitudinal angle velocity, is the influence coefficient of the derivative of the vertical thrust of the underwater unmanned vehicle to the vertical velocity, is the influence coefficient of the derivative of the longitudinal moment of the underwater unmanned vehicle to the longitudinal angle velocity.

[0087] and , , , , .

[0088] According to the simplified underwater unmanned vehicle longitudinal motion equation, horizontal plane motion equation, vertical plane motion equation and auxiliary motion equation, the longitudinal motion state space model, horizontal plane motion state space model and vertical plane motion state space model of the underwater unmanned vehicle are established, which can provide a linear state space model basis for designing a dynamic positioning control method based on discrete optimal control.

[0089] S3: According to the current position information and the command position information of the underwater unmanned vehicle, the command longitudinal displacement, the command lateral displacement and the command heading of the underwater unmanned vehicle are calculated;

[0090] Since the underwater unmanned vehicle generally issues a command position point in a fixed coordinate system as the command information of the dynamic positioning control, the position point in the fixed coordinate system cannot be directly used as the command input of the above-mentioned state space motion model, and therefore a certain conversion method is required to convert the position point information in the fixed coordinate system into the command heading information, the command longitudinal displacement information and the command lateral displacement information of the underwater unmanned vehicle during dynamic positioning.

[0091] Specifically, the command longitudinal displacement of the underwater unmanned vehicle can be calculated according to formula (15), the command lateral displacement of the underwater unmanned vehicle can be calculated according to formula (16), and the command heading of the underwater unmanned vehicle can be calculated according to formula (17):

[0092] (15);

[0093] (16);

[0094] (17);

[0095] wherein: is the longitudinal displacement of the underwater unmanned vehicle in the command ship coordinate system, is the northward displacement of the underwater unmanned vehicle in the command fixed coordinate system, is the northward displacement of the underwater unmanned vehicle at the moment, ​a command heading angle of the underwater unmanned vehicle, an eastward displacement of the underwater unmanned vehicle in the command coordinate system, an eastward displacement of the underwater unmanned vehicle, an eastward displacement of the underwater unmanned vehicle at a time instant, a command lateral displacement of the underwater unmanned vehicle, an initial eastward displacement of the underwater unmanned vehicle in the fixed coordinate system, an initial northward displacement of the underwater unmanned vehicle in the fixed coordinate system.

[0096] S4: According to the established longitudinal motion state space model, horizontal plane motion state space model and vertical plane motion state space model of the underwater unmanned vehicle, a dynamic positioning control law of the underwater unmanned vehicle is designed through an incremental discrete optimal control method, and the command longitudinal displacement, command lateral displacement and command heading of the underwater unmanned vehicle are combined to calculate the command longitudinal thrust, command lateral thrust, command yaw moment, command vertical thrust and command trim moment of the underwater unmanned vehicle;

[0097] The specific process is as follows:

[0098] To design a zero-static-error form dynamic positioning control method, first, the linear state space model is discretized according to the control period The specific discretization method can adopt the Euler integral method. The discretized longitudinal motion state space model is formula (25), the discretized horizontal plane motion state space model is formula (26), and the discretized vertical plane motion state space model is formula (27):

[0099] (25);

[0100] (26);

[0101] (27);

[0102] Wherein: denotes a longitudinal velocity of the underwater unmanned vehicle at a time instant, denotes a longitudinal displacement of the underwater unmanned vehicle at a time instant, denotes a longitudinal thrust generated by the main propeller at a time instant, denotes a longitudinal velocity of the underwater unmanned vehicle at a time instant, denotes a longitudinal displacement of the underwater unmanned vehicle at a time instant, denotes a lateral velocity of the underwater unmanned vehicle at a time instant, represents the rate of change of the heading angle of the underwater unmanned vehicle at time t, represents the heading angle of the underwater unmanned vehicle at time t, represents the lateral displacement of the underwater unmanned vehicle at time t, represents the lateral thrust generated by the main thruster at time t, represents the yaw moment generated by the main thruster at time t, represents the cross-track velocity of the underwater unmanned vehicle at time t, represents the rate of change of the heading angle of the underwater unmanned vehicle at time t, represents the heading angle of the underwater unmanned vehicle at time t, represents the lateral displacement of the underwater unmanned vehicle at time t, represents the vertical velocity of the underwater unmanned vehicle at time t, represents the rate of change of the pitch angle of the underwater unmanned vehicle at time t, represents the pitch angle of the underwater unmanned vehicle at time t, represents the depth of the underwater unmanned vehicle at time t, represents the vertical thrust generated by the main thruster at time t, represents the pitch moment generated by the main thruster at time t, represents the vertical velocity of the underwater unmanned vehicle at time t, represents the rate of change of the pitch angle of the underwater unmanned vehicle at time t, represents the rate of change of the pitch angle of the underwater unmanned vehicle at time t, represents the depth of the underwater unmanned vehicle at time t, is the control period.

[0103] If the above-mentioned discretized linear state-space model is used for optimal controller design, dynamic positioning control without steady-state error cannot be achieved. Therefore, further improvements are made based on the above-mentioned discretized state-space model. The control input in the discretized state-space model is adjusted to the increment of the control input for matrix augmentation, and the incremental state variables are used as new state variables. Thus, the augmented discrete longitudinal motion state-space model is given by equation (28), the augmented discrete horizontal motion state-space model is given by equation (29), and the augmented discrete vertical motion state-space model is given by equation (30).

[0104] (28);

[0105] (29);

[0106] (30);

[0107] in: express The longitudinal velocity increment of the underwater unmanned vehicle at any given moment. express The longitudinal displacement increment of the underwater unmanned vehicle at any given time express The longitudinal thrust increment generated by the main thruster at all times express The longitudinal velocity increment of the underwater unmanned vehicle at any given moment. express The longitudinal displacement increment of the underwater unmanned vehicle at any given time express The lateral velocity increment of the underwater unmanned vehicle at all times express The constant-time angular rate increment of the underwater unmanned vehicle express The constant-time heading angle increment of the underwater unmanned vehicle express The lateral displacement increment of the underwater unmanned vehicle at any given moment. express The lateral velocity increment of the underwater unmanned vehicle at all times express The constant-time angular rate increment of the underwater unmanned vehicle express The constant-time heading angle increment of the underwater unmanned vehicle express The lateral displacement increment of the underwater unmanned vehicle at any given moment. express The vertical velocity increment of the underwater unmanned vehicle at any given moment express The increment of the pitch rate of the underwater unmanned vehicle at any given moment express The increment of the pitch angle of the underwater unmanned vehicle at any time. express The depth increment of the underwater unmanned vehicle in real time. express The vertical velocity increment of the underwater unmanned vehicle at any given moment express The increment of the pitch rate of the underwater unmanned vehicle at any given moment express The increment of the pitch angle of the underwater unmanned vehicle at any time. express The depth increment of the underwater unmanned vehicle in real time.

[0108] By applying the discrete optimal control method to the augmented discrete longitudinal motion state space model, the augmented discrete horizontal motion state space model, and the augmented discrete vertical motion state space model of the underwater unmanned vehicle, the control law for the longitudinal motion state space of the underwater unmanned vehicle is obtained as Equation (18), the control law for the horizontal motion state space of the underwater unmanned vehicle is obtained as Equation (19), and the control law for the vertical motion state space of the underwater unmanned vehicle is obtained as Equation (20).

[0109] (18);

[0110] (19);

[0111] (20);

[0112] in: for The command specifies the longitudinal thrust increment. This represents the gain coefficient corresponding to the longitudinal velocity increment of the underwater unmanned vehicle. This represents the gain coefficient corresponding to the longitudinal displacement increment of the underwater unmanned vehicle. This represents the gain coefficient corresponding to the longitudinal displacement of the underwater unmanned vehicle. for The command specifies the lateral thrust increment. This refers to the gain coefficient corresponding to the lateral velocity increment in the lateral motion control parameters of an underwater unmanned vehicle. This refers to the gain coefficient corresponding to the bow angular velocity increment in the lateral motion control parameters of an underwater unmanned vehicle. This refers to the gain coefficient corresponding to the lateral displacement increment in the lateral motion control parameters of an underwater unmanned vehicle. A gain coefficient corresponding to a lateral displacement in a lateral motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a lateral displacement in a lateral motion control parameter of the underwater unmanned vehicle, A moment increment of a rudder at a time instant, A gain coefficient corresponding to a lateral velocity increment in a rudder motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a rudder angular velocity increment in a rudder motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a heading angular rate increment in a rudder motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a heading angle in a rudder motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a lateral displacement in a lateral motion control parameter of the underwater unmanned vehicle, A vertical thrust increment at a time instant, A gain coefficient corresponding to a vertical velocity increment in a pitch motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a pitch angular velocity increment in a pitch motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a pitch increment in a pitch motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a depth increment in a pitch motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a pitch in a pitch motion control parameter of the underwater unmanned vehicle, A commanded pitch, A gain coefficient corresponding to a pitch increment in a pitch motion control parameter of the underwater unmanned vehicle, A moment increment of a rudder at a time instant, A gain coefficient corresponding to a vertical velocity increment in a depth motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a pitch angular velocity increment in a depth motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a pitch increment in a depth motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a depth increment in a depth motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a pitch in a depth motion control parameter of the underwater unmanned vehicle, A gain coefficient corresponding to a depth in a depth motion control parameter of the underwater unmanned vehicle, A commanded depth.

[0113] The gain coefficients are calculated as follows: , , , , After that, the commanded longitudinal thrust, the commanded lateral thrust, the commanded rudder moment, the commanded vertical thrust and the commanded pitch moment of the underwater unmanned vehicle are obtained by using the Euler integral method, and are specifically as follows:

[0114] (21);

[0115] wherein: is the vertical thrust of the underwater unmanned vehicle at the moment, is the vertical thrust of the underwater unmanned vehicle at the moment, is the lateral thrust of the underwater unmanned vehicle at the moment, is the lateral thrust of the underwater unmanned vehicle at the moment, is the rudder moment of the underwater unmanned vehicle at the moment, is the rudder moment of the underwater unmanned vehicle at the moment, is the vertical thrust of the underwater unmanned vehicle at the moment, is the vertical thrust of the underwater unmanned vehicle at the moment, is the rudder moment of the underwater unmanned vehicle at the moment, is the rudder moment of the underwater unmanned vehicle at the moment, is the vertical thrust of the underwater unmanned vehicle at the moment, is the vertical thrust of the underwater unmanned vehicle at the moment, is the trim moment of the underwater unmanned vehicle at the moment, is the trim moment of the underwater unmanned vehicle at the moment, is the trim moment of the underwater unmanned vehicle at the moment. is the trim moment of the underwater unmanned vehicle at the moment. is the trim moment of the underwater unmanned vehicle at the moment. is the trim moment of the underwater unmanned vehicle at the moment. is the trim moment of the underwater unmanned vehicle at the moment. is the trim moment of the underwater unmanned vehicle at the moment.

[0116] The control period of the command longitudinal thrust, the command lateral thrust, the command rudder moment, the command vertical thrust and the command trim moment of the underwater unmanned vehicle is solved. It can be preferably 0.1 seconds.

[0117] S5: according to the command longitudinal thrust, the command lateral thrust, the command rudder moment, the command vertical thrust and the command trim moment solved, the bow and stern side thrust command speed and the bow and stern vertical thrust command speed are solved, the main thrust command speed is solved according to the command longitudinal thrust, the bow and stern side thrust, the bow and stern vertical thrust and the main thrust are driven by the bow and stern side thrust command speed, the bow and stern vertical thrust command speed and the main thrust command speed respectively, and the dynamic positioning control of the underwater unmanned vehicle is completed.

[0118] In summary, the underwater unmanned vehicle dynamic positioning control method based on discrete optimal control is provided, the underwater vehicle motion model is incrementally discretely controlled, the static error control law of the underwater vehicle dynamic positioning is designed, the static error problem of the traditional PID control and the LQR optimal control is effectively avoided, and the design process is closely combined with the underwater vehicle motion model, so that the software implementation of the control law is effectively guided.

[0119] The above merely provides the preferred embodiments of the present application, and is not used to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modifications, equivalent replacements, improvements, etc. made within the principles and technical scope of the present application shall fall into the scope of the present application.

Claims

1. A method for dynamic positioning control of an underwater unmanned vehicle based on discrete optimal control, characterized in that: Comprising the following steps: S1: Simplify the underwater motion six degrees of freedom motion equation of the underwater unmanned vehicle, the auxiliary motion equation of the underwater unmanned vehicle, to obtain the simplified longitudinal motion equation of the underwater unmanned vehicle, the horizontal plane motion equation, the vertical plane motion equation and the auxiliary motion equation; The auxiliary motion equation of the underwater unmanned vehicle is formula (7): （7）； wherein: is a north velocity of the underwater unmanned vehicle in a fixed coordinate system, is a longitudinal velocity of the underwater unmanned vehicle, is a heading angle of the underwater unmanned vehicle, is a pitch angle of the underwater unmanned vehicle, is a lateral velocity of the underwater unmanned vehicle, is a roll angle of the underwater unmanned vehicle, is a vertical velocity of the underwater unmanned vehicle, is an east velocity of the underwater unmanned vehicle in a fixed coordinate system, is a depth rate, is a roll angle rate of the underwater unmanned vehicle, is a roll angle velocity of the underwater unmanned vehicle, is a pitch angle velocity of the underwater unmanned vehicle, is a heading angle velocity of the underwater unmanned vehicle, is a pitch angle rate of the underwater unmanned vehicle, is a heading angle rate of the underwater unmanned vehicle; S2: According to the simplified longitudinal motion equation of the underwater unmanned vehicle, the horizontal plane motion equation, the vertical plane motion equation and the auxiliary motion equation, the longitudinal motion state space model, the horizontal plane motion state space model and the vertical plane motion state space model of the underwater unmanned vehicle are established; S3: According to the current position information and the command position information of the underwater unmanned vehicle, the command longitudinal displacement, the command lateral displacement and the command heading of the underwater unmanned vehicle are calculated; S4: According to the established longitudinal motion state space model, horizontal plane motion state space model and vertical plane motion state space model of the underwater unmanned vehicle, the dynamic positioning control law of the underwater unmanned vehicle is designed by using the incremental discrete optimal control method, and the command longitudinal thrust, the command lateral thrust, the command rudder moment, the command vertical thrust and the command trim moment of the underwater unmanned vehicle are calculated in combination with the calculated command longitudinal displacement, command lateral displacement and command heading of the underwater unmanned vehicle; Wherein, according to the established longitudinal motion state space model, horizontal plane motion state space model and vertical plane motion state space model of the underwater unmanned vehicle, the dynamic positioning control law of the underwater unmanned vehicle is designed by using the incremental discrete optimal control method, including: first, the longitudinal motion state space model, horizontal plane motion state space model and vertical plane motion state space model of the underwater unmanned vehicle are discretized according to the control period by using the Euler integral method, to obtain the corresponding discrete motion state space model, the control input in the discretized motion state space model is adjusted to the increment of the control input, and the increment form of the state variable is taken as the new state variable, so as to obtain the corresponding augmented discrete motion state space model, for the augmented discrete longitudinal motion state space model of the underwater unmanned vehicle, the augmented discrete horizontal plane motion state space model and the augmented discrete vertical plane motion state space model, the discrete optimal control method is used respectively, to obtain the longitudinal motion state space control law of the underwater unmanned vehicle as formula (18), the horizontal plane motion state space control law of the underwater unmanned vehicle as formula (19), and the vertical plane motion state space control law of the underwater unmanned vehicle as formula (20): （18）； （19）； （20）； wherein: is is the longitudinal thrust increment at time is the gain coefficient corresponding to the longitudinal velocity increment of the underwater vehicle, is is the longitudinal velocity increment of the underwater vehicle at time is the gain coefficient corresponding to the longitudinal displacement increment of the underwater vehicle, is is the longitudinal displacement increment of the underwater vehicle at time is the gain coefficient corresponding to the longitudinal displacement of the underwater vehicle, is is the longitudinal displacement of the underwater vehicle at time is the commanded heading angle of the underwater vehicle, is is the lateral thrust increment at time is the gain coefficient corresponding to the lateral velocity increment in the lateral motion control parameters of the underwater vehicle, is is the lateral velocity increment of the underwater vehicle at time is the gain coefficient corresponding to the yaw rate increment in the lateral motion control parameters of the underwater vehicle, is is the yaw rate increment of the underwater vehicle at time is the gain coefficient corresponding to the lateral displacement increment in the lateral motion control parameters of the underwater vehicle, is is the lateral displacement increment of the underwater vehicle at time is the gain coefficient corresponding to the lateral displacement in the lateral motion control parameters of the underwater vehicle, is is the lateral displacement of the underwater vehicle at time is the commanded lateral displacement of the underwater vehicle, is is the rudder moment increment at time is the gain coefficient corresponding to the lateral velocity increment in the rudder motion control parameters of the underwater vehicle, is the gain coefficient corresponding to the yaw rate increment in the rudder motion control parameters of the underwater vehicle, is the gain coefficient corresponding to the heading angle rate increment in the rudder motion control parameters of the underwater vehicle, is the gain coefficient corresponding to the heading angle in the rudder motion control parameters of the underwater vehicle, is is the heading angle of the underwater vehicle at time is The constant-time heading angle increment of the underwater unmanned vehicle To provide the heading angle for the underwater unmanned vehicle. for The increment of the pitching moment is given by the command. This refers to the gain coefficient corresponding to the vertical velocity increment in the pitch motion control parameters of an underwater unmanned vehicle. for The vertical velocity increment of the underwater unmanned vehicle at any moment This is the gain coefficient corresponding to the pitch angular velocity increment in the pitch motion control parameters of the underwater unmanned vehicle. for The increment of the pitch angular velocity of the underwater unmanned vehicle at any moment This refers to the gain coefficient corresponding to the pitch increment in the pitch motion control parameters of an underwater unmanned vehicle. for The constant pitch increment of the underwater unmanned vehicle This represents the gain coefficient corresponding to the depth increment in the pitch motion control parameters of an underwater unmanned vehicle. for Real-time depth increments for underwater unmanned vehicles This refers to the gain coefficient corresponding to the pitch motion control parameters of an underwater unmanned vehicle. for The pitch angle of the underwater unmanned vehicle at all times. For the command to tilt, for The command specifies the vertical thrust increment. This refers to the gain coefficient corresponding to the vertical velocity increment in the depth motion control parameters of an underwater unmanned vehicle. This refers to the gain coefficient corresponding to the pitch angular velocity increment in the depth motion control parameters of an underwater unmanned vehicle. This represents the gain coefficient corresponding to the pitch increment in the depth motion control parameters of an underwater unmanned vehicle. This refers to the gain coefficient corresponding to the depth increment in the depth motion control parameters of an underwater unmanned vehicle. This represents the gain coefficient corresponding to the pitch in the depth motion control parameters of an underwater unmanned vehicle. This refers to the gain coefficient corresponding to depth in the depth motion control parameters of an underwater unmanned vehicle. for The depth of the underwater unmanned vehicle at all times. Instruction depth; S5: According to the calculated command longitudinal thrust, command lateral thrust, command rudder moment, command vertical thrust and command trim moment, the command speed of the bow and stern side thrust and the command speed of the bow and stern vertical thrust are calculated, the command speed of the main thrust is calculated according to the calculated command longitudinal thrust, and the command speed of the bow and stern side thrust, the command speed of the bow and stern vertical thrust and the command speed of the main thrust are used to drive the bow and stern side thrust, the bow and stern vertical thrust and the main thrust respectively, to complete the dynamic positioning control of the underwater unmanned vehicle.

2. The discrete optimal control-based underwater unmanned vehicle dynamic positioning control method of claim 1, wherein: The six-degree-of-freedom motion equation of the underwater motion of the underwater unmanned vehicle in step S1 includes an axial motion equation, a lateral motion equation, a vertical motion equation, a roll motion equation, a pitch motion equation and a yaw motion equation.

3. The discrete optimal control-based dynamic positioning control method for an underwater unmanned vehicle according to claim 1, characterized in that: The simplified longitudinal motion equation of the underwater unmanned vehicle obtained in step S1 is formula (8), the simplified horizontal plane motion equation of the underwater unmanned vehicle is formula (9), and the simplified vertical plane motion equation of the underwater unmanned vehicle is formula (10): （8）； （9）； （10）； in: For the mass of underwater unmanned vehicles, The longitudinal velocity of the underwater unmanned vehicle. The lateral velocity of the underwater unmanned vehicle. The vertical velocity of the underwater unmanned vehicle. For the longitudinal acceleration of the underwater unmanned vehicle, For the lateral acceleration of the underwater unmanned vehicle, The longitudinal force coefficient caused by the longitudinal acceleration of the underwater unmanned vehicle. The longitudinal force coefficient caused by the square of the longitudinal velocity of the underwater unmanned vehicle. The longitudinal thrust generated by the main thruster For the heading angular velocity of the underwater unmanned vehicle, Let be the derivative of the heading angular velocity of the underwater unmanned vehicle with respect to time. The lateral force coefficient caused by the lateral acceleration of the underwater unmanned vehicle. The lateral force coefficient is caused by the derivative of the heading angular velocity of the underwater unmanned vehicle with respect to time. The lateral force coefficient is caused by the combined effect of the heading angular velocity and longitudinal velocity of the underwater unmanned vehicle. The lateral force coefficient is caused by the combined effect of the lateral and longitudinal velocities of the underwater unmanned vehicle. To assist the lateral thrust generated by the thruster, For underwater unmanned vehicles to circle Moment of inertia of the shaft The turning moment coefficient is caused by the derivative of the heading angular velocity of the underwater unmanned vehicle with respect to time. The turning moment coefficient caused by the lateral acceleration of the underwater unmanned vehicle. The turning moment coefficient is caused by the combined effect of the heading angular velocity and longitudinal velocity of the underwater unmanned vehicle. The turning moment coefficient is caused by the combined effect of the lateral and longitudinal velocities of the underwater unmanned vehicle. The turning torque generated by the auxiliary thruster, The vertical acceleration of the underwater unmanned vehicle. Let be the derivative of the pitch angular velocity of the underwater unmanned vehicle with respect to time. The vertical force coefficient caused by the derivative of the pitch angular velocity of the underwater unmanned vehicle with respect to time. The vertical force coefficient caused by the vertical acceleration of the underwater unmanned vehicle. The vertical force coefficient is caused by the combined effect of the pitch angular velocity and longitudinal velocity of the underwater unmanned vehicle. For the pitch angular velocity of the underwater unmanned vehicle, a vertical force coefficient caused by the vertical velocity and the longitudinal velocity of the underwater unmanned vehicle, a vertical force caused by the auxiliary thruster, a moment of inertia of the underwater unmanned vehicle about an axis, a trim moment coefficient caused by the derivative of the trim angle of the underwater unmanned vehicle with respect to time, a trim moment coefficient caused by the vertical acceleration of the underwater unmanned vehicle, a trim moment coefficient caused by the trim angle and the longitudinal velocity of the underwater unmanned vehicle, a trim moment coefficient caused by the vertical velocity and the longitudinal velocity of the underwater unmanned vehicle, a metacentric height, a gravitational acceleration, a trim angle of the underwater unmanned vehicle, a trim moment caused by the auxiliary thruster.

4. The discrete optimal control-based underwater unmanned vehicle dynamic positioning control method of claim 1, wherein: The simplified auxiliary motion equation of the underwater unmanned vehicle obtained in step S1 is formula (11): （11）。 5. The discrete optimal control-based dynamic positioning control method for an underwater unmanned vehicle according to claim 3, characterized in that: The longitudinal motion state space model of the underwater unmanned vehicle established in step S2 is formula (12), the horizontal plane motion state space model of the underwater unmanned vehicle is formula (13), and the vertical plane motion state space model of the underwater unmanned vehicle is formula (14): （12）； （13）； （14）； wherein: is a coefficient of influence of the longitudinal velocity of the underwater unmanned vehicle on the longitudinal acceleration of the underwater unmanned vehicle, is a coefficient of influence of the longitudinal thrust of the underwater unmanned vehicle on the longitudinal acceleration of the underwater unmanned vehicle, is a longitudinal displacement of the underwater unmanned vehicle, is a longitudinal displacement rate of the underwater unmanned vehicle, is a heading angle of the underwater unmanned vehicle, is a heading angle rate of the underwater unmanned vehicle, is a lateral displacement of the underwater unmanned vehicle, is a lateral displacement rate of the underwater unmanned vehicle, is a coefficient of influence of the lateral velocity of the underwater unmanned vehicle on the lateral acceleration, is a coefficient of influence of the lateral velocity of the underwater unmanned vehicle on the lateral acceleration, is a coefficient of influence of the lateral velocity of the underwater unmanned vehicle on the derivative of the heading angle rate with respect to time, is a coefficient of influence of the heading angle rate of the underwater unmanned vehicle on the derivative of the heading angle rate with respect to time, is a coefficient of influence of the lateral thrust generated by the auxiliary thruster on the lateral acceleration, is a coefficient of influence of the yaw moment generated by the auxiliary thruster on the derivative of the heading angle rate with respect to time, is a pitch rate of the underwater unmanned vehicle, is a depth of the underwater unmanned vehicle, is a depth rate, is a coefficient of influence of the vertical velocity of the underwater unmanned vehicle on the derivative of the vertical velocity, is a coefficient of influence of the pitch rate of the underwater unmanned vehicle on the derivative of the vertical velocity, is a coefficient of influence of the vertical velocity of the underwater unmanned vehicle on the derivative of the pitch rate, is a coefficient of influence of the pitch rate of the underwater unmanned vehicle on the derivative of the pitch rate, is a coefficient of influence of the pitch angle of the underwater unmanned vehicle on the derivative of the pitch rate, is a coefficient of influence of the vertical thrust of the underwater unmanned vehicle on the derivative of the vertical velocity, is a coefficient of influence of the pitch moment of the underwater unmanned vehicle on the derivative of the pitch rate.

6. The discrete optimal control-based underwater unmanned vehicle dynamic positioning control method of claim 1, wherein: In step S3, the command longitudinal displacement of the underwater unmanned vehicle is calculated according to formula (15), the command lateral displacement of the underwater unmanned vehicle is calculated according to formula (16), and the command heading of the underwater unmanned vehicle is calculated according to formula (17): （15）； （16）； （17）； wherein: is a longitudinal displacement of the underwater unmanned vehicle in the commanded body coordinate system, is a north displacement of the underwater unmanned vehicle in the commanded fixed coordinate system, is a north displacement of the underwater unmanned vehicle at time t, is a north displacement of the underwater unmanned vehicle at time t, is a commanded heading angle of the underwater unmanned vehicle, is an east displacement of the underwater unmanned vehicle in the commanded coordinate system, is an east displacement of the underwater unmanned vehicle at time t, is an east displacement of the underwater unmanned vehicle at time t, is a lateral displacement of the underwater unmanned vehicle in the commanded body coordinate system, is an initial east displacement of the underwater unmanned vehicle in the fixed coordinate system, is an initial north displacement of the underwater unmanned vehicle in the fixed coordinate system.

7. The discrete optimal control-based underwater unmanned vehicle dynamic positioning control method of claim 1, wherein: In step S4, the command longitudinal thrust, the command lateral thrust, the command yaw moment, the command vertical thrust and the command trim moment of the underwater unmanned vehicle are calculated according to formula (21): （21）； wherein: is the longitudinal thrust of the underwater unmanned vehicle at time t, is the longitudinal thrust of the underwater unmanned vehicle at time t, is the lateral thrust of the underwater unmanned vehicle at time t, is the lateral thrust of the underwater unmanned vehicle at time t, is the rudder moment of the underwater unmanned vehicle at time t, is the rudder moment of the underwater unmanned vehicle at time t, is the vertical thrust of the underwater unmanned vehicle at time t, is the vertical thrust of the underwater unmanned vehicle at time t, is the trim moment of the underwater unmanned vehicle at time t, is the trim moment of the underwater unmanned vehicle at time t.

8. The discrete optimal control-based underwater unmanned vehicle dynamic positioning control method of claim 7, wherein: The control period of the command longitudinal thrust, the command lateral thrust, the command yaw moment, the command vertical thrust and the command trim moment of the underwater unmanned vehicle is 0.1 seconds.

Images