An ocean three-dimensional combined observation system and trajectory tracking method

Abstract

The present invention discloses an ocean three-dimensional joint observation system and a trajectory tracking method, belonging to the field of ocean observation. The observation system includes a surface mobile device and an underwater robot, and the surface mobile device is connected to the underwater robot through a cable. The trajectory tracking method receives GPS signals and shore station control instructions through the surface mobile device, performs trajectory tracking according to a preset reference trajectory, and then transmits the position information and control instructions to the underwater robot in real time through the cable, so that the underwater robot tracks the surface mobile device, achieving trajectory tracking control of the ocean three-dimensional joint observation system. The observation system provided by the present invention can realize synchronous real-time three-dimensional observation of the ocean above and below the water surface. The trajectory tracking method provided by the present invention can not only perform prediction and constraint processing on the model of the above-water and underwater joint observation system, but also ensure the accuracy and real-time performance of tracking when the reference trajectory changes greatly.

Description

Technical Field

[0001] The present invention relates to the field of ocean observation, and more particularly to a three-dimensional joint ocean observation system and a trajectory tracking method for water surface and underwater Background Art

[0002] An ocean ranch is a new model of marine fishery production and an important measure to promote the development of the marine economy and the construction of marine ecological civilization. As an important part of the modern ocean ranch construction system, ocean ranch monitoring is the basis for comprehensively and accurately grasping and studying ocean ranches, involving multiple key links such as resource and environmental monitoring, ecological carrying capacity assessment, production process control, and safety forecasting. At present, the monitoring technologies and equipment for ocean ranches are still imperfect, mostly using buoys or underwater robots, etc., which cannot achieve three-dimensional real-time monitoring of water surface and underwater, seriously restricting the construction of modern ocean ranches.

[0003] Currently, most of the trajectory tracking methods for observation systems are for single device trajectory tracking control. For example, PID, as a classic control method, is commonly used in trajectory tracking control systems. It adjusts the control output according to the deviation between the current trajectory of the system and the reference trajectory to reach the desired state. The method is simple, easy to understand, and widely used. The LQR method, as a commonly used trajectory tracking method, can directly solve the optimal controller parameters, ensuring the global optimal solution of the system under given performance indicators. The method is relatively simple and easy to understand, and can adjust the input according to the real-time feedback information of the system to achieve precise control of the system. The LOS guidance method determines the line of sight as the reference trajectory, obtains the state information of the system in real time, and adjusts the control command according to the error between the actual state and the reference to achieve closed-loop control. It has strong real-time performance, can quickly respond to changes, and has strong anti-interference ability.

[0004] However, the above trajectory tracking methods cannot perform joint optimization and control for systems with coupling relationships and mutual influences, nor can they accurately predict and constraint process the system model. In addition, most of the current trajectories are relatively smooth trajectories, and when facing large-angle changes in the reference trajectory, the real-time performance and accuracy of tracking cannot be guaranteed. Summary of the Invention

[0005] Based on the above technical problems, the present invention proposes a three-dimensional joint ocean observation system and a trajectory tracking method.

[0006] The technical solution adopted by the present invention is as follows:

[0007] One of the purposes of the present invention is to provide a three-dimensional joint ocean observation system, which includes a water surface mobile device and an underwater robot, and the water surface mobile device is connected to the underwater robot through a cable;

[0008] The water surface mobile device includes a first floating body. A first thruster group is arranged at the bottom of the first floating body. An electric control electrical box is arranged at the top of the first floating body. An antenna support rod is also arranged on the first floating body, and an antenna is arranged at the top of the antenna support rod.

[0009] The underwater robot includes a support frame. A second thruster group is arranged on the support frame. An electronic cabin is also arranged on the support frame, and the electronic cabin is connected to the electric control electrical box through a cable.

[0010] Observation devices are also equipped on both the water surface mobile device and / or the underwater robot. The observation devices include sensors such as a CTD.

[0011] Preferably, the first floating body is cylindrical. The first thruster group includes four first horizontal thrusters, and the four first horizontal thrusters are arranged at intervals along the circumference of the bottom surface of the floating body. A thruster guard plate is arranged below the first thruster group. The thruster guard plate is annular, and the thruster guard plate is connected to the first floating body through a vertical support rod.

[0012] A bracket is arranged at the top of the first floating body, and the electric control electrical box is installed on the bracket. The antenna includes a GPS antenna and a data transmission radio antenna. Two antenna support rods are provided in total. The GPS antenna is arranged at the top of one antenna support rod, and the data transmission radio antenna is arranged at the top of the other antenna support rod.

[0013] A second floating body is arranged at the upper part of the support frame, and a weight piece is arranged at the lower part of the support frame. The second thruster group includes a second horizontal thruster and a vertical thruster. An LED lighting lamp is also arranged on the support frame.

[0014] The second object of the present invention is to provide a trajectory tracking method for the joint observation system as described above, including the following steps: receiving GPS signals and shore station control commands through the water surface mobile device, performing trajectory tracking according to a preset reference trajectory, and then transmitting the position information and control commands to the underwater robot in real time through a cable, so that the underwater robot tracks the water surface mobile device to achieve trajectory tracking control of the marine stereo joint observation system.

[0015] Preferably, the water surface mobile device performs trajectory tracking according to a preset reference trajectory, specifically including the following steps:

[0016] a. Establish a kinematic model of the water surface mobile device;

[0017] b. Through the kinematic model of the water surface mobile device obtained in step a, establish a state space equation, and perform linearization processing and discretization processing on the state space equation in sequence to construct a new state space expression and output equation.

[0018] c. Derive the prediction equation from the new state - space expression and output equation obtained in step b; establish the objective function and use it to design a real - time optimized trajectory tracking controller based on fuzzy control;

[0019] d. Use the real - time optimized trajectory tracking controller based on fuzzy control obtained in step c to perform trajectory tracking on the water surface mobile device.

[0020] The beneficial technical effects of the present invention are as follows:

[0021] 1. The present invention proposes a new type of water - surface and underwater joint observation system combining a water - surface mobile device and an underwater robot. This system can achieve synchronous real - time three - dimensional observation of the ocean above and below the water surface, which plays a promoting role in the construction of modern ocean ranches.

[0022] 2. The present invention also proposes a trajectory tracking method for the ocean three - dimensional joint observation system, that is, a real - time optimized trajectory tracking method based on fuzzy control. This method takes the change amounts of the lateral and longitudinal speeds at each moment as the inputs of the fuzzy controller, and the coefficients of the control quantity weight matrix in the objective function as the outputs, and adjusts the coefficients in real - time for different trajectories, increasing the real - time performance, accuracy and efficiency of trajectory tracking; and effectively solves the problems of excessive tracking errors caused by large - angle changes in the reference trajectory heading angle and slow convergence speed of the actual trajectory oscillation during the trajectory tracking process. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is a schematic diagram of the overall structural principle of the ocean three - dimensional joint observation system of the present invention;

[0024] Figure 2 is a schematic diagram of the structural principle of the water - surface mobile device in the ocean three - dimensional joint observation system of the present invention;

[0025] Figure 3 is Figure 2 a schematic diagram of the layout of one implementation manner of the first thruster group in

[0026] Figure 4 is a schematic diagram of the structural principle of the underwater robot in the ocean three - dimensional joint observation system of the present invention;

[0027] Figure 5 is a schematic diagram of the flow of the trajectory tracking method of the ocean three - dimensional joint observation system of the present invention;

[0028] Figure 6 is a schematic diagram of the coordinate system when establishing the kinematic model in the trajectory tracking method of the present invention;

[0029] Figure 7 is a flow chart of real - time optimizing the coefficients in the trajectory tracking method of the present invention;

[0030] Figure 8 This is the sine wave trajectory tracking effect diagram when verifying the trajectory tracking method of the present invention;

[0031] Figure 9 This is the rectangular trajectory tracking effect diagram when verifying the trajectory tracking method of the present invention;

[0032] Figure 10 is Figure 9 partial enlarged view of;

[0033] Figure 11 This is the oblique square wave trajectory tracking effect diagram when verifying the trajectory tracking method of the present invention;

[0034] Figure 12 is Figure 11 partial enlarged view of.

[0035] In the figure: 1 - water surface moving device, 2 - underwater robot, 3 - cable, 4 - first floating body, 5 - first thruster group, 501 - first horizontal thruster, 6 - electric control electrical box, 7 - antenna support rod, 8 - thruster guard plate, 9 - bracket, 10 - GPS antenna, 11 - data transmission radio antenna, 12 - support frame, 13 - electronics cabin, 14 - second floating body, 15 - load-bearing steel sheet, 16 - second horizontal thruster, 17 - vertical thruster, 18 - LED lighting lamp, 19 - electronics cabin fixing ring, 20 - umbilical cable hook, 21 - backing plate. Detailed implementation manners

[0036] Aiming at the problem that current ocean observations mostly use single devices such as buoys or underwater robots and cannot achieve real-time three-dimensional monitoring of the water surface and underwater, the present invention proposes an ocean three-dimensional joint observation system, which can achieve synchronous real-time three-dimensional observation of the water surface and underwater of the ocean. At the same time, the present invention also provides a trajectory tracking method for the ocean three-dimensional joint observation system. This is mainly because the existing trajectory tracking methods are not fully applicable in the ocean field, exposing problems such as excessive tracking error, poor anti-interference ability, and poor real-time performance and flexibility. The trajectory tracking method proposed by the present invention, namely a real-time optimized MPC trajectory tracking method based on fuzzy control, can not only perform prediction and constraint processing on the model of the water surface and underwater joint observation system, but also ensure the accuracy and real-time performance of tracking when the reference trajectory changes greatly.

[0037] The present invention will be further described below in conjunction with the accompanying drawings and specific implementation manners.

[0038] As Figure 1 shown, an ocean three-dimensional joint observation system includes a water surface moving device 1 and an underwater robot 2, and the water surface moving device 1 is connected to the underwater robot 2 through a cable 3.

[0039] As Figure 2As shown, the water surface mobile device includes a first floating body 4. A first thruster group 5 is arranged at the bottom of the first floating body 4. An electric control electrical box 6 is arranged at the top of the first floating body 4. An antenna support rod 7 is also arranged on the first floating body 4, and an antenna is arranged at the top of the antenna support rod 7.

[0040] Specifically, the first floating body 4 is in a flat cylindrical shape. The first thruster group 5 includes four first horizontal thrusters 501, and the four first horizontal thrusters 501 are arranged at intervals along the circumference of the bottom surface of the first floating body, as Figure 3 shown. A thruster guard plate 8 is arranged below the first thruster group. The thruster guard plate 8 is in an annular shape, and the thruster guard plate 8 is connected to the first floating body 4 through a vertical support rod. A bracket 9 is arranged at the top of the first floating body 4, and the electric control electrical box 6 is installed on the bracket 9. The antenna includes a GPS antenna 10 and a data transmission radio antenna 11; two antenna support rods 7 are provided in total. The GPS antenna 10 is arranged at the top of one antenna support rod, and the data transmission radio antenna 11 is arranged at the top of the other antenna support rod. The antenna support rod can be processed and made of fiberglass material.

[0041] As Figure 4 shown, the underwater robot 2 includes a support frame 12. A second thruster group is arranged on the support frame 12. An electronic cabin 13 is also arranged on the support frame. The electronic cabin 13 is connected to the electric control electrical box 6 through a cable 3. A second floating body 14 is arranged at the upper part of the support frame 12, and a weight steel sheet 15 is arranged at the lower part of the support frame 12. The second thruster group includes a second horizontal thruster 16 and a vertical thruster 17. Four second horizontal thrusters 16 and four vertical thrusters 17 are both provided, jointly realizing the traveling and turning of the underwater robot as well as underwater positioning. An LED lighting lamp 18 is also arranged on the support frame. The electronic cabin 13 is connected to an electronic cabin fixing ring 19. A umbilical cable hook 20 is also arranged at the top of the support frame 12.

[0042] Observation sensors such as a CTD (Conductivity, Temperature, Depth) sensor are also equipped on both the water surface mobile device and the underwater robot, which are not shown in the figure.

[0043] In summary, the present invention designs a brand-new joint underwater and surface observation system or operation system. This system consists of two parts. One is the surface mobile device on the water surface, which can also be called an unmanned surface mobile relay (USMR), and the other is the underwater robot (ROV) underwater. The unmanned surface mobile relay is respectively composed of a GPS module, a data transmission radio module, an electric control electrical box, a floating body, and four thrusters. The four thrusters of the USMR satisfy axial symmetry in the transverse and longitudinal directions to give it better maneuverability. The control equipment and batteries are placed in the electric control electrical box. The floating body is used to ensure stability and better load-carrying capacity during movement on the water surface. It is also equipped with a data transmission radio with long-distance transmission ability to receive and send signals. The ROV is composed of a floating body, an electric control unit, and eight thrusters. The USMR and the ROV are connected by a cable. By combining the GPS signal received by the USMR with underwater positioning, the accurate geographical coordinates of the ROV can be obtained, thus achieving the effect of "underwater GPS". When the USMR is not combined with the underwater robot, it can be used alone as a high-quality unmanned surface vehicle or buoy, carrying various sensors for detection.

[0044] The present invention also provides a trajectory tracking method for the above-mentioned ocean stereo joint observation system. This method is to receive GPS signals and shore station control instructions through the surface mobile device, perform high-precision trajectory tracking according to a pre-set reference trajectory, and then transmit the position information and control instructions to the underwater robot in real time through the cable, so as to achieve the precise tracking of the underwater robot to the surface mobile device, thereby achieving the trajectory tracking control of the ocean stereo joint observation system.

[0045] It can be seen that based on the above-mentioned trajectory tracking method of the ocean stereo joint observation system, only the precise trajectory tracking control of the surface mobile device on the water surface is required.

[0046] As Figure 5 shown, the surface mobile device performs trajectory tracking according to a pre-set reference trajectory, which specifically includes the following steps:

[0047] a. Establish the kinematic model of the surface mobile device.

[0048] As Figure 6 shown, first establish an inertial reference coordinate system {O E X E Y E} and a body-fixed reference coordinate system {O B X B Y B}.

[0049] Only for the motion of three degrees of freedom of surge, sway, and yaw, the velocity vector is v = [u, v, r] T , and the position vector is where [u, v, r] are the longitudinal velocity, lateral velocity, and yaw angular velocity respectively, which are the ordinate, abscissa, and heading angle in the position information respectively. The three-degree-of-freedom surface mobile device model in Newton-Lagrange form is established as follows:

[0050]

[0051] is the differential of the position vector, Z(η) is the rotation matrix, M is the inertia force matrix, is the differential of the velocity vector, C(v) is the Coriolis centripetal force matrix, D(v) is the damping force matrix, τ is the dynamic input matrix, τ w is the environmental disturbance matrix.

[0052] Without considering the influence of high-order nonlinear damping, the kinematic model of the surface mobile device is finally obtained as follows:

[0053]

[0054] x, y, are defined in the inertial coordinate system, which are the position and heading angle of the USMR respectively, is the differential of the ordinate, is the differential of the abscissa, is the differential of the heading angle, and u, ν, r are the longitudinal velocity, lateral velocity, and yaw angular velocity in the body-fixed coordinate system respectively.

[0055] b. Based on the kinematic model of the surface mobile device obtained in step a, establish the state-space equation, and then perform linearization and discretization processing on the state-space equation in sequence to construct a new state-space expression and output equation.

[0056] Specifically, it includes the following steps:

[0057] b1. Establish the state-space equation.

[0058] To better distinguish each component in the control quantity, change the above longitudinal velocity to V u , change the lateral velocity to V v , and based on the kinematic model established in step a, establish the state-space equation of the system as follows:

[0059]

[0060] And obtain the following nonlinear equations:

[0061]

[0062] is the differential of the system state quantity, and the system state quantity represent the position and the heading angle in the two-dimensional coordinate system respectively, and the control quantity u of the system is u = [V u , V v , r] T , f 1 , f 2 , f 3 represent the relationships between different state quantities and control quantities in the non-linear state space equation respectively.

[0063] b2. Linearization of the state space equation.

[0064] The joint observation system is a complex non-linear system. To simplify the control problem and improve efficiency, the non-linear equation obtained in step b1 is linearized to establish an error state equation:

[0065]

[0066] In the above formula χ c = [x c , y c , r c , is the reference value of the state quantity, and u c = [u c , v c , r c , is the reference value of the control quantity.

[0067]

[0068] The linearized state space equation is:

[0069]

[0070] m is in the above formula n is in the above formula

[0071] b3. Discretization of the state space equation.

[0072] After the linearization of the model in step b2, the forward Euler method is used to discretize the linear state space equation, and the linear discrete state space equation is obtained:

[0073]

[0074] represents the matrix represents the matrix

[0075] In order to more intuitively observe the change in the control quantity in the subsequent constraint part, a new state quantity needs to be constructed, and the change in the control quantity at the previous moment needs to be introduced into the new state quantity.

[0076]

[0077] From the linear discrete state-space equation and the new state quantity ζ(k), we can obtain:

[0078]

[0079] I nu is the identity matrix of the order of the number of control quantities.

[0080] Construct the new state-space expression as follows:

[0081]

[0082] The output equation is:

[0083]

[0084] I nx is the identity matrix of the order of the number of state quantities and is a third-order identity matrix. This definition is to make the output equation contain only state quantities and not control quantities through the matrix K = [I nx 0]. In addition, the output equation can be flexibly changed to control the state quantities to be output through the identity matrix I nx this identity matrix.

[0085] c. Through the new state-space expression and output equation obtained in step b, derive the prediction equation; establish the objective function and use it to design the real-time optimal trajectory tracking controller based on fuzzy control.

[0086] c1. Construct the prediction equation;

[0087] Derive the prediction equation through the new state-space expression and output equation established in step b3;

[0088] First, derive the new state-space expression:

[0089]

[0090] N p is the prediction time domain, and N c is the control time domain, satisfying N p ≥N c .

[0091] Similarly, derived from the output equation:

[0092]

[0093] Through derivation, the relationship between the output λ(k), the state ζ(k), and the change in the control variable ΔU is obtained and rewritten in the following form:

[0094] Z = ρη(k) + σΔU

[0095] where is the output within the prediction horizon N p ; is the state coefficient; is the coefficient in front of the change in the control variable; is the change in the control variable.

[0096] It can be seen from the above equation that knowing the state ζ(k) at the current moment and the control increment within the control horizon N C , the prediction result within the future prediction horizon N P can be obtained, that is, the output of the output equation, thus completing the prediction part.

[0097] c2. Design and optimize the objective function;

[0098] Let Q and R be identity matrices of size N c , is the Kronecker product, an operation symbol in linear algebra; the objective function is designed as follows:

[0099]

[0100] After simplification, we get:

[0101] J = ΔU T (σ T Q q σ + R r )ΔU + 2E T Q q σΔU (1 - 16)

[0102] Let H = σ T Q q σ + R r , f = E T Q q σ, the above equation can be written as follows:

[0103]

[0104] And add constraints to the objective function. For the control variable and the change in the control variable, it has the following form:

[0105] u(k) = u(k - 1) + Δu(k) (1 - 18)

[0106]

[0107] U min ≤U t +A I ΔU t ≤U max (1 - 20)

[0108] Limit the control quantity at each moment. U min is the minimum value of the control quantity, and U max is the maximum value of the control quantity. Rewrite the above formula into the following form:

[0109]

[0110] Adding such constraints is because the control quantity is restricted by some factors, such as the speed and acceleration of the thruster. Therefore, when designing the controller, these restrictions need to be considered and used as one of the constraints of the objective function to ensure that these inputs are within the feasible range. Secondly, optimize the performance index. By adding constraints and adjusting the control input, optimize the effect of trajectory tracking to the greatest extent, such as reducing the tracking error and the time to converge to the reference trajectory.

[0111] In this way, the model predictive control problem is transformed into a quadratic programming problem with inequality constraints, and the controller is designed based on this.

[0112] c3. Design of a real-time optimized trajectory tracking controller based on fuzzy control, or a fuzzy controller for short.

[0113] In the objective function, the matrices Q q and R r are weight coefficient matrices, and they play different roles in trajectory tracking. Q q is the state variable weight matrix, which can reflect the degree of emphasis on different state variables. The larger the element value in the Q q matrix, the more the system emphasizes the trajectory tracking accuracy and can converge to the reference trajectory faster to reduce the tracking error. R r is the control quantity weight matrix, which can reflect the degree of emphasis on different control quantities. The larger the element value in the R r matrix, the smoother the system can adjust when there is a deviation from the reference trajectory, ensuring a more stable approach to the reference trajectory. Since the two matrices are relative, when adjusting the matrices, a fixed and appropriate coefficient is given to the matrix Q q , and the coefficient of the R r matrix is optimized in real time through a fuzzy controller to achieve optimal control, such asFigure 7 as shown

[0114] More specifically, the following steps are further included:

[0115] Define a fuzzy controller with two inputs and one output. The two input variables are the longitudinal velocity change ΔV of the joint observation system u and the lateral velocity change ΔV v , and the output variable is the matrix coefficient optimized in real time. Define the fuzzy quantities of seven membership functions, which are {NB, NM, NS, ZO, PS, PM, PB} respectively, and set the membership functions as Gaussian membership functions and S-shaped membership functions respectively.

[0116] The Gaussian membership function satisfies:

[0117]

[0118] The Gaussian membership function is determined by two parameters σ and c. Generally, σ is positive, and c determines the center of the curve.

[0119] The S-shaped membership function satisfies:

[0120]

[0121] The S-shaped membership function is determined by two parameters a and u. The positive or negative of a determines the opening direction of the function.

[0122] After completing the fuzzification, continuously optimize and adjust to generate a fuzzy control rule table. Given that the input variables are ΔV u and ΔV v , and the output variable is μ. Assume that and are the i-th and j-th fuzzy sets of ΔV u and ΔV v respectively, and O k is the k-th fuzzy set of the output variable μ. The fuzzy rule is expressed in the following form:

[0123]

[0124] P = m × n is the total number of rules. According to the above expression, the fuzzy control rule table can be formulated.

[0125] Finally, defuzzification is required. Generally, the centroid method is used for defuzzification. First, calculate the weighted average value, which is achieved by weighted averaging the membership functions of each fuzzy output:

[0126]

[0127] N is the number of discrete values of the output variable, ε(μ) is the membership value corresponding to each membership function, and μ is the centroid value of the region covered by the membership function, with its range being [μ min , μ max .

[0128] Subsequently, the normalization coefficient is calculated to ensure that the output value is within an appropriate range. The normalization function is usually defined as the sum of the membership functions of the fuzzy output quantity:

[0129]

[0130] Finally, the defuzzified result is calculated:

[0131]

[0132] μ f represents the output quantity after defuzzification. Such a fuzzy control process is basically completed. Optimize and rewrite the R r matrix in the objective function:

[0133]

[0134] μ f (k) is the final output quantity of the fuzzy controller at time k. It changes in real time with the two input quantities at different times and is a constant. Multiply the real-time optimization coefficient in front of the matrix R r (k): R f (k) represents the final R matrix at time k.

[0135] d. Perform trajectory tracking on the water surface mobile device through the real-time optimization trajectory tracking controller based on fuzzy control obtained in step c.

[0136] The trajectory tracking method of the present invention improves the real-time performance, accuracy, and efficiency of trajectory tracking. And it effectively solves the problems of excessive tracking errors caused by large-angle changes in the reference trajectory heading angle and slow convergence speed of the actual trajectory oscillation during the trajectory tracking process.

[0137] Next, the trajectory tracking method of the present invention is verified accordingly.

[0138] Specifically, the method proposed by the present invention is verified through simulation, and the verification through three trajectories: sine wave trajectory, rectangular trajectory, and trapezoidal wave trajectory shows that the method proposed by the present invention is not only applicable to the case of small-range real-time changes in the reference trajectory, but also has good tracking effects when facing sudden large-angle changes in the same and opposite directions.

[0139] Among them, Figure 8 is the tracking effect diagram of the sine wave trajectory during the verification of the trajectory tracking method of the present invention.Figures 9 - 10 This is the effect diagram of rectangular trajectory tracking during the verification of the trajectory tracking method of the present invention. Figures 11 - 12 This is the effect diagram of trapezoidal wave trajectory tracking during the verification of the trajectory tracking method of the present invention.

[0140] The performance indicators of rectangular trajectory tracking are shown in Table 1.

[0141] Table 1

[0142]

[0143] The performance indicators of trapezoidal wave trajectory tracking are shown in Table 2.

[0144] Table 2

[0145]

[0146] The present invention proposes three performance indicators for performance tracking, namely longitudinal error, lateral error, and the time for the actual trajectory to converge to the reference trajectory. From Figures 8 - 12 As can be seen from Table 1 - Table 2, for the sine wave trajectory, the tracking effects before and after real-time optimization are very good. In the case of sudden large-angle changes in the reference trajectory such as rectangles and trapezoidal waves, the tracking effect after real-time optimization is significantly improved compared with that before optimization, improving the accuracy and real-time performance of trajectory tracking.

Claims

1. A trajectory tracking method for a marine stereoscopic joint observation system, the joint observation system comprising a surface mobile device and an underwater robot, the surface mobile device being connected to the underwater robot via a cable; The surface mobile device comprises a first floating body, a first propeller group is arranged at the bottom of the first floating body, an electric control box is arranged at the top of the first floating body, an antenna support rod is also arranged on the first floating body, and an antenna is arranged at the top of the antenna support rod; The underwater robot comprises a support frame, a second thruster group is arranged on the support frame, an electronic cabin is also arranged on the support frame, and the electronic cabin is connected to the electric control box through a cable; The surface mobile device and / or the underwater robot are also equipped with observation equipment; It is characterized in that The method comprises the following steps: receiving GPS signals and shore station control instructions through a surface mobile device, tracking the trajectory according to a preset reference trajectory, and then transmitting the position information and control instructions to an underwater robot through a cable in real time, so that the underwater robot tracks the surface mobile device, thereby achieving trajectory tracking control of the ocean stereo joint observation system; The surface mobile device performs trajectory tracking according to a preset reference trajectory, which specifically includes the following steps: a. Establish the kinematic model of the surface mobile device; b. Establishing a state space equation based on the kinematic model of the surface mobile device obtained in step a, and sequentially performing linearization and discretization on the state space equation to construct a new state space expression and output equation; c. Derivation of the prediction equation using the new state space expression and output equation obtained in step b; establishment of the objective function, and use it to design a real-time optimization trajectory tracking controller based on fuzzy control; d. Tracking the trajectory of the surface mobile device using the real-time optimized trajectory tracking controller based on fuzzy control obtained in step c; In step c: A two-input and one-output fuzzy controller is defined. The two input quantities are the longitudinal velocity change ΔV of the joint observation system. u and the lateral velocity change ΔV v , an output is the matrix coefficient of real-time optimization, defining the fuzzy quantities of seven membership functions, which are {NB, NM, NS, ZO, PS, PM, PB}, and setting the membership functions to Gaussian membership function and S-type membership function respectively; The Gaussian membership function satisfies: The Gaussian membership function is determined by two parameters σ and c, σ is positive, and c determines the center of the curve; The S-type membership function satisfies: The S-type membership function is determined by two parameters a and u. The positive or negative value of a determines the opening direction of the function. After the fuzzification is completed, the fuzzy control rule table is continuously optimized and adjusted. The known input quantity is ΔV u and ΔV v , the output is μ, assuming and They are ΔV u and ΔV v The i-th and j-th fuzzy sets of O k is the kth fuzzy set of the output variable μ, which is expressed in the following form using fuzzy rules: Rule 1: If ΔV u yes And ΔV v yes Then the value of μ is O1; Rule 2: If Δr u yes And ΔV v yes Then the value of μ is O2; Rule P: If ΔV u yes And ΔV v yes Then the value of μ is O k ; P = m × n is the total number of rules. Based on the above expression, a fuzzy control rule table can be formulated; Finally, to defuzzify, first calculate the weighted average, which is achieved by taking the weighted average of the membership function of each fuzzy output: N is the number of discrete values ​​of the output variable, ε(μ) is the membership value corresponding to each membership function, and μ is the centroid value of the area covered by the membership function, which ranges from [μ min ,μ max ]; The normalization coefficient is then calculated. The normalization function is usually defined as the sum of the membership functions of the fuzzy output quantity: Finally, calculate the defuzzified result: μ f Represents the output after defuzzification; for R in the objective function r The matrix is ​​rewritten for optimization: μ f (k) is the final output of the fuzzy controller at time k. It changes in real time with the two inputs at different times and is a constant. r (k) is multiplied by the real-time optimization coefficient R f (k) represents the final R matrix at time k.

2. The trajectory tracking method of the ocean stereoscopic joint observation system according to claim 1, characterized in that: The first floating body is cylindrical, and the first thruster group includes four first horizontal thrusters, which are arranged at intervals along the circumference of the bottom surface of the first floating body; a thruster guard plate is arranged below the first thruster group, the thruster guard plate is in a circular ring shape, and the thruster guard plate is connected to the first floating body through a vertical support rod; A bracket is provided on the top of the first floating body, and the electric control box is installed on the bracket; the antenna includes a GPS antenna and a data transmission radio antenna; two antenna support rods are provided, and a GPS antenna is provided on the top of one antenna support rod, and a data transmission radio antenna is provided on the top of the other antenna support rod; A second floating body is arranged on the upper part of the supporting frame, and a weight plate is arranged on the lower part of the supporting frame; the second thruster group includes a second horizontal thruster and a vertical thruster; and an LED lighting lamp is also arranged on the supporting frame.

3. The trajectory tracking method of the ocean stereo joint observation system according to claim 1, characterized in that: Step a includes the following steps: First, establish an inertial reference coordinate system {O E X E Y E } and the body reference coordinate system {O B X B Y B }; For the motion of three degrees of freedom, namely surge, sway and pitch, the velocity vector is v = [u, v, r] T , the position vector is Where [u,v,r] are the longitudinal velocity, lateral velocity and yaw angular velocity, respectively. They are the ordinate, abscissa and heading angle in the position information respectively; the three-degree-of-freedom water surface mobile device model under the Newton-Lagrangian form is established as follows: is the differential of the position vector, Z(η) is the rotation matrix, M is the inertial force matrix, is the differential of the velocity vector, C(v) is the Coriolis centripetal force matrix, D(ν) is the damping force matrix, τ is the power input matrix, τ w is the environmental interference matrix; Without considering the influence of high-order nonlinear damping, the kinematic model of the surface mobile device is as follows: x,y, are defined in the inertial coordinate system, which are the position and heading angle of the surface mobile device, is the differential of the ordinate, is the differential of the horizontal axis, is the differential of the heading angle, and u, v, r are the longitudinal velocity, lateral velocity and yaw angular velocity in the body coordinate system.

4. The trajectory tracking method of the ocean stereo joint observation system according to claim 3, characterized in that: Step b includes the following steps: b1. Establish state space equations; Change the vertical speed to V u , the lateral speed is changed to V v , through the kinematic model established in step a, the state space equation of the surface mobile device is established as follows: The following nonlinear equation is obtained: The state of the system Respectively represent the position and heading angle in the two-dimensional coordinate system, is the differential of the system state quantity, the control quantity of the system u=[V u ,V v ,r] T , f1, f2, f3 respectively represent the relationship between different state quantities and control quantities in the nonlinear state space equation; b2, linearization of state space equations; b3. Discretization of state space equations.

5. The trajectory tracking method of the ocean stereo joint observation system according to claim 4, characterized in that: The processing process of step b2 is as follows: The nonlinear equation obtained in step b1 is linearized to establish the error state equation: In the above formula χ c =[x c ,y c ,r c ], is the reference value of the state quantity, u c =[u c ,v c ,r c ], is the reference value of the control quantity; The linearized state space equation is: m is the n is the 6. The trajectory tracking method of the ocean stereo joint observation system according to claim 5, characterized in that: The processing process of step b3 is as follows: After the linearization of the model in step b2, the forward Euler method is used to discretize the linearized state space equation to obtain the linear discrete state space equation: Representative Matrix Representative Matrix Construct a new state quantity, the new state quantity needs to introduce the change of the control quantity at the previous moment From the linear discrete state space equation and the new state quantity ζ(k), we can get: I nu is the identity matrix of the number of control quantities, The new state space expression is constructed as follows: The output equation is: I nx It is the identity matrix of the number of state quantities and the third-order identity matrix. This definition is obtained by K = [I nx 0] This matrix makes the output equation contain only state quantities but not control quantities; the output equation is flexible and can be changed by I nx This unit matrix is ​​used to control the state quantity you want to output.

7. The trajectory tracking method of the ocean stereo joint observation system according to claim 6, characterized in that: Step c includes the following steps: c1. Construct prediction equation; The prediction equation is obtained by deducing the new state space expression and output equation established in step b3; First, the new state space expression is derived: N p is the prediction time domain, N c is the control time domain, satisfying N p ≥N c ; Similarly, the output equation is derived as follows: By derivation, the relationship between the output λ(k), the state quantity ζ(k), and the control quantity change ΔU is obtained, and rewritten into the following form: Z=ρζ(k)+σΔU in In the prediction domain N p Output within is the state quantity coefficient; It is the coefficient in front of the control quantity change; is the change in the controlled quantity, K=[I nx 0], From the above formula, we can know that the current state quantity ζ(k) and the control time domain N C The control increment within can get the future prediction time domain N P The predicted result within is the output of the output equation; c2. Design and optimize the objective function; Let E = ρζ(k), Q and R are N c The identity matrix of numbers, is the Kronecker product, and the design objective function is as follows: Simplifying, we get: J=ΔU T (s T Q q σ+R r )ΔU+2E T Q q sΔU (1-16) Let H = σ T Q q σ+R r , f=E T Q q σ, the above formula can be written as follows: And add constraints to the objective function, the control amount and control amount increment have the following forms: u(k)=u(k-1)+Δu(k) (1-18) U min ≤U t +A I ΔU t ≤U max (1-20) Limit the control amount at each moment, U min is the minimum value of the control quantity, U max is the maximum value of the control quantity, rewrite the above formula into the following form: c3. Design of real-time optimal trajectory tracking controller based on fuzzy control; In the objective function, Q q is the state weight matrix, Q q The larger the value of the element in , the more the system pays attention to the trajectory tracking accuracy, and the faster it can converge to the reference trajectory to reduce the tracking error; R r is the control weight matrix, R r The larger the element value in the matrix, the smoother the system can adjust when it deviates from the reference trajectory, ensuring a more stable approach to the reference trajectory. Since the two matrices are relative, when adjusting the matrix, the given matrix Q q A fixed suitable coefficient, for R r The coefficients of the matrix are optimized in real time through a fuzzy controller to achieve optimal control.

Images